解读 68 · 阶段 B/C:IMU 接入两条路与摄像头
解读对象:
docs/HL-2915路线全流程教程.md(380 行)§3–§4,交叉核对duck-control/src/imu.rs(355 行)
需要的前置:解读 44(LSM6DSV16X 读取路径)、解读 06(ImuData 怎么被消费)
61 维观测里有 6 维来自 IMU——机体角速度 3 + 重力投影 3——而且教程 §3 开门见山:摔倒检测就靠重力向量,没有 IMU,策略等于闭眼走路。阶段 B 是让这 6 维以和官方逐位相同的口径到达策略;阶段 C 的摄像头几乎不用写代码。
1. 两条路:I²C 直连,还是挂总线
教程 §3.1 给了两个选项:I²C 模块直连(推荐)——LSM6DSV16X 模块 4 根线接 Radxa 40-pin I²C,少一块自绘板、少一个总线故障点(原型机当年就是 BNO055 走 I²C);imu_to_dxl 自绘板挂总线——教程注明"板子固件本来就备了飞特协议分支",整机走通、想还原官方架构时再上(见解读 43)。
接线是断电操作:VCC→3.3V、GND→GND、SDA→SDA、SCL→SCL(具体哪两个脚是 I²C 查 Radxa 文档),接好后 i2cdetect -y <总线号> 扫到 0x6A(或 0x6B)就是接对了。
2. 朝向是硬约束,融合交给芯片
duck-control/src/imu.rs 把传感器→躯干的安装旋转写死为 DEFAULT_MOUNT:trunk = [+raw_z, +raw_y, −raw_x](imu.rs:76 起,绕 Y 的 +90° 旋转)。躯干 +X 向前、+Y 向左、+Z 向上时,传感器应为 +Z 向前、+Y 向左、−X 向上;轴向要通过实际转动确认,不能凭接线丝印猜。做不到就改 mount;物理安装可以不同,但输出的 gyro/gravity/quat 必须保持训练使用的躯干坐标约定,并把安装旋转记进构建日志。方向不对时先查安装旋转,不要在下游给单轴乘 −1。
阶段 E 写 Feetech2915Io 时要一并实现 IMU 读取,产出的 ImuData 三项与官方完全相同(imu.rs 真实结构):gyro[3] 机体角速度 rad/s(±500dps 量程 17.5 mdps/LSB,GYRO_RAD_PER_LSB = 0.0175 × π/180);gravity[3] 投影重力单位向量,正立 = [0, 0, −1](ImuData 的默认值)——策略观测与摔倒判定共用的那个数;quat[4] 标量在前 [w,x,y,z],用芯片 SFLP 硬融合输出,别自己写融合。
3. 五步验证法:给传感器做体检
§3.4 的验收,每步一个数:平放 30 秒,gyro 每轴 <0.05 rad/s;正立 [0,0,−1] → 倒立 [0,0,+1],z 变号、模长 ≈1;低头时 gyro 的 Y 轴符号与训练约定一致(不对就改 mount,别改符号表);慢转 90° 四元数无跳变、模长 ≈1;50Hz 连读 10 分钟,"与上次完全相同"的 stale 读连续不超过 3 帧。
最后一条在官方代码里有对应物:io.rs 的 ImuStale 计数器——总线版告警阈值是 STALE_RUN_WARN = 25 帧(半秒,bus.rs),台架验收的 3 帧更严:那是在给传感器定性。
4. 阶段 C:摄像头链路不用写驱动
§4 的定位很清楚:官方软件栈已备好——mediad 出 720p30 流,duck-detect 在帧上跑检测(Radxa 上走 NPU/RKNN,运行时由 duck-detect dlopen)。要做的只有三件事:
- 装对:IMX219(Pi Cam v2)FPC 排线接主控 MIPI CSI 座,排线 4–15cm 足够(主控和摄像头同在头部,不过脖子);壳内装配时摄像头转 90°——官方就是横着的,逆向出来的结论,不是错误。
- 在 Radxa 上验(Mac 没有 MIPI):
v4l2-ctl --list-devices认出/dev/video*;v4l2-ctl -d /dev/video0 --stream-mmap --stream-count=3连续 3 帧不报错;robotctl health应全绿;duckctl open开 :8080 控制台看画面。 - 看方向:横置 90° 的补偿已生效、检测框画在目标上。
你带走的收获
- 传感器接入先选"数据路径最短"的:I²C 直连比"伪装成总线设备"少一个故障点,代价只是改一处 mount。
- 安装朝向属于训练契约:硬件贴装、代码 mount、重训数据三者必须同源,改任何一处都要留档。
- 姿态融合用芯片硬融合(SFLP),主机只做解码和朝向旋转——
imu.rs整个文件没有一个滤波器。 - 验收要用数字定义"传感器是好的":零漂、变号、符号、连续性、新鲜度,五步各管一件事。