朱九鵬，李 安，覃方君，李冬毅

（海軍工程大學(xué)，武漢 430033）

0 引言

慣導(dǎo)系統(tǒng)作為全自主式的導(dǎo)航定位系統(tǒng)，可工作在空中、水面、水下等多種環(huán)境，并輸出姿態(tài)、速度、位置等導(dǎo)航信息。但是由于傳感器誤差的存在，使得長航時工作的慣導(dǎo)系統(tǒng)的誤差發(fā)散，進而影響導(dǎo)航的準確性。目前高精度的慣導(dǎo)系統(tǒng)的應(yīng)用使得重力擾動的影響不能忽略。重力擾動包括垂線偏差和重力異常，重力異常在與姿態(tài)角誤差耦合后形成的等效誤差量級很小，仿真時通常忽略。而垂線偏差是實際重力在當(dāng)?shù)氐乩硐迪屡c正常重力的夾角，造成東向、北向均存在擾動分量，稱為東向、北向重力擾動。目前為抑制重力擾動對慣導(dǎo)系統(tǒng)的影響，常采用重力補償、阻尼抑制等方式。周瀟等采用小波神經(jīng)網(wǎng)絡(luò)的方法進行重力擾動分量的補償，速度、位置誤差精度均有提升；楊功流等采用GPS 和慣導(dǎo)速度誤差組合的方式進行Kalman濾波重力擾動估計從而實現(xiàn)補償；翁海娜等采用傳統(tǒng)固定系數(shù)的超前串聯(lián)校正阻尼網(wǎng)絡(luò)對重力擾動引起的誤差進行抑制。對于長時間工作的水下載體，在未知外部信息的情況下，必須采用自主式的抑制方法。傳統(tǒng)的阻尼網(wǎng)絡(luò)參數(shù)需要經(jīng)驗調(diào)控，而且對于常值、隨機性的重力擾動作用較小，一旦確定不可更改。本文提出利用Kalman 濾波的阻尼方法實現(xiàn)重力擾動誤差的抑制，該方法可以自適應(yīng)調(diào)整阻尼系數(shù)，仿真結(jié)果表明可以很快抑制舒拉振蕩，同時位置、速度誤差精度均有提升。

1 引入重力擾動的慣導(dǎo)誤差模型建立

在以當(dāng)?shù)氐乩碜鴺讼底鳛閷?dǎo)航坐標系，未考慮重力擾動情況下采用正常重力γ進行解算，由于參考橢球面和大地水準面不完全重合的情況，不能直接使用正常重力直接解算，通常采用真實重力g，重力擾動δg的計算如下

式中，γ通常由1980 大地參考系下得到，如式（3）

重力擾動的分量表達式如式（4）所示

式中，η 和ξ 分別為垂線偏差的卯酉面分量和子午面分量；Δg、Δg、Δg分別為重力擾動的東向、北向、天向的分量。

由于艦船運動不考慮天向通道，因此，忽略天向重力擾動即重力異常的影響，只考慮水平方向的重力擾動對導(dǎo)航參數(shù)誤差的影響。具體的姿態(tài)誤差方程、速度誤差方程、位置誤差方程如式（5）所示。

式中，? 為東向、北向、天向姿態(tài)角誤差；δv 為東向、北向速度誤差；δp 為位置誤差；w為地球自轉(zhuǎn)角速度；δg 為東向、北向重力擾動；f為載體系下的加速度計輸出的比力，此處假設(shè)卯酉圈半徑和子午圈半徑相等。

從上述的式子能夠看出，水平重力擾動對導(dǎo)航參數(shù)的影響等效于加速度計誤差，實際上是重力擾動在導(dǎo)航系下的投影影響了加速度計的測量值，進而影響了速度誤差、位置誤差，并與姿態(tài)角誤差進行耦合。為觀察水平重力擾動帶來的誤差，必須先獲得重力擾動的大小，這里利用2190 階的EIGEN-6C4 模型獲取全球各地的水平重力擾動信息。當(dāng)陀螺儀、加速度計精度達到一定量級時，在長時間航行狀態(tài)的慣導(dǎo)中，重力擾動帶來的影響不可被忽略。下面給出不同程度重力擾動對慣導(dǎo)速度、位置誤差的影響。

圖2 不同重力擾動對位置誤差影響

2 基于傳統(tǒng)阻尼網(wǎng)絡(luò)的重力擾動抑制方法

通過上一章的分析可知，重力擾動會使得慣導(dǎo)系統(tǒng)產(chǎn)生振蕩性誤差，尤其對于長航時的慣導(dǎo)系統(tǒng)，長時間的誤差積累會嚴重影響導(dǎo)航的準確性，因此，對振蕩性誤差必須抑制。阻尼技術(shù)作為一種有效的手段經(jīng)常被用來抑制周期性、振蕩性誤差。

根據(jù)傳統(tǒng)阻尼方法對東向水平修正回路分析，得到姿態(tài)、速度、位置誤差的傳遞函數(shù)如下

式中，ΔA為北向加速度計誤差；ε為東向陀螺漂移；δg為北向重力擾動；w為舒拉角頻率。

目前抑制重力擾動的阻尼網(wǎng)絡(luò)的選取一般為超前串聯(lián)校正網(wǎng)絡(luò)，采取的形式如式（9）所示，其中，ξ 為阻尼系數(shù)。

水平阻尼網(wǎng)絡(luò)抑制的是舒拉振蕩分量和傅科周期分量，但是地球周期振蕩則需要由方位阻尼進行抑制。這種由水平阻尼和方位阻尼構(gòu)成的形式稱為內(nèi)全阻尼工作模式。目前采用的方位阻尼系數(shù)一般為0.5 左右，阻尼網(wǎng)絡(luò)的各項參數(shù)則通過試湊法得到，常采用的方位阻尼網(wǎng)絡(luò)如式（10）所示。

3 基于Kalman 濾波阻尼的重力擾動抑制方法

從上一章傳統(tǒng)阻尼網(wǎng)絡(luò)形式可知，阻尼的存在使得3 種振蕩性分量均得到抑制，并且阻尼系數(shù)越大，振蕩性誤差的收斂速度越快。但是從其結(jié)構(gòu)能夠看出，傳統(tǒng)阻尼網(wǎng)絡(luò)的參數(shù)確定之后難以更改，控制參數(shù)需要根據(jù)經(jīng)驗得到，不具有自適應(yīng)性。同時，艦船在長航時狀態(tài)下，重力擾動并非定值而是呈現(xiàn)非規(guī)律性、隨機性的變化，固定的阻尼系數(shù)針對常值重力擾動的誤差抑制具有一定作用，但是在變化的重力擾動下，效果并不明顯。因此，必須對不同程度的重力擾動進行不同程度的阻尼，以抑制重力擾動所導(dǎo)致的舒拉振蕩性的誤差，即采用變阻尼的方法實現(xiàn)不同程度的重力擾動抑制。本文采用一種基于Kalman 濾波阻尼的重力擾動抑制方法，反饋參數(shù)由Kalman 直接得到，并不需要反復(fù)調(diào)試。Kalman 濾波器是基于均方誤差最小原理，使得輸出的反饋系數(shù)具有最優(yōu)性，同時減小誤差的穩(wěn)態(tài)值。

首先以東向水平修正回路為例，在如圖4 所示的回路中可以得到臨界穩(wěn)定的東向回路，如式（11）

為實現(xiàn)系統(tǒng)的穩(wěn)定，將系統(tǒng)的極點全部配置到左半平面內(nèi)，因此，引入輸出反饋的方式，引入輸出反饋的東向回路的原理圖如圖3 所示，圖中，k、k為反饋回路的比例系數(shù)，δv為外部傳感器參考速度誤差。

圖3 引入輸入反饋的東向水平修正回路

引入輸入反饋后，東向回路的速度、姿態(tài)誤差的微分方程則改變?yōu)槭剑?2）

通過對上式方程分析，速度、姿態(tài)誤差的微分方程均為二階方程進行拉普拉斯變換后同為二次多項式，即誤差傳遞函數(shù)為二階系統(tǒng)。要想使系統(tǒng)由臨界穩(wěn)定變成穩(wěn)定，則必須保證極點全部位于左半平面，即二階系統(tǒng)分母根均小于0。根據(jù)分析，若要求根均小于0，則必保證反饋系數(shù)k>0，k＜0。根據(jù)自動控制理論，該二階系統(tǒng)的等效阻尼比ξ 以及無阻尼自振蕩頻率w滿足式（13）

通過對東向水平回路引入輸入反饋系數(shù)實現(xiàn)阻尼比的自適應(yīng)調(diào)節(jié)，初始時具有較大的阻尼比使振蕩誤差快速收斂，振蕩誤差基本消失時，阻尼比同樣收斂。為實現(xiàn)k、k的自適應(yīng)調(diào)節(jié)，從而減小速度、姿態(tài)誤差，因此，引入Kalman 濾波器實現(xiàn)求得使得誤差最小的濾波增益矩陣，從而得到反饋回路的控制參數(shù)。

艦船在運動時重力擾動才會呈現(xiàn)出非規(guī)律性的變化，因此，需要考慮慣導(dǎo)在動基座的情況下的誤差方程，將式（5）～式（7）以及引入輸入反饋后，變換成矩陣形式如下式

其中，

假定某線性系統(tǒng)結(jié)構(gòu)如下

則根據(jù)Kalman 濾波器原理可得估計誤差的方程為式（16）

引入外速度誤差下的Kalman 濾波器以參考速度誤差作為觀測量，因此，有

對比式（15）和式（17）則可以得到

當(dāng)式（16）代表的系統(tǒng)滿足式（18）時，可將動基座下的慣導(dǎo)誤差量x視為Kalman 濾波器的估計誤差進行處理，由濾波器求得的增益矩陣K 滿足均方誤差最小的性質(zhì)，此時K 中的每一行元素代表每個反饋回路的比例系數(shù)。由于只需要求解增益矩陣即可，因此，采用簡化后的Kalman 濾波方程，如式（18）所示

式（18）為離散化處理后的Kalman 濾波器方程，T 為濾波周期；P（k，k-1）為一步預(yù)測方差陣；P（k）為狀態(tài)估計均方誤差陣；R（k）為量測噪聲方差陣，由DVL 測得的外速度誤差的方差決定；Q（k）為過程噪聲方差陣，由陀螺和加速度計以及水平重力擾動的噪聲方差決定。

通過對系數(shù)矩陣A 的分析可知，緯度誤差幾乎始終貫穿其他誤差，相對其他誤差影響較大，因此，在考慮動基座情況下不對緯度誤差回路進行輸入反饋，即使k=k=0。

4 仿真驗證

根據(jù)對圖1 的阻尼回路，使慣導(dǎo)工作在內(nèi)全阻尼的模式下，對于式（10）所示的阻尼網(wǎng)絡(luò)取ξ=0.5；針對慣導(dǎo)常見誤差值進行阻尼方法的驗證，加速度計常值零偏取ΔA=9.78em/s，ΔA=9.78em/s；加速度隨機零偏取ΔA=9.78em/s；陀螺常值漂移取ε=0.003°/h，ε=0.003°/h，ε=0.003°/h；陀螺隨機漂移取ε=0.000 2°/h；初始速度誤差為δv=0.05 m/s，δv=0.05 m/s；初 始 失 準 角 分 別 為：?=3'，?=3'，?=5'；艦船東向、北向航速v=0 m/s，v=3 m/s；艦船初始位置為20°N，203°E，艦船航行總時間為24 h，仿真步長0.1 s，為反映重力擾動變化，下面給出艦船運動的航跡上水平重力擾動的變化趨勢，仿真所用重力擾動數(shù)據(jù)分辨率1'×1'，仿真結(jié)果如圖4 所示。

圖1 不同重力擾動對速度誤差影響

圖4 水平重力擾動曲線變化圖

對Kalman 濾波器初始值進行設(shè)計，均方誤差陣為初始各導(dǎo)航參數(shù)誤差P=diag（0.05 m/s 0.05 m/s 0.001 rad 0.001 rad 3' 3' 5'），過程噪聲陣的設(shè)置按照上面Q=diag（1 μg 1 μg 0 0 0.003°/h 0.003°/h 0.003°/h），R 為DVL 的噪聲方差，這里為量測噪聲方差R=diag（0.005 m/s 0.005 m/s），濾波周期T=1 s。采用本文方法效果和超前串聯(lián)校正阻尼網(wǎng)絡(luò)方法如圖5～圖11 所示。

圖5 東向速度誤差結(jié)果對比圖

圖6 北向速度誤差結(jié)果對比圖

圖11 天向姿態(tài)角誤差結(jié)果對比圖

1）通過對表1 中速度誤差的分析，很明顯舒拉振蕩和傅科振蕩都被很好地抑制，初始2 h 內(nèi)東向、北向速度誤差較大的超調(diào)量，而后不斷減小直至收斂；而Kalman 濾波阻尼方法則能夠使振蕩誤差收斂加快，效果完全優(yōu)于超前串聯(lián)校正阻尼網(wǎng)絡(luò)；后期振蕩誤差收斂之后，由于水平重力擾動誤差量級大于高精度慣導(dǎo)系統(tǒng)慣性器件誤差，且非規(guī)律變化，因此，兩者變化趨勢都呈現(xiàn)出波動狀，但是Kalman 濾波阻尼方法能夠使速度誤差幾乎保持在0 m/s 附近，而超前串聯(lián)校正阻尼網(wǎng)絡(luò)則呈現(xiàn)出較大的波動，經(jīng)過阻尼后Kalman 濾波阻尼方法能夠使速度誤差降低約0.8 m/s。

圖7 緯度誤差結(jié)果對比圖

圖8 經(jīng)度誤差結(jié)果對比圖

圖9 東向姿態(tài)角誤差結(jié)果對比圖

圖10 北向姿態(tài)角誤差結(jié)果對比圖

表1 導(dǎo)航參數(shù)誤差均方值（RMSE）及誤差降低百分比

2）對于位置誤差，同樣地振蕩性誤差均被抑制，但是緯度誤差整體呈現(xiàn)出較大的波動，說明超前串聯(lián)校正阻尼網(wǎng)絡(luò)的地球周期在一個周期內(nèi)沒有被完全抑制，且最大的緯度誤差可達6 000 m，而Kalman 濾波阻尼方法最大1 000 m，且一個地球周期內(nèi)就被完全地抑制，緯度誤差降低約3 000 m；經(jīng)度誤差依舊呈發(fā)散趨勢，但是從兩種方法的比較來看，Kalman 濾波阻尼方法明顯優(yōu)于超前串聯(lián)校正阻尼網(wǎng)絡(luò)，兩者之間的差距較大；從上述緯度誤差和經(jīng)度誤差來看，Kalman 濾波阻尼方法能夠非常有效地降低位置誤差，并且減弱經(jīng)度誤差的發(fā)散趨勢。

3）對于姿態(tài)誤差角的變化，東向和北向的誤差角最后收斂的趨勢比較一致，均方誤差變化不大，差異主要在前期抑制振蕩性誤差的速度，Kalman 濾波阻尼方法比超前串聯(lián)校正阻尼網(wǎng)絡(luò)要快近2 h。天向誤差角的抑制效果要明顯一些，Kalam 濾波阻尼方法降低約1.7'。

5 結(jié)論

隨著慣性器件的制作工藝提高，加速度計和陀螺儀的精度也隨之提升，高精度慣導(dǎo)系統(tǒng)在航海領(lǐng)域應(yīng)用越來越廣泛，重力擾動的數(shù)量級相比于加速度計零偏也變得不可忽略，甚至超過了慣性器件誤差的固有影響。因此，必須對其進行抑制或者補償，對于未知重力擾動信息或者難以獲得重力信息的情況下，阻尼抑制方法作為一種手段非常有效。本文首先建立了考慮重力擾動的慣導(dǎo)誤差模型，分析了重力擾動對導(dǎo)航參數(shù)誤差的影響。針對超前相位串聯(lián)阻尼網(wǎng)絡(luò)不適用于隨機性、非規(guī)律性的水平重力擾動，采用基于Kalman 的變阻尼技術(shù)，通過引入反饋回路，改變系統(tǒng)穩(wěn)定性，再根據(jù)Kalman 濾波器的原理，求解反饋回路的控制系數(shù)，實現(xiàn)等效阻尼比的自適應(yīng)性變化。通過仿真進行驗證該方法的有效性，仿真結(jié)果表明，基于Kalman 濾波的阻尼方法除了可以快速、有效抑制振蕩性誤差，還可以有效地降低重力擾動帶來的速度、位置誤差。24 h 的仿真結(jié)果表明，速度誤差能減小約0.8 m/s，降低75.46%，位置誤差減小約12 000 m，降低83.32%，天向誤差減小約1.7'，降低約65%。