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

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

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

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

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

1 SINS/DVL/AST組合導(dǎo)航濾波模型

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

圖1 水下組合導(dǎo)航系統(tǒng)結(jié)構(gòu)Fig.1 Structure of underwater integrated navigation system

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

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

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

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

(1)

(2)

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

(3)

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

2 FRKF算法

2.1 FKF魯棒化

記SINS/DVL組合導(dǎo)航系統(tǒng)對應(yīng)的子濾波器為“子濾波器1”；SINS/AST組合導(dǎo)航系統(tǒng)對應(yī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。水下復(fù)雜環(huán)境導(dǎo)致DVL測速信息、AST定位信息易受野值等非高斯噪聲的干擾，這會導(dǎo)致基于高斯分布假設(shè)的FKF濾波性能變差甚至濾波發(fā)散。通過對子濾波器分別魯棒化的方式實現(xiàn)FKF的魯棒化，具體過程如下。

(8)

(9)

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

(10)

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

(11)

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

(12)

2.2 信息分配系數(shù)自適應(yīng)方法

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

(13)

(14)

3 半物理仿真試驗驗證

3.1 數(shù)據(jù)獲取

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

表2 DVL性能指標(biāo)

表1 IMU性能指標(biāo)

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

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

圖3 DVL輸出Fig.3 Output of DVL

3.2 試驗結(jié)果與分析

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

3.2.1 魯棒性檢驗

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

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

(a) 俯仰角誤差

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

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

(a) 東向速度誤差

(a) 緯度誤差

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

表3 不同方法得到的組合導(dǎo)航速度、位置誤差標(biāo)準(zhǔn)差Tab.3 Standard deviation of velocity errors and position errors of integrated navigation by using different methods

3.2.2 信息分配系數(shù)自適應(yīng)方法有效性驗證

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

(a) 緯度誤差

4 結(jié)論

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