A_路径规划_路径规划栅格A_路径规划matlab_A路径规划_A算法_

clear; clc; disp('A Star Path Planing start!!') map.XYMAX=20; %%代表我们要画一个地图的长和宽 map.start=[1,1]; %起始点 注意必须在地图范围内 map.goal=[18,18]; %目标点 注意必须在地图范围内 obstacle=GetBoundary(map);%得到边界数据 nObstacle=0;%在地图中随机加入XX个障碍物 obstacle=GetObstacle(nObstacle,obstacle,map);%障碍物和边界坐标 obstacle = [obstacle;4,1; 4,2; 4,3; 4,4; 3,4 ;2,4;9,12];%全封死的情况，是没有路的 %obstacle = [obstacle;1,2;2,1;2,2];%此也为全封死的情况，也没有路的 %obstacle = [obstacle;1,3;2,3;3,3;3,2;3,1];%此也为全封死情况，也没有路的 % %load('obstacle1.mat'); %画出网格线 PlotGrid(map); hold on; %画出障碍点 FillPlot(obstacle,'k'); path=AStar(obstacle,map)%A*算法 %画出路径 % if length(path)>=1 plot(path(:,1),path(:,2),'-c','LineWidth',5);hold on; end %} function path=AStar(obstacle,map) %{ Astar算法思路 1.将起始点放在Openlist中 2.重复以下过程： 首先判断是否到达目标点，或无路径 >>如果终点已加入到Openlist中，则已找到路径（此时起始点就是目标点，无需再找） >>Openlist为空，无路径 a.按照Openlist中的第三列（代价函数F）进行排序，查找F值最小的节点 b.把这个F值最小的节点移到Closelist中作为 当前节点 c.对当前节点周围的8个相邻节点： >>如果它不可达，忽略它 >>如果它在Closelist中，忽略它 >>如果它不在Openlist中，加放Openlist，并把当前节点设置为它的父节点，记录该节点的F值 >>如果它已经在Openlist中，检查经当前节点到达那里是否更好(用G或F值判断)， >如果更好，则将当前节点设置为其父节点，并更新F,G值；如果不好，则不作处理 3.保存路径 %} %用于存储路径 path=[]; %OpenList open=[]; %CloseList close=[]; %findFlag用于判断while循环是否结束 findFlag=false; %================1.将起始点放在Openlist中====================== %open变量每一行 [节点坐标，代价值F=G+H,代价值G,父节点坐标] open =[map.start(1), map.start(2) , 0+h(map.start,map.goal) , 0 , map.start(1) , map.start(2)]; %更新状态--下一步的八个点 next=MotionModel(); %=======================2.重复以下过程============================== while ~findFlag %--------------------首先判断是否达到目标点，或无路径----- if isempty(open(:,1)) disp('No path to goal!!'); return; end %判断目标点是否出现在open列表中 [isopenFlag,Id]=isopen(map.goal,open); if isopenFlag disp('Find Goal!!'); close = [open(Id,:);close] findFlag=true; break; end %------------------a.按照Openlist中的第三列（代价函数F）进行排序，查找F值最小的节点 [Y,I] = sort(open(:,3)); %对OpenList中第三列排序 open=open(I,:);%open中第一行节点是F值最小的 %------------------b.将F值最小的节点(即open中第一行节点)，放到close第一行(close是不断积压的)，作为当前节点 close = [open(1,:);close]; current = open(1,:); open(1,:)=[];%因为已经从open中移除了，所以第一列需要为空 %--------------------c.对当前节点周围的8个相邻节点，算法的主体：------------------------ for in=1:length(next(:,1)) %获得相邻节点的坐标,代价值F先等于0,代价值G先等于0 ,后面两个值是其父节点的坐标值，暂定为零(因为暂时还无法判断其父节点坐标是多少) m=[current(1,1)+next(in,1) , current(1,2)+next(in,2) , 0 , 0 , 0 ,0]; m(4)=current(1,4)+next(in,3); % m(4) 相邻节点G值 m(3)=m(4)+h(m(1:2),map.goal);% m(3) 相邻节点F值 %>>如果它不可达，忽略它，处理下一个相邻节点 (注意，obstacle这个数组中是包括边界的) if isObstacle(m,obstacle) continue; end %flag == 1：相邻节点 在Closelist中 targetInd = close中行号 %flag == 2：相邻节点不在Openlist中 targetInd = [] %flag == 3：相邻节点 在Openlist中 targetInd = open中行号 [flag,targetInd]=FindList(m,open,close); %>>如果它在Closelist中，忽略此相邻节点 if flag==1 continue; %>>如果它不在Openlist中，加入Openlist,并把当前节点设置为它的父节点 elseif flag==2 m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点 open = [open;m];%将此相邻节点加放openlist中 %>>剩下的情况就是它在Openlist中，检查由当前节点到相邻节点是否更好，如果更好则将当前节点设置为其父节点，并更新F,G值；否则不操作 else %由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数F值) if m(3) < open(targetInd,3) %更好，则将此相邻节点的父节点设置为当前节点，否则不作处理 m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点 open(targetInd,:) = m;%将此相邻节点在Openlist中的数据更新 end end %下面的end是判断八个相邻节点的for循环的end end %=====绘制====== PlotGrid(map); hold on; pause(0.01); %绘制节点close和open节点 FillPlot(close,'r'); hold on; FillPlot(open,'g') hold on; end %追溯路径 path=GetPath(close,map.start); end function [flag,targetInd]=FindList(m,open,close) %{ 函数功能： 如果相邻节点(m存储其信息) 已经在Closelist中，则flag = 1 targetInd = 其所在close的行数，用来定位 如果相邻节点(m存储其信息) 不在Openlist 中，则flag = 2 targetInd = [] 如果相邻节点(m存储其信息) 已经在Openlist 中，则flag = 3 targetInd = 其所在open的行数，用来定位 %} %如果openlist为空，则一定不在openlist中 if isempty(open) flag = 2; targetInd = []; else %open不为空时，需要检查是否在openlist中 %遍历openlist，检查是否在openlist中 for io = 1:length(open(:,1)) if isequal( m(1:2) , open(io,1:2) ) %在Openlist中 flag = 3; targetInd = io; return; else %不在Openlist中 flag = 2; targetInd = []; end end end %如果能到这一步，说明： 一定不在Openlist中 那么需要判断是否在closelist中 %遍历Closelist（注意closelist不可能为空） for ic = 1:length(close(:,1)) if isequal( m(1:2) , close(ic,1:2) ) %在Closelist中 flag = 1; targetInd = ic; return;%在Closelist中直接return end end %{ 以下代码用于测试此函数的正确性： open = [ 1,1 ; 2,2; 3,3; 4,4; 5,5; 6,6; 7,7; 8,8; 9,9]; %还有一种情况：open列表为空 close = [1,2 ; 2,3; 3,4; 4,5; 5,6; 6,7; 7,8]; m1 = [5,6]; %在close中 此时flag应为1 targetInd 应为 5 m2 = [0,0]; %不在open中， 此时flag应为2 targetInd 应为空[] m3 = [3,3]; %在open中 此时flag应为3 targetInd 应为 3 [flag,targetInd] = FindList(m1,open,close) %flag = 1 targetInd = 5 [flag,targetInd] = FindList(m2,open,close) %flag = 2 targetInd = [] [flag,targetInd] = FindList(m3,open,close) %flag = 3 targetInd = 3 %} end function boundary=GetBoundary(map) %获得地图的边界的坐标 boundary=[]; for i1=0:(map.XYMAX+1) boundary=[boundary;[0 i1]]; end for i2=0:(map.XYMAX+1) boundary=[boundary;[i2 0]]; end for i3=0:(map.XYMAX+1) boundary=[boundary;[map.XYMAX+1 i3]]; end for i4=0:(map.XYMAX+1) boundary=[boundary;[i4 map.XYMAX+1]]; end end function obstacle=GetObstacle(nob,obstacle,map) %生成障碍点的坐标 ob=round(rand([nob,2])*map.XYMAX); %生成的障碍点有可能是和start点和goal点坐标重合的，需要删除，removeInd为重合点的数组索引index removeInd=[]; %遍历ob数组，检查哪些坐标与start和goal重合，并将其索引存在removeInd中 for io=1:length(ob(:,1)) if(isequal(ob(io,:),map.start) || isequal(ob(io,:),map.goal)) removeInd=[removeInd;io]; end end %将重复的节点置空，去掉 ob(removeInd,:)=[]; %将ob障碍点加入到obstacle中（obstacle中已经包括边界节点的坐标了，这里还要再加上内部障碍点的坐标） obstacle=[obstacle;ob]; end function path=GetPath(close,start) ind=1; path=[]; while 1 path=[path; close(ind,1:2)]; if isequal(close(ind,1:2),start) break; end for io=1:length(close(:,1)) if isequal(close(io,1:2),close(ind,5:6)) ind=io; break; end end end end function hcost = h( m,goal ) %计算�