置信檢驗自適應聯邦卡爾曼濾波及其水下機器人組合導航應用

陳帥，王寧*，陳廷凱，楊毅，田嘉禾

1 大連海事大學 船舶電氣工程學院，遼寧 大連 116026

2 中車大連電力牽引研發中心有限公司，遼寧 大連 116085

0 引 言

水下機器人作為一種智能化設備，已逐漸成為人類開發利用海洋資源的重要工具和海工裝備的中堅力量。水下機器人在執行任務時，精準定位是極其重要的一環。因此，提升機器人組合導航系統的精度有著重大的研究意義。目前，GPS/INS是應用最為廣泛的組合導航方式，其本質是一種誤差特性互補的組合導航，通常與卡爾曼濾波相結合。具體而言，一方面利用GPS信息修正INS的導航結果，以調整其隨時間積累的誤差；另一方面，利用INS短時間內的高精度定位優勢，解決GPS信號在受遮擋、中斷等不利條件下的定位問題[1]。需要注意的是，當載體受到外界強擾動或量測信息不可靠情況下，傳統卡爾曼濾波效果將會受到影響，不可避免地導致GPS/INS組合導航的精度受到影響。

高精度的組合導航技術引起了國內外學者的廣泛關注。Yang等[2]提出了一種基于預測殘差協方差陣的聯邦卡爾曼濾波自適應因子改進方法，有效提高了聯邦卡爾曼濾波的濾波精度；周先林等[3]提出了一種基于新息的自適應卡爾曼濾波算法，該算法通過卡方檢驗檢測出量測異常值，在量測異常值處調整量測噪聲方差陣，有效解決了濾波器中出現不可靠測量時濾波發散的問題；呂建新等[4]將里程計與GPS/INS組合導航系統相結合，提出了自適應信息分配因子的策略，提升了組合導航系統的抗擾動能力；Li等[5]針對GPS和INS更新速率不同步問題，提出了一種針對GPS量測值的觀測性擴展方法，提升了組合導航系統在載體低速率時的導航精度；Jiang等[6]將澳大利亞的Locata定位系統、精準點定位全球衛星導航系統（PPP-GNSS）和INS相結合，并利用隨機向量空間方法設計了一種全局最優濾波器，有效提升了導航精度；Ma等[7]將電磁羅盤（magnetic compass）、地形輔助導航（terrain aided navigation）、多普勒測速儀（Doppler velocity log）與INS相結合，并且利用殘差提出了一種自適應聯邦卡爾曼濾波算法，有效提升了組合導航系統的穩定性；Liu等[8]提出了魯棒卡爾曼濾波算法并將其用于聯邦卡爾曼濾波器，有效抑制了傳感器誤差對濾波器結果的影響；Malleswaran等[9]將輸入延遲動態神經網絡與GPS/INS組合導航系統相結合，在GPS信號不可用時利用神經網絡對INS進行補償，有效提升了組合導航系統的導航精度；Sung等[10]將數字羅盤與GPS相結合，并提出了一種簡化卡爾曼濾波器，一定程度上提高了導航精度；Abosekeen等[11]利用簡化的慣性傳感器系統替代INS，并利用離散余弦變換對數據進行處理，提高了GPS信號被遮擋時的濾波精度；García等[12]通過構造可信度量判斷量測信息的可靠性，解決了量測信息不可靠問題對濾波結果的影響。需要注意的是，上述方法在量測值發生異常時，無法解決組合導航系統穩定性變差和濾波精度降低的問題。

為解決上述問題，本文將提出一種置信檢驗自適應聯邦卡爾曼濾波算法，通過構造高斯分布對量測值進行可靠性檢驗，保證量測值的有效性；根據殘差協方差設計局部濾波器的自適應調節因子，對系統噪聲協方差進行實時調整，有效提高濾波精度和系統穩定性。

1 INS/GPS/EC聯邦濾波

1.1 聯邦卡爾曼濾波

聯邦卡爾曼濾波的狀態方程和量測方程為：

式中：X(k+1)為系統k時刻對k+1時刻的狀態估計值；F(k)，X(k) 和G(k)分別為系統在k時刻的狀態轉移矩陣、狀態變量和控制矩陣；w(k)為系統的白噪聲矩陣；i=1,2,···,n，為第i個局部濾波器；Zi(k)，Hi(k)和vi(k)分別為系統局部濾波器i在k時刻的量測值、量測矩陣和量測噪聲。

聯邦卡爾曼濾波器濾波機制主要包括4個步驟：信息分配、信息時間更新、信息量測更新和最優信息融合[13]。

1） 信息分配。

假設在初始時刻全局狀態的初始值為X0，其協方差矩陣為P0，系統噪聲方差矩陣為Q0。將這一信息通過信息分配因子按以下規則分配到各局部濾波器和全局濾波器：

式中：βi為信息分配因子，必須滿足信息守恒原則，即 β1+β2+···+βn+βm=1,0≤βi≤1；下標n代表第n個局部濾波器，下標m代表主濾波器。

2） 信息時間更新。

時間更新包括2個部分：狀態預測和協方差預測。

式中：Pi(k+1|k)，X?i(k+1|k)，Fi(k+1|k)和Qi(k+1|k)分別為局部濾波器i的一步預測誤差協方差矩陣、狀態一步預測、一步轉移矩陣和一步預測系統噪聲方差矩陣。

3） 信息量測更新。

量測更新包括3個部分：卡爾曼濾波增益計算、狀態最優估計以及協方差修正。

式中，Ki(k)和Ri(k)分別為局部濾波器i的增益矩陣和量測噪聲方差矩陣。

4） 最優信息融合。

最優信息融合則是利用各個局部濾波器的協方差和估計值，在主濾波器中進行最優狀態估計。

上面4個步驟即為聯邦卡爾曼濾波的基本步驟，通過以上步驟，可以實現多測量值系統對狀態的最優估計。

1.2 組合導航建模

通常而言，選取慣性導航系統的位置、速度和姿態等15維誤差信息作為狀態變量，分別為位置誤差、速度誤差、姿態角誤差、陀螺儀漂移誤差以及加速度計的零偏誤差[14]。故狀態變量X(k)為：

式常速度值中 計漂：ε 零移bx， 偏；εΔb誤yb，x，差εbΔz；b分yw，別(Δkb)z為分=[東別ωg、為x北 ω東g、y、天 ω北g方z、 ω向天ax的方ω陀向ay螺的ωaz儀 加]T為白噪聲矩陣，其中ωgx，ωgy，ωgz分別為東、北、天方向的陀螺儀白噪聲，ωax，ωay，ωaz為東、北、天方向的加速度計一階馬爾科夫過程白噪聲；狀態轉移矩陣F(k)和 控制矩陣G(k)可參考文獻[15]。

在建立組合導航系統的狀態方程后，需建立觀測方程對組合導航系統的位置、速度、姿態信息進行觀測。慣性導航系統能夠測量水下機器人的位置、姿態、速度信息，全球定位系統能夠測量水下機器人的位置、速度信息，電子羅盤能夠測量水下機器人的姿態信息。以慣性導航系統作為公共系統，分別求取與電子羅盤、全球定位系統的相同信息差值，選取慣性導航系統和全球定位系統的速度信息和位置信息作為觀測變量建立INS/GPS局部濾波器，選取慣性導航系統和電子羅盤的姿態信息作為觀測變量建立INS/EC局部濾波器[16]。

1.2.1 INS/GPS局部濾波器

因慣性導航系統和全球定位系統均可直接測得水下機器人在某一時刻的位置和速度信息，故選取慣性導航系統和全球定位系統的位置和速度測量值作為量測值。慣性導航系統和全球定位系統的位置信息可表示為：

式中：X1，Y1，Z1，X2，Y2，Z2分別為慣性導航系統和全球定位系統的經度、緯度、高度的測量值；λ，L，h分別為水下機器人經度、緯度、高度的真實值；δλ ，δL， δh， δX2，δY2，δZ2分別為慣性導航系統和全球定位系統的經度、緯度、高度的測量誤差；R為地球赤道半徑，約為 6.37×106m。

定義位置觀測方程為，

式中，O1×13為零矩陣。

慣性導航系統和全球定位系統的速度信息可表示為

式中：vx，vy，vz，vx2，vy2，vz2分別為慣性導航系統和全球定位系統的東、北、天方向速度的測量值；vxr，vyr，vzr分別為水下機器人東、北、天方向速度的真實值；δvx，δvy，δvz， δvx2， δvy2， δvz2分別為慣性導航系統和全球定位系統的東、北、天方向速度的測量誤差。

定義速度觀測方程為

1.2.2 INS/EC局部濾波器

通過慣性導航系統和電子羅盤可直接獲得水下機器人的三軸姿態角信息，故以兩者之差作為觀測量，慣性導航系統和電子羅盤的姿態信息可表示為

式中：φx，φy，φz，φx2，φy2，φz2分別為慣性導航系統和電子羅盤的東、北、天方向姿態的測量值；φxr，φyr，φzr分別為水下機器人東、北、天方向姿態的真實值；δφx，δφy，δφz， δφx2， δφy2， δφz2分別為慣性導航系統和電子羅盤的東、北、天方向姿態的測量誤差。

姿態觀測方程為

2 置信檢驗自適應聯邦卡爾曼濾波

將本文提出的置信檢驗自適應聯邦卡爾曼濾波器應用于水下機器人的INS/GPS/EC組合導航系統中，構成2個局部濾波器和1個主濾波器，INS/GPS/EC組合導航系統結構圖如圖1所示。

傳統聯邦濾波器的各子濾波器因為采用了相同的狀態方程，若狀態方程出現擾動會影響子濾波器的性能，在載體發生擾動異常或存在較大動力學模型誤差時，濾波的效果往往不太理想。由圖1可知，在本文設計的聯邦濾波器中，INS，GPS和羅盤的量測信息需先經過置信檢驗，可靠的量測值Zi被送入局部濾波器，基于INS/GPS和INS/EC的自適應局部濾波器并行運行，利用自適應調節因子調節系統噪聲協方差，得到預測誤差協方差矩陣Pi和 局部估計值X?i，并將其送入主濾波器并進行融合，得到系統狀態的最優估計值Pg和X?g。同時，為進一步提升濾波精度，本系統選用了有重置結構的聯邦卡爾曼濾波器，X?g和被放大為β?i1Pg的最優估計值被反饋到局部濾波器，對局部濾波器的估計值進行重置。

圖1 INS/GPS/EC組合導航系統結構圖Fig.1 Structure diagram of INS/GPS/EC integrated navigation system

2.1 置信檢驗

由卡爾曼濾波器的結構可知，量測值對下一階段的估計值具有直接影響，而對于聯邦卡爾曼濾波器而言，當任意一個傳感器出現故障或不可靠測量時，主濾波器輸出的最優估計值的估計精度都會受損。針對這一問題，本文提出CC-AFKF框架，對每一時刻的量測值均進行一次置信檢驗，滿足條件的量測值認為是可靠測量，可用于進行下一時刻的預測；而不滿足條件的量測值利用量測方程進行更改，保證最優估計值的精度。

對任何一個局部濾波器的任意一個時刻而言，下一時刻狀態的預測值為

為判定量測值的可靠度，將預測向量中第t維的預測值X?it(k+1|k)表 示為。計算G(k)w(k)第t維的方差。G(k)w(k)第t維參數的均值 μ為

由系統噪聲w(k)的高斯特性，可認為下一時刻斯的分最布優，即估X?計it(k值|k均)～服N從(Xˉ均it,σ值2)為。X引ˉit，入方一差個為置σ2信的概高率α， 為使得Zti(k)處于置信區間內，根據標準正態分布的分布函數 Φ(x)，量測值的邊界取值、均值、方差和置信概率的關系可表示為

因傳感器每次測量時，真實值均服從正態分布[17]，所以根據傳感器的測量特性，x1和x2應滿足x1+x2=2Xˉit，當Zti(k)∈(x1,x2]時，可認為下一時刻的Zti(k)屬于可靠測量，能夠用于接下來的預測環節；當Zti(k)?(x1,x2]時，則認為量測值屬于不可靠測量，量測值利用觀測方程Z(k)=HX(k)+v(k)獲得。

同時，根據正態分布的“3σ”法則，當α設置為0.27%時，置信檢驗可以過濾掉傳感器在出廠誤差范圍之外的量測值。但在實際應用場合中，為提高融合精度，可通過重復試驗合理選取α值。

2.2 自適應局部濾波器

在對量測值進行了置信檢驗后，確保了每次用于下一階段預測的量測值的可靠性，保證了濾波器的穩定性，進而考慮設計自適應調節因子對濾波器中的相關變量進行實時調整，提高濾波器的濾波精度。

式中，P(k+1|k)為預測誤差協方差矩陣在計算區間內的估計值。kt?k0一般取20，當殘差的實際協方差與理論協方差保持一致時，卡爾曼濾波算法的精度最高[18]。根據殘差的實際協方差和理論協方差，可用兩者的跡tr的比值η來衡量真實值與理論值的偏差程度：

考慮到濾波器結構中包含多個局部濾波器，調整Ri(k)會不可避免地增加系統的運算量，又因為Pi(k+1|k)=Fi(k+1|k)Pi(k|k)FiT(k+1|k)+Qi(k)，故調整殘差的實際協方差可通過調整帶有調節因子 τ的Qi(k)來實現：

式中，e為自然常數，每次迭代時令Qi(k)=τQi(k?1)，不斷調整每次更新過程中的系統噪聲協方差，實現自適應狀態估計。

本文所提出的置信檢驗自適應聯邦卡爾曼濾波算法流程圖如圖2所示。

圖2 置信檢驗自適應聯邦卡爾曼濾波算法流程圖Fig.2 Flow process of confidence check-adaptive federated Kalman filter algorithm

首先設定好濾波器的初值，包括狀態初值、系統噪聲協方差和量測噪聲協方差，其次根據預測值和置信概率進行量測值的置信檢驗，當量測值屬于不可靠測量時，利用量測方程修正量測值，然后計算自適應調節因子實時調節系統噪聲協方差矩陣，利用調節后的系統噪聲協方差進行狀態估計，最后得到卡爾曼濾波結果。

3 試驗結果分析

為驗證本文所提出算法的有效性和優越性，將本文所提出的CC-AFKF框架與經典的KF，FKF框架在融合精度層面相比較。本次試驗地點為大連市凌海港，試驗所用的組合導航模塊信息如表1所示，水下機器人試驗平臺如圖3所示。

圖3 水下機器人試驗平臺Fig.3 Test platform of underwater vehicle

表1 組合導航模塊器件清單Table 1 Parts list of integrated navigation module

試驗初始參數設置如下：本次試驗僅考慮東向和北向速度，并忽略水下機器人的天向速度[19]，水下機器人的初始位置為東經121.546°，北緯38.872°，初始東向和北向速度均為0 m/s，試驗將第三方SBG Ekinox 2的組合導航輸出結果（理論定位精度為0.02 m）作為參考值，試驗分析的各項定位測速誤差都是以該參考值為基準得出。水下機器人真實運動軌跡如圖4所示。

圖4 水下機器人的真實運動軌跡Fig.4 Real trajectory of underwater vehicle

根據GPS，INS和EC所測得的數據，得到GPS，INS和使用本文算法CC-AFKF融合后的經緯度和速度數據，分別如圖5和圖6所示。

圖5 原始經緯度信息對比圖Fig.5 Comparison of original latitude and longitude information

圖6 原始北、東向速度信息對比圖Fig.6 Comparison of original information of eastward and northward speed

從圖5和圖6中可以看出，本文所提出的CC-AFKF算法在位置和速度信息融合方面具備有效性。其中，圖5顯示INS框架定位誤差隨著時間的增加而增加，在經度為121.546 8°時其測量結果就已開始明顯偏離實際參考值，選取圖5中INS發生明顯偏差的一部分進行放大，通過觀察可知，本文所提出的CC-AFKF算法所得到的經、緯度基本上與參考值保持一致；由于試驗海域不存在遮擋物，因此GPS單獨導航的效果與參考值相比也相差不大，但限于GPS單系統的不穩定性，可以發現GPS的數據存在較大波動。同理，圖6也明顯展示出無論是原始東向速度還是北向速度，INS框架的速度誤差也在隨時間累積；可以發現，在INS運行一段時間后，速度開始偏離參考值，而CC-AFKF框架和GPS框架在速度信息上與參考值進行對比時，展現的特性與經緯度信息幾乎完全一致。

由此可知，本文提出的CC-AFKF算法在位置和速度的融合方面具備一定的可靠性，為證明置信檢驗環節的有效性，在150 s時對GPS的測量數據施加一定的擾動，得到AFKF和CC-AFKF的經緯度和速度對比圖（圖7和圖8）。

從圖7和圖8可知，施加擾動前，AFKF框架下的融合結果與CC-AFKF和參考值幾乎完全重合；但在施加擾動后，無論是位置信息還是速度信息，均發生了較大偏差；由此可見，本文所提出的具備置信檢驗環節的CC-AFKF框架具備一定的抗擾動性能。

圖7 受擾動后不同框架的經緯度對比圖Fig.7 Comparison of latitude and longitude between different frameworks due to perturbation

圖8 受擾動后不同框架的北、東向速度對比圖Fig.8 Comparison of northward and eastward speed between different frameworks after perturbation

現有組合導航方式多為運用KF的GPS/INS組合導航模式[20]，為充分體現本文算法的優越性，將本文所提出的CC-AFKF算法與經典的KF和FKF算法相比較，結果如圖9至圖14所示。其中，圖9至圖11分別是經緯度對比、誤差對比和均方根誤差對比；圖12至圖14分別是東北向速度對比、誤差對比和均方根誤差對比。

圖9 不同框架的經緯度對比圖Fig.9 Comparison of latitude and longitude between different frameworks

圖10 不同框架的經緯度誤差對比圖Fig.10 Comparison of longitude and latitude errors between different frameworks

圖11 不同框架的位置均方根誤差對比圖Fig.11 Comparison of root mean square error of position between different frameworks

圖12 不同框架的北、東向速度對比圖Fig.12 Comparison of northward and eastward speed between different frameworks

圖13 不同框架下的北、東向速度誤差對比圖Fig.13 Comparison of northward and eastward speed errors between different frameworks

圖14 不同框架的速度均方根誤差對比圖Fig.14 Comparison of root mean square error of speed between different frameworks

從圖9至圖14可以看出，在KF框架和FKF框架下速度和位置信息相較于參考值不會發生明顯偏差，但本文所提出的CC-AFKF框架在速度和位置融合精度方面能夠實現更優越的性能。從圖9和圖12的細節放大圖可知，KF框架下的數據波動較大，KFK框架雖然相較于KF框架穩定度有所提高，但提升并不明顯，CC-AFKF框架下的位置和速度信息與參考值最為接近，誤差圖對比也證明了這一點。在位置融合方面，經度和緯度誤差能夠穩定在 [?1,1]區間，與KF框架相比較，本文所提出的CC-AFKF框架在緯度和經度的融合精度上分別提升了71.4%和68.5%，相較于FKF則分別提升了32.5%和36.9%。在速度的融合方面，本文所提出的CC-AFKF框架使用自適應系統噪聲協方差，能夠更加精準地實現速度融合。具體而言，本文提出的CC-AFKF框架在北向速度和東向速度的融合精度上相較于KF分別提升了46.9%和59.2%，相較于FKF則分別提升了29.4%和42.7%。綜上所述，由于KF和FKF缺少對系統噪聲的跟隨性，故其難以實現與本文所提出的CC-AFKF框架相一致的融合性能。

4 結 語

本文設計了一種基于GPS/INS/EC的聯邦卡爾曼濾波器框架，利用系統狀態真實值的高斯分布特性對量測值進行置信檢驗，有效剔除系統中的不可靠測量值；根據殘差設計自適應分配因子，對系統噪聲協方差自適應進行調整，提高了系統精度。大量的試驗和比較證明了本文所提出的CC-AFKF框架的有效性和優越性。