Volume 16 Number 6
December 2019
Article Contents
J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio and R. Carelli. Continuous Probabilistic SLAM Solved via Iterated Conditional Modes. International Journal of Automation and Computing, vol. 16, no. 6, pp. 838-850, 2019. doi: 10.1007/s11633-019-1186-7
Cite as: J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio and R. Carelli. Continuous Probabilistic SLAM Solved via Iterated Conditional Modes. International Journal of Automation and Computing, vol. 16, no. 6, pp. 838-850, 2019. doi: 10.1007/s11633-019-1186-7

Continuous Probabilistic SLAM Solved via Iterated Conditional Modes

Author Biography:
  • J. Gimenez received the B. Sc. degree inmathematics from the National University of San Juan (UNSJ), Argentina in 2009, and the Ph. D. degree in mathematics from the National University of Córdoba (UNC), Argentina in 2014. Currently, he is an assistant researcher of the Argentinean National Council for Scientific Research (CONICET), and an adjunct professor in the Institute of Automatics, UNSJ-CONICET, Argentina. His research interests include probabilistic and statistical implementations of robotics, such as SLAM algorithms. E-mail: jgimenez@inaut.unsj.edu.ar ORCID iD: 0000-0002-1731-8520

    A. Amicarelli received the Dipl-Ing degree in chemical engineering from the National University of San Juan, Argentina in 2000, and the Ph. D. degree in control systems from the Institute of Automatics (INAUT) of the same University in 2007. Her main works are related with state estimations of complex non-linear systems and with bioprocess control. She is adjunct researcher of the Argentinean National Council for Scientific Research (CONICET) from 2012. Her research interests include systems modeling, process control and state estimation to control porpoises.E-mail: amicarelli@inaut.unsj.edu.ar ORCID iD: 0000-0002-4303-8776

    J. M. Toibero received the B. Eng. degree in electronic engineering from the Facultad Tecnológica Nacional (UTNParaná) of Argentina, Argentina in 2002, and the Ph. D. degree in control systems from the Institute of Automatics (INAUT), the National University of San Juan, Argentina in 2007. His main works are related to nonlinear control of robotic platforms and robotics applications in agriculture. He is with the National Council for Scientific and Technological Research (CONICET) of Argentina since 2011, actually he is adjunct researcher. He leads different technological projects and his current scientific research is at the Institute of Automatics (INAUT) of San Juan, Argentina. His research interests include wheeled mobile robots, manipulators force/impedance, switched, hybrid, nonlinear control methods applied to automatic control and visual servoing.E-mail: mtoibero@inaut.unsj.edu.ar (Corresponding author) ORCID iD: 0000-0001-9347-566X

    F. di Sciascio received B. Sc. degree in electromechanical engineering with orientation in electronics from the National University of Buenos Aires (UBA), Argentina in 1986. He received the M. Sc. degree in engineering of control Systems in 1994, and the Ph. D. degree in engineering in 1997, from the Institute of Automatics (INAUT), National University of San Juan (UNSJ), Argentina. Since 1987, he is a professor and researcher at the INAUT and he is currently a full professor in charge of the subjects artificial intelligence complements and identification and adaptive control (degree in electronic engineering) in the Department of Electronics and Automation, at the same time, he is a professor of the postgraduate degree at the INAUT, Argentina. His research interests include modelling, identification and estimation in dynamical systems, and technology developments in automatic process control.E-mail: fernando@inaut.unsj.edu.ar ORCID iD: 0000-0002-4382-6289

    R. Carelli received B. Sc. degree in engineering from the National University of San Juan, Argentina in 1976, and the Ph. D. degree in electrical engineering from the National University of Mexico (UNAM), Mexico in 1989. He is a full professor at the National University of San Juan and a senior researcher of the National Council for Scientific and Technical Research, Argentina. He is Director of the Automatics Institute, National University of San Juan, Argentina. He is a Senior Member of IEEE and a Member of Argentine Association of Automatic Control – International Federation of Automatic Control (AADECA-IFAC). His research interests include robotics, manufacturing systems, adaptive control and artificial intelligence applied to automatic control. E-mail: rcarelli@inaut.unsj.edu.ar ORCID iD: 0000-0003-0688-7020

  • Received: 2019-02-22
  • Accepted: 2019-04-29
  • Published Online: 2019-08-08
  • This article proposes a simultaneous localization and mapping (SLAM) version with continuous probabilistic mapping (CP-SLAM), i.e., an algorithm of simultaneous localization and mapping that avoids the use of grids, and thus, does not require a discretized environment. A Markov random field (MRF) is considered to model this SLAM version with high spatial resolution maps. The mapping methodology is based on a point cloud generated by successive observations of the environment, which is kept bounded and representative by including a novel recursive subsampling method. The CP-SLAM problem is solved via iterated conditional modes (ICM), which is a classic algorithm with theoretical convergence over any MRF. The probabilistic maps are the most appropriate to represent dynamic environments, and can be easily implemented in other versions of the SLAM problem, such as the multi-robot version. Simulations and real experiments show the flexibility and excellent performance of this proposal.
  • 加载中
  • [1] H. Durrant-Whyte, T. Bailey.  Simultaneous localization and mapping: Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110. doi: 10.1109/MRA.2006.1638022
    [2] T. S. Ho, Y. C. Fai, E. S. Lee Ming. Simultaneous localization and mapping survey based on filtering techniques. In Proceedings of the 10th Asian Control Conference, IEEE, Kota Kinabalu, Malaysia, pp. 1–6, 2015. DOI: 10.1109/ASCC.2015.7244836.
    [3] Y. Yang, F. Qiu, H. Li, L. Zhang, M. L. Wang, M. Y. Fu.  Large-scale 3D semantic mapping using stereo vision[J]. International Journal of Automation and Computing, 2018, 15(2): 194-206. doi: 10.1007/s11633-018-1118-y
    [4] A. Yassin, Y. Nasser, M. Awad, A. Al-Dubai, R. Liu, C. Yuen, R. Raulefs, E. Aboutanios.  Recent advances in indoor localization: A survey on theoretical approaches and applications[J]. IEEE Communications Surveys & Tutorials, 2017, 19(2): 1327-1346. doi: 10.1109/COMST.2016.2632427
    [5] X. Yuan, J. F. Martínez-Ortega, J. A. Sánchez Fernández, M. Eckert. AEKF-SLAM: A new algorithm for robotic underwater navigation. Sensors, vol. 17, no. 5, Article number 1174, 2017.
    [6] H. Roh, J. Jeong, A. Kim.  Aerial image based heading correction for large scale SLAM in an urban canyon[J]. IEEE Robotics and Automation Letters, 2017, 2(4): 2232-2239. doi: 10.1109/LRA.2017.2725439
    [7] P. Kim, J. D. Chen, Y. K. Choc.  SLAM-driven robotic mapping and registration of 3D point clouds[J]. Automation in Construction, 2018, 89(): 38-48. doi: 10.1016/j.autcon.2018.01.009
    [8] S. Thrun, W. Burgard, D. Fox. Probabilistic Robotics (Intelligent Robotics and Autonomous Agents), Cambridge, USA: The MIT Press, 2005.
    [9] P. Xu, G. Dherbomez, E. Hery, A. Abidli, P. Bonnifait.  System architecture of a driverless electric car in the grand cooperative driving challenge[J]. IEEE Intelligent Transportation Systems Magazine, 2018, 10(1): 47-59. doi: 10.1109/MITS.2017.2776135
    [10] J. C. Trujillo, R. Munguia, E. Guerra, A. Grau. Cooperative monocular-based SLAM for multi-UAV systems in GPS-denied environments. Sensors, vol. 18, no. 5, Article number 1351, 2018.
    [11] J. Li, M. Kaess, R. M. Eustice, M. Johnson-Roberson.  Pose-graph SLAM using forward-looking sonar[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 2330-2337. doi: 10.1109/LRA.2018.2809510
    [12] R. Mur-Artal, J. D. Tardós.  ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262. doi: 10.1109/TRO.2017.2705103
    [13] L. T. Hsu, Y. Wada, Y. L. Gu, S. Kamijo. Rectification of 3D building models based on GPS signal collected by vehicle. In Proceedings of IEEE International Conference on Vehicular Electronics and Safety, IEEE, Yokohama, Japan, 2015. DOI: 10.1109/ICVES.2015.7396907.
    [14] F. Auat Cheein, G. Steiner, G. P. Paina, R. Carelli.  Optimized EIF-SLAM algorithm for precision agriculture mapping based on stems detection[J]. Computers and Electronics in Agriculture, 2011, 78(2): 195-207. doi: 10.1016/j.compag.2011.07.007
    [15] J. A. Hesch, D. G. Kottas, S. L. Bowman, S. I. Roumeliotis.  Consistency analysis and improvement of vision- aided inertial navigation[J]. IEEE Transactions on Robotics, 2014, 30(1): 158-176. doi: 10.1109/TRO.2013.2277549
    [16] G. P. Huang, A. I. Mourikis, S. I. Roumeliotis.  A quadratic-complexity observability-constrained unscented Kalman filter for SLAM[J]. IEEE Transactions on Robotics, 2013, 29(5): 1226-1243. doi: 10.1109/TRO.2013.2267991
    [17] J. Gimenez, D. Herrera, S. Tosetti, R. Carelli.  Optimization methodology to fruit grove mapping in precision agriculture[J]. Computers and Electronics in Agriculture, 2015, 116(): 88-100. doi: 10.1016/j.compag.2015.06.013
    [18] Y. Xu, T. Shen, X. Y. Chen, L. L. Bu, N. Feng. Predictive adaptive Kalman filter and its application to INS/UWB-integrated human localization with missing UWB-based measurements. International Journal of Automation and Computing, to be published. DOI: 10.1007/s11633-018-1157-4.
    [19] P. J. Huber. Robust Statistics, New York, USA: Wiley-Interscience, 1981.
    [20] D. M. Rosen, C. DuHadway, J. J. Leonard. A convex relaxation for approximate global optimization in simultaneous localization and mapping. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Seattle, USA, 2015. DOI: 10.1109/ICRA.2015.7140014.
    [21] I. J. Cox.  A review of statistical data association techniques for motion correspondence[J]. International Journal of Computer Vision, 1993, 10(1): 53-66. doi: 10.1007/BF01440847
    [22] M. Adams, B. N. Vo, R. Mahler, J. Mullane.  SLAM gets a PHD: New concepts in map estimation[J]. IEEE Robotics & Automation Magazine, 2014, 21(2): 26-37. doi: 10.1109/MRA.2014.2304111
    [23] J. Folkesson, H. I. Christensen. Graphical SLAM - a self-correcting map. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, New Orleans, USA, pp. 383–389, 2004. DOI: 10.1109/ROBOT.2004.1307180.
    [24] R. G. Cowell, A. P. Dawid, S. L. Lauritzen, D. J. Spiegelhalter, Probabilistic Networks and Expert Systems, New York, USA: Springer, 1999. DOI: 10.1007/b97670.
    [25] J. M. Hammersley, P. Clifford. Markov Field on Finite Graphs and Lattices, Technical Report, Department of Statistics, Oxford University, Oxford, UK, 1971.
    [26] R. Kindermann, J. L. Snell. Markov Random Fields and Their Applications, Providence, USA: American Mathematical Society, 1980.
    [27] S. Z. Li. Markov Random Field Modeling in Image Analysis, 3rd ed., London, USA: Springer, 2009. DOI: 10.1007/978-1-84800-279-1.
    [28] F. Dellaert, M. Kaess.  Square root SAM: Simultaneous localization and mapping via square root information smoothing[J]. International Journal of Robotics Research, 2006, 25(12): 1181-1203. doi: 10.1177/0278364906072768
    [29] M. Jadaliha, J. Choi. Fully Bayesian simultaneous localization and spatial prediction using Gaussian Markov random fields (GMRFs). In Proceedings of American Control Conference, IEEE, Washington, USA, pp. 4592–4597, 2013. DOI: 10.1109/ACC.2013.6580547.
    [30] Y. Latif, C. Cadena, J. Neira. Robust graph SLAM back-ends: A comparative analysis. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Chicago, USA, pp. 2683–2690, 2014. DOI: 10.1109/IROS.2014.6942929.
    [31] N. Carlevaris-Bianco, M. Kaess, R. M. Eustice.  Generic node removal for factor-graph SLAM[J]. IEEE Transactions on Robotics, 2014, 30(6): 1371-1385. doi: 10.1109/TRO.2014.2347571
    [32] B. Suger, G. D. Tipaldi, L. Spinello, W. Burgard. An approach to solving large-scale slam problems with a small memory footprint. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Hong Kong, China, pp. 3632–3637, 2014. DOI: 10.1109/ICRA.2014.6907384.
    [33] M. Mazuran, G. D. Tipaldi, L. Spinello, W. Burgard. Nonlinear graph sparsification for SLAM. In Proceedings of Robotics: Science and Systems, RSS, Berkeley, USA, 2014. DOI: 10.15607/RSS.2014.X.040.
    [34] S. Choudhary, V. Indelman, H. I. Christensen, F. Dellaert. Information-based reduced landmark SLAM. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Seattle, USA, 2015. DOI: 10.1109/ICRA.2015.7139839.
    [35] G. Dissanayake, S. B. Williams, H. Durrant-Whyte, T. Bailey.  Map management for efficient simultaneous localization and mapping (SLAM)[J]. Autonomous Robots, 2002, 12(3): 267-286. doi: 10.1023/A:1015217631658
    [36] J. Besag.  On the statistical analysis of dirty pictures[J]. Journal of the Royal Statistical Society, Series B, 1986, 48(3): 259-302.
    [37] J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio, R. Carelli.  Iterated conditional modes to solve simultaneous localization and mapping in Markov random fields context[J]. International Journal of Automation and Computing, 2018, 15(3): 310-324. doi: 10.1007/s11633-017-1109-4
    [38] S. Kullback, R. A. Leibler.  On information and sufficiency[J]. Annals of Mathematical Statistics, 1951, 22(1): 79-86. doi: 10.1214/aoms/1177729694
    [39] Z. I. Botev, J. F. Grotowski, D. P. Kroese.  Kernel density estimation via diffusion[J]. The Annals of Statistics, 2010, 38(5): 2916-2957. doi: 10.1214/10-AOS799
    [40] M. P. Wand, M. C. Jones.  Comparison of smoothing parameterizations in bivariate kernel density estimation[J]. Journal of the American Statistical Association, 1993, 88(422): 520-528. doi: 10.1080/01621459.1993.10476303
    [41] J. Gimenez, S. Tosetti, L. Salinas, R. Carelli.  Bounded memory probabilistic mapping of out-of-structure objects in fruit crops environments[J]. Computers and Electronics in Agriculture, 2018, 115(): 11-20. doi: 10.1016/j.compag.2018.05.018
    [42] Z. I. Botev. Kernel density estimation using Matlab, 2007. [Online], Available: https://la.mathworks.com/matlabcentral/fileexchange/17204-kernel-density-estimation?focused=5829342&tab=function, 2007.
    [43] V. H. Andaluz, F. Roberti, J. M. Toibero, R. Carelli, B. Wagner. Adaptive dynamic path following control of an unicycle-like mobile robot. In Proceedings of the 4th International Conference on Intelligent Robotics and Applications, Springer, Aachen, Germany, pp. 563–574, 2011. DOI: 10.1007/978-3-642-25486-4_56.
    [44] H. Secchi, R. Carelli, V. Mut.  An experience on stable control of mobile robots[J]. Latin American Applied Research, 2003, 33(4): 379-385.
  • 加载中
  • [1] R. Bharathi, R. Selvarani. Hidden Markov Model Approach for Software Reliability Estimation with Logic Error . International Journal of Automation and Computing, 2020, 17(2): 305-320.  doi: 10.1007/s11633-019-1214-7
    [2] Hai-Qiang Zhang, Hai-Rong Fang, Bing-Shan Jiang, Shuai-Guo Wang. Dynamic Performance Evaluation of a Redundantly Actuated and Over-constrained Parallel Manipulator . International Journal of Automation and Computing, 2019, 16(3): 274-285.  doi: 10.1007/s11633-018-1147-6
    [3] S.P. Mishra, P.K. Dash. Short Term Wind Speed Prediction Using Multiple Kernel Pseudo Inverse Neural Network . International Journal of Automation and Computing, 2018, 15(1): 66-83.  doi: 10.1007/s11633-017-1086-7
    [4] J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio, R. Carelli. Iterated Conditional Modes to Solve Simultaneous Localization and Mapping in Markov Random Fields Context . International Journal of Automation and Computing, 2018, 15(3): 310-324.  doi: 10.1007/s11633-017-1109-4
    [5] Yuan Ge, Yaoyiran Li. SCHMM-based Compensation for the Random Delays in Networked Control Systems . International Journal of Automation and Computing, 2016, 13(6): 643-652.  doi: 10.1007/s11633-016-1001-7
    [6] Abhijit Gosavi, Anish Parulekar. Solving Markov Decision Processes with Downside Risk Adjustment . International Journal of Automation and Computing, 2016, 13(3): 235-245.  doi: 10.1007/s11633-016-1005-3
    [7] Bao-Chang Xu,  Ying-Ying Zhang. An Improved Gravitational Search Algorithm for Dynamic Neural Network Identification . International Journal of Automation and Computing, 2014, 11(4): 434-440.  doi: 10.1007/s11633-014-0810-9
    [8] Chun-Ling Dong,  Qin Zhang,  Shi-Chao Geng. A Modeling and Probabilistic Reasoning Method of Dynamic Uncertain Causality Graph for Industrial Fault Diagnosis . International Journal of Automation and Computing, 2014, 11(3): 288-298.  doi: 10.1007/s11633-014-0791-8
    [9] Xin-Min Xu, Yao Chen, Wen-Yao Xu, Fang Gong. An Efficient Algorithm for Mobile Localization in Sensor Networks . International Journal of Automation and Computing, 2012, 9(6): 594-599 .  doi: 10.1007/s11633-012-0684-7
    [10] Bin Tang,  Qi-Jie Zeng,  De-Feng He,  Yun Zhang. Random Stabilization of Sampled-data Control Systems with Nonuniform Sampling . International Journal of Automation and Computing, 2012, 9(5): 492-500.  doi: 10.1007/s11633-012-0672-y
    [11] Arthur Anyakwo,  Crinela Pislaru,  Andrew Ball. A New Method for Modelling and Simulation of the Dynamic Behaviour of the Wheel-rail contact . International Journal of Automation and Computing, 2012, 9(3): 237-247.  doi: 10.1007/s11633-012-0640-6
    [12] Khalid Jebari, Abdelaziz Bouroumi, Aziz Ettouhami. Fuzzy Genetic Sharing for Dynamic Optimization . International Journal of Automation and Computing, 2012, 9(6): 616-626 .  doi: 10.1007/s11633-012-0687-4
    [13] Xiu-Lan Wang, Chun-Guo Fei, Zheng-Zhi Han. Adaptive Predictive Functional Control for Networked Control Systems with Random Delays . International Journal of Automation and Computing, 2011, 8(1): 62-68.  doi: 10.1007/s11633-010-0555-z
    [14] Abdellatif Naceri, Habib Hamdaoui, Mohamed Abid. An Advanced FMRL Controller for FACTS Devices to Enhance Dynamic Performance of Power Systems . International Journal of Automation and Computing, 2011, 8(3): 309-316.  doi: 10.1007/s11633-011-0586-0
    [15] Ming-Zhe Hou,  Guang-Ren Duan. Adaptive Dynamic Surface Control for Integrated Missile Guidance and Autopilot . International Journal of Automation and Computing, 2011, 8(1): 122-127.  doi: 10.1007/s11633-010-0563-z
    [16] Development and Application of a Marine Ecosystem Dynamic Model . International Journal of Automation and Computing, 2011, 8(2): 154-160.  doi: 10.1007/s11633-011-0568-2
    [17] Xiao-Yuan Luo,  Zhi-Hao Zhu,  Xin-Ping Guan. Adaptive Fuzzy Dynamic Surface Control for Uncertain Nonlinear Systems . International Journal of Automation and Computing, 2009, 6(4): 385-390.  doi: 10.1007/s11633-009-0385-z
    [18] Hesham Abusaimeh,  Shuang-Hua Yang. Dynamic Cluster Head for Lifetime Efficiency in WSN . International Journal of Automation and Computing, 2009, 6(1): 48-54.  doi: 10.1007/s11633-009-0048-0
    [19] Shengxiang Yang, Renato Tinós. A Hybrid Immigrants Scheme for Genetic Algorithms in Dynamic Environments . International Journal of Automation and Computing, 2007, 4(3): 243-254.  doi: 10.1007/s11633-007-0243-9
    [20] Siddhartha Shakya, John McCall. Optimization by Estimation of Distribution with DEUM Framework Based on Markov Random Fields . International Journal of Automation and Computing, 2007, 4(3): 262-272.  doi: 10.1007/s11633-007-0262-6
通讯作者: 陈斌, bchen63@163.com
  • 1. 

    沈阳化工大学材料科学与工程学院 沈阳 110142

  1. 本站搜索
  2. 百度学术搜索
  3. 万方数据库搜索
  4. CNKI搜索

Figures (10)


Abstract Views (929) PDF downloads (49) Citations (0)

Continuous Probabilistic SLAM Solved via Iterated Conditional Modes

Abstract: This article proposes a simultaneous localization and mapping (SLAM) version with continuous probabilistic mapping (CP-SLAM), i.e., an algorithm of simultaneous localization and mapping that avoids the use of grids, and thus, does not require a discretized environment. A Markov random field (MRF) is considered to model this SLAM version with high spatial resolution maps. The mapping methodology is based on a point cloud generated by successive observations of the environment, which is kept bounded and representative by including a novel recursive subsampling method. The CP-SLAM problem is solved via iterated conditional modes (ICM), which is a classic algorithm with theoretical convergence over any MRF. The probabilistic maps are the most appropriate to represent dynamic environments, and can be easily implemented in other versions of the SLAM problem, such as the multi-robot version. Simulations and real experiments show the flexibility and excellent performance of this proposal.

J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio and R. Carelli. Continuous Probabilistic SLAM Solved via Iterated Conditional Modes. International Journal of Automation and Computing, vol. 16, no. 6, pp. 838-850, 2019. doi: 10.1007/s11633-019-1186-7
Citation: J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio and R. Carelli. Continuous Probabilistic SLAM Solved via Iterated Conditional Modes. International Journal of Automation and Computing, vol. 16, no. 6, pp. 838-850, 2019. doi: 10.1007/s11633-019-1186-7
    • Simultaneous localization and mapping (SLAM) is the problem in which one or more mobile robots build and update a map of their surroundings whereas, at the same time, this map is used to compute its or their locations[1, 2]. Solving the SLAM problem is fundamental to obtain genuinely autonomous mobile robots. Today, there are outdoor[3] and indoor[4] SLAM applications considering from subsea[5] to aerial[6] domains. Consequently, not only wheeled mobile robots[79] are considered but also unmanned aerial vehicles (UAVs)[10]; autonomous underwater vehicles (AUVs) or unmanned surface vehicles (USVs). Depending on the applications, several sensors have been considered such as active 3D lidar devices[7], sonars[11], different passive red-green-blue (RGB) cameras[12], or stereo camera devices red-green-blue-depth (RGBD). Also, global positioning systems (GPS)[13] or combinations of all previous sensors[14] are considered.

      The SLAM problem is complex due to the localization-mapping interdependence that requires the simultaneous, consistent, speedy and precise resolution of two issues[15, 16]. The difficulty of solving both problems simultaneously is such that today there are articles that study them separately[17, 18]. The solution must contemplate the three primary sources of uncertainty: sensing errors, matching between new observations and already mapped areas, and odometric errors cumulated during the robot navigation. This way, it is advisable 1) to obtain models for any sensory data; 2) to optimize or to avoid dealing with observation-object matchings, and 3) to incorporate precise vehicle motion models (dynamic or highly nonlinear).

      Complete comparative reviews of SLAM solvers are given in [1, 2]. The SLAM problem is generally specified as a sparse high-dimensional non-convex M-estimation[19] solved via optimization methods of first or second order. Generally, the optimized solution is not global, and thus, the algorithm initialization affects the estimation quality considerably. These considerations along with the high-dimensionality of the state space produce objective functions with many local minima at which the SLAM solver usually get entrapped[20].

      Several versions of the SLAM problem were proposed, among the most notorious appear the SLAM based on landmarks and the probabilistic SLAM. In the first, it is assumed that the environment is constituted by punctual and static objects whose position must be estimated, continually updated, and simultaneously used to auto-localize the robot. On the other hand, the probabilistic map assigns an occupancy probability to each site of the environment, to then estimate the robot location, and vice-versa.

      Grid-based mapping is the most considered version of the probabilistic mapping, in which a probability is stored for each cell, making the estimation unscalable for large working zones. Some alternatives have been proposed to optimize storage and processing resources. For instance, “octress” generates a low resolution spatial grid initially, for then, locally refine it in the most visited regions. On the other hand, continuous probabilistic SLAM (CP-SLAM) proposes to map without partitioning the environment by using a subsample of the observations acquired during navigation. This subsampling must be done on-line each time any storage or processing bound is reached, and must also contain a representative sample of each zone of the environment. An appropriate recursive subsampling methodology is explained in Appendix.

      In this modeling methodology, static objects produce small regions with high probability whereas dynamic objects produce fringes in the map with less probability. This way, eventual dynamic objects are forgotten over time. In the multi-robot SLAM problem with probabilistic mapping, each robot maps to the other robots as dynamic objects.

      Data association between observations and landmarks is a critical problem for most SLAM solvers since possible links can grow exponentially turning the process computationally demanding[21]. In [22], a method is presented based on random finite sets (RFS) where the matching is performed into the optimization process avoiding heuristic association rules and solving the SLAM problem using a Bayesian set-based estimator probability hypothesis density (PHD) filter. However, this matching problem is generally addressed by an algorithm external to the estimation process. Unlike the continuous probabilistic mapping, a matching process is required for both: landmark based mapping and the probabilistic mapping based on grids.

      Folkesson and Christensen presented GraphSLAM[23], which computes the best robot trajectory based on a nonlinear optimization procedure. Graph theory[24] illustrates the model states with their probabilistic interdependencies assigning a node to each state whereas direct and indirect correlations are represented by edges and paths respectively. When the conditional probabilities of the model are consistent with the topological structure of the graph the random field is Markovian (MRF)[25, 26]. These Markovian models have been implemented in the image processing context in order to solve several issues[27]. MRF has remarkable benefits as the possibility of incorporating 1) motion models for any robot architecture, 2) observation models for any sensor, 3) map models, and 4) sensorial fusion preserving theoretical convergence[28]. Using a Gaussian MRF, [29] reports satisfactory results for the simultaneous localization and spatial prediction (SLAP) problem. Dellaert and Kaess[28] presents square root SAM (square root smoothing and mapping information), which exploits the inherent sparsity of the SLAM problem. Many works focused on reducing the graph complexity[3034], which can cause statistically inconsistent estimations[35].

      Iterated conditional modes (ICM)[36] finds the state configuration that locally maximizes the posterior probability of any Markovian model. Its SLAM particularization should be initialized with an approximate map estimation as well as the robot trajectory[37], which are recursively improved until it converges to sub-optimal estimations. However, these sub-optimal results are better than the initial estimates and can be used to improve the output of any SLAM procedure. Moreover, the better the initialization, the better the improvement. It is ideal to work with on-line initializations and enhance it by ICM iterations performed at crucial times, such as loop closures.

      In short, MRF allows incorporating any prior information whereas ICM allows to formally incorporate heuristic rules of filtering, association, and optimization within a theoretical framework with convergence properties. In [37], the authors modeled the SLAM problem based on landmarks using MRF and solved it using ICM.

      The main contributions of this paper are:

      1) Definition of the CP-SLAM problem, which has the following advantages:

      a) Maps with high spatial resolution avoiding any environment partition.

      b) Online calculation of point probabilities instead of grid cells probabilities.

      c) Adaptability and robustness to work in dynamic environments.

      d) Avoidance of the association problem.

      2) Modeling of the CP-SLAM problem through MRF, even outside the structure of linear or Gaussian models.

      3) Presentation of a subsampling methodology that keeps the observation database bounded and updated, allowing online calculations.

      4) Resolution of the CP-SLAM problem via ICM with a high-quality online initialization that requires few iterations to converge.

      5) Analyze the ICM convergence by using the Kullback-Leibler divergence[38].

      6) Report of experimental results of the proposal in real environments using a commercial robot.

      7) Show the CP-SLAM performance in dynamic environments.

      The article is organized as follows: The CP-SLAM problem is presented in Section 2, which is modeled via a novel MRF in Section 3, and solved by using a novel ICM particularization in Section 4. Simulations are given in Section 5 whereas experimental results in real environments including dynamics objects are shown in Section 6. Conclusions are stated in Section 7.

    • Let $ x_t\in{\cal X} $ be the vector state of the vehicle at time $ t = 0, 1, \cdots, T $. For example, if the vehicle is a differential drive mobile robot (DDMR), then its pose is characterized by the state vector $ x_t = [x_{t, x},\;x_{t, y},\; x_{t, \theta}]^{\rm T}$, where $ (x_{t, x},\;x_{t, y})\in{\bf R}^2 $ is its location and $ x_{t, \theta}\in[0, 2\pi) $ is its orientation at time $ t $ (see Fig. 1). Then $ {\cal X} = {\bf R}^2\times[0,\;2\pi) $. Assume without loss of generality that $ x_0 $ is known. Then, $ { x} = \{x_t:t = 1, 2, \cdots, T\}\in{\cal X}^{\rm T} $ represents the robot trajectory.

      Figure 1.  Diagram of a DDMR

      The control action applied to the robot at time $ t = 0, 1, \cdots, T $ is noted with $ u_t $; whereas $ { u} $ denotes the set $ \{u_t:t = 0, 1, \cdots, T\} $. In general, the control actions are motor torques, desired velocities, or functions of them. If the vehicle has perfect tracking of the control actions with disturbance-free movement, then there is a functional relation $ x_t = g(x_{t-1}, u_{t-1}) $ that predicts its future state given the current state and control action. However, $ g(x_{t-1}, u_{t-1}) $ is usually an estimation of $ x_t $. In the DDMR example, the control action could be $ u_t = [\nu_t, \omega_t] $, where $ \nu_t $ and $ \omega_t $ are the linear and angular desired velocities. The DDMR motion can be characterized by the following function:

      $ g(x_{t-1}, u_{t-1}) = x_{t-1}+\Delta t\left[\!\!\begin{array}{c}\nu_{t-1}\cos(x_{t-1, \theta})\\ \nu_{t-1}\sin(x_{t-1, \theta})\\ \omega_{t-1}\end{array}\!\!\right]$

      where $ \Delta t>0 $ is a fixed sampling period.

      Assume that the vehicle operates in an environment in which there are other objects whose spatial distribution is characterized by the set $ { m} = \{m_\ell:\ell\in{\cal L}\}\in{\cal M} $, where $ {\cal M} $ is the set of all the possible maps. The definition of each $ m_\ell $ and $ {\cal L} $ varies depending on the considered map. For example, in an environment with a finite set of landmarks, $ {\cal L} $ can be $ \{1, 2, \cdots, L\} $ and $ m_\ell $ can be the location of the $ \ell $-th landmark (see [37] for more details). In SLAM with a probabilistic map based on grid, $ m_\ell $ is the probability of observing an object in the $ \ell $-th cell of the grid, and $ {\cal L} $ is the index set of the grid. In the CP-SLAM, $ {\cal L}\subseteq {\bf R}^d $, $ m_\ell $ is the probability of observing an object in $ \ell\in {\bf R}^d $, and $ {\cal M} $ is the set of all the probabilistic densities defined on $ {\bf R}^d $. There are many possible definitions for the map depending on the application addressed. For example, for indoor applications, $ { m} $ can contain centers, lengths, and orientations of a segment set.

      Consider that the robot is equipped with sensors for detecting objects of the environment, and that this information can be synthesized in a set $ z_t = \{z_{t, i}:i = 1, 2, \cdots, n_t\} $ at each time $ t = 0, 1, \cdots, T $. Let $ { z} $ be the set $ \{z_t:t = 0, 1, \cdots, T\} $. For example, a range finder laser estimates the distance and direction to the landmarks located in the observation range (see Fig. 2). This sensory information can be synthesized through $ z_t = \{z_{t, i}:i = 1, \cdots, n_t\} $ at each time $ t $, where each $ z_{t, i} = [z_{t, i, d}, z_{t, i, \theta}] $ contains the robot-object distance $ z_{t, i, d} $, and the direction $ z_{t, i, \theta} $ from which the object is observed. This direction takes values between $ 0 $ (right side of the robot) and $ \pi $ (left side of the robot).

      Figure 2.  SLAM simulation setting

      The SLAM problem consists in looking at the pair $ ({ x},{ m})\in{\cal X}^{\rm T}\times{\cal M} $ that maximizes $ P({ x}, { m}|{ z}, { u}) $, where $ P $ is the probability distribution that characterizes the model. Over this optimization process $ { z} $ and $ { u} $ are neglected, since they are constant during this optimization process.

      In other SLAM versions, like SLAM based on landmarks (see [37]), it is necessary to incorporate a vector of landmark-observation matchings $c_t \!=\! \{c_{t, i}:i \!=\! 1, 2, \cdots, n_t\}$$ \in{\cal C}_t = {\cal L}^{n_t} $, where $ c_{t, i} = \ell $ if $ z_{t, i} $ ($ i $-th observation at time $ t $) corresponds to the label $ \ell $. The same happens in probabilistic SLAM based on grids where the grid-observation matching $ c_{t, i} = \ell $ implies that the observation $ z_{t, i} $ comes from the $ \ell $-th cell of the grid. In these cases, the quantity to optimize is $ P({ x}, { m}, { c}|{ z},{ u}) $.

    • Given a finite set of vertices or nodes $ {\cal V} $, a random field (RF) is an array $ { X} = \{X_v:v\in{\cal V}\} $, where each $ X_v $ is a random variable (RV) that takes values from the set of possible states $ \Omega_v $. Then, the set $ \Omega = \prod_{v\in{\cal V}}\Omega_v $ contains all the possible realizations of $ { X} $. In the CP-SLAM problem $ \Omega = {\cal X}^{\rm T}\times{\cal M} $, since a single node for each $ x_t $ and an extra node for $ { m} $ are considered.

      Let $ X_A $ be the RF composed by the RVs $ X_v $, with $ v\in A\subset{\cal V} $. In the particular case that $ A = {\cal V}-\{v\} $, it will be denoted with $ X_{-v} $. The entry subset of $ \omega\in\prod_{v\in{\cal V}}\Omega_v $ defined over $ A $ is denoted by $ \omega_A\in\prod_{v\in A}\Omega_v $.

      In principle, each $ X_v $ depends probabilistically on all other RVs. However, this can be simplified by defining for each node $ v\in{\cal V} $ a neighborhood $ \partial v\subset{\cal V} $ that determines its radius of conditional dependence. The neighborhood system $ \{\partial v:v\in{\cal V}\} $ must satisfy that 1) $ v\notin\partial v $ $ \forall v $, and 2) $ v'\in\partial v\Leftrightarrow v\in\partial v' $ $ \forall v, v' $ (see Fig. 3 (a)). In this way, a graph $ {\cal G} = ({\cal V}, {\cal E}) $ is defined, where the edge set $ {\cal E}\subseteq{\cal V}\times{\cal V} $ contains all pairs of neighboring nodes. A random field $ { X} $ with associated probability $ P $ defined on a graph $ {\cal G} $ is a Markov random field (MRF) if

      Figure 3.  Diagram of topological concepts defined

      $ P(\omega_v|\omega_{-v}) = P(\omega_v|\omega_{\partial v}), \ \ \ \forall v\in{\cal V}. $

      That is, if the conditional probability of observing $ \omega_v $ given the value on all nodes can be calculated knowing only the state of their neighbors. A subset $ C\subseteq{\cal V} $ is a clique if $ \#C = 1 $ or if $ C\times C\subseteq {\cal E} $ (see Fig. 3(b) and 3(c)). The set of all the cliques of the graph is noted with $ {\cal C} $.

      Fig. 4 diagrams the graph used to contextualize the CP-SLAM problem in the MRF theory. This graph was built following two premises: that it contains the fewest amount possible of edges; and that it contains the cliques of the form $ \{x_{t-1}, x_t\} $ ($ t\geq2 $), and $ \{x_t, { m}\} $ ($ t\geq1 $), which allow it to include a motion model and an observation model to the CP-SLAM problem.

      Figure 4.  Graph of CP-SLAM problem in MRF context

      Consider a family of potentials $ \Phi = \{\Phi_C:C\in{\cal C}\} $, where each $ \Phi_C:\prod_{v\in C}\Omega_v\rightarrow{\bf R} $ is a function such that $ \Phi_C(\omega_C)>\Phi_C(\omega'_C) $ if the configuration $ \omega_C $ is more likely than $ \omega'_C $ in the problem to be modeled.

      The potentials for the motion model are defined by

      $\begin{split} &\Phi_{\{x_1\}}(x_1) = -\|g(x_0, u_0)-x_1\|_R^2\\ & \Phi_{\{x_{t-1}, x_t\}}(x_{t-1}, x_t)= -\|g(x_{t-1}, u_{t-1})-x_t\|_R^2, \ t\geq2 \end{split}$


      where $ R>0 $ is a design matrix, and $ \|\cdot\|_R $ is the Mahalanobis norm. These potentials highlight successive state vectors $ (x_{t-1}, x_t) $ that best match with the motion model and the control actions in ideal conditions. Note that kinematic and dynamic models can be used in these definitions.

      Besides, odometry data $ {\widehat x}_t $ can also be incorporated since it can be helpful when few useful observations feedback the SLAM process. In practice, when $ t\gg0 $ is large, $ {\widehat x}_t $ is not a good estimate of $ x_t $, due to the cumulative odometry errors. However, if $ \tau(\theta) $ is the rotation matrix given by

      $ \tau(\theta) = \left[\!\!\begin{array}{ccc}\cos\theta&\sin\theta&0\\-\sin\theta&\cos\theta&0\\0&0&1\end{array}\!\!\right]$

      then $ \tau({\widehat x}_{t-1, \theta})({\widehat x}_t-{\widehat x}_{t-1}) $ is a good estimator of the odometry step $ \tau(x_{t-1, \theta})(x_t-x_{t-1}) $, and the potentials (1) can be redefined by

      $\begin{split} &\Phi_{\{x_1\}}(x_1) = -\|g(x_0, u_0)-x_1\|_R^2-\|x_1-{\widehat x}_1\|_S^2\\ & \Phi_{\{x_{t-1}, x_t\}}(x_{t-1}, x_t) = -\|g(x_{t-1}, u_{t-1})-x_t\|_R^2- \\ &\quad \|\tau(x_{t-1, \theta})(x_t-x_{t-1})-\tau({\widehat x}_{t-1, \theta})({\widehat x}_t-{\widehat x}_{t-1})\|_S^2,\; t\geq2 \end{split}$


      where $ S>0 $ is a design matrix.

      Each SLAM version contains different potentials to characterize the observation model, which depends on the map model and the sensor used to observe the environment. The observation model employed to cope with the CP-SLAM problem is defined in the next lines. Given the point cloud of observations $ {\cal O}\subseteq {\bf R}^2 $, a 2D density estimation $ { m} $ of the map based on Kernel (KDE)[39, 40] can be performed. Thus, $ {\cal M} $ is the set of all densities defined over the environment $ {\cal L}\subseteq {\bf R}^2 $. If the point cloud collected so far using any sensor is $ {\cal O} = \{z_j\}_{j = 1}^n $, then KDE estimates the observation probability at $ \ell\in{\cal L} $ with

      $ m_\ell\propto f_{{ m}}(\ell)= \frac{1}{n}\sum\limits_{j = 1}^nK(H^{-1}(\ell-z_j))$


      where $ K $ is the Kernel and $ H>0 $ is the bandwidth matrix. In this paper, $ K $ is the bivariate Gaussian density and $ H = {\rm diag}(h_x, h_y)>0 $. At each sampling period, it is only required to calculate $ m_{z_{t, i}} $, $ i = 1, 2, \cdots, n_t $, and therefore, the full calculation of $ { m} $ is only needed when showing results. This methodology is used in [41] for mapping out-of-structure objects in an agricultural environment.

      The maps based on point clouds continuously grow and require the elimination of outliers and non-informative repetitive observations to make possible its processing or even its storage. An appropriate recursive subsampling of the database allows it to remain bounded and updated. In Appendix, it is shown a novel recursive subsampling method that successfully fulfills these objectives, and furthermore, delivers the same pertinence probability of the subsample to any other obtained observations no matter when they were obtained. This novel characteristic is not present in standard iterative subsampling algorithms.

      The potentials chosen to characterize the observation model for CP-SLAM are

      $ \Phi_{\{x_t, { m}\}}(x_t, { m}): = \frac{Q}{n_t}\sum\limits_{i = 1}^{n_t}f_{{ m}}(h(x_t,\; z_{t, i})), \ \ \ t\geq1$

      where $ Q>0 $ is a design constant and $ h $ is the function defined as

      $ h(x_t, z_{t, i}) = \left[\!\!\begin{array}{c}x_{t, x}\\ x_{t, y}\end{array}\!\!\right] +z_{t, i, d}\left[\!\!\begin{array}{c}\cos\left(z_{t, i, \theta}+x_{t, \theta}-\dfrac{\pi}{2}\right)\\ \sin\left(z_{t, i, \theta}+x_{t, \theta}-\dfrac{\pi}{2}\right)\end{array}\!\!\right]. $


      These potentials prioritize configuration linkages $ (x_t, { m}) $ that best match with the observation model under ideal conditions (see Fig. 5).

      Figure 5.  Robot pose and variables based on laser measurements that characterize the relative position of the detected objects

      Outlier observations produce zones with low probability, which can be deleted by using a map model. Given a threshold $ \vartheta>0 $, the potential chosen to characterize the model is defined by $ \Phi_{\{{ m}\}}({ m}) = 0 $ if there exists some $ \ell\in{\cal L} $ such that $ f_{{ m}}(\ell)<\vartheta $; and $ \Phi_{\{{ m}\}}({ m})\gg0 $ for the other maps. Then, the map potential is constant and large enough to always choose a desired map as optimal.

      The matrices $ R $, $ Q $ and $ S $ regulate the influence of the potentials according to the confidence degree of the involved sensors. All these potentials are responsible for the prevailing of those states $ x_t $ and the map $ { m} $ that best explain the observations $ z_t $, that best suit the control actions $ u_t $, and that best fit to the sensory data. The potentials for the remaining cliques are defined equal to a null potential.

      GPS information is not considered here to highlight the ICM ability to solve SLAM problems. This modeling methodology has great flexibility when exploiting specific characteristics of the problem allowing a straightforward inclusion of the sensory fusion, or to consider multi-robot designs (see [37] for more details).

      From the theorem of Hammersley-Clifford[25, 26], the probability or Gibbs measure $ P $ over $ \Omega $ associated to the MRF X is

      $ P(\omega) = \frac{1}{Z}\exp\left\{\sum\limits_{C\in{\cal C}}\Phi_C(\omega_C)\right\} $


      where $ Z= \sum_{\omega\in\Omega}\exp\{\sum_{C\in{\cal C}}\Phi_C(\omega_C)\} $ is a normalizing constant. Then, from (5) and algebraic steps, it can be proved that the conditional probabilities are

      $ P(\omega_v|\omega_{-v})\propto\exp\left\{\sum\limits_{C\in{\cal C}:v\in C}\Phi_C(\omega_C)\right\} $


      where $ \omega_C $ is composed of the corresponding entries of $ \omega_v $ and $ \omega_{-v} $. From (6), the conditional probabilities associated to CP-SLAM problem are given by

      $\begin{split} & P({{x}_{t}}|{{{ x}}_{-t}}, { m}, { z}, { u})=P({{x}_{t}}|{{x}_{t-1}}, {{x}_{t+1}}, { m}, {{z}_{t}}, {{u}_{t}})\propto\\ & \quad\exp \left\{ -\|g({{x}_{t-1}},{{u}_{t-1}})-{{x}_{t}}\|_{R}^{2}-\|g({{x}_{t}},{{u}_{t}})-{{x}_{t+1}}\|_{R}^{2}- \right.\\ & \quad\|\tau ({{x}_{t-1, \theta }})({{x}_{t}}-{{x}_{t-1}})-\tau ({{{\hat{x}}}_{t-1, \theta }})({{{\hat{x}}}_{t}}-{{{\hat{x}}}_{t-1}})\|_{S}^{2}- \\ & \quad\|\tau ({{x}_{t, \theta }})({{x}_{t+1}}-{{x}_{t}})-\tau ({{{\hat{x}}}_{t, \theta }})({{{\hat{x}}}_{t+1}}-{{{\hat{x}}}_{t}})\|_{S}^{2}+ \\ & \quad\left. \frac{Q}{{{n}_{t}}}\sum\limits_{i=1}^{{{n}_{t}}}{{{f}_{ m}}}(h({{x}_{t}}, {{z}_{t, i}})) \right\} \end{split} $



      $P({ m}|{ x},{ z}, { u})\!\propto\!\exp\left\{\!\Phi_{\{{ m}\}}({ m})\!+\!\sum\limits_{t = 0}^{T}\frac{Q}{n_t}\sum\limits_{i = 1}^{n_t}f_{{ m}}(h(x_t, z_{t, i}))\!\right\}.$

    • Maximizing (5) is not viable due to the cardinality of $ \Omega $. Instead, the joint probability of an MRF can be locally maximized using ICM[36], which is a deterministic algorithm that iteratively maximizes the conditional probability of each variable given the last update of the other variables. For example, in the CP-SLAM problem, the equations (7) and (8) should be iteratively maximized. In many applications, this local maximum gives very satisfactory results if the initial configuration is properly chosen. The general convergence proof can be found in [36], and an intuitive explanation of it is given in [37].

      The solution to the CP-SLAM problem via ICM is summarized below. Superscripts indicate to which iteration corresponds the update of the state in question. The procedure requires the use of Algorithm 2 which is given in Appendix.

      Algorithm 1. ICM for CP-SLAM.

      1) ICM Initialization:

      a) Set $ {\cal O}^{(0)} = \{h(x_0, z_{0, i}):i = 1, \cdots, n_0\} $

      b) Calculate $ x_1^{(0)} $ with

      $ \begin{split} x_{1}^{(0)}= &\arg \underset{x}{\mathop{\min }}\,\left\{ \|g({{x}_{0}},{{u}_{0}})-x\|_{R}^{2}+\|x-{{{\hat{x}}}_{1}}\|_{S}^{2} \right. -\\ &\left. \frac{Q}{{{n}_{1}}}\sum\limits_{i=1}^{{{n}_{1}}}{{{f}_{{{ m}^{(0)}}}}}(h(x,{{z}_{1,i}})) \right\}. \end{split} $


      c) Update the observation set $ {\cal O}^{(0)} $ with the data $ \{h(x_1^{(0)}, z_{1, i}):i = 1, \cdots, n_1\} $ by using Algorithm 2.

      d) Calculate $ x_2^{(0)} $ with

      $\begin{split} x_2^{(0)} = & \arg \mathop {\min }\limits_x \left\{ {\left\| {g(x_1^{(0)},{u_1}) - x} \right\|_R^2} \right.+\\ & \|\tau(x_{1, \theta}^{(0)})(x-x_1^{(0)})-\tau({\widehat x}_{t-1, \theta})({\widehat x}_t-{\widehat x}_{t-1})\|_S^2-\\ & \left.\frac{Q}{n_2}\sum\limits_{i = 1}^{n_2}f_{{ m}^{(0)}}(h(x, z_{2, i}))\right\}. \end{split}$


      e) Update $ {\cal O}^{(0)} $ with the observation set $ \{h(x_2^{(0)}, z_{2, i}): $$i = 1, \cdots, n_2\}$ by using Algorithm 2.

      Repeat this procedure until $ t = T $.

      f) Calculate $ { m}^{(0)} $ using the bounded set of observations $ {\cal O}^{(0)} $ with

      $\begin{split}{{{ m}}^{(0)}}= &\arg \mathop {\max }\limits_{ m} \left\{ {{\Phi }_{\{{ m}\}}}({ m}) \right.+\\ &\left.\frac{Q}{\#{\cal O}^{(0)}}\sum\limits_{z_{t, i}\in{\cal O}^{(0)}}f_{{ m}}(h(x_t^{(0)}, z_{t, i}))\right\}.\end{split}$


      2) Generate a visit scheme, i.e., choose in what order the model states will be updated. It can be: $ x_1,\,x_2,\,\cdots $, $ x_{T} $ and m.

      3) Perform a sweep, i.e., visit the nodes according to the order specified in 2), and update the states maximizing the corresponding conditional probabilities (7) and (8). The $ n $-th iteration of the ICM-update procedure is

      a) Set $ {\cal O}^{(n)} = \{h(x_0, z_{0, i}):i = 1, \cdots, n_0\} $.

      $\begin{split}\;\;\;\; b) \; x_{t}^{(n)}= &\arg \mathop {\min }\limits_x \left\{ \|g(x_{t-1}^{(n)}, {{u}_{t-1}})-x\|_{R}^{2} \right.+\\ &\|g(x, u_t)-x_{t+1}^{(n-1)}\|_R^2+\\ &\|\tau(x_{t-1, \theta}^{(n)})(x-x_{t-1}^{(n)})-\!\tau({\widehat x}_{t-1, \theta})({\widehat x}_t-{\widehat x}_{t-1})\|_S^2+\\ &\|\tau(x_\theta)(x_{t+1}^{(n-1)}-x)-\tau({\widehat x}_{t, \theta})({\widehat x}_{t+1}-{\widehat x}_t)\|_S^2-\\ &\left. \frac{Q}{n_t} \sum\limits_{i = 1}^{n_t}f_{{ m}^{(n-1)}}(h(x, z_{t, i}))\right\}. \end{split}$


      c) Update $ {\cal O}^{(n)} $ with the observation set $ \{h(x_t^{(n)}, z_{t, i}):$$ i = 1, \cdots, n_t\} $ by using Algorithm 2.

      d) Repeat the steps b) and c) for $ t = 1:T $.

      e) Calculate $ { m}^{(n)} $ using the bounded set of observations $ {\cal O}^{(n)} $ with

      $\begin{split}{{ m}^{(n)}} = & \arg \mathop {\max }\limits_{ m} \left\{ {{\Phi _{\{ { m}\} }}({ m})} \right.+\\ & \left.\frac{Q}{\#{\cal O}^{(n)}}\sum\limits_{z_{t, i}\in{\cal O}^{(n)}}f_{{ m}}(h(x_t^{(n)}, z_{t, i}))\right\}. \end{split}$


      4) Repeat the Step 3) as often as wished.

      In this paper, the numerical resolution of (9), (10) and (12) is performed by using the Matlab optimized function fminunc, with initial point setting by the kinematic model and default options. A two-step procedure is used to solve (11) and (13). In the first step, the second term is optimized using the KDE estimation of $ { m} $. This can be performed by using the Matlab function kde2d[42], which is a fast and accurate state-of-the-art bivariate kernel density estimator with diagonal bandwidth matrix. In the second step, it is set to $ m_\ell = 0 $ for each $ \ell\in{\cal L} $ such that $ f_{{ m}}(\ell)<\vartheta $. This procedure is also performed to finish the Step 1) of Algorithm 1.

    • Simulation results for the CP-SLAM are presented in this Section. The MobileSim program is used to simulate the dynamic model of a robot Pioneer p3dx-sh and Matlab is used for calculations. A C++ interface based on shared memory resource is used for the Matlab-MobileSim connection allowing reading of the laser range measurement and the sending of online control actions. A PC with a Processor Intel®CoreTM i7-4790 3.60 GHz and 16 GB RAM is used to simulate.

      Fig. 2 shows the considered environment and the initial pose of the DDMR. The robot follows a preset irregular path and acquires laser range-finder measurements of the environment. The maximum range observations are deleted since these indicate that there are no objects in these directions. Although adjacent observations come from the same object, the algorithm considers them as different observations. The chosen parameters are

      1) $ Q = 1 $ and $ S = R = { I}_3 $ to give equal weight to each model;

      2) the probability threshold is $ \vartheta = 0.01 $.

      Fig. 6 shows simulation results of the Algorithm 1 where ICM initial estimations appear in Fig. 6(a), and ICM estimations for the iterations 10, 100 and 200 are reported in Figs. 6(b), 6(c) and 6(d), respectively. In these graphics, the probabilistic maps are plotted in color shades, in which probabilistic maximums are indicated with red, intermediate probabilities are represented with yellow, and zones without probability are shown in white. The estimation of the robot trajectory converges practically in the initialization. On the other hand, the probabilistic map needs some iterations to concentrate probabilities around the object locations. Notice that ICM already delivers quality results on its on-line initialization. However, this algorithm can improve these results later employing ICM iterations that run in parallel whereas the robot is navigating and acquiring new observations.

      Figure 6.  Simulation results

      In order to reveal convergence, a criterion for calculating errors must be determined, which varies depending on whether the actual location of the objects is known or not. When the actual map is not known, each $ { m}^{(n)} $, $ n = 1, 2, \cdots $, is compared with the initial estimate $ { m}^{(0)} $ by using the Kullback-Leibler divergence[38]. Fig. 6(e) shows the evolution of these KL divergences, whose asymptotic behavior implies the desired convergence. The index curve shows that ICM only modifies to $ { m}^{(0)} $ in the first iterations. However, the ideal situation is to know the actual location of the objects, which cannot be represented by single points due to the fact they occupy physical spaces. Thus, it is convenient to choose a probability density that represents the set of actual locations. A Gaussian mixture with means in the real positions, covariance matrices equal to $ 0.01{ I}_2 $, and uniform mixture proportions is selected. The KL divergence evolution between this Gaussian mixture and each $ { m}^{(n)} $, $ n = 1, 2, \cdots $, is shown in Fig. 6(f). The initial map estimation is close to the Gaussian mixture, but subsequent estimates are better. The small index fluctuations are due to the random process of subsampling.

      As in [37], the observation range must be reduced to 10 meters to reduce the mapping errors. Several simulations were run considering many parameter combinations, and the best results were obtained by giving greater weight to the observations, for example, setting $ Q = 10 $. Fig. 7 shows the outputs obtained by introducing these modifications, i.e., $ Q = 10 $ and maximum observation range equal to 10 m. It is observed that ICM converges practically in its initialization (see Fig. 7(a)) presenting few changes in the subsequent ICM iterations (see ICM estimations of the iteration 200 in Fig. 7(b)). The divergence evolution curve reveals this as well (see Fig. 7(c)).

      Figure 7.  Simulation results reducing the observation range to 10 m and considering Q = 10

      Other important parameters to be considered are the bandwidths $ h_x $ and $ h_y $ of the Kernel-based map estimator (3), which determine the amplitude of the probabilistic clusters that shape the estimated map. The Matlab function kde2d[42] also gives an optimal estimation of the bandwidths, which must be initialized with small values to assign low probabilities to erroneous observations and avoid cumulative estimation errors. In this paper, the computation of probabilities for the ICM initialization are made using $ h_x = h_y = 0.1 $, and for the ICM iterations, the bandwidths estimated by the Matlab function kde2d, based on the most recent estimated map, are used. The estimation evolutions of the bandwidth parameters are shown in Fig. 7(d).

    • The ICM performance is verified through experimental results, for which a Pioneer 3AT robot from ActiveMedia equipped with an outdoor laser range-finder sensor from Sick (see Fig. 8(a)) is teleoperated in an open field setting. Fig. 8(b) shows the used environment consisting of 11 static reference points: a teleoperator, a videographer, 8 cones forming a regular polygon with a radius of 7.12 m, and a cone on the octagon center.

      Figure 8.  Experimental setting

      The parameters chosen are

      1) $ R = S = { I}_3 $ and $ Q = 10 $;

      2) the probability threshold is set in $ \vartheta = 0.01 $.

      Fig. 9 shows the initial ICM estimations and the estimation corresponding to the iteration 20 of ICM, where the probabilistic maps are also plotted in color shades. Moreover, it is incorporated the feature-based map and the trajectory estimated in [37]. Fig. 9(c) shows the evolution of the KL divergence between each $ { m}^{(n)} $, $ n = 1, 2, \cdots $, and the Gaussian mixture with means in the localizations estimated in [37], covariance matrices $ 0.01{ I}_2 $, and uniform mixture proportions. Note that iteration-by-iteration the maps estimated by both proposals are more coincident. On the other hand, the estimates of the DDMR trajectory are similar.

      Figure 9.  Experimental results

      Finally, an experiment is performed in order to show the potential of the CP-SLAM to work in environments with dynamic objects. Sixteen cones are placed in the environment as shown Fig. 10(a). Besides, during experimentation, a person repeatedly back and forth between the second and third cone rows (see Fig. 10(a)). The robot is programmed to follow a path among the cone rows as shown Fig. 10(a) avoiding collisions. For this, the path tracking controller given in [43] and the obstacle avoider based on impedance given in [44] are used.

      Figure 10.  Experimental results with a dynamic object

      Fig. 10(b) shows the experimental results using the same parameters selected for the previous experiment. The dynamic object generates a point cloud less compact than that the generated by the static objects, which produce large zones with less probability. The subsampling procedure filters observations of dynamic objects in atypical places, wrong or eventual observations. Here again, ICM iterations are merely proposed as an initialization improver, whose use introduces slight improvements if it is adequately initialized.

    • This paper presents and models the CP-SLAM problem via MRF, and then solves it by considering ICM. This modeling procedure is simple, flexible, and can be used to work with dynamic environments. Furthermore, it is also reported its capability to filter atypical and redundant observations employing a quick subsampling process. Point cloud-based mapping allows the avoidance of environment discretization based on grids, and, for this reason, it does not limit the spatial resolution of the estimated map. The proposed procedure works on-line if only the initial high-quality ICM estimations are considered, but it has the additional capacity to improve them iteratively. The obtained algorithm is proved under simulation and experiments in dynamic environments using real robots, showing not only its feasibility but also its good performance.

    • The size of the observation set is continually increasing throughout the data acquisition, and therefore, the probability computation can become a very expensive task if a mechanism for eliminating outliers and non-informative observations is not incorporated. Although most methodologies incorporate processes for the detection and elimination of outliers, it is impossible to eliminate them altogether. On the other hand, it is very common that there are repeated or redundant observations. For both cases, the observation subsampling reduces atypical observations considerably, and in the same proportion, it eliminates redundant observations.

      Subsampling is the random process of reducing from $ K $ to $ k<K $ the sample size (subsampling performed in one step), in which each observation remains in the subsample with probability $\dfrac{k}{K} $. Suppose now that there is a bound of $ C $ observations for the storage capacity. Then, it is required to perform a recursive subsampling, i.e., a subsampling must be performed every time the number of stored observations exceeds the bound. In each subsampling, each observation has a probability $\dfrac{C}{N} $ of remaining in the subsample. Algorithm 2 produces a recursive subsampling equivalent to a subsampling performed in one step, in which all observations (regardless of when they were acquired) have the same probability of belonging to the point cloud. This purpose is fulfilled through an acceptance-rejection method of new observations.

      At the beginning of the algorithm, the point cloud is empty ($ N = 0 $), and the probability of accepting new observations is $ p = 1 $. The algorithm incorporates all the observations acquired to the point cloud until the storage bound is exceeded $ N>C $. When this happens, $ C $ observations are subsampled without replacement, and the remaining observations are deleted. The Matlab function randsample performs this task. Then, the probability of belonging to the point cloud is updated to $ p = \dfrac{C}{N} $, and $ N $ is set to $ C $. From this moment, all new observations must undergo an acceptance-rejection process in order to equal their probability of belonging to the point cloud with the probability of belonging of the observations acquired previously. This probability of belonging $ p $ decreases every time a new subsampling is performed (updating the acceptance probability). In this way, all observations (original and new) have the same probability $ p $ of belonging to the point cloud. The acceptance-rejection procedure can be efficiently performed in matrix form.

      Algorithm 2. Recursive subsampling.

      Set $ N = 0 $      /* size of the point cloud */

      Set $ p = 1 $ /* prob. for the acceptance-rejection procedure */

      For $ t = 1: {\rm end}$   /* for each sampling period */

        Include the $ n_t $ new observations in the point cloud.

        $ N = N+n_t $

        If $ N>C $ /* if the storage bound is exceeded */

         For $ i = 1:n_t $  /* for each new observation */

          $ u = rand(0, 1) $    /* random number */

          If $ u>p $ /* acceptance-rejection procedure */

           $ i $-th new observation is deleted *

           $ N = N-1 $



         If $ N>C $ /* if the storage bound is still exceeded a subsampling of size $ C $ is performed. */

          $ p = p\dfrac{C}{N} $  /* update the acceptance prob. */

          $ N = C $




    • This research was financed by Argentinean National Council for Scientific Research (CONICET) and the National University of San Juan (UNSJ) of Argentina. We also thank NVIDIA Corporation for their support.

Reference (44)



    DownLoad:  Full-Size Img  PowerPoint