A Bayesian framework for optimal motion planning with uncertainty.
In *Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)*. Pasadena, CA, May 2008.
pdfdoi supp. material

**Abstract** --
Modeling robot motion planning with uncertainty in a
Bayesian framework leads to a computationally intractable
stochastic control problem. We seek hypotheses that can
justify a separate implementation of control, localization
and planning. In the end, we reduce the stochastic control
problem to path-planning in the extended space of poses x covariances; the transitions between states are modeled
through the use of the Fisher information matrix. In this
framework, we consider two problems: minimizing the
execution time, and minimizing the final covariance, with
an upper bound on the execution time. Two correct and
complete algorithms are presented. The first is the direct
extension of classical graph-search algorithms in the
extended space. The second one is a back-projection
algorithm: uncertainty constraints are propagated backward
from the goal towards the start state.

### Additional material

Master's thesis, May 2007 (PDF)

C++ implementation of the method:

- only the source code (600KB tgz)
- the source code, plus the animations (15MB tgz)

### Animations in Flash format

These are much fun to see! --- well, only if you are into planning.

The animations are Flash objects: **use the left and right arrows to make the search process go back and forward**. I did not make an MPEG video for ICRA with these, because it would take well over 5MB to present these with a good resolution. The animations are also available in PDF format -- click the link under the Flash object.

#### Environment shown in the paper

- Planning
**without**uncertainty (for comparison): - Planning with uncertainty; solving PPU-MinTime:
- Planning with uncertainty; solving PPU-MinCov:

#### Another simple environment with non-orthogonal walls

- Planning
**without**uncertainty (for comparison): - Planning with uncertainty; solving PPU-MinTime:
- Planning with uncertainty; solving PPU-MinCov:

### Guided tour

The animations show the number of nodes in the OPEN queue (turquoise numbers), and in the VISITED (closed) set (violet numbers).

For a start, you might want to take a look at PP without uncertainty using A*. Nothing surprising here, of course.
But notice: for each pose, there is, at most, *one* node, in
either the OPEN or VISITED set. This is because the dominance relation
used is *a total order*. In this HTML text, let » be the dominance relation. Then, for simple PP, if n_{1} and n_{2} have
the same pose, either n_{1} » n_{2} or n_{2} » n_{1}.

This is not true when using A* to solve path planning with uncertainty (PPU). For example, in the same situation (env2-forward-A*), there are several nodes per cell in both OPEN and VISITED.

The point is that in PPU the covariance is part of the state space and should not be treated like another "cost". Once one gets this intuition, the rest is downhill.

There are many interesting sights to see, if you peruse through the animations. In env1-forward-A*, note the successive "waves" of nodes. The first waves that fail are the direct paths which are not localized enough.

Also in the case of env2-forward-A*, the search process tries to go straight to the goal, but no direct path is feasible. Then, a new path is tried, which differs slightly at the beginning, and then goes straight to the goal; this goes on until a path which acquires enough information is found.

The backward algorithm is conceptually much more complicated. In this
case, the nodes do not contain "states" (a pose + a covariance), but
*constraints* on the states. However, graphically, they behave in the same way.
See for example env2-backward-A*.

For the backward method, at the end of the animation there is
a representation of the *reverse* path (q_{k},{M_{i}}_{k}).
The pink ellipses represent {M_{i}}_{k} -- the back-projection of CONSTRAINTS(q) -- and together they form an "envelope" in which
the *forward* path (q_{k}, Σ_{k}) must pass.

It could happen (because of unmodelled phenomena), that, during execution, the actual covariance is different from the planned covariance. In that case, the planned path is still admissible and optimal, as long as the actual covariance is inside the envelope.