Microduck 怎么知道自己在哪：没有 GPS 和激光雷达的小鸭子靠脚底定位
做过移动机器人的人都知道，定位是最难的问题之一。室内不能用 GPS，激光雷达又贵又重，视觉 SLAM 对算力要求高。Microduck 是一个 25 厘米、800 克的小型双足机器人，RK3566 主控板上没有跑 SLAM 的余裕。它用的是一种古老但有效的方法：靠脚底接触点和 IMU 估算自己在世界里的位置。
这个方案的核心思路其实不复杂。任何时候，机器人总有一只脚踩在地上。踩住的那个点就是机器人跟世界的锚点——它的世界坐标固定不变。通过 IMU 知道躯干朝向，通过正运动学知道脚相对躯干在哪，就能反推出躯干在世界的什么位置。换脚的时候锚点移动，但坐标连续不跳。
听起来简单，魔鬼在实现细节里。今天拆开看 Microduck 的 odometry crate 源码，看它怎么处理这个问题的。
锚点不是脚中心，是脚底网格点
第一件事可能就跟你直觉不一样。Microduck 的接触锚点不是脚底板的几何中心，而是一组从脚底网格上采样的候选点。
脚底不是平的。Microduck 的脚底有斜切边、有弧度，不同位置的高度差有 7-8 毫米。如果你用脚底中心做锚点，但实际接触地面的点是脚尖边缘，你的正运动学算出来的躯干高度就会差几毫米——对一台 25 厘米的机器人来说，几毫米的高度误差意味着重心估算偏移，后续所有决策都受影响。
Microduck 定义了三套锚点方案，全部由 microduck_rl/scripts/odom_anchor_points.py 从 MuJoCo 场景的脚底网格自动生成，写死在 anchors.rs 里。
V15 是老版本遗留方案，用脚底包围盒的四个角，在 Z=0 平面上。这是 v1.5 硬件的脚底形状，四个对称点，间距约 54mm × 41mm。简单但精度有限——在 MuJoCo 仿真里走 3.2 米后漂移 17 毫米，而且站立高度偏高 3.7 毫米。偏高原因很直接：包围盒角点在网格平面上，但真实脚底在那些位置是悬空的，Z 坐标不是 0。
ALPHA4 用 Alpha 版脚底平坦接触区的四个角。点是从网格上采的，Z 坐标带真实高度（0.001-0.002 米范围）。3.2 米行走漂移 16 毫米，比 V15 好一点但没质变。
ALPHA16 是生产方案。4×4 网格覆盖整个脚底，包括斜切边。16 个点，每个都有从网格采样的真实 Z 坐标。3.2 米行走漂移 9 毫米——差不多砍半。站立高度精确：不偏高。额外计算成本？约 0.5 微秒。
为什么 16 个点比 4 个点好这么多？因为行走时脚跟地面的接触点不是固定的。起步时脚尖先离地，接触点移到脚跟；落地时脚跟先触地，接触点在脚跟边缘。4 个角的方案里，最近接触点可能在两个对角之间跳，每次跳都带一点误差。16 个点的网格把脚底切得更细，最近接触点跟实际接触位置的偏差更小。
锚点切换：两 tick 确认，防抖动
锚点不是每 tick 重新选的。选锚点的逻辑有一个防抖机制。
每个 tick，里程计算出所有候选点的世界 Z 坐标，找到最低的那个。如果某个点的世界 Z 低于 -10 毫米（SWITCH_MARGIN），它就是一个候选切换目标。但切换不会立刻发生——候选点必须连续 2 个 tick 保持最低（SWITCH_CONFIRM_TICKS = 2），锚点才会真正移动。
为什么需要这个确认期？因为正运动学和 IMU 读数都有噪声。一个 tick 的异常读数可能让某个点暂时看起来比当前锚点低。如果立刻切换，锚点会在噪声驱动下来回跳，躯干位置估计就跟着抖。2 个 tick = 40 毫秒（50Hz 循环），远小于一个步态周期（通常 400-800 毫秒），但足以滤掉单次噪声。
切换时还有一个关键设计：锚点保留它当前的世界 X/Y 坐标，不是重新计算。这意味着位置估计在切换瞬间是连续的——不会跳。新的锚点在它自己脚底的局部坐标系里有一个位置，在世界坐标系里继承了切换前的 X/Y。
SWITCH_MARGIN 设成 -10 毫米而不是 0，是给正运动学和 IMU 的噪声留余量。锚点本身在 Z=0（地面），但候选点要低于 -10 毫米才能挑战锚点地位。这个负margin确保只有真正踩下去的点才能触发切换，不是噪声波动里碰巧低了一点的点。
正运动学：从关节角度到脚的位置
里程计的核心计算量在正运动学（FK）。每个 tick 要算两只脚相对躯干的位置。
Microduck 的 kinematics crate 用了一个编译时优化：MJCF 模型在加载时被编译成扁平的链表结构。每个 site（比如 left_foot）有一条从根到 site 的链，链上每个环节是一个固定变换加一个可选的铰链旋转。查询时遍历这条链做前向计算——没有 HashMap 查找，没有内存分配，纯数组 fold。
这个优化是被迫的。prototype 的 kinematics crate 用 HashMap 传关节角度，每次查询一个 site 要重算整个机器人所有 body。在 bench 上跑没问题，但塞进 50Hz 控制循环里就太慢了——每 tick 要查两个脚的位姿，9 次链计算变成 2 次，HashMap 查找变成数组索引。
关节角度的映射也有讲究。JOINT_NAMES 是 15 个舵机的名字列表，嘴部舵机排在里面但不影响腿的运动学。joint_map 把 JOINT_NAMES 的位置映射到 kinematics 模型的关节索引，嘴部对应的映射是 None，在更新角度时直接跳过。
IMU 提供朝向，但不提供位置
IMU 在这个系统里只做一件事：提供躯干的世界朝向四元数。不提供位置。
为什么不用 IMU 做位置积分？因为双足机器人的 IMU 位置积分漂移非常严重。脚着地时有冲击，冲击产生加速度尖峰，积分后变成速度漂移，再积分变成位置漂移。几步之内位置估计就能飘到几米外。
Microduck 的做法是让 IMU 只管朝向，位置完全由脚底接触锚点决定。朝向漂移？Yaw 方向确实会漂——没有磁力计，世界坐标系是”开机时机器人面朝的方向”。但对一个用游戏手柄遥控的机器人来说，相对朝向就够了。不需要知道北在哪，只需要知道”我相对出发位置走了多远、转了多少”。
IMU 四元数在喂给里程计之前会被归一化（Quat::new(w, x, y, z).normalized()）。这一步不能省——ONNX 策略推理或总线读取可能偶尔吐出一个非单位四元数，直接用它做旋转矩阵会导致 FK 结果缩放变形。
初始化：第一次 tick 锚定当前脚
里程计的第一次 update 调用有一个特殊路径。needs_init 标志为 true 时，不计算候选点，直接把当前锚定脚的世界 X/Y 设为脚当前位置。这让机器人从 (0, 0) 开始——不管它启动时脚在哪。
如果不做这个初始化，机器人启动时锚点在 (0, 0)，但脚实际不在 (0, 0)。第一个 tick 算出来的躯干位置就会带一个初始偏移。虽然这个偏移是常数不影响相对运动，但下游消费位置数据的代码可能假设从原点出发。
初始化完成后，needs_init 设为 false，后续 tick 走正常路径。
不是独立服务，嵌在控制循环里
里程计不是一个独立的守护进程。它是一个纯 struct，在 robotd 的控制循环里每 tick 调用一次。
原因在于它的输入。里程计需要的东西——关节角度和 IMU 四元数——恰好是控制循环每个 tick 已经从总线上读到的数据。不需要额外的传感器读取，不需要 IPC 通信。把它做成独立服务意味着每 tick 要把关节和 IMU 数据序列化发出去，再把位置结果收回来——两趟 IPC 开销换零收益。
Cargo.toml 的注释说得很明确：它依赖 duck-ipc-proto（关节名字列表）和 kinematics（MJCF 模型），不依赖 tokio、不依赖 socket、不依赖 systemd。纯计算，纯函数式。
9 毫米漂移意味着什么
3.2 米行走 9 毫米漂移——相对误差约 0.28%。这个数字在实际机器人上意味着什么？
先说这个数字的来源。它是在 MuJoCo 仿真里测的，真机上的漂移会更大，因为真机有更多噪声源：脚底打滑、IMU 温漂、舵机编码器误差、机械变形。保守估计真机漂移在 1-3% 量级，也就是走 1 米偏 1-3 厘米。
对 Microduck 的用途来说够用了。它不是在仓库里跑物流分拣需要厘米级定位的 AGV。它是一个用游戏手柄遥控的小型双足机器人，位置估计主要用于两个场景：一是 robotctl monitor 显示机器人的大致位置和朝向，二是作为策略的观测向量的一部分（如果策略需要位置信息的话）。
如果你要做自主导航——比如让 Microduck 自己走到指定位置——9 毫米的仿真漂移在真机上可能变成几厘米，走 10 米可能偏半米。这种场景下你需要额外的定位手段：UWB 信标、天花板视觉、或者预置地图。但这些都超出了 Microduck 的设计目标。
对复刻者的实操建议
如果你在搭自己的小型双足机器人里程计，Microduck 的方案有几个值得借鉴的点。
锚点用多点网格而不是单点。4 个角看起来够用但精度差一半，16 个点的网格计算成本几乎为零（0.5 微秒），精度翻倍。如果你的脚底形状更复杂（有弧度、有花纹），考虑用更密的网格。
切换要有确认期。1 个 tick 的确认太短，噪声会穿过去；5 个 tick 又太长，快速步态可能在确认期内就抬脚了。2 个 tick（40ms）是一个在 50Hz 控制频率下的合理值，根据你的控制频率调整。
IMU 只管朝向不管位置。这个决策看起来是放弃了信息，实际上是避开了更大的麻烦。IMU 位置积分在双足机器人上的漂移速度远超接触式里程计，混在一起用反而更差。
FK 要编译不要每次解析。如果你用 Python 的 mujoco_py 做 FK，每 tick 的解析开销可能占掉控制循环一半的时间预算。Microduck 的方案是 MJCF 在加载时编译成链表结构，运行时纯数组计算。如果不用 Rust，用 C++ 的 pinocchio 库或者 Python 的 yourdfpy 预编译也能做到类似效果。
锚点坐标从 CAD 网格采不是从包围盒采。包围盒角点不在网格表面，高度差几毫米。这几毫米在站立姿态估计里直接变成高度偏差。从网格采样能拿到真实的 Z 坐标。
最后一点：里程计不是独立服务不代表它不能独立测试。Microduck 的 kinematics crate 有 tests/fk_against_mujoco.rs，用 64 个随机姿态跟 MuJoCo 自己的 mj_kinematics 对比验证。这种测试是必须的——FK 的正确性是里程计一切的基础，一个符号错误能让所有位置估计偏向一边。
—
数据来源：pollen-robotics/microduck GitHub 仓库 odometry/src/lib.rs（Odometry struct 完整实现、update 方法逻辑、SWITCH_MARGIN 和 SWITCH_CONFIRM_TICKS 常量、锚点切换防抖机制、reproject 和 lowest_corner 算法、初始化流程），odometry/src/anchors.rs（V15/ALPHA4/ALPHA16 三套锚点方案的坐标数据、AnchorSet 结构定义），odometry/Cargo.toml（依赖关系和设计注释），kinematics/src/lib.rs（MJCF 编译时优化、Model struct、SiteId、链式 FK 查询路径、ALPHA_MJCF include_str 嵌入），docs/design/architecture.md（七守护进程架构中 odometry 的定位、纯 struct 非独立服务的设计决策）。
