Quadrotor Stabilization:

This tutorial walks through the setup, simulation and visuallization of a Quadrotor (Quadcopter) drone model controlled using a Geometric controller. Specifically the simulation will demonstrate the use of a Geometric controller to stabilize when initiallized upside down.

Copy-pasteable code:

using AerialVehicles
using JuliaSimControl
using ModelingToolkit
using WGLMakie
using Rotations

# Generating structs for model and control parameters
QuadModel = QuadrotorModel(
    mass = 4.34,
    I = cat(0.0820, 0.0845, 0.1377, dims = (1, 2)),
    Ts = 0.01,
    g = 9.81,
)
ControlGains =
    GeometricControlParams(kx = 4 * 4.34, kv = 5.6 * 4.34, kr = 8.81, kOmega = 2.54);
GeometricControllers =
    Controllers(geometric_force_controller, geometric_attitude_controller)
func_sys, mtk_quad = build_quad_dynamics(QuadModel)
quad_mtk = complete(mtk_quad)

# Initial conditions
x0 = [0, 0, 0]
v0 = [0, 0, 0]
R0 = [1 0 0; 0 -0.9995 -0.0314; 0 0.0314 -0.9995]
ω0 = [0, 0, 0]
x0_vec = build_initial_conditions(x0, v0, R0, ω0; quad_mtk = quad_mtk, func_sys = func_sys)

function stabilize_rotor(t)
    xdes = zeros(15)
    b1des = zeros(9)
    b1des[1] = 1
    return xdes, b1des
end

#Initiallising sim
Tf = 10.0
state_arr = run_quadrotor_sim(
    Tf,
    x0_vec,
    quadrotor_dynamics!,
    func_sys,
    QuadModel,
    GeometricControllers,
    ControlGains,
    stabilize_rotor,
)

time_arr = state_arr.t
x_arr = state_arr[1, 1, :]
y_arr = state_arr[2, 1, :]
z_arr = state_arr[3, 1, :]

# Generating Quaternion matrix for Makie animation
R_arr = state_arr[7:15, 1, :]
quat_arr = quadrotor_RtoQuat(R_arr)

# Setting limits of animation
Scenelim = Rect(Vec3f(-0.5, -0.5, -0.5), Vec3f(2, 2, 2))
# Animating quadrotor
quad_simulator(
    time_arr,
    x_arr,
    y_arr,
    z_arr,
    quat_arr;
    quad_scale = 1.0,
    limit_scene = Scenelim,
)
┌ Warning: Using arrays or dicts to store parameters of different types can hurt performance.
│ Consider using tuples instead.
└ @ SciMLBase /home/github_actions/depots/deepsea1.2/packages/SciMLBase/GmToj/src/performance_warnings.jl:32

Step-by-Step Solution:

1. Import required packages:

  • AerialVehicles: Provides the model components
  • JuliaSimControl: Provides the control design framework
  • ModelingToolkit: Provides the acausal modeling framework
  • WGLMakie: Provides the visuallization and animation framework
  • Rotations: Provides tools for coordinate transformtion
using AerialVehicles
using JuliaSimControl
using ModelingToolkit
using WGLMakie
using Rotations

1. Define required properties and build model:

Specify:

  • Properties of the quadrotor to be simulated
  • Control gains
  • Choice of controllers

After defining the model, build the model and use ModellingToolkit.jl's complete function 

QuadModel = QuadrotorModel(
    mass = 4.34,
    I = diagm([0.0820, 0.0845, 0.1377]),
    Ts = 0.01,
    g = 9.81,
)
ControlGains =
    GeometricControlParams(kx = 4 * 4.34, kv = 5.6 * 4.34, kr = 8.81, kOmega = 2.54);
GeometricControllers =
    Controllers(geometric_force_controller, geometric_attitude_controller)

func_sys, mtk_quad = build_quad_dynamics(QuadModel)
quad_mtk = complete(mtk_quad)

2. Define initial conditions

Set up the initial conditions for the simulation. The states of the quadrotor are:

  • (x,y,z) position of the quadrotor
  • Velocity components in the x,y,z direction
  • Rotation matrix of representing orientation of the quadrotor
  • Angular velocity components along the axes
x0 = [0, 0, 0]
v0 = [0, 0, 0]
R0 = [1 0 0; 0 -0.9995 -0.0314; 0 0.0314 -0.9995]
ω0 = [0, 0, 0]
x0_vec = build_initial_conditions(x0, v0, R0, ω0; quad_mtk = quad_mtk, func_sys = func_sys)

3. Setup objective trajectory

The model of the quadrotor requires a trajectory or destination to be reached modelled as a function so we write a function to set the destination of the quadrotor back to the origin and set the orientation of the quadrotor to be stabilized facing right side up

function stabilize_rotor(t)
    xdes = zeros(15)
    b1des = zeros(9)
    b1des[1] = 1
    return xdes, b1des
end

4. Running the simulation

Here we set up and run the simulation for 10 sec. We use the run_quadrotor_sim function to obtain the states of the quadrotor 

#Initiallising sim
Tf = 10.0
state_arr = run_quadrotor_sim(
    Tf,
    x0_vec,
    quadrotor_dynamics!,
    func_sys,
    QuadModel,
    GeometricControllers,
    ControlGains,
    stabilize_rotor,
)

5. Visualising the trajectory

We use WGLMakie.jl to visualize the orientation and trajectory of the quadrotor being simulated. In order to input data into the WGLMakie.jl visualization tool, we need the rotation matrix to be converted to quaternion data. To do this we use quadrotor_RtoQuat function. Once the converted quaternion array is obtained, we can run the visualization tool

time_arr = 0:tn:n*tn

x_arr = state_arr[13, :]
y_arr = state_arr[15, :]
z_arr = state_arr[17, :]

u_arr = state_arr[14, :]
v_arr = state_arr[16, :]
w_arr = state_arr[18, :]

ω_arr = state_arr[10:12, :]

# Generating Quaternion matrix for Makie animation
R_arr = state_arr[1:9, :]
quat_arr = quadrotor_RtoQuat(R_arr)
# Setting limits of animation
Scenelim = Rect(Vec3f(-0.5, -0.5, -0.5), Vec3f(2, 2, 2))
# Animating quadrotor
quad_simulator(
    time_arr,
    x_arr,
    y_arr,
    z_arr,
    quat_arr;
    quad_scale = 1.0,
    limit_scene = Scenelim,
)