Gridless A* path finding algorithm. Problem with cost function

user_13998076 0
Viewed 419

The problem I have been attempting to solve is the path finding from a given position to a given goal for a dubins car (no backwards motion, constant velocity) with obstacles. I've attempted to implement a gridless A* algorithm with some simple obstacle avoidance. I expected the generated path to head straight towards the goal, and make minor adjustments to drive around the obstacles that it found. However, as soon as obstacles are introduced to the map, the path instead seems to get stuck at local minimum points of the algorithm's cost function.

The cost function I've implemented is the following:

f(x) = c(x) + g(x)

where c(x) is total travel cost, namely the cumulative cost of moving from node i-1 to i. Also, g(x) is the cost of the optimal path from the current node to the end goal, which becomes a straight line as it ignores obstacles.

The cost is used as a priority value in a min heap, where each iteration pops the minimum node and generates children nodes. As the children are generated, it is controlled that they are not out of bounds, have not already been visited and are not inside an obstacle. If these controls return false, then the child is added to the heap.

I've attempted introducing a weighting factor k * g(x) to the path cost, hoping that this would "incentivize" the algorithm to move towards the goal instead of getting stuck at a point. However, this merely shifted the minimum point to another location, but still resulted in getting stuck.

I will include my code implementation of the A* algorithm below:

# Description: Pathfinding algorithm, iteratively generates new neighbouring
# nodes and selects the cheapest of these through utilizing a min heap. 
# In: Car class object, a node as starting point.
# Out: The finishing node, with attached parent pointers.
def Astar(car, current):
    minHeap = [] #initialize heap as list
    h.heappush(minHeap, current) #push initial node onto heap
    heapCount = 1 #add upon pushes to heap, subtract upon pop
    # Iterate through nodes in priority queue
    while not ((goal(car, current)) or heapCount == 0):
        current = h.heappop(minHeap)
        heapCount -= 1
        for phi in [-m.pi/4, 0, m.pi/4]: #Full turns or straight are optimal, according to Pontryagins maximum principle
            #calculate new values for each phi (steering angle)
            xn, yn, thetan = step(car, current.x, current.y, current.theta, phi)
            #control feasibility of position
            if validCheck(car, xn, yn, current):
                #calculate costs for these directives
                costC = current.travelled + m.hypot(current.x - xn, current.y - yn) #cost of travel from start position
                costG = m.hypot(car.xt - xn, car.yt - yn) #current optimal distance to goal
                totalCost = costC + costG
                #renew time stamp
                newTime = current.time + 0.01
                #create child from new data
                child = Node(xn, yn, thetan, phi, totalCost, costC, newTime, current)
                #push child onto heap
                h.heappush(minHeap, child)
                heapCount += 1
        
    return current

Note that car is a class which includes certain attributes:

  • x0 : float: initial x-position [m]
  • y0 : float: initial y-position [m]
  • xt : float: target x-position [m]
  • yt : float: target y-position [m]
  • xlb : float: minimum x-position [m]
  • xub : float: maximum x-position [m]
  • ylb : float: minimum y-position [m]
  • yub : float: maximum y-position [m]
  • obs : list: list of tuples for each obstacle obs[i]

It also includes a method step which can generate a new heading angle and position when given a steering angle and previous heading and position.

Any advice or help regarding this problem, why it is occurring and what I can do to improve the path finding would very much be appreciated.

path-finding a-star algorithm python
1 Answers

I don't have a solution ready, but an explanation what's going on and and maybe a hint what you can do.

Analysis

The A* algorithm is for graph searching and, given a decent cost function, can greatly reduce the search space when compared with uninformed strategies like BFS. But still, the size of the problem graph matters.

In your code, I see a time increment of 0.01, and I read that as a hint that you are doing very small steps from parent to child nodes. That surely makes sense, to most closely approximating a smooth, non-quantized movement. But at the same time, it results in a huge growth of your problem graph.

Without obstacles, A* will still handle that huge graph gracefully. It will postpone all deviations from the straight line, as their cost will be higher than the node on the straight line. Your heap will grow (have some debug output show you its size...), but most nodes will never be explored further.

With obstacles, the game changes drastically. Let's say, there's an obstacle so that the resulting best path is 1.00 units longer than the straight line. Then A* will explore all nonsense paths, starting from somewhere on the line from start to obstacle, arbitrarily turning left or right until these paths reach an additional length of 1.00. There will be lots of these useless paths, and A* gets stuck in exploring nonsense.

Suggestion

I'd have the A* operate on a higher level.

I guess your obstacles are polygons. So the resulting total path will either ignore an obstacle or touch it at one of its corners. The elements between the touching points will start at a touching point with some heading direction, consist of an initial full-turn part, then a straight part, and then a final full-turn part, and then arrive at the next touching point with some (different) heading (to be honest, I'm not absolutely sure that this turn-straight-turn pattern will really cover all possible situations). Given start and end points and the desired end heading of such an element, you can compute the parts using some geometry, and by the way, check for collisions.

You can't know in advance the optimum heading when passing some touching point, so you'd have to check all possible headings.

I'd take these elements as the steps to be explored by A*.

So, that's how I'd apply A* to your task:

  • To compute the children of a node, for all other polygon corners and all headings at that corner, compute the element from the parent corner to the other corner, resulting in the given heading there. Check if such an element is geometrically possible and does not collide with some obstacle.

  • As the cost function, accumulate the length travelled so far, and then add the shortest obstacle-ignoring path to the target. This can either be the straight Pythagorean distance, or a more elaborate distance, taking into account the neccessary initial turn from the current heading to facing the target.

user_82072280
Related