1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97
| %Declaration des variables
%velocity vehicle
ee=-1;
ff=1;
e=-4;
f=4;
g=-3;
h=3;
%position vehicle
a=-3;
b1=3;
c=-1;
d=1;
for i=1:100
u(i)=e + (f-e).*rand(1,1);
v(i)=e + (f-e).*rand(1,1);
w(i)=e + (f-e).*rand(1,1);
% v(i)=ee + (ff-ee).*rand(1,1);
% w(i)=ee + (ff-ee).*rand(1,1);
p(i)=g + (h-g).*rand(1,1);
q(i)=g + (h-g).*rand(1,1);
r(i)=g + (h-g).*rand(1,1);
x(i)=a + (b1-a).*rand(1,1);
y(i)=a + (b1-a).*rand(1,1);
z(i)=a + (b1-a).*rand(1,1);
k(i)=c + (d-c).*rand(1,1);
m(i)=c + (d-c).*rand(1,1);
n(i)=c + (d-c).*rand(1,1);
%position desired
xd(i)=0.*rand(1,1);
yd(i)=0.*rand(1,1);
zd(i)=0.*rand(1,1);
kd(i)=0.*rand(1,1);
md(i)=0.*rand(1,1);
nd(i)=0.*rand(1,1);
%velocity desired
ud(i)=0.*rand(1,1);
vd(i)=0.*rand(1,1);
wd(i)=0.*rand(1,1);
pd(i)=0.*rand(1,1);
qd(i)=0.*rand(1,1);
rd(i)=0.*rand(1,1);
end
lamda=1;
%Constant sliding mode
K_d=[63.3004485960159,162.181423423896,41.8291472868677,69.3656369201298,181.131563796614,59.2394702220616;
87.6242209836225,179.701126219635,32.4269047893673,97.9391054737903,156.085717079579,30.9086058793113;
123.734446799507,194.811665420599,99.5774725775969,105.927816083716,126.146305208428,87.1503969246747;
6.99648015775473,5.84553562976381,133.977042475848,114.814578882928,0,30.5880507117889;
0.515305730309783,28.8612978668872,59.1138539405062,183.016078215629,122.679564408306,0.113027658301087;
0.993665945227155,124.569556387969,38.7705712695965,9.82764286610618,27.5194202095333,22.9051359636596];
K=[0.557788044423199,0.318498624584605,0.254549116842879,0.159196682102989,0.679198874384360,0.173483553886239;
0.223452419745479,0.0191751545697429,0.528611505553041,0.409127720415397,1,0.689693792403705;
0.313711701357736,0.397954067342930,0.605385327082023,1,0.217279758360192,0.511747087051874;
0.420943516966844,0.126587123532503,0.622183543976304,0.332166125472057,0.674439352790208,0.804636884027220;
0.249668547348253,0.929462603490907,0.882514791253589,0.911941497749215,0.402566940321656,0.856218851921692;
0.138975064982323,0,0.572384258384079,1,0.900391989174519,0.544293383282120];
%Calcul
for i=1:100
xn(:,i)=[x(1,i);y(1,i);z(1,i);k(1,i);m(1,i);n(1,i)];
xdot1(:,i)=[u(i);v(i);w(i)];
xdot2(:,i)=[p(i);q(i);r(i)];
xdot(:,i)=[xdot1(:,i);xdot2(:,i)];
x_d(:,i)=[xd(i);yd(i);zd(i);kd(i);md(i);nd(i)];
xdot_d1(:,i)=[ud(i);vd(i);wd(i)];
xdot_d2(:,i)=[pd(i);qd(i);rd(i)];
xdot_d(:,i)=[xdot_d1(:,i);xdot_d2(:,i)];
end
for i=1:100
xdot_r(:,i)=xdot_d(:,i)-(lamda*(xn(:,i)-x_d(:,i)));
end
for i=1:100
S(:,i)=xdot(:,i)-xdot_r(:,i);
end
for i=1:100
S2(:,i)=K*tan(S(:,i)/n(1,i));
end
for i=1:100
S1(:,:,i)=S(:,i)*K_d;
end |
Partager