理论部分
概念
运动学正解,简而言之,就是给出6个关节变量,求得机械臂末端的位置和姿态
即给出
j
1
−
j
6
j_1 - j_6
j1−j6,求
x
,
y
,
z
,
r
x
,
r
y
,
r
z
x,y,z,rx,ry,rz
x,y,z,rx,ry,rz
DH参数
只单一地给出关节值或直角坐标值,是不能直接互相转化的,还与具体的机器人有关,这部分有关的内容可以用DH参数表来表示,其描述了机器人各关节坐标系之间的关系
表中内容
连杆长度 (length) :2个相邻关节轴线之间的距离
连杆扭角 (angle) :2个相邻关节轴线之间的角度
连杆偏距 (d) :2个关节坐标系的X轴之间的距离
eg:UR5e
- DH参数表
关节编号 | legth(mm) | d(mm) | angle(deg) |
---|---|---|---|
1 | 0 | 162.5 | 90 |
2 | -425 | 0 | 0 |
3 | -392.2 | 0 | 0 |
4 | 0 | 133.3 | 90 |
5 | 0 | 99.7 | -90 |
6 | 0 | 99.6 | 0 |
计算
根据DH参数表以及
j
1
−
j
6
j_1 - j_6
j1−j6,建立6个关节矩阵
A
1
−
A
6
A_1-A_6
A1−A6,计算出转换矩阵
T
1
−
T
6
T_1-T_6
T1−T6,计算
A
1
−
A
6
A_1-A_6
A1−A6相乘得到矩阵R
R
=
[
r
o
t
3
∗
3
P
3
∗
1
0
1
∗
3
1
]
R=\begin{bmatrix} {rot_{3*3}}&{P_{3*1}}\\ {0_{1*3}}&{1}\\ \end{bmatrix}
R=[rot3∗301∗3P3∗11]
P
3
∗
1
=
(
x
,
y
,
z
)
T
P_{3*1}=(x,y,z)^T
P3∗1=(x,y,z)T
则求出R即求出x,y,z
关节矩阵
A
i
A_i
Ai由当前的关节的
j
i
j_i
ji和DH参数导出
设当前
j
i
j_i
ji为
β
\beta
β,legth为
l
l
l,d为
d
d
d,angle为
α
\alpha
α
A
i
=
[
c
o
s
β
−
s
i
n
β
c
o
s
α
s
i
n
β
s
i
n
α
l
c
o
s
β
s
i
n
β
c
o
s
β
c
o
s
α
−
c
o
s
β
s
i
n
α
l
s
i
n
β
0
s
i
n
α
c
o
s
α
d
0
0
0
1
]
A_i= \left[ \begin{matrix} cos\beta & -sin\beta cos\alpha & sin\beta sin\alpha & lcos\beta \\ sin\beta & cos\beta cos\alpha & -cos\beta sin\alpha & lsin\beta \\ 0 & sin\alpha & cos\alpha & d \\ 0 & 0 & 0 & 1 \end{matrix} \right]
Ai=
cosβsinβ00−sinβcosαcosβcosαsinα0sinβsinα−cosβsinαcosα0lcosβlsinβd1
R
=
A
1
A
2
A
3
A
4
A
5
A
6
R=A_1A_2A_3A_4A_5A_6
R=A1A2A3A4A5A6
然后再求rx,ry,rz
r
o
t
3
∗
3
=
[
r
00
r
01
r
02
r
10
r
11
r
12
r
20
r
21
r
22
]
rot_{3*3}= \left[ \begin{matrix} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \\ \end{matrix} \right]
rot3∗3=
r00r10r20r01r11r21r02r12r22
r
x
=
a
r
c
t
a
n
(
r
[
1
]
[
2
]
,
r
[
2
]
[
2
]
)
rx = arctan(r[1][2], r[2][2])
rx=arctan(r[1][2],r[2][2])
r
y
=
a
r
c
t
a
n
(
r
[
0
]
[
2
]
,
r
[
0
]
[
0
]
2
+
r
[
0
]
[
1
]
2
)
ry = arctan(r[0][2], \sqrt{r[0][0] ^2 + r[0][1]^2})
ry=arctan(r[0][2],r[0][0]2+r[0][1]2)
r
z
=
a
r
c
t
a
n
(
r
[
0
]
[
1
]
,
r
[
0
]
[
0
]
)
rz = arctan(r[0][1], r[0][0])
rz=arctan(r[0][1],r[0][0])
代码(C++)
可见https://github.com/jeekzhang/6DOF-KFS
验证(UR5e)
j . t x t : j.txt: j.txt:
57.3 | 57.3 | 57.3 | 57.3 | 57.3 | 57.3 |
---|
输出:
x | y | z | rx | ry | rz |
---|---|---|---|---|---|
174.032973 | -75.257828 | -464.848688 | -106.158882 | 64.782997 | 67.592110 |
robodk C#API结果:
x | y | z | rx | ry | rz |
---|---|---|---|---|---|
174.0 | -75.3 | -464.9 | -106.2 | 64.8 | 67.6 |
参考资料:
6轴机器人运动学正解,逆解1
机器人导论 学习笔记2 - 运动学(正解)
欧拉角,四元数,旋转矩阵相互转化(c++, python)