以三连杆平面机械臂为例,来说明D-H矩阵和雅可比矩阵的求法,并且比较后置法和前置法的异同和运动学末端位姿的一致性和雅可比矩阵的一致性。
一、 后置法。
(1)、模型、坐标系及D-H参数表。
(2)、齐次矩阵。
如图建立个连杆的坐标系,D-H参数如下表所示:
连杆 | a | alpha | d | theta |
---|---|---|---|---|
1 | a1 | 0 | 0 | theta1 |
2 | a2 | 0 | 0 | theta2 |
3 | a3 | 0 | 0 | theta3 |
对应的齐次矩阵是:
Ti−1i(θi)=⎡⎣⎢⎢⎢cisi00−sici000010aiciaisi01⎤⎦⎥⎥⎥
T
i
i
−
1
(
θ
i
)
=
[
c
i
−
s
i
0
a
i
c
i
s
i
c
i
0
a
i
s
i
0
0
1
0
0
0
0
1
]
(3)、运动学方程。
To3(q)=⎡⎣⎢⎢⎢c123s12300−s123c123000010a1c1+a1c12+a1c123a1s1+a1s12+a1s12301⎤⎦⎥⎥⎥
T
3
o
(
q
)
=
[
c
123
−
s
123
0
a
1
c
1
+
a
1
c
12
+
a
1
c
123
s
123
c
123
0
a
1
s
1
+
a
1
s
12
+
a
1
s
123
0
0
1
0
0
0
0
1
]
(4)、雅可比矩阵。
To1(q)=⎡⎣⎢⎢⎢c1s100−s1c1000010a1c1a1s101⎤⎦⎥⎥⎥
T
1
o
(
q
)
=
[
c
1
−
s
1
0
a
1
c
1
s
1
c
1
0
a
1
s
1
0
0
1
0
0
0
0
1
]
T02(q)=⎡⎣⎢⎢⎢c12s1200−s12c12000010a1c1+a1c12a1s1+a1s1201⎤⎦⎥⎥⎥
T
2
0
(
q
)
=
[
c
12
−
s
12
0
a
1
c
1
+
a
1
c
12
s
12
c
12
0
a
1
s
1
+
a
1
s
12
0
0
1
0
0
0
0
1
]
根据以上方程,可以得到:
z0=z1=z2=z3=⎡⎣⎢001⎤⎦⎥
z
0
=
z
1
=
z
2
=
z
3
=
[
0
0
1
]
p0=⎡⎣⎢000⎤⎦⎥
p
0
=
[
0
0
0
]
p1=⎡⎣⎢a1c1a1s10⎤⎦⎥
p
1
=
[
a
1
c
1
a
1
s
1
0
]
p2=⎡⎣⎢a1c1+a1c12a1s1+a1s120⎤⎦⎥
p
2
=
[
a
1
c
1
+
a
1
c
12
a
1
s
1
+
a
1
s
12
0
]
p3=⎡⎣⎢a1c1+a1c12+a1c123a1s1+a1s12+a1s1230⎤⎦⎥
p
3
=
[
a
1
c
1
+
a
1
c
12
+
a
1
c
123
a
1
s
1
+
a
1
s
12
+
a
1
s
123
0
]
根据旋转关节的雅可比计算公式,
JPi=[Zi−1∗(Pe−Pi−1)Zi−1]
J
P
i
=
[
Z
i
−
1
∗
(
P
e
−
P
i
−
1
)
Z
i
−
1
]
可以得到:
J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢−s1(a2c2+a3c23)c1(a2c2+a3c23)0001−c1(a2s2+a3s23)−s1(a2s2+a3s23)a2c2+a3c23s1−c10−a3c1s23−a3s1s23a3c23s1−c10⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥
J
=
[
−
s
1
(
a
2
c
2
+
a
3
c
23
)
−
c
1
(
a
2
s
2
+
a
3
s
23
)
−
a
3
c
1
s
23
c
1
(
a
2
c
2
+
a
3
c
23
)
−
s
1
(
a
2
s
2
+
a
3
s
23
)
−
a
3
s
1
s
23
0
a
2
c
2
+
a
3
c
23
a
3
c
23
0
s
1
s
1
0
−
c
1
−
c
1
1
0
0
]
二、 前置法。
(1)、模型、坐标系及D-H参数表。
(2)、齐次矩阵。
如图建立个连杆的坐标系,D-H参数如下表所示:
连杆 | a | alpha | d | theta |
---|---|---|---|---|
1 | 0 | 0 | 0 | theta1 |
2 | a1 | 0 | 0 | theta2 |
3 | a2 | 0 | 0 | theta3 |
4 | a3 | 0 | 0 |
对应的齐次矩阵是:
To1(θ1)=⎡⎣⎢⎢⎢c1s100−s1c10000100001⎤⎦⎥⎥⎥
T
1
o
(
θ
1
)
=
[
c
1
−
s
1
0
0
s
1
c
1
0
0
0
0
1
0
0
0
0
1
]
Ti−1i(θi)=⎡⎣⎢⎢⎢cisi00−sici000010ai−1ciai−1si01⎤⎦⎥⎥⎥i=2,3
T
i
i
−
1
(
θ
i
)
=
[
c
i
−
s
i
0
a
i
−
1
c
i
s
i
c
i
0
a
i
−
1
s
i
0
0
1
0
0
0
0
1
]
i
=
2
,
3
T34(θi)=⎡⎣⎢⎢⎢100001000010a3001⎤⎦⎥⎥⎥
T
4
3
(
θ
i
)
=
[
1
0
0
a
3
0
1
0
0
0
0
1
0
0
0
0
1
]
(3)、运动学方程。
To4(q)=⎡⎣⎢⎢⎢c123s12300−s123c123000010a1c1+a1c12+a1c123a1s1+a1s12+a1s12301⎤⎦⎥⎥⎥
T
4
o
(
q
)
=
[
c
123
−
s
123
0
a
1
c
1
+
a
1
c
12
+
a
1
c
123
s
123
c
123
0
a
1
s
1
+
a
1
s
12
+
a
1
s
123
0
0
1
0
0
0
0
1
]
(4)、雅可比矩阵。
To1(θ1)=⎡⎣⎢⎢⎢c1s100−s1c10000100001⎤⎦⎥⎥⎥
T
1
o
(
θ
1
)
=
[
c
1
−
s
1
0
0
s
1
c
1
0
0
0
0
1
0
0
0
0
1
]
To2(q)=⎡⎣⎢⎢⎢c1s100−s1c1000010a1c1a1s101⎤⎦⎥⎥⎥
T
2
o
(
q
)
=
[
c
1
−
s
1
0
a
1
c
1
s
1
c
1
0
a
1
s
1
0
0
1
0
0
0
0
1
]
T03(q)=⎡⎣⎢⎢⎢c12s1200−s12c12000010a1c1+a1c12a1s1+a1s1201⎤⎦⎥⎥⎥
T
3
0
(
q
)
=
[
c
12
−
s
12
0
a
1
c
1
+
a
1
c
12
s
12
c
12
0
a
1
s
1
+
a
1
s
12
0
0
1
0
0
0
0
1
]
根据以上方程,可以得到:
z0=z1=z2=z3=z4=⎡⎣⎢001⎤⎦⎥
z
0
=
z
1
=
z
2
=
z
3
=
z
4
=
[
0
0
1
]
p0=p1=⎡⎣⎢000⎤⎦⎥
p
0
=
p
1
=
[
0
0
0
]
p2=⎡⎣⎢a1c1a1s10⎤⎦⎥
p
2
=
[
a
1
c
1
a
1
s
1
0
]
p3=⎡⎣⎢a1c1+a1c12a1s1+a1s120⎤⎦⎥
p
3
=
[
a
1
c
1
+
a
1
c
12
a
1
s
1
+
a
1
s
12
0
]
p4=⎡⎣⎢a1c1+a1c12+a1c123a1s1+a1s12+a1s1230⎤⎦⎥
p
4
=
[
a
1
c
1
+
a
1
c
12
+
a
1
c
123
a
1
s
1
+
a
1
s
12
+
a
1
s
123
0
]
根据旋转关节的雅可比计算公式,
JPi=[Zi∗(Pe−Pi)Zi]
J
P
i
=
[
Z
i
∗
(
P
e
−
P
i
)
Z
i
]
可以得到:
J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢−s1(a2c2+a3c23)c1(a2c2+a3c23)0001−c1(a2s2+a3s23)−s1(a2s2+a3s23)a2c2+a3c23s1−c10−a3c1s23−a3s1s23a3c23s1−c10⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥
J
=
[
−
s
1
(
a
2
c
2
+
a
3
c
23
)
−
c
1
(
a
2
s
2
+
a
3
s
23
)
−
a
3
c
1
s
23
c
1
(
a
2
c
2
+
a
3
c
23
)
−
s
1
(
a
2
s
2
+
a
3
s
23
)
−
a
3
s
1
s
23
0
a
2
c
2
+
a
3
c
23
a
3
c
23
0
s
1
s
1
0
−
c
1
−
c
1
1
0
0
]
三、 比较说明。
通过比较可以发现,后置法和前置法计算的运动学方程和雅可比矩阵是相同的。
四、 参考文献
1) John J Craig, 机器人学导论(第三版),机械工业出版社,2006.6.
2) Saeed B.Niku 等,机器人学导论——分析、系统及应用,电子工业出版社,2004.1.
3) Bruno Siciliano 等,机器人学 建模、规划与控制,西安交通大学出版社,2013.11.