@section(Sonar Mapping)

Primarily because of computational expense, practical real-world stereo
vision navigation systems @cite(moravec80, thorpe83) build very sparse depth
maps of their surroundings.  Even with this economy our fastest system,
described in @cite(matthies84), takes 30 - 60 seconds per one meter step
on a 1 mips machine.

Direct sonar range measurements promised to provide basic navigation and
denser maps with considerably less computation.  The readily available
Polaroid ultrasonic range transducer @cite(polaroid) was selected and a ring
of 24 of these sensors was mounted on Neptune.

We find sonar sensors interesting also because we would like to investigate
how qualitatively different sensors, such as a sonar array and a pair of
cameras, could cooperate in building up a more complex and rich description
of the robot's environment.

@subsection(Approach)

	Multiple wide-angle sonar range measurements are combined to map
the surroundings of an autonomous mobile robot. A sonar range reading
provides information concerning @r(empty) and @r(occupied) volumes in a cone
(subtending 30 degrees in our case) in front of the sensor. The reading is
modelled as probability profiles projected onto a rasterized map, where
somewhere occupied and everywhere empty areas are represented. Range
measurements from multiple
points of view (taken from multiple sensors on the robot, and from the same
sensors after robot moves) are systematically integrated in the map.
 As more readings are added the area deduced
to be empty expands, and the expanding empty area encroaches on and sharpens
the possibly occupied region.  The map becomes gradually more detailed.
The final map shows regions probably occupied,
probably unoccupied, and unknown areas. The method deals effectively with
clutter, and can be used for motion planning and for extended landmark
recognition.
It was tested in cluttered environments using our @i(Neptune)
mobile robot.

For navigation and recognition we developed a way of convolving two sonar
maps, giving the displacement and rotation that best brings one map into
registration with the other, along with a measure of the goodness of the
match.

The sonar maps happen to be very useful for motion planning. They are denser
than those made by our stereo vision programs, and computationally about an
order of magnitude faster to produce.  We presently use them with the
Path Relaxation method @cite(thorpe84) to plan local paths for our robot. 

@subsection(The Sensor)

The sonar devices being used are Polaroid laboratory grade ultrasonic
transducers @cite(polaroid). These sonar elements have a useful measuring
range of 0.9 to 35.0 ft.  The main lobe of the sensitivity function
corresponds to a beam angle of 30@+(o) at @m{-38} dB.  Experimental results
showed that the range accuracy of the sensors is on the order of 1 %.  We
are using the control circuitry provided with the unit, which is optimized
for giving the range of the nearest sound reflector in its field of view,
and works well for this purpose.

@subsection(The Array)

The sonar array, built at Denning Mobile Robotics, and mounted on the
 @i(Neptune) mobile robot is composed of:

@begin(itemize)
A ring of 24 Polaroid sonar elements, spaced 15@+(o) apart, and
mounted at an height of 31 inches above the ground (see Fig. 
@ref(Neptunerobot)).

A Z80 controlling microprocessor which selects and fires
the sensors, times the returns and provides a range value.

A serial line over which range information is sent to a VAX
mainframe that presently interprets the sonar data and performs
the higher level mapping and navigation functions.
@end(itemize)

@subsection(Representing the Sonar Beam)

Because of the wide beam angle individual rangings
provide only indirect information about the location
of the detected objects.  We combine the constraints from individual
readings to reduce the uncertainty.  Our inferences are represented
as probabilities in a discrete grid.

A range reading is interpreted as providing information about space volumes
that are probably @c(EMPTY) and somewhere @c(OCCUPIED).  We model the sonar
beam by probability distribution functions. Informally, these functions
model our confidence that the various points inside the cone of the beam are
empty and our uncertainty about the location of the point, somewhere on the
range surface of the cone, that caused the echo. The functions are based on
the range reading and on the spatial sensitivity pattern of the sonar and
are a maximum near the center axis of the beam and taper to zero
near the edges.

These probability density functions are projected on a horizontal plane
to generate map information.  We use the profiles that correspond to a
horizontal section of the sonar beam.

@subsection(Building Maps)

Sonar Maps are two-dimensional arrays of cells corresponding to a horizontal
grid imposed on the area to be mapped.  The final map has cell values in the
range (@m(-)1,1), where values less than 0 represent probably empty regions,
exactly zero represents unknown occupancy, and greater than 0 implies a
probably occupied space.  This map is computed in a final step from two
separate arrays analogous to the empty and occupied probability
distributions introduced above.  The position and the orientation of the
sonar sensor at the time of the reading are used to register the profiles of
each beam with the map.

Different readings asserting that a cell is @c(EMPTY) will enhance each
other, as will readings implying that the cell may be @c(OCCUPIED) while
evidence that the cell is @c(EMPTY) will weaken the certainty of it being
 @c(OCCUPIED) and vice-versa.

The operations performed on the empty and occupied probabilities are not
symmetrical. The probability distribution for @i(empty) areas represents a
solid volume whose totality is probably empty but the @i(occupied)
probability distribution for a single reading represents a lack of knowledge
we have about the location of a single reflecting point somewhere in the
range of the distribution. Empty regions are simply added using a
probabilistic addition formula. The @i(occupied) probabilities for a single
reading, on the other hand, are reduced in the areas that the other data
suggests is empty, then normalized to make their sum unity. Only after this
narrowing process are the @i(occupied) probabilities from each reading
combined using the addition formula.

One range measurement contains only a small amount of information.  By
combining the evidence from many readings as the robot moves in its
environment, the area known to be empty is expanded. The number of regions
somewhere containing an occupied cell increases, while the range of
uncertainty in each such region decreases.  The overall effect as more
readings are added is a gradually increasing coverage along with an
increasing precision in the object locations.  Typically after a few hundred
readings (and less than a second of computer time) our process is able to
"condense out" a comprehensive map covering a thousand square feet with
better than one foot position accuracy of the objects detected.  Note that
such a result does not violate information theoretic or degree of freedom
constraints, since the detected boundaries of objects are linear, not
quadratic in the dimensions of the map.  A thousand square foot map may
contain as little as a hundred linear feet of boundary.

@section(Map Matching)

	We have also developed a procedure that can match two maps and
report the displacement and rotation that best takes one into the other.

	Our most successful programs begin with the thresholded maps
described above, with cell values that are negative if the cell is empty,
positive if occupied and zero if unknown.

	A measure of the goodness of the match between two maps at a trial
displacement and rotation is found by computing the sum of products of
corresponding cells in the two maps.  An occupied cell falling on
an occupied cell contributes a positive increment to the sum, as does an
empty cell falling on an empty cell (the product of two negatives). An empty
cell falling on an occupied one reduces the sum, and any comparison
involving an unknown value causes neither an increase nor a decrease.
This naive approach is very slow. Applied to maps with a linear
dimension of @i(n), each trial position requires @p(O)(@i(n@+(2)))
multiplications. Each search dimension (two axes of displacement and
one of rotation) requires @p(O)(@i(n)) trial positions. The total
cost of the approach thus grows as @p(O)(@i(n)@+(5)). With a typical
@i(n) of 50 this approach can burn up a good fraction of an hour
of Vax time.

Considerable savings come from the observation that most of the
information in the maps is in the occupied cells alone.  Typically
only @p(O)(@i(n)) cells in the map, corresponding to wall and object
boundaries, are labelled occupied. A revised matching procedure compares
maps A and B through trial transformation @i(T) (represented by a 2x2
rotation matrix and a 2 element displacement vector) by enumerating
the occupied
cells of A, transforming the co-ordinates of each such cell through
 @i(T) to find a corresponding cell in B. The [A, B] pairs obtained this way
are multiplied and summed, as in the original procedure.
The occupied cells in B are enumerated and multiplied with
corresponding cells in A, found by transforming the B co-ordinates through
 @i(T)@+(@ @m(-1)) (the inverse function of @i(T)), and these products are
also added to the sum. The result is normalized by dividing by the total
number of terms.  This procedure is implemented efficiently by
preprocessing each sonar map to give both a raster representation and a
linear list of the co-ordinates of occupied cells. The cost growns as
 @p(O)(@i(n)@+(4)), and the typical Vax running time is down to a few
minutes.

	A further speedup is achieved by generating a hierarchy of
reduced resolution versions of each map.  A coarser map is produced from a
finer one by converting two by two subarrays of cells in the original into
single cells of the reduction. Our existing programs assign the maximum
value found in the subarray as the value of the result cell, thus preserving
occupied cells.  If the original array has dimension @i(n), the first
reduction is of size @i(n)/2, the second of @i(n)/4 and so on. A list of
occupied cell locations is produced for each reduction level so that the
matching method of the previous paragraph can be applied. The maximum number
of reduction levels is @b(log)@-(2)@i(n). A match found at one level can be
refined at the next finer level by trying only about three values of each of
the two translational and one rotational parameters, in the vicinity of the
values found at the coarser level, for a total of 27 trials. With a moderate
a-priori constraint on the transformation this amount of search is adequate
even at the first (coarsest) level.  Since the cost of a trial evaluation is
proportional to the dimension of the map, the coarse matches are inexpensive
in any case.  Applied to its fullest, this method brings the matching
cost down to slightly larger than @p(O)(@i(n)), and
typical Vax times to under a second.

	We found one further preprocessing step is required to make the
matching process work in practice. Raw maps at standard resolutions (6 inch
cells) produced from moderate numbers (about 100) of sonar measurements have
narrow bands of cells labelled @i(occupied).  In separately generated maps
of the same area the relative positions of these narrow bands shifts by as
much as several pixels, making good registration of the occupied areas of
the two maps impossible.  This can be explained by saying that the high
spatial frequency component of the position of the bands is noise, only the
lower frequencies carry information.  The problem is fixed by filtering
(blurring) the occupied cells to remove the high frequency noise.
Experiment suggests that a map made from 100 readings should be blurred with
a spread of about two feet, while for map made from 200 readings a one foot
smear is adequate.  Blurring increases the number of cells labelled
 @i(occupied). So as not to increase the computational cost from this
effect, only the final raster version of the map is blurred. The occupied
cell list used in the matching process is still made from the unfiltered
raster.

With the process outlined here, maps with about 3000 six inch cells made
from 200 well spaced readings can be matched with an accuracy of about six
inches displacement and three degrees rotation in one second of Vax time.

@section(Results)

We incorporated the sonar map builder into a system that successfully
navigates the Neptune robot through cluttered obstacle courses.  The
existing program incrementally builds a single sonar map by combining
the readings from successive vehicle stops made about one meter apart.
Navigation is by dead reckoning - we do not yet use the sonar map matching
code. The next move is planned in the most recent version of the map by a
path-planning method based on path relaxation @cite(thorpe84).  Since this
method can cope with a probabilistic representation of occupied and empty
areas and does path-planning in a grid, it fits naturally into our present
framework. The system has successfully driven Neptune the length of our
cluttered 30 by 15 foot laboratory using less than one minute of computer
time.