第4章正运动学进阶

4. 正运动学进阶：6自由度工业机器人正解、PUMA560案例分析

好，咱们继续往前推。上一节我们把正运动学的基础打牢了，D-H参数、齐次变换矩阵，这些工具你都熟悉了吧？

这一节，咱们直接上硬菜——6自由度工业机器人的正解。我选了一个经典中的经典：PUMA560。为什么选它？因为我在项目里吃过它的亏，也靠它赚过钱。你想想看，搞懂了PUMA560，市面上90%的六轴机器人你都能拿捏。

4.1 为什么是PUMA560？

PUMA560是Unimation公司在70年代推出的。它是个标志性产品。说白了，后来的工业机器人，很多都是它的徒子徒孙。

它的结构很典型：

前三个关节（J1, J2, J3）决定手腕的位置
后三个关节（J4, J5, J6）决定手腕的姿态

这种“位置+姿态”的分离设计，让计算变得清爽很多。我个人习惯把这种结构叫做“肩-肘-腕”结构，好记。

  核心要点： PUMA560的正解，就是已知6个关节角θ1~θ6，求末端执行器的位姿矩阵T06。

4.2 D-H参数表：PUMA560的“身份证”

要算正解，第一步就是建立D-H参数表。我当年第一次做这个，对着图纸量了半天，结果发现参数给错了……嗯，这里要注意，不同版本的PUMA560，参数可能有细微差别。

下面是我常用的标准参数表：

连杆 i	θi (关节角)	di (连杆偏距)	ai (连杆长度)	αi (连杆扭角)
1	θ1	0	0	-90°
2	θ2	0	a2	0°
3	θ3	0	a3	90°
4	θ4	d4	0	-90°
5	θ5	0	0	90°
6	θ6	d6	0	0°

其中，a2, a3, d4, d6 是机器人的固定尺寸。比如常见的PUMA560，a2=431.8mm, a3=20.32mm, d4=433.07mm, d6=56.25mm。

我的小技巧： 写代码时，把这些参数定义成常量。别问我为什么，我曾经在代码里硬编码数值，结果换了个型号的机器人，改得我头皮发麻。

4.3 正解推导：一步一步来

有了D-H表，剩下的就是机械计算了。每个关节的变换矩阵是：

T_i = Rot(z, θi) * Trans(z, di) * Trans(x, ai) * Rot(x, αi)

展开后就是：

T_i = [cosθi  -sinθi*cosαi   sinθi*sinαi   ai*cosθi]
      [sinθi   cosθi*cosαi  -cosθi*sinαi   ai*sinθi]
      [0       sinαi         cosαi          di      ]
      [0       0             0              1       ]

然后，把6个矩阵乘起来：

T06 = T1 * T2 * T3 * T4 * T5 * T6

这个乘法，手算能算到你怀疑人生。我建议你直接用符号计算工具，比如Matlab的Symbolic Toolbox，或者Python的SymPy。

为什么会这样？因为每个矩阵里都有sin和cos，乘起来之后，表达式会变得非常长。但别怕，最终结果是有规律的。

4.4 最终结果：T06的解析形式

经过一番推导（或者让计算机帮你算），T06可以写成：

T06 = [nx  ox  ax  px]
      [ny  oy  ay  py]
      [nz  oz  az  pz]
      [0   0   0   1 ]

其中，px, py, pz 是末端的位置，nx, ny, nz 等是姿态。

举个例子，px的表达式是：

px = cosθ1*(a2*cosθ2 + a3*cos(θ2+θ3) - d4*sin(θ2+θ3)) - d6*(cosθ1*cos(θ2+θ3)*sinθ4*sinθ5 + cosθ1*cosθ4*cosθ5*sin(θ2+θ3) - sinθ1*sinθ4*sinθ5)

看着复杂吧？但这就是正解。你给6个角度，它就能算出唯一的位置和姿态。

注意： 正解是唯一的，但逆解不唯一。这是机器人学里一个非常重要的概念。我曾经在调试时，正解算出来位置是对的，但姿态不对，查了半天发现是D-H参数里的αi符号搞反了。

4.5 代码实现：Python实战

光说不练假把式。咱们直接上代码。我用Python写了一个简单的正解函数：

import numpy as np

def dh_transform(theta, d, a, alpha):
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d   ],
        [0,   0,      0,     1   ]
    ])

def puma560_forward(theta1, theta2, theta3, theta4, theta5, theta6):
    # 固定参数
    a2 = 431.8
    a3 = 20.32
    d4 = 433.07
    d6 = 56.25
    
    # 各关节变换矩阵
    T1 = dh_transform(theta1, 0, 0, -np.pi/2)
    T2 = dh_transform(theta2, 0, a2, 0)
    T3 = dh_transform(theta3, 0, a3, np.pi/2)
    T4 = dh_transform(theta4, d4, 0, -np.pi/2)
    T5 = dh_transform(theta5, 0, 0, np.pi/2)
    T6 = dh_transform(theta6, d6, 0, 0)
    
    # 总变换矩阵
    T06 = T1 @ T2 @ T3 @ T4 @ T5 @ T6
    return T06

# 示例：所有关节角为0
T = puma560_forward(0, 0, 0, 0, 0, 0)
print("末端位姿矩阵：")
print(T)

运行这段代码，你会得到末端在初始姿态下的位姿。你可以试着改几个角度，看看位置怎么变。我建议你多试试，找找感觉。

4.6 知识体系：一张图看懂

为了让你更直观地理解整个正解流程，我画了一张流程图：

4.7 避坑指南：我踩过的雷

最后，分享几个我亲身经历过的坑：

D-H参数符号搞反：αi的正负号，决定了旋转方向。我曾经因为α3的符号搞反，导致仿真里机器人手臂扭成了麻花。
关节角单位搞错：代码里用弧度，但示教器给的是角度。这个错误我犯了不止一次。
矩阵乘法顺序：T06 = T1 * T2 * ... * T6，顺序不能乱。我见过有人写成T6 * T5 * ... * T1，结果完全不对。

我的建议： 写代码时，每一步都打印中间结果。比如算完T1，打印一下，看看是不是单位矩阵的变形。这样调试起来快得多。

好了，PUMA560的正解就讲到这里。你只要把D-H参数搞对，矩阵乘对，正解就是水到渠成的事。下一节，咱们聊聊逆解——那才是真正考验智商的地方。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录

4. 正运动学进阶：6自由度工业机器人正解、PUMA560案例分析

4.1 为什么是PUMA560？

4.2 D-H参数表：PUMA560的“身份证”

4.3 正解推导：一步一步来

4.4 最终结果：T06的解析形式

4.5 代码实现：Python实战

4.6 知识体系：一张图看懂

4.7 避坑指南：我踩过的雷