4. 机器人学基础（三）：机械臂动力学简介、雅可比矩阵、奇异性分析

好，咱们继续往下走。前两章我们把机械臂的位姿描述和运动学搞定了，说白了就是知道了“胳膊往哪伸”。但光知道位置够吗？不够。你想想看，一个机械臂要真的动起来，你得知道它需要多大的力，关节转得快不快，以及——会不会突然卡住动不了。

这一章，我们就来啃这三块硬骨头：动力学、雅可比矩阵和奇异性分析。我个人觉得，这是从“看懂”到“会用”的分水岭。

4.1 机械臂动力学简介

动力学研究的是力与运动的关系。正问题：给定关节力矩，求运动。逆问题：给定期望运动，求所需力矩。在实际控制中，我们绝大多数时候都在解逆动力学。

常用的方法有两种：

方法	原理	计算量	我的经验
牛顿-欧拉法	基于力/力矩平衡，递推计算	O(n)，较快	适合实时控制，我项目里常用
拉格朗日法	基于能量（动能-势能）	O(n⁴)，较慢	适合理论推导，写好用

我个人习惯用牛顿-欧拉法做实时控制。为什么？因为它快。你想想看，一个六轴机械臂，控制周期通常只有1毫秒，你必须在几百微秒内算完所有关节的力矩。拉格朗日法虽然物理意义清晰，但计算量太大，实时性扛不住。

动力学方程的标准形式长这样：

M(q) * q̈ + C(q, q̇) * q̇ + G(q) + F(q̇) = τ

其中：

• M(q)：惯性矩阵，跟关节位置有关
• C(q, q̇)：科里奥利力和离心力项
• G(q)：重力项
• F(q̇)：摩擦力项（库仑+粘滞）
• τ：关节驱动力矩

嗯，这里要注意：M矩阵一定是正定对称的。如果你算出来不对称，那肯定是哪里算错了。我曾经在调试一个七轴协作机器人时，发现力矩算出来总是不对，查了两天才发现是惯性矩阵里一个交叉项符号写反了……

4.2 雅可比矩阵

雅可比矩阵，说白了就是一把“转换钥匙”。它把关节空间的速度映射到操作空间（末端）的速度。

定义很简单：

ẋ = J(q) * q̇

其中：

• ẋ：末端速度（线速度 + 角速度，共6维）
• q̇：关节速度（n维）
• J(q)：6×n 的雅可比矩阵

对于六轴机械臂，J是6×6的方阵。如果是七轴冗余臂，J就是6×7，这时候就有冗余自由度了。

求雅可比矩阵的方法，我推荐微分法：对正运动学方程直接求偏导。比如你的末端位置是 p = f(q)，那么 J = ∂f/∂q。对于旋转关节，角速度部分的雅可比可以直接从旋转矩阵的导数得到。

实际写代码时，我一般会分两步走：

1. 先算位置部分的雅可比（3行）
2. 再算姿态部分的雅可比（3行）

姿态部分稍微麻烦点，因为欧拉角表示有万向锁问题。我建议用旋转向量或四元数来避免奇异性。

4.3 奇异性分析

奇异性，就是机械臂的“死穴”。当机械臂处于某个特殊位形时，雅可比矩阵的秩会下降，导致某些方向上的运动能力丧失。

为什么会这样？你想想看，当两个关节的轴线平行时，它们的运动效果就“重叠”了。这时候你虽然有两个关节，但能控制的方向反而少了一个。

常见的奇异位形有三种：

类型	描述	典型场景
边界奇异	机械臂完全伸直或完全折叠	手臂打直够东西
内部奇异	两个关节轴线共线	腕部关节对齐（如PUMA 560）
锁定奇异	某个关节无法产生末端运动	肩关节与肘关节共面

判断奇异的方法很简单：计算雅可比矩阵的行列式。如果行列式为0（或接近0），就是奇异。

// 伪代码：奇异检测
if |det(J)| < epsilon:
    // 进入奇异区域，切换控制策略
    q̇ = J⁺ * ẋ   // 使用伪逆 + 阻尼
else:
    q̇ = inv(J) * ẋ  // 正常求逆

这里的 epsilon 取多少？我一般取0.01到0.001之间，具体看机械臂的尺寸和精度要求。太小了容易漏检，太大了又容易误报。

我记得有一次做医疗机器人的力反馈控制，机械臂在某个位形下突然抖得厉害。一查日志，发现雅可比的条件数（condition number）已经超过1000了。这就是典型的“接近奇异”状态。后来我们在轨迹规划阶段就提前绕开这些区域，问题就解决了。

4.4 小结

这一章的内容，说白了就是三件事：

• 动力学告诉你“要多大力”
• 雅可比矩阵告诉你“速度怎么映射”
• 奇异性分析告诉你“哪里不能去”

这三块东西，在实际的视觉定位与机械臂协同仿真中，一个都跑不掉。你写控制代码时，动力学参数要准；你做手眼标定时，雅可比矩阵要算对；你规划轨迹时，奇异位形要避开。嗯，都是硬功夫。