Perception-Aware Motion Planning via Multiobjective Search on GPUs

  • Brian IchterEmail author
  • Benoit Landry
  • Edward Schmerling
  • Marco Pavone
Conference paper
Part of the Springer Proceedings in Advanced Robotics book series (SPAR, volume 10)


In this paper we describe a framework towards computing well-localized, robust motion plans through the perception-aware motion planning problem, whereby we seek a low-cost motion plan subject to a separate constraint on perception localization quality. To solve this problem we introduce the Multiobjective Perception-Aware Planning (MPAP) algorithm which explores the state space via a multiobjective search, considering both cost and a perception heuristic. This framework can accommodate a large range of heuristics, allowing those that capture the history dependence of localization drift and represent complex modern perception methods. We present two such heuristics, one derived from a simplified model of robot perception and a second learned from ground-truth sensor error, which we show to be capable of predicting the performance of a state-of-the-art perception system. The solution trajectory from this heuristic-based search is then certified via Monte Carlo methods to be well-localized and robust. The additional computational burden of perception-aware planning is offset by GPU massive parallelization. Through numerical experiments the algorithm is shown to find well-localized, robust solutions in about a second. Finally, we demonstrate MPAP on a quadrotor flying perception-aware and perception-agnostic plans using Google Tango for localization, finding the quadrotor safely executes the perception-aware plan every time, while crashing in over 20% of the perception-agnostic runs due to loss of localization.


Planning Perception-aware Robust Learning Parallel Quadrotor 


  1. 1.
    Kaelbling, L.P., Littman, M.L., Cassandra, A.R.: Planning and acting in partially observable stochastic domains. Artif. Intell. (1998)Google Scholar
  2. 2.
    Kurniawati, H., Hsu, D., Lee, W.S.: SARSOP: efficient point-based POMDP planning by approximating optimally reachable belief spaces. In: Robotics: Science and Systems (2008)Google Scholar
  3. 3.
    Prentice, S., Roy, N.: The belief roadmap: efficient planning in linear POMDPs by factoring the covariance. Int. J. Robot. Res. (2009)Google Scholar
  4. 4.
    Bry, A., Roy, N.: Rapidly-exploring random belief trees for motion planning under uncertainty. In: Proceedings of the IEEE Conference on Robotics and Automation (2011)Google Scholar
  5. 5.
    van den Berg, J., Patil, S., Alterovitz, R.: Motion planning under uncertainty using iterative local optimization in belief space. Int. J. Robot. Res. (2012)Google Scholar
  6. 6.
    Patil, S., Kahn, G., Laskeym, M., Schulman, J., Goldberg, K., Abbeel, P.: Scaling up gaussian belief space planning through covariance-free trajectory optimization and automatic differentiation. In: Workshop on Algorithmic Foundations of Robotics (2014)Google Scholar
  7. 7.
    Indelman, V., Carlone, L., Dellaert, F.: Planning in the continuous domain: a generalized belief space approach for autonomous navigation in unknown environments. Int. J. Robot. Res. (2015)Google Scholar
  8. 8.
    van den Berg, J., Abbeel, P., Goldberg, K.: LQG-MP: optimized path planning for robots with motion uncertainty and imperfect state information. Int. J. Robot. Res. (2011)Google Scholar
  9. 9.
    Agha-mohammadi, A., Agarwal, S., Chakravorty, S., Amato, N.M.: Simultaneous localization and planning for physical mobile robots via enabling dynamic replanning in belief space. IEEE Trans. Robot. (2016). arXiv:1510.07380 (submitted)
  10. 10.
    Platt, R., Tedrake, R., Kaelbling, L., Lozano-Perez, T.: Belief space planning assuming maximum likelihood observations. In: Robotics: Science and Systems (2010)Google Scholar
  11. 11.
    Aloimonos, J., Weiss, I., Brandyopadhyay, A.: Active vision. Int. J. Comput. Vis. (1988)Google Scholar
  12. 12.
    Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT Press, Cambridge (2005)Google Scholar
  13. 13.
    Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., Reid, I., Leonard, J.: Past, present, and future of simultaneous localization and mapping: towards the robust-perception age. IEEE Trans. Robot. (2016)Google Scholar
  14. 14.
    Sadat, S.A., Chutskoff, K., Jungic, D., Wawerla, J., Vaughan, R.: Feature-rich path planning for robust navigation of MAVs with Mono-SLAM. In: Proceedings of the IEEE Conference on Robotics and Automation (2014)Google Scholar
  15. 15.
    Costante, G., Forster, C., Delmerico, J., Valigi, P., Scaramuzza, D.: Perception-aware path planning. IEEE Trans. Robot. (2017). arXiv:1605.04151 (submitted)
  16. 16.
    Carlone, L., Lyons, D.: Uncertainty-constrained robot exploration: a mixed-integer linear programming approach. In: Proceedings of the IEEE Conference on Robotics and Automation (2014)Google Scholar
  17. 17.
    Ichter, B., Schmerling, E., Agha-mohammadi, A., Pavone, M.: Real-time stochastic kinodynamic motion planning via multiobjective search on GPUs. In: Proceedings of the IEEE Conference on Robotics and Automation (2017)Google Scholar
  18. 18.
    Kavraki, L.E., Švestka, P., Latombe, J.-C., Overmars, M.H.: Probabilistic roadmaps for path planning in high-dimensional spaces. IEEE Trans. Robot. Autom. (1996)Google Scholar
  19. 19.
    Amato, N.M., Dale, L.K.: Probabilistic roadmap methods are embarrassingly parallel. In: Proceedings of the IEEE Conference on Robotics and Automation (1999)Google Scholar
  20. 20.
    Janson, L., Schmerling, E., Pavone, M.: Monte Carlo motion planning for robot trajectory optimization under uncertainty. In: International Symposium on Robotics Research (2015)Google Scholar
  21. 21.
    Davison, A.J., Murray, D.W.: Simultaneous localization and map-building using active vision. IEEE Trans. Pattern Anal. Mach. Intell. (2002)Google Scholar
  22. 22.
    Scaramuzza, D., Fraundorfer, F.: Visual odometry part I: the first 30 years and fundamentals. IEEE Robot. Autom. Mag. (2011)Google Scholar
  23. 23.
    LaValle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)Google Scholar
  24. 24.
    Armeni, I., Sener, O., Zamir, A.R., Jiang, H., Brilakis, I., Fischer, M., Savarese, S.: 3D semantic parsing of large-scale indoor spaces. In: IEEE Conference on Computer Vision and Pattern Recognition (2016)Google Scholar
  25. 25.
    PX4 Development Team: PX4 autopilot.

Copyright information

© Springer Nature Switzerland AG 2020

Authors and Affiliations

  • Brian Ichter
    • 1
    Email author
  • Benoit Landry
    • 1
  • Edward Schmerling
    • 2
  • Marco Pavone
    • 1
  1. 1.Department of Aeronautics and AstronauticsStanford UniversityStanfordUSA
  2. 2.Institute for Computational and Mathematical EngineeringStanford UniversityStanfordUSA

Personalised recommendations