【OctoMap ROS 3D建图】ROS Noetic 基于 octomap_server 实现三维地图构建、可视化与地图保存

前言最近有个项目需要借助稠密点云完成在立体建筑内的导航与规划在正是开始进行路径规划之前我们需要对周遭的环境进行建图。本文默认大家已经具备map-odom定位的能力如果使用的是mid360获取的稠密点云可以考虑使用fastlio2进行定位如果使用的是深度相机获取的稠密点云可以考虑使用RTAB-MAP进行定位可以参考文章【RTAB-Map ROS2视觉SLAM】基于Astra Pro Plus的视觉建图与定位本文就将从ros noetic出发借助传感器提供的稠密点云使用octomap-server对周遭环境进行建图最终达到以下效果1 背景概述1-1 3D建图所面临的问题在基于稠密点云的3D建图与导航系统中相较于传统2D栅格地图其核心难点主要体现在数据维度高计算与存储开销大考虑到与2D Occupancy Grid不同3D环境需要在x,y,z三个维度上同时表达空间占用信息。当使用如LiDAR或深度相机生成的稠密点云时点云数量通常达到数十万甚至百万级别直接进行路径规划会导致内存占用急剧增加、搜索空间指数级增长、实时性难以保证3D路径规划搜索空间爆炸假设我们仍在3D空间中使用A*时直接将状态空间从2D的(x,y)扩展为(x,y,z)这回直接导致节点数量指数级增长、搜索效率下降、实时规划困难等一系列问题。1-2 类型为了解决3D建图带来的高维度存储与计算复杂度问题工程上通常不会直接显式构建完整的(x,y,z)三维栅格地图而是采用不同形式的空间表达方法来降低计算开销并提升查询效率。现有主流方法大致可以分为三类基于体素的概率占据地图Voxel/Octree Map基于距离场的稠密表达方法TSDF/ESDF基于高度或投影的降维表示方法2.5D Elevation Map1-2-1基于体素的概率占据地图Voxel/Octree Map其中OctoMap属于典型的八叉树结构概率占据地图通过递归划分空间实现稀疏存储从而在保证一定精度的同时显著降低内存消耗是当前机器人领域最常用的3D建图方案之一1-2-2基于距离场的稠密表达方法TSDF/ESDF而TSDF与ESDF则进一步引入“距离”信息相比单纯的占据状态能够提供更适合路径优化与轨迹生成的连续代价信息尤其ESDF可直接用于梯度计算与优化类规划算法1-2-3基于高度或投影的降维表示方法2.5D Elevation Map此外对于地面移动机器人或机器狗等应用场景还可以采用2.5D高度图方式将三维信息压缩到二维栅格中并附加高度或可通行性属性从而在保证实时性的同时降低规划复杂度。2 octomap2-1 Octree八叉树Octree八叉树是一种典型的三维空间递归划分数据结构其核心思想是将三维空间不断划分为8个子空间Octants从而以树结构的形式对空间进行层次化表达。在每一次划分过程中一个立方体单元会被均匀分解为8个更小的立方体节点分别对应空间在x、y、z三个维度上的二分组合因此该结构特别适用于三维环境建模与稀疏数据存储。相比于传统的三维栅格voxel gridOctree能够根据空间复杂度自适应地调整分辨率在空旷区域使用低分辨率节点而在复杂区域进行细粒度划分从而在保证表达能力的同时显著降低内存与计算开销。2-2OctoMap介绍OctoMap是基于Octree结构实现的一种概率三维占据建图框架其核心目标是在机器人系统中高效表达未知环境中的可通行空间、障碍物以及未知区域。根据octomap的设计其通过概率更新机制将传感器输入的点云数据融合进八叉树结构中从而形成一个可动态更新的三维占据地图。相比于传统点云地图OctoMap不仅存储空间占用信息还显式维护free、occupied与unknown三种状态使其能够直接服务于路径规划与自主探索任务。此外由于OctoMap采用多分辨率树结构它在保证全局建图能力的同时具备较高的存储效率并支持快速的空间查询操作因此被广泛应用于移动机器人、无人机以及四足机器人等自主系统中。2-3OctoMap原理OctoMap的核心原理是基于贝叶斯概率更新的八叉树占据建图方法其主要流程包括首先将传感器获取的点云数据如LiDAR或RGB-D相机通过射线投射ray casting转换为观测信息其中射线路径上的空间被更新为“free”而终点位置被更新为“occupied”。随后OctoMap将这些更新信息递归传播至Octree结构中的对应节点并以对数几率log-odds形式进行概率累积从而实现对噪声测量的鲁棒融合。在数据结构层面OctoMap通过Octree对空间进行自适应划分使得不同区域可以采用不同分辨率进行表达在查询层面可以通过树结构快速定位任意空间点的占据信息其时间复杂度通常为O(log n)。同时OctoMap还支持多分辨率查询与子树剪枝操作使其在大规模三维环境中仍然保持较高的实时性。最终系统输出的是一个可用于机器人规划的三维概率地图该地图不仅能够表达障碍物结构还能为后续的路径搜索算法如A*或RRT提供基础环境支持。2-4 安装我们可以直接通过apt进行安装sudoaptinstallros-noetic-octomap ros-noetic-octomap-msgs ros-noetic-octomap-server检查是否安装成功rospackfindoctomap_server2-5 前置需求octomap-server一般本身不提供map-odom的定位信息输入的点云信息需要位于完整的map-点云坐标系的tf树上map └── odom └── base_link └── mid360以本项目距离输入的点云是位于的坐标系是位于mid360上再通过fastlio2发布map-dom的坐标变换2-6 建图我们可以通过下属代码简单启用一个octomap_server节点launchparamname/use_sim_timevaluetrue/nodepkgoctomap_servertypeoctomap_server_nodenameoctomap_serveroutputscreenremapfromcloud_into/livox/Pointcloud2/paramnameframe_idvaluemap/paramnameresolutionvalue0.1/paramnamesensor_model/max_rangevalue50/paramnamepointcloud_min_zvalue-100/paramnamepointcloud_max_zvalue100/paramnamefilter_specklesvaluefalse/paramnamefilter_groundvaluefalse/paramnameground_filter/distancevalue0.2/paramnameground_filter/anglevalue0.3/paramnameoccupancy_min_zvalue-2.0/paramnameoccupancy_max_zvalue2.5//node/launch参数说明cloud_in指定输入点云来源frame_idOctoMap 输出的所有数据都会以该 frame 发布必须存在map → odom → base_link → lidar核心参数resolution分辨率单位m用于决定地图精度sensor_model/max_range最大感知距离pointcloud_min_z、pointcloud_max_zZ轴过滤filter_speckles是否去除孤立噪点filter_ground是否做地面分割ground_filter/distance、ground_filter/angle用于地面识别occupancy_min_z、occupancy_max_z占据更新范围运行roslaunch slam octomap_slam.launch运行会输出一帧Nothing to publish, octree is empty这是初始时八叉树为空不用管他2-7 可视化建图我们打开rviz2-8 参数说明rosnode info octomap_server发布者/octomap_binary [octomap_msgs/Octomap]核心 3D 地图数据压缩版/octomap_full完整未压缩 octree/projected_map [nav_msgs/OccupancyGrid]3D投影2D地图/occupied_cells_vis_array用于RViz 可视化/free_cells_vis_array可通行区域可视化/octomap_point_cloud_centers把 octree 转回点云服务/octomap_server/reset清空地图/octomap_server/clear_bbx清理某个区域/octomap_server/set_parameters动态调参数/octomap_binary请求“当前地图快照”/octomap_full获取完整树结构订阅/livox/Pointcloud2 [sensor_msgs/PointCloud2]点云输入/tf坐标变换/clock仿真时间2-9 地图保存在OctoMap中存在着两种存储地图的方式.bt.ot这两种是 OctoMap 的“序列化八叉树地图文件”但压缩方式不同。格式特点用途.btBinary Tree二进制压缩版最常用 / 推荐.otOctree 原始结构存储更偏调试 / 兼容地图保存保存为btrosrun octomap_server octomap_saver map.bt地图保存保存为otrosrun octomap_server octomap_saver map.ot2-10 查看地图在进行建图以后我们可以launchnodepkgoctomap_servertypeoctomap_server_nodenameoctomap_serveroutputscreenargs$(find slam)/maps/map-6-29_17-50.btparamnameframe_idvaluemap/paramnameresolutionvalue0.1/paramnamepublish_2d_mapvaluetrue//node/launch进行加载地图roslaunch slam octomap_load.launch总结本文基于 ROS Noetic 与 OctoMap介绍了八叉树三维建图的基本原理并结合octomap_server完成稠密点云地图的构建、可视化、保存与加载为后续三维路径规划与导航提供环境地图基础。如有错误欢迎指出感谢观看