Quadrotor Trajectory Optimization:

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 track an optimal trajectory between an array of custom waypoints.

Copy-pasteable code:

using JuliaSimControl
using AerialVehicles
using SparseArrays, OSQP
using ModelingToolkit
using ControlSystemsBase.Polynomials
using StaticArrays
using WGLMakie, Rotations


Ta = [3, 3, 2, 2]
x = [0; 0; 0; 0]
xwp = [10 10 -10 0; 10 20 -10 0; 0 10 -5 0; 0 0 0 0]
vel = 0
px, py, pz, pyaw = optimal_trajectory_gen(Ta, x, xwp, vel)

# Wrapper for polynomial trajectory
gen_optimal_trajectory(t) = SynthesizePolynomialTrajectory(t, px, py, pz, pyaw, Ta)


# 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.0; 0.0 1.0 0.0; 0.0 0.0 1.0]
ω0 = [0, 0, 0]
x0_vec = build_initial_conditions(x0, v0, R0, ω0; quad_mtk = quad_mtk, func_sys = func_sys)

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

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.1, -0.1, -0.1), Vec3f(15, 15, 15))
quad_simulator(
    time_arr,
    x_arr,
    y_arr,
    z_arr,
    quat_arr;
    quad_scale = 2.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:

  • JuliaSimControl: Provides the control design framework
  • AerialVehicles: Provides the model components
  • ModelingToolkit: Provides the acausal modeling framework
  • StaticArrays: provides a framework for implementing statically sized arrays
  • WGLMakie: Provides the visuallization and animation framework
  • Rotations: Provides tools for coordinate transformtion
using JuliaSimControl
using AerialVehicles
using SparseArrays, OSQP
using ModelingToolkit
using ControlSystemsBase.Polynomials
using StaticArrays
using WGLMakie, Rotations

2. Specify trajectory properties and time segments

Specify:

  • Array of time segments
  • The starting point of the Quadrotor
  • Array of waypoints to be traversed
  • Starting velocity

We utillize JuliaSimControl's polynomial trajectory generation function optimal_trajectory_gen to compute the coefficients of the polynomial trajectory in time. Finally, we utillize the SynthesizePolynomialTrajectory function in AerialVehicles along with a wrapper function to define the desired trajectory properties. 

Ta = [3, 3, 2, 2]
x = [0; 0; 0; 0]
xwp = [10 10 -10 0; 10 20 -10 0; 0 10 -5 0; 0 0 0 0]
vel = 0
px, py, pz, pyaw = optimal_trajectory_gen(Ta, x, xwp, vel)

# Wrapper for polynomial trajectory
gen_optimal_trajectory(t) = SynthesizePolynomialTrajectory(t, px, py, pz, pyaw, Ta)

3. 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 = 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)

5. 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.0; 0.0 1.0 0.0; 0.0 0.0 1.0]
ω0 = [0, 0, 0]
x0_vec = build_initial_conditions(x0, v0, R0, ω0; quad_mtk = quad_mtk, func_sys = func_sys)

6. 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. This function requires

  • Final time of the simulation
  • Initial conditions
  • Quadrotor dynamics function
  • JuliaSimControl's FunctionSystem model of the dynamics
  • Choice of controllers
  • Gains of the controllers
  • Desired trajectory function wrapped over SynthesizePolynomialTrajectory function
Tf = 10.0
state_arr = run_quadrotor_sim(
    Tf,
    x0_vec,
    quadrotor_dynamics!,
    func_sys,
    QuadModel,
    GeometricControllers,
    ControlGains,
    gen_optimal_trajectory,
)

7. 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 = 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.1, -0.1, -0.1), Vec3f(15, 15, 15))
quad_simulator(
    time_arr,
    x_arr,
    y_arr,
    z_arr,
    quat_arr;
    quad_scale = 2.0,
    limit_scene = Scenelim,
)