Convert multi-robot waypoint sequences into smooth piecewise polynomial trajectories.
This repository contains a Matlab implementation of the continuous trajectory optimization stage of the algorithm described in:
Downwash-Aware Trajectory Planning for Large Quadcopter Teams
James A. Preiss, Wolfgang Hönig, Nora Ayanian, Gaurav S. Sukhatme
Accepted at IEEE IROS 2017, Preprint at https://arxiv.org/abs/1704.04852
The purpose of this program is to convert a waypoint sequence for multiple robots into a set of smooth trajectories. It is assumed that the waypoint sequence comes from a planner (typically graph-based) that models robots moving on straight lines between waypoints. It is impossible to execute such a plan in real life with nonzero velocity at the waypoints, because it would require infinite acceleration. Therefore, we want to "smoothen" the trajectories while maintaining the collision avoidance guarantee.
We model robots as axis-aligned ellipsoids to deal with the downwash effect of quadrotors. The output format is piecewise polynomial trajectories with user-specified smoothness and degree. Output is given as Matlab's ppform structs.
- Make sure your Matlab MEX compiler is set up correctly, and uses at least -O2 optimization.
- Run
make. - From the
smoothenerroot directory , open a Matlab session and runmain_grid. Computation should take several seconds, and you should see a 3D plot when it is done.
- run
git submodule init && git submodule update. cdintooctomap_corridor/octomapand follow the "to only compile the library" instructions in Octomap'sREADME.md.cdback into thesmoothenerroot directory and runmake octomap.- From the
smoothenerroot directory , open a Matlab session and runmain_octomap. Computation should take several seconds, and you should see a 3D plot when it is done.
Currently supported environment models are:
- Octomaps, given as binary files on the disk
- sets of axis-aligned boxes
This part of the code is modularized, making it easy to add support for other environment representations.
| item | description |
|---|---|
<top level> |
main routines, obstacle model implementations, and makefile. The main entry point is smoothener.m. |
examples/ |
example problems for both "octomap" and "list of boxes" environment models, and an obstacle-free problem. |
external/ |
third-party code from the Matlab file exchange. |
octomap_corridor/ |
a standalone program that computes the safe corridor for a single robot in an octomap. A separate process is used instead of a mex-function because the auto-generated SVM solver C code from CVXGEN uses global variables, making multithreading impossible. |
svm_sephyp_*point/ |
generated C code from the CVXGEN package to solve ellipsoid-weighted hard margin SVM problems for separating hyperplanes in 3D. Used for both robot-robot and robot-obstacle separation. Also contains mex-functions implementing the outer loop for all robots. |
tests/ |
very few unit tests, need to write some more... |
utils/ |
low-level simple subroutines. |
- Support 2D problems. Most "difficult" parts of the code are dimension-generic, but some places assume 3D, particularly the CVXGEN generated code for separating hyperplane problems. In the meantime, use a discrete plan with all zeroes for the z-axis, and set the z bounding box to something like +/- 0.001.
- Support varying time allocation to each step in the discrete plan (each step is same duration for all robots, but not all steps in sequence need to be same duration)
- Fix the hard-coded ellipsoid dimensions in
octomap_corridor.cpp. - Write more unit tests.
- Port to Python, Julia, ... (outside contributors welcomed!)
- Port to C++
The implementation uses generated code from CVXGEN to solve many small separating-hyperplane problems. This is necessary to avoid excessive overhead of interpreting Matlab code. The makefile compiles all C/C++ code.
The implementation has been profiled and optimized. There is no known low-hanging fruit. For medium-sized problems (20-80 robots), the bottleneck is solving the large quadratic programs for each robot's corridor-constrained trajectory optimization problem. If CPLEX is installed, it will be used to solve these QPs.
Wherever possible, computational bottlenecks are parallelized. Multiple CPU cores up to the number of robots will show a large benefit.
This project is developed under the supervision of Gaurav S. Sukhatme as a part of the Robotic Embedded Systems Lab (RESL).
This work originated as a project in Nora Ayanian's course Coordinated Mobile Robotics at USC in Fall 2016.
Ongoing development has been a collaboration with Wolfgang Hönig who provided the discrete graph planning front end and contributed to debugging and experiments.
The method builds upon Sarah Tang's IROS 2016 paper.
The project name is from Anna Lukina.