李慶成 王玉林 逯宇 于奕軒 沈政華

摘要：針對使用低精度慣性測量單元（inertial measurement unit，IMU）的礦用車定位系統(tǒng)存在無法與混合動力能量管理控制策略相結(jié)合的問題，本文設(shè)計了一種融合IMU、GNSS和車速傳感器信號的定位系統(tǒng)。該系統(tǒng)的結(jié)構(gòu)基于直接配置擴(kuò)展卡爾曼濾波方法，濾波器由運動學(xué)模型和傳感器誤差模型組成。引入車輛車速傳感器信號，對車輛車速進(jìn)行修正，作為預(yù)測下一步位置的輔助信息。該體系結(jié)構(gòu)的設(shè)計方式使其易于標(biāo)定不同傳感器參數(shù)，以便應(yīng)用于不同定位需求的車輛。同時，為了測試本文所設(shè)計系統(tǒng)的性能，選取青島大學(xué)校內(nèi)路段，采用不同精度的車速傳感器對定位系統(tǒng)進(jìn)行仿真實驗，并將仿真實驗結(jié)果與傳統(tǒng)定位方法進(jìn)行對比。仿真結(jié)果表明，融合IMU、GNSS和車速傳感器信號的定位系統(tǒng)，定位誤差大幅減小，車輛在出現(xiàn)胎壓不足或車輪打滑時，定位系統(tǒng)的定位精度依然滿足需求，定位精度顯著提高，并在車速傳感器精度受到干擾的情況下，具有較好的魯棒性，可較好適應(yīng)各種惡劣路況，證明本算法是一套穩(wěn)健可靠的低精度傳感器融合定位算法。該研究具有一定的實際應(yīng)用價值。

關(guān)鍵詞：車輛定位； 擴(kuò)展卡爾曼濾波； 信息融合； 車輛車速傳感器

中圖分類號：TN967.2； P228.4

文獻(xiàn)標(biāo)識碼：A

文章編號：1006-9798（2023）02-0075-07； DOI：10.13306/j.1006-9798.2023.02.011

作者簡介：李慶成（1997-），男，碩士研究生，主要研究方向為混動控制策略。

通信作者：王玉林（1964-），男，博士，教授，碩士生導(dǎo)師，主要研究方向為電控與液壓。 Email：wangyuln@163.com

非公路礦用自卸車是礦區(qū)用于露天采礦和物料運輸?shù)闹饕煌üぞ?，承?dān)著國家基礎(chǔ)設(shè)施建設(shè)和經(jīng)濟(jì)發(fā)展的重要任務(wù)。近年來，隨著無人駕駛技術(shù)和車聯(lián)網(wǎng)技術(shù)在礦車上的應(yīng)用及迅速發(fā)展，將礦用車定位系統(tǒng)與混合動力能量管理控制策略相結(jié)合成為今后的研究關(guān)鍵。全球?qū)Ш叫l(wèi)星系統(tǒng)[1]（global navigation satellite system，GNSS）和慣性導(dǎo)航系統(tǒng)[2]（inertial navigation system，INS）組合形成的定位導(dǎo)航系統(tǒng)在大量現(xiàn)實場景中得到應(yīng)用[3-4]。由于衛(wèi)星信號易受地形地物遮擋等外界因素影響，存在信號干擾、信號遮擋和信號欺騙的問題[5]，而隨著時間的增長，慣性導(dǎo)航誤差會積累，因此將兩者結(jié)合可以同時克服這些缺陷，并充分發(fā)揮各自的優(yōu)勢[6]。但二者的傳感器誤差可導(dǎo)致定位精度不高，通過集成慣導(dǎo)、全球定位系統(tǒng)（global positioning system，GPS）及傳感器誤差的方法來估計車輛運動學(xué)狀態(tài)[7]，此方法能夠提高定位精度，但對速度的感知能力有限。嚴(yán)恭敏等人[8]討論了傳統(tǒng)卡爾曼濾波組合導(dǎo)航算法實際應(yīng)用中可能遇到的問題及解決思路，采用其他輔助傳感器或?qū)Ш较嚓P(guān)信息協(xié)助INS[9]，如考慮里程計比例因子誤差量補(bǔ)償?shù)乃俣容o助更新方法[10]和將速度約束引入 INS 估計循環(huán)[11] ，可以提高定位系統(tǒng)的性能，達(dá)到提高低成本IMU精度的目的。為提高定位系統(tǒng)精度，學(xué)者們提出了許多采用不同結(jié)構(gòu)的方法，針對實際車載組合導(dǎo)航系統(tǒng)量測中不確定噪聲問題，肖廣聰[12]提出利用車道數(shù)據(jù)改進(jìn)協(xié)作貝葉斯濾波器的方法；王慧麗等人[13]提出一種不確定融合估計的GPS/INS組合導(dǎo)航濾波算法；N.MUSAVI等人[14]集成了低成本GPS/INS 定位系統(tǒng)中自適應(yīng)模糊神經(jīng)觀測器的方法；鄧天民等人[15]將速度和姿態(tài)作為非線性自回歸（nonlinear autoregressive exogenous，NARX）神經(jīng)網(wǎng)絡(luò)的輸入變量，進(jìn)行車輛位置的動態(tài)預(yù)測方法；Y.B.MAISSA等人[16]使用擴(kuò)展卡爾曼濾波器（extended kalman filter，EKF）和支持向量機(jī)，通過限制EKF缺陷，可靠地估計車輛位置的方法；姜維等人[17]通過PPP-GPS/IMU緊組合的定位方法，為車輛提供連續(xù)的高精度定位；韓志強(qiáng)[18]提出采用基于深度學(xué)習(xí)圖像處理的方法實現(xiàn)定位，YAO Y Q等人[19]為提高GPS/INS組合導(dǎo)航在GPS中斷時的性能，提出了一種混合融合算法，來提供偽位置信息以輔助組合導(dǎo)航系統(tǒng)[19]。以上這些方法與經(jīng)典卡爾曼濾波相比都具有優(yōu)越性，但均不適合與能量管理控制策略相結(jié)合。J.H.RYU等人[20]將EKF與無跡卡爾曼濾波（unscented kalman filter，UKF）用于GPS/INS的集成，證明該方法能夠提供準(zhǔn)確的航向和位置信息。由于混合動力礦用車的能量管理控制策略與未來速度、道路坡度、駕駛員行為和交通信息等因素密切相關(guān)，因此將未來道路信息作為能量管理控制策略的輸入量，對能量分配進(jìn)行干預(yù)和調(diào)整，需要設(shè)計一種連續(xù)可靠，且低成本的定位系統(tǒng)。為此，在慣性導(dǎo)航系統(tǒng)和GNSS定位導(dǎo)航系統(tǒng)的基礎(chǔ)上，通過擴(kuò)展卡爾曼濾波器，引入車速傳感器信號協(xié)助低精度IMU，結(jié)合非完整性約束構(gòu)成車載導(dǎo)航的重要輔助，對車輛車速進(jìn)行修正，能夠有效抑制低精度IMU的定位誤差發(fā)散和降低GNSS系統(tǒng)誤差。因此，本文在GNSS和慣性導(dǎo)航的組合定位技術(shù)基礎(chǔ)上，融合車速傳感器信號，并考慮各傳感器測量信號的偏移量，構(gòu)建一種定位系統(tǒng)。該系統(tǒng)的結(jié)構(gòu)基于直接配置的擴(kuò)展卡爾曼濾波方法，主要包括慣導(dǎo)系統(tǒng)狀態(tài)方程構(gòu)建GNSS和車速傳感器觀測更新方程兩部分，并通過Matlab實現(xiàn)，同時對不同精度的車速傳感器進(jìn)行仿真模擬，檢驗了面對不同路況的定位效果。該研究具有實際應(yīng)用價值。

1 定位系統(tǒng)構(gòu)建

本文的目標(biāo)是估計系統(tǒng)狀態(tài)量，即

式中，q為車身姿態(tài)的四元數(shù)；Xg為陀螺儀的偏差；rN是用NED坐標(biāo)系表示車輛坐標(biāo)系的原點；vN是車輛在NED坐標(biāo)系中的速度，m/s；Xa是車輛加速度偏差；Xv是車速傳感器偏差。

為了估計系統(tǒng)狀態(tài)量x，本文融合了IMU、車速傳感器和GNSS 3種傳感器信號。IMU是由三軸陀螺儀和三軸加速度傳感器組成，測量車輛姿態(tài)和加速度，可建模為

式中，yg是陀螺儀測量的姿態(tài)；ωB是車身三軸的轉(zhuǎn)動角速度，rad/s；wg是角速度高斯白噪聲；ya是加速度傳感器的測量值，m/s2；aB是車輛在車身坐標(biāo)系中的加速度，m/s2；gB是車身坐標(biāo)系的重力矢量；wa是加速度高斯白噪聲。

GNSS和車速傳感器測量車輛位置和車輪轉(zhuǎn)速，可建模為

2 基于擴(kuò)展卡爾曼濾波的融合定位方法

該系統(tǒng)架構(gòu)是基于預(yù)測-更新步驟的典型循環(huán)，在擴(kuò)展卡爾曼濾波配置的基礎(chǔ)上，增加車速傳感器信號，修正車速的更新模塊。在系統(tǒng)預(yù)測步驟中，預(yù)測方程通過從陀螺儀和加速度計，獲得測量值對系統(tǒng)狀態(tài)的估計。預(yù)測方程在高速率運算下提供正確的估計，但只適用于短期。為了修正和限制系統(tǒng)狀態(tài)估計中的偏移，通過更新方程，將全局參考信息及關(guān)于系統(tǒng)動力學(xué)的先驗假設(shè)融合到系統(tǒng)中。

本文考慮4種類型的系統(tǒng)更新，包括通過動力學(xué)約束進(jìn)行更新，即以中速率將車輛動力學(xué)的先驗假設(shè)并入系統(tǒng)；通過車速傳感器信號進(jìn)行更新，即車輛車輪轉(zhuǎn)速信號以低速率并入系統(tǒng)；通過GNSS測量進(jìn)行更新，即車輛的位置和航向信息以低速率并入系統(tǒng)；通過觀測重力矢量進(jìn)行更新，即關(guān)于車輛姿態(tài)的信息以可變速率并入系統(tǒng)。

通過參數(shù)σ2g和σ2a，利用過程噪聲協(xié)方差矩陣U，將陀螺儀和加速計的測量噪聲并入系統(tǒng)。狀態(tài)轉(zhuǎn)移雅可比矩陣F（式（14））是由非線性預(yù)測模型（式（7））對系統(tǒng)狀態(tài)x的偏導(dǎo)數(shù)組成。系統(tǒng)輸入雅可比矩陣G（式（15））是由非線性預(yù)測模型（式（17））對系統(tǒng)輸入的偏導(dǎo)數(shù)組成。

2.2 系統(tǒng)更新

式中，zi為當(dāng)前測量值；hi為預(yù)測值，由輸出方程式得到；W為卡爾曼增益；H為測量預(yù)測模型對系統(tǒng)狀態(tài)x的偏導(dǎo)數(shù)所形成的雅可比矩陣；P為更新的狀態(tài)協(xié)方差矩陣；R為測量噪聲方差協(xié)方差矩陣。

2.2.1 車速傳感信號融合

車速傳感器測得的車速是車輛在車身坐標(biāo)系下的車速，當(dāng)通過車速傳感器測得的車速修正車輛在北東地（north east down，NED）坐標(biāo)系中的車速時，需要將車輛在NED坐標(biāo)系中的車速轉(zhuǎn)換為車輛在車身坐標(biāo)系下的車速。車速傳感器測得的車速值為vbx；vby=0；vbz=0是對系統(tǒng)的約束。通過融合車速傳感器測得的車速，結(jié)合車輛運動學(xué)約束模型，能夠約束慣性導(dǎo)航系統(tǒng)的誤差發(fā)散。

用于預(yù)測車輛車速傳感器測量值的模型為

2.2.2 GNSS定位信號融合

本文采用一種松耦合的方法，將GNSS單元提供的數(shù)據(jù)合并到系統(tǒng)狀態(tài)中。在松耦合方法中，GNSS單元提供的高電平輸出通過其相應(yīng)的測量預(yù)測模型直接并入系統(tǒng)狀態(tài)。

用于預(yù)測車輛位置的GNSS測量值的模型為

2.2.3 重力加速度約束

如果車輛沒有加速，則式（3）可近似為ya=gB+Xa。在這種情況下，加速度計可以觀測在NED坐標(biāo)系中的重力矢量，重力矢量g可作為修正橫搖和俯仰的外部參考。

3 仿真與結(jié)果分析

3.1 實驗場景

為了測試本文所設(shè)計系統(tǒng)的性能，對其進(jìn)行了模擬仿真。實驗場景取自青島大學(xué)校內(nèi)路段，定位仿真實驗路徑如圖1所示。由圖1可以看出，從起始點（36.072，120.414，0）逆時針勻速行駛，車速取2.5 m/s。根據(jù)路徑和速度信息，計算得到車輛加速度和角速度的NED坐標(biāo)系分量，并應(yīng)用Matlab慣性傳感器模型，得到含有正態(tài)分布噪聲的車輛坐標(biāo)系下傳感器信號。

傳感器誤差參數(shù)分別為：陀螺儀偏差正態(tài)分布方差σ2g，加速度計偏差正態(tài)分布方差σ2a，GNSS偏差正態(tài)分布方差σ2r，陀螺儀偏移量正態(tài)分布方差σ2xg，加速度計偏移量正態(tài)分布方差σ2xa，車速傳感器偏移量正態(tài)分布方差σ2xv，角速度隨時間變化的衰減系數(shù)λxg，加速度隨時間變化的衰減系數(shù)λxa，車速隨時間變化的衰減系數(shù)λxv，傳感器測量誤差參數(shù)值如表1所示。

3.2 仿真實驗結(jié)果

加速度信號隨時間變化曲線如圖2所示，陀螺儀信號隨時間變化曲線如圖3所示。當(dāng)相對精度為5%時，車速傳感器信號隨時間變化曲線如圖4所示。

實驗輸入加速度信號、陀螺儀信號和車速傳感器信號，得到定位誤差和姿態(tài)誤差仿真結(jié)果，定位誤差隨時間變化曲線如圖5所示，姿態(tài)誤差隨時間變化曲線如圖6所示。由圖5可以看出，x、y軸的定位誤差最大漂移量在±0.3 m左右，z軸的定位誤差最大漂移量在±0.1 m左右；由圖6可以看出，x、y軸的姿態(tài)誤差最大漂移量在±0.1°左右，z軸的定位誤差最大漂移量在±0.2°左右。系統(tǒng)輸出結(jié)果與真實測量值之間的均方根誤差即為系統(tǒng)的定位精度，計算可得系統(tǒng)的定位精度為[0.103 m，0.109 m，0.049 m]，姿態(tài)角度精度為[0.0354°，0.0445°，0.0811°]，定位精度良好，并未出現(xiàn)較大偏移，且隨著時間的推移，具有明顯的回歸趨勢，證明了該技術(shù)方案的有效性。

為更加客觀地評價引入車速傳感器信號后的效果，本文將設(shè)計的融合慣性導(dǎo)航、車速傳感器和GNSS的定位系統(tǒng)（integrated inertial navigation、speed sensor and GNSS positioning system，ISGS）與傳統(tǒng)的融合慣性導(dǎo)航和GNSS的定位系統(tǒng)（integration of inertial navigation and GNSS positioning system，IGS）模擬結(jié)果進(jìn)行對比，ISGS與IGS對比結(jié)果如表2所示。

由表2可以看出，引入車速傳感器信號后，系統(tǒng)的定位誤差大幅減小。由于礦場惡劣的路面狀況，車速傳感器的精度可能會波動，為檢驗引入車速傳感器信號后，車速傳感器精度對定位精度的影響，對該系統(tǒng)分別采用不同精度車速傳感器進(jìn)行模擬實驗，不同精度車速傳感器定位系統(tǒng)對比結(jié)果如表3所示。由表3可以看出，該定位系統(tǒng)在采用低精度車速傳感器時，依然能夠達(dá)到較為理想的定位效果，說明車輛在出現(xiàn)胎壓不足或車輪打滑時，定位系統(tǒng)的定位精度依然滿足需求，證明本文所提出的融合車速傳感器信號輔助速度更新算法，精度改善效果顯著，是一套具有實用價值和穩(wěn)健可靠的低精度傳感器融合定位算法。

4 結(jié)束語

本文主要對基于IMU/GNSS/車速傳感器的礦用車定位系統(tǒng)進(jìn)行研究，提了一種通過擴(kuò)展卡爾曼濾波器集成IMU、車速傳感器和GNSS信號的定位方法，并采用不同精度的車速傳感器對定位系統(tǒng)進(jìn)行仿真實驗，同時將仿真實驗結(jié)果與傳統(tǒng)定位方法進(jìn)行對比。實驗結(jié)果表明，該方法可充分利用低成本傳感器實現(xiàn)高精度定位，定位精度可達(dá)0.1～0.3 m，且對惡劣路況具有良好的魯棒性，能夠很好地滿足對能量管理控制策略進(jìn)行干預(yù)的定位需要。以后將考慮礦場路面的極端破壞性對IMU產(chǎn)生較大影響的情況，減少對IMU的依賴，這是在實際工程應(yīng)用中重點研究的方向。

參考文獻(xiàn)：

[1] 卞鴻巍. 現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M]. 北京：國防工業(yè)出版社，2010.

[2] 霍國平. GPS定位定向中的若干問題研究[D]. 北京：北京理工大學(xué)，2013.

[3] BORTZ J E. A new mathematical formulation for strapdown inertial navigation[J]. IEEE Transactions on Aerospace and Electronic Systems，1971（1）：61-66.

[4] COX D B. Integration of GPS with inertial navigation systems[J]. Navigation，1978，25（2）：236-245.

[5] 朱鋒. GPS/SINS/視覺多傳感器融合的精密定位定姿方法與關(guān)鍵技術(shù)[D]. 武漢：武漢大學(xué)，2019.

[6] 張輝. 復(fù)雜環(huán)境下基于GPS＋BDS-3的PPP／INS／ODO組合導(dǎo)航性能分析[J]. 大地測量與地球動力學(xué)，2022，42（3）：275-280.

[7] MUNGUIA R. A GPS-aided inertial navigation system in direct configuration[J]. Applied Research and Technology，2014，12（4）：803-814.

[8] 嚴(yán)恭敏，鄧瑀. 傳統(tǒng)組合導(dǎo)航中的實用Kalman濾波技術(shù)評述[J]. 導(dǎo)航定位與授時，2020，7（2）：50-64.

[9] SHIN E-H. Estimation techniques for low-cost inertial navigation[D]. Alberta：University of Calgary，2005.

[10] 彭益堂. GNSS/INS/相機(jī)車道線多傳感器融合定位算法研究[D]. 武漢：武漢大學(xué)，2020.

[11] GODHA S，CANNON M E. GPS/MEMS INS integrated system for navigation in urban areas[J]. GPS Solut，2007，11：193-203.

[12] 肖廣聰. 基于多傳感器信息融合的智能汽車定位方法研究[D]. 長沙：湖南大學(xué)，2021.

[13] 王慧麗，史忠科． 基于不確定融合的GPS/INS組合導(dǎo)航濾波算法[J]． 控制與決策，2015，30（7）：1201-1206.

[14] MUSAVI N，KEIGHOBADI J． Adaptive fuzzy neuro-observer applied to low cost INS/GPS[J]． Applied Soft Computing，2015，29：82-94.

[15] 鄧天民，楊其芝，方芳，等． 基于非線性自適應(yīng)回歸神經(jīng)網(wǎng)絡(luò)的GPS/IMU組合導(dǎo)航方法[J]． 科學(xué)技術(shù)與工程，2019，19（24）：274-280.

[16] BELHAJEM I，MAISSA Y B，TAMTAOUI A． Improving vehicle localization in a smart city with low cost sensor networks and support vector machines[J]． Mobile Networks and Applications，2018，23（4）：854-863.

[17] 姜維，劉夢楊． 基于PPP-GPS/IMU緊組合的列車定位方法[J]． 鐵道學(xué)報，2022，44（4）：54-62.

[18] 韓志強(qiáng). 基于深度學(xué)習(xí)圖像處理的城市軌道交通列車定位方法研究[D]. 杭州：浙江大學(xué)，2022.

[19] YAO Y Q，XU X S. A hybrid fusion algorithm for GPS/INS integration during GPS outages[J]. Measurement，2017，103：711-719.

[20] RYU J H，GANKHUYAG G，CHONG K T． Navigation system heading and position accuracy improvement through GPS and INS data fusion[J]. Journal of Sensors，2016，2016（3）：1-6.

Abstract：Aiming at the problem that the positioning system of mine vehicle using low precision inertial measurement unit （IMU） cannot be combined with the hybrid energy management control strategy，this paper designs a positioning system that integrates IMU，GNSS and speed sensor signals. The structure of the system is based on the direct configuration extended Kalman filter method. The filter is composed of kinematic model and sensor error model. The vehicle speed sensor signal is introduced to correct the vehicle speed as the auxiliary information to predict the next position. The architecture is designed in such a way that it is easy to calibrate different sensor parameters so that it can be applied to vehicles with different positioning requirements. At the same time，in order to test the performance of the system designed in this paper，the road section in Qingdao University was selected in the simulation experiment，and the speed sensor with different accuracy was used to simulate the positioning system，and the simulation results were compared with the traditional positioning method. The simulation results show that the positioning error of the system，which integrates IMU，GNSS and speed sensor signals，is greatly reduced. When the vehicle has low tire pressure or wheel slip，the positioning accuracy of the positioning system still meets the demand，and the positioning accuracy is significantly improved. In addition，it has good robustness when the accuracy of the speed sensor is interfered with，and it can better adapt to all kinds of bad road conditions. This verifies that this algorithm is a practical and robust low precision sensor fusion location algorithm. This research has certain theoretical significance and practical application value.

Key words：vehicle positioning； extended Kalman filter； information fusion； vehicle speed sensor