基于雙因子抗差估計(jì)的SINS/BDS有軌電車組合導(dǎo)航算法研究

何浩洋，夏榮斌

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

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

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

針對(duì)組合導(dǎo)航中的各導(dǎo)航傳感器的信息更新頻率異步導(dǎo)致的延時(shí)誤差和列車復(fù)雜環(huán)境導(dǎo)致異常噪聲導(dǎo)致估計(jì)精度下降問(wèn)題，提出一種基于雙因子抗差濾波和時(shí)延估計(jì)與補(bǔ)償?shù)腟INS/BDS列車組合定位算法，在傳統(tǒng)組合估計(jì)模型中引入延時(shí)估計(jì)模型，并利用雙因子敏感系統(tǒng)的動(dòng)力學(xué)模型誤差和觀測(cè)模型誤差，實(shí)現(xiàn)不同導(dǎo)航子系統(tǒng)的時(shí)間不同步估計(jì)與補(bǔ)償和降低噪聲對(duì)導(dǎo)航精度的影響。

1 SINS/BDS松組合誤差模型

松組合數(shù)據(jù)融合常采用Kalman濾波方法，利用慣導(dǎo)的誤差方程作為系統(tǒng)方程，根據(jù)應(yīng)用環(huán)境及需求來(lái)選擇系統(tǒng)誤差狀態(tài)量，并構(gòu)建狀態(tài)方程。量測(cè)方程是利用BDS輸出的位置、速度信息與INS更新的位置、速度信息差值作為Kalman濾波器的量測(cè)輸入，濾波估計(jì)的狀態(tài)量通過(guò)反饋校正來(lái)實(shí)時(shí)修正組合系統(tǒng)的導(dǎo)航信息，從而獲得組合導(dǎo)航系統(tǒng)的輸出值，其系統(tǒng)模型見(jiàn)圖1。

圖1 SINS/BDS松組合結(jié)構(gòu)圖

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

( 1 )

( 2 )

2 SINS/BDS延時(shí)誤差模型

在SINS/BDS組合導(dǎo)航系統(tǒng)中，由于BDS和SINS系統(tǒng)更新時(shí)間的不同，組合導(dǎo)航計(jì)算機(jī)對(duì)各導(dǎo)航子系統(tǒng)的采樣時(shí)間也存在差異性，傳感器信息之間不同步造成導(dǎo)航計(jì)算結(jié)果的時(shí)間延遲誤差，尤其針對(duì)160 km/h及以上的市域、城際列車來(lái)說(shuō)，時(shí)間滯后將引起較大的速度和位置誤差，必須對(duì)時(shí)間不同步誤差進(jìn)行估計(jì)和補(bǔ)償，BDS與SINS之間的時(shí)間不同步模型見(jiàn)圖2。

由圖2可知，SINS的慣性滯后與BDS的衛(wèi)星滯后并不相同，兩類傳感器之間的相對(duì)滯后為時(shí)間不同步誤差，且完整的時(shí)間延時(shí)補(bǔ)償應(yīng)包含SINS與BDS之間導(dǎo)航計(jì)算機(jī)采集的時(shí)間差以及定位更新的耗時(shí)時(shí)間。

圖2 SINS/BDS時(shí)間不同步模型

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

基于SINS/BDS的列車組合模型中，Kalman濾波算法是一個(gè)被廣泛使用的信息融合算法。然而在實(shí)際應(yīng)用系統(tǒng)中，由于列車運(yùn)行環(huán)境的復(fù)雜多變會(huì)導(dǎo)致觀測(cè)數(shù)據(jù)中存在異常噪聲，影響組合導(dǎo)航的定位精度，降低了列車運(yùn)行的可靠性。因此，提出一種基于雙因子抗差估計(jì)的SINS/BDS組合算法模型，其原理見(jiàn)圖3。由IMU輸出的角速度和加速度信息經(jīng)過(guò)慣性導(dǎo)航系統(tǒng)解算出速度和位置信息，然后與BDS輸出的速度位置信息進(jìn)行融合，經(jīng)過(guò)雙因子抗差濾波估計(jì)，最終輸出高精度的組合定位信息，實(shí)現(xiàn)列車可靠穩(wěn)定運(yùn)行。

圖3 基于雙因子抗差估計(jì)的SINS/BDS的列車組合系統(tǒng)框原理

2.2 系統(tǒng)狀態(tài)模型

根據(jù)式(1)和式(2)，在SINS誤差模型的基礎(chǔ)上，考慮桿臂誤差和時(shí)間不同步誤差，系統(tǒng)19維狀態(tài)向量為

( 3 )

2.3 系統(tǒng)量測(cè)模型

在慣導(dǎo)誤差分析的基礎(chǔ)上，考慮SINS與BDS之間的桿臂誤差和時(shí)間不同步誤差，則基于速度和位置觀測(cè)的SINS/BDS系統(tǒng)量測(cè)模型如下

(4)

2.3.1 桿臂誤差模型

SINS定位解算基準(zhǔn)為IMU的幾何中心，BDS以接收機(jī)天線的相位中心為參考基準(zhǔn)，在列車組合導(dǎo)航系統(tǒng)中，SINS與BDS安裝位置之間的偏差通常會(huì)引入一定程度的速度和位置誤差，桿臂誤差示意見(jiàn)圖4、圖5。

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

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

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

r=R+δl

( 5 )

對(duì)式(5)求導(dǎo)，得相對(duì)于地理坐標(biāo)系e下得關(guān)系

( 6 )

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

( 7 )

將式(7)投影至導(dǎo)航坐標(biāo)系，則

( 8 )

相應(yīng)地，位置誤差可表示為

( 9 )

2.3.2 時(shí)延誤差模型

在基于BDS/SINS的列車車載自主定位系統(tǒng)中，組合導(dǎo)航計(jì)算時(shí)刻與傳感器信息采集之間通常包含一定的時(shí)間滯后，見(jiàn)圖6。

圖6 BDS與SINS時(shí)間不同步誤差

SINS速度與BDS速度之間的關(guān)系可表示為

(10)

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

(11)

相應(yīng)地，位置不同步誤差可表示為

(12)

綜合桿臂誤差和時(shí)延誤差模型，式(4)中量測(cè)轉(zhuǎn)移矩陣和量測(cè)噪聲矩陣為

(13)

(14)

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

3 自適應(yīng)雙因子抗差估計(jì)

由于有軌列車跨域長(zhǎng)、運(yùn)行環(huán)境復(fù)雜多變，BDS受到多徑干擾、隨機(jī)噪聲，SINS受到列車機(jī)動(dòng)干擾等多種噪聲影響導(dǎo)致系統(tǒng)量測(cè)中包含各種異常野值，從而影響組合導(dǎo)航的系統(tǒng)精度。針對(duì)傳統(tǒng)Kalman系統(tǒng)對(duì)噪聲不敏感的特點(diǎn)，引入自適應(yīng)雙因子抗差濾波算法，對(duì)組合系統(tǒng)的動(dòng)力學(xué)模型和異常野值引起的噪聲模型不適應(yīng)問(wèn)題，利用動(dòng)力學(xué)模型信息自適應(yīng)因子和觀測(cè)模型自適應(yīng)因子建立系統(tǒng)局部模型認(rèn)可度，調(diào)節(jié)模型誤差和觀測(cè)誤差對(duì)狀態(tài)估計(jì)的影響。

3.1 自適應(yīng)抗差算法

針對(duì)有軌列車非線性特點(diǎn)，對(duì)式(1)進(jìn)行離散化，系統(tǒng)非線性模型為[4]

(15)

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

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

根據(jù)最小二乘原則構(gòu)造損失函數(shù)Jk

(16)

(17)

(18)

則式(18)可變換為

(19)

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

(20)

3.2 自適應(yīng)因子求解

(1) ?k觀測(cè)自適應(yīng)因子觀測(cè)自適應(yīng)選取

(21)

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

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

(22)

相應(yīng)地

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

(2)βk動(dòng)力學(xué)模型信息的自適應(yīng)因子選取

(23)

式中：dk表示預(yù)測(cè)狀態(tài)殘差,可表示為

(24)

相應(yīng)的

4 仿真及實(shí)驗(yàn)

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

4.1 仿真

為了驗(yàn)證本文所提出的時(shí)延計(jì)算模型和雙因子抗差估計(jì)算法的有效性進(jìn)行相關(guān)仿真，傳感器具體參數(shù)如表1所示。

表1 傳感器仿真參數(shù)

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

圖7 理想軌跡示意圖

圖8 理想速度示意圖

(1) 延時(shí)估計(jì)模型仿真比較

為驗(yàn)證組合系統(tǒng)模型的可行性，將傳統(tǒng)SINS/BDS松組合模型和延時(shí)估計(jì)模型進(jìn)行比較，圖9、圖10為仿真的速度誤差和位置誤差。

圖9 速度誤差比較

圖10 位置誤差比較

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

圖11 延時(shí)估計(jì)結(jié)果

圖12 桿臂估計(jì)結(jié)果

(2) 抗差估計(jì)仿真驗(yàn)證

為驗(yàn)證本文所提出的雙因子抗差濾波算法對(duì)定位系統(tǒng)的抗野值效果，在BDS定位系統(tǒng)測(cè)量得到的位置和速度信息中增加了一定的噪聲，量測(cè)噪聲根據(jù)如下方式產(chǎn)生

(25)

(26)

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

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

圖13 BDS仿真位置數(shù)據(jù)

圖14 BDS仿真速度數(shù)據(jù)

圖15 位置誤差

圖16 速度誤差

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

為了更加直觀的對(duì)比量測(cè)異常情況下算法的定位精度。表2給出了定位結(jié)果的均值、標(biāo)準(zhǔn)差和均方根誤差統(tǒng)計(jì)結(jié)果。

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

表2 Mean和RMSE統(tǒng)計(jì)結(jié)果

4.2 車載實(shí)驗(yàn)

為了驗(yàn)證基于雙因子抗差估計(jì)的SINS/BDS有軌列車組合導(dǎo)航算法研究在實(shí)際工程中的有效性及可靠性，利用有軌電車車載試驗(yàn)進(jìn)行驗(yàn)證。圖17為測(cè)試平臺(tái)設(shè)計(jì)原理。

圖17 車載定位系統(tǒng)測(cè)試平臺(tái)設(shè)計(jì)原理

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

圖18 車載實(shí)驗(yàn)軌跡結(jié)果

圖19 速度誤差比較

圖20 位置誤差比較

表3 速度、位置誤差統(tǒng)計(jì)表

圖21 車載實(shí)驗(yàn)桿臂估計(jì)結(jié)果

圖22 車載實(shí)驗(yàn)延時(shí)估計(jì)結(jié)果

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

5 結(jié)論

本文提出了一種基于雙因子抗差估計(jì)的SINS/BDS有軌列車組合導(dǎo)航算法，對(duì)SINS/BDS之間的位置安裝引起的桿臂誤差和子系統(tǒng)數(shù)據(jù)采集引起的時(shí)間滯后誤差進(jìn)行估計(jì)和補(bǔ)償。同時(shí)，為了修正中高速列車高動(dòng)態(tài)復(fù)雜環(huán)境中異常野值對(duì)定位精度的影響，引入雙因子抗差估計(jì)算法，通過(guò)自適應(yīng)因子降低量測(cè)噪聲對(duì)系統(tǒng)動(dòng)力學(xué)模型和噪聲模型的影響。通過(guò)仿真和車載實(shí)驗(yàn)證明，本文提出的模型和算法能更好適應(yīng)有軌列車的實(shí)際情況，對(duì)異常野值具有更好的抑制能力，相對(duì)于傳統(tǒng)組合導(dǎo)航系統(tǒng)，定位精度更高。