楊 光 黃 楠 張 騰

（中國(guó)船舶集團(tuán)有限公司第七一〇研究所 宜昌 443003）

1 引言

水下航行體的發(fā)射平臺(tái)在水下為垂直系留狀態(tài)，當(dāng)識(shí)別到目標(biāo)時(shí)，水下航行體快速完成對(duì)準(zhǔn)建立垂直方向的載體坐標(biāo)系，當(dāng)需要進(jìn)行攻擊時(shí)，水下航行器從發(fā)射平臺(tái)彈出進(jìn)入工作狀態(tài)，到達(dá)一定水深后進(jìn)行拐彎控制轉(zhuǎn)水平攻擊，整個(gè)過(guò)程中俯仰通道實(shí)現(xiàn)了0°～90°附近的大幅機(jī)動(dòng)。當(dāng)俯仰角為±90°時(shí)，歐拉角會(huì)出現(xiàn)奇點(diǎn)，三個(gè)通道的角度將會(huì)出現(xiàn)突變。歐拉角是控制系統(tǒng)的重要參數(shù)，突變的歐拉角將導(dǎo)致航行體控制系統(tǒng)的失穩(wěn)［1］。

目前計(jì)算航行體姿態(tài)的算法主要有四元數(shù)法、方向余弦法、歐拉角法等。其中：四元數(shù)法通過(guò)求解四個(gè)微分方程可以實(shí)現(xiàn)全姿態(tài)的解算，精度高，計(jì)算量小，但解算的過(guò)程中需要經(jīng)過(guò)正交化的誤差修正，當(dāng)有界參數(shù)誤差超出范圍會(huì)導(dǎo)致解算結(jié)果錯(cuò)誤［3～5］，方向余弦法可以進(jìn)行全姿態(tài)結(jié)算，但求解的微分方程較多，對(duì)硬件平臺(tái)的計(jì)算資源提出了考驗(yàn)［2］，歐拉角法只需對(duì)三個(gè)微分方程進(jìn)行解算，與以上算法相比，求解的方程個(gè)數(shù)最少，姿態(tài)矩陣求解無(wú)需進(jìn)行正交化處理，工程實(shí)現(xiàn)容易，只需要解決姿態(tài)解算出現(xiàn)奇點(diǎn)的問(wèn)題［6］。

本文針對(duì)水下航行體在垂直彈道轉(zhuǎn)水平彈道過(guò)程中使用歐拉法解算容易出現(xiàn)奇異值的問(wèn)題，提出了雙參考坐標(biāo)系控制法，建立垂直、水平兩套參考坐標(biāo)系描述系統(tǒng)，利用各自的奇異特點(diǎn)，設(shè)計(jì)了在彈道初始階段利用垂直參考坐標(biāo)系控制爬升彈道、在拐彎過(guò)程中，當(dāng)傾斜角大于45°時(shí)，切換為水平參考坐標(biāo)系控制水平彈道，通過(guò)半實(shí)物仿真試驗(yàn)驗(yàn)證了該方法的有效性。

2 坐標(biāo)系及姿態(tài)角定義

2.1 坐標(biāo)系定義

大地坐標(biāo)系與地面固連在一起，其原點(diǎn)O0位于地面某定點(diǎn)，O0x0軸鉛直向上，O0y0軸在水平面內(nèi)指向任意方向，O0z0軸垂直于O0x0y0平面，坐標(biāo)系遵守右手法則。平移坐標(biāo)系為大地坐標(biāo)系原點(diǎn)O0平移到航行器浮心位置。

載體坐標(biāo)系與航行器固連，航行器的浮心為其原點(diǎn)，沿航行器縱軸指向頭部為Ox軸，Oy軸與Ox軸為垂直關(guān)系，當(dāng)航行器在地面上水平放置時(shí)，Oy軸指向上為正，Oz軸垂直O(jiān)xy平面，坐標(biāo)系遵守右手法則［7～8］。大地坐標(biāo)系和載體坐標(biāo)系的關(guān)系見(jiàn)圖1。

圖1 坐標(biāo)系定義

2.2 姿態(tài)角定義

水下航行體浮心在平移坐標(biāo)系內(nèi)的坐標(biāo)值定義其空間位置，平移坐標(biāo)系的歐拉角定義其空間姿態(tài)。載體坐標(biāo)系相對(duì)平移坐標(biāo)系的角位置關(guān)系可用偏航角ψ，俯仰角θ，橫滾角?來(lái)表示［9～10］，如圖2所示，其轉(zhuǎn)動(dòng)順序（逆時(shí)針為正）為

轉(zhuǎn)換矩陣為

歐拉方程為

3 參考坐標(biāo)系建立

3.1 垂直參考坐標(biāo)系

水下航行體發(fā)射后需要根據(jù)目標(biāo)進(jìn)行射面對(duì)準(zhǔn)，彈道控制系統(tǒng)姿態(tài)控制指令θg=0、ψg=0、φg=φa，其中?a為使水下航行體縱對(duì)稱面與射面重合需要滾轉(zhuǎn)的角度。

完成射面對(duì)準(zhǔn)后，水下航行體建立垂直參考系on1xn1yn1zn1，平移坐標(biāo)系ox′y′z′與垂直參考坐標(biāo)系on1xn1yn1zn1的相對(duì)關(guān)系見(jiàn)圖3。

圖3 平移坐標(biāo)系與垂直參考坐標(biāo)系示意圖

設(shè)水下航行體在建立垂直參考坐標(biāo)系前的姿態(tài)角為θ0、ψ0、?0。載體坐標(biāo)系oxyz、平移坐標(biāo)系ox′y′z′、垂直參考坐標(biāo)系on1xn1yn1zn1之間的關(guān)系如下：

因此

建立垂直參考坐標(biāo)系后姿態(tài)角為θ1、ψ1、?1，則

3.2 水平參考坐標(biāo)系

為了使水下航行體姿態(tài)計(jì)算適應(yīng)俯仰角超過(guò)90°的大范圍變化，并且便于進(jìn)行水平航行控制，需要在射面內(nèi)建立水平參考坐標(biāo)系on2xn2yn2zn2，垂直參考坐標(biāo)系on1xn1yn1zn1與水平參考坐標(biāo)系on2xn2yn2zn2的相對(duì)關(guān)系見(jiàn)圖4。

設(shè)切換到水平參考坐標(biāo)系前的姿態(tài)角為θ1、ψ1、?1。載體坐標(biāo)系oxyz、參考坐標(biāo)系on1xn1yn1zn1、參考坐標(biāo)系on2xn2yn2zn2之間的關(guān)系如下：

因此

建立水平參考坐標(biāo)系后的姿態(tài)角為θ2、ψ2、φ2，則

圖4 水平參考坐標(biāo)系與垂直參考坐標(biāo)系示意圖

4 控制方案及半實(shí)物仿真

水下航行體的動(dòng)力學(xué)模型為非線性化模型，需要通過(guò)小擾動(dòng)線性化的方法消除動(dòng)態(tài)系統(tǒng)的非線性部分，建立航行體線性化運(yùn)動(dòng)模型，通過(guò)狀態(tài)反饋的PID 閉環(huán)控制來(lái)實(shí)現(xiàn)航行過(guò)程的穩(wěn)定控制［11］。整個(gè)控制系統(tǒng)主要控制單元包括捷聯(lián)慣性測(cè)量系統(tǒng)、導(dǎo)航穩(wěn)定控制系統(tǒng)和舵機(jī)調(diào)節(jié)系統(tǒng)，捷聯(lián)慣性測(cè)量系統(tǒng)完成水下航行體工作過(guò)程中運(yùn)動(dòng)學(xué)信息的測(cè)量和解算；導(dǎo)航穩(wěn)定控制系統(tǒng)利用角速度和過(guò)載的反饋通過(guò)穩(wěn)定控制算法得到操舵指令，舵機(jī)調(diào)節(jié)系統(tǒng)根據(jù)操舵指令進(jìn)行操舵完成水下航行體的姿態(tài)調(diào)整［12］，控制原理見(jiàn)圖5。

圖5 控制系統(tǒng)原理框圖

4.1 三通道姿態(tài)控制

以俯仰、偏航及橫滾三通道為控制目標(biāo)，由慣導(dǎo)、舵機(jī)、校正網(wǎng)絡(luò)以及水下航行體動(dòng)力學(xué)等環(huán)節(jié)組成閉合回路。其中俯仰、偏航和差動(dòng)舵分別由三個(gè)獨(dú)立的舵系統(tǒng)實(shí)現(xiàn)，俯仰、偏航、橫滾指令由穩(wěn)定控制算法產(chǎn)生，校正網(wǎng)絡(luò)用于動(dòng)態(tài)調(diào)整水下航行體的阻尼系統(tǒng)，提高系統(tǒng)的穩(wěn)定性和魯棒性［13］，姿態(tài)控制原理見(jiàn)圖6。

圖6 三通道姿態(tài)控制原理框圖

三通道姿態(tài)控制器如下。

Uδi為輸送給舵機(jī)的操舵指令；Ui(i-y，p，r)為輸入穩(wěn)定控制系統(tǒng)對(duì)應(yīng)通道的指令；為控制器參數(shù)及校正網(wǎng)絡(luò)參數(shù)。

4.2 半實(shí)物仿真及結(jié)果

半實(shí)物仿真需要將水下航行體的控制系統(tǒng)硬件平臺(tái)接入到實(shí)時(shí)仿真系統(tǒng)中，通過(guò)模擬實(shí)時(shí)工況檢驗(yàn)水下航行體控制系統(tǒng)的工作流程和控制規(guī)律，是最接近真實(shí)試驗(yàn)的仿真手段。

對(duì)水下航行體進(jìn)行半實(shí)物仿真的仿真系統(tǒng)一般由三軸運(yùn)動(dòng)轉(zhuǎn)臺(tái)，實(shí)時(shí)仿真系統(tǒng)，深度模擬器，水下航行體運(yùn)動(dòng)學(xué)仿真模型以及檢測(cè)設(shè)備組成。半實(shí)物仿真過(guò)程中，通過(guò)設(shè)置約束變量進(jìn)行各種工作環(huán)境的模擬，驗(yàn)證水下航行體控制系統(tǒng)實(shí)時(shí)控制性能以及控制參數(shù)［14］，半實(shí)物仿真原理框圖見(jiàn)圖7，部分仿真結(jié)果見(jiàn)圖8～10。

圖7 半實(shí)物仿真原理框圖

圖8 姿態(tài)角曲線

圖9 位移曲線

圖10 三維軌道曲線

5 結(jié)語(yǔ)

雙參考坐標(biāo)系控制法在彈道前期初始對(duì)準(zhǔn)、爬升等機(jī)動(dòng)控制時(shí)采用垂直參考坐標(biāo)系進(jìn)行控制，當(dāng)傾斜角超過(guò)45°時(shí)，切換至水平參考坐標(biāo)系進(jìn)行控制，通過(guò)半實(shí)物仿真驗(yàn)證，該方法能夠解決水下航行體俯仰角在90°附近，姿態(tài)角出現(xiàn)奇異的問(wèn)題，通過(guò)反饋閉環(huán)控制能夠保證水下航行體的穩(wěn)定航行。