4、运动学基础（二）：正运动学建模与逆运动学求解

好，咱们接着聊运动学。上一章我把坐标变换和旋转矩阵的底子给你打好了，这一章咱们直接上硬菜——正运动学建模和逆运动学求解。说白了，就是回答两个问题：“我的腿现在到底在哪儿？” 和 “我想让脚踩到那个点，关节得转多少度？”

这两个问题搞不定，人形机器人别说走路了，站都站不稳。我当年刚入行时，在仿真里把一条腿拧成了麻花，就是因为正运动学算错了符号。嗯，咱们今天就把这个坑填平。

4.1 正运动学建模：D-H 参数法

正运动学，就是已知关节角度，求末端位姿。对于人形机器人的腿部，我们通常把每条腿看作一个开链机构，从髋关节到脚底。

我个人习惯用 改进型 D-H 参数法（也叫 Craig 版本）。为啥？因为它的坐标系定义更直观，尤其适合树形结构的人形机器人。

4.1.1 D-H 参数的四个核心量

每个关节连杆，只需要四个参数就能唯一确定坐标系变换关系：

4.1.2 人形机器人腿部 D-H 建模实例

以常见的 6-DOF 腿部为例（髋关节 3 个 + 膝关节 1 个 + 踝关节 2 个），我习惯把坐标系这样安排：

• 基坐标系 {0}：骨盆中心，Z 轴向上
• 髋关节 {1}~{3}：分别对应髋关节的偏航、俯仰、翻滚
• 膝关节 {4}：只有一个俯仰自由度
• 踝关节 {5}~{6}：俯仰 + 翻滚
• 末端坐标系 {7}：脚底中心

对应的 D-H 参数表大概长这样（以左腿为例）：

有了这张表，你只需要把每个连杆的变换矩阵 ^i-1T_i 连乘起来，就能得到脚底相对于骨盆的位姿了。代码实现也很直接：

// 改进型 D-H 变换矩阵
Eigen::Matrix4f dh_transform(float a, float alpha, float d, float theta) {
    Eigen::Matrix4f T;
    T <<  cos(theta),       -sin(theta),       0,          a,
            sin(theta)*cos(alpha),  cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha),
            sin(theta)*sin(alpha),  cos(theta)*sin(alpha),  cos(alpha),  d*cos(alpha),
            0, 0, 0, 1;
    return T;
}

// 正运动学：6个关节角 -> 脚底位姿
Eigen::Matrix4f forward_kinematics(const float q[6]) {
    Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
    // 依次乘上每个关节的变换
    T = T * dh_transform(0, 0, L_hip_width, q[0]);
    T = T * dh_transform(0, -M_PI/2, 0, q[1] - M_PI/2);
    // ... 后续关节
    return T;
}

4.2 逆运动学求解：解析法与数值法

正运动学是“给角度求位置”，逆运动学反过来——“给脚底目标位姿，求六个关节角”。这才是真正头疼的地方。

为什么头疼？因为逆运动学通常有多解、无解、甚至奇异解。你想想看，一个点在空中，我的腿可以弯着膝盖够到，也可以伸直了够到，到底选哪个？

4.2.1 解析法（几何法）

对于人形机器人腿部这种 6-DOF 且满足 Pieper 准则（相邻三个关节轴交于一点）的结构，解析法是首选。速度快，精度高，适合实时控制。

我一般把腿部逆解拆成两步：

1. 位置解耦：先根据脚底位置，解出髋关节和膝关节的角度（决定脚在哪儿）
2. 姿态解耦：再根据脚底姿态，解出踝关节的角度（决定脚掌朝向）

以膝关节为例，已知大腿长度 L₁、小腿长度 L₂，以及髋关节到脚底的直线距离 d，膝关节角度 θ_knee 可以用余弦定理直接算：

// 几何法求膝关节角度
float solve_knee_angle(float d) {
    // 余弦定理：d^2 = L1^2 + L2^2 - 2*L1*L2*cos(pi - theta_knee)
    float cos_theta = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
    // 防止数值误差导致 acos 越界
    cos_theta = std::clamp(cos_theta, -1.0f, 1.0f);
    return M_PI - acos(cos_theta);
}

4.2.2 数值法（迭代法）

如果机器人腿部的结构不满足 Pieper 准则（比如髋关节有特殊偏置），或者你需要考虑关节限位、避障等约束，那就得上数值法了。

最常用的是 雅可比矩阵的伪逆法（也叫 DLS，阻尼最小二乘法）。核心思路是：

• 先给一组初始关节角
• 计算当前末端位姿与目标位姿的误差
• 用雅可比矩阵的伪逆，把误差映射回关节空间
• 迭代直到误差收敛

代码骨架大概这样：

// 数值法逆运动学（伪逆法）
bool inverse_kinematics_numerical(
    const Eigen::Matrix4f& T_target,
    float q_init[6], float q_out[6]) {
    
    float q[6] = {q_init[0], q_init[1], ...};
    for (int iter = 0; iter < 100; iter++) {
        // 1. 正运动学求当前位姿
        Eigen::Matrix4f T_curr = forward_kinematics(q);
        
        // 2. 计算位姿误差（位置 + 姿态）
        Eigen::VectorXf error = compute_error(T_target, T_curr);
        if (error.norm() < 1e-6) break;
        
        // 3. 计算雅可比矩阵
        Eigen::MatrixXf J = compute_jacobian(q);
        
        // 4. 伪逆迭代：dq = J^+ * error
        Eigen::MatrixXf J_pinv = J.completeOrthogonalDecomposition().pseudoInverse();
        Eigen::VectorXf dq = J_pinv * error;
        
        // 5. 更新关节角
        for (int j = 0; j < 6; j++) q[j] += dq[j];
    }
    // 输出结果
    memcpy(q_out, q, 6 * sizeof(float));
    return true;
}

4.3 人形机器人腿部运动学特点

跟工业机械臂不同，人形机器人的腿部运动学有几个特别的地方，我单独拎出来说说。

4.3.1 闭链约束

人形机器人走路时，两条腿和地面构成了一个 闭链。一条腿摆动时，另一条腿必须支撑。这意味着你不能独立解每条腿的逆运动学，必须考虑躯干的位姿变化。

我的做法是：先固定支撑腿的脚底，解出躯干的运动轨迹，再反过来解摆动腿的关节角。这叫 “支撑腿优先” 策略。

4.3.2 奇异位形规避

人形机器人最怕两种奇异：

• 膝关节打直：腿完全伸直时，膝关节失去一个自由度，踝关节很难补偿
• 髋关节内收极限：两条腿交叉时，髋关节容易卡住

我一般在逆运动学求解后，加一个 关节限位检查 和 可操作度（manipulability）计算。如果可操作度低于阈值，就调整步态参数，避免进入奇异区域。

4.3.3 足底姿态约束

人形机器人的脚底必须 始终与地面平行或按规划角度接触。你不能让脚掌在空中乱转。所以逆运动学求解时，脚底的翻滚角和俯仰角通常是固定的（或者按步态规划变化）。

这其实降低了逆运动学的难度——6 个自由度里，脚底姿态占了 3 个，剩下 3 个位置自由度刚好对应髋关节和膝关节的 3 个主要驱动关节。

4.4 本章小结

正运动学是基础，D-H 参数法是标准工具，建好模型后一劳永逸。逆运动学是实战，解析法快但适用范围窄，数值法灵活但要注意奇异和收敛问题。

我个人建议：能用解析法就用解析法，实在搞不定再上数值法。毕竟在机器人控制里，实时性就是生命线。下一章咱们会把这些运动学知识用到实际的步态规划里去，到时候你就知道今天这些推导有多重要了。