二、A_star算法简介
1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.
2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.
如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:
图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.
图3 A Star算法流程
三、部分源代码
%% A* demo
clc;
clear;
% pause(3);
%% init
n=30;
starNum=1;
% starNum = randi(n*n,[1,1]);
goalNum=172;
goalNum = randi(n*n,[1,1]);
banper=0.25;
%% map init
figure('name','A*','NumberTitle','off','MenuBar','none');
global point
for ii=1:n*n
point(ii).num = ii;
point(ii).father=[];
point(ii).Gcost=[];
point(ii).Hcost=[];
end
%% banper
banList=[randi(n*n,[1,floor(banper*n*n)])];
load banList
banList(find(banList==goalNum))=[];
for jj = 1:length(banList)
if banList(jj)~=goalNum || banList(jj)~=starNum
point(banList(jj)).Gcost = Inf;
end
end
point(starNum).Gcost=0;
point(starNum).father = point(starNum).num;
point(starNum).Hcost=getHcost(point(starNum),point(goalNum),n);
%% A*core
openList = [];
closeList = [];
closeListNum=[];
openListNum=[];
openList = [openList,point(starNum)];
while length(openList)
% opneList
costList = getCost(openList,point(goalNum),n);
currentPoint = openList(find(costList==min(costList),1));
openList(find(min(costList)==costList,1))=[];
closeList = [closeList,currentPoint];
neighbourNum = getNeighbour(currentPoint,n);
closeListNum = cat(1,closeList.num);
openListNum = cat(1,openList.num);
for ii = 1:length(neighbourNum)
if neighbourNum(ii)==point(goalNum).num
point(neighbourNum(ii)).father = currentPoint.num;
point(goalNum).father = currentPoint.num;
disp('ok')
routPlot(goalNum,n);
return;
end
log1=0;
try
tmp=point(neighbourNum(ii)).Gcost;
if tmp ==inf
log1 = 1;
end
catch
log1=0;
end
if log1 || ismember(neighbourNum(ii),closeListNum)
continue;
elseif (ismember(neighbourNum(ii),openListNum))
oldGcost = getGcost(point(neighbourNum(ii)),n);
father = point(neighbourNum(ii)).father;
point(neighbourNum(ii)).father = currentPoint.num;
newGcost = getGcost(point(neighbourNum(ii)),n);
if newGcost>oldGcost
point(neighbourNum(ii)).father = father;
else
point(neighbourNum(ii)).Gcost = newGcost;
end
continue;
elseif ~ismember(neighbourNum(ii),closeListNum)
point(neighbourNum(ii)).father = currentPoint.num;
point(neighbourNum(ii)).Gcost = getGcost(point(neighbourNum(ii)),n);
point(neighbourNum(ii)).Hcost = getHcost(point(neighbourNum(ii)),point(goalNum),n);
openList = [openList,point(neighbourNum(ii))];
end
end
closeListNum = cat(1,closeList.num);
openListNum = cat(1,openList.num);
pause(0.1);
mydrawnow(starNum,goalNum,banList,closeListNum,openListNum,n);
end
**1 matlab版本**
2014a
**2 参考文献**
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)
