呼和浩特商城网站建设,移动互联网以什么为技术核心,三亚推广公司,常用的网站开发语言我宁愿永远做我自己#xff0c;也不愿成为别人#xff0c;即使那个人比你更快乐。
—《成为简奥斯汀》
#x1f3f0;代码及环境配置#xff1a;请参考 环境配置和代码运行! 4.5.1 概述
Dijkstra算法是基于广度优先搜索策略来遍历空间内的所有节点#xff0c;最终计算出…我宁愿永远做我自己也不愿成为别人即使那个人比你更快乐。
—《成为简·奥斯汀》
代码及环境配置请参考 环境配置和代码运行! 4.5.1 概述
Dijkstra算法是基于广度优先搜索策略来遍历空间内的所有节点最终计算出全局最优的路径其计算量非常大。而基于启发式的贪婪最佳优先搜索greedy best first searchGBFS速度快但结果可能不是最优的。那么如何将二者的优势结合呢即在Dijkstra算法基础上引入启发式策略。这就是A*算法。 **Note**最佳优先搜索算法是在广度优先搜索的基础上用启发估价函数对将要被遍历到的点进行估价然后选择代价小的进行遍历直到找到目标节点或者遍历完所有点算法结束。
4.5.2 算法详解
A*算法结合了GBFS算法和Dijkstra算法的优点通过评估函数 f ( n ) f(n) f(n)来指导搜索过程 f ( n ) f(n) f(n)定义为从起点到节点n 的实际代价 g ( n ) g(n) g(n)与从节点n 到目标节点的估计代价 h ( n ) h(n) h(n)之和即 f ( n ) g ( n ) h ( n ) f(n)g(n) h(n) f(n)g(n)h(n)。 g ( n ) g(n) g(n)从起点到节点n的实际代价通常是距离或消耗。 h ( n ) h(n) h(n)从节点n到目标节点的估计代价启发式函数。
**Note**为了确保 g ( n ) g(n) g(n)和 h ( n ) h(n) h(n)的相加有意义这两个值必须使用相同的衡量单位。
4.5.1.1 启发式函数
启发式函数可以控制A*的行为
一种极端情况如果 h ( n ) h(n) h(n)是0则只有 g ( n ) g(n) g(n)起作用此时A*演变成Dijkstra算法计算量增大但可以确保找到最优路径。如果 h ( n ) h(n) h(n)经常都比从节点n移动到目标节点的实际代价小或者相等则A可以保证能找到一条最优路径。 h ( n ) h(n) h(n)越小A扩展的节点越多计算量越大运行得越慢。如果 h ( n ) h(n) h(n)精确地等于从节点n移动到目标节点的代价则A*将会仅仅寻找最优路径而不扩展别的任何结点这会运行得非常快。尽管这不可能在所有情况下发生但仍可以在一些特殊情况下让它们精确地相等。如果 h ( n ) h(n) h(n)有时比从节点n移动到目标节点的实际代价高则A*不能保证找到一条最优路径但它运行得更快。另一种极端情况如果 h ( n ) h(n) h(n)比 g ( n ) g(n) g(n)大很多则只有 h ( n ) h(n) h(n)起作用A*演变成GBFS算法。
下图可以清晰的看出不同的 h ( n ) h(n) h(n)对算法的影响图中绿色的为起始点红色的为目标点 a 1 , b 0 a1, b0 a1,b0如图Fig.1此时A*算法变成了Dijkstra算法虽然可以得到最优解但是扩展了非常多的节点计算量很大。 a 0 , b 1 a0,b1 a0,b1如图Fig.2此时A*算法变成了GBFS算法计算量减少但可能找不到最优解。 a 1 , b 1 a1,b1 a1,b1如图Fig.3即为A*算法平衡了计算量和最优解之间的关系。
Note: 图中的算法展示来源于https://qiao.github.io/PathFinding.js/visual/ 经过分析可以看到在A*算法中启发式函数 h ( n ) h(n) h(n)扮演着及其重要的角色以下是一些常见的启发式函数类型
曼哈顿距离Manhattan Distance 曼哈顿距离是A*算法中常用的启发式函数之一。它计算的是两点在标准坐标系上的绝对轴距总和即只能沿着坐标轴或网格的边界移动的距离。对于网格地图曼哈顿距离非常适合因为它反映了实际移动中的限制如只能上下左右移动。公式 h ( n ) ∣ x n − x g o a l ∣ ∣ y n − y g o a l ∣ h(n)\left | x_{n} - x_{goal} \right | \left | y_{n} - y_{goal} \right | h(n)∣xn−xgoal∣∣yn−ygoal∣其中 ( x n , y n ) (x_{n},y_{n}) (xn,yn)和 ( x g o a l , y g o a l ) (x_{goal},y_{goal}) (xgoal,ygoal)分别是当前点和目标点的坐标。 欧几里得距离Euclidean Distance 欧几里得距离是两点之间的直线距离在平面直角坐标系中它可以通过勾股定理计算得到。虽然欧几里得距离在物理上更准确但在网格地图中由于只能沿网格线移动它可能不总是反映实际的最短路径。然而在某些情况下为了简化计算或适应特定需求也可以使用欧几里得距离作为启发式函数。公式 h ( n ) ( x n − x g o a l ) 2 ( y n − y g o a l 2 ) h(n)\sqrt{(x_{n} - x_{goal})^{2} (y_{n} - y_{goal}^{2} )} h(n)(xn−xgoal)2(yn−ygoal2) 切比雪夫距离Chebyshev Distance 切比雪夫距离是各坐标数值差的最大值也称为棋盘距离或 L ∞ L\infty L∞ 度量。在网格地图中它表示从一个点到另一个点所需改变的最大坐标值即沿任一坐标轴移动的最大步数。虽然不如曼哈顿距离常用但在某些特定场景下切比雪夫距离也能作为有效的启发式函数。公式 h ( n ) m a x ( ∣ x n − x g o a l ∣ , ∣ y n − y g o a l ∣ ) h(n)max(\left | x_{n} - x_{goal} \right |, \left | y_{n} - y_{goal} \right |) h(n)max(∣xn−xgoal∣,∣yn−ygoal∣) 自定义启发式函数 除了上述常见的启发式函数外还可以根据具体问题设计自定义的启发式函数。自定义启发式函数可以更加精确地反映问题的实际情况从而提高搜索效率和准确性。然而设计有效的自定义启发式函数需要深入了解问题的本质和特性。
4.5.1.2 算法步骤
其算法步骤如下
初始化 创建一个数组g[]其中g[i]表示从源节点start到节点i的最小cost初始时源节点的cost为0。创建一个数组f[] 其中f[i] 表示从源节点start到节点i的最小cost 节点i 到目标节点的启发式cost。创建一个布尔数组 visited[] 来跟踪每个节点是否已被访问过初始时所有顶点都未访问创建一个优先队列open_list用于根据cost选择下一个要处理的节点。优先队列中的每个元素是一个包含顶点和cost的配对初始时将源节点start和其f cost加入优先队列。 循环处理优先队列中的节点 从优先队列中取出f cost最小的节点v 并将节点v 标记为已访问若节点v就是目标节点则提前返回。遍历节点v 中所有未被访问过的邻接节点对于每一个邻接节点next 计算邻接节点next 的g cost和h cost。若邻接节点next 不在优先队列中则将邻接节点next 和其对应的f cost加入优先队列并在数组g[] 和f[]中设置分别设置邻接节点next 的g cost和f cost最后将临接节点next 的父节点设为v 。若邻接节点next 在优先队列中则在数组g[]f[]和优先队列open_list中更新节点next 的相关信息最后将临接节点next 的父节点设为v 。 继续处理优先队列中的顶点直到队列为空或所有顶点都被访问。 从目标节点goal开始通过父节点向回追溯直到起始节点最终得到一条路径。
其伪代码如下
Algorithm A star(G, start, goal):let **open_list** be a priority_queueg[**start**] 0f[**start**] g[**start**] h[**start**]**open_list.**push(**start,** f[**start**])while **open_list** is not empty dov **open_list**.pop()mark v as vistedif v is the **goal**:return vfor **all unvisited neighbours** next of v in Graph G do next_g_cost g[v] cost(v, next)next_h_cost h[next]if next is not in **open_list**:g[next] next_g_costf[next] next_g_cost next_h_cost**open_list**.push(next, f[next])next.parent velse:if g[next] next_g_cost:g[next] next_g_costf[next] next_g_cost next_h_cost**open_list**.**update_priority**(next, f[next])next.parent v4.5.1.3 算法图解
以从下面的无向图中寻找出节点A 到节点E的最短路径为例其中两个节点之间的权重表示他们之间的距离或者cost节点旁边的数字表示预定义的启发项并初始话两个表visited nodes info和unvisited nodes info。
所有节点的初始cost如下
源节点到自身的g-cost为0到目标节点的h-cost为6因此源节点的f-cost为6。源节点到其它节点的cost还没有确定暂时先标记为无穷大。
从源节点A 开始将其加入到已访问列表中并从未访问列表中删除。同时在未访问列表中更新源节点A 的邻近节点节点B和节点C的信息。 节点B 节点B最短路径为A — B g-cost为1.5h-cost为5f-cost为6.5父节点为A节点C 节点C最短路径为A — C g-cost为2h-cost为3f-cost为5父节点为A 查看未访问列表节点C 的cost最小因此将其加入到已访问列表中并从未访问列表中删除。同时在未访问列表中更新节点C 的邻近节点节点E的信息。 节点E节点E最短路径为A — C — E g-cost为5h-cost为0f-cost为5父节点为C。 查看未访问列表节点E的cost最小因此将其加入到已访问列表中并从未访问列表中删除此时节点E 为目标节点因此搜索结束经过回溯父节点节点A到节点E最短路径为A — C — E 。 4.5.3 优缺点
优点 高效性A*算法结合了最佳优先搜索Best-First Search和Dijkstra算法的优点通过启发式函数heuristic function来评估节点的优先级从而能够快速找到从起点到终点的最短路径。最优性在启发式函数满足一定条件如 h ( n ) h(n) h(n)始终小于等于节点 n n n到目标节点的实际代价的情况下A*算法能够保证找到从起点到终点的最短路径。 缺点 启发式函数的选择虽然A*算法允许选择不同的启发式函数但启发式函数的选择会直接影响算法的性能和结果。如果启发式函数选择不当可能会导致算法无法找到最短路径或搜索效率降低。
4.5.c A*代码解析
本节提供了A*算法的代码测试
python3 tests/search_based_planning/a_star_test.py4.5.c.1 构图的代码实现
基于图搜的运动规划中最重要的一步是构图构建的图比较简单主要包含map border和obstacles读者也可根据需求修改构图方式。
def construct_env_info():border_x []border_y []ox []oy []# map border.for i in range(-10, 60):border_x.append(i)border_y.append(-10.0)for i in range(-10, 60):border_x.append(60.0)border_y.append(i)for i in range(-10, 61):border_x.append(i)border_y.append(60.0)for i in range(-10, 61):border_x.append(-10.0)border_y.append(i)# Obstacle 1.for i in range(40, 55, 1):for j in range(5, 15, 1):ox.append(i)oy.append(j)# Obstacle 2.for i in range(40):for j in range(20, 25, 1):ox.append(j)oy.append(i)# Obstacle 3.for i in range(30):for j in range(40, 45, 1):ox.append(j)oy.append(58.0 - i)# Obstacle 4.for i in range(0, 20, 1):for j in range(35, 40, 1):ox.append(i)oy.append(j)return border_x, border_y, ox, oy4.5.c.2 A*的代码实现
在A*算法中首先我们定义了节点Node 这是图的基础元素。其中x, y表示节点的位置cost表示从源节点到当前节点的costparent 表示当前节点的父节点。
class Node:def __init__(self, x, y, cost, parent_index):self.x x self.y y self.cost costself.parent_index parent_indexdef __str__(self):return (str(self.x) , str(self.y) , str(self.cost) , str(self.parent_index))在A*中启发式函数使用的是欧式距离如下
def calc_heuristic(n1, n2):w 1.0 # weight of heuristicd w * math.hypot(n1.x - n2.x, n1.y - n2.y)return d下面是A*的核心算法基于环境信息起始点位置目标点位置搜到最优路径其中
sx:起始点的x坐标的值
sy:起始点的y坐标的值
gx:目标点的x坐标的值
gy:目标点的y坐标的值
最终返回一条路径rx, ry
def planning(self, sx, sy, gx, gy):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_nodewhile True:if len(open_set) 0:print(Open set is empty..)breakc_id min(open_set,keylambda o: open_set[o].cost self.calc_heuristic(goal_node, open_set[o]),)current open_set[c_id]# show graphif show_animation: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)plt.savefig(gif_creator.get_image_path())if current.x goal_node.x and current.y goal_node.y:print(Find goal)goal_node.parent_index current.parent_indexgoal_node.cost current.costbreak# Remove the item from the open setdel open_set[c_id]# Add it to the closed setclosed_set[c_id] current# expand_grid search grid based on motion modelfor 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 nothingif not self.verify_node(node):continueif n_id in closed_set:continueif n_id not in open_set:open_set[n_id] node # discovered a new nodeelse:if open_set[n_id].cost node.cost:# This path is the best until now. record itopen_set[n_id] noderx, ry self.calc_final_path(goal_node, closed_set)return rx, ry4.5.c.3 A*的代码测试
在main 函数中设置起始点目标点grid的分辨率和机器人的半径在创建grid map之后并运行A算法即可找到最优路径。如下图所示红色路径即为最终A搜出来的最优路径。
def main():# start and goal positionstart_x 10.0 # [m]start_y 10.0 # [m]goal_x 50.0 # [m]goal_y 0.0 # [m]grid_size 2.0 # [m]robot_radius 1.0 # [m]# construct environment info.border_x, border_y, ox, oy construct_env_info()if show_animation: plt.plot(border_x, border_y, s, color(0.5, 0.5, 0.5), markersize10)plt.plot(ox, oy, s, colork)plt.plot(start_x, start_y, og, markersize10)plt.plot(goal_x, goal_y, ob, markersize10)plt.grid(True)plt.axis(equal)ox.extend(border_x)oy.extend(border_y)a_star AStarPlanner(ox, oy, grid_size, robot_radius)rx, ry a_star.planning(start_x, start_y, goal_x, goal_y)if show_animation: plt.plot(rx, ry, -r)plt.savefig(gif_creator.get_image_path())plt.pause(0.01)gif_creator.create_gif()plt.show()4.5.4 A*算法的变种
A*算法有很多的变种此处不在详细介绍感兴趣者可参考以下链接。
Anytime A*Block A*D*Field D*FringeFringe Saving A (FSA*)*Generalized Adaptive A (GAA*)*Incremental heuristic searchIterative deepening A (IDA*)*Jump point searchLifelong Planning A (LPA*)*Simplified Memory bounded A (SMA*)*Theta*
4.5.5 参考
https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode
推荐阅读
端到端理论与实战动手学轨迹预测动手学运动规划动手学行为决策强化学习入门笔记
