Nghiên cứu và ứng dụng các thuật toán điều khiển thích nghi LiSlotine cho hệ chuyển động Robot Almega 16
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (1.05 MB, 47 trang )
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
- Mô phỏng cho 3 khớp đầu sao cho vị trí khớp thực bám vị trí khớp đặt
với sai số cho phép < 0.1
2
Giảng Viên Hướng Dẫn
Võ Thu Hà
CHƯƠNG 2: NGHIÊN CỨU ROBOT ALMEGA 16.
2.1.GIỚI THIỆU VỀ ROBOT ALMEGA 16.
2.1.1. Lịch sử phát triển của Robot Almega 16.
Hình 2.1. Mô hình robot Almega 16
Robot Almega 16 là Robot được sản xuất ở Nhật Bản cũng đã khá lâu, có 6
trục quay, được thiết kế riêng cho ngành công nghiệp chế tạo sử dụng tự động hóa
3
O
0
O
1
O
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
1
C
2
l
C1
l
C2
l
C3
Y
0
Y
1
Y
2
Y
3
dựa trên các tay máy có tính linh hoạt được ứng dụng rất mạnh trong công nghiệp
như vận chuyển, sắp xếp, đóng gói hàng, đặc biệt là trong ngành phun sơn, hàn…,
nó có một cấu trúc mở để đáp ứng yêu cầu sử dụng linh hoạt và có thể giao tiếp
một cách sâu rộng với các hệ thống bên ngoài. Cơ cấu của Robot được thiết kể một
cách chắc chắn, có thể chịu đựng được các môi trường khắc nhiệt hay những đòi
hỏi khắt khe của mối trường. Các phần trụ, đệm, bánh răng và các phần nhạy cảm
khác đều được bảo vệ một cách rất tốt. Ngày nay Robot Almega 16 này được sử
dụng với nhiều phiên bản khác nhau tùy thuộc vào khả năng thích ứng với môi
trường và có thể được trang bị các phần mềm ứng dụng tùy theo các yêu cầu ứng
dụng.
2.1.2.Xác định các thông số của Robot Almega 16
Do cấu trúc phức tạp của Robot mà ở đây ta không có điều kiện xác định hoàn
toàn chính xác các thông số của Robot như khối lượng các khớp, chiều dài các
khớp. Việc xác định các thông số của Robot ở đây mang tính gần chính xác nhất
bằng cách xấp xỉ các khâu của Robot về các hình đơn giản có thể tính toán được.
2.1.2.1. Đế
Đế là một bộ phận của Robot hay chính là thanh nối thứ nhất, cố định gắn với
khớp quay thứ nhất.
Thể tích của đế:
Hình 2.2. Hình dạng
thật của đế
Hình 2.3. Hình dạng
đơn giản hóa của đế
4
2 3
0
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
V 0,65.0,13.0,13 0,01099 (m )
= =
(2.3)
2.1.2.3.Khâu thứ ba
Khâu thứ ba là một phần của Robot nằm giữa khớp quay thứ ba và khớp quay
thứ tư. Thể tích của khâu thứ ba như sau:
2 3
3
0,12
V ( ) .0,47 0,00532 (m )
2
= π =
(2.4)
2.1.2.4.Khâu thứ tư
Là một phần của Robot nằm giữa khớp quay thứ tư và khớp quay thứ năm
của Robot.
Thể tích của khâu thứ tư như sau:
2 3
4
0,11
V ( ) .0,40 0,00380 (m )
2
= π =
(2.5)
Hình 2.8. Hình dạng thật của
khâu thứ ba
Hình 2.9. Hình dạng đơn
giản của khâu thứ ba
Hình 2.10. Hình dạng thật của
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.7)
2.1.3.Khối lượng của các khâu của Robot
Để cho đơn giản ở đây ta giả thiết là toàn bộ vật liệu làm Robot là cùng loại,
và đồng nhất vì vậy mà khối lượng sẽ tỉ lệ với thể tich.
Tổng thể tích của Robot là: V = 0,12035
3
m
Do tổng khối lượng của Robot là 250kg và với thể tích của các khâu được
xác định ở phần trên tính được gần đúng khối lượng của các khâu như sau:
0 4
1 5
2 6
3
m 100 kg m 10 kg
m 67 kg m 4 kg
m 52 kg m 1 kg
m 16 kg
= =
= =
= =
=
2.2.PHƯƠNG TRÌNH ĐỘNG HỌC THUẬN
2.1.1.Lập bảng DH cho 3 bậc đầu của Robot Almega 16.
8
O
0
O
1
O
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
1
C
2
l
C1
l
C2
l
C3
Y
0
Y
1
Y
2
Y
3
Hình 2.15 Mô hình Robot Almega 16
Bảng DH
Khâu
i
θ
i
a
i
α
i
d
1
1
θ
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
A
d
−
=
2 2 2 2
2 2 2 2
1
2
0
0
0 0 1 0
0 0 0 1
c s c l
s c s l
A
−
=
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
2 1 2
1
0 0 0
0 0 0
0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
c s c s c l
s c s c s l
A A A
d
−
−
= × = ×
1 2 1 2 1 1 2 2
1 2 1 2 2 1 2 2
0 0 1
2 1 2
2 2 1 2 2
0
0 0 0 1
c c c s s c c l
s s s s c s c l
A A A
s c d l s
−
− −
= × =
+
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
= × ×
− −
− −
= × = ×
+
1 23 1 23 1 1 23 3 1 2 2
1 23 1 23 1 1 2 2 1 23 3
0
3
23 23 1 2 2 23 3
0
0 0 0 1
c c c s s c c l c c l
s c s s c s c l s c l
A
s c d l s s l
− +
− − +
=
+ +
Với:
23 2 3
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
θ
=
&
2.2.1.1Khớp 1:
0
1 1
A A=
Vị trí các trục x,y,z:
1 1 1 1
0, 0,x y z d= = =
- Động năng:
10
2 0 0
1 1
1 1
. w . . w
2 2
T
i ci i i
K v m I= +
Thay số vào ta có:
.
2 2 2
1 2 2
0, 0, 0
1
x y z
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
θ
=
&
- Thế năng.
0 0
. . .
T
i i ci
P m G g P= −
,
[ ]
0
0 0 1 1
T
G = −
→
[ ]
1 1 1
. 0 0 1 1 . .P m g d= − −
1 1 1
. .P m g d=
2.2.1.2.Khớp 2:
0 0 1
2 1 2
.A A A=
2 1 2 2
cos .cosX l
θ θ
=
2 1 2 1 2 1 2 2
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
θ θ
⇒ =
&
&
2 2 2 2
2 2 2 2
2 2
2 1 2 1 1 2 2
2
1 2 1 1 2 2 2 2
2 2 2 2 2
2 2 2 1 2
( ) .[ (sin cos . +cos sin . ) +
+(cos cos . +sin sin . ) + (cos . ) ]
( ) . cos .
c
c
c c
v X Y Z
l
v l
θ θ θ θ θ θ
θ θ θ θ θ θ θ θ
θ θ θ
= + +
= −
= +
& & &
& &
& & &
& &
Độ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= −
với
[ ]
0
0 0 1 1
T
G = −
o
[ ]
0
2 2 2
2 2 1 2 2 2
. 0 0 1 1 . .
. . . . .sin
c
c
P m g P
P m g d m g l
θ
= − −
= +
2.2.1.3.Khớp 3:
0 0 0 2
3 1 2 3
. .A A A A=
.
3 1 23 3 1 2 2
3 1 23 1 3 1 23 2 3 1 2 2 1 1 2 2 2
. .
. ( )
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
θ θ
= + +
& &
&
( )
3 23 2 3 3 2 2 2c
Z c l c l
θ θ θ
⇒ = + +
& & &
&
.
( ) ( )
2 2
2 2
3 3 1 23 1 23 1 2 3
( )
c
X l s c s c
θ θ θ
⇒ = − − +
& & &
&
( ) ( )
2 2
2 2
2 1 2 1 1 2 2
l s c c s
θ θ
+ − +
& &
( ) ( ) ( )
( )
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 1 2 1 1 2 2
l c c s s
θ θ
+ + − −
& &
( )
( )
2 2
2 3 1 23 1 1 23 2 3 1 2 1 1 2 2
2
c
l l c c s s 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 3 2 23 1 3 2 2 3
2
c
l l c c c
θ θ θ θ
+ + +
& & & &
.
Động năng của tâm khối 3 là:
( ) ( )
(
)
( )
( )
( )
( )
2
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
[ ]
( )
0
3 3 3 3 1 3 2 2 23 3
0 0 1 1 . d cs
c c
P m g P m g m g l s l
= − − = + +
Thay động năng và thế năng của các khớp 1, 2, 3 vào hàm Lagarange:
L = K – P
Ta được:
L = K – P
( ) ( )
(
)
( ) ( )
( )
( )
( )
2
2 2 2 2
3 3 23 1 2 3 2 2 1 2 2 3 2 23 1 3 2 2 3
2
3 2 3 3 1 3 2 2 23 3
1
2
2
1
d cs
2
c c
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
c c c c
I l c m l l l l c I I m l
θ θ
+ + + + + + + + +
& &
( )
( ) ( )
2
2 3 3 3 3 2 3 3 1 2 3 1 3 2 2 3 3 2 2 2c c c c
I m l l l c m m m d g m g s l s l m gs l
θ θ
+ + + − + + − + −
& &
2.2.2.Sử dụng đạo hàm Lagarange cho các khớp.
Lấy đạo hàm hàm Lagrange theo biến
à
i i
v
θ θ
&
(i=1,2,3)ta được.
2.2.2.1.Khớp 1.
( )
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
3 3 2 3 2 3 3 2 3 2 2 3
2 sin os 2 sin sin
c c
m l c m l l
θ θ θ θ θ θ θ
+ + + + + +
& &
( ) ( )
2
1 3 3 3 2 3 2 3 3 2 3 2 23
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
2 2
1 1 1 2 2 2 3 3 23 2 2c c
I l c m m l c l c
τ θ
= + + + −
&&
13
2 2
1 2 2 2 2 2 3 2 2 2 3 2 3 2 23
2
1 3 3 3 23 23 3 2 3 2 23
2 s 2 2
2 2
c c
c c
m l c m l s c m l l s s
m l s c m l l c s
θ θ
θ θ
− + + −
− +
& &
& &
Viết ở dạng rút gọn:
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 2 3 3 2 3 3 3
2
2 2
3 3 3 3 2 3 3 3 2 3 3 2 3 3
( 2 ) +
+ ( ) (2 )
c c c c
c c c
L
I l m m l l l l c I
dt
I m l l l c m l l s
θ
θ
θ θ θ θ
∂ ∂
× = × + + + + +
∂
+ + − +
&&
&
&& & & &
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 )
( ) (2 )
sin( ) cos( )
2 sin sin(
c c c
c c c
c c
c
L L
dt
I l m m l l l l c I
I m l l l c m l l s
m l s c m l m l s c
m l l
τ
θ θ
τ θ
θ θ θ θ
θ θ θ θ θ
θ
∂ ∂ ∂
= × −
∂ ∂
= + + + + + +
+ + + − + +
+ + + × + + +
+
&
&&
&& & & &
& & &
]
( )
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
( )
c c
M I m l l l c= + +
2
11 2 2 2 2 3 3 2 3 2 3
2
3 2 2 2 3 2 3 2 2 3
sin( ) cos( )
2 sin sin( )
c c
c
H m l s c m l
m l s c m l l
θ θ θ θ
θ θ θ
= + + × + +
+ + +
& &
14
23 22 3 2 3 3c
H H m l l s
= −
( )
2 3 2 2 23 2 2 2 2c c
g m g c l c l m gc l= + +
2.2.2.3.Khớp 3.
2 2
3 3 3 3 2 3 3 3 2 3 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
2 3 3 2 3 2 3 3 2 3 2 23
3
2
2 3 2 3 3 2 3 3 2 3 3 23 3
1
[ 2m sin( + ).cos( + )+2m c s ] -
2
m s m s m gc
c c
c c
L
l l l
l l l l
θ θ θ θ θ
θ
θ θ θ
∂
=
∂
− − −
&
& & &
2 2
3 3 3 3 3 2 3 3 3 1 3 3
3 3
2 2
2 3 3 2 3 2 3 3 2 3 2 23
2 3 2 3 3 3 23 3 2 3 3 2 3 2 3 3 2 3
. .[ ) ] + .[ +m ( c )] +
1
+ . [ 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:
2
33 3 3 3c
M I m l= +
,
2
32 3 3 3 2 3 3
( )
c c
M I m l l l c= + +
,
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= − −
3 3 23 3c
g m gc l=
Phương trình động lực học của robot có thể viết lại ở dạng ma trận
( ) ( ; ) ( )M q q H q q G 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
M I m l= +
,
2
32 3 3 3 2 3 3
( )
c c
M I m l l l c= + +
12 1 2 13 1 3
2 2
11 1 23 2 3 22 2
2
22 2 23 2 3
2 2
2
H H
H H H H
H H
θ θ θ θ
θ θ θ θ
θ θ θ
+
= + +
+
& & &
& & &
& & &
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
=
2.2.3.Phương trình động lực học dưới dạng ma trận trạng thái.
Phương trình động lực học dưới dạng ma trận trạng thái dạng tổng quát như
sau:
M(q,q,q)q H(q,q) G(q)
τ = + +
& && && &
1 11 12 13 1 11 1
2 21 22 23 2 21 2
3 31 32 33 3 31 3
M M M q H G
M M M q H G
M M M q H G
τ
τ = + +
τ
&&
&&
&&
Trong đó:
16
( )
( )
( )
( )
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
= + + +
= + + + + +
= + +
= + +
= +
= =
= =
( ) ( )
( )
( ) ( )
( )
2 3 3
3 3
3 2
3 3 3
3 3
2
11 2 c 2 2 3 23 c 2 2 23 c 2 2 1 2
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
θ
&
( )
2 3
3
11
21 2 c 2 3 23 c 2 2
31 3 23 c
G 0
G 2m l c g m c l c a g
G m c l g
=
= − +
=
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)
&
được thiết lập như ở phần sau đây
Xác định ma trận C:
11 12 13
21 22 23
31 32 33
C C C
C C C C
C C C
=
Trong đó các hệ số C
kj
(k, j = 1÷3) được xác định như sau:
3
kj ij
ki
kj i
i=1
i j k
+ - ) .
H H
H
1
C =
2
∂ ∂
∂
θ
∂ ∂ ∂
θ θ θ
∑
&
Để đơ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
∂ ∂
∂
⇒ θ
∂θ ∂θ ∂θ
∑
&
•
Tính C
ijk
11 11 11 11
111
1 1 1 1
12 11 12 11
121
1 2 1 2
1 H H H 1 H
C = + - = = 0
2 2
1 H H H 1 H
C = + - =
2 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
C = + - = = C = m
2 2
∂ ∂ ∂ ∂
∂θ ∂θ ∂θ ∂θ
18
12 12 22
221
2 2 1
1 H H H
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
13 2312
231
2 3 1
H H1 H
C = + - = 0
2
∂ ∂∂
∂θ ∂θ ∂θ
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
∂ ∂ ∂
∂θ ∂θ ∂θ
21 21 11 11
112 121
1 1 2 2
1 H H H 1 H
C = + - = - = - C = - m
2 2
∂ ∂ ∂ ∂
∂θ ∂θ ∂θ ∂θ
22 21 12
122
1 2 2
1 H H H
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
23 1321
132
1 3 2
H H1 H
C = + - = 0
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
232
2 3 2 3
3 c 2 3
H H
1 H 1 H
C = + - =
2 2
= - m l a sin = h
∂ ∂
∂ ∂
∂θ ∂θ ∂θ ∂θ
θ
23 3121
312
3 1 2
H H1 H
C = + - = 0
2
∂ ∂∂
∂θ ∂θ ∂θ
19
23 3222 22
322
3 2 2 3
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 31 12
123
1 2 3
H H1 H
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
33 31 13
133
1 3 3
H H H1
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
31 32 21
213
2 1 3
H H1 H
C = + - = 0
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
2
∂ ∂ ∂
∂θ ∂θ ∂θ
32 33 32
323
3 2 3
H H H1
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
33 33 33
333
3 3 3
H H H1
C = + - = 0
2
∂ ∂ ∂
∂θ ∂θ ∂θ
•
Tính C
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
θ θ θ θ
& & & &
32 123 1 223 2 323 3 2
C = C + C + C = - h
θ θ θ θ
& & & &
33 133 1 233 2 333 3
C = C + C + C = 0
θ θ θ
& & &
Phương trình động lực học được viết lại như sau:
2 3
2 3 3
3 3
3 3
3
2 2 2
1 2 c 2 3 23 c 2 2
1
2 2 2
2 2 c 3 c 2 c 2 3 2 3
2
3
3 c c 2 3 3
1 2 3
2
3 c c 2 3 3 2
3
3 23 c
I m l c m (c l c a ) 0
0 m l m (l a 2l a c ) I I
0 m (l l a c ) I
0
m n
m (l l a c ) I .
m c l g
+ + +
= = + + + + +
+ +
θ θ + θ
+ + θ +
θ
τ
τ τ
τ
&& & &
&&
&&
1 1 1
1 3 2 3 2
1 2 3
m n
m h h( ) .
n h 0
θ θ θ
− θ − θ θ + θ θ
− θ − θ θ
& & &
& & & & &
& & &
2 3
3
2 c 2 3 23 c 2 2
3 c 23
0
2m l gc m g(c l c a )
m l gc
+ − +
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
tham số mô hình thì luật điều khiển thích nghi theo Li-Slotine đề xuất bằng việc
ước lượng các thông số động lực học không biết chính xác như ma trận quán tính
M, vectơ tương hỗ và ly tâm H, vectơ gia tốc trọng trường G.
Trong luật điều khiển thích nghi theo Li-Slotine luôn có hệ thống nhận dạng
tham số động của hệ thống, luôn cập nhật các tham số đó so sánh giá trị ước lượng
được với giá trị thực rồi đưa vào bộ điều khiển để hiệu chỉnh đảm bảo hệ thống ổn
định và sai lệch các khớp quay sẽ hội tụ về không, luật điều khiển cũng được xây
dựng dựa trên tiêu chuẩn ổn định Lyapunov.
Xuất phát từ phương trình động lực học của robot viết dưới dạng:
- Giả thiết
0,0 == dF
σ
( ) ( , ) ( )M q q C q q q G q
τ
= + +
&& & &&
(3.9)
Luật điều khiển thích nghi Li-Slotine được đưa ra như sau:
22
ˆ ˆ
ˆ
( ) ( , ) ( )
ˆ
( , , , )
đk D
D
M q v C q q v G q K r
Y q q v v p K r
τ
= + + +
= +
& &
& &
(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
ˆ
:p
tham số ước lượng của mô hình robot
( , , , ) :Y p p v v
& &
là ma trận hồi quy, viết gọn Y(.)
Kết hợp phương trình (3.9) và (3.10) được phương trình động lực học kín
của robot :
ˆ ˆ
ˆ
( ) ( , ) ( ) ( ) ( , ) ( )
D
M q q C q q q G q M q v C q q G q K r+ + = + + +
&& & & & &
(3.11)
Viết gọn thành :
ˆ
( ), ( )M q M q
chỉ viết
ˆ
,M M
ˆ
( , ), ( , )C q q C q q
& &
chỉ viết
ˆ
,C C
ˆ
( ), ( )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
= −
= −
= −
= −
%
% %
%
%
(3.13)
Với việc đặt như vậy phương trình động lực học kín của robot (3.11) trở
thành :
( , , , )
D
Mr Cr K r Mv Cv g
Y q q v v p
+ + = + +
=
%
%
& & %
& & %
Luật cập nhật :
ˆ
T
p y r= −Γ
&
Với bộ điều khiển thích nghi Li-Slotine đưa ra như trên thì hệ thống Robot
là ổn định theo tiêu chuẩn Lyapunov.
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ó :
[ ] [ ]
( ) ( , ) ( ) ( , ) ( ) ( ) ( , ) ( )M q r C q q r M q v C q q v G q M q q C q q q G p+ = + + − + +
& & & & && & &
( ) ( , , , ) ( , )M q r Y q q v v p C q q r
τ
⇒ = − −
& & & &
(3.19)
24
Kết hợp (3.17) và (3.19) được :
1
1
( ( , , , ) ) ( ( ) ( , ))
2
T T T
V r Y p p v v p r M q C q q r p p
τ
−
= − + − + Γ
&
& &
& & & % %
Do
1
( , ) ( )
2
S C q q M q= −
&
&
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
−
= − − + Γ
&
&
& & & & % %
1
1
T T T
D
T T T T
D
V r K r r Yp p p
V r K r p Y r p p
−
−
⇒ = − + + Γ
⇒ = − + + Γ
&
&
% % %
&
&
% % %
1
( )
T T T
D
V r K r p Y r 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
→∞
→∞ →∞
→∞
=
= ⇒ = ⇒
=
&
&
Điều này đảm bảo cho quỹ đạo của robot tiến tới quỹ đạo đặt khi thời gian
tiến tới vô cùng.
25
