基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法

摘要

本发明公开了基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，GNSS使用载波相位差分观测值求得位置量，采用捷联惯性导航的独立推算，结合惯导元器件的精度水平给出系统噪声协方差矩阵、状态向量的协方差矩阵进行正向滤波，使用第一次正向滤波结束时刻的估计结果作为反向滤波的初始信息，以反向滤波的结束时刻的估计结果作为第二次正向滤波的初始信息，反向滤波和第二次正向滤波的估计结果进行平滑得到最终的位置、速度和姿态角信息。本发提高GNSS/INS组合导航后处理过程中的精度水平，尤其是在GNSS卫星故障期间和观测数据的开始阶段的精度。

说明书

技术领域

本发明涉及高精度的GNSS/INS组合定位测姿后处理技术领域，具体是基于 三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法。

背景技术

在GNSS/INS组合定位定向数据处理过程中，人们关心的侧重点会随着应用 领域的不同而不同，如无人机的导航，关心的重点在于组合的实时性和可靠性， 而在测绘领域，数据可以进行后处理，相应的对于精度的要求会大幅度提高。在 城市峡谷中，常见高楼、隧道、立交桥等建筑物会对GNSS卫星的信号产生遮挡， 造成卫星失锁或者比较严重的多路劲效应，降低数据的质量。导致GNSS卫星无 法提供无时间积累误差的位置信息，原先的组合导航定定向模式会转变成惯性导 航系统单独工作，此时系统的精度主要取决于惯性元器件的精度、卫星失锁的时 间长短以及再失锁前整个系统的精度水平。惯性导航的原理也就决定了系统的精 度会受到时间积累的影响，对于高精度的后处理技术来说这是必须要解决的问题 点。

另一方面在GNSS/INS组合导航中，多使用的是卡尔曼滤波器进行组合处理， 此时滤波器中系统噪声的初试协方差矩阵、状态向量的协方差矩阵一般是根据经 验值预设的，这和实际的真值之间差值一般挺大，组合导航滤波器中特有的姿态 角对准精度也是不高。这些量都必须要在滤波器中进行估计后才能慢慢达到收敛， 收敛之前解算的数据精度通常不高。

以上两个问题点都会使得系统的精度水平波动很大，在实际的应用中，人们 最希望的是整个观测时间段内都能保持高精度的定位定向精度。因此有人采用双 向滤波结果加权平均的方法，有人采用顺向滤波，反向平滑的方法。除此之外， 少有文献和专利来解决这个问题。

发明内容

本发明的目的在于提供基于三向卡尔曼滤波平滑器的GNSS/INS组合定位 定向算法，有效解决了因GNSS数据丢弃期间IMU独立导航时参数快速漂移的 问题，提高GNSS/INS组合导航后处理过程中的精度水平，尤其是在GNSS卫星 故障期间和观测数据的开始阶段的精度。

为实现上述目的，本发明提供如下技术方案：

基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法，包括下列步骤：

(1)GNSS使用载波相位差分观测值求得厘米级的位置量，INS采用开始时 一定长度的观测数据做粗对准或者精对准得到初始的姿态角，利用所算的初始姿 态角进行捷联惯性导航的独立推算，结合惯导元器件的精度水平给出系统噪声协 方差矩阵Q，状态向量的协方差矩阵P₀，使用上述数据进行正向滤波，储存过 程中各个时刻的估计结果其中，是状态向量，k是刻度因子，P 是状态量的方差；

(2)在第一次正向滤波后，使用第一次正向滤波结束时刻的估计结果 作为反向滤波的初始信息，即第二次滤波，保存第二次过程中各个 时刻的估计结果

(3)以反向滤波的结束时刻的估计结果作为第二次正向滤波 的初始信息，即第三次滤波，同样保存第三次过程中各个时刻的估计结果

(4)以第二次滤波和第三次滤波的估计结果进行平滑得到最终的位置、速 度和姿态角信息。

作为本发明进一步的方案：反向滤波的初始历元是正向滤波的结束历元，初 始滤波参数是正向滤波结束时的参数。

作为本发明进一步的方案：第二次正向滤波的初始历元是反向滤波的结束历 元，初始滤波参数是反向滤波结束时的参数。

作为本发明进一步的方案：最终输出的结果是以反向滤波和第二次正向滤波 做平滑后的结果。

本发明的原理：

在GNSS/INS的组合定位测姿中，滤波的状态方程是非线性的，在经过线性 化后各状态量也就变成各状态的误差量，因而得采用扩展卡尔曼滤波的形式进行 组合，其状态、量测方程如下式：

$\stackrel{·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z＝HX(t)+V(2)

式中：X(t)是状态向量，其协方差矩阵为P阵，W(t)是系统噪声，其相应的 协方差矩阵为Q，G(t)是噪声驱动矩阵，F(t)是状态转移矩阵；Z是量测值，V的 协方差矩阵是R阵。

滤波开始时需要根据惯导元器件的精度水平给出系统噪声协方差矩阵Q，状 态向量的协方差矩阵P₀，而此时给出的P₀一般会比实际的值偏大，滤波器处于 发散状态，因而采用一次滤波就会导致系统开始阶段的组合精度不高。

当以地固系作为导航坐标系时，惯导系统的导航推算方程是：

$\begin{array}{c}{\mathrm{\Delta v}}^e={\int }_{t_k}^{t_{k+1}}{R}_b^e{f}^{b}dt-{\int }_{t_k}^{t_{k+1}}2{\Omega }_{ie}^e{V}^{e}dt+{\int }_{t_k}^{t_{k+1}}{g}^{e}dt\\ =R_b^e{f}^b\Delta t-2{\Omega }_{ie}^e{V}^e\Delta t+r^e\Delta t\end{array}---\left(3\right)$

$V_{k+1}^e=V_k^e+\frac{1}{2}({\mathrm{\Delta V}}_k^e+{\mathrm{\Delta V}}_{k+1}^e)---\left(4\right)$

$r_{k+1}^e=r_k^e+0.5(V_k^e+V_{K+1}^e)\Delta t---\left(5\right)$

即可见惯导的惯导推算是对先利用陀螺仪求出旋转矩阵(利用陀螺输出 的角速度算得)，按照公式把加速度计输出的f^b转化成f^e，经公式一次积 分得到速度v^e，公式二次积分得到位置x^e。而受惯导器材硬件属性的影响，其 输出的原始观测量中存在误差，需按照公式和进行误差补偿后，才能得到真值。

${\omega }_{{\mathrm{bi}}_{\mathrm{Re}al}}^b=k_b\times ({\omega }_{{\mathrm{bi}}_{Raw}}^b-b)---\left(6\right)$

${f_{\mathrm{Re}al}}^b=k_d\times ({f_{Raw}}^b-d)---\left(7\right)$

通常陀螺和加表的零偏、刻度因子的初值按照表1给出，这和其真值存在较 大偏差，需在组合导航中实时校正。基于以上两个原因，在本发明内容的步骤1 中，第一次正向滤波的开始阶段系统精度较低。当GNSS卫星有效的时候，可按 照公式对陀螺和加表的零偏量b、d，刻度因子k_b、k_d进行实时的校准，各状态 量的方差p_k/k降低；当GNSS卫星无效时，此时的导航参数只能由惯导单独算 得，即按照公式(3)-(5)计算，此时陀螺和加表的零偏量b、d，刻度因子k_b、 k_d只能使用失锁前的值，和系统当前的状态量不匹配，且从公式(3)-(5)可 知，利用惯导计算导航参数时，是基于积分的原理，所以误差会随着时间累积， 状态量的方差p_k/k只能依据公式推估得到，因而系统的精度随着时间累积而降 低。在进行正向滤波时，系统的精度在卫星无效期间的前半部分增长缓慢，反向 滤波时，卫星无效期间的后半部分误差增长缓慢(如图2所示)。即反应在本发 明的步骤2-3中，在卫星失锁期间，系统的精度随着时间累积而降低。随着卫星 的恢复和观测时间的推进，滤波器会逐渐趋于平稳，因采用第一次正向反向结束 时的结果作为反向滤波的初始信息，同理反向滤波的结束信息作为第二次正向滤 波的初始信息，因而本发明的步骤2-3中提到在观测数据开始阶段反向滤波和第 二次正向滤波均能保持高精度的系统状态。

在步骤4中，使用反向滤波和第二次正向滤波的结果做平滑，作为本发明的 最终结果。使用的平滑公式是公式(8)-(9)。

${\hat{x}}_{k/n}^s=p_{k/n}({p^{-1}}_{k/k}{\hat{x}}_{k/k}+p_{k/k}^{b-1}{\hat{x}}_{k/k+1}^b)---\left(8\right)$

$p_{k/n}^s={(p_{k/k}^{-1}+p_{k/k+1}^{b-1})}^T---\left(9\right)$

式中上标S表示最终的平滑结果，上标b表示反向滤波结果，k＝0,1,2,...,n-1,n。

与现有技术相比，本发明的有益效果是：

本发明提供了基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法， 以第一次正向滤波的结束历元结果做为反向滤波的起始历元和初始滤波参数，以 反向滤波的结束历元结果作为第二次正向滤波的起始历元和初始滤波参数，提高 单次正向滤波初始收敛前参数精度，有效解决了因GNSS数据丢弃期间IMU独 立导航时参数快速漂移的问题，提高了参数精度，尤其是位置参数。本发明提高 GNSS/INS组合导航后处理过程中的精度水平，尤其是在GNSS卫星故障期间和 观测数据的开始阶段的精度。

附图说明

图1是基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法流程图；

图2是基于三向卡尔曼滤波平滑器的GNSS/INS组合定位定向算法效果示意 图；

图3是车载运动轨迹图；

图4是人为删除GNSS结果轨迹图；

图5是经GNSS/INS单次正向滤波组合后的陀螺零偏图；

图6是经GNSS/INS单次正向滤波组合后的陀螺刻度因子图；

图7是经GNSS/INS单次正向滤波组合后的加表零偏图；

图8是经GNSS/INS单次正向滤波组合后的加表刻度因子图；

图9是本发明对于高斯投影东向坐标精度提高的效果图；

图10是本发明对于高斯投影北向坐标精度提高的效果图；

图11是本发明对于椭球高精度提高的效果图；

图12是本发明对于东向速度精度提高的效果图。

具体实施方式

下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地 描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。 基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获 得的所有其他实施例，都属于本发明保护的范围。

实施例1

本发明实施例中，如图1所示，基于三向卡尔曼滤波平滑器的GNSS/INS组 合定位定向算法，主要步骤包括GNSS载波相位差分解算模块以得到位置和速度 结果，SINS捷联惯性导航算法，状态向量先验协方差矩阵和系统噪声驱动矩阵 确定，正反向滤波算法，正反结果平滑滤波器平滑。

本发明的技术解决方案为基于三向卡尔曼滤波平滑器的GNSS/INS组合定 位定向算法。具体操作步骤如下：

(1)首先进行对整个观测时间段内的GNSS载波相位差分观测值求得厘米 级的位置、速度信息。

(2)选用观测开始时一定长度的IMU观测数据做粗对准或者精对准得到初 始的姿态角，利用所算的初始姿态角进行惯导的独立推算，结合惯导自身元器件 的精度水平给出卡尔曼滤波器的状态噪声协方差矩阵，以及状态向量的的协方差 矩阵，当该IMU历元处有GNSS定位结果时对相同时间点上的两种数据进行卡 尔曼滤波器的正向滤波，闭环反馈校正INS中各个状态误差，储存过程中各个 时刻的估计结果

(3)在第一次滤波后，使用第一次滤波结束时刻的估计结果作 为反向滤波的初始信息，这样第二次滤波就有了很准确的“先验信息”，从一开 始就能达到很高的定位定向精度。同样保存过程中各个时刻的估计结果 在GNSS卫星失锁期间，IMU导航误差是随时间累积的，反正滤波 能让卫星失锁期间后半段的数据有较好的质量。同时也解决在第一次正向滤波开 头数据因使用精确度不高的先验值而带来误差。

(4)虽然第二次结果使的观测开始阶段的精度提高了，但因第一次结果开 始阶段的精度并不高，若现采用前两次结果做平滑滤波时，这会拉低开始阶段的 精度，为此本发明提出了以第二次滤波结束时刻的估计结果作为第三次滤波的初 始信息，这也解决了观测开始阶段精度不高的问题。同样，也能够提高了GNSS 卫星失锁期间的前半段定位精度。

(5)以第二次和第三次滤波的估计结果进行平滑得到最终的位置、速度、 和姿态角信息。

为考察本发明对于GNSS失锁时系统输出量精度提升的效果，以具体数据说 明。以2014年5月20日在武汉某地的车载POS系统采集的数据进行分析，采 用战术级IMU，实验全程中GNSS观测条件良好(目的是为了模拟仿真出GNSS 失锁时输出量的真值，便于进行比对)，速度在15m/s，车辆首先静止约10分 钟，完成IMU热启动后，按照预先规划好的线路行驶，如图3所示。

首先计算出GNSS卫星未失锁时和IMU组合的结果，作为比对的基准，选 择路线中一段曲线路段，人为删除掉数据长度为2分钟的GNSS定位结果，如图 4所示，东西运动轨迹图，在该时间段只能由IMU进行独立导航，按照本发明 提出的三向滤波器和常规的正向滤波器分别计算，和完整GNSS/INS组合的结果 做对比。陀螺和加表的零偏、刻度因子初值如表2所示，图5、6、7、8依次画 出了经GNSS/INS单次正向滤波组合后的陀螺、加表的零偏和刻度因子值，图中 可见开始阶段零偏和刻度因子的振动较大，和预设的初值差别较大，滤波器未能 收敛，即反应出本发明内容的步骤1的单次正向滤波的开始阶段，系统稳定性差， 精度低，经过约20分钟后才追渐趋于收敛。

为了定量的对比本发明的优势，把人为的删除2分钟GNSS数据后得到的结 果和使用本发明解算后的结果都减去GNSS数据完整处理的结果，统计结果列在 表1中。图9至图12定性的画出了三种结果之间的差异。图9给出本发明对于 高斯投影东坐标精度提高的效果，2分钟时间内东方向最大漂移了约50m，使用 本发明后可以明显看出解算出东向坐标和GNSS数据完整时的重合度，最大漂移 量降至7m，大大提高了卫星失锁时的位置精度；图10给出本发明对于高斯投影 北坐标精度提高的效果，2分钟时间内北方向最大漂移了约1.5m，使用本发明后 降至0.8m；图11给出本发明对于椭球高精度提高的效果，2分钟时间内椭球高 方向最大漂移了约2.3m，使用本发明后降至0.6m；图12给出本发明对于东向速 度精度提升的效果，2分钟内东向速度最大漂移了约1.1m/s，使用本发明后降至 0.2m/s。表1中所列的其余统计项，因差异值较小，故没有用画出。表2是陀螺 仪和加表的零偏、刻度因子初值。

表1GNSS失锁且未三向滤波、三向滤波和GNSS数据完整的差值统计量

表2陀螺仪和加表的零偏、刻度因子初值

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而 且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发 明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性 的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要 求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方 式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领 域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组 合，形成本领域技术人员可以理解的其他实施方式。