从点云到地图：一台扫地机器人的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 问题本质鸡生蛋问题SLAMSimultaneous Localization And Mapping的难处在于它是自举的要画地图你得知道自己在哪否则点云全是歪的要知道自己在哪你得先有地图否则没东西可参照。死循环。slam_toolbox 的解法是图优化Graph-based SLAM机器人每走一小段把当前 LiDAR 扫描存成一个节点相邻节点之间用扫描匹配scan matching估计相对位移——前后两帧点云长得最像的时候机器人大概挪了多远、转了多少除了相邻节点还会寻找回环机器人绕一圈回到老地方当前扫描和很久以前的某个节点对上了就在图里补一条回环约束所有约束丢进优化器整体调整所有节点的位置把累积误差摊平。回环检测是 SLAM 精度的生命线。没有回环轮速计扫描匹配的误差一路累积走一圈回家地图上客厅和客厅对不上能错开半米。有了回环优化误差被拉平。2.2 在 ROS2 图上长什么样slam_toolbox 订阅/scanLiDAR 的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 的经典三层结构层干什么核心组件全局规划在地图上算出 A→B 的粗路径规划器 全局代价地图局部控制实时避开突发障碍、平滑跟踪路径控制器 局部代价地图恢复行为卡住了怎么办spin / backup / drive_on_heading / wait两个代价地图costmap是理解 Nav2 的钥匙全局 costmap 基于静态地图LiDAR 更新管这条路通不通局部 costmap 基于实时/scan滚动更新管眼前 3 米内冒出来的拖鞋。机器人的外形349mm 圆盘被膨胀成 costmap 上的膨胀层——障碍物周围一圈涂成危险色规划器自动绕出安全距离。对上层模块Nav2 暴露的是标准 action 接口注意是 action 不是 topic——长时间任务需要反馈和可取消/navigate_to_posenav2_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 Assistantfloor-care沿边清扫、贴边、地面材质判断。模块按清扫这件事的子问题来切而不是按技术组件来切——每个模块的 RFC 都声明自己消费/发布哪些话题模块间只靠接口握手。这是整个项目协作开发能并行的前提也是我们系列第一课接口即契约的实例。3.3 碰撞条LiDAR 的盲区补丁2D LiDAR 有个天生盲区低于扫描平面的东西看不见——数据线、拖鞋、宠物粪便、垂下来的床单边。所以 OOMWOO 保留了机械碰撞条作为冗余。仿真里它是两个 Gazebo 接触传感器发布/bumper_left和/bumper_rightros_gz_interfaces/msg/Contacts。接口文档里有个很实在的提醒消费方要自己过滤掉与地面的接触len(msg.contacts) 0才算碰撞事件而且Gazebo 特有的解析逻辑要隔离在一个小适配器里别让它渗进跨模块契约——不然将来换硬件碰撞条一片代码跟着遭殃。记住这个组合拳LiDAR 看得见的交给规划LiDAR 看不见的交给碰撞兜底物理不可靠的交给 MCU 急停上一篇的双脑分工。三层防线各管一段。3.4 定位的另一半重定位与绑架问题建图之后日常清扫走的是已知地图定位OOMWOO 的nav-localize模块LiDAR 当前扫描与已保存地图做匹配典型如 AMCL 蒙特卡洛定位持续输出map→odom修正。这里有个经典难题叫绑架问题kidnapped robot机器人被抱到另一个房间重启或者定位彻底跑飞它对我在哪的先验全错了。解法通常分两步检测定位置信度持续过低粒子云散而不收敛就触发重定位状态——注意 OOMWOO 的开放决策清单里专门有一条定位置信度接口留给重定位和绑架检测用恢复全局重定位——拿当前扫描在整个地图上暴力搜索最优对齐找回坐标找回后回到正常导航。生活化的对应你半夜醒来发现自己在酒店房间环顾四周当前扫描认出这是酒店全局重定位然后才能规划去电梯的路线。定位丢失时诚实地报告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_*看碰撞事件。第一性调试法先确认数据的河流在流再谈算法。五、带得走的三个思考SLAM 的难点不在算法名词在误差管理——回环检测、map→odom与odom→base_link的分离全是在回答误差从哪来、往哪去导航栈是分层防御全局规划管大局、局部 costmap 管突发、恢复行为管失败、碰撞条MCU 管物理兜底。任何单层都会失败体系的价值在于失败不叠加QoS、action vs topic、cmd_vel 仲裁这些软细节恰恰是 ROS2 项目烂尾的高发区。接口契约写得越细系统活得越久。下一篇我们看一个不起眼但决定生死的子系统软件栈怎么向 STM32证明自己还活着——一套完整的心跳/健康监控设计QoS 细节抠到让你拍大腿。