基于车辆运动模型辅助的智能手机平台车载组合导航算法

许建新，熊 智，刘建业，黄 欣，张 苗

（南京航空航天大学 导航研究中心，南京 211106）

基于车辆运动模型辅助的智能手机平台车载组合导航算法

许建新，熊 智，刘建业，黄 欣，张 苗

（南京航空航天大学 导航研究中心，南京 211106）

随着智能手机硬件性能的提升以及MEMS传感器技术的发展和应用普及，现有中高端智能手机平台中大多安装有消费级的微惯性测量单元和GPS接收机模块，将这两者相互结合，利用一定的信息融合方法，即可实现连续的车载导航定位。在智能手机平台中所采用的消费级MEMS惯性传感器模块其精度很低，通常难以满足车载DR导航算法的性能需求；同时在车辆行驶过程中，不可避免会经过高架、隧道等路径，也会导致GPS信号受到遮挡和屏蔽从而无法定位。针对以上问题，设计了适用于智能手机平台的基于车辆运动模型辅助的车载DR/GPS组合导航方案，推导建立了基于车辆侧向速度约束的组合导航算法模型，并以智能手机作为验证平台，测试验证了所设计的算法在无GPS环境下，采用智能手机平台中的消费级MEMS惯性测量组件，仍然可以在短时间内维持较高的导航定位性能。

车载导航；车辆运动模型；组合导航；卡尔曼滤波

车载导航定位技术是智能交通系统的一个重要组成部分，也是关键技术之一[1]。车载导航要求能够连续、实时、准确地确定车辆的位置，目前可用于车载导航技术的导航传感器和导航方式有卫星导航（如GPS、北斗）、里程计、MEMS传感器、磁传感器、地图匹配等[2-3]。卫星导航定位精度较高，技术成熟，使用方便，但是它容易受到高楼、高架、隧道、地下车库等使用环境的遮蔽影响而失效[4-5]；而以里程计、MEMS传感器进行导航定位的航位推算算法（Dead-reckoning, DR）是一种低成本、自主的车载导航定位方式，它的优点是抗干扰能力强，能根据提供的传感器数据，在短时间内提供车辆较高精度的导航参数，但是DR导航算法的误差随时间积累，不适合长时间的单独导航[6]；地图匹配算法主要作为一种辅助导航手段，结合数字地图中道路路线情况对导航结果进行辅助修正，以提高定位精度，但是需要预存地图信息，并需要定时更新，在没有地图数据的情况下该方法受到限制[7]。

车载导航仪成本昂贵，而随着智能手机的硬件性能提升及应用普及，采用智能手机内置的GPS接收机模块实现车载导航也得到了广泛使用，但其在应用中由于车辆不可避免会经过高架、隧道等运行路径，导致GPS信号受到遮挡和屏蔽从而无法定位[8]。为提高车载导航的连续性和可靠性，利用智能手机平台中的消费级微惯性测量单元和 GPS接收机模块组合实现车载定位近年来也得到了逐步的重视，但在智能手机平台中所采用的消费级 MEMS惯性传感器模块其精度很低，通常难以满足车载DR导航算法的性能需求。

为此，本文基于智能手机平台提出一种针对车载导航中以运动模型作为辅助的 DR/卫星组合导航方法，在卫星信号受到遮挡无输出时，以车辆横侧向速度约束作为虚拟观测量，进行DR/车辆运动模型组合。在航向上，引入磁传感器数据，计算磁航向角并进行误差补偿修正，提高航向精度[9-10]，从而实现车辆的连续精确定位。

1 基于车辆运动模型辅助的智能手机平台车载导航总体方案设计

车载导航要求能够连续、实时、准确地确定车辆的位置，而以里程计、MEMS传感器进行导航定位的航位推算算法抗干扰能力强，能在短时间内提供车辆较精确的导航参数。但是DR导航算法的误差随时间积累，特别是采用智能手机平台中的低成本消费级MEMS惯性器件，如果没有GPS等外界辅助信息修正的情况下，在很短的时间内导航误差就急剧发散，无法适用于车载导航的性能需求。因此，迫切需要研究适用于智能手机平台中的低成本消费级 MEMS惯性器件的车载组合导航方法。

车辆在平整道路上行驶时，不考虑侧滑等运动，一般在垂直于车辆前进方向的侧向不会有运动速度，这样在无GPS观测信息的条件下，车辆侧向速度近似为零就构成了天然的组合量测方程[11]。基于上述的分析，本文设计了基于车辆运动模型辅助的智能手机平台车载导航总体方案，如图1所示。

图1 基于车辆运动模型辅助的DR/卫星组合导航方案图Fig.1 Integrated navigation scheme based on vehicle kinematical model

在图1中，通过MEMS传感器输出积分后获得速度和位置信息，在卫星信号有效的情况下，通过卡尔曼滤波算法进行速度、位置组合，获得修正量，对导航信息进行修正。在卫星信号失效时，根据车辆运动模型，设计虚拟的横向速度约束模型，进行组合。车辆的航向信息可由磁航向角进行辅助修正。

图2给出了智能手机平台进行车载导航定位时的一种安装方式。在车载导航中，大多用手机支架将其固定于汽车出风口或前挡风玻璃处。如图2所示，竖直安装时，可选择前向和侧向加速度计输出敏感加减速运动，选择竖直方向陀螺输出敏感转弯运动。

图2 手机安装模式Fig.2 Installation mode of mobile phone

2 基于车辆运动模型辅助的车载组合导航算法设计

2.1 车载DR航位推算模型建立

2.1.1 基本的DR推算算法模型

考虑到车辆主要在水平路面上运动，因此以图 2中的安装方式，导航坐标系选择东北天坐标系，载体坐标系选为右前上坐标系，建立相应的DR航位推算算法模型如下。

航向角计算：

其中，kΦ为当前时刻航向角，Φk-1为前一时刻航向角，T为传感器采样时间间隔，ωz,k为当前时刻误差修正后的竖直方向陀螺输出。初始航向角由磁航向计算获得。

速度位置计算：

其中， fe,k和 fn,k分别为东向和北向加速度投影， fx,k和 fy,k分别为误差修正后的侧向和前向加速度计输出。由此可推算出东向和北向速度ve,k和vn,k：

纬度Lk和经度kλ位置推算为：

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

2.1.2 手机平台安装不水平的处理方法

理想情况下，手机竖直安装后的俯仰角和横滚角应为零度，但在实际安装时，由于不能保证完全竖直，手机平台与水平面之间会存在安装误差角，因此水平方向加速度计输出会引入重力方向投影分量值，如果在DR推算过程中不加以处理，会引起DR推算误差。为消除该误差，本文设计了一种基于水平姿态投影的处理方法，可以将不水平安装的加速度计测量输出转换为水平测量输出，从而消除手机平台安装不水平时所带来的DR推算误差。

采用静止时的加速度计输出根据式(5)和式(6)计算出安装误差角，在DR推算过程中再根据式(7)将加速度计实时投影到水平面上，以消除安装误差带来的重力分量影响。

俯仰角误差：

横滚角误差：

加速度计投影：

其中，C1和C2为姿态转换矩阵，

2.2 组合导航算法模型建立

2.2.1 状态建模

根据2.1节建立的DR算法模型，选择DR推算输出的航向角误差、水平二维速度误差、水平二维位置误差为基本状态量，将天向轴陀螺误差和水平加速度误差也作为误差状态量进行建模，由此建立状态方程。

系统状态量：

系统状态方程：

式中，A为状态转移矩阵，G为系统噪声矩阵，W为白噪声随机误差矢量。根据DR推算算法模型，可推导出各个矩阵的表达式：

2.2.2 量测建模

1）DR/GPS组合量测建模

在卫星信号有效时，选取经度、纬度、东向速度和北向速度作为观测量，量测方程为[12]

其中，HSat为观测矩阵，VSat为观测噪声，

2）基于侧向速度约束的DR组合量测建模

在卫星信号受到遮挡无输出时，需要建立DR/车辆运动模型组合导航算法模型。车辆在平整道路上行驶时，不考虑侧滑等运动，一般在垂直于车辆前进方向的侧向方向上不会有运动速度，这样在无GPS观测信息的条件下，车辆侧向速度近似为零就构成了天然的组合量测方程。

根据DR推算输出的东向和北向速度，计算出车辆横侧向速度参数为

将 DR推算获得的横侧向速度进行小量化展开，获得线性化的量测方程：

其中，vx为横侧向运动速度真实值。

在实际车辆运动情况下，考虑到路面不平整以及转弯侧滑等因素，则横向速度约束方程为

式中，δvx表示车辆运动时由于侧滑所引起的附加横向速度变化，建模为白噪声，其取值大小和车辆的具体运动特性有关。

从而可以构造横向速度虚拟量测方程为

其中，量测矩阵为

量测噪声为

3 测试数据分析

本论文以华为 Mate7智能手机型号作为测试平台，通过测试静态传感器数据，分析确定组合导航卡尔曼滤波参数，并在此基础上通过实际跑车实验，设计行车路线，经过高架、隧道等环境，获得不同条件下的传感器数据。最后应用本论文提出的算法进行仿真计算得到导航结果，并分析误差精度。

3.1 组合导航滤波参数确定

手机中安装的MEMS器件为低精度传感器，因此在使用传感器数据之前必须对误差进行标定获得相应的模型参数[13]。在实验室条件下，将手机置于水平转台上，测试获得静态数据，数据采样频率为50 Hz。

图3和图4所示分别为三轴陀螺和加速度计的原始数据输出。从图3和图4可以看出，陀螺的零偏较大，可以达到上千度每小时，并且陀螺零偏随时间漂移，需要对零偏数据进行扣除。加速度计输出相对较为稳定，精度较高。

图3 三轴陀螺输出信息Fig.3 Output of three-axis gyros

图4 三轴加速度计输出信息Fig.4 Output of three-axis accelerometers

根据测得的静态数据，当传感器输出稳定后，对原始数据进行百十秒平均分析，并对均值和标准差进行统计，得到表1所示结果。

根据表1所得的静态数据并结合导航测试结果，在卡尔曼滤波参数设计上，陀螺随机常数误差和一阶马尔科夫误差设定为1500 (°)/h，加速度计一阶马尔科夫误差设定为0.03 m/s2。

量测方程中，GPS东向和北向位置误差均设为5 m，东向和北向速度误差分别设为0.5 m/s，横向速度约束方程量测误差根据经验值设定为0.5 m/s。

表1 IMU静态数据分析Tab.1 Analysis of IMU static data

3.2 基于车辆运动模型辅助的组合导航算法性能测试

根据实际跑车实验，采集传感器数据，图5为测试的一组跑车航迹路线图，测试时间约为1200 s。

在时间点250 s、770 s、1090 s分别将GPS设定为无效，时间长度为60 s。以GPS导航结果作为参照，图6和图7为所得导航结果与GPS信息的差值曲线。

图5 跑车航迹路线图Fig.5 Trajectory of the test

图6 速度误差曲线Fig.6 Velocity error curves

图7 位置误差曲线Fig.7 Position error curves

根据以上测试实验，对每段的误差 RMS数据进行统计分析，得到表2所示的结果。从表2中数据可以看到，采用本文提出的横向速度约束车载导航方案，可以很大程度上提高在GPS信号丢失后的定位精度，实现车载导航的连续定位。

表2 GPS失效情况下的导航数据误差统计Tab.2 S tatistic of positioning errors with GPS outages

4 结 论

本文基于智能手机平台，提出了一种针对车载导航中以车辆运动模型作为辅助的车载 DR组合导航方法，设计了基于车辆运动模型辅助的车载DR组合导航方案。在GPS信号丢失环境下，通过对低成本消费级MEMS传感器输出进行误差修正，以车辆横侧向速度约束作为虚拟观测量，进行DR/车辆运动模型组合导航。此外，本文还以华为Mate7智能手机型号作为测试平台，经过实际跑车数据实验采集数据对所提出的算法进行验证，从导航对比结果可以看出该方法能显著提高GPS失效环境下的导航精度，抑制DR推算的误差发散。

（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]. 中国惯性技术学报, 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多路径效应实验测试与分析研究[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] 贾继超, 吴训忠, 夏家和. 一种车辆航位推算改进算法[J]. 中国惯性技术学报, 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陀螺标定方法[J]. 传感技术学报, 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] 付强文, 秦永元, 李四海, 等. 车辆运动学约束辅助的惯性导航算法[J]. 中国惯性技术学报, 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

国家自然科学基金项目（61533008, 61374115, 61533009, 61673208）；江苏省“333工程”科研资助立项项目（BRA2016405）；留学人员择优资助项目（2016年）；江苏省六大人才高峰资助项目（2013-JY-013）；航空科学基金项目（20165552043）；江苏高校优势学科建设工程项目和江苏省“物联网与控制技术”重点实验室基金资助; 中央高校基本科研业务费专项资金资助（NS2017016, NZ2016104）；江苏省普通高校研究生科研创新计划项目（KYLX15_0264）

许建新（1990—），男，博士生，从事组合导航技术研究。Email: xujianxin@nuaa.edu.cn