机器人正逆运动学分析（ABB |
您所在的位置：网站首页 › puma机器人dh参数表 › 机器人正逆运动学分析（ABB |
机器人正逆运动学分析（ABB
|
IRB2600的改进D-H法
建立IRB2600的改进D-H模型： 
Fig.1 ABB IRB2600 coordinate system
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} θi100 d 1 ( 445 ) d_{1}(445) d1(445) θ 1 \theta_{1} θ12 − 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} θ34 9 0 ∘ 90^{\circ} 90∘ a 3 ( − 115 ) a_{3}(-115) a3(−115) d 4 ( 795 ) d_{4}(795) d4(795) θ 4 \theta_{4} θ45 − 9 0 ∘ -90^{\circ} −90∘00 θ 5 \theta_{5} θ56 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+k22 k12+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+k22 k2，cosϕ=k12+k22 k1(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 |
【本文地址】
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |