低成本車載MEMS慣導(dǎo)導(dǎo)航定位方法

（清華大學(xué) 自動(dòng)化系，北京 100084）

（清華大學(xué) 自動(dòng)化系，北京 100084）

當(dāng)GPS衛(wèi)星信號(hào)受到遮擋，車載GPS無(wú)定位輸出時(shí)，通過(guò)低成本MEMS慣導(dǎo)進(jìn)行定位是車輛導(dǎo)航的一種彌補(bǔ)方法。然而低成本MEMS慣導(dǎo)測(cè)量誤差大，定位誤差會(huì)快速累積。針對(duì)此問(wèn)題，研究了一種應(yīng)用于MEMS慣導(dǎo)導(dǎo)航定位的Kalman濾波算法。通過(guò)分析車輛的運(yùn)動(dòng)特性，在已有的研究基礎(chǔ)上，提出了向心加速度差值誤差這一新型觀測(cè)量，并推導(dǎo)了誤差狀態(tài)系統(tǒng)模型的狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣。200 s時(shí)長(zhǎng)的實(shí)車實(shí)驗(yàn)表明，單純MEMS慣導(dǎo)定位的誤差率是75.27%，而所述新方法的定位誤差率是3.86%，定位精度有了大幅度提高，取得了良好的效果。

MEMS慣導(dǎo)；車載導(dǎo)航；定位；Kalman濾波；向心加速度；觀測(cè)量

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

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

1 定位算法

對(duì)于慣性導(dǎo)航系統(tǒng)，載體的姿態(tài)、速度和位置的變化率可以通過(guò)以下方程計(jì)算得到[7]：

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

其中 RM、RN分別為當(dāng)?shù)刈游缛兔先Φ那拾霃健?/p>

當(dāng)初始導(dǎo)航參數(shù)已知，通過(guò)加速度計(jì)和陀螺儀可以獲取 fb和，根據(jù)式(1)～(3)計(jì)算姿態(tài)、速度和位置的變化率后，再通過(guò)積分運(yùn)算就可以計(jì)算得到載體的當(dāng)前姿態(tài)、速度和位置。

2 誤差傳播

通過(guò)慣導(dǎo)輸出的加速度和角速度數(shù)據(jù)來(lái)直接計(jì)算得到的姿態(tài)、速度和位置都含有誤差[8]。計(jì)算值和真實(shí)值的關(guān)系如下：

在導(dǎo)航坐標(biāo)系下慣導(dǎo)誤差傳播方程如下[8]：

式中， εg、 εa分別是陀螺儀和加速度計(jì)的測(cè)量誤差，它們可以簡(jiǎn)化看作是一階馬爾科夫過(guò)程：

式中，τg、τa分別是 εg、εa的時(shí)間相關(guān)常數(shù)，wg、wa都是白噪聲。

3 觀測(cè)量

3.1 已有觀測(cè)量

通常GPS/INS組合導(dǎo)航的觀測(cè)量為GPS輸出值和慣導(dǎo)計(jì)算值之間的差值，包括位置差和速度差。然而當(dāng)GPS無(wú)有效輸出，只通過(guò)慣導(dǎo)定位時(shí)，獲取不了這些觀測(cè)量。對(duì)于車輛運(yùn)動(dòng)，可以假設(shè)車輛不會(huì)發(fā)生側(cè)滑，并且不會(huì)離開地面。設(shè)車輛載體坐標(biāo)系如圖1所示。

圖1 車輛載體坐標(biāo)系Fig.1 Vehicle body frame coordinates

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

3.2 新觀測(cè)量

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

圖2 車輛圓周運(yùn)動(dòng)示意圖Fig.2 Schematic diagram of vehicle circular motion

假設(shè)車輛在很短的時(shí)間dt內(nèi)從A點(diǎn)移動(dòng)到 A′點(diǎn)，移動(dòng)的距離為dl，速度方向變化的角度為dφ，如圖3所示。

圖3 圓周運(yùn)動(dòng)細(xì)節(jié)分析Fig.3 Detail analysis of vehicle circular motion

根據(jù)圓周運(yùn)動(dòng)定律，可以推導(dǎo)得到向心加速度等于速度乘以角速度：

式中， ac為向心加速度。

設(shè)J為向心加速度差值，其通過(guò)下式計(jì)算：

根據(jù)方程(17)可得：

因?yàn)榈厍蜃赞D(zhuǎn)角速度和車輛線運(yùn)動(dòng)形成的相對(duì)于地球的旋轉(zhuǎn)角速度相對(duì)較小，本文中將這兩項(xiàng)角速度忽略，因而：

4 誤差狀態(tài)系統(tǒng)模型

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

式中， g0是當(dāng)?shù)刂亓铀俣却笮　?/p>

則可以建立誤差狀態(tài)系統(tǒng)模型，誤差狀態(tài)向量包含15個(gè)參數(shù)。系統(tǒng)誤差向量X設(shè)為：

設(shè)離散化后的系統(tǒng)狀態(tài)模型如下：

式中，Φk,k-1是15× 15維的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣； Hk是3× 15維的觀測(cè)矩陣； Wk-1是過(guò)程噪聲向量； Vk是觀測(cè)噪聲向量。設(shè)觀測(cè)矩陣 Hk為：

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

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

建立好系統(tǒng)狀態(tài)模型后，再根據(jù) Kalman濾波的遞推算法對(duì)狀態(tài)進(jìn)行估計(jì)。Kalman濾波遞推算法可參考文獻(xiàn)[11][12]。

5 實(shí)驗(yàn)驗(yàn)證

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

RT3100內(nèi)建高精度慣導(dǎo)，能夠輸出高精度位置、速度、姿態(tài)、加速度和角速度等數(shù)據(jù)。取一段RT3100的輸出數(shù)據(jù)進(jìn)行分析，該段數(shù)據(jù)采集時(shí)間為500 s，共有10 000個(gè)數(shù)據(jù)點(diǎn)。向心加速度的觀測(cè)值及載體坐標(biāo)系y軸速度與z軸角速度乘積的觀測(cè)值如圖5所示。

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

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

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

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

通過(guò)慣導(dǎo)輸出數(shù)據(jù)直接推算定位在前10 s具有較好的準(zhǔn)確度，然而由于低成本MEMS慣導(dǎo)測(cè)量誤差較大，10 s之后出現(xiàn)了快速的發(fā)散，200 s時(shí)定位誤差率達(dá)到75.46%。相對(duì)于直接推算，通過(guò)Kalman濾波進(jìn)行定位的結(jié)果在200 s內(nèi)準(zhǔn)確性較好。濾波1的定位誤差率為9.47%。增加了向心加速度差值的誤差為觀測(cè)量的濾波2定位誤差率降為3.86%，定位精度有較大提高。

圖6 3種計(jì)算方式的定位結(jié)果Fig.6 Positioning results of 3 kinds of calculations

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

6 結(jié) 論

由于車輛行駛的區(qū)域可能被嚴(yán)重遮擋，車載GPS不能有效定位，此時(shí)通過(guò)慣導(dǎo)來(lái)進(jìn)行車輛定位是一種較好的技術(shù)方法。從經(jīng)濟(jì)性方面考慮，低成本MEMS慣導(dǎo)可以普及應(yīng)用于民用車輛。然而低成本MEMS慣導(dǎo)測(cè)量誤差大，將其輸出的原始數(shù)據(jù)直接用于導(dǎo)航定位，定位誤差會(huì)隨著時(shí)間增加而快速累加，難以直接用于車輛的定位導(dǎo)航。

針對(duì)此問(wèn)題，本文研究了一種新式的MEMS慣導(dǎo)導(dǎo)航定位Kalman濾波算法。實(shí)車實(shí)驗(yàn)結(jié)果表明，使用本文所述方法后，定位精度有較大提高，取得了良好的效果。本文所提出的向心加速度差值誤差這一觀測(cè)量是通過(guò)慣導(dǎo)自身的輸出計(jì)算得來(lái)，因而當(dāng)慣導(dǎo)與GPS等其他定位方式組合用于對(duì)車輛導(dǎo)航時(shí)，其仍然可以作為觀測(cè)量，將有助于進(jìn)一步提高定位精度。

（References）：

[1] 卡普蘭，赫加蒂. GPS原理與應(yīng)用(第二版) [M]. 寇艷紅，譯. 2版，北京：電子工業(yè)出版社，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原理與接收機(jī)設(shè)計(jì)[M]. 北京：電子工業(yè)出版社，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應(yīng)用與方法[M].楊東凱，樊江濱，張波，張敏，譯. 北京：電子工業(yè)出版社，2011：108.

[8] 羅建軍，馬衛(wèi)華，袁建平，岳曉奎. 組合導(dǎo)航原理與應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，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] 米奇克，瓦倫托維茲. 汽車動(dòng)力學(xué)[M]. 陳三蔭，余強(qiáng)，譯. 4版. 北京：清華大學(xué)出版社，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] 秦永元，張洪鉞，汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安：西北工業(yè)大學(xué)出版社，2012：33-35.

低成本車載MEMS慣導(dǎo)導(dǎo)航定位方法

李博文，姚丹亞

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

國(guó)家高技術(shù)研究發(fā)展計(jì)劃（863計(jì)劃）（2011AA110401）

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

聯(lián) 系 人：姚丹亞（1966—），男，教授，博士生導(dǎo)師。E-mail：yaody@tsinghua.edu.cn