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.
This is a preview of subscription content, log in via an institution to check access.
Similar content being viewed by others
Explore related subjects
Discover the latest articles, news and stories from top researchers in related subjects.References
Bailey, T., & Durrant-Whyte, H. F. (2006). Simultaneous localization and mapping (SLAM): Part II, state of the art. IEEE Robotics and Automation Magazine, 13(3), 108–117.
Balsamo, A., Mana, G., & Pennecchi, F. (2006). The expression of uncertainty in non-linear parameter estimation. Metrologia 43(5):396–402. http://stacks.iop.org/0026-1394/43/i=5/a=009.
Barclay, L. (2003). Propagation of radiowaves. London: The Institution of Engineering and Technology.
Barkby, S., Williams, S., Pizarro, O., & Jakuba, M. (2009). An efficient approach to bathymetric SLAM. In IEEE/RSJ international conference on intelligent robots and systems, IROS 2009 (pp. 219–224). doi:10.1109/IROS.2009.5354248.
Bar-Shalom, Y., & Fortmann, T. E. (1988). Tracking and data association. New York: Academic Press.
Bengtsson, O., & Baerveldt, A. J. (2003). Robot localization based on scan-matching—Estimating the covariance matrix for the IDC algorithm. Robotics and Autonomous Systems, 44(1), 29–40. doi:10.1016/S0921-8890(03)00008-3, best Papers of the Eurobot ’01 Workshop.
Bülow, H., & Birk, A. (2011). Spectral registration of noisy sonar data for underwater 3d mapping. Autonomous Robots, 30, 307–331. doi:10.1007/s10514-011-9221-8.
Bülow, H., Pfingsthorn, M., Birk, A. (2010). Using robust spectral registration for scan matching of sonar range data. In 7th symposium on intelligent autonomous vehicles (IAV) IFAC.
Burguera, A., Oliver, G., & González, Y. (2010). Scan-based SLAM with trajectory correction in underwater environments. In IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 2546–2551). doi:10.1109/IROS.2010.5649492.
Burguera, A., González, Y., & Oliver, G. (2008). A probabilistic framework for sonar scan matching localization. Advanced Robotics, 22(11), 1223–1241.
Carpenter, R. N. (1998). Concurrent mapping and localization with FLS. In Workshop on autonomous underwater vehicles, Cambridge, MA, USA, pp. 133–148. doi:10.1109/AUV.1998.744449.
Castellani, U., Fusiello, A., Murino, V., Papaleo, L., Puppo, E., Repetto, S., & Pittore, M. (2004). Efficient on-line mosaicing from 3D acoustical images. OCEANS’04 MTTS/IEEE TECHNO-OCEAN’04 (Vol. 2, pp. 670–677).
Censi, A. (2007). An accurate closed-form estimate of ICP’s covariance. In Proceedings of the IEEE international conference on robotics and automation (ICRA) (pp. 3167–3172). Rome, Italy. doi:10.1109/ROBOT.2007.363961, http://purl.org/censi/2006/icpcov.
Elibol, A. (2011). Efficient topology estimation for large scale optical mapping. Ph.D. thesis, University of Girona, Doctoral Programme in Technology.
Elibol, A., Gracias, N., & Garcia, R. (2010). Augmented state-extended Kalman filter combined framework for topology estimation in large-area underwater mapping. Journal of Field Robotics, 27(5), 656–674. doi:10.1002/rob.20357.
Estrada, C., Neira, J., & Tardós, J. D. (2005). Hierarchical SLAM: Real-time accurate mapping of large environments. IEEE Transactions on Robotics, 21, 588–596.
Eustice, R., Singh, H., Leonard, J., Walter, M., & Ballard, R. (2005). Visually navigating the RMS Titanic with SLAM information filters. In Proceedings of robotics: Science and systems (pp, 57–64). Cambridge, MA: MIT Press.
Fairfield, N., Kantor, G., & Wettergreen, D. (2007). Real-time SLAM with octree evidence grids for exploration in underwater tunnels. Journal of Field Robotics, 24(1), 2.
Ferrer, J., Elibol, A., Delaunoy, O., Gracias, N., & Garcia, R. (2007). Large-area photo-mosaics using global alignment and navigation data. In Proceedings of the Oceans MTS/IEEE. Vancouber, Canada.
Foley, B., Sakellariou, D., Dellaporta, K., Bingham, B., Camilli, R., Eustice, R., et al. (2009). New methods for underwater archaeology: The 2005 Chios ancient shipwreck survey. HESPERIA—Journal of the American Archaeological School at Athens, 78(2), 269–305. doi:10.2972/hesp.78.2.269.
Fossen, T. (1994). Guidance and control of ocean vehicles. New York: John Wiley and Sons Ltd.
Garcia, R., Cufi, X., Ridao, P., & Carreras, M. (2006). Constructing photo-mosaics to assist UUV navigation and station-keeping, AUTOMAR Thematic Network, Chap. Robotics and Automation in the Maritime Industries, pp. 195–234.
German, C., Connelly, D., Prien, R., Yoerger, D., Jakuba, M., Bradley, A., et al. (2004). New techniques for hydrothermal exploration: In situ chemical sensors on AUVs—Preliminary results from the Lau Basin. Trans Amer Geophys Union Fall Meet: EOS Supplement.
Haralick, R. (1998). Propagating covariance in computer vision. Performance Characterization in Computer Vision, 1, 95–115.
Hernandez, E., Ridao, P., Ribas, D., & Batlle, J. (2009). MSISpIC: A probabilistic scan matching algorithm using a mechanical scanned imaging sonar. Journal on Physical Agents (JoPhA), 3(1), 3–12.
Hill, J., Driscoll, N., Weissel, J., Kastner, M., Singh, H., Cormier, M., et al. (2004). A detailed near-bottom survey of large gas blowout structures along the US Atlantic shelf break using the autonomous underwater vehicle (AUV) SeaBED. Trans Am Geophysical Union: EOS.
Johnson-Roberson, M., Pizarro, O., Williams, S., & Mahon, I. (2010). Generation and visualization of large-scale three-dimensional reconstructions from underwater robotic surveys. Journal of Field Robotics, 27(1), 21–51.
Kinsey, J., Eustice, R., Whitcomb, L. (2006) .A survey of underwater vehicle navigation: Recent advances and new challenges. In Proceedings of the 7th conference on maneuvering and control of marine craft (MCMC’2006) IFAC, Lisbon.
Leonard, J. J., Carpenter, R. N., & Feder, H. J. S. (2001). Stochastic mapping using forward look sonar. Robotica, 19(5), 467–480.
Leonard, J. J., & Feder, H. (2001). Decoupled stochastic mapping. IEEE Journal of Oceanic Engineering, 26(4), 561–571.
Lu, F. (1995). Shape registration using optimization for mobile robot navigation. Ph.D. thesis, Department of C.S., University of Toronto.
Lu, Z., Hu, Z., & Uchimura, K. (2009). SLAM estimation in dynamic outdoor environments: A review. In M., Xie, Y., Xiong, C., Xiong, H., Liu, Z. Hu, (Eds.), Intelligent robotics and applications, Lecture Notes in Computer Science (Vol. 5928), Springer Berlin/Heidelberg, pp. 255–267.
Mahon, I., Pizarro, O., Johnson-Roberson, M., Friedman, A., Williams, S. B., & Henderson, J. C. (2011). Reconstructing Pavlopetri: Mapping the world’s oldest submerged town using stereo-vision. In IEEE International conference on robotics and automation, ICRA (pp. 2315–2321).
Mahon, I., Williams, S., Pizarro, O., & Johnson-Roberson, M. (2008). Efficient view-based SLAM using visual loop closures. IEEE Transactions on Robotics, 24(5), 1002–1014.
Mallios, A., Ridao, P., Ribas, D., Maurelli, F., & Petillot, Y., (2010). EKF-SLAM for AUV navigation under probabilistic sonar scan-matching. In IEEE/RSJ International Conference on intelligent robots and systems (IROS), 2010 (pp. 4404–4411), doi:10.1109/IROS.2010.5649246.
Monteiro, L., Moore, T., & Hill, C. (2005). What is the accuracy of DGPS? The Journal of Navigation, 58(02), 207–225.
Montesano, L., Minguez, J., & Montano, L. (2005). Probabilistic scan matching for motion estimation in unstructured environments. In IEEE/RSJ international conference on intelligent robots and systems, (IROS 2005) (pp. 3499–3504). doi:10.1109/IROS.2005.1545182.
Newman, P. M., Leonard, J. J., & Rikoski, R. J. (2003). Towards constant-time SLAM on an autonomous underwater vehicle using synthetic aperture sonar. In Proceedings of the 11th international symposium on robotics research Sienna, Italy.
Nieto, J., Bailey, T., & Nebot, E. (2007). Recursive scan-matching SLAM. Robotics and Autonomous Systems, 55(1), 39–49. doi:10.1016/j.robot.2006.06.008.
Pfingsthorn, M., Birk, A., & Bulow, H. (2012). Uncertainty estimation for a 6-DoF spectral registration method as basis for sonar-based underwater 3D SLAM. In IEEE international conference on robotics and automation (ICRA) (pp. 3049–3054).
Pfister, S., Kriechbaum, K., Roumeliotis, S., & Burdick, J. (2002). Weighted range sensor matching algorithms for mobile robot displacement estimation. In Proceedings of the IEEE international conference on robotics and automation, 2002. ICRA’02 (Vol. 2, pp. 1667–1674). doi:10.1109/ROBOT.2002.1014782.
Pizarro, O., Friedman, A., Johnson-Roberson, M., Williams, S., Mahon, I., Camilli, R., et al. (2010). High-resolution optical and acoustic 3D seafloor reconstructions from robot and diver-based surveys. In XIX congress of the Carpathian–Balkan geological association, Carpathian–Balkan Geological Association Abstracts volume (p. 314).
Reynolds, J., Highsmith, R., Konar, B., Wheat, C., & Doudna, D. (2001). Fisheries and fisheries habitat investigations using undersea technology. In IEEE OCEANS-conference, 1998 (Vol. 2, pp. 812–820).
Ribas, D., Palomer, N., Ridao, P., Carreras, M., & Hernandez, E. (2007). Ictineu AUV wins the first SAUC-E competition. In: IEEE international conference on robotics and automation Roma, Italy.
Ribas, D., Ridao, P., Tardós, J., & Neira, J. (2008). Underwater SLAM in man made structured environments. Journal of Field Robotics, 25(11–12), 898–921. doi:10.1002/rob.20249.
Roman, C., & Singh, H. (2005). Improved vehicle based multibeam bathymetry using sub-maps and SLAM. In 2005 IEEE/RSJ International Conference on intelligent robots and systems, 2005. (IROS 2005) (pp. 3662–3669).
Singh, H., Armstrong, R., Gilbes, F., Eustice, R., Roman, C., Pizarro, O., et al. (2004). Imaging coral I: Imaging coral habitats with the SeaBED AUV. Subsurface Sensing Technologies and Applications, 5(1), 25–42.
Singh, H., Roman, C., Pizarro, O., Eustice, R., & Can, A. (2007). Towards high-resolution imaging from underwater vehicles. The International Journal of Robotics Research, 26(1), 55.
Smith, R., Self, M., & Cheeseman, P. (1990). Estimating uncertain spatial relationships in robotics (pp. 167–193). New York, NY, USA: Springer-Verlag New York, Inc.
Stutters, L., Liu, H., Tiltman, C., & Brown, D. (2008). Navigation Technologies for Autonomous Underwater Vehicles. IEEE Transactions on Systems, Man, and Cybernetics, Part C: Applications and Reviews, 38(4), 581–589.
Tena, I., de Raucourt, S., Petillot, Y., & Lane, D. M. (2004). Concurrent mapping and localization using sidescan sonar. IEEE Journal of Oceanic Engineering, 29(2), 442–456.
Walter, M., Hover, F., & Leonard, J. (2008). SLAM for ship hull inspection using exactly sparse extended information filters. In IEEE international conference on robotics and automation, ICRA, 2008 (pp. 1463–1470).
White, C., Hiranandani, D., Olstad, C. S., Buhagiar, K., Gambin, T., & Clark, C. M. (2010). The malta cistern mapping project: Underwater robot mapping and localization within ancient tunnel systems. Journal of Field Robotics, 27(4), 399–411. doi:10.1002/rob.20339.
Williams, S. B., Newman, P. M., Rosenblatt, J., Dissanayake, G., & Durrant-Whyte, H. (2001). Autonomous underwater navigation and control. Robotica, 19(5), 481–496.
Acknowledgments
This research work was partially sponsored by the Spanish project DPI2011-27977-C03-02 (COMAROB) and two European Commission’s Seventh Framework Program 2007-2013 Projects: ICT-248497 (TRIDENT) and Marie Curie PERG-GA-2010-276778 (Surf3DSLAM). The dataset was acquired with the help of the members (staff and students) of the Computer Vision and Robotics research group at the University of Girona.
Author information
Authors and Affiliations
Corresponding author
Electronic supplementary material
Below is the link to the electronic supplementary material.
Appendix
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):
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:
where \(\mathbf{R}\) is the stacked vector of the measurement errors (of dimension \(2k\times 1\)):
and \(\mathbf{W}\) is the inverted block diagonal matrix of the measurement errors covariances (of dimension \(2k\times 2k\)):
Because \(\mathbf{W}\) is the inverse of a covariance matrix, it is positive definite and hence a Cholesky decomposition exists:
Now, for simplicity and without lost of generality, let us define:
\(\mathbf{W}\) is block diagonal because the scan points are assumed to be uncorrelated, so as is \(\mathbf{L}\):
Now, Eq. (37) can be rewritten as:
and \(g(\mathbf{z}, \mathbf{x})\) can be defined as the Jacobian of the cost function, being a \((1\times 3)\) matrix;
where \({\hat{\mathbf{J}}}_{x}\) is the \((2k\times 3)\) Jacobian matrix of error vector \(\hat{\mathbf{R}}\) :
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:
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:
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:
where \(\otimes \) denotes Kronecker product of two matrices. Similarly, the \(\frac{\partial g}{\partial \mathbf{z}}\) is a \(3 \times 4k\) matrix, computed as:
where \({\hat{\mathbf{J}}}_{z}\) is the Jacobian of the error vector \(\hat{\mathbf{R}}\), being a \((2k\times 4k)\) matrix:
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:
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:
Rights and permissions
About this article
Cite this article
Mallios, A., Ridao, P., Ribas, D. et al. Scan matching SLAM in underwater environments. Auton Robot 36, 181–198 (2014). https://doi.org/10.1007/s10514-013-9345-0
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10514-013-9345-0