PD controller is developed to control a quadrotor in 2-dimensional space. The quadrotor has two inputs: motor thrusts from the left and the right motors. They cause a net force and torque applied to the quadrotor: The net force and torque will be used as the two inputs to the quadrotor to simplify subsequent calculations. The new schematic diagram becomes: (input) f(t) = force applied on the quadrotor by the motors (input) τ(t) = torque applied on the quadrotor by the motors (output) y(t) = horizontal position unknown (output) z(t) = vertical position (height) unknown (output) Φ(t) = tilt (angle) unknown m = mass Ixx = moment of inertia g = gravitational acceleration

## Differential Equation

3 unknowns so 3 equations are needed. Newton's 2nd law (y-axis):

(eq. 1) Newton's 2nd law (z-axis):

(eq. 2) Newton's 2nd law (rotation):

(eq. 3) ## State Space Model

States: Position and velocity are chosen as state variables because,

• Position and velocity are commonly chosen when there is mass in the system.
• Position and velocity along with the input variable (force) are sufficient to determine the system's future output (position).
• Highest derivative in the differential equation is n = 2 (acceleration) so this is a second order system. States are chosen up to (n-1)th derivative (velocity).

State vector: Input vector: Output vector: Rewrite (eq. 1) in these new notations: Rewrite (eq. 2) in these new notations: Rewrite (eq. 3) in these new notations: Rearrange equations to express ẋ(t) and y(t) in terms of x(t) and u(t): Rewrite as matrix:

(solution) The dynamics model is nonlinear because it is not of the form ẋ=Ax+Bu.

## Linearization

PD controller is designed for linear systems so the dynamics model is first linearized. Model is linearized at the equilibrium point, that is, at hover configuration. At this point, the following is true: Trigonometric functions can also be linearized at this equilibrium point using first order Taylor approximation: Using these facts, linearized dynamics model can be written as:

(eq. 4) ## Controller

The objective is to design a controller (find the input force and torque functions) which makes the quadrotor track a trajectory (position, velocity, and acceleration as a function of time). PD controller can be written as, (eq. 4) can be solved for the inputs: Substitute the PD controller to obtain the control equations: There are still four unresolved variables: Because Φ cannot be commanded directly, let Φcommanddesired: The derivatives of Φcommand can be approximated as 0: The derivative of Φcommand is proportional to the third derivative of y (jerk in the horizontal direction). The second derivative of Φcommand is proportional to the fourth derivative of y (snap in the horizontal direction). For non-aggressive trajectories with small jerk and snap, these can be approximated as 0.

## Simulation

Python code simulation. Quadrotor hovers for 1 second and then tries to reach target position of z=0m and y=0.5.

```from math import sin, cos
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt

# Constants
g     = 9.81    # Gravitational acceleration (m/s^2)
m     = 0.18    # Mass (kg)
Ixx   = 0.00025 # Mass moment of inertia (kg*m^2)
L     = 0.086   # Arm length (m)

# Returns the desired position, velocity, and acceleration at a given time.
# Trajectory is a step (y changes from 0 to 0.5 at t=1)
#
# t     : Time (seconds), scalar
# return: Desired position & velocity & acceleration, y, z, vy, vz, ay, az
def trajectory(t):
if t < 1:
y = 0
else:
y = 0.5
z = 0
vy = 0
vz = 0
ay = 0
az = 0

return y, z, vy, vz, ay, az

# Returns force and moment to achieve desired state given current state.
# Calculates using PD controller.
#
# x     : Current state, [y, z, phi, vy, vz, phidot]
# y_des : desired y
# z_des : desired z
# vy_des: desired y velocity
# vz_des: desired z velocity
# ay_des: desired y acceleration
# az_des: desired z acceleration
# return: Force and moment to achieve desired state
def controller(x, y_des, z_des, vy_des, vz_des, ay_des, az_des):
Kp_y   = 0.4
Kv_y   = 1.0
Kp_z   = 0.4
Kv_z   = 1.0
Kp_phi = 18
Kv_phi = 15

phi_c = -1/g * (ay_des + Kv_y * (vy_des - x) + Kp_y * (y_des - x))
F = m * (g + az_des + Kv_z * (vz_des - x) + Kp_z * (z_des - x))
M = Ixx * (Kv_phi * (-x) + Kp_phi * (phi_c - x))

return F, M

# Limit force and moment to prevent saturating the motor
# Clamp F and M such that u1 and u2 are between 0 and 1.7658
#
#    u1      u2
#  _____    _____
#    |________|
#
# F = u1 + u2
# M = (u2 - u1)*L
def clamp(F, M):
u1 = 0.5*(F - M/L)
u2 = 0.5*(F + M/L)

if u1 < 0 or u1 > 1.7658 or u2 < 0 or u2 > 1.7658:
print(f'motor saturation {u1} {u2}')

u1_clamped = min(max(0, u1), 1.7658)
u2_clamped = min(max(0, u2), 1.7658)
F_clamped = u1_clamped + u2_clamped
M_clamped = (u2_clamped - u1_clamped) * L

return F_clamped, M_clamped

# Equation of motion
# dx/dt = f(t, x)
#
# t     : Current time (seconds), scalar
# x     : Current state, [y, z, phi, vy, vz, phidot]
# return: First derivative of state, [vy, vz, phidot, ay, az, phidotdot]
def xdot(t, x):
y_des, z_des, vy_des, vz_des, ay_des, az_des = trajectory(t)
F, M = controller(x, y_des, z_des, vy_des, vz_des, ay_des, az_des)
F_clamped, M_clamped = clamp(F, M)

# First derivative, xdot = [vy, vz, phidot, ay, az, phidotdot]
return [x,
x,
x,
-F_clamped * sin(x) / m,
F_clamped * cos(x) / m - g,
M_clamped / Ixx]

x0     = [0, 0, 0, 0, 0, 0] # Initial state [y0, z0, phi0, vy0, vz0, phidot0]
t_span = [0, 20]            # Simulation time (seconds) [from, to]

# Solve for the states, x(t) = [y(t), z(t), phi(t), vy(t), vz(t), phidot(t)]
sol = solve_ivp(xdot, t_span, x0)

# Plot
fig, axs = plt.subplots(3)
axs.plot(sol.t, sol.y) # y   vs t
axs.plot(sol.t, sol.y) # z   vs t
axs.plot(sol.t, sol.y) # phi vs t
plt.show()
```

Result: Note 1: PID is better when there are disturbances (like wind) and modeling errors (unknown mass) because it makes steady-state error go to zero. In such case, the closed loop system becomes third-order.

Note 2: information on this page is based off of an example in an online course. It is modified and extended with additional information.

 Coursera, Robotics: Aerial Robotics