A*算法实现路径规划matlab（下）
A*算法程序、结果分析与思考
2.1 程序目录
1.createMAP.m（总调用程序）
2.AStar.m
3.FindList.m
4.Getpath.m
5.isObstacle.m
6.isopen.m
7.MotionModel.m
8.plot_map.m
9.h.m
2.2 程序
1. createMAP.m（生成地图、包括障碍点、鼠标选取起点、终点以及主要运行程序）
clear all;
clear all;
figure;
MAX_X=50;
MAX_Y=50;
map.XYMAX=50;
p_obstocle = 0.3;%障碍率
O = ones(MAX_X,MAX_Y)*p_obstocle;%生成一个50*50的矩阵，里面都是0.3
MAP = 9999*((rand(MAX_X,MAX_Y))>O)-1;%随机生成50*50的矩阵，里面0到1之间随机，大于0.3的返回1，小于的返回0，再都乘9999，都减一。里面一堆9998和-1
j=0;
x_val = 1;
y_val = 1;
axis([0.5 MAX_X+0.5 0.5 MAX_Y+0.5])%坐标轴范围和纵横比
set(gca,'YTick',0:1:MAX_Y);
set(gca,'XTick',0:1:MAX_X);%设定xy坐标轴显示轴的刻度线
for i = 1:map.XYMAX+3
line([-0.5,map.XYMAX+1.5],[i-1.5,i-1.5]);
end
for j = 1:map.XYMAX+3
line([j-1.5,j-1.5],[-0.5,map.XYMAX+1.5]);
end
hold on; %添加新绘图时候保留原绘图
obstacle=[];
for i=1:MAX_X %遍历map数组，是-1的画红叉
for j=1:MAX_Y
if MAP(i,j) == -1
obstacle=[obstacle;[i j]];
plot(i,j,'rx');%在每一格是障碍物的地方画红色叉
end
end
end
%%地图上选择起始位置
pause(1);
h=msgbox('Please Select the Vehicle initial position using the Left Mouse button');%出现一句话
uiwait(h,5);%出现5秒后消失
if ishandle(h) == 1
delete(h);
end
xlabel('Please Select the Vehicle initial position ','Color','black');
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked反复执行除非点左键
[xval,yval,but]=ginput(1);%录入这个鼠标左键
xval=floor(xval);%点的地方向负无穷方向四舍五入
yval=floor(yval);
end
xStart=xval;%Starting Position
yStart=yval;%Starting Position
MAP(xval,yval) = 0;
plot(xval,yval,'bo');%画个圈
%%地图上选择目标点
pause(1);
h=msgbox('Please Select the Target using the Left Mouse button in the space');
uiwait(h,5);
if ishandle(h) == 1
delete(h);
end
xlabel('Please Select the Target using the Left Mouse button','Color','black');
but = 0;
while (but ~= 1) %Repeat until the Left button is not clicked
[xval,yval,but]=ginput(1);
end
xval = floor(xval);
yval = floor(yval);
xTarget = xval;
yTarget = yval;
MAP(xval,yval) = 9998;
plot(xval,yval,'gd');
text(xval+1,yval+.5,'Target');%标一下
node = [xStart,yStart,xTarget,yTarget];
map.start=[xStart,yStart];
map.goal=[xTarget,yTarget];
save map MAP;
save point node;
path=AStar(obstacle,map)%A*算法
%画出路径
%
if length(path)>=1
plot(path(:,1),path(:,2),'-c','LineWidth',2);hold on;
end
%}
2. AStar.m（A*算法的函数程序，调用方法path=AStar(obstacle,map)要输入obstacle和map，其中obstacle是n行1列的数组，每一行中都是一个点坐标；map是一个结构数组，其中包括属性：map.start（初始点）、map.goal（目标点）、map.XYMAX（坐标轴的范围）。输出一条各坐标点连成的路径和open、closed点，分别为蓝色、绿色和红色）。
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
plot_map(map,obstacle,open,close);
end
%追溯路径
path=GetPath(close,map.start);
3.FindList.m
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
4.GetPath.m
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
5.isobstal.m
function flag = isObstacle( m,obstacle )
%判断节点m是否为障碍点，如果是就返为1，不是就返回0
for io=1:length(obstacle(:,1))
if isequal(obstacle(io,:),m(1:2))
flag=true;
return;
end
end
flag=false;
end
6.isopen.m
function [isopenFlag,Id] = isopen( node,open )
%判断节点是否在open列表中，在open中，isopenFlag = 1,不在open中，isopenFlag = 0 .并反回索引号
isopenFlag = 0;
Id = 0;
%如果open列表为空，则不在open列表中
if isempty(open)
isopenFlag = 0;
else %open列表不为空时
for i = 1:length( open(:,1) )
if isequal( node(1:2) , open(i,1:2) ) %在Openlist中
isopenFlag = 1;
Id = i;
return;
end
end
end
end
7.MotionModel.m
function next = MotionModel()
%当前节点 周围的八个相邻节点 与 当前节点的坐标差值（前两列）
%当前节点 周围的八个相邻节点 与 当前节点的距离值（最后一列）
next = [-1,1,14;...
0,1,10;...
1,1,14;...
-1,0,10;...
1,0,10;...
-1,-1,14;...
0,-1,10;...
1,-1,14];
End
8.plot_map.m（画openlist里面的点为红色，画closedlist里的点为绿色）
function plot_map( map,obstacle,open,close )
plot(close(:,1),close(:,2),'sr','MarkerFaceColor','r');
hold on;
%pause(0.1);
plot(open(:,1),open(:,2),'sg','MarkerFaceColor','g');
hold on;
%pause(0.1);
End
9.h.m（这里写了两种启发函数，想用哪种启发函数就选择哪种）
function hcost = h( m,goal )
%欧几里得启发函数
hcost=10*(sqrt((m(1)-goal(1))^2+(m(2)-goal(2))^2));
%计算启发函数代价值 ，这里采用曼哈顿算法
%hcost =10* abs( m(1)-goal(1) )+10*abs( m(2)-goal(2) );
end
结果分析、优点思考与不足思考
1.由结果可以看出，这里的A* 算法是允许对角寻路的，也有不允许对角寻路的策略。
2.这里目标点和初始点都是自己随机点的，不用每次再重新该改变程序参数，网上更多是自己设定好参数的程序。
3.这里面CreateMAP.m文件程序中，没有考虑初始点以及目标点点到障碍物的处理情况，当点到障碍点时，程序会卡住停止运行。当然这也考虑到，我们在真正的考虑路径规划时，并不会选取障碍物作为初始点和目标点，但我认为，应该要考虑到选取失误的情况。可以在这里进一步优化。
4.这里是对CreateMAP.m文件做了修改的。
5.如果要进一步思考，可以考虑将AStar.m文件修改，不允许对角寻路，或者考虑A* 算法双向寻路，从初始点和目标点同时寻路，或者考虑运用其他估价函数程序。
6.这是本人的某项作业，对A* 算法做了一些总结和应用，代码大部分参考了网上的，然后个人修改了一些，可以讨论学习，望轻喷。
参考
- https://www.pianshen.com/article/8034306603/#4GetBoundarym_233（程序）
- https://blog.csdn.net/autonavi2012/article/details/80923431（原理）
- https://blog.csdn.net/m0_38054145/article/details/81808541（原理）
- http://qiao.github.io/PathFinding.js/visual/（图解）
- https://www.icourse163.org/learn/BIT-1207432808?tid=1207763214#/learn/announce（程序与思想）