Drone Trajectory Tracking with Python

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} $$

Where:

  • $x_k$: system state at step $k$.

  • $u_k$: control input at step $k$.

  • $x_{\text{ref}}, u_{\text{ref}}$: reference state and input.

  • $Q, R$: weighting matrices.

  • $N$: prediction horizon.

  • $x_{\min}, x_{\max}, u_{\min}, u_{\max}$: state/input bounds.

Contents