CANH TAY ROBOT MOT BAC TU DO

FINAL PROJECT
NHẬN DẠNG VÀ THIẾT KẾ BỘ ĐIỀU KHIỂN TRƯỢT, BỘ QUAN SÁT CHO
HỆ CÁNH TAY ROBOT MỘT BẬC TỰ DO
Questions 1:
a. Chọn một đối tượng tuỳ ý, thực hiện mơ hình hố để thu được mơ hình tốn
Chọn đối tượng là hệ thống “Cánh tay robot một bậc tự do”.
 Mơ hình hóa cho cánh tay robot một bậc tự do

Hình 1. Mơ hình cánh tay robot
m : Khối lượng vật nặng
M : Khối lượng cánh tay máy
l : Chiều dài cánh tay máy

lc : Khoảng các từ gốc tọa độ đến điểm chính

giữa cánh tay máy
B : Hệ số ma sát

Dựa theo hình 1, ta có:

x  l cos(  )  l sin 
2

111Equation

Chapter

(Next)

Section

1212\*

MERGEFORMAT (.)

y  l sin(  )  l cos 
2

MERGEFORMAT (.)
Động năng của vật nặng m:

313\*


1
1
K1  mv 2  mvT v
2
2
 x& 1
1
 m  x&&
y     m  x&2  y&2 
2
 y& 2

414\*

MERGEFORMAT (.)
Từ (1.1),(1.2) và (1.3), ta có:
1

 K1  ml 2&2
2

515\*

MERGEFORMAT (.)
Động năng của cánh tay M:
1
1
Mv 2  MvT v
2
2
 x&c  1
1
 M  x&c y&c     M  x&c 2  y&c 2 
2
 y&c  2

K2 

616\*

MERGEFORMAT (.)
Từ (1.1),(1.2) và (1.5), ta có:
 K2 

1
Mlc 2&2
2

717\*

MERGEFORMAT (.)
Từ (1.4) và (1.6), ta có tổng động năng:
1
K  K1  K 2  &2  ml 2  Mlc2 
2

818\*

MERGEFORMAT (.)
Thế năng của vật nặng m: P1  mgl cos 

919\*

MERGEFORMAT (.)
Thế năng của cánh tay M: P2   Mglc cos 
MERGEFORMAT (.)
Từ (1.8) và (1.9), tổng thế năng:
Phương trình Lagrange:

P  P1  P2  mgl cos   Mglc cos 

10110\*


1
L  K  P  &2 (ml 2  Mlc2 )  g (ml  Mlc ) cos 
2

Ta tính được:
L &
 &
(ml 2  Mlc2 )
&


11111\*

MERGEFORMAT (.)
L
 g (ml  Mlc ) sin 


12112\*

MERGEFORMAT (.)
Phương trình Euler- Lagrange:

L L

F
& &

13113\*

MERGEFORMAT (.)
Từ (1.10), (1.11) và (1.12), ta có:
&
&

(ml 2  Mlc2 )  g (ml  Mlc )sin   u  B&
&
& u  B  g (ml  Mlc ) sin 
 &
ml 2  Mlc2

14114\*

MERGEFORMAT (.)
b. Design the controller và bộ quan sát cho hệ thống này
 Sliding mode control

Từ (1.13) ta có:

&
& u  B  g (ml  Mlc ) sin 
&
ml 2  Mlc2

&
Đặt các biến trạng thái x1   , x2   , tín hiệu ra y  

 Bước 1: Tính đạo hàm của tín hiệu ra
y& x&1
&
&
y& &
x&
1  x2

&
y& 

g (ml  Mlc )
B
1
sin   2
& 2
u
2
2
2
ml  Mlc
ml  Mlc
ml  Mlc2

MERGEFORMAT (.)

15115\*


&
y& a ( x)  b( x)u

Với

a( x)  

g (ml  Mlc )
B

sin   2
&
2
2
2
ml  Mlc
ml  Mlc ,

b( x) 

1
ml  Mlc2
2

 Bước 2: Chọn mặt trượt
  e& k1e
Biểu thức mặt trượt: e  yd  y

Đa thức Harwitz:

( s )  s  k1

Nghiệm của ( s)  0 càng nằm xa trục ảo thì e(t )  0 càng nhanh khi   0
Chọn cực của mặt phẳng trượt -500 => k1  500
 Bước 3: Biểu thức bộ điều khiển trượt
u

1
&
y&

 a( x)  &
d  k1e  Ksign ( ) 
b( x )

K càng lớn thì   0 càng nhanh => Chọn K=1000
 Bước 4: Thiết kế bộ lọc tín hiệu vào
Chọn bộ lọc tín hiệu thơng thấp bậc 2 để tín hiệu yd (t ) khả vi bị chặn đến đạo hàm bậc 2.
Hàm truyền bộ lọc là:
GLF ( s) 

1
(0.03s  1) 2

 Bộ quan sát
Từ (1.13) ta có:

Đặt các biến trạng thái:

&
& u  B  g (ml  Mlc ) sin 
&
ml 2  Mlc2

x1    x&1  &
&
x  & x&  &
x& &
2

Phương trình khơng gian trạng thái:

2

1


 x2
 0

 x&1  


u

1
B
 x&    g (ml  Mlc ) sin( x ) 



x
1
2
 2   ml 2  Ml 2
ml 2  Mlc2   ml 2  Mlc2 
c

y  x1

Tuyến tính hóa hệ thống tại điểm:

x1   


4


Khai triển Taylor f(x,u) và h(x,u) xung quanh điểm àm việc tĩnh ( 4 ,0,0) ta có thể mơ tả

hệ thống bằng phương trình tuyến tính như sau:
 x&(t )  Ax (t )  Bu (t )
A
 y (t )  Cx(t )

Trong đó:

0
1


f


A
  g  ml  Mlc 
B
 





cos    2
x  ,0,0  
 ml 2  Mlc2
4

ml  Mlc2 
4
0


f

 , C  h
B

  1 0
1


u   ,0,0 
x  ,0,0 
2
2
 ml  Mlc 
4

4


Thay các giá trị m  0.1, l  1, lc  1, M  1, g  9,8, B  0.01 , ta thu được:
1
 0

A
3 
 6.93 9.091.10 
 0 
B
 , C   1 0
 0.091
C  1 0 
O 

CA 0 1 
 rank (O )  2

Ta tính được:
Vậy, hệ thống quan sát được

 x&
ˆ  Axˆ (t )  Bu (t )  L( y (t )  yˆ (t ))

 yˆ (t )  Cxˆ (t )

Bộ quan sát:
xˆ : ước lượng của x
với
L: tham số quan sát
Sai số ước lượng: e  x  xˆ

Nếu e  x  xˆ  0 thì xˆ  x => Bộ quan sát đạt chính xác


Xét:

ˆ
e& x& x&

e& Ax  Bu  Axˆ  Bu  L ( y  yˆ )
 A( x  xˆ )  LC ( x  xˆ )
 e& ( A  LC )e

Chọn Lyaponov funtion:

V (e)  eT Pe
V&(e)  e&T Pe  eT Pe&
 (( A  LC )e)T Pe  eT P (( A  LC )e)
 eT ( AT P  PA  C T LT P  PLC )e

&

Theo Lyaponov V (e) <0 để hệ thống ổn định toàn cục
 AT P  PA  C T LT P  PLC  0

Đặt Q=PL
AT P  PA  C T Q T  QC  0

Dùng LIM ta tính được kết quả:
 92.9033 -30.4014 
 255.3028 

P
Q


 -30.4014 93.4560  ;
 -548.8444 
 0.9247 
L  P 1Q  

 -5.5719 

Chương trình matlab giải LMI
clc; clear all;
% Solve LMI
A=[0 1;-9.8 -0.01/1.1];
B=[0; 1/1.1];
C=[1 0];
O=[C; C*A]
rank(O)
setlmis([]); % tao LMI
% Khai bao bien
P=lmivar(1,[2 1]);
Q=lmivar(2,[2 1]);
% LMI1 P>0


lmiterm([-1 1 1 P],1,1);
lmiterm([1 1 1 0],0);
% LMI2 A'*P + P*A - C'*Q' - Q*C<0
lmiterm([2 1 1 P],1,A,'s');

lmiterm([2 1 1 Q],-1,C,'s');
dfog2lmid=getlmis;
[tmin,Xfeas]=feasp(dfog2lmid);
P=dec2mat(dfog2lmid,Xfeas,P)
Q=dec2mat(dfog2lmid,Xfeas,Q)
D= eig(P)
L=inv(P)*Q
c. Mô phỏng hệ thống bằng Simulink cho cả 2 trường hợp có, và khơng có bộ
điều khiển, cho trường hợp bộ quan sát và nhận xét kết quả thu được
 Mô phỏng hệ thống bằng Simulink cho cả 2 trường hợp có, và khơng có bộ điều
khiển
 Có bộ điều khiển trượt
Mơ hình hệ thống mơ phỏng:

Hình 2. Bộ điều khiển trượt cánh tay robot một bậc
Hệ thống sử dụng tín hiệu có nhiễu khi đưa tín hiệu ra phản hồi về hệ thống.
Trong đó:


Hệ cánh tay robot mơ tả như sau:

Hình 3. Code mô tả cánh tay robot
Kết quả sau khi mô phỏng hệ thống:

Hình 4. Kết quả ngõ ra sử dụng bộ điều khiển trượt
Nhận xét: Khi có tác động của nhiễu nhưng tín hiệu ngõ ra vẫn có thể đáp ứng các yêu
cầu chung như độ vọt lố nhỏ, thời gian lên và thời gian xác lập nhanh, tín hiệu ngõ ra
bám với tín hiệu đặt, sai số nhỏ.
 Khi khơng có bộ điều khiển

Hình 5. Hệ cánh tay robot khơng có bộ điều khiển

Hình 6. Kết quả ngõ ra cánh tay robot khi khơng có bộ điều khiển
Nhận xét: Khi khơng có bộ điều khiển tín hiệu ngõ ra khơng thể đạt được tín hiệu đặt,
chỉ có thể dao động một khoảng nhỏ theo tín hiệu đặt
 Bộ quan sát

Hình 7. Bộ quan sát góc theta của cánh tay robot một bậc


Chương trình bộ quan sát:

Hình 8. Code mơ tả bộ quan sát
Kết quả bộ quan sát:

Hình 9. Kết quả quan sát góc theta của cánh tay robot một bậc
Nhận xét: Bộ quan sát được thiết kế cho tín hiệu quan sát bám sát tín hiệu ra của hệ
thống với tốc độ bám sát nhanh <5 (s).