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.
Buy single article
Instant access to the full article PDF.
Price excludes VAT (USA)
Tax calculation will be finalised during checkout.
The application of Bayes filter to the localization problem is called Markov localization .
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.
Sigma hulls are convex hulls of the geometry of individual robot links transformed according to the sigma points in joint space .
For example, the approach in  computes a value lower than the actual when the robot state covariance is small.
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/
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
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
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
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
Du Toit Noel E, Burdick Joel W (2011) Probabilistic collision checking with chance constraints. IEEE Trans Robot 27(4):809–815
Martin Grötschel, László Lovász, Alexander Schrijver (1988) Geometric algorithms and combinatorial optimization. Springer-Verlag, New York
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
Janson Lucas, Schmerling Edward, Pavone Marco (2018) Monte Carlo motion planning for robot trajectory optimization under uncertainty in Robotics Research. Springer, New York
Ashkan M Jasour and Brian C Williams. Risk contours map for risk bounded motion planning under perception uncertainties. Robotics: Science and Systems, 2019
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
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
Sertac Karaman, Emilio Frazzoli (2011) Sampling-based algorithms for optimal motion planning. Int J Robot Res 30(7):846–894
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
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
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
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
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
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
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
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
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
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
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
Serge B Provost and AM Mathai. Quadratic forms in random variables: theory and applications. M. Dekker, 1992
Elon Rimon, Boyd Stephen P (1997) Obstacle collision detection using best ellipsoid fit. J Intell Robot Syst 18(2):105–126
Dorsa Sadigh and Ashish Kapoor. Safe control under uncertainty with probabilistic signal temporal logic. Robotics: Science and Systems, 2016
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
Luke Shimanuki and Brian Axelrod. Hardness of 3D Motion Planning Under Obstacle Uncertainty. Workshop on Algorithmic Foundations of Robotics, 2018
Antony Thomas, Fulvio Mastrogiovanni, and Marco Baglietto. Task-Motion Planning for Navigation in Belief Space. In The International Symposium on Robotics Research, 2019
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
Thrun Sebastian, Burgard Wolfram, Fox Dieter (2005) Probabilistic robotics. MIT press, Cambridge
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
Hai Zhu, Javier Alonso-Mora (2019) Chance-constrained collision avoidance for mavs in dynamic environments. IEEE Robot Automat Lett 4(2):776–783
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
About this article
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