机器人正逆运动学分析(ABB-IRB2600)

笔记 专栏收录该内容
2 篇文章 0 订阅

IRB2600的标准DH法

建立IRB2600的标准DH模型:

机器人结构示意图

标准DH法中相邻坐标系之间的齐次变换矩阵:

机器人坐标结构图

IRB2600的标准DH参数表:

轴号 i i i α i − 1 \alpha_{i-1} αi1 a i − 1 a_{i-1} ai1 d i d_{i} di θ i \theta_{i} θi
100 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
30 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} 9000 θ 5 \theta_{5} θ5
6 9 0 ∘ 90^{\circ} 900 d 6 ( 85 ) d_{6}(85) d6(85) θ 6 \theta_{6} θ6

6 0 T = [ 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 ] = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T (10) ^{0}_{6}T=\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} ={{^{0}_{1}T}{^{1}_{2}T}{^2_{3}T}{^3_{4}T}{^4_{5}T}{^5_{6}T}} \tag{10} 60T=nxnynz0oxoyoz0axayaz0pxpypz1=10T21T32T43T54T65T(10)
其中:
1 0 T = [ c o s ( θ 1 ) − s i n ( θ 1 ) 0 0 s i n ( θ 1 ) c o s ( θ 1 ) 0 0 0 0 1 d 1 0 0 0 1 ] (11) ^{0}_{1}T=\begin{bmatrix} cos(\theta_{1}) & -sin(\theta_{1}) & 0 & 0\\ sin(\theta_{1}) & cos(\theta_{1}) & 0 & 0\\ 0 & 0 & 1 & d_{1}\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{11} 10T=cos(θ1)sin(θ1)00sin(θ1)cos(θ1)00001000d11(11)

KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ ^{1}_{2}T &=…

3 2 T = [ c o s ( θ 3 ) − s i n ( θ 3 ) 0 a 2 s i n ( θ 3 ) c o s ( θ 3 ) 0 0 0 0 1 0 0 0 0 1 ] (13) ^{2}_{3}T=\begin{bmatrix} cos(\theta_{3}) & -sin(\theta_{3}) & 0 & a_{2}\\ sin(\theta_{3}) & cos(\theta_{3}) & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{13} 32T=cos(θ3)sin(θ3)00sin(θ3)cos(θ3)000010a2001(13)

KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ ^{3}_{4}T &=\b…

5 4 T = [ c o s ( θ 5 ) − s i n ( θ 5 ) 0 0 0 0 1 0 − s i n ( θ 5 ) − c o s ( θ 5 ) 0 0 0 0 0 1 ] (15) ^{4}_{5}T=\begin{bmatrix} cos(\theta_{5}) & -sin(\theta_{5}) & 0 & 0\\ 0 & 0 & 1 & 0\\ -sin(\theta_{5}) & -cos(\theta_{5}) & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{15} 54T=cos(θ5)0sin(θ5)0sin(θ5)0cos(θ5)001000001(15)

6 5 T = [ c o s ( θ 6 ) − s i n ( θ 6 ) 0 0 0 0 − 1 − d 6 s i n ( θ 6 ) c o s ( θ 6 ) 0 0 0 0 0 1 ] (16) ^{5}_{6}T=\begin{bmatrix} cos(\theta_{6}) & -sin(\theta_{6}) & 0 & 0\\ 0 & 0 & -1 & -d_{6}\\ sin(\theta_{6}) & cos(\theta_{6}) & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{16} 65T=cos(θ6)0sin(θ6)0sin(θ6)0cos(θ6)001000d601(16)

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 (T^1_0)^{-1}T_{end}=T^2_1T^3_2T^4_3T^5_4T^6_5 (T01)1Tend=T12T23T34T45T56 θ 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 ) (1) \displaystyle\frac{T_{left}(2,4)}{T_{left}(2,3)}=\frac{T_{right}(2,4)}{T_{right}(2,3)}\tag{1} Tleft(2,3)Tleft(2,4)=Tright(2,3)Tright(2,4)(1)
化简得到:
θ 1 = a r c t a n a y d 6 − p y a x d 6 − p x (2) \displaystyle\theta_{1}=arctan\frac{a_yd_6-p_y}{a_xd_6-p_x}\tag2 θ1=arctanaxd6pxayd6py(2)
对于 θ 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 T_{end}(T^5_4T^6_5)^{-1}=T^1_0T^2_1T^3_2T^4_3 Tend(T45T56)1=T01T12T23T34,首先为确定 θ 2 \theta_2 θ2关于 θ 1 \theta_1 θ1的关系式,通过矩阵中如下元素的等式关系:
T l e f t ( 2 , 4 ) = T r i g h t ( 2 , 4 ) (3) \displaystyle T_{left}(2,4)=T_{right}(2,4) \tag3 Tleft(2,4)=Tright(2,4)(3)
T l e f t ( 3 , 4 ) = T r i g h t ( 3 , 4 ) (4) \displaystyle T_{left}(3,4)=T_{right}(3,4) \tag4 Tleft(3,4)=Tright(3,4)(4)
化简得:
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 (5) \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} \tag5 a3sin(θ2+θ3)d4cos(θ2+θ3)=a1a2sinθ2sinθ1pyd6ay(5)
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 ) (6) d_4sin(\theta_2+\theta_3)+a_3cos(\theta_2+\theta_3)=d_1-a_2cos(\theta_2)-(p_z-d_6a_z) \tag6 d4sin(θ2+θ3)+a3cos(θ2+θ3)=d1a2cos(θ2)(pzd6az)(6)
为化简需要,令 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),通过式(5)和(6)联立起来可以消除 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 (7) \displaystyle {a_3}^2+{d_4}^2=(k_1-a_2sin\theta_2)^2+(k_2-a_2cos\theta_2)^2 \tag7 a32+d42=(k1a2sinθ2)2+(k2a2cosθ2)2(7)
其中:
k 1 = a 1 − p y − d 6 a y s i n θ 1 (8) \displaystyle k_1=a_1-\frac{p_y-d_6a_y}{sin\theta_1} \tag8 k1=a1sinθ1pyd6ay(8)
k 2 = d 1 − ( p z − d 6 a z ) (9) \displaystyle k_2=d_1-(p_z-d_6a_z) \tag9 k2=d1(pzd6az)(9)
观察发现,式(7)中只有 θ 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 − ϕ (10) \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{10} θ2=arcsin2a2k12+k22 k12+k22+a22(a32+d42)ϕ(10)
s i n ϕ = k 2 k 1 2 + k 2 2 , c o s ϕ = k 1 k 1 2 + k 2 2 (11) \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{11} sinϕ=k12+k22 k2cosϕ=k12+k22 k1(11)
对于 θ 2 \theta_2 θ2的计算,亦会产生两个解。

以上求出了 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2,将式(5)(6)中 X X X Y Y Y作为未知数,求出:
X = s i n ( θ 2 + θ 3 ) = a 3 A + d 4 B a 3 2 + d 4 2 (12) \displaystyle X=sin(\theta_2+\theta_3)=\frac {a_3A+d_4B}{{a_3}^2+{d_4}^2} \tag{12} X=sin(θ2+θ3)=a32+d42a3A+d4B(12)
Y = c o s ( θ 2 + θ 3 ) = a 3 B − d 4 A a 3 2 + d 4 2 (13) \displaystyle Y=cos(\theta_2+\theta_3)=\frac {a_3B-d_4A}{{a_3}^2+{d_4}^2} \tag{13} Y=cos(θ2+θ3)=a32+d42a3Bd4A(13)
其中:
A = k 1 − a 2 s i n θ 2 , B = k 2 − a 2 c o s θ 2 (14) \displaystyle A = k_1-a_2sin\theta_2 ,B=k_2-a_2cos\theta_2 \tag{14} A=k1a2sinθ2B=k2a2cosθ2(14)
结合式(12)(13)(14)和已算出的 θ 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 T_{right} Tright矩阵中元素表达式只含 θ 4 θ 5 θ 6 \theta_4\theta_5\theta_6 θ4θ5θ6且简单, T l e f t 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 (T^1_0T^2_1T^3_2)^{-1}T_{end}=T^4_3T^5_4T^6_5 (T01T12T23)1Tend=T34T45T56为推导 θ 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 ) (15) \displaystyle T_{left}(2,3)=T_{right}(2,3) \tag{15} Tleft(2,3)=Tright(2,3)(15)
可以直接求得 θ 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 ) ) (16) \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{16} θ5=arccos(axcos(θ2+θ3)cosθ1+aycos(θ2+θ3)sinθ1azsin(θ2+θ3))(16)
用该解析解计算 θ 5 \theta_5 θ5,会得到两个解。

为求得 θ 4 \theta_4 θ4,可建立如下关系式:
T l e f t ( 1 , 3 ) = T r i g h t ( 1 , 3 ) (17) T_{left}(1,3)=T_{right}(1,3) \tag{17} Tleft(1,3)=Tright(1,3)(17)
T l e f t ( 3 , 3 ) = T r i g h t ( 3 , 3 ) (18) T_{left}(3,3)=T_{right}(3,3) \tag{18} Tleft(3,3)=Tright(3,3)(18)
化简得:
s i n θ 4 = o y c o s θ 1 − o x s i n θ 1 s i n θ 5 (19) \displaystyle sin\theta_4=\frac{o_ycos\theta_1-o_xsin\theta_1}{sin\theta_5} \tag{19} sinθ4=sinθ5oycosθ1oxsinθ1(19)
c o s θ 4 = − o z c o s ( θ 2 + θ 3 ) − o x s i n ( θ 2 + θ 3 ) c o s θ 1 − o y s i n ( θ 2 + θ 3 ) s i n θ 1 s i n θ 5 (20) \displaystyle cos\theta_4=\frac{-o_zcos(\theta_2+\theta_3)-o_xsin(\theta_2+\theta_3)cos\theta_1-o_ysin(\theta_2+\theta_3)sin\theta_1}{sin\theta_5} \tag{20} cosθ4=sinθ5ozcos(θ2+θ3)oxsin(θ2+θ3)cosθ1oysin(θ2+θ3)sinθ1(20)
如此可以确定解 θ 4 \theta_4 θ4

利用矩阵元素对应相等关系:
T l e f t ( 2 , 2 ) = T r i g h t ( 2 , 2 ) (21) T_{left}(2,2)=T_{right}(2,2) \tag{21} Tleft(2,2)=Tright(2,2)(21)
T l e f t ( 2 , 1 ) = T r i g h t ( 2 , 1 ) (22) T_{left}(2,1)=T_{right}(2,1) \tag{22} Tleft(2,1)=Tright(2,1)(22)
化简为:
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 (23) \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{23} sinθ6=sinθ5oxcos(θ2+θ3)cosθ1+oycos(θ2+θ3)sinθ1ozsin(θ2+θ3)(23)
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 (24) \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{24} cosθ6=sinθ5nzsin(θ2+θ3)nxcos(θ2+θ3)cosθ1nycos(θ2+θ3)sinθ1(24)
由式(23)(24)可求得 θ 6 \theta_6 θ6,其解与 θ 5 \theta_5 θ5唯一对应。

——————————————————————————-——————————————————————

J i a b i n P a n JiabinPan JiabinPan 原创文章,禁止抄袭,转载请注明出处,谢谢

  • 8
    点赞
  • 6
    评论
  • 28
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值