低成本车载 MEMS 惯导导航定位方法

低成本车载 MEMS 惯导导航定位方法 | 厚势汽车

2018 年 3 月 28 日 厚势李博文，姚丹亚

厚势按：当 GPS 卫星信号受到遮挡，车载 GPS 无定位输出时，通过低成本 MEMS 惯导进行定位是车辆导航的一种弥补方法。然而低成本 MEMS 惯导测量误差大，定位误差会快速累积。

针对此问题，本文研究了一种应用于 MEMS 惯导导航定位的 Kalman 滤波算法。通过分析车辆的运动特性，在已有的研究基础上，提出了向心加速度差值误差这一新型观测量，并推导了误差状态系统模型的状态转移矩阵和观测矩阵。200s 时长的实车实验表明，单纯 MEMS 惯导定位的误差率是 75.27%，而所述新方法的定位误差率是 3.86%，定位精度有了大幅度提高，取得了良好的效果。

本文来自 2014 年 12 月 15 日出版的《 中国惯性技术学报 》，作者是清华大学自动化系的李博文，姚丹亚教授。

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 上的分解；

根据式 (1)~(3) 计算姿态、速度和位置的变化率后，再通过积分运算就可以计算得到载体的当前姿态、速度和位置。

2. 误差传播

通过惯导输出的加速度和角速度数据来直接计算得到的姿态、速度和位置都含有误差[8]。计算值和真实值的关系如下：

δP 是位置误差矢量；δv 是速度误差矢量；Φ 是姿态误差角矢量，Φ = [ Φ_x Φ_y Φ_z ]^T，Φ_x、Φ_y、Φ_z 分别为姿态误差角在 x、y、z 轴上的分量；Φ_x 是 Φ 的反对称矩阵；I 是 3 维单位矩阵。

在导航坐标系下惯导误差传播方程如下 [8]：

式中，ε_g、ε_a 分别是陀螺仪和加速度计的测量误差，它们可以简化看作是一阶马尔科夫过程：

式中，τ_g、τ_a 分别是 ε_g、ε_a的时间相关常数，w_g、w_a都是白噪声。

3. 观测量

3.1 已有观测量

通常 GPS/INS 组合导航的观测量为 GPS 输出值和惯导计算值之间的差值，包括位置差和速度差。然而当 GPS 无有效输出，只通过惯导定位时，获取不了这些观测量。对于车辆运动，可以假设车辆不会发生侧滑，并且不会离开地面。设车辆载体坐标系如图 1 所示。

图 1 车辆载体坐标系

载体坐标系的原点 O 位于车辆后轮轴中央上方一点，x 轴平行于后轮轴指向车辆的右侧，y 轴指向车辆前方，z 轴垂直于 x、y 轴指向车辆上方。由于车辆不会发生侧滑，并且不会离开地面，则可以得到：

3.2 新观测量

大多数车辆的转向轮为前轮，当车辆转向行驶时，车辆的瞬时运动可以看作是围绕后轮轴线上一点为圆心的圆周运动 [9]。当车辆直线行驶时，车辆运动也可以看作是曲率半径无限大的圆周运动。如图 2 所示，车辆载体坐标系原点 O 的速度方向与 y 轴平行，圆周运动的圆心 P 点在 x 轴轴线上。

图 2 车辆圆周运动示意图

假设车辆在很短的时间 dt 内从 A 点移动到 A' 点，移动的距离为 dl，速度方向变化的角度为 dφ，如图 3 所示。

图 3 圆周运动细节分析

图 3 中，ρ 是车辆运动的瞬时曲率半径，v^b_y 是车辆在载体坐标系 y 轴方向上的速度。用 φ 表示车辆载体坐标系 z 轴的旋转角速度，则曲率半径 ρ 可以表示为 [10]：

根据圆周运动定律，可以推导得到向心加速度等于速度乘以角速度：

式中，a_c 为向心加速度。

设 J 为向心加速度差值，其通过下式计算：

根据方程 (17) 可得：

我们可以通过惯导的输出及其计算值来获取向心加速度、载体坐标系 y 轴速度以及角速度的观测值。

因为地球自转角速度和车辆线运动形成的相对于地球的旋转角速度相对较小，本文中将这两项角速度忽略，因而：

则 J 的观测值 Jˆ 为：

用 δJ 表示向心加速度差值的误差，可以将其作为误差状态系统模型的一种新观测量，其计算式可为：

4. 误差状态系统模型

通过扰动方程 (18)，可以得到 J 的误差方程：

式中，g_0 是当地重力加速度大小。

则可以建立误差状态系统模型，误差状态向量包含 15 个参数。系统误差向量 X 设为：

设离散化后的系统状态模型如下：

设观测矩阵 H_k 为：

根据误差方程 (27)，可以推导得到 M 矩阵，M 中非零元素有：

M 矩阵的其余元素均为 0。

建立好系统状态模型后，再根据 Kalman 滤波的递推算法对状态进行估计。Kalman 滤波递推算法可参考文献 [11][12]。

5. 实验验证

图 4 车辆行驶路线

在清华大学内进行了实车实验。车辆上安装 OxTSRT3100、MEMS 惯导（Invensense mpu6050）以及 bu353 GPS。RT3100 是一款高精度 GPS/IMU 组合导航产品；mpu6050 是低成本 MEMS 惯导，内置加速度计和陀螺仪，其价格约为 3 美元；bu353 是一款低成本 GPS，其价格约为 30 美元。RT3100 和 mpu6050 的数据输出频率均设为 20 Hz。各设备均安装于车辆后轮轴中间上方。RT3100 输出的行车路线如图 4 所示。

图 5 向心加速度观测值及载体坐标系 y 轴速度与 z 轴角速度乘积观测值

RT3100 内建高精度惯导，能够输出高精度位置、速度、姿态、加速度和角速度等数据。取一段 RT3100 的输出数据进行分析，该段数据采集时间为 500s，共有 10000 个数据点。

这段数据的向心加速度差值的平均值为 0.0034，接近于 0，证实了本文所述方法理论基础的正确性。

从图 4 中 S 点位置时开始通过MEMS惯导导航定位，终点为 E 点。在车辆静止时测量得到 mpu6050 的各轴加速度和角速度零偏，以 bu353 在 S 点输出的位置和速度为初始值进行推算。通过以下 3 种计算方式进行定位：

1）使用惯导数据直接推算；

2）使用载体坐标系 x 轴、z 轴方向上的速度误差作为观测量进行 kalman 滤波，称其为滤波 1；

3）使用载体坐标系 x 轴、z 轴方向上的速度误差以及向心加速度差值的误差作为观测量进行 Kalman 滤波，称其为滤波 2。

图 6 3 种计算方式的定位结果

三种计算方式的定位结果如图 6。此间车辆行驶 200s，行驶总距离为 1429.53m，三种计算方式的最后定位误差如表 1 所示。

表 1 3 种计算方式的最后定位误差

通过惯导输出数据直接推算定位在前 10s 具有较好的准确度，然而由于低成本 MEMS 惯导测量误差较大，10s 之后出现了快速的发散，200s 时定位误差率达到 75.46%。相对于直接推算，通过 Kalman 滤波进行定位的结果在 200s 内准确性较好。滤波 1 的定位误差率为 9.47%。增加了向心加速度差值的误差为观测量的滤波 2 定位误差率降为 3.86%，定位精度有较大提高。

6. 结论

由于车辆行驶的区域可能被严重遮挡，车载 GPS 不能有效定位，此时通过惯导来进行车辆定位是一种较好的技术方法。从经济性方面考虑，低成本 MEMS 惯导可以普及应用于民用车辆。然而低成本 MEMS 惯导测量误差大，将其输出的原始数据直接用于导航定位，定位误差会随着时间增加而快速累加，难以直接用于车辆的定位导航。

针对此问题，本文研究了一种新式的 MEMS 惯导导航定位 Kalman 滤波算法。实车实验结果表明，使用本文所述方法后，定位精度有较大提高，取得了良好的效果。本文所提出的向心加速度差值误差这一观测量是通过惯导自身的输出计算得来，因而当惯导与 GPS 等其他定位方式组合用于对车辆导航时，其仍然可以作为观测量，将有助于进一步提高定位精度。

参考文献

[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

编辑整理：厚势分析师伊龙马

转载请注明来自厚势和厚势公号：iHoushi

-END-

文章精选

企业家

马斯克和贾跃亭｜福特CEO下台｜正道汽车仰融

任正非裁员｜电池大牛凯尔提离开特斯拉

智能驾驶

BBC自动驾驶纪录片｜自动驾驶第一案，谷歌讼Uber

为什么说百度阿波罗画虎不成反类犬？

考虑线控转向非线性和不确定性的转向角控制