GNSS/INS緊組合最大熵卡爾曼濾波算法

李松, 唐小妹, 孫鵬躍, 張可, 王飛雪

(國防科技大學 電子科學學院,湖南 長沙 410073)

0 引 言

GNSS/INS組合導航系統通過數據融合能夠克服單個系統存在的缺點,例如,全球衛星導航系統(GNSS)在遮擋環境下無法定位以及慣性導航系統(INS)隨時間的誤差累積問題. GNSS/INS組合導航又分為松組合與緊組合,其中緊組合利用偽距、偽距率等原始量測信息,能夠在可視衛星少于4顆時保證組合系統的正常工作. 數據融合算法對于GNSS/INS組合導航系統的性能起著關鍵作用[1]. 其中,卡爾曼濾波(KF)是應用最為廣泛的數據融合算法,且對于高斯系統而言,其能夠在最小均方誤差(MMSE)準則下得到狀態的最優估計. 然而,在實際應用中,對于GNSS/INS組合導航系統而言,GNSS接收機容易受到異常干擾的影響,并且在惡劣的環境(如城市樓宇中和樹陰下)中GNSS的量測質量可能會降低[2],進而導致量測野值的存在. 因此,系統實際面對的是非高斯的厚尾噪聲[3],而傳統的KF無法應對這種情況并導致估計精度降低.

目前,能夠處理非高斯噪聲的濾波算法包含以下幾類：1)序貫蒙特卡洛采樣方法,其中粒子濾波是最常用的技術,它使用一組帶有權值的隨機樣本來近似狀態的后驗分布,能實現對任何概率分布的近似[4],但其存在計算量大,粒子退化和粒子貧乏等問題,使其難以滿足實時性的需要[5-6]; 2)多模型濾波方法,其中高斯和濾波算法較為典型,在這種方法中,用若干個高斯分布的加權和來近似非高斯分布的噪聲[7],因此,全局狀態后驗估計則為若干個并行濾波算法的加權和. 然而,這種方法同樣面臨計算復雜度的問題,其計算復雜度隨著子高斯項的增加而顯著增加; 3)M估計或抗差估計方法,其通過增大異常量測值對應的量測噪聲協方差矩陣元素,能夠有針對性地減輕某一測量通道的異常量測值的影響. 這類方法計算復雜度較低,比較實用,并已成功運用于組合導航領域. 比如,文獻[8]基于IGGIII模型構建的抗差KF算法能夠抑制緊組合的偽距和載波相位量測野值的影響. 然而,如果量測野值較大,則會影響這類方法對于異常量測值的檢測,進而導致其性能降低. 文獻[9]利用新息構造檢測量,利用Huber函數計算等價協方差矩陣,對量測野值具有一定的抑制作用,但其無法將明顯異常的量測值完全丟棄,并且對于近似正常的量測值分配的權重較小,難以充分利用量測信息.

因此,為了提高濾波算法在實際環境下的魯棒性,有必要開發新的魯棒濾波算法. 近年來,由于相關熵包含信號的高階統計信息,文獻[10-11]將相關熵的概念引入到濾波算法的設計中,基于最大熵準則(MCC)推導了一系列最大熵濾波算法,并證明了其在非高斯噪聲環境下的魯棒性. 然而,由于這些濾波算法是通過修改卡爾曼增益而非量測噪聲協方差矩陣,因此對于GNSS/INS緊組合導航而言,其無法有針對性地抑制某顆衛星的異常量測值,因此在實際應用中表現不佳.

本文基于MCC和M估計的思想,將相關熵的概念引入KF的代價函數中,推導了最大熵卡爾曼濾波(MCKF)算法. 該算法結合了最大熵方法和M估計方法的優點,與傳統KF相比,所提算法通過權矩陣修改了量測噪聲協方差矩陣,使得異常量測值能夠被分配較小的權重,與函數的卡爾曼濾波(HKF)相比,其能夠更有效地利用量測信息,因此所提算法更加魯棒. 通過GNSS/INS緊組合車載實測實驗,證明了所提算法在實際環境下的魯棒性,結果表明其能夠得到更高的估計精度.

1 最大熵卡爾曼濾波

本節首先介紹MCC的概念,然后基于M估計的思想,將其與KF的代價函數結合,進而推導出所提MCKF算法,最后對算法的魯棒性進行分析.

1.1 最大熵準則

考慮隨機變量X和Y,兩者之間的相關熵定義為

V(X,Y)=E[κ(X,Y)]=?κ(x,y)p(x,y)

dxdy,

(1)

式中:E表示求期望;p(x,y)表示隨機變量X和Y的聯合概率密度函數;κ(x,y)表示核函數.

由于實際情況中聯合概率密度p(x,y)難以精確獲得,一般可用樣本的平均值來計算相關熵的估計值:

(2)

通常將核函數選取為高斯核函數,即

(3)

式中,ei=xi-yi;σ為核函數帶寬.

對高斯核函數進行泰勒展開,得

(4)

可以看出,相關熵為X-Y的所有偶數階矩的加權和,因此其包含了高階矩信息. 同時,當且僅當X=Y時,V(X,Y)取最大值1.

1.2 MCKF推導

考慮如下線性離散時間系統:

xk=Φk|(k-1)xk-1+wk-1,

(5)

zk=Hkxk+vk.

(6)

式中:xk和zk分別為k時刻的n維狀態向量和m維量測向量;Φk|(k-1)和Hk分別為系統轉移矩陣和量測矩陣;wk-1和vk分別為相互獨立且均值為零的過程噪聲和量測噪聲,其協方差矩陣分別為Qk-1和Rk.

在MMSE準則下,通過最小化如下的代價函數可以得到KF的遞推表達式.

(7)

(8)

式中，ek,i表示ek的第i個分量,i=(1,…,m). 可以看出,在MMSE準則下,KF的代價函數對于所有量測量的殘差分配相等的權值,因此其無法減輕異常量測值的影響.

為了增強濾波算法在非高斯量測噪聲下的魯棒性,基于M估計的思想,將相關熵的概念引入代價函數的量測部分,將式(8)的代價函數修改為

(9)

可以看出,式(9)用核函數代替了KF中量測誤差部分的MMSE估計.

對式(9)在xk處求導,并令其為零,得

(10)

式(10)可以進一步寫為如下的矩陣形式:

(11)

式中：ψk=diag[ψ(ek,i)]為權矩陣；ψ(ek,i)=Gσ(ek,i)為權函數；diag[·]表示取對角矩陣.

將ek的表達式代入式(11),得

(12)

式(12)可視為式(13)在xk處的導數:

(13)

下面總結MCKF的濾波流程.

1) 濾波初始化

2) 時間更新

3) 量測更新

根據時間更新后所得的量測信息計算ek,然后基于ek計算權矩陣ψk,并用權矩陣修改量測噪聲協方差矩陣:

Pk=(In-KkHk)Pk|(k-1).

其中,In表示n×n維的單位矩陣.

1.3 算法魯棒性分析

將本文的基于MCC的權函數與文獻[9]中的Huber權函數以及MMSE準則下的權函數進行對比,結果如表1和圖1所示.

表1 不同準則下的權函數形式

圖1 不同準則下的權函數示意圖

表1描述了不同準則下的權函數形式,圖1給出了相應的權函數示意圖,其中,本文將Huber函數調節因子取為γ=1.345,MCC核函數帶寬取為σ=5. 從圖1可以看出,在MMSE準則下的權函數ψMMSE(e)對任何量測量都分配相等的權重,因此當存在量測野值時,KF無法抑制異常量測值的影響,其魯棒性與估計精度均受到影響. 而Huber權函數ψHuber(e)與基于MCC的權函數ψMCC(e)均能夠根據量測量的殘差給異常量測值分配較小的權重,因此兩者均能減輕異常量測值對于狀態估計的不良影響,進而提高算法魯棒性. 然而,與ψHuber(e)相比,ψMCC(e)以指數形式衰減,呈現出先慢后快的衰減趨勢,因此其能夠給近似正常的量測值分配相對較大的權重以有效利用量測信息,而對于明顯異常的量測值,其能夠快速衰減到零附近,這表明異常量測信息將被丟棄.ψHuber(e)的衰減趨勢與ψMCC(e)相反,其無法較好地利用近似正常的量測值,且對于明顯異常的量測值,由于ψHuber(e)無法取到零附近,因此其不能徹底丟棄異常量測信息. 因此,相比于HKF,MCKF算法具有更強的魯棒性.

2 GNSS/INS緊組合模型

本文對所提算法在GNSS/INS緊組合實驗中進行驗證,首先構建其狀態模型和量測模型.

2.1 狀態模型

本文采用INS在東北天導航坐標系下的誤差狀態模型,狀態量為

xINS=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,

(14)

式中:(φE,φN,φU)表示INS在東北天方向的姿態誤差;(δvE,δvN,δvU)表示東北天方向速度誤差;(δL,δλ,δh)表示緯經高方向位置誤差;(εx,εy,εz)和(x,y,z)分別表示載體坐標系下陀螺儀常值漂移和加速度計零偏.

INS的誤差方程可以寫為如下的微分方程

(15)

式中:FINS(t)和wINS(t)的具體形式可參考文獻[12].由于GNSS接收機的鐘差和鐘漂會影響量測的偽距和偽距率,因此還需要對這一部分誤差進行估計,GNSS接收機的誤差方程為

(16)

因此,聯立式(15)和式(16),可得GNSS/INS緊組合的狀態方程為

(17)

2.2 量測模型

(18)

(19)

(20)

(21)

(22)

式中:R1=RN+h;R2=RN(1-e2)+h;RN為卯酉圈曲率半徑;e為地球偏心率.

因此,結合式(19)、(20)可得量測方程為

zk=Hkxk+vk，

(23)

其中,

3 實測實驗

為驗證所提MCKF算法在GNSS/INS緊組合下的有效性,進行車載實測實驗,在城市環境下采集了一組車載GNSS/INS導航實驗數據,時間持續約50 min. GNSS數據由頻率為1 Hz的偽距、偽距率組成,INS數據頻率為100 Hz. INS采用閉環校正的方式,組合周期為1 s,每次量測更新后將狀態量置為零. 初始狀態協方差中位置誤差設為10 m,速度誤差設為0.1 m/s,姿態角誤差設為20°;偽距量測噪聲設為2.5 m;偽距率量測噪聲設為0.1 m/s.

圖2～3分別示出了實驗過程中的可視衛星數和三維位置精度因子(PDOP). 從圖2可以看出,實驗過程中可視衛星的數目頻繁波動,且在量測時段末端,可視衛星數降為5顆. 從圖3也可以看出,實驗過程中的PDOP值波動較大,量測質量較差.

圖2 可視衛星數

圖3 PDOP值

采用本文提出的MCKF算法與HKF以及KF分別對導航數據進行處理,不同算法的位置誤差結果如圖4～6所示. 從圖4～6可以看出,KF的結果含有較多突刺,這些突刺大多出現在如圖2所示的衛星數較少的時刻和如圖3所示的PDOP值較大的時刻,這表明隨著量測質量下降,量測量中不可避免地存在野值,這破壞了對于噪聲為高斯分布的假設,進而使得KF的估計性能隨之下降,其位置誤差最大達到了20 m(高度方向). HKF與MCKF均能夠對異常量測值分配更小的權重,因此HKF與MCKF的結果更為平滑,估計性能較KF而言有所改善. 然而,由于KF無法抑制異常量測值,而HKF如1.3節所述無法有效地利用量測信息,因此兩者在實驗初期北向和高度方向的定位誤差很大,HKF的位置誤差最大達到了30 m(高度方向),兩者均需要較長時間才能收斂. 然而,如前所述,本文提出的MCKF算法能夠對異常量測值分配更小的權重,且相比于HKF能夠更有效地利用量測信息,因此,從圖4～6可以看出,MCKF的結果在實驗初期能夠快速收斂,其整體結果相比于KF和HKF而言更好.

圖4 北向誤差

圖5 東向誤差

圖6 高度誤差

圖7 不同算法對1號星偽距分配的權重

圖8 不同算法對1號星偽距率分配的權重

圖7～8分別示出了實驗過程中不同算法對于1號星偽距與偽距率量測值分配的權重,其中1號星累計可見時長為2 903 s,其不可見時段在圖中被忽略. 從圖7可以明顯看出,HKF對偽距分配的權重集中在0.5以下,而MCKF分配的權重更加均勻,因此MCKF相比于HKF能更有效地利用量測信息. 從圖8可以看出,HKF對偽距率分配的權重無法取到零附近,而MCKF能夠近似取到零附近,因此MCKF相比于HKF能更有效地丟棄異常量測信息. 因此,圖7～8的結果與本節以及1.3節中對算法性能的分析是相吻合的.

表2示出了不同算法所得結果的均方根誤差比較. 從表2可以看出,傳統KF由于無法抑制異常量測值的影響,因此其均方根誤差較大,HKF能夠減輕異常量測值的影響,但在實驗初期收斂較慢,其性能同樣受到影響,而由本節以及1.3節可知,本文提出的MCKF算法能夠在充分利用量測信息的同時丟棄明顯異常的量測信息,因此其對于量測野值幅度較大的場景尤其適用. 對于GNSS/INS緊組合導航而言,在多徑衰落環境下工作或接收機的時鐘發生異常均可能導致量測偽距與偽距率有較大幅度的跳變,此時所提MCKF算法能表現出較好的魯棒性.

表2 不同算法均方根誤差比較

4 結束語

本文基于MCC和M估計的思想,提出了MCKF算法. 通過GNSS/INS緊組合車載實測實驗對所提算法進行了驗證,結果表明,由于實驗過程中可視衛星數和PDOP值頻繁波動,緊組合采用的GNSS的偽距與偽距率等原始量測信息的質量不佳使KF的收斂速度與估計精度均受到影響,且誤差曲線突刺較多,HKF能夠減輕異常量測值的影響,但由于無法有效利用量測信息,其在實驗初期收斂較慢,整體性能也受到影響,而本文提出的MCKF算法相比于KF和HKF能夠有效地抑制異常量測值的影響,從而更快地收斂,并且其誤差曲線更為平滑,因此其估計精度也更高. 因此,本文提出的算法在實際環境中魯棒性更強,適用于GNSS/INS緊組合導航.