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

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

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

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

$^{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​=i+1iR iwi​+θ˙i+1 i+1Zi+1(5-45)
$^{i+1}v_{i+1}=^{i+1}_iR(^iv_i+^iw_i\times^iP_{i+1}) \tag{5-47}$i+1vi+1​=ii+1​R(ivi​+iwi​×iPi+1​)(5-47)

• 连杆i+1为移动关节时有
  $^{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+1​R iwi​i+1vi+1​=ii+1​R(ivi​+iwi​×iPi+1​)+d˙i+1​ i+1Zi+1​(5-48)

例子

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

连杆间的旋转变换矩阵为
$^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]$10​R=⎣⎡​cosθ1​sinθ1​0​−sinθ1​cosθ10​001​⎦⎤​,21​R=⎣⎡​cosθ2​sinθ2​0​−sinθ2​cosθ20​001​⎦⎤​,32​R=⎣⎡​100​010​001​⎦⎤​

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

$^1v_1=\left [ \begin{matrix} 0 \\ 0 \\ 0 \end {matrix}\right] \tag{5-51}$1v1​=⎣⎡​000​⎦⎤​(5-51)
$^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​=12​R 1w1​+θ˙2​ 2Z2​=⎣⎡​00θ˙1​+θ˙2​​⎦⎤​(5-52)
$^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​=12​R(1v1​+1w1​×1P2​)=⎣⎡​c2​−s2​0​s2​c2​0​001​⎦⎤​⎝⎛​⎣⎡​000​⎦⎤​+⎣⎡​00θ˙1​​⎦⎤​×⎣⎡​0l1​0​⎦⎤​⎠⎞​=⎣⎡​l1​s2​θ˙1​l1​c2​θ1​0​⎦⎤​(5-53)

$^3w_3=^3_2R ^2w_2+\dot \theta_2\ ^2\widehat Z_2=^2w_2 \tag{5-54}$3w3​=23​R2w2​+θ˙2​ 2Z2​=2w2​(5-54)

$^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​=23​R(2v2​+2w2​×2P3​)=⎣⎡​l1​s2​θ˙1​l1​c2​θ˙1​+l2​(θ˙1​+θ˙2​)0​⎦⎤​(5-55)
求末端速度相对于固定基坐标系的变换
$^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}$30​R=10​R 21​R 32​R=⎣⎡​c12​s12​0​−s12​c12​0​001​⎦⎤​(5-56)
通过这个变换得到
$^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​=⎣⎡​−l1​s1​θ˙1​−l2​s12​(θ˙1​+θ˙2​)l1​c1​θ˙1​+l2​c12​(θ˙1​+θ˙2​)0​⎦⎤​(5-57)

参考文献

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