一種狀態約束下的抗差自適應聯邦濾波算法

馮抗洪 宋迎春 崔先強

1 中南大學地球科學與信息物理學院,長沙市麓山南路932號,410083

合理地利用先驗約束信息可以提高單傳感器系統和多傳感器系統的導航定位服務精度及可靠性。目前,將約束信息納入到卡爾曼濾波中來處理單傳感器導航數據的研究較多[1],而在多傳感器導航系統中加入先驗狀態約束信息的研究較少。相較于單傳感器系統,多傳感器系統的觀測量明顯增多,但不同傳感器的定位原理不同,受干擾的情形也不同。如在由捷聯慣導(strapdown inertial navigation system,SINS)、GNSS和里程計(odometer,ODO)組成的SINS/GNSS/ODO組合導航系統[2]中,GNSS信號易受高樓、樹木等遮擋[3],車載體側滑、滑行等現象易導致ODO的數據無效[4]。此外,即便在所有傳感器都正常工作的情況下,仍有必要考慮約束信息,因為約束通常是一個硬性界限,是必須滿足的條件。

本文基于對傳統聯邦卡爾曼濾波算法(federated Kalman filter,FKF)[5]的研究,將狀態約束下的卡爾曼濾波擴展到聯邦濾波中,提出狀態約束下的聯邦濾波(federated filtering with state constraints,CON-FKF)算法。另外,針對載體處于復雜環境時傳感器可能出現故障或數據異常的情形,本文還對狀態約束下的聯邦濾波進行了改進。首先,在各個子濾波器中加入Huber抗差方法[6],以抵制觀測異常對導航精度的影響;其次,在信息分配階段加入基于馬氏距離的自適應信息分配因子[7-8],得到一種狀態約束下的自適應抗差聯邦濾波算法(a robust adaptive federated filtering algorithm with state constraints,CON-RAFKF),以進一步減小子濾波器的不準確估計對全局估計的影響;最后,基于SINS/GNSS/ODO組合導航系統,利用仿真實驗驗證本文算法的有效性。

1 SINS/GNSS/ODO組合導航數學模型

1.1 狀態方程

在多傳感器組合導航中,基于SINS誤差方程建立狀態方程,其線性狀態方程可表示為[9]:

Xk=ΦkXk-1+wk

(1)

式中,Xk=[δψ,δv,δp,bω,bf]T為15維狀態誤差向量,δψ、δv、δp分別為姿態誤差、速度誤差和位置誤差,bω、bf分別為陀螺零偏和加速度計零偏,可被建模為隨機常數[10];Φk為系統狀態轉移矩陣;wk為狀態過程噪聲向量,其協方差矩陣為Σwk。

1.2 觀測方程

1)GNSS/SINS 觀測方程。GNSS/SINS的觀測值為GNSS輸出的位置與SINS計算的位置之差,觀測方程為[10]:

Z1k=PGNSS-PSINS=H1kXk+τ1k

(2)

式中,Z1k為k時刻濾波位置觀測值;PGNSS為GNSS位置觀測值;PSINS為SINS位置計算值;H1k為觀測矩陣;τ1k為觀測噪聲,滿足高斯白噪聲特性,其協方差矩陣為R1k。

2)ODO/SINS觀測方程。ODO/SINS的觀測值為ODO輸出的速度與SINS計算的速度之差,觀測方程為[2]:

Z2k=VODO-VSINS=H2kXk+τ2k

(3)

式中,Z2k為k時刻濾波速度觀測值;VODO為ODO速度觀測值;VSINS為SINS速度計算值;H2k為觀測矩陣;τ2k為觀測噪聲,滿足高斯白噪聲特性,其協方差矩陣為R2k。

2 CON-FKF算法

CON-FKF算法具有2層結構。在第1層中,SINS分別與GNSS和ODO組合,并通過并行的方式獲得狀態約束下SINS/GNSS和SINS/ODO的局部狀態估計值;在第2層中,將各個子濾波器的狀態估計進行融合,獲得SINS/GNSS/ODO組合導航系統的全局狀態估計。算法結構如圖1所示。

圖1 CON-FKF算法結構

2.1 信息融合和信息分配過程

(4)

(5)

式中,r=2為子濾波器的個數。

(6)

(7)

Σwik=1/βiΣwgk

(8)

(9)

式中,Σwgk、Σwik分別為主濾波器和第i個子濾波器的狀態噪聲協方差矩陣。

2.2 子濾波器狀態估計

在SINS/GNSS/ODO組合導航系統中,2個子濾波器的狀態方程與主濾波器的狀態方程均如式(1)所示,而第i個子濾波器的觀測方程可以統一表達為:

Zik=HikXk+τik

(10)

式中,Zik為第i個子濾波器的觀測量;Hik為觀測矩陣;τik為第i個子濾波器的觀測噪聲,其協方差矩陣為Σik,i=1,2。則各子濾波器的時間更新和觀測更新為:

(11)

(12)

(13)

(14)

(15)

(16)

(17)

由于本文以大地坐標系位置誤差[δL,δλ,δh]作為濾波的狀態量,而式(16)為導航坐標系(ENU)下的位置約束,因此還需進行誤差擾動和轉換。對于第i個傳感器,可得如下約束方程:

(18)

式中,Cik=[0,(RN+H)cos(L),0;RM+H,0,0;0,0,1],RM和RN分別為沿地球子午圈和卯酉圈的主曲率半徑,H為當地高程,L為當地緯度。

為便于推導,將式(18)簡化為:

DikXik=dik

(19)

(20)

通過最優化中的Lagrange乘子法求解式(20):

(21)

由式(19)可知DikXik-dik=0,則:

(22)

(23)

求出各子濾波器在狀態約束下的狀態估計值及其協方差矩陣后,按照式(4)和式(5)融合,即可得到狀態約束下聯邦濾波的全局狀態估計值及其協方差矩陣。

3 CON-RAFKF算法

當載體處于一些復雜環境時,傳感器可能會出現觀測數據缺失或因受到外界干擾使觀測量出現異常的情況。為進一步提高導航系統的定位精度和可靠性,對上節提出的CON-FKF算法進行改進。首先,在各個子濾波器中加入Huber抗差方法,以抵制觀測異常對導航精度的影響;其次,在信息分配階段加入基于馬氏距離的自適應信息分配因子,以進一步減小子濾波器的不準確估計對全局估計的影響[8]。

3.1 Huber抗差方法

Huber抗差方法通過結合l1、l2兩種范數來構建代價函數,其抗差性優于基于l2范數的估計方法。Huber代價函數表達式為[6]:

(24)

Huber權值函數表達式如下[11]:

(25)

式中,φ(vj)由ρ(vj)求導所得。

(26)

利用上式對觀測噪聲矩陣Σik進行修正,以提高系統的抗差性,修正公式為:

(27)

3.2 計算信息分配因子

在傳統的聯邦濾波中,信息分配因子被設置為固定值,當子濾波器出現異常情況時,其錯誤的估計信息會被帶入到主濾波器中,導致系統的容錯性降低。因此,自適應確定信息分配因子對提高系統性能具有重要意義[10,12]。本文選取第i個子濾波的狀態估計和融合后的狀態估計的馬氏距離作為評價子濾波器濾波效果的指標,并以此來計算信息分配因子。具體公式為:

(28)

(29)

4 仿真實驗及分析

初始誤差協方差矩陣和過程噪聲矩陣設置為:

ΣX0=diag[(1′)2,(1′)2,(1.5′)2,

(0.2 m/s)2I3×3,(10 m)2,(10 m)2,(20 m)2

0.5(°)/h)2I3×3,(2 000μg)2I3×3]

(30)

(31)

觀測噪聲設置為:

R1k=diag[(5 m)2,(5 m)2,(10 m)2]

(32)

R2k=diag[(0.1 m/s)2,

(0.1 m/s)2,(0.1 m/s)2]

(33)

4.1 測試1

測試1主要分析加入約束條件對FKF狀態估計的影響。由圖2和3可知,加入約束條件后,狀態估計精度有所提高。為了防止隨機性,對該系統進行50次蒙特卡洛仿真,計算50次仿真的RMSE,得到圖4和5。可以看出,加入約束條件后,水平方向位置的平均RMSE分別下降12.46%、34.27%,速度的平均RMSE分別下降10.87%、34.80%;高程方向CON-FKF與FKF相比,位置和速度的RMSE變化都很小,這是由于本文采用的約束信息僅針對水平方向,在高程方向沒有約束信息。

圖2 SINS/GNSS/ODO組合導航位置誤差

圖4 FKF和CON-FKF位置RMSE

圖5 FKF和CON-FKF速度RMSE

4.2 測試2

測試2主要是分析本文算法在SINS/GNSS/ODO組合導航系統出現異常時的性能(圖6和7)。在200～250 s,模擬ODO觀測異常,將10σv加入到ODO觀測的速度上,σv=[0.1 m/s,0.1 m/s,0.1 m/s];在400～450 s,模擬GNSS觀測異常,將10σp加入GNSS觀測的位置上,σp=[5 m,5 m,10 m]。

圖6 添加誤差后 SINS/GNSS/ODO組合導航位置誤差

圖7 添加誤差后 SINS/GNSS/ODO組合導航速度誤差

由圖6和7可知,FKF無法處理觀測情況,會把誤差信息帶入到最終融合解中,致使導航系統的準確性下降;CON-FKF由于加入了先驗約束信息,在一定程度上抑制了觀測異常;CON-RAFKF在CON-FKF的基礎上進一步加入了Huber抗差方法和自適應信息分配因子,與前2種算法相比,能夠更好地應對觀測異常的情況。

同樣,對觀測異常情況下的導航系統進行50次蒙特卡洛仿真,其RMSE結果見圖8和9,平均RMSE統計結果見表1。可以看出,與FKF相比,CON-RAFKF三方向的位置平均RMSE分別下降53.84%、68.12%、43.37%,速度平均RMSE分別下降40.05%、62.25%、39.21%;與CON-FKF相比,CON-RAFKF三方向的位置平均RMSE分別下降40.58%、38.60%、43.30%,速度平均RMSE分別下降26.11%、44.75%、39.07%,可見CON-RAFKF顯著提高了系統的定位精度和可靠性。

表1 3種算法的平均均方根誤差

圖8 添加誤差后3種算法的位置均方根誤差對比

圖9 添加誤差后3種算法的速度均方根誤差對比

5 結 語

針對多傳感器導航系統中狀態量之間存在先驗約束信息的情形,本文提出一種狀態約束的聯邦卡爾曼濾波算法。此外,當觀測量出現異常時,本文將Huber抗差方法及自適應信息分配策略加入到狀態約束下的聯邦卡爾曼濾波中。理論分析和仿真實驗表明,在聯邦卡爾曼濾波中,考慮先驗約束條件能夠有效提高組合導航系統的定位精度;在帶有約束條件的聯邦卡爾曼濾波中,加入Huber抗差方法及自適應信息分配策略能夠進一步提高復雜環境下組合導航系統的定位精度和可靠性。