In recent years, quadrotor unmanned aerial vehicles (UAVs) have gained significant attention due to their agility, vertical take-off and landing capabilities, and adaptability in various applications such as surveillance, logistics, and environmental monitoring. However, single quadrotor operations often face limitations in complex scenarios, leading to the emergence of multi-quadrotor formations. These formations enhance robustness, redundancy, and mission efficiency. A critical challenge in multi-quadrotor systems is trajectory optimization and obstacle avoidance under dynamic environmental conditions, such as hybrid wind fields. This paper addresses this issue by proposing a trajectory planning algorithm based on the Gauss pseudo-spectral method (GPM) for a leader-follower formation of quadrotors. The approach transforms follower constraints into leader constraints, minimizing energy consumption while ensuring safe navigation through obstacles in mixed wind environments.
The dynamics of a single quadrotor in a hybrid wind field are complex due to aerodynamic forces and moments. The quadrotor is an underactuated system with four control inputs governing its motion. In a wind-affected environment, additional forces like wind-induced lift and drag must be accounted for. The total force acting on the quadrotor can be expressed as:
$$ ma = (F_w + F_T) \cos \theta – G – f $$
where \( m \) is the mass, \( a \) is acceleration, \( F_w \) is the wind-induced lift, \( F_T \) is the total thrust from rotors, \( \theta \) is the pitch angle, \( G \) is gravity, and \( f \) is the drag force. The wind-induced lift \( F_w \) depends on air density \( \rho_w \), rotor swept area \( A \), wind velocity \( V_w \), and induced velocity \( V_d \):
$$ F_w = 2 \rho_w A V_w V_d $$
The total thrust \( F_T \) is generated by the four rotors:
$$ F_T = k_T ( \bar{w}_1^2 + \bar{w}_2^2 + \bar{w}_3^2 + \bar{w}_4^2 ) $$
where \( k_T \) is the thrust coefficient and \( \bar{w}_i \) are rotor speeds. The drag force \( f \) is modeled as:
$$ f = \frac{1}{2} \rho_w S V^2 C_d $$
with \( S \) as the cross-sectional area, \( V \) as the quadrotor velocity, and \( C_d \) as the drag coefficient. The moments acting on the quadrotor include contributions from rotor thrusts, aerodynamic damping, and wind disturbances. The total moment \( \sum \tau \) is:
$$ \sum \tau = \tau_d – \tau_f – \tau_w $$
where \( \tau_d \) is the moment from rotors, \( \tau_f \) is the aerodynamic damping moment, and \( \tau_w \) is the wind disturbance moment. The full dynamics model for a quadrotor in a hybrid wind field is derived as:
$$
\begin{align*}
\dot{x} &= \frac{1}{m} \left( U_1 (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi) \right) – \frac{F_{wx}}{m} – \frac{f_x}{m} \\
\dot{y} &= \frac{1}{m} \left( U_1 (\cos \phi \sin \theta \sin \psi – \sin \phi \cos \psi) \right) – \frac{F_{wy}}{m} – \frac{f_y}{m} \\
\dot{z} &= \frac{1}{m} \left( U_1 (\cos \theta \cos \phi) – mg \right) – \frac{F_{wz}}{m} – \frac{f_z}{m} \\
\dot{\theta} &= \frac{I_{zz} – I_{xx}}{I_{yy}} \dot{\phi} \dot{\psi} + \frac{U_2}{I_{yy}} – \frac{d f_\theta}{I_{yy}} \dot{\theta} – \frac{\tau_{w\theta}}{I_{yy}} \\
\dot{\phi} &= \frac{I_{yy} – I_{zz}}{I_{xx}} \dot{\theta} \dot{\psi} + \frac{U_3}{I_{xx}} – \frac{d f_\phi}{I_{xx}} \dot{\phi} – \frac{\tau_{w\phi}}{I_{xx}} \\
\dot{\psi} &= \frac{I_{xx} – I_{yy}}{I_{zz}} \dot{\phi} \dot{\theta} + \frac{U_4}{I_{zz}} – \frac{d f_\psi}{I_{zz}} \dot{\psi} – \frac{\tau_{w\psi}}{I_{zz}}
\end{align*}
$$
Here, \( \phi, \theta, \psi \) are roll, pitch, and yaw angles; \( I_{xx}, I_{yy}, I_{zz} \) are moments of inertia; \( U_1, U_2, U_3, U_4 \) are control inputs; and \( F_{wx}, F_{wy}, F_{wz} \) are wind force components. The hybrid wind field \( V_w \) combines steady wind \( V_{con} \), gust wind \( V_{gus} \), gradient wind \( V_{gra} \), and noisy wind \( V_{ron} \):
$$ V_w = \alpha_1 V_{con} + \alpha_2 V_{gus} + \alpha_3 V_{gra} + \alpha_4 V_{ron} $$
with weights \( \alpha_i \) summing to 1. For instance, gust wind is modeled as:
$$ V_{gus} = \begin{cases} \frac{V_{g \max}}{2} \left[ 1 – \cos(2\pi \eta_g) \right], & 0 \leq \eta_g < 1 \\ 0, & \eta_g \geq 1 \end{cases} $$
where \( V_{g \max} \) is the maximum gust velocity and \( \eta_g \) is the duration ratio.
In a leader-follower formation, the leader quadrotor’s trajectory is optimized, and follower constraints are transformed into leader constraints to simplify the problem. For a formation of \( N \) quadrotors, the leader’s state variables include position \( (x_L, y_L, z_L) \), velocity, and attitude angles. Followers maintain relative positions to the leader, defined by safe distances \( (x_{Li}, y_{Li}, z_{Li}) \) for the \( i \)-th follower. The collision avoidance constraint between leader and followers is:
$$ | x_L(t) – x_i(t) – x_{Li} | \leq 0.1, \quad | y_L(t) – y_i(t) – y_{Li} | \leq 0.1, \quad | z_L(t) – z_i(t) – z_{Li} | \leq 0.1 $$
This allows follower positions to be expressed in terms of the leader’s position. Similarly, obstacle avoidance for followers is converted to leader constraints. For an obstacle at \( (x_n, y_n) \) with radius \( R_n \), the constraint becomes:
$$ (x_L(t) – x_{Li} – x_n)^2 + (y_L(t) – y_{Li} – y_n)^2 – R_n \geq R_s $$
where \( R_s \) is the safety distance. The minimum turning radius constraint for followers is also transformed. For a follower with maximum roll angle \( \phi_{i \max} \), the constraint in terms of the leader’s turning radius \( r_L \) and velocity \( V_L \) is:
$$ g \tan \phi_{i \max} \geq \frac{ \left( (x_L(t) – x_{Li} – x_n)^2 + (y_L(t) – y_{Li} – y_n)^2 \right)^{3/2} }{ r_L^2 } V_L^2 $$
Other constraints for the leader include initial and terminal states, control input limits, flight altitude, and obstacle avoidance. For example, control inputs must satisfy:
$$ U_{i \min} \leq U_i \leq U_{i \max}, \quad i = 1,2,3,4 $$
and the altitude constraint is \( z_L(t) \leq h_{\max} \). The performance index minimizes energy consumption across all quadrotors:
$$ J = \min \left( \sum_{i = L, 1}^{N} U_{i1}^2 + U_{i2}^2 + U_{i3}^2 + U_{i4}^2 \right) $$
where \( U_{i1} \) to \( U_{i4} \) are control inputs for each quadrotor.
The Gauss pseudo-spectral method is employed to transcribe the optimal control problem into a nonlinear programming problem (NLP). Time domain \( [t_0, t_f] \) is transformed to \( [-1, 1] \) using:
$$ \tau = \frac{2t}{t_f – t_0} – \frac{t_f + t_0}{t_f – t_0} $$
State and control variables are approximated using Lagrange interpolation polynomials at Legendre-Gauss points. For \( K \) discretization points, the state approximation is:
$$ x(\tau) \approx X(\tau) = \sum_{i=0}^{K} L_i(\tau) X(\tau_i) $$
where \( L_i(\tau) \) are Lagrange basis polynomials. The derivative is approximated as:
$$ \dot{x}(\tau_k) \approx \sum_{i=0}^{K} D_{ki} X(\tau_i) $$
with \( D_{ki} \) being the differentiation matrix. The dynamics constraints become algebraic equations:
$$ \sum_{i=0}^{K} D_{ki} X(\tau_i) = \frac{t_f – t_0}{2} f(X(\tau_k), U(\tau_k), \tau_k; t_0, t_f) $$
The terminal state is constrained via Gauss quadrature:
$$ X(\tau_f) = X(\tau_0) + \frac{t_f – t_0}{2} \sum_{k=1}^{K} \omega_k f(X(\tau_k), U(\tau_k), \tau_k; t_0, t_f) $$
where \( \omega_k \) are weights. The performance index is discretized as:
$$ J = \Phi(X_0, t_0, X_f, t_f) + \frac{t_f – t_0}{2} \sum_{k=1}^{K} \omega_k \vartheta(X_k, U_k, \tau_k; t_0, t_f) $$
The resulting NLP is solved using sequential quadratic programming (SQP), which handles nonlinear constraints efficiently. SQP iteratively solves quadratic subproblems to find the optimal trajectory.
Simulations were conducted for a formation of six quadrotors in a hybrid wind field. The leader starts at (20, 20, 2) m with initial velocity (2, 0, 0) m/s and attitude (60°, 0°, 0°), aiming to reach (1200, 1200, 2) m in 350 s. Followers are positioned in an equilateral triangle formation with relative offsets. The wind field parameters are set as follows: steady wind \( V_{con} = 3.3 \) m/s, gust wind \( V_{g \max} = 5 \) m/s, gradient wind \( V_{gr \max} = 4 \) m/s, and noisy wind \( V_{r \max} = 4.1 \) m/s, with weights \( \alpha_1 = 0.3, \alpha_2 = 0.3, \alpha_3 = 0.2, \alpha_4 = 0.2 \). The terrain includes multiple obstacles modeled as peaks:
$$ z_1(x, y) = \left| \sin(y + a) + b \cdot \sin(x) + c \cdot \cos(d \cdot \sqrt{x^2 + y^2}) + e \cdot \cos(y) + f \cdot \sin(g \cdot \sqrt{x^2 + y^2}) \right| $$
$$ z_2(x, y) = \sum_{i=1}^{n} h_i \exp \left( -\left( \frac{x – x_i}{x_{si}} \right)^2 – \left( \frac{y – y_i}{y_{si}} \right)^2 \right) $$
Quadrotor parameters are listed in Table 1, and constraint bounds in Table 2.
| Parameter | Value |
|---|---|
| Mass \( m \) | 0.5 kg |
| Rotor distance \( d \) | 0.075 m |
| Thrust coefficient \( k_T \) | 1.105 × 10^{-5} N/(rad/s)^2 |
| Moment coefficient \( k_d \) | 1.779 × 10^{-7} N·m/(rad/s)^2 |
| Moment of inertia \( I_{xx}, I_{yy} \) | 0.0219 kg·m² |
| Moment of inertia \( I_{zz} \) | 0.0366 kg·m² |
| Air density \( \rho_w \) | 1.184 kg/m³ |
| Rotor area \( A \) | 0.02 m² |
| Steady wind \( K_c \) | 3.3 m/s |
| Noisy wind max \( V_{r \max} \) | 4.1 m/s |
| Parameter | Min/Max |
|---|---|
| Velocity \( V_{Lx}, V_{Ly}, V_{Lz} \) | ±5 m/s |
| Roll angle \( \phi_L \) | ±π rad |
| Pitch angle \( \theta_L \) | ±π/3 rad |
| Yaw angle \( \psi_L \) | ±π/3 rad |
| Angular rates \( \dot{\phi}_L, \dot{\theta}_L, \dot{\psi}_L \) | ±1.7 rad/s |
| Control input \( U_1 \) | ±12.03 N |
| Control input \( U_2, U_3 \) | ±0.67 N·m |
| Control input \( U_4 \) | ±0.2 N·m |
The optimized trajectory shows that all six quadrotors successfully navigate from start to end while avoiding obstacles and maintaining formation. The leader’s control inputs \( U_1, U_2, U_3, U_4 \) vary within bounds, as shown in Figure 3, ensuring stable attitude control under wind disturbances. The attitude angles of the leader (Figure 4) demonstrate adjustments during obstacle avoidance, with yaw changes corresponding to turns around peaks. The formation maintains safe inter-quadrotor distances, as illustrated in Figure 5 for the y-direction, with errors within 0.1 m. Position data in Figure 6 confirms smooth paths without significant deviations, validating the constraint transformations and GPM approach.
In conclusion, the proposed method effectively optimizes trajectories for multi-quadrotor formations in hybrid wind fields. By transforming follower constraints into leader constraints and applying GPM with SQP, the algorithm minimizes energy consumption while ensuring safety and formation integrity. Future work could explore real-time adaptations and more complex wind models to enhance practicality for dynamic environments.