CN113175933A - 一种基于高精度惯性预积分的因子图组合导航方法 - Google Patents

Abstract

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

Description

一种基于高精度惯性预积分的因子图组合导航方法

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

陀螺数据

步骤二：基于所述加速度计数据

和陀螺数据

计算高精度惯性预积分值及相邻两优化时刻之间载体导航信息，所述载体导航信息包括载体速度信息、载体位置信息；

步骤三：基于所述加速度计数据

和所述陀螺数据

计算得到高精度惯性预积分误差传递方程；

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

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

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

1)输出加速度计数据

陀螺数据

其中，

为机体系相对于导航系n在机体系b下的分量，即实际导航解算所需要的载体运动角速率，

为陀螺的输出值，b_ω为陀螺零偏，n_ω为陀螺白噪声，

为导航系到机体系的坐标转换矩阵，

为载体实际运动的加速度在导航系下的分量，

为机体系到导航系的坐标转换矩阵，f^b为加速度计的输出值，g为地球重力加速度，n_a为加速度计白噪声，b_a为加速度计零偏；

为地球自转角速率在导航系下的投影，其中下标i表示惯性坐标系，e表示地球坐标系，

为载体运动带来的导航系与地球坐标系的转动角速率；

2)相邻两优化时刻t_k与t_k+1之间，一共有l个IMU数据，计算载体姿态信息：

其中，n_k+1、b_k+1分别表示t_k+1时刻的导航系与机体系，n_k、b_k分别表示t_k时刻的导航系与机体系，

分别表示t_k与t_k+1时刻的载体姿态信息，

表示t_k+1时刻高精度姿态预积分；

3)相邻两优化时刻t_k与t_k+1之间，计算载体速度信息：

其中，

表示t_k+1时刻导航系到t_k时刻机体系的坐标转换矩阵，

分别表示t_k+1时刻与t_k时刻载体速度信息。

表示t_k时刻地球自转角速率，

表示t_k时刻由载体运动带来的导航系与地球坐标系的转动角速率，ΔT表示相邻两优化时刻的时间间隔，

表示t_k时刻重力加速度，

表示t_k+1时刻高精度速度预积分；

4)相邻两优化时刻t_k与t_k+1之间，按如下形式计算载体位置信息：

其中，

分别表示t_k+1时刻与t_k时刻载体在地球系下的位置信息，初始时刻载体在地球系下的位置为载体的经度、纬度、高度信息，

表示t_k时刻导航系到地球系的坐标转换矩阵。

优选地，所述

的表达式为：

其中，

表示载体东向运动速率，

表示载体北向运动速率，h为载体所处高度，R_m为子午圈曲率半径，R_n为卯酉圈曲率半径，L为在台所处位置的纬度信息。

优选地，所述

的表达式为：

其中，b_k、b_k+1分别表示初始t_k与t_k+1时刻的机体坐标系，

表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻机体系到t_k时刻机体系的相对姿态量，该信息用四元数表示，Ω(·)按如下方式进行表示：

优选地，所述

的表达式为：

其中，

表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻机体系到t_k时刻机体系的坐标转换矩阵，

表示其中某一IMU采样时刻加速度计的输出，

表示该时刻加速度计零偏。

优选地，所述

的表达式为：

表示t_k+1时刻高精度位置预积分：

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

1)计算误差状态转移矩阵F_i与噪声状态转移矩阵G_i：

其中，δt表示IMU采样时间间隔，I表示单位矩阵，R(γ)表示将四元数转化为旋转矩阵，

2)对于相邻两个IMU采样时刻t_i与t_i+1，系统的误差传递方程A_i+1、雅可比矩阵

与协方差P_i+1按如下形式进行计算：

A_i+1＝F_iA_i+G_iN

P_i+1＝F_iP_iF_i ^T+G_iQG_i ^T

其中：

其中，

表示t_i时刻载体的位置预积分误差状态，

表示t_i时刻载体的速度预积分误差状态，

表示t_i时刻载体的姿态预积分误差状态、

分别表示t_i时刻载体的加速度计、陀螺的零偏误差状态；

表示t_i时刻陀螺与加速度计的白噪声，

表示t_i+1时刻陀螺与加速度计的白噪声，

表示加速度计零偏白噪声，

表示陀螺零偏白噪声；

分别为t_i时刻、t_i+1时刻系统状态的雅可比矩阵，初始时刻系统状态的雅可比矩阵为单位矩阵；Q为噪声N的协方差矩阵，P_i、P_i+1分别表示t_i时刻、t_i+1时刻惯性量测协方差矩阵，初始时刻为0。

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

1)当采集到其他导航传感器数据S(k+1)时，记该t_k+1时刻机体系为b_k+1，在t_k时刻到t_k+1时刻间，一共有l个IMU数据，然后计算补偿过误差后的载体的位置预积分值

速度预积分值

与姿态预积分值

基于所述步骤三所给出的相邻两IMU帧的惯性预积分的误差状态从第一帧IMU数据，递推至第l帧IMU数据计算A_l得出：

其中：

计算惯性残差

其中，姿态用四元数表示，

表示提取四元数的虚部，

为中间变量；

2)基于步骤三中的相邻两IMU帧的协方差矩阵P_i+1计算惯性残差的增量方程：

其中，

表示惯性残差

相对待优化状态变量X的雅可比矩阵，δX表示优化过程中的误差状态量，

表示t_k时刻到t_k+1时刻间系统的协方差矩阵，由步骤三所给出的相邻两IMU帧的协方差矩阵递推计算得出；

3)惯性残差

相对待优化状态变量X的雅可比矩阵

按如下方式进行计算：由惯性残差

的表达式可知，其相对待优化状态变量x_s与x_λ的雅可比矩阵为0，即：

将剩余待优化变量

分为四组：

记惯性残差相对待优化变量的雅可比矩阵为J[0]、J[1]、J[2]、J[3]。

J[0]、J[1]、J[2]、J[3]按如下形式进行计算：

其中，J[0]₁₂、J[0]₁₃、J[0]₂₂、J[0]₃₂按如下形式进行计算：

对于四元数q＝[x y z λ]＝[ω λ]，[·]_L与[·]_R可表示为：

4)联合其他量测传感器所提供的误差项，按如下方式构建目标函数：

其中，||e_p-H_pX||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其他测量和状态则被边缘化并转换为先验；

为惯性残差，B是所有IMU测量的集合，

为惯性量测协方差；

为其他导航传感器所提供的误差项，

表示其他导航传感器协方差矩阵。

优选地，所述

所述

所述

的表达式分别为：

其中，各雅可比矩阵按如下方式进行计算：

其中，

分别表示t_k时刻到t_k+1时刻间，载体的位置预积分量、速度预积分量与姿态预积分量的误差状态。

优选地，所述δX包括其他导航传感器与惯性传感器的相对平移与旋转的误差量δx_s，其他导航传感器所提供的误差状态量δx_λ，滑动窗口内n+1个量测优化帧的载体位置、速度、四元数、加速度计和陀螺零偏的误差状态量：

δX＝[δx₀,δx₁,…,δx_n,δx_s,δx_λ]

表示惯性残差

相对待优化状态变量X的雅可比矩阵，具体地，待优化状态变量X为：

X＝[x₀,x₁,…,x_n,x_s,x_λ]

其中x_n表示滑动窗口内某个量测优化帧的载体位置、速度、姿态、加速度计和陀螺的零偏；x_s表示其它量测传感器与IMU的相对平移与旋转量；x_λ表示其他导航传感器所提供的误差。

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

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明方法的流程示意图；

图2为卫星阶段性拒止时X方向轨迹误差对比图；

图3为卫星阶段性拒止时Y方向轨迹误差对比图。

具体实施方式

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

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1-3所示，本发明提供一种基于高精度惯性预积分的因子图组合

步骤一：采集加速度计数据

和陀螺数据

步骤二：利用加速度计数据

和陀螺数据

计算高精度惯性预积分值，并更新载体导航信息。

1)输出加速度计与陀螺：

其中，

为机体系相对于导航系(n)在机体系(b)下的分量，即实际导航解算所需要的载体运动角速率，

为陀螺的输出值，b_ω为陀螺零偏，n_ω为陀螺白噪声，

为导航系到机体系的坐标转换矩阵。

为载体实际运动的加速度在导航系下的分量，

为机体系到导航系的坐标转换矩阵，f^b为加速度计的输出值，g为地球重力加速度，n_a为加速度计白噪声，b_a为加速度计零偏。

为载体运动导致导航系相对于地球系的运动速度在导航系上的投影，直观上可以理解为载体的运动速率。

为地球自转角速率在导航系下的投影，其中下标i表示惯性坐标系，e表示地球坐标系。

为载体运动带来的导航系与地球坐标系的转动角速率，具体地，按如下形式进行计算：

其中，

表示载体东向运动速率，

表示载体北向运动速率，h为载体所处高度，R_m为子午圈曲率半径，R_n为卯酉圈曲率半径，L为载体所处位置的纬度信息。

2)相邻两优化时刻t_k与t_k+1之间，一共有l个IMU数据，按如下形式计算载体姿态信息：

其中，n_k+1、b_k+1分别表示t_k+1时刻的导航系与机体系，n_k、b_k分别表示t_k时刻的导航系与机体系，

分别表示t_k与t_k+1时刻的载体姿态信息。

表示t_k+1时刻高精度姿态预积分，具体地，按如下形式进行计算：

其中，b_k、b_k+1分别表示初始t_k与t_k+1时刻的机体坐标系，

表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻机体系到t_k时刻机体系的相对姿态量，该信息用四元数表示。

分别表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻陀螺的输出值、脱落零偏、地球自转角速率在导航系下的投影、载体运动带来的导航系与地球坐标系的转动角速率。

表示t_k时刻导航系到机体系的坐标转换矩阵。Ω(·)按如下方式进行表示：

3)相邻两优化时刻t_k与t_k+1之间，按如下形式计算载体速度信息：

其中，

表示t_k+1时刻导航系到t_k时刻机体系的坐标转换矩阵，

分别表示t_k+1时刻与t_k时刻载体速度信息。

表示t_k时刻地球自转角速率，

表示t_k时刻由载体运动带来的导航系与地球坐标系的转动角速率，ΔT表示相邻两优化时刻的时间间隔，

表示t_k时刻重力加速度。

表示t_k+1时刻高精度速度预积分，具体地，按如下形式进行计算：

其中，

表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻机体系到t_k时刻机体系的坐标转换矩阵，

表示其中某一IMU采样时刻加速度计的输出，

表示该时刻加速度计零偏，n_a为加速度计白噪声。

4)相邻两优化时刻t_k与t_k+1之间，按如下形式计算载体位置信息：

其中，

分别表示t_k+1时刻与t_k时刻载体在地球系下的位置信息，初始时刻载体在地球系下的位置为载体的经度、纬度、高度信息。

表示t_k时刻导航系到地球系的坐标转换矩阵，具体地，按如下形式进行计算：

R_m为子午圈曲率半径，R_n为卯酉圈曲率半径，L为载体所处位置的纬度信息；

表示t_k+1时刻高精度位置预积分，具体地，按如下形式进行计算：

步骤三：利用惯性传感器数据

和

计算高精度惯性预积分误差传递方程

1)系统的误差状态转移矩阵F_i与噪声状态转移矩阵G_i按如下形式进行计算：

其中，δt表示IMU采样时间间隔，I表示单位矩阵，R(γ)表示将四元数转化为旋转矩阵。

矩阵中其它未显示表达的元素按如下形式进行计算：

2)对于相邻两个IMU采样时刻t_i与t_i+1，系统的误差传递方程、雅可比矩阵与协方差按如下形式进行计算：

A_i+1＝F_iA_i+G_iN

P_i+1＝F_iP_iF_i ^T+G_iQG_i ^T

其中：

其中，

表示t_i时刻载体的位置预积分误差状态，

表示t_i时刻载体的速度预积分误差状态，

表示t_i时刻载体的姿态预积分误差状态、

分别表示t_i时刻载体的加速度计、陀螺的零偏误差状态；

表示t_i时刻陀螺与加速度计的白噪声，

表示t_i+1时刻陀螺与加速度计的白噪声，

表示加速度计零偏白噪声，

表示陀螺零偏白噪声；

分别为t_i时刻、t_i+1时刻系统状态的雅可比矩阵，初始时刻系统状态的雅可比矩阵为单位矩阵；Q为噪声N的协方差矩阵，P_i、P_i+1分别表示t_i时刻、t_i+1时刻惯性量测协方差矩阵，初始时刻为0。

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

1)当采集到GPS导航传感器数据S(k)时，记该t_k+1时刻机体系为b_k+1，在t_k时刻到t_k+1时刻间，一共有l个IMU数据，补偿过误差后的载体的位置预积分值、速度预积分值与姿态预积分值

按如下形式进行计算：

其中，各雅可比矩阵按如下方式进行计算：

其中，

分别表示t_k时刻到t_k+1时刻间，载体的位置预积分量、速度预积分量与姿态预积分量的误差状态，由步骤三所给出的相邻两IMU帧的惯性预积分的误差状态从第一帧IMU数据，递推至第l帧IMU数据计算A_l得出，具体地，有：

其中：

由此，惯性残差按如下形式进行计算：

其中，姿态用四元数表示，

表示提取四元数的虚部，

用于简化表示的记号式，具体地，按如下形式进行计算：

2)惯性残差的增量方程按如下方式进行计算：

其中，

表示惯性残差

相对待优化状态变量X的雅可比矩阵，

表示t_k时刻到t_k+1时刻间系统的协方差矩阵，由步骤三所给出的相邻两IMU帧的协方差矩阵递推计算得出。

δX表示优化过程中的误差状态量，包括GPS传感器与惯性传感器的相对平移与旋转的误差量δx_s，GPS传感器所提供的误差状态量δx_λ，滑动窗口内n+1个量测优化帧的载体位置、速度、四元数、加速度计和陀螺零偏的误差状态量：

δX＝[δx₀,δx₁,…,δx_n,δx_s,δx_λ]

表示惯性残差

相对待优化状态变量X的雅可比矩阵，具体地，待优化状态变量X为：

X＝[x₀,x₁,…,x_n,x_s,x_λ]

其中x_n表示滑动窗口内某个量测优化帧的载体位置、速度、姿态、加速度计和陀螺的零偏；x_s表示GPS传感器与IMU的相对平移与旋转量；x_λ表示GPS传感器所提供的误差。

3)惯性残差

相对待优化状态变量X的雅可比矩阵

按如下方式进行计算：

由惯性残差

的表达式可知，其相对待优化状态变量x_s与x_λ的雅可比矩阵为0，即：

将剩余待优化变量

分为四组：

记惯性残差相对待优化变量的雅可比矩阵为J[0]、J[1]、J[2]、J[3]。

J[0]、J[1]、J[2]、J[3]按如下形式进行计算：

其中，J[0]₁₂、J[0]₁₃、J[0]₂₂、J[0]₃₂按如下形式进行计算：

对于四元数q＝[x y z λ]＝[ω λ]，[·]_L与[·]_R可表示为：

4)联合其他量测传感器所提供的误差项，按如下方式构建目标函数：

其中，||e_p-H_pX||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其他测量和状态则被边缘化并转换为先验；

为惯性残差，B是所有IMU测量的集合，

为惯性量测协方差；

为GPS传感器所提供的误差项，

表示其他导航传感器协方差矩阵。使用高斯牛顿非线性优化方法，当达到误差收敛状态时或迭代次数达到阈值时则停止优化，输出载体的导航信息。

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

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (10)

1.一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，包括如下步骤：

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

陀螺数据

步骤二：基于所述加速度计数据

和陀螺数据

计算高精度惯性预积分值及相邻两优化时刻之间载体导航信息，所述载体导航信息包括载体速度信息、载体位置信息；

步骤三：基于所述加速度计数据

和所述陀螺数据

计算得到高精度惯性预积分误差传递方程；

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

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

2.根据权利要求1所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述步骤二具体为：

1)输出加速度计数据

陀螺数据

其中，

为机体系相对于导航系n在机体系b下的分量，即实际导航解算所需要的载体运动角速率，

为陀螺的输出值，b_ω为陀螺零偏，n_ω为陀螺白噪声，

为导航系到机体系的坐标转换矩阵，

为载体实际运动的加速度在导航系下的分量，

为机体系到导航系的坐标转换矩阵，f^b为加速度计的输出值，g为地球重力加速度，n_a为加速度计白噪声，b_a为加速度计零偏；

为地球自转角速率在导航系下的投影，其中下标i表示惯性坐标系，e表示地球坐标系，

为载体运动带来的导航系与地球坐标系的转动角速率；

为载体运动导致导航系相对于地球系的运动速度在导航系上的投影；

2)相邻两优化时刻t_k与t_k+1之间，一共有l个IMU数据，计算载体姿态信息：

其中，n_k+1、b_k+1分别表示t_k+1时刻的导航系与机体系，n_k、b_k分别表示t_k时刻的导航系与机体系，

分别表示t_k与t_k+1时刻的载体姿态信息，

表示t_k+1时刻高精度姿态预积分；

3)相邻两优化时刻t_k与t_k+1之间，计算载体速度信息：

其中，

表示t_k+1时刻导航系到t_k时刻机体系的坐标转换矩阵，

分别表示t_k+1时刻与t_k时刻载体速度信息，

表示t_k时刻地球自转角速率，

表示t_k时刻由载体运动带来的导航系与地球坐标系的转动角速率，ΔT表示相邻两优化时刻的时间间隔，

表示t_k时刻重力加速度，

表示t_k+1时刻高精度速度预积分；

4)相邻两优化时刻t_k与t_k+1之间，按如下形式计算载体位置信息：

其中，

分别表示t_k+1时刻与t_k时刻载体在地球系下的位置信息，初始时刻载体在地球系下的位置为载体的经度、纬度、高度信息，

表示t_k时刻导航系到地球系的坐标转换矩阵。

3.根据权利要求2所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述

的表达式为：

其中，

表示载体东向运动速率，

表示载体北向运动速率，h为载体所处高度，R_m为子午圈曲率半径，R_n为卯酉圈曲率半径，L为载体所处位置的纬度信息。

4.根据权利要求3所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述

的表达式为：

其中，b_k、b_k+1分别表示初始t_k与t_k+1时刻的机体坐标系，

表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻机体系到t_k时刻机体系的相对姿态量，该信息用四元数表示，Ω(·)按如下方式进行表示：

5.根据权利要求4所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述

的表达式为：

其中，

表示相邻两优化时刻t_k与t_k+1之间某一IMU采样时刻机体系到t_k时刻机体系的坐标转换矩阵，

表示其中某一IMU采样时刻加速度计的输出，

表示该时刻加速度计零偏。

6.根据权利要求5所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述

的表达式为：

表示t_k+1时刻高精度位置预积分：

7.根据权利要求6所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述步骤三具体为：

1)计算误差状态转移矩阵F_i与噪声状态转移矩阵G_i：

其中，δt表示IMU采样时间间隔，I表示单位矩阵，R(γ)表示将四元数转化为旋转矩阵，

2)对于相邻两个IMU采样时刻t_i与t_i+1，系统的误差传递方程A_i+1、雅可比矩阵

与协方差P_i+1按如下形式进行计算：

A_i+1＝F_iA_i+G_iN

其中：

其中，

表示t_i时刻载体的位置预积分误差状态，

表示t_i时刻载体的速度预积分误差状态，

表示t_i时刻载体的姿态预积分误差状态、

分别表示t_i时刻载体的加速度计、陀螺的零偏误差状态；

表示t_i时刻陀螺与加速度计的白噪声，

表示t_i+1时刻陀螺与加速度计的白噪声，

表示加速度计零偏白噪声，

表示陀螺零偏白噪声；

分别为t_i时刻、t_i+1时刻系统状态的雅可比矩阵，初始时刻系统状态的雅可比矩阵为单位矩阵；Q为噪声N的协方差矩阵，P_i、P_i+1分别表示t_i时刻、t_i+1时刻惯性量测协方差矩阵，初始时刻为0。

8.根据权利要求7所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，所述步骤四具体为：

1)当采集到其他导航传感器数据S(k+1)时，记该t_k+1时刻机体系为b_k+1，在t_k时刻到t_k+1时刻间，一共有l个IMU数据，然后计算补偿过误差后的载体的位置预积分值

速度预积分值

与姿态预积分值

基于所述步骤三所给出的相邻两IMU帧的惯性预积分的误差状态从第一帧IMU数据，递推至第l帧IMU数据计算A_l得出：

其中：

计算惯性残差

其中，姿态用四元数表示，

表示提取四元数的虚部，

为记号式；

2)基于步骤三中的相邻两IMU帧的协方差矩阵P_i+1计算惯性残差的增量方程：

其中，

表示惯性残差

相对待优化状态变量X的雅可比矩阵，δX表示优化过程中的误差状态量，

表示t_k时刻到t_k+1时刻间系统的协方差矩阵，由步骤三所给出的相邻两IMU帧的协方差矩阵递推计算得出；

3)惯性残差

相对待优化状态变量X的雅可比矩阵

按如下方式进行计算：由惯性残差

的表达式，其相对待优化状态变量x_s与x_λ的雅可比矩阵为0，即：

将剩余待优化变量

分为四组：

记惯性残差相对待优化变量的雅可比矩阵为J[0]、J[1]、J[2]、J[3]；

J[0]、J[1]、J[2]、J[3]按如下形式进行计算：

其中，J[0]₁₂、J[0]₁₃、J[0]₂₂、J[0]₃₂按如下形式进行计算：

对于四元数q＝[x y z λ]＝[ω λ]，[·]_L与[·]_R表示为：

4)联合其他量测传感器所提供的误差项，按如下方式构建目标函数：

其中，||e_p-H_pX||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其他测量和状态则被边缘化并转换为先验；

为惯性残差，B是所有IMU测量的集合，

为惯性量测协方差；

为其他导航传感器所提供的误差项，

表示其他导航传感器协方差矩阵。

9.根据权利要求8所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，

所述

所述

所述

的表达式分别为：

其中，各雅可比矩阵按如下方式进行计算：

其中，

分别表示t_k时刻到t_k+1时刻间，载体的位置预积分量、速度预积分量与姿态预积分量的误差状态。

10.根据权利要求9所述的一种基于高精度惯性预积分的因子图组合导航方法，其特征在于，

所述δX包括其他导航传感器与惯性传感器的相对平移与旋转的误差量δx_s，其他导航传感器所提供的误差状态量δx_λ，滑动窗口内n+1个量测优化帧的载体位置、速度、四元数、加速度计和陀螺零偏的误差状态量：

δX＝[δx₀,δx₁,…,δx_n,δx_s,δx_λ]

表示惯性残差

相对待优化状态变量X的雅可比矩阵，具体地，待优化状态变量X为：

X＝[x₀,x₁,…,x_n,x_s,x_λ]

其中x_n表示滑动窗口内某个量测优化帧的载体位置、速度、姿态、加速度计和陀螺的零偏；x_s表示其它量测传感器与IMU的相对平移与旋转量；x_λ表示其他导航传感器所提供的误差。

Images