cartoon.asv

本例采用最优控制的方法,利用二次型性能指标,保证小车所滑过的距离和整个过程所花费的能量达到最优值.程序包括initial.m,yjlqgctr.m,cartoon.m,simlqgyj.slk等文件

clear;
clc;
%参数
h=1.5;
k=5;
l=10;
r=0.5;
M=2.0;
m=0.1;
g=9.81;
dt=0.001;

%系统初始值
sf=1;
s0=3;
z=[0 0 s0 0]';
zf=[0 0 sf 0]';



%创建系统
c1=M*l;c2=m*l;c3=m*g;c4=(M+m)*g;
A=[0 1 0 0;c4/c1 0 0 0;0 0 0 1;-c3/M 0 0 0];
B=[0 -1/c1 0 1/M]';
C=[1 0 0 0;0 0 1 0];
D=[0 0]';
H=[0 -1/M*l 0 0]';
sys1=ss(A,B,C,D);

%最优控制器
Q=[ 1 0 0 0;
    0 1 0 0;
    0 0 50 0;
    0 0 0 1];
R=100;
kx=lqr(sys1,Q,R);
M=A-B*kx;

for i=1:10000
    z1=M*(z-zf);
    z=z+z1*dt;
    y(:,i)=z;
%作动画
x1=z(3)-k/2+z(4)*dt*i;x2=z(3)+k/2+z(4)*dt*i;
y1=-4;y2=-2.5;
x4=(x1+x2)/2+l*sin(z(1));
y4=y2+l*cos(z(1));
t=0:pi/100:2*pi;
x3=x4+r*cos(t);
y3=y4+r*sin(t);
plot([-20,20],[-4,-4],[-20:19;-19:20],[-5,-4],x3,y3,[(x1+x2)/2,x4],[y2,y4],[x1,x2],[y1,y1],[x2,x2],[y1,y2],[x1,x2],[y2,y2],[x1,x1],[y1,y2],'color','k','linewidth',3)
axis([-10 10 -10 10])
pause(0)
axis square
%if (i==1)
   % pause(0.01);
%end
if (abs(z(1)-zf(1))<=0.001)&(abs(z(3)-zf(3))<=0.002)
    break;
end
end