跨距與運(yùn)動(dòng)約束的隧道內(nèi)列車慣性導(dǎo)航方法

趙啟超，吳文啟，張禮廉，唐康華

(國(guó)防科技大學(xué)智能科學(xué)學(xué)院，長(zhǎng)沙 410073)

0 引言

在高速列車行駛中，列車的導(dǎo)航定位在列車調(diào)度、安全運(yùn)行與鐵路檢測(cè)維護(hù)等方面起著非常重要的作用。在列車導(dǎo)航中，高速度、隧道環(huán)境以及高可靠性需求等方面都面臨著很大的挑戰(zhàn)，但同時(shí)行駛路線的固定性和方向穩(wěn)定性也能為導(dǎo)航定位提供輔助。本文主要解決的問題是列車在隧道內(nèi)的定位，鐵路隧道的特點(diǎn)是結(jié)構(gòu)比較固定，但缺失衛(wèi)星信號(hào)并且距離可能較長(zhǎng)，光照較弱而且缺少紋理。車載導(dǎo)航常用的傳感器有全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)、慣性測(cè)量單元(Inertial Measurement Unit，IMU)、相機(jī)、激光雷達(dá)、輪式里程計(jì)等，在列車導(dǎo)航中，也使用應(yīng)答器、多普勒雷達(dá)、渦流傳感器等定位方式[1]。GNSS可以為列車提供高精度的位置和速度信息，但是在隧道等被遮擋環(huán)境中，GNSS將失去作用。慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)使用IMU作為傳感器，不受外部環(huán)境干擾，可以提供連續(xù)的導(dǎo)航信息，但是導(dǎo)航精度受器件誤差特性影響，而且存在累計(jì)誤差。

關(guān)于列車在隧道內(nèi)導(dǎo)航的相關(guān)研究，劉射德等設(shè)計(jì)了一種GNSS/INS/地圖匹配的列車組合定位系統(tǒng)[2]，但是沒有評(píng)估GNSS缺失時(shí)的定位精度，無(wú)法確定列車在隧道中的地圖匹配能否抑制INS的誤差累計(jì)。Kim等設(shè)計(jì)了一種多傳感器融合的列車導(dǎo)航系統(tǒng)，包括IMU、輪式測(cè)速儀、多普勒雷達(dá)、差分GNSS、無(wú)線射頻模塊，以及運(yùn)動(dòng)約束和地圖匹配算法，即使在隧道中也具有較高的定位精度和容錯(cuò)性，但傳感器占用的空間、多傳感器標(biāo)定以及射頻芯片在鐵軌上的安裝等都是實(shí)際存在的問題。不限于列車導(dǎo)航，GNSS拒止環(huán)境下的導(dǎo)航也有很多研究，例如李增科等提出了對(duì)GNSS、INS、里程計(jì)進(jìn)行神經(jīng)網(wǎng)絡(luò)訓(xùn)練，以生成里程計(jì)的速度校正，在GNSS中斷時(shí)可以提高位置精度[3]。萬(wàn)國(guó)偉等設(shè)計(jì)的無(wú)人車導(dǎo)航系統(tǒng)包括MEMS IMU、激光雷達(dá)、GNSS，此系統(tǒng)在公路隧道中利用IMU和激光雷達(dá)組合導(dǎo)航，可以獲得較好的定位效果[4]，但是激光雷達(dá)導(dǎo)航需要預(yù)先建圖，具有一定的局限性。Tolga等針對(duì)管道內(nèi)的檢測(cè)無(wú)人機(jī)設(shè)計(jì)了一種范圍定位算法，基于管道的圓柱形信息，使用相機(jī)、激光雷達(dá)和IMU對(duì)其內(nèi)部環(huán)境建圖并對(duì)自身定位[5]。

接觸網(wǎng)是懸掛于鐵軌與列車上方為列車提供電能的裝置，在列車運(yùn)行期間容易與列車受電弓之間產(chǎn)生電火花。本文的應(yīng)用場(chǎng)景是確定列車運(yùn)行時(shí)產(chǎn)生的電火花的位置，這個(gè)位置與列車位置有對(duì)應(yīng)關(guān)系。從成本和安裝空間考慮，本文使用GNSS、MEMS IMU和相機(jī)作為傳感器，其中相機(jī)與檢測(cè)電火花的相機(jī)是同一臺(tái)。接觸網(wǎng)的支柱上固定了路標(biāo)，理想情況下，通過識(shí)別路標(biāo)上的編號(hào)可以從數(shù)據(jù)庫(kù)中確定路標(biāo)的位置，進(jìn)而對(duì)列車定位；但是由于列車速度較快以及光照不均勻等原因，路標(biāo)編號(hào)大多難以識(shí)別，因此本文忽略路標(biāo)編號(hào)，直接對(duì)路標(biāo)進(jìn)行不加區(qū)分的檢測(cè)，然后使用路標(biāo)的間距和高度信息輔助慣性導(dǎo)航。路標(biāo)的間距近似等于接觸網(wǎng)支柱的跨距，跨距是保持受電弓和接觸網(wǎng)良好接觸的重要參數(shù)，它的取值有著嚴(yán)格規(guī)定[6]，在一段距離內(nèi)是保持不變的。

本文針對(duì)高速列車通過隧道的場(chǎng)景，提出了一種跨距與運(yùn)動(dòng)約束的慣性導(dǎo)航方法，通過對(duì)路標(biāo)的檢測(cè)獲取跨距信息，并結(jié)合列車運(yùn)動(dòng)約束，輔助慣性導(dǎo)航得到更精確的定位結(jié)果。具體如下：

1)跨距約束與運(yùn)動(dòng)約束結(jié)合，可對(duì)列車形成全方向的約束，提高定位精度；

2)與基于地標(biāo)點(diǎn)的位置修正方法相比，此方法不需要識(shí)別路標(biāo)編號(hào)，計(jì)算效率更高，并且對(duì)圖像模糊不敏感，適用于高速運(yùn)動(dòng)；

3)使用改進(jìn)的卡爾曼濾波方法，濾波時(shí)間更新方程與測(cè)量修正方程采用相同的計(jì)算頻率，可減小實(shí)時(shí)計(jì)算負(fù)擔(dān)，有利于高速運(yùn)動(dòng)下的組合導(dǎo)航。

1 列車運(yùn)動(dòng)約束模型

1.1 運(yùn)動(dòng)約束

首先定義本文使用的坐標(biāo)系，n系代表導(dǎo)航坐標(biāo)系，坐標(biāo)軸方向?yàn)楸睎|地(NED)；m系代表車體坐標(biāo)系，坐標(biāo)軸方向?yàn)榍坝蚁?；b系代表IMU坐標(biāo)系，安裝時(shí)使其坐標(biāo)軸方向盡量與m系一致；c系代表相機(jī)坐標(biāo)系，x軸和y軸與相機(jī)平面平行，z軸指向相機(jī)前方。

陸地車輛在行駛時(shí)，如果不發(fā)生側(cè)滑和跳動(dòng)，沿車體系側(cè)向(y軸)和法向(z軸)的速度為0[7]，這稱為車輛運(yùn)動(dòng)約束。運(yùn)動(dòng)約束是提高慣導(dǎo)精度的一種常用方法，但由于缺少對(duì)前向速度的約束，是一種非完整約束，因此常與里程計(jì)組合[8-9]。忽略IMU安裝桿臂的影響，這種約束可以表示為

(1)

(2)

(3)

(4)

(5)

其中，[?×]代表向量的反對(duì)稱矩陣，δθm為安裝角誤差，由橫滾、俯仰和方位安裝角誤差構(gòu)成，φn為失準(zhǔn)角，δvn為速度誤差，可以推導(dǎo)出側(cè)向與法向速度誤差為

(6)

1.2 基于ST-EKF的誤差狀態(tài)模型

(7)

這種方法稱為狀態(tài)變換卡爾曼濾波(State Transformation Extended Kalman Filter，ST-EKF)，它可以消去系統(tǒng)矩陣中的比力項(xiàng)，避免比力量化噪聲的影響，使系統(tǒng)方程更接近線性定常系統(tǒng)，從而降低濾波時(shí)間更新方程的計(jì)算頻率，提高計(jì)算效率，避免誤差的累計(jì)[11-12]。

設(shè)導(dǎo)航系下基于ST-EKF的誤差狀態(tài)方程為

(8)

式中，δX為誤差狀態(tài)向量，F(xiàn)為系統(tǒng)矩陣，G為噪聲轉(zhuǎn)移矩陣，w為過程噪聲向量，它們定義如下

(9)

(10)

(11)

(12)

1.3 運(yùn)動(dòng)約束觀測(cè)模型

將式(7)代入式(6)，得ST-EKF下的側(cè)向和法向速度誤差為

(13)

(14)

(15)

2 跨距約束模型

2.1 路標(biāo)檢測(cè)

圖1所示為隧道內(nèi)的路標(biāo)示意，路標(biāo)形狀為矩形，并可以反射火車頂部的燈光，因此可以從形狀和像素強(qiáng)度2個(gè)方面來檢測(cè)路標(biāo)，檢測(cè)的效率和魯棒性要優(yōu)于對(duì)路標(biāo)編號(hào)的識(shí)別。設(shè)計(jì)的算法主要包括二值化、區(qū)域標(biāo)記和模板匹配等流程，實(shí)驗(yàn)表明該算法具有較高的計(jì)算效率和魯棒性。

圖1 原始圖像(左)與局部放大圖像(右)中的路標(biāo)Fig.1 Landmarks in original image (left) and partially enlarged image (right)

根據(jù)路標(biāo)檢測(cè)結(jié)果，可以計(jì)算相機(jī)相對(duì)路標(biāo)的距離。設(shè)路標(biāo)在圖像中的坐標(biāo)為zl，在相機(jī)坐標(biāo)系中的位置表示為

(16)

根據(jù)相機(jī)投影方程為

(17)

(18)

(19)

問題來了，這也是你一生中猝不及防的一跤，你受不了他人的哄笑，但你真的想要同情嗎？前者是善意的：你本不該如此，你應(yīng)該有更好的未來，只因?yàn)橛薮琅c莽撞，做了可笑的事；后者是更大的善意：你已經(jīng)盡力了，你不能走得更遠(yuǎn)，此地就是你的極限。你失敗了，因?yàn)槟悴皇浅晒Φ牧?，我們?cè)從憬邮苣恪阏嬗X得，后面的對(duì)待，是你想要的？

需要注意的是，一個(gè)路標(biāo)會(huì)被連續(xù)觀測(cè)數(shù)幀，可以利用這幾幀的觀測(cè)對(duì)路標(biāo)的位置進(jìn)行優(yōu)化[13]；并且如果hl未知，也可以在優(yōu)化中對(duì)其進(jìn)行估計(jì)，之后選取一幀用于距離約束即可，此處不再贅述。

2.2 跨距約束觀測(cè)模型

設(shè)列車在2個(gè)路標(biāo)間行進(jìn)時(shí)，INS解算N次，解算周期為T，跨距約束可表示為

(20)

(21)

δZsc=δs=HscδX+υs

(22)

(23)

2.3 濾波流程

根據(jù)1.2節(jié)中的描述，ST-EKF可通過使用較低的時(shí)間更新方程計(jì)算頻率，提高計(jì)算效率，有利于高速運(yùn)動(dòng)下的組合導(dǎo)航。實(shí)際使用中，時(shí)間更新方程與測(cè)量修正方程采用相同的計(jì)算頻率。

當(dāng)列車在隧道外時(shí)，GNSS速度和位置以及運(yùn)動(dòng)約束為慣性導(dǎo)航提供觀測(cè)，使用卡爾曼濾波可以估算安裝角。設(shè)GNSS速度和位置的誤差觀測(cè)量為δZg，觀測(cè)方程為

(24)

其中，υgv和υgp是GNSS速度和位置的觀測(cè)白噪聲，Hg是觀測(cè)矩陣，這里直接寫出它的具體形式為[10]

(25)

GNSS與運(yùn)動(dòng)約束同步進(jìn)行測(cè)量更新，誤差觀測(cè)量與觀測(cè)矩陣分別為

(26)

進(jìn)入隧道后，運(yùn)動(dòng)約束周期保持不變繼續(xù)進(jìn)行，誤差觀測(cè)量和觀測(cè)矩陣分別為

δZin/mc=δZmc,Hin/mc=Hmc

(27)

在路標(biāo)可見時(shí)進(jìn)行跨距約束，誤差觀測(cè)量和觀測(cè)矩陣分別為

δZin/sc=δZsc,Hin/sc=Hsc

(28)

運(yùn)動(dòng)約束與跨距約束在時(shí)間上不同步，可以使用序貫濾波的方法對(duì)它們進(jìn)行組合。

3 仿真實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

由于隧道內(nèi)無(wú)GNSS信號(hào)作為位置基準(zhǔn)，為了檢驗(yàn)算法的有效性和導(dǎo)航精度，設(shè)計(jì)了一組基于真實(shí)實(shí)驗(yàn)數(shù)據(jù)的仿真實(shí)驗(yàn)。其中IMU和GNSS數(shù)據(jù)來自隧道外高速列車行駛實(shí)驗(yàn)，圖像數(shù)據(jù)來自隧道內(nèi)列車行駛實(shí)驗(yàn)，列車行駛速度約為350km/h，軌跡如圖2所示，設(shè)置仿真場(chǎng)景如下。

圖2 高速列車行駛軌跡Fig.2 High-speed train trajectory

首先使用RTS(Rauch-Tung-Striebel)平滑算法計(jì)算IMU與GNSS的組合導(dǎo)航結(jié)果，作為仿真實(shí)驗(yàn)的定位真值，模擬120s的隧道內(nèi)行駛，這段時(shí)間內(nèi)列車行駛距離約為11490m，也即模擬隧道長(zhǎng)11490m。路標(biāo)跨距為50m，設(shè)置第1個(gè)路標(biāo)距離入隧道口10m，然后根據(jù)路標(biāo)跨距設(shè)置路標(biāo)，最終隧道內(nèi)共有231個(gè)路標(biāo)。之后，根據(jù)相機(jī)與IMU的時(shí)間同步，在每個(gè)路標(biāo)的位置插入4幀包含路標(biāo)的連續(xù)圖像，其他位置插入不包含路標(biāo)的圖像。

由于列車高速行駛，傳感器時(shí)間不對(duì)準(zhǔn)將會(huì)對(duì)導(dǎo)航造成較大影響，因此IMU、GNSS與相機(jī)采用硬件同步。其中GNSS輸出秒脈沖信號(hào)，經(jīng)同步控制器分頻后產(chǎn)生所需頻率的觸發(fā)脈沖信號(hào)，觸發(fā)沿按周期對(duì)齊。

3.2 實(shí)驗(yàn)結(jié)果

GNSS和運(yùn)動(dòng)約束估計(jì)的IMU安裝角如圖3所示。在列車靜止階段，角度不發(fā)生變化；在起步加速階段，安裝角開始變化并逐漸收斂；勻速階段基本保持不變。另外方位安裝角比俯仰安裝角的收斂速度慢，這與列車缺少轉(zhuǎn)向機(jī)動(dòng)，慣性導(dǎo)航方位角收斂較慢有關(guān)[15]。

圖3 IMU安裝角Fig.3 IMU installation angle

820～940s斷開GNSS，對(duì)比純慣導(dǎo)(Pure INS)、運(yùn)動(dòng)約束的慣導(dǎo)(Motion Constrained INS, MC-INS)以及跨距和運(yùn)動(dòng)約束的慣導(dǎo)(Catenary Span and Motion Constrainted INS, CSMC-INS)，圖4和圖5所示為三種算法在無(wú)GNSS時(shí)的北向和東向位置誤差，表1列出了最大位置誤差的值。

圖4 北向位置誤差Fig.4 Northward position error

圖5 東向位置誤差Fig.5 Eastward position error

表1 不同算法的最大位置誤差

Tab.1 Maximum position error of different algorithms

導(dǎo)航算法Pure INSMC-INSCSMC-INS北向最大位置誤差/m187.624.324.5東向最大位置誤差/m313.334.12.4

從圖4、圖5和表1中可以看出，運(yùn)動(dòng)約束明顯減小了北向和東向的位置誤差。由于列車近似東西向行駛，北向位置誤差的減小是易于理解的，而東向位置的減小是由于運(yùn)動(dòng)約束使前向速度的誤差增長(zhǎng)更慢[7]。在運(yùn)動(dòng)約束的基礎(chǔ)上添加跨距約束后，北向位置誤差基本不變，而東向位置誤差進(jìn)一步減小，定位精度得到提高。

為了檢驗(yàn)CSMC-INS對(duì)位置誤差的校正，畫出位置誤差的發(fā)散情況，如圖6所示，這里為了使效果更加明顯，設(shè)置了每2個(gè)路標(biāo)有1個(gè)路標(biāo)不被識(shí)別，也就是每100m提供一次觀測(cè)。純慣導(dǎo)的位置誤差在5s內(nèi)發(fā)散14m，平均發(fā)散2.8m/s。使用CSMC-INS后，當(dāng)列車在2個(gè)路標(biāo)之間時(shí)，誤差發(fā)散0.48m，平均發(fā)散0.48m/s，這說明經(jīng)過約束后的導(dǎo)航參數(shù)估計(jì)值更為準(zhǔn)確。在經(jīng)過測(cè)量更新后發(fā)散誤差減小至0.07m，說明CSMC-INS可以有效抑制誤差發(fā)散，但同時(shí)仍然會(huì)存在誤差累計(jì)。

圖6 水平位置誤差發(fā)散情況Fig.6 Horizontal position error divergence

根據(jù)以上實(shí)驗(yàn)結(jié)果的分析，可以確定本文提出的CSMC-INS算法在GNSS缺失時(shí)，可以有效抑制INS誤差的發(fā)散，提高列車在隧道內(nèi)的定位精度。

4 結(jié)論

本文針對(duì)高速列車通過隧道的場(chǎng)景，基于對(duì)隧道內(nèi)路標(biāo)的識(shí)別，提出了一種跨距和運(yùn)動(dòng)約束的慣性導(dǎo)航方法。該算法分析與實(shí)驗(yàn)結(jié)果表明,跨距和運(yùn)動(dòng)約束可以抑制慣性導(dǎo)航誤差的發(fā)散，顯著提高了列車在隧道內(nèi)的導(dǎo)航定位精度，在通過11.5km的隧道后，水平定位誤差約為25m，定位精度優(yōu)于3‰行程，具有實(shí)際應(yīng)用價(jià)值。

附錄

(29)