3. 运动学基础
人形机器人的运动控制与平衡问题，本质上是在回答“关节如何运动才能让身体到达期望位姿”这一核心问题。运动学作为连接关节空间与操作空间的桥梁，是动力学分析、轨迹规划与力控制的前提。本章聚焦于刚体变换的数学表达、正运动学的链式传递法则，以及逆运动学中解析法与数值法的工程取舍，并结合人形机器人特有的多分支链、闭链约束与奇异位形进行深入讨论。
3.1 刚体变换：从旋转矩阵到齐次坐标
人形机器人由多个刚性连杆通过关节连接而成，描述各连杆之间的相对位姿关系是运动学分析的第一步。刚体变换包含旋转与平移两个分量，其中旋转矩阵 R ∈ SO(3) 描述姿态，平移向量 t ∈ R³ 描述位置。将两者组合为 4×4 齐次变换矩阵 T，即可统一表达坐标系之间的映射关系。
旋转矩阵的构造方式包括绕固定轴旋转（如 R = Rz(θ)·Ry(φ)·Rx(ψ)）和绕自身轴旋转（欧拉角 ZYX 或 ZYZ 序列）。在实际工程中，人形机器人常采用 ZYX 欧拉角表示躯干姿态，因为其物理意义直观，便于与 IMU 传感器数据对齐。但欧拉角存在万向锁问题，因此在内部计算中应使用旋转矩阵或四元数，仅在用户接口层转换为欧拉角。
齐次变换矩阵的链式乘法规则是正运动学的基础：若已知相邻关节坐标系间的变换 Tii-1，则末端坐标系相对于基座坐标系的位姿为 Tn0 = T10·T21·…·Tnn-1。这一过程在计算上仅涉及矩阵乘法，计算量小，适合实时控制循环（通常 1kHz 以上）。
| 表示方法 | 维度 | 优点 | 缺点 | 人形机器人应用场景 |
|---|---|---|---|---|
| 旋转矩阵 R | 3×3 | 无奇异，组合方便 | 9个参数冗余，需正交化 | 内部计算、坐标变换 |
| 欧拉角 (ZYX) | 3 | 直观，便于调试 | 万向锁，插值非线性 | 用户接口、传感器读数 |
| 四元数 | 4 | 无奇异，插值平滑 | 物理意义不直观 | 姿态插值、滤波融合 |
| 齐次矩阵 T | 4×4 | 统一表达位姿，链式乘法 | 16个参数，存储稍大 | 运动学正解、坐标变换 |
3.2 正运动学：从关节空间到操作空间
正运动学（Forward Kinematics, FK）解决“给定关节角度，求末端位姿”的问题。对于人形机器人，需要分别建立左腿、右腿、左臂、右臂、头部等运动链的正运动学模型。每条链的基座坐标系通常固定在骨盆中心或躯干质心，末端坐标系则位于脚掌、手掌或头顶。
建立正运动学模型的标准流程是：首先为每个关节分配坐标系（通常采用 Denavit-Hartenberg 参数法或改进的 DH 法），然后确定连杆参数（ai, αi, di, θi），最后逐级计算变换矩阵并相乘。对于人形机器人，由于腿部结构近似于串联机械臂，DH 参数法可以直接套用；但需要注意腿部存在闭链约束（双脚同时着地时），此时正运动学需要结合支撑脚的位置进行修正。
以右腿为例，典型的 6-DOF 腿部结构包含髋关节 3 个自由度（横滚、俯仰、偏航）、膝关节 1 个自由度（俯仰）、踝关节 2 个自由度（俯仰、横滚）。其正运动学可表示为：
T_foot^pelvis = T_hip^pelvis · T_knee^hip · T_ankle^knee · T_foot^ankle其中每个 T 矩阵由 DH 参数代入标准公式计算。实际编程中，建议将每个关节的变换矩阵封装为函数，输入关节角 θ，输出 4×4 矩阵，便于复用和调试。
- 浮动基座：人形机器人没有固定基座，骨盆相对于世界坐标系的位姿由支撑腿的接触状态决定。因此，全身正运动学需要先确定浮动基座位姿，再计算各分支链的末端位姿。
- 闭链处理：双脚支撑时，两条腿构成闭链，正运动学结果必须满足脚掌与地面的接触约束。此时需要引入约束方程，通过数值方法求解一致解。
- 冗余分支：手臂通常为 7-DOF（多一个冗余自由度），正运动学本身唯一，但逆运动学存在无穷多解，需要额外优化准则。
3.3 逆运动学：解析法与数值法的工程对决
逆运动学（Inverse Kinematics, IK）解决“给定末端期望位姿，求关节角度”的问题。这是运动规划与控制中最具挑战性的环节，因为解的存在性、唯一性和数值稳定性都受到机构结构、关节限位和奇异位形的影响。
3.4 解析法：追求封闭解
解析法通过代数或几何方法直接求解关节角，计算速度快、精度高，适合实时性要求极高的平衡控制。其适用前提是运动链满足 Pieper 准则：相邻三个关节轴交于一点，或三个相邻关节轴平行。人形机器人的腿部结构通常满足该条件（髋关节三轴交于一点），因此腿部 IK 可以解析求解。
以右腿为例，解析求解步骤为：
- 根据脚掌期望位姿 Tfootpelvis，分解出位置向量 p 和旋转矩阵 R。
- 利用踝关节位置与期望位置的关系，求解膝关节角 θknee（余弦定理）。
- 根据髋关节到踝关节的向量方向，求解髋关节横滚角 θhip_roll 和俯仰角 θhip_pitch。
- 由旋转矩阵残差求解踝关节两个角度。
解析法的多解问题需要结合关节限位和运动连续性进行筛选。例如，膝关节通常只能向一个方向弯曲（人体膝关节不能过伸），这直接排除了一半的解。
3.5 数值法：通用但需谨慎
数值法（迭代法）适用于任意结构，包括手臂的 7-DOF 冗余链和全身运动规划。最常用的方法包括：
- 雅可比伪逆法：利用雅可比矩阵 J 的伪逆 J⁺ 迭代更新关节角，公式为 Δθ = J⁺·Δx。该方法在奇异位形附近会出现数值不稳定，需要加入阻尼项（DLS 方法）。
- Levenberg-Marquardt 优化：将 IK 转化为最小二乘问题，通过调节阻尼因子平衡收敛速度与稳定性，适合处理带约束的 IK。
- 任务空间优先级法：将多个任务（如保持平衡、跟踪手部轨迹、避免关节限位）按优先级排序，低优先级任务在零空间内优化。
数值法的核心挑战在于初始值敏感性和局部极小问题。对于人形机器人，通常以上一控制周期的关节角作为初始值，利用运动连续性保证收敛到正确解。此外，在奇异位形附近，雅可比矩阵秩亏，需要引入奇异鲁棒性逆解（SR-Inverse）或阻尼最小二乘。
| 对比维度 | 解析法 | 数值法（迭代） |
|---|---|---|
| 计算速度 | 微秒级，适合 1kHz 控制 | 毫秒级，适合 100Hz-500Hz 规划 |
| 精度 | 浮点精度，无迭代误差 | 受迭代次数和容差影响 |
| 通用性 | 仅适用于满足 Pieper 准则的链 | 任意结构，包括冗余链 |
| 多解处理 | 显式多解，需筛选 | 收敛到单一解，依赖初始值 |
| 奇异鲁棒性 | 解析式在奇异处仍可计算 | 需特殊处理（阻尼、SVD） |
| 实现复杂度 | 需推导公式，调试较难 | 通用框架，易于实现 |
3.6 案例拆解：双足支撑下的腿部 IK 混合求解策略
某双足人形机器人（身高 1.2m，腿长 0.55m）在行走过程中需要实现平滑的步态切换。控制周期为 1ms，要求腿部 IK 计算耗时不超过 0.2ms。初始方案采用纯数值法（LM 算法），实测平均耗时 0.8ms，且在地面反作用力突变时出现迭代发散。经分析，发散原因是踝关节接近奇异位形（膝关节接近完全伸直，θknee ≈ 5°），雅可比矩阵条件数超过 2000。
改进方案采用 解析法 + 数值法混合：
- 在单脚支撑期，使用解析法求解支撑腿 IK，耗时约 0.05ms，精度达 1e-12。
- 在双脚支撑期，先解析求解骨盆位姿（基于两脚位置约束），再对每条腿分别解析求解，最后用一步 LM 迭代修正闭链残差。
- 对于手臂操作任务（7-DOF），采用任务优先级数值法，以腿部解析解为基准，在零空间内优化手臂姿态。
实测结果：混合方案平均耗时 0.12ms，最大耗时 0.18ms，满足实时性要求。在膝关节接近奇异时，解析法仍能给出有效解（虽然精度略有下降），避免了数值发散。同时，通过监控雅可比矩阵条件数，当条件数超过 500 时自动切换为阻尼最小二乘，进一步增强了鲁棒性。
该案例的复盘经验是：不要盲目追求纯解析或纯数值，而应根据任务阶段和机构特性选择混合策略。解析法提供快速可靠的基准解，数值法处理冗余和约束，两者结合才能兼顾实时性与通用性。
3.7 常见误区与总结
在运动学建模与求解过程中，工程师常犯以下错误：
- 坐标系定义不一致：不同模块（视觉、规划、控制）使用不同的基座坐标系或旋转约定，导致数据对接时出现位姿偏差。应在项目启动时统一坐标系规范，并编写坐标变换工具库。
- 忽略浮动基座影响：将人形机器人当作固定基座机械臂处理，导致全身运动学错误。必须显式建模骨盆浮动基座，并考虑支撑状态切换。
- 奇异位形处理不当：在膝关节伸直或手臂完全展开时，数值法容易发散。应提前检测奇异，并采用阻尼或降级策略。
- 多解选择缺乏准则：解析法产生多个解时，仅凭关节限位筛选可能不够，还需考虑运动连续性（与上一周期解的距离最小）和避障需求。
- 精度评估缺失：未对 IK 结果进行末端位姿误差验证。建议在每个控制周期计算正运动学残差，并设置报警阈值（如位置误差 > 1mm 或姿态误差 > 0.1°）。
总结而言，运动学基础是人形机器人控制的地基。刚体变换提供了统一的数学语言，正运动学建立了关节与末端之间的映射，而逆运动学则是连接规划与控制的关键桥梁。在实际工程中，应根据任务需求、机构特性和计算资源，灵活选择解析法、数值法或混合策略，并始终关注数值稳定性与实时性之间的平衡。