BỘ CÔNG THƯƠNG
TRƯỜNG ĐẠI HỌC KINH TÊ - KỸ THUẬT CÔNG NGHIỆP
LUẬN VĂN TỐT NGHIỆP
TÊN ĐỀ TÀI LUẬN VĂN:
Nghiên cứu và ứng dụng các thuật toán điều khiển thích nghi Li-Slotine cho hệ
chuyển động Robot Almega 16
Ngành đào tạo: KỸ THUẬT ĐIỆN
Mã số ngành: 103
Họ và tên sinh viên: Nguyễn Minh Đông
Người hướng dẫn luận văn tốt nghiệp
1. TS. Võ Thu Hà
Hà nội - Năm 2012
1
Đề tài 2
Tên đề tài:
Nghiên cứu và ứng dụng các thuật toán điều khiển thích nghi Li-Slotine cho
hệ chuyển động Robot Almega 16
Nội dung đồ án:
Mục tiêu đề tài: Điều khiển bám quỹ đạo chính xác
Tên sinh viên: Nguyễn Minh Đông
Chương 1: Tổng quan về Robot công nghiệp
Chương 2: Nghiên cứu Robot Almega 16
- Phương trình động học thuận
- Phương trình động lực học
Chương 3: Thuật toán điều khiển thích nghi Li-Slotine cho hệ chuyển động Robot
Almega 16
- Nội dung phương pháp
- Tính toán áp dụng cho Robot Almega 16
Chương 4: Mô phỏng và kết luận
- Giới thiệu phương trình mô phỏng Matlab/Simulink và
Matlab/Simechanic

θ
1
θ
2
θ
3
Z
1
Z
0
X
3
Z
3
C
0
C
1
C
2
l
C1
l
C2
l
C3
Y
0
Y
1

0.35
V 0,35.0,35.0,40 ( ) .0.25 0.07305 m
2
= + π =
(2.1)
2.1.2.2.Khâu thứ nhất
Là một phần của Robot nối giữa khớp quay thứ nhất và khớp quay thứ hai.
Thể tích của khâu thứ nhất như sau:
2 2 3
1
0,35 0,18
V ( ) .0,22 2. ( ) .0,1 0,02625 (m )
2 2
= π + π =
(2.2)
2.1.2.3.Khâu thứ hai
Là một phần của Robot gắn giữa khớp quay thứ hai và khớp quay thứ ba.
Thể tích của khâu thứ hai như sau:
Hình 2.5. Hình dạng đơn
giản hóa của khâu thứ nhất
Hình 2.4. Hình dạng thật của
khâu thứ nhất
Hình 2.6. Hình dạng thật của khâu thứ
hai
Hình 2.7. Hình dạng đơn giản hóa của
khâu thứ hai
5
3
2


khâu thứ tư
Hình 2.11. Hình dạng đơn giản
hóa của khâu thứ tư
6
2.1.2.5.Khâu thứ năm và khâu thứ sáu
Khâu thứ 5 một phần của Robot nằm giữa khớp quay thứ năm và khớp quay thứ
sáu của Robot.
Khâu thứ 6 là một phần của Robot gắn giữa khâu thứ năm của Robot vào
khâu tác động cuối của Robot.
Thể tích của khâu thứ năm:
3
5
0,10
V ( ).0,12 0,00094 (m )
2
= π =
(2.6)
Thể tích của khâu thứ sáu:
Hình 2.12. Hình dạng thật của
khâu thứ năm và khâu thứ sáu
Hình 2.13. Hình dạng đơn
giản hóa của khâu thứ năm
Hình 2.14. Hình dạng đơn giản
hóa của khâu thứ sáu
7
2 3
6
0,08
V ( ) .0,02 0,00010 (m )
2

2
O
3
X
0
X
1
Z
2
X
2
d
1
θ
1
θ
2
θ
3
Z
1
Z
0
X
3
Z
3
C
0
C

θ
0 90
1
d
2
2
θ
2
l
0 0
3
3
θ
3
l
0 0
2.1.2.Thiết lập các ma trận A
i

1 1
1 1
0
1
1
0 0
0 0
0 1 0
0 0 0 1
c s
s c


3 3 3 3
3 3 3 3
2
3
0
0
0 0 1 0
0 0 0 1
c s c l
s c s l
A
−
 
 
 
=
 
 
 

2.1.3. Xác định phương trình động học thuận
- Tính các ma trận
0
A
i
9
1 1 2 2 2 2
1 1 2 2 2 2
0 0 1

−
 
 
− −
 
= × =
 
+
 
 
0 0 1 2
3 1 2 3
1 2 1 2 1 1 2 2 3 3 3 3
1 2 1 2 1 1 2 2 3 3 3 3
0 2
2 3
2 2 1 2 2
0
0
0 0 0 1 0
0 0 0 1 0 0 0 1
A A A A
c c c s s c c l c s c l
s c s s c s c l s c s l
A A
s c d l s
= × ×
− −
   
   

23 2 3
cos( )
sin( )
c
s
θ θ
θ θ
= +
= +
[ ]
1 23 3 1 2 2 1 23 3 1 2 2 23 3 2 2 1
, , ( ),( ),( )
T
x y z
P P P P c c l c c l s c l s c l s l s l d
 
= = + + + +
 
2.2.PHƯƠNG TRÌNH ĐỘNG LỰC HỌC.
2.2.1.Động năng, thế năng.
1 1
0,0,w
θ
 
=
 
&

2 2
0,0,w

d
= = = =
& &
&
. . .
2
2 2 2
0
1 1 1
ci
v
x y z
= + + =
→

2 0 0
1 1 1 1 1
1 1
. w . . w
2 2
T
c i
K v m I= +
2
1 1 1
1
2
K I
θ
=

sin cos . cos sin .
c c
l l
θ θ θ θ θ θ
= − −
& &
2 1 2 2
sin cosY l
θ θ
=

2 1 2 2 2 1 2 2
.sin .cos . .cos .sin .
c c
Y l l
θ θ θ θ θ θ
⇒ = −
& &
&
2 1 2 2
sinZ d l
θ
= +

2 2 2 2
. os .
c
Z l c
θ θ
⇒ =

& &
 Động năng tâm khối khớp 2 là:
2 0
2 2 2 2. 2
1 1
.
2 2
T
c
K v m I
ω ω
= +
2 2 2 2 2
2 2 2 2 1 2 2 2
1 1
( ) (cos . )
2 2
c
K m l I
θ θ θ θ
= + +
& & &
 Thế năng của thanh nối 2 là:
11
0 0
2 2 2
. . .
T
c
P m G g P= −

c c
X c c l c c l
l s c l c s s c l c s l
θ θ θ θ θ
= +
= − − + − −
& & & &
3 1 23 3 1 2 2
Y s c l s c l= +
3 3 1 23 1 3 1 23 2 3 1 2 3 1 1 2 2 2
( )
c c
Y l c c l s c c c l s s l
θ θ θ θ θ
⇒ = − + + −
& & & & &
&
3 23 3 2 2 1
dZ s l s l
= + +

( )
3 23 2 3 3 2 2
os
c
Z c l c l
θ θ
= + +
& &
&

 
+ − +
 
 
& &

( ) ( ) ( )
( )
2 3 1 23 1 1 23 2 3 1 1 1 1 2 2
2
c
l l s c c s s c c s
θ θ θ θ θ
   
+ + +
   
& & & & &
.

( ) ( )
( )
2
2
2 2 2 2
3 3 1 23 1 1 23 2 3c
Y l c c s s
θ θ θ
 
⇒ = + − +
 


( )
( )
( )
2
2
2 2 2
3 3 23 2 3 2 2 2c
Z l c l c
θ θ θ
⇒ = + +
& & &
&
( ) ( )
( )
2 3 23 2 3 2 2
2
c
l l c c
θ θ θ
+ +
& & &
.

( )
2
2 2 2 2 2 2 2 2 2 2
3 3 3 3 23 1 2 3 2 2 1 2c
X Y Z l c l c
θ θ θ θ θ

2 2 2 2
3 23 1 2 3 2 2 1 2
2
3 3 3 2 3
2 3 2 23 1 3 2 2 3
1 1
2 2
2
c
c
l s l s
K m I
l l c c c
θ θ θ θ θ
θ θ
θ θ θ θ
 
+ + + +
 
= + +
 
+ + +
 
 
& & & & &
& &
& & & &
12
 Thế năng của khối 3
[ ]

c
L m l s l s l l c c c
I m g m g l s l
θ θ θ θ θ θ θ θ θ
θ θ
 
= + + + + + + +
 
 
+ + − + +
& & & & & & & & &
& &

( )
2
2 2 2
1 1 2 2 2 3 3 23 2 2
1
2
c c
L K P I l c m m l c l c
θ
 
= − = + + + +
 
( )
2 2 2 2 2 2
2 2 2 2 3 2 2 3 3 3 3 3 3 3
1 1
2

2
2 2
1 1 2 2 2 3 3 23 2 2
1
c c
L
I l c m m l c l c
θ
θ
∂
 
= + + +
 
∂
&
&
( )
2
2 2
1 1 2 2 2 3 3 23 2 2
1
2
1 2 2 2 2 2 3 2 2 2
2 cos sin 2 sin cos
c c
c
L
I l c m m l c l c
dt
m l m l

2 sin os 2
c c
m l c m l l c s
θ θ θ θ θ θ
 
− + + +
 
& & & & & &
.
Với
1
0
L
θ
∂
=
∂
Suy ra:
1
1 1
L L
dt
τ
θ θ
∂ ∂ ∂
= × −
∂ ∂
&
( )
2

1 11 1 12 1 2 13 1 3 1
2 2M H H g
τ θ θ θ θ θ
= + + +
&& & & & &
Với :
( )
2
2 2
11 1 2 2 2 3 3 23 2 2c c
M I l c m m l c l c= + + +
2 2
12 2 2 2 2 3 2 2 2 3 2 3 2 23
s
c c
H m l c m l s c m l l s s= + +
2
13 3 3 23 23 3 2 3 2 23c c
H m l s c m l l c s= +
2.2.2.2.Khớp 2.
2 2 2 2
2 2 2 2 3 3 2 2 3 3 3 3 3 3 3 2 3 3
2
[ + m + m ( + + 2 c ) + ] + .[ + m ( + c ) ]
c c c c c
L
I l l l l l I I l l l
θ θ
θ
∂

&& & & &
2
2 2 2 2 2 3 3 23 23 3 2 2 2 3 2 3 2 3
2
3 2 2 23 3 2 2 3
sin(2 )
( )
c c c
c
L
m l s c m l s c m l s c m l l
m g c l c l m gc l
θ θ θ
θ
∂
 
= − + + + + −
 
∂
− + −
&
2
2 2
2 2 2
2 2 2 2 2 3 3 2 2 3 3 3
2 2
3 3 3 3 2 3 3 3 2 3 3 2 3 2
2 2 2
1 2 2 2 2 3 3 2 3 2 3 3 2 2 2
3 2 3 2

+ + + × + + +

+
&
&&
&& & & &
& & &
]
( )
2 3 3 2 2 23 3 2 2 2
)
c c
m g c l c l m gc l
θ θ
+ + + +
Viết rút gọn lại:
2
2 22 2 23 3 11 1 23 2 3 2 22 2
2M M H H g H
τ θ θ θ θ θ θ
= + + + + +
&& && & & & &
Với:
2 2 2
22 2 2 2 3 3 2 2 3 3 3
( 2 )
c c c
M I l m m l l l l c I= + + + + +
2
23 3 3 3 2 3 3

2
c c c
L
I m l I m l l l c
θ θ
θ
∂
 
= + + + +
 
& &
&
2 2 2
3 3 3 3 2 3 3 3 2 3 3 3 2 3 3 2 3
3
. ( ) ( )
2
c c c c
L
I m l I m l l l c m l l s
dt
θ θ θ θ
θ
∂ ∂
   
= + + + + −
   
&& && & &
&
2 2

+ . [ 2m .sin( + ).cos( + )+2m ]
2
m s + m gc + m s m
c c c
c c
c c c
L L
I m l I l l l
dt
l l l c s
l l l l l l
τ θ θ
θ θ
θ θ θ θ θ
θ θ θ θ θ
∂ ∂ ∂
= − = + +
∂ ∂
− −
&& &&
&
&
& & & & &
3
s

Hoặc viết ở dạng rút gọn:
2
3 33 3 32 2 22 2 23 2 3 3
. . .M M H H g

= + +
&& &
Với
11 12 12
21 22 23
31 32 33
M M M
M M M M
M M M
 
 
=
 
 
 
Với thành phần:
M
11
= M
12
= M
13
= M
21
= M
31
= 0
15
2
33 3 3 3c

& & &
Với:
2
22 3 3 2 3 2 3 3 2 3 3
sin( ). os( ) 2
c c
H m l c m l l s
θ θ θ θ
= + + +

23 3 3 2 3
( )
c
H m s l l= − −
g
1
= 0
( )
2 3 2 2 23 2 2 2 2c c
g m g c l c l m gc l= + +
3 3 23 3c
g m gc l=
1
2
3
g
G g
g
 
 

( )
( )
1 2 3
2 3 3
3 3
3 3
3
2
2 2
11 2 c 2 3 23 c 2 2
2 2 2
22 2 c 3 c 2 3 c 2 2 3
2
32 3 c 3 c 2 3
2
23 3 c 3 c 2 3
2
33 3 c 3
12 21
13 31
M I m l c m c l c a
M m l m l a 2c l a I I
M m l c l a I
M m l c l a I
M m l I
M M 0
M M 0

= + + +


3 23 c 2 2 23 c 1 3
2
21 3 3 c 2 2 3 2 c 2 2
2 2
3 23 c 2 2 23 c 2 2 1 3 c 3 2 3
2
31 3 23 c 23 c 2 2 1
H 2m l c s m c l c a s l s a
2m c l c a s l
H 2m s l a m l c s
m c l c a s l s a m l s a
H m s l c l c a
θ
 
= − + + + + θ θ
 
− + θ θ
= − θ θ +
+ + + θ −
= + θ +
&
& &
& &
& &
&
&
3
2
3 3 c 2 2
m s l a

Phương trình động lực học của robot cũng có thể được viết dưới nhiều dạng
khác nhau để có thể áp dụng được các thuật toán điều khiển một cách dễ dàng. Ở
chương 2 nghiên cứu các thuật toán điều khiển thông minh nên ta có thể đưa
phương trình động lực học của robot Almega 16 về dạng sau:
M(q,q,q)q C(q,q)q G(q)
τ = + +
& && && & &
1 11 12 13 1 11 12 13 1 1
2 21 22 23 2 21 22 23 2 2
3 31 32 33 3 31 32 33 3 3
M M M q C C C q G
M M M q C C C q G
M M M q C C C q G
τ
           
           
τ = + +
           
τ
           
&& &
&& &
&& &
Trong đó:
M(q)q,G(q)
&&
giống với ở phương trình động lực học trên
17
C(q,q)
&

∂
θ
∂ ∂ ∂
 
 
 
θ θ θ
 
 
∑
&
Để đơn giản ta đi đặt các hệ số C
ijk
(i, j, k = 1÷3) như sau:
3
kj ij
ki
ijk kj ijk i
i j k i=1
H H
1 H
C = + - ) C = C .
2
∂ ∂
∂
⇒ θ
∂θ ∂θ ∂θ
 
 
 

2
2 3
3
2 c 2 2 3 c 2 3 2 2
c 2 3 2 2
= - m l cos sin m [l cos( ) a cos ].
[l sin( ) a sin ] = m
θ θ − θ + θ + θ
θ + θ + θ
3 3
13 1311 11
131
1 3 1 3
3 c 2 3 2 2 c 2 3
H H
1 H 1 H
C = + - =
2 2
= - m [l cos( ) a cos ].l sin( ) = n
∂ ∂
∂ ∂
∂θ ∂θ ∂θ ∂θ
θ + θ + θ θ + θ
 
 
 
11 12 21 11
211 121
2 1 1 2
1 H H H 1 H

13 3111 11
311 131
3 1 1 3
H H1 H 1 H
C = + - = = C = n
2 2
∂ ∂∂ ∂
∂θ ∂θ ∂θ ∂θ
 
 
 
13 3212
321
3 2 1
H H1 H
C = + - = 0
2
∂ ∂∂
∂θ ∂θ ∂θ
 
 
 
13 13 33
331
3 3 1
H H H1
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ

2
∂ ∂∂
∂θ ∂θ ∂θ
 
 
 
21 22 21 22
212
2 1 2 1
1 H H H 1 H
C = + - = = 0
2 2
∂ ∂ ∂ ∂
∂θ ∂θ ∂θ ∂θ
 
 
 
22 22 22 22
222
2 2 2 2
1 H H H 1 H
C = + - = = 0
2 2
∂ ∂ ∂ ∂
∂θ ∂θ ∂θ ∂θ
 
 
 
3
23 2322 22

H H1 H 1 H
C = + - = - = - h
2 2
∂ ∂∂ ∂
∂θ ∂θ ∂θ ∂θ
 
 
 
23 23 33
332
3 3 2
H H H1
C = + - = h
2
∂ ∂ ∂
∂θ ∂θ ∂θ
 
 
 
31 31 11
113 131
1 1 3
H H1 H
C = + - = - C = - n
2
∂ ∂ ∂
∂θ ∂θ ∂θ
 
 
 

 
 
 
32 32 22
223 232
2 2 3
H H1 H
C = + - = - C = - h
2
∂ ∂ ∂
∂θ ∂θ ∂θ
 
 
 
33 32 23
233
2 3 3
H H H1
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
 
 
 
31 33 31
313
3 1 3
H H H1
C = + - = 0

kj
11 111 1 211 2 311 3 2 3
C = C + C + C = m + n
θ θ θ θ θ
& & & & &
20
12 121 1 221 2 321 3 1
C = C + C + C = m
θ θ θ θ
& & & &
13 131 1 231 2 331 3 1
C = C + C + C = n
θ θ θ θ
& & & &
21 112 1 212 2 312 3 1
C = C + C + C = - m
θ θ θ θ
& & & &
22 122 1 222 2 322 3 3
C = C + C + C = - h
θ θ θ θ
& & & &
23 132 1 232 2 332 3 2 3
C = C + C + C = h( )
θ θ θ θ + θ
& & & & &
31 113 1 213 2 313 3 1
C = C + C + C = - n
θ θ θ θ
& & & &

0
m n
m (l l a c ) I .
m c l g
+ + +
= = + + + + +
+ +
θ θ + θ
+ + θ +
θ

 

 

 

 



 

 

 

 
 


+ − +
 
 
 
 
 
Trong đó:

2 3 3
2
2 c 2 2 3 c 2 3 2 2 c 2 3
m - m l cos sin m [l cos( ) a cos ].[l sin( )
= θ θ − θ + θ + θ θ + θ
2 2
a sin ]
+ θ

3 3
3 c 2 3 2 2 c 2 3
n - m [l cos( ) a cos ].l sin( )
= θ + θ + θ θ + θ
3
3 c 2 3
h - m l a sin
= θ
21
CHƯƠNG 3: THUẬT TOÁN ĐIỀU KHIỂN THÍCH NGHI LI-SLOTINE
CHO HỆ CHUYỂN ĐỘNG ROBOT ALMEGA 16.
3.1. NỘI DUNG PHƯƠNG PHÁP:
Khi mô hình robot là không biết rõ hoặc không xác định được chính xác các

= +
& &
& &
(3.10)
Trong đó:

ˆ ˆ
ˆ
( ), ( , ), ( ):M q C q q G q
&
các thành phần ước lượng của
( ), ( , ), ( )M q C q q G q
&

( )
d d
v q q q q e= + ∧ − = + ∧
& &

:∧
là ma trận chéo, xác định dương

r v q= −
&

:
D
K
hệ số hiệu chỉnh


( ), ( )G q G q
chỉ viết
ˆ
,G G
Đặt
q v r r v q
q v r r v q
= − = −
 
⇒
 
= − = −
 
&& & & & & &&
& &
(3.12)
Và các sai lệch giữa giá trị thật và ước lượng như sau :
23
ˆ
ˆ
ˆ
M M M
C C C
g g g
p p p

= −

= −


Chọn hàm Lyapunov xác định dương :
1
1 1
( )
2 2
T T
V r M q r p p
−
= + Γ
% %
(3.16)
1
1
( ) ( )
2
T T T
V r M q r r M q r p p
−
⇒ = + + Γ
&
& &
& % %
(3.17)
Đặt :
( , , , ) ( ) ( , ) ( )Y p p v v p M q v C q q v G p= + +
& & & &
(3.18)
Trong đó :
p: tham số động lực học của mô hình robot
Kết hợp với phương trình động lực học (3.9) ta có :

&
là ma trận nghiêng tuyệt đối
Nên
1
( ( ) ( , )) 0
2
T
r M q C q q r− =
&
&
(3.20)
Do đó :
1
( ( , , , ) )
T T
V r Y p p v v p p p
τ
−
= − + Γ
&
&
& & % %
(3.21)
Với luật điều khiển như ở (3.10) thì :
1
ˆ
( ( , , , ) ( , , , ) )
T T
D
V r Y p p v v p Y p p v v p K r p p

&
&
% %
(3.22)
Với luật cập nhật thích nghi như ở hình (3.15) luôn đảm bảo
0
T
D
V r K r= − ≤
&
Có nghĩa là hệ thống ổn định.
Hay :
lim 0
lim 0 lim 0
lim 0
x
x x
x
e
V r
e
→∞
→∞ →∞
→∞
=


= ⇒ = ⇒

=