ĐỒ ÁN MÔN HỌC Đề tài ĐIỀU KHIỂN CÁNH TAY ROBOT

TRƯỜNG ĐẠI HỌC GIAO THƠNG VẬN TẢI TP. HỒ CHÍ MINH
KHOA ĐIỆN – ĐIỆN TỬ VIỄN THÔNG

ĐỒ ÁN MÔN HỌC
Đề tài:
ĐIỀU KHIỂN CÁNH TAY ROBOT
Ngành:

Kỹ thuật điều khiển và tự động hóa.

Chun ngành: Tự động hóa cơng nghiệp.

Giảng viên hướng dẫn : Th.s Trương Ngọc Bảo
Nhóm sinh viên thực hiện

:

Họ và tên

MSSV

Lớp

Trần Đình Cung

1751050004

TD17A

Phạm Anh Kiệt

1751050016

TD17A

Nguyễn Tấn Lập

1751050024

TD17A

Nguyễn Văn Phương

1751050036

TD17A

Nguyễn Thanh Quy

1551030318

TD17A

Lê Sỹ Thức

1751050046

TD17A

TP. Hồ Chí Minh, 2021

NHẬN XÉT CỦA GIẢNG VIÊN

.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................
.............................................................................................................................


MỤC LỤC
MỤC LỤC HÌNH VÀ BẢNG .......................................................................................... iii
Mục lục hình.................................................................................................................. iii
Mục lục bảng ................................................................................................................. iv

CHƯƠNG 1.

TỔNG QUÁT VỀ ROBOT .................................................................... 1

1.1. Sơ lược về phát triển Robot ................................................................................ 1
1.2. Những ứng dụng điển hình của Robot ............................................................... 2
1.3. Phạm vi đề tài ....................................................................................................... 3
1.4. Nội dung nghiên cứu ............................................................................................ 3
CHƯƠNG 2.

CƠ SỞ LÝ THUYẾT ............................................................................. 4

2.1. Khái niệm ban đầu ............................................................................................... 4
2.1.1.

Hệ tọa độ ...................................................................................................... 4

2.1.2.

Bậc tự do của robot (DOF: Degree Of Freedom) ........................................ 5

2.1.3.

Bài toán động học thuận .............................................................................. 5

2.1.4.

Bài tốn động học ngược ............................................................................. 5

2.2. Phương trình động học của robot....................................................................... 5

2.2.1.

Dẫn nhập ...................................................................................................... 5

2.2.2.

Bộ thông số Denavit – Hartenberg (DH) ..................................................... 7

2.2.3.

Ma trận A của các khâu ............................................................................... 9

2.2.4.

Xác định ma trận biến đổi .......................................................................... 10

2.3. Thiết lập hệ phương trình động học của robot ............................................... 11
2.3.1.

Trình tự thiết lập hệ phương trình.............................................................. 11

2.3.2.

Phương trình động học cánh tay robot của đồ án ...................................... 11

2.3.3.

Bài toán động học thuận ............................................................................ 15

2.4. Phương trình động học ngược .......................................................................... 16

2.4.1.

Các điều kiện của bài toán động học ngược .............................................. 16

2.4.2.

Bài toán động học ngược ........................................................................... 17

CHƯƠNG 3.

NỘI DUNG NGHIÊN CỨU ................................................................ 20

3.1. Giới thiệu về phần mềm và phần cứng ............................................................ 20
3.1.1.

Cánh tay robot EEZY ................................................................................ 20

3.1.2.

Động cơ servo SG90 .................................................................................. 20

3.1.3.

Mạch Arduino UNO R3............................................................................. 21

3.1.4.

Board mở rộng Arduino Nano ................................................................... 22
i

3.1.5.

Dây kết nối ................................................................................................. 23

3.1.6.

Mơ hình hồn chỉnh ................................................................................... 23

3.2. Giới thiệu phần mềm ......................................................................................... 24
3.2.1.

Phần mềm Matlab ...................................................................................... 24

3.2.2.

Các tính năng của Matlab .......................................................................... 24

3.3. Thi cơng phần cứng ........................................................................................... 25
3.3.1.

Sơ đồ đấu nối thực tế ................................................................................. 25

3.3.2.

Phần cứng thực tế....................................................................................... 26

3.3.3.

Giới hạn phần cứng .................................................................................... 26

3.4. Thiết kế phần mềm ............................................................................................ 26
3.4.1.

Lưu đồ giải thuật ........................................................................................ 26

3.4.2.

Hàm giải bài toán động học thuận ............................................................. 27

3.4.3.

Hàm giải bài toán động học ngược ............................................................ 28

3.4.4.

Kết nối Matlab với Arduino....................................................................... 28

3.4.5.

Điều khiển động cơ .................................................................................... 29

3.4.6.

Thiết kế giao diện ...................................................................................... 31

CHƯƠNG 4.

KẾT QUẢ NGHIÊN CỨU .................................................................. 33

4.1. Kết quả của đồ án .............................................................................................. 33
4.2. Hạn chế của đồ án .............................................................................................. 33
TÀI LIỆU THAM KHẢO............................................................................................... 34

ii


MỤC LỤC HÌNH VÀ BẢNG
Mục lục hình
Hình 2.1. Các tọa độ suy rộng của robot. .......................................................................... 4
Hình 2.2. Quy tắc bàn tay phải. ......................................................................................... 4
Hình 2.3. Các vector định vị trí và hướng của bàn tay máy ............................................. 6
Hình 2.4. Chiều dài và góc xoắn của 1 khâu .................................................................... 7
Hình 2.5. Các thơng số của khâu θ,d,a,α . ........................................................................ 8
Hình 2.6. Vật thể và robot ................................................................................................ 10
Hình 2.7. Tốn đồ vị trí của robot ................................................................................... 10
Hình 2.8. Mơ hình 3D cánh tay robot ............................................................................. 12
Hình 2.9. Mơ hình đơn giản và đặt hệ trục các khâu của robot .................................... 12
Hình 2.10. Quy ước chiều dương tọa độ của khâu chấp hành cuối .............................. 15
Hình 2.11. Khoảng cách giữa điểm cuối tính tốn và thực tế........................................ 16
Hình 2.12. Góc quay của các khâu .................................................................................. 17
Hình 3.1. Cánh tay Robot. ................................................................................................ 20
Hình 3.2. Động cơ servo SG90. ........................................................................................ 20
Hình 3.3. Nguyên lý hoạt động động cơ servo SG90. ..................................................... 21
Hình 3.4. Mạch Arduino UNO R3. .................................................................................. 22
Hình 3.5. Board mở rộng Arduino Nano. ....................................................................... 22
Hình 3.6. Mặt sau Shield Mở Rộng Arduino NaNo. ...................................................... 23
Hình 3.7. Dây kết nối........................................................................................................ 23
Hình 3.8. Mơ hình hồn chỉnh. ....................................................................................... 23
Hình 3.9. Icon Matlab. ..................................................................................................... 24

Hình 3.10. Giao diện làm việc của Matlab và Simulink. ................................................ 25
Hình 3.11. Sơ đồ đấu nối thực tế. .................................................................................... 25
Hình 3.12. Mơ hình phần cứng thực tế. .......................................................................... 26
Hình 3.13. Lưu đồ giải thuật của phần mềm .................................................................. 27
Hình 3.14. Góc quay của trục động cơ ............................................................................ 29
Hình 3.15. So sánh giữa trục động cơ khâu 1 và hệ trục khâu 1 ban đầu .................... 30
Hình 3.16. So sánh giữa trục động cơ khâu 2 và hệ trục khâu 2 ban đầu .................... 30
Hình 3.17. So sánh giữa trục động cơ khâu 3 và hệ trục khâu 3 ban đầu .................... 31
Hình 3.18. Giao diện phần mềm điều khiển ................................................................... 32
iii


Hình 3.19. Trường hợp đặc biệt của động học thuận .................................................... 33

Mục lục bảng
Bảng 2.1. Bảng DH của robot. ......................................................................................... 12
Bảng 3.1. Code hàm giải bài toán động học thuận ......................................................... 27
Bảng 3.2. Code hàm giải bài toán động học ngược ........................................................ 28
Bảng 3.3. Code kết nối Arduino với Matlab .................................................................... 28
Bảng 3.4. Code điều khiển động cơ theo góc mong muốn ............................................. 31

iv


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

CHƯƠNG 1. TỔNG QUÁT VỀ ROBOT
1.1. Sơ lược về phát triển Robot

Nhu cầu nâng cao năng suất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng
rộng rãi các phương tện tự động hóa sản xuất. Xu hướng tạo ra các dây chuyền và thiết
bị tự động có tính linh hoạt cao đang hình thành. Các thiết bị nay đang thay thế dần các
máy tự động ‘cứng’ chỉ đáp ứng một việc nhất định trong khi thị trường ln địi hỏi
thay đổi mặt hàng về chủng loại, về kích cỡ và tính năng… Vì thế ngày càng tăng nhanh
nhu cầu ứng dụng robot để tạo ra các hệ thống sản xuất tự động linh hoạt.
Thuật ngữ ‘robot’ lần đầu tiên xuất hiện năm 1922 trong tác phẩm ‘Rossum’s
Universal Robot’ của Karel Capek. Theo tiếng Séc thì robot là người làm tạp dịch. Trong
tác phẩm này nhân vật Rossum là con trai của ông đã tạo ra nhưng chiếc máy gần giống
như con người để hầu hạ con người.
Hơn 20 năm sau, ước mơ viễn tưởng của Karel Capek bắt đầu hiện thực. Ngay sau
chiến tranh thế giới thứ 2, ở Hoa Kỳ đã xuất hiện những tay máy chép hình điều khiển
từ xa trong các phịng thí nghiệm về vật liệu phóng xạ.
Vào cuối những năm 50, bên cạnh những máy chép hình cơ khí đó, đã xuất hiện
những loại máy chép hình thủy lực và điện từ, như tay máy Minitaur I hoặc tay máy
Handyman của Genaral Electric. Năm 1954 George C.Devol đã thiết kế 1 thiết vị có tên
là ‘cơ cấu bản lề’ dùng để chuyển hàng theo chương trình. Đến năm 1956 Devol cung
với Goseph F. Engelber, một kỹ sư trẻ của công nghiệp hàng không, đã tạo ra loại robot
công nghiệp đầu tiên năm 1559 ở công ty Unimation. Chi đến năm 1975 Công ty
Unimation đã bắt đầu có lợi nhuận từ sản phẩm robot đầu tiền này.
Chiếc robot công nghiệp được đưa vào ứng dụng đầu tiên, năm 1961, ở một nhà
máy ô tô của General Motors tại Trenton, New Jersey Hoa Kỳ.
Năm 1967, Nhật Bản mới nhập chiếc robot công nghiệp đầu tiên từ Công ty AMF.
Đến năm 1990 có hơn 40 Cơng ty Nhật Bản, trong đó có những Cơng ty khổng lồ như
Cơng ty Hatachi và Công ty Mitsubishi, đã đưa ra thị trường quốc tế nhiều loại robot
nổi tiếng.
Từ những năm 70 việc nghiên cứu nâng cao tính năng của robot đã chi nhiều đến
sự lắp đặt thêm các cảm biến ngoại tín hiệu để nhận biết môi trường làm việc. Tại trường
đại học tổng hợp Ford người ta đã tạo loại robor lắp ráp tự động điều khiển bằng máy
tính trên cơ sỡ xử lý thông tin từ các cảm biển lực và thị giác. Vào thời gian này Công

ty IBM đã chế tạo loại robot có nhưng loại cảm biến xúc giác và cảm biến lực, điều
khiển bằng máy tính để lắp ráp các máy in gồm 20 cụm chi tiết.
Nhóm trưởng: Nguyễn Thanh Quy

1


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

Vào giai đoạn này ở nhiều nước khác cũng tiến hành các công trình nghiên cứu
tương tự, tọa ra các loại robot đều khiển bằng máy vi tính, có lắp đặt các thiết bị cảm
biến và thiết bị ngoại tiếp người – máy.
Một lĩnh vực mà nhiều phịng thí nghiệm quan tâm là việc chế tọa robot tự hành.
Các cơng trình nghiên cứu tọa ra robot tự hành theo hướng bắt chước chân người và súc
vật. Các robot này cịn chưa có nhiều ứng dụng trong công nghiệp tuy nhiên các loại xe
robot (robocar) lại nhanh chóng được đưa vào hoạt động trong các hệ thống sản xuất tự
động linh hoạt.
Từ những năm 80, nhất là vào những năm 90, áp dụng rộng rãi các tiến bộ kỹ thuật
về vi xử lý và công nghệ thông tin, số lượng robot công nghiệp đã gia tăng, giá thành
giảm đi rõ rệt, tính năng đã có nhiều bước tiến vượt bậc. Nhờ vậy robot cơng nghiệp đã
có vị trí quan trọng trong các dây chuyền sản xuất hiện đại.
Ngày này chuyên ngành khoa học về robot ‘robotics’ đã trở thành một lĩnh vực
rộng trong khoa học, bao gồm các vấn đề cấu trúc cơ bản động học, lập trình quỹ đạo,
cảm biến tín hiệu, điều khiển chuyển động,…
Robot công nghiệp được hiểu là những thiết bị tự động linh hoạt, bắt chước được
các chức năng lao động cơng nghiệp của con người. Nói đến thiết bị tự động linh hoạt
là nói đến khả năng thao tác với nhiều bậc tự do, được điều khiển trợ động và lập trình
thay đổi được. Cịn nói đến sự bắt chước các chức năng lao động công nghiệp của con

người là có ý nói đến sự khơng hạn chế từ các chức năng lao động chân tay đơn giản
đến trí khơn nhân tạo, tùy theo loại hình cơng việc lao động cần đến chức năng đó hay
khơng. Đồng thời cũng nói đến mức độ cần thiết bắt chước được như con người hay
khơng.
1.2. Những ứng dụng điển hình của Robot
 Ứng dụng trong công nghiệp.
 Lắp đặt vật liệu, hàn điểm và phun sơn.
 Phục vụ máy công cụ, làm khuôn trong công nghiệp đồ nhựa,…
 Ứng dụng trong phịng thi nghiệm.
 Dùng để thực hiện các cơng việc thủ công, thực hiện các công việc lặp đi lặp
lại.
 Ứng dụng robo trong thao tác cần khuyếch đại lực.
 Dùng trong những khu vực nguy hiểm (nhiễm xạ,….)

Nhóm trưởng: Nguyễn Thanh Quy

2


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

 Dùng bốc dỡ hàng hóa, vật liệu, phơ có trọng lượng lớn cồng kềnh trong các
ngành công nghiệp nặng…
 Ứng dụng robot trong nông nghiệp.
 Robot cát long cừu, mổ xẻ thịt heo,…
 Ứng dụng trong không gian.
 Tay máy chế tạo nhằm tăng cường khả năng bốc dỡ hàng hóa tiếp tế, lắp ghép
với các trạm không gian.

 Các xe tự hành trang thiết bị tay máy linh hoạt, các robot bảo trì vệ tính, xây
dựng trong khơng gian…
 Ứng dụng trong tàu lặn.
 Phát triển các tàu lặn không người lái trong cơng tác kiểm tra, dị tìm thám
hiểm dưới đại dương.
 Ứng dụng trong giáo dục.
 Robot được sử dụng là phương tiện giảng dạy trong các chương trình giáo
dục.
 Robot kết hợp được sử dụng với ngôn ngữ LOGO để giảng dạy và nhận thức
máy tính.
1.3. Phạm vi đề tài
Đối tượng nghiên cứu của đề tài là mô hình cánh tay robot 5 bậc tự do sử dụng
mạch Arduino để điều khiển. Nghiên cứu tổng quan về cấu tạo nguyên lý hoạt động của
cánh tay robot và mạch điều khiển. Vận dụng kiến thức để vận hành và lập trình điều
khiển cánh tay robot.
1.4. Nội dung nghiên cứu
 Nghiên cứu cơ sở khoa học và thực tiễn về chế tại và điều khiển robot.
 Nghiên cứu về phương trình động học người của robot.
 Nghiên cứu và ứng dụng của các phần gia cơng cơ khí để chế tạo cánh tay robot.

 Nghiên cứu và ứng dụng mạch Arduino để điều khiển cánh tay robot.

Nhóm trưởng: Nguyễn Thanh Quy

3


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

CHƯƠNG 2. CƠ SỞ LÝ THUYẾT
2.1. Khái niệm ban đầu
2.1.1. Hệ tọa độ
Mỗi robot thường được 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ệ tọa độ gắn với khâu cơ bản gọi là hệ tọa độ cơ bản (hay hệ tọa độ chuẩn).
 Hệ tọa độ suy rộng là các hệ tọa độ trung gian khác gắn với các khâu động.
Trong từng thời điểm hoạt động, các tọa độ 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 của các khớp quay (hình
2.1). Các tọa độ suy rộng cịn gọi là biến khớp.

Hình 2.1. Các tọa độ suy rộng của robot.
Xác định các hệ tọa độ gắn với các khâu robot bằng phương pháp bàn tay phải:
 Dùng tay phải, nắm hai ngón tay
út và áp út vịng lịng bàn tay, xịe
3 ngón: cái, trỏ và giữa theo 3
phương vng 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 2.2).
Hình 2.2. Quy tắc bàn tay phải.
Nhóm trưởng: Nguyễn Thanh Quy

4


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

2.1.2. Bậc tự do của robot (DOF: Degree Of Freedom)
Bậc tự do của robot 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). Bậc tự do của nó có thể tính theo cơng thức:
5

w  6n   ipi

(2.1)

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 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 các khớp động.
2.1.3. Bài toán động học thuận
Cho trước cơ cấu và quy luật của các yếu tố chuyển động thể hiện bằng các tọa độ
suy rộng qi ta phải xác định quy luật chuyển động của điểm trên khâu tác động cuối nói
riêng hoặc của điểm bất kỳ trên một khâu nào đó của tay máy nói chung trong hệ trục
tọa độ vng góc.
2.1.4. Bài tốn động học ngược
Cho trước cơ cấu và quy luật chuyển động của điểm trên khâu tác động cuối hoặc
quy luật chuyển động của khâu cuối bao gồm vị trí và hướng của nó được biểu diễn
trong hệ trục tọa độ vng góc, ta phải xác định quy luật chuyển động của các khâu
thành viên thể hiện thông qua các hệ tọa độ suy rộng qi .

2.2. Phương trình động học của robot
2.2.1. Dẫn nhập
Bất kỳ một robot nào cũng có thể coi là một tập hợp các khâu (links) gắn với các
khớp (joints). Ta hãy đặt trên mỗi khâu của robot một hệ tọa độ. Sử dụng các phép biến
đổi thuần nhất có thể mơ tả vị trí tương đối và hướng giữa các hệ tọa độ này. Denavit.J.
đã gọi biến đổi thuần nhất mô tả mối quan hệ giữa một khâu và một khâu kế tiếp là một
ma trận A. Nói đơn giản hơn, một ma trận A là một mô tả biến đổi thuần nhất bởi phép
quay và phép tịnh tiến tương đối giữa hệ tọa độ của hai khâu liền nhau. A1 mơ tả vị trí
và hướng của khâu đầu tiên; A2 mơ tả vị trí và hướng của khâu thứ hai so với khâu thứ
nhất. Như vậy vị trí và hướng của khâu thứ hai so với hệ tọa độ gốc được biểu diễn bởi
ma trận:
T2  A1 A2

Nhóm trưởng: Nguyễn Thanh Quy

5


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

Cũng như vậy, A3 mô tả khâu thứ ba so với khâu thứ hai và:
T3  A1 A2 A3 ; ...

Cũng theo Denavit, tích của các ma trận A được gọi là ma trận T, thường có hai
chỉ số: trên và dưới. Chỉ số trên chỉ tọa độ tham chiếu tới, bỏ qua chỉ số trên nếu chỉ số
đó bằng 0. Chỉ số dưới thường dùng để chỉ khâu chấp hành cuối. Nếu một robot có 6
khâu ta có:
T6  A1 A2 A3 A4 A5 A6

(2.2)

T6 mô tả mối quan hệ về hướng và vị trí của khâu chấp hành cuối đối với hệ tọa

độ gốc. Một robot 6 khâu có thể có 6 bậc tự do và có thể xác định vị trí và định hướng
trong trường vận động của nó (range of motion). Ba bậc tự do xác định vị trí thuần túy
và ba bậc tự do khác xác định hướng mong muốn. T6 sẽ là ma trận trình bày cả hướng
và vị trí của robot. Hình 2.1 mơ tả quan hệ đó với bàn tay máy. Ta đặt gốc tọa độ của hệ
mơ tả tại điểm giữa của các ngón tay. Gốc tọa độ này được mô tả bởi vector p (xác định
vị trí của bàn tay). Ba vector đơn vị mô tả hướng của bàn tay được xác định như sau:

Hình 2.3. Các vector định vị trí và hướng của bàn tay máy
 Vector có hướng mà theo đó bàn tay sẽ tiếp cận đến đối tượng, gọi là vector a
(approach).
 Vector có hướng mà theo đó các ngón tay của bàn tay nắm vào nhau khi cầm
nắm đối tượng, gọi là vector o (occupation).
 Vector cuối cùng là vector pháp tuyến n (normal), do vậy ta có:
n  o.a

Chuyển vị T6 như vậy sẽ bao gồm các phần tử:

Nhóm trưởng: Nguyễn Thanh Quy

6


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

 nx
n
T6   y
 nz

0

Ox

ax

Oy

ay

Oz

az

0

0

px 
p y 
pz 

1

(2.3)

Tổng quát, ma trận T6 có thể biểu diễn gọn hơn như sau:
R
T6  
0

p
1 

(2.4)

Ma trận R có kích thước 3  3 , là ma trận trực giao biểu diễn hướng của bàn kẹp
(khâu chấp hành cuối) đối với hệ tọa độ cơ bản. Việc xác định hướng của khâu chấp
hành cuối cịn có thể thực hiện theo phép quay Euler hay phép quay Roll, Pitch, Yaw.
Vector điểm p có kích thước 3 1 , biểu diễn mối quan hệ tọa độ vị trí của gốc hệ
tọa độ gắn trên khâu chấp hành cuối đối với hệ tọa độ cơ bản.
2.2.2. Bộ thông số Denavit – Hartenberg (DH)
Một robot nhiều khâu cấu thành từ các khâu nối tiếp nhau thông qua các khớp
động. Gốc chuẩn (Base) của một robot là khâu số 0 và khơng tính vào số các khâu. Khâu
1 nối với khâu chuẩn bởi khớp 1 và khơng có khớp ở đầu mút của khâu cuối cùng. Bất
kỳ khâu nào cũng được đặc trưng bởi hai kích thước:
 Độ dài pháp tuyến chung: a n
 Góc giữa các trục trong mặt phẳng vng góc với a n :  n

Hình 2.4. Chiều dài và góc xoắn của 1 khâu
Thơng thường, người ta gọi a n là chiều dài và  n là góc xoắn của khâu (Hình 2.5).
Phổ biến là hai khâu liên kết với nhau ở chính trục của khớp (Hình 2.5).

Nhóm trưởng: Nguyễn Thanh Quy

7


ĐAMH: Robot cơng nghiệp

GVHD: Th.s Trương Ngọc Bảo

Hình 2.5. Các thơng số của khâu θ,d,a,α .
Mỗi trục sẽ có hai pháp tuyến với nó, mỗi pháp tuyến dùng cho mỗi khâu (trước
và sau một khớp). Vị trí tương đối của hai khâu liên kết như thế được xác định bởi d n
là khoảng cách giữa các pháp tuyến đo dọc theo trục khớp n và  n là góc giữa các pháp
tuyến đo trong mặt phẳng vng góc với trục.
d n và  n thường được gọi là khoảng cách và góc giữa các khâu.

Để mơ tả mối quan hệ giữa các khâu ta gắn vào mỗi khâu một hệ tọa độ. Nguyên
tắc chung để gắn hệ tọa độ lên các khâu như sau:
 Gốc của hệ tọa độ gắn lên khâu thứ n đặt tại giao điểm của pháp tuyến a n với
trục khớp thứ n  1. Trường hợp hai trục khớp cắt nhau, gốc tọa độ sẽ đặt tại
chính điểm cắt đó. Nếu các trục khớp song song với nhau, gốc tọa độ được chọn
trên trục khớp của khâu kế tiếp, tại điểm thích hợp.
 Trục z của hệ tọa độ gắn lên khâu thứ n đặt dọc theo trục khớp thứ n  1.
 Trục x thường được đặt dọc theo pháp tuyến chung và hướng từ khớp n đến
n  1. Trong trường hợp các trục khớp cắt nhau thì trục x chọn theo tích vector




zn . zn 1 .
Trường hợp khớp quay thì  n là các biến khớp, trong trường hợp khớp tịnh tiến

thì d n là biến khớp và an  0 .
Các thông số an , n , d n , n được gọi là thơng số bộ DH.

Nhóm trưởng: Nguyễn Thanh Quy

8


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

2.2.3. Ma trận A của các khâu
Trên cơ sở các hệ tọa độ đã ấn định cho tất cả các khâu liên kết của robot, ta có thể
thiết lập mối quan hệ giữa các hệ tọa độ nối tiếp nhau (n  1) và (n) bởi các phép quay
và tịnh tiến sau đây:
 Quay quanh zn1 một góc  n .
 Tính tiến dọc theo zn1 một khoảng d n .
 Tịnh tiến dọc theo xn1  xn một đoạn a n .
 Quay quanh xn một góc xoắn  n .
Bốn phép biến đổi thuần nhất này thể hiện quan hệ của hệ tạo độ thuộc khâu thứ n
so với hệ tọa độ thuộc khâu thứ n 1và tích của chúng được gọi là ma trận A:

An  Rot  z,  Trans  0,0, d  Trans  a,0,0  Rot  x,  
cos 
 sin 
An  
 0

 0

 sin 
cos 
0
0

cos 
 sin 
An = 
 0

 0

0 0  1
0 0  0
1 0  0

0 1  0

0 0 a  1
0


1 0 0  0 cos 
0 1 d  0 sin 

0 0 1  0
0

 sin  cos 

sin  sin 

cos cos 

 cos  sin 

sin 

cos 

0

0

0
 sin 
cos 
0

a cos  
a sin  
d 

1 

(2.5)

0
0 

0

1

(2.6)

Đối với khớp tịnh tiến  a  0, i  0  thì ma trận A có dạng:

0
1
0 cos 
An  
0 sin 

0
0

0
 sin 
cos 
0

0
0 
0

1

(2.7)

 Đối với một khâu đi theo một khớp quay thì d , a, là hằng số. Như vậy ma trận
A của khớp quay là một hàm số của biến khớp  .
 Đối với một khâu đi theo một khớp tịnh tiến thì  ,  là hằng số. Ma trận A của
khớp tịnh tiến là một hàm số của biến số d .

Nhóm trưởng: Nguyễn Thanh Quy

9


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

 Nếu các biến số được xác định thì giá trị của các ma trận A theo đó cũng được
xác định.
2.2.4. Xác định ma trận biến đổi
Ta đã biết: T6  A1 A2 A3 A4 A5 A6 .
Trong đó: T6 được miêu tả trong hệ tọa độ gốc (hệ tọa độ gắn với khâu cơ bản cố
định của robot). Nếu mô tả T6 theo các hệ tọa độ trung gian thứ n - 1 thì:
6

T6   Ai

n 1

i n

Trong trường hợp tổng quát, khi xét quan hệ của robot với các thiết bị khác, nếu
hệ tọa độ cở bản của robot có liên hệ với một hệ tọa độ nào đó bởi phép biến đổi Z. khâu

chấp hành cuối lại có gắn một cơng cụ, có quan hệ với vật thể bởi phép biến đổi E (hình
2.6) thì vị trí và hướng của điểm cuối của công cụ, khảo sát ở hệ tọa độ tham chiếu mô
tả bởi X sẽ được xác định bởi:
X  ZT6 E

Hình 2.6. Vật thể và robot
Quan hệ này được thể hiện trên tốn đồ sau:

Hình 2.7. Tốn đồ vị trí của robot
Nhóm trưởng: Nguyễn Thanh Quy

10


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

2.3. Thiết lập hệ phương trình động học của robot
2.3.1. Trình tự thiết lập hệ phương trình
Để thiết lập hệ phương trình động học của robot, ta tiến hành theo các bước sau:
 Chọn hệ tọa độ cơ sở, gắn các hệ tọa độ mở rộng lên các khâu. Việc gắn hệ tọa
độ lên các khâu đóng vai trị rất quan trọng khi xác lập hệ phương trình động
học của robot, thơng thường đây cũng là bước khó khăn nhất. Nguyên tắc gắn
hệ tọa độ lên các khâu đã được trình bày một cách tổng quát trong phần 2.2.2.
Trong thực tế, các trục khớp của robot thường song song hoặc vng góc với
nhau, đồng thời thông qua các phép biến đổi của ma trận A ta có thể xác định
các hệ tọa độ gắn trên các khâu của robot theo trình tự sau:
 Giả định một vị trí ban đầu của robot.
 Chọn gốc tọa độ O0 , O1 , ...

 Các trục z n phải chọn cùng phương với trục khớp thứ n+1 .
 Chọn trục xn là trục quay của z n thành zn1 và góc của z n với zn1 chính là

 n1 . Nếu z n và zn1 song song hoặc trùng nhau thì ta có thể căn cứ nguyên
tắc chung hay chọn xn theo xn1 .
 Các hệ tọa độ Oxyz phải tuân theo quy tắc bàn tay phải.
 Khi gắn hệ tọa độ lên các khâu, phải tuân thủ theo các phép biến đổi của ma
trận An , trong đó là bốn phép biến đổi được thể hiện như sau

An  Rot  z,  Trans  0,0, d  Trans  a,0,0  Rot  x,   . Nghĩa là ta coi hệ tọa
độ thứ n+1 là biến đổi của hệ tọa độ thứ n; các phép quay và tịnh tiến của
biến đổi này phải là một trong các phép biến đổi của An , các thông số DH
cũng được xác định dựa vào các phép biến đổi này. Trong quá trình gắn hệ
tọa độ lên các khâu, nếu xuất hiện phép quay của trục z n đối với zn1 quanh
trục yn 1 thì vị trí ban đầu của robot đã giả định là khơng đúng, ta cần chọn
lại vị trí ban đầu khác cho robot.
 Lập bảng thông số DH (Denavit – Hartenberg).
 Dựa vào các thông số DH xác định các ma trận An .
 Tính các ma trận T và viết các phương trình động học của robot.
2.3.2. Phương trình động học cánh tay robot của đồ án
Mơ hình 3D cánh tay robot có dạng như sau:

Nhóm trưởng: Nguyễn Thanh Quy

11


ĐAMH: Robot cơng nghiệp

GVHD: Th.s Trương Ngọc Bảo

Hình 2.8. Mơ hình 3D cánh tay robot
Để thiết lập phương trình động học cho robot, ta vẽ lại robot dưới dạng đơn giản
và đặt hệ trục tọa độ như sau:

Hình 2.9. Mơ hình đơn giản và đặt hệ trục các khâu của robot
Lập bảng thông số DH:
Bảng 2.1. Bảng DH của robot.
Khâu

a



d



1

0

90o

d1

1

2

d2

0

0

 2

3

d3

0

0

 3

Nhóm trưởng: Nguyễn Thanh Quy

12


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

Ma trận A tổng quát:

cos i

 sin 
i
An  
 0

 0

 cos  i sin i

sin  i sin i

cos  i cos i

 sin  i cos i

sin  i

cos  i

0

0

ai cos i 
ai sin  i 
di 

1 

Thay các giá trị từ bảng DH và viết gọn ta có:

cos 1
 sin 
1
A1  
 0

 0
cos 1
 sin 
1

 0

 0

 cos 1 sin 1

cos  2
 sin 
2
A2  
 0

 0

 cos  2 sin  2

sin  2 sin  2

cos  2 cos  2

 sin  2 cos  2

sin  2

cos  2

0

0

cos  2
 sin 
2

 0

 0
cos3
 sin 
3
A3  
 0

 0
cos3
 sin 
3


 0

 0

sin 1 sin 1

a1 cos 1 
cos 1 cos 1  sin 1 cos 1 a1 sin 1 
sin 1
cos 1
d1 

0
0
1 
0 sin 1
0  C1 0 S1 0 
0  cos 1 0   S1 0 C1 0 

1
0
d1   0 1 0 d1 
 

0
0
1 0 0 0
1

 sin  2

cos  2
0
0

0 d 2 cos  2  C2
0 d 2 sin  2   S2

1
0  0
 
0
1  0

 cos  3 sin 3

sin  3 sin 3

cos  3 cos3

 sin  3 cos3

sin  3

cos  3

0

0

 sin 3

cos3
0
0

0 d3 cos3  C3
0 d3 sin 3   S3

1
0  0
 
0
1  0

a2 cos  2 
a2 sin  2 
d2 

1 
 S 2 0 d 2 C2 
C2 0 d 2 S 2 
0 1
0 

0 0
1 
a3 cos3 
a3 sin 3 
d3 

1 

 S3 0 d3C3 
C3 0 d3 S3 
0 1
0 

0 0
1 

Các ma trận biến đổi thuần nhất tổng quát T :

Nhóm trưởng: Nguyễn Thanh Quy

13


ĐAMH: Robot công nghiệp

C3
S
2
T3  A3   3
0

0

 S3
C3
0
0

GVHD: Th.s Trương Ngọc Bảo

0 d3C3 
0 d3 S3 
1
0 

0
1 

 C2  S 2 0
S
C2 0
1
T3  A2 A3   2
0
0 1

0 0
0
C2C3  S2 S3
S C  C S
2 3
 2 3

0

0


d 2C2  C3  S3
d 2 S 2   S3 C3
0  0
0

1  0
0
C2 S3  S 2C3 0
 S 2 S3  C2C3
0
0

0 d3C3 
0 d3 S3 
1
0 

0
1 
d3C2C3  d3 S 2 S3  d 2C2 
0 d3 S 2C3  d3C2 S3  d 2 S 2 

1
0

0
1


T3  A1 A2 A3

0

0  C2C3  S 2 S3 C2 S3  S 2C3 0 d 3C2C3  d 3 S 2 S3  d 2C2 
C1 0 S1
 S 0 C 0   S C  C S  S S  C C 0 d S C  d C S  d S 
1
2 3
2 3
2 3
3 2 3
3 2 3
2 2
 2 3
 1
 0 1 0 d1  

0
0
1
0



1 
0
0
0
1
0 0 0


C1C2C3  C1S 2 S3 C1C2 S3  C1S 2C3 S1 d 3C1C2C3  d 3C1S 2 S 3  d 2C1C2 
S C C S S S
 S1C2 S3  S1S 2C3 C1 d 3 S1C2C3  d 3 S1S 2 S3  d 2 S1C2 
1 2 3
1 2 3


 S 2C3  C2 S3
 S 2 S3  C2C3
0
d 3 S 2C3  d 3C2 S3  d 2 S 2  d1 


0
0
0
1



Vậy hệ phương trình động học của robot là:

nx  C1C2C3  C1S 2 S3
n y  S1C2C3  S1S 2 S3
nz  S 2C3  C2 S3
Ox  C1C2 S3  C1S 2C3
Oy   S1C2 S3  S1S 2C3
Oz   S 2 S3  C2C3
ax  S1

a y  C1
az  0
px  d3C1C2C3  d3C1S 2 S3  d 2C1C2
p y  d3 S1C2C3  d3 S1S 2 S3  d 2 S1C2

Nhóm trưởng: Nguyễn Thanh Quy

14


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo
pz  d3 S2C3  d3C2 S3  d 2 S2  d1

2.3.3. Bài toán động học thuận
Bài toán động học thuận là khi biết trước góc quay của các biến khớp

q  1  2 3  , tìm tọa độ của khâu chấp hành cuối p   px
T

py

T

pz 1 .

Quy ước chiều dương của các trục tọa độ như sau:

Hình 2.10. Quy ước chiều dương tọa độ của khâu chấp hành cuối

Từ hệ phương trình động học ta thu được tọa độ của khâu chấp hành cuối:

 px  d3C1C2C3  d3C1S 2 S3  d 2C1C2

 p y  d3 S1C2C3  d3 S1S 2 S3  d 2 S1C2

 pz  d3 S 2C3  d3C2 S3  d 2 S 2  d1
Tuy nhiên, hệ phương trình tọa độ trên chỉ đúng với mơ hình đã được đơn giản
hóa (hình 2.7). Đối với robot thật, ta cần phải chuẩn hóa tọa độ ấy về điểm cuối thực tế
như sau:

 px  d3C1C2C3  d3C1S 2 S3  d 2C1C2  d5

 p y  d3 S1C2C3  d3S1S 2 S3  d 2 S1C2

 pz  d3 S2C3  d3C2 S3  d 2 S 2  d1  d 4

Nhóm trưởng: Nguyễn Thanh Quy

(2.8)

15


ĐAMH: Robot cơng nghiệp

GVHD: Th.s Trương Ngọc Bảo

Hình 2.11. Khoảng cách giữa điểm cuối tính tốn và thực tế
2.4. Phương trình động học ngược

2.4.1. Các điều kiện của bài tốn động học ngược
Việc giải bài toán động học ngược của robot cần thỏa mãn các điều kiện sau:
 Điều kiện tồn tại nghiệm:
 Điều kiện này nhằm khẳng định: có ít nhất một tệp nghiệm 1 , 2 ,..., 6 , d i  
sao cho robot có hình thể cho trước.
 (“Hình thể” là khái niệm mơ tả tường minh của vector cuối T6 cả về vị trí và
hướng).
 Điều kiện duy nhất của tệp nghiệm. Trong khi xác định các tệp nghiệm cần phân
biệt rõ hai nghiệm:
 Nghiệm tốn (Mathematical Solution): các nghiệm này thỏa mãn các phương
trình cho trước của T6 .
 Nghiệm vật lý (Physical Solution): là các tệp con của nghiệm toán, phụ thuộc
vào các giới hạn vật lý (giới hạn về góc quay, kích thước, ...) nhằm xác định
tệp nghiệm duy nhất.
Việc giải phương trình động học có thể được tiến hành theo hai phương pháp cơ
bản sau:

Nhóm trưởng: Nguyễn Thanh Quy

16


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

 Phương pháp giải tích (Analytical Method): tìm ra các cơng thức hay các
phương trình tốn giải tích biểu thị quan hệ giữa các giá trị của không gian biến
trục và các thông số khác của bộ thông số DH.
 Phương pháp số (Numberical Method): tìm ra các giá trị của tệp nghiệm bằng

kết quả của một q trình lặp.
2.4.2. Bài tốn động học ngược
Bài toán động học ngược là khi biết tọa độ của khâu chấp hành cuối

p   px

py

pz 1 tìm các góc xoay của các biến khớp q  1  2 3  .
T

T

Tương tự như bài toán động học thuận, ta phải chuẩn hóa tọa độ khâu chấp hành
cuối thực tế về điểm cuối của mơ hình đơn giản hóa. Ma trận p lúc này trở thành:

p   px  d5

py

pz  d 4 1

T

Để đơn giản hóa cơng thức, ta sẽ xem px  px  d5 , pz  pz  d 4 (đã chuẩn hóa
rồi).
Đối với robot của nhóm, các động cơ chỉ có thể quay được tối đa 180 độ. Nên khi
giải bài toán động học ngược, ta quy ước giới hạn vật lý lý tưởng (chưa xét đến lực và
va chạm cơ khí) của các khâu như sau:

90o  1  90o
 o
o
0   2  180

o
o
180  3  0
Ngồi ra có thể xem hình 2.10 để có thể hiểu được các góc quay lại có giới hạn
như vậy.

Hình 2.12. Góc quay của các khâu
Nhóm trưởng: Nguyễn Thanh Quy

17


ĐAMH: Robot cơng nghiệp

GVHD: Th.s Trương Ngọc Bảo

Ta có:
C1
S
1
A1   1
0

0

0

S1

0 C1
1

0

0

0

1

0
C1

0
0

 S1
d1 


1
0

S1
0

C1
0

0 
1 d1 
0 0 

0 1 
0

T3  A1.1T3  A11. 0T3  1T3

0

C1 S1 0 0   nx Ox ax px 
0
0 1  d1   n y Oy a y p y 


 S1 C1 0 0   nz Oz az pz 



0 0 1  0 0 0 1 
0
C2C3  S 2 S3 C2 S3  S 2C3 0 d3C2C3  d 3 S 2 S3  d 2C2 
S C  C S S S  C C 0 d S C  d C S  d S 
2 3
2 3
2 3

3 2 3
3 2 3
2 2
 2 3


0
0
1
0


0
0
0
1



 nxC1  n y S1

nz

 nx S1  n y C1

0

C2C3  S2 S3
S C  C S
2 3

 2 3

0

0


OxC1  Oy S1 axC1  a y S1

pxC1  p y S1 
Oz
az
pz  d1 
Ox S1  Oy C1 ax S1  a y C1 px S1  p yC1 

0
0
1

C2 S3  S2C3 0 d3C2C3  d3 S2 S3  d 2C2 
 S2 S3  C2C3 0 d3 S2C3  d3C2 S3  d 2 S2 

0
1
0

0
0
1


Cân bằng ma trận ta được:

 pxC1  p y S1  d3C2C3  d3 S 2 S3  d 2C2


 pz  d1  d3 S2C3  d3C2 S3  d 2 S 2

 px S1  p y C1  0

(1)
(2)
(3)

Ta có:

(3) 

S1 p y

C1 px

 1  arctan 2  p y , px 

Nhóm trưởng: Nguyễn Thanh Quy

18


ĐAMH: Robot công nghiệp

GVHD: Th.s Trương Ngọc Bảo

(1) 2  (2) 2
  pxC1  p y S1    pz  d1    d3C2C3  d 3 S 2 S3  d 2C2    d 3S 2C3  d 3C2 S3  d 2 S 2 
2




2

2

 C12 px 2  p y 2  S12 px 2  p y 2
px 2  p y 2

 p d 
2

z

2

1

 p d 
2

z

1

2

  d 3C23  d 2C2    d 3S 23  d 2 S 2 
2

2

2

 d32  C232  S 232   2d 2 d 3  C23C2  S 23S 2   d 2 2  C2 2  S 2 2 

 px 2  p y 2   pz  d1   d 2 2  d32  2d 2 d3C3
2

px 2  p y 2   pz  d1   d 2 2  d32
2

 C3 

2d 2 d 3

 px 2  p y 2   pz  d1 2  d 2 2  d32 

 3   arccos 


2d 2 d 3




3  0

Đặt:
d3C3  d 2  r cos   rC
2
2
, r   d 3C3  d 2    d 3 S3 

d3 S3  r sin   rS
 d3C3  d 2
 C

r

   arctan 2  d3 S3 , d3C3  d 2 
d
S
3
3

 S
 r

1
 p C  p y S1  rCC2  rS S 2
 x 1


 pz  d1  rC S 2  rS C2
 2 
 pxC1  p y S1
 C 2

r

    2  arctan 2  pz  d1 , pxC1  p y S1 
p

d
1
 z
 S 2
 r
  2  arctan 2  pz  d1 , pxC1  p y S1   arctan 2  d3 S3 , d3C3  d 2 

Vậy:


1  
arctan 2  p y , px 

  

q   2   arctan 2  pz  d1 , pxC1  p y S1   arctan 2  d3 S3 , d3C3  d 2  
  

    arccos  p 2  p 2   p  d 2  d 2  d 2 / 2d d  
 3

y
z
1
2
3
2 3
 x
 




Nhóm trưởng: Nguyễn Thanh Quy

(2.9)



19