Quadrotor Control Using Model Reference Adaptive Control

Since R2023b

This example shows how to control quadrotor vehicle performing waypoint guidance. For this example, you assume that the quadrotor system model is unknown and is perturbed by external disturbances, such as payload drop. For such a system with model uncertainties and external disturbances, you can use model reference adaptive control (MRAC) to have the controlled system track an ideal reference model. The six degree-of-freedom (DOF) quadrotor dynamics are modeled in Simulink® and Simscape, the MRAC controller is implemented using the Model Reference Adaptive Control block.

The goal of this example is to control the quadrotor to follow a trajectory defined by waypoints in the presence of uncertainties in both the model and external disturbances.

This example uses Curve Fitting Toolbox™ for waypoint trajectory planning.

Quadrotor Model

A quadrotor is an agile aerial vehicle that uses four rotors to produce lifting and translation forces. The quadrotor model is highly nonlinear and, due to uncertainty in the actuators, is practically very unstable. Therefore, a quadrotor requires an active onboard controller for stable flight. Given a 6-DOF model, designing a nonlinear controller can be difficult and requires exact knowledge of the model parameters. Tuning PID controllers for three-position three-attitude control requires a lot of iteration and simulation effort. Also, such fixed-gain controllers are generally not robust to external disturbances or changes in model parameters. Given these controller design challenges, an adaptive MRAC controller is well-suited for control of uncertain systems.

The 6-DOF quadrotor dynamics are defined in Euler angles as follows.

Outer Guidance Loop

$$\ddot{\zeta }=-\mathit{G}+\frac{1}{\mathit{M}}{\mathit{R}}_{\mathit{t}}\mathit{F}$$

Inner Attitude Loop

$\ddot{\xi }=-{\mathit{R}}_{\mathit{r}}^{-1}{\mathit{J}}^{-1}\left({\mathit{R}}_{\mathit{r}}\dot{\xi }+\mathit{J}{\mathit{R}}_{\mathit{r}}\dot{\xi }\right)-{\mathit{R}}_{\mathit{r}}^{-1}\left(\frac{\partial {\mathit{R}}_{\mathit{r}}}{\partial \theta }\dot{\theta }+\frac{\partial {\mathit{R}}_{\mathit{r}}}{\partial \psi }\dot{\psi }\right)\dot{\xi }+{\mathit{R}}_{\mathit{r}}^{-1}{\mathit{J}}^{-1}\mathit{T}$

Here:

• The state vector $\zeta ={\left[\mathit{x},\mathit{y},\mathit{z}\right]}^{\mathit{T}},\xi ={\left[\varphi ,\theta ,\psi \right]}^{\mathit{T}}$ contains positions and attitude angles in the inertial frame of reference.

• $\mathit{V}={\left[\mathit{u},\mathit{v},\mathit{w}\right]}^{\mathit{T}},\Omega ={\left[\mathit{p},\mathit{q},\mathit{r}\right]}^{\mathit{T}}$contains the translation velocity and angular rotation rates in the body fixed frame.

• $\mathit{G}=\left[0,0,\mathit{g}\right]$ is the gravity vector

• $\mathit{F}$ is the control input for the outer guidance loop.

• $\mathit{T}$ is the control input for the inner attitude loop.

The rotation matrices for translation and rotation are ${\mathit{R}}_{\mathit{t}}$ and ${\mathit{R}}_{\mathit{r}}$, respectively.

${\mathit{R}}_{\mathit{t}}=\left[\begin{array}{ccc}{\mathit{C}}_{\theta }{\mathit{C}}_{\psi }& {-\mathit{C}}_{\theta }{\mathit{S}}_{\psi }& {\mathit{S}}_{\theta }\\ {\mathit{S}}_{\varphi }{\mathit{S}}_{\theta }{\mathit{C}}_{\psi }+{\mathit{C}}_{\varphi }{\mathit{S}}_{\psi }& {-\mathit{S}}_{\varphi }{\mathit{S}}_{\theta }{\mathit{S}}_{\psi }+{\mathit{C}}_{\varphi }{\mathit{C}}_{\psi }& {-\mathit{S}}_{\varphi }{\mathit{C}}_{\theta }\\ {-\mathit{C}}_{\varphi }{\mathit{S}}_{\theta }{\mathit{C}}_{\psi }+{\mathit{S}}_{\varphi }{\mathit{S}}_{\psi }& {\mathit{C}}_{\varphi }{\mathit{S}}_{\theta }{\mathit{S}}_{\psi }+{\mathit{S}}_{\varphi }{\mathit{C}}_{\psi }& {\mathit{C}}_{\varphi }{\mathit{C}}_{\theta }\end{array}\right]$

${\mathit{R}}_{\mathit{r}}=\left[\begin{array}{ccc}{\mathit{C}}_{\psi }{\mathit{C}}_{\theta }& {\mathit{S}}_{\psi }& 0\\ {-\mathit{S}}_{\varphi }{\mathit{C}}_{\theta }& {\mathit{C}}_{\psi }& 0\\ {\mathit{S}}_{\theta }& 0& 1\end{array}\right]$

Here, the $\mathit{C}$ and $\mathit{S}$ elements are the cosine and sine of their respective attitude angles.

Open the quadrotor control project and model.

open("MRACQuadrotorControlExample/Quadcopter_Drone.prj")
mdl = "quadcopter_package_delivery";

MRAC for Quadrotor Control and Navigation

The quadrotor vehicle has outer and inner layers for control. The outer-loop dynamics affect the position of the vehicle in $\mathit{X}$, $\mathit{Y}$, and $\mathit{Z}$ spatial directions and the outer-loop control is used for guidance and navigation. The inner loop is the attitude loop, which affects the vehicle body roll, pitch and yaw angles. The inner-loop control tracks desired reference angles derived from the outer loop.

Open the controller subsystem.

open_system(mdl + "/Maneuver Controller")

Outer-Loop Control

The baseline outer-loop controller, which is responsible for vehicle position control, is evaluated using the dynamics inversion method. The linear feedback representation of the outer-loop system dynamics are:

$$\ddot{\zeta }=\mathit{w}$$

The pseudo-control $\mathit{w}$ is computed using a PID controller.

$$\mathit{w}=-\left({\mathit{K}}_{\mathit{p}}\mathit{e}+{\mathit{K}}_{\mathit{d}}\dot{\mathit{e}}+{\mathit{K}}_{\mathit{i}}\int \mathit{e}\mathrm{dt}\right)$$

Denoting the term $\mathit{U}=\left[{\mathit{u}}_1,{\mathit{u}}_2,{\mathit{u}}_3\right]={\mathit{R}}_{\mathit{t}}\mathit{F}$, you can define the desired controller as $\mathit{U}=\mathrm{Mw}+\mathrm{MG}$. Therefore, $|\mathit{F}|=|\mathit{U}|$ is the total thrust for steady hovering.

The reference command for the desired roll and pitch attitude can be derived as follows.

${\varphi }_{\mathit{d}}=-\mathrm{sgn}\left({\mathit{u}}_2\right){\cos }^{-1}\left(\sqrt{\frac{{\mathit{u}}_3^2}{{\mathit{u}}_2^2+{\mathit{u}}_3^2}}\right)$

${\theta }_{\mathit{d}}=-\mathrm{sgn}\left({\mathit{u}}_1\right){\cos }^{-1}\left(\sqrt{\frac{{\mathit{u}}_2^2+{\mathit{u}}_3^2}{{{\mathit{u}}_1^2+\mathit{u}}_2^2+{\mathit{u}}_3^2}}\right)$

Since trajectory tracking is independent of the yaw orientation of the vehicle, the desired yaw angle is given by the user through a stick command.

Inner-Loop Control

The inner-loop control assumes no knowledge of the model and therefore uses MRAC to achieve trajectory tracking despite model uncertainty and external disturbances. The inner loop control assumes a linear nominal model with the following form.

$$\ddot{\xi }=\mathit{A}\xi +\mathrm{BT}$$

Here, $\xi$contains the inner-loop states and $\mathit{T}$ contains the roll, pitch, and yaw input torques.

The total unmodeled dynamics and external uncertainties are considered to be one lumped term $\Delta \left(\zeta ,\xi \right)$. Express the nominal model in control affine form as follows.

$$\ddot{\xi }=\mathit{A}\xi +\mathit{B}\left(\Delta \left(\zeta ,\xi \right)+\mathit{T}\right)$$

Using MRAC, you can define the total controller torque $\mathit{T}$ as follows.

$$\mathit{T}=-{\mathit{k}}_{\xi }\xi +{\mathit{k}}_{\mathit{r}}\mathit{r}-\hat{\Delta }$$

Here, ${\mathit{k}}_{\xi }$ and ${\mathit{k}}_{\mathit{r}}$ are the feedback and feedforward controller gains and $\hat{\Delta }$is the MRAC estimate of the total uncertainty.

The closed-loop nominal system with the MRAC controller is:

$$\ddot{\xi }=\mathit{A}\xi +\mathit{B}\left(-{\mathit{k}}_{\xi }\xi +{\mathit{k}}_{\mathit{r}}\mathit{r}-\hat{\Delta }+\Delta \left(\zeta ,\xi \right)\right)$$

$$\ddot{\xi }=\left(\mathit{A}-{\mathit{k}}_{\xi }\right)\xi +\mathit{B}{\mathit{k}}_{\mathit{r}}\mathit{r}+\mathit{B}\left(\Delta \left(\zeta ,\xi \right)-\hat{\Delta }\right)$$

The gains ${\mathit{k}}_{\xi }$and ${\mathit{k}}_{\mathit{r}}$ are updated to acheive model matching condition as follows.

$\left(\mathit{A}-{\mathit{k}}_{\xi }\right)={\mathit{A}}_{\mathit{m}}$ and $\mathit{B}{\mathit{k}}_{\mathit{r}}={\mathit{B}}_{\mathit{m}}$

And $\hat{\Delta }$ is learned to approximate the total uncertainty. For more information on the update equations for feedback gains, feedforward gains, and uncertainty approximation, see Model Reference Adaptive Control .

Nominal Model and Reference Model Parameters

Initialize the A and B matrices of the MRAC nominal model, which is a double integrator system.

A = [0 1 0 0 0 0;
     1 1 0 0 0 0;
     0 0 0 1 0 0;
     0 0 1 1 0 0;
     0 0 0 0 0 1;
     0 0 0 0 1 1];
B = [0 0 0;
     1 0 0;
     0 0 0;
     0 1 0;
     0 0 0;
     0 0 1];

The reference model defines the desired response of the closed-loop system and is designed based on the required transient and steady-state behavior. For this example, define the desired second-order response by selecting the natural frequency wn and damping coefficient d.

wn = 4;
d = 0.6;

Define the A and B matrices of the reference model, Am and Bm, respectively.

Am = [   0       1     0       0     0       0;
     -wn^2 -2*d*wn     0       0     0       0;
         0       0     0       1     0       0;
         0       0 -wn^2 -2*d*wn     0       0;
         0       0     0       0     0       1;
         0       0     0       0 -wn^2 -2*d*wn];

Bm = [   0    0    0;
      wn^2    0    0;
         0    0    0;
         0 wn^2    0;
         0    0    0;
         0    0 wn^2];

Controller Parameters

The MRAC controller contains three control elements: feedback, feedforward, and adaptive control.

Initialize feedback gain kx and feedforward gain kr for the controller.

kx = 0;
kr = 0;

In this example, the Model Reference Adaptive Controller block is configured to adjust the feedback and feedforward gains online. Specify the learning rate for this gain adaptation.

gamma_k = 100;

Define the parameters for the adaptive control component of the MRAC controller. The adaptive control estimates and cancels model uncertainties online. For this example, the controller is configured to use a single hidden layer (SHL) neural network to estimate the uncertainties. The SHL network has two layers for adapting the controller model.

To configure the adaptive controller, specify the following parameters.

• N — Number SHL hidden neurons.

• gamma_w — Outer-layer learning rate.

• gamma_v — Inner-layer learning rate for updating the weights of the SHL network.

gamma_w = 5;
gamma_v = 1;
N = 25;

Finally, define the Lyapunov coefficient for the controller weight updates.

Q_mrac = [100 175 100 175 100 125];

Simulate Model

Simulate the model. The quadrotor reference trajectory is designed using waypoint guidance markers. Midway through the flight, the vehicle performs a yaw maneuver. Finally, before final waypoint, the vehicle payload is jettisoned to simulate sudden payload change, which introduces uncertainty in the vehicle mass.

sim(mdl)

The following plot shows the reference tracking for the inner loops, which controls the body pitch, roll, and yaw angles.

open_system(mdl + "/Maneuver Controller" + ...
    "/Inner Loop-Attitude Control/YPR-Body Angles")

The following plot shows the reference tracking for the outer loop, which controls the vehicle $\mathit{X}$, $\mathit{Y}$, and $\mathit{Z}$ positions.

open_system(mdl + "/Maneuver Controller" + ...
    "/Outer-Loop-Position Control/XYZ-Position")