CN117053802A - 一种基于旋转mems imu的车载导航系统定位误差减小的方法 - Google Patents

Abstract

本发明涉及一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法，包括：利用绕X轴旋转系统中MEMS IMU模块输出的数据进行惯性解算得到经度、纬度和速度，以及旋转矩阵；当GNSS当前数据的时间大于等于前一时刻MEMS IMU数据到来的时间且小于当前时刻MEMS IMU数据到来的时间(观测量有效)，利用GNSS输出的地速作为观测量，通过扩展卡尔曼滤波将惯性解算的信息与观测量信息进行信息融合输出L、λ、h、vⁿ、当GNSS当前数据的时间小于前一时刻MEMS IMU数据到来的时间或者大于当前时刻MEMS IMU数据到来的时刻则(观测量无效)进行误差状态传播。本发明使用旋转的方法提高系统的可观测性，达到导航定位精度提高的目的。

Description

一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法

技术领域

本发明涉及导航定位技术领域，尤其涉及一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法。

背景技术

全球卫星导航系(Global Navigation Satellite System，GNSS)信号受到阻挡或失效的环境下，惯性导航系统(Inertial Navigation System，INS)成为一种重要的定位解决方案。然而，INS存在误差累积的问题，这可能导致定位精度下降。INS的可观测性与车辆的机动性有着紧密的联系，即使采用辅助INS的方法，即利用里程计、激光测距仪、相机等传感器输出的信息作为外部观测，低动态下的车辆也会造成INS的误差累积。

从可观测性的角度可以解释为惯性系统中存在不可观测的误差状态量，或者多个误差状态量耦合在一起导致其可观测度低。利用PWCS可观测性分析方法，对非旋转系统与绕X轴旋转的系统的误差状态量做可观测性分析表明，绕X轴旋转系统中可观测的误差状态量以及可独立观测的误差状态量明显多余非旋转系统，其中航向角误差在以上系统中都是无法被直接观测的，但影响航向角最重要的因素也就是Z轴的陀螺仪零偏在绕X轴系统中变得可观测，因此，本次方案的提出就是为了解决传统组合导航中航向角误差累积的问题。

需要说明的是，在上述背景技术部分公开的信息只用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

发明内容

本发明的目的在于克服现有技术的缺点，提供了一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法，缓解了基于MEMS IMU的惯性系统在低动态下存在的误差累积，提高了定位精度。

本发明的目的通过以下技术方案来实现：一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法，所述定位误差消除方法包括：

利用绕X轴旋转系统中MEMS IMU模块输出的数据经过传感器系和车载平台系的旋转矩阵转换到载体系下的IMU数据，通过惯性解算得到经度λ、纬度L和高度h导航系n系下的速度vⁿ，以及导航系与载体系的旋转矩阵/>

当GNSS当前数据的时间大于等于前一时刻MEMS IMU数据到来的时间且小于当前时刻MEMS IMU数据到来的时间(观测量有效)，利用GNSS输出的地速作为观测量，通过扩展卡尔曼滤波将惯性解算的信息与观测量信息进行信息融合输出L、λ、h、vⁿ、

当GNSS当前数据的时间小于前一时刻MEMS IMU数据到来的时间或者大于当前时刻MEMS IMU数据到来的时刻则(观测量无效)进行误差状态传播。

计算所述旋转矩阵包括：旋转平台初始位置与地平线平行，旋转速度为N°/s，旋转开始时间与MEMS IMU开始记录数据的时间一致，通过记录的时间计算MEMS IMU模块旋转的角度，利用该角度计算出传感器系S系到车载平台系b系的旋转矩阵/>

所述惯性解算包括以下内容：

根据导航系下的速度微分公式进行速度与位置更新，其中，f_b表示加速度计测量的比力，/>表示导航系和载体系的旋转矩阵，/>为载体运动和地球自转引起的哥氏加速度，gⁿ是重力加速度，/>为需要扣除的有害加速度，获得运载体在导航系下的几何运动加速度，对加速度积分依次可得到速度，再次积分可以得到位置；

姿态更新：选取东-北-天地理坐标系作为捷联惯导系统的导航参考坐标系，记为n，则以n系作为参考坐标系的姿态微分方程为表示b系相对于n系的角速度，其中矩阵/>表示载体系b系相较于导航系n系的旋转矩阵；

对姿态微分方程进行变换为其中，/>为陀螺仪输出的角速度，/>为陀螺仪输出的载体系b系相对于惯性i系的角速度，/>为陀螺仪输出的载体系b系相对于惯性i系的角速度，/>表示n系相对于i系的旋转角度，它包括地球自转引起的导航系的旋转以及惯导系统在地球表面移动因地球表面弯曲引起的n系的旋转，即其中，/> R_Mh＝R_M+h，R_Nh＝R_N+h，ω_ie为地球自转角速率，L和h分别是地理纬度和高度，对/>进行积分可以得到旋转矩阵最后计算出姿态角，经过惯性解算后输出L、λ、h、vⁿ、/>

所述误差状态传播包括：。

更新每时刻的误差状态转移矩阵其中，F_rv，F_vr和F_vε子阵分别表示位置误差、速度误差和姿态误差之间的扰动关系，β_f和β_ω为加速度计和陀螺仪零偏相关的系数。

所述扩展卡尔曼滤波包括：

将15维误差状态方程表示为15维误差状态量包括导航系下位置误差(东向、北向、天向)、导航系下的速度误差(东向、北向、天向)、导航系下姿态误差(俯仰、横滚、航向)、传感器系下陀螺仪零偏三轴零偏、传感器系下加速度三轴零偏；x_k表示上一时刻的状态量，w_k表示上一时刻的系统噪声，扩展卡尔曼滤波的观测模型表示为/>H_k表示观测矩阵，v_k表示零均值噪声序列，R_k表示噪声协方差矩阵；

通过系统的误差状态转移矩阵Φ_k,k-1和当前状态估计值预测下一时刻的系统状态/>

通过系统的误差状态转移矩阵Φ_k,k-1、当前状态估计误差协方差矩阵P_k-1(+)，以及噪声驱动矩阵G_k-1和系统噪声协方差矩阵Q_k-1，预测下一时刻的状态估计误差协方差矩阵表示过程噪声；

通过状态估计误差协方差矩阵P_k和测量噪声协方差矩阵R_k的值来确定卡尔曼滤波增益K_k表示卡尔曼滤波增益；

通过预测状态和测量更新后的状态估计值，得到当前时刻的最优状态估计值

通过预测状态估计误差协方差矩阵和测量更新后的状态估计误差协方差矩阵，计算当前时刻的最优状态估计误差协方差矩阵P_k(+)＝(I-K_kH_k)P_k(-)，I表示15维单位矩阵；

重复上述步骤直到得到最接近于真实值的状态协方差矩阵P。

本发明具有以下优点：

1、一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法，使用旋转的方法提高系统的可观测性，并结合卡尔曼滤波实现导航定位误差的消除，通过本方法既不会引入车辆的线性运动但是增强了测量模块的机动性，从而有效改善了惯性导航系统误差状态量的可观测性，最终有效提高了车载导航的定位精度，提高定位精度。

2、本方法在价格低廉的车载平台上得到了验证且定位精度有大幅度的提高，充分说明该方法具备两方面的优势：其一是方案的有效性；其二是实现该方案成本的低廉。

3、该方案对GNSS信号中断时低动态下的载体应用场景具有参考价值。

附图说明

图1为本发明的流程示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下结合附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的保护范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。下面结合附图对本发明做进一步的描述。

如图1所示，本发明具体涉及一种基于旋转MEMS IMU的车载导航系统定位误差减小的方法，具体包括以下内容：

利用绕X轴旋转系统中MEMS IMU模块输出的数据经过传感器系和车载平台系的旋转矩阵转换到载体系下的IMU数据，通过惯性解算得到经度λ、纬度L和高度h导航系n系下的速度vⁿ，以及导航系与载体系的旋转矩阵/>MEMS IMU输出时间时0.01s，GNSS输出数据的时间是1s；

当GNSS当前数据的时间大于等于前一时刻MEMS IMU数据到来的时间且小于当前时刻MEMS IMU数据到来的时间(观测量有效)，利用GNSS输出的地速作为观测量，通过扩展卡尔曼滤波将惯性解算的信息与观测量信息进行信息融合输出L、λ、h、vⁿ、

当GNSS当前数据的时间小于前一时刻MEMS IMU数据到来的时间或者大于当前时刻MEMS IMU数据到来的时刻则(观测量无效)进行误差状态传播。

进一步地，计算所述旋转矩阵包括：

旋转平台初始位置与地平线平行，旋转速度为10°/s，旋转开始时间与MEMS IMU开始记录数据的时间一致，通过记录的时间计算MEMS IMU模块旋转的角度，利用该角度计算出传感器系S系到车载平台系b系的旋转矩阵

其中，MEMS IMU模块记录车辆行进的数据(旋转测量模块的加速度计测量f_s，陀螺仪测量ω_s)，利用传感器系S系到车载平台系b系的旋转矩阵就可以将s系下的数据f_s、ω_s转换到b系下f_b、ω_b。

进一步地，惯性解算包括以下内容：

(1)速度与位置更新：根据公式进行速度与位置更新，其中，f_b表示加速度计测量的比力，/>表示导航系和载体系的旋转矩阵，/>为载体运动和地球自转引起的哥氏加速度，gⁿ是重力加速度，/>为需要扣除的有害加速度，获得运载体在导航系下的几何运动加速度，对加速度积分依次可得到速度，再次积分可以得到位置；

(2)姿态更新：选取东-北-天地理坐标系作为捷联惯导系统的导航参考坐标系，记为n，则以n系作为参考坐标系的姿态微分方程为表示b系相对于n系的角速度，其中矩阵/>表示载体系b系相较于导航系的姿态阵；

对姿态微分方程进行变换为其中，/>为陀螺仪输出的角速度，/>为陀螺仪输出的载体系b系相对于惯性i系的角速度，/>表示n系相对于i系的旋转角度，它包括地球自转引起的导航系的旋转以及惯导系统在地球表面移动因地球表面弯曲引起的n系的旋转，即/>其中，R_Mh＝R_M+h，R_Nh＝R_N+h，ω_ie为地球自转角速率，L和h分别是地理纬度和高度，对/>进行积分可以得到旋转矩阵最后计算出姿态角，经过惯性解算后输出L、λ、h、vⁿ、/>

其中，误差状态传播包括：

更新每时刻的误差状态转移矩阵其中，F_rv，F_vr和F_vε子阵分别表示位置误差、速度误差和姿态误差之间的扰动关系，β_f和β_ω为加速度计和陀螺仪零偏相关的系数。

进一步地，扩展卡尔曼滤波包括：

将15维误差状态方程表示为15维误差状态量包括导航系下位置误差(东向、北向、天向)、导航系下的速度误差(东向、北向、天向)、导航系下姿态误差(俯仰、横滚、航向)、传感器系下陀螺仪零偏三轴零偏、传感器系下加速度三轴零偏；x_k表示上一时刻的状态量，w_k表示上一时刻的系统噪声，扩展卡尔曼滤波的观测模型表示为/>H_k表示观测矩阵，v_k表示零均值噪声序列，R_k表示噪声协方差矩阵；

(1)预测状态方程：通过系统的误差状态转移矩阵Φ_k,k-1和当前状态估计值预测下一时刻的系统状态/>

(2)预测误差协方差：通过系统的误差状态转移矩阵Φ_k,k-1、当前状态估计误差协方差矩阵P_k-1(+)，以及噪声驱动矩阵G_k-1和系统噪声协方差矩阵Q_k-1，预测下一时刻的状态估计误差协方差矩阵表示过程噪声；

(3)计算卡尔曼滤波增益：通过状态估计误差协方差矩阵P_k和测量噪声协方差矩阵R_k的值来确定卡尔曼滤波增益K_k表示卡尔曼滤波增益；

(4)更新状态方程：通过预测状态和测量更新后的状态估计值，得到当前时刻的最优状态估计值

(5)更新误差协方差：通过预测状态估计误差协方差矩阵和测量更新后的状态估计误差协方差矩阵，计算当前时刻的最优状态估计误差协方差矩阵P_k(+)＝(I-K_kH_k)P_k(-)，I表示15维单位矩阵；状态协方差矩阵P是状态之间的协方差组成的矩阵，对角线元素是各个状态的方差，其余元素是协方差。协方差的大小可以衡量系统模型和观测值对状态量估计的比重。每一轮的预测与更新P都会得到新的值，随着迭代的进行也越来越接近真实值。

表1位置误差的RMS统计表

参数	北向位置误差(m)	东向位置误差(m)
			X-旋转系统	4.418702	4.95238
非旋转系统	13.3092	20.9356

本发明的实验中车辆在1-50s进行了粗对准，50-150s车辆进行正常行驶。位置误差是指车辆实际位置与期望位置之间的差异，是衡量车辆控制精度和定位准确性的重要指标之一，并且位置误差均方根是位置误差的统计量，它可以更全面地反映位置误差的大小和分布情况，其中纯惯性导航在前50s内误差迅速累积，导致位置误差严重漂移。因此，上表1对非旋转系统、绕X轴旋转的系统的东向和北向的位置误差做了均方根(RMS)统计，结果表明绕X轴旋转系统北向和东向的位置误差控制在5m以内与非旋转系统相比的北向定位精度提升66.8％；东向定位精度提升了76.5％；这个结果表明，绕X轴旋转系统可以显著改善导航系统的定位精度。

以上所述仅是本发明的优选实施方式，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (5)

1.一种基于旋转MEMSIMU的车载导航系统定位误差减小的方法，其特征在于：所述车载导航系统定位误差减小的方法包括：

利用绕X轴旋转系统中MEMSIMU模块输出的数据经过传感器系和车载平台系的旋转矩阵转换到载体系下的IMU数据，通过惯性解算得到经度λ、纬度L和高度h导航系n系下的速度vⁿ，以及导航系与载体系的旋转矩阵/>

当GNSS当前数据的时间大于等于前一时刻MEMSIMU数据到来的时间且小于当前时刻MEMSIMU数据到来的时间(观测量有效)，利用GNSS输出的地速作为观测量，通过扩展卡尔曼滤波将惯性解算的信息与观测量信息进行信息融合输出L、λ、h、vⁿ、

当GNSS当前数据的时间小于前一时刻MEMSIMU数据到来的时间或者大于当前时刻MEMSIMU数据到来的时刻则(观测量无效)进行误差状态传播。

2.根据权利要求1所述的一种基于旋转MEMSIMU的车载导航系统定位误差减小的方法，其特征在于：计算所述旋转矩阵包括：旋转平台初始位置与地平线平行，旋转速度为N°/s，旋转开始时间与MEMSIMU开始记录数据的时间一致，通过记录的时间计算MEMS IMU模块旋转的角度，利用该角度计算出传感器系S系到车载平台系b系的旋转矩阵/>

3.根据权利要求1所述的一种基于旋转MEMSIMU的车载导航系统定位误差减小的方法，其特征在于：所述惯性解算包括以下内容：

根据导航系下的速度微分公式进行速度与位置更新，其中，f_b表示加速度计测量的比力，/>表示导航系和载体系的旋转矩阵，/>为载体运动和地球自转引起的哥氏加速度，gⁿ是重力加速度，/>为需要扣除的有害加速度，获得运载体在导航系下的几何运动加速度，对加速度积分依次可得到速度，再次积分可以得到位置；

姿态更新：选取东-北-天地理坐标系作为捷联惯导系统的导航参考坐标系，记为n，则以n系作为参考坐标系的姿态微分方程为 表示b系相对于n系的角速度，其中矩阵/>表示载体系b系相较于导航系n系的旋转矩阵；

对姿态微分方程进行变换为其中，/>为陀螺仪输出的角速度，/>为陀螺仪输出的载体系b系相对于惯性i系的角速度，/>为陀螺仪输出的载体系b系相对于惯性i系的角速度，/>表示n系相对于i系的旋转角度，它包括地球自转引起的导航系的旋转以及惯导系统在地球表面移动因地球表面弯曲引起的n系的旋转，即其中，/> R_Mh＝R_M+h，R_Nh＝R_N+h，ω_ie为地球自转角速率，L和h分别是地理纬度和高度，对/>进行积分可以得到旋转矩阵最后计算出姿态角，经过惯性解算后输出L、λ、h、vⁿ、/>

4.根据权利要求1所述的一种基于旋转MEMSIMU的车载导航系统定位误差减小的方法，其特征在于：所述误差状态传播包括：。

更新每时刻的误差状态转移矩阵其中，F_rv，F_vr和F_vε子阵分别表示位置误差、速度误差和姿态误差之间的扰动关系，β_f和β_ω为加速度计和陀螺仪零偏相关的系数。

5.根据权利要求4所述的一种基于旋转MEMSIMU的车载导航系统定位误差减小的方法，其特征在于：所述扩展卡尔曼滤波包括：

将15维误差状态方程表示为15维误差状态量包括导航系下位置误差(东向、北向、天向)、导航系下的速度误差(东向、北向、天向)、导航系下姿态误差(俯仰、横滚、航向)、传感器系下陀螺仪零偏三轴零偏、传感器系下加速度三轴零偏；x_k表示上一时刻的状态量，w_k表示上一时刻的系统噪声，扩展卡尔曼滤波的观测模型表示为/>H_k表示观测矩阵，v_k表示零均值噪声序列，R_k表示噪声协方差矩阵；

通过系统的误差状态转移矩阵Φ_k,k-1和当前状态估计值预测下一时刻的系统状态/>

通过系统的误差状态转移矩阵Φ_k,k-1、当前状态估计误差协方差矩阵P_k-1(+)，以及噪声驱动矩阵G_k-1和系统噪声协方差矩阵Q_k-1，预测下一时刻的状态估计误差协方差矩阵 表示过程噪声；

通过状态估计误差协方差矩阵P_k和测量噪声协方差矩阵R_k的值来确定卡尔曼滤波增益K_k表示卡尔曼滤波增益；

通过预测状态和测量更新后的状态估计值，得到当前时刻的最优状态估计值

通过预测状态估计误差协方差矩阵和测量更新后的状态估计误差协方差矩阵，计算当前时刻的最优状态估计误差协方差矩阵P_k(+)＝(I-K_kH_k)P_k(-)，I表示15维单位矩阵；

重复上述步骤直到得到最接近于真实值的状态协方差矩阵P。