3、运动控制基础（二）：正逆运动学求解（以2-DOF机械臂为例），雅可比矩阵与奇异性分析
好，咱们接着聊运动控制。上一章我们把坐标系和变换矩阵搞清楚了，这章就来点真格的——怎么让机械臂动起来，并且动得明白。
我个人习惯，讲运动学一定要从最简单的2-DOF机械臂入手。为啥？因为两个关节的模型，你用手都能算出来，直观得很。等你把2-DOF吃透了，6-DOF的工业臂无非就是多叠几层矩阵而已。
3.1 正运动学：给角度，求位置
正运动学，说白了就是：我知道每个关节转了多少度，你告诉我末端在哪儿。
拿一个平面2-DOF臂举例。两个连杆长度分别是L1和L2，两个关节角分别是θ1和θ2。末端坐标(x, y)怎么算？
嗯，这里其实就是一个几何投影问题。你看：
- 第一个关节转θ1，末端相对于第一个关节的位置是(L1·cosθ1, L1·sinθ1)
- 第二个关节相对第一个关节又转了θ2，所以第二个连杆的方向是θ1+θ2
- 末端坐标就是两个向量相加
公式写出来就是：
x = L1·cos(θ1) + L2·cos(θ1+θ2)
y = L1·sin(θ1) + L2·sin(θ1+θ2)我在项目中遇到过有人直接用这个公式去算6轴臂，结果发现算出来不对。为啥？因为6轴臂的关节轴线不在一个平面上，得用齐次变换矩阵连乘。但2-DOF这个例子，是理解正运动学最好的起点。
核心要点：正运动学是唯一确定的。给定一组关节角，末端位姿是确定的。不存在多解问题。
3.2 逆运动学：给位置，求角度
逆运动学就反过来了：我知道末端要走到(x, y)，你告诉我每个关节该转多少度。
这才是真正头疼的地方。你想想看，正运动学是1对1的映射，逆运动学却是1对多的——同一个位置，机械臂可能有好几种姿态都能到达。
还是拿2-DOF臂来说。已知末端坐标(x, y)，求θ1和θ2。
第一步，先求θ2。用余弦定理：
cos(θ2) = (x² + y² - L1² - L2²) / (2·L1·L2)这里有个坑——cos的值必须在[-1, 1]之间。如果超出这个范围，说明目标点超出了机械臂的工作空间，到不了。
注意：我曾经调试一个焊接机器人，发现末端总是差几毫米对不准焊缝。查了半天，原来是逆运动学求解时选了错误的解——肘部朝上和朝下都能到同一个点，但焊接姿态要求肘部必须朝下。选错解，轨迹就偏了。
第二步，求θ1。这里要用到atan2函数，而不是普通的atan。为啥？因为atan2能处理四个象限的角度，不会出现符号错误。
θ1 = atan2(y, x) - atan2(L2·sin(θ2), L1 + L2·cos(θ2))你看，逆运动学求解时，θ2有两个可能值（正负根号），对应肘部朝上和朝下两种构型。这就是所谓的多解问题。
| 构型 | θ2范围 | 适用场景 |
|---|---|---|
| 肘部朝上 | θ2 > 0 | 避障、工作空间上方 |
| 肘部朝下 | θ2 < 0 | 桌面操作、工作空间下方 |
3.3 雅可比矩阵：速度与力的桥梁
正逆运动学解决的是位置问题。但实际控制中，我们更关心速度——末端要以多快速度运动，关节该转多快？
雅可比矩阵J，就是连接关节速度(q̇)和末端速度(ẋ)的桥梁：
ẋ = J · q̇对于2-DOF臂，雅可比矩阵是2×2的：
J = [ -L1·sin(θ1) - L2·sin(θ1+θ2), -L2·sin(θ1+θ2) ]
[ L1·cos(θ1) + L2·cos(θ1+θ2), L2·cos(θ1+θ2) ]这个矩阵的每一列，其实对应的是每个关节对末端速度的贡献。第一列是关节1单独转动时末端的速度方向，第二列是关节2的。
我建议你把这个矩阵和正运动学的公式对照着看——雅可比其实就是正运动学对关节角求偏导。说白了，就是位置对角度变化的敏感度。
3.4 奇异性分析：机械臂的"死穴"
每个机械臂都有它的"死穴"——某些姿态下，它会失去某个方向的运动能力。这就是奇异性。
从数学上看，奇异性就是雅可比矩阵的行列式为零的时候：
det(J) = L1·L2·sin(θ2)你看，当sin(θ2)=0时，行列式为零。也就是θ2=0°或θ2=180°的时候。
θ2=0°意味着两个连杆完全伸直，机械臂处于"全伸展"状态。这时候，末端只能沿着垂直于臂的方向运动，沿臂方向动不了。
θ2=180°意味着两个连杆完全折叠，末端缩到关节1附近。这时候，末端同样失去一个自由度。
实战经验：我曾经调试一个码垛机器人，在抓取最远端的箱子时，机械臂突然剧烈抖动。检查代码发现，逆运动学求解时关节速度变得极大——这就是接近奇异点的典型症状。解决方案是在轨迹规划时加入奇异回避，让路径稍微绕开奇异区域。
奇异点为什么危险？因为当det(J)→0时，逆雅可比矩阵的元素会变得非常大。这意味着：末端只要有一点点速度需求，关节就需要极高的速度来响应。实际电机有速度上限，跟不上就会出问题。
3.5 知识体系总览
下面这张图，把本章的核心逻辑串起来了：
你看，正运动学和逆运动学解决的是"位置"层面的问题，雅可比矩阵把问题扩展到"速度"层面，而奇异性分析则是安全层面的保障。这三层，缺一不可。
实际项目中，我一般这样用：轨迹规划阶段用逆运动学算关节角，运动过程中用雅可比做速度前馈，同时实时监测det(J)的值，一旦接近零就触发保护逻辑。
本章小结：
- 正运动学：关节角→末端位姿，唯一解
- 逆运动学：末端位姿→关节角，多解需选择
- 雅可比矩阵：关节速度与末端速度的线性映射
- 奇异性：det(J)=0时机械臂失去自由度，需回避
嗯，运动学基础就讲到这里。下一章我们会把这些知识用到实际控制中——怎么让机械臂沿着一条直线平滑运动，而不是走一步停一步。到时候你会看到，雅可比矩阵在轨迹插补中扮演着关键角色。
公众号:蓝海资料掘金营，微信deep3321