VINS-MONO添加轮速计的问题

Question

我想对VINS-MONO的初始化做修改，额外添加一个轮速计的速度观测，推的公式和参考如图，但是我这样改之后貌似与没改之前的效果差不多，代码部分的修改一个是添加了轮速计数据，在estimate.cpp中的proceIMU函数里对每一帧imu数据添加一个对应的轮速速度，判断条件是每次遍历轮速数据，找到时间戳对应的那一帧轮速放到imu帧里，然后在初始化那里，对速度和尺度的初始化函数中加了图中公式这样的修改(公式里的V_w是轮速的速度)，也就是tmp_A和tmp_b的维度变成了12。最后的效果貌似没啥变化，但是我看公式也没啥问题，实在是不知道那里出了问题，望各位帮忙看看，祝各位新年快乐~

Answer 1

在 VINS‐Mono 里直接往初始化阶段加“轮速计速度观测”这种改动，看公式往往很直观，但实做起来有一些容易被忽略的细节。如果改完后效果跟没改之前“几乎没区别”，通常说明要么是新的观测约束没有真正参与到优化，要么是参与了但权重(不确定度)等处理不当，导致对最终结果影响很小。下面给你一些可能的排查思路，供你逐步检查。

---

1. 检查轮速观测与 IMU 坐标系、时间戳是否对齐

• 轮速计的速度是在哪个坐标系下定义的？ 通常车轮速度是车体坐标系、或者某个水平平面坐标等；而 VINS‐Mono 中 IMU 是机载坐标系(一般安装方向又不同)。

  • 你需要先将轮速速度 $\mathbf{v}{w}$ 变换到 IMU 坐标系 下，再写进那个$\mathbf{v}^{b}{k}$的观测里。若漏了这一步，或用了错误的外参，会使得新约束要么被视为“噪声”扔掉，要么无法提供有效信息。

• 时间同步：你的代码里是“遍历轮速数据，与 IMU 数据做时间戳匹配并放到同一个 IMU frame 里”，要确保插值或匹配方法正确，否则会产生较大误差，进而降低约束在初始化中的作用。

---

2. 确认新添加的观测因子是否真在优化中被使用

VINS‐Mono 的初始化(尤其是 InitialAlignment / RefineGravity 这些函数)里，会构造一个 $\mathbf{H}^T \mathbf{H} \mathbf{x} = \mathbf{H}^T \mathbf{b}$ 的线性系统来解算速度、重力、尺度等。你在原公式上添加了轮速计的观测，意味着你要在这个大矩阵/向量里 额外多出一块残差 及其对应的雅可比。

• 你提到把维度从“10”扩展到“12”，那就要确认：
  1. 有没有正确地在构造 H 和 b 时，把这块的 $\mathbf{J}$（Jacobian）和残差加进去？
  2. 有没有相应更新求解顺序？ (譬如原本只解 $\mathbf{v}_b, \mathbf{g}^b, \mathbf{p}_c^b, \dots$，现在又多了轮速相关的量或新的残差，会不会漏了一些变量？)
  3. 求解结束后，是否把新的求解结果(或者说校正量)应用回到系统状态中？ 如果没有，可能这块观测对最终状态没任何影响。

一个最简单的调试方式：

• **把轮速观测噪声先设得极小(等于非常信任它)**，看看结果是否发生明显变化。
  • 如果依旧跟没加这项时一样，那很可能说明这条“因子”没有被正确地放进残差方程里，或者它被“忽略”了(比如某个标志位没打开，或是维度没有对上，Jacobian 被置零等)。
  • 如果结果出现了明显的跑飞(表明轮速信息过强，甚至冲击到了别的部分)，那至少说明轮速约束是起作用的，只是原本给的噪声(权重)太大/太小或者数据不准确而效果不明显。

---

3. 校验你的公式/雅可比是否真正能约束到“速度”和“尺度”

很多人在 VINS 里加轮速传感器，最关心的是“在纯旋转或视差较小的场景下能不能靠轮速帮忙估计尺度、速度”。如果你只是把 $\mathbf{v}^b - \mathbf{v}^b_w$ 当作残差，那要确保它确实在同一个参考系下，并且卷进了尺度因子(如果你把速度记在世界系，需要再跟车体系做变换)；或者你要保证轮速计测到的“线速度”也能在公式里与 VINS 的“速度状态”关联到同一个量。而且要留意系统有没有“轮速比例因子”(wheel radius scale factor)之类的待估计量——有时候车轮半径并不精确，也会影响到精度。

---

4. 轮速计信息本身可能不够精确或与实际运动不符

在真实车辆上，轮速计往往会受：

• 车辆打滑(尤其是急转弯或非平路面)
• 不同轮胎气压、磨损(导致轮子实际外径跟标称不一致)
• 时间延迟等

这些都会让它“表面上看是一个速度观测”，但其实误差也不小。如果你在 VINS‐Mono 初始化时又给它设置了一个和 IMU 差不多量级或更大的噪声，那么它能起到的约束作用也就不大，结果看起来就跟没加差不多。

---

5. 你观察“效果差不多”的衡量方式

• 是在一些本来就能很好跟踪的场景下比较吗？ 如果视觉特征丰富且 IMU 本身也很准确，那么额外加轮速计的改动确实可能“肉眼看不出差别”。
• 是在纯转动、特征较少(容易尺度漂移)的场景下，对比有没有改善？ 这可能更能看出差异。
• 有没有在系统内输出 residual 大小、或者 Jacobian 矩阵观察？看一下每一帧轮速的残差值在优化里究竟如何变化，是否被明显减小或者直接被当作 outlier 舍弃？

---

6. 小结及建议

1. 坐标系转换：确保轮速测量转换到 IMU 坐标系时正确，且时间对齐没问题。
2. 调试你的“新因子”是否真实参与优化：
   • 增大(减小)轮速观测的置信度，看对最终速度/重力/尺度解算的影响是否显著。
   • 打印或可视化残差，看在迭代中是否被使用、是否在减小。
3. 检查公式中的推导与实际系统的变量维度：例如 VINS 原本 10 维(3 维速度、3 维重力方向、1 维重力大小或与尺度耦合、3 维外参或偏置等)，现在扩到 12 维，你每个分量的物理含义有没有对上？
4. 检查轮速计本身的可靠性：如果轮子打滑严重或者尺度不准，给再多观测也可能没帮助。
5. 测试在“视觉不利场景”：如果在正常场景下视觉+IMU 已经很稳了，轮速带来的提升不一定能显著体现。

总之，最常见的“加传感器没效果”的原因，往往不是公式本身有错(公式通常大家都差不多)，而是要么加进去的观测没有被系统真正使用；要么使用了但观测与现有信息相比太弱或噪声权重设置太大；要么坐标对齐、外参或时间戳处理不当，导致在优化里反而起不到正面作用。
建议你逐项排查上述问题，并尝试给轮速因子一个极高权重(或者场景中减少视觉特征的可靠性)，看是否能逼着系统“按照轮速走”，这样就能判断它是不是真在起作用了。祝调试顺利，新年愉快!