Stream functions for smoother potential fields

Let's say we're at this red start point (drag it around) and we want to reach the blue end point without bumping into the green obstacles (drag them around too). How would we do that?

This is one example of a path planning problem. There's a lot of ways to solve this (I'll do some other solutions in the future), but the solution above uses stream function potential fields.

Potential Fields

Potential field planning involves pretending to be a ball rolling down a hill. If we set the "height" of the imaginary hill (height is proportional to potential energy by $E = mass * gravity * height$) based on distance to the goal, we get a nice funnel that should draw the ball to the goal.

To stay away from obstacles, we can represent them as hills. We can set the height based on an inverse square law, a phenomenon observed in magnets repelling each other, light diffusion, and lots of other physical systems:

One neat aspect of potential fields is that we can simply add together the energy resulting from the valley to the goal and the hills from each obstacle! Try enabling/disabling the different fields and see the resulting height map:

If we let the ball roll, the ball's motion should give us a nice smooth path that safely steers us to the goal.

So how do we compute that path?

Vector fields

The height gives the ball potential energy. At any given point, the ball will want to roll in the direction that reduces that potential energy the most (the steepest decline). This is the same as the gradient ($\nabla E_{total}$) of the potential field.

A nice property of the gradient is that the gradient of the total energy can be distributed to the sum of the gradients of each contributing energy:

We can just compute the direction vectors resulting from the attractive field and each of the obstacle fields separately and add them together. The gradient vector of the attractive field will simply be the direction vector pointing to the goal times the slope:

The gradient from one repulsive obstacle will be:

Howie Choset's slides give a more in-depth review of the math behind potential fields. The plot above uses the simple conical attractive and inverse square repulsive fields from those slides.

Streams

In addition to the standard potential field energy function and vector field, I wanted to experiment with stream functions. Stream functions is an alternate formulation which uses the potential for liquid to flow rather than the height energy of solid objects. To compute the stream function direction vectors, we switch out the gradient from the repulsive obstacles for a vortex gradient function (equation 9 from Vehicle Motion Planning Using Stream Functions).

The total gradient gives us a direction arrow for any point we test in the field. If we plot a grid of test point direction arrows, we get the vector field resulting from the potential field. Try enabling/disabling the different fields and see the resulting vector field. Try enabling/disabling the attractive/repulsive fields for both the potential field formulation and the stream function vectors:

To get a path from the vector field, we can use Euler integration. Starting at the start location, we can:

1. Get the direction vector at our current location
2. Move in that direction a tiny bit
3. Repeat until we reach the goal

The result of this is the potential field/ streamfunction path:

Evaluation

Since there are many ways to solve the path planning problem, it can be useful to evaluate how good the potential field and stream paths are. In particular, we care about:

1. Safety
2. Comfort
3. Efficiency

(roughly in that order). For now, let's just come up with a measure of comfort, since both paths plan similarly in terms of safety (and they both fail similarly too)

We can estimate 'comfort' by measuring how tight the turns are over the path, given by the curvature:

We can get the turn radius at each point along the path by fitting a circle to the previous, current, and next point on the path using the equation of a circle from three points.

Conclusion

Let's compare both methods. I see a couple of differences:

• Stream functions seem to have lower curvature and distributed more evenly along the path. Potential fields are relatively straight with sharp turns right before obstacles. Note that this breaks down when the obstacles are close together, where
• Potential fields are willing to choose a path outside of the obstacles, while stream functions get sucked in to the obstacles when they can't plan a path through the middle.
• Potential fields seem to have the tightest turn right before an obstacle, while stream functions vary where the tightest turn is. This is super apparent if you drag the red starting location around.
• Stream functions tend to have their turning centers encircle obstacles, while potential fields do not. Neat.

I think the most important thing to keep in mind is that any vector field method for path planning is an approximate solution. Even in this simple demo, there are ways to arrange the obstacles where neither method will plan a safe path to the goal point. This is because gradient descent is susceptible to local minima - we only compute the path in a forward pass once, so there's no way we can thoroughly explore the possible space. The solution to this is to only use these methods as one of several paths to evaluate at any given time, and to not blindly rely on them without computing some cost metrics.

For Python code implementing streamfunction path planning, check out github.com/torshepherd/streamfunction.