第4章 逆运动学

逆运动学，说白了就是：已知末端位姿，反推关节角度。你给机器人一个目标位置和姿态，它得算出每个关节转多少度才能到那儿。这问题看着简单，实际坑不少。我做了十几年运动控制，每次遇到逆解不唯一或者奇异点，都得打起十二分精神。

4.1 逆运动学问题描述

正运动学是“因→果”，逆运动学是“果→因”。给定末端执行器的位姿矩阵 T（4×4 齐次变换矩阵），求关节变量 θ₁, θ₂, …, θₙ。数学上就是解一组非线性方程：

T(θ₁, θ₂, …, θₙ) = T_desired

每个关节可能是旋转或平移，方程高度耦合。你想想看，6 个自由度，6 个未知数，但方程个数往往超过 6（因为旋转部分有约束）。我个人习惯先判断是否存在封闭解，再决定用解析法还是数值法。

4.2 解析法与数值法

解析法：通过代数或几何推导出显式公式。速度快，精度高，但只适用于特定构型。比如 Pieper 准则下的 6 轴机器人，后三个关节轴线交于一点，就能得到封闭解。

数值法：迭代逼近，比如牛顿-拉夫森、雅可比伪逆。通用性强，但可能陷入局部最优，而且实时性差。我在调试一款协作机器人时，数值法偶尔会“跳变”，吓得我赶紧加了限幅和重初始化。

4.3 可解性与多解性

逆运动学不一定有解，也不一定唯一。常见情况：

• 无解：目标位姿超出工作空间。比如你让机器人去够天花板外面的点，它胳膊没那么长。
• 唯一解：某些特殊位姿，比如腕部奇异点附近。
• 多解：肘上/肘下、腕部翻转等。6 轴机器人最多可能有 8 组解。

多解怎么选？我一般按“最短关节行程”原则，同时避开奇异和碰撞。曾经有个项目，机器人总选到“绕远路”的解，后来我加了加权距离，问题就解决了。

4.4 实例：6轴机器人逆解（Pieper准则）

Pieper 准则：如果 6 轴机器人的后三个关节（4、5、6）的轴线交于一点，那么逆解可以分解为“位置逆解”和“姿态逆解”。前三个关节决定腕部位置，后三个关节决定腕部姿态。这大大简化了计算。

具体步骤（以典型 6R 机器人为例）：

1. 从目标位姿矩阵提取腕部位置 P_w（末端位置减去工具长度沿末端 Z 轴方向）。
2. 用几何法解 θ₁, θ₂, θ₃（通常有 2~4 组解）。
3. 根据腕部姿态矩阵 R_w 解 θ₄, θ₅, θ₆（欧拉角或等效角）。
4. 组合所有解，剔除超出关节限位或碰撞的解。

4.5 代码片段：Pieper 逆解核心思路（伪代码）

function inverse_kinematics(T_desired):
    // 1. 提取腕部位置
    P_w = T_desired[0:3, 3] - d6 * T_desired[0:3, 2]
    // 2. 解前三个关节 (几何法)
    solutions_123 = solve_theta1_theta2_theta3(P_w)
    // 3. 对每组解，计算腕部姿态矩阵
    for each (θ1, θ2, θ3) in solutions_123:
        R_w = compute_wrist_rotation(θ1, θ2, θ3, T_desired)
        // 4. 解后三个关节 (ZYZ欧拉角)
        (θ4, θ5, θ6) = euler_from_rotation(R_w)
        // 5. 检查关节限位 & 碰撞
        if within_limits(θ1..θ6) and not_collision():
            add_to_solution_set(θ1..θ6)
    // 6. 按最短行程排序，返回最优解
    return best_solution_by_joint_travel()

嗯，这里要注意：d6 是腕部到末端法兰的距离，不同机器人不一样。我习惯把工具参数也考虑进去，不然标定完一运行，偏差好几毫米。

4.6 小结

逆运动学是机器人控制的“心脏”。你得理解可解性、多解性，掌握 Pieper 准则，并且在实际代码里做好异常处理。我个人觉得，解析法 + 数值法混合才是工程上最稳妥的方案。别指望一种方法打天下，灵活切换才是老手风范。