ABB工业机器人运动学研究报告

ABB IRB 6600工业机器人运动学
研究报告
目录
1机器人结构简介 (1)
2机器人的运动学 (2)
2.1、机器人正运动学 (2)
2.2、机器人逆运动学 (8)
2.2.1求各关节到末端的坐标变换矩阵 (8)
2.2.2求Jacobian矩阵各列 (13)
1机器人结构简介
ABB工业机器人可以用于实现喷雾、涂胶、物料搬运、点焊等多种功能，是典型的机械臂，在网络上可以查找到较多的相关资料。

本次作业就选取ABB IRB 6600机器人作为研究对象，首先对其结构进行简单简介。

图 1
图 2
ABB IRB 6600是六自由度机器人，具有六个旋转关节，底座固定，通过各关节的旋
转可以完成三维空间内的运动。

图1是ABB IRB 6600机器人的照片及工作范围图，图2是其结构简图和各轴的转动的参数。

2机器人的运动学
在这部分中运用所学知识对ABB IRB 6600 机器人进行D-H建模并求出对应的转换矩阵，并运用Jacobian 法进行逆运动学分析，求出Jacobian变换矩阵。

2.1、机器人正运动学
为了计算方便把机器人各关节前后两连杆共线作为初始状态，画出结构简图如图3
图 3
图3中的关节7实际上是末端执行机构。

运用学过的D-H建模方法建立模型，建模过程中为了方便画出各关节坐标系，将部分连杆进行了拉长，且由于部分关节坐标的Z 轴垂直于纸面，所以用X轴Y轴画出坐标系，用右手定则既得到对应的Z轴。

最终建立模型如图4：
图 4 根据图4的可以得到对应的D-H参数表：
由此算出各关节变换矩阵：
⎥
⎥
⎥⎥⎦
⎤
⎢
⎢⎢⎢⎣⎡+-=1000
10000)cos()sin(0
0)sin()cos(
21111110
h h T θθθθ ⎥⎥
⎥
⎥⎥
⎦
⎤⎢⎢⎢⎢⎢⎣⎡----=100000)sin()cos(01000)cos()sin(22'
22221θθθθh T
⎥⎥
⎥⎥⎦⎤
⎢
⎢⎢⎢⎣⎡-=1000010000)sin()cos(0)cos()sin(3333332θθθθh T ⎥
⎥
⎥⎥⎦⎤
⎢
⎢⎢⎢⎣
⎡+-=100000)cos()sin(1000
)sin()cos(44544443
θθθθh h T ⎥⎥
⎥
⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎣⎡-=100000)cos()sin(01000)sin()cos(55'55554θθθθh T
⎥⎥
⎥⎥⎦
⎤
⎢
⎢⎢⎢⎣⎡-=100000)cos()sin(10000
)sin()cos(6666665
θθθθh T ⎥⎥⎥
⎥
⎥⎦
⎤
⎢⎢⎢⎢⎢⎣⎡=100
000010000
07'776
h h T
将这些关节坐标变换矩阵连乘就得到了由基坐标系到末端的坐标变换矩阵：
T T T T T T T T 7665544332211070
=
但是由于矩阵规模较大，不便用矩阵形式写出，所以把malab 计算得出的矩阵用分项的形式写出：
=
a
11
sin(θ1)*sin(θ6) + cos(θ6)*(cos(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) + sin(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2)))
a
=
12
cos(θ6)*sin(θ1) - sin(θ6)*(cos(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) + sin(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2)))
=
a
13
sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) - cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)),
a
=
14
h5’*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) - h6*(cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) - sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3))) -
h7*(cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) - sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3))) +
h2’*cos(θ1) - (h4 + h5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2)) + h7’*(sin(θ1)*sin(θ6) +
cos(θ6)*(cos(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) + sin(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)))) - h3*cos(θ1)*sin(θ2)
=
a
21
cos(θ6)*(cos(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1)) + sin(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2))) - cos(θ1)*sin(θ6),
=
a
22
- c os(θ1)*cos(θ6) - sin(θ6)*(cos(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1)) + sin(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2)))
a
=
23
sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) - cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)),
a
=
24
sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) - cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)),
h5’*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) -
h7’*(cos(θ1)*sin(θ6) - cos(θ6)*(cos(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) + sin(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2)))) - h6*(cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) - sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1))) - h7*(cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) - sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1))) + h2’*sin(θ1) - (h4 +
h5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) -
h3*sin(θ1)*sin(θ2)
a
=
31
-cos(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3))),
a
=
32
sin(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3))),
a
=
33
cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)),
a
=
34
h1 + h2 + (h4 + h5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - h5’*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + h3*cos(θ2) + h6*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2))) + h7*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2))) -
h7’*cos(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)))
041=a 042=a 043=a
144=a
用各关节转角的初值来检查变换矩阵内的正确性：
设：0654321======θθθθθθ
代入各关节变换矩阵可以求出各矩阵初值)
0(1==-i i
i i T a θ，由于T
7
6
中没有关节变
量，所以保持不变记为矩阵g
图 5
将图5中的初值矩阵连乘可以得到基坐标系到末端坐标系的变换矩阵：
g a a a a a a T ⨯⨯⨯⨯⨯⨯==65432170
)0(θ计算结果见图6：
图 6
观察图6中的矩阵，检查初始状态末端坐标系)
,,(777Z Y X 在基坐标系
)
,,(000Z Y X 的位姿，很容易看出：
)
,,(777Z Y X 在
)
,,(000Z Y X 中的坐标为
),0,(7654321'7'5'2h h h h h h h h h h ++++++--，并且
7
X 与
X 反向，7Y
与
Y 反向，
7
Z 与
Z 同向，这与D-H 建模图（图4）中所得到的结果相同，可以确定计算过程是正
确的。

2.2、机器人逆运动学
运用所学的Jacobian 法进行机器人逆运动学的求解。

2.2.1求各关节到末端的坐标变换矩阵
首先已经列出的
T
T T T T T 65544332211
0,,,,,，可以依次求出T
T T T T T 6
0616263646
5
,,,,,
使用matlab 计算过程中：
把
T
T T T T T 655443322110,,,,,依次记作a ，b ，c ，d ，e ，f
把
i θ记作（sita i ）
计算结果如下:
T 6
5
T T T 6
5
5464
=
T T T T 6
554
4363
=
T T T T T 6
55443
3262
= 由于T 6
2矩阵元素表达式较复杂，不便列出整个矩阵，所以逐列写出： 第一列：
第二列：
第三列：
第四列：
T T T T T T 65544332
216
1= 由于T 61比T 6
2的矩阵元素表达式更为复杂，所以逐项列出： =11a
-cos(θ6)*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2))),
=12a
sin(θ6)*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2))),
=13a
- cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) -
sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)),
=14a
h2’ - (h4 + h5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) -
h5’*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - h3*sin(θ2) -
h6*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) +
sin(θ5)*(cos(θ2)*cos(θ3) - sin (θ2)*sin(θ3)))
=21a -sin(θ6),
=22a -cos(θ6),
023=a
024=a
=31a
-cos(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) +
sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)))
=32a
sin(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) +
sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3))),
=33a
cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)),
=34a
(h4 + h5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - h5’*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + h3*cos(θ2) + h6*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2))) 041=a
042=a
043=a
144=a
T T T T T T T 6554433221106
0=
由于末端坐标系与第6
关节坐标系固连，没有转动变量，所以T 6
0与正运动学计算时求得的T 7
0除了最后一列之外完全相同，下面写出最后一列的元素：
=14a
h5’*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) -
h6*(cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) - sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3))) + h2’*cos(θ1) - (h4 + h5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2)) - h3*cos(θ1)*sin(θ2)
=24a
h5’*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) -
h6*(cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) - sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1))) + h2’*sin(θ1) - (h4 + h5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2)) - h3*sin(θ1)*sin(θ2)
=
34a
H1 + h2 + (h4 + h5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
h5’*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + h3*cos(θ2) +
h6*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)))
144=a
2.2.2求Jacobian 矩阵各列
利用求出各关节到末端的坐标变换矩阵，可以求出Jacobian 矩阵各列。

Jacobian 矩阵第i 列i J 的计算方法如下：
⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡=-100061z z
z
z y y y y
x x x x i p a o n p a o n p a o n T , 并且()()()⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⨯⨯⨯x y y x x y y x x y y x z z z a p a p o p o p n p n p a p o p n p 那么:()()()⎥⎥⎥⎥⎥⎥⎥
⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡⨯⨯⨯=z z z z z z i a o n a p o p n p J 根据此公式可以算出Jacobian 矩阵的各列:
⎥⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡-=010sin 0cos 6666θθh J ⎥⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡=5556'565656'56556'55cos 0sin )sin θh + (h sin sin )cos sin θh + (h sin cos )sin h + (h -θθθθθθθθJ ⎥⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡=0cos cos cos )sin h + (h -)sin cos h + h + (h sin sin )sin h + (h -cos )sin cos h + h + (h sin cos )sin h + (h -cos )cos cos h + h + (h 55656'5656546556'5655654
6556'56556544θθθθθθθθθθθθθθθθθθJ
=3J
sin(θ6)*(cos(θ3)*sin(θ5) + cos(θ5)*sin(θ3))*(h3 +
h6*(cos(θ3)*cos(θ5) - sin(θ3)*sin(θ5)) + cos(θ3)*(h4 + h5) - h5’*sin(θ3)) + cos(θ6)*(cos(θ3)*sin(θ5) + cos(θ5)*sin(θ3))*(h6*(cos(θ3)*sin(θ5) + cos(θ5)*sin(θ3)) + sin(θ3)*(h4 + h5) + h5’*cos(θ3))
- sin(θ6)*(cos(θ3)*cos(θ5) - sin(θ3)*sin(θ5))*(h3 + h6*(cos(θ3)*cos(θ5) - sin(θ3)*sin(θ5)) + cos(θ3)*(h4 + h5) - h5’*sin(θ3)) - cos(θ6)*(cos(θ3)*cos(θ5) - sin(θ3)*sin(θ5))*(h6*(cos(θ3)*sin(θ5) + cos(θ5)*sin(θ3)) + sin(θ3)*(h4 + h5) + h5’*cos(θ3)),
cos(θ6)*(h3 + h6*(cos(θ3)*cos(θ5) - sin(θ3)*sin(θ5)) + cos(θ3)*(h4 + h5) - h5’*sin(θ3)) - sin(θ6)*(h6*(cos(θ3)*sin(θ5) + cos(θ5)*sin(θ3)) + sin(θ3)*(h4 + h5) + h5’*cos(θ3))]
cos(θ3)*cos(θ5) - sin(θ3)*sin(θ5)
cos(θ3)*sin(θ5) + cos(θ5)*sin(θ3),
0,
J
2
-sin(θ6)*(cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) - sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)))*((h4 +
h5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) - h2’ + h5’*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) + h3*sin(θ2) + h6*(cos(θ5)*(cos(θ2)*sin(θ3) +
cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)))),
cos(θ6)*((h4 + h5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) - h2’ + h5’*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) + h3*sin(θ2) +
h6*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) +
sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)))),
-sin(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)))*((h4 +
h5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) - h2’ + h5’*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) + h3*sin(θ2) + h6*(cos(θ5)*(cos(θ2)*sin(θ3) +
cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3))))
- cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) -
sin(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)),
0,
cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)),
J
1
(sin(θ1)*sin(θ6) + cos(θ6)*(cos(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) + sin(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2))))*(h6*(cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) - sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1))) - h5’*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1)) - h2’*sin(θ1) + (h4 +
h5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) +
h3*sin(θ1)*sin(θ2)) - (cos(θ6)*sin(θ1) -
sin(θ6)*(cos(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) + sin(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2))))*(h6*(cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) - sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) -
cos(θ1)*cos(θ2)*cos(θ3))) - h5’*(cos(θ1)*sin(θ2)*sin(θ3) -
cos(θ1)*cos(θ2)*cos(θ3)) - h2’*cos(θ1) + (h4 +
h5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) +
h3*cos(θ1)*sin(θ2)),
(cos(θ1)*cos(θ6) + sin(θ6)*(cos(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) + sin(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2))))*(h6*(cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) - sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) -
cos(θ1)*cos(θ2)*cos(θ3))) - h5’*(cos(θ1)*sin(θ2)*sin(θ3) -
cos(θ1)*cos(θ2)*cos(θ3)) - h2’*cos(θ1) + (h4 +
h5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) +
h3*cos(θ1)*sin(θ2)) - (cos(θ1)*sin(θ6) -
cos(θ6)*(cos(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) + sin(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2))))*(h6*(cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) - sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1))) - h5’*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1)) - h2’*sin(θ1) + (h4 +
h5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) +
h3*sin(θ1)*sin(θ2)),
- cos(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2)) +
sin(θ5)*(cos(θ2)*cos(θ3) -
sin(θ2)*sin(θ3)))*(h6*(cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) +
cos(θ3)*sin(θ1)*sin(θ2)) - sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1))) - h5’*(sin(θ1)*sin(θ2)*sin(θ3) -
cos(θ2)*cos(θ3)*sin(θ1)) - h2’*sin(θ1) + (h4 +
h5)*(co s(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)) +
h3*sin(θ1)*sin(θ2)) - sin(θ6)*(cos(θ5)*(cos(θ2)*sin(θ3) +
cos(θ3)*sin(θ2)) + sin(θ5)*(cos(θ2)*cos(θ3) -
sin(θ2)*sin(θ3)))*(h6*(cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) +
cos(θ1)*cos(θ3)*sin(θ2)) - sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3))) - h5’*(cos(θ1)*sin(θ2)*sin(θ3) -
cos(θ1)*cos(θ2)*cos(θ3)) - h2’*cos(θ1) + (h4 +
h5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)) +
h3*cos(θ1)*sin(θ2))
sin(θ5)*(cos(θ1)*sin(θ2)*sin(θ3) - cos(θ1)*cos(θ2)*cos(θ3)) - cos(θ5)*(cos(θ1)*cos(θ2)*sin(θ3) + cos(θ1)*cos(θ3)*sin(θ2)),
sin(θ5)*(sin(θ1)*sin(θ2)*sin(θ3) - cos(θ2)*cos(θ3)*sin(θ1)) - cos(θ5)*(cos(θ2)*sin(θ1)*sin(θ3) + cos(θ3)*sin(θ1)*sin(θ2)),
cos(θ5)*(cos(θ2)*cos(θ3) - sin(θ2)*sin(θ3)) -
sin(θ5)*(cos(θ2)*sin(θ3) + cos(θ3)*sin(θ2))
上面求出的矩阵各列，组合起来就是Jacobian 矩阵：
[]654321J J J J J J J =
通过公式：
•
-•=P J Q 1 •
P ：末端的连杆的速度
•Q : 各关节的广义速度
可以进行运动学的求逆运算。

在本次研究学习报告中，主要运用所学的知识对ABB IRB 6600机器人简化后的结构进行了运动学分析，包括D-H 建模，和Jacobian 法求逆；运用的方法较为简单基础，随着今后对机器人的相关知识的更多了解、学习，会对机器人有更为深入的学习研究。