基于運(yùn)動約束抑制慣導(dǎo)誤差的高速列車隧道內(nèi)定位方法

孫振乾, 唐康華, 吳美平, 郭妍, 王雪瑩

(國防科技大學(xué) 智能科學(xué)學(xué)院, 湖南 長沙 410073)

隨著中國高速鐵路網(wǎng)的布局更加完善，在網(wǎng)運(yùn)行的列車數(shù)量越來越多，發(fā)車頻次越來越高，這就需要給運(yùn)行控制系統(tǒng)提供更加精準(zhǔn)的位置、速度及姿態(tài)信息,以實現(xiàn)列車的控制、調(diào)度與監(jiān)測[1]。列車運(yùn)行過程中會出現(xiàn)GNSS信號遮擋、城市及山間峽谷可觀測的衛(wèi)星少,特別是在長隧道內(nèi)衛(wèi)星信號長時間缺失，對定位系統(tǒng)是個很大挑戰(zhàn)[3]。

傳統(tǒng)列車定位系統(tǒng)采用測速測距方式，定位精度差，提供的列車信息有限，系統(tǒng)維護(hù)復(fù)雜[4]，設(shè)備成本高，且難以解決長隧道內(nèi)的定位問題，已無法滿足高速列車定位需求。隨著MEMS慣性器件的精度越來越高，可靠性和成本優(yōu)勢讓其在車載領(lǐng)域應(yīng)用廣泛[5]。GNSS星基增強(qiáng)技術(shù)將衛(wèi)星定位精度提高到亞米級[6]。INS和GNSS良好的互補(bǔ)性，使其成為高鐵定位的核心，但在長隧道內(nèi),GNSS信號缺失,僅依賴MIMU定位精度難以保證[7-9]。

近年來，針對隧道內(nèi)無GNSS信號時列車定位研究成為熱點(diǎn)。目前主要有2種方式：①在系統(tǒng)中加入額外傳感器，主要有激光雷達(dá)、多普勒測速儀、氣壓高度計、里程計、相機(jī)、無線定位、地圖匹配、RFID等[4]，與INS組合導(dǎo)航實現(xiàn)列車定位。但是，加多余傳感器會提高系統(tǒng)的成本和實現(xiàn)難度，增加維修保養(yǎng)負(fù)擔(dān)。②利用軌道或列車運(yùn)動信息約束抑制慣導(dǎo)誤差發(fā)散保證列車在隧道內(nèi)的定位精度。王鵬飛等提出了基于列車運(yùn)動模型和軌道約束的方法[10]，在GNSS缺失40 s時水平定位誤差達(dá)21.2 m。梁飛采用列車運(yùn)動模型和陀螺儀組合的方法對衛(wèi)星盲區(qū)的列車定位[11]，衛(wèi)星信號缺失10 s后，水平誤差高達(dá)31 m。楊菊花等人采用列車運(yùn)動約束來抑制慣導(dǎo)的誤差[12]，明顯提高了列車導(dǎo)航精度。但在運(yùn)動約束模型中沒有考慮安裝角影響，50 s內(nèi)水平定位誤差為12.6 m，水平速度誤差約為0.71 m/s，精度有待進(jìn)一步提升。在毫米級精度要求的軌道檢測領(lǐng)域，陳起金等人設(shè)計了基于高精度激光INS/GNSS組合導(dǎo)航的軌檢小車[1]，使用運(yùn)動約束提高系統(tǒng)在GNSS信號受干擾時的精度。由于沒有考慮安裝角，對系統(tǒng)精度的提升有限。張全等人對上述系統(tǒng)進(jìn)行了改進(jìn)，在運(yùn)動約束模型中考慮了安裝角[13]，但是沒有提到怎么得到安裝角。朱峰等人在無GNSS信號時，通過事后數(shù)據(jù)處理利用相似旋轉(zhuǎn)的方法得到安裝角[14]，但安裝角并未被用于運(yùn)動約束。準(zhǔn)確得到MIMU的安裝角，是提高運(yùn)動約束效果的關(guān)鍵[9,15]。

高速列車在長隧道內(nèi)GNSS信號缺失且其他可用信息少，現(xiàn)有方法定位精度低，隧道內(nèi)建立精確的電子地圖和布設(shè)高精度參考點(diǎn)都很難實現(xiàn)。針對此，在不增加額外傳感器的情況下，本文提出CIAMC-INS方法。由于安裝角的存在，使得相對車體m系的車輛運(yùn)動約束假設(shè)在慣導(dǎo)系統(tǒng)的b系中不再成立。該方法先首在考慮MIMU安裝角的基礎(chǔ)上建立了運(yùn)動約束模型；其次通過分析列車轉(zhuǎn)彎對運(yùn)動約束模型的影響，得到運(yùn)動約束的使用條件；然后在GNSS信號良好且滿足運(yùn)動約束使用條件時，在線估計MIMU的安裝角。最后在GNSS缺失的隧道內(nèi)，將估計的安裝角用于運(yùn)動約束輔助INS定位。實驗結(jié)果表明，考慮安裝角的運(yùn)動約束方法對純慣導(dǎo)誤差的抑制作用更明顯，列車在隧道內(nèi)的定位精度得到明顯提高，驗證了方法的有效性。

1 車輛運(yùn)動學(xué)約束

1.1 列車運(yùn)動約束關(guān)系

定義列車車體坐標(biāo)系為m系,原點(diǎn)為列車車廂中心,x,y軸分別指向列車的右向、前向,z軸與x,y軸構(gòu)成右手直角坐標(biāo)系。定義MIMU坐標(biāo)系為b系,安裝過程中b系的x,y,z軸盡量和m系的x,y,z軸朝向保持一致,保證安裝角為小角度。

(1)

根據(jù)文獻(xiàn)[17]中的結(jié)論,橫滾安裝角對運(yùn)動約束沒有影響。一旦MIMU固定在列車上,安裝角誤差可以視為小角度隨機(jī)常數(shù)。不考慮δaγ的影響,將δaθ和δaψ擴(kuò)張成三維向量形式

δa=[δaθ0δaψ]T

(2)

安裝角余弦矩陣誤差可表示為

(3)

1.2 列車轉(zhuǎn)彎對運(yùn)動約束的影響

高鐵軌道的曲線線路一般由直線、緩和曲線、圓曲線組成[16],如圖1所示。在車體轉(zhuǎn)彎的過程中,由于MIMU安裝位置與載車旋轉(zhuǎn)中心存在桿臂,在MIMU安裝處存在桿臂速度[18-19]。

圖1 軌道曲線線路平面圖

列車在前向牽引力、軌道、轉(zhuǎn)向架、車輪等機(jī)構(gòu)的配合下實現(xiàn)轉(zhuǎn)彎[20],允許車箱和轉(zhuǎn)向架之間有一定程度旋轉(zhuǎn)[9],轉(zhuǎn)向架的實物如圖2所示。

圖2 轉(zhuǎn)向架實物圖

為了簡化分析,選擇圖1中的S段,也即列車從B點(diǎn)到C點(diǎn)左轉(zhuǎn)彎,轉(zhuǎn)彎模型如圖3所示。轉(zhuǎn)向架行駛在半徑為R的列車軌道上,且行駛方向與軌道近似相切。P1和P2為前后轉(zhuǎn)向架的插銷中點(diǎn),車廂在通過兩點(diǎn)的割線上。

圖3 列車轉(zhuǎn)彎幾何分析

因此,列車轉(zhuǎn)向架的速度方向和車廂軸線之間存在偏差角β[9],由幾何關(guān)系可得

(4)

式中,d為P1和P2點(diǎn)之間的距離。

轉(zhuǎn)彎過程中,車廂繞O1點(diǎn)旋轉(zhuǎn),旋轉(zhuǎn)角速度為

(5)

式中,vD1為前轉(zhuǎn)向架插銷的速度。

側(cè)向速度為零的假設(shè)在轉(zhuǎn)向架坐標(biāo)系中的中心點(diǎn)P1處依然成立,由于車廂繞O1的旋轉(zhuǎn),在m系中此假設(shè)不在成立。設(shè)MIMU安裝在P1點(diǎn),此時,MIMU在m系中的速度為

(6)

設(shè)列車速度為350 km/h,2個轉(zhuǎn)向架中心點(diǎn)距離為20 m,轉(zhuǎn)彎半徑為5 000 m[16],由于轉(zhuǎn)彎引起的側(cè)向速度為

(7)

這將嚴(yán)重影響卡爾曼濾波器狀態(tài)估計精度,甚至導(dǎo)致濾波器發(fā)散。

將(5)式帶入(6)式可得

(8)

式中,ωz為z軸陀螺的輸出。

在實際行駛過程中,列車的機(jī)動轉(zhuǎn)彎將更加復(fù)雜,前后轉(zhuǎn)向架的轉(zhuǎn)彎角速度和車廂的旋轉(zhuǎn)中心均不固定。很難建立精確的側(cè)向速度補(bǔ)償模型。但(8)式仍然為判斷側(cè)向速度為零的假設(shè)是否成立提供了理論依據(jù)。

1.3 運(yùn)動學(xué)約束的閾值

為了減小水平轉(zhuǎn)向機(jī)動對運(yùn)動約束的不利影響,以車體天向陀螺輸出的轉(zhuǎn)彎角速率為判斷否可使用運(yùn)動約束的條件,表述為

|ωz-ωz-bias|<ωz-threshold

(9)

式中:ωz為MIMU的z軸陀螺輸出;ωz-bias為z軸陀螺的零偏;ωz-threshold為設(shè)定的判定閾值,高鐵運(yùn)行比較平穩(wěn)且轉(zhuǎn)彎少,可以根據(jù)實測數(shù)據(jù)設(shè)定閾值。實驗采集的列車運(yùn)行過程中z軸陀螺數(shù)據(jù)如圖4所示。

圖4 z軸陀螺測量數(shù)據(jù)

(10)

式中,η為閾值的調(diào)節(jié)系數(shù),可視運(yùn)動狀況及精度要求適當(dāng)選取。

2 組合導(dǎo)航模型及算法流程

本節(jié)根據(jù)對GNSS信號是否可用和列車運(yùn)行狀態(tài)的判斷,設(shè)計相應(yīng)的濾波模型,并給出算法流程。

2.1 MIMU/GNSS松組合模型

當(dāng)列車轉(zhuǎn)向且GNSS信號良好時,運(yùn)動約束不可用。列車定位采用15維MIMU/GNSS松組合EKF濾波器,依據(jù)慣導(dǎo)誤差傳播模型,忽略安裝桿臂的影響,系統(tǒng)的15維狀態(tài)誤差向量定義為

X1=

(11)

式中：φn為姿態(tài)誤差；δvn為速度誤差；δPn為位置誤差；δεb為陀螺在b系中的零漂誤差；δb為加速度計在b系中的零偏誤差。

系統(tǒng)狀態(tài)方程為

(12)

式中：FINS為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；G1為噪聲轉(zhuǎn)移矩陣；W1為過程噪聲向量。

(13)

(14)

GNSS/MIMU組合的觀測模型表示為

(15)

(16)

2.2 MIMU/GNSS/運(yùn)動約束組合模型

當(dāng)運(yùn)動約束和GNSS均可用時,可構(gòu)建濾波器在線估計MIMU相對車體的安裝角。將俯仰角安裝角誤差δaθ和航向角安裝角誤差δaψ也擴(kuò)充為誤差狀態(tài)量,17維狀態(tài)變量為

(17)

(18)

系統(tǒng)狀態(tài)方程為

(19)

式中,Fa=02×2,Ga=I2×2,Wa為安裝角誤差噪聲。

結(jié)合(3)式,(1)式的全微分為

(20)

由列車的運(yùn)動約束構(gòu)成的觀測方程為

(21)

(22)

式中,Mi(k,×)為矩陣Mi的第k(k=1,2,3)行的所有元素。

GNSS和運(yùn)動約束的聯(lián)合觀測方程為

(23)

令運(yùn)動約束的觀測頻率為1 Hz,且與GNSS觀測數(shù)據(jù)同步。

2.3 MIMU/MC組合模型

在鐵路線路設(shè)計中,隧道內(nèi)盡量避免設(shè)置曲線軌道,即便在特殊情況下設(shè)置曲線,曲線的長度會盡量小[16]。因此,在隧道中若存在短時曲線轉(zhuǎn)彎,可依靠純慣性導(dǎo)航實現(xiàn)定位。在隧道中裝誤差角不再估計,系統(tǒng)的狀態(tài)方程如(12)式所示。

將在隧道外已經(jīng)估計得到的安裝角作為已知常值帶入(20)式,可得

(24)

由運(yùn)動約束提供的觀測方程為

(25)

(22)式改寫為

(26)

2.4 定位算法的流程

根據(jù)列車運(yùn)行狀態(tài)和GNSS數(shù)據(jù)的觀測環(huán)境,設(shè)計了3種濾波算法模,如圖5所示。在GNSS信號良好且滿足運(yùn)動約束條件時,17維EKF1使用GNSS和運(yùn)動約束提供觀測,估計各狀態(tài)量,確定MIMU安裝角。在無GNSS信號且滿足運(yùn)動約束條件時,15維EKF2僅使用車輛運(yùn)動學(xué)約束提供觀測,安裝角為EKF1的估計值。在GNSS信號良好但不滿足運(yùn)動約束條件時,15維EKF3僅使用GNSS速度位置作為觀測。當(dāng)定位模式發(fā)生變化時,公共狀態(tài)的估計值及其誤差協(xié)方差陣從老算法直接傳遞給新算法,其他量不更新或者重置[21]。

圖5 定位算法流程圖

3 試驗說明

高鐵定位系統(tǒng)主要由MIMU(50 Hz)、GNSS接收機(jī)(1 Hz)、Cortex-A9+FPGA硬件平臺等部件組成。組合導(dǎo)航模塊固定在前轉(zhuǎn)向架中心附近的車廂地板上,GNSS接收機(jī)固定在組合導(dǎo)航模塊正上方的車廂頂部,PC機(jī)通過串口接收并存儲試驗數(shù)據(jù),整個系統(tǒng)組成如圖6所示。

圖6 組合導(dǎo)航及數(shù)據(jù)采集系統(tǒng)

4 試驗結(jié)果與分析

4.1 MIMU安裝角估計結(jié)果

列車運(yùn)行在衛(wèi)星信號良好且滿足使用運(yùn)動約束條件的情況下,對安裝角估計,其他情況下,安裝角保持原值不更新,安裝角估計的結(jié)果如圖7所示。

圖7 MIMU安裝角估計

在啟動初期,加速度不穩(wěn)定且速度很小,安裝角估計值波動較大,安裝角的可觀測性與速度和加速度緊密相關(guān)。在進(jìn)入穩(wěn)定加速階段,安裝角的估計有較快收斂,在勻速行駛階段安裝角變化較小。相比俯仰安裝角,航向安裝角收斂速度較慢,與列車缺少航向機(jī)動及慣導(dǎo)航向角初始化的精度低有關(guān),姿態(tài)失準(zhǔn)角與安裝角存在一定的耦合關(guān)系。

4.2 隧道出口處定位結(jié)果

列車進(jìn)入隧道GNSS信號完全丟失,出隧道后接收機(jī)需要對GNSS信號再次捕獲、跟蹤,在隧道出口附近的參考點(diǎn)處,仍無法定位。在途中的隧道段分別采用CIAMC-INS算法、傳統(tǒng)的不考慮安裝角的運(yùn)動約束抑制慣導(dǎo)(traditional motion constraints suppress INS error,TMC-INS)、P-INS 3種算法定位。以隧道入口最后一個GNSS定位點(diǎn)為參考原點(diǎn),繪制列車運(yùn)行軌跡,如圖8至9所示。紅色標(biāo)識點(diǎn)為參考點(diǎn),圖8為參考點(diǎn)11,圖9中從左到右依次為參考點(diǎn)21,22,23,24。兩隧道出口參考點(diǎn)處水平定位誤差統(tǒng)計在表1和表2中。

表1 1 412 m隧道出口參考點(diǎn)處水平定位誤差統(tǒng)計

表2 9 063 m隧道口參考點(diǎn)處水平定位誤差統(tǒng)計

圖8 1 412 m隧道出口處定位軌跡

圖9 9 063 m隧道出口處定位軌跡

在1 412 m長隧道出口11號參考點(diǎn)處,3種定位算法的誤差百分比分別為:0.94%,0.41%,0.24%。CIAMC-INS算法在隧道口參考點(diǎn)處水平定位誤差分別比P-INS算法和TMC-INS算法減小了74.1%,38.1%。

9 063 m長隧道中包含一段長約800 m的轉(zhuǎn)彎隧道。3種定位算法的平均誤差百分比分別為:3.23%,0.80%,0.47%。CIAMC-INS算法在隧道口參考點(diǎn)處水平定位誤差分別比P-INS和TMC-INS減小了84.2%,36.3%。

從統(tǒng)計結(jié)果可知,列車在隧道段運(yùn)動約束方法明顯抑制了慣性導(dǎo)航系統(tǒng)定位誤差的發(fā)散?？紤]MIMU安裝角影響的CIAMC-INS算法,可以進(jìn)一步增強(qiáng)抑制作用,提高定位精度。

4.3 隧道內(nèi)定位結(jié)果

因隧道內(nèi)無定位基準(zhǔn),為進(jìn)一步分析CIAMC-INS算法在隧道內(nèi)的有效性,選擇一段開闊地帶,以平滑后的GNSS/MIMU組合導(dǎo)航定位結(jié)果為基準(zhǔn),將GNSS數(shù)據(jù)斷開100 s,以模擬隧道內(nèi)GNSS信號缺失環(huán)境。衛(wèi)星斷開時間內(nèi)列車直線行駛9 515 m。圖10和圖11分別顯示了3種算法的東向和北向位置誤差,圖12至13分別顯示了3種算法的東向和北向速度誤差,圖14顯示了3種算法的航向角誤差。表3列出了3種算法的水平位置、水平速度、航向角的100 s誤差及均方根誤差。

圖10 東向位置誤差

圖11 北向位置誤差

圖12 東向速度誤差 圖13 北向速度誤差 圖14 航向角誤差

從表3的統(tǒng)計結(jié)果可知,CIAMC-INS算法相比P-INS和TMC-INS算法,水平位置誤差、水平速度位置誤差和航向角誤差均大幅減小。

表3 隧道口定位誤差統(tǒng)計

在衛(wèi)星信號缺失時,2種運(yùn)動約束算法對慣導(dǎo)系統(tǒng)的水平位置、水平速度及航向角的誤差發(fā)散均有一定程度的抑制作用, CIAMC-INS算法對誤差的抑制作用更明顯。由于采用的慣性器件的精度較低,列車在隧道中缺少機(jī)動影響誤差估計的效果,且沒有約束列車前向速度等原因, CIAMC-INS算法不能完全消除慣導(dǎo)系統(tǒng)誤差發(fā)散,仍然隨時間存在一定程度的誤差累積。列車車輪安裝有輪式脈沖速度傳感器,在將來的工作中可用于約束列車的前向速度,進(jìn)一步提高定位的精度。

5 結(jié) 論

本文針對高速列車在隧道環(huán)境衛(wèi)星信號缺失時定位精度差的問題,在不增加額外傳感器的情況下,提出了考慮MIMU相對車體安裝角的CIAMC-INS算法。在沈陽至朝陽段進(jìn)行了高鐵車載試驗,對比分析了采用P-INS,TMC-INS,CIAMC-INS 3種算法定位結(jié)果。試驗結(jié)果表明,在GNSS觀測良好和滿足運(yùn)動約束條件時,MIMU在列車上的安裝角可以通過擴(kuò)展卡爾曼濾波器有效估計;在無GNSS信號的隧道段,CIAMC-INS算法對慣導(dǎo)水平位置誤差、水平速度誤差及航向角的誤差抑制作用比TMC-INS算法更加明顯。在不添加額外傳感器的情況下,通過9 063 m長且中間有長約800 m彎曲段的隧道,在無GNSS情況下行駛9 163 m的水平定位誤差大概為43.3 m。人為斷開GNSS情況下,列車直線行駛9 515 m后,水平定位誤差約為38.9 m。試驗結(jié)果表明,在包含轉(zhuǎn)彎的隧道和直隧道內(nèi),定位精度均得到大幅提高,曲線隧道內(nèi)精度稍差,驗證了提出算法的有效性,具有重要的應(yīng)用價值。