基于馬氏距離的聯邦卡爾曼濾波在SINS/SRS/CNS導航中的應用

高社生，洪根元，高廣樂，高兵兵

（西北工業大學自動化學院，西安 710000）

隨著時代的發展，各種飛行器應用在生活、軍事中的各個方面，因此建立一個既穩定又可靠的導航系統[1,2]，成為研究的重點。其中，基于多傳感器組合的導航方式是實現既穩定又可靠的導航系統的關鍵技術。在諸多的導航系統中，光譜紅移導航系統(SRS)[3]是一種前瞻性的自主導航方式，通過利用天體光譜的多普勒效應實現飛行器導航，有著自主性強、計算量小且無時間延時的優點。將SRS 與捷聯式慣導系統(SINS)[4]以及天文導航系統(CNS)[5]進行組合的SINS/SRS/CNS 組合導航系統，能夠在CNS 選星困難的時候，通過SRS 和CNS 輔助糾正SINS 導航系統的誤差，保證了導航系統的可靠性。

對于多傳感器的組合導航方式[6-8]，國內外做了很多研究。常用的方法是用卡爾曼濾波來進行導航系統的信息融合，卡爾曼濾波器包括集中式濾波和分布式濾波。集中式的Kalman 濾波器有著計算量大、沒有處理異常噪聲的缺陷。在1988 年Carlson 提出了一種分布式的濾波[9]——聯邦卡爾曼濾波，解決了集中式濾波器的弊端，提高濾波的精度。之后，MOORE J B等研究了卡爾曼濾波信息融合方法[10]，文獻[11]提出了最小加權的聯邦卡爾曼濾波融合算法。這些算法，較傳統的卡爾曼濾波算法，導航的精度都有很大的提高。

當飛行器處于一些復雜環境時，傳感器可能會失效或者受到外界的干擾使得量測誤差增大，聯邦卡爾曼濾波[12-14]這種不經過處理量測信息，直接在主濾波器中融合的方法，將導致濾波的精度下降，影響導航系統的準確性[15]。

為了提高導航系統的可靠性，基于對傳統的聯邦卡爾曼濾波算法的研究，在不損失導航系統精度的條件下，改進了傳統的聯邦卡爾曼濾波[16-18]。首先，在子濾波器中加入基于馬氏距離的抗差方法[19]，動態地調整子系統中的量測噪聲方差陣，達到減小子濾波器量測信息受到污染對主濾波器的影響。其次，在卡爾曼濾波融合和分配階段加入基于馬氏距離[20]的自適應融合系數和分配系數，減小子濾波器不準確的估計對主濾波器的影響。通過對SINS/SRS/CNS 組合導航系統仿真驗證，當傳感器出現量測異常的時候，本文所提出的導航系統濾波融合方法能夠解決量測異常影響導航精度的問題。

1 SINS/SRS/CNS 組合導航系統

1.1 組合導航系統結構

SINS/SRS/CNS組合導航系統選取SINS作為主系統，同時選取SRS和CNS作為輔助導航系統糾正SINS的誤差。其中，通過SRS 導航系統求解飛行器的速度信息，糾正SINS 的速度誤差，通過CNS 系統獲取位置信息，修正SINS 的位置誤差。由于CNS 無法獲得高度信息，SINS/SRS/CNS 組合導航系統采用了氣壓高度計(BA)，用來彌補CNS 無法測量高度信息的缺陷。具體的導航系統結構如圖1 所示。

圖1 SINS/CNS/SRS 導航系統結構圖Fig.1 Structure of SINS / CNS / SRS navigation system

1.2 SINS/SRS/CNS 組合導航系統的狀態方程

根據SINS 的誤差模型，建立組合導航系統的狀態空間模型：

為了進行計算機仿真和Kalman 濾波估計，所以首先需要對式(1)離散化，即得到：

式中，F|1kk是導航系統的系統矩陣，Gk是系統噪聲驅動矩陣，Xk為導航系統的狀態向量，具體為：

式中，φ=(φE,φN,φH)表示姿態的誤差信息，δv=(δ vE,δvN,δvH)表示速度的誤差信息，δp=(δL,δλ,δh)表示位置的誤差信息，εb和?b分別表示三個陀螺儀常值漂移和三個加速度計常值偏差，wk是服從N(0,Qk)的系統噪聲矩陣。

1.3 SINS/SRS 子系統的量測方程

SINS/SRS 子系統的量測信息是由SINS 解算出的速度信息和光譜紅移推算出的速度信息的差值。但是光譜紅移解算出的速度信息是在慣性系下的，首先要進行坐標變換，然后才能建立量測方程，具體坐標變換公式如下：

在SINS 與SRS 組合的子濾波器中，建立如下的量測方程。

其中，V1是子濾波器服從N(0,R1,k)量測噪聲矩陣，H1是子濾波器的量測矩陣。

1.4 SINS/CNS 子系統的量測方程

SINS/CNS 子系統的量測信息是由慣導解算出的位置信息和CNS 提供的經緯度位置信息以及BA 提供的高度信息的差值。建立的量測方程如下所示：

其中，V2是子濾波器服從N(0,R2,k)量測噪聲矩陣，H2是子濾波器的量測矩陣。

2 基于馬氏距離的自適應聯邦卡爾曼濾波算法

集中式Kalman 濾波有計算量大、容錯性差的缺點，為了解決這一問題，通常采取聯邦Kalman 濾波這種分布式結構。但是，當飛行器處在復雜的環境時，傳感器可能會受到干擾，導致傳感器的量測信息不準確，主濾波器不經過判斷處理，直接將有異常的信息進行融合，導致了聯邦Kalman 濾波的精度會下降。所以，采取一種基于馬氏距離的自適應聯邦Kalman濾波(MD-AFKF)可以解決量測異常時導航精度下降的問題，具體的過程如下，假設子濾波器的模型為：

式中，Xi,k表示第i個子濾波器k時刻的狀態量，Fi,k|k-1表示第i個子濾波器的系統矩陣，Gi,k表示i個子濾波器k時刻的系統噪聲驅動矩陣，Vi表示子濾波器的量測噪聲矩陣，Hi表示子濾波器的量測矩陣，wi,k是服從N(0,Qi,k)的系統噪聲矩陣。

首先子系統濾波過程如下，

狀態一步預測：

一步預測均方誤差：

濾波增益：

狀態估計：

估計均方誤差：

其次，子濾波器濾波完成后，將濾波后的結果在主濾波器中進行融合。考慮到各個系統的濾波效果不同，不能簡單地將各個濾波器的信息相加融合起來。因此，選取融合系數αi來評估子濾波器濾波的好壞。具體融合公式如下：

最后將融合的結果按如下公式分配到各個子濾波器中，作為下一時刻子濾波器的初始值。

其中，βi為分配系數，滿足守恒原則下標i表示第i個局部濾波器，g表示的是主濾波器。

2.1 子濾波器

為了提高導航系統的穩定性和可靠性，在子濾波器中加入抗差方法。該方法引入馬氏距離Mi,k、概率閾值χα及顯著性水平α,滿足式(18)

式中，mi,k表示子濾波器殘差。

當Mi,k低于該閾值時，傳感器正常工作，反之，傳感器出現故障。此時，將量測方差放大。

隨之會引起Ki,k的變小。當狀態估計和計算均方誤差陣的時候，由于Ki,k的減小，會較少地考慮量測帶來的信息，繼而更多地考慮狀態預測和協方差預測矩陣帶來的信息。

具體的濾波過程：

① 由式(11)和(12)計算狀態預測和均方誤差預測。

② 抗差方法：

如果Mi,k＞χα，計算放大系數ηk。

通過牛頓迭代法求解該方程，

③ 更新Rk和Mk：

更新后的Mi,k如果仍然大于χα，就重復執行上述步驟(2)和(3)，直到使得Mi,k小于χα。

④ 根據式(13)(14)和(15)，計算狀態估計和均方誤差估計值。

2.2 計算各濾波器的分配系數

選取第i個子濾波器的狀態估計和主濾波器融合后的狀態估計的馬氏距離，把其作為評價子濾波器濾波效果的指標，具體公式如下：

式中，Pg,i是和的協方差矩陣。當越小的時候，說明子濾波器的估計值和主濾波器的估計值越接近，表示該子濾波器狀態估計的效果越好。因此，下一時刻可以選擇分配更多的信息給該子濾波器，具體的分配系數βi為：

2.3 計算各濾波器的融合系數

子濾波器信息融合時，如果子濾波器量測信息異常，將有誤差的信息融合到主濾波器中，降低導航系統的精度。為減少此情況對主濾波器的影響，需要評估子濾波器的效果，同樣選取馬氏距離作為評價標準，具體公式如下：

當子濾波器的馬氏距離fusi越小時，說明量測值和狀態預測之間的差異越小，該子濾波器濾波量測信息沒有受到干擾或出現異常值。因此該子濾波器在信息融合的過程中提供更多的信息，融合系數如下：

3 仿真驗證

基于SINS/GPS/CNS 組合導航系統，進行仿真驗證，導航系統選取東北天坐標系。飛行的軌跡如圖2所示，飛行的總時間為1000 s，飛行器初始位置(108.909 °,34.246 °,10000m)，初始速度為(5 m/s, 5 m/s,0 m/s)。

圖2 飛行器仿真軌跡圖Fig.2 Trajectory diagram of aircraft simulation

在傳感器正常工作下，具體參數如表1 所示。模擬傳感器的量測異常信息，具體量測狀況如表2 和表3 所示。按照如下所設置情況，進行相應的仿真試驗。分別進行傳統的聯邦卡爾曼濾波和自適應加權聯邦卡爾曼濾波算法，得到的位置和速度誤差圖如圖3 和圖4。MD-AFKF 算法中各子濾波器計算的馬氏距離曲線如圖5 所示。

表1 傳感器仿真參數設置Tab.1 parameter setting of sensor simulation

表2 CNS 系統模擬量測異常Tab.2 fault simulation of CNS system

表3 SRS 系統模擬量測異常Tab.3 fault simulation of SRS system

圖3 SINS/CNS/SRS 組合導航速度誤差Fig.3 Speed error of SINS / CNS / SRS integrated navigation

圖4 SINS/CNS/SRS 組合導航位置誤差Fig.4 Position error of SINS / CNS / SRS integrated navigation

根據圖5 可知，在100 s～200 s 和500 s～600 s 之間，SINS/SRS 子系統量測出現異常，其對應的馬氏距離變大。在 100 s～200 s 和 600 s～700 s 的時候，SINS/CNS 系統出現故障，對應時刻的馬氏距離迅速增加。由此可知，馬氏距離可以有效地檢測出哪一個傳感器在何時出現異常信息，并且以此為依據可以計算出融合系數。

圖5 SINS/CNS 子系統和SINS/SRS 子系統的馬氏距離Fig.5 Mahalanobis distance of SINS / CNS subsystem and SINS / SRS subsystem

由圖3 和圖4 可知，在100 s～200 s 的時候，兩個子濾波器的量測信息均出現了較大的誤差，傳統的聯邦卡爾曼濾波無法處理這種情況，會把誤差信息帶入到主濾波器，導致導航系統的準確性下降，而自適應加權的聯邦卡爾曼濾波算法，可以一定程度上減小誤差，但是相比于傳感器正常工作的時候還是要大一些。在500 s～600 s 和600 s～800 s 的時候，模擬兩個子濾波器分別單獨出現異常量測，此時自適應聯邦卡爾曼濾波算法對這種情況能夠進行很好地處理。

為了便于分析，對該系統進行了50 次的仿真，計算了50 次的均方根誤差，得到的結果如圖6 和圖7。自適應聯邦卡爾曼濾波算法相比于傳統的聯邦卡爾曼濾波，位置的三個方向的平均的均方誤差下降了57.9%、56.35%、54.93%，速度的三個方向的平均的均方根誤差下降了79.09%、78.98%、54.61%，提高了導航系統的容錯性。

圖6 FKF 和自適應加權聯邦卡爾曼濾波位置均方誤差Fig.6 Position mean square error of FKF and adaptive weighted federated Kalman filter

圖7 FKF 和自適應聯邦卡爾曼濾波速度均方誤差Fig.7 Velocity mean square error of FKF and adaptive weighted federated Kalman filter

表3 傳統的FKF 和MD-AFKF 對比Tab.3 Comparison between traditional FKF and MD-AFKF

4 結 論

本文針對SINS/SRS/CNS 組合導航系統，提出一種基于馬氏距離的加權自適應聯邦卡爾曼濾波算法，在提高導航系統的穩定性和可靠性的方面進行了相關的研究。

(1)首先在聯邦卡爾曼濾波中加入了自適應的相關系數。經過前文分析和仿真驗證，出現量測異常時，通過調整相關系數的大小，可減小該異常量測對主濾波器的影響，從而提高導航系統的容錯性。

(2)在每一個子濾波器中加入抗差的方法。當子濾波器中的量測信息出現較大的噪聲時，減小增益矩陣，使狀態估計更少地利用量測信息，更多地利用預測信息，提高了子濾波器的容錯性能。

(3)通過仿真驗證了本文所提出的自適應聯邦卡爾曼濾波算法，相較于傳統的卡爾曼濾波算法，在容錯方面有了一定的提高。