基于RGB-D图像的机器视觉定位算法

林秋叶，胡志恒

(成都信息工程大学通信工程学院，成都 610225)

0 引言

近几年，通信和电子等科技领域取得的创新和进步为机器人技术的进步做了极大的铺垫，然而距离机器人完全普及化和实用化还有许多技术壁垒需要突破，其中一个难点就是机器人的定位问题[1]。视觉定位精度高、系统柔性强、能够提供丰富的场景信息、携带方便且成本较低，符合移动机器人智能化的发展趋势[2-6]。因此基于视觉的同时定位与构建地图(Visual Simultaneous Localization and Mapping，VSLAM)[3]逐渐成为当前移动机器人领域研究的热点。

VSLAM主要分为前端视觉里程计(VO)、后端优化、闭环检测和地图构建四个主要部分[2]。其中视觉里程计根据相邻图像帧的运动的估计出相机的运动，并选取合适关键帧给后端优化和建图提供较好的初始值[3-4]。VO可以根据利用图像信息的不同分为特征点法的前端和不提特征的直接法前端[4]。

其中，前端使用直接法实现的SLAM系统主要代表有德国慕尼黑工业大学J.Engel等人提出的LSD-SLAM[3]，其使用单目相机选择针对像素运算的直接法构建半稠密地图，能够不使用GPU或者深度相机就能构建半稠密地图，但是系统易受相机曝光和内参影响，但在相机运动变化快时常出现跟丢情况[3-7]。而基于特征点法的前端对光照和动态物体不敏感，运行较为稳定，因此也常被选为视觉里程计的主要解决方案。特征点法中，Hentry等人搭建了一套基于特征点的 RGB-D SLAM[4]，可以完成稠密点云的重建，但系统运算时间长，实时性不高。Endres在迭代最近点(Iterative Closest Point，ICP)的基础上提出了 RGB-D SLAMv2系统，建立了一套完整的RGB-D SLAM系统[4]，但系统在运行时有卡顿现象，且生成的点云文件占用空间存储量过大，机器人在导航时难以应用。西班牙Zaragoz大学的Raul Mur-Artal基于PTAM架构[12]下提出ORB-SLAM，相对于原始的PTAM方案提升了追踪效果和地图精度，增加了地图初始化、回环检测功能，且结合场景特点优化了地图构建及关键帧选取方法，该方案图像处理中使用具有良好不变性的ORB特征点和描述子进行特征追踪和回环检测，使用基于关键帧的后端非线性优化，所以命名为ORB-SLAM[15]，在2017年提出ORB-SLAM2[4]，其创新地使用了三线程，跟踪和建图效果较好，回环检测也有效地防止和消除累积误差，但特征纹理弱的地方容易跟踪丢失，且重定位误差较大[4-14]。

针对以上研究现状，本文基于RGB-D相机搭建一套VSLAM系统，前端视觉传感器使用RGB-D相机采集图像信息，并采用改进的RGB算法进行特征提取与匹配，解决特征纹理弱特征点跟踪丢失的问题，提高系统鲁棒性；基于RANSAC改进的PROSAC算法统计置信度更高的点，剔除误匹配，得到优化配对点，优化计算单应矩阵的迭代过程，提高运算效率，保证系统实时性；最后再利用PNP(Perspective N Point)与ICP算法结合求解相机位姿，后端优化使用光束平差法对前面求解的相机位姿即机器人的位姿进行图优化，得到机器人全局位姿估计。

1 算法结构

本文的算法整体结构如图1所示，主要分为图像特征提取与匹配，相机位姿估计，全局优化几个部分。其中数据输入为深度相机在同一时间戳采集到的RGB彩色图像帧和Deep深度图像帧；将彩色图像序列和深度图像序列按时间戳对齐后，基于改进的四叉树策略均匀化提取特征点的FAST关键点，计算这些关键点的BRIEF描述子，然后根据描述子的汉明距离进行粗匹配，再用PROSAC剔除误匹配的特征点对，筛选得到质量较高的匹配点对；最后按照RANSAC迭代方式，结合PNP和ICP算法估计出机器人的位姿，基于BA算法求解得到机器人位姿轨迹的最优解。

图1 本算法整体结构

1.1 图像特征提取与匹配

常用的图像特征提取方法有ORB(Oriented FAST and Rotated BRIEF)[9]、SIFT(Scale Invariant Feature Transform)[11]、SURF(Speeded Up Robust Features)[12]等。其中SIFT具有尺度、旋转和光照等的不变性，但计算时间长；基于SIFT特征改进的SURF特征提高了特征提取的速度；同样以速度快著称的典型代表有FAST，但是FAST特征不具有方向信息[13]；ORB特征改进了FAST角点不具有方向性的问题[2]，采用描述特征点周围图像信息的二进制描述子BRIEF[13]，能够有效替代SIFT和SURF的特征。

ORB保留了特征子具有旋转不变性，尺度不变性的特点，虽然降低了提升了计算速度，是目前SLAM方案中性能与质量的较好折中[2]。SIFT、SURF、ORB在同一幅图像中同时提取1000个特征点的情况下，ORB算法在运算速度上比SIFT算法快几百倍，比SURF算法快几十倍[7]，且鲁棒性也能满足定位需求，在实时性要求较高的情况下，本文选用ORB特征点法。

表1 特征点提取算法对比

本方法在前端VO通过关键点FAST和描述子BRIEF进行计算，进行图像特征点提取与匹配。首先构建图像金字塔，在金字塔的每一层检测FAST，实现特征点的尺度不变性。基于金字塔的锥形比例可知金字塔层数i越高，图像的面积si越小，所能提取到的特征点数量ni就越小，即第i层金字塔面积si和特征点数量ni成正比。我们假设第0层图像的面积为s0，缩放因子为α(0<α<1)，特征总数为N，金字塔总面积为每层面积之和S，那么每个单位缩放系数所希望的特征点个数Ns为：

FAST不具备方向信息，因此选用灰度质心法和矩法确定FAST的方向，实现特征点的旋转不变性。定义图像块B的灰度表达式I(x，y)，定义p，q={0，1}，那么图像块B的矩为：

通过图像块B的矩mpq可以求得B的质心：

针对传统方法提取ORB特征点过于集中，出现扎堆的现象[9]，基于四叉树策略均匀化提取特征，使得关键点分布均匀，针对图像纹理弱区域无法取到有效特征点的现象，提出一种自适应阈值算法，根据图像不同区域亮度分布，计算其局部阈值。

对前面构建的金字塔每一层划分大小为30×30像素的局部区域，基于初始阈值对每个局部区域提取FAST关键点，若在初始区域提取不到关键点，则使用自适应阈值算法取值，若达到最小阈值还为取到FAST时标记该区域特征点为0。利用四叉树策略对所有提取到的关键点进行划分，均匀的选取N个FAST关键点。

过程1均匀提取特征点流程输入:金字塔每一层图像mi,特征点数目N,初始阈值h0输出:特征点图像坐标Step1.初始节点数目ni=1,初始化金字塔图像m0,初始化阈值h0;Step2.计算当前节点的特征点数量;Step3.若该节点特征点数量小于1,停止该节点的划分,否则在当前节点继续按照四叉树进行划分;Step4.比较当前节点数目ni与特征点数目N,若ni>N,则停止所有划分;Step5.遍历所有节点,若该节点特征数大于1,则选取响应值最高的特征点作为该节点的代表;Step6.若该节点特征点数小于等于1,则调用自适应阈值算法降低阈值,继续检测,直到检测到特征点,选出响应值最高的特征点,若直到达到最小阈值还未检测到特征点则淘汰此节点;Step7.最后得到每一层金字塔选出N个局部区域,每个局部区域有一个FAST关键点,实现N个均匀分配的特征点。

在提取FAST关键点后，我们对每个FAST计算其描述子，本文采用改进的BRIEF。采用高斯平滑法进行灰度值大小的比较，令BREIEF更加稳定。描述子距离表示两个特征间的相似程度[2]，因此采用汉明距离来表示特征点的相似程度进行特征点的匹配。

特征匹配是VSLAM数据关联部分，可以将当前观察到的路标与前一时刻的路标进行匹配比较，估计相机的运动轨迹。然后由于场景中常常存在大量重复纹理，使得特征描述非常相似，误匹配率情况广泛存在，这已经成为VSLAM中制约性能提升的一个大瓶颈[]。

1.2 图像剔除误匹配

特征描述子完成描述子匹配之后，会得到一些关键点对。最小二乘方法在描述子匹配输出的点对质量很好，理想情况下是图像没有噪声污染与像素迁移与光线恒定，但是实际情况下图像特别容易受到光线、噪声导致像素迁移，从而产生额外的多余描述子匹配，这些点对可以分为outlier跟inlier两类。RANSAC(Random Sample Consensus)[14]可以很好的过滤掉outlier点对，它会从给定的数据中随机选取一部分进行模型参数计算，然后使用全部点对进行计算结果评价，不断迭代，直到选取的数据计算出来的错误是最小。RANSAC算法可以剔除误匹配点对，但是算法是一种全随机的数据选取方式，导致迭代次数不稳定，效率较低，鲁棒性较差，算法效率相对较低。

因此本文采用基于RANSAC改进的算法PROSAC(Progressive Sampling Consensus)[16]，该方法采用半随机方法，对所有特征点的匹配质量好坏进行排序，定义一个质量因子q来衡量匹配点对质量的好坏，然后根据q值降序排列，取质量较高的点对求取单应矩阵。

在图像特征点描述子匹配过程中用汉明距离表示特征点的相似程度。用最小距离dmin1和次小距离dmin2的比值β表示特征点匹配质量。

PROSAC 算法的具体流程如下所示：

过程2误匹配剔除算法流程输入:最大迭代次数Mm,内点的误差门限δ,内点数目阈值Y输出:单应性矩阵HStep1.计算特征点最小欧氏距离dmin1和dmin2,欧氏距离比值β,质量因子q;Step3.取质量排名最好的m个点组合,计算每组的质量之和,并按组合的质量之和排列;Step4.选排序中最高的4组匹配点,作为初始的内点集合,通过该内点集合计算出变换矩阵T和单应性矩阵H;Step5.依次判断剩余的匹配点对按照H计算对应的投影点;Step6.计算投影点与匹配点的误差e与误差门限δ比较,若e<δ则将当前点加入到内点集合中;Step7.统计内点数目y并与内点数目阈值Y比较,若y>Y,则内点数目更新为y,反之迭代次数加1,回到Sep4;Step8.利用更新后的y个内点重新计算单应性矩阵H以及新的内点;Step9.若M

1.3 位姿估计

本文根据小孔相机模型描述成像原理[17]，采用PNP(Perspective N Point)[18]和BA[19]算法结合求解相机位姿。小孔相机模型是相机将三维空间数据信息反映到二维图像平面信息的最简单、最常用的模型，小孔相机模型的定义如下：

P3P[18]算法是PnP三对3D-2D匹配点估计位姿中的一种，对数据要求较少。如图2所示，相机光心为O，3D点A，B，C为世界坐标系中的坐标，2D点为a，b，c为相机坐标系上的坐标，即A，B，C在相机成像平面上的投影。根据相似三角形定理可以求得关于x，y的二元二次方程，其中已知的变量有，由3D点的世界坐标A，B，C求出的u，v，2D点的图像位置a，b，c和三个余弦角cos，cos，cos。x，y是未知变量，随着相机移动发生变化。

图2 3D-2D匹配点投影关系

2 实验及结果分析

在为了验证本系统的有效性，在如表2的实验环境下，采用德国慕尼黑工业大学的TUM数据集[20]，做不同特征点提取算法对比实验，不同误匹配剔除算法对比实验和定位的绝对轨迹误差分析实验。

表2 实验环境

2.1 特征点提取实验

本文分别在传统ORB特征点提取算法上和本系统特征提取算法上，选用图像特征较为明显的图像Img1和图像特征较为模糊的图像Img2进行特征点的提取。实验中的设置金字塔层数为8，缩放因子为1.2，得到如图2所示实验结果，传统ORB算法在特征较为明显的Img1取的特征点数量较多，但是提取的特征点较为集中，不能有效覆盖图像的各个部分信息；在特征较为模糊的Img2不能提取到的特征点非常少。而本文算法能在图像1和图像2中都能提取到有效特征点，且特征点数量满足要求，分布均匀。可见，在耗时几乎相等的情况下，本算法能够在图像特征点文理较弱的地方也能提取到有效特征点，减少传统ORB算法容易在图像较弱区域特征点容易跟踪丢失的风险，提高了整个系统的鲁棒性。

图3 ORB特征点提取

图4 本算法特征点提取

2.2 特征点提取实验

将两组相邻连续2幅图像进行特征提取与匹配，其中第一组图像特征点较为明显，图像纹理清晰，第二组图像纹理较弱。将提取到的关键点计算其描述子汉明距离，进行特征点的匹配，对误匹配的特征点进行剔除，本系统误匹配剔除算法与传统RANSAC算法的实验计算出的性能指标如表3所示。结果显示，本算法在图像纹理较弱的地方也能取到有效特征点进行匹配，经过在误匹配剔除阶段的额耗时比传统算法耗时少了近10倍，总匹配时间耗时少了1倍，整体提高了系统的实时性。

表3 误匹配剔除实验结果

图5 RANSAC算法误匹配剔除效果

2.3 定位分析实验

相机的真实姿态和估计姿态之间的直接差值可以用绝对轨迹误差(Absolute Trajectory Error，ATE)表示，可以非常直观地反映算法精度和轨迹全局一致性。当算法估计位姿表示为p1，…，pn∈SE(3)，真实位姿表示为Q1，…，Qn∈SE(3)，通过最小二乘法计算从估计位姿到真实位姿的转换矩阵S∈SE(3)，则第i帧的ATE为：

使用均方根误差(Root Mean Square Error，RMSE)统计总体值：

本算法在两组序列的估计轨迹与真实轨迹的绝对位姿误差岁时间的变化趋势曲线如图6所示，本系统算法与传统算法针对数据集的两个不同序列在本系统上运行的得到的估计轨迹(蓝色实线表示)与真实轨迹(灰色虚线表示)的对比图，红色到蓝色色带表示误差大小程度，如图7所示。图7为两组序列的估计轨迹与真实轨迹绝对位姿APE随时间的变化趋势曲线，表4为误差量化指标数据，包括最大值max、最小值min、均值mean、中位数median和标准差std。结果显示，本文算法精度，在光照特征不明显的图像序列中，也能跟踪到特征点，且定位的绝对误差较低。

表4 系统APE精度对比结果

图6 本算法误匹配剔除效果

图7 绝对轨迹误差趋图

图8 估计相机位姿与真实轨迹对比

3 结语

本文构建的SLAM系统在运行效率上较传统算法提高了一倍，在特征纹理较弱场景下的鲁棒性强于传统视觉定位方法，且定位精度与传统算法的定位精度几乎相同。本文构建的视觉定位方法构建的地图是稀疏地图，无法利用于实际场景下机器人的导航，接下来我们会基于八叉树构建实时地图，探索机器人的路径规划与避障，实现机器人的自主导航。