Quadrotor Trajectory Tracking Using Iterative Learning Control

Since R2025a

This example demonstrates quadrotor trajectory tracking control using the Iterative Learning Control (ILC) block in Simulink® Control Design™ library. The ILC is used in the outer-loop position control for quadcopter trajectory tracking. The quadrotor is commanded to track a circular trajectory, the vehicle is also subjected to external aerodynamic disturbance caused by wind bias. This example uses ILC over repeated iteration the controller learns to minimize error and get close tracking of given reference signal.

Quadrotor Model

This example demonstrates the quadrotor trajectory control using PID augmented by ILC controller. Quadrotor use two-loop cascaded control. The outer-loop control the position of the vehicle in x-y-z in the inertial frame. The inner-loop controls the attitude of the vehicle yaw-pitch-roll also in inertial frame. You use Iterative learning control in the outer loop along with the PID for position control. The inner-loop use a stand-alone PID controller.

For a 6-DOF quadrotor model, consider the following:

• $\mathit{u},\mathit{v},\mathit{w}$ are the vehicle velocity in the body frame

• $\mathit{p},\mathit{q},\mathit{r}$ are the angular velocity of the vehicle in body frame

• $\mathrm{Roll}-\varphi ,\mathrm{Pitch}-\theta ,\mathrm{Yaw}-\psi$are defined in vehicle-2, vehicle-1 and vehicle frame respectively.

• $\mathit{x},\mathit{y},\mathit{z}$ Is the vehicle translation motion in inertial frame.

This example use a simplified quadrotor dynamics. Following assumption are made to derive a simplified model of the quadrotor for control.

• Assume the $\mathrm{Roll}-\varphi$ and $\mathrm{Pitch}-\theta$angles are small and hence body rates are approximately equal to inertial rates,

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}\dot{\mathit{p}}\\ \dot{\mathit{q}}\\ \dot{\mathit{r}}\end{array}\right]$$

• Assume affect of Coriolis components are negligible, $\mathrm{qr},\mathrm{pr},\mathrm{pq}\approx 0$

• In this example you will control position and attitude. Heading angle $\mathrm{Yaw}-\psi$is assumed to be held zero.

With these simplifying assumptions the simplified quadrotor model can be written as [1],

Outer-Loop quadrotor dynamics for position in inertial frame is s follows,

$$\begin{array}{l}\ddot{\mathit{x}}=-\cos \left(\varphi \right)\sin \left(\theta \right)\frac{\mathit{F}}{\mathit{m}}\\ \ddot{\mathit{y}}=\sin \left(\varphi \right)\frac{\mathit{F}}{\mathit{m}}\\ \ddot{\mathit{z}}=\mathit{g}-\cos \left(\varphi \right)\cos \left(\theta \right)\frac{\mathit{F}}{\mathit{m}}\end{array}$$

Inner-Loop quadrotor dynamics of attitude angles in inertial frame is governed by following equations

$$\begin{array}{l}\ddot{\varphi }=\frac{1}{{\mathit{J}}_{\mathit{x}}}\tau {}_{\varphi }\\ \ddot{\theta }=\frac{1}{{\mathit{J}}_{\mathit{y}}}\tau {}_{\theta }\\ \ddot{\psi }=\frac{1}{{\mathit{J}}_{\mathit{z}}}\tau {}_{\psi }\end{array}$$

Where $\mathit{F}={\mathit{f}}_1+{\mathit{f}}_2+{\mathit{f}}_3+{\mathit{f}}_4$ is the total propeller forces,$\tau {}_{\varphi },\tau {}_{\theta },\tau {}_{\psi }$are the torques applied to the quadrotor, $\mathit{m}$ is vehicle mass and $\mathit{g}=9.81$ is the gravitation constant.

Two-loop controller for quadrotor trajectory tracking

Here is the schematic of two-loop cascaded control for quadrotor vehicle. The outer-loop generate the translation forces to control the position of the vehicle. Inner loop tracks the reference attitude angles and generate body torques.

Simplifying the outer-loop translation equation of motion, you can define following control terms

$$\begin{array}{l}{\mathit{u}}_{\mathit{x}}=-\cos \left(\varphi \right)\sin \left(\theta \right)\frac{\mathit{F}}{\mathit{m}}\\ {\mathit{u}}_{\mathit{y}}=\sin \left(\varphi \right)\frac{\mathit{F}}{\mathit{m}}\\ {\mathit{u}}_{\mathit{z}}=\mathit{g}-\cos \left(\varphi \right)\cos \left(\theta \right)\frac{\mathit{F}}{\mathit{m}}\end{array}$$

Where ${\mathit{u}}_{\mathit{x}}$, ${\mathit{u}}_{\mathit{y}}$, and ${\mathit{u}}_{\mathit{z}}$ are output of outer-loop PID controller for quadrotor position. Using these three outer-loop control you can solve for commanded roll ${\varphi }_{\mathit{d}}$, pitch ${\theta }_{\mathit{d}}$ and yaw ${\psi }_{\mathit{d}}$ as reference for inner-loop control as follows,

$${\varphi }_{\mathit{d}}={\tan }^{-1}\left(\frac{{\mathit{u}}_{\mathit{y}}\cos \left(\theta \right)}{\mathit{g}-{\mathit{u}}_{\mathit{z}}}\right),{\theta }_{\mathit{d}}={\tan }^{-1}\left(\frac{{\mathit{u}}_{\mathit{x}}}{{\mathit{u}}_{\mathit{z}}-\mathit{g}}\right)\mathrm{and}{\psi }_{\mathit{d}}=0$$

The inner loop implements a PID controller to track the commanded reference attitude angles ${\varphi }_{\mathit{d}},{\theta }_{\mathit{d}},\mathrm{and}{\psi }_{\mathit{d}}$

Quadrotor trajectory tracking

The aim of this example is to control a quadrotor to track a circular trajectory. Quadrotor starts a initial position $\left[{\mathit{x}}_0,{\mathit{y}}_0,{\mathit{z}}_0\right]=\left[\begin{array}{ccc}3& 0& 1\end{array}\right]$ in the inertial frame and executes a circle for radius $\mathit{R}=3\mathit{m}$. The time for quadrotor to complete one circle trajectory is set to $\mathrm{Tspan}=15\sec$.

% Set the Circular trajectory radius
R = 3;

% Quadrotor initial condition in inertial frame
IC = [3 0 -1]; % In North-East-Down co-ordinate frame

Outer Loop position control PID

The outer loop control for quadrotor tracks the reference trajectory in x-y-z frame. The three axis independent controller for quadrotor is as follows. The controller gain can be tuned one at a time using successive loop closure method.

The individual control loop in X, Y and Z, have a baseline PID controller designed to do the trajectory tracking such that $\mathit{x}\to {\mathit{x}}_{\mathit{d}},\mathit{y}\to {\mathit{y}}_{\mathit{d}}\mathrm{and}\mathit{z}\to {\mathit{z}}_{\mathit{d}}$.

$${\mathit{u}}_{\xi }={\mathit{k}}_{{\mathit{p}}_{\xi }}\left({\xi }_{\mathit{d}}-\xi \right)+{\mathit{k}}_{{\mathit{d}}_{\xi }}\left(\dot{{\xi }_{\mathit{d}}}-\dot{\xi }\right)+{\mathit{k}}_{{\mathit{I}}_{\xi }}\int \left({\xi }_{\mathit{d}}-\xi \right)\mathrm{dt}$$

where state $\xi =\left[\begin{array}{ccc}\mathit{x}& \mathit{y}& \mathit{z}\end{array}\right]\mathrm{and}{\xi }_{\mathit{d}}=\left[\begin{array}{ccc}{\mathit{x}}_{\mathit{d}}& {\mathit{y}}_{\mathit{d}}& {\mathit{z}}_{\mathit{d}}\end{array}\right]$

The tuned controller gains used for this example for both X and Y loop are as follows

% Outer-Loop PID gain
% X-axis PID gains
Kp_x = 1;
Kd_x = 0.97;
Ki_x = 0.01;

% Y-Axis PID Gains
Kp_y = 0.1;
Kd_y = 2.5;
Ki_y = 0.01;

Iterative learning Control Overview

The X and Y outer-loop position control are augmented with ILC control. Where as height (Z) is a standalone PID controller, designed to maintain the height at $\mathit{Z}=1\mathit{m}$. The quadrotor repeats the circular trajectory for K iterations. The ILC controller aim is, over the iteration to improve tracking performance. A brief description of ILC control is as follows, for more details see Iterative Learning Control.

Iterative learning control (ILC) is an improvement in run-to-run control. It uses frequent measurements in the form of the error trajectory from the previous batch to update the control signal for the subsequent batch run. The focus of ILC has been on improving the performance of systems that execute a single, repeated operation, starting at the same initial operating condition. This focus includes many practical industrial systems in manufacturing, robotics, and chemical processing, where mass production on an assembly line entails repetition.

• Suitable for repetitive task + repetitive disturbances.

• Use knowledge from previous iteration to improve next iteration.

A general model based ILC control update is as follows [2],

$u_{\mathit{k}+1}\left(t\right)=Q\left(q\right)\left[u_{\mathit{k}}\right(t)+L(q\left)e_{\mathit{k}}\right(t+1\left)\right]$

Where L is learning function based on the system model G. In this example you use inverse model ILC, which use learning function $\mathit{L}={\mathit{G}}^{-1}$. where G is the input-output relation for LTI system, in the lifted form as follows ${\mathit{y}}_{\mathit{k}}={\mathrm{Gu}}_{\mathit{k}}+\mathit{d}$.

ILC mode

At runtime, ILC switches between two modes: control and reset. In the control mode, ILC outputs ${\mathit{u}}_{\mathit{k}}\left(\mathit{t}\right)$at the desired time points and measures the error between the desired reference $\mathit{r}\left(\mathit{t}\right)$ and output ${\mathit{y}}_{\mathit{k}}\left(\mathit{t}\right)$. At the end of the control mode, ILC calculates the new control sequence ${\mathit{u}}_{\mathit{k}+\mathrm{1}}\left(\mathit{t}\right)$ to use in the next iteration. In the reset mode, ILC output is 0. The reset mode must be long enough such that the return to home controller in this mode brings the plant back to its initial condition.

ILC Design

To design an ILC controller, following parameters are required to be configured,

• Sample time and Iteration duration — These parameters determine how many control actions ILC provides in the control mode. If sample time is too large, ILC might not provide sufficient compensation. If sample time is too small, ILC might take too much resources, especially it might lead to large memory footprint when model-based ILC is used.

• Model information --- This example use model based ILC. A nominal closed loop model information is necessary to design the ILC controller.

• ILC gains — The gain determine how well ILC learns between iterations. If ILC gain is too big, it might make the closed-loop system unstable (robustness). If ILC gains are too small, it might lead to slower convergence (performance).

• Filter time constant — The optional low-pass filter to remove control chatter which may otherwise be amplified during learning. By default it is not enabled in the ILC block.

Lets define the parameters for the ILC block.

% Sample Time
Ts = 0.05;
% single iteration duration
Tspan = 15;

you use a model based version of ILC for this example. The linear, nominal discrete model for X and Y axis is defined as follows

% Nominal plant for ILC
A = [1 0.05;-0.02 0.97];
B = [0;0.05];
C = eye(2);

ILC gain for X and Y axis are defined as follows,

% ILC gain
gamma_x = 0.3;
gamma_y = 0.08;

ILC implements a low pass filter to filter out high-gain control oscillations. Filter adds trade-off between rate of convergence and robustness.

% Filter Coeff
Filter_coeff = 2;

Inner-Loop attitude control PID

The inner loop control for quadrotor tracks the reference attitude. The individual control loop in roll, pitch and yaw, have a baseline PID controller designed to do the attitude tracking such that $\varphi \to {\varphi }_{\mathit{d}},\theta \to {\theta }_{\mathit{d}}\mathrm{and}\psi \to {\psi }_{\mathit{d}}$.

$$\tau {}_{\xi }={\mathit{k}}_{{\mathit{p}}_{\xi }}\left({\xi }_{\mathit{d}}-\xi \right)+{\mathit{k}}_{{\mathit{d}}_{\xi }}\left(\dot{{\xi }_{\mathit{d}}}-\dot{\xi }\right)+{\mathit{k}}_{{\mathit{I}}_{\xi }}\int \left({\xi }_{\mathit{d}}-\xi \right)\mathrm{dt}$$

where $\xi =\left[\begin{array}{ccc}\varphi & \theta & \psi \end{array}\right]\mathrm{and}{\xi }_{\mathit{d}}=\left[\begin{array}{ccc}{\varphi }_{\mathit{d}}& {\theta }_{\mathit{d}}& {\psi }_{\mathit{d}}\end{array}\right]$

you do not augment the baseline PID in the inner loop with ILC. The PID controller gains ${\mathit{k}}_{{\mathit{p}}_{\xi }},{\mathit{k}}_{{\mathit{d}}_{\xi }}\mathrm{and}{\mathit{k}}_{{\mathit{I}}_{\xi }}$ can be tuned one at a time using successive loop closures. In this example you use pid auto-tune in PID controller block.

Simulate Model and Plot Results

To make the problem more challenging, you add an external wind disturbance to the vehicle. The disturbance is also repetitive in nature and adds drag in Y direction. The wind disturbance in Y direction is modeled as follows,

$${\mathit{f}}_{\mathrm{dragY}}=\frac{1}{2}\mathrm{Cd}\rho {\mathit{u}}_{\mathit{y}}^2\mathrm{if}\mathit{x}>0,\mathrm{else}{\mathit{f}}_{\mathrm{dragY}}=0$$

where ${\mathit{C}}_{\mathit{d}}=0.1$ is coefficient of drag, $\rho =1\frac{\mathrm{kg}}{{\mathit{m}}^3}$ is density of air and ${\mathit{u}}_{\mathit{y}}$ is vehicle velocity in Y direction.

Simulate the model for 10 iterations. In the first iteration, ILC controller outputs 0 in the control mode because it just starts learning. Therefore, the closed-loop control performance displayed in the first iteration comes from the nominal controller, which serves as the baseline for the comparison.

mdl = "quadTrajTrackingILC";
open_system(mdl)
simout = sim(mdl);

As the iterations progress, the ILC controller improves the reference tacking performance in both X and Y direction

% Plot Quadrotor Tracking performance in X
figure(1)
hold on
grid on
plot(simout.tout, simout.x.Data)
plot(simout.tout, simout.r.Data(:,1),'Color','r')
xlabel('Time (secs)')
ylabel('Position-X')

% Plot Quadrotor Tracking performance in Y
figure(2)
hold on
grid on
plot(simout.tout, simout.y.Data)
plot(simout.tout, simout.r.Data(:,2),'Color','r')
xlabel('Time (secs)')
ylabel('Position-y')

% Get Final trajectory index
final_traj_idx = find(simout.tout >=180);

% Plotting Quadrotor trajectory in X-Y Plane
figure(3)
hold on
plot(simout.x.Data(1:final_traj_idx(1)-1), simout.y.Data(1:final_traj_idx(1)-1),'--')
plot(simout.x.Data(final_traj_idx), simout.y.Data(final_traj_idx),'LineWidth',2,'Color','r')
plot(simout.r.Data(final_traj_idx,1),simout.r.Data(final_traj_idx,2),'Color','b')

% Plot Wind disturbance vector plot
quiver(simout.x.data(:,1), simout.y.data(:,1),simout.wind.Data(:,1),simout.wind.Data(:,2))
axis equal

Plot the control effort in the outer loop PID+ILC controller

f =tiledlayout(2,2,TileSpacing="tight");

% Plot ILC control for X Direction
nexttile
plot(squeeze(simout.ux_ilc.Data))
xlabel('Time')
ylabel('u');
title('ILC Control X-axis')
% Plot PID control for X Direction
nexttile
plot(squeeze(simout.ux_pid.data))
xlabel('Time')
ylabel('u');
title('PID Control X-axis')
% Plot ILC control for Y Direction
nexttile
plot(squeeze(simout.uy_ilc.data))
xlabel('Time')
ylabel('u');
title('ILC Control Y-axis')
% Plot PID control for Y Direction
nexttile
plot(squeeze(simout.uy_pid.data))
xlabel('Time')
ylabel('u');
title('PID Control Y-axis')

As iterations progress the ILC controller learns to compensate for the tracking error and the nominal PID control effort is reduced to a minimum.

% Close the model
close_system(mdl,0);

References

1. Beard, Randal. "Quadrotor dynamics and control rev 0.1." (2008).

2. Bristow, Douglas A., Marina Tharayil, and Andrew G. Alleyne. “A Survey of Iterative Learning Control.” IEEE Control Systems 26, no. 3 (June 2006): 96–114.