A*算法的理解与实现
A*的理解与实现
一句话概括：A*是一种启发式搜索算法。
原理参考：https://www.gamedev.net/reference/articles/article2003.asp
代码参考：https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/AStar/a_star.py
现在让我们一点点掰开这个代码的逻辑：
整体结构：
Astar类：
class AStarPlanner：
# 地图初始化
def __init__(self, ox, oy, resolution, rr):
# 节点初始化
class Node:
def __init__(self, x, y, cost, parent_index):
def __str__(self):
#路径规划函数
def planning(self, sx, sy, gx, gy):
def calc_final_path(self, goal_node, closed_set):
def calc_heuristic(n1, n2):
def calc_grid_position(self, index, min_position):
def calc_xy_index(self, position, min_pos):
def calc_grid_index(self, node):
def verify_node(self, node):
def calc_obstacle_map(self, ox, oy):
def get_motion_model():
主函数：
# set obstacle positions 生成障碍物位置
ox, oy = [], []
for i in range(-10, 60):
ox.append(i)
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
oy.append(i)
for i in range(-10, 61):
ox.append(i)
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
oy.append(i)
for i in range(-10, 40):
ox.append(20.0)
oy.append(i)
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k") # 障碍物
plt.plot(sx, sy, "og") # 起始点
plt.plot(gx, gy, "xb") # 目标点
plt.grid(True)
plt.axis("equal")
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
# 初始化，输入地图信息
"""
Initialize grid map for a star planning
ox: x position list of Obstacles [m]
ox: list[Union[int, float]] = []
oy: y position list of Obstacles [m]
oy: list[Union[float, int]] = []
grid_size/resolution: grid resolution [m] float
robot_radius/rr: robot radius[m] float
"""
rx, ry = a_star.planning(sx, sy, gx, gy)
# 路径规划函数：传入起始点和终点（注意这里和前面传参数的区别
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.pause(0.001)
plt.show()
地图初始化
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
def __init__(self, ox, oy, resolution, rr):
"""
Initialize grid map for a star planning
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
self.resolution = resolution # 分辨率
self.rr = rr
self.min_x, self.min_y = 0, 0
self.max_x, self.max_y = 0, 0
self.obstacle_map = None
self.x_width, self.y_width = 0, 0
self.motion = self.get_motion_model() # 搜索移动模式
self.calc_obstacle_map(ox, oy) # 构建地图
self.calc_obstacle_map(ox, oy)
def calc_obstacle_map(self, ox, oy):
self.min_x = round(min(ox))
self.min_y = round(min(oy))
self.max_x = round(max(ox))
self.max_y = round(max(oy))
print("min_x:", self.min_x)
print("min_y:", self.min_y)
print("max_x:", self.max_x)
print("max_y:", self.max_y)
# round(x,n) 四舍五入 当n不存在时，返回整数
self.x_width = round((self.max_x - self.min_x) / self.resolution) # 计算map宽度，从真实值换到栅格个数
self.y_width = round((self.max_y - self.min_y) / self.resolution)
print("x_width:", self.x_width) #栅格障碍物横坐标
print("y_width:", self.y_width) #栅格障碍物纵坐标
# obstacle map generation
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]
for ix in range(self.x_width):
x = self.calc_grid_position(ix, self.min_x)
for iy in range(self.y_width):
y = self.calc_grid_position(iy, self.min_y)
for iox, ioy in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if d <= self.rr:
self.obstacle_map[ix][iy] = True
break
# PS：这里的x，y相当于global_planner里的plan_ox,plan_oy
# zip()函数用于将可迭代的对象作为参数，将对象中对应的元素打包成一个个元组，然后返回由这些元组组成的列表。在这里相当于每一个ox和oy组成（ox,oy）：栅格中的障碍物坐标
# math.hypot 计算平方和的平方根——计算距离
def calc_grid_position(self, index, min_position):
pos = index * self.resolution + min_position
return pos
# 总之，这里的for循环相当于在栅格地图里将robot可能碰到障碍物的栅格都标为true? 障碍物膨胀？以栅格为单位？
# 最终得到obstacle_map，横纵坐标均以栅格为单位，值False表示无障碍，True表示障碍
self.get_motion_model()
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
Node类
def __init__(self, x, y, cost, parent_index):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.parent_index = parent_index
def __str__(self): # 对我来说似乎没什么用？但好像可以实时展示搜索
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.parent_index)
'''
__str__()函数的作用：
打印一个实例化对象时，打印的其实时一个对象的地址。而通过__str__()函数就可以帮助我们打印对象中具体的属性值，或者你想得到的东西。
因为在python中调用print()打印实例化对象时会调用__str__()如果__str__()中有返回值，就会打印其中的返回值。
所以在这里如果我们print(Node),就会显示_str_里面的东西
'''
def planning(self, sx, sy, gx, gy)
"""
A star path search
input:
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
output:
rx: x position list of the final path
ry: y position list of the final path
"""
# 将初始点、目标点坐标映射到栅格地图，并定义cost和parent_index
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y), 0.0, -1)
goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
self.calc_xy_index(gy, self.min_y), 0.0, -1)
open_set, closed_set = dict(), dict() # dict() 字典
open_set[self.calc_grid_index(start_node)] = start_node
while 1:
if len(open_set) == 0:
print("Open set is empty..")
break
c_id = min(
open_set,
key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
open_set[
o]))
current = open_set[c_id]
'''
# show graph
if show_animation: # pragma: no cover
plt.plot(self.calc_grid_position(current.x, self.min_x),
self.calc_grid_position(current.y, self.min_y), "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(
0) if event.key == 'escape' else None])
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
'''
if current.x == goal_node.x and current.y == goal_node.y:
print("Find goal")
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# Remove the item from the open set
del open_set[c_id]
# Add it to the closed set
closed_set[c_id] = current
# expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id)
n_id = self.calc_grid_index(node)
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
if n_id in closed_set:
continue
if n_id not in open_set:
open_set[n_id] = node # discovered a new node
else:
if open_set[n_id].cost > node.cost:
# This path is the best until now. record it
open_set[n_id] = node
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
里面用到的数据处理函数：
def calc_final_path(self, goal_node, closed_set):
# generate final course
rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
self.calc_grid_position(goal_node.y, self.min_y)]
parent_index = goal_node.parent_index
while parent_index != -1:
n = closed_set[parent_index]
rx.append(self.calc_grid_position(n.x, self.min_x))
ry.append(self.calc_grid_position(n.y, self.min_y))
parent_index = n.parent_index
return rx, ry
def calc_xy_index(self, position, min_pos):
return round((position - min_pos) / self.resolution)
def calc_grid_index(self, node):
return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
def verify_node(self, node):
px = self.calc_grid_position(node.x, self.min_x)
py = self.calc_grid_position(node.y, self.min_y)
if px < self.min_x:
return False
elif py < self.min_y:
return False
elif px >= self.max_x:
return False
elif py >= self.max_y:
return False # collision check
if self.obstacle_map[node.x][node.y]: return False
return True
