許建新，熊 智，劉建業(yè)，黃 欣，張 苗

（南京航空航天大學(xué) 導(dǎo)航研究中心，南京 211106）

基于車輛運(yùn)動(dòng)模型輔助的智能手機(jī)平臺(tái)車載組合導(dǎo)航算法

許建新，熊 智，劉建業(yè)，黃 欣，張 苗

（南京航空航天大學(xué) 導(dǎo)航研究中心，南京 211106）

隨著智能手機(jī)硬件性能的提升以及MEMS傳感器技術(shù)的發(fā)展和應(yīng)用普及，現(xiàn)有中高端智能手機(jī)平臺(tái)中大多安裝有消費(fèi)級的微慣性測量單元和GPS接收機(jī)模塊，將這兩者相互結(jié)合，利用一定的信息融合方法，即可實(shí)現(xiàn)連續(xù)的車載導(dǎo)航定位。在智能手機(jī)平臺(tái)中所采用的消費(fèi)級MEMS慣性傳感器模塊其精度很低，通常難以滿足車載DR導(dǎo)航算法的性能需求；同時(shí)在車輛行駛過程中，不可避免會(huì)經(jīng)過高架、隧道等路徑，也會(huì)導(dǎo)致GPS信號受到遮擋和屏蔽從而無法定位。針對以上問題，設(shè)計(jì)了適用于智能手機(jī)平臺(tái)的基于車輛運(yùn)動(dòng)模型輔助的車載DR/GPS組合導(dǎo)航方案，推導(dǎo)建立了基于車輛側(cè)向速度約束的組合導(dǎo)航算法模型，并以智能手機(jī)作為驗(yàn)證平臺(tái)，測試驗(yàn)證了所設(shè)計(jì)的算法在無GPS環(huán)境下，采用智能手機(jī)平臺(tái)中的消費(fèi)級MEMS慣性測量組件，仍然可以在短時(shí)間內(nèi)維持較高的導(dǎo)航定位性能。

車載導(dǎo)航；車輛運(yùn)動(dòng)模型；組合導(dǎo)航；卡爾曼濾波

車載導(dǎo)航定位技術(shù)是智能交通系統(tǒng)的一個(gè)重要組成部分，也是關(guān)鍵技術(shù)之一[1]。車載導(dǎo)航要求能夠連續(xù)、實(shí)時(shí)、準(zhǔn)確地確定車輛的位置，目前可用于車載導(dǎo)航技術(shù)的導(dǎo)航傳感器和導(dǎo)航方式有衛(wèi)星導(dǎo)航（如GPS、北斗）、里程計(jì)、MEMS傳感器、磁傳感器、地圖匹配等[2-3]。衛(wèi)星導(dǎo)航定位精度較高，技術(shù)成熟，使用方便，但是它容易受到高樓、高架、隧道、地下車庫等使用環(huán)境的遮蔽影響而失效[4-5]；而以里程計(jì)、MEMS傳感器進(jìn)行導(dǎo)航定位的航位推算算法（Dead-reckoning, DR）是一種低成本、自主的車載導(dǎo)航定位方式，它的優(yōu)點(diǎn)是抗干擾能力強(qiáng)，能根據(jù)提供的傳感器數(shù)據(jù)，在短時(shí)間內(nèi)提供車輛較高精度的導(dǎo)航參數(shù)，但是DR導(dǎo)航算法的誤差隨時(shí)間積累，不適合長時(shí)間的單獨(dú)導(dǎo)航[6]；地圖匹配算法主要作為一種輔助導(dǎo)航手段，結(jié)合數(shù)字地圖中道路路線情況對導(dǎo)航結(jié)果進(jìn)行輔助修正，以提高定位精度，但是需要預(yù)存地圖信息，并需要定時(shí)更新，在沒有地圖數(shù)據(jù)的情況下該方法受到限制[7]。

車載導(dǎo)航儀成本昂貴，而隨著智能手機(jī)的硬件性能提升及應(yīng)用普及，采用智能手機(jī)內(nèi)置的GPS接收機(jī)模塊實(shí)現(xiàn)車載導(dǎo)航也得到了廣泛使用，但其在應(yīng)用中由于車輛不可避免會(huì)經(jīng)過高架、隧道等運(yùn)行路徑，導(dǎo)致GPS信號受到遮擋和屏蔽從而無法定位[8]。為提高車載導(dǎo)航的連續(xù)性和可靠性，利用智能手機(jī)平臺(tái)中的消費(fèi)級微慣性測量單元和 GPS接收機(jī)模塊組合實(shí)現(xiàn)車載定位近年來也得到了逐步的重視，但在智能手機(jī)平臺(tái)中所采用的消費(fèi)級 MEMS慣性傳感器模塊其精度很低，通常難以滿足車載DR導(dǎo)航算法的性能需求。

為此，本文基于智能手機(jī)平臺(tái)提出一種針對車載導(dǎo)航中以運(yùn)動(dòng)模型作為輔助的 DR/衛(wèi)星組合導(dǎo)航方法，在衛(wèi)星信號受到遮擋無輸出時(shí)，以車輛橫側(cè)向速度約束作為虛擬觀測量，進(jìn)行DR/車輛運(yùn)動(dòng)模型組合。在航向上，引入磁傳感器數(shù)據(jù)，計(jì)算磁航向角并進(jìn)行誤差補(bǔ)償修正，提高航向精度[9-10]，從而實(shí)現(xiàn)車輛的連續(xù)精確定位。

1 基于車輛運(yùn)動(dòng)模型輔助的智能手機(jī)平臺(tái)車載導(dǎo)航總體方案設(shè)計(jì)

車載導(dǎo)航要求能夠連續(xù)、實(shí)時(shí)、準(zhǔn)確地確定車輛的位置，而以里程計(jì)、MEMS傳感器進(jìn)行導(dǎo)航定位的航位推算算法抗干擾能力強(qiáng)，能在短時(shí)間內(nèi)提供車輛較精確的導(dǎo)航參數(shù)。但是DR導(dǎo)航算法的誤差隨時(shí)間積累，特別是采用智能手機(jī)平臺(tái)中的低成本消費(fèi)級MEMS慣性器件，如果沒有GPS等外界輔助信息修正的情況下，在很短的時(shí)間內(nèi)導(dǎo)航誤差就急劇發(fā)散，無法適用于車載導(dǎo)航的性能需求。因此，迫切需要研究適用于智能手機(jī)平臺(tái)中的低成本消費(fèi)級 MEMS慣性器件的車載組合導(dǎo)航方法。

車輛在平整道路上行駛時(shí)，不考慮側(cè)滑等運(yùn)動(dòng)，一般在垂直于車輛前進(jìn)方向的側(cè)向不會(huì)有運(yùn)動(dòng)速度，這樣在無GPS觀測信息的條件下，車輛側(cè)向速度近似為零就構(gòu)成了天然的組合量測方程[11]。基于上述的分析，本文設(shè)計(jì)了基于車輛運(yùn)動(dòng)模型輔助的智能手機(jī)平臺(tái)車載導(dǎo)航總體方案，如圖1所示。

圖1 基于車輛運(yùn)動(dòng)模型輔助的DR/衛(wèi)星組合導(dǎo)航方案圖Fig.1 Integrated navigation scheme based on vehicle kinematical model

在圖1中，通過MEMS傳感器輸出積分后獲得速度和位置信息，在衛(wèi)星信號有效的情況下，通過卡爾曼濾波算法進(jìn)行速度、位置組合，獲得修正量，對導(dǎo)航信息進(jìn)行修正。在衛(wèi)星信號失效時(shí)，根據(jù)車輛運(yùn)動(dòng)模型，設(shè)計(jì)虛擬的橫向速度約束模型，進(jìn)行組合。車輛的航向信息可由磁航向角進(jìn)行輔助修正。

圖2給出了智能手機(jī)平臺(tái)進(jìn)行車載導(dǎo)航定位時(shí)的一種安裝方式。在車載導(dǎo)航中，大多用手機(jī)支架將其固定于汽車出風(fēng)口或前擋風(fēng)玻璃處。如圖2所示，豎直安裝時(shí)，可選擇前向和側(cè)向加速度計(jì)輸出敏感加減速運(yùn)動(dòng)，選擇豎直方向陀螺輸出敏感轉(zhuǎn)彎運(yùn)動(dòng)。

圖2 手機(jī)安裝模式Fig.2 Installation mode of mobile phone

2 基于車輛運(yùn)動(dòng)模型輔助的車載組合導(dǎo)航算法設(shè)計(jì)

2.1 車載DR航位推算模型建立

2.1.1 基本的DR推算算法模型

考慮到車輛主要在水平路面上運(yùn)動(dòng)，因此以圖 2中的安裝方式，導(dǎo)航坐標(biāo)系選擇東北天坐標(biāo)系，載體坐標(biāo)系選為右前上坐標(biāo)系，建立相應(yīng)的DR航位推算算法模型如下。

航向角計(jì)算：

其中，kΦ為當(dāng)前時(shí)刻航向角，Φk-1為前一時(shí)刻航向角，T為傳感器采樣時(shí)間間隔，ωz,k為當(dāng)前時(shí)刻誤差修正后的豎直方向陀螺輸出。初始航向角由磁航向計(jì)算獲得。

速度位置計(jì)算：

其中， fe,k和 fn,k分別為東向和北向加速度投影， fx,k和 fy,k分別為誤差修正后的側(cè)向和前向加速度計(jì)輸出。由此可推算出東向和北向速度ve,k和vn,k：

緯度Lk和經(jīng)度kλ位置推算為：

其中，Rm和Rn分別為子午圈半徑和卯酉圈半徑，初始速度和位置由GPS提供。

2.1.2 手機(jī)平臺(tái)安裝不水平的處理方法

理想情況下，手機(jī)豎直安裝后的俯仰角和橫滾角應(yīng)為零度，但在實(shí)際安裝時(shí)，由于不能保證完全豎直，手機(jī)平臺(tái)與水平面之間會(huì)存在安裝誤差角，因此水平方向加速度計(jì)輸出會(huì)引入重力方向投影分量值，如果在DR推算過程中不加以處理，會(huì)引起DR推算誤差。為消除該誤差，本文設(shè)計(jì)了一種基于水平姿態(tài)投影的處理方法，可以將不水平安裝的加速度計(jì)測量輸出轉(zhuǎn)換為水平測量輸出，從而消除手機(jī)平臺(tái)安裝不水平時(shí)所帶來的DR推算誤差。

采用靜止時(shí)的加速度計(jì)輸出根據(jù)式(5)和式(6)計(jì)算出安裝誤差角，在DR推算過程中再根據(jù)式(7)將加速度計(jì)實(shí)時(shí)投影到水平面上，以消除安裝誤差帶來的重力分量影響。

俯仰角誤差：

橫滾角誤差：

加速度計(jì)投影：

其中，C1和C2為姿態(tài)轉(zhuǎn)換矩陣，

2.2 組合導(dǎo)航算法模型建立

2.2.1 狀態(tài)建模

根據(jù)2.1節(jié)建立的DR算法模型，選擇DR推算輸出的航向角誤差、水平二維速度誤差、水平二維位置誤差為基本狀態(tài)量，將天向軸陀螺誤差和水平加速度誤差也作為誤差狀態(tài)量進(jìn)行建模，由此建立狀態(tài)方程。

系統(tǒng)狀態(tài)量：

系統(tǒng)狀態(tài)方程：

式中，A為狀態(tài)轉(zhuǎn)移矩陣，G為系統(tǒng)噪聲矩陣，W為白噪聲隨機(jī)誤差矢量。根據(jù)DR推算算法模型，可推導(dǎo)出各個(gè)矩陣的表達(dá)式：

2.2.2 量測建模

1）DR/GPS組合量測建模

在衛(wèi)星信號有效時(shí)，選取經(jīng)度、緯度、東向速度和北向速度作為觀測量，量測方程為[12]

其中，HSat為觀測矩陣，VSat為觀測噪聲，

2）基于側(cè)向速度約束的DR組合量測建模

在衛(wèi)星信號受到遮擋無輸出時(shí)，需要建立DR/車輛運(yùn)動(dòng)模型組合導(dǎo)航算法模型。車輛在平整道路上行駛時(shí)，不考慮側(cè)滑等運(yùn)動(dòng)，一般在垂直于車輛前進(jìn)方向的側(cè)向方向上不會(huì)有運(yùn)動(dòng)速度，這樣在無GPS觀測信息的條件下，車輛側(cè)向速度近似為零就構(gòu)成了天然的組合量測方程。

根據(jù)DR推算輸出的東向和北向速度，計(jì)算出車輛橫側(cè)向速度參數(shù)為

將 DR推算獲得的橫側(cè)向速度進(jìn)行小量化展開，獲得線性化的量測方程：

其中，vx為橫側(cè)向運(yùn)動(dòng)速度真實(shí)值。

在實(shí)際車輛運(yùn)動(dòng)情況下，考慮到路面不平整以及轉(zhuǎn)彎側(cè)滑等因素，則橫向速度約束方程為

式中，δvx表示車輛運(yùn)動(dòng)時(shí)由于側(cè)滑所引起的附加橫向速度變化，建模為白噪聲，其取值大小和車輛的具體運(yùn)動(dòng)特性有關(guān)。

從而可以構(gòu)造橫向速度虛擬量測方程為

其中，量測矩陣為

量測噪聲為

3 測試數(shù)據(jù)分析

本論文以華為 Mate7智能手機(jī)型號作為測試平臺(tái)，通過測試靜態(tài)傳感器數(shù)據(jù)，分析確定組合導(dǎo)航卡爾曼濾波參數(shù)，并在此基礎(chǔ)上通過實(shí)際跑車實(shí)驗(yàn)，設(shè)計(jì)行車路線，經(jīng)過高架、隧道等環(huán)境，獲得不同條件下的傳感器數(shù)據(jù)。最后應(yīng)用本論文提出的算法進(jìn)行仿真計(jì)算得到導(dǎo)航結(jié)果，并分析誤差精度。

3.1 組合導(dǎo)航濾波參數(shù)確定

手機(jī)中安裝的MEMS器件為低精度傳感器，因此在使用傳感器數(shù)據(jù)之前必須對誤差進(jìn)行標(biāo)定獲得相應(yīng)的模型參數(shù)[13]。在實(shí)驗(yàn)室條件下，將手機(jī)置于水平轉(zhuǎn)臺(tái)上，測試獲得靜態(tài)數(shù)據(jù)，數(shù)據(jù)采樣頻率為50 Hz。

圖3和圖4所示分別為三軸陀螺和加速度計(jì)的原始數(shù)據(jù)輸出。從圖3和圖4可以看出，陀螺的零偏較大，可以達(dá)到上千度每小時(shí)，并且陀螺零偏隨時(shí)間漂移，需要對零偏數(shù)據(jù)進(jìn)行扣除。加速度計(jì)輸出相對較為穩(wěn)定，精度較高。

圖3 三軸陀螺輸出信息Fig.3 Output of three-axis gyros

圖4 三軸加速度計(jì)輸出信息Fig.4 Output of three-axis accelerometers

根據(jù)測得的靜態(tài)數(shù)據(jù)，當(dāng)傳感器輸出穩(wěn)定后，對原始數(shù)據(jù)進(jìn)行百十秒平均分析，并對均值和標(biāo)準(zhǔn)差進(jìn)行統(tǒng)計(jì)，得到表1所示結(jié)果。

根據(jù)表1所得的靜態(tài)數(shù)據(jù)并結(jié)合導(dǎo)航測試結(jié)果，在卡爾曼濾波參數(shù)設(shè)計(jì)上，陀螺隨機(jī)常數(shù)誤差和一階馬爾科夫誤差設(shè)定為1500 (°)/h，加速度計(jì)一階馬爾科夫誤差設(shè)定為0.03 m/s2。

量測方程中，GPS東向和北向位置誤差均設(shè)為5 m，東向和北向速度誤差分別設(shè)為0.5 m/s，橫向速度約束方程量測誤差根據(jù)經(jīng)驗(yàn)值設(shè)定為0.5 m/s。

表1 IMU靜態(tài)數(shù)據(jù)分析Tab.1 Analysis of IMU static data

3.2 基于車輛運(yùn)動(dòng)模型輔助的組合導(dǎo)航算法性能測試

根據(jù)實(shí)際跑車實(shí)驗(yàn)，采集傳感器數(shù)據(jù)，圖5為測試的一組跑車航跡路線圖，測試時(shí)間約為1200 s。

在時(shí)間點(diǎn)250 s、770 s、1090 s分別將GPS設(shè)定為無效，時(shí)間長度為60 s。以GPS導(dǎo)航結(jié)果作為參照，圖6和圖7為所得導(dǎo)航結(jié)果與GPS信息的差值曲線。

圖5 跑車航跡路線圖Fig.5 Trajectory of the test

圖6 速度誤差曲線Fig.6 Velocity error curves

圖7 位置誤差曲線Fig.7 Position error curves

根據(jù)以上測試實(shí)驗(yàn)，對每段的誤差 RMS數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分析，得到表2所示的結(jié)果。從表2中數(shù)據(jù)可以看到，采用本文提出的橫向速度約束車載導(dǎo)航方案，可以很大程度上提高在GPS信號丟失后的定位精度，實(shí)現(xiàn)車載導(dǎo)航的連續(xù)定位。

表2 GPS失效情況下的導(dǎo)航數(shù)據(jù)誤差統(tǒng)計(jì)Tab.2 S tatistic of positioning errors with GPS outages

4 結(jié) 論

本文基于智能手機(jī)平臺(tái)，提出了一種針對車載導(dǎo)航中以車輛運(yùn)動(dòng)模型作為輔助的車載 DR組合導(dǎo)航方法，設(shè)計(jì)了基于車輛運(yùn)動(dòng)模型輔助的車載DR組合導(dǎo)航方案。在GPS信號丟失環(huán)境下，通過對低成本消費(fèi)級MEMS傳感器輸出進(jìn)行誤差修正，以車輛橫側(cè)向速度約束作為虛擬觀測量，進(jìn)行DR/車輛運(yùn)動(dòng)模型組合導(dǎo)航。此外，本文還以華為Mate7智能手機(jī)型號作為測試平臺(tái)，經(jīng)過實(shí)際跑車數(shù)據(jù)實(shí)驗(yàn)采集數(shù)據(jù)對所提出的算法進(jìn)行驗(yàn)證，從導(dǎo)航對比結(jié)果可以看出該方法能顯著提高GPS失效環(huán)境下的導(dǎo)航精度，抑制DR推算的誤差發(fā)散。

（References）：

[1] Jeralovics V, Levinskis A. The analysis of Kalman filtering algorithm for land vehicle navigation[C]//14th Biennial Baltic Electronics Conference. 2014: 53-56.

[2] Jafri S R U N, Jafri S M U N, Shakeel S Z. Improved path planning and controlling for a low cost navigation solution of unmanned land vehicle[C]//International Conference on Computer Modelling and Simulation, 2009: 14-18.

[3] 李磊磊, 陳家斌, 楊黎明, 等. 基于道路信息的智能地圖匹配算法[J]. 中國慣性技術(shù)學(xué)報(bào), 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.

[4] 張光勝, 李任之, 程鋼. GPS多路徑效應(yīng)實(shí)驗(yàn)測試與分析研究[J]. 測繪工程, 2014, 23(12): 31-34. Zhang Guang-sheng, Li Ren-zhi, Cheng Gang. Test and analysis on the multipath effects of GPS observation[J]. Engineering of Surveying and Mapping, 2014, 23(12): 31-34.

[5] Xu Q, Li X, Li B, et al. A reliable hybrid positioning methodology for land vehicles using low-cost sensors[J]. IEEE Transactions on Intelligent Transportation Systems, 2016, 17(3): 834-847.

[6] 賈繼超, 吳訓(xùn)忠, 夏家和. 一種車輛航位推算改進(jìn)算法[J]. 中國慣性技術(shù)學(xué)報(bào), 2009, 17(4): 453-455. Jia Ji-chao, Wu Xun-zhong, Xia Jia-he. Improved method for vehicular dead reckoning[J]. Journal of Chinese Inertial Technology, 2009, 17(4): 453-455.

[7] Drevelle V, Bonnifait P. iGPS: Global positioning in urban canyons with road surface maps[J]. IEEE Intelligent Transportation Systems Magazine, 2012, 4(3): 6-18.

[8] Lin Hui-fei, Chen Chi-hua. Design and application of augmented reality query-answering system in mobile phone information navigation[J]. Expert Systems with Applications, 2015, 42(2): 810-820.

[9] 劉詩斌, 陳露蘭, 張韋. 一種基于磁傳感器的 MEMS陀螺標(biāo)定方法[J]. 傳感技術(shù)學(xué)報(bào), 2013(12): 1700-1703. Liu Shi-bin, Chen Lu-lan, Zhang Wei. A calibration method of MEMS gyroscope based on magnetic sensor [J]. Chinese Journal of Sensors and Actuators, 2013, 26 (12): 1700-1703.

[10] Alandry B, Latorre L, Mailly F, et al. A fully integrated inertial measurement unit: Application to attitude and heading determination[J]. IEEE Sensors Journal, 2011, 11 (11): 2852-2860.

[11] 付強(qiáng)文, 秦永元, 李四海, 等. 車輛運(yùn)動(dòng)學(xué)約束輔助的慣性導(dǎo)航算法[J]. 中國慣性技術(shù)學(xué)報(bào), 2012, 20(6): 640-643. Fu Qiang-wen, Qin Yong-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

[12] Xiong Zhi, Chen Ji-hui, Wang Rong. A new dynamic vector formed information sharing algorithm in federated filter[J]. Aerospace Science and Technology, 2013, 29(1): 37-46.

[13] Stebler Y, Guerrier S, Skaloud J. An approach for observing and modeling errors in MEMS-based inertial sensors under vehicle dynamic[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(11): 2926 - 2936.

Vehicle integrated navigation algorithm based on kinematical model for intelligent mobile phone platform

XU Jian-xin, XIONG Zhi, LIU Jian-ye, HUANG Xin, ZHANG Miao
(Navigation Research Center, Nanjing University of Aeronautics & Astronautics, Nanjing 211106, China)

With the improvement of smartphone hardware performance and the development of the MEMS sensor technology, most of the existing mid- and high-end smartphone platforms have installed the consumer micro inertial measurement unit. Combined with GPS receiver, it is easy to obtain the information of position and velocity and realize continuous positioning. Due to the low precision of the consumer MEMS inertial sensor module in the intelligent mobile phone platform, it is difficult to meet the requirements of the vehicle navigation algorithm. In addition, the vehicle will inevitably go through the elevated, tunnels or other specific path in the running process, thus the GPS signal could be blocked and cannot work under these circumstances. To solve these problems, a DR/GPS integrated navigation scheme applicable for smartphone platform is designed based on the vehicle kinematical model. The results of road tests show that the designed method can effectively restrain the error divergence of the pure DR algorithm in the absence of GPS positioning information, and realize continuous accurate positioning. This method has the important engineering application value.

vehicle navigation; vehicle kinematical model; integrated navigation; Kalman filter

U666.1

A

1005-6734(2017)02-0203-06

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

2017-01-04；

2017-03-28

國家自然科學(xué)基金項(xiàng)目（61533008, 61374115, 61533009, 61673208）；江蘇省“333工程”科研資助立項(xiàng)項(xiàng)目（BRA2016405）；留學(xué)人員擇優(yōu)資助項(xiàng)目（2016年）；江蘇省六大人才高峰資助項(xiàng)目（2013-JY-013）；航空科學(xué)基金項(xiàng)目（20165552043）；江蘇高校優(yōu)勢學(xué)科建設(shè)工程項(xiàng)目和江蘇省“物聯(lián)網(wǎng)與控制技術(shù)”重點(diǎn)實(shí)驗(yàn)室基金資助; 中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金資助（NS2017016, NZ2016104）；江蘇省普通高校研究生科研創(chuàng)新計(jì)劃項(xiàng)目（KYLX15_0264）

許建新（1990—），男，博士生，從事組合導(dǎo)航技術(shù)研究。Email: xujianxin@nuaa.edu.cn