饱和约束下轮式机器人的轨迹跟踪控制算法 （MATLAB实现）

轨迹跟踪效果图： 

h=1e-2;
T=40;
num=T/h+1;
time=0:h:T;
%Initialize status
[q,qr,E,e]=deal(zeros(3,num));
q(:,1)=[-0.3; -0.3; 0.4];
[Vp,Vc,Vr,u,Ev]=deal(zeros(2,num));

%Initialize parameter
[m,I,r,b,d]=deal(5,0.5,0.08,0.2,0.00);  %vehicle_param
[v_max, w_max]=deal(0.7,0.8);
u_max=[0.8; 0.8];
u0=[0; 0];


Mq=[m 0;0 I];  %kinetic_param
Bq=1/r*[1 1;b -b];
B0=1/r*[1 b;1 -b];
f_k=[1; 1];
K2=0.03;

k_theta=1.66;  %kinematic_param
h1=1;
kx=1.1;

[belta1, belta2, alpha1, delta1]=deal(48,  246,   0.4,  12);   %eso_param
[e1,e2,z1,z2]=deal(zeros(2,num));
bB=inv(Mq)*Bq;

inte=[0; 0];   %smc_param
[k1,k2, alpha2, delta2]=deal(8, 4, 0.5, 7.0);


for i=1:num
    
    %ref
    t=i*h;
    Vr(1,i)=0.3*sqrt(4*(cos(0.3*t)).^2+(cos(0.15*t)).^2);
    Vr(2,i)=(-0.3*cos(0.3*t)*sin(0.15*t)+0.6*cos(0.15*t)*sin(0.3*t))/(4*(cos(0.3*t)).^2+(cos(0.15*t)).^2);
    qr(1,i)=2*sin(0.3*t);
    qr(2,i)=2*sin(0.15*t);
    qr(3,i)=h*sum(Vr(2,:));
    
    %status
    u(:,i)=[sat(u0(1)); sat(u0(2))];
    g1=[cos(q(3,i)); sin(q(3,i)); 0];
    g2=[-d*sin(q(3,i)); -d*cos(q(3,i)); 1];
    Sq=[g1 g2];
    q(:,i+1)=q(:,i)+h*Sq*Vc(:,i);
    Vc(:,i+1)=Vc(:,i)+h*inv(Mq)*(f_k-K2*Bq*B0*Vc(:,i)+Bq*u(:,i));
    
    %kinematic controller
    E(:,i)=qr(:,i)-q(:,i);
    Te=[cos(q(3,i)) sin(q(3,i)) 0; -sin(q(3,i)) cos(q(3,i)) 0; 0 0 1];
    e(:,i)=Te*E(:,i);
    ft=k_theta*e(3,i)+sign(Vr(1,i)*e(2,i))*atan(sqrt(h1+(e(1,i))^2+(e(2,i)^2)));
    Vp(1,i+1)=v_max *sat((kx*e(1,i)+Vr(1,i)*cos(e(3,i)))/v_max);
    Vp(2,i+1)=w_max*sign(ft);     %Vp from 2 on
    
    %ESO
    e1(:,i)=z1(:,i)-Vc(:,i);
    z1(:,i+1)=z1(:,i)+h*(z2(:,i)-belta1*e1(:,i)+bB*u(:,i));
    z2(:,i+1)=z2(:,i)-h*belta2*[fal(e1(1,i),alpha1,delta1); fal(e1(2,i),alpha1,delta1)];
    
    %SMC
    Ev(:,i+1)=Vp(:,i+1)-Vc(:,i);   %Ev from 2 on
    inte=inte+k1*h/2*[ fal(Ev(1,i+1),alpha2,delta2)+fal(Ev(1,i),alpha2,delta2) ; 
                        fal(Ev(2,i+1),alpha2,delta2)+fal(Ev(2,i),alpha2,delta2) ];
    s=Ev(:,i+1)+inte;
    dVp=1/h*(Vp(:,i+1)-Vp(:,i));
    u1=inv(bB)*(dVp-z2(:,i)+ k1*[fal(Ev(1,i+1),alpha2,delta2); fal(Ev(2,i+1),alpha2,delta2)] + k2*sign(s)); 
    
    if abs(u1(1))>u_max(1)
        u0(1)=sign(u1(1))*u_max(1);
    else
        u0(1)=u1(1);
    end
    
    if abs(u1(2))>u_max(2)
        u0(2)=sign(u1(2))*u_max(2);
    else
        u0(2)=u1(2);
    end
    
end

%visualize
Vp(:,1)=[];
Vc(:,end)=[];
z1(:,end)=[];
z2(:,end)=[];

figure(1)
plot(qr(1,:),qr(2,:),'--b');
hold on
text(0,0,'o','color','b');
plot(q(1,:),q(2,:),'r');
text(-0.3,-0.3,'*','color','r');
hold off
xlabel('Longitudinal(m)');
ylabel('Lateral(m)');
title('Trajectory tracking analysis');
legend('Reference trajectory','Actual trajectory');
axis([-2.5 2.5 -2.5 2.5]);

figure(2);
plot(time,E(1,:));
hold on
plot(time,E(2,:));
hold on
plot(time,E(3,:));
hold off
xlabel('Time(s)');
ylabel('Position error(m) / Angle error(rad)');
title('Position error analysis in world coordinate system');
legend('E_x(t)','E_y(t)','E_θ(t)');
axis([0 45 -1 1]);

figure(3);
plot(time,e(1,:));
hold on
plot(time,e(2,:));
hold on
plot(time,e(3,:));
hold off
xlabel('Time(s)');
ylabel('Position error(m) / Angle error(rad)');
title('Position error analysis in vehicle coordinate system');
legend('e_x(t)','e_y(t)','e_θ(t)');
axis([0 45 -1 1]);

figure(4);
plot(time,Vp(1,:));
hold on
plot(time,Vc(1,:));
hold off
xlabel('Time(s)');
ylabel('Linear velocity(m/s)');
title('Tracking effect of SMC on linear velocity');
legend('v_p(t)','v(t)');
axis([0 45 0 1]);

figure(5);
plot(time,Vp(2,:));
hold on
plot(time,Vc(2,:));
hold off
xlabel('Time(s)');
ylabel('Angular velocity(rad/s)');
title('Tracking effect of SMC on angular velocity');
legend('w_p(t)','w(t)');
axis([0 45 -2 2]);

figure(6);
plot(time,Vc(1,:));
hold on
plot(time,z1(1,:));
hold off
xlabel('Time(s)');
ylabel('Linear velocity(m/s)');
title('Estimation of linear velocity by ESO');
legend('v(t)','z_v(t)');
axis([0 45 0 1]);

figure(7);
plot(time,Vc(2,:));
hold on
plot(time,z1(2,:));
hold off
xlabel('Time(s)');
ylabel('Angular velocity(m/s)');
title('Estimation of angular velocity by ESO');
legend('w(t)','z_w(t)');
axis([0 45 -1 1]);

figure(8);
plot(time,z2(1,:));
hold on
plot(time,z2(2,:));
hold off
xlabel('Time(s)');
ylabel('Angular velocity(m/s)');
title('Estimation of distrubance');
legend('f_v(t)','f_w(t)');
axis([0 45 -3 3]);

figure(9)
plot(time,u(1,:));
hold on
plot(time,u(2,:));
hold off
xlabel('Time(s)');
ylabel('Input voltage(v)');
title('Input voltage analysis');
legend('u_1(t)','u_2(t)');
axis([0 45 -1 1]);