室外环境下移动机器人视觉SLAM算法改进

吴俊君 ，胡国生

（1.广东食品药品职业学院 基础部，广东 广州510520；2.华南理工大学 机械与汽车工程学院，广东 广州510640）

0 引 言

同时定位与地图构建SLAM （simultaneous localization and mapping）技术是使移动机器人具有自主导航能力的关键技术。它确保移动机器人在未知环境中，从未知起点增量式地构建环境地图，同时利用环境地图进行自我定位［1］。在视觉SLAM过程中，机器人利用配备的 “眼睛”观测周边自然环境特征，进行自身定位和构建环境拓扑地图［2］。此类地图能有效地表达环境的拓扑结构，对大范围内的移动机器人视觉导航尤为适用。

目前视觉SLAM一般是在贝叶斯框架下建立系统模型［3－5］，然后采用基于扩展卡尔曼滤波器或粒子滤波器对模型进行求解。研究者们致力于提升SLAM算法有效性和实时性能［6］，以及增强对感知混淆环境的适应能力［7］。在视觉SLAM过程中，由于各种噪声 （例如：传感器测量误差，机器人运动模型误差等）的存在，机器人在执行算法迭代过程中会不断地积累误差，导致SLAM收敛失败。如何有效抑制各种噪声和控制误差的积累与传播是视觉SLAM成功的保证。

错误的闭环探测是产生误差积累的主要因素之一，它会降低地图构建的准确率，因此机器人需要快速有效地进行闭环探测，即：判断自己是否到过某些场景位置，以降低误差积累概率。目前基于视觉传感器的SLAM，闭环检测主要分为概率计算方法［8］和图像匹配方法［9］。此外，地图节点生成方式关系着地图构建的质量，它是视觉SLAM过程中另一个重要环节。

现有的视觉闭环探测方法存在：计算过程复杂繁琐，效率欠佳等问题；而视觉地图节点的探测方法没能有效地控制地图节点的冗余度，也进一步浪费了计算资源和存储资源，制约了视觉导航系统的性能。针对目前闭环探测和地图节点探测存在的问题，本文对此进行了改进，设计了一种新的视觉SLAM算法，它具有如下特点：①算法一方面从降低场景图像相似性计算复杂度入手改进闭环探测效率，另一方面采用场景间共有信息量阈值来控制地图节点的冗余度，在有效性、实时性和节约计算资源方面提升视觉定位与地图构建效果，力求算法简洁实用；②采用具有二进制描述符的BRISK特征作为场景建模的局部特征点，有效地提高了计算效率，降低了存储量；③基于Mini哈希进行场景模型的相似性计算，避免了繁杂的计算过程，整个过程快速有效。

1 改进的视觉SLAM算法

闭环探测和地图节点生成这两个环节密切影响着SLAM算法的性能。闭环探测错误会导致拓扑地图构建失败，探测的实时性不佳也会制约SLAM的效率；地图节点冗余度大，会制约闭环探测效率，而无效的地图节点又进一步增加了闭环探测的失误率。本节针对上述问题，对SLAM从闭环探测和地图节点生成环节进行了改进。

1.1 SLAM算法框架

贝叶斯滤波器对 “离散时间非线性动态系统”的状态变化具有较强的描述能力。SLAM过程具有明显的离散时间非线性动态系统的特征，SLAM过程的贝叶斯模型如式（1），连续定位和构图过程分为2步：预测和更新

式中：p （zt｜ xt）——观测模型，p （xt｜ ut，xt－1）——运动模型。虽然贝叶斯模型能较好地描述SLAM系统，但是直接求解比较困难。

本文采用粒子滤波器方法对SLAM过程的贝叶斯模型进行求解。机器人按照本文提出的地图节点探测方法，连续地构建拓扑节点。针对当前最新探测的拓扑节点，机器人会检测自己是否到过这个节点，这就是闭环探测过程。如图1所示，根据闭环探测的结果算法选择不同的地图构建方式：如果存在闭环，则更新现有节点的连接关系 （n－＞2）；如果不存在闭环，则新增拓扑节点 （n－＞n＋1）。

图1 拓扑地图构建与闭环探测

SLAM算法流程，如图2所示。闭环探测和地图节点探测方法的具体过程在1.3和1.4小节详述。

图2 算法流程

1.2 场景的相似性测量方法

两个场景的相似性测量是本文算法中闭环探测和地图节点生成的基础。本小节提出一种快速的场景相似性测量方法：首先提取图像的BRISK特征，然后基于Minhash求解离散特征点集合的相似性，将图像的相似性问题转化为集合相似性问题。

1.2.1 BRISK特征点描述与场景图像刻画

算法采用BRISK特征［10］作为表征场景图像的局部不变性特征点。首先在图像金字塔内的多个尺度上探测图像的极值点作为有效的特征点P：位置 ｛x，y｝，尺度k。特征点P邻域的像素点对形成的集合A被分为两部分，如公式（2）。距离较远的像素点对集合L用来计算灰度梯度方向，距离较近的像素点对集合S，其元素的灰度大小关系被用来生成该特征点的二进制描述符

算法采用二进制向量的集合S（p1，p2，p3…pn）来刻画一幅场景图像I（提取到n个特征点），集合元素与提取的特征点描述符相对应。如图3所示。

图3 场景表示：特征向量

1.2.2 场景图像的相似性测量

在上一节中图像被刻画为特征点集合，因此两幅图像的相似性测量被转化为两个集合的相似性计算。算法采用Minhash方法生成签名矩阵计算集合的相似度。下面以四幅图像为例来详述计算过程：

四幅图像分别对应集合A （p1，p2，p3，p4）；B （p3，p4，p5，p6）；C （p3，p7）；D （p2，p6），其中集合元素pi是特征点描述向量。首先生成集合的特征矩阵 （如表1），然后利用哈希函数生成压缩的签名矩阵 （如表2），最后统计签名矩阵中各列出现相同Minhash值的概率H。概率H便是两个集合的相似系数，即：两幅图像的相似度S。

表1 特征矩阵

表2 签名矩阵

1.3 闭环探测过程

闭环探测依赖于图像的相似度测量。本文基于上一节提出的场景相似性测量方法，在粒子滤波器框架下，进行闭环探测。

算法采用60个基本粒子作为运动样本，初始化时用粒子的均匀分布概率表示机器人最初的位置估计。每次重采样后，粒子的当前位置概率分布g，如公式 （3）。机器人根据当前的观测值估计自身的位置概率分布f，如式 （4）

闭环探测的位置与粒子最终的收敛位置相关，粒子的收敛过程如下：利用上一节提出的相似性测量方法，测量当前观测图像与地图节点中已有的图像的相似度M。利于相似度M和预设的相似阈值A，估计机器人处于当前位置的分布概率f。算法利用机器人上一次位置的后验概率分布g和当前位置的先验概率分布f，估算每个粒子的权重w，如式 （5）

根据粒子权重进行重采样，若粒子位置最终被收敛，则该位置即为闭环探测的位置；若粒子位置未收敛则表明无闭环现象发生。相似阈值A的设定在第2节进行。

1.4 地图节点的生成

一幅好的拓扑地图应该以尽可能少的拓扑节点较完整地描述环境区域的拓扑结构。拓扑节点过于密集会产生大量冗余的信息导致计算量倍增，过于稀疏又无法较好地反映环境拓扑结构，因此地图节点的生成方式对地图质量有较大的影响。机器人在视觉SLAM过程中，可能在某个地点或小范围区域停留，若基于时间步长或距离步长生成拓扑节点可能会产生上述的冗余信息。

本文算法采用基于信息减量的方式生成拓扑节点。机器人捕获场景图像的频率为25fps，为了保证计算效率，机器人每隔10帧取一幅图像作为有效帧的关键帧进行测量，过程描述如下：算法采用文中提出的相似性测量方法，连续地计算候选关键帧与最新的节点之间的相似度，若相似度低于节点生成阈值B，则认为机器人已经到达另外一个场景，然后进行闭环探测，如果不存在闭环现象则将该新场景作为拓扑节点加入；若存在闭环现象则直接对现有的节点连接关系进行升级。该探测方法既保证了关键帧节点对环境的覆盖率又降低了信息冗余度，有利于提升SLAM算法性能。节点生成阈值B在第二节设定。

2 仿真实验分析

本小节通过实验确定闭环探测过程中粒子权重阈值A和地图节点的生成阈值B，以满足后续室外环境下移动机器人实验的需要。

算法分析了机器人在室外采集的图像数据集G （分辨率640x480）。首先将该数据集G中的80幅图像分类为4个子集：不同场景集A1，相似场景集A2，较小近似场景集A3，相同场景集A4.利用1.2小节提出的相似性测量方法，分别对4个数据子集里的图像进行两两测试，相似系数范围：H （A1）∈ ｛S｜0＜＝S＜＝0.05｝；H （A2～3）∈ ｛S｜0.15＜＝S＜＝0.9｝；H （A4）∈ ｛S｜S＝1｝；测试结果部分数据，如图4所示。根据实验结果：粒子权重阈值A设为0.15，地图节点探测阈值B设为0.05。

图4 场景相似系数

实时性分析：1.2小节中图像相似测量的时间消耗是影响闭环探测乃至整个SLAM算法计算效率的主要因素，为此文本在PC （Intel core （TM）2Duo CPU E7500，2.93GHz，1G内存）上对该方法的实时性进行了测试，如图5所示。与当前经典的BOW （Bag of Words）方法相比，该方法无需离线构建单词本，最快每秒可处理40帧图像 （平均33帧），可以进行在线测量，因此它具有更高的计算效率。

图5 每两幅图像相似测量时间

3 室外环境下移动机器人实验

3.1 闭环探测的有效性实验

机器人在室外校园环境中，沿着如图6所示的路径拍摄一组视频 （593帧），随后又在这段场景里随机拍摄了10组图像，每组包含5幅图像作为机器人在不同位置的观测值。下面以一组图像为例，利用本文的闭环探测方法，在视频流中对这5幅图像进行相似性计算，将最相似的场景作为每幅图像的闭环位置，然后检查探测结果是否与主观判断结果相吻合，从而验证闭环探测方法的有效性。

图6 机器人移动路径

图7展示了对其中2个场景的闭环探测结果：左边2幅图像表示机器人到过的位置，针对这些位置，在视频流里每隔10帧做一次闭环探测计算，根据粒子权重阈值A确定闭环位置 （右边2幅场景）。上述两对闭环位置的场景相似度分别为：0.21和0.17。通过人眼也可以看到：左右两列场景图像相似度很高。在593帧的图像集里，其他闭环位置均被准确地探测出来，结果与人眼判断完全吻合。

图7 闭环场景位置探测

3.2 视觉SLAM算法验证

在室外环境下 （见图8），采用先锋轮式机器人平台（Pioneer 3－DX）进一步验证了SLAM算法的性能，包括以下2个指标：视觉定位的准确性及地图节点的冗余度。验证方法如下：让机器人按两条不同的路径移动，先后采集图像数据集 （见图9），两条路径有一段重叠的部分以检验闭环探测效果。机器人在定位与构图过程中需要处理两条路径的共同部分才能正确构建这一区域的拓扑地图。

算法首先处理机器人沿路径1行走所捕获的视频：按文中提出的减量式方法构建拓扑节点并不断进行闭环探测，机器人共构建拓扑节点45个，闭环数目为0。随后算法处理机器人按路径2行走时捕获的视频：继续执行SLAM算法，每10帧做一次闭环探测，并对发生闭环的节点进行标注，在未发生闭环的地方，升级拓扑地图。闭环探测结果显示的场景区域，如图9（b）所示，与实际情况完全吻合。

此外，对本文的节点生成方法进行了验证：将实验生成的拓扑地图与基于距离和基于时间步长生成的地图从信息冗余度上进行了对比分析。机器人在同一个区域分别采集3段视频流，并按3种方式生成地图节点，然后统计地图节点数目，并利用场景相似测量方法计算节点之间的信息冗余度，返回冗余节点数。结果表明 （见表4）：在确保对环境区域的有效覆盖率前提下，基于时间步长的方式对机器人产生的冗余节点机率最高，距离步长次之，基于信息减量的方式产生的地图节点冗余度最低。

表4 地图节点生成方式对比

分析发现：冗余节点主要发生在机器人转弯或停留的位置。由于基于时间和基于距离的方法分别只与时间阈值和距离阈值相关，它们不关注场景节点的区分度，所以当机器人速度减慢或在原地停留、打转的时候必然增加了节点冗余度发生的概率；而本文的节点生成方法本质上是利用场景间的区分度来生成地图节点，只有场景达到一定的可区分度，节点才会被作为候选对象加入到地图中，因此，无论机器人以多少速度、沿着何种路径构建地图，节点冗余度的发生概率都会维持在一个较低的水平。

4 结束语

SLAM算法是保证移动机器人自主导航的重要技术。视觉SLAM生成环境拓扑地图供视觉导航使用，其中闭环探测关系着地图生成的正确性，地图节点的探测方法决定着地图的质量。针对视觉SLAM算法中的闭环探测效率实时性不高，计算过程繁琐；地图节点生成方法所产生的地图信息量冗余度大等问题。本文对视觉SLAM算法进行了改进。首先提出了一种简洁有效的场景相似性测量方法，该方法首次采用了BRISK作为场景图像的局部特征点，并基于Minihash方法完成了场景的相似性计算，然后将该方法应用于闭环探测和地图节点的生成等关键环节，改善了视觉SLAM性能。

在后续的研究工作中，将进一步研究大范围环境下闭环探测的效率以及动态环境下视觉SLAM算法的鲁棒性等问题。

［1］Durrant Whyte H，Bailey T.Simultaneous localization and mapping：Part I［J］.IEEE Robotics ＆ Automation Magazine，2006，13 （2）：99－110.

［2］Ranganathan A，Dellaert F.Online probabilistic topological mapping ［J］.International Journal of Robotics Research，2011，30 （6）：755－771.

［3］LIU Yang，ZHANG Hong.Visual loop closure detection with a compact image descriptor ［C］／／International Conference on Intelligent Robots and Systems.Vilamoura－Algarve，Portugal：IEEE Press，2012：1051－1056.

［4］Angeli A.Visual topological SLAM and global localization［C］／／IEEE International Conference on Robotics and Automation.Kobe：IEEE Press，2009：4300－4305.

［5］Angeli A，Doncieux S.Incremental vision－based topological SLAM ［C］／／International Conference on Intelligent Robots and Systems.Nice：IEEE Press，2008：1031－1036.

［6］Davison A J，Reid I D.MonoSLAM：Real－time single camera SLAM ［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2007 （29）：1052－1067.

［7］Cummins M，Newman P.Fab－map：Probabilistic localization and mapping in the space of appearance［J］.The International Journal of Robotics Research，2008 （27）：647－665.

［8］Cummins M M，Newman P.Probabilistic appearance based navigation and loop closing［C］／／IEEE International Conference on Robotics and Automation.Rome：IEEE Press，2007：2042－2048.

［9］Ho K L，Newman P.Detecting loop closure with scene sequences［J］.International Journal of Computer Vision，2007，74 （3）：261－286.

［10］Leutenegger S，Chli M.Binary robust invariant scalable keypoints［C］／／IEEE International Conference on Computer Vision.Barcelona：IEEE Press，2011：2548－2555.