Why doesn't my a* algorithm take the shortest route?

Viewed 2376

My a* algorithm doesn't always take the shortest path.

In this image the robot has to traverse to the black squares, the river and trees are obstacles. The black line is the path it took, which is clearly not the shortest path as it should not have dipped.

http://imgur.com/GBoq9py

Here is my code for a* and the heuristic I am using:

def HeuristicCostEstimate(start, goal):
    (x1, y1) = start
    (x2, y2) = goal
    return abs(x1 - x2) + abs(y1 - y2)

def AStar(grid, start, goal):
    entry = 1
    openSet = []
    heappush(openSet,(1, entry, start))
    cameFrom = {}
    currentCost = {}
    cameFrom[tuple(start)] = None
    currentCost[tuple(start)] = 0
    while not openSet == []:
        current = heappop(openSet)[2]
        print(current)
        if current == goal:
            break

        for next in grid.Neighbours(current):
            newCost = currentCost[tuple(current)] + grid.Cost(current, next)
            if tuple(next) not in currentCost or newCost < currentCost[tuple(next)]:
                currentCost[tuple(next)] = newCost
                priority = newCost + HeuristicCostEstimate(goal, next)
                entry +=1
                heappush(openSet,(priority, entry, next))
                cameFrom[tuple(next)] = current

    return cameFrom, current

http://pastebin.com/bEw8x0Lx

Thanks for any help! And feel free to ask me to clarify anything.

Edit: removing the heuristic by returning 0 solves this problem. Which suggests the problem lies with my heuristic. Would anyone know what might be causing it?

2 Answers

Change your heuristic to (x1-x2)^2 + (y1-y2)^2 and you should find it generates a more accurate path.

Related