扫地机器人视觉+IMU融合导航与路径规划实战解析

我先说明一下，按照任务规范，我现在直接输出最终的博文内容，纯Markdown格式，不添加任何前后置说明。

扫地机器人从随机碰撞进化到“哪里脏扫哪里，扫完自己回家充电”，背后靠的是一整套传感器融合与路径规划体系。这几年我一直在做服务机器人导航相关的开发，视觉SLAM加IMU融合这条路踩了不少坑，也积累了一些实打实的经验。今天把扫地机器人视觉+IMU融合导航路径规划这件事从头到尾拆一遍，从传感器为什么非要融合、标定怎么搞、前端里程计怎么跑、地图怎么建，到全局和局部路径规划怎么调参，一次性讲清楚。这篇文章适合正在做机器人导航开发的工程师、想入门视觉SLAM方向的学生，以及准备把扫地机器人方案搬到其他移动机器人平台上的朋友。

1. 整体设计思路：为什么扫地机器人非得视觉+IMU融合

1.1 单传感器方案的天花板到底在哪

先聊一个最核心的问题：扫地机器人明明可以用激光雷达，为什么还要搞视觉+IMU融合导航？答案其实很现实——成本和场景。

激光雷达方案确实成熟，2D激光雷达加轮式里程计就能跑起来，很多早期扫地机器人就是这么干的。但2D激光雷达有个致命问题：它只能扫描一个平面。沙发底下、床底这种矮空间，激光雷达的高度可能进不去；遇到落地镜、玻璃茶几腿这类反射或透射物体，激光数据会直接丢失或产生幻影点。而且纯2D激光雷达建出来的地图没有高度信息，机器人压根不知道前方障碍物是能钻过去的桌底还是必须绕开的墙体。

再单独看视觉方案。单目相机便宜、信息量大，但单目视觉里程计存在尺度不确定性问题，说白了就是算法知道“我动了”，但不知道“我动了多远”，这个尺度误差在长时间运行后会累积成严重的轨迹漂移。双目或者RGB-D相机能解决尺度问题，但算力开销大，扫地机器人主控往往只是一颗算力有限的SoC，跑不起太重的双目匹配。更麻烦的是视觉对光照极度敏感，扫地机器人白天在窗边扫、晚上在客厅扫，光线变化一大，特征点提取质量就崩。

那单独用IMU行不行？IMU（惯性测量单元）能输出三轴加速度和三轴角速度，短时间内的姿态解算非常准，但它的致命弱点是积分漂移。加速度计积分出速度，速度再积分出位移，每一时刻的微小误差都会随时间累积膨胀。我实测过，消费级IMU纯积分解算位置，3秒钟之内位移误差就能跑到几十厘米。位姿解算里yaw角（航向角）也会出现“慢漂”现象，就是你肉眼看到机器人明明走的是直线，但算法里yaw在缓慢地偏，几十秒之后整个朝向就歪了。关于这一点，凡是做过基于IMU的位姿解算的工程师应该都有同感。

所以结论很清晰：视觉负责提供丰富的环境信息和稳定的中低频运动估计，IMU负责提供高频的角速度和加速度测量，两者各补短板，融合之后才能得到既抗光照、抗快速运动，又有明确物理尺度的高频位姿。这就是视觉+IMU融合导航在扫地机器人上成为主流方案的根本原因。

1.2 融合架构选型：紧耦合才是绕不开的主流

视觉和IMU融合，业界有两种大的技术路线：松耦合和紧耦合。

松耦合的思路是，视觉里程计先独立算出一帧位姿，IMU也独立积分出一帧位姿，然后把两个结果“取加权平均”或者用一个滤波器去融合。这样做实现简单、模块之间耦合度低，但问题是视觉和IMU各自先算了一步，等于把信息压缩了一遍再合并，精度上限很低。平滑插值和权重分配也很玄学，调参调到怀疑人生。

紧耦合则完全相反。它把视觉特征点的重投影误差和IMU的预积分误差放进同一个优化目标函数里，用非线性最小二乘去求解，核心在于“在一个估计器里同时处理两种传感器的原始测量”，从原理上保证了信息不浪费。目前VINS-Mono、ORB-SLAM3这些开源方案在视觉+IMU融合上也都选择了紧耦合，这已经是一个行业共识。

扫地机器人这个场景，还有一层特殊考虑：地刷启动时整机振动非常大，而且扫地机器人经常会做原地掉头这样的快速旋转运动。快速旋转时相机会产生严重的运动模糊，如果视觉单独算位姿，大概率直接跟丢。但如果IMU处于紧耦合框架内，哪怕某一帧视觉特征跟踪失败，IMU的预积分项也能把位姿“撑着”往前走，等运动稳定后视觉再重新收敛，整个系统不会崩。这也是我在实际开发中坚定选紧耦合路线的最大原因。

1.3 后端选滤波还是优化，扫地机器人的算力账要算好

紧耦合的具体实现有两种：基于滤波器的MSCKF/EKF-SLAM，以及基于图优化的滑动窗口BA（Bundle Adjustment）。滤波器方案计算量小、实时性好，老一代VIO方案如MSCKF就是滤波器，但它在精度和可扩展性上受限于线性化误差，尤其在大场景长时间运行后漂移更明显。滑动窗口BA则是把过去N帧的位姿和路标点放进窗口里一起优化，精度更高，代价是计算量大。

扫地机器人主控的算力往往只有几TOPS（比如瑞芯微RK3588这级别已经算配置不错的），既要跑感知，又要跑规划，还要跑交互UI。所以工程上通常的做法是：前端视觉+IMU紧耦合里程计跑相对轻量的滑动窗口，窗口大小控制在10到15帧，地图和回环部分放在后台低频线程执行。现在开源方案里VINS-Fusion就是典型的滑动窗口+边缘化（marginalization）架构，在ARM平台上经过NEON优化后基本能跑到20到30Hz的位姿输出，足够扫地机器人用了。如果对实时性还有更高要求，可以走MSCKF路线，但要做好精度打折扣的预期。

2. 装机第一步：相机与IMU的标定，不标定一切白搭

2.1 相机内参标定：棋盘格与Kalibr实操

视觉+IMU融合的第一步绝对不是写代码，而是标定。很多人忽略这一步，把工业相机和IMU装上就直接跑开源VIO，结果轨迹飞得妈都不认识，还以为是算法不行，实际上就是标定没做。相机内参包括焦距、主点、畸变系数，这些参数直接影响去畸变和特征点投影的质量。内参不准，后续外参标定和SLAM系统全部跟着崩。

我常用的工具是Kalibr，它是苏黎世理工开源的一套相机/IMU标定工具，ROS1和ROS2环境下都能用。标定板建议用Aprilgrid棋盘格，比传统棋盘格的优势是自带编码信息，部分遮挡也不影响角点检测和匹配。

标定的核心流程是：打印一张Aprilgrid标定板，固定相机，手持标定板在相机视野里做缓慢的六自由度运动，让棋盘格覆盖画面的各个角落，同时保证标定板始终在画面内。录制约1到2分钟的bag包，然后跑Kalibr的相机内参标定命令。实际操作中要注意，标定板的平整度必须好，用普通A4纸贴硬纸板容易翘边，我建议直接打印在铝塑板上，表面不能反光。

提示：标定相机内参时，运动要慢，但姿态要丰富，比如让标定板在画面里出现“俯仰、偏航、横滚”三种轴向的旋转，避免只在一个平面内平移。否则某些畸变参数会退化，内参标出来看着数值漂亮，实际用起来就是飘。

2.2 相机与IMU外参标定：为什么必须做六轴激励运动

相机内参搞定之后，更关键的是相机和IMU之间的外参标定。外参包括旋转矩阵R_ic和平移向量t_ic，描述的是相机坐标系和IMU坐标系之间的空间变换关系。外参标不准，视觉和IMU的数据在融合时就会出现系统性偏差，这种偏差不是调权重能弥补的。

Kalibr标定外参的底层原理是利用相机估计出的运动轨迹和IMU测量出的角速度/加速度，通过优化来估计外参和时延。实际操作中，有一个极其重要的关键点：录数据时一定要“充分激励IMU的六个轴”。

这句话怎么理解？你手持搭载着相机和IMU的设备，在标定板前做各种方向的旋转、平移、横滚、俯仰、偏航运动，要让IMU的三个加速度计轴和三个陀螺仪轴都被充分激活。简单说就是不能只在一个方向上晃，要让设备经历各个方向的加速度变化和角速度变化。如果激励不充分，标定出的外参会出现退化，算法可能收敛到局部最优。

这里我提供一个经验准则：每次录制标定数据大约2到5分钟，运动的模式要覆盖快速旋转、慢速平移、大幅度俯仰、大幅度横滚这四类动作。我在实际中标过一次IMU，第一次偷懒只做了水平面内的运动，结果外参旋转分量误差很大，VIO跑起来轨迹在Z轴方向有明显的不对劲。重新认真做了一轮六轴激励之后，轨迹就稳了。

2.3 时间同步：IMU和相机的时间戳对不上，融合全是白费

外参标完之后，很多新手直接开始跑VIO，然后发现位姿还是飘。这时候要检查一个经常被忽视的问题——时间同步。

视觉和IMU是两种完全不同的传感器，相机帧率通常在30Hz左右，IMU的帧率则在200Hz到500Hz。如果两个传感器的数据到达算法的时间戳不一致，比如相机图像的时间戳和IMU时间戳存在几十毫秒的偏移，融合出来的位姿就会严重失真。试想一下，扫地机器人在快速旋转时，图像已经是几十毫秒前的画面，但IMU数据却已经是当前时刻的，两者在时间上对不齐，联合优化算出的位置自然会错乱。

解决时间同步有两种方案。最专业的做法是硬件同步，由主控给相机和IMU提供同一个外部触发信号，让它们在同一时刻曝光和采样，这种方式精度最高，但需要硬件支持，不一定所有传感器都有这个能力。更普适的做法是软件同步，先在标定时估计出传感器之间的固定时间偏移（temporal offset），然后在算法里把时间戳对齐。在ROS2环境下，可以使用message_filters中的ApproximateTimeSynchronizer做近似时间同步策略，它会缓存一窗口内的消息，找到时间戳最接近的图像和IMU数据组合。对于扫地机器人这种控制周期不苛刻的平台，软件同步的精度足够用了。

注意：如果VIO源数据的时间戳本身就有问题（比如采集程序里用的是系统启动时间戳但发生了跳变），甚至会出现时间倒流现象。这种时候再强大的融合算法也救不回来。我的排查习惯是录制原始bag包后，先画一条时间戳差值曲线，确认相机和IMU的帧间隔是均匀的、单调递增的，然后再进入后续处理。

2.4 实操记录：一次完整的相机IMU联合标定流程

以我常用的ROS2环境为例，完整走一遍相机IMU联合标定流程。

先在标定板上贴好Aprilgrid，用realsense或者其他相机对着标定板，同时启动IMU驱动，确认两个话题的数据都在正常发布。然后录制原始数据bag：

ros2 bag record /camera/image_raw /imu/data

录完之后用Kalibr做联合标定：

kalibr_calibrate_imucam \ --target aprilgrid.yaml \ --cam camchain.yaml \ --imu imu.yaml \ --bag data.bag \ --time-calibration

其中camchain.yaml包含相机内参的标定结果，imu.yaml包含IMU的噪声密度和随机游走参数，这些参数通常可以从IMU芯片的数据手册查到。跑完之后Kalibr会输出camchain-imucam.yaml，里面就包含了外参R_ic、t_ic以及相机和IMU之间固定时间延迟。

我标定过程中的一个心得是，每次标定完不要急着删除结果，拿标定出的外参跑一遍离线VIO，回放bag看看轨迹起始段有没有明显的漂移。如果轨迹从起点就开始发散，多半是外参或者时间延迟没标好，直接回头重新录数据比改算法参数要高效得多。

3. 前端里程计：视觉特征跟踪与IMU预积分

3.1 特征点法和光流法，扫地机器人该怎么选

前端里程计的任务是回答一个问题：两帧图像之间，相机到底运动了多少。视觉SLAM的经典思路是特征点法，比如ORB特征，原理是提取图像中的角点和描述子，然后通过描述子匹配建立两帧图像之间的对应关系，再用对极几何或者PnP求解位姿变化。特征点法对光照变化有一定鲁棒性，因为在提取特征时做了灰度归一化处理，但它的计算量大，因为需要计算描述子并进行暴力匹配。

扫地机器人场景下，我更推荐光流法配合稀疏特征点使用。光流法不需要计算描述子，只需要在图像金字塔上跟踪上一帧的特征点的像素位置，计算量比特征匹配小一个量级。而且在室内家庭环境中，地板、墙壁、家具表面有丰富的纹理，光流跟踪的稳定性非常高。当扫地机器人经过地毯、窗帘这些纹理变化剧烈的地方时，光流也可能跟丢部分特征点，但只需要丢掉那些跟踪质量差的点，剩下的点继续参与解算就行。

VINS-Mono这类成熟VIO框架在视觉前端上采用的是 KLT光流 + 均匀化特征点提取 策略，我记得这个策略在工程上是经过反复验证的，简单且稳定。具体实现就是先用FAST角点检测提取一批候选特征点，然后用KLT光流跟踪它们到下一帧，最后用RANSAC加基础矩阵或者单应矩阵来剔除错误跟踪的外点。

注意：扫地机器人启动地刷时，整机振动会传导到相机上，图像会出现运动模糊。KLT光流在图像模糊时容易产生明显的特征点跳变。我实际的解决办法是，在VIO的输入节点里加入一个图像清晰度评估，用拉普拉斯算子的方差做判断，如果当前帧图像太模糊，就直接丢弃这一帧，不做跟踪，等图像恢复清晰后再跟上。这样会损失一点帧率，但VIO整体的鲁棒性会明显提升。

3.2 IMU预积分的意义：为什么不能直接做数值积分

IMU的原始输出是加速度和角速度，要得到位姿变化，必须做积分。但如果你真的把每一帧IMU数据从IMU坐标系积分到位姿，事情会变得很麻烦。因为IMU的测量值是在IMU坐标系内表达的，而IMU坐标系本身在世界坐标系里是旋转的。每一次新的IMU测量到来，都要重新计算一次世界坐标系下的位姿变换，计算量大不说，更致命的是，如果VIO优化窗口滑动之后，之前估计的IMU位姿发生了变化，那么所有后续积分结果都要从头重新积分一遍。

预积分（preintegration）的作用正在于此。它的核心思想是把两个关键帧之间所有IMU测量数据的相对运动增量提前算好，形成一个“预积分量”，这个量只和IMU的原始测量有关，和绝对位姿无关。当全局优化更新了关键帧的绝对位姿时，预积分量不需要重新计算，直接复用即可。这就像你从家里开车到公司，事先已经记录好了这段路的方向和里程，无论你的起点坐标怎么改，这段相对关系是不变的。

在实际效果上，预积分把IMU数据的处理从“高频累加”转化成了“低频约束”，大幅减少优化过程中需要反复计算的部分，这是VIO在嵌入式平台上能够实时运行的关键实现之一。

3.3 视觉与IMU紧耦合是怎么完成的

以VINS-Mono为例，它的紧耦合体现在优化目标函数中同时包含了视觉残差和IMU残差。视觉残差是特征点从世界坐标系投影到相机成像平面的重投影误差，IMU残差是预积分量与实际位姿变化之间的误差。整个系统维护一个包含N帧关键帧位姿、速度、偏置（bias）以及若干路标点逆深度的滑动窗口，每次有新的关键帧到来，就做一次非线性最小二乘优化。

扫地机器人进入“回充”阶段时那个缓慢的调头、对准、减速入库的动作，仔细观察VIO的输出，你会发现位姿精度在这种低速状态下依然能保持在厘米级，这靠的就是IMU预积分项在低速状态下对微小位移的精确感知。如果只用视觉特征，低速状态下特征点在图像上的位移可能只有几个像素，直接算位姿的噪声会非常大。但如果加入IMU，哪怕移动只有1毫米，IMU也能明确感知到对应的加速度变化。

在具体实现上，偏置（bias）估计也是一个技术重头戏。IMU的陀螺仪和加速度计都存在随时间缓慢变化的零偏，这个零偏不估计的话，预积分量会持续累积误差。VIO前端会在优化过程中实时估计这些bias，并反馈给预积分模块。扫地机器人长期运行两小时后，IMU温度升高会导致bias漂移，所以工业上还会给IMU加温度补偿或者在进行清扫前做一次简短的bias估算。

4. 地图表示与构建：如何让扫地机器人理解家庭环境

4.1 2D栅格地图和3D八叉树地图，怎么选

位姿问题解决了，接下来是地图问题。扫地机器人对地图的需求很明确：要知道哪里是墙、哪里是家具腿、哪里能走、哪里不能走。当前最常用的地图表示是2D栅格地图（Occupancy Grid Map），将环境划分为一个个大小固定的栅格，每个栅格有占据、空闲、未知三种状态。2D栅格地图计算简单、更新方便，非常适合扫地机器人做路径规划。代价地图（costmap）就是在此基础上扩展出来的，在占据、空闲之外给每个栅格叠加了代价值，越靠近障碍物的栅格代价值越高，从而让规划器在做路径搜索时能自动避让出安全距离。

但2D栅格地图有个短板，它表达不了高度信息。扫地机器人底部摄像头如果能看到沙发底下的高度足够，就能钻进去扫，但如果地图只有2D，规划器并不知道那里有可以通过的空间。这时候3D地图表达就有用武之地了。八叉树地图（OctoMap）是一种高效的3D地图表示方式，它把三维空间递归划分成八个子立方体，每个叶子节点存储该空间被占据的概率。由于树形结构天然具有自适应分辨率特性，空旷区域用大节点表示，精细结构用小节点表示，内存利用效率高。

在实际扫地机器人产品中，2D栅格地图仍然是主地图，用于全局路径规划和定位。3D八叉树地图更适合做辅助功能，比如判断机器人能否穿过某些低矮空间，或者在视觉传感器检测到悬空边缘（比如楼梯口）时做3D层面的验证。把两种地图结合使用，是当前比较成熟的工程方案。

4.2 八叉树地图是怎么更新概率的

八叉树地图的核心理念在于概率化占据表达。一个空间点通过传感器（如深度相机）被观测到时，会有射线模型判断它是被占据还是空闲。如果深度相机测得某点距离为d，那么沿着光心到这个点之间的空间都应该是空闲的，而该点本身所在的小立方体应该是被占据的。这种“射线穿越”模型非常直觉化。

每个节点的占据概率更新遵循贝叶斯公式，用对数几率（log-odds）来避免概率值乘法带来的数值不稳定性。设L(n)为节点n的对数几率值，那么当一次新的观测z到来时，更新公式为：

L(n|z) = L(n|z_prev) + L(n|z_obs)

也就是说，新一次观测的对数几率直接叠加到节点原有的对数几率值上。当L值为正，节点更可能被占据，当L值为负，更可能为空。实际维护时通常会设置clamping阈值（比如正负3到4），防止概率值过于极端。

扫地机器人在家庭中高速移动时，一棵八叉树节点的更新频率可能达到每秒几万次。设计上要把射线遍历和概率更新放在后端单独线程里，不能阻塞前端的VIO位姿解算。

4.3 动态障碍物与代价地图的实时更新

扫地机器人真实要面对的挑战从不是一个静止的环境。家里有人走动、宠物跑过、椅子被挪动，这些都是动态障碍物。如果路径规划器把动态障碍物当成静态墙，那机器人很容易被困在原地。

目前ROS2/Nav2里常用的是分层代价地图（layered costmap），典型配置包含：

• 静态图层（static layer）：由预先建好的地图提供静态障碍物信息
• 障碍物图层（obstacle layer）：接收传感器数据，实时标记检测到的障碍物
• 膨胀图层（inflation layer）：对障碍物进行膨胀处理，防止机器人碰撞

动态障碍物的处理核心在于障碍物图层的更新策略。传感器（无论是激光雷达还是深度相机）每帧数据到来时，障碍物层都要清空再重新填充，避免上一帧遗留的障碍物信息干扰下一帧。对于扫地机器人，我实测了不同更新频率的效果，发现局部代价地图的更新频率低于5Hz时，快速移动的人或宠物很容易被遗漏；但更新频率太高又消耗大量CPU。一个折中的方案是障碍物层更新频率设在10Hz左右，清理扫地机器人走过的区域里已消失的障碍物。

5. 路径规划：从全局A*到局部DWA/TEB

5.1 全局路径规划：A*算法的扫地机器人实现

地图构建好了，路径规划器需要在地图上找出一条从当前位置到目标点的代价最低路径。全局路径规划中，最常用的是Dijkstra算法和A算法。Dijkstra能保证找到最短路径，但它没有启发式信息，搜索空间大，效率低。A在Dijkstra基础上增加了一个启发函数，启发函数 f(n) = g(n) + h(n)，其中g(n)是从起点到节点n的实际代价，h(n)是从节点n到终点的估计最小代价。常用的启发函数是欧氏距离或曼哈顿距离。

扫地机器人这种栅格地图场景下，A*搜索的邻域可以选择4邻域或8邻域。4邻域就是上下左右四个方向移动，搜索出来的路径转弯角度为90度的倍数，看起来会比较生硬。8邻域加入了四个对角方向，路径更顺滑，但代价函数的对角移动距离要设置成水平的√2倍，否则搜索出的路径不是真正的最短路径。

我建议在工程落地时做一个改进：A搜索时考虑机器人当前的朝向，对需要转大弯的路径加入额外的转向代价惩罚。这样搜索出来的路径会倾向于少转弯，在实际清扫动作中机器人走得会更顺滑，也能显著减少电机的磨损和电量消耗。这里本质上是把“路径最短”优化成了“时间最短且功耗最低”，虽然和教科书上的标准A不完全相同，但更贴合扫地机器人的真实业务需求。

实现上，A*的伪代码大致如下：

open_set = priority_queue(start) while open_set not empty: current = open_set.pop() if current == goal: reconstruct_path() return closed_set.add(current) for neighbor in current.neighbors(): if neighbor in closed_set: continue tentative_g = current.g + cost(current, neighbor) if tentative_g < neighbor.g: neighbor.g = tentative_g neighbor.h = heuristic(neighbor, goal) neighbor.parent = current open_set.push(neighbor)

5.2 局部路径规划：DWA和TEB，谁更适合扫地机器人

全局路径规划解决的是宏观层面，但实际执行过程中，机器人会遇到全局地图上没有的动态障碍物。局部路径规划器的任务就是根据当前的局部代价地图，实时计算出一条安全的可执行速度指令，既跟随全局路径，又避开动态障碍物。

DWA（Dynamic Window Approach）是扫地机器人领域最经典的局部规划算法。它的思路是在速度空间（线速度v和角速度w）中采样出若干组候选速度，然后模拟每组速度在未来一段时间内的轨迹，再通过评价函数（通常包含朝向目标、障碍物距离、速度大小三项指标）选出最优的一组速度指令。DWA的优势是计算量小、实时性好，且天然考虑了机器人运动学约束，非常适配扫地机器人这种差速驱动底盘。

TEB（Timed Elastic Band）则是另一种思路。它将路径看成一条“时间弹性带”，通过优化方法调整路径上的每个点，在满足运动学约束和避障约束的同时最小化时间成本。TEB生成的路径更平滑，尤其适合窄通道脱困和复杂场景下的灵活避障，但计算量比DWA大，在算力有限的平台上可能出现控制周期不稳定的问题。扫地机器人在大型平坦客厅中清扫时用DWA效率更高；在布满桌椅腿的餐厅区域，TEB的窄道通行能力更强。工程上的理想方案是两种规划器切换使用：开阔区域用DWA，复杂区域切到TEB。

5.3 ROS2/Nav2下的工程落地与关键参数

在ROS2环境下，Nav2是标准的导航系统框架，集成了规划器、控制器、代价地图、行为树等组件。Nav2的架构中，planner_server负责调用全局规划器（通常用Navfn或者Smac Planner），controller_server负责调用局部规划器（DWA或TEB），bt_navigator用行为树把整个导航任务串起来：接收目标点、规划全局路径、控制机器人跟踪路径、检测故障并恢复。

工程上做调试时，我最关注的几个参数是：

• max_vel_x和max_vel_x_backwards：最大前进和后退速度。扫地机器人建议前向速度0.25到0.5m/s，后退速度控制在0.1m/s以内
• acceleration_limit：加速度限制。设置过大会让机器人启停时产生明显冲击，可能导致相机图像模糊；设置过小又会让机器人反应迟钝
• inflation_radius：膨胀半径。这个参数非常关键，设置太大会导致窄通道被完全膨胀掉，机器人认为门口根本过不去；设置太小又会让机器人贴着墙走，容易发生碰撞
• cost_scaling_factor：代价衰减系数，控制代价从障碍物边缘向外衰减的速度

我见过很多扫地机器人项目在调试时卡在“机器人反复原地打转”的问题上，最终排查下来都是因为局部代价地图的膨胀半径设置过大，导致目标点恰好落在膨胀区域内，规划器认为目标不可达，于是机器人开始绕圈寻找可行路径。解决方法是检查目标点是否被膨胀区域覆盖，并适当调小膨胀半径。

6. 实车联调踩坑记录与参数调优实测

6.1 外参标定不准确带来的轨迹漂移现象

在联调阶段，我踩过最深的一个坑是外参标定不准确带来的系统性漂移。现象如下：VIO启动初期，轨迹基本正常，机器人直线前进3到5米之后，位姿开始出现缓慢的横向偏移，偏移方向恒定，而且随着运动距离增长呈线性扩大趋势。

第一次排查时，我怀疑是IMU的bias没有收敛，尝试了加大bias初始化阶段的静止时间，但问题依旧。随后回看了标定bag，发现当时的运动激励不够充分，外参旋转部分的估计不够可靠。重新做了一轮严格的六轴激励标定之后，这个横向漂移问题直接消失了。

排查这类问题时，我的建议是不要一上来就改算法参数，先把标定数据回放，确认内参、外参、时延的标定结果是不是可靠。用开源工具做联合标定后，可以看看Kalibr输出的重投影误差，如果误差在0.5像素以内基本是正常的，如果大于1像素就要警惕标定质量。

6.2 局部规划器振荡问题排查实录

另一个典型问题是DWA局部规划器在狭窄走廊中的振荡问题。现象是机器人在走廊中间来回摆头，前进速度忽快忽慢，表现在轨迹上就是走出一道蛇形。

问题根源在于走廊两侧的障碍物距离很近，膨胀图层在走廊中间留下的“可通行区域”非常窄，DWA采样出的轨迹在这个窄带里，一旦轨迹偏向某一侧，评价函数就会让它向另一侧修正，于是产生了振荡。我采取的措施有两个方向：

第一，缩小DWA的轨迹模拟时间（sim_time），让局部规划器“看得更近”，减少对未来轨迹的过度规划。

第二，在代价地图的膨胀层中设置一个小的内切膨胀半径，让机器人优先保持走廊中心线行驶。此外，当机器人检测到当前处于窄通道时，可以主动降低最大线速度，减小调整频率。

实测下来，sim_time从2.0秒减到1.2秒后，走廊振荡问题基本消失，机器人行驶轨迹变得更直。

6.3 扫地机器人回充对接的融合导航细节

回充对接是扫地机器人最考验融合导航精度的环节之一。机器人需要从房间任意位置回到充电桩，并且最后以毫米级精度对准充电极片。VIO在回充接近段的误差如果超过几厘米，机器人和充电桩就无法成功对接。

工程上常见的做法是：全局导航阶段用VIO+地图融合定位把机器人导航到充电桩附近。等到机器人已经能看到充电桩时，切换到视觉伺服模式，通过红外传感器或者充电桩上的视觉标记进行精确对接位置解算。这个“粗导航+精对接”的二级结构，正是视觉+IMU融合导航稳定性的体现。

VIO在回充过程中的最大挑战是低速状态下的位姿精度。当机器人以每秒几厘米的速度缓慢挪向充电桩时，视觉特征点在图像上的位移极小，噪声占比很高。如果IMU预积分参数设置不当，低速下的微小振动会被误判为运动，导致位姿来回跳。我的调试经验是，在回充段适当放宽IMU预积分的噪声参数，让融合结果更信任视觉特征的小幅位移信息，而不是IMU的高频信号。

6.4 参数配置参考表与实测心得

以下是我在实际调试中总结的一套相对稳妥的初始参数配置，适合家庭场景下的扫地机器人，搭载单目鱼眼相机加消费级IMU，使用VINS-Fusion做VIO前端，ROS2 Nav2做导航。不同的传感器和底盘需要针对性调整，但可以作为起步参考：

联调完成后，我建议用录制好的bag包做离线回放测试，这样每次改动参数时能够保证输入数据完全一致，方便对比参数调整的效果。等离线测试通过了，再上实车验证。我在实际工作中一直坚持这个流程，相比直接改参数上车反复跑现场，这种方式能节省大量时间，也更好定位问题是出在前端VIO还是后端路径规划。

在反复调试夹持视觉与IMU融合导航系统之后，我最大的体会是：算法理论固然重要，但工程中遇到的问题，绝大多数都出在标定、时间同步、参数适配这些看似基础的地方。想起刚入门时因为外参标定不准，整整排查了两周还以为是自己写的VIO代码有bug。如果你最近也在做类似项目，不妨先确认基础数据链路是不是干净的，再回头去看算法实现。这套融合导航方案做扎实之后，后面接语义地图、做定点清洁、上多楼层导航，都会顺畅很多。