\documentstyle[12pt,../named,psfig]{article}
\oddsidemargin 0in	\textwidth 6.5in
\topmargin 0in		\textheight 8.5in

\newcommand{\strich}[1]{}
% \newcommand{\strich}[1]{\nopagebreak \rule{\textwidth}{0.3mm}}
% \newcommand{\putfig}[3]{\begin{figure}[t] \centering \ \strich{} \psfig{figure=fig/#1.eps,#3} \caption{#2 \label{#1}} \strich{} \end{figure}}
% \newcommand{\putfih}[2]{\begin{figure}[t] \centering \ \strich{} \psfig{figure=fig/#1.eps} \caption{#2 \label{#1}} \strich{} \end{figure}}

\begin{document}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Obstacle Avoidance Planning}
  \label{sec:obstAvoid}

A key to our approach is that the rover should, in real-time, evaluate where
it is safe for it to be heading, based on local sensing (e.g., stereo
vision).  These evaluations can then be used either to drive the rover
autonomously, or to safeguard the rover by over-riding (or even by modifying)
user commands (Section~\ref{sec:teleopTrials}).

In our previously reported experiments with Ratler
\cite{KROTKOV95AUTONROBOT,SimmonsSSI95,Simmons95iroslunar}, we adapted a local
obstacle avoidance planner called Ranger, which was developed at CMU for
ARPA's Unmanned Ground Vehicle (UGV) program for cross-country navigation
\cite{Kelly}.  Ranger works by analyzing the paths the vehicle would traverse
along the terrain for a number of different steering angles, and choosing the
one that evaluates as the safest.

Terrain data for Ranger is obtained from stereo vision.  To make the stereo
computation tractable, only a small segment of the stereo image (about 2\%) is
requested, at reduced resolution (every fifth row and column).  Experiments
show that this is sufficient for reliably detecting features on the order of
20 cm high.  The local obstacle avoidance algorithm dynamically chooses which
portion of the image should be processed, based on the current vehicle speed,
stopping distance, and expected cycle time of the perception/planning/control
loop.  The Ranger algorithm merges individual stereo-produced elevation maps
to create a 25 cm resolution grid map up to seven meters in front of the
rover.  Map merging is necessary because the limited fields of view of the
cameras do not allow a single image to view sufficient terrain.

Ranger works by projecting the rover's state (position, roll, pitch, yaw) as
it travels along a path.  The projection is based on a desired steering angle,
the vehicle dynamics, and the underlying terrain.  The vehicle's current pose,
its dynamics and the steering angle are used to determine the position and yaw
of the vehicle at the next time step.  The height of the terrain under the
wheels is then used to determine the roll and pitch of the vehicle at that
point.

Once a projection of the vehicle along a path has been computed, the vehicle
state at each point in time is evaluated.  Four criteria are used to determine
the ``goodness'' of a path: roll, pitches of the left and right body segments
of Ratler, and number of known terrain points the vehicle crosses along the
path.  If any of the criteria exceed a given threshold of safety (such as
excessive roll or pitch), the whole path is given a very low evaluation.
Otherwise, the criteria are normalized to the range [0..1] and are combined
using a linear weighted function.  This determines the overall merit of
choosing that steering angle for the rover.  These evaluations are then
combined with the user's preferences (Section~\ref{sec:teleopTrials}) to
determine the overall best command, which is then sent to the rover to be
executed.  The cycle time for this process is about 2 seconds, with the stereo
computations taking up about 75\% of the time.

While the Ranger algorithm has worked well for high-speed navigation of
Humvees, it is not entirely well-suited to the much smaller, and slower,
lunar rover.

As is often the case in robotics, the problems are mainly attributable to an
abundance of noise: noise in the the stereo-produced terrain maps and noise in
the dead-reckoning (particularly, the roll, pitch and yaw angles).  The main
effect of the noise was to make it difficult to cleanly merge terrain maps
acquired from separate images, which is required by the Ranger algorithm since
it uses only a small percentage of each image.  Map merging often produced
artifacts in the map, such as crevasses and ridges, which the rover would
refuse to cross.  This is less of an issue with the Humvees, since they can
cross much taller obstacles.  We tried several merging techniques in an
attempt to minimize the artifacts, but none was robust enough to yield
consistent driving results.

Another effect of noisy terrain data is that, because of the rover's
relatively short wheelbase, small deviations in perceived terrain elevation
under the wheels produced relatively large changes in estimated roll and
pitch.  For example, a 20 cm ``spike'' in the terrain map (not uncommon)
produces a 13 degree change in pitch, given a 90 cm wheelbase.  Thus, it was
often difficult to distinguish noise from steep bumps.  This same problem also
made it difficult to reliably determine whether high-centering might occur,
since the clearance of the rover was not much more than the noise in the map.
Finally, the Ranger algorithm presumed that the rover could track the path
exactly, and not not account for uncertainty in the execution of commands or
for uncertainty in the vehicle dynamics models used to project paths.

To address these problems, we modified parts of the Ranger algorithm, creating
a program called Morphin (a ``power'' Ranger).  In contrast to the {\em
path-based} approach of Ranger, Morphin is {\em area-based:} it analyzes
patches of terrain to determine the traversability of each patch, and
evaluates the traversability of a path by determining the set of patches it
travels through.  As such, it is actually more akin to Ranger's predecessor
\cite{Stentz}.

\begin{figure}
\centerline{\psfig{figure=../fig/morphin.eps}}
\caption{Local obstacle avoidance planner}
\label{fig:morphin}
\end{figure}

Each local terrain map, produced from a single stereo pair, is analyzed
independently. The terrain is divided into overlapping patches, each 125 cm on
a side, with patches offset by 25 cm from one another.  Thus, each terrain
point contributes to 25 patches (Figure~\ref{fig:morphin}).  

To determine traversability, a plane is fit to each patch using least-squared
error.  To avoid redundant computation, statistics (e.g., sum of X, sum of XY)
are collected for smaller 25 squared cm patches and then aggregated to
determine the plane parameters for each 125 squared cm patch.  The plane
parameters are used in determining the vehicle roll and pitch (see below), and
the residual to the plane fit is used to estimate the roughness of the
terrain.  Actually, two ``roughness'' measures are computed: one based on the
residual in fitting the plane to the whole patch, and one based on the
residual of each small (25 squared cm patch).  The former indicates the
roughness of the overall area, while the latter indicates if the patch is
bumpy/spiky.  Finally, the uncertainty in the evaluation is estimated in two
ways: based on the total number of terrain points within the patch and the
distribution of those points (essentially, the entropy of the spatial
distribution).

Morphin then uses the Ranger projection algorithm to project the path of the
rover over the terrain patches.  If the center of the rover falls within the
central 25 squared cm of a patch, the associated roughness and uncertainty
measures for that patch are added to the overall path evaluation.  The pitch
of the vehicle is easily calculated as the slope of the line along the plane
in the direction of the current heading (yaw).  A similar calculation yields
the vehicle roll.  If there are overlapping patches from different images,
only the one associated with the most recently acquired image is used (given
the dead-reckoning uncertainty of the rover, we find this to be much more
effective than trying to combine overlapping evaluations in some way).

Our navigation experiments (Section~\ref{sec:10K}) have demonstrated the
superiority of the Morphin algorithm for our rover.  Morphin addresses the
problem of noisy data by aggregating independent data points into an overall
statistic, thus dramatically lessening the impact of any single point.  While
this aggregation can sometimes cause the rover to behave more conservatively
than would otherwise be warranted, in our application it is better to be too
conservative than to allow the rover to head into danger.

The remaining weakness of the algorithm is that the stereo vision often cannot
see craters, and reports them as unknown areas on the map.  To handle such
situations intelligently, Morphin needs to have a better understanding of
spatial continuity: it needs to distinguish disjoint patches of truly unknown
areas from contiguous unknown patches that may represent craters and other
drop-offs.  

% We are also investigating the use of a laser proximity sensor to detect
% craters and high-centering obstacles, and may look into the use of wavelets
% to estimate terrain roughness.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\bibliography{../bib}
\bibliographystyle{named}

\end{document}
