4. 逆运动学求解：解析法与数值法对比、Pieper准则、多解选择策略

逆运动学，说白了就是“反着算”。正运动学给你关节角度，你算出手爪在哪儿。逆运动学反过来——手爪要放到那个位置，关节该转多少度？

这个问题，比正运动学麻烦得多。我刚开始做机器人时，觉得不就是解方程嘛，有什么难的？结果第一次碰到六轴臂，方程列出来一看，好家伙，非线性、多解、奇异点全来了。嗯，这一章咱们就把这事儿掰扯清楚。

4.1 解析法 vs 数值法：两条路，各有各的坑

求解逆运动学，主流就两条路：解析法和数值法。我个人的习惯是，能解析就解析，实在不行再上数值。为什么？往下看。

对比项	解析法	数值法
求解速度	极快，微秒级	较慢，毫秒级甚至更慢
精度	理论精确解	依赖迭代次数和收敛阈值
多解处理	可显式枚举所有解	只能得到一个解，依赖初值
适用场景	满足Pieper准则的机器人	任意构型，包括冗余机器人
实现难度	需要推导封闭解，数学要求高	通用算法，但需处理奇异和收敛

4.2 Pieper准则：什么时候能用解析法？

Pieper准则是个好东西。它告诉你：如果你的机器人满足下面两个条件之一，逆运动学就有封闭解。

• 条件A：三个相邻关节轴交于一点（比如六轴臂的后三轴）
• 条件B：三个相邻关节轴相互平行

为什么？因为交于一点时，位置和姿态可以解耦。先算手腕位置，再算手腕姿态。我在项目里遇到过一台老式六轴臂，后三轴不交于一点，结果只能上数值法，调试时那个头疼啊……

4.3 多解选择策略：哪个解才是对的？

逆运动学通常有多个解。比如一个六轴臂，同一个末端位姿，可能有8组关节角。你选哪个？

我一般按这个优先级来选：

1. 关节限幅优先：先排除超出物理限位的解
2. 行程最短：选离当前关节角最近的那组解
3. 避障需求：如果附近有障碍物，选不碰撞的那组
4. 奇异回避：避开接近奇异位形的解

4.4 知识结构图：逆运动学求解全景

下面这张图，把逆运动学求解的整个流程串起来了。你写代码前，先把这个图理清楚。

4.5 实战代码：解析法求解六轴臂逆解

下面这段代码，是我在STM32F407上跑过的。它实现了六轴臂的逆运动学解析求解，后三轴交于一点。

// 逆运动学解析求解函数
// 输入：末端位姿矩阵 T (4x4)
// 输出：最多8组关节角，存放在 solutions 数组中
// 返回值：有效解的数量
int IK_Solve(float T[4][4], float solutions[8][6]) {
    float px = T[0][3];
    float py = T[1][3];
    float pz = T[2][3];
    
    // 1. 求解 θ1
    float theta1[2];
    theta1[0] = atan2f(py, px);
    theta1[1] = atan2f(-py, -px);
    
    int sol_count = 0;
    
    for (int i = 0; i < 2; i++) {
        // 2. 根据 θ1 求解 θ3
        float r = sqrtf(px*px + py*py);
        float s = pz - d1;
        float D = (r*r + s*s - a2*a2 - a3*a3) / (2*a2*a3);
        
        if (fabsf(D) > 1.0f) continue;  // 超出可达范围
        
        float theta3[2];
        theta3[0] = atan2f(sqrtf(1-D*D), D);
        theta3[1] = atan2f(-sqrtf(1-D*D), D);
        
        for (int j = 0; j < 2; j++) {
            // 3. 求解 θ2
            float c3 = cosf(theta3[j]);
            float s3 = sinf(theta3[j]);
            float theta2 = atan2f(-s, r) - atan2f(a3*s3, a2 + a3*c3);
            
            // 4. 从旋转矩阵提取 θ4, θ5, θ6
            // ... (篇幅原因，省略具体推导)
            
            // 保存解
            solutions[sol_count][0] = theta1[i];
            solutions[sol_count][1] = theta2;
            solutions[sol_count][2] = theta3[j];
            // solutions[sol_count][3..5] = theta4, theta5, theta6
            sol_count++;
            
            if (sol_count >= 8) return sol_count;
        }
    }
    
    return sol_count;
}

4.6 数值法：当解析法走不通时

如果机器人不满足Pieper准则，或者你懒得推公式，那就上数值法。最常用的是牛顿-拉夫森迭代法。

核心思路很简单：

1. 猜一组关节角
2. 算正运动学，得到当前位姿
3. 算误差：期望位姿 - 当前位姿
4. 用雅可比矩阵的逆，修正关节角
5. 重复直到误差足够小

4.7 多解选择：实战中的决策树

最后，我整理了一个多解选择的决策树。你在STM32上实现时，可以直接套用这个逻辑。

步骤	操作	说明
1	枚举所有解析解	最多8组
2	剔除超限解	检查每个关节是否在物理限位内
3	计算关节空间距离	与当前关节角的欧氏距离
4	选择距离最小的解	保证运动连续性
5	检查奇异	如果接近奇异，换次优解

嗯，这一章的内容就到这儿。逆运动学是机器人控制的“硬骨头”，但啃下来之后，你会发现后面的轨迹规划、速度控制都顺了。代码写起来，心里也有底了。