基于EKF的軌道探傷小車組合定位技術(shù)研究

劉長軍，王黛月，余天樂，郭建志，李錦

基于EKF的軌道探傷小車組合定位技術(shù)研究

劉長軍1，王黛月1，余天樂2，郭建志2，李錦1

(1.華東理工大學(xué) 機(jī)械與動力工程學(xué)院，上海 200237；2. 上海市東方海事工程技術(shù)有限公司，上海 200011)

軌道探傷小車是新一代鐵路軌道探傷設(shè)備，現(xiàn)已具備傷損點(diǎn)自動識別功能，但其定位依賴單一里程計，仍需人工標(biāo)定來完成傷損點(diǎn)的實(shí)時地理位置定位，無法滿足傷損定位的精度要求和實(shí)現(xiàn)全面自動傷損掃查檢測功能的需求。針對鐵路線上傷損掃查過程中的自動定位問題，使用擴(kuò)展卡爾曼濾波(EKF)實(shí)現(xiàn)了里程計與全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)的多傳感器數(shù)據(jù)融合，并基于新息自適應(yīng)估計(IAE)對融合算法作自適應(yīng)處理，建立GNSS/里程計組合定位模型，以提高探傷小車定位的精確性與可靠性。最后設(shè)計開發(fā)了硬件板卡，并通過滬杭線上測試實(shí)驗(yàn)，對組合定位模型進(jìn)行驗(yàn)證。實(shí)驗(yàn)表明基于EKF的組合定位相較于單一GNSS定位其定位精度和魯棒性明顯提高。

軌道探傷；擴(kuò)展卡爾曼濾波；數(shù)據(jù)融合；自適應(yīng)；組合定位

隨著我國鐵路交通進(jìn)入高速時代，運(yùn)營里程與載客量在不斷上升，為保證鐵路安全，鋼軌探傷密度也隨之增加，這對原有的探傷設(shè)備提出了新的要求。長期以來，我國用于鋼軌探傷的設(shè)備主要有2種，分別是手推式探傷儀與大型鋼軌探傷車，前者工作效率低且工作強(qiáng)度高，后者效率高但其價格過于昂貴無法大量使用[1]。軌道探傷小車是新一代鐵路軌道探傷設(shè)備，相較于前兩者，具有輕量化、模塊化、合理化等特點(diǎn)，目前探傷小車已能自動識別出鋼軌損傷點(diǎn)，但其對傷損點(diǎn)的定位主要依靠車載里程計，里程計定位屬于相對定位，短期內(nèi)定位精度高但不可避免的存在累積誤差，需要人為地根據(jù)地理路標(biāo)進(jìn)行航跡推算與誤差校正。而為了推進(jìn)軌道探傷車工作效率的進(jìn)一步提升，實(shí)現(xiàn)損傷全面自動化檢測功能，這就需要探傷小車具備全自動的精確定位功能。全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)是各種衛(wèi)星定位系統(tǒng)的統(tǒng)稱，其定位范圍廣、精度高，能夠獲得車輛的實(shí)時坐標(biāo)信息，可以有效解決探傷小車自動定位的問題，但是GNSS定位原理是基于衛(wèi)星發(fā)送的電磁波信號，當(dāng)小車行駛路段存在過多遮擋與干擾時，就會導(dǎo)致定位偏差過大或信號丟失等問題[2]。由此可見，單一傳統(tǒng)的定位傳感器滿足不了探傷小車所需高精度、高可靠性的定位要求。而多傳感器信息融合技術(shù)可以有效解決這一問題，將探傷小車的里程定位與GNSS定位相結(jié)合，互相取長補(bǔ)短，所構(gòu)成的組合定位系統(tǒng)，具有更高的精度與魯棒性。目前，能夠?qū)崿F(xiàn)數(shù)據(jù)融合方法有很多，主要包括加權(quán)平均法、貝葉斯估計法、卡爾曼濾波法、產(chǎn)生式規(guī)則、人工神經(jīng)網(wǎng)絡(luò)、模糊推理等[3]。其中，卡爾曼濾波法是多傳感器信息融合技術(shù)在定位研究應(yīng)用中比較常用的方法，其設(shè)計原理源于最小線性方程估計[4]，是線性動態(tài)系統(tǒng)中的最優(yōu)化估計。但實(shí)際上在現(xiàn)實(shí)中大多數(shù)問題都是由非線性和時變系統(tǒng)所組成，使用線性卡爾曼濾波難以進(jìn)行分析估計[5]。而人們在處理一些非線性系統(tǒng)的問題中仍期望使用卡爾曼線性濾波估計的形式，進(jìn)而發(fā)展出了擴(kuò)展卡爾曼濾波(EKF)，具有更強(qiáng)的廣泛適應(yīng)性和穩(wěn)定性[6]。然而，與標(biāo)準(zhǔn)卡爾曼濾波器一樣，EKF在使用前需要已知目標(biāo)系統(tǒng)的噪聲統(tǒng)計特性，但對于更多系統(tǒng)的來說其外部環(huán)境是未知且多變的，這就容易引起濾波發(fā)散，從而容易引起濾波發(fā)散[7]。為了防止濾波發(fā)散，需要先判斷出系統(tǒng)當(dāng)前狀況，然后使用不同方法通過自適應(yīng)的方式來抑制發(fā)散，新息可以用作判斷和調(diào)節(jié)濾波器的依據(jù)[8]?？柭鼮V波器的自適應(yīng)處理方法主要有兩種，一是基于多模型自適應(yīng)估計(MMAE)建立多模型卡爾曼濾波器[9]，二是基于新息自適應(yīng)估計(IAE)來修正過程參數(shù)[10]。而后者還與模糊控制相結(jié)合，進(jìn)一步形成了基于模糊控制的卡爾曼濾波自適應(yīng)技術(shù)[11]。除此之外，國內(nèi)外的自適應(yīng)方法還存在有Saga-Husa濾波法[12]，抗差濾波法[13]，漸消記憶因子濾波法[14]等。針對GNSS定位存在的信號不穩(wěn)定以及里程計定位的累積誤差等問題，使用EKF來實(shí)現(xiàn)GNSS與里程計數(shù)據(jù)融合的定位方法，通過該融合算法對定位坐標(biāo)進(jìn)行實(shí)時的校準(zhǔn)與更新，并基于IAE對融合算法中的誤差參量作自適應(yīng)處理，建立了組合定位模型，提高了在GNSS觀測量發(fā)生突變時該卡爾曼濾波器的精度和魯棒性。本文針對GNSS定位存在的不穩(wěn)定以及里程計定位的累積誤差等問題，使用擴(kuò)展卡爾曼濾波(EKF)來實(shí)現(xiàn)GNSS與里程計融合的定位方法，通過該融合算法對定位坐標(biāo)進(jìn)行實(shí)時的校準(zhǔn)與更新，從而實(shí)現(xiàn)對GNSS失信狀態(tài)與里程計累積誤差的周期性補(bǔ)償和校正，并基于IAE對融合算法中的誤差參量作自適應(yīng)處理，建立組合定位模型，提高了在GNSS觀測量發(fā)生突變時該卡爾曼濾波器的精度和魯棒性。并設(shè)計開發(fā)基于樹莓派3b的硬件板卡，實(shí)現(xiàn)探傷小車精確定位的自動輸出，最終在上海市松江區(qū)滬杭線上進(jìn)行測試，對本文所提出的方法和理論進(jìn)行驗(yàn)證。

1 基于EKF的自適應(yīng)算法

1.1 EKF算法

在面對非線性問題時，EKF的基本方法是對其估計模型進(jìn)行一階泰勒展開，在短區(qū)間內(nèi)將非線性模型線性化，然后再進(jìn)行濾波估計[15]。

對于非線性系統(tǒng)模型，其狀態(tài)方程與測量方程如下：

式中：為當(dāng)前時刻；為系統(tǒng)狀態(tài)描述矩陣；為由上一時刻到下一時刻的狀態(tài)轉(zhuǎn)移方程；為測量轉(zhuǎn)移方程；w與v分別代表系統(tǒng)過程噪聲與測量噪聲，兩者互不相關(guān)，他們的協(xié)方差矩陣分別為跟。有了系統(tǒng)狀態(tài)變化過程的公式，擴(kuò)展卡爾曼濾波算法具體流程如下：

假設(shè)系統(tǒng)存在一個維的狀態(tài)變量:

則其轉(zhuǎn)置矩陣為：

EKF基于微分思想將非線性問題化為多段線性問題并加以解決處理，極大的提升了卡爾曼濾波的適用性與其解決實(shí)際問題的能力。

1.2 自適應(yīng)算法

雖然EKF可以解決非線性問題，然而基本過程還是基于標(biāo)準(zhǔn)卡爾曼濾波，其濾波方程中的與需要提前確定，常用的方法是利用相關(guān)噪聲在系統(tǒng)環(huán)境中的統(tǒng)計特性來代替。但在實(shí)際過程中，系統(tǒng)環(huán)境可能是復(fù)雜且多變的，這就很難保證統(tǒng)計特性的準(zhǔn)確性。因此，將濾波方程中的噪聲協(xié)方差矩陣跟不再設(shè)為預(yù)先統(tǒng)計而得的固定值，而是作為變量隨系統(tǒng)時間與環(huán)境的變化而變化，達(dá)到其自適應(yīng)性的目的。對于噪聲更迭的自適應(yīng)方式有很多，其中最常見的是利用系統(tǒng)新息來自適應(yīng)的迭代更新噪聲[16]。

新息的定義為：

新息的方差為：

取時刻前個觀測新息可計算出觀測新息協(xié)方差的估計值：

1.3 融合算法推導(dǎo)

將上述1.2中的式(11)、式(14)～(16)代入到1.1中式(3)～(8)的EKF算法的流程中去，即可得到經(jīng)自適應(yīng)處理后的EKF算法，最終融合算法如下：

2 GNSS/里程計組合定位系統(tǒng)模型

2.1 坐標(biāo)轉(zhuǎn)換

利用GNSS進(jìn)行定位時，探傷小車上的接收機(jī)能直接獲得的是WGS-84下的經(jīng)緯度數(shù)據(jù)，而使用里程計作航跡推算時一般是采用高斯平面直角坐標(biāo)系。因此，不能直接將兩者定位信息進(jìn)行融合處理，需要統(tǒng)一坐標(biāo)系。本文使用高斯?克呂格投影法將接收機(jī)獲取的經(jīng)緯度坐標(biāo)轉(zhuǎn)至平面坐標(biāo)。

把接收機(jī)獲取的經(jīng)緯度坐標(biāo)(,,)按如下公式轉(zhuǎn)換成平面坐標(biāo)：

2.2 組合定位系統(tǒng)預(yù)測模型

設(shè)探傷小車的位姿向量為:

位姿向量的預(yù)測方程為：

由此，與系統(tǒng)預(yù)測過程所對應(yīng)的估計誤差協(xié)方差矩陣更新方程為：

2.3 組合定位系統(tǒng)測量模型

設(shè)在時刻，經(jīng)GNSS直接測量轉(zhuǎn)換所得的觀測量Z與位姿向量的關(guān)系為：

觀測方程關(guān)于探傷小車位姿向量經(jīng)雅比克變換后的轉(zhuǎn)置矩陣為：

至此，將系統(tǒng)模型與觀測模型代入基于EKF的自適應(yīng)算法中進(jìn)行測試分析。

3 探傷小車松江線上測試結(jié)果與分析

為驗(yàn)證融合定位算法的效果，設(shè)計開發(fā)了組合定位系統(tǒng)，圖1為該系統(tǒng)的總體框架圖，主要包括工控機(jī)(即探傷小車控制系統(tǒng))、中央處理模塊、定位模塊、通訊模塊以及遠(yuǎn)程監(jiān)控平臺，其中工控機(jī)采用的是上海東方海事工程技術(shù)有限公司研發(fā)的雙軌式鋼軌超聲波探傷儀，中央處理模塊為搭載Raspbian系統(tǒng)的樹莓派3b板卡，GNSS模塊為QUELTEL公司旗下的GNSS L76，通訊模塊為華為ME909S-821，遠(yuǎn)程監(jiān)控平臺為基于C#在QT平臺下開發(fā)的界面操作軟件，軟件中搭載有自適應(yīng)融合算法，能分別輸出單一GNSS定位數(shù)據(jù)與組合定位數(shù)據(jù)。

圖1 組合定位系統(tǒng)

本文采用一組位于上海市松江區(qū)滬杭線鐵路工段進(jìn)行的試驗(yàn)數(shù)據(jù)對本文提出的自適應(yīng)融合算法進(jìn)行測試，測試路段全長約為6.4 km。組合定位系統(tǒng)的里程精度與定位測量精度分別為：里程1.5%，定位小于2.5 m CEP，但同時在實(shí)際測試過程中發(fā)現(xiàn)，GNSS易受戶外環(huán)境因素干擾，定位精度不穩(wěn)定。行駛平均速度為8 m/s，探傷小車行駛時間為800 s。選取其中勻速行駛的兩段數(shù)據(jù)，其中段一存在GNSS無信號區(qū)域，全長約300 m，包含無信號區(qū)200m。段二全程GNSS信號良好，全長約800 m，其GNSS數(shù)據(jù)點(diǎn)=200，采樣周期為= 0.5 s。利用GNSS獲得的探傷小車位姿作為觀測量引入卡爾曼濾波器，得到誤差的最優(yōu)估計。圖2為段一即存在信號丟失段的定位軌跡比較圖，圖3為段二即信號良好段所選取的GNSS定位軌跡點(diǎn)。

首先對段一測試效果進(jìn)行評價，如圖2所示，當(dāng)探傷小車途徑無信號區(qū)段時，單一GNSS將失去定位功能，定位軌跡中出現(xiàn)缺失段；而組合定位依靠里程計的航跡推算仍能持續(xù)定位，相較于單一GNSS定位，更能有效的保障探傷小車連續(xù)定位，提高了定位系統(tǒng)的魯棒性。

圖2 信號丟失段定位軌跡

圖3 地圖上探傷小車GNSS定位點(diǎn)

在段二中，本測試首先在圖示地圖上對該鐵路段真實(shí)坐標(biāo)進(jìn)行圖上采點(diǎn)，再經(jīng)坐標(biāo)轉(zhuǎn)換后進(jìn)行擬合，得到該路段探傷小車行駛的真實(shí)軌跡，然后以真實(shí)軌跡作為基準(zhǔn)，分別計算出單一GNSS定位與組合定位的偏離程度，并進(jìn)行對比分析。圖4為GNSS定位與組合定位相較于真實(shí)軌跡的偏離誤差比較。

圖4 單獨(dú)GNSS定位與組合定位的誤差對比

由圖4可以看出，相較于單一GNSS定位，組合定位在GNSS測量發(fā)生較大變化時可以有效減小定位誤差，從GNSS定位誤差最大的7.479 m減小到組合定位誤差的3.010 m，以及從GNSS定位誤差均值的2.396 m減小到1.498 m。單一GNSS定位與組合定位誤差比較如表1所示。

表1 定位誤差統(tǒng)計

3 結(jié)論

1) 針對目前軌道探傷小車對于全自動精準(zhǔn)定位功能的需求，基于多傳感器信息融合技術(shù)將小車的里程計與衛(wèi)星定位系統(tǒng)相結(jié)合，構(gòu)建多傳感器組合定位系統(tǒng)，實(shí)現(xiàn)了兩者的優(yōu)勢互補(bǔ)，保證了小車自動定位的連續(xù)性和可靠性。

2) 采用EKF對小車組合定位系統(tǒng)的融合算法進(jìn)行設(shè)計，并基于IAE對融合算法中的誤差參量作自適應(yīng)處理，建立基于GNSS/里程計的組合定位模型，能有效的提高定位精度與系統(tǒng)的魯棒性。

3) 設(shè)計開發(fā)組合定位軟硬件系統(tǒng)并使用該系統(tǒng)對融合算法進(jìn)行數(shù)值模擬和實(shí)驗(yàn)驗(yàn)證。結(jié)果表明使用該組合定位系統(tǒng)能彌補(bǔ)單一GNSS定位精度不足的問題，減小定位誤差并提升穩(wěn)定性，即使在GNSS信號受到遮擋時，也可以利用里程計航位推算的定位方式進(jìn)行輔助定位。該算法為鐵路自動探傷提供了可靠的定位方法，為實(shí)現(xiàn)全面自動探傷掃測定位提供了可靠的現(xiàn)場應(yīng)用方案。

[1] 韓小平. 基于高速探傷技術(shù)的雙軌自行式鋼軌探傷小車研制[J]. 中國鐵路, 2014(3): 77?81. HAN Xiaoping. Development of double rail self- propelled rail inspection car based on high speed inspection technology[J]. China Railway, 2014(3): 77?81.

[2] 蔡煊, 王長林. 基于抗差估計的BDS/ODO組合列車定位方法[J].鐵道科學(xué)與工程學(xué)報, 2018, 15(10): 2654?2660. CAI Xuan, WANG Changling. BDS/ODO integrated train positioning method based on robust estimation[J]. Journal of Railway Science and Engineering, 2018, 15(10): 2654?2660.

[3] 胡勝豪. 室外移動機(jī)器人的定位與運(yùn)動控制研究[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué), 2016: 5?6. HU Shenghao. Research on localization and motion control for outdoor mobile robot[D]. Harbin: Harbin Institute of Technology, 2016: 5?6.

[4] ZHAO Yingwei. Performance evaluation of Cubature Kalman filter in a GPS/IMU tightly-coupled navigation system[J]. Signal Processing, 2016, 119: 67?79.

[5] WANG Shiyuan, WANG Wanli, CHEN Badong, et al. Convergence analysis of nonlinear Kalman filters with novel innovation-based method[J]. Neurocomputing, 2018, 289: 188?194.

[6] 劉夢佳, 孫自強(qiáng), 卿湘運(yùn). 一種基于簡化MRAS無速度傳感器的永磁電機(jī)EKF磁鏈辨識[J]. 華東理工大學(xué)學(xué)報(自然科學(xué)版), 2015, 41(2): 222?230. LIU Mengjia, SUN Ziqiang, Qing Xiangyun. Flux identification of SPMSM based on simplified MRAS senseless control system[J]. Journal of East China University of Science and Technology (Natural Science Edition), 2015, 41(2): 222?230.

[7] 李旦, 秦永元, 梅春波. 組合導(dǎo)航自適應(yīng)卡爾曼濾波改進(jìn)算法研究[J]. 測控技術(shù), 2011, 30(3): 114?116. LI Dan, QIN Yongyuan, MEI Chunbo. An improved adaptive Kalman filter algorithm for SINS/GPS integrated navigation system[J]. Measurement & Control Technology, 2011, 30(3): 114?116.

[8] 卞鴻巍, 李安, 覃方君, 等. 現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M]. 北京: 國防工業(yè)出版社, 2010: 115?117. BIAN Hongwei, LI An, QIN Fangjun, et al. Application of modern information fusion technology in integrated navigation[M]. Beijing: National Defense Industry Press, 2010: 115?117.

[9] Alsuwaidan B N, Crassidis J L, CHENG Y. Generalized multiple-model adaptive estimation using an autocorrelation approach[J]. IEEE Transactions on Aerospace and Electronic Systems, 2011, 47(3): 2138? 2152.

[10] DING Weidong, WANG Jinling, RIZOS C. Improving adaptive Kalman estimation in GPS/INS integration[J]. Journal of Navigation, 2007, 60(3): 517?529.

[11] JIN Mei, ZHAO Jinge, JIN Ju, et al. The adaptive Kalman filter based on fuzzy logic for inertial motion capture system[J]. Measurement, 2014, 49: 196?204.

[12] WANG Qinghui, HUANG Wangyuan, WEI Lifeng, et al. A wireless localization algorithm based on strong tracking Kalman filter[J]. Sensors & Transducers, 2014, 183(12): 155?161.

[13] Chingiz H, Halil E S. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites[J]. International Journal of Adaptive Control and Signal Processing, 2014, 28(2): 107?120.

[14] Hajiyev C, Soken H E. Adaptive Kalman filter with multiple fading factors for UAV state estimation[J]. IFAC Proceedings Volumes, 2009, 42(8): 77?82.

[15] 谷豐, 周楹君, 何玉慶, 等. 非線性卡爾曼濾波方法的實(shí)驗(yàn)比較[J]. 控制與決策, 2014, 29(8): 1387?1393. GU Feng, ZHOU Yingjun, HE Yuqing, et al. Experimental investigation and comparison of nonlinear Kalman filters[J]. Control and Decision, 2014, 29(8): 1387?1393.

[16] 符拯, 王書滿, 劉丙杰. 自適應(yīng)卡爾曼濾波的最新進(jìn)展[J]. 戰(zhàn)術(shù)導(dǎo)彈技術(shù), 2009(6): 62?66. FU Zheng, WANG Shuman, LIU Bingjie. An overview of the development of adaptive Kalman filtering[J]. Tactical Missile Technology, 2009(6): 62?66.

[17] 馬瑞平, 魏東, 張明廉. 一種改進(jìn)的自適應(yīng)卡爾曼濾波及在組合導(dǎo)航中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2006, 14(6): 37?40. MA Ruiping, WEI Dong, ZHANG Minglian. Improved adaptive Kalman filter for GPS/INS integrated system [J]. Journal of Chinese Inertial Technology, 2006, 14(6): 37?40.

[18] 卞鴻巍, 金志華, 王俊璞, 等. 組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法[J]. 上海交通大學(xué)學(xué)報, 2006, 40(6): 1000?1004. BIAN Hongwei, JIN Zhihua, WANG Junpu, et al. The innovation based estimation adaptive Kalman filter algorithm for INS/GPS integrated navigation system[J]. Journal of Shanghai Jiaotong University, 2006, 40(6): 1000?1004.

[19] 吳顯. 基于多傳感器信息融合的移動機(jī)器人定位方法研究[D]. 北京: 北京交通大學(xué), 2016: 17?19. WU Xian. Research on localization method of mobile robots based on multi-sensor data fusion[D]. Beijing: Beijing Jiaotong University, 2016: 17?19.

Research on combined location method of dual rail inspection vehicle based on extended Kalman filter

LIU Changjun1, WANG Daiyue1, YU Tianle2, GUO Jianzhi2, LI Jing1

(1. School of Mechanical and Power Engineering, East China University of Science and Technology, Shanghai 200237, China;2. Shanghai Oriental Maritime Affairs Engineering Technology Co., Ltd., Shanghai 200011, China)

Dual rail inspection vehicle is a new generation of railway inspection equipment, which has the function of automatic inspection of rail defect points. Conventionally, the localization of the inspection vehicle uses an odometer and the real-time localization of rail defect points still needs manual calibration. Thus, it cannot achieve the localization accuracy as well as the requirements of vehicle automatic operation. The automatic localization methods were discussed in this paper. The Extended Kalman Filter (EKF) was employed for the multi-sensor data fusion of the odometer and the Global Navigation Satellite System (GNSS). Then the fusion algorithm was processed using the Innovation Adaptive Estimation(IAE). The GNSS/odometer combined localization model has been built to improve the accuracy and reliability of the inspection vehicle localization. The hardware board was designed. Finally the on-line test has been performed on the Shanghai-Hangzhou railway line. The experimental results show that our combined localization method can significantly improve the accuracy and robustness compared with that of the single GNSS method.

rail inspection; extended Kalman filter; data fusion; adaptive; combined localization

TP29

A

1672 ? 7029(2020)10 ? 2649 ? 07

10.19713/j.cnki.43?1423/u.T20191176

2019?12?27

國家重點(diǎn)研發(fā)計劃項(xiàng)目(2017YFC0805704)

李錦(1983?)，女，四川德陽人，副教授，博士，從事特種機(jī)器人研究工作；E?mail：lijinme@ecust.edu.cn

(編輯 陽麗霞)