Lập trình quỹ đạo và điều khiển chuyển động của robot công nghiệp

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
--------------------------------

CAO XUÂN HOÀNG

LẬP TRÌNH QUỸ ĐẠO VÀ ĐIỀU KHIỂN CHUYỂN ĐỘNG
CỦA ROBOT CÔNG NGHIỆP

CHUYÊN NGÀNH: CƠ ĐIỆN TỬ

LUẬN VĂN THẠC SỸ KHOA HỌC
NGÀNH: CƠ ĐIỆN TỬ

NGƯỜI HƯỚNG DẪN KHOA HỌC:
GS.TSKH. NGUYỄN VĂN KHANG

Hà nội – Năm 2011


MỤC LỤC
LỜI CAM ĐOAN .................................................................................................3
DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT .......................................4
DANH MỤC CÁC BẢNG ...................................................................................6
DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ.........................................................7
MỞ ĐẦU...............................................................................................................9
Chương 1 - THIẾT LẬP PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA
RÔBỐT CÔNG NGHIỆP BẰNG DẠNG THỨC LAGRANGE II ...................11
1.1. Thiết lập phương trình vi phân Lagrange loại II cho hệ nhiều vật............11
1.2. Các thí dụ áp dụng .....................................................................................15
Chương 2 - LẬP TRÌNH QUỸ ĐẠO .................................................................33

2.1. Mở đầu.......................................................................................................33
2.1.1. Một vài thuật ngữ sử dụng khi lập trình quỹ đạo.................................33
2.1.2. Nhiệm vụ lập trình quỹ đạo của rôbốt .................................................34
2.1.3. Các bước của bài toán lập trình quỹ đạo..............................................35
2.1.4. Một vài tiêu chuẩn và điều kiện phụ....................................................36
2.2. Lập trình quỹ đạo động học.......................................................................37
2.2.1. Mô tả đường cong không gian .............................................................38
2.2.2. Prôphin của vận tốc quỹ đạo ................................................................40
2.2.3. Việc xây dựng các quỹ đạo tổng quát bằng các đa thức nội suy .........44
2.3. Lập trình quỹ đạo tối ưu và động lực học ngược ......................................47
2.3.1. Tham số hóa các phương trình vi phân chuyển động ..........................48
2.3.2. Các chú ý về lập trình quỹ đạo.............................................................50
Chương 3 - ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA RÔBỐT............................77
3.1. Thiết lập bài toán .......................................................................................77
3.1.1. Điều khiển chuyển động của rôbốt trong không gian khớp.................78
3.1.2. Điều khiển chuyển động của rôbốt trong không gian thao tác ............78
3.2. Phương pháp điều khiển mômen ...............................................................79
3.3. Phương pháp điều khiển trượt ...................................................................81
1


3.3.1. Tổng quan.............................................................................................81
3.3.2. Xây dựng luật điều khiển .....................................................................82
3.3.3. Điều khiển trượt rôbốt Scara................................................................87
KẾT LUẬN.........................................................................................................92
TÀI LIỆU THAM KHẢO ..................................................................................94
PHỤ LỤC............................................................................................................97

2

LỜI CAM ĐOAN

Tôi xin cam đoan đây là công trình khoa học của tôi. Các kết quả nghiên cứu trong
luận văn là trung thực và có nguồn gốc cụ thể, rõ ràng. Các kết quả của luận văn chưa
từng được công bố trong bất cứ công trình khoa học nào.
Hà Nội, tháng 05 năm 2011.
Người cam đoan

Cao Xuân Hoàng

3


DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT
STT

Ký hiệu

Diễn giải

1

Ci

cos(qi)

2

Cij

cos(qi + qj)

3

Si

sin(qi)

4

Sij

sin(qi + qj)

5

ph

6

h

7

Ci

Ma trận Craig của khâu thứ i so với hệ R0

8

Ki

Ma trận Craig của khâu thứ i so với hệ Ri-1

9

mi

Khối lượng khâu thứ i

10

M (q )

Ma trận khối lượng suy rộng thực tế

11

)
M (q )

Ma trận khối lượng suy rộng của bộ điều khiển

28

% (q )
M

Sai số của ma trận khối lượng suy rộng

12

J Ti

Ma trận Jacobi tịnh tiến của khâu i

13

C ( q, q& )

Ma trận hệ số quán tính ly tâm và Côriôlis của mô hình thực

14

ˆ ( q, q& )
C

Ma trận hệ số quán tính ly tâm và Côriôlis của mô hình điều khiển

15

% ( q, q& )
C

Sai số giữa C ( q, q& ) và Cˆ ( q, q& )

16

qi

Tọa độ suy rộng thứ i (biến khớp i)

17

q

Véc tơ tọa độ suy rộng

18

q&i

Vận tốc suy rộng thứ i

19

q&

Véc tơ vận tốc suy rộng

20

q&&i

Gia tốc suy rộng thứ i

21

&&

q

Véc tơ gia tốc suy rộng

22

ei

Sai số của biến khớp thứ i

23

e

Véc tơ sai số của véc tơ tọa độ suy rộng

24

e&i

Sai số của vận tốc suy rộng thứ i

r,r

r

Tọa độ vật lý của một điểm
Tọa độ thuần nhất của một điểm

4

STT

Ký hiệu

Diễn giải

25

e&

Véc tơ sai số của vận tốc suy rộng

26

e&&i

Sai số của gia tốc suy rộng thứ i

27

&e&

Véc tơ sai số của gia tốc suy rộng

28

si

Sai số suy rộng của véc tơ tọa độ suy rộng thứ i

29

s

Véc tơ sai số suy rộng của tọa độ suy rộng

30

s&

Véc tơ sai số suy rộng của vận tốc suy rộng

31

&s&

Véc tơ sai số suy rộng của gia tốc suy rộng

32

n

Số tọa độ của khâu thao tác

33

m

Số bậc tự do của hệ

34

T

Động năng tay máy

35

Π

Thế năng tay máy

36

x

Véc tơ tọa độ khâu thao tác

37

x&

Véc tơ vận tốc khâu thao tác

38

&x&

Véc tơ gia tốc khâu thao tác

39

x = f (q)

Phương trình động học rôbôt

40

τi

Mômen hoặc lực suy rộng tác dụng lên khớp i

41

τ

Véc tơ mômen điều khiển

42

kPi

Hệ số điều khiển quán tính cho động cơ khớp i

43

kDi

Hệ số điều khiển vi phân cho động cơ khớp i

44

kIi

Hệ số điều khiển tích phân cho động cơ khớp i

45

KSi

Hệ số điều khiển trượt cho động cơ khớp i

46

KS

Véc tơ hệ số điều khiển trượt

47

ϕ,ψ , θ

Ba góc quay theo các trục của khâu

48

T

Ma trận biến đổi thuần nhất

5


DANH MỤC CÁC BẢNG

Bảng 2.1. Các tham số động học Craig rôbốt Scara ...........................................67
Bảng 2.2. Các tham số động lực rôbốt Scara......................................................67
Bảng 3.1. Các tham số Craig rôbốt Scara ...........................................................88
Bảng 3.2. Các tham số động lực rôbốt Scara......................................................88
Bảng 3.3. Thông số rôbốt Scara..........................................................................88

6


DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ

Hình 1.1. Vị trí vật rắn trong không gian ...........................................................11
Hình 1.2. Vị trí vật rắn trong không gian ...........................................................13
Hình 1.3. Mô hình con lắc kép............................................................................16
Hình 1.4. Cơ cấu 3 thanh phẳng .........................................................................19
Hình 1.5. Mô hình rôbốt Scara 3 bậc tự do.........................................................24
Hình 1.6. Hình chiếu mô hình rôbốt Scara 3 bậc tự do ......................................25
Hình 1.7. Mô hình rôbốt Scara 4 bậc tự do.........................................................28
Hình 1.8. Hình chiếu rôbốt Scara 4 bậc tự do.....................................................29
Hình 2.1. Mô tả đường cong không gian ............................................................38
Hình 2.2. Mô tả tham số của đường tròn không gian .........................................40
Hình 2.3. Các prôphin vận tốc quỹ đạo điển hình ..............................................41
Hình 2.4. Prôphin vận tốc dạng hình thang đối xứng.........................................43

Hình 2.5. Xấp xỉ bằng đa thức ............................................................................44
Hình 2.6. Nối các quỹ đạo thẳng bằng đa thức bậc ba .......................................47
Hình 2.7. Miền cho phép (với n=3 và f Ri = 0 )....................................................50
Hình 2.8. Tay máy cực........................................................................................52
Hình 2.9. Miền cho phép với

s
= 1 ....................................................................56
R

Hình 2.10. Miền cho phép với

s
= 1.5 ...............................................................56
R

Hình 2.11. Vận tốc quỹ đạo và prôphin vận tốc cho phép .................................57
Hình 2.12. Tính toán thời gian làm việc .............................................................58
Hình 2.13. Mô hình cơ cấu tay máy hai khâu phẳng ..........................................59
Hình 2.14. Miền giới hạn cho phép với s =1 ......................................................65
Hình 2.15. Miền giới hạn cho phép với s =1.25 .................................................65
Hình 2.16. Rôbốt Scara có khâu thao tác quay...................................................66
Hình 2.17. Miền cho phép với s = 0.5 ................................................................75
Hình 2.18. Miền cho phép với s =0.5 .................................................................75
7


Hình 2.19. Miền cho phép với s = 1.0 ................................................................76
Hình 2.20. Miền cho phép với s =1.0 .................................................................76
Hình 3.1. Ví dụ quỹ đạo chuyển động của rôbôt ................................................77

Hình 3.2. Sơ đồ phương pháp điều khiển trong không gian khớp......................78
Hình 3.3. Sơ đồ phương pháp điều khiển trong không gian thao tác .................78
Hình 3.4. Hàm sgn ..............................................................................................83
Hình 3.5. Mô hình toán và mô phỏng điều khiển rôbôt......................................85
Hình 3.6. Hàm sat ...............................................................................................86
Hình 3.7. Hàm artan có chỉnh tỉ lệ......................................................................86
Hình 3.8. Rôbôt Scara.........................................................................................87
Hình 3.9. Sơ đồ khối hệ điều khiển trượt rôbôt Scara ........................................90
Hình 3.10. Mômen điều khiển trên các khớp .....................................................90
Hình 3.11. Quỹ đạo điểm đầu khâu thao tác.......................................................91

8


MỞ ĐẦU

Trong sự nghiệp công nghiệp hóa và hiện đại hóa đất nước ta, lĩnh vực điều
khiển và tự động hóa có một vai trò quan trọng. Các rôbốt công nghiệp có một vị trí
đặc biệt quan trọng trong quá trình tự động hóa. Mục tiêu ứng dụng của rôbốt công
nghiệp trong sản xuất là nhằm nâng cao năng suất của các dây chuyền công nghiệp,
nâng cao chất lượng sản phẩm, cải thiện điều kiện lao động.
Ở nước ta từ những năm 90 của thế kỷ 20 đến nay, nhiều cơ sở sản xuất đã sử
dụng nhiều loại rôbốt công nghiệp phục vụ cho quá trình sản xuất. Ở nhiều trường đại
học và cao đẳng kỹ thuật đã đưa môn học “Rôbốt công nghiệp” vào giảng dạy cho
sinh viên các ngành Cơ điện tử, Công nghệ chế tạo máy, Ô tô và xe chuyên
dụng,v..v..[2,3,4,5]. Việc nghiên cứu các vấn đề về động học, động lực học và điều
khiển rôbốt công nghiệp được quan tâm ở trường đại học Bách khoa Hà Nội, Học viện
kỹ thuật quân sự, đại học Bách khoa TP Hồ Chí Minh, đại học Bách khoa Đà Nẵng,
Viện khoa học Việt Nam [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 24, 25, 26].
Trong luận văn này, trên cơ sở lý thuyết động lực học hệ nhiều vật [1] tác giả

trình bày một số vấn đề về lập trình quỹ đạo và điều khiển chuyển động của rôbốt
công nghiệp. Công cụ tin học dùng để tính toán là phần mềm Maple 9.5 và phần mềm
Matlab 6.5.
Trong lĩnh vực cơ điện tử, người ta thường quan tâm nhiều đến bài toán ngược:
cho biết chuyển động khâu thao tác, tìm chuyển động của các tọa độ trạng thái và lực
phát động. Trong luận văn này đi theo hướng trên, nghĩa là thiết lập phương trình
chuyển động của rôbốt công nghiệp bằng dạng thức Lagrange II và lập trình quỹ đạo
chuyển động của nó. Luận văn này gồm có các chương sau:
Chương 1: Dựa trên lý thuyết về động lực học hệ nhiều vật, chúng ta đi xây
dựng và giải bài toán “Thiết lập phương trình Lagrange II cho hệ nhiều vật”, sau đó áp
dụng và thiết lập phương trình vi phân cho một số dạng chuyển động cụ thể.
Chương 2: Lập trình quỹ đạo chuyển động của rôbốt công nghiệp, cho trước cụ
thể điểm tác động cuối của rôbốt, bằng cách giải bài toán động học ngược và động lực
học ngược, ta tìm qũy đạo chuyển động của rôbốt và tìm miền giới hạn các đại lượng
dẫn động của rôbốt.

9


Chương 3: Trình bày sơ bộ về việc điều khiển rôbốt công nghiệp. Các chuyển
động theo chương trình chỉ có ý nghĩa khi chúng ổn định. Ở đây đã trình bày các khái
niệm về thiết lập bài toán điều khiển trong không gian khớp và không gian thao tác.
Sau đó trình bày các phương pháp điều khiển mômen và phương pháp điều khiển
trượt. Đây là những vấn đề tương đối khó, do thời gian hạn chế, nên chưa có điều kiện
nghiên cứu đầy đủ các bài toán này.
Trong quá trình thực hiện luận văn này, do thời gian và trình độ còn hạn chế,
nên luận văn còn nhiều thiếu sót và chỉ lựa chọn nghiên cứu một số vấn đề tương đối
đơn giản. Tác giả kính mong nhận được sự chỉ bảo hướng dẫn của các thầy để hoàn
thiện mình hơn.
Tác giả xin chân thành cảm ơn GS.TSKH Nguyễn Văn Khang đã tận tình

hướng dẫn và chỉ bảo, giúp tác giả hoàn thành luận văn tốt nghiệp này. Tác giả cũng
xin chân thành cảm ơn các thầy trong bộ môn Cơ học ứng dụng trường đại học Bách
Khoa Hà Nội đã tạo điều kiện và giúp đỡ tác giả trong quá trình làm luận văn này.
Học viên

Cao Xuân Hoàng

10


Chương 1 - THIẾT LẬP PHƯƠNG TRÌNH VI PHÂN CHUYỂN ĐỘNG CỦA
RÔBỐT CÔNG NGHIỆP BẰNG DẠNG THỨC LAGRANGE II
1.1. Thiết lập phương trình vi phân Lagrange loại II cho hệ nhiều vật

Xét p vật rắn, chịu r liên kết độc lập. Khi đó số bậc tự do của hệ được xác định bởi hệ
thức:
f = 6p − r

Ta chọn hệ tọa độ suy rộng tối thiểu: q = [q1 ,q 2 ,...q n ,]T để xác định vị trí của hệ các
vật rắn.

z0

dm

r
u

A

B

r
r

r
rA

y0

x0
Hình 1.1. Vị trí vật rắn trong không gian
Động năng của một phân tố được tính như sau
1r
dT = v 2 dm
2
Động năng của vật rắn B sẽ có dạng:
1 r2
1 r
v dm = ∫ r& 2 dm
∫
2B
2B
r r
r r r
v = r& = v A + ω × u

T=

(1.1)

(1.2)

Từ đó ta có động năng của vật rắn B là

11


T=

r r
r r
r r
1 r
1 r
r
(v A + ω × u )2 dm = ∫ [v 2A +2v A ⋅ (ω × u )+(ω × u ) 2 ]dm
∫
2B
2B

=

1 r2
1 r r
r r
r
v A ∫ dm + v A .ω × ∫ udm + ∫ (ω × u ) 2 dm
2 B
2B
B

=

1 r2
r r r 1 r r
mv A + mv A .(ω × u ) + ∫ (ω × u ) 2 dm
2
2B

(1.3)

Trong giáo trình động lực học hệ nhiều vật [1], ta có công thức sau
r r r r rr r
r r r
a × (b × c ) = (b ⊗ a − a.bE ).c

Do đó ta có

r r r
r r r
r rr r r r
u × (ω × u ) = −u × (u × ω ) = (u 2 E − u ⊗ u ).ω
Từ
r r r
r r r
a × (b × c ) = ( a × b ) × c ,

ta suy ra
r r r r
r r r r

r r rr r r r
(ω × u )(ω × u ) = ω[u × (u × ω )] = ω.[u 2 E − u ⊗ u ].ω

Áp dụng các công thức trên vào tính số hạng cuối của (1.3)
⎤ r
1 r r 2
1 r r r r
1 r ⎡ r 2 rr r r
×
=
×
×
=
u
ω
dm
ω
u
u
ω
dm
ω. ⎢ ∫ [u E − u ⊗ u ]dm ⎥ .ω
(
)
[
(
)]
∫
∫
2B

2 B
2 ⎣B
⎦

Thế biểu thức trên vào (1.3) ta được
T=

1 r2
1 r rr r
r r r
mv A + mv A .(ω × uc ) + ω.I .ω
2
2

(1.4)

với
rr
r 2 rr r r ⎤
⎡
I = ∫ ⎢u E − u ⊗ u ⎥ dm
⎣
⎦
B
r
Nếu chọn A ≡ C thì uc = 0 , khi đó

1 r 1 r rr r
T = mvc2 + ω.I .ω
2

2

(1.5)

Chiếu biểu thức (1.5) lên hệ cố định R0 ta được
1
1
T = mvTc v c + ωT I cω
2
2

(1.6)

Chiểu biểu thức (1.5) lên hệ động R1 gắn liền với vật rắn ta có

12


1
1
T = mvTc v c + ω′T I′cω′
2
2

(1.7)

z1
z0

r

r

x1

r
rc

R0

x0

y1

r R11
u
C

y0

O

Hình 1.2. Vị trí vật rắn trong không gian

Như thế đối với vật rắn thứ i, ta có biểu thức động năng
1
1
(1.8)
T = mi vTci v ci + ωiT I ci ωi
2
2

r
Trong đó ω′ là hình chiếu của ω trên các trục của hệ tọa độ động R1 , còn ω là hình
r
chiếu của ω trên hệ tọa độ cố định R0 .
Ta xét hệ hôlônôm giữ và dừng, vận tốc khối tâm vCi có dạng
r
drCi ∂rCi
v Ci =
=
q& = J Ti (q)q&
dt
∂q
ωi =

dϕi ∂ϕ i dq ∂ϕi
q& = J Ri (q)q&
=
=
dt
∂q dt
∂q

(1.9a)
(1.9b)

Trong đó J Ti là ma trận Jacobi tịnh tiến, J Ri là ma trận Jacobi quay. Chúng được xác
định như sau:

13

⎡ ∂rix
⎢
⎢ ∂q1
∂r ⎢ ∂r
J Ti = i = ⎢ iy
∂q ⎢ ∂q1
⎢
⎢ ∂riz
⎢⎣ ∂q1
⎡ ∂ωix
⎢ &
⎢ ∂q1
∂ω ⎢ ∂ω
J Ri = i = ⎢ iy
∂q& ⎢ ∂q&1
⎢
⎢ ∂ωiz
⎢⎣ ∂q&1

∂rix
∂q2
∂riy
∂q2
∂riz
∂q2

L
L
L

∂ωix
∂q& 2
∂ωiy
∂q& 2
∂ωiz
∂q& 2

∂rix ⎤
⎥
∂q f ⎥
∂riy ⎥
⎥
∂q f ⎥
⎥
∂riz ⎥
∂q f ⎥⎦

(1.10)

∂ωix ⎤
⎥
∂q& f ⎥
∂ωiy ⎥
⎥
L
∂q& f ⎥
⎥
∂ωiz ⎥
L

∂q& f ⎥⎦

(1.11)

L

Trong đó ri = rCi là véctơ xác định vị trí khối tâm Ci của vật rắn Bi trong hệ tọa độ cố
định R0 . ωi là vận tốc góc của vật rắn Bi trong hệ tọa độ cố định R0 .
Thay (1.9) vào (1.8) ta có công thức động năng vật rắn thứ i như sau:
T=

1
1
mi (J Ti q& )T J Ti q& + (J Riq& )T I i J Riq&
2
2

(1.12)

Động năng của hệ p vật rắn sẽ được tính như sau:

T=

1 p T
1 p T T
T
&
&
q
m

J
J
q
+
q& J Ri I i J Ri q&
∑ i Ti Ti 2 ∑
2 i =1
i =1

(1.13)

Do q& không phụ thuộc vào dấu của tổng nên:
⎤
1 ⎡ p
T = q& T ⎢ ∑ (mi J TTi J Ti + J TRi I i J Ri ) ⎥ q&
2 ⎣ i =1
⎦
rr
Với: ( I i ) nhận được khi ta chiếu I lên trục của hệ tọa độ cố định
rr
( I′i ) nhận được khi ta chiếu I lên trục của hệ tọa độ động

(1.14)

p

Gọi

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

(1.15)

1
T = q& T M (q)q&
2

(1.16)

i =1

vậy

Trong công thức (1.16) M (q) là ma trận vuông cấp f và có dạng sau:

14


⎡ q1 ⎤
⎡ q&1 ⎤
⎡ m11 (q) ... m1 f (q) ⎤
⎢q ⎥
⎢ q& ⎥
⎢
⎥
2 ⎥
⎢
⎢ 2⎥
M (q) = ⎢ M
O M
⎥ ; q = ⎢ M ⎥ ; q& = ⎢ M ⎥

⎢ m f 1 (q) ... m ff (q) ⎥
⎢ ⎥
⎢ ⎥
⎣
⎦
⎢⎣ q f ⎥⎦
⎢⎣ q& f ⎥⎦

(1.17)

Thay các biểu thức (1.17) vào (1.16) ta nhận được:

T=

1 f f
∑∑ mik (qi ...q f )q&i q&k
2 i =1 k =1

(1.18)

f
∂T
= ∑ m jk q&k
∂q& j k =1

d ⎛ ∂T
⎜
dt ⎜⎝ ∂q& j

f

f
⎞ f
∂m jk
&&
m
q
q&k q&l
=
+
⎟⎟ ∑ jk k ∑∑
&
∂
q
k
k
l
=
1
=
1
=
1
l
⎠

f
f
∂m
∂T
= ∑∑ jk q&k q&l

∂q k =1 l =1 ∂q j

(1.19)

Phương trình Lagrange loại II tổng quát có dạng:
d ⎛ ∂T
⎜
dt ⎜⎝ ∂q& j

⎞ ∂T
∂Π
=−
+ Q*j ( j = 1,..., f )
⎟⎟ −
∂q& j
⎠ ∂q j

(1.20)

Thay vào phương trình trên ta sẽ nhận được:
f
f
1
∂Π
&&
+ Q*j
m jk qk + ∑∑ (mlj ,k − mkl , j )q& k q&l =
∑
2
∂q& j

k =1
k =1 l =1
f

Trong đó: m jk ,l =

∂m jk
∂ql

, mkl , j =

(1.21)

∂mkl
∂q j

Chú ý rằng ma trận M là ma trận đối xứng, m jk = mkj .
Phương trình (1.21) có thể viết lại dưới dạng như sau:
f

∑ m jk q&&k +
k =1

1 f f
∂Π
(mlj ,k + m jk ,l − mkl , j )q&k q&l =
+ Q*j
∑∑
2 k =1 l =1
∂q& j

1.2. Các thí dụ áp dụng
a) Thí dụ 1.1

Thiết lập phương trình vi phân chuyển động của con lắc kép
15

(1.22)


y

x

0

l1

ϕ1

z

m1 g

l2

ϕ2

m2 g

Hình 1.3. Mô hình con lắc kép
Chuyển động của con lắc kép là chuyển động phẳng.
Ta chọn hệ tọa độ suy rộng có dạng:
⎡ϕ ⎤
q=⎢ 1⎥
⎣ϕ 2 ⎦
Áp dụng công thức tính động năng
1
T = q& T (J TT 1m1J T 1 + J TT 2 m2 J T 2 )q&
2
Tọa độ của các chất điểm
x1 = l1 sin ϕ1 ,
y1 = −l1cosϕ1 ,
z1 = 0 ,

x2 = l1 sin ϕ1 + l2 sin ϕ2 ,
y2 = −l1cosϕ1 − l2 cosϕ 2 ,
z2 = 0

Ma trận Jacobi tịnh tiến có dạng sau:
⎡l1cosϕ1 0 ⎤
J T 1 = ⎢⎢l1sinϕ1 0 ⎥⎥
⎢⎣ 0
0 ⎥⎦
16


JT 2

⎡l1cosϕ1 l2cosϕ 2 ⎤

= ⎢⎢l1sinϕ1 l2sinϕ2 ⎥⎥
⎢⎣ 0
0 ⎥⎦

Áp dụng công thức (1.15):
p

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

Thay vào ta được:
⎡l1 cos ϕ1 0 ⎤
⎡l1 cos ϕ1 l1 sin ϕ1 0 ⎤ ⎢
⎥
M (q) = m1 ⎢
⎥ ⎢l1 sin ϕ1 0 ⎥
0
0
0
⎣
⎦⎢
0 ⎥⎦
⎣ 0
⎡l1 cos ϕ1 l2 cos ϕ 2 ⎤
⎡l1 cos ϕ1 l1 sin ϕ1 0 ⎤ ⎢
+ m2 ⎢
l1 sin ϕ1 l2 sin ϕ2 ⎥⎥
⎥
⎢
⎣l2 cos ϕ2 l2 sin ϕ2 0 ⎦ ⎢

⎥⎦
0
⎣ 0
⎡
l12
l1l2 cos(ϕ1 - ϕ2 ) ⎤
⎡l12 0 ⎤
= m1 ⎢
⎥
⎥ + m2 ⎢
l12
⎢⎣l1l2 cos(ϕ1 - ϕ 2 )
⎥⎦
⎣0 0 ⎦
⎡(m1 + m2 )l12
=⎢
⎣⎢ m2l1l2 cos(ϕ1 - ϕ 2 )

m2l1l2 cos(ϕ1 - ϕ 2 ) ⎤
⎥
m2l12
⎦⎥

Thay vào công thức động năng (1), ta được
⎡(m1 + m2 )l12
1
T = [ϕ&1 ϕ&2 ] ⎢
2
⎢⎣ m2l1l2 cos(ϕ1 - ϕ 2 )

m2l1l2 cos(ϕ1 - ϕ 2 ) ⎤ ⎡ϕ&1 ⎤
⎥⎢ ⎥
m2l12
⎥⎦ ⎣ϕ&2 ⎦

Vậy biểu thức động năng của hệ có dạng
T=

1
⎡⎣(m1 + m2 )l12ϕ&12 + 2m 2l1l2 cos(ϕ1 - ϕ 2 )ϕ&1ϕ&2 + m2l22ϕ&22 ⎤⎦
2

Biểu thức thế năng của hệ có dạng
Π = − m1 gl1 cos ϕ1 − m2 g (l1 cos ϕ1 + l2 cos ϕ 2 )

Khi đó đối với tọa độ suy rộng ϕ1 ta có
∂T
= (m1 + m2 )l12ϕ&1 + m 2l1l2 cos(ϕ1 - ϕ 2 )ϕ&2
∂ϕ&1

d ∂T
= (m1 + m2 )l12ϕ&&1 + m 2l1l2 cos(ϕ1 - ϕ2 )ϕ&&2
dt ∂ϕ&1
− m 2l1l2 sin(ϕ1 - ϕ 2 )ϕ&1ϕ&2 + m 2l1l2 sin(ϕ1 - ϕ 2 )ϕ&22
17


∂T
= − m 2l1l2 sin(ϕ1 - ϕ 2 )ϕ&1ϕ&2
∂ϕ1

∂Π
= m1 gl1 sin ϕ1 + m2 gl1 sin ϕ1
∂ϕ1

Thay các biểu thức trên vào phương trình Lagrange II ta được
( m1 + m2 )l12ϕ&&1 + m2l1l2cos(ϕ1 − ϕ 2 )ϕ&&2 − m2l1l2sin(ϕ1 − ϕ 2 )ϕ&1ϕ&2
+ m2l1l2sin(ϕ1 − ϕ 2 )ϕ&22 + m2l1l2sin(ϕ1 − ϕ 2 )ϕ&1ϕ&2
= − m1 gl1sinϕ1 − m2 gl1sinϕ1

Hay
(m1 + m2 )l12ϕ&&1 + m2l1l2cos(ϕ1 − ϕ2 )ϕ&&2 + m2l1l2sin(ϕ1 − ϕ 2 )ϕ&22
+ (m1 + m2 ) gl1sinϕ1 = 0
Đối với tọa độ suy rộng ϕ 2 ta có
∂T
= m 2l1l2 cos(ϕ1 - ϕ 2 )ϕ&1 + m2l22ϕ&2
∂ϕ&2

d ∂T
= m 2l1l2 cos(ϕ1 - ϕ2 )ϕ&&1 − m 2l1l2 sin(ϕ1 - ϕ 2 )(ϕ&12 - ϕ&1ϕ&2 ) + m2l22ϕ&&2
dt ∂ϕ&2

∂T
= m 2l1l2 sin(ϕ1 - ϕ2 )ϕ&1ϕ&2
∂ϕ 2
∂Π
= m2 gl2 sin ϕ 2
∂ϕ1
Thay các biểu thức trên vào phương trình Lagrange II ta được
m 2l1l2 cos(ϕ1 - ϕ 2 )ϕ&&1 − m 2l1l2 sin(ϕ1 - ϕ2 )(ϕ&12 - ϕ&1ϕ&2 ) + m2l22ϕ&&2
− m 2l1l2 sin(ϕ1 - ϕ2 )ϕ&1ϕ&2 = − m2 gl2 sin ϕ2

Hay
m 2l1l2 cos(ϕ1 - ϕ 2 )ϕ&&1 + m2l22ϕ&&2 − m 2l1l2 sin(ϕ1 - ϕ 2 )ϕ&12 + m2 gl2 sin ϕ 2 =0

b) Thí dụ 1.2

Xét một hệ bao gồm ba thanh thẳng như hình vẽ. Các thanh OA, OB, OC được xem là
các thanh đồng chất có khối lượng m1 ,m 2 , m3 chịu tác dụng của ngẫu lực M1 ,M 2 , M 3 .

18


C

y0
O

l3

l1

M3

M1

ϕ1

l2

x0

ϕ2

M2

ϕ3

Hình 1.4. Cơ cấu 3 thanh phẳng

Chuyển động của cơ cấu là chuyển động phẳng.
Ta chọn các tọa độ suy rộng đủ là:
⎡ϕ1 ⎤
q = ⎢⎢ϕ 2 ⎥⎥
⎢⎣ϕ3 ⎥⎦

(1)

Các thanh là thẳng và đồng chất nên các khối tâm C1 ,C 2 ,C3 nằm ở chính giữa thanh.
Tọa độ của các khối tâm được xác định như sau:
⎡1
⎤
⎢ 2 l1cosϕ1 ⎥
⎡ xC1 ⎤ ⎢
⎥
1
⎢
⎥
⎢
r1 = ⎢ yC1 ⎥ = l1sinϕ1 ⎥
⎢2
⎥

⎢⎣ zC1 ⎥⎦ ⎢
⎥
⎢ 0
⎥
⎣⎢
⎦⎥
1
⎡
⎤
⎢l1cosϕ1 + 2 l2cosϕ 2 ⎥
⎡ xC 2 ⎤ ⎢
⎥
1
⎢
⎥
⎢
r2 = ⎢ yC 2 ⎥ = l1sinϕ1 + l2sinϕ 2 ⎥
⎢
⎥
2
⎢⎣ zC 2 ⎥⎦ ⎢
⎥
0
⎢
⎥
⎣⎢
⎦⎥

(2)

19


1
⎡
⎤
os
os
l
c
ϕ
l
c
ϕ
l2 cosϕ3 ⎥
+
+
1
1
2
2
⎢
2
⎡ xC 3 ⎤ ⎢
⎥
1
⎢ ⎥ ⎢
r3 = ⎢ yC 3 ⎥ = l1sinϕ1 + l2sinϕ 2 + l2sinϕ3 ⎥
⎢
⎥

2
⎢⎣ zC 3 ⎥⎦ ⎢
⎥
0
⎢
⎥
⎢⎣
⎥⎦

Ma trận Jacobi tịnh tiến có dạng sau:
⎡ ∂rix
⎢ ∂q
⎢ 1
∂r ⎢ ∂r
J Ti = i = ⎢ iy
∂q ⎢ ∂q1
⎢ ∂riz
⎢
⎣ ∂q1

∂rix
∂q2
∂riy
∂q2
∂riz
∂q2

∂rix ⎤
∂q2 ⎥
⎥

∂riy ⎥
⎥
∂q3 ⎥
∂riz ⎥
⎥
∂q3 ⎦

(3)

Thay (2) vào (3) ta được
⎡ ∂xC1
⎢
⎢ ∂q1
∂r ⎢ ∂yC
J T1 = 1 = ⎢ 1
∂q ⎢ ∂q1
⎢ ∂z
⎢ C1
⎢⎣ ∂q1

∂xC1
∂q2
∂yC1
∂q2
∂zC1
∂q2

∂xC1 ⎤
⎤
⎥ ⎡ 1

∂q2 ⎥ ⎢ − 2 l1sinϕ1 0 0 ⎥
⎢
⎥
∂yC1 ⎥ ⎢ 1
l1cosϕ1 0 0 ⎥
⎥=
⎥
∂q3 ⎥ ⎢ 2
⎢
0
0 0 ⎥⎥
∂zC1 ⎥ ⎢
⎥
⎥⎦
∂q3 ⎥⎦ ⎢⎣

1
⎡
⎤
⎢ −l1sinϕ1 − 2 l2sinϕ 2 0 ⎥
⎢
⎥
∂r2 ⎢
1
J T2 =
= l1cosϕ1
l2cosϕ 2 0 ⎥
⎢
⎥
∂q

2
⎢
0
0
0 ⎥⎥
⎢
⎢⎣
⎥⎦
⎡
⎢ −l1sinϕ1 − l2sinϕ 2
⎢
∂r3 ⎢
J T3 =
= l1cosϕ1 l2cosϕ 2
∂q ⎢
⎢
0
0
⎢
⎢⎣

1
⎤
− l3sinϕ3 ⎥
2
⎥
1
l3cosϕ3 ⎥
⎥
2

⎥
0
⎥
⎥⎦

Khi đó véctơ vận tốc góc của các thanh lần lượt là:

20

(4)


⎡ω1x ⎤ ⎡0 ⎤
⎡0 ⎤
⎡0 ⎤
⎢ ⎥ ⎢ ⎥
⎢
⎥
ω1 = ⎢ω1 y ⎥ = ⎢0 ⎥ ; ω2 = ⎢ 0 ⎥ ; ω3 = ⎢⎢0 ⎥⎥
⎢ω ⎥ ⎢⎣ϕ&1 ⎥⎦
⎢⎣ϕ&3 ⎥⎦
⎢⎣ϕ&2 ⎥⎦
⎣ 1z ⎦

(5)

Các ma trận Jacobi quay có dạng:
⎡ ∂ω1x
⎢ ∂q&
⎢ 1

∂ω ⎢ ∂ω
J R1 = 1 = ⎢ 1 y
∂q& ⎢ ∂q&1
⎢ ∂ω1z
⎢
⎣ ∂q&1

J R2

∂ω1x
∂q& 2
∂ω1 y
∂q& 2
∂ω1z
∂q&2

∂ω1x ⎤
∂q&3 ⎥
⎥ ⎡0 0 0⎤
∂ω1 y ⎥ ⎢
⎥
⎥ = ⎢0 0 0⎥
∂q&3 ⎥
⎢⎣1 0 0 ⎥⎦
∂ω1z ⎥
⎥
∂q&3 ⎦

⎡0 0 0 ⎤
⎡0 0 0⎤

∂ω3 ⎢
∂ω 2 ⎢
⎥
=
= 0 0 0 ; J R3 =
= 0 0 0⎥
⎢
⎥
⎥
∂q&
∂q& ⎢
⎢⎣0 1 0 ⎥⎦
⎢⎣ 0 0 1 ⎥⎦

(6)

Mômen quán tính của các thanh đối với hệ trục quán tính chính trung tâm.
⎡
⎤
⎡
⎢0
⎢0
0
0 ⎥
⎢
⎥
⎢
1
m1l12
0 ⎥ , I 2 = ⎢0

I1 = ⎢0
⎢ 12
⎥
⎢
⎢
⎥
⎢
1
⎢0
⎢0
0
m1l12 ⎥
12
⎣
⎦
⎣
⎡
⎤
⎢0
0
0 ⎥
⎢
⎥
1
m3l32
0 ⎥
I 3 = ⎢0
⎢ 12
⎥
⎢

⎥
1
⎢0
0
m3l32 ⎥
12
⎣
⎦

⎤
⎥
⎥
1
m 2l22
0 ⎥
⎥
12
⎥
1
0
m 2l22 ⎥
12
⎦
0

0

(7)

Ma trận khối lượng của hệ có dạng:

3

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

(8)

i =1

21


1
⎡
2
⎢ ( 3 m1 + m2 + m3 )l1
⎢
1
M = ⎢( m2 + m3 )l1l2 .c(ϕ1 − ϕ 2 )
⎢ 2
⎢
1
⎢
m3l1l3cos(ϕ1 − ϕ3 )
⎢⎣
2

1
( m2 + m3 )l1l2 .c(ϕ1 − ϕ 2 )
2
1

( m2 + m3 )l22
2
1
m3l2l3cos(ϕ 2 − ϕ3 )
2

1
m3l1l3cos(ϕ1 − ϕ3 )
2
1
m3l2l3cos(ϕ 2 − ϕ3 )
2
1
m3l32
12

⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥⎦

- Động năng của hệ:
1
T = q& T Mq&
2

(9)

1
1
1
T = (m1 + 3m2 + 3m3 )l12ϕ&12 + (m2 + 3m3 )l22ϕ&22 + m3l32ϕ&32
6
6
6
1
⎛1
⎞
+ ⎜ m2 + m3 ⎟ l1l2cos(ϕ1 − ϕ 2 )ϕ&1ϕ&2 + m3l1l3cos(ϕ1 − ϕ3 )ϕ&1ϕ&3
2
⎝2
⎠
1
+ m3l2l3cos(ϕ2 − ϕ3 )ϕ&2ϕ&3
2

(10)

- Thế năng của hệ:
1
1
Π = − m1 gl1 cos ϕ1 − m2 g (l1 cos ϕ1 + l2 cos ϕ 2 )
2
2
1
− m3 g (l1 cos ϕ1 + l2 cos ϕ 2 + l3 cos ϕ3 )

2

(11)

- Các phương trình vi phân chuyển động của hệ
Thế các biểu thức (10) và (11) vào các phương trình Lagrange loại II
d ⎛ ∂T
⎜
dt ⎜⎝ ∂q& j

⎞ ∂T
∂Π
=−
+ Q*j ( j = 1, 2,3)
⎟⎟ −
&
∂
q
∂
q
j
j
⎠

(12)

Trong đó
Q1* = M 1;

Q2* = M 2 ; Q3* = M 3 ;

Ta sẽ thu được ba phương trình vi phân chuyển động của hệ đã cho.
Bằng cách lập trình và giải trong phần mềm Maple V9.0 ta thu được các kết quả như
sau.
Phương trình thứ nhất:

22


⎛1
⎞ 2
⎛1
⎞
⎜ m1 + m2 + m3 ⎟ l1 ϕ&&1 + ⎜ m2 + m3 ⎟ l1l2cos(ϕ1 − ϕ2 )ϕ&&2 +
⎝3
⎠
⎝2
⎠
1
⎛1
⎞
+ m3l1l3cos(ϕ1 − ϕ3 )ϕ&&3 + ⎜ m2 + m3 ⎟ l1l2sin(ϕ1 − ϕ 2 )ϕ&22 +
2
⎝2
⎠

(13)

1
⎛1

⎞
+ m3l1l3sin(ϕ1 − ϕ3 )ϕ&32 + ⎜ m1 + m2 + m3 ⎟ gl1 sin ϕ1 = M 1
2
⎝2
⎠

Phương trình thứ hai:
⎛1
⎞
⎛1
⎞ 2
⎜ m2 + m3 ⎟ l1l2cos(ϕ1 − ϕ2 )ϕ&&1 + ⎜ m2 + m3 ⎟ l2 ϕ&&2 +
⎝2
⎠
⎝3
⎠
1
⎛1
⎞
+ m3l2l3cos(ϕ2 − ϕ3 )ϕ&&3 − ⎜ m2 + m3 ⎟ l1l2sin(ϕ1 − ϕ2 )ϕ&12 +
2
⎝2
⎠

(14)

1
⎛1
⎞
+ m3l2l3sin(ϕ 2 − ϕ3 )ϕ&32 + ⎜ m2 + m3 ⎟ gl2 sin ϕ 2 = M 2

2
⎝2
⎠

Phương trình thứ ba:
1
1
m3l1l3cos(ϕ1 − ϕ3 )ϕ&&1 + m3l2l3cos(ϕ2 − ϕ3 )ϕ&&2 +
2
2
1
1
1
+ m3l32ϕ&&3 − m3l1l3sin(ϕ1 − ϕ3 )ϕ&12 − m3l2l3sin(ϕ 2 − ϕ3 )ϕ&22 +
3
2
2
1
+ m3 gl3 sin ϕ3 = M 3
2

c) Thí dụ 1.3

Cho mô hình rôbốt Scara 3 bậc tự do có dạng như sau:

23

(15)

a1

l2

θ2

θ1

C2

C1

C3

d3

d1
z0

O0

x0
Hình 1.5. Mô hình rôbốt Scara 3 bậc tự do

y0

a2

O2

l2

C2

θ2

a1
l1
O0

C1

O1

θ1

24

x0