CN116086466B - 一种提高ins误差精度的方法 - Google Patents

Abstract

本发明公开了一种提高INS误差精度的方法，所述方法包括：利用GNSS/INS松组合模式，将GNSS接收机输出的位置与速度解作为改进自适应滤波SKF的输入，同时结合INS输出的位置与速度，利用改进的自适应滤波对GNSS接收机的输出和INS的输出进行差分运算，并根据差分运算输出的观测值建立误差模型，通过剔除观测值粗差，解析INS误差，选择合适的因子并利用INS误差预测估计最优的INS速度、位置值，实现对INS结果的输出矫正。本发明使用改进的自适应滤波算法，有效的解决了GNSS/INS组合导航中观测值异常的问题，并且提高了滤波的计算精度，保证了滤波的可靠性和稳定性。

Description

一种提高INS误差精度的方法

技术领域

本发明主要涉及组合导航定位技术领域，具体涉及一种自动驾驶领域的GNSS/INS组合导航定位。

背景技术

随着汽车电子技术的日益发展，人们对于汽车智能网联化的需求也在逐步上升，自动驾驶技术也应运而生，对于自动驾驶技术的实现，涉及到GNSS与INS导航定位与自适应滤波等方面，由于汽车载体运动状态变化以及观测粗差影响，滤波的性能会严重降低，影响定位精度。

GNSS/INS组合导航系统一般采用Kalman滤波算法进行融合，但卡尔曼滤波需要确定且准确的动力模型、噪声统计特性，二者实际上难以确定，而且系统存在未知性，会发生异常的观测和动力模型扰动等问题，会常常影响滤波的性能甚至会发散。若采用EKF算法，是基于前一时刻的状态参数估计值，所以要是前一刻估计值误差较大，则线性化的状态方程以及观测方程相应的误差也大。

基于KF和EKF的不足，组合导航定位需要一种既可靠又稳定的方法，能够提高定位的精度，而且计算量不大，容易实现，能广泛应用于自动驾驶领域。

发明内容

发明目的：为了克服上述现有技术的不足，本发明提供了一种提高INS误差精度的方法，可提高定位精度与可靠性。

技术方案：本发明公开一种提高INS误差精度的方法，通过改进的自适应滤波算法估计最优的速度、位置，具体包括如下步骤：

步骤1：获取INS的误差状态变量组成的15维误差向量，作为系统的状态向量；

步骤2：获取INS与GNSS的位置、速度，并将INS与GNSS输出的位置、速度差值作为建立松组合导航系统的量测方程；

步骤3：利用改进的自适应滤波算法估计最优的速度、位置；具体为利用INS与GNSS观测值和预测值计算协方差估计值和协方差理论值，将其作差后的绝对值作为观测依据，若观测值异常则剔除k时刻的观测值，若无异常，则代入合适自适应因子调节观测值与预测值的权重，从而获取INS速度、位置的最优估计值。

进一步地，所述步骤1中获取INS的误差状态变量组成的15维误差向量的方法包括：

式中：F(t)为状态转移矩阵，X(t)为系统状态向量，G(t)为系统噪声矩阵，W(t)为系统噪声向量，系统状态向量X(t)表示为：

式中，φ_E，φ_N，φ_U，δv_E，δv_N，δv_U，δλ,δh分别表示为INS的姿态误差、速度误差和位置误差，ε_bx，ε_by，ε_bz为陀螺仪偏差，/>为加速度计偏差；

系统状态转移矩阵F(t)表示为：

式中，F_w(9×9)表示INS的姿态误差、速度误差和位置误差的状态转移矩阵，F_s(9×6)表示为惯性导航参数和惯性元器件之间的转换矩阵；

系统的噪声分配矩阵为：

系统的噪声向量表示为：

W(t)＝[ω_gx，ω_gy，ω_gz，ω_ax，ω_ay，ω_az]^T

式中，ω_gx，ω_gy，ω_gz和ω_ax，ω_ay，ω_az分别表示为陀螺仪和加速度计的随机游走误差。

进一步地，所述步骤2中具体包括如下步骤：

基于INS和GNSS输出的三向速度和位置信息的差值建立松组合导航系统的量测方程：

将INS解算速度和GNSS输出速度作差可得组合导航系统的速度量测方程，具体表示为：

将INS解算位置和GNSS输出位置作差可得组合导航系统的位置量测方程，具体表示为：

量测矩阵H(t)和量测噪声向量V(t)分别为：

进一步地，所述步骤2中获取GNSS位置的方法包括：

采用GNSS RTK载波相位差分定位方法，将流动站GNSS接收机输出的模糊定位解GGA注入千寻SDK，进而从基准站获取RTCM载波相位观测值，同时利用回调的方式，再次将RTCM写入GNSS模组，将误差较大的模糊定位解提升为厘米级的高精度定位解。

进一步地，所述步骤3中改进的自适应滤波算法具体包括：

1)在时间更新阶段，假设在X_k-1是k时刻相对于k-1时刻的最优状态估计值，状态一步预测方程为：

X_k，k-1＝Φ_k，k-1X_k-1

2)假设P_k,k-1是k时刻相对于k-1时刻的预测协方差矩阵，P_k-1是k-1时刻的预测协方差矩阵，状态一步预测协方差为：

3)在量测阶段可得到k时刻系统的卡尔曼滤波增益方程为：

4)滤波状态估计值为：

X_k＝X_k，k-1+K_k(Z_k-H_kX_k，k-1)

5)状态估计均方误差为：

改进的自适应滤波算法选择合适自适应因子α_k，调节增益K，继而调节观测值与预测值的比重。

进一步地，所述步骤3中选择合适自适应因子调节增益K，具体包括如下步骤：

预测残差：

其协方差矩阵理论计算值为：

协方差估计值与协方差理论值之差：Δε_k＝(ε^Tε)-tr(Σε_k)

(1)进行缩小范围，判断观测量是否有故障，采用残差χ²的检验法，当|Δε_k|＞5时，进行判断是否发生故障，引入故障检测函数为：

式中λ_k表示自由度为m的χ²分布，其中m为Z_k的维数，则判定规则如下：λ_k＞T_D，判定为有故障；λ_k＜T_D，判断无故障；

(2)如果检测出故障，则舍弃k时刻故障观测值，同时取合适自适应因子，用来平衡量测值与观测值的关系，减少误差：

当残差过大时，通过α_k自适应因子降低估计过程中预测量的比重，增大观测量的比重；反之观测信息不可靠时，需要通过残差χ²检测，淘汰粗差，则增加预测量比重。

有益效果：

1、本发明设计改进的自适应滤波算法，先利用残差的协方差理论值与估计值差值来缩小范围进行判断观测值是否有故障，若是存在故障则剔除k时刻的观测值，反之则调节合适因子，避免滤波发散或者不稳定，同时选定合适因子，可以进一步提高定位精度与可靠性。

2、本发明选定残差χ²检测法，既能检测突变型故障，也能检测渐变型故障，通过多次选取比较误差最小的检测门限值，而且相比状态χ²检测法计算数据量小，有效降低CPU的负荷，提高误差解算的运行效率。

3、本发明GNSS模组使用RTK载波相位差分定位方法，将流动站GNSS接收机输出的模糊定位解GGA注入千寻SDK，进而从基准站获取RTCM载波相位观测值，同时利用回调的方式，再次将RTCM写入GNSS模组，得到厘米级高精度定位解。

附图说明

图1是本发明提供的一种改进的自适应滤波算法(简称SKF)流程图；

图2是本发明提供的一种载波相位差分定位原理图；

图3是本发明提供的一种INS机械编排原理图；

图4是本发明提供的一种GNSS/INS松组合原理图；

图5是本发明提供的一种AKF速度误差仿真图；

图6是本发明提供的一种SKF速度误差仿真图；

图7是本发明提供的一种AKF位置误差仿真图；

图8是本发明提供的一种SKF位置误差仿真图。

具体实施方式

为了清晰表述本发明所解决的技术问题和实施方案，下面结合附图对本发明做进一步的详细说明。

惯性导航系统简称INS，通过测量自身的运动状态的变化，比如测量值主要为加速度角速率，再进行积分运算，得到载体三维的定位信息，比如位置和速度、姿态等参数。但由于长时间丢失信号的导航情况下，连续积分会产生累积误差。而GNSS不会产生累积误差，因此将GNSS与INS组合导航，能够纠正INS累积误差。

GNSS获取位置计算过程中存在多种误差，差分法是消除误差的有效办法。使用载波相位观测量，可以通过卫星与接收机之间载波信号相位差进行解算，从而得到接收机的位置信息。根据载波相位观测方程为：

式中，下标a表示观测接收机，λ为载波的波长，N为载波相位变化量的整周数部分，为不足整数的小数部分，ε表示观测噪声等其他误差总和。

松组合结构中就是GNSS模块与INS模块独立工作，低频输出是GNSS模块中RTK定位信息，而高频输出的是INS测量信息，这两者都作为数据处理模块的输入。

根据原理图4，要是没收到GNSS信号，那么就以INS当前推测的定位数据为准。若反之受到GNSS信号，则通过比较GNSS与INS定位信息差值，建立误差模型，估计INS累积误差，并补偿给INS模块，同时输出定位数据的最佳信息，一直往复循环。

融合滤波前，需将GNSS与INS传感器解算的定位信息根据投影、转换成统一坐标系，选取导航坐标系为参考系，根据实际场景应用，选取初始位置为坐标系原点。

本发明通过改进的自适应滤波算法估计最优的速度、位置，具体包括如下步骤：

步骤1：获取INS的误差状态变量组成的15维误差向量，作为系统的状态向量。

状态方程为：

式中，式中：F(t)为状态转移矩阵，X(t)为系统状态向量，G(t)为系统噪声矩阵，W(t)为系统噪声向量；φ_E，φ_N，φ_U，δv_E，δv_N，δv_U，δλ,δh分别表示为INS的姿态误差、速度误差和位置误差，ε_bx，ε_by，ε_bz为陀螺仪偏差，/>为加速度计偏差。

系统状态转移矩阵F(t)表示为：

式中，F_w(9×9)表示INS的姿态误差、速度误差和位置误差的状态转移矩阵，F_s(9×6)表示为惯性导航参数和惯性元器件之间的转换矩阵；

系统的噪声分配矩阵为：

系统的噪声向量表示为：

W(t)＝[ω_gx，ω_gy，ω_gz，ω_ax，ω_ay，ω_az]^T

式中，ω_gx，ω_gy，ω_gz和ω_ax，ω，_ay，ω_az分别表示为陀螺仪和加速度计的随机游走误差。

步骤2：获取INS与GNSS的位置、速度，并将INS与GNSS输出的位置、速度差值作为建立松组合导航系统的量测方程。

基于INS和GNSS输出的三向速度和位置信息的差值建立松组合导航系统的量测方程：

将INS解算速度和GNSS输出速度作差可得组合导航系统的速度量测方程，具体表示为：

将INS解算位置和GNSS输出位置作差可得组合导航系统的位置量测方程，具体表示为：

量测矩阵H(t)和量测噪声向量V(t)分别为：

GNSS位置获取方法包括：

采用GNSS RTK载波相位差分定位方法，将流动站GNSS接收机输出的模糊定位解GGA注入千寻SDK，进而从基准站获取RTCM载波相位观测值，同时利用回调的方式，再次将RTCM写入GNSS模组，将误差较大的模糊定位解提升为厘米级的高精度定位解。

步骤3：利用改进的自适应滤波算法估计最优的速度、位置；具体为利用INS与GNSS观测值和预测值计算协方差估计值和协方差理论值，将其作差后的绝对值作为观测依据，若观测值异常则剔除k时刻的观测值，若无异常，则代入合适自适应因子调节观测值与预测值的权重，从而获取INS速度、位置的最优估计值。

为了计算k时刻最优速度、位置时，先进行状态更新。

1)第一步用上一个时刻的状态来估计下一个时刻的状态，假设在X_k-1是k时刻相对于k-1时刻的最优状态估计值，状态一步预测方程为：

X_k，k-1＝Φ_k，k-1X_k-1

2)把协方差矩阵考虑进去，假设P_k，k-1是k时刻相对于k-1时刻的预测协方差矩阵，P_k-1是k-1时刻的预测协方差矩阵。状态一步预测协方差为：

3)在量测阶段可得到k时刻系统的卡尔曼滤波增益方程为：

4)滤波状态估计值为：

X_k＝X_k，k-1+K_k(Z_k-H_kX_k，k-1)

5)状态估计均方误差为：

改进的自适应滤波算法选择合适自适应因子α_k，调节增益K，继而调节观测值与预测值的比重。

预测残差：

其协方差矩阵理论计算值为：

协方差估计值与协方差理论值之差：Δε_k＝(ε^Tε)-tr(∑ε_k)。

计算增益K之前进行缩小范围，判断观测量是否有故障，采用残差χ²的检验法。当|Δε_k|＞5时，进行判断是否发生故障。

根据上式，引入故障检测函数为：

式中λ_k表示自由度为m的χ²分布，其中m为Z_k的维数。则判定规则如下：λ_k＞T_D，判定为有故障。λ_k＜T_D，判断无故障。

根据奈曼-皮尔逊准则，通过合理选取误检率P_f来达到相对较好的检测效果。残差χ²检测法对突变型故障出现频繁，而残差χ²检测法可以很好检测出故障。但对渐变故障检测不是十分有效，容易漏检、敏感程度低、检测效果差，所以需要选择检测门限，尤为注意。这里使用两种误警概率对应检测门限，选择使INS误差最小的门限值。

根据奈曼-皮尔逊准则，本发明需要合理选取误检率P_f来达到相对较好的检测效果。若检测出故障，则舍弃k时刻故障观测值。

考虑合适的增益来权衡预测值与观测值之间关系，需要从P_k，k-1预测协方差矩阵和观测矩阵H_k和观测噪声协方差矩阵R_k入手，本发明创新点从预测协方差入手，公式如下：

如果检测出故障，则舍弃k时刻故障观测值，同时取合适自适应因子，用来平衡量测值与观测值的关系，减少误差：

当残差过大时，通过α_k自适应因子降低估计过程中预测量的比重，增大观测量的比重；反之观测信息不可靠时，需要通过残差χ²检测，淘汰粗差，则增加预测量比重。

通过代入公式X_k＝X_k，k-1+K_k(Z_k-H_kX_k，k-1，计算最优估计值速度、位置。

通过仿真图，本发明改进的自适应滤波算法(SKF)与自适应滤波算法(AKF)相比，结合图6可见，通过三维空间分别向北、东、地方向速度误差精度提升了75％、92％、93％。其中，北方向观测值存在粗差导致定位精度稍微低些。结合图8可见，位置误差精度向北、向东、向地方向分别提高了94％、95％、95％。

以上所述仅为本发明的技术实现及特点，但不用于限制本发明，对于本领域的技术人员，本发明方案可以有各种修改和变化，在不脱离本发明原理的前提下，还可以实现其他的时间解算方案，这类修改也应包含在本发明的保护范围之内。

Claims (3)

1.一种提高INS误差精度的方法，其特征在于，通过改进的自适应滤波算法估计最优的速度、位置，具体包括如下步骤：

步骤1：获取INS的误差状态变量组成的15维误差向量，作为系统的状态向量；

步骤2：获取INS与GNSS的位置、速度，并将INS与GNSS输出的位置、速度差值作为建立松组合导航系统的量测方程；

采用GNSS RTK载波相位差分定位方法，将流动站GNSS接收机输出的模糊定位解GGA注入千寻SDK，进而从基准站获取RTCM载波相位观测值，同时利用回调的方式，再次将RTCM写入GNSS模组，将误差较大的模糊定位解提升为厘米级的高精度定位解；

步骤3：利用改进的自适应滤波算法估计最优的速度、位置；具体为利用INS与GNSS观测值和预测值计算协方差估计值和协方差理论值，将其作差后的绝对值作为观测依据，若观测值异常则剔除k时刻的观测值，若无异常，则代入合适自适应因子调节观测值与预测值的权重，从而获取INS速度、位置的最优估计值；

所述步骤3中改进的自适应滤波算法具体包括：

1)在时间更新阶段，假设在X_k-1是k时刻相对于k-1时刻的最优状态估计值，状态一步预测方程为：

X_k,k-1＝Φ_k,k-1X_k-1

2)假设P_k，k-1是k时刻相对于k-1时刻的预测协方差矩阵，P_k-1是k-1时刻的预测协方差矩阵，状态一步预测协方差为：

3)在量测阶段可得到k时刻系统的卡尔曼滤波增益方程为：

4)滤波状态估计值为：

X_k＝X_k,k—1+K_k(Z_k-H_kX_k,k-1)

5)状态估计均方误差为：

所述改进的自适应滤波算法选择合适自适应因子α_k，调节增益K，继而调节观测值与预测值的比重；

所述步骤3中选择合适自适应因子调节增益K，具体包括如下步骤：

预测残差：

其协方差矩阵理论计算值为：

协方差估计值与协方差理论值之差：Δε_k＝(ε^Tε)-tr(∑ε_k)

(1)进行缩小范围，判断观测量是否有故障，采用残差χ²的检验法，当|Δε_k|＞5时，进行判断是否发生故障，引入故障检测函数为：

式中λ_k表示自由度为m的χ²分布，其中m为Z_k的维数，则判定规则如下：λ_k＞T_D，判定为有故障；λ_k＜T_D，判断无故障；

(2)如果检测出故障，则舍弃k时刻故障观测值，同时取合适自适应因子，用来平衡量测值与观测值的关系，减少误差：

当残差过大时，通过α_k自适应因子降低估计过程中预测量的比重，增大观测量的比重；反之观测信息不可靠时，需要通过残差χ²检测，淘汰粗差，则增加预测量比重。

2.根据权利要求1所述的一种提高INS误差精度的方法，其特征在于，所述步骤1中获取INS的误差状态变量组成的15维误差向量的方法包括：

式中：F(t)为状态转移矩阵，X(t)为系统状态向量，G(t)为系统噪声矩阵，W(t)为系统噪声向量，系统状态向量X(t)表示为：

式中，φ_E，φ_N，φ_U，δυ_E，δυ_N，δυ_U，δλ，δh分别表示为INS的姿态误差、速度误差和位置误差，ε_bx，ε_by，ε_bz为陀螺仪偏差，/>为加速度计偏差；

系统状态转移矩阵F(t)表示为：

式中，F_w(9×9)表示INS的姿态误差、速度误差和位置误差的状态转移矩阵，F_w(9×6)表示为惯性导航参数和惯性元器件之间的转换矩阵；

系统的噪声分配矩阵为：

系统的噪声向量表示为：

W(t)＝[ω_gx，ω_gy，ω_gz，ω_ax，ω_ay，ω_az]^T

式中，ω_gx，ω_gy，ω_gz和ω_ax，ω_ay，ω_az分别表示为陀螺仪和加速度计的随机游走误差。

3.根据权利要求1所述的一种提高INS误差精度的方法，其特征在于，所述步骤2中具体包括如下步骤：

基于INS和GNSS输出的三向速度和位置信息的差值建立松组合导航系统的量测方程：

式中，X(t)为系统状态向量；

将INS解算速度和GNSS输出速度作差可得组合导航系统的速度量测方程，具体表示为：

将INS解算位置和GNSS输出位置作差可得组合导航系统的位置量测方程，具体表示为：

量测矩阵H(t)和量测噪声向量V(t)分别为：