\section{Integrated Walking System}
  \label{sec:walk}

%!

The Ambler integrated walking system (Figure \ref{fig:modules})
consists of a number of distributed
modules (processes), each with a specific functionality.
This section describes each of the modules 
illustrated in Figure \ref{fig:modules}:
the perception modules which acquire images and construct
terrain elevation maps;
the planning modules which plan leg and body movements;
the real-time module which controls the robot and monitors its internal sensors;
the task-control module which coordinates components and monitors progress;
and the graphical user interface which enables paths to be entered.

\begin{figure}
%! \vspace{1.5 in}
%! \special{psfile=}
\caption{Modules in the integrated walking system}
\label{fig:modules}
\end{figure}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Example of Operation}
 \label{sec:walkAlg}

To initiate walking, a user inputs a series of arcs via a graphical user interface 
(Figure~\ref{fig:gui}).  The user indicates desired waypoints and the orientation to be 
achieved at each waypoint, and the user interface computes a series of arcs to achieve 
those position and orientation constraints.

\begin{figure}
\vspace*{2.5in}
\caption{Graphical user interface}
\label{fig:gui}
\end{figure}

Each arc is forwarded in turn to the gait planning module, which plans a series of leg 
and body moves.  The planner first determines the leg
that maximizes subsequent body motion.  It finds a region of geometrically
feasible footfalls based on the limits of the leg's motion, the need to
avoid colliding with the current leading leg, the desire to avoid placing
the leg in the path of the body, and the need to maintain 
support in subsequent moves \cite{Wettergreen90}.

Next, the terrain within the geometrically feasible footfall region is analyzed to 
estimate the goodness of the footfalls with respect to the underlying terrain.  The 
footfall planner sends a request to the perception subsystem for a terrain elevation 
map within the boundaries of the footfall region.  The local terrain mapping module 
sends a request to the image queue module requesting subimages that contain laser 
range data relevant to constructing the desired terrain map.  It then combines the 
images to construct a terrain elevation map, and returns it to the footfall planning 
module.  The footfall planner uses 
a neural net to evaluate the terrain map, estimating the goodness of each foot-sized 
patch in terms of stability and traction, and the gait planner chooses the best footfall 
location within the specified region.

Given a leg move, the planner finds the maximum body move that follows along the 
arc, does not violate limits on leg motion, and keeps the Ambler's center of gravity 
within the {\em conservative support polygon (CSP)}, which guarantees that the 
Ambler will remain stable even if any single leg fails (Figure~\ref{fig:CSP}).
The gait planner then forwards the chosen leg move to the leg-recovery planner, 
which plans a time-efficient trajectory that avoids terrain collisions.  The chosen leg 
and body moves are then forwarded to the real-time controller module.  
Concurrently, if the end of the arc has not been reached, the gait planner starts 
planning the next pair of leg and body moves.

The real-time controller generates leg move trajectories that are linear in joint
space, coordinating the leg joints so that all
motions start and end simultaneously.  The controller module checks the leg
move for feasibility, refusing to perform the move if it would exceed kinematic or 
geometric limits.  During execution, the controller monitors  
the force sensors in the feet and the inclinometers.
If an unexpected terrain collision or tilt occurs, all motions 
are halted, the brakes are engaged, and an exception is raised to the task-level 
controller.  Many of these exceptions are handled autonomously (see 
Section~\ref{sec:taskCtl}) and walking continues when the steps are replanned.

In a similar manner, the real-time controller executes planned body moves.
It computes, for each leg, the trajectory of horizontal joint motions that will 
simultaneously translate and rotate the body to achieve the commanded position.  
The twelve horizontal joints are synchronized to achieve
smooth acceleration and deceleration of the body.  When the body move
completes, the task-level controller notifies the perception subsystem to acquire a 
new laser range image.  This enables pre-processing of the image to occur 
concurrently with planning and control tasks.
Typically, by the time the leg and body moves have been executed, the planners are 
ready with a new set of moves.  In this way, the Ambler achieves nearly continuous 
motion.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Perception}
  \label{sec:perception}

From the challenges identified in Section \ref{sec:intro}
follows a need for perception of the remote, natural, unknown environment:
\begin{itemize}
\item Self-reliant control.
The rover must avoid stepping in unsafe areas that could cause it to lose traction or 
become unstable and tip over.

\item Limited power.
Given limited power, the rover maximizes its 
benefits by planning motions minimizing terrain contact.
This requires models of terrain geometry that make explicit the spatial
layout of the environment.

\item Rugged terrain traversal.
For rugged terrain traversal, the rover must select favorable footholds.
In turn, 
this requires quantitative information about terrain properties such as 
elevation, slope, and roughness.
\comment{Is this ever addressed?: and material strength.}
\end{itemize}

To address these challenges,
we have developed perception algorithms and systems 
that build maps for the purpose of locomotion.
The primary terrain representation is an elevation map, in which each 
$(x,y)$ grid point is labeled with the maximum terrain elevation at that point.  Elevation 
maps are well-suited for representing natural, rugged terrain, since they make few 
assumptions about the shape of the terrain.  In addition, subsections of the map can 
be accessed by other system modules simply by specifying the polygonal boundary of 
a region of interest.  These characteristics make them well matched to the needs of 
the Ambler's planning algorithms (see Section \ref{sec:planning}).

\subsubsection{Sensing and Calibration}

The primary sensor is a scanning laser rangefinder, which measures both
reflectance and range (Figure \ref{fig:rawImages}).  
We use a laser scanner because it directly recovers
the environment's three-dimensional structure, supplying 3-D
data more rapidly and reliably than passive vision techniques such as 
structure-from-motion and binocular stereo (although, 
recent advances \cite{Boissier92icra,Matthies92ijcv} make stereo
far more feasible for planetary rovers than it was in 1987, 
when the decision was made to use a laser rangefinder).

The particular sensor used (a Perceptron scanner) acquires
data in $256\times256$ pixel images at a rate of 2 Hz.  
It digitizes to 12 bits over approximately 40 m, 
providing a theoretical range resolution of approximately 1 cm.  
The measurements cover $60^{\rm o}$ in azimuth and $60^{\rm o}$ in elevation.

\begin{figure}
%! \vspace{2 in}
%! \special{psfile=/usr/epk/paper/fig/bitmap/perceptron/1206.ps
%!   hoffset= -72 voffset= -324}
\caption{Reflectance (left) and range images}
{\small The scene consists of a sand base with boulders 1-2 m tall
(foreground) and a wooden ramp.}
\label{fig:rawImages}
\end{figure}

To ensure accurate sensor data,
we have developed an automatic calibration procedure that identifies the 
transformation between the Ambler-centered reference frame and the 
sensor-centered frame.
The procedure moves the leg to various positions within the scanner field
of view, and processes the reflectance image to locate the leg.  
The leg positions in the images and those positions computed by the
Controller from the vehicle kinematics are used to compute the transformation that 
minimizes the distance between pairs of corresponding points referred to the 
Ambler-centered frame.
The technique achieves an accuracy of 2--7 cm with a precision no lower
than 2--5 cm \cite{Krotkov91racal}.

\subsubsection{Perception Subsystem}

The perception system consists of three major
modules that acquire images, preprocess and store images,
and construct terrain maps (Figure \ref{fig:percSys}).
Approximately ten other modules perform data analysis and display tasks,
each executing concurrently.
Many different system configurations, including this one,
suffice to meet the perception needs identified at the beginning of Section
\ref{sec:perception}.
We selected this configuration because it is modular, 
extends easily to more and different sensors,
decouples imaging and mapping, and manages large quantities of data.

\begin{figure}
\vspace{2 in}
\caption{Perception system modules}
\label{fig:percSys}
\end{figure}

\subsubsection*{Image Acquisition}

The Perception Imaging Manager (PIM) acquires frames of range and reflectance 
data, selecting either
1) real images from the rangefinder, 
2) virtual images stored previously, or
3) synthetic images computed by a 3-D graphics simulator.
The PIM tags each image pair with robot state information
(the body pose and six leg positions),
the transformation relating the scanner frame and
world coordinate frames (composed from transformations derived from the
calibration procedure and the Ambler's dead-reckoned pose),
and other useful pieces of information.

\subsubsection*{Image Preprocessing, Storage, and Access}

The PIM transmits the tagged images to the Image Queue Manager (IQM).
The IQM preprocesses, stores, and provides access to the images.

The preprocessing steps seek to filter out spurious data, and to detect
range discontinuities.
Figure \ref{fig:preImages} illustrates the results of preprocessing
a typical scene.

\begin{figure}
%! \vspace{4 in}
%! \special{psfile=/usr/epk/paper/fig/bitmap/perceptron/1206.masked.ps
%!   hoffset=-72 voffset=-252}
\caption{Preprocessed range image}
\label{fig:preImages}
\end{figure}

\begin{itemize}
\item Mark pixels suspected to be corrupted by limitations of the
particular sensor used, including internal reflections at specific
locations, and a ``vignette'' effect (caused by the exit window)
appearing at the bottom corners of the image \cite{Krotkov92ijivc}.

\item Mark pixels belonging to the image of the robot's legs.
First, predict where in the image the leg(s) appear, and what the range
values should be.
Then search the predicted regions, marking those pixels with range values
consistent with the leg location.

\item Identify pixels corresponding to scene points that are not connected
to the ground plane.  We use standard image processing techniques to
extract the connected regions.
These ``floaters'' can occur under several circumstances documented elsewhere 
\cite{Krotkov92ijivc}, and summarized here:

\begin{enumerate}

\item If the scene point lies at a distance $\rho$
beyond the ambiguity interval of the sensor
(i.e., farther than 40 m), then the range reported by the sensor will
be $\rho \bmod 40$.

\item If the scene point belongs to an object that poorly reflects the
infrared radiation, then the range reported by the sensor will be
significantly less than the true range.

\item If the scene point belongs to an object oriented so that the angle
of incidence is small (far from perpendicular), then the range reported by
the sensor will be significantly less than the true range.

\end{enumerate}

In each case, the incorrectly reported range of the scene point causes the
computed elevation of the point to be substantially different from the
ground elevation.  In effect, the reconstructed scene point appears to
float above the terrain.

\item Identify range discontinuities using the Canny operator.  
The extracted edges are used in subsequent mapping algorithms to prevent
interpolation between distinct objects in the scene.

\end{itemize}

After preprocessing, 
the IQM stores the processed images in a fixed-length queue, removing older 
images as the queue fills up.
For most of the experiments, the queue size is 30, since that 
has been empirically sufficed for point turns in rugged terrain.  
The image queue data structure facilitates access to ordered sequences of
images.
Sequences are needed because the single forward-looking sensor on the Ambler 
cannot possibly image obstacles either below or behind the vehicle, which is 
necessary to accomplish tasks such as planning the trajectory of a recovering leg.

The IQM provides access to the images stored in the queue.
The access is selective because transferring images is expensive:
the size of the images is large (256K per pair,
plus variable-size auxiliary structures such as edge images),
and many images may be required
(with typical queries for $1 {\rm m}^2$ areas,
5 are required for a straight-line trajectory on flat terrain,
and 25 are required for a point turn over obstacles).
Given a polygonal region-of-interest, the IQM selects only rectangular
subimages that, after projection onto a ground plane, enclose or intersect
the given region-of-interest.
In typical operation, this reduces data transfer by 75-90 percent
compared to the policy of transferring the entire image whenever it
contains some part of the region-of-interest.

\subsubsection*{Terrain Mapping}

Terrain elevation maps are constructed and maintained by
the Local Terrain Mapper (LTM) module.
Other modules request elevation maps, specifying (i) a polygonal region, (ii) a map 
resolution, and (iii) an arbitrary reference frame to be used as the map origin.
Typical reference frames are 
the current body frame, the world frame, and a future body frame (to allow for 
advance planning).
To construct the requested maps, the LTM first queries the IQM
for a sequence of  subimages potentially containing information about the
polygonal region.
Then the LTM uses the subimages, starting with the most recent,
to compute the elevation of each grid point in the polygonal region.
The LTM computes and stores the terrain elevation
with the Locus Method, an efficient algorithm for
transforming and interpolating range data from the sensor frame into
Cartesian coordinates.

To summarize the Locus Method let $\rho$ be the sensed range, 
and let $\phi$ and $\theta$ be the
vertical and horizontal scanning angles of the laser beam direction
corresponding to row and column position in the image.  
The problem of finding the elevation $z$ of a point $(x,y)$ is equivalent
to computing the intersection of the surface observed by the sensor with
the vertical line passing through $(x,y)$.  The basic idea of the locus
algorithm is to convert the latter formulation into a problem in image
space.  A vertical line is a locus (curve) in $(\phi, \theta, \rho)$ space.
Similarly, the range image can be viewed as a surface $\rho = I(\phi, \theta)$.  
The problem then is to find the intersection, if it exists, between a
curve parameterized by $\phi$ and a discrete surface.  Since the surface is
known only from a sample of data, the intersection cannot be computed
analytically.  Instead, we must search along the curve for the
intersection point (we refer interested readers to \cite{Kweon92pami} for 
details of the search).

The LTM assigns one of three labels to each point in a rectangular grid 
enclosing the specified polygon:

\begin{itemize}

\item {\em unknown}, for those cells outside the polygon,
outside the field of view of all images, or 
containing pixels marked as invalid by the preprocessing steps.

\item {\em occluded}, for those cells occluded by other objects.
An $(x,y)$ location in the map is in an occluded area if its locus intersects
the image at a pixel that lies on an occluding edge.
We implement this idea by first detecting edges in the range image using
the Canny operator.
Then, when we apply the Locus Method and observe that the locus of a
given location intersects the image at an edge pixel, we mark that
location as lying in an occluded area, and compute an upper bound
on the elevation of the occluded terrain.

\item {\em known}, for all other cells.  

\end{itemize}

Figure \ref{fig:elevMap} illustrates the elevation map computed
from the range image of Figure \ref{fig:preImages}.
This map contains points with all three labels.

\begin{figure}
%! \vspace{4.75 in}
%! \special{psfile=/usr/epk/paper/fig/bitmap/map/exampleMap.eps 
%!    hoffset=-126 voffset=-252}
\caption{Elevation map}
{\small Elevation map computed from range images like those in Figures
\ref{fig:rawImages} and \ref{fig:preImages}.
The units are meters.}
\label{fig:elevMap}
\end{figure}

\comment{I don't think this adds much, especially given the description in Section 
3.1.  If you feel the need to add more information on system operation, please add it 
to that section}
% \subsubsection*{System Operation}
% 
% Figure \ref{fig:percConcurrent} illustrates the concurrent execution of
% the three main modules.  
% It shows that when the Ambler completes a body move, the PIM acquires an
% image, then sends it to the IQM, which preprocesses and stores it.
% When a planning module requests a map, the LTM requests the necessary
% subimages, and uses them to compute the map.  
% If the body moves while the LTM is active, as shown in Figure
% \ref{fig:percConcurrent}, the PIM is free to initiate the image
% acquisition and storage process.
% 
% \begin{figure}
% \vspace{1 in}
% \caption{Concurrent operation of perception system}
% \label{fig:percConcurrent}
% \end{figure}

\subsubsection{Performance}

Table \ref{tbl:percPerformance} records perception system statistics
gathered from several representative walking trials.
The large volume of data processed, both images and maps, indicates the
robustness of the system.

\begin{table}
\begin{center}
\begin{tabular}{c r r r r r r}
Trial	& Distance	& Images	& Subimages	& Maps	& Map \\
 	& (m)		& Acquired	& Transferred	& Built	& Points \\ 
\hline
Outdoor	& 46		& 151		& 8,974		& 626	& 
375,003 \\
Indoor	& 68		& 185		& 10,015	& 640	& 267,104 \\
Indoor	& 107		& 397		& 20,286	& 1,306	& 
597,078 \\ 
Outdoor	& 527		& 1219		& 101,129	& 4,716	& 
2,578,102 \\  \hline
\end{tabular}
\end{center}
\caption{Perception system statistics in four separate trials}
{\small The distance term represents the distance traveled by the robot
during the trial.
The results vary because the terrain and software modules vary between
trials.  For example, in the 46 m run in hilly terrain,
a module planned vertical body
motions, computing more map points than in the 68 m run
in non-hilly terrain, during
which that module did not execute.}
\label{tbl:percPerformance}
\end{table}

Over these trials, we find that taking a single step
requires approximately 20 sec of perception system processing,
which involves examining between 5 and 28 range subimages in order to
compute elevation values for some 1700 points, on average.
Breaking the timing down further, we find that for SPARC 2 workstations, 
image acquisition takes about 1 second,
image pre-processing and storage requires about 10 seconds,
and construction of elevation maps takes about 5 msec per point.  

One measure of map accuracy is the difference between the
computed elevation and the elevation determined by encoders on the
Ambler's legs.
According to this measure, we find that the 
map accuracy varies from step to step and from trial to trial,
but never strays far from the scanner precision of 10--15 cm.
This accuracy is sufficient for the Ambler, whose feet are 30 cm in
diameter.

As the Ambler walks, images and maps consume more and more memory.
The perception system handles this with 
memory management routines that detect memory saturation,
and then reclaim storage for new use and delete less recent data.
In one typical trial, after acquiring 100 images, memory usage levels peaked at
1.6 Mb for PIM, 2.8 Mb for LTM, and 30.5 Mb for IQM 
(with queue size 30).
After this, the perception system maintained essentially constant
memory utilization, thus demonstrating the effectiveness
of memory management.

In summary, the results show that
the implemented perception system constructs quantitative
models of terrain geometry that enable power-efficient locomotion in
rugged terrain.
\comment{In summary, there is a lot of "what" we did, but not much "how" or "why"}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Planning}
 \label{sec:planning}

Our research has developed automated planning algorithms for the Ambler that
address the three main challenges identified in Section~\ref{sec:intro}.

\begin{itemize}

\item Self-reliant control. Given a path to follow, the rover must
autonomously plan leg and body moves that are safe and effective (i.e,
maintain good progress along the path).

\item Limited power. The planner should choose moves that minimize energy
expenditure. This includes minimizing leg and body travel, and avoiding
unnecessary terrain collisions.

\item Rugged terrain. The algorithms must enable the rover to climb hills,
traverse rock-strewn terrain, and to cross meter-tall and meter-wide obstacles
(such as boulders and trenches).

\end{itemize}

All three challenges indicate the utility of a very deliberative approach to
planning leg and body moves. In particular, for the type of remote, rugged
terrain walking that we must address, the effects of ill-considered
decisions can be catastrophic. Thus, to provide for safe, power-efficient
movements, the planning algorithms all utilize detailed models of the vehicle
kinematics, the terrain, and vehicle/terrain interaction.

One potential problem with deliberative planning of moves is the computation
involved due to the need to search for acceptable moves. This is exacerbated
in the Ambler, since it is so mobile (which means that a large number of moves
are possible from a given pose). The planning algorithms attempt to minimize
the amount of search needed by adding constraints to the rover motion, such as
always using a circulating gait.

To date, our research in autonomous planning has concentrated on path
following: the rover is constrained to walk along an arc of a circle: if the
arc has large radius, the path is nearly straight; if the arc has zero radius,
the rover turns in place. We have developed a hierarchy of three planning
algorithms---gait, footfall and leg recovery---to handle this path
following task.

\subsubsection{Gait Planning}
 \label{sec:gait}

Gait planning involves choosing the sequence of leg and body moves to
traverse the rover along a given path. Typically, walking robots use a
fixed gait pattern, such as tripod or wave gaits
\cite{Brooks89,Song89book}. However, due to the mobility of the Ambler and
the extreme terrain it has to traverse, we developed a planner that uses a
{\em free} gait, one in which the choice of which leg to move is made
step-by-step, depending on current kinematic and terrain
constraints.\footnote{We have also developed a planner for a {\em crab}
gait, one that enables the Ambler to walk from side to side.} The planner
also capitalizes on the unique features of the Ambler to implement a
{\em circulating} gait, one in which the rearmost legs move through the
body cavity to become the new frontmost legs (Figure~\ref{fig:circGait})
\cite{Bares90,Wettergreen90}.

The input to the gait planner is a radius and angle that describes an arc
to traverse (the angle can be positive or negative, to enable the rover to
walk forwards or backwards).  The basic walking cycle is to alternate
moving the body so that it remains oriented tangent to the arc and then
moving the leg that most constrains subsequent body motion.  For arc radii
less than the tread width of the Ambler (about 2.5 m), the gait planner
first circulates the rear leg on one stack to the front, and then
circulates the front leg on the other stack to the rear.  In this way, the
Ambler can turn in place by having the legs on one stack move retrograde
with respect to the legs on the other stack.

The gait planner utilizes two constraints to determine the extent of the body
move. The first constraint is based on the kinematic limits of the legs. Given
that the feet are fixed, the planner determines how far each stack can move
along the arc before reaching a kinematic limit (Figure~\ref{fig:stackMove}a).
The second constraint is based on stability. To be statically stable, the
rover's center of mass must lie within the support polygon formed by the
convex hull of the feet. For added safety, we insist that the Ambler always
lie within the {\em conservative support polygon (CSP)}, which is defined as
the intersection of all five-legged support polygons (Figure~\ref{fig:CSP})
\cite{Swami}. Thus, if the Ambler remains within the CSP, any one leg can fail
and the vehicle will still remain statically stable. The gait planner finds
where the arc of body travel intersects the leading edge of the CSP, and uses
this as an additional constraint on body motion. The minimum motion determined
by the kinematic and support constraints is then chosen as the next body move.

\begin{figure}
\vspace*{2.5in}
\caption{Stack reachability constraint}
\label{fig:stackMove}
\end{figure}

\begin{figure}
\vspace*{2.5in}
\caption{Conservative support polygon}
\label{fig:CSP}
\end{figure}

This planning suffices for movement in the horizontal plane. To plan vertical
movements, the planner obtains a terrain map from the perception system of a
region in the direction of travel. It finds the maximum elevation in the
region and, if needed, commands a vertical raise or lower to keep the Ambler
within a fixed window above the terrain (typically, 50-90 cm). This decoupling
of horizontal and vertical body motion planning is possible because the Ambler
always walks with a level body motion (parallel to gravity) and its legs
decouple horizontal and vertical motion. Thus, changing the body elevation has
no effect on the kinematic limits of leg travel or on the static stability of
the machine.

After commanding body motion, the planner then chooses the leg move that will
afford the greatest subsequent body progress.  This is done by
determining the maximum body movement assuming the leg were not fixed to the
ground. In most situations, the planner considers the rearmost legs on each
stack; however, for tight turns (arc radii less than the distance between the
body and stack centers, or about two meters), one of the stacks moves in
retrograde motion (Figure~\ref{fig:stackMove}b), so that the planner must
alternate moving the legs on one stack from rear to front, and the legs on the
other stack from front to rear.  There are a number of geometric constraints
that come into play in determining where to place the feet. The kinematic
limits of the leg define a torus outside which the leg cannot reach.
In addition, the leg must be placed so that it does not collide with the
current frontmost leg, nor should it be placed within the path of the body
(Figure~\ref{fig:legConstraints}). Finally, there are constraints needed to
ensure the feasibility of subsequent movements: the leg must be placed far
enough forward so that a subsequent leg move on the opposite stack keeps the
body within the CSP, and the leg must not be placed so far forward that
subsequent leg moves on the same stack become kinematically infeasible.

\begin{figure}
\vspace*{2.5in}
\caption{Constraints on geometrically feasible footfalls}
\label{fig:legConstraints}
\end{figure}

These kinematic constraints define an area within which all footfalls are
geometrically feasible. If the area for a leg is null, the planner tries
moving the leg on the other stack. If there are no feasible moves on either
stack, the planner attempts to {\em shuffle} the legs, and tries to plan from
there. To shuffle legs, the planner moves legs forward, backward, and
side-to-side (without circulating through the body) so that the legs are
placed close to a standard configuration. Although in principle up to five
legs might have to be moved, in practice we have observed that shuffling
usually involves moving one or two of the front or middle legs, and is needed,
on average, every 30 steps or so.

\subsubsection{Footfall Planning}

Given the region of geometrically feasible footfalls determined by the gait
planner, the task of the footfall planner is to find the best terrain patch
within that region, in terms of its ability to provide stability and traction.

One problem was that it was difficult to operationalize those criteria.  While
flat, smooth areas are obviously ideal, and steep, rough areas tend to provide
the worst footing, it is unclear how to quantify the areas in between.  We
tried several methods of evaluating footfall quality, including hand-coded
rules and non-linear regression techniques.  We eventually settled on a
neural-net learning scheme, which was found to consistently produce the best
results \cite{Hsu91}.

The idea is to estimate footfall quality from terrain features, such as slope,
roughness, and curvature.  The neural net learns an evaluation function which
combines these features.  It is trained off-line using human-supplied
indications of footfall quality (e.g., ``good'' or ``very bad'') in a variety
of terrain.  Training data are the pairs of locations where the numerical
evaluation produced using the current set of net weights differs from the
ordering specified by the human preferences.  The difference in evaluations is
used as an error term to update the network weights using backpropagation
\cite{Rumelhart88}.

This method of using a neural combine precomputed features (rather than
operating directly on the raw data) has proven quite effective, both in
learning speed and on-line operation. In particular, the features provide
sufficient structure to make the learning tractable and the features are
fairly easy to compute from the terrain elevation maps produced by the
perception system.  Initially, we considered refining the net weights on the
fly, as the Ambler gained experience walking, but the initial function learned
proved adequate enough, and so the adaptive approach was not actively pursued.
On-line learning techniques remain a possibility, however, especially if the
rover would be exposed to novel environmental conditions (such as might occur
on Mars).

\subsubsection{Leg Recovery Planning}

The task of the leg recovery planner is to plan energy and time efficient
trajectories for the legs.  This involves minimizing joint movement
(especially the slow vertical actuators) and maximizing the amount of time all
three leg joints are actuating simultaneously.  The planner must also ensure
that the legs do not collide with the terrain, or any part of the mechanism
itself.

In general, this is a complex three-dimensional search problem.  An early
version of the leg recovery planner did in fact search a 3D space to find
optimal moves \cite{Krotkov90iros}.  This proved to be very expensive, both
in terms of planning time and the demands it placed on the perception
system.  The current leg recovery planner minimizes search by
decoupling the planning of horizontal and vertical motions.

The planner first finds a trajectory in the horizontal plane that minimizes
(horizontal) joint travel while avoiding leg/leg and leg/body collisions.  The
planner uses an explicit kinematic and geometric model of the Ambler, as well
as information on maximum joint velocities.  The algorithm handles both
circulating and non-circulating recoveries (the latter are used primarily
when shuffling legs).

Once a horizontal trajectory is determined, the planner projects an
``envelope'' of vertical motions along the trajectory.  The envelope assumes
that the leg raises/lowers at full speed while traveling horizontally.  Purely
vertical moves are needed only when the envelope is projected to intersect the
terrain, or if needed to clear the top of the Ambler's body arch.  Although
this algorithm is not necessarily optimal (a longer horizontal trajectory
might result in significantly less vertical travel), in practice the moves
produced by the leg recovery planner are very good.

\subsubsection{Remarks}

While the circulating gait and independently controllable legs give the
Ambler advantages in efficiency and mobility, planning for the Ambler is
significantly more complex than for other walkers, due to its many degrees
of freedom and relatively narrow front-to-back stability margin
\cite{Bares92ijrr}.  On the other hand, in several situations we have been
able to take advantage of the decoupling of horizontal and vertical motions
afforded by the orthogonal legs to simplify planning algorithms and to
reduce search.  Overall, the planning system is very robust and reliable,
enabling Ambler to autonomously walk along varied paths and through varied
terrains (see Section~\ref{sec:results}).

The Ambler's very deliberative mode of walking contrasts sharply with the
reactive approaches taken by most other researchers
\cite{Brooks89,Pfeiffer,Raibert86}.  The main advantage of the deliberative
approach is that by carefully analyzing kinematic and terrain constraints
it can produce more reliable and efficient moves.  Reliability is increased
by decreasing the chance of tipover or placing feet in unstable locations.
Efficiency is increased by minimizing actuator motion and minimizing the
number of energy-losing leg/terrain interactions (collisions).

The main disadvantage of the deliberative approach is that it is
computationally intensive: the Ambler planners take 30-40 seconds per step (of
which about 75\% of that time is spent by the perception system creating
terrain maps for the various planners).  This is acceptable for the Ambler due
to the slow top speed of the mechanism: the planning is still about 50\%
faster than the cycle time of body/leg moves.  By planning one step while
executing the previous step (see Section~\ref{sec:taskCtl}), the Ambler
planning system can provide a continual stream of moves to the real-time
control system, so computation is not a bottleneck for the Ambler.  With a
faster, mechanically simpler rover, we would probably change our planning
strategy, relying more on reactive (although not totally reflexive) control.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Control}
 \label{sec:control}

The Ambler system essentially uses two types of control: real-time and task-level 
control.  Real-time control is used to interface with the mechanism 
(driving servo loops, reading internal sensors, maintaining the dead-reckoned 
position).  Task-level control is concerned with integrating 
perception, planning, and real-time control into a concurrent, distributed 
system.  The task-level control manages the system's resources, schedules 
and synchronizes tasks, and provides for execution monitoring and error 
recovery.

\subsubsection{Real-Time Control}
\label{sec:rtCtl}

The design of the Ambler's real-time control system 
directly addresses the three challenges identified in Section
\ref{sec:intro}:
\begin{itemize}

\item Self-reliant control.
The rover must autonomously react to unanticipated
conditions in order to safeguard itself.  These include detecting and handling a variety 
of mechanical and electrical failures, such as amplifier faults, and unexpected 
environmental situations, such as soil failure and excessive tilt.

\item Limited power.
To avoid wasteful transfer of energy to the terrain,
the rover must faithfully execute the leg and body trajectories planned to
avoid obstacles.

\item Rugged terrain traversal.
The rover must move its legs with sufficient precision to step in tightly
constrained spaces (e.g., between two boulders).
\comment{We never really did this: Further, the rover must remain stable even when 
irregular materials underfoot shift, fracture, or deform.}

\end{itemize}

The Controller module implements the real-time control system, 
and serves as the interface to the Ambler mechanism.
The Controller has three main tasks: 
it controls the execution of leg and body moves, monitors the electronics and sensors 
to detect faults, and maintains the Ambler's dead-reckoned position.
The Controller runs on two 68030 CPU boards in a real-time, multi-tasking 
environment.
One CPU generates trajectories, maintains the dead-reckoned position,
and communicates with other modules.
The other executes horizontal and vertical body trajectories at 10 msec intervals.

\subsubsection*{Motion Control and Safeguarding}

The Controller's main task is to move the legs to a given position and to move the 
body to a given {\em pose} (position and orientation).
Given a commanded body position, 
the Controller decomposes the motion control task into two parts,
a planar translation and/or rotation followed by a vertical lift or lower.
For the planar motion, the Controller computes
the horizontal joint motions that will simultaneously
translate and rotate the body to achieve the commanded horizontal position.
It executes a horizontal body move by first locking the vertical joints,
and then servoing the twelve horizontal joints to
achieve smooth acceleration and deceleration of the body.
For the vertical motion, the Controller follows the same scheme,
but servos only the six  vertical axis joints.

Given a path for the leg to travel, denoted by a series of waypoints, 
the Controller generates a sequence of joint positions beginning with the
current leg position and ending with the final waypoint position.
It then interpolates those joint positions, producing a
trajectory that is linear in joint space.
In executing this trajectory,
the Controller coordinates the three leg joints so that all motions start
and end simultaneously, as with the body move.
During  execution, the force sensor is set to trigger an interrupt if
either normal or lateral forces exceed threshold values, signifying terrain contact.
The Controller treats the interrupt either as the successful completion of a leg 
move or as an unexpected collision,
depending on where in the trajectory it occurred.

The Controller includes many safeguards to protect the Ambler mechanism.
Before committing to execute a move, the Controller performs a kinematic and 
geometric analysis of the move to check that the move
will not exceed joint limits, that leg motions will not collide with other
parts of the mechanism (in particular, that the leg will pass safely through the center 
of the body while executing a circulating gait), and that the vehicle will remain 
statically stable at all times during the move.
If any unexpected situations occur during execution, such as premature ground 
contact, the Controller halts all motion and engages the brakes.
In addition, the Controller monitors inclinometers, and automatically levels 
the robot when the tilt exceeds a threshold value.  This also helps safeguard against 
failure of the force sensors --- if a force sensor fails to register ground contact and 
the leg is driven into the ground, at some point the tilt monitor will register and stop 
motions before the Ambler tips itself over.
For further safeguarding, a hardware safety circuit sets the brakes and shuts down all 
motions if it detects anomalies such as servo motion or amplifier faults.

\comment{Sub-section on power measurements here?  Discuss tradeoffs of driving 12 
vs. 3 horizontal actuators during body moves} 

\subsubsection*{Dead Reckoning}

Many exploration missions require or benefit from
estimates of robot pose (position and orientation) and course (sequence of
poses).
For example, without course estimation 
it would be impossible to produce a map of an
area wider than the sensor field of view, 
or to achieve such objectives as searching exhaustively
for ice in a specified region.
For such missions, dead reckoning is a widely used method 
that identifies the pose of a vehicle by integrating its position history.
We have developed a novel dead reckoning algorithm that, 
to the best of our knowledge, 
is the only one formulated for legged mechanisms.
This algorithm increases the robot's capability to address 
the challenge of self-reliant control.

To formulate the problem, we define a stationary world reference frame
$\cal W$, and a body frame $\cal B$ attached to the Ambler body,
which moves with respect to $\cal W$.
Let ${\bf x}_{w,i}$ denote the position of foot $i$ in $\cal W$.
Let ${\bf x}_{b,i}$ denote the position of foot $i$ in $\cal B$;
these positions can be computed using the mechanism kinematics.

We assume that the Ambler can be treated as a rigid body,
or equivalently,
that the feet do not move unless commanded. 
Then the dead reckoning problem amounts to
finding the rigid body transformation (the rotation matrix {\bf R} and 
the translation vector ${\bf t}$) such that
\begin{equation}
{\bf x}_{w,j} = {\bf R} {\bf x}_{b,j} + {\bf t} ~.
\label{eq:main}
\end{equation}
We seek a solution that minimizes the squared error
\begin{equation}
\sum_{j=1}^{6} w_j {\bf e}^T_j {\bf e}_j ~,
\label{eq:sum}
\end{equation}
where ${\bf e}_j = {\bf x}_{w,j} - {\bf R} {\bf x}_{b,j} - {\bf t}$,
and the $w_j$ are weights on the observed positions.

We implemented a solution to this minimization problem in three steps
\cite{Krotkov92irosdeadreck}:
\begin{enumerate}
\item Identify feet that have slipped,
and prevent them from being used in  further computation
by setting $w$ to zero.  Foot slippage is detected using the constraint that the 
distances between feet should not be affected by a body move: by noting the 
distances before and after a move, it can be determined which feet might have 
slipped relative to one another. 

\item Solve for the rigid body transformation that best transforms foot
positions in $\cal B$ to $\cal W$, 
using a technique, developed by Matthies \cite{Matthies89phd},
based on singular value decomposition.

\item Update foot positions, if necessary,
based on the new body pose and the current leg joint angles.
Ideally, no update should be required, but in practice, the least-squares
solution may cause discrepancies between the current and stored 
foot positions.

\end{enumerate}

To test the implementation of dead reckoning, we conducted
trials in which we commanded the Ambler to advance a specified distance.
After execution of the command, we computed the dead reckoned pose and
measured the {\em ground truth} pose by using a surveying device.
For each move we recorded the dead reckoned and ground truth poses.

Figure \ref{fig:positioning} illustrates the results of one trial with
12 body advances covering six meters along a straight line.
The figure plots the planar error $\sqrt{ \delta_x^2 + \delta_y^2 }$,
where the $\delta$ terms represent the component-wise differences 
between
dead reckoned and ground truth poses.
This data indicates that the dead reckoning error $u$ is two percent of the
distance traveled.
Given the high precision of the prismatic and rotary joints,
and the high encoder resolution, this accuracy is lower than expected.

\begin{figure}
\vspace{1.5 in}
\special{psfile=fig/plotErrors.ps hoffset=108}
\caption{Dead-reckoned position error}
{\small The points represent the difference between the dead reckoned and
ground truth body positions in the plane of travel.
With each advance, 
the error increases by two percent of the distance traveled.}
\label{fig:positioning}
\end{figure}

Further analysis of the data show that systematic error dominates
random error:  dead reckoning ``thinks'' that the body travels a
smaller distance than does the surveying device.
We believe the source of the systematic error to be an incomplete
kinematic model of the Ambler mechanism,
one that does not  adequately address factors such as small differences
between leg dimensions, or structural deflection.
To reduce the systematic error, we could adjust the dead reckoned planar pose by
adding $( u \cos \theta , u \sin \theta)$, 
where $\theta$ is the heading.
With this correction, we anticpate accuracy better than one percent.

\subsubsection{Task-Level Control}
\label{sec:taskCtl}

Task-level control involves the integration and coordination of perception,
planning and real-time control to produce a complete robotic system.  The
Ambler's use of task-level control impacts two of the three challenges
described in Section~\ref{sec:intro}.

\begin{itemize}

\item Self-reliant control.  The rover must be able to autonomously detect
and recover from a wide range of exceptional conditions, ranging from
hardware faults to situations that the normal planning software cannot
handle.

\item Limited power. Efficient use of the rover's resources can minimize
overall power consumption.  This includes reducing the idle time, since
there is a fixed overhead incurred just standing still, which argues for use
of a concurrent, distributed software architecture.

\end{itemize}

The Ambler subsystems are integrated using the {\em Task Control
Architecture (TCA)}.  The architecture provides a language of
communication and control constructs for coordinating distributed mobile
robot systems, and a library of utilities to facilitate development of
concurrent robotic systems \cite{Simmons90darpa,Simmons92conc}.  The
communication constructs support coarse-grained message passing between
processes, with TCA handling the tasks of encoding and decoding message
data and routing messages and responses to the appropriate processes.
The TCA control constructs support hierarchical planning, task and
resource scheduling, monitors and exception handling.  The control
constructs are designed around the concept of hierarchical {\em task
trees,} which is the TCA representation for plans (see
Figure~\ref{fig:taskTree}).

\paragraph{Concurrency}

Besides encoding hierarchical goal/subgoal relationships, TCA task trees
encode temporal constraints between subtasks.  The temporal constraints
enable the specification of partial orderings between subtasks.  For
example, the task tree in Figure~\ref{fig:taskTree} indicates that to walk
along some arc (following the algorithm described in Section~\ref{sec:walkAlg}), the 
rover must take a sequence of steps.  Each step consists
of recovering the leg, moving the body, and recursively taking a sequence of
steps (until the desired goal position is reached).  The ``sequential
achievement'' temporal constraints indicate that subtasks must be executed
in order; in particular, all command messages (the leaf nodes in one
subtree) must be handled before any of the commands of the subsequent
subtask can begin.

\begin{figure}
\vspace{2.5 in}
\caption{Task tree for Ambler walk planning}
\label{fig:taskTree}
\end{figure}

A ``delay planning'' constraint indicates that a goal message cannot be
handled (i.e., decomposed into subtasks) until the previous task has been
achieved.  Thus, due to accumulated dead reckoning error we specify that 
the
plan for walking along one arc should not be planned out until the previous
arc has been traversed.  On the other hand, the absence of a ``delay
planning'' constraint between the ``moveBody'' and ``takeSteps'' subtasks
indicates that the system can plan out one step while executing the previous
one.  By having planning and execution occur concurrently, the Ambler 
system
can walk nearly continuously, doubling the average walking speed compared
with the original sequential sense/plan/act system \cite{Simmons92conc}.  
In
a similar manner, we use TCA control constructs to specify when laser scans
should be initiated and to indicate that processing of the range images can
occur concurrently with planning and execution.

\begin{figure}
\vspace{2.5 in}
\caption{Timings of Ambler modules during concurrent walking cycle}
\label{fig:timings}
\end{figure}

Figure~\ref{fig:timings} shows a Gantt chart of the resource utilization of
the various modules.  Utilization of the planning and perception modules
increased by about 33\% over the original sequential system, while the
real-time controller was nearly always busy (95\% of the time).  The average
walking cycle dropped from 80 seconds to about 60 seconds.  This reduces
power consumption per distance traveled, since many components (e.g,
computers and amplifiers) draw significant current at all times.

\paragraph{Monitoring and Error Recovery}

To be truly self-reliant, an autonomous mobile robot system needs to react
to environmental changes in an appropriate manner.  In particular, it must
determine when its current course of action is failing, and try alternative
plans.  The Task Control Architecture facilitates building such self-reliant
systems through its monitoring and exception handling constructs.

The Ambler system utilizes several strategies to provide self-reliance:
1)~use of redundant monitoring and sensing, 2)~use of a hierarchy of
exception handlers, and 3)~incrementally increasing reliability by layering
monitors and exception handlers.

Redundancy is important in constructing reliable systems --- single points
of failure should be avoided if at all possible.  This is manifested in the
Ambler system partly through the use of redundant hardware and software
safety checks during critical operations, and partly through the use of
redundant sensing and monitoring.  For example, the planning code uses a
model of the Ambler to plan moves that fall within the kinematic limits of
the mechanism.  The real-time controller uses a similar geometric model to
assure the validity of moves prior to executing them.  Finally, hardware
limit switches will be triggered in cases where the software fails to limit
the moves.  Similarly, the real-time controller monitors the force sensors
in the feet to detect ground contact.  Occasionally the sensors fail,
however, causing the leg to jack up the body as it drives into the ground.
This situation is detected by monitoring the inclinometers: if they detect
more than one degree of tilt, all motion is halted and the body is
automatically releveled.

An organizing principle used in the Ambler software system was to keep
planning and exception handling functions separate, using separate modules
for each, but with the exception handlers organized into a hierarchy similar
to the planning hierarchy.  These principles of separating and
hierarchically structuring exception handlers are directly supported by TCA.
TCA provides control constructs to associate monitors and exception handlers
with nodes in the task tree.  When a module raises an exception, or a
monitor detects a particular condition, TCA searches up the task tree to
dispatch the closest handler.  If that handler cannot deal with the error,
the search continues up the tree.  In this way, exception handling is kept
context-dependent and  handlers are tried in order of decreasing
specificity.  This usually results in the lower levels handling the majority
of exceptions, with minimal impact on the overall walking plan.

\begin{figure}
\vspace{2.5 in}
\caption{Task tree with monitors and exception handlers}
\label{fig:errors}
\end{figure}

Figure~\ref{fig:errors} shows the positions in the task tree of some of the
monitors and exception handlers used in the Ambler system.  Before every
move, a stability monitor checks whether the move keeps the Ambler's 
center
of gravity within the support polygon of the legs, aborting the walking
cycle if the move might cause the Ambler to tip over.  After every footfall,
a monitor analyzes the force/torque measurements to determine if the foot is
in an unstable configuration (i.e., if the point of ground contact is very
off-center).  If so, the leg is moved 10 cm at a time away from the point of
contact until a stable footfall is found.

During leg motion, the controller monitors the force sensor and stops motion
if contact is detected.  An exception handler moves the foot up and away
from the point of contact and then commands the controller to finish the leg
move.  The controller also detects hardware faults (e.g., amplifier faults, servo
faults, limit switches).  For faults that might be intermittent, the Ambler
system retires the move, giving up (and cancelling all active tasks) if the
fault continues after several attempts.  

Finally, there are several exception handlers to deal with failures of the
planners to find suitable plans.  If the gait planner fails, the first
exception handler tries to replan with the latest robot pose (since the
system plans in advance, sometimes the planned and actual poses differ).  If
this fails, the system {\em shuffles} the legs, putting them  into a
standard configuration, after which the normal planning cycle takes over.
Failing that, the walking tasks are terminated, and the user must input a
new path to traverse.

These monitors and exception handlers (and there are several others, as
described in \cite{Simmons92iros}), together with those implemented in the
real-time controller (described in Section~\ref{sec:rtCtl}) provide the
software system with enough reliability to walk many steps without human
intervention.  In particular, the Ambler has walked continually over 1000
steps (over half a kilometer).  Exceptional situations are detected on the
average of once every 10--20 steps, depending on the ruggedness of the
terrain and the curvature of the path.  Only on rare occasions is control
passed back to the human operator --- most situations are handled at the
lower levels.  Even on those rare occasions, however, the Ambler system
safely halts motions and maintains itself in a stable configuration while
awaiting further goals from humans.  This is exactly the type of behavior
one requires of a spatially and temporally remote vehicle, such as one
operating on the Martian surface.

\paragraph{Remarks}

One of the more difficult tasks in developing concurrent, distributed mobile 
robot systems is dealing with interactions: both interactions between the 
vehicle and the environment, and interactions among subsystems.  These 
include problems of handling changes and uncertainty in the environment, 
resource contention between modules, race conditions, etc.  The Task Control 
Architecture provides support for controlling interactions, such as managing 
physical and computational resources, synchronizing subtasks, and 
monitoring and handling exceptional situations.

Besides providing a set of commonly needed control constructs, the system 
development methodology supported by TCA (and used in building the 
Ambler system) facilitates dealing with interactions.  First, all control 
decisions are encoded explicitly in the module software and maintained 
explicitly by TCA.  This provides for greater predictability and 
understandability of what the system will do in various situations: exactly 
how subtasks are temporally related, under which contexts monitors and 
exception handlers are active, etc.  Second, TCA enables new functionality to 
be added incrementally, without modifying existing code.  For example, with 
Ambler we first developed a sequential sense/plan/act system (which was 
easier to test and debug) and then added concurrency by removing temporal 
constraints \cite{Simmons92conc}.  Similarly, we added new monitors and 
exception handlers incrementally as our experimentation with Ambler 
uncovered limitations in the way the robot reacted to unexpected situations 
\cite{Simmons92iros}.

All this flexibility comes for a price, however, since TCA must maintain the 
task trees and track all message traffic in order to assure that the desired 
control decisions are carried out.  While there is the potential for a 
centralized system bottleneck, in practice we have not found this to be a 
problem.  For the Ambler system, the TCA central control module is active 
only about 3\% of the time.  We view this as a small price to pay for the 
flexibility and ease of use afforded by using the architecture.

