A* search algorithm浅谈

最近跟着老师做了一个稍简单的关于AGV小车的项目，其中小车寻路部分用的是A*算法，用Matlab实现。整个流程下来，学到了挺多的东西，写这篇文章既是为了给广大朋友分享点学习心得，又是给自己一个交代，对前期的学习做一个阶段性的总结 ^_^

Contents：

- History

- Description

- Searching process

- Code in Matlab

---

History

From Wikipedia:
In 1968, AI researcher Nils Nilsson was trying to improve the path planning done by Shakey the Robot, a prototype robot that could navigate through a room containing obstacles. This path-finding algorithm, that Nilsson called A1, was a faster version of the then best known method, Dijkstra’s algorithm, for finding shortest paths in graphs. Bertram Raphael suggested some significant improvements upon this algorithm, calling the revised version A2. Then Peter E. Hart introduced an argument that established A2, with only minor changes, to be the best possible algorithm for finding shortest paths. Hart, Nilsson and Raphael then jointly developed a proof that the revised A2 algorithm was optimal for finding shortest paths under certain well-defined conditions.

　　大概意思是在遥远的1968年，一位名为Nilsson的人工智能探索者尝试着改进基于［Shakey　the　Robot］的路径规划算法，其中Shakey the Robot是一种能在设有障碍的房间内寻迹的原型机器人。这个被Nilsson称为A1的算法比广为人知的Dijkstra算法在图路径规划中拥有更快的运行速度。之后，Bertram　Raphael对A1算法做了一些重要的改进并更新为A2算法，而在这个算法被公开发表前Peter E.Hart又做了微小的改进终于使它成为了接近最优的寻路算法。这一说法后来被Nilsson和Raphael两位好基友证实——确实在特定的条件下，A2是最理想的寻路算法。

　　确实。。。好基友是成双成对的，有一个理想的队友是一件多么令人幸福的事情啊 (☆＿☆)

---

Description

原文（英）链接

首先请允许我隆重地介绍一个基本计算公式：

F=G+H

• 其中G代表着从原格到目标格的行驶代价，H代表目标格到终点的代价，F代表总代价（务必分清楚是哪个格）

• 从原格到目标格移动，每次只能移动一格，上、下、左、右、左上、左下、右上、右下(在最后给出的代码中可能是出于简化的目的，只考虑了上、下、左、右四种情况)

• H(Heuristic,启发代价)的算法有很多种，本文主要介绍的是Manhattan法，据说曼哈顿的的街区都是横纵分布的，只有上下左右(不能斜着走)，而本文采用的H算法为:
  

  H=(终点格纵坐标−目标格纵坐标)+(终点格横坐标−目标格横坐标)

接着引入几个概念（Concepts）：

1. 节点(Node)：将小车所在的每一个位置抽象为一个节点，每个节点又可以具体化为一个方块、圆什么的，这个由你来决定

2. 开放表(Open set)：只有开放表内的节点才可以被检测、拓展

3. 关闭表(Closed set)：被搜索过的节点都会被从开放表中取出来扔进关闭表，说明它已经失去利用价值并在之后的检测中忽略关闭表里面的节点

---

Searching process

1. 初始化：将各节点具体化为小方块，然后设置起始点、终点以及障碍的位置

其中，绿色–>起始格，红色–>终点格，蓝色–>障碍，障碍处禁止通行

2. 然后我们一起看看起始格四周

让我们看个有趣的小对话吧 ^_^

8个方向，让我们假设向上、下、左、右即横纵走付出的代价G都为10，则左上、左下、右上、右下的代价根据勾股定理应该是多少请小明同学回答一下！

14.414 (10 * 根号2)……….

计算正确! 但是，但是，答案应该是14，因为无论是根号还是看似不多的小数位，在多次的迭代中，都会给计算机增加十分巨大的运算量，会极大地减慢程序运行的速度。不信你试试咯 @_@

(1) 将起始点S加入开放表，如图所示用浅蓝色框框框住

(2) 遍历起始点S四周的可通过的点（也就是除了障碍之外的其他点），并将他们全部加入开放列表。同时将起始点S设为父节点
Tips：父节点是一系列点；别小看这些父节点哦，最后的回路寻址就靠他

(3) 这时候起始点S没有利用价值啦，怎么办？扔掉，扔哪？小黑屋！好吧你赢了，扔关闭表里面。

(4) 接下来是计算周围几个格子的F值
- 先算G. 刚刚已经说过了，横纵走代价为10，斜着走代价为14，简单
- 再算H. 嗯？又不记得怎么算了？拖出去打5分钟…….查查一开头就给了的那个公式好嘛？终点与各点横纵差的和
- 然后是F等于G和H的和

结果如下图，每个小方块左上角标为F值，左下角标为G值，右下角标为H值

3. 继续搜索

(1) 选择其中F值最小的点N(new)，把它从开放表里面拿出来扔进关闭表 。明显N点为左边点，蓝框框住、拉进关闭表！

(2) 检查点N周围的点：（重点来了） 设为点C（check）

①若C为障碍点或者已经在关闭表里面的点，忽略

②若C不在开放表里，加进开放表

③若C已经在开发表里面，对比两条线路（即 从上一父节点直接到C 和 从上一父节点经过N到C）分别F值，若第二条路的F低，则不做任何事情；若第一条线路的F低，返回父节点周围，循环步骤(1)直到确定最佳的点N,并将之设为父节点同时储存从新N节点到上一父节点的指向，如此不断循环。第一条斜着走F1=14，第二条经过N绕着走F2=10+14=24，很明显，还是第一条好，返回父节点周围重新检查周围还在开放表里面的点，此时出去起始点、左边点，剩下7个点，检查其中F最小的为右下角的点，将它设为父节点，记录它到开始点指向为左上，结束本次循环重复步骤(1)直到搜索结束

4. 搜索结束

有两种情况：

① 寻路成功，终点被扔进关闭表

② 寻路失败，终点被扔进关闭表前开放表已经为空

补充说明，关于斜着走

斜着走情况比较复杂，需要具体分析，例如此时

若墙在对角线，是否允许擦边斜跨？嗯哼？

---

Code in Matlab

代码可以百度到，用的时候修改了一下，对源码中的一些语句进行了封装，方便写多小车路径规划的 ^_^

代码有点多，就只放最核心的计算部分啦！有需要完整代码的再联系博主

work.m

“`function [setOpen, goalposind, fieldpointers] = work(field, startposind, …
goalposind, costchart, fieldpointers,setOpen,setOpenCosts,setOpenHeuristics,…
setClosed,setClosedCosts,movementdirections,axishandle, num, WAY)
while ~max(ismember(setOpen,goalposind)) && ~isempty(setOpen)
[temp, ii] = min(setOpenCosts + setOpenHeuristics);
[costs,heuristics,posinds] = findFValue(setOpen(ii),setOpenCosts(ii), …
field,goalposind,’euclidean’); %扩展temp的四个方向点，获得其坐标posinds，各个方向点的实际代价costs，启发代价heuristics
% put node in CLOSED and record its cost
setClosed = [setClosed; setOpen(ii)]; %将temp插入CLOSE表中
setClosedCosts = [setClosedCosts; setOpenCosts(ii)]; %将temp的花费计入ClosedCosts
% update OPEN and their associated costs 更新OPEN表 分为三种情况
if (ii > 1 && ii < length(setOpen)) %temp在OPEN表的中间，删除temp
setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];
setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];
setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];
elseif (ii == 1)
setOpen = setOpen(2:end); %temp是OPEN表的第一个元素，删除temp
setOpenCosts = setOpenCosts(2:end);
setOpenHeuristics = setOpenHeuristics(2:end);
else %temp是OPEN表的最后一个元素，删除temp
setOpen = setOpen(1:end-1);
setOpenCosts = setOpenCosts(1:end-1);
setOpenHeuristics = setOpenHeuristics(1:end-1);
end
% for each of these neighbor spaces, assign costs and pointers;
% and if some are in the CLOSED set and their costs are smaller,
% for each of these neighbor spaces, assign costs and pointers;
% and if some are in the CLOSED set and their costs are smaller,
% update their costs and pointers
for jj=1:length(posinds) %对于扩展的四个方向的坐标
% if cost infinite, then it’s a wall, so ignore
if ~isinf(costs(jj)) %如果此点的实际代价不为Inf,也就是没有遇到墙
% if node is not in OPEN or CLOSED then insert into costchart and
% movement pointers, and put node in OPEN
if ~max([setClosed; setOpen] == posinds(jj)) %如果此点不在OPEN表和CLOSE表中
fieldpointers(posinds(jj)) = movementdirections(jj); %将此点的方向存在对应的fieldpointers中
costchart(posinds(jj)) = costs(jj); %将实际代价值存入对应的costchart中
setOpen = [setOpen; posinds(jj)]; %将此点加入OPEN表中
setOpenCosts = [setOpenCosts; costs(jj)]; %更新OPEN表实际代价
setOpenHeuristics = [setOpenHeuristics; heuristics(jj)]; %更新OPEN表启发代价
% else node has already been seen, so check to see if we have
% found a better route to it.
elseif max(setOpen == posinds(jj)) %如果此点在OPEN表中
I = find(setOpen == posinds(jj)); %找到此点在OPEN表中的位置
% update if we have a better route
if setOpenCosts(I) > costs(jj) %如果在OPEN表中的此点实际代价比现在所得的大
costchart(setOpen(I)) = costs(jj); %将当前的代价存入costchart中，注意此点在costchart中的坐标与其自身坐标是一致的（setOpen(I)其实就是posinds(jj)），下同fieldpointers
setOpenCosts(I) = costs(jj); %更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristics
setOpenHeuristics(I) = heuristics(jj); %更新OPEN表中的此点启发代价(窃以为这个是没有变的)
fieldpointers(setOpen(I)) = movementdirections(jj); %更新此点的方向
end
% else node has already been CLOSED, so check to see if we have
% found a better route to it.
end
end
end
if isempty(setOpen)
break;
end %当OPEN表为空，代表可以经过的所有点已经查询完毕
set(axishandle,’CData’,[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);
% hack to make image look right
set(gca,’CLim’,[0 1.1*max(costchart(find(costchart < Inf)))]); %CLim将CData中的值与colormap对应起来: [cmin cmax] Color axis limits （不过不太明白为什么要*1.1）
drawnow; %cmin iapmap. cmax is the value of the data mapped to the last color in the colormap
hold on;
end
end