-sinθ1 px +cosθ2 py =d2
（式5）
（2）求θ3 在选定θ1后，令等式两边矩阵第4列第1行和第4列第3行
的元素对应相等，得到 对上式取平方和，有
利用三角代换 带入式5中，得θ1的解为
同样的，用三角代换求出θ3
（3）求θ2 式3左右乘以A1A2 A3的逆矩阵，得
A3-1 A2-1 A1-1 T=A4 A5 A6 经过一系列变换得
nx ox ax px T= ny oy ay py =A1 A2 A3 A4 A5 A6
nz oz az pz 000 1
（式3）
位姿运动学方程 c1表示cosθ1 ；c23 表示cos(θ2+θ3)其他类推
（1）求θ1 对式3两边左乘A1-1,得 A1-1T=A2 A3 A4 A5 A6 将等式两端分别展开得
2.2 机器人正向运动学
第i 坐标系相对于第i -1 坐标系的位姿Ai , 则第i 坐标系相对于基坐标系的位姿的齐次变
（式1）
当i =6 时, 0T6 确定了机器人末端连杆坐标系
相对于基坐标系的位姿。
0T6 =A1 A2 …A6 , 其中：
(1)沿Xi-1 轴平移ai-1 , 将Oi-1 移动到O’i-1 ; (2)以Xi-1为转轴, 旋转αi-1 角度, 使新的Zi-1轴与Zi轴 同向; (3)沿Zi 平移di, 使O’i-1 移动到Oi ; (4)以Zi 轴为转轴, 旋转θi 角度, 使新的Xi-1轴与Xi 轴 同向。 Ai =Trans(ai-1,0,0) Rot(Xi-1, αi-1) Trans(0,0, di ) Rot(Zi , θi) =
（式4）
t11 t12 t13 cosθ1p+sinθ1p
m11 m12 m13 a2c2+a3c23–d4s23
t21 t22 t23 -sinθ1p+cosθ1p = m21 m22 m23
d2
t31 t32 t33
p
m31 m32 m33 -a2s2-a3s23–d4c23
000
1
0 00
1
将等式两边的矩阵中第4列第2行元素对应，得
（4）求θ4 根据矩阵对应，得到等式
θ5≠180°时，便有
θ5=0°时，这时z4 与z6轴重合， θ4 与 θ6 的转动效果相同， 会有无穷组解
（式2）
大家应该也有点累了，稍作休息
大家有疑问的，可以询问和交流
2.3 Matlab求解机器人末端位姿
将PUMA560机器人的参数带入上述矩阵中， 然后在matlab中计算求解,得到末端位姿。
编程：
2.4 PUMA560机器人逆运动学
即为针对下式给定的末端位姿，求解机 器人各个关节角θ1~θ6。
PUMA560机器人运动学分析
1.PUMA560机器人的参数设计 2.PUMA560机器人运动学分析
1. PUMA560机器人的参数设计 1.1 坐标系的建立
PUMA560 机器人及其坐标系的建立 示意图
1.2 PUMA560机器人连杆参数
2. PUMA560机器人的运动学分析 2.1 连杆变换矩阵（D-H矩阵）