Python implementation of A * algorithm

A * as the most commonly used path search algorithm, we should go deep research. Path Planning project. Let's look at the algorithm explained by Wikipedia: Https://en.wikipedia.org/wiki/a*_search_algorithm

A * Yes Best-in- priority search It solves the problem by searching through all possible paths (targets) in the solution to the problem that leads to the least cost (shortest travel distance, shortest time, etc.). ), and in these paths, it first considers the paths that appear to be the quickest to boot to the solution . It is based on a weighted graph : Starting with A specific node of the graph , which constructs a tree of paths starting from that node , extending the path one step at a time until one of its paths ends at the intended target node.

in each iteration of its main loop, a * needs to determine which of its part of the path expands to one or more longer paths. It is estimated to still reach the target node based on the cost (total weight). Specifically, A * Select the minimized path

where n is the last node on the path, g ( n ) from the start node to n The cost of the path , h ( n ) is a heuristic , Used to estimate the cost of the cheapest path from n to target . The heuristic is problem-specific. in order to find an algorithm for the actual shortest path, the heuristic function must be acceptable , This means that it never overestimated the actual cost to reach the nearest target node.

The pseudo-code given by Wikipedia:

functionA*(Start,Goal)The set of nodes already evaluatedClosedset:={}The set of currently discovered nodes that is not evaluated yet.Initially, only the start node is known.Openset:={Start}For each node, the which node it can most efficiently is reached from.If a node can be reached from many nodes, Camefrom'll eventually contain theMost efficient previous step.Camefrom:=AnEmptyMapFor each node, the cost of getting from the start node to that node.Gscore:=MapWithDefaultValueOfInfinityThe cost of going from start to start is zero.Gscore[Start]:=0For each node, the total cost of getting from the start node to the goalBy passing to that node. That value is partly known, partly heuristic.Fscore:=MapWithDefaultValueOfInfinityFor the first node, then value is completely heuristic.Fscore[Start]:=Heuristic_cost_estimate(Start,Goal)WhileOpensetIsNotEmptyCurrent:=TheNodeInchOpensetHavingTheLowestFscore[]ValueIfCurrent=GoalReturnReconstruct_path(Camefrom,Current)Openset.Remove(Current)Closedset.Add(Current)ForeachNeighborOfCurrentIfNeighborInchClosedsetContinueIgnore the neighbor which is already evaluated.IfNeighborNotInchOpensetDiscover A new nodeOpenset.Add(Neighbor)The distance from start to a neighborThe "Dist_between" function may vary as per the solution requirements.Tentative_gscore:=Gscore[Current]+Dist_between(Current,Neighbor)IfTentative_gscore>=Gscore[Neighbor]ContinueThis is a better path.This path was the best until now. Record it!Camefrom[Neighbor]:=CurrentGscore[Neighbor]:=Tentative_gscoreFscore[Neighbor]:=Gscore[Neighbor]+Heuristic_cost_estimate(Neighbor,Goal) return failurefunction  Reconstruct_path (camefromcurrent ) total_path := {current} while span class= "n" >current in camefrom. Keys: current := camefrom[current total_path. Append (current) return total_ Path                  

The following is a path planning project in the Udacity course, combined with the pseudocode above, to implement a * with Python

Import mathdef 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]=0Fscore={} Fscore[start]=h Camefrom={} SUMG=0NEW=0BOOL=False whileLen (openset)! =0: MAX= +         for New inchOpenset:print ("New",New)            iffscore[New]<Max:max=fscore[New] #print ("max=", MAX) NEW=New Current=NEW Print ("current=", current)ifcurrent==Goal:returnReconstruct_path (camefrom,current) Openset.remove (current), Closedset.add (current) #dafult=m.roads (current) forNeighborinchM.roads[current]: BOOL=False Print ("key=", neighbor) a={neighbor}ifLen (A&closedset) >0:                ContinuePrint ("key is not in CloseSet")            ifLen (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) * (yy1)) H=math.sqrt ((X1-GX) * (X1-GX) + (y1-gy) * (y1-Gy)) New_gscore=gscore[current]+gifbool==True:ifnew_gscore>=Gscore[neighbor]:ContinuePrint ("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 ( "reached Lllll") Total_path=[] total_path.append (current) forKey,valueinchcamefrom.items (): Print ("Key", Key,":","value", value) whileCurrentinchCamefrom.keys (): current=Camefrom[current] Total_path.append (current) Total_path=list (Reversed (Total_path))returnTotal_path

Python implementation of A * algorithm