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

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

0 引言

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

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

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

1 SINS/DVL 組合導航數(shù)學模型

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

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

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

1.2 DVL 誤差模型

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

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

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

投影到導航系下的多普勒計程儀速度為

1.3 SINS/DVL 誤差模型

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

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

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

取慣導與DVL 的速度之差為組合導航觀測量，即：

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

2 SINS/DVL 組合導航濾波算法設計

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

2.1 UKF 濾波

對卡爾曼濾波算法中的一步預測方程使用UT變換來處理均值和協(xié)方差的非線性傳遞，就得到了UKF 算法。對于非線性誤差模型，標準的UKF 濾波算法如下：

1）初始化

2）Sigma 點計算與時間更新

3）量測更新

濾波增益矩陣為

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

當量測模型為線性時，量測更新過程可簡化為

式中，Hk為線性量測矩陣。

2.2 Sage-Huga 自適應濾波改進

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

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

因此，為了增強濾波的自適應能力，本文提出基于一種基于漸消因子動態(tài)調整的改進Sage-Husa自適應濾波，具體如下：

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

3 實驗與結果分析

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

圖1 實驗軌跡圖Fig.1 Experimental trajectory diagram

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

UKF：無跡卡爾曼濾波

AUKF：Sage-Husa 無跡自適應卡爾曼濾波

SAUKF：基于漸消因子的無跡自適應卡爾曼濾波

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

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

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

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

圖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 結論

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