ĐIỀU KHIỂN ROBOT CÔNG NGHIỆP 7 BẬC TỰ DO

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

dùng bộ điều khiển vị trí là đủ để điều khiển bám quĩ đạo. Khi có thêm phần điều khiển tốc độ
thì sai lệch vị trí có được cải thiện đôi chút nhưng nó cũng gây lên sự phức tạp cho việc cài đặt
thuật toán điều khiển xuống bộ vi xử lý.

2. Thiết kế hệ thống điều khiển
Hình 1 dưới đây miêu tả sơ đồ cấu trúc của hệ thống điều khiển robot công nghiệp. Ở đó, bộ điều
khiển PID sẽ được lựa chọn để mô phỏng. Phần mô hình của robot bao gồm khối động lực học,
khối động cơ, khối khuếch đại công suất và khối lọc. Tất cả các khối này sẽ được mô hình hóa để
thiết kế một hệ thống điều khiển đầy đủ như chạy trong thực tế.

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









Hình 1 Sơ đồ mô phỏng hệ thống điều khiển robot công nghiệp
Ở trên hình 1, khối trajectory planning có chức năng chuyển quĩ đạo từ hệ tọa độ đề các sang các
góc quay của khớp (giải động học ngược). Đầu ra của bộ điều khiển PID là giá trị điện áp sẽ
được lọc để giảm nhiễu trước khi tới bộ khuếch đại công suất (nếu dùng động cơ một chiều thì
nó là Driver, nếu dùng động cơ xoay chiều thì là biến tần Inverter). Đầu ra của khối khuếch đại
công suất này sẽ cung cấp dòng đủ lớn cho động cơ. Dòng điện chạy trong các cuộn dây của
động cơ sẽ sinh ra mô men trên trục được lắp với bộ truyền động để tăng mô men cung cấp cho
các khâu của robot.

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

´
miêu tả ảnh hưởng của trọng lực.
Từ phương trình động lực học này, véc tơ gia tốc của các khớp có thể được tính là,
1 1 1
( , ) ( )q M V q q M G q M
t
- - -
= - - +
&& &

(2)
ở đó,
t
là mô men quay quanh các khớp, được cung cấp bởi đầu ra của bộ truyền động và có
dạng hàm vi phân bậc nhất dưới đây,


1 1
m
g g
t t at
t t
= - +
&

(3)

Gear
Tiến sĩ Vũ Minh Hùng
3

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)

ở đó, V là điện áp đầu vào (chính là giá trị đầu ra của bộ điều khiển),
p
t
là hằng số thời gian của
bộ khuếch đại công suất và
pa
K
là hệ số khuếch đại.
Kết hợp giữa mô hình của động cơ và bộ khuếch đại công suất, thu được
1 1
m m t pa
p p
K K V
t t
t t
= - +
&

(6)

Như vậy, các thành phần của hệ thống robot đã được mô hình hóa. Bước tiếp theo là thiết kế bộ
điều khiển PID dưới dạng tổng quát, rồi sau đó áp dụng cho điều khiển vị trí và vận tốc của các
khâu robot.

2.2 Thiết kế bộ điều khiển PID

Hàm truyền của bộ điều khiển PID với bộ lọc bậc nhất được biểu diễn như sau,
2
2
( ) ( )

( ) 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

là sai lệch giữa giá trị mong muốn và phản hồi, V là điện áp, từ đầu ra của bộ điều khiển,

, ,
p i d
K K K
là hệ số tỉ lệ, tích phân và vi phân,
f
t
là hằng số thời gian bộ lọc

Sơ đồ khối của bộ điều khiển PID cùng với bộ lọc bậc nhất được thể hiện trên sơ đồ sau.





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

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)

Viết gọn các phương trình này dưới dạng không gian trạng thái như sau,
1 1
2 2
1
2
2
1
0
1
0
1 0
f
p
d i d
f f f f
x x
e
x x
K x
K K K
V e
x
t
t t t t
é ù
-
é ù é ù
é ù
ê ú
= +

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

(9)

đặt
1
f
t

1
s

1
s

1
f
t
d
f
K
t
d
f
K
t
+

V
E
-

V
&&
V
&

V

1
x
&

1

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

0
1 0
p
d i d
f
f f f f
x x
X X
x x
K
K K K
A B C D
t
t t t t
é ù é ù
= =
ê ú ê ú
ë û ë û
é ù
-
é ù
é ù
ê ú
= = = - =
ê ú
ê ú
ê ú
ê úë û
ë û
ê ú

ë û
&
&
&


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 1 1
d
d
X A X B q B q
V C X D q D q
= - +
= - +
&

(11)
với,
1
1 1 1
1 1 1 1
2
1
0
1
0
1 0
p
d i d
f
f f f f
K
K K K
A B C D
t
t t t t
é ù

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

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

là
q
&
. Vì vậy, phương trình không gian trạng thái điều khiển vận tốc là;
2 2 2 2 2
2 2 2 2 2
d
d
X A X B q B q
V C X D q D q
= - +
= - +
&
& &
& &

(12)
ở đó
2
2 2 2
2 2 2 2
2
1
0
1
0
1 0
p
d i d
f

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 độ.

Như vậy, tổng điện áp từ hai bộ điều khiển tới bộ khuếch đại công suất là,

1 2
V V V
= +

và mô men của động cơ được tính như sau,
( )
1 2
1 1
m m t pa
p p
K K V V
t t
t t
= - + +
&

(13)
Tổng hợp các phương trình trên, thu được hệ phương dưới đây,
( )
1 1 1
1 2
1 1 2 2
1 2
1 2
1 1 1 1 1
2 2 2 2 2
( , ) ( )
1 1

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

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

1 1
0 0 0 0 0
0 0 0 0 0
1
0 0 0 0
1 1 1 1 1
0
0 0 0 0
0 0 0 0
( , )
g g
m
m
t pa t pa t pa t pa
p p p p p
M
q
q
I
q
q
K K D K K D K K C K K C
X X
X
B A
X
B A
M V q q M G
a
t

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

ê ú
ê ú
ê ú
ê ú
ê ú
ê ú
-
ê ú
ë û
ë û
ê ú
ê ú
-
ë û
- -
+
&&
&
&
&
&
&
&
&
1 2
1
2
( )
0
0

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

(xem hướng dẫn cách giải ở phần cuối của tài liệu này).

3. Mô phỏng trên phần mềm MATLAB
Mô hình 3D của một loại robot công nghiệp 7 bậc tự do, sử dụng trong phần mô phỏng này,
được miêu tả như trên hình dưới đây.








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



Hình 3 Mô hình 3D của một loại robot công nghiệp 7 bậc tự do
Robot công nghiệp này có 6 khớp quay và 2 khớp tịnh tiến nhưng do khớp 2 và khớp 4 có ràng
buộc với nhau để cho khớp tịnh tiến 5 luôn song song với mặt đất, nên robot này chỉ có 7 bậc tự
do. Cấu trúc robot hơi phức tạp nhưng với 2 khớp tịnh tiến nó có thể thu nhỏ cấu hình, mở rộng
không gian làm việc và trở lên linh hoạt hơn. Robot loại này sẽ có nhiều ứng dụng trong thực tế
sản xuất như việc lắp ráp 2 chi tiết máy trong điều kiện nhiệt độ cao.
Các hệ tọa độ của robot được định nghĩa như trên hình 4.

Khớp quay 1
Khớp quay 2
Khớp tịnh tiến 3

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

90
o

2 0 -90
o
0 Θ
2
0
3 0 90
o
d
3
(variable) 0 1.025
4 0 0 d
4
(variable)

0 0.5645
5 a
4
(0.207m) 90
o
0 Θ
5
= (90
o
-Θ
2
) 90
o

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 )

I2x = 3.556; I2y = 0.458; I2z = 3.572; (k
a a d d m
m m m m kg
m m m m kg
= = = =
= = = =
= = = =
2
2
2
2
2
gm )
I3x = 1.257; I3y = 1.292; I3z = 0.119;
(kgm )
I4x = 1.500; I4y = 1.500; I4z = 1.500; (
kgm )
I5x = 6.170; I5y = 6.225; I5z = 0.274;
(kgm )
I6x = 0.104; I6y = 0.086; I6z = 0.039; (
kgm )
I7x = 0.007; I
2
2
2
7y = 0.007; I7z = 0.005; (kgm )
I8x = 0.0003; I8y = 0.0003; I8z = 0.0005
; (kgm )
r1 = 0.15; r2 = 0.296; r3 = 0.736; r4 = 0.1; (m)
r5 = 0.4; r6 = 0.7; r7 = 0.01; r8 = 0.086; (m)

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
= = =
= =
= = = =

= = = =


Tham số của bộ PID điều khiển vị trí được lựa chọn sau nhiều lần mô phỏng là:
Tiến sĩ Vũ Minh Hùng
11
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

1: 5 0.005 0.5
2: 400 0.03 5
3: 300 0.02 0.5
4: 400 0.02 0.5
5: 5 0.005 0.5
6 : 60 0.005 0.01
7 : 70 0.009 0.5
8: 0.01 0.0001 0.0
p d i
p d i
p d i
p d i
p d i
p d i
p d i
p d i
Join K K K
Join K K K
Join K K K
Join K K K
Join K K K
Join K K K

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

Join K K K
Join K K K
Join K K K
Join K K K
Join K K K
Join K K K
Join K K
= = =
= = =
= = =
= = =
= = =
= = =
= = =
= =
0.0001 0.01
i
K =

Quĩ đạo của robot được thiết kể để chạy đến tốc độ tối đa 1.5 m/s tại gốc cái tay kẹp (end
effector). Thời gian mô phỏng ngắn (chỉ có 3 giây) để robot có thể đạt được gia tốc rất lớn. Đồ
thị vận tốc của robot được biểu diễn trên hình 5.

Hình 5 Vận tốc của robot tại điểm gốc của tay kẹp (end effector)
Tiến sĩ Vũ Minh Hùng
12
Diễn đàn Cơ điện tử, Haptic và Thực tế ảo (Diễn đàn CHAT)

Hình 6 bên dưới là sự so sánh giữa vị trí mong muốn (đường xanh da trời) và vị trí phản hồi
(đường xanh lá cây) của các khớp. Từ đây, chúng ta thấy rằng robot đã bám rất tốt quĩ đạo thiết

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ơ

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

4. Kết luận
Tài liệu này đã trình bày một giải pháp thiết kế và mô phỏng bộ điều khiển PID cho robot công
nghiệp 7 bậc tự do. Các thành phần của hệ thống robot đã được mô hình hóa để kết quả mô
phỏng gần với kết quả thực nghiệm. Tiến sĩ Vũ Minh Hùng cũng đã cài đặt các thuật toán điều
khiển cho robot này tại một công ty ở Hàn Quốc. Trong thời gian tới, các sản phẩm robot có cấu
trúc loại này sẽ xuất hiện trên thị trường và ứng dụng trong thực tế.

Hướng dẫn giải phương trình không gian trạng thái bên trên để có được kết quả mô phỏng
Đặt giá trị khởi đầu cho các véc tơ
% initial values for solving equations
q_init= [0 0 0.854 1.5708 0.8740 0 0 0]'; % initial values of joint position
qd_init= zeros(8,1); % initial values of joint velotiy
torque= zeros(8,1); % initial values of joint torque
torque_m= zeros(8,1); % initial values of motor torque
X1 = zeros(16,1); % initial values of X1
X2 = zeros(16,1); % initial values of X2

winit= [qd_init; q_init; torque; torque_m; X1; X2]; % inital value vector
k=1; % index, global
tf= 3.1; % time for simulation
delt= 0.1; % step for simulation

tspan= [0: delt: tf]; % time vector for simulation

Giải phương trình bằng hàm ode23 trong matlab

[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: