【路径规划】基于matlab A_star算法机器人动态避障【含Matlab源码 2571期】.zip

%% 基于a星算法的机器人动态避障处理 clc clear all close all %% 清空变量 Xminglobal=0; Xmaxglobal=200; Yminglobal=0; Ymaxglobal=200; %% 障碍物 statobs1=[20 30 10 20]; rectangle('Position',statobs1,'FaceColor',[0.4 0.6 0.7]); hold on axis([0 200 0 200]) statobs2=[50 10 10 20]; rectangle('Position',statobs2,'FaceColor',[0.4 0.6 0.7]); hold on axis([0 200 0 200]) statobs3=[70 40 10 20]; rectangle('Position',statobs3,'FaceColor',[0.1 0.3 0.7]); hold on axis([0 200 0 200]) statobs4=[60 80 10 10]; rectangle('Position',statobs4,'FaceColor',[0.1 0.3 0.7]); hold on axis([0 200 0 200]) statobs5=[160 180 10 20]; rectangle('Position',statobs5,'FaceColor',[1 0 0]); hold on axis([0 200 0 200]) statobs6=[100 130 10 20]; rectangle('Position',statobs6,'FaceColor',[1 0 0]); hold on axis([0 200 0 200]) statobs7=[100 70 10 10]; rectangle('Position',statobs7,'FaceColor',[1 0 0]); hold on axis([0 200 0 200]) statobs8=[140 120 10 10]; rectangle('Position',statobs8,'FaceColor',[0.1 0.3 0.7]); hold on axis([0 200 0 200]) START_X=10; START_Y=90; GOAL_X=180; GOAL_Y=30; k=plot(START_X,START_Y,'bx','MarkerSize',10); set(k,'XData',START_X,'YData',START_Y); u=plot(GOAL_X,GOAL_Y,'bx','MarkerSize',10); set(u,'XData',GOAL_X,'YData',GOAL_Y); grid_len=10; MAP_IDX=1; for i=0:grid_len:Xmaxglobal for j=0:grid_len:Ymaxglobal MAP(MAP_IDX, 1)=i; MAP(MAP_IDX, 2)=j; MAP(MAP_IDX, 3)=2; MAP_IDX=MAP_IDX+1; end end MAP_IDX=MAP_IDX-1; OPEN=[]; CLOSED=[]; CLOSED_IDX=1; OPEN_IDX=1; OBS_COORD_LIST=[]; OBS_IDX=1; OBS_COORD_LIST(OBS_IDX, 1)=statobs1(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs1(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs1(1)+statobs1(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs1(2)+statobs1(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs1(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs1(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs1(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs1(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs1(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs1(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs1(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs1(2)+20;% 20,40 OBS_IDX=OBS_IDX+1; %obstacle 2 OBS_COORD_LIST(OBS_IDX, 1)=statobs2(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs2(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs2(1)+statobs2(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs2(2)+statobs2(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs2(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs2(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs2(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs2(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs2(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs2(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs2(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs2(2)+20;% 20,40 OBS_IDX=OBS_IDX+1; %obstacle 3 OBS_COORD_LIST(OBS_IDX, 1)=statobs3(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs3(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs3(1)+statobs3(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs3(2)+statobs3(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs3(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs3(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs3(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs3(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs3(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs3(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs3(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs3(2)+20;% 20,40 OBS_IDX=OBS_IDX+1; %obstacle 4 OBS_COORD_LIST(OBS_IDX, 1)=statobs4(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs4(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs4(1)+statobs4(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs4(2)+statobs4(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs4(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs4(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs4(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs4(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs4(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs4(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs4(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs4(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; %obstacle 5 OBS_COORD_LIST(OBS_IDX, 1)=statobs5(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs5(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs5(1)+statobs5(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs5(2)+statobs5(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs5(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs5(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs5(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs5(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs5(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs5(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs5(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs5(2)+20;% 20,40 OBS_IDX=OBS_IDX+1; %obstacle 6 OBS_COORD_LIST(OBS_IDX, 1)=statobs6(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs6(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs6(1)+statobs6(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs6(2)+statobs6(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs6(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs6(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs6(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs6(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs6(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs6(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs6(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs6(2)+20;% 20,40 OBS_IDX=OBS_IDX+1; %obstacle 7 OBS_COORD_LIST(OBS_IDX, 1)=statobs7(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs7(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs7(1)+statobs7(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs7(2)+statobs7(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs7(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs7(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs7(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs7(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs7(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs7(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs7(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs7(2)+10;% 20,40 %obstacle 8 OBS_COORD_LIST(OBS_IDX, 1)=statobs8(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs8(2);% 20,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs8(1)+statobs8(3);OBS_COORD_LIST(OBS_IDX, 2)=statobs8(2)+statobs8(4);% 30,50 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs8(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs8(2);% 30,30 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs8(1)+10;OBS_COORD_LIST(OBS_IDX, 2)=statobs8(2)+10;% 30,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs8(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs8(2)+10;% 20,40 OBS_IDX=OBS_IDX+1; OBS_COORD_LIST(OBS_IDX, 1)=statobs8(1);OBS_COORD_LIST(OBS_IDX, 2)=statobs8(2)+10;% 20,40 Optimal_path=a_star(OPEN, CLOSED, OPEN_IDX, CLOSED_IDX, OBS_COORD_LIST, OBS_IDX, MAP, MAP_IDX, START_X, START_Y, GOAL_X, GOAL_Y, Xminglobal, Xmaxglobal, Yminglobal, Ymaxglobal); Optimal_path=flipud(Optimal_path); path=[]; %state lattice primitive decision curr_heading=0; TurnRadius = 2; PathStep = -1; t1=0; t2=5; v1=4; v2=4; Optimal_path(1,3)=0; Optimal_path(1,4)=0; manu='x'; %dynamic obs path obsx0=130; obsy0=120; obsvx0= 0; obsvy0=-90/60; dt=0.1; DYN_IDX=1; for tobs=0:dt:70 obsx0=obsx0 + obsvx0*dt; obsy0=obsy0 + obsvy0*dt; %disp(obsy0); dyn_obs_path(DYN_IDX,1)= obsx0; dyn_obs_path(DYN_IDX,2)= obsy0; dyn_obs_path(DYN_IDX,3)= tobs; DYN_IDX=DYN_IDX+1; end for OPT_IDX=1:size(Optimal_path,1)-1 Optimal_path(OPT_IDX,4)=t1; disp(OPT_IDX); disp('!!!!!!!!!!!!!!!'); if OPT_IDX<size(Optimal_path,1)-1 [manu, next_manu]=find_direction(Optimal_path(OPT_IDX,1),Optimal_path(OPT_IDX,2),Optimal_path(OPT_IDX+1,1),Optimal_path(OPT_IDX+1,2),Optimal_path(OPT_IDX+2,1),Optimal_path(OPT_IDX+2,2)); d