一種車輛模型輔助的MEMS-SINS導航方法

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

（北京理工大學 自動化學院，北京 100081）

一種車輛模型輔助的MEMS-SINS導航方法

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

（北京理工大學 自動化學院，北京 100081）

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

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

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

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

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

1 整體方案

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

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

2 基于運動模型的ABS和方向盤轉角信息融合

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

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

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

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

圖3 車輛轉向過程位移關系圖Fig.3 Elementary displacement of vehicle in turning motion

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

圖4 車輛轉彎幾何關系圖Fig.4 Geometry of car in a turning motion

繼而得到：

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

由ΔS、cω進一步推導得到車輛的縱向速度vxc和向心加速度fyc（也即車輛側向加速度）：

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

3 車輛運動學輔助的MEMS-SINS導航濾波方法

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

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

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

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

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

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

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

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

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

根據擾動法則，進一步得到車體速度的擾動增量方程：

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

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

4 跑車驗證與分析

4.1 實驗環境及條件

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

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

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

圖6 實驗設備Fig.6 Testing equipment

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

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

4.2 實驗對比及討論

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

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

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

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

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

圖8 二維定位結果圖Fig.8 Positioning results inx-yplain

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

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

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

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

5 結 論

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

（References）：

[1] 崔留爭. MEMS-SINS/GPS組合導航關鍵技術研究[D].中國科學院大學, 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] 呂品, 賴際舟, 楊天雨, 等. 基于氣動模型輔助的四旋翼飛行器室內自主導航方法[J]. 航空學報, 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] 付強文, 秦永元, 李四海, 等. 車輛運動學約束輔助的慣性導航算法[J]. 中國慣性技術學報, 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慣性組合導航機體系約束算法研究[J]. 北京理工大學學報, 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慣導導航定位方法[J]. 中國慣性技術學報, 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]. 中國慣性技術學報, 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

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

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