机械手末端速度计算（实例）

灰太狼 2022-12-08 14:21 114阅读 0赞

# 机械手末端速度计算(实例) #

上一篇博文已经推导了相邻连杆i和连杆i+1间速度的传递

*  连杆i+1为旋转关节时有

i + 1 w i + 1 = i i + 1 R   i w i + θ ˙ i + 1   i + 1 Z ^ i + 1 (5-45) ^\{i+1\}w\_\{i+1\}=^\{i+1\}\_iR \\ ^iw\_i+\\dot\\theta\{i+1\}\\ ^\{i+1\}\\widehat Z\{i+1\} \\tag\{5-45\} i\+1wi\+1=ii\+1R iwi\+θ˙i\+1 i\+1Zi\+1(5\-45)

i + 1 v i + 1 = i i + 1 R ( i v i + i w i × i P i + 1 ) (5-47) ^\{i+1\}v\_\{i+1\}=^\{i+1\}\_iR(^iv\_i+^iw\_i\\times^iP\_\{i+1\}) \\tag\{5-47\} i\+1vi\+1=ii\+1R(ivi\+iwi×iPi\+1)(5\-47)

*  连杆i+1为移动关节时有

i + 1 w i + 1 = i i + 1 R   i w i i + 1 v i + 1 = i i + 1 R ( i v i + i w i × i P i + 1 ) + d ˙ i + 1   i + 1 Z ^ i + 1 (5-48) ^\{i+1\}w\_\{i+1\}=^\{i+1\}\_iR\\ ^iw\_i \\\\ ^\{i+1\}v\_\{i+1\}=^\{i+1\}\_iR(^iv\_i+^iw\_i\\times ^iP\_\{i+1\})+\\dot d\_\{i+1\}\\ ^\{i+1\}\\widehat Z\_\{i+1\} \\tag\{5-48\} i\+1wi\+1=ii\+1R iwii\+1vi\+1=ii\+1R(ivi\+iwi×iPi\+1)\+d˙i\+1 i\+1Zi\+1(5\-48)

## 例子 ##

如下图所示，计算出操作臂末端的速度，将它表达成关节速度的函数。给出两种形式的解答，一种是用坐标系\{3\}表示，另一种是用坐标系\{0\}表示。

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]

连杆间的旋转变换矩阵为  
 1 0 R = \[ cos ⁡ θ 1 − sin ⁡ θ 1 0 sin ⁡ θ 1 cos ⁡ θ 1 0 0 0 1 \] 2 1 R = \[ cos ⁡ θ 2 − sin ⁡ θ 2 0 sin ⁡ θ 2 cos ⁡ θ 2 0 0 0 1 \] 3 2 R = \[ 1 0 0 0 1 0 0 0 1 \] ^0\_1R= \\left \[ \\begin\{matrix\} \\cos\\theta\_1 & -\\sin\\theta\_1 & 0\\\\ \\sin\\theta\_1 & \\cos\\theta1 & 0 \\\\ 0 & 0 & 1 \\end \{matrix\} \\right\] \\\\ ^1\_2R= \\left \[ \\begin\{matrix\} \\cos\\theta\_2 & -\\sin\\theta\_2 & 0\\\\ \\sin\\theta\_2 & \\cos\\theta2 & 0 \\\\ 0 & 0 & 1 \\end \{matrix\} \\right\] \\\\ ^2\_3R= \\left \[ \\begin\{matrix\} 1 & 0 & 0\\\\ 0 & 1 & 0 \\\\ 0 & 0 & 1 \\end \{matrix\} \\right\] 10R=⎣⎡cosθ1sinθ10−sinθ1cosθ10001⎦⎤21R=⎣⎡cosθ2sinθ20−sinθ2cosθ20001⎦⎤32R=⎣⎡100010001⎦⎤

对连杆依次使用上一篇博文中的式（5-45）和（5-47）,就有  
 1 w 1 = \[ 0 0 θ ˙ 1 \] (5-50) ^1w\_1=\\left \[ \\begin \{matrix\} 0 \\\\ 0 \\\\ \\dot \\theta\_1 \\end \{matrix\} \\right \] \\tag\{5-50\} 1w1=⎣⎡00θ˙1⎦⎤(5\-50)

1 v 1 = \[ 0 0 0 \] (5-51) ^1v\_1=\\left \[ \\begin\{matrix\} 0 \\\\ 0 \\\\ 0 \\end \{matrix\}\\right\] \\tag\{5-51\} 1v1=⎣⎡000⎦⎤(5\-51)

2 w 2 = 1 2 R   1 w 1 + θ ˙ 2   2 Z ^ 2 = \[ 0 0 θ ˙ 1 + θ ˙ 2 \] (5-52) ^2w\_2=^2\_1R\\ ^1w\_1+\\dot \\theta\_2\\ ^2\\widehat Z\_2=\\left \[ \\begin \{matrix\} 0 \\\\ 0 \\\\ \\dot\\theta\_1+\\dot \\theta\_2 \\end \{matrix\}\\right \] \\tag\{5-52\} 2w2=12R 1w1\+θ˙2 2Z2=⎣⎡00θ˙1\+θ˙2⎦⎤(5\-52)

2 v 2 = 1 2 R ( 1 v 1 + 1 w 1 × 1 P 2 ) = \[ c 2 s 2 0 − s 2 c 2 0 0 0 1 \] ( \[ 0 0 0 \] + \[ 0 0 θ ˙ 1 \] × \[ 0 l 1 0 \] ) = \[ l 1 s 2 θ ˙ 1 l 1 c 2 θ 1 0 \] (5-53) ^2v\_2=^2\_1R(^1v\_1+^1w\_1\\times ^1P\_2) =\\left \[\\begin\{matrix\} c\_2 &s\_2& 0 \\\\ -s\_2& c\_2 &0 \\\\ 0 & 0& 1 \\end \{matrix\}\\right\] \\left ( \\left\[ \\begin\{matrix\} 0\\\\ 0 \\\\ 0 \\end\{matrix\} \\right\] +\\left\[ \\begin\{matrix\} 0\\\\ 0 \\\\ \\dot\\theta\_1 \\end\{matrix\} \\right\] \\times \\left\[ \\begin\{matrix\} 0\\\\ l\_1\\\\ 0 \\end\{matrix\} \\right\] \\right)\\\\ =\\left \[ \\begin\{matrix\} l\_1s\_2\\dot \\theta\_1 \\\\ l\_1c\_2\\theta\_1 \\\\ 0 \\end\{matrix\}\\right\] \\tag\{5-53\} 2v2=12R(1v1\+1w1×1P2)=⎣⎡c2−s20s2c20001⎦⎤⎝⎛⎣⎡000⎦⎤\+⎣⎡00θ˙1⎦⎤×⎣⎡0l10⎦⎤⎠⎞=⎣⎡l1s2θ˙1l1c2θ10⎦⎤(5\-53)

3 w 3 = 2 3 R 2 w 2 + θ ˙ 2   2 Z ^ 2 = 2 w 2 (5-54) ^3w\_3=^3\_2R ^2w\_2+\\dot \\theta\_2\\ ^2\\widehat Z\_2=^2w\_2 \\tag\{5-54\} 3w3=23R2w2\+θ˙2 2Z2=2w2(5\-54)

3 v 3 = 2 3 R ( 2 v 2 + 2 w 2 × 2 P 3 ) = \[ l 1 s 2 θ ˙ 1 l 1 c 2 θ ˙ 1 + l 2 ( θ ˙ 1 + θ ˙ 2 ) 0 \] (5-55) ^3v\_3=^3\_2R(^2v\_2+^2w\_2\\times ^2P\_3)=\\left \[ \\begin\{matrix\} l\_1s\_2\\dot \\theta\_1 \\\\ l\_1c\_2\\dot\\theta\_1+l\_2(\\dot\\theta\_1+\\dot\\theta\_2) \\\\ 0 \\end\{matrix\} \\right\]\\tag\{5-55\} 3v3=23R(2v2\+2w2×2P3)=⎣⎡l1s2θ˙1l1c2θ˙1\+l2(θ˙1\+θ˙2)0⎦⎤(5\-55)

求速度相对于固定极坐标系的变换  
 3 0 R = 1 0 R   2 1 R   3 2 R = \[ c 12 − s 12 0 s 12 c 12 0 0 0 1 \] (5-56) ^0\_3R=^0\_1R\\ ^1\_2R\\ ^2\_3R=\\left\[\\begin\{matrix\} c\_\{12\} &-s\_\{12\} &0\\\\ s\_\{12\} &c\_\{12\} &0 \\\\ 0 & 0& 1 \\end\{matrix\}\\right\]\\tag\{5-56\} 30R=10R 21R 32R=⎣⎡c12s120−s12c120001⎦⎤(5\-56)  
通过这个变换得到  
 0 v 3 = \[ − l 1 s 1 θ ˙ 1 − l 2 s 12 ( θ ˙ 1 + θ ˙ 2 ) l 1 c 1 θ ˙ 1 + l 2 c 12 ( θ ˙ 1 + θ ˙ 2 ) 0 \] (5-57) ^0v\_3=\\left\[\\begin\{matrix\} -l\_1s\_1\\dot\\theta\_1-l\_2s\_\{12\}(\\dot\\theta\_1+\\dot\\theta\_2) \\\\ l\_1c\_1\\dot\\theta\_1+l\_2c\_\{12\}(\\dot\\theta\_1+\\dot\\theta\_2) \\\\0 \\end\{matrix\}\\right\]\\tag\{5-57\} 0v3=⎣⎡−l1s1θ˙1−l2s12(θ˙1\+θ˙2)l1c1θ˙1\+l2c12(θ˙1\+θ˙2)0⎦⎤(5\-57)

参考文献

\[1\] JOHN J.CRAIG. 机器人学导论: 第3版\[M\]. 机械工业出版社, 2006.

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/fecb92d68a93483281525216f5930eba.png