Optimal control of a drone using an iterative Linear-Quadratic Regulator

TL;DR

LQR and iLQR controllers flying a 2D quadrotor through acrobatics. Three controllers were designed: circular tracking, a vertical hold, and a full 360° flip.

The quadrotor completing a full flip under iLQR control.

Approach

The quadrotor dynamics are defined as:

\begin{equation} \dot{x} = v_x \end{equation} \begin{equation} m\dot{v_x} = -(u_1 + u_2)\sin \theta \end{equation} \begin{equation} \dot{y} = v_y \end{equation} \begin{equation} m\dot{v_y} = (u_1 + u_2)\cos \theta -mg \end{equation} \begin{equation} \dot{\theta} = \omega \end{equation} \begin{equation} I\dot{\omega} = r(u_1 + u_2) \end{equation}

Where \(x\) is the horizontal and \(y\) the vertical position of the quadrotor, and \(\theta\) its orientation with respect to the horizontal plane. \(v_x\) and \(v_y\) are the linear velocities and \(\omega\) the angular velocity. \(u_1\) and \(u_2\) are the forces produced by the rotors — our control inputs. \(m\) is the quadrotor mass, \(I\) its moment of inertia (a scalar), \(r\) the distance from the centre of the frame to the propellers, and \(g\) the gravity constant. The full state is \(z = [x, v_x, y, v_y, \theta, \omega]^T\), with control \(u = [u_1, u_2]^T\).

Following a circular trajectory

A tracking controller, using an LQ design with linear approximations, follows a circular trajectory.

Circular trajectory tracking.

Reaching a vertical orientation

A controller that brings the robot to a vertical orientation, \(\theta = \frac{\pi}{2}\), at \(x = 3,\ y = 3\) and time \(t = 5\), starting from the origin. For the remainder of the motion it tries to stay close to the origin.

Vertical orientation hold.

Performing a full flip

A controller that makes the robot perform a full flip, reaching the upside-down state \(\theta = \pi\) at \(x = 1.5,\ y = 3\) at time \(t = 5\), then \(\theta = 2\pi\) at \(x = 3,\ y = 0\) at time \(t = 10\), starting from the origin.

A full 360° flip under iLQR.

Acknowledgements

I would like to specially thank Professor Ludovic Righetti for allowing me to showcase this project on my website.