A Robust and Efficient Trajectory Planner for Quadrotors

Overview

Fast-Planner

Fast-Planner is developed aiming to enable quadrotor fast flight in complex unknown environments. It contains a rich set of carefully designed planning algorithms.

News:

  • Mar 13, 2021: Code for fast autonomous exploration is available now! Check this repo for more details.

  • Oct 20, 2020: Fast-Planner is extended and applied to fast autonomous exploration. Check this repo for more details.

Authors: Boyu Zhou and Shaojie Shen from the HUKST Aerial Robotics Group, Fei Gao from ZJU FAST Lab.

Complete videos: video1, video2, video3. Demonstrations about this work have been reported on the IEEE Spectrum: page1, page2, page3 (search for HKUST in the pages).

To run this project in minutes, check Quick Start. Check other sections for more detailed information.

Please kindly star this project if it helps you. We take great efforts to develope and maintain it 😁 😁 .

Table of Contents

1. Quick Start

The project has been tested on Ubuntu 16.04(ROS Kinetic) and 18.04(ROS Melodic). Take Ubuntu 18.04 as an example, run the following commands to setup:

  sudo apt-get install libarmadillo-dev ros-melodic-nlopt
  cd ${YOUR_WORKSPACE_PATH}/src
  git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git
  cd ../ 
  catkin_make

You may check the detailed instruction to setup the project. After compilation you can start the visualization by:

  source devel/setup.bash && roslaunch plan_manage rviz.launch

and start a simulation (run in a new terminals):

  source devel/setup.bash && roslaunch plan_manage kino_replan.launch

You will find the random map and the drone in Rviz. You can select goals for the drone to reach using the 2D Nav Goal tool. A sample simulation is showed here.

2. Algorithms and Papers

The project contains a collection of robust and computationally efficient algorithms for quadrotor fast flight:

  • Kinodynamic path searching
  • B-spline-based trajectory optimization
  • Topological path searching and path-guided optimization
  • Perception-aware planning strategy (to appear)

These methods are detailed in our papers listed below.

Please cite at least one of our papers if you use this project in your research: Bibtex.

All planning algorithms along with other key modules, such as mapping, are implemented in fast_planner:

  • plan_env: The online mapping algorithms. It takes in depth image (or point cloud) and camera pose (odometry) pairs as input, do raycasting to update a probabilistic volumetric map, and build an Euclidean signed distance filed (ESDF) for the planning system.
  • path_searching: Front-end path searching algorithms. Currently it includes a kinodynamic path searching that respects the dynamics of quadrotors. It also contains a sampling-based topological path searching algorithm to generate multiple topologically distinctive paths that capture the structure of the 3D environments.
  • bspline: A implementation of the B-spline-based trajectory representation.
  • bspline_opt: The gradient-based trajectory optimization using B-spline trajectory.
  • active_perception: Perception-aware planning strategy, which enable to quadrotor to actively observe and avoid unknown obstacles, to appear in the future.
  • plan_manage: High-level modules that schedule and call the mapping and planning algorithms. Interfaces for launching the whole system, as well as the configuration files are contained here.

Besides the folder fast_planner, a lightweight uav_simulator is used for testing.

3. Setup and Config

Prerequisites

  1. Our software is developed and tested in Ubuntu 16.04(ROS Kinetic) and 18.04(ROS Melodic). Follow the documents to install Kinetic or Melodic according to your Ubuntu version.

  2. We use NLopt to solve the non-linear optimization problem. The uav_simulator depends on the C++ linear algebra library Armadillo. The two dependencies can be installed by the following command, in which ${ROS_VERSION_NAME} is the name of your ROS release.

sudo apt-get install libarmadillo-dev ros_${ROS_VERSION_NAME}_nlopt

Build on ROS

After the prerequisites are satisfied, you can clone this repository to your catkin workspace and catkin_make. A new workspace is recommended:

  cd ${YOUR_WORKSPACE_PATH}/src
  git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git
  cd ../
  catkin_make

If you encounter problems in this step, please first refer to existing issues, pull requests and Google before raising a new issue.

Now you are ready to run a simulation.

Use GPU Depth Rendering (can be skipped optionally)

This step is not mandatory for running the simulations. However, if you want to run the more realistic depth camera in uav_simulator, installation of CUDA Toolkit is needed. Otherwise, a less realistic depth sensor model will be used.

The local_sensing package in uav_simulator has the option of using GPU or CPU to render the depth sensor measurement. By default, it is set to CPU version in CMakeLists:

set(ENABLE_CUDA false)
# set(ENABLE_CUDA true)

However, we STRONGLY recommend the GPU version, because it generates depth images more like a real depth camera. To enable the GPU depth rendering, set ENABLE_CUDA to true, and also remember to change the 'arch' and 'code' flags according to your graphics card devices. You can check the right code here.

    set(CUDA_NVCC_FLAGS 
      -gencode arch=compute_61,code=sm_61;
    ) 

For installation of CUDA, please go to CUDA ToolKit

4. Run Simulations

Run Rviz with our configuration firstly:

  
  source devel/setup.bash
  roslaunch plan_manage rviz.launch

Then run the quadrotor simulator and Fast-Planner. Several examples are provided below:

Kinodynamic Path Searching & B-spline Optimization

In this method, a kinodynamic path searching finds a safe, dynamically feasible, and minimum-time initial trajectory in the discretized control space. Then the smoothness and clearance of the trajectory are improved by a B-spline optimization. To test this method, run:

  
  source devel/setup.bash
  roslaunch plan_manage kino_replan.launch

Normally, you will find the randomly generated map and the drone model in Rviz. At this time, you can trigger the planner using the 2D Nav Goal tool. When a point is clicked in Rviz, a new trajectory will be generated immediately and executed by the drone. A sample is displayed below:

Related algorithms are detailed in this paper.

Topological Path Searching & Path-guided Optimization

This method features searching for multiple trajectories in distinctive topological classes. Thanks to the strategy, the solution space is explored more thoroughly, avoiding local minima and yielding better solutions. Similarly, run:

  
  source devel/setup.bash
  roslaunch plan_manage topo_replan.launch

then you will find the random map generated and can use the 2D Nav Goal to trigger the planner:

Related algorithms are detailed in this paper.

Perception-aware Replanning

The code will be released after the publication of associated paper.

5. Use in Your Application

If you have successfully run the simulation and want to use Fast-Planner in your project, please explore the files kino_replan.launch or topo_replan.launch. Important parameters that may be changed in your usage are contained and documented.

Note that in our configuration, the size of depth image is 640x480. For higher map fusion efficiency we do downsampling (in kino_algorithm.xml, skip_pixel = 2). If you use depth images with lower resolution (like 256x144), you might disable the downsampling by setting skip_pixel = 1. Also, the depth_scaling_factor is set to 1000, which may need to be changed according to your device.

Finally, for setup problem, like compilation error caused by different versions of ROS/Eigen, please first refer to existing issues, pull request, and Google before raising a new issue. Insignificant issue will receive no reply.

6. Updates

  • Oct 20, 2020: Fast-Planner is extended and applied to fast autonomous exploration. Check this repo for more details.

  • July 5, 2020: We will release the implementation of paper: RAPTOR: Robust and Perception-aware Trajectory Replanning for Quadrotor Fast Flight (submitted to TRO, under review) in the future.

  • April 12, 2020: The implementation of the ICRA2020 paper: Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths is available.

  • Jan 30, 2020: The volumetric mapping is integrated with our planner. It takes in depth image and camera pose pairs as input, do raycasting to fuse the measurements, and build a Euclidean signed distance field (ESDF) for the planning module.

Acknowledgements

We use NLopt for non-linear optimization.

Licence

The source code is released under GPLv3 license.

Disclaimer

This is research code, it is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of merchantability or fitness for a particular purpose.

Comments
  • Few questions regarding the kino_replan demo

    Few questions regarding the kino_replan demo

    First of all, thank you for sharing this great piece of work!

    I am testing the kino dynamic part and I have few questions for you:

    • I do not have cuda available so I use CPU only and I saw in this case that this is the pcl_render_node.cpp which is used to create point clouds.

    In kino_replan.launch you mentioned that we need to choose between depth image or point cloud but actually in sdf_map.cpp, the /sdf_map/cloud topic is subscribed anyway so comment <arg name="cloud_topic" value="/pcl_render_node/cloud"/> in the kino_replan.launch file leads to an error at start.

    • When I run the demo, one thing I observe is that very often the target / traj to be track by the drone is really far ahead, see for instance the pic below:

    Capture du 2020-09-04 10-39-53

    and sometimes the drone is flying thru obstacles: Capture du 2020-09-03 17-50-38

    Do you know how can I handle these issues (or is it because my computer is too slow)? How to provide a new traj which is within a known small range from the current drone pose? fsm/thresh_replan is a parameter to tell when to replan and has no effect on the trajectory tracking.

    • the parameter traj_server/time_forward (kino_replan.launch) is not used in the code so far, do you plan to use it later on?

    • in the demo it is not possible to provide different z values unless we change the planning mode (2) and provide fixed waypoints? Thanks again for the great work

    opened by FaboNo 10
  • Avoiding large obstacles

    Avoiding large obstacles

    Hi @ZbyLGsc I am trying to use topo_replan in Gazebo simulation setup where I have some large obstacles like building and warehouses. With the default parameters of topo_replan, the drone doesn't find a good path around, e.g. large building (see attached picture) and ends up crashing into it. I would like to know what parameters can be tuned to be able to avoid such large obstacles (not necessarily flat objects).

    Thanks gazebo

    opened by mzahana 9
  • some question about how to push node into open set in kinodynamic_astar.cpp

    some question about how to push node into open set in kinodynamic_astar.cpp

    In file kinodynamic_astar.cpp, the code to judge whether a node has been in the closed set is: Eigen::Vector3i pro_id = posToIndex(pro_state.head(3)); PathNodePtr pro_node = expanded_nodes_.find(pro_id); The code only consider about the positon of the node while ignore the vecility. Is it possible that, a node in the most optimal path can not be push into the open set because there has been another node with same position and different vecility?

    opened by tiemuhua 7
  • Add a description of the project

    Add a description of the project

    Hi guys, you have a very interesting project. I want to try flying in the woods with your algorithm. However, your package descriptions are sorely lacking. Can we expect a description of the project in the near future?

    opened by GigaFlopsis 7
  • Sending the UAV to multiple points without using 2D Nav Goal on rviz

    Sending the UAV to multiple points without using 2D Nav Goal on rviz

    I'm trying to make the UAV go on a set path. I changed the flight_type to 2 in kino_replan.launch to use and set the preset points in kino_alogorithm.xml and kino_replan.launch. But during execution I still have to use the 2D Nav Goal button in rviz to make the UAV navigate to the next preset point.

    Is there a way to make it automatically go through all the set points.

    Thanks

    opened by bharat787 6
  • Depth image based obstacle avoidance not working

    Depth image based obstacle avoidance not working

    I have generated depth images and camera pose in the world frame from a high fidelity simulator and fed them into fast planner. However the planner is unable to detect or identify these images as obstacles it needs to avoid. I ensured the camera specifics across both the systems are in tune and replaced the controller quad dynamics to that of the simulator quadrotor. I can not understand what am I doing wrong!

    This is a high priority task and any help will be greatly appreciated.

    opened by Stavya1993 6
  • questions about FastPlannerManager::planYaw()

    questions about FastPlannerManager::planYaw()

    Hi, I have some questions about the yaw plan function.

    1. How is this derived? states2pts << 1.0, -dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw, 1.0, 0.0, -(1 / 6.0) * dt_yaw * dt_yaw, 1.0, dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw;
    2. the size of control points is seg_num + 3, why ? I find that In calcWaypointsCost function there are only seg_num+2 control points being used to calc cost. Can I undestand that the extra point is used to contrain end_yaw? But I dont know how (states2pts * end_yaw and states2pts * start_yaw) works, as mentioned in ques 1. Thanks!
    opened by WangChanglong17 6
  • Can this great project be used for 2D navigation of mobile robots?

    Can this great project be used for 2D navigation of mobile robots?

    Thank you very much for this great open source project. I want to use it in 2D navigation of mobile robot. Do you think it is suitable? Any suggestions?Thanks.

    opened by shaoxiang 6
  • Error during Compilation: cannot find -lpose_utils

    Error during Compilation: cannot find -lpose_utils

    Hi. I am trying to compile this package using catkin build. Environment is Ubuntu 18 with ROS melodic. I am getting the following error. Could you please give some hints on how to solve this? Thanks

    Errors     << so3_disturbance_generator:make /home/aaal/catkin_ws/logs/so3_disturbance_generator/build.make.000.log                                                                                                                   
    /usr/bin/ld: cannot find -lpose_utils
    collect2: error: ld returned 1 exit status
    make[2]: *** [/home/aaal/catkin_ws/devel/lib/so3_disturbance_generator/so3_disturbance_generator] Error 1
    make[1]: *** [CMakeFiles/so3_disturbance_generator.dir/all] Error 2
    make: *** [all] Error 2
    
    
    opened by mzahana 6
  • Can't make the map size larger

    Can't make the map size larger

    Hey all, I am applying the solution in my own application and change the map size to 20010010 in kino_replan.launch. However, the planner said that:

    ...
    process[fast_planner_node-1]: started with pid [22489]
    process[traj_server-2]: started with pid [22491]
    process[waypoint_generator-3]: started with pid [22492]
    process[odom_visualization-4]: started with pid [22503]
    hit: 0.619039
    miss: -0.619039
    min log: -1.99243
    max: 2.19722
    thresh log: 1.38629
    [ WARN] [1627410159.670548166]: [Traj server]: ready.
    [fast_planner_node-1] process has died [pid 22489, exit code -9, cmd /home/rui/fast_ws/devel/lib/plan_manage/fast_planner_node /odom_world:=/airsim_node/drone_1/odom_local_ned /sdf_map/odom:=/airsim_node/drone_1/odom_local_ned /sdf_map/cloud:=/airsim_node/drone_1/points /sdf_map/pose:=/pcl_render_node/camera_pose /sdf_map/depth:=/pcl_render_node/depth __name:=fast_planner_node __log:=/home/rui/.ros/log/3029c27e-ef06-11eb-8be3-e86a647151f5/fast_planner_node-1.log].
    log file: /home/rui/.ros/log/3029c27e-ef06-11eb-8be3-e86a647151f5/fast_planner_node-1*.log
    

    The error messages didn't provide any helpful information and the log file can't find when i try to open the mentioned logs. Have anyone or FAST team faced this problem? I'd be grateful if you someone give me any recommendations.

    opened by Kevinlibaba 4
  • Abnormal Behaviour using kino_replan

    Abnormal Behaviour using kino_replan

    FastPlannerFail

    As you can see, even though there is no obstacle in front, the algorithm performs an obstacle avoiding maneuver. Just wanted to confirm how I can fix this behaviour.

    Also wanted to extend my kudos to developers of this amazing algorithm!

    opened by Stavya1993 4
  • Is the intention of this segment code to keep the initial acceleration A0 unchanged, when A1 or A2 excceds the pysical limit?

    Is the intention of this segment code to keep the initial acceleration A0 unchanged, when A1 or A2 excceds the pysical limit?

    https://github.com/HKUST-Aerial-Robotics/Fast-Planner/blob/f4a74cf87eac86419983a092c580acc7e495848a/fast_planner/bspline/src/non_uniform_bspline.cpp#L294

      if (i == 1 || i == 2) {
        // cout << "acc i: " << i << endl;
        for (int j = 2; j <= 5; ++j) {
          u_(j) += double(j - 1) * t_inc;
        }
    
        for (int j = 6; j < u_.rows(); ++j) {
          u_(j) += 4.0 * t_inc;
        }
    
      }
    
    opened by xiaozialei 0
  • axis of /sdf_map/occupancy is wired....

    axis of /sdf_map/occupancy is wired....

    video : https://youtu.be/JdJ_o9QwtR0

    Currently I am linking ardupilot and its simulation.

    Each tf is created, connected, and data is exchanged through a topic. All axes of tf coincide. In the meantime, data from /sdf_map/occupancy of the local map does not come in in the middle. And the axis of the data is changed. Please check the video.

    how to fix that.....?

    ------add-----------

    video : https://youtu.be/5Hnz6qOWarc

    I noticed that the topic is not updated when the topic occurs vertically. When the topic was entered, it started to come out in a normal direction. But I don't know why that topic is not being entered.

    opened by romaster93 0
  • No path planning

    No path planning

    I used these commands in two terminal and set a goal

    1. source devel/setup.bash roslaunch plan_manage rviz.launch

    2. source devel/setup.bash roslaunch plan_manage kino_replan.launch This is the result I got . It's not planning any trajectory.

    Screenshot from 2022-03-31 10-46-47

    opened by chethana-amin 0
  • about the

    about the "w_time_ " parameter in KinodynamicAstar::estimateHeuristic()

    I want to ask about the intention for adding the "w_time_ " parameter in the KinodynamicAstar::estimateHeuristic(). In this function, the trajectory cost J(T) equals to -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + "w_time_" * t, but in the paper "Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight,2019" the cost J(T) doesn't have the w_time_ parameter.

    opened by xiaozialei 1
  • Model Predictive Controller

    Model Predictive Controller

    Hello. This is an amazing repo. I am using it for drone racing in cluttered environment.

    I want to implement a Non-Linear MPC using the B-spline non-linear optimization framework, essentially coupling the trajectory generation with the SO(3) controller. Can this be achiveved given the current state of the code?

    Thank you.

    opened by brunopinto900 1
Owner
HKUST Aerial Robotics Group
HKUST Aerial Robotics Group
A robust sensor fusion library for online localization.

libRSF - A Robust Sensor Fusion Library

ProAut 162 Oct 1, 2022
Quick sort is a highly efficient sorting algorithm and is based on partitioning of array of data into smaller arrays.

Quick sort is a highly efficient sorting algorithm and is based on partitioning of array of data into smaller arrays. A large array is partitioned into two arrays one of which holds values smaller than the specified value, say pivot, based on which the partition is made and another array holds values greater than the pivot value.

Lakshan Sandanayaka 2 Sep 21, 2021
🚀 A open sourced, extremely efficient Texas Hold'em and short deck solver

?? A open sourced, extremely efficient Texas Hold'em and short deck solver

icybee 751 Oct 2, 2022
C++ fundamentals and questions for beginners and intermediates. Welcoming developers, content writers, and programming enthusiasts.

Rules To Contribute To This Repo You can write solutions only in C++ for Data Structure and Algorithms (if you dont know C++ you can submit only algor

Sushree Satarupa 207 Sep 28, 2022
Collection of algorithms and data structures in C++ and Java

Collection of algorithms and data structures in C++ and Java

Andrei Navumenka 1.7k Oct 3, 2022
Organic Maps is a better fork of MAPS.ME, an Android & iOS offline maps app for travelers, tourists, hikers, and cyclists based on top of crowd-sourced OpenStreetMap data and curated with love by MAPS.ME founders.

?? Organic Maps is a better fork of MAPS.ME, an Android & iOS offline maps app for travelers, tourists, hikers, and cyclists based on top of crowd-sourced OpenStreetMap data and curated with love by MAPS.ME founders. No ads, no tracking, no data collection, no crapware.

Organic Maps 3.9k Sep 27, 2022
WasmEdge Runtime is a high-performance, extensible, and hardware optimized WebAssembly Virtual Machine for automotive, cloud, AI, and blockchain applications.

WasmEdge Runtime is a high-performance, extensible, and hardware optimized WebAssembly Virtual Machine for automotive, cloud, AI, and blockchain applications.

null 3.8k Oct 2, 2022
Emusicchain is a blockchain built using Cosmos SDK and Tendermint and created with Starport.

emusicchain emusicchain is a blockchain built using Cosmos SDK and Tendermint and created with Starport. Get started starport chain serve serve comma

null 2 May 13, 2022
A library of common data structures and algorithms written in C.

C Algorithms The C programming language includes a very limited standard library in comparison to other modern programming languages. This is a coll

Simon Howard 2.8k Oct 3, 2022
Several algorithms and data structures implemented in C++ by me (credited to others where necessary).

Algorithms This repository contains my implementations of several algorithms and data structures in C++ (credited to others where necessary). It has i

Petar Veličković 581 Oct 1, 2022
C++ implementations of well-known (and some rare) algorithms, while following good software development practices

ProAlgos: C++ This project is focused on implementing algorithms and data structures in C++, while following good software engineering practices, such

ProAlgos 472 Sep 26, 2022
Xournal++ is a handwriting notetaking software with PDF annotation support. Written in C++ with GTK3, supporting Linux (e.g. Ubuntu, Debian, Arch, SUSE), macOS and Windows 10. Supports pen input from devices such as Wacom Tablets.

Xournal++ is a hand note taking software written in C++ with the target of flexibility, functionality and speed. Stroke recognizer and other parts are based on Xournal Code

Xournalpp 7.2k Oct 4, 2022
Rufus is a utility that helps format and create bootable USB flash drives.

Rufus is a utility that helps format and create bootable USB flash drives.

Pete Batard 20.2k Sep 29, 2022
Provide building blocks (software, hardware and algorithms) for implementing SLAM using small sensors

RemoteSLAM The purpose of this repo is to provide the building blocks (software drivers, hardware and algorithms) for implementing SLAM systems using

Autonomous Drones Lab, Tel Aviv University 38 Jan 20, 2022
Pseudofermion functional renormalization group solver for (frustrated) quantum magnets in two and three spatial dimensions.

SpinParser SpinParser ("Spin Pseudofermion Algorithms for Research on Spin Ensembles via Renormalization") is a software platform to perform pseudofer

Finn Lasse Buessen 19 Sep 5, 2022
Fundamentals of Data structures and algorithms in c++

Data Structures & Algorithms About the repository: Contains theories and programming questions related to fundamentals of data structures and algorith

fifu 44 Sep 23, 2022
CXXGraph is a Header-Only C++ Library for Graph Representation and Algorithms

CXXGraph is a small library, header only, that manages the Graph and it's algorithms in C++. In other words a "Comprehensive C++ Graph Library".

ZigRazor 164 Sep 26, 2022
Official ACM CSS PEC repository for resources and all things Open source

Contributing Guidelines Ahoy there! Welcome to the PEC-ACM Open Source Repository. We request you to read the whole guidelines before making any pull

PEC CSS 76 Oct 1, 2022
Header-only C++ library for robotics, control, and path planning algorithms.

Header-only C++ library for robotics, control, and path planning algorithms.

null 350 Oct 1, 2022