解读 14 · kinematics/head.rs:头部 3 自由度运动学
解读对象:
kinematics/src/head.rs(352 行)
需要的前置:解读 13 的Quat/Pose。
两个常量四元数是一层翻译
MJCF 给出的是 site 位姿,视觉栈说的却是另一套坐标系:cv2 相机系(+x 右、+y 下、+z 前)和 ToF 传感器系(+x 沿光轴、+y 左、+z 上)。SITE_TO_CV2(head.rs:16)与 SENSOR_IN_CV2_Q(head.rs:20)就是这两条翻译规则,数值来自原型运行时对真实图像的调试。注释还指出它们跨机器人版本不变:各版本 site 四元数只差一个绝对系选择,q_site⁻¹ * q_cv2 结果相同。
HeadFk(head.rs:27)构造时把名字解析成索引(HEAD_JOINTS,head.rs:23:neck_pitch/head_pitch/head_yaw/head_roll),此后每帧 FK 不做任何字符串工作。camera_in_trunk_cv2(head.rs:61)给相机在躯干系的位姿;tof_in_trunk(head.rs:74)优先锚在 MJCF 自己的 tof site——真实安装点,离相机几厘米——资产没有该 site 时退回借用相机位姿。site_in_trunk(head.rs:89)开一个 [0.0f64; 32] 角度数组,只填头部四关节、其余取零:头链挂在躯干上,腿动不了"躯干系里的头"。
look_at:2×2 的阻尼高斯-牛顿
凝视 IK(head.rs:118)只解两个关节:head_pitch 与 head_yaw;neck_pitch 是调用者选的姿态,head_roll 保持水平。为什么不逐轴迭代?因为两个关节真耦合——pitch 在 yaw 上游,pitch 会倾斜 yaw 所扫的平面;yaw 接近 ±90° 时 pitch 轴与相机前向对齐,完全失去高度控制权(head.rs:104-111)。
于是正经解 2×2 系统:对真实 FK 做有限差分雅可比(步长 STEP_H = 1e-5,head.rs:121),手写求解 (JᵀJ + λI)Δ = -Jᵀe(head.rs:167-181),Levenberg 阻尼 LAMBDA = 1e-3 用于穿过奇异性。FK 约 50 ns,整个求解几微秒——这个量级的问题不值得引迭代库。
三个工程决定:
- 每步都夹到 MJCF 行程限位(head.rs:127-132)。舵机机械上就遵守这些限位,不夹的答案是一个机器人摆不出的姿势。
Gaze::clamped(head.rs:195)说真话:结果仍偏离目标时如实报告,不追究是行程限位还是万向节几何——这是调用者的"机器人已经尽力看了"。测试里正后方的目标(需 180°,MJCF 只给 ±170°)必须钉在限位上并报告 clamped(head.rs:300-313)。- 符号钉测试(head.rs:238):+head_yaw 向左看、+head_pitch 向下看。MJCF 更新若弄反方向,失败的是这条测试,而不是点头点反的机器人。
你带走的收获
- 坐标系转换可浓缩成常量四元数,前提是有人对真实数据调过一次、并用测试钉住。
- 小规模 IK 手写阻尼高斯-牛顿 + 数值雅可比即可,几微秒级问题不引库。
- IK 结果必须夹行程限位,并把"没对准"作为返回值的一部分说出口。
- 名字在构造时解析成索引,是热路径消灭字符串工作的通用做法。