BỘ GIÁO DỤC VÀ ĐÀO TẠO
ĐẠI HỌC QUỐC GIA TP. HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA
---oOo---

NGUYỄN ĐÌNH PHÚ

ĐIỀU KHIỂN CÓ GIÁM SÁT DÙNG LOGIC MỜ.
ỨNG DỤNG ĐIỀU KHIỂN MÔ HÌNH ROBOT
SCARA DÙNG DSP TMS320C50.

CHUYÊN NGÀNH: KỸ THUẬT VÔ TUYẾN – ĐIỆN T Ử
MÃ SỐ NGÀNH:

LUẬN ÁN CAO HỌC

Tp. HCM, tháng 4 năm 2004


LỜI CẢM ƠN
Tôi xin gởi lời cảm ơn chân thành nhất đến:
-

Thầy hướng dẫn Ts. Dương Hoài Nghóa phó Khoa
Điện – Điện Tử trường Đại học Kỹ Thuật Tp HCM
đã trực tiếp hướng dẫn tôi hoàn thành luận án này.

-

Tất cả các thầy cô đã giảng dạy, truyền đạt kiến
thức giúp tôi trưởng thành trong nghề nghiệp và

cuộc sống.

-

Tất cả các bạn bè đồng nghiệp và người thân trong
gia đình đã động viên giúp đỡ, thăm hỏi trong thời
gian tôi thực hiện luận án tốt nghiệp.

Tháng 04/2004
Nguyễn Đình Phú.


ABSTRACT
In the control systems, the parameters of the control systems change acording to
load capacity – they are called system which adjust itself. The adjustable parameters are
performed according to an algorithm with very large calculation which is not able to do
with the real time.
The purposes of the thesis:
ƒ Studying the supervise control system use the repeating method.
ƒ Self adjust system uses DSP KIT and the model of the robot is simulinked on PC.


MỤC LỤC
Chương I:

Trang

GIỚI THIỆU VỀ ROBOT SCARA

1


I. Giới thiệu. ...........................................................................................................................2
II. Mô hình toán học của robot scara.......................................................................................3
III. Mô hình trạng thái của robot scara.....................................................................................5
Chương II:

GIẢI THUẬT ĐIỀU KHIỂN ROBOT........................................................... 6

I. Giới thiệu. ............................................................................................................................7
II. Vòng điều khiển trong: hồi tiếp tuyến tính hóa....................................................................7
III. Vòng điều khiển ngoài: điều khiển PD. ..............................................................................8
IV. Thiết kế hệ thống điều khiển ............................................................................................10
1. Thông số của robot. ........................................................................................................10
2. Vòng điều khiển trong.................................................................................................... 11
3. Vòng điều khiển ngoài. .................................................................................................15
V. Mô phỏng hệ thống điều khiển trên Matlab......................................................................17
1. Hệ thống danh định. .......................................................................................................18
2. Ảnh hưởng của sự thay đổi tải trọng...............................................................................18
3. Ảnh hưởng của sự thay đổi hệ số ma sát. .......................................................................20
Chương III:

ĐIỀU KHIỂN GIÁM SÁT DÙNG LOGIC MỜ ......................................... 25

I. Logic mờ. ...........................................................................................................................26
1. Tập mờ. .........................................................................................................................26
2. Quan hệ mờ. ..................................................................................................................29
3. Suy diễn mờ. ..................................................................................................................30
5. Hệ thống xử lý mờ. .........................................................................................................33
5. Nhận dạng hàm thuộc và luật hợp thành. .......................................................................36
II. Ứng dụng logic mờ trong giám sát bộ điều khiển..............................................................38

1. Giới thiệu........................................................................................................................38
2. Thông số chỉnh định. .....................................................................................................38
3. Phương pháp chỉnh định lặp ..........................................................................................39
4. Hệ thống chỉnh định mờ ................................................................................................40
III. Mô phỏng hệ thống điều khiển có giám sát trên Matlab. ................................................44
Chương IV:

KẾT QUẢ THỰC NGHIỆM TRÊN KIT DSP............................................ 46

I. Giới thiệu. ..........................................................................................................................47
II. Mô phỏng robot scara (trên máy tính PC). ........................................................................48
III. Giải thuật điều khiển moment tính (trên kit DSP). ...........................................................51
IV. Giải thuật giám sát dùng logic mờ. ...................................................................................52
V. Đáp ứng với bước nhảy đơn vị...........................................................................................52
VI. Chuyển động theo qũy đạo cho trước................................................................................55
VII. Chỉnh định các thông số của bộ điều khiển dùng logic mờ. .............................................59
VIII.Ảnh hưởng của sự thay đổi tải trọng và hệ số ma sát. .....................................................62
IX. Kết luận ..........................................................................................................................66

i


KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN. .............................................................................. 67
Phụ lục A.......................................................................................................................................68
Phụ lục B. .....................................................................................................................................71
Phụ lục C. .....................................................................................................................................81
Phụ lục D. .................................................................................................................................. 94
TÀI LIỆU THAM KHẢO ...................................................................................................... 109

ii



GIỚI THIỆU
Trong các bộ điều khiển thường phải thay đổi các thông số của bộ
điều khiển theo sự thay đổi của tải trọng – gọi là các hệ thống tự chỉnh
định. Việc chỉnh định các thông số được thực hiện theo một giải thuật đòi
hỏi khối lượng tính toán lớn – khó đáp ứng được trong thời gian thực.
Mục tiêu của luận văn tốt nghiệp là nghiên cứu hệ thống điều khiển
có giám sát dùng phương pháp lập và logic mờ. Áp dụng hệ thống điều
khiển có giám sát để điều khiển robot 2 bậc tự do.
Nghiên cứu thực hiện hệ thống điều khiển trong thời gian thực với độ
dài từ dữ liệu hữu hạn (2byte).
Hệ thống điều khiển có giám sát thực hiện trên kit DSP, mô hình
robot 2 bậc tự do được mô phỏng trên máy tính.
Đề tài chia làm 4 chương:
Chương 1: GIỚI THIỆU VỀ ROBOT SCARA.
Giới thiệu về robot scara, mô hình toán học và mô hình trạng thái của
robot scara.
Chương 2: GIẢI THUẬT ĐIỀU KHIỂN ROBOT.
Trình bày giải thuật điều khiển robot dùng hồi tiếp tuyến tính hóa và
điều khiển PD. Mô phỏng hệ thống điều khiển trên Matlab.
Chương 3: ĐIỀU KHIỂN GIÁM SÁT DÙNG LOGIC MỜ.
Giới thiệu về logic mờ để chỉnh định các thông số của bộ điều khiển
sử dụng phương pháp chỉnh định lặp. Mô phỏng hệ thống điều khiển có
chỉnh định trên Matlab.
Chương 4: KẾT QUẢ THỰC NGHIỆM TRÊN KIT DSP.
Khảo sát đáp ứng của hệ thống điều khiển với bước nhảy đơn vị,
khảo sát tính hiệu quả của bộ giám sát với sự thay đổi của tải trọng và sự
thay đổi của hệ số ma sát.




CHƯƠNG 1:

GIỚI THIỆU VỀ ROBOT SCARA

I.

GIỚI THIỆU.

II.

MÔ HÌNH TOÁN HỌC CỦA ROBOT SCARA.

III.

MÔ HÌNH TRẠNG THÁI CỦA ROBOT SCARA.


Luận Văn Cao Học

I.

Điều Khiển Robot Scara

GIỚI THIỆU

Robot là một hệ thống điều khiển tự động vận hành, đa chức năng, có thể lập trình, được
thiết kế để di chuyển các chi tiết, các bộ phận hoặc và các thiết bị đặc biệt thông qua các
chuyển động khác nhau để thực thi các công việc khác nhau.

Có rất nhiều loại robot khác nhau, ở đây xin giới thiệu 2 loại robot: loại PUMA và loại
SCARA.

Hình 1-1. Robot loại PUMA:

Hình 1-2. Robot loại SCARA:
Robot scara thường được sử dụng để nhặt và đặt 1 vật thể hay dùng để siết và mở các
vít hoặc hàn linh kiện tử.
Việc nghiên cứu về robot ngày càng phát triển mạnh ở các nước phát triển và được sử
dụng rất nhiều trong các nhà máy công nghiệp.
Robot người máy đã được chế tạo ngày càng hoàn hảo và gần gũi con người. Các nhà
nghiên cứu xây dựng ngày càng nhiều tính năng cho robot.

GVHD: TS. Dương Hoài Nghóa

2


Luận Văn Cao Học

Điều Khiển Robot Scara

Để có thể vận hành thì robot phải có một hệ thống điều khiển và một hệ thống cơ cấu
chấp hành. Hệ thống điều khiển có thể sử dụng là các chip vi xử lý lập trình, bộ nhớ và các hệ
thống cảm biến.
Lónh vực điều khiển robot tiếp tục phát triển trong tương lai và cũng chính là lónh vực
nghiên cứu hấp dẫn của các nhà nghiên cứu và đó là lý do tôi chọn để thực hiện đề tài tốt
nghiệp.
Robot có nhiều ứng dụng vì thế robot cũng được phân loại theo nhiều dạng. Trong đề
tài tốt nghiệp, người nghiên cứu chọn robot scara 2 bậc tự do.


II. MÔ HÌNH TOÁN HỌC CỦA ROBOT SCARA:

Robot SCARA 2 bậc tự do có dạng
như hình 1-3 – gồm 2 cánh tay với chiều
dài tương ứng là l1, l2 và 2 khớp tác động.
Khớp thứ nhất được motor 1 tác động trực
tiếp một moment τ1. Khớp nối thứ 2 được
motor 2 tác động gián tiếp một moment τ2,
thông qua bộ nhông tăng lực có hệ số
truyền động ke. Đầu mút của cánh tay 2
(end-effector) mang một tải trọng m.
Phương trình mô tả hệ thống được cho
bởi (1-1), (1-2):

Joint2

Joint1

I1

τ2
Link1 ϕ2

τ1
a)

I2

ϕ1


l1

Với: M11 = I1+m l12 ;
C11 = V1;

Link2
Payload m
l2

y

θ2

(x,y)

b)
(0,0)

θ1

⎤⎡ • ⎤
C12 sin( k e (ϕ1 + ϕ 2 )) ϕ 2 ⎥ ⎢ϕ1 ⎥ ⎡τ 1 ⎤
+⎢ ⎥
•
⎥ ⎢ϕ ⎥ ⎣τ 2 ⎦
C 22
⎦⎣ 2 ⎦

M12 = M21 = -ml1l2;

C22 = V2;

x

Hình 1-3.- Robot SCARA

••
M 11
M 12 cos(k e (ϕ1 + ϕ 2 ))⎤ ⎡ϕ1 ⎤
⎡
⎢
⎥ ⎢ • • ⎥⎥ =
⎢ M cos(k (ϕ + ϕ ))
M
1
2
22
e
⎦ ⎣ϕ 2 ⎦
⎣ 21

⎡
C11
−⎢
•
⎢C sin( k (ϕ + ϕ )) ϕ
1
2
1
e

⎣ 21

I3

•

(1-1)

M22 = I2+I3+ m l 22
C12 = C21 = ml1l2ke;

θ1: góc tạo bởi cánh tay 1 và phương nằm ngang.
θ2: góc tạo bởi 2 cánh tay 1 và 2.
[τ1, τ2]: moment của cánh tay 1 và cánh tay 2.
[φ1, φ 2]: góc quay của khớp nối 1 và khớp nối 2.
Các thông số của robot:
I1
moment quán tính qui về khớp nối 1.
moment quán tính của cánh tay 2 qui về khớp nối 2.
I2
moment quán tính tải qui về khớp nối 2.
I3
m
khối lượng tải.
chiều dài cánh tay 1.
l1
chiều dài cánh tay 2.
l2

GVHD: TS. Dương Hoài Nghóa


3


Luận Văn Cao Học

Điều Khiển Robot Scara

hệ số truyền động của khớp nối 2.
hệ số ma sát của khớp nối thứ 1.
hệ số ma sát của khớp nối thứ 2.
ϕ1 và ϕ2 là góc tương đối của các cánh tay xác định trước bộ nhông tăng lực. Quan hệ giữa
[ϕ1, ϕ2]T và các góc lệch [θ1, θ2]T ở hình 1-3 được cho bởi
⎡ θ1 ⎤ ⎡− k e 0 ⎤ ⎡ ϕ1 ⎤
(1-2)
⎥⎢ ⎥
⎢θ ⎥ = ⎢ k
⎣ 2 ⎦ ⎣ e k e ⎦ ⎣ϕ 2 ⎦

Ke
V1
V2

Robot được ứng dụng trong công nghiệp như đã trình bày ở trên, khi thực hiện thao tác
thì các thông số thường ảnh hưởng đến sự vận hành của robot là tải và ma sát của cơ cấu chấp
hành. Việc thay đổi của tải và ma sát của các cơ cấu chấp hành phải nằm trong một phạm vi
cho phép.
Với các đặc tính vừa mô tả về robot người nghiên cứu xin thực hiện các yêu cầu nghiên
cứu sau:
- Xây dựng giải thuật điều khiển robot scara dùng logic mờ đáp ứng được các yêu cầu:

™ Có khả năng bám theo qũy đạo định trước với sai số tónh bằng 0.
™ Ít bị ảnh hưởng bởi sự biến động của các thông số tải.
™ Tính toán trong thời gian thực.
™ Với chiều dài từ dữ liệu hữu hạn.
- Thực hiện giải thuật điều khiển trên kit DSP dùng TMS320C50.
- Mô phỏng hệ thống điều khiển.
Với các yêu cầu người nghiên cứu xây dựng sơ đồ khối điều khiển robot Scara như hình 1-4:

Tín
hiệu
đặt

+

DSP

DAC
ROBOT

ADC
Khối điều khiển

Khối đối tượng

Hình 1-4: Sơ đồ mô phỏng hệ thống điều khiển robot.
Các thành phần trong sơ đồ khối:
- “Tín hiệu đặt” là φr1 và φr2: giá trị mong muốn của các góc quay φ1 và φ2. Tín hiệu
đặt có thể nhập vào từ bàn phím của kit DSP cho từng góc quay hoặc tập hợp nhiều
giá trị của các góc quay đã lưu trong bộ nhớ.
- “DSP” là kit DSP [Digital Signal Processor] sử dụng chip TMS320C50.

- “DAC” là bộ chuyển đổi tín hiệu số sang tín hiệu tương tự để điều khiển các motor
của robot scara.
- “ADC” là bộ chuyển tín hiệu tương tự thành tín hiệu số. Tín hiệu tương tự từ các cảm
biến vị trí của 2 cánh tay được chuyển đổi thành tín hiệu số.
Để chế tạo ra robot là một việc rất phức tạp về cơ khí đảm bảo các cơ cấu vận hành của
robot và hệ thống điều khiển các cơ cấu vận hành theo đúng yêu cầu ứng dụng của robot. Do

GVHD: TS. Dương Hoài Nghóa

4


Luận Văn Cao Học

Điều Khiển Robot Scara

không có khả năng đáp ứng được về cơ cấu chấp hành nên người nghiên cứu xây dựng cơ cấu
chấp hành robot trên máy tính.

III. MÔ HÌNH TRẠNG THÁI CỦA ROBOT SCARA:
Định nghóa các biến trạng thái:
x1 = ϕ1
•

•

(1-3)

•


•

(1-4)

x2 = x1 = ϕ1
x3 = ϕ2

x4 = x3 = ϕ2
Từ phương trình (1-1) ta được:
⎡• ⎤
M 11
M 12 cos(ke ( x1 + x3 )) ⎤ ⎢ x2 ⎥
⎡
⎢
⎥⎢ • ⎥ =
M 22
⎣ M 21 cos(ke ( x1 + x3 ))
⎦ ⎢x ⎥
⎣ 4⎦
⎡
C11
−⎢
⎢
⎣⎢C21 sin(ke ( x1 + x3 )) x2

⎤⎡ ⎤
C12 sin(ke ( x1 + x3 )) x4 ⎥ ⎢ x2 ⎥ ⎡τ 1 ⎤
+
⎥ ⎢ ⎥ ⎢⎣τ 2 ⎥⎦
C22

⎦⎥ ⎣⎢ x4 ⎦⎥

(1-5)

Kết hợp các phương trình (1-3), (1-4), (1-5) ta được phương trình trạng thái sau:
•

(1-6a)

x1 = x2
•

x2 =

2
2
2
−m2L12L22ke ABx22 −(I2 + I3 + mL22)mLL
1 2keBx4 −(I2 + I3 + mL2 )Vx
1 2 − mLLV
1 2 2 Ax4 + (I2 + I3 + mL2 )τ1 + mLL
1 2 Aτ2
(1-6b)
2
2
2 2 2 2
⎡⎣(I1 + mL1 )(I2 + I3 + mL2) −m L1L2 A ⎤⎦

•


(1-6c)

x3 = x4
•

x4 =

Với

[−(I1 + mL12 )mL1L2ke Bx22 − m2 L12 L22ke ABx42 − mL1L2V1 Ax4 − (I1 + mL12 )V2 x4 + mL1L2 Aτ1 + (I1 + mL12 )τ 2
(1-6d)
⎡⎣(I1 + mL12 )(I2 + I3 + mL22 ) − m2 L12 L22 A2 ⎤⎦
A = cos(ke ( x1 + x3 ))
B = sin(ke ( x1 + x3 ))

GVHD: TS. Dương Hoài Nghóa

5


CHƯƠNG 2:

GIẢI THUẬT ĐIỀU KHIỂN ROBOT

I.

GIỚI THIỆU.

II.


VÒNG ĐIỀU KHIỂN TRONG : HỒI TIẾP TUYẾN TÍNH HÓA

III.

VÒNG ĐIỀU KHIỂN NGOÀI : ĐIỀU KHIỂN PD.

IV.

THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN

V.

-

THÔNG SỐ CỦA ROBOT.

-

VÒNG ĐIỀU KHIỂN TRONG.

-

VÒNG ĐIỀU KHIỂN NGOÀI

MÔ PHỎNG HỆ THỐNG ĐIỀU KHIỂN TRÊN MATLAB.
-

HỆ THỐNG DANH ĐỊNH.

-


ẢNH HƯỞNG CỦA SỰ THAY ĐỔI CỦA TẢI TRỌNG.

-

ẢNH HƯỞNG CỦA SỰ THAY ĐỔI CỦA HỆ SỐ MA SÁT.


Luận Văn Cao Học

I.

Điều Khiển Robot Scara

GIỚI THIỆU
Môhình của robot như hình 2-1:

φ1

Robot
Scara

τ1
τ2

φ2

Hình 2-1. Mô hình vào ra của robot.
τ1 và τ2 là moment của các motor truyền động.
φ1 và φ2 là các góc của khớp nối 1 và 2.

Phương trình vi phân của Robot:
••
M 11
M 12 cos(k e (ϕ1 + ϕ 2 ))⎤ ⎡ϕ1 ⎤
⎡
⎥ ⎢⎢ • • ⎥⎥ +
⎢ M cos(k (ϕ + ϕ ))
M 22
1
2
e
⎦ ⎣ϕ 2 ⎦
⎣ 21

⎤⎡ • ⎤
C12 sin(k e (ϕ1 + ϕ 2 )) ϕ 2 ⎥ ⎢ϕ1 ⎥ ⎡τ 1 ⎤
= ⎢ ⎥ = [τ ]
•
τ
⎥
⎥
⎢
C 22
⎦ ⎣ϕ 2 ⎦ ⎣ 2 ⎦

⎡
C11
⎢
•
⎢C sin(k (ϕ + ϕ )) ϕ

1
2
1
e
⎣ 21

•

(2-1)

⎡τ ⎤
Tín hiệu vào điều khiển: ⎢ 1 ⎥
⎣τ 2 ⎦

⎡ϕ ⎤
Tín hiệu ra điều khiển: ⎢ 1 ⎥
⎣ϕ 2 ⎦

Luật điều khiển được chia ra làm 2 vòng:
- Vòng điều khiển trong: có nhiệm vụ tuyến tính hóa và phân li quan hệ vào ra bằng
hồi tiếp trạng thái.
- Vòng điều khiển ngoài: vòng điều khiển PD.
Ngoài ra ta còn có cơ chế tự động chỉnh định thông số của bộ điều khiển PD dùng logic
mờ.

II.

VÒNG ĐIỀU KHIỂN TRONG : HỒI TIẾP TUYẾN TÍNH HÓA

Vòng điều khiển trong có nhiệm vụ tuyến tính hóa và phân li quan hệ vào ra của robot bằng

hồi tiếp trạng thái có sơ đồ hình 2-2:

τ
τa

ROBOT

+
+

φ

τb
•

••

G(φ, ϕ , ϕ )
Hình 2-2. Sơ đồ vòng điều khiển trong.
GVHD: TS. Dương Hoài Nghóa

7


Luận Văn Cao Học

Điều Khiển Robot Scara

Ta có:


τ = τ a +τb

(2-2)

Trong đó thành phần hồi tiếp τb được xác định bởi:
••
0
M 12 cos(k e (ϕ1 + ϕ 2 ))⎤ ⎡ϕ1 ⎤
⎡
τb= ⎢
⎥ ⎢⎢ • • ⎥⎥ +
0
⎣ M 21 cos(k e (ϕ1 + ϕ 2 ))
⎦ ⎣ϕ 2 ⎦

⎡
0
⎢
•
⎢C sin( k (ϕ + ϕ )) ϕ
e
1
2
1
⎣ 21

⎤⎡ • ⎤
• ••
C12 sin( k e (ϕ1 + ϕ 2 )) ϕ 2 ⎥ ⎢ϕ1 ⎥
= G (ϕ , ϕ , ϕ )

•
⎥ ⎢ϕ ⎥
0
⎦⎣ 2 ⎦
•

(2-3)

Thay phương trình (2-3) và (2-1) vào phương trình (2-2) và đơn giản ta được hệ thống
mới với tín hiệu vào là τa và tín hiệu ra là φ:
••
•
0 ⎤ ⎡ϕ1 ⎤
C11 0 ⎤ ⎡ϕ1 ⎤ ⎡τ a1 ⎤
⎡
⎢ ⎥+ ⎢
⎥ ⎢ • ⎥ = ⎢τ ⎥ = τ a
M 22 ⎥⎦ ⎢ϕ• • ⎥
0
C
22 ⎦ ⎢ϕ ⎥
⎣
⎣ 2 ⎦ ⎣ a2 ⎦
⎣ 2⎦

⎡M 11
⎢ 0
⎣

(2-4)


Ta được 2 hệ phương trình vi phân tuyến tính riêng biệt.
••

•

M ϕ+ Cϕ = τ

(2-5)

a

trong đó M và C là các ma trận hệ số (đường chéo):
⎡ M 11
⎣ 0

[M ] = ⎢

0 ⎤
M 22 ⎥⎦

(2-6)

⎡C11 0 ⎤
⎥
⎣ 0 C22 ⎦

[C ] = ⎢

(2-7)


VÒNG ĐIỀU KHIỂN NGOÀI : ĐIỀU KHIỂN PD.

III.

Luật điều khiển PD (tỷ lệ vi phân) được xác định bởi:
•

•

••

•

•

•

••

τ a = K d (ϕ − ϕ r ) + K p (ϕ − ϕ r ) + M ϕ r + C ϕ r = H (ϕ , ϕ , ϕ r , ϕ r , ϕ r )

(2-8)

Trong đó Kp, Kd là các ma trận hệ số có dạng đường chéo:
⎡K
K d = ⎢ d1
⎣ 0
⎡ K p1
Kp = ⎢

⎣ 0

0 ⎤
K d 2 ⎥⎦

(2-9)

0 ⎤
K p 2 ⎥⎦

(2-10)

⎡ϕ ⎤
ϕ r = ⎢ r1 ⎥ : các giá trị đặt của
⎣ϕ r 2 ⎦

GVHD: TS. Dương Hoài Nghóa

⎡ϕ1 ⎤
⎢ϕ ⎥
⎣ 2⎦

8


Luận Văn Cao Học

Điều Khiển Robot Scara

Thay phương trình (2-8) vào phương trình (2-5):

••

•

•

••

•

•

M ϕ + C ϕ = K d (ϕ − ϕ r ) + K p (ϕ − ϕ r ) + M ϕ r + C ϕ r

(2-11)

Viết lại:
••

••

•

•

M (ϕ − ϕ r ) + (C − K d )(ϕ − ϕ r ) − K p (ϕ − ϕ r ) = 0

(2-12)

Đặt ϕe = (ϕ − ϕ r ) : sai lệch giữa tín hiệu ra và tín hiệu đặt. Ta có:

••

•

M ϕ e + ( C − K d ) ϕ e − K pϕ e = 0

(2-13)

Phương trình (2-13) có dạng là phương trình vi phân tuyến tính thuần nhất cấp 2.
Chọn Kd và Kp sao cho phương trình đặt trưng:

Mp 2 + (C − K d ) p − K p = 0

(2-14)

Có nghiệm với phần thực âm.
Sơ đồ điều khiển kết hợp được cho ở hình 2-3:

DSP

φr

τa
• ••

•

••

H (ϕ,ϕ, ϕ ϕr ,ϕr , ϕ r )


φ

τ
+

ROBOT

+

φ

τb
•

••

G(φ, ϕ , ϕ )

Hình 2-3. Sơ đồ khối của hệ thống điều khiển gồm 2 vòng.
(Các khối G và H được thực hiện bởi DSP).

Vòng thích nghi:
Vì m và C có các giá trị không xác định chính xác phụ thuộc vào tải m và các hệ số ma
sát nên cần có khối ước lượng m và C để giải thuật điều khiển có thể thích nghi với sự thay
đổi của tải.
Các thông số tải m và hệ số ma sát C sẽ làm thay đổi thời gian đáp ứng và độ vọt lố của
hệ thống nên việc ước lượng các thông số tải và hệ số ma sát trong hệ thống này được thay
thế bằng cách xác định thời gian đáp ứng và độ vọt lố của hệ thống sau đó tiến hành chỉnh
định các thông số Kp và Kd sao cho thời gian đáp ứng nhanh nhưng độ vọt lố ít nhất.

™ Vòng thích nghi tính toán độ vọt lố và thời gian đáp ứng của hệ thống.
™ Xác định Kd và Kp dùng logic mờ.

GVHD: TS. Dương Hoài Nghóa

9


Luận Văn Cao Học

Điều Khiển Robot Scara

Khi đó sơ đồ khối của toàn bộ hệ thống như sau hình 2-4:
Xác định độ vọt lố và thời
gian đáp ứng.
Hiệu chỉnh các thông số
của bộ điều khiển.

φr

τa

H

τ

+
+

φ


ROBOT

φ

τb
G

DSP

Hình 2-4. Sơ đồ khối toàn hệ thống điều khiển.
Sơ đồ khối của hệ thống chỉnh định các thông số Kp và Kd dùng logic mờ như hình 2-5
Độ vọt lố và thời
gian đáp ứng

Mờ hóa

Luật hợp
thành

Giải
mờ

Kp
Kd

Hình 2-5. Sơ đồ khối của hệ thống chỉnh định.

IV.


THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN:

1. THÔNG SỐ CỦA ROBOT:
Phương trình của robot (2-1) được viết lại:
⎡• • ⎤
M11
M12 cos(ke (ϕ1 +ϕ2 ))⎤ ⎢ϕ1 ⎥
⎡
⎢
⎥ •• =
M22
⎣M21 cos(ke (ϕ1 +ϕ2 ))
⎦ ⎢⎢ϕ ⎥⎥
⎣ 2⎦

•
⎡
⎤⎡ • ⎤
C
C
sin(
k
(
ϕ
ϕ
))
ϕ
+
11
12

e
1
2
2
⎥⎢ϕ1 ⎥ + ⎡τ1 ⎤ (2-15)
−⎢
•
⎢
⎥⎢ • ⎥ ⎢⎣τ2 ⎥⎦
C22
⎣⎢C21 sin(ke (ϕ1 +ϕ2 ))ϕ1
⎦⎥ ⎣⎢ϕ2 ⎦⎥

Với các phương trình và các thông số thiết kế:
M12 = M21 = -mL1L2;
M11 = I1+m L12 ;
C22 = V2;
C11 = V1;

M22 = I2+I3+ m L22
C12 = C21 = mL1L2ke;

θ1: goùc tạo bởi cánh tay 1 và phương nằm ngang.
θ2: góc tạo bởi 2 cánh tay 1 và 2.
[τ1, τ2]: moment của cánh tay 1 và cánh tay 2.
[φ1, φ 2]: góc quay của khớp nối 1 và khớp nối 2.
I 1 = 1.8 kgm 2
I 2 = 0.041 kgm

2


moment quaùn tính qui về khớp nối 1.
moment quán tính của cánh tay 2 qui về khớp nối 2.

GVHD: TS. Dương Hoài Nghóa

10


Luận Văn Cao Học

Điều Khiển Robot Scara

I 3 = 0.134 kgm 2

moment quán tính tải qui về khớp nối 2.

m=10kg
L1 = 0.4m
L2 = 0.35m
k e = 0.5

khối lượng tải.
chiều dài cánh tay 1.
chiều dài cánh tay 2.
hệ số truyền động của khớp nối 2.

V1 = 5.0 kgm

2


V2 = 2.0 kgm

hệ số ma sát của khớp nối thứ 1.

s
2

hệ số ma sát của khớp nối thứ 2.
s
ϕ1 và ϕ2 là góc tương đối của các cánh tay xác
định trước bộ nhông tăng lực. Quan hệ giữa [ϕ1, ϕ2]T
và các góc lệch [θ1, θ2]T ở hình 1 được cho bởi phương
trình:
⎡ θ1 ⎤ ⎡− k e 0 ⎤ ⎡ ϕ1 ⎤
⎥⎢ ⎥
⎢θ ⎥ = ⎢ k
⎣ 2 ⎦ ⎣ e k e ⎦ ⎣ϕ 2 ⎦

Joint1

Joint2
I1

τ2
Link1 ϕ2

τ1
a)


ϕ1

l1

I2

I3
Link2
Payload m
l2

y

θ2

(x,y)

b)
(0,0)

x

θ1
Hình 2-5 - Robot SCARA

2. VÒNG ĐIỀU KHIỂN TRONG:

Để thực hiện việc điều khiển robot phải giải phương trình (2-1) theo các vòng điều khiển
đã xây dựng. Trong phương trình robot có đại lượng tính đạo hàm bậc nhất và đạo hàm bậc 2
của các giá trị φ1, φ2, để có thể tính toán ta áp dụng phương pháp xấp xỉ khâu đạo hàm:

Tính đạo hàm cấp 1 cho φ1, φ2:
•

dϕ1
ϕ [( k + 1) T ] − ϕ 1 [ k T ]
≅ 1
dt
T

(2-16)

•

dϕ2
ϕ [ ( k + 1) T ] − ϕ 2 [ k T ]
≅ 2
dt
T

(2-17)

ϕ1 =
ϕ2 =

Trong đó T là chu kỳ lấy mẫu được chọn là T= 0.01s.
Tính đạo hàm cấp 2 cho φ1, φ2:
••

•


ϕ1 =

=

•

•

d ϕ1 ϕ1[( k + 1)T ] − ϕ1[ kT ]
≅
=
dt 2
T

ϕ1[( k + 2)T ] − ϕ1[( k + 1)T ] ϕ1[( k + 1)T ] − ϕ1[ kT ]
−

T
T

T

(2-18)

ϕ1[( k + 2)T ] − 2ϕ1[( k + 1)T ] + ϕ1[ kT ]
T

2

GVHD: TS. Dương Hoài Nghóa


11


Luận Văn Cao Học
•

Điều Khiển Robot Scara

•

•

dϕ
ϕ [( k + 1)T ] − ϕ 2 [ kT ]
=
ϕ 2 = 22 ≅ 2
dt
T
••

=

ϕ 2 [( k + 2)T ] − ϕ 2 [( k + 1)T ] ϕ 2 [( k + 1)T ] − ϕ 2 [ kT ]
−

T
T

T


(2-19)

ϕ 2 [( k + 2)T ] − 2ϕ 2 [( k + 1)T ] + ϕ 2 [ kT ]
T

2

Để cho đơn giản ta có thể đặt:

ϕ1[(k )T ] = ϕ1[kT ]

ϕ1[(k + 1)T ] = ϕ1[k1T ]
ϕ1[(k + 2)T ] = ϕ1[k2T ]
ϕ2 [(k )T ] = ϕ2 [kT ]

ϕ2 [(k + 1)T ] = ϕ2 [k1T ]
ϕ2 [(k + 2)T ] = ϕ2 [k2T ]
Khi đó các phương trình đạo hàm được viết gọn lại như sau:
•

dϕ1
ϕ [ k T ] − ϕ 1[ k T ]
≅ 1 1
dt
T

(2-20)

•


dϕ2
ϕ [k T ] − ϕ 2[kT ]
≅ 2 1
dt
T

(2-21)

ϕ1 =
ϕ2 =

Phương trình đạo hàm cấp 2:
•

d ϕ 1 ϕ 1 [ k 2T ] − 2ϕ 1 [ k1T ] + ϕ 1 [ kT ]
ϕ1 =
≅
dt 2
T2
• •

•

d ϕ 2 ϕ 2 [ k 2T ] − 2ϕ 2 [ k1T ] + ϕ 2 [ kT ]
ϕ2 =
≅
dt 2
T2
• •


(2-22)
(2-23)

Áp dụng cách tính khâu xấp xỉ đạo hàm để giải phương trình của vòng điều khiển trong.
Thông số đầu vào của hệ thống này là các giá trị momemt điều khiển các motor của robot: τ1
và τ2, thông số cần tìm là các giá trị góc :

ϕ1[(k + 2)T ] = ϕ1[k2T ] vaø ϕ2 [(k + 2)T ] = ϕ2 [k2T ]

GVHD: TS. Dương Hoài Nghóa

12


Luận Văn Cao Học

Điều Khiển Robot Scara

Thay các giá trị tính đạo hàm cấp 1 và 2 vào phương trình của robot (2-1) ta có phương trình:
⎡
M11
1⎢
T 2 ⎢M21 cos ke ⎡ϕ1 ( kT ) +ϕ2 ( kT ) ⎤
⎣
⎦
⎣

{


}

{

}

M12 cos ke ⎡⎣ϕ1 ( kT ) +ϕ2 ( kT ) ⎤⎦ ⎤ ⎡ ϕ [ k T ] − 2ϕ [ k T ] +ϕ [ kT ] ⎤
1 1
1
⎥×⎢ 1 2
⎥
⎥ ⎣ϕ2 [ (k2T ] − 2ϕ2 [ k1T ] + ϕ2 [ kT ]⎦
M22
⎦

⎡
ϕ [ k T ] −ϕ2 ( kT ) ⎤
C11
C12 sin ke ⎣⎡ϕ1 ( kT ) +ϕ2 ( kT ) ⎦⎤ × 2 1
⎢
⎥
1
T
⎥ (2-24)
+ ⎢
T⎢
ϕ1 [ k1T ] −ϕ1 ( kT )
⎥
C22
⎢C21 sin ke ⎡⎣ϕ1 ( kT ) +ϕ2 ( kT )⎤⎦ ×

⎥
T
⎣
⎦

{

{

}

}

⎡ϕ [ k T ] −ϕ1 ( kT ) ⎤ ⎡τ1 ( kT ) ⎤
×⎢ 1 1
⎥=⎢
⎥
⎣ϕ2 [ k1T ] −ϕ2 ( kT ) ⎦ ⎣τ 2 ( kT ) ⎦

Khai trieån phương trình (2-24) trên thành 2 phương trình:

M11
M
cos{ke [ϕ1(kT ) +ϕ2 (kT )]}× ⎡⎣ϕ2 (k2T ) − 2ϕ2 (k1T ) +ϕ2 ( kT ) ⎤⎦
⎡ϕ (k T ) − 2ϕ1 ( k1T ) +ϕ1 ( kT ) ⎤⎦ + 12
2 ⎣ 1 2
T
T2
(2-25a)
C

C
2
+ 11 ⎡⎣ϕ1 ( k1T ) −ϕ1 ( kT ) ⎤⎦ + 122 sin{ke [ϕ1(kT ) +ϕ2 (kT )]}×[ϕ2 (k1T ) −ϕ2 (kT )] = τ1(kT )
T
T

M21
M
cos{ke [ϕ1(kT ) +ϕ2 (kT )]}× ⎡⎣ϕ1(k2T ) − 2ϕ1(k1T ) +ϕ1 ( kT ) ⎤⎦ + 222 ⎡⎣ϕ2 (k2T ) − 2ϕ2 ( kT
1 ) + ϕ2 ( kT ) ⎤
2
⎦
T
T
(2-25b)
C21
2 C22
+ 2 sin{ke [ϕ1(kT ) +ϕ2 (kT )]}×[ϕ1(kT
⎡ϕ2 ( k1T ) −ϕ2 ( kT ) ⎤⎦ =τ2 (kT )
1 ) −ϕ1 (kT )] +
T
T ⎣
Viết lại các phương trình trên để tính đạo hàm các góc:
M11
M
M
M
ϕ (k T) +ϕ2(k2T) 122 cos{ke [ϕ1(kT) +ϕ2(kT)]}= 112 ⎡⎣2ϕ1 ( kT
+ 122 cos{ke [ϕ1(kT) +ϕ2(kT)]}×⎡⎣2ϕ2(kT
1 ) −ϕ1 ( kT) ⎤

1 ) −ϕ2 ( kT) ⎤
2 1 2
⎦
⎦
T
T
T
T
C
C
2
− 11 ⎡⎣ϕ1 ( kT
− 122 sin{ke [ϕ1(kT) +ϕ2(kT)]}×[ϕ2(kT
1 ) −ϕ1 ( kT) ⎤
1 ) −ϕ2 (kT)] +τ1(kT)
⎦
T
T

M21
M
cos{ke [ϕ1(kT ) +ϕ2 (kT )]}× ⎡⎣ϕ1(k2T) − 2ϕ1(kT
+ 222 ⎡⎣ϕ2 (k2T ) − 2ϕ2 ( k1T ) +ϕ2 ( kT ) ⎤⎦
1 ) +ϕ1 ( kT ) ⎤
2
⎦
T
T
C
2 C22

+ 212 sin{ke [ϕ1(kT ) +ϕ2 (kT )]}×[ϕ1(kT
⎡ϕ2 ( k1T ) −ϕ2 ( kT ) ⎤⎦ =τ2 (kT )
1 ) −ϕ1(kT )] +
T
T ⎣

GVHD: TS. Dương Hoài Nghóa

13


Luận Văn Cao Học

Điều Khiển Robot Scara

Tiếp tục đặt

A1 = ⎡⎣ 2ϕ1 (k1T ) − ϕ1 ( kT ) ⎤⎦
A2 = ⎡⎣ 2ϕ2 (k1T ) − ϕ2 ( kT ) ⎤⎦
B1 = ⎡⎣ϕ1 (k1T ) − ϕ1 ( kT ) ⎤⎦
B2 = ⎡⎣ϕ2 (k1T ) − ϕ2 ( kT ) ⎤⎦
E = ke ⎡⎣ϕ1 (kT ) + ϕ2 ( kT ) ⎤⎦
Thế vào 2 phương trình (2-25a) và (2-25b) ta được phương trình (2-26a) và (2-26b):
M 11
M
M
M
C
C
2

ϕ1 ( k 2T ) + ϕ 2 ( k 2T ) 12
cos{E } = 11
[ A1 ] + 122 cos{E} × [ A2 ] − 11 [ B1 ] − 122 sin{E} × [ B2 ] + τ 1 ( kT ) (2-26a)
T2
T2
T2
T
T
T

ϕ1 ( k2T )

M 21
M
M
M
C
C
2
cos{E} + ϕ 2 (k2T ) 222 = 221 cos{E} × [ A1 ] + 222 [ A2 ] − 212 sin{E} × [ B1 ] − 22 [ B2 ] + τ 2 (kT ) (2-26b)
T2
T
T
T
T
T

Đặt các hệ số trong phương trình như sau:
M
M

u = 11
; v = 12
cos( E )
2
T
T2
M
M
q = 221 cos( E ); p = 222
T
T

z=

M11
M
C
C
2
A + 12
cos{E}×[ A2 ] − 11 [ B1 ] − 122 sin{E}×[ B2 ] +τ1 (kT )
2 [ 1]
2
T
T
T
T

w=


M21
M
C
C
2
cos{E}×[ A1 ] + 222 [ A2 ] − 212 sin{E}×[ B1 ] − 22 [ B2 ] +τ 2 (kT )
2
T
T
T
T

Khi đó có thể viết lại hệ phương trình (2-26) thành hệ phương trình:
⎡ uX + vY ⎤ ⎡ z ⎤
⎢ qX + pY ⎥ = ⎢ w⎥
⎣
⎦ ⎣ ⎦

(2-27)

trong đó và Y = ϕ 2 (k2T )
Giải hệ phương trình (2-27) với các hệ số vừa tính ôû treân:
Δ X = zp − wv ;
Δ = up − qv ;

GVHD: TS. Dương Hoài Nghóa

14



Luận Văn Cao Học
Giải tìm được góc

Điều Khiển Robot Scara

ϕ1[k2T ]

- gọi là phương trình (2-28a)

VX zp−wv
X=ϕ1(kT
2 )= =
V up−qv
M12
C11
C12
M22
C21
2
2 C22
⎛M11
⎞⎛M22 ⎞ ⎛M21
⎞⎛M12
⎞
⎜ 2 [ A1] + 2 cos{E}×[ A2] − [ B1] − 2 sin{E}×[ B2] +τ1(kT)⎟⎜ 2 ⎟−⎜ 2 cos{E}×[ A1] + 2 [ A2] − 2 sin{E}×[ B1] − [ B2] +τ2(kT)⎟⎜ 2 cos(E)⎟
T
T
T
T
T ⎠ ⎝T

T
T
T
T
⎝
⎠⎝
⎠⎝
⎠
=
⎞⎛M12
⎞
⎛M11 ⎞⎛M22 ⎞ ⎛M21
⎜ 2 ⎟⎜ 2 ⎟−⎜ 2 cos(E)⎟⎜ 2 cos(E)⎟
T
T
T
T
⎠⎝
⎠
⎝ ⎠⎝ ⎠ ⎝

Giải tìm được góc

ϕ2 [k2T ]

- gọi là phương trình (2-28b)

VY
Y=ϕ2(kT
2 )=

V
M22
C21
M12
C11
C12
2 C22
2
⎛M11 ⎞⎛M21
⎞ ⎛M11
⎞⎛M22 ⎞
⎜ 2 ⎟⎜ 2 cos{E}×[ A1] + 2 [ A2] − 2 sin{E}×[ B1] − [ B2] +τ2(kT)⎟−⎜ 2 [ A1] + 2 cos{E}×[ A2] − [ B1] − 2 sin{E}×[ B2] +τ1(kT)⎟⎜ 2 ⎟
T T
T
T
T
T
T
T
⎠ ⎝T
⎠⎝ T ⎠
=⎝ ⎠⎝
⎛M11 ⎞⎛M22 ⎞ ⎛M21
⎞⎛M12
⎞
⎜ 2 ⎟⎜ 2 ⎟−⎜ 2 cos(E)⎟⎜ 2 cos(E)⎟
⎝ T ⎠⎝ T ⎠ ⎝ T
⎠⎝ T
⎠
Đến đây các giá trị của ϕ1[(k + 2)T ] = ϕ1[k2T ] vaø ϕ2 [(k + 2)T ] = ϕ2 [k2T ] đã được

tính xong. Kết quả này sẽ được gởi trở lại vòng điều khiển ngoài để tính toán xem robot đã
quay đúng vị trí yêu cầu hay chưa nếu chưa đúng thì vòng điều khiển ngoài sẽ tính toán các
giá trị moment τ1 và τ2, và tiếp tục điều khiển robot quay và quá trình này diễn ra cho đến
khi robot quay đúng vị trí yêu cầu.
3. VÒNG ĐIỀU KHIỂN NGOÀI.
Vòng điều khiển này sẽ nhận giá trị như sau:
- Vị trí robot cần di chuyển đến – thông số này được nhập vào ở dạng góc quay. Do có
2 khớp quay nên phải nhập 2 giá trị kí hiệu là φr1, φr2.
- Vị trí hiện tại của robot là 2 giá trị φ1, φ2 được gởi về từ robot.
Vòng điều khiển này sẽ thực hiện việc tính toán ra các giá trị moment τ1 và τ2 từ các giá
trị góc φ1, φ2 và φr1, φr2 và điều khiển trở lại robot cho đến khi φ1= φr1; φ2 = φr2.
Từ phương trình tính moment (2-2) có thể viết lại như sau – gọi là phương trình (2-29):
• ••

τ =τa +τb = G(ϕ,ϕ, ϕ )

•

•

••

+H(ϕ,ϕ,ϕr ,ϕr , ϕ r )

⎡• • ⎤
0
M12 cos(ke (ϕ1 +ϕ2 ))⎤ ⎢ϕ1 ⎥
⎡
=⎢
⎥ ⎢• • ⎥ +

0
⎣M21 cos(ke (ϕ1 +ϕ2 ))
⎦ ⎢ϕ ⎥
⎣ 2⎦
•

•

••

•
⎡
⎤⎡ • ⎤
+
0
C
sin(
k
(
ϕ
ϕ
))
ϕ
e 1
12
2
2 ⎥⎢ϕ1 ⎥
⎢
•
⎢

⎥⎢ • ⎥
0
⎢⎣C21 sin(ke (ϕ1 +ϕ2 ))ϕ1
⎥⎦ ⎢⎣ϕ2 ⎥⎦

•

+Kd (ϕ−ϕr ) + Kp (ϕ −ϕr ) + M ϕ r + Cϕr

GVHD: TS. Dương Hoài Nghóa

15


Luận Văn Cao Học

Điều Khiển Robot Scara

Phương trình trên được viết lại thành phương trình (2-30):
⎡• •⎤
0
M12 cos(ke (ϕ1 + ϕ2 )) ⎤ ⎢ϕ1 ⎥
⎡
⎢
⎥ •• +
0
⎣ M 21 cos(ke (ϕ1 + ϕ2 ))
⎦ ⎢⎢ϕ ⎥⎥
⎣ 2⎦


•
⎡
⎤⎡ • ⎤
0
C12 sin(ke (ϕ1 + ϕ2 )) ϕ2 ⎥ ⎢ϕ1 ⎥
⎢
•
⎢
⎥⎢ • ⎥
0
⎣⎢C21 sin(ke (ϕ1 + ϕ2 )) ϕ1
⎦⎥ ⎣⎢ϕ2 ⎦⎥

⎡ •• ⎤
⎡• • ⎤
⎡ • ⎤
0 ⎤ ⎢ϕr1 ⎥ ⎡C11 0 ⎤ ⎢ϕr1 ⎥ ⎡τ1 ⎤
⎡ Kd 1 0 ⎤ ⎢ ϕ1 − ϕ r1 ⎥ ⎡ K p1 0 ⎤ ⎡ 1 r1 M11
+
ì
ì ã ã +
+
•• +⎢
⎥ • =⎢ ⎥
⎣ 0 Kd 2 ⎦ ⎢⎢ϕ − ϕ ⎥⎥ ⎣ 0 K p 2 ⎦ ⎣ϕ2 − ϕr 2 ⎦ ⎣ 0 M 22 ⎦ ⎢⎢ϕ ⎥⎥ ⎣ 0 C22 ⎦ ⎢⎢ϕ ⎥⎥ ⎣τ 2 ⎦
⎣ 2 r2 ⎦
⎣ r2 ⎦
⎣ r2 ⎦

Phương trình (2-30) được tách ra thành 2 phương trình (2-31a):

•

••

•

•

••

•

ϕ2 M12 cos[ke (ϕ1 +ϕ2 )] + C12 sin[ke (ϕ1 +ϕ2 )]×[ϕ2 ]2 +[ϕ1−ϕr1]× Kd1 + Kp1 ×[ϕ1 −ϕr1] + M11 ×ϕr1+ C11 ϕr1 = τ1
và phương trình (2-31b):
•

••
••
•
⎡• • ⎤
M21 cos(ke (ϕ1 + ϕ2 )) ×ϕ1 + C21 sin(ke (ϕ1 + ϕ2 )) ×[ϕ1 ]2 + Kd 2 × ⎢ϕ 2 − ϕ r 2 ⎥ + K p2 ×[ϕ2 − ϕr 2 ] + M22 ×ϕr 2 + C22 ϕr 2 = τ 2
⎣
⎦

Các giá trị φr1, φr2 và φ1, φ2 có thể tính bằng phương pháp xấp xỉ khâu đạo hàm từ các
phương trình (2-20), (2-21), (2-22) và (2-23).
Trong hệ phương trình (2-31) có các hệ số K d , K p là các ma trận hệ số có dạng đường
chéo cần được xác định:
⎡K
K d = ⎢ d1

⎣ 0

0 ⎤
K d 2 ⎥⎦

(2-32)

⎡ K p1
Kp = ⎢
⎣ 0

0 ⎤
K p 2 ⎥⎦

(2-33)

Viết lại phương trình (2-13):
••

•

M ϕ e + (C − K d ) ϕ e − K pϕ e = 0
Phương trình này có dạng là phương trình vi phân tuyến tính thuần nhất cấp 2 có phương
trình đặc tröng:
(2-34)
Mp 2 + ( C − K d ) p − K p = 0
Chọn Kd và Kp sao cho phương trình đặt trưng có nghiệm với phần thực âm.

Δ = (C − K d ) 2 − 4 MK p = 0
Để đơn giản cho phương trình có nghiệm kép và cho nghiệm bằng -10 suy ra nghiệm:

C − K d
p = −
= −10
2M

GVHD: TS. Dương Hoài Nghóa

16


Luận Văn Cao Học
Tính được: K

Điều Khiển Robot Scara

d

= − 2 0 M + C và K

p

= −100M

Từ các kết quả ở trên thay vào các ma trận để tính các giá trị Kd1 và Kd2, Kp1 và Kp2 ta

có:

⎡K
Kd = ⎢ d1
⎣ 0


0 ⎤
⎡M
= −20 ⎢ 11
⎥
Kd 2 ⎦
⎣ 0

0 ⎤ ⎡C11 0 ⎤
⎡ K + C11
+⎢
= −20 ⎢ d 1
⎥
⎥
0
M 22 ⎦ ⎣ 0 C22 ⎦
⎣

0
⎤
K d 2 + C22 ⎥⎦

0 ⎤
0 ⎤
⎡ K p1
⎡M
Kp = ⎢
= −100 ⎢ 11
⎥
⎥

⎣ 0 M 22 ⎦
⎣ 0 K p2 ⎦
Suy ra các hệ số Kp1, Kp2, Kd1, Kd2:
K d 1 = −20M 11 + C11
K d 2 = −20 M 22 + C22

(2-35)

K p1 = −100M 11
K p 2 = −100M 22

Thế tất cả các hệ số và các kết quả tính toán của các thông số vào hệ phương trình (231a) và (2-31b) để tính moment τ1 và τ2.
Do sử dụng máy tính thay thế robot nên các công việc tính toán của vòng điều khiển
trong được thực hiện trên máy tính.
Nhiệm vụ của máy tính là xây dựng mô hình cánh tay robot và chờ nhận giá trị đặt
moment τ1 và τ2 rồi tính các góc φ1, φ2 – điều khiển robot quay và chờ gởi các giá trị vừa tính
của φ1, φ2 về cho kit DSP.
Các công việc tính toán của vòng điều khiển ngoài được thực hiện trên kit DSP và giao
tiếp với máy tính để trao đổi các thông số góc quay của robot và moment. Phương thức giao
tiếp song song qua cổng LPT.
Giải thuật và chương trình điều khiển cho máy tính và kit DSP được trình bày ở chương
4 và trong phụ lục.
V.

MÔ PHỎNG HỆ THỐNG ĐIỀU KHIỂN TRÊN MATLAB

Robot sẽ được ứng dụng để thực hiện một công việc tùy theo yêu cầu ví dụ như gắp
một chi tiết máy trong một dây chuyền lắp ráp, quá trình lắp ráp hay vận hành robot làm việc
với các trường hợp không tải, có tải và tải thay đổi.
Trong quá trình giải phương trình tính toán moment τ1 và τ2 có chọn nghiệm của

phương trình đặt trưng (2-34) bằng -10 kết quả tìm được 4 thông số theo phương trình (2-35)
với các thông số thiết kế của robot có giá trị như sau:
Kp1
-340

Kp2
-140

Kd1
-63

Kd2
-26

Để khảo sát sự ảnh hưởng của các thông số tải và hệ số ma sát nên hệ thống được mô
phỏng trên máy tính sử dụng ngôn ngữ lập trình MATLAB.

GVHD: TS. Dương Hoài Nghóa

17


Luận Văn Cao Học

Điều Khiển Robot Scara

1.
HỆ THỐNG DANH ĐỊNH:
Hệ thống được mô phỏng theo các thông số của robot và các thông số vừa chọn để tính
toán. Chương trình mô phỏng được cho ở phụ lục A:

Kết quả của quá trình mô phỏng cho các giá trị đặt “φr1=fi1= 1; φr2 =fi2= -1;” như hình
2-6:

Hình 2-6. Kết qủa mô phỏng theo Kp, Kd danh định.
Hình 2-6 là kết quả mô phỏng của các giá trị góc “φ1= theta1” và “φ2= theta2” và
moment τ1 = To1 và τ2 = To2.
2.

ẢNH HƯỞNG CỦA SỰ THAY ĐỔI CỦA TẢI TRỌNG.

Trong phương trình tính moment τ1 và τ2 có liên quan đến hệ số “Kp1” và “Kp2”. Hai
thông số này có liên quan đến 2 thông số “M11” và “M22” và “M11” và “M22” lại có liên hệ
với khối lượng tải “m”.
Khi tải trọng “m” thay đổi sẽ làm thay đổi đến các thông số có liên quan, sự ảnh hưởng
này được mô phỏng bằng cách thay đổi giá trị danh định của Kp1 và Kp2 trong 2 trường hợp:
Trường hợp 1: giữ nguyên các hệ số Kd và thay đổi Kp mới bằng 10 lần Kp danh định:
Kết quả của quá trình mô phỏng cho các giá trị đặt “φr1= Ref1= 1; φr2 =Ref2= -1;” như
hình 2-7:

GVHD: TS. Dương Hoài Nghóa

18