机器人导航为什么要同时保存 2D 地图和 3D 点云地图？因为一个管走路，一个管找自己

项目已开源到Github欢迎StarGitHub - Ikunio/Lidar_nav2_ws: 基于 Livox MID-360 3D LiDAR 的 ROS 2 自主导航工作空间集成 LIO 里程计、重定位、Nav2 导航支持仿真与实机部署。 · GitHub基于 Livox MID-360 3D LiDAR 的 ROS 2 自主导航工作空间集成 LIO 里程计、重定位、Nav2 导航支持仿真与实机部署。 - Ikunio/Lidar_nav2_wshttps://github.com/Ikunio/Lidar_nav2_ws很多人第一次做 ROS 2 导航时都会有一个疑问我不是已经有map.yaml和map.pgm了吗为什么还要保存一个.pcd点云地图这不是重复劳动吗表面看确实像重复。就像你已经有了一张小区平面图却又掏出一份三维建筑扫描图。初学者看到这里很容易产生一种朴素而合理的怀疑你们搞机器人的是不是喜欢把事情复杂化答案是是的。但这次真不是为了炫技。在 ROS 2 3D LiDAR LIO Nav2 这类系统里2D 地图和 3D 点云地图不是互相替代的关系而是分工合作的关系。一句话总结2D 地图负责告诉 Nav2怎么走。3D 点云地图负责告诉重定位算法我在哪。如果只保存 2D 地图机器人能规划路线但开机后可能不知道自己在地图里的准确位置。如果只保存 3D 点云地图机器人能做点云配准和定位但 Nav2 不一定能直接拿它做成熟的 2D 路径规划。所以在我的Lidar_nav2_ws项目里建图完成后通常要保存两类地图2D 栅格地图map.yaml map.pgm 3D 点云地图xxx.pcd它们看起来都是“地图”但工作完全不同。一、先说结论2D 地图和 3D 地图不是一回事初学者最容易犯的错误就是把“地图”理解成一个东西。实际上在机器人导航系统里地图至少可以分成两种2D Occupancy Grid Map给 Nav2 用 3D Point Cloud Map给重定位 / 点云配准用简单对比一下地图类型常见文件主要用途谁用它2D 栅格地图.pgm.yaml路径规划、代价地图、避障Nav23D 点云地图.pcd点云配准、全局重定位ICP / GICP / small_gicp / KISS-MatcherLIO 局部地图内部维护短时间里程计估计FAST-LIO / Point-LIO所以你不能简单地说我有地图了为什么还要地图这就像你不能说我有身份证了为什么还要驾驶证它们都能证明点东西但证明的不是同一个东西。二、2D 地图是给 Nav2 “走路”用的先讲 2D 地图。Nav2 最经典、最成熟的工作方式是基于 2D 栅格地图做导航。常见文件是map.pgm map.yaml其中map.pgm是一张灰度图map.yaml描述地图分辨率、原点、阈值等信息。它表达的东西很简单白色可通行区域 黑色障碍物 灰色未知区域Nav2 拿到这张地图以后就可以做几件事情1. 全局路径规划 2. 局部路径跟踪 3. 代价地图构建 4. 到点导航 5. 避障控制你可以把 2D 地图理解成机器人世界里的“高德地图”。它不关心桌子的腿有几根也不关心墙壁有多高。它只关心一件事这个地方能不能走对于移动底盘来说这已经足够了。因为底盘是在地面上跑的它本质上是一个二维运动系统x y yaw也就是往前走 往后退 左转 右转它不会突然原地起飞也不会从天花板上开过去。所以Nav2 主要需要的是一张二维地图。三、那为什么 3D LiDAR 还要转成 2D LaserScan这个问题也很经典。很多人第一次看到 3D LiDAR 接 Nav2会疑惑我都用 3D 雷达了为什么还要转成 2D这不是买了 4090然后只用它打扫雷吗表面看确实有点浪费。但从工程角度看这个做法很实用。Nav2 的 2D 导航链路非常成熟。很多代价地图、局部规划器、障碍物层配置都天然适合使用LaserScan而 3D LiDAR 输出的是PointCloud2所以中间通常需要一层转换3D PointCloud2 → 高度切片 → 2D LaserScan也就是只取某个高度范围内的点云把它投影成类似 2D 激光雷达的数据。举个例子只取 z 0.1m ~ 0.6m 之间的点 过滤地面 过滤过高物体 保留真正影响底盘通行的障碍物这样 Nav2 就可以像使用普通 2D 激光雷达一样使用 3D LiDAR。这一步的价值是保留 3D LiDAR 的感知能力同时复用 Nav2 成熟的 2D 导航生态。这不是降维打击。这是降维省命。四、3D 点云地图是给机器人“找自己”用的现在讲 3D 点云地图。如果 2D 地图负责“怎么走”那么 3D 点云地图负责我现在到底在地图里的哪里这就是重定位问题。机器人开机以后LIO 可以估计相对运动我从开机位置开始向前走了 1 米 我左转了 30 度 我又向前走了 2 米但问题是开机位置本身在全局地图里的哪里如果机器人开机时不知道自己在map中的位置那么它的里程计再连续也只是“从一个未知起点开始连续”。就像你在商场里迷路了然后你打开计步器。计步器会认真告诉你你向前走了 20 米 你左转了 你又走了 10 米但它不会告诉你你现在在三楼火锅店门口这时候就需要重定位。3D 点云地图的作用就是提供一个全局参照物。机器人当前扫描到一帧点云然后和历史保存好的 PCD 地图进行匹配当前点云 vs 先验 PCD 地图如果匹配成功就能算出机器人当前在全局地图里的位姿。这一步常用的方法包括ICP GICP NDT small_gicp KISS-Matcher Scan Context在我的项目思路里比较典型的做法是KISS-Matcher 负责全局粗配准 small_gicp 负责局部细配准和持续跟踪也就是先大概猜到在哪再精细对齐。这就像你在商场里看见一家奶茶店“哦我大概在三楼东边。”然后再看旁边的电梯、扶梯、店铺门头“确定了我就在三楼 A 区。”这就是粗配准 细配准。五、为什么 2D 地图不能直接完成 3D 重定位有些人可能会问既然 2D 地图也有墙和障碍物能不能直接用它来重定位可以但效果和场景有关。2D 地图的信息量比较少。它基本只保留了哪里能走 哪里不能走很多三维结构都会被压扁。比如真实世界里有墙面 桌子 门框 立柱 柜子 楼梯边缘 玻璃门框 设备架 天花板结构这些在 3D 点云里都可能是非常明显的几何特征。但是投影到 2D 地图后很多信息会丢失。这就像你原本有一张高清 3D 人脸扫描最后压成了身份证黑白复印件。不是不能认但难度上来了。尤其是在一些长走廊、重复场景、对称环境里2D 地图很容易出现歧义这个走廊像那个走廊 这个拐角像那个拐角 这个房间门口像那个房间门口机器人看完以后可能会说我好像来过这里。但我又好像每个地方都来过。3D 点云地图保留了更多空间结构所以更适合做全局重定位和点云配准。六、那为什么 3D 点云地图不能直接给 Nav2 走路反过来再问既然 3D 点云地图信息更丰富那能不能只用 PCD不要 2D 地图理论上可以做 3D 导航但对普通移动底盘来说不一定划算。Nav2 的成熟链路主要还是围绕 2D 导航设计的2D costmap 2D global planner 2D local planner 2D footprint 2D obstacle layer而 3D PCD 原始点云并不直接等价于可通行区域。点云只告诉你这里有点 那里有点 这里可能是墙 那里可能是桌子但它不直接告诉你这块地能不能走 机器人 footprint 会不会碰撞 局部代价应该是多少 全局路径怎么规划所以通常需要把 3D 信息转换成 2D 导航可用的信息。在实际工程里一个更稳的组合是3D PCD用于全局重定位 2D map用于 Nav2 路径规划 3D LiDAR 实时点云用于障碍物感知 2D LaserScan用于 Nav2 costmap 输入这套组合非常工程化。虽然看起来多了一张地图但每一张地图都有自己的岗位。不要让 3D PCD 去干 2D map 的活。也不要让 2D map 去干 PCD 的活。这就像公司里产品经理写需求 工程师写代码 测试提 bug 老板画饼大家都有自己的分工。一旦混了项目就开始有味道了。七、完整导航链路应该怎么理解以 3D LiDAR LIO 重定位 Nav2 为例一套完整链路大概是这样LiDAR IMU ↓ FAST-LIO / Point-LIO ↓ LIO 里程计 ↓ 标准 odom → base_footprint ↓ 当前点云 / registered_scan ↓ 3D 点云地图 PCD 重定位 ↓ 发布 map → odom ↓ 2D map LaserScan ↓ Nav2 规划与控制这里最关键的是两个地图和两个 TF2D mapNav2 用来规划路线 3D PCD重定位用来确定全局位置 odom → base_footprintLIO 提供局部连续运动 map → odom重定位提供全局纠偏如果画成更清楚的形式3D PCD Map ↓ 当前点云 → 点云配准 → map → odom ↓ 2D Map → Nav2 → odom → base_footprint这就是系统闭环。LIO 让机器人知道自己怎么动。重定位让机器人知道自己在全局哪里。Nav2 让机器人知道怎么走到目标点。三个模块各干各的系统才不会互相打架。八、建图时为什么要同时保存 2D 和 3D在实际项目中建图阶段一般不仅是为了得到一张图而是为了后续导航准备完整资产。1. 保存 2D 地图2D 地图一般用于 Nav2ros2 run nav2_map_server map_saver_cli -f map_name保存后会得到map_name.yaml map_name.pgm后续 Nav2 启动时会加载它map_yaml_file: /path/to/map_name.yaml这张图负责让 Nav2 知道哪里是墙 哪里是空地 哪里可以规划路径2. 保存 3D 点云地图3D PCD 通常来自 LIO 建图结果或者注册后的点云map.pcd后续重定位模块会加载它prior_pcd_file: /path/to/map.pcd它负责让机器人知道当前扫描到的三维结构 在历史地图里的哪个位置换句话说2D 地图是导航路线图 3D 点云是定位参照物如果只保存 2D不保存 PCD机器人后续就少了一个很重要的全局重定位依据。如果只保存 PCD不保存 2DNav2 又缺少成熟的 2D 规划地图。所以两个都要。这不是冗余。这是分工。九、一个初学者最容易搞混的地方建图、定位、导航不是一回事很多新手会把这三个词混在一起建图 定位 导航它们其实是三件不同的事。建图我把环境记录下来建图阶段的目标是得到地图2D map 3D PCD map这时候机器人主要在探索环境采集传感器数据构建地图。定位我知道自己在地图哪里定位阶段的目标是估计当前位姿map → base_footprint但实际系统里通常拆成map → odom odom → base_footprint其中odom → base_footprintLIO 负责 map → odom重定位负责导航我从当前位置走到目标点导航阶段的目标是让机器人执行路径当前位置 → 目标点Nav2 需要2D 地图 TF 树 传感器障碍物数据 机器人 footprint 规划器和控制器参数所以不要看到机器人能建图就以为它能导航。也不要看到它有/odom就以为它能全局定位。这三件事是相互关联但不是一回事。就像会画地图 ≠ 知道自己在哪 知道自己在哪 ≠ 知道怎么过去 知道怎么过去 ≠ 真的能不撞墙地过去机器人很老实它不会自动脑补这些逻辑。十、如果只保存 2D 地图会发生什么假设你只保存了map.yaml map.pgm然后没有保存 PCD。Nav2 当然可以加载地图也可以显示地图。但是如果你没有可靠的全局定位来源就会遇到问题。典型现象1. 机器人开机后不知道自己在 map 中的位置 2. 需要手动给 2D Pose Estimate 3. 初始位姿给错后导航直接跑偏 4. LIO 虽然连续但整体坐标系和 map 对不上 5. 机器人觉得自己在 A 点实际上在 B 点这时候 Nav2 不是不能规划。它会认真规划。只是它以为你在另一个地方。就像你打开导航软件本来人在北京南站定位却飘到了天津。导航软件依然会很努力地给你规划路线。问题是它努力错了方向。所以 3D 重定位的意义就是让机器人开机后自动把自己对齐到全局地图中。十一、如果只保存 3D 点云地图会发生什么反过来假设你只保存了map.pcd没有保存 2D map。重定位可能可以做。点云配准可能可以成功。机器人也可能知道自己在三维地图中的位置。但 Nav2 启动时会问我的 2D 栅格地图呢你说没有但我有一坨很漂亮的点云。Nav2 说谢谢我不太会直接吃这个。然后你就需要自己把 3D 点云转换成可通行区域生成 2D costmap 或 occupancy grid。这当然可以做但你等于又绕回了从 3D PCD 生成 2D map所以实际工程里更干净的做法是建图时就同时保存 2D map 和 3D PCD一个给导航一个给定位。十二、2D 地图和 3D 点云地图之间要不要对齐要。而且非常重要。这两个地图虽然用途不同但它们必须处在同一套全局坐标关系里。理想情况下2D map 的 map 坐标系 3D PCD 的 map 坐标系 Nav2 使用的 map frame 重定位发布的 map frame这些应该是一致的。否则就会出现很抽象的问题2D 地图认为墙在这里 3D 点云认为墙在那里 重定位认为机器人在 A 点 Nav2 认为机器人在 B 点然后机器人开始表演我知道我要去哪 我也知道我在哪 但我知道的东西互相矛盾这类问题调起来非常痛苦。所以保存地图时要注意1. 2D map 和 3D PCD 来自同一次建图 2. 使用同一套 map 坐标系 3. 不要随便移动 / 旋转 PCD 4. 不要手动改 map.yaml 的 origin 后忘记同步 5. 重定位结果要和 Nav2 map 对齐地图可以有两份。世界观不能有两套。十三、实际工程中的推荐结构我个人更推荐把导航系统理解成下面这个结构/map ├── 2d/ │ ├── nav_map.yaml │ └── nav_map.pgm │ └── pcd/ └── prior_map.pcd配置里分别加载# Nav2 map server map_yaml_file: /path/to/nav_map.yaml # 3D relocalization prior_pcd_file: /path/to/prior_map.pcd启动流程可以理解成1. 启动 LIO 2. 启动 LIO bridge发布 odom → base_footprint 3. 启动 3D 重定位加载 prior_map.pcd发布 map → odom 4. 启动 2D map server加载 nav_map.yaml 5. 启动 Nav2 6. 发送目标点这套结构的核心就是2D map给 Nav2 看 3D PCD给重定位看 TF把它们串起来十四、初学者排查清单如果你正在做 3D LiDAR Nav2建议按这个顺序查。1. 你有没有保存 2D 地图应该有xxx.yaml xxx.pgm没有这两个Nav2 的 map server 通常没法正常加载静态地图。2. 你有没有保存 3D PCD应该有xxx.pcd如果你想做 3D 点云重定位这个文件就是先验地图。3. 两张地图是不是同一次建图得到的不要拿今天的 PCD 配昨天的 2D map。除非你非常确定坐标系完全一致。否则机器人可能会进入哲学状态我是谁我在哪这墙为什么长了两份4. 重定位有没有发布map → odom检查ros2 run tf2_ros tf2_echo map odom如果没有说明全局定位没有接上。5. LIO 有没有发布odom → base_footprint检查ros2 run tf2_ros tf2_echo odom base_footprint机器人运动时这个变换应该连续变化。6. Nav2 的global_frame和robot_base_frame是否正确常见配置类似global_frame: map robot_base_frame: base_footprint如果参数里写的是base_footprintTF 里就必须真的有这个 frame。不要让 Nav2 猜。Nav2 不是算命的。十五、这套“双地图”设计的真正价值同时保存 2D 地图和 3D PCD看起来多了一步但它带来的好处非常明显。1. 让 Nav2 使用成熟的 2D 导航链路Nav2 不需要直接处理复杂 3D 点云地图。它只需要加载 2D map处理 LaserScan / costmap然后规划路径。这让系统更稳定也更容易调试。2. 让重定位算法使用更丰富的 3D 几何信息3D PCD 保留了环境中的空间结构。它可以给 GICP、small_gicp、KISS-Matcher 等算法提供更强的匹配依据。这对任意位置开机重定位非常关键。3. 让系统模块之间解耦2D map 和 3D PCD 分开之后各模块职责更清楚Nav2 不管 PCD 重定位不管路径规划 LIO 不管全局地图 TF 负责把它们连接起来这就是工程系统最重要的思路每个模块只做自己擅长的事。十六、总结机器人导航里同时保存 2D 地图和 3D 点云地图并不是因为系统设计复杂也不是为了多存几个文件显得高级。根本原因是2D 地图和 3D 点云地图服务的是两个不同问题。2D 地图解决的是机器人怎么走3D 点云地图解决的是机器人在哪更准确地说2D map给 Nav2 做路径规划和代价地图 3D PCD给重定位算法做点云匹配 LIO给机器人提供连续里程计 TF把定位、里程计、导航连接起来所以一套比较完整的 3D LiDAR 导航系统通常不是只保存一张地图而是保存一套导航资产map.yaml map.pgm prior_map.pcd如果只保存 2D map机器人可能能规划但开机不知道自己在哪。如果只保存 3D PCD机器人可能能重定位但 Nav2 不一定能直接规划。两个都保存系统才真正完整。一句话记住2D 地图是给机器人走路用的3D 点云地图是给机器人认路用的。走路和认路缺一个都不太行。你可以只会走但不知道在哪那叫迷路。你可以知道在哪但不知道怎么走那叫站桩。机器人导航要做的就是既不迷路也不站桩。所以下次建图完成后别只保存map.yaml和map.pgm。记得也保存一份.pcd。不然机器人开机之后可能会非常诚实地告诉你地图我有。路我也会走。但我不知道我现在站在哪。