Path Planning Using A* Algorithm for a Mobile Robot

Dec. 31, 2018, 2:10 p.m.

A* is an informed search algorithm or a best first search meaning that it solves problems by searching among all possible paths to the solution (goal) for the one that incurs the smallest cost (least distance travelled, shortest time, etc.), and among these paths it first considers the ones that appear to lead most quickly to the solution. It is formulated in terms of weighted graphs: starting from a specific node of a graph, it constructs a tree of paths starting from that node, expanding paths one step at a time, until one of its paths ends at the predetermined goal node. At each iteration of its main loop, A* needs to determine which of its partial paths to expand into one or more longer paths. It does so based on an estimate of the cost (total weight) still to go to the goal node. Specifically, A* selects the path that minimizes where n is the last node on the path, g(n) is the cost of the path from the start node to n, and h(n) is a heuristic that estimates the cost of the cheapest path from n to the goal. The heuristic is problem-specific. For the algorithm to find the actual shortest path, the heuristic function must be admissible, meaning that it never overestimates the actual cost to get to the nearest goal node. Typical implementations of A* use a priority queue to perform the repeated selection of minimum (estimated) cost nodes to expand. This priority queue is known as the open set or fringe. At each step of the algorithm, the node with the lowest f(x) value is removed from the queue, the f and g values of its neighbours are updated accordingly, and these neighbours are added to the queue. The algorithm continues until a goal node has a lower f value than any node in the queue (or until the queue is empty). The f value of the goal is then the length of the shortest 12 path, since h at the goal is zero in an admissible heuristic. The algorithm described so far gives us only the length of the shortest path. To find the actual sequence of steps, the algorithm can be easily revised so that each node on the path keeps track of its predecessor. After this algorithm is run, the ending node will point to its predecessor, and so on, until some node's predecessor is the start node. As an example, when searching for the shortest route on a map, h(x) might represent the straight line distance to the goal, since that is physically the smallest possible distance between any two points. If the heuristic h satisfies the additional condition h(x) ≤ d(x, y) + h(y) for every edge (x, y) of the graph (where d denotes the length of that edge), then h is called monotone or consistence. In such a case, A* can be implemented more efficiently—roughly speaking, no node needs to be processed more than once (see closed set below)—and A* is equivalent to running Dijkstra’s algorithm with the reduced cost d'(x, y) = d(x, y) + h(y) − h(x). Therefore, • Uses Heuristic value to estimate the shortest path from source node to the destination node. • F(x) = G(x) + H(X) • Each node has 1. Node data: 1. H value 2. G value 3. F value 4. Parent node 2. Lists: 1. Open list: List of nodes that need to be checked 2. Closed list: List of node that has been checked

                        import math
import numpy as np

map1 = np.zeros((33,40), dtype = np.int)
row = range(0,33,1)
for i in row:
    map1[i][0] = 20000

for i in row:
    map1[i][39] = 20000
col = range(0,40,1)

for i in col:
    map1[0][i] = 20000

for i in col:
    map1[32][i] = 20000

row1 = range(0,27,1)
cvr  = range(4,17,1)
for i in row1:
    for j in cvr:
         map1[i][j] = 20000

row2 = range(0,27,1)
cvr1  = range(21,33,1)
for i in row2:
    for j in cvr1:
         map1[i][j] = 20000

row3 = range(0,27,1)
cvr2  = range(36,40,1)
for i in row3:
    for j in cvr2:
         map1[i][j] = 20000
place_map = np.asarray(map1, dtype = np.uint8)

def give_cord(x,y,cord):
    ittr = [-1,0,1]
    for i in ittr:
        for j in ittr:
            cord[i+1][j+1] = [x+i, y+j]
def heuristic_matrix(x,y,heur,cord):
    ittr = [-1,0,1]
    for i in ittr:
        for j in ittr:
            heur[i+1][j+1] = math.sqrt(((cord[i+1][j+1][0]-x)**2)+((cord[i+1][j+1][1]-y)**2))
def gain_matrix(x,y,xd,yd,gain,cord,path_traced):
    ittr = [-1,0,1]
    for i in ittr:
        for j in ittr:
            gain[i+1][j+1] = math.sqrt(((cord[i+1][j+1][0]-x)**2)+((cord[i+1][j+1][1]-y)**2))
    for i in ittr:
        for j in ittr:
            if cord[i+1][j+1] in path_traced:
                gain[i+1][j+1] = 20000

    if yd > y:
        gain[1][0] = 20000
    if yd < y:
        gain[1][2] = 20000

    return gain

def cost_matrix(cost,gain,heur):
    ittr = [-1,0,1]
    for i in ittr:
        for j in ittr:
            cost[i+1][j+1] = gain[i+1][j+1] + heur[i+1][j+1]
def final_cost_matrix(final_cost,cost,cord):
    ittr = [-1,0,1]
    for i in ittr:
        for j in ittr:
            final_cost[i+1][j+1] = cost[i+1][j+1] + place_map[cord[i+1][j+1][0]][cord[i+1][j+1][1]]
    return final_cost

def next_from_astar(xi,yi,xt,yt):
    cord = np.zeros((3,3), dtype=np.ndarray)
    heur = np.zeros((3,3), dtype = np.int)
    gain = np.zeros((3,3), dtype = np.int)
    cost = np.zeros((3,3), dtype = np.int)
    final_cost= np.zeros((3,3), dtype = np.int)
    gain = gain_matrix(xi,yi,xd,yd,gain,cord,path_traced)
    final_cost = np.asarray(final_cost_matrix(final_cost,cost,cord))
    next_index = list(np.unravel_index(final_cost.argmin(),final_cost.shape))
    next_index1= list(np.unravel_index(final_cost.argmin(),final_cost.shape))
    next_index2 = cord[next_index1[0]][next_index1[1]]
    return next_index2
xi = input("Enter the x co-ordinate of the current location : ")
yi = input("Enter the y co-ordinate of the current location : ")
xd = input("Enter the x co-ordinate of the goal location : ")
yd = input("Enter the y co-ordinate of the goal location : ")
path_traced = [[xi,yi]]
while True:
    path_traced = path_traced + [[xi,yi]]
    if ([xi,yi]==[xd-1,yd] or [xi,yi]==[xd,yd-1] or [xi,yi] == [xd+1,yd] or [xi,yi]==[xd,yd+1]):



New York

Embedded Systems

Dec. 30, 2018, 11:18 a.m.

New York

Internet of Things

Dec. 30, 2018, 11:19 a.m.

New York

ML in Computer Vision

Dec. 30, 2018, 11:20 a.m.



Need help?


Want to convey anything? Drop a Message.

Mumbai, India
Phone: +91 9555551613
Email: ayamamit@gmail.com

.iframeclass{ position: absolute; top: 0; width: 100%; } .iframecontainer{ position: relative; width: 100%; height: auto; padding-top: 61%; }