Quadrotor Path and Trajectory Planning

I designed several algorithms involved in flying and controlling a simulated quadrotor.

Dijkstra/A* Path Planning

Figure 1: Solution to a sample problem. Solved by Dijkstra’s algorithm in 4.6s and by A* in 4.2s.

Figure 1: Solution to a sample problem. Solved by Dijkstra’s algorithm in 4.6s and by A* in 4.2s.

I designed a MATLAB implementation of Dijkstra’s Algorithm and A* - two classic graph search algorithms. Given a map containing rectangular obstacles, a degree of discretization, and some margin of safety, the function finds the shortest path between designated start and goal locations. The algorithm uses a 26-connected 3D voxel graph.

One of the goals of this project was to optimize the speed with which the program finds the shortest path. Since the implementation was in MATLAB, this meant removing loops and performing as many operations as possible using vector algebra.

A sample solution is shown in Figure 1.

Trajectory Planning

The solutions returned by Dijkstra’s Algorithm or A* are shortest paths, but they are only shortest within the graph discretization used in the algorithm. We defined the set of 26 neighbors that can be reached from the grid point the quadrotor is occupying - up, down, left, right, and the associated diagonals - and the simulated robot is restricted to those motion. But in reality the choice of direction is continuous, and we can draw much straighter and more efficient trajectories by “cutting corners.”

Another issue with the Dijkstra shortest path is that it is not differentiable (it displays sharp corners), so a physical quadrotor will be unable to follow it exactly. We know, however, that the quadrotor can follow sufficiently differentiable trajectories since it is differentially flat (ADD CITATION). Thus if the trajectory is described as a 7th-order polynomial we can be guaranteed that a well-designed controller can keep the quadrotor on the specified path.

Thus the process of going from a Dijkstra shortest path to a quadrotor trajectory involves:

  1. Simplifying the Dijkstra shortest path to improve efficiency.

  2. Approximate the resulting path via polynomials.

I designed a MATLAB function to perform these tasks, interfacing it with the Dijkstra implementation from above to go from a map and start/goal positions to a complete trajectory that a quadrotor can track.

  1. The Dijkstra shortest path is improved by using a ray-tracing approach. The first non-start position is selected, and then the algorithm searches for the furthest point that can be connected to this point via a straight line without hitting an obstacle. This furthest point is added to the new path, and the procedure is repeated until the goal is reached. This path is calculated starting at the start and working to the goal and starting at the goal and working backwards to the start, and the shortest result is used.

  2. The resulting path consists of a small number of straight line segments. The algorithm then replaces each straight line with a cubic (3rd order) polynomial, with the segments split in time such that there is time to accelerate and decelerate smoothly without overshooting a maximum specified acceleration. Although 3rd order polynomials are not perfectly achievable they display a close approximation of the original straight line path; this ensures that the new trajectory is still collision-free.


ATTRIBUTION

This project was done as work for MEAM620 - Advanced Robotics, taught by Dr. James Paulos and Dr. Vijay Kumar at the University of Pennsylvania, Spring 2019.