Nghiên cứu động học, động lực học và thiết kế bộ điều khiển RISE cho robot 3 bậc tự do RRP

1
1

Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền
MỤC LỤC

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


2
2

Đồ án tốt nghiệp
GVHD: TS. Đinh Thị Thanh Huyền
DANH MỤC CÁC THUẬT NGỮ, CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT.
STT
1
2
3
4
5
6
7

Ký hiệu

n
DH

Diễn giải nội dung đầy đủ
Khoảng cách từ trục đến trục đo dọc theo trục
Góc giữa trục và trục xác định theo trục
Khoảng cách từ trục đến trục đo dọc theo trục
Góc giữa trục và trục xác định theo trục
Số bậc tự do của Robot
Denavit- Hartenberg
Vận tốc của biến khớp thứ i

8
9
10
11

Gia tốc của biến khớp i
Ma trận biến đổi thuần nhất mô tả quan hệ giữa khâu i-1 và
khâu i
Ma trận biến đổi thuần nhất.
cos(qi )

12

sin ( 2.q2 )

13

cos ( q1 ) .cos(q 2 )

14

cos ( q1 ) .sin(q 2 )

15

sin(q1 ).cos ( q2 )

16

sin(qi )

17

sin ( q1 ) .sin ( q2 )

18
19
20
21
22
23
24
25
26
27
28

,,
,,

,,

,,
,,

Vector tọa độ khối tâm trong hệ quy chiếu của khâu i.
Các momen quán tính độc cực.
Các momen quán tính tích.
Vector gia tốc trọng trường.
Ma trận giả quán tính của khâu i.
Khối lượng khâu i.
Lực tổng quát của khâu thứ i.
Chiều dài của khâu i.
Vector mô tả vị trí.
Vector có hướng mà bàn tay sẽ tiếp cận đối tượng.
Vector có hướng mà theo đó các ngón tay nắm vào nhau khi
cầm nắm đối tượng.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


3
3

Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền
DANH MỤC HÌNH VẼ

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53

Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền
LỜI MỞ ĐẦU

Trong những năm gần đây, cùng với sự phát triển của công nghệ và các hệ thống
máy móc hiện đại, các Robot đang không ngừng phát triển về số lượng cũng như khả
năng của chúng. Robot ngày càng thông minh, đảm nhận thay thế những công việc mà
con người không thể thực hiện như: làm việc trong môi trường có tia phóng xạ, giải
cứu con người trong hỏa hoạn, thu thập các thông số của môi trường ở những nơi khắc
nghiệt.... Các loại Robot ngày càng linh hoạt hơn do chúng được điều khiển bởi bộ xử
lý trung tâm được tích hợp thêm các hệ thống điều khiển thông minh. Robot là một
phát minh vĩ đại của con người vì mục đích giải phóng con người khỏi những công
việc nặng nhọc, sự nhàm chán của công việc (do sự lặp lại các thao tác của một công
việc nào đó nhiều lần nguy hiểm của môi trường lao động như môi trường nóng bức
trong các lò hơi, sự ô nhiễm bụi bặm của các hầm mỏ, hay sự nguy hiểm ở duới đáy
đại dương, trên không gian vũ trụ… cũng như tăng tính tự động hóa trong các dây
truyền sản xuất. Nhưng ở nước ta thì việc nghiên cứu chưa được mở rộng và phát triển
chỉ sử dụng trong các cơ sở sản xuất lớn. Các Robot đa số thường được nhập khẩu từ
nước ngoài về với giá thành rất là đắt, vận dụng Robot vào sản xuất còn gặp nhiều khó
khăn cần sự hướng dẫn của bạn bè quốc tế, lực lượng cán bộ kỹ sư có trình độ cao …
Từ hiện thực trên, cùng với sự đam mê, ham học hỏi, nghiên cứu khoa học công
nghệ tiên tiến trên thế giới nên em đã quyết định chọn đề tài liên quan tới lĩnh vực
nghiên cứu Robot. Cụ thể là đề tài: “Nghiên cứu động học, động lực học và thiết kế
bộ điều khiển RISE cho robot 3 bậc tự do RRP” do TS. Đinh Thị Thanh Huyền
hướng dẫn.
Hoàn thành đồ án tốt nghiệp, em xin chân thành cảm ơn sự hướng dẫn, chỉ bảo
của cô giáo TS. Đinh Thị Thanh Huyền đã giúp đỡ em hoàn thành môn học này.
Măc dù em đã cố gắng nhưng do còn hạn chế về mặt kiến thức nên chắc chắn
đề tài của em không tránh khỏi những sai sót, kính mong nhận được sự chỉ bảo của

thầy cô và sự góp ý của các bạn để đề tài của em được hoàn thiện hơn.
Em xin chân thành cảm ơn!
Sinh viên thực hiện
Lê Bá Thành
SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền
CHƯƠNG 1: TỔNG QUAN VỀ ROBOT.

1.1. Định nghĩa Robot.
- Định nghĩa theo tiêu chuẩn AFNOR (Pháp): Robot 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 tọa độ; có khả năng định vị, định hướng, di chuyển các đối tượng của
vật chất: chi tiết, dụng cụ, gá lắp,… theo những hành trình thay đổi đã chương trình
hóa nhằm thực hiện các nhiệm vụ công nghệ khác nhau.
-

Định nghĩa 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 theo ГOCT (Nga): Robot là 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.
Robot có khả năng chương trình hóa linh hoạt trên nhiều trục chuyển động, sự
linh hoạt này biểu thị cho số bậc tự do của chúng.
1.2. Vai trò của Robot.
Robot được áp dụng trong nhiều lĩnh vực dưới góc độ thay thế sức người. Mục
đích sử dụng trong các dây chuyền sản suất nhằm nâng cao năng suất, chất lượng và
hiệu quả sản xuất từ đó giảm giá thành sản phẩm nâng cao khả năng cạnh tranh.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.1: Robot trong công nghiệp.
Đặc điểm Robot:
-

Làm việc không biết mệt mỏi, không có hiện tượng nhầm lẫn trong khi làm việc
Làm việc được trong các môi trường nguy hiểm như phóng xạ, độc hại
Thay đổi các thao tác làm việc dễ dàng bằng cách thay đổi chương trình lập trình
Trong ngành cơ khí, Robot được sử dụng nhiều trong công nghệ đúc, hàn, cắt kim loại,

-

sơn, phun phủ kim loại, vận chuyển phôi, lắp ráp sản phẩm,…
Trong các dây chuyền sản xuất tự động, nhờ sự có mặt của Robot mà các dây chuyền
đạt mức độ tự động hóa cao, mức độ linh hoạt cao. Các Robot này được điều khiển

bằng một hệ thống các chương trình được lập trình sẵn. Robot còn được sử dụng trong
lĩnh vực y học, trong quốc phòng, trong chinh phục vũ trụ, trong công nghiệp nguyên
tử,…

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.2: Robot công nghiệp trong một dây truyền sản xuất ô tô ở Nhật Bản.
Rõ ràng Robot có vai trò và ý nghĩa quan trọng trong nhiều lĩnh vực của cuộc
sống, tuy nhiên Robot chưa thể linh hoạt như con người, trong quá trình làm việc vẫn
có thể bị hỏng sẽ làm ảnh hưởng tới sự hoạt động của cả dây chuyền. Vì vậy, Robot
vẫn phải hoạt động dưới sự giám sát của con người.
1.3. Bậc tự do của Robot.
Bậc tự do của Robot là số khả năng chuyển động độc lập của một cơ cấu
Robot. Để 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. Hầu hết 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:
(1.1)
Trong đó:
+ n: Số khâu động.
+ : Số khớp loại i (i= 1,..5: số bậc tự do bị hạn chế).
Đối với các cơ cấ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 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 khớp động.
Để định vị và định hướng khâu chấp hành cuối một cách tùy ý trong không gian
3 chiều Robot cần có 6 bậc tự do, trong đó 3 bậc tự do để định vị và 3 bậc tự do để

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

định hướng. Đối với một số công việc đơn giản như nâng hạ, sắp xếp,… có thể yêu
cầu số bậc tự do ít hơn. Các Robot hàn, sơn,… thường yêu cầu 6 bậc tự do. Trong một
số trường hợp cần sự khéo léo, linh hoạt hoặc khi cần tối ưu hóa quỹ đạo,… người ta
dùng Robot với số bậc tự do lớn hơn 6.
1.4. Hệ tọa độ.
Mỗi Robot bao gồm nhiều khâu (links) liên kết với nhau bởi 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ệ quy
chiếu gắn với khâu cơ bản đó gọi là hệ quy chiếu chuẩn. Các hệ quy chiếu trung gian
khác gắn với khâu động gọi là hệ quy chiếu suy rộng. Trong từng thời điểm hoạt động,
các tham số của hệ quy chiếu suy rộng xác định cấu hình của Robot bằng các chuyển
động dài hoặc các chuyển dịch góc của các khớp tịnh tiến hoặc khớp quay như trên
Hình 1.1. Các tham số của hệ quy chiếu suy rộng còn gọi là các biến khớp. Các hệ quy
chiếu gắn trên các khâu của Robot phải tuân theo quy 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, xòe 3 ngón: cái, trỏ và giữa theo 3
phương vuông góc với nhau. Nếu chọn ngón cái làm phương 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 như
trên Hình 1.2.

Hình 1.3. Các tọa độ suy rộng của robot

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53

Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.4. Quy tắc bàn tay phải.
Cấu trúc cơ bản của robot.
Các thành phần chính của robot.
1.5.
1.5.1.

Một Robot thường bao gồm các thành phần chính như: Cánh tay Robot, nguồn
động lực, dụng cụ lắp trên khâu chấp hành cuối, các cảm biến, bộ điều khiển, thiết bị
dạy học, máy tính… các phần mềm lập trình cũng được gọi là một thành phần của hệ
thống Robot. Mối quan hệ giữa các thành phần trong Robot như Hình 1.3

Hình 1.5. Các thành phần chính của hệ thống robot.
-

Cánh tay Robot là kết cấu cơ khí gồm các khâu liên kết với nhau bởi các khớp động để

-

có thể tạo nên những chuyển động của Robot.
Nguồn động lực là các động cơ điện một chiều hoặc động cơ bước, các hệ thống xy

-

lanh thủy lực, khí nén để tạo động lực cho tay máy hoạt động.
Dụng cụ thao tác được gắn trên khâu cuối của Robot, dụng cụ có thể nhiều kiểu khác
nhau như: dạng bàn tay để nắm bắt đối tượng hoặc các công cụ làm việc như mỏ hàn,

-

đá mài, đầu phun sơn…
Thiết bị dạy học dùng để dạy cho Robot các thao tác cần thiết theo yêu cầu của quá
trình làm việc, sau đó Robot tự lặp lại các động tác đã được dạy để làm việc.
SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp
-

GVHD: TS. Đinh Thị Thanh Huyền

Các phần mềm để lập trình và các chương trình điều khiển Robot được cài đặt trên
máy tính, dùng để điều khiển Robot thông qua bộ điều khiển. Bộ điều khiển hay còn
gọi là Module điều khiển, nó thường được kết nối với máy tính. Một Mudule điều
khiển có thể còn có các cổng Vào-Ra (I/O port) để làm việc với nhiều thiết bị khác
nhau như các cảm biến giúp Robot nhận biết trạng thái của bản thân, xác định vị trí
của đối tượng làm việc; điều khiển các băng tải hoặc cơ cấu cấp phôi hoạt động phối
hợp với Robot…
1.5.2. Kết cấu của tay máy.
Như đã nói ở trên, tay máy là thành phần quan trọng, nó quyết định khả năng
làm việc của robot. Các kết cấu của nhiều tay máy được phỏng theo cấu tạo và chức
năng của tay người; tuy nhiên ngày nay, tay máy được thiết kế rất đa dạng, nhiều cánh
tay Robot có hình dáng rất khác xa cánh tay người. Trong thiết kế và sử dụng tay máy,
chúng ta cần quan tâm tới các thông số hình học-động học, là những thông số liên
quan đến khả năng làm việc của robot như: tầm với (hay hướng công tác), số bậc tự do
(thể hiện sự khéo léo linh hoạt của Robot), độ cứng vững, tải trọng vật nặng, lực kẹp,

-

… Các khâu của robot thường thực hiện được hai chuyển động cơ bản:
Chuyển động tịnh tiến theo phương x, y, z trong không gian Đề các, thông thường tạo
nên các hình khối, các chuyển động này thường ký hiệu là T (Translation) hoặc P

-

(Prismatic).
Chuyển động quay quanh trục x, y, z ký hiệu là R (Rotation). Tùy thuộc vào số khâu
và sự tổ hợp các chuyển động (R&T) mà tay máy có các kết cấu khác nhau với vùng

-

làm việc khác nhau.
Các kết cấu thường gặp của Robot là robot kiểu tọa độ Đề các, tọa độ trụ, tọa độ cầu,
robot kiểu SCARA, hệ tọa độ góc (phóng sinh)….
Robot kiểu tọa độ Đề các: là tay máy có 3 chuyển động cơ bản tịnh tiến theo
phương của các trục hệ tọa độ gốc (hình 1.4). Không gian làm việc có dạng khối chữ
nhật. Do kết cấu đơn giản, loại tay máy này có độ cứng vững cao, độ chính xác cơ khí
dễ đảm bảo vì vậy nó thường dùng để vận chuyển phôi liệu, lắp ráp, hàn trong mặt
phẳng…

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.6. Robot kiểu tọa độ Đề các.
Robot kiểu tọa độ trụ: Vùng làm việc của Robot có dạng trụ rỗng. Thường
khớp thứ nhất chuyển động quay. Ví dụ robot 3 bậc tự do, cấu hình R.T.T

Hình 1.7. Robot kiểu tọa độ trụ.
Robot kiểu tọa độ cầu: Vùng làm việc có dạng hình cầu, thường độ cứng
vững của loại Robot này thấp hơn so với hai loại trên. Ví dụ robot 3 bậc tự do, cấu
hình R.R.R hoặc R.R.T làm việc theo kiểu tọa độ cầu.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

Hình 1.8. Robot kiểu R.R.R

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.9. Robot kiểu R.R.T

Robot kiểu hệ tọa độ góc (Hệ phóng sinh): Đây là kiểu robot dùng nhiều hơn
cả. Ba chuyển động đầu tiên là chuyển động quay, trục quay thứ nhất vuông góc với
hai trục kia. Các chuyển động định hướng khác cũng là các chuyển động quay. Vùng
làm việc của tay máy này gần giống một phần khối cầu. Tất cả các khâu đều nằm trong
mặt phẳng thẳng đứng nên các tính toán cơ bản là bài toán phẳng.
Ưu điểm nổi bật đó là gọn nhẹ, tức là có vùng làm việc tương đối lớn so với kích
cỡ của bản thân robot, độ linh hoạt cao.
Ví dụ :

Hình 1.10. Robot hoạt động theo kiểu hệ tọa độ góc, cấu hình RRR.RRR

Robot kiểu SCARA: Robot này thường dùng trong công việc lắp ráp. Ba khớp
đầu tiên của kiểu Robot này có cấu hình R.R.T, các trục khớp đều theo phương thẳng
đứng.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.11. Robot kiểu SCARA.
1.6.
1.6.1.

Ứng dụng Robot công nghiệp trong sản xuất.
Các ưu điểm của Robot.
Các loại Robot tham gia vào quy trình sản xuất cũng như trong đời sống sinh

hoạt của con người nhằm nâng cao năng suất lao động của dây truyền công nghệ, giảm
giá thành sản phẩm, nâng cao chất lượng cũng như khả năng cạnh tranh của sản phẩm
tạo ra.
Robot có thể thay thế con người làm việc ổn định bằng các thao tác đơn giản
hợp lý, đồng thời có khả năng thay đổi công việc để thích nghi với sự thay đổi của quy
trình công nghệ, góp phần vào giảm giá thành sản phẩm, chi tiết nhân công nhất là ở
những nước mà nguồn nhân công rất ít hoặc chi phí cao. Ngoài ra các robot còn có khả
năng nghe được siêu âm, cảm nhận được từ trường.
Bên cạnh đó, một ưu điểm nổi bật của Robot là môi trường làm việc. Chúng có
thể thay thế con người làm việc ở những môi trường độc hại, ẩm ướt, bụi bặm hay
nguy hiểm như ở các nhà máy hóa chất, các nhà máy phóng xạ, trong đại dương, dưới

lòng đất hay các hành tinh khác…
1.6.2. Tình hình phát triển Robot và ứng dụng Robot hiện nay.
Với những ưu điểm trên, ngày nay Robot được ứng dụng rộng rãi trong sản
xuất ở các nước trên thế giới nhất là các nước có nền công nghiệp phát triển. Theo ước
tính của tổ chức UNECE thuộc liên đoàn Robot quốc tế IRF (Năm 2003) thì hiện nay
trên toàn thế giới có khoảng 770.000 robot đang được sử dụng trong các lĩnh vực sản
xuất công nghiệp. Trong số đó, Nhật Bản là nước có số lượng Robot dùng trong sản
xuất công nghiệp nhiều nhất trên thế giới với khoảng 350.000 (chiếm 45,5%), 233.000
(chiếm 30,3%) ở các nước EU và khoảng 104.000 (chiểm 13,5%) ở Bắc Mỹ. Ở châu

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Âu, Đức dẫn đầu với 105.000 Robot, tiếp theo là Italia với 47.000, Pháp 24.000, Tây
Ban Nha 18.000 Robot và 14.000 Robot.
Những lĩnh vực công nghiệp sử dụng Robot nhiều nhất là công nghiệp ô tô,
công nghiệp điện và điện tử, công nghiệp chế tạo máy và công nghiệp chế biến thực
phẩm. Những công việc thường được áp dụng Robot là hàn, lắp ráp, vận chuyển sản
phẩm, cấp phôi trong các dây truyền tự động. Tính linh hoạt trong vận hành khai thác,
khả năng hoạt động tinh vi, nhanh và chuẩn xác, khả năng thay thế con người trong
môi trường làm việc độc hại và không an toàn là những yếu tố quyết định của việc áp
dụng Robot trong các lĩnh vực công nghiệp nói trên.
Một số robot được chế tạo và ứng dụng vào cuộc sống:

Hình 1.12: Robot phun bê tông sản xuất tại vinacrete Việt Nam.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 1.13: Robot hàn hồ quang 3 BTD RRP.

Hình 1.14: Robot người máy ASIMO (Nhật Bản).
SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp
1.7.

GVHD: TS. Đinh Thị Thanh Huyền

Thiết kế mô hình Robot 3 bậc tự do trên phần mềm Topsolid.
Thiết kế mô hình là một nhiệm vụ rất quan trọng trước khi tính toán động học và
mô phỏng Robot. Việc thiết kế mô hình có ý nghĩa quan trọng, nó giúp cho nhà thiết
kế có thể nghiên cứu biết được đặc điểm, kết cấu của từng khâu trên Robot cũng như
toàn thể Robot. Tùy theo yêu cầu làm việc mà ta xây dựng mô hình với hình dáng,
kích thước mô hình Robot khác sao cho hợp lý, đảm bảo được các yêu cầu như: độ
cững vững, thao tác dễ dàng trong quá trình làm việc…Hiện nay, khoa học kỹ thuật
ngày càng phát triển, tiêu biểu là máy tính, laptop với sự trợ giúp của nhiều phần mềm
vẽ, thiết kế mô hình 3D giúp cho việc thiết kế mô hình Robot trở nên dễ dàng. Điển
hình trong số đó phải kể đến các phần mềm như: AutoCAD, Solidworks, Catia,
Topsolid,…
Trong đồ án tốt nghiệp này em sử dụng phần mềm Topsolid để xây dựng mô

hình robot nhờ những ưu điểm như khả năng đồ họa mạnh, kết hợp với sự hiểu biết
trước đó từ môn học “Thực tập chuyên ngành”, thư viện phần tử lớn thuận lợi trong
thiết kế, cho phép xuất ra kết quả dưới nhiều dạng khác nhau. Phần mềm Topsolid là
hệ thống CAD/CAM/CAE 3D hoàn chỉnh và có thể gia công chi tiết khi muốn chế tạo
Robot. Vì vậy em chọn phần mềm này để thiết kế Robot.

Hình 1.15 : Mô hình robot RRP trên TopSolid.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền
CHƯƠNG 2: ĐỘNG HỌC ROBOT RRP.

2.1.

Động học thuận.

2.1.1. Tổng quan.
- Để mô tả Robot ta đặt lên mỗi khâu của Robot một hệ quy chiếu và sử dụng các
-

phép biến đổi thuần nhất để mô tả vị trí tương đối và hướng của các hệ quy chiếu.
i và khâu liền kề trước
Denavit đã gọi biến đổi thuần nhất mô tả quan hệ giữa khâu
đó là i-1 là một ma trận

-

i −1
i

T

Như vậy ta có thể mô tả vị trí và hướng của một khâu bất kỳ i trong hệ quy chiếu
gốc:
0
i

-

T = 10T 12T ...ii −−12T

i −1
i

T

Khi nghiên cứu về động học Robot ta thường chú ý đến vị trí và hướng của khâu
cuối cùng (bàn tay máy, mỏ kẹp, tay kẹp).

-

Xét bàn tay máy của Robot gồm n khâu, bàn tay máy được mô tả bởi:
0

+ Hệ quy chiếu gốc:

Pnorg =  p x p y p z 

T

đặt ở giữa điểm các ngón tay.

Hình 2.1 Các véc tơ vị trí và hướng của bàn tay máy
+ Ba vector đơn vị mô tả hướng của bàn tay máy:
• Vector tiếp cận đến đối tượng:
• Vector cầm nắm đối tượng:
Vector pháp tuyến:
Ma trận biến đổi thuần nhất mô tả bàn tay máy trong hệ tọa độ gốc:
•

-

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

 nx
n
0
 y
nT =
 nz


0

ox
oy

ax
ay

oz
0

az
0

px 
0

nR

py  
= K K K
pz  
  0 0 0
1 

M 0 Pnorg
K KKK
M
1







2.1.2. Bộ thông số DH.
-

Khâu bất kỳ luôn được đặc trưng bởi hai kích thước:

ai
+ Độ dài pháp tuyến chung:

ai
+ Góc giữa các trục trong mặt phẳng vuông góc với

:

ai
Người ta thường gọi:

là chiều dài, là góc xoắn của khâu.

Hình 2.2. Các thông số đặc trưng của một khâu.
-

Vị trí tương dối của hai khâu được xác định bởi:
di
+

là khoảng cách giữa các pháp tuyến chung đo dọc theo trục khớp i.

θi

+

là góc giữa các pháp tuyến chung trong mặt phẳng vuông góc với trục.

θi

di
Người ta thường gọi

là khoảng cách giữa các khâu,

là góc giữa các khâu.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 2.3. Vị trí tương đối của hai khâu kề nhau.
-

Nguyên tắc gắn hệ quy chiếu vào mỗi khâu:
Ζi
+ Trục

của hệ quy chiếu gắn lên khâu thứ i đặt dọc theo trục khớp thứ i.

ai
+ Gốc của hệ quy chiếu gắn lên khâu thứ i đặt tại giao điểm của pháp tuyến
với trục khớp thứ i, chú ý: Gốc tọa độ đặt tại điểm cắt khi hai trục cắt nhau. Gốc
tọa độ chọn thích hợp khi các trục khớp song song với nhau.
Xi
+ Trục
được đặt dọc theo pháp tuyến chung và hướng từ khớp i đến i+1.

X i = Z i x Z i +1

Xi

Trường hợp các trục khớp cắt nhau thì trục
chọn theo tích vector
Yi
+ Trục còn lại xác định theo quy tắc bàn tay phải.
2.1.3. Đặc trưng ma trận biến đổi thuần nhất (MTBĐTN) trong Robot.
- Khi đã đặt các hệ quy chiếu lên các khâu của Robot, ta có thể thiết lập mối quan
hệ giữa các hệ quy chiếu liên tiếp nhau bởi MTBĐTN như sau:
X i −1
• Quay quanh trục
một góc xoắn .
X i −1
• Tịnh tiến dọc trục
một đoạn .
Ζi
θi
•

một góc .
Quay quanh trục
Ζi
di
• Tịnh tiến dọc trục
một đoạn .

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Hình 2.4. Quan hệ giữa hai hệ quy chiếu liên tiếp nhau.
Ta có thể viết bằng biểu thức toán như sau:
T = Rot ( X ,α i −1 ) Trans ( α i −1 , 0, 0 ) Rot ( Z , θi ) Trans ( 0, 0, d i )

i −1
i

Trong đó:
cos θi
 sin θ
i
Rot ( Z , θi ) = 
 0

 0
1

0
Trans ( ai −1 , 0,0 ) = 
0

0

− sin θi
cos θi
0
0
0
1
0
0

0 0
1
0
0 0 
Trans ( 0, 0, di ) = 
0
1 0


0 1
0

0 ai −1 
0 0 
1 0 


0 1 

Rot ( X , α i −1 )

0 0 0
1 0 0 
0 1 di 

0 0 1

0
1
 0 cos α
i −1
=
 0 sin α i −1

0
0

0
− sin α i −1
cos α i −1
0

0
0
0


1

Biến đổi biểu thức trên ta có:
cos θi

cos α .sin θ
i −1
i
i −1

iT =
 sin α i −1.sin θi

0


− sin θi
cos α i −1.cos θi
sin α i −1.cos θi
0

ai −1

−di sin α i −1 
di cos α i −1 

1


0

− sin α i −1
cos α i −1
0

2.1.4. Ma trận biến đổi thuần nhất mô tả bàn tay máy.
Khi ta đã biết hết các MTBĐTN của các hệ quy chiếu gắn trên các khâu của

Robot thì ta xác định MTBĐTN của bàn tay máy như sau:

T = 01T . 02T ...

0
T

n −1
n

T . TnT .

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

Trong đó:
n là số khâu của Robot.
T là hệ quy chiếu gắn lên bàn tay máy.
0 là hệ quy chiếu gốc của Robot.

Trong trường hợp tổng quát, khi xét quan hệ của Robot so với các thiết bị khác.

Giả sử đã biết

R
B

T , TGT

R
G

T

và

B
T

T

thì

xác định như sau:

T = RBT . BTT . GT T => BTT = RBT −1. GRT . GTT −1

R
G

.

Hình 2.5. Tay máy Robot đang tiếp cận bulông.
2.1.5. Trình tự thiết lập hệ phương trình động học.


Chọn hệ quy chiếu cơ sở, gắn các hệ quy chiếu suy rộng lên các khâu:

-

Giả định một vị trí ban đầu của Robot.

-

Chọn các gốc của hệ quy chiếu Oi.
Zi
Chọn các trục
cùng phương với trục của khớp thứ i.
Xi
Zi
Z i +1
Zi
Z i +1
αi
Zi
của
thành
và góc của
với
chính là . Nếu

và
Chọn các trục

-

Z i +1

theo
-

Xi
song song hoặc trùng nhau thì có thể căn cứ vào nguyên tắc chung hay chọn

X i +1

.

Các hệ quy chiếu phải tuân theo quy tắc bàn tay phải.
SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp


GVHD: TS. Đinh Thị Thanh Huyền

Khi gắn hệ quy chiếu lên các khâu, phải tuân theo các phép biến đổi của

MTBĐT.
T = Rot(X ,α i −1 ).Trans (α i −1 , 0, 0).Rot ( Z , θi ).Trans(0, 0, d i ).

i −1
i

Nếu xuất hiện phép quay quanh trục
(

Yi −1

thì vị trí ban đầu của Robot giả định là không

đúng cần phải chọn lại vị trí ban đầu cho Robot).
 Lập bảng thông số DH.
i

α i −1

ai −1

di

θi

1
2
3
…

…
…

….
…

…
…
…
…

…
…
…
…

…
…
…
…

 Dựa vào các thông số DH xác định MTBĐTN của từng khâu
 Tính các MTBĐTN biểu diễn vị trí và hướng của các khâu so với hệ quy chiếu

gốc.

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

2.1.6. Bài toán động học thuận Robot RRP.
Từ mô hình thiết kế trên phần mềm TopSolid, ta đơn giản hóa mô hình sau đó
đặt các hệ trục tọa độ cho từng khâu như sau:

Dựa vào mô hình ta xây dựng được bảng thông số DH như sau:
Bảng 2.1. Bảng thông số DH của Robot 3 bậc tự do RRP.
Khâu i
1
2

0
90

0
0

3
4

90
0

0
0

q2 + 90°

0

0

90

Thay các giá trị từ bảng DH vào ma trận tổng quát
Ta có:

c1
s
0
 1
1T =
0

0

− s1 0 0 
c1 0 0 
0 1 l1 

0 0 1

π
π

 c(q 2 + 2 ) −s(q 2 + 2 ) 0

0
0
−1

1

2T = 
π
π
 s(q 2 + ) c(q 2 + ) 0
2
2


0
0
0


0
 − s2
 
0  0
 = c
0  2
  0
1 

−c2
0
− s2
0

0 0
−1 0 
0 0


0 1

SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp
1
0
2

T
=
3
0

0

GVHD: TS. Đinh Thị Thanh Huyền

0 0
0 
0 −1 l2 + q3 
1 0
0 

0 0
1 

 − c1 .s 2

 − s .s
0
0
1
 1 2
2T = 1T . 2T =
 c2

 0

−c1.s2

0
0
2
 − s1.s2
3T = 2T . 3T =
 c2

 0

−c1.c 2
− s1.c 2
− s2
0

1
0
0
0

0
0
1
0

0
0 
l3 

1

s1 0 
−c1 0 
0 l1 

0 1

− s1 c1.c2
−c1 s1.c2
0

s2

0

0

−c1.s2
− s1 .s2

c2
0

 s1
 −c
0
0
3
 1
4T = 3T . 4T =
 0

 0

0
1
3

T
=
4
0

0

c1 .c2
s1 .c2
s2
0
0

1

c1.c2 . ( l2 + q3 ) 

c2 .s1. ( l2 + q3 ) 
l1 + s2 . ( l2 + q3 ) 

1

l3 .c1 .c2 − (l2 + q3 ).c1 .c2 
l3 .s1 .c2 − (l2 + q3 ).s1 .c2 
l1 − l3 .s2 + (l2 + q3 ).s2 

1


T

Cân bằng các hệ số

với các hệ số mô tả vị trí và hướng của bàn tay Robot, ta

có hệ phương trình động học Robot RRP:
 nx
n
 y
 nz

0

ox
oy
oz
0

ax
ay
az
0

px 
p y 
pz 

1

=

 s1
 −c
 1
 0

 0

−c1.s2
− s1.s2
c2
0

c1 .c2
s1.c2
s2
0

l3 .c1 .c2 − (l2 + q3 ).c1 .c2 
l3 .s1 .c2 − (l2 + q3 ).s1 .c2 
l1 − l3 .s2 + (l2 + q3 ).s2 

1


(2.1)

Phương trình (2.1) gọi là phương trình động học của Robot RRP.
Code Matlab thiết lập phương trình động học.
function [px, py, pz] = giaidonghocthuan(q1, q2, q3)
% Hang so
l1 = 0.5;l2 = 0.50; l3 = 1.50 ;

% chieu dai l1 l2 l3 (cm)

%Ma tran bien doi thuan nhat
T01= [cos(q1) -sin(q1) 0 0;sin(q1) cos(q1) 0 0;0 0 1 l1;0 0 0 1] ;
T12= [cos(q2+pi/2) -sin(q2+pi/2) 0 0;0 0 -1 0;sin(q2+pi/2) cos(q2+pi/2) 0 0;0 0 0 1];
SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53


Đồ án tốt nghiệp

GVHD: TS. Đinh Thị Thanh Huyền

T23= [1 0 0 0;0 0 -1 l2+q3;0 1 0 0;0 0 0 1] ;
T34= [0 1 0 0;1 0 0 0 ;0 0 1 l3;0 0 0 1];
T01;
T02= T01*T12;
T03= T02*T23;
T04= T03*T34;
px = l3*cos(q1)*sin(pi/2 + q2) - cos(q1)*sin(pi/2 + q2)*(l2 + q3);
py = l3*sin(q1)*sin(pi/2 + q2) - sin(q1)*sin(pi/2 + q2)*(l2 + q3);
pz = l1 + cos(pi/2 + q2)*(l2 + q3) - l3*cos(pi/2 + q2);
Sau khi thành lập được phương trình động học thuận ta sẽ tìm không gian làm
việc của Robot. Sau đây là chương trình Matlab tìm không gian làm việc.
clc; clear all;
l1=0.5; %chieu dai khau 1 (m)
l2=0.5; %chieu dai khau 2 (m)
l3=1.5; %chieu dai khau 3 (m)
T04=zeros(4,4);
theta1=0:2*pi/100:2*pi;
theta2=0:pi/50:pi;
d3=0:0.5:20;
X = zeros(length(theta1)*length(theta2)*length(d3),1);
Y = zeros(length(theta1)*length(theta2)*length(d3),1);
Z = zeros(length(theta1)*length(theta2)*length(d3),1);
stt = 0;
for i = 1: length(theta1)
for j = 1: length(theta2)
for k = 1: length(d3)
T01=[cos(theta1(i)) -sin(theta1(i)) 0 0;sin(theta1(i)) cos(theta1(i)) 0 0;0 0 1 l1; 0 0 0
1];

T12=[cos(pi/2+theta2(j)) -sin(pi/2+theta2(j)) 0 0;0 0 -1 0;sin(pi/2+theta2(j))
cos(pi/2+theta2(j)) 0 0;0 0 0 1];
SVTH: Lê Bá Thành – Lớp: Cơ Điện Tử K53