FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots

Euclidean Signed Distance Field (ESDF) is useful for online motion planning of aerial robots since it can easily query the distance and gradient information against obstacles. Fast incrementally built ESDF map is the bottleneck for conducting real-time motion planning. In this paper, we investigate this problem and propose a mapping system called FIESTA to build global ESDF map incrementally. By introducing two independent updating queues for inserting and deleting obstacles separately, and using Indexing Data Structures and Doubly Linked Lists for map maintenance, our algorithm updates as few as possible nodes using a BFS framework. Our ESDF map has high computational performance and produces near-optimal results. We show our method outperforms other up-to-date methods in term of performance and accuracy by both theory and experiments. We integrate FIESTA into a completed quadrotor system and validate it by both simulation and onboard experiments. We release our method as open-source software for the community 11https://github.com/hlx1996/FIESTA

[1]  Yi Lin,et al.  Online Safe Trajectory Generation for Quadrotors Using Fast Marching Method and Bernstein Basis Polynomial , 2018, 2018 IEEE International Conference on Robotics and Automation (ICRA).

[2]  Shaojie Shen,et al.  Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[3]  Wolfram Burgard,et al.  OctoMap : A Probabilistic , Flexible , and Compact 3 D Map Representation for Robotic Systems , 2010 .

[4]  Fei Gao,et al.  Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments , 2018, J. Field Robotics.

[5]  Roland Siegwart,et al.  Topomap: Topological Mapping and Navigation Based on Visual SLAM Maps , 2017, 2018 IEEE International Conference on Robotics and Automation (ICRA).

[6]  Matthias Nießner,et al.  Real-time 3D reconstruction at scale using voxel hashing , 2013, ACM Trans. Graph..

[7]  Andrew W. Fitzgibbon,et al.  KinectFusion: Real-time dense surface mapping and tracking , 2011, 2011 10th IEEE International Symposium on Mixed and Augmented Reality.

[8]  Roland Siegwart,et al.  Continuous-time trajectory optimization for online UAV replanning , 2016, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[9]  Siddhartha S. Srinivasa,et al.  CHOMP: Gradient optimization techniques for efficient motion planning , 2009, 2009 IEEE International Conference on Robotics and Automation.

[10]  Fei Gao,et al.  Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight , 2019, IEEE Robotics and Automation Letters.

[11]  Yi Lin,et al.  Gradient-based online safe trajectory generation for quadrotor flight in complex environments , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[12]  Roland Siegwart,et al.  Voxblox: Incremental 3D Euclidean Signed Distance Fields for on-board MAV planning , 2016, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[13]  Daniel P. Huttenlocher,et al.  Distance Transforms of Sampled Functions , 2012, Theory Comput..

[14]  Roland Siegwart,et al.  Sparse 3D Topological Graphs for Micro-Aerial Vehicle Planning , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[15]  Russ Tedrake,et al.  NanoMap: Fast, Uncertainty-Aware Proximity Queries with Lazy Search Over Local 3D Data , 2018, 2018 IEEE International Conference on Robotics and Automation (ICRA).

[16]  Daniel Cremers,et al.  Real-time trajectory replanning for MAVs using uniform B-splines and a 3D circular buffer , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).