A* 算法原理以及在二维环境地图中的应用 -- Python 代码实现

上节学习了 Dijkstra 路径规划规划算法，虽然能够找到最短的路径，但是其遍历的搜索过程具有盲目性，因此效率比较低，计算量非常大。而实际中电子地图的结点数量是非常庞大的，Dijkstra 算法在有限的时间内可能无法搜索到目标点，此时就要用到启发式搜索。

启发式搜索就是在搜索的过程中加入与问题有关的的启发式信息，引导搜索朝着最优的方向前进。这样就可以忽略大量与启发式信息无关的结点，提高搜索效率。在启发式搜索中，对结点的估价十分重要，采用不同的估价标准会产生不同的结果。

A*算法是建立在 Dijkstra 算法基础上的启发式搜索算法。 该算法的主要特点是：在选择下一个搜索结点时，通过引入多种有用的路网信息，计算所有的候选结点与目标点之间的某种目标函数，例如最短行车距离、最短行车时间、最少行车费用等，以此目标函数值为标准来评价候选结点是否为最优路径应该选择的结点，符合所选择的最优目标函数的候选结点将优先选择为进行下一次搜索的的起点。

A* 算法是一种智能搜索算法，它通过引入与目标点有关的启发式信息，指引算法沿着最有希望的方向进行搜索。选择带有合理、准确的启发式信息的估价函数，有助于减少搜索空间、提高搜索效率。采用启发信息的目的是估计当前结点与目标结点之间的距离，在进行结点选择时，优先选择具有最小估价函数的结点。

A* 算法的关键是确立如下形式的启发式估价函数：
f(n)=g(n)+h(n)f(n)=g(n)+h(n) f(n)=g(n)+h(n)
式中，g(n) 是从起点 s 到候选结点 n 的实际代价；h(n) 是候选结点到目标点 D 的估计代价。在此 必须保证 h(n) <= h*(n), 其中 h*(n)表示结点 n 到目标结点的实际最小代价。

A* 算法在搜索的过程中优先搜索 f(n) 值最小的结点。

回顾 Dijkstra 算法，A* 算法的不同是在 Dijkstra 算法中选择下一个搜索结点时的评价函数 g(n) 的基础上加入了启发式函数 h(n)，将 g(n) 与 h(n) 的和作为选择下一个搜索结点的新的评价函数。

Python 代码实现

在上一节 Dijkstra 算法的基础上，修改一下选择结点的评价函数即可。修改如下：

# 选择扩展点 f(n) = g(n) + h(n)
c_id = min(open_set,key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[o]))

其中启发式函数代码如下：

def calc_heuristic(n1, n2):w = 1.0  # weight of heuristicd = w * math.sqrt((n1.x - n2.x)**2, (n1.y - n2.y)**2)return d

A* 算法的实验结果如下所示：

Dijkstra 算法和 A* 算法的路径规划对比如下：

由对比结果可得，Dijkstra 算法规划的最短路径与 A* 算法规划的最短路径并不一致，那是不是有问题的，其实没有问题，我们这个环境相对简单，从起点到终点的最短路径不止一条，现在来看一下两种算法规划的最短路径分别是多少。

由结果可得，Dijkstra 算法和 A* 算法规划的路径虽然在本环境中路径不一致，但皆为最短路径！

A*算法完整 Python 代码：

import matplotlib.pyplot as plt
import mathclass A_star:def __init__(self, ox, oy, grid_size, robot_radius):# 初始化地图的情况self.min_x = Noneself.max_x = Noneself.min_y = Noneself.max_y = Noneself.x_grid_num = Noneself.y_grid_num = Noneself.obstacle_map = Noneself.grid_size = grid_sizeself.robot_radius = robot_radiusself.calc_obstacle_grid_map(ox, oy)         # 构建环境栅格地图self.robot_motion = self.get_motion_model()def calc_obstacle_grid_map(self, ox, oy):""" 构建环境栅格地图 """# 1. 获取环境的 上、 下、 左、 右 四个边界值self.min_x = round(min(ox))self.max_x = round(max(ox))self.min_y = round(min(oy))self.max_y = round(max(oy))# 2. 根据四个边界值和栅格的大小计算 x, y 方向上 栅格的数量self.x_grid_num = round((self.max_x - self.min_x) / self.grid_size)self.y_grid_num = round((self.max_y - self.min_y) / self.grid_size)# 3. 初始化环境栅格地图self.obstacle_map = [[False for _ in range(self.x_grid_num)] for _ in range(self.y_grid_num)]# 4. 将障碍物占据栅格""" 遍历每一个 栅格（前两个 for 循环）以及 遍历每一个障碍物（后两个循环）, 并计算障碍物到栅格的距离比较该距离和机器人半径的大小，判断该栅格是否应该被障碍物占据"""for ix in range(self.x_grid_num):for iy in range(self.y_grid_num):x = self.calc_position(ix, self.min_x)y = self.calc_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.sqrt((iox - x)**2 + (ioy - y)**2)if d <= self.robot_radius:self.obstacle_map[ix][iy] = Truebreakdef planning(self, sx, sy, gx, gy):""" 进行路径规划 """## 1. 将机器人的坐标进行结点化sx_index = self.calc_xy_index(sx, self.min_x)sy_index = self.calc_xy_index(sy, self.min_y)gx_index = self.calc_xy_index(gx, self.min_x)gy_index = self.calc_xy_index(gy, self.min_y)start_node = self.Node(sx_index, sy_index, 0.0, -1)goal_node = self.Node(gx_index, gy_index, 0.0, -1)# 2. 初始化 open_set, close_set,并将起点放进 open_set 中open_set, close_set = dict(), dict()open_set[self.calc_index(start_node)] = start_node# 3.开始循环while True:# (1). 取 open_set 中 cost 最小的结点: f(n) = g(n) + h(n)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]if show:  # pragma: no coverplt.plot(self.calc_position(current.x, self.min_x),self.calc_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(close_set.keys()) % 10 == 0:plt.pause(0.001)# (2). 判断该节点是否为终点if current.x == goal_node.x and current.y == goal_node.y:print('Find Goal!')print('A*: 从起点{}到终点{}的最短路径长度为：{}'.format((sx, sy), (gx, gy), current.cost))goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# (3). 将该节点从 open_set 中取出，并加入到 close_set 中del open_set[c_id]close_set[c_id] = current# (4). 根据机器人的运动模式，在栅格地图中探索当前位置出发到达的下一可能位置for move_x, move_y, move_cost in self.robot_motion:node = self.Node(current.x + move_x,current.y + move_y,current.cost + move_cost, c_id)n_id = self.calc_index(node)if n_id in close_set:continueif not self.verify_node(node):continueif n_id not in open_set:open_set[n_id] = node   # 发现新的结点else:if open_set[n_id].cost >= node.cost:# 当前节点的路径到目前来说是最优的，进行更新open_set[n_id] = noderx, ry = self.calc_final_path(goal_node, close_set)return rx, rydef calc_final_path(self, goal_node, close_set):""" 从终点开始进行回溯，生成从起点到终点的最优路径 """rx = [self.calc_position(goal_node.x, self.min_x)]ry = [self.calc_position(goal_node.y, self.min_y)]parent_index = goal_node.parent_indexwhile parent_index != -1:n = close_set[parent_index]rx.append(self.calc_position(n.x, self.min_x))ry.append(self.calc_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, ryclass Node:def __init__(self, x, y, cost, parent_index):self.x = x      # 栅格的 x 轴索引self.y = y      # 栅格的 y 轴索引self.cost = cost        # g(n)self.parent_index = parent_index       # 当前节点的父节点## def __str__(self):#     return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)def calc_index(self, node):"""将栅格化后的地图进行编号索引，从左下角向右一行一行进行编号索引,如下面示例[7, 8, 9][4, 5, 6][1, 2, 3]"""index = node.y * self.x_grid_num + node.xreturn indexdef calc_xy_index(self, pos, min_p):""" 将机器人在二维环境地图中的坐标转化成栅格地图中的坐标 """index = round((pos - min_p) / self.grid_size)return indexdef calc_position(self, index, min_p):""" 将栅格地图的坐标转化成在真实环境中的坐标 """pos = min_p + index * self.grid_sizereturn posdef verify_node(self, node):""" 验证机器人的当前位置是否合理 """px = self.calc_position(node.x, self.min_x)py = self.calc_position(node.y, self.min_y)# 检查当前位置是否在环境内if px < self.min_x or px > self.max_x:return Falseif py < self.min_x or py > self.max_y:return False# 检查当前位置是否处于障碍物中if self.obstacle_map[node.x][node.y]:return Falsereturn True@staticmethoddef calc_heuristic(n1, n2):""" 启发式函数 h(n)"""w = 1.0  # weight of heuristicd = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2)return d@staticmethoddef get_motion_model():# dx, dy, costmodel = [[0, 1, 1],      # 上[0, -1, 1],     # 下[-1, 0, 1],     # 左[1, 0, 1],      # 右[1, 1, math.sqrt(2)],    # 右上[1, -1, math.sqrt(2)],   # 右下[-1, -1, math.sqrt(2)],  # 左下[-1, 1, math.sqrt(2)]    # 左上]return modeldef main():# 设置起点，终点sx, sy = 30, 30gx, gy = 70, 70gird_size = 1.0       # 栅格的大小robot_radius = 2.0        # 机器人的半径# 设置环境地图ox, oy = [], []# 设置四条边for i in range(20, 80):         # 下边ox.append(i)oy.append(20.0)for i in range(20, 80):         # 右边ox.append(80.0)oy.append(i)for i in range(20, 80):         # 上边ox.append(i)oy.append(80.0)for i in range(20, 80):         # 左边ox.append(20)oy.append(i)# 设置内部的障碍物for i in range(20, 60):ox.append(40)oy.append(i)for i in range(40, 80):ox.append(60)oy.append(i)if show:plt.plot(ox, oy, '.k')plt.plot(sx, sy, 'og')plt.plot(gx, gy, 'or')# plt.grid('True')plt.axis('equal')# plt.show()a_star = A_star(ox, oy, gird_size, robot_radius)rx, ry = a_star.planning(sx, sy, gx, gy)if show:plt.plot(rx, ry, '-r')plt.pause(0.01)plt.show()if __name__ == '__main__':show = Truemain()

如果本文章对您有帮助，记得在下面点赞呦！也欢迎在下方评论区留言讨论！

A* 算法原理以及在二维环境地图中的应用 -- Python 代码实现相关推荐

数学建模——一维、二维插值模型详解Python代码
数学建模--一维.二维插值模型详解Python代码一.一维插值 # -*-coding:utf-8 -*- import numpy as np from scipy import interpol ...
程序员的乐趣，生成自定义二维码，5 行 Python 代码就搞定
选自 | towardsdatascience 作者 | Arindom Bhattacharjee 转自 | 机器之心参与 | 杜伟.小舟随处可见的二维码是怎么生成的?自己做一个试试吧. 随着互 ...
剑指offer：二维数组中的查找python实现
题目描述在一个二维数组中(每个一维数组的长度相同),每一行都按照从左到右递增的顺序排序,每一列都按照从上到下递增的顺序排序.请完成一个函数,输入这样的一个二维数组和一个整数,判断数组中是否含有该整数 ...
程序员的乐趣，生成自定义二维码，5行Python代码就解决
随处可见的二维码是怎么生成的?自己做一个试试吧. 随着互联网和智能移动设备不断普及,二维码(Quick Response code)已经成为世界上应用最为广泛的信息载体之一.生成二维码的工具也层出不穷 ...
RRT* 算法原理以及在二维仿真环境中的实现 -- Python代码实现
RRT* 算法是在 RRT 的基础上做出了一些改进,主要改进的点有两点: 新结点生成后,优化其父结点. 在生成新结点 new_node 后,首先设置一个搜索区域的半径,搜索该区域中的树结点,并计算其中 ...
Dijkstra 路径规划算法在二维仿真环境中的应用 -- Python代码实现
在上一节中,介绍了 Dijkstra 算法的原理以及在图中的应用,这一节将一步步实现 Dijkstra 路径规划算法在二维环境中的路径规划,来进一步加深对 Dijkstra 算法的理解. 所需要用到的 ...
种子点生长算法（上）——二维种子点生长
本文只用作笔记,方便日后查阅. 种子点生长算法上--二维种子点生长下文提到的种子点生长算法,包括泛洪法,扫描线法,区段法三种.文本先从最简单的泛洪法入手介绍种子点生长算法的相关概念.之后进一步讨论了 ...
RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现
在上一节中,介绍了 RRT 算法的原理,这一节将一步步实现 RRT 路径规划算法在二维环境中的路径规划,来进一步加深对 RRT 算法的理解. 二维环境的搭建我们将搭建下图所示的二维环境,绿色点为起点 ...
DPC密度峰值聚类算法原理详解二
DPC密度峰值聚类算法原理详解二 1.计算数据点两两之间的距离 1.使用 Numpy 模块查找两点之间的欧几里得距离: 2.使用 distance.euclidean() 函数查找两点之间的欧式距离: ...

A* 算法原理以及在二维环境地图中的应用 -- Python 代码实现

A* 算法原理以及在二维环境地图中的应用 -- Python 代码实现相关推荐

最新文章

热门文章