UAV Inflight Failure Recovery with Reference Trajectory Tracking

Since R2024b

This example shows how to design a controller to recover a multicopter aircraft experiencing fault conditions. Here, you use a gain-scheduled approach to recover from a single rotor failure, complete a predetermined mission, and then land the UAV.

Getting Started

Open the UAVRotorFault project file.

prj = openProject("UAVRotorFault");

In this example, to simulate a single rotor failure, the model injects a multiplicative gain vector in the plant. To demonstrate the simulation results of the gain-scheduled controllers, the examples uses the actuator commands and angular velocity measurements as features to design a fault detection algorithm, and uses a threshold based on nominal and fault induced simulations to detect the fault.

Then, open the model.

open_system(mdl);

The model is tuned for the nominal operating condition. However, introducing a fault fundamentally changes the dynamics of the model, and thus you must re-tune the model for the fault condition.

Fault Detection

set_param([mdl '/Autopilot/Multicopter Controller/Attitude Control'],'LabelModeActiveChoice','FaultDetection');

A fault in one of the rotors of the UAV leads to a change in dynamics of the roll and pitch loops. In this example, you use online polynomial model estimation to identify the roll-torque to roll-angular velocity plant model and detect the rotor failure based on change in the estimated parameters.

The input to the Recursive Polynomial Model Estimator block is the roll torque command by the attitude controller and the output is the measured roll angular velocity. The example uses the ARX model structure to identify the system as a second order model. A forgetting factor of 0.99 is set to detect the change in the estimated parameters. The enable port of the block starts parameter estimation only when the input is above a certain threshold and uses a simple averaging filter to smooth small perturbations in the result.

In the model, the visualizations help you observe the UAV during simulation. The first is an animation that shows the attitude of the UAV and the second plots the trajectory that the UAV follows.

simout = sim(mdl);

exampleHelperPlotEstimatedParameter(simout,faultParam);

The estimated B_1 parameter changes as the input torque changes, signifying the different operating conditions the UAV moves through as it navigates the trajectory. However, at the time of the rotor failure, the parameter estimate drops to a value close to 0 showing a reduction in the gain of the identified system. The model uses a threshold of 1 to detect the fault and simulations show that the detection happens within 0.5 seconds of the fault occurrence.

Control Design Reconfiguration and Tuning for Fault Condition

set_param([mdl '/Autopilot/Multicopter Controller/Attitude Control'],'LabelModeActiveChoice','ControlDesign');
open_system([mdl '/Autopilot/Multicopter Controller/Attitude Control/Control Design'])

Open Yaw Loop to Manage Yaw Rate

When the model introduces the fault condition, the new plant model loses a degree of controllability as one of its rotors becomes inoperable. For this example, control over the heading of the UAV is no longer maintained, but it is still possible to control the yaw rate of the UAV. By choosing this control design, the UAV can maintain altitude and track a predetermined trajectory while maintaining a constant yaw rate.

In this example, the yaw rate reference is set to a multiplicative factor of the height of the UAV aircraft. This way, the yaw rate can be decreased as the UAV craft approaches the ground. However, the UAV craft can only maintain control within a tight band of yaw rate references- if the yaw rate is too small, the UAV will lose lift, and if the yaw rate is too large, the UAV will be unable to actuate sufficiently to follow the planned trajectory.

Even after reconfiguration, the tracking of the predetermined path for the UAV is still quite poor. The reference tracking of the UAV can be improved by tuning the controllers at the new operating condition.

Linearize VTOL Plant Model and Tune Control Design

To tune the controllers, you must set the phase margin. The pidtune function uses a default value of 60 degrees. This value balances robustness and response time. A lower value of phase margin leads to more oscillations with a faster response, and a higher value leads to fewer oscillations with a slower response.

Run the exampleHelperAutomatedGainScheduledTuning.m script to linearize and trim each controller of affected control loops to achieve a phase margin of 60 degrees. In order to tune each loop, the script performs the following steps:

1. Obtain the linearization io points

2. Linearize the system using linearize

3. Obtain tuned controller using pidtune

4. Ensure the tuned filter coefficient is nonzero

5. Write the tuned gains to the workspace

For reference, the detailed comments outlining the steps for tuning the controllers are present in exampleHelperAutomatedGainScheduledTuning.m. Note that the process of tuning the parameters can take some time. For convenience, the tuned parameters are loaded by default.

After tuning, the tracking of the reference path for the UAV is much improved. In the following image, you can see that the UAV tracks the reference trajectory with acceptable performance despite the loss of a rotor.

The animation shows the UAV maintaining a specific yaw rate, but able to control the pitch and roll angles.

simout = sim(mdl);

exampleHelperPlotTrajectory(simout,faultParam,HoverMission);

Examine the attitude parameters. You can see that the yaw rate reference is closely matched, and the pitch and roll remain within acceptable limits.

exampleHelperPlotAttitude(simout,faultParam);

Summary

In this example, you tuned the control design of a multicopter aircraft in failure conditions. Additionally, you can follow the workflow described in this examples and perform gain scheduling for the altitude and position controllers for further improved tracking.

Autotuning Helper Script Definition

To examine the gain scheduling tuning process in more detail, see the exampleHelperAutomatedGainScheduledTuning.m file or use the following command.

type exampleHelperAutomatedGainScheduledTuning.m

%exampleHelperAutomatedGainScheduledTuning tune all control gains for hover
%mode.

% Copyright 2024 The MathWorks, Inc.

% Have fault occur instantly rather than over a period of time
isTuning = true; %#ok<NASGU>
% Take snapshot 1 time step after fault occurs
snapshotTime = faultParam.StartTime+0.005;
% Linearize in discrete time and tune with a phase margin of 60 degrees
options = linearizeOptions(SampleTime=0.005,RateConversionMethod='tustin');
ctrloptions = pidtuneOptions(PhaseMargin=60,DesignFocus='reference-tracking');

% Disable linearization warnings
state = warning('off', 'Simulink:blocks:BmathSqrtOfNegativeNumber');
cleanup = onCleanup(@()warning(state));

Tune_r = true;
Tune_phitheta = true;

%% Yaw rate
if Tune_r
    % STABLE FOR RATES -15 to -25
    faultParam.YawRateRef = -20;
    io(1) = linio([mdl '/Autopilot/Multicopter Controller/Attitude Control/Derivative Gain/Yaw rate controller'],1,'openinput');
    io(2) = linio([mdl '/Autopilot/Multicopter Controller/Attitude Control/Derivative Gain/Demux2'],3,'openoutput');
    yawRateSys=linearize(mdl,io,options,snapshotTime);

    [C,~]=pidtune(yawRateSys,'PIDF',50,ctrloptions);
    controlParams.P_YAW_RATE_FAILURE=C.Kp;
    controlParams.I_YAW_RATE_FAILURE=C.Ki;
    controlParams.D_YAW_RATE_FAILURE=C.Kd;
    if C.Tf<eps
        C.Tf = 0.01;
    end
    controlParams.N_YAW_RATE_FAILURE=1/C.Tf;
end

%% Roll
if Tune_phitheta
    io(1) = linio([mdl '/Autopilot/Multicopter Controller/Attitude Control/Derivative Gain/Roll Controller'],1,'openinput');
    io(2) = linio([mdl '/Autopilot/Multicopter Controller/Attitude Control/Derivative Gain/Demux3'],1,'openoutput');
    rollSys=linearize(mdl,io,options,snapshotTime);

    [C,~]=pidtune(rollSys,'PDF',10,ctrloptions);
    controlParams.P_ROLL_FAILURE=C.Kp;
    controlParams.I_ROLL_FAILURE=C.Ki; % NO I Gain because of 1/s plant
    controlParams.D_ROLL_FAILURE=C.Kd;
    if C.Tf<eps
        C.Tf = 0.01;
    end
    controlParams.N_ROLL_FAILURE=1/C.Tf;
end

%% Pitch
if Tune_phitheta
    io(1) = linio([mdl '/Autopilot/Multicopter Controller/Attitude Control/Derivative Gain/Pitch Controller'],1,'openinput');
    io(2) = linio([mdl '/Autopilot/Multicopter Controller/Attitude Control/Derivative Gain/Demux3'],2,'openoutput');
    pitchSys=linearize(mdl,io,options,snapshotTime);
    
    [C,~]=pidtune(pitchSys,'PDF',15,ctrloptions);
    controlParams.P_PITCH_FAILURE=C.Kp;
    controlParams.I_PITCH_FAILURE=C.Ki; % NO I Gain because of 1/s plant
    controlParams.D_PITCH_FAILURE=C.Kd;
    if C.Tf<eps
        C.Tf = 0.01;
    end
    controlParams.N_PITCH_FAILURE=1/C.Tf;
end

%% Discard IO points
setlinio(mdl,[]);
isTuning = false;