一种车辆模型辅助的MEMS-SINS导航方法

王美玲，冯国强，李亚峰，于华超，刘 彤

（北京理工大学 自动化学院，北京 100081）

一种车辆模型辅助的MEMS-SINS导航方法

王美玲，冯国强，李亚峰，于华超，刘 彤

（北京理工大学 自动化学院，北京 100081）

针对城市环境中GNSS因遮挡导致MEMS-SINS精度快速降低的问题，在车辆运动学约束的基础上，结合四通道ABS轮速传感器和方向盘转角信息，提出一种新的适用于陆地车辆的MEMS-SINS导航方法。该方法通过分析车辆转弯和运动约束特性，构建角速度和加速度观测量，从而实现基于模型辅助的MEMS误差在线补偿；其次，ABS轮速信息与非完整约束条件结合可额外增加三维车体速度观测量，进一步维持卫星失效时组合滤波器的量测更新。跑车实验表明，在GNSS信号频繁丢失甚至长时间无法定位时，低精度MEMS惯性器件引起的快速误差积累得到有效抑制，与经典车体约束结合里程计算法相比，航向精度提高约70%，位置、速度精度也有相应的提高，验证了算法的有效性。

MEMS-SINS；ABS轮速传感器；运动学约束；模型辅助

随着微机电系统（Micro Electro Mechanical System，MEMS）的快速发展，基于MEMS器件的低成本、小型化捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）成为国内外惯性导航技术的主要研究方向之一[1]。但现有MEMS惯性器件测量精度有限，导致MEMS-SINS定位误差随着时间增加而快速积累，在短时间内定位精度也较低，因此与其他定位系统的组合是提高其定位精度的重要措施。目前通常采用全球卫星导航系统（Global Navigation Satellite System，GNSS）与MEMS-SINS相结合的方式来弥补各自的不足，从而为载体提供较高精度的位置、速度信息[2-3]。但是在复杂城市峡谷环境下，GNSS信号频繁丢失甚至长时间无法定位，MEMS-SINS独立工作时间增加，从而导致组合系统定位、测姿精度迅速恶化。因此，如何提高GNSS信号缺失时MEMS-SINS的导航精度，对于车辆导航定位具有重要意义。

模型辅助导航是一种廉价有效的MEMS-SINS误差修正方法[4]，主要有动力学辅助和运动学约束两种模式。由于地面车辆具有大量子系统和强非线性特性[5]，所以建立简洁实用的动力学模型比较困难。相对来讲，运动学约束是车载SINS误差控制的常用手段。常规运动学约束主要对车体横向和垂向速度施加约束，以限制惯性器件误差积累[6-9]。Godha将高度约束用于陆用GNSS/SINS紧组合[6]，进一步增加了约束信息；Yang假设车辆运行过程中只有航向角发生变化，进而引入加速度和姿态角约束[9]。但无论哪种算法，都是将非完整约束条件作为伪测量值引入组合滤波器来估计、反馈SINS误差，对间接观测量如姿态和MEMS器件误差的估计效果较差[10]。另外，约束条件无法提供全面的载体运动信息，尤其是车体纵向速度以及航向变化信息，所以仅依靠运动学约束难以保证GNSS长时间失效时MEMS-SINS的导航精度，因此有必要在运动约束的基础上引入车辆自带传感器提供的低成本运动信息，形成信息互补，达到共同辅助MEMS-SINS导航的目的。

综合以上考虑，进一步将运动学约束直接用于导航解算前，补偿MEMS测量误差，并引入车辆自带四通道制动防抱死系统（Antilock Brake System，ABS)和方向盘转角传感器，通过分析车辆转弯特性和运动学约束，提出一种新的基于角速度、加速度信息的滤波算法，同时在横向、垂向速度约束中引入ABS轮速传感器提供的纵向速度，在GNSS失效时可持续保持组合滤波器的更新。最后，通过跑车试验验证了算法的有效性。

1 整体方案

针对城市峡谷环境的复杂性，在传统运动学约束的基础上，增加车辆自带四通道ABS和方向盘转角传感器组成一种新的MEMS-SINS导航系统。如图1所示，本文提出的算法首先基于ABS轮速信息和方向盘转角信息，通过分析车辆转弯运动模型后获得车辆纵向速度、横摆角速度。然后结合车辆运动学约束，构建一组角速度、加速度值，以此作为前置滤波器的量测信息。该前置滤波器通过建立MEMS随机漂移模型，并引入以上量测信息，实现导航解算前MEMS漂移误差的补偿，从而减少导航解算的误差积累。其次，纵向速度信息结合车体速度非完整约束条件一起作为新的观测量，引入组合滤波器中，持续估计、反馈SINS误差。在整个导航系统中，GNSS仅在其定位信息有效时引入组合滤波器中，而上述方法不论GNSS是否有效，均可用于提高MEMS-SINS导航精度。

图1 新型MEMS-SINS导航方法原理框图Fig.1 Schematic of new MEMS-SINS navigation method

2 基于运动模型的ABS和方向盘转角信息融合

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

设ABS传感器采样周期为Tc，假设车辆在Tc时段从A点移动到B点，移动的距离为ΔS，车辆横摆角速度即航向角变化率为cω，如图3所示。

图3中，将两个前轮虚拟为一个中心轮，δ为虚拟前轮转角，ΔSF为虚拟前轮的移动距离，ΔSRL、ΔSRR分别为左后轮、右后轮移动距离，L为车身轴距，2B为两车轮轮距。根据图3中几何关系可得：

图2 车辆圆周运动示意图Fig.2 Schematic of vehicle circular motion

图3 车辆转向过程位移关系图Fig.3 Elementary displacement of vehicle in turning motion

式(1)中，δ正比于方向盘转角，ΔSF不能由传感器直接测量，但根据阿克曼转向原理可得图4所示四个车轮的几何关系。

图4 车辆转弯几何关系图Fig.4 Geometry of car in a turning motion

继而得到：

因为轮胎与地面存在相对滑动，为了提高ΔS、cω的精度，应融合式(3)所示关于未知量ΔS、cω的全部表达式。考虑ABS测量信息不涉及加速度等变量，可得如下EKF融合模型[12]：

由ΔS、cω进一步推导得到车辆的纵向速度vxc和向心加速度fyc（也即车辆侧向加速度）：

另外，假设汽车纵向加速度在很短周期Tc内保持不变，则i时刻纵向加速度为：

3 车辆运动学辅助的MEMS-SINS导航滤波方法

3.1 基于角速度、加速度信息的滤波方法

MEMS惯性器件误差可建模为自相关函数具有指数形式的一阶高斯-马尔可夫过程，选取MEMS测量误差作为状态变量，可得：

式(11)(13)为通过分析车辆转弯特性和运动学约束后所得的角速度、加速度信息，与MEMS测量值相比，两者物理意义相同，但数据源不同，因此可将两者相减作为卡尔曼滤波的量测方程，得到：

通过以上方程可实现车辆运动学模型和MEMS测量值的融合，有效去除加速度、角速度中的随机噪声，实现了基于模型辅助的MEMS误差在线补偿，从而提高SINS导航解算的精度。

3.2 基于车体速度辅助的MEMS-SINS滤波器

基于车体的速度辅助是在车体横向、垂向速度约束的基础上，引入ABS轮速传感器提供的纵向速度，然后同SINS导航解算所得车体速度相减作为组合滤波器的量测方程，从而保持卫星失效时组合滤波器的持续更新。采用摄动法建立SINS误差模型，离散化后，其状态方程为

在正常城市路况下，假设车辆仅存在轻微的侧滑和跳跃[9]，以此作为车体系速度约束。因此车系y、z向速度可建模为

同时考虑式(6)所得车体x向速度：

另一方面，SINS解算速度nv与其在车体坐标系下的投影满足：

根据扰动法则，进一步得到车体速度的扰动增量方程：

将不同信源的SINS解算速度、ABS速度同车体约束速度信息相减作为量测方程，并通过式(19)引入SINS误差模型中，即：

通过式(18)(19)可知，利用速度辅助和约束能够同时估计SINS解算的速度和姿态误差，从而减缓GNSS信号受遮挡期间定位结果的发散。相比于常规车体速度约束算法[6-8]，增加了车体纵向速度的辅助，可进一步提高MEMS-SINS的导航精度。

4 跑车验证与分析

4.1 实验环境及条件

如图5所示，试验平台为北京理工大学组合导航与智能导航（Integrated Navigation and Intelligent Navigation，ININ）实验室自主改装的无人驾驶车辆（Unmanned Ground Vehicle，UGV），该UGV自带4通道ABS传感器和方向盘转角传感器，数据更新率为25 Hz。

MEMS惯性器件测量值通过东方联星公司PNS100-BGI型组合系统采集，数据更新率为50 Hz，其主要参数通过Allan方差分析所得，结果如表1所示。GNSS采用NovAtel公司OEM628三模接收机，数据更新率为1 Hz。测试时同时搭载ININ 实验室自主开发的高精度RTK-GNSS/SINS组合系统（位置1σ精度：2cm；航向精度：0.06°）作为本文算法评判的精度基准，并以GPS时间作为数据融合的时间对准基准。各实验设备如图6所示。

图5 UGV实验平台Fig.5 UGV experimental platform

图6 实验设备Fig.6 Testing equipment

GNSS记录跑车行进轨迹如图7所示。UGV从北京理工大学6号教学楼出发，沿西北三环辅路、魏公村路行驶一圈后回到起点，全程4.8 km，历时17 min。行进过程中，GNSS信号频繁受到桥梁、高楼、树木的遮挡，导致GNSS接收机出现5次无定位（路段1～5），1次定位精度差现象（路段6），其中路段2由于高架桥影响GNSS无定位时间高达132 s。

图7 GNSS轨迹图Fig.7 Trajectory of GNSS data

4.2 实验对比及讨论

实验结果首先与经典车体横向、垂向速度约束算法作比较，从而说明前置滤波器和车体三维速度辅助对于提高导航精度的效果，此为对比实验之一；其次，为了说明在相同传感器信息条件下本文算法的有效性，在经典车体速度约束的基础上加入里程计信息，作为对比实验之二。而本文算法主要分为以下三种方式进行讨论：仅采用前置滤波器（方式1）；仅采用车体速度辅助（方式2）；同时采用前置滤波器和车体速度辅助（方式3）。对比结果如表2所示。

可以看出，相比于经典车体约束算法，方式1、方式2均能够显著提高组合导航精度。对于方式1，由于前置滤波器减少了惯性器件原始测量值的误差，因此位置、速度、航向精度均有明显提高，其中东向、北向位置精度分别提高了71.7%、36.7%，航向精度提高了38.8%。

表2 三种方式位置、速度、航向均方根误差值（RMS）Tab.2 RMS errors of position, velocity, and heading from the different methods

对于方式2，速度辅助提供了完整的三个维度的速度观测量，而且根据SINS误差传播特性，车体坐标系的速度观测量对于卡尔曼滤波器的系统状态中，与位置相关和速度相关的状态向量可观测性强，所以位置和速度精度提高比例较大，平均提高80%以上，而对于提高航向精度效果较弱，仅提高9.7%。方式3结果显示同时使用前置滤波器和车体速度辅助可融合两者优点，进一步提高导航精度，其中，位置速度精度与方式2持平，但航向精度提高76%。

为进一步评价GNSS长时间失效期间，本文所述算法的性能，对比了经典车体约束算法和方式3的定位轨迹，结果如图8所示。由图8可知，在GNSS信号无遮挡的情况下，两种方式的轨迹与基准轨迹吻合度均较好，但在GNSS信号受遮挡期间，尤其是路段2长时间无定位情况下，经典车体约束算法位置误差迅速增大。

图8 二维定位结果图Fig.8 Positioning results inx-yplain

图9给出了路段2的位置及航向角误差，由图9可知，经典车体约束算法北向、东向、高度误差最大分别达到264.72 m、44.63 m、15.80 m，航向角误差最大超过2.32°，完全无法满足导航精度要求。由于此时GNSS信息完全失效，经典车体约束算法在长时间无外部传感器辅助的情况下，MEMS-SINS定位误差快速发散，达到每秒数米的量级，横向、垂向速度约束虽然能够减少误差积累，但效果有限。而方式3北向、东向、高度误差最大分别为3.20 m、4.50 m、2.02 m，航向角误差最大0.91°。可见即使在长时间无定位的情况下，也能够满足UGV导航精度要求。

对于对比实验二，通过表2可知，经典车体约束+里程计算法在东向和北向维度上，位置和速度精度要高于方式1情况。这是因为前置滤波器虽然能够提高MEMS原始测量值精度，但是其误差仍会随时间积累，因此需要进一步通过卡尔曼滤波来估计反馈SINS解算误差。

图9 路段2位置及航向误差Fig.9 Position and heading errors of road section 2

另外，通过表2可知，经典车体约束+里程计导航精度与方式2处于同一数量级。这是由于两者均能够提供全部维度的速度信息，不同之处在于传统算法将里程计速度投影到导航系进行量测更新，而方式2直接在车体系同时进行速度辅助和约束，形式更为简洁。方式3导航精度均优于经典车体约束+里程计算法，尤其是航向精度提高较大，约提高70%，克服了传统组合系统对航向信息估计效果较差的缺点。

5 结 论

本文针对复杂城市峡谷环境下，传统运动学约束算法难以保证GNSS长时间失效时MEMS-SINS的导航精度问题，实现了一种车辆模型辅助的MEMS-SINS导航方法。该方法通过引入ABS轮速传感器及车辆运动学模型，建立了基于角速度、加速度信息的滤波方案，并增加车体系的三维速度观测量作为新的滤波量测更新。跑车结果表明组合系统定位、测姿精度均有较大改善，证明了算法的有效性，为MEMS-SINS组合系统提供了一种简单有效的低成本导航方法。

（References）：

[1] 崔留争. MEMS-SINS/GPS组合导航关键技术研究[D].中国科学院大学, 2014. Cui Liu-zheng. Research on the key technologies of MEMS-SINS/GPS integration navigation syetem[D]. University of Chinese Academy of Sciences, 2014.

[2] Rabbou M A, El-Rabbany A. Tightly coupled integration of GPS precise point positioning and MEMS-based inertial systems[J]. GPS Solutions, 2015, 19(4): 601-609.

[3] Quinchia A G, Falco G, Falletti E, et al. A comparison between different error modeling of MEMS applied to GPS/INS integrated systems[J]. Sensors, 2013, 13(8): 9549-9588.

[4] 吕品, 赖际舟, 杨天雨, 等. 基于气动模型辅助的四旋翼飞行器室内自主导航方法[J]. 航空学报, 2015, 36(4): 1275-1284. Lv Pin, Lai Ji-zhou, Yang Tian-yu, et al. Autonomous navigation method aided by aerodynamics model for an indoor quadrotor[J]. Acta Aeronautica Et Astronautica Sinica, 2015, 36(4): 1275-1284.

[5] Fernández J G. A vehicle dynamics model for driving simulators[D]. Chalmers University of Technology, 2012.

[6] Godha S, Cannon M E. GPS/MEMS INS integrated system for navigation in urban areas[J]. GPS Solutions, 2007, 11(3): 193-203.

[7] 付强文, 秦永元, 李四海, 等. 车辆运动学约束辅助的惯性导航算法[J]. 中国惯性技术学报, 2012, 20(6): 640- 643. Fu Qiang-wen, Qin Yun-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.

[8] 刘华, 刘彤, 张继伟. 陆地车辆 GNSS/MEMS惯性组合导航机体系约束算法研究[J]. 北京理工大学学报, 2013, 33(5): 510-515. Liu Hua, Liu Tong, Zhang Ji-wei. Body Frame constraint for a GNSS/MEMS INS integrated system in land vehicle navigation[J]. Journal of Beijing Institute of Technology, 2013, 33(5): 510-515.

[9] Yang L, Li Y, Wu Y, et al. An enhanced MEMSINS/GNSS integrated system with fault detection and exclusion capability for land vehicle navigation in urban areas[J]. GPS Solutions, 2014, 18(4): 593-603.

[10] Crocoll P, Görcke L, Trommer G F, et al. Unified model Technique for inertial navigation aided by vehicle dynamics model[J]. Navigation, 2013, 60(3): 179-193.

[11] 李博文, 姚丹亚. 低成本车载MEMS惯导导航定位方法[J]. 中国惯性技术学报, 2014, 22(6): 719-723. Li Bo-wen, Yao Dan-ya. Low-cost MEMS IMU navigation positioning method for land vehicle[J]. Journal of Chinese Inertial Technology, 2014, 22(6): 719-723.

[12] Bonnifait P, Bouron P, Crubille P, et al. Data fusion of four ABS sensors and GPS for an enhanced localization of car-like vehicles[C]//IEEE International Conference on Robotics & Automation. 2001, Vol.2: 1597-1602.

[13] 李磊磊, 陈家斌, 杨黎明, 等. 基于道路信息的智能地图匹配算法[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.

MEMS-SINS navigation method aided by vehicle model

WANG Mei-ling, FENG Guo-qiang, LI Ya-feng, YU Hua-chao, LIU Tong
(School of Automation, Beijing Institute of Technology, Beijing 100081, China)

In view that the frequent outages of GNSS in urban environments can quickly degrade the performance of MEMS-SINS, a new MEMS-SINS navigation method for land vehicles is proposed based on the vehicle constraints and combined with the four-channel ABS wheel speed sensors and steering angle information. By analyzing the vehicle turning and constraint characteristics, the angular velocity and acceleration are constructed as the measurements to achieve on-line compensation for MEMS’s rapid drifting errors. Three-dimension vehicle-body velocity provided by ABS information and non-holonomic constraint is applied to further maintain the update of the integration Kalman filtering during GNSS outages. The road-test results demonstrate the proposed method can effectively reduce the rapid accumulation errors of SINS due to low-cost MEMS inherent bias in the circumstances of long-time outrages of GNSS. Compared with conventional body velocity constraint and odometer algorithm, the heading accuracy is improved by 70%, and the accuracies of position and velocity are also improved.

MEMS-SINS; ABS wheel speed sensors; vehicle constraints; model aiding

U666. 1

A

1005-6734(2017)02-0209-07

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

2017-01-11；

2017-03-28

国家自然科学基金面上项目（61173076，61473042）；国家自然科学基金重大研究计划培育项目（91120003）

王美玲（1970—），女，教授，从事组合导航与智能导航研究。E-mail: wangml@bit.edu.cn