4、逆运动学求解：解析法与数值法，多解选择与奇异性分析

逆运动学，说白了就是「给出手的位置，算出关节的角度」。

正运动学是确定的——你给一组关节角，末端位姿就唯一确定了。但逆运动学不一样，它是个多解问题。同一个末端位置，可能有几种不同的关节姿态都能到达。甚至有些位置，机器人根本到不了。

我刚开始做机器人控制时，觉得逆运动学不就是解方程嘛。直到第一次在产线上调试，机器人突然「抽风」一样地乱转……嗯，那次就是奇异性搞的鬼。从那以后，我对逆运动学就多了一份敬畏。

4.1 解析法：直接求角，快且准

解析法，也叫封闭解法。就是通过几何关系或代数变换，直接写出关节角的表达式。

它的优点是快——计算量小，适合实时控制。缺点是通用性差——每个机器人结构不同，公式都得重新推导。

4.1.1 几何法：画图求解

对于简单的机器人，比如两连杆平面臂，直接用余弦定理就能搞定。

// 两连杆平面臂逆解（几何法）
// 已知：末端位置 (x, y)，杆长 L1, L2
// 求：关节角 θ1, θ2

double cos_theta2 = (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2);
double theta2 = acos(cos_theta2);  // 肘部向上或向下

double theta1 = atan2(y, x) - atan2(L2*sin(theta2), L1 + L2*cos(theta2));

这里有个坑：acos 的输入必须在 [-1, 1] 之间。如果末端位置超出了工作空间，cos_theta2 会超出这个范围，那就无解了。

注意： 几何法推导时，一定要考虑「肘上」和「肘下」两种构型。这是多解的根源之一。

4.1.2 代数法：用矩阵消元

对于六轴工业机器人，常用的是 Pieper 解法。它要求机器人的后三个关节轴线交于一点（腕部）。

步骤大致是：

  1. 从末端位姿矩阵中分离出腕部位置
  2. 用前三个关节求解腕部位置（三个方程，三个未知数）
  3. 再用后三个关节求解腕部姿态

我在项目中遇到过一台老式六轴机器人，它的腕部不是球腕。那解析法就不好使了，只能上数值法。

4.2 数值法：迭代逼近，通用但慢

数值法不直接求公式，而是通过迭代逐步逼近解。最常用的是牛顿-拉夫森法。

它的核心思想是：

  1. 猜一组初始关节角
  2. 计算当前末端位姿与目标位姿的误差
  3. 用雅可比矩阵的逆，把误差映射回关节空间
  4. 更新关节角，重复直到误差足够小
// 牛顿-拉夫森法逆解（伪代码）
q = q0;  // 初始猜测
while (norm_error > tolerance) {
    T_current = forward_kinematics(q);
    error = target_pose - T_current;  // 位姿误差
    J = jacobian(q);                  // 计算雅可比矩阵
    dq = pinv(J) * error;            // 用伪逆求解关节增量
    q = q + dq;                       // 更新关节角
}
经验之谈： 初始猜测很重要。如果猜得太离谱，迭代可能发散。我一般会先做个粗略的几何估计，再用数值法精调。

4.2.1 解析法 vs 数值法

对比项 解析法 数值法
计算速度 快（微秒级） 慢（毫秒级，取决于迭代次数）
通用性 差（每台机器人单独推导） 好（只要正运动学已知即可）
精度 理论精确（浮点误差除外） 取决于迭代终止条件
多解处理 可直接枚举所有解 只能得到一个解（依赖初值）
奇异性 公式中可提前识别 雅可比矩阵奇异时失效

我个人习惯是：能解析就解析，解析不了再上数值。如果两者都能用，解析法做初值，数值法做精调，效果最好。

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

逆运动学有多个解，但机器人只能选一个。怎么选？

常见的策略有：

  • 最短路径法： 选择离当前关节角最近的解。这样机器人运动最平滑。
  • 避障法： 排除会导致碰撞的解。
  • 关节限位法： 排除超出关节物理限位的解。
  • 构型偏好法： 比如优先选「肘下」而不是「肘上」。

实际项目中的做法：

我曾经在焊接机器人项目里，遇到过选错解导致焊枪撞工件的事故。后来我加了一个规则：先排除所有碰撞解，再在剩余解里选最短路径。从那以后，再也没出过问题。

4.4 奇异性分析：机器人的「死穴」

奇异性，就是机器人失去某个方向运动能力的位形。说白了，你让机器人往某个方向动，它动不了，或者关节速度会变得极大。

4.4.1 奇异性的本质

从数学上看，奇异性就是雅可比矩阵 J 的秩降低，行列式为零。此时 J 不可逆，数值法会崩溃。

常见的奇异位形有：

  • 边界奇异： 手臂完全伸直或完全折叠
  • 腕部奇异： 腕部两个关节轴线共线
  • 肩部奇异： 肩关节与腕关节中心重合

4.4.2 如何检测奇异性

最简单的方法：计算雅可比矩阵的行列式。如果值接近零，就说明接近奇异。

// 奇异性检测
double det = determinant(J);
if (fabs(det) < singularity_threshold) {
    // 进入奇异区域，切换策略
    // 比如：用阻尼最小二乘法代替伪逆
    dq = J_transpose * inv(J * J_transpose + lambda * I) * error;
}
注意： 阈值不能设得太小，否则数值不稳定。也不能设得太大，否则正常运动也会被误判为奇异。我一般取 0.01 到 0.001 之间，具体要看机器人的尺寸。

4.4.3 奇异回避策略

遇到奇异性，硬闯是不行的。常用的方法有：

  • 阻尼最小二乘法（DLS）： 在雅可比伪逆中加入阻尼项，牺牲精度换取稳定性
  • 奇异分离法： 把运动分解为奇异方向和非奇异方向，只处理非奇异方向
  • 路径重规划： 绕开奇异区域

我记得有一次调试喷涂机器人，轨迹刚好经过一个奇异点。机器人每到那个位置就剧烈抖动。后来我用 DLS 方法，把阻尼系数设成 0.1，抖动就消失了。虽然末端轨迹有了一点偏差，但喷涂效果完全能接受。

4.5 本章知识体系

下面这张图，把逆运动学的核心脉络梳理了一遍。你可以把它当作一个快速参考。

逆运动学求解知识体系 逆运动学求解 解析法（封闭解） 数值法（迭代解） 几何法（画图） 代数法（Pieper） 牛顿-拉夫森 阻尼最小二乘 多解选择 & 奇异性分析 多解选择策略 奇异性检测与回避 核心原则：能解析就解析，解析不了上数值；多解选最近，奇异用阻尼

逆运动学这块，说难不难，说简单也不简单。关键是要理解它的本质——从末端空间映射回关节空间，而这个映射不是一对一的。

多解和奇异性，是实际工程中最容易出问题的地方。我建议你在仿真环境里多跑跑，把各种奇异位形都试一遍。等真到了产线上，心里就有底了。

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