基于改進(jìn)Sage-Husa 自適應(yīng)濾波算法的SINS/DVL 組合導(dǎo)航*

朱天高，劉 勇，李文魁*，樓鑒路，祝西枰

（1.海軍工程大學(xué)，武漢 430000；2.海警局南海分局，廣東 深圳 518048）

0 引言

自主水下航行器（autonomous underwater vehicle，AUV）在軍事和民用方面均得到廣泛的應(yīng)用，在AUV 在水下執(zhí)行任務(wù)時(shí)，精確、自主、可靠的導(dǎo)航系統(tǒng)是關(guān)鍵技術(shù)［1-2］。由于在水下工作，AUV 無(wú)法接收衛(wèi)星導(dǎo)航信號(hào)。捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system，SINS）和多普勒計(jì)程儀（doppler velocity log，DVL）組合導(dǎo)航是目前應(yīng)用較多的水下導(dǎo)航算法［3］。

組合導(dǎo)航系統(tǒng)信息處理的核心是導(dǎo)航濾波器［4］。在水下航行過(guò)程中，難免遇到魚(yú)群、水下地形等復(fù)雜環(huán)境，從而導(dǎo)致DVL 數(shù)據(jù)出現(xiàn)異常。若遇到此等情況，常用的手段是通過(guò)自適應(yīng)濾波對(duì)量測(cè)噪聲矩陣進(jìn)行相應(yīng)的在線調(diào)整，一定程度上抵消DVL精度下降帶來(lái)的負(fù)面影響［5-6］。而在自適應(yīng)濾波過(guò)程中，漸消因子對(duì)于系統(tǒng)參數(shù)估計(jì)性能的影響比較大，以往的自適應(yīng)濾波中漸消因子都是事先根據(jù)經(jīng)驗(yàn)進(jìn)行設(shè)定的，若遇到高動(dòng)態(tài)的復(fù)雜外界環(huán)境，難以及時(shí)有效對(duì)噪聲矩陣進(jìn)行在線估計(jì)［7-10］。

針對(duì)以上問(wèn)題，本文提出一種基于漸消因子動(dòng)態(tài)調(diào)整的改進(jìn)Sage-Husa 自適應(yīng)濾波，引入量測(cè)故障檢測(cè)與調(diào)整機(jī)制，解決自適應(yīng)濾波估計(jì)量測(cè)噪聲參數(shù)性能對(duì)漸消因子依賴性強(qiáng)的問(wèn)題。當(dāng)觀測(cè)量存在異常時(shí)對(duì)量測(cè)矩陣進(jìn)行故障檢測(cè)與在線調(diào)整，從而抑制誤差。最后，通過(guò)實(shí)測(cè)數(shù)據(jù)驗(yàn)證了對(duì)于組合導(dǎo)航系統(tǒng)，該自適應(yīng)濾波算法能夠有效抑制復(fù)雜外界環(huán)境導(dǎo)致的DVL 測(cè)速失準(zhǔn)帶來(lái)的定位誤差，當(dāng)DVL 測(cè)速精度明顯降低時(shí)，實(shí)現(xiàn)定位精度的保持。

1 SINS/DVL 組合導(dǎo)航數(shù)學(xué)模型

1.1 SINS 姿態(tài)誤差微分方程

經(jīng)典SINS 微分方程包含姿態(tài)矩陣微分方程、速度微分方程和位置微分方程，分別如下

由于實(shí)際應(yīng)用中存在各種誤差，理想導(dǎo)航坐標(biāo)系n'系和計(jì)算導(dǎo)航坐標(biāo)系系之間存在一定的轉(zhuǎn)動(dòng)誤差，記n 系和n'系之間的3 次轉(zhuǎn)動(dòng)角為。根據(jù)文獻(xiàn)［5］，可得SINS 非線性誤差微分方程為

1.2 DVL 誤差模型

本文中認(rèn)為多普勒計(jì)程儀安裝角誤差在組合導(dǎo)航之前已經(jīng)進(jìn)行了標(biāo)定，多普勒計(jì)程儀的測(cè)量誤差主要為刻度系數(shù)誤差，則速度表達(dá)式為

其中，vD為實(shí)際速度；δKD為刻度系數(shù)因子，由于b 系和m 系重合，則載體坐標(biāo)系下的多普勒計(jì)程儀速度為

建議可區(qū)分ABC類設(shè)施，確定建設(shè)方向和投資比例。A類為重點(diǎn)投資設(shè)施（例如：機(jī)房、值班室、辦公室、門(mén)廳配套及裝飾），B類為必須投資設(shè)施（例如：基礎(chǔ)、結(jié)構(gòu)、綜合管線，消防，防水，隔熱），C類為可節(jié)省投資設(shè)施（例如：樓梯間、休息室、食堂、地下室裝潢修繕）。

投影到導(dǎo)航系下的多普勒計(jì)程儀速度為

1.3 SINS/DVL 誤差模型

SINS/DVL 采用速度組合方式，即采用DVL 速度信息輔助慣導(dǎo)，將DVL 和慣導(dǎo)輸出的速度差值作為量測(cè)值，利用卡爾曼濾波器對(duì)慣導(dǎo)系統(tǒng)誤差進(jìn)行估計(jì)，然后對(duì)慣導(dǎo)誤差進(jìn)行修正。組合導(dǎo)航中，一般需要對(duì)器件誤差進(jìn)行建模估計(jì)，此處考慮陀螺漂移、加速度計(jì)零偏和DVL 刻度系數(shù)誤差，且認(rèn)為其為常值，因此，系統(tǒng)的狀態(tài)量取為

系統(tǒng)狀態(tài)方程為

其中，F(xiàn)SINS/DVL為狀態(tài)轉(zhuǎn)移矩陣，可由上文的SINS 誤差模型和DVL 誤差模型得出；w 為系統(tǒng)噪聲；G 為系統(tǒng)噪聲矩陣。

取慣導(dǎo)與DVL 的速度之差為組合導(dǎo)航觀測(cè)量，即：

系統(tǒng)觀測(cè)方程為

2 SINS/DVL 組合導(dǎo)航濾波算法設(shè)計(jì)

由于姿態(tài)誤差陣的存在，速度誤差方程為非線性的，因此，考慮對(duì)SINS/DVL 組合導(dǎo)航系統(tǒng)采用UKF 方法進(jìn)行濾波，在此基礎(chǔ)上通過(guò)自適應(yīng)濾波算法對(duì)噪聲的量測(cè)矩陣進(jìn)行實(shí)時(shí)估計(jì)修正，達(dá)到有效抑制定位誤差的目的。

2.1 UKF 濾波

對(duì)卡爾曼濾波算法中的一步預(yù)測(cè)方程使用UT變換來(lái)處理均值和協(xié)方差的非線性傳遞，就得到了UKF 算法。對(duì)于非線性誤差模型，標(biāo)準(zhǔn)的UKF 濾波算法如下：

1）初始化

2）Sigma 點(diǎn)計(jì)算與時(shí)間更新

3）量測(cè)更新

濾波增益矩陣為

狀態(tài)估計(jì)及其均方誤差陣為

當(dāng)量測(cè)模型為線性時(shí)，量測(cè)更新過(guò)程可簡(jiǎn)化為

式中，Hk為線性量測(cè)矩陣。

2.2 Sage-Huga 自適應(yīng)濾波改進(jìn)

Sage-Huga 自適應(yīng)濾波是在卡爾曼濾波的基礎(chǔ)上加入時(shí)變?cè)肼暤臑V波，以實(shí)時(shí)估計(jì)并修正量測(cè)噪聲矩陣，從而減小模型誤差，抑制濾波器發(fā)散，進(jìn)而提高濾波精度。利用上節(jié)講述的UKF 濾波算法，Sage-Huga 自適應(yīng)濾波算法如下：

其中，b（0＜b＜1）稱為漸消因子。b 取得越小，則系統(tǒng)對(duì)于新量測(cè)噪聲變化的自適應(yīng)能力越強(qiáng)，常取b=0.9～0.999。然而，在高動(dòng)態(tài)的變化環(huán)境中，b 保持常值對(duì)于系統(tǒng)的自適應(yīng)能力有很大的消極影響，不利于系統(tǒng)保持高精度的導(dǎo)航定位。

因此，為了增強(qiáng)濾波的自適應(yīng)能力，本文提出基于一種基于漸消因子動(dòng)態(tài)調(diào)整的改進(jìn)Sage-Husa自適應(yīng)濾波，具體如下：

其中，κ 為常值，由通過(guò)經(jīng)驗(yàn)值進(jìn)行設(shè)定，可控制漸消因子的變化速度。在濾波過(guò)程中，當(dāng)量測(cè)噪聲發(fā)生變化時(shí)，可通過(guò)對(duì)u 濾波的狀態(tài)進(jìn)行判斷。若u的值較大，說(shuō)明量測(cè)噪聲變化較大，濾波異常，則調(diào)節(jié)漸消因子取較小的值以提高對(duì)量測(cè)噪聲矩陣的跟蹤速度；當(dāng)量測(cè)噪聲變化較小且比較穩(wěn)定時(shí)，u 值相應(yīng)較小，漸消因子的取值適當(dāng)增大，以便能夠更精確地估計(jì)出量測(cè)噪聲矩陣。通過(guò)本文提出的基于漸消因子的自適應(yīng)濾波，系統(tǒng)的魯棒性與準(zhǔn)確性大大增強(qiáng)，更有利于實(shí)際的導(dǎo)航應(yīng)用。

3 實(shí)驗(yàn)與結(jié)果分析

下面通過(guò)實(shí)驗(yàn)船實(shí)測(cè)數(shù)據(jù)驗(yàn)證設(shè)計(jì)的基于非線性誤差模型的SINS/DVL 自適應(yīng)組合濾波算法的有效性。測(cè)量設(shè)備包括IMU、DVL 和GPS 接收機(jī)，記錄4 000 s 的實(shí)驗(yàn)數(shù)據(jù)。IMU 參數(shù)為：陀螺零偏為0.02°/h，加速度計(jì)零偏穩(wěn)定性為50 μg，采樣頻率200 Hz。DVL 的參數(shù)為：測(cè)速精度約為刻度因子誤差為1%，采樣頻率1 Hz。GPS 水平定位精度10 m，測(cè)速精度0.1 m/s，采樣頻率1 Hz。以SINS/GNSS 組合濾波輸出作為參考基準(zhǔn)，對(duì)應(yīng)的實(shí)驗(yàn)軌跡圖如圖1 中黑色曲線所示。為了更好模擬復(fù)雜的海洋環(huán)境，設(shè)置初值失準(zhǔn)角為，且對(duì)DVL 速度疊加2 m/s 的速度誤差隨機(jī)噪聲，以模擬較差的觀測(cè)輸入。

圖1 實(shí)驗(yàn)軌跡圖Fig.1 Experimental trajectory diagram

基于非線性線性誤差模型，采用常規(guī)UKF 濾波，Sage-Husa 自適應(yīng)UKF 濾波以及本文提出的基于漸消因子的自適應(yīng)濾波等3 種組合模式進(jìn)行SINS/DVL 組合導(dǎo)航，并對(duì)估計(jì)誤差進(jìn)行對(duì)比分析。實(shí)驗(yàn)采用的組合導(dǎo)航卡爾曼濾波如下：

UKF：無(wú)跡卡爾曼濾波

AUKF：Sage-Husa 無(wú)跡自適應(yīng)卡爾曼濾波

SAUKF：基于漸消因子的無(wú)跡自適應(yīng)卡爾曼濾波

圖2～圖4 分別為3 種組合濾波的SINS/DVL組合導(dǎo)航姿態(tài)誤差曲線。由圖可知，對(duì)于3 種模型，都表現(xiàn)為水平誤差角明顯比方位誤差角收斂速度快，方位誤差角收斂則較為緩慢，這主要是因?yàn)镈VL提供的是載體系下的速度觀測(cè)值，SINS/DVL 方位誤差角的可觀測(cè)度較弱。對(duì)4 種組合模型的性能比較，SAUKF 最優(yōu)，AUKF 次之，UKF 最差。改進(jìn)的算法相比較原Sage-Husa 自適應(yīng)濾波算法在2 500 s 處可提升航向角精度約30%，本文提出的基于漸消因子動(dòng)態(tài)調(diào)整的的Sage-Husa 自適應(yīng)卡爾曼具有更優(yōu)的性能。由此可知，3 種濾波中，SAUKF 性能最優(yōu)，對(duì)姿態(tài)估計(jì)最為準(zhǔn)確。

圖2 縱傾角誤差Fig.2 Trim angle error

圖4 航向角誤差Fig.4 Course angle error

圖5～圖6 分別為東向速度誤差和北向速度誤差，圖7～圖8 分別為緯度誤差和經(jīng)度誤差。由圖可知，與其他兩種組合濾波相比，SAUKF 濾波算法可獲得更高的姿態(tài)角估計(jì)精度，進(jìn)而提高了速度和位置的精度，相比于AUKF 和UKF 濾波整體優(yōu)勢(shì)較大，尤其在2 500 s～4 000 s 時(shí)SAUKF 濾波的速度估計(jì)優(yōu)勢(shì)更為明顯。同時(shí)圖1 也給出了3 種濾波以及參考軌跡對(duì)比圖，可明顯看出，SAUKF 軌跡最接近參考軌跡，而AUKF 濾波和UKF 濾波則離參考軌跡較遠(yuǎn)。

圖5 東向速度誤差Fig.5 Eastward velocity error

圖6 北向速度誤差Fig.6 Northern velocity error

圖7 緯度誤差Fig.7 Latitude error

圖8 經(jīng)度誤差Fig.8 Longitude error

4 結(jié)論

SINS/DVL 組合導(dǎo)航是水下導(dǎo)航的最常用方式之一。但在實(shí)際應(yīng)用過(guò)程中，水下環(huán)境復(fù)雜多變從而出現(xiàn)DVL 數(shù)據(jù)異常，導(dǎo)致組合導(dǎo)航精度下降。Sage-Husa 自適應(yīng)卡爾曼濾波常用于解決此類問(wèn)題，但該自適應(yīng)濾波估計(jì)量測(cè)噪聲參數(shù)性能對(duì)漸消因子依賴性較強(qiáng)，常用的自適應(yīng)濾波中漸消因子往往為事先設(shè)定的常值，難以應(yīng)對(duì)高動(dòng)態(tài)的復(fù)雜水下環(huán)境。針對(duì)此情況，本文提出一種基于漸消因子動(dòng)態(tài)調(diào)整的改進(jìn)自適應(yīng)濾波，通過(guò)引入漸消因子的變化函數(shù)，對(duì)漸消因子進(jìn)行相應(yīng)的動(dòng)態(tài)調(diào)節(jié)，克服了常值漸消因子對(duì)高動(dòng)態(tài)復(fù)雜水下環(huán)境跟蹤能力差的局限性，從而提高濾波算法的估計(jì)精度和魯棒性，在實(shí)際工程應(yīng)用中具有一定的應(yīng)用價(jià)值。