自己做了一台扫地机器人底盘从零把Realsense D435的原始点云数据一路喂进SLAM建图最后用Nav2让它自己规划路径、绕过障碍物。这篇文章想把这条链路完整记录下来包括我选的方案、踩过的坑、调过的参数以及每次调整背后的原因。适合正在做ROS机器人、想把视觉传感器和导航栈打通的朋友也适合准备入手slam_toolbox和Nav2、但被各种概念绕晕的初学者。先说结论用D435这类结构光相机做扫地机器人的SLAM和导航完全可行但有一个前提——你必须理解点云处理和地图表达之间的关系。很多人一上来就想着怎么把3D点云做得漂亮结果导航时发现代价地图一塌糊涂真正决定导航性能的往往是那个不起眼的点云转2D scan环节。这篇文章不会只讲怎么跑通demo更会把每个关键选型背后的道理讲清楚。1. 全链路拆解从“看得见”到“走得动”1.1 一台扫地机器人的“感知三角”扫地机器人本质上是一个移动机器人它做任何决策前脑子里要有三个东西我在哪、周围有什么、我该怎么走。这三个问题分别对应SLAM中的定位与建图、感知中的障碍物检测、导航中的路径规划而它们的共同起点就是传感器数据。在传感器选型上主流方案有三条路单线激光雷达、多线激光雷达、深度相机。单线激光雷达价格便宜、点云密度低但精度稳定很多商业扫地机都在用多线激光雷达贵但能提供完整3D信息适合户外复杂环境深度相机在这三者之间比较折中——价格不高能拿到RGB图和深度图通过内参可以恢复成3D点云。我最终选了Realsense D435因为它在室内场景下工作得非常好0.3米左右的最小深度距离很适合扫地机器人这种需要贴近障碍物的场景。它的红外主动立体视觉方案本质上就是结构光的一种变体通过投射红外纹理图案并计算左右红外图像的视差来获取深度所以对纯色墙面、白墙这类缺乏纹理的区域比单纯的双目相机要稳得多。1.2 方案选型的底层逻辑2D地图还是3D地图这是整个链路里最容易被忽略、也最影响后续工作量的一个决定。扫地机器人的成本、算力、电机控制方式都决定了它不太适合直接做全3D SLAM和3D导航。主流做法是把3D信息“降维”用深度相机获取3D点云然后把点云滤波干净转成2D激光扫描数据喂给2D SLAM和2D代价地图。这么做的原因有三2D SLAM在平面场景里的成熟度极高闭环检测、全局优化这些算法经过多年工程验证稳定性远超很多3D方案。扫地机器人底盘的运动被约束在地平面障碍物在某个高度范围内的分布就足以决定通行性不需要把所有几何信息都保留。算力开销小。树莓派或者低端x86板子跑2D SLAM绰绰有余但跑3D体素建图就吃力了。当然2D降维会丢失信息比如低矮的毛毯边缘、桌面下的横梁可能被滤波参数误伤。这个问题的解法不是靠SLAM而是靠后续导航时的传感器融合——把3D点云保留一部分用于局部避障逻辑。我后面会在Nav2部分具体讲怎么处理。2. 点云获取与预处理相机到障碍物坐标系的最后一米2.1 D435的安装与参数配置如果你是第一次用D435建议先把librealsense2装上然后跑一下官方的realsense-viewer确认深度流正常。ROS环境里用realsense2-camera这个驱动包把相机数据桥接到ROS话题上输出的基础设施包括/camera/depth/image_rect_raw、/camera/depth/color_aligned_color等。我实际用的启动参数arg namedepth_width default848/ arg namedepth_height default480/ arg namedepth_fps default30/ arg nameenable_pointcloud defaulttrue/ arg namealign_depth defaulttrue/分辨率选848×480是因为它和30fps组合时USB3.0带宽占用合理点云密度也够建图用。很多人贪分辨率开到1280×720结果帧率掉到15fps以下SLAM前端里程计匹配时出现明显残留误差。我自己的经验是建图时宁可用低一点的分辨率保证帧率稳定也不要让帧率忽高忽低。相机需要标定外参。把D435固定到扫地机器人底盘正面用Kalibr或者简单的手眼标定方法算出相机坐标系到base_link的变换矩阵。不标定也经常能跑但地图会和真实环境有明显旋转偏移后续导航路径看着很别扭。提示D435的深度有效范围大约在0.28m到3m之间。扫地机器人贴地工作相机安装高度最好在10cm到30cm之间太高会把桌面和层板扫进来太低又会丢失远处信息。2.2 点云转2D scan之前必须做的三步滤波结构光相机的点云不能用真的一点都不能直接用。红外纹理在强光下会失效黑色物体吸收红外导致深度空洞镜面和玻璃则直接产生飞点。我总结了一个固定的预处理流水线用PCL的ROS封装pcl_ros实现第一步直通滤波只保留高度在0.1m到0.5m之间的点云。扫地机器人关心的障碍物基本在这个区间低于底盘的可以直接无视高于半米的值对2D导航没有任何意义只会让后续scan匹配更脏。第二步体素滤波降采样leaf size设0.02m到0.03m。这一步的作用是均匀化点云密度避免近处点云过密导致匹配权重失衡。D435一个点云帧常常有十万个点分割到scan数据也用不到这么多降采样后速度能快一个数量级。第三步统计离群点去除。用PCL的StatisticalOutlierRemovalmean k设为50stddev设为1.5。这一步会把那些孤立飞点清掉尤其是镜面和玻璃边缘造成的假障碍。这三步之后点云才算能转成scan。我见过很多人跳过硬是把原始点云塞给pointcloud_to_laserscan结果生成的scan数据里全是毛刺地图建出来像起了雾一样。2.3 pointcloud_to_laserscan的另一个隐藏参数点云转laserscan的包是scan_tools里的pointcloud_to_laserscan常规用法是设一下target_frame、min_height、max_height就够了。但实际操作中还有一个重要参数use_inf。很多默认配置把use_inf设为false这意味着超出传感器距离的点会被丢弃而不是标成inf导致生成的scan范围不满360度。SLAM前端在匹配scan的时候空旷方向的缺失反而可能造成错误约束。我在建图时把use_inf设为true让超出深度的方向明确表示为无穷远匹配算法的鲁棒性会好很多。还需要设置angle_min和angle_max。点云转成scan时原本是X轴正方向作为0度按ROS标准逆时针展开如果你的相机装在机器人前方那么scan对机器人来说就只有前向180度左右的视野。这一点必须在接入SLAM前想清楚因为后面所有建图、导航都依赖scan在哪个坐标系下、覆盖范围是多少。3. slam_toolbox建图把点云熬成一张能用的栅格地图3.1 为什么选slam_toolboxROS环境下2D SLAM的选择有好几个gmapping、cartographer、slam_toolbox、hector_slam。我没有选gmapping因为它虽然是经典粒子滤波方案但长时间建图会出现粒子耗散问题地图容易糊测绘性能更强的cartographer则是配置复杂度高对不是专门做SLAM的人来说学习曲线太抖。slam_toolbox其实是基于KartoSLAM的优化版本做的是scan-to-map匹配加位姿图优化。它最大的优势是支持大规模的闭环检测在建图过程中能不断回到前序轨迹把累积漂移拉回去。这一点对扫地机器人特别重要因为扫地机器人是回字形清扫路线经常要绕同一个区域转好几圈没有闭环修正的话地图很快就开始错位。3.2 建图时的核心参数没有一套参数能通吃所有环境但我会从这套基线开始调slam_toolbox: ros__parameters: max_laser_range: 2.5 minimum_time_interval: 0.0 minimum_travel_distance: 0.2 minimum_travel_heading: 0.2 scan_buffer_size: 100 resolution: 0.05 loop_search_resolution: 0.05 loop_search_angle_resolution: 0.005 loop_match_minimum_response_coh: 0.35 correlation_search_space_resolution: 0.02 correlation_search_space_smear_deviation: 0.04这里逐个说为什么max_laser_range为什么取2.5而不是直接拿传感器的最大量程这其实关系到地图表示。栅格地图的栅格尺寸是0.05m也就是5cm一格。如果max_laser_range设太大远处点云的离散误差会被放大扫描匹配时相关性计算的窗口也会变大导致计算变慢。取2.5m是大多数室内扫地机器人建图时的合理折中远一点能覆盖客厅对角线近一点能保证匹配精度。minimum_travel_distance和minimum_travel_heading是控制建图频率的。扫地机器人原地转向时会出现大量相同位姿的scan这些scan的不同角度会产生冗余位姿图节点白白增加优化负担。我设成0.2m和0.2rad也就是至少移动20厘米或转11度才插入一个新节点大幅减少位姿图规模建图速度提升了接近一倍。loop_match_minimum_response_coh是闭环匹配的最低响应值。这个值设太大会导致闭环检测极其保守明明已经回到原点也会因为一点噪声而拒绝闭环设太小又会出现错误闭环把两块本来不在同一个位置的地图强行粘到一起。0.35是我在普通家居环境实测比较稳的值如果你的环境模式化严重可以微调到0.3左右。3.3 建图过程中的操作心得跑slam_toolbox建图时要牢记一个原则先走边界再走内部。如果一开始就直接在房间中心转圈闭环检测会一团糟因为scan匹配的初始猜测完全依赖里程计而里程计在转向时往往有较大漂移。我推荐的操作路线是先把所有房间的外围走一遍回到起点完成闭环然后再进入房间内部来回扫。这样做的原因是让位姿图先形成一个“骨架”主循环闭合成一个大回路大回路的全局误差被闭环检测压制后再叠加内部细节时误差会控制在小范围内。建图时速度也很关键底盘速度我控制在0.15m/s到0.25m/s。速度快了scan之间重叠度不够扫描匹配容易丢失太慢又会拖长时间。D435在30fps下点云帧率足够快但slam_toolbox的scan来源如果是80线甚至更低的2D scan每帧scan之间的重叠度主要取决于移动距离而不只是帧率。建完图后记得保存地图时用以下命令ros2 run nav2_map_server map_saver_cli -f map保存下来的map.pgm和map.yaml就是后续导航的地图输入。4. Nav2导航从静态地图到动态避障4.1 导航栈的基本框架Nav2的逻辑可以简单理解为机器人拿着地图静态图层实时叠加传感器看到的障碍障碍物图层点上人行道膨胀图层然后全局路径规划器找一条从起点到目标点的路径局部路径规划器在跟踪这条路径的同时实时避障最后输出速度指令。我搭建时用了这几个组件map_server加载建图阶段保存的map.yamlAMCL负责定位。因为slam_toolbox建出的地图没有包含实时定位功能导航需要单独的定位模块planner全局规划器我用的是NavFncontroller局部控制器我用的是DWBAMCL的作用经常被新人低估。建图时slam_toolbox会在位姿图里优化整个轨迹获得全局一致的地图但是导航时机器人并没有在全局地图中准确知道自己在哪AMCL通过粒子滤波根据当前scan和地图的匹配程度估计位姿。把AMCL的初始位姿估计错后面所有导航都会崩。4.2 行为树给导航加上恢复逻辑Nav2和早期move_base最大的不同就是引入了行为树。行为树BT是一种比状态机更灵活、比有限自动机更可组合的控制逻辑它能让机器人在遇到问题时“有策略地尝试恢复”而不是卡住或原地崩溃。我用的默认BT文件是navigate_to_pose.xml它的核心逻辑可以拆成四段先用ComputePathToPose计算出全局路径再交给FollowPath循环执行每次循环检查controller是否超时超时则进入恢复流程先清除代价地图上的障碍物再旋转360度重新规划最后才会让整个导航失败这个“先清除再旋转最后报错”的顺序很重要。实际环境中大量导航失败是传感器瞬时噪声导致代价地图上出现假障碍清一次地图往往就好了。直接放弃或者原地乱转都是错误设计。用Groot2这个工具可以可视化行为树把树节点拖出来看状态排查导航失败时到底卡死在哪个节点上这是最节省时间的调试方式。4.3 代价地图配置里最容易翻车的三个参数代价地图层有很多参数但真正决定扫地机器人导航体感的就三个第一个是obstacle_range。它决定了传感器数据在多远距离会被标记为障碍物。设太大噪声更容易混进来设太小机器人对远处障碍反应迟钝。我用的是3.0m和slam_toolbox的max_laser_range保持一致性。第二个是raytrace_range。它是自由空间标记范围一般要比obstacle_range大一点大约4.0m确保传感器扫描路径上的空间被标记为可通行。如果这里设得比obstacle_range还小会出现障碍物和自由空间同时存在的自相矛盾。第三个是inflation_radius。这个值决定障碍物周围多少范围会被设置为“靠近障碍但还可通行”的高成本区域。对于扫地机器人这种小底盘inflation_radius设到0.25m就够设太大机器人会离墙远远的导致清扫覆盖率下降设太小又会频频贴边导航路径看起来有点惊险。costmap的local_costmap我特意把width和height设成2.0m乘以2.0m分辨率0.05m。这个尺寸对扫地机器人来说足够覆盖车周三米范围又不至于让局部代价地图的计算开销影响到DWB的高频控制。4.4 3D激光雷达和深度相机在Nav2中的接入差异如果是多线激光雷达习惯做法是直接把激光数据喂给costmap的obstacle层。但如果你用的是像Livox那样旋转频率不规律的3D雷达或者像我这样用深度相机点云就不能直接塞给costmap因为costmap_2d的obstacle层默认只订阅sensor_msgs/LaserScan。接入方式有两种常见选择第一种把3D点云转成2D scan再喂给Nav2的所有层。优点是兼容性极好代码改动少slam_toolbox建图和Nav2导航共用同一条数据管线一致性高。缺点是可调节性差高度过滤一旦固定局部代价地图就无法再细分。第二种用costmap_3d或者把点云投影出来的多层2D scan送到不同的costmap层。扫地机器人用这种方式可以做“低层费用层”和“高层费用层”比如低层超过5cm的障碍都算障碍高层只看30cm以上的部分这样桌面边缘不会被当作不可通行区域。我实际用的是第一种加一个额外的本地3D点云订阅器——全局代价地图走2D scan局部代价地图额外订阅一份降采样后的3D点云。这样导航规划时主要依据2D scan做长期规划局部避障却有3D体感可以处理悬空障碍物。5. 实战里踩过的坑与排查方法5.1 建图漂移和地图糊掉现象是地图整体看起来像被“揉皱”过一样某些墙壁出现双层线或扭曲。排查思路按这个顺序来先看里程计数据是不是干净的。扫地机器人通常用轮式里程计D435也可以做视觉里程计估计两者融合后数据是否稳定。我在跑slam_toolbox前用一个rviz终端盯着/odom话题的位姿轨迹如果车子走直线但odom轨迹画出曲线说明里程计标定有问题怎么调SLAM都救不回来。再看scan数据质量。打开rviz把/scan话题显示出来看有没有满屏的杂散点。我这里遇到过一个典型案例因为D435安装在底盘侧面点云转scan后把扫地机底盘的边沿扫进来了scan数据里永远有一堆固定的“假障碍”地图建出来像面条一样蜿蜒。解决办法是调发pointcloud_to_laserscan的min_height参数把底盘以上区域排除掉。5.2 Nav2导航时高频左右摆动这个现象特别典型机器人沿着路径走但车头左右来回晃速度指令一突一突。很多人第一反应是控制器PID参数没调好但在我这个场景里导致抖动的主因是局部代价地图的分辨率和DWB控制频率之间不匹配。解决办法是先把local_costmap的resolution从0.05m降到0.02m这样局部规划器能发现更细的通路再把DWB的max_vel_trans从0.25m/s降到0.15m/smin_vel_trans同样调低这样机器人在规划路径附近不会做过猛的速度变化。另外还要检查AMCL的粒子滤波参数。粒子数太少或者更新频率过低会导致定位精度在时间上忽高忽低反应到路径上就是抖动。AMCL粒子数我设的是2000个对于扫地机器人这种小底盘足够了。5.3 行为树一直卡在ComputePathToPose如果导航任务发出去之后机器人完全没有反应而且Groot2里看到ComputePathToPose节点一直是RUNNING状态问题通常不在行为树而在于全局代价地图。这个典型的原因是全局代价地图的obstacle层里把传感器数据的世界坐标换算错了。我排查时先看rviz里的全局costmap发现整个地图亮了一片红色——障碍物铺满。此时是点云转scan时把坐标系写错了scan本来应该发布在base_link坐标系我误设成odom导致传感器数据全被往世界坐标方向偏置了一截。解决方法是统一所有传感器的frame_id并且用tf2工具检查transform树是否正确ros2 run tf2_tools view_frames看到完整的tf树之后再对照rviz里显示的点云和scan能否正确叠加在代价地图上这种坐标错位的问题能快速定位。5.4 局部代价地图上不断出现闪烁的障碍物D435这类结构光相机在遇到玻璃、镜面或者反光地板时会出现深度空洞和飞点。这些飞点转成scan后表现为局部代价地图上的“幽灵障碍”一会儿消失一会儿出现机器人路径规划会频繁重新规划。我的解决办法是做两级防护第一级是预处理时把点云的置信度过滤加上。D435的深度图可以通过设置深度预设或者开启temporal filter短期时间平滑能够抑制大多数随机噪声点。realsense2-camera中我把initializer设置为2并把frames_queue_size加到4飞点明显减少。第二级是在Nav2的obstacle层里加上observation_filter。参数可以用observation_sources: scan scan: topic: /scan data_type: LaserScan obstacle_range: 3.0 raytrace_range: 4.0 inf_is_valid: true observation_persistence: 0.0 min_obstacle_height: 0.05 max_obstacle_height: 0.6这里min_obstacle_height设成0.05m地面的小石子、硬币不会被标记成障碍物导航对这类微小凸起不再有激烈反应。如果你发现障碍物闪烁依旧强烈可以考虑把observation_persistence设置成0.5它会让障碍物在消失后继续保留0.5秒给局部规划器更多的缓冲时间。5.5 建图效果好导航却频繁撞墙如果建图过程中地图看起来清晰准确但导航时机器人在某个特定区域总是规划失败或贴近墙体大概率不是SLAM的问题而是建的这张地图里包含了建图时传感器噪声的痕迹。建图时如果有人走来走去、门开开关关栅格地图上会留下“闪电状”的痕迹。这些痕迹在导航时会被代价地图当作可通行区域的边缘计划器在其中穿插路径就会走得很偏。最简单的修法是把这些区域在地图编辑器里抹掉或者重新建图时让人撤离。更工程化的做法是在导航定位阶段给AMCL加一个static_map的更新阈值过低置信度的地图区域会被实时传感器数据覆盖掉。这个参数藏在AMCL的update_min_d和update_min_a里调低后地图更新会更频繁代价是算力开销变大。最后再分享一个经验这台扫地机器人从硬件选型到导航跑通前后花了我大概三周时间每天调参、跑图、回放bag数据。走完这条全链路之后我的体会是真正决定系统稳不稳的不是某一个环节而是每个环节之间数据流是否干净。点云转scan时多滤一步后面SLAM的匹配就很轻松SLAM建图时多注意几步闭环导航时的代价地图就干净得多。SLAM和Nav2的报错信息往往不会直说病根你需要顺着数据流一层层往回查——先查传感器再查预处理再查TF变换最后才动算法参数。如果你打算在这个基础上继续做扩展我建议优先做两件事。一是把建图阶段的数据保存成rosbag这样每次调参都能用同一份数据反复回放不用每次都推着机器人在家里跑一遍调试速度快非常多。二是尝试给机器人加一个顶部相机或者固定的3D点云订阅器把高处的悬空障碍物也纳进局部代价地图扫地机器人在低矮家具下行驶时会从容很多。建图导航这条路没有太多玄学每一个现象背后都会有明确的链路原因剩下的事情就是耐心排查。 
长河君 · 2026/10/5 1:09:12 · 阅读 1.2k 从D435点云到Nav2：扫地机器人SLAM与导航全记录
自己做了一台扫地机器人底盘从零把Realsense D435的原始点云数据一路喂进SLAM建图最后用Nav2让它自己规划路径、绕过障碍物。这篇文章想把这条链路完整记录下来包括我选的方案、踩过的坑、调过的参数以及每次调整背后的原因。适合正在做ROS机器人、想把视觉传感器和导航栈打通的朋友也适合准备入手slam_toolbox和Nav2、但被各种概念绕晕的初学者。先说结论用D435这类结构光相机做扫地机器人的SLAM和导航完全可行但有一个前提——你必须理解点云处理和地图表达之间的关系。很多人一上来就想着怎么把3D点云做得漂亮结果导航时发现代价地图一塌糊涂真正决定导航性能的往往是那个不起眼的点云转2D scan环节。这篇文章不会只讲怎么跑通demo更会把每个关键选型背后的道理讲清楚。1. 全链路拆解从“看得见”到“走得动”1.1 一台扫地机器人的“感知三角”扫地机器人本质上是一个移动机器人它做任何决策前脑子里要有三个东西我在哪、周围有什么、我该怎么走。这三个问题分别对应SLAM中的定位与建图、感知中的障碍物检测、导航中的路径规划而它们的共同起点就是传感器数据。在传感器选型上主流方案有三条路单线激光雷达、多线激光雷达、深度相机。单线激光雷达价格便宜、点云密度低但精度稳定很多商业扫地机都在用多线激光雷达贵但能提供完整3D信息适合户外复杂环境深度相机在这三者之间比较折中——价格不高能拿到RGB图和深度图通过内参可以恢复成3D点云。我最终选了Realsense D435因为它在室内场景下工作得非常好0.3米左右的最小深度距离很适合扫地机器人这种需要贴近障碍物的场景。它的红外主动立体视觉方案本质上就是结构光的一种变体通过投射红外纹理图案并计算左右红外图像的视差来获取深度所以对纯色墙面、白墙这类缺乏纹理的区域比单纯的双目相机要稳得多。1.2 方案选型的底层逻辑2D地图还是3D地图这是整个链路里最容易被忽略、也最影响后续工作量的一个决定。扫地机器人的成本、算力、电机控制方式都决定了它不太适合直接做全3D SLAM和3D导航。主流做法是把3D信息“降维”用深度相机获取3D点云然后把点云滤波干净转成2D激光扫描数据喂给2D SLAM和2D代价地图。这么做的原因有三2D SLAM在平面场景里的成熟度极高闭环检测、全局优化这些算法经过多年工程验证稳定性远超很多3D方案。扫地机器人底盘的运动被约束在地平面障碍物在某个高度范围内的分布就足以决定通行性不需要把所有几何信息都保留。算力开销小。树莓派或者低端x86板子跑2D SLAM绰绰有余但跑3D体素建图就吃力了。当然2D降维会丢失信息比如低矮的毛毯边缘、桌面下的横梁可能被滤波参数误伤。这个问题的解法不是靠SLAM而是靠后续导航时的传感器融合——把3D点云保留一部分用于局部避障逻辑。我后面会在Nav2部分具体讲怎么处理。2. 点云获取与预处理相机到障碍物坐标系的最后一米2.1 D435的安装与参数配置如果你是第一次用D435建议先把librealsense2装上然后跑一下官方的realsense-viewer确认深度流正常。ROS环境里用realsense2-camera这个驱动包把相机数据桥接到ROS话题上输出的基础设施包括/camera/depth/image_rect_raw、/camera/depth/color_aligned_color等。我实际用的启动参数arg namedepth_width default848/ arg namedepth_height default480/ arg namedepth_fps default30/ arg nameenable_pointcloud defaulttrue/ arg namealign_depth defaulttrue/分辨率选848×480是因为它和30fps组合时USB3.0带宽占用合理点云密度也够建图用。很多人贪分辨率开到1280×720结果帧率掉到15fps以下SLAM前端里程计匹配时出现明显残留误差。我自己的经验是建图时宁可用低一点的分辨率保证帧率稳定也不要让帧率忽高忽低。相机需要标定外参。把D435固定到扫地机器人底盘正面用Kalibr或者简单的手眼标定方法算出相机坐标系到base_link的变换矩阵。不标定也经常能跑但地图会和真实环境有明显旋转偏移后续导航路径看着很别扭。提示D435的深度有效范围大约在0.28m到3m之间。扫地机器人贴地工作相机安装高度最好在10cm到30cm之间太高会把桌面和层板扫进来太低又会丢失远处信息。2.2 点云转2D scan之前必须做的三步滤波结构光相机的点云不能用真的一点都不能直接用。红外纹理在强光下会失效黑色物体吸收红外导致深度空洞镜面和玻璃则直接产生飞点。我总结了一个固定的预处理流水线用PCL的ROS封装pcl_ros实现第一步直通滤波只保留高度在0.1m到0.5m之间的点云。扫地机器人关心的障碍物基本在这个区间低于底盘的可以直接无视高于半米的值对2D导航没有任何意义只会让后续scan匹配更脏。第二步体素滤波降采样leaf size设0.02m到0.03m。这一步的作用是均匀化点云密度避免近处点云过密导致匹配权重失衡。D435一个点云帧常常有十万个点分割到scan数据也用不到这么多降采样后速度能快一个数量级。第三步统计离群点去除。用PCL的StatisticalOutlierRemovalmean k设为50stddev设为1.5。这一步会把那些孤立飞点清掉尤其是镜面和玻璃边缘造成的假障碍。这三步之后点云才算能转成scan。我见过很多人跳过硬是把原始点云塞给pointcloud_to_laserscan结果生成的scan数据里全是毛刺地图建出来像起了雾一样。2.3 pointcloud_to_laserscan的另一个隐藏参数点云转laserscan的包是scan_tools里的pointcloud_to_laserscan常规用法是设一下target_frame、min_height、max_height就够了。但实际操作中还有一个重要参数use_inf。很多默认配置把use_inf设为false这意味着超出传感器距离的点会被丢弃而不是标成inf导致生成的scan范围不满360度。SLAM前端在匹配scan的时候空旷方向的缺失反而可能造成错误约束。我在建图时把use_inf设为true让超出深度的方向明确表示为无穷远匹配算法的鲁棒性会好很多。还需要设置angle_min和angle_max。点云转成scan时原本是X轴正方向作为0度按ROS标准逆时针展开如果你的相机装在机器人前方那么scan对机器人来说就只有前向180度左右的视野。这一点必须在接入SLAM前想清楚因为后面所有建图、导航都依赖scan在哪个坐标系下、覆盖范围是多少。3. slam_toolbox建图把点云熬成一张能用的栅格地图3.1 为什么选slam_toolboxROS环境下2D SLAM的选择有好几个gmapping、cartographer、slam_toolbox、hector_slam。我没有选gmapping因为它虽然是经典粒子滤波方案但长时间建图会出现粒子耗散问题地图容易糊测绘性能更强的cartographer则是配置复杂度高对不是专门做SLAM的人来说学习曲线太抖。slam_toolbox其实是基于KartoSLAM的优化版本做的是scan-to-map匹配加位姿图优化。它最大的优势是支持大规模的闭环检测在建图过程中能不断回到前序轨迹把累积漂移拉回去。这一点对扫地机器人特别重要因为扫地机器人是回字形清扫路线经常要绕同一个区域转好几圈没有闭环修正的话地图很快就开始错位。3.2 建图时的核心参数没有一套参数能通吃所有环境但我会从这套基线开始调slam_toolbox: ros__parameters: max_laser_range: 2.5 minimum_time_interval: 0.0 minimum_travel_distance: 0.2 minimum_travel_heading: 0.2 scan_buffer_size: 100 resolution: 0.05 loop_search_resolution: 0.05 loop_search_angle_resolution: 0.005 loop_match_minimum_response_coh: 0.35 correlation_search_space_resolution: 0.02 correlation_search_space_smear_deviation: 0.04这里逐个说为什么max_laser_range为什么取2.5而不是直接拿传感器的最大量程这其实关系到地图表示。栅格地图的栅格尺寸是0.05m也就是5cm一格。如果max_laser_range设太大远处点云的离散误差会被放大扫描匹配时相关性计算的窗口也会变大导致计算变慢。取2.5m是大多数室内扫地机器人建图时的合理折中远一点能覆盖客厅对角线近一点能保证匹配精度。minimum_travel_distance和minimum_travel_heading是控制建图频率的。扫地机器人原地转向时会出现大量相同位姿的scan这些scan的不同角度会产生冗余位姿图节点白白增加优化负担。我设成0.2m和0.2rad也就是至少移动20厘米或转11度才插入一个新节点大幅减少位姿图规模建图速度提升了接近一倍。loop_match_minimum_response_coh是闭环匹配的最低响应值。这个值设太大会导致闭环检测极其保守明明已经回到原点也会因为一点噪声而拒绝闭环设太小又会出现错误闭环把两块本来不在同一个位置的地图强行粘到一起。0.35是我在普通家居环境实测比较稳的值如果你的环境模式化严重可以微调到0.3左右。3.3 建图过程中的操作心得跑slam_toolbox建图时要牢记一个原则先走边界再走内部。如果一开始就直接在房间中心转圈闭环检测会一团糟因为scan匹配的初始猜测完全依赖里程计而里程计在转向时往往有较大漂移。我推荐的操作路线是先把所有房间的外围走一遍回到起点完成闭环然后再进入房间内部来回扫。这样做的原因是让位姿图先形成一个“骨架”主循环闭合成一个大回路大回路的全局误差被闭环检测压制后再叠加内部细节时误差会控制在小范围内。建图时速度也很关键底盘速度我控制在0.15m/s到0.25m/s。速度快了scan之间重叠度不够扫描匹配容易丢失太慢又会拖长时间。D435在30fps下点云帧率足够快但slam_toolbox的scan来源如果是80线甚至更低的2D scan每帧scan之间的重叠度主要取决于移动距离而不只是帧率。建完图后记得保存地图时用以下命令ros2 run nav2_map_server map_saver_cli -f map保存下来的map.pgm和map.yaml就是后续导航的地图输入。4. Nav2导航从静态地图到动态避障4.1 导航栈的基本框架Nav2的逻辑可以简单理解为机器人拿着地图静态图层实时叠加传感器看到的障碍障碍物图层点上人行道膨胀图层然后全局路径规划器找一条从起点到目标点的路径局部路径规划器在跟踪这条路径的同时实时避障最后输出速度指令。我搭建时用了这几个组件map_server加载建图阶段保存的map.yamlAMCL负责定位。因为slam_toolbox建出的地图没有包含实时定位功能导航需要单独的定位模块planner全局规划器我用的是NavFncontroller局部控制器我用的是DWBAMCL的作用经常被新人低估。建图时slam_toolbox会在位姿图里优化整个轨迹获得全局一致的地图但是导航时机器人并没有在全局地图中准确知道自己在哪AMCL通过粒子滤波根据当前scan和地图的匹配程度估计位姿。把AMCL的初始位姿估计错后面所有导航都会崩。4.2 行为树给导航加上恢复逻辑Nav2和早期move_base最大的不同就是引入了行为树。行为树BT是一种比状态机更灵活、比有限自动机更可组合的控制逻辑它能让机器人在遇到问题时“有策略地尝试恢复”而不是卡住或原地崩溃。我用的默认BT文件是navigate_to_pose.xml它的核心逻辑可以拆成四段先用ComputePathToPose计算出全局路径再交给FollowPath循环执行每次循环检查controller是否超时超时则进入恢复流程先清除代价地图上的障碍物再旋转360度重新规划最后才会让整个导航失败这个“先清除再旋转最后报错”的顺序很重要。实际环境中大量导航失败是传感器瞬时噪声导致代价地图上出现假障碍清一次地图往往就好了。直接放弃或者原地乱转都是错误设计。用Groot2这个工具可以可视化行为树把树节点拖出来看状态排查导航失败时到底卡死在哪个节点上这是最节省时间的调试方式。4.3 代价地图配置里最容易翻车的三个参数代价地图层有很多参数但真正决定扫地机器人导航体感的就三个第一个是obstacle_range。它决定了传感器数据在多远距离会被标记为障碍物。设太大噪声更容易混进来设太小机器人对远处障碍反应迟钝。我用的是3.0m和slam_toolbox的max_laser_range保持一致性。第二个是raytrace_range。它是自由空间标记范围一般要比obstacle_range大一点大约4.0m确保传感器扫描路径上的空间被标记为可通行。如果这里设得比obstacle_range还小会出现障碍物和自由空间同时存在的自相矛盾。第三个是inflation_radius。这个值决定障碍物周围多少范围会被设置为“靠近障碍但还可通行”的高成本区域。对于扫地机器人这种小底盘inflation_radius设到0.25m就够设太大机器人会离墙远远的导致清扫覆盖率下降设太小又会频频贴边导航路径看起来有点惊险。costmap的local_costmap我特意把width和height设成2.0m乘以2.0m分辨率0.05m。这个尺寸对扫地机器人来说足够覆盖车周三米范围又不至于让局部代价地图的计算开销影响到DWB的高频控制。4.4 3D激光雷达和深度相机在Nav2中的接入差异如果是多线激光雷达习惯做法是直接把激光数据喂给costmap的obstacle层。但如果你用的是像Livox那样旋转频率不规律的3D雷达或者像我这样用深度相机点云就不能直接塞给costmap因为costmap_2d的obstacle层默认只订阅sensor_msgs/LaserScan。接入方式有两种常见选择第一种把3D点云转成2D scan再喂给Nav2的所有层。优点是兼容性极好代码改动少slam_toolbox建图和Nav2导航共用同一条数据管线一致性高。缺点是可调节性差高度过滤一旦固定局部代价地图就无法再细分。第二种用costmap_3d或者把点云投影出来的多层2D scan送到不同的costmap层。扫地机器人用这种方式可以做“低层费用层”和“高层费用层”比如低层超过5cm的障碍都算障碍高层只看30cm以上的部分这样桌面边缘不会被当作不可通行区域。我实际用的是第一种加一个额外的本地3D点云订阅器——全局代价地图走2D scan局部代价地图额外订阅一份降采样后的3D点云。这样导航规划时主要依据2D scan做长期规划局部避障却有3D体感可以处理悬空障碍物。5. 实战里踩过的坑与排查方法5.1 建图漂移和地图糊掉现象是地图整体看起来像被“揉皱”过一样某些墙壁出现双层线或扭曲。排查思路按这个顺序来先看里程计数据是不是干净的。扫地机器人通常用轮式里程计D435也可以做视觉里程计估计两者融合后数据是否稳定。我在跑slam_toolbox前用一个rviz终端盯着/odom话题的位姿轨迹如果车子走直线但odom轨迹画出曲线说明里程计标定有问题怎么调SLAM都救不回来。再看scan数据质量。打开rviz把/scan话题显示出来看有没有满屏的杂散点。我这里遇到过一个典型案例因为D435安装在底盘侧面点云转scan后把扫地机底盘的边沿扫进来了scan数据里永远有一堆固定的“假障碍”地图建出来像面条一样蜿蜒。解决办法是调发pointcloud_to_laserscan的min_height参数把底盘以上区域排除掉。5.2 Nav2导航时高频左右摆动这个现象特别典型机器人沿着路径走但车头左右来回晃速度指令一突一突。很多人第一反应是控制器PID参数没调好但在我这个场景里导致抖动的主因是局部代价地图的分辨率和DWB控制频率之间不匹配。解决办法是先把local_costmap的resolution从0.05m降到0.02m这样局部规划器能发现更细的通路再把DWB的max_vel_trans从0.25m/s降到0.15m/smin_vel_trans同样调低这样机器人在规划路径附近不会做过猛的速度变化。另外还要检查AMCL的粒子滤波参数。粒子数太少或者更新频率过低会导致定位精度在时间上忽高忽低反应到路径上就是抖动。AMCL粒子数我设的是2000个对于扫地机器人这种小底盘足够了。5.3 行为树一直卡在ComputePathToPose如果导航任务发出去之后机器人完全没有反应而且Groot2里看到ComputePathToPose节点一直是RUNNING状态问题通常不在行为树而在于全局代价地图。这个典型的原因是全局代价地图的obstacle层里把传感器数据的世界坐标换算错了。我排查时先看rviz里的全局costmap发现整个地图亮了一片红色——障碍物铺满。此时是点云转scan时把坐标系写错了scan本来应该发布在base_link坐标系我误设成odom导致传感器数据全被往世界坐标方向偏置了一截。解决方法是统一所有传感器的frame_id并且用tf2工具检查transform树是否正确ros2 run tf2_tools view_frames看到完整的tf树之后再对照rviz里显示的点云和scan能否正确叠加在代价地图上这种坐标错位的问题能快速定位。5.4 局部代价地图上不断出现闪烁的障碍物D435这类结构光相机在遇到玻璃、镜面或者反光地板时会出现深度空洞和飞点。这些飞点转成scan后表现为局部代价地图上的“幽灵障碍”一会儿消失一会儿出现机器人路径规划会频繁重新规划。我的解决办法是做两级防护第一级是预处理时把点云的置信度过滤加上。D435的深度图可以通过设置深度预设或者开启temporal filter短期时间平滑能够抑制大多数随机噪声点。realsense2-camera中我把initializer设置为2并把frames_queue_size加到4飞点明显减少。第二级是在Nav2的obstacle层里加上observation_filter。参数可以用observation_sources: scan scan: topic: /scan data_type: LaserScan obstacle_range: 3.0 raytrace_range: 4.0 inf_is_valid: true observation_persistence: 0.0 min_obstacle_height: 0.05 max_obstacle_height: 0.6这里min_obstacle_height设成0.05m地面的小石子、硬币不会被标记成障碍物导航对这类微小凸起不再有激烈反应。如果你发现障碍物闪烁依旧强烈可以考虑把observation_persistence设置成0.5它会让障碍物在消失后继续保留0.5秒给局部规划器更多的缓冲时间。5.5 建图效果好导航却频繁撞墙如果建图过程中地图看起来清晰准确但导航时机器人在某个特定区域总是规划失败或贴近墙体大概率不是SLAM的问题而是建的这张地图里包含了建图时传感器噪声的痕迹。建图时如果有人走来走去、门开开关关栅格地图上会留下“闪电状”的痕迹。这些痕迹在导航时会被代价地图当作可通行区域的边缘计划器在其中穿插路径就会走得很偏。最简单的修法是把这些区域在地图编辑器里抹掉或者重新建图时让人撤离。更工程化的做法是在导航定位阶段给AMCL加一个static_map的更新阈值过低置信度的地图区域会被实时传感器数据覆盖掉。这个参数藏在AMCL的update_min_d和update_min_a里调低后地图更新会更频繁代价是算力开销变大。最后再分享一个经验这台扫地机器人从硬件选型到导航跑通前后花了我大概三周时间每天调参、跑图、回放bag数据。走完这条全链路之后我的体会是真正决定系统稳不稳的不是某一个环节而是每个环节之间数据流是否干净。点云转scan时多滤一步后面SLAM的匹配就很轻松SLAM建图时多注意几步闭环导航时的代价地图就干净得多。SLAM和Nav2的报错信息往往不会直说病根你需要顺着数据流一层层往回查——先查传感器再查预处理再查TF变换最后才动算法参数。如果你打算在这个基础上继续做扩展我建议优先做两件事。一是把建图阶段的数据保存成rosbag这样每次调参都能用同一份数据反复回放不用每次都推着机器人在家里跑一遍调试速度快非常多。二是尝试给机器人加一个顶部相机或者固定的3D点云订阅器把高处的悬空障碍物也纳进局部代价地图扫地机器人在低矮家具下行驶时会从容很多。建图导航这条路没有太多玄学每一个现象背后都会有明确的链路原因剩下的事情就是耐心排查。