从点云到地图：一台扫地机器人的SLAM与Nav2导航全链路

03-从点云到地图：一台扫地机器人的SLAM与Nav2导航全链路

引子：它凭什么知道你家长什么样

大家好，我是黒漂技术佬。

上一篇我们讲了 OOMWOO 的双脑架构——小脑 STM32 管安全，大脑 CM4 管智能。这一篇就钻进大脑，看它最核心的智能从哪来：LiDAR 扫出一圈圈点云，怎么变成你 App 里那张客厅地图？机器人怎么知道自己在地图的哪个位置、下一步该往哪开？

这套东西商业厂商包装成"激光导航 8.0"之类的营销名词，OOMWOO 把它还原成三个朴素的开源组件：slam_toolbox 建图、Nav2 导航、Gazebo 仿真。每一个都是 ROS2 生态里打磨多年的标准件。

一、感知源头：一颗 5Hz 的 2D LiDAR

整条链路的起点是一颗 2D 激光雷达，UART 接口，约 5Hz 扫描频率——每秒绕垂直轴转 5 圈，每圈输出一束水平面上的距离数据。

为什么扫地机用 2D 不用 3D？高度够用论：扫地机器人是一个贴地的圆盘（OOMWOO 基线机身直径约 349mm），它关心的障碍物——墙、桌腿、沙发脚、充电座——都是在"地面往上一个机身高度"这个薄层里。一颗 2D LiDAR 扫这个薄层，信息密度刚刚好，价格却只是 3D 雷达的零头。这也是为什么你们家扫地机顶部有个凸起的小塔：那里面是激光头，转一圈，量一遍。

LiDAR 挂在 CPU 上（不是 MCU），驱动层面有个现成的轮子值得记住：kaiaai/LDS 和 lds2d——支持 23+ 款常见 2D LiDAR 的开源库（C++/Python）。OOMWOO 直接规定"凡是这个库支持的雷达，接口即兼容"。这就是上一篇说的"一切皆可换"：传感器选型不绑死任何一家供应商。

IMU（惯性测量单元）也挂在 CPU 上，与轮式里程计互补：轮子在光滑地板上打滑时里程计会漂，IMU 的角速度和加速度能兜住短期姿态估计。两者融合出的就是/odom。

二、建图：slam_toolbox 的"边走边画"

机器人开机第一件事通常是建图——在没地图的世界里，一边遥控走，一边画图。

2.1 问题本质：鸡生蛋问题

SLAM（Simultaneous Localization And Mapping）的难处在于它是自举的：要画地图，你得知道自己在哪（否则点云全是歪的）；要知道自己在哪，你得先有地图（否则没东西可参照）。死循环。

slam_toolbox 的解法是图优化（Graph-based SLAM）：

1. 机器人每走一小段，把当前 LiDAR 扫描存成一个节点；
2. 相邻节点之间用扫描匹配（scan matching）估计相对位移——前后两帧点云长得最像的时候，机器人大概挪了多远、转了多少；
3. 除了相邻节点，还会寻找回环：机器人绕一圈回到老地方，当前扫描和很久以前的某个节点对上了，就在图里补一条"回环约束"；
4. 所有约束丢进优化器，整体调整所有节点的位置，把累积误差摊平。

回环检测是 SLAM 精度的生命线。没有回环，轮速计+扫描匹配的误差一路累积，走一圈回家，地图上"客厅"和"客厅"对不上，能错开半米。有了回环优化，误差被拉平。

2.2 在 ROS2 图上长什么样

slam_toolbox 订阅/scan（LiDAR 的sensor_msgs/msg/LaserScan）和/odom//tf（里程计），产出两样东西：

• /map话题：nav_msgs/msg/OccupancyGrid——一张栅格地图，每格标记 空闲/占用/未知（-1），这就是你 App 里看到的地图本体；
• /tf里map → odom的变换：修正过的位姿。

整个机器人身上的坐标系链条是一条 TF 树：

map ──(slam_toolbox/AMCL)──> odom ──(轮式里程计)──> base_footprint ──> base_link ──> base_scan(LiDAR) · base_imu · 各轮子 ...

map→odom和odom→base_link分成两段，是 ROS2 导航体系的精髓：odom系局部连续、短时准确但长时漂移；map→odom这一段由 SLAM/定位节点持续修正漂移。局部平滑性和全局准确性解耦，各管各的。第一次做 ROS 导航的人在这里栽跟头最多——把两个变换捏在一个节点里发，TF 冲突到怀疑人生。

另外注意 OOMWOO 接口文档里的一个细节：/map要求transient-local QoS——晚加入的节点（比如后来才启动的 App 或作业模块）也能收到最新地图，而不是干等下一帧。建图产物是"状态"不是"流"，QoS 要跟着语义走。

三、导航：Nav2 的"给定地图怎么走"

有了地图，进入第二阶段：自主导航。这一层 OOMWOO 几乎全盘复用 Nav2——ROS2 官方导航栈，也是整个行业的事实标准。

3.1 分层决策：全局规划、局部避障、恢复行为

Nav2 的经典三层结构：

两个代价地图（costmap）是理解 Nav2 的钥匙：全局 costmap 基于静态地图+LiDAR 更新，管"这条路通不通"；局部 costmap 基于实时/scan滚动更新，管"眼前 3 米内冒出来的拖鞋"。机器人的外形（349mm 圆盘）被膨胀成 costmap 上的膨胀层——障碍物周围一圈涂成危险色，规划器自动绕出安全距离。

对上层模块，Nav2 暴露的是标准 action 接口（注意是 action 不是 topic——长时间任务需要反馈和可取消）：

• /navigate_to_pose（nav2_msgs/action/NavigateToPose）：去一个点；
• /navigate_through_poses：依次经过一组点——覆盖清扫和回充接近都会用到它。

还有一条铁律写在接口文档里：任何想直接控制/cmd_vel的模块，必须定义它与 Nav2 和恢复节点之间的仲裁（arbitration）方式。两个节点同时往/cmd_vel上发速度指令、互相打架，是 ROS 机器人最经典的翻车现场。cmd_vel 只能有仲裁后的一个写者。

3.2 扫地机导航的特殊性：去"点"容易，扫"面"难

这里要点破一个认知：Nav2 解决的是"点到点"，而扫地机的本职是"面覆盖"。

把客厅每个角落都扫到，是**覆盖路径规划（coverage path planning）**问题：把已建好的地图切分成区域，在区域内生成弓字形（boustrophedon）往返路径，用NavigateThroughPoses逐段执行。OOMWOO 把这块拆成了独立的软件模块：

• clean-and-map：首遍覆盖清扫 + 完整建图 + 完成判定 + 地图保存；
• nav-localize：已有地图下的定位与导航、重定位、断点续扫；
• cleaning-jobs：房间分区、清扫任务状态机（适合未来对接 Home Assistant）；
• floor-care：沿边清扫、贴边、地面材质判断。

模块按"清扫这件事的子问题"来切，而不是按技术组件来切——每个模块的 RFC 都声明自己消费/发布哪些话题，模块间只靠接口握手。这是整个项目协作开发能并行的前提，也是我们系列第一课"接口即契约"的实例。

3.3 碰撞条：LiDAR 的盲区补丁

2D LiDAR 有个天生盲区：低于扫描平面的东西看不见——数据线、拖鞋、宠物粪便、垂下来的床单边。所以 OOMWOO 保留了机械碰撞条作为冗余。

仿真里它是两个 Gazebo 接触传感器，发布/bumper_left和/bumper_right（ros_gz_interfaces/msg/Contacts）。接口文档里有个很实在的提醒：消费方要自己过滤掉与地面的接触，len(msg.contacts) > 0才算碰撞事件，而且Gazebo 特有的解析逻辑要隔离在一个小适配器里，别让它渗进跨模块契约——不然将来换硬件碰撞条，一片代码跟着遭殃。

记住这个组合拳：LiDAR 看得见的交给规划，LiDAR 看不见的交给碰撞兜底，物理不可靠的交给 MCU 急停（上一篇的双脑分工）。三层防线各管一段。

3.4 定位的另一半：重定位与"绑架"问题

建图之后，日常清扫走的是已知地图定位（OOMWOO 的nav-localize模块）：LiDAR 当前扫描与已保存地图做匹配（典型如 AMCL 蒙特卡洛定位），持续输出map→odom修正。

这里有个经典难题叫绑架问题（kidnapped robot）：机器人被抱到另一个房间重启（或者定位彻底跑飞），它对"我在哪"的先验全错了。解法通常分两步：

1. 检测：定位置信度持续过低（粒子云散而不收敛）就触发重定位状态——注意 OOMWOO 的开放决策清单里专门有一条"定位置信度接口"，留给重定位和绑架检测用；
2. 恢复：全局重定位——拿当前扫描在整个地图上暴力搜索最优对齐，找回坐标，找回后回到正常导航。

生活化的对应：你半夜醒来发现自己在酒店房间，环顾四周（当前扫描）认出这是酒店（全局重定位），然后才能规划去电梯的路线。定位丢失时诚实地报告LOCALIZATION_LOST并停下，比自信地继续开要可靠得多——这一点和 04 篇的心跳哲学一脉相承：不确定，就是最大的不安全。

四、仿真优先：没有机器人也能开发全套软件

OOMWOO 的第二设计原则"仿真优先"落到实处，就是 Gazebo + URDF：

• 机器人描述包oomwoo-one里有完整的 URDF（~349mm 圆形机身、LiDAR、轮系、碰撞条），在 Gazebo 里 1:1 还原；
• 配套一批住宅户型世界（residential-layout worlds），专门用来测导航和覆盖；
• 提供proscenic-m6pro替身真机教程——拿一台真实的商业扫地机 Proscenic M6 Pro 接进 ROS2，在真硬件上跑同一套逻辑。

仿真优先的深层价值不只是省钱：同一套 ROS2 接口契约，仿真和真机必须一模一样（接口文档专门列了live-robot-bringup模块来验证这件事）。于是"虚拟客厅里跑通的算法"和"真机上跑通的算法"之间，只隔一个硬件桥。算法开发、测试、CI 全部可以无人化。

把机器人在 Gazebo 里跑起来只要几条命令：

ros2 topic list ros2 topicecho/scan--onceros2 topicecho/odom--onceros2 topicecho/bumper_left ros2 run tf2_tools view_frames

这是 OOMWOO 官方的模块验证清单——/scan看雷达数据有没有来，/tf看坐标树有没有接对，/bumper_*看碰撞事件。第一性调试法：先确认数据的河流在流，再谈算法。

五、带得走的三个思考

1. SLAM 的难点不在算法名词，在误差管理——回环检测、map→odom与odom→base_link的分离，全是在回答"误差从哪来、往哪去"；
2. 导航栈是分层防御：全局规划管大局、局部 costmap 管突发、恢复行为管失败、碰撞条+MCU 管物理兜底。任何单层都会失败，体系的价值在于失败不叠加；
3. QoS、action vs topic、cmd_vel 仲裁这些"软细节"，恰恰是 ROS2 项目烂尾的高发区。接口契约写得越细，系统活得越久。

下一篇我们看一个不起眼但决定生死的子系统：软件栈怎么向 STM32"证明自己还活着"——一套完整的心跳/健康监控设计，QoS 细节抠到让你拍大腿。