Hệ nơron mờ và ứng dụng cho robot 5 bậc tự do (LV thạc sĩ)

i

ĐẠI HỌC THÁI NGUYÊN
TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP

PHAN VĂN DƯ

HỆ NƠRON MỜ VÀ ỨNG DỤNG CHO ROBOT 5 BẬC
TỰ DO

Chuyên ngành: Kỹ thuật điều khiển và tự động hóa
Mã số: 60520216

LUẬN VĂN THẠC SĨ KỸ THUẬT

Người hướng dẫn khoa học

PGS.TS LẠI KHẮC LÃI

THÁI NGUYÊN - 2017


LỜI MỞ ĐẦU
Trong thời gian thực hiện luận văn, tác giả đã nhận được sự quan tâm rất lớn của
nhà trường, các khoa, phòng ban chức năng, các thầy cô giáo và đồng nghiệp.
Tác giả xin chân thành cảm ơn Ban giám hiệu, Khoa sau đại học, các giảng viên
đã tạo điều kiện cho tôi hoàn thành luận văn này.
Tác giả xin bày tỏ lời cảm ơn chân thành nhất đến thầy hướng dẫn khoa học
PGS.TS Lại Khắc Lãi về những chỉ dẫn khoa học, định hướng nghiên cứu và tận tình
hướng dẫn tôi trong suốt quá trình làm luận văn.
Mặc dù đã rất cố gắng, song do trình độ và kinh nghiệm còn hạn chế nên có thể

luận văn còn những thiếu sót. Tác giả rất mong nhận được những ý kiến đóng góp từ các
thầy cô giáo và các bạn đồng nghiệp để luận văn được hoàn thiện và có ý nghĩa ứng dụng
trong thực tế.
Cuối cùng, tôi xin cảm ơn gia đình và bạn bè, những người đã luôn ủng hộ và
động viên tôi để tôi yên tâm nghiên cứu luận văn này.

Thái nguyên, tháng 07 năm 2017
Tác giả

Phan Văn Dư


iii
LỜI CAM ĐOAN
Tôi tên là Phan Văn Dư lớp CHK17-TĐH tôi xin cam đoan bản luận văn: " Hệ
nơron mờ và ứng dụng cho robot 5 bậc tự do" là do tôi tự tổng hợp và nghiên cứu, không
sao chép của ai.
Mọi tham khảo trong luận văn đều được trích dẫn rõ ràng tên tác giả, tên công
trình, thời gian, địa điểm công bố.
Tôi xin chịu trách nhiệm về những gì tôi khai trước nhà trường và hội đồng khoa
học!

Thái nguyên, tháng 07 năm 2017
Tác giả luận văn

Phan Văn Dư


MỤC LỤC
Trang

Lời mở đầu

ii

Lời cam đoan

iii

Mục lục

iv

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

vi

Danh mục các bảng

vii

Danh mục các hình vẽ, đồ thị

viii

MỞ ĐẦU

1

NỘI DUNG

3

CHƯƠNG 1. XÂY DỰNG MÔ HÌNH TOÁN HỌC ROBOT 5 BẬC TỰ
DO

3

1.1. TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP

3

1.1.1. Sơ lược quá trình phát triển của robot công nghiệp

3

1.1.2. Cấu trúc cơ bản của robot công nghiệp

4

1.1.3. Cơ cấu robot 5 bậc tự do toàn khớp quay

5

1.2. ĐỘNG HỌC ROBOT

6

1.2.1. Bảng thông số DH

7

1.2.2. Tính toán ma trận mô tả quan hệ khâu i đối với hệ tọa độ gốc

9

1.3. ĐỘNG LỰC HỌC ROBOT

10

1.3.1. Hàm Lagrange

10

1.3.2. Phương trình động lực học robot

12

1.3.3. Phương trình động lực học robot 5 bậc tự do

14

CHƯƠNG 2. TỔNG QUAN VỀ HỆ NƠRON MỜ

24

2.1. ĐẶT VẤN ĐỀ

24

2.2. TỔNG QUAN VỀ ĐIỀU KHIỂN MỜ

24

2.2.1. Giới thiệu
2.2.2. Cấu trúc của bộ điều khiển mờ

24
25


2.2.3. Thiết kế bộ điều khiển mờ

26

2.3. TỔNG QUAN VỀ MẠNG NƠRON

28

2.3.1. Mạng nơron sinh học

28

2.3.2. Mạng nơron nhân tạo

29

2.3.3. Cấu trúc mạng

35

2.4. HỆ NƠRON MỜ

39

2.4.1. Sự kết hợp giữa logic mờ và mạng nơron

39

2.4.2. Cấu trúc chung của hệ mờ và mạng nơron

41

2.4.3. Huấn luyện mạng nơron mờ

46

CHƯƠNG 3. ỨNG DỤNG NƠRON MỜ ĐIỀU KHIỂN TAY MÁY 5 DOF

52

3.1. MÔ PHỎNG ĐỐI TƯỢNG ĐIỀU KHIỂN

52

3.1.1. Cơ cấu robot 5 DOF

52

3.1.2. Mô hình cơ cấu chấp hành

53

3.2. THIẾT KẾ BỘ ĐIỀU KHIỂN PID ĐIỀU KHIỂN ĐỐI TƯỢNG

55

3.2.1. Tổng hợp vòng điều chỉnh động cơ

55

3.2.2. Thiết kế bộ điều khiển PID điều khiển đối tượng

58

3.2.3. Kết quả mô phỏng dùng PID

61

3.3. BỘ ĐIỀU KHIỂN NƠ RON MỜ ĐIỀU KHIỂN ROBOT 5 DOF

62

3.3.1. Thu thập bộ dữ liệu để huấn luyện mạng

63

3.3.2. Cấu trúc hệ nơron mờ

65

3.3.3. Huấn luyện mạng nơron mờ

67

3.3.4. Kết quả mô phỏng dùng NEFCON

68

KẾT LUẬN VÀ KIẾN NGHỊ

71

TÀI LIỆU THAM KHẢO

72


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

Ký hiệu

Tiếng Anh

Tiếng Việt

Trang

NEFCON

Neural Fuzzy Controller

Bộ điều khiển nơron
mờ

1

DOF

Degree of Freedom

Bậc tự do

6

DH

Denavit Hartenberg

Tr

Trace

Vết ma trận

16

SISO

Single Input Single Output

Một vào một ra

47

DC

Direct current

Động cơ một chiều

53

PID

Proportional Integral Derivative

Bộ điều khiển tỷ lệ
vi tích phân

58

FIS

Fuzzy Inference System

Hệ thống suy diễn
mờ

65

7


DANH MỤC CÁC BẢNG

Bảng

Tên hình

Trang

Bảng 1.1

Bảng thông số DH

8

Bảng 1.2

Thông số DH của robot 5 DOF

15

Bảng 2.1

Tiêu chí so sánh giữa logic mờ và mạng nơron

40

Bảng 3.1

Tham số và giá trị của động cơ DC

59

Bảng 3.2

Bảng thông số DH của robot 5 DOF chọn mô phỏng

59

Bảng 3.3

Kết quả đạt được dùng PID

61

Bảng 3.4

Tập dữ liệu huấn luyện

63

Bảng 3.5

Kết quả đạt được dùng NEFCON

69

Bảng 3.6

So sánh kết quả của bộ PID và NEFCON

69


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

Hình

Tên hình

Trang

Hình 1.1

Các thành phần chính của hệ thống robot

4

Hình 1.2

Sơ đồ kết cấu tay máy

4

Hình 1.3

Sơ đồ tay máy robot 5 DOF

6

Hình 1.4

Sơ đồ khối động học

6

Hình 1.5

Minh họa phương pháp DH

7

Hình 1.6

Khảo sát tốc độ của vi khối lượng dm

11

Hình 1.7

Hệ tọa độ của robot 5 DOF

14

Hình 2.1

Các khối chức năng của bộ Điều khiển mờ

26

Hình 2.2

Cấu trúc tổng quát một hệ mờ

27

Hình 2.3

Mô hình 2 nơron sinh học

28

Hình 2.4

Mô hình một nơron

30

Hình 2.5

Mạng nơron 3 lớp

30

Hình 2.6

Cấu trúc huấn luyện mạng nơron

31

Hình 2.7

Mô hình nơron đơn giản

32

Hình 2.8

Một số dạng hàm chuyển của mạng nơron

33

Hình 2.9

Nơron với R đầu vào

34

Hình 2.10

Ký hiệu nơron với R đầu vào

34

Hình 2.11

Cấu trúc mạng nơron 1 lớp

35

Hình 2.12

Ký hiệu mạng R đầu vào và S nơron

36

Hình 2.13

Ký hiệu một lớp mạng

37

Hình 2.14

Cấu trúc mạng nơron 3 lớp

38

Hình 2.15

Ký hiệu tắt của mạng nơron 3 lớp

39

Hình 2.16

Mô hình hệ mờ - nơron

41


Hình 2.17

Cấu trúc chung của hệ mờ - nơron

41

Hình 2.18

Rời rạc hóa hàm liên thuộc

44

Hình 2.19

Hàm liên thuộc các tập mờ vào và ra

44

Hình 3.1

Sơ đồ khối mô hình robot 5 DOF trong Simulink

52

Hình 3.2

Sơ đồ động cơ một chiều

53

Hình 3.3

Sơ đồ cấu trúc của động cơ một chiều kích thích độc lập

54

Hình 3.4

Mô hình SIMULINK của động cơ một chiều kích thích độc
lập

55

Hình 3.5

Sơ đồ hệ thống điều khiển vòng kín

55

Hình 3.6

Sơ đồ hệ chấp hành có điều khiển

56

Hình 3.7

Vòng điều chỉnh dòng của động cơ một chiều

56

Hình 3.8

Sơ đồ khâu điều chỉnh dòng phần ứng

57

Hình 3.9

Cấu trúc hệ thống điều chỉnh tốc độ quay của động cơ một
chiều

58

Hình 3.10

Sơ đồ điều khiển độc lập một khớp nối

58

Hình 3.11

Sơ đồ mô phỏng hệ điều khiển robot 5 DOF bằng PID

60

Hình 3.12

Đặc tính quá độ của các khớp khi điều khiển bằng PID

61

Hình 3.13

Sơ đồ mô phỏng hệ thống điều khiển tay máy bằng nơron
mờ

62

Hình 3.14

Tải dữ liệu huấn luyện

65

Hình 3.15

Lựa chọn số lượng và dạng hàm liên thuộc đầu vào, ra

66

Hình 3.16

Sơ đồ cấu trúc bộ điều khiển nơron mờ

66

Hình 3.17

Sai số giữa tập mẫu và sau 30 kỳ huấn luyện

67

Hình 3.18

Dữ liệu vào ra của ANFIS sau huấn luyện

68

Hình 3.19

Đặc tính quá độ của các khớp khi điều khiển bằng NEFCON

68

Hình 3.20

So sánh chất lượng bộ điều khiển PID và NEFCON

69


10
MỞ ĐẦU
1. Lý do chọn đề tài
Cùng với sự phát triển mạnh mẽ lý thuyết điều khiển hiện đại và thông minh; hệ
mờ và mạng noron ngày càng được ứng dụng rộng rãi trong nhiều lĩnh vực. Hệ mờ dựa

trên logic của con người, với ưu điểm đơn giản và xử lý chính xác những thông tin không
chắc chắn và mạng nơ ron có khả năng tự học hỏi, tự chỉnh định cho phù hợp với sự thay
đổi của đối tượng điều khiển; sự kết hợp giữa hệ mờ và mạng noron nhân tạo đã đem lại
nhiều hiệu quả đáng kể trong lĩnh vực điều khiển và tự động hóa. Trong những năm gần
đây đã có nhiều đề tài nghiên cứu ứng dụng hệ nơ ron mờ để điều khiển các đối tượng phi
tuyến [6], [7], [10] và cũng đã thu được một số kết quả nhất định. Việc nghiên cứu thiết
kế bộ điều khiển cho robot đảm bảo các yêu cầu chất lượng là rất cần thiết và có ý nghĩa.
Trong đề tài này tác giả trình bày ứng dụng mạng noron nhân tạo và hệ điều khiển mờ để
thiết kế bộ điều khiển nâng cao chất lượng điều khiển cánh tay máy.
Xuất phát từ tính cấp thiết nâng cao chất lượng điều khiển robot và mong muốn áp
dụng hệ điều khiển nơ ron mờ (NEFCON) vào thực tiễn là đối tượng robot 5 bậc tự do
nên bản thân tôi chọn đề tài : “Hệ nơ ron mờ và ứng dụng cho robot 5 bậc tự do”
2. Mục đích nghiên cứu
Xây dựng bộ điều khiển noron mờ cho cánh tay robot đảm bảm các yêu cầu chất
lượng, kiểm tra kết quả thông qua mô phỏng và thực nghiệm.
3. Đối tượng nghiên cứu
Điều khiển tay robot theo hệ noron mờ
4. Ý nghĩa khoa học, ý nghĩa thực tiễn của đề tài
a) Ý nghĩa khoa học
Hệ thống noron mờ đang nổi lên như một công cụ điều khiển các hệ thống phi
tuyến với các thông số chưa xác định. Việc kết hợp giữa phương pháp noron và phương
pháp mờ đem lại khả năng tuyệt vời cho sự linh hoạt và học theo thao tác của con người.
Điều này có ý nghĩa rất lớn về mặt khoa học trong việc điều khiển các đối tượng phi
tuyến.


Đề tài này sẽ đề cập đến ứng dụng của no ron mờ trong việc điều khiển đối tượng
phi tuyến đặc biệt là điều khiển cánh tay robot.
b) Ý nghĩa thực tiễn
Việc điều khiển cánh tay robot ứng dụng hệ noron mờ có ý nghĩa thực tiễn rất lớn.

Bởi vì robots được áp dụng rộng rãi trong nhiều lĩnh vực khác nhau, chúng buộc phải có
khả năng làm việc trong các môi trường không xác định trước và thay đổi. Đặc biệt
chúng phải nhạy cảm với môi trường làm việc và thực hiện thao tác bất chấp sự có mặt
của vật cản trong vùng làm việc. Việc nâng cao chất lượng điều khiển robot sẽ góp phần
nâng cao chất lượng sản phẩm, nâng cao năng suất và hiệu quả lao động.


CHƯƠNG 1 XÂY DỰNG MÔ HÌNH TOÁN HỌC ROBOT 5 BẬC TỰ DO
1.1 TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP
1.1.1 Sơ lược quá trình phát triển của robot công nghiệp
Cùng với sự phát triển của khoa học công nghệ robot ngay càng được ứng dụng
rộng rãi trong các lĩnh vực của đời sống xã hội.Thuật ngữ Robot xuất phát từ tiếng Séc
(Czech) "Robota" có nghĩa là công việc tạp dịch trong vở kịch Romssum's Universal
Robots của Karel Capek vào năm 1921. Trong vở kịch Rossum và con trai của ông đã
chế tạo ra những chiếc máy gần giống con người để phục vụ con người. Có lẽ đó là một
gợi ý ban đầu cho các nhà sáng chế kỹ thuật về những cơ cấu, máy móc bắt chước các
hoạt động cơ bắp của con người.
Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundy 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 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. Về mặt kỹ thuật, những robot công nghiệp ngày nay có nguồn gốc từ
hai lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa (Teleoperators) và
các máy công cụ điều khiển số. Dưới đây chúng ta sẽ điểm qua một số thời điểm lịch sử
phát triển của người máy công nghiệp. Một trong những robot công nghiệp đầu tiên được
chế tạo là robot Versatran của công ty AMF, Mỹ. Cũng vào khoảng thời gian này ở Mỹ
xuất hiện loại robot Unimate -1900 được dùng đầu tiên trong kỹ nghệ ô 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,
Thụy Điển và Nhật - 1968 theo bản quyền của Mỹ, CHLB Đức - 1971, Pháp -1972, …
Tính năng làm việc của robot ngày càng được nâng cao nhất là khả năng nhận biết và xử

lý. Năm 1967 trường Đại học Tổng hợp Stanford (Mỹ) đã chế tạo ra mẫu robot hoạt động
theo mô hình mắt-tay, có khả năng định hướng và nhận biết bàn kẹp theo vị trí vật kẹp
nhờ các cảm biến. Năm 1974 công ty Mỹ Cincinnati đưa ra loại robot được điều khiển
bằng máy vi tính gọi là robot T3 (The Tomorrow). Robot này có khả năng nâng được vật
có khối lượng lên đến 40kg. Có thể nói Robot là sự tổ hợp khả năng hoạt động linh hoạt
của 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ông 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 được phát triển cùng với
những thành tựu to lớn trong lĩnh vực Tin học- Điện tử.
1.1.2 Cấu trúc cơ bản của robot công nghiệp
Một robot công nghiệp thường bao gồm các thành phần như hình 1.1
Môi trường
-Đối tượng
-Lực, mômen

Phần công tác

Truyền động cơ khí

Cảm biến giám sát trạng thái hệ thống

Cảm biến giám sát thông số môi trường
Cơ cấu chấp hành
Hệ thống điều khiển

Giao diện Người - Máy

Hinh 1.1 Các thành phần chính của hệ thống robot
Tay Máy: (Manipulator) là cơ cấu cơ khí gồm các khâu, khớp. Chúng hình thành
cánh tay (arm) để tạo các chuyển động cơ bản. Cổ tay (Wrist) tạo nên sự khéo léo, linh
hoạt và bàn tay (Hand) hoặc phần công tác (End Effector) để trực tiếp hoàn thành các
thao tác trên đối tượng.

Hình 1.2 Sơ đồ kết cấu tay máy


Cơ cấu chấp hành: Tạo chuyển động cho các khâu của tay máy. Nguồn động lực
của các cơ cấu chấp hành là động cơ các loại: Điện, thuỷ lực, khí nén hoặc kết hợp giữa
chúng.
Hệ thống cảm biến: Gồm các sensor và thiết bị chuyển đổi tín hiệu khác. Các
robot cần hệ thống sensor trong để nhận biết trạng thái của bản thân các cơ cấu của robot
và các sensor ngoài để nhận biết trạng thái của môi trường.
Hệ thống điều khiển (controller): Hiện nay thường là hệ thống điều khiển số có
máy tính để giám sát và điều khiển hoạt động của robot.
1.1.3 Cơ cấu robot 5 bậc tự do toàn khớp quay
Để robot có thể hoạt động trong môi trường và thực hiện tốt các chức năng của nó
thì robot phải có một chương trình điều khiển. Chương trình này sẽ hướng dẫn robot di
chuyển từ vị trí ban đầu tới đích sao cho tránh được những va chạm trên đường đi và có
thể nâng được đối tượng tới vị trí khác mong muốn.
Các khâu của robot thường thực hiện hai chuyển động cơ bản:
- Chuyển động tịnh tiến theo hướng x, y, z trong không gian Descarde, thông thường tạo
nên các hình khối, các chuyển dộ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 chuyển động (R và T) mà tay máy có kết cấu khác
nhau với vùng làm việc khác nhau.
Trong luận văn này tác giả xét một cơ cấu robot 5 bậc tự do toàn khớp quay (RRRRR)

như hình 1.3
…..
;;;;;


Hình 1.3 Sơ đồ tay máy robot 5 DOF
1.2 ĐỘNG HỌC ROBOT
Trong mô phỏng robot, phân tích hệ thống là cần thiết như là phân tích động học,
mục đích là mang lại những hiểu biết của những chuyển động của từng phần cơ khí robot
và mối quan hệ giữa chúng. Phân tích động học được chia thành động học thuận và động
học ngược. Động học thuận bao gồm tìm ra vị trí cuối trong không gian chuyển động của
các khớp là F (θ ,θ , ,θ ) = [ x, y, z,
1 2
n

và động học ngược bao gồm xác định thông số

R]

các biến khớp để có được vị trí cuối và hướng mong muốn
Hình 1.4 biểu diễn sơ đồ khối đơn giản của động học robot.

F (θ1,θ2 , ,θn ) = [ x, y, z, R] .

Động học thuận

Chuyển động các khớp

Thông số hình
học

Vị trí và hướng của
điểm cuối

Động học ngược

Hình 1.4 Sơ đồ khối động học


Một phương pháp phổ biến sử dụng thuận tiện cho việc chọn lựa khung tham chiếu
trong ứng dụng robot là phương pháp Denavit-Hartenberg hoặc phương pháp D-H biểu
diễn như hình 1.5
ai

αi
zi

di

xi−1 zi−1

θi

yi
Oi

xi

Oi−1
yi−1

Hình 1.5 Minh họa phương pháp DH
- ai: khoảng cách theo phương xi từ Oi đến giao điểm của các trục xi và zi-1.
- di: khoảng cách theo phương zi-1 từ Oi-1 đến giao điểm của các trục xi và zi-1 ,di thay
đổi khi khớp i là khớp trượt.
- αi : là góc quay quanh trục xi từ zi-1 đến zi.
- θi : là góc quay quanh trục zi-1 từ xi-1 đến xi.
1.2.1. Bảng thông số DH
Bước 1: Chọn hệ tọa độ cơ sở, gắn các hệ tọa độ mở rộng lên các khâu:
- Giả định vị trí ban đầu của robot.
- Xác định các trục khớp và đặt tên tương ứng z 0 … z n−1 .
- Xác định hệ tọa độ nền. Đặt gốc của hệ tọa độ này tại bất kỳ điểm nào trên
trục

z0 . Các trục x0 và y0 được chọn thỏa qui tắc tam diện thuận.
……………………………..


- Chọn gốc tọa độ O i là giao điểm của đường vuông góc chung giữa z i và z i−1 với z i . Nếu
z i giao với z

i−1

, đặt O i tại điểm này. Nếu z i song song với z

i−1

, đặt O i tại bất kỳ vị trí

nào trên z i sao cho thuận tiện.

- Xác định x i đi qua O i và dọc theo đường vuông góc chung giữa z i−1 và z i .
Trong trường hợp các trục khớp cắt nhau thì trục x i chọn theo hướng vuông góc với mặt
phẳng tạo bởi z i−1 và z i .
- Xác định y i thỏa quy tắc tam diện thuận.
Bước 2: Lập bảng thông số Denavit – Hartenberg (D-H) cho các khâu trên robot.
Bước 3: Dựa vào bảng thông số D-H xác định các ma trận A i bằng cách thay các thông
số ở bước 2.
Trường hợp đang xét ở đây là khớp quay thì θi là biến, còn d = const . Bảng thông
i
số DH như sau:
Bảng 1.1


Khâu Khớp nối θi

αi

di

ai

1

0-1

θ1

α1

d1

a1

2

1-2

θ2

α2

d2

a2

…..

…..

….. ….. ….. …..

j

(j-1)-j

…..

…..

n

(n-1)-n

θj

αj

dj

aj

….. ….. ….. …..

θn

αn

dn

an


Dựa vào bảng thông số DH, mỗi ma trận chuyển đổi thuần nhất

Ai được trình bày

như là tích của 4 chuyển đổi cơ bản sử dụng thông số của khâu i và khớp nối (i-1 và i).

Ai = Rot ( z,θi )Trans ( z, di )Trans ( x, ai ) Rot

(1.1)

( x,αi )
Ký hiệu

Rot (x, ai viết tắt quay quanh trục

xi một góc

)

chuyển theo trục

xi một khoảng

ai . Giải thích tương tự

αi . Trans (x, ai )là dịch

Rot ( z,θi

và Trans (z, di )

)
cθi

A = i

sθi

0

0

cθi

sθi
A 
=
i

−sθ

0 0  1
i

cθ
0 0 0
i
0

0 1 0 0
0 0  
s1θi sα0i
−sθicα
i
−cθi
cθicαi
sαi

0

s

 0

0

αi

c
αi

0

0 0
1 0

0  1 0 0 ai  1 0


cα
0 0 1 0 0

0
−sαi

i




0 1 di  0 0 1 0  0 sαi

0
0 0
 
0
1 0
0 1  0
aicθi 

ai αi


0

0

0

1

αi

c
0

(1.2)

d 

i

1 

Đối với một khâu đi theo một khớp quay thì

αi , di , là hằng số. Như vậy ma trận

Ai của khớp quay là một hàm số của biến khớp θi ai

0

1.2.2 Tính toán ma trận mô tả quan hệ khâu i đối với hệ tọa độ gốc T i
Như ta đã trình bày trong mục 1.2.1 ma trận A mô tả vi trí và hướng của khâu thứ i
i
0

so với khâu thứ i-1. Như vậy tích của các ma trận A là ma trận T mô tả vị trí và hướng
i

i

của khâu thứ i so với phần đế cố định.
0

T=T=AA
i

i

1

2

Ai

Nếu một robot có n khâu ta có ma trận T của khâu chấp hành cuối:
An

(1.3)

Tn = A1 A


(1.4)
Tn 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,
có kích thước 4x4, có thể biểu diễn như sau:
…………………….


Ma trận định hướng R

Vecto vị trí p

Tn =
0

0

0

1

Ma trận R có kích thước 3x3, là ma trận biểu diễn hướng của khâu chấp hành cuối.
Vecto p có kích thước 3x1, biểu diễn mối quan hệ tọa độ vị trí của gốc tọa độ gắn
trên khâu chấp hành cuối đối với hệ tọa độ gốc.
1.3 ĐỘNG LỰC HỌC ROBOT
Nghiên cứu động lực học robot là công việc cần thiết khi phân tích cũng như tổng
hợp quá trình điều khiển chuyển động. Việc nghiên cứu động lực học robot thường giải
quyết hai nhiệm vụ sau đây:
-Xác định momen và lực động xuất hiện trong quá trình chuyển động. Khi đó qui
luật biến đổi của biến khớp

qi (t) coi như đã biết. Việc tính toán lực trong cơ cấu tay máy

là rất cần thiết để chọn công suất động cơ, kiểm tra độ bền, độ cứng vững, đảm bảo độ tin
cậy của robot.
-Xác định các sai số động tức là sai lệch so với qui luật chuyển động theo chương
trình. Lúc này cần khảo sát Phương trình chuyển động của robot có tính đến đặc tính
động lực của động cơ và các khâu.
Có nhiều phương pháp nghiên cứu động lực học robot, nhưng thường gặp hơn cả
là phương pháp cơ học Lagrange, cụ thể là dùng phương trình Lagrange - Euler. Đối với
các khâu khớp của robot, với các nguồn động lực và kênh điều khiển riêng biệt, không
thể bỏ qua các hiệu ứng trọng trường (gravity effect), quán tính (initial), tương hổ
(Coriolis), ly tâm (centripetal)... mà những khía cạnh này chưa được xét đầy đủ trong cơ
học cổ điển; Cơ học Lagrange nghiên cứu các vấn đề nêu trên như một hệ thống khép kín
nên đây là nguyên lý cơ học thích hợp đối với các bài toán động lực học robot.
Xét khâu thứ i của robot có n khâu
1.3.1 Hàm Lagrange
Một điểm trên khâu thứ i được mô tả trong hệ tọa độ cơ bản là:

i

r=T r
i

; i = 1÷
n

(1.5)

Tốc độ của vi khối lượng dm được tính bởi công thức:
 i ∂T
dr d i
i
r=
= Ti r = ∑ i q j  r


dt dt
∂q
j=1
j



(1.6)

Khi tính bình phương của vận tốc này ta có:

rr rx , y ,z0 Tr00rr

(1.7)





2

T

Hình 1.6 Khảo sát tốc độ của vi khối lượng dm
Với
Tr

a11


a1n 



a
 n1

a 
nn 

n

= ∑ aii
i=1

Do vậy


T

2

(

r = Tr rr

d

d

i

) = Tr 
 dt

Ti r
Ti

TiT



∂T

i



i

T

∂T T



i

r  = Tr  ∑∑
dt

i

 j=1 k =1

i

rr

i

(1.8)

q j qk


∂qk

∂q j



i

Động năng của vi khối lượng dm đặt tại vị trí r trên khâu thứ i
 ii

T
T T
T
i
i i iT i
∂T r ir
q
qdm
 
∂Tj k  
q
q
j
k

j1 k 1

i
i
T
i
i
= Tr  ∑∑
( rdm r ) q q 

dKi = 1
1
r 2 dm Tr
1
2
2



(1.9)


2

 j=1 k =1 ∂q j

Động năng của khâu thứ i sẽ là:

∂qk




 i i
i 
dK = Tr  ∑∑ ∂T 
1

K =

∫2

i

∫

∂q 

Khau i

 j=1 k

j

=1

i

i

T

 ∂T
i
rdm r  q q 
i T

 ∂q

 Khau

 i i ∂T
= 2Tr  ∑∑ ∂i J
q
 j=1 k =1 j



i

i

q qj k

∂q

k

k

(1.10)





T

∂T

1

jk



Với Ji là ma trận quán tính được tính:
Ji =

∫

i

i T

rdm r

Khau i

Động năng của robot có n khâu được tính:

K = ∑ Ki

(1.11)

i=1

Thế năng của khâu i có khối lượng dm, trọng tâm được xác định bởi
vecto
P = −m g r = −m g T
r
i

i

i

i

i

ri

i

(1.12)

i

Trong đó vecto gia tốc trọng trường được biểu diễn dưới dạng:
g =  g x

g

gy

0T

=

[0

−9.8 0 0]T

z

Thế năng của robot có n khâu được tính:
P = ∑ Pi

(1.13)

i=1

Hàm Lagrange của một hệ thống năng lượng:
L=K−P

(1.14)

Sau khi xác định động năng và thế năng của toàn cơ cấu, ta có hàm Lagrange của
robot có 5 bậc tự do:
1

L=

n

ii


∂T

∑∑∑Trace
2

i=1

jk

i

∂T
T

 ∂q
j

i

Ji

∂q
k

 1

∑m

q j qk +



n

2

ri

i=1

i

g Ti

(1.15)


1.3.2 Phương trình động lực học robot
Ta có lực tác dụng lên khâu thứ i (lực tổng quát) được xác định bởi phương trình
Lagrange:
===