# Quadcopter 2D
## Summary
This notebook outlines a position controller for a quadcopter in 2D space.

## Goals
The ultimate goal is to apply the control system to a simulated environment - for this I have chosen Python and specifially [pymunk](http://www.pymunk.org) as the 2D physics simulator and [pyglet](http://www.pyglet.org) as the game engine to render the results.

### Control System Requirements
The design criteria for the quadrotor is to reach a step input (1, 1):
* Settling time for y, z and Θ of less than 5 seconds
* Rise time for y, z and Θ of less than 2 seconds
* Overshoot of y, and z less than 5%

## System Description
We will use the following diagram to derive the equations of motion:



* $\theta$ is 0 radians when the quadcopter is level and is positive in the CCW direction
* $y, z$ are the quadcopter's center of mass position
* $F_1, F_2$ are the force from the right and left rotors respectively
* $u_1$ is the sum of thrusts and is equal to $F_1 + F_2$
* $u_2$ is the sum of moments and is equal to $l(F_1 - F_2)$
* $I$ is the quadcopter moment of inertia
* $m$ is the mass of the quadcopter, in kg
* $l$ is the distance of the rotors with respect to the center of mass
* $g$ is gravity

### Equations of Motion
The equations of motion for this system are derived using the Lagrangian and Euler equations. This derivation references the following sources:
* [Brilliant: Lagrangian Mechanics](https://brilliant.org/wiki/lagrangian-formulation-of-mechanics/)


```python
import sympy
import matplotlib.pyplot as plt
import numpy as np

sympy.init_printing()
```


```python
# create our functions for y, z and Θ against time, t
t = sympy.symbols('t')
y = sympy.Function('y')(t)
z = sympy.Function('z')(t)
Θ = sympy.Function('Θ')(t)

# constants: length from CoM to rotor, mass, inertia, gravity, force from left rotor, force from right rotor
l, m, I, g, F1, F2 = sympy.symbols('l m I g F1 F2')

# velocity of the drone is made up of velocity in y and z directions
v = sympy.sqrt(y.diff(t)**2 + z.diff(t)**2)

# kinetic energy of the drone subtract potential energy
L = sympy.Rational(1, 2)*(m*v**2 + I*Θ.diff(t)**2) - m*g

# print out the Lagrangian
L
```


```python
# euler equations
euler1 = L.diff(y.diff(t)).diff(t) - L.diff(y)
euler2 = L.diff(z.diff(t)).diff(t) - L.diff(z)
euler3 = L.diff(Θ.diff(t)).diff(t) - L.diff(Θ)

# create the equations, first arg is LHS, second is RHS
eq1 = sympy.Eq(euler1, -(F1 + F2)*sympy.sin(Θ))
eq2 = sympy.Eq(euler2, (F1 + F2)*sympy.cos(Θ) - m*g)
eq3 = sympy.Eq(euler3, (F1-F2)*l)

# print out the equations
eq1, eq2, eq3
```


```python
# linearize our system for hover
# sin(Θ) = Θ, cos(Θ) = 1, (d/dt(Θ))**2 = 0
linearize = [(sympy.sin(Θ), Θ), (sympy.cos(Θ), 1), (Θ.diff(t)**2, 0)]
eq1_lin = eq1.subs(linearize)
eq2_lin = eq2.subs(linearize)
eq3_lin = eq3.subs(linearize)

# print out the linearized equations
eq1_lin, eq2_lin, eq3_lin
```


```python
# solve our linear system for linear and angular accelerations
eq_of_mot = sympy.linsolve([eq1_lin, eq2_lin, eq3_lin], [y.diff(t, t), z.diff(t, t), Θ.diff(t, t)])

eq_of_mot
```

#### State Space Representation

*NOTE: linearized equations for hover*

$x(t) =
\begin{bmatrix}
x_1 \\
x_2 \\
x_3 \\
x_4 \\
x_5 \\
x_6 \\
\end{bmatrix} = 
\begin{bmatrix}
y \\
z \\
\theta \\
\dot{y} \\
\dot{z} \\
\dot{\theta} \\
\end{bmatrix}$

$\dot{x(t)} =
\begin{bmatrix}
\dot{x_1} \\
\dot{x_2} \\
\dot{x_3} \\
\dot{x_4} \\
\dot{x_5} \\
\dot{x_6} \\
\end{bmatrix} = 
\begin{bmatrix}
x_4 \\
x_5 \\
x_6 \\
\ddot{y} \\
\ddot{z} \\
\ddot{\theta} \\
\end{bmatrix} = 
\begin{bmatrix} 
\dot{y} \\
\dot{z} \\
\dot{\theta} \\
\dfrac{-u_1\theta}{m} \\
\dfrac{u_1}{m} - g \\
\dfrac{u_2}{I}
\end{bmatrix}= 
\begin{bmatrix} 
x_4 \\
x_5 \\
x_6 \\
\dfrac{-u_1x_3}{m} \\
\dfrac{u_1}{m} - g \\
\dfrac{u_2}{I}
\end{bmatrix}$

where:

$u = 
\begin{bmatrix}
u_1 \\
u_2
\end{bmatrix} = 
\begin{bmatrix}
F_1 + F_2 \\
l(F_1 - F_2)
\end{bmatrix}$


```python
# define our ivp function using our state space representation
def quadcopter(t, y, F1, F2, constants):
    u1 = F1 + F2
    u2 = constants['l'] * (F1 - F2)
    x1dot = y[3]
    x2dot = y[4]
    x3dot = y[5]
    x4dot = (-u1 * y[2]) / constants['m']
    x5dot = (u1 / constants['m']) - constants['g']
    x6dot = u2 / constants['I']
    return [x1dot, x2dot, x3dot, x4dot, x5dot, x6dot]
```


```python
from scipy.integrate import solve_ivp

y0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

F1, F2 = 0.0, 0.0
constants = dict(
    l = 0.15,  # 15 cm from CoM to rotor
    m = 0.2,   # 200 g
    I = 0.001, # kg*m^2
    g = 9.81   # gravity, m/s^2
)
args = (F1, F2, constants)
sol = solve_ivp(quadcopter, [0, 10], y0, args=args, dense_output=True)
```


```python
def plot_all(title, t, z):
    plt.figure(figsize=(20, 12))

    plt.subplot(2, 3, 1)
    plt.plot(t, z.T[:,0], 'r')
    plt.xlabel('time (s)')
    plt.ylabel('Y position (m)')
    plt.title(title)
    plt.grid(True)

    plt.subplot(2, 3, 2)
    plt.plot(t, z.T[:,1], 'b')
    plt.xlabel('time (s)')
    plt.ylabel('Z position (m)')
    plt.grid(True)

    plt.subplot(2, 3, 3)
    plt.plot(t, z.T[:,2], 'g')
    plt.xlabel('time (s)')
    plt.ylabel('Θ angle (radians)')
    plt.grid(True)

    plt.subplot(2, 3, 4)
    plt.plot(t, z.T[:,3], 'y')
    plt.xlabel('time (s)')
    plt.ylabel('Y velocity (m/s)')
    plt.grid(True)

    plt.subplot(2, 3, 5)
    plt.plot(t, z.T[:,4], 'm')
    plt.xlabel('time (s)')
    plt.ylabel('Z velocity (m/s)')
    plt.grid(True)

    plt.subplot(2, 3, 6)
    plt.plot(t, z.T[:,5], 'c')
    plt.xlabel('time (s)')
    plt.ylabel('Θ angular velocity (radians/s)')
    plt.grid(True)

    plt.xlabel('time (s)')
    plt.show()
```


```python
t = np.linspace(0, 10, 100)
z = sol.sol(t)

# we expect the quadcopter to fall from the sky, z decreasing exponentially, and zdot (velocity) increasing linearly, all other states should be 0
plot_all('Quadcopter Falling', t, z)
```


```python
# each rotor will exert half of the exact force to keep it hovering
hover_force = constants['g'] * constants['m'] / 2

# F1, F2, l, m, I, g
args = (hover_force, hover_force, constants)
sol = solve_ivp(quadcopter, [0, 10], y0, args=args, dense_output=True)

t = np.linspace(0, 10, 100)
z = sol.sol(t)

# we expect all states to be 0 in this case, as the quadcopter is hovering at its start position
plot_all('Quadcopter Hovering', t, z)
```


```python
# set F2 to slightly more
# F1, F2, l, m, I, g
args = (hover_force, hover_force+0.00001, constants)
sol = solve_ivp(quadcopter, [0, 10], y0, args=args, dense_output=True)

t = np.linspace(0, 10, 100)
z = sol.sol(t)

# in this case, we expect the quadcopter to to start moving in the y direction, gain some height, and start rotating CW
plot_all('Quadcopter - Constant Uneven Rotor Force', t, z)
```

#### Control Equations - PD
Design a controller that hover's the quadcopter at a given position. In this case, the desired accelerations and velocities will be zero at steady state.

We will ignore any error at this point - we assume that the quadcopter will not be affected by external sources, such as wind.

From our previous derivation, we know the following:

$\ddot{y}=\dfrac{-u_1\theta}{m}\\
\ddot{z}=\dfrac{u_1}{m} - g\\
\ddot{\theta}=\dfrac{u_2}{I}$

To create a PD control equation, we can apply a proportional gain to the distance error and a derivative gain to the velocity error. Since we are going to command the system to a location, we will want the reference velocity to be zero, and we can specify the derivative and proportional gains as $K_d, K_p$, and our location reference with a subscript $_r$:  
$e_z(t) = K_{dz}(\dot{z_r}-\dot{z}) + K_{pz}(z_r - z)\\
e_y(t) = K_{dy}(\dot{y_r}-\dot{y}) + K_{py}(y_r - y)\\
e_Θ(t) = K_{dΘ}(\dot{Θ_r}-\dot{Θ}) + K_{pΘ}(Θ_r - Θ)$

and we can now solve for $u_1$ and substitute in our PD equation:  
$u_1 = m(g + \ddot{z_r} + K_{dz}(\dot{z_r}-\dot{z}) + K_{pz}(z_r - z))\\
\theta_r = \dfrac{-\ddot{y}}{g} = \dfrac{-1}{g}(\ddot{y_r} + K_{dy}(\dot{y_r}-\dot{y}) + K_{py}(y_r - y))\\
u_2 = K_{dΘ}(-\dot{Θ}) + K_{pΘ}(Θ_r - Θ)$

**NOTE:** for a step input with hover equilibrium, we can zero out the reference accelerations and velocities, but if using a trajectory generator that computes these values, then you can use them. See the python code in quadcopter2d.py where the reference accelerations and velocites are used.


```python
# define our ivp function using the control equations, and pass in our controller PD variables for each state
# pd objects are tuple where element 0 is proportional, and element 1 is derivative
# ref is a 2 element tuple represent y, z
def quadcopter_pid(t, y, constants, pdy, pdz, pdΘ, ref):
    u1 = constants['m']*(constants['g'] - y[4]*pdz[1] + (ref[1] - y[1])*pdz[0])
    Θc = (-1 / constants['g']) * (-pdy[1]*y[3] + (ref[0] - y[0])*pdy[0])
    u2 = -pdΘ[1]*y[5] + pdΘ[0]*(Θc - y[2])
    x1dot = y[3]
    x2dot = y[4]
    x3dot = y[5]
    x4dot = (-u1 * y[2]) / constants['m']
    x5dot = (u1 / constants['m']) - constants['g']
    x6dot = u2 / constants['I']
    return [x1dot, x2dot, x3dot, x4dot, x5dot, x6dot]
```


```python
# PD gains for y, z, and Θ and finally the reference point
pdy, pdz, pdΘ, ref = (12, 6.5), (12, 6.5), (50, 3), (1, 1)
args = (constants, pdy, pdz, pdΘ, ref)
sol = solve_ivp(quadcopter_pid, [0, 10], y0, args=args, dense_output=True)

t = np.linspace(0, 10, 100)
z = sol.sol(t)

# in this case we expect the PD controllers to move the quad copter to the reference point
plot_all(f'Quadcopter moving from (0,0) to {ref}', t, z)
```

#### Applying this to Code
The gains above will not work for a discrete system - we would need to simulate our system using a discrete model - which I will try to do in future. 

In this case, I manually tuned the discrete gains in the code and ran the simulation until I was happy (eyeballed it). In the control loop, we will sample our current state, and apply the gains in much the same way as the formulae above. Once we have the value for u1, and u2, we can then determine the actual forces that need to be applied by the rotors. We have 2 unknowns and 2 equations, and can use linear algebra to solve the system of equations:

$Ax = b$

where:

$A = \begin{bmatrix}
  1 & 1 \\
  l & -l \\
\end{bmatrix} \\
x = \begin{bmatrix}
F_1 \\
F_2
\end{bmatrix} \\
b = \begin{bmatrix}
u_1 \\
u_2 
\end{bmatrix}$

and we can solve the system by inverting A and matrix multiplying it to b:

$x = A^-1b$

so we can pre-compute the inverse of A ahead of time, since it will be constant:
```python
A = np.array([[1, 1], [arm_length, -arm_length]])
A_inv = linalg.inv(A)
```

and in our code, we can calculate $u_1, u_2$ and then calculate our rotor forces:
```python
u1 = qc_mass * (-space.gravity[1] - velz * pdz[1] + (ref[1] - posz) * pdz[0])
thetac = (-1 / -space.gravity[1]) * (-pdy[1] * vely + (ref[0] - posy) * pdy[0])
u2 = -pdtheta[1] * angv + pdtheta[0] * (thetac - ang)

f = A_inv.dot(np.array([[u1], [u2]]))

# clamp the forces to the allowable range [0, MAX_FORCE]
f1 = max(0, min(f[0][0], MAX_FORCE))
f2 = max(0, min(f[1][0], MAX_FORCE))
```


```python

```

e4967c504a46ec55ef43f897a5a60e3fd2e98dbe
ipynb
Jupyter Notebook
notebooks/quadcopter-2d.ipynb
tristeng/control
dbf99de467e92d998f4fd078057476cdf98537c7
MIT
2020-11-27T10:49:46.000Z
2021-04-04T03:41:19.000Z
notebooks/quadcopter-2d.ipynb
tristeng/control
dbf99de467e92d998f4fd078057476cdf98537c7
MIT
notebooks/quadcopter-2d.ipynb
tristeng/control
dbf99de467e92d998f4fd078057476cdf98537c7
MIT
2021-09-23T16:07:38.000Z
2022-02-08T04:23:46.000Z
Qwen/Qwen-72B
1. YES
2. YES