基于人形机器人的RGB-D SLAM方法比较评估

文章： Comparative Evaluation of RGB-D SLAM Methods for Humanoid Robot Localization and Mapping

作者： Amirhosein Vedadi， Aghil Yousefi-Koma， Parsa Yazdankhah， Amin Mozayyan

编辑：点云PCL

摘要

本文针对SURENA-V人形机器人定位与建图任务，对RTAB-Map、ORB-SLAM3和OpenVSLAM三种RGB-D SLAM（同步定位与建图）算法进行了对比评估。实验中，通过在机器人头部安装英特尔® RealSense™ D435 RGB-D摄像头，使机器人沿完整环形路径行进。在定位精度评估中，ORB-SLAM3以0.1073的绝对轨迹误差（ATE）表现最优，其次是RTAB-Map（0.1641）和OpenVSLAM（0.1847）。然而需指出的是，当机器人遇到特征点匮乏的墙面时，ORB-SLAM3和OpenVSLAM均面临里程计精度下降的挑战。值得注意的是，OpenVSLAM在机器人接近起始位置时能够实现闭环检测并完成地图内的重定位。在建图能力评估中，RTAB-Map凭借可生成稠密地图、八叉树地图和占据栅格地图等多样化输出占据优势，而ORB-SLAM3和OpenVSLAM仅能提供稀疏地图。

主要贡献

本文针对人形机器人行走场景，从精度、效率、鲁棒性、可扩展性和适应性等多个维度，对多种RGB-D SLAM方法进行了严格评估。通过深入分析RTAB-Map、ORB-SLAM3和OpenVSLAM等主流技术，系统阐述了其核心原理与算法框架。同时详细介绍了实验评估方案，确保对比分析结果具有可靠性和可复现性。

图1：(a) SLAM算法的世界坐标系、基坐标系与相机坐标系关系示意图 (b) SURENA-V人形机器人头部Intel® RealSense™ D435 RGB-D相机安装位置示意图

主要内容

介绍三种视觉SLAM方法，其简要对比见表1。

A. RTAB-Map算法

RTAB-Map是一种基于图优化的SLAM算法，其突出优势在于与机器人操作系统（ROS）的兼容性，这使其成为机器人应用的理想选择。该算法支持多模态传感器输入，包括RGB-D相机、立体相机及激光雷达，并能融合外部里程计（如编码器里程计或全球定位系统）数据。针对多传感器数据频率差异问题，RTAB-Map内置同步模块确保时空对齐。

该算法特别设计了内存管理模块以应对大规模环境建图需求，通过优化图结构增长方式和计算耗时，显著提升系统可扩展性与鲁棒性。在无外部里程计情况下，算法默认采用GFTT（Good Features to Track）特征检测器提取可追踪特征点，结合BRIEF（Binary Robust Independent Elementary Features）描述符进行特征匹配。为提升匹配效率，采用恒定速度运动模型缩小搜索范围，并通过PnP（Perspective-n-Point）算法计算相机相对于关键帧的位姿变化。

闭环检测采用词袋模型（Bag-of-Words）方法：首先通过视觉里程计特征聚类构建视觉词典，随后将当前帧特征与地图中的视觉单词比对以识别回环。检测到闭环后，采用g2o图优化算法进行位姿图优化。需特别说明的是，若优化后误差仍超过阈值，则该闭环将被判定为无效。优化过程通过迭代最小化误差残差，使回环约束与里程计数据达成一致，从而获得更精确的地图表达。

在建图阶段，RTAB-Map可根据应用需求生成稠密地图、占据栅格地图或稀疏地图等多种地图形式。其中：

• 稠密地图：包含完整三维点云信息
• 占据栅格地图：适用于路径规划的二维/三维栅格地图
• 稀疏地图：仅保留特征点信息的高效表示

B. ORB-SLAM3

ORB-SLAM3是2020年发布的开源SLAM软件库[6]，支持单目、双目及RGB-D相机等多种传感器。作为首个采用最大后验估计实现视觉-惯性SLAM的系统，其创新性地引入了多地图"Atlas"架构，包含活跃地图（用于实时帧定位）与非活跃地图（用于重定位、闭环检测及地图融合）。系统运行依托三大并行线程：

• 1. 跟踪线程：处理传感器数据、估计当前帧位姿，并通过投影匹配特征点最小化误差。视觉-惯性模式下可同步估计机体速度与IMU偏差，并在跟踪丢失时实现全地图重定位。
• 2. 局部建图线程：管理关键帧与地图点插入、冗余数据剔除、局部BA优化及IMU参数初始化。
• 3. 闭环与地图融合线程：检测活跃地图与Atlas库的共视区域，执行闭环校正或跨地图融合（融合后生成新活跃地图）。

C. OpenVSLAM

OpenVSLAM是2019年发布的模块化视觉SLAM框架[18]，具有以下特性：

• 多相机模型支持：兼容透视、鱼眼及等距柱面投影模型，支持单目/双目/RGB-D配置
• 地图管理功能：提供地图存储/加载接口，支持基于预建图的定位任务
• 跨平台部署：支持浏览器端访问的跨平台可视化

该框架采用基于图优化的间接法，以ORB特征为核心构建三模块架构：

• 跟踪模块：通过特征匹配与位姿优化实现逐帧相机跟踪，决策关键帧插入
• 建图模块：执行局部窗口BA优化（Local Bundle Adjustment）构建三维点云地图
• 全局优化模块：集成闭环检测、位姿图优化（基于g2o）与全局BA

相比ORB-SLAM，其技术优势包括：

• 独有的等距柱面相机SLAM能力，提供360°全景观测视角
• 优化的ORB特征提取实现，跟踪耗时降低约30%
• 完备的地图序列化功能（ORB-SLAM缺失的关键特性）

实验与结果

A. 系统概述

实验平台采用先进系统与技术中心（CAST）研制的SURENA-V人形机器人，该机器人具有44个自由度，身高168厘米，体重68公斤。如图1(b)所示，机器人头部搭载Intel® RealSense™ D435 RGB-D相机（视场角87°×58°）。计算终端采用配备Intel Core i7-11370H四核处理器、8GB GDDR6内存及NVIDIA GeForce RTX 3070显卡的笔记本电脑。通过200Hz控制频率的键盘指令，机器人以每步旋转0.17弧度、平移0.15米的运动参数在平坦地面执行环形路径行走。数据集由RGB-D相机采集，包含640×480分辨率、30帧率的RGB图像与深度图像。

B. 定位精度评估

为评估定位精度，我们采用基于标记物的真值获取方法：在机器人每步行走稳定后，在地面放置标记物，实验结束后通过测量这些标记物的位置与朝向来确定机器人每步终止时刻的真实位姿。随后将算法输出位姿与实测位姿进行对比，并通过公式计算各算法的绝对轨迹误差（ATE）。表II的ATE计算结果表明：ORB-SLAM3定位精度最优（ATE=0.1073），其次为RTAB-Map（0.1641）与OpenVSLAM（0.1847）。

C. 挑战场景与闭环检测评估

人形机器人SLAM实施面临的主要挑战在于步行时固有的机体晃动问题。实验表明，在特征点充足的情况下，三种算法均能保持稳定的定位性能。

当机器人行经特征贫乏的白色墙面区域时（如图3所示），仅RTAB-Map能维持里程计更新，其余两种算法均出现定位丢失。值得注意的是，当机器人返回起始位置附近时，RTAB-Map与OpenVSLAM成功检测到闭环。特别地，OpenVSLAM在闭环检测后展现出在已构建地图中重新定位的能力：通过匹配当前帧与地图关键帧的特征关联，实现位姿校正与地图一致性优化。

图3：各算法里程计输出对比。(A)区域显示当环境特征点减少时，仅RTAB-Map能持续跟踪机器人位姿，其余两种方法均出现跟踪丢失；(B)区域表明OpenVSLAM在检测到闭环后成功实现地图内重定位。

D. 建图能力评估

在建图阶段，ORB-SLAM3和OpenVSLAM均展现出生成稀疏特征地图的能力。如图4所示，这些地图是在算法失去里程计跟踪前获取的，它们通过场景中检测到的特征点分布和密度，直观呈现了机器人感知的环境信息。

与上述两种算法不同，RTAB-Map在建图阶段提供了更丰富的输出选项。图4显示，RTAB-Map能够实时生成环境稠密地图，这种地图通过更高密度的特征点云呈现更精细的环境细节。

图4：各算法建图输出对比。(a) RTAB-Map生成的稠密三维点云地图，(b) OpenVSLAM生成的稀疏特征地图，(c) ORB-SLAM3生成的稀疏特征地图。

此外图5展示了RTAB-Map生成的占据栅格地图。这类地图采用二值化表征（可通行区域/障碍物），特别适用于机器人导航任务。占据栅格地图能为路径规划、避障等自主导航功能提供关键环境信息，有效增强机器人的环境交互能力。

图5：RTAB-Map多模态地图输出 (a) 二维占据栅格地图（黑白二值表示障碍物分布） (b) 稠密点云地图的俯视投影图

总结

本文对三种主流RGB-D SLAM算法（RTAB-Map、ORB-SLAM3和OpenVSLAM）进行了全面评估。在定位精度方面，ORB-SLAM3表现出最优的绝对轨迹误差（ATE）性能，其次是RTAB-Map和OpenVSLAM。在闭环检测能力测试中，当机器人遇到特征点急剧减少的墙面时，RTAB-Map成功维持了位姿跟踪，而其余两种算法均出现跟踪丢失。值得注意的是，OpenVSLAM通过闭环检测后实现了地图内重定位，部分解决了这一问题；相比之下，ORB-SLAM3既未能检测闭环，也无法在丢失跟踪后恢复轨迹。在建图能力评估中，RTAB-Map展现出显著优势，可生成稠密地图、八叉树地图和占据栅格地图等多种形式，而另外两种算法仅能提供稀疏特征地图。

相关阅读：2024年度历史文章大汇总

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除