Motion Planning for Active Data Association and Localization in Non-Gaussian Belief Spaces

Part of the Springer Proceedings in Advanced Robotics book series (SPAR, volume 13)


This paper presents a method for motion planning under uncertainty to resolve situations where ambiguous data associations result in a multimodal hypothesis on the robot state. Simultaneous localization and planning for a lost (or kidnapped) robot requires that given little to no a priori pose information, a planner should generate actions such that future observations allow the localization algorithm to recover the correct pose of a mobile robot with respect to a global reference frame. We present a Receding Horizon approach, to plan actions that sequentially disambiguate a multimodal belief to achieve tight localization on the correct pose in finite time. In our method, disambiguation is achieved through active data associations by picking target states in the map which allow distinctive information to be observed for each belief mode and creating local feedback controllers to visit the targets. Experimental results are presented for a kidnapped physical ground robot operating in an artificial maze-like environment.


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Fox, D., Burgard, W., Thrun, S.: Active markov localization for mobile robots. Robotics and Autonomous Systems 25(34) (1998) 195 – 207 Autonomous Mobile Robots.Google Scholar
  2. 2.
    Jensfelt, P., Kristensen, S.: Active global localization for a mobile robot using multiple hypothesis tracking. Robotics and Automation, IEEE Transactions on 17(5) (Oct 2001) 748–760Google Scholar
  3. 3.
    Gasparri, A., Panzieri, S., Pascucci, F., Ulivi, G.: A hybrid active global localisation algorithm for mobile robots. In: Robotics and Automation, 2007 IEEE International Conference on. (April 2007) 3148–3153Google Scholar
  4. 4.
    Reuter, J.: Mobile robot self-localization using pdab. In: Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE International Conference on. Volume 4. (2000) 3512–3518 vol.4Google Scholar
  5. 5.
    Roumeliotis, S.I., Bekey, G.A.: Bayesian estimation and kalman filtering: A unified framework for mobile robot localization. In: Robotics and Automation, 2000. Proceedings. ICRA’00. IEEE International Conference on. Volume 3., IEEE (2000) 2985–2992Google Scholar
  6. 6.
    Thrun, S., Burgard, W., Fox, D.: Probabilistic robotics. MIT press (2005)Google Scholar
  7. 7.
    Prentice, S., Roy, N.: The belief roadmap :Efficient planning in belief space and by factoring the covariance. International Journal of Robotics Research 28 (11-12) (October 2009)Google Scholar
  8. 8.
    Bry, A., Roy, N.: Rapidly-exploring random belief trees for motion planning under uncertainty. In: ICRA. (2011) 723–730Google Scholar
  9. 9.
    van den Berg, J., Abbeel, P., Goldberg, K.: LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information. In: Proceedings of Robotics: Science and Systems (RSS). (June 2010)Google Scholar
  10. 10.
    Kurniawati, H., Bandyopadhyay, T., Patrikalakis, N.: Global motion planning under uncertain motion, sensing, and environment map. Autonomous Robots 33(3) (2012) 255–272Google Scholar
  11. 11.
    Platt, R., Tedrake, R., Kaelbling, L., Lozano-Perez, T.: Belief space planning assuming maximum likelihood observatoins. In: Proceedings of Robotics: Science and Systems (RSS). (June 2010)Google Scholar
  12. 12.
    Agha-mohammadi, A., Chakravorty, S., Amato, N.: FIRM: Sampling-based feedback motion planning under motion uncertainty and imperfect measurements. International Journal of Robotics Research 33(2) (2014) 268–304Google Scholar
  13. 13.
    Agha-mohammadi, A., Agarwal, S., Mahadevan, A., Chakravorty, S., Tomkins, D., Denny, J., Amato, N.: Robust online belief space planning in changing environments: Application to physical mobile robots. In: IEEE Int. Conf. Robot. Autom. (ICRA), Hong Kong, China (2014)Google Scholar
  14. 14.
    Kavraki, L., Svestka, P., Latombe, J., Overmars, M.: Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation 12(4) (1996) 566–580Google Scholar
  15. 15.
    Platt, R., Kaelbling, L., Lozano-Perez, T., , Tedrake, R.: Efficient planning in non-Gaussian belief space and its application to robot grasping. In: Proc. of International Symposium of Robotics Research, (ISRR). (2011)Google Scholar
  16. 16.
    Platt, R., Kaelbling, L., Lozano-Perez, T., Tedrake, R.: Non-gaussian belief space planning: Correctness and complexity. In: IEEE International Conference on Robotics and Automation (ICRA). (2012)Google Scholar
  17. 17.
    Platt, R.: Convex receding horizon control in non-Gaussian belief space. In: Workshop on the Algorithmic Foundations of Robotics (WAFR). (2012)Google Scholar
  18. 18.
    Rafieisakhaei, M., Tamjidi, A., Chakravorty, S., Kumar, P.: Feedback motion planning under non-gaussian uncertainty and non-convex state constraints. In: 2016 IEEE International Conference on Robotics and Automation (ICRA), IEEE (2016) 4238–4244Google Scholar
  19. 19.
    Dudek, G., Romanik, K., Whitesides, S.: Localizing a robot with minimum travel. SIAM Journal on Computing 27(2) (1998) 583–604Google Scholar
  20. 20.
    O’Kane, J.M., LaValle, S.M.: Localization with limited sensing. IEEE Transactions on Robotics 23(4) (Aug 2007) 704–716Google Scholar
  21. 21.
    Pilania, V., Gupta, K.: A localization aware sampling strategy for motion planning under uncertainty. In: Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on. (Sept 2015) 6093–6099Google Scholar
  22. 22.
    Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research 30(7) (June 2011) 846–894Google Scholar
  23. 23.
    Bertsekas, D.: Dynamic Programming and Optimal Control: 3rd Ed. Athena Scientific (2007)Google Scholar
  24. 24.
    Chakravorty, S., Erwin, R.S.: Information space receding horizon control. In: IEEE Symposium on Adaptive Dynamic Programming And Reinforcement Learning (ADPRL). (April 2011)Google Scholar
  25. 25.
    He, R., Brunskill, E., Roy, N.: Efficient planning under uncertainty with macro-actions. Journal of Artifical Intelligence Research 40 (February 2011) 523–570Google Scholar
  26. 26.
    Patil, S., van den Berg, J., Alterovitz, R.: Estimating probability of collision for safe motion planning under gaussian motion and sensing uncertainty. In: Robotics and Automation (ICRA), 2012 IEEE International Conference on. (May 2012) 3238–3244Google Scholar
  27. 27.
    Agarwal, S., Tamjidi, A., Chakravorty, S.: Video of M3P physical experiments.
  28. 28.
    Agarwal, S., Tamjidi, A., Chakravorty, S.: Motion planning for global localization in nongaussian belief spaces. (2015) arXiv:1511.04634 [cs.RO].
  29. 29.
    Garrido-Jurado, S., Muoz-Salinas, R., Madrid-Cuevas, F., Marn-Jimnez, M.: Automatic generation and detection of highly reliable fiducial markers under occlusion. Pattern Recognition 47(6) (2014) 2280 – 2292Google Scholar

Copyright information

© Springer Nature Switzerland AG 2020

Authors and Affiliations

  1. 1.Dept. of Aerospace EngineeringTexas A&M UniversityTexasUnited States

Personalised recommendations