前言最近在学习激光 SLAM尝试在 ROS 2 Gazebo 环境下部署 DLODirect LiDAR Odometry激光里程计并进一步研究动态环境中的点云处理问题。在实际实验过程中我发现一个值得研究的问题动态物体会干扰激光点云配准而不少动态点剔除算法又需要机器人位姿作为输入。如何处理动态点检测与里程计估计之间的依赖关系围绕这个问题我尝试将 DLO 与 DUFOMap 结合通过仿真数据、坐标变换和 PCD 数据导出完成动态场景下的激光里程计与点云剔除实验。本文主要记录整个项目的实现思路、部分关键代码及遇到的问题。项目源码GitCodehttps://gitcode.com/mwbszz/dlo-dufomap关于项目来源机器人模型和 Gazebo 基础仿真环境主要参考 FishBot / 鱼香 ROS 的教学视频进行学习和搭建。在已有环境的基础上我重点完成了DLO 激光里程计在 ROS 2 仿真环境中的部署与集成。动态环境下的激光点云数据采集。基于 TF 和 Gazebo Ground Truth 的点云坐标变换。面向 DUFOMap 的 PCD 数据适配与动态点云剔除实验。DLO 和 DUFOMap 均为已有开源算法本项目侧重工程部署、系统集成与实验验证。一、项目环境项目配置操作系统Ubuntu 22.04ROS 版本ROS 2 Humble仿真平台Gazebo Classic机器人FishBot 差速轮式机器人激光雷达Gazebo 仿真三维 LiDAR激光里程计DLO动态点云处理DUFOMap开发语言C、Python数据格式PointCloud2、TF、PCD项目中使用的是 Gazebo Classic 仿真环境并非真实宇树 Go2 平台。1.1 Gazebo 仿真场景在已有机器人仿真环境中我使用了包含墙壁、柜子、书架等静态结构的室内场景并添加移动汽车和行人用于模拟动态目标的干扰。动态物体对应的模型名称为dynamic_cardynamic_person通过 ROS 2 节点调用 Gazebo 的/gazebo/set_entity_state服务可以让物体按照设定轨迹运动。图 1ROS 2 Gazebo 动态环境仿真场景二、整个系统的架构本项目主要由三个部分构成1. Gazebo提供机器人模型、动态场景和三维激光点云。2. DLO根据连续的激光点云估计机器人位姿。3. DUFOMap基于多帧空间观测的空闲空间证据对点云地图中的动态结构进行检测和清理。整体数据处理流程如下Gazebo 仿真 | 3D LiDAR | /lidar_points | --------------------- | | v v DLO 激光里程计 原始点云 | | Odometry / TF | | | --------------------- | collect_data.py | 坐标变换与 PCD 导出 | v DUFOMap | 动态点检测 | 静态地图处理另外我也增加了一条使用 Gazebo Ground Truth 位姿的数据导出链路用于与 DLO 估计位姿下生成的点云进行对照。需要特别说明当前代码主要完成位姿数据获取、点云转换、离线数据导出及 DUFOMap 处理实验并不是已经实现“DUFOMap 实时过滤点云再反馈给 DLO”的完整在线闭环。图 2整个系统tf树三、DLO 激光里程计部署3.1 DLO 是什么DLO 全称 Direct LiDAR Odometry是一种直接利用三维激光点云进行位姿估计的激光里程计方法。DLO 的核心包括Scan-to-ScanS2S点云配准。Scan-to-MapS2M点云配准。基于 GICP 的几何匹配与位姿优化。局部地图维护。DLO 使用高效的 NanoGICP 配准实现利用点云几何信息进行相对位姿估计。它的基本思路是当前帧点云 | v Scan-to-Scan | v 初始位姿估计 | v Scan-to-Map | v 优化当前位姿 | v 更新局部地图DLO 的重点在于高效估计连续运动是激光 SLAM 中的前端里程计方法不包含完整的全局回环优化系统。3.2 将 DLO 接入 GazeboGazebo 中的三维激光雷达通过 ROS 2 发布点云/lidar_points消息类型为sensor_msgs/msg/PointCloud2项目使用的是已适配 ROS 2 的 DLO 工作空间。需要注意DLO 上游原始项目主要面向 ROS 1不能假定原版仓库直接支持 ROS 2 Humble。在配置好对应 DLO 工作空间后可以通过其启动文件运行ros2 launch direct_lidar_odometry dlo.launch.py实际运行前需要确认点云输入话题和 TF 配置与 Gazebo 一致。四、为什么要进行动态点云剔除DLO 的点云配准主要依赖环境中的几何一致性。当场景基本静态时相邻帧点云描述的是同一个空间结构因此较容易建立有效的几何对应关系。但动态环境会破坏这种假设。例如机器人在走廊移动时一个行人从雷达前方经过。前一帧中行人位于位置 A下一帧中行人移动到位置 B。如果这些运动点被当成静态点参与配准可能会产生错误的几何对应关系。当动态目标占据较大的视野范围时问题会更加明显点云配准受到动态点干扰。局部地图可能保存不稳定的动态结构。历史点云叠加后可能出现重影。严重情况下可能影响里程计估计。因此我希望在实验中引入动态点云剔除模块使点云地图尽可能保留稳定的静态结构。【周围两个竖着的平面即为运动的两个物体】图 3动态目标对激光点云的影响图 4同一场景去除动态点云效果图五、为什么选择 DUFOMap5.1 最初遇到的问题算法之间的循环依赖最初研究动态点云剔除时我遇到了一个比较有意思的问题。部分动态检测方案需要利用里程计提供的位姿将相邻帧点云转换到统一坐标系再判断哪些点属于动态物体。但另一方面DLO 又希望输入尽可能干净的点云以减少动态目标对位姿估计的干扰。于是出现DLO 希望获得干净点云 ↑ | 动态剔除 | ↓ 需要位姿估计 | ↓ DLO这是一种算法上的循环依赖而不是操作系统意义上的线程死锁。为了研究不同的动态检测思路我开始了解 DUFOMap。5.2 DUFOMap 的核心思想DUFOMapEfficient Dynamic Awareness Mapping是一个基于三维空间占据与空闲状态进行动态检测的方法。与直接估计物体运动速度的方法不同DUFOMap 更关注一个位置是否曾经被可靠地观测为空闲空间假设有两次雷达观测第一次激光束穿过某片区域并命中了后方墙壁说明射线路径中的部分空间可能是空闲的。第二次该位置出现新的点云。如果历史观测已经可靠地确认过这里是空闲的那么新出现的点就可能具有动态属性。DUFOMap 利用射线投射和历史空间观测积累这些证据同时对空闲空间判断采取保守策略以降低稀疏采样和测量噪声造成的错误。这种方法的特点是不必先通过物体语义类别判断它是不是人或车也不需要逐个物体估计运动速度。5.3 DUFOMap 是否真的完全不需要里程计这里有一个值得特别说明的细节。DUFOMap 不必依赖 DLO 的 GICP 配准过程进行动态物体识别但在移动雷达场景中依然需要位姿信息来对齐不同时间的空间观测。如果机器人在运动每帧点云首先位于自身的雷达坐标系中。假设某个点在雷达坐标系下为ptLp_t^L要将其转换到统一的地图坐标系需要ptWTWL(t)ptLp_t^WT_{WL}(t)p_t^L其中 TWL(t)T_{WL}(t) 表示对应时刻雷达相对于世界坐标系的位姿。因此我在项目中准备了两种位姿来源DLO 里程计对应的 TF 变换。Gazebo 仿真提供的 Ground Truth 位姿。通过两种方式导出点云可以进一步研究位姿来源对 DUFOMap 处理结果的影响。这也是我在项目中增加两个点云采集节点的原因。六、关键实现将 ROS 2 点云导出为 DUFOMap 使用的 PCD6.1 根据 DLO 相关 TF 导出点云对应文件src/fishbot_application/fishbot_application/collect_data.py首先订阅 Gazebo 发布的点云数据self.subscriber self.node.create_subscription( PointCloud2, lidar_points, self.callback, qos_profile_sensor_data )点云到达后从 TF 中查询雷达相对于odom的位姿。关键代码为transform self.tf_buffer.lookup_transform( odom, lidar_link, Time.from_msg(cloud_msg.header.stamp) )这里有三个重要信息odom目标坐标系。lidar_link雷达坐标系。cloud_msg.header.stamp当前点云的时间戳。为了将点云转换到统一坐标系使用 PyKDL 构造变换矩阵rotation PyKDL.Rotation.Quaternion( qx, qy, qz, qw ) position PyKDL.Vector(tx, ty, tz) T_odom_lidar PyKDL.Frame( rotation, position )之后对每个激光点进行坐标变换point_lidar PyKDL.Vector(x, y, z) point_odom T_odom_lidar * point_lidar这样原本位于雷达局部坐标系的点就被转换到了odom坐标系中。6.2 生成 PCD 文件点云输出采用 PCD 格式保存位置为~/dufomap/data/my_data/pcd/文件名称按照帧序号生成000000.pcd 000001.pcd 000002.pcd ...PCD 文件中包含以下字段FIELDS x y z intensity同时保存激光雷达的位姿f.write( fVIEWPOINT {tx} {ty} {tz} f{qw} {qx} {qy} {qz}\n )需要注意项目输出的点坐标已经转换到了odom坐标系而VIEWPOINT记录的是对应坐标系中的雷达观察位姿。后续数据读取时不能再把这些点误当作原始雷达局部坐标。6.3 利用 Gazebo Ground Truth 导出对照数据对应文件src/fishbot_application/fishbot_application/collect_gazebo_data.py在这个节点中我订阅 Gazebo 提供的模型位姿/gazebo/model_states获得 FishBot 在世界坐标系下的位姿再结合雷达与机器人底盘之间的 TF计算Tworld,lidarTworld,baseTbase,lidarT_{world,lidar} T_{world,base} T_{base,lidar}这样就能把点云从lidar_link转换到world坐标系中。输出目录为~/dufomap/data/my_data_gazebo/pcd/这里保存的数据可以作为仿真位姿来源下的对照输入。需要注意当前实现使用的是最新收到的 Gazebo 模型位姿并未进行严格的逐帧时间戳同步因此在快速运动场景中还可能存在时间误差。七、项目运行步骤以下命令基于我整理的 ROS 2 工作空间外部 DLO 和 DUFOMap 需要提前配置好。7.1 编译工作空间进入包含src/的工作空间目录cd ~/terry_robot/terry_robot source /opt/ros/humble/setup.bash colcon build --symlink-install source install/setup.bash7.2 启动 Gazeboros2 launch fishbot_description gazebo_sim.launch.py检查点云话题ros2 topic list ros2 topic info /lidar_points7.3 启动 DLO在单独的终端中加载已配置好的 ROS 2 DLO 工作空间source /path/to/dlo_ros2_ws/install/setup.bash ros2 launch direct_lidar_odometry dlo.launch.py实际工作空间路径需要根据自己的环境修改。7.4 启动动态目标ros2 run fishbot_application move该节点会通过 Gazebo 服务更新场景中汽车与行人的位置。7.5 采集点云数据使用 TF 位姿导出ros2 run fishbot_application collect_data使用 Gazebo Ground Truth 位姿导出ros2 run fishbot_application collect_gazebo_data项目还提供了一个联合启动文件ros2 launch fishbot_description collect_data.launch.py该启动文件包含仿真、DLO、动态目标与数据采集节点的定时启动流程。不过源码中存在原实验环境的绝对路径/home/tangrui/ros2_ws使用前需要替换成实际的 DLO 工作空间路径。7.6 运行 DUFOMapDUFOMap 使用独立的上游工程进行处理。本项目主要负责数据准备和格式适配。在dufomap文件夹内启动./build/dufomap_run mydata/data assets/config.toml如果出现这个报错删除最后一帧点云即可解决00m:11s00m:00s] terminate called after throwing an instance of std::__ios_failure what(): basic_ios::clear: iostream error 已中止 (核心已转储)之后使用pcl点云查看器对点云进行查看即可pcl_viewer dufomap_output.pcd -fc 255,255,255完整的 DUFOMap 安装、编译及参数配置可以参考原作者仓库https://github.com/KTH-RPL/dufomap八、实验结果与观察本项目重点在于搭建动态场景下的 DLO 数据链路并完成 DUFOMap 所需点云与位姿数据的准备及处理实验。我主要关注以下几个现象1. DLO 在动态场景下的点云配准表现。观察动态行人、车辆存在时原始点云和累积地图中是否出现明显的动态结构及重影。2. DUFOMap 的动态点云处理效果。对比处理前后的点云重点观察移动物体的点云是否得到剔除同时检查静态墙壁、柜子等结构是否得到保留。3. 位姿来源对点云处理的影响。分别使用 DLO 相关位姿与 Gazebo Ground Truth 位姿准备数据观察两种位姿来源下地图对齐和动态点云处理的差异。图 5DUFOMap 动态点云处理后DLO定位图图 6DUFOMap 动态点云处理后Gazebo真值定位图目前尚未整理出完整、可复核的动态点分类精度、ATE/RPE 或运行耗时统计因此这里主要展示系统流程与定性结果不虚构算法精度提升。九、开发过程中遇到的几个问题9.1 动态剔除和里程计存在依赖关系动态点剔除算法往往希望获得准确位姿而里程计又可能受到动态点云影响。因此需要根据算法类型选择合适的集成方式例如独立粗位姿来源、延迟处理或者先进行粗配准再进行动态判断。在当前项目中使用了位姿数据导出与离线 DUFOMap 处理的实验方式。9.2 ROS 2 中的 TF 坐标系不能混用调试点云时要明确world odom base_footprint base_link lidar_link这些坐标系分别表示什么以及点云当前位于哪个坐标系。如果点云坐标已经变换到odom却再次应用同一个坐标变换就可能导致地图产生重影或错位。因此我在数据导出部分显式保存点云坐标和雷达VIEWPOINT信息。9.3 点云时间戳需要与 TF 匹配使用 TF 查询时如果所请求时间没有对应变换可能出现Transform lookup failed这通常需要从消息时间戳、TF 缓存、节点启动顺序和仿真时钟等方面排查。直接使用最新 TF 虽然可能暂时绕过问题但会引入额外时间误差不适合不加验证地用于严格的精度评估。9.4 PointCloud2 与 PCD 字段需要匹配代码读取了x、y、z和intensity。因此需要确认仿真雷达是否实际发布这些字段。不同雷达和 ROS 驱动的 PointCloud2 字段结构可能不同不能直接假设所有点云都有intensity。9.5 DUFOMap 不能保证从第一帧识别所有动态目标DUFOMap 依赖历史空闲空间证据。如果某个位置一直被物体遮挡系统尚未积累到可靠的空闲观测那么该位置的动态性可能无法立即判断。此外位姿误差、点云稀疏程度和观测覆盖范围也可能影响结果。十、总结与下一步计划这个项目是我在学习激光 SLAM 过程中的一次工程实践。虽然 FishBot 和 Gazebo 基础环境参考了现有教学项目但通过 DLO 的部署、动态场景数据采集、TF 坐标变换和 DUFOMap 数据对接我对激光 SLAM 中几个重要环节有了更具体的认识DLO 如何利用连续点云进行相对位姿估计。为什么动态点云会影响 GICP 配准和局部地图。多帧点云处理为什么需要统一坐标系。DUFOMap 如何利用历史空闲空间证据进行动态感知。仿真真值与里程计估计位姿如何用于对照实验。后续希望继续完善对 DLO 与 Gazebo Ground Truth 的轨迹进行定量评估。补充 DUFOMap 动态点剔除的定量对比。研究在线动态点剔除与激光里程计之间的集成方式。进一步学习激光 SLAM 的回环检测与后端图优化。对于正在学习 ROS 2、点云处理和激光 SLAM 的同学希望这个项目中的代码和踩坑记录能够提供一些参考。十一、开源代码与参考资料个人项目DLO DUFOMap 仿真实验https://gitcode.com/mwbszz/dlo-dufomapDLO 官方项目https://github.com/vectr-ucla/direct_lidar_odometryDUFOMap 官方项目https://github.com/KTH-RPL/dufomapFishBot / 鱼香 ROShttps://github.com/fishros/fishbot本文为个人学习与项目实践记录。感谢相关教学资源及开源算法作者项目中涉及的第三方代码与算法请遵循各自许可证和引用要求。