A Lightweight, Embeddable Navigation Planner

Introduction

On mobile robots, computational processing power is one of the biggest limitations . Pushing more compute power onto your platform not only requires more physical space for the devices but it also is one of the biggest sinks of current. During my first year of grad school, I worked on an independent study project that looked at devising a robust, real-time navigation planner for our mobile humanoid platform uBot6. Although the resulting planner wasn't novel enough for publication it offered obstacle avoidance, re-planning based failure detection, and an interchangeable sampling based planner while retaining a rather low computational cost. 

Example of the LEHF planner in a real world environment. The robot maps the environment with an RGB-D sensor and builds a representation using OctoMap. In this image our mobile humanoid robot, uBot6, has been navigating around part of the lab. The LEHF potential field is shown surrounding the robot. No part of the map was given before start up.


Local Egocentric Harmonic Function (LEHF)

Since the early days of robotics, navigation planners have looked at various ways to use potential fields to solve motion problems. Potential based methods operate by setting the goal (or goals) as sinks and obstacles / boundary as sources of a potential. Because potential flows from sources to the sink a gradient is defined over freespace that points to the goal. By following this gradient, robots can achieve smooth, obstacle avoiding motion to the goal. However, there are many practical problems to this approach, involving the poorly chosen potential fields creating local minima and the need to solve for the potential over a vast area which is largely untraveled by the robot.

Harmonic function potentials are a kind of potential field that is guaranteed to be smooth and have only one minima, a problem of quadratic potentials, and as such serve as a good basis for a navigation function (Connolly et al., 1993). A function is a harmonic function if it satisfies the differential equation 

\[
 \begin{equation}
\nabla^2 \phi = \sum_{i=1}^n \frac{\partial^2 \phi}{\partial x_i^2} = 0  \nonumber
\end{equation}
\]

This equation can be solved by setting the boundary conditions to be a constant value of the potential at the boundary / source and using a numerical relaxer such as Gauss-Seidel to converge to a solution. As new obstacles are detected, they can be added as boundary conditions to the current potential field and re-relaxed.

The difficulty of using harmonic potentials alone is that they must be defined over the whole navigable space in order to be complete (guarantee to find a solution if one exists). Furthermore, goals that are far away result in a weak gradient since the potential field is hardly affected by sinks that are very distant. This leads to computational difficulties because numerically solving the potential at a meaningful resolution is an \( O(f(\phi)n^2) \) operation where n is the dimension of the navigable space and \(f(\phi)\) is the number of iterations to relax the potential at each time step. This is not good. Instead, let us center the potential field around the robot with a fixed size window boundary a configurable distance \(d\) away. If the goal falls inside the window then we relax the harmonic potential as usual. If the goal is outside of the window, then we map the goal to a point on the boundary of the window called cell \( i^* \), and change the window's boundary conditions to be a the following transition function between the goal potential and obstacle potential.

\[
 \begin{equation}
\phi_i = \phi_G* e^{\frac{-z_i^2}{2\sigma^2}} + \phi_O * (1-e^{\frac{-z_i^2}{2\sigma^2}}) \nonumber
 \end{equation}
\]

where \( \phi_i \) is the potential of the i'th cell on the window boundary, \( \phi_G \) is the goal potential, \( \phi_0 \) is the obstacle potential, and \( z_i \) is the distance of the cell on the window boundary from the goal projection \(i^* \) while traveling along the boundary.

Examples of the potential field. The pictures on the left are the 3D representations of the contour plots on the right. The top images shows the potential field when the goal is inside the LEHF window. The bottom images show the potential field when the goal is outside of the boundary window and has been projected back onto the boundary. Note that the gradient of the potential will be strong even if the goal is far away from the window.

Let us call this local planner LEHF. The robot remains obstacle avoiding inside of the window and responds strongly to goal locations regardless of how far away they are. While this separates the computational cost from the size of navigable space, the LEHF is not complete as it can only consider paths that immediately fit inside of its window. Thus we will need to reinsert completeness using another kind of planner.


RRTs and Sampling Based Planners

Rapidly-Exploring Random Trees are a family of planners that use a random exploration techniques to solve a graph connectivity problem over freespace. They are still very popular to this day, and the OMPL planning library offers a large collection of open source implementations of these kinds of planners along with references to their original papers (Șucan et al., 2012). A good overview of how RRT planners work can be found here. In particular, the bi-directional RRT algorithm tries to find a connected path through freespace by growing a search tree from the start and the goal at the same time (Kuffner et al., 2005). They are probabilistically complete, meaning that if a solution exist then the probability the RRT will find is a continuously increasing function over time. RRTs can span large areas of space very quickly, and each node of the solution path can be treated as a subgoal for a more local planner.

Example of the bidirectional RRT path planning algorithm. The planner proceeds from top left to bottom right. Image from (Kuffner et al., 2005).

Example of the bidirectional RRT path planning algorithm. The planner proceeds from top left to bottom right. Image from (Kuffner et al., 2005).


Hierarchical Egocentric Harmonic Function

If we combine an RRT planner with our egocentric harmonic function planner, we get a hierarchical planner that operates under the following steps

1. Plan a path to the goal using the RRT planner
2. Post process RRT solution path to be sparse and smooth
3. Treat each node in the solution path as a subgoal and hand them one by one to the LEHF planner.
4. If the potential gradient disappears, replan with RRT planner or continue until goal is reached.

Since the map is not known before planning, or new obstacles might appear, the robot might encounter the situation where the current planned path to the goal is completely blocked by obstacles from the perspective of the LEHF and the gradient will go to 0. This alters the RRT planner that a failure has occurred and needs to replan a new path since the current direction is blocked by previously unseen obstacles. In this manner, the robot will continue to explore a new environment until the global planner is unable to find any path to the goal.

Local Egocentric Harmonic Function Planner test run in lab. The blue line is the path produce by the RRT planner. The purple grid surrounding the robot represents the potential field values. The green line estimated path to goal based on the gradient.


References

Connolly C., and Grupen R. (1993) The application of harmonic potential functions to robotics. Journal of Robotic Systems, 10(7):931-946. [PDF]

Kuffner J. J., and LaValle S. M. (2005) An efficient approach to path planning using balanced bidirectional RRT search. Technical Report CMU-RI-TR-05-34, Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, August 2005. [PDF]

Ioan A. Șucan, Mark Moll, Lydia E. Kavraki, (2012) The Open Motion Planning Library, IEEE Robotics & Automation Magazine, 19(4):72–82, December 2012. DOI: 10.1109/MRA.2012.2205651 [PDF] http://ompl.kavrakilab.org

Octomap: http://octomap.github.io/