一种具有同步时间误差估计的双目视觉惯性里程计方法

摘要

本发明公开了一种具有同步时间误差估计的双目视觉惯性里程计方法，包括如下步骤：获取在机体坐标系下的相机图像和IMU的数据；将双目相机外参和同步时间误差加入到状态向量中，构建状态向量和误差向量；进行特征点提取、双目特征匹配跟踪以及剔除外点操作；计算两帧之间带时间偏移量的IMU预积分值；当相机采集到一次新的视觉图像时，增广该状态向量及协方差矩阵；建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。本发明能有效地提高视觉和惯导滤波融合下的里程计定位精度。

说明书

技术领域

本发明涉及一种具有同步时间误差估计的双目视觉惯性里程计方法，属于多传感器数据融合方法技术领域。

背景技术

状态估计在过去几十年里一直是机器人和计算机视觉领域的一个基本研究课题。机器人导航、自动驾驶、虚拟现实(VR)和增强现实(AR)等各种应用都高度依赖于精确的状态估计。为了解决众所周知的尺度模糊问题，多传感器融合方法受到越来越多的关注。借助IMU(惯性测量单元)辅助摄像机，在六自由度SLAM(同步定位与测绘)中取得了令人瞩目的性能。一方面，惯性测量使得俯仰和滚转角以及比例尺可见。另一方面，惯性测量通过弥补视觉跟踪失败时的间隙，改善了运动跟踪性能。为了融合来自不同传感器的数据，必须精确地知道测量记录的时间间隔。

实践中，每个传感器的时间戳通常会受到触发和传输延迟的影响，从而导致不同传感器流之间的时间错位(时间偏移)。因此，传感器的时间同步可能成为多传感器系统的一个关键问题。对于视觉惯性系统，摄像机与IMU之间的时间偏移严重影响鲁棒性和精度。由于触发延迟和传输延迟，相机和IMU之间总是存在时间失调(时间偏移)。时间偏移通常从几毫秒到几百毫秒不等。数十毫秒将导致IMU序列与图像流完全错位，从而极大地影响视觉惯性系统的性能。

发明内容

为解决上述问题，本发明公开了一种具有同步时间误差估计的双目视觉惯性里程计方法，能有效地提高视觉惯性里程计在定位方面的精度。

为达到上述目的，本发明的技术方案如下：

一种具有同步时间误差估计的双目视觉惯性里程计方法，包括如下步骤：

S1：视觉惯性里程计系统坐标系定义；

S2：构建状态向量和误差状态向量；

S3：相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据；

S4：当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新；

S5：计算两帧之间带时间偏移量的IMU预积分值；

S6：当相机采集到一次新的视觉图像时，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，增广该状态向量及协方差矩阵；

S7：建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。

优选的，所述步骤S1具体包括以下过程：

视觉惯性里程计系统坐标系定义：

以初始时刻相机所在位置为世界坐标系的原点，定义载体“前右下”为世界坐标系(G系)的XYZ坐标轴；定义固连在载体上的IMU坐标系为I系，是IMU加速度计、角速度计的测量坐标系；定义固连在载体上的相机坐标系为C系；定义载体坐标系为B系，载体系与IMU坐标系(I系)重合。

优选的，所述步骤S2具体包括以下过程：

构建状态向量和误差状态向量：

定义加入双目外参和同步时间误差的IMU状态向量为

优选的，所述步骤S3具体包括以下过程：

相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作：

相机根据相机的采样频率采集双目视觉图像；IMU的加速度计和陀螺仪以IMU的采样频率采集加速度和角速度信息。对获得的视觉图像进行特征提取和特征匹配，并且利用RANSAC算法筛选出前后帧图像匹配的外点，并且利用网格均匀化提取出的特征，得到特征点在图像平面的二维观测，记为

优选的，所述步骤S4具体包括以下过程：

当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新：

S4.1列写IMU状态向量

S4.2为简化模型，假设时间同步误差t

S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程

,S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量

S4.5根据步骤S4.3得到的近似线性化方程

优选的，所述步骤S5具体包括以下过程：

计算两帧之间带时间偏移量的IMU预积分值。

相机的采样频率明显低于IMU的采样频率，采用IMU预积分的方式，利用IMU的加速度计和陀螺仪的数据，再根据前一时刻的预积分量就可以更新当前时间点处的预积分量，从而得到两帧图像之间的IMU的观测，求得IMU预测到t+t

优选的，所述步骤S6具体包括以下过程：

当相机采集到一次新的视觉图像时，增广该状态向量及协方差矩阵：

S6.1对于双目惯性里程计，只将左目相机在t+t

S6.2计算加入左目相机状态之后的协方差矩阵

其中J

其中，

J

其中，

优选的，所述步骤S7具体包括以下过程：

建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。

S7.1观测模型为

其中，

S7.2单个特征对单相机残差模型为：

其中r

其中

其中

其中，各偏导项公式如下：

S7.3将所有特征点的观测模型合并，得到

其中r

然后对H

其中T

S7.4若不满足状态更新条件，则转到步骤S.4；若满足更新条件，则直接套用扩展卡尔曼滤波更新公式：

K＝PH

ΔX＝Kr

P

其中，K表示卡尔曼增益，ΔX表示系统状态更新量，P

本发明的有益效果是：

本发明将双目相机之间的外参和相机与IMU之间的时间同步误差加入到误差状态向量中进行估计，提高了视觉惯性里程计的精度。

附图说明

图1为本发明的具有同步时间误差估计的双目视觉惯性里程计方法原理图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

实施例1：本发明所述的一种具有同步时间误差估计的双目视觉惯性里程计方法，实现原理如图1所示，实验流程如图1所示。其流程主要包括以下步骤：

步骤S1，视觉惯性里程计系统坐标系定义。

以初始时刻相机所在位置为世界坐标系的原点，定义载体“前右下”为世界坐标系(G系)的XYZ坐标轴；定义固连在载体上的IMU坐标系为I系，是IMU加速度计、角速度计的测量坐标系；定义固连在载体上的相机坐标系为C系；定义载体坐标系为B系，载体系与IMU坐标系(I系)重合。

步骤S2，构建状态向量和误差状态向量。

定义加入双目外参和同步时间误差的IMU状态向量为

步骤S3，相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据。

相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作。

相机根据相机的采样频率采集双目视觉图像；IMU的加速度计和陀螺仪以IMU的采样频率采集加速度和角速度信息。对获得的视觉图像进行特征提取和特征匹配，并且利用RANSAC算法筛选出前后帧图像匹配的外点，并且利用网格均匀化提取出的特征，得到特征点在图像平面的二维观测，记为

步骤S4，当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新。

当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新。

S4.1列写IMU状态向量X

S4.2为简化模型，假设时间同步误差t

S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程

,S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量

S4.5根据步骤S4.3得到的近似线性化方程

步骤S5，计算两帧之间带时间偏移量的IMU预积分值。

相机的采样频率明显低于IMU的采样频率，采用IMU预积分的方式，利用IMU的加速度计和陀螺仪的数据，再根据前一时刻的预积分量就可以更新当前时间点处的预积分量，从而得到两帧图像之间的IMU的观测，求得IMU预测到t+t

步骤S6，当相机采集到一次新的视觉图像时，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，增广该状态向量及协方差矩阵：

S6.1对于双目惯性里程计，只将左目相机在t+t

S6.2计算加入左目相机状态之后的协方差矩阵

其中J

其中，

J

其中，

步骤S7，建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。

S7.1观测模型为

其中，

S7.2单个特征对单相机残差模型为：

其中r

其中

其中

其中各偏导项公式如下：

S7.3将所有特征点的观测模型合并，得到

其中r

然后对H

其中T

S7.4若不满足状态更新条件，则转到步骤S.4；若满足更新条件，则直接套用扩展卡尔曼滤波更新公式：

K＝PH

ΔX＝Kr

P

其中，K表示卡尔曼增益，ΔX表示系统状态更新量，P

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。