Tiến sĩ Vũ Minh Hùng
1
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

ĐIỀU KHIỂN ROBOT CÔNG NGHIỆP 7 BẬC TỰ DO
Phân tích thiết kế và mô phỏng trên phần mềm Matlab

1. Đặt vấn đề
Trong quá trình thiết kế robot công nghiệp, thì việc lựa chọn động cơ có đủ mô men không hề
đơn giản. Nếu chọn động cơ có mô men quá lớn thì kích thước và khối lượng của nó cũng lớn.
Ngược lại nếu chọn động cơ có mô men nhỏ thì robot có thể không hoạt động được ở một số
không gian làm việc nào đó hoặc không đạt được vận tốc, gia tốc mong muốn. Việc tính toán mô
men cần thiết của động cơ có thể dựa vào các phần mềm động lực học. Một cách khác, là thông
qua việc mô phỏng thuật toán điều khiển trong không gian làm việc của robot với các vận tốc và
gia tốc mong muốn (đạt được giá trị lớn nhất).
Tuy nhiên, nhiệm vụ chính trong phần mô phỏng hệ thống điều khiển là tìm ra qui luật biến thiên
của lỗi khi thay đổi đầu vào và các tham số điều khiển. Qua đó sẽ xác định được tham số điều
khiển tốt nhất để cài đặt xuống bộ điều khiển thực tế.
Đối với các robot công nghiệp có ít bậc tự do (2 hoặc 3), thì việc mô phỏng hệ thống điều khiển
có thể thực hiện trên Simulink. Trong những trường hợp này, việc thiết kế các bộ điều khiển
tương đối dễ dàng. Kỹ sư thiết kế điều khiển chỉ việc gắp các khối trong Simulink và chèn vào
cửa sổ màn hình của chương trình. Việc thiết kế như vậy tuy đơn giản nhưng khi chạy chương
trình mô phỏng thì thường gặp nhiều rắc rối. Thứ nhất, việc giải phương trình động lực học trên
simulink thường có lỗi ở các khối tích phân (tính vận tốc bằng cách tích phân gia tốc, tính vị trí
bằng cách tích phân vận tốc). Thứ hai, khi phương trình động lực học phức tạp, việc chạy mô
phỏng rất tốn thời gian và nhiều khi không thực hiện được.
Do vậy, tài liệu tham khảo này sẽ trình bày một giải pháp khác là thiết kế bộ điều khiển để chạy
mô phỏng trên m file của Matlab thay vì chạy trên Simulnk. Giải pháp này có thể áp dụng cho
robot công nghiệp với hầu hết các loại bậc tự do. Để minh họa rõ hơn cho ưu điểm của cách này
so với cách chạy trên Simulink, robot công nghiệp 7 bậc tự do sẽ được lựa chọn để mô phỏng.
Cả hai bộ điều khiển vị trí và vận tốc cùng được phân tích và thiết kế. Tuy nhiên, thực tế chỉ cần


2.1 Mô hình hóa

Phương trình động lực học tổng quát của robot có dạng như sau;
( ) ( , ) ( )M q q V q q G q
t
+ + =
&& &
(1)
ở đó,
( )
M q
là ma trận
n n
´
với n là số khớp của robot. Ma trận này chứa các tham số khối lượng và
mô men quán tính.
( , )
V q q
&
là ma trận
1
n
´
miêu tả ảnh hưởng qua lại giữa vận tốc góc của các khớp.
( )
G q
là ma trận
1
n

Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

với,
a
là tỉ số truyền,
g
t
là hằng số thời gian của bộ truyền, và
m
t
là mô men của động cơ, có
quan hệ với hằng số mô men động cơ
t
K
và dòng điện i như sau,
m t
K i
t
=

(4)
Dòng điện chạy trong động cơ được cung cấp từ bộ khuếch đại công suất cũng có dạng hàm vi
phân bậc nhất là,
p pa
i i K V
t
+ =
&

(5)

( ) 1 ( )
p
d i
i
p d
f f f
f
f
K
K K
K
s s
K K s
V s V s
s
s
E s s E s
s
t t t
t
t
+ +
+ +
= ® =
+
+

(7)
ở đó,
E

Hình 2 Sơ đồ khối của bộ điều khiển PID và bộ lọc
Từ sơ đồ khối này, chúng ta có,
1 1
2 1
2 1 1 1 2
2
1
f
p p
i d d i d
f f f f f f f
x x e
x x
K K
K K K K K
V x x x x x e
t
t t t t t t t
= - +
=
æ ö
= + + = - + +
ç ÷
ç ÷
è ø
&
&
&

(8)

ê ú ê ú
ê ú
ê ú
ë û
ë û ë û
ê ú
ë û
é ù
é ù
= - +
ê ú
ê ú
ê ú
ë û
ë û
&
&

(9)

đặt
1
f
t

1
s

1
s

x
2
x
&
2
x
+

+

+

+

+

+

+

Tiến sĩ Vũ Minh Hùng
5
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

1 1
2 2
2
1
0
1

ë û
&
&
&dẫn tới,
X AX Be
V CX De
= +
= +
&

(10)
Đây là phương trình của bộ điều khiển PID và bộ lọc dưới dạng không gian trạng thái.

Áp dụng cho điều khiển vị trí

Giả sử, sai lệch vị trí của các khớp là
d
e q q
= -
, ở đó, vị trí mong muốn là
d
q
và phản hồi là
q
.
Do vậy, phương trình không gian trạng thái điều khiển vị trí sẽ là;
1 1 1 1 1

-
é ù
é ù
ê ú
= = = - =
ê ú
ê ú
ê ú
ê úë û
ë û
ê ú
ë û

1 1 1
, ,
p i d
K K K
là hệ số tỉ lệ, tích phân và vi phân, ứng với bộ PID điều khiển vị trí.

Áp dụng cho điều khiên vận tốc
Giả sử sai lệch vận tốc của các khớp là
v d
e q q
= -
& &
, ở đó, vận tốc mong muốn là
d
q
&
và phản hồi

f f f f
K
K K K
A B C D
t
t t t t
é ù
-
é ù
é ù
ê ú
= = = - =
ê ú
ê ú
ê ú
ê ú
ë û
ë û
ê ú
ë û

Tiến sĩ Vũ Minh Hùng
6
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

2 2 2
, ,
p i d
K K K là hệ số tỉ lệ, tích phân và vi phân ứng với bộ PID điều khiển tốc độ.


1 1
1 1 1
1 1
1 1
m
g g
m m t pa
p p
m t pa t pa
p p p
t pa t pa
p p
t pa d t pa d
p p
d
d
q M V q q M G q M
q q
K K V V
K K C X K K C X
K K D q K K D q
K K D q K K D q
X A X B q B q
X A X B q B q
t
t t at
t t
t t
t t
t

ï
ï
ï
ï
ï
ï
ï
ï
ï
ï
ï
ï
ï
ï
î

(14)

Định dạng lại hệ phương trình này như sau;.
Tiến sĩ Vũ Minh Hùng
7
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

1
2 1 1 2
1 1
2
1 1
2
2 2

t t
t
t
t
t t t t t
-
- -
é ù
ê ú
é ù
é ù
ê ú
ê ú
ê ú
ê ú
-
ê ú
ê ú
ê ú
ê ú
ê ú
ê ú
=ê ú
ê ú
ê ú
ê ú
ê ú
ê ú
- - -
ê ú

1 1
t pa d t pa d
p p
d
d
q
K K D q K K D q
B q
B q
t t
é ù
ê ú
ê ú
ê ú
ê ú
ê ú
+
ê ú
ê ú
ê ú
ê ú
ë û
&
&
(15)
Đây là phương trình vi phân bậc nhất dạng
X AX Bu
= +
&
nên có thể giải dễ dàng trên Matlab

Khớp quay 4
Khớp tịnh tiến 5
Khớp quay 6, 7, 8

Tiến sĩ Vũ Minh Hùng
9
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT) Hình 4 Hệ trục tọa độ của robot công nghiệp 7 bậc tự do
Các tham số DH được miêu tả dưới bảng sau:
Axis a
i
α
i
d
i
Θ
i
Initial
value
1 a
1
(0.22m) 90
o

d
1
(0.420m) Θ
1


6 0 -90
o
0 Θ
6
0
7 0 90
o
0 Θ
7
0
8 0 0 d
8
(0.018m) Θ
8
0
Tiến sĩ Vũ Minh Hùng
10
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

Các tham số của robot 7 bậc tự do được cho bên dưới.
1 4 1 8
1 2 3 4
5 6 7 8
2
0.2; 0.2; 0.475; 0.093 ( )
85.058; 62.157; 37.636; 55 ( )
84.165; 14.661; 6.591; 2.358 ( )
I1x = 1.620; I1y = 1.595; I1z = 1.077; (
kgm )

m
gc = 9.81 ;
s
t0=atan(4
æ ö
ç ÷
è ø
75/200);

Tham số mô hình của bộ lọc, động cơ (thường dùng động cơ xoay chiều vì nó có kích thước nhỏ
gọn và mô men lớn) và bộ truyền động (giả sử hiệu suất bằng 100%), được chọn là:

1 2 3 4
5 6 7 8
1 1 1
; ;
30000 30000 30000
1; 1;
: 56; 56; 500; 56;
500; 1; 1; 1;
p g f
pa t
K K
Gear ratio
t t t
a a a a
a a a a
= = =
= =
= = = =

Join K K K
Join K K K
= = =
= = =
= = =
= = =
= = =
= = =
= = =
= = =
1

và của bộ PID điều khiển vận tốc là:
1: 50 0.0005 0.5
2: 10000 0.03 0.5
3: 4000 0.02 0.5
4: 4000 0.02 0.5
5: 100 0.0005 0.5
6 : 50 0.0005 0.01
7 : 25 0.0009 0.5
8: 0.1
p d i
p d i
p d i
p d i
p d i
p d i
p d i
p d
Join K K K

kế. Tuy nhiên tại thời điểm gia tốc lớn thì sai lệch vị trí cũng tăng theo.

Hình 6 So sánh vị trí của các khớp
Tiến sĩ Vũ Minh Hùng
13
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

Hình 7 bên dưới là sự so sánh giữa vận tốc mong muốn và phản hồi. Ở đó cũng thể hiện rằng sai
lệch vận tốc lớn khi gia tốc tăng.

Hình 7 So sánh vận tốc của các khớp

Tiến sĩ Vũ Minh Hùng
14
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

Hình 8 là đồ thị gia tốc của các khớp. Giá trị của các gia tốc này lớn để kiểm tra khả năng của
động cơ.

Hình 8 Gia tốc của các khớp
Tiến sĩ Vũ Minh Hùng
15
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

Hình 9 là đồ thị mô men động cơ để robot có thể hoạt động được với quĩ đạo bên trên. Tuy nhiên
để ước lượng chính xác mô men lớn nhất của động cơ thì cần phải mô phỏng cho các không gian
làm việc khác, và tính cụ thể cho một số điểm đặc biệt.

Hình 9 Mô men cần thiết của động cơ


[t,w]= ode23('cal_dynamics_PID', tspan, winit); % Using ode23 to solve state space equations % subfunction for cal_dynamics_PID
function wdot = cal_dynamics_PID(t,w) insert dynamics equation and other equations here

wdot = ;
t
k= fix(t/delt)+1;
Các bạn quan tâm có thể liên hệ với chúng tôi theo địa chỉ email: để
có được file chương trình mô phỏng đầy đủ.
Khi có bất kỳ vấn đề gì cần trao đổi, đừng ngần ngại liên hệ với chúng tôi!
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)
Website:
Email: