Nghiên cứu thuật toán thích nghi, ứng dụng cho điều khiển chuyển động robot trong không gian decac

ĐàO MINH TUấN

bộ giáo dục và đào tạo
trường đại học bách khoa hà nội
---------------------------------------

ngành điều khiển và tự động hóa

nghiên cứu thuật toán thích nghi, ứng dụng cho điều khiẻn chuyển động
robot trong không gian decac

luận văn thạc sĩ khoa học
điều khiển và tự động hóa

2009 - 2011
Hà Nội 9 - 2011


bộ giáo dục và đào tạo
trường đại học bách khoa hà nội
--------------------------------------Đào Minh Tuấn

nghiên cứu thuật toán thích nghi, ứng dụng cho điều khiẻn chuyển động
robot trong không gian decac

Chuyên ngành :

Điều khiển và tự động hóa

luận văn thạc sĩ điều khiển và tự động hóa

người hướng dẫn khoa học :
TS. Nguyễn Phạm Thục Anh

Hà Nội 9 – 2011


Trang 1

Lời cam đoan
Tôi là Đào Minh Tuấn - học viên lớp cao học tự động hóa khóa 2009 - 2011
xin cam đoan:
- Luận văn tốt nghiệp thạc sĩ này là công trình nghiên cứu của bản thân
dưới sự hướng dẫn của TS. Nguyễn Phạm Thục Anh.
- Các kết quả trong luận văn tốt nghiệp thạc sĩ này là trung thực, không
phải sao chép toàn văn của bất kỳ công trình nào khác.
Hà Nội, ngày 26 tháng 09 năm 2011
Học viên

Đào Minh Tuấn

 


Trang 4

Lời cảm ơn
Xin chân thành cảm ơn Khoa Điện, Viện đào tạo sau đại học - Trường
Đại Học Bách khoa Hà Nội đã tạo điều kiện thuận lợi giúp tác giả hoàn thành
luận văn tốt nghiệp này.
Xin chân thành cảm ơn sự hướng dẫn, chỉ bảo tận tình của cô Nguyễn

Phạm Thục Anh trong suốt thời gian thực hiện đề tài, cảm ơn cô đã hướng
dẫn tác giả những kiến thức và kỹ năng cơ bản nhất để có thể hoàn thành
được đề tài, cảm ơn cô đã luôn dành thời gian giúp đỡ tác giả, và cảm ơn cô vì
tác giả đã học được nhiều điều từ cô.
Xin chân thành cảm ơn thầy Giang Hồng bắc, thầy Phạm Đức Hùng,
các đồng nghiệp khác công tác tại khoa điện - điện tử trường Đại học Sư
phạm Kỹ thuật Hưng Yên đã ủng hộ, giúp đỡ và tạo điều kiện giúp đỡ tác giả
hoàn thành luận văn.
Xin cảm ơn các anh chị, bạn bè học viên lớp TĐH nghành ĐKTĐ khóa
2009-2011 đã ủng hộ, giúp đỡ và động viên trong thời gian học tập và nghiên
cứu.
Cuối cùng xin cảm ơn những người thân yêu, những người luôn động
viên giúp đỡ về mọi mặt mọi hoàn cảnh.
Qua một thời gian tác giả đã hoàn thành luận văn tốt nghiệp với đề tài
“Nghiên cứu thuật toán thích nghi ứng dụng cho điều khiển chuyển động
robot trong không gian khớp và không gian Decac”. Tuy đã cố gắng nhưng
chắc chắn vẫn không tránh khỏi những thiếu sót. Kính mong nhận được sự
thông cảm và đóng góp ý kiến của quý Thầy Cô và các bạn.
Hà Nội, ngày 23 tháng 09 năm 2011

 


Trang 2

Mục lục
Lời cam đoan ...............................................................................................................1
Mục lục........................................................................................................................2
Lời cảm ơn ..................................................................................................................4
Danh mục các ký hiệu, các chữ viết tắt .......................................................................5

Danh mục các bảng .....................................................................................................6
Danh mục các hình vẽ, đồ thị ......................................................................................7
MỞ ĐẦU .....................................................................................................................9
Chương 1 XÂY DỰNG MÔ HÌNH TOÁN HỌC CỦA ROBOT BA BẬC TỰ DO
...................................................................................................................................11
1.1. Sơ lược quá trình phát triển của Robot công nghiệp .....................................11
1.2. Ứng dụng Robot công nghiệp trong sản xuất .................................................12
1.3. Các định nghĩa về Robot công nghiệp ............................................................12
1.3.3. Hệ toạ độ ..................................................................................................14
1.3.4. Trường công tác của Robot ......................................................................16
1.4 Lựa chọn Robot ...............................................................................................16
1.5 Xác định động học thuận tau máy theo bảng Đ-H ..........................................17
1.6 Xác định điểm tác động cuối của tay máy ......................................................20
1.7 Bài toán động học ngược của tay máy Robot RRT .........................................22
1.8 Xây dựng phương trình động lực học Robot RRT ..........................................23
1.8.1 Thiết lập phương trình động lực học .........................................................23
1.8.2 Mô phỏng phương trình động lực học Robot sử dụng Simulink ..............31
Chương 2: CÁC THUẬT TOÁN THÔNG DỤNG ĐIỀU KHIỂN ROBOT ROBOT
...................................................................................................................................33
2.1 Phương pháp PD bù trọng trường ....................................................................33
2.1.1 Cơ sở lý thuyết ..........................................................................................33
2.1.2. Mô phỏng điều khiển Robot sử dụng phương pháp PD bù trọng trường.34
2.2. Phương pháp tuyến tính hóa chính xác ...........................................................37

 


Trang 3

2.2.1. Cơ sở lý thuyết .........................................................................................37

2.2.2. Mô phỏng điều khiển Robot sử dụng phương pháp tuyến tính hóa chính
xác.......................................................................................................................38
2.3. Điều khiển bền vững .......................................................................................42
2.3.1. Cơ sở lý thuyết phương pháp điều khiển trượt.........................................42
2.3.2. Mô phỏng điều khiển Robot sử dụng phương pháp điều khiển trượt ......43
2.4. Điều khiển thích nghi......................................................................................46
2.4.1. Cơ sở lý thuyết phương pháp Slotine- Li .................................................46
2.4.2. Mô phỏng điều khiển Robot sử dụng phương pháp Slotine-Li ................48
Chương 3: THUẬT TOÁN ĐIỀU KHIỂN THÍCH NGHI VÀ ỨNG DỤNG CHO
ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA ROBOT TRONG KHÔNG GIAN DECAC
...................................................................................................................................53
3.1 Cơ sở lý thuyết .................................................................................................53
3.2 Thiết kế bộ điều khiển .....................................................................................55
3.3 Kết quả mô phỏng tín hiệu điều kihiển và tín hiẹu đầu ra của robot ...............56
TÀI LIỆU THAM KHẢO .........................................................................................58

 


Trang 5

Danh mục các ký hiệu, các chữ viết tắt
Từ viết tắt

Viết đầy đủ

Ý nghĩa
Người máy công nghiệp

IR

Industrial Robot

AMF

American Machine and
Foundry Company

Công ty đúc và chế tạo máy của Mỹ

DOF

Degrees Of Freedom

Bậc tự do của Robot hay tay máy

hay Robot công nghiệp

phương pháp điều khiển tỷ lệ và vi
phân

PD
RRT

Ro bot RRT

Một loại robot có ba bậc tự do sử
dụng trong công nghiệp

D-H

Bảng tham số D-H của robot

Biểu diễn tham số động học của
robot

 


Trang 6

Danh mục các bảng
Bảng 1 Bảng D-H của robot RRT .....................................................................................................19

 


Trang 7

Danh mục các hình vẽ, đồ thị
Hình 1 Robot công nghiệp Kuka (Đức)...................................................................13
Hình 2 Minh họa tay máy ........................................................................................15
Hình 3 Quy tắc bàn tay phải ....................................................................................15
Hình 4 Trường công tác của Robot .........................................................................16
Hình 5 Robot RRT ...................................................................................................17
Hình 6 Các khâu của Robot RRT ............................................................................17
Hình 7 Xác định tọa độ của Robot RRT..................................................................18
Hình 8 Các tham số hệ trục tọa độ Robot RRT .......................................................19
Hình 9 Mô hình Robot sử dụng simulink ................................................................31
Hình 10 Xác định G(Q) ..........................................................................................32

Hình 11 Xác định V (Q , Q& ) .....................................................................................32
Hình 12 Xác định H(Q) ..........................................................................................32
Hình 13 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp PD bù trọng
trường

35

Hình 14 Bộ điều khiển PD bù trọng trường ...........................................................35
Hình 15 Đáp ứng đầu ra hệ thống ..........................................................................36
Hình 16 Đáp ứng đầu ra hệ thống ..........................................................................36
Hình 17 Đáp ứng đầu ra hệ thống với sự thay đổi tham số m2=0.5 và m3= 0.5 ...37
Hình 18 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp tuyến tính hóa
chính xác 39
Hình 19 Bộ điều khiển tuyến tính hóa chính xác ...................................................39
Hình 20 Khâu tạo tín hiệu đặt và giá trị đặt các khớp Qd, Qd' và Qd'' .................40
Hình 21 Đáp ứng đầu ra bộ điều khiển tuyến tính hóa chính xác ..........................40
Hình 22 Đáp ứng đầu ra hệ thống ..........................................................................41
Hình 23 Đáp ứng đầu ra hệ thống khi thay đổi tham số m2=0.5 và m3= 0.5 ........41
Hình 24 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp điều khiển trượt
43

 


Trang 8

Hình 25 Bộ điều khiển trượt...................................................................................44
Hình 26 Khâu tạo tín hiệu đặt và giá trị đặt các khớp Qd, Qd' và Qd'' .................44
Hình 27 Đáp ứng đầu ra bộ điều khiển trượt .........................................................45
Hình 28 Đáp ứng đầu ra bộ điều khiển trượt .........................................................45

Hình 29 Đáp ứng đầu ra bộ điều khiển trượt khi thay đổi tham số m2=0.5 và m3=
0.5

46

Hình 30 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp điều khiển thích
nghi

48

Hình 31 Bộ điều khiển thích nghi ..........................................................................49
Hình 32 Khâu cập nhật tham số .............................................................................49
Hình 33 Khâu tạo tín hiệu đặt và giá trị đặt các khớp Qd, Qd' và Qd'' .................50
Hình 34 Quá trình cập nhật tham số.......................................................................50
Hình 35 Đáp ứng đầu ra bộ điều khiển thích nghi .................................................51
Hình 36 Quá trình cập nhật tham số.......................................................................51
Hình 37 Đáp ứng đầu ra bộ điều khiển thích nghi .................................................52
Hình 38 Mô hình Điều khiển chuyển động của robot trong không gian Decac dùng
thuật toán điều khiển thích nghi. ...............................................................................55
Hình 39 Hinh Bộ điều khiển thích nghi điều khiển chuyển động robot trong không
gian Decac .................................................................................................................55
Hình 40 Hình Bộ cập tham số ................................................................................56
Hình 41 Hình Quá trình cập nhập tham số cho bộ điều khiển ..............................56
Hình 42 Hình Tín hiệu đầu ra của của robot ..........................................................57
Hình 43 Hình Dáp ứng đầu ra bộ điều khiển thích nghi .......................................57

 


Trang 9

MỞ ĐẦU
Đặt vấn đề
Trong nền sản xuất công nghiệp hiện nay của nước ta cũng như của các nước
khác trên thế giới, mức độ tự động hóa càng cao thì robot công nghiệp càng đóng
vai trò quan trọng trong việc giảm cường độ lao động cho người lao động, tăng
năng suất, độ chính xác của sản phẩm góp phần năng cao chất lượng, số lượng và
giảm giá thành sản phẩm.
Để tiếp cận, nghiên cứu và ứng dụng các loại robot công nghiệp trong sản
xuất đòi hỏi người ký sư, kỹ thuật viên phải hiểu và nắm vững được kết cấu cơ khí,
tham số động học của robot và các bộ điều khiển được ứng dụng trong các robot
công nghiệp. Chọn và thực hiện đề tài luật văn thạc sĩ “Nghiên cứu thuật toán

thích nghi ứng dụng cho điều khiển chuyển động robot trong không gian
khớp và không gian Decac”. là một phương pháp tốt nhất hà hiệu qủa nhất để
tôi thực hiện được mục tiêu trên.
Các hướng tiếp cận chính để thực hiện đề tài trên là áp dụng các kiến thức cơ
sở về lý thuyết điều khiển, động học và các phương pháp điều khiển về robot công
nghiệp đã được trang bị trong quá trình học thạc sĩ.
Mục đích của luận văn
Mục đích của luận văn là nghiên cứu về động học robot, các phương pháp
điều khiển và phát triển thuật toán điều khiển thích nghi trong không gian Decac.
Trong phạm vi của luận văn này tác giả tập trung nghiên cứu động học của
robot ba bậc tự do RRT và ứng dụng các thuật toán điều khiển để điều khiển robot
RRT, phát triển thật toán điều khiển thích nghi để điều khiển robot RRT trong
không gian Decac.

 

Trang 10

Phương pháp thực hiện và bố cục
Để đạt được mục đích trên, trình tự nội dung nghiên cứu theo tuần tự từ xây
dựng mô hình toán học của robot RRT, nghiên cứu các thuật toán thông dụng để
điều khiển robot và ứng dụng vào điều khiển robot RRT, phát triển thuật toán điều
khiển thích nghi để điều khiển robot RRT trong không gian làm việc.
Nội dung của luận văn được tổ chức như sau:
Chương 1 xây dựng mô hình toán học của robot ba bậc tư do RRT.
Chương 2 các thuật toán thông dụng điều khiển robot, ứng dụng để điều
khiển robot RRTvà các kết quả mo phỏng.
Chương 3 tập trung trình bày thuật toán điều khiển thích nghi và ứng dụng
cho điều khiển chuyển động của robot trong không gian Decac.

 


Trang 11

Chương 1 XÂY DỰNG MÔ HÌNH TOÁN HỌC CỦA
ROBOT BA BẬC TỰ DO
1.1. Sơ lược quá trình phát triển của Robot công nghiệp
Thuật ngữ “Robot” xuất phát từ tiếng Séc “Robota” có nghĩa là công việc tạp
dịch trong vở kịch Rossum’s Universal Robots của Karel Capek (1921). Đến đầu
thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry Company) quảng
cáo một loại máy tự động vạn năng và gọi là “Người máy công nghiệp” (Industrial
Robot). Ngày nay người ta đặt tên người máy công nghiệp (hay Robot công nghiệp)
cho những loại thiết bị có dáng dấp và một vài chức năng như tay người được điều
khiển tự động để thực hiện một số thao tác sản xuất.
Tiếp theo Mỹ, các nước khác bắt đầu sản xuất Robot công nghiệp: Anh

(1967), Thuỵ Điển và Nhật (1968) theo bản quyền của Mỹ; CHLB Đức (1971);
Pháp (1972); ở Ý (1973)...
Có thể nói, Robot là sự tổ hợp khả năng hoạt động linh hoạt của các cơ cấu
điều khiển từ xa với mức độ “tri thức” ngày càng phong phú của hệ thống điều
khiển theo chương trình số cũng như kỹ thuật chế tạo các bộ cảm biến, công nghệ
lập trình và các phát triển của trí khôn nhân tạo, hệ chuyên gia...
Trong những năm sau này, việc nâng cao tính năng hoạt động của Robot
không ngừng phát triển. Các Robot được trang bị thêm các loại cảm biến khác nhau
để nhận biết môi trường đã có vị trí quan trọng trong chung quanh, cùng với những
thành tựu to lớn trong lĩnh vực Tin học - Điện tử đã tạo ra các thế hệ Robot với
nhiều tính năng đặc biệt, số lượng Robot ngày càng gia tăng, giá thành ngày càng
giảm. Nhờ vậy, Robot công nghiệp các dây chuyền sản xuất hiện đại.
Mỹ là nước đầu tiên phát minh ra Robot, nhưng nước phát triển cao nhất
trong lĩnh vực nghiên cứu chế tạo và sử dụng robot tại Nhật.

 


Trang 12

1.2. Ứng dụng Robot công nghiệp trong sản xuất
Từ khi mới ra đời, Robot công nghiệp được áp dụng trong nhiều lĩnh vực
dưới góc độ thay thế sức người. Nhờ vậy các dây chuyền sản xuất được tổ chức lại,
năng suất vỡ hiệu quả sản xuất tăng lên rõ rệt.
Mục tiêu ứng dụng Robot công nghiệp nhằm góp phần nâng cao năng suất
dây chuyền công nghệ, giảm giá thành, nâng cao chất lượng vỡ khả năng cạnh tranh
của sản phẩm đồng thời cải thiện điều kiện lao động. Đạt được các mục tiêu trên lỡ
nhờ vỡ những khả năng to lớn của Robot như: làm việc không biết mệt mỏi, rất dễ
dàng chuyển nghề một cách thành thạo, chịu được phóng xạ vỡ các môi trường làm
việc độc hại, nhiệt độ cao, “cảm thấy” được cả từ trường và “nghe” được cả siêu

âm... Robot được dùng thay thế con người trong các trường hợp trên hoặc thực hiện
các công việc tuy không nặng nhọc nhưng đơn điệu, dễ gây mệt mỏi, nhầm lẫn.
Trong ngành cơ khí, Robot được sử dụng nhiều trong công nghệ đúc, công
nghệ hàn, cắt kim loại, sơn, phun phủ kim loại, tháo lắp vận chuyển phôi, lắp ráp
sản phẩm...
1.3. Các định nghĩa về Robot công nghiệp
1.3.1. Định nghĩa Robot công nghiệp
Hiện nay có nhiều định nghĩa về Robot, có thể điểm qua một số định nghĩa
như sau:
Định nghĩa Robot công nghiệp (theo tiêu chuẩn AFNOR (Pháp))
Robot công nghiệp lỡ một cơ cấu chuyển động tự động có thể lập trình, lặp
lại các chương trình, tổng hợp các chương trình đặt ra trên các trục toạ độ; có khả
năng định vị, định hướng, di chuyển các đối tượng vật chất: chi tiết, dao cụ, gá
lắp... theo những hành trình thay đổi đã chương trình hoá nhằm thực hiện các
nhiệm vụ công nghệ khác nhau.

 


Trang 13

Định nghĩa Robot công nghiệp (theo RIA (Robot institute of America))
Robot là một tay máy vạn năng có thể lặp lại các chương trình được thiết kế
để di chuyển vật liệu, chi tiết, dụng cụ hoặc các thiết bị chuyên dùng thông qua các
chương trình chuyển động có thể thay đổi để hoàn thành các nhiệm vụ khác nhau.
Định nghĩa Robot công nghiệp (theo TOCT 25686-85 (Nga))
Robot công nghiệp lỡ một máy tự động, được đặt cố định hoặc di động được,
liên kết giữa một tay máy vỡ một hệ thống điều khiển theo chương trình, có thể lập
trình lại để hoàn thành các chức năng vận động vỡ điều khiển trong quá trình sản
xuất.

Hình 1 Robot công nghiệp Kuka (Đức)

 


Trang 14

1.3.2. Bậc tự do của Robot
Bậc tự do lỡ số khả năng chuyển động của một cơ cấu (chuyển động quay
hoặc tịnh tiến). Để dịch chuyển được một vật thể trong không gian, cơ cấu chấp
hành của Robot phải đạt được một số bậc tự do. Nói chung cơ hệ của Robot là một
cơ cấu hở, do đó bậc tự do của nó có thể tính theo công thức:
5

w = 6n − ∑ ipi
i =1

Trong đó:

n - Số khâu động
pi - Số khớp loại i (i = 1, 2, ... , 5: Số bậc tự do bị hạn chế)

Đối với các cơ cấu có các khâu được nối với nhau bằng khớp quay hoặc tịnh
tiến (khớp động loại 5) thì số bậc tự do bằng với số khâu động. Đối với cơ cấu hở,
số bậc tự do bằng tổng số bậc tự do của các khớp động.

1.3.3. Hệ toạ độ
Mỗi Robot thường bao gồm nhiều khâu (links) liên kết với nhau qua các khớp
(joints), tạo thành một xích động học xuất phát từ một khâu cơ bản (base) đứng yên.

Hệ toạ độ (Coordinate frames) gắn với khâu cơ bản gọi là hệ toạ độ cơ bản (hay hệ
toạ độ chuẩn). Các hệ toạ độ trung gian khác gắn với các khâu động gọi là hệ toạ độ
suy rộng. Trong từng thời điểm hoạt động, các toạ độ suy rộng xác định cấu hình
của Robot bằng các chuyển dịch dài hoặc các chuyển dịch góc cuả các khớp tịnh
tiến hoặc khớp quay (hình 1.1). Các toạ độ suy rộng còn được gọi là biến khớp.

 


Trang 15

Hình 2  Minh họa tay máy 
Các hệ toạ độ gắn trên các khâu của Robot phải tuân theo qui tắc bàn tay phải:
Dùng tay phải, nắm hai ngón tay út và áp út vào lòng bàn tay, xoè 3 ngón: cái, trỏ
và giữa theo 3 phương vuông góc nhau, nếu chọn ngón cái là phương và chiều của
trục z, thì ngón trỏ chỉ phương, chiều của trục x và ngón giữa sẽ biểu thị phương,
chiều của trục y (hình 1.2).

Hình 3 Quy tắc bàn tay phải
Trong Robot ta thường dựng chữ O và chỉ số n để chỉ hệ toạ độ gắn trờn khâu
thứ n. Như vậy hệ toạ độ cơ bản (Hệ toạ độ gắn với khâu cố định) sẽ được ký hiệu
là O0; hệ toạ độ gắn trên các khâu trung gian tương ứng sẽ là O1, O2,..., On-1, hệ toạ
độ gắn trên khâu chấp hành cuối ký hiệu lỡ On.

 


Trang 16

1.3.4. Trường công tác của Robot

Trường công tác (Workspace or Range of motion) hay vùng làm việc - không
gian công tác của Robot là toàn bộ thể tích được quét bởi khâu chấp hành cuối khi
Robot thực hiện tất cả các chuyển động có thể. Trường công tác bị ràng buộc bởi
các thông số hình học của Robot cũng như các ràng buộc cơ học của các khớp; ví
0

dụ, một khớp quay có chuyển động nhỏ hơn một góc 360 . Người ta thường dùng
hai hình chiếu để mô tả trường công tác của một Robot (hình 1.3).

Hình 4 Trường công tác của Robot

1.4 Lựa chọn Robot
Robot được lựa chọn trong bài tập này là Robot RRT với 2 khớp quay và một khớp
tịnh tiến. Với trường tương tác rộng có thể được sử dụng trong các ứng dụng thực
tế:

 


Trang 17

Hình 5 Robot RRT
Robot chọn là Robot RRT, là Robot có ba khớp và bốn khâu.
Khâu 1 trùng với đế của Robot.
Khâu 2, khâu 3, Khâu 4 như hình vẽ.
Khớp 1 là khớp quay qoanh trúc Z0 một góc θ1
Khớp 2 là khớp quay qoanh trục Z1 một góc θ2
Khớp 3 là khớp tịnh tiến dọc theo trục Z2 một đoạn r3

1.5 Xác định động học thuận tau máy theo bảng Đ-H

Robot RRT có ba khâu như hình vẽ:

Hình 6 Các khâu của Robot RRT

 


Trang 18

Gắn hệ trục tọa độ lên các khâu.
Giả định vị trí ban đầu và chọn gốc tọa độ O0 của Robot như hình vẽ. Các
trục Z đặt cùng phương với các trục khớp.

Hình 7 Xác định tọa độ của Robot RRT

Chọn Z1 quay tương đối một góc 90o so với trục Z0, đây chính là góc quay
quanh trục X0 một góc α1 (phép biến đổi ROT(X0, α1) trong biểu thức tính An).
Nghĩa là trục x0 vuông góc với z0 và trục z1. Ta chọn chiều của x0 từ trái qua phải
thì góc quay α1 = 900(chiều dương ngược chiều kim đồng hồ). Đồng thời ta cũng
thấy O1 tịnh tiến một đoạn dọc theo z0 so với O0, đó chính là phép đổi Trans(0,0,d1),
tịnh tiến dọc treo z0 một đoạn d1, các trục y0 và y1= xác định theo quy tắc bàn tay
phải như hình vẽ.
Tiếp tục ta chọn gốc tọa độ O2 đặt trùng với O1 vì trục khớp thứ ba và trục
khớp thứ 2 cắt nhau tại O1. Trục z2 trùng phương với trục khớp thứ ba, tức là đã
quay đi một góc 900 so với z1 quanh trục y1. Phép biến đổi này không có trong biểu
thức tính An nên không dung được, ta phải chọn lại vị trí ban đầu của Robot nghĩa là
ta chỉ cần thay đổi vị trí của khâu thứ ba

 

Trang 19

Hình 8 Các tham số hệ trục tọa độ Robot RRT

Theo như hình vẽ, O2 vẫn được đặt trùng với O1, trục z2 có phương thẳng
đứng, tức là ta phải quay trục z1 thành trục z2 quanh trục x1 một goác -900. (α2 = 900).
Đầu cuối của khâu thứ ba không có khớp, ta đặt O3 tại điểm giữa các ngón
tay, trục x3 và trục z3 chọn như hình vẽ, như vậy, ta tịnh tiến gốc tọa độ theo z2 một
đoạn d3 (phép Trans(0,0,d3)) suy ra đây là khâu tịnh tiến và d3 là biến.
Như vậy, đã gắn ba hệ tọa độ nên ba khớp của Robot và bảng Đ-H được xác
định như sau:
Khâu

θi

αi

a1

di

1

θ1*

90

0

d1

2

θ 2*

-90

0

0

3

0

0

0

d *3

Bảng 1

 


Trang 20

Xác định ma trận A:

Ma trận An có dạng:

An =

⎡ cosθ
⎢ sin θ
⎢
⎢ 0
⎢
⎣ 0

− sin θ cosα
cosθ cosα

sin θ sin α
−cosθ sin α

sin α
0

cosα
0

0⎤
0 ⎥⎥
d⎥
⎥
1⎦

Với quy ước viết tắt: C1 = cosθ1 ; S1 = sinθ1 ; C2 = Cosθ 2 ……

Ta tính được lần lượt:
⎡C1
⎢S
A1 = ⎢ 1
⎢0
⎢
⎣0

1
0

⎡ C2
⎢S
A2 = ⎢ 2
⎢0
⎢
⎣0

0 − S2
0 C2
−1 0
0
0

⎡1
⎢0
A3 = ⎢
⎢0
⎢
⎣0

0 S1
0 −C1
0
0

0⎤
0 ⎥⎥
d1 ⎥
⎥
1⎦
0⎤
0 ⎥⎥
0⎥
⎥
1⎦

0⎤
0 ⎥⎥
0 1 d3 ⎥
⎥
0 0 1⎦

0 0
1 0

Các ma trận A1, A2, A3 được khai báo trong Matlab là:
A1=[c1 0 s1 0;s1 0 -c1 0;0 1 0 d1;0 0 0 1];
A2=[c2 0 -s2 0;s2 0 c2 0;0 -1 0 0;0 0 0 1];
A3=[1 0 0 0;0 1 0 0;0 0 1 d3;0 0 0 1];

1.6 Xác định điểm tác động cuối của tay máy
2

 

T3 = A3;

(3.1)


Trang 21

1

T3 = A2. 2 T3 ;

(3.2)

Phép toán tính T3 được tính trong Matlab là
%Toa do diem tac dong cuoi cua tay may
%so voi he toa do O2(T32)
T32=A3
%Toa do diem tac dong cuoi cua tay may
%so voi he toa do O1(T31)
T31=A2*T32
%Toa do diem tac dong cuoi cua tay may
%so voi he toa do O(T3)
T3=A1*T31
⎡ C2

⎢S
1
Suy ra: T3 = ⎢ 2
⎢0
⎢
⎣0

0
0

− S2

−1

0
0

0

C2

0⎤
0 ⎥⎥
.
0⎥
⎥
1⎦

⎡1
⎢0

⎢
⎢0
⎢
⎣0

0⎤
⎡ C2
⎢
⎥
0⎥
S
= ⎢ 2
⎢0
0 1 d3 ⎥
⎢
⎥
0 0 1⎦
⎢⎣ 0

0 0
1 0

0 − S2
0 C2
−1 0
0
0

T3 = A1.1T3

⎡C1
⎢S
T3 = ⎢ 1
⎢0
⎢
⎣0

0 S1
0 −C1
1
0

0
0

− S2 .* d3 ⎤
⎥
C2 .* d3 ⎥
0 ⎥
⎥
1 ⎦⎥

3.3
0 ⎤ ⎡ C2
⎢
0 ⎥⎥ ⎢ S 2
.
d1 ⎥ ⎢ 0
⎥ ⎢
1 ⎦ ⎣⎢ 0

0 − S2
0 C2
−1 0
0
0

⎡C1.C2
− S 2 .* d3 ⎤
⎥
⎢
*
C2 . d 3 ⎥
Sd
= ⎢ 1 2
⎢ S2
0 ⎥
⎥
⎢
1 ⎦⎥
⎣⎢ 0

− S1
C1
0
0

−C1S 2
− S1S 2
C2

0

−C1.S 2 .* d3 ⎤
⎥
− S1S 2 .* d3 ⎥
C2*d3 + d1 ⎥
⎥
1
⎦⎥

Với tọa độ của điểm tác động cuối được khai báo trong Matlab như sau:
Te=[Nx Ox Ax Px;Ny Oy Ay Py;Nz Oz Az Pz;0 0 0 1]%diemtacdongcuoi
⎡ nx
⎢n
TE= ⎢ y
⎢ nz
⎢
⎣0

 

ox
oy

ax
ay

oz
0

az
0

px ⎤
p y ⎥⎥
pz ⎥
⎥
1⎦


Trang 22

Ta có hệ phương trình động học của Robot RRT như sau:
⎧nx = C1C2
⎫
⎪o = − S
⎪
⎪ x
⎪
1
⎨
⎬;
⎪ax = −C1S 2 ⎪
⎪⎩ px = −C1S 2 d3 ⎪⎭

⎧n y = S1C2
⎫
⎪
⎪
⎪o y = C1

⎪
⎨
⎬;
⎪a y = − S1S 2 ⎪
⎪ p = −S S d ⎪
1 2 3⎭
⎩ y

⎧ nz = S 2
⎫
⎪o = 0
⎪
⎪ z
⎪
⎨
⎬
⎪ a z = C2
⎪
⎪⎩ pz = C2 d3 + d1 ⎭⎪

(3.4)

1.7 Bài toán động học ngược của tay máy Robot RRT
Xuất phát từ xây dựng hệ pt động học của Robot RRT ta đã có:
A1−1.0 T = A2 A3 =1 T3
123
14243
VT

3

(4.1)

VP

Phép toán được khai báo trong Matlab như sau
Syms Nx Ny Nz Ox Oy Oz Ax Ay Az Px Py Pz
A11=inv(A1)%Ma tran nghich dao cua A1
VP=A2*A3 %ve phai
Te=[Nx Ox Ax Px;Ny Oy Ay Py;Nz Oz Az Pz;0 0 0 1]
%diem tac dong cuo
VT=A11*Te %ve trai
⎡ C2
⎢S
Vế phải = A2 A 3 = ⎢ 2
⎢0
⎢
⎣0
⎡C1
⎢0
−1
⎢
Ta có: A1 =
⎢ S1
⎢
⎣0

 

0

0

− S2
C2

−1
0

0
0

S1
0
−C1
0

0⎤
0 ⎥⎥
.
0⎥
⎥
1⎦

0 0 ⎤
1 − d1 ⎥⎥
0 0 ⎥
⎥
0 1 ⎦

⎡1

⎢0
⎢
⎢0
⎢
⎣0

0⎤
⎡ C2 0 − S 2
⎢
⎥
0⎥
S
0 ( C2 )
= ⎢ 2
⎢( 0 ) −1
0 1 d3 ⎥
0
⎢
⎥
0 0 1⎦
0
0
⎣ 0

0 0
1 0

( − S2 d3 )⎤
( C2 d3 ) ⎥⎥
0

1

⎥
⎥
⎦

(4.2)


Trang 23

⎡C1
⎢0
Suy ra vế trái = ⎢
⎢ S1
⎢
⎣0

S1
0
−C1
0

⎡ C1nx + S1n y C1ox + S1o y
⎢
nz
oz
⎢
=⎢

⎢( S1nx − C1n y ) S1ox − C1o y
⎢
0
0
⎣

0 0 ⎤ ⎡ nx
1 − d1 ⎥⎥ ⎢⎢ n y
.
0 0 ⎥ ⎢ nz
⎥ ⎢
0 1 ⎦ ⎣0
C1ax + S1a y

( az )
S1ax − C1a y
0

ox
oy

ax
ay

oz
0

az
0

(C p

px ⎤
p y ⎥⎥
=
pz ⎥
⎥
1⎦

+ S1 p y ) ⎤
⎥
( pz − d1 ) ⎥
⎥
S1 px − C1 p y ⎥
⎥
1
⎦
1

x

Với điều kiện:
⎡ nx
⎢n
⎢ y
⎢ nz
⎢
⎣0

ox

oy

ax
ay

oz
0

az
0

px ⎤
p y ⎥⎥
đã biết.
pz ⎥
⎥
1⎦

Cân bằng các phần tử của vế trái và vế phải ta có
⎧C2 = nz (1)
⎫
⎪⎪
⎪⎪
⎨ S1o y + C1ox = 0 ( 2 ) ⎬
⎪
⎪
⎩⎪C2 d3 = pz − d1 ( 3) ⎭⎪

(

)

Từ (1) ⇒ C2 = az ⇒ S2 = 1 − az2 ⇒ θ 2 = ATAN 2 ± 1 − az2 , az .
⎡θ1 = ATAN 2(ox , −oy )
Từ (2) suy ra ⎢
⎢⎣θ1 = ATAN 2(−ox , oy )

Từ (1) suy ra θ 2 = arc cos nz
Từ (3) suy ra d3 =

( pz − d1 )
C2

1.8 Xây dựng phương trình động lực học Robot RRT
1.8.1 Thiết lập phương trình động lực học
Đặt các biến của các khớp 1, 2 và 3 lần lượt là q1, q2 và q3.