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

（西北工業(yè)大學自動化學院，西安 710000）

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

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

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

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

1 SINS/SRS/CNS 組合導航系統(tǒng)

1.1 組合導航系統(tǒng)結構

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

圖1 SINS/CNS/SRS 導航系統(tǒng)結構圖Fig.1 Structure of SINS / CNS / SRS navigation system

1.2 SINS/SRS/CNS 組合導航系統(tǒng)的狀態(tài)方程

根據SINS 的誤差模型，建立組合導航系統(tǒng)的狀態(tài)空間模型：

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

式中，F(xiàn)|1kk是導航系統(tǒng)的系統(tǒng)矩陣，Gk是系統(tǒng)噪聲驅動矩陣，Xk為導航系統(tǒng)的狀態(tài)向量，具體為：

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

1.3 SINS/SRS 子系統(tǒng)的量測方程

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

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

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

1.4 SINS/CNS 子系統(tǒng)的量測方程

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

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

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

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

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

首先子系統(tǒng)濾波過程如下，

狀態(tài)一步預測：

一步預測均方誤差：

濾波增益：

狀態(tài)估計：

估計均方誤差：

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

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

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

2.1 子濾波器

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

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

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

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

具體的濾波過程：

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

② 抗差方法：

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

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

③ 更新Rk和Mk：

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

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

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

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

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

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

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

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

3 仿真驗證

基于SINS/GPS/CNS 組合導航系統(tǒng)，進行仿真驗證，導航系統(tǒng)選取東北天坐標系。飛行的軌跡如圖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 所示。按照如下所設置情況，進行相應的仿真試驗。分別進行傳統(tǒng)的聯(lián)邦卡爾曼濾波和自適應加權聯(lián)邦卡爾曼濾波算法，得到的位置和速度誤差圖如圖3 和圖4。MD-AFKF 算法中各子濾波器計算的馬氏距離曲線如圖5 所示。

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

表2 CNS 系統(tǒng)模擬量測異常Tab.2 fault simulation of CNS system

表3 SRS 系統(tǒng)模擬量測異常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 子系統(tǒng)量測出現(xiàn)異常，其對應的馬氏距離變大。在 100 s～200 s 和 600 s～700 s 的時候，SINS/CNS 系統(tǒng)出現(xiàn)故障，對應時刻的馬氏距離迅速增加。由此可知，馬氏距離可以有效地檢測出哪一個傳感器在何時出現(xiàn)異常信息，并且以此為依據可以計算出融合系數。

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

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

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

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

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

表3 傳統(tǒng)的FKF 和MD-AFKF 對比Tab.3 Comparison between traditional FKF and MD-AFKF

4 結 論

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

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

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

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