Hierarchical Trajectory Planning for Quadrotor Flight in Unknown Environments

GitHub Repo | Project Report | Project Partners: Vandan Rao, Furqan Ahmed, Sangita Sahu

This was a course project where my teammates and I studied the problem of trajectory planning for high speed quadrotor flight in unknown environments.

Overview

Motion planning in environments that are unknown apriori is challenging for traditional sampling based planners because many samples end up becoming wasted when new obstacles invalidate entire sections of the tree. Numerous alternate approaches exist for this problem class, one of which is hierarchical planners. These use a fast global planner to generate a collision free path that is then refined by a local planner to generate a dynamically feasible trajectory, usually by solving an optimization problem.

For our project we investigated this class of methods by implementing a simplified version of the FASTER planner [1].

Approach

For global planning, FASTER uses Jump Point Search (JPS) to generate a shortest piecewise linear path to the goal. It then performs a convex decomposition of the free space to generate a set of polytopes in which the plan can lie. The local plan is parameterized with a set of Bezier curves and an optimization problem is solved to find spline control points that minimize squared jerk and remain within vehicle dynamic limits. The convex hull property of Bezier curves reduces collision checking to keeping the control points within at least one polytope, but requires adding binary variables to allocate each segment to a polytope, promoting the problem to a MIQP.

FASTER generates two plans, an optimistic one in both the free known and unknown space and a safe one in only the free known space with a stopping condition. This allows for higher speeds without risking collision if there is an object in the unknown space. To reduce scope for this project we planned only in the free known space, which sacrifices speed to maintain safety. The picture below shows our version, which we jokingly referred to as SLOWER.

We implemented the planner in a series of Python ROS nodes, one for each major step including the mapper (Furqan), global planner (Sangita), convex decomposition (Vandan) and local planner (myself). We tested it in Gazebo simulations making use of the quadrotor simulator used by the original authors in their GitHub repository [2].

My Contributions

For my part, I lead the definition of the ROS architecture and software interfaces as well as writing the local planner and master node.

The local planner at each replanning iteration defines the MIQP optimization problem, calls the solver(MOSEK) and then stores the result so that the time interpolated trajectory can be output at the high frequency needed for motion. This requires managing the full set of constraints including initial/boundary conditions, continuity between segments, vehicle dynamic limits(speed, acceleration and jerk), and planar collision constraints for each polytope as well as mixed continuous and binary variables.

The video below shows the local planner operating in isolation using fixed global plan and convex decomposition inputs corresponding to a simple S shaped corridor. The RViz view in the lower left shows the full spline trajectory generated by the local planner. Since the inputs are fixed the cylinder forest world is for visualizing the motion only.

Final Results

Due to implementation challenges with the global planner, it was necessary to integrate our convex decomposition and local planner with the JPS global planner implemented in the original FASTER implementation [2]. We tested this combination in both a small forest and office environment, which are illustrated below.

Sample videos of the flights in the both environments are shown below. In testing we achieved maximum speeds of up to 4.2 m/s and replanning rates of up to 10 Hz. The full results summary is available in the report here.

$$ $$

Credits for the RViz tools to visualize the polyhedra and ellipsoids go to Sikang Liu [4].

Challenges

We encountered a number of challenges over the course of the project. The first was run time for the convex decomposition phase due to the cost of processing the incoming point cloud. Performance improved over the course of the project, but it can be seen in the videos that the decomposition still lags behind the speed of the vehicle. This delay causes the local planner to stop periodically while waiting for it to update.

The second was the time allocation heuristic. FASTER determines the time allocation for the trajectory by multiplying the minimum straight line time to the goal by a scale factor to account for path curvature. The scale factor is increased or decreased between replanning iterations depending on if a feasible solution is found in a form of line search to attempt to find the fastest possible trajectory time.

The use of infeasibility as the only stop criterion was found to not be robust and lead to scenarios where the factor was continually made more aggressive until the velocity saturated over long portions of the planning horizon, leading to the problem eventually becoming infeasible without violating the speed constraints. This lead to requiring large cutbacks on the time allocation factor and created an oscillatory mode.

The original authors did not mention this behaviour so it could be an issue of our implementation, however it seems inherent to the method. I attempted the use of soft speed constraints to alleviate the issue, but did not reach a robust solution in the time available.

The last challenge was the effect of the local plan on the observability of the world. The optimization problem does not consider observability and thus does not attempt to keep the camera level or limit angular rates to reduce motion blur. It was found that this can lead to the planner commanding large vehicle angles briefly that could cause the convex decomposition to fail due to poor point cloud data.

Conclusion

In conclusion, we managed to produce implementations of the local planning and convex decomposition components of a hierarchical quadrotor trajectory planner capable of generating flyable trajectories in simulation. While challenges and robustness issues remained, this was still a great opportunity for me to practice motion planning, solving problems via optimization and ROS implementation.

References

[1] - J. Tordesillas and J. P. How, “FASTER: Fast and safe trajectory planner for navigation in unknown environments,” IEEE Transactions on Robotics, 2021.

[2] - J. Tordesillas, “FASTER: Fast and Safe Trajectory Planner for Navigation in Unknown Environments.” https://github.com/mit-acl/faster, 2021.

[3] - https://gitlab.com/mit-acl/lab/acl-gazebo

[4] - S. Liu, “MRSL Decomp Util ROS.” https://github.com/sikang/DecompROS, 2018.