成都犀牛网站建设公司,找南昌seo服务商,百度推广自己怎么做,公司名称注册查询系统基本概念
A*算法最早于1964年在IEEE Transactions on Systems Science and Cybernetics中的论文《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》中首次提出。其属于一种经典的启发式搜索方法#xff0c;所谓启发式搜索#xff0c;就在于当前搜索…基本概念
A*算法最早于1964年在IEEE Transactions on Systems Science and Cybernetics中的论文《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》中首次提出。其属于一种经典的启发式搜索方法所谓启发式搜索就在于当前搜索结点往下选择下一步结点时可以通过一个启发函数来进行选择选择代价最少的结点作为下一步搜索结点而跳转其上。
传统的算法中深度优先搜索(DFS)和广度优先搜索(BFS)在展开子结点时均属于盲目型搜索也就是说它不会选择哪个结点在下一次搜索中更优而去跳转到该结点进行下一步的搜索。在运气不好的情形中均需要试探完整个解集空间, 显然只能适用于问题规模不大的搜索问题中。而与DFS,BFS不同的是一个经过仔细设计的启发函数往往在很快的时间内就可得到一个搜索问题的最优解。
在原论文中A*算法的步骤设计如下
1、标记起点s为open计算起点的估计代价
2、选择open点集中估计代价最小的点
3、如果选中的点∈目标集合T即到达目标标记该点closed算法结束
4、否则还未到达目标标记该点closed对其周围直达的点计算估计代价如果周围直达的点未标记为closed将其标记为open如果已经标记为closed的如果重新计算出的估计代价比它原来的估计代价小更新估计代价并将其重新标记open。返回第2步。举个栗子
例1
如下图所示初始时起点位置为5需要前往位置20每条路径上的数值代表从该路径经过的代价值。 根据上述公式首先设置5号点为起点则它向下有两条路径前往2号点与前往7号点。两条路径的代价值分别为4跟5。则根据代价函数我们选择走代价值小一点的点即前往2号点 然后根据第三步判断是否到达终点可知2不是终点。跳转第四步标记该点位置同时计算它到周围点的代价值从2号点可以去7号点代价值为9可以去22号点代价值为16可以去18号点代价值为18可以去10号点代价值为6。然后调转回第二步。
根据第二步选择上述点中代价值最小的点也就是10号点 然后根据第三步判断是否到达终点可知10不是终点。跳转第四步标记该点位置同时计算它到周围点的代价值从10号点可以去66号点代价值为20。
根据第二步选择上述点中代价值最小的点也就是66号点 然后根据第三步判断是否到达终点可知66不是终点。跳转第四步标记该点位置同时计算它到周围点的代价值从66号点可以去20号点代价值为6。
根据第二步选择上述点中代价值最小的点也就是20号点 然后根据第三步判断是否到达终点可知20号是终点至此算法结束。得到最优路径5-2-10-66-20。可以看到A在搜索速度以及准确性上都是很高的明显优于DFS与BFS。但是注意到一个问题。与Dijkstra不同的是Dijkstra虽然基于BFS导致搜索速度比较低但是它的搜索结果一定是最优的。而A虽然能够快速找到一条路径但是它不一定是最优的。例如例2所示
例2
将例1中2到10的代价值从6改为10 此时按照上述方式得到的路径应该是 即通过5-2-18-66-20总代价值为42。但是实际上从5-20的最优路径应该是5-2-10-66-20总代价值为40。所以从这里我们也可以看出来A*虽然在搜索速度上比Dijkstra要快很多但是它的结果可能不是最优的。
A*与启发函数
看完了例子对于A* 算法也有了一个初步的了解。那么对于A* 算法而言它相对于其他算法的优点是什么呢深度优先搜索的好处是时间快但是很少能求出最优解而广度优先搜索确实可以求出最优解但由于广度优先搜索是一层层搜下去的必须扩展每一个点所以时间效率和空间效率都不高。而A* 算法恰可以解决这两个缺点既有极大概率求出最优解又可以减少冗余的时间。
那么对于A* 而言它的使用又有哪些需要注意的地方呢最关键的一点就是在于它的启发函数的确定了也就是确定上面例子中从一个点到另一个点的代价值。在原算法中定义了一个代价函数 f ( n ) g ( n ) h ( n ) f(n)g(n)h(n) f(n)g(n)h(n) 其中f[n]是 是从初始状态经由状态n到目标状态的代价估计g(n)是从初始到n的实际代价而h(n)是从n到目标状态的估计代价。
在g和h的估计中h的估计方法是比较宽泛的可以是直线距离也可以是其他如坐标和之类的但作者提到估计h必须要小于真实h才能保证算法是可接收到的即能找到最优解。如果估计h比真实h大则可能找到的的不是最优解。
那么设置h为0可以但就会变成类似Dijkstra算法会找到到达地图上各点的最优路线。最终一定也会到达目标集合T中的节点并标记closed结束算法但过程中会展开更多无谓的节点漫无目的地展开。所以A*算法中很重要的一点就在于这个h的选择。
A*算法与路径规划
A*在移动机器人的路径规划中使用的也非常广泛对于需要求出一条起点到终点的有效路径的情况下是一种非常合适的算法。
举个非常经典的例子 从图中的绿色方块运动到红色方块蓝色为障碍物。首先我们把地图栅格化把每一个方格的中心称为节点这个特殊的方法把我们的搜索区域简化为了 2 维数组。数组的每一项代表一个格子它的状态就是可走 (walkalbe) 和不可走 (unwalkable) 。通过计算出从 A 到 目标点需要走过哪些方格就找到了路径。一旦路径找到了便从一个方格的中心移动到另一个方格的中心直至到达目的地。 一旦我们把搜寻区域简化为一组可以量化的节点后我们下一步要做的便是查找最短路径。在 A* 中我们从起点开始检查其相邻的方格然后向四周扩展直至找到目标
1、从起点 A 开始定义A为父节点并把它加入到 openList中。现在 openList 里只有起点 A 后面会慢慢加入更多的项。 2、如上图所示父节点A周围共有8个节点定义为子节点。将子节点中可达的reachable或者可走的walkable放入openList中成为待考察对象。 3、若某个节点既未在openList也没在closeList中则表明还未搜索到该节点。 4、初始时 节点A离自身距离为0路径完全确定将其移入closeList中 closeList 中的每个方格都是现在不需要再关注的。 5、路径优劣判断依据是移动代价单步移动代价采取Manhattan 计算方式即 d ∣ x 1 − x 2 ∣ ∣ y 1 − y 2 ∣ d|x_1-x_2||y_1-y_2| d∣x1−x2∣∣y1−y2∣即把横向和纵向移动一个节点的代价定义为10。斜向移动代价为14 6、现在openList {B,C,D,E,F,G,H,I}, closeList {A}
下面我们需要去选择节点A相邻的子节点中移动代价 f 最小的节点下面以节点I的计算为例。
移动代价评价函数为 f ( n ) g ( n ) h ( n )。 f ( n )是从初始状态经由状态n到目标状态的代价估计 g ( n ) 是在状态空间中从初始状态到状态n的实际代价 h ( n ) 是从状态n到目标状态的最佳路径的估计代价。
首先考察 g由于从A到该格子是斜向移动单步移动距离为14故 g 14.
再考察估计代价 h。估计的含义是指忽略剩下的路径是否包含有障碍物不可走 完全按照Manhattan计算方式计算只做横向或纵向移动的累积代价横向向右移动3步纵向向上移动1步总共4步故为 h 40.
因此从A节点移动至I节点的总移动代价为 f g h 54
以此类推分别计算当前openList中余下的7个子节点的移动代价 f 从中挑选最小代价节点F移到closeList中。
现在openList {B,C,D,E,G,H,I}, closeList {A,F}
然后继续往下搜索
从openList中选择 f 值最小的 ( 方格 ) 节点I(D节点的 f值跟I相同任选其一即可不过从速度上考虑选择最后加入 openList 的方格更快。这导致了在寻路过程中当靠近目标时优先使用新找到的方格的偏好。 对相同数据的不同对待只会导致两种版本的 A* 找到等长的不同路径 )从 openList里取出放到 closeList 中。
检查所有与它相邻的子节点忽略不可走 (unwalkable) 的节点、以及忽略已经存在于closeList的节点如果方格不在openList中则把它们加入到 openList中并把它们作为节点I的子节点 。
如果某个相邻的节点(假设为X)已经在 opeLlist 中则检查这条路径是否更优也就是说经由当前节点( 我们选中的节点)到达节点X是否具有更小的 g 值。如果没有不做任何操作。否则如果 g 值更小则把X的父节点设为当前方格 然后重新计算X的 f 值和 g 值。
判断完所有子节点后现在openList {B,C,D,E,G,H,J,K,L}, closeList {A,F,I}
依次类推不断重复。一旦搜索到目标节点T完成路径搜索结束算法。
完成路径搜索后从终点开始向父节点移动这样就被带回到了起点这就是搜索后的路径。如下图所示。从起点 A 移动到终点 T就是简单从路径上的一个方格的中心移动到另一个方格的中心直至目标。
代码实现
import os
import sys
import math
import heapq
import matplotlib.pyplot as pltclass AStar:AStar set the cost heuristics as the prioritydef __init__(self, s_start, s_goal, heuristic_type,xI, xG):self.s_start s_startself.s_goal s_goalself.heuristic_type heuristic_typeself.u_set [(-1, 0), (0, 1), (1, 0), (0, -1)] # feasible input setself.obs self.obs_map() # position of obstaclesself.OPEN [] # priority queue / OPEN setself.CLOSED [] # CLOSED set / VISITED orderself.PARENT dict() # recorded parentself.g dict() # cost to comeself.x_range 51 # size of backgroundself.y_range 31self.xI, self.xG xI, xGself.obs self.obs_map()def update_obs(self, obs):self.obs obsdef animation(self, path, visited, name):self.plot_grid(name)self.plot_visited(visited)self.plot_path(path)plt.show()def plot_grid(self, name):obs_x [x[0] for x in self.obs]obs_y [x[1] for x in self.obs]plt.plot(self.xI[0], self.xI[1], bs)plt.plot(self.xG[0], self.xG[1], gs)plt.plot(obs_x, obs_y, sk)plt.title(name)plt.axis(equal)def plot_visited(self, visited, clgray):if self.xI in visited:visited.remove(self.xI)if self.xG in visited:visited.remove(self.xG)count 0for x in visited:count 1plt.plot(x[0], x[1], colorcl, markero)plt.gcf().canvas.mpl_connect(key_release_event,lambda event: [exit(0) if event.key escape else None])if count len(visited) / 3:length 20elif count len(visited) * 2 / 3:length 30else:length 40## length 15if count % length 0:plt.pause(0.001)plt.pause(0.01)def plot_path(self, path, clr, flagFalse):path_x [path[i][0] for i in range(len(path))]path_y [path[i][1] for i in range(len(path))]if not flag:plt.plot(path_x, path_y, linewidth3, colorr)else:plt.plot(path_x, path_y, linewidth3, colorcl)plt.plot(self.xI[0], self.xI[1], bs)plt.plot(self.xG[0], self.xG[1], gs)plt.pause(0.01)def update_obs(self, obs):self.obs obsdef obs_map(self):Initialize obstacles positions:return: map of obstaclesx 51y 31obs set()for i in range(x):obs.add((i, 0))for i in range(x):obs.add((i, y - 1))for i in range(y):obs.add((0, i))for i in range(y):obs.add((x - 1, i))for i in range(10, 21):obs.add((i, 15))for i in range(15):obs.add((20, i))for i in range(15, 30):obs.add((30, i))for i in range(16):obs.add((40, i))return obsdef searching(self):A_star Searching.:return: path, visited orderself.PARENT[self.s_start] self.s_startself.g[self.s_start] 0self.g[self.s_goal] math.infheapq.heappush(self.OPEN,(self.f_value(self.s_start), self.s_start))while self.OPEN:_, s heapq.heappop(self.OPEN)self.CLOSED.append(s)if s self.s_goal: # stop conditionbreakfor s_n in self.get_neighbor(s):new_cost self.g[s] self.cost(s, s_n)if s_n not in self.g:self.g[s_n] math.infif new_cost self.g[s_n]: # conditions for updating Costself.g[s_n] new_costself.PARENT[s_n] sheapq.heappush(self.OPEN, (self.f_value(s_n), s_n))return self.extract_path(self.PARENT), self.CLOSEDdef get_neighbor(self, s):find neighbors of state s that not in obstacles.:param s: state:return: neighborsreturn [(s[0] u[0], s[1] u[1]) for u in self.u_set]def cost(self, s_start, s_goal):Calculate Cost for this motion:param s_start: starting node:param s_goal: end node:return: Cost for this motion:note: Cost function could be more complicate!if self.is_collision(s_start, s_goal):return math.infreturn math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])def is_collision(self, s_start, s_end):check if the line segment (s_start, s_end) is collision.:param s_start: start node:param s_end: end node:return: True: is collision / False: not collisionif s_start in self.obs or s_end in self.obs:return Trueif s_start[0] ! s_end[0] and s_start[1] ! s_end[1]:if s_end[0] - s_start[0] s_start[1] - s_end[1]:s1 (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))s2 (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))else:s1 (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))s2 (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))if s1 in self.obs or s2 in self.obs:return Truereturn Falsedef f_value(self, s):f g h. (g: Cost to come, h: heuristic value):param s: current state:return: freturn self.g[s] self.heuristic(s)def extract_path(self, PARENT):Extract the path based on the PARENT set.:return: The planning pathpath [self.s_goal]s self.s_goalwhile True:s PARENT[s]path.append(s)if s self.s_start:breakreturn list(path)def heuristic(self, s):Calculate heuristic.:param s: current node (state):return: heuristic function valueheuristic_type self.heuristic_type # heuristic typegoal self.s_goal # goal nodeif heuristic_type manhattan:return abs(goal[0] - s[0]) abs(goal[1] - s[1])else:return math.hypot(goal[0] - s[0], goal[1] - s[1])def main():s_start (5, 5)s_goal (45, 25)astar AStar(s_start, s_goal, euclidean,s_start,s_goal)path, visited astar.searching()astar.animation(path, visited, A*) # animationif __name__ __main__:main()效果如下 简单解析一下
在获取起点后算法维护了两个字典 self.PARENT[self.s_start] self.s_startself.g[self.s_start] 0self.g[self.s_goal] math.infPARENT字典中存取的是这个点由哪个点延伸出来的即它的上一个节点是谁用于最后的路径的返回g这个字典中存储的是每个坐标的代价值即g[s]。
然后算法通过堆栈的概念维护了一个堆栈 heapq.heappush(self.OPEN,(self.f_value(self.s_start), self.s_start))将初始值的f[s]存储在这里然后进行while循环
首先从堆栈中取出栈顶元素
_, s heapq.heappop(self.OPEN)注意到这里使用的heapq堆栈功能中的两个函数heapq.heappop与heapq.heappush。heappop会取出栈顶元素并将原始数据从堆栈中删除而heappush则是对插入的数据按大小排序并存储在堆栈中。所以每一个遍历的点都会按照它的代价值放入堆栈中同时每次取出的都是代价值最小的那个。
然后判断出栈顶元素是否为目标点如果为目标点则退出 if s self.s_goal: # stop conditionbreak如果不是则更新该点附近点的代价值 for s_n in self.get_neighbor(s):for s_n in self.get_neighbor(s):new_cost self.g[s] self.cost(s, s_n)if s_n not in self.g:self.g[s_n] math.infif new_cost self.g[s_n]: # conditions for updating Costself.g[s_n] new_costself.PARENT[s_n] sheapq.heappush(self.OPEN, (self.f_value(s_n), s_n))get_neighbor为获取该点周围的点的坐标。heappush入栈时需要存储的该点的代价值的计算方式为 def f_value(self, s):f g h. (g: Cost to come, h: heuristic value):param s: current state:return: freturn self.g[s] self.heuristic(s)其中self.g[s]为A算法中的gself.heuristic(s)为A算法中的h。而作为一个启发函数h的计算可以根据实际情况进行选择例如在栅格地图中计算h的方式一般可以分为两种曼哈顿距离与欧几里德距离。
欧式距离是我们平时用的比较多的即求两点之间的直线长度 else:#sqrt(x^2y^2)return math.hypot(goal[0] - s[0], goal[1] - s[1])而曼哈顿距离相对不是那么普遍简单的来说就是找到一个当前点到终点的X方向需要走过的格子的数量以及Y方向需要走过的格子数量。 if heuristic_type manhattan:return abs(goal[0] - s[0]) abs(goal[1] - s[1])应用这两种不同的计算方式会得到不同的效果
采用曼哈顿距离计算的结果 采用欧式距离计算的结果 可以看到采用不同的h的方式遍历的结果会有一定的差异。除此之外在对周围点进行搜索的时候的点的选择也会对算法的遍历产生影响例如上面的代码中u_set设置为 self.u_set [(-1, 0), (0, 1), (1, 0), (0, -1)] # feasible input set即每个点只能搜索前后左右四个点。但是如果将其改为搜索8个点的话得到的搜索结果就会变成 至此A*算法的简单原理整理完毕。
参考
1、 十五个经典算法研究与总结1-A*搜索算法
2、【全局路径规划】A算法 A Search Algorithm
3、【路径规划】全局路径规划算法——A*算法含python实现 | c实现
4、 路径规划之 A* 算法
