5024 IEEE SENSORS JOURNAL, VOL. 15, NO. 9, SEPTEMBER …

5024 IEEE SENSORS JOURNAL, VOL. 15, NO. 9, SEPTEMBER 2015

Improving Localization Accuracy for an UnderwaterRobot With a Slow-Sampling Sonar

Through Graph OptimizationLing Chen, Member, IEEE, Sen Wang, Student Member, IEEE, Huosheng Hu, Senior Member, IEEE,

Dongbing Gu, Senior Member, IEEE, and Liqing Liao

Abstract— This paper proposes a novel localization algorithmfor an autonomous underwater vehicle equipped with amechanical scanning sonar that has a slow frequency of datasampling. The proposed approach incrementally constructs apose graph and conducts graph optimization to correct the robotposes. The construction of a pose graph has three stages: 1) scangeneration which incorporates an extended Kalman filter-baseddead reckoning algorithm that takes the robot motion intoaccount while eliminating the sonar scan distortion caused bythe motion; 2) data association which is based on Mahanalobisdistance and shape matching for determining loop closures; and3) scan matching which calculates constraints constructs posegraph. The constructed pose graph is then fed into a graphoptimizer to find the optimal poses corresponding to each scan.A trajectory correction module uses these optimized poses tocorrect intermediate poses during the process of scan generation.Both simulation and practical experiments are conducted toverify the viability and accuracy of the proposed algorithm.

Index Terms— Autonomous underwater vehicle, localization,mechanical scanning sonar, graph optimization.

I. INTRODUCTION

OVER years, Autonomous Underwater Vehicles (AUVs)have been increasingly developed for underwater

searching [1], exploring [2], reconnaissance [3], mining [4],etc. To achieve these tasks, it is necessary for algorithmsto keep track of the AUVs’ positions, which is knownas localization. The various solutions have been proposedto the problem of underwater localization from differentperspectives, ranging from simple dead reckoning,baseline-based acoustic positioning system to more complexbeacon-based sensor fusion techniques, including most

Manuscript received March 17, 2015; accepted May 7, 2015. Date ofpublication May 12, 2015; date of current version July 13, 2015. This workwas supported by the National Natural Science Foundation of China underGrant 61165016. The work of L. Chen and S. Wang was supported in part bythe China Scholarship Council and in part by the University of Essex. Theassociate editor coordinating the review of this paper and approving it forpublication was Dr. M. R. Yuce.

L. Chen is with the School of Mechatronics Engineering and Automation,Shanghai University, Shanghai 200072, China (e-mail: [email protected]).

S. Wang, H. Hu, and D. Gu are with the School of Computer Scienceand Electronic Engineering, University of Essex, Colchester CO4 3SQ, U.K.(e-mail: [email protected]; [email protected]; [email protected]).

L. Liao is with the School of Information Science and Engineering, CentralSouth University, Changsha 410083, China (e-mail: [email protected]).

Color versions of one or more of the figures in this paper are availableonline at http://ieeexplore.ieee.org.

Digital Object Identifier 10.1109/JSEN.2015.2432082

popular ones such as Simultaneous Localization andMapping (SLAM).

Due to the nature of underwater environment, the perceptionsensors used in AUVs are normally restricted to vision andsonar. Many vision-based SLAM algorithms have beendeveloped since last 90’s. However, most of the research hasbeen focused for terrestrial [5], [6] or flying robot [7], [8],and very few are focused on underwater environments. Thisis because underwater illumination upon objects is seriouslydegraded and distorted by water, making it difficult to selectrobust features for SLAM. In contrast, sonar sensors areimmune to light conditions, and work well underwater sincesound transmits faster. Thus, sonar becomes the frequentlyused perception sensors for underwater localization andmapping.

Since a Mechanical Scanning Sonar (MSS) sonar islow-cost, small-size, low-power-consumption and light, it isvery popular in small AUV applications, e.g. Ictineu AUV [9],a robotic fish [10]. However, MSS suffers from slow samplingfrequency and low accuracy, which is difficult to be directlyused for effective and accurate SLAM algorithms.

Throughout the literature, the existing SLAM algorithmsusing MSS are either feature-based Extended Kalman FilterSLAM [9] or scan matching-based EKF SLAM [11].However, EKF SLAM is subject to quadratic updatecomplexity and linearization errors [12]. GraphSLAM, whichformulates SLAM using a graph, has the access to the fulldata set when building the map so that it can revise past dataassociation and linearize more than once and hence is moreaccurate and robust than EKF SLAM [12]. Although thereare various state-of-art approaches in literature, no existingwork has focused on the employment of GraphSLAM inAUVs with MSS. Due to the slow scanning rate of MSS,the challenge lies in how to construct a robust front-end posegraph.

This paper proposes a novel implementation of theGraphSLAM algorithm for improving localization accuracyof an AUV equipped with a MSS. In order to achieve goodaccuracy, it takes the advantages of GraphSLAM and adoptsvarious strategies such as scan generation module to eliminatesonar image distortion and trajectory correction module tocorrect poses during the process of scan generation. Therobust front-end pose graph is constructed firstly by the graph

1530-437X © 2015 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.

CHEN et al.: IMPROVING LOCALIZATION ACCURACY FOR UNDERWATER ROBOT WITH SLOW-SAMPLING SONAR BY GRAPH OPTIMIZATION 5025

construction algorithm which mainly consists of three stages:1) scan generation which incorporates a novel EKF based deadreckoning algorithm while preliminarily eliminating the sonarimage distortion caused by the robot motion during scanning;2) data association which is based on Mahanalobis distanceand shape matching for determining loop closures; 3) scanmatching which is conducted to calculate constraints andconstruct pose graph based on the results of data association.Once the pose graph has been constructed, the back-endoptimizing framework iSAM [13] is then utilized to obtainthe optimal estimation of the poses corresponding to eachgenerated scan. Utilizing the optimized poses, a trajectorycorrection module is used to correct poses during the processof scan generation, which further eliminates the impact ofslow-sampling characteristic of the MSS. As verified by theexperiments, thanks to the utilization of GraphSLAM andadoption of various special strategies for error correction, theproposed localization algorithm produces more accurate poseestimation of the AUV compared with dead-reckoning anduspIC [14] which is known as the most accurate existingalgorithm using MSS.

The rest of the paper is organized as follows. Relatedresearch is outlined in Section II. Section III describes thearchitecture and algorithm of the proposed novel localizationapproach, which is a GraphSLAM using MSS. Section IVintroduces a scan generation process which takes theAUV’s motion into account. Then, a data association methodis presented in Section V and the scan matching is explained inSection VI respectively. In Section VII, a trajectory correctionmodule is illustrated to correct poses during the process of scangeneration. Experimental results are given in Section VIII todemonstrate the feasibility and performance of the proposedmethod. Finally, a brief conclusion and future work arepresented in Section IX.

II. RELATED RESEARCH

Methodologies applied for underwater localization vary withthe sensors that are involved. There are a number of sensorsused in localization of AUVs, such as Inertial MeasurementUnit (IMU), Doppler Velocity Log (DVL) and exteroceptivesensors (acoustic-based sonars and cameras). Since it isdifficult for the AUVs to deploy accurate encoders to provideodometry information as terrestrial mobile robot does,IMU and DVL [15]–[17] are utilized to obtain the deadreckoning by integrating accelerations and rotational ratesfrom IMU and velocities from DVL. The dead reckoningestimation generally serves as the prediction phase of aBayesian-filtering based localization algorithm. IMU and DVLare widely mounted on AUVs due to the fact that they donot require any external aid, but there are accumulated errorsresulted from the process of integration calculation. Thus,instead of being used alone, they are normally fused with otherexteroceptive sensors to obtain more accurate localization.

One of the typical Bayesian-filtering based localizationsystems is to fuse IMU, DVL and one or more geo-referencedsurface mobile beacons with known positions by externalsources such as GPS. This is also known as beacon-basedlocalization. The surface mobile beacons broadcast their

real-time absolute poses to the AUV. The AUV measures theranges to the mobile beacon in order to use them to bound thelocalization errors accumulated by dead-reckoning. The filtersapplied are normally EKF [18] and particle filter [19] whichfuse the sensor information from IMU, DVL and the rangesbetween the AUV and the mobile beacon. The cooperationof AUV and the mobile surface enables the effectivelocalization area to be enlarged enormously, however,external geo-referenced mobile beacon is needed whichincreases running cost and complexity.

In contrast to the beacon-based localization, underwaterSLAM system fuses IMU, DVL with exteroceptive sensorssuch as cameras and acoustic-based sonars deployed onthe AUVs, which does not need any external sensors.These exteroceptive sensors provide observation about theunderwater environment for the updating phase of theSLAM algorithm. Due to the fact that the light condition inunderwater is seriously deteriorated by water and underwaterobjects, it is difficult to select robust features for SLAM usingvision sensors. In contrast to vision sensors, sonar sensors arenot affected by bad light condition as they are designed basedon acoustics. Sonar works better in underwater than on landsince sound transmits faster in water than in the air. Thus,sonar becomes the more frequently used perception sensorsfor underwater SLAM.

There are three types of underwater sonar sensors thathave been deployed in underwater SLAM applications, namelyside-scan sonar [20], [21], multi-beam sonar [22], [23],MSS [9], [11], [24]. For a side-scan sonar, the inertial sensorssuch as DVL and IMU are firstly used to provide navigationdata relative to the vehicle reference frame, such as velocities,orientations and depth. Then the features extracted fromside-scan sonar are applied as the landmarks for updating boththe navigation data and the positions of landmarks under theEKF SLAM. However, features are difficult to identify andhave typically low spatial density [23], degrading the accuracyof SLAM.

One of the examples of applying multi-beam sonar forSLAM is the work [23]. For a multi-beam sonar, themeasurement model is either the depth governed withdistributed particle filter framework [22] or the displacementestimated by scan matching of different patches, instead ofextracting features. However, both the side-scan sonar and themulti-beam sonar are heavy even in underwater environmentwhich hinders its application on small AUV whose payload islimited.

Whereas, for a MSS sonar, its advantages of low cost,small size and weight and low power make it very competitivein AUV applications. By parsing the image data from theMSS sonar, the range-angle data can be obtained and usedfor SLAM. Ribas et al. [9] extract line features from therange-angle data of MSS by developing a voting algorithm.An EKF SLAM algorithm is then designed to govern theextracted features to produce accurate pose estimation ofthe AUV. However, in most cases, it is difficult to extractfeatures in natural areas. Instead of extracting features,Angelos et al. [11] conduct scan matching between groupedscans from the range-angle data to generate the


observation measurement. A EKF SLAM is proposedfor pose estimation by registering consecutive scans andnon-consecutive but overlapping previous ones. The utilizationof EKF SLAM algorithm in these two work take effects onlywhen both the area of environment and the number of featuresare small, due to the fact that computational complexity andlinearization errors increase dramatically with the growingscale of environment and features.

Rather than deploying SLAM-based framework,[14] proposed a scan matching based localization algorithmuspIC. In order to eliminate the impact caused by slowsampling frequency, a set of discrete scanned points aregrouped to build the scan taking into account vehicle motionestimated via dead reckoning. Registration is then conductedbetween consecutive grouped scans to obtain the displacementthat the AUV has travelled during these two scans. Althoughspecial strategies on data structure are taken to alleviate themotion induced distortion, the error of scan matching is stillaccumulated, which leads to unbounded error as the vehicletravels.

The SLAM algorithm that has come into sight and gainedmuch attraction recently is GraphSLAM. It provides anintuitive way of formulating SLAM by using a graph whosenodes correspond to poses of the robot at different pointsin time and whose edges represent constraints between theposes [25]. The major advantage of GraphSLAM is that it hasthe access to the full data set when building the map so that itcan revise past data association and linearize more than once,which enables it to be more accurate than EKF SLAM [12].GraphSLAM consists of two parts, i.e. a front-end that is incharge of graph construction and a back-end that optimallycomputes the best map. The literature on back-end is vast,which will not be recapitulated here. Instead, we focus ourattention mainly on the front-end which constructs a robustpose graph using MSS while taking slow scanning rate ofMSS into account. In addition, special strategies such as scangeneration module and trajectory correction module are takento decrease the pose estimation error.

III. THE PROPOSED LOCALIZATION ALGORITHM

A. The Slow-Sampling Problem With MSS

A MSS performs scans in a horizontal 2D plane by rotatinga mechanically actuated transducer head at pre-set angularincrements as shown in Fig. 1. At each angular position, theMSS head emits an acoustic fan shaped beam with a narrowhorizontal beamwidth and returns a set of bins whose valueare the amplitudes of echo. Gathering all the amplitude aswell as the range at each angular position, an image of echoamplitude vs. range measurements is then generated as can beseen Fig. 2a.

With a slow sampling frequency, a MSS needs a fewseconds to complete one full 360° scan at the nominal setting.The time could be even longer for the long range scan andhigh step resolution. During such a long period of scanning,the AUV can move significantly far away from the originalposition where the first reading is taken. Such motion willseriously distort the scanning image. As can be seen inFig. 2a which shows the image without considering the vehicle

Fig. 1. A working MSS.

Fig. 2. Sonar images of 360° over a real rectangular testing site whereMSS data was sampled. For the sake of better illustration, a small fraction ofthe image is omitted. The color depth represents the echo amplitude of thesonar bins. (a) Sonar image without considering vehicle motion. (b) Sonarimage corrected by the proposed algorithm.

motion, there is a big breach and discontinuity on the right sideas opposed to the actual rectangular testing site. This meansthe sonar image is seriously distorted by the vehicle motionduring the sonar scanning process. Such distortion imposeschallenges in utilizing the distorted image for localization.However, when motion is taken into account and vehicleposes are accurately estimated by our proposed algorithm,the corrected image shown in Fig. 2b represents the realenvironment.

B. Pose Graph Formulation

In order to improve the localization accuracy thatis affected by the slow-sampling characteristic of MSS,a GraphSLAM algorithm using MSS is proposed here. A posegraph consists of nodes and edges representing constraintsbetween nodes. Denote X = {X1, . . . XT } to be a set of poses,where X i describes the pose of node i . There are two sourcesof constraints among nodes namely odometry constraints andloop closure constraints. The odometry constraint U i relatestwo consecutive pose X i and X i+1 by

X i+1 = f (X i ,U i )+ Wi (1)

Loop closure constraints U i j between two arbitrary posesX i and X j are expressed as:

X j = f (X i ,U i j )+ λi j (2)


Fig. 3. A pose-graph representation of a SLAM process.

where f is a nonlinear function that implements the motionmodel of the robot. Wi and λi j are zero-mean Gaussian errorterms with covariances �i and �i j respectively. The processof determining U i , U i j , �i and �i j is known as the front-endof GraphSLAM.

Fig. 3 shows a pose-graph representation of aSLAM process. The large circles in light green representthe nodes. The small red circles and black squares representedges. The red circles are the odometry constraints betweentwo consecutive nodes while the black squares are theloop closure constraints between two non-consecutive nodesdetermined by data association. By combining (1) and (2), theconditional probability over all robot poses X and constraintsU = {U i ∪ U i j } can be represented as

p(X|U) ∼∏

i

p(X i+1|(X i ,U i ))

︸︷︷︸Odomtry constraints

·∏

i j

p(X j |(X i ,U i j ))

︸︷︷︸Loop closure constraints

(3)

The GraphSLAM is to seek the Maximum a Posteriori (MAP)configuration of the robot poses X∗ so that the distributionof p(X|U) has its maximum. Under the assumption that theconditional probabilities are Gaussian, maximizing p(X|U) isequivalent to

X∗ = argmaxX

p(X|U) = arg minX

− log p(X|U)

= arg minX

∑

i

‖ f (X i ,U i )− X i+1 ‖2�i

+∑

i j

‖ f (X i ,U i j )− X j ‖2�i j

(4)

where ‖ α − β ‖2� denotes the squared Mahalanobis distance

with covariance �. The process of solving (4) is regarded asback-end of GraphSLAM.

C. The Proposed Algorithm and Its Architecture

By pre-processing the MSS raw data using dynamicthresholding based beam segmentation method [14], therange readings from the sensor to the obstacle are groupedtogether to be formed as reading history (RH). An EKF-basedalgorithm calculates the dead reckoning of the AUV positionsand groups them to form dead reckoning history (DRH). Thescan generation process aims to group a set of scanned pointsfrom the RH to form a full 360° scan while eliminating the

Fig. 4. The architecture of the GraphSLAM. The notations are describedthroughout this paper.

distortion caused by the vehicle motion represented by DRH.The reason why the full 360° scan is formed rests with thefact that the scan matching calculation will be more robust andaccurate when larger scanning angle range of scanned point isinvolved.

Data association is in charge of determining loop closuresbetween poses by combining the Mahalanobis distance andshape matching using the results of scan generation. TheMahalanobis distance threshold Mt and shape matchingthreshold St are used as the determining condition of loopclosures. The output O of data association are the sets ofassociated scans and their corresponding poses. The scanmatching between associated scans calculates both theconstraints between consecutive poses and the loop closureconstraints between non-consecutive poses. Combiningdead reckoning poses and constraints produces the posegraph G. The detailed description of these parts arepresented in Algorithm 2, 3 and 4 in the sections thatfollow.

Fig. 4 presents the proposed architecture of the wholeGraphSLAM algorithm for an AUV using MSS. The front-endpart consists of three parts, namely scan generation, dataassociation and scan matching and pose graph construction.Given the constructed graph G, the back-end part of theGraphSLAM algorithm estimates the optimal vehicle posesand their covariances. Using these optimal poses, the trajectorycorrection module corrects the dead reckoning positionsresulting in the corrected dead reckoning history representedas CDRH.

The corrected dead reckoning positions take part in theScanGeneration process to generate the more accuratereference scan Sre f which is used for scan matching whenconstructing the pose graph. Note that the pose graph G isconstructed incrementally and the optimally estimated posesfurther facilitate data association and pose graph constructionof next iteration.

Algorithm 1 presents the whole procedures of theproposed GraphSLAM algorithm. The algorithm starts with


Algorithm 1: GraphSLAM Algorithm With MSS

1 [TL,S] = GraphSLAM(){2 RH = [ ]; DRH = [ ]; TL = [ ]; S = [ ],X = [ ],

PX = [ ];3 for All sonar readings [ρ,Pρ ] do4 RH = RH∪[ρ,Pρ ];5 [TW

R ,PW R] = Dead Reckoning();6 DRH = DRH ∪ [TW

R ,PW R];7 if ScanDetermined(RH) then8 [Scur, Xcur,PXcur ] = SG(RH,DRH);9 if Exist(Sre f ) then

10 O = D A(S,X,PX , Scur ,Xcur ,PXcur ,Mt , St );11 [G] =

SM PGC(O, Sre f ,Xre f ,Pre f , Scur ,Xcur ,Pcur );12 [X, PX ] = i S AM(G,X);13 CDRH = T C(X, PX ,DRH);14 [Sref , X ref ,Pref ] = SG(RH,CDRH);15 TL = TL ∪ CDRH;16 else17 Sref = Scur;18 Xref = Xcur;19 Pref = Pcur;20 S = {S ∪ Sre f };21 X = {X ∪ Xre f };22 PX = {PX ∪ Pre f };23 RH = [ ];24 DRH = [ ];

grouping a set of reading history (RH) and dead reckoninghistory (DRH), and the function ScanDetermined (RH)determines whether a full 360° of sonar readings has alreadybeen sampled or not by comparing the bearing of the currentreading with 0° and 360°. The functions SG(), DA() andSMPGC() correspond to scan generation, data association,scan matching and pose-graph construction parts, respectively.The function T C() is the trajectory correction module, whosedetailed description is presented in VII. The outputs of thealgorithm are TL which stores all the corrected vehicleposes and S which groups all the sonar readings. With thesetwo vectors, a map can be rendered by:

pWo ( j) = TL( j)⊕ TR

S ⊕ p2C(ρ( j)) (5)

where pWo ( j) is the corrected global position of scanned

point pj at pose TL( j). p2C(ρ( j)) is a function ofconverting polar coordinates into Cartesian coordinates. TR

S isthe transform that describes the position and attitude of thesensor frame {S} with respect to frame {R}. ⊕ is thecompounding transformations proposed in [26].

IV. SCAN GENERATION

Front-end of the GraphSLAM is the main part of theproposed algorithm. From this section, each of the three partsthat constitute front-end will be elaborated. The scangeneration process is presented in this section which takes the

motion of the AUV into account to preliminarily eliminatethe motion distortion. The vehicle motion is estimated by anEKF-based dead reckoning. The subsequent two sections willpresent the other two parts of the front-end.

A. Dead Reckoning

It is commonly known that the mean-squared navigationerrors of an IMU increase with time. Therefore, insteadof adopting the integration-based dead reckoning scheme,an EKF is used to estimate the AUV’s pose while the sonaris scanning.

1) Process Prediction: Consider the position sk andattitude ϕk (roll, pitch and yaw) of AUV in the globalcoordinate frame as the state Xk = [sT

k ,ϕTk ]T to be estimated

at time k, where

sk = [xk yk zk

]T, ϕk = [

φk θk ψk]T.

The control input is uk = [vTk ,ω

Tk ]T where

vk = [uk νk wk

]T, ωk = [

pk qk rk]T

are the body-frame linear and angular velocities ofAUV respectively. Then, on the assumption that the samplingtime interval is �T , the process model for AUV can berepresented as the following nonlinear discrete time system:

Xk+1 = f (Xk, uk) = Xk +�T J (Xk)uk (6)

where

J (X)

=

⎡

⎢⎢⎢⎢⎢⎢⎣

cψcθ cψsθsφ − sψcφ cψsθcφ + sψsφ 0 0 0sψcθ sφsψsθ+ cψcφ sψsθcφ+ sφcψ 0 0 0−sθ cθsφ cθcφ 0 0 0

0 0 0 1 sφtθ cφtθ0 0 0 0 cφ −sφ0 0 0 0 sφ/cθ cφ/cθ

⎤

⎥⎥⎥⎥⎥⎥⎦

is the transformation matrix. Note that s, c and t in the matrixrepresent sin, cos and tan functions, respectively. Therefore,the propagation of the movement can be achieved by virtueof (6). As DVL is not applied to provide uk , it is assumedto be constant but corrupted by additive Gaussian noisewk ∼ N (0, Qu). Due to this additional noise, the error ofthe dead reckoning will be unbounded, which demands othersensor information to be applied for the error correction in theupdate phase of EKF.

Given (6), the estimate of the state is obtained as:

Xk+1|k = f (Xk|k, uk) (7)

and the prediction error covariance matrix is given by:

Pk+1|k = Fk+1Pk|kFTk+1 + Gk+1 QuGT

k+1 (8)

where Fk+1 and Gk+1 are the Jacobian matrices of the partialderivatives of the non-linear model function f with respect tothe state xk and the noise wk , respectively.

2) Update With Altimeter: The depth measured by pressuresensor is related to the vertical position of AUV. Then, thedepth measurement equation is

zd,k = Hd,k Xk + μd,k = [0 0 1 01×3

]Xk + μd,k (9)

where μd,k is a zero-mean Gaussian noise with thecovariance Rd .


Fig. 5. The relationship between various coordinate systems during scangeneration.

3) Update With Orientation From IMU: The IMUmeasurement is used to estimate the orientation by thefollowing measurement model:

za,k = Ha,k Xk + μa,k = [03×3 I3×3

]Xk + μa,k (10)

where I denotes the identity matrix of compatible dimensions.The μa,k is a zero-mean Gaussian noise with thecovariance Ra .

Then the standard EKF equations are used to update themodel prediction.

B. Scan Generation

The results of dead reckoning can then be used for scangeneration. As the sonar head rotates with a set of small-anglesteps (angle resolution), the MSS emits a fan-shaped sonarbeam. For each emitted beam at a specific bearing, an echointensity profile is returned from the environment anddiscretized into a set of data bins (echo amplitude valuesfor each distance step). By applying thresholding andsegmentation [9] to this set of data bins, the scanned point forthis particular bearing is then obtained. The purpose of scangeneration is to group each separately scanned point whichis represented with respect to a specific coordinate frame toform a full scan with the angle range of 360° as if all scannedpoints are read instantaneously like a laser scanner.

Theoretically, when each scanned point is sampled, anylocal coordinate frame of AUV could be chosen as a referenceframe. For simplicity, we choose the coordinate frame ofthe AUV at the first scan (the scanning angle is −180°)as the reference frame, namely {I }. This means all thescanned points will be represented in frame {I }, as shownin Fig. 5.

The scan generation process is described in Algorithm 2.The input parameters are the sonar’s reading historyrepresented as RH and the dead reckoning history representedas DRH. As a returned value, the generated scan is denotedas Scur . All Scur form a set of scan represented as S.Xcur and PXcur are the vehicle position and its covariancewhen the 1st scan is obtained. The function FindCorrespondingDR() aims for finding the correct deadreckoning corresponding to the specific sonar readingsaccording to the time when dead reckoning and sonar readingsare calculated and sampled. pI is the Cartesian coordinate of

Algorithm 2: Scan Generation

1 [Scur, Xcur,PXcur ] = SG(RH,DRH);2 { [TW

I ,PW I ] = DRH{1};3 Scur = [ ];4 for All sonar readings [ρ,Pρ] ∈ RH do5 [TW

R ,PW R] = FindCorrespondingDR(DRH);6 pI = �TW

I ⊕ TWR ⊕ TR

S ⊕P2C(ρ);7 PI = JI 1⊕JI�PW I JT

I�JTI 1⊕+ JI 2⊕PW JT

I 2⊕;8 Scur = Scur ∪ {[ pI ,PI ]};9 Xcur = TW

I ;10 PXcur = PW I ;11 };

current sonar reading with respect to the reference frame {I }.⊕ and � are the compounding and inversion transformationsproposed in [26]. PW is the covariance of current sonar readingrepresented in global frame {W }, JI 1⊕, JI� and JI 2⊕ are the

Jacobian matrices of pI with respect to TWI and pW. All Xcur

for each scan in S constitute the node part of a pose graph.

V. DATA ASSOCIATION

In the pose graph, constraints between nodes comefrom two scenarios: constraints between consecutive nodesand constraints resulting from loop closures. Constraintsbetween consecutive nodes can be easily obtained by aligningtwo generated consecutive scans using scan matchingalgorithms. The determination of constraints from loopclosures has to start with correct data association which tellswhether the robot is revisiting previously mapped areas.

As proposed in [24], the data association is achieved bychoosing those scans whose corresponding poses fall in theneighbouring range of the pose of current scans as the loopclosing candidates. We refer this strategy as pose-thresholdbased association. This strategy does not take into accountthe uncertainty of poses. So instead of using pose-thresholdbased association, the Mahalanobis distances between posesand shape matching are combined together to give reliableassociations. The Mahalanobis distance with uncertainty canbe calculated as:

DM = (X i − X j )

[Pi + P j

2

]−1

(X i − X j )T (11)

where DM is the Mahalanobis distance between poseX i and X j with covariances Pi and P j respectively.

In addition to the Mahalanobis distance, the shape matchingmethod is also introduced here to determine whether two scansare associated. Originally designed for shape matching inimage processing, Angle Histogram of Vectors in [27] candirectly be used for shape matching between two sonarscans. Two scans, Si and Sj , can be represented bySi = {h2

1, · · · , h2K 1} and Sj = {h2

1, · · · , h2K 2}, where hj is

the angle histogram from each row of the angle matrix ofS1 and S2. The cost Cij for matching two points is defined as:

Cij = C(pi , qj ) = 1

2

N∑

k=1

[hi (k)− hj (k)]2

hi (k)+ hj (k)(12)


Algorithm 3: Data Association

1 O = D A(S,X,PX , Scur ,Xcur ,PXcur ,Mt , St ){2 len_scan = length(S);3 O = [ ];4 for i = 1 to (len_scan) do5 DM = M_dis(Xi ,PXi ,Xcur ,PXcur );6 DS = ShapeMatching(Si ,Scur );7 if DM < Mt and DS < St then8 O = {O ∪ {Si ,Xi }}9 }

where pi and qj are the points on each segments,N is the number of the bins of the corresponding anglehistograms. Then the distance between these two segments iscalculated as:

D(I1, I2)= 1

K1

∑

i∈I1

arg minj∈I2

C(i, j)+ 1

K2

∑

j∈I2

arg mini∈I1

C(i, j)

(13)

where I1 = {i |i = 1 · · · K1} and I2 = { j | j = 1 · · · K2}.This D(I1, I2) is then assigned to DS representing theshape matching distance between scans Si and Sj . Whenscans DM and DS are less than some predefined thresholds(Mt for Mahalanobis distance, St for shape matching), theyare considered as associated.

Algorithm 3 presents the data association strategy. Itsinputs include all the previous scan sets S, robot pose setsfrom the corrected dead reckoning X, their correspondingcovariance PX , the current scan Scur and its correspondingrobot pose Xcur and covariances PXcur . Its returns are a set ofscans and their corresponding robot poses which are associatedwith the current scan Scur , grouped as O. These associatedscans are then used to calculate the constraints resulting fromloop closures which are added to the pose graph G.

VI. SCAN MATCHING AND POSE GRAPH CONSTRUCTION

Scan matching is to determine both the constraints betweenconsecutive scans and the constraints resulting from loopclosures, i.e. to calculate the relative displacement betweentwo range scans. The most popular technique for scanmatching is the ICP algorithm proposed in [28], which wasthen utilized in the mobile robot localization scenario [29].Due to its ease of usability, many subsequent proposed scanmatching algorithms are based on the same basic conceptas ICP, forming a series of ICP-variants. However, all thesealgorithms assume that the range readings are gained from afast laser so that the motion of the robot can be neglected.They cannot be simply used in the system with aslow-scanning sonar to retrieve range scans.

Taking the robot motion and the sonar reading uncertaintyinto account, sonar probabilistic Iterative Correspon-dence (spIC) [30] provides a robust solution to the localizationproblem in the presence of large amounts of noisy readingsand odometric errors. Similar to pIC, spIC is also accom-plished by means of Mahalanobis distance which incorporatesrange uncertainty to determine the correspondence between

Algorithm 4: Scan Matching and Pose Graph Construction

1 [G] = SM PGC(O, Sre f ,Xre f ,Pre f , Scur ,Xcur ,Pcur ){2 G = [ ];3 lrc0 = �Xref ⊕ Xcur;4 Prc0 = J1⊕J�PXref J

T�JT1⊕ + J2⊕PXcur J

T2⊕;

5 [Urc,�rc, success] = spIC(Sre f , Scur , lrc0,Prc0);6 if success then7 Add constraint (Urc,�rc) to G;

8 for All [Si ,Xi ] ∈ O do9 l0 = �Xi ⊕ Xcur;

10 P0 = J1⊕J�PXi JT�JT

1⊕ + J2⊕PXcur JT2⊕;

11 [U,�, success] = spIC(Si , Scur , l0,P0);12 if success then13 Add constraint (U,�) to G;

Fig. 6. Trajectory correction process illustration, with (a) before correctionand (b) after correction. The composition of corrected odometry posescorresponding to Sre f meets with the relative displacement Xre f

cur .

two points from two different scans. Thus, spIC is adoptedhere as the scan matching algorithm.

Algorithm 4 shows the detailed process of bothscan matching and pose graph construction. As can beseen, spIC takes 4 arguments, namely reference scan, currentscan, initial displacement estimation between these two scansand the covariance of the initial estimation. The output ofspIC is the final displacement and its covariance betweenthe two scans that minimizes the error of correspondencesbetween them. As the constraint, the output of spIC isthen added to the pose graph G. There are two sources ofconstraint: one is the displacement between two consecutivescans shown as Sre f and Scur in the algorithm, and the otheris the constraint from displacement between the current scanand those associated with it and determined by the dataassociation module.


Fig. 7. Simulation setup for the AUV.

Fig. 8. The simulated environment with anomalous shape.

Fig. 9. The network of graph constrains generated through the proposed dataassociation. The red plus represents estimated poses by GraphSLAM, greencircle line is ground truth. The associated nodes are linked via gray lines.

VII. TRAJECTORY CORRECTION

By applying the back-end optimization algorithm, thevehicle poses are able to be corrected. These poses are the oneswhere the first sonar reading in each scan is sampled. However,other poses where sonar readings are sampled still need tobe corrected. The trajectory correction module is designedfor this purpose. Take the vehicle poses for two consecutivescans Sre f and Scur for example, the corrected poses of thescans are Xre f and Xcur .

Let X R = {RXre f ,R X1,

R X2 · · · ,R Xn} be all the vehicleposes corresponding to each range readings in Sre f , andthe starting pose for Scur be Xcur . Their relative odometryare defined as X O = {O Xre f

1 ,O X12,

O X23 · · · ,O Xn−1

n ,O Xncur }

from dead reckoning history DRH before correction. Thepurpose of trajectory correction is to refine these relative

Fig. 10. Trajectories for simulation with anomalous shape of environmentby dead reckoning, uspIC, GraphSLAM and ground truth, respectively.

Fig. 11. Distance errors between positions estimated by different algorithmsand the ground truth.

odometry so that the refined poses agree with the relativedisplacement Xre f

cur between corrected poses Xre f and Xcur .In order to evenly distribute the correction to eachelement in X O , a δX is defined to be composited with eachelement in X O to satisfy the following equation:

f (δX) =(

O Xre f1 ⊕ δX

)⊕

(O X1

2 ⊕ δX)

⊕(

OX23 ⊕ δX

)· · ·

(OXn−1

n ⊕ δX)

⊕(

OXncur ⊕ δX

)= Xre f

cur (14)

where OXn−1n = (

O Xn−1n ⊕ δX

)is the corrected relative

odometry of O Xn−1n .

Finding a solution of δX subject to (14) solves the trajectorycorrection problem. As (14) is non-linear equation dueto the angular term, the problem is transformed into anon-constrained nonlinear minimization problem:

arg minδX

g(δX) = arg minδX

( f (δX)− Xre fcur )

T( f (δX)− Xre fcur )

(15)


Fig. 12. Maps rendered by the positions estimated by dead reckoning, uspIC and GraphSLAM, respectively.

The Nelder-Mead method [31] is used to search theoptimal δX. The corrected vehicle poses then constituteCDRH as the output of the trajectory correction module.Fig. 6 illustrates the circumstances of vehicle poses before andafter the trajectory correction. It shows that the compositionof corrected odometry poses corresponding to Sre f meetswith the relative displacement Xre f

cur . It should be noticed thatthe corrected poses will improve the robustness of the dataassociation as more accurate poses facilitate loop closuresbased on Mahalanobis distance.

VIII. EXPERIMENTAL RESULTS

Both simulation and practical experiments are conducted toverify the effectiveness of the proposed algorithm.

A. Simulation

1) Simulation Setup: Simulation based on Robot OperatingSystem (ROS) [32] is conducted to verify the effectiveness ofthe proposed MSS GraphSLAM algorithm. Fig. 7 presentsthe simulation setup of AUV. As shown in Fig. 7, an IMUand a forward looking MSS are the mainly used sensorswhose frames are represented by {I } and {S}, respectively.Frame {R} represents the body frame of the AUV whileframe {W } denotes the global frame which is an ENU systemwith axis x being pointing to East, axis y pointing to Northand axis z pointing to up direction.

When the simulation is running, the simulated environmentwith an anomalous shape shown in Fig. 8 is firstly imported tothe 3D simulator Gazebo [33]. In the simulation experiment,the AUV is controlled to swim around the middle object fortwo rounds. In the simulation, the AUV is tele-operated toswim around the middle object for two rounds with a 166 mtrajectory at an average speed of 0.1 m/s corrupted by aGaussian noise with standard variance of 0.02 m/s.

The MSS sonar obtains 360° scans of the environmentby rotating a sonar beam through 240 angular steps inabout 12 seconds which is set according to the specifics ofa real MSS sonar. During the experiment, IMU readings,sonar readings and the ground truth of the AUV pose arestored for off-line processing and performance analysis. Theproposed algorithm is then deployed to optimally estimatethe positions of the AUV. In literature, there are severalopen-source firmware for the back-end of GraphSLAM such

Fig. 13. The AUV platform: SCPBot.

as iSAM [13], g2o [34], etc. The iSAM is applied as theoptimization solver in this research since it is general, easyto use and provides covariance of estimated poses. Forcomparison, the positions estimated by dead reckoning anduspIC are also provided.

2) Simulation Result: Fig. 9 shows the network ofconstraints generated by the data association algorithm for allthe dead reckoning poses. The grey lines link the associatedposes representing both odometry constraints and loop closureconstraints. It is clear that the poses are correctly associatedand that the proposed data association algorithm is effective.

Fig. 10 shows the trajectories generated by dead reckoning,uspIC, GraphSLAM and the ground truth, respectively. It canbe clearly seen that among all the trajectories, the trajectorycalculated from the GraphSLAM (red dot line) is the closestone to the ground truth (green diamond line). The trajectoryfrom dead reckoning (blue star line) suffers from significantdrift from the ground truth due to its unbounded accumulatederrors. The trajectory generated by uspIC (black circle line) ismore accurate than that from dead reckoning, but less accuratethan that from the GraphSLAM.

Fig. 11 presents the distance errors between positionsestimated by each algorithm and the ground truth. It isobvious that the dead reckoning (blue star line) has the largestdistance errors without boudary, uspIC (black circle line) isin the middle and the GraphSLAM (red dot line) is thebest. The average distance errors for dead reckoning,uspIC and GraphSLAM are 6.09m, 2.53m and 0.67m,respectively.

Fig. 12 shows the maps rendered by using the positionsestimated by dead reckoning, uspIC and Graph SLAMrespectively in comparison with the ground truth. As expected,


Fig. 14. Test site and the underwater vehicle gathering data.

the map built by the GraphSLAM shown in Fig. 12(c) matcheswell with the ground truth without ghosting effect. The mapbuilt by dead reckoning, however, is seriously distorted andhas unbounded accumulated errors. Although the map builtby uspIC outperforms the map built by the dead reckoning,it is still far from the ground truth.

B. Practical Experiment

In order to further examine the proposed algorithm,a practical experiment is conducted by using a real underwatervehicle shown in Fig. 13. The testing site is a 33m × 13mrectangular pond in Woodbridge, UK as shown in the middleof Fig. 14a. An embedded system governing the data collectionfrom IMU and MSS is deployed on the underwatervehicle. IMU is a usual MEMS sensor from Mongooseproviding 3D orientation. MSS used for the experiment isa product named Micron DST from the Tritech InternationalLimited.

The MSS is set as continuous rotation in the clockwisescanning direction, i.e. from 0° to 360°. The range scale ofMSS is configured as 20m and the step angle size is 1.8° whichis deliberately set as the lowest resolution for balancing themeasurement accuracy and computational cost. The numberof range bins that the sonar will generate for each sonarbearing is set as 350, then the range resolution of MSS canbe calculated as 20m/350(bins) = 0.057 m/bin. The wirelessantenna is above the water surface for the purpose of receivingthe operating signal from a joystick so that the vehicle can bedriven remotely.

Similar to the simulation, the vehicle is tele-operated toswim around the pond for 2 rounds with the starting positionand stopping position being the same spot. The data fromIMU and MSS are logged on the embedded flash drives forpost processing and analysis. Fig.14b shows the vehicle inthe pond when gathering sensor data. The collected MSS datais firstly pre-processed to get rid of abnormal noises causedby reflection of walls [9] and is then combined withIMU data to be fed into the proposed GraphSLAM algorithmin Fig. 4.

Fig. 15 shows a complete sonar scan before and after scangeneration, as well as after trajectory correction, compared tothe boundary of the test site. It can be seen that the scan beforescan generation (red circle) is seriously distorted by the vehicle

Fig. 15. A complete scan before (red circle), after (blue dot) scan generationand after trajectory correction (green diamond), compared to the boundary ofthe test site (black line).

motion during the sonar scanning process, while the scan afterscan generation is greatly corrected by taking into account thevehicle motion estimated by EKF-based dead reckoning. Afterthe trajectory correction process, the scan is further correctedand free from any motion distortion which matches best withthe boundary of the test site (black line). This result validatesthe effectiveness of the scan generation and the trajectorycorrection module.

Fig. 16 shows three ortophotomaps that contain thetrajectories and the corresponding maps estimated by the deadreckoning, uspIC and GraphSLAM algorithm respectively.As DGPS was far away from the base station and notworking properly at the testing site, the ground truth cannot be obtained for the performance evaluation. However,the pose difference between the starting position and theestimated end position is able to indicate the localizationaccuracy of each algorithm. It is clear that the pose differencewith the dead reckoning is the largest one, i.e. 7.77m, andthe proposed GraphSLAM has the smallest pose differenceat 1.08m. The pose difference with uspIC is 4.37m. Thismatches to the result from simulation experiments, i.e., theproposed GraphSLAM provides the most accurate poseestimation.


Fig. 16. Trajectories generated by dead reckoning, uspIC and GraphSLAM respectively and the corresponding maps.

As shown in Fig. 16, that the map produced byGraphSLAM aligns best with the actual rectangle pond andhas the smallest offset compared with other two maps. Themap generated with dead reckoning exhibits serious ghostshadows and suffers from most considerable drift from theactual boundary of the rectangle pond. Although the maprendered by uspIC is much better than the dead reckoningmap, it still shows certain amount of drift.

IX. CONCLUSION AND FUTURE WORK

This paper presents the framework of a graph optimizationbased SLAM algorithm for an AUV equipped with aslow-sampling MSS while moving around an underwaterenvironment. The algorithm includes constructing a graph,solving the constructed graph by a graph optimizationalgorithm and correcting the robot poses during thesonar scanning process by using the optimized poses. Morespecifically, the EKF-based dead reckoning algorithm is firstlydeployed for scan generation, which takes the AUV motioninto account, thereby eliminating the sonar image distortioncaused by the robot motion. Then the data associationalgorithm based on Mahalanobis distance and shape matchingis deployed to determine loop closures, leading to associatedscan pairs used for calculating constraints of the pose graph.The constructed graph is solved by a graph optimizationalgorithm iSAM, resulting in optimized poses correspondingto each generated scan. A trajectory correction module isused to correct those middle poses during the scan generationprocess which further improves the localization accuracy.

The algorithm is verified with both the ROS basedsimulation and the home-made AUV platform. Experimentalresults clearly show that the proposed algorithm outperformsother two traditional algorithms such as dead reckoning anduspIC in terms of both localization and mapping accuracy.However, the current implementation of the algorithm isoff-line post processing due to the limited computingresources of the embedded system. A real-time version ofimplementation is more practically effective. In addition,for convenience of program debugging and data collection,the home-made platform SCPBot is simply designed andfloats on the surface of the water when collecting sonar data.However, the algorithm proposed in this paper are originallydesigned for AUV to localize itself in an underwaterenvironment. A real AUV that can go deeper and travel

longer distance is required for future experiments.Therefore, our future work will focus on real-timeimplementation of the proposed algorithm on a morecomputationally powerful real AUV.

ACKNOWLEDGMENT

The authors thank R, Dowling and I, Dukes for theirtechnical support during the experiments.

REFERENCES

[1] M. Shaukat, M. Chitre, and S. H. Ong, “A bio-inspired distributedapproach for searching underwater acoustic source using a team ofAUVs,” in Proc. MTS/IEEE OCEANS, Bergen, Norway, Jun. 2013,pp. 1–10.

[2] D. Sutantyo, P. Levi, C. Moslinger, and M. Read, “Collective-adaptiveLévy flight for underwater multi-robot exploration,” in Proc. IEEE Int.Conf. Mechatronics Autom., Aug. 2013, pp. 456–462.

[3] A.-R. Diercks et al., “Site reconnaissance surveys for oil spill researchusing deep-sea AUVs,” in Proc. IEEE Int. Conf. Oceans, Sep. 2013,pp. 1–5.

[4] E. Seedhouse, “Deep-sea mining and energy exploitation,” in OceanOutpost. Berlin, Germany: Springer-Verlag, 2011, pp. 127–140.

[5] R. A. Newcombe, S. J. Lovegrove, and A. J. Davison, “DTAM: Densetracking and mapping in real-time,” in Proc. IEEE Int. Conf. Comput.Vis., Nov. 2011, pp. 2320–2327.

[6] F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard, “3-Dmapping with an RGB-D camera,” IEEE Trans. Robot., vol. 30, no. 1,pp. 177–187, Feb. 2014.

[7] M. Blösch, S. Weiss, D. Scaramuzza, and R. Siegwart, “Vision basedMAV navigation in unknown and unstructured environments,” in Proc.IEEE Int. Conf. Robot. Autom., May 2010, pp. 21–28.

[8] M. W. Achtelik, S. Lynen, S. Weiss, L. Kneip, M. Chli, and R. Siegwart,“Visual-inertial SLAM for a small helicopter in large outdoor environ-ments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2012,pp. 2651–2652.

[9] D. Ribas, P. Ridao, J. D. Tardós, and J. Neira, “Underwater SLAM inman-made structured environments,” J. Field Robot., vol. 25, nos. 11–12,pp. 898–921, Nov./Dec. 2008.

[10] S. Wang, L. Chen, H. Hu, Z. Xue, and W. Pan, “Underwater localiza-tion and environment mapping using wireless robots,” Wireless Pers.Commun., vol. 70, no. 3, pp. 1147–1170, Jun. 2013.

[11] A. Mallios, P. Ridao, E. Hernández, D. Ribas, F. Maurelli, and Y. Petillot,“Pose-based SLAM with probabilistic scan matching algorithm usinga mechanical scanned imaging sonar,” in Proc. OCEANS EUROPE,May 2009, pp. 1–6.

[12] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, vol. 1.Cambridge, MA, USA: MIT Press, 2005.

[13] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smooth-ing and mapping,” IEEE Trans. Robot., vol. 24, no. 6, pp. 1365–1378,Dec. 2008.

[14] A. Burguera, Y. González, and G. Oliver, “The UspIC: Performing scanmatching localization using an imaging sonar,” Sensors, vol. 12, no. 6,pp. 7855–7885, 2012.


[15] D. Kim, D. Lee, H. Myung, and H.-T. Choi, “Artificial landmark-basedunderwater localization for AUVs using weighted template matching,”Intell. Service Robot., vol. 7, no. 3, pp. 175–184, Jul. 2014.

[16] M. Bayat and A. P. Aguiar, “Underwater localization and mapping:Observability analysis and experimental results,” Ind. Robot, Int. J.,vol. 41, no. 2, pp. 213–224, 2014.

[17] J. Zhang, W. Wang, G. Xie, and H. Shi, “Camera-IMU-based under-water localization,” in Proc. 33rd Chin. Control Conf., Jul. 2014,pp. 8589–8594.

[18] S. E. Webster, R. M. Eustice, H. Singh, and L. L. Whitcomb, “Advancesin single-beacon one-way-travel-time acoustic navigation for underwatervehicles,” Int. J. Robot. Res., vol. 31, no. 8, pp. 935–949, 2012.

[19] M. F. Fallon, G. Papadopoulos, J. J. Leonard, and N. M. Patrikalakis,“Cooperative AUV navigation using a single maneuvering surface craft,”Int. J. Robot. Res., vol. 29, no. 12, pp. 1461–1474, 2010.

[20] I. Tena Ruiz, S. de Raucourt, Y. Petillot, and D. M. Lane, “Concurrentmapping and localization using sidescan sonar,” IEEE J. Ocean. Eng.,vol. 29, no. 2, pp. 442–456, Apr. 2004.

[21] J. Aulinas, X. Lladó, J. Salvi, and Y. R. Petillot, “Feature based slamusing side-scan salient objects,” in Proc. OCEANS, Sep. 2010, pp. 1–8.

[22] C. Roman and H. Singh, “Improved vehicle based multibeam bathymetryusing sub-maps and SLAM,” in Proc. IEEE/RSJ Int. Conf. Intell. RobotsSyst., Aug. 2005, pp. 3662–3669.

[23] S. Barkby, S. Williams, O. Pizarro, and M. Jakuba, “An efficientapproach to bathymetric SLAM,” in Proc. IEEE/RSJ Int. Conf. Intell.Robots Syst., Oct. 2009, pp. 219–224.

[24] A. Mallios, P. Ridao, D. Ribas, F. Maurelli, and Y. Petillot, “EKF-SLAMfor AUV navigation under probabilistic sonar scan-matching,” in Proc.IEEE/RSJ Int. Conf. Intell. Robots Syst., Oct. 2010, pp. 4404–4411.

[25] G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard, “A tutorialon graph-based SLAM,” IEEE Intell. Transp. Syst. Mag., vol. 2, no. 4,pp. 31–43, Jan. 2010.

[26] R. Smith, M. Self, and P. Cheeseman, “Estimating uncertain spatialrelationships in robotics,” in Autonomous Robot Vehicles, New York,NY, USA: Springer, 1990, pp. 167–193.

[27] C. Zhang and V. Prinet, “Shape matching using the included anglehistogram of vectors,” in Proc. Chin. Conf. Pattern Recognit., Oct. 2010,pp. 1–5.

[28] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,”IEEE Trans. Pattern Anal. Mach. Intell., vol. 14, no. 2, pp. 239–256,Feb. 1992.

[29] F. Lu and E. Milios, “Robot pose estimation in unknown environmentsby matching 2D range scans,” J. Intell. Robot. Syst., vol. 18, no. 3,pp. 249–275, Mar. 1997.

[30] A. Burguera, Y. Gonzalez, and G. Oliver, “Probabilistic sonar scanmatching for robust localization,” in Proc. IEEE Int. Conf. Robot.Autom., Apr. 2007, pp. 3154–3160.

[31] Nelder-Mead Algorithm. [Online]. Available: http://www.scholarpedia.org/article/Nelder-Mead_algorithm, accessed Jun. 18, 2014.

[32] Robot Operating System. [Online]. Available: http://www.ros.org/wiki/,accessed May 28, 2014.

[33] 3D Simulator-Gazebo. [Online]. Available: http://www.ros.org/wiki/gazebo, accessed Mar. 28, 2014.

[34] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard,“G2o: A general framework for graph optimization,” in Proc. IEEE Int.Conf. Robot. Autom., May 2011, pp. 3607–3613.

Ling Chen received the M.Eng. degree in control science and engineer-ing from Central South University, Changsha, China, in 2010, and thePh.D. degree in computing and electronics engineering from the Universityof Essex, U.K., in 2014. He is currently a Lecturer with the School ofMechatronics Engineering and Automation, Shanghai University, Shanghai,China. His research interests include underwater Simultaneous Localizationand Mapping.

Sen Wang received the B.E. degree in automation from the GuangdongUniversity of Technology, China, in 2009, and the M.Eng. degree in controlscience and engineering from the Harbin Institute of Technology, China,in 2011. He is currently pursuing the Ph.D. degree in computer science atthe University of Essex, U.K. His research interests include wireless sensornetwork localization, simultaneous localization and mapping, and cooperativelocalization of multirobot systems.

Huosheng Hu (SM’15) received the M.Sc. degree in industrial automationfrom Central South University, China, in 1982, and the Ph.D. degree in robot-ics from the University of Oxford, U.K., in 1993. He is currently a Professorwith the School of Computer Science and Electronic Engineering, Universityof Essex, U.K., leading robotics research. He has authored over 380 researcharticles in journals, books, and conference proceedings. His research interestsinclude autonomous robots, human-robot interaction, multirobot collaboration,pervasive computing, and networked robots. He is a Fellow of the Institution ofEngineering and Technology, and The Institute of Measurement and Control,U.K., and a Senior Member of the Association for Computing Machinery,and a Chartered Engineer. He is also an Editor-in-Chief of the InternationalJournal of Automation and Computing, the Founding Editor-in-Chief for theRobotics Journal, and an Executive Editor of the International Journal ofMechatronics and Automation.

Dongbing Gu received the B.Sc. and M.Sc. degrees in control engineeringfrom the Beijing Institute of Technology, Beijing, China, and the Ph.D. degreein robotics from the University of Essex, Essex, U.K. He was an AcademicVisiting Scholar with the Department of Engineering Science, University ofOxford, Oxford, U.K., from 1996 to 1997. In 2000, he joined the Universityof Essex, as a Lecturer, where he is currently a Professor with the School ofComputer Science and Electronic Engineering. His current research interestsinclude multiagent systems, wireless sensor networks, distributed controlalgorithms, distributed information fusion, cooperative control, reinforcementlearning, fuzzy logic and neural network-based motion control, and modelpredictive control.

Liqing Liao received the M.Sc. degree in industrial automation and thePh.D. degree in control theory and control engineering from Central SouthUniversity, China, in 1990 and 2010, respectively. He is currently a Professorwith the School of Information Science and Engineering. He has authoredover 60 research articles in journals, books, and conference proceedings.His research interests include real-time simulation, robotics, and modelingand controlling in complex electromechanical system.

Documents

5024 IEEE SENSORS JOURNAL, VOL. 15, NO. 9, SEPTEMBER …