Fast-LIO2 轮速计融合是我最近折腾得比较久的一个方向前前后后踩了快两周的坑从最开始一发车就飞到后面终于能在室内外各种地面跑稳中间积累了不少值得记录的细节。网上关于 Fast-LIO2 本身的教程已经不少了但专门讲轮速计融合、特别是打滑处理与协方差调参的资料非常零散我干脆把整个流程整理成一份完整的避坑指南从传感器配置、外参标定、打滑检测到协方差初值与在线估计全程都给出可以直接参考的参数和经验值希望能让你少走几周弯路。这个方案适合谁看如果你手里有一台带轮式编码器的移动机器人差速、阿克曼都行想在 Fast-LIO2 框架里加入轮速计来提升低速、长走廊、重复纹理环境下的定位稳定性那么这篇文章就是为你准备的。纯视觉或纯激光方案的朋友也可以看因为轮速计融合的核心思想“用一个低频率但不易漂移的传感器去约束高频里程计”在很多场景下是通用的。1. 融合思路与整体架构解析1.1 为什么要做轮速计融合先说说动机。Fast-LIO2 本身是激光雷达惯性里程计雷达帧和 IMU 紧耦合理论上在大多数场景下精度已经很好。但实际跑起来你会发现几个很典型的痛点一是长直走廊或者隧道里雷达观测的前向约束很弱即使有 IMU 帮忙Z 轴和航向角也很容易漂二是地面纹理重复、墙面反光严重的时候特征匹配会退化三是小车低速行驶时点云畸变和特征不确定性会被放大轨迹容易出现小幅度抖动。轮速计恰好能在这些方面补位。编码器直接测量轮子的角速度再结合轮距、轮径换算成速度它对纵向速度的测量非常直接不会像激光匹配那样受环境结构影响。哪怕只有两轮差速的普通编码器也能给前向速度一个比较硬的约束这样 Fast-LIO2 在雷达约束薄弱时就不至于“乱跑”。我实测过一组数据在 40 米长走廊里来回跑纯激光IMU 的轨迹横向偏差大约在 0.3 米左右航向角漂了约 2 度加了轮速计之后横向偏差降到 0.08 米以内航向角漂移明显改善。这个提升不是玄学而是因为轮速计给系统增加了一个与绝对位置无关的短时速度约束相当于在优化方程里多了一个非常可靠的先验。1.2 融合方案的选型考量目前主流的融合方式有两种一种是直接把轮速计当做一个独立的因子加入 Fast-LIO2 的 ESIKF 框架进行紧耦合另一种是在 Fast-LIO2 输出之后再做一层松耦合的滤波或图优化。两者的差异很大但绝大多数情况下我推荐前者——紧耦合。原因很简单紧耦合可以让轮速计参与到每一帧的状态估计中而不是事后修正。Fast-LIO2 的迭代误差状态卡尔曼滤波天然支持多传感器测量更新你只需要实现一个轮速计的测量模型把残差写进迭代更新里就行。而松耦合方案虽然实现简单但存在两个问题一是高频位姿和低频轮速之间的时间同步比较麻烦二是当激光退化时松耦合的后端修正往往“反应太慢”等发现问题时轨迹已经偏出去很远了。另外还要考虑一个现实问题轮速计在打滑时会产生严重错误的测量值。紧耦合方案可以结合检测结果动态调整测量噪声协方差错误的轮速读数会被自适应地降权从而避免污染整体状态估计。这是松耦合很难做到的。1.3 Fast-LIO2 的测量模型与接口Fast-LIO2 的代码结构里与传感器测量相关的部分主要在 ESIKF 的更新流程中。对于激光雷达它通过 ikd-Tree 构建的地图来生成残差对于 IMU它负责状态预测。轮速计融合本质上是往这个框架里插入一个新的测量更新步骤。具体来说轮速计给出的通常是轮速角速度或者左右轮的速度经过运动学模型后可以得到机器人坐标系下的线速度 v_odo 和角速度 w_odo。在 ESIKF 更新时我们构造轮速计的残差方程z h(x) - z_odo其中 h(x) 是根据当前状态位置、姿态、速度、角速度预测出的机器人底部速度z_odo 是编码器解算出的速度观测。残差配合雅可比矩阵 H 和测量噪声协方差 R一起进入迭代更新。Fast-LIO2 原本的迭代更新框架不需要改动太多核心工作是三个实现轮速计测量模型、提供轮速计数据的预处理节点、设计打滑检测与协方差调整逻辑。这三个点也是下面几章要展开讲的重点。2. 硬件配置与数据预处理关键点2.1 传感器安装与底盘选型如果你是从零开始搭建测试车底盘选型上优先考虑带高分辨率编码器的电机。分辨率至少要在 512 线以上推荐 1024 线甚至更高因为轮速计在低速时每个控制周期转过的角度很小分辨率不够的话量化噪声会非常明显。安装上要特别注意编码器的安装位置。直接测轮轴的编码器是最准的但很多商业底盘使用的是电机后端编码器中间隔着减速器和传动机构。这种情况下减速比和轮径的标定就变得极其重要。我的经验是轮径一定要做“实际滚动距离标定”而不是查规格书。规格书上的轮胎直径在负载状态下会变化胎压不同、地面不同实际滚动半径能差出 2% 以上这对测速来说已经算很大的误差了。具体标定方法在平整地面上让机器人走一段足够长的距离比如 10 米以上用卷尺测量实际距离同时记录编码器累计脉冲数。然后反推出有效轮径。建议正反方向各测一次取平均可以抵消编码器安装相位和地面平整度的系统性偏差。还有一个容易被忽略的细节底盘动力学对轮速测量有影响。急加速和急刹车时轮胎与地面之间会有瞬态滑移即使没有宏观打滑轮速计的读数和真实地面速度也会存在短暂偏差。这个偏差无法完全消除但可以在打滑检测逻辑里做阈值判断。2.2 轮速计数据的坐标系变换轮速计测量值是在“轮速计坐标系”下给出的通常我们关心的是机器人 base_link 坐标系下的速度。如果轮速计的安装位置和 base_link 之间有平移和旋转理论上要做外参变换。不过大多数情况下轮速计坐标系跟 base_link 的朝向是一致的只有平移差异。平移不影响速度所以这个变换通常退化为一个简单的旋转对齐。如果你使用的是差速底盘轮速计给出的速度模型还依赖轮距参数轮距标定不准确会直接影响旋转角速度的测量。角速度这块我多说一句差速底盘的 w_odo (v_right - v_left) / wheel_base其中轮距 wheel_base 的误差对 w_odo 的影响是线性的。如果你的车转向频繁一定要认真标定轮距否则融合后容易出现明显的航向漂移。我的做法是在地面上画一条直线让车以不同速度做直线行驶观察左右轮计数是否一致再做原地旋转用外部航向参考比如高性能陀螺仪或视觉基准对比轮速计积分出来的角度变化反推 wheel_base 的修正值。2.3 时间同步与消息频率匹配轮速计融合最容易被忽视但又最关键的问题之一就是时间同步。Fast-LIO2 内部以 IMU 时间为基准激光帧和轮速计测量都要对齐到同一个时间轴上。我建议的做法是在轮速计数据进入融合节点之前先做一次时间戳校正。采用“最近邻时间戳匹配 插值”的策略——先根据轮速计消息的 header.stamp 找到最近的 IMU 时刻然后用前后两帧轮速计做线性插值得到该时刻的等效速度。插值对低速车来说精度足够代码实现也很简单。频率建议轮速计发布频率最好在 50Hz 以上IMU 是 200Hz激光雷达是 10Hz。如果你的底盘只能输出 20Hz 的轮速也可以跑但相对位置更新的噪声会变大因为两次测量之间的车辆速度变化只能靠 IMU 来“猜”误差会累积。还有一个常见的坑是轮速计的“零速输出”。很多底盘在停车时会持续发布速度为 0 的轮速计数据这个本身没问题但如果底盘在停车瞬间有微小反向抖动齿轮回差、机械间隙轮速计会短暂输出比较大的非零速度这种异常数据对 ESIKF 的冲击很大。我后来在预处理节点里加了一个零速粘滞窗口当检测到持续 200ms 内轮速接近零时强制后续 300ms 的轮速输出为 0效果立竿见影。3. 打滑检测模型、阈值与实战实现3.1 打滑现象的本质与危害打滑对轮速计融合的威胁远大于噪声。噪声是随机的、零均值的卡尔曼滤波器可以通过概率模型有效抑制而打滑是系统性的、持续的错误它会让滤波器引入一个与实际运动完全不符的“强观测”并且该观测的置信度可能还很高。举例来说小车在光滑地板上全力加速轮子空转左右轮速度瞬间飙升到 2m/s但实际车身根本没动。此时轮速计给出的速度观测是 2m/s如果滤波器没有检测到这个异常它会将状态估计硬拉过去导致位置瞬间产生大幅度偏移。由于 ESIKF 是迭代收敛的这种错误观测还可能破坏后续帧的收敛稳定性带来连锁反应。所以打滑检测不是一个“锦上添花”的功能而是轮速计融合能够可靠运行的前提。3.2 基于运动学一致性的检测方法最实用的打滑检测思路是“运动学一致性检测”。说白了就是用多个独立来源的速度估计互相验证。Fast-LIO2 在融合轮速计之前本身已经有由 IMU 预测和雷达匹配得到的估计速度我们可以用这两个速度与轮速计速度做交叉比对。核心公式是这样的设 v_ekf 为当前滤波器估计的机器人速度v_odo 为轮速计解算的速度。计算两者的夹角偏差和模长偏差angle_error arccos(v_ekf · v_odo / (|v_ekf| |v_odo|)) speed_error |v_ekf| - |v_odo|如果 angle_error 和 speed_error 同时超阈值就判定为疑似打滑。这个检测的物理直觉是短暂加速或轻微滑动时单一传感器可能有偏差但两个独立传感器同时发生一致偏差的概率极低。当 v_ekf 和 v_odo 差异显著时至少有一个传感器的测量不可信考虑到轮速计更容易受地面条件影响我们倾向于相信滤波器估计而降低轮速计权重。注意阈值的选取不能太激进。底盘自身的加减速也会造成轮速计与 IMU 速度之间的暂时差异。我常用的阈值是 angle_error 大于 15 度、speed_error 大于 0.15 倍当前车速时判定为打滑。如果车速本身就很低比如小于 0.1m/s直接放松阈值因为低速下很小的绝对误差就会导致很大的百分比误差。还有一种更简单的方案是基于加速度量级。打滑瞬间轮速的加速度会远超正常值。你可以对轮速做差分得到轮加速度 a_odo如果 a_odo 超过一个经验阈值比如 3m/s²并且持续时间大于 30ms就判定为打滑。这个方案实现简单但误报率相对高一些我一般把它作为“辅助判据”而不是“唯一判据”。3.3 基于轮速自洽性的检测方法对于差速底盘还有一个天然的自洽判据左轮和右轮的速度关系。如果机器人模型正确左右轮速度和车体速度之间的关系应该满足刚体运动学约束。当只有一个轮子打滑时左右轮速度会出现明显不对称。具体实现上可以建立一个“预期轮速”估计根据当前 v_ekf 和 w_ekf滤波器估计的线速度和角速度结合轮距和轮径反推左右轮的理论转速。然后与编码器直接读到的左右轮转速做比较v_left_expected v_ekf - w_ekf * wheel_base / 2 v_right_expected v_ekf w_ekf * wheel_base / 2如果某一侧的误差超过阈值比如 0.2m/s则该侧判定为打滑。这个方法的好处是能定位到具体是哪个轮子打滑而不是笼统地怀疑整个轮速计对后续的容错处理很有用。我实际用下来这个自洽性检测是误报率最低的方案推荐优先采用。它的前提是轮距和轮径已经标定得比较准否则会把标定误差误判为打滑。所以在做打滑检测之前先把标定做完这是顺序问题也是很多新手踩坑的地方。3.4 打滑后的处理策略降权、剔除法与重置机制检测到打滑之后不能简单地丢弃后续所有轮速数据因为打滑结束后轮速计又会恢复正常。我的处理策略分三档第一档是轻度异常angle_error 或 speed_error 超阈值但持续时间低于 50ms直接增大轮速计的测量噪声 R让它权重降低。R 增大倍数建议在 5 到 10 倍之间太大会让观测完全失效太小又起不到降权作用。第二档是中度异常持续时间在 50ms 到 200ms 之间将该时刻的轮速计测量标记为 Invalid不参与滤波器更新但没有必要完全重置。第三档是重度异常持续时间超过 200ms这意味着车辆可能处于持续打滑状态比如在冰面上起步此时需要将轮速计测量完全剔除并且在底层执行状态协方差重置机制——把轮速计对应的速度分量的过程噪声临时调大给滤波器更大的自由来跟随其他传感器避免被之前的错误观测“锁死”。这套策略我落地之后效果明显。特别是有一次在雨后环氧地坪上测试小车起步瞬间打滑 300ms 左右没有这套逻辑时轨迹直接偏出 1 米多加了对策后稳定在 10 厘米以内的偏差。4. 协方差调参初值、过程噪声与在线调整4.1 协方差矩阵的物理含义与设置顺序很多人在 Fast-LIO2 里调参时最头疼的就是协方差矩阵——一堆数字不知道含义只能瞎试。先说清楚这些协方差本质上是“你对传感器测量有多信任”的数学表达。数值越小表示测量越可信滤波器会更多地相信这个测量数值越大表示测量越不可信滤波器会更多地依靠状态预测。调参顺序有个原则先调好 IMU 过程噪声再调雷达观测噪声最后再动轮速计的 R。因为轮速计是叠加在原有系统上的增量如果底层 IMU 和雷达的协方差不对轮速计再准也救不回来。Fast-LIO2 配置中与轮速计融合相关的协方差参数大致分为三类轮速计观测噪声 R_odo、预设原点处初始协方差 P_0以及运动模型的过程噪声 Q。Q 通常由 IMU 噪声模型推导不需要手工调得太多重点放在 R_odo 和 P_0 上。4.2 轮速计观测噪声 R_odo 的初值估计R_odo 的初值可以基于编码器分辨率、底盘控制频率和运动学模型误差来估算。以 1024 线编码器为例假设轮径 0.15m编码器每转输出 1024 脉冲那么每个脉冲对应的轮面位移大约是 0.46mm。如果控制周期是 20ms速度量化误差约为 0.023m/s。考虑到轮径标定误差、轮距误差和轮胎形变实际速度误差可能会达到 0.05m/s 量级。所以 R_odo 的线速度分量的初值我习惯取 0.01 到 0.04(m/s)² 这个区间注意这是方差不是标准差开方之后对应 0.1 到 0.2m/s 的标准差。角速度分量的初值取 0.01 到 0.04(rad/s)²。这个范围在多数室内机器人上是合理的后续可以根据实验微调。如果你使用的是电机端编码器且减速比较大建议把 R_odo 初值调高一档因为减速器背隙和皮带打滑都会引入额外的非确定性误差。4.3 在线自适应协方差基于残差序列的调整协方差调参不是一劳永逸的。地面条件变化、载荷变化、轮胎磨损都会让固定 R 变得不再合适。最实用的一种在线自适应方法是基于残差序列的协方差估计——也就是经典的 Sage-Husa 自适应滤波思想。做法如下维护一个滑动窗口长度取 N50存储最近 50 次轮速计更新时的残差序列r_k z_k - h(x_k)计算残差的样本方差R_residual (1/(N-1)) * sum((r_k - r_mean)^2)然后对 R_odo 做指数平滑更新R_updated alpha * R_residual (1-alpha) * R_odo_old其中 alpha 取 0.1 到 0.3越大表示对残差变化越敏感但过大容易震荡。这个在线估计方法有一个问题残差不仅包含测量噪声还包含模型误差和滤波器收敛误差所以直接用 R_residual 代替 R_odo 会偏大。经验做法是给 R_updated 设置上下限上限是当前值的 3 倍下限是 0.3 倍避免自适应导致参数极端化。实测效果在粗糙室外地面轮速计真实噪声明显增大残差序列方差上升在线调整后 R 自动放大滤波器对轮速计的信任自动降低整体轨迹精度反而比固定 R 更好。4.4 初始协方差 P_0 的合理设置P_0 表示滤波器初始状态的不确定性。设置过小会让滤波器一开始就“自信满满”如果初始速度估计有偏差可能迟迟无法修正设置过大会导致初始几帧抖动明显。在轮速计融合场景中P_0 中速度分量的设置要与 R_odo 配合。我一般把 P_0 的速度分量放宽到 0.1(m/s)² 左右角速度分量放宽到 0.1(rad/s)² 左右。这样滤波器在启动阶段会快速接受轮速计观测将速度收敛到合理范围而不会出现因为初始协方差太紧导致的前几米轨迹偏硬。4.5 协方差调参的实测方法与评价指标参数到底调得行不行不能只看轨迹图要看残差序列和新息序列。我每次调参后都会做三个层面的评估第一残差均值应接近零。如果轮速计更新残差长期为正说明轮速计测量的均值存在系统偏差优先检查标定而不是调 R。第二残差方差应与 R 的量级大致匹配。如果实际残差方差远大于你设置的 R说明设置太乐观滤波器对轮速计的信任超过了实际这种情况在打滑或颠簸路面下特别危险。第三对比融合前后的轨迹闭合误差。在室内场馆跑一个矩形闭环记录起点终点误差。固定 R 参数下融合后误差应该显著小于纯激光方案。若融合后误差反而变大首先怀疑不是 R 的问题而是打滑检测逻辑没有生效。5. 实操过程从零开始集成轮速计到 Fast-LIO25.1 代码结构与关键文件说明Fast-LIO2 的代码结构我就不展开全讲了只聚焦与轮速计融合相关的部分。核心修改点通常在以下几个文件中odometry 节点负责订阅激光雷达、IMU、轮速计数据并触发融合流程。ESIKF 更新函数在这里加入轮速计的测量更新方程。配置 YAML 文件加入 R_odo 和轮速计相关参数。打滑检测模块可以写成一个独立的类或函数在轮速计数据进入滤波器之前做预处理。在动手改之前建议先在 ROS 环境里写好一个轮速计话题转换节点把底盘自带的轮速数据可能是左右轮速度或轮速角速度转换成标准的机器人速度消息。这一步看似简单但带来的调试收益很大因为后面所有融合逻辑都只依赖这个统一格式。5.2 核心代码框架示例轮速计测量模型的核心代码不复杂我给出一个精简版的伪代码框架方便你理解数据流向和更新逻辑。假设已经得到机器人坐标系下的速度观测 z_odo [v_odo, w_odo]对应测量噪声 R diag(0.02, 0.02)// 在 ESIKF 更新流程中新增轮速计更新 void esikf_update_with_odo(StatesGroup state, const Eigen::Vector2d z_odo, const Eigen::Matrix2d R_odo) { // 预测测量值从当前状态提取速度 Eigen::Vector2d z_pred; z_pred state.vel.norm(), state.rot.norm(); // 具体形式取决于模型 // 计算残差 Eigen::Vector2d r z_odo - z_pred; // 计算雅可比矩阵 H Eigen::Matrixdouble, 2, 18 H; H.setZero(); H.block2, 2(0, 6) Eigen::Matrix2d::Identity(); // 对应速度状态 // 计算卡尔曼增益并更新状态 // 这里实际与 Fast-LIO2 的 ESIKF 更新流程一致 // K P * H^T * (H * P * H^T R_odo)^(-1) // state state K * (r - H * state_error) }这段伪代码省略了 ESIKF 的迭代细节但主流程就是“预测残差 - 计算雅可比 - 更新状态”。编写时要注意轮速计的测量值不是全局速度而是机体坐标系下的速度因此雅可比矩阵要正确表达机体速度与全局状态量之间的关系。打滑检测模块的伪代码也很直接我一般写成bool slip_detected false; double angle_error compute_angle_error(v_ekf, v_odo); double speed_error compute_speed_error(v_ekf, v_odo); if (angle_error deg2rad(15.0) speed_error 0.15 * v_ekf.norm()) { slip_detected true; } if (slip_detected) { // 根据持续时间分为降权、剔除、重置三类 }实际工程中滑动窗口、阈值和降权系数都需要在线调试。我建议先用 rosbag 录制几组包含直线、转弯、打滑场景的数据离线回放调整参数而不是一上来就在实车上反复试效率太低也容易损耗设备。5.3 实车测试流程与效果评估代码改完后不要直接跑完整流程。我的建议是分三步测试第一步纯轮速计积分测试。关闭激光和 IMU只让轮速计积分得到航迹走一个直线和矩形验证标定是否准确。这一步能快速暴露轮径和轮距的标定问题。第二步Fast-LIO2 不加轮速计跑一遍原有流程记录 baseline 轨迹。这一步是用于后续对比的基准。第三步开启轮速计融合重复相同的路径。对比第二步和第三步的轨迹、闭环误差、状态估计的平滑度。如果融合后轨迹出现毛刺或抖振优先检查打滑检测阈值和时间同步而不是急于调整协方差。我测试过的一个典型参数组合差速小车轮径 0.15m轮距 0.35m激光雷达 10HzIMU 200Hz编码器 50Hz如下参数初始值说明R_odo 线速度0.02 (m/s)²对应标准差约 0.14m/sR_odo 角速度0.02 (rad/s)²对应标准差约 0.14rad/s打滑角度阈值15 度与当前速度方向比较打滑速度阈值0.15 倍车速相对阈值适配不同速度轻度异常降权倍数8 倍直接乘在 R_odo 上零速粘滞窗口200ms防止底盘回差干扰这套参数在室内瓷砖、环氧地坪、短草地、以及有限的路面颠簸场景下都跑出了比较稳定的效果。6. 常见问题与排查技巧实录6.1 融合后轨迹出现周期性抖动这个问题的根源通常是轮速计和 IMU 的时间戳没有严格对齐。周期性抖动最容易出现在轮速计频率较低比如 20Hz且插值不够平滑的情况下。排查方法是把轮速计和 IMU 的时间戳画在同一张图看是否存在固定相移。我遇到的案例是底盘驱动节点的轮速发布时间戳比实际采样时刻晚了约 30ms导致轮速计“看到”的是过去的速度与当前滤波器状态不匹配。修正方法是在驱动节点发布消息时改用硬件时间戳如果编码器外设支持或者在融合节点里对轮速计做前向补偿即把轮速数据向后平移一个固定的延迟量。6.2 打滑检测频繁误报如果你发现车辆在正常行驶时也经常触发打滑检测首先检查速度阈值是否设置得太绝对。比如你把 speed_error 设定为固定 0.1m/s在车速为 0.5m/s 时这个阈值相当于 20% 的偏差偏严了。改成相对阈值当前车速的百分比会好很多。另一个常见原因是轮距或轮径标定不准。如果轮径偏大轮速计解算出的速度会系统性地比真实速度高残差序列长期为正打滑检测也容易被触发。先重新做标定再调检测阈值。6.3 协方差自适应导致滤波器发散在线协方差估计是一把双刃剑。如果你发现 R 在自适应过程中持续减小有可能是残差序列里包含的“新息”被低估了。解决办法是限制 R 的下界不能让 R 低于物理下限。我的建议是将 R 的最小值设为基础值的 0.3 倍。这样即使残差很小滤波器也会保留对轮速计最基本的“不信任”防止系统过度相信某个传感器导致的发散。6.4 轮速计融合后航向漂移反而变大这种情况很反常但确实会发生。最大嫌疑是轮距标定不准。差速底盘的航向角速度 w_odo (v_right - v_left) / wheel_base如果 wheel_base 标得偏小轮速计给出的角速度会偏大在转弯时滤波器会把航向往错误方向拉。另一个可能是零速粘滞窗口太大导致车辆停车后开始转弯时轮速计仍然在输出零速度而这个零速观测与真实的转弯角速度冲突。把零速粘滞窗口缩小或者在检测到右轮/左轮有显著差速后立刻退出粘滞状态。6.5 快速 FAQ 速查表症状可能原因解决方向轨迹抖动时间同步偏差检查时间戳对齐与插值打滑误报阈值过严或标定偏差改相对阈值重新标定轮径轮距融合后漂移更大轮距误差、零速粘滞过大标定轮距优化粘滞窗口直行跑偏左右轮径不一致左右轮分别标定滚距转弯轨迹扭曲轮距或角速度噪声设置偏小重新标定轮距放大 R_odo 角速度分量这组速查表基本能覆盖我踩过的绝大部分坑。当然每个底盘的机械特性不一样你遇到的问题可能更多、更奇怪但只要把握住“标定 - 时间同步 - 打滑检测 - 协方差”这条主线任何古怪问题都能拆解成可排查的环节。7. 实操心得与下一步扩展方向这套轮速计融合方案我个人最满意的地方是它并不需要大改 Fast-LIO2 的框架而是在原有 ESIKF 流程中增加了一个相对独立、逻辑清晰的测量源。相比自己去写一套完整的组合导航系统这种做法投入产出比高很多尤其适合已经有 Fast-LIO2 基础、希望通过增量传感器提升鲁棒性的团队。关于参数调整我想再补充一个很个人向的经验不要试图一开始就把所有参数调到完美先把打滑检测的逻辑跑通再调协方差。因为打滑检测本身就是一个保护机制它不正常工作的话后续协方差调参都会失真。把保护机制做扎实剩下的参数调整只是在已有鲁棒性基础上的“锦上添花”。后续你可以做的扩展方向不少。一是加入轮速计零速检测与静止判定让机器人在启动阶段有更好的初始状态估计二是加入地面坡度估计让轮速计在坡道上的模型更加精确三是将轮速计与 GNSS 融合在室外场景进一步压制长时漂移。这些方向本质上都还是在“给滤波器提供更多可靠的信息源”遵循的思路与轮速计融合完全一致。如果你也遇到类似问题欢迎带着你的具体底盘参数和实验现象来交流。实际踩坑获得的经验往往比理论推导更值得记录。 Fast-LIO2轮速计融合实战：打滑检测与协方差调参避坑指南
Fast-LIO2 轮速计融合是我最近折腾得比较久的一个方向前前后后踩了快两周的坑从最开始一发车就飞到后面终于能在室内外各种地面跑稳中间积累了不少值得记录的细节。网上关于 Fast-LIO2 本身的教程已经不少了但专门讲轮速计融合、特别是打滑处理与协方差调参的资料非常零散我干脆把整个流程整理成一份完整的避坑指南从传感器配置、外参标定、打滑检测到协方差初值与在线估计全程都给出可以直接参考的参数和经验值希望能让你少走几周弯路。这个方案适合谁看如果你手里有一台带轮式编码器的移动机器人差速、阿克曼都行想在 Fast-LIO2 框架里加入轮速计来提升低速、长走廊、重复纹理环境下的定位稳定性那么这篇文章就是为你准备的。纯视觉或纯激光方案的朋友也可以看因为轮速计融合的核心思想“用一个低频率但不易漂移的传感器去约束高频里程计”在很多场景下是通用的。1. 融合思路与整体架构解析1.1 为什么要做轮速计融合先说说动机。Fast-LIO2 本身是激光雷达惯性里程计雷达帧和 IMU 紧耦合理论上在大多数场景下精度已经很好。但实际跑起来你会发现几个很典型的痛点一是长直走廊或者隧道里雷达观测的前向约束很弱即使有 IMU 帮忙Z 轴和航向角也很容易漂二是地面纹理重复、墙面反光严重的时候特征匹配会退化三是小车低速行驶时点云畸变和特征不确定性会被放大轨迹容易出现小幅度抖动。轮速计恰好能在这些方面补位。编码器直接测量轮子的角速度再结合轮距、轮径换算成速度它对纵向速度的测量非常直接不会像激光匹配那样受环境结构影响。哪怕只有两轮差速的普通编码器也能给前向速度一个比较硬的约束这样 Fast-LIO2 在雷达约束薄弱时就不至于“乱跑”。我实测过一组数据在 40 米长走廊里来回跑纯激光IMU 的轨迹横向偏差大约在 0.3 米左右航向角漂了约 2 度加了轮速计之后横向偏差降到 0.08 米以内航向角漂移明显改善。这个提升不是玄学而是因为轮速计给系统增加了一个与绝对位置无关的短时速度约束相当于在优化方程里多了一个非常可靠的先验。1.2 融合方案的选型考量目前主流的融合方式有两种一种是直接把轮速计当做一个独立的因子加入 Fast-LIO2 的 ESIKF 框架进行紧耦合另一种是在 Fast-LIO2 输出之后再做一层松耦合的滤波或图优化。两者的差异很大但绝大多数情况下我推荐前者——紧耦合。原因很简单紧耦合可以让轮速计参与到每一帧的状态估计中而不是事后修正。Fast-LIO2 的迭代误差状态卡尔曼滤波天然支持多传感器测量更新你只需要实现一个轮速计的测量模型把残差写进迭代更新里就行。而松耦合方案虽然实现简单但存在两个问题一是高频位姿和低频轮速之间的时间同步比较麻烦二是当激光退化时松耦合的后端修正往往“反应太慢”等发现问题时轨迹已经偏出去很远了。另外还要考虑一个现实问题轮速计在打滑时会产生严重错误的测量值。紧耦合方案可以结合检测结果动态调整测量噪声协方差错误的轮速读数会被自适应地降权从而避免污染整体状态估计。这是松耦合很难做到的。1.3 Fast-LIO2 的测量模型与接口Fast-LIO2 的代码结构里与传感器测量相关的部分主要在 ESIKF 的更新流程中。对于激光雷达它通过 ikd-Tree 构建的地图来生成残差对于 IMU它负责状态预测。轮速计融合本质上是往这个框架里插入一个新的测量更新步骤。具体来说轮速计给出的通常是轮速角速度或者左右轮的速度经过运动学模型后可以得到机器人坐标系下的线速度 v_odo 和角速度 w_odo。在 ESIKF 更新时我们构造轮速计的残差方程z h(x) - z_odo其中 h(x) 是根据当前状态位置、姿态、速度、角速度预测出的机器人底部速度z_odo 是编码器解算出的速度观测。残差配合雅可比矩阵 H 和测量噪声协方差 R一起进入迭代更新。Fast-LIO2 原本的迭代更新框架不需要改动太多核心工作是三个实现轮速计测量模型、提供轮速计数据的预处理节点、设计打滑检测与协方差调整逻辑。这三个点也是下面几章要展开讲的重点。2. 硬件配置与数据预处理关键点2.1 传感器安装与底盘选型如果你是从零开始搭建测试车底盘选型上优先考虑带高分辨率编码器的电机。分辨率至少要在 512 线以上推荐 1024 线甚至更高因为轮速计在低速时每个控制周期转过的角度很小分辨率不够的话量化噪声会非常明显。安装上要特别注意编码器的安装位置。直接测轮轴的编码器是最准的但很多商业底盘使用的是电机后端编码器中间隔着减速器和传动机构。这种情况下减速比和轮径的标定就变得极其重要。我的经验是轮径一定要做“实际滚动距离标定”而不是查规格书。规格书上的轮胎直径在负载状态下会变化胎压不同、地面不同实际滚动半径能差出 2% 以上这对测速来说已经算很大的误差了。具体标定方法在平整地面上让机器人走一段足够长的距离比如 10 米以上用卷尺测量实际距离同时记录编码器累计脉冲数。然后反推出有效轮径。建议正反方向各测一次取平均可以抵消编码器安装相位和地面平整度的系统性偏差。还有一个容易被忽略的细节底盘动力学对轮速测量有影响。急加速和急刹车时轮胎与地面之间会有瞬态滑移即使没有宏观打滑轮速计的读数和真实地面速度也会存在短暂偏差。这个偏差无法完全消除但可以在打滑检测逻辑里做阈值判断。2.2 轮速计数据的坐标系变换轮速计测量值是在“轮速计坐标系”下给出的通常我们关心的是机器人 base_link 坐标系下的速度。如果轮速计的安装位置和 base_link 之间有平移和旋转理论上要做外参变换。不过大多数情况下轮速计坐标系跟 base_link 的朝向是一致的只有平移差异。平移不影响速度所以这个变换通常退化为一个简单的旋转对齐。如果你使用的是差速底盘轮速计给出的速度模型还依赖轮距参数轮距标定不准确会直接影响旋转角速度的测量。角速度这块我多说一句差速底盘的 w_odo (v_right - v_left) / wheel_base其中轮距 wheel_base 的误差对 w_odo 的影响是线性的。如果你的车转向频繁一定要认真标定轮距否则融合后容易出现明显的航向漂移。我的做法是在地面上画一条直线让车以不同速度做直线行驶观察左右轮计数是否一致再做原地旋转用外部航向参考比如高性能陀螺仪或视觉基准对比轮速计积分出来的角度变化反推 wheel_base 的修正值。2.3 时间同步与消息频率匹配轮速计融合最容易被忽视但又最关键的问题之一就是时间同步。Fast-LIO2 内部以 IMU 时间为基准激光帧和轮速计测量都要对齐到同一个时间轴上。我建议的做法是在轮速计数据进入融合节点之前先做一次时间戳校正。采用“最近邻时间戳匹配 插值”的策略——先根据轮速计消息的 header.stamp 找到最近的 IMU 时刻然后用前后两帧轮速计做线性插值得到该时刻的等效速度。插值对低速车来说精度足够代码实现也很简单。频率建议轮速计发布频率最好在 50Hz 以上IMU 是 200Hz激光雷达是 10Hz。如果你的底盘只能输出 20Hz 的轮速也可以跑但相对位置更新的噪声会变大因为两次测量之间的车辆速度变化只能靠 IMU 来“猜”误差会累积。还有一个常见的坑是轮速计的“零速输出”。很多底盘在停车时会持续发布速度为 0 的轮速计数据这个本身没问题但如果底盘在停车瞬间有微小反向抖动齿轮回差、机械间隙轮速计会短暂输出比较大的非零速度这种异常数据对 ESIKF 的冲击很大。我后来在预处理节点里加了一个零速粘滞窗口当检测到持续 200ms 内轮速接近零时强制后续 300ms 的轮速输出为 0效果立竿见影。3. 打滑检测模型、阈值与实战实现3.1 打滑现象的本质与危害打滑对轮速计融合的威胁远大于噪声。噪声是随机的、零均值的卡尔曼滤波器可以通过概率模型有效抑制而打滑是系统性的、持续的错误它会让滤波器引入一个与实际运动完全不符的“强观测”并且该观测的置信度可能还很高。举例来说小车在光滑地板上全力加速轮子空转左右轮速度瞬间飙升到 2m/s但实际车身根本没动。此时轮速计给出的速度观测是 2m/s如果滤波器没有检测到这个异常它会将状态估计硬拉过去导致位置瞬间产生大幅度偏移。由于 ESIKF 是迭代收敛的这种错误观测还可能破坏后续帧的收敛稳定性带来连锁反应。所以打滑检测不是一个“锦上添花”的功能而是轮速计融合能够可靠运行的前提。3.2 基于运动学一致性的检测方法最实用的打滑检测思路是“运动学一致性检测”。说白了就是用多个独立来源的速度估计互相验证。Fast-LIO2 在融合轮速计之前本身已经有由 IMU 预测和雷达匹配得到的估计速度我们可以用这两个速度与轮速计速度做交叉比对。核心公式是这样的设 v_ekf 为当前滤波器估计的机器人速度v_odo 为轮速计解算的速度。计算两者的夹角偏差和模长偏差angle_error arccos(v_ekf · v_odo / (|v_ekf| |v_odo|)) speed_error |v_ekf| - |v_odo|如果 angle_error 和 speed_error 同时超阈值就判定为疑似打滑。这个检测的物理直觉是短暂加速或轻微滑动时单一传感器可能有偏差但两个独立传感器同时发生一致偏差的概率极低。当 v_ekf 和 v_odo 差异显著时至少有一个传感器的测量不可信考虑到轮速计更容易受地面条件影响我们倾向于相信滤波器估计而降低轮速计权重。注意阈值的选取不能太激进。底盘自身的加减速也会造成轮速计与 IMU 速度之间的暂时差异。我常用的阈值是 angle_error 大于 15 度、speed_error 大于 0.15 倍当前车速时判定为打滑。如果车速本身就很低比如小于 0.1m/s直接放松阈值因为低速下很小的绝对误差就会导致很大的百分比误差。还有一种更简单的方案是基于加速度量级。打滑瞬间轮速的加速度会远超正常值。你可以对轮速做差分得到轮加速度 a_odo如果 a_odo 超过一个经验阈值比如 3m/s²并且持续时间大于 30ms就判定为打滑。这个方案实现简单但误报率相对高一些我一般把它作为“辅助判据”而不是“唯一判据”。3.3 基于轮速自洽性的检测方法对于差速底盘还有一个天然的自洽判据左轮和右轮的速度关系。如果机器人模型正确左右轮速度和车体速度之间的关系应该满足刚体运动学约束。当只有一个轮子打滑时左右轮速度会出现明显不对称。具体实现上可以建立一个“预期轮速”估计根据当前 v_ekf 和 w_ekf滤波器估计的线速度和角速度结合轮距和轮径反推左右轮的理论转速。然后与编码器直接读到的左右轮转速做比较v_left_expected v_ekf - w_ekf * wheel_base / 2 v_right_expected v_ekf w_ekf * wheel_base / 2如果某一侧的误差超过阈值比如 0.2m/s则该侧判定为打滑。这个方法的好处是能定位到具体是哪个轮子打滑而不是笼统地怀疑整个轮速计对后续的容错处理很有用。我实际用下来这个自洽性检测是误报率最低的方案推荐优先采用。它的前提是轮距和轮径已经标定得比较准否则会把标定误差误判为打滑。所以在做打滑检测之前先把标定做完这是顺序问题也是很多新手踩坑的地方。3.4 打滑后的处理策略降权、剔除法与重置机制检测到打滑之后不能简单地丢弃后续所有轮速数据因为打滑结束后轮速计又会恢复正常。我的处理策略分三档第一档是轻度异常angle_error 或 speed_error 超阈值但持续时间低于 50ms直接增大轮速计的测量噪声 R让它权重降低。R 增大倍数建议在 5 到 10 倍之间太大会让观测完全失效太小又起不到降权作用。第二档是中度异常持续时间在 50ms 到 200ms 之间将该时刻的轮速计测量标记为 Invalid不参与滤波器更新但没有必要完全重置。第三档是重度异常持续时间超过 200ms这意味着车辆可能处于持续打滑状态比如在冰面上起步此时需要将轮速计测量完全剔除并且在底层执行状态协方差重置机制——把轮速计对应的速度分量的过程噪声临时调大给滤波器更大的自由来跟随其他传感器避免被之前的错误观测“锁死”。这套策略我落地之后效果明显。特别是有一次在雨后环氧地坪上测试小车起步瞬间打滑 300ms 左右没有这套逻辑时轨迹直接偏出 1 米多加了对策后稳定在 10 厘米以内的偏差。4. 协方差调参初值、过程噪声与在线调整4.1 协方差矩阵的物理含义与设置顺序很多人在 Fast-LIO2 里调参时最头疼的就是协方差矩阵——一堆数字不知道含义只能瞎试。先说清楚这些协方差本质上是“你对传感器测量有多信任”的数学表达。数值越小表示测量越可信滤波器会更多地相信这个测量数值越大表示测量越不可信滤波器会更多地依靠状态预测。调参顺序有个原则先调好 IMU 过程噪声再调雷达观测噪声最后再动轮速计的 R。因为轮速计是叠加在原有系统上的增量如果底层 IMU 和雷达的协方差不对轮速计再准也救不回来。Fast-LIO2 配置中与轮速计融合相关的协方差参数大致分为三类轮速计观测噪声 R_odo、预设原点处初始协方差 P_0以及运动模型的过程噪声 Q。Q 通常由 IMU 噪声模型推导不需要手工调得太多重点放在 R_odo 和 P_0 上。4.2 轮速计观测噪声 R_odo 的初值估计R_odo 的初值可以基于编码器分辨率、底盘控制频率和运动学模型误差来估算。以 1024 线编码器为例假设轮径 0.15m编码器每转输出 1024 脉冲那么每个脉冲对应的轮面位移大约是 0.46mm。如果控制周期是 20ms速度量化误差约为 0.023m/s。考虑到轮径标定误差、轮距误差和轮胎形变实际速度误差可能会达到 0.05m/s 量级。所以 R_odo 的线速度分量的初值我习惯取 0.01 到 0.04(m/s)² 这个区间注意这是方差不是标准差开方之后对应 0.1 到 0.2m/s 的标准差。角速度分量的初值取 0.01 到 0.04(rad/s)²。这个范围在多数室内机器人上是合理的后续可以根据实验微调。如果你使用的是电机端编码器且减速比较大建议把 R_odo 初值调高一档因为减速器背隙和皮带打滑都会引入额外的非确定性误差。4.3 在线自适应协方差基于残差序列的调整协方差调参不是一劳永逸的。地面条件变化、载荷变化、轮胎磨损都会让固定 R 变得不再合适。最实用的一种在线自适应方法是基于残差序列的协方差估计——也就是经典的 Sage-Husa 自适应滤波思想。做法如下维护一个滑动窗口长度取 N50存储最近 50 次轮速计更新时的残差序列r_k z_k - h(x_k)计算残差的样本方差R_residual (1/(N-1)) * sum((r_k - r_mean)^2)然后对 R_odo 做指数平滑更新R_updated alpha * R_residual (1-alpha) * R_odo_old其中 alpha 取 0.1 到 0.3越大表示对残差变化越敏感但过大容易震荡。这个在线估计方法有一个问题残差不仅包含测量噪声还包含模型误差和滤波器收敛误差所以直接用 R_residual 代替 R_odo 会偏大。经验做法是给 R_updated 设置上下限上限是当前值的 3 倍下限是 0.3 倍避免自适应导致参数极端化。实测效果在粗糙室外地面轮速计真实噪声明显增大残差序列方差上升在线调整后 R 自动放大滤波器对轮速计的信任自动降低整体轨迹精度反而比固定 R 更好。4.4 初始协方差 P_0 的合理设置P_0 表示滤波器初始状态的不确定性。设置过小会让滤波器一开始就“自信满满”如果初始速度估计有偏差可能迟迟无法修正设置过大会导致初始几帧抖动明显。在轮速计融合场景中P_0 中速度分量的设置要与 R_odo 配合。我一般把 P_0 的速度分量放宽到 0.1(m/s)² 左右角速度分量放宽到 0.1(rad/s)² 左右。这样滤波器在启动阶段会快速接受轮速计观测将速度收敛到合理范围而不会出现因为初始协方差太紧导致的前几米轨迹偏硬。4.5 协方差调参的实测方法与评价指标参数到底调得行不行不能只看轨迹图要看残差序列和新息序列。我每次调参后都会做三个层面的评估第一残差均值应接近零。如果轮速计更新残差长期为正说明轮速计测量的均值存在系统偏差优先检查标定而不是调 R。第二残差方差应与 R 的量级大致匹配。如果实际残差方差远大于你设置的 R说明设置太乐观滤波器对轮速计的信任超过了实际这种情况在打滑或颠簸路面下特别危险。第三对比融合前后的轨迹闭合误差。在室内场馆跑一个矩形闭环记录起点终点误差。固定 R 参数下融合后误差应该显著小于纯激光方案。若融合后误差反而变大首先怀疑不是 R 的问题而是打滑检测逻辑没有生效。5. 实操过程从零开始集成轮速计到 Fast-LIO25.1 代码结构与关键文件说明Fast-LIO2 的代码结构我就不展开全讲了只聚焦与轮速计融合相关的部分。核心修改点通常在以下几个文件中odometry 节点负责订阅激光雷达、IMU、轮速计数据并触发融合流程。ESIKF 更新函数在这里加入轮速计的测量更新方程。配置 YAML 文件加入 R_odo 和轮速计相关参数。打滑检测模块可以写成一个独立的类或函数在轮速计数据进入滤波器之前做预处理。在动手改之前建议先在 ROS 环境里写好一个轮速计话题转换节点把底盘自带的轮速数据可能是左右轮速度或轮速角速度转换成标准的机器人速度消息。这一步看似简单但带来的调试收益很大因为后面所有融合逻辑都只依赖这个统一格式。5.2 核心代码框架示例轮速计测量模型的核心代码不复杂我给出一个精简版的伪代码框架方便你理解数据流向和更新逻辑。假设已经得到机器人坐标系下的速度观测 z_odo [v_odo, w_odo]对应测量噪声 R diag(0.02, 0.02)// 在 ESIKF 更新流程中新增轮速计更新 void esikf_update_with_odo(StatesGroup state, const Eigen::Vector2d z_odo, const Eigen::Matrix2d R_odo) { // 预测测量值从当前状态提取速度 Eigen::Vector2d z_pred; z_pred state.vel.norm(), state.rot.norm(); // 具体形式取决于模型 // 计算残差 Eigen::Vector2d r z_odo - z_pred; // 计算雅可比矩阵 H Eigen::Matrixdouble, 2, 18 H; H.setZero(); H.block2, 2(0, 6) Eigen::Matrix2d::Identity(); // 对应速度状态 // 计算卡尔曼增益并更新状态 // 这里实际与 Fast-LIO2 的 ESIKF 更新流程一致 // K P * H^T * (H * P * H^T R_odo)^(-1) // state state K * (r - H * state_error) }这段伪代码省略了 ESIKF 的迭代细节但主流程就是“预测残差 - 计算雅可比 - 更新状态”。编写时要注意轮速计的测量值不是全局速度而是机体坐标系下的速度因此雅可比矩阵要正确表达机体速度与全局状态量之间的关系。打滑检测模块的伪代码也很直接我一般写成bool slip_detected false; double angle_error compute_angle_error(v_ekf, v_odo); double speed_error compute_speed_error(v_ekf, v_odo); if (angle_error deg2rad(15.0) speed_error 0.15 * v_ekf.norm()) { slip_detected true; } if (slip_detected) { // 根据持续时间分为降权、剔除、重置三类 }实际工程中滑动窗口、阈值和降权系数都需要在线调试。我建议先用 rosbag 录制几组包含直线、转弯、打滑场景的数据离线回放调整参数而不是一上来就在实车上反复试效率太低也容易损耗设备。5.3 实车测试流程与效果评估代码改完后不要直接跑完整流程。我的建议是分三步测试第一步纯轮速计积分测试。关闭激光和 IMU只让轮速计积分得到航迹走一个直线和矩形验证标定是否准确。这一步能快速暴露轮径和轮距的标定问题。第二步Fast-LIO2 不加轮速计跑一遍原有流程记录 baseline 轨迹。这一步是用于后续对比的基准。第三步开启轮速计融合重复相同的路径。对比第二步和第三步的轨迹、闭环误差、状态估计的平滑度。如果融合后轨迹出现毛刺或抖振优先检查打滑检测阈值和时间同步而不是急于调整协方差。我测试过的一个典型参数组合差速小车轮径 0.15m轮距 0.35m激光雷达 10HzIMU 200Hz编码器 50Hz如下参数初始值说明R_odo 线速度0.02 (m/s)²对应标准差约 0.14m/sR_odo 角速度0.02 (rad/s)²对应标准差约 0.14rad/s打滑角度阈值15 度与当前速度方向比较打滑速度阈值0.15 倍车速相对阈值适配不同速度轻度异常降权倍数8 倍直接乘在 R_odo 上零速粘滞窗口200ms防止底盘回差干扰这套参数在室内瓷砖、环氧地坪、短草地、以及有限的路面颠簸场景下都跑出了比较稳定的效果。6. 常见问题与排查技巧实录6.1 融合后轨迹出现周期性抖动这个问题的根源通常是轮速计和 IMU 的时间戳没有严格对齐。周期性抖动最容易出现在轮速计频率较低比如 20Hz且插值不够平滑的情况下。排查方法是把轮速计和 IMU 的时间戳画在同一张图看是否存在固定相移。我遇到的案例是底盘驱动节点的轮速发布时间戳比实际采样时刻晚了约 30ms导致轮速计“看到”的是过去的速度与当前滤波器状态不匹配。修正方法是在驱动节点发布消息时改用硬件时间戳如果编码器外设支持或者在融合节点里对轮速计做前向补偿即把轮速数据向后平移一个固定的延迟量。6.2 打滑检测频繁误报如果你发现车辆在正常行驶时也经常触发打滑检测首先检查速度阈值是否设置得太绝对。比如你把 speed_error 设定为固定 0.1m/s在车速为 0.5m/s 时这个阈值相当于 20% 的偏差偏严了。改成相对阈值当前车速的百分比会好很多。另一个常见原因是轮距或轮径标定不准。如果轮径偏大轮速计解算出的速度会系统性地比真实速度高残差序列长期为正打滑检测也容易被触发。先重新做标定再调检测阈值。6.3 协方差自适应导致滤波器发散在线协方差估计是一把双刃剑。如果你发现 R 在自适应过程中持续减小有可能是残差序列里包含的“新息”被低估了。解决办法是限制 R 的下界不能让 R 低于物理下限。我的建议是将 R 的最小值设为基础值的 0.3 倍。这样即使残差很小滤波器也会保留对轮速计最基本的“不信任”防止系统过度相信某个传感器导致的发散。6.4 轮速计融合后航向漂移反而变大这种情况很反常但确实会发生。最大嫌疑是轮距标定不准。差速底盘的航向角速度 w_odo (v_right - v_left) / wheel_base如果 wheel_base 标得偏小轮速计给出的角速度会偏大在转弯时滤波器会把航向往错误方向拉。另一个可能是零速粘滞窗口太大导致车辆停车后开始转弯时轮速计仍然在输出零速度而这个零速观测与真实的转弯角速度冲突。把零速粘滞窗口缩小或者在检测到右轮/左轮有显著差速后立刻退出粘滞状态。6.5 快速 FAQ 速查表症状可能原因解决方向轨迹抖动时间同步偏差检查时间戳对齐与插值打滑误报阈值过严或标定偏差改相对阈值重新标定轮径轮距融合后漂移更大轮距误差、零速粘滞过大标定轮距优化粘滞窗口直行跑偏左右轮径不一致左右轮分别标定滚距转弯轨迹扭曲轮距或角速度噪声设置偏小重新标定轮距放大 R_odo 角速度分量这组速查表基本能覆盖我踩过的绝大部分坑。当然每个底盘的机械特性不一样你遇到的问题可能更多、更奇怪但只要把握住“标定 - 时间同步 - 打滑检测 - 协方差”这条主线任何古怪问题都能拆解成可排查的环节。7. 实操心得与下一步扩展方向这套轮速计融合方案我个人最满意的地方是它并不需要大改 Fast-LIO2 的框架而是在原有 ESIKF 流程中增加了一个相对独立、逻辑清晰的测量源。相比自己去写一套完整的组合导航系统这种做法投入产出比高很多尤其适合已经有 Fast-LIO2 基础、希望通过增量传感器提升鲁棒性的团队。关于参数调整我想再补充一个很个人向的经验不要试图一开始就把所有参数调到完美先把打滑检测的逻辑跑通再调协方差。因为打滑检测本身就是一个保护机制它不正常工作的话后续协方差调参都会失真。把保护机制做扎实剩下的参数调整只是在已有鲁棒性基础上的“锦上添花”。后续你可以做的扩展方向不少。一是加入轮速计零速检测与静止判定让机器人在启动阶段有更好的初始状态估计二是加入地面坡度估计让轮速计在坡道上的模型更加精确三是将轮速计与 GNSS 融合在室外场景进一步压制长时漂移。这些方向本质上都还是在“给滤波器提供更多可靠的信息源”遵循的思路与轮速计融合完全一致。如果你也遇到类似问题欢迎带着你的具体底盘参数和实验现象来交流。实际踩坑获得的经验往往比理论推导更值得记录。