基于Kalman 濾波的慣導(dǎo)系統(tǒng)重力擾動(dòng)阻尼抑制方法*

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

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

0 引言

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

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

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

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

重力擾動(dòng)的分量表達(dá)式如式（4）所示

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

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

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

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

圖2 不同重力擾動(dòng)對(duì)位置誤差影響

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

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

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

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

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

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

3 基于Kalman 濾波阻尼的重力擾動(dòng)抑制方法

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

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

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

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

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

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

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

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

其中，

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

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

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

對(duì)比式（15）和式（17）則可以得到

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

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

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

4 仿真驗(yàn)證

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

圖1 不同重力擾動(dòng)對(duì)速度誤差影響

圖4 水平重力擾動(dòng)曲線變化圖

對(duì)Kalman 濾波器初始值進(jìn)行設(shè)計(jì)，均方誤差陣為初始各導(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 的噪聲方差，這里為量測(cè)噪聲方差R=diag（0.005 m/s 0.005 m/s），濾波周期T=1 s。采用本文方法效果和超前串聯(lián)校正阻尼網(wǎng)絡(luò)方法如圖5～圖11 所示。

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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