这是我在参与AGV调度系统开发工作中形成的一些认识，是我的个人观点，想到什么写到什么。我自己也在学习，有不同观点可以一起讨论。由于涉及企业知识产权，文中代码为另外单独实现的DEMO，文章内容仅供参考。
A*算法是路径规划中使用得比较多的算法，其实现起来比较简单，实践效果也挺好且便于在规划中引入一些定制化规则。故在AGV调度的应用场景需求下，其相比D*之类的算法要更加适合。
在AGV调度场景下，D*之类算法重规划上的优势用处不大，因为AGV调度系统的重规划往往是由交管系统发起的，要么交管模块用其他算法直接搜索出策略，要么交管模块更新状态代价之后重规划。而这两者都使得上次规划得到的状态失去意义。
故本章来讨论一下A*和双向A*算法在AGV调度系统里的应用。在调度系统中，双向A*算法相比于A*算法其实有好有坏。好的是部分情况下搜索量变小，坏的是会引入一些A*算法没有的问题。而且双向A*代价其实并不一定比A*低。
A*算法不管是在栅格地图还是拓扑地图的场景都是可以使用的，实际调度系统项目中，用的比较多的还是拓扑地图。因为栅格地图通常只用在控制点不偏心的AGV小车上，而拓扑地图则通用所有场景并且也可以给地图附加更多的属性信息。但是拓扑地图比较抽象也不好呈现效果，为了方便演示，这里使用的是栅格地图。
A*算法：
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.colors as colors
import heapq as hq
# 搜索节点
class Node(object):
def __init__(self, nowx=0, nowy=0, cost=0, dist=0, lastx=0, lasty=0) -> None:
self.nowx = nowx
self.nowy = nowy
self.cost = cost
self.dist = dist
self.lastx = lastx
self.lasty = lasty
def __lt__(self, other):
return self.dist < other.dist
def __le__(self, other):
return self.dist <= other.dist
def __gt__(self, other):
return self.dist > other.dist
def __ge__(self, other):
return self.dist >= other.dist
def __eqeq__(self, other):
return self.dist == other.dist
def __ne__(self, other):
return self.dist != other.dist
# 求距离
def GetDist(start, end):
# return abs(end[0] - start[0]) + abs(end[1] - start[1])
return (abs(end[0] - start[0])**2 + abs(end[1] - start[1])**2)**0.5 * 2
def Contain(list, point):
for i in range(len(list)):
if list[i].nowx == point[0] and list[i].nowy == point[1]:
return True
map_x = 20
map_y = 20
obsindex = 3
# 产生地图矩阵
# Map = np.zeros(map_x * map_y)
# obs_num = int(Map.size * 0.1)
# obs_a = np.random.randint(low=0, high=Map.size, size=obs_num)
# Map[obs_a] = obsindex
# map = Map.reshape([map_x, map_y])
# start = [9, 4]
# end = [9, 14]
# map = [ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 3, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 3, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# ]
start = [0, 0]
end = [19, 19]
map = [ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 3, 3, 3, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3, 3, 3, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
]
cmap = colors.ListedColormap(
['none', 'black', 'white', 'magenta', 'yellow', 'cyan', 'green', 'red', 'blue'])
# 初始化open与close数组
open = []
close = []
hq.heapify(open)
hq.heappush(open, (GetDist(start, end), Node(nowx=start[0], nowy=start[1], dist=GetDist(start, end))))
# 搜索路径
while len(open) > 0:
item = hq.heappop(open)
nowx = item[1].nowx
nowy = item[1].nowy
# 避免超出地图界限
if nowx < 0 or nowx >= map_x or nowy < 0 or nowy >= map_y:
continue
# 避开障碍
if map[nowx][nowy] == obsindex:
continue
# 标记地图
map[nowx][nowy] = 1
# 搜索到终点
if nowx == end[0] and nowy == end[1]:
break
# 取出的节点已经在close列表里面则不在使用
if Contain(close, [nowx, nowy]):
continue
# 当前节点加入close列表
close.append(item[1])
# 生成当前节点的下一步策略
newcost = item[1].cost + 1
hq.heappush(open, (GetDist([nowx + 1, nowy], end) + newcost, Node(nowx=nowx + 1, nowy=nowy, dist=GetDist([nowx + 1, nowy], end), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(open, (GetDist([nowx, nowy + 1], end) + newcost, Node(nowx=nowx, nowy=nowy + 1, dist=GetDist([nowx, nowy + 1], end), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(open, (GetDist([nowx - 1, nowy], end) + newcost, Node(nowx=nowx - 1, nowy=nowy, dist=GetDist([nowx - 1, nowy], end), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(open, (GetDist([nowx, nowy - 1], end) + newcost, Node(nowx=nowx, nowy=nowy - 1, dist=GetDist([nowx, nowy - 1], end), lastx=nowx, lasty = nowy, cost = newcost)))
map[start[0]][start[1]] = 5
map[end[0]][end[1]] = 6
# 打印最终结果
print(map)
plt.imshow(map, cmap=cmap, interpolation='nearest', vmin=0, vmax=7)
plt.show()
第一种障碍物场景的效果 
第二种障碍物场景的效果
双向A*：
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.colors as colors
import heapq as hq
# 搜索节点
class Node(object):
def __init__(self, nowx=0, nowy=0, cost=0, dist=0, lastx=0, lasty=0) -> None:
self.nowx = nowx
self.nowy = nowy
self.cost = cost
self.dist = dist
self.lastx = lastx
self.lasty = lasty
def __lt__(self, other):
return self.dist < other.dist
def __le__(self, other):
return self.dist <= other.dist
def __gt__(self, other):
return self.dist > other.dist
def __ge__(self, other):
return self.dist >= other.dist
def __eqeq__(self, other):
return self.dist == other.dist
def __ne__(self, other):
return self.dist != other.dist
# 求距离
def GetDist(start, end):
# return abs(end[0] - start[0]) + abs(end[1] - start[1])
return (abs(end[0] - start[0])**2 + abs(end[1] - start[1])**2)**0.5 * 2
def Contain(list, point):
for i in range(len(list)):
if list[i].nowx == point[0] and list[i].nowy == point[1]:
return True
map_x = 20
map_y = 20
obsindex = 3
# 产生地图矩阵
# Map = np.zeros(map_x * map_y)
# obs_num = int(Map.size * 0.1)
# obs_a = np.random.randint(low=0, high=Map.size, size=obs_num)
# Map[obs_a] = obsindex
# map = Map.reshape([map_x, map_y])
# start = [9, 4]
# end = [9, 14]
# map = [ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 3, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0],
# [0, 3, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# ]
start = [0, 0]
end = [19, 19]
map = [ [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 3, 3, 3, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 3, 3, 3, 3, 3, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
]
cmap = colors.ListedColormap(
['none', 'black', 'white', 'magenta', 'yellow', 'cyan', 'green', 'red', 'blue'])
# 初始化open与close数组
openA = []
closeA = []
hq.heapify(openA)
hq.heappush(openA, (GetDist(start, end), Node(nowx=start[0], nowy=start[1], dist=GetDist(start, end))))
openB = []
closeB = []
hq.heapify(openB)
hq.heappush(openB, (GetDist(end, start), Node(nowx=end[0], nowy=end[1], dist=GetDist(end, start))))
# 搜索路径
while len(openA) > 0 and len(openB) > 0:
item = hq.heappop(openA)
nowx = item[1].nowx
nowy = item[1].nowy
# 避免超出地图界限
if nowx < 0 or nowx >= map_x or nowy < 0 or nowy >= map_y:
continue
# 避开障碍
if map[nowx][nowy] == obsindex:
continue
# 搜索到终点
if Contain(closeB, [nowx, nowy]):
break
# 标记地图
map[nowx][nowy] = 1
# 取出的节点已经在close列表里面则不在使用
if Contain(closeA, [nowx, nowy]):
continue
# 当前节点加入close列表
closeA.append(item[1])
# 生成当前节点的下一步策略
newcost = item[1].cost + 1
hq.heappush(openA, (GetDist([nowx + 1, nowy], end) + newcost, Node(nowx=nowx + 1, nowy=nowy, dist=GetDist([nowx + 1, nowy], end), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(openA, (GetDist([nowx, nowy + 1], end) + newcost, Node(nowx=nowx, nowy=nowy + 1, dist=GetDist([nowx, nowy + 1], end), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(openA, (GetDist([nowx - 1, nowy], end) + newcost, Node(nowx=nowx - 1, nowy=nowy, dist=GetDist([nowx - 1, nowy], end), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(openA, (GetDist([nowx, nowy - 1], end) + newcost, Node(nowx=nowx, nowy=nowy - 1, dist=GetDist([nowx, nowy - 1], end), lastx=nowx, lasty = nowy, cost = newcost)))
item = hq.heappop(openB)
nowx = item[1].nowx
nowy = item[1].nowy
# 避免超出地图界限
if nowx < 0 or nowx >= map_x or nowy < 0 or nowy >= map_y:
continue
# 避开障碍
if map[nowx][nowy] == obsindex:
continue
# 搜索到终点
if Contain(closeA, [nowx, nowy]):
break
# 标记地图
map[nowx][nowy] = 4
# 取出的节点已经在close列表里面则不在使用
if Contain(closeB, [nowx, nowy]):
continue
# 当前节点加入close列表
closeB.append(item[1])
# 生成当前节点的下一步策略
newcost = item[1].cost + 1
hq.heappush(openB, (GetDist([nowx, nowy - 1], start) + newcost, Node(nowx=nowx, nowy=nowy - 1, dist=GetDist([nowx, nowy - 1], start), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(openB, (GetDist([nowx - 1, nowy], start) + newcost, Node(nowx=nowx - 1, nowy=nowy, dist=GetDist([nowx - 1, nowy], start), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(openB, (GetDist([nowx, nowy + 1], start) + newcost, Node(nowx=nowx, nowy=nowy + 1, dist=GetDist([nowx, nowy + 1], start), lastx=nowx, lasty = nowy, cost = newcost)))
hq.heappush(openB, (GetDist([nowx + 1, nowy], start) + newcost, Node(nowx=nowx + 1, nowy=nowy, dist=GetDist([nowx + 1, nowy], start), lastx=nowx, lasty = nowy, cost = newcost)))
map[start[0]][start[1]] = 5
map[end[0]][end[1]] = 6
# 打印最终结果
print(map)
plt.imshow(map, cmap=cmap, interpolation='nearest', vmin=0, vmax=7)
plt.show()
第一种障碍物场景的效果
第二种障碍物场景的效果
从上面对比效果来看由于启发值的存在，双向搜索对A*的改进并没有体现出优势来。可能双向算法对Dijkstar算法比A*算法要有效果。这还是写这篇文章的时候我才认识到的，之前在公司实现的时候没有去对比两者，想当然以为双向A*会比A*好，现在看来怕不是改了个寂寞。
而在问题上面，我们参考下面的图片：
在调度系统中，很明显搜索结果是1这条路线，但是实际1和2的行驶代价是一样的，但是转向代价不一样。我们更希望车辆走2的路线而不是1的路线，所以通常我们会加上转向代价并调整启发值的计算方式。如果是双向A*，由于交汇的是中间点，即使加上代价，我们求得的仍然不是2这条路径。
2959
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
