CN108827315A - 基于流形预积分的视觉惯性里程计位姿估计方法及装置 - Google Patents

Abstract

本发明公开了一种基于流形预积分的视觉惯性里程计位姿估计方法及装置，所述方法在完成视觉惯性里程计***的初始化后,进行如下步骤：视觉惯性数据对齐；视觉光流位姿推算；进行惯性预积分；视觉和惯性联合优化；窗口边缘化；最后重复上述步骤，实现对相机位姿的连续估计。本发明采用基于流形预积分算法的视觉惯性里程计***比单独视觉里程计***更高的定位精度，基于流形的预积分算法有效地将惯性信息用到到里程计***中，并抑制***的噪声传播，降低惯性零漂对里程计定位精度的影响。

Description

基于流形预积分的视觉惯性里程计位姿估计方法及装置

技术领域

本发明涉及一种视觉惯性里程计位姿估计方法，尤其是一种基于流形预积分的视觉惯性里程计位姿估计方法及装置，属于自主导航领域。

背景技术

视觉测量法(VO)和同时定位和测绘(SLAM)已成为自主导航研究的重要组成部分，因为它们为经典的机器人/车辆定位解决方案提供了方便可靠的替代方案。与激光扫描仪等环境感知传感器相比，相机的优点是重量轻，可以检测大多数固体材料，并以相当高的频率获得读数。因此，它们在导航，运动规划和避障中至关重要。然而，在典型的应用中，相机的视野是有限的。靠近的障碍物遮挡了大部分场景，无结构的表面往往缺乏视觉线索，而重复的纹理使查找对应关系变得复杂。使用惯性传感器可以帮助减轻这些影响。包括摄像头和IMU的传感器设置非常适合于导航，但却带来了巨大的研究挑战。融合惯性和视觉线索是非常有益的，因为它们提供了补充信息，有助于估计过程的稳健性。

在纯视觉里程计***中，***利用摄像头传感器获取周围环境的图像信息，通过对图像进行分析，估计***的运动状态。但是在单目视觉***中存在着尺度问题，即***无法得知所获得的运动过程的实际尺度，只能获取相对的长度信息。同时，单目视觉***还存在初始化问题。但***初始工作时，需要将摄像头进行平移运动才能顺利工作，若只进行纯旋转操作，则会导致***初始化过程失败。

对于视觉惯性里程计***，由于***中添加了惯性传感器信息，针对纯视觉***无法解决的尺度问题和初始化问题，可以通过利用惯性信息进行尺度对准，顺利的获取***的尺度信息并完成初始化过程。但是惯性传感器在连续工作时会存在累积漂移的问题。因此，在长时间工作时利用视觉信息对惯性元件的测量结果进行零漂校正成为提高组合里程计***定位精度的关键。

发明内容

本发明的目的是为了解决上述现有技术的缺陷，提供了一种基于流形预积分的视觉惯性里程计位姿估计方法，该方法有效地将惯性信息用到到里程计***中，并抑制***的噪声传播，降低惯性零漂对里程计定位精度的影响。

本发明的另一目的在于提供一种基于流形预积分的视觉惯性里程计位姿估计装置。

本发明的目的可以通过采取如下技术方案达到：

基于流形预积分的视觉惯性里程计位姿估计方法，所述方法在完成视觉惯性里程计***的初始化后，进行如下步骤：

通过对视觉数据和惯性数据标记时间戳，获取处于两帧视觉图像之间的所有惯性数据；

对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，获得两帧视觉图像间的跟踪点信息，利用跟踪点信息对两帧视觉图像数据进行PnP匹配工作，获得基于视觉图像信息的相机估计位姿；

对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，并进行误差传播更新，获得两帧视觉图像之间的运动约束关系；

以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化；采用g2o库作为优化工具进行优化，以李代数表示相机运动的旋转过程，有效降低优化过程的计算量。利用视觉和惯性紧耦合优化获取的相机位姿估计有效降低惯性数据随时间漂移的影响，有效抑制误差的传播，获得更高的定位精度；

判断当前帧是否为关键帧，若是，此时若当前的优化窗口已满，则在下一帧来临时，将时间最早的一帧进行边缘化处理，将边缘化处理后的该帧剔除出优化窗口，并将当前帧加入优化窗口；若否，则将当前帧数据直接从优化窗口中剔除，保证***的计算量在一定限度内，实现里程计工作的实时性；

重复上述步骤，实现对相机位姿的连续估计。

进一步的，所述完成视觉惯性里程计***的初始化，具体为：利用惯性信息获得视觉图像的尺度大小，完成视觉惯性里程计***的初始化操作。

进一步的，所述利用惯性信息获得视觉图像的尺度大小完成初始化操作，具体包括：

进行纯视觉运动估计，获得两对应时刻间的无尺度相对运动估计；

对惯性信息进行积分，获取基于惯性信息的相机运动关系；

对比基于视觉的无尺度运动关系和基于惯性的实际运动关系，获得视觉估计过程的尺度因子，完成视觉惯性里程计***的初始化过程。

进一步的，所述对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，具体包括：

对图像约束方程进行泰勒展开；

根据泰勒展开后的图像约束方程，采用LK光流法计算光流向量；

根据光流向量，获得当前帧的视觉图像点在下一帧视觉图像中的对应位置，完成LK光流法匹配工作。

进一步的，所述图像约束方程，如下式：

I(x,y,z,t)＝I(x+δx,y+δy,z+δz,t+δt)

其中，I(x,y,z,t)为在(x,y,z)位置的体素；

对图像约束方程进行泰勒展开，如下式：

其中，V_x,V_y,V_z为I(x,y,z,t)的光流向量的x,y,z分量，则是图像在(x,y,z,t)该点处相应方向的差分；

将上式简写为：

▽I^TV＝-I_t

根据泰勒展开后的图像约束方程，采用LK光流法计算光流向量，如下式：

I_x1V_x+I_y1V_y+I_z1V_z＝-I_t1

I_x2V_x+I_y2V_y+I_z2V_z＝-I_t2

I_xnV_x+I_ynV_y+I_znV_z＝-I_tn

采用最小二乘法解这个方程组，如下式：

进一步的，所述对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，具体包括：

对惯性测量过程进行建模，如下式：

其中，B代表惯性坐标系，W代表世界坐标系；

考虑惯性元件的运动模型，如下式：

在t+Δt时间内积分，离散化，结合测量方程，得到下式：

将上式中只与惯性测量值有关的变量汇集到一起，进行预积分处理，获得预积分结果；

定义预积分变量ΔR_ij,Δv_ij,Δp_ij如下：

在获得预积分变量后，通过下式计算获得基于流形上预积分的估计位姿：

R_j＝R_iΔR_ij

v_j＝v_i-WgΔt_ij+R_iΔv_ij

进一步的，所述进行误差传播更新，获得两帧视觉图像之间的运动约束关系，具体包括：

将预积分变量中的误差部分分离开：

其中，δφ_ij,δv_ij,δp_ij为预积分变量对应的噪声变量，整理得到：

算子(·)^表示向量对应的反对称矩阵；

构造误差传递方程，定义噪声的状态量为预积分变量对应的噪声，噪声的输入量为惯性数据的原始噪声：

整理获得惯性预积分的噪声传递方程：

进一步的，所述以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化，具体包括：

定义视觉惯性里程计***的状态向量如下：

χ＝[p,v,q,b_a,b_g]

***的状态向量为15维向量，包含相机运动的平移距离、运动速度、旋转角度、惯性加速度零漂和角速度零漂，对所述状态向量进行图优化，定义如下优化函数：

其中，表示惯性预积分变量对应的优化残差，而表示视觉测量对应的优化残差。

进一步的，所述判断当前帧是否为关键帧，具体为：根据当前帧的位姿估计结果，判断当前帧是否为关键帧；其中，关键帧的选取依据为当前帧与上一关键帧运动距离超过设定的阈值。

本发明的另一目的可以通过采取如下技术方案达到：

基于流形预积分的视觉惯性里程计位姿估计装置，所述装置包括：

初始化模块，用于完成视觉惯性里程计***的初始化；

惯性数据对齐模块，用于通过对视觉数据和惯性数据标记时间戳，获取处于两帧视觉图像之间的所有惯性数据；

视觉光流位姿推算模块，用于对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，获得两帧视觉图像间的跟踪点信息，利用跟踪点信息对两帧视觉图像数据进行PnP匹配工作，获得基于视觉图像信息的相机估计位姿；

惯性预积分模块，用于对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，并进行误差传播更新，获得两帧视觉图像之间的运动约束关系；

第一优化模块，用于以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化；

第二优化模块，用于判断当前帧是否为关键帧，若是，此时若当前的优化窗口已满，则在下一帧来临时，将时间最早的一帧进行边缘化处理，将边缘化处理后的该帧剔除出优化窗口，并将当前帧加入优化窗口；若否，则将当前帧数据直接从优化窗口中剔除。

本发明相对于现有技术具有如下的有益效果：

1、本发明使用光流法对视觉图像信息进行处理，获取***的初始位姿估计。对惯性信息进行流形上的预积分处理，形成视觉信息的帧间约束，采用联合优化器对视觉惯性输出数据进行融合优化，经过理论上证明基于流形预积分的视觉惯性里程计***比单独视觉里程计***更高的定位精度，基于流形的预积分算法有效地将惯性信息用到到里程计***中，并抑制***的噪声传播，降低惯性零漂对里程计定位精度的影响。

2、本发明采用的光流法基本原理是当相机在运动时，假定在很短的时间内，上一帧图像上的某点的位置不发生剧烈变化并且亮度基本不变，同时上一图像上的临近点投影到新的图像上仍是临近点并且速度一致，此时利用图像灰度对位置求偏导的过程，获取上一图像上的点在下一图像上的对应位置，进而估计相机的运动状态，通过使用图像金字塔，在金字塔中寻找光流对应点，能获取更精确的光流匹配，抑制运动估计的误差。

3、本发明对于惯性信息的处理，为了避免在优化过程中，加速度和陀螺仪信息的重复积分过程，将惯性信息中与***当前状态无关的变化独立出来，构造预积分变量，大大降低优化过程的计算量，采用李代数表示相机的旋转过程，利用李群流形上的运算性质，推导里程计***的噪声传播方程，抑制***运行过程中的误差增长，提高里程计定位精度；同时，采用李代数进行运算和优化，降低了***状态向量的维数，减少***计算量。

4、本发明在联合优化器的数学模型选择上，采用滑动窗口法对里程计的传感器信息进行优化。里程计记录固定数量的视觉和惯性传感器信息，对窗口内的传感器数据进行优化调整，当新的传感器信息进入窗口时，对最久(时间最早)的信息进行边缘化处理，将其移出窗口范围，并对新的窗口信息进行优化。在视觉和惯性信息的组合方式上，采用紧耦合优化的方法，将两个子***的输出信息进行特征层融合优化，将优化结果作为定位结果输出。采用这种组合方案充分考虑各子***的信息优势，形成信息互补，提高***工作的稳定性和鲁棒性，获取更高的定位精度。

附图说明

图1为本发明的视觉惯性里程计***的结构框图。

图2为本发明的视觉惯性里程计位姿估计方法流程图。

图3为本发明的惯性预积分的计算流程图。

图4为公开数据集上的本发明方法与真实轨迹、经典Okvis方法定位结果的对比图。

图5a为公开数据集上的本发明方法定位结果与经典Okvis方法定位结果在x轴上的误差对比图。

图5b为公开数据集上的本发明方法定位结果与经典Okvis方法定位结果在y轴上的误差对比图。

图5c为公开数据集上的本发明方法定位结果与经典Okvis方法定位结果在z轴上的误差对比图。

图5d为公开数据集上的本发明方法定位结果与经典Okvis方法定位结果在总位移上的误差对比图。

图6为本发明的视觉惯性里程计位姿估计装置的结构框图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例1：

本实施例的视觉惯性里程计***结构如图1所示，在视觉惯性里程计***中，关键步骤是设计惯性信息的处理方法以及将视觉和惯性信息相结合的联合优化器。联合优化器的主要作用是作为两个定位子***实现***组合的接口，并对***接收来的定位信息进行数据融合。在对惯性子***的研究过程中，流形上的预积分方法发挥重要的作用。当获取惯性信息后，将惯性测量值通过在黎曼流形上的预积分过程，形成两帧视觉测量信息间的约束。利用联合优化器，将视觉信息和惯性信息共同进行优化，进而提高视觉惯性里程计***的定位精度。

如图2所示，本实施例提供了一种基于流形预积分的视觉惯性里程计位姿估计方法，该方法包括如下步骤：

S101、视觉惯性里程计***的初始化

利用惯性信息获得视觉图像的尺度大小，完成时间里程计***的初始化操作。首先进行纯视觉运动估计，获得两对应时刻间的无尺度相对运动估计。接着，对惯性信息进行积分，获取基于惯性信息的相机运动关系。由于在较短时间内，惯性信息的漂移可以近似忽略，认为此时惯性信息具有较高精确度。对比基于视觉的无尺度运动关系和基于惯性的实际运动关系，获得视觉估计过程的尺度因子，完成里程计***的初始化过程。

S102、视觉惯性数据对齐

由于基于流形上的预积分算法需要将两视觉图像帧之间的惯性数据进行整体预积分，因此通过在获取图像和惯性数据时打上时间戳来进行视觉和惯性数据的对齐工作；同时，对惯性原始数据进行简单的均值滤波处理，消除野值；由于所选用的加速度计产生的随机误差会大大影响后续的数据精度，在一开始对原始数据进行简单处理很有必要；在数据读取过程中采用均值滤波，连续读取五次的加速度值取平均值，可以有效的去除掉杂值。

S103、视觉光流位姿推算

本实施例采用的视觉处理算法为光流法，对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，获得两帧视觉图像间的跟踪点信息，利用跟踪点信息对两帧视觉图像数据进行PnP匹配工作，获得基于视觉图像信息的相机估计位姿。

由于在光流跟踪过程中，由于相机的移动，光流跟踪到的点会不断减少。为解决该问题，本***采用更新光流点的方法。在每次光流匹配时，跟踪上的点可以用于后续图像光流跟踪。而对于跟踪失败的点，视为该点已经离开视野范围，全部丢弃，然后重新对图像进行Fast角点提取，将光流点数量补全至设定值，用于后续图像跟踪。

LK光流法是目前光流法中使用最普遍的光流法，LK光流法计算两帧在时间到之间每个每个像素点位置的移动，由于它是基于图像信号的泰勒级数，这种方法称为差分，这就是对于空间和时间坐标使用偏导数。

图像约束方程，如下式：

I(x,y,z,t)＝I(x+δx,y+δy,z+δz,t+δt)

其中，I(x,y,z,t)为在(x,y,z)位置的体素。

假设移动足够小，对图像约束方程进行泰勒展开，如下式：

其中，V_x,V_y,V_z为I(x,y,z,t)的光流向量的x,y,z分量，则是图像在(x,y,z,t)该点处相应方向的差分。

将上式简写为：

▽I^TV＝-I_t

其中，未知量为三维光流向量V_x,V_y,V_z，该问题被称为光圈问题，LK光流法采用非迭代法计算光流向量。

假设流(V_x,V_y,V_z)在一个大小为m*m*m(m＞1)的小窗中是一个常数，那么从像素1,2,...,n(n＝m³)中可以得到下列一组方程：

I_x1V_x+I_y1V_y+I_z1V_z＝-I_t1

I_x2V_x+I_y2V_y+I_z2V_z＝-I_t2

I_xnV_x+I_ynV_y+I_znV_z＝-I_tn

该方程为超定方程，采用最小二乘法解这个方程组，如下式：

根据该光流向量，获得当前帧的视觉图像点在下一帧视觉图像中的对应位置，完成LK光流法匹配工作。

由图像信息***可以计算得到一个相机的位姿估计，但是基于纯图像光流跟踪加PnP匹配算法计算得到的位姿有很大的误差，无法用于实际应用，为了解决该问题，本实施例采用惯性预积分算法，进行视觉和惯性数据紧耦合优化，获取更精确的相机定位结果。

S104、进行惯性预积分

在视觉惯性里程计***中，惯性数据的频率高于图像关键帧的频率，惯性预积分通过重新参数化，把关键帧之间的惯性测量值积分成相对运动的约束，避免了因为初始条件变化造成的重复积分，预积分的流程如图3所示。

惯性数据包含三维的加速度信息和撒么维的角速度信息。由于惯性数据中夹杂着测量噪声和零漂，为了更精确地估计物体相机位姿，首先对惯性测量过程进行建模。

其中B代表惯性坐标系，W代表世界坐标系。

考虑惯性元件的运动模型：

在t+Δt时间内积分，离散化，结合测量方程，可得：

由上式即可由***上一时刻的状态和惯性数据积分获得当前时刻的***状态。但是在视觉里程计***中，由于***位姿不断优化，当***上一时刻位姿改变时，***当前时刻的位姿也需要重新进行积分，为了避免重积分过程，本实施例采用预积分算法。

将上式中只与惯性测量值有关的变量汇集到一起，先进行预积分处理，获得预积分结果后再计算***当前位姿。

定义预积分变量ΔR_ij,Δv_ij,Δp_ijΔR_ij,Δv_ij,Δp_ij如下：

在获得预积分变量后，通过下式计算获得基于流形上预积分的估计位姿：

R_j＝R_iΔR_ij

v_j＝v_i-_WgΔt_ij+R_iΔv_ij

下面考虑惯性预积分过程中的噪声传播。

假定在相邻两帧图像间的惯性零漂恒定，考虑白噪声对预积分变量的影响。

将预积分变量中的误差部分分离开：

其中δφ_ij,δv_ij,δp_ij为预积分变量对应的噪声变量，整理得到：

算子(·)^表示向量对应的反对称矩阵。

构造误差传递方程，定义噪声的状态量为预积分变量对应的噪声，噪声的输入量为惯性数据的原始噪声：

整理获得惯性预积分的噪声传递方程：

其中

利用该噪声传播方程，可以利用迭代的方法递推估计惯性***的噪声，进而在优化估计时直接将噪声项去除，大大提高里程计***的定位精度。

S105、视觉和惯性联合优化

在获得基于视觉跟踪和基于惯性预积分的相机位姿估计后，本视觉惯性里程计***采用图优化的方法，对视觉和惯性进行紧耦合优化，获得更高的相机位姿估计精度。

定义本***的状态向量如下：

χ＝[p,v,q,b_a,b_g]

***的状态向量为十五维向量，包含相机运动的平移距离，运动速度，旋转角度，惯性加速度零漂和角速度零漂。对该状态向量进行图优化，获得更精确的***位姿估计。本***采用g2o库进行图优化操作，定义如下优化函数：

其中，代表惯性预积分变量对应的优化残差，而表示视觉测量对应的优化残差。通过进行惯性预积分过程和基于g2o的图优化过程，能最终获得相机的估计位姿。由于在运算过程中将惯性信息和视觉测量信息进行融合，本实施例的视觉惯性里程计***能获得比普通视觉里程计***更高的定位精度。

S106、窗口边缘化

本实施例采用滑动窗口法控制视觉惯性里程计***的计算量在一定限度内，进而保证视觉惯性里程计***运行的实时性，将需要进行优化的视觉和惯性测量值放入优化窗口中，而不需要进行优化的则剔除出优化窗口，保证视觉惯性里程计***的总计算量恒定，在每次联合位姿优化结束后，都需要根据定位结果进行滑动窗口调整，使得获取新的数据时仍能进行优化，具体包括：

1)确定当前帧是否为关键帧。首先根据当前帧的位姿估计结果，判断当前帧是否为关键帧，关键帧的选取依据为当前帧与上一帧关键帧运动距离超过阈值大小。

2)若当前帧不为关键帧，说明***与上一关键帧运动距离较短，则将当前帧数据直接从优化窗口中剔除。

3)若当前帧为关键帧，则将滑动窗口中时间最早的一帧从优化窗口中剔除，将当前帧作为倒数第二帧。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，相应的程序可以存储于一计算机可读取存储介质中，该存储介质如ROM/RAM、磁盘或光盘等。

本发明方法与真实轨迹、经典Okvis方法定位结果的对比如图4所示，可以看到本发明方法的定位更接近真实轨迹，本发明方法定位结果与经典Okvis方法定位结果在x轴、y轴、z轴和总位移上的误差对比图分别如图5a～5d所示，可以看到本发明方法的误差较小。

实施例2：

如图6所示，本实施例提供了一种基于流形预积分的视觉惯性里程计位姿估计装置，该装置包括初始化模块、惯性数据对齐模块、视觉光流位姿推算模块、惯性预积分模块、第一优化模块和第二优化模块，各个模块的功能如下：

所述初始化模块，用于完成视觉惯性里程计***的初始化；

所述惯性数据对齐模块，用于通过对视觉数据和惯性数据标记时间戳，获取处于两帧视觉图像之间的所有惯性数据；

所述视觉光流位姿推算模块，用于对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，获得两帧视觉图像间的跟踪点信息，利用跟踪点信息对两帧视觉图像数据进行PnP匹配工作，获得基于视觉图像信息的相机估计位姿；

所述惯性预积分模块，用于对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，并进行误差传播更新，获得两帧视觉图像之间的运动约束关系；

所述第一优化模块，用于以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化；

所述第二优化模块，用于判断当前帧是否为关键帧，若是，此时若当前的优化窗口已满，则在下一帧来临时，将时间最早的一帧进行边缘化处理，将边缘化处理后的该帧剔除出优化窗口，并将当前帧加入优化窗口；若否，则将当前帧数据直接从优化窗口中剔除。

在此需要说明的是，本实施例的装置仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。

可以理解，上述实施例的装置所使用的术语“第一”、“第二”等可用于描述各种模块，但这些模块不受这些术语限制。这些术语仅用于将第一个模块与另一个模块区分。举例来说，在不脱离本发明的范围的情况下，可以将称为第一优化模块称为第二优化模块，且类似地，可将第二优化模块称为第一优化模块，第一优化模块和第二优化模块两者都是优化模块，但其不是同一优化模块。

综上所述，本发明使用光流法对视觉图像信息进行处理，获取***的初始位姿估计。对惯性信息进行流形上的预积分处理，形成视觉信息的帧间约束，采用联合优化器对视觉惯性输出数据进行融合优化，经过理论上证明基于流形预积分的视觉惯性里程计***比单独视觉里程计***更高的定位精度，基于流形的预积分算法有效地将惯性信息用到到里程计***中，并抑制***的噪声传播，降低惯性零漂对里程计定位精度的影响。

以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明专利构思加以等同替换或改变，都属于本发明专利的保护范围。

Claims (10)

1.基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述方法在完成视觉惯性里程计***的初始化后，进行如下步骤：

通过对视觉数据和惯性数据标记时间戳，获取处于两帧视觉图像之间的所有惯性数据；

对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，获得两帧视觉图像间的跟踪点信息，利用跟踪点信息对两帧视觉图像数据进行PnP匹配工作，获得基于视觉图像信息的相机估计位姿；

对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，并进行误差传播更新，获得两帧视觉图像之间的运动约束关系；

以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化；

判断当前帧是否为关键帧，若是，此时若当前的优化窗口已满，则在下一帧来临时，将时间最早的一帧进行边缘化处理，将边缘化处理后的该帧剔除出优化窗口，并将当前帧加入优化窗口；若否，则将当前帧数据直接从优化窗口中剔除；

重复上述步骤，实现对相机位姿的连续估计。

2.根据权利要求1所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述完成视觉惯性里程计***的初始化，具体为：利用惯性信息获得视觉图像的尺度大小，完成视觉惯性里程计***的初始化操作。

3.根据权利要求2所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述利用惯性信息获得视觉图像的尺度大小完成初始化操作，具体包括：

进行纯视觉运动估计，获得两对应时刻间的无尺度相对运动估计；

对惯性信息进行积分，获取基于惯性信息的相机运动关系；

对比基于视觉的无尺度运动关系和基于惯性的实际运动关系，获得视觉估计过程的尺度因子，完成视觉惯性里程计***的初始化过程。

4.根据权利要求1所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，具体包括：

对图像约束方程进行泰勒展开；

根据泰勒展开后的图像约束方程，采用LK光流法计算光流向量；

根据光流向量，获得当前帧的视觉图像点在下一帧视觉图像中的对应位置，完成LK光流法匹配工作。

5.根据权利要求4所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述图像约束方程，如下式：

I(x,y,z,t)＝I(x+δx,y+δy,z+δz,t+δt)

其中，I(x,y,z,t)为在(x,y,z)位置的体素；

对图像约束方程进行泰勒展开，如下式：

其中，V_x,V_y,V_z为I(x,y,z,t)的光流向量的x,y,z分量，则是图像在(x,y,z,t)该点处相应方向的差分；

将上式简写为：

根据泰勒展开后的图像约束方程，采用LK光流法计算光流向量，如下式：

I_x1V_x+I_y1V_y+I_z1V_z＝-I_t1

I_x2V_x+I_y2V_y+I_z2V_z＝-I_t2

I_xnV_x+I_ynV_y+I_znV_z＝-I_tn

采用最小二乘法解这个方程组，如下式：

6.根据权利要求1所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，具体包括：

对惯性测量过程进行建模，如下式：

其中，B代表惯性坐标系，W代表世界坐标系；

考虑惯性元件的运动模型，如下式：

在t+Δt时间内积分，离散化，结合测量方程，得到下式：

将上式中只与惯性测量值有关的变量汇集到一起，进行预积分处理，获得预积分结果；

定义预积分变量ΔR_ij,Δv_ij,Δp_ij如下：

在获得预积分变量后，通过下式计算获得基于流形上预积分的估计位姿：

R_j＝R_iΔR_ij

v_j＝v_i-_WgΔt_ij+R_iΔv_ij

7.根据权利要求6所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述进行误差传播更新，获得两帧视觉图像之间的运动约束关系，具体包括：

将预积分变量中的误差部分分离开：

其中，δφ_ij,δv_ij,δp_ij为预积分变量对应的噪声变量，整理得到：

算子(·)^∧表示向量对应的反对称矩阵；

构造误差传递方程，定义噪声的状态量为预积分变量对应的噪声，噪声的输入量为惯性数据的原始噪声：

整理获得惯性预积分的噪声传递方程：

8.根据权利要求1所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化，具体包括：

定义视觉惯性里程计***的状态向量如下：

χ＝[p,v,q,b_a,b_g]

***的状态向量为15维向量，包含相机运动的平移距离、运动速度、旋转角度、惯性加速度零漂和角速度零漂，对所述状态向量进行图优化，定义如下优化函数：

其中，表示惯性预积分变量对应的优化残差，而表示视觉测量对应的优化残差。

9.根据权利要求1所述的基于流形预积分的视觉惯性里程计位姿估计方法，其特征在于：所述判断当前帧是否为关键帧，具体为：根据当前帧的位姿估计结果，判断当前帧是否为关键帧；其中，关键帧的选取依据为当前帧与上一关键帧运动距离超过设定的阈值。

10.基于流形预积分的视觉惯性里程计位姿估计装置，其特征在于：所述装置包括：

初始化模块，用于完成视觉惯性里程计***的初始化；

惯性数据对齐模块，用于通过对视觉数据和惯性数据标记时间戳，获取处于两帧视觉图像之间的所有惯性数据；

视觉光流位姿推算模块，用于对每一帧获取的视觉图像，与***存储的上一帧视觉图像进行LK光流法匹配，获得两帧视觉图像间的跟踪点信息，利用跟踪点信息对两帧视觉图像数据进行PnP匹配工作，获得基于视觉图像信息的相机估计位姿；

惯性预积分模块，用于对两帧视觉图像之间的所有惯性数据进行预积分处理，计算定义的预积分变量，并进行误差传播更新，获得两帧视觉图像之间的运动约束关系；

第一优化模块，用于以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对***状态进行优化；

第二优化模块，用于判断当前帧是否为关键帧，若是，此时若当前的优化窗口已满，则在下一帧来临时，将时间最早的一帧进行边缘化处理，将边缘化处理后的该帧剔除出优化窗口，并将当前帧加入优化窗口；若否，则将当前帧数据直接从优化窗口中剔除。