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

一种车辆模型辅助的MEMS-SINS导航方法
王美玲;冯国强;李亚峰;于华超;刘彤
【摘要】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 pared 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.%针对城市环境中GNSS因遮挡导致MEMS-SINS精度快速降低的问题,在车辆运动学约束的基础上,结合四通道ABS轮速传感器和方向盘转角信息,提出一种新的适用于陆地车辆的MEMS-SINS导航方法.该方法通过分析车辆转弯和运动约束特性,构建角速度和加速度观测量,从而实现基于模型辅助的MEMS误差在线补偿;其次,ABS轮速信息与非完整约束条件结合可额外增加三维车体速度观测量,进一步维持卫星失效时组合滤波器的量测更新.跑
车实验表明,在GNSS信号频繁丢失甚至长时间无法定位时,低精度MEMS惯性器件引起的快速误差积累得到有效抑制,与经典车体约束结合里程计算法相比,航向精度提高约70％,位置、速度精度也有相应的提高,验证了算法的有效性.
【期刊名称】《中国惯性技术学报》
【年(卷),期】2017(025)002
【总页数】7页(P209-215)
【关键词】MEMS-SINS;ABS轮速传感器;运动学约束;模型辅助
【作者】王美玲;冯国强;李亚峰;于华超;刘彤
【作者单位】北京理工大学自动化学院,北京100081;北京理工大学自动化学院,北京100081;北京理工大学自动化学院,北京100081;北京理工大学自动化学院,北京100081;北京理工大学自动化学院,北京100081
【正文语种】中文
【中图分类】U666.1
随着微机电系统（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失效时可持续保持组合滤波器的更新。

最后，通过跑车试验
验证了算法的有效性。

针对城市峡谷环境的复杂性，在传统运动学约束的基础上，增加车辆自带四通道ABS和方向盘转角传感器组成一种新的MEMS-SINS导航系统。

如图1所示，本
文提出的算法首先基于ABS轮速信息和方向盘转角信息，通过分析车辆转弯运动
模型后获得车辆纵向速度、横摆角速度。

然后结合车辆运动学约束，构建一组角速度、加速度值，以此作为前置滤波器的量测信息。

该前置滤波器通过建立MEMS
随机漂移模型，并引入以上量测信息，实现导航解算前MEMS漂移误差的补偿，从而减少导航解算的误差积累。

其次，纵向速度信息结合车体速度非完整约束条件一起作为新的观测量，引入组合滤波器中，持续估计、反馈SINS误差。

在整个导航系统中，GNSS仅在其定位信息有效时引入组合滤波器中，而上述方法不论GNSS是否有效，均可用于提高MEMS-SINS导航精度。

多数车辆以前轮为转向轮，转向行驶时，车辆的瞬时运动可看作以后轴中点为圆心的圆周运动，车辆直线行驶可看作是曲率半径无限大的圆周运动[11]。

如图2，车体坐标系原点O固连于车辆后轴中心，车辆速度方向与x轴平行，圆周运动的圆
心P点在y轴轴线上。

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

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

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

继而得到：
式中，Lδ、Rδ分别为左前轮和右前轮转角。

设ΔSFL、分别为左前轮、右前轮移
动距离，分别用替代式(1)中，可得用于计算未知量ΔS和cω的5个冗余表达式：因为轮胎与地面存在相对滑动，为了提高ΔS、cω的精度，应融合式(3)所示关于
未知量ΔS、cω的全部表达式。

考虑ABS测量信息不涉及加速度等变量，可得如
下EKF融合模型[12]：
由ΔS、cω进一步推导得到车辆的纵向速度vxc和向心加速度fyc（也即车辆侧向加速度）：
另外，假设汽车纵向加速度在很短周期Tc内保持不变，则i时刻纵向加速度为：3.1 基于角速度、加速度信息的滤波方法
MEMS惯性器件误差可建模为自相关函数具有指数形式的一阶高斯-马尔可夫过程，选取MEMS测量误差作为状态变量，可得：
式中：分别为MEMS惯性器件测得的角速度、加速度值；F为系统状态矩阵；为
测量白噪声；τ为马尔可夫相关时间。

为了减少惯性器件测量误差，考虑引入车辆模型，以实现额外增加角速度、加速度量测信息的目的。

设γ、θ、α分别为导航系（n系-北东地）到车体坐标系（b系-前右下）的横滚角、俯仰角、航向角。

由于车辆行驶在城市平坦道路时，横滚、俯仰角几乎不发生变化，因此假设姿态角中只有航向角发生变化，以此作为姿态约束。

进一步引入式(4)所得航向角速率，则有。

又MEMS测量角速率中可忽略地球自转角速率的影响，因此可得与姿态角速率存在如下关系：
同样考虑车辆行驶在城市环境中时，车辆加速度在车体坐标系中垂向的投影近似为0，以此作为加速度约束，进一步引入式(7)(8)所得的车体侧向、纵向加速度，则有：。

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

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

3.2 基于车体速度辅助的MEMS-SINS滤波器
基于车体的速度辅助是在车体横向、垂向速度约束的基础上，引入ABS轮速传感器提供的纵向速度，然后同SINS导航解算所得车体速度相减作为组合滤波器的量测方程，从而保持卫星失效时组合滤波器的持续更新。

采用摄动法建立SINS误差模型，离散化后，其状态方程为
在正常城市路况下，假设车辆仅存在轻微的侧滑和跳跃[9]，以此作为车体系速度约束。

因此车系y、z向速度可建模为
同时考虑式(6)所得车体x向速度：
另一方面，SINS解算速度nv与其在车体坐标系下的投影满足：
根据扰动法则，进一步得到车体速度的扰动增量方程：
式中，代表速度的斜对称矩阵。

将不同信源的SINS解算速度、ABS速度同车体约束速度信息相减作为量测方程，并通过式(19)引入SINS误差模型中，即：
式中：为均值为0，方差为σ2的白噪声；H为量测矩阵。

比较式(15)(19)可得：通过式(18)(19)可知，利用速度辅助和约束能够同时估计SINS解算的速度和姿态误差，从而减缓GNSS信号受遮挡期间定位结果的发散。

相比于常规车体速度约束算法[6-8]，增加了车体纵向速度的辅助，可进一步提高MEMS-SINS的导航精度。

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所示。

GNSS记录跑车行进轨迹如图7所示。

UGV从北京理工大学6号教学楼出发，沿西北三环辅路、魏公村路行驶一圈后回到起点，全程4.8 km，历时17 min。

行进过程中，GNSS信号频繁受到桥梁、高楼、树木的遮挡，导致GNSS接收机出现
5次无定位（路段1～5），1次定位精度差现象（路段6），其中路段2由于高
架桥影响GNSS无定位时间高达132 s。

4.2 实验对比及讨论
实验结果首先与经典车体横向、垂向速度约束算法作比较，从而说明前置滤波器和车体三维速度辅助对于提高导航精度的效果，此为对比实验之一；其次，为了说明在相同传感器信息条件下本文算法的有效性，在经典车体速度约束的基础上加入里程计信息，作为对比实验之二。

而本文算法主要分为以下三种方式进行讨论：仅采用前置滤波器（方式1）；仅采用车体速度辅助（方式2）；同时采用前置滤波器和车体速度辅助（方式3）。

对比结果如表2所示。

可以看出，相比于经典车体约束算法，方式1、方式2均能够显著提高组合导航精度。

对于方式1，由于前置滤波器减少了惯性器件原始测量值的误差，因此位置、速度、航向精度均有明显提高，其中东向、北向位置精度分别提高了71.7%、36.7%，航向精度提高了38.8%。

对于方式2，速度辅助提供了完整的三个维度的速度观测量，而且根据SINS误差传播特性，车体坐标系的速度观测量对于卡尔曼滤波器的系统状态中，与位置相关
和速度相关的状态向量可观测性强，所以位置和速度精度提高比例较大，平均提高80%以上，而对于提高航向精度效果较弱，仅提高9.7%。

方式3结果显示同时使用前置滤波器和车体速度辅助可融合两者优点，进一步提高导航精度，其中，位置速度精度与方式2持平，但航向精度提高76%。

为进一步评价GNSS长时间失效期间，本文所述算法的性能，对比了经典车体约
束算法和方式3的定位轨迹，结果如图8所示。

由图8可知，在GNSS信号无遮
挡的情况下，两种方式的轨迹与基准轨迹吻合度均较好，但在GNSS信号受遮挡
期间，尤其是路段2长时间无定位情况下，经典车体约束算法位置误差迅速增大。

图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解算误差。

另外，通过表2可知，经典车体约束+里程计导航精度与方式2处于同一数量级。

这是由于两者均能够提供全部维度的速度信息，不同之处在于传统算法将里程计速度投影到导航系进行量测更新，而方式2直接在车体系同时进行速度辅助和约束，形式更为简洁。

方式3导航精度均优于经典车体约束+里程计算法，尤其是航向精
度提高较大，约提高70%，克服了传统组合系统对航向信息估计效果较差的缺点。

本文针对复杂城市峡谷环境下，传统运动学约束算法难以保证GNSS长时间失效
时MEMS-SINS的导航精度问题，实现了一种车辆模型辅助的MEMS-SINS导航方法。

该方法通过引入ABS轮速传感器及车辆运动学模型，建立了基于角速度、
加速度信息的滤波方案，并增加车体系的三维速度观测量作为新的滤波量测更新。

跑车结果表明组合系统定位、测姿精度均有较大改善，证明了算法的有效性，为MEMS-SINS组合系统提供了一种简单有效的低成本导航方法。

【相关文献】
[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.。