CN112284379B - 一种基于非线性积分补偿的组合运动测量***的惯性预积分方法 - Google Patents

Abstract

本发明提供了一种基于非线性积分补偿的组合运动测量***的惯性预积分方法，在车辆自动驾驶、无人机姿态控制、运动捕捉和增强现实等应用领域中，常用的基于惯性的组合导航定位***或位姿测量***的状态非线性优化估计过程涉及惯性预积分的多个关键步骤。本发明通过在组合运动测量***的惯性预积分中引入多子样非线性积分补偿，构建包括基于非线性积分补偿的姿态、速度和位置预积分计算模型，及其对应的惯性状态更新残差雅克比和惯性状态更新残差协方差的实时计算模型，实现在包括强振动和大机动等高动态运动时的高精度运动估计，并显著提高组合***的计算效能。

Description

一种基于非线性积分补偿的组合运动测量***的惯性预积分 方法

技术领域

本发明涉及智能车自主导航定位、无人机自主导航定位和姿态控制、载体运动捕捉和增 强现实等技术领域，尤其涉及一种基于非线性积分补偿的组合运动测量***的惯性预积分方 法。

背景技术

近年来自动驾驶汽车产业与技术发展迅速，作为自动驾驶核心之一的导航定位***是其 实现自动驾驶的必要组件。另一方面，近年来无人机技术及行业应用急速发展，其在农业植 保、灾难救援、快递运输、测绘、航拍、新闻报道、电力巡检、影视拍摄等领域应用拓展迅速。无人机飞行环境的高动态性、不确定性和复杂性是无人机自主控制面临的主要问题。不 管是自动驾驶所需的导航定位，还是无人机自主飞行所需的姿态控制都离不开无人载体的精 确且可靠的实时位姿测量。另外，增强现实、医疗康复、模拟训练以及动漫制作等应用领域 对运动捕捉技术的需求强烈，且递增显著。

在现有自主导航定位、位姿测量和运动捕捉等运动测量技术领域中，运动测量方案以多 传感信息融合的组合方式为主，其所依赖的多种传感器件中，惯性测量单元因能提供丰富的 高速率且高动态运动信息而几乎不可或缺。常用的惯性基组合运动测量***有：惯性/视觉组 合***、惯性/卫星组合***、惯性/里程计组合***等。

惯性基组合运动测量***中的一个关键组成部分-惯性预积分，通常在一个相对较大的时 间间隔上实施，以形成相对运动约束。然而，以标准积分形式实施的惯性预积分效率低下。 这一问题被Lupton等人提出的基于欧拉角表达的、Forster等人提出的基于SO₃流形表达的和 Eckenhoff等人提出的基于四元数表达的线性化惯性预积分方法所解决。然而，Eckenhoff等 人和Nisar等人的研究均表明，在线性化惯性预积分中存在明显的高阶非线性误差，载体动 态运动强度越剧烈，非线性误差越明显。在包括强振动、大机动等高强度的动态运动环境中， 现行线性化惯性预积分的非线性误差更加明显。

因此，亟需从惯性预积分入手，设计一种能够解决高动态运动环境下惯性基组合***运 动测量精度问题的技术方案。

发明内容

针对现有技术中惯性基组合***运动测量在高动态运动环境下精确性差的问题，本发明 提供一种基于非线性积分补偿的组合运动测量***的惯性预积分方法，旨在通过在组合运动 测量***的惯性预积分模型的设计过程中引入非线性积分补偿，进而解决传统线性化惯性预积分方法在高动态运动环境下精确性差的问题，以提高惯性基组合***的运动测量精度。

本发明是通过以下技术手段实现上述技术目的的。

一种基于非线性积分补偿的组合运动测量***的惯性预积分方法，组合运动测量***的 惯性预积分过程中引入多子样非线性积分补偿，构建预积分模型、惯性状态更新残差雅克比 模型和惯性状态更新残差协方差模型，进行惯性预积分；

所述多子样非线性积分补偿的形式包括旋转矢量、速度矢量和位置矢量；

所述预积分模型包括姿态预积分模型、速度预积分模型和位置预积分模型。

进一步，所述旋转矢量为其中φ_ij是与[t_i t_j]时间段上 姿态变化相等效的旋转矢量，ω_k、ω_m和ω_n分别为t_k、t_m和t_n时刻的理想角速度采样值，A_k、 Β_mn分别为旋转积分线性拟合系数、旋转不可交换性补偿系数，×为矢量叉乘，∑为求和算 符，t_i、t_j、t_k、t_m和t_n分别为第i个、第j个、第k个、第m个和第n个惯性采样时刻，且时刻t_k、t_m和t_n均在时间段[t_i t_j]上。

进一步，所述速度矢量为其中ΔV_ij为[t_i t_j]时间段上 测量加速度引起的速度矢量，a_k、a_n分别为t_k、t_n时刻的理想加速度采样值，ω_m为t_m时刻的 理想加速度采样值，C_k为速度积分线性拟合系数，D_mn为速度不可交换性补偿系数。

进一步，所述位置矢量为其中ΔP_ij为[t_i t_j]时间段上测 量加速度引起的位置矢量，a_k、a_n分别为t_k、t_n时刻的理想加速度采样值，ω_m为时刻的理想 加速度采样值，E_k为位置积分线性拟合系数，F_mn为位置不可交换性补偿系数。

更进一步，所述姿态预积分模型计算姿态积分对应的旋转矢量φ_ij的初值为：其中/>和/>分别为t_k、t_m和t_n时刻的角速度矢量测量值，/>为组合***优化时角速度漂移初值。

更进一步，所述速度预积分模型计算速度积分ΔV_ij的初值 其中/>分别为t_k、t_n时刻的加速度矢量测量值，/>为组合***优化时加速度漂移初值，/>为t_m时刻的角速度矢量测量值，/>为 组合***优化时角速度漂移初值。

更进一步，所述位置预积分模型计算位置积分ΔP_ij的初值 其中/>分别为t_k、t_n时刻的加速度矢量测量值，/>为组合***优化时加速度漂移初值，/>为t_m时刻的角速度矢量测量值，/>为 组合***优化时角速度漂移初值。

进一步，所述惯性状态更新残差雅克比模型包括惯性姿态更新残差雅克比、惯性速度更 新残差雅克比和惯性位置更新残差雅克比；

所述惯性姿态更新残差雅克比的获取为：

其中，e_φ是由表达的姿态残差四元数的矢量部分，/>分别为t_i、 t_j时刻的姿态，/>为与/>等价的姿态变化，/>为由惯性器件漂移引起的从t_i时刻到t_j时刻的 姿态变化相对于初值/>的姿态偏差更新量；/>与/>等价，/> 为矢量/>的模值，

δb_i ^g、δb_j ^g分别为t_i、t_j时刻角速度漂移更新量；和/>分别为e_φ对t_i时刻旋转扰动δφ_i、t_j时刻旋转扰动δφ_j、t_i时刻角速度漂移扰动/>和t_j时刻角速度漂移扰动/>的雅克比；/>q_mb分别为四元数/>的共轭四元数的标量部分、矢量部分，q_n分别为四元数/>的标量部分、矢量部分，/>q_r分别为四元数/>的标量部分、矢量部分；()^为求矢量叉乘反对称矩阵算符，()^T为求矩阵转置算符，I为 3行3列单位矩阵；f_ij和f_c为中间变量，/> A_k、Β_mn分别为旋转积分线性拟合系数、旋转不可交换性补偿系数；

所述的惯性速度更新残差雅克比的获取为：

其中，e_V为由表达的速度残差矢量，V_i、V_j分别为t_i、t_j时 刻的速度，R_i为t_i时刻从载体系b到全球坐标系w的旋转矩阵，ΔV_g为从t_i时刻到t_j时刻重力 加速度引起的速度变化，/>为由惯性器件漂移引起的ΔV_ij相对于初值/>的速度偏差更新量，/>δb_i ^a、δb_j ^a分 别为t_i、t_j时刻加速度漂移更新量；/>分别为e_V对t_i时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比，/>分别为e_V对t_j时刻角速度漂移扰动/>角速 度漂移扰动/>的雅克比；h1_ij、h2_ij、h1_c和h2_c为中间变量，

所述惯性位置更新残差雅克比的获取为：

其中，e_P为由表达的速度残差矢量，P_i、P_j分别为t_i、 t_j时刻的位置，ΔP_v为从t_i时刻到t_j时刻V_i引起的位置变化，ΔP_g为从t_i时刻到t_j时刻重力加速 度引起的位置变化，/>为由惯性器件漂移引起的ΔP_ij相对于初值/>的位置偏差更新量，/> 分别为e_P对t_i时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比，/>分别 为e_P对t_j时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比；s1_ij、s2_ij、s1_c和s2_c为中间变量，/>

更进一步，所述惯性状态更新残差协方差模型包括惯性姿态更新残差协方差、惯性速度 更新残差协方差和惯性位置更新残差协方差；

所述惯性状态更新残差协方差模型的获取为：

其中，cov(δφ_ε)、cov(δV_ε)和cov(δP_ε)分别为姿态更新残差、速度更新残差和位置更新 残差的协方差， 为矢量/>的模值，/>

δb_i ^a、δb_j ^a分别为t_i、t_j时刻加速度漂移更新量，δb_i ^g、δb_j ^g分别为t_i、t_j时刻角速度漂移更新量，/>分别为t_k、t_n时 刻的加速度矢量测量值，/>分别为t_m、t_n时刻的角速度矢量测量值，/>为t_k时刻的角 速度测量噪声，/>为t_k时刻的加速度测量噪声，/>为t_i时刻的加速度漂移，/>为t_i时刻的角 速度漂移。

与现有技术相比，本发明具有以下优点：

(1)本发明中的组合运动测量***惯性预积分模型中采用了多子样非线性积分补偿方式， 包括多子样非线性补偿形式的旋转矢量、速度矢量和位置矢量，而非单子样线性化积分方式， 使得组合运动测量***可以更好的适用于高动态运动环境。

(2)本发明方法实现容易、并且算法效率高；传统的组合运动测量***惯性预积分过程 中需要频繁地对运动参数实施耗时的旋转变换，而本发明采用的多子样非线性积分补偿方式 只需要进行很少的旋转变换，从而提高了运算效率。

(3)本发明所提出的方法具有很强的工程应用灵活性和适用性，可以灵活的扩展到惯性 与其它传感器的组合运动测量***中，如惯性/视觉、惯性/卫星和惯性/里程计等组合运动测 量***，广泛用于车辆自动驾驶、无人机姿态控制、运动捕捉和增强现实等领域。

附图说明

图1为本发明所述基于非线性积分补偿的组合运动测量***的惯性预积分方法流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将结合附图及详细叙述 以清晰说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施 例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。

图1是本实施例的流程图，一种基于非线性积分补偿的组合运动测量***的惯性预积分 方法，具体包括如下步骤：

步骤(1)，定义惯性测量模型

定义惯性基组合运动测量***中的惯性测量角速度和加速度模型，如下：

其中，分别为t时刻惯性测量角速度矢量、加速度矢量在载体系b上的投影，^bω(t)、^ba(t)分别为t时刻理想可测角速度矢量、理想可测加速度矢量在载体系b上的投影， b^g(t)、b^a(t)分别为t时刻惯性测量角速度漂移、加速度漂移在载体系b上的表达，分别为b^g(t)、b^a(t)对时间的导数，^wa(t)为t时刻加速度矢量在全球坐标系w上的表达，^wg为 重力加速度在全球坐标系w上的表达，/>为t时刻从全球坐标系w到载体系b的旋转矩阵， ε^g(t)、ε^a(t)、ε^bg(t)和ε^ba(t)均为零均值高斯白噪声。

步骤(2)，定义刚体运动微分方程

刚体运动微分方程为：

其中， ^wa、^wV和^wP分别为t时刻载体系b相对于全球坐标系w的姿态(四元数)、 载体系b相对于全球坐标系w的加速度在w系上的投影、载体系b相对于全球坐标系w的速 度在w系上的投影和载体系b相对于全球坐标系w的位移在w系上的投影，/>和/>分 别为 ^wV和^wP对时间的导数，/>为t时刻载体系b相对于全球坐标系w的旋转角速度，/>为四元数乘法；式(6)、式(7)和式(8)分别称为刚体运动的旋转微分方程、速度微分方 程和位置微分方程。

步骤(3)，计算旋转积分

基于式(6)，对旋转四元数微分在[t_i t_j]上积分可得：

其中，为t_j时刻的姿态(四元数)，/>为t_i时刻的姿态(四元数)，/>为/>的逆，为从t_i时刻到t_j时刻的姿态变化(四元数)；

将写成如下等效旋转矢量表达形式：

其中，φ_ij是与在表达姿态变化上等效的旋转矢量，φ_ij为φ_ij的模值；在φ_ij为小量时， 式(11)中的近似表达成立；

将旋转矢量φ_ij表达为如下多子样非线性补偿形式：

其中，ω_k、ω_m和ω_n分别为t_k、t_m和t_n时刻的理想角速度采样，A_k、Β_mn分别为旋转积分线性拟合系数、旋转不可交换性补偿(又称圆锥补偿)系数，×为矢量叉乘，∑为求和算符，t_i、t_j、t_k、t_m和t_n分别为第i个、第j个、第k个、第m个和第n个惯性采样时刻，且t_k、 t_m和t_n均在[t_i t_j]上；

根据式(1)描述，定义t_k时刻的角速度测量为：

其中，为t_k时刻的角速度矢量测量值，/>为t_k时刻的角速度漂移，/>为组合***优化 时/>的初值，/>为每次优化时/>相对于初值的偏差更新量，/>为t_k时刻的角速度测量噪声；

由式(12)、式(13)、式(14)和式(15)可导出：

其中，和/>分别为t_k、t_m和t_n时刻的角速度矢量测量值，/>为组合***优化时 角速度漂移初值；I_3×3为3×3单位矩阵(简记为I)，/>为组合***优化时φ_ij的初值，为 由惯性器件(陀螺)漂移引起的φ_ij相对于初值的偏差更新量(等效旋转矢量)，δφ_ij为由惯性 测量噪声引起的φ_ij的随机偏差(等效旋转矢量)，()∧为矢量叉乘反对称矩阵算符；

定义：为组合***优化时/>或q(φ_ij)的初值，记为/>q(δφ_b)为由惯性器件(陀螺) 漂移引起的/>相对于初值的偏差更新量(四元数)，记为/>q(δφ_ε)为由惯性测量噪声引起 的/>的随机偏差(四元数)，记为q_ε；则有：

当δφ_b和δφ_ε为小量时，有如下近似形式：

其中，为/>的模值。

步骤(4)，计算速度积分

基于式(2)、式(3)和式(7)，对速度微分在[t_i t_j]上积分可得：

V_j＝V_i+ΔV_g+R_iΔV_ij (24)

其中，V_j为t_j时刻的速度，V_i为t_i时刻的速度，R_i为t_i时刻从载体系b到全球坐标系w的 旋转矩阵，ΔV_g为从t_i时刻到t_j时刻重力加速度g引起的速度变化，ΔV_ij为从t_i时刻到t_j时刻 测量加速度引起的速度变化；

将速度矢量ΔV_ij表达为如下多子样非线性补偿形式：

其中，a_k、a_n分别为t_k、t_n时刻的理想加速度采样，C_k、D_mn分别为速度积分线性拟合系数、速度不可交换性补偿(又称旋转效应补偿与划桨效应补偿)系数，t_k、t_n均在[t_i t_j]上；

根据式(2)和式(3)描述，定义t_k时刻的加速度测量：

其中，为t_k时刻的加速度矢量测量值，/>为t_k时刻的加速度漂移，/>为组合***优化 时/>的初值，/>为每次优化时/>相对于初值的偏差更新量，/>为t_k时刻的加速度测量噪声；

由式(13)～(15)和式(26)～(29)可导出：

其中，为组合***优化时ΔV_ij的初值，/>为由惯性器件(加速度计)漂移引起的ΔV_ij相对于初值的偏差更新量，δV_ε为由惯性测量噪声引起的ΔV_ij的随机偏差；/>分别为t_k、 t_n时刻的加速度矢量测量值，/>为组合***优化时加速度漂移初值。

步骤(5)，计算位置积分

基于式(2)、式(3)和式(8)，对位置微分在[t_i t_j]上积分可得：

P_j＝P_i+ΔP_v+ΔP_g+R_iΔP_ij (34)

ΔP_v＝V(t_i)(t_j-t_i) (35)

其中，P_j为t_j时刻的位置，P_i为t_i时刻的位置，R_i为t_i时刻从载体系b到全球坐标系w的 旋转矩阵，ΔP_v为从t_i时刻到t_j时刻V_i引起的位置变化，ΔP_g为从t_i时刻到t_j时刻重力加速度 引起的位置变化，ΔP_ij为从t_i时刻到t_j时刻测量加速度引起的位置变化；

将位置矢量ΔP_ij表达为如下多子样非线性补偿形式：

其中，E_k、F_mn分别为位置积分线性拟合系数、位置不可交换性补偿系数；

由式(13)～(15)、式(27)～(29)和式(37)可导出：

其中，为组合***优化时ΔP_ij的初值，/>为由惯性器件(加速度计)漂移引起的ΔP_ij相对于初值的偏差更新量，δP_ε为由惯性测量噪声引起的ΔP_ij的随机偏差。/>

步骤(6)，构建惯性预积分状态更新残差模型

定义姿态(四元数)、速度、位置和惯性测量漂移更新模型如下：

其中，q_r、δV_r、δP_r、和/>分别是从t_i时刻到t_j时刻的姿态(四元数)更新残差、速度更新残差、位置更新残差、角速度测量漂移更新残差和加速度测量漂移更新残差， 分别是从t_i时刻到t_j时刻的角速度测量随机偏差、加速度测量随机偏差；

当q_r和q_ε表征微小旋转时，有：

其中，vec()是取四元数矢量部分算符；

针对基于非线性优化方法的组合***，定义惯性状态更新残差向量e，即由姿态、速 度、位置、角速度测量漂移和加速度测量漂移的更新残差组成的向量，分别写为：

选取t_i时刻和t_j时刻的姿态(四元数)、速度、位置、角速度测量漂移和加速度测量漂移 为组合***非线性优化的状态变量即由/>V_i、V_j、P_i、P_j、δb_i ^g、δb_j ^g、δb_i ^a和 δb_j ^a组成的矢量；对应的待优化状态变量的更新量/>为由δφ_i、δφ_j、δV_i、δV_j、δP_i、δP_j、组成的矢量；各状态量更新关系表示为：/> V_i←V_i+δV_i、V_j←V_j+δV_j、P_i←P_i+δP_i、P_j←P_j+δP_j、/>和/>

根据非线性优化方法，状态变量的更新量有下式计算得到：

其中，J为状态更新残差雅克比矩阵，Λ为状态更新残差协方差矩阵。

步骤(7)，构建状态更新残差雅克比矩阵

根据式(48)～(52)和式(17)～(18)、(21)、(23)、(31)～(32)、(35)、(39)～(40)，可导出各状态更新残差分量的雅克比如下：

/>

其中，δb_i ^g、δb_j ^g分别为t_i、t_j时刻角速度漂移更新量，和/>分 别为e_φ对t_i时刻旋转扰动δφ_i、t_j时刻旋转扰动δφ_j、t_i时刻角速度漂移扰动/>和t_j时刻角速 度漂移扰动/>的雅克比；/>q_mb分别是四元数/>的共轭四元数的标量部分、矢量部分，/>q_n分别是四元数/>的标量部分、矢量部分，/>q_r分别是四元数/>的标量部分、矢量部分，()^T是求矩阵的转置算符；f_ij和f_c为中间变量； e_V是由表达的速度残差矢量，δb_i ^a、δb_j ^a分别为t_i、t_j时刻加速 度漂移更新量，/>分别为e_V对t_i时刻角速度漂移扰动/>角速度漂移扰动/>的 雅克比，/>分别为e_V对t_j时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比，h1_ij、h2_ij、h1_c和h2_c为中间变量；/>分别为e_P对t_i时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比，/>分别为e_P对t_j时刻角速度漂移扰动/>角速 度漂移扰动/>的雅克比；s1_ij、s2_ij、s1_c和s2_c为中间变量；

/>

未写出的状态更新残差分量的雅克比均为零矩阵。

步骤(8)，构建状态更新残差协方差矩阵

根据式(22)、式(33)、式(41)和式(4)～(5)、(19)、(23)，可导出各状态更新残 差协方差如下：

/>

其中，cov(δφ_ε)、cov(δV_ε)、cov(δP_ε)、和/>分别为姿态更新残差、速 度更新残差、位置更新残差、角速度漂移更新残差和加速度漂移更新残差的协方差，/>为矢 量/>的模值，/>为t_i时刻的加速度漂移，/>为t_i时刻的角速度漂移。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本 发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均 属于本发明的保护范围。

Claims (6)

1.一种基于非线性积分补偿的组合运动测量***的惯性预积分方法，其特征在于，组合运动测量***的惯性预积分过程中引入多子样非线性积分补偿，构建预积分模型、惯性状态更新残差雅克比模型和惯性状态更新残差协方差模型，进行惯性预积分；

所述多子样非线性积分补偿的形式包括旋转矢量、速度矢量和位置矢量；

所述预积分模型包括姿态预积分模型、速度预积分模型和位置预积分模型；

所述旋转矢量为其中φ_ij是与[t_i t_j]时间段上姿态变化相等效的旋转矢量，ω_k、ω_m和ω_n分别为t_k、t_m和t_n时刻的理想角速度采样值，A_k、B_mn分别为旋转积分线性拟合系数、旋转不可交换性补偿系数，×为矢量叉乘，∑为求和算符，t_i、t_j、t_k、t_m和t_n分别为第i个、第j个、第k个、第m个和第n个惯性采样时刻，且时刻t_k、t_m和t_n均在时间段[t_i t_j]上；

所述速度矢量为其中ΔV_ij为[t_i t_j]时间段上测量加速度引起的速度矢量，a_k、a_n分别为t_k、t_n时刻的理想加速度采样值，ω_m为t_m时刻的理想加速度采样值，C_k为速度积分线性拟合系数，D_mn为速度不可交换性补偿系数；

所述位置矢量为其中ΔP_ij为[t_i t_j]时间段上测量加速度引起的位置矢量，a_k、a_n分别为t_k、t_n时刻的理想加速度采样值，ω_m为时刻的理想加速度采样值，E_k为位置积分线性拟合系数，F_mn为位置不可交换性补偿系数。

2.根据权利要求1所述的基于非线性积分补偿的组合运动测量***的惯性预积分方法，其特征在于，所述姿态预积分模型计算姿态积分对应的旋转矢量φ_ij的初值为：其中/>和/>分别为t_k、t_m和t_n时刻的角速度矢量测量值，/>为组合***优化时角速度漂移初值。

3.根据权利要求1所述的基于非线性积分补偿的组合运动测量***的惯性预积分方法，其特征在于，所述速度预积分模型计算速度积分ΔV_ij的初值 其中/>分别为t_k、t_n时刻的加速度矢量测量值，/>为组合***优化时加速度漂移初值，/>为t_m时刻的角速度矢量测量值，/>为组合***优化时角速度漂移初值。

4.根据权利要求1所述的基于非线性积分补偿的组合运动测量***的惯性预积分方法，其特征在于，所述位置预积分模型计算位置积分ΔP_ij的初值 其中/>分别为t_k、t_n时刻的加速度矢量测量值，/>为组合***优化时加速度漂移初值，/>为t_m时刻的角速度矢量测量值，/>为组合***优化时角速度漂移初值。

5.根据权利要求1-4任一项所述的基于非线性积分补偿的组合运动测量***的惯性预积分方法，其特征在于，所述惯性状态更新残差雅克比模型包括惯性姿态更新残差雅克比、惯性速度更新残差雅克比和惯性位置更新残差雅克比；

所述惯性姿态更新残差雅克比的获取为：

其中，e_φ是由表达的姿态残差四元数的矢量部分，/>分别为t_i、t_j时刻的姿态，/>为与/>等价的姿态变化，/>为由惯性器件漂移引起的从t_i时刻到t_j时刻的姿态变化相对于初值/>的姿态偏差更新量；/>与/>等价，/> 为矢量/>的模值，/>δb_i ^g、δb_j ^g分别为t_i、t_j时刻角速度漂移更新量；/>和/>分别为e_φ对t_i时刻旋转扰动δφ_i、t_j时刻旋转扰动δφ_j、t_i时刻角速度漂移扰动/>和t_j时刻角速度漂移扰动/>的雅克比；/>q_mb分别为四元数/>的共轭四元数的标量部分、矢量部分，/>q_n分别为四元数/>的标量部分、矢量部分，/>q_r分别为四元数/>的标量部分、矢量部分；()^为求矢量叉乘反对称矩阵算符，()^T为求矩阵转置算符，I为3行3列单位矩阵；f_ij和f_c为中间变量，/> A_k、B_mn分别为旋转积分线性拟合系数、旋转不可交换性补偿系数；

所述的惯性速度更新残差雅克比的获取为：

其中，e_V为由表达的速度残差矢量，V_i、V_j分别为t_i、t_j时刻的速度，R_i为t_i时刻从载体系b到全球坐标系w的旋转矩阵，ΔV_g为从t_i时刻到t_j时刻重力加速度引起的速度变化，/>为由惯性器件漂移引起的ΔV_ij相对于初值/>的速度偏差更新量，/>δb_i ^a、δb_j ^a分别为t_i、t_j时刻加速度漂移更新量；/>分别为e_V对t_i时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比，/>分别为e_V对t_j时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比；h1_ij、h2_ij、h1_c和h2_c为中间变量，/>

所述惯性位置更新残差雅克比的获取为：

其中，e_P为由表达的速度残差矢量，P_i、P_j分别为t_i、t_j时刻的位置，ΔP_v为从t_i时刻到t_j时刻V_i引起的位置变化，ΔP_g为从t_i时刻到t_j时刻重力加速度引起的位置变化，/>为由惯性器件漂移引起的ΔP_ij相对于初值/>的位置偏差更新量，/>分别为e_P对t_i时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比，/>分别为e_P对t_j时刻角速度漂移扰动/>角速度漂移扰动/>的雅克比；s1_ij、s2_ij、s1_c和s2_c为中间变量，/>

6.根据权利要求1-4任一项所述的基于非线性积分补偿的组合运动测量***的惯性预积分方法，其特征在于，所述惯性状态更新残差协方差模型包括惯性姿态更新残差协方差、惯性速度更新残差协方差和惯性位置更新残差协方差；

所述惯性状态更新残差协方差模型的获取为：

其中，cov(δφ_ε)、cov(δV_ε)和cov(δP_ε)分别为姿态更新残差、速度更新残差和位置更新残差的协方差，分别为角速度测量噪声协方差、加速度测量噪声协方差，为矢量/>的模值，/> k＝i+1,i+2,...,j-1，δb_i ^a、δb_j ^a分别为t_i、t_j时刻加速度漂移更新量，δb_i ^g、δb_j ^g分别为t_i、t_j时刻角速度漂移更新量，/>分别为t_k、t_n时刻的加速度矢量测量值，/>分别为t_m、t_n时刻的角速度矢量测量值，/>为t_i时刻的加速度漂移，/>为t_i时刻的角速度漂移。