[发明专利]一种基于高精度惯性预积分的因子图组合导航方法

说明书

本发明公开了一种基于高精度惯性预积分的因子图组合导航方法，将地球自转角速率以及由于载体运动带来的导航系与地球坐标系的转动角速率的影响建模到传统惯性预积分模型中，同时考虑到地球参考椭球体的影响，计算得到高精度惯性预积分误差传递方程。在此基础之上，通过基于因子图模型的优化理论将IMU输出的导航信息与其它量测传感器所提供的导航信息相融合。再优化求解后得到载体的导航信息。本发明能够有效解决传统基于惯性预积分方法的因子图组合导航系统对惯性输出信息建模不精确，导致导航系统精度降低的问题，且可用于多种基于高精度惯性器件的因子图组合导航系统中。

技术领域

本发明属于组合导航技术领域，具体涉及一种基于高精度惯性预积分的因子图组合导航方法。

背景技术

因子图算法可同时利用历史时刻的多个量测信息对载体当前时刻的导航状态量进行估计，从而得到载体的导航信息。传统因子图算法忽略了高精度惯性器件自身的敏感特性，主要面向以低精度惯性导航器件为基础的多源信息融合组合导航系统。然而类似煤矿井下的完全封闭环境，其复杂的电磁干扰、无明显特征的环境特征使得激光雷达、视觉等某些导航传感器的适应性降低，需要依赖基于高精度惯性器件的组合导航系统以实现挖煤机器人的自主作业任务。因此，高精度惯性导航系统在复杂环境下载体的高精度定位中发挥着重要作用。

传统因子图算法使用惯性预积分方法处理惯性器件的输出信息，将相邻两优化时刻之间数百个的IMU测量结果，汇总为一个相邻两优化时刻间载体的相对运动约束。但传统惯性预积分方法忽略了地球自转角速率以及载体运动带来的导航系与地球坐标系的转动角速率，造成预积分模型精度下降，利用预积分求解的载体导航信息精度降低。同时，传统惯性预积分方法将地球水平面建模为平面，忽略了地球参考椭球体对载体运行过程中导航系带来的影响，造成长航时导航精度的降低。

因此，需要一种针对高精度惯性器件输出特性的高精度惯性预积分方法，以提高基于高精度惯性器件的因子图组合导航系统的导航精度。

发明内容

针对现有技术的不足，本发明的目的在于提供一种基于高精度惯性预积分的因子图组合导航方法，以提高导航的精度、适用性。

为达到上述目的，本发明的技术方案为：一种基于高精度惯性预积分的因子图组合导航方法，包括步骤如下：

步骤一：采集k时刻的惯性传感器数据，所述惯性传感器数据包括加速度计数据陀螺数据

步骤二：基于所述加速度计数据和陀螺数据计算高精度惯性预积分值及相邻两优化时刻之间载体导航信息，所述载体导航信息包括载体速度信息、载体位置信息；

步骤三：基于所述加速度计数据和所述陀螺数据计算得到高精度惯性预积分误差传递方程；

步骤四：当采集到其他导航传感器数据S(k+1)时，计算惯性残差，并联合其他量测传感器所提供的残差项优化求解载体导航信息。

步骤五：输出k+1时刻载体导航信息，并跳转至步骤一，用于求解k+2时刻载体导航信息。

优选地，所述步骤二具体为：

1)输出加速度计数据陀螺数据

其中，为机体系相对于导航系n在机体系b下的分量，即实际导航解算所需要的载体运动角速率，为陀螺的输出值，b_ω为陀螺零偏，n_ω为陀螺白噪声，为导航系到机体系的坐标转换矩阵，为载体实际运动的加速度在导航系下的分量，为机体系到导航系的坐标转换矩阵，f^b为加速度计的输出值，g为地球重力加速度，n_a为加速度计白噪声，b_a为加速度计零偏；为地球自转角速率在导航系下的投影，其中下标i表示惯性坐标系，e表示地球坐标系，为载体运动带来的导航系与地球坐标系的转动角速率；