路径规划算法之A*算法（附代码及函数功能和参数详细注释）
效果如下
A*（A star）算法是一种在图或网络中寻找从起始节点到目标节点的最短路径的启发式搜索算法。A* 算法结合了 Dijkstra 算法的最短路径搜索和贪心算法的启发式搜索，因此在许多情况下比其他算法更高效地找到最优路径。它常用于路径规划、游戏中的路径查找以及人工智能领域的图搜索问题。
A* 算法的主要思想是维护两个集合：一个是已探索的节点集合，另一个是待探索的节点集合。算法在每一步选择待探索集合中的节点，计算该节点的代价（包括实际代价和启发式代价），然后选取代价最小的节点进行探索。这种选择过程通过使用启发式函数（估计从当前节点到目标节点的代价）来指导，从而使搜索更加方向性和高效。
A* 算法的伪代码如下：
初始化开始节点，将其加入待探索集合。
当待探索集合不为空时：
a. 从待探索集合中选择代价最小的节点作为当前节点。
b. 如果当前节点是目标节点，则搜索完成，回溯生成最优路径。
c. 否则，将当前节点从待探索集合移至已探索集合，并考虑扩展其邻居节点。
d. 计算邻居节点的代价，并将其添加到待探索集合（如果未加入）或更新已有节点的代价（如果更小）。
如果待探索集合为空且未找到目标节点，则路径不存在。
A* 算法的关键在于如何计算节点的代价。代价通常由两部分组成：从起始节点到当前节点的实际代价（即路径长度）和从当前节点到目标节点的启发式代价。这两个代价的权重可以根据问题调整，从而平衡实际代价和启发式代价的影响。
总之，A* 算法通过不断选择代价最小的节点进行搜索，同时利用启发式代价指导搜索方向，能够高效地找到图或网络中的最短路径。
函数功能详细注释
以下是代码中各个重要函数的详细解释：
-
__init__(self, ox, oy, resolution, rr):- 参数：
ox：障碍物的 x 坐标列表oy：障碍物的 y 坐标列表resolution：网格的分辨率rr：机器人的半径
- 功能：初始化 A* 路径规划器的各项参数，并调用
calc_obstacle_map函数计算障碍物地图。 - 返回值：无
- 参数：
-
planning(self, sx, sy, gx, gy):- 参数：
sx：起始点的 x 坐标sy：起始点的 y 坐标gx：目标点的 x 坐标gy：目标点的 y 坐标
- 功能：执行 A* 路径搜索算法，找到从起始点到目标点的最优路径。
- 返回值：最优路径的 x 坐标列表
rx和 y 坐标列表ry。
- 参数：
-
calc_final_path(self, goal_node, closed_set):- 参数：
goal_node：目标节点，表示找到的最终目标节点closed_set：已探索节点的集合
- 功能：从目标节点出发，通过
closed_set回溯计算出最终路径。 - 返回值：最优路径的 x 坐标列表
rx和 y 坐标列表ry。
- 参数：
-
calc_heuristic(n1, n2):- 参数：
n1：第一个节点n2：第二个节点
- 功能：计算两个节点之间的启发式代价，用于引导搜索方向。
- 返回值：两个节点之间的代价值。
- 参数：
-
calc_grid_position(self, index, min_position):- 参数：
index：网格索引min_position：最小位置值
- 功能：将网格索引转换为实际坐标位置。
- 返回值：实际坐标位置。
- 参数：
-
calc_xy_index(self, position, min_pos):- 参数：
position：位置坐标min_pos：最小位置值
- 功能：将实际坐标位置转换为网格索引。
- 返回值：网格索引。
- 参数：
-
calc_grid_index(self, node):- 参数：
node：节点对象
- 功能：将节点对象转换为在网格中的索引。
- 返回值：网格索引。
- 参数：
-
verify_node(self, node):- 参数：
node：待验证的节点对象
- 功能：验证节点是否合法，即是否在地图内且没有碰撞障碍物。
- 返回值：布尔值，表示节点是否合法。
- 参数：
-
calc_obstacle_map(self, ox, oy):- 参数：
ox：障碍物的 x 坐标列表oy：障碍物的 y 坐标列表
- 功能：根据障碍物位置信息生成障碍物地图。
- 返回值：无
- 参数：
-
get_motion_model():- 参数：无
- 功能：返回机器人的运动模型，包括不同运动方向的步长和代价。
- 返回值：运动模型列表。
-
main():- 参数：无
- 功能：主函数，定义起点、终点、障碍物信息，执行路径规划，并绘制路径和地图。
- 返回值：无
这些函数相互协作，实现了A*路径规划算法中的关键步骤，包括初始化、节点扩展、代价计算、路径回溯等。在主函数中，初始化地图和路径规划器后，通过调用 planning 函数找到最优路径，并将路径和地图用 matplotlib 进行可视化展示。
具体代码：
import math
import matplotlib.pyplot as plt
show_animation = True
class AStarPlanner:
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)
class 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)
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
"""
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()
open_set[self.calc_grid_index(start_node)] = start_node
while True:
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
@staticmethod
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
def calc_grid_position(self, index, min_position):
"""
calc grid position
:param index:
:param min_position:
:return:
"""
pos = index * self.resolution + min_position
return pos
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
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)
self.x_width = round((self.max_x - self.min_x) / self.resolution)
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
@staticmethod
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
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
# 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)
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()
if __name__ == '__main__':
main()
希望本章博客对您有帮助
文章出处登录后可见！
已经登录？立即刷新
