Probabilistic sonar scan matching for an AUV

This paper proposes MSISpIC, a probabilistic sonar scan matching algorithm for the localization of an autonomous underwater vehicle (AUV). The technique uses range scans gathered with a Mechanical Scanning Imaging Sonar (MSIS), the robot displacement estimated through dead-reckoning using a Doppler velocity log (DVL) and a motion reference unit (MRU). The proposed method is an extension of the pIC algorithm. An extended Kalman filter (EKF) is used to estimate the robot-path during the scan in order to reference all the range and bearing measurements as well as their uncertainty to a scan fixed frame before registering. The major contribution consists of experimentally proving that probabilistic sonar scan matching techniques have the potential to improve the DVL-based navigation. The algorithm has been tested on an AUV guided along a 600 m path within an abandoned marina underwater environment with satisfactory results.


I. INTRODUCTION
During a long term mission with an autonomous robot it is necessary to keep the track of the vehicle's position. Scan matching is a technique that can be used to estimate the vehicle displacement using successive range scans. Many applications in robotics like mapping, localization or tracking use this technique to estimate the robot's relative displacement [1], [2]. Scan Matching estimates the robot relative displacement between two configurations, by maximizing the overlap between the range scans normally gathered with a laser or a sonar sensor. Moreover, the scans can then be used to set up a local map of the robot's surrounding environment to be used for reactive/deliberative obstacle avoidance [3].
The existing scan matching techniques can be divided into groups depending on if they use high-level entities like lines or planes or otherwise they rely on the raw scan. On one hand, it is possible to assume the existence of polygonal structures in structured environments as is supposed in [4] [5], or even in some underwater applications [6]. However, extracting simple polygons for representing the environment is not always possible, particularly in unstructured scenarios which are the most common in underwater robotics. On the other hand, there is a second type of algorithms that work with raw data from the scanner to solve the matching. Usually, these techniques are based on a two step iterative process which is repeated till convergence. The sensor displacements are computed by approximating the solution to the best overlap between two scans by looking for the closest point for each single data of the scan. After that, a minimization process to estimate the solution is done. The process is repeated until convergence. The most popular technique is the Iterative Closest Point (ICP) algorithm [7] which has been modified in different ways [8]. Most of the versions of the ICP algorithm use the Euclidian distance to estimate the correspondences between scans. However, this distance does not take into account that small rotations of the sensor mean large displacements as the distance is increased. To overcome this limitation several approaches have been done. The Iterative Dual Correspondence (IDC) algorithm [9] computes two different minimizations, one for the translation and another for the rotation. The Metric-based Iterative Closest Point (MbICP) algorithm [10] establishes a new distance concept which captures the sensor displacement and rotation at the same time. However, none of these algorithms take into account the sensor nor the displacement uncertainties which are very important, specially when sonar sensors are used. The probabilistic Iterative Correspondence (pIC) method [11] explicitly deals with those uncertainties to decide which points in the reference scan are statistically compatible with a certain point of the new scan. Moreover, the minimization process is carried out over the Mahalanobis distance. Although in this case the algorithm was applied to laser scans, its probabilistic formulation is able to improve the robustness and accuracy. On the other hand, in [12] another probabilistic approach is proposed in order to deal with the noisy and sparse measurements obtained from conventional time-of-fly sonar sensors of a mobile robot. This method deals with the sparsity of readings by grouping sonar measurements along short trajectories. It uses probabilistic models of ultrasonic and odometry sensors as well as a method to propagate the error through them in order to estimate a group of scan positions together with their uncertainty.
Although a large literature exists reporting successful applications of scan matching to mobile robots, very few attempts have been done to use sonar scan matching in underwater applications. In [13] a non-probabilistic variation of ICP is proposed to achieve on-line performance for registering multiple views captured with a 3D acoustic camera. Silver et al. [14], proposed to use a particle filter to deal with the sonar noisy data. Like us, they use a MSIS but in their case only simulated results are reported. A very nice application of sonar scan matching underwater is reported in [15] were the standard ICP is used for registering bathymetric sub-maps gathered with a multibeam sonar profiler. In their work the standard ICP algorithm is used and hence the uncertainty of the scan points is not taken into account during the registering. In this paper we propose the MSISpIC, an extension of the basic pIC algorithm to deal with data gathered by an AUV using a MSIS. In our approach, the robot moves while scanning the environment. Hence, an EKF using a constant velocity model with acceleration noise, updated with velocity and attitude measurements obtained from a DVL and a MRU respectively, is used to estimate the trajectory followed by the robot along the scan. This trajectory is used to remove the motion induced distortion of the acoustic image as well as to predict the uncertainty of the range scans prior to register them through the standard pIC algorithm. To the best of the authors knowledge, this is the first reported application of a probabilistic sonar-based scan matching technique over a real data dataset gathered with an AUV using a MSIS.
The paper is structured as follows. In section II the pIC algorithm is described. Section III details the process to grab complete scans from the MSIS to be used in our scan matching algorithm which is described in section IV. Section V reports the experimental results before conclusions and future work.

II. PROBABILISTIC ITERATIVE CORRESPONDENCE
As stated before, the pIC algorithm is a statistical extension of the ICP algorithm which is able to deal with sparse sonar data. As detailed in Algorithm 1, the inputs are the reference scan S ref , the new scan S new and the initial relative displacement estimationq between them with its covariance P q . The following procedure is iteratively executed until convergence. First, the points of S new are compounded with the robot displacement (q k ). The result are the points of the new scan referenced to the reference frame (c j ). Then, for each c j , a set (A j ) of all the compatible points in the reference scan (S ref ) is established using a compatibility test over the Mahalanobis distance. Next step consists of computing the virtual association point (a j ) as the expectancy over the random variable defined by the set (A j ). It is worth noting that if q ≡ N (q, P q ), n j ≡ N (n j , P n j ) and r i ≡ N (r i , P r i ), it is possible to compute the probability p(r i = c j ) for every element (r i ) of A j of being a correct matching for c j . In order to do so, let us define a random Gaussian variable (r.g.v.) which describes the error of the {r i , c j } pairing: where p(e ij = 0) can be computed as follows: where f e ij is the probability density function of e ij r.g.v. Onceâ j has been computed, a similar procedure can be used to estimate its uncertainty P a j , before computing the uncertainty P e j of the matching error (â j −ĉ j ). Now, it is possible to estimate the displacementq min which minimizes the square error of the Mahalanobis Distance betweenâ j and c j . This is done using Least Squares minimization method. If there is convergence, the function returns, otherwise another iteration is required.

III. SCAN GRABBING USING A MSIS
The pIC algorithm was conceived to accept as input parameters two range scans and optionally a rough displacement estimation between them. Moreover, it uses a laser range finder which gathers scans almost instantaneously. However, for the underwater environment, commercially available scan sensors are based on acoustics. Most of these sensors have a mechanical head that rotates at fixed angular steps. At each step, a beam is emitted and received, measuring ranges to the obstacles found across its trajectory. Hence, getting a complete scan lasts few seconds while the vehicle is moving, generating deformed scans. Therefore, a correction taking into account the robot pose when the beam was grabbed is necessary.

A. Beam segmentation and range detection
The MSIS returns a polar acoustic image composed of beams. Each beam has a particular bearing angle value and a set of intensity measurements. The angle corresponds to the orientation of the sensor head when the beam was emitted. The acoustic linear image corresponding to one beam is returned as an array of acoustic intensities detected at certain distances. The beam is then segmented using a predefined threshold to compute the intensity peaks. Due to the noisy nature of the acoustic data, a minimum distance between peaks criteria is also applied. Hence, positions finally considered are those corresponding to high intensity values above the threshold with a minimum distance between each other.

B. Trajectory estimation
The pIC algorithm needs a complete scan to be registered with the previous one in order to estimate the robot's displacement. Since MSIS needs a considerable period of time to obtain a complete scan, if the robot does not remain static, the robot's motion induces a distortion in the acoustic image (Fig. 1). To deal with this problem it is necessary to know the robot's pose at the beam reception time. Hence, it is possible to define an initial coordinate system I to reference all the range measurements belonging to the same scan. In our case, this initial frame is fixed at the robot pose where the first beam of the current scan was read.
The localization system used in this work is a slight modification of the navigation system described in [16]. In this system, a Xsense MRU provides heading measurements and a SonTek Argonaut DVL unit which includes 2 inclinometers and a depth sensor is used to estimate the robot's pose during the scan (navigation problem). MSIS beams are read at 30Hz while DVL and MRU readings arrive asynchronously at a frequency of 1.5Hz and 10Hz respectively. An EKF is used to estimate the robot's 6DOF pose whenever a sonar beam is read. DVL and MRU readings are used asynchronously to update the filter. To reduce the noise inherent to the DVL measurements, a simple 6DOF constant velocity kinematics model is used.
The information of the system at step k is stored in the state vector x k with estimated meanx k and covariance P k : with: where, as defined in [17], η B is the position and attitude vector referenced to a base frame B, and ν R is the linear and angular velocity vector referenced to the robot's coordinate frame R. The coordinate frame B is chosen coincident with I but oriented to the north, hence the compass measurements can be integrated in a straight forward manner.
The vehicle's movement prediction is performed using the 6DOF kinematic model: Although in this model the velocity is considered to be constant, in order to allow for slight changes, a velocity perturbation modeled as the integral of a stationary white noise v k is introduced. The covariance matrix Q k of this acceleration noise is diagonal and in the order of magnitude of the maximum acceleration increment that the robot may experience over a sample period.
Hence, v k is the acceleration noise which is integrated and added in velocity (6), being nonlinearly propagated to the position. Finally, the model prediction and update is carried out as detailed below: 1) Prediction: The estimate of the state is obtained as: and its covariance matrix as: where F k and G k are the Jacobian matrices of partial derivatives of the non-linear model function f with respect to the state x k and the noise v k , respectively.
2) Update using DVL measurements: The model prediction is updated by the standard Kalman filter equations each time a new DVL measurement arrives: where subindex b stands for bottom tracking velocity, w for through water velocity, i for inclinometers and c represents the compass. The measurement model is: where w k (measurement noise) is a gaussian zero-mean white noise: Since the DVL sensor provides a status measurement for bottom tracking and water velocity, depending on the quality of the measurements, different versions of the H matrix are used to fuse one (removing row 2), the other (removing row 1), or both readings (using the full matrix).
3) Update using MRU measurements: Whenever a new attitude measurement is available from the MRU sensor, the model prediction is updated using the standard EKF equations: where w k (measurement noise) is a gaussian zero-mean white noise: C. Scan forming The navigation system presented above is able to estimate the robot's pose, but the uncertainty will grow without limit due to its dead-reckoning nature. Moreover, we are only interested in the robot's relative position (and uncertainty) with respect to the beginning of the scan (I). Hence a slight modification to the filter is introduced making a reset in position (setting x, y, z to 0 in the vector state) whenever a new scan is started. Therefore, while the filter is working, the estimated position is always relative to the position where the first beam of the scan was gathered (I). Note that it is important to keep the ψ value (it is not reset) because it represents an absolute angle with respect to the magnetic north and a reset would mean an unreal high rotation during the scan. The same thing applies to φ and θ. Since we are only interested in the uncertainty accumulated during the scan, the reset process also affects the x, y, and z terms of the covariance matrix P. Now, the modified filter provides the robot's relative position where the beams were gathered including its uncertainty accumulated during the scan. Hence, using a similar procedure than in [18], it is possible to reference all the ranges computed from the beams to the initial frame I, removing the distortion induced by the robot's motion by using the following method. Let • ρ ≡ N (ρ, P ρ ) be a r.g.v. corresponding to the polar measurement whereρ = (β, r) is the observed measurement and P ρ its corresponding uncertainty.
corresponding to the robot's uncertain position where the ρ beam was gathered. This value is estimated by the EKF and is represented in the northern referenced frame B. • x I B ≡ N (x I B , P IB ) be a r.g.v. corresponding to the transformation needed to map B frame to I frame. In our particular case, this is a null translation followed by a rotation used to align B with I. • x R S be a deterministic vector that describes the position and attitude of the sensor frame S with respect to the robot's frame R. Note that this is non-random rigid body transformation.
then, it is possible to compute the position (and uncertainty) of any observed point referenced to the initial frame I as follows: where P 2C(ρ) turns polar into Cartesian coordinates First, the function P 2C transforms the range and bearing data ρ = (β, r) T from Polar space to Cartesian space. The result is the observed point p S referenced to the S frame. As stated, p S is a r.g.v which mean (p S ) and covariance (P S ) can be easily computed. Then, by means of a rigid body transformation, the point is referenced to the robot's frame R. Again, the new representation p R is a r.g.v with mean p R and covariance P R . Now, the robot's relative position x B R computed with the EKF is compounded with the robot's referenced point p R to get the r.g.v. p B with meanp B and covariance P B . Finally, the last compounding operation rotates the point to reference it to the initial frame I. As in the previous cases, p I is a r.g.v. with a known mean (p I ) and covariance P I . Fig. 2 illustrates this process while the scan grabbing process in algorithmic notation is described in Algorithm 2.

IV. THE MSISPIC ALGORITHM
Once the pIC and the ScanGrabbing algorithms have been set up, it is very simple to localize the robot. This is the purpose of the MSISpIC algorithm (see Algorithm 3), which iteratively grabs two scans and register them using the pIC algorithm. It is worth noting that the pIC takes as input two consecutive scans (S new and S ref ) and its relative displacement which coincides with the position occupied by the robot at the end of the first scan (q ref ). The output is an improved estimation of the robot displacement (q new ). The iterative compounding of the relative displacement allows to track the global robot position.
The method described in this paper has been used with a dataset obtained in an abandoned marina located in Sant Pere Pescador, on the Catalan coast [18]. The experiment was carried out using ICTINEU AUV [6] traveling along a 600m path. Because the Differential Global Positioning System (DGPS) signal is not available when submerged, a surface buoy equipped with a DPGS receiver was attached to the robot in order to gather the ground truth trajectory. The MSIS was configured to scan the whole 360 • sector and it was set to fire up to a 50m range with a 0.1m resolution and a 1.8 • angular step. Dead-reckoning was computed using the velocity reading coming from the DVL and the heading data obtained from the MRU sensor, both merged using the described EKF. Standard deviation for the MSIS sensor was set as it is specified by the manufacturer, 0.1m in range and 1.8 • in angular measurements. The whole dataset was acquired in 53min and the off-line execution of the algorithm implemented in MATLAB took around 16min in a CoreDuo@1,83GHz laptop. Fig. 3.a shows the trajectory of the DGPS and the raw map plotted on an orthophotomap used as a ground truth. Fig. 3.b and 3.c show the trajectories and raw maps estimated using the dead-reckoning method and the MSISpIC algorithm respectively. It can be clearly appreciated that the deadreckoning estimated trajectory suffers from an important drift which is considerably reduced when the MSISpIC algorithm is used. Most of the error of the MSISpIC estimated trajectory appears when the robot is traversing an area where the scan only observes one or two walls parallel to the robot path, being able to correct the lateral displacement but still drifting in the forward direction. It is worth noting that, even in the presence of structures in all the directions, scan matching algorithms are expected to drift due to its iterative formulation. Note that we are performing scan to scan registration and not scan to map registration.
The absolute error of dead-reckoning and MSISpIC trajectories with respect to the DPGS ground truth are depicted in Fig. 3.d. The results show an important improvement of the MSISpIC respect to dead-reckoning trajectory, which after traveling the 600m path differs from DGPS around 40m while the MSISpIC maximum difference is set at 12m. Fig. 3.e and 3.f show an occupancy grid map built using the dead-reckoning and MSISpIC estimated trajectories respectively plotted on the orthophotomap. Both maps are built using an standard occupancy grid mapping algorithm [19] with simple range finder model [20] to project the sensor data into the map. The cell resolution of the map is 0.4m. The probabilities for the sensor model were set up experimentally to be 0.3, 0.5 and 0.9 for the free, the unknown and the occupied cells respectively. The color scale of the maps is blue for the free space, transparent green for the unknown area and from yellow to red for the occupied cells.
Using the dead-reckoning trajectory, most of the map (Fig. 3.e) allows to distinguish the shape of the marina except at last part (after the corridor) where the points that should compound the left wall are spread away. The map generated with the scan matching trajectory (Fig. 3.f) shows a substantial improvement with respect to the one generated using dead-reckoning as the walls are clearly less sparse.

VI. CONCLUSIONS
This paper proposes a variation of the pIC algorithm called MSISpIC which is able to perform underwater scan matching using a MSIS. To deal with the motion induced distortion of the acoustic image, an EKF is used to estimate the robot motion during the scan. The filter uses a constant velocity model with acceleration noise for motion prediction and velocity (DVL) and attitude measurements (MRU) for updating the state. Through the compounding of the relative robot position within the scan, with the range and bearing measurements of the beams gathered with the sonar, the acoustic image gets undistorted. Assuming Gaussian noise, the algorithm is able to predict the uncertainty of the sonar measurements with respect to a frame located at the position occupied by the robot at the beginning of the scan, before applying the standard pIC algorithm. The proposed method has been tested with a dataset acquired during a survey mission in an abandoned marina located in the Girona coast. The results show substantial improvements in trajectory correction and map reconstruction.

VII. FUTURE WORK
Once it has been experimentally proven that sonar scan matching has the potential to improve the DVL-based navigation, we are currently working on the cloning of the robot pose within the EKF in order to: 1) smooth the trajectory during the scan and 2) use the result of the scan matching as a constrain between the boundary poses of the scan in order to correct the full scan trajectory. Next step will consist of gathering a new dataset in an unstructured environment to check how the algorithm performs in such situation.