机械手末端速度计算(实例)
上一篇博文已经推导了相邻连杆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
+
1
w
i
+
1
=
i
i
+
1
R
i
w
i
+
θ
˙
i
+
1
i
+
1
Z
i
+
1
(
5
–
4
5
)
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
+
1
v
i
+
1
=
i
i
+
1
R
(
i
v
i
+
i
w
i
×
i
P
i
+
1
)
(
5
–
4
7
)
- 连杆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
+
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
–
4
8
)
例子
如下图所示，计算出操作臂末端的速度，将它表达成关节速度的函数。给出两种形式的解答，一种是用坐标系{3}表示，另一种是用坐标系{0}表示。
连杆间的旋转变换矩阵为
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]
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
⎦
⎤
对连杆依次使用上一篇博文中的式（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}
1
w
1
=
⎣
⎡
0
0
θ
˙
1
⎦
⎤
(
5
–
5
0
)
1
v
1
=
[
0
0
0
]
(5-51)
^1v_1=\left [ \begin{matrix} 0 \\ 0 \\ 0 \end {matrix}\right] \tag{5-51}
1
v
1
=
⎣
⎡
0
0
0
⎦
⎤
(
5
–
5
1
)
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}
2
w
2
=
1
2
R
1
w
1
+
θ
˙
2
2
Z
2
=
⎣
⎡
0
0
θ
˙
1
+
θ
˙
2
⎦
⎤
(
5
–
5
2
)
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}
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
–
5
3
)
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}
3
w
3
=
2
3
R
2
w
2
+
θ
˙
2
2
Z
2
=
2
w
2
(
5
–
5
4
)
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}
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
–
5
5
)
求速度相对于固定极坐标系的变换
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}
3
0
R
=
1
0
R
2
1
R
3
2
R
=
⎣
⎡
c
1
2
s
1
2
0
−
s
1
2
c
1
2
0
0
0
1
⎦
⎤
(
5
–
5
6
)
通过这个变换得到
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}
0
v
3
=
⎣
⎡
−
l
1
s
1
θ
˙
1
−
l
2
s
1
2
(
θ
˙
1
+
θ
˙
2
)
l
1
c
1
θ
˙
1
+
l
2
c
1
2
(
θ
˙
1
+
θ
˙
2
)
0
⎦
⎤
(
5
–
5
7
)
参考文献
[1] JOHN J.CRAIG. 机器人学导论: 第3版[M]. 机械工业出版社, 2006.