AGV小车 matlab仿真源代码

clc;clear; %%仿真步长参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% dt=0.001;% 仿真时间间隔 单位米 ds=0.001;% 地图采样点间距 单位米 %%车身及电机参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% m=1;%车重 单位 千克 p0=2;%车辆启动所需功率 单位 瓦 power=2.5;%启动时刻电机总功率，后续变化由控制程序决定。 kf=0.5;%摩擦系数 g=9.8; %重力系数 u=0.8;%电机效率 L=0.3;%车长 单位米 W=0.12;%车宽 单位米 v_c=0;%车速 单位 米/秒 x0=0;y0=0;%车辆起始点 a0=0;%当前轮转角 初始为0 后续由程序和舵机机械性能共同决定。 a=0; %下一刻前轮转角 初始为0 后续由程序和舵机机械性能共同决定。 wheel_angle=0;%初始时刻程序输出目标转角。 turn_force=pi;%转向能力设置 单位 角度/秒 full_angle=pi/4;%前轮最大转向角，相对车身中轴线。 CA0=0;%起始车身角度，相对于x轴正方向 %%摄像头参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% hc=0.2;%摄像头放置高度 单位米 near_point=0.2;%摄像头图像近点距离 单位米 far_point=1.2;%摄像头图像远点距离 单位米 camera_angle=pi/2;%摄像头的角度 pixel_w=60;%摄像头像素 宽 pixel_h=80;%摄像头像素 高 %%摄像头参数计算%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [dh,dw]=cal_camera(hc,far_point,near_point,camera_angle,pixel_w,pixel_h); %%程序性能（仿真图像帧数）设置%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FPS=60;%帧率 fps_time=floor((1/FPS)*1000)/1000;%每帧图像之间的时间间隔 %%初始化赛道程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% load('map.mat');%下载地图 [track_x,track_y]=creat_track(ds);%%赛道中心线轨迹 image=get_image(x0,y0,CA0,dw,dh,IMAGE,pixel_h,pixel_w);%初始化摄像头图像 figure(1); imh=imshow(image); set(imh, 'erasemode', 'none'); gif_num=1;%图像序列号（及第几帧） i=1; for time=0:dt:300 [x(i),y(i)]=run(CA0,v_c,x0,y0,dt); x0=x(i); y0=y(i); if(mod(time*1000,fps_time*1000)==0)%%%由设置的程序性能（图像帧数）判断是否进入控制程序 [image,black_pixel_num]=get_image(x0,y0,CA0,dw,dh,IMAGE,pixel_h,pixel_w); [power,wheel_angle]=control(v_c,image); set(imh, 'cdata', image); %%%%%%%%%%不需要生成GIF，将以下代码屏蔽。此代码很影响程序执行速度%%%%%%%%%%%%%%%%%%%%%%% % if(mod(time*1000,100)==0) % picname=['./picture/',num2str(gif_num),'.fig']; % saveas(gcf,picname); % gif_num=gif_num+1; % end %%%%%%%%%%不需要生成GIF，将以上代码屏蔽。此代码很影响程序执行速度%%%%%%%%%%%%%%%%%%%%%%% end if(black_pixel_num>4500) %%%判断赛车是否冲出赛道 break; end a=frontwheel_angle(wheel_angle,full_angle,turn_force,a0,dt); v(i)=speed_model(v_c,power,dt,m,p0,kf,g,u);% current speed; power ; dt. CA(i)=turn_model(CA0,v_c,a,dt,L,W);% current body angle;current speed;wheel angle;dt v_c=v(i); CA0=CA(i); a0=a; i=i+1; pause(0.001) end figure(2); time=0:dt:100; axis([-3 3 -1 4]); hold on; grid on; plot(track_x,track_y); hold on; plot(x,y);