WO2022088700A1

WO2022088700A1 - 一种基于双速数值积分结构的惯性预积分的优化方法

Info

Publication number: WO2022088700A1
Application number: PCT/CN2021/101261
Authority: WO
Inventors: 汤传业; 蔡英凤; 陈建锋; 陈龙; 汤新华; 张炳力; 葛红飞
Original assignee: 江苏大学
Priority date: 2020-10-27
Filing date: 2021-06-21
Publication date: 2022-05-05
Also published as: CN112464432B; CN112464432A

Abstract

本发明提供了一种基于双速数值积分结构的惯性预积分的优化方法，在车辆自动驾驶、无人机姿态控制、运动捕捉和增强现实等应用领域中，常用的惯性基组合运动测量系统中涉及到的惯性预积分是系统中非常关键的环节。本发明通过在组合运动测量系统惯性预积分中引入双速数值积分结构，包括双速旋转积分结构、双速速度积分结构和双速位置积分结构，通过实施双速数值积分结构的参数优化，优化设计出线性积分项和非线性积分项中的参数。使用本发明公开的方法能够显著提高惯性基组合运动测量系统的运动估计精度、计算实时性和数值稳定性。

Description

一种基于双速数值积分结构的惯性预积分的优化方法

技术领域

本发明涉及智能车自主导航定位、无人机自主导航定位和姿态控制、载体运动捕捉和增强现实等技术领域，尤其涉及一种基于双速数值积分结构的惯性预积分的优化方法。

背景技术

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

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

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

发明内容

针对现有技术中存在不足，本发明提供了一种基于双速数值积分结构的惯性预积分的优化方法，通过优化设计数值积分结构中的参数，进而提高惯性基组合运动测量系统的运动估计精度、计算实时性和数值稳定性。

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

一种基于双速数值积分结构的惯性预积分的优化方法，在组合运动测量系统惯性预积分设计过程中引入双速数值积分结构，并实施双速数值积分结构的参数优化；

所述双速数值积分结构包括双速旋转积分结构、双速速度积分结构和双速位置积分结构；

所述双速数值积分结构的参数优化包括线性积分参数优化和非线性积分参数优化。

进一步，所述双速旋转积分结构为φ _ij＝φ1 _ij+φ2 _ij，

其中，φ1 _ij为旋转矢量的线性积分项，φ2 _ij为双边形式的旋转矢量非线性积分补偿项，ω _k为理想惯性测量角速度原始采样，ω _k的采样间隔为δt，ω' _m和ω' _n为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样，ω' _m和ω' _n的采样间隔为δT _φ、个数为

A _k为旋转矢量线性积分拟合系数，Αl _m和Αr _n为旋转矢量非线性积分补偿系数，∑为求和运算符，×为矢量叉乘。

进一步，所述双速速度积分结构为ΔV _ij＝ΔV1 _ij+ΔV2 _ij，

其中，ΔV1 _ij为速度矢量的线性积分项，ΔV2 _ij为双边形式的速度矢量非线性积分补偿项，a _k为理想惯性测量加速度原始采样，a _k的采样间隔为δt，ω _1,m为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样，ω _1,m的采样间隔为

个数为M ₁，a _1,n为从原始采样{a _k|k＝i,i+1,...,j}中抽取的加速度采样，a _1,n的采样间隔为

个数为N ₁，B _k为速度矢量线性积分拟合系数，Bl _m和Br _n为速度矢量非线性积分补偿系数。

进一步，所述双速位置积分结构为ΔP _ij＝ΔP1 _ij+ΔP2 _ij，

其中，ΔP1 _ij为位置矢量的线性积分项，ΔP2 _ij为双边形式的位置矢量非线性积分补偿项，a _k为理想惯性测量加速度原始采样，a _k的采样间隔为δt，ω _2,m为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样，ω _2,m的采样间隔为

个数为M ₂，a _2,n为从原始采样{a _k|k＝i,i+1,...,j}中抽取的加速度采样，a _2,n的采样间隔为

个数为N ₂，C _k为位置矢量线性积分拟合系数，Cl _m和Cr _n为位置矢量非线性积分补偿系数。

进一步，所述线性积分结构参数优化步骤为：

(1)定义一种多段多项式表达形式的惯性矢量：

其中，ω(t)为t时刻的理想测量惯性矢量，M为每段多项式的项数，N为分段多项式函数的段数，g _j,i(i＝1,2,...,M，j＝1,2,...,N)为第j段多项式的系数矢量，t ₀为初始时刻，多项式阶数为M-1；

定义在时间段[t ₀,t ₀+(M+N-2)δt]上对ω(t)进行等时间间隔δt采样：

其中，ω _j(j＝1,2,...,M+N-1)为对ω(t)在t ₀+(j-1)δt时刻的采样；

ω _j与g _j,i之间关系式的矩阵表达为：

W＝T _ωG

其中，W＝[W ₁ ^T W ₂…W _N] ^T，

[] ^T为取矩阵的转置运算符，∏为求乘积运算符，Ο为空矩阵；W ₁＝[(ω _j) _j,1] _M×1(j＝1,2,...,M)，

为以列矩阵G _j的最后一个元素构成的1×1单元素矩阵；T _gω＝[((j-1) ^i-1) _j,i] _M×M，T _M为取矩阵T _gω的第M行构成的行矩阵，

为取矩阵T _gω的前M-1行和前M-1列构成的(M-1)×(M-1)矩阵，

为取矩阵T _gω的非第1列构成的(M-1)×M矩阵，T' _M为取矩阵T _gω的第M列的前M-1行的元素构成的(M-1)×1矩阵，[] ^-1为取矩阵的逆运算符；

(2)从t ₀时刻到t ₀+(M+N-2)δt时刻对惯性矢量ω(t)进行一次积分，获得积分矢量Δα：

Δα与g _j,i之间关系式的矩阵表达为：

Δα＝δt·O _nesT _αG

其中O _nes＝[(1) _1,i] _1×N，

O _nes为以1为元素的1×N行矩阵，

(3)从t ₀时刻到t ₀+(M+N-2)δt时刻对惯性矢量ω(t)进行二次积分，获得积分矢量ΔΑ：

ΔΑ与g _j,i之间关系式的矩阵表达为：

ΔΑ＝δt ²·O _nesT _ΑG

其中

(4)Δα、ΔΑ与W之间的关系式为：

Δα＝δt·T _rαW，ΔΑ＝δt ²·T _rΑW

其中，T _rα＝O _nesT _αT _ω ^-1，T _rΑ＝O _nesT _ΑT _ω ^-1；

(5)通过A _φ＝T _rα＝O _nesT _αT _ω ^-1获取旋转线性积分拟合系数A _k的值，通过B _V＝T _rα＝O _nesT _αT _ω ^-1获取线性积分拟合系数B _k的值，通过C _P＝T _rΑ＝O _nesT _ΑT _ω ^-1获取位置线性积分拟合系数C _k的值，其中，A _φ＝[A _i A _i+1…A _j]，B _V＝[B _i B _i+1…B _j]，C _P＝[C _i C _i+1…C _j]。

进一步，所述非线性积分结构参数优化步骤为：

(1)令传统的旋转矢量非线性积分补偿项表达式、传统的速度矢量非线性积分补偿项表达式和传统的位置矢量非线性积分补偿项表达式分别与引入双边形式的旋转矢量非线性积分补偿项表达式、引入双边形式的速度矢量非线性积分补偿项表达式和引入双边形式的位置矢量非线性积分补偿项表达式等价，即：

(2)约束关系为：

Βc ₁₁Βc _ij-Βc _1jΒc _i1＝0(2≤i≤M ₁，2≤j≤N ₁)

Cc ₁₁Cc _ij-Cc _1jCc _i1＝0(2≤i≤M ₂，2≤j≤N ₂)

(3)在约束关系的限定下，设计旋转矢量非线性积分补偿系数Αl _m和Αr _n、速度矢量非线性积分补偿系数Βl _m和Br _n以及位置矢量非线性积分补偿系数Cl _m和Cr _n。

更进一步，传统的旋转矢量非线性积分补偿项表达式为

所述传统的速度矢量非线性积分补偿项表达式为

所述传统的位置矢量非线性积分补偿项表达式为

其中Αc _mn为传统的旋转矢量非线性积分补偿系数，Βc _mn为传统的速度矢量非线性积分补偿系数，Cc _mn为传统的位置矢量非线性积分补偿系数。

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

(1)本发明首次在惯性基组合运动测量系统惯性预积分中引入双速数值积分结构，采用不同频率的惯性测量采样以并行实施线性积分和非线性积分，计算实时性更高。

(2)本发明首次使用多段多项式的假设进行线性积分优化设计，并非单段多项式假设，使得设计出的数值积分适应性更强、积分精度更高、数值稳定性更好。

(3)本发明首次在惯性基组合运动测量系统惯性预积分中使用双边补偿形式的非线性积分结构，并非传统通常的非线性积分结构，使得设计出的数值积分计算实时性更高。

附图说明

图1为本发明所述基于双速数值积分结构的惯性预积分的优化方法流程图。

具体实施方式

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

图1是本实施例的流程图，一种基于双速数值积分结构的惯性预积分的优化方法，具体包括如下步骤：

步骤(1)，定义组合运动测量系统惯性预积分

定义刚体运动微分方程形式，如下：

其中，

^wa、 ^wV和 ^wP分别为t时刻b系相对于w系的姿态(四元数)、b系相对于w系的加速度在w系上的投影、b系相对于w系的速度在w系上的投影和b系相对于w系的位移在w系上的投影，

和

分别为

^wV和 ^wP对时间的导数，

为t时刻b系相对于w系的旋转角速度，

为四元数乘法。

通过对式(1)-(3)两边变量在[t _i t _j]上进行积分，可得：

V _j＝V _i+ΔV _g+R _iΔV _ij (5)

P _j＝P _i+ΔP _v+ΔP _g+R _iΔP _ij (6)

其中，

V _j和P _j分别为t _j时刻的姿态(四元数)、速度和位置，

V _i和P _i分别为t _i时刻的姿态(四元数)、速度和位置，

ΔV _ij和ΔP _ij分别为从t _i时刻到t _j时刻的姿态变化(四元数)、测量加速度引起的速度变化和测量加速度引起的位置变化，ΔV _g和ΔP _g分别为从t _i时刻到t _j时刻重力加速度引起的速度变化和位置变化，R _i为t _i时刻从b系到w系的旋转矩阵，ΔP _v为从t _i时刻到t _j时刻V _i引起的位置变化。

步骤(2)，引入双速数值积分结构

设与

等价的旋转矢量为φ _ij，将旋转矢量φ _ij用如下双速旋转数值积分形式来计算，即：

φ _ij＝φ1 _ij+φ2 _ij (7)

其中，φ1 _ij和φ2 _ij分别为旋转矢量的线性积分项和双边形式的旋转矢量非线性积分补偿项，ω _k(k＝i,i+1,...,j)为理想惯性测量角速度原始采样(ω _k的采样间隔为δt)，ω' _m和ω' _n为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样(ω' _m和ω' _n的采样间隔为δT _φ，ω' _m和ω' _n的个数为

)，A _k为旋转矢量线性积分拟合系数，Αl _m和Αr _n为旋转矢量非线性积分补偿(又称圆锥效应补偿)系数，∑为求和运算符，×为矢量叉乘。

另外，将ΔV _ij和ΔP _ij分别用如下双速速度数值积分形式和双速位置数值积分形式来计算，即：

ΔV _ij＝ΔV1 _ij+ΔV2 _ij (10)

ΔP _ij＝ΔP1 _ij+ΔP2 _ij (13)

其中，ΔV1 _ij和ΔV2 _ij分别为速度矢量的线性积分项和双边形式的速度矢量非线性积分补偿项，ΔP1 _ij和ΔP2 _ij分别为位置矢量的线性积分项和非线性积分补偿项，a _k(k＝i,i+1,...,j)为理想惯性测量加速度原始采样(a _k的采样间隔为δt)，ω _1,m为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样(ω _1,m的采样间隔为

ω _1,m的个数为M ₁)，a _1,n为从原始采样{a _k|k＝i,i+1,...,j}中抽取的加速度采样(a _1,n的采样间隔为

a _1,n的个数为N ₁)，ω _2,m为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样(ω _2,m的采样间隔为

ω _2,m的个数为M ₂)，a _2,n为从原始采样{a _k|k＝i,i+1,...,j}中抽取的加速度采样(a _2,n的采样间隔为

a _2,n的个数为N ₂)，B _k为速度矢量线性积分拟合系数，C _k为位置矢量线性积分拟合系数，Bl _m和Br _n为速度矢量非线性积分补偿(又称旋转效应和划桨效应补偿)系数，Cl _m和Cr _n为位置矢量非线性积分补偿(又称旋转效应和涡卷效应补偿)系数。

步骤(3)，优化设计线性积分结构参数

下面给出一种基于线性积分结构的参数优化设计过程，该设计过程及设计方法具有一般性。

首先，给出一种多段多项式表达形式的惯性矢量定义：

或写成：

其中，ω(t)为t时刻的理想测量惯性矢量，M为每段多项式的项数(多项式阶数为M-1)，N为分段多项式函数的段数，g _j,i(i＝1,2,...,M)为第j(j＝1,2,...,N)段多项式的系数矢量，t ₀为初始时刻。

设在时间段[t ₀,t ₀+(M+N-2)δt]上对ω(t)进行等时间间隔(记为δt)采样，即：

或写成：

其中，ω _j(j＝1,2,...,M+N-1)为对ω(t)在t ₀+(j-1)δt时刻的采样。

进一步，定义矩阵或向量：

W ₁＝[(ω _j) _j,1] _M×1,j＝1,2,...,M (20)

W _j-(M-1)＝[(ω _j) _1,1] _1×1,j＝M+1,M+2,...,M+N-1 (21)

T _gω＝[((j-1) ^i-1) _j,i] _M×M (22)

T _M＝T _gω(M,:) (23)

T' _M＝T _gω(1:M-1,M) (26)

G _j＝[(g _j,iδt ^i-1) _i,1] _M×1,j＝1,2,...,N (27)

其中，W ₁为以ω _j(j＝1,2,...,M)为元素的M×1列矩阵，W _j-(M-1)(j＝M+1,M+2,...,M+N-1)为以ω _j为元素的1×1单元素矩阵，T _gω为以(j-1) ^i-1为元素的M×M矩阵，T _M为取矩阵T _gω的第M行构成的行矩阵，

为取矩阵T _gω的前M-1行和前M-1列构成的(M-1)×(M-1)矩阵，

为取矩阵T _gω的非第1列构成的(M-1)×M矩阵，T' _M为取矩阵T _gω的第M列的前M-1行的元素构成的(M-1)×1矩阵，G _j为以g _j,iδt ^i-1为元素的M×1列矩阵，

为取列矩阵G _j的前M-1个元素构成的(M-1)×1矩阵，

为以列矩阵G _j的最后一个元素构成的1×1单元素矩阵。

根据式(19)～(29)，有：

W ₁＝T _gωG ₁ (30)

进一步，补充关于g _j,i的约束关系：

则有：

其中，[] ^-1为取矩阵的逆运算符。

进一步定义矩阵：

根据式(31)、式(33)、式(36)和式(37)，可推导出：

进一步整合式(30)和式(38)，可获得矩阵方程：

W＝T _ωG (39)

W＝[W ₁ ^T W ₂…W _N] ^T (40)

其中，[] ^T为取矩阵的转置运算符，∏为求乘积运算符，Ο为空矩阵。

其次，求解由式(16)或式(17)定义的惯性矢量的积分，如下：

或写成：

其中，α(t)为从t ₀时刻到t(t ₀≤t≤t ₀+(M+N-2)δt)时刻对惯性矢量ω(t)的积分。

根据式(43)，可得从t ₀时刻到t ₀+(M+N-2)δt时刻对惯性矢量ω(t)的积分(记为Δα)：

进一步，定义矩阵：

其中，T _gα为以

为元素的1×M行矩阵，T _g' _α为以

为元素的1×M行矩阵。

则式(45)可写成如下形式：

进一步，定义矩阵：

O _nes＝[(1) _1,i] _1×N (49)

其中，O _nes为以1为元素的1×N行矩阵。

则式(48)可进一步写成如下形式：

Δα＝δt·O _nesT _αG (51)

再次，直接求解由式(43)或式(44)描述的矢量α(t)在[t ₀,t ₀+(M+N-2)δt]上的积分(记为ΔΑ)，如下：

并定义矩阵：

其中，T _Α1为以

为元素的1×M行矩阵，T _Α2为以

为元素的1×M行矩阵。

则式(52)可写成如下形式：

进一步，定义矩阵：

则式(57)可进一步写成如下形式：

ΔΑ＝δt ²·O _nesT _ΑG (59)

最后，将式(39)带入式(51)和式(59)，可得：

Δα＝δt·O _nesT _αT _ω ^-1W (60)

ΔΑ＝δt ²·O _nesT _ΑT _ω ^-1W (61)

式(60)和式(61)可进一步写成：

Δα＝δt·T _rαW (62)

ΔΑ＝δt ²·T _rΑW (63)

其中，

T _rα＝O _nesT _αT _ω ^-1 (64)

T _rΑ＝O _nesT _ΑT _ω ^-1 (65)

令t _i＝t ₀，t _j＝t ₀+(M+N-2)δt，且：

A _φ＝[A _i A _i+1…A _j] (66)

B _V＝[B _i B _i+1…B _j] (67)

C _P＝[C _i C _i+1…C _j] (68)

若取式(16)或式(17)中定义的惯性矢量为惯性角速度，则设计出式(8)中旋转线性积分拟合系数A _k，A _k的值用下述方程来计算：

A _φ＝T _rα＝O _nesT _αT _ω ^-1 (69)

若取式(16)或式(17)中定义的惯性矢量为惯性加速度，则设计出式(11)中速度线性积分拟合系数B _k和式(14)中位置线性积分拟合系数C _k，B _k和C _k的值分别用下述方程来计算：

B _V＝T _rα＝O _nesT _αT _ω ^-1 (70)

C _P＝T _rΑ＝O _nesT _ΑT _ω ^-1 (71)

步骤(4)，优化设计非线性积分结构参数

传统的旋转矢量非线性积分补偿项通常有如下形式：

其中，Αc _mn为传统的旋转矢量非线性积分补偿(又称圆锥效应补偿)系数。

为在传统的旋转矢量非线性积分补偿系数设计方法的基础上设计式(9)定义的双边形式的旋转矢量非线性积分补偿项中的系数Αl _m和Αr _n，令：

根据式(73)，可得如下关系式：

在式(74)确定的约束关系的限定下，采用传统方法设计出旋转矢量非线性积分补偿系数Αc _mn，再根据式(73)确定的关系计算出Αl _m和Αr _n。

进一步，传统的速度矢量非线性积分补偿项通常有如下形式：

其中，Βc _mn为传统的速度矢量非线性积分补偿(又称旋转效应和划桨效应补偿)系数。

为在传统的速度矢量非线性积分补偿系数设计方法的基础上设计式(12)定义的双边形式的速度矢量非线性积分补偿项中的系数Βl _m和Br _n，令：

根据式(76)，可得如下关系式：

Βc ₁₁Βc _ij-Βc _1jΒc _i1＝0,2≤i≤M ₁,2≤j≤N ₁ (77)

在式(77)确定的约束关系的限定下，采用传统方法设计出速度矢量非线性积分补偿系数Βc _mn，再根据式(76)确定的关系计算出Βl _m和Br _n。

进一步，传统的位置矢量非线性积分补偿项通常有如下形式：

其中，Cc _mn为传统形式的位置矢量非线性积分补偿(又称旋转效应和涡卷效应补偿)系数。

为在传统的位置矢量非线性积分补偿系数设计方法的基础上设计式(15)定义的双边形式的位置矢量非线性积分补偿项中的系数Cl _m和Cr _n，令：

根据式(79)，可得如下关系式：

Cc ₁₁Cc _ij-Cc _1jCc _i1＝0,2≤i≤M ₂,2≤j≤N ₂ (80)

在式(80)确定的约束关系的限定下，采用传统方法设计出位置矢量非线性积分补偿系数Cc _mn，再根据式(79)确定的关系计算出Cl _m和Cr _n。

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

Claims

一种基于双速数值积分结构的惯性预积分的优化方法，其特征在于，在组合运动测量系统惯性预积分设计过程中引入双速数值积分结构，并实施双速数值积分结构的参数优化；

所述双速数值积分结构包括双速旋转积分结构、双速速度积分结构和双速位置积分结构；

所述双速数值积分结构的参数优化包括线性积分参数优化和非线性积分参数优化。
根据权利要求1所述的基于双速数值积分结构的惯性预积分的优化方法，其特征在于，所述双速旋转积分结构为
其中，φ1 _ij为旋转矢量的线性积分项，φ2 _ij为双边形式的旋转矢量非线性积分补偿项，ω _k为理想惯性测量角速度原始采样，ω _k的采样间隔为δt，ω' _m和ω' _n为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样，ω' _m和ω' _n的采样间隔为δT _φ、个数为
A _k为旋转矢量线性积分拟合系数，Αl _m和Αr _n为旋转矢量非线性积分补偿系数，Σ为求和运算符，×为矢量叉乘。
根据权利要求1所述的基于双速数值积分结构的惯性预积分的优化方法，其特征在于，所述双速速度积分结构为ΔV _ij＝ΔV1 _ij+ΔV2 _ij，

其中，ΔV1 _ij为速度矢量的线性积分项，ΔV2 _ij为双边形式的速度矢量非线性积分补偿项，a _k为理想惯性测量加速度原始采样，a _k的采样间隔为δt，ω _1,m为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样，ω _1,m的采样间隔为
个数为M ₁，a _1,n为从原始采样{a _k|k＝i,i+1,...,j}中抽取的加速度采样，a _1,n的采样间隔为
个数为N ₁，B _k为速度矢量线性积分拟合系数，Bl _m和Br _n为速度矢量非线性积分补偿系数。
根据权利要求1所述的基于双速数值积分结构的惯性预积分的优化方法，其特征在于，所述双速位置积分结构为ΔP _ij＝ΔP1 _ij+ΔP2 _ij，

其中，ΔP1 _ij为位置矢量的线性积分项，ΔP2 _ij为双边形式的位置矢量非线性积分补偿项，a _k为理想惯性测量加速度原始采样，a _k的采样间隔为δt，ω _2,m为从原始采样{ω _k|k＝i,i+1,...,j}中抽取的角速度采样，ω _2,m的采样间隔为
个数为M ₂，a _2,n为从原始采样{a _k|k＝i,i+1,...,j}中抽取的加速度采样，a _2,n的采样间隔为
个数为N ₂，C _k为位置矢量线性积分拟合系数，Cl _m和Cr _n为位置矢量非线性积分补偿系数。
根据权利要求1-4任一项所述的基于双速数值积分结构的惯性预积分的优化方法，其特征在于，所述线性积分结构参数优化步骤为：

(1)定义一种多段多项式表达形式的惯性矢量：

其中，ω(t)为t时刻的理想测量惯性矢量；M为每段多项式的项数；N为分段多项式函数的段数；g _j,i为第j段多项式的系数矢量，且i＝1,2,...,M、j＝1,2,...,N；t ₀为初始时刻；多项式阶数为M-1；

定义在时间段[t ₀,t ₀+(M+N-2)δt]上对ω(t)进行等时间间隔δt采样：

其中，ω _j为对ω(t)在t ₀+(j-1)δt时刻的采样，且j＝1,2,...,M+N-1；

ω _j与g _j,i之间关系式的矩阵表达为：

W＝T _ωG

其中，W＝[W ₁ ^T W ₂ … W _N] ^T，
[] ^T为取矩阵的转置运算符，Π为求乘积运算符，Ο为空矩阵；W ₁＝[(ω _j) _j,1] _M×1且j＝1,2,...,M；W _j-(M-1)＝[(ω _j) _1,1] _1×1，且j＝M+1,M+2,...,M+N-1；G _j＝[(g _j,iδt ^i-1) _i,1] _M×1且j＝1,2,...,N；
为以列矩阵G _j的最后一个元素构成的1×1单元素矩阵； T _gω＝[((j-1) ^i-1) _j,i] _M×M，T _M为取矩阵T _gω的第M行构成的行矩阵，

且j＝2，3，...，N-1；
为取矩阵T _gω的前M-1行和前M-1列构成的(M-1)×(M-1)矩阵，
为取矩阵T _gω的非第1列构成的(M-1)×M矩阵，T' _M为取矩阵T _gω的第M列的前M-1行的元素构成的(M-1)×1矩阵，[] ^-1为取矩阵的逆运算符；

(2)从t ₀时刻到t ₀+(M+N-2)δt时刻对惯性矢量ω(t)进行一次积分，获得积分矢量Δα：

Δα与g _j,i之间关系式的矩阵表达为：

Δα＝δt·O _nesT _αG

其中O _nes＝[(1) _1,i] _1×N，
O _nes为以1为元素的1×N行矩阵，

(3)从t ₀时刻到t ₀+(M+N-2)δt时刻对惯性矢量ω(t)进行二次积分，获得积分矢量ΔΑ：

ΔΑ与g _j,i之间关系式的矩阵表达为：

ΔΑ＝δt ²·O _nesT _ΑG

其中
且j＝2,3,...,N，

(4)Δα、ΔΑ与W之间的关系式为：

Δα＝δt·T _rαW，ΔΑ＝δt ²·T _rΑW

其中，T _rα＝O _nesT _αT _ω ^-1，T _rΑ＝O _nesT _ΑT _ω ^-1；

(5)通过A _φ＝T _rα＝O _nesT _αT _ω ^-1获取旋转线性积分拟合系数A _k的值，通过B _V＝T _rα＝O _nesT _αT _ω ^-1获取线性积分拟合系数B _k的值，通过C _P＝T _rΑ＝O _nesT _ΑT _ω ^-1获取位置线性积分拟合系数C _k的值，其中，A _φ＝[A _i A _i+1 … A _j]，B _V＝[B _i B _i+1 … B _j]，C _P＝[C _i C _i+1 … C _j]。
根据权利要求1-4任一项所述的基于双速数值积分结构的惯性预积分的优化方法，其特征在于，所述非线性积分结构参数优化步骤为：

(1)令传统的旋转矢量非线性积分补偿项表达式、传统的速度矢量非线性积分补偿项表达式和传统的位置矢量非线性积分补偿项表达式分别与引入双边形式的旋转矢量非线性积分补偿项表达式、引入双边形式的速度矢量非线性积分补偿项表达式和引入双边形式的位置矢量非线性积分补偿项表达式等价，即：

(2)约束关系为：

Βc ₁₁Βc _ij-Βc _1jΒc _i1＝0，2≤i≤M ₁，2≤j≤N ₁

Cc ₁₁Cc _ij-Cc _1jCc _i1＝0，2≤i≤M ₂，2≤j≤N ₂

(3)在约束关系的限定下，设计旋转矢量非线性积分补偿系数Αl _m和Αr _n、速度矢量非线性积分补偿系数Βl _m和Br _n以及位置矢量非线性积分补偿系数Cl _m和Cr _n。
根据权利要求6所述的基于双速数值积分结构的惯性预积分的优化方法，其特征在于，传统的旋转矢量非线性积分补偿项表达式为
所述传统的速度矢量非线性积分补偿项表达式为
所述传统的位置矢量非线性积分补偿项表达式为
其中Αc _mn为传统的旋转矢量非线性积分补偿系数，Βc _mn为传统的速度矢量非线性积分补偿系数，Cc _mn为传统的位置矢量非线性积分补偿系数。