企业官网建设流程全解析

说起扫地机器人，很多人都有过这样的体验：明明地上没有障碍，机器却在阳光下绕了一大圈；或者刚扫到一半，机器突然原地打转，地图全乱了套。过去两年我一直在折腾视觉+IMU融合导航这块，踩了不少坑，也把整套链路跑通了一遍。今天这篇就把视觉+IMU融合导航和路径规划的实现原理、标定方法、融合细节、落地实测一次性讲透。我不打算只讲概念，而是把每一步背后“为什么这么做”都拆开：为什么视觉要配IMU而不是只用摄像头？为什么标定能直接决定路径规划的上限？为什么扫地机回充、断点续扫、动态避障，最后比拼的其实是同一个东西——位姿估计的稳定性。

这篇文章适合三类读者：一是做机器人导航算法、想从单传感器方案转向多传感器融合的工程师，二是准备用RK3588、Jetson等平台跑视觉SLAM的硬件选型阶段的朋友，三是已经有导航产品落地、被地图漂移、重定位失败、地毯区域乱跑等问题折磨过的开发者和售后工程师。我会用实际可复现的流程来讲，参考的是业内常用的开源方案和我在真实机器人平台上跑的配置，不是纸上谈兵。

1. 为什么是视觉+IMU？扫地机器人导航的技术选型逻辑

1.1 几种主流导航方案的对比

扫地机器人行业这十多年，导航方案走过了一条非常清晰的演进路线。早期是随机碰撞，机器撞到哪里扫到哪里，纯靠陀螺仪保持一个大概方向；后来出现了激光导航，用单线激光雷达扫描房间轮廓，建图精度高、定位稳定，一下子成了中高端产品的标配；再后来视觉导航崛起，用摄像头替代激光雷达，成本更低、还能识别物体，但问题也不少。现在真正主流的高端方案，其实是视觉+IMU融合，有些还会再加激光雷达做补充。

这几种方案的核心差异，本质上不是“传感器多贵”，而是“如何解决定位问题”。

• 随机碰撞方案：没有地图，没有定位，靠碰撞传感器和红外感知附近障碍，路径完全不可控。
• 纯陀螺仪方案：只有IMU，能感知自身旋转，积分出粗略的位移，但误差随时间快速累积，扫大面积房间时路径会偏。
• 激光SLAM方案：用激光雷达测距，精度高、不受光照影响，能建出精确的栅格地图，适合空旷环境。
• 纯视觉方案：用摄像头连续采集图像，通过特征点匹配估算自身运动，能识别物体和纹理，但天黑、强光、白墙、地毯区域容易丢特征。
• 视觉+IMU融合方案：摄像头提供丰富的环境信息，IMU提供高频运动估计，两者互补，不仅定位更稳，还能支撑更高阶的动力行为。

有人会觉得，既然激光SLAM精度最高，为什么不全面淘汰视觉？原因在于成本，也在于感知维度。激光雷达只能拿到几何轮廓，看不到地板的材质、看不到家具的分类、看不到墙角的污渍，而这些恰恰是扫地机从“能扫”走向“懂扫”必须跨过的坎。视觉+IMU融合是兼顾成本、算力、感知维度、可靠性的一个平衡点。

1.2 视觉与IMU的优势互补：一个像眼睛，一个像内耳

视觉传感器的本质是“被动感知场景结构”。它给扫地机提供的是环境层面的绝对参照——墙在哪、桌子腿在哪、门在哪。但是单目相机有个天生弱点：它无法直接感知尺度。你看到两帧图像里特征点移动了100个像素，但到底是相机自己移动了1米，还是前方3米处有个物体移动了0.3米？在没有深度信息的情况下，单目视觉只能估算出运动的“方向”和“比例”，算不出“真实位移”。这也是很多低端视觉扫地机扫完地图后比例失调的根源。

IMU的作用正好补上这块。它测量的是三轴角速度和三轴线性加速度，频率通常有200Hz甚至更高，不依赖外部环境，能在两个视觉关键帧之间高频积分出相对运动。更关键的是，IMU能感受到重力方向，直接给整个系统提供一个绝对的“水平基准”，这极大地帮助了地图构建和路径规划中的坐标系对齐。

打个比方：视觉像人的眼睛，告诉你房间长什么样、你在哪个位置；IMU像人的内耳和前庭系统，每时每刻感知你是加速了、转弯了还是撞到了。单独用眼睛，闭一下或者晃一下就可能晕；单独用内耳，走一段路就会逐渐偏离方向。两个一起用，视力正常的人闭着眼睛转三圈，睁开眼能迅速重新定位，这就是扫地机要的效果。

1.3 这套方案适用的产品定位与场景边界

视觉+IMU融合并不是万能的，它有自己的适用边界。我最常对产品团队说的一句话是：这套方案解决的是“动态环境中的稳定自主定位”，而不是“无特征环境中的救世主”。

适用场景包括：普通家庭客厅、卧室、办公室，环境里有足够的纹理特征（家具边缘、墙纸图案、地板纹路、地毯花色），光照相对稳定，玻璃门和镜面占比不高。这种环境下，融合方案能提供非常稳定的厘米级定位，足以支撑弓字型全覆盖清扫、沿边清扫、定点清扫、自动回充。

不适用或需要额外兜底的场景包括：大面积纯白墙壁和纯色地面、强阳光直射导致的过曝、整屋落地镜、超大面积空旷空间（特征极度稀疏）、以及剧烈光照跳变（比如夜灯熄灭的瞬间）。这些场景下，视觉特征会大量丢失，如果IMU不能及时兜底，系统就会出现漂移或重定位失败。这也是为什么很多商用扫地机会在视觉基础上再加一颗激光雷达——多一个传感器多一层保底。

2. 动手前先把传感器标定做扎实：标定流程与关键参数

2.1 IMU内参标定：零偏、噪声密度与Allan方差

很多人一上来就急着跑SLAM，结果跑出来的轨迹歪得不成样子，第一反应是算法不行，其实是IMU的标定参数压根就没做对。IMU内参标定的核心是确定三个东西：零偏（bias）、噪声密度（noise density）、随机游走（random walk）。这些概念听着抽象，但直接影响融合结果。

零偏是最直观的：静止放置的IMU，理论测出的角速度是0，但实际会有微小偏移，比如0.01度/s。这个偏移在单帧看微不足道，但如果系统连续运行1小时，光角速度零偏累积出来的角度误差就是36度，扫地机会以为自己在直走，实际早偏到隔壁房间了。所以在初始化阶段测算零偏、并在运行过程中持续估计和补偿bias，是视觉惯性里程计里最核心的问题之一。

Allan方差是用来分析IMU噪声特性的标准工具。把IMU静止放置2到4小时，采集数据后用Allan方差分析，能分离出量化噪声、角度随机游走、零偏不稳定性、速率随机游走等参数。这些参数决定了你在融合算法里应该给IMU的测量值多大的信任权重。一个噪声特性差的IMU，即使外参标定再准，融合效果也会被打折扣。

实操建议是：标定IMU时把设备稳定固定在一个绝对静止的平台上，远离振动源，采集时间不少于2小时。我自己在实验室标定一块BMI088时，发现即使放在桌上，如果旁边有人走路，Allan方差的曲线尾部都会有明显毛刺——微小的振动直接影响低频特性。还有一点必须注意：在标定之前把IMU预热10分钟以上。冷启动的IMU温度漂移非常明显，零偏会随着温度缓慢变化，这个细节不做，后面所有标定数据都得作废。

2.2 相机与IMU外参联合标定

相机和IMU各自的内参标定好之后，还要知道它们之间的相对位姿关系，也就是外参。外参由一个旋转矩阵和一个平移向量组成，描述的是“IMU坐标系在相机坐标系下的位置和朝向”。

外参为什么重要？因为它决定了两路传感器数据能否对齐到同一个空间框架下。如果外参不准，视觉给出的姿态估计和IMU给出的姿态估计就会互相矛盾，融合算法会无所适从——今天觉得偏左，明天觉得偏右，最后地图和真实房间对不上，路径规划自然也全线崩溃。

相机IMU联合标定的标准做法是使用标定板（如Aprilgrid、棋盘格），在相机视野内晃动设备，同时让IMU充分激励（六个方向都动到、多旋转），记录图像和IMU数据，然后用Kalibr这类开源工具离线优化出外参。看起来简单，实际操作有几个坑值得专门提醒：

1. 标定板要足够大，占画面比例尽量高，这样角点检测更稳。
2. 运动要包含充分旋转，特别是绕重力方向的偏航角运动。只平移不旋转，外参的旋转部分不可观，标定结果会非常差。
3. 光照要均匀，标定板不要反光，拍的时候不要有动态物体挡在画面里。
4. 数据的同步精度要好，时间戳对不齐的话，即使外参真值在手，联合优化也会收敛到错误的区域。

我用Kalibr标过一组双目相机和IMU的组合，前两次因为运动模式单一，标出来的平移量明显偏了，后来刻意在IMU的三个轴上都做了大幅旋转运动，重标定后残差下降了近60%。这说明外参标定的质量上限，很大程度取决于你给优化器喂了什么样的运动激励。

2.3 时间同步：数据融合里最容易被低估的一环

传感器标定中，内参外参大家都会做，但时间同步是很多人忽视却又致命的一环。视觉通常以30fps输出图像帧，IMU以200Hz甚至更高频率输出测量值。如果两路数据的时间基准不一致，比如图像时间戳和IMU时间戳差了几十毫秒，融合算法等于在拿“上午的照片”和“下午的陀螺仪数据”去推断当前姿态，结果自然是灾难性的。

更隐蔽的问题是，不少低成本模组的时间戳本身有抖动。比如摄像头驱动在曝光完成后才打时间戳，这个时间戳并不是曝光开始时刻，而曝光过程本身就有10到30毫秒的时长。对快速旋转的扫地机器人来说，10毫秒的时间误差足以让姿态估计出现明显偏差。这也是为什么很多VIO系统会在状态向量里额外估计相机与IMU之间的时间延迟，把它当成一个在线优化的参数，而不是直接信任驱动给的时间戳。

在工程实践上，我建议做两件事：一是给IMU数据打上尽可能精确的硬件时间戳，最好由MCU在IMU中断里直接读取系统时间，避免经过操作系统调度造成的延迟；二是在采集图像时记录曝光开始时间，而不是帧完成时间。这两点做到了，融合系统的稳定性会上一个台阶，很多莫名其妙的抖动问题会不治而愈。

3. 融合的核心：视觉惯性里程计的位姿估计原理

3.1 视觉SLAM与IMU融合的两种主流架构

外参标定好了、时间戳对齐了，下一步进入真正的融合，也就是视觉惯性里程计（VIO）。VIO的架构可以分为两大类：松耦合和紧耦合。理解这两者的差异，有助于你决定自己的系统用哪种方案。

松耦合的思路是：视觉SLAM和IMU分别独立估计位置和姿态，然后两个结果在更高层做融合，通常用扩展卡尔曼滤波（EKF）或简单的加权平均。这种架构实现简单、模块之间耦合度低，视觉算法挂了IMU还能单独顶一阵。缺点是：视觉和IMU没有在底层数据上相互校正，精度和鲁棒性都受限。

紧耦合的思路则是：把视觉特征观测和IMU测量放到同一个优化框架里，让它们互相约束。经典做法有两个流派，一是基于滤波的MSCKF（Multi-State Constraint Kalman Filter），二是基于图优化的滑动窗口法，代表性的算法有VINS-Mono、VINS-Fusion、ORB-SLAM3里的惯性模型。紧耦合的精度和鲁棒性都明显优于松耦合，因为IMU的先验信息能帮助视觉特征匹配，视觉的观测又能反过来修正IMU的漂移。

扫地机器人因为运动模式相对平缓、场景重复度高，我更推荐紧耦合方案。实际跑下来，VINS-Fusion这类开源方案在扫地机场景下表现相当稳定，而且集成了回环检测，可以直接复用。

3.2 IMU预积分与关键帧机制

看到“IMU预积分”这个词，很多人望而生畏，其实它解决的是一个非常具体的问题：同时优化多个关键帧时，IMU测量值太多，不可能每一条都作为优化变量塞进图里，需要把它压缩成一个约束项。

理解预积分最好的方式是类比“记账”。IMU在两个关键帧之间产生了成百上千条加速度和角速度测量，预积分做的事情是：把这一堆测量先自己加总成“这两帧之间的相对运动变化量”，同时估算这个累积过程引入的噪声协方差。之后优化器只需要拿着这个“汇总账单”，而不用去翻每一笔流水。

这个设计还带来一个额外好处：IMU的零偏在优化过程中会发生变化，而预积分对bias的变化是敏感的。所以现代VIO系统在预积分公式里显式地建立了“累积量对bias的雅可比”，当优化器更新了bias估计后，可以快速修正预积分结果，而不需要重新积分全部原始数据。

关键帧机制则是控制计算量的核心。如果每一帧图像都参与后端优化，算力消耗会随帧数快速膨胀，手机、扫地机这种嵌入式平台根本扛不住。关键帧的挑选原则是：当前帧和上一个关键帧之间的视差足够大，或者跟踪的特征点数下降明显，才新建一个关键帧。这样既保证了位姿估计不会因为帧间隔太长而失去约束，也让后端优化只面对一小组有限的关键帧，计算负担可控。

3.3 初始化为什么重要

VIO的初始化是整个系统最脆弱、也最容易被忽略的环节。所谓初始化，就是系统开机的头一两秒里，临时估算出一组初始状态，包括：重力方向、初始速度、IMU零偏、相机与IMU的外参初值，以及最关键的单目尺度因子。

单目相机本身无法获取尺度，但有了IMU的重力观测和加速度积分，系统可以在几分钟的运动中把“视觉轨迹的比例尺”校准出来。这就是为什么VIO系统启动时会提示你“缓慢移动设备”——你需要让IMU充分激励，让算法能够区分重力分量和运动加速度。如果开机就静止不动几秒钟，初始化往往失败，或者给出一组极不合理的尺度估计。

扫地机器人上有一个非常典型的工程处理：开机后先原地转圈，或者走一段“八字形”路径，再开始建图。这本质上就是在主动帮助VIO完成初始化。有些用户在扫地机器人刚启动时看到机器原地转了一圈觉得“很傻”，其实是初始化策略在起作用。如果跳过这个转圈动作直接开扫，建图质量会明显下降，地图比例就偏了。

3.4 回环检测与地图修正

位姿估计无论多好，误差总会缓慢累积，扫地机绕房间扫完一圈再回到起点时，系统以为自己在坐标(10, 3)，实际在(9.8, 3.1)，这就是累积漂移。回环检测是重置这个累积误差的关键机制。

回环检测的思路是：用图像描述子（比如ORB特征、NetVLAD全局描述子）判断“当前看到的场景是不是以前来过的地方”。一旦系统识别出当前帧和地图里某个历史关键帧是同一个地方，就会在图上建立一个“回环边”，把所有积累的位姿误差在这个闭环里重新分配。效果就是，地图经过一次全局优化后，起点和终点漂亮地接上，重合误差被分摊到整条轨迹上。

扫地机家庭场景算是回环检测的“天堂”——每个房间的墙角、家具布局都提供了丰富的区分信息，只要特征提取稳定，回环识别成功率非常高。VINS-Fusion里用的词袋模型（DBoW2）对这类场景处理得很快，全屋地图大概几百到上千张关键帧的规模，回环检测的耗时都在毫秒级，不影响实时性。

不过回环检测也有一个容易翻车的点：重复纹理。我遇到过全屋木地板纹理一致、光线均匀的情况，算法把不同房间误判成同一个地方，回环直接回错了。解决办法是融合环节里对回环候选帧做严格几何一致性校验，也就是用本质矩阵或单应矩阵检查匹配的特征点是否符合真实的位姿关系，通过校验才接受这个回环。这一步不能省，宁可少回一个环，也不能回一个错环。

4. 地图构建与定位：从稀疏特征点到扫地机可用的栅格地图

4.1 从视觉里程计到栅格地图

VIO输出的是什么？是相机的位姿轨迹，是一串“什么时刻我在哪里、朝向哪里”的六自由度位姿。但扫地机器人规划路径不能用“一串位姿轨迹”，它要的是一个可查询的平面占用地图：哪里能走、哪里有墙、哪里是家具底部。

所以系统还需要一个“建图前端”，把VIO给出的轨迹和图像信息转化成2D栅格地图。实用做法是：利用视觉特征点的深度估计，或者配合深度相机/结构光测距，把障碍物投影到地面平面上；同时利用碰撞传感器、红外测距的信息，标记出障碍物的存在区域。这个过程业内常称为costmap（代价地图）构建，每个栅格有一个占据概率值，供路径规划查询。

这里面有一个非常关键的工程技巧：栅格地图不是只维护一张就行，要区分静态层、动态层、膨胀层。静态层记录墙和固定家具，长期保留不更新；动态层实时记录移动的人和宠物，短时间有效，人走远了就慢慢清除；膨胀层是路径规划用的安全缓冲，把障碍物栅格向外扩张，避免机器贴着墙边走过去把外壳撞花。

4.2 特征点选择与光照对抗

视觉SLAM在扫地机器人上最容易翻车的不是算法精度，而是光照。清晨的阳光斜射进客厅，地板上拖出一道长长的影子，这个影子在图像里形成了强烈的明暗边界，会被特征检测器当成一个“很显著的特征点”。等到下午阳光角度变了，影子消失了，系统发现“那个特征点不见了”，于是这一小块区域的位姿约束丢失，轨迹开始飘。

应对光照问题的第一层防线是特征点算法选择。ORB特征点因为在不同光照下相对稳定、速度又极快，是目前嵌入式视觉SLAM的主流选择。但ORB也有弱点，它依赖图像金字塔和灰度梯度，对过曝和欠曝区域很敏感。所以第二层防线是在图像预处理阶段做自适应直方图均衡，把过亮和过暗的细节拉回正常范围。第三层防线是特征点分布控制：强制在全图中均匀采样特征点，防止特征点全集中在高对比度的角落，而让大片光滑地面区域成为“信息盲区”。

我实际测试过一个场景：下午3点，西晒阳光从落地窗照进客厅，地板上有一个超长阴影。如果不做预处理，特征点在阴影边界处扎堆，融合轨迹在阴影边界附近出现肉眼可见的抖动。加上自适应直方图均衡后，抖动基本消失，轨迹平滑度大幅改善。这个优化不需要改算法核心架构，纯粹是前处理技巧，但收益非常明显。

4.3 重定位与绑架问题

扫地机器人工作中经常遇到被用户“动手”的情况——拎起来放到另一个房间，或者被卡住后手动推开。这种状态在SLAM领域被称为robot kidnapping（机器人绑架），意思是机器人的实际位置和内部估计位置完全不一致。此时如果继续按旧位置规划路径，机器人会一头撞向完全不存在的“墙”。

重定位能力决定了绑架后系统能否自救。实现方式主要是全局定位，也就是在地图中搜索与当前图像最相似的历史关键帧，找出来之后，就知道了自己大概在哪。VINS-Fusion里集成了实时重定位模块，扫地机在绑架发生后可以快速恢复定位，然后继续执行未完成的清扫任务。

这里有一个落地价值非常高的细节：重定位之后，尽量让扫地机原地旋转一圈，采集四周图像做一次局部观测匹配，再开始移动。因为单张图像只能确定“大概在哪”，旋转一圈之后，位姿的不确定性会大幅收敛。很多扫地机被拎起来再放回去之后，先转个圈再走，就是这个原因，不是无用动作。

5. 路径规划：把“我在哪”变成“我该怎么走”

5.1 全覆盖路径规划：弓字型清扫为何主流

定位建图解决的是“我在哪”和“房间什么样”两个问题，接下来就是“我该怎么扫”。扫地机器人的核心清扫需求不是从A点到B点的导航，而是一块区域全覆盖、不重不漏地走完。这在规划领域被称为全覆盖路径规划，覆盖质量直接决定了用户的满意度。

最经典的全覆盖算法是弓字形覆盖，也叫牛耕式往返覆盖。思路非常简单：把待清扫区域分解成若干个长条形子区域，机器人沿每个长条从头走到尾，然后转到下一列反向走，像牛耕地一样把整块地犁一遍。这种路径的优势是转弯次数少、覆盖率高、不容易漏扫。

但实际家庭环境绝不会是矩形，客厅里有电视柜、茶几、地毯，弓字形不能直接硬套。所以工程实现上要把区域进行空间分解，常用的有梯形分解法（把非凸区域切成多个凸多边形分别覆盖）和基于栅格的形态学处理（先膨胀障碍物，再把可通行区域按连通域切块）。每个连通域内部做弓字型规划，连通域之间再规划连接路径。

我见过不少开发者以为全网覆盖就是把地图网格化，然后一行一行扫描过去，结果遇到U型区域就卡在角落里出不来。原因是网格化的弓字型路径需要知道区域的边界走向，如果区域边界是不规则多边形，必须先把区域分解为多个近似凸的子区域，在每个子区域内部做往返覆盖，再处理子区域间的衔接。这一步不做，覆盖率永远上不去。

5.2 局部规划与动态避障：DWA与TEB的取舍

全覆盖路径是全局层面的规划，但执行过程中随时可能出现动态障碍物——家里的人走动着、宠物趴在地板上、小孩丢了一个玩具。这些障碍物不在全局地图上，需要局部规划器来实时绕开。

常用的局部规划器有两种路线：DWA（动态窗口法）和TEB（时间弹性带）。DWA的原理是：在当前速度和角速度的可达范围内采样出一组候选速度组合，对每个组合模拟出一条轨迹，再根据“是否接近目标、是否撞障碍物、是否偏离全局路径”这个评分函数打分，选最高分的轨迹执行。它计算简单、实时性好，适合差速底盘。

TEB则把一条路径表示成一串带有时间信息的位姿序列，把“避开障碍物”“平滑”“符合机器人运动学”“尽快到达”这些目标都写成代价项，然后用图优化求解。TEB生成的轨迹更平滑，在窄通道和复杂障碍物环境中的表现优于DWA，但计算量更大，参数也更多，调试起来更费精力。

在扫地机这种算力有限的平台上，我的经验是：全局路径平滑、动态障碍物不多的情况下，DWA已经足够；如果要做更细腻的绕障动作，比如绕开地上的袜子再回到弓字型路径上，TEB的表现更接近人预期。但对扫地机来说，关键是“回到未覆盖区域”的能力，这个能力更多依赖全局路径的断点续扫逻辑，而不只是局部规划器选型。

5.3 规划与感知/定位的耦合：路径规划质量其实是定位质量的上层体现

路径规划看似是规划层的事，但在真实的扫地机器人系统里，规划质量直接受限于定位质量。一个很容易被忽视的结论是：如果定位误差是10厘米，哪怕全局规划、局部规划算法再完美，机器人在真实世界里的扫街效果也一定会出现10厘米级别的偏差——墙边扫不到、转角扫不干净、弓字型路径越来越歪。

这就是“规划是位姿估计的上层应用”这句话的含义。很多团队在路径规划上反复调参，弓字形间距调小、沿边算法改了几版，覆盖率还是上不去，最后发现根因是VIO漂移导致地图边界本身就偏了。所以我在做系统设计时，始终坚持一个原则：先把定位系统的漂移误差控制好，再谈路径规划优化。视觉+IMU融合的价值也正在这里——它不是显式出现在“规划算法”的字眼里，但它决定了规划算法能在多可靠的坐标系里工作。

另一个耦合点是卡尔曼滤波或因子图框架中，定位结果会提供当前位姿的不确定性协方差。路径规划可以利用这个协方差动态调整安全缓冲区——定位不确定度大的时候，扫地机离墙远一点、速度慢一点；不确定度小的时候，路径可以更贴边走、效率更高。这种“不确定性感知的规划”目前是前沿方向，但在高端扫地机产品中已经有初步落地。

6. 落地实测：我在RK3588平台上跑整套方案的配置与避坑记录

6.1 算力选型与硬件配置

上面讲了那么多理论，真正落地的时候，第一个问题就是算力够不够。我测试用的平台是瑞芯微RK3588，8核CPU，集成6 TOPS NPU，对扫地机器人这种功耗受限的产品来说，算力处于中高端水平。

跑VIO的算力分布大致是这样的：视觉前端（图像预处理、特征提取、特征匹配）最吃CPU，建议跑在大核上；IMU积分和预积分计算量不大，可以放在小核；后端图优化（滑动窗口优化、回环检测）是稀疏矩阵优化，CPU浮点性能决定速度，但频率不需要太高；如果要跑神经网络，比如语义分割、物体识别，NPU负责这部分。

我用的传感器配置是：30fps全局快门单目摄像头，200Hz的6轴IMU模块，图像分辨率640×480，VIO前端特征点数量限制在150个左右，关键帧间隔控制在5到10帧之间。实测CPU占用率在30%到45%之间波动，整体发热可控，电池续航影响不大。

这里有一个很有价值的经验：VIO的前端特征提取不建议用分辨率过高的图像。720P比640×480的特征提取耗时多了近一倍，但特征点匹配精度提升非常有限。对扫地机这种运动速度本就不快、场景尺度又不大的设备来说，640×480足够支撑高质量的位姿估计，省下来的算力可以留给语义感知或更频繁的局部路径重规划。

6.2 从标定到实跑的完整流程记录

把整套流程串起来，我按下面这个顺序操作，每一步都验证通过后才进入下一步：

1. IMU内参标定。用静置2小时的6轴数据做Allan方差分析，得到零偏、角度随机游走、速率随机游走参数。同时记录温度，确认零偏随温度漂移的范围，写进驱动层做温度补偿。
2. 相机内参标定。用棋盘格采集50到80张不同角度的图像，用OpenCV标定出焦距、主点、畸变系数。注意两个图像边缘的畸变系数一定要准确，边缘畸变误差会让后续特征点匹配在图像边缘区域频繁失败。
3. 相机IMU联合外参标定。用Kalibr，带Aprilgrid标定板，做多组大范围旋转运动，每组数据时长约3分钟。标定后检查重投影误差，误差控制在0.3像素以内算合格。
4. 跑VINS-Fusion，用录制好的bag数据离线验证轨迹精度，再切到实时模式在线跑。
5. 构建代价地图，做区域分解，验证弓字型全覆盖路径在真实房间里的覆盖率。
6. 加入DWA局部规划，测试动态避障效果；加入全局重定位，测试绑架恢复。

这套流程走完，整个导航栈算是成型了。整个过程中每一步都有可能出现偏差，但最耗时的环节永远是标定——标定数据好，后面所有问题都好解决；标定数据差，后面所有模块都在“将错就错”中互相挖坑。

6.3 实测中那些文档里不会写的坑

我把自己踩过的几个坑列出来，希望能帮你少走弯路。

第一个是地毯区域的视觉特征丢失。家中长毛地毯几乎没有可提取的角点特征，VIO在这个区域会临时退化为纯IMU积分模式，时间一长就会有漂移。我的方案是：检测到地毯区域（可以用深度信息或语义分割识别），主动降低对该区域的视觉特征权重，更多信任IMU，同时触发沿地毯边缘的清扫策略，减少进入地毯内部的弓字形路径长度。这样既减轻了特征丢失带来的漂移，又保证了清扫覆盖。

第二个是轮子打滑对IMU的干扰。扫地机在推过门槛、压到充电线时，轮式里程计会瞬时失真。如果融合方案里把轮式里程计和IMU做了联合，打滑瞬间的IMU加速度计也会被污染，VIO的初始化条件会短期变差。解决思路是检测轮子打滑事件，打滑期间降低轮式里程计权重、暂时提高视觉权重，直到机器人恢复平稳移动。

第三个是偏航角慢漂。IMU的yaw角长期来看一定会缓慢漂移，即使有视觉修正，也架不住长时间的重复纹理环境。我做的处理是周期性触发回环检测，让系统回到已探索区域时主动进行一次闭环修正。在扫地机场景里，弓字型路径天然会频繁回到起点附近，只要回环检测阈值设置合理，yaw漂移基本能被控制在一个很小的范围内。

第四个是回充对接时的位姿精度问题。回充需要万向轮对充电桩插座的定位达到厘米级，VIO在靠近充电桩时会因为视野过近、特征点过于集中的问题导致位姿估计不稳定。我的做法是：回充前切到低速模式，用充电桩上的红外/视觉标签做局部精确对准，VIO只提供粗定位。这也是为什么很多扫地机回充的最后一步会有个缓慢的“找准”动作，不是规划迟钝，而是传感器模式在切换。

这些坑在开源文档和官方wiki里通常不会写，但它们恰恰是决定一个导航系统“能用”和“好用”之间巨大差距的关键。

从标定到融合再到规划，视觉+IMU融合导航这条路的价值不在单点算法有多先进，而在于把每个环节的误差都控制在可接受范围，让系统在真实的家庭环境里连续跑几个小时不漂、不丢、不撞。我个人的体会是：一个导航系统跑得稳不稳，七成取决于标定和数据质量，两成取决于融合策略，只有一成取决于路径规划本身的算法创新。先把前面九成做好，路径规划自然就顺畅了。