1. 项目概述当视觉与惯性导航在城市中“跛脚”前行如果你尝试过在室内或者城市峡谷高楼林立的街道里跑过视觉SLAM或者惯性导航大概率会碰到一个让人头疼的问题定位轨迹飘得亲妈都不认识。纯视觉单目容易在纹理缺失、快速运动时跟丢而低成本的惯性测量单元IMU比如手机里的那种自己单独用误差累积速度堪比雪球下山几分钟就能偏出几百米。这个项目——“在城市地区使用低等级IMU的单目视觉惯性车轮里程计”——瞄准的正是这个痛点。它的核心思路非常巧妙既然视觉和低等级IMU在城市复杂环境下各自都有“残疾”那为什么不给它们找一个稳定可靠的“拐杖”呢这个“拐杖”就是车轮里程计。简单来说这不是一个从零开始的SLAM系统而是一种紧耦合的传感器融合框架。它把单目相机、低成本IMU我们称之为“低等级”通常指消费级MEMS IMU噪声大、零偏不稳定和车轮编码器通过测量车轮转速推算位移的数据拧成一股绳共同估算机器人的位置、姿态和速度。在城市场景中GNSS信号时有时无纯视觉可能因为玻璃幕墙的反光、动态车辆行人、重复纹理而失效低等级IMU又靠不住。此时车轮里程计提供的相对位移信息虽然也有滑移等误差但在短时间尺度内其精度和可靠性往往远超纯惯性积分这就为整个系统提供了一个至关重要的“锚点”能有效抑制IMU零偏的快速发散和视觉的尺度不确定性。我做过不少机器人定位项目实测下来这种融合方案对于地面移动机器人如配送机器人、巡检车在城市半结构化环境中的长期运行稳定性提升是立竿见影的。它不追求绝对的高精尖传感器而是用算法弥补硬件的不足非常务实。接下来我就为你拆解这套系统的设计思路、核心实现细节以及那些只有实际调过才知道的“坑”。2. 系统核心为什么是紧耦合与因子图2.1 传感器特性与互补性分析首先得明白我们手头这三个“伙计”的脾气。单目相机提供丰富的环境纹理信息能恢复出稀疏或半稠密的地图点云并通过特征匹配估计相机运动。但它天生有缺陷一是尺度不确定性单张图片无法知道真实距离必须通过运动三角化来恢复这个尺度容易漂移二是对光照、纹理、动态物体非常敏感。在城市里一片光滑的墙面、一辆突然驶过的公交车都可能导致特征跟踪失败。低等级IMU通常包含三轴加速度计和三轴陀螺仪以高频通常100-500Hz提供角速度和线加速度测量。它的优势是高频、独立、不受光照影响。但劣势更明显噪声大尤其是零偏Bias会随着温度和时间缓慢变化对加速度计和陀螺仪的积分会分别导致速度、位置的误差呈二次方和三次方增长。简单积分几分钟位置误差就可能达到上百米这就是所谓的“发散”。车轮里程计通过安装在驱动轮上的编码器测量车轮转过的圈数结合已知的车轮半径可以计算出车辆在短时间内的相对位移。它的优点是在非打滑、非空转的条件下位移测量在短时间内相对准确误差增长近似线性与时间成正比且不受外界环境光照、纹理影响。但缺点也很致命无法感知侧滑对于非全向轮模型、车轮半径会因胎压、磨损而变化需要标定且长时间积分也会累积误差。互补性体现在哪视觉提供绝对约束但缺尺度车轮里程计提供带尺度的相对位移但缺绝对参考将视觉估计的无尺度的相对运动与车轮里程计提供的有尺度的相对位移对齐可以瞬间解决单目视觉的尺度模糊问题并校正车轮里程计的尺度因子如果车轮半径标定不准。IMU提供高频姿态但位置会飘车轮里程计提供相对位置但无法直接提供姿态IMU的高频角速度积分可以提供平滑的姿态旋转但位置估计会因加速度计零偏而快速发散。车轮里程计提供了相对位置变化通过与IMU估计的位置变化进行比较可以反过来约束和估计IMU的加速度计零偏从而大幅抑制位置发散。视觉在特征丰富时准但可能丢车轮里程计在打滑时不准但持续有当视觉因环境问题暂时失效时系统可以退化为“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。我们需要将它转换为对状态量的约束。这里涉及两个关键部分外参标定T_b_o即IMU/相机坐标系我们估计状态的主体与车轮里程计坐标系之间的变换。这是一个需要在线或离线标定的常量。通常包含一个平移p_b_o和一个旋转R_b_o。运动模型根据机器人模型如两轮差分、阿克曼转向将车轮编码器读数转换为车身坐标系下的位移增量。假设我们通过运动模型得到了在里程计坐标系O下从时刻i到j的相对位姿变换T_oij。那么在主体坐标系BIMU系下我们预测的相对变换应该是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_bjT_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 融合策略与初始化系统启动时状态是未知的。一个鲁棒的初始化流程至关重要纯视觉SfM首先系统会运行一段时间的纯视觉特征跟踪和三角化恢复出一些地图点并估计出一些关键帧的位姿但此时尺度是未知的。对齐与尺度恢复同时车轮里程计会提供有尺度的位移信息。在收集了足够多的视觉帧和对应的里程计位移后我们可以通过一个最小二乘问题将视觉估计的无尺度轨迹与车轮里程计的有尺度轨迹进行对齐相似变换尺度s 旋转R 平移t从而一次性求解出视觉的尺度s、视觉坐标系到里程计坐标系的旋转R外参旋转的一部分以及重力方向。IMU偏置初始化有了尺度和重力方向就可以将视觉估计的速度与IMU预积分得到的速度进行比较初步估计出陀螺仪和加速度计的零偏。联合优化初始化完成后系统就进入正常的紧耦合优化流程。新的关键帧被加入到滑动窗口中旧的被移出同时不断添加新的视觉、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/视觉估计的方向不一致引入矛盾。一个实用的在线标定方法是让机器人进行匀速直线运动或绕圈运动。在此期间视觉IMU可以估计出一个比较可靠的轨迹因为运动简单视觉不易丢IMU发散慢。将视觉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 实践问题排查清单当你发现系统轨迹飘移、抖动或突然跳变时可以按以下顺序排查现象可能原因排查方法尺度持续缓慢漂移车轮里程计尺度因子未标定准或在线优化时未固定/约束不足视觉特征深度估计有系统性误差。检查车轮半径标定值在平坦长直道路上测试比较融合输出的位移与真实位移如RTK-GNSS结果观察优化中尺度因子的变化。转弯时轨迹畸变里程计外参T_b_o旋转部分不准转弯时车轮滑移严重里程计模型未考虑。进行“绕八字”或正方形路径标定对比轨迹在转弯时临时降低里程计权重观察是否改善。高度方向z轴发散IMU加速度计零偏b_a在z方向估计不准缺乏视觉对地面的观测。确保初始化时重力方向估计准确尝试在路面有纹理时引入地面平面约束假设地面是平的。剧烈运动后轨迹跳变IMU预积分在剧烈运动时线性化误差大滑动窗口边缘化导致的信息矩阵病态。增加IMU采样频率如果硬件支持检查预积分代码中噪声传播的雅可比计算是否正确尝试调整边缘化策略。长时间运行后轨迹弯曲IMU陀螺仪零偏b_g在线估计发生漂移未完全约束住。增加陀螺仪零偏的随机游走噪声参数让系统更灵活地估计它检查是否有纯旋转运动此时视觉和里程计对b_g的约束很弱。动态物体导致轨迹毛刺动态特征未被有效剔除。开启动态特征检测如语义分割或几何检查观察特征点分布是否避开了车辆和行人。最后我想分享一点个人体会这种多传感器融合系统其鲁棒性往往不是由最先进的算法决定的而是由最薄弱传感器的故障处理机制决定的。在城市里视觉可能会短暂失效车轮可能会打滑IMU可能会受到冲击。一个好的系统必须在设计时就考虑到这些故障模式并设计相应的降级策略。例如当视觉跟踪的特征数低于阈值时自动增大里程计和IMU的权重当检测到车轮空转电机转速高但里程计位移小时暂时抛弃里程计数据。这种“传感器健康管理”的逻辑和核心的融合算法同样重要它决定了系统在实际复杂环境中能否真正“活下去”。这套“单目视觉低等级IMU车轮里程计”的方案正是在成本和可靠性之间找到了一个非常棒的平衡点让城市环境下的自主移动机器人有了一双更值得信赖的“眼睛”和“腿脚”。