The main goal for this project was to write a symbolic algorithm that developes a trajectory while minimizing snap (fourth derivative of position). The main reason for minimising snap as the objective is to reduce the control effort for the quadrotor.
To prove this we can write the mathematical state space model for a 2D quadrotor:
From the model above is clear that degree of differentiation will result in a relationship between input and output i.e. the system is not input output linearizable. To solve for this we extend the states of the quadrotor and rewrite the model as:
Now by using nonlinear control principles we can see that there is a dependency between input and outputs through the 4th relative degree lie derivative. Therefore the control inputs are dependent on the 4th derivative of position x and y (2D model) which are known as snap. The same approach also holds true for a 3D quadrotor which also has dependancy on snap. As a result to minimise the efforts of the controller it is also important to minimise snap.
To design a minimum snap trajectory algorithm we use principles in calculus of variation. We define the running cost as the the square of the norm of the snap therefore the cost functional subject to minimization is:
Solving for the Euler-Lagrange, the condition for optimal trajectory is derived as 8th derivative of x to be 0 at all times.Therefore solving for this differential equation the minimum snap trajectory for a single segment will be a 7th order polynomial:
This means that in order to find complete trajectories 8 coefficients from c0 to c7 need to be solved per segment. We use the exact same process for multiway point trajectory generation where the number of unknown coefficients is proportional to the number of waypoints i.e. 8.(way_pts) = #coefficients. To solve for the coefficients within the given polynomials it is required to have as many boundary conditions as the coefficients. The image below shows the process of deriving boundary conditions for multiway point trajectories.
As result, for a 3 way point min snap trajectory in order to solve for all the 24 coefficients boundary conditions upto 4th derivative at initial and final points and 8 boundary conditions for each intermediate way point is required. Thid will result in 24 linear equations where by using LU factorisation we can solve for all coefficients.
Click on this for Matlab files
The result of a matlab code for a 3 way point trajectory at
boundary = [0; 0; 0; 0; 1; 0; 0; 0];
ps = [2; 3];
ts = [1; 3; 5];
where at x1(t0) = 0 and x3(t4) = 1 with intermediate positions at 2 and 3 at times 1 and 3. the desired plot based on calculated 24 coefficients: