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

技术领域

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

背景技术

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

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

惯性基组合运动测量系统中的一个关键组成部分-惯性预积分，通常在一个相对较大的时间间隔上实施，以形成相对运动约束。然而，以标准积分形式实施的惯性预积分效率低下。这一问题被Lupton等人提出的基于欧拉角表达的、Forster等人提出的基于SO

发明内容

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

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

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

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

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

进一步，所述双速旋转积分结构为φ

进一步，所述双速速度积分结构为ΔV

进一步，所述双速位置积分结构为ΔP

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

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

其中，ω(t)为t时刻的理想测量惯性矢量，M为每段多项式的项数，N为分段多项式函数的段数，g

定义在时间段[t

其中，ω

ω

W＝T

其中，W＝[W

(2)从t

Δα与g

Δα＝δt·O

其中O

(3)从t

ΔA与g

ΔA＝δt

其中

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

Δα＝δt·T

其中，T

(5)通过A

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

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

(2)约束关系为：

Βc

Cc

(3)在约束关系的限定下，设计旋转矢量非线性积分补偿系数Al

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

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

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

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

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

附图说明

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

具体实施方式

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

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

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

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

其中，

通过对式(1)-(3)两边变量在[t

V

P

其中，

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

设与

φ

其中，φ1

另外，将ΔV

ΔV

ΔP

其中，ΔV1

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

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

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

或写成：

其中，ω(t)为t时刻的理想测量惯性矢量，M为每段多项式的项数(多项式阶数为M-1)，N为分段多项式函数的段数，g

设在时间段[t

或写成：

其中，ω

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

W

W

T

T

T'

G

其中，W

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

W

进一步，补充关于g

则有：

其中，[]

进一步定义矩阵：

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

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

W＝T

W＝[W

其中，[]

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

或写成：

其中，α(t)为从t

根据式(43)，可得从t

进一步，定义矩阵：

其中，T

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

进一步，定义矩阵：

O

其中，O

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

Δα＝δt·O

再次，直接求解由式(43)或式(44)描述的矢量α(t)在[t

并定义矩阵：

其中，T

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

进一步，定义矩阵：

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

ΔA＝δt

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

Δα＝δt·O

ΔA＝δt

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

Δα＝δt·T

ΔA＝δt

其中，

T

T

令t

A

B

C

若取式(16)或式(17)中定义的惯性矢量为惯性角速度，则设计出式(8)中旋转线性积分拟合系数A

A

若取式(16)或式(17)中定义的惯性矢量为惯性加速度，则设计出式(11)中速度线性积分拟合系数B

B

C

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

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

其中，Ac

为在传统的旋转矢量非线性积分补偿系数设计方法的基础上设计式(9)定义的双边形式的旋转矢量非线性积分补偿项中的系数Al

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

在式(74)确定的约束关系的限定下，采用传统方法设计出旋转矢量非线性积分补偿系数Ac

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

其中，Βc

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

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

Βc

在式(77)确定的约束关系的限定下，采用传统方法设计出速度矢量非线性积分补偿系数Βc

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

其中，Cc

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

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

Cc

在式(80)确定的约束关系的限定下，采用传统方法设计出位置矢量非线性积分补偿系数Cc

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