matlab 模拟小车轨迹

clear %清除工作区变量 clc %清除命令行历史窗口 close all %尺寸说明见《8字组国赛报告》的附录 %输出参数的尺寸单位都为m %参数输入 n=400; %高度的细分程度， %重物下降的高度h h=linspace(0,0.1,n);%这里把高度0.1m分成n份 %驱动轴传动比ii % ii=18/68;%原传动比，分子和分母分别对应主动动轮的齿数，模为0.5mm % ii=26/60; ii=42/130; %后轮半径 % R=0.150/2; R=0.110/2; %驱动轮A与转向轮横向偏距a1 a1=0.0465; %驱动轮B与转向轮横向偏距a2 a2=0.0465; %曲柄半径r1, % r1=0.03;%原曲柄半径 r1=0.0255; %绳轮半径r2 r2=0.004; %驱动轴与转向轮的距离d d=0.113; %转向杆的长c c=0.0358; %连杆长l % l=0.1107; l=0.071; %转向轮的初始偏角p0 p0=-9.43/180*pi; %驱动轴转过的角度sd1 sd1=h/r2; %空间曲柄连杆机构的模型，输入的角度为sd1，输出的角度为af, %原理见论文《8字组国赛报告》 x0=sqrt(l^2-r1^2); C=-(r1^2-l^2+2*c^2+x0^2-2*x0*r1*cos(sd1)); A=2*r1*c.*cos(sd1)-2*x0*c; B=2*c^2; %转向轮转过的角度af af=asin(C./sqrt(A.^2+B^2))-acos(A./sqrt(A.^2+B^2))+pi+p0; %计算三个轮子的轨迹，原理见《8字组国赛报告》 format long %改变数值显示的位数 %轨迹曲率半径rou rou=d./(tan(af))+a1; %驱动轮转过的角度sd2 sd2=sd1/ii; %小车行驶的路程s s=sd2*R; ds=s(2)-s(1); %小车的速度ds,即路程的差分，相当于连续轨迹的微分 dbd=ds./(rou); %小车行驶过程中的方位角的变化率dbd bd=cumsum(dbd);%小车方位角bd,对方位角的变化dbd进行累加，即积分 dx=-ds*sin(bd);%小车x方向上的位移dx dy=ds*cos(bd); %小车y方向上的位移y x=cumsum(dx); %小车x方向的位移x y=cumsum(dy); %小车y方向的位移y a=-pi/90; %小车初始方位角a，即小车发车时的角度 %小车最终x方向的位移x x=x*cos(a)-y*sin(a)+0.313; %小车最终y方向的位移y y=x*sin(a)+y*cos(a)+0.005; %左轮的轨迹 xb=x-(a1+a2).*cos(bd); yb=y-(a1+a2).*sin(bd); %右轮的轨迹 % xc=x-(a1-0.01)*cos(bd)-(d-0.1)*sin(bd); % yc=y-(a1-0.01)*sin(bd)+(d-0.1)*cos(bd); xc=x-(a1)*cos(bd)-(d)*sin(bd); yc=y-(a1)*sin(bd)+(d)*cos(bd); %画出三个轮的轨迹 % plot(x*1000,y*1000,'k',xb*1000,yb*1000,'k',xc*1000,yc*1000,'k');%这里的单位为mm % grid on %给图加上网格线 % set(gca,'DataAspectRatio',[1 1 1])%令x，y坐标轴的比例为一比一 % set(gca,'xtick',-200:100:400);%x轴网格线的具体范围 % set(gca,'ytick',-700:100:740);%y轴网格线的具体范围 x=x*1000; y=y*1000; xb=xb*1000; yb=yb*1000; xc=xc*1000; yc=yc*1000; figure set(gcf,'Unit','normalized','Position',[0,0,1,0.9]) %展示动态曲线 h2 = plot(0,0); hold all h = plot(0,0); h1 = plot(0,0); grid on axis equal xlabel('X坐标/mm'); ylabel('Y坐标/mm'); title('小车轨迹动态曲线'); % axis([-60000 60000 -60000 60000 -60000 60000]) for m = 1:numel(x) set(h2,'color','blue'); set(h,'color','black'); set(h1,'color','black'); % set(h,'XData',x(1:m),'XData',xb(1:m),'YData',y(1:m),'YData',yb(1:m)); set(h2,'XData',yc(1:m),'YData',xc(1:m)); set(h,'XData',y(1:m),'YData',x(1:m)); set(h1,'XData',yb(1:m),'YData',xb(1:m)); drawnow;%刷新屏幕 pause(0.02); end legend('前轮','左后轮','右后轮');