3、机械臂运动学基础（下）：正运动学（DH参数法）、逆运动学求解思路、典型6轴臂的构型分析
好，咱们接着聊。上一节我们把坐标系变换和旋转矩阵这些数学工具铺垫好了，这一节就要真刀真枪地用到机械臂上了。
运动学，说白了就是回答两个问题：“我给了关节角度，末端到底在哪儿？” 和 “我想让末端去某个位置，关节该怎么转？”。前者叫正运动学，后者叫逆运动学。嗯，听起来逆运动学好像更实用一些？别急，没有正运动学打底，逆运动学就是空中楼阁。
3.1 正运动学：DH参数法
正运动学，就是已知关节角度，求末端位姿。我最早接触这玩意儿的时候，觉得公式推导特别繁琐，一个六轴臂能写满一整页纸。后来发现，DH参数法就是用来干这个的——它把复杂的几何关系，抽象成了四个参数，然后通过四次齐次变换矩阵相乘，就能得到结果。
我个人习惯把DH参数法分成两步走：建系和填表。
3.1.1 建立连杆坐标系
这一步是基础，也是最容易出错的。标准DH法（Craig版）的建系规则，我总结成一句话：Z轴沿关节轴线，X轴沿公垂线。
- Zi轴：沿着第i+1个关节的轴线方向。
- Xi轴：沿着Zi-1和Zi的公垂线，方向从Zi-1指向Zi。
- Yi轴：由右手定则确定。
你想想看，如果相邻两个关节轴线是平行的，那公垂线就不唯一了。这时候怎么办？我一般取经过下一个坐标系原点的那个公垂线。嗯，这里要注意，原点位置的选择会影响后面的参数，但最终算出来的末端位姿是一样的。
3.1.2 四个DH参数
建好坐标系之后，我们只需要提取四个参数，就能唯一确定相邻两个连杆的变换关系。这四个参数分别是：
| 参数 | 符号 | 物理含义 | 我的理解 |
|---|---|---|---|
| 连杆长度 | ai-1 | 沿Xi-1轴，从Zi-1到Zi的距离 | 说白了就是两个关节轴之间的最短距离 |
| 连杆扭角 | αi-1 | 绕Xi-1轴，从Zi-1到Zi的转角 | 两个关节轴之间的夹角 |
| 连杆偏距 | di | 沿Zi轴，从Xi-1到Xi的距离 | 可以理解为关节的“偏移量” |
| 关节角 | θi | 绕Zi轴，从Xi-1到Xi的转角 | 这就是我们控制的变量 |
有了这四个参数，相邻坐标系的变换矩阵 i-1Ti 就可以直接写出来：
// 标准DH法的变换矩阵公式
// 注意：这是绕X轴旋转和平移，再绕Z轴旋转和平移
// 顺序是：RotX(α) -> TransX(a) -> RotZ(θ) -> TransZ(d)
// 用代码表示就是：
// T = RotX(alpha) * TransX(a) * RotZ(theta) * TransZ(d)
把从基座到末端的每个关节的变换矩阵连乘起来，就得到了正运动学方程：
0Tn = 0T1 * 1T2 * ... * n-1Tn
3.2 逆运动学求解思路
正运动学是“唯一解”，逆运动学就复杂多了。给定末端位姿，可能有多个关节角度组合能到达，也可能一个都没有。这就是逆运动学的多解性和可达性问题。
逆运动学的求解方法，我把它分成两大类：解析法和数值法。
3.2.1 解析法（封闭解）
解析法，就是通过代数或几何的方法，直接推导出关节角度的表达式。它的优点是快、准、稳，但前提是机械臂的构型必须满足Pieper准则：相邻的三个关节轴交于一点，或者相互平行。
大多数工业六轴臂都满足这个条件，所以解析法是首选。我记得在做一个码垛项目时，用解析法算逆解，单次求解时间不到1毫秒，完全满足实时性要求。
解析法的一般步骤是：
- 分离变量：通过矩阵方程，把包含未知关节角的项移到一边。
- 利用三角函数恒等式：比如用
sin²θ + cos²θ = 1消去变量。 - 求解单个关节角：通常用
atan2函数，它能处理四个象限。 - 递推求解：解出一个关节角后，代入下一个方程，继续求解。
arccos 或 arcsin，结果发现解出来的角度不对。后来才意识到，arccos 的值域是 [0, π]，而实际关节角度可能是负的。所以，能用 atan2 就别用 arccos。
3.2.2 数值法（迭代法）
如果机械臂构型不满足Pieper准则，或者你懒得推导解析式，那就用数值法。数值法的核心思想是：从一个初始猜测开始，不断迭代，让末端位姿误差逐渐减小到零。
最常用的数值法是牛顿-拉夫森法和雅可比伪逆法。它的优点是通用性强，但缺点是：
- 计算量大，实时性不如解析法。
- 依赖初始值，选不好可能收敛到错误的解。
- 可能不收敛，尤其是在奇异点附近。
我个人建议，能用解析法就用解析法。数值法可以作为备选，或者用在冗余自由度机械臂上。
3.3 典型6轴臂的构型分析
市面上常见的六轴工业机器人，比如库卡、ABB、发那科，它们的构型其实大同小异。我把它总结为“3+3”结构：前三个关节决定位置，后三个关节决定姿态。
下面这张图，是我画的一个典型六轴臂的构型示意图，你可以直观地看到各个关节的轴线关系。
从这张图里，你能看到几个关键点：
- J1（腰部）：绕基座垂直轴旋转，决定了机器人的朝向。
- J2、J3（大臂、小臂）：这两个关节决定了手臂在垂直平面内的位置。J2通常是大臂俯仰，J3是小臂俯仰。
- J4、J5、J6（腕部）：这三个关节的轴线通常交于一点，称为腕点。这个设计就是为了满足Pieper准则，方便求逆解。
嗯，说到构型，我还想提一下“非球腕”的情况。有些特殊用途的机器人，比如协作机器人，它的腕部可能不是球形的。这时候，逆运动学就没有解析解了，只能用数值法。我在做一款七轴协作臂的项目时，就遇到了这个问题，最后用了迭代法，虽然慢了点，但也能用。
好了，这一节的内容就到这里。正运动学是基础，逆运动学是工具，构型分析是理解机器人的关键。把这些搞明白了，后面做视觉抓取的时候，你就能知道“相机看到了目标，机器人该怎么动”了。