Sunday, October 10, 2010

Finding the shortest path between two points: An example of the A* algorithm in Python

Introduction:

One common challenge is finding the shortest or least expensive path between two points. This simple problem can represent many different engineering and information processing problems. One of the most familiar versions of this problem is finding the shortest or fastest way to travel through between two points on a map.A famous variant of this problem is the traveling salesman problem. To set up this kind of problem, a directed graph needs to be created. A directed graph defines the connection between nodes. It also defines the cost to travel between nodes.

Given an origin and a destination in the graph, there are many ways to find the shortest path (or least cost) path between the origin and destination. One way which is usually intractable is to do a brute force search. However, of the tractable ways to solve the problem, the A* algorithm can be fast and efficient if a few conditions are met: an approximate estimate of the cost to reach the destination from every point in the graph is available and this approximation is always equal to or less than the optimal cost to reach the destination. 


An Example Problem:

A shortest path problem has the goal of finding a path through a graph which costs the least. Consider the simple graph shown below. Each node is represented by a red circle. The ways to travel between the nodes (the edges, arcs, arrows, etc) are shown by arrows between the nodes. each node has a name which is called out. To make the graph interesting, there is no way to travel in one step between the pair of nodes in 002 and 003 and the other pair of 013 and 014. To keep the problem simple, the cost to travel on every edge is equal to the Euclidean distance along the edge.

graph

The Astar algorithm can find a route from an origin to a destination node. For this example, the goal is to find a minimum cost route from node 001 to node 015. The graph is stored as a dictionary which has an entry for each node. This entry has a dictionary which defined the x and y coordinates of the node along with a list of nodes which can be reached. Furthermore, the approximate cost from each node to the destination is defined as the Euclidean distance from a given node to the destination. This distance satisfies the requirements for a good approximation in this model because it will always be equal or less than the best distance that can be achieved following the edges.

This implementation of the Astar algorithm is almost identical to the algorithm outlined in Wikipedia.

Once the Astar algorithm solves for the shortest path, the shortest path is visualized by laying it on top of the graph.

route

One key result can easily be seen in this example. By looking carefully at the graph, it should be obvious that there are other routes which can travel between the origin and destination with  the same distance or cost. For some problems, there is not a single shortest path, there are potentially many paths which have the same cost. This algorithm will generate only one. There may be other routes which are the shortest path.


Code Example:

Download

def Astar(start,goal,dist_between,neighbor_nodes,heuristic_estimate_of_dist):
    closedset = set([])             # The set of nodes already evaluated.     
    openset   = set([start])    # The set of tentative nodes to be evaluated.
                                    # starts off containing initial node
    came_from = set([])             # The map of navigated nodes.
    
    g_score={start:0}             # Distance from start along optimal path.
    h_score={start:heuristic_estimate_of_dist(start,goal)}
    f_score={start:h_score[start]}  #Estimated total distance from start to goal through y.
    came_from={start:None}
    
    while len(openset)>0:   # open set is not empty
         #x := the node in openset having the lowest f_score[] value
        bestX = None
        for x in openset:
            if bestX==None:
                bestX = x
            elif f_score[x] < f_score[bestX]:
                bestX = x
        x = bestX
        if x == goal:
            return reconstruct_path(came_from,came_from[goal])
        openset.discard(x)
        closedset.add(x)
        
        neighbor_list = neighbor_nodes(x)
        for y in neighbor_nodes(x):
            if y in closedset:
                continue
            tentative_g_score = g_score[x] + dist_between(x,y)

            if y not in openset:
                openset.add(y)
                tentative_is_better = True
            elif tentative_g_score <  g_score[y]:
                tentative_is_better = True
            else:
                tentative_is_better = False
            if tentative_is_better == True:
                 came_from[y] = x
                 g_score[y] = tentative_g_score
                 h_score[y] = heuristic_estimate_of_dist(y, goal)
                 f_score[y] = g_score[y] + h_score[y]
    return None
 
def reconstruct_path(came_from, current_node):
     if not came_from[current_node] == None:
        p = reconstruct_path(came_from, came_from[current_node])
        return (p + [current_node])
     else:
        return [current_node]
    
    
    
#####################################

if __name__=='__main__':

    import pylab as p


    graph = {
        '001':dict(x=1, y=0,nextList=['002','011']),
#        '002':dict(x=2, y=0,nextList=['001','003','012']),
#        '003':dict(x=3, y=0,nextList=['002','004','013']),
        '002':dict(x=2, y=0,nextList=['001','012']),
        '003':dict(x=3, y=0,nextList=['004','013']),
        '004':dict(x=4, y=0,nextList=['003','005','014']),
        '005':dict(x=5, y=0,nextList=['004','015']),
        '011':dict(x=1, y=1,nextList=['012','001']),
        '012':dict(x=2, y=1,nextList=['011','013','002']),
#        '013':dict(x=3, y=1,nextList=['012','014','003']),
#        '014':dict(x=4, y=1,nextList=['013','015','004']),
        '013':dict(x=3, y=1,nextList=['012','003']),
        '014':dict(x=4, y=1,nextList=['015','004']),
        '015':dict(x=5, y=1,nextList=['014','005']),
        }
        
    def neighbor_nodes(x):
        return graph[x]['nextList']

    def distance_between(x,y):
        _x1 = graph[x]['x']
        _x2 = graph[y]['x']
        _y1 = graph[x]['y']
        _y2 = graph[y]['y']
        
        return ((_x1-_x2)**2+(_y1-_y2)**2)**(0.5)
        

    def drawArrow(xFrom,xTo,yFrom,yTo):
        length = ((xFrom-xTo)**2+(yFrom-yTo)**2)**(0.5)
        head_len = 0.1
        head_width = 0.05
        dx = ((length-head_len)/length)*(xTo-xFrom)
        dy = ((length-head_len)/length)*(yTo-yFrom)
        p.arrow(xFrom,yFrom,dx,dy,
                head_width=head_width,head_length=head_len)

    def plotGraph(graph,ax):
        first = True
        for origin in graph.keys():
            for dest in graph[origin]['nextList']:
                xFrom = graph[origin]['x']
                xTo   = graph[dest]['x']
                yFrom = graph[origin]['y']
                yTo   = graph[dest]['y']
                drawArrow(xFrom,xTo,yFrom,yTo)
                if first:
                   minX = xFrom
                   maxX = xFrom
                   minY = yFrom
                   maxY = yFrom
                   first = False
                else:
                   minX = min(minX,xFrom)
                   maxX = max(maxX,xFrom)
                   minY = min(minY,yFrom)
                   maxY = max(maxY,yFrom)
            p.plot([xFrom],[yFrom],'or')
            ax.annotate(origin, xy=(xFrom,yFrom),  xycoords='data',
                xytext=(-50, 30), textcoords='offset points',
                size=20,
                #bbox=dict(boxstyle="round", fc="0.8"),
                arrowprops=dict(arrowstyle="fancy",
                                fc="0.6", ec="none",
                #                patchB=el,
                                connectionstyle="angle3,angleA=0,angleB=-90"),
                )

        #p.axis([minX-0.25,maxX+0.25,minY-0.25,maxY+0.25])

    def plotRoute(route,graph):
        for idx,point in enumerate(route):
            if idx < len(route)-1:
                nextPoint = route[idx+1]
                xFrom = graph[point]['x']
                xTo   = graph[nextPoint]['x']
                yFrom = graph[point]['y']
                yTo   = graph[nextPoint]['y']
                p.plot([xFrom,xTo],[yFrom,yTo],'-g',lw=10,alpha=0.5,solid_capstyle='round')


    fig = p.figure()
    ax = fig.add_subplot(111, autoscale_on=False, xlim=(0,5.5), ylim=(-0.5,1.75))

    route = Astar('001','015',distance_between,neighbor_nodes,distance_between) 
    route.append('015')
    print route
    plotRoute(route,graph)
    plotGraph(graph,ax)
    print 'Done'
    p.show()


References:

No comments:

Post a Comment