基于改进视觉里程计算法的移动机器人定位

任平洋，李文锋

（武汉理工大学 物流工程学院，武汉430063）

自主移动机器人要实现导航[1]，首先需要确定自身的位置和姿态，即实现定位。常用的定位方法有激光雷达定位、基于惯性导航系统的定位、GPS、基于人工标志[2-3]的定位、视觉里程计定位[4]等。其中，视觉里程计定位技术以其结构简单、获取环境信息丰富、对累计误差的抑制能力强等优势，在移动机器人定位导航、深空探测及增强现实等领域受到广泛关注[5-7]。视觉里程计利用连续图像帧的信息，估计相机初始位姿。传统的视觉里程计算法假定环境中的特征点为静态的，计算相机相对于静态特征点运动状况从而得到相机位姿。然而，在实际应用场景中，可能存在人、动物、车辆等动态物体，若未能有效剔除动态物体产生的特征点，视觉里程计位姿估计会出现较大误差。

针对动态环境中视觉里程计面临的问题，文献[8]提出了三帧差分法，利用相邻三帧的图像进行相互差分判断动态物体，在相机静态或运动较缓慢的场景中取得了较好效果。文献[9]采用光流法计算两帧间相机变换矩阵，利用求解的变换矩阵对相机进行位姿补偿，随后利用帧间差分法检测运动物体，剔除在运动物体上的特征点，但所提算法对于垂直于相机平面的运动物体检测较差，且差分法受图像阈值影响较大。文献[10]中通过在ORB-SLAM2 系统中添加前端实现检测，使用CNN 对帧中的先验动态对象进行像素级分割，后续算法不会在这些对象上提取特征，但此方法仅对先验对象有作用，且运动估计的结果依赖于训练样本质量。

上述算法在相机运动缓慢、动态特征少、静态特征丰富的场景中取得了较好的效果。然而，在纹理较少或弱纹理场景下，难以提取到丰富的特征来生成准确的描述子，导致算法性能大幅降低。而在结构化的环境存在许多平面特征，这些高维特征能为传统视觉里程计提供更加稳定和丰富的几何信息，从而改善VO 算法性能。此外，面特征在低纹理、光照变化的场景中相较于点特征与像素光度信息更加鲁棒。因此，本文提出一种动态环境下基于点面特征的视觉里程计算法，能够在结构化、多纹理以及低纹理环境中实现高精度机器人定位。

1 基于点面特征的视觉里程计

图1 为动态环境下基于平面特征的视觉里程计算法框架。算法分为以下3 个部分：①利用ORB提取场景中的特征点，基于两点相对距离不变原则对特征点分类；②恢复场景点云，对点云进行降采样。计算局部法向量，利用DBSCAN+KDTREE 对法向量进行聚类拟合，得到平面点云参数；③根据将得到的相互垂直的平面参数不同分类计算相机位姿变换矩阵。

图1 基于点面特征的视觉里程计算法Fig.1 Visual odometry based on the features of points and planes

1.1 基于相对距离不变原则的特征点分类

相机在空间中的运动是一种欧式几何变换，因此相机观测到的两特征点之间欧式距离不会随着相机运动而产生影响，只和特征点自身的运动状态相关。移动机器人通过相机采集连续帧的图片，利用ORB 算法提取图像特征点。如果两特征点之间无相对运动，则两特征点之间的相对空间距离不变[11]。因此，计算两个特征点在前后两帧的相对距离是否发生变化，即可判断出两特征点的运动状态是否一致。

利用ORB 提取图像帧的特征点集。对点集进行轮询，找到两点之间相互距离不变的两点放入运动状态点集D 中。循环特征点集其余点，判断特征点和运动状态点集D 中随机选取的一个特征点之间的距离是否发生变化，如果未发生变化则将该点放入此运动状态点集D 中。放入点集中的点都会标记已被分类，后续循环到此点时不会被再次分类。该类聚类完成后找到下一个两点之间相对距离不变的特征点放入新的运动状态点集，重复上述聚类过程直到所有运动状态点集聚类完成。算法流程如图2 所示。

图2 特征点分类算法Fig.2 Feature point classification

1.2 平面参数的获取

在室内环境中，难以在图像色彩不丰富的地面和墙面中提取ORB 特征点。但丰富的平面参数可以构建约束条件，以去除场景中的动态点，优化相机位姿计算。

为此，本文利用RGB-D 相机获取场景彩色图片和深度图片，通过相机内参恢复场景三维点云；对点云降采样减少运算数据量； 计算各点的局部法向量，利用DBSCAN[12]对法向量聚类以得到平面点云。在聚类过程中，利用KDTREE 对聚类进行搜索加速。

DBSCAN 是一种基于密度的聚类算法，具有抗噪声、无需指定类别种数、可以在空间数据中发现任意形状的类等优点。DBSCAN 算法的复杂度取决于邻域点查找的复杂度。最直接的方法是线性扫描查找，此时算法整体时间复杂度为O（n2）。而在数据量较大时，DBSCAN 算法速度会明显降低。为加快寻找速度，本文采用KDTREE 对最近邻搜索进行加速。KDTREE 是以二叉搜索树为基础的用于空间检索的数据结构，能够实现在随机分布的空间内以O（logn）的时间复杂度对点的搜索。

具体平面聚类步骤如下：

（1）得到降采样点云数据中点pi（xpi，ypi，zpi）的局部法向量n=（Api，Bpi，Cpi），带入平面方程可得：

由点的坐标和局部法向量求出平面参数Dpi。

（2）以（Api，Bpi，Cpi，Dpi）为聚类点进行DBSCAN+KDTREE 聚类。法向量3 个坐标值的取值范围均为（-1，1），而平面参数Dpi的取值范围为（0，10）。平面参数Dpi在距离度量中占比较大，因此在聚类时需对Dpi乘一个比例系数k=0.1。

3）利用SVD 分解求出每个聚类点集的平面参数。

上述聚类方法需求场景中点的局部法向量，得到平面参数较耗时间，为充分利用上一帧平面检测的结果，本文采用构造虚拟平面点的方法得到平面参数。瞬时内，相机运动可近似为匀速运动，由上一帧得到的平面点云经过上一帧的位姿变换后得到当前帧的虚拟平面点云。找到虚拟平面点云在当前帧近点，对近点拟合平面得到平面参数。记上一帧为k-1 帧、当前帧为k 帧、下一帧为k+1 帧。

式中：pk-1为k-1 时刻点p 的空间位置；pk为k 时刻点p 的空间位置；为相机k-1 时刻到k 时刻匹配点的旋转矩阵为两时刻之间平移向量。由于相邻两帧时间间隔小，可近似认为相机是匀速运动。用k-1 时刻到k 时刻的位姿变换近似表示k 时刻到k+1 时刻的位姿变换，则有：

1.3 带平面参数的位姿计算

设第k-1 帧到第k 帧之间相机位姿变换矩阵T为

式中：R 为第k-1 帧到第k 帧的旋转矩阵；t 为两帧间的平移矩阵。旋转矩阵和平移矩阵都为3 个自由度。

假定第k-1 帧的平面参数为（Ak-1，Bk-1，Ck-1，Dk-1），其中平面法向量为nk-1=（Ak-1，Bk-1，Ck-1）；与其匹配的第k 帧的平面参数为（Ak，Bk，Ck，Dk），平面法向量为nk=（Ak，Bk，Ck）。将平面方程写成矩阵形式为

第k-1 帧平面内的点（x，y，z）经位姿变换矩阵T 变换为第k 帧的点（x′，y′，z′）：

由式（5）～式（7）进行矩阵变换可得：

对比式（5）和式（8）可得匹配平面间满足：

获得相邻两帧的平面参数后，计算平面法向量间是否相互垂直。如果存在3 个相互垂直的法向量，则可由平面参数直接计算出相机位姿变换矩阵。如果存在3 对匹配的相互垂直平面的平面参数与由（9）式得：

由式（10）得：

如果场景中提取的相互垂直平面没有3 对，仅由平面参数无法获得准确的位姿变换。此时根据平面参数区分动、静态特征点集。基于两点相对距离不变原则对特征点聚类，对每个特征点集计算位姿变换矩阵。因为平面在场景中通常为静态物体，静态特征点集的位姿变换矩阵应使当前帧场景中的平面进行重投影得到的平面参数，与上一帧平面参数相比得到的误差最小。由平面参数转换公式对每一个位姿变换求误差err，取err 的最小值为静态特征点集。err 由式（14）可得：

式中：n 为场景中匹配的平面数量；［nkDk］为第k帧的平面参数；［nk-1Dk-1］为第k-1 帧的平面参数。如图3 所示，虚线平面为第k 帧平面投影至第k-1帧的结果。

图3 平面位姿投影Fig.3 Planar pose projection

考虑系统的稳定性，如果当前帧场景中未提取出足够的平面区分静态特征点集与动态特征点集，即动态点集与静态点集计算出的err 误差相近。此时算法退化为默认静态物体在空间场景中占主要成分。计算各特征点集凸包，取凸包面积最大的点集为静态特征点集，利用静态特征点集求解出R 和t。

1.4 基于静态匹配点对的位姿计算

当平面参数不足时则可通过平面参数和空间参数对动态特征点去除。获得静态匹配点后，采用EPNP 位姿估计的方法求得相机位姿变换矩阵T。但此时求得的位姿变换矩阵R 和t 不是最优解，需通过非线性的PnP[13]方法进行相机运动位姿优化计算。将上一帧图像的三维特征点重投影到下一帧图像上，最小化重投影误差以获取相机位姿。如图4所示，通过特征匹配可知和p1是p2同一个空间点P的投影；p1′为通过重投影计算出的像素坐标；err 为重投影计算出来的误差值。

图4 重投影误差Fig.4 Reprojection error

式中：K 为相机内参矩阵；T 为位姿变换矩阵。通过最小化如下误差函数可以获取准确的相机的位姿估计。

式中：ui为匹配点的真实像素坐标；Pi为第i 个特征点的三维世界坐标。

2 实验结果与分析

为验证本文算法对视觉里程计定位精度的提升，分别在TUM 数据集及真实室内场景进行对比实验。算法运行环境： 操作系统Ubuntu16.04，CPU型号I7-7700HQ，CPU 主频2.8 GHz，内存8 G。实验为CPU 实时运行，未经GPU 加速。

2.1 TUM 数据集结果与分析

TUM 数据集是慕尼黑工业大学制作的实验数据集，其中fr3 系列为动态场景数据集，其包括sitting_halfphere、sitting_rpy、walking_rpy、walking_static、walking_xyz 等子数据集，其中sitting 子类环境中人与相机运动程度较低，walking 子类环境中人与相机运动剧烈。在walking 高动态数据集中，分别应用本文算法与ORB_SLAM2 算法估计相机位姿，并与真实的相机轨迹进行比较。

图5 为walking_xyz 数据集对比实验，虚线代表相机真实轨迹，实线代表算法计算的相机轨迹。由图可知，ORB-SLAM2 算法在处理高动态环境时，得到的相机轨迹误差较大。该算法会在动态物体上提取特征点，视为静态点并用于相机位姿计算。本文提出的算法基于运动状态对特征点分类，检测出人身上的动态特征点，从而在相机位姿估计时使用较为准确的静态匹配点对。

图5 动态场景下算法估计相机轨迹与真实轨迹比较Fig.5 Comparison of camera trajectory and groudtruth in dynamic scene

表1 对比实验结果Tab.1 Comparison of experimental results

本文用RMSE 来衡量真实轨迹和算法得到的相机轨迹之间的误差。实验分别对比了ORBSLAM2、文献[9]和文献[8]算法的定位精度。与其他三种算法相比，本文定位精度平均提升67%。本文所提算法在低动态和高动态环境中相较于其它算法都提升了定位精度，尤其在高动态场景中定位精度提升幅度较大。对比实验充分说明本文算法在动态场景中具有的良好鲁棒性和精确性。

2.2 室内实际场景实验结果与分析

为进一步验证基于点面特征的视觉里程计算法的有效性，本文设计如下的在线实验场景：相机搭载在turtlebot3 移动机器人上部，基于ROS 平台，通过键盘控制机器人走出一个直角路径。实验检验本文算法在直线运动以及旋转运动中的稳定性。场景中有人在行走形成一个运动场景，从而验证算法在动态场景下的抗干扰能力。本次实验轨迹为一个长宽都为3 m 的直角路径，具体运动为先直行、再左转、再直行。

图6（a）是由Kinect_v2 拍摄的场景RGB 图像，场景中有地面、墙面、椅子、人体等物体；图6（b）是由RGB 图像和深度图像恢复而出的场景点云图；图6（c）是本文提出的基于局部法向量聚类提取平面算法的结果。由图可知在面对复杂环境时，可以较为快速地、完整地、准确地提取场景中的平面点云，为位姿计算、动态特征点的判别提供精准的平面参数。同时实验得出计算一帧平面的时间为22.457 ms，虽然耗时增加，但在移动机器人行走时能实时运行，且对动态环境下定位精准度有较大的提升。

图6 平面点云提取Fig.6 Planar point cloud extraction

动态特征点点去除情况如图7 所示，图7（a）为原始提取ORB 特征点，两帧图像间特征点匹配的结果；图7（b）是去除动态特征点后特征点匹配的结果。从图中可以看出，人体运动产生的动态特征点得到了去除，从而有利于视觉里程计定位精度的提升。

图7 动态特征点去除Fig.7 Dynamic feature point removal

由于在地面上运动，垂直于地面的y 轴上没有位移，因此仅考虑xz 平面上的定位效果。本次实验取到终点（-3，3）的误差。分别对Orb-slam2 算法、仅考虑利用空间参数去除动态点集的点特征算法与综合考虑点面特征结合算法进行对比实验。实验结果轨迹图如图8 所示。

图8 室内定位实验Fig.8 Indoor localization experiment

图中虚线为ORB-SLAM2 算法的运行结果，由图可见由于人在场景中的影响，算法计算出来的视觉里程计产生了偏差。图中点线为仅考虑空间参数去除动态点时的轨迹曲线，相较于不去除动态特征点，定位精度有的较大提升。图中实线为综合考虑点面特征时的轨迹曲线。有了平面参数的补充，定位精度进一步提升。ORB-SLAM2 算法得到的终点坐标为（-2.819，2.802），综合偏差为0.268 m；仅考虑点特征算法得到的终点坐标为（-2.906，3.089），综合偏差为0.129 m；综合考虑点面特征算法得到的终点坐标为（-2.978，3.028），综合偏差为0.034 m。实验验证了本文提出算法在真实场景下对视觉里程计定位精度的提升。

3 结语

针对自主移动机器人定位问题，本文提出一种基于点面特征的视觉里程计算法。基于两点相对距离不变原则对提取的特征点分类，将运动状态相同的特征点分为一类；计算点云局部法向量，由DBSCAN+KDTREE对局部法向量聚类提取出场景中平面参数；根据提取出相互垂直平面数量给出不同的视觉里程计位姿变换求解办法。通过TUM 数据集和实验室环境进行实验验证了算法的可行性和有效性。本文所提算法有效的减少了动态物体对相机位姿估计产生的影响。同时提高了视觉里程计在特征点提取较少，平面较多的场合运行时的精准度，为移动机器人使用RGB-D 在室内场景中完成定位建图提供了有效的技术方案。但此方法在静态特征点占比较小且提取出的平面参数无法判断出动态特征点时会失效，后续可以针对此场景加上其它传感器进一步判断动态物体，去除动态物体的影响。