3. 机器人运动学基础（下）：逆运动学求解方法与雅可比矩阵

上一节我们把正运动学讲透了——给定关节角度，算出末端位姿。但实际干活的时候，问题往往是反过来的：我知道焊枪要走到那个点，可六个关节该转多少度？这就是逆运动学要干的事。

说实话，逆运动学比正运动学麻烦得多。正运动学是唯一的，你给一组角度，末端位姿就定了。但逆运动学可能有多个解，也可能无解。我在调试六轴焊接机器人时遇到过这种情况——明明轨迹规划出来的点就在工作空间内，但逆解就是算不出来，后来发现是腕部奇异点的问题。嗯，这些坑我们一个个填。

3.1 逆运动学问题概述

逆运动学（Inverse Kinematics, IK）可以这样描述：已知机器人末端执行器在基坐标系下的位姿矩阵 T，求所有满足 T = f(q) 的关节变量 q = [q₁, q₂, ..., qₙ]ᵀ。

这里有几个关键特性你必须记住：

  • 多解性：同一个末端位姿，可能对应多组关节角度。比如肘部在上和肘部在下，就是两种构型。
  • 存在性：只有末端位姿在机器人的工作空间内，才有解。
  • 求解难度：解析法只适用于满足 Pieper 准则的机器人（三个相邻关节轴交于一点，或三个相邻关节轴平行）。

核心判断标准：如果你的机器人是六轴工业机器人，且后三个关节轴线交于一点（腕部），恭喜你，可以用解析法。否则，老老实实用数值法。

3.2 解析法求解

解析法，也叫封闭解法。它的好处是快、准、稳。在STM32上跑实时控制，解析法几乎是唯一选择——数值法迭代次数多了，CPU扛不住。

我以典型的六轴工业机器人为例，讲讲解析法的思路。假设机器人满足 Pieper 准则，我们可以把问题拆成两步：

  1. 位置解耦：先求前三个关节（肩部、大臂、小臂）的角度，确定腕部中心点的位置。
  2. 姿态解耦：再根据末端姿态和腕部中心位置，反推后三个关节（腕部）的角度。

具体到代码实现，我们以 PUMA 560 构型为例，前三个关节的求解可以用几何法：

// 逆运动学解析法示例：前三个关节求解（简化版）
// 输入：末端位姿矩阵 T，连杆参数 a2, a3, d1, d4
// 输出：关节角度 q1, q2, q3

void ik_analytic_position(float T[4][4], float *q1, float *q2, float *q3) {
    float px = T[0][3];
    float py = T[1][3];
    float pz = T[2][3];
    
    // 求 q1：两个可能解（左肩/右肩）
    float r = sqrtf(px*px + py*py);
    float phi = atan2f(py, px);
    *q1 = phi - atan2f(d4, r);  // 第一个解
    // 第二个解：*q1 = phi - atan2f(d4, -r) + PI;
    
    // 求 q3：肘部上/下两个解
    float c3 = (px*px + py*py + (pz-d1)*(pz-d1) - a2*a2 - a3*a3) / (2*a2*a3);
    *q3 = atan2f(sqrtf(1-c3*c3), c3);  // 肘部在上
    // 另一个解：*q3 = atan2f(-sqrtf(1-c3*c3), c3);  // 肘部在下
    
    // 求 q2
    float s3 = sinf(*q3);
    float t1 = a2 + a3*c3;
    float t2 = a3*s3;
    *q2 = atan2f(pz-d1, sqrtf(px*px+py*py)) - atan2f(t2, t1);
}

我的经验：解析法求出来的角度，一定要做范围检查。有些解虽然在数学上成立，但关节限位过不去。我曾经在调试时没做限位检查，结果机器人直接撞到工作台上——从那以后，我每次逆解完都会加一段限位过滤逻辑。

3.3 数值法求解

如果机器人不满足 Pieper 准则，或者你懒得推导解析公式，那就用数值法。数值法的核心思想是迭代逼近：从一个初始猜测开始，不断调整关节角度，让末端位姿误差越来越小。

最常用的数值法是 牛顿-拉夫森法 和 雅可比伪逆法。这里我重点讲后者，因为它在工程中更实用。

数值法的迭代步骤：

  1. 给定初始关节角度 q₀
  2. 计算当前末端位姿 T_curr 与目标位姿 T_target 的误差 Δx
  3. 如果 ||Δx|| 小于阈值，停止迭代
  4. 否则，计算雅可比矩阵 J(q)，求 Δq = J⁺ · Δx
  5. 更新 q = q + Δq，回到第2步
// 数值法逆运动学：雅可比伪逆法
// 输入：目标位姿 T_target，初始猜测 q_init
// 输出：关节角度 q_solution

void ik_numerical(float T_target[4][4], float q_init[6], float q_solution[6]) {
    float q[6];
    float delta_x[6];
    float delta_q[6];
    float J[6][6];      // 雅可比矩阵
    float J_pinv[6][6]; // 伪逆
    float T_curr[4][4];
    int iter = 0;
    float error;
    
    memcpy(q, q_init, 6 * sizeof(float));
    
    do {
        // 正运动学：计算当前位姿
        forward_kinematics(q, T_curr);
        
        // 计算位姿误差（位置+姿态）
        delta_x[0] = T_target[0][3] - T_curr[0][3];
        delta_x[1] = T_target[1][3] - T_curr[1][3];
        delta_x[2] = T_target[2][3] - T_curr[2][3];
        // 姿态误差用轴角表示法...
        compute_orientation_error(T_target, T_curr, &delta_x[3]);
        
        error = sqrtf(delta_x[0]*delta_x[0] + delta_x[1]*delta_x[1] + 
                      delta_x[2]*delta_x[2] + delta_x[3]*delta_x[3] +
                      delta_x[4]*delta_x[4] + delta_x[5]*delta_x[5]);
        
        if (error < 1e-6) break;
        
        // 计算雅可比矩阵
        compute_jacobian(q, J);
        
        // 计算伪逆：J_pinv = J^T * (J * J^T)^(-1)
        pseudo_inverse(J, J_pinv);
        
        // 计算关节增量
        matrix_multiply(J_pinv, delta_x, delta_q);
        
        // 更新关节角度
        for (int i = 0; i < 6; i++) {
            q[i] += delta_q[i];
        }
        
        iter++;
    } while (iter < 100);
    
    memcpy(q_solution, q, 6 * sizeof(float));
}

注意：数值法有两个致命问题。第一，初始猜测离真实解太远，迭代可能发散。第二，在奇异点附近，雅可比矩阵会降秩，伪逆计算不稳定。我曾经在调试一个七自由度冗余机器人时，数值法在奇异点附近疯狂震荡，最后加了阻尼最小二乘法（DLS）才稳住。

3.4 雅可比矩阵与速度运动学

雅可比矩阵 J(q) 是机器人学里最重要的概念之一。它把关节速度映射到末端速度：

v = J(q) · q̇

其中 v = [v_x, v_y, v_z, ω_x, ω_y, ω_z]ᵀ 是末端线速度和角速度，q̇ 是关节速度。

雅可比矩阵的每一列，对应一个关节运动对末端速度的贡献。对于六轴机器人，J 是 6×6 的矩阵。

计算雅可比矩阵有两种方法：

  • 矢量积法：对移动关节和转动关节分别用不同的公式计算。转动关节 i 对末端线速度的贡献是 zᵢ × (pₙ - pᵢ)，对角速度的贡献是 zᵢ。
  • 微分变换法：从正运动学表达式直接求偏导。适合用符号计算工具推导。

在STM32上，我推荐用矢量积法。它不需要求导，只需要正运动学过程中的中间变换矩阵，计算量可控。

// 矢量积法计算雅可比矩阵
// 输入：关节角度 q，连杆变换矩阵 T[7][4][4]（包含基座到各连杆的变换）
// 输出：雅可比矩阵 J[6][6]

void compute_jacobian(float q[6], float T[7][4][4], float J[6][6]) {
    float p_end[3];  // 末端位置
    float p_i[3];    // 第 i 个坐标系原点位置
    float z_i[3];    // 第 i 个坐标系的 z 轴方向
    
    p_end[0] = T[6][0][3];
    p_end[1] = T[6][1][3];
    p_end[2] = T[6][2][3];
    
    for (int i = 0; i < 6; i++) {
        // 提取第 i 个坐标系的原点和 z 轴
        p_i[0] = T[i][0][3];
        p_i[1] = T[i][1][3];
        p_i[2] = T[i][2][3];
        
        z_i[0] = T[i][0][2];
        z_i[1] = T[i][1][2];
        z_i[2] = T[i][2][2];
        
        // 线速度部分：z_i × (p_end - p_i)
        float dx = p_end[0] - p_i[0];
        float dy = p_end[1] - p_i[1];
        float dz = p_end[2] - p_i[2];
        
        J[0][i] = z_i[1]*dz - z_i[2]*dy;
        J[1][i] = z_i[2]*dx - z_i[0]*dz;
        J[2][i] = z_i[0]*dy - z_i[1]*dx;
        
        // 角速度部分：z_i
        J[3][i] = z_i[0];
        J[4][i] = z_i[1];
        J[5][i] = z_i[2];
    }
}

3.5 奇异点分析

奇异点，说白了就是机器人"卡住"的位置。在奇异点附近，雅可比矩阵的秩下降，某些方向上的运动能力丧失。

常见的奇异点有三种：

奇异类型 产生原因 典型表现
边界奇异 机器人完全伸展或收缩 某个方向无法继续运动
腕部奇异 关节4和关节6的轴线共线 腕部无法实现某些姿态
肩部奇异 腕部中心位于关节1轴线正上方 关节1角度突变

工程应对策略：在奇异点附近，用阻尼伪逆代替普通伪逆。公式是 J⁺ = Jᵀ(J·Jᵀ + λ²I)⁻¹，其中 λ 是阻尼系数。λ 越大，越稳定，但精度会下降。我一般取 λ = 0.01~0.1，具体值靠实验调。

3.6 本章知识体系

下面这张图把逆运动学和雅可比矩阵的核心脉络串起来了。你写代码的时候，可以对照这张图理清思路。

逆运动学与雅可比矩阵知识体系 逆运动学与雅可比矩阵知识体系 逆运动学求解 解析法（封闭解） 数值法（迭代解） 位置解耦 + 姿态解耦 适用条件：Pieper 准则 牛顿-拉夫森法 雅可比伪逆法 雅可比矩阵 J(q) 速度运动学：v = J·q̇ 奇异点分析 力域映射：τ = Jᵀ·F 奇异点类型：边界奇异 | 腕部奇异 | 肩部奇异 应对策略：阻尼伪逆 J⁺ = Jᵀ(J·Jᵀ + λ²I)⁻¹

3.7 工程选型建议

在STM32上实现逆运动学，我建议你这样选：

  • 如果机器人满足 Pieper 准则：用解析法。速度快，确定性好，适合实时控制。STM32F4 系列跑一次解析逆解大概 20~50μs。
  • 如果机器人不满足 Pieper 准则：用数值法。但要注意，每次迭代都要算雅可比矩阵和伪逆，计算量较大。STM32H7 系列可以跑到 100Hz 的控制频率，F4 系列建议降到 50Hz。
  • 如果机器人有冗余自由度：用数值法 + 零空间优化。可以在满足主任务的同时，优化关节限位、避障等次要目标。

我的建议：不管用哪种方法，一定要在代码里加超时保护。数值法迭代超过最大次数还没收敛，就报错退出，别让机器人"蒙着眼睛"乱动。我在一个协作机器人项目里吃过这个亏——数值法在奇异点附近迭代了 200 多次没收敛，关节速度指令直接飞了，机器人抖得像筛糠一样。

← 上一章 📖 返回目录 下一章 →