Scan matching SLAM in underwater environments

Abstract

This paper proposes a pose-based algorithm to solve the full simultaneous localization and mapping problem for autonomous underwater vehicle (AUV) navigating in unknown and possibly unstructured environments. The proposed method first estimates the local path traveled by the robot while forming the acoustic image (scan) with range data coming from a mono-beam rotating sonar head, providing position estimates for correcting the distortions that the vehicle motion produces in the scans. Then, consecutive scans are cross-registered under a probabilistic scan matching technique for estimating the displacements of the vehicle including the uncertainty of the scan matching result. Finally, an augmented state extended Kalman filter estimates and keeps the registered scans poses. No prior structural information or initial pose are considered. The viability of the proposed approach has been tested reconstructing the trajectory of a guided AUV operating along a 600 m path within a marina environment.

Appendix

1.1 Index to electronic supplementary material

Online resource	Media type	Description
1	Video mpeg1	SLAM algorithm results in the marina dataset

1.2 Closed-form formulation of the scan matching uncertainty

A closed-form formula for propagating the uncertainty from matched image pairs to homography parameters describing the image motion for a 2D image mosaic optimization problem, is presented in Elibol (2011). The formula is based on the first order approximation for the bundle adjustment (BA) minimization algorithm. Hereafter, this method was adapted to the estimation of the covariance matrix of the scan matching displacement estimate.

Let,

• $\mathbf{z} \equiv N\left( \hat{\mathbf{z}},\mathbf{P_z}\right)$, be the vector of the measured scan points assumed to be perturbed with a zero mean Gaussian random noise. In our case, the scan matching does point-to-point association, therefore the measurements vector $\hat{\mathbf{z}}$ is a dimension of $4k\times 1$:

  $$\begin{aligned} \hat{\mathbf{z}} = [\underbrace{a_{x_1},a_{y_1},n_{x_1},n_{y_1}}_{\hat{\mathbf{z}}_1} \cdots \underbrace{a_{x_k},a_{y_k},n_{x_k},n_{y_k}}_{\hat{\mathbf{z}}_k}]^T \end{aligned}$$

  (32)

  and its covariance, given by $\mathbf{P}_\mathbf{z}$, is a $(4k\times 4k)$ matrix consisted of the uncertainties of the scan points:

  $$\begin{aligned} \mathbf{P}_\mathbf{z} = \left[ \begin{array}{ccccccc} \mathbf{P}_{a_1} &{} &{} &{} \cdots &{} &{} &{} 0\\ &{} \mathbf{P}_{n_{1}} &{} &{} &{} &{} &{} \\ \vdots &{} &{} &{} \ddots &{} &{} &{} \vdots \\ &{} &{} &{} &{} &{} \mathbf{P}_{a_{k}} &{} \\ 0 &{} &{} &{} \cdots &{} &{} &{} \mathbf{P}_{n_{k}} \end{array} \right] \end{aligned}$$

  (33)

  being block diagonal since the scan points are assumed to be uncorrelated. As a difference with laser scanners or multi-beam sonar profilers, using a rotating mono-beam sonar head, the scan points become correlated when represented in the scan $I$ frame. Nevertheless, taking into account the short duration of the scan building process, the slow motion of the vehicle and without loss of generality, those correlations have been neglected in this work.

• $\mathbf{{x}}$ be the unknown parameters vector corresponding to the ${\mathbf{q}}_{min}$ estimated by the the pIC.

  $$\begin{aligned} \mathbf{{x}} = \left[ x, y, \psi \right] ^T \end{aligned}$$

  (34)

• $f(\mathbf{z},\mathbf{x})$ be an scalar, continuous, non-negative cost function of the overall square Mahalanobis distance of the matching error (from algorithm 1, line 13):

  $$\begin{aligned} f(\mathbf{z},\mathbf{x})= \frac{1}{2}\sum _{i=1}^{k} \left( \mathbf{e}_i^T \cdot \mathbf{P}^{-1}_{e_i} \cdot \mathbf{e}_i \right) \end{aligned}$$

  (35)

Then, we can apply Haralick’s method (Haralick 1998) in order to estimate the covariance $\mathbf{P}_\mathbf{x}$ of the estimated $\hat{\mathbf{x}}$ which minimizes the above cost function (35):

$$\begin{aligned} \mathbf{{P}}_{\mathbf{x}} = \left( \frac{\partial g}{\partial \mathbf{x}}\right) ^{-1}\cdot \frac{\partial g}{\partial \mathbf{z}}\cdot \mathbf{P}_\mathbf{z}\cdot \left( \frac{\partial g}{\partial \mathbf{z}}\right) ^{T}\cdot \left( \frac{\partial g}{\partial \mathbf{x}}\right) ^{-1} \end{aligned}$$

(36)

where $g\left( \mathbf{z},\mathbf{x} \right) = \left[ \frac{\partial f( \mathbf{z},\mathbf{x})}{\partial {\mathbf{x}}} \right] ^T$. To do this, it is necessary to compute $g\left( \mathbf{z},\mathbf{x} \right) ,\,\frac{\partial g(\mathbf{z},\mathbf{x})}{\partial \mathbf{x}}$ and $\frac{\partial g(\mathbf{z},\mathbf{x})}{\partial \mathbf{z}}$.

Let us begin rewriting the function (35) as:

$$\begin{aligned} f\left( \mathbf{z},\mathbf{x} \right) = \frac{1}{2}\cdot {\mathbf{R}^T}\cdot {\mathbf{W}}\cdot {\mathbf{R}} \end{aligned}$$

(37)

where $\mathbf{R}$ is the stacked vector of the measurement errors (of dimension $2k\times 1$):

$$\begin{aligned} {\mathbf{R}} = \left[ \mathbf{e}_{1}^T \cdots \mathbf{e}_{k}^T\right] ^T \end{aligned}$$

(38)

and $\mathbf{W}$ is the inverted block diagonal matrix of the measurement errors covariances (of dimension $2k\times 2k$):

$$\begin{aligned} \mathbf{W} = blockdiag\left( \mathbf{P}_{e_1} \cdots \mathbf{P}_{e_k} \right) ^{-1} \end{aligned}$$

(39)

Because $\mathbf{W}$ is the inverse of a covariance matrix, it is positive definite and hence a Cholesky decomposition exists:

$$\begin{aligned} \mathbf{W}=\mathbf{L}^T\cdot \mathbf{L} \end{aligned}$$

(40)

Now, for simplicity and without lost of generality, let us define:

$$\begin{aligned} \hat{\mathbf{R}}=\mathbf{L}\cdot \mathbf{R} \end{aligned}$$

(41)

$\mathbf{W}$ is block diagonal because the scan points are assumed to be uncorrelated, so as is $\mathbf{L}$:

$$\begin{aligned} \mathbf{L} = blockdiag\left( \mathbf{L}_{e_1} \cdots \mathbf{L}_{e_k} \right) \end{aligned}$$

(42)

Now, Eq. (37) can be rewritten as:

$$\begin{aligned} f\left( \mathbf{z}, \mathbf{x} \right) = \frac{1}{2} \cdot {\hat{\mathbf{R}}}^{T}\cdot \hat{\mathbf{R}} \end{aligned}$$

(43)

and $g(\mathbf{z}, \mathbf{x})$ can be defined as the Jacobian of the cost function, being a $(1\times 3)$ matrix;

$$\begin{aligned} g\left( \mathbf{z},\mathbf{x} \right) = \frac{\partial f}{\partial {x}} = \hat{\mathbf{R}}^{T}\cdot \hat{\mathbf{J}}_{x} \end{aligned}$$

(44)

where ${\hat{\mathbf{J}}}_{x}$ is the $(2k\times 3)$ Jacobian matrix of error vector $\hat{\mathbf{R}}$ :

$$\begin{aligned} {\hat{\mathbf{J}}}_{x}=\frac{\partial \hat{\mathbf{R}}}{\partial \mathbf{x}} = \left[ \begin{array}{c@{\quad }c@{\quad }c} \frac{\partial (\mathbf{L}_{e_1} \cdot \mathbf{e}_1)}{\partial x} &{} \frac{\partial (\mathbf{L}_{e_1} \cdot \mathbf{e}_1)}{\partial y} &{} \frac{\partial (\mathbf{L}_{e_1} \cdot \mathbf{e}_1)}{\partial \theta }\\ \vdots &{} \vdots &{} \vdots \\ \frac{\partial (\mathbf{L}_{e_k} \cdot \mathbf{e}_k)}{\partial x} &{} \frac{\partial (\mathbf{L}_{e_k} \cdot \mathbf{e}_k)}{\partial y} &{} \frac{\partial (\mathbf{L}_{e_k} \cdot \mathbf{e}_k)}{\partial \theta }\\ \end{array} \right] \end{aligned}$$

(45)

The $\frac{\partial g\left( \mathbf{z},\mathbf{x} \right) }{\partial \mathbf{x}}$ is the $(3\times 3)$ Hessian of $f\left( \mathbf{z},\mathbf{x} \right)$ and is calculated as follows:

$$\begin{aligned} \frac{\partial g}{\partial \mathbf{x}} = 2 \cdot \hat{\mathbf{J}}_{\mathbf{x}}^{T}\cdot \hat{\mathbf{J}}_{\mathbf{x}} + 2 \cdot \hat{\mathbf{R}}^{T}\frac{\partial \hat{\mathbf{J}}_{\mathbf{x}}}{\partial \mathbf{x}} \end{aligned}$$

(46)

where $\frac{\partial \hat{\mathbf{J}}_{\mathbf{x}}}{\partial \mathbf{x} }$ is the $(6k \times 3)$ Hessian of $\hat{\mathbf{R}}$ which can be computed in the following way:

$$\begin{aligned} \frac{\partial {\hat{\mathbf{J}}_\mathbf{x}}}{\partial \mathbf{x}} = \frac{\partial }{\partial \mathbf{x}} \left( \frac{\partial \hat{\mathbf{R}}}{\partial \mathbf{x}}\right) = \sum _{i=1}^3 \left( vec \left( \frac{\partial \hat{\mathbf{J}}_{\mathbf{x}}}{\partial {x}_i}\right) \right) \cdot \mathbf{r}^T_i \end{aligned}$$

(47)

being $\mathbf{r}_i$ a $(3 \times 1)$ vector, with all zeros except its $i^{th}$ row which is equal to $1$. To compute the second part of (46), the (47) is multiplied by $\hat{\mathbf{R}}^{T}$ as follows:

$$\begin{aligned} \hat{\mathbf{R}}^{T}\frac{\partial {\hat{\mathbf{J}}_{\mathbf{x}}} }{\partial \mathbf{x}} = \left( \hat{\mathbf{R}}^{T}\otimes \mathbf{I}_{3}\right) \cdot \frac{\partial {\hat{\mathbf{J}}_{\mathbf{x}}}}{\partial \mathbf{x}} \end{aligned}$$

(48)

where $\otimes$ denotes Kronecker product of two matrices. Similarly, the $\frac{\partial g}{\partial \mathbf{z}}$ is a $3 \times 4k$ matrix, computed as:

$$\begin{aligned} \frac{\partial g}{\partial \mathbf{z}} = 2 \cdot \hat{\mathbf{J}}_{\mathbf{x}}^{T}\cdot \hat{\mathbf{J}}_\mathbf{z} + 2 \cdot \hat{\mathbf{R}}^{T}\frac{\partial \hat{\mathbf{J}}_{\mathbf{x}}}{\partial \mathbf{z}} \end{aligned}$$

(49)

where ${\hat{\mathbf{J}}}_{z}$ is the Jacobian of the error vector $\hat{\mathbf{R}}$, being a $(2k\times 4k)$ matrix:

$$\begin{aligned} {\hat{\mathbf{J}}}_{z} = \frac{\partial \hat{\mathbf{R}}}{\partial \mathbf{z}} = \left[ \begin{array}{c@{\quad }c@{\quad }c} \frac{\partial \hat{\mathbf{R}}_1}{\partial \mathbf{z}} &{} \cdots &{} 0\\ \vdots &{} \ddots &{} \vdots \\ 0 &{} \cdots &{} \frac{\partial \hat{\mathbf{R}}_k}{\partial \mathbf{z}} \end{array} \right] \end{aligned}$$

(50)

where each $\frac{\partial \hat{\mathbf{R}}(i)}{\partial \mathbf{z}}$ is a $(2\times 4)$ matrix, and $\frac{\partial \hat{\mathbf{J}}_{\mathbf{x}}}{\partial \mathbf{z}}$ is the following $(6k \times 4k)$ matrix:

$$\begin{aligned} \frac{\partial {\hat{\mathbf{J}}_{\mathbf{x}}}}{\partial \mathbf{z}} = \sum _{i=1}^{4k} \left( vec \left( \frac{\partial {\hat{\mathbf{J}}_{\mathbf{x}}}}{\partial \mathbf{z}_{i}}\right) \right) \cdot \mathbf{r}_{i}^{T} \end{aligned}$$

(51)

being this time $r_i$ a $(4k\times 1)$ vector of all zeros except its $i_{th}$ row which is equal to $1$. As previously, the second part of (49) is given by:

$$\begin{aligned} \hat{\mathbf{R}}^{T}\frac{\partial }{\partial \mathbf{z}} \left( \frac{\partial \hat{\mathbf{R}}}{\partial \mathbf{x}} \right) = \left( \hat{\mathbf{R}}^{T} \otimes \mathbf{I}_{3}\right) \cdot \frac{\partial {\hat{\mathbf{J}}_{\mathbf{x}}}}{\partial \mathbf{z}} \end{aligned}$$

(52)