Multibeam 3D Underwater SLAM with Probabilistic Registration

Abstract

This paper describes a pose-based underwater 3D Simultaneous Localization and Mapping (SLAM) using a multibeam echosounder to produce high consistency underwater maps. The proposed algorithm compounds swath profiles of the seafloor with dead reckoning localization to build surface patches (i.e., point clouds). An Iterative Closest Point (ICP) with a probabilistic implementation is then used to register the point clouds, taking into account their uncertainties. The registration process is divided in two steps: (1) point-to-point association for coarse registration and (2) point-to-plane association for fine registration. The point clouds of the surfaces to be registered are sub-sampled in order to decrease both the computation time and also the potential of falling into local minima during the registration. In addition, a heuristic is used to decrease the complexity of the association step of the ICP from $O\left(n^2\right)$ to $O\left(n\right)$. The performance of the SLAM framework is tested using two real world datasets: First, a 2.5D bathymetric dataset obtained with the usual down-looking multibeam sonar configuration, and second, a full 3D underwater dataset acquired with a multibeam sonar mounted on a pan and tilt unit.

1. Introduction

For Autonomous Underwater Vehicles (AUVs), addressing the navigation and mapping problems is crucial to achieve a fully operational status. Dead reckoning navigation systems suffer from an unbounded drift error, regardless of using high-end Internal Navigation Systems (INS) [1]. To avoid this, such systems are commonly aided with absolute positioning fixes. Using the measurements from a Global Positioning System (GPS) receiver is a typical solution during operations taking place on the surface. When the vehicle is submerged, Long Base Line (LBL) systems [2] can be used for the same purpose, although complex calibration of the acoustic beacon network is required prior to its operation. Using single beacon/transponder methods may reduce the calibration burden [3,4] or even eliminate it, at the cost of a reduced accuracy, when inverted LBL [5] or Ultra Short Base Line (USBL) [6] systems are used instead.

All those methods share the limitation of confining the robot operation to the area of coverage of the system. Terrain-based navigation (TBN) methods [7] can mitigate this limitation when an a priori Digital Terrain Map (DTM) is available on the target area. However, for an underwater vehicle to become truly autonomous, it should be able to localize itself using only on-board sensors and without the help of any external infrastructure. The Simultaneous Localization And Mapping (SLAM) concept aims to achieve that. Although more than 20 years of research have provided different approaches to solve the SLAM problem, mostly in land mobile robotics [8], there are still few solutions for underwater use, mainly due to the sensing limitations imposed by the medium and the complexity of the environment.

Underwater SLAM can be divided in two main categories: sonar and vision based SLAM. Although vision sensors may suffer from poor visibility in turbid waters, they provide fast refresh rates and high-resolution data at a fraction of the cost of a sonar sensor. Several noteworthy examples of underwater visual SLAM have been presented during recent years [9,10,11,12]. On the other hand, sonar sensors can work in bad visibility conditions, penetrating further (10–150 m) because of the low attenuation of sound in water. However, the refresh rate and resolution are medium to low and are generally expensive. Although the number of underwater SLAM examples using sonar is still reduced, they are promising.

Regarding imaging sonar mosaicking, [13] presented a feature-based registration method for two-dimensional forward-looking sonar images, while [14] developed a Fourier-based registration method to build large-scale mosaics. Moreover, several feature-based methods have been reported using: (1) point features extracted from mechanically/electronically scanned imaging sonars [15,16,17] or using a synthetic aperture imaging sonar [18]; and (2) line features extracted from a Mechanical Scanning Imaging Sonar (MSIS) in a man-made environment [19]. However, it is extremely difficult to extract features robustly in a natural underwater environment. Therefore, some researchers have focused on using featureless methods such as scan matching or occupancy grids. The work presented in [20] proposed a SLAM algorithm using a Particle Filter (PF) and range measurements from multiple pencil-beam sonars to generate an occupancy grid of a sinkhole. The method was time and computationally efficient because of the use of an octree structure to represent the environment. Although bathymetric (elevation 2.5D) maps are commonly used in the context of TBN, there have been few studies reporting successful SLAM implementations using bathymetric maps generated with data from a multibeam profiler. The pioneering work in [21] used cross correlation and Iterative Closest Point (ICP) for coarse and fine registration of bathymetric surfaces. More recently, [22] presented the bathymetric distributed particle SLAM (BPSLAM), an algorithm based on the distributed particle SLAM (DPSLAM) [23], which used a PF similar to the one proposed in [20] but representing the environment as a bathymetric map distributed across the ancestry of a given particle. It is worth mentioning that those methods were specifically designed for 2.5D elevation data, and, therefore, they are not suited for full 3D underwater environments.

This paper presents the extension to 3D of the work previously presented in [24]. The registration algorithm is a 3D-capable evolution of the 2D MSIS probabilistic Iterative Correspondence (MSISpIC) algorithm [25], which has been already applied to 2D SLAM in underwater man-made [26,27] and natural [28] environments. Our method is similar to the previous work of [21] but takes advantage of recent results obtained using the probabilistic ICP algorithms mentioned above, which are better suited to dealing with the uncertainty inherent in sonar data. Moreover, our method is not restricted to using solely 2.5D bathymetric data, and, hence, new results obtained with full 3D data are also reported here.

The 3D underwater SLAM framework presented here corrects the robot trajectory in order to produce high consistency underwater maps. The algorithm, like other state of the art SLAM techniques [21], divides the mission into a set of submaps, or surface patches, created by combining multibeam data and an estimate of the navigation until certain criteria are fulfilled. Every time a submap is created, possible overlaps with other existing patches are checked to look for loop closures. If any are found, the registration process takes place between the patches in order to refine the robot navigation. One of the novelties of the proposed method is the implementation of a two-step Probabilistic ICP (pICP) with point-to-point and point-to-plane for rough and fine registration, respectively. The improved registration method also incorporates a point cloud subsampling strategy to decrease the number of involved points as well as a novel method to decrease the complexity in the association step of the pICP from $O\left(n^2\right)$ to $O\left(n\right)$.

The rest of the paper is organized as follows. First, Section 2 focuses on the submap creation. In Section 3, the registration algorithm is explained, followed by the SLAM algorithm in Section 4. Section 5 presents the experiments and results, and, finally, Section 6 presents the conclusions and future work.

2. Submap Creation

For bathymetric mapping, multibeam sonars are generally fixed to the vehicle so that the 2D swath profiles are generated perpendicular to the surge direction. In that way, 2.5D surfaces are built by composing the multibeam data with the displacement of the vehicle. Alternatively, more complex environments can be inspected by sweeping a multibeam sonar mounted on a pan and tilt unit, so it is the rotation of the sonar head, and not solely the vehicle motion, that leads to the coverage of the surfaces. The point clouds resulting from the collection of multibeam data (Figure 1), along with other information such as boundaries or position with respect to the world, are what we refer to in this work as patches or submaps. This section describes the process of building these submaps during a mission.

2.1. Dead Reckoning

To be able to construct the submaps, regardless of whether the sonar is being swept or mounted on a fixed position, it is necessary to estimate the AUV position at the time each multibeam reading was acquired. As will be later detailed in Section 2.2, our procedure uses the relative displacements made by the vehicle between consecutive multibeam swaths to compound the point clouds. Moreover, given the probabilistic nature of the proposed registration algorithm, it is also necessary to estimate how the uncertainty evolves during these motions. To obtain this information, an Extended Kalman Filter (EKF) is used.

The state vector of the filter (see Equations (1) and (2)) contains 12 elements representing the current six Degrees of Freedom (DoF) vehicle position and velocity, as well as two more elements corresponding with the stored x and y position of the vehicle at the time when the last multibeam reading was obtained ($x_{mb},y_{mb}$):

$${\widehat{\mathit{x}}}_k={\left[\begin{array}{cccccccccccccc}x& y& z& \varphi & \theta & \psi & u& v& w& p& q& r& x_{mb}& y_{mb}\end{array}\right]}_k^T$$

(1)

$${\mathit{P}}_{{\mathit{x}}_k}=E{\left(\left[{\mathit{x}}_k-{\widehat{\mathit{x}}}_k\right]{\left[{\mathit{x}}_k-{\widehat{\mathit{x}}}_k\right]}^T\right)}^T$$

(2)

A constant velocity kinematic model is used for prediction of the vehicle states, while those regarding the stored previous vehicle position are assumed static. In the correction stage, updates are performed asynchronously with the measurements coming from an Attitude and Heading Reference System (AHRS), a Doppler Velocity Log (DVL), and a pressure sensor. The filter iterates normally until a new multibeam reading is received. When this occurs, one last prediction is made to get an updated estimation of the vehicle’s position before calculating ${\mathit{o}}_k=N({\widehat{\mathit{o}}}_k,{\mathit{P}}_{{\mathit{o}}_k})$, a new vector containing the displacement executed by the vehicle in the horizontal plane during the period of time between the current and the previous multibeam readings, as well as the z position and orientation of the vehicle at the current time:

$${\widehat{\mathit{o}}}_k={\left[\begin{array}{cccccc}x-x_{mb},& y-y_{mb},& z,& \varphi ,& \theta ,& \psi \end{array}\right]}_k^T$$

(3)

$${\mathit{P}}_{{\mathit{o}}_k}={\mathit{J}}_o{\mathit{P}}_{{\mathit{x}}_k}{\mathit{J}}_o^T;{\mathit{J}}_o=\left[\begin{array}{cccc}{\mathit{I}}_{2\times 2}& {\mathbf{0}}_{2\times 4}& {\mathbf{0}}_{2\times 6}& -{\mathit{I}}_{2\times 2}\\ {\mathbf{0}}_{4\times 2}& {\mathit{I}}_{4\times 4}& {\mathbf{0}}_{4\times 6}& {\mathbf{0}}_{4\times 2}\end{array}\right]$$

(4)

Note that the two first elements of ${\mathit{o}}_k$ correspond to incremental values, while the other four are absolute with respect to the base reference frame used for the dead reckoning. The calculation of those increments is motivated by the cumulative drift that affects the motion in the horizontal plane. Since those states are only estimated indirectly by the velocity measurements from the DVL, the uncertainty in the $xy$ position grows without bound. As will be introduced in the following section, working with those increments allows for a better distribution of the uncertainties within the point cloud. On the other hand, the remaining states in ${\mathit{o}}_k$ are observed directly by other sensors (the z position is observed by the pressure sensor, and the orientation by the AHRS), and therefore their uncertainties are bounded.

Once ${\mathit{o}}_k$ has been calculated, it is stored until the current submap is finalized. To continue with the execution of the dead reckoning filter, and to keep track of the displacements from the current position to that of the next multibeam measurement, it is necessary to replace the last two elements of the state vector ($x_{mb},y_{mb}$) with the current position of the vehicle x and y:

$${\widehat{\mathit{x}}}_k^{*}={\left[\begin{array}{cccccccccccccc}x& y& z& \varphi & \theta & \psi & u& v& w& p& q& r& x& y\end{array}\right]}_k^T$$

(5)

$${\mathit{P}}_{{\mathit{x}}_k^{*}}={\mathit{J}}_o^{*}{\mathit{P}}_{{\mathit{x}}_k}{\mathit{J}}_o^{*T};{\mathit{J}}_o^{*}=\left[\begin{array}{cc}{\mathit{I}}_{12\times 12}& {\mathbf{0}}_{12\times 2}\\ {\mathit{I}}_{2\times 2}& {\mathbf{0}}_{2\times 12}\end{array}\right]$$

(6)

Given that, the execution of the filter can continue by replicating the procedure we have just described.

2.2. Submap Forming

During the execution of the mission, the information required for the generation of the patches is stored in a temporal data structure ${\mathit{S}}_{temp}$:

$${\mathit{S}}_{temp}=\left\{\begin{array}{ccc}\mathit{O}& \mathit{M}& \mathit{R}\end{array}\right\}$$

(7)

where $\mathit{O}=\{{\mathit{o}}_1,...,{\mathit{o}}_n|{\mathit{o}}_i=N({\widehat{\mathit{o}}}_i,{\mathit{P}}_{{\mathit{o}}_i})\}$ is the set of displacements and positions as computed in Equations (3) and (4), while $\mathit{M}=\{{\mathit{m}}_1,...,{\mathit{m}}_n\}$, with ${\mathit{m}}_i=\{{\mathit{\delta }}_1,...,{\mathit{\delta }}_m|{\mathit{\delta }}_i=N({\widehat{\mathit{\delta }}}_i,{\mathit{P}}_{{\mathit{\delta }}_i})\}$, is the set of all the multibeam swaths m, each one containing the corresponding polar range measurements $\mathit{\delta }$. Finally, $\mathit{R}=\{{\mathit{r}}_1,...,{\mathit{r}}_n|{\mathit{r}}_i=N({\widehat{\mathit{r}}}_i,{\mathit{P}}_{{\mathit{r}}_i})\}$ is the set of transformations required to represent the multibeam data with respect to the vehicle frame. This is particularly relevant in the case of a multibeam sonar mounted on a pan and tilt unit, since the transformations will change continuously because of the sweeping motion.

When the amount of accumulated data is deemed sufficient (see the conditions below), the current patch is closed and the contents of ${\mathit{S}}_{temp}$ are used to generate the point cloud and other information that will be necessary later during the registration process. In addition, the position of the recently terminated patch is stored in the state vector of the pose-based EKF in charge of the SLAM process (see Section 4). Before beginning a new patch, the ${\mathit{S}}_{temp}$ is reset to store a new batch of data.

The criteria to close a patch depend on which scenario we are dealing with. If the sonar is scanning a tri-dimensionally rich environment by means of a pan and tilt unit, each complete sweep is taken as an independent submap because, unless a very fast vehicle is used, successive scans will re-visit the same area, which only contributes to increasing the number of points without incorporating significant new information. On the other hand, the situation with typical bathymetric survey missions where the multibeam is fixed on the vehicle is substantially different. Scanned areas are generally not re-visited (not in the same transect), and the seabed is often scarce in features, which may make the successful matching of surface patches difficult. In this case, a combination of three criteria is used to determine when to close a patch:

• Minimum size: A minimum size is defined to avoid handling a large number of tiny patches augmenting unnecessarily the length of the SLAM state vector and reducing the overlapping.
• Maximum size: The maximum size is bounded to avoid handling huge patches with a high uncertainty in the surface points due to the accumulated dead reckoning error.
• Normal occupancy: The surface relief is analyzed to determine when the patch is rich enough to be successfully matched. The procedure basically consists in finding surface normals for each point on the cloud and representing their parametrization on a histogram. If the histogram is sufficiently occupied, the submap is closed.

Once sufficient data has been acquired and the submap is closed, all the stored data is processed to generate the point cloud and the information required for the potential registration with other submaps. In [21], the reference frame for each submap was defined as the position of the robot when the patch was started. Here, the point cloud is generated with respect to a new reference frame $\mathbb{I}$, which is placed on top of the central position of the trajectory executed during the creation of the patch, but oriented like the base frame $\mathbb{B}$ used for the dead reckoning navigation. By placing this frame in the center of the submap, the uncertainty of the points grows from the center of the patch to the edges (see Figure 2b) instead of growing from the beginning to the end (see Figure 2a). This gives a more convenient distribution of the uncertainty in the point cloud which improves the registration [29].

The process of generating the point cloud begins by selecting the central position that will be associated to $\mathbb{I}$, and that will be referenced hereafter with the $mp$ subindex. Then, the ${\mathit{q}}_k$ vector relating a given k position in which a multibeam reading was acquired, and the $\mathbb{I}$ reference frame can be computed from the corresponding ${\mathit{o}}_k$ and ${\mathit{o}}_{mp}$ (both pertaining to O and stored in ${\mathit{S}}_{temp}$) as:

$${\widehat{\mathit{q}}}_k=\left\{\begin{array}{ccc}{\left[\begin{array}{cccccc}{x}_{q_{k-1}}+x_{o_k},& y_{q_{k-1}}+y_{o_k},& z_{o_k}-z_{o_{mp}},& {\varphi }_{o_k},& {\theta }_{o_k},& {\psi }_{o_k}\end{array}\right]}^T& & k>mp\\ {\left[\begin{array}{cccccc}0,& 0,& 0,& {\varphi }_{o_k},& {\theta }_{o_k},& {\psi }_{o_k}\end{array}\right]}^T& & k=mp\\ {\left[\begin{array}{cccccc}{x}_{q_{k+1}}-x_{o_{k+1}},& y_{q_{k+1}}-y_{o_{k+1}},& z_{o_k}-z_{o_{mp}},& {\varphi }_{o_k},& {\theta }_{o_k},& {\psi }_{o_k}\end{array}\right]}^T& & k<mp\end{array}\right.$$

(8)

where $x_{\mathit{a}}$ should be read here as the element x contained in the vector a. Note that for computing ${\mathit{q}}_k$, the vectors ${\mathit{q}}_{k-1}$ (if $k>mp$) or ${\mathit{q}}_{k+1}$ (if $k<mp$) also need to be known. This means that the calculation needs to be done sequentially starting by the $mp$ position and then moving towards both ends of the submap (1 and n). The uncertainty of ${\mathit{q}}_k$ is then computed as:

$${\mathit{P}}_{{\mathit{q}}_k}=\left\{\begin{array}{ccc}{\mathit{J}}_1{\mathit{P}}_{{\mathit{q}}_{k-1}}{\mathit{J}}_1^T+{\mathit{J}}_2{\mathit{P}}_{{\mathit{o}}_k}{\mathit{J}}_2^T+{\mathit{J}}_3{\mathit{P}}_{{\mathit{o}}_{mp}}{\mathit{J}}_3^T& & k>mp\\ {\mathit{J}}_4{\mathit{P}}_{{\mathit{o}}_k}{\mathit{J}}_4^T& & k=mp\\ {\mathit{J}}_1{\mathit{P}}_{{\mathit{q}}_{k+1}}{\mathit{J}}_1^T+{\mathit{J}}_5{\mathit{P}}_{{\mathit{o}}_{k+1}}{\mathit{J}}_5^T+{\mathit{J}}_3{\mathit{P}}_{{\mathit{o}}_{mp}}{\mathit{J}}_3^T+{\mathit{J}}_4{\mathit{P}}_{{\mathit{o}}_k}{\mathit{J}}_4^T& & k<mp\end{array}\right.$$

(9)

being ${\mathit{J}}_j$ the Jacobians of the function:

$$\begin{array}{cc}\hfill {\mathit{J}}_1=& \left[\begin{array}{cc}{\mathit{I}}_{2\times 2}& {\mathbf{0}}_{2\times 4}\\ {\mathbf{0}}_{4\times 2}& {\mathbf{0}}_{4\times 4}\end{array}\right]\end{array}$$

(10)

$$\begin{array}{cc}\hfill {\mathit{J}}_2=& {\mathit{I}}_{6\times 6}\end{array}$$

(11)

$$\begin{array}{cc}\hfill {\mathit{J}}_3=& \left[\begin{array}{ccc}{\mathbf{0}}_{2\times 2}& {\mathbf{0}}_{2\times 1}& {\mathbf{0}}_{2\times 3}\\ {\mathbf{0}}_{1\times 2}& -\mathbf{1}& {\mathbf{0}}_{1\times 3}\\ {\mathbf{0}}_{3\times 2}& {\mathbf{0}}_{3\times 1}& {\mathbf{0}}_{3\times 3}\end{array}\right]\end{array}$$

(12)

$$\begin{array}{cc}\hfill {\mathit{J}}_4=& \left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]\end{array}$$

(13)

$$\begin{array}{cc}\hfill {\mathit{J}}_5=& -{\mathit{J}}_1\end{array}$$

(14)

It is worth noting that ${\mathit{q}}_k$ is composed of both relative (first three elements representing a displacement) and absolute (last three elements being the orientation) measurements. Assuming that correlations in attitude estimates are negligible, computing the relative increment of the orientation would end up adding uncertainty in these 3 DoF artificially. This would apply also to the z displacement. However, we observed that if the same approach is taken in this DoF, the lever-arm effect in the registration process (the depth is referenced to the water surface) makes it much more prone to error. Therefore, we decided to reference the z position to the actual depth of the AUV regardless of the increment of uncertainty in order to make the registration process more stable.

With all the ${\mathit{q}}_k$ computed, the point cloud can now be generated. The first step is to transform all the polar range measurements ${\mathit{\delta }}_i=N(\widehat{{\mathit{\delta }}_i},{\mathit{P}}_{{\mathit{\delta }}_i})$ which are represented in the sensor frame to that of the vehicle using the ${\mathit{r}}_k$ transformations stored in R:

$${\widehat{\mathit{p}}}_i^{\times }={\widehat{\mathit{r}}}_k\oplus g({\widehat{\mathit{\delta }}}_i)$$

(15)

$${\mathit{P}}_{{\mathit{p}}_i^{\times }}={\mathit{J}}_{1\oplus }{\mathit{P}}_{{\mathit{r}}_k}{\mathit{J}}_{1\oplus }^T+{\mathit{J}}_{2\oplus }\left({\mathit{J}}_g{\mathit{P}}_{\mathit{\delta }}{\mathit{J}}_g^T\right){\mathit{J}}_{2\oplus }^T$$

(16)

where $g(.)$ is the polar to Cartesian conversion function, ${\mathit{J}}_g$ is its corresponding Jacobian and ⊕ is the compounding operator with Jacobians ${\mathit{J}}_{1\oplus }$ and ${\mathit{J}}_{2\oplus }$ as defined in [30]. With the point ${\mathit{p}}_i^{\times }$ referenced to the vehicle frame and ${\mathit{q}}_k$ being the vehicle position referenced to $\mathbb{I}$, we can calculate the position of a point ${\mathit{p}}_i$ as:

$${\widehat{\mathit{p}}}_i={\widehat{\mathit{q}}}_k\oplus {\widehat{\mathit{p}}}_i^{\times }$$

(17)

$${\mathit{P}}_{{\mathit{p}}_i}={\mathit{J}}_{1\oplus }{\mathit{P}}_{{\mathit{q}}_k}{\mathit{J}}_{1\oplus }^T+{\mathit{J}}_{2\oplus }{\mathit{P}}_{{\mathit{p}}_i^{\times }}{\mathit{J}}_{2\oplus }^T$$

(18)

After all these calculations, the information regarding the patch is saved in a new structure S as:

$$\mathit{S}=\left[\begin{array}{cccc}\mathit{x}& \mathit{O}& \mathit{W}& \mathit{B}\end{array}\right]$$

(19)

where $\mathit{x}=N(\widehat{\mathit{x}},{\mathit{P}}_x)$ is the position of the frame $\mathbb{I}$ that will be used to anchor the submap in the state vector of the pose-based SLAM framework described in Section 4; O is the same set of transformations as in ${\mathit{S}}_{temp}$; $\mathit{W}=\{{\mathit{p}}_{\mathbf{1}},...,{\mathit{p}}_n|{\mathit{p}}_i=N({\widehat{\mathit{p}}}_i,{\mathit{P}}_{{\mathit{p}}_i})\}$ is the set of points referenced to $\mathbb{I}$ that have been calculated and, finally, B is a volume containing all the points that pertain to the patch. On the horizontal plane, B is the polygon containing all the ${\widehat{\mathit{p}}}_i$ points, while, on the z direction, the boundary is defined by the minimum and maximum depth of all the points ${\widehat{\mathit{p}}}_i$ (see Figure 2).

3. Registration Algorithm

This section explains the procedure to register two submaps using probabilistic ICP. The inputs of the algorithm are a reference submap ${\mathit{S}}_{ref}$, which has been already stored in the SLAM framework; a newly generated submap ${\mathit{S}}_{new}$, and an initial guess of their relative displacement ${\mathit{q}}_{\mathbf{0}}=N(\widehat{{\mathit{q}}_0},{\mathit{P}}_{{\mathit{q}}_0})$ obtained from the navigation. The algorithm uses a two-stage correction procedure. First, a point-to-point correction is performed to roughly align the two submaps (until their relative displacement in two consecutive iterations falls below a threshold), and then, a point-to-plane correction is executed to refine the result. Point-to-point association tends to produce undesired effects in the presence of small misalignments (see for instance the lateral displacement depicted in Figure 3a). This is because associated points do not necessarily correspond to the exact same spot in the original surface and therefore their arbitrary occurrence may prevail over the general shape of the surface. However, point-to-point association is powerful when large displacements are present (see Figure 3b). On the other hand, point-to-plane associations tend to be driven by the shape of the surface and hence, perform better in the presence of small misalignments (see Figure 3a), but may fail when dealing with large displacements (see Figure 3b). To complement their strengths and weaknesses, we combine both methods by using an error threshold which determines when to switch from one strategy to the other.

3.1. Point-to-Point Association

Given a certain point ${\mathit{n}}_i=N({\widehat{\mathit{n}}}_i,{\mathit{P}}_{{\mathit{n}}_i})$ from the new patch ${\mathit{S}}_{new}$, and a matching candidate ${\mathit{r}}_j=N({\widehat{\mathit{r}}}_j,{\mathit{P}}_{{\mathit{r}}_j})$ from the reference surface ${\mathit{S}}_{ref}$, both represented in Cartesian coordinates and referenced to their respective frames (${\mathbb{I}}_{new}$ and ${\mathbb{I}}_{ref}$), the association error ${\mathit{e}}_{ij}=N({\widehat{\mathit{e}}}_{ij},{\mathit{P}}_{{\mathit{e}}_{ij}})$ can be defined as:

$${\widehat{\mathit{e}}}_{ij}=\widehat{{\mathit{r}}_j}-\widehat{{\mathit{q}}_0}\oplus {\widehat{\mathit{n}}}_i$$

(20)

$${\mathit{P}}_{ij}={\mathit{P}}_{{\mathit{r}}_j}+{\mathit{J}}_{1\oplus }{\mathit{P}}_{{\mathit{q}}_0}{\mathit{J}}_{1\oplus }^T+{\mathit{J}}_{2\oplus }{\mathit{P}}_{{\mathit{n}}_i}{\mathit{J}}_{2\oplus }^T$$

(21)

so the point-to-point association may be solved through a simple individual compatibility test over the corresponding Mahalanobis distance:

$$\begin{array}{c}\hfill d^{\mathbf{2}}={\widehat{\mathit{e}}}_{ij}^T{\mathit{P}}_{ij}^{-1}{\widehat{\mathit{e}}}_{ij}<{\chi }_{d,\alpha }^2\end{array}$$

(22)

All the points individually compatible with ${\mathit{n}}_i$ form the set ${\mathit{A}}_i$. From this set, the one with smaller Mahalanobis distance is chosen to be associated with ${\mathit{n}}_i$.

3.2. Point-to-Plane Association

At the second stage, the metric changes and the point-to-plane distance is used instead. Now, the set of compatible points ${\mathit{A}}_i$ is used to estimate a local plane $\Pi ({\mathit{\nu }}_i,d_i)$ whose equations are given by ${\mathit{\nu }}_i^T\mathit{x}-d_i=0$, being $d_i$ the plane distance to the origin and ${\mathit{\nu }}_i$ its normal vector. Because of the probabilistic nature of our algorithm, we are interested not only in the plane parameters but also in their uncertainty. An iterative method is reported in [31] for this purpose, being too computationally expensive for our case. In [32], the authors use a two-step minimization method for estimating: (1) the plane using region growing algorithms and (2) its uncertainty. Finally, in [33], the error of a set of samples is minimized using the uncertainty related to the range of the sensor by means of a weighted Principal Component Analysis (PCA). This last method is the one which best fits our requirements because of its reduced computational complexity, and also because of its nature, since it does not search for the points forming the plane, but fits the plane among the given points.

Given a plane $\Pi (\mathit{\nu },d)$, whose equation is ${\mathit{\nu }}^T\mathit{x}=d$, the likelihood of observing a plane point ${\mathit{r}}_j\in {\mathit{A}}_i$ is given by:

$$p\left({\mathit{r}}_j\right|\mathit{\nu },d)=\frac{1}{\sqrt{2\pi }\left|{\mathit{P}}_{{\mathit{r}}_j}\right|}\exp \left(({\mathit{\nu }}^T{\mathit{r}}_j-d)*\mathit{\nu }{)}^T{\mathit{P}}_{{\mathit{r}}_j}^{-1}((\mathit{\nu }{\mathit{r}}_j-d)*\mathit{\nu }\right)$$

(23)

The objective here is to maximize the sum of the log-likelihood of the previous equation. The problem cannot be solved in a simple way since the error of the uncertainty depends twice on the normal $\mathit{\nu }$. To solve the problem in an efficient way, it was necessary to approximate the uncertainty by the trace of ${\mathit{P}}_{{\mathit{r}}_j}$: $\mathrm{Tr}\left({\mathit{P}}_{{\mathit{r}}_j}\right)$. In this way, the error ellipsoid is approximated to a sphere, and it is possible to solve the equation analytically and as efficiently as in [33] (please refer to this work for a more extended derivation).

The log-likelihood that we want to maximize, ignoring constants, is the approximate least squares problem:

$$\ell =\underset{\mathit{\nu },d}{\mathrm{argmax}}-\frac{1}{2}\sum _{i=1}^N\frac{{({\mathit{\nu }}^T{\mathit{r}}_j-d)}^2}{\mathrm{Tr}{\left({\mathit{P}}_{{\mathit{r}}_j}\right)}^2}$$

(24)

with Lagrangian

$$\mathcal{L}=-\frac{1}{2}\sum _{i=1}^N\frac{{({\mathit{\nu }}^T{\mathit{r}}_j-d)}^2}{\mathrm{Tr}{\left({\mathit{P}}_{{\mathit{r}}_j}\right)}^2}-\lambda ({\mathit{\nu }}^T\mathit{\nu }-1)$$

(25)

Setting $\frac{\partial \mathcal{L}}{\partial d}=0$, we find the solution

$$d_{\star }={\mathit{\nu }}_{\star }^T{\mathit{p}}_{\mu },\mathrm{with}{\mathit{p}}_{\mu }=\left(\sum _{i=1}^N\mathrm{Tr}{\left({\mathit{P}}_{{\mathit{r}}_j}\right)}^{-2}{\mathit{r}}_j\right){\left(\sum _{i=1}^N\mathrm{Tr}{\left({\mathit{P}}_{{\mathit{r}}_j}\right)}^{-2}\right)}^{-1}$$

(26)

${\mathit{p}}_{\mu }$ being the weighted center of the set of points ${\mathit{A}}_i$. Finally,

$${\mathit{\nu }}_{\star }=\underset{\mathit{\nu }}{\mathrm{argmin}}{\mathit{\nu }}^T\left(\sum _{i=1}^N\frac{({\mathit{r}}_j-{\mathit{p}}_{\mu }){({\mathit{r}}_j-{\mathit{p}}_{\mu })}^T}{\mathrm{Tr}{\left({\mathit{P}}_{{\mathit{r}}_j}\right)}^2}\right)\mathit{\nu }$$

(27)

The minimizing normal ${\mathit{\nu }}_{\star }$ is defined by the eigenvalues of the covariance matrix of the points as in the common weighted PCA method. The uncertainty of the estimator is found as:

$${\mathit{P}}_f=-{\mathit{H}}^{+}=\left(\begin{array}{cc}{\mathit{P}}_{\mathit{\nu }}& {\mathit{P}}_{\mathit{\nu }}{\mathit{P}}_d\\ {\mathit{P}}_d{\mathit{P}}_{\mathit{\nu }}& {\mathit{P}}_d,\end{array}\right)$$

(28)

where H is the Hessian of the Lagrangian in the optimal plane.

Given the point ${\mathit{n}}_i$ and the plane ${\Pi }_i({\mathit{\nu }}_i,d_i)$ estimated from all the compatible points in ${\mathit{A}}_i$, the point ${\mathit{a}}_i$ is defined as the orthogonal projection of ${\mathit{n}}_i$ over the plane ${\Pi }_i({\mathit{\nu }}_i,d_i)$:

$$\widehat{{\mathit{a}}_i}={\widehat{\mathit{q}}}_{\mathbf{0}}\oplus {\widehat{\mathit{n}}}_i-({({\widehat{\mathit{q}}}_{\mathbf{0}}\oplus {\widehat{\mathit{n}}}_i)}^T{\widehat{\mathit{\nu }}}_i-{\widehat{d}}_i){\widehat{\mathit{\nu }}}_i$$

(29)

$$P_{{\mathit{a}}_i}=\frac{\partial {\mathit{a}}_i}{\partial {\mathit{q}}_0}{\mathit{P}}_{{\mathit{q}}_0}{\frac{\partial {\mathit{a}}_i}{\partial {\mathit{q}}_0}}^T+\frac{\partial {\mathit{a}}_i}{\partial {\mathit{n}}_i}{\mathit{P}}_{{\mathit{n}}_i}{\frac{\partial {\mathit{a}}_i}{\partial {\mathit{n}}_i}}^T+\frac{\partial {\mathit{a}}_i}{\partial {\mathit{\nu }}_i}{\mathit{P}}_{{\mathit{\nu }}_i}{\frac{\partial {\mathit{a}}_i}{\partial {\mathit{\nu }}_i}}^T+\frac{\partial {\mathit{a}}_i}{\partial d_i}{\mathit{P}}_{d_i}{\frac{\partial {\mathit{a}}_i}{\partial d_i}}^T$$

(30)

This new virtual point ${\mathit{a}}_i$ is actually the point that will be associated with ${\mathit{n}}_i$ to execute the new registration phase using the same point-to-point equations we already presented in Equations (20) and (21), but using ${\mathit{a}}_i$ instead of ${\mathit{r}}_j$.

3.3. Minimization

At the end of each association stage, a minimization process is executed to estimate the robot displacement ${\mathit{q}}_{\min }$ that minimizes the addition of the Mahalanobis distance of the association error:

$$\begin{array}{c}\hfill {\mathit{q}}_{\min }=\underset{\mathit{q}}{\mathrm{argmin}}\sum \left\{\xi {\mathit{P}}_{\xi }^{-1}\xi \right\}\end{array}$$

(31)

$\xi$ being a vector composed of all the ${\widehat{\mathit{e}}}_{ij}$ error vectors (see Equation (20)) after associating all the points (either virtual or real) and ${\mathit{P}}_{\xi }$ the block diagonal matrix with their corresponding covariances ${\mathit{P}}_{{\mathit{e}}_{ij}}$ (Equation (21)). This minimization is done using weighted least squares:

$$\begin{array}{c}\hfill {\mathit{q}}_{\min }={[{\mathit{J}}^T{\mathit{P}}_{\xi }^{-1}\mathit{J}]}^{-1}{\mathit{J}}^T{\mathit{P}}_{\xi }^{-1}\xi \end{array}$$

(32)

J being the Jacobian matrix of the error function at the previous estimation evaluated in all the points.

3.4. Submap Simplification

Traditional ICP-based methods may encounter some problems in a scenario like the one depicted in Figure 4a, where two almost flat surfaces share a poorly visible feature. For instance, ICP tends to associate each point with its closest neighbour according to a particular metric. Because of that, it may be difficult to correctly associate the feature areas when the displacement is large (i.e., they are far from each other, and the proximity of flat areas may lead to a local minimum). This particular issue will benefit from the proposed probabilistic ICP approach, since the uncertainty of the points should constrain the possible matching candidates to those compatible with the real accumulated error. For instance, uncertainties may be large in the horizontal plane, making it possible to match two distant features, but small in the z direction, so points in the flat areas will not be compatible with those in the features.

Another inconvenience related to traditional ICP-based methods is the weak contribution of flat areas to the registration (they all look alike and their matching possibilities are high). Moreover, when few features are present in scenarios of large flat areas, the planar areas may prevail and lead to poor matching. ICP algorithms have better results when the associated points are significant (i.e., distinguishable from each other). For that reason, a new sampling procedure to reduce the number of points in the cloud by removing the less informative ones is presented (see Figure 4b). Since the surface distribution is not available, the sampling procedure is performed using the discrete points. This resampling improves the odds of successful matchings, even when large displacements are present, as well as decreasing the computation time in the registration by drastically decreasing the number of points to be associated, thus increasing the performance of the algorithm.

The approach proposed here uses an octree structure to sample the scan in its most significant areas (i.e., areas with rich relief). The subsampling algorithm works as follows: the point cloud is contained in a discretized tridimensional space structured as an octree. Using the points contained in each cell of the octree, a relief-based subsampling criteria is evaluated recursively, and if the condition is fulfilled, the cell is divided into eight subcells. After the subdivision process comes to an end, only one point is taken from each cell of the octree (see Figure 5). This makes areas with bigger (i.e., not significant) cells contribute with fewer points than areas with smaller (i.e., significant) cells. In [34], several different criteria were studied to drive the octree subdivision. Although some criteria were more suitable for specific types of environments, in this work, the difference from principal plane method has been selected given its overall performance in both 2.5D and 3D. This criteria basically dictates that a cell should be divided if the average distance between the points in the cell, and the best fitting plane of the cloud is higher than a given threshold. For a more detailed description, please refer to [34].

3.5. Association in Linear Time

The association step in ICP based methods has an $O\left(n^2\right)$ computational cost because it is necessary to compare each reference point ${\mathit{r}}_j$ in ${\mathit{S}}_{ref}$ against all the ${\mathit{n}}_i$ points in ${\mathit{S}}_{new}$ to compute their distances. Moreover, the probabilistic implementation of the ICP method requires several matrix operations, including an inversion, to calculate the uncertainty of the association of the points from the two point clouds. Hereafter, a new method for reducing complexity taking advantage of the uncertainty estimates of the points, which are already available, is proposed.

A probabilistic point p with uncertainty P can be represented graphically as an ellipsoid defined by a ${\chi }^2$ distribution at a certain confidence level α and for d degrees of freedom (DoF):

$$\left\{\left(\mathit{x}\right|{(\mathit{x}-\mathit{p})}^T{\mathit{P}}^{-1}(\mathit{x}-\mathit{p})={\chi }_{d,\alpha }^2\right\}$$

(33)

Given that, in our approach, a 3D grid is generated covering the two patches to be matched, and for each point in ${\mathit{S}}_{ref}$, the cells falling inside its uncertainty ellipse are marked (see Figure 6a). During the association process, the same procedure is followed for each point in ${\mathit{S}}_{new}$.

At this point, the following heuristic is applied: To check the compatibility of two points ${\mathit{n}}_i$ and ${\mathit{r}}_j$, we define their ellipsoids given a certain confidence level α. If the ellipsoids do not intersect with each other, the corresponding points are assumed not to be individually compatible. In other words, if:

$$\begin{array}{c}\hfill \left\{\left(\mathit{x}\right|{(\mathit{x}-{\mathit{n}}_i)}^T{\mathit{P}}_{{\mathit{n}}_i}^{-1}(\mathit{x}-{\mathit{n}}_i)={\chi }_{d,\alpha }^2\right\}\cap \\ \hfill \left\{\left(\mathit{x}\right|{(\mathit{x}-{\mathit{r}}_j)}^T{\mathit{P}}_{{\mathit{r}}_j}^{-1}(\mathit{x}-{\mathit{r}}_j)={\chi }_{d,\alpha }^2\right\}=\varnothing \end{array}$$

(34)

then,

$$({\mathit{n}}_i-{\mathit{r}}_j){({\mathit{P}}_{{\mathit{n}}_i}+{\mathit{P}}_{{\mathit{r}}_j})}^{-1}{({\mathit{n}}_i-{\mathit{r}}_j)}^T>{\chi }_{d,\alpha }^2$$

(35)

Note that evaluating the compatibility in this way is still computationally expensive. However, in our method, the space occupied by the ellipses has been previously registered inside a grid, so it is possible to rapidly find the intersecting ellipsoids using a direct grid look-up (see Figure 6b). In other words, association candidates for a given point in the new scan can be easily identified by searching only among the cells occupied by its own ellipse, for tags denoting occupancy of those same cells by ellipses corresponding to points in the reference scan. In that way, candidates are directly determined, without a need to evaluate all the remaining points in the reference scan, and thus, the complexity is reduced to $O\left(n\right)$.

4. SLAM Algorithm

This section describes the EKF implementation of the pose-based SLAM framework in charge of optimizing the surface map.

Every time a submap is finished, the estimate of the robot pose at the reference point of each surface patch $x_S$ is incorporated to the state vector x so it contains all the information regarding the submap distribution:

$${\widehat{\mathit{x}}}_k={\left[{\widehat{\mathit{x}}}_{{\mathit{S}}_n}\cdots {\widehat{\mathit{x}}}_{{\mathit{S}}_1}\right]}_k^T$$

(36)

with a pose state ${\mathit{x}}_{\mathit{S}}$ being:

$${\mathit{x}}_{\mathit{S}}={\left[xyz\varphi \theta \psi \right]}^T$$

(37)

where (x,y,z) is the position of the robot and $(\varphi ,\theta ,\psi )$ are the roll, pitch and yaw angles. The poses are referred to the same common frame B that was used during the dead reckoning. The covariance matrix for this state is defined as:

$${\mathit{P}}_k=E\left([{\mathit{x}}_k-{\widehat{\mathit{x}}}_k]{[{\mathit{x}}_k-{\widehat{\mathit{x}}}_k]}^T\right)$$

(38)

4.1. Prediction and State Augmentation

The submap poses stored in the state vector are assumed to be static during the execution of the mission. Therefore, the prediction stage of the EKF just maintains the estimated values for the state vector and its covariance. However, every time a new patch is completed, and its pose is introduced in the state vector. This is done during the prediction stage. To be able to fit the requirements of this algorithm (such as the location of the frame $\mathbb{I}$), a new procedure has been developed for the prediction and state augmentation. The procedure explained hereafter uses the previously computed ${\mathit{o}}_k$ to find the relationship between the patch ${\mathit{S}}_n$ and ${\mathit{S}}_{n+1}$ by adding all the incremental displacements in the $xy$ plane and copying the position of the other 4 DoF at the position chosen as frame $\mathbb{I}$ in patch ${\mathit{S}}_{n+1}$.

Let ${\mathit{S}}_{n+1}$ be the new patch to be added to the state vector and ${\mathit{S}}_n$ the last one already added. Then, we need to estimate the transformation ${}^n{\mathit{q}}_{n+1}=N{(}^n{\widehat{\mathit{q}}}_{n+1},{\mathit{P}}_{{}^n{\mathit{q}}_{n+1}})$ relating ${\mathit{S}}_n$ and ${\mathit{S}}_{n+1}$. The process begins by defining two functions that will be applied to the set of stored ${\mathit{o}}_k$ relationships between the two patches:

$$f_{\mathbf{1}}\left(\widehat{\mathit{o}}\right)=f_1\left(\left[\begin{array}{cccccc}x,& y,& z,& \varphi ,& \theta ,& \psi \end{array}\right]\right)=\left[\begin{array}{cccccc}x,& y,& 0,& 0& 0,& 0\end{array}\right]$$

(39)

$$f_{\mathbf{2}}\left(\widehat{\mathit{o}}\right)=f_2\left(\left[\begin{array}{cccccc}x,& y,& z,& \varphi ,& \theta ,& \psi \end{array}\right]\right)=\left[\begin{array}{cccccc}0,& 0,& z,& \varphi ,& \theta ,& \psi \end{array}\right]$$

(40)

with Jacobians:

$${\mathit{F}}_1=\left[\begin{array}{cc}{\mathit{I}}_{2\times 2}& {\mathbf{0}}_{2\times 4}\\ {\mathbf{0}}_{4\times 2}& {\mathbf{0}}_{4\times 4}\end{array}\right],{\mathit{F}}_2=\left[\begin{array}{cc}{\mathbf{0}}_{2\times 2}& {\mathbf{0}}_{2\times 4}\\ {\mathbf{0}}_{4\times 2}& {\mathit{I}}_{4\times 4}\end{array}\right]$$

(41)

Then, taking the stored $\mathit{O}=\{{\mathit{o}}_{\mathbf{1}},...,{\mathit{o}}_m\}\in {\mathit{S}}_n$, the parameter ${\mathit{q}}_{\mathbf{1}}=N(\widehat{{\mathit{q}}_1},{\mathit{P}}_{{\mathit{q}}_1})$ representing the distance from the central position of the ${\mathit{S}}_n$ patch (defined with the subindex $mp$) and its last position can be calculated as:

$${\widehat{\mathit{q}}}_1=\sum _{k=mp+1}^m{f}_{\mathbf{1}}\left({\widehat{\mathit{o}}}_k\right),{\mathit{P}}_{{\mathit{q}}_1}=\sum _{k=mp+1}^m{\mathit{F}}_{\mathbf{1}}{\mathit{P}}_{{\mathit{o}}_k}{\mathit{F}}_1^T$$

(42)

Next, using the stored $\mathit{O}\in {\mathit{S}}_{n+1}$, the parameter ${\mathit{q}}_{\mathbf{2}}=N(\widehat{{\mathit{q}}_2},{\mathit{P}}_{{\mathit{q}}_2})$ representing the distance from the beginning of ${\mathit{S}}_{n+1}$ to its center plus the final orientation of the patch can be obtained as:

$${\widehat{\mathit{q}}}_2=f_{\mathbf{2}}\left({\widehat{\mathit{o}}}_{mp}\right)+\sum _{k=1}^{mp}{f}_{\mathbf{1}}\left({\widehat{\mathit{o}}}_k\right),{\mathit{P}}_{{\mathit{q}}_2}={\mathit{F}}_2{\mathit{P}}_{{\mathit{o}}_{mp}}{\mathit{F}}_2^T+\sum _{k=1}^{mp}{\mathit{F}}_1{\mathit{P}}_{{\mathit{o}}_k}{\mathit{F}}_1^T$$

(43)

Finally, the complete transformation ${}^n{\mathit{q}}_{n+1}$ relating the centers of both patches is calculated as:

$${}^n{\widehat{\mathit{q}}}_{n+1}={\widehat{\mathit{q}}}_{\mathbf{1}}+{\widehat{\mathit{q}}}_{\mathbf{2}},{\mathit{P}}_{{}^n{\widehat{\mathit{q}}}_{n+1}}={\mathit{P}}_{q_1}+{\mathit{P}}_{q_2}$$

(44)

Knowing this ${}^n{\mathit{q}}_{n+1}$ transformation, the state of the filter can be augmented with the new position of ${\mathit{S}}_{n+1}$ by doing:

$${\widehat{\mathit{x}}}_k^{+}={\left[\begin{array}{ccccc}{\widehat{\mathit{x}}}_{{\mathit{S}}_n}{\odot }^n{\widehat{\mathit{q}}}_{n+1}& {\widehat{\mathit{x}}}_{{\mathit{S}}_n}& {\widehat{\mathit{x}}}_{{\mathit{S}}_{n-1}}& \cdots & {\widehat{\mathit{x}}}_{{\mathit{S}}_1}\end{array}\right]}_k^T$$

(45)

$${\mathit{P}}_{{\widehat{\mathit{x}}}_k^{+}}={\mathit{J}}_{1\odot }{\mathit{P}}_{{\widehat{\mathit{x}}}_k}{\mathit{J}}_{1\odot }^T+{\mathit{J}}_{2\odot }{\mathit{P}}_{{}^n{\widehat{\mathit{q}}}_{n+1}}{\mathit{J}}_{2\odot }^T$$

(46)

Note that the ⊙ operator is introduced here to define the way in which the global coordinates of the ${\mathit{S}}_n$ patch are combined with the relationship between consecutive patches ${\mathit{S}}_n$ and ${\mathit{S}}_{n+1}$ to find the position of the patch ${\mathit{S}}_{n+1}$ in the world frame. The ⊙ operator is described as:

$${\widehat{\mathit{x}}}_{{\mathit{S}}_n}{\odot }^n{\widehat{\mathit{q}}}_{n+1}=\left[\begin{array}{c}{\mathit{x}}_{{\mathit{x}}_{{\mathit{S}}_n}}+{\mathit{x}}_{{}^n{\widehat{\mathit{q}}}_{n+1}}\\ {\mathit{y}}_{{\mathit{x}}_{{\mathit{S}}_n}}+{\mathit{y}}_{{}^n{\widehat{\mathit{q}}}_{n+1}}\\ z_{{}^n{\widehat{\mathit{q}}}_{n+1}}\\ {\varphi }_{{}^n{\widehat{\mathit{q}}}_{n+1}}\\ {\theta }_{{}^n{\widehat{\mathit{q}}}_{n+1}}\\ {\psi }_{{}^n{\widehat{\mathit{q}}}_{n+1}}\end{array}\right]$$

(47)

with Jacobians:

$${\mathit{J}}_{1\odot }=\left[\begin{array}{c}\begin{array}{cc}{\mathit{I}}_{2\times 2}& {\mathbf{0}}_{2\times (4+6(n-1\left)\right)}\\ {\mathbf{0}}_{4\times 2}& {\mathbf{0}}_{4\times (4+6(n-1\left)\right)}\end{array}\\ {\mathit{I}}_{6n\times 6n}\end{array}\right],{\mathit{J}}_{2\odot }=\left[\begin{array}{c}{\mathit{I}}_{6\times 6}\\ {\mathbf{0}}_{6n\times 6}\end{array}\right]$$

(48)

4.2. Matching Strategy

When a new patch is available, potential matches are searched among the previously created patches. This is done by determining the intersection between the volumes B (see Equation (19)) of the two potentially matching patches. In this way, two patches are considered to be intersecting if more than a given % of their volumes is shared. The new patch may potentially intersect with several of the patches that already exist, which may or may not be consecutive in time (see the ones overlapping with patch number 13 in Figure 7). As can also be observed, consecutive patches (such as number 1 and 2, or 8 and 9) may have a small overlap with the new patch. For this reason, a new approach is used that consists in joining consecutive patches to maximize the intersecting area. However, this is not recommended for contiguous non-consecutive patches since the drift between them might be significant (e.g., patches number 1 and 6). The proposed approach involves three steps: (1) search for patches intersecting with the new one; (2) search for consecutive patches among those previously selected; and (3) join the patches that are found to be consecutive. The resulting patches are the result of combining the points of the two surfaces and representing them in the frame $\mathbb{I}$ of the earliest created patch.

4.3. Scan Matching

In order to execute the probabilistic registration algorithm, given two overlapping scans ${\mathit{S}}_i$ and ${\mathit{S}}_n$ with related poses ${\widehat{\mathit{x}}}_{S_i}$ and ${\widehat{\mathit{x}}}_{S_n}$, an initial guess ${\mathit{q}}_{\mathbf{0}}=N({\widehat{\mathit{q}}}_{\mathbf{0}},{\mathit{P}}_{{\mathit{q}}_0})$ of their relative displacement is necessary. This can be easily extracted from the state vector using the tail-to-tail transformation:

$${\widehat{\mathit{q}}}_{\mathbf{0}}=\ominus {\widehat{\mathit{x}}}_{{\mathit{S}}_i}\oplus {\widehat{\mathit{x}}}_{{\mathit{S}}_n}$$

(49)

where ⊕ and ⊖ are the compounding and inverse compounding operators as defined in [30]. Since the tail-to-tail transformation is actually a non-linear function of the state vector ${\widehat{\mathit{x}}}_k$, the uncertainty of the initial guess can be computed using:

$${\mathit{P}}_{{\mathit{q}}_0}=\mathit{H}{\mathit{P}}_{{\mathit{x}}_k}{\mathit{H}}^T$$

(50)

where H is the Jacobian computed as:

$$\mathit{H}=\left[{\mathit{J}}_{2{\oplus }_{6\times 6}}{\mathbf{0}}_{6\times 6(n-i-1)}{\mathit{J}}_{1\oplus }{\mathit{J}}_{{\ominus }_{6\times 6}}{\mathbf{0}}_{6\times 6(i-1)}\right]$$

(51)

where ${\mathit{J}}_{1\oplus }$, ${\mathit{J}}_{2\oplus }$ and $J_{\ominus }$ are the Jacobians of the compounding and inverse compounding functions as defined in [30]. Finally, ${\mathbf{0}}_{6\times 6(n-i-1)}$ and ${\mathbf{0}}_{6\times 6(i-1)}$ are zero matrices whose dimensions are determined according to the position in the state vector of the surfaces to be registered.

Once the initial displacement guess is available, the registration algorithm presented in Section 3 can be used to produce an updated measurement of this displacement.

4.4. State Update

The initial guess in Equation (49) defines the relationship between two patch poses in the state vector. This can be expressed by means of the following measurement model:

$${\mathit{z}}_k=h({\mathit{x}}_k,{\mathit{v}}_k)=\ominus {\mathit{x}}_{{\mathit{S}}_i}\oplus {\mathit{x}}_{{\mathit{S}}_n}+{\mathit{v}}_k$$

(52)

${\mathit{z}}_k$ being the estimated displacement ${\mathit{q}}_{\min }$ and ${\mathit{v}}_k$ a zero-mean white Gaussian noise with covariance ${\mathit{P}}_{{\mathit{q}}_{\min }}$ accounting for the errors in the registration process. Given that, the standard EKF equations can be used to update the state vector.

5. Experiments and Results

The algorithm has been used to produce the maps for two different underwater datasets. The first one is a bathymetric (2.5D) survey carried out by the Sirius AUV [35] on a site of geological interest off the coast of Tasmania (Australia) which has been previously used for bathymetric SLAM [22], while the second one is a full 3D dataset gathered in the Sant Feliu de Guíxols Harbor (Spain) using the Girona 500 AUV [36] with the multibeam mounted on a pan and tilt unit. The parameters and thresholds that were set for the execution of the algorithm in these experiments can be found in

Table 1. Thresholds used for the experiments.

	Experiment
	2.5D	3D
Minimum patch size (Section 2.2)	30 m	-
Maximum patch size (Section 2.2)	80 m	-
Normal occupancy (Section 2.2)	23%	-
Patch overlapping (Section 4.2)	30%	30%
Point cloud subsampling (Section 3.4)	0.5 m	1.5 m
Relative displacement to switch from point-to-point to point-to-plane association (Section 3)	1 cm	1 cm

. Unfortunately, none of the datasets used during the experimental testing have ground truth of the terrain. Therefore, the only option to assess the performance of the algorithm is evaluating the consistency of the resulting map.

5.1. Bathymetric Survey

This dataset includes depth from a pressure sensor, bottom lock velocities from a DVL, attitude measurements from an AHRS and bathymetric data from a multibeam echosounder installed in the conventional down-looking configuration. The mission surveyed a rectangular area of geological interest several times to generate multiple loop closures. The explored area, mainly flat, has a number of pockmarks with depths of approximately three meters.

Figure 8 shows the elevation maps built using the dead reckoning navigation (Figure 8a) as well as the one obtained with the proposed technique (Figure 8b). In these two maps, it is possible to observe several differences in the pockmarks. While in the corrected solution (Figure 8b), the pockmarks appear clearly and without much bathymetry-related artefacts on the dead reckoning map, and they are blurred and with some artefacts.

To better assess the correction, the consistency-based error [37] is computed for each cell of the bathymetric map. In Figure 9, it can be seen how the areas of high discrepancy (yellow to dark red) on the dead reckoning error map (Figure 9a) are drastically reduced when the proposed technique is applied (Figure 9b).

Table 2. Numerical results of the algorithm applied to the pockmarks dataset. The first column (Sum) contains the sum of the error in all the cells, the second one (Mean) contains the mean of the error while the 3rd one (#Cells) contains the number of cells occupied on a 3D grid of 0.5 m resolution.

	Sum	Mean	#Cells
Dead reckoning	70,986.2	0.3988	37,3121
SLAM	57,521.8	0.3223	36,5014
Improvement *	18.97%	19.2%	2.17%

* The improvement is computed as $\frac{dr-slam}{dr}$ where $dr$ stands for dead reckoning.

contains the numerical evaluation of the results over the bathymetric data. There, it is possible to see that using the 2.5D statistics (Sum and Mean for the consistency-based error) the improvement is around 19%. Moreover, an additional 3D statistic we have named #Cells has been computed. This statistic consists in counting the number of cells that each map occupies within the same 3D grid. If a map occupies less cells, it is probably because their point clouds are more densely packed due to a better registration. Using this statistic, the improvement of the proposed approach compared to the dead reckoning navigation is 2.17%.

5.2. 3D Experiments

The data was gathered during some field trials for the MORPH European project in 2015. The experiment involved a formation of four marine vehicles (an Autonomous Surface Craft (ASC) and 3 AUVs) exploring a submerged area of the St. Feliu harbor. The Girona 500 was leading the formation while exploring with the multibeam sonar mounted on a pan and tilt unit (Figure 10) both the seabed and the pier walls, so the formation could be adapted to the presence of obstacles. The mission performed one and a half loops following a zero-shaped trajectory at one corner of the harbor (Figure 11).

During the experiment, the Girona 500 (Figure 10) was equipped with a DVL, an AHRS, a pressure sensor and a multibeam echosounder. An acoustic modem on Girona 500 was also used to gather position measurements from a USBL mounted on the ASC navigating on the surface with help of a GPS receiver. The multibeam mounted on the pan and tilt unit allowed us to get full 3D scans by vertically steering the multibeam in front of the robot. Note that, in this experiment, the closure of the surface patches is determined by the completion of a sweep of the pan and tilt, and not by the size or richness of the covered area (see Table 1).

As far as we know, there is no general method to evaluate the consistency of a 3D map. However, it is possible to use the 3D statistic #Cells presented in the previous section. As previously commented, if a map occupies fewer cells, it is probably because their point clouds are better registered. Nonetheless, this has to be supervised since it is also possible to find other positions of the point clouds that can minimize the number of occupied cells. For this reason, the consistency of the 3D experiments will also be evaluated subjectively (visually assessing the consistency) as well as numerically (counting the number of occupied cells).

The top view of the 3D maps produced after the experiments are presented in Figure 12. There, three surfaces are created for different navigation methods: dead reckoning (Figure 12a), USBL-aided (Figure 12b) and the currently proposed algorithm (Figure 12c). Regarding the number of occupied cells, the proposed method occupies 32570 cells, 5.76% less than the dead reckoning model (34559) while the one aided by the USBL occupies 7.24% less cells (32057). Moreover, the black squares represented in each one of the views highlight the places where it is easier to observe the consistency of the map near to the harbor wall. This area is analyzed in detail in Figure 13. In the left column, the one corresponding to the dead reckoning navigation (views Figure 13a,d,g), clearly shows two parallel lines on the point clouds which correspond to the wall being observed during the first and second laps of the mission. In the other two columns, the one corresponding to the usbl navigation (views Figure 13b,e,h), and the one of the proposed SLAM algorithm (views Figure 13c,f,i) show a single wall, and, thus, a better agreement between the different scans. However, if the point cloud from the USBL navigation is analyzed in detail in the bottom left corner (see Figure 13h), there are still some residues of the two observations of the wall that do not appear in the one from the proposed approach.

6. Conclusions

This paper has presented a probabilistic underwater 3D SLAM for multibeam sonar data that deals with the subdivision of the surface into patches, taking into account the motion uncertainty during their formation. An adaptive sampling procedure for the sensor data has been introduced to deal with areas of the patches that are not relevant (i.e., without relief) to avoid the pICP converging to local minima as well as reducing the computational time. Furthermore, an heuristic has been used to decrease the complexity of the association step of the pICP from $O\left(n^2\right)$ to $O\left(n\right)$ taking advantage of the probabilistic ellipsoid of each point and using a support grid.

The algorithm has been tested using two real world datasets. In both of them, it is possible to observe how the consistency of the model obtained using the proposed algorithm is higher than that obtained with dead reckoning and is even comparable to the one obtained using USBL navigation in the case of the 3D dataset.

Future work will have to focus on correcting the internal patch error. In the method presented here, only the relative positions of the patches are corrected, but the patch itself is not modified once closed. Although the proposed method has been proved to be useful for obtaining consistent maps, it is not possible to use it online due to its computational cost if the point sampling is not tuned properly. Therefore, further investigation could be done in this field to allow the algorithm to work online. Finally, in the future, we plan to test the algorithm using a camera-laser system, which produces data of similar characteristics to that of a multibeam sonar (2D swath profiles) but with a much different uncertainty level in the measurements.