基于单目相机与里程计的室内机器人定位研究

温倩雯，苏毅辉，李 庚

（福州大学 物理与信息工程学院，福建 福州 350108）

0 引 言

随着机器人技术的发展，越来越多需要人力完成的重复烦琐工作可利用机器人完成。对于自主移动机器人，尤其是室内动态环境中的服务型移动机器人，准确的自我定位技术尤为重要。传统方法常采用在室内放置如红外和超声波发射装置等主动信号源［1］，或布置一些已知信息的路标实现定位任务［2］。但是，这些方法需要安装额外的装置或对环境进行特殊布置，无法做到普遍适用。

Smith等提出的同时定位与建图（Simultaneous Localization And Mapping，SLAM）技术在不依赖外部设备或特殊标志的情况下，利用传感器信息计算机器人位姿，并且构建环境地图，逐渐成为解决室内服务机器人定位问题的热门技术之一［3］。在室内动态环境、天花板信息基本稳定不变的情况下，选择其作为视觉信息将减少动态因素改变所带来的误差。此外，学术研究［4-5］和市场应用方面也逐渐出现了采用摄像头朝向天花板方向的SLAM定位方案实例，如科沃斯和石头等扫地机器人。

针对室内场景下的机器人应用，本文提出两点改进。一是选择体积小、成本低且适合在嵌入式系统上实现的单目相机作为视觉传感器，通过固定摄像头方向使其朝向天花板，以克服室内动态场景下视觉信息变化多端不可靠的问题，同时优化特征匹配算法，以应对纹理重复度高或纹理稀疏的天花板场景。二是融合轮式里程计，结合里程计预积分信息，为视觉里程计提供初值，加速非线性优化收敛速度，以保证视觉里程计在稀疏纹理场景中不丢失。

1 系统框架

该定位系统主要分为视觉跟踪、局部建图及全局优化3个模块，如图1所示。视觉跟踪模块通过追踪特征点建立几何关系以筛选出关键帧。局部建图模块求解机器人位姿。全局优化模块通过回环检测消除累积误差，提高机器人定位的精度。此外，重定位功能可以在定位丢失或机器人被人为移动后重新进行定位，极大地提升了机器人定位的鲁棒性。

图1 定位系统流程图

2 系统实现

系统参考ORB-SLAM3的结构实现。

2.1 视觉里程计

对于空间中一个特征点WP=W(X,Y,Z)，在相机坐标系下记为CP=C(X,Y,Z)，通过针孔模型投影到相机平面为p(u,v)。将这样的投影关系描述为：

式中，K为相机的内参数矩阵。这里先不考虑相机的畸变，于是有：

式中，WφC表示世界坐标到相机坐标的旋转，是一个3×3的正交矩阵；WΔC表示世界坐标到相机坐标的1×3平移向量；符号d(·)表示齐次坐标变换。

在图像上提取特征点时，需要特别关注特征点旋转和光照不变性。室内环境存在大量人工光源，光源距离不同会导致较大的明暗差异。对同一场景下的天花板图片分别用 ORB［6］、SURF［7］、GFTT［8］、FAST［9］等常用特征点提取特征，结果如图2所示，从左上至右下的特征点分别为SIFT、ORB、BRISK、KAZE、AKAZE、FAST、AGAST、GFTT和SURF。由实验结果可知，SURF特征点在不同光照条件下均有较好的表现。相比于其他特征点，无论场景中是否存在人造光源，其数量及分布都表现得更为鲁棒。

图2 不同特征点提取算法的效果对比图

对于视觉里程计而言，准确的图像配准结果是后续所有工作的基础，需先保证配准结果的准确度。在特征匹配方面，考虑到扫地机器人移动速度较低，前后帧相机位姿变换量较小，故采用基于邻域的特征点搜索算法替代传统的全局搜索，以显著降低计算量。由于天花板视图具有纹理重复的特性，在图像配准过程中更容易得到误匹配，如图3（a）所示，其中绿色直线为正确匹配点对，蓝色直线为误匹配点对。为了改善特征点匹配的精度，依据里程计变化值预估出图像的仿射变换矩阵，并对参考帧的特征点做仿射投影，以限制邻域搜索范围。仿射变换是线性变换与平移变换的叠加，具有共线性、平行性及共线比例不变性的性质。进行变换后，图像结构信息不会发生变化。

通过图3的匹配结果和图4的统计数据可知，在一定范围内，两帧间隔越大，本方法的改进效果越明显。特别地，在重复纹理下，当两帧视图存在较大旋转时，普通方法下的错误匹配极高，内点率仅达30%，而本文所改进方法在该情况下内点率仍可达到85%，如图3（b）所示。基于里程计信息的特征点邻域搜索算法可显著降低误匹配率，提升匹配精度，同时降低计算量。

图3 纹理重复的场景下传统特征点匹配算法与本文改进算法匹配结果对比图

对于单目相机，仅凭单张图像无法获得所求点的深度信息，需通过三角测量的方法估计地图点的深度。三角测量由相机平移得到，测量结果存在不确定性，如图5所示。红色部分为测量的不确定度，当平移较小时，深度估计上的不确定性较大，三角化精度过低。但是，增大平移可能导致图像观测发生过大变化而无法保证足够多的匹配数量，因此存在三角测量的矛盾。本文采用深度滤波［10］思想，通过融合多帧多次三角化测量结果以克服偶然误差。

图4 传统特征点匹配算法及本文改进算法匹配结果统计数据对比

图5 三角化误差示意图

2.2 数据融合

轮式里程计通过航迹推算的方式获得机器人轨迹绝对值，可以帮助单目视觉确定绝对尺度。但是，由于轮胎打滑等因素的影响，里程计信息会造成累积漂移误差。将视觉信息与轮式里程计信息相结合可优劣互补，但在实际应用中，即使按照固定频率采集，图像与轮式里程计数据仍会在硬件和软件等相关因素的影响下无法得到时间戳严格对齐的数据。因此，在两种数据结合使用前应先对其进行时间戳对齐。一般情况下，里程计的采样频率高于图像采样频率，且更容易实现插值，故以相机作为核心传感器，以采集图像数据的时刻作为要插入里程计数据的时间点，通过插值的方法获取这一时刻的里程计数据，且需要索引这一时间点前后的两个数据，并利用前后数据插值计算得到对应时刻的里程计信息。

将对齐后的里程计信息通过预积分计算出相邻两帧图像时刻之间的变换信息作为视觉里程计的一个初值估计，以提高非线性优化的迭代收敛速度和精度。k和k+1时刻之间里程计的测量为：

式中，ηok为噪声项，包含平移分量ηvk以及旋转分量ηφk。在室内环境下时，机器人的运动可以近似在一个平面上，忽略Z方向的运动，只考虑X、Y分方向上的运动。所以，V=［vx,vy］T表示平移的X、Y分量，φ为沿轴的旋转。在k和k+1时刻间的状态传播为：

式中，Vk+1依赖φk。对式（5）左右各乘一个Φ(-φi)，得到：

于是，关键帧i到关键帧j之间的旋转预积分测量表示为：

噪声项为：

积分的噪声项传播迭代过程为：

通过预积分得到两图像之间的相对位姿作为视觉里程计的一个初值估计，以提高非线性优化的迭代收敛速度和精度。

3 实验结果

为了验证该系统的有效性，在常见的室内办公环境进行实验。实验设备如图6（a）所示。设备搭载的相机型号为乐视深度相机LeTMC-520，采集图像分辨率为640×480，图像采集频率为10 Hz，轮式里程计输出频率为20 Hz。需要特别说明的是，实验设备搭载的相机虽然为深度相机，但在实验中仅使用其单目图像信息，未使用其深度计算功能。

实验测试了ORB-SLAM3以及改进方法在室内动态环境的建图与定位情况。实验环境如图6（b）所示，行走轨迹大致如图6（b）中红色线条所示。该场景为典型的室内办公场景，有行走的人、易移动的椅子等。由于纹理重复、信息单一且存在动态障碍物，ORB-SLAM3在实验中存在初始化困难、多次丢失位姿、无法成功重定位以及建图结果混乱无法识别轮廓等问题，而本文所提方法基本不存在上述问题。

图6 实验设备及环境

该实验两种方法的建图效果对比如图7所示，图中矩形框为关键帧的位姿，红色矩形框为起始帧位姿，绿色矩形框为当前帧位姿，蓝色矩形框为一般相机位姿。此外，若存在不同颜色矩形框，则说明机器人在运动过程中曾丢失位姿，构建了一个新的子地图，其中黑点为全局地图点，红点为局部地图点。从图7（a）与图7（b）可以看出，所提方法构建的地图轮廓清晰，关键帧间隔合理。从图7（c）与图7（d）可以看出，ORB-SLAM3多次丢失位姿，计算出多条轨迹，如图7（c）中多种颜色关键帧所示，无法与实际情况对应。本文所提方法可从地图点分辨出天花板纹理，绝大多数地图点的深度能保持在同一平面，且关键帧分布可显示出机器人的实际行动轨迹。

图7 建图结果对比图

实验场景2如图8所示，图中红色线条为行走轨迹。机器人在行走过程中有直行和旋转等运动状态。图9为两种方法的轨迹对比图，其中Trajectory_OURS为所提方法轨迹，Trajectory_GT为轨迹真值，Trajectory_ORB为ORB-SLAM3轨迹。实验时，ORB-SLAM3成功初始化耗时久，且短暂运行后即丢失位置，故其轨迹不全。所提方法的轨迹与轨迹真值的平均误差为0.155 m。

图8 实验场景2

图9 实验场景2下的轨迹结果对比图

4 结 语

基于单目相机和轮式里程计，本文提出了一种室内动态环境下的机器人定位方法。该方法固定单目相机朝向天花板，以减少室内动态环境下信息多变带来的误差。为适应天花板纹理弱且重复性较高的特点，通过改变特征匹配策略提升纹理重复度高场景下的匹配精度，以获得更加准确的位姿求解；结合里程计预积分为视觉里程计提供初值，并为单目相机固定尺度信息。实验证明，在室内动态场景和重复纹理场景下，所提方法能有效避免动态信息的影响，能在常见的室内场景得到可靠的机器人行进轨迹并保证其定位精度。该方法在室内服务机器人的自主运动方面意义重大，具有较好的应用前景。