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

（海軍工程大學，武漢 430033）

0 引言

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

1 引入重力擾動的慣導誤差模型建立

在以當地地理坐標系作為導航坐標系，未考慮重力擾動情況下采用正常重力γ進行解算，由于參考橢球面和大地水準面不完全重合的情況，不能直接使用正常重力直接解算，通常采用真實重力g，重力擾動δg的計算如下

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

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

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

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

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

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

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

2 基于傳統阻尼網絡的重力擾動抑制方法

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

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

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

目前抑制重力擾動的阻尼網絡的選取一般為超前串聯校正網絡，采取的形式如式（9）所示，其中，ξ 為阻尼系數。

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

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

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

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

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

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

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

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

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

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

其中，

假定某線性系統結構如下

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

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

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

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

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

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

4 仿真驗證

根據對圖1 的阻尼回路，使慣導工作在內全阻尼的模式下，對于式（10）所示的阻尼網絡取ξ=0.5；針對慣導常見誤差值進行阻尼方法的驗證，加速度計常值零偏取Δ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，為反映重力擾動變化，下面給出艦船運動的航跡上水平重力擾動的變化趨勢，仿真所用重力擾動數據分辨率1'×1'，仿真結果如圖4 所示。

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

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

對Kalman 濾波器初始值進行設計，均方誤差陣為初始各導航參數誤差P=diag（0.05 m/s 0.05 m/s 0.001 rad 0.001 rad 3' 3' 5'），過程噪聲陣的設置按照上面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。采用本文方法效果和超前串聯校正阻尼網絡方法如圖5～圖11 所示。

圖5 東向速度誤差結果對比圖

圖6 北向速度誤差結果對比圖

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

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

圖7 緯度誤差結果對比圖

圖8 經度誤差結果對比圖

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

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

表1 導航參數誤差均方值（RMSE）及誤差降低百分比

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

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

5 結論

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