解读 06 · imu.rs:从 IMU 原始数据到姿态
解读对象:
duck-control/src/imu.rs(355 行)
需要的前置:《什么鸭公开课》第 04 课(61→14 契约)+ 解读 02
这一篇回答:61 维观测里最前面的 6 个数(陀螺仪 + 投影重力)从哪来。答案的反直觉之处在于:主机上没有任何姿态融合。IMU 板(imu_to_dxl,主芯片 LSM6DSV16X)挂在 Dynamixel 总线上,芯片自己的 SFLP 硬件融合块直接输出游戏旋转四元数,还自己估计陀螺零偏(imu.rs:1-7)。控制循环拿到的 12 字节块 IMU_BLOCK_LEN = 12(imu.rs:22),和 15 个舵机共用 解读 01 里那次批量读;板子的完整诊断块其实有 20 字节,只取前 12 是为了塞进同一笔事务(imu.rs:15-17)。
1. 解码:有符号计数、半精度、缺位的 w
SflpDecoder::decode(imu.rs:103)分两半。前 6 字节是三轴陀螺仪原始计数:有符号 i16 小端,乘 GYRO_RAD_PER_LSB = 0.0175 × π/180(imu.rs:48)换成 rad/s——量程 ±500 dps,每计数 17.5 mdps。测试提醒读成无符号的后果:所有负角速度变成大正数,"看起来像机器人在打转"(imu.rs:339-341)。
后 6 字节是四元数的 x/y/z,IEEE 754 半精度(binary16)打包;w 不传,用 w = √(1 − x² − y² − z²) 补回(imu.rs:13)。半精度解码 half(imu.rs:171)是手写的,所以有专门测试钉住 0x3C00=1.0、0xBC00=−1.0 这类已知值——指数偏置错一点,机器人就朝错误的方向看地平线(imu.rs:249-252)。
2. 未知不等于直立:全零块的处理
芯片刚上电、SFLP 还没写完查找表时,四元数字节是全零。直接解释成单位四元数意味着"报告完美直立"——注释称之为"能对摔倒检测撒的最坏的谎"(imu.rs:59-62)。所以 decode 遇到全零就保留上一个好四元数 last_quat(imu.rs:118-137),全零块也不计入 quat_samples。合法性检查有两道:x/y/z 有限且 norm_sq ≤ 1.02(容忍半精度舍入,imu.rs:123),合成并做挂载校正后四元数范数要大于 0.5 才更新(imu.rs:133-135)。
坐标系同样在这里处理:板子装的方向和躯干不一致,DEFAULT_MOUNT(imu.rs:78)是一个绕 Y 轴 +90° 的旋转,把传感器系转到躯干系——"trunk = [+raw_z, +raw_y, −raw_x]"。陀螺仪用 rotate(q·v·q⁻¹)正变换,投影重力用 rotate_inverse(q⁻¹·v·q)把世界系的 [0, 0, −1] 变到躯干系(imu.rs:146):直立时它就是 [0,0,−1],倾斜时 z 往 0 爬,这正是 解读 04 的阈值和 解读 05 的速率所消费的那个量。
3. 两道便宜的防御:ready 门与三点中值
ready()(imu.rs:99)要求 quat_samples ≥ 25——约 100 Hz 下的 0.25 秒——才承认"芯片确实产出过融合结果"。在此之前姿态是默认值而非测量值,摔倒检测必须闭嘴。
median3_each(imu.rs:161)对陀螺仪和重力各做一个三点逐分量中值:丢块或坏块表现为单个野值,中值恰好把它扔掉,又不像平均那样引入滞后。一个诚实的注脚(imu.rs:140-145):归一化在中值之前做,于是瞬态期间三个单位向量的逐分量中值本身不再是单位向量,策略会看到略短的向量——这是刻意的,为的是和参考实现行为一致,并用测试 gravity_stays_close_to_unit_through_a_transient 把边界钉在 0.5 到 1.0 之间(imu.rs:324-334)。
你带走的收获
- 融合下放到传感器芯片(SFLP),主机只剩解码与防御——省 CPU,也少一个要调的滤波器。
- "数据缺失"绝不能用无害默认值顶替:全零四元数补成直立,是给下游安全逻辑喂最坏谎言。
- 硬件量程换算(
i16有符号性、mdps/LSB、半精度指数偏置)每一项都值得一个已知值测试。 - 三点中值是单点野值剔除的标准答案:无滞后,常数开销,适合 50 Hz 热路径。
- 与参考实现"保持同样的小毛病"有时比修掉它更重要——sim2real 的行为一致性优先。