SINS/DVL/AST水下組合導航中的魯棒信息融合方法*

朱 兵，常國賓，何泓洋，許江寧

(1. 北京跟蹤與通信技術研究所, 北京 100094； 2. 中國礦業(yè)大學 環(huán)境與測繪學院, 江蘇 徐州 221116；3. 海軍工程大學 電氣工程學院, 湖北 武漢 430033)

捷聯式慣導系統(tǒng)(Strapdown Inertial Navigation System，SINS)由于隱蔽性高、自主性和抗干擾能力強、結構簡單和成本低等諸多優(yōu)勢，在航空、航天、航海等領域受到廣泛關注和應用[1-4]，已成為現代慣性導航技術研究和發(fā)展的主流[5]，并成為自主式水下航行器(Autonomous Underwater Vehicle，AUV)長航時、長航程、高精度導航的主要手段。但是，SINS導航誤差隨著時間不斷積累，借助外部導航傳感器輔助SINS以實現對導航誤差的補償是實現AUV水下長周期精確導航的關鍵[6]。深水環(huán)境對電磁波、無線電等信號有拒止屏蔽作用，致使無線電導航、衛(wèi)星導航等手段無法正常使用[7]。多普勒計程儀(Doppler Velocity Log，DVL)測速精度穩(wěn)定，可為SINS提供外部速度輔助信息；水下聲學定位單應答器成本低、布設靈活以及局部水域定位精度高，可在局部水域為SINS提供外部位置輔助信息[8-9]。

AUV在深水環(huán)境中航行時，主要手段是以DVL的測速信息、水下聲學定位系統(tǒng)(Acoustic Positioning System，APS)或聲學單應答器(Acoustic Single-Transponder，AST)的定位信息輔助SINS進行導航。但是，SINS/DVL組合導航方式得到的定位誤差隨著時間是發(fā)散的[10]，這就需要適時地利用AST的定位信息來限制SINS/DVL導航誤差的發(fā)散以提高AUV縱深航行的能力。對于水下多導航傳感器的信息，需利用有效、可靠的信息融合方法以提高導航系統(tǒng)的導航精度和容錯能力。相比于集中式卡爾曼濾波(Centralized Kalman Filter，CKF)算法，聯邦卡爾曼濾波(Federated Kalman Filter，FKF)算法[11-13]具有結構靈活、計算量小、容錯能力強等優(yōu)勢，使其在組合導航系統(tǒng)中受到廣泛研究和應用[14-17]。信息分配系數是決定FKF結構和性能的重要因素[17]，傳統(tǒng)的FKF(Traditional FKF，TFKF)在子濾波器之間按固定比例分配信息[13]，即選取信息分配系數βi為固定常值。在此種分配方式下，某個子濾波器性能下降可能會對TFKF的全局濾波性能造成較大影響。同時，水下環(huán)境的復雜性致使AST的定位信息及DVL的測速信息易受到諸如野值等非高斯噪聲的污染，這會導致TFKF濾波性能下降，甚至濾波發(fā)散。

針對上述問題，本文提出基于馬氏距離(Mahalanobis Distance，MD)算法的聯邦魯棒卡爾曼濾波(Federated Robust Kalman Filter，FRKF)算法。FRKF利用MD算法對觀測量進行辨識，若判斷觀測量異常，則通過引入膨脹因子對量測噪聲協(xié)方差陣進行膨脹以減小卡爾曼濾波增益，從而提高組合導航系統(tǒng)的魯棒性。在FRKF中，基于子濾波器濾波性能自適應地確定信息分配系數，進而確保FRKF整體的濾波精度。

1 SINS/DVL/AST組合導航濾波模型

SINS/DVL/AST組合導航系統(tǒng)的結構如圖1所示。由圖1可看出，AUV水下組合導航系統(tǒng)主要由SINS、DVL和AST等導航傳感器組成。各個導航傳感器提供相應的導航信息，利用FKF將這些導航信息進行融合，進而得到更高精度、更加可靠的導航信息。SINS/DVL/AST組合導航原理如圖2所示。

圖1 水下組合導航系統(tǒng)結構Fig.1 Structure of underwater integrated navigation system

圖2 SINS/DVL/AST組合導航原理示意圖Fig.2 Schematic diagram of SINS/DVL/AST integrated navigation

由圖2可以看出，當AUV在水下機動航行時，為了抑制SINS位置誤差的持續(xù)發(fā)散可設計以下組合導航方案：當AUV進入AST的作用區(qū)域時，采用SINS/DVL/AST組合導航模式，此時SINS的導航誤差保持穩(wěn)定；當AUV離開AST的作用區(qū)域時，采用SINS/DVL組合導航模式，此時AUV在DVL測速輔助條件下導航誤差緩慢發(fā)散，在即將達到誤差上限時，使得AUV能夠再次進入AST作用區(qū)域。以上方案可最大限度地保證AUV水下導航的精度。

在FKF中，由于SINS能夠全面給出AUV的姿態(tài)、速度和位置信息，因此選取SINS作為公共參考系統(tǒng)。SINS的高度通道是獨立、發(fā)散的，其高度通道信息可借助外部傳感器如水壓計等準確獲得。記導航坐標系為n系，載體坐標系為b系，慣性坐標系為i系，地球坐標系為e系，計算坐標系為n′系。因此不考慮高度通道的速度、位置信息，選取SINS狀態(tài)量為：

XSINS=[δL,δλ,δvE,δvN,αx,αy,αz,

(1)

(2)

假設AST定位系統(tǒng)已經過校準修正，此時系統(tǒng)誤差源可以忽略不計，可認為由噪聲引起的誤差是整個定位系統(tǒng)水聲定位誤差的主要來源[18]。也就是說，若定位系統(tǒng)已通過校準補償，定位系統(tǒng)誤差的主要來源可認為是隨機誤差。基于AST的定位信息pAST=[LAST，λAST，hAST]，選取緯度誤差δL和經度誤差δλ作為SINS/AST組合導航子系統(tǒng)的觀測量，則有：

(3)

式中，位置量測噪聲Vp～N(0,Rp)，Rp為位置量測噪聲陣；量測矩陣Hp=[I2×2,02×11]。

2 FRKF算法

2.1 FKF魯棒化

記SINS/DVL組合導航系統(tǒng)對應的子濾波器為“子濾波器1”；SINS/AST組合導航系統(tǒng)對應的子濾波器為“子濾波器2”。由于各個觀測源相互獨立，k時刻FKF的量測更新方程[19]為：

(4)

(5)

(6)

Pi,k=(I-Ki,kHi,k)Pi,k|k-1

(7)

其中：μi,k為k時刻第i個子濾波器的觀測新息向量，i=1,2。水下復雜環(huán)境導致DVL測速信息、AST定位信息易受野值等非高斯噪聲的干擾，這會導致基于高斯分布假設的FKF濾波性能變差甚至濾波發(fā)散。通過對子濾波器分別魯棒化的方式實現FKF的魯棒化，具體過程如下。

(8)

(9)

為了從理論上推導并確定膨脹因子λi,k的值，將式(9)帶入式(8)可得：

(10)

式(10)可以轉化為求解λi,k的非線性問題，如式(11)所示。

(11)

式中，λi,k可以通過牛頓迭代法求解[21-22]。因此λk(j+1)與λk(j)的關系可表示為：

(12)

2.2 信息分配系數自適應方法

由圖2可知，AST作用范圍有限致使AUV不能持續(xù)獲取位置輔助信息，即AUV會駛出AST作用范圍導致定位信息丟失，此時可認為AST發(fā)生故障。TFKF在子濾波器之間按固定比例分配信息[13]，即選取信息分配系數βi為固定的常值。在此種分配方式下，某個子濾波器性能下降可能會對FKF的全局濾波性能造成較大影響[17-18]。因此根據各子濾波器的濾波性能自適應地調整βi對提升FKF的整體性能具有重要意義。子濾波器的狀態(tài)估計誤差協(xié)方差Pi,k是衡量子濾波器估計精度的重要指標，子濾波器的估計精度越高，Pi,k越小；子濾波器的估計精度越低，Pi,k越大。因此，定義βi為：

(13)

(14)

3 半物理仿真試驗驗證

3.1 數據獲取

試驗數據是從一套船載實驗系統(tǒng)中采集得到的。實驗系統(tǒng)使用的慣性測量單元(Inertial Measurement Unit，IMU)和DVL主要性能指標分別見表1和表2[23]。

表2 DVL性能指標

表1 IMU性能指標

試驗船上同時安裝了一個單天線的GPS接收機，輸出速度和位置信息，其數據更新率為1 Hz。利用GPS輸出數據與IMU輸出數據進行組合導航，生成參考姿態(tài)、速度和位置信息，分別作為實驗中的姿態(tài)、速度和位置基準。船載試驗在長江內進行，試驗過程設計為：當實驗系統(tǒng)開機時，試驗船保持系泊狀態(tài)，時長大約15 min；然后試驗船駛出，運動大約6 h。記錄整個運動過程中的IMU和DVL輸出的原始數據、GPS輸出的速度和位置數據。

選取3600 s船載實測數據用于水下組合導航半物理仿真試驗，選取的數據包含：陀螺儀和加速度計的原始數據、DVL輸出的速度數據、GPS輸出的位置數據，以及對應的姿態(tài)、速度和位置基準。3600 s數據對應的DVL輸出如圖3所示。文獻[18]指出，若AST定位系統(tǒng)已經過校準修正，AST定位誤差的主要來源可認為是噪聲引起的隨機誤差。因此，根據文獻[9]的青島膠州灣淺海(海深約30 m，三級海況)AST導航試驗結果可知，AST淺海導航誤差最大不超過15 m。由此，仿真試驗在模擬生成AST定位信息時，基于降頻后(0.1 Hz)的GPS位置信息人為地引入幅值為20 m的隨機定位誤差。考慮到水下環(huán)境的復雜性，AST輸出易受野值等非高斯噪聲污染，每隔150 s人為地引入幅值為500 m的位置誤差。

圖3 DVL輸出Fig.3 Output of DVL

3.2 試驗結果與分析

在SINS/DVL/AST組合導航系統(tǒng)中，各個子濾波器狀態(tài)向量相同，因此設計主濾波器狀態(tài)量和協(xié)方差陣不進行時間更新。設計TFKF、FRKF均為無反饋模式，即各個子濾波器進行獨立濾波。

3.2.1 魯棒性檢驗

復雜的外部環(huán)境對于水下SINS組合導航來說是不可避免的，也就是說DVL測速信息和AST定位信息不可避免地會受到野值等非高斯噪聲的干擾。本節(jié)在3.1節(jié)DVL輸出數據以及模擬生成的AST定位數據的基礎上，分別利用TFKF、FRKF進行組合導航試驗。DVL屬于主動聲吶設備，在特殊環(huán)境下頻繁使用DVL發(fā)射聲吶信號容易降低AUV的隱蔽性。為此，模擬AUV在進入AST作用范圍后降低DVL使用頻率的情形：對DVL的輸出進行人為地阻隔，使AUV每隔30 s獲取一個速度觀測信號。

圖4(a)～(c)分別為利用不同方法進行組合導航的俯仰角誤差、橫滾角誤差和航向角誤差。由圖4可以看出，當AST定位信息受到野值污染時，TFKF的組合導航姿態(tài)誤差曲線在野值出現的時刻發(fā)生了突變，而FRKF的組合導航姿態(tài)誤差曲線在整個組合導航過程中都是平穩(wěn)的。通過試驗可知，當組合導航收斂后，FRKF得到的俯仰角誤差、橫滾角誤差收斂到0.01°以內，航向角誤差收斂到0.2°以內。

(a) 俯仰角誤差

圖5和圖6分別為利用不同方法進行組合導航的速度誤差和位置誤差。圖5和圖6中，藍色實線為利用TFKF進行組合導航的速度、位置誤差曲線，紅色虛線為利用FRKF進行組合導航的速度、位置誤差曲線。

通過試驗可知，當組合導航收斂后，利用FRKF組合導航的東向速度誤差收斂到0.2 m/s以內，北向速度誤差收斂到0.3 m/s以內；緯度誤差收斂到21 m以內，經度誤差收斂到16 m以內。由圖5和圖6可以明顯看出，當觀測信息受到野值污染時，FRKF的組合導航精度和穩(wěn)定性明顯優(yōu)于TFKF。通過試驗及計算得到利用TFKF和FRKF進行組合導航的速度誤差標準差和位置誤差標準差，如表3所示。

(a) 東向速度誤差

(a) 緯度誤差

由圖4～6及表3可以看出，相比于TFKF，FRKF可有效抑制野值對濾波結果的影響，且FRKF具有更好的數據平穩(wěn)性。試驗結果初步表明：相比于TFKF，FRKF在非高斯條件下具有更高的組合導航精度和穩(wěn)定性。

表3 不同方法得到的組合導航速度、位置誤差標準差Tab.3 Standard deviation of velocity errors and position errors of integrated navigation by using different methods

3.2.2 信息分配系數自適應方法有效性驗證

初始時刻組合導航子系統(tǒng)無信息，信息分配系數初始值設置為β1=β2=0.5。結合在AUV水下航行的實際情況(如圖2所示)，即AUV會駛出AST作用范圍的情況，設置從第1500 s開始AUV駛出AST的作用范圍，從第2500 s開始AUV重新進入AST的作用范圍。在1500～2500 s期間AST定位信號丟失，可認為AST發(fā)生故障。分別利用RKF(SINS/DVL)、RKF(SINS/AST)和FRKF(SINS/DVL/AST)進行組合導航試驗，得到的緯度誤差、經度誤差結果分別如圖7(a)和圖7(b)所示。其中，紅色虛線為RKF(SINS/DVL)得到的位置誤差，藍色虛線為RKF(SINS/AST)得到的位置誤差，黑色實線為FRKF(SINS/DVL/AST)得到的位置誤差。

(a) 緯度誤差

4 結論

借助多個導航傳感器輔助SINS導航是實現AUV水下精確導航的重要途徑。FKF是解決多傳感器信息融合的有效手段，信息分配系數是決定FKF濾波性能的重要因素。AUV在水下航行的過程中，針對水下復雜環(huán)境中AST、DVL輸出的觀測信息易伴隨非高斯噪聲，提出基于馬氏距離算法的FRKF算法。同時，針對AUV不能持續(xù)獲取AST提供的定位信息，信息分配系數為常值將會降低FKF濾波性能的問題，設計了基于子濾波器濾波性能的信息分配系數自適應方法。利用船載實測數據進行水下組合導航半物理仿真試驗，仿真試驗模擬了AST出現故障以及觀測信息受到野值干擾的情形。試驗結果初步表明：相比于TFKF，FRKF在非高斯環(huán)境中具有更高的組合導航精度和穩(wěn)定性；相比于RKF，FRKF具有更優(yōu)的容錯性能。FRKF能夠滿足基于SINS/DVL/AST的水下組合導航系統(tǒng)對魯棒性和容錯性的要求。