Hostname: page-component-78c5997874-8bhkd Total loading time: 0 Render date: 2024-11-09T01:33:34.476Z Has data issue: false hasContentIssue false
  • English
  • Français

Quaternion-based EKF-SLAM from relative pose measurements: observability analysis and applications

Published online by Cambridge University Press:  01 April 2014

Luca Carlone ,
Vito Macchia ,
Federico Tibaldi  and
Basilio Bona
Luca Carlone*
Affiliation:
College of Computing, Georgia Institute of Technology, Atlanta, USA
Vito Macchia
Affiliation:
Dipartimento di Automatica e Informatica, Politecnico di Torino, Torino, Italy
Federico Tibaldi
Affiliation:
Dipartimento di Automatica e Informatica, Politecnico di Torino, Torino, Italy
Basilio Bona
Affiliation:
Istituto Superiore Mario Boella, Torino, Italy
*
*Corresponding author. E-mail: [email protected]
Get access Rights & Permissions [Opens in a new window]

Summary

In this work, we investigate a quaternion-based formulation of 3D Simultaneous Localization and Mapping with Extended Kalman Filter (EKF-SLAM) using relative pose measurements. We introduce a discrete-time derivation that avoids the normalization problem that often arises when using unit quaternions in Kalman filter and we study its observability properties. The consistency of the estimation errors with the corresponding covariance matrices is also evaluated. The approach is further tested on real data from the Rawseeds dataset and it is applied within a delayed-state EKF architecture for estimating a dense 3D map of an unknown environment. The contribution is motivated by the possibility of abstracting multi-sensorial information in terms of relative pose measurements and for its straightforward extensions to the multi robot case.

Keywords

3D EKF-SLAMUnit quaternionsObservabilityRelative pose measurementsDelayed-state filters
Type
Articles
Information
Robotica , Volume 33 , Issue 6 , July 2015 , pp. 1250 - 1280
Copyright
Copyright © Cambridge University Press 2014 

Access options

Get access to the full version of this content by using one of the access options below. (Log in options will check for institutional or personal access. Content may require purchase if you do not have access.)

References

1. Aghili, F., “3D simultaneous localization and mapping using IMU and its observability analysis,” Robotica 29 (6), 805–814 (2010).Google Scholar
2. Artieda, J., Sebastian, J. M., Campoy, P., Correa, J. F., Mondragon, I. F., Martinez, C. and Olivares, M., “Visual 3-d SLAM from UAVs,” J. Intell. Robot. Syst. 55 (4–5), 299–321 (2009).Google Scholar
3. Azad, P., Asfour, T. and Dillmann, R., “Stereo-Based 6D Object Localization for Grasping with Humanoid Robot Systems,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (2007) pp. 919–924.Google Scholar
4. Bailey, T., Nieto, J., Guivant, J., Stevens, M. and Nebot, E., “Consistency of the EKF-SLAM Algorithm,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (2006) pp. 3562–3568.Google Scholar
5. Bar-Shalom, Y., Li, X. R. and Kirubarajan, T., Estimation with Applications to Tracking and Navigation (John Wiley and Sons, 2001).Google Scholar
6. Bayro-Corrochano, E. and Falcon-Morales, L. E., “Geometric algebra of points, lines, planes and spheres for computer vision and robotics,” Robotica 23 (6), 755–770 (2005).Google Scholar
7. Bayro-Corrochano, E., Falcon-Morales, L. E. and Zamora-Esquivel, J., “Visually Guided Robotics Using Conformal Geometric Computing,” In: Mobile Robots: Perception & Navigation, (InTech, 2007).Google Scholar
8. Breckenridge, W. G., “Quaternions - proposed standard conventions,” In” JPL, Tech. Rep. INTEROFFICE MEMORANDUM IOM 343-79-1199 (1999).Google Scholar
9. Bryson, M. and Sukkarieh, S., “Observability analysis and active control for airborne SLAM”. IEEE Transactions on Aerospace and Electronic System 44 (1), 261–280 (2008).Google Scholar
10. Cadena, C., Galvez-Lopez, D., Ramos, F., Tardós, J. D. and Neira, J., “Robust Place Recognition With Stereo Cameras,” IEEE/RSJ International Conference on Intelligent Robots and Systems (2010) pp. 5182–5189.Google Scholar
11. Calafiore, G. and Bona, B., “Constrained optimal fitting of three-dimensional vector patterns,” IEEE Trans. Robot. Autom. 14 (5), 838–844 (1998).Google Scholar
12. Carlone, L. and Bona, B., “On registration of Uncertain Three-Dimensional Vectors with Application to Robotics,” Proceedings of the IFAC World Congress (2011).Google Scholar
13. Carlone, L., Macchia, V., Tibaldi, F. and Bona, B., “Robot Localization and 3D Mapping: Observability Analysis and Applications,” Proceedings of the Int. Symposium on Artificial Intelligence, Robotics and Automation in Space (2012).Google Scholar
14. Casarrubias-Vargas, H., Petrilli-Barceló, A. H. and Bayro-Corrochano, E., “EKF-SLAM and Machine Learning Techniques for Visual Robot Navigation,” Proceedings of the 20th International Conference on Pattern Recognition (2010) pp. 396–399.Google Scholar
15. Castellanos, J. A., Martinez-Cantin, R., Tardós, J. D. and Neira, J., “Robocentric map joining: Improving the consistency of EKF-SLAM,” Robot. Auton. Syst. 55 (1), 21–29 (2007).Google Scholar
16. Castellanos, J. A., Neira, J. and Tardós, J. D., “Limits to the Consistency of EKF-Based SLAM,” 5th IFAC Symp. on Intelligent Autonomous Vehicles (2004) pp. 1244–1249.Google Scholar
17. Ceriani, S., Fontana, G., Giusti, A., Marzorati, D., Matteucci, M., Migliore, D., Rizzi, D., Sorrenti, D. G. and Taddei, P., “RAWSEEDS ground truth collection systems for indoor self-localization and mapping,” Auton. Robots J. 27 (34), 353–371 (2009).Google Scholar
18. Chiu, H., Williams, S., Dellaert, F., Samarasekera, S. and Kumar, R., “Robust Vision-Aided Navigation Using Sliding-Window Factor Graphs,” Proceedings of the IEEE International Conference on Robotics and Automation (2013).Google Scholar
19. Cole, D. M. and Newman, P. M., “Using Laser Range Data for 3D SLAM in Outdoor Environments,” IEEE International Conference on Robotics and Automation (2006) pp. 1556–1563.Google Scholar
20. Cristofaro, A., Renzaglia, A. and Martinelli, A., “Distributed Information Filters for MAV Cooperative Localization,” Proceedings of the 10th International Symposium on Distributed Autonomous Robotics Systems (2010) pp. 287–293.Google Scholar
21. Dementhon, D. F. and Davis, L. S., “Model-based object pose in 25 lines of code,” Int. J. Comput. Vis. 15 (1-2), 123–141 (1995).Google Scholar
22. Doucet, A., de Freitas, N., Murphy, K. and Russel, S., “Rao-Blackwellized Particle Filtering for Dynamic Bayesian Networks,” Proceedings of the Conf. on Uncertainty in Artificial Intelligence (2000) pp. 176–183.Google Scholar
23. Durrant-Whyte, H. and Bailey, T., “Simultaneous localization and mapping (SLAM): Part I,” Robot. Autom. Mag. 13, 99–110 (2006).Google Scholar
24. Durrant-Whyte, H. and Bailey, T., “Simultaneous localization and mapping (SLAM): Part II,” Robot. Autom. Mag. 13, 108–117 (2006).Google Scholar
25. Durrant-Whyte, H. F., “Uncertain geometry in robotics,” IEEE Trans. Robot. Automat. 4 (1), 22–31 (1988).Google Scholar
26. Estrada, C., Neira, J. and Tardós, J. D., “Hierarchical SLAM: Real-time accurate mapping of large environments,” IEEE Trans. Robot. 21 (4), 588–596 (2005).Google Scholar
27. Sorrenti, D., Burgard, W., Grisetti, G., Ruhnke, M., Stachniss, C., Fontana, G., Matteucci, M., Marzorati, D., Tardós, J. D., et al. “Rawseed Project: Deliverable D4.1, Benchmark problems”, Technical Report, available online at: http://www.rawseeds.org/home/category/documents/deliverables/, 2009.Google Scholar
28. Eustice, R. M., Singh, H. and Leonard, J. J., “Exactly sparse delayed-state filters for view-based SLAM,” Int. J. Robot. Res. 22 (6), 1100–1114 (2006).Google Scholar
29. Garcia, M. A. and Solanas, A., “3D Simultaneous Localization and Modeling from Stereo Vision,” IEEE International Conference on Robotics and Automation (2004) pp. 847–853.Google Scholar
30. Goshen-Meskin, D. and Bar-Itzhack, I. Y., “Observability analysis of piece-wise constant systems, Part 1: Theory,” IEEE Trans. Aerosp. Electron. Syst. 28 (4), 1056–1067 (1992).Google Scholar
31. Goshen-Meskin, D. and Bar-Itzhack, I. Y., “Observability analysis of piece-wise constant systems, Part 2: Application to inertial navigation in-flight alignment,” IEEE Trans. Aerosp. Electron. Syst. 28 (4), 1068–1075 (1992).Google Scholar
32. Hartley, R. I. and Zisserman, A.,” Multiple View Geometry in Computer Vision (Cambridge University Press, 2000) ISBN: 0521623049.Google Scholar
33. Horn, R. A. and Johnson, C. R., Matrix Analysis (Cambridge University Press, UK, 1985).Google Scholar
34. Huang, G., Mourikis, A. I. and Roumeliotis, S. I., “Observability-based rules for designing consistent EKF-SLAM estimators,” Int. J. Robot. Res. 29 (5), 502–528 (2010).Google Scholar
35. Huang, S. and Dissanayake, G., “Convergence and consistency analysis for extended kalman filter based SLAM,” IEEE Trans. Robot. 23 (5), 1036–1049 (2001).Google Scholar
36. Ila, V., Porta, J. M. and Andrade-Cetto, J., “Information-based compact pose SLAM,” IEEE Trans. Robot. 26 (1), 78–93 (2010).Google Scholar
37. Julier, S. and Uhlmann, J., “A Counter Example to the Theory of Simultaneous Localization and Map Building,” Proceedings of the IEEE International Conference on Robotics and Automation (2001) pp. 4238–4243.Google Scholar
38. Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. and Dellaert, F., “iSAM2:incremental smoothing and mapping using the bayes tree,” Int. J. Robot. Res. 31, 217–236 (2012).Google Scholar
39. Kim, J. H. and Sukkarieh, S., “Improving the real-time efficiency of inertial SLAM and understanding its observability,” IEEE International Conference on Intelligent RObots and Systems, pp. 1264–1269 (2004).Google Scholar
40. Kuipers, J. B., Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace, and Virtual Reality (Princeton University Press, 1999).Google Scholar
41. Lefferts, E. J., Markley, F. L. and Shuster, M. D., “Kalman filtering for spacecraft attitude estimation,” J. Guid. Control Dyn. 5 (5), 417–429 (1982).Google Scholar
42. Lemaire, T., Berger, C., Jung, I. K. and Lacroix, S., “Vision-based SLAM: Stereo and monocular approaches,” Int. J. Comput. Vis. 74 (3), 343–364 (2007).Google Scholar
43. Leonard, J., Rikoski, R., Newman, P. and Bosse, M., “Mapping partially observable features from multiple uncertain vantage points,” Int. J. Robot. Res. 21 (10), 943–975 (2002).Google Scholar
44. Lu, F. and Milios, E., “Globally consistent range scan alignment for environment mapping,” Auton. Robots 4, 333–349 (1997).Google Scholar
45. Marins, J. L., Yun, X., Bachman, E. R. and McGhee, R. B., “An Extended Kalman Filter for Quaternion-Based Orientation Estimation using MARG Sensors,” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (2001) pp. 2003–2011.Google Scholar
46. Mourikis, A. I., Trawny, N., Roumeliotis, S. I., Johnson, A., Ansar, A. and Matthies, L., “Vision-aided inertial navigation for spacecraft entry, descent, and landing,” IEEE Trans. Robot. 25 (2), 264–280 (2009).Google Scholar
47. Newman, P., On the Structures and Solution of Simultaneous Localization and Mapping Problem, Ph.D. thesis (Department of Mathematical Sciences, Division of Mathematics, Chalmers University of Technology and Goteborg University, 2007).Google Scholar
48. Nister, D. and Stewenius, H., “Scalable Recognition with a Vocabulary Tree,” Proceedings of the 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (2005) pp. 2161–2168.Google Scholar
49. Nüchter, A., Surmann, H., Lingemann, K., Hertzberg, J. and Thrun, S., “6D SLAM with an Application in Autonomous Mine Mapping,” IEEE International Conference on Robotics and Automation (2004) pp. 1998–2003.Google Scholar
50. Piniés, P., Paz, L. M. and Tardós, J. D., “CI-graph SLAM for 3D reconstruction of large and complex environments using a multicamera system,” Int. J. Field Robot. 27 (5), 561–586 (2010).Google Scholar
51. Piniés, P. and Tardós, J. D., “Scalable SLAM Building Conditionally Independent Local Maps,” IEEE-RSJ International Conference on Intelligent Robots and Systems (2007).Google Scholar
52. Sabatini, A. M., “Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing,” IEEE Trans. Biomed. Eng. 53 (7), 1346–1356 (2006).Google Scholar
53. Sabatta, D., Scaramuzza, D. and Siegwart, R., “Improved Appearance-Based Matching in Similar and Dynamic Environments using a Vocabulary Tree,” Proceedings of the IEEE International Conference on Robotics and Automation (2010) pp. 2262–2269.Google Scholar
54. Scaramuzza, D. and Siegwart, R., “Appearance guided monocular omnidirectional visual odometry for outdoor ground vehicles,” IEEE Trans. Robot. 24 (5) (2008).Google Scholar
55. Shuster, M. D., “A survey of attitude representations,” J. Astronaut. Sci. 41 (4), 439–517 (1993).Google Scholar
56. Siciliano, B. and Khatib, O.,” Handbook of Robotics (Springer, 2008).Google Scholar
57. Smith, R. and Cheesman, P., “On the representation of spatial uncertainty,” Int. J. Robot. Res. 5 (4), 56–68 (1987).Google Scholar
58. Stachniss, C., Grisetti, G., Hahnel, D. and Burgard, W., “Improved Rao-Blackwellized Mapping by Adaptive Sampling and Active Loop-Closure,” Proceedings of the Workshop on Self-Organization of AdaptiVE behavior (2004) pp. 1–15.Google Scholar
59. Thrun, S., Koller, D., Ghahramani, Z., Durrant-Whyte, H. and Ng, A. Y., “Simultaneous Mapping and Localization with Sparse Extended Information Filters,” Proceedings of the 5th Int. Workshop on Algorithmic Foundations of Robotics (2002).Google Scholar
60. Trawny, N. and Roumeliotis, S. I., Indirect Kalman Filter for 3D Attitude Estimation, Tech. Rep. 2005-002 (University of Minnesota, Dept. of Comp. Sci. and Eng. 2005).Google Scholar
61. Walter, M., Eustice, R. and Leonard, J., “Exactly sparse extended information filters for feature-based SLAM,” Int. J. Robot. Res. 26 (4), 335–359 (2007).Google Scholar
62. Weingarten, J. and Siegwart, R., “EKF-Based 3D SLAM for Structured Environment Reconstruction,” Proceedings of the International Conference on Intelligent Robots and Systems (2005) pp. 2–6.Google Scholar