Real-Time Planning with Multi-Fidelity Models for Agile Flights in Unknown Environments

Real-Time Planning with Multi-Fidelity Models for Agile Flights in Unknown Environments

Jesus Tordesillas, Brett T. Lopez, John Carter, John Ware and Jonathan P. How J. Tordesillas, B. Lopez, J. How are with the Aerospace Controls Laboratory, MIT, 77 Massachusetts Ave., Cambridge, MA, USA {jtorde, btlopez, jhow} J. Carter and J. Ware are with the MIT Robust Robotics Group. {jakeware, jcarter}

Autonomous navigation through unknown environments is a challenging task that entails real-time localization, perception, planning, and control. UAV’s with this capability have begun to emerge in the literature with advances in lightweight sensing and computing. Although the planning methodologies vary from platform to platform, many algorithms adopt a hierarchical planning architecture where a slow, low-fidelity global planner guides a fast, high-fidelity local planner. However, in unknown environments, this approach can lead to erratic or unstable behavior due to the interaction between the global planner, whose solution is changing constantly, and the local planner; a consequence of not capturing higher-order dynamics in the global plan. This work proposes a planning framework in which multi-fidelity models are used to reduce the discrepancy between the local and global planner. Our approach uses high-, medium-, and low-fidelity models to compose a path that captures higher-order dynamics while remaining computationally tractable. In addition, we address the interaction between a fast planner and a slower mapper by considering the sensor data not yet fused into the map during the collision check. This novel mapping and planning framework for agile flights is validated in simulation, showing replanning times of 5-40 ms in cluttered environments, a value that is 3-30 times faster than similar state-of-the-art planning algorithms.

I Introduction

UAV autonomous navigation in unknown environments has received special interest in the last few years because of its unlimited applications, ranging from aerial surveying and inspection to search and rescue. However, these applications are often reduced to low-speed flights due to the current limitations and low rates of the state-of-the-art mappers and planners. The inherent non-convexity of the path planning optimization problem, together with the high mapping and planning rate needed for agile flights make this problem especially hard. This work presents a novel framework to perform high-rate mapping and planning in unknown environments suitable for agile maneuvers, addressing the fundamental problem between the interaction of a global planner and a local planner.

Computational tractability of the planning problem leads to the use of a low-fidelity global planner that computes a cost-to-go (CTG) needed by the high-fidelity local planner. However, the fact that the global planner does not account for the dynamics results in erratic behaviors when the world model is changing rapidly. There is therefore a need of an accurate CTG calculation that captures both the global environment and the dynamic feasibility, maintaining relatively low computation times at the same time.

Fig. 1: Global optimum and our method. When the map is completely known, the optimal trajectory computed using the approach of [1] is shown in blue (). The red trajectory () is the solution found by our method, where the world is not known and it is being discovered as the UAV flies forward. The grid is m m, and the sensing range is 10 m.

Moreover, the choice of the representation of the environment and the size of the “global” map (larger scale than the sensor FOV and the local representation, but typically does not contain all information observed to reduce effort) have a significant impact on the computational cost, but for most systems updates of these models cannot be done at the sensor frame rates ( Hz) and updates are typically slower than the re-plan rate. Thus a second design challenge is how to combine the global knowledge (available at a slower rate) with the high-rate local information in the planner representation of the environment. Finally, the state-of-the-art mappers and planners run onboard at Hz, so the the third key challenge is how to optimize the planning and mapping algorithms to achieve higher rates, suitable for aggressive flights.

This work addresses these challenges with the following contributions:

  • A novel formulation of the planning problem that takes into account the dynamics of the vehicle in the cost-to-go calculation to solve the negative interaction that usually occurs between the global and local planners when operating in unknown environments.

  • A lightweight fused-based mapping framework using a sliding map to reduce the estimation error influence that runs onboard fusing a depth image in 50 ms.

  • An integration of a high-rate planner with a slower-rate mapper, with a collision check algorithm that accounts for both the most recent fused information and the available sensed data not included in that map.

  • Simulation experiments showing agile flights in completely unknown cluttered environments, achieving replanning times of 5-40 ms.

    I-a Hardware Experiments

    The UAV used for the future hardware experiments is shown in Fig. 2. All the perception, planning and control runs onboard, and the position, velocity, attitude, and IMU biases are estimated by fusing propagated IMU measurements with an external motion capture system via a Kalman filter. The mapping fusion times achieved onboard are ms and ms for depth image resolutions of and respectively. All these experiments are available in this link and also in the video accompanying this work.

    Fig. 2: UAV used in the experiments. It is equipped with a Qualcomm® SnapDragon Flight, an Nvidia® Jetson TX2 and an Intel® RealSense Depth Camera D435.

    Ii Conclusions

    This work presented a novel planning a mapping framework suitable for agile flights in unknown environments. The key properties of this framework is its ability to solve the interaction between the global planner and the local planner considering the dynamics of the vehicle, and its ability to address efficiently the integration between a fast planner and a slower mapper. The replanning and mapping rates are several times faster than the state of the art.


    Thanks to Boeing Research & Technology for support of the hardware, to Helen Oleynikova (ASL-ETH) for the data of the forest simulation, and to Pablo Tordesillas (ETSAM-UPM) for his help with some figures of this paper. Supported in part by Defense Advanced Research Projects Agency (DARPA) as part of the Fast Lightweight Autonomy (FLA) program, HR0011-15-C-0110. Views expressed here are those of the authors, and do not reflect the official views or policies of the Dept. of Defense or the U.S. Government.


    • [1] Sikang Liu, Kartik Mohta, Nikolay Atanasov, and Vijay Kumar. Search-based motion planning for aggressive flight in se (3). IEEE Robotics and Automation Letters, 3(3):2439–2446, 2018.
    • [2] Daniel Mellinger and Vijay Kumar. Minimum snap trajectory generation and control for quadrotors. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pages 2520–2525. IEEE, 2011.
    • [3] Michiel J Van Nieuwstadt and Richard M Murray. Real-time trajectory generation for differentially flat systems. International Journal of Robust and Nonlinear Control: IFAC-Affiliated Journal, 8(11):995–1020, 1998.
    • [4] Charles Richter, Adam Bry, and Nicholas Roy. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In Robotics Research, pages 649–666. Springer, 2016.
    • [5] Giuseppe Loianno, Chris Brunner, Gary McGrath, and Vijay Kumar. Estimation, control, and planning for aggressive flight with a small quadrotor with a single camera and imu. IEEE Robotics and Automation Letters, 2(2):404–411, 2017.
    • [6] Mark W Mueller, Markus Hehn, and Raffaello D’Andrea. A computationally efficient motion primitive for quadrocopter trajectory generation. IEEE Transactions on Robotics, 31(6):1294–1310, 2015.
    • [7] Sikang Liu, Nikolay Atanasov, Kartik Mohta, and Vijay Kumar. Search-based motion planning for quadrotors using linear quadratic minimum time control. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, pages 2872–2879. IEEE, 2017.
    • [8] Helen Oleynikova, Michael Burri, Zachary Taylor, Juan Nieto, Roland Siegwart, and Enric Galceran. Continuous-time trajectory optimization for online uav replanning. In Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on, pages 5332–5339. IEEE, 2016.
    • [9] Helen Oleynikova, Zachary Taylor, Roland Siegwart, and Juan Nieto. Safe local exploration for replanning in cluttered unknown environments for microaerial vehicles. IEEE Robotics and Automation Letters, 3(3):1474–1481, 2018.
    • [10] Arthur Richards and Jonathan P How. Aircraft trajectory planning with collision avoidance using mixed integer linear programming. In American Control Conference, 2002. Proceedings of the 2002, volume 3, pages 1936–1941. IEEE, 2002.
    • [11] Changliu Liu, Chung-Yen Lin, and Masayoshi Tomizuka. The convex feasible set algorithm for real time optimization in motion planning. SIAM Journal on Control and Optimization, 56(4):2712–2733, 2018.
    • [12] Sikang Liu, Michael Watterson, Kartik Mohta, Ke Sun, Subhrajit Bhattacharya, Camillo J Taylor, and Vijay Kumar. Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robotics and Automation Letters, 2(3):1688–1695, 2017.
    • [13] Michael Watterson, Sikang Liu, Ke Sun, Trey Smith, and Vijay Kumar. Trajectory optimization on manifolds with applications to so (3) and r 3 s2. Robotics: Science and Systems (RSS), 2018.
    • [14] Yuanqi Mao, Michael Szmuk, and Behcet Acikmese. Successive convexification: A superlinearly convergent algorithm for non-convex optimal control problems. arXiv preprint arXiv:1804.06539, 2018.
    • [15] Federico Augugliaro, Angela P Schoellig, and Raffaello D’Andrea. Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. In Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on, pages 1917–1922. IEEE, 2012.
    • [16] John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Jia Pan, Sachin Patil, Ken Goldberg, and Pieter Abbeel. Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research, 33(9):1251–1270, 2014.
    • [17] Debadeepta Dey, Kumar Shaurya Shankar, Sam Zeng, Rupesh Mehta, M Talha Agcayazi, Christopher Eriksen, Shreyansh Daftry, Martial Hebert, and J Andrew Bagnell. Vision and learning for deliberative monocular cluttered flight. In Field and Service Robotics, pages 391–409. Springer, 2016.
    • [18] Peter R Florence, John Carter, Jake Ware, and Russ Tedrake. Nanomap: Fast, uncertainty-aware proximity queries with lazy search over local 3d data. arXiv preprint arXiv:1802.09076, 2018.
    • [19] Brett T Lopez and Jonathan P How. Aggressive 3-D collision avoidance for high-speed navigation. In Robotics and Automation (ICRA), 2017 IEEE International Conference on, pages 5759–5765. IEEE, 2017.
    • [20] Brett T Lopez and Jonathan P How. Aggressive collision avoidance with limited field-of-view sensing. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, pages 1358–1365. IEEE, 2017.
    • [21] Boris Lau, Christoph Sprunk, and Wolfram Burgard. Improved updating of euclidean distance maps and voronoi diagrams. In Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, pages 281–286. IEEE, 2010.
    • [22] Helen Oleynikova, Zachary Taylor, Marius Fehr, Roland Siegwart, and Juan Nieto. Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017.
    • [23] Mihail Pivtoraiko, Daniel Mellinger, and Vijay Kumar. Incremental micro-uav motion replanning for exploring unknown environments. In Robotics and Automation (ICRA), 2013 IEEE International Conference on, pages 2452–2458. IEEE, 2013.
    • [24] Jing Chen, Tianbo Liu, and Shaojie Shen. Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments. In Robotics and Automation (ICRA), 2016 IEEE International Conference on, pages 1476–1483. IEEE, 2016.
    • [25] Daniel Damir Harabor, Alban Grastien, et al. Online graph pruning for pathfinding on grid maps. In AAAI, 2011.
    • [26] Jacob Mattingley and Stephen Boyd. Cvxgen: A code generator for embedded convex optimization. Optimization and Engineering, 13(1):1–27, 2012.
    • [27] Jack E Bresenham. Algorithm for computer control of a digital plotter. IBM Systems journal, 4(1):25–30, 1965.
    • [28] Nathan P Koenig and Andrew Howard. Design and use paradigms for gazebo, an open-source multi-robot simulator. In IROS, volume 4, pages 2149–2154. Citeseer, 2004.
    • [29] Andreas Bircher, Mina Kamel, Kostas Alexis, Helen Oleynikova, and Roland Siegwart. Receding horizon” next-best-view” planner for 3d exploration. In Robotics and Automation (ICRA), 2016 IEEE International Conference on, pages 1462–1468. IEEE, 2016.
Comments 0
Request Comment
You are adding the first comment!
How to quickly get a good reply:
  • Give credit where it’s due by listing out the positive aspects of a paper before getting into which changes should be made.
  • Be specific in your critique, and provide supporting evidence with appropriate references to substantiate general statements.
  • Your comment should inspire ideas to flow and help the author improves the paper.

The better we are at sharing our knowledge with each other, the faster we move forward.
The feedback must be of minimum 40 characters and the title a minimum of 5 characters
Add comment
Loading ...
This is a comment super asjknd jkasnjk adsnkj
The feedback must be of minumum 40 characters
The feedback must be of minumum 40 characters

You are asking your first question!
How to quickly get a good answer:
  • Keep your question short and to the point
  • Check for grammar or spelling errors.
  • Phrase it like a question
Test description