机器人路径规划：基于双向A*算法（bidirectional a star）的机器人路径规划（提供Python代码）

一、双向A*算法简介

传统A*算法是一种静态路网中求解最短路径最有效的方法，它结合了BFS 算法和迪杰斯特拉算法(Dijkstra)的优点。和迪杰斯特拉算法(Dijkstra)一样， A*算法能够用于搜索最短路径；和BFS 算法一样， A*算法可以用启发式函数引导给出当下的最佳解。传统A*算法的重点在于扩展下一个节点时引入了启发式函数ｈ（ｎ），对当前节点到目标节点的距离代价进行了评估。通过计算函数的相对最优解来筛选当前节点周围的扩展节点，能适用于各种场景，相当灵活。

双向A*算法具体操作步骤如下：

１）建立两个open列表和两个close列表， open 列表用来存放正反方向上已经生成但还没有被遍历即等待检查的节点数据， close列表用来记录正反方向上已访问过的即不需要再检查的节点数据。

２）将开始结点和目标节点分别放在两个open 列表中作为当前节点，对它所有的可达到且没有标记过的子节点进行扩展，由于此时待搜索的节点只有一个，即开始结点和目标节点的评估值肯定为最优值，则将这两个节点分别加入到两个close列表中。

３）对open 列表中的子节点计算评估值，按照评估值的大小进行排列，找出评估值最小的节点，并给它作标记加入到对应的close列表中。

４）如果满足搜索条件，表示找到路径，停止搜索；否则将该节点继续进行扩展，重复执行第三步；或者open列表空了，表示没有路径。

５）从两个close列表的相遇节点开始进行父亲节点遍历，规划出最终的路径。

参考文献：

[1]刘梦杰,朱希安,王占刚,等.基于双向A*算法的矿井水灾逃生路径应用研究[J].煤炭工程, 2019(9):6.DOI:CNKI:SUN:MKSJ.0.2019-09-011.

二、部分代码

import mathimport matplotlib.pyplot as pltshow_animation = Falseclass BidirectionalAStarPlanner:def __init__(self, ox, oy, resolution, rr):"""Initialize grid map for a star planningox: x position list of Obstacles [m]oy: y position list of Obstacles [m]resolution: grid resolution [m]rr: robot radius[m]"""self.min_x, self.min_y = None, Noneself.max_x, self.max_y = None, Noneself.x_width, self.y_width, self.obstacle_map = None, None, Noneself.resolution = resolutionself.rr = rrself.calc_obstacle_map(ox, oy)self.motion = self.get_motion_model()class Node:def __init__(self, x, y, cost, parent_index):self.x = x  # index of gridself.y = y  # index of gridself.cost = costself.parent_index = parent_indexdef __str__(self):return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)