CN110455309A - 具备在线时间校准的基于msckf的视觉惯性里程计 - Google Patents

Abstract

本发明提出具备在线时间校准的基于MSCKF的视觉惯性里程计，属于定位导航技术领域。该方法将时间偏移变量加入状态向量；令相机和惯性测量单元分别采集视觉图像和测量加速度与角速度；对视觉图像进行特征提取与特征匹配，得到特征点在图像平面的二维观测；当惯性测量单元的加速度计与陀螺仪分别采集到一次加速度和角速度数据，传播视觉惯性里程计的状态向量及协方差矩阵；当相机采集到一次新的视觉图像，增广该状态向量以及协方差矩阵；当满足状态更新条件时，建立带时间偏移的特征点投影模型,并对状态向量与协方差矩阵进行更新。最后，按照边缘化条件对状态向量进行边缘化处理。本发明使VIO算法性能不受时间偏移影响，具有很高的实用性。

Description

具备在线时间校准的基于MSCKF的视觉惯性里程计

技术领域

本发明涉及定位导航技术领域，具体涉及一种具备在线时间校准的基于MSCKF的视觉惯性里程计。

背景技术

近年来，状态估计成为了计算机视觉和机器人领域的一个热门话题，它在自动驾驶、移动机器人、增强现实和虚拟现实等应用中均有着重要体现。在不同的传感器组合中，视觉惯性传感器系统(由相机和惯性测量单元IMU组成,简称视觉惯性系统)由于其具有体积小、成本低，好的互补性等优势而受到了广泛关注。在视觉惯性里程计VIO算法中，为获得精确的位姿估计，常常要求视觉惯性系统中的IMU数据与相机的图像数据严格时间同步，但由于传感器自身延迟、时钟不同步，数据堵塞等原因，IMU数据与图像数据之间却存在着一定的时间偏移，并且其值是未知的。该时间偏移对VIO算法的精确度与鲁棒性有着不容忽视的影响，几十毫秒的时间偏移可导致图像数据和IMU数据序列之间完全错位，从而使视觉惯性里程计算法整体性能严重下降。

因此，视觉惯性系统的时间同步问题在最近的研究中得到了大量讨论。一般来说，视觉惯性系统的时间同步存在两种方式，一种是硬件同步，另一种是软件同步。但前者成本非常高并且工艺复杂，因此对于一般市场级、低成本的视觉惯性系统并不适用。对于软件同步，2013年，P.Furgale等人提出了目前一个主流的校准工具箱Kalibr。在这项工作中，可在极大似然估计的严格理论框架内，联合估计不同传感器之间的时间偏移和空间转换。2017年，J.Rehder等人提出了一种通用的时间偏移的标定方法。该方法利用基函数估计时间偏差，使得该校正方法存在于极大似然估计的严格理论框架内。2014年，J.Kelly等人提出了一种多本体感知与外部感知传感器的时间校准通用框架。在这项工作中，时间偏移校准被看作是一项配准任务，通过利用一种类ICP(iterative closest poin，迭代最近点)方法在空间中对不同传感器的三维旋转曲线进行对准来完成时间偏移标定。该方法不仅适用于相机和IMU传感器，还可用于其它本体感知和外部感知传感器的时间偏移校准。但该方法的不足之处在于只对准了三维旋转曲线，忽略了其他有用信息，比如位置曲线等。

上述的所有时间偏移校准方法均是离线方法，需配合标定板使用，因此在时间效率与实用性相比在线方法具有局限性。2018年，T.Qing等人提出了一种基于优化的VIO框架下的在线时间标定方法。在这项工作中，引入了特征速度来建模和补偿时间偏差，该方法虽是在线校准，但其局限性在于基于优化的VIO算法所要求的计算量是非常大的，且该方法的相机在两帧图像间隔时间内是是匀速运动的基础假设并不符合实际情况。此外，该方法不能用于对时变的时间偏移的在线校准。2014年，M.Li等人提出了一种基于扩展卡尔曼滤波EKF的VIO框架下的在线时间偏移校准方法。在这项工作中，时变的时间偏移可以被估计。但该方法的不足之处在于，求解时间偏移的雅可比矩阵过程不独立，影响其它已有变量雅可比矩阵的求解，导致所有的雅可比矩阵均需重新计算，计算分析过程复杂。此外，该方法需要使用估计的IMU测量数据，从而引入了额外的误差。

发明内容

本发明的目的是为解决由于在视觉惯性传感器系统中图像数据与IMU数据序列存在着时间不同步，从而导致整个VIO算法性能下降甚至失效的这一问题，提出一种具备在线时间校准的基于MSCKF的视觉惯性里程计。本方法具有通用性，可直接适用于甚至一般的低成本、市场级、或自组装的视觉惯性传感器系统，使得VIO算法性能不受时间偏移的影响，在自动驾驶、移动机器人、增强现实和虚拟现实等领域中有较大的应用前景。

本发明提出一种具备在线时间校准的基于MSCKF的视觉惯性里程计，其特征在于，所述视觉惯性里程计执行以下步骤：

1)定义视觉惯性里程计的状态向量x，定义视觉惯性里程计的误差状态向量具体步骤如下：

1-1)定义增广惯性测量单元IMU状态向量为其中上标T表示向量转置，x_I为IMU状态向量，即其中单位四元数表示从IMU坐标系到全局坐标系的相对旋转，^Gp_I和^Gv_I分别表示相机坐标系在全局坐标系下的位置和速度，b_g和b_a分别表示IMU的加速度计和陀螺仪的随机游走，和^Cp_I分别表示相机坐标系与IMU坐标系之间的相对旋转变换与相对位置变换；t_d为时间偏移变量，t_d＝t₂-t₁，其中t₁为IMU的延迟时间，t₂为相机的延迟时间；

定义增广IMU误差状态向量为x其中为IMU误差状态向量，为时间偏移t_d的误差状态；

1-2)定义视觉惯性里程计VIO的状态向量为定义VIO的误差状态向量为其中表示t_i时刻相机坐标系在全局坐标系下的位姿，i＝1,2,…,n,…,其中表示t_i时刻相机坐标系到全局坐标系的相对旋转，表示t_i时刻相机坐标系到全局坐标系的相对平移，表示t_i时刻相机状态的误差向量；

2)相机按照相机的工作频率采集视觉图像；惯性测量单元的加速度计与陀螺仪分别按照各自工作频率分别测量加速度与角速度；

3)按照多状态约束卡尔曼滤波器MSCKF算法，对步骤2)采集的视觉图像进行特征提取与特征匹配，得到特征点在图像平面的二维观测，记为表示特征点f_j在t_i时刻图像I_i中的坐标，其中u、v分别表示特征点在图像平面的横、纵坐标；

4)当惯性测量单元的加速度计与陀螺仪分别采集到一次加速度和角速度数据，传播状态向量x；具体步骤如下：

4-1)按照MSCKF算法，列写IMU状态向量x_I的动力学方程；

4-2)若t_d为非时变，则列写时间偏移t_d的状态方程为若t_d为时变，则列写t_d的状态方程为其中n_d为高斯白噪声过程；

4-3)根据步骤4-1)与步骤4-2)所列写的方程，按照MSCKF算法，列写近似线性化方程其中F与G分别为状态转移矩阵与噪声输入矩阵，为过程噪声序列；

4-4)按照MSCKF算法，利用测得的加速度与角速度，根据步骤4-1)的IMU状态x_I的动力学方程传播IMU状态向量x_I得到当前IMU的状态向量x_I；VIO状态向量x中的其他变量保持不变；

4-5)按照MSCKF算法，根据步骤4-3)的近似线性化方程计算协方差矩阵P；

5)当相机采集到一次新的视觉图像I_k，状态向量x增广一个相机状态得到增广后的状态向量x，并计算增广后的协方差矩阵P；具体步骤如下：

5-1)按照MSCKF算法，根据步骤4-4)得到的当前IMU状态向量x_I计算相机状态并将加入到状态向量x中，得到增广后的状态向量

5-2)按照MSCKF算法，计算增广后的协方差矩阵P；

6)根据MSCKF算法的状态更新条件进行判定：若满足，则进入步骤7)；若不满足,则重新返回步骤4)；

7)建立带时间偏移t_d的特征点投影模型,并对步骤5)增广后的状态向量x进行状态更新；具体步骤如下：

7-1)建立带时间偏移的t_d特征点投影模型为：

其中M_j是步骤5)增广后的状态向量x中观测到特征点f_j的相机状态序号的集合，z^j(t_i+t_d)为特征点f_j在t_i+t_d时刻在图像平面的二维观测，π(·)代表相机投影模型，表示t_i时刻特征点f_j在相机坐标系中的三维坐标，表示观测噪声，为高斯白噪声过程；

7-2)近似展开z^j(t_i+t_d)；具体步骤如下：

7-2-1)令：

其中 为当前时刻对时间偏移t_d的估计值；

7-2-2)对式(2)作一阶泰勒展开，得到：

7-2-3)步骤3)获得特征点f_j在视觉图像I_i附近的n个视觉图像中的二维观测为其中m,n均为整数，n≥2,0≤m＜n；根据采用插值方法得到插值函数进一步得到与的值；利用与的值近似步骤7-2-2)中的与得到：

7-3)将式(4)带入步骤7-1)中的特征点观测方程式(3)中，得到：

按照MSCKF算法，进一步得到线性化方程：

其中为当前对的估计值，为相机状态的雅可比矩阵，为特征点f_j的雅可比矩阵；

7-4)将式14)中的所有观测到特征点f_j的相机状态和时间偏移t_d合并到状态向量x中，得到：

7-5)按照MSCKF算法，将式(15)中的r^j投影到的零空间V，得到：

7-6)则根据式(16)，并利用步骤5)得到的状态向量x与协方差矩阵P，按照MSCKF算法，完成标准的扩展卡尔曼滤波EKF状态更新过程，输出更新后的状态向量x，当前时刻校准完毕；

8)根据MSCKF算法的边缘化条件进行判定,若满足，则对状态向量x进行边缘化处理，移除状态向量x中的部分相机状态，进入步骤9)；若不满足，则对状态向量x不进行处理，进入步骤9)；

9)当惯性测量单元的加速度计与陀螺仪采集到新的一次加速度和角速度数据时，重新返回步骤4)。

本发明的特点及有益效果在于：

本发明通过将时间偏移t_d作为一个需要被估计的变量加入到MSCKF多状态约束卡尔曼滤波器的状态向量中，在状态传播过程中增加时间偏移t_d的状态方程，在状态更新过程中通过修正投影模型，使IMU积分约束与视觉投影约束对齐，从而进行正确的状态更新，得到正确的状态估计。

本发明具有通用性，操作简便，VIO算法性能不受视觉惯性传感器系统的时间偏移的影响，从而可直接适用于甚至一般的低成本、市场级或自组装的视觉惯性传感器系统；本发明可进行在线的时间偏移校准，相比其他已有的离线的校准方法，本发明无需标定板，在时间效率、实用性、一致性上更具优势；本发明是基于扩展卡尔曼滤波的VIO算法框架，相比其他基于优化的算法，在获得相同估计精度下，在时间效率、资源消耗上更具优势；本发明的修正观测模型的方法中时间偏移t_d偏导数的求解过程独立，不影响其他状态量雅可比矩阵的求解，从而简化分析计算；此外本发明亦可用于对时变时间偏移的估计与校准。

附图说明

图1是本发明方法的整体流程图。

图2是本发明方法在真实传感器小觅双目相机上的轨迹测试结果示意图。

具体实施方式

本发明提出一种具备在线时间校准的基于MSCKF的视觉惯性里程计，下面结合具体实施例进一步详细说明如下。

本发明提出一种具备在线时间校准的基于MSCKF的视觉惯性里程计，该视觉惯性里程计执行以下步骤：

1)定义视觉惯性里程计VIO的状态向量x，定义VIO的误差状态向量具体步骤如下：

1-1)定义增广IMU状态向量为其中上标T表示向量转置，x_I为IMU状态向量，即其中单位四元数表示从IMU坐标系到全局坐标系的相对旋转，^Gp_I和^Gv_I分别表示相机坐标系在全局坐标系下的位置和速度，b_g和b_a分别表示IMU的加速度计和陀螺仪的随机游走，和^Cp_I分别表示相机坐标系与IMU坐标系之间的相对旋转变换与相对位置变换；t_d为时间偏移变量，定义为：设IMU的延迟时间为t₁，相机的延迟时间为t₂，则时间偏移为t_d＝t₂-t₁；定义增广IMU误差状态向量为其中为IMU误差状态向量，为时间偏移t_d的误差状态；

1-2)定义视觉惯性里程计VIO的状态向量为定义VIO的误差状态向量为其中表示t_i时刻相机坐标系在全局坐标系下的位姿，即其中表示t_i时刻相机坐标系到全局坐标系的相对旋转，表示t_i时刻相机坐标系到全局坐标系的相对平移，表示t_i时刻相机状态的误差向量；

2)相机按照相机的工作频率采集视觉图像，所述视觉图像为单目、双目或多目图像的一种或多种的组合；惯性测量单元的加速度计与陀螺仪分别按照各自工作频率分别测量加速度与角速度；

3)按照MSCKF算法，对步骤2)所采集的视觉图像进行特征提取与特征匹配，得到特征点在图像平面的二维观测，如表示特征点f_j在t_i时刻图像I_i中的坐标，其中u、v分别表示特征点在图像平面的横、纵坐标；

4)当惯性测量单元的加速度计与陀螺仪分别采集到一次加速度和角速度数据，传播状态向量x，具体步骤如下：

4-1)按照MSCKF算法，列写IMU状态x_I的动力学方程；

4-2)若t_d为非时变，则列写时间偏移t_d的状态方程为若t_d为时变，则列写t_d的状态方程为其中n_d为高斯白噪声过程；

4-3)根据步骤4-1)与步骤4-2)所列写的方程，按照MSCKF算法，列写近似线性化方程其中F与G分别为状态转移矩阵与噪声输入矩阵，为过程噪声序列；

4-4)按照MSCKF算法，利用测得的加速度与角速度，根据步骤4-1)的IMU状态x_I的动力学方程传播IMU状态向量x_I得到当前IMU的状态向量x_I；VIO状态向量x中的其他变量(包括时间偏移t_d)保持不变；

4-5)按照MSCKF算法，根据步骤4-3)的近似线性化方程计算协方差矩阵P；

5)当相机采集到一次新的视觉图像I_k，状态向量x增广一个相机状态得到增广后的状态向量x，并计算增广后的协方差矩阵P；具体步骤如下：

5-1)按照MSCKF算法，根据步骤4-4)得到的当前IMU状态向量x_I计算相机状态并将加入到状态向量x中，得到增广后的状态向量

5-2)按照MSCKF算法，计算增广后的协方差矩阵P；

6)根据MSCKF算法的状态更新条件进行判定：若满足，则进入步骤7)；若不满足,则重新返回步骤4)；

7)建立带时间偏移t_d的特征点投影模型,并对步骤5)增广后的状态向量x进行状态更新。具体步骤如下：

7-1)建立带时间偏移的t_d特征点投影模型为：

其中M_j是步骤5)增广后的状态向量x中观测到特征点f_j的相机状态序号的集合，z^j(t_i+t_d)为特征点f_j在t_i+t_d时刻在图像平面的二维观测，π(·)代表相机模型，表示t_i时刻特征点f_j在相机坐标系中的三维坐标，表示观测噪声，为高斯白噪声过程。

7-2)近似展开z^j(t_i+t_d)；具体步骤如下：

7-2-1)令：

其中 为当前时刻对时间偏移t_d的估计值。

7-2-2)对式(2)作一阶泰勒展开，得到：

7-2-3)步骤3)中可获得特征点f_j在视觉图像I_i附近的n个视觉图像中的观测为其中m,n均为整数，n≥2,0≤m＜n。根据采用插值方法得到插值函数进一步得到与的值。利用与的值近似步骤7-2-2)中的与得到：

其中推荐的插值方法如下：

a.若步骤7-2-3)中的n≥3，则选用步骤7-2-3)中的3个观测点采用拉格朗日抛物线插值方法，可得：

其中f_p为相机帧的频率，为已知常数。此外，若为进一步简化计算，可简化(3)与(4)式得：

b.若步骤7-2-3)中的n＝2，则采用拉格朗日线性插值方法。若步骤7-2-3)中的m＝0，则有：

若步骤7-2-3)中的m＝1，则有：

7-3)将步骤7-2-3)中的式(4)带入步骤7-1)中的特征点观测方程式(3)中，得到：

按照MSCKF算法，进一步得到线性化方程：

其中为当前对的估计值，为相机状态的雅可比矩阵，为特征点f_j的雅可比矩阵；其值均在MSCKF算法中给出；

7-4)将式14)中的所有观测到特征点f_j的相机状态和时间偏移t_d合并到状态向量x中，得到：

7-5)按照MSCKF算法，将式(15)中的r^j投影到的零空间V，得到：

7-6)则根据式(16)，并利用步骤5)得到的状态向量x与协方差矩阵P，按照MSCKF算法，完成标准的扩展卡尔曼滤波EKF状态更新过程，计算更新后的状态向量x与协方差矩阵P，并输出更新后的状态向量x，当前时刻校准完毕；

8)根据MSCKF算法的边缘化条件进行判定,若满足，则对状态向量x进行边缘化处理，移除状态向量x中的部分相机状态，进入步骤9)；若不满足，则对状态向量x不进行处理，进入步骤9)；

9)当惯性测量单元的加速度计与陀螺仪采集到新的一次加速度和角速度数据时，重新返回步骤4)。

表1、表2是本发明方法在公共数据集EuRoC上的测试结果，并与传统MSCKF算法(S-MSCKF)进行对比。从实验结果可以看到，本发明方法可以精确估计出时间偏移t_d，估计误差均在0.5ms以内；此外，通过比较轨迹的均方根误差(RMSE)，可以看出本发明方法的定位精度明显高于S-MSCKF算法，由此验证本发明提出的时间偏移校准方法提高了VIO性能。

表1

*stands for algorithm failure.

表2

图2是本发明方法在真实传感器小觅双目相机上的轨迹测试结果示意图。本发明方法的VIO轨迹与传统MSCKF算法(S-MSCKF)的VIO轨迹以及Optitrack(室内定位系统，定位精度小于1mm)测得的真实轨迹进行对比。从实验结果可以看出本发明方法的定位精度明显高于S-MSCKF算法，由此验证本发明提出的时间偏移校准方法明显提高了VIO性能。

Claims (1)

1.一种具备在线时间校准的基于MSCKF的视觉惯性里程计，其特征在于，所述视觉惯性里程计执行以下步骤：

1)定义视觉惯性里程计的状态向量x，定义视觉惯性里程计的误差状态向量具体步骤如下：

1-1)定义增广惯性测量单元IMU状态向量为其中上标T表示向量转置，x_I为IMU状态向量，即其中单位四元数表示从IMU坐标系到全局坐标系的相对旋转，^Gp_I和^Gv_I分别表示相机坐标系在全局坐标系下的位置和速度，b_g和b_a分别表示IMU的加速度计和陀螺仪的随机游走，和^Cp_I分别表示相机坐标系与IMU坐标系之间的相对旋转变换与相对位置变换；t_d为时间偏移变量，t_d＝t₂-t₁，其中t₁为IMU的延迟时间，t₂为相机的延迟时间；

定义增广IMU误差状态向量为其中为IMU误差状态向量，为时间偏移t_d的误差状态；

1-2)定义视觉惯性里程计VIO的状态向量为定义VIO的误差状态向量为其中表示t_i时刻相机坐标系在全局坐标系下的位姿，i＝1,2,…,n,…,其中表示t_i时刻相机坐标系到全局坐标系的相对旋转，表示t_i时刻相机坐标系到全局坐标系的相对平移，表示t_i时刻相机状态的误差向量；

2)相机按照相机的工作频率采集视觉图像；惯性测量单元的加速度计与陀螺仪分别按照各自工作频率分别测量加速度与角速度；

3)按照多状态约束卡尔曼滤波器MSCKF算法，对步骤2)采集的视觉图像进行特征提取与特征匹配，得到特征点在图像平面的二维观测，记为表示特征点f_j在t_i时刻图像I_i中的坐标，其中u、v分别表示特征点在图像平面的横、纵坐标；

4)当惯性测量单元的加速度计与陀螺仪分别采集到一次加速度和角速度数据，传播状态向量x；具体步骤如下：

4-1)按照MSCKF算法，列写IMU状态向量x_I的动力学方程；

4-2)若t_d为非时变，则列写时间偏移t_d的状态方程为若t_d为时变，则列写t_d的状态方程为其中n_d为高斯白噪声过程；

4-3)根据步骤4-1)与步骤4-2)所列写的方程，按照MSCKF算法，列写近似线性化方程其中F与G分别为状态转移矩阵与噪声输入矩阵，为过程噪声序列；

4-4)按照MSCKF算法，利用测得的加速度与角速度，根据步骤4-1)的IMU状态x_I的动力学方程传播IMU状态向量x_I得到当前IMU的状态向量x_I；VIO状态向量x中的其他变量保持不变；

4-5)按照MSCKF算法，根据步骤4-3)的近似线性化方程计算协方差矩阵P；

5)当相机采集到一次新的视觉图像I_k，状态向量x增广一个相机状态得到增广后的状态向量x，并计算增广后的协方差矩阵P；具体步骤如下：

5-1)按照MSCKF算法，根据步骤4-4)得到的当前IMU状态向量x_I计算相机状态并将加入到状态向量x中，得到增广后的状态向量

5-2)按照MSCKF算法，计算增广后的协方差矩阵P；

6)根据MSCKF算法的状态更新条件进行判定：若满足，则进入步骤7)；若不满足,则重新返回步骤4)；

7)建立带时间偏移t_d的特征点投影模型,并对步骤5)增广后的状态向量x进行状态更新；具体步骤如下：

7-1)建立带时间偏移的t_d特征点投影模型为：

其中M_j是步骤5)增广后的状态向量x中观测到特征点f_j的相机状态序号的集合，z^j(t_i+t_d)为特征点f_j在t_i+t_d时刻在图像平面的二维观测，π(·)代表相机投影模型，表示t_i时刻特征点f_j在相机坐标系中的三维坐标，表示观测噪声，为高斯白噪声过程；

7-2)近似展开z^j(t_i+t_d)；具体步骤如下：

7-2-1)令：

其中 为当前时刻对时间偏移t_d的估计值；

7-2-2)对式(2)作一阶泰勒展开，得到：

7-2-3)步骤3)获得特征点f_j在视觉图像I_i附近的n个视觉图像中的二维观测为其中m,n均为整数，n≥2,0≤m＜n；根据采用插值方法得到插值函数进一步得到与的值；利用与的值近似步骤7-2-2)中的与得到：

7-3)将式(4)带入步骤7-1)中的特征点观测方程式(3)中，得到：

按照MSCKF算法，进一步得到线性化方程：

其中为当前对的估计值，为相机状态的雅可比矩阵，为特征点f_j的雅可比矩阵；

7-4)将式14)中的所有观测到特征点f_j的相机状态和时间偏移t_d合并到状态向量x中，得到：

7-5)按照MSCKF算法，将式(15)中的r^j投影到的零空间V，得到：

7-6)则根据式(16)，并利用步骤5)得到的状态向量x与协方差矩阵P，按照MSCKF算法，完成标准的扩展卡尔曼滤波EKF状态更新过程，输出更新后的状态向量x，当前时刻校准完毕；

8)根据MSCKF算法的边缘化条件进行判定,若满足，则对状态向量x进行边缘化处理，移除状态向量x中的部分相机状态，进入步骤9)；若不满足，则对状态向量x不进行处理，进入步骤9)；

9)当惯性测量单元的加速度计与陀螺仪采集到新的一次加速度和角速度数据时，重新返回步骤4)。