一種車輛模型輔助的MEMS-SINS導(dǎo)航方法

王美玲，馮國強，李亞峰，于華超，劉 彤

（北京理工大學(xué) 自動化學(xué)院，北京 100081）

一種車輛模型輔助的MEMS-SINS導(dǎo)航方法

王美玲，馮國強，李亞峰，于華超，劉 彤

（北京理工大學(xué) 自動化學(xué)院，北京 100081）

針對城市環(huán)境中GNSS因遮擋導(dǎo)致MEMS-SINS精度快速降低的問題，在車輛運動學(xué)約束的基礎(chǔ)上，結(jié)合四通道ABS輪速傳感器和方向盤轉(zhuǎn)角信息，提出一種新的適用于陸地車輛的MEMS-SINS導(dǎo)航方法。該方法通過分析車輛轉(zhuǎn)彎和運動約束特性，構(gòu)建角速度和加速度觀測量，從而實現(xiàn)基于模型輔助的MEMS誤差在線補償；其次，ABS輪速信息與非完整約束條件結(jié)合可額外增加三維車體速度觀測量，進(jìn)一步維持衛(wèi)星失效時組合濾波器的量測更新。跑車實驗表明，在GNSS信號頻繁丟失甚至長時間無法定位時，低精度MEMS慣性器件引起的快速誤差積累得到有效抑制，與經(jīng)典車體約束結(jié)合里程計算法相比，航向精度提高約70%，位置、速度精度也有相應(yīng)的提高，驗證了算法的有效性。

MEMS-SINS；ABS輪速傳感器；運動學(xué)約束；模型輔助

隨著微機(jī)電系統(tǒng)（Micro Electro Mechanical System，MEMS）的快速發(fā)展，基于MEMS器件的低成本、小型化捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）成為國內(nèi)外慣性導(dǎo)航技術(shù)的主要研究方向之一[1]。但現(xiàn)有MEMS慣性器件測量精度有限，導(dǎo)致MEMS-SINS定位誤差隨著時間增加而快速積累，在短時間內(nèi)定位精度也較低，因此與其他定位系統(tǒng)的組合是提高其定位精度的重要措施。目前通常采用全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System，GNSS）與MEMS-SINS相結(jié)合的方式來彌補各自的不足，從而為載體提供較高精度的位置、速度信息[2-3]。但是在復(fù)雜城市峽谷環(huán)境下，GNSS信號頻繁丟失甚至長時間無法定位，MEMS-SINS獨立工作時間增加，從而導(dǎo)致組合系統(tǒng)定位、測姿精度迅速惡化。因此，如何提高GNSS信號缺失時MEMS-SINS的導(dǎo)航精度，對于車輛導(dǎo)航定位具有重要意義。

模型輔助導(dǎo)航是一種廉價有效的MEMS-SINS誤差修正方法[4]，主要有動力學(xué)輔助和運動學(xué)約束兩種模式。由于地面車輛具有大量子系統(tǒng)和強非線性特性[5]，所以建立簡潔實用的動力學(xué)模型比較困難。相對來講，運動學(xué)約束是車載SINS誤差控制的常用手段。常規(guī)運動學(xué)約束主要對車體橫向和垂向速度施加約束，以限制慣性器件誤差積累[6-9]。Godha將高度約束用于陸用GNSS/SINS緊組合[6]，進(jìn)一步增加了約束信息；Yang假設(shè)車輛運行過程中只有航向角發(fā)生變化，進(jìn)而引入加速度和姿態(tài)角約束[9]。但無論哪種算法，都是將非完整約束條件作為偽測量值引入組合濾波器來估計、反饋SINS誤差，對間接觀測量如姿態(tài)和MEMS器件誤差的估計效果較差[10]。另外，約束條件無法提供全面的載體運動信息，尤其是車體縱向速度以及航向變化信息，所以僅依靠運動學(xué)約束難以保證GNSS長時間失效時MEMS-SINS的導(dǎo)航精度，因此有必要在運動約束的基礎(chǔ)上引入車輛自帶傳感器提供的低成本運動信息，形成信息互補，達(dá)到共同輔助MEMS-SINS導(dǎo)航的目的。

綜合以上考慮，進(jìn)一步將運動學(xué)約束直接用于導(dǎo)航解算前，補償MEMS測量誤差，并引入車輛自帶四通道制動防抱死系統(tǒng)（Antilock Brake System，ABS)和方向盤轉(zhuǎn)角傳感器，通過分析車輛轉(zhuǎn)彎特性和運動學(xué)約束，提出一種新的基于角速度、加速度信息的濾波算法，同時在橫向、垂向速度約束中引入ABS輪速傳感器提供的縱向速度，在GNSS失效時可持續(xù)保持組合濾波器的更新。最后，通過跑車試驗驗證了算法的有效性。

1 整體方案

針對城市峽谷環(huán)境的復(fù)雜性，在傳統(tǒng)運動學(xué)約束的基礎(chǔ)上，增加車輛自帶四通道ABS和方向盤轉(zhuǎn)角傳感器組成一種新的MEMS-SINS導(dǎo)航系統(tǒng)。如圖1所示，本文提出的算法首先基于ABS輪速信息和方向盤轉(zhuǎn)角信息，通過分析車輛轉(zhuǎn)彎運動模型后獲得車輛縱向速度、橫擺角速度。然后結(jié)合車輛運動學(xué)約束，構(gòu)建一組角速度、加速度值，以此作為前置濾波器的量測信息。該前置濾波器通過建立MEMS隨機(jī)漂移模型，并引入以上量測信息，實現(xiàn)導(dǎo)航解算前MEMS漂移誤差的補償，從而減少導(dǎo)航解算的誤差積累。其次，縱向速度信息結(jié)合車體速度非完整約束條件一起作為新的觀測量，引入組合濾波器中，持續(xù)估計、反饋SINS誤差。在整個導(dǎo)航系統(tǒng)中，GNSS僅在其定位信息有效時引入組合濾波器中，而上述方法不論GNSS是否有效，均可用于提高M(jìn)EMS-SINS導(dǎo)航精度。

圖1 新型MEMS-SINS導(dǎo)航方法原理框圖Fig.1 Schematic of new MEMS-SINS navigation method

2 基于運動模型的ABS和方向盤轉(zhuǎn)角信息融合

多數(shù)車輛以前輪為轉(zhuǎn)向輪，轉(zhuǎn)向行駛時，車輛的瞬時運動可看作以后軸中點為圓心的圓周運動，車輛直線行駛可看作是曲率半徑無限大的圓周運動[11]。如圖2，車體坐標(biāo)系原點O固連于車輛后軸中心，車輛速度方向與x軸平行，圓周運動的圓心P點在y軸軸線上。

設(shè)ABS傳感器采樣周期為Tc，假設(shè)車輛在Tc時段從A點移動到B點，移動的距離為ΔS，車輛橫擺角速度即航向角變化率為cω，如圖3所示。

圖3中，將兩個前輪虛擬為一個中心輪，δ為虛擬前輪轉(zhuǎn)角，ΔSF為虛擬前輪的移動距離，ΔSRL、ΔSRR分別為左后輪、右后輪移動距離，L為車身軸距，2B為兩車輪輪距。根據(jù)圖3中幾何關(guān)系可得：

圖2 車輛圓周運動示意圖Fig.2 Schematic of vehicle circular motion

圖3 車輛轉(zhuǎn)向過程位移關(guān)系圖Fig.3 Elementary displacement of vehicle in turning motion

式(1)中，δ正比于方向盤轉(zhuǎn)角，ΔSF不能由傳感器直接測量，但根據(jù)阿克曼轉(zhuǎn)向原理可得圖4所示四個車輪的幾何關(guān)系。

圖4 車輛轉(zhuǎn)彎幾何關(guān)系圖Fig.4 Geometry of car in a turning motion

繼而得到：

因為輪胎與地面存在相對滑動，為了提高ΔS、cω的精度，應(yīng)融合式(3)所示關(guān)于未知量ΔS、cω的全部表達(dá)式?？紤]ABS測量信息不涉及加速度等變量，可得如下EKF融合模型[12]：

由ΔS、cω進(jìn)一步推導(dǎo)得到車輛的縱向速度vxc和向心加速度fyc（也即車輛側(cè)向加速度）：

另外，假設(shè)汽車縱向加速度在很短周期Tc內(nèi)保持不變，則i時刻縱向加速度為：

3 車輛運動學(xué)輔助的MEMS-SINS導(dǎo)航濾波方法

3.1 基于角速度、加速度信息的濾波方法

MEMS慣性器件誤差可建模為自相關(guān)函數(shù)具有指數(shù)形式的一階高斯-馬爾可夫過程，選取MEMS測量誤差作為狀態(tài)變量，可得：

式(11)(13)為通過分析車輛轉(zhuǎn)彎特性和運動學(xué)約束后所得的角速度、加速度信息，與MEMS測量值相比，兩者物理意義相同，但數(shù)據(jù)源不同，因此可將兩者相減作為卡爾曼濾波的量測方程，得到：

通過以上方程可實現(xiàn)車輛運動學(xué)模型和MEMS測量值的融合，有效去除加速度、角速度中的隨機(jī)噪聲，實現(xiàn)了基于模型輔助的MEMS誤差在線補償，從而提高SINS導(dǎo)航解算的精度。

3.2 基于車體速度輔助的MEMS-SINS濾波器

基于車體的速度輔助是在車體橫向、垂向速度約束的基礎(chǔ)上，引入ABS輪速傳感器提供的縱向速度，然后同SINS導(dǎo)航解算所得車體速度相減作為組合濾波器的量測方程，從而保持衛(wèi)星失效時組合濾波器的持續(xù)更新。采用攝動法建立SINS誤差模型，離散化后，其狀態(tài)方程為

在正常城市路況下，假設(shè)車輛僅存在輕微的側(cè)滑和跳躍[9]，以此作為車體系速度約束。因此車系y、z向速度可建模為

同時考慮式(6)所得車體x向速度：

另一方面，SINS解算速度nv與其在車體坐標(biāo)系下的投影滿足：

根據(jù)擾動法則，進(jìn)一步得到車體速度的擾動增量方程：

將不同信源的SINS解算速度、ABS速度同車體約束速度信息相減作為量測方程，并通過式(19)引入SINS誤差模型中，即：

通過式(18)(19)可知，利用速度輔助和約束能夠同時估計SINS解算的速度和姿態(tài)誤差，從而減緩GNSS信號受遮擋期間定位結(jié)果的發(fā)散。相比于常規(guī)車體速度約束算法[6-8]，增加了車體縱向速度的輔助，可進(jìn)一步提高M(jìn)EMS-SINS的導(dǎo)航精度。

4 跑車驗證與分析

4.1 實驗環(huán)境及條件

如圖5所示，試驗平臺為北京理工大學(xué)組合導(dǎo)航與智能導(dǎo)航（Integrated Navigation and Intelligent Navigation，ININ）實驗室自主改裝的無人駕駛車輛（Unmanned Ground Vehicle，UGV），該UGV自帶4通道ABS傳感器和方向盤轉(zhuǎn)角傳感器，數(shù)據(jù)更新率為25 Hz。

MEMS慣性器件測量值通過東方聯(lián)星公司PNS100-BGI型組合系統(tǒng)采集，數(shù)據(jù)更新率為50 Hz，其主要參數(shù)通過Allan方差分析所得，結(jié)果如表1所示。GNSS采用NovAtel公司OEM628三模接收機(jī)，數(shù)據(jù)更新率為1 Hz。測試時同時搭載ININ 實驗室自主開發(fā)的高精度RTK-GNSS/SINS組合系統(tǒng)（位置1σ精度：2cm；航向精度：0.06°）作為本文算法評判的精度基準(zhǔn)，并以GPS時間作為數(shù)據(jù)融合的時間對準(zhǔn)基準(zhǔn)。各實驗設(shè)備如圖6所示。

圖5 UGV實驗平臺Fig.5 UGV experimental platform

圖6 實驗設(shè)備Fig.6 Testing equipment

GNSS記錄跑車行進(jìn)軌跡如圖7所示。UGV從北京理工大學(xué)6號教學(xué)樓出發(fā)，沿西北三環(huán)輔路、魏公村路行駛一圈后回到起點，全程4.8 km，歷時17 min。行進(jìn)過程中，GNSS信號頻繁受到橋梁、高樓、樹木的遮擋，導(dǎo)致GNSS接收機(jī)出現(xiàn)5次無定位（路段1～5），1次定位精度差現(xiàn)象（路段6），其中路段2由于高架橋影響GNSS無定位時間高達(dá)132 s。

圖7 GNSS軌跡圖Fig.7 Trajectory of GNSS data

4.2 實驗對比及討論

實驗結(jié)果首先與經(jīng)典車體橫向、垂向速度約束算法作比較，從而說明前置濾波器和車體三維速度輔助對于提高導(dǎo)航精度的效果，此為對比實驗之一；其次，為了說明在相同傳感器信息條件下本文算法的有效性，在經(jīng)典車體速度約束的基礎(chǔ)上加入里程計信息，作為對比實驗之二。而本文算法主要分為以下三種方式進(jìn)行討論：僅采用前置濾波器（方式1）；僅采用車體速度輔助（方式2）；同時采用前置濾波器和車體速度輔助（方式3）。對比結(jié)果如表2所示。

可以看出，相比于經(jīng)典車體約束算法，方式1、方式2均能夠顯著提高組合導(dǎo)航精度。對于方式1，由于前置濾波器減少了慣性器件原始測量值的誤差，因此位置、速度、航向精度均有明顯提高，其中東向、北向位置精度分別提高了71.7%、36.7%，航向精度提高了38.8%。

表2 三種方式位置、速度、航向均方根誤差值（RMS）Tab.2 RMS errors of position, velocity, and heading from the different methods

對于方式2，速度輔助提供了完整的三個維度的速度觀測量，而且根據(jù)SINS誤差傳播特性，車體坐標(biāo)系的速度觀測量對于卡爾曼濾波器的系統(tǒng)狀態(tài)中，與位置相關(guān)和速度相關(guān)的狀態(tài)向量可觀測性強，所以位置和速度精度提高比例較大，平均提高80%以上，而對于提高航向精度效果較弱，僅提高9.7%。方式3結(jié)果顯示同時使用前置濾波器和車體速度輔助可融合兩者優(yōu)點，進(jìn)一步提高導(dǎo)航精度，其中，位置速度精度與方式2持平，但航向精度提高76%。

為進(jìn)一步評價GNSS長時間失效期間，本文所述算法的性能，對比了經(jīng)典車體約束算法和方式3的定位軌跡，結(jié)果如圖8所示。由圖8可知，在GNSS信號無遮擋的情況下，兩種方式的軌跡與基準(zhǔn)軌跡吻合度均較好，但在GNSS信號受遮擋期間，尤其是路段2長時間無定位情況下，經(jīng)典車體約束算法位置誤差迅速增大。

圖8 二維定位結(jié)果圖Fig.8 Positioning results inx-yplain

圖9給出了路段2的位置及航向角誤差，由圖9可知，經(jīng)典車體約束算法北向、東向、高度誤差最大分別達(dá)到264.72 m、44.63 m、15.80 m，航向角誤差最大超過2.32°，完全無法滿足導(dǎo)航精度要求。由于此時GNSS信息完全失效，經(jīng)典車體約束算法在長時間無外部傳感器輔助的情況下，MEMS-SINS定位誤差快速發(fā)散，達(dá)到每秒數(shù)米的量級，橫向、垂向速度約束雖然能夠減少誤差積累，但效果有限。而方式3北向、東向、高度誤差最大分別為3.20 m、4.50 m、2.02 m，航向角誤差最大0.91°?？梢娂词乖陂L時間無定位的情況下，也能夠滿足UGV導(dǎo)航精度要求。

對于對比實驗二，通過表2可知，經(jīng)典車體約束+里程計算法在東向和北向維度上，位置和速度精度要高于方式1情況。這是因為前置濾波器雖然能夠提高M(jìn)EMS原始測量值精度，但是其誤差仍會隨時間積累，因此需要進(jìn)一步通過卡爾曼濾波來估計反饋SINS解算誤差。

圖9 路段2位置及航向誤差Fig.9 Position and heading errors of road section 2

另外，通過表2可知，經(jīng)典車體約束+里程計導(dǎo)航精度與方式2處于同一數(shù)量級。這是由于兩者均能夠提供全部維度的速度信息，不同之處在于傳統(tǒng)算法將里程計速度投影到導(dǎo)航系進(jìn)行量測更新，而方式2直接在車體系同時進(jìn)行速度輔助和約束，形式更為簡潔。方式3導(dǎo)航精度均優(yōu)于經(jīng)典車體約束+里程計算法，尤其是航向精度提高較大，約提高70%，克服了傳統(tǒng)組合系統(tǒng)對航向信息估計效果較差的缺點。

5 結(jié) 論

本文針對復(fù)雜城市峽谷環(huán)境下，傳統(tǒng)運動學(xué)約束算法難以保證GNSS長時間失效時MEMS-SINS的導(dǎo)航精度問題，實現(xiàn)了一種車輛模型輔助的MEMS-SINS導(dǎo)航方法。該方法通過引入ABS輪速傳感器及車輛運動學(xué)模型，建立了基于角速度、加速度信息的濾波方案，并增加車體系的三維速度觀測量作為新的濾波量測更新。跑車結(jié)果表明組合系統(tǒng)定位、測姿精度均有較大改善，證明了算法的有效性，為MEMS-SINS組合系統(tǒng)提供了一種簡單有效的低成本導(dǎo)航方法。

（References）：

[1] 崔留爭. MEMS-SINS/GPS組合導(dǎo)航關(guān)鍵技術(shù)研究[D].中國科學(xué)院大學(xué), 2014. Cui Liu-zheng. Research on the key technologies of MEMS-SINS/GPS integration navigation syetem[D]. University of Chinese Academy of Sciences, 2014.

[2] Rabbou M A, El-Rabbany A. Tightly coupled integration of GPS precise point positioning and MEMS-based inertial systems[J]. GPS Solutions, 2015, 19(4): 601-609.

[3] Quinchia A G, Falco G, Falletti E, et al. A comparison between different error modeling of MEMS applied to GPS/INS integrated systems[J]. Sensors, 2013, 13(8): 9549-9588.

[4] 呂品, 賴際舟, 楊天雨, 等. 基于氣動模型輔助的四旋翼飛行器室內(nèi)自主導(dǎo)航方法[J]. 航空學(xué)報, 2015, 36(4): 1275-1284. Lv Pin, Lai Ji-zhou, Yang Tian-yu, et al. Autonomous navigation method aided by aerodynamics model for an indoor quadrotor[J]. Acta Aeronautica Et Astronautica Sinica, 2015, 36(4): 1275-1284.

[5] Fernández J G. A vehicle dynamics model for driving simulators[D]. Chalmers University of Technology, 2012.

[6] Godha S, Cannon M E. GPS/MEMS INS integrated system for navigation in urban areas[J]. GPS Solutions, 2007, 11(3): 193-203.

[7] 付強文, 秦永元, 李四海, 等. 車輛運動學(xué)約束輔助的慣性導(dǎo)航算法[J]. 中國慣性技術(shù)學(xué)報, 2012, 20(6): 640- 643. Fu Qiang-wen, Qin Yun-yuan, Li Si-hai, et al. Inertial navigation algorithm aided by motion constraints of vehicle[J]. Journal of Chinese Inertial Technology, 2012, 20(6): 640-643.

[8] 劉華, 劉彤, 張繼偉. 陸地車輛 GNSS/MEMS慣性組合導(dǎo)航機(jī)體系約束算法研究[J]. 北京理工大學(xué)學(xué)報, 2013, 33(5): 510-515. Liu Hua, Liu Tong, Zhang Ji-wei. Body Frame constraint for a GNSS/MEMS INS integrated system in land vehicle navigation[J]. Journal of Beijing Institute of Technology, 2013, 33(5): 510-515.

[9] Yang L, Li Y, Wu Y, et al. An enhanced MEMSINS/GNSS integrated system with fault detection and exclusion capability for land vehicle navigation in urban areas[J]. GPS Solutions, 2014, 18(4): 593-603.

[10] Crocoll P, G?rcke L, Trommer G F, et al. Unified model Technique for inertial navigation aided by vehicle dynamics model[J]. Navigation, 2013, 60(3): 179-193.

[11] 李博文, 姚丹亞. 低成本車載MEMS慣導(dǎo)導(dǎo)航定位方法[J]. 中國慣性技術(shù)學(xué)報, 2014, 22(6): 719-723. Li Bo-wen, Yao Dan-ya. Low-cost MEMS IMU navigation positioning method for land vehicle[J]. Journal of Chinese Inertial Technology, 2014, 22(6): 719-723.

[12] Bonnifait P, Bouron P, Crubille P, et al. Data fusion of four ABS sensors and GPS for an enhanced localization of car-like vehicles[C]//IEEE International Conference on Robotics & Automation. 2001, Vol.2: 1597-1602.

[13] 李磊磊, 陳家斌, 楊黎明, 等. 基于道路信息的智能地圖匹配算法[J]. 中國慣性技術(shù)學(xué)報, 2016, 24(2): 170-174. Li Lei-lei, Chen Jia-bin, Yang Li-ming, et al. Intelligent map-matching algorithm based on map information[J]. Journal of Chinese Inertial Technology, 2016, 24(2): 170-174.

MEMS-SINS navigation method aided by vehicle model

WANG Mei-ling, FENG Guo-qiang, LI Ya-feng, YU Hua-chao, LIU Tong
(School of Automation, Beijing Institute of Technology, Beijing 100081, China)

In view that the frequent outages of GNSS in urban environments can quickly degrade the performance of MEMS-SINS, a new MEMS-SINS navigation method for land vehicles is proposed based on the vehicle constraints and combined with the four-channel ABS wheel speed sensors and steering angle information. By analyzing the vehicle turning and constraint characteristics, the angular velocity and acceleration are constructed as the measurements to achieve on-line compensation for MEMS’s rapid drifting errors. Three-dimension vehicle-body velocity provided by ABS information and non-holonomic constraint is applied to further maintain the update of the integration Kalman filtering during GNSS outages. The road-test results demonstrate the proposed method can effectively reduce the rapid accumulation errors of SINS due to low-cost MEMS inherent bias in the circumstances of long-time outrages of GNSS. Compared with conventional body velocity constraint and odometer algorithm, the heading accuracy is improved by 70%, and the accuracies of position and velocity are also improved.

MEMS-SINS; ABS wheel speed sensors; vehicle constraints; model aiding

U666. 1

A

1005-6734(2017)02-0209-07

10.13695/j.cnki.12-1222/o3.2017.02.013

2017-01-11；

2017-03-28

國家自然科學(xué)基金面上項目（61173076，61473042）；國家自然科學(xué)基金重大研究計劃培育項目（91120003）

王美玲（1970—），女，教授，從事組合導(dǎo)航與智能導(dǎo)航研究。E-mail: wangml@bit.edu.cn