matlab模拟人工势场法.rar

close all; clc; clear; f=5; n=360/f; j=1; %% uav_num=3; % startpoints=[52 12;10 18;15 50];%起点 startpoints=[10 2;2 30;2 20];%起点 % uav_num=1; % startpoints=[10 10];%起点 goal1=[35 30];%终点 goal2=[10 10];%终点 goal3=[30 35];%终点 uuv_r=0.5;%机器人半径 step=0.2;%机器人速度变化 %% obs_r=5;%障碍物半径 T=0; %% uavs=[]; obstacles=[]; C=[]; D=[]; for i=1:uav_num uavs(1,1+2*(i-1))=startpoints(i,1); uavs(1,2*i)=startpoints(i,2); fillcircle(startpoints(i,1),startpoints(i,2),uuv_r,i); %填充颜色 end obstacles=[ 10,20;20,10;20,20; 20,30;30,20;5,10];%障碍物 for i=1:size(obstacles,1) fillcircle_obs(obstacles(i,1),obstacles(i,2),1);%静态障碍，半径为5 % rectangle('Position',[obstacle_origin(1,i),obstacle_origin(2,i),5,5],'Curvature',[1,1],'FaceColor','r'); end sim_time=2; core=10000000; while core>5*step for uav_th=1:uav_num uav=[uavs(sim_time-1,2*uav_th-1),uavs(sim_time-1,2*uav_th)]; if uav_th==1 APF_force=comput_goal_att(uav,goal1)+... comput_obs_rep(uav,goal1,obstacles)+... comput_uav_rep(uav,uavs(sim_time-1,:)); end if uav_th==2 APF_force=comput_goal_att(uav,goal2)+... comput_obs_rep(uav,goal2,obstacles)+... comput_uav_rep(uav,uavs(sim_time-1,:)); end if uav_th==3 APF_force=comput_goal_att(uav,goal3)+... comput_obs_rep(uav,goal3,obstacles)+... comput_uav_rep(uav,uavs(sim_time-1,:)); end if sim_time==2 angle=pi/4; else if APF_force(1)>0 if APF_force(2)>0 angle=acos(abs(APF_force(1))/norm(APF_force)); else angle=-acos(abs(APF_force(1))/norm(APF_force)); end else if APF_force(2)>0 angle=pi-acos(abs(APF_force(1))/norm(APF_force)); else angle=-pi+acos(abs(APF_force(1))/norm(APF_force)); end end end uavs(sim_time,2*uav_th-1)=uav(1)+step*cos(angle); uavs(sim_time,2*uav_th)=uav(2)+step*sin(angle); end for uav_th=1:uav_num fillcircle(uavs(sim_time,2*uav_th-1),uavs(sim_time,2*uav_th),uuv_r,uav_th) %填充颜色 pause(0.0001); end sim_time=sim_time+1; core=sum(abs(uavs(sim_time-1,1:2)-goal1)+abs(uavs(sim_time-1,3:4)-goal2)+abs(uavs(sim_time-1,5:6)-goal3)); end