CN112749665A - 一种基于图像边缘特征的视觉惯性slam方法 - Google Patents

Abstract

本发明公开了一种基于图像边缘特征的视觉惯性SLAM方法，首先获取经过时间同步的IMU测量数据和相机图像；其次利用Canny算法对相机图像进行边缘特征提取及跟踪，同时对IMU测量数据进行预积分，构建误差传递方程；然后进行视觉惯性联合初始化；再者通过采用具有边缘化策略的滑动窗口进行局部视觉惯性非线性优化及建图；随后判断当前帧是否是关键帧，并构建关键帧序列；最后基于随机蕨词袋进行回环检测，将回环信息加入到滑动窗口进行全局视觉惯性优化及建图。本发明的优势在于，图像边缘具有更多的环境信息，具有更高的精度和信噪比，同时对光照变化的抗干扰能力也较强；融合了视觉和惯性信息，可以在长时间实时定位与建图中保持较高的精度。

Description

一种基于图像边缘特征的视觉惯性SLAM方法

技术领域

本发明涉及SLAM技术领域，具体来说涉及一种基于视觉惯性的SLAM方法，更具体地说，涉及一种基于图像边缘特征的视觉惯性SLAM方法。

背景技术

即时定位与建图(Simultaneous Localization and Mapping,SLAM)是当前机器人领域不可或缺的技术，其主要作用是在机器人在未知环境中根据环境信息进行定位，并在定位的基础上实时构建具有全局一致性的地图。用于获取环境信息的传感器多种多样，包括激光雷达、UWB、GPS、视觉传感器等等。视觉传感器由于价格低廉和包含更多环境信息，且与人类的定位相仿，在近年来在工业界和学术界引起较多关注，视觉SLAM因此成为最近的研究热点。

SLAM的基本模块包括前端里程计、后端优化、闭环检测、全局优化四个步骤。视觉SLAM(VSLAM)根据前端里程计采用的方法可以分为基于点线面等特征描述子的特征法和基于原始图像像素的直接法。基于特征的非直接法的VSLAM在低纹理的环境下表现不佳，并且检测子和描述子耗费较长时间，在建图环节，特征点一般具有稀疏的属性，不能很好地代表整体图像，无法充分反映环境信息。基于直接法的VSLAM一般基于图像灰度不变这一强假设，因此受光照和运动模糊影响较大，容易在这些情况下出现失效的情况。

除事件相机外的单目相机、RGB-D相机、结构光相机等视觉传感器在使用过程中都无可避免的存在受运动模糊、光照等外界条件的影响。因此在实际应用中，视觉传感器通常与其他传感器进行多传感器信息融合来提高系统鲁棒性。惯性测量单元(IMU)能够测量载体的各向加速度和角速度，能够感受自身的运动，但其中的陀螺仪存在随时间的漂移等无法避免的问题，而视觉传感器几乎不存在漂移，但无法感知自身的运动，两者因此被认为具有天然的互补性，视觉惯性SLAM也因此成为机器人领域的实际应用和学术研究的重点研究方向。

发明内容

本发明的目的在于克服上述现有技术的不足，提出了一种基于图像边缘特征的视觉惯性SLAM方法。

为了实现上述目的，本发明的目的通过以下技术方案实现：

一种基于图像边缘特征的视觉惯性SLAM方法，具体包括以下步骤：

S1获取经过时间同步的IMU测量数据和相机图像；

S2对上述相机图像进行边缘特征提取及跟踪；

S3对上述IMU测量数据进行预积分，构建误差传递方程，计算协方差及其一阶Jacobian；

S4进行视觉惯性联合初始化；

S5将上述视觉惯性信息放置入优化滑动窗口，进行局部视觉惯性非线性优化及建图；

S6对所述当前帧是否是关键帧，并构建关键帧序列；

S7比较所述关键帧序列中的最新关键帧与其他关键帧的相似度，基于随机蕨词袋进行回环检测，将回环信息加入到滑动窗口进行全局视觉惯性优化及建图。

进一步的，所述步骤S1还具体包括以下过程：

S1.1利用硬件连接进行多传感器数据时间同步，通过使用具有外部触发快门功能的相机和具有外部触发输出信号功能的IMU来实现，也可以利用微处理的外部中断来实现两种传感器的时间同步；

S1.2为便于后续的数据处理，在机器人操作系统ROS平台上利用消息过滤器进行传感器采集信息时间戳的匹配，可以有效减少由于时间戳不匹配引入的误差。

进一步的，所述步骤S2还包括以下过程：

S2.1将当前获取的图像作为当前帧，使用Canny边缘特征检测算法提取边缘特征，输出边缘像素相关的参数；

S2.2通过计算当前帧与离当前帧最近的关键帧之间的相对运动来实现跟踪，更具体的，通过对齐两帧的边缘特征来实现两帧之间的相对运动的估计，对齐则是将当前帧的边缘特征的像素重投影至另一帧，然后使用距离变换(DT)预计算每个像素到另一帧中最近的边缘的欧氏距离，以此构建残差方程进行非线性优化进行运动估计,从而实现边缘特征的跟踪；

S2.3为加速计算，进一步优化上述距离变换的方式，采用从粗糙到精细的策略，使用迭代权重再分配的非线性优化方法。更具体的，在图像的尺度金字塔内，只显式地计算最高分辨率的一层的距离变换，其他层次的距离变换则通过相机内参从最高层投影到该层来计算，可以在不降低算法的鲁棒性的同时显著提高算法的运行速度。

进一步的，所述步骤S3还包括以下过程：

S3.1对IMU获得的测量值在两帧图像之间的时间间隔对时间积分，并使用数值积分的方法对预积分项进行求解；

S3.2根据上述预积分项构建误差状态传递方程；

S3.3以误差状态传递方程为基础，计算协方差更新方程及其一阶Jacobian；

S3.4进行偏置修正，当IMU的偏置发生细微的变化时，通过误差状态传递一阶近似方程来进行修正，当IMU的偏置发生明显的变化时，则在新的偏置重新计算误差传递，可以有效减少计算Jacobian耗费的时间。

进一步的，所述步骤S4还包括以下过程：

S4.1进行纯视觉运动恢复结构(SfM),利用S2中所述的两帧图像之间的边缘特征像素估计相对运动，计算相对位移和旋转；

S4.2进行视觉惯性配准，进一步的，首先通过连续两帧的运动恢复结构得到的位姿与IMU预积分构建残差方程，对陀螺仪的偏置进行估计；然后进行速度、重力矢量和度量尺度初始化，最后进行重力再优化，为后续的姿态估计提供了较好的初始姿态估计。

进一步的，所述步骤S5还包括以下过程：

S5.1将当前帧及其对应的惯性信息放入优化窗口，构建包含视觉重投影误差和惯性残差方程的联合残差方程，以帧间相对位姿为优化目标，结合Huber鲁棒核函数与联合残差方程构建优化目标函数，利用非线性优化方法求解得到优化后的位姿；

S5.2根据优化后的位姿，更新边缘特征的3D信息，以此建立局部地图；

S5.3为了在优化方面不耗费过多算力，采用边缘化策略使得滑动窗口内的优化变量数量维持在一个适当的水平，具体的，将较早的变量和与当前帧无联系的数据帧移除，在保持滑动窗口内信息可以有效求解位姿的同时，保证了Hessian矩阵的稀疏性。

进一步的，所述步骤6还包括以下过程：

S6.1根据时间间隔判断，关键帧时间间隔较长可能使得帧间的惯性误差较大，因此如果当前帧与上一关键帧的时间间隔超过设定的阈值时，便将当前帧设置为关键帧；

S6.2根据图像中的信息变化，条件一，计算衡量视场内变化的平均平方光流以及用以衡量阻塞的无旋转的平均平方光流，以两者之和大于设定阈值为当前帧增加为关键帧，否则不增加，条件二，边缘重投影数目新增和减少的量大于设定的阈值，根据不同情况选择新增关键帧，使得滑动窗口内的可以维持在一定数量，从而高效地实现位姿估计。

进一步的，所述步骤7还包括以下过程：

S7.1在当前帧下采样的随机图像块中计算Fern描述子；

S7.2计算当前帧的Fern描述子与已经保存的关键帧的Fern描述子的Hamming距离，以此作为两帧之间的相似度，从而以此筛选回环候选帧；

S7.3检测到回环后，估计回环候选帧与当前帧之间的位姿，以视觉信息和惯性信息构建位姿图优化，

相对于现有技术，本发明具有如下优点：

首先，本发明采用的图像中的边缘特征相较于特征点可以包含更多的环境信息，在低纹理环境下依然具有较为丰富的边缘特征，可以进行更为稠密的建图，图像中的边缘具有更高的精度和信噪比，相较于直接法，对光照变化的抗干扰能力也较强；因此，本发明的视觉里程计部分可以在低纹理，光照变化较为明显的环境下正常工作。

其次，本发明在利用视觉传感器图像检出边缘特征的基础上，结合了惯性传感器可以感知自身载体运动的优点，在进行定位时有效地融合了视觉和惯性信息，可以在长时间实时定位与建图中保持较高的精度。

再者，本发明设计了较为合理的关键帧选择策略和基于Fern的闭环检测和验证环节，可以有效地保持后端优化的高效运行，并通过回环检测环节有效地减少系统估计位姿的累积误差。

附图说明

图1为本发明的系统框图；

图2为本发明进行回环检测的流程图。

具体实施方式

为了使本发明的目的，技术方案及优点更加清楚明白，以下结合附图通过具体实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施案例仅用于解释本发明，并不用于限定本发明。

本发明提供了一种基于图像边缘特征的视觉惯性SLAM方法，实现流程图如图1所示，其流程主要包括以下步骤：

步骤S1获取经过时间同步的IMU测量数据和相机图像，还具体包括以下过程：

S1.1利用硬件连接进行多传感器数据时间同步，通过使用具有外部触发快门功能的相机和具有外部触发输出信号功能的IMU来实现，也可以利用微处理的外部中断来实现两种传感器的时间同步。

相机的种类繁多，有单目相机、双目相机、RGB-D相机、事件相机等诸多门类。优选的，通过内部集成有IMU的RGB-D相机获取周围环境的RGB图像与深度序列图像。

优选的，定义RGB-D相机在时间t的输出的图像包含RGB图像I_t和深度图Z_t，并且深度图和RGB图像已经经过对准和同步，即对于RGB图像中的每个像素都有深度图中对应的深度信息。即对于I_t中的像素p＝(x,y)，其对应的深度为Z＝Z_t(p).

对于世界坐标系下的3D点P以及像素坐标系下的像素p，定义从世界坐标系到像素坐标系的投影函数如下：

定义从像素坐标系下投影到世界坐标系的逆投影函数如下：

优选的，在李群上定义图像帧F_i和F_j的旋转和平移及其对应的李代数。

对应的李代数为ξ_ji.

由此定义计算将第i帧中的像素p_i重投影至第j帧的翘曲函数如下：

p'＝τ(ξ_ji,p_i,Z_i(p))＝π(T_jiπ^-1(p_i,Z_i(p_i))).

S1.2为便于后续的数据处理，在机器人操作系统ROS平台上利用消息过滤器进行传感器采集信息时间戳的匹配。

相机的数据的输出频率一般在30Hz左右，IMU的数据输出频率一般在100Hz左右，甚至达到上千赫兹，硬件时间同步使得数据具有大致相同的时钟，但为了便于后续的处理在ROS系统将数据再进行对齐，本实施例中，通过搜索距离图像帧最近的IMU数据作为该数据帧的惯性信息。

步骤S2对上述相机图像进行边缘特征提取及跟踪，还包括以下过程：

S2.1将当前获取的图像作为当前帧，使用Canny边缘特征检测算法提取边缘特征，输出边缘像素相关的参数；

S2.2通过计算当前帧与离当前帧最近的关键帧之间的相对运动来实现跟踪，更具体的，通过对齐两帧的边缘特征来实现两帧之间的相对运动的估计，对齐则是将当前帧的边缘特征的像素重投影至另一帧，然后使用距离变换(DT)预计算每个像素到另一帧中最近的边缘的欧氏距离，以此构建残差方程进行非线性优化进行运动估计,从而实现边缘特征的跟踪；

优选的，定义重投影的边缘距离误差方程如下：

E_pi＝D_j(τ(ξ_ji,p_i,Z_i(p_i))).

其中，D_j记为帧F_j和帧F_i中的边缘像素位置p_i之间的距离变换。

定义非线性优化模型如下：

其中，ε_i为具有有效深度信息的边缘像素点集合，δ_H为Huber权重函数。

优选的，使用迭代权重再分配的Levenberg-Marquardt方法对上述非线性优化方程进行求解。

S2.3为加速计算，进一步优化上述距离变换的方式，采用从粗糙到精细的策略，使用迭代权重再分配的非线性优化方法。更具体的，在图像的尺度金字塔内，只显式地计算最高分辨率的一层的距离变换，其他层次的距离变换则通过相机内参从最高层投影到该层来计算。

优选的，定义以下递推公式计算尺度金字塔内相邻层次的距离变换：

其中，N_p为上一更高精度层次的图像块大小。

步骤S3对上述IMU测量数据进行预积分，构建误差传递方程，计算协方差及其一阶Jacobian，还包括以下过程：

S3.1对IMU获得的测量值在两帧图像之间的时间间隔对时间积分，并使用数值积分的方法对预积分项进行求解；

优选的，对于连续的两个图像帧b_k和b_k+1，在其时间间隔[t_k,t_k+1]内具有若干的惯性测量数据，在IMU的偏置估计已知的情况下，在时域上积分如下：

其中，

对于离散时间上的积分，需要利用数值积分的方法进行求解，优选的，使用Euler法进行求解。

由于四维的旋转矢量

是过参数的，其误差项可以用其平均值的一个扰动来表示。

S3.2根据上述预积分项构建误差状态传递方程；

根据预积分项推导其动态误差项如下：

S3.3以误差状态传递方程为基础，计算协方差更新方程及其一阶Jacobian；

对于零阶保持离散化，F_t在积分的阶段是恒定的，因此对于给定的时间步长δt，有

F_d＝exp(F_tδt)，做泰勒级数展开忽略高次项得到F_d≈I+F_tδt。

定义连续时间的噪声协方差矩阵

则离散时间的噪声协方差矩阵为：

由此得到协方差更新方程：

同时一阶Jacobian矩阵通过初始的Jacobian递推得到

J_t+δt＝(I+F_tδt)J_t,t∈[k,k+1].

S3.4进行偏置修正，当IMU的偏置发生细微的变化时，通过误差状态传递一阶近似方程来进行修正，当IMU的偏置发生明显的变化时，则在新的偏置下重新计算误差传递。

在上述过程的基础上，得到预积分项的一阶近似方程：

步骤S4进行视觉惯性联合初始化，还包括以下过程：

S4.1进行纯视觉运动恢复结构(SfM),利用S2中所述的两帧图像之间的边缘特征像素估计相对运动，计算相对位移和旋转；

优选的，对于现有窗口内的所有帧，利用视觉信息求解得到位姿。假设有连续的两帧b_k和b_k+1，其对应的位姿四元数分别为

和

S4.2进行视觉惯性配准，进一步的，首先通过连续两帧的运动恢复结构得到的位姿与IMU预积分构建残差方程，对陀螺仪的偏置进行估计；然后进行速度、重力矢量优化。

优选的，结合陀螺仪预积分项构建以陀螺仪偏置为优化目标的优化方程如下：

其中

由此得到陀螺仪偏置的初始校正值，使用该值更新所有的预积分项。

优选的，对于在第k帧的载体系速度

为在初始帧的重力加速度，得到要初始化的状态量为：

对于两个连续帧有以下方程：

以此构建量测方程：

通过求解以下最小二乘问题可以得到初始相机坐标系下的重力加速度和载体系的速度。

步骤S5将上述视觉惯性信息放置入优化滑动窗口，进行局部视觉惯性非线性优化及建图包括以下过程：

S5.1将当前帧及其对应的惯性信息放入优化窗口，构建包含视觉重投影误差和惯性残差方程的联合残差方程，以帧间相对位姿为优化目标，结合Huber鲁棒核函数与联合残差方程构建优化目标函数，利用非线性优化方法求解得到优化后的位姿。

优选的，选择优化变量如下：

x_k为IMU的状态变量，

为相机的姿态。

优选的，结合先验信息以及观测量残差的Mahalanobis范数，以及Huber核函数得到以下最大后验估计：

(r_p,H_p)为先验信息，r_B和r_C分别为惯性残差函数和视觉信息残差函数。

优选的，在编程中使用Ceres非线性优化库对上述问题进行求解。

S5.2根据优化后的位姿，更新边缘特征的3D信息，以此建立局部地图。

S5.3为了在优化方面不耗费过多算力，采用边缘化策略使得滑动窗口内的优化变量数量维持在一个适当的水平，具体的，将较早的变量和与当前帧无联系的数据帧移除。

步骤S6对所述当前帧是否是关键帧，并构建关键帧序列，还包括以下过程：

S6.1根据时间间隔判断，关键帧时间间隔较长可能使得帧间的惯性误差较大，因此如果当前帧与上一关键帧的时间间隔超过设定的阈值时，便将当前帧设置为关键帧；

优选的，设置当前帧与上一关键帧的时间间隔大于0.5秒，则将当前帧作为关键帧。

S6.2根据图像中的信息变化，条件一，计算衡量视场内变化的平均平方光流以及用以衡量阻塞的无旋转的平均平方光流，以两者之和大于设定阈值为当前帧增加为关键帧，否则不增加，条件二，边缘重投影数目新增和减少的量大于设定的阈值。

定义平均平方光流C_fov，无旋转的平均平方光流C_occ如下：

记边缘重投影数目的新增数量为N_in，减少的数量为N_out。

优选的，上述值符合以下条件之一时，将当前帧作为关键帧。

C_fov+C_occ＞1或N_in＜2N_out。

步骤S7比较所述关键帧序列中的最新关键帧与其他关键帧的相似度，基于随机蕨词袋进行回环检测，将回环信息加入到滑动窗口进行全局视觉惯性优化及建图，还包括以下过程：

S7.1在当前帧下采样的随机图像块中计算Fern描述子；

定义一个蕨如下所示：

连续的节点f_i为通过(θ_i,τ_i)参数化的二进制数，

τ是图像的像素值I(θ)，

由此得到一帧图像的二进制词向量b_F＝[f₁,f₂,f₃,...f_n]∈Bⁿ。

当定义有多个蕨时：

则对应的有全局的词向量

S7.2计算当前帧的Fern描述子与已经保存的关键帧的Fern描述子的Hamming距离，以此作为两帧之间的相似度，从而以此筛选回环候选帧；

定义Fern描述子的Hamming距离计算公式如下：

S7.3检测到回环后，估计回环候选帧与当前帧之间的位姿，将回环信息第v帧的旋转

和平移

加入到位姿图中；得到如下所示的非线性代价函数：

对上述非线性代价函数进行优化得到全局优化后的位姿。

需要说明的是上述实施例仅仅是本发明的较佳实施例，并没有用来限定本发明的保护范围，在上述技术方案的基础上做出的等同替换或者替代均属于本发明的保护范围。

Claims (8)

1.一种基于图像边缘特征的视觉惯性SLAM方法，其特征在于，包括以下步骤：

S1、获取经过时间同步的IMU测量数据和相机图像；

S2、对上述相机图像进行边缘特征提取及跟踪；

S3、对上述IMU测量数据进行预积分，构建误差传递方程，计算协方差及其一阶Jacobian；

S4、进行视觉惯性联合初始化；

S5、将上述视觉惯性信息放置入优化滑动窗口，进行局部视觉惯性非线性优化及建图；

S6、对所述当前帧是否是关键帧，并构建关键帧序列；

S7、比较所述关键帧序列中的最新关键帧与其他关键帧的相似度，基于随机蕨词袋进行回环检测，将回环信息加入到滑动窗口进行全局视觉惯性优化及建图。

2.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，其特征在于，所述步骤S1具体包括以下过程：

S1.1利用硬件连接进行多传感器数据时间同步，通过使用具有外部触发快门功能的相机和具有外部触发输出信号功能的IMU来实现；

S1.2为便于后续的数据处理，在机器人操作系统ROS平台上利用消息过滤器进行传感器采集信息时间戳的匹配。

3.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，其特征在于，所述步骤S2包括以下过程：

S2.1将当前获取的图像作为当前帧，使用Canny边缘特征检测算法提取边缘特征，输出边缘像素相关的参数；

S2.2通过计算当前帧与离当前帧最近的关键帧之间的相对运动来实现跟踪，通过对齐两帧的边缘特征来实现两帧之间的相对运动的估计，对齐则是将当前帧的边缘特征的像素重投影至另一帧，然后使用距离变换(DT)预计算每个像素到另一帧中最近的边缘的欧氏距离，以此构建残差方程进行非线性优化进行运动估计,从而实现边缘特征的跟踪；

S2.3为加速计算，进一步优化上述距离变换的方式，采用从粗糙到精细的策略，使用迭代权重再分配的非线性优化方法，在图像的尺度金字塔内，只显式地计算最高分辨率的一层的距离变换，其他层次的距离变换则通过相机内参从最高层投影到该层来计算。

4.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，所述步骤S3包括以下过程：

S3.1对IMU获得的测量值在两帧图像之间的时间间隔对时间积分，并使用数值积分的方法对预积分项进行求解；

S3.2根据上述预积分项构建误差状态传递方程；

S3.3以误差状态传递方程为基础，计算协方差更新方程及其一阶Jacobian；

S3.4进行偏置修正，当IMU的偏置发生细微的变化时，通过误差状态传递一阶近似方程来进行修正，当IMU的偏置发生明显的变化时，则在新的偏置重新计算误差传递。

5.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，所述步骤S4包括以下过程：

S4.1进行纯视觉运动恢复结构(SfM),利用S2中所述的两帧图像之间的边缘特征像素估计相对运动，计算相对位移和旋转；

S4.2进行视觉惯性配准，首先通过连续两帧的运动恢复结构得到的位姿与IMU预积分构建残差方程，对陀螺仪的偏置进行估计；然后进行速度、重力矢量和度量尺度初始化，最后进行重力再优化。

6.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，所述步骤S5包括以下过程：

S5.1将当前帧及其对应的惯性信息放入优化窗口，构建包含视觉重投影误差和惯性残差方程的联合残差方程，以帧间相对位姿为优化目标，结合Huber鲁棒核函数与联合残差方程构建优化目标函数，利用非线性优化方法求解得到优化后的位姿；

S5.2根据优化后的位姿，更新边缘特征的3D信息，以此建立局部地图；

S5.3为了在优化方面不耗费过多算力，采用边缘化策略使得滑动窗口内的优化变量数量维持在一个适当的水平，将较早的变量和与当前帧无联系的数据帧移除。

7.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，所述步骤6还包括以下过程：

S6.1根据时间间隔判断，关键帧时间间隔较长可能使得帧间的惯性误差较大，因此如果当前帧与上一关键帧的时间间隔超过设定的阈值时，便将当前帧设置为关键帧；

S6.2根据图像中的信息变化，条件一，计算衡量视场内变化的平均平方光流以及用以衡量阻塞的无旋转的平均平方光流，以两者之和大于设定阈值为当前帧增加为关键帧，否则不增加，条件二，边缘重投影数目新增和减少的量大于设定的阈值。

8.根据权利要求1所述的一种基于图像边缘特征的视觉惯性SLAM方法，所述步骤7包括以下过程：

S7.1在当前帧下采样的随机图像块中计算Fern描述子；

S7.2计算当前帧的Fern描述子与已经保存的关键帧的Fern描述子的Hamming距离，以此作为两帧之间的相似度，从而以此筛选回环候选帧；

S7.3检测到回环后，估计回环候选帧与当前帧之间的位姿，以视觉信息和惯性信息构建位姿图优化。

Images