Skip to main content
SpringerLink
Log in

An Integrated Localization, Motion Planning and Obstacle Avoidance Algorithm in Belief Space

Intelligent Service Robotics volume 14, pages 235–250 (2021)Cite this article

Abstract

As robots are being increasingly used in close proximity to humans and objects, it is imperative that robots operate safely and efficiently under real-world conditions. Yet, the environment is seldom known perfectly. Noisy sensors and actuation errors compound to the errors introduced while estimating features of the environment. We present a novel approach (1) to incorporate these uncertainties for robot state estimation and (2) to compute the probability of collision pertaining to the estimated robot configurations. The expression for collision probability is obtained as an infinite series, and we prove its convergence. An upper bound for the truncation error is also derived, and the number of terms required is demonstrated by analyzing the convergence for different robot and obstacle configurations. We evaluate our approach using two simulation domains which use a roadmap-based strategy to synthesize trajectories that satisfy collision probability bounds.

This is a preview of subscription content, access via your institution.

Access options

Buy single article

Instant access to the full article PDF.

USD 39.95

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7

Notes

  1. The application of Bayes filter to the localization problem is called Markov localization [31].

  2. Note that the concepts discussed here are applicable to any sensor used for robot localization. In particular, in this work (Section 5) we use a laser range finder and beacons that give signal measurements in terms of the distance to the beacons.

  3. Sigma hulls are convex hulls of the geometry of individual robot links transformed according to the sigma points in joint space [18].

  4. For example, the approach in [5] computes a value lower than the actual when the robot state covariance is small.

  5. For the comparison, the approaches in [5, 19] have been reproduced to the best of our understanding and the reproduced codes (including numerical integration and our approach) can be found here—https://bitbucket.org/1729antony/comparison_cp_methods/src/master/

References

  1. Ali-Akbar Agha-Mohammadi, Suman Chakravorty, Amato Nancy M (2014) FIRM: sampling-based feedback motion-planning under motion uncertainty and imperfect measurements. Int J Robot Res 33(2):268–304

    Article  Google Scholar 

  2. Brian Axelrod, Pack Kaelbling Leslie, Tomás Lozano-Pérez (2018) Provably safe robot navigation with obstacle uncertainty. Int J Robot Res 37(13–14):1760–1774

    Google Scholar 

  3. Adam Bry and Nicholas Roy. Rapidly-exploring random belief trees for motion planning under uncertainty. In IEEE International Conference on Robotics and Automation, pages 723–730, 2011

  4. Xu Chu Ding, Alessandro Pinto, and Amit Surana. Strategic planning under uncertainties via constrained markov decision processes. In IEEE International Conference on Robotics and Automation, pages 4568–4575, 2013

  5. Du Toit Noel E, Burdick Joel W (2011) Probabilistic collision checking with chance constraints. IEEE Trans Robot 27(4):809–815

    Article  Google Scholar 

  6. Martin Grötschel, László Lovász, Alexander Schrijver (1988) Geometric algorithms and combinatorial optimization. Springer-Verlag, New York

    MATH  Google Scholar 

  7. Astghik Hakobyan, Chan Kim Gyeong, Insoon Yang (2019) Risk-aware motion planning and control using CVaR-constrained optimization. IEEE Robot Automat Lett 4(4):3924–3931

    Article  Google Scholar 

  8. Janson Lucas, Schmerling Edward, Pavone Marco (2018) Monte Carlo motion planning for robot trajectory optimization under uncertainty in Robotics Research. Springer, New York

    Google Scholar 

  9. Ashkan M Jasour and Brian C Williams. Risk contours map for risk bounded motion planning under perception uncertainties. Robotics: Science and Systems, 2019

  10. Pack Kaelbling Leslie, Littman Michael L, Cassandra Anthony R (1998) Planning and acting in partially observable stochastic domains. Artif Intell 101(1–2):99–134

    MathSciNet  MATH  Google Scholar 

  11. Leslie Pack Kaelbling and Tomás Lozano-Pérez (2013) Integrated task and motion planning in belief space. The International Journal of Robotics Research 32(9–10):1194–1227

  12. Sertac Karaman, Emilio Frazzoli (2011) Sampling-based algorithms for optimal motion planning. Int J Robot Res 30(7):846–894

    Article  Google Scholar 

  13. Kavraki LE, Svestka P, Latombe JC, Overmars MH (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Trans Robot Automat 12(4):566–580

    Article  Google Scholar 

  14. Kotz Samuel, Johnson Norman L, Boyd DW (1967) Series representations of distributions of quadratic forms in normal variables. I. Central case. The Annals of Mathematical Statistics 38(3):823–837

  15. Samuel Kotz, Norman L Johnson, and DW Boyd. Series representations of distributions of quadratic forms in normal variables ii. non-central case. The Annals of Mathematical Statistics, 38(3):838–848, 1967

  16. Hanna Kurniawati and Vinay Yadav. An Online POMDP Solver for Uncertainty Planning in Dynamic Environment. In Robotics Research: The 16th International Symposium ISRR, pages 611–629, 2016

  17. Alain Lambert, Dominique Gruyer, and Guillaume Saint Pierre. A fast Monte Carlo algorithm for collision probability estimation. In 10th IEEE International Conference on Control, Automation, Robotics and Vision, pages 406–411, 2008

  18. Alex Lee, Yan Duan, Sachin Patil, John Schulman, Zoe McCarthy, Jur Van Den Berg, Ken Goldberg, and Pieter Abbeel. Sigma hulls for gaussian belief space planning for imprecise articulated robots amid obstacles. In IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 5660–5667, 2013

  19. Chonhyon Park, Park Jae S, Dinesh Manocha (2018) Fast and bounded probabilistic collision detection for high-DOF trajectory planning in dynamic environments. IEEE Trans Automat Sci Eng 15(3):980–991

    Article  Google Scholar 

  20. Shashank Pathak, Antony Thomas, Vadim Indelman (2018) A unified framework for data association aware robust belief space planning and perception. Int J Robot Res 37(2–3):287–315

    Google Scholar 

  21. Sachin Patil, Jur Van Den Berg, and Ron Alterovitz. Estimating probability of collision for safe motion planning under Gaussian motion and sensing uncertainty. In IEEE International Conference on Robotics and Automation, pages 3238–3244, 2012

  22. Robert Platt Jr, Russ Tedrake, Leslie Kaelbling, and Tomas Lozano-Perez. Belief space planning assuming maximum likelihood observations. In Proceedings of Robotics: Science and Systems, Zaragoza, Spain, 2010

  23. Samuel Prentice, Nicholas Roy (2009) The belief roadmap: efficient planning in belief space by factoring the covariance. Int J Robot Res 28(11–12):1448–1465

    Google Scholar 

  24. Serge B Provost and AM Mathai. Quadratic forms in random variables: theory and applications. M. Dekker, 1992

  25. Elon Rimon, Boyd Stephen P (1997) Obstacle collision detection using best ellipsoid fit. J Intell Robot Syst 18(2):105–126

    Article  Google Scholar 

  26. Dorsa Sadigh and Ashish Kapoor. Safe control under uncertainty with probabilistic signal temporal logic. Robotics: Science and Systems, 2016

  27. Oren Salzman, Brian Hou, and Siddhartha Srinivasa. Efficient motion planning for problems lacking optimal substructure. In Twenty-Seventh International Conference on Automated Planning and Scheduling, 2017

  28. Luke Shimanuki and Brian Axelrod. Hardness of 3D Motion Planning Under Obstacle Uncertainty. Workshop on Algorithmic Foundations of Robotics, 2018

  29. Antony Thomas, Fulvio Mastrogiovanni, and Marco Baglietto. Task-Motion Planning for Navigation in Belief Space. In The International Symposium on Robotics Research, 2019

  30. Antony Thomas, Fulvio Mastrogiovanni, Marco Baglietto (2020) Towards Multi-Robot Task-Motion Planning for Navigation in Belief Space. In European Starting AI Researchers’ Symposium, CEUR

  31. Thrun Sebastian, Burgard Wolfram, Fox Dieter (2005) Probabilistic robotics. MIT press, Cambridge

    MATH  Google Scholar 

  32. Den Berg Van, Jur Patil Sachin, Ron Alterovitz (2012) Motion planning under uncertainty using iterative local optimization in belief space. Int J Robot Res 31(11):1263–1278

    Article  Google Scholar 

  33. Hai Zhu, Javier Alonso-Mora (2019) Chance-constrained collision avoidance for mavs in dynamic environments. IEEE Robot Automat Lett 4(2):776–783

    Article  Google Scholar 

Download references

Author information

Authors and Affiliations

  1. Department of Informatics, Bioengineering, Robotics, and Systems Engineering, University of Genoa, Via All’Opera Pia 13, 16145, Genoa, Italy

    Antony Thomas, Fulvio Mastrogiovanni & Marco Baglietto

Authors
  1. Antony Thomas
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Fulvio Mastrogiovanni
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Marco Baglietto
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Antony Thomas.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Reprints and Permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Thomas, A., Mastrogiovanni, F. & Baglietto, M. An Integrated Localization, Motion Planning and Obstacle Avoidance Algorithm in Belief Space. Intel Serv Robotics 14, 235–250 (2021). https://doi.org/10.1007/s11370-021-00359-6

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s11370-021-00359-6

Keywords

search

Navigation