CN115164886B - 车载gnss/ins组合导航系统比例因子误差补偿方法 - Google Patents

Abstract

本发明公开了一种车载GNSS/INS组合导航系统比例因子误差补偿方法，属于导航技术领域。具体步骤为：一，建立INS误差方程；二，建立GNSS/INS组合导航系统模型；三，将比例因子误差在GNSS/INS组合导航系统模型方程中表征；四，将包含比例因子误差的GNSS/INS组合导航系统模型解耦，得到并行的导航状态量估计器和比例因子误差观测器，用来同时估计导航状态量和比例因子误差。本发明提出的基于卡尔曼滤波的导航状态量和比例因子误差解耦估计算法，可以实现对于比例因子误差的估计，进而实时补偿导航状态量的估计误差。仿真结果表明，本方法能够提高GNSS/INS组合导航系统精度；特别是能够缓解在GNSS信号失效情况下纯惯导解算的发散现象。

Description

车载GNSS/INS组合导航系统比例因子误差补偿方法

技术领域

本发明属于导航技术领域，具体涉及一种车载GNSS/INS组合导航系统比例因子误差补偿方法。

背景技术

全球导航卫星系统(Global Navigation Satellite System，GNSS)具有定位精度高，体积小，误差不随时间累积，易与其它传感器融合等优点，其缺点在于信号容易受到外界环境影响，如处于地下室或高楼信号则被遮挡，此时GNSS失效。惯性导航系统(InertialNavigation System，INS)能够提供比GNSS更高频率的位置、速度和姿态的信息，能够免于外界信号干扰和遮挡，但导航精度容易受到惯性传感器的误差影响，长时间单独使用会使导航误差急剧增加。GNSS/INS组合导航系统利用惯性测量单元(Inertial MeasurementUnit，IMU)加速度计和陀螺仪的输出解算出导航结果，并在基于INS误差模型的卡尔曼滤波器(Kalman Filter，KF)中通过两子系统导航结果的差值校正INS导航误差，改善整体导航性能。但是当GNSS失效时，卡尔曼滤波器得不到观测量，无法正常工作，组合导航系统只能通过INS解算得到导航结果。

为降低GNSS失效对组合导航精度产生的不利影响，目前采用的方法主要有：(1)增加视觉传感器、里程计以及磁罗盘等辅助传感器，提供卡尔曼滤波器所需的导航状态观测量。显然，辅助传感器会增加硬件成本，且无法像GNSS一样提供绝对位置信息。(2)采用地图匹配算法来增强GNSS，修正INS导航误差。该算法依赖于预制的高精度地图，无法实时更新。(3)运用神经网络算法，通过训练建立良好的经验模型来预测GNSS信号丢失时速度和位置误差，并提供给滤波器对INS导航误差进行估计校正，减小导航误差的发散。神经网络预测方法依赖大量数据训练的模型，不便于工程应用。

在GNSS失效时，影响INS解算精度的主要因素是IMU的测量误差。如果对IMU测量误差进行合理建模并集成到组合导航误差模型状态方程中作为系统状态量进行估计，反馈校正传感器误差，那么导航解算精度将得以提高。由于这种方法对于GNSS有效时组合导航系统的精度提升同样有帮助，所以被广泛采用。IMU测量误差可分为两个部分：确定性误差和随机误差。确定性误差主要包括常值误差和比例因子误差等。卡尔曼滤波的计算量近似与状态量维数的3次方成正比。为兼顾模型精确度与计算量，通常采用“随机常值+白噪声”形式对IMU误差进行建模，不再计入比例因子等误差。而实际上，低成本IMU的加工工艺和工作温度变化都会产生比例因子误差。未得到良好标定的传感器会存在较大的剩余比例因子误差。一些消费级IMU的比例因子误差甚至可以达到10％。不同于随机常值误差，虽然比例因子误差为常数，但是其引起的误差大小并不是固定的，而是与IMU的测量值有关。由于传感器比例因子误差的影响，使得IMU误差建模不准确，系统中会存在模型误差。

以上各种方法均存在一定的问题。因此，研究一种能够对比例因子误差进行估计与补偿使得模型精度得以提高同时又不会导致计算量急剧增加的GNSS/INS组合导航卡尔曼滤波算法具有积极意义。

发明内容

为了解决IMU模型中比例因子误差的识别、估计和补偿的问题，本发明提供一种车载GNSS/INS组合导航系统比例因子误差补偿方法。

本发明采用的技术方案是：

一种车载GNSS/INS组合导航系统比例因子误差补偿方法，包括如下步骤：

步骤一，建立INS误差方程，其表达式为：

其中，ψⁿ、δVⁿ和δPⁿ分别为导航坐标系(n系)下的姿态误差、速度误差和位置误差；fⁿ为导航坐标系下的比力；为车身坐标系(b系)转换到导航坐标系的方向余弦矩阵；/>为地球自转角速度在导航坐标系的投影，/>为导航坐标系相对于地球坐标系的角速度在导航坐标系的投影，/>为导航坐标系相对于惯性坐标系的角速度在导航坐标系的投影；δf^b和δω^b分别为车身系下加速度计和陀螺仪的测量误差；

步骤二，建立GNSS/INS组合导航系统模型

加速度计和陀螺仪误差模型分别为：

δf＝b_f+ε_f

δω＝b_ω+ε_ω

式中，

其中，δf和δω分别为加速度计和陀螺仪的误差；b_f和b_ω分别为加速度计和陀螺仪的随机常值误差；ε_f和ε_ω分别表示加速度计和陀螺仪中的随机噪声；进而与INS误差方程结合，得到基于卡尔曼滤波的GNSS/INS组合导航系统模型；

步骤三，将比例因子误差在GNSS/INS组合导航系统模型方程中表征

当加速度计和陀螺仪测量中存在比例因子误差时，其误差模型可分别表示为：

其中，和/>分别为加速度计和陀螺仪中由比例因子误差产生的模型误差；s_f和s_ω分别表示加速度计和陀螺仪中的比例因子误差，为常数；f和ω分别表示加速度计和陀螺仪的测量值；

步骤四，将包含比例因子误差的GNSS/INS组合导航系统模型解耦，得到并行的导航状态量估计器和比例因子误差观测器，用来同时估计导航状态量和比例因子误差。

进一步地，步骤二中，

所述基于卡尔曼滤波的GNSS/INS组合导航系统模型方程为：

式中，w和v为零均值且不相关的高斯白噪声；

y＝[V_INS-V_GNSS P_INS-P_GNSS]^T＝[δV_3×1 δP_3×1]^T；

C＝[0_6×3 I_6×6 0_6×6]；

其中，ψ、δV、δP分别为姿态误差、速度误差和位置误差，b_f和b_ω分别为加速度计和陀螺仪的随机常值误差，V_INS和P_INS分别为INS输出的速度和位置，V_GNSS和P_GNSS分别为GNSS输出的速度和位置，0为零矩阵，I为单位矩阵。

进一步地，步骤三，将比例因子误差在GNSS/INS组合导航系统模型方程中表征，并对方程进行离散化处理，得到：

其中，I为单位矩阵；A_k为状态转移矩阵，由矩阵F离散化得到；b_k为陀螺仪和加速度计比例因子误差向量；B_k表示模型误差进入状态方程的传播矩阵；其余符号意义同前，只是转为离散化形式；并且有：

A_k＝(I+FΔt)；

其中，Δt为系统采样周期，ω^b和f ^b分别为车身系下陀螺仪和加速度计的测量值。

进一步地，步骤四中，导航状态量估计器迭代过程为：

当k＝0时，初始化：

式中，为状态量初始值；/>为状态量误差协方差初始值；x₀为状态量真实值，

当k＝1,2,3……时，

状态量先验估计值更新：式中，/>是状态量先验估计值，/>是状态量后验估计值；

状态量先验误差协方差更新：式中，/>是导航状态量估计器的状态量先验误差协方差，/>是导航状态量估计器的状态量后验误差协方差，Q_k-1是上述组合导航系统模型方程中噪声w_k-1的协方差；

计算卡尔曼增益：式中，K_x,k是导航状态量估计器的卡尔曼增益，R_k是上述组合导航系统模型方程中噪声v_k的协方差；

新息更新：式中，r_x,k是导航状态量估计器的新息；

状态量后验估计值更新：

状态量后验误差协方差更新：

进一步地，步骤四中，比例因子误差观测器迭代过程为：

当k＝0时，初始化：

式中，为比例因子误差量初始值；U₀为迭代矩阵初始值；P_b,0为比例因子误差量的误差协方差初始值；b₀为比例因子误差量真实值；

当k＝1,2,3……时，

先验估计值更新：式中，/>是比例因子误差量先验估计值，/>是比例因子误差量后验估计值；

更新算法迭代矩阵：式中，V_k、U_k、S_k是为算法迭代计算方便设置的中间环节矩阵，无特殊含义；

误差协方差矩阵更新：式中，P_b,k是比例因子误差量的误差协方差；

计算卡尔曼增益：式中，K_b,k是比例因子误差观测器的卡尔曼增益；

新息更新：式中，r_b,k是比例因子误差观测器的新息，

后验估计值更新：

进一步地，步骤四中，

每一次迭代的最后，需用比例因子误差估计值对导航状态量估计值进行补偿，从而得到导航状态量的最优估计为：

式中，为导航状态量的最优估计值。

与现有技术相比，本发明提供了一种车载GNSS/INS组合导航系统比例因子误差补偿方法，具有以下有益效果：

本发明保留了GNSS和INS组合导航的优点，导航状态量估计器融合了两系统的信息，从而提高导航精度；同时，并行的比例因子误差观测器可以估得IMU测量误差中的比例因子误差，及时对IMU测量误差方程进行修正，避免因存在模型误差使得导航状态量估计不准确，造成导航精度下降。本发明实现了比例因子模型误差的识别和估计，并且能够对导航状态量进行误差补偿，得到更为准确的导航状态量估计；同时，通过模型解耦降低了导航滤波器的计算量。

附图说明

图1为本发明车载GNSS/INS组合导航系统比例因子误差补偿方法实施例中导航状态量和比例因子误差并行估计算法流程图。

图2为本发明车载GNSS/INS组合导航系统比例因子误差补偿方法实施例中GNSS信号失效前导航仿真结果。

图3为本发明车载GNSS/INS组合导航系统比例因子误差补偿方法实施例中比例因子误差估计结果。

图4为本发明车载GNSS/INS组合导航系统比例因子误差补偿方法实施例中GNSS信号失效期间导航仿真结果。

图5为本发明车载GNSS/INS组合导航系统比例因子误差补偿方法实施例中GNSS信号恢复后导航仿真结果。

具体实施方式

下面结合具体实施例对本发明作进一步描述。实施例用来解释本发明，但不作为对本发明的限定。

本发明提供了一种车载GNSS/INS组合导航系统比例因子误差补偿方法，包括如下步骤：

步骤一，建立INS误差方程。

INS误差方程表达式为：

其中，ψⁿ、δVⁿ和δPⁿ分别为导航坐标系(n系)下的姿态误差、速度误差和位置误差；fⁿ为导航坐标系下的比力；为车身坐标系(b系)转换到导航坐标系的方向余弦矩阵；/>为地球自转角速度在导航坐标系的投影，/>为导航坐标系相对于地球坐标系的角速度在导航坐标系的投影，/>为导航坐标系相对于惯性坐标系的角速度在导航坐标系的投影；δf^b和δω^b分别为车身系下加速度计和陀螺仪的测量误差。

步骤二，建立GNSS/INS组合导航系统模型。

采用“随机常值+白噪声”形式对IMU误差进行建模，即加速度计和陀螺仪误差模型分别为：

δf＝b_f+ε_f

δω＝b_ω+ε_ω

式中，

δf和δω分别为加速度计和陀螺仪的误差；b_f和b_ω分别为加速度计和陀螺仪的随机常值误差；ε_f和ε_ω分别表示加速度计和陀螺仪中的随机噪声。

进而与INS误差方程结合，得到基于卡尔曼滤波的GNSS/INS组合导航系统模型方程为：

式中，w和v为零均值且不相关的高斯白噪声；

y＝[V_INS-V_GNSS P_INS-P_GNSS]^T＝[δV_3×1 δP_3×1]^T；

C＝[0_6×3 I_6×6 0_6×6]。

其中，ψ、δV、δP分别为姿态误差、速度误差和位置误差，b_f和b_ω分别为加速度计和陀螺仪的随机常值误差，V_INS和P_INS分别为INS输出的速度和位置，V_GNSS和P_GNSS分别为GNSS输出的速度和位置，0为零矩阵，I为单位矩阵。

这种以“随机常值+白噪声”形式对IMU误差进行建模的基于卡尔曼滤波的组合导航算法即为常用的GNSS/INS组合导航卡尔曼滤波算法，其中没有考虑比例因子误差。

步骤三，将比例因子误差在GNSS/INS组合导航系统模型方程中表征。

当加速度计和陀螺仪测量中存在比例因子误差时，其误差模型可分别表示为：

其中，和/>分别为加速度计和陀螺仪中由比例因子误差产生的模型误差；s_f和s_ω分别表示加速度计和陀螺仪中的比例因子误差，为常数；f和ω分别表示加速度计和陀螺仪的测量值。

将比例因子误差在GNSS/INS组合导航系统模型方程中表征，并对方程进行离散化处理，得到：

其中，I为单位矩阵；A_k为状态转移矩阵，由矩阵F离散化得到；b_k为陀螺仪和加速度计比例因子误差向量；B_k表示模型误差进入状态方程的传播矩阵；其余符号意义同前，只是转为离散化形式；并且有：

A_k＝(I+FΔt)；

其中，Δt为系统采样周期，ω^b和f ^b分别为车身系下陀螺仪和加速度计的测量值。

步骤四，将包含比例因子误差的GNSS/INS组合导航系统模型解耦，得到并行的导航状态量估计器和比例因子误差观测器，用来同时估计导航状态量和比例因子误差。

在状态量维数较高的情况下，这种将单一卡尔曼滤波器解耦为两个并行滤波器的方法可以显著降低计算量。

(1)导航状态量估计器迭代过程：

当k＝0时，初始化：

式中，为状态量初始值；/>为状态量误差协方差初始值；x₀为状态量真实值。

当k＝1,2,3……时，

状态量先验估计值更新：式中，/>是状态量先验估计值，/>是状态量后验估计值。

状态量先验误差协方差更新：式中，/>是导航状态量估计器的状态量先验误差协方差，/>是导航状态量估计器的状态量后验误差协方差，Q_k-1是上述组合导航系统模型方程中噪声w_k-1的协方差。

计算卡尔曼增益：式中，K_x,k是导航状态量估计器的卡尔曼增益，R_k是上述组合导航系统模型方程中噪声v_k的协方差。

新息更新：式中，r_x,k是导航状态量估计器的新息。

状态量后验估计值更新：

状态量后验误差协方差更新：

(2)比例因子误差观测器迭代过程：

当k＝0时，初始化：

式中，为比例因子误差量初始值；U₀为迭代矩阵初始值；P_b,0为比例因子误差量的误差协方差初始值；b₀为比例因子误差量真实值。

当k＝1,2,3……时，

先验估计值更新：式中，/>是比例因子误差量先验估计值，/>是比例因子误差量后验估计值。

更新算法迭代矩阵：式中，V_k、U_k、S_k是为算法迭代计算方便设置的中间环节矩阵，无特殊含义。

误差协方差矩阵更新：式中，P_b,k是比例因子误差量的误差协方差。

计算卡尔曼增益：式中，K_b,k是比例因子误差观测器的卡尔曼增益。

新息更新：式中，r_b,k是比例因子误差观测器的新息。

后验估计值更新：

每一次迭代的最后，需用比例因子误差估计值对导航状态量估计值进行补偿，从而得到导航状态量的最优估计为：

式中，为导航状态量的最优估计值。

以上提供了导航状态量与比例因子误差并行估计及比例因子误差补偿方法，其流程图如图1所示。为方便起见，将本发明所提出的算法简称为BCKF(Bias-corrected KalmanFilter)算法。

下面通过仿真将BCKF算法与常用的基于卡尔曼滤波的不考虑比例因子误差的组合导航状态量估计算法(以下简称为KF算法)进行对比。假设IMU测量中存在1％的比例因子误差。仿真过程分为三段，第一段为前2000s，GNSS信号正常；第二段为2000s到2060s期间，GNSS信号失效，系统进行纯惯导解算；第三段为2060s到最后，GNSS信号恢复，系统重新进行组合导航解算。

图2为GNSS信号失效前的导航解算结果。可以看到，在GNSS信号有效的前提下，BCKF算法和KF算法均能对导航误差状态量进行较好地估计。由于BCKF算法能够实现对比例因子误差的有效估计和补偿，因此对于水平方向速度与位置估计的误差略小于KF算法，在航向角的估计上优势则比较明显。图3表明BCKF算法能够追踪比例因子误差至1％的真实值，即可以有效估计加速度计和陀螺仪的比例因子误差。

图4是系统在GNSS信号失效期间进行纯惯导解算的结果。该阶段BCKF算法的水平方向最大速度和位置误差分别是0.16m/s和3.96m，航向角误差是1.38′，远小于KF算法的0.28m/s、12.96m和24.14′。表明，BCKF算法在经过第一阶段前期对于加速度计和陀螺仪的比例因子误差的有效估计与补偿后，在GNSS信号失效期间，系统进行纯惯导解算时，能够维持较好的导航精度。

图5为GNSS信号恢复后的系统导航解算结果。可以看到，BCKF算法的导航精度依然优于KF算法，特别是BCKF的航向角误差曲线与水平方向速度误差曲线相比于KF算法波动均显著减小，并且误差数值也同样显著减小。

本发明从卡尔曼滤波模型误差的角度展开研究，提出了一种可以实现车载GNSS/INS组合导航系统中比例因子误差实时估计与补偿的方法。以上仿真结果表明，该算法对车载导航系统的复杂工况有较好的适应性，可以实现比例因子误差的实时在线估计，并且提高导航系统状态量估计的准确度。应予着重指出的是，由于在车辆运动过程中，GNSS信号经常会被遮挡，此时组合导航系统只能进行纯惯导解算，本发明所提出的方法对于此种工况下系统导航精度的改善能够提供一定的帮助。

以上所述仅是本发明的优选实施方式。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (6)

1.一种车载GNSS/INS组合导航系统比例因子误差补偿方法，其特征在于，包括如下步骤：

步骤一，建立INS误差方程，其表达式为：

其中，ψⁿ、δVⁿ和δPⁿ分别为导航坐标系下的姿态误差、速度误差和位置误差，导航坐标系为n系；fⁿ为导航坐标系下的比力；为车身坐标系转换到导航坐标系的方向余弦矩阵，车身坐标系为b系；/>为地球自转角速度在导航坐标系的投影，/>为导航坐标系相对于地球坐标系的角速度在导航坐标系的投影，/>为导航坐标系相对于惯性坐标系的角速度在导航坐标系的投影；δf^b和δω^b分别为车身系下加速度计和陀螺仪的测量误差；

步骤二，建立GNSS/INS组合导航系统模型

加速度计和陀螺仪误差模型分别为：

δf＝b_f+ε_f

δω＝b_ω+ε_ω

式中，

其中，δf和δω分别为加速度计和陀螺仪的误差；b_f和b_ω分别为加速度计和陀螺仪的随机常值误差；ε_f和ε_ω分别表示加速度计和陀螺仪中的随机噪声；进而与INS误差方程结合，得到基于卡尔曼滤波的GNSS/INS组合导航系统模型；

步骤三，将比例因子误差在GNSS/INS组合导航系统模型方程中表征

当加速度计和陀螺仪测量中存在比例因子误差时，其误差模型可分别表示为：

其中，和/>分别为加速度计和陀螺仪中由比例因子误差产生的模型误差；s_f和s_ω分别表示加速度计和陀螺仪中的比例因子误差，为常数；f和ω分别表示加速度计和陀螺仪的测量值；

步骤四，将包含比例因子误差的GNSS/INS组合导航系统模型解耦，得到并行的导航状态量估计器和比例因子误差观测器，用来同时估计导航状态量和比例因子误差。

2.根据权利要求1所述的一种车载GNSS/INS组合导航系统比例因子误差补偿方法，其特征在于，步骤二中，

所述基于卡尔曼滤波的GNSS/INS组合导航系统模型方程为：

式中，w和v为零均值且不相关的高斯白噪声；

y＝[V_INS-V_GNSS P_INS-P_GNSS]^T＝[δV_3×1 δP_3×1]^T；

C＝[0_6×3 I_6×6 0_6×6]；

其中，ψ、δV、δP分别为姿态误差、速度误差和位置误差，b_f和b_ω分别为加速度计和陀螺仪的随机常值误差，V_INS和P_INS分别为INS输出的速度和位置，V_GNSS和P_GNSS分别为GNSS输出的速度和位置，0为零矩阵，I为单位矩阵。

3.根据权利要求2所述的一种车载GNSS/INS组合导航系统比例因子误差补偿方法，其特征在于，步骤三，将比例因子误差在GNSS/INS组合导航系统模型方程中表征，并对方程进行离散化处理，得到：

其中，I为单位矩阵；A_k为状态转移矩阵，由矩阵F离散化得到；b_k为陀螺仪和加速度计比例因子误差向量；B_k表示模型误差进入状态方程的传播矩阵；其余符号意义同前，只是转为离散化形式；并且有：

A_k＝(I+F△t)；

其中，Δt为系统采样周期，ω^b和f^b分别为车身系下陀螺仪和加速度计的测量值。

4.根据权利要求3所述的一种车载GNSS/INS组合导航系统比例因子误差补偿方法，其特征在于，步骤四中，导航状态量估计器迭代过程为：

当k＝0时，初始化：

式中，为状态量初始值；/>为状态量误差协方差初始值；x₀为状态量真实值，

当k＝1,2,3……时，

状态量先验估计值更新：

式中，是状态量先验估计值，/>是状态量后验估计值；

状态量先验误差协方差更新：

式中，是导航状态量估计器的状态量先验误差协方差，/>是导航状态量估计器的状态量后验误差协方差，Q_k-1是上述组合导航系统模型方程中噪声w_k-1的协方差；

计算卡尔曼增益：

式中，K_x,k是导航状态量估计器的卡尔曼增益，R_k是上述组合导航系统模型方程中噪声v_k的协方差；

新息更新：

式中，r_x,k是导航状态量估计器的新息；

状态量后验估计值更新：

状态量后验误差协方差更新：

5.根据权利要求4所述的一种车载GNSS/INS组合导航系统比例因子误差补偿方法，其特征在于，步骤四中，比例因子误差观测器迭代过程为：

当k＝0时，初始化：

U₀＝0,/>

式中，为比例因子误差量初始值；U₀为迭代矩阵初始值；P_b,0为比例因子误差量的误差协方差初始值；b₀为比例因子误差量真实值；

当k＝1,2,3……时，

先验估计值更新：

式中，是比例因子误差量先验估计值，/>是比例因子误差量后验估计值；

更新算法迭代矩阵：

式中，V_k、U_k、S_k是为算法迭代计算方便设置的中间环节矩阵，无特殊含义；

误差协方差矩阵更新：

式中，P_b,k是比例因子误差量的误差协方差；

计算卡尔曼增益：

式中，K_b,k是比例因子误差观测器的卡尔曼增益；

新息更新：

式中，r_b,k是比例因子误差观测器的新息，

后验估计值更新：

6.根据权利要求5所述的一种车载GNSS/INS组合导航系统比例因子误差补偿方法，其特征在于，步骤四中，

每一次迭代的最后，需用比例因子误差估计值对导航状态量估计值进行补偿，从而得到导航状态量的最优估计为：

式中，为导航状态量的最优估计值。