Sidescan Sonar Aided Inertial Drift Compensation
in Autonomous Underwater Vehicles
Romain Michalec and Cédric Pradalier
Georgia Institute of Technology – CNRS (UMI 2958), Metz, France
romain.michalec|cedric.pradalier@gatech.edu
Abstract—We introduce our first work on terrain-based navigation of autonomous underwater vehicles using sidescan sonars
and sidescan sonar maps. It consists in estimating the state of
the vehicle from its inertial proprioceptions and sidescan exteroceptions using a particle filter. The novelty is the use of sidescan
acoustic perceptions and sidescan sonar maps instead of singlebeam bottom-looking acoustic perceptions and bathymetric maps.
Also, the approach is not landmark-based. Our first simulations,
although conducted under simplifying hypotheses, show that the
approach is valid and may be applied to more complex situations.
Therefore, we propose to investigate the use of sidescan
sonars and sidescan maps for terrain-based navigation, with a
Sequential Monte Carlo method, or particle filter. In this paper,
we study the feasability of this approach on a simple trajectory:
a constant-depth, constant-speed straight line, used both for
the acquisition of the sidescan map and as the current mission.
II. R ELATED WORK
To the best of our knowledge, the approach we propose
has not been tried before, contrary to particle filtering using
elevation maps and single-beam bathymetric perceptions,
which is a very simple case (Bachmann and Williams 2003;
I. I NTRODUCTION
Karlsson et al. 2003; Karlsson and Gustafsson 2003), and of
Because they image large portions of the sea floor in a course localization using EKF-based feature-based SLAM and
relatively short time, sidescan sonars are commonly mounted as sidescan sonar data, which is extensively studied (e.g. Tena
imaging payload on autonomous underwater vehicles (AUVs). Ruiz et al. 2001, 2003, 2004; Newman et al. 2003; Williams
They are less often used as a source of information for and Mahon 2004; Ribas et al. 2006; Stalder et al. 2008; He et
navigation. The reason is that automated extraction of machine- al. 2009; Pinto et al. 2009; Aulinas et al. 2010).
usable information from sidescan sonar data is more difficult
The main difficulty in feature-based SLAM approaches is
than from simpler sensors.
automatic feature recognition, and it is still an ongoing problem
Inertial and Doppler measurements for instance, as well (Stutters et al. 2008). The reason for that is that naturally
as GPS and LBL/USBL fixes, are easy to integrate into a occurring underwater features that are physically meaningful
navigation control framework, through Kalman filtering for can have very different sonar returns when insonified from
example. Single-beam bathymetric data and, to a lesser extent, different viewing angles (Leblond et al. 2005, 2008), and they
multibeam bathymetric data are also easy to take into account, often have amorphous shapes (while SLAM works well with
since their interpretation as distances to the sea floor or point and line features). Again, this is representative of the
obstacles is straightforward. Sidescan images, on the other difficulty for a computer to interpret images and choose relevant
hand, are easy to interpret for humans but less so for computers, features on which to localize. On top of that, SLAM algorithms
especially onboard ones that are limited in terms of processing don’t scale well: their computational requirements increase
power.
quadratically with the number of landmarks used. This makes
This is a pity because sidescan images are potentially very FastSLAM necessary when dealing with large areas. Last but
informative, due to their large swath range. They might bring not least, reducing a whole image to a set of point features
good accuracy and fast convergence to localization methods, as also means that information that could help localization is lost.
well as reliable following of learned trajectories when repeating Avoiding that loss and the difficulty of feature extraction are
missions. In the later case, using sidescan sonars instead of reasons to consider particle filters on sidescan sonar data.
only inertial or Doppler sensors would bring the benefit of drift
Of course, the dependency on a map makes particle filtering
compensation in missions when GPS resurfacing and LBL/ unadapted for missions in areas which have not been previously
USBL localization are undesirable or impossible: missions near mapped. However, if an AUV needs to navigate precisely
the sea floor or in the water column, under-ice operations, cost in a well-known environment such as the area surrounding
of the instrumentation of the mission location with acoustic a harbor, an existing map will often be available (Stutters
beacons and their surface vessels/buoys, and so on.
et al. 2008). Such maps are almost always elevation maps
(bathymetric maps) constructed from multibeam surveys. As
The research leading to these results has received funding from the European
Commission FP7-ICT Cognitive Systems, Interaction, and Robotics under the mentioned before, particle filters have been investigated for use
contract #270180 (NOPTILUS).
with these maps and single-beam bathymetric perceptions only
Index Terms—Autonomous underwater vehicles, terrain-based
navigation, sidescan sonar, particle filtering.
Figure 1. Intensity of the reference starboard acoustic echoes vs. the echo
return time (top: s) and the sonar-echo distance (bottom: m). Horizontal line:
mean.
Figure 2. Intensity of the reference starboard acoustic echoes in the (depth,
starboard distance) across-track insonification plane (m).
(Bachmann and Williams 2003; Karlsson et al. 2003; Karlsson
and Gustafsson 2003).
III. M ODELING
We consider the following model for the underwater vessel:
0
rk
I dtI2
rk−1
= 2
+ dt INS 2 INS
ak−1 − wk−1
vk
02 I2
vk−1
hSSS
= h(rk ) + wkSSS
k
Figure 3. Intensity of the starboard acoustic echoes predicted by the observation
function h from the reference perception of the figure 1 and at the position
P indicated by the blue dot on the figure 2, vs. the echo return time (top: s)
and the sonar-echo distance (bottom: m). The padding constant used in this
observation function was 12 , hence the value of the last samples. The mean of
the whole sidescan return is a better padding constant.
2
wkSSS ∼ N (02ns , σSSS
I2ns ) is the measurement noise of the
sidescan sonars, and ns is the number of echoes in one acoustic
return (the high to very high number of samples used in the
discretization of the acoustic signal received after a pulse).
The observation function h is defined below. SSS stands for
sidescan sonar.
2ns
A set (hSSS
of ni reference percepref (ri ))i∈[|0,ni −1|] ⊂ R
tions is available, previously acquired by the sonars along a
constant-depth, constant-speed straight line. This set constitutes
a sidescan map of the navigated environment, traditionally
drawn as a two-dimensional image of width 2ns and height
ni (figure 4). Let’s consider one starboard perception: it is a
function that maps the sonar-echo distance r ∈ [rmin , rmax ]
to the intensity of the echo Iref (r) ∈ [0, 1] (figure 1). We can
draw this function in the across-track vertical plane that shows
the fan-shaped volume of insonified environment (figure 2).
More precisely, the function illustrated in this plane is defined
as follow, in polar coordinates with origin on the reference
trajectory:
(
Iref (r) if (r, θ) ∈ [rmin , rmax ] × [θmin , θmax ]
I¯ref (r, θ) =
cst
otherwise
The padding constant can be, for instance, the mean of the
acoustic echoes of the sidescan return.
Let’s consider a point P in this plane, and the insonified
environment seen from it (blue fan on figure 2). We define the
following function, in polar coordinates (r0 , θ0 ) with origin at
P , by integrating the intensities of the reference echoes that
are at distance r0 from the sonar:
Z θmax
1
I¯ref (r̃, θ̃)dθ̃
Isim (r0 ) =
θmax − θmin θmin
In the state relation, (rk , vk ) ∈ R4 is the horizontal
position/velocity state vector, aINS
∈ R2 are the horizonk
tal accelerations measured by the onboard accelerometers,
2
wkINS ∼ N (02 , σINS
I2 ) is the measurement noise of these
accelerometers. INS stands for inertial navigation system.
The observation relation consists of sidescan sonar mea- where (r̃, θ̃) are the polar coordinates with origin on the
surements hSSS
∈ R2ns of port and starboard environments, reference trajectory of the points at distance r0 from the
k
Figure 4. Particle filtering on sidescan sonar map, projected onto the AUV horizontal plane. Top: initialization. Bottom: estimated state after 150 observations
in 5 s over 4.5 m. The green line is the actual trajectory, which was previously used as the insonification trajectory to create the sidescan map. The red line is
an inertial estimate of this trajectory.
sonar (both non-linear functions of (r0 , θ0 ) with square roots
and arc tangents). The resulting function is a model for the
starboard perception at point P , as simulated from the reference
perception (figure 3). Together with a port part, they make up
our observation function h.
So defined, the observation function does not depend on
the vertical position or on the attitude of the AUV. This is a
simplifying hypothesis that can be removed relatively easily
for yaw at least: indeed, since the set of reference perceptions
was acquired along a straight line, it forms a collection of
parallel across-track insonification planes, and the intersections
of the fan-shaped insonified environment seen from a point with
these planes can be computed. A number of other simplifying
hypotheses are used in this work. Those about the reference
and current trajectories have already been mentioned.
IV. S IMULATIONS
Using the developped model, we implement a sampling
importance resampling particle filter for reference trajectory
following, meaning that we consider the initial state (r0 , v0 )
to be the same as the trajectory start, known to the filter
as the mean of a normal distribution of covariance matrix
diag[σr20 I2 , σv20 I2 ].
Convergence of this filter on the map we used (ni = 900,
ns = 1000, figure 4) is achieved in less than 30 observations in
1 s over 0.9 m, and accuracy is satisfactory after less than a few
more seconds (the estimation error drops below half a meter).
This is of course due in part to the simplifying hypotheses of
this feasability study. Still, this encouraging result shows that
our approach is valid and may be applied to less simplified
situations, a goal we are currently working toward.
Because of the high number of samples in the acoustic returns
(ns ) and the high number of reference insonifications (ni ), the
computer implementation of the observation function requires
an important amount of memory and computing power, both
scarce resources on AUVs. We have found that subsampling the
acoustic returns and/or subsampling across the insonifications
too can hardly be avoided, which unfortunately results in a loss
of information and therefore a slower, less precise localization.
On the plus side, subsampling can also be used to average
out a part of the noise of the insonifications (figure 5), which
counteracts the aforementioned loss in localization quality.
Figure 5. Smoothing effect of subsampling-averaging the sidescan map. Top row: full map, ni = 769, ns = 700. Middle row: subsampling 1 insonification
in 4 without averaging and 1 echo in 5 with averaging (that is to say we averaged 5 echoes into 1). Bottom row: same subsampling, this time with averaging
across the insonifications too (that is to say we averaged 4 insonifications into 1). Graphs: starboard insonification no. 225 (no. 57 after subsampling),
corresponding to the square object visible at about 7 m along-track, 11 m across-track on starboard.
R EFERENCES
Aulinas, Josep, Xavier Lladó, Joaquim Salvi, and Yvan Petillot (2010).
“Feature based SLAM using side-scan salient objects.” IEEE Oceans (Seattle,
WA, USA, 20-23 September 2010), 1-8. URL : http://dx.doi.org/10.1109/
OCEANS.2010.5664461.
Bachmann, Alexander, and Stefan B. Williams (2003). “Terrain aided underwater navigation: a deeper insight into generic Monte Carlo localization.”
Australasian Conference on Robotics and Automation (Brisbane, Australia,
2003). URL : http://www.araa.asn.au/acra/acra2003/papers/22.pdf.
He, Bo, Ke Yang, Shuai Zhao, and Yitong Wang (2009). “Underwater simultaneous localization and mapping based on EKF and point features.” IEEE
International Conference on Mechatronics and Automation (Changchun,
China, 9-12 August 2009), 4845-4850. URL : http://dx.doi.org/10.1109/
ICMA.2009.5246398.
Karlsson, Rickard, Fredrik Gusfafsson, and Tobias Karlsson (2003). “Particle
filtering and Cramér-Rao lower bound for underwater navigation.” IEEE
International Conference on Acoustics, Speech, and Signal Processing (6-10
April 2003), 6:65. URL : http://dx.doi.org/10.1109/ICASSP.2003.1201619.
Karlsson, Rickard, and Fredrik Gustafsson (2003). “Particle filter for underwater
terrain navigation.” IEEE Workshop on Statistical Signal Processing (28
September - 1 October 2003), 526-529. URL : http://dx.doi.org/10.1109/SSP.
2003.1289507.
Leblond, Isabelle, Michel Legris, and Basel Solaiman (2005). “Use of
classification and segmentation of sidescan sonar images for long term
registration.” IEEE Oceans (20-23 June 2005), 1:322-327. URL : http:
//dx.doi.org/10.1109/OCEANSE.2005.1511734.
Leblond, Isabelle, Michel Legris, and Basel Solaiman (2008). “Apport de la
classification automatique d’images sonar pour le recalage à long terme.”
Traitement du signal (2008), 25.1-2:87-104. URL : http://hdl.handle.net/2042/
17769.
Newman, Paul M., John J. Leonard and Richard J. Rikoski (2003). “Towards
constant-time SLAM on an autonomous underwater vehicle using synthetic
aperture sonar.” International Symp. on Robotics Research (Sienna, Italy,
2003). Springer Tracts in Advanced Robotics (2005), 15:409-420. URL :
http://dx.doi.org/10.1007/11008941_44.
Pinto, Miguel, Bruno Ferreira, Aníbal Matos, and Nuno Cruz (2009). “Using
side scan sonar to relative navigation.” MTS/IEEE Oceans (Biloxi, MS, USA,
26-29 October 2009), 1-9. IEEE Industrial Electronics (Porto, Portugal, 3-5
November 2009), 2126-2141. URL : http://ieeexplore.ieee.org/xpl/login.jsp?
arnumber=5422162.
Ribas, David, Pere Ridao, José Neira, and Juan D. Tardós (2006). “SLAM
using an imaging sonar for partially structured underwater environments.”
IEEE/RSJ International Conference on Intelligent Robots and Systems
(Beijing, China, 9-15 October 2006), 5040-5045. URL : http://dx.doi.org/10.
1109/IROS.2006.282532.
Stalder, Severin, Hannes Bleuler, and Tamaki Ura (2008). “Terrain-based
navigation for underwater vehicles using side scan sonar images.” IEEE
Oceans (Quebec City, Quebec, Canada, 15-18 September 2008), 1-3. URL :
http://dx.doi.org/10.1109/OCEANS.2008.5151809.
Stutters, Luke, Honghai Liu, Carl Tiltman, and David J. Brown (2008). “Navigation technologies for autonomous underwater vehicles.” IEEE Transactions
on Systems, Man and Cybernetics, Part C: Applications and Reviews,
38.4:581-589. URL : http://dx.doi.org/10.1109/TSMCC.2008.919147.
Tena Ruiz, Ioseba, Yvan Petillot, David M. Lane and C. Salson (2001).
“Feature extraction and data association for AUV concurrent mapping and
localisation.” IEEE International Conference on Robotics and Automation
(2001), 3:2785-2790. URL : http://dx.doi.org/10.1109/ROBOT.2001.933044.
Tena Ruiz, Ioseba, Yvan Petillot, and David M. Lane (2003). “Improved
AUV navigation using side-scan sonar.” IEEE Oceans (San Diego, CA,
USA, 22-26 September 2003), 3:1261-1268. URL : http://dx.doi.org/10.1109/
OCEANS.2003.178034.
Tena Ruiz, Ioseba, Sébastien de Raucourt, Yvan Petillot, and David M. Lane
(2004). “Concurrent mapping and localization using sidescan sonar.” IEEE
Journal of Oceanic Engineering (April 2004), 29.2:442-456. URL : http:
//dx.doi.org/10.1109/JOE.2004.829790.
Williams, Stefan B., and Ian Mahon (2004). “Simultaneous localisation and
mapping on the Great Barrier Reef.” IEEE International Conference on
Robotics and Automation (26 April - 1 May 2004), 2:1771-1776. URL :
http://dx.doi.org/10.1109/ROBOT.2004.1308080.