【技术实现步骤摘要】
一种基于高精度惯性预积分的因子图组合导航方法
本专利技术属于组合导航
，具体涉及一种基于高精度惯性预积分的因子图组合导航方法。
技术介绍
因子图算法可同时利用历史时刻的多个量测信息对载体当前时刻的导航状态量进行估计，从而得到载体的导航信息。传统因子图算法忽略了高精度惯性器件自身的敏感特性，主要面向以低精度惯性导航器件为基础的多源信息融合组合导航系统。然而类似煤矿井下的完全封闭环境，其复杂的电磁干扰、无明显特征的环境特征使得激光雷达、视觉等某些导航传感器的适应性降低，需要依赖基于高精度惯性器件的组合导航系统以实现挖煤机器人的自主作业任务。因此，高精度惯性导航系统在复杂环境下载体的高精度定位中发挥着重要作用。传统因子图算法使用惯性预积分方法处理惯性器件的输出信息，将相邻两优化时刻之间数百个的IMU测量结果，汇总为一个相邻两优化时刻间载体的相对运动约束。但传统惯性预积分方法忽略了地球自转角速率以及载体运动带来的导航系与地球坐标系的转动角速率，造成预积分模型精度下降，利用预积分求解的载体导航信息精度降低。同时，传统惯性...
【技术保护点】
1.一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，包括如下步骤：/n步骤一：采集k时刻的惯性传感器数据，所述惯性传感器数据包括加速度计数据
【技术特征摘要】 【专利技术属性】
1.一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，包括如下步骤：
步骤一：采集k时刻的惯性传感器数据，所述惯性传感器数据包括加速度计数据陀螺数据
步骤二：基于所述加速度计数据和陀螺数据计算高精度惯性预积分值及相邻两优化时刻之间载体导航信息，所述载体导航信息包括载体速度信息、载体位置信息；
步骤三：基于所述加速度计数据和所述陀螺数据计算得到高精度惯性预积分误差传递方程；
步骤四：当采集到其他导航传感器数据S(k+1)时，计算惯性残差，并联合其他量测传感器所提供的残差项优化求解的载体导航信息；
步骤五：输出k+1时刻载体导航信息，并跳转至步骤一，用于求解k+2时刻载体导航信息。
2.根据权利要求1所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述步骤二具体为：
1)输出加速度计数据陀螺数据
其中，为机体系相对于导航系n在机体系b下的分量，即实际导航解算所需要的载体运动角速率，为陀螺的输出值，bω为陀螺零偏，nω为陀螺白噪声，为导航系到机体系的坐标转换矩阵，为载体实际运动的加速度在导航系下的分量，为机体系到导航系的坐标转换矩阵，fb为加速度计的输出值，g为地球重力加速度，na为加速度计白噪声，ba为加速度计零偏；为地球自转角速率在导航系下的投影，其中下标i表示惯性坐标系，e表示地球坐标系，为载体运动带来的导航系与地球坐标系的转动角速率；为载体运动导致导航系相对于地球系的运动速度在导航系上的投影；
2)相邻两优化时刻tk与tk+1之间，一共有l个IMU数据，计算载体姿态信息：
其中，nk+1、bk+1分别表示tk+1时刻的导航系与机体系，nk、bk分别表示tk时刻的导航系与机体系，分别表示tk与tk+1时刻的载体姿态信息，表示tk+1时刻高精度姿态预积分；
3)相邻两优化时刻tk与tk+1之间，计算载体速度信息：
其中，表示tk+1时刻导航系到tk时刻机体系的坐标转换矩阵，分别表示tk+1时刻与tk时刻载体速度信息，表示tk时刻地球自转角速率，表示tk时刻由载体运动带来的导航系与地球坐标系的转动角速率，ΔT表示相邻两优化时刻的时间间隔，表示tk时刻重力加速度，表示tk+1时刻高精度速度预积分；
4)相邻两优化时刻tk与tk+1之间，按如下形式计算载体位置信息：
其中，分别表示tk+1时刻与tk时刻载体在地球系下的位置信息，初始时刻载体在地球系下的位置为载体的经度、纬度、高度信息，表示tk时刻导航系到地球系的坐标转换矩阵。
3.根据权利要求2所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述的表达式为：
其中，表示载体东向运动速率，表示载体北向运动速率，h为载体所处高度，Rm为子午圈曲率半径，Rn为卯酉圈曲率半径，L为载体所处位置的纬度信息。
4.根据权利要求3所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述的表达式为：
其中，bk、bk+1分别表示初始tk与tk+1时刻的机体坐标系，表示相邻两优化时刻tk与tk+1之间某一IMU采样时刻机体系到tk时刻机体系的相对姿态量，该信息用四元数表示，Ω(·)按如下方式进行表示：
5.根据权利要求4所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述的表达式为：
其中，表示相邻两优化时刻tk与tk+1之间某一IMU采样时刻机体系到tk时刻机体系的坐标转换矩阵，表示其中某一IMU采样时刻加速度计的输出，表示该时刻加速度计零偏。
6.根据权利要求5所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述的表达式为：
表示tk+1时刻高精度位置预积分：
7.根据权利要求6所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述步骤三具体为：
1)计算误差状态转移矩阵Fi与噪声状态转移矩阵Gi：
其中，δt表示IMU采样时间间隔，I表示单位矩阵，R(γ)表示将四元数转化为旋转矩阵，
技术研发人员：吕品，王炳清，赖际舟，白师宇，李志敏，许晓伟，岑益挺，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏;32
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。