基于GNSS與IMU的現(xiàn)代有軌電車組合定位方法研究

石建強 ，王迪 ，陳光武 ，劉射德 ，徐琛

(1. 蘭州交通大學(xué) 自動控制研究所，甘肅 蘭州 730070；2. 甘肅省高原交通信息工程及控制重點實驗室，甘肅 蘭州 730070)

現(xiàn)代有軌電車作為一種新型的城市交通工具，其定位方式與列車大致相同，主要依靠里程計、應(yīng)答器、交叉環(huán)線、全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System, GNSS)等技術(shù)[1]。為滿足現(xiàn)代有軌電車運行控制系統(tǒng)，需要在適當(dāng)?shù)木群统浞滞暾麠l件下持續(xù)地更新電車位置，這給電車定位系統(tǒng)帶來了更高的要求[2?3]。基于 GNSS的現(xiàn)代有軌電車定位系統(tǒng)能夠自主定位獲取位置信息，減少傳統(tǒng)軌旁設(shè)備，提高運輸效率以及減少建設(shè)成 本[4]。羅淼等[5]提出一種基于BP神經(jīng)網(wǎng)絡(luò)的GPS/ RFID現(xiàn)代有軌電車定位方法，通過引入BP神經(jīng)網(wǎng)絡(luò)實現(xiàn)在弱信號環(huán)境下的連續(xù)精確定位，但由于 RFID定位精度隨距離增大而誤差增大，限制了組合定位的精度；米根鎖等[6]提出一種混沌微粒群優(yōu)化的BP 神經(jīng)網(wǎng)絡(luò)算法應(yīng)用于有軌電車定位中，但其復(fù)雜的優(yōu)化算法難以滿足有軌電車定位實時性的要求；王迪等[7]通過卡爾曼濾波和中值濾波實現(xiàn)BDS/GPS雙模定位數(shù)據(jù)的處理，并采用了分段定位方法，但其需要依賴一定的軌旁設(shè)備且實現(xiàn)過程復(fù)雜；孫永梅等[8]提出一種基于GPS/DR的定位方法，通過航位推算解決GPS信號缺失問題，但未能從根本上解決 GPS定位精度差的問題。IMU(Inertial Measurement Unit)慣性測量單元具有高自主性，不易受外界條件及人為因素干擾的特點，但存在誤差積累，必須利用高精度定位信息定期校正[7]。綜合以上分析，提出一種基于模糊投票機制的GNSS與IMU組合定位方法，該方法能夠克服單一傳感器定位不足的缺點，同時具有較高的可靠性與可用性，能夠完全滿足現(xiàn)代有軌電車的定位需求。

1 GNSS與IMU組合定位系統(tǒng)

基于模糊投票機制的GNSS與IMU組合定位系統(tǒng)由3部分組成：模糊投票機制決策單元、故障診斷隔離單元、數(shù)據(jù)融合處理單元。為提高系統(tǒng)的可靠性與準(zhǔn)確性，采用3個IMU慣性測量單元分別安裝在車的前中后3個位置，通過模糊投票機制進行數(shù)據(jù)決策。GNSS衛(wèi)星定位傳感器分別安裝在車的前后2個位置。如圖1所示，由于衛(wèi)星定位易受環(huán)境和人為因素的影響，因此本系統(tǒng)通過故障診斷隔離單元來提高系統(tǒng)的自主完好性檢測。最后為了使得整個系統(tǒng)具有很好的容錯能力，采用自適應(yīng)聯(lián)邦卡爾曼濾波算法對多傳感器數(shù)據(jù)進行融合處理。

圖1 現(xiàn)代有軌電車組合定位系統(tǒng)平臺結(jié)構(gòu)Fig. 1 Platform structure of combined positioning system for modern tramcar

系統(tǒng)中模糊投票機制決策單元主要通過模糊投票機制完成多個 IMU的數(shù)據(jù)融合，降低隨機誤差；故障診斷隔離單元主要完成對GNSS故障的探測和剔除，提高系統(tǒng)的可靠性及故障檢測能力；數(shù)據(jù)融合處理單元主要采用聯(lián)邦自適應(yīng)卡爾曼濾波完成對GNSS和IMU的數(shù)據(jù)融合，降低IMU存在的漂移誤差和積累誤差。其結(jié)構(gòu)示意圖如圖2所示。

圖2 GNSS與IMU組合定位系統(tǒng)結(jié)構(gòu)示意圖Fig. 2 Schematic diagram of GNSS and IMU integrated positioning system

2 定位算法與可靠性分析

下面分別從模糊投票機制決策單元、故障診斷隔離單元、數(shù)據(jù)融合處理單元這3個部分來分析定位傳感器位置解算方法和可靠性分析。

2.1 模糊投票機制決策算法

投票機制分為精確投票機制和非精確投票機制[9]?？紤]到現(xiàn)代有軌電車在行駛過程中的震動、干擾各個因素以及 IMU傳感器輸入量之間存在差異，因此，無法使用精確投票機制來完成融合多傳感器的數(shù)據(jù)；與之相反，非精確投票機制則允許以上情況的發(fā)生，但是對于輸入量之間的偏差需要滿足一致性閾值門限。非精確投票機制的閾值分為固定閾值與動態(tài)閾值，動態(tài)閾值允許用戶設(shè)定一定的范圍，在實現(xiàn)上，通過權(quán)重機制實現(xiàn)動態(tài)閾值的選取[10]。

模糊投票機制是非精確投票機制中的一種重要方法。模糊投票機制不僅充分利用動態(tài)閾值，同時，還考慮到輸入量的不確定性[6]。本系統(tǒng)中采用3個 IMU進行模糊投票決策，其系統(tǒng)構(gòu)架如圖 3所示。

圖3 3個IMU輸入模糊投票機制Fig. 3 Three IMU input fuzzy voting mechanism

其實現(xiàn)過程如下：

1) 確定投票閾值，考慮到3個IMU的安裝位置以及車頭和車尾的IMU傳感器存在較大的漂移，因此閾值采用加權(quán)計算：

式中：K為投票閾值；d12為輸入1和輸入2之間的距離；d23為輸入2和輸入3之間的距離；d13為輸入1和輸入3之間的距離；一共有3個距離分別為：d12，d23和 d13。(根據(jù)現(xiàn)代有軌電車車長確定)然后確定3個距離等級，等級匹配模型如圖4所示，同時定義每個距離的模糊元函數(shù)μ：

圖4 距離等級劃分模型Fig. 4 Distance classification model

2) 根據(jù)模糊準(zhǔn)則，判斷每個輸入量的置信度，置信度集合為{Vhigh, High, Medium, Low, Vlow}。

3) 量化每個輸入量的置信度，根據(jù)鏡像準(zhǔn)則，將置信度集合權(quán)重分別量化為：

根據(jù)式(4)確定每個輸入量的權(quán)重wi。

④ 計算模糊投票系統(tǒng)的輸出結(jié)果y：

最后，根據(jù)每個 IMU的加速度權(quán)重對其輸出的位置信息加權(quán)處理，獲得輸出坐標(biāo)信息。

2.2 GNSS故障診斷隔離

IMU故障主要通過增加硬件冗余度來進行識別和隔離，而GNSS故障可以通過算法進行探測和剔除[11]。下面將針對GNSS建立其觀測方程并闡述故障的探測和剔除。

假設(shè)GNSS接收機時鐘誤差模型為：接收機鐘差δtu，接收機鐘漂δtru，其微分方程模型為[12]：

β為相關(guān)時間；ωu和ωru分別為接收機時鐘差白噪聲和時鐘漂白噪聲。

由式(6)可知卡爾曼濾波矩陣為：

式中：x為導(dǎo)航參數(shù)誤差項；GNSS接收機鐘差和鐘漂項；F為狀態(tài)轉(zhuǎn)移矩陣；G為噪聲驅(qū)動矩陣；w為系統(tǒng)高斯白噪聲[13]。

GNSS的偽距觀測方程為[14]：

因而，可得到n顆衛(wèi)星的導(dǎo)航觀測方程為：

其中：觀測矩陣為 H=[D 0n×30n×30n×30n×3?1n×10n×1]；

觀測噪聲為

則，式(7)和式(9)對應(yīng)的離散卡爾曼濾波方程為：

其中：xk和xk?1分別為k和k?1時刻的狀態(tài)向量；為離散后的狀態(tài)轉(zhuǎn)移矩陣；1k-G 為k?1時刻噪聲驅(qū)動矩陣；wk?1為方差是Qk?1的系統(tǒng)白噪聲序列；zk是觀測向量；Hk為觀測矩陣；vk為方差是Rk的觀測噪聲。

卡爾曼濾波方程式(10)的等價最小二乘形式為[14]：

式中：為觀測向量；為系數(shù)矩陣；為殘差；E為單位矩陣；為觀測噪聲；為系統(tǒng)噪聲。殘差的方差?協(xié)方差矩陣可由觀測噪聲矩陣和卡爾曼濾波預(yù)測噪聲矩陣組成：

式中：為狀態(tài)預(yù)測的協(xié)方差矩陣。根據(jù)最小二乘法求得狀態(tài)參數(shù)的最優(yōu)估計及協(xié)方差矩陣為：

對應(yīng)的估計殘差和協(xié)方差矩陣為：

首先利用全局檢驗法來判斷故障是否發(fā)生，構(gòu)建方差因子[14]

其中：n為觀測值維數(shù)；u為未知數(shù)個數(shù)；當(dāng)GNSS無故障時滿足以下關(guān)系：

一旦方差因子大于給定的檢驗閾值，則判斷存在故障，反之認(rèn)為無故障。為了評估能夠探測故障大小的能力，定義了最小可探測偏差：

式中：為非中心化參數(shù)，由顯著水平α和檢驗功效β計算得到。MDB值越小，說明GNSS系統(tǒng)能夠成功探測的故障越小，系統(tǒng)的抗差性越好，反之很大的MDB值說明只有更大的故障才能夠被探測到。本系統(tǒng)中選取α=0.1%，β=20%此時，δ0=4.13。

通過全局檢驗法確定故障發(fā)生后，進一步利用ω檢驗量對故障進行定位[15]：

式中：h=[0 … 1 … 0]T為第i元素是1的單位向量。當(dāng)無故障時，服從標(biāo)準(zhǔn)正態(tài)分布。

故障的可區(qū)分性表示故障發(fā)生在兩觀測值之間可能被區(qū)分的能力，是由它們之間的相關(guān)系數(shù)確定的[16]：

相關(guān)系數(shù)表征了兩者之間的互相影響能力，其值越大，則出現(xiàn)在一個觀測值上的故障對另一個觀測值對應(yīng)的統(tǒng)計量的影響就越大，因此誤判的概率就越大。當(dāng)觀測值冗余為1時，任意2個觀測值之間的相關(guān)系數(shù)都為 1，此時，粗差智能探測不能被區(qū)分，為了能夠保證粗差被分離，多余容量至少為2。

2.3 數(shù)據(jù)融合處理算法設(shè)計

多傳感器的信息融合中運用 Kalman濾波有 2種途徑：標(biāo)準(zhǔn)集中式和分散式，目前大多采用標(biāo)準(zhǔn)集中式，但在實際應(yīng)用中存在以下缺點：系統(tǒng)狀態(tài)維數(shù)高，計算量大，嚴(yán)重影響濾波器的動態(tài)性能和實時性、集中處理導(dǎo)致濾波器的容錯性較差，濾波精度和穩(wěn)定度下降[17]。而分散式濾波技術(shù)正是為了解決集中式的缺陷而提出的，它將單獨的 Kalman濾波器用一個主濾波器和一組局部濾波器來代替，其數(shù)據(jù)處理過程分為兩部分。第1部分，每個傳感器的信息送入局部濾波器處理，產(chǎn)生局部最優(yōu)狀態(tài)估計；第2部分，主濾波器處理來自各個局部濾波器的最優(yōu)狀態(tài)估計，并產(chǎn)生全局狀態(tài)最優(yōu)估計。

聯(lián)邦濾波器作為一種特殊的分散式Kalman濾波器。本文采用自適應(yīng)聯(lián)邦Kalman濾波器來實現(xiàn)IMU和GNSS數(shù)據(jù)最優(yōu)融合和可容錯處理?，F(xiàn)代有軌電車采用GNSS與IMU組合定位方案，在應(yīng)用中一方面需要用GNSS定位數(shù)據(jù)輔助IMU的初始化和誤差積累校正；另一方面，當(dāng)GNSS失鎖或定位誤差較大時，需要依靠 IMU完成自主定位或平滑定位功能。采用自適應(yīng)聯(lián)邦卡爾曼濾波利用信息分配原理實現(xiàn)多傳感器的最優(yōu)綜合，使系統(tǒng)具有一定的容錯能力?；谧赃m應(yīng)聯(lián)邦卡爾曼濾波的結(jié)構(gòu)框圖如圖5所示。

圖5 GNSS與IMU組合定位數(shù)據(jù)融合結(jié)構(gòu)框圖Fig. 5 Block diagram of data fusion between GNSS and IMU

現(xiàn)代有軌電車的運動可認(rèn)為二維平面運動，Kalman濾波的整體狀態(tài)變量可取為

式中：xe和xn分別為電車東向和北向的位置分量；ve和vn分別為東向和北向的速度分量；ae和an分別為東向和北向的加速度分量；εω為角速率陀螺的漂移誤差；qω為角速率陀螺標(biāo)度因子的相對誤差；經(jīng)過離散化后，可得局部濾波器離散狀態(tài)方程為

其中：φ為狀態(tài)轉(zhuǎn)移矩陣；U(k)為控制向量；W(k)為噪聲矩陣[17]。

將GNSS接收機輸出的現(xiàn)代有軌電車的東向和北向位置坐標(biāo)分量er和nr作為外部觀測量，則與狀態(tài)變量之間的關(guān)系(測量方程)為

其中：1()X k為狀態(tài)變量；1()kH 為GNSS傳感器測量矩陣；1()V k為GNSS傳感器的測量噪聲。同理，IMU的測量方程為[18]

主濾波器對2個局部濾波器的估計信息進行融合，融合后得到全局最優(yōu)估計值，融合算法如下：

測量更新方程：

最優(yōu)融合方程：

信息分配方程：

其中：信息分配系數(shù)1()kβ和2()kβ滿足信息分配原理：

3 算法驗證與分析

本文主要使用 InvenSense公司的 6自由度MPU6050慣性測量單元來獲取現(xiàn)代有軌電車的動力學(xué)信息。采用 UBLOX公司的 VK1612U8M3LF衛(wèi)星定位接收模塊來獲取現(xiàn)代有軌電車的位置信息。數(shù)據(jù)融合單元采用ATMEL公司的ARM7微處理器AT91SAM7X256。由于受現(xiàn)場測試限制，因此選擇在校園模擬測試。

為比較傳統(tǒng)的Kalman算法與本文的投票決策機制對 IMU的影響，在某段路線進行測試，圖 6為安裝在車上IMU的加速度輸出曲線與經(jīng)過2種方法處理的結(jié)果；圖7為2種算法的加速度誤差比較。從圖中可以明顯看出采用本文提出的投票決策機制算法減少了IMU的隨機誤差和漂移誤差。

圖6 IMU輸出加速度曲線Fig. 6 IMU output acceleration curve

圖7 傳統(tǒng)Kalman算法與投票決策機制誤差比較Fig. 7 Comparison between traditional Kalman algorithm and voting decision mechanism

為分析GNSS故障診斷隔離單元的性能，通過軟件仿真，采樣間隔30 s，采樣時間12 h，選取某衛(wèi)星為故障星，在偽距中加入故障偏差，其差值從0 m增加到200 m，并與單獨的GPS進行對比。仿真結(jié)果如圖8和9所示。當(dāng)偏差達到120 m時GNSS系統(tǒng)能夠100%檢測到故障；而單獨的GPS只能到達96%；通過圖9可知其故障識別率與GPS相比有了明顯的提高。

為驗證系統(tǒng)最終數(shù)據(jù)融合輸出結(jié)果，采用傳統(tǒng)的基于Kalman濾波的GNSS/IMU組合與本文的方案進行比較。如圖10所示。

圖8 GNSS與GPS故障檢測率比較Fig. 8 Comparison of GNSS and GPS fault detection rate

圖9 GNSS與GPS故障識別率比較Fig. 9 Comparison of GNSS and GPS fault recognition rate

圖10 2種方法定位誤差比較Fig. 10 Comparison of two methods of positioning error

根據(jù)圖10數(shù)據(jù)計算其統(tǒng)計方差：

式中：1()D x為普通GPS/IMU組合定位誤差數(shù)據(jù)的方差；2()D x 為本方案組合定位數(shù)據(jù)誤差的方差；如圖10所示普通GPS/IMU組合定位精度為8 m左右而本文提出的方法其定位精度為4 m左右。由式子(28)可知其定位精度大約提高了40.2%。

4 結(jié)論

1) 本文針對基于GNSS與IMU的現(xiàn)代有軌電車組合定位系統(tǒng)，利用GNSS和IMU傳感器的不同安裝方法獲取現(xiàn)代有軌電車的定位信息，提出了一種基于模糊投票機制決策的融合算法，減小隨機誤差。

2) 分析了GNSS系統(tǒng)的故障診斷與隔離方法，給出了故障判別條件；提出了采用自適應(yīng)聯(lián)邦卡爾曼濾波算法實現(xiàn)GNSS與IMU數(shù)據(jù)最優(yōu)融合和可容錯處理。

3) 測試表明，該方法能夠初步實現(xiàn)現(xiàn)代有軌電車的定位功能，在以后的研究中希望能夠進行現(xiàn)場測試，提出更加有效、可靠的方案。

[1] Barbu G. Requirements of rail applications[EB/OL].GNSS Rail User Forum. http://etcs.uic.asso.fr/docs/specifications/etcs_lc_satellite.pdf

[2] Mazl R, Preucil L. Sensor data fusion for inertial navigation of trains in GPS-dark areas[C]// Intelligent Vehicles Symposium, 2003. Proceedings. IEEE, 2003:345?350.

[3] 劉江, 蔡伯根, 唐濤, 等. 基于 GPS 與慣性測量單元的列車組合定位系統(tǒng)[J]. 中國鐵道科學(xué), 2010, 31(1):123?129.LIU Jiang, CAI Baigen, TANG Tao, et al. Tain integrated positioning system based on GPS and intertial measurement unit[J]. China Railway Science, 2010, 31(1):123?129.

[4] 樊玉明, 蔡伯根, 王劍, 等. 基于北斗?陀螺儀組合定位的軌道占用判別方法研究[J]. 鐵道學(xué)報, 2013, 35(9):46?51.FAN Yuming, CAI Baigen, WANG Jian, et al. Research on the method of track occupancy discrimination based on the combination of Beidou and gyroscope[J]. Journal of the China Railway Society, 2013, 35(9): 46?51.

[5] 羅淼, 米根鎖. BP神經(jīng)網(wǎng)絡(luò)在城市有軌電車 GPS/RFID 組合定位中的應(yīng)用研究[J]. 鐵道標(biāo)準(zhǔn)設(shè)計, 2014,58(12): 125?130.LUO Miao, MI Gensuo. Research on the application of BP neural network in the GPS/RFID positioning of urban tram [J]. Railway Standard Design, 2014, 58(12):125?130.

[6] 米根鎖, 羅淼, 牛彥霞. 混沌微粒群優(yōu)化 BP神經(jīng)網(wǎng)絡(luò)算法在城市有軌電車定位中的應(yīng)用[J]. 鐵道學(xué)報,2015, 37(6): 67?72.MI Gensuo, LUO Miao, NIU Yanxia. Application network algorithm in the positioning of urban tram[J].Journal of the China Railway Society, 2015, 37(6):67?72.

[7] 王迪, 陳光武, 楊廳. BDS/GPS雙模定位技術(shù)在現(xiàn)代有軌電車中的應(yīng)用研究[J]. 鐵道科學(xué)與工程學(xué)報, 2016,13(11): 2270?2275.WANG Di, CHEN Guangwu, YANG Ting. Application research on BDS/GPS dual-mode positioning technology for modern streetcar[J]. Journal of Railway Science and Engineering, 2016, 13(11): 2270?2275.

[8] 孫永梅, 王富章. 基于 GPS/DR 的現(xiàn)代有軌電車定位方法研究[J]. 鐵道標(biāo)準(zhǔn)設(shè)計, 2016, 60(9): 137?140.SUN Yongmei, WANG Fuzhang. Research on the positioning method of modern tram based on GPS/DR[J].Railway Standard Design, 2016, 60(9): 137?140.

[9] Rawat R M, Gupta T K, Sajid M, et al. An improved fuzzy voting scheme for fault tolerant systems[J].International Journal of Applied Evolutionary Computation (IJAEC), 2015, 6(2): 41?49.

[10] Hajdu A, Hajdu L, Jonas A, et al. Generalizing the majority voting scheme to spatially constrained voting[J].IEEE Transactions on Image Processing, 2013, 22(11):4182?4194.

[11] 吳有龍, 王曉鳴, 楊玲, 等. GNSS/INS 緊組合導(dǎo)航系統(tǒng)自主完好性監(jiān)測分析[J]. 測繪學(xué)報, 2014, 43(8):786?795.WU Youlong, WANG Xiaoming, YANG Ling, et al.Analysis of autonomous integrity monitoring for GNSS/INS integrated navigation system[J]. Acta Geodaetica et Cartographica Sinica, 2014, 43(8): 786?795.

[12] Georgy J, Noureldin A, Goodall C. Vehicle navigator using a mixture particle filter for inertial sensors/odometer/map data/GPS integration[J]. IEEE Transactions on Consumer Electronics, 2012, 58(2): 544?552.

[13] Soon B K H, Scheding S, Lee H K, et al. An approach to aid INS using time-differenced GPS carrier phase (TDCP)measurements[J]. GPS Solutions, 2008, 12(4): 261?271.

[14] WU Y, WANG J, JIANG Y. Advanced receiver autonomous integrity monitoring (ARAIM) schemes with GNSS time offsets[J]. Advances in Space Research, 2013,52(1): 52?61.

[15] YANG L, WANG J, Knight N L, et al. Outlier separability analysis with a multiple alternative hypotheses test[J]. Journal of Geodesy, 2013, 87(6): 591?604.

[16] YANG L, Knight N L, LI Y, et al. Optimal fault detection and exclusion applied in GNSS positioning[J]. The Journal of Navigation, 2013, 66(5): 683.

[17] 王曉軍, 劉佳, 李輝, 等. 聯(lián)邦卡爾曼濾波技術(shù)在列車組合定位中的應(yīng)用研究[J]. 鐵道通信信號, 2015(4):17?20.WANG Xiaojun, LIU Jia, LI Hui, et al. Research on the application of federated Calman filtering technique in the train positioning[J]. Railway Signalling & Communication, 2015(4): 17?20.

[18] 高為廣, 楊元喜, 崔先強, 等. IMU/GPS 組合導(dǎo)航系統(tǒng)自適應(yīng) Kalman 濾波算法[J]. 武漢大學(xué)學(xué)報(信息科學(xué)版), 2006, 31(5): 466?469.GAO Weiguang, YANG Yuanxi, CUI Xianqiang, et al.Adaptive Kalman filtering algorithm for IMU/GPS integrated navigation system[J]. Geomatics and Information Science of Wuhan University, 2006, 31(5):466?469.