解读 16 · odometry/lib.rs:里程估计
解读对象:
odometry/src/lib.rs(348 行)
需要的前置:解读 13;第一辑《解读 06》(IMU)有助于理解输入。
没有轮子,锚住一只脚
双足机器人没有轮编码器可积。这份代码(lib.rs:1-21)是原型运行时接触式里程计的移植,源自 Rhoban 的人形 model_service。核心想法一句话:任一时刻,一只脚底上的某个点就是机器人与大地的全部接触——把它锚定在世界系(平地,故其世界 Z 恒为 0,X/Y 即它成为锚那一刻的位置),躯干朝向由 IMU 给出,躯干的世界位置由前向运动学反推。换步时,另一只脚的某个角落降到锚之下,锚就搬过去、并保留该角已有的世界 X/Y——估计因此永不跳变。
实现是一个小而有状态的类型 Odometry(lib.rs:47):锚在哪只脚(anchor_foot)、锚在脚底的局部坐标(anchor_local)、锚的世界 X/Y(anchor_xy),外加一个待确认候选 pending。每帧 update(lib.rs:104)只做三件事:把关节角经 joint_map 写进模型序数组(嘴关节映射为 None,lib.rs:50-52——嘴动不了腿);两只脚各做一次 FK(lib.rs:114-117;原型对每个角重走整条腿链,每 tick 9 次链求值,现在 2 次);重投影。
位置是差出来的,换锚要确认
reproject(lib.rs:183-191)就是那句"差出来":躯干位置 = 锚的 X/Y 减去世界系下躯干→接触向量,Z = -contact_z。lowest_corner(lib.rs:195-220)扫两只脚各四个角(角点由 SOLE_HALF_LEN = 0.0270、SOLE_HALF_WIDTH = 0.0206 构成,lib.rs:31-32——注释明说是 v1.5 鞋底 bbox 的占位值,alpha 的 sole_left.stl 尚未实测,待验证),低于 -SWITCH_MARGIN(−0.010 m,lib.rs:37)者获得竞标资格。
但竞标不等于当选:SWITCH_CONFIRM_TICKS = 2(lib.rs:42,50 Hz 下即 40 ms)要求候选连续两帧保持最低,FK 与 IMU 的单帧毛刺就偷不走锚。测试 the_anchor_switches_only_after_the_claim_holds(lib.rs:279)专门演这一条:单帧滚转不换脚,连续保持才换。状态切换加时间确认,是对抗传感器毛刺最廉价通用的模式。
两个诚实的细节:
- 朝向完全取 IMU 的 yaw(lib.rs:155),没有磁力计,世界系就是"开机时机器人看着的方向"(lib.rs:19-21)——对相对运动的消费者够用,而且明说了。
- 首帧
needs_init(lib.rs:119-123)用实际启动姿态播种锚,让躯干从 (0,0) 起步,而不是带着脚的偏移。
测试的取向也值得学:standing_still_stays_at_the_origin(lib.rs:239)防"常量输入积成漂移";stance_leg_motion_translates_the_trunk_continuously(lib.rs:309)要求每 tick 位移小于 2 cm——连续性本身是断言。
你带走的收获
- 接触式里程计 = FK + IMU + "锚定最低脚底点",无轮机器人照样有位置。
- 任何"换状态"的决定都可加 N 帧确认,成本低到不该省。
- "站着不动必须原地不动"是一条防积分漂移的关键测试。
- 占位常量要写明占位状态与出处,否则会活成永久事实。