第四章：雅可比矩阵与奇异性——速度雅可比、力雅可比、奇异位形分析与规避

各位同学，今天我们来聊聊机器人学里一个绕不开的话题——雅可比矩阵与奇异性。

说实话，我刚入行那会儿，觉得雅可比就是个数学工具，算算速度映射就完事了。直到有一次在产线上调试六轴机器人，程序跑着跑着，机械臂突然在某个位姿下“僵住”了，末端执行器怎么动都动不了，电机却在那嗡嗡响。我当时冷汗就下来了。后来一查，就是碰到了奇异位形。从那以后，我对雅可比和奇异性就再也不敢马虎了。

4.1 速度雅可比：关节速度与末端速度的桥梁

我们先从最直观的速度雅可比说起。

你想想看，机器人每个关节都在转动或移动，这些关节速度怎么映射到末端执行器的速度？说白了，雅可比矩阵就是干这个的。

定义很简单：

v = J(q) * q_dot

其中 v 是末端速度（包含线速度和角速度），q_dot 是关节速度，J(q) 就是雅可比矩阵。

我个人习惯把雅可比理解成一个“变速器”。不同的关节位姿 q，这个变速器的传动比会变。比如在某个位姿下，关节转1度，末端可能移动10毫米；换个位姿，同样转1度，末端可能只动1毫米。

计算雅可比的方法有很多，最常用的是矢量积法。对于旋转关节 i，它对末端线速度和角速度的贡献分别是：

J_vi = z_i × (p_n - p_i)
J_ωi = z_i

这里 z_i 是关节 i 的旋转轴方向，p_i 是关节 i 的位置，p_n 是末端位置。

我在项目中遇到过一个问题：用数值法算雅可比时，如果步长选得太大，结果会严重偏离真实值。我建议用解析法，或者至少用中心差分法，步长取 1e-6 左右比较靠谱。

4.2 力雅可比：力的对偶关系

讲完速度，咱们聊聊力。这里有个非常漂亮的对偶关系。

你想想，能量守恒嘛。关节做的功等于末端做的功：

τ^T * q_dot = F^T * v

把 v = J * q_dot 代入，就得到：

τ = J^T * F

这就是力雅可比。关节力矩 τ 等于雅可比转置乘以末端力 F。

嗯，这里要注意：力雅可比是速度雅可比的转置，不是逆矩阵。很多初学者会搞混。

我记得有一次做力控项目，需要在机器人末端施加一个恒定的力。我直接用 J^T 把期望力映射到关节力矩，结果发现实际力总是偏小。后来排查了半天，发现是忽略了重力补偿和摩擦力。所以啊，力雅可比只是运动学层面的映射，动力学补偿还得另外算。

小技巧： 在做力控时，我习惯先做重力补偿，再用 J^T 映射期望力。这样能避免大部分偏差。

4.3 奇异位形：机器人的“死穴”

奇异位形，说白了就是雅可比矩阵秩不足的位形。这时候，机器人在某些方向上会失去自由度。

为什么会这样？因为雅可比矩阵的列向量线性相关了。比如两个关节的旋转轴平行了，它们对末端速度的贡献就变得一样，相当于少了一个自由度。

奇异位形分几种：

  • 边界奇异： 机器人手臂完全伸直或完全折叠时。比如六轴机器人的肘关节伸直，末端在径向方向上就动不了。
  • 内部奇异： 两个或多个关节轴共线。比如腕关节的轴4和轴6重合时。
  • 腕部奇异： 腕关节的三个轴共面时，末端无法绕某个方向旋转。

我曾经在调试一个焊接机器人时，程序里有个路径刚好经过腕部奇异点。结果机器人一到那个位姿，腕关节就疯狂旋转，差点把焊枪甩出去。从那以后，我写轨迹规划代码时，一定会加奇异检测。

警告： 在奇异位形附近，关节速度会趋于无穷大。即使你规划的末端速度很小，关节电机也可能因为速度超限而报警或损坏。

4.4 奇异性的数学检测

怎么检测奇异性？最直接的方法是看雅可比矩阵的行列式。

对于六轴机器人，雅可比是 6×6 的方阵。当 det(J) = 0 时，就是奇异位形。但实际中，我们更关心的是接近奇异的情况，因为这时候数值稳定性已经变差了。

我一般用可操作度（manipulability）来衡量：

w = sqrt(det(J * J^T))

当 w 接近 0 时，就说明接近奇异了。我会设一个阈值，比如 w < 0.01 时就触发规避策略。

代码实现很简单：

def check_singularity(J, threshold=0.01):
    w = np.sqrt(np.linalg.det(J @ J.T))
    if w < threshold:
        return True, w
    return False, w

嗯，这里要注意：对于非方阵的雅可比（比如七轴机器人），要用 J*J^T 的行列式，而不是直接算 det(J)。

4.5 奇异规避策略

检测到奇异后怎么办？我有几种常用的方法：

  1. 阻尼最小二乘法（DLS）： 在求逆雅可比时加一个阻尼项。公式是 J^T * (J*J^T + λ^2*I)^{-1}。λ 是阻尼系数，我一般取 0.01 到 0.1 之间。
  2. 路径重规划： 如果检测到即将进入奇异区域，就修改路径，绕过去。比如在奇异点附近加一个过渡点。
  3. 关节限位调整： 有时候奇异是因为关节角度限制导致的。适当调整关节限位，可以避免边界奇异。

我个人最常用的是 DLS 方法。它实现简单，而且效果不错。但要注意，阻尼太大会导致轨迹偏差，太小又起不到作用。我一般会根据可操作度动态调整 λ：

if w < threshold:
    lambda_damp = lambda_max * (1 - w/threshold)
else:
    lambda_damp = 0

这样在远离奇异时不影响精度，接近奇异时自动增加阻尼。

核心要点： 奇异规避不是完全避免奇异点，而是让机器人在奇异区域附近仍然能稳定运行。完全避免往往不现实，因为有些路径必须经过奇异区域。

4.6 实战中的注意事项

最后，分享几个我在项目中踩过的坑：

  • 数值精度问题： 雅可比矩阵的元素可能相差几个数量级。我建议在做奇异检测前，先对雅可比进行归一化处理。
  • 实时性要求： 在实时控制系统中，雅可比的计算和奇异检测必须在控制周期内完成。我一般用 C++ 实现，并且提前把运动学公式展开，避免循环计算。
  • 多解问题： 同一个末端位姿可能对应多个关节构型。有些构型是奇异的，有些不是。我建议在离线规划时就选好非奇异的构型。

我记得有一次，一个学生问我：“老师，为什么我的机器人一到某个位姿就抖动？”我让他打印出可操作度，发现那个位姿的 w 值只有 0.0001。这就是典型的奇异问题。后来他用了 DLS 方法，问题就解决了。

好了，关于雅可比矩阵和奇异性，今天就讲到这里。下一章我们会讲轨迹插补算法，到时候会用到今天讲的雅可比知识。大家回去可以写个小程序，算算自己机器人的雅可比矩阵，看看哪些位姿是奇异的。

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