Skip to content

Quadrotor

Example

Quadrotor/

Where you can find this system

M. Hehn and R. D’Andrea, “A flying inverted pendulum,” ICRA, Shanghai, China, 2011, pp. 763–770.

Problem Description

The quadrotor we want to control is like this:

  • The state vector of the quadrotor is x = [X,\dot{X} , Y ,\dot{Y} ,Z ,\dot{Z} ,\gamma ,\beta ,\alpha ]^{T}\in\mathbb{R}^{9}, where (X,Y,Z) and (\gamma,\beta,\alpha) are the position and angles of the quadrotor, respectively.

  • The input vector is u=[a,\omega_{X},\omega_{Y},\omega_{Z}]^T, where a represents the thrust and (\omega_{X},\omega_{Y},\omega_{Z}) the rotational rates. The control input is bounded by u_{min}=[0,-1,-1,-1]^T \leq u \leq [11,1,1,1]^T = u_{max}.

  • The dynamics of the quadrotor are given by the following equations:

\begin{align} &\ddot{X} = a(\cos\gamma\sin\beta\cos\alpha+\sin\gamma\sin\alpha)\\ &\ddot{Y} = a(\cos\gamma\sin\beta\sin\alpha-\sin\gamma\cos\alpha)\\ &\ddot{Z} = a\cos\gamma\cos\beta-g\\ &\dot{\gamma} = (\omega_{X}\cos\gamma+\omega_{Y}\sin\gamma)/\cos\beta\\ &\dot{\beta} = -\omega_{X}\sin\gamma+\omega_{Y}\cos\gamma\\ &\dot{\alpha} = \omega_{X}\cos\gamma\tan\beta + \omega_{Y}\sin\gamma\tan\beta+\omega_{Z} \end{align}

The task is to use NMPC to control the quadrotor to track the given position reference under a variable prediction horizon.

OCP in ParNMPC

The inequality constraints are handled using the logarithmic barrier functions.

The underlying OCP defined in ParNMPC is formulated as:

  • State: x=[X,\dot{X} , Y ,\dot{Y} ,Z ,\dot{Z} ,\gamma ,\beta ,\alpha ]^T.
  • Input: u=[a,\omega_{X},\omega_{Y},\omega_{Z}].
  • Parameter: p=[X_{ref},Y_{ref},Z_{ref},\gamma,T]^T, where \gamma>0 is the barrier parameter.
  • L(u,x,p) = L_s(u,x,p) + L_{u}(u,x,p), where L_s(u,x,p) = \frac{1}{2}\|x-x_{ref}\|_{Q}^2+\frac{1}{2}\|u-u_{ref}\|_{R}^2 with x_{ref}=\text{diag}([X_{ref},0,Y_{ref},0,Z_{ref},0,0,0,0]), and L_u(u,x,p) is the summation of the barrier functions with parameters \gamma.
  • f(u,x,p) is shown before.
  • Prediction horizon T.
  • Number of the discritization grids N=40.
  • Discretization method: Euler in reverse time (backward Euler).

Closed-loop Simulation using ParNMPC

Step 1. NMPC problem formulation

See Workflow of ParNMPC > NMPC Problem Formulation.

Example

Quadrotor/NMPC_Problem_Definition.m

See Workflow of ParNMPC > Code Generation and Deployment > Simulink.

  1. Code generation

    Example

    Quadrotor/Simu_Simulink_Setup.m

  2. Deployment

    Example

    Quadrotor/Simu_Simulink.slx

Step 2. Speed up the closed-loop simulation in Matlab using mex

  1. Code generation

    Example

    Quadrotor/Simu_Simulink_Setup.m

    Modify the generation target to mex: 

    NMPC_Iter_CodeGen('mex','C',args_NMPC_Iter);
    and run.

  2. Deployment

    Example

    Quadrotor/Simu_Matlab.m

    Modify NMPC_Iter to NMPC_Iter_mex to call the generated mex function: 

    [lambdaSplit,muSplit,uSplit,xSplit,...
 thetaSplit,cost,error,timeElapsed] =
 NMPC_Iter_mex(x0,lambdaSplit,muSplit,...
               uSplit,xSplit,pSplit,...
               thetaSplit,discretizationMethod,...
               isMEnabled);
    and run.