基于光束法平差的RGB-D视觉里程计

刘祥

（上海海事大学信息工程学院,上海 201306）

0 引言

视觉里程计一直是机器视觉中研究的热点问题。视觉传感器，例如数码相机，已经成为机器人导航系统中不可缺少的组成部分，通过相机获取并处理图像，获得机器人当前的位置、姿态和距离等信息，简称视觉里程计。它仅仅通过机器人携带的相机获取到的图像，来追踪和判断机器人当前的位置和姿态，为机器人导航提供有效依据。为了估计机器人的运动情况，传统方法采用激光传感器，利用迭代最近点法对激光点云数据进行匹配，从而求解机器人的位姿。它的算法思想是在两次扫描点云数据之间重复进行选择对应关系点对，计算最优的刚体变换，直到满足正确匹配的收敛精度要求。在实际应用中，由于初始值的选择不确定极易出现局部最优解的情况，这在一定程度上限制了它的应用。相反的，视觉里程计法成为当下的主流方法，它通过对图像进行特征提取、特征匹配、位姿估计以及优化等完成室内定位任务。

视觉里程计的发展已经有20多年了，在国内外广泛应用于商业和军事导航系统中。将视觉里程计技术应用于机器人中，机器人可以追踪自己的位置，进行定位，同时可以构建周围环境的地图。随着图像处理技术的发展，也因视觉传感器方便携带，功耗低，无需GPS等优点，视觉里程计技术在室内导航和定位得到了广泛的应用。使用普通的相机一直是视觉里程计的经典方法，而近几年推出的商业传感器可以同时捕获彩色图像和深度图像，即RGB-D传感器，它无需三角测量等算法便可得到图像深度数据，在三维重构和里程计方面有着巨大的应用价值。

考虑到实时性和实用性，在对图像序列进行特征提取时，并没有采用效果最好的SIFT特征，而是选用了ORB特征，该特征具有较好的尺度不变性和旋转不变性，同时能达到工程实用的目的。首先，根据特征描述子对相邻时间序列的图像进行匹配，采用随机抽样一致法进行反复迭代，剔除误匹配，求出最优的本质矩阵。然后通过矩阵分解，得到旋转量和平移量，从而恢复出机器人的运动轨迹和姿态。最后采用光束法平差优化，对局部位姿和周围的点云地图进行局部和全局优化，从而减小里程计累积漂移误差并提高定位精度。

1 视觉里程计算法

1.1 特征提取与匹配

特征点的提取和选择，首先要考虑到处理每帧图像不能耗时过多，同时不能占用过多的CPU资源，为此我们选用ORB特征。它是一种提取图像视觉特征的算法，包括特征点的检测和特征点的描述两部分，它采用FAST角点检测作为关键点，采用BRIEF描述子做特征描述[1]。FAST角点具有运算速度非常快的优点，同时可以检测出丰富的角点特征，但是不具有方向性。ORB算法在其基础上进行改进，通过定义形心，规定了特征点的主方向，使其具有了旋转不变性。同时，通过图像金字塔，在每层图像上进行角点检测，使其具有较好的尺度不变性。FAST特征点如图1所示：

图1 FAST特征点

FAST特征点的检测和判定原则：通过对比中心像素点与周围像素点的像素值的灰度差异，超过某个阈值时即判定该像素为特征点。通过（1）式角点响应函数可以判断FAST特征点。

式中：I（x）表示待测点周围内可选的随机点的灰度值。

ORB算法首先计算特征点周围的灰度的形心，再将特征点到该特征点周围的图形形心的方向作为该特征点的主方向[2]，局部区域矩的公式如下：

则计算出特征点周围的区域内的图像的灰度形心为：

此时则可以判定FAST特征点的主方向为：

ORB特征点的描述采用BRIEF描述子，它是由二进制位组成的一个向量，每个二进制位的0和1代表了特征点周围像素的大小关系。我们用特征点周围的128个像素点比较大小关系后得到128维的向量来描述该特征点[3]。通过帧与帧，或者帧与地图之间的特征描述子的匹配，我们可以估计图像之间的变换关系并进行位姿优化等。由于我们采用了BRIEF描述子，所以需要采用汉明距离来衡量描述子的相似程度，然后确立特征点的对应关系。

由于图像特征比较丰富，且需要匹配当前帧和地图之间的关系，若采用暴力匹配法则所需时间过久，所以采用快速近似最近邻算法，该算法适合于匹配点数量极多的情况。该方法是根据K均值或KD-Tree搜索操作所实现的，可以根据数据集的分布特点,对映射精度和空间资源消耗的要求来推荐索引类型和检索的参数，在高位空间最近邻查找不受局部敏感哈希影响。快速近似最近邻算法模型的特征空间一般是n维的实数的向量空间，它的关键在于使用欧氏距离找到特征点的邻居。图2是特征提取和匹配的结果：

图2 特征提取与匹配

1.2 剔除误匹配

特征匹配后我们可以看到，并不是每一个特征点都能正确匹配，存在一定的误匹配。因此我们选用RANSAC算法来剔除误匹配[4]，该算法的核心思想是根据两帧图像之间的对极约束关系，并通过多次反复迭代来剔除误匹配，并根据最优的匹配对计算出本质矩阵。该算法的具体做法是：

①先设定采样的次数N，有效匹配点与对极线间的距离阈值M；同时将相邻两帧图像的所有特征点的像素坐标做归一化处理，分别得到归一化的变换矩阵。

②在匹配点集中随机选取八个点计算本质矩阵。

③利用本质矩阵分别与其余的匹配点计算匹配点与对极线之间的距离。

④找出尚未匹配的点集中，对极线距离满足小于阈值M的匹配点。

⑤统计满足上述条件的特征点的数ni和特征点的索引号。

⑥反复执行上述步骤，直到误差函数取值达到最小，并且标记对应的索引号，则我们认为此时的匹配满足最优的匹配。

⑦最后我们用最佳的匹配点集计算出本质矩阵。

该算法是通过反复迭代，并不断更新基本矩阵的值，找到最佳匹配关系[5]。采样次数N设置过大，会增加处理每帧图像的时长，设置过小则不能找到最佳的匹配对，算法失效，起不到剔除误匹配的效果[6]。下图是经过RANSAC算法后，误匹配已经明显减少。

图3 剔除误匹配后

1.3 位姿估计

相同一个摄像机采集的两帧图像之间是有很强的约束关系的，它们之间满足下式：

其中X'和X分别代表匹配图像上的一对匹配特征点的像素坐标，F是基本矩阵，表明了两帧图像之间的变换关系，它是一个秩为2的3×3的齐次矩阵。

采用归一化8点法计算基本矩阵，它的重要思想是在求取基本矩阵之前对数据进行归一化，这样做并不会显著增加计算量，并且可以容易的求取矩阵[7]。下面给出8点算法的具体步骤：

①归一化：根据X,TX变换图像的像素坐标，其中T为归一化的矩阵变换，包括平移和缩放两部分。

②由最小奇异值的奇异矢量确定基本矩阵F，即得到线性解。也可以由其行列式的值为零得到强迫约束解。

(三)新会计制度中增加预算会计的概念。使得单位会计要素包括财务会计要素和预算会计要素，实行财务会计和预算会计平行记账法，单位对于纳入部门预算管理的现金收支业务，在采用财务会计核算的同时又进行预算会计核算，增加了预算收支、预算结余的账务处理，不仅清晰反映了财务主体的运行状况还有效反映和监督了会计主体预算收支执行情况。

③解除归一化，得到基本矩阵F。

得到基本矩阵后，并在保持投影点不变的情况下，对基本矩阵分解得到四个可能的解。如图4所示。

图4 四种可能情况的解

我们根据P点应该在两幅视图中都具有正的深度即可筛选出正确的解。分解基本矩阵后得到R和T，R是一个正交旋转矩阵，T是一个平移矩阵。求解出R和T，即完成相机运动的估计，得到相机的运动轨迹和姿态[8]。

1.4 光束法平差优化算法

估计相机的位姿和3D点的空间位置一直是计算机视觉的经典问题，优化相机位姿和图像中的特征点，在视觉里程计中是很重要的一个部分[9]。采用光束法平差对路标点和相机位姿进行优化，在给定相同场景不同图像当中的多个2D点时，则光束法平差可以表示为一个非线性最小二乘问题：

求解这个方程的最小值，需要获得较多的观察图像，才能得到一个较好的解。在实际操作中，通过迭代的方式来求解。通常采用L-M梯度下降策略来求取最优位姿,在求解过程中如果能提供良好的初始值，则算法会快速收敛[10]。在此之前，可用8点法求解初值。其次是步长的控制，选择一个合理的步长算法会立即收敛，否则算法可能会在几个谷底之间跳转，出现不收敛的情况。

使用局部的光束法平差对关键帧和路标点进行优化，这样有利于提高局部的定位精度，同时也可以减少累积误差。在构建好地图之后，采用全局的光束法平差对整体的位姿和地图中的路标点进行优化调整。光束法平差的应用，有利于减少噪声对系统带来的影响，它同时调整图像的观测值和估计值，使整个系统得到持续的优化。

2 算法实现及结果分析

算法在Ubuntu14.04操作系统实现，使用了开源的计算机视觉库OpenCV，并且采用C++11新标准编程，提高了系统整体的稳定性。在程序设计过程中，一个帧通常包括了一个图像和特征点、位姿、内参等信息，由于相机采集的数据很多，所以我们用关键帧来代表整个相机的运动轨迹，同时利用关键帧恢复出运动和三维的点云地图和空间结构。此外，地图点对于定位来讲，可以提供丰富的位置信息，我们还可以将新的帧与路标点进行匹配，从而估计运动信息[11]。对于基于特点的视觉里程计来讲，最关键的是相邻两帧图像之间的运动估计，即当前帧与参考帧的匹配关系，下面给出它的操作步骤：

①对采集到的新的当前帧，提取关键点和描述子。

②如果系统没有初始化，则以该帧为参考帧，并且根据对应的深度图来计算特征点的3D位置信息，然后返回①。

④判断上述估计是否成功。

⑤如果算法成功，则把当前帧作为新的参考帧，返回①。

⑥如果失败，我们统计连续丢失的帧数，当连续丢失的帧数过大，则设置里程计的状态为丢失，算法结束。若未超过，返回①。

本算法最终在广泛使用的TUM数据集上进行测试，TUM的数据集通过外部设备和GPS获取了标准轨迹，同时提供了一些比较工具和脚本程序，适合做研究。本次试验从三维重构结果、定位精度、旋转误差和平移误差来对算法进行衡量。三维重构结果如图5所示，其中黄色线代表的是相机的运动轨迹，从三维重构的结果来看，空间点的3D位置还是比较准确的，同时也基本恢复了场景的结构。

图5 场景的三维重构结果

图6的定位结果中，红色线代表的是估计轨迹与标准轨迹的差值，可以看到在数据集的多个场景下，算法表现优秀，特别是在长时间运行的（d）数据集中，系统的累积漂移误差很小。接下来，对比光束法平差算法和普通视觉里程计的旋转误差和平移误差，在不同数据集的实验结果见表1。

表1 旋转和平移误差结果

通过对比研究，我们可以明显的看到使用光束法平差优化后，算法能有效减小帧与帧之间的旋转和平移误差，由此提高了里程计的定位精度。

3 结语

通过加入光束法平差优化算法与普通视觉里程计进行对比研究，验证了算法的有效性。从实验结果看，该算法可以有效地减少平移误差和旋转误差，提高定位精度，并且在一定程度上，减小了里程计的累积误差。

图6 定位结果分析