2. 运动学基础（上）：正运动学、逆运动学与雅可比矩阵

各位好，欢迎来到《机械臂导航调参避坑指南》的第二章。

运动学，说白了就是研究机械臂“怎么动”的学问。你不需要懂复杂的数学推导，但必须理解它的物理意义。我见过太多人，调了一周参数，最后发现是运动学模型建错了——那种感觉，就像你跑完马拉松才发现跑错了赛道。

这一章，我们只讲三件事：正运动学（给定关节角度，求末端位姿）、逆运动学（给定末端位姿，求关节角度）、雅可比矩阵与奇异性（速度映射与危险区域）。

2.1 正运动学：DH参数法

正运动学，就是已知每个关节的角度，求末端执行器在空间中的位置和姿态。说白了，就是“输入角度，输出位姿”。

我个人习惯用DH参数法（Denavit-Hartenberg）。这个方法的核心，是在每个关节上建立一个坐标系，然后用四个参数来描述相邻坐标系之间的变换关系。

这四个参数分别是：

参数	符号	物理含义
连杆长度	ai-1	沿xi-1轴，从zi-1到zi的距离
连杆扭转角	αi-1	绕xi-1轴，从zi-1到zi的转角
连杆偏距	di	沿zi轴，从xi-1到xi的距离
关节角	θi	绕zi轴，从xi-1到xi的转角

有了这四个参数，我们就可以写出相邻坐标系之间的齐次变换矩阵：

T = Rot_x(α) * Trans_x(a) * Rot_z(θ) * Trans_z(d)

然后，把每个关节的变换矩阵连乘起来，就得到末端相对于基座的位姿：

T_0_n = T_0_1 * T_1_2 * ... * T_{n-1}_n

2.2 逆运动学：解析解与数值解

逆运动学，就是已知末端位姿，求关节角度。这是运动学里最让人头疼的部分，没有之一。

为什么？因为正运动学是“一对一”的映射，而逆运动学是“一对多”的映射——同一个末端位姿，可能有多个关节角度组合。

求解方法分两种：解析解和数值解。

2.2.1 解析解

解析解，就是通过代数或几何方法，直接求出关节角度的闭式表达式。它的优点是快、准、稳。但缺点也很明显——不是所有机械臂都有解析解。

一般来说，满足Pieper准则的机械臂才有解析解：三个相邻关节轴交于一点，或者三个相邻关节轴平行。

常见的6轴工业机器人，比如KUKA、ABB、FANUC，它们的腕部三个关节轴交于一点，所以有解析解。

// 以典型的6轴机器人为例，逆解流程：
// 1. 根据末端位姿，求解腕部中心位置
// 2. 由腕部中心位置，求解前三个关节角（θ1, θ2, θ3）
// 3. 由末端姿态，求解后三个关节角（θ4, θ5, θ6）
// 4. 检查关节限位，选择有效解

2.2.2 数值解

数值解，就是通过迭代优化的方式，逼近逆解。常用的方法有牛顿法、梯度下降法、LM算法等。

数值解的优点是通用性强——任何机械臂都能用。但缺点也很明显：

• 慢：每次迭代都要计算正运动学和雅可比矩阵
• 不稳定：可能收敛到局部最优，甚至发散
• 依赖初值：初值选不好，结果差很远

什么时候用数值解？我个人建议：

• 机械臂没有解析解（比如7轴冗余臂）
• 需要在线避障，实时调整路径
• 作为解析解的补充（比如解析解失败时，用数值解兜底）

2.3 雅可比矩阵与奇异性

雅可比矩阵，是连接关节空间速度与笛卡尔空间速度的桥梁。

公式很简单：

v = J(q) * q_dot

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

雅可比矩阵有什么用？

• 速度控制：给定末端速度，反算关节速度
• 力控制：通过转置，将末端力映射到关节力矩
• 奇异性检测：当雅可比矩阵的行列式为0时，机械臂处于奇异位形

2.3.1 奇异性

奇异性，说白了就是机械臂的“死穴”。在奇异点附近，机械臂会失去某个方向的运动能力，或者关节速度会变得非常大。

常见的奇异类型：

奇异类型	发生条件	表现
边界奇异	机械臂完全伸直或完全折叠	无法沿径向运动
腕部奇异	关节5角度为0°或180°	关节4和关节6共线，失去一个自由度
肩部奇异	腕部中心位于肩关节轴线上	关节1无法产生有效运动

2.3.2 避坑指南

关于奇异性，我踩过不少坑。这里分享几个实用的经验：

• 规划路径时，尽量避开奇异区域。如果无法避开，降低速度通过。
• 在奇异点附近，使用阻尼最小二乘法（DLS）代替直接求逆。公式：J^# = J^T(JJ^T + λI)^-1，其中λ是阻尼系数。
• 监控关节速度。如果某个关节速度突然变得很大，很可能是接近奇异了。
• 不要完全依赖数值解。在奇异点附近，数值解会变得非常不稳定。我建议在奇异区域使用预先规划的轨迹。