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

Abstract

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

Description

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

技术领域

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

背景技术

状态估计在过去几十年里一直是机器人和计算机视觉领域的一个基本研究课题。机器人导航、自动驾驶、虚拟现实(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状态向量为其中上标T表示向量转置，X_IMU为IMU状态向量，即为/>其中单位四元数/>表示从世界坐标系到IMU坐标系的相对旋转，/>和/>分别表示IMU在世界坐标系的速度和位置信息，b_g ^T和/>分别表示IMU的陀螺仪和加速度计的随机游走；/>和/>分别表示左目相机坐标系和右目相机坐标系之间的相对旋转变换和相对位置变换；t_d为时间同步误差，t_d＝t_C-t_I，其中t_C和t_I分别为由于曝光，数据传输需要时间等问题而导致的收到相机和IMU数据延迟的时间。增广之后的IMU误差状态向量为

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

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

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

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

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

S4.1列写IMU状态向量的动力学方程；

S4.2为简化模型，假设时间同步误差t_d、和/>为非时变量，则t_d的状态方程为

S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程其中F和G分别为状态转移矩阵和噪声输入矩阵，n_IMU为噪声序列；

,S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量进行预测得到当前IMU的误差状态向量；/>中的其他变量保持不变；

S4.5根据步骤S4.3得到的近似线性化方程计算协方差矩阵P。

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

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

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

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

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

S6.1对于双目惯性里程计，只将左目相机在t+t_d时刻的状态加入到状态向量中，得到增广之后的误差状态向量/>根据双目外参可以算出相机状态对应的右目相机位姿；

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

其中J_new＝[J_I J_t 0]，其中J_new是对状态向量的一阶偏导数形成的雅克比矩阵；J_I表示/>对IMU误差状态向量的一阶偏导数形成的雅克比矩阵，即

其中，为将相机和IMU之间相对旋转四元数转化为的旋转矩阵；/>为相机在IMU坐标系的位移向量的反对称矩阵。

J_t表示对t_d的一阶偏导数形成的雅克比矩阵，即

其中，为IMU在时间t+t_d的旋转角速度向量，/>为其反对称矩阵；/>为在世界坐标系的速度；/>为IMU坐标系和世界坐标系之间的旋转矩阵；

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

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

S7.1观测模型为

其中，表示第i个相机观测到第j个特征点的像素坐标，/>分别为第j个特征点在第i个左目相机的三维坐标；

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

其中r_i ^(j)表示第i个相机对第j个特征点的观测残差，为误差状态向量/>的雅克比矩阵，/>为第j个特征点的雅克比矩阵：

其中为表示第i个左目相机观测到第j个特征点的像素坐标；/>为表示第i个右目相机观测到第j个特征点的像素坐标；^Gp_j为第j个特征点在世界坐标系的位置信息；

其中为第i个相机状态的雅克比矩阵:

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

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

其中r₀表示合并之后的观测残差，H_X表示合并之后的雅克比矩阵，n₀表示合并之后的噪声序列；

然后对H_X进行QR分解来减少观测模型的规模，得：

其中T_H为QR分解之后的对误差状态向量的雅克比矩阵，n_n为QR分解之后的噪声序列；

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

K＝PH^T(HPH^T+R)^T

ΔX＝Kr_n

P_K+1/K+1＝(I+K^TH)P_K+1/K(I+K^TH)^T+KRK^T

其中，K表示卡尔曼增益，ΔX表示系统状态更新量，P_K+1/K+1表示k+1时刻协方差矩阵，R表示噪声协方差矩阵；

本发明的有益效果是：

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

附图说明

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

具体实施方式

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

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

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

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

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

定义加入双目外参和同步时间误差的IMU状态向量为其中上标T表示向量转置，X_IMU为IMU状态向量，即为/>其中单位四元数/>表示从世界坐标系到IMU坐标系的相对旋转，/>和/>分别表示IMU在世界坐标系的速度和位置信息，b_g ^T和/>分别表示IMU的陀螺仪和加速度计的随机游走；/>和/>分别表示左目相机坐标系和右目相机坐标系之间的相对旋转变换和相对位置变换；t_d为时间同步误差，t_d＝t_C-t_I，其中t_C和t_I分别为由于曝光，数据传输需要时间等问题而导致的收到相机和IMU数据延迟的时间。增广之后的IMU误差状态向量为

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

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

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

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

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

S4.1列写IMU状态向量X_IMU的动力学方程；

S4.2为简化模型，假设时间同步误差t_d、和/>为非时变量，则t_d的状态方程为

S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程其中F和G分别为状态转移矩阵和噪声输入矩阵，/>为噪声序列；

,S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量进行预测得到当前IMU的误差状态向量；/>中的其他变量保持不变；

S4.5根据步骤S4.3得到的近似线性化方程计算协方差矩阵P；

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

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

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

S6.1对于双目惯性里程计，只将左目相机在t+t_d时刻的状态加入到状态向量中，得到增广之后的误差状态向量/>根据双目外参可以算出相机状态对应的右目相机位姿；

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

其中J_new＝[J_I J_t 0]，其中J_new是对状态向量的一阶偏导数形成的雅克比矩阵；J_I表示/>对IMU误差状态向量的一阶偏导数形成的雅克比矩阵，即

其中，为将相机和IMU之间相对旋转四元数转化为的旋转矩阵；/>为相机在IMU坐标系的位移向量的反对称矩阵。

J_t表示对t_d的一阶偏导数形成的雅克比矩阵，即

其中，为IMU在时间t+t_d的旋转角速度向量，/>为其反对称矩阵；/>为在世界坐标系的速度；/>为IMU坐标系和世界坐标系之间的旋转矩阵；

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

S7.1观测模型为

其中，表示第i个相机观测到第j个特征点的像素坐标，/>分别为第j个特征点在第i个左目相机的三维坐标；

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

其中r_i ^(j)表示第i个相机对第j个特征点的观测残差，为误差状态向量/>的雅克比矩阵，/>为第j个特征点的雅克比矩阵：

其中为表示第i个左目相机观测到第j个特征点的像素坐标；/>为表示第i个右目相机观测到第j个特征点的像素坐标；Gp_j为第j个特征点在世界坐标系的位置信息；

其中为第i个相机状态的雅克比矩阵:

其中各偏导项公式如下：

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

其中r₀表示合并之后的观测残差，H_X表示合并之后的雅克比矩阵，n₀表示合并之后的噪声序列；

然后对H_X进行QR分解来减少观测模型的规模，得：

其中T_H为QR分解之后的对误差状态向量的雅克比矩阵，n_n为QR分解之后的噪声序列；

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

K＝PH^T(HPH^T+R)^T

ΔX＝Kr_n

P_K+1/K+1＝(I+K^TH)P_K+1/K(I+K^TH)^T+KRK^T

其中，K表示卡尔曼增益，ΔX表示系统状态更新量，P_K+1/K+1表示k+1时刻协方差矩阵，R表示噪声协方差矩阵。

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

Claims (4)

1.一种具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

所述步骤S1具体包括以下过程：

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

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

所述步骤S2具体包括以下过程：

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

定义加入双目外参和同步时间误差的IMU状态向量为其中上标T表示向量转置，X_IMU为IMU状态向量，即为/>其中单位四元数/>表示从世界坐标系到IMU坐标系的相对旋转，/>和/>分别表示IMU在世界坐标系的速度和位置信息，b_g ^T和/>分别表示IMU的陀螺仪和加速度计的随机游走；/>和/>分别表示左目相机坐标系和右目相机坐标系之间的相对旋转变换和相对位置变换；t_d为时间同步误差，t_d＝t_C-t_I，其中t_C和t_I分别为由于曝光、数据传输需要时间问题而导致的收到相机和IMU数据延迟的时间，增广之后的IMU误差状态向量为其中向量都为上述向量的误差向量；

所述步骤S5具体包括以下过程：

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

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

所述步骤S6具体包括以下过程：

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

S6.1对于双目惯性里程计，只将左目相机在t+t_d时刻的状态加入到状态向量中，得到增广之后的误差状态向量/>根据双目外参可以算出相机状态对应的右目相机位姿；

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

其中J_new＝[J_I J_t 0]，其中J_new是对状态向量的一阶偏导数形成的雅克比矩阵；J_I表示/>对IMU误差状态向量的一阶偏导数形成的雅克比矩阵，即

其中，为将相机和IMU之间相对旋转四元数转化为的旋转矩阵；/>为相机在IMU坐标系的位移向量的反对称矩阵；

J_t表示对t_d的一阶偏导数形成的雅克比矩阵，即

其中，为IMU在时间t+t_d的旋转角速度向量，/>为其反对称矩阵；为在世界坐标系的速度；/>为IMU坐标系和世界坐标系之间的旋转矩阵。

2.根据权利要求1所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S3具体包括以下过程：

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

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

3.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S4具体包括以下过程：

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

S4.1列写IMU状态向量的动力学方程；

S4.2为简化模型，假设时间同步误差t_d、左目和右目相机之间的相对旋转和左目和右目之间的相对位移/>为非时变量，则t_d的状态方程为/>

S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程其中F和G分别为状态转移矩阵和噪声输入矩阵，/>为噪声序列；

S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量进行预测得到当前IMU的误差状态向量；/>中的其他变量保持不变；

S4.5根据步骤S4.3得到的近似线性化方程计算协方差矩阵P。

4.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S7具体包括以下过程：

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

S7.1观测模型为

其中，表示第i个相机观测到第j个特征点的像素坐标，/>分别为第j个特征点在第i个左目相机的三维坐标；S7.2单个特征对单相机残差模型为：

其中r_i ^(j)表示第i个相机对第j个特征点的观测残差，为误差状态向量/>的雅克比矩阵，/>为第j个特征点的雅克比矩阵：

其中为表示第i个左目相机观测到第j个特征点的像素坐标；/>为表示第i个右目相机观测到第j个特征点的像素坐标；^Gp_j为第j个特征点在世界坐标系的位置信息；其中为第i个相机状态的雅克比矩阵:

其中各偏导项公式如下：

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

其中r₀表示合并之后的观测残差，H_X表示合并之后的雅克比矩阵，n₀表示合并之后的噪声序列；

然后对H_X进行QR分解来减少观测模型的规模，得：

其中T_H为QR分解之后的对误差状态向量的雅克比矩阵，n_n为QR分解之后的噪声序列；

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

K＝PH^T(HPH^T+R)^T

ΔX＝Kr_n

P_K+1/K+1＝(I+K^TH)P_K+1/K(I+K^TH)^T+KRK^T

其中，K表示卡尔曼增益，ΔX表示系统状态更新量，P_K+1/K+1表示k+1时刻协方差矩阵，R表示噪声协方差矩阵。