Understanding Robot Motion: Path Smoothing

This is part of a series of articles related to working through the AI for Robotics course on Udacity. This write-up is about Lesson 5.

In discrete environments, such as a grid world, shortest path problems are solved by using an algorithm that finds the minimal number of grid positions needed to travel given a starting position to a terminal position.

This works well for objects, like a user-controlled square in a 2-d world, that can assuredly go from one position to another without worrying about limitations in movement.

Assume we start off as a Yellow box in the start position. There are many solutions to this grid but the purpose is to show that our object has no problem moving from one grid cell to the next.

This isn’t the case for vehicles. It’s entirely possible a planning algorithm provides the most efficient way to get to the destination in purely straight lines, as shown below.

Simple optimized planning solutions for objects can navigate grid worlds in straight lines.

Planning a curved turn would be a ideal solution for the bus due to the fact that it can’t make a 90 degree turn on the spot. An additional reason for smoothing is allowing for clearance to allow for the error that naturally occurs through the process of movement. There could be a slight error in the sensory data determining the position of the vehicle, and the act of moving itself has the likelihood of accumulating some error.

Curving the Trajectory

Assume we already have an optimal path for the car to take, but we wish to smooth the trajectory. For these equations, y represents an smoothed coordinate at time step i while x represents the original unsmoothed coordinate. The ith element of x refers to a list containing the x and y coordinates of an object at the ith time step.

The unsmoothed path coordinates.

The smoothed path coordinates.

We initialize y_i as x_i before going through an iterative process of adjusting the smoothed path based on optimization steps.

Initialized y as x, the unsmoothed path.

The first optimization is to minimize the distance between the coordinates of the original path(x) and the smoothed path(y).

Minimizing the distance between the original path and the smoothed path.

Then, in the same iteration, we need to minimize the difference between the coordinates of the smoothed path at time step i, and adjacent time steps. This wouldn’t work for the terminal step, as there is so successive step beyond it, but this is acceptable because we shouldn’t been smoothing the terminal step (nor the original time step).

If we were to only minimize the distance between the unsmoothed path and smooth path, we’d end up not changing the smooth path at all. This is because the result of the equation would always be zero. Minimizing zero would not change anything.

On the other hand, if we were to only minimize the distance between two consecutive smooth points, you’d end up with no path at all. This is because the points in between the initial and terminal step would be minimized onto a single point.

Since the two optimizations are in some ways in conflict with one another, you can adjust the importance of the smoothing optimization with hyper parameter alpha. The higher the alpha, the more the coordinates should be smoothed, while the opposite will result in coordinates that have a greater adherence to the original unsmoothed set of coordinates.

Alpha adjusts the importance of the smoothing output.

To minimize both terms, we can use Gradient Ascent. In the video, Sebastian talks about using Gradient Descent (which made sense to me having used it extensively in a variety of Deep Networks). However, in the solution, the teaching assistant recommended we use Gradient Ascent so that the algorithm would converge, which leads me to believe the error function must be convex.

For the first update, we can update the smoothed coordinates in the direction that minimizes the distance between the non-smoothed coordinate and the smoothed coordinate.

Gradient Ascent update for y_i with respect to the un-smoothed trajectory.

For the second update, we can update the smoothed coordinates in the direction that minimizes the deviation between adjacent smoothed coordinates. Beta acts like alpha in that is adjusts how much emphasis to put on the smoothed coordinate update.

Gradient Ascent update for y_i with respect to neighbouring smoothed coordinates.

When implementing this update, it was important to update y_i in one step, as both error terms are dependent on y_i. Updating one before the other (as was recommended by Sebastian) would result in the second error term updating a different y_i.

Solution Code

# Coordinate Updates:
[0.000, 0.000] -> [0.000, 0.000]
[0.000, 1.000] -> [0.021, 0.979]
[0.000, 2.000] -> [0.149, 1.851]
[1.000, 2.000] -> [1.021, 1.979]
[2.000, 2.000] -> [2.000, 2.000]
[3.000, 2.000] -> [2.979, 2.021]
[4.000, 2.000] -> [3.851, 2.149]
[4.000, 3.000] -> [3.979, 3.021]
[4.000, 4.000] -> [4.000, 4.000]

I refactored my code (which had the correct solution) to the Udacity solution code specifically because while my solution was able to get the correct output, it wasn’t modular to multi-dimensions. In other words, if the input list contained entries that were anything other than two dimensions, my code would break.

To make sure I wasn’t updating the first and last coordinates, I had initially used enumerate and a conditional to see if the index position of the coordinate list did not include the first and last index position. Ultimately, I found their idea of using custom ranges to be much more efficient.

The final optimized path to the solution using hyper-parameters provided by the problem code.

The resulting trajectory is more in line with how a driver of a vehicle would operate, which would be to turn ahead of time around a corner.