机器人SLAM的特征点实时提取改进

陆佳嘉，柯福阳，余晓栋，董一鸣

(1.南京信息工程大学 自动化学院，江苏 南京 210031；2.南京信息工程大学 遥感与测绘学院，江苏 南京 210031；3.南京信息工程大学 无锡研究院，江苏 无锡 214000；4.无锡学院 物联网学院，江苏 无锡 214000)

0 引 言

近20年来，视觉SLAM(simultaneous localization and mapping)始终是实现机器人真正智能化的研究关键[1]，SLAM系统追求成本低、计算速度快，以及实时性和稳定性。早期的SLAM技术多是基于单目相机和双目相机，虽然成本较低，但是存在尺度不一、初始化不准、视角受限制等问题。2010年，微软公司推出了第一代RGB_D相机，能够直接获取图像的深度信息，避免了初始化带来的问题，加快了建模的速度，因此RGB_D相机得到了研究者的青睐[2]。Labbe M提出的RTAB_MAP算法是基于RGB_D相机的经典算法，包含SLAM系统的前端以及后端优化，同时可以构建多种类型的地图。2019年，Labbe M团队更新的RTAB_MAP算法，有效解决定位初始化问题[3]。2017年，ORB_SLAM2[4]被提出，将SLAM分成多线程进行单独工作，实现了对RGB_D相机的支持，并且可以在保证精度的同时实时运行，系统鲁棒性良好。2019年，PL_SLAM[5]被提出，PL_SLAM在SLAM系统里加入了线特征，在特征点少的环境中保持了系统的鲁棒性。

目前有很多完整且成熟的SLAM系统，其应用的场景多是稳定，光照环境好，无干扰的优良环境，但是现实的场景多为复杂的场景，导致系统的实时性和稳定性差，所以能够在已有的框架基础上，让SALM系统应用到现实的环境中并保持实时性和稳定性成为了急需解决的问题之一。

1 系统框架

本文基于特征点的视觉SLAM方法，对本文的系统进行设计，系统的整体框架如图1所示。框架主要包括前端的视觉里程计设计、后端优化、闭环检测以及地图创建。前端采用Kinect v2相机作为机器人的外部感知设备，来获取彩色图像以及深度图像，同时对图像进行配准。然后对特征点进行提取匹配。目前比较常见的特征提取算法有SIFI、SURF以及ORB。其中ORB算法在提取速度以及实时性上拥有绝对的优势[6]。本文采用提取ORB特征来确保算法的实时性，同时针对ORB特征点在复杂场景下分布过于密集这一问题，采用划分像素结合四叉树的方法来合理分配特征点，根据随机采样一致性的算法思想(RANSAC)对特征点进行迭代式匹配和筛选，提高匹配的效率和精度。然后采用EPNP组合ICP的方法求解机器人的姿态信息坐标，同时基于词袋模型形成系统的闭环检测，减少里程计位姿估计的累计误差以及机器人受到外界不可抗因素导致的漂移误差。进一步优化机器人的姿态信息。后端基于G2O库构建BA模型对获取到的机器人的姿态信息进行处理优化，得到最优的估计运动轨迹，最后构建出与轨迹一致的全局三维点云地图。

图1 系统框架

2 特征点

特征点指的是角点、关键点等具有特征性质的点，能够在不同角度采集的图像中具有尺度不变性的点。在SLAM中，特征点是具有丰富信息的点，一般特征点出现在拐角以及纹理变化丰富的地方。本文采用主流的ORB算法作为本文的特征提取算法。ORB特征包含关键点和描述子，ORB计算了关键点的主方向，保证二进制描述子的旋转不变性，同时加快了ORB特征提取的速度。然后通过构建图像金字塔保证尺度不变。采用划分像素的方法对整个图像的像素进行区域划分，来提高区域提取到特征点的概率，让每个有效区域的角点可以提取到特征点。利用四叉树算法进一步使角点均匀分布，避免特征点过于密集，减少冗余点，提高特征提取的效率和精度。改进的ORB特征提取算法流程如图2所示。

图2 ORB特征提取流程

2.1 改进ORB特征提取

为了整个图像中的特征点基本均匀地分布，不会出现特征点过于密集的情况，采用划分像素结合四叉树的方法来达到特征点均匀分布的需求。

2.1.1 对像素进行划分

首先，建立图像金字塔[7]，将尺度系数设置为1.2，降采样通过双线差值的方式保证图像效果，同时将图像各层可以提取到特征点数量设置为1000。然后对各层图像进行去噪处理，如式(1)，用二维高斯分布函数作为卷积核，对原图中的每一个像素进行卷积运算得到新的像素值，进而对整个图像进行去噪处理。在式(1)里β代表的是模糊半径，半径和图像的模糊度成正比

(1)

得到金字塔之后，然后对每一层图像进行像素划分，在每个像素尺度内提取FAST角点，计算像素尺度区域中可以提取特征点的有效区域，以及坐标轴方向能够确定被划分出的最大像素数量，然后确定每个像素区域的范围大小。同时设立区域阈值，即在正常情况下提取到特征点的区域阈值，如果在这个阈值内没有提取到特征点，那么就在有效区域内扩大阈值重新提取特征点，直至区域阈值达到有效区域。这样可以增加原本没有特征点的区域提取到特征的点的概率，使得特征点在图像中可以大致均匀分布。

2.1.2 四叉树均匀特征点

在使用了划分像素的方法使得特征点的提取概率提高以后，利用四叉树算法来进一步改进特征点的均匀分布。四叉树多应用于图像处理，数据存储索引中，算法首先建立一个根节点，然后向下划分出4个节点，每次划分的4个节点保持与父节点的形状结构相似。同时需要保证点存在，才会在这个点的基础上划分4个子节点，而没有点的节点则不会划分。使得特征点的过于密集的区域得到稀释，均匀的铺满整个图像。图3是四叉树。

图3 四叉树方法

该方法对ORB特征点的处理步骤顺序如下：

(1)首先将图像有效区域里的特征点输入算法中，构建并初始化根节点N；

(2)将空间区域分别分为4个节点区域a、b、c、d，将N中特征点分配到对应的有效子节点区域范围内；

(3)计算每个子节点有效区域内的特征点数量，若数量为零，则将该节点区域记录为结束，若数量不为零则继续划分；

(4)计算总共划分的节点数量，若数量超过所需的特征点，则停止划分，同时保存节点区域内响应值最大的特征点；若比需要的特征点少，则返回步骤(2)，继续重新划分得到下一层。

2.1.3 计算角点方向

当提取的特征点在图像发生旋转的情况下，同一个特征点就会被认为是两个点，所以需要求解特征点的方向信息，增加特征点的旋转不变性，本文通过计算像素点的灰度权重值来求解特征点的方向信息，确保旋转不变性。设l(m,n) 为点 (m,n) 处的亮度阈值，则该处的图像块矩阵为

Nxy=∑mxnyl(m,n)

(2)

式中：矩阵N为 (x+y)∈(0,1,2) 阶矩阵，由此就可以得出灰度质心，即图像的质心为

(3)

对于特征点的方向可以用式(4)求解，公式如下

(4)

2.1.4 描述子

一般的BRIEF描述子利用二进制串码对特征点进行描述，但是不具有旋转不变性，本文同样利用灰度质心法对描述子的方向进行描述，假设将描述子对应的τ对像素点的坐标表示为 (qi,pi),i=1,2,3,…2n, 可以得到矩阵T

(5)

取特征点的方向角度为γ， 那么旋转矩阵Rγ就可以表示成

(6)

最后使用旋转矩阵Rγ对矩阵T进行优化处理

Tγ=RγT

(7)

使用优化过后的矩阵Tγ来对特征点进行描述，保持其旋转不变性。

2.2 特征点的匹配与筛选

对于本文的实验场景，对特征匹配的精度和运行速度都有一定的要求，所以我们采用暴力匹配[8]的方法，通过求解描述子之间的汉明距离，寻找到距离最近的匹配点。同时我们采用随机采样一致性的算法思想，用多次迭代的方式进行求解，使得匹配的准确度的概率逐次变大。

3 机器人的运动估计

完成ORB特征的匹配以后，通过匹配的特征点信息来估计机器人的运动，在随机采样(RANSAC)的基础上，采用EPNP组合ICP的方法来求解机器人的位姿，传统的EPNP算法求解位姿利用控制点进行变换，控制点的方法不能充分利用整个图像的信息，且图像当中存在噪声或者匹配失误的情况下，会导致误差增大。ICP[9]的方法则需要图像特征提前给定，一个图像的深度如果丢失，那么ICP的方法就会失效，同时ICP的方法对初始值要求很高。结合这些问题，本文采用上文提到的改进的特征提取匹配算法，减少误匹配，来提高EPNP算法的鲁棒性，同时将EPNP算法求解出来的位姿进行BA优化用来当作ICP的初始值，然后求解机器人的位姿。

为了避免初始化带来的麻烦，直接使用深度相机中的深度图，作为特征点的3D位置，然后将世界坐标系下的3D坐标用引入的4个控制点来表达，且控制点在互不共面的平面内。EPNP算法的思想是把这4个控制点在机器人坐标系下的坐标求解出来，然后根据这个坐标求解机器人的位姿。整体的流程如下：

(8)

同理在相机坐标系下

(9)

将选取出来的3D参考点中心作为控制点的中心

(10)

同时相机的外参系数是可以得到的，将相机的外参设为 [Rt]， 那么在不同坐标系中控制点存在以下关系

(11)

代入到式(8)可以得到

(12)

处理过后

(13)

这样就可以得出任意一个3D点在相机坐标系下的坐标可以用控制点的坐标来表示，这样就可以反解出控制点在相机坐标系中的坐标。其中控制点的选取根据式(10)，在求解出控制点在相机坐标系下的坐标以后，可以计算得出3D点在世界坐标系和相机坐标系下的质心

(14)

同理可以通过任意3D点的质心相对坐标，得到矩阵A，B

(15)

可以对矩阵H进行SVD方法分解

H=U∑VT

(16)

其中，矩阵H

H=BTA

(17)

基于此就可以求解出相机的位姿

(18)

EPNP算法求解出相机的位姿以后，采用BA模型对获取的结果进行优化，可以把优化的结果当作ICP的初始值，然后求解机器人的位姿，假设我们有一对已经用本文算法匹配好的3D点

T=(t1,t2…tn),S=(s1,s2…sn)

(19)

其中，满足∀i， 使得

ti=Rsi+t

(20)

利用SVD的方法求解ICP，首先将第i对特征点的误差表示为

ei=ti-(Rsi+t)

(21)

然后构建最小二乘模型Z，求出满足误差平方极小的旋转和平移矩阵

(22)

经过处理可以得到

(23)

式中：kj，k′j是T点，S点的去质心坐标，kj=ti-t,k′j=si-s。

3.1 机器人的闭环检测

如果可以准确识别出机器人是否运动到之前来过的地方，将二次来到这个地方的数据进行闭合关联处理，进行位姿对比分析，那么就减小机器人多次运动而产生的累计误差，使得估计的运动轨迹更加接近真实轨迹。这种方法称为闭环检测。本文主要基于词袋(BOW)模型[10]进行闭环检测，用图像的关键帧得出视觉向量来解决图像的相似度问题。该方法独立于SLAM系统存在，相较于其它方法具有效率高、准确率高的特点。

词袋模型首先将提取到的ORB特征点当作单词，然后将这些单词构造成一个词典。由于词典需要广泛适用性，需要提取训练多场景的特征点，以保证词典的有效。其中本文的字典表示利用k叉树算法，将特征点的描述子作为根节点，然后每一层节点由k-means聚类来获取中心点，中心点就是子节点，在子节点上重复上述步骤，对特征点进行分层聚类。当词典构造完成以后，就可以利用词典计算图像之间的相似度，即计算视觉向量相似度。公式如下

(24)

式中：w为相似度，s为视觉向量。

3.2 G2O图优化

图优化算法能够对地图上的关键路标点以及机器人的位姿进行处理，提高准确度。为了提高系统的实时性以及运算效率，本文只对位姿进行图优化，由于前端的局部优化会对路标点进行优化，所以将路标点作为已知量约束条件，只对相机的位姿进行优化。如图4所示。

图4 优化

基于位姿图优化模型，如图4所示，只需要构建位姿的图优化，将相机的运动模型作为图优化的边，相机的位姿节点作为图优化的顶点。其中位姿节点为待优化变量，边被定义成位姿节之间的运动估计。机器人的位姿节点的边可以通过前端的特征匹配求得初始值，然后进行优化。SLAM可以表达成一个非线性最小二乘问题，包括运动方程以及观测方程。这里利用图优化来表达最小二乘问题，并且基于G2O库求得最优解。其中机器人的位姿节点用四元数表示，假设位姿节点ξi和ξj之间有相对运动Δξij， 则可表示为

(25)

其误差项可以定义位eij

(26)

当应用到全部的位姿节点上，则目标函数为

(27)

G2O库里将边作为约束关系，机器人位姿作为节点待优化量，当目标函数取得最小值时，就是位姿的最优解，从而得到机器人的最优位姿。

3.3 构建三维点云地图

地图构建是SLAM工程的最后一个环节，根据不同的工程要求可以呈现出不同的地图类型，根据特征点法可以分为稀疏地图、稠密地图等。整个SLAM工程在完成机器人的运动轨迹估计的基础上，完成对周围环境信息的获取，为了达到最后构建的地图的精度良好，在本文算法对前端的改良下，利用词袋模型进行闭环检测，G2O进行图优化，减少误差。把图像处理成点云数据，根据轨迹一致性生成点云地图。根据生成的地图可以完成高级的应用，比如路径规划、导航等。

4 实验结果分析

本文对算法的一系列改进，为了展现出改进算法的实时性和稳定性提高，将采用公开的TUM数据集对算法的性能进行验证，通过传统算法和改进后的算法进行实验对比，分析改进后算法的实时性效果以及稳定性效果。

4.1 实验平台搭建

本文的全部实验基于Ubuntu16.04版本环境下进行开发，操作系统基于ROS Melodic版本，硬件平台采用4 GB版本的Jetson Nano TX2开发板，Kinectv2相机等，利用笔记本电脑通过SSH工具访问Jetson Nano，并在室内进行环境下利用无人车展开实验，最后通过TUM数据集进行验证，实验平台以及实验环境如图5所示。

图5 实验平台与环境

4.2 ORB特征实验效果分析

实验设备采用的是Kinect v2相机，利用张友正的棋盘法对相机进行标定，得到相机的内参

(28)

图6是使用本文改进的ORB特征提取算法和传统算法的对比，很明显，经过本文的算法改进以后，使得部分区域可以提取出特征点，解决了特征点分布过于密集的情况，同时能够基本保持均匀分布在图像上。由此可以得出，本文算法在ORB特征提取方面优于传统的ORB提取算法，并且效果良好。

图6 特征提取对比

进行实验前，首先要对相机进行测试，测试结果如图7所示。同时将对从Kinect v2相机获取出来的深度图和原始图像进行配准实验，减少外部因素对实验结果的影响，保证整个系统的精度，按照传统深度图和原始图像对齐的方法，会出现深度图和原始图的空间坐标没有对齐，所以本文采用先对深度图和原始图进行配准，然后在进行对齐，这样可以让深度图和原始图的空间点一致，减少外部误差。相机的深度图和原始图的配准效果如图8所示。其中图8(a)是原始图像，图8(b)是在没有配准前获取的深度图，很明显有较大的误差。图8(c)是在配准以后获取的深度图，图像的对齐效果，明显优于没有配准的，结果表明相机的效果可以达到算法的要求。

图7 相机测试

图8 相机图像配准实验

在特征点提取出来以后，需要进行特征点的匹配，原始的算法经常会出现大量的的误匹配，采用本文的算法可以减少误匹配，提高算法的准确度，图9是基于本文算法的实验效果对比，其中图9(a)是算法没有改进前的效果，图9(b)是改进过后的效果。

图9 特征匹配算法改进效果对比

4.3 算法实时稳定性以及误差分析

为了对改良后算法的实时性和稳定性进行定量分析，采集前1000帧图像，对第100帧、第300帧、第500帧、第750帧和第1000帧进行采样，通过深度学习的方法对各个算法进行定性分析，得到本文改良后的算法的实时性和稳定性。分析结果见表1、表2。从表1、表2可以得知在ORB算法组合光流法的情况下，算法的实时性能最优，但是算法的稳定性在急速下降，SURF算法在稳定性方面很好，但是无法保持实时性，本文的算法在保持算法实时性的同时也保证了算法的稳定性。因此在本实验环境下，本文算法的稳定性和实时性得到了较好的改进。

表1 实时性指标比对/fps

绝对姿态误差(absolute posture error，APE)指标是评价SLAM算法的重要工具标准，利用TUM数据集对本文提出算法和传统的RGB_D SLAMv2算法进行比较，验证本文算法的优良性。表3是本文算法在均方根误差rmse、平均值误差mean以及标准差std方面和传统算法在同一数据集下的测试结果。如表3所示本文改进的算法在不同的数据序列测试下的误差明显优于传统的算法，可以得知本文算法性能具有更好的定位精度。

表2 稳定性指标比对/%

表3 算法误差对比/m

SLAM的最终结果是构建实时地图，构图中，预估轨迹和真实轨迹对比的误差大小能够很好地反映出SLAM结果的好坏，轨迹误差的对比是通过建立评估文件夹，将不同算法生成的轨迹文件和数据集里的groundtruth利用evo评测工具进行评估，并且可以将轨迹可视化表达，自动生成均值、方差、标准差等信息对比图。其中APE指标是绝对轨迹误差，将位姿真实值和估计值按照时间戳对齐，求解出位姿之间的误差，并最终以图表的形式输出。图10是估计轨迹与真实轨迹的对比，其中图10(a)是利用fr1/xyz数据序列，其中实线代表的是真实轨迹groundtruth，虚线表示的是算法估计的轨迹KeyFrameTrajectory。图10(b)是机器人在x、y、z方向上的轨迹对比。其中可以发现估计的轨迹与真实轨迹误差较小，几乎是重合。图11(a)是利用fr3/warking_xyz数据序列求出的RGB_D SLAMv2的机器人轨迹误差对比，图11(b)是本文算法求出的机器人轨迹误差对比。其中热力条表示误差大小由下往上逐渐变大，虚线表示的是真值，其它线条表示估计出来的计算值。图12是本文算法和RGB_D SLAMv2算法根据APE指标评价的绝对轨迹误差分析，其中图12(a)代表RGB_D SLAMv2算法，图12(b)代表本文的算法。

4.4 构建点云地图

利用TUM数据集对传统算法和改进的算法构建的点云地图进行对比，图13是采用fr1/xyz数据序列进行建图，其中图13(a)是传统算法的建图效果，图13(b)是改进后的建图效果。图14是采用Kinect v2相机在实验室内的建图效果，图14(a)是传统算法的效果，图14(b)是改进后的效果。结果表明在室内静态信息环境，以及环境复杂度不高的情况下改进后的算法比传统算法构建的地图有更好的三维效果。

5 结束语

本文为了改善SLAM系统的实时性和稳定性问题，在系统的前端ORB特征提取部分以及SLAM系统后端做出了相应的改进，能够让特征点均匀分布于整个图像，同时减少了累计误差，提升估计位姿精度，构建轨迹一致地图。在算法的实时性和稳定性方面均有提升，系统的鲁棒性良好。但是，在更加复杂的场景下系统鲁棒性能还有待提升，后期可以考虑加入IMU等提升定位精度。同时当环境信息中存在动态信息，可以考虑结合YOLO目标检测器，对动态目标进行剔除，用来提高算法的鲁棒性。

图10 真实轨迹和预估轨迹对比

图11 不同算法轨迹误差对比

图12 APE指标误差分析

图13 数据集建图效果对比

图14 实验室建图效果对比