CN106568449A - 一种基于mems的车辆模型辅助和约束的gnss/ins组合导航方法 - Google Patents

Abstract

本发明公开了一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航方法，结合动态约束并引入车辆ABS传感器和方向盘转角，直接在INS导航解算前对MEMS‑IMU测量值进行辅助和约束，从根本上提高了测量值的精度；在速度约束中引入了车辆模型计算的纵向速度，从而实现了对INS导航解算纵向速度的辅助；另外，传统零速修正是通过判断车辆处于静止状态时，以零速作为观测值，来限制静止条件下INS导航解引起的误差积累，而本发明中采用的速度辅助和约束方法，在车辆静止情况下，无须额外操作，或额外增加零速检测和零速修正模块，便可具有传统零速修正的功能；在低成本MEMS‑IMU传感器的基础上，直接引入四通道ABS和方向盘转角测量传感器，是一种无额外成本的提高组合系统精度方法。

Description

一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航 方法

技术领域

本发明属于导航定位技术领域，涉及一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航方法，适用于城市环境中智能车辆的导航定位。

背景技术

GNSS/INS组合导航已广泛应用于当前的智能车辆中。GNSS导航定位精度不随时间推移而改变，但易受干扰、遮挡等影响，导致精度降低甚至无法定位。惯性导航自主性强，短时间内相对定位精度很高，但误差随时间推移而积累。两者通过组合，实现功能互补。

然而在城市环境中，GNSS信号频繁受到高楼、桥梁、树木的影响，卫星数减少，多路径效应加剧，GNSS出现定位精度差，甚至长时间无法定位的情况。此时，整个导航系统的精度取决于惯性测量单元(IMU)的性能。目前，智能车辆多采用高精度IMU，例如光纤陀螺、激光陀螺等，显著增加了智能车辆的成本。微机电系统(MEMS)IMU是伴随半导体集成电路发展而成长起来的一种惯性传感器，具有体积小、成本低、功耗小等优点,是智能车辆降低成本的首选。但是如何限制GNSS信号缺失时，MEMS-IMU快速漂移引起的定位精度的降低，是当前研究的热点。

文献[1]([1]Klein I,Filin S,Toledo T.Vehicle Constraints Enhancementfor Supporting INS Navigation in Urban Environments[J].Navigation,2011,58(1):7–15.)基于车辆运动学约束，考虑车辆在城市环境道路正常行驶时，不发生侧滑和跳跃；高度变化缓慢，保持为常数；姿态角仅航向发角生变化，横滚角和俯仰角不变。综合上述三种情况，文献[1]将侧向速度、加速度，垂向速度、加速度，以及横滚角、俯仰角角速率漂移建模为均值为零的白噪声，然后作为伪测量值参与惯导解算的误差估计，这也是目前广泛采用的车辆约束方法。但是该方法忽略了车辆转弯时的侧向加速度，而且并没有直接对横滚角、俯仰角角速率进行约束，对车辆纵向速度和航向角角速率也无辅助作用。

发明内容

本发明的目的是克服已有技术的不足，结合车辆ABS传感器以及方向盘转角，提出了一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航方法。该方法无须额外增加传感器，便可显著提高MEMS-INS解算的精度，不仅可用于GNSS信号良好时，提高整个GNSS/INS组合系统的精度，也是GNSS信号缺失时抑制MEMS-IMU快速漂移的有效措施。本发明结合车辆制动防抱死系统(ABS)传感器以及方向盘转角，通过车辆运动学模型和扩展卡尔曼滤波(EKF)，直接对MEMS-IMU测量值进行辅助和约束，另外在速度约束模块中引入车辆模型所得纵向速度，改进传统约束方法对纵向速度无辅助作用的缺陷，最后将GNSS所得位置和速度，以及经过车辆模型辅助和车辆运动学约束后的INS导航解算所得的位置和速度，通过卡尔曼滤波进行组合，可以显著提高GNSS/INS组合系统的精度。

本发明的目的是通过下述技术方案实现的：

一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航方法，包括：

步骤1.ABS传感器和方向盘转角的信息融合,具体为：

基于车辆的四通道ABS传感器获得车辆4个轮子的轮速，基于方向盘转角传感器获得方向盘的绝对转角，通过EKF滤波器将车辆4个轮子的轮速和方向盘的绝对转角进行信息融合，得到航向角角速率ω和车辆后轴中心在采样时间内的位移；进而得到当前车辆的纵向速度v_bxV,航向角角速率ω_bz以及侧向加速度值f_by；

步骤2.对MEMS-IMU传感器测量值的辅助和约束，具体为:

将MEMS-IMU传感器固连于车辆后轴中心，测量车辆后轴中心相对于惯性空间三个方向的运动角速度ω_bxI,ω_byI,ω_bzI和三个方向的比力f_bxI,f_byI,f_bzI；采用步骤1得到的航向角角速率ω_bz代替导航坐标系到车辆坐标系的欧拉角微分方程中的航向角角速率，将该欧拉角微分方程中的横滚角角速率和俯仰角角速率均置为零；之后令天向加速度等于重力加速度，同时忽略MEMS-IMU速度的旋转效应和划桨效应，结合步骤1中所得纵向加速度f_by，从而直接得到车辆后轴中心相对于惯性空间y和z方向的比力计算值f_byV和f_bzV；最后，将MEMS-IMU的测量值中的ω_bxI,ω_byI,ω_bzI,f_byI,f_bzI同上述通过车辆模型以及车辆运动约束得到的ω_bxV，ω_byV，ω_bzV，f_byV，f_bzV分别进行融合，得到MEMS-IMU的测量误差，实现对MEMS-IMU测量值的补偿；

步骤3.对INS导航解算速度进行辅助和约束，具体为：

设定车辆在车体坐标系中的y、z方向的速度均为0，即v_by≈0，v_bz≈0；建立INS导航解算误差模型，从而得到卡尔曼滤波的状态方程，然后基于步骤2中修正后的MEMS-IMU测量值进行INS导航解算，将所得车辆坐标系的三方向速度[v_bx v_by v_bz]^T同步骤1中所得车辆模型在车辆坐标系速度[v_bxV 0 0]^T之差作为卡尔曼滤波算法的观测量，从而估计出采用速度辅助和约束所得的INS导航解算的误差，最后将误差值反馈回INS导航解算误差模型中，对INS导航计算相关参数进行修正；

步骤4.采用步骤3建立的INS导航解算误差模型作为系统状态方程,从而得到卡尔曼滤波的状态方程，将步骤3中INS导航解算所得的位置和速度分别与GNSS解算所得位置和速度求差值，并将所得两个差值作为卡尔曼滤波的观测量，从而实现对INS导航解算误差的估计，最后将误差值反馈回INS导航计算中。

在所述步骤3中，对INS导航解算进行高度约束，具体为:采用步骤3建立的INS导航解算误差模型作为系统状态方程，将INS导航解算所得高度h和垂向速度v_D分别同设定高度h_c和0作差，所得两个差值作为卡尔曼滤波算法观测量，从而估计出采用高度约束所得的INS导航解算的误差，最后将误差值反馈回INS导航计算中。

本发明与现有技术相比优点在于：

(1)与传统车辆动态约束方法不同，本发明结合动态约束并引入车辆ABS传感器和方向盘转角，直接在INS导航解算前对MEMS-IMU测量值进行辅助和约束，从根本上提高了测量值的精度；

(2)改进了传统速度约束方法。传统速度约束只考虑横向、垂向速度，对纵向速度无能为力，本发明在速度约束中引入了车辆模型计算的纵向速度，从而实现了对INS导航解算纵向速度的辅助。另外，传统零速修正是通过判断车辆处于静止状态时，以零速作为观测值，来限制静止条件下INS导航解引起的误差积累，而本发明中采用的速度辅助和约束方法，在车辆静止情况下，无须额外操作，或额外增加零速检测和零速修正模块，便可具有传统零速修正的功能。

(3)本发明无须额外增加传感器，在低成本MEMS-IMU传感器的基础上，直接引入车辆自带四通道ABS和方向盘转角测量传感器，是一种无额外成本的提高组合系统精度的方法。

附图说明

图1为本发明方法的整体框图；

图2为车辆模型辅助和约束方法具体实施的流程图；

图3为阿克曼转向原理图；

图4为本发明所用车辆转弯时的运动模型图。

具体实施方式

下面结合附图和理论分析，对本发明进行详细描述。

本发明的一种车辆模型辅助和约束的低成本组合导航方法的结构如图1所示，传感器主要包括四通道ABS1、方向盘转角2、MEMS-IMU3，GNSS4；算法部分有车辆数据融合5，用于辅助和约束MEMS-IMU传感器测量值的卡尔曼滤波算法6，INS导航解算7，高度约束8，速度辅助和约束9，GNSS/INS组合的卡尔曼滤波算法10。

四通道ABS1提供四个轮速传感器，方向盘转角传感器2提供方向盘的绝对转角，然后通过车辆数据融合5计算出当前车辆的航向角角速率以及纵向速度。该航向角角速率以及纵向速度，结合城市车辆运行特性，可以通过卡尔曼滤波6来辅助和约束传感器MEMS-IMU3的测量值，从而有效提高三轴角速率和纵向、垂向比力的精度。之后，利用矫正后的角速率和比力进行INS导航解算7。高度约束8用于限制导航解算中垂向高度的误差积累。速度辅助和约束9利用车辆数据融合5所得的车身纵向速度进行辅助，利用车辆不侧滑和不跳跃条件进行约束。最后，GNSS4提供的位置和速度，与经过辅助和约束后的INS导航解算所得的位置和速度，通过卡尔曼滤波10进行组合，最终得到提高精度后的位置、速度、姿态信息。

图2所示为本发明的具体实施过程：

步骤1.ABS传感器和方向盘转角的信息融合：基于车辆的四通道ABS传感器获得车辆4个轮子的轮速，基于方向盘转角传感器获得方向盘的绝对转角，通过EKF滤波器将车辆4个轮子的轮速和方向盘的绝对转角行信息融合，得到航向角角速率ω和车辆后轴中心在采样时间T内的位移ΔS；进而得到当前车辆的纵向速度v_bxV,航向角角速率ω_bz以及侧向加速度值f_by。

目前大多数车辆装有四通道ABS，可以提供四个轮速传感器；方向盘转角传感器可以测得方向盘的绝对转角，然后通过车辆基本运动模型可以冗余的计算出航向角角速率以及当前车辆的纵向速度。具体原理如下：

根据阿克曼转向原理，如图3所示，以及车辆转弯时的运动模型，如图4所示，可以得到以下几何等式：

其中，本发明车体坐标系(b系)以车辆后轴中心为坐标原点，车辆纵轴为x轴，后轴为y轴，z轴同x轴和y轴构成右手直角坐标系，即垂直指向地面。ω为航向角角速率；ΔS为后轴中心即b系中心在采样时间T内的位移；L为车身轴距；2B为车辆后轴长度；ΔS_RL、ΔS_RR、ΔS_FL、ΔS_FR分别为采样时间内左后轮、右后轮、左前轮、右前轮在采样时间T内的位移，分别由四个ABS传感器测得；δ为虚拟前轮转角，正比于方向盘传感器测得的转角；δ_L为左前轮转角；δ_R为右前轮转角；δ_L、δ_R可由式(2)计算得到：

通过式(1)可冗余的计算出[ΔS,ω]，本发明采用文献[2]([2]Bonnifait P,Bouron P,Crubille P,et al.Data fusion of four ABS sensors and GPS for anenhanced localization of car-like vehicles[J].2001,2:1597-1602vol.2.)提出的里程仪EKF对传感器测得的值进行融合。融合系统的状态方程和量测方程为：

其中k代表离散时间点；建模为白噪声；为量测噪声；H为观测矩阵，由下式计算所得：

通过EKF对传感器信息的融合，我们能够得到当前车辆的纵向速度v_bxV,航向角角速率ω_bz以及侧向加速度值f_by：

步骤2.对MEMS-IMU传感器测量值的辅助和约束:将MEMS-IMU传感器固连于车辆后轴中心，MEMS-IMU传感器可以提供车辆后轴中心相对于惯性空间三个方向的运动角速度ω_bxI,ω_byI,ω_bzI和三个方向的比力f_bxI,f_byI,f_bzI；利用步骤1中车辆运动模型计算所得航向角角速率ω_bz、以及车辆运动学约束所得横滚角角速率几乎为零、俯仰角角速率几乎为零，将这些值带入到欧拉角(导航坐标系到车辆坐标系的欧拉角)微分方程中，同样可以得到车辆后轴中心相对于惯性空间三个方向的运动角速度计算值ω_bxV,ω_byV,ω_bzV，具体过程为用步骤1得到的航向角角速率代替微分方程中的航向角角速率，实现对航向角计算的辅助，将微分方程中的横滚角、俯仰角的角速率置为零，实现横滚角、俯仰角的约束；之后利用步骤1中车辆运动模型计算所得纵向加速度f_by，车辆运动学约束所得天向加速度几乎为重力加速度，同时考虑对于低精度MEMS-IMU可以忽略速度的旋转效应和划桨效应，从而可以直接得到车辆后轴中心相对于惯性空间y，z方向的比力计算值f_byV，f_bzV；最后，将MEMS-IMU的测量值ω_bxI,ω_byI,ω_bzI,f_byI,f_bzI同通过车辆模型以及车辆运动约束得到的ω_bxV，ω_byV，ω_bzV，f_byV，f_bzV进行融合，得到MEMS-IMU的测量误差，实现对IMU测量值的补偿。

详细过程如下：i.建立IMU测量值的误差模型

选取IMU陀螺误差和加速度计y，z方向比力误差作为状态变量：

δω_bxI,δω_byI,δω_bzI为MEMS-IMU所得车辆后轴中心相对于惯性空间三个方向的运动角速度误差，δf_byI,δf_bzI为MEMS-IMU所得车辆后轴中心相对于惯性空间y，z方向的比力误差。建立其误差模型为:

其中为5维的白噪声向量，IMU测量误差可建模为相关时间为τ的一阶马尔科夫过程，可得系统转移矩阵F为：

τ_bgx，τ_bgy，τ_bgz为三轴陀螺测量误差的马尔科夫相关时间，τ_bay，τ_baz为加速度计y，z方向测量误差的马尔科夫相关时间。

ii.构建观测值

三轴的角速率中只有航向角角速率发生变化：

其中，α,χ,η分别为横滚角、俯仰角、航向角；n为北东地导航坐标系；为n系到b系的方向余弦矩阵；是n系相对于惯性空间的绝对运动角速度；是b系相对于惯性空间的绝对运动角速度。为小量，这里忽略其影响，并将车辆模型计算得到的航向角角速率ω_bz带入式(9)中，即用ω_bz代替得：

整理后有：

考虑到高度几乎不发生变化以及式(5)得到的y方向的加速度，同时对于低精度MEMS-IMU可以忽略速度的旋转效应和划桨效应，从而可以直接得到b系中y、z方向的比力为：

式(11)和式(12)构成了卡尔曼滤波的观测向量，即:

其中为均值为0的5维白噪声向量，H为观测矩阵。通过式(7)和(13)，可以采用标准卡尔曼滤波方法估计MEMS-IMU传感器测量值的误差，并将修正后的测量值直接应用于INS导航解算中，从而提高解算精度。

步骤3.对INS导航解算速度进行辅助和约束：对速度的约束主要依据是车辆在正常行驶时一般不发生侧滑和跳跃，所以b系y、z方向的速度几乎为0，即v_by≈0，v_bz≈0，对车体速度的辅助主要是利用步骤1所得v_bxV对导航解算结果中b系x方向的速度v_bx进行辅助。具体过程是首先建立INS导航解算误差模型，从而得到卡尔曼滤波的状态方程，然后基于步骤2中修正后的IMU测量值进行INS导航解算，将所得b系速度[v_bx v_by v_bz]^T同步骤1中所得车辆模型在b系速度[v_bxV 0 0]^T之差作为卡尔曼滤波算法的观测量，从而估计出采用速度辅助和约束所得的INS导航解算的误差，最后将误差值反馈回INS导航计算中，对INS导航计算相关参数进行修正。

详细过程如下：

i.建立INS导航解算误差模型

INS导航解算误差模型采用常用的15维误差状态变量：

式中，分别为位置误差、速度误差、平台姿态误差角、加速度计和陀螺慢变漂移。离散化后的系统状态方程为：

W_k为过程噪声方差阵；状态转移矩阵中各子阵计算过程如下：

为n系相对于地球坐标系的角速率，为b系中的比力，ΔT为离散时间间隔，g为重力加速度，R为地球半径。

ii.构建观测值

b系中的速度可用下式进行计算：

其扰动方程为：

其中为惯导解算所得n系中的速度，为其斜对称矩阵。构造观测方程为：

式中为ABS传感器测量噪声，为伪测量值噪声。在实际情况中，b系y、z方向速度的速度并不完全为0，用来补偿由于假设b系中y、z方向速度为0所应起的和实际情况的矛盾。H为观测矩阵，通过式(18)可以确定H的具体计算公式如下：

对式(15)和(19)采用标准卡尔曼滤波方法，可以实时估计出采用速度辅助和约束所得的INS导航解算的误差，并将估计的误差值反馈到导航解算中，对相关参数进行修正。

步骤4.对INS导航解算高度的约束:高度约束认为车辆在城市环境行驶时，高度几乎保持不变,所以假设有h＝h_c，具体过程是采用步骤3建立的INS导航解算误差模型作为系统状态方程，将INS导航解算所得高度和垂向速度[h v_D]^T同车辆约束所得高度和垂向速度[h_c 0]^T作差，两者差值作为卡尔曼滤波算法观测量，从而估计出采用高度约束所得的INS导航解算的误差，最后将误差值反馈回INS导航计算中。

详细过程为：

构建观测方程为：

式中为伪测量值噪声。在实际情况中，h、v_D并不完全等于0， 用来补偿由于假设h为常值以及v_D为0所引起的和实际情况的矛盾。H为观测矩阵，状态方程仍采用式(15)所示状态方程，所以易得H的具体计算公式如下：

对式(15)和(21)采用标准卡尔曼滤波方法，可以实时估计出采用高度约束所得的INS导航解算的误差，并将估计的误差值反馈到导航解算中，对相关参数进行修正。

步骤5.最后将GNSS所得位置和速度以及经过车辆模型辅助和车辆运动学约束后的INS导航解算所得的位置和速度通过卡尔曼滤波进行组合。具体过程是采用步骤3建立的INS导航解算误差模型作为系统状态方程,取INS与GNSS对应位置、速度之差作为观测量，从而实现对INS导航解算误差的估计，最后将误差值反馈回INS导航计算中。

详细过程为：

构建观测方程如下：

式中，为六维白噪声向量，其方差分别为σ_pN,σ_pE,σ_pD,σ_vN,σ_vE,σ_vD，具体计算公式如下：

上式中，σ_p为伪距测量误差，HDOP_N为GNSS北向精度因子，HDOP_E为GNSS东向精度因子，VDOP为GNSS垂直分量精度因子，这些值均可由GNSS接收机提供。

式(23)中，H为观测矩阵，系统状态方程仍采用式(15)所示INS导航解算误差模型，所以易得H的具体计算公式如下：

对式(15)和(23)采用卡尔曼滤波方法，可以估计出INS导航解算的误差，然后对INS导航解算进行校正，并将该误差反馈到INS导航解算中，对相关参数进行修正。

本发明所述的一种车辆模型辅助和约束的低成本组合导航方法，在MEMS-IMU传感器的基础上，引入了四通道ABS传感器和车轮转角，并结合车辆模型显著提高了INS导航解算所需的角速率和比力的精度；其次，利用车辆模型以及城市环境车辆运行特性，对导航解算所得高度和速度进行辅助和约束；最后，将GNSS所得位置和速度，以及经过车辆模型辅助和车辆运动学约束后的INS导航解算所得的位置和速度，通过卡尔曼滤波进行组合，从而在不增加成本的基础上使组合系统获得更高的精度。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航方法，其特征在于，包括：

步骤1.ABS传感器和方向盘转角的信息融合,具体为：

基于车辆的四通道ABS传感器获得车辆4个轮子的轮速，基于方向盘转角传感器获得方向盘的绝对转角，通过EKF滤波器将车辆4个轮子的轮速和方向盘的绝对转角进行信息融合，得到航向角角速率ω和车辆后轴中心在采样时间内的位移；进而得到当前车辆的纵向速度v_bxV,航向角角速率ω_bz以及侧向加速度值f_by；

步骤2.对MEMS-IMU传感器测量值的辅助和约束，具体为:

将MEMS-IMU传感器固连于车辆后轴中心，测量车辆后轴中心相对于惯性空间三个方向的运动角速度ω_bxI,ω_byI,ω_bzI和三个方向的比力f_bxI,f_byI,f_bzI；采用步骤1得到的航向角角速率ω_bz代替导航坐标系到车辆坐标系的欧拉角微分方程中的航向角角速率，将该欧拉角微分方程中的横滚角角速率和俯仰角角速率均置为零；之后令天向加速度等于重力加速度，同时忽略MEMS-IMU速度的旋转效应和划桨效应，结合步骤1中所得纵向加速度f_by，从而直接得到车辆后轴中心相对于惯性空间y和z方向的比力计算值f_byV和f_bzV；最后，将MEMS-IMU的测量值中的ω_bxI,ω_byI,ω_bzI,f_byI,f_bzI同上述通过车辆模型以及车辆运动约束得到的ω_bxV,ω_byV,ω_bzV,f_byV,f_bzV分别进行融合，得到MEMS-IMU的测量误差，实现对MEMS-IMU测量值的补偿；

步骤3.对INS导航解算速度进行辅助和约束，具体为：

设定车辆在车体坐标系中的y、z方向的速度均为0，即v_by≈0，v_bz≈0；建立INS导航解算误差模型，从而得到卡尔曼滤波的状态方程，然后基于步骤2中修正后的MEMS-IMU测量值进行INS导航解算，将所得车辆坐标系的三方向速度[v_bx v_by v_bz]^T同步骤1中所得车辆模型在车辆坐标系速度[v_bxV 0 0]^T之差作为卡尔曼滤波算法的观测量，从而估计出采用速度辅助和约束所得的INS导航解算的误差，最后将误差值反馈回INS导航解算误差模型中，对INS导航计算相关参数进行修正；

步骤4.采用步骤3建立的INS导航解算误差模型作为系统状态方程,从而得到卡尔曼滤波的状态方程，将步骤3中INS导航解算所得的位置和速度分别与GNSS解算所得位置和速度求差值，并将所得两个差值作为卡尔曼滤波的观测量，从而实现对INS导航解算误差的估计，最后将误差值反馈回INS导航计算中。

2.如权利要求1所述的一种基于MEMS的车辆模型辅助和约束的GNSS/INS组合导航方法，其特征在于，在所述步骤3中，对INS导航解算进行高度约束，具体为:采用步骤3建立的INS导航解算误差模型作为系统状态方程，将INS导航解算所得高度h和垂向速度v_D分别同设定高度h_c和0作差，所得两个差值作为卡尔曼滤波算法观测量，从而估计出采用高度约束所得的INS导航解算的误差，最后将误差值反馈回INS导航计算中。