·294· 智能系统学报 第13卷 5.2.3三维环境重构效果对比 本算法实时性也远优于RGBD-SLAM,与ORB 图6(a)为采用了ORB-SLAM方法得到的三维 SLAM的实时性相当。RGBD-SLAM处理一帧数 环境重构结果，所构建的三维地图为稀疏点云。图 据平均用时约为350ms,每秒钟只能处理3帧数 6(b)为采用RGBD-SLAM方法得到的三维环境重 据，因此不适于实时SLAM。而本算法前端处理一 构结果，物体轮廓可以辨别，但冗余点相对较多。 帧数据平均约为31ms,后端经过位姿优化与点云 图6(c)为采用本文的SLAM方法得到的三维环境 优化，以约5帧/秒的频率更新点云地图。可以满 重构结果，物体轮廓清晰可辨，冗余点明显减少。 足SLAM系统对实时性要求。 (a)ORB-SLAM重构结果 (b)RGBD-SLAM重构结果 (©)本文方法重构结果 图6室内环境三维重构结果 Fig.6 3D reconstruction result of the indoor environment 5.2.4机器人运动轨迹跟踪 的投影，如图7所示。表1则记录了不同算法在不 为了获得机器人位姿的变化，本文以3D-2D的 同数据集下，由计算所得的机器人在三维空间中的 特征点约束，采用PNP求解的方法对机器人相邻位 运动轨迹的真实位姿之间的均方根误差(RMSE)。 姿进行估计，并借助图优化方法对估计位姿进行优 通过均方根误差的对比，可有效地评估算法对机器 化。为了使运动轨迹更为直观，实验给出了本文算 人位姿跟踪的误差大小。可以看出本文方法能较好 法在不同数据集下，机器人运动轨迹在XY平面上 地实现机器人的运动轨迹跟踪。 1.0 真实位姿 1.0 真实位姿 估计位姿 一估计位姿 0.5 两种位姿的差异 0.5 两种位姿的差异 0.0 00 -0.5 -1.0 -1.0 5 -1.0-0.50.00.51.01.5 10 -0.5-0.00.51.01.52.0 m r/m (a)FR1/room (b)FR1/desk 0.2 真实位姿 1.0 真实位姿 0.0 估计位姿 0.9 估计位姿 0.2 一两种位姿的差异 0.8 两种位姿的差异 -0.4 0.6 0.6 -0.8 0.5 -1.0 -12 0.3 -0.50.00.51.01.52.0 1.1 1.2 1.3 1.4 m x/m (a)FR1/floor (b)FR1/xyz 图7真实位姿与估计位姿对比图 Fig.7 Comparison between ground truth and estimated trajectories5.2.3 三维环境重构效果对比 图 6(a) 为采用了 ORB-SLAM 方法得到的三维 环境重构结果，所构建的三维地图为稀疏点云。图 6(b) 为采用 RGBD-SLAM 方法得到的三维环境重 构结果，物体轮廓可以辨别，但冗余点相对较多。 图 6(c) 为采用本文的 SLAM 方法得到的三维环境 重构结果，物体轮廓清晰可辨，冗余点明显减少。 本算法实时性也远优于 RGBD-SLAM，与 ORB￾SLAM 的实时性相当。RGBD-SLAM 处理一帧数 据平均用时约为 350 ms，每秒钟只能处理 3 帧数 据，因此不适于实时 SLAM。而本算法前端处理一 帧数据平均约为 31 ms，后端经过位姿优化与点云 优化，以约 5 帧/秒的频率更新点云地图。可以满 足 SLAM 系统对实时性要求。 (a) ORB-SLAM 䛹Ჰ㏿᳈ (b) RGBD-SLAM 䛹Ჰ㏿᳈ (c) ᱘᪳᫥∁䛹Ჰ㏿᳈ 图 6 室内环境三维重构结果 Fig. 6 3D reconstruction result of the indoor environment 5.2.4 机器人运动轨迹跟踪 为了获得机器人位姿的变化，本文以 3D-2D 的 特征点约束，采用 PNP 求解的方法对机器人相邻位 姿进行估计，并借助图优化方法对估计位姿进行优 化。为了使运动轨迹更为直观，实验给出了本文算 法在不同数据集下，机器人运动轨迹在 X/Y 平面上 的投影，如图 7 所示。表 1 则记录了不同算法在不 同数据集下，由计算所得的机器人在三维空间中的 运动轨迹的真实位姿之间的均方根误差 (RMSE)。 通过均方根误差的对比，可有效地评估算法对机器 人位姿跟踪的误差大小。可以看出本文方法能较好 地实现机器人的运动轨迹跟踪。 1.0 0.5 0.0 −0.5 −1.0 −1.5 y/m x/m −1.5 −1.0 −0.5 0.0 0.5 1.0 1.5 真实位姿 两种位姿的差异 估计位姿 (a) FR1/room 1.0 0.5 0.0 −0.5 −1.0 −1.5 y/m x/m −1.0 −0.5 −0.0 0.5 1.0 1.5 2.0 (b) FR1/desk 0.2 0.0 −0.2 −0.4 −0.6 −0.8 −1.0 −1.2 −1.4 y/m x/m −1.0 −0.5 0.0 0.5 1.0 1.5 2.0 (a) FR1/floor 1.0 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 y/m x/m 1.0 1.1 1.2 1.3 1.4 1.5 (b) FR1/xyz 真实位姿 估计位姿 两种位姿的差异 真实位姿 估计位姿 两种位姿的差异 真实位姿 估计位姿 两种位姿的差异 图 7 真实位姿与估计位姿对比图 Fig. 7 Comparison between ground truth and estimated trajectories ·294· 智 能 系 统 学 报 第 13 卷