Test_人工势场动态_动态障碍物_动态人工势场_路径规划动态障碍物_人工势场法

close all clear all clc format short; global dongP; global h1; dongP=[80,0,10]; startXY=[0 0]; endXY=[200 200];%起点终点位置 nowXY=startXY; searchR=4; %探寻半径 circleCencerR=10; %静态障碍物圆半径 circleCencer=[40,50]; %确定静态障碍物中心 r=rectangle('Position',[circleCencer(1)-circleCencerR,circleCencer(2)-circleCencerR,2*circleCencerR,2*circleCencerR],... 'Curvature',[1,1]); %静态障碍物 figure(1) hold on r.FaceColor = [0 0 0]; %障碍物颜色 axis([0 200 0 200]); %坐标系大小 Rout=[startXY]; %路径点集合 Routsize=0; %% 开始寻找起始点的8位图，寻找路径 while(sqrt((endXY(2)-nowXY(2))^2+(endXY(1)-nowXY(1))^2)>=2*searchR)%当没有到达终点 Pot8=wei8(nowXY,endXY,searchR);%当前点的8方位点 longMin=Inf; %下一个位置点到终点的距离 for i=1:8 %对于每一个位置点 if(~isInBr(Pot8(i,:),circleCencer,circleCencerR))%不在障碍物内 Pot18=wei8(Pot8(i,:),endXY,searchR);%下一次的8方位点 for j=1:8 if(~isInBr(Pot18(j,:),circleCencer,circleCencerR))%如果不在静动障碍物内 Long=sqrt((endXY(2)-Pot18(j,2))^2+(endXY(1)-Pot18(j,1))^2);%下一点到终点距离 if Long<longMin%且距离比之前的小 longMin=Long; preRout=Pot8(i,:);%保存前一个路径点 end end end end end nowXY=preRout; %更新新的路径点 Rout=[Rout;preRout]; Routsize=Routsize+searchR;%路径长度 circle(nowXY(1),nowXY(2),3); dongP=dongtai(3,12,80,0,0,1);%添加动态障碍物 pause(0.1); delete(h1); end %% 画图 Rout=[Rout;endXY]; Routsize=Routsize+sqrt((endXY(2)-preRout(2))^2+(endXY(1)-preRout(1))^2); circle(endXY(1),endXY(2),3); plotRout(Rout);