CN105929431A - 一种低动态高抖动环境下的gps/ins紧组合方法 - Google Patents

Abstract

本发明提供一种低动态高抖动环境下的GPS/INS紧组合方法，该方法获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据；对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据；将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。本发明解决了在低动态高抖动环境中常规GPS/INS紧组合导航结果不可靠的问题，大幅提升导航定位的精度，能满足高精度的导航应用要求，不仅提供载体的位置信息，还可以提供载体的速度信息和姿态状态信息，能适应于更广泛的应用场景，还具有实时高精度导航的潜力。

Description

一种低动态高抖动环境下的GPS/INS紧组合方法

技术领域

本发明属于导航技术领域，具体涉及一种低动态高抖动环境下的GPS/INS紧组合方法。

背景技术

GPS/INS组合导航能高效利用各自的特点进行优势互补，是目前最理想组合导航方式。相对于GPS/INS松组合，GPS/INS紧组合具有更高的导航精度，能进行复杂环境的导航定位，是目前GPS/INS组合中常用的组合方式。

组合导航中的复杂环境多指GPS卫星被遮挡、GPS信号不稳定的环境。本文发明认为该复杂环境不仅应包括上述情况，还应考虑由于外界环境和量测手段等导致INS原始数据中存在大量噪声以及不连续的环境。例如，当搭载GPS和INS的载体行驶速度缓慢，但该载体却处于高频不规则振动中，本发明将该环境简称为低动态高抖动环境。

这种低动态高抖动环境将同时GPS和INS的定位解算：对于GPS，快速抖动会导致GPS多路径效应影响变大，甚至会导致GPS卫星信号跟踪失锁；对于INS，低动态高抖动会使惯性器件的观测信号淹没在高频噪声中。在该环境下GPS信号的处理已有较合适的方法，但对于该环境下的INS信号处理一直没有得到较好的重视。低动态高抖动环境下的INS观测值误差会大幅降低GPS/INS紧组合导航定位精度，甚至使GPS/INS紧组合发生错误。

发明内容

针对现有技术的不足，本发明提出一种低动态高抖动环境下的GPS/INS紧组合方法。

本发明的技术方案是：

一种低动态高抖动环境下的GPS/INS紧组合方法，包括以下步骤：

步骤1：获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据；

步骤2：对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据；

步骤2.1：对INS原始数据进行小波分解：构造变换矩阵，确定分解层数N，对INS原始数据进行N层小波分解，得到各层小波分解系数；

步骤2.2：对各层小波分解的高频系数进行阈值处理，去除高频部分的噪声，得到阈值处理后的小波分解的高频系数；

步骤2.3：对每一层小波分解的低频系数和阈值处理后的小波分解的高频系数进行小波重构，得到小波降噪后的INS数据。

步骤3：将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。

步骤3.1：构建GPS/INS紧组合的状态方程：包括INS的误差状态和GPS的误差状态；

步骤3.2：构建GPS/INS紧组合的量测方程：包括伪距差观测方程和伪距率差观测方程；

步骤3.3：将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行kalman滤波解算，得到载体当前时刻在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息；

步骤3.4：重复步骤3.1至步骤3.3，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。

本发明的有益效果：

本发明提出一种低动态高抖动环境下的GPS/INS紧组合方法，本发明解决了在低动态高抖动环境中常规GPS/INS紧组合导航结果不可靠的问题，大幅提升导航定位的精度，能满足高精度的导航应用要求；本发明不仅提供载体的位置信息，还可以提供载体的速度信息和姿态状态信息；本发明中能够提供高达200HZ的数据更新结果，能适应于更广泛的应用场景，还具有实时高精度导航的潜力。

附图说明

图1为本发明具体实施方式中低动态高抖动环境下的GPS/INS紧组合方法的流程图；

图2为本发明具体实施方式中实验小车的示意图；

图3为本发明具体实施方式中陀螺Y轴的原始数据；

图4为本本发明具体实施方式中加速度计Y轴的原始数据；

图5为本发明具体实施方式中GPS卫星的PDOP值；

图6为本发明具体实施方式中对INS原始数据进行小波阈值降噪处理的流程图；

图7为本发明具体实施方式中三层小波分解的示意图；

图8为本发明具体实施方式中陀螺Y轴的小波降噪后的数据；

图9为本发明具体实施方式中加速度计Y轴的小波降噪后的数据；

图10为本发明具体实施方式中将小波降噪后的INS数据与GPS原始数据进行紧组合处理的流程图；

图11为本发明具体实施方式中小波降噪后的INS数据与GPS原始数据紧组合解算轨迹、常规GPS/INS紧组合解算轨迹以参考真值轨迹图。

图12为本发明具体实施方式中小波降噪后的INS数据与GPS原始数据的解算高程、常规GPS/INS紧组合解算的高程以参考真值的高程图。

具体实施方式

下面结合附图对本发明具体实施方式加以详细的说明。

一种低动态高抖动环境下的GPS/INS紧组合方法，如图1所示，包括以下步骤：

步骤1：获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据。

本实施方式中，为了验证一种低动态高抖动环境下的GPS/INS紧组合方法的有效性，进行实测实验，实验小车如图2所示无任何缓冲设施，实验人员步行推动小车先通过一段较平缓路面(含部分砂砾)，然后通过复杂环境(含多砂砾和碎石块路面，多次穿越台阶)，最后再通过一段较平缓路面(含部分砂砾)并结束实验；实验小车行驶速度小于1.5m/s，行驶平均速度约为0.6m/s。

本实施方式中，陀螺Y轴的原始输数据如图3所示，加速度计Y轴的原始数据如图4所示。GPS卫星的PDOP值如图5所示，其中倒三角为阶段间分割标记，由图5可知，整个实验中GPS观测环境较为复杂，存在信号不稳定及信号中断。

步骤2：对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据，如图6所示。

步骤2.1：对INS原始数据进行小波分解：构造变换矩阵，确定分解层数，对INS原始数据进行N层小波分解，得到各层小波分解系数。

本实施方式中，对含噪声的INS原始数据X进行小波分解：构造变换矩阵W，确定分解层数N，对INS原始数据X进行N层小波分解如式(1)所示：

$\tilde{X}=WX---\left(1\right)$

其中，为INS原始数据X的小波变换。

步骤2.2：对各层小波分解的高频系数进行阈值处理，去除高频部分的噪声，得到阈值处理后的小波分解的高频系数。

本实施方式中，对各层小波分解的高频系数进行阈值处理的软阈值函数如式(2)所示：

其中，d为未处理的小波分解的高频系数，为阈值处理后的小波分解的高频系数，λ为设定的阈值，

步骤2.3：对每一层小波分解的低频系数和阈值处理后的小波分解的高频系数进行小波重构，得到小波降噪后的INS数据。

本实施方式中，对每一层小波分解的低频系数和阈值处理后的小波分解的高频系数进行小波重构的公式如式(3)所示：

$X^{\′}=W^T{\tilde{X}}^{\′}---\left(3\right)$

其中，X′为小波降噪后的INS数据，为小波分解的低频系数和阈值处理后的小波系数。

本实施方式中，三层小波分解的示意图如图7所示，其中，S为高频系数部分，D为低频系数部分。陀螺Y轴的小波降噪后的数据如图8所示，加速度计Y轴的小波降噪后的数据如图9所示，由图8和图9可以看出小波降噪方法能有效分离INS原始数据中的噪声和有用信号，能剔除大量的噪声。

步骤3：将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息，如图10所示。

步骤3.1：构建GPS/INS紧组合的状态方程：包括INS的误差状态和GPS的误差状态。

本实施方式中，INS的误差状态包括当地地理坐标系下三方向的位置误差、当地地理坐标系下三方向的速度误差、当地地理坐标系下三方向的姿态角误差、当地地理坐标系下三轴的陀螺漂移和当地地理坐标系下三轴的加速度计漂移。

GPS的误差状态包括：与接收机钟误差等效的距离误差Cδt_u和与接收机钟频率误差等效的距离率误差Cδt_ru，如式(4)所示：

$\{\begin{array}{c}C\mathrm{\δ}{\stackrel{\·}{t}}_u={\mathrm{C\δt}}_u+{\mathrm{\ω}}_{tu}\\ C\mathrm{\δ}{\stackrel{\·}{t}}_{ru}=-{\mathrm{C\βC\δt}}_{ru}+{\mathrm{\ω}}_{tru}\end{array}---\left(4\right)$

其中，为与接收机钟误差等效的距离误差的微分形式，为与接收机钟频率误差等效的距离率误差的微分形式，C为光速，β为相关时间，ω_tu为与接收机钟误差等效的距离误差的驱动白噪声，ω_tru为与接收机钟误差等效的距离误差的驱动白噪声。

本实施方式中，构建的GPS/INS紧组合的状态方程如式(5)所示：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_{IK}\\ {\stackrel{\·}{X}}_{GK}\end{array}\right]=\left[\begin{array}{cc}{F}_{IK}& 0\\ 0& F_{GK}\end{array}\right]\left[\begin{array}{c}{X}_{IK}\\ X_{GK}\end{array}\right]+\left[\begin{array}{cc}{G}_{IK}& 0\\ 0& G_{GK}\end{array}\right]\left[\begin{array}{c}{W}_{IK}\\ W_{GK}\end{array}\right]---\left(5\right)$

其中， 为X_IK的微分形式，δB、δL、δh为当地地理坐标系下三方向的位置误差，δv_E、δv_N、δv_U为当地地理坐标系下三方向的速度误差，为当地地理坐标系下三方向的姿态角误差，ε_bx、ε_by、ε_bz为当地地理坐标系下三轴的陀螺漂移，为当地地理坐标系下三轴的加速度计漂移，F_IK为小波降噪后INS的状态转移矩阵，G_IK为小波降噪后INS的动态噪声矩阵，W_IK为INS的系统过程噪声向量，

本实施方式中，小波降噪后INS的动态噪声矩阵G_IK如式(6)所示：

$G_{IK}={\left[\begin{array}{cc}{0}_{6\×3}& 0_{6\×3}\\ C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3\end{array}\right]}_{15\×6}---\left(6\right)$

其中，为载体坐标系到当地地理坐标系的姿态矩阵，I₃为单位矩阵。

本实施方式中，INS的系统过程噪声向量W_IK如式(7)所示：

W_IK＝[w_gx w_gy w_gz w_ax w_ay w_az]^T (7)

其中，w_gx为陀螺x轴的噪声，w_gy为陀螺y轴的噪声，w_gz为陀螺z轴的噪声，w_ax为加速度计x轴的噪声，w_ay为加速度计y轴的噪声，w_az为加速度计z轴的噪声。

本实施方式中，小波降噪后的INS的状态转移矩阵F_IK如式(8)所示：

$F_{IK}={\left[\begin{array}{cc}{F}_w& F_s\\ 0_{6\×9}& 0_{6\×6}\end{array}\right]}_{15\×15}---\left(8\right)$

其中，F_w对应9个误差的状态转移矩阵，非零元素为：F_3，6＝1， F_4，8＝-f_z，F_4，9＝f_y， F_5，7＝f_z，F_5，9＝-f_x，F_6，1＝-2_wiev_E sin B， F_6，7＝-f_y，F_6，8＝f_x， F_8，1＝-w_iesin B， R为地球参考椭球长半轴，B为载体在当地地理坐标系中的纬度，L为载体在当地地理坐标系中的经度，h为载体在当地地理坐标系中的高，v_N为当地地理坐标系下的北向速度，v_E为当地地理坐标系下的东向速度，v_U为当地地理坐标系下的垂向速度，w_ie为地球角速率，f_x为比力向量在当地地理坐标系下的x轴方向分量，f_y为比力向量在当地地理坐标系下的y轴方向分量，f_z为比力向量在当地地理坐标系下的z轴方向分量。

将公式(5)离散化得式(9)如下：

X_k＝Φ_k，k-1X_k-1+w_k (9)

其中，为k时刻GPS/INS紧组合的状态向量，X_k-1为k-1时刻GPS/INS紧组合的状态向量，w_k为噪声向量，其协方差矩阵为∑w_k，Φ_k，k-1为离散化之后GPS/INS紧组合的状态转移矩阵。

步骤3.2：构建GPS/INS紧组合的量测方程：包括伪距差观测方程和伪距率差观测方程。

本实施方式中，伪距差观测方程如式(10)所示：

δρ_j＝e_j1δx+e_j2δy+e_j3δz-δt_u-v_ρj (10)

其中，δρ_j为GPS得到的伪距与INS得到的伪距的差值，e_j1为卫星观测方向与x轴的方向余弦，e_j2为卫星观测方向与y轴的方向余弦，e_j3为卫星观测方向与z轴的方向余弦，j＝1...n，δx为x轴方向上的位置误差，δy为y轴方向上的位置误差，δz为z轴方向上的位置误差，δt_u为GPS接收机钟误差，v_ρj为GPS得到的伪距观测噪声。

其中，载体在地心地固坐标系中位置与载体在当地地理坐标系中为纬度、经度和高的关系如式(11)所示：

$\left\{\begin{array}{c}x=(R+h)\cos L\cos B\\ y=(R+h)\sin L\cos B\\ z=\[R(1-e^2)+h\]\sin B\end{array}\right.---\left(11\right)$

其中，x为载体在地心地固坐标系的x轴，y为载体在地心地固坐标系的y轴，z为载体在地心地固坐标系的z轴。

伪距率差观测方程如式(12)所示：

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j=e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}-{\mathrm{\δt}}_{ru}-v_{\stackrel{\·}{\mathrm{\ρ}}j}---\left(12\right)$

其中，为伪距率误差，为x轴方向上的位置误差的导数，为y轴方向上的位置误差的导数，为z轴方向上的位置误差的导数，δt_ru为GPS接收机钟频率误差，为GPS得到的伪距率观测噪声。

则可以得到GPS/INS紧组合的量测方程如式(13)所示：

$\left[\begin{array}{c}\∂\mathrm{\ρ}\\ \∂\stackrel{\·}{\mathrm{\ρ}}\end{array}\right]=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]\left[\begin{array}{c}{X}_{IK}\\ X_{GK}\end{array}\right]+\left[\begin{array}{c}{V}_{\mathrm{\ρ}}\\ V_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]---\left(13\right)$

其中，

将公式(13)离散化可得公式(14)如下：

Z_k＝H_kX_k+V_k (14)

其中，为k时刻GPS/INS紧组合的观测向量，H_k为k时刻GPS/INS紧组合的量测矩阵，V_k为k时刻的量测噪声，其协方差矩阵为∑V_k。

步骤3.3：将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行kalman滤波解算，得到载体当前时刻在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。

本实施方式中，将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行kalman滤波解算，得到解如式(15)所示：

$X_k^{+}=X_k^{-}+K_k(Z_k-H_k{X}_k^{-})---\left(15\right)$

其中，为X_k的kalman滤波估值，为滤波增益矩阵，为预测的状态向量协方差阵，R_k为量测噪声V_k的对称正定方差阵，Q_k为噪声向量w_k的非负定方差阵，为当前时刻状态向量的协方差阵。

本实施方式中，得到的载体当前时刻在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息的一组数据如下：

GPS时间：285582.000s、经度：121.6551869212deg、纬度：42.0492113952deg、高：166.828m、东向速度：0.038m/s、北向速度：1.105m/s、垂向速度：-0.077m/s、横滚角：-0.02281844deg、俯仰角：2.04958623deg、航向角：4.06268929deg。

步骤3.4：重复步骤3.1至步骤3.3，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。

本实施方式中，通过kalman解算得到的状态估值作为下一时刻的状态估值重复步骤3.1至步骤3.3，再构建新的GPS/INS紧组合的状态方程和量测方程进行kalman解算得到新的状态估值，直至所有的小波降噪后的INS数据与GPS原始数据迭代完成，则可以得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。

本实施方式中，小波降噪后的INS数据与GPS原始数据紧组合解算轨迹、常规GPS/INS紧组合解算轨迹以参考真值轨迹图如图11所示，小波降噪后的INS数据与GPS原始数据的解算高程、常规GPS/INS紧组合解算的高程以参考真值的高程图如图12所示。

由图11和图12可知，在低动态高抖动环境中，由于GPS观测环境复杂，IMU数据中存在大量噪声，常规GPS/INS解算平面和高程结果存在较大漂移，无法正常导航定位，而本发明的小波降噪的GPS/INS紧组合中有效降低了IMU中噪声对导航结果的影响，大幅提升导航定位的精度。

在低动态高抖动环境中，当GPS观测环境较好时，小波降噪的GPS/INS紧组合能将导航定位精度从5m提升至3m以内；当GPS观测环境较差(GPS信号不稳定，存在信号多次中断)时，常规GPS/INS紧组合精度均大于15m，且存在大幅的漂移，定位结果不可靠，小波降噪的GPS/INS紧组合能大幅提升导航定位精度至5m左右，大幅提升导航定位的稳定可靠性，使低动态高抖动环境中的导航定位具有更好的实用性。

Claims (3)

1.一种低动态高抖动环境下的GPS/INS紧组合方法，其特征在于，包括以下步骤：

步骤1：获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据；

步骤2：对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据；

步骤3：将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。

2.根据权利要求1所述的GPS/INS紧组合方法，其特征在于，所述步骤2包括以下步骤：

步骤2.1：对INS原始数据进行小波分解：构造变换矩阵，确定分解层数，对INS原始数据进行N层小波分解，得到各层小波分解系数；

步骤2.2：对各层小波分解的高频系数进行阈值处理，去除高频部分的噪声，得到阈值处理后的小波分解的高频系数；

步骤2.3：对每一层小波分解的低频系数和阈值处理后的小波分解的高频系数进行小波重构，得到小波降噪后的INS数据。

3.根据权利要求1所述的GPS/INS紧组合方法，其特征在于，所述步骤3包括以下步骤：

步骤3.1：构建GPS/INS紧组合的状态方程：包括INS的误差状态和GPS的误差状态；

步骤3.2：构建GPS/INS紧组合的量测方程：包括伪距差观测方程和伪距率差观测方程；

步骤3.3：将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行kalman滤波解算，得到载体当前时刻在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息；

步骤3.4：重复步骤3.1至步骤3.3，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。