基于相關熵的自適應擴展卡爾曼濾波定位方法

王韋舒，上官偉,2,3，劉 江,2,3，姜 維,2,3

(1.北京交通大學 電子信息工程學院，北京 100044；2.北京交通大學 軌道交通控制與安全國家重點實驗室，北京 100044；3.北京市電磁兼容與衛星導航工程技術研究中心，北京 100044)

隨著衛星導航系統的發展完善，以衛星定位為主要特征的下一代列車運行控制系統已成為新的重點研究方向。衛星定位技術的引入，旨在實現列車自主感知，減少對軌道電路或應答器等地面設備的依賴，提高車載設備的智能化水平，降低運營維護成本[1-2]。通過衛星導航技術進行列車位置估計和列車完整性檢查，有助于推動既有列車運行控制系統中閉塞制式的轉變，由固定閉塞向移動閉塞的跨越將有利于實現列車動態高效追蹤，進一步縮短列車間隔，從而提高運輸效率。在列車動態追蹤運行中，精準可靠的列車位置將是列控系統生成列車許可和速度控制曲線的重要前提，直接影響著列車的安全運行。

目前，擴展卡爾曼濾波(Extended Kalman Filter, EKF)是實施衛星定位計算的常用方法，其估計準則是最小均方誤差(Minimum Mean Square Error, MMSE)準則，在量測噪聲服從高斯分布的條件下能夠實現列車位置狀態的最優估計。但衛星觀測量容易受到鐘跳、軌道參數建模誤差、多徑效應等的影響產生階躍故障或者瞬時粗大偏差[3]，量測噪聲不再服從高斯分布。如果不對異常量測進行處理，將會造成列車定位性能下降，影響列車控制決策。針對MMSE準則下殘差二次型對于故障的敏感性問題，故障檢測與排除(Fault Detection and Exclude, FDE)和魯棒濾波估計是兩種典型的解決方案。FDE的核心是排除所有可能會對定位結果產生不利影響的量測，即盡最大可能檢測出異常量測并剔除。當可見衛星數量足夠多時，FDE算法能夠保障定位估計過程的穩定性。但在受限觀測環境或可見衛星較少條件下，排除故障量測所導致的空間幾何結構的改變，同樣會直接影響到定位解算性能[4]。不同于FDE，魯棒估計方法能夠對未知統計特性的有界噪聲進行處理，重在通過權函數降低異常數據的不利影響。基于Huber的魯棒濾波已經應用于列車定位中用于抑制環境中多源噪聲對定位性能的影響[5-6]。但該方法受到權函數的約束，對于明顯異常的量測仍舊會賦予其一定的權值，使得濾波精度由于錯誤的量測而出現下降；并且對于近似正常的量測其權值較小，未能充分利用量測。

目前，基于信息理論學習的優化準則逐漸受到研究人員廣泛的關注[7]。其中，相關熵描述了兩個隨機變量之間的廣義相似性，相關熵越大表示估計值與真實值越接近[8]。與MMSE不同的是，相關熵能夠捕獲更高階的統計量，提取由于高斯性偏離引起的各種信息，包含了二階統計量沒有的大量有效信息。由相關熵特性產生的最大相關熵準則作為一種局部相似性度量，對量測噪聲非零均值、非高斯或者有較大異常值等情況都具有較強的抑制能力，開始逐漸引入到濾波理論中，增強既有濾波算法的魯棒性[9]。在此思路下，本文對傳統列車定位解算進行改進，推導最大相關熵準則下的擴展卡爾曼濾波，通過重建EKF的量測噪聲矩陣，實現對濾波增益中先驗信息的權重調整；在此基礎上，結合M估計理論提出一種自適應核寬度的擴展卡爾曼濾波定位方法，并利用現場數據對該方法進行驗證和分析。

1 問題描述

要實現列車自主定位，僅僅依靠衛星導航定位系統難以滿足安全需求。通常采用融合算法將不同類型的定位傳感器信息進行聯合處理，如慣性導航[10]、輪速傳感器[11]、激光雷達[12]、光電傳感器[13]等。其中慣性導航系統(Inertial Navigation System, INS)作為一種自主式導航系統，不依賴于外部信息，具有采樣頻率高、隱蔽性強、輸出信息豐富等優點，已成為解決自主定位問題的重要選擇[14]。根據信息融合的深度，可選擇松組合或者緊組合的方式將衛星導航系統與慣性器件測量的姿態、加速度相融合。與松組合相比，緊組合在偽距域將衛星原始觀測數據和INS計算的預測偽距/偽距率相結合，能夠克服可觀測衛星數量的約束，在觀測條件欠佳下仍能實現有效定位。GNSS/INS緊組合處理流程見圖1。

圖1 GNSS/INS緊組合處理流程

列車組合定位系統的性能主要取決于數據融合算法。對于一個非線性系統而言，其狀態方程和量測方程可表示為

( 1 )

式中：xk為k時刻列車狀態向量；zk為k時刻量測向量；f(·)、h(·)分別為二階可微的狀態方程和觀測方程；wk-1、vk分別為相互獨立且服從零均值高斯分布的過程噪聲、量測噪聲，相應的協方差矩陣分別為Qk-1、Rk，系統的先驗誤差協方差矩陣記為Pk-1。

通過最小均方誤差準則，損失函數JMMSE可表示為

( 2 )

求解argminJ(xk)能夠獲取系統狀態量統計意義上的最優估計，其前提在于系統噪聲和量測噪聲為服從零均值正態分布的高斯白噪聲。然而在實際應用中，由于運行環境等不確定因素的影響有可能導致量測故障或產生粗大偏差，此時如果繼續使用MMSE準則的濾波算法，將使得濾波估計性能顯著下降甚至濾波發散，造成較大定位偏差。因此，如何設計一個魯棒性強的濾波算法來保證列車定位性能是需要解決的關鍵問題。

2 基于相關熵的自適應擴展卡爾曼濾波

2.1 最大相關熵基本原理

相關熵是兩個隨機變量間的廣義相似度量。對于給定的隨機變量X和Y，其聯合分布函數為FX,Y(X,Y)，則熵V(X,Y)可以表示為

V(X,Y)=E[φ(X,Y)]=?φ(X,Y)dFX,Y(X,Y)

( 3 )

式中：E為計算期望；φ(X,Y)為核函數，通常選擇高斯核函數，表示為

( 4 )

其中，e=X-Y；σ為核寬度。

在實際場景中，只有有限的數據是已知的，并且聯合分布函數FXY(X,Y)大多數情況是未知的。在這種情況下，相關熵通過樣本均值估計得到，即

( 5 )

式中：e(i)=X(i)-Y(i)；N為抽取的總樣本數。

將高斯核函數按照泰勒級數展開，可得熵為

( 6 )

由式(6)可以看出，熵為變量X-Y各階項的加權和；核寬度為二階及更高階項的權重參數；核寬度越大，高階項的影響越小，熵值大小將主要由二階展開項所決定。

最大相關熵損失函數JMCC可以寫為

( 7 )

假設W為自適應系統中需要求解的參數向量，X(i)和Y(i)分別為系統模型輸出和期望響應，則通過最大熵得到的最優解可以表示為

( 8 )

2.2 基于相關熵的自適應擴展卡爾曼濾波

基于最大相關熵準則的特性，重新推導新準則下的擴展卡爾曼濾波。將式(1)線性化可得

xk=Fk-1xk-1+wk-1

( 9 )

zk=Hkxk+vk

(10)

將式(9)、式(10)重構成非線性遞推方程的形式，可表示為

(11)

可進一步得到

(12)

(13)

Lk=Γkxk+ek

(14)

為解決最小均方誤差準則對于故障的敏感性問題，不再采用式(2)作為代價函數，而是引入最大相關熵準則降低偽距量測故障所帶來的不利影響。重新構建的代價函數為

(15)

狀態量的最優解可表示為

xk=argmaxJMCC(xk)

(16)

通過對式(16)進行一階求導得

(17)

定義權重矩陣為

(18)

CP,k=diag(Gσ(e1,k),…,Gσ(em,k))

(19)

CR,k=diag(Gσ(em+1,k),…,Gσ(en,k))

(20)

(21)

將式(13)、式(18)代入式(21)可得

(22)

修改后的先驗誤差協方差矩陣、量測噪聲協方差矩陣為

(23)

(24)

(25)

(26)

(27)

上述過程推導了基于固定核寬度的相關熵擴展卡爾曼濾波(Fixed Maximum Correntropy Extended Kalman Filter, FMCEKF)，選用高斯核函數代替MMES估計，能夠獲取更高階的量測統計信息，降低含故障量測信息的權重，從而保證濾波估計性能的穩健性。當量測中夾雜有較大的偽距偏差時，濾波器的ek將大于正常值，通過核函數所獲得的權重值將非常小。不同核寬度d下權函數變化情況見圖2。

圖2 不同核寬度下權函數變化情況

由圖2可以進一步獲知，不同核寬度下對于殘差的權重分配差異較大。當核寬度較小時，相同量測誤差下所設定的權重更小，更有利于增強系統的穩健性。因此，如何選擇合適的核寬度將影響到濾波算法對異常點的削弱能力。

受到魯棒估計思想的啟發，將Pseudo-Huber權函數引入到FMCEKF來調整核寬度大小，提出一種自適應核寬度的相關熵擴展卡爾曼濾波(Adaptive Maximum Correntropy Extended Kalman Filter, AMCEKF)。根據量測質量的不同而設定可變的核寬度，最大程度上利用觀測到的信息，并削弱異常數據對估計結果的影響。

(28)

第i顆衛星觀測量對應的核寬度di,k可表示為

(29)

(30)

式中：ri為第i顆衛星的新息，由k時刻量測信息和狀態預測值之差得到；ξr,k為k時刻新息標準差；ωi,k為第i顆衛星的標準化新息；τ為調節因子，通常取1.210 7[13]。當一個或者多個量測受到外界擾動發生異常時，其標準化新息將顯著增大，所對應的核寬度將選擇一個相對較小的值，進而在增益矩陣中分配給其更小的權重比例，以提高狀態估計的魯棒性。

2.3 基于偽距/偽距率的組合定位系統

2.3.1 狀態方程

基于偽距、偽距率的GNSS/INS組合狀態方程由INS誤差方程和GNSS誤差方程組成，可以表示為

xk=Fk-1xk-1+wk-1

(31)

(32)

式中：Fk-1為狀態轉移矩陣，由k-1時刻GNSS、INS的誤差傳播模型組成。xk-1為系統的17維狀態估計量。

由慣性導航系統的姿態、速度、位置、加速度計零偏、陀螺漂移誤差，以及接收機的鐘差、鐘漂誤差組成，具體表示為

X=[φxφyφzδvxδvyδvzδxδyδzεxεyεz

(33)

式中：(φx,φy,φz)為姿態誤差；(δvx,δvy,δvz)為速度誤差；(δx,δy,δz)為位置誤差；(εx,εy,εz)為陀螺儀的三軸漂移誤差；(x,y,z)為加速度計的三軸隨機誤差；分別為等效鐘差距離誤差和等效鐘漂距離誤差。

2.3.2 量測方程

系統的量測方程可以表示為

zk=Hkxk+vk

(34)

式中：Hk為組合系統的量測矩陣；zk為系統量測值，由慣性導航推算得到的預測偽距、偽距率，以及定位接收機測量得到的偽距、偽距率所確定，表示為

(35)

(36)

同理，接收機與第i顆衛星的測量偽距為

δb+η+δe

(37)

式中：δb為接收機鐘差造成的距離誤差；η為衛星時鐘偏差、電離層誤差、對流層誤差的總和，能夠通過近似模型加以估計；δe為殘留的隨機誤差。

INS估計的偽距率可以表示為

(38)

(39)

同理，可將GNSS偽距率建模為

(40)

綜合式(36)～式(40)可得量測矩陣為

(41)

2.4 基于AMCEKF的組合定位算法

本文所提出的算法以EKF為基礎，根據最大相關熵準則和Pseudo-Huber權函數重新構造量測噪聲矩陣，實現自適應調節量測所占配比，充分利用所觀測到的量測信息，并降低故障量測對狀態估計的影響。具體步驟如下：

Step1初始化濾波器參數，包括狀態估計量X、系統噪聲、量測噪聲等。

Step2構建新息，根據式(35)～式(40)計算偽距和偽距率量測誤差，確定量測矩陣，并對式(13)采用Cholesky分解誤差協方差矩陣，進而計算系統新息。

Step3權重更新，根據式(28)對每一個量測計算特定核寬度，并結合式(18)重構權重矩陣。

Step4確定增益矩陣，根據式(23)～式(25)，由新的權重矩陣更新先驗誤差協方差矩陣和量測噪聲協方差矩陣，并計算各狀態的增益。

Step5濾波輸出，基于增益矩陣更新狀態估計量和誤差協方差矩陣。

3 驗證與分析

本文采用2019年于拉薩—林芝鐵路采集的列車行駛數據對所提出的定位算法進行測試和驗證。主要分析瞬時故障、單顆衛星階躍故障、多顆衛星階躍故障場景下算法的實際性能。

所選實驗數據位于日喀則—拉薩，列車上安裝諾瓦泰SPAN-FSAS分體式組合導航定位系統，用于采集衛星原始觀測量和慣性導航數據，數據更新頻率分別為1、200 Hz。在列車從日喀則向拉薩行駛過程中，衛星觀測條件良好，水平精度因子在0.6左右，可見衛星數不低于10顆。

3.1 瞬時故障下定位性能驗證

為測試組合定位算法在瞬時故障下的定位性能，需仿真故障衛星所對應的量測數據。選擇PRN 26號衛星，在原始觀測量中每隔50 s加入50 m的偽距偏差。分別用EKF、FMCEKF、AMCEKF算法對列車進行定位解算，瞬時故障下位置誤差見圖3。

圖3 瞬時故障下位置誤差

從圖3可以看出：EKF對于瞬時故障異常敏感，濾波估計始終處于跳變和收斂過程中，最大定位誤差達到了4.8 m。對于FMCEKF而言，不同的核寬度下定位結果差異較大；當核寬度選擇5.0時，在故障發生時會發生突變，定位結果與EKF相似，位置誤差在5.0 m左右波動；但不同于EKF，在無故障后FMCEKF能夠快速收斂；進一步當核寬度選取較小的值0.5時，大部分情況下定位誤差趨于穩定，能夠較好地抑制脈沖異常的影響。相比EKF和FMCEKF，AMCEKF估計結果更加平穩，均值為1.675 m，能夠最大程度上減少瞬時故障對于定位性能的影響，對所注入的偽距偏差具有較低的敏感性。

與FMCEKF相比，AMCEKF具有較強魯棒性的原因在于核寬度不再是恒定不變的值，而是隨觀測情況進行調整。瞬時故障中不同衛星量測的核寬度變化過程見圖4。當PRN 26存在故障時，在自適應調整策略下，核寬度均被設置成更小的值為0.32；無故障時，核寬度在0.85～0.96之間波動。這取決于不同時刻各個衛星的偽距新息變化。新息越大，所確定的核寬度越小，從而獲得較小的相對熵，以達到降低異常影響的目的。

圖4 瞬時故障中不同衛星量測的核寬度變化過程

為了對估計誤差對比情況進行量化分析，選擇均方根誤差RMSE和距離均方根誤差DRMS評價定位結果[16]，瞬時故障下不同算法定位誤差統計結果見表1。從均方根誤差來看，與EKF相比，核寬度=0.5的FMCEKF算法在北向、東向、地向上分別增加了26.8%、41.7%、13.3%；AMCEKF則增加了52.6%、78%、23%；對水平誤差，FMCEKF和AMCEKF分別增加了29.4%和56.5%。

表1 瞬時故障下不同算法定位誤差 m

3.2 階躍故障下定位性能驗證

為進一步檢驗所提算法的實際性能，對單顆衛星階躍故障和多顆衛星階躍故障兩種場景下的定位結果進行對比分析。單故障時，選擇PRN 25，在100～150 s注入50 m的偽距偏差；多故障時，在350～400 s對PRN 25、26號衛星注入30 m的偽距偏差。階躍故障下不同算法對列車位置估計的結果見圖5。

圖5 階躍故障下位置誤差

從圖5可以得出：單星故障時，EKF估計誤差會隨著時間不斷增大，北向、東向、地向最大誤差分別達到11、15、45 m。對于FMCEKF而言，如果核寬度選擇不當，其對異常的敏感性將會增強，如圖5中核寬度為5.0時所示；當核寬度為0.8時，在正常觀測范圍時，其估計性能與EKF基本一致；但在異常區域時，仍能保持穩定的精度水平，受到故障值的影響較小。AMCEKF的估計誤差略微有所增大，但北向、東向誤差最大不超過2 m，估計準確性和魯棒性均優于FMCEKF和EKF。多故障下，FMCEKF在核寬度為5.0時，對于東向、地向上的估計性能退化較為嚴重；而核寬度為0.8時，仍能夠保持穩定的估計性能。AMCEKF雖然略微有所浮動，但整體而言，無論是在正常還是故障條件下均能夠實現更高的定位精度。

階躍故障下不同衛星量測的核寬度變化見圖6，示出了實驗過程中PRN 22、25、26、31號衛星所設定的核寬度變化情況。由圖6可知，在100～150、350～400 s時刻，AMCEKF對故障衛星(PRN 25、26)的核寬度自適應調整到了0.3～0.4，通過較小的核寬度獲得增益矩陣中較小的權重，以減弱故障量測的影響。對于正常衛星(PRN 22、31)的量測，AMCEKF也能夠根據其觀測質量動態選擇相應的核寬度，從而實現充分利用有效量測的目的。

圖6 階躍故障下不同衛星量測的核寬度變化

階躍故障下不同算法定位誤差見表2。從表2可以看出，EKF算法由于無法抑制故障值的影響，其水平位置的均方根誤差較大；d=5.0時的FMCEKF在北向、東向的RMSE分別增加12.4%、7.9%，d=0.8時則增加了20.6%、62.9%；從DRMS來看，AMCEKF相比EKF、FMCEKF(d=0.8)，分別增加了61.2%、35%。總體可得，FMCEKF的估計性能受核寬度影響較大，選擇一個合適的核寬度將直接決定著定位算法的魯棒性能。AMCEKF能夠合理選擇相應的核大小，無論是在瞬時故障還是階躍故障情況下，都能表現出較好的魯棒性。

表2 階躍故障下不同算法定位誤差 m

4 結論

以衛星導航技術為核心的列車位置感知已經成為下一代列控系統實際工程應用中必須攻克的關鍵技術。面對復雜多變的環境，如何保障外界干擾條件下列車位置估計的準確性和穩定性，是列車組合定位融合估計中面臨的重大難題。

(1)針對外界異變條件下濾波精度下降，以及固定核寬度對估計性能的約束問題，將魯棒估計思想和信息理論學習相結合，融入到組合定位系統的狀態估計中，設計了基于最大相關熵準則的自適應擴展卡爾曼濾波定位方法。

(2)以拉薩—林芝鐵路某區段實測數據對所提出的算法進行了實際驗證。結果表明，所提方法在瞬時故障、階躍單故障和階躍多故障3種不同的場景中，均能夠有效抑制故障檢測對列車定位估計準確性的不利影響，能夠隨時變觀測質量動態調整各個衛星量測在定位估計中所占的比重，有效解決了故障量測帶來的估計失準問題，提高了整體定位精度，具有較強的魯棒性和適應性，體現出該方法在未來實際應用中的巨大潛力。