python 实现A*算法的示例代码_Python

A*作为最常用的路径搜索算法，值得我们去深刻的研究。路径规划项目。先看一下维基百科给的算法解释：https://en.wikipedia.org/wiki/A*_search_algorithm

A *是最佳优先搜索它通过在解决方案的所有可能路径（目标）中搜索导致成本最小（行进距离最短，时间最短等）的问题来解决问题。），并且在这些路径中，它首先考虑那些似乎最快速地引导到解决方案的路径。它是根据加权图制定的：从图的特定节点开始，它构造从该节点开始的路径树，一次一步地扩展路径，直到其一个路径在预定目标节点处结束。

在其主循环的每次迭代中，A *需要确定将其部分路径中的哪些扩展为一个或多个更长的路径。它是基于成本（总重量）的估计仍然到达目标节点。具体而言，A *选择最小化的路径

F（N）= G（N）+ H（n）

其中n是路径上的最后一个节点，g（n）是从起始节点到n的路径的开销，h（n）是一个启发式，用于估计从n到目标的最便宜路径的开销。启发式是特定于问题的。为了找到实际最短路径的算法，启发函数必须是可接受的，这意味着它永远不会高估实际成本到达最近的目标节点。

维基百科给出的伪代码：

									function A*(start, goal)

									  // The set of nodes already evaluated

									  closedSet := {}

									  // The set of currently discovered nodes that are not evaluated yet.

									  // Initially, only the start node is known.

									  openSet := {start}

									  // For each node, which node it can most efficiently be reached from.

									  // If a node can be reached from many nodes, cameFrom will eventually contain the

									  // most efficient previous step.

									  cameFrom := an empty map

									  // For each node, the cost of getting from the start node to that node.

									  gScore := map with default value of Infinity

									  // The cost of going from start to start is zero.

									  gScore[start] := 0

									  // For each node, the total cost of getting from the start node to the goal

									  // by passing by that node. That value is partly known, partly heuristic.

									  fScore := map with default value of Infinity

									  // For the first node, that value is completely heuristic.

									  fScore[start] := heuristic_cost_estimate(start, goal)

									  while openSet is not empty

									    current := the node in openSet having the lowest fScore[] value

									    if current = goal

									      return reconstruct_path(cameFrom, current)

									    openSet.Remove(current)

									    closedSet.Add(current)

									    for each neighbor of current

									      if neighbor in closedSet

									        continue // Ignore the neighbor which is already evaluated.

									      if neighbor not in openSet // Discover a new node

									        openSet.Add(neighbor)

									      // The distance from start to a neighbor

									      //the "dist_between" function may vary as per the solution requirements.

									      tentative_gScore := gScore[current] + dist_between(current, neighbor)

									      if tentative_gScore >= gScore[neighbor]

									        continue // This is not a better path.

									      // This path is the best until now. Record it!

									      cameFrom[neighbor] := current

									      gScore[neighbor] := tentative_gScore

									      fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal) 

									  return failure

									function reconstruct_path(cameFrom, current)

									  total_path := {current}

									  while current in cameFrom.Keys:

									    current := cameFrom[current]

									    total_path.append(current)

									  return total_path

下面是UDACITY课程中路径规划项目，结合上面的伪代码，用python 实现A*

									import math

									def shortest_path(M,start,goal):

									  sx=M.intersections[start][0]

									  sy=M.intersections[start][1]

									  gx=M.intersections[goal][0]

									  gy=M.intersections[goal][1] 

									  h=math.sqrt((sx-gx)*(sx-gx)+(sy-gy)*(sy-gy))

									  closedSet=set()

									  openSet=set()

									  openSet.add(start)

									  gScore={}

									  gScore[start]=0

									  fScore={}

									  fScore[start]=h

									  cameFrom={}

									  sumg=0

									  NEW=0

									  BOOL=False

									  while len(openSet)!=0: 

									    MAX=1000

									    for new in openSet:

									      print("new",new)

									      if fScore[new]<MAX:

									        MAX=fScore[new]

									        #print("MAX=",MAX)

									        NEW=new

									    current=NEW

									    print("current=",current)

									    if current==goal:

									      return reconstruct_path(cameFrom,current)

									    openSet.remove(current)

									    closedSet.add(current)

									    #dafult=M.roads(current)

									    for neighbor in M.roads[current]:

									      BOOL=False

									      print("key=",neighbor)

									      a={neighbor}

									      if len(a&closedSet)>0:

									        continue

									      print("key is not in closeSet")

									      if len(a&openSet)==0:

									        openSet.add(neighbor)  

									      else:

									        BOOL=True

									      x= M.intersections[current][0]

									      y= M.intersections[current][1]

									      x1=M.intersections[neighbor][0]

									      y1=M.intersections[neighbor][1]

									      g=math.sqrt((x-x1)*(x-x1)+(y-y1)*(y-y1))

									      h=math.sqrt((x1-gx)*(x1-gx)+(y1-gy)*(y1-gy)) 

									      new_gScore=gScore[current]+g

									      if BOOL==True:

									        if new_gScore>=gScore[neighbor]:

									          continue

									      print("new_gScore",new_gScore) 

									      cameFrom[neighbor]=current

									      gScore[neighbor]=new_gScore     

									      fScore[neighbor] = new_gScore+h

									      print("fScore",neighbor,"is",new_gScore+h)

									      print("fScore=",new_gScore+h)

									    print("__________++--------------++_________")

									def reconstruct_path(cameFrom,current):

									  print("已到达lllll")

									  total_path=[]

									  total_path.append(current)

									  for key,value in cameFrom.items():

									    print("key",key,":","value",value)

									  while current in cameFrom.keys():

									    current=cameFrom[current]

									    total_path.append(current)

									  total_path=list(reversed(total_path))  

									  return total_path