A, Dijkstra algorithm
Dijkstra's algorithm from the initial point of the start object is located, access nodes in FIG. It checks the iteration check node to be centralized node, and the closest node and the check node has not been added to the node to be examined is set. The node set extend outwardly from the initial node, until it reaches the destination node. Dijkstra's algorithm guaranteed to find a shortest path from the initial point to the target point, as long as all sides have a non-negative cost values.
1.1 algorithm principle and effect diagram
Dijkstra's algorithm using the greedy algorithm thought to solve the problem can be described as: In the undirected graph G = (V, E), assuming each edge E [i] of length w [i], to find the remaining vertex vs the shortest path to each point.
When calculating the shortest path by Dijkstra G in FIG VS specify the starting point (i.e., counted from the vertex vs). In addition, the introduction of two sets S and U. S has the effect of recording the shortest path calculated vertex (and the corresponding path length), and U is the apex of the recording has not been determined (the vertex and the distance to the beginning vs) of the shortest path. Initial, S vs, only a starting point; vs U is other than the apex, and the path is vertices U "path to the starting point of the vertex vs." Then, find the shortest path from vertex U, and added to the S; Next, update the path corresponding to the vertex and vertices in U. Then, find the shortest path from a vertex in U, S, and added to; Next, update the path corresponding to the vertex and vertices in U. This operation is repeated until all the vertices been traversed.
1.2 source code analysis
Source from: https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/Dijkstra/dijkstra.py
The main code algorithm analysis:
DEF dijkstra_planning (SX, SY, GX, Gy, OX, Oy, Reso, RR): "" "
SX: Start position X [m]
SY: Start position Y [m] Starting point # gx: goal x position [m ] Gy: goal position Y [m] # a target point coordinates OX: List of obstacles position X [m] Oy: List of obstacles position Y [m] # disorders coordinates reso: grid resolution [m] # resolution grid map rr: robot radius [m] # robot radius "" " Nstart = the Node (round (SX / Reso), round (SY / Reso), 0.0, -1 ) # map resolution with the addition, after rounding the normalized coordinates ngoal = the Node (round (GX / Reso), round (Gy / Reso), 0.0, -1 ) OX = [IOX / Resofor IOX in OX] Oy = [ioy / Reso for ioy in Oy] obmap, minX, minY, maxx, Maxy, XW, YW = calc_obstacle_map (OX, Oy, Reso, rr) # obstacle map range calculation, save Motion = get_motion_model () # node motion vector 8 corresponding set of eight movement directions (X, Y, cost) openset, closedset = dict (), dict () # Create openset and closedset, were stored not found and found waypoints openset [calc_index (Nstart, XW, minX, minY)] = Nstart the while . 1 : c_id = min (openset, Key = the lambda O: openset [O] .cost) # Openlist find the nearest point from the starting point Current = openset [c_id] # draw, draw current waypoint IF show_animation: plt.plot (current.x * Reso, current.y * Reso, " XC " ) IF len (closedset .keys ())% 10 == 0: plt.pause ( 0.001 )
# If the destination point, the end of the cycle IF current.x == ngoal.x and current.y == ngoal.y: Print ( " the Find. goal " ) ngoal.pind = current.pind ngoal.cost =current.cost BREAK # will meet the requirements of waypoints out openList del openset [c_id] # The qualified path point add closedlist closedset [c_id] = Current # based on motion vectors, a search range expanded for I, _ in the enumerate (Motion ): Node = the Node (current.x Motion + [I] [0], current.y Motion + [I] [. 1 ], current.cost + Motion [I] [2 ], c_id) N_ID = calc_index (Node, XW, minX, minY)
# If the new path point does not meet the requirements, the end of this cycle IF notverify_node (the Node, obmap, minX, minY, maxx, Maxy): the Continue
# If the new path point is already in closedlist, the end of this cycle IF N_ID in closedset: the Continue # If the new route point in openlist, the IF N_ID in openset: IF openset [N_ID] .cost> node.cost: openset [N_ID] .cost = node.cost openset [N_ID] .pind = c_id effect #pind like pointer, a point to the current node, convenience and finally a route back the else : openset [N_ID] = Node # node to add a new path in openlist rx, ry = calc_final_path (ngoal, closedset, Reso) # rx, ry array, the storage nodes satisfying the condition of the path return RX, Ry
Two, A * algorithm
BFS algorithm runs Dijkstra algorithm in accordance with a similar process, except that it is possible to assess the cost of any node reaches the target point. And Dijkstra's algorithm selects the nearest node different from the initial node, it selects the nearest node from the target. BFS algorithm can not be guaranteed to find a shortest path, but it .A faster than the speed * Dijkstra algorithm is a combination of BFS algorithm and Dijkstra's algorithm has the characteristics of heuristics, and can guaranteed to find a shortest path.
2.1 algorithm principle and effect diagram
-
Heuristic search elements : heuristic search is to search in the state space to evaluate each position of the search, the best position, then from this location until the target search. Such a large number may be omitted Intrepid search path, he referred efficiency. In the heuristic search, the valuation of the location is very important. Using different valuation can have different effects
- Evaluation function (cost): current mobile node to the destination node from the estimated cost; this estimate is heuristic. In pathfinding issues and problems in the maze, the estimated cost of the evaluation function normally with Manhattan (manhattan)
- : start starting point for route planning
- : goal end of the path planning
- g_score: from the current point (current_node) to the starting point (start) the cost of moving
- h_score: not considering obstacles, from the current point (current_node) to the target point estimate of the cost of moving
- f_score：f_score=g_score+h_score
- openlist: pathfinding process node list to be searched
- closelist: you do not need a list of nodes retrieved again
- comaFrom: a list of nodes storing parent-child relationship, used to back the optimal path, not necessarily, may be realized by the node pointer
2.2 source code analysis
Source from: https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/AStar/a_star.py
The main contents of the code and Dijkstra Similarly, increasing the Euclidean distance based calc_heuristic () function:
def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr): """ gx: goal x position [m] gy: goal y position [m] ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] reso: grid resolution [m] rr: robot radius[m] """ nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1) ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) ox = [iox / reso for iox in ox] oy = [ioy / reso for ioy in oy] obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr) motion = get_motion_model() openset, closedset = dict(), dict() openset[calc_index(nstart, xw, minx, miny)] = nstart while 1: c_id = min(openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o])) #对应于F=G+H current = openset[c_id] # show graph if show_animation: # pragma: no cover plt.plot(current.x * reso, current.y * reso, "xc") if len(closedset.keys()) % 10 == 0: plt.pause(0.001) if current.x == ngoal.x and current.y == ngoal.y: print("Find goal") ngoal.pind = current.pind ngoal.cost = current.cost break # Remove the item from the open set del openset[c_id] # Add it to the closed set closedset[c_id] = current # expand search grid based on motion model for i, _ in enumerate(motion): node = Node(current.x + motion[i][0], current.y + motion[i][1], current.cost + motion[i][2], c_id) n_id = calc_index(node, xw, minx, miny) if n_id in closedset: continue if not verify_node(node, obmap, minx, miny, maxx, maxy): continue if n_id not in openset: openset[n_id] = node # Discover a new node else: if openset[n_id].cost >= node.cost: # This path is the best until now. record it! openset[n_id] = node rx, ry = calc_final_path(ngoal, closedset, reso) return rx, ry
Increased calc_heuristic () function:
DEF calc_heuristic (N1, N2): W = 1.0 # weight of heuristic (H weight weight function) D = W * Math.sqrt ((n1.x - n2.x) ** 2 + (n1.y - n2.y ) ** 2 ) # Euclidean distance calculating return D
2.3 grid map of heuristics
- Manhattan Distance:
Standard heuristic Manhattan distance (Manhattan distance). Consider your move and find the cost function from a position adjacent to the position of the minimum cost D. Thus, the map heuristic should Manhattan distance D times, used only in case of moving around on the map of:
H(n) = D * (abs ( n.x – goal.x ) + abs ( n.y – goal.y ) )
- Diagonal distance:
If your map you allow diagonal movement then you need a different heuristic function. (4 east, 4 north) Manhattan distance becomes 8 * D. However, you can simply move (4 northeast) in place, so the heuristic function should be 4 * D. This function uses the diagonal, and the cost is assumed that the straight line is diagonal D:
h(n) = D * max(abs(n.x - goal.x), abs(n.y - goal.y))
- Euclidean distance:
If your units can move (instead of a grid direction) along any angle, then you should probably use the straight-line distance:
h (n) = D * sqrt ((nx-goal.x) ^ 2 + (ny-goal.y) ^ 2)
However, if this is the case, direct use will have trouble when A *, because the cost function g does not match heuristic function h. Because the Euclidean distance is shorter than the distance of Manhattan and diagonal distance, you can still get the shortest route, but the A * will run a lot longer:
- Euclidean distance squared:
I have seen some of the pages of A *, which refers to let you by using the square of the distance and the Euclidean distance to avoid expensive square root operation:
h (n) = D * ((nx-goal.x) ^ 2 + (ny-goal.y) ^ 2)
Do not do this! This obviously causes problems denominations. When calculating A * f (n) = g (n) + h (n), it is much larger than the square of the distance g price, and you will because heuristic evaluation value is too high and stops. For longer distances, this will close the extreme case g (n) is calculated no longer anything, A * degraded into BFS:
Third, the set of selected data structure