0 引言

在现实环境中自主移动的机器人必须具备在大尺度、非结构化、动态和未知空间中进行导航的能力。为此, 移动机器人必须能够构建环境地图并实现自定位, 即同时定位和地图构建(Simultaneous Localization And Mapping, SLAM)。SLAM在过去的近30年中得到了广泛的关注与研究, 学者们提出了大量的SLAM方法, 包括各种各样的传感器、优化技术和地图描述方式。激光雷达能够以高频率提供精确的环境几何测量, 存在的主要不足是价格高、耗能大和体积重量大, 而摄像机能够克服上述不足, 且能够提供更多的信息。

视觉SLAM(Visual SLAM, VSLAM)方法使用摄像机作为传感器估计机器人的位姿和环境地图^[1]。Klien等^[2]提出在单目VSLAM系统中将跟踪和地图构建分为两个并行线程, 实现了基于捆集调整(Bundle Adjustment, BA)的并行跟踪与地图构建(Parallel Tracking And Mapping, PTAM)系统。RGB-D(RGB-Depth)摄像机能提供彩色图像和深度图像, 相对于单目摄像机能够获得绝对尺度, 避免了深度求取和尺度模糊等问题；相对于双目摄像机，由于深度信息能直接获取, 所以计算量较小, 降低了实现难度。

Kerl等^[3]提出优化光测一致性误差项以配准连续RGB-D帧, 构建RGB-D里程计, 随后又提出一种结合光测一致性误差和基于稠密深度数据的误差项的鲁棒方法^[4], 注重位姿估计而忽略地图构建。Henry等^[5]提出使用RGB-D摄像机的SLAM系统, 结合迭代最近点(Iterative Closest Point, ICP)算法和视觉特征计算摄像机位姿并优化, 构建了稠密的点云地图但不具备实时性。Endres等^[6]提出的RGB-D SLAM(RGB-Depth SLAM)算法与Kerl算法^[3]一样在CPU上实时运行, 采用相同的硬件进行评估实验, 显示出更好的鲁棒性和精度。这些算法中使用随机采样一致性(RANdom SAmple Consensus, RANSAC)估计关联视觉特征点之间的闭环, 并使用非线性方法优化位姿图。最后, 生成环境的体素三维地图, 能够应用于机器人的避障、路径规划和导航。由于算法使用ICP进行位姿图优化, 过度依赖于良好的初始化估计且缺乏对整体匹配质量的衡量。

由于视觉特征倾向于在实物的边缘, 导致深度测量数据易受噪声干扰, 为了减小估计误差, 文献[7]通过可靠的深度数值选择和提取特征, 提出基于平面特征的RGB-D SLAM算法。采用平面点特征有利于提高传统的ICP的精度和鲁棒性, 同时保持合理的计算量以保证实时性。ElasticFusion^[8]通过融合ICP和稠密的光度重投影误差实现视觉里程计, 并构建了点云地图, 具有较高的精度但是需要GPU加速；同文献[5]和文献[7]一样, 由于构建的是点云地图, 存在资源消耗严重和不利于维护的问题, 不适合机器人应用。Mur-Artal等^[9]提出的ORB-SLAM(Oriented FAST and Rotated BRIEF SLAM)根据图像序列实时地估计位姿和地图, 精度和鲁棒性均优于上述方法, 支持主流的单目、双目和RGB-D摄像机。ORB-SLAM能够在小尺度和大尺度、室内和室外环境实时地完成位姿估计的同时构建稀疏特征点地图, 实现宽基线的闭环检测和重定位, 并且能够实现全自动的初始化。该系统是迄今为止基于稀疏特征的VSLAM的最好系统, 其精度和鲁棒性是很难超越的。相对于RGB-D SLAM, 该方法存在的不足是构建的稀疏特征点地图应用领域有限, 侧重于定位功能, 不能够用于机器人的避障、路径规划和自主导航。文献[10]基于ORB-SLAM构建点云地图, 通过手柄遥控Turtlebot运动并构建地图。

本文基于ORB-SLAM, 使用RGB-D传感器提供的彩色图像与深度图像将原系统构建的稀疏地图扩展成稠密的点云地图, 在此基础上提出使用八叉树结构^[11]构建可供机器人应用的八叉树地图。最后, 将本文算法成功应用于自主移动机器人, 实验结果表明本文算法能够在充满挑战的条件下、长距离轨迹的情况下, 精确地跟踪机器人的位姿并构建环境3D地图, 并实现自主避障。

1 八叉树地图构建

通常将VSLAM分为前端、后端和最终地图描述三个模块:前端解决传感器数据关联和位姿估计问题, 后端解决局部图优化和闭环检测及全局优化问题。本文根据ORB-SLAM算法优化后的数据构建八叉树地图, 并成功应用于移动机器人的地图构建与避障, 也能够应用于机器人路径规划、导航和交互式操作。

ORB-SLAM中的关键帧存储与之对应的摄像机位姿、彩色图像与深度图像, 根据当前关键帧中的数据构建点云地图, 进而创建八叉树地图, 用于机器人的自主避障。

在点云地图中, 将3D环境描述为一系列的点: P={p₁, p₂, …, p_n}, 其中:p_i=(x_i^C, y_i^C, z_i^C)^T, 表示3D空间中该点的位置信息; i表示第i个点; C表示在摄像机坐标系下。

根据摄像机针孔模型, 空间中的3D点(x_i^C, y_i^C, z_i^C)^T被摄像机投影为像平面上的2D像素点(u, v)^T, 根据几何关系可知, u=x_i^C·f/z_i^C, v=y_i^C·f/z_i^C, 根据像素点和摄像机的内部参数可以反推3D点的坐标, 采用矩阵形式表达为:

$ \left[{\begin{array}{*{20}{c}} {x_i^C}\\ {y_i^C}\\ {z_i^C} \end{array}} \right] = z_i^C \cdot {\left[{\begin{array}{*{20}{c}} {{f_u}}&0&{{c_u}}\\ 0&{{f_v}}&{{c_v}}\\ 0&0&1 \end{array}} \right]^{ - 1}} \cdot \left[{\begin{array}{*{20}{c}} u\\ v\\ 1 \end{array}} \right] $

(1)

其中: f_u、f_v、c_u和c_v是摄像机内部参数, 通过校准摄像机获得。对于RGB-D摄像机, z_i^C可以通过深度图像的数据获得。至此, 由式(1) 可以推算出像素点(u, v)^T对应的空间点(x_i^C, y_i^C, z_i^C)^T在摄像机坐标系下的坐标。对关键帧中所有像素点, 通过式(1) 计算得到在摄像机坐标系下的坐标。3D空间中的点, 在世界坐标系下的坐标p_i^W与摄像机坐标系下坐标p_i之间的变换可由式(2) 计算:

$ \mathit{\boldsymbol{p}}_i^W = \mathit{\boldsymbol{R}} \cdot {\mathit{\boldsymbol{p}}_i} + \mathit{\boldsymbol{t}} $

(2)

其中:R和t分别表示摄像机的旋转矩阵和位移矢量。根据式(2) 变换得到世界坐标系下的坐标, 则获得对应关键帧的点云地图。点云地图包含关键帧的所有像素构成的大量的由三维位置信息构成的点, 随着关键帧规模的增大, 维护显得更加困难。同时, 对于机器人应用而言, 如此大规模的空间点, 冗余量大并且不实用。

八叉树地图能够描述任意形式的3D环境, 具有更新方式灵活、无需先验知识和地图形式紧凑的优点。相对于点云地图, 以八叉树结构分层描述方式存储地图, 能够高效地压缩和更新地图, 并且地图分辨率可调, 可解决点云地图不紧凑和不利于避障和导航的问题。

八叉树结构中的叶子节点, 采用概率的形式表示该空间的占据、空旷或未知状态, 即3D空间中该点是否存在障碍或可通行。在构建地图的过程中, 对于叶子节点n, 在1, 2, …, k时刻的观测数据分别为z₁, z₂, …, z_k, 由贝叶斯理论可知, 该节点记录的信息为:

$ \begin{array}{l} Q\left( {n|{z_{1\;:\;k}}} \right) = \\ {\left[{1 + \frac{{1-Q\left( {n|{z_k}} \right)}}{{Q\left( {n|{z_k}} \right)}}\frac{{1-Q\left( {n|{z_{1:k-1}}} \right)}}{{Q\left( {n|{z_{1:k - 1}}} \right)}}\frac{{Q\left( n \right)}}{{1 - Q\left( n \right)}}} \right]^{ - 1}} \end{array} $

(3)

其中Q(n)一般情况下假设为0.5, 此处引入logit变换:

$ {\rm{logit}}\left( q \right) = \ln \left( {q/\left( {1 - q} \right)} \right) $

对式(3) 两边取logit变换, 代入Q(n)=0.5, 化简后得:

$ L\left( {n|{z_{1\;:\;k}}} \right) = L\left( {n|{z_{1\;:\;k - 1}}} \right) + L\left( {n|{z_k}} \right) $

(4)

由式(4) 可知, 对于三维空间中任意一点, 每次获得新的观测直接加到上一次观测结果, 更新方式灵活易实现。八叉树地图中父节点的占据概率可以根据子节点的数值计算, 主要有取平均值$ \bar l\left( n \right) = \frac{1}{8}\sum\limits_{i = 1}^8 {L\left( {{n_i}} \right)} $和取最大值$ \hat l\left( n \right) = \mathop {\max }\limits_i L\left( {{n_i}} \right) $两种方式。

2 自主移动机器人平台

为将本文算法构建的八叉树地图应用到地面移动机器人的自主避障中, 搭建自主移动机器人平台, 其中车身架构为Axial AX10, 摄像机为Kinect 2.0, 控制器为Pixhawk^[12-13], 数据处理单元为微型计算机, 硬件条件是4核i7处理器, 16 GB内存, 256 GB固态硬盘, 系统的结构框架和搭建的移动机器人平台实物见图 1。

微型计算机通过USB3.0接口读取Kinect 2.0的彩色图像和深度图像, 并以ROS(Robot Operating System)^[14]主题的形式发布, 本文算法通过订阅该主题的数据完成位姿估计与跟踪、特征地图构建与优化、闭环检测和八叉树地图构建。将摄像机的位姿估计作为移动机器人的状态估计, 将创建的八叉树地图用于移动机器人自主避障。其中, 运动控制指令采用ROS主题的形式发布(geometry_msgs::Twist), Pixhawk控制器订阅该主题数据, 并输出信号到电子调速器以控制电机, 实现移动机器人平台的运动控制。

3 自主避障策略

有关自主移动机器人平台根据八叉树地图进行路径规划与导航, 不属于本文研究的范畴。为验证本文算法能够成功地应用于移动机器人在未知的、复杂的动态环境中的自主导航能力, 运动控制部分主要采用漫游式的自主避障策略。根据已构建的八叉树地图, 检测移动机器人前方障碍物的距离。当机器人前方没有障碍物或障碍物距离大于安全距离时, 给定直行前进的命令。当前方障碍物的距离小于等于安全距离时, 则给定左转前进的命令。若转向后障碍物仍小于安全距离, 则给定右转后退的命令, 直到前方没有障碍, 再给定直行前进的命令继续探索环境。避障策略代码如下。

输入  障碍物最近距离(closestRange)。

输出  运动控制指令。

1) 如果closestRange >安全距离且正在前进

2) throttle←0.4 // throttle为推力，前进

3) yaw←0 // yaw为转向，直行

4) 如果closestRange ≤安全距离且直行前进

5) throttle ← 0.4 //前进

6) yaw ← -0.6 //左转

7) 如果closestRange ≤安全距离且左转前进

8) throttle ← -0.4 //后退

9) yaw ← 0.6 //右转

10) 如果closestRange >安全距离且右转后退

11) throttle ← 0.4 //前进

12) yaw ← 0 //直行

4 实验评估

为验证本文算法的鲁棒性、精度和实时性, 采用TUM(Technische Universität München)的RGB-D数据集^[15]进行实验评估, 并将本文算法应用于移动机器人的三维同时定位和三维地图构建。

4.1 RGB-D标准数据集

RGB-D数据集(http://vision.in.tum.de/data/datasets/rgbd-dataset)包含两个Kinect传感器和一个Xtion Pro Live传感器捕获的若干序列的数据, 包含由高精度的运动捕获系统提供的同步真实轨迹。本文在实验中使用绝对轨迹误差(Absolute Trajectory Error, ATE)的均方根误差(Root Mean Square Error, RMSE), 评估本文算法估计的轨迹相对真实轨迹的偏离程度。对于估计轨迹$ \mathit{\boldsymbol{\hat X = }}\left\{ {{{\mathit{\boldsymbol{\hat x}}}_1}{{\mathit{\boldsymbol{\hat x}}}_2} \cdots {{\mathit{\boldsymbol{\hat x}}}_n}} \right\} $和对应的真实轨迹$ \mathit{\boldsymbol{X = }}\left\{ {{\mathit{\boldsymbol{x}}_1}{\mathit{\boldsymbol{x}}_2} \cdots {\mathit{\boldsymbol{x}}_n}} \right\} $, 定义$ AT{E_{{\rm{RMSE}}}}\left( {\mathit{\boldsymbol{\hat X}}, \mathit{\boldsymbol{X}}} \right) $表示真实轨迹与估计位姿之间的欧氏距离的根均方，其计算公式为：

$ \mathit{AT}{\mathit{E}_{{\rm{RMSE}}}}\left( {\mathit{\boldsymbol{\hat X}}, \mathit{\boldsymbol{X}}} \right) = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {{{\left\| {trans\left( {{{\mathit{\boldsymbol{\hat x}}}_i}} \right) - trans\left( {{\mathit{\boldsymbol{x}}_i}} \right)} \right\|}^2}} } $

(5)

本文选择的误差度量虽然不能直接衡量地图的质量, 但是SLAM系统同时完成位姿(轨迹)估计和地图构建, 且二者相辅相成, 因此轨迹偏差程度与地图偏差程度直接相关。

4.2 精度与鲁棒性

对比本文算法同RGB-D SLAM算法^[6]、ElasticFusion算法^[8]和ORB-SLAM算法^[9]的精度和鲁棒性, 如图 2所示, 其中ATE的RMSE单位为米(m)。由图 2可知, 运行文献[6]中评估性能所采用的4个数据集, 本文算法的RMSE约为文献[6]的25%, 精度提高了4倍。相对文献[6]采用ICP进行位姿图优化, 本文基于ORB-SLAM采用BA同时优化摄像机的位姿和地图, 有利于提升算法的精度和鲁棒性。文献[8]在fr2_large_no_loop数据集失败, 本文算法的精度与之相当, 但是鲁棒性更好。本文与文献[9]的精度和鲁棒性相当。

4.3 实时性

本文采用数据集进行验证的硬件条件为4核i5处理器, 4 GB内存, 文献[6]采用i7处理器和GTX570显卡。将实验的耗时情况绘制如图 3, 可见本文算法在处理器性能较低的情况下, 平均耗时降低为文献[6]的56%, 这主要得益于加入了关键帧的剔除和优化功能, 尤其在环境规模大、长时间构图的情况下, 优势更加明显。文献[8]需要GPU加速, 因此不进行对比。本文基于文献[9]扩展了地图构建方法, 耗时约为其2倍, 主要是本文构建点云地图和八叉树地图时耗时较大。

4.4 3D地图构建

ORB-SLAM侧重于提升摄像机位姿估计的精度、实时性和鲁棒性, 所构建的稀疏地图主要应用于提升位姿估计精度以及回环检测以保证构建地图的全局一致性, 更适合于虚拟现实(Virtual Reality, VR)而不适合于机器人应用, 如图 4(a)为运行“fr2_large with loop”数据集获得的稀疏地图。本文基于ORB-SLAM扩展使用RGB-D传感器构建稠密的点云地图和适合机器人应用的八叉树地图, 图 4(b)、(c)为本文算法运行fr2_large_with_loop和fr2_pioneer_slam数据集创建稠密点云地图和八叉树地图。此外, 图 4(b)、(c)两个点云地图和八叉树地图占用的存储空间分别为96.5 MB和220 KB, 显然后者的形式更紧凑、易于维护。

此外, 将本文算法获得的摄像机轨迹估计与由数据集提供的高精度运动捕获系统测量的摄像机轨迹真实值进行对比, 对比结果表明本文算法有效且精度高, 如图 5所示(RMSE分别为0.10 m和0.15 m)。

4.5 移动机器人应用

最终, 在实验室环境且地图未知的情况下, 基于第2章搭建的移动机器人平台, 测试本文算法。测试结果表明, 本文算法能够实时地实现自主避障和八叉树地图重构, 所有数据采集与处理均依靠在板的传感器和计算机完成, 图 6为构建的3D地图, 从上到下分别为点云地图和八叉树地图。在实际运行的过程中, 随着地图规模的增大, 算法的实时性降低, 原因是为提高点云地图和八叉树地图的可视化效果, 地图创建的精度为0.02 m, 远高于移动机器人导航所需。

5 结语

本文基于ORB-SLAM系统, 使用RGB-D传感器提出了一种新的VSLAM系统, 利用关键帧的数据改进了原系统的地图构建方法, 由不利于机器人应用的稀疏地图扩展为适合机器人避障与导航, 且易于维护的八叉树地图。本文算法在精度和速度上要优于RGB-D SLAM方法^[6], 相对于ElasticFusion^[8]精度相当而鲁棒性更好, 相对于ORB-SLAM方法扩展其地图构建技术, 实时地构建八叉树地图, 并成功应用于所搭建的自主移动机器人平台, 实现未知复杂环境下的自主避障与探索, 但是耗时增加。下一步将对图优化算法进行改进, 进一步提升实时性, 并研究将八叉树地图应用于路径规划和自主导航。