Xây dựng bảng Denavit-Hartenberg

Ma trâ ̣n biến đổi

i−1
cos θi −sin θi cos α i sin θi sin α i ai cos θi

i
[
T = sin θi cos θi cos α i−cos θi sin α i a i sin θi
0 sin α i cos α i d i
0 0 01
]
0
cos θ 1 0 sin θ1 0

❑
[
T 1= sin θ1 0−cos θ1 0
0100
0001
]
1
cos θ2−sin θ2 0 l 2 cos θ 2

❑
[
T 2= sin θ 2 cos θ 2 0 l 2 sin θ 2
0 0 1 l1
0001
]
0
cos θ1 . cos θ 2−cos θ1 . sinθ 2 sin θ1 l 2 . cos θ1 . cos θ2 +sin θ1 . l 1

❑
[
T 2= sin θ1 . cos θ2−sin θ 1 . sin θ2−cos θ 1 l 2 sinθ 1 . cos θ2−cos θ1 .l 1
sin θ2 cos θ2 0l 2 .sin θ 2
0001
]
 x 2=l 2 C 2 v 2 x =−l 2 S2 q̇2

{ y 2=l 2 S2 ¿>
z2 =l 2 C 2 S1 {
y 2 y =l2 C 2 q̇ 2
v2 z=−l 2 S 2 S1 q̇ 2 +l 2 C 2 C 1 q̇1

1 1
K 1= m 1 v 21= m1 ( v 2x + v 2y + v 2z )=0
2 2 1 1 1

1 1
K 2= m 2 v 22= m2 ( v 2x + v 2y + v 2z )
2 2 2 2 2

1
¿ m 2 [ ( −l 2 S2 q̇2 )2 + ( l 2 C2 q̇2 )2 + (−l 2 S 2 S1 q̇ 2 +l 2 C 2 C 1 q̇1 )2 ]
2

1
¿ m 2 [ l 22 q̇22 S 22+ l 22 q̇ 22 C 22+l 22 q̇ 22 S22 S 21+l 22 q̇12 C 12 C 21−2l 2 S 2 S1 q̇ 2 q̇ 1 l 2 C 2 C 1 ]
2

1 2 2 2 2 2 2 2 2 2 2 2
= m2 [ l2 q̇2 +l2 q̇2 S 2 S1 +l 2 q̇1 C2 C1−2 l 2 q̇1 q̇2 S 2 S1 C2 C1 ]
2
P1=0

P2=m2 g y 2 =m2 g l 2 S2

L=K −P=K 1 + K 2 −P 1−P2

1
¿ m 2 [ l 22 q̇22 +l 22 q̇22 S 22 S21 +l 22 q̇21 C 22 C21−2 l 22 q̇1 q̇2 S 2 S1 C2 C1 ] −m2 g l 2 S2
2

∂L
=m 2 q̇1 l 22 C22 C21 −m2 l 22 q̇ 2 S 2 S1 C 2 C 1
∂ q̇1

∂L
=m 2 l 22 q̇2 +m 2 l 22 q̇❑2 S22 S 21−m 2 l 22 q̇1 S 2 S1 C2 C1
∂ q̇2

d ∂L
( )
dt ∂ q̇1
=m2 l 22 [ (−2 C2 S 2 q̇2 ) C21 q̇1 +C 22 (−2 C 1 S1 q̇1 ) q̇1 +C 22 C 21 q̈1 ]−m2 l 22 [−S2 C1 S 2 S1 q̇ 2−S 2 S1 C2 S 1 q̇2 +C 2 C 1 C 2 S1 q̇2

d ∂L
( )
dt ∂ q̇2
=m2 l 22 [ ( 2 C2 S 2 q̇ 2 ) q̇2 + S22 q̈2 ] +m2 l 22 [ (−2C 2 S2 q̇2 ) q̇ 2+C 22 q̈2 ] +m2 l 22 [ ( 2 C 2 S2 q̇2 ) S 21 q̇2 + S22 (2 C1 S 1 q̇ 1) q̇ 2+ S22 S 21 q̈2

d ∂L 1
( ) = m l 2 S 2 ( 2 S1 C1 ) q̇22 +l 22 C22 (−2 C1 S 1 ) C 21 q̇21−2 l 22 C 2 S2 q̇1 q̇ 2 (−S 21+C 21 )
dt ∂ q1 2 2 2 2 [ ]

d ∂L 1
( ) = m l 2 (2 S 2 C 2) q̇ 22+l 22 (−2 S2 C2 ) q̇2 +l 22 (2 S2 C2 ) S 21 q̇ 22+l 22 (−2 S2 C2 )C 21 q̇21−2 l 22 C 1 S1 q̇1 q̇2 (−S 22+C 22 )
dt ∂ q2 2 2 2 [ ]