UAV Inflight Failure Recovery with Emergency Landing

This example shows how to use Control System Tuner to tune the fixed-structure PID controllers of a multicopter for nominal flight conditions and fault conditions. Here, you use a gain-scheduled approach to recover from a single rotor failure and land the UAV.

UAV Package Delivery Model

This example uses a model based on the model discussed in the UAV Package Delivery (UAV Toolbox) example. The high-fidelity 6-DOF plant model is based on Simulink Drone Reference Application.

Open the Simulink® Project.

prj = openProject('scdUAVInflightFailureRecovery');

In this example, a single rotor failure is simulated by injecting a multiplicative gain vector in the plant. To demonstrate the simulation results of the gain-scheduled controllers, the actuator commands are used as features to design a fault detection algorithm, and a threshold based on nominal and fault induced simulations is used to detect the fault.

The controller consists of a position (X, Y) and attitude (pitch, roll) loop, a yaw control loop, and an altitude (Z) control loop. The Rotor Fault Injection Gains block is used to define the fault condition for the plant model. Control System Tuner is used to tune the inner attitude controller and the altitude controller. The gains are tuned for nominal flight conditions and fault conditions. A fault-recovery control strategy is implemented using Varying PID Controller and Lookup Table blocks scheduled based on the fault detection indicator. The following figure shows the controller subsystem used for tuning.

open_system('MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/Controller/Controller')

Controller Tuning

This section describes how to specify tunable elements and create tuning goals using Control System Tuner. To launch a preconfigured session instead, use Project Shortcuts.

The controller and plant are extracted in a separate model, MultirotorModelControlDesign.slx, set up for tuning controller gains using Control System Tuner. The initial condition for the integral gains is set as 0.01 to suppress overshoot while maintaining zero steady-state error. The rotor gain multiplier parameter is set to indicate nominal mode with all rotors operational.

Open the model.

open_system("MultirotorModelControlDesign.slx")

To launch the Control System Tuner app, in the Simulink model window, in the Apps gallery, click Control System Tuner.

Specify to linearize the model at a sample time of 0.005 seconds. To do so, click Linearization Options, select Discrete with sample time, and enter 0.005.

Parameter Variations for Multiple Models

To specify parameter variations, on the Control System tab, click Select parameters to vary from the Parameter Variations list. To select model variables, on the Parameter Variations tab, click Manage Parameters. Select the rotor4ThrustGain parameter and click OK to add it to the Parameter Variations table.

Enter 0 for the second row value. The parameter variation defines the nominal flight (rotor4ThrustGain = 1) and single rotor failure (rotor4ThrustGain = 0) conditions.

Select Tunable Blocks

To select blocks for tuning, on the Tuning tab, click Select Blocks. Then, on the Select Tuned Blocks dialog, click Add Blocks. This opens the editor for tuned blocks where you can specify which blocks are tunable.

Select the controller gains in the attitude controller and altitude controller subsystems as tunable.

Specify Tuning Goals

To specify a tuning goal, on the Tuning tab, click New Goal. The Step Tracking Goal is used to specify the response and desired characteristics for controlling the pitch and roll angles. The reference pitch and roll signals and measured signals in the UAVState signal are marked as inputs and outputs for the tuning goal, respectively. The position controller loop is opened to tune the attitude control loop in isolation. Enter the Time constant parameter as 0.1 s and apply the tuning goal to the nominal model (first row of Parameter Variation).

Similarly, use the Step Tracking Goal and Loop Shape Goal dialogs to create the full set of tuning goals required to tune the attitude and altitude controller gains.

The control objectives for the attitude loop are:

• Step tracking tuning goal to control pitch and roll with a desired first order response with a time constant of 0.1 seconds

• Loop shape goal to suppress gain of feedback of pitch and roll loops at high frequency, specified as an integrator with bandwidth of 10 Hz

The control objectives for the altitude loop are:

• Step tracking tuning goal with a desired first order response with a time constant of 1 second

• Loop shape goal to suppress gain of feedback at high frequency, specified as an integrator with bandwidth of 10 Hz

Tuning Controller Parameters for Nominal Model

Follow the preceding section to set up the Control System Tuner session for tuning controller gains for nominal flight mode. Alternatively, use the shortcut Tune controller for nominal flight in the project to launch Control System Tuner with a preconfigured session file.

The tuning goal plots show that with the untuned controller gains, the attitude control loop is unstable and the altitude control loop does not have the desired response. Use the Manage Goals option to select and edit a tuning goal.

Click Tune to adjust the values of the tunable blocks to achieve the tuning goals. Both the attitude and altitude control loops are tuned together with the yaw control loop fixed.

To see the values of tuned controller gains, select a block in the Tuned Blocks and view the value in the Data Preview area of the Control System Tuner. For more information, see Examine Tuned Controller Parameters in Control System Tuner.

The tuned controller gains are:

• Outer proportional loop control for pitch — $$K_p=9.667$$

• Inner PI control for pitch angular velocity — $$K_p=0.004296,K_i=0.01$$

• Outer proportional loop control for roll — $$K_p=9.572$$

• Inner PI control for roll angular velocity — $$K_p=0.003494,K_i=0.01$$

• PID control for altitude — $$K_p=2.856,K_i=0.01,K_d=3.242$$

Tune Parameters for Fault Model

The defined parameter variation, rotor4ThrustGain = 0, generates the model for single rotor failure. Change the tuning goals to apply to the fault model instead of the nominal model. The yaw control loop is set to open for all tuning goals because with the loss of thrust to one rotor the diagonal rotor pairs are unbalanced and the yaw is uncontrolled. The desired time constant for the StepTracking - Altitude tuning goal is modified to 2 seconds, which reduces the landing velocity of the multicopter.

Double-click to open and modify each objective under Tuning Goals in Data Browser. Alternatively, use the shortcut Tune controller for rotor failure in the project to launch Control System Tuner with a preconfigured session file.

Click Tune to retune the parameters based on the fault model. As seen in the following plots, small overshoots and oscillations exist in the step response of pitch and roll as a result of the rotor failure.

The tuned controller gains for a plant with a single rotor failure are:

• Outer proportional loop control for pitch — $$K_p=11.02$$

• Inner PI control for pitch angular velocity — $$K_p=0.006415,K_i=0.01$$

• Outer proportional loop control for roll — $$K_p=11.42$$

• Inner PI control for roll angular velocity — $$K_p=0.005209,K_i=0.01$$

• PID control for altitude — $$K_p=1.667,K_i=0.01,K_d=4.098$$

Simulation with Injected Fault and Gain-Scheduled PID Controllers

The simulation model implements a fault detection subsystem that extracts features from the actuator commands and sets a threshold to detect the fault in rotor 4. The fault detection indicator is used as the scheduling variable for the gain-scheduled controllers in the attitude loop and altitude loop. Also, the pitch, roll, and altitude reference inputs are reconfigured to command the multicopter to land with a safe velocity.

load_system("MultirotorModel");
open_system("MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault");

The gain-scheduled controllers are implemented using the Varying PID Controller block and Lookup Table blocks to specify the gains, as shown for the pitch angular velocity controller. Update the gains computed in the previous section in the table data of the blocks defined at breakpoints of nominal operation (0) and fault (1).

Similarly, update the gains for the remaining controllers.

Alternatively, you can set the block parameters using the following commands. If you modify any tuning goal, replace the provided values with your tuned controller gain values.

Set the pitch controller parameters as follows.

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/Attitude Controller/Gain_PitchAngle'],'TableData','[9.669, 11.02]');

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/Attitude Controller/PI Pitch/Kp'],'TableData','[0.004296, 0.006416]');

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/Attitude Controller/PI Pitch/Ki'],'TableData','[0.01, 0.01]');

Set the roll controller parameters as follows.

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/Attitude Controller/Gain_RollAngle'],'TableData','[9.572, 11.42]');

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/Attitude Controller/PI Roll/Kp'],'TableData','[0.003493, 0.005209]');

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/Attitude Controller/PI Roll/Ki'],'TableData','[0.01, 0.01]');

Set the altitude controller parameters as follows.

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/gravity feedforward//equilibrium thrust/Kp'],'TableData','[3.004, 1.667]');

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/gravity feedforward//equilibrium thrust/Ki'],'TableData','[0.01, 0.01]');

set_param(['MultirotorModel/Inner Loop and Plant Model/High-FidelityModel_RotorFault/' ...
    'Controller/Controller/gravity feedforward//equilibrium thrust/Kd'],'TableData','[3.308, 4.098]');

Use the Simulate model with fault injection project shortcut to simulate the model to takeoff and fly the multicopter based on position commands from the guidance logic subsystem. A rotor fault is introduced at 30 seconds by reducing the rotor thrust to 30%. Results for altitude position (top row) and the attitude (middle row) in the Simulation Data Inspector show that the UAV settles to within 5% of its desired altitude and has smooth pitch and roll to enable it to track X and Y positions. The bottom row shows the actuator commands for the four rotors.

When a rotor fails at 30 seconds, the UAV starts pitching and rolling. The controller is reconfigured as the failure is detected. The pitch and roll are controlled to settle at 0 radians and the UAV lands while maintaining a low velocity. As expected, the yaw is uncontrolled and the UAV does spin around the vertical axis. The maximum yaw rate that the UAV reaches is verified through simulation, and the attitude and altitude controllers are retuned with modified tuning goals, if necessary.

The UAV is visualized in a photorealistic environment and shows the UAV flying in a realistic world. As the simulation starts, press 'F' to set the camera mode to Free in the AutoVrtlEnv window and use the mouse scroll wheel to increase the camera distance from the UAV. Using these controls, you can visualize the landing sequence following the rotor failure.

The following plot shows the transients after occurrence of the fault at 30 seconds.

Close the project.

close(prj);