Risk-Aware Path Planning with A-Star

Interactive Planner

The blue dot is the "nest" (starting location), the red dots are "sites" (destinations). The dark blue zone is the Keep Out zone and cannot be entered under any circumstance; the lighter blue/green is the High Risk zone described below, and entering it must be balanced with trying to get to sites quickly. Feel free to mess with the High Risk Multiplier and Max Range, and see how the paths change! Note: the High Risk Multiplier is fairly non-linear, and some trajectories will change drastically between values of 0 and 0.3, while others take up until 10 to avoid the High Risk zone properly. Enjoy!

Context

An interview take-home once asked me to design a path planning algorithm that was risk-aware; essentially, the robot, operating on an 8-connected grid, had to get from a starting coordinate to a destination coordinate, balancing the number of steps it takes to reach the objective and entering into a Keep Out zone. My Path Planning and Decision Making professor once said that A-Star (A*) is the bread and butter of basic path planning, so I started with that and added a penalty term and weight which could be used to pull the robot out of risky areas. I've added my code here, and an explanation of the algorithm, along with an interactive version, below!

A-Star

A* is a graph-based path planning algorithm which combines the best qualities of uniform cost search (breadth-first search with a priority queue) and greedy best-first search (depth first search with a priority queue) to find the optimal path efficiently.

Some necessary notation:

• Cost-to-reach: the total distance from the starting coordinate to the current coordinate along the best path found so far
• Cost-to-go: the distance from the current coordinate to the destination coordinate (delivery site). Since this what we're trying to find, we approximate it with an upper bound on the shortest possible distance (in our case, it's impossible for the distance to the destination to be shorter than a straight line, so we use the Euclidian distance as our heuristic cost-to-go).

The algorithm functions as follows:

Initialization:

1. Create a priority queue which will release the coordinate with the lowest sum of cost-to-reach and cost-to-go (lowest total estimated path length)
2. Set the cost-to-reach of the starting coordinate to 0
3. Set the cost-to-go of the starting coordinate to the straight line distance (euclidian norm) between it and the destination
4. Set the parent coordinate of the starting coordinate to None, since it is the first coordinate

Main Loop

Apologies for the broken formatting, my static site generator doesn't support nested markdown yet; the same content can be found in Github with proper formatting.

The main loop of the algorithm repeats until the priority queue is empty or the destination coordinate is found, effectively signaling there is either no path to the destination, or the destination was found and further exploration isn't required. Per iteration of the main loop:

1. Pull the coordinate with the lowest combined cost-to-reach and cost-to-go from the priority queue
2. If the coordinate is the destination, we can rebuild the path from start to finish by visiting the parent of the destination, then the parent of that coordinate, and so forth until we reach the starting coordinate
3. If the coordinate is not the destination, find all valid neighboring coordinates (neighbors within the bounds of the map), and per neighbor, do step A.

Step A: per neighbor:

1. Calculate the new cost-to-reach of the neighbor as the cost-to-reach of the current coordinate, plus the cost to get from the current coordinate to the neighbor (1 or sqrt(2) for an 8-connected grid)
2. If either a) the neighbor hasn't been visited before or b) the new cost-to-reach is smaller than its previous cost-to-reach (indicating we found a more efficient path to it), then do Step B.

Step B: if the neighbor hasn't been visited or the new cost-to-reach is smaller than its previous one:

1. Set the neighbor's cost-to-reach as the newly calculated one
2. Set the neighbor's parent coordinate as the current coordinate
3. Add the neighbor to the priority queue for further exploration, with its priority equal to its cost-to-reach plus its cost-to-go

Risk-Aware A-Star

A* finds an optimal path to a given destination, but does not natively account for a maximum acceptable path length, nor High Risk zone that we wish to avoid, and Keep Out zone that we must avoid.

To solve the former, an additional condition is added to step A2 above: the sum of the cost-to-reach and cost-to-go (so lower bound on the total path length) must not exceed the maximum acceptable path length.

To stay out of a Keep Out zone, we can add an additional condition to step 3 where the neighbor cannot be in the "Keep Out" zone. Easy enough.

To account for minimizing the total risk, I've added a second cost-to-reach, called cost-to-reach-penalty, which sums up the total distance between the start coordinate and the current coordinate, as well as the total penalty accumulated for traversing the High Risk zone, multiplied by a weighing factor (High Risk Multiplier in the demo below). cost-to-reach-penalty is then used everywhere that the cost-to-reach would be used in the regular algorithm. In step A2, this means we only consider the path to a coordinate as improved if the sum of the new cost-to-reach and the accumulated penalty is lower. In step 1, the coordinate with the lowest sum of cost-to-reach and penalty is pulled first.

This approach is flexible, in that we can tune the High Risk Multiplier to prioritize a shorter path or lesser risk. For example, a High Risk Multiplier of 0 is effectively the same as the original A* algorithm and gives the shortest path possible, without regard for total risk. As the penalty increases, the trajectory will begin to avoid the high risk area fairly quickly (the jump from 0 to 1 is very large); anything larger than about 10 will avoid the High Risk zone entirely.

Update Sep 2023: web version that works right on this page! Now I just need to update wssg to support javascript...

Note: I suspect A-Star is fundamentally incompatible with a max range condition and penalty term; if you keep the risk high and set the max range to exactly 51.7, the far left site will have a path to it, but if you set the max range to 52 or 60, then there will be no path; it'll recover the path around a max range of 70, but there should still be a path between those ranges! To "solve" this, I've included a backup path planner than ignores the high-risk penalty. It seems as though this issue occurs mainly when the maximum range is low enough that there's only a single path (essentially straight) to get to a site. Under those circumstances, the high risk penalty is pretty much already ignored in the valid solution, so a solver which doesn't take it into account won't be far off from the optimal solution anyways.