BÀI TẬP LỚN CHUYÊN ĐỀ
ĐỘNG LỰC HỌC HỆ NHIỀU VẬT
NHÓM 2
Câu 1: Thiết lập phương trình vi phân chuyển động của các robot sau
a. Robot 2 khâu như hình dưới:

Tọa độ suy rộng của cơ hệ: q = [q1 , q2 ]T
Từ hình vẽ ta có vị trí tọa độ khối tâm của hai khâu:
l1 cos q1 + 0.5l2 cos ( q1 + q2 ) 
0.5l1 cos q1 




r1 =  0.5l1 sin q1  , r2 =  l1 sin q1 + 0.5l2 sin ( q1 + q2 ) 




0
0

(1)

Matrix Jacobian tịnh tiến:
 −0.5l1 sin q1 0 
∂r1 
J T1 =
=  0.5l1 cos q1 0 
∂q


0
0 
 −l1 sin q1 − 0.5l2 sin ( q1 + q2 )
∂r2 
J T2 =
= l1 cos q1 + 0.5l2cos ( q1 + q2 )
∂q 

0

(2)
−0.5l2 sin ( q1 + q2 ) 

0.5l2cos ( q1 + q2 ) 

0

Vận tốc góc các khâu:

1

(3)


0
 
ω1 =  0  ,
.
q 
 1


 0 


ω2 =  0 
.

.
q + q 
 1 2

(4)

Vậy matrix Jacobian quay của các khâu:
0 0 
J R1 = . = 0 0  ,
∂ q 1 0 


∂ω1

JR2

0 0 
= . = 0 0 
∂ q 1 1 


∂ω2


(5)

Matrix tenxo quán tính của các khâu đối với khối tâm của chúng có dạng:
0 0 0 
I1 = 0 0 0  ,
0 0 I1 

0 0 0 
I 2 =  0 0 0 
 0 0 I 2 

(6)

Biểu thức matrix khối lượng suy rộng của hệ có dạng:
2

M ( q ) = ∑ ( J TTi mi JTi + J TRi I i J Ri )

(7)

i =1

Thế các biểu thức (2) (3) (5) (6) vào (7) ta được:
 l12
 2 l22

m
+
m
+ l1l2 cos q2 ÷+ I1 + I 2

 1
2  l1 +
4
4


M ( q) = 

 l2 l l


m2  2 + 1 2 cos q2 ÷+ I 2

4 4



 l2 l l

m2  2 + 1 2 cos q2 ÷+ I 2 
4 4


2
 (8)
l2

m2 + I 2
4



Biểu thức động năng:
.
1 .T
T = q M ( q) q
2

Thay matrix khối lượng suy rộng vào ta có:
T=

 . 2  l22
. . 
 2 l22

 . 2   l22

1   l12
m
+
m
l
+
+
l
l
cos
q
+
I
+

I
q
+
m
+
I
+ l1l2 cos q2 ÷+ 2I 2  q1 q 2 
 1
 2
2 1
1 2
2÷
1
2 1
2 ÷q 2 +  m2 
2   4
4
4







 2


Thế năng của cơ hệ:
Π=


1
m1 gl1 sin q1 + m2 g ( l1 sin q1 + 0.5l2 sin ( q1 + q2 ) )
2

(9)

Công ảo của lực suy rộng không có thế:
δ A = τ 1δ q1 + τ 2δ q2 − Pxδ xB − Pyδ yB

(10)

Ta lại có:

xB = l1 cos q1 + l2cos(q1 + q2 )

yB = l1 sin q1 + l2 sin ( q1 + q2 )

Suy ra:

δ xB =  −l1 sin q1 − l2 sin ( q1 + q2 )  δ q1 − l2 sin ( q1 + q2 ) δ q2
δ yB = l1 cos q1 + l2cos ( q1 + q2 )  δ q1 + l2cos ( q1 + q2 ) δ q2

Vậy suy ra biểu thức lực suy rộng:

2


Q1np = τ 1 + Px l1 sin q1 + l2 sin ( q1 + q2 )  − Py l1 cos q1 + l2cos ( q1 + q2 ) 


(11)

Q2np = τ 2 + Px l2 sin ( q1 + q2 ) − Py l2 cos ( q1 + q2 )

(12)

Thế các biểu thức trên vào phương trình lagrange tổng quát:

d  ∂T
dt  ∂ q.
 i


÷− ∂T = − ∂Π + Qinp
÷ ∂qi
∂qi


i = 1, 2

(13)

Ta có phương trình vi phân chuyển động của robot 2 khâu phẳng khớp quay:

 .. 
 ..
m l 2 + m2l22
m l2 m l l
τ 1 =  I1 + I 2 + m2l1l2 cos q2 + 1 1
+ m2l12 ÷q1 +  I 2 + 2 2 + 2 1 2 cos q2 ÷q 2

4
4
2




2
. .
.
 m
ml


−m2l1l2 q1 q 2 sin q2 − 0.5m2l1l2 q 2 sin q2 + g l1  1 + m2 ÷cos q1 + 2 2 cos ( q1 + q2 ) 
2

  2


(14)

− Px l1 sin q1 + l2 sin ( q1 + q2 )  + Py l1 cos q1 + l2 cos ( q1 + q2 ) 


 .. 
m l2 m l l
m l 2  ..
τ 2 =  I 2 + 2 2 + 2 1 2 cos q2 ÷q1 +  I 2 + 2 2 ÷q 2
4

2
4 




(15)

.2

+0.5m2l1l2 q1 sin q2 + 0.5m2 gl2 cos ( q1 + q2 )
− Px l2 sin ( q1 + q2 ) + Py l2 cos ( q1 + q2 )

b. Robot tay máy dạng cực

Tọa độ suy rộng của robot q = [ q1 u1 ]
Từ hình vẽ ta xác định được tọa độ khối tâm của từng khâu:
T

3


 a cos q1 
r1 =  a sin q1  ,
 0 

u1 cos q1 
r2 =  u1 sin q1 
 0 


(1)

Suy ra matrix jacobian tịnh tiến:
 −a sin q1
∂r1 
JT 1 =
= a cos q1
∂q 
 0
 − u sin q1
∂r2 
JT 2 =
= u cos q1
∂q 
 0

0
0 
0 

(2)

cos q1 
sin q1 
0 

(3)

Quan sát hình vẽ cho ta vận tốc góc các khâu một cách đơn giản:
0

 
ω1 =  0  ,
. 
q 
 1

0
 
ω2 =  0 
. 
q 
 1

(4)

Suy ra matrix Jacobian quay:
0 0 
J R1 = . = 0 0  ,
∂ q 1 0 


∂ω1

JR2

0 0 
= . = 0 0 
∂ q 1 0 



∂ω2

(5)

Matrix tenxo quán tính của các khâu quanh trọng tâm của chúng:
0 0 0 
I1 = 0 0 0  ,
0 0 I1 

0 0 0 
I 2 =  0 0 0 
 0 0 I 2 

(6)

Matrix khối lượng suy rộng:
2

M ( q ) = ∑ ( J TTi mi JTi + J TRi I i J Ri )

(7)

i =1

Thế các biểu thức (2) (3) (5) (6) vào (7) ta được:
 m a 2 + m2u12 + I1 + I 2
M ( q) =  1
0

2

Đặt I = m1a + I1 + I 2 vậy:
m u 2 + I
M ( q) =  2 1
 0

0

m2 

0

m2 

(8)

Biểu thức động năng của robot:
.
1 .T
T = q M ( q) q
2

Thay (8) vào ta được:
T=

.2
. 2
1
2
m
u

+
I
q
+
m
u
) 1 2 1
( 2 1
2


(9)

Thế năng của cơ hệ:
4


Π = m1 ga sin q1 + m2 gu1 sin q1

(10)

Công ảo của lực suy rộng không thế:
δ A = M δ q1 + F δ u1 − Pxδ xB − Pyδ yB

(11)

Ta lại có:

xB = u cos q1 + b cos q1


yB = u sin q1 + b sin q1
1
b = l2 là một nửa độ dài khâu 2
2

Suy ra:

δ xB = − ( u1 sin q1 + b sin q1 ) δ q1 + cos q1δ u1
δ yB = ( u1 cos q1 + b cos q1 ) δ q1 + sin q1δ u1

Thay vào (11) suy ra:

δ A =  M + ( u1 sin q1 + b sin q1 ) Px − ( u1 cos q1 + b cos q1 ) Py  δ q1
+ ( F − Px cos q1 − Py sin q1 ) δ u1

Suy ra biểu thức lực suy rộng:

Q1np = M + Px ( u1 + b ) sin q1 − Py ( u1 + b ) cos q1

(12)
(13)

Q = F − Px cos q1 − Py sin q1
np
2

Thay các biểu thức trên vào phương trình Lagrange tổng quát:

d  ∂T
dt  ∂ q.

 i


÷− ∂T = − ∂Π + Qinp
÷ ∂qi
∂qi


i = 1,2

(14)

Ta có phương trình vi phân chuyển động của tay máy dạng cực hai bậc tự do:

( I + m u ) q + 2m u q u + ( m a + m u ) g cos q − P ( u
2
2 1

..

.

1

..

2 1

.


1

1

1

2 1

1

.2

x

1

+ b ) sin q1 + Py ( u + b ) cos q1 = M (15)

m2 u1 − m2u1 q1 + m2 g sin q1 + Px cos q1 + Py sin q1 = F

(16)

Câu 2: Thiết lập phương trình vi phân chuyển động của Robot song song
phẳng khớp cực
Phương pháp sử dụng trong phần này là tách cấu trúc. Ta tách robot song
song cực ra làm ba robot tay máy cực ( dạng bài tâp 1b ) và một bàn máy. Khi
đó sử dụng phương trình Lagrange thành lập phương trình vi phân của robot.

5



a. Chân thứ nhất
Tách cấu trúc cho ta mô hình như dưới đây. Mô hình này hoàn toàn giống
với tay máy cực như của phần câu 1b.
T
Tọa độ suy rộng của robot q = [ q1 u1 ]

Áp dụng một cách tương tự ta có phương trình vi phân chuyển động của chân
thứ nhất:

( I + m u ) q + 2m u q u + ( m a + m u ) g cos q − P ( u
2
4 1

..

.

1

..

4 1

.

1

1


1

4 1

1

.2

x

1

+ b ) sin q1 + Py ( u1 + b ) cos q1 = M 1 (15)

m4 u1 − m4u1 q1 + m4 g sin q1 + Px cos q1 + Py sin q1 = F1
6

(16)


b. Chân thứ hai của robot

Tọa độ suy rộng: q = [ q2 u2 ]
Quan sát hình vẽ ta có tọa độ khối tâm của từng khâu:
T

 c + a cos q2 
r2 =  a sin q2  ,



0

c + u2 cos q2 
r5 =  u2 sin q2 


0

(1)

Từ (1) cho ta matrix Jacobian tịnh tiến:
JT 2

JT 5

 −a sin q2 0 
∂r2 
=
=  a cos q2 0 
∂q
 0
0 
 −u2 sin q2 cos q2 
∂r5 
=
=  u2 cos q2 sin q2 
∂q

0
0 


(2)

(3)

Tương tự ta cũng có vận tốc góc các khâu là:
0
 
ω2 =  0  ,
. 
q 
 2

0
 
ω5 =  0 
. 
q 
 2

(4)

Suy ra matrix Jacobian quay của các khâu:
J R2

0 0
= . =  0 0  ,
∂ q 1 0 



∂ω2

J R5

0 0
= . =  0 0 
∂ q 1 0 


∂ω5

(5)

Matrix tenxo quán tính các khâu đối với khối tâm của chúng có dạng:
7


0 0 0 
I 2 = 0 0 0  ,
0 0 I 2 

0 0 0 
I 5 = 0 0 0 
0 0 I 5 

(6)

Matrix khối lượng suy rộng:
2


M ( q ) = ∑ ( J TTi mi JTi + J TRi I i J Ri )

(7)

i =1

Thay các biểu thức (2) (3) (5) (6) vào (7) ta được matrix khối lượng suy rộng:
 m2 a 2 + m5u22 + I 2 + I 5 0 
M ( q) = 

0
m5 

Đặt m2 a 2 + I 2 + I 5 = I khi đó:
 m5u22 + I
M ( q) = 
 0

0

m5 

(8)

Biểu thức động năng của robot:
.
1 .T
T = q M ( q) q
2


Thay matrix khối lượng suy rộng vào ta có:
 . 
0   q2 

m5   . 
u 2 
.2
. 2
1
T = ( m5u22 + I ) q 2 + m5 u 2 
2

1.
T = q 2
2

2
  I + m5u2
u2  
 0
.

(9)

Biểu thức thế năng của cơ hệ:
Π = m2 ga sin q2 + m5 gu2 sin q2

(10)

Công ảo của các lực suy rộng không thế:

δ A = M 2δ q2 + F2δ u2 − Px 2δ xB 2 − Py 2δ y B 2

(11)

Ta lại có:

δ xB 2 = c + ( u2 + b ) cos q2

δ yB 2 = ( u2 + b ) sin q2

Thay vào (11) cho:

δ A = ( M 2 + Px 2 ( u2 + b ) sin q2 − Py 2 ( u2 + b ) cos q2 ) δ q2 +
+ ( F2 − Px 2 cos q2 − Py 2 sin q2 ) δ u2

Vậy lực suy rộng:

Q2np = M 2 + Px 2 ( u2 + b ) sin q2 − Py 2 ( u2 + b ) cos q2

(12)
Q = F2 − Px 2 cos q2 − Py 2 sin q2
(13)
np
Như vậy đã có các biểu thức T , Π, Q j thay vào phương trình Lagarange
tổng quát:
np
5

8




d  ∂T
dt  ∂ q.
 i


÷− ∂T = − ∂Π + Qinp
÷ ∂qi
∂qi


i=1,2

Ta được phương trình vi phân chuyển động của khâu 2:

( I + m u ) q + 2m u
2
5 2

..

2

.

5 2

.


q 2 u 2 + ( m2 a + m5u2 ) g cos q2 − Px 2 ( u2 + b ) sin q2 + Py 2 ( u2 + b ) cos q2 = M 2

(14)
.2

..

(15)

m5 u 2 − m5u2 q 2 + m5 g sin q2 + Px 2 cos q2 + Py 2 sin q2 = F2

c. Chân thứ ba của robot

Tọa độ suy rộng:
q = [ q3 u3 ]

T

Vị trí các khối tâm của hai khâu 3 và 6:
c cos 600 + a cos q3 
 c cos 600 + u3 cos q3 




r3 =  c sin 600 + a sin q3  , r6 =  c sin 600 + u3 sin q3 





0
0




 
0
ω3 =  0  ,
. 
q 
 3

(1)

 
0
ω6 =  0 
. 
q 
 3

Do chỉ thêm các thành phần hằng số trong các biểu thức tọa độ, nên khi
đạo hàm không ảnh hưởng. Một cách tương tự ta có phương trình vi phân
chuyển động của chân số ba của robot:

9


( I + m u ) q + 2m u

2
6 3

..

3

..

.

6 3

.

q 3 u 3 + ( m3a + m6u3 ) g cos q3 − Px 3 ( u3 + b ) sin q3 + Py 3 ( u3 + b ) cos q3 = M 3

. 2

m6 u 3 − m6u3 q 3 + m6 g sin q3 + Px 3 cos q3 + Py 3 sin q3 = F3

d. Thiết lập phương trình vi phân của bàn máy động

Phương trình lực:
..

m
X
 P P = X B1 + X B 2 + X B 3


..
 mP Y P = YB1 + YB 2 + YB 3 − mP g


( 1)
( 2)

Phương trình moment:
..

J P ϕ = X B1

l1 3
l 3
π

π

sin  + ϕ ÷− YB1 1 cos  + ϕ ÷
3
3
6

6


+ X B2

l1 3
l 3

π

π

sin  − ϕ ÷+ YB 2 1 cos  − ϕ ÷
3
3
6

6


− X B3

l1 3
l 3
π

π

sin  − ϕ ÷− YB 3 1 cos  − ϕ ÷
3
3
2

2


Sáu phương trình liên kết:


l1 3
π

cos  + ϕ ÷ = 0
 xP − ( u1 + b ) cos q1 −
3

6


 y − u + b sin q − l1 3 sin  π + ϕ  = 0
1

÷
 P ( 1 )
3
6


10



l1 3
π

cos  − ϕ ÷− ( u2 + b ) cos q2 − c = 0
 xP +
3


6


 y − l1 3 sin  π − ϕ  − u + b sin q = 0
)
2

÷ ( 2
 B
3
6


l1 3
c
π

cos  − ϕ ÷− ( u3 + b ) cos q3 − = 0
 xP −
3
2

2


 y + l1 3 sin  π − ϕ  − u + b sin q − c 3 = 0
)
2

÷ ( 3

 B
3
3
2


Kết hợp ta được phương trình vi phân chuyển động của robot song song

( I + m u ) q + 2m u q u + ( m a + m u ) g cos q − P ( u
2
4 1

..

.

1

.

4 1

1

1

1

4 1


1

x

1

+ b ) sin q1 + Py ( u1 + b ) cos q1 = M 1

.2

..

m4 u1 − m4u1 q1 + m4 g sin q1 + Px cos q1 + Py sin q1 = F1

( I + m u ) q + 2m u
2
5 2

..

2

5

.

.

2 q 2 u 2 + ( m2 a + m5u 2 ) g cos q2 − Px 2 ( u 2 + b ) sin q2 + Py 2 ( u 2 + b ) cos q2 = M 2
.2


..

m5 u 2 − m5u2 q 2 + m5 g sin q2 + Px 2 cos q2 + Py 2 sin q2 = F2

( I + m u ) q + 2m u
2
6 3

..

3

6

.

.

3 q 3 u 3 + ( m3 a + m6u3 ) g cos q3 − Px 3 ( u3 + b ) sin q3 + Py 3 ( u3 + b ) cos q3 = M 3
. 2

..

m6 u 3 − m6u3 q 3 + m6 g sin q3 + Px 3 cos q3 + Py 3 sin q3 = F3
..

m
X
 P P = X B1 + X B 2 + X B 3


..
 mP Y P = YB1 + YB 2 + YB 3 − mP g

..

J P ϕ = X B1

( 1)
( 2)

l1 3
l 3
π

π

sin  + ϕ ÷− YB1 1 cos  + ϕ ÷
3
3
6

6


+ X B2

l1 3
l 3
π


π

sin  − ϕ ÷+ YB 2 1 cos  − ϕ ÷
3
3
6

6


− X B3

l1 3
l 3
π

π

sin  − ϕ ÷− YB 3 1 cos  − ϕ ÷
3
3
2

2



l1 3
π


cos  + ϕ ÷ = 0
 xP − ( u1 + b ) cos q1 −
3

6


 y − u + b sin q − l1 3 sin  π + ϕ  = 0
1

÷
 P ( 1 )
3
6


l1 3
π

cos  − ϕ ÷− ( u2 + b ) cos q2 − c = 0
 xP +
3

6


 y − l1 3 sin  π − ϕ  − u + b sin q = 0
)
2


÷ ( 2
 B
3
6


11



l1 3
c
π

cos  − ϕ ÷− ( u3 + b ) cos q3 − = 0
 xP −
3
2

2


 y + l1 3 sin  π − ϕ  − u + b sin q − c 3 = 0
)
2

÷ ( 3
 B
3

3
2


12