IRB2600的改进D-H法
建立IRB2600的改进D-H模型:
![](https://i-blog.csdnimg.cn/blog_migrate/0d6e0538f713714e03834af72cb7b66b.png#pic_center)
IRB2600的改进D-H参数表:
轴号 i i i | α i − 1 \alpha_{i-1} αi−1 | a i − 1 a_{i-1} ai−1 | d i d_{i} di | θ i \theta_{i} θi |
---|---|---|---|---|
1 | 0 | 0 | d 1 ( 445 ) d_{1}(445) d1(445) | θ 1 \theta_{1} θ1 |
2 | − 9 0 ∘ -90^{\circ} −90∘ | a 1 ( 150 ) a_{1}(150) a1(150) | 0 | θ 2 + 9 0 ∘ \theta_{2}+90^{\circ} θ2+90∘ |
3 | 0 | a 2 ( − 700 ) a_{2}(-700) a2(−700) | 0 | θ 3 \theta_{3} θ3 |
4 | 9 0 ∘ 90^{\circ} 90∘ | a 3 ( − 115 ) a_{3}(-115) a3(−115) | d 4 ( 795 ) d_{4}(795) d4(795) | θ 4 \theta_{4} θ4 |
5 | − 9 0 ∘ -90^{\circ} −90∘ | 0 | 0 | θ 5 \theta_{5} θ5 |
6 | 9 0 ∘ 90^{\circ} 90∘ | 0 | d 6 ( 85 ) d_{6}(85) d6(85) | θ 6 \theta_{6} θ6 |
则机器人的运动学可以公式为:
T
0
6
=
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
=
T
1
0
⋅
T
2
1
⋅
T
3
2
⋅
T
4
3
⋅
T
5
4
⋅
T
6
5
(1)
\mathbf{T}^{6}_{0}=\begin{bmatrix} n_x & o_x & a_x & p_x\\ n_y & o_y & a_y & p_y\\ n_z & o_z & a_z & p_z\\ 0 & 0 & 0 & 1 \end{bmatrix} ={{\mathbf{T}^{0}_{1}}\cdot{\mathbf{T}^{1}_{2}}\cdot{\mathbf{T}^2_{3}}\cdot{\mathbf{T}^3_{4}}\cdot{\mathbf{T}^4_{5}}\cdot{\mathbf{T}^5_{6}}} \tag{1}
T06=
nxnynz0oxoyoz0axayaz0pxpypz1
=T10⋅T21⋅T32⋅T43⋅T54⋅T65(1)
其中:
T
i
−
1
i
=
[
cos
(
θ
i
)
−
sin
(
θ
i
)
0
a
i
−
1
sin
(
θ
i
)
cos
(
α
i
−
1
)
cos
(
θ
i
)
cos
(
α
i
−
1
)
−
sin
(
α
i
−
1
)
−
d
i
sin
(
α
i
−
1
)
sin
(
θ
i
)
sin
(
α
i
−
1
)
cos
(
θ
i
)
sin
(
α
i
−
1
)
cos
(
α
i
−
1
)
d
i
cos
(
α
i
−
1
)
0
0
0
1
]
(2)
\mathbf{T}_{i-1}^{i}=\begin{bmatrix} \cos(\theta_i) & -\sin(\theta_{i}) & 0 & a_{i-1}\\ \sin(\theta_{i})\cos(\alpha_{i-1}) & \cos(\theta_{i})\cos(\alpha_{i-1}) & -\sin(\alpha_{i-1}) & -d_i\sin(\alpha_{i-1}) \\ \sin(\theta_{i})\sin(\alpha_{i-1}) & \cos(\theta_{i})\sin(\alpha_{i-1}) & \cos(\alpha_{i-1}) & d_i\cos(\alpha_{i-1}) \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \tag{2}
Ti−1i=
cos(θi)sin(θi)cos(αi−1)sin(θi)sin(αi−1)0−sin(θi)cos(θi)cos(αi−1)cos(θi)sin(αi−1)00−sin(αi−1)cos(αi−1)0ai−1−disin(αi−1)dicos(αi−1)1
(2)
IRB2600的逆运动学解析解推算
推算 θ 1 \theta_{1} θ1:
利用关系式:
(
T
0
1
)
−
1
⋅
T
e
n
d
=
T
1
2
⋅
T
2
3
⋅
T
3
4
⋅
T
4
5
⋅
T
5
6
(\mathbf{T}^1_0)^{-1}\cdot\mathbf{T}_{end}=\mathbf{T}^2_1\cdot\mathbf{T}^3_2\cdot\mathbf{T}^4_3\cdot\mathbf{T}^5_4\cdot\mathbf{T}^6_5
(T01)−1⋅Tend=T12⋅T23⋅T34⋅T45⋅T56对
θ
1
\theta_1
θ1进行推算,通过对比矩阵元素之间关系,可按照如下过程求解
θ
1
\theta_1
θ1
T
l
e
f
t
(
2
,
4
)
T
l
e
f
t
(
2
,
3
)
=
T
r
i
g
h
t
(
2
,
4
)
T
r
i
g
h
t
(
2
,
3
)
(3)
\displaystyle\frac{\mathbf{T}_{left}(2,4)}{\mathbf{T}_{left}(2,3)}=\frac{\mathbf{T}_{right}(2,4)}{\mathbf{T}_{right}(2,3)}\tag{3}
Tleft(2,3)Tleft(2,4)=Tright(2,3)Tright(2,4)(3)
化简得到:
θ
1
=
a
r
c
t
a
n
a
y
d
6
−
p
y
a
x
d
6
−
p
x
(4)
\displaystyle\theta_{1}=arctan\frac{a_yd_6-p_y}{a_xd_6-p_x}\tag{4}
θ1=arctanaxd6−pxayd6−py(4)
对于
θ
1
\theta_1
θ1的计算,将会产生两个解。
推算 θ 2 \theta_2 θ2和 θ 3 \theta_3 θ3:
θ
2
\theta_2
θ2和
θ
3
\theta_3
θ3可以由一个等式关系确定:
T
e
n
d
⋅
(
T
4
5
⋅
T
5
6
)
−
1
=
T
0
1
⋅
T
1
2
⋅
T
2
3
⋅
T
3
4
\mathbf{T}_{end}\cdot(\mathbf{T}^5_4\cdot\mathbf{T}^6_5)^{-1}=\mathbf{T}^1_0\cdot\mathbf{T}^2_1\cdot\mathbf{T}^3_2\cdot\mathbf{T}^4_3
Tend⋅(T45⋅T56)−1=T01⋅T12⋅T23⋅T34,首先为确定
θ
2
\theta_2
θ2关于
θ
1
\theta_1
θ1的关系式,通过矩阵中如下元素的等式关系:
T
l
e
f
t
(
2
,
4
)
=
T
r
i
g
h
t
(
2
,
4
)
(5)
\displaystyle \mathbf{T}_{left}(2,4)=\mathbf{T}_{right}(2,4) \tag{5}
Tleft(2,4)=Tright(2,4)(5)
T l e f t ( 3 , 4 ) = T r i g h t ( 3 , 4 ) (6) \displaystyle \mathbf{T}_{left}(3,4)=\mathbf{T}_{right}(3,4) \tag{6} Tleft(3,4)=Tright(3,4)(6)
化简得:
a
3
s
i
n
(
θ
2
+
θ
3
)
−
d
4
c
o
s
(
θ
2
+
θ
3
)
=
a
1
−
a
2
s
i
n
θ
2
−
p
y
−
d
6
a
y
s
i
n
θ
1
(7)
\displaystyle a_3sin(\theta_2+\theta_3)-d_4cos(\theta_2+\theta_3)=a_1-a_2sin\theta_2-\frac{p_y-d_6a_y}{sin\theta_1} \tag{7}
a3sin(θ2+θ3)−d4cos(θ2+θ3)=a1−a2sinθ2−sinθ1py−d6ay(7)
d 4 s i n ( θ 2 + θ 3 ) + a 3 c o s ( θ 2 + θ 3 ) = d 1 − a 2 c o s ( θ 2 ) − ( p z − d 6 a z ) (8) d_4sin(\theta_2+\theta_3)+a_3cos(\theta_2+\theta_3)=d_1-a_2cos(\theta_2)-(p_z-d_6a_z) \tag{8} d4sin(θ2+θ3)+a3cos(θ2+θ3)=d1−a2cos(θ2)−(pz−d6az)(8)
为化简需要,令
X
=
s
i
n
(
θ
2
+
θ
3
)
X=sin(\theta_2+\theta_3)
X=sin(θ2+θ3)和
Y
=
c
o
s
(
θ
2
+
θ
3
)
Y=cos(\theta_2+\theta_3)
Y=cos(θ2+θ3),通过式(7)和(8)联立起来可以消除
X
X
X和
Y
Y
Y,从而得到:
a
3
2
+
d
4
2
=
(
k
1
−
a
2
s
i
n
θ
2
)
2
+
(
k
2
−
a
2
c
o
s
θ
2
)
2
(9)
\displaystyle {a_3}^2+{d_4}^2=(k_1-a_2sin\theta_2)^2+(k_2-a_2cos\theta_2)^2 \tag{9}
a32+d42=(k1−a2sinθ2)2+(k2−a2cosθ2)2(9)
其中:
k
1
=
a
1
−
p
y
−
d
6
a
y
s
i
n
θ
1
(10)
\displaystyle k_1=a_1-\frac{p_y-d_6a_y}{sin\theta_1} \tag{10}
k1=a1−sinθ1py−d6ay(10)
k 2 = d 1 − ( p z − d 6 a z ) (11) \displaystyle k_2=d_1-(p_z-d_6a_z) \tag{11} k2=d1−(pz−d6az)(11)
观察发现,式(9)中只有
θ
1
\theta_1
θ1和
θ
2
\theta_2
θ2未知量,由此可建立
θ
2
\theta_2
θ2关于
θ
1
\theta_1
θ1的关系表达式,即
θ
2
\theta_2
θ2的解析解:
θ
2
=
a
r
c
s
i
n
k
1
2
+
k
2
2
+
a
2
2
−
(
a
3
2
+
d
4
2
)
2
a
2
k
1
2
+
k
2
2
−
ϕ
(12)
\displaystyle \theta_2=arcsin \frac{{k_1}^2+{k_2}^2+{a_2}^2-({a_3}^2+{d_4}^2)}{2a_2\sqrt{{k_1}^2+{k_2}^2}}-\phi \tag{12}
θ2=arcsin2a2k12+k22k12+k22+a22−(a32+d42)−ϕ(12)
s i n ϕ = k 2 k 1 2 + k 2 2 , c o s ϕ = k 1 k 1 2 + k 2 2 (13) \displaystyle sin\phi = \frac{k_2}{\sqrt{{k_1}^2+{k_2}^2}} ,cos\phi = \frac{k_1}{\sqrt{{k_1}^2+{k_2}^2}} \tag{13} sinϕ=k12+k22k2,cosϕ=k12+k22k1(13)
对于 θ 2 \theta_2 θ2的计算,亦会产生两个解。
以上求出了
θ
1
\theta_1
θ1和
θ
2
\theta_2
θ2,将式(7)(8)中
X
X
X和
Y
Y
Y作为未知数,求出:
X
=
s
i
n
(
θ
2
+
θ
3
)
=
a
3
A
+
d
4
B
a
3
2
+
d
4
2
(14)
\displaystyle X=sin(\theta_2+\theta_3)=\frac {a_3A+d_4B}{{a_3}^2+{d_4}^2} \tag{14}
X=sin(θ2+θ3)=a32+d42a3A+d4B(14)
Y = c o s ( θ 2 + θ 3 ) = a 3 B − d 4 A a 3 2 + d 4 2 (15) \displaystyle Y=cos(\theta_2+\theta_3)=\frac {a_3B-d_4A}{{a_3}^2+{d_4}^2} \tag{15} Y=cos(θ2+θ3)=a32+d42a3B−d4A(15)
其中:
A
=
k
1
−
a
2
s
i
n
θ
2
,
B
=
k
2
−
a
2
c
o
s
θ
2
(16)
\displaystyle A = k_1-a_2sin\theta_2 ,B=k_2-a_2cos\theta_2 \tag{16}
A=k1−a2sinθ2,B=k2−a2cosθ2(16)
结合式(14)(15)(16)和已算出的
θ
1
θ
2
\theta_1 \theta_2
θ1θ2可以计算出关节角
θ
3
\theta_3
θ3。
推算 θ 5 \theta_{5} θ5、 θ 4 \theta_4 θ4和 θ 6 \theta_6 θ6:
经过几次推算尝试,观察矩阵元素的特征(
T
r
i
g
h
t
\mathbf{T}_{right}
Tright矩阵中元素表达式只含
θ
4
θ
5
θ
6
\theta_4\theta_5\theta_6
θ4θ5θ6且简单,
T
l
e
f
t
\mathbf{T}_{left}
Tleft矩阵中只含
θ
1
θ
2
θ
3
\theta_1\theta_2\theta_3
θ1θ2θ3且已求得),因此确定关系式:
(
T
0
1
⋅
T
1
2
⋅
T
2
3
)
−
1
⋅
T
e
n
d
=
T
3
4
⋅
T
4
5
⋅
T
5
6
(\mathbf{T}^1_0\cdot\mathbf{T}^2_1\cdot\mathbf{T}^3_2)^{-1}\cdot\mathbf{T}_{end}=\mathbf{T}^4_3\cdot\mathbf{T}^5_4\cdot\mathbf{T}^6_5
(T01⋅T12⋅T23)−1⋅Tend=T34⋅T45⋅T56为推导
θ
4
θ
5
θ
6
\theta_4\theta_5\theta_6
θ4θ5θ6的等式关系,根据矩阵中元素等式关系:
T
l
e
f
t
(
2
,
3
)
=
T
r
i
g
h
t
(
2
,
3
)
(17)
\displaystyle \mathbf{T}_{left}(2,3)=\mathbf{T}_{right}(2,3) \tag{17}
Tleft(2,3)=Tright(2,3)(17)
可以直接求得
θ
5
\theta_5
θ5:
θ
5
=
a
r
c
c
o
s
(
a
x
c
o
s
(
θ
2
+
θ
3
)
c
o
s
θ
1
+
a
y
c
o
s
(
θ
2
+
θ
3
)
s
i
n
θ
1
−
a
z
s
i
n
(
θ
2
+
θ
3
)
)
(18)
\displaystyle \theta_5=arccos(a_xcos(\theta_2+\theta_3)cos\theta_1+a_ycos(\theta_2+\theta_3)sin\theta_1-a_zsin(\theta_2+\theta_3)) \tag{18}
θ5=arccos(axcos(θ2+θ3)cosθ1+aycos(θ2+θ3)sinθ1−azsin(θ2+θ3))(18)
用该解析解计算
θ
5
\theta_5
θ5,会得到两个解。
为求得
θ
4
\theta_4
θ4,可建立如下关系式:
T
l
e
f
t
(
1
,
3
)
=
T
r
i
g
h
t
(
1
,
3
)
(19)
\mathbf{T}_{left}(1,3)=\mathbf{T}_{right}(1,3) \tag{19}
Tleft(1,3)=Tright(1,3)(19)
T l e f t ( 3 , 3 ) = T r i g h t ( 3 , 3 ) (20) \mathbf{T}_{left}(3,3)=\mathbf{T}_{right}(3,3) \tag{20} Tleft(3,3)=Tright(3,3)(20)
化简得:
s
i
n
θ
4
=
a
y
c
o
s
θ
1
−
a
x
s
i
n
θ
1
s
i
n
θ
5
(21)
\displaystyle sin\theta_4=\frac{a_ycos\theta_1-a_xsin\theta_1}{sin\theta_5} \tag{21}
sinθ4=sinθ5aycosθ1−axsinθ1(21)
c o s θ 4 = − a z c o s ( θ 2 + θ 3 ) − a x s i n ( θ 2 + θ 3 ) c o s θ 1 − a y s i n ( θ 2 + θ 3 ) s i n θ 1 s i n θ 5 (22) \displaystyle cos\theta_4=\frac{-a_zcos(\theta_2+\theta_3)-a_xsin(\theta_2+\theta_3)cos\theta_1-a_ysin(\theta_2+\theta_3)sin\theta_1}{sin\theta_5} \tag{22} cosθ4=sinθ5−azcos(θ2+θ3)−axsin(θ2+θ3)cosθ1−aysin(θ2+θ3)sinθ1(22)
如此可以确定解 θ 4 \theta_4 θ4。
利用矩阵元素对应相等关系:
T
l
e
f
t
(
2
,
2
)
=
T
r
i
g
h
t
(
2
,
2
)
(23)
\mathbf{T}_{left}(2,2)=\mathbf{T}_{right}(2,2) \tag{23}
Tleft(2,2)=Tright(2,2)(23)
T l e f t ( 2 , 1 ) = T r i g h t ( 2 , 1 ) (24) \mathbf{T}_{left}(2,1)=\mathbf{T}_{right}(2,1) \tag{24} Tleft(2,1)=Tright(2,1)(24)
化简为:
s
i
n
θ
6
=
o
x
c
o
s
(
θ
2
+
θ
3
)
c
o
s
θ
1
+
o
y
c
o
s
(
θ
2
+
θ
3
)
s
i
n
θ
1
−
o
z
s
i
n
(
θ
2
+
θ
3
)
s
i
n
θ
5
(25)
\displaystyle sin\theta_6=\frac{o_xcos(\theta_2+\theta_3)cos\theta_1+o_ycos(\theta_2+\theta_3)sin\theta_1-o_zsin(\theta_2+\theta_3)}{sin\theta_5} \tag{25}
sinθ6=sinθ5oxcos(θ2+θ3)cosθ1+oycos(θ2+θ3)sinθ1−ozsin(θ2+θ3)(25)
c o s θ 6 = n z s i n ( θ 2 + θ 3 ) − n x c o s ( θ 2 + θ 3 ) c o s θ 1 − n y c o s ( θ 2 + θ 3 ) s i n θ 1 s i n θ 5 (26) \displaystyle cos\theta_6=\frac{n_zsin(\theta_2+\theta_3)-n_xcos(\theta_2+\theta_3)cos\theta_1-n_ycos(\theta_2+\theta_3)sin\theta_1}{sin\theta_5} \tag{26} cosθ6=sinθ5nzsin(θ2+θ3)−nxcos(θ2+θ3)cosθ1−nycos(θ2+θ3)sinθ1(26)
由式(25)(26)可求得 θ 6 \theta_6 θ6,其解与 θ 5 \theta_5 θ5唯一对应。
————————————————————————————————————————————————
J
i
a
b
i
n
P
a
n
JiabinPan
JiabinPan 原创文章,禁止抄袭,转载请注明出处,谢谢
如果可以的话希望大家投稿论文时可以引用一下我的文章,哈哈,谢谢
[1] Pan, J., Fu, Z., Xiong, J. et al. RobMach: G-Code-based off-line programming for robotic machining trajectory generation. Int J Adv Manuf Technol 118, 2497–2511 (2022). https://doi.org/10.1007/s00170-021-08082-3