一种低动态高抖动环境下的gps/ins紧组合方法
【专利摘要】本发明提供一种低动态高抖动环境下的GPS/INS紧组合方法，该方法获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据；对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据；将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。本发明解决了在低动态高抖动环境中常规GPS/INS紧组合导航结果不可靠的问题，大幅提升导航定位的精度，能满足高精度的导航应用要求，不仅提供载体的位置信息，还可以提供载体的速度信息和姿态状态信息，能适应于更广泛的应用场景，还具有实时高精度导航的潜力。
【专利说明】
一种低动态高抖动环境下的GPS/1 NS紧组合方法
技术领域
[0001] 本发明属于导航技术领域，具体涉及一种低动态高抖动环境下的GPS/INS紧组合 方法。
【背景技术】
[0002] GPS/INS组合导航能高效利用各自的特点进行优势互补，是目前最理想组合导航 方式。相对于GPS/INS松组合，GPS/INS紧组合具有更高的导航精度，能进行复杂环境的导航 定位，是目前GPS/ INS组合中常用的组合方式。
[0003] 组合导航中的复杂环境多指GPS卫星被遮挡、GPS信号不稳定的环境。本文发明认 为该复杂环境不仅应包括上述情况，还应考虑由于外界环境和量测手段等导致INS原始数 据中存在大量噪声以及不连续的环境。例如，当搭载GPS和INS的载体行驶速度缓慢，但该载 体却处于高频不规则振动中，本发明将该环境简称为低动态高抖动环境。
[0004] 这种低动态高抖动环境将同时GPS和INS的定位解算：对于GPS，快速抖动会导致 GPS多路径效应影响变大，甚至会导致GPS卫星信号跟踪失锁;对于INS，低动态高抖动会使 惯性器件的观测信号淹没在高频噪声中。在该环境下GPS信号的处理已有较合适的方法，但 对于该环境下的INS信号处理一直没有得到较好的重视。低动态高抖动环境下的INS观测值 误差会大幅降低GPS/INS紧组合导航定位精度，甚至使GPS/INS紧组合发生错误。
【发明内容】
[0005] 针对现有技术的不足，本发明提出一种低动态高抖动环境下的GPS/INS紧组合方 法。
[0006] 本发明的技术方案是：
[0007] -种低动态高抖动环境下的GPS/INS紧组合方法，包括以下步骤：
[0008] 步骤1:获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据； [0009]步骤2:对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据；
[0010]步骤2.1:对INS原始数据进行小波分解:构造变换矩阵，确定分解层数N，对INS原 始数据进行N层小波分解，得到各层小波分解系数；
[0011] 步骤2.2:对各层小波分解的高频系数进行阈值处理，去除高频部分的噪声，得到 阈值处理后的小波分解的高频系数；
[0012] 步骤2.3:对每一层小波分解的低频系数和阈值处理后的小波分解的高频系数进 行小波重构，得到小波降噪后的INS数据。
[0013] 步骤3:将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过 程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息。
[0014] 步骤3.1:构建GPS/INS紧组合的状态方程:包括INS的误差状态和GPS的误差状态； [0015] 步骤3.2:构建GPS/INS紧组合的量测方程:包括伪距差观测方程和伪距率差观测 方程；
[0016] 步骤3.3:将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行kalman 滤波解算，得到载体当前时刻在当地地理坐标系中的位置、当地地理坐标系下三方向的速 度和姿态信息；
[0017] 步骤3.4:重复步骤3.1至步骤3.3，得到整个导航过程中载体在当地地理坐标系中 的位置、当地地理坐标系下三方向的速度和姿态信息。
[0018]本发明的有益效果：
[0019] 本发明提出一种低动态高抖动环境下的GPS/INS紧组合方法，本发明解决了在低 动态高抖动环境中常规GPS/INS紧组合导航结果不可靠的问题，大幅提升导航定位的精度， 能满足高精度的导航应用要求;本发明不仅提供载体的位置信息，还可以提供载体的速度 信息和姿态状态信息;本发明中能够提供高达200HZ的数据更新结果，能适应于更广泛的应 用场景，还具有实时高精度导航的潜力。
【附图说明】
[0020] 图1为本发明【具体实施方式】中低动态高抖动环境下的GPS/INS紧组合方法的流程 图；
[0021 ]图2为本发明【具体实施方式】中实验小车的示意图；
[0022]图3为本发明【具体实施方式】中陀螺Y轴的原始数据；
[0023]图4为本本发明【具体实施方式】中加速度计Y轴的原始数据；
[0024]图5为本发明【具体实施方式】中GPS卫星的roop值；
[0025]图6为本发明【具体实施方式】中对INS原始数据进行小波阈值降噪处理的流程图；
[0026] 图7为本发明【具体实施方式】中三层小波分解的示意图；
[0027] 图8为本发明【具体实施方式】中陀螺Y轴的小波降噪后的数据；
[0028] 图9为本发明【具体实施方式】中加速度计Y轴的小波降噪后的数据；
[0029]图10为本发明【具体实施方式】中将小波降噪后的INS数据与GPS原始数据进行紧组 合处理的流程图；
[0030] 图11为本发明【具体实施方式】中小波降噪后的INS数据与GPS原始数据紧组合解算 轨迹、常规GPS/INS紧组合解算轨迹以参考真值轨迹图。
[0031] 图12为本发明【具体实施方式】中小波降噪后的INS数据与GPS原始数据的解算高程、 常规GPS/INS紧组合解算的高程以参考真值的高程图。
【具体实施方式】
[0032]下面结合附图对本发明【具体实施方式】加以详细的说明。
[0033] 一种低动态高抖动环境下的GPS/INS紧组合方法，如图1所示，包括以下步骤：
[0034]步骤1:获取低动态高抖动环境下的载体的INS原始数据以及同步的GPS原始数据。 [0035]本实施方式中，为了验证一种低动态高抖动环境下的GPS/INS紧组合方法的有效 性，进行实测实验，实验小车如图2所示无任何缓冲设施，实验人员步行推动小车先通过一 段较平缓路面(含部分砂砾），然后通过复杂环境(含多砂砾和碎石块路面，多次穿越台阶）， 最后再通过一段较平缓路面(含部分砂砾)并结束实验;实验小车行驶速度小于1.5m/s，行 驶平均速度约为〇. 6m/s。
[0036]本实施方式中，陀螺Y轴的原始输数据如图3所示，加速度计Y轴的原始数据如图4 所示。GPS卫星的ro〇P值如图5所示，其中倒三角为阶段间分割标记，由图5可知，整个实验中 GPS观测环境较为复杂，存在信号不稳定及信号中断。
[0037]步骤2:对INS原始数据进行小波阈值降噪处理，得到小波降噪后的INS数据，如图6 所示。
[0038]步骤2.1:对INS原始数据进行小波分解:构造变换矩阵，确定分解层数，对INS原始 数据进行N层小波分解，得到各层小波分解系数。
[0039] 本实施方式中，对含噪声的INS原始数据X进行小波分解:构造变换矩阵W，确定分 解层数N，对INS原始数据X进行N层小波分解如式(1)所示：
[0040] A" = WX ⑴
[0041 ]其中，f为INS原始数据X的小波变换。
[0042]步骤2.2:对各层小波分解的高频系数进行阈值处理，去除高频部分的噪声，得到 阈值处理后的小波分解的高频系数。
[0043]本实施方式中，对各层小波分解的高频系数进行阈值处理的软阈值函数如式(2) 所示：
[0044] d = \ h X ^ .1 7 1 (2)
[0045] 其中，d为未处理的小波分解的高频系数，J为阈值处理后的小波分解的高频系数， ],d >0 入为设定的阈值，sgn(t/) = z 0，(?/ = 0 : -\,d <0
[0046] 步骤2.3:对每一层小波分解的低频系数和阈值处理后的小波分解的高频系数进 行小波重构，得到小波降噪后的INS数据。
[0047] 本实施方式中，对每一层小波分解的低频系数和阈值处理后的小波分解的高频系 数进行小波重构的公式如式(3)所示：
[0048] X} = Wrl' (3)
[0049] 其中，V为小波降噪后的INS数据，|?为小波分解的低频系数和阈值处理后的小波 系数。
[0050] 本实施方式中，三层小波分解的示意图如图7所示，其中，S为高频系数部分，D为低 频系数部分。陀螺Y轴的小波降噪后的数据如图8所示，加速度计Y轴的小波降噪后的数据如 图9所示，由图8和图9可以看出小波降噪方法能有效分离INS原始数据中的噪声和有用信 号，能剔除大量的噪声。
[0051] 步骤3:将小波降噪后的INS数据与GPS原始数据进行紧组合处理，得到整个导航过 程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息，如图 10所示。
[0052] 步骤3.1:构建GPS/INS紧组合的状态方程:包括INS的误差状态和GPS的误差状态。 [0053]本实施方式中，INS的误差状态包括当地地理坐标系下三方向的位置误差、当地地 理坐标系下三方向的速度误差、当地地理坐标系下三方向的姿态角误差、当地地理坐标系 下三轴的陀螺漂移和当地地理坐标系下三轴的加速度计漂移。
[0054] GPS的误差状态包括：与接收机钟误差等效的距离误差CStu和与接收机钟频率误 差等效的距离率误差CStru，如式(4)所示： \ C 8t = C St + (〇t
[0055] { (4) \cam=-C(38tni+mtm
[0056] 其中，0)飞,为与接收机钟误差等效的距离误差的微分形式，€^4为与接收机钟频 率误差等效的距离率误差的微分形式，C为光速，0为相关时间，co tuS与接收机钟误差等效 的距离误差的驱动白噪声，《 tru为与接收机钟误差等效的距离误差的驱动白噪声。
[0057] 本实施方式中，构建的GPS/INS紧组合的状态方程如式(5)所示： X Jt, 1 0. 「X& 0 T WrK
[0058] ? JA = ' + (5) jGK _ - 〇 ^GK GK _ _ 〇 G(；K._ _'GK _
[0059] 其中，K你 & 说 ^ 如％ 咚伙.％ % c ^ \ vJr，t 为Xk的微分形式，SB、SL、Sh为当地地理坐标系下三方向的位置误差，SVE、SVN、S VU为当地地 理坐标系下三方向的速度误差，外、朽、只为当地地理坐标系下三方向的姿态角误差， ebx、eby、ebz为当地地理坐标系下三轴的陀螺漂移，、V fe为当地地理坐标系下三轴 的加速度计漂移，Fk为小波降噪后INS的状态转移矩阵，Gk为小波降噪后INS的动态噪声矩 「C没 1 「1 0] 「1 0 _ 阵，WlK为INS的系统过程卩栄声向M，? Fe：K = ^ , GGk = ^ ^ , L??」
[0060]本实施方式中，小波降噪后INS的动态噪声矩阵Gik如式(6)所不： 0 6x3 0.6x3 C" 0,,
[0061] Glk^ b JX3 (6) ^*3x3 ^；3x3 _〇3x3 ^3 _isx6
[0062] 其中，为载体坐标系到当地地理坐标系的姿态矩阵，I3为单位矩阵。
[0063 ] 本实施方式中，INS的系统过程噪声向量Wik如式(7)所示：
[0064] WlK-[wgx Wgy Wgz Wax Way Waz ] (7 )
[0065] 其中，Wgx为陀螺x轴的噪声，wgy为陀螺y轴的噪声，w gz为陀螺z轴的噪声，wax为加速 度计x轴的噪声，way为加速度计y轴的噪声，w az为加速度计z轴的噪声。
[0066]本实施方式中，小波降噪后的INS的状态转移矩阵Fik如式(8)所示： 一 'p p
[0067] Fik = w s (80 L〇_ 〇6x&」15x15 〇3x3 〇3x3
[0068] 其中，€=〇3、3 ，FW对应9个误差的状态转移矩阵，非零元素为： _Q 〇3x3_9x6
度，L为载体在当地地理坐标系中的经度，h为载体在当地地理坐标系中的高，VN为当地地理 坐标系下的北向速度，VE为当地地理坐标系下的东向速度，VU为当地地理坐标系下的垂向速 度，Wie为地球角速率，fx为比力向量在当地地理坐标系下的X轴方向分量，f y为比力向量在 当地地理坐标系下的y轴方向分量，f z为比力向量在当地地理坐标系下的z轴方向分量。 [0069]将公式(5)离散化得式(9)如下：
[0070] Xk=① k,k-iXk-i+Wk (9)
[0071] 其中，为k时亥IJGPS/INS紧组合的状态向量，Xk-Ak-l时亥IJGPS/INS紧组 合的状态向量，wk为噪声向量，其协方差矩阵为Ewk，〇k,k-1为离散化之后GPS/INS紧组合的 状态转移矩阵。
[0072] 步骤3.2:构建GPS/INS紧组合的量测方程:包括伪距差观测方程和伪距率差观测 方程。
[0073] 本实施方式中，伪距差观测方程如式(10)所示：
[0074] 8pj = eji8x+ej28y+ej38z-8tu-Vpj (10)
[0075]其中，Spj为GPS得到的伪距与INS得到的伪距的差值，ejl为卫星观测方向与x轴的 方向余弦，e」2为卫星观测方向与y轴的方向余弦，ej3为卫星观测方向与z轴的方向余弦，j = l...n，Sx为x轴方向上的位置误差，Sy为y轴方向上的位置误差，S Z为z轴方向上的位置误 差，Stu为GPS接收机钟误差，vPj为GPS得到的伪距观测噪声。
[0076] 其中，载体在地心地固坐标系中位置与载体在当地地理坐标系中为炜度、经度和 尚的关系如式（11)所不： a* --(R + h)cosLcos B
[0077] v = (/? + /?)sinZ.cos5 、11) z = \^R{\ - e2) + h | sin 5
[0078] 其中，x为载体在地心地固坐标系的x轴，y为载体在地心地固坐标系的y轴，z为载 体在地心地固坐标系的z轴。
[0079] 伪距率差观测方程如式（12)所示：
[0080] Sp j = e jX8x + ej2Sy + e l3Sz - Stm - V Aj 112)
[0081] 其中为伪距率误差，次e为x轴方向上的位置误差的导数，在j，为y轴方向上的位 置误差的导数，泡为z轴方向上的位置误差的导数，Stru为GPS接收机钟频率误差，va；为GPS 得到的伪距率观测噪声。
[0082] 则可以得到GPS/INS紧组合的量测方程如式（13)所示：
[0085]将公式（13)离散化可得公式（14)如下：
[0086] Zk = HkXk+Vk (14)
[0087] 其中，為=k为k时刻GPS/INS紧组合的观测向量，Hk为k时刻GPS/INS紧组合的 _ap_ 量测矩阵，Vk为k时刻的量测噪声，其协方差矩阵为EVk。
[0088] 步骤3.3:将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行kalman 滤波解算，得到载体当前时刻在当地地理坐标系中的位置、当地地理坐标系下三方向的速 度和姿态信息。
[0089] 本实施方式中，将GPS/INS紧组合的状态方程和GPS/INS紧组合的量测方程进行 kalman滤波解算，得到解如式（15)所示：
[0090] X1 = ^ +K,(Z,-!!,X, ) K 15)
[0091]其中，尤！为Xk的kalman滤波估值，不=气，& =4_扣(凡1双f +尾r丨为 滤波增益矩阵，f 乃二中Lm +&M为预测的状态向量协方差阵，Rk为量测噪声Vk的 对称正定方差阵，Qk为噪声向量wk的非负定方差阵，/匕为当前时刻状态向量的协方差阵。 [0092] 本实施方式中，得到的载体当前时刻在当地地理坐标系中的位置、当地地理坐标 系下三方向的速度和姿态信息的一组数据如下：
[0093] GPS时间：285582.000s、经度：121.6551869212deg、炜度：42.0492113952deg、高： 166.828111、东向速度 ：0.038111/8、北向速度：1.105111/8、垂向速度：-0.077111/8、横滚角 ：-0 ? 02281844deg、俯仰角：2 ? 04958623deg、航向角：4 ? 06268929deg。
[0094] 步骤3.4:重复步骤3.1至步骤3.3，得到整个导航过程中载体在当地地理坐标系中 的位置、当地地理坐标系下三方向的速度和姿态信息。
[0095]本实施方式中，通过kalman解算得到的状态估值作为下一时刻的状态估值， 重复步骤3.1至步骤3.3，再构建新的GPS/INS紧组合的状态方程和量测方程进行kalman解 算得到新的状态估值，直至所有的小波降噪后的INS数据与GPS原始数据迭代完成，则可以 得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和 姿态信息。
[0096]本实施方式中，小波降噪后的INS数据与GPS原始数据紧组合解算轨迹、常规GPS/ INS紧组合解算轨迹以参考真值轨迹图如图11所示，小波降噪后的INS数据与GPS原始数据 的解算高程、常规GPS/INS紧组合解算的高程以参考真值的高程图如图12所示。
[0097]由图11和图12可知，在低动态高抖动环境中，由于GPS观测环境复杂，IMU数据中存 在大量噪声，常规GPS/INS解算平面和高程结果存在较大漂移，无法正常导航定位，而本发 明的小波降噪的GPS/INS紧组合中有效降低了頂U中噪声对导航结果的影响，大幅提升导航 定位的精度。
[0098]在低动态高抖动环境中，当GPS观测环境较好时，小波降噪的GPS/INS紧组合能将 导航定位精度从5m提升至3m以内；当GPS观测环境较差(GPS信号不稳定，存在信号多次中 断)时，常规GPS/INS紧组合精度均大于15m，且存在大幅的漂移，定位结果不可靠，小波降噪 的GPS/INS紧组合能大幅提升导航定位精度至5m左右，大幅提升导航定位的稳定可靠性，使 低动态高抖动环境中的导航定位具有更好的实用性。
【主权项】
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，得到整个导航过程中载体在当地地理坐标系中的位 置、当地地理坐标系下三方向的速度和姿态信息。
【文档编号】G01S19/47GK105929431SQ201610243007
【公开日】2016年9月7日
【申请日】2016年4月19日
【发明人】徐爱功, 刘韬, 隋心, 郝雨时, 郭哲, 王长强, 杨东辉
【申请人】辽宁工程技术大学