Improved Vehicle Based Multibeam Bathymetry using Sub-maps and ...

Improved Vehicle Based Multibeam Bathymetry using Sub-maps and SLAM Chris Roman

Hanumant Singh

MIT/WHOI Joint Program in Applied Ocean Science and Engineering Massachusetts Institute of Technology Cambridge, MA, USA [email protected]

Department of Applied Ocean Physics and Engineering Woods Hole Oceanographic Institution Woods Hole, MA, USA [email protected]

ground referenced position estimates with bounded error. Unfortunately, the actual error statistics are highly coupled to the environment and are difficult to characterize [3]. LBL systems also require the deployment of additional infrastructure that makes quickly surveying an area difficult. Subsea vehicles also employ dead reckoning (DR) navigation using acoustic measurements of the vehicle’s ground relative velocity and attitude, obtained from an inertial navigation system (INS). This method of navigation is subject to unbounded position error growth over long time scales. However, this error growth can be slowed by using high quality INS sensors to produce accurate short term navigation [4]. The primary navigation sensor for DR navigation underwater is the acoustic Doppler velocity log (DVL). In recent years the simultaneous mapping and localization community has focused on the coupled problem of mapping an area with a robot while concurrently deriving improved position estimates from the map. Many of the proposed solutions to this problem are applicable to improving subsea mapping. These solutions differ according to the types of environmental measurements they utilize and the manner in which they fuse the measurements with available navigation data. Feature based approaches identify and track specific features in the environment. This requires a data association step to match measurements to features and is typically used where discrete features are present in the environment, such as in indoor and man made environments. Feature based techniques using natural features have been applied in limited number to underwater sonar problems [5]. This is primarily due to the difficulty in identifying features that appear invariant to viewpoint. Featureless approaches do not attempt the extraction of specific features from the mapping sensor measurements and instead use the raw sensor measurements directly. This is commonly done with sensors that map a large section of the environment at once. For bathymetric mapping we pursue a featureless approach because of the unstructured nature of the environment and our desire to eventually produce a surface terrain map. We accomplish this by combining multibeam pings taken over short time windows to form small sub-maps

Abstract— This paper presents an algorithm to improve subsea acoustic multibeam bottom mapping based on the simultaneous mapping and localization (SLAM) methodology. Multibeam bathymetry from underwater water vehicles can yield valuable large scale terrain maps of the sea floor, but the overall accuracy of these maps is typically limited by the accuracy of the vehicle position estimates. The solution presented here uses small bathymetric patches created over short time scales in a sub-mapping context. These patches are registered with respect to one another and assembled in a single coordinate frame to produce a more accurate terrain estimate and provide improved renavigation of the vehicle trajectory. The mapping is implemented using a delayed state extended Kalman filter (EKF) and results are shown for a real world multibeam data set collected at the Mid-Atlantic ridge using the JASON ROV. Index Terms— bathymetry, acoustic mapping, SLAM, multibeam

I. I NTRODUCTION Bathymetric terrain maps generated from acoustic range data provide useful information to support marine geology, biology and subsea archeology. Range data is often collected using high frequency multibeam and scanning sonars mounted on robotic vehicles that perform dedicated survey operations. Maps are typically constructed by projecting the range data from a processed vehicle position & attitude track derived from data supplied by on board and possibility external navigation instruments. The achievable resolution of the resulting maps is more often limited by the accuracy of the vehicle navigation than the accuracy of the acoustic mapping sensor [1]. This limitation is a result of a missing feedback path to enforce consistency between the vehicle navigation data and the mapping data. When this consistency is not enforced maps will often contain overlapping sections of data that are incorrectly registered and biased. Underwater navigation for map making is difficult. Satellite based GPS, which can be used for accurate position estimation on land, does not work subsea due to the rapid attenuation of electromagnetic radiation in water. The closest analog underwater is Long Base Line (LBL) navigation [2] which uses external acoustic beacons and time of flight measurements to triangulate the vehicle position in two and three dimensions. LBL systems have the desirable property that they produce 1

that can then be registered and assembled into a final map. Proposed SLAM frameworks to integrate the mapping and navigation data include the extended Kalman filter (EKF), particle filters, sparse information filters, junction tree filters and constraint networks. Each of these approaches have advantages and disadvantages in the context of bathymetry mapping. A delayed state version of the recursive EKF solution provides a iterative formulation for mapping and retaining knowledge of prior platform positions [6]. This solution is subject to severe scale limits if additional methods are not used to reduce the O(n2 ) computational burden per state update [7], [8]. The EKF solutions are also subject to linearization errors as the constraints between delayed states are linearized only when they are incorporated into the recursion. If the trajectory of delayed states is deformed significantly, such as with a large loop closure, the constraints may no longer be accurate and bias the solution. However, this iterative solution has a possible real time implementation that could produce adjusted navigation information useful to the surveyor trying to ensure complete coverage of a survey area. Recently, the attractive sparseness properties of the information matrix have been utilized in methods for reducing the computation of similar linear Gaussian iterative methods [9], [10]. In the context of underwater photographic mapping Eustice [11], [12] has addressed the scale problem for delayed state filters. Alternatively, if the navigation problem is treated as a network of possibly nonlinear constraints linking sub-map positions (previous vehicle poses) to one another, several other solutions exist. In a scan-matched representation of the environment that assumes independent Gaussian measurement errors between scan poses a maximum likelihood (ML) solution for the pose locations can be formulated as a linear problem involving a large constraint matrix [13]. Extensions of this methodology have been used for very large maps [14]. Frese [15] presents a constraint based multi-grid solution designed as an incremental mapping approach to achieve O(n) update computation and retain the ability to relinearize relative pose constraints during the solution process. Bosse’s ATLAS framework [16] keeps all the constraint information in a relative framework and uses a non-linear least squares solution to resolve the resulting constraint network. To avoid potential problems with overconfidence associated with unknown cross correlations Schlegel [17] advocates a solution based on Covariance Intersection (CI). All of these constraint based approaches require an accurate initial guess for the solution and a correct network topology of links. A survey of these SLAM techniques suggests there are many avenues toward a solution which will help enforce consistency between vehicle navigation and mapped terrain. In choosing a framework for our application we make the following observations. An important distinction between this application and many land based applications is that in our case the surveyor can design the vehicle trajectory to avoid the need for closing large loops. This is often not possible in land based applications where the vehicle trajectory is constrained by the environment, such as in a building. As a result we

consider this application to be less prone to linearization and link proposal issues associated with closing large loops. We also consider it our goal to accurately map a specific area of interest on the seafloor rather than cover expansive amounts of terrain. As such, we pursue an EKF delayed state solution as a proof of concept for bathymetric sub-mapping knowing that there are impending scale limitations. In this paper we outline a procedure for EKF based bathymetric map creation using subsea vehicles similar to the JASON ROV described in Section II. Section III describes the application of a delayed state filter to bathymetric submapping. Section IV shows how pairwise registration can be done between bathymetric patches using correlation and an iterative closest point (ICP) algorithm. We have found that this SLAM algorithm is capable of producing terrain maps of higher quality than those created using more standard renavigation & mapping approaches. Section V shows results from a real world data set. II. V EHICLE PLATFORM AND EXAMPLE SURVEY The vehicle platform used for this work was the JASON remotely operated vehicle (ROV) which is part of the US nation deep submergence facility. The ROV contains on board navigation sensors for three axis attitude, three axis bottom relative velocity and surface relative depth. Acoustic LBL navigation fixes from external beacons are also obtained for ground referenced position estimates. Table I shows the specific characteristics of these sensors and measurements. TABLE I

NAVIGATION SENSORS Measurement

Sensor

Precision

Heading Pitch/Roll Depth (surface relative) Vehicle velocity (bottom relative) Position (x,y,z)

FOG1 Tilt sensors Pressure sensor Acoustic Doppler Long Base Line

±0.1◦ ±0.1◦ ±0.01m ±0.01m/s O(1m)

1

The Fiber Optic Gyro (FOG) also has the desirable property of zero heading dependent deviation.

The multibeam sonar used was the SM2000 (KongsbergMesotech Ltd) mounted in a down looking configuration. This 200kHz sonar contains a transmit transducer with a nominal 3◦ fore-aft beam width and a 120◦ athwart ships beam width, and an 80 element receiving array. The returned signals are beamformed to 128 beams across the 120◦ beam width. For this particular survey the sonar returns were sampled to produce range bins of O(5cm). For this paper we will use data taken from a 12 hour survey over a hydrothermal mount in 3000 meters of water. The vehicle tracklines shown in Fig. 1 were flown at a nominal forward speed of 25cm/s and the vehicle altitude was controlled in closed loop to an approximately constant 15m. Fig. 1 shows both an integrated DR navigation trajectory and LBL navigation position fixes. Note the disparity between the DR path and the ground referenced LBL that makes obtaining an accurate bathymetric map by simply projecting 2

origins. The state vector in (1) shows the vehicle state and N delayed sub-map origins.

the sonar ranges from the entire DR track impossible. This survey pattern was specifically designed to develop our SLAM algorithm by avoiding loop large closures, creating overlap of the acoustically imaged swath with adjacent parallel tracklines and revisiting some areas multiple times with crossing tracklines.

xsi = [x, y, z, θ, φ, ψ]>

(2)

This state vector will grow in length as new sub-maps are created and delayed states are added to the filter. The notation for the delayed states indicate that the delayed state, xsi , marked by subscript s points from the common origin to the origin of sub-map i. The delayed state poses consist of 6 DOF frames defined by:

Dead reckoning vehicle trajectory and LBL Estimated position LBL nav fixes

3300

3250

Y [m]

(1)

delayed states

3400

3350

> > > xaug = [x> v , x s1 , · · · , x sN ] | {z }

3200

where, θ, φ, ψ are Euler angles. The covariance matrix for the filter describes the covariance of the vehicle, Pxv xv , the covariance of the sub-map origins, Pxsi xsi , and all of the respective cross correlations, Pxv xsi and Pxsi xsj .

3150

3100

3050

3000 3100

3150

3200

3250

3300

3350

3400

3450

3500

3550

X [m]



Fig. 1. Doppler integrated DR tracklines and LBL position fixes for our example survey. The DR trajectory integration was started at an LBL position fix and by the end of the trajectory a ∼ 20m difference has grown. Also, note the region with no LBL coverage most likely do to the terrain blocking the direct acoustic path.

  Paug =  

Px v x v P x s1 x v .. . P x sN x v

P x v x s1 P x s1 x s1 .. . P x sN x s1

··· ··· .. . ···

P x v x sN P x s1 x sN .. . P x sN x sN

    

(3)

As the new sub-maps are created additional rows and columns are added to the covariance matrix. These new elements are non-zero because the current state is correlated with all prior states. Intuitively, setting the covariance of a newly added delayed state, PxsN +1 xsN +1 , equal to the current vehicle covariance, Pxv xv , allows that sub-map origin to inherit the correct uncertainty of its position estimate.

III. EKF FRAMEWORK A continuous-discrete formulation of a delayed state EKF is used to integrate the vehicle navigation sensors to provide an estimate of the vehicle pose and retain estimates of the previously visited vehicle positions [6], [18]. In our application the delayed states serve as anchor points for small bathymetric sub-maps that are created as the filter progresses. The diagram in Fig. 2 shows the data paths for the filter. The creation of bathymetry sub-maps requires knowledge of the vehicle state and beamformed sonar data. Newly created sub-maps are stored and overlapping sub-maps are registered to generate relative pose measurements between delayed states.

A. Vehicle model To describe the pose of the vehicle we use a six DOF parameterization with position and attitude variables measured in a local level reference frame. The additional states included in the vehicle state vector are the vehicle body frame velocities and angular rates (4)

˙ φ, ˙ ψ˙ ]> . xv = [x, y, z, θ, φ, ψ , u, v, w, θ, {z } | {z } | position

velocity

For a vehicle dynamic model we consider a constant velocity model f (xv (t)) perturbed by white noise, w, with zero mean and diagonal covariance Q x˙ v (t)

Fig. 2. This block diagram indicates the flow of the sub-mapping and registration algorithm.

=

=

The complete filter state vector, xaug , is partitioned into a vehicle state, xv , which describes the current estimate of the vehicle pose, and a delayed portion containing the sub-map 3

f (xv (t)) + w(t)    lv R(φ, θ, ψ)         J(φ, θ, ψ)    O[6×1]

(5)  

u v  w p q  r



        +         

0 .. . .. . .. . 0 w[6×1]



      , (6)    

where w[6×1] = [w1 , w2 , w3 , w4 , w5 , w6 ]> . The constant velocity assumption seems adequate for this application where the vehicle dynamics are relatively slow. This vehicle model relates the vehicle body frame velocities to local level frame velocities through the non-linear rotation l v R(φ, θ, ψ). The matrix J(φ, θ, ψ) maps the body frame angular rates to the local level frame angular rates and the white noise w adds to the linear and angular accelerations. When the entire augmented state is considered, thehdelayed states are i not affected by the vehicle model, x˙ aug = x˙ v (t)> , 0> [6N ×1]

>

These equations and the update equations (13) require the Jacobians of the process model and the navigation sensor measurements with respect to the vehicle state ∂hn (x[tk ]) ∂f (x(t)) , Hn = . (12) Fv = ∂xv (t) xv (t) ∂xv [tk ] xv [tk ]

The prediction equations are nonlinear and are integrated numerically using a Runge-Kutta approximation. The integration ¯− produces the prediction of the mean vehicle state x v and − covariance Paug using all the measurements prior to time tk . The update equations are then written in a discrete form  − > −1 > W = P− HPaug H + R (13a) aug H

.

B. Measurements Navigation sensor measurements, z[tk ], are available at discrete times represented by tk and handled using non-linear measurement models of the form hn (xv [tk ]). These measurement models are implemented as mixed-coordinate functions that predict the sensor measurement in the individual sensor coordinate systems. The sensor measurements are assumed to be corrupted by a time independent zero mean Gaussian noise v with covariance R, where E[wv> ] = 0. z[tk ] = hn (xv [tk ]) + v

− WH] + WRW . (13c)   Here, z = z[tk ], h = hn ( · ) and H = Hn , 0[m×6N ] when the update is for a navigation sensor updating m states. When the update is for a delayed state measurement z = zsij , h = hsij ( · ) and H = Hsij . The matrix R contains the appropriate measurement covariance for the navigation sensor or relative pose measurement. Paug [tk ] = [I −

(7)

Hsij

= hs (xsi , xsj ) = xsi ⊕ xsj ∂xsij ∂xsij , 0, , 0]. = [0, ∂xsi ∂xsj

WH]P− aug [I

>

>

IV. S UB - MAP CREATION AND MATCHING

For this application we incorporate navigation measurements of the body frame velocities, surface relative depth and, three axis attitude. We use the LBL position estimates to evaluate the output of the SLAM algorithm, but do not incorporate them or any other ground reference position estimates in the filter. Thus, the vehicle model within this framework is dead reckoning. To make measurements between delayed states we use the tail-to-tail operation as defined by Smith [19]. This nonlinear operation predicts the relative 6 DOF transform, xsij , between the filter states, xsi and xsj , ˆsij z

(13b)

¯ aug [tk ] = x ¯− x aug + W[z − h]

A. Sonar processing Prior to mapping the raw sonar data collected for each ping is beamformed and range detected. Range detection is accomplished by looking at each beam and choosing the range at which the peak amplitude was returned. To reduce possible errors associated with weak returns, beams not striking the bottom and other false detections some ranges are thrown out using amplitude thresholding and median filtering with neighboring beams. A sample ping is shown in Fig. 3.

(8) (9) (10)

The sparse Jacobian, Hsij , of the relative pose estimate is used in the update equations when the measurement is incorporated into the filter. C. Update and prediction

Fig. 3. A single ping from the multibeam sonar can be beamformed and plotted as an intensity image in 2D Cartesian space. The shading indicates received intensity. This ping is oriented as if the vehicle is flying into the page with a steep terrain rise to port.

The EKF uses the continuous time prediction equations (11) to move the state estimate forward in time until the next navigation or delayed state measurement can be made,

Further filtering of the detected beam ranges can be done by median filtering over pings which are adjacent in time. This x ¯˙ v (t) = f (xv (t)) (11a)    >   is beneficial in removing potential range errors caused by an F (t) 0 F (t) 0 Q 0 entire ping that has been corrupted. These filters can be tuned v v P˙ aug (t) = Paug (t) + Paug (t) + . 0 0 0 0 0 0 aggressively if coverage redundancy was built into the survey (11b) path. 4

As a measure of uncertainty we use the determinant of the upper left 3 × 3 partition of Pxsi vp xsi vp as a volumetric error measurement [16].

B. Map creation & matching Sub-maps are created using the range data and the vehicle position estimates extracted from, xv (t)− , at the time each ping is taken. The beam ranges are projected and kept as points in 3D dot clouds, Mi , using the delayed states i = 1 · · · N as local origins. Using the notation from Smith [19] the locations of an individual point in space can be written as mi = ( xsi ⊕ xvp (t)− ) ⊕ xvp

C. Relative pose measurements After a sub-map is closed, links are proposed for possible relative pose measurements to the other previously generated sub-maps. Since all the states are represented in a common coordinate frame this can be accomplished in a straight forward manner by looking for a minimal size intersection of the sub-map borders anchored to each delayed state. At worst case this is a cost of O(N ), where N is the number of sub-maps. For each proposed link a pairwise registration is attempted to produce a 6DOF relative pose measurement between the delayed states. The registration attempts to measure the ∆ vector shown in Fig. 4. As an example, the common area of two overlapping sub-maps are shown in Fig. 5.

(14)

where, xvp is the location of the sonar range point in the vehicle coordinate frame and xvp (t)− is the vehicle pose extracted from xv (t)− . Sub-maps are created by collecting pings in this manner over short time scales during which the vehicle position error associated with the DR vehicle process model grows. Determining the size at which to break a terrain map and begin another is a balance between • creating a map small enough that it itself does not contain a significant amount of internal error or distortion, and • creating a map large enough to contain enough 3D information that it can be registered to another map. Given these criteria there are a few obvious limitations in applying this technique. First, the DR navigation must be reasonable enough that sub-maps can be made at all. Second, PSfrag replacements the seafloor can not be flat and featureless. Fortunately, there is little interest in mapping such areas. In an effort to algorithmically break and initiate sub-maps we monitor the characteristics of the sub-maps as they are created. To estimate the amount to 3D information in a sub-map we monitor the Eigen structure of the principal Fig. 4. Vector component matrix N  1 X ¯ ¯ > , (mi − m)(m i − m) N i=1

zs12

2’

∆

2 1

ˆs12 z

x s1 x s2 O

diagram showing a sub-map registration measurement. Two overlapping sub-maps are shown with their locations in the local level frame ˆs12 indicates the relative pose between indicated by xs1 and xs2 . The vector z the origins as predicted by the EKF. The addition correction vector, ∆, is determined by the map registration and used to create the actual terrain relative measurement zs12 .

(15)

where mi = [x, y, z]> is one of N points in the dot-cloud Mi . A minimum map size threshold is defined by the condition number of this matrix. If this matrix is poorly conditioned the sub-map is dominantly planar or two small to contain sufficient 1 1 3D information. PSfrag replacements The maximum map size is limited by comparing a measure 1 of vehicle’s position covariance relative to PSfrag the current subreplacements 12 map origin to a threshold. This threshold is set proportional to 12 12 the approximate accuracy of the sonar range detection and an estimate of the potential bias between the heading sensor and Fig. 5. Sub-maps can be projected into a common coordinate frame prior to the velocity sensor. When the threshold is exceeded a new map registration. Here map 12 is projected into frame 1. The common bordered is started, independent of the minimum map size condition. region will be used for registration and the [x, y, z] points are color coded by Intuitively this test is breaking the sub-map before the DR height in z direction. The number of displayed points has been down sampled navigation error begins dominating the mapping error. The from the actual number by 5. navigation uncertainty for this comparison can be determined Sub-map registration is accomplished using a two step on line using the Jacobian Hs associated with the relative process. For coarse alignment overlapping sub-map dot clouds pose operation between the map origin and the current vehicle are transformed to a common coordinate frame using the pose, xsi vp = hs (xsi , xvp ), and the current augmented state prediction determined by hs ( · ), gridded and matched using a covariance matrix Paug 2D correlation method. Experimentally, correlation has proven (16) to be a robust method of providing an initial guess for a final Pxsi vp xsi vp = Hs Paug H> s . 40

40

20

20

30

30

10

10

0

0

20

−30

0

−40

10

−20 −30

0

−40

−10

−50

−10

−50

−60

−20

−70

−60

−20

−70

−30

−20

0

20

X [m]

5

40

60

80

−30

−20

0

20

X [m]

40

60

80

Depth [m]

−20

Y [m]

Y [m]

20

−10

10

Depth [m]

−10

Comparison of surface error between registration steps

registration based on the ICP algorithm. The 2D transform ∆corr = [δx, δy] is determined from

300

EKF predicted 250

N 1 X ∆corr = arg min (Z1 − Z2 (∆)) (17) ∆ N where, N is the number of common grid nodes between the gridded height surface for the base map, Z1 , and the gridded height surface for the matching sub-map, Z2 , shifted by ∆ = [δx, δy]> . The gridded surfaces can be created using any gridding approach for a non-uniformly sampled surfaces. We have used a Gaussian gridding technique that computes grid values based on a distance dependent weighting of the dot cloud points. The fine sub-map registration with an ICP method is then performed using the sub-map dot clouds after they are transformed by the initial ∆corr . We have implemented and are evaluating the typical point-to-point and point-to-plane methods for this alignment [20], [21]. Initial results suggest that the point-to-plane method converges more consistently than the point-to-point method. An example convergence pattern for the translation motion using randomized starting locations is shown in Fig. 7. To reduce some computational costs the sub-map dot clouds are randomly down sampled. The randomization helps limit the effect of the sampling pattern on the registration. If sampling effects such as striping, low point density and occlusion are present the registration can be easily be biased to align artifacts in the sampling rather than the underlying terrain. The linear footprint pattern of the sonar is noticeable in the sub-maps in Fig. 5. To evaluate the result of the registration we combine both sub-map dot clouds into a single dot cloud and use a local measurement of the surface variance. Without ground truth for the unstructured surface it is difficult to evaluate the absolute accuracy of the registration. By locally fitting planes to small sections of the surface which have been binned in x and y and calculating the standard deviation of the orthogonal projection errors to this plane we get an estimate of the local error. Ideally, the merged dot cloud would represent the sampled surface and have zero thickness. Although this measure of surface variance removes the overall effect of terrain, it is still lower bounded by the sonar range accuracy and terrain shape within the individual bins. This measure is also upper bounded by how varied the neighboring terrain is. Over rugged terrain mis-registration adds directly to the surface variance and over flat terrain mis-registration will not contribute to the surface variance. We’ve found that coarse correlation followed by ICP registration will reduce the surface variance indicating that the surfaces being aligned correctly, see Fig. 6. As an initial effort to associate a covariance statistic with the relative pose measurements we have run registration trials over many sub-maps and examined the convergence behavior, Fig. 7 shows single pair example. Several registrations were performed starting at initial guesses scattered randomly around the nominal solution. Although all of the iterations converged to a single region, the convergence pattern is a function of

Correlation ICP Sub−map 1

Frequency

200

Sub−map 2

150

100

50

0

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Surface error standard deviation, [m]

Fig. 6. Histograms of the surface standard deviation over all bins on the registered dot clouds. The surface errors are incrementally reduced starting with the EKF predicted relative pose, the correlation and ICP alignment. This plots also shows the histograms for the individual dot clouds. Ideal registration would push the combined dot cloud error to equal the surface error distribution of the individual dot clouds.

the terrain in these specific dot clouds. Ideally, we would like an error statistic which describes the shape and size of this region for each individual pair of submaps. However, in this initial effort we have chosen a constant error covariance that bounds this convergence region for many map pairs in a χ2 99% confidence sense. Point to plane trajectories, maps [5, 6] 2 Start End

1.5 1

Y [m]

0.5 0 −0.5 −1 −1.5 −2 −2

−1

0

1

2

X [m]

Fig. 7. This image shows x y trajectories for a point-to-plane ICP submap registration starting at randomized points around a nominial solution. At trajectories have converged to an O(.25m) size region.

V. R ESULTS Our sub-mapping approach has been applying to the survey data described in Section II. We can extract the final pose locations of the sub-map origins from the delayed state vector after the final sub-map has been broken and all possible relative pose measurements to prior maps have been incorporated in the filter. The network shown in Fig. 8 indicates the submap poses and the links between them where relative pose measurements have been made. This network of poses is dense and was assembled without large loop closures. This is largely do to the design of the 6

From the total dot cloud a surface of the form z = f (x, y) can be produced by gridding the data. Fig. 10 shows a comparison between the map created with our SLAM algorithm and a map created from the same navigation data processed by a standard smoothing algorithm for combining LBL and Doppler navigation [22].

Fig. 8. This shows the pose network developed by the delayed state EKF. This survey was broken down into 44 sub-maps with 166 links connecting them. The poses are shown as ellipses whose size has been scaled to 5 times the actual 99% χ2 bound. The network is shown over top of the LBL navigation fixes. Note that the sub-map poses have aligned themselves over the LBL paths better that the DR track shown in Fig. 1. Also note that one sub-map was not registered with the network and has a larger error bound.

survey path that keeps the vehicle close to previously visited areas at all times. As such the sub-map origins do not move far from where they were originally placed and the relative pose constraints were incorporated into the filter. Large deviations from the initial positions would suggest that linearization errors could be affecting the solution.

Fig. 10. Comparison between the terrain map created using sub-mapping, (TOP), and a version of the map created using a standard smoothing method (BOTTOM). The sub-map created map shows significantly less registration error and sonar scan patterning. The sub-mapped version also brings out details that are were lost in the smoothed map due to mis-registration.

Fig. 9. color.

Using the surface error metric described in Section IV-C we can evaluate the total terrain reconstruction. Fig. 11 shows a comparison between the surface errors for the maps shown in Fig. 10. The SLAM map shows a lower overall surface variance and most importantly a reduced variance in the center region where the amount of sub-map overlap is high, as shown in Fig. 9. If our method was doing a poor job of registration the surface variance would increase in proportion to the number of overlapping sub-maps. The map created by smoothing shows higher overall surface error. To evaluate the navigation aspect of our method we can compare the final sub-map poses to the original LBL data, Fig. 8, and see that the large scale distortion shown in Fig. 1 has been taken out of the navigation track. We are encouraged to see the sub-map poses migrate back to the LBL fixes because the LBL is a ground based position estimate. However, we

The stacking depth of the individual sub-maps is indicated here by

The total amount of sub-map overlap can be seen in Fig. 9. The stacking depth of the sub-maps indicates the number of times a particular part of the sea floor was imaged. Note that the sub-map outlines indicate larger maps closer to the outside of the survey area, where the terrain is flatter, and smaller maps closer to the center where the terrain is more featured. This was caused automatically by the map breaking criteria described in Section IV-B. A final composite of the entire survey area is constructed by merging all of the sub-maps dot clouds into a single dot cloud. 7

1

3050

3100

0.8

0.5 3250

0.4

0.8 0.7

3150 0.6 Y [m]

Y [m]

0.6 3200

Standard deviation [m]

0.7

3150

0.9

3100

3200

0.5 0.4

3250

0.3

0.3

3300 0.2 3350 3100

3150

3200

3250

3300 X [m]

3350

3400

3450

0.1

Standard deviation [m]

0.9

[2] P. H. Milne, Underwater Acoustic Positioning Systems. Gulf Publishing Company, Houston, 1983. [3] B. Bingham, “Precision Autonomous Underwater Navigation,” Ph.D. dissertation, Massachusetts Institute of Technology, 2003. [4] L. Whitcomb, D. Yoerger, and H. Singh, “Advances in Doppler-Based Navigation of Underwater Robotic Vehicles,” in Proceedings of the 1999 International Conference on Robotics and Automation, vol. 1, 1999, pp. 399–406. [5] S. Majumder, “Sensor fusion and feature based navigation for sub-sea robots,” Ph.D. dissertation, The University of Sydney, ACFR, 2001. [6] J. Leonard, R. Rikoski, P. Newman, and M. Bosse, “Mapping partially observable features from multiple uncertian vantage points,” The International Journal of Robotics Research, pp. 943–975, October-November 2002. [7] J. Guivant and E. Nebot, “Optimization of the simultaneous localization and map building algorithm for real time implementation,” IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp. 242–257, June 2001. [8] J. Leonard and P. Newman, “Consistent, Convergent, and Constant-time SLAM,” in Proc. 18th Int. Joint Conf. on Artificial Intelligence (IJCAI03), 2003, pp. 1143–1150. [9] U. Frese and G. Hirzinger, “Simultaneous localization and mapping - a discussion,” in Proc. IJCAI Workshop on Reasoning with Uncertainty in Robotics, August 2001. [10] S. Thrun, Y. Liu, Z. Koller, H. Ghahramani, H. Durrant-Whyte, and A. G. Ng, “Simultaneous mapping and localization with sparse extended information filters,” Internation Journal of Robotics Research, vol. 23, pp. 693–716, 2004. [11] R. Eustice, O. Pizarro, and H. Singh, “Visually augmented navigation in an unstructured environment using a delayed state history,” in IEEE ICRA 2004, April 2004. [12] R. Eustice, H. Singh, and J. Leonard, “Exactly Sparse Delayed-State Filters,” in Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona, SPAIN, April 2005. [13] F. Lu and E. Milios, “Globally consistent range scan alignment for evironment mapping,” Autonomous Robots, vol. 4, pp. 333–349, 1997. [14] J. Gutmann and K. Konolige, “Incremental mapping of large cyclic environments,” in In Proc. IEEE Int. Symposium on Computational Intelligence in Robotics and Automation - CIRA-99, November 1999. [15] U. Frese and T. Duckett, “A multigrid approach for accelerated relaxation-based SLAM,” in In Proc. IJCAI Workshop on Reasoning with Uncertainty in Robotics, 2003, pp. 39–46. [16] M. Bosse, “ATLAS, A Framework for Large Scale Automated Mapping and Localization,” Ph.D. dissertation, Massachusetts Institute of Technology, 2004. [17] C. Schlegel and T. Kampke, “Filter design for simultaneous localization and map building (SLAM),” in In Proc. IEEE Internation Conference on Robotics & Automation, May 2002. [18] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation. John Wiley & Sons, Inc, 2001. [19] R. Smith, M. Self, and P. Cheeseman, Estimating uncertain spatial relationships in robotics. Autonomous Robot Vehicles, Springer-Verlag, 1990, pp. 167–193. [20] P. J. Besl and N. D. McKay, “A method for registration of 3-d shapes,” IEEE PAMI, vol. 14, pp. 239–256, 1992. [21] S. Rusinkiewic and M. Levoy, “Efficient variants of the ICP algorithm,” in Proc. the International Conference on Recent Advances in 3-D Digital Imaging and Modeling, Canda, 2001, pp. 145–152. [22] M. V. Jakuba and D. R. Yoerger, “High-Resolution Multibeam Sonar Mapping With The Autonomous Benthic Explorer (ABE),” in UUST 2003, Durham NH, August 2003.

1

Surface error for the sub−mapped map

Surface error for the smoothed navigation map

3300

0.2

3100

3150

3200

3250

3300 X [m]

3350

3400

3450

0.1

Fig. 11. Comparison of the errors between the SLAM created terrain map and the smoothed map. A reduction in surface variance is achieved using the SLAM sub-mapping method. This surface estimate was generated by gridding the entire dot cloud in 2m square bins.

remain skeptical of incorporating all of the LBL data into the position estimation because of the lower quality of the terrain map created using the LBL and Doppler smoothing. The majority of the error between the LBL the DR tracklines that was removed by our method was created from a static bias between the Doppler velocity sensor and the attitude senor. Through the sub-mapping process we have been able to reduce the global distorting affect of this bias and simultaneously generate a better terrain map. However, we do note that each individual sub-map is still affected by this bias and that the algorithm has broken down the total bias into smaller pieces. VI. C ONCLUSIONS We have presented a framework to improve acoustic terrain mapping using a SLAM algorithm that enforces consistency between the vehicle navigation data and the acoustic mapping data. This delayed state EKF based solution has proven adequate for surveys containing up to 50 sub-maps in a dense network of poses. Our example has shown that this technique can reduce many of the common problems associated with creating accurate terrain maps. Maps constructed using this sub-mapping technique show more detail and have less surface variance than maps created using more standard methods. To improve this method we would like to develop better error statistics for the relative pose measurements between bathymetric sub-maps. This will require further investigation of the underlying navigation errors, sonar errors and bias contained within the individual sup-maps. We would also like investigate the registration algorithm itself and tailor it more toward acoustic sensing and compare it to ground truthed data. VII. ACKNOWLEDGEMENTS This work was funded in part by the Censsis ERC of the National Science Foundation under grant EEC-9986821 and in part by the Woods Hole Oceanographic Institution through a grant from the Penzance Foundation. The authors are thankful for the opportunities and efforts provided by Dr. Rob Reeves-Sohn, Dr. Susan Humphris and the members of the WHOI Deep Submergence Laboratory. R EFERENCES [1] H. Singh, L. Whitcomb, D. Yoerger, and O. Pizarro, “Microbathymetric Mapping from Underwater Vehicles in the Deep Ocean,” Computer Vision and Image Understanding, vol. 79, no. 1, pp. 143–161, July 2000.

8