扫地机器人视觉+IMU融合导航：SLAM、路径规划与工程实践

扫地机器人这行做了快十年从最早的单目摄像头撞来撞去到后来激光雷达普及再到现在视觉IMU逐渐成为中高端机型的标配能明显感受到导航技术在一层层往深走。很多朋友问我的第一句话就是视觉SLAM和激光SLAM到底差在哪为什么非得加个IMU路径规划又到底怎么配合定位数据工作这些问题问得多了我发现自己很难用几句话讲清楚。这篇就把这几年做扫地机器人视觉IMU融合导航路径规划的经验整理出来从传感器选型、标定、建图、规划到真机部署尽量把每一个关键环节的“为什么”也讲明白希望能给正在做机器人导航或者准备入行的朋友一些实实在在的参考。1. 为什么扫地机器人需要视觉IMU这套组合拳1.1 单一传感器解决不了的问题先回到最基础的问题一台扫地机器人要真正做到自主清扫至少得回答三个问题——我在哪我要去哪我该怎么去这三个问题分别对应定位、建图和路径规划而所有环节的第一步就是传感器。激光雷达是扫地机上用得最早也最成熟的方案它测距准、抗干扰强能直接给出二维平面障碍物分布。但激光雷达有个天然短板它扫出来的是一个平面slice扫地机遇到台阶、地毯边缘、桌腿下方的横梁这类低矮障碍激光很容易漏检或误判。更麻烦的是激光雷达对环境的几何结构依赖很强在空旷的客厅或两面长走廊里特征太少匹配容易飘。纯视觉方案呢相机能看到丰富的纹理信息理论上能识别物体、判断场景语义但普通单目相机根本没有尺度信息——一张照片里你很难知道障碍物到底离你1米还是3米。而且相机帧率再高遇到快速旋转、剧烈晃动时图像模糊、特征丢失都是家常便饭。IMU惯性测量单元能测量三轴加速度和三轴角速度更新频率高短期位姿推算非常平滑。但它最怕积分漂移——加速度计稍微有点零偏积分两分钟位置就跑偏不少。用句行话说IMU是“短期准、长期飘”视觉和激光是“长期稳、短期容易丢”。单独拿任何一个出来都撑不起一台稳定工作的扫地机。1.2 视觉和IMU是怎么互相“补位”的视觉IMU融合本质上就是让两种传感器互相兜底。视觉帧率一般30fpsIMU可以到200Hz甚至更高。当你快速旋转扫地机时相机图像可能已经糊成一团但IMU依然能精确输出两帧图像之间的旋转增量帮助系统维持位姿估计。反过来IMU的积分漂移可以由视觉观测——比如图像里的特征点匹配——来持续修正相当于用相机反复给IMU“校零”。用大白话讲IMU是那个闭着眼也能短时间内不乱脚步的人视觉是那个睁着眼能告诉你“你现在在哪、周围长什么样”的人。但闭眼走久了一定会偏睁着眼看的时候又怕眼前晃得太快看不清。两个人配合起来一个负责连续、一个负责修正才能走出长期稳定、短期平滑的轨迹。这个配合一旦做好扫地机就可以在没有明显几何特征的场景里继续定位也能在地毯、门槛这种激光雷达容易出错的区域保持稳定。因此视觉IMU融合不只是一个营销卖点而是真正解决复杂家庭环境可靠性问题的工程方案。2. 传感器选型与硬件搭建2.1 视觉方案怎么选单目、双目还是RGB-D视觉方案的选择直接决定后续算法复杂度我在不同项目里三种方案都试过各自的取舍很明确。单目相机成本最低一个几十块钱的摄像头就能跑视觉SLAM。但单目有尺度的不确定性问题也就是说算法知道轨迹的形状却不知道具体是多少米。这个尺度如果靠融合IMU来估计可以在初始化阶段慢慢收敛但收敛过程中扫地机的定位是偏的对于需要精确贴边清扫的场景影响很大。所以在量产扫地机上纯单目IMU的方案我更倾向于仅用于辅助避障而不是主定位。双目相机通过左右目视差直接计算深度尺度问题是天然解决的。它不需要发射红外光户外强光下也能工作室内暗光环境只要补光够就行。缺点是标定麻烦两个镜头之间的小角度偏差都会影响深度精度计算量也成倍增加。RGB-D相机比如结构光或ToF方案能直接输出稠密深度图对视觉导航最友好。扫地机上常见的是ToF测距范围有限一般5米内效果不错超过就噪声很大。而且ToF在黑色物体、阳光直射下容易失效选型时要搭配IMU做短时预测兜底。我自己的经验是如果是研发样机优先验证算法RGB-DIMU最容易起步如果目标是量产降成本双目IMU更稳单目IMU则适合做低成本辅助定位层。千万别迷信参数表关键看你的算法管道吃不吃得下这些数据。2.2 IMU选型与安装要点IMU的选型不是看单轴性能而是看零偏稳定性、噪声密度和各轴正交误差。常见消费级IMU比如ICM-42688、BMI270零偏稳定性大概在几度每小时到十几度每小时之间用在扫地机上已经够。工业级IMU比如ADIS16470贵很多性能好一个量级但扫地机这种场景未必需要。真正影响融合质量的反而是安装。原则很简单IMU的几何中心尽量靠近相机的光心三轴方向尽量与相机坐标系对齐。原因也很实际相机和IMU之间的外参旋转和平移需要通过标定获得如果你的IMU装得离相机光心很远平移外参会变大同时振动时两边的运动差异也更明显标定残差很难收敛。还有一点往往被忽略IMU一定要远离电机磁铁和电源大电流走线。扫地机底盘电机启动瞬间电流很大磁场变化会干扰IMU读数。我们曾遇到过一开机YAW就漂的怪问题最后排查半天发现是电池走线贴着IMU板。把IMU挪开几厘米问题直接消失这类教训很常见。2.3 主控选型rk3588为什么常见很多人看到RK3588觉得大材小用但扫地机内部其实很需要这类带NPU的SoC。视觉SLAM的前端特征提取、光流跟踪、语义分割都能用NPU加速把CPU腾出来给路径规划和控制。RK3588的优势在于四核A76加四核A55的大小核架构既有高负载算力又有低功耗核8K视频编解码模块对图像传感器的接入很友好6 TOPS NPU跑轻量级神经网络足够。而且Rockchip在Linux生态上做了大量适配OpenCL、RKNN、ROS2这些常见依赖都能比较容易地编译部署。不过选RK3588也意味着板级设计要更用心DDR带宽、散热、电源纹波都会成为瓶颈。如果你只是做算法验证直接用开发板起步完全可行真正量产时再考虑裁剪。别一开始就上定制主板调试成本太高。3. 视觉IMU融合的核心SLAM与状态估计3.1 从视觉SLAM说起特征点与光流视觉SLAM的老祖宗思路是特征点法。每一帧图像提取ORB、SIFT这类关键点描述子匹配相邻帧然后通过多视角几何解算相机运动和地图点位置。特征点法稳健适合基线变化大的场景但描述子计算量大在纹理稀疏场景容易空手而归。后来光流法逐渐流行它不计算描述子而是直接跟踪像素块的运动。LK光流在帧间位移不太大的情况下又快又准很适合扫地机这种低速平台。缺点是光流法怕模糊、怕剧烈光照变化一丢就全丢。所以实际工程里我倾向于前端用光流跟踪后端定期做关键帧检测和重定位两者结合既保证速度又保证鲁棒性。你把这些特征点输出给后端优化时视觉SLAM才算真正进入状态估计阶段。后端要做的事情是滑窗优化或位姿图优化把每一帧的位置调整到全局一致。扫地机面积不大一圈下来地图闭环几次全局误差已经能控制在厘米级。3.2 IMU的作用短时预测与尺度恢复IMU在视觉SLAM系统里承担的不只是“备份”。当相机图像因为快速旋转或者被抹布遮挡而暂时失效时IMU反馈的角速度和加速度可以持续推演位姿保证定位输出不会瞬间跳变。这个功能在扫地机钻进沙发底、桌下等光线暗区域时尤其重要。另一个关键任务是恢复单目视觉的尺度。纯单目SLAM解出的轨迹是归一化尺度下的结果不知道真实米制。IMU能测到真实加速度重力向量也能直接提供绝对俯仰和横滚参考这些信息参与联合优化后轨迹尺度就能被约束到真实值。这也是VINS-Mono这类经典视觉惯性系统能做单目版本的原因。但在融合IMU时要注意一个隐性问题IMU加速度计测到的是“比力”包含重力而视觉观测的又是纯几何运动。两个量融合过程中对重力方向的准确估计至关重要。如果初始化时挪动过快重力向量收敛得不好后面整个系统的俯仰横滚都会歪。3.3 融合方式松耦合与紧耦合视觉和IMU融合从架构上看分松耦合和紧耦合两种路线。松耦合的思路很直接视觉SLAM先独立跑一套输出位姿IMU另外单独积分输出位姿最后用一个滤波器常见是误差状态卡尔曼滤波把两个位姿合起来。这类方案实现简单两个子系统可以分开调试适合快速原型。缺点是没有共享原始观测任何一个子系统的误差都会被直接带进融合结果精度天花板有限。紧耦合则把视觉特征、IMU测量值丢进同一个优化问题里同时求解。VINS-Mono、ORB-SLAM3的视觉惯性模式都是这个思路。紧耦合格局下视觉和IMU相互约束任一传感器的短时退化都能被另一方补偿精度和鲁棒性都更高。代价是计算量大、初始化敏感、标定要求高。扫地机器人量产项目普遍走紧耦合因为家庭环境复杂你没法保证每个房间光线、纹理都理想。既然紧耦合是主流那就重点把相机与IMU的标定做扎实标定做不好融合精度再玄学也白搭。3.4 相机与IMU联合标定不能跳过的步骤相机内参标定大家相对熟悉棋盘格或Apriltag跑一遍就行。但相机与IMU之间的外参标定很多人容易忽略或者只是随手写个近似值。这里必须强调外参错了融合算法根本不可能收敛。联合标定的做法是同时采集相机图像和IMU数据让设备做充分的旋转、平移激励然后用Kalibr这类工具离线优化出相机与IMU之间的旋转和平移。操作上有几个要点标定场景光照稳定、纹理丰富设备固定好后每转动一下停一停让IMU充分激励采集时长不要太短一般要3-5分钟让平移和旋转都覆盖。还有时间同步问题。相机和IMU的时钟如果不一致融合算法会有额外延迟误差。一般是让IMU硬件同步输出PPS信号给相机或者在驱动层用系统时间戳近似同步。后者虽然简单但误差有几个毫秒的话在快速旋转时影响明显。量产出厂前必须跑一遍内参、外参、时间延迟的联合标定流水线这一步省不了。4. 地图构建与全局路径规划4.1 地图形式栅格地图、八叉树地图、拓扑地图定位训好后扫地机还需要一张可以“画路线”的地图。最常见的是二维栅格地图Occupancy Grid Map把环境切分成等大小格子每个格子标记空闲、占据或未知。栅格地图直观、便于做路径规划但也存在分辨率与内存的折中。一张100平米房间的5cm分辨率栅格地图大概就要几十万格子内存和建图更新压力都不小。三维场景可以考虑八叉树地图OctoMap。它通过八叉树递归划分空间只在有障碍物的区域细分空旷地带用大节点表示内存效率高很多。扫地机本身在二维平面移动八叉树地图更多用于处理悬空家具、低矮障碍的空间信息比如判断机器人能否从茶几下面钻过去。如果规划层只用二维栅格那么八叉树地图可以作为语义避障辅助层。拓扑地图则更适合大范围多区域的导航它把房间抽象成节点和边节点表示关键路口或充电座边表示可通行路径。扫地机多楼层或超大平层时先构建拓扑图再在每个子区域内部跑栅格规划综合效率和精度都可以兼顾。4.2 全局路径规划算法Dijkstra、A*、RRT全局路径规划的任务是在已知地图上找一条从当前位置到清扫目标点的无碰撞路径。经典算法是Dijkstra它从起点逐步向四周扩展保证找到最短路径但效率低不适合大范围实时计算。A是实际工程中用最多的全局规划算法它加入了启发式函数优先扩展估计总代价最小的节点比Dijkstra快得多。扫地机产品里加一点改进比如JPS跳点搜索或A的变体在栅格地图上效果非常明显。要注意的是A*在障碍物密集环境容易产生贴墙或转折多的路径所以规划出的原始路径往往还要做平滑处理。RRT系列是采样类方法适用于高维空间或者地图特别大的情况。扫地机这种平面低速平台上A*基本够用RRT的优势发挥不出来。不过如果你在做机械臂或者无人机路径规划RRT就会成为主力。规划算法没有绝对优劣关键看你平台的运动约束和地图规模。4.3 导航地图的生产流程地图不是建好就直接用的机器人第一次扫全屋时会生成一张“原始地图”这张地图往往带有噪声、动态障碍物残留和错误闭环痕迹。标准做法是让扫地机跑一遍“建图模式”尽量缓慢地覆盖全屋过程中不要出现移动的人和宠物最后再通过回环检测修正全局一致性。建完图后我还建议做两件事一是给地图标记语义信息比如房间、门槛、充电座位置二是做障碍物膨胀。在栅格地图上把障碍物周围指定半径内的格子标记为不可通行这个半径至少要等于机器人半径加安全余量。膨胀半径太小机身容易擦撞太大狭窄通道会被提前堵死。实测下来扫地机直径约35cm时膨胀半径设在20-25cm比较合适。全局路径虽然规划完了但扫地机真正跑起来还会碰到地图上没有的动态障碍这时就需要下一层的局部路径规划上场。5. 局部路径规划与动态避障5.1 DWA、TEB这些经典局部规划器局部路径规划是在全局路径引导下根据实时传感器数据生成短时间内的速度指令。DWA动态窗口法是扫地机器人上最常见的局部规划器它的思路是在当前速度空间采样一系列可能的线速度和角速度组合模拟未来一段时间内的轨迹再根据障碍物距离、目标朝向、速度大小等做加权评分选最优速度执行。DWA的优点是好调、运行稳定但也存在容易陷入U形障碍物里的问题。TEB时间弹性带是另一类经典算法它把局部路径看作一条有弹性的带子在满足运动学约束和避障约束的前提下通过优化让带子尽量靠近全局路径且时间最短。TEB在窄通道和复杂障碍场景下表现明显好于DWA但调参复杂计算量也大。我个人的习惯是量产扫地机用DWA做基础避障在危险场景额外叠加一个安全策略比如检测到近距离障碍强制减速甚至停车。千万别把DWA参数调到它性能极限留出安全冗余才是家用产品该有的思路。5.2 动态障碍物处理动态障碍物的本质问题是地图上不存在的东西突然出现在传感器范围内。扫地机遇到人脚、宠物、掉落数据线都需要快速反应。这里视觉的作用就体现出来了视觉能够识别障碍物类别比激光单纯测距更有针对性。例如看到“人脚”时策略是保持距离并暂停清扫等障碍物移开后继续看到“数据线”时不能硬撞要绕开避免吸入缠住。动态避障的工程实现一般分多层底层是紧急制动逻辑由ToF或红外传感器触发毫秒级响应中层是局部规划器的动态障碍物代价层把实时感知到的障碍物注入局部代价地图上层是行为决策决定是停车等待、绕行还是回归全局路径。三层配合才能真正做到既安全又高效。视觉动态障碍检测要避免误报。曾经有用户反馈扫地机对着墙上的装饰画绕来绕去查下来是识别模型把画框边缘误判成了障碍物。后续在模型训练时增加了大量正样本和前后帧一致性校验误报率才降下来。5.3 覆盖清扫路径如何规划扫地机器人跟配送机器人最大区别在于它的路径规划目标不是“从A到B”而是“覆盖整个区域且尽量不重复”。覆盖路径规划的主流做法是分块牛耕法先把房间按凸划分成若干子区域再在每个子区域内沿直线来回清扫。转90度左转或右转形成高效的弓字形路径。实现弓字形覆盖时IMU的航向精度非常关键。直线清扫时扫地机需要保持航向稳定一旦偏航两条相邻路径之间会出现漏扫带或重叠带。视觉IMU融合能提供低频稳定的航向角估计配合轮式里程计弓字形路径就能做得比较直。我们实测过在5m长的房间里如果只用轮式里程计路径末端偏航能差到20cm以上加入视觉IMU后偏差能压到5cm以内。覆盖面积最大化还依赖边界识别。扫地机贴着墙边走一圈记录边界坐标然后向内逐步覆盖。这个过程对贴边精度要求高视觉能识别踢脚线、家具底部边缘帮助机器人维持5-10mm的贴边距离比激光雷达检测平面障碍的效果更细腻。6. 基于ROS2与Nav2的落地实践6.1 为什么从ROS1迁到ROS2很多实验室和早期产品都是基于ROS1开发的我也一样。ROS1的master中心化架构在单机原型阶段很方便但一进入多机协同、长期运行它的问题就暴露了节点掉线后整个系统无主、通讯实时性差、安全机制薄弱。ROS2引入了DDS数据分发服务作为通信底层节点发现机制是分布式的单个节点挂了不会拖垮全局而且支持QoS策略能针对不同消息配置不同的可靠性和时效性要求。对导航这种延时敏感任务可以把局部代价地图和速度指令设成“尽力传输”把地图、日志设成“可靠传输”。在嵌入式平台上ROS2的Daemon和发现机制带来额外资源开销实机上要合理屏蔽不必要的Topic并且选用轻量级RMW实现比如CycloneDDS或FastDDS并且做好网络配置。别直接在默认配置下上真机延迟和丢包会让你怀疑人生。6.2 Nav2的组成与配置要点Nav2是ROS2下的导航框架整个过程比我早期手写规划器要规范得多。它由几个核心模块协作行为树服务器Behavior Tree负责任务流程控制Planner Server跑全局路径规划Controller Server跑局部轨迹跟踪Costmap管理代价地图BT Navigator则负责把这些串起来。用Nav2跑扫地机重点配置的是costmap层和规划器参数。全局代价地图分辨率可以低一点比如5cm局部代价地图分辨率需要更高比如2.5cm以便捕捉近距离障碍细节。静态层、障碍物层、膨胀层都必须开动态障碍物如果要视觉感知就再挂一个语义障碍物层。还有一点容易踩坑Nav2默认的机器人模型是差速轮扫地机也是差速轮运动学参数直接套用问题不大但如果你用全向轮或者阿克曼底盘必须自己实现符合运动学的控制器插件。别指望默认控制器能适配所有底盘。6.3 相机/IMU里程计接入Nav2的流程在Nav2里位姿来源是里程计odom和定位系统amcl或SLAM节点。我们用的视觉IMU融合结果本质上就是一套视觉惯性里程计VIO它输出的是odom话题。把VIO节点输出的transform和odometry消息接入Nav2流程首先确保VIO节点发布的frame_id是“odom”child_frame_id是“base_link”并且频率至少达到20Hz以上Nav2的costmap更新和控制器执行都比较依赖高频里程计。其次AMCL或SLAM建图节点接收到VIO位姿后需要融合激光或视觉观测做全局修正否则长时间运行还是会缓慢漂移。我遇到过最多的情况是VIO本身精度没问题但Timestamps不同步导致Nav2的transform树频繁报错。排查口诀很简单rostopic hz检查频率tf tree检查坐标树完整性rqt_bag检查时间戳偏差。这三大件查完90%的接入问题都能定位。7. 仿真与部署经验7.1 用MuJoCo训练和验证导航策略这几年强化学习在机器人导航里越来越热MuJoCo因为物理仿真精度高、速度快成为很多人训练扫地机导航策略的工具。用MuJoCo训练扫地机器人导航最大的优势是可以大规模生成随机场景——随机布置障碍物、随机起点终点让策略学会在未见过环境中泛化这在真机上是不可想象的。但仿真训练也有明显的坑Sim-to-Real的差距。MuJoCo里摩擦系数、电机响应、传感器噪声都太干净策略在仿真里无敌搬到真机可能第一步就撞墙。我的建议是训练时对IMU和视觉观测加入高斯噪声、随机时间延迟、随机传感器缺失做一些domain randomization逼着策略学习鲁棒行为。MuJoCo的定位其实更适合验证底层运动规划和避障策略整个系统的SLAM、全局路径规划还是建议在Gazebo或者真实数据回放上调试不同仿真器各有适用场景别指望一个工具全包。7.2 真机调试的坑真机调试是整个项目里最考验耐心的阶段。第一个坑就是振动。扫地机底盘电机和滚刷振动会把IMU信号污染融合轨迹出现高频抖动。解决办法除了物理上加减振泡棉还可以在IMU驱动里加陷波滤波器专门滤掉电机基频附近的噪声。第二个坑是光照变化。相机在夕阳西下时照进客厅整个画面的白平衡剧烈变化特征点跟踪很容易崩。工程上一个是让相机尽量工作在自动曝光区域内同时在VIO前端做亮度归一化另一个是配置IMU的权重光照突变时适当提高IMU的影响等图像恢复后再拉回来。第三个坑是地毯和深色地板。RGB-D在黑色长毛地毯上基本失效双目也经常匹配失败。这时候必须依靠IMU和轮式里程计短时间维持位姿等相机恢复。视觉IMU融合的价值在这类场景体现得最明显。7.3 常见问题排查速查表现象常见原因排查和解决思路开机后航向快速漂移IMU零偏未校准或受电机磁干扰重新执行IMU静态校准检查IMU与电机/电源线的距离加磁屏蔽建图重影或回环错位相机曝光不一致或特征点匹配失败检查白平衡和曝光设置调大回环检测范围提高IMU权重视觉定位时好时坏时间戳不同步或VIO外参不准检查驱动时间戳重新跑Kalibr联合标定全局路径规划一直失败膨胀半径过大导致窄通道封闭下调膨胀系数检查地图边界是否正常动态避障反应迟钝局部代价地图更新频率低或传感器延迟高提高局部costmap发频率降低感知链路时延MuJoCo策略真机失效仿真环境过于理想域随机化不足加入传感器噪声、随机障碍物、模拟清扫负载我见过不少人把大量时间花在算法调参上最后发现是标定或者时间同步这种基础问题没解决。做融合导航先保证数据干净再谈算法优化顺序不能反。说到时间同步想再提醒一个容易被忽略的细节如果在系统里同时跑视觉SLAM和IMU并且还有激光雷达参与建图三个传感器之间的时钟偏差会在长时间运行后慢慢累积。量产产品建议用硬件级别的同步信号把所有传感器锁在同一个时钟源上别指望软件时间戳对齐能长期稳定这是我们从多次翻车中总结出来的教训。最后分享一个我们团队一直沿用的原则视觉IMU融合导航不是某一个算法的胜利而是传感器标定、状态估计、路径规划和系统工程的综合结果。每次调试只改一个变量记录一组数据保持耐心问题总会浮出水面。希望这篇整理能让你在扫地机器人导航这条路上少走一些弯路。