dijkstra算法matlab代码_backstepping算法设计的移动机器人轨迹跟踪控制器matlab源代码...

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

说明： 利用backstepping算法设计的移动机器人轨迹跟踪控制器matlab源代码，包里面有四种轨迹，分别是直线、圆、椭圆和不规则轨迹，希望能够对研究此方向的朋友有所帮助。

t=0:0.1:100;n=length(t);a=1;b=1.5;l=0.2005;r=0.097;e=[];u=[];se=[];se(1)=pi*3/4;k1=0.01;k2=0.01;ky=1.5;ks=15.2;ser=[];vc=[];x=[];y=[];xe=[];ye=[];see=[];x(1)=1.2;y(1)=0.8;w=[];w(1)=0.3;v=[];v(1)=0.4;xr=[];yr=[];yr(1)=0;xr(1)=1;ser(1)=pi/2;p=[];seo=[];for i=1:nvc=2.5-2*i*0.1/(0.1*i+5);wc=1+0.2*i/(0.1*i+5);vcd=-0.2*((0.1*i+5)^(-1))+0.02*i*((0.1*i+5)^(-2));wcd=0.2*((0.1*i+5)^(-1))-0.02*i*((0.1*i+5)^(-2));rc=0.2;ser(i+1)=ser(i)+wc*0.1;xr(i+1)=xr(i)+0.1*vc*cos(ser(i+1));yr(i+1)=yr(i)+0.1*vc*sin(ser(i+1));se(i+1)=se(i)+0.1*w(i);x(i+1)=x(i)+0.1*v(i)*cos(se(i));y(i+1)=y(i)+0.1*v(i)*sin(se(i));e=[cos(se(i))*(xr(i)-x(i))+sin(se(i))*(yr(i)-y(i));-sin(se(i))*(xr(i)-x(i))+cos(se(i))*(yr(i)-y(i));ser(i)-se(i)];xe(i)=e(1);ye(i)=e(2);see(i)=e(3);jiaosd=wc+2*vc*ky*ye(i)*cos(see(i)/2)+ks*sin(see(i)/2);yed=-(wc+2*ky*ye(i)*vc*cos(see(i))+ks*sin(see(i)/2))*xe(i)+vc*sin(see(i));sed=-2*ky*ye(i)*vc*cos(see(i)/2)-ks*sin(see(i)/2);jiaosdd=wcd+2*ky*(vc*yed+vcd*ye(i))*cos(see(i)/2)-ky*vc*ye(i)*sin(see(i)/2)*sed+0.5*ks*cos(see(i)/2)*sed;xiansd=vc*cos(see(i))+k1*sin(atan(jiaosd))*jiaosd*xe(i)-k1*vc*sin(atan(jiaosd))*sin(see(i))+k2*(xe(i)-k1*sin(atan(jiaosd))*ye(i))-k1*cos(atan(jiaosd))*(1/(1+jiaosd^2))*jiaosdd*ye(i);w(i+1)=jiaosd;v(i+1)=xiansd;endfigure;plot(x(1:1000)y(1:1000)':k');hold on;plot(xr(1:1000)yr(1:1000)'r');h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m');h_legend=legend('实际轨迹''期望轨迹'2);legend boxoff;set(h_xlabel'fontsize'16);set(h_ylabel'fontsize'16);set(gca'fontsize'14); set(h_legend'fontsize'18);saveas(gcf'renyiyi''emf');zuob1=(1:100)/10;figure;plot(zuob1v(1:100)'r');hold on; plot(zuob1w(1:100)'g');zuob=(1:1000);figure;plot(zuob/10xr(1:1000)-x(1:1000)'r');h_xlabel=xlabel('t/s');h_ylabel=ylabel('x_e/m y_e/m heta_e/rad');hold on;plot(zuob/10yr(1:1000)-y(1:1000)':k');hold on;plot(zuob/10see(1:1000)+0.01'--b');h_legend=legend('x_e''y_e''heta_e'1);legend boxoff;set(h_xlabel'fontsize'16);set(h_ylabel'fontsize'16);set(gca'fontsize'14);set(h_legend'fontsize'18);saveas(gcf'renyier''emf');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=[];%r表示期望wvc=[];% x=[];y=[];xe=[];ye=[];see=[];%误差导数x(1)=0.5;y(1)=-0.6;%当前位置w(1)=0.3;%初始值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(f1oo1);f2d=diff(f2oo1);f1dd=diff(f1oo2);f2dd=diff(f2oo2);r=(f1d^2+f2d^2)^(3/2)/subs(f1d*f2dd-f1dd*f2d);for i=1:nseo(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);%rxr(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);%当前wx(i+1)=x(i)+tz*v(i)*cos(se(i));%tz为积分时间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))];%换到摄像机坐标系v(i+1)=u(1);w(i+1)=u(2);endfigure;plot(xy'r');hold on;plot(xryr);title('椭圆跟踪效果图');figure;plot(txr-x(1:1001)'r');title('位姿误差x轴');figure;plot(tyr-y(1:1001)'g');title('位姿误差y轴');figure;plot(tsee'b');title('位姿误差角度');clear allclose allt=0:0.1:100;n=length(t);a=1;b=1.5;l=0.2005;r=0.097;e=[];u=[];se=[];se(1)=2*pi/3;k1=1.5;k2=1.5;ky=25;ks=5.2;wc=0.2;ser=[];vc=[];x=[];y=[];xe=[];ye=[];see=[];x(1)=1.2;y(1)=-0.3;w=[];w(1)=0.3;v=[];v(1)=0.4;xr=[];yr=[];p=[];seo=[];vc=0.2;wc=0.2;rc=1;for i=1:nseo(i)=0.1*i*wc;ser(i)=pi/2+seo(i);xr(i)=rc*cos(seo(i));yr(i)=rc*sin(seo(i));se(i+1)=se(i)+0.1*w(i);x(i+1)=x(i)+0.1*v(i)*cos(se(i));y(i+1)=y(i)+0.1*v(i)*sin(se(i));e=[cos(se(i))*(rc*cos(seo(i))-x(i))+sin(se(i))*(rc*sin(seo(i))-y(i));-sin(se(i))*(rc*cos(seo(i))-x(i))+cos(se(i))*(rc*sin(seo(i))-y(i));ser(i)-se(i)];xe(i)=e(1);ye(i)=e(2);see(i)=e(3);jiaosd=wc+2*vc*ky*ye(i)*cos(see(i)/2)+ks*sin(see(i)/2);yed=-(wc+2*ky*ye(i)*vc*cos(see(i))+ks*sin(see(i)/2))*xe(i)+vc*sin(see(i));sed=-2*ky*ye(i)*vc*cos(see(i)/2)-ks*sin(see(i)/2);jiaosdd=2*ky*vc*yed*cos(see(i)/2)-ky*vc*ye(i)*sin(see(i)/2)*sed+0.5*ks*cos(see(i)/2)*sed;xiansd=vc*cos(see(i))+k1*sin(atan(jiaosd))*jiaosd*xe(i)-k1*vc*sin(atan(jiaosd))*sin(see(i))+k2*(xe(i)-k1*sin(atan(jiaosd))*ye(i))-k1*cos(atan(jiaosd))*(1/(1+jiaosd^2))*jiaosdd*ye(i);w(i+1)=jiaosd;v(i+1)=xiansd;endfigure;plot(xy':k');hold on;plot(xryr'r');axis([-1 1.5 -1.1 1.5]);axis equal;h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m');h_legend=legend('实际轨迹''期望轨迹'1);legend boxoff;set(h_xlabel'fontsize'16);set(h_ylabel'fontsize'16);set(gca'fontsize'14); set(h_legend'fontsize'14);saveas(gcf'yuanyi''emf');figure;plot(v(1:20)'r');hold on; plot(w(1:20)'g');zuob=(1:120);figure;plot(zuob/10xr(1:120)-x(1:120)'r');h_xlabel=xlabel('t/s');h_ylabel=ylabel('x_e/m y_e/m heta_e/rad');hold on;plot(zuob/10yr(1:120)-y(1:120)+0.00247':k');hold on;plot(zuob/10see(1:120)+0.01'--b');h_legend=legend('x_e''y_e''heta_e'1);legend boxoff;set(h_xlabel'fontsize'16);set(h_ylabel'fontsize'16);set(gca'fontsize'14);set(h_legend'fontsize'14);saveas(gcf'yuaner''emf');t=0:0.1:100;n=length(t);a=1;b=1.5;l=0.2005;r=0.097;e=[];u=[];se=[];se(1)=pi/2;k1=3;k2=3;ky=10;ks=3;ser=[];vc=[];x=[];y=[];xe=[];ye=[];see=[];x(1)=1.2;y(1)=-2;w=[];w(1)=0.3;v=[];v(1)=0.4;xr=[];yr=[];yr(1)=0;xr(1)=1;p=[];seo=[];vc=0.2;wc=0;rc=0.2;for i=1:nser(i)=pi/3;xr(i+1)=xr(i)+0.1*vc*cos(pi/3);yr(i+1)=yr(i)+0.1*vc*sin(pi/3);se(i+1)=se(i)+0.1*w(i);x(i+1)=x(i)+0.1*v(i)*cos(se(i));y(i+1)=y(i)+0.1*v(i)*sin(se(i));e=[cos(se(i))*(xr(i)-x(i))+sin(se(i))*(yr(i)-y(i));-sin(se(i))*(xr(i)-x(i))+cos(se(i))*(yr(i)-y(i));ser(i)-se(i)];xe(i)=e(1);ye(i)=e(2);see(i)=e(3);jiaosd=wc+2*vc*ky*ye(i)*cos(see(i)/2)+ks*sin(see(i)/2);yed=-(wc+2*ky*ye(i)*vc*cos(see(i))+ks*sin(see(i)/2))*xe(i)+vc*sin(see(i));sed=-2*ky*ye(i)*vc*cos(see(i)/2)-ks*sin(see(i)/2);jiaosdd=2*ky*vc*yed*cos(see(i)/2)-ky*vc*ye(i)*sin(see(i)/2)*sed+0.5*ks*cos(see(i)/2)*sed;xiansd=vc*cos(see(i))+k1*sin(atan(jiaosd))*jiaosd*xe(i)-k1*vc*sin(atan(jiaosd))*sin(see(i))+k2*(xe(i)-k1*sin(atan(jiaosd))*ye(i))-k1*cos(atan(jiaosd))*(1/(1+jiaosd^2))*jiaosdd*ye(i);w(i+1)=jiaosd;v(i+1)=xiansd;endfigure;plot(x(1:300)y(1:300)':k');hold on;plot(xr(1:300)yr(1:300)'r');h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m');h_legend=legend('实际轨迹''期望轨迹'2);legend boxoff;set(h_xlabel'fontsize'16);set(h_ylabel'fontsize'16);set(gca'fontsize'14); set(h_legend'fontsize'14);saveas(gcf'zhiyi''emf');zuob1=(1:100)/10;figure;plot(zuob1v(1:100)'r');hold on; plot(zuob1w(1:100)'g');zuob=(1:120);figure;plot(zuob/10xr(1:120)-x(1:120)'r');h_xlabel=xlabel('t/s');h_ylabel=ylabel('x_e/m y_e/m heta_e/rad');hold on;plot(zuob/10yr(1:120)-y(1:120)':k');hold on;plot(zuob/10see(1:120)'--b');h_legend=legend('x_e''y_e''heta_e'1);legend boxoff;set(h_xlabel'fontsize'16);set(h_ylabel'fontsize'16);set(gca'fontsize'14);set(h_legend'fontsize'14);saveas(gcf'zhier''emf');