CN113066127A - 一种在线标定设备参数的视觉惯性里程计方法和系统 - Google Patents

Abstract

一种在线标定设备参数的视觉惯性里程计方法和系统，采用卡尔曼滤波器在线标定相机外参和IMU的bias，采用非线性优化和滑动窗口在线标定相机内参。本发明利用设备所搭载的相机和惯性测量单元采集图像数据和IMU数据，不依赖设备和标定板标定参数，利用卡尔曼滤波器标定相机外参和IMU的bias保证了计算速度，利用非线性优化和滑动窗口标定相机内参满足了精度要求，且本发明的卡尔曼滤波器和非线性优化滑动窗口是紧耦合关系，更加鲁棒。本发明提供一套完善的实时卡尔曼滤波器和非线性优化滑动窗视觉惯性里程计的算法流程和系统，显著提高了视觉惯性里程计在不同环境和不同设备下的性能，本发明具有流程通用、针对性强、在线运行过程简单易用等优点。

Description

一种在线标定设备参数的视觉惯性里程计方法和系统

技术领域

本发明属于计算机视觉领域，具体涉及一种在线标定设备参数的视觉惯性里程计方法和系统。

背景技术

目前大多数移动端设备都配备了相机和惯性测量单元(IMU)，采用视觉和惯性融合的里程计定位算法受到越来越多的关注。利用设备本身的相机和惯性测量单元的数据进行实时姿态追踪的方法，称为视觉惯性里程计(VIO)算法。

视觉惯性里程计算法广泛应用于增强现实、虚拟现实、导航、移动机器人、无人机、无人驾驶等领域。视觉惯性里程计算法普遍采用非线性优化或者卡尔曼滤波中的一种，非线性优化准确但占用计算资源多，卡尔曼滤波占用计算资源少但准确度略低。消费者的移动端设备大多为自动对焦，相机内参不是一个固定值，所以需要实时优化，由于相机内参对视觉惯性里程计影响很大，所以需要精确的数据，特征点深度也需要非常精确的值。而相机到IMU的转换关系是一个刚体变换，预先标定后即可使用，由于产品线做工存在误差，每一台设备的误差都很小，所以只需要在预先的标定值附近轻微校正即可，同时，IMU的bias比较小，影响不大，进行轻微校正即可。

由此可见，如果采用非线性优化，满足了相机内参和特征点深度对精度的要求，但是计算资源多，计算速度慢；如果采用卡尔曼滤波，计算速度快，但是无法满足精度要求。此外，现有的视觉惯性里程计算法多为离线标定设备相关参数，虽然有不依赖离线标定的视觉惯性里程计算法，通常是采用非线性优化或者松耦合算法，这类算法性能不佳或者效果不佳。因此，需要一种兼顾精度和计算速度的高性能视觉惯性里程计算法。

中国专利文献CN201810150612.7公开了一种视觉惯性里程计的实现方法及系统，该专利旨在描述一种视觉惯性里程计的实现方法，未涉及在线标定和滑动窗流程。

中国专利文献CN201810315048.X公开了一种基于惯性辅助的高效视觉里程计，该专利描述另一种视觉惯性里程计的实现方法，也未涉及在线标定和滑动窗相关流程。

中国专利CN201610012754.8公开了一种双目立体相机与惯性测量单元相对姿态标定方法，该专利仅针对一种双目立体相机的标定流程，应用较为局限。同时该方法仅有一个标定流程。

发明内容

为了克服现有技术缺陷，本发明的一个目的是提供一种在线标定设备参数的视觉惯性里程计的方法，本发明的另一个目的是提供一种在线标定设备参数的视觉惯性里程计系统。为了实现上述目的，本发明采用的技术方案如下：

本发明的一个方面提供了一种在线标定设备参数的视觉惯性里程计方法，所述设备参数包括相机外参、IMU的bias和相机内参，所述方法包括：

通过所述设备采集图像数据和IMU数据；

采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias并计算所述图像数据中图像帧的位姿；

采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参。

优选地，所述采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias，包括：

将所述相机外参和IMU的bias作为所述卡尔曼滤波器状态量，通过对所述卡尔曼滤波器状态量进行初始化、预测、匹配、更新，计算所述相机外参和IMU的bias。

优选地，所述卡尔曼滤波器的状态量包括IMU的位置、IMU的速度、IMU的姿态、加速度计的bias、陀螺仪的bias、IMU到相机的平移和IMU到相机的旋转，其中：

所述加速度计的bias、陀螺仪的bias为所述IMU的bias；

所述IMU到相机的平移和IMU到相机的旋转为所述相机外参。

优选地，所述初始化包括：

对所述图像数据和IMU数据进行处理，输出初始化的点云、图像帧的位姿、二维特征点和二维特征点的描述子或patch；

初始化所述卡尔曼滤波器状态量，计算所述卡尔曼滤波器协方差矩阵、转换矩阵和卡尔曼增益；

初始化所述相机外参、IMU的bias和相机内参。

优选地，所述预测包括：

根据与当前图像帧同时段的IMU和卡尔曼滤波器状态量、协方差矩阵，预测当前图像帧的卡尔曼滤波器状态量和协方差矩阵。

优选地，所述匹配包括：

将上一图像帧的二维特征点与所述当前图像帧的特征点进行匹配，剔除与当前图像帧不匹配的特征点；

输出与当前图像帧匹配的二维特征点的坐标和投影误差。

优选地，所述更新包括：

根据所述投影误差、所述预测的当前图像帧的卡尔曼滤波器状态量、转换矩阵，更新所述当前图像帧的卡尔曼滤波器状态量和协方差矩阵，通过更新后的卡尔曼滤波器状态量得到所述相机外参和IMU的bias。

优选地，所述在线标定设备参数的视觉惯性里程计方法还包括提取新特征，所述提取新特征包括：

根据当前图像帧的图像和所述与当前图像帧匹配的二维特征点的坐标，提取当前图像帧新增的二维特征点坐标、描述子或者patch。

优选地，所述采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参，包括：

建立滑动窗口；

将所述滑动窗口中图像帧的二维特征点进行匹配并三角化；

采用非线性优化算法对所述滑动窗口中的图像帧进行优化；

输出优化后的当前图像帧的相机内参和特征点深度。

优选地，所述建立滑动窗口，包括：

设定所述滑动窗口的长度；

当判断当前图像帧为关键帧时，将当前图像帧存储在所述滑动窗口内，同时删除一帧旧的图像帧。

优选地，所述滑动窗口包括所述滑动窗口中与当前图像帧匹配的二维特征点的坐标和当前图像帧新增的二维特征点坐标、所述二维特征点的深度初始值、所述图像帧的位姿和相机内参。

优选地，所述采用非线性优化算法对所述滑动窗口中的图像帧进行优化，包括：

以所述滑动窗口中图像帧的所有二维特征点和对应的三维特征点的投影误差之和最小为优化目标，以相机内参和特征点深度作为优化量，输出优化后的相机内参和特征点深度。

本发明的另一个方面提供了一种在线标定设备参数的视觉惯性里程计系统，包括存储器和处理器，其中：

所述存储器，用于存储代码和相关数据；

所述处理器，用于调用所述存储器中的数据，执行所述存储器中的代码；执行所述存储器中的代码能实施前述在线标定设备参数的视觉惯性里程计的方法和步骤。

本发明的又一个方面提供了另一种在线标定设备参数的视觉惯性里程计系统，所述设备参数包括相机外参、IMU的bias和相机内参，所述视觉惯性里程计系统包括数据采集模块、相机外参和IMU的bias标定模块和相机内参标定模块，其中：

所述数据采集模块用于采集图像数据和IMU数据；

所述相机外参和IMU的bias标定模块用于采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias并计算所述图像数据中的图像帧的位姿；

所述相机内参标定模块，用于采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参。

优选地，所述相机外参和IMU的bias标定模块包括初始化模块、预测模块、匹配模块、更新模块和提取新特征模块，其中：

所述初始化模块对所述图像数据和IMU数据进行处理，输出初始化的点云、图像帧的位姿、二维特征点和二维特征点的描述子或patch；根据所述IMU数据初始化所述卡尔曼滤波器状态量，计算所述卡尔曼滤波器协方差矩阵、转换矩阵和卡尔曼增益；初始化所述相机外参、IMU的bias和相机内参；

所述预测模块根据与当前图像帧同时段的IMU和卡尔曼滤波器状态量、协方差矩阵，预测当前图像帧的卡尔曼滤波器状态量和协方差矩阵；

所述匹配模块将上一图像帧的二维特征点与当前图像帧的特征点进行匹配，剔除与当前图像帧不匹配的特征点；输出与当前图像帧匹配的二维特征点的坐标和投影误差；

所述更新模块根据所述投影误差、所述预测的当前卡尔曼滤波器状态量、转换矩阵，更新所述当前卡尔曼滤波器状态量和协方差矩阵，通过更新后的卡尔曼滤波器状态量得到所述相机外参和IMU的bias；

所述提取新特征模块根据当前图像帧的图像和所述与当前图像帧匹配的二维特征点的坐标，提取当前图像帧新增的二维特征点坐标、描述子或者patch。

优选地，所述相机内参标定模块包括建立滑动窗口模块、匹配并三角化模块、非线性优化模块，其中：

所述建立滑动窗口模块设定所述滑动窗口的长度，当判断当前图像帧为关键帧时，根据所述窗口长度存储所述图像帧；

所述匹配并三角化模块将所述滑动窗口中的图像帧的二维特征点进行匹配并三角化；

所述非线性优化模块采用非线性优化算法对所述滑动窗口中的图像帧进行优化，输出优化后的当前图像帧的相机内参和特征点深度。

本发明的在线标定设备参数的视觉惯性里程计方法和系统，利用设备所搭载的相机和惯性测量单元采集图像数据和IMU数据，不依赖设备和标定板标定参数，采用卡尔曼滤波器和非线性优化滑动窗口实时计算设备在当前环境中的位置和姿态信息；本发明利用卡尔曼滤波器标定所述相机外参和IMU的bias，保证了计算速度，利用非线性优化算法和滑动窗口标定所述相机内参，满足了精度要求；本发明的卡尔曼滤波器和非线性优化滑动窗口为紧耦合关系，更加鲁棒。本发明提供了一套完善的实时卡尔曼滤波器和非线性优化滑动窗口的视觉惯性里程计的算法流程，显著提高了视觉惯性里程计在不同环境和不同设备下的性能，本发明具有流程通用、针对性强、在线运行过程简单易用等优点。

附图说明

读者在参照附图阅读了本发明的具体实施方式以后，将会更加清楚地了解本发明的各个方面。其中，

图1为本发明的一个实施例的在线标定设备参数的视觉惯性里程计总体流程图；

图2为本发明的一个实施例的在线标定设备参数的视觉惯性里程计系统结构图；

图3为本发明的另一个实施例的在线标定设备参数的视觉惯性里程计系统框架图。

附图标记：

11：存储器；12：处理器；

21：数据采集模块；22：相机外参和IMU的bias标定模块；23：相机内参标定模块；

221：初始化模块；222：预测模块；223：匹配模块；224：更新模块；225：提取新特征模块；

231：建立滑动窗口模块；232：匹配并三角化模块；233：非线性优化模块。

具体实施方式

为了使本申请所揭示的技术内容更加详尽与完备，可参照附图以及本发明的下述具体实施例。然而，本技术领域的普通技术人员应当理解，下文中所提供的实施例并非用来限制本发明所涵盖的范围。此外，附图仅仅用于示意性地加以说明，并未依照其原尺寸进行绘制。

下面参照附图，对本发明各个方面的具体实施方式作进一步的详细描述。

实施例一：

本实施例提供了一种在线标定设备参数的视觉惯性里程计方法，所述设备参数包括相机外参、IMU的bias和相机内参，请参阅图1，所述方法包括：

S1：通过所述设备采集图像数据和IMU数据；具体地，所述设备通过配备的相机实时采集图像数据，通过配备的IMU实时采集IMU数据，所述IMU数据包括陀螺仪数据和加速度数据；

S2：采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias并计算所述图像数据中图像帧的位姿；具体地，所述相机外参包括IMU到相机的平移和旋转，所述IMU的bias包括加速度计的bias和陀螺仪的bias，由于IMU到相机的转换关系是一个刚体变换，预先标定后即可使用，而且产品线做工产生的每一台设备的误差都很小，因此只需要在预先的标定值附近进行轻微校正即可，同时所述IMU的bias比较小，也只需要进行轻微校正，因此，所述相机外参和IMU的bias对准确度要求不高，将所述相机外参和IMU的bias放入所述卡尔曼滤波器中进行优化，占用计算资源少，可以提升计算速度；

S3：采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参；具体地，所述相机内参包括相机的主点、焦距和畸变参数，由于消费级移动端设备大多为自动对焦，相机内参不是一个固定值，所以需要实时优化，又由于相机内参对VIO影响很大，所以需要精确的数据，特征点深度也需要非常精确的值，将相机内参和特征点深度放入滑动窗口，采用非线性优化可以获取准确的参数，满足对精度的要求。

因此可见，在本实施例中，将卡尔曼滤波器和非线性优化的滑动窗口结合的视觉惯性里程计方法兼顾了精度要求和计算速度的要求。

在本实施例中，每一帧图像和IMU都是先由卡尔曼滤波器计算图像帧的位姿，更新IMU的bias和IMU到相机的转换关系，所述滑动窗口再根据所述图像帧的位姿采用非线性优化算法计算图像的相机内参和特征点深度，由此可见，所述在线标定设备参数的视觉惯性里程计方法为紧耦合算法，相比现有技术的松耦合算法，本发明的紧耦合算法更加鲁棒。

在本实施例中，所述S2采用卡尔曼滤波器根据所述图像帧的位姿在线标定所述相机外参和IMU的bias，包括将所述相机外参和IMU的bias作为所述卡尔曼滤波器状态量，通过对所述卡尔曼滤波器状态量进行S22初始化、S23预测、S24匹配、S25更新，计算所述相机外参和IMU的bias；具体地，在所述S1通过所述设备采集图像和IMU数据后，进入S21判断所述图像和IMU数据是否初始化，如果所述图像和IMU数据已经初始化，则进入所述S23预测，如果所述图像和IMU数据没有初始化，则进入所述S22初始化，对所述图像和IMU数据初始化，完成所述S22初始化后，返回所述S21判断所述图像和IMU数据是否初始化；在完成所述S23预测后，进入所述S24匹配，完成所述S24匹配后，进入所述S25更新，完成所述S25更新后，输出所述相机外参和IMU的bias。

在本实施例中，所述卡尔曼滤波器的状态量包括r：IMU的位置、v：IMU的速度、q：IMU的姿态、b_f：加速度计的bias、b_w：陀螺仪的bias、c：IMU到相机的平移和z：IMU到相机的旋转，其中：

所述加速度计的bias、陀螺仪的bias为所述IMU的bias；

所述IMU到相机的平移和IMU到相机的旋转为所述相机外参。

在本实施例中，所述S22初始化包括：

对所述图像数据和IMU数据进行处理，输出初始化的点云、图像帧的位姿、二维特征点和二维特征点的描述子或patch；

根据所述IMU数据初始化所述卡尔曼滤波器状态量，计算所述卡尔曼滤波器协方差矩阵、转换矩阵和卡尔曼增益。具体地，所述IMU数据包括IMU的位置、IMU的速度、IMU的姿态。

初始化的相机外参、IMU的bias和相机内参，输出所述点云中特征点的位姿和深度；具体地，根据经验给出相机外参与相机内参不精确的初始值，例如bias为0、初始速度为0、位置为(0，0，0)、姿态为单位阵、重力大小为9.81等，然后通过连续的图像帧通过sfm(运动推断结构：Structure from Motion)计算点云和图像帧的位姿，再通过同时段的IMU恢复尺度，具体地，通过IMU计算相机位移长度，将所述相机位移长度与图像计算的相机位移长度对齐，获取尺度值，具体地，所述尺度值为轨迹尺度单位，根据所述轨迹尺度可以获取深度值。

在本实施例中，所述S23预测包括：

根据与当前图像帧同时段的IMU和卡尔曼滤波器状态量、协方差矩阵，预测当前图像帧的卡尔曼滤波器状态量和协方差矩阵。具体地，所述同时段IMU为从上一图像帧到当前图像帧之间的时间内的IMU数据(图像帧一般为30fps，IMU一般为200fps以上，因此两个图像帧之间有多个IMU帧)；

具体地，在所述S22初始化中，根据经验值设置所述协方差矩阵P、转换矩阵A和测量噪声Q的初始值，k时刻协方差矩阵由k-1时刻预测：

具体地，使用IMU数据(加速度和陀螺仪的测量值数据)，根据以下公式依次对所述卡尔曼滤波器的状态量进行预测，具体地，所述状态量为：r：IMU的位置、v：IMU的速度、q：IMU的姿态、b_f：加速度计的bias、b_w：陀螺仪的bias、c：IMU到相机的平移、z：IMU到相机的旋转。其中：

为加速度的原始测量值，

为陀螺仪的原始测量值，所述加速度的原始测量值和陀螺仪的原始测量值由设备采集作为输入值；

g为重力加速度，

为使用bias校正且受噪声影响的线速度测量值，

为使用bias校正且受噪声影响的角速度测量值；

上标·表示对应的预测后的状态量，形式为w_*的项为高斯白噪声过程，上标×表示向量的斜对称矩阵；

具体地，使用bias和白噪声得到所述使用bias校正且受噪声影响的加速度测量值和角速度测量值；

图像帧的线速度

和角速度

使用对应时刻的IMU(包含加速度计和陀螺仪)转换而来。

在本实施例中，所S24述匹配包括：

将上一图像帧的二维特征点与所述当前图像帧的特征点进行匹配，剔除与当前图像帧不匹配的特征点，输出与当前图像帧匹配的二维特征点的坐标和投影误差。

具体地，所述上一图像帧为当前图像帧在时序上的前一帧；具体地，根据所述卡尔曼滤波器状态量计算当前图像帧的位姿，由所述S22初始化输出所述上一图像帧的二维特征点坐标、描述子或者patch、深度值，根据所述上一图像帧的二维特征点坐标、描述子或者patch、深度值与所述当前图像帧的位姿，采用以下原则剔除与当前图像帧不匹配的特征点：

原则1：匹配的描述子或者patch相似度达不到预先设定的阈值；

原则2：所述上一图像帧的特征点投影不在当前图像帧范围内；

原则3：跟踪次数大于M次；具体地，所述图像帧的特征点在每个图像帧上跟踪一次就累加一次，得到所述跟踪次数，根据经验确定所述M的值；

具体地，将上一图像帧的三维特征点按照当前图像帧的预测位姿投影到当前帧图像坐标上，所述投影点坐标为(px1,py1)，在所述投影点(px1,py1)的附近做局部匹配，最佳匹配点坐标为(px2,py2)，其中(px2-px1,py2-py1)即为投影误差。

在本实施例中，所述S25更新包括：

根据所述投影误差、所述预测的当前图像帧的卡尔曼滤波器状态量、转换矩阵，更新所述当前图像帧的卡尔曼滤波器状态量和协方差矩阵，通过更新后的卡尔曼滤波器状态量得到所述相机外参和IMU的bias、位置，姿态，速度。

在本实施例中，所述在线标定设备参数的视觉惯性里程计方法还包括S26提取新特征，所述S26提取新特征根据当前图像帧的图像和与当前图像帧匹配的二维特征点的坐标，提取当前图像帧新增的二维特征点坐标、描述子或者patch。具体地，所述与当前图像帧匹配的二维特征点的坐标由所述S24匹配输出；具体地，对图像提取特征点，剔除已有的p个特征在图像坐标半径m个像素内的特征点，剩下的特征点按照特征显著度排序，保留n-p个新增特征点(特征点上限n个)，得到所述新增的二维特征点坐标；具体地，根据所述与当前图像帧匹配的二维特征点的坐标，避开所述与当前图像帧匹配的二维特征点，避免在已匹配的特征点上提取新增特征点。

在本实施例中，所述S3采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参，包括：

S31：建立滑动窗口；

S32：将所述滑动窗口中图像帧的二维特征点进行匹配并三角化；具体地，通过图像帧与图像帧匹配或者通过图像帧的二维特征点的光流或者描述子匹配；

S33：采用非线性优化算法对所述滑动窗口中的图像帧进行优化，输出优化后的当前图像帧的相机内参和特征点深度。

在本实施例中，所述S31建立滑动窗口，包括：

设定所述滑动窗口的长度；

当判断当前图像帧为关键帧时，将当前图像帧存储在所述滑动窗口内，同时删除一帧旧的图像帧，具体地，通过判断所述图像帧的旋转角度或平移距离是否达到预先设置的阈值和/或跟踪所述图像帧的特征点的数量判断所述图像帧是否为关键帧；

在本实施例中，所述滑动窗口包括所述滑动窗口中与当前图像帧匹配的二维特征点和当前图像帧新增的二维特征点的坐标、所述二维特征点的深度初始值、所述图像帧的位姿和相机内参。具体地，上一图像帧的相机内参作为当前图像帧的相机内参的初值，在所述S22初始化时，根据经验给第一帧图像帧的相机内参一个初值，所述初值不需要很精确。

在本实施例中，所述S33采用非线性优化算法对所述滑动窗口中的图像帧进行优化，包括：

以所述滑动窗口中图像帧的所有二维特征点和对应的三维特征点的投影误差之和最小为优化目标，以相机内参和特征点深度作为优化量，输出优化后的相机内参和特征点深度。

具体地，所述投影误差为三维特征点投影到到图像上的像素点坐标与二维特征点坐标的像素偏差；

具体地，所述滑动窗口中每一帧图像帧的位姿是固定的，因此只需优化相机内参和三维特征点的坐标，每一帧图像帧的二维特征点对应的三维特征点反投影到图像上的像素点坐标pxy2与二维特征点的坐标pxy1存在像素偏差δpxy＝abs(pxy1-pxy2)，所述非线性优化算法的约束为所有二维特征点对应的三维特征点的投影误差和Sump最小，具体地，Sump＝sum(δpxyi,其中i＝1.....j),j个有对应的三维特征点的二维特征点。

实施例二：

本实施例提供一种在线标定设备参数的视觉惯性里程计系统，请参阅图2，所述视觉惯性里程计系统包括存储器11和处理器12，其中：

所述存储器11，用于存储代码和相关数据；

所述处理器12，用于调用所述存储器11中的数据，执行所述存储器11中的代码；执行所述存储器11中的代码能实施前述在线标定设备参数的视觉惯性里程计的方法和步骤，具体方法和步骤参照前述实施例的描述，在此不再赘述。

在本实施例中，所述存储器11可能包含易失性存储器，例如，随机存取存储器(random access memory，RAM)，所述RAM可以包括静态RAM或动态RAM。所述存储器11也可能包括非易失性存储器(non-volatile memory)，例如只读存储器(read-only memory，PROM)、可编程只读存储器(programmable read-only memory，PROM)、可擦写可编程只读存储器(erasable programmable read-only memory，EPROM)、电可擦写可编程只读存储器(electrically erasable programmable read-only memory，EEPROM)或闪存(flashmemory)。所述存储器11还可能是外部闪存、至少一个磁盘存储器或缓存器。

在本实施例中，所述处理器12可以是中央处理单元(Central Processing Unit,CPU)，所述处理器12还可以是其他通用控制处理器、数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。所述通用控制处理器可以是微控制处理器或者是任何常规的控制处理器，例如单片机等。

实施例三：

本实施例提供了另一种在线标定设备参数的视觉惯性里程计系统，所述设备参数包括相机外参、IMU的bias和相机内参，请参阅图3，所述视觉惯性里程计系统包括数据采集模块21、相机外参和IMU的bias标定模块22和相机内参标定模块23，其中：

所述数据采集模块21用于采集图像数据和IMU数据；具体地，所述设备通过配备的相机实时采集图像数据，通过配备的IMU实时采集IMU数据；

所述相机外参和IMU的bias标定模块22用于采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias并计算所述图像数据中的图像帧的位姿；

所述相机内参标定模块23采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参。

由此可见，本实施例的在线标定设备参数的视觉惯性里程计系统为紧耦合系统，相比现有技术的松耦合系统，本发明的紧耦合系统更加鲁棒。

在本实施例中，所述相机外参和IMU的bias标定模块22包括初始化模块221、预测模块222、匹配模块223、更新模块224、提取新特征模块225，其中：

所述初始化模块221对所述图像数据和IMU数据进行处理，输出初始化的点云、图像帧的位姿、二维特征点和二维特征点的描述子或patch、初始化的相机外参、IMU的bias和相机内参；初始化所述卡尔曼滤波器状态量，计算所述卡尔曼滤波器协方差矩阵、转换矩阵和卡尔曼增益；具体地，当所述视觉惯性里程计系统启动时，处于未初始化状态，在接受到所述数据采集模块21采集到的图像和IMU数据后，首先将所述图像和IMU数据送到所述初始化模块221进行系统的初始化，当初始化完成后，后续系统接收到的所述数据采集模块21输入的图像和IMU数据将被发送到所述预测模块222，如果初始化未完成，则下一次所述数据采集模块21的输入继续重复所述初始化过程。

所述预测模块222根据与当前图像帧同时段的IMU和卡尔曼滤波器状态量、协方差矩阵，预测当前图像帧的卡尔曼滤波器状态量和协方差矩阵；

所述匹配模块223根据所述卡尔曼滤波器状态量计算当前图像帧的位姿；根据上一图像帧的二维特征点坐标、描述子或者patch、深度值与所述当前图像帧的位姿，剔除与当前图像帧不匹配的特征点；输出与当前图像帧匹配的二维特征点的坐标和投影误差；

所述更新模块224根据所述投影误差、所述预测的当前卡尔曼滤波器状态量、转换矩阵，更新所述当前卡尔曼滤波器状态量和协方差矩阵，通过更新后的卡尔曼滤波器状态量得到所述相机外参和IMU的bias；

所述提取新特征模块225根据当前图像帧的图像和与当前图像帧匹配的二维特征点的坐标，提取当前图像帧新增的二维特征点坐标、描述子或者patch。

在本实施例中，所述相机内参标定模块23包括建立滑动窗口模块231、匹配并三角化模块232、非线性优化模块233，其中：

所述建立滑动窗口模块231设定所述滑动窗口的长度，当判断当前图像帧为关键帧时，根据所述窗口长度存储所述图像帧；

所述匹配并三角化模块232将所述滑动窗口中图像帧的二维特征点进行匹配并三角化；

所述非线性优化模块233采用非线性优化算法对所述滑动窗口中的图像帧进行优化，输出优化后的当前图像帧的相机内参和特征点深度。

本发明的在线标定设备参数的视觉惯性里程计方法和系统，利用设备所搭载的相机和惯性测量单元采集图像数据和IMU数据，不依赖设备和标定板标定参数，采用卡尔曼滤波器和非线性优化滑动窗口实时计算设备在当前环境中的位置和姿态信息；本发明利用卡尔曼滤波器标定所述相机外参和IMU的bias，保证了计算速度，利用非线性优化和滑动窗口标定所述相机内参，满足了精度要求。本发明的卡尔曼滤波器和非线性优化滑动窗口为紧耦合关系，更加鲁棒。本发明提供一套完善的实时卡尔曼滤波和非线性优化滑动窗口的视觉惯性里程计的算法流程，显著提高了视觉惯性里程计在不同环境和不同设备下的性能，本发明具有流程通用、针对性强、在线运行过程简单易用等优点。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所做的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员而言，在上述说明的基础上还可以做出其他不同形式的变化或变动，本发明所例举的实施例无法对所有的实施方式予以穷尽，凡是属于本发明的技术方案所引申出的显而易见的变化或变动仍处于本发明的保护范围之列。在本发明中提及的所有文献都在本申请中引用作为参考，就如同一篇文献被单独引用为参考那样。

Claims (16)

1.一种在线标定设备参数的视觉惯性里程计方法，所述设备参数包括相机外参、IMU的bias和相机内参，其特征在于，所述方法包括：

通过所述设备采集图像数据和IMU数据；

采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias并计算所述图像数据中图像帧的位姿；

采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参。

2.根据权利要求1所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias，包括：

将所述相机外参和IMU的bias作为所述卡尔曼滤波器状态量，通过对所述卡尔曼滤波器状态量进行初始化、预测、匹配、更新，计算所述相机外参和IMU的bias。

3.根据权利要求2所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述卡尔曼滤波器的状态量包括IMU的位置、IMU的速度、IMU的姿态、加速度计的bias、陀螺仪的bias、IMU到相机的平移和IMU到相机的旋转，其中：

所述加速度计的bias、陀螺仪的bias为所述IMU的bias；

所述IMU到相机的平移和IMU到相机的旋转为所述相机外参。

4.根据权利要求3所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述初始化包括：

对所述图像数据和IMU数据进行处理，输出初始化的点云、图像帧的位姿、二维特征点和二维特征点的描述子或patch；

初始化所述卡尔曼滤波器状态量，计算所述卡尔曼滤波器的协方差矩阵、转换矩阵和卡尔曼增益；

初始化所述相机外参、IMU的bias和相机内参。

5.根据权利要求4所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述预测包括：

根据与当前图像帧同时段的IMU和卡尔曼滤波器状态量、协方差矩阵，预测当前图像帧的卡尔曼滤波器状态量和协方差矩阵。

6.根据权利要求5所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述匹配包括：

将上一图像帧的二维特征点与所述当前图像帧的特征点进行匹配，剔除与当前图像帧不匹配的特征点；

输出与当前图像帧匹配的二维特征点的坐标和投影误差。

7.根据权利要求6所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述更新包括：

根据所述投影误差、所述预测的当前图像帧的卡尔曼滤波器状态量、转换矩阵，更新所述当前图像帧的卡尔曼滤波器状态量和协方差矩阵，通过更新后的卡尔曼滤波器状态量得到所述相机外参和IMU的bias。

8.根据权利要求7所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述在线标定设备参数的视觉惯性里程计方法还包括提取新特征，所述提取新特征包括：

根据当前图像帧的图像和所述与当前图像帧匹配的二维特征点的坐标，提取当前图像帧新增的二维特征点坐标、描述子或者patch。

9.根据权利要求1所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参，包括：

建立滑动窗口；

将所述滑动窗口中图像帧的二维特征点进行匹配并三角化；

采用非线性优化算法对所述滑动窗口中的图像帧进行优化；

输出优化后的当前图像帧的相机内参和特征点深度。

10.根据权利要求1所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述建立滑动窗口，包括：

设定所述滑动窗口的长度；

当判断当前图像帧为关键帧时，将当前图像帧存储在所述滑动窗口内，同时删除一帧旧的图像帧。

11.根据权利要求9所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述滑动窗口包括所述滑动窗口中与当前图像帧匹配的二维特征点和当前图像帧新增的二维特征点坐标、所述二维特征点的深度初始值、所述图像帧的位姿和相机内参。

12.根据权利要求9所述的在线标定设备参数的视觉惯性里程计方法，其特征在于，所述采用非线性优化算法对所述滑动窗口中的图像帧进行优化，包括：

以所述滑动窗口中图像帧的所有二维特征点和对应的三维特征点的投影误差之和最小为优化目标，以相机内参和特征点深度作为优化量，输出优化后的相机内参和特征点深度。

13.一种在线标定设备参数的视觉惯性里程计系统，其特征在于，包括存储器和处理器，其中：

所述存储器，用于存储代码和相关数据；

所述处理器，用于调用所述存储器中的数据，执行所述存储器中的代码；执行所述存储器中的代码能实施如权利要求1至12任一项所述的方法和步骤。

14.一种在线标定设备参数的视觉惯性里程计系统，所述设备参数包括相机外参、IMU的bias和相机内参，其特征在于，所述视觉惯性里程计系统包括数据采集模块、相机外参和IMU的bias标定模块和相机内参标定模块，其中：

所述数据采集模块用于采集图像数据和IMU数据；

所述相机外参和IMU的bias标定模块用于采用卡尔曼滤波器根据所述图像数据和IMU数据在线标定所述相机外参和IMU的bias并计算所述图像数据中的图像帧的位姿；

所述相机内参标定模块，采用非线性优化算法和滑动窗口根据所述图像帧的位姿在线标定所述相机内参。

15.根据权利要求14所述的在线标定设备参数的视觉惯性里程计系统，所述相机外参和IMU的bias标定模块包括初始化模块、预测模块、匹配模块、更新模块和提取新特征模块，其中：

所述初始化模块对所述图像数据和IMU数据进行处理，输出初始化的点云、图像帧的位姿、二维特征点和二维特征点的描述子或patch；初始化所述相机外参、IMU的bias和相机内参；初始化所述卡尔曼滤波器状态量，计算所述卡尔曼滤波器协方差矩阵、转换矩阵和卡尔曼增益；

所述预测模块根据与当前图像帧同时段的IMU和卡尔曼滤波器状态量、协方差矩阵，预测当前图像帧的卡尔曼滤波器状态量和协方差矩阵；

所述匹配模块将上一图像帧的二维特征点与当前图像帧的特征点进行匹配，剔除与当前图像帧不匹配的特征点，输出与当前图像帧匹配的二维特征点的坐标和投影误差；

所述更新模块根据所述投影误差、所述预测的当前卡尔曼滤波器状态量、转换矩阵，更新所述当前卡尔曼滤波器状态量和协方差矩阵，通过更新后的卡尔曼滤波器状态量得到所述相机外参和IMU的bias；

所述提取新特征模块根据当前图像帧的图像和所述与当前图像帧匹配的二维特征点的坐标，提取当前图像帧新增的二维特征点坐标、描述子或者patch。

16.根据权利要求14所述的在线标定设备参数的视觉惯性里程计系统，其特征在于，所述相机内参标定模块包括建立滑动窗口模块、匹配并三角化模块、非线性优化模块，其中：

所述建立滑动窗口模块设定所述滑动窗口的长度，当判断当前图像帧为关键帧时，根据所述窗口长度存储所述图像帧；

所述匹配并三角化模块将所述滑动窗口中图像帧的二维特征点进行匹配并三角化；

所述非线性优化模块采用非线性优化算法对所述滑动窗口中的图像帧进行优化，输出优化后的当前图像帧的相机内参和特征点深度。

Images