基于雙因子抗差估計的SINS/BDS有軌電車組合導航算法研究

何浩洋，夏榮斌

(1.南京恩瑞特實業有限公司，江蘇 南京 211100; 2. 蘭州交通大學 常州研究院，江蘇 常州 213000)

隨著城市軌道交通的不斷發展，作為列車運行控制基礎的列車定位技術發揮著日益重要的作用。實時、精確、可靠的列車位置信息是縮短列車追蹤間隔，提高列車調度效率，保證列車行車安全，實現列車最佳服務的前提。目前城市軌道交通主要采用速度里程計和多普勒雷達，隨著有軌電車、無人駕駛、車車通信和市域城際鐵路，以及北斗衛星的不斷發展，如何利用北斗衛星的優勢，把北斗系統應用于列車定位領域，構建基于北斗衛星的“天空地一體化”列車車載自主定位系統，降低列車定位系統建設和維護成本，提高列車定位精度，確保列車安全行車間隔，提升列車移動閉塞效率，是列控系統向智能化和信息化及四網融合發展的需要。目前，在基于北斗(Beidou Navigation Satellite System, BDS)的定位應用系統中，與其慣性導航系統(Strapdown Inertial Navigation System，SINS) 的組合導航是應用最廣的組合模式之一。然而在實際應用系統中，北斗信號容易受到列車復雜運行環境的影響導致觀測數據中引入異常噪聲，以及列車運行過程中的高頻震動會導致SINS解算精度結果下降。并且，組合導航計算機信息融合結果輸出時刻一般與實際傳感器的輸出存在一定的時間滯后，這種滯后性將帶來較大的時間不同步誤差，降低定位系統的實時性。

目前，針對組合導航系中的噪聲抑制，提高濾波估計精度和增強濾波過程的魯棒性問題，國內外學者做了大量的研究，提出了如基于M估計的抗差濾波方法[1-2]，基于信息χ2檢測的粗差探測方法[3-4]以及基于Huber的魯棒濾波方法[5]等。經典的多傳感器信息融合算法多以Kalman濾波理論為核心，后提出的擴展Kalman濾波算法(Extended Kalman Filtering, EKF)[6-7]、無跡Kalman濾波算法(Uncensed Kalman Filtering, UKF)[8]和容積Kalman濾波算法(Cubature Kalman Filtering, CKF)[9-10]解決非線性系統線性化的問題，優化傳統Kalman濾波只適用于線性系統的缺陷，而針對系統噪聲均方差陣模型與實際噪聲特征不匹配則提出了自適應Kalman濾波算法(Adaptive Kalman Filtering, AKF)[11]。隨著傳感器維數的增加和列車運行環境的復雜多變，傳感器有時會產生異常觀測，而這些異常觀測對融合精度有很大的影響且難以被建模和預測，為了保證系統的容錯能力，使系統能實時檢測并隔離故障傳感器的測量結果，保證整個系統在有故障的情況下依舊能正常工作，需要對傳感器融合架構進行優化，改變傳統的融合方式。戴海發等[12]等提出一種改進的多傳感器數據自適應融合方法, 在貝葉斯理論的框架下，利用傳感器測量值之間的差值自適應建立傳感器的后驗概率分布模型，并結合互信息的理論實時識別和剔除異常觀測值，在存在異常觀測值的情況下，信息融合的性能明顯優于一般的貝葉斯融合方法。嚴春滿等[13]針對濾波算法在模型失配和噪聲時變情況下出現精度下降甚至發散的問題，提出一種基于多重漸消因子的自適應IEKF算法，利用一個基于正態分布的限定記憶信息協方差估值器來計算信息協方差估計值，并根據估計均方誤差把多重漸消因子分配給各數據通道；再依照χ2檢驗原理判斷系統是否異常，僅在系統異常時才引入漸消因子；最后利用目標與觀測站間的徑向距離和方位角信息，實現了IEKF迭代次數的自適應控制。在組合導航延時誤差修正方面，付廷強等[14]建立了GNSS/SINS系統的延時估計模型，估計時間同步誤差，然后構建殘差傳播方程，利用殘差重構的方式進行延時補償，實現基于軟件的時間同步。

針對組合導航中的各導航傳感器的信息更新頻率異步導致的延時誤差和列車復雜環境導致異常噪聲導致估計精度下降問題，提出一種基于雙因子抗差濾波和時延估計與補償的SINS/BDS列車組合定位算法，在傳統組合估計模型中引入延時估計模型，并利用雙因子敏感系統的動力學模型誤差和觀測模型誤差，實現不同導航子系統的時間不同步估計與補償和降低噪聲對導航精度的影響。

1 SINS/BDS松組合誤差模型

松組合數據融合常采用Kalman濾波方法，利用慣導的誤差方程作為系統方程，根據應用環境及需求來選擇系統誤差狀態量，并構建狀態方程。量測方程是利用BDS輸出的位置、速度信息與INS更新的位置、速度信息差值作為Kalman濾波器的量測輸入，濾波估計的狀態量通過反饋校正來實時修正組合系統的導航信息，從而獲得組合導航系統的輸出值，其系統模型見圖1。

圖1 SINS/BDS松組合結構圖

在位置速度方式下的松組合導航誤差模型中，選取慣導系統中的姿態誤差、速度誤差、位置誤差以及陀螺儀和加速度計的常值漂移誤差作為組合導航系統的狀態變量，采用位置量測值(慣導系統給出的緯度、經度及高度信息和BDS接收機給出的相應位置信息的差值)和2個系統給出的各方向上速度的差值作為量測信息，系統的狀態空間模型如下[15]

( 1 )

( 2 )

2 SINS/BDS延時誤差模型

在SINS/BDS組合導航系統中，由于BDS和SINS系統更新時間的不同，組合導航計算機對各導航子系統的采樣時間也存在差異性，傳感器信息之間不同步造成導航計算結果的時間延遲誤差，尤其針對160 km/h及以上的市域、城際列車來說，時間滯后將引起較大的速度和位置誤差，必須對時間不同步誤差進行估計和補償，BDS與SINS之間的時間不同步模型見圖2。

由圖2可知，SINS的慣性滯后與BDS的衛星滯后并不相同，兩類傳感器之間的相對滯后為時間不同步誤差，且完整的時間延時補償應包含SINS與BDS之間導航計算機采集的時間差以及定位更新的耗時時間。

圖2 SINS/BDS時間不同步模型

2.1 基于SINS/BDS的列車組合模型

基于SINS/BDS的列車組合模型中，Kalman濾波算法是一個被廣泛使用的信息融合算法。然而在實際應用系統中，由于列車運行環境的復雜多變會導致觀測數據中存在異常噪聲，影響組合導航的定位精度，降低了列車運行的可靠性。因此，提出一種基于雙因子抗差估計的SINS/BDS組合算法模型，其原理見圖3。由IMU輸出的角速度和加速度信息經過慣性導航系統解算出速度和位置信息，然后與BDS輸出的速度位置信息進行融合，經過雙因子抗差濾波估計，最終輸出高精度的組合定位信息，實現列車可靠穩定運行。

圖3 基于雙因子抗差估計的SINS/BDS的列車組合系統框原理

2.2 系統狀態模型

根據式(1)和式(2)，在SINS誤差模型的基礎上，考慮桿臂誤差和時間不同步誤差，系統19維狀態向量為

( 3 )

2.3 系統量測模型

在慣導誤差分析的基礎上，考慮SINS與BDS之間的桿臂誤差和時間不同步誤差，則基于速度和位置觀測的SINS/BDS系統量測模型如下

(4)

2.3.1 桿臂誤差模型

SINS定位解算基準為IMU的幾何中心，BDS以接收機天線的相位中心為參考基準，在列車組合導航系統中，SINS與BDS安裝位置之間的偏差通常會引入一定程度的速度和位置誤差，桿臂誤差示意見圖4、圖5。

圖4 列車安裝桿臂誤差示意

圖5 SINS與BDS之間的桿臂誤差

圖5中，b系為SINS所在載體坐標系；e系為地理坐標系；n系為東-北-天導航坐標系。假設SINS相對于地心的矢量為r，BDS相對于地心的矢量為R，BDS中心相對于SINS的矢量為δl，則三者之間滿足

r=R+δl

( 5 )

對式(5)求導，得相對于地理坐標系e下得關系

( 6 )

式中：ωeb為SINS輸出的相對于地理坐標系的角速度，SINS與BDS由桿臂引起的速度誤差可表示為

( 7 )

將式(7)投影至導航坐標系，則

( 8 )

相應地，位置誤差可表示為

( 9 )

2.3.2 時延誤差模型

在基于BDS/SINS的列車車載自主定位系統中，組合導航計算時刻與傳感器信息采集之間通常包含一定的時間滯后，見圖6。

圖6 BDS與SINS時間不同步誤差

SINS速度與BDS速度之間的關系可表示為

(10)

式中：an為列車不同步時間的平均線加速度，利用SINS高頻率、短時精度高的特點，an可近似為

(11)

相應地，位置不同步誤差可表示為

(12)

綜合桿臂誤差和時延誤差模型，式(4)中量測轉移矩陣和量測噪聲矩陣為

(13)

(14)

式中：Vv為速度噪聲；Vp為位置噪聲。

3 自適應雙因子抗差估計

由于有軌列車跨域長、運行環境復雜多變，BDS受到多徑干擾、隨機噪聲，SINS受到列車機動干擾等多種噪聲影響導致系統量測中包含各種異常野值，從而影響組合導航的系統精度。針對傳統Kalman系統對噪聲不敏感的特點，引入自適應雙因子抗差濾波算法，對組合系統的動力學模型和異常野值引起的噪聲模型不適應問題，利用動力學模型信息自適應因子和觀測模型自適應因子建立系統局部模型認可度，調節模型誤差和觀測誤差對狀態估計的影響。

3.1 自適應抗差算法

針對有軌列車非線性特點，對式(1)進行離散化，系統非線性模型為[4]

(15)

式中：Φk/k-1為狀態一步轉移矩陣；Γk/k-1為系統噪聲分配矩陣；Hk為量測矩陣；Wk-1為l維的系統噪聲向量；Vk為m維的量測噪聲向量，兩者都是零均值的高斯白噪聲向量序列(服從正態分布)，且它們之間互不相關，即滿足

式中：E[]為求矩陣的誤差。

根據最小二乘原則構造損失函數Jk

(16)

(17)

(18)

則式(18)可變換為

(19)

綜上，根據Kalman估計原則，雙因子抗差迭代求解算法為

(20)

3.2 自適應因子求解

(1) ?k觀測自適應因子觀測自適應選取

(21)

式中：tr(·)為矩陣的跡；ek為預測殘差向量

ek=Zk-Zk,k-1=Zk-HXk,k-1

(22)

相應地

式中：c0和c1為常量，c0取值范圍為1.0～1.5，c1取值范圍為3.0～4.5。

(2)βk動力學模型信息的自適應因子選取

(23)

式中：dk表示預測狀態殘差,可表示為

(24)

相應的

4 仿真及實驗

式中：c0和c1為常量，c0取值范圍為1.0～1.5，c1取值范圍為3.0～4.5。

4.1 仿真

為了驗證本文所提出的時延計算模型和雙因子抗差估計算法的有效性進行相關仿真，傳感器具體參數如表1所示。

表1 傳感器仿真參數

整個仿真過程持續時間為673 s，初始位置為[32.106°N, 103.726°E, 100 m]，初始姿態為[0°, 0°, 0°]，初始速度為0 m/s，SINS輸出頻率為200 Hz，BDS輸出頻率為1 Hz，SINS與BDS之間存在0.1 s的延時，桿臂誤差為[1.0，2.0，2.5] m，假設系統噪聲為已知的高斯白噪聲，仿真理想軌跡和速度見圖7、圖8。

圖7 理想軌跡示意圖

圖8 理想速度示意圖

(1) 延時估計模型仿真比較

為驗證組合系統模型的可行性，將傳統SINS/BDS松組合模型和延時估計模型進行比較，圖9、圖10為仿真的速度誤差和位置誤差。

圖9 速度誤差比較

圖10 位置誤差比較

從圖9和圖10可以看出，當SINS與BDS之間存在桿臂誤差和延時誤差時，采用延時估計模型能提高更好的精度。圖11和圖12為系統延時估計結果和桿臂誤差估計結果，與系統仿真所設置的0.1 s延時誤差和[1.0，2.0，2.5] m的桿臂誤差結果相一致。

圖11 延時估計結果

圖12 桿臂估計結果

(2) 抗差估計仿真驗證

為驗證本文所提出的雙因子抗差濾波算法對定位系統的抗野值效果，在BDS定位系統測量得到的位置和速度信息中增加了一定的噪聲，量測噪聲根據如下方式產生

(25)

(26)

式中：PBDS為北斗位置的噪聲特性；VBDS為北斗速度的噪聲特性；～為仿真的北斗速度和位置測量的誤差分布；w.p.為概率，即量測信息中有10%的概率出現方差為正常值200倍或以上的量測噪聲，以此來模擬BDS量測信息中的異常值。

圖13為BDS有無野值的位置仿真結果，圖14為BDS有無野值的速度仿真結果，圖15和圖16為仿真位置誤差和速度誤差示意。

圖13 BDS仿真位置數據

圖14 BDS仿真速度數據

圖15 位置誤差

圖16 速度誤差

從圖15和圖16中可以看出，紅色曲線所表示的基于線性Kalmam濾波的松組合方法受到BDS系統量測異常值的影響，其速度和位置誤差具有較大的波動性，即在量測存在野值的情況下線性卡爾曼濾波方法已不再適用。藍色曲線代表基于雙因子抗差估計的魯棒濾波松組合方法(抗差Kalman)，在BDS系統量測信息出現野值時，雙因子抗差Kalman方法的位置誤差仍然會出現一定程度的波動，這是由于抗差估計方法對于量測信息中的較大野值無法完全隔離，量測異常信息仍然對濾波結果造成的一定的影響。然而從藍色曲線和紅色曲線的波動情況可知，本文提出的抗差濾波估計算法具有較高的精度，比傳統Kalman算法能更好的提高組合導航系統的定位性能。

為了更加直觀的對比量測異常情況下算法的定位精度。表2給出了定位結果的均值、標準差和均方根誤差統計結果。

從表2中可以看出，在異常量測噪聲的影響下，基于雙因子的SINS/BDS抗差估計算法具有更高的精度。

表2 Mean和RMSE統計結果

4.2 車載實驗

為了驗證基于雙因子抗差估計的SINS/BDS有軌列車組合導航算法研究在實際工程中的有效性及可靠性，利用有軌電車車載試驗進行驗證。圖17為測試平臺設計原理。

圖17 車載定位系統測試平臺設計原理

本次實驗持續時間為1 500 s，傳感器參數與仿真實驗相同。由于BDS收到多徑干擾、空間環境和各種不確定呢因素的干擾，BDS的位置結果包含不同程度的隨機噪聲。實驗同時比較了傳統Kalman算法、基于時延估計的Kalman算法D-Kalman以及雙因子抗差估計算法(R-Kalman)對車載自主定位系統的最優估計結果。表3為此次車載實驗的速度和位置誤差統計。圖18、圖19和圖20給出此次車載實驗的軌跡結果、速度誤差結果和位置誤差結果對比，圖21為桿臂估計結果，圖22為時延估計結果。

圖18 車載實驗軌跡結果

圖19 速度誤差比較

圖20 位置誤差比較

表3 速度、位置誤差統計表

圖21 車載實驗桿臂估計結果

圖22 車載實驗延時估計結果

如圖19和圖20所示，傳統Kalman估計對量測數據中的異常值較敏感。通過對表3中的統計結果的分析，可以得出結論，本文所采用的抗差延時估計算法算法具有最佳的定位性能，其x軸速度誤差的均方根誤差為0.023 1 m/s,比延時Kalman和傳統Kalman分別降低了74.53%和78.63%，y軸降低了58.3%和68.38%，x軸位置誤差降低了62.51%和71.28%，y軸位置誤差降低了67.49%和64.49%。由實驗結果分析可知，對SINS與BDS安裝位置引起的桿臂誤差和數據采集頻率差異引起的子系統之間的延時誤差進行有效估計和補償能提高系統的定位精度。并且，針對列車定位復雜環境對量測地影響，本文所采用的雙因子抗差估計算法能有效地抑制系統異常噪聲，提高定位精度。

5 結論

本文提出了一種基于雙因子抗差估計的SINS/BDS有軌列車組合導航算法，對SINS/BDS之間的位置安裝引起的桿臂誤差和子系統數據采集引起的時間滯后誤差進行估計和補償。同時，為了修正中高速列車高動態復雜環境中異常野值對定位精度的影響，引入雙因子抗差估計算法，通過自適應因子降低量測噪聲對系統動力學模型和噪聲模型的影響。通過仿真和車載實驗證明，本文提出的模型和算法能更好適應有軌列車的實際情況，對異常野值具有更好的抑制能力，相對于傳統組合導航系統，定位精度更高。