城市移动机器人定位：单目视觉+低等级IMU+车轮里程计融合方案

1. 项目概述：当视觉与惯性导航在城市中“跛脚”前行

如果你尝试过在室内或者城市峡谷（高楼林立的街道）里跑过视觉SLAM或者惯性导航，大概率会碰到一个让人头疼的问题：定位轨迹飘得亲妈都不认识。纯视觉（单目）容易在纹理缺失、快速运动时跟丢，而低成本的惯性测量单元（IMU，比如手机里的那种）自己单独用，误差累积速度堪比雪球下山，几分钟就能偏出几百米。这个项目——“在城市地区使用低等级IMU的单目视觉惯性车轮里程计”——瞄准的正是这个痛点。它的核心思路非常巧妙：既然视觉和低等级IMU在城市复杂环境下各自都有“残疾”，那为什么不给它们找一个稳定可靠的“拐杖”呢？这个“拐杖”就是车轮里程计。

简单来说，这不是一个从零开始的SLAM系统，而是一种紧耦合的传感器融合框架。它把单目相机、低成本IMU（我们称之为“低等级”，通常指消费级MEMS IMU，噪声大、零偏不稳定）和车轮编码器（通过测量车轮转速推算位移）的数据拧成一股绳，共同估算机器人的位置、姿态和速度。在城市场景中，GNSS信号时有时无，纯视觉可能因为玻璃幕墙的反光、动态车辆行人、重复纹理而失效，低等级IMU又靠不住。此时，车轮里程计提供的相对位移信息，虽然也有滑移等误差，但在短时间尺度内，其精度和可靠性往往远超纯惯性积分，这就为整个系统提供了一个至关重要的“锚点”，能有效抑制IMU零偏的快速发散和视觉的尺度不确定性。

我做过不少机器人定位项目，实测下来，这种融合方案对于地面移动机器人（如配送机器人、巡检车）在城市半结构化环境中的长期运行，稳定性提升是立竿见影的。它不追求绝对的高精尖传感器，而是用算法弥补硬件的不足，非常务实。接下来，我就为你拆解这套系统的设计思路、核心实现细节以及那些只有实际调过才知道的“坑”。

2. 系统核心：为什么是紧耦合与因子图？

2.1 传感器特性与互补性分析

首先得明白我们手头这三个“伙计”的脾气。

单目相机：提供丰富的环境纹理信息，能恢复出稀疏或半稠密的地图点云，并通过特征匹配估计相机运动。但它天生有缺陷：一是尺度不确定性，单张图片无法知道真实距离，必须通过运动（三角化）来恢复，这个尺度容易漂移；二是对光照、纹理、动态物体非常敏感。在城市里，一片光滑的墙面、一辆突然驶过的公交车，都可能导致特征跟踪失败。

低等级IMU：通常包含三轴加速度计和三轴陀螺仪，以高频（通常100-500Hz）提供角速度和线加速度测量。它的优势是高频、独立、不受光照影响。但劣势更明显：噪声大，尤其是零偏（Bias）会随着温度和时间缓慢变化，对加速度计和陀螺仪的积分会分别导致速度、位置的误差呈二次方和三次方增长。简单积分几分钟，位置误差就可能达到上百米，这就是所谓的“发散”。

车轮里程计：通过安装在驱动轮上的编码器测量车轮转过的圈数，结合已知的车轮半径，可以计算出车辆在短时间内的相对位移。它的优点是：在非打滑、非空转的条件下，位移测量在短时间内相对准确，误差增长近似线性（与时间成正比），且不受外界环境（光照、纹理）影响。但缺点也很致命：无法感知侧滑（对于非全向轮模型）、车轮半径会因胎压、磨损而变化（需要标定），且长时间积分也会累积误差。

互补性体现在哪？

1. 视觉提供绝对约束，但缺尺度；车轮里程计提供带尺度的相对位移，但缺绝对参考：将视觉估计的无尺度的相对运动与车轮里程计提供的有尺度的相对位移对齐，可以瞬间解决单目视觉的尺度模糊问题，并校正车轮里程计的尺度因子（如果车轮半径标定不准）。
2. IMU提供高频姿态，但位置会飘；车轮里程计提供相对位置，但无法直接提供姿态：IMU的高频角速度积分可以提供平滑的姿态（旋转），但位置估计会因加速度计零偏而快速发散。车轮里程计提供了相对位置变化，通过与IMU估计的位置变化进行比较，可以反过来约束和估计IMU的加速度计零偏，从而大幅抑制位置发散。
3. 视觉在特征丰富时准，但可能丢；车轮里程计在打滑时不准，但持续有：当视觉因环境问题暂时失效时，系统可以退化为“IMU+车轮里程计”的惯性里程计模式，虽然精度下降，但不会完全丢失定位。当车辆起步、刹车导致车轮轻微打滑时，视觉和IMU的约束可以帮助检测和补偿这种滑移误差。

2.2 紧耦合因子图优化框架选型

主流的多传感器融合状态估计框架主要有两种：基于滤波的（如ESKF）和基于优化的（如图优化）。这个项目选择基于因子图的非线性优化，这是目前学术界和工业界在精度和灵活性上的主流选择。为什么不用EKF？

• 更好的线性化点：EKF在每一步都用当前状态估计值进行线性化，如果估计值不准，线性化误差会很大。而图优化（如iSAM2）通常是在一个时间窗口内进行多次迭代优化，能收敛到更优的点，对初始值不敏感。
• 更自然的回环处理：因子图可以非常方便地加入回环检测因子，进行全局优化，纠正累积误差。而EKF处理回环需要复杂的状态增广或维护多个滤波器。
• 灵活性：因子图可以轻松地融合不同类型的约束（视觉重投影误差、IMU预积分误差、车轮里程计误差、先验误差等），模块化程度高。

在这个系统中，我们估计的状态量通常是滑动窗口内的一系列关键帧状态。每个关键帧的状态X_i可能包括：

• 位置p_i(3x1)
• 姿态（四元数q_i或旋转矩阵R_i) (4x1 or 3x3)
• 速度v_i(3x1)
• IMU加速度计零偏b_a_i(3x1)
• IMU陀螺仪零偏b_g_i(3x1)
• （可选）车轮里程计尺度因子s和外参T_b_o（车身到里程计坐标系的变换）

这些状态通过不同类型的因子连接起来，形成一个因子图。优化的目标就是找到一组状态X，使得所有因子的误差之和最小。

3. 核心因子设计与误差模型

这是整个系统的算法心脏。我们需要为每一种传感器测量设计对应的误差项（因子）。

3.1 视觉重投影因子

这是最经典的因子。对于在关键帧i中观测到的地图点l，其像素坐标为z_i^l。我们根据该点的世界坐标P^l_w、相机位姿T_i和相机内参K，可以计算出重投影的像素坐标zhat_i^l。视觉重投影误差就是观测值与预测值之差：

e_vis = z_i^l - zhat_i^l

这个误差服从高斯分布，其信息矩阵（协方差的逆）通常与特征点检测的不确定性有关。在优化时，我们最小化所有此类误差的加权平方和。这里的一个关键技巧是使用逆深度参数化来初始化单目地图点，这对优化数值稳定性更友好。

3.2 IMU预积分因子

直接使用IMU的原始测量值进行优化非常低效，因为IMU频率高，且其测量值与状态之间的关系是动态的。预积分技术解决了这个问题。它在两个连续关键帧i和j之间，将这段时间内的所有IMU测量值积分起来，得到一个与状态无关的“预积分量”ΔR_ij, Δv_ij, Δp_ij。这个预积分量只依赖于IMU的测量值和零偏。

然后，我们可以建立IMU因子误差。例如，位置预积分误差可以写为：

e_p = R_i^T (p_j - p_i - v_i Δt - 0.5 g Δt^2) - Δp_ij

其中R_i是帧i的姿态，p_j, p_i是位置，v_i是速度，g是重力矢量，Δt是时间差。Δp_ij是预积分得到的位置变化。速度、姿态的误差形式类似。

预积分的核心优势：当对状态X_i进行优化迭代时，我们不需要重新积分从i到j的所有IMU数据，只需要用更新后的零偏对预积分量进行一阶近似修正，计算量大大降低。这是紧耦合VIO能实时运行的关键。

注意：IMU预积分的推导和实现是工程上的一个难点，涉及到了李群李代数、伴随性质、噪声传递等。通常我们会直接使用GTSAM或VINS-Mono等开源库中已经实现好的IMU预积分模块，但理解其原理对于调试至关重要。

3.3 车轮里程计因子

这是本项目区别于普通VIO的核心。车轮里程计测量的是车辆在里程计坐标系（通常固定在车体上，比如后轴中心）下的二维平面运动（Δx, Δy, Δθ），或者对于差分轮式机器人，是左右轮的增量行程Δs_l, Δs_r。

我们需要将它转换为对状态量的约束。这里涉及两个关键部分：

1. 外参标定T_b_o：即IMU/相机坐标系（我们估计状态的主体）与车轮里程计坐标系之间的变换。这是一个需要在线或离线标定的常量。通常包含一个平移p_b_o和一个旋转R_b_o。
2. 运动模型：根据机器人模型（如两轮差分、阿克曼转向），将车轮编码器读数转换为车身坐标系下的位移增量。

假设我们通过运动模型，得到了在里程计坐标系O下，从时刻i到j的相对位姿变换T_oij。那么，在主体坐标系B（IMU系）下，我们预测的相对变换应该是：

T_bij_pred = T_b_o * T_oij * T_o_b（其中T_o_b是T_b_o的逆）

而我们根据状态估计值得到的主体坐标系相对变换为：

T_bij_est = T_w_bi^{-1} * T_w_bj（T_w_b是世界系到主体系的变换）

那么，车轮里程计因子的误差就可以定义为这两个变换之间的差异。通常我们使用李代数上的差值来定义误差。例如，对于位置部分：

e_odo_p = trans(T_bij_est) - trans(T_bij_pred)

其中trans()表示取变换矩阵的平移部分。对于旋转部分，可以使用旋转矩阵差值的李代数。

更实用的建模：对于地面车辆，我们常常可以引入平面运动假设（在平坦地面行驶）。此时，我们可以将车轮里程计的约束简化为对x, y, yaw（偏航角）的约束，而放松对z, roll, pitch（高度、横滚、俯仰）的约束，因为车轮里程计对后三者的观测能力很弱。这可以通过设计合适的信息矩阵（给x, y, yaw较大的权重，给z, roll, pitch较小的权重或零权重）来实现。

3.4 融合策略与初始化

系统启动时，状态是未知的。一个鲁棒的初始化流程至关重要：

1. 纯视觉SfM：首先，系统会运行一段时间的纯视觉特征跟踪和三角化，恢复出一些地图点，并估计出一些关键帧的位姿，但此时尺度是未知的。
2. 对齐与尺度恢复：同时，车轮里程计会提供有尺度的位移信息。在收集了足够多的视觉帧和对应的里程计位移后，我们可以通过一个最小二乘问题，将视觉估计的无尺度轨迹与车轮里程计的有尺度轨迹进行对齐（相似变换：尺度s+ 旋转R+ 平移t），从而一次性求解出视觉的尺度s、视觉坐标系到里程计坐标系的旋转R（外参旋转的一部分）以及重力方向。
3. IMU偏置初始化：有了尺度和重力方向，就可以将视觉估计的速度与IMU预积分得到的速度进行比较，初步估计出陀螺仪和加速度计的零偏。
4. 联合优化：初始化完成后，系统就进入正常的紧耦合优化流程。新的关键帧被加入到滑动窗口中，旧的被移出，同时不断添加新的视觉、IMU和里程计因子，进行局部图优化。

4. 工程实现与关键参数调试

理论很美，但让系统跑起来且跑得稳，才是真正的挑战。这里分享一些工程实现上的要点和参数调试的经验。

4.1 传感器同步与时间戳处理

这是多传感器系统的“地基”，没打好，后面全塌。

• 硬件同步：最理想的是所有传感器使用同一个硬件触发信号（如FPGA产生脉冲）。但对于低成本系统，通常做不到。
• 软件同步：更常见的是在软件层面进行时间对齐。为每个传感器的数据流打上主机时间戳（CPU时钟）。由于传输和采集延迟，这个时间戳并不精确。我们需要做：
  • 时间戳补偿：测量并补偿每个传感器的固定延迟（如相机曝光到图像到达内存的时间）。
  • 插值对齐：IMU和车轮编码器频率高，相机频率低。在创建因子时，对于相机时刻t_c，我们需要通过插值得到t_c时刻的IMU状态和里程计增量。对于IMU预积分，起始和结束时间必须精确对应两个关键帧的时间戳。

  • 实操心得：务必记录原始传感器时间戳和主机时间戳。调试时，绘制不同传感器数据的时间序列图，检查是否有明显的错位或跳跃。一个简单的检查方法是让机器人静止，观察融合后的位姿输出是否还有漂移，如果有，很可能是时间戳没对齐导致的。

4.2 外参标定

T_b_c(IMU到相机) 和T_b_o(IMU到里程计) 这两个外参必须足够准确。

• T_b_c：可以使用Kalibr等工具进行离线标定，精度要求较高。
• T_b_o：这个外参的标定更容易被忽视，但也非常关键。特别是旋转部分，如果不准，会导致里程计提供的运动方向与IMU/视觉估计的方向不一致，引入矛盾。一个实用的在线标定方法是：
  1. 让机器人进行匀速直线运动（或绕圈运动）。
  2. 在此期间，视觉+IMU可以估计出一个比较可靠的轨迹（因为运动简单，视觉不易丢，IMU发散慢）。
  3. 将视觉+IMU估计的轨迹与车轮里程计积分轨迹进行对齐（同样是求解一个相似变换或刚体变换），即可得到T_b_o的初始值。在后续优化中，可以将T_b_o的一部分参数（如平移的z分量，旋转的x, y分量）设置为固定值，只优化其他不确定的部分。

4.3 噪声参数与信息矩阵设置

每个因子都有一个信息矩阵（协方差矩阵的逆），它代表了我们对这个传感器测量值的信任程度。设置不当，会导致某个传感器“话语权”过大或过小。

• 视觉因子：信息矩阵通常与特征点提取的尺度（金字塔层级）相关。更底层的特征，定位更准，信息矩阵权重更大。也可以根据特征跟踪的长度来动态调整权重，跟踪时间长的特征更可靠。
• IMU因子：噪声参数包括陀螺仪和加速度计的白噪声（ng,na）和随机游走噪声（nbg,nba）。这些参数通常可以在IMU的数据手册中找到，但“数据手册参数”往往过于理想。一个更可靠的方法是进行IMU静止采集，通过计算艾伦方差来估计这些噪声参数。如果参数设得太大，系统会过于信任IMU，在IMU零偏变化时反应迟钝；设得太小，则会过于依赖视觉和里程计，IMU的高频姿态平滑效果变差。
• 里程计因子：这是调试的重点。我们需要为里程计因子的不同分量设置不同的权重。
  • 平面移动机器人：对于x, y, yaw的测量，我们可以给较高的置信度。权重可以基于经验设置，例如，假设里程计在1米位移内的误差是0.01米，那么位置分量的标准差可以设为0.01，信息矩阵对应元素就是1/(0.01^2)=10000。
  • 对于z, roll, pitch：由于车轮里程计几乎不提供这些信息，我们应该给它们非常大的标准差（例如10米，10弧度），这样在优化中这些分量几乎不起作用，完全由IMU和视觉（如果可见地面特征）来约束。

  • 避坑技巧：当机器人转弯时，由于轮胎形变和非完整约束，里程计在y方向（侧向）的测量可能非常不准。可以考虑在转弯时，动态降低里程计y方向分量的权重，或者使用更复杂的运动模型来估计滑移角。

4.4 滑窗优化与边缘化

为了控制计算量，我们只优化滑动窗口内最近N个关键帧的状态。当新的关键帧加入时，最老的关键帧会被移出窗口。直接扔掉它是不行的，因为老帧携带的约束信息（特别是IMU预积分）对当前状态仍有影响。这时就需要边缘化。

边缘化是一种将旧状态变量从优化问题中移除，但将其携带的信息以先验因子的形式保留下来的技术。这个先验因子会作用于剩余的所有状态变量，保持系统的信息一致性。如果边缘化处理不当（比如边缘化了一个位姿变化剧烈的帧），会导致系统线性化点不准，从而引入人为的“信息矩阵病态”问题，表现为估计结果出现诡异的漂移或抖动。VINS-Mono中提出的“优先边缘化视差大的帧”的策略，就是针对这个问题的有效经验。

5. 城市场景挑战与针对性处理

城市环境给这个系统带来了独特的挑战，需要针对性地处理。

5.1 动态物体干扰

车辆、行人、自行车等动态物体会污染视觉特征。如果这些动态物体上的特征点被用于建图和定位，会导致严重的估计错误。

• 动态特征剔除：
  • 几何一致性检查：通过多视图几何（本质矩阵或单应矩阵）检查特征点是否满足静态场景约束。动态点会表现为外点。
  • 语义分割：使用轻量级的深度学习模型（如MobileNet-SSD）实时检测出车辆、行人等动态物体，并屏蔽这些区域内的特征点。这是目前最有效的方法，但会增加计算开销。
  • 光流一致性：对于密集的动态物体（如人流），可以计算稠密光流场，将运动模式不一致的区域剔除。

5.2 光照与天气变化

清晨、正午、黄昏的光照差异巨大，玻璃幕墙的反光、夜间灯光不足等都是问题。

• 特征描述子选择：传统的ORB特征在光照变化下稳定性一般。可以考虑使用更鲁棒的特征，如SIFT（计算量大）或学习得到的特征描述子（如SuperPoint，需要GPU）。
• 多曝光或HDR图像：如果相机支持，使用自动曝光或拍摄多帧不同曝光的图像进行融合，以增强特征在不同光照下的可见性。
• 直接法补充：在纹理极度稀疏的区域（如纯色墙面），特征法可能完全失效。可以引入稀疏直接法（如DSO），直接利用图像灰度梯度进行跟踪，对特征法形成补充。

5.3 GNSS信号缺失与融合

在城市峡谷中，GNSS信号时有时无，且多路径效应严重，定位结果可能跳变。

• 松耦合融合：将我们估计的视觉惯性车轮里程计的输出，与GNSS的定位结果进行松耦合融合（例如使用EKF）。当GNSS信号质量好时（如HDOP值小），用它来校正里程计的全局位置漂移；当GNSS信号差时，完全信任里程计。这可以为系统提供绝对的全局坐标，避免长期运行的累积误差。
• 信号质量评估：使用GNSS接收机提供的DOP（精度因子）、卫星数、信噪比等指标，实时判断GNSS数据的可靠性，动态调整融合权重。

5.4 实践问题排查清单

当你发现系统轨迹飘移、抖动或突然跳变时，可以按以下顺序排查：

最后，我想分享一点个人体会：这种多传感器融合系统，其鲁棒性往往不是由最先进的算法决定的，而是由最薄弱传感器的故障处理机制决定的。在城市里，视觉可能会短暂失效，车轮可能会打滑，IMU可能会受到冲击。一个好的系统，必须在设计时就考虑到这些故障模式，并设计相应的降级策略。例如，当视觉跟踪的特征数低于阈值时，自动增大里程计和IMU的权重；当检测到车轮空转（电机转速高但里程计位移小）时，暂时抛弃里程计数据。这种“传感器健康管理”的逻辑，和核心的融合算法同样重要，它决定了系统在实际复杂环境中能否真正“活下去”。这套“单目视觉+低等级IMU+车轮里程计”的方案，正是在成本和可靠性之间找到了一个非常棒的平衡点，让城市环境下的自主移动机器人，有了一双更值得信赖的“眼睛”和“腿脚”。