奔粵陽(yáng)，張瀚軒，崔文婷，尹冬寒

（1.哈爾濱工程大學(xué)自動(dòng)化學(xué)院，哈爾濱 150001；2.中國(guó)船舶工業(yè)集團(tuán)公司第七〇八研究所，上海 201100）

由于極區(qū)處于高緯度地區(qū)，受其特殊地理位置的影響，經(jīng)線在極點(diǎn)附近收斂于極點(diǎn)，這使得常用的指北方位機(jī)械編排和游移方位機(jī)械編排都不再適用[1]。針對(duì)該問(wèn)題，有學(xué)者提出橫坐標(biāo)系慣導(dǎo)系統(tǒng)[2-4]和格網(wǎng)坐標(biāo)系慣導(dǎo)系統(tǒng)[5-7]兩種導(dǎo)航方式并得到一定的發(fā)展。通過(guò)分析發(fā)現(xiàn)，基于橫坐標(biāo)系和格網(wǎng)坐標(biāo)系的兩種機(jī)械編排仍然存在舒勒周期、傅科周期和地球周期三種振蕩，這對(duì)于長(zhǎng)時(shí)間航行的艦船來(lái)說(shuō)，需要采用一些技術(shù)去消除這些周期振蕩來(lái)提高慣導(dǎo)系統(tǒng)的導(dǎo)航精度[8-10]。文獻(xiàn)[11]和文獻(xiàn)[12]分別基于橫坐標(biāo)系慣導(dǎo)系統(tǒng)和格網(wǎng)坐標(biāo)系慣導(dǎo)系統(tǒng)推導(dǎo)了系統(tǒng)的誤差模型并采用了傳統(tǒng)阻尼網(wǎng)絡(luò)算法，有效地抑制周期振蕩到來(lái)的誤差。但是這種傳統(tǒng)阻尼網(wǎng)絡(luò)通常存在兩個(gè)問(wèn)題:阻尼網(wǎng)絡(luò)參數(shù)的選擇需要進(jìn)行不斷的試湊；參數(shù)一旦確定就無(wú)法改變，這與我們希望在導(dǎo)航初期系統(tǒng)具有較大阻尼比使得誤差快速收斂，進(jìn)入穩(wěn)態(tài)后系統(tǒng)具有較小阻尼比減小載體機(jī)動(dòng)的影響相矛盾。

本文針對(duì)傳統(tǒng)阻尼網(wǎng)絡(luò)設(shè)計(jì)存在的問(wèn)題，設(shè)計(jì)了一種自適應(yīng)反饋增益阻尼方法。該方法在無(wú)阻尼格網(wǎng)慣導(dǎo)系統(tǒng)中加入自適應(yīng)反饋增益，該反饋增益可以通過(guò)有效估計(jì)手段進(jìn)行求取并且能夠根據(jù)外速度信息的變化進(jìn)行實(shí)時(shí)地調(diào)整，從而抑制格網(wǎng)慣導(dǎo)系統(tǒng)的振蕩誤差。仿真結(jié)果表明，本文所設(shè)計(jì)的自適應(yīng)反饋增益阻尼網(wǎng)絡(luò)能夠加快慣導(dǎo)系統(tǒng)誤差的收斂速度，減小系統(tǒng)阻尼狀態(tài)切換時(shí)產(chǎn)生的超調(diào)量。

1 格網(wǎng)坐標(biāo)系建立及導(dǎo)航原理

1.1 格網(wǎng)坐標(biāo)系的建立

如圖1所示，以載體所在地P點(diǎn)處平行于格林威治子午面的平面作為格網(wǎng)平面，以載體所在地的水平面作為切平面，格網(wǎng)平面與切平面的交線定義為格網(wǎng)北向，格網(wǎng)北向同真北方向的夾角記為σ，格網(wǎng)天向同當(dāng)?shù)氐乩硖煜蛑睾?，格網(wǎng)東向在切平面內(nèi)且與格網(wǎng)北向垂直構(gòu)成右手直角坐標(biāo)系，此即為格網(wǎng)坐標(biāo)系G系（eGE,eGN,eGU）?？梢?jiàn)，G 系同樣為P點(diǎn)處的水平坐標(biāo)系，僅與當(dāng)?shù)氐乩碜鴺?biāo)系g 系（東-北-天）在方位上偏差σ角。其中，L、λ分別為載體所在地的緯度和經(jīng)度，ωie為地球自轉(zhuǎn)角速度，eE,eN,eU分別為當(dāng)?shù)氐乩碜鴺?biāo)系的東向、北向和天向。

圖1 格網(wǎng)坐標(biāo)系Fig.1 Grid coordinate system

1.2 格網(wǎng)坐標(biāo)系導(dǎo)航原理

以格網(wǎng)坐標(biāo)系為導(dǎo)航坐標(biāo)系，它的運(yùn)動(dòng)學(xué)方程與采用固定指北方位類(lèi)似，由于格網(wǎng)北向同真北方向存在夾角σ，所以兩者的主要區(qū)別是一些運(yùn)動(dòng)參數(shù)在導(dǎo)航系下的投影分量不同。根據(jù)文獻(xiàn)[13,14]，假設(shè)地球?yàn)閳A球體，可以得到在格網(wǎng)坐標(biāo)系下慣導(dǎo)系統(tǒng)的運(yùn)動(dòng)學(xué)方程:

姿態(tài)基本方程:

速度基本方程:

位置基本方程:

式中

2 自適應(yīng)反饋增益阻尼方法

通過(guò)對(duì)格網(wǎng)坐標(biāo)系下慣導(dǎo)系統(tǒng)的運(yùn)動(dòng)學(xué)方程進(jìn)行分析,并假設(shè)系統(tǒng)中陀螺儀和加速度計(jì)的安裝誤差和標(biāo)度因數(shù)誤差已經(jīng)得到補(bǔ)償，可以建立慣導(dǎo)系統(tǒng)的誤差模型，即無(wú)阻尼誤差模型。在無(wú)阻尼誤差模型基礎(chǔ)上，通過(guò)加入反饋的方式，將原本臨界穩(wěn)定的系統(tǒng)轉(zhuǎn)換為漸近穩(wěn)定的系統(tǒng)[15]。我們假設(shè)在地球是圓球體的情況下，加入反饋增益的格網(wǎng)慣導(dǎo)系統(tǒng)誤差方程為:

位置誤差方程:

速度誤差方程:

失準(zhǔn)角方程:

格網(wǎng)角誤差方程:

其中，R為地球半徑，g為重力加速度，δL,δλ分別為緯度誤差和經(jīng)度誤差，分別為格網(wǎng)坐標(biāo)系下的東向和北向速度誤差，分別為格網(wǎng)坐標(biāo)系的東向、北向和天向失準(zhǔn)角，δσ為格網(wǎng)角誤差，分別為格網(wǎng)坐標(biāo)系下的東向和北向加速度計(jì)誤差，分別為在格網(wǎng)坐標(biāo)系下的東向、北向和天向陀螺漂移。Ux1,Ux2,Uy1,Uy2分別為北向和東向回路的反饋控制量，它們的具體表達(dá)式為:

其中kx1,kx2,ky1,ky2為自適應(yīng)反饋增益，分別為格網(wǎng)坐標(biāo)系下外速度參考誤差。為了更加容易地研究探討問(wèn)題，假定是在靜基座情況下，此時(shí)艦船不運(yùn)動(dòng)，即。根據(jù)加入反饋增益的格網(wǎng)慣導(dǎo)系統(tǒng)誤差方程畫(huà)出加入反饋增益格網(wǎng)慣導(dǎo)系統(tǒng)方框圖，如圖2所示。

圖2 加入反饋增益格網(wǎng)慣導(dǎo)系統(tǒng)誤差方框圖Fig.2 Add feedback gain grid inertial navigation system error block diagram

由圖2可見(jiàn)，緯度誤差δL和格網(wǎng)角誤差δσ包含在系統(tǒng)的閉環(huán)回路中，當(dāng)整個(gè)系統(tǒng)穩(wěn)定的時(shí)候，緯度誤差和格網(wǎng)角誤差亦將是有界的，即不會(huì)隨時(shí)間的增長(zhǎng)而產(chǎn)生積累誤差；但對(duì)于經(jīng)度誤差δλ卻不然，它是開(kāi)環(huán)的，即經(jīng)度誤差會(huì)隨著時(shí)間積累而產(chǎn)生積累誤差。

下面具體分析在兩個(gè)水平回路上加入反饋增益后，是如何使周期振蕩誤差得到抑制的。因?yàn)楦悼浦芷谡袷幷{(diào)節(jié)舒勒周期振蕩，所以我們忽略傅科周期振蕩的影響。我們不難得到忽略傅科振蕩周期后，加入反饋增益慣導(dǎo)系統(tǒng)誤差方框圖的特征多項(xiàng)式為:

對(duì)比無(wú)阻尼格網(wǎng)慣導(dǎo)系統(tǒng)的特征多項(xiàng)式:

可見(jiàn)，當(dāng)kx1,ky1＞ 0;kx2,ky2＜0時(shí)，在兩條水平回路上加入的反饋增益會(huì)使舒勒周期振蕩誤差分量得到抑制，但地球周期振蕩誤差仍然存在。當(dāng)然傅科周期振蕩誤差分量也將隨著舒勒周期振蕩的抑制而消失，這將說(shuō)明前面所述的忽略傅科周期振蕩是可以被允許的。如果我們選取兩水平通道的反饋增益相同，即k1=kx1=ky1,k2=kx2=ky2的話，式(13)可以化簡(jiǎn)為:

此時(shí)格網(wǎng)慣導(dǎo)系統(tǒng)東向和北向水平回路的特征多項(xiàng)式均為:

將按式(16)設(shè)計(jì)的水平阻尼系統(tǒng)等效為典型二階系統(tǒng)，根據(jù)系數(shù)對(duì)應(yīng)原則可得到在水平回路上引入反饋增益后慣導(dǎo)系統(tǒng)的阻尼比和自然振蕩角頻率為:

其中，ξ為系統(tǒng)阻尼比，ωn為自然振蕩角頻率。

也就是我們可以通過(guò)動(dòng)態(tài)調(diào)節(jié)k1,k2的取值來(lái)調(diào)節(jié)水平回路中阻尼比的大小。在導(dǎo)航初期較大的阻尼比可以使系統(tǒng)的振蕩誤差快速收斂，進(jìn)而使舒勒周期和傅科周期振蕩誤差得到抑制，從而起到了阻尼的效果；系統(tǒng)達(dá)到穩(wěn)態(tài)后，較小的阻尼比會(huì)減小載體機(jī)動(dòng)對(duì)系統(tǒng)造成的不利影響。

如何動(dòng)態(tài)地調(diào)節(jié)k1,k2取值就成為了本文的研究重點(diǎn)。受Kalman 濾波原理[16]的啟發(fā)，可以將原本的控制問(wèn)題轉(zhuǎn)化為估計(jì)問(wèn)題[17]，通過(guò)求解濾波中的增益矩陣K得到加入到慣導(dǎo)系統(tǒng)中水平回路的反饋增益k1和k2的值。

從圖2中可以看出，格網(wǎng)慣導(dǎo)系統(tǒng)北向水平回路誤差模型和東向水平回路誤差模型是彼此對(duì)稱的，所以我們僅以北向水平回路進(jìn)行分析。

首先將加入反饋增益格網(wǎng)慣導(dǎo)系統(tǒng)北向水平回路模型寫(xiě)成矩陣的形式，表示如下:

其中

通過(guò)式(18)可以看出，慣導(dǎo)系統(tǒng)誤差狀態(tài)變量北向速度誤差和東向失準(zhǔn)角僅受k1和k2的影響。那么我們想使兩個(gè)誤差狀態(tài)變量達(dá)到最小，就需要得到最優(yōu)的反饋增益k1和k2。下面給出求解反饋增益k1和k2的方法。

首先設(shè)計(jì)一個(gè)隨機(jī)的線性系統(tǒng)M 系統(tǒng)，M 系統(tǒng)的系統(tǒng)模型如下所示:

其中，A1、B1、C1分別是M 系統(tǒng)的系統(tǒng)矩陣、輸入矩陣和輸出矩陣，x1、y1分別是系統(tǒng)的狀態(tài)向量和觀測(cè)向量。其中，y1為慣性系統(tǒng)速度與外速度之差。

然后對(duì)M 系統(tǒng)使用龍伯格觀測(cè)器進(jìn)行估計(jì)，可以得到系統(tǒng)狀態(tài)變量的估計(jì)方程為:

如果我們令

則慣導(dǎo)系統(tǒng)誤差狀態(tài)變量xG(t)和M 系統(tǒng)的狀態(tài)變量(t)變化規(guī)律就會(huì)相同；此時(shí)我們可以得到:

那么M 系統(tǒng)就由隨機(jī)線性系統(tǒng)變成了一個(gè)確定的線性系統(tǒng)，并且(t)和xG(t)的變化規(guī)律相同，問(wèn)題也就從尋找使xG(t)達(dá)到最小的k1和k2變成了尋找使(t)達(dá)到最小的增益矩陣K。我們又知道Kalman濾波的原理就是去求解增益矩陣K使得估計(jì)誤差的協(xié)方差達(dá)到最小，那么我們就可以對(duì)M 系統(tǒng)利用Kalman 濾波技術(shù)，求解出增益矩陣K的值。通過(guò)式(24)，我們就進(jìn)而得到了k1和k2的值。此時(shí)求得的反饋增益k1和k2會(huì)使北向速度誤差和東向失準(zhǔn)角的協(xié)方差達(dá)到最小。另外我們考慮到引入外速度的同時(shí)，同樣引入了外速度誤差，我們應(yīng)該實(shí)時(shí)根據(jù)外部速度信息的質(zhì)量來(lái)進(jìn)行調(diào)整，外速度信息有效時(shí)可以使系統(tǒng)處于外速度水平阻尼狀態(tài)，而外速度信息無(wú)效時(shí)，即我們的量測(cè)噪聲Rk?［Rmin,Rmax］，可以自動(dòng)切換至無(wú)阻尼狀態(tài)，因此本文采用簡(jiǎn)化的自適應(yīng)濾波方法，具體實(shí)施過(guò)程如下所示[18-19]:

計(jì)算加權(quán)系數(shù):

一步預(yù)測(cè)均方誤差:

計(jì)算新息序列:

估計(jì)量測(cè)噪聲:

計(jì)算濾波增益:

估計(jì)均方誤差:

狀態(tài)一步預(yù)測(cè):

狀態(tài)估計(jì)計(jì)算:

其中，φk,k-1、Γk-1、Hk是已知的系統(tǒng)結(jié)構(gòu)參數(shù)，分別稱為狀態(tài)一步轉(zhuǎn)移矩陣、系統(tǒng)噪聲分配矩陣和量測(cè)矩陣。給定初值P0,R0,Q0,b,k=1，以及量測(cè)噪聲的界限Rmax,Rmin；置位:k=k+ 1；b為遺忘因子，常取b=0.9 ～0.999。在自適應(yīng)濾波過(guò)程中，量測(cè)噪聲Rk與系統(tǒng)的狀態(tài)和狀態(tài)估計(jì)協(xié)方差交錯(cuò)在線估計(jì)，采用遺忘因子可以限制濾波的記憶長(zhǎng)度，加重新近觀測(cè)數(shù)據(jù)對(duì)現(xiàn)實(shí)估計(jì)的作用，每次更新完量測(cè)噪聲Rk與量測(cè)噪聲界限進(jìn)行比較，若不在規(guī)定的范圍內(nèi)，則視為外速度信息無(wú)效，自動(dòng)將反饋增益賦零使其轉(zhuǎn)換為無(wú)阻尼狀態(tài)。

以上即為通過(guò)自適應(yīng)濾波方法求取格網(wǎng)系統(tǒng)北向回路反饋增益矩陣K的全過(guò)程。將矩陣K中的k1和k2帶入到自適應(yīng)反饋增益北向水平回路中，可以使東向失準(zhǔn)角和北向速度誤差有效抑制，達(dá)到阻尼的效果。同時(shí)也可以利用式(17)求得新阻尼網(wǎng)絡(luò)中的阻尼比，該阻尼比是隨k1和k2變化而變化的，因此進(jìn)一步驗(yàn)證了本方法在格網(wǎng)系統(tǒng)中加入了變阻尼的思想。

同時(shí)我們還需要考慮我們加入的自適應(yīng)反饋增益是否可以使格網(wǎng)北向回路穩(wěn)定?；诰€性系統(tǒng)理論，若系統(tǒng)是可控又可觀測(cè)的，那么該系統(tǒng)就是穩(wěn)定的[20]。所以我們需要進(jìn)一步驗(yàn)證系統(tǒng)的可觀測(cè)性和可控性。

由式(24)所示的系統(tǒng)，其可觀測(cè)性矩陣為

由式(33)可知，該系統(tǒng)的可觀測(cè)性矩陣為滿秩，即系統(tǒng)完全可觀。

由式(24)所示的系統(tǒng)，其可控性矩陣為

由式(34)可知，該系統(tǒng)的可控性矩陣為滿秩，即系統(tǒng)完全可控。這就表明由自適應(yīng)kalman 濾波求解得到的反饋增益k1和k2可使格網(wǎng)北向水平回路達(dá)到穩(wěn)定狀態(tài)。

3 仿真實(shí)驗(yàn)

為了驗(yàn)證所設(shè)計(jì)的格網(wǎng)坐標(biāo)系下自適應(yīng)反饋增益阻尼方法有效性，對(duì)該方法進(jìn)行了36 小時(shí)的仿真驗(yàn)證，并與無(wú)阻尼方法和傳統(tǒng)外速度水平阻尼方法進(jìn)行了對(duì)比。仿真的初始參數(shù)設(shè)置如表1所示，載體沿著120 °經(jīng)線以10 m/s 的速度向北勻速航行。計(jì)程儀參考速度誤差設(shè)置如下:t＜10h或t＞30h時(shí)，計(jì)程儀提供的外速度隨機(jī)誤差波動(dòng)較大，無(wú)法提供準(zhǔn)確的外速度信息；10h≤t≤ 30h 時(shí)，計(jì)程儀提供的外速度誤差較小，可以提供準(zhǔn)確的外速度信息。為了更好地驗(yàn)證自適應(yīng)阻尼算法，本文設(shè)置了計(jì)程儀階段性常值誤差變化，當(dāng)10h≤t≤ 20h 時(shí)，計(jì)程儀提供的外速度誤差為-0.5 m/s；當(dāng)20h≤t≤ 30h 時(shí)，計(jì)程儀提供的外速度誤差為0.5 m/s。仿真中的初始參數(shù)如表1所示。

表1 初始參數(shù)Tab.1 Initial parameters

無(wú)阻尼方法、傳統(tǒng)外速度阻尼方法和本文設(shè)計(jì)的自適應(yīng)反饋增益阻尼方法的誤差結(jié)果對(duì)比如圖3～5所示。圖中藍(lán)線為無(wú)阻尼情況下的誤差曲線，紅線為采用傳統(tǒng)外速度水平阻尼方法的誤差曲線，黑線為采用本文方法的誤差曲線。

圖3 速度誤差對(duì)比圖Fig.3 Velocity error comparison diagram

圖4 位置誤差對(duì)比圖Fig.4 Position error comparison diagram

圖5 失準(zhǔn)角對(duì)比圖Fig.5 Error angle comparison diagram

通過(guò)圖3～5 三種方法誤差曲線的對(duì)比，可以看到在第10 小時(shí)、第20 小時(shí)和第30 小時(shí)，因?yàn)橛?jì)程儀提供的外速度參考信息發(fā)生變化，各導(dǎo)航參數(shù)都產(chǎn)生了超調(diào)變化。尤其是第10 小時(shí)，系統(tǒng)進(jìn)行了阻尼狀態(tài)的切換（由無(wú)阻尼狀態(tài)切換到有阻尼狀態(tài)），超調(diào)較為明顯，采用本文方法產(chǎn)生的超調(diào)變化小于采用傳統(tǒng)阻尼產(chǎn)生的超調(diào)變化并且收斂更快。以速度誤差為例進(jìn)行定量分析，在第10 小時(shí)系統(tǒng)從無(wú)阻尼切換到有阻尼狀態(tài)時(shí)，采用本文方法產(chǎn)生的東向速度誤差超調(diào)量和北向速度誤差超調(diào)量分別比傳統(tǒng)外速度水平阻尼方法產(chǎn)生的超調(diào)量減小13%和15%。

圖6 阻尼比Fig.6 Damping ratio

由圖6可知，在第10 小時(shí)因計(jì)程儀判定外速度信息有效，導(dǎo)航系統(tǒng)自動(dòng)進(jìn)入阻尼狀態(tài)，初期阻尼比很大，隨后逐漸減小并趨于一個(gè)穩(wěn)定值。在第20 小時(shí)，計(jì)程儀提供的外速度誤差發(fā)生變化，系統(tǒng)經(jīng)過(guò)判定仍視為外速度信息為有效信息，但外速度誤差的變化會(huì)導(dǎo)致阻尼比再次發(fā)生變化，當(dāng)系統(tǒng)再次達(dá)到穩(wěn)態(tài)時(shí)，阻尼比將再次趨于一個(gè)穩(wěn)定值。通過(guò)自適應(yīng)方法去自動(dòng)調(diào)整反饋增益參數(shù)，進(jìn)而得到對(duì)系統(tǒng)更合適的阻尼比，所以此時(shí)較小的阻尼比更適合于系統(tǒng)。阻尼比這樣的變化過(guò)程對(duì)慣導(dǎo)系統(tǒng)是有利的，在導(dǎo)航系統(tǒng)初期較大的阻尼比加快了震蕩誤差的收斂速度，系統(tǒng)穩(wěn)態(tài)時(shí)較小的阻尼比有利于載體機(jī)動(dòng)對(duì)該系統(tǒng)造成的不利影響。在第30 小時(shí)，因?yàn)镽k?［Rmin,Rmax］，所以系統(tǒng)判定計(jì)程儀無(wú)法提供準(zhǔn)確的外速度信息，自動(dòng)切換至無(wú)阻尼狀態(tài)。

4 結(jié) 論

本文以極區(qū)航行為背景，針對(duì)傳統(tǒng)格網(wǎng)慣性導(dǎo)航系統(tǒng)阻尼技術(shù)存在的問(wèn)題，提出了一種自適應(yīng)反饋增益阻尼方法。利用該方法將原本的控制問(wèn)題轉(zhuǎn)化為有效估計(jì)問(wèn)題，能夠克服傳統(tǒng)阻尼網(wǎng)絡(luò)參數(shù)選擇的復(fù)雜性問(wèn)題及阻尼網(wǎng)絡(luò)參數(shù)固定的問(wèn)題。實(shí)驗(yàn)仿真結(jié)果表明，所提出方法加快了慣導(dǎo)系統(tǒng)誤差的收斂速度，能夠自主進(jìn)行不同阻尼狀態(tài)間的優(yōu)化切換，而且比傳統(tǒng)外速度水平阻尼誤差超調(diào)量減小了15%左右，提高了系統(tǒng)的導(dǎo)航性能。