Drone Trajectory Tracking with Python#

The dynamics and kinematics of a drone are crucial in understanding and controlling its flight behavior.

1. Dynamics and Kinematics of the Drone#

The drone’s state can be described by its position \(x, y, z\), orientation (roll \(\phi\), pitch \(\theta\), and yaw \(\psi\)), and their respective velocities and angular velocities.

Translational Motion#

The translational motion of the drone can be described by Newton’s second law of motion:

\[ m\ddot{\mathbf{r}} = \mathbf{F} - mg\hat{z} \]

where:

\(m\) is the mass of the drone.
\(\ddot{\mathbf{r}}\) represents the linear acceleration.
\(\mathbf{F}\) is the total thrust force generated by the drone’s motors.
\(g\) is the acceleration due to gravity.
\(\hat{z}\) is the unit vector in the vertical direction.

Rotational Motion#

The rotational motion of the drone is described using Euler’s equations of motion for a rigid body:

\[ I \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I \boldsymbol{\omega}) = \boldsymbol{\tau} \]

where:

\(I\) represents the moment of inertia matrix.
\(\boldsymbol{\omega}\) is the angular velocity vector (p, q, r).
\(\boldsymbol{\tau}\) is the vector of external torques acting on the drone.

Dynamic System Matrix#

The dynamics of the drone can be represented in a state-space format:

\[ \dot{\mathbf{x}} = A\mathbf{x} + B\mathbf{u} \]

\[ \mathbf{y} = C\mathbf{x} + D\mathbf{u} \]

Where:

\(\mathbf{x}\) is the state vector.
\(\mathbf{u}\) is the input vector (like motor thrusts).
\(\mathbf{y}\) is the output vector (like measured position and orientation).
\(A\) is the system matrix.
\(B\) is the input matrix.
\(C\) is the output matrix.
\(D\) is the direct transmission matrix.

2. Control System#

Control Inputs#

The control inputs for the drone include thrust \(U_1\) and torques \(U_2, U_3, U_4\). These are calculated as:

\[ U_1 = c_t(\omega_1^2 + \omega_2^2 + \omega_3^2 + \omega_4^2) \]

\[ U_2 = c_t l (\omega_2^2 - \omega_4^2) \]

\[ U_3 = c_t l (\omega_3^2 - \omega_1^2) \]

\[ U_4 = c_q (-\omega_1^2 + \omega_2^2 - \omega_3^2 + \omega_4^2) \]

3. Trajectory Generation#

The desired trajectory as a function of time \(t\) is:

\[ X_{\text{ref}}(t),\quad Y_{\text{ref}}(t),\quad Z_{\text{ref}}(t),\quad \psi_{\text{ref}}(t) \]

4. Linear Parameter Varying (LPV) Systems#

LPV systems are described by:

\[ \dot{x}(t) = A(t)x(t) + B(t)u(t) \]

\[ y(t) = C(t)x(t) + D(t)u(t) \]

5. Model Predictive Control (MPC)#

MPC uses a dynamic model to optimize future control actions. The objective function is:

\[ \min_{u} \sum_{k=0}^{N-1} \left( (x_k - x_{\text{ref}})^T Q (x_k - x_{\text{ref}}) + (u_k - u_{\text{ref}})^T R (u_k - u_{\text{ref}}) \right) \]

Subject to:

\[ x_{k+1} = A x_k + B u_k \]

\[ x_{\min} \leq x_k \leq x_{\max} \]

\[ u_{\min} \leq u_k \leq u_{\max} \]