低成本車載MEMS慣導導航定位方法

（清華大學 自動化系，北京 100084）

（清華大學 自動化系，北京 100084）

當GPS衛星信號受到遮擋，車載GPS無定位輸出時，通過低成本MEMS慣導進行定位是車輛導航的一種彌補方法。然而低成本MEMS慣導測量誤差大，定位誤差會快速累積。針對此問題，研究了一種應用于MEMS慣導導航定位的Kalman濾波算法。通過分析車輛的運動特性，在已有的研究基礎上，提出了向心加速度差值誤差這一新型觀測量，并推導了誤差狀態系統模型的狀態轉移矩陣和觀測矩陣。200 s時長的實車實驗表明，單純MEMS慣導定位的誤差率是75.27%，而所述新方法的定位誤差率是3.86%，定位精度有了大幅度提高，取得了良好的效果。

MEMS慣導；車載導航；定位；Kalman濾波；向心加速度；觀測量

GPS(Global Positioning System，全球定位系統)是目前在民用車輛上廣泛使用的導航定位設備。GPS能夠在地球表面絕大部分地區準確確定車輛的位置，為車輛提供路徑導航等服務。GPS進行定位解算需要接收到至少4顆衛星的信號[1]。然而當車輛行駛到被樹木或建筑物遮擋嚴重的道路、隧道、地下停車場等區域時，GPS不能接收到足夠多的衛星信號來進行定位解算，這種情況下將沒有定位輸出，此時可以借助慣導設備進行推算定位。高精度慣導由于其價格高，難以應用于民用車輛。隨著技術的快速發展，出現了低成本MEMS(Micro-Electro-Mechanical System，微機電)慣導產品，為民用車輛使用慣導設備創造了機會[2]。

MEMS慣導體積小、能耗低、價格低，但是其測量誤差大[3]。一般來說，MEMS慣導設備價格越低，測量誤差越大。使用低成本 MEMS慣導進行導航定位，其定位誤差會隨著時間增加而快速積累，在短時間內定位精度也較低，因而研究如何降低低成本MEMS慣導導航的定位誤差，對于無GPS定位輸出時的車輛導航定位具有積極意義。慣導導航定位降低誤差的方法，通常是先建立適當的誤差狀態系統模型，再通過 Kalman等數據融合方法進行誤差估計，最后根據誤差估計對姿態、速度和位置進行校正[4-5]。當GPS沒有位置和速度輸出時，可用車輛坐標系橫向和豎直方向的速度誤差作為觀測量[6]。然而僅通過此觀測量進行狀態估計后，定位精度仍有待提高。本文結合車輛的運動特性，提出了一種新的觀測量，以進一步提高慣導導航定位時的定位精度。

1 定位算法

對于慣性導航系統，載體的姿態、速度和位置的變化率可以通過以下方程計算得到[7]：

式中：v是速度矢量；P表示載體的位置矢量，P= [L λh]T，其中L、λ、h分別為緯度、經度和高度；f是比力矢量；g是重力加速度矢量；i、e、n、b分別是慣性系、地心地固坐標系、東北天導航坐標系和載體坐標系； ωie、 ωen、 ωib、 ωin分別表示坐標系e相對于i，n相對于e，b相對于i，n相對于i的旋轉角速度矢量；上標n、b分別表示相應的值在坐標系n和b上的分解；是載體坐標系到導航坐標系的旋轉矩陣，也稱為姿態矩陣；表示的反對稱矩陣；ξ是載體線速度到經緯度角速度的轉換矩陣：

其中 RM、RN分別為當地子午圈和卯酉圈的曲率半徑。

當初始導航參數已知，通過加速度計和陀螺儀可以獲取 fb和，根據式(1)～(3)計算姿態、速度和位置的變化率后，再通過積分運算就可以計算得到載體的當前姿態、速度和位置。

2 誤差傳播

通過慣導輸出的加速度和角速度數據來直接計算得到的姿態、速度和位置都含有誤差[8]。計算值和真實值的關系如下：

在導航坐標系下慣導誤差傳播方程如下[8]：

式中， εg、 εa分別是陀螺儀和加速度計的測量誤差，它們可以簡化看作是一階馬爾科夫過程：

式中，τg、τa分別是 εg、εa的時間相關常數，wg、wa都是白噪聲。

3 觀測量

3.1 已有觀測量

通常GPS/INS組合導航的觀測量為GPS輸出值和慣導計算值之間的差值，包括位置差和速度差。然而當GPS無有效輸出，只通過慣導定位時，獲取不了這些觀測量。對于車輛運動，可以假設車輛不會發生側滑，并且不會離開地面。設車輛載體坐標系如圖1所示。

圖1 車輛載體坐標系Fig.1 Vehicle body frame coordinates

載體坐標系的原點O位于車輛后輪軸中央上方一點，x軸平行于后輪軸指向車輛的右側，y軸指向車輛前方，z軸垂直于x、y軸指向車輛上方。由于車輛不會發生側滑，并且不會離開地面，則可以得到：車輛在載體坐標系x軸方向速度和z軸方向速度均為0，即

3.2 新觀測量

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

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

假設車輛在很短的時間dt內從A點移動到 A′點，移動的距離為dl，速度方向變化的角度為dφ，如圖3所示。

圖3 圓周運動細節分析Fig.3 Detail analysis of vehicle circular motion

根據圓周運動定律，可以推導得到向心加速度等于速度乘以角速度：

式中， ac為向心加速度。

設J為向心加速度差值，其通過下式計算：

根據方程(17)可得：

因為地球自轉角速度和車輛線運動形成的相對于地球的旋轉角速度相對較小，本文中將這兩項角速度忽略，因而：

4 誤差狀態系統模型

通過擾動方程(18)，可以得到J的誤差方程：

式中， g0是當地重力加速度大小。

則可以建立誤差狀態系統模型，誤差狀態向量包含15個參數。系統誤差向量X設為：

設離散化后的系統狀態模型如下：

式中，Φk,k-1是15× 15維的系統狀態轉移矩陣； Hk是3× 15維的觀測矩陣； Wk-1是過程噪聲向量； Vk是觀測噪聲向量。設觀測矩陣 Hk為：

矩陣 Φk,k-1、A的參數可參考文獻[6][8]。根據誤差方程(27)，可以推導得到M矩陣，M中非零元素有：

M矩陣的其余元素均為0。

建立好系統狀態模型后，再根據 Kalman濾波的遞推算法對狀態進行估計。Kalman濾波遞推算法可參考文獻[11][12]。

5 實驗驗證

在清華大學內進行了實車實驗。車輛上安裝OxTS RT3100、MEMS慣導(Invensense mpu6050)以及bu353 GPS。RT3100是一款高精度GPS/IMU組合導航產品；mpu6050是低成本MEMS慣導，內置加速度計和陀螺儀，其價格約為3美元；bu353是一款低成本GPS，其價格約為30美元。RT3100和mpu6050的數據輸出頻率均設為20 Hz。各設備均安裝于車輛后輪軸中間上方。RT3100輸出的行車路線如圖4所示。

RT3100內建高精度慣導，能夠輸出高精度位置、速度、姿態、加速度和角速度等數據。取一段RT3100的輸出數據進行分析，該段數據采集時間為500 s，共有10 000個數據點。向心加速度的觀測值及載體坐標系y軸速度與z軸角速度乘積的觀測值如圖5所示。

圖4 車輛行駛路線Fig.4 Travel route of vehicle

圖5 向心加速度觀測值及載體坐標系y軸速度與z軸角速度乘積觀測值Fig.5 Centripetal acceleration vs. y-axis velocity multiplied by z-axis angular velocity in body frame

從圖4中S點位置時開始通過MEMS慣導導航定位，終點為E點。在車輛靜止時測量得到mpu6050的各軸加速度和角速度零偏，以bu353在S點輸出的位置和速度為初始值進行推算。通過以下3種計算方式進行定位：1）使用慣導數據直接推算；2）使用載體坐標系x軸、z軸方向上的速度誤差作為觀測量進行kalman濾波，稱其為濾波 1；3）使用載體坐標系 x軸、z軸方向上的速度誤差以及向心加速度差值的誤差作為觀測量進行Kalman濾波，稱其為濾波2。

三種計算方式的定位結果如圖 6。此間車輛行駛200 s，行駛總距離為1429.53 m，三種計算方式的最后定位誤差如表1所示。

通過慣導輸出數據直接推算定位在前10 s具有較好的準確度，然而由于低成本MEMS慣導測量誤差較大，10 s之后出現了快速的發散，200 s時定位誤差率達到75.46%。相對于直接推算，通過Kalman濾波進行定位的結果在200 s內準確性較好。濾波1的定位誤差率為9.47%。增加了向心加速度差值的誤差為觀測量的濾波2定位誤差率降為3.86%，定位精度有較大提高。

圖6 3種計算方式的定位結果Fig.6 Positioning results of 3 kinds of calculations

表1 3種計算方式的最后定位誤差Tab.1 Final positioning errors of 3 kinds of calculations

6 結 論

由于車輛行駛的區域可能被嚴重遮擋，車載GPS不能有效定位，此時通過慣導來進行車輛定位是一種較好的技術方法。從經濟性方面考慮，低成本MEMS慣導可以普及應用于民用車輛。然而低成本MEMS慣導測量誤差大，將其輸出的原始數據直接用于導航定位，定位誤差會隨著時間增加而快速累加，難以直接用于車輛的定位導航。

針對此問題，本文研究了一種新式的MEMS慣導導航定位Kalman濾波算法。實車實驗結果表明，使用本文所述方法后，定位精度有較大提高，取得了良好的效果。本文所提出的向心加速度差值誤差這一觀測量是通過慣導自身的輸出計算得來，因而當慣導與GPS等其他定位方式組合用于對車輛導航時，其仍然可以作為觀測量，將有助于進一步提高定位精度。

（References）：

[1] 卡普蘭，赫加蒂. GPS原理與應用(第二版) [M]. 寇艷紅，譯. 2版，北京：電子工業出版社，2007：39.

[2] Angrisano A, Petovello M, Pugliano G. Benefits of combined GPS/GLONASS with low-cost mEMS IMUs for vehicular urban navigation[J]. Sensors, 2012, 12(4): 5134-5158.

[3] Badri A E, Sinha J K, Albarbar A. A typical filter design to improve the measured signals from MEMS accelerometer [J]. Measurement, 2010, 43(10): 1425-1430.

[4] 謝鋼. GPS原理與接收機設計[M]. 北京：電子工業出版社，2009：201-207.

[5] Noureldin A, El-Shafie A, Bayoumi M. GPS/INS integration utilizing dynamic neural networks for vehicular navigation[J]. Information Fusion, 2010, 12(1): 48-57.

[6] Sukkarieh S. Low cost, high integrity, aided inertial navigation systems for autonomous land vehicles[D]. Sydney：University of Sydney, 2000.

[7] 格里森，加布雷格齊亞布澤爾. GNSS應用與方法[M].楊東凱，樊江濱，張波，張敏，譯. 北京：電子工業出版社，2011：108.

[8] 羅建軍，馬衛華，袁建平，岳曉奎. 組合導航原理與應用[M].西安：西北工業大學出版社，2012：68, 142.

[9] Bouron P, Crubille P, Meizel D. Data fusion of four ABS sensors and GPS for an enhanced localization of car-like vehicles[C]//2001 ICRA IEEE International Conference on Robotics and Automation. Seoul, Korea: 2001: 1597-1602.

[10] 米奇克，瓦倫托維茲. 汽車動力學[M]. 陳三蔭，余強，譯. 4版. 北京：清華大學出版社，2009：475.

[11] Simon D. Kalman filtering with state constraints: a survey of linear and nonlinear algorithms[J]. IET Control Theory & Applications, 2009, 4(8): 1303-1318

[12] 秦永元，張洪鉞，汪叔華. 卡爾曼濾波與組合導航原理[M]. 西安：西北工業大學出版社，2012：33-35.

低成本車載MEMS慣導導航定位方法

李博文，姚丹亞

Low-cost MEMS IMU navigation positioning method for land vehicle

LI Bo-wen, YAO Dan-ya
(Department of Automation, Tsinghua University, Beijing 100084, China)

When vehicle GPS has no positioning output due to GPS satellite signal’s being blocked, a positioning method based on low-cost MEMS IMU can be used to compensate the vehicle navigation. However, the measurement error of the low-cost MEMS IMU is large, and its positioning error would accumulate quickly. To solve this problem, a Kalman filtering algorithm for MEMS IMU navigation was studied. By analyzing the motion characteristics of vehicle, a new observation, i.e. the error of the centripetal acceleration difference, was presented. Then the state transition matrix and observation matrix of the error state system model were deduced. The real vehicle experiment for 200 s shows that the positioning error rate of autonomous MEMS IMU is 75.27%, and the positioning error rate by this new method is 3.86%, which show that the positioning accuracy has been significantly improved.

MEMS IMU; vehicle navigation; positioning; Kalman filter; centripetal acceleration; observation

1005-6734(2014)06-0719-05

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

TP23

A

2014-7-14；

2014-11-14

國家高技術研究發展計劃（863計劃）（2011AA110401）

李博文（1986—），男，博士研究生，研究方向智能交通系統。E-mail：libowen20@126.com

聯 系 人：姚丹亞（1966—），男，教授，博士生導師。E-mail：yaody@tsinghua.edu.cn