Model Predictive Contol (MPC) - set Path/Trajectory for Quadcopter

10 views (last 30 days)
I'm implementing MPC for a quadcopter. I need to make time-dependent reference in my cost function "running costs" to set the flight scenario: define the trajectory by setting the points to reach and simulate the fault of the rotor by setting one of the control input to zero.
Could you recommend how can this be implemented in code and should look like? Currently the vehicle goes to one point for all simulation which set in the running costs.
I attach the code below. The control input vector consists of four elements (4 rotors), state vector has 12 elements (3 translational, 3 rotational degrees of freedom and their first derivatives).
Thank you in advance for your attention!
%% Parameters of MPC
mpciterations = 10; % Number of MPC iterations to be performed
N = 10; % Length of optimization horizon
T = 0.1; % Sampling interval
u0 = ones(4,N); % Initial guess of open loop control
tmeasure = 0.0; % Time measurement of initial value
xmeasure=[1 1 1 0 0 0 0 0 0 0 0 0]% initial values of state vector. x(1)-x(3) are vehicle's NED coordinates.
function [c,ceq] = constraints(t, x, u)
c = [];
c(1) =-x(3); % height cannot be less than zero
ceq = [];
end
function cost = runningcosts(t, x, u)
r1=[1 1 1 1 1 1 1 1 1 1 1 1];
R=diag(r1);
q2=[1 1 1 1];
Q=diag(q2);
cost = (x-[0 0 -10 0 0 0 0 0 0 0 0 0])*R*(x-[0 0 -10 0 0 0 0 0 0 0 0 0])'...
+(u-[300; 300; 300; 300])'*Q*(u-[300; 300; 300; 300]);
end

Answers (1)

Sai Bhargav Avula
Sai Bhargav Avula on 14 Aug 2019
You can refer the following Non-Linear MPC example for trajectory optimization

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!