2D和3D空间中的A*寻路算法的实现（A Star path finding algorithm）
说明作为经典的寻路算法，A*在网上已经有太多的介绍和教程了。尽管确实存在不少直接转载或者复制粘贴的博文，但还是存在具有指导性意义的文章的，我在这里就不在赘述，也不贴出我看了哪些文章了。希望大家稍微花点时间自行查阅（中外结合效果更加），每个人的理解和表达各有千秋，汲取不同人的文章更有助于我们能从多方面去认知问题。尽管如此，我还是给出一个自认为说的简单明了的视频地址，来源YTB，被网友转载于B站，无字
说明
作为经典的寻路算法，A*在网上已经有太多的介绍和教程了。尽管确实存在不少直接转载或者复制粘贴的博文，但还是存在具有指导性意义的文章的，我在这里就不在赘述，也不贴出我看了哪些文章了。希望大家稍微花点时间自行查阅（中外结合效果更佳），每个人的理解和表达各有千秋，汲取不同人的文章更有助于我们能从多方面去认知问题。尽管如此，我还是给出一个自认为说的简单明了的视频地址，来源YTB，被网友转载于B站，无字幕，但是影响不大，传送门：A *算法
本文最后会直接贴出基于上述视频指导，用Golang实现的一个必定能编译运行的程序源码。视频最后提供了伪代码，使用任何高级语言都能很容易实现，希望大家挑选自己熟悉的编程语言动手实现。接下来我将对视频中的伪代码进行2D空间的逐行实现。
正文
如下图所示，我使用细圆点“·”表示可移动区域，用粗圆点“●”表示障碍物，用“S”表示起点，用“X”表示终点。
然后用“*”表示最短路径：
以上仅为一个随机地图示例，不再贴出更多了。接下来开始正式实现。
在正式往下实现之前，我们需要先统一下认知：
1、我们将地图进行简化抽象后，组成地图的最小单元称之为Node（有时候我会称之为节点），在地图上的行进就是在Node之间的转移;
2、Node之间的转移只会发生在相邻的Node之间（即上下左右和四个角共8个，或者你的定义不允许斜着移动，那么就只有上下左右4个相邻Node）;
3、存在特殊的Node，我们无法立足，即障碍物;
4、终点不能为障碍物;
5、地图由是行和列（或者理解为Y和X）组成的二维平面地图。
希望大家在自己的理解基础上，先尝试自己按照自己的想法实现，如果碰到疑问，可以参考下我的实现。
OK，我们开始看伪代码, step by step（要求先看过几遍视频内容，对A*算法有基本的认识）：
0.0、数据类型的定义
为了简化实现和便于理解，文章粘贴的代码并不完整，只表达意思，且涉及的类型越少越好。这里我只定义一个类型：Node
//Node：地形中的一个最小单位，可以是可移动的平地，也可以是无法穿过的障碍物
type Node struct {
row, col int //在地图中的位置，哪一行哪一列
parent *Node //父节点，用于寻路追溯的关键角色
isWall bool //是否为障碍物，这里是墙...
f, g, h int //三个cost
}
//Node唯一的方法
//用于表示当前Node的唯一标识
func (n *Node) GetKey() string {
return fmt.Sprintf("%d, %d", n.row, n.col)
}0.1、创建初始条件
首先是地图场景scene，直接上二维，便于理解和操作：
var scene [][]*Node //关于scene的内容，请查看完整代码。起点和终点：
var start, end *Node1、 OPEN和CLOSED的创建。
我们知道，这两个集合分别用于存放有希望成为路径一部份的Node类型，和不用再去分析的Node。我选择用golang的map去实现这种集合：1、不关心顺序; 2、方便删除增加元素。
//key为Node.GetKey()
//open集合，里面的Node都有可能为最终路线上的成员
open := make(map[string]*Node)
//closed集合，里面的点已经探讨过，后续迭代不再处理存在与closed集合中的Node
closed := make(map[string]*Node)2、add the start node to open
这一步没啥解释：
open[start.GetKey()] = start3、loop
1) current = node in OPEN with the lowest f_cost
这一步的意思是从OPEN集合中找出f cost最低的一个Node作为研究对象。我们可能已经知道：
F = G + H
H = 当前研究Node和目标Node之间的预估距离，而不是求得的实际距离，因此H仅作参考（我这里采用manhattan计算法）
G = 当前研究点到起点Node之间的路径距离。注意，这里要强调一下，这个距离不是当前研究的Node和起点之间的manhattan距离。
我自己的总结就是，G值的求解只在一种情况下会计算，就是求某个Node的相邻点的F cost的时候。而邻居点的G cost = 某个Node的
G + 某个Node到邻居点的距离．如果计算Ｇ用了与起点的manhattan距离，最后路线是能够找到，但并不是最短路径，有时还会歪歪
扭扭地绕路．有特殊情况就是，OPEN中可能存在几个f cost相同的Node，这时我们选哪个作为当前Node？根据视频中的方法，可以选择h cost最小的Node，因为它距离终点的预估距离最短，认为它最有希望成为最短路径上的一个必经之地。如果，同时还有几个h cost也相同的Node，怎么选呢？这个就无所谓了，这种情况下就存在两条相同cost的最短路径，尽管他们的行进路线不一样，但是他们的行程是一样的；结果选取哪一条路，取决于你先看哪个邻居。
实际上，按照我的理解，对于OPEN中几个具有相同cost的NodeX，如果不考虑在规模较大的地图中以最快的速度查找最短路径，那么取NodeX中的任意Node最终都可以找到最短路径，只是会在非最短路径Node上产生额外的查找代价。因此这是一个算法优化手段。
current := GetLowestF(open)
//获取具有最小综合cost的Node
func GetLowestF(collection map[string]*Node) *Node {
if len(collection) == 0 {
panic("不可达")
}
var p0 *Node
//开始不知道最小的cost会是多少，我们直接先用一个大的离谱的cost作为初始cost：
minF := WIDTH*HEIGHT*10
for _, k := range collection {
//这个点的f cost更小，更新minF，继续尝试寻找更小的cost的Node
if k.f < minF {
minF = k.f
p0 = k
}
}
return p0
}2）remove current from OPEN
delete(open, current.GetKey()) 3）add current to CLOSED
closed[current.GetKey()] = current4) if current is the target node； return
如果当前所研究的Node已经轮到了目标点，则停止查找。
if current.GetKey() == end.GetKey() {
//退出for循环
break
}5）for each neighbour of the current node
这个步骤需要我们找出当前点的相邻点，也没什么难度
neighbours := GetNeighbours(current, scene)//寻找邻居，这里使用了8个邻居，如果你不允许走对角线，那么只保留上下左右的4个邻居即可
//只为寻找到最短路线的话，邻居顺序无所谓，但有多条最短路径时，路径的判定会根据邻居提取顺序变化
//唯一需要注意的就是，邻居的行和列不能越界
func GetNeighbours(self *Node, scene [][]*Node) []*Node {
res := make([]*Node, 0)
//左上
ulRow := self.row-1
ulCol := self.col-1
if ulRow >= 0 && ulCol >= 0 {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == ulRow && c.col == ulCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//上
uRow := self.row-1
uCol := self.col
if uRow >= 0 {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == uRow && c.col == uCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//右上
urRow := self.row-1
urCol := self.col+1
if urRow >= 0 && ulCol < len(scene[0]) {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == urRow && c.col == urCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//左
lRow := self.row
lCol := self.col-1
if lCol >= 0 {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == lRow && c.col == lCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//右
rRow := self.row
rCol := self.col+1
if rCol < len(scene[0]) {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == rRow && c.col == rCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//左下
ldRow := self.row+1
ldCol := self.col-1
if ldRow < len(scene) && ldCol >= 0 {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == ldRow && c.col == ldCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//下
dRow := self.row+1
dCol := self.col
if dCol < len(scene[0]) {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == dRow && c.col == dCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
//右下
rdRow := self.row+1
rdCol := self.col+1
if rdRow < len(scene) && rdCol < len(scene[0]) {
for _, r := range scene {
found := false
for _, c := range r {
if c.row == rdRow && c.col == rdCol {
res = append(res, c)
found = true
break
}
}
if found {
break
}
}
}
return res
}6）if neighbour is not traversable or neighbour is in CLOSED
倘若当前研究的邻居不可研究（即被标记为障碍物）或者它已经处于CLOSED集合中了，我们就跳过它，研究下一个邻居。
//如果这个邻居是障碍物，则忽略
if neighbour.isWall {
continue
}
//如果这个邻居在closed集合中，我们也忽略
if _, ok := closed[neighbour.GetKey()]; ok {
continue
}7）if new path to neighbour is shorter OR neighbour is not in OPEN
set f_cost of neighbour
set parent of neighbour to current
if neighbour is not in OPEN
add neighbour to OPEN
这里需要再强调一下这个new path to neighbour（下面简称为nPath）的计算方法。在之前的loop第一步中我们了解了COST的计算方法，而cost也同时是描述path长短的单位，cost越低则路径越短。new path不是直接计算neighbour和起点之间的manhattan距离得到的，而是一路研究某个节点P(x)的邻居,以及P(x)邻居的邻居(以此类推，最后到达当前的neighbour)...最后把研究过的P(x)所组成的路径的cost统计出来，最后得到一个表征真实行进路径的cost，这个cost就是new path to neighbour。如果我没说明白，还是直接看代码比较好理解:
//计算当前邻居的cost: f g h
tmpG, tmpH := 0, 0
//[注意]：邻居的g cost根据当前的Node来计算的，即当前Node的g加上当前点到邻居的cost。而当前Node的cost也是经过了同样方式计算而来的，
//所以我们直接取其cost，不用再关心它的计算过程。
//说的更简单点，当前点·current·的邻居的 g cost 计算方式为：
// g = 14 + current.g （四个角上的邻居）
// 或
// g = 10 + current.g （上下左右的邻居）
if neighbour.row!=current.row && neighbour.col!=current.col {
tmpG = 14 + current.g
}else {
tmpG = 10 + current.g
}
//邻居的h值，注意，h只是预估cost，采用manhattan计算法其与终点之间的cost即可
tmpH = GetManhattan(neighbour, end)
//最终综合cost f = h + g
tmpF := tmpH+tmpG
_, ok := open[neighbour.GetKey()]
//如果邻居通过当前点所需要的综合 cost f 比邻居之前作为别人邻居所需的更低（有点拗口）
//或者邻居之前没有作为候选点被加入open集合，那么我们就更新它的状态
if tmpF < neighbour.f || !ok {
//首先当前点设置为这个邻居的parent，之前也说过了，如果最后找到路了，可以方便最后反向追溯，获取路径
neighbour.parent = current
//更新邻居的cost为最小cost
neighbour.g = tmpG
neighbour.h = tmpH
neighbour.f = tmpF
//添加邻居进入open集合，让它后面作为“当前点”，以同样的流程研究它的邻居们
open[neighbour.GetKey()] = neighbour
}
//至此一个邻居的研究完成
至此，关键的部分已经完成了。最后需要真正把路线找出来，需要向上查找终点的parent
//经过上一步骤的寻找，我们应该摸到终点了
//此时通过访问终点的parent,我们就能够知道到达终点的最后一个Node：Final是谁，同理访问Final的parent, 及parent的parent...我们就能回到起点，
//那么就找到了我们的路径了。
path := make([]*Node, 0)
p := end.parent
for {
if p != nil {
if p.parent == nil {
break
}
path = append(path, p)
p = p.parent
}else {
break
}
}这个path就是我们的目标最短路径。为了验证结果的正确与否，我们需要画出来直观地检查结果，具体方法可以自己用喜欢的方法实现。
最后一句
本文介绍了的是2D的寻路算法，而3D的空间寻路也可以根据同样的思想进行实现，我的实现已经同步更新到gitee中，欢迎指导。
结束
由于篇幅不是很短，我就不直接在文中粘贴全部代码了，全部代码托管于gitee传送门
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
0
0- 0
已为社区贡献1条内容
所有评论(0)