tuoyuan2lj.m

本人写的利用backstepping算法设计的移动机器人轨迹跟踪控制器matlab源代码

t=0:0.1:100;
n=length(t);
af=1;bf=1.5;
l=0.2005;r=0.097;
e=[];u=[];se=[];
se(1)=pi/4;
kx=1;ky=124;ks=9;
wc=0.05;
ser=[];vc=[];
x=[];y=[];xe=[];ye=[];see=[];
x(1)=0.5;y(1)=-0.6;
w=[];w(1)=0.3;
v=[];v(1)=0.5;
xr=[];yr=[];
p=[];tz=0.1;seo=[];
syms oo a b;
f1=a*b/sqrt(b^2*cos(oo)^2+a^2*sin(oo)^2)*cos(oo);
f2=a*b/sqrt(b^2*cos(oo)^2+a^2*sin(oo)^2)*sin(oo);
f1d=diff(f1,oo,1);f2d=diff(f2,oo,1);f1dd=diff(f1,oo,2);f2dd=diff(f2,oo,2);
r=(f1d^2+f2d^2)^(3/2)/subs(f1d*f2dd-f1dd*f2d);
for i=1:n
    seo(i)=tz*i*wc;
    ser(i)=pi/2+seo(i);
    p(i)=af*bf/sqrt(bf^2*cos(seo(i))^2+af^2*sin(seo(i))^2);
    xr(i)=p(i)*cos(seo(i));yr(i)=p(i)*sin(seo(i));
    oo=seo(i);
    a=1;b=1.5;
    vc(i)=wc*eval(r);
    se(i+1)=se(i)+tz*w(i);
    x(i+1)=x(i)+tz*v(i)*cos(se(i));
y(i+1)=y(i)+tz*v(i)*sin(se(i));
e=[cos(se(i))*(p(i)*cos(seo(i))-x(i))+sin(se(i))*(p(i)*sin(seo(i))-y(i));
    -sin(se(i))*(p(i)*cos(seo(i))-x(i))+cos(se(i))*(p(i)*sin(seo(i))-y(i));
    ser(i)-se(i)];
xe(i)=e(1);ye(i)=e(2);see(i)=e(3);
u=[vc(i)*cos(see(i))+kx*xe(i);wc+vc(i)*ky*ye(i)+vc(i)*ks*sin(see(i))];
w(i+1)=u(2);v(i+1)=u(1);
end
figure;
plot(x,y,'r');
hold on;
plot(xr,yr);title('椭圆跟踪效果图');
figure;plot(t,xr-x(1:1001),'r');title('位姿误差x轴');figure;plot(t,yr-y(1:1001),'g');title('位姿误差y轴');figure;plot(t,see,'b');title('位姿误差角度');