六自由度运动方程计算

S2 C2 0 0
0 − l 2 C1 0 0 0 1 0 S1 0 1 0
S1 0 − C1 0
0 1 0 0
0 0 0 1
n x n y nz 0
ox oy oz 0
ax ay az 0
C1 C23 S = 1 C1 S 23 0 根据
1 =90 2 =0 3 =90 4 =-90 5 =90 6 =0
n
杆长 ln (mm) l1 =0 l2 = l3 =0 l4 =0 l5 =0 l6 =0
)
Cθn Sθ An = n 0 0 C1 S A1 = 1 0 0
− Sθ n Cαn Cθ n Cαn Sαn 0 0 S1 0 − C1 1 0 0 0
− l 2C3 n x ox ax px 0 n y oy a y p y − l2 S3 nz oz az pz 1 0 0 0 1 等式两边矩阵的第四列的第一 三行对应相等 则有 S1C23 − C1 S1 S23 0
3
C4 0 −1 A4 = − S 4 0
0 d4 0 1
由 A1−1 T6 = A2 A3 A4 A5 A6 C1 0 左边 = S1 0 S1 0 − C1 0 0 1 0 0
0 n x 0 ny 0 n z 1 0
n x n 0 由正运动学方程 T6 = T6 = A1 A2 A3 A4 A5 A6 = y nz 0
2
ox oy oz 0
ax ay az 0
px py 可得 pz 1
n x = C1 [C23 (C4 C5 C 6 − S 4 S 6 ) − S 23 S 5 C 6 ] + S1 (C 4 S 6 + S 4 C5 C6 ) n y = − C1 (C4 S 6 + S 4 C5 C 6 ) + S1 [C 23 (C 4 C5 C6 − S 4 S 6 ) − S 23 S 5 C6 ] n z = C23 S 5 C 6 + S 23 (C4 C5 C 6 − S 4 S 6 ) o x = C1 [−C 23 (C 4 C5 S 6 + S 4 C6 ) + S 23 S 5 S 6 ] + S1 ( C4 C6 − S 4 C5 S 6 ) o y = − C1 (C4 C6 − S 4 C5 S 6 ) + S1 [−C 23 (C 4 C5 S 6 + S 4 C6 ) + S 23 S 5 S 6 ] o z = − C23 S 5 S 6 − S 23 (C 4 C 5 S 6 + S 4 C6 ) a x = C1 (C23 C 4 S 5 + S 23 C5 ) + S1 S 4 S 5 a y = − C1 S 4 S 5 + S1 (C 23 C4 S 5 + S 23 C5 ) a z = − C23 C5 + S 23 C4 S 5 p x = C1 (l 2 C2 + d 4 S 23 ) p y = S1 (l 2 C 2 + d 4 S 23 ) p z = l 2 S 2 − d 4 C23 三 机器人逆运动学分析 S1 0 − C1 0 S3 0 − C3 0 0 1 0 0 0 0 0 1 0 1 0 0 C2 − S −1 A2 = 2 0 0 0 0 0 1 S2 C2 0 0 0 0 1 0 S4 0 C4 0 − l2 0 0 1 0 −1 0 0 C1 0 A1−1 = S1 0 C 3 0 −1 A3 = S3 0 １ 求 θ1 ox oy oz 0 ax ay az 0 px py pz 1
T6 = A3 A4 A5 A6
− C 3 (C 4 C 5 S 6 + S 4C 6 ) + S3 S5 S 6 − S 3 (C 4 C 5 S 6 + S 4C 6 ) − C3 S5 S 6 − S 4 C 5 S 6 + C 4 C6 0 C 3 C 4 S 5 + S 3C 5 S 3C 4 S 5 − C 3C 5 S4 S5 0 S3d4 − C3 d 4 0 1
C 3 (C 4 C 5C 6 − S 4 S 6 ) − S 3 S 5C 6 = S3 (C 4C 5 C 6 − S 4 S 6 ) + C 3 S 5C 6 S 4 C 5C 6 + C 4 S6 0
1
T6 = A2 A3 A4 A5 A6
− C23 (C4C5S 6 + S 4 C6 ) + S 23S 5 S 6 − S 23( C4C5S 6 + S 4C6 ) − C23S 5 S 6 − S 4C5S 6 + C4C6 0 C23C4S 5 + S 23C5 S 23C4S 5 − C23C5 S4 S5 0 l2C2 + S 23d 4 l2 S 2 − C23d 4 0 1
− C5 S 6 − S5 S6 C6 0
0 0 0 1 0 0 d4 1
C4 C5 C6 − S 4 S 6 S C C + C S 4 6 3 T6 = A4 A5 A6 = 4 5 6 − S 5C6 0
2
− C4C5 S 6 − S 4 C6 C4 S5 − S 4 C5 S6 + C4 C6 S4 S5 S5 S6 C5 0 0
求 θ2 px py pz 1
−1 由 A3 A2−1 A1−1T6 = A4 A5 A6
C3 0 左边 = S3 0
S3 0 − C3 0
0 1 0 0
0 C 2 0 − S 2 0 0 1 0 S23 0 − C23 0
C1 n x + S 1 n y nz = S1 n x − C1 n y 0 根据
C1 o x + S1 o y oz S 1 ox − C1 o y 0
C1 a x + S1 a y az S1 a x − C1 a y 0
C1 p x + S1 p y pz S1 p x − C1 p y 1 则有
等式两边矩阵的第四列的前三行对应相等
C1 p x + S1 p y = d 4 S 23 + l 2 C 2 p z = −d 4 C 23 + l 2 S 2 S1 p x − C1 p y = 0 由 ２ 将 得 求 θ3
C23( C4C5C6 − S 4 S 6 ) − S 23S 5C6 = S 23 (C4 C5C6 − S 4 S 6 ) + C23S 5C6 S 4C5C6 + C4 S 6 0
0
T6 = A1 A2 A3 A4 A5 A6
C1[ C23 (C 4C5 C6 − S4 S6 ) − S 23S5C 6 ] + S1( S 4C5 C6 + C4 S6 ) C1 [ −C23 ( C4C 5S 6 + S 4C6 ) + S23S 5S6 ] − S1 ( S4 C5 S6 − C 4C 6 ) C1 (C 23C 4 S5 + S23C 5 ) + S1S 4 S5 C1 ( l2C 2 + S 23d4 ) S1[ C23 ( C4C 5C6 − S4 S6 ) − S23S5 C6 ] − C1( S 4C5 C6 + C 4 S6 ) S1 [ −C 23( C4 C5 S6 + S4C 6 ) + S 23S5S 6 ] + C1 ( S4 C5 S6 − C 4C 6 ) S1 ( C23C4 S5 + S23C5 ) − C1S 4S 5 S1( l2 C2 + S23d 4 ) = S23 ( C4C 5C6 − S4 S6 ) + C23S5C 6 S23C4 S5 − C23C5 l2 S2 − C 23d4 − S23 (C 4C 5S 6 + S 4C6 ) − C 23S5 S6 0 0 0 1
4
联立
有
S 23 =
( d 4 + l 2 S3 ) A + p z l 2 C 3 A2 + p z
2
C23 =
Al2 C3 − ( d 4 + l 2 S 3 ) p z A2 + pz
2
其中 A = p x C1 + p y S 1
令 U = ( d 4 + l 2 S 3 ) A + p z l 2 C3
C 4 S A4 = 4 0 0
0 − S4 0 C4 −1 0 0 0
0 S5 0 − C5 1 0 0 0
0 C6 S 0 ， A6 = 6 0 0 1 0
− S6 C6 0 0
0 0 1 0
0 0 ， 0 1
C5 C6 S C 4 T6 = A5 A6 = 5 6 S6 0
２
θ1
a tan 2( p y , p x )
代入
２
２
中得
2d 4 l 2
Sinθ3 = K
其中 K =
2 2 2 2 p2 x + p y + p z − d 4 − l2
（则 Cosθ3 = ± 1 − K 2 ） 所以 ３ θ3 = arcsin K ，（ θ3 = a tan 2( K ,± 1 − K 2 ) ）
0 S3 0 − C3 1 0 0 0
0 0 ， 0 1
0 C5 S 0 ， A5 = 5 0 d4 1 0 由连杆 ６ 开始，依次导出下列方程： C6 S 5 T6 = A6 = 6 0 0 − S6 C6 0 0 0 0 1 0 0 0 0 1 S5 − C5 0 0
机器人运动学分析
一 机器人齐次坐标系的建立
D—H 坐Байду номын сангаас图
二
机器人正运动学分析
杆件 1 2 3 4 5 6 变量为转角 n (图示角度值) 1 (0)
2 (90 3 (0) 4 (0) 5 (0) 6 (0)
偏距 dn (mm) d1 =0 d2 =0 d3 =0 d4 = d5 =0 d6 =0
扭角
p x C1 C23 + p y S1 C23 + p z S 23 − l 2 C3 = 0 p x C1 S 23 + p y S1 S 23 − p z C 23 − l 2 S 3 = d 4
Sθn Sαn − Cθn Sαn Cαn 0
l n Cθn l n Sθn dn 1 − S2 C2 0 0
1
0 C 2 0 ， A2 = S 2 0 0 1 0
0 l 2 C2 C3 0 l2 S 2 ， A3 = S 3 0 1 0 0 1 0