Mô phỏng bộ điểu khiển trượt cho hệ PENDUBOT

Một phần của tài liệu báo cáo cuối kì môn học nhận dạng và điềukhiển hệ thống đề tài mô hình hoá và thiết kế bộ điều khiển cho hệ pendubot (Trang 39 - 50)

4. Bộ điều khiển phi tuyến

4.1 Bộ điều khiển trượt

4.1.4 Mô phỏng bộ điểu khiển trượt cho hệ PENDUBOT

Hình 21:Sơ đồ bộ điều khiển trượt Trong đó cái khối a1, a2, c1, c2, k2, nuy2 là các khối constant

Ta sẽ sử dụng hàm F X B X F X B X1( ), ( ), ( ),1 2 2( ) cho việc xây dựng bộ điều khiển trượt.

Ta sẽ chọn thông số đầu vào cho góc lệch thanh 1, vận tốc góc thanh 1, góc lệch thanh 2, vận tốc góc thanh 2;

q1_init=pi/2+0.001;

q1_d_init=0.002;

q2_init=0.003;

q2_d_init=0.004;

Chương trình trong bộ điều khiển trượt:

function u = fcn(X1,X2,X3,X4,a1,a2,c1,c2,k2,nuy2) m1=0.137;

m2=0.042;

l1=0.2;

l2=0.22;

lc1=0.1016;

lc2=0.1052;

I1=0.0017;

I2=0.0001477;

Kb=0.0649;

Kt=0.0649;

Rm=6.835;

Cm=0.00005;

Jm=0.0001;

g=9.81;

K1=Kt/Rm;

K2=((Kt*Kb)/Rm)+Cm;

K3=Jm;

beta1=m1*lc1^2+m2*l1^2+I1;

beta2=m2*lc2^2+I2;

beta3=m2*l1*lc2;

beta4=m1*lc1+m2*l1;

beta5=m2*lc2;

F1=1/(beta1*beta2 -beta3^2*(cos(X3))^2+K3*beta2) *

(beta2*beta3*sin(X3)*(X2+X4)^2 + beta3^2*cos(X3)*sin(X3)*X2^2

beta2*beta4*g*cos((X1+pi/2)) + beta3*beta5*g*cos(X3)*cos((X1+pi/2)+X3) + beta2*K1*0-K2*beta2*X2);

F2=-(beta5*g*cos((X1+pi/2) + X3)*(K3 + beta1 + beta3*cos(X3)) –

beta4*g*cos((X1+pi/2))*(beta2 + beta3*cos(X3)) - K2*X2*(beta2 + beta3*cos(X3)) + beta3*sin(X3)*(X2 + X4)^2*(beta2+beta3*cos(X3)) + K3*beta3*X2^2*sin(X3) + beta3*X2^2*sin(X3)*(beta1+beta3*cos(X3)))/(- beta3^2*cos(X3)^2 + K3*beta2 + beta1*beta2) - (K1*0*(beta2 + beta3*cos(X3)))/(- beta3^2*cos(X3)^2 + K3*beta2 + beta1*beta2);

B1 = (K1*beta2)/(- beta3^2*(cos(X3))^2 + K3*beta2 + beta1*beta2);

B2= -(K1*(beta2 + beta3*cos(X3)))/(- beta3^2*(cos(X3))^2 + K3*beta2 + beta1*beta2);

%si=ci*x2i-1+x2i

s1=c1*X1+X2; %i=1 s2=c2*X3+X4; %i=2

%ueqi=-(ci*x2i+fi)/bi u_eq_1=-(c1*X2+F1)/B1;

u_eq_2=-(c2*X4+F2)/B2;

S1=a1*s1;

S2=(a1*a2)*s1+a2*s2;

u=((((a1*a2)*b1*u_eq_1) + (a2*b2*u_eq_2))-(k2*S2+nuy2*sign(S2))) / (((a1*a2)*b1) + (a2*b2));

Ở trong bài báo cáo này nhóm sẽ sử dụng giải thuật di truyền GA để tìm các thông số a1, a2, c1, c2, k2, nuy2 nên chúng ta sẽ sử dụng thêm khối Stop simulation:

Sơ đồ khối chương trình GA:

Hình 22 Lưu đồ giải thuật GA

Thông số GA:

- Số thế hệ tối đa: 200.

- Số lượng cá thể: 20.

- Hệ sốlai ghép: 0,7.

- Hệ số đột biến: 0,3.

- Kiểu mã hóa: mã hóa thập phân.

-Chọn hàm mục tiêu:

1 1 2 2

1

[ ( ) ( ) ( ) ( ) ( ( ) ( )]

n T T T

i

J e i e i e i e i rho u i u i

  

1094109\* MERGEFORMAT (.)

Chương trình GA:

%Programmed by Pham Minh Nghia

%date 01/06/2023 clc;

clear all;

rand('state',sum(100*clock));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%

q1_init=pi/2+0.001;

q1_d_init=0.002;

q2_init=0.003;

q2_d_init=0.004;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

max_generation=200; %% có 200 lan chay trong quá trình max_stall_generation=50; %% 50 the he giong nhau thì dung

epsilon=0.0001; %% J chuan ( neu the he nào có J<=epsilon tuc là tìm dc thông so thoa man, thì dung)

pop_size=20; %%so cá the trong quan the, moi cá the có moi bo PID riêng biet npar = 6; %% có 6 nhiem sac the trong 1 cá the

range=[ 0 0 0 0 0 0;...

9 9 9 9 9 9 ]; %% giá tri Ki kp kd nam trong khoang 0,9 dec=[1 1 1 1 1 1]; %% so có 3 chu so thap phân

sig=[3 3 3 3 3 3]; %% 5 chu so có nghia, có nghia là 0.000 den 0.999 cross_prob = 0.7; %% muc do cá the con giong ca the me

mutate_prob = 0.3; %% he so dot bien, he so càng lon thì cá the con càng ít giong cá the me elitism = 1; %% luon giu lai giá tri tot nhat trong khi lai tao

rho=0.02; %% trong so quyet dinh cái nào quan trong vs J hon

par = Init(pop_size,npar,range); %%khai tao 20 ca the cha me dau tien Terminal=0; %khoi dong

generation = 0; %%các giá tri

stall_generation=0; %dau tien truoc khi chay GA for pop_index=1:pop_size,

a1=par(pop_index,1);

a2=par(pop_index,2);

c1=par(pop_index,3);

c2=par(pop_index,4);

k2=par(pop_index,5);

nuy2=par(pop_index,6);

sim('pendubottruotmohinh.slx'); %%bat dau mo phong if length(e1)>990

a1 a2 c1 c2 k2

nuy2

J=(e1'*e1)+(e2'*e2)+rho*(u'*u) fitness(pop_index)=1/(J+eps);

else

J=10^100;

fitness(pop_index)=1/(J+eps);

end end;

[bestfit0,bestchrom]=max(fitness);

a10=par(bestchrom,1);

a20=par(bestchrom,2);

c10=par(bestchrom,3);

c20=par(bestchrom,4);

k20=par(bestchrom,5);

nuy20=par(bestchrom,6);

J0=1/bestfit0-0.001;%do elitism= nen doi hoi phai co 1 cha me tot nhat de so sanh

while ~Terminal,%terminal se bang 1 neu chay du 200 the he hoac trong qua trinh chay co 1 the he con cai best

generation = generation+1;%truoc moi lan chay cho hien thi

disp(['generation #' num2str(generation) ' of maximum ' num2str(max_generation)]);

pop=Encode_Decimal_Unsigned(par,sig,dec); %ma hoa thap phan(NST cua cac cha me) parent=Select_Linear_Ranking(pop,fitness,elitism,bestchrom); %sap hang cha me tuyen tinh(cha me tot nhat)

child=Cross_Twopoint(parent,cross_prob,elitism,bestchrom);%con cai sinh ra se dc lai ghep

pop=Mutate_Uniform(child,mutate_prob,elitism,bestchrom);%dot bien theo dang phan bo deu

par=Decode_Decimal_Unsigned(pop,sig,dec);%giai ma ket qua ve lai dang nst

for pop_index=1:pop_size,%lan luot test tung ca the trong quan the a1=par(pop_index,1);

a2=par(pop_index,2);

c1=par(pop_index,3);

c2=par(pop_index,4);

k2=par(pop_index,5);

nuy2=par(pop_index,6);

sim('pendubottruotmohinh.slx');%tien hanh chay mo phong de kiem tra if length(e1)>990

a1 a2 c1 c2 k2

nuy2

J=(e1'*e1)+(e2'*e2)+rho*(u'*u) fitness(pop_index)=1/(J+eps);

else

J=10^100;

fitness(pop_index)=1/(J+eps);

end end;

[bestfit(generation),bestchrom]=max(fitness);%ca the nao co kp1,ki1,kd1,kp2,ki2,kd2 se dc chon la ca the toi uu

if generation == max_generation %neu chay du 200 the he roi ma chua co ca the toi uu Terminal = 1; %thi cho terminal=1,stop GA

elseif generation>1,

if abs(bestfit(generation)-bestfit(generation-1))<epsilon, stall_generation=stall_generation+1;

if stall_generation == max_stall_generation, Terminal = 1;end %con trong qtr chay ma xuat hien ca the toi uu thi dung

else

stall_generation=0;

end;

end;

end; %While

plot(1./bestfit) a10

a20 c10 c20 k20 nuy20 J0

a1=par(pop_index,1) a2=par(pop_index,2) c1=par(pop_index,3) c2=par(pop_index,4) k2=par(pop_index,5)

nuy2=par(pop_index,6)

J = 1/bestfit(end) %ham tieu chuan tuong ung ca the con tot nhat do

sim('pendubottruotmohinh.slx');%tien hanh mo phong lai de kiem tra ca the con tot nhat do cho dap ung he thong nhu the nao

Chương trình trong khối dừng mô phỏng:

function y = fcn(e1,e2) if(abs(e1)>5||abs(e2)>5) y=1;

else y=0;

end

Thông số các khối “ To Workspace” được dùng trong chương trình GA:

Hình 23: Tín hiêu điều khiển u

Hình 24:Góc lệch thanh 1

Hình 25:Góc lệch thanh 2

Sau khi chạy chương trình GA thì ta tìm được các thông số của bộ điều khiển trượt như sau a1 = 1.6400

a2 = 9.0800 c1 = 6.2000 c2 = 8.5000 k2 = 7.0300 nuy2 = 0.7500

1.1.1 Kết quả mô phỏng bộ điểu khiển trượt cho hệ PENDUBOT:

Hình 26: Kết quả mô phỏng ngõ ra góc thanh 1

Hình 27: Kết quả mô phỏng ngõ ra góc thanh 2

Hình 28 Tín hiệu điều khiển ngõ ra của bộ điều khiển trượt

Nhận xét: Với bộ điều khiển trượt được thiết kế như trên thì ta thấy rằng hệ thống đáp ứng khá tốt. Trong vòng khoảng 3s thì hệ thống đã ổn định tại vị trí cân bằng và tín hiệu điều khiển ít bị dao động (chattering).

Một phần của tài liệu báo cáo cuối kì môn học nhận dạng và điềukhiển hệ thống đề tài mô hình hoá và thiết kế bộ điều khiển cho hệ pendubot (Trang 39 - 50)

Tải bản đầy đủ (DOCX)

(50 trang)
w