Microduck 的 IMU 集成：为什么姿态传感器和舵机共享一条总线

做机器人控制的人都知道 IMU 是命门。策略每 20 毫秒要做一次决策，身体朝哪歪了、是不是要摔了，全靠这个传感器说了算。但 IMU 怎么接进系统，接在哪里，用什么协议传数据——这些问题看起来是硬件细节，实际上影响的是整个控制链路的延迟、可靠性和 sim2real 一致性。

Microduck 在这件事上做了一个不太常规的设计选择：IMU 和 14 个舵机挂在同一条串行总线上，用同一个协议读取。这不是图省事，是有原因的。

一条总线，十五个设备

Microduck 的 Dynamixel 总线拓扑是这样的：一条 UART，1 Mbps，Dynamixel 协议 v2。上面挂着 15 个设备——14 个 XL330 舵机加上一个 ID 为 200 的 imu_to_dxl 板子。没有第二条总线，没有第二个端口。

这个 imu_to_dxl 板子是 v2 版本。它不做 I2C 转 USB 那种事，而是直接坐在 Dynamixel 总线上，像一个舵机一样响应总线指令。control loop 每周期发一次 sync_read，IMU 板子和所有舵机在同一个事务里返回数据。IMU 的 ID 是 200，排在 ID 向量最前面，所以它最先应答。

为什么这么设计？因为这是唯一能保证 IMU 数据和舵机位置数据在同一时刻被采集的方案。如果 IMU 走 I2C 或者 USB，你需要两个独立的读取事务，两个时间戳，两套错误处理逻辑。在一个 50 Hz 的控制循环里，20 毫秒就是全部预算，你不想花一半在等两个传感器同步上。

一条总线、一次读取、一个时间戳。这就是 v2 板子存在的理由。

SFLP 四元数：不传欧拉角

imu_to_dxl 板子在总线寄存器里提供的是一个 SFLP 四元数。不是欧拉角，不是原始加速度计读数，是已经融合好的姿态四元数。

SFLP 是一种压缩四元数格式。标准四元数是四个 float，SFLP 只存三个分量加一个符号位，第四个分量可以通过前三个推出来。省了一个 float 的带宽，对于总线通信来说是有意义的。

为什么不用欧拉角？因为欧拉角有万向锁。当 pitch 接近 ±90° 的时候，roll 和 yaw 的定义会退化。一只鸭子翻跟头的时候真的会经过这种姿态。四元数没有这个问题，它是全局连续的。

为什么不用原始加速度计？因为加速度计测的是比力（specific force），不是重力方向。鸭子在走路的时候，脚落地冲击产生的加速度远大于重力。如果你直接用加速度计向量判断姿态，每走一步都会被冲击脉冲干扰。板载的融合算法已经把这个处理好了，输出的是一个平滑的、已校正的姿态。

projected gravity：观测向量里的唯一 IMU 量

Microduck 的策略观测向量是 61 维。48 维本体感知加 13 维命令块。本体感知里包含关节位置、关节速度、上一帧动作。但 IMU 相关的量，整个观测向量里只有一个：projected gravity。

Projected gravity 是什么？把重力向量 [0, 0, -9.81] 通过当前姿态的逆旋转，投影到身体坐标系上。如果鸭子直立站着，projected gravity 就是 [0, 0, -1]（归一化后）。如果前倾 30°，它变成 [0, 0.5, -0.87] 左右。如果躺平了，它就是 [0, 0, 1]。

这个量的好处是：它直接告诉你身体相对于重力的朝向，不需要积分，不会漂移。它是从四元数算出来的，但比四元数更紧凑——三维向量，刚好塞进观测向量。

在训练配置里有一个开关：USE_PROJECTED_GRAVITY = True。如果打开，用 projected gravity 替代原始加速度计读数。默认是开的。原因前面说过了——加速度计在动态运动中噪声太大，projected gravity 是已融合的、稳定的姿态表征。

为什么训练时要给 IMU 加 6° 随机噪声

sim2real 的一个经典坑：仿真里的 IMU 是完美的，真实 IMU 有安装误差。你把 IMU 焊在板子上，贴到机器人身上，不可能完全没有倾斜。几度的安装偏差看起来不大，但 projected gravity 是归一化向量，每偏离 1° 都会改变它的值，策略会学到依赖一个”完美安装”的 IMU，上真机就蒙了。

Microduck 在训练时做了 IMU 方向随机化：IMU_ORIENTATION_RANDOMIZATION_ANGLE = 6.0，每个环境随机一个最多 6° 的旋转轴加旋转角度，施加到 IMU 读数上。注意这是零中心的随机轴旋转——训练的是对安装偏差幅度的容忍度，不是对某个固定方向偏差的适应。

真正的板子上有一个系统性的约 5° pitch 偏移。这个偏差不是在训练里处理的，而是在 runtime 层面通过 imu-pitch-offset 参数校正的。训练时随机化的是”不知道往哪个方向偏了最多 6°”，runtime 时校正的是”我知道这个板子往前偏了 5°”。两个层面，两种处理方式。

stale read：当 IMU 停止融合

robotctl monitor 的界面上有一个状态行，其中有一个你不想看到的提示：”orientation frozen — 25 stale reads”。

这是什么意思？imu_to_dxl 板子还在总线上，还能应答 sync_read，但返回的四元数不再更新了。板子上的融合算法挂了——可能是 I2C 总线松动、可能是板载 MCU 死了、可能只是融合算法进入了一个不输出的状态。但总线通信层是好的，所以没有任何错误抛出。

这就是为什么 stale-read counter 存在。每个控制周期检查四元数是否和上一帧相同，如果相同就计数。连续 25 次相同就报 frozen。这个数字不能太小——鸭子站着不动的时候，如果融合算法的精度刚好让输出在某个量子化边界上抖动，也可能出现连续几帧相同。25 次（0.5 秒）是一个能区分”静止”和”真的冻结”的阈值。

问题是 frozen 状态下，projected gravity 还在观测向量里，还是一个看起来合理的值。策略不知道 IMU 冻了，它会基于一个过时的姿态继续做决策。如果你正在走路，这 0.5 秒的过时姿态可能让策略输出错误的目标位置，鸭子就开始歪了。

安全层在这里起作用。safety.apply 检查的不是 IMU 状态，是目标位置的合理性和非有限值。IMU frozen 不会直接触发安全层，但 control loop 的健康检查会把它标记为 degraded 状态，monitor 界面上会显示出来。操作者看到这个提示就应该停下来检查硬件了。

没有磁力计，所以方向是相对的

Microduck 的 IMU 没有磁力计。这意味着 yaw 方向会漂移——陀螺仪积分不可能完美，时间一长，累计误差就让朝向偏了。

这不是设计疏忽，是成本和尺寸的权衡。小型双足机器人上加磁力计，要处理电机干扰、需要校准、还会被铁磁体影响。对于一个 25 厘米的鸭子来说，绝对朝向不是刚需。它在室内走，不需要知道北在哪。

所以 odometry 是相对的。robotctl monitor 的 odometry 面板会画一条轨迹，起点标 +，当前位置标 ●。这条轨迹能告诉你”鸭子是不是在转圈走”，但不能告诉你”鸭子走到了房间的哪个角落”。面板说明里写得很直接：”it answers ‘did it walk in a circle’ and not ‘where is it'”。

训练策略也不依赖绝对朝向。观测向量里的命令块是 twist 命令——线速度和角速度的目标值，不是全局位置。策略学的是”往这个方向走这么快”，不是”去这个坐标”。所以 yaw 漂移不影响策略执行，只影响你的可视化判断。

从总线到策略的完整链路

把整个数据流串一遍：

imu_to_dxl 板子上的 IMU 芯片采集加速度和角速度
板载融合算法输出 SFLP 四元数，写入 Dynamixel 寄存器
robotd 的 50 Hz control loop 发 sync_read，IMU 在同一事务里和舵机一起被读取
四元数被解压成完整的 4D quaternion
重力向量 [0,0,-1] 通过四元数的逆旋转投影到身体坐标系，得到 projected gravity（3D）
projected gravity 进入 61 维观测向量，和关节位置、关节速度、上一帧动作、命令块一起组成完整输入
ONNX 策略推理，输出 14 维动作
动作经过 safety.apply 检查和 clamp，加到 home pose 上得到目标位置
目标位置写回总线，舵机执行

整个链路从 IMU 采样到舵机执行，20 毫秒完成一次。没有多余的拷贝，没有跨进程通信（IMU 读取和策略推理都在 robotd 进程内），没有等待传感器同步。

这就是为什么 IMU 要放在 Dynamixel 总线上。不是因为简单，是因为这个链路的每一步都省了一个时间源。在一个要 50 Hz 实时控制的系统里，少一个不确定性就是少一个摔鸭子的理由。