The operational domain of unmanned aerial vehicles (UAVs), particularly quadrotor drones, has expanded tremendously in recent years. From critical infrastructure inspection and precision agriculture to search & rescue and aerial surveillance, the demand for coordinated multi-agent systems is ever-growing. A fundamental capability for such systems is the execution of formation flight to cover areas efficiently, maintain communication links, or achieve specific sensor configurations. However, the real-world environments where these quadrotor drone teams operate are rarely pristine; they are cluttered with static and dynamic obstacles. Therefore, a robust control framework must seamlessly integrate three core objectives: navigating towards a mission goal, maintaining a specified geometric formation, and proactively avoiding collisions with both environmental obstacles and teammate drones. This article presents a comprehensive control strategy that addresses this tri-objective challenge for a team of quadrotor drones, employing a Null-Space-Based (NSB) behavioral approach for task fusion and a classical PID structure for low-level actuation.
The core challenge lies in managing potential conflicts between these objectives. For instance, a command to avoid an obstacle might directly contradict the command required to hold a formation position. Simple averaging of conflicting velocity vectors can lead to unpredictable and unsafe behavior. The NSB method provides an elegant solution by establishing a strict priority hierarchy among tasks. It ensures that a higher-priority task is always fulfilled completely, while lower-priority tasks are fulfilled only to the extent possible within the null space of the higher-priority task’s Jacobian matrix. This guarantees that secondary tasks never interfere with primary ones, such as safety-critical obstacle avoidance. In our framework, we assign the highest priority to obstacle and inter-agent collision avoidance, the second priority to cooperative formation-keeping, and the lowest priority to goal attraction. This prioritization reflects the fundamental principle that the integrity of the quadrotor drone platform is paramount for any mission success.
Mathematical Modeling of the Quadrotor Drone Dynamics
To design an effective controller, we first establish a dynamic model for a quadrotor drone. We define two coordinate frames: an inertial earth-fixed frame $\{E\}$ and a body-fixed frame $\{B\}$ attached to the drone’s center of mass. The position of the drone in $\{E\}$ is given by $\mathbf{p} = [x, y, z]^T$. Its orientation is defined by the Z-Y-X Euler angles $\boldsymbol{\Theta} = [\phi, \theta, \psi]^T$, representing roll, pitch, and yaw, respectively. The full nonlinear dynamics of a quadrotor drone can be derived using the Newton-Euler formalism:
$$ \begin{aligned}
m \ddot{x} &= (\cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi) U_1 \\
m \ddot{y} &= (\cos\phi \sin\theta \sin\psi – \sin\phi \cos\psi) U_1 \\
m \ddot{z} &= (\cos\phi \cos\theta) U_1 – mg \\
I_x \ddot{\phi} &= \dot{\theta}\dot{\psi}(I_y – I_z) – J_r \dot{\theta} \Omega_r + U_2 \\
I_y \ddot{\theta} &= \dot{\phi}\dot{\psi}(I_z – I_x) + J_r \dot{\phi} \Omega_r + U_3 \\
I_z \ddot{\psi} &= \dot{\phi}\dot{\theta}(I_x – I_y) + U_4
\end{aligned} $$
where $m$ is the mass, $g$ is gravitational acceleration, $I_x, I_y, I_z$ are moments of inertia, and $J_r$ is the rotor inertia. The control inputs are the total thrust $U_1$ and the torques $U_2, U_3, U_4$ around the body axes. The term $\Omega_r = \omega_1 – \omega_2 + \omega_3 – \omega_4$ is the residual propeller speed. The quadrotor drone is an underactuated system; it has four control inputs $(U_1, U_2, U_3, U_4)$ but six degrees of freedom $(x, y, z, \phi, \theta, \psi)$. To simplify controller design, we introduce virtual control inputs for the translational dynamics:
$$ \begin{aligned}
U_x &= \ddot{x}_d = \ddot{x} \\
U_y &= \ddot{y}_d = \ddot{y} \\
U_z &= \ddot{z}_d = \ddot{z}
\end{aligned} $$
We can then solve for the actual thrust $U_1$ and the desired attitude angles $(\phi_d, \theta_d)$ that produce these virtual accelerations, assuming a desired yaw angle $\psi_d$ (often set to zero for simple navigation):
$$ \begin{aligned}
U_1 &= \frac{m (U_z + g)}{\cos\phi_d \cos\theta_d} \\
\theta_d &= \arctan\left( \frac{U_x \cos\psi_d + U_y \sin\psi_d}{U_z + g} \right) \\
\phi_d &= \arctan\left( \frac{\cos\theta_d (U_x \sin\psi_d – U_y \cos\psi_d)}{U_z + g} \right)
\end{aligned} $$
The torques $U_2, U_3, U_4$ are then used to track these desired angles $(\phi_d, \theta_d, \psi_d)$. This transformation effectively decouples the position and attitude control loops. The primary output of our high-level NSB behavioral controller will be a desired velocity vector $\mathbf{v}_d = [\dot{x}_d, \dot{y}_d, \dot{z}_d]^T$, which is then integrated and processed through a PID controller to generate the virtual control inputs $(U_x, U_y, U_z)$.
| Parameter | Symbol | Typical Value | Unit |
|---|---|---|---|
| Mass | $m$ | 1.4 | kg |
| Gravity | $g$ | 9.81 | m/s² |
| Roll Inertia | $I_x$ | 0.03 | kg·m² |
| Pitch Inertia | $I_y$ | 0.03 | kg·m² |
| Yaw Inertia | $I_z$ | 0.04 | kg·m² |
| Rotor Inertia | $J_r$ | ~6e-5 | kg·m² |
The Null-Space-Based Behavioral Control Methodology
The NSB method is a prioritized task-regulation framework. For a quadrotor drone with velocity $\mathbf{v}$, a task is defined as controlling a set of output variables $\boldsymbol{\sigma} \in \mathbb{R}^m$ to a desired value. The task dynamics are $\dot{\boldsymbol{\sigma}} = \mathbf{J}(\mathbf{p})\mathbf{v}$, where $\mathbf{J}(\mathbf{p}) \in \mathbb{R}^{m \times 3}$ is the task Jacobian. The minimum-norm velocity command that fulfills a single task $\dot{\boldsymbol{\sigma}} = \dot{\boldsymbol{\sigma}}_d$ is given by the pseudo-inverse solution:
$$ \mathbf{v}_{task} = \mathbf{J}^{\dagger} \dot{\boldsymbol{\sigma}}_d = \mathbf{J}^T (\mathbf{J}\mathbf{J}^T)^{-1} \dot{\boldsymbol{\sigma}}_d $$
When multiple tasks coexist, they are stacked in order of descending priority. The overall velocity command is computed recursively, projecting the solution of each lower-priority task onto the null space of the higher-priority tasks’ Jacobians. The null space projector for a task Jacobian $\mathbf{J}$ is:
$$ \mathcal{N}(\mathbf{J}) = \mathbf{I} – \mathbf{J}^{\dagger} \mathbf{J} $$
where $\mathbf{I}$ is the identity matrix. For three tasks with priorities Task 1 > Task 2 > Task 3, the combined NSB velocity command is:
$$ \mathbf{v}_{NSB} = \mathbf{v}_1 + \mathcal{N}(\mathbf{J}_1)\left[ \mathbf{v}_2 + \mathcal{N}(\mathbf{J}_2) \mathbf{v}_3 \right] $$
This structure ensures that $\mathbf{J}_1 \mathbf{v}_{NSB} = \dot{\boldsymbol{\sigma}}_{1,d}$, meaning Task 1 is perfectly executed. Task 2 is executed only with the components of $\mathbf{v}_2$ that do not affect Task 1, and so on. We now define the three specific tasks for our quadrotor drone formation.
| Task Name | Priority | Controlled Variable $\boldsymbol{\sigma}$ | Objective |
|---|---|---|---|
| Obstacle & Collision Avoidance | 1 (Highest) | Distance to obstacles/neighbors | Maintain safe distance $d_{safe}$ |
| Cooperative Formation Keeping | 2 | Position relative to formation center | Achieve desired offset $\mathbf{d}_{f,i}$ |
| Goal Attraction | 3 (Lowest) | Agent’s own position | Reach target point $\mathbf{p}_{d,i}$ |
1. Obstacle and Collision Avoidance Task
This highest-priority task ensures the safety of each quadrotor drone. We consider both static environmental obstacles and other drones in the team as potential collision threats. For a drone at position $\mathbf{p}_i$ and an obstacle/neighbor at $\mathbf{p}_{o,m}$, the task variable is the distance $D_{o,i} = ||\mathbf{p}_i – \mathbf{p}_{o,m}||$. The desired distance is a predefined safety radius $d_{safe}$. We define a simple proportional controller to generate a desired task velocity that pushes the drone away if it gets too close:
$$ \dot{\sigma}_{o,d} = \lambda_o (d_{safe} – D_{o,i}) $$
where $\lambda_o > 0$ is a gain. The 1×3 Jacobian for this task is the unit vector pointing from the obstacle to the drone:
$$ \mathbf{J}_{o,i} = \frac{\partial D_{o,i}}{\partial \mathbf{p}_i} = \frac{(\mathbf{p}_i – \mathbf{p}_{o,m})^T}{D_{o,i}} $$
The corresponding velocity vector for this single task is $\mathbf{v}_{o,i} = \mathbf{J}_{o,i}^{\dagger} \dot{\sigma}_{o,d}$. For multiple nearby obstacles, the most restrictive one (i.e., the one causing the smallest $D_{o,i}$) can be selected, or contributions can be summed. For inter-agent collision avoidance, a “priority-based” strategy is effective to avoid oscillatory behavior: a drone only actively avoids those drones with a lower ID number, preventing mutual repulsion loops.
2. Cooperative Formation Keeping Task
This mid-priority task maintains the geometric shape of the quadrotor drone team. We define a formation centroid $\mathbf{p}_c$, which can be the position of a virtual leader or the average of all drone positions. Each drone $i$ has a desired offset $\mathbf{d}_{f,i}$ from this centroid. The task variable is thus the current offset error:
$$ \boldsymbol{\sigma}_{f,i} = \mathbf{p}_i – \mathbf{p}_c $$
The desired state is $\boldsymbol{\sigma}_{f,d} = \mathbf{d}_{f,i}$. Using a proportional control law, the desired task derivative is $\dot{\boldsymbol{\sigma}}_{f,d} = \lambda_f (\mathbf{d}_{f,i} – (\mathbf{p}_i – \mathbf{p}_c))$. The Jacobian for this task is simply the identity matrix $\mathbf{J}_{f,i} = \mathbf{I}_{3\times3}$, assuming the centroid moves independently or its velocity is known. The single-task velocity is $\mathbf{v}_{f,i} = \mathbf{J}_{f,i}^{\dagger} \dot{\boldsymbol{\sigma}}_{f,d} = \lambda_f (\mathbf{d}_{f,i} – (\mathbf{p}_i – \mathbf{p}_c))$.
3. Goal Attraction Task
The lowest-priority task drives each quadrotor drone towards its designated goal or waypoint $\mathbf{p}_{d,i}$. The task variable is the drone’s own position, $\boldsymbol{\sigma}_{d,i} = \mathbf{p}_i$, with the desired state being $\mathbf{p}_{d,i}$. To ensure the commanded velocity remains within the drone’s physical limits, we use a distance-dependent gain:
$$ \dot{\boldsymbol{\sigma}}_{d,d} = \lambda_d (\mathbf{p}_{d,i} – \mathbf{p}_i), \quad \text{with} \quad \lambda_d = v_{max} \cdot \frac{1}{\sqrt{||\mathbf{p}_{d,i} – \mathbf{p}_i||^2 + r_d^2}} $$
Here, $v_{max}$ is the maximum allowable speed and $r_d$ is a smoothing parameter. This gain ensures the velocity command asymptotically approaches $v_{max}$ when far from the goal and decreases smoothly upon approach. The Jacobian is again the identity matrix, $\mathbf{J}_{d,i} = \mathbf{I}$, yielding $\mathbf{v}_{d,i} = \lambda_d (\mathbf{p}_{d,i} – \mathbf{p}_i)$.
Task Fusion via NSB
The final desired velocity for drone $i$ is obtained by fusing the three task velocities according to the NSB formula:
$$ \mathbf{v}_{d,i} = \mathbf{v}_{o,i} + \mathcal{N}(\mathbf{J}_{o,i}) \left[ \mathbf{v}_{f,i} + \mathcal{N}(\mathbf{J}_{f,i}) \mathbf{v}_{d,i} \right] $$
This velocity vector is the output of the high-level behavioral planner. It guarantees that collision avoidance is never compromised, formation keeping is pursued in directions that do not cause collisions, and goal attraction is only acted upon in the null spaces of both higher-priority tasks.
Control Algorithm Design for the Quadrotor Drone
The desired velocity $\mathbf{v}_{d,i}$ must now be translated into actuator commands for the quadrotor drone. We employ a cascaded PID control structure. First, a velocity controller generates the virtual acceleration commands $(U_x, U_y, U_z)$. Then, the attitude controller generates the torque commands $(U_2, U_3, U_4)$ to achieve the roll and pitch angles required for those accelerations.
Let the velocity tracking error be $\mathbf{e}_v = \mathbf{v} – \mathbf{v}_{d,i}$. A PID controller computes the virtual control input:
$$ \mathbf{U}_v = \begin{bmatrix} U_x \\ U_y \\ U_z \end{bmatrix} = \mathbf{K}_P \mathbf{e}_v + \mathbf{K}_I \int \mathbf{e}_v \, dt + \mathbf{K}_D \frac{d\mathbf{e}_v}{dt} $$
where $\mathbf{K}_P, \mathbf{K}_I, \mathbf{K}_D$ are diagonal matrices of positive gains. Using equations (7) and (8), we compute the desired pitch $\theta_d$ and roll $\phi_d$ angles from $(U_x, U_y, U_z, \psi_d)$.
Next, define the attitude error $\mathbf{e}_a = \boldsymbol{\Theta} – [\phi_d, \theta_d, \psi_d]^T$. Another PID controller generates the desired angular acceleration for the attitude loop:
$$ \mathbf{U}_a = \begin{bmatrix} U_\phi \\ U_\theta \\ U_\psi \end{bmatrix} = \mathbf{k}_P \mathbf{e}_a + \mathbf{k}_I \int \mathbf{e}_a \, dt + \mathbf{k}_D \frac{d\mathbf{e}_a}{dt} $$
Finally, these virtual torque commands $\mathbf{U}_a$ and the total thrust $U_1$ (from Eq. 3) are mapped to the four motor speeds $[\omega_1, \omega_2, \omega_3, \omega_4]^T$ through the quadrotor drone‘s mixing matrix, completing the control pipeline.
| Control Loop | Error Signal | Virtual Control Output | Gain Matrices |
|---|---|---|---|
| Velocity Control | $\mathbf{e}_v = \mathbf{v} – \mathbf{v}_{d,i}$ | $\mathbf{U}_v = [U_x, U_y, U_z]^T$ | $\mathbf{K}_P, \mathbf{K}_I, \mathbf{K}_D$ |
| Attitude Control | $\mathbf{e}_a = \boldsymbol{\Theta} – \boldsymbol{\Theta}_d$ | $\mathbf{U}_a = [U_\phi, U_\theta, U_\psi]^T$ | $\mathbf{k}_P, \mathbf{k}_I, \mathbf{k}_D$ |
Simulation Analysis and Performance Evaluation
To validate the proposed framework, a numerical simulation of a four-quadrotor drone team was conducted. The drones were tasked with flying in a predefined tetrahedron formation towards a distant goal line, while encountering two spherical obstacles. The simulation parameters for the drones are listed in the first table. The NSB parameters were set as: $\lambda_o = 5$, $d_{safe} = 7m$, $\lambda_f = 5$, $v_{max} = 8 \, m/s$, $r_d = 1$. The desired formation offsets were defined relative to a moving centroid.
The resulting 3D flight trajectories clearly demonstrate the efficacy of the controller. As the formation encountered the first obstacle, all drones successfully deviated from their nominal paths to maintain a safe distance. Crucially, the priority-based inter-agent avoidance prevented drones from repelling each other into unstable oscillations. After passing the obstacle, the drones smoothly reconverged to the desired tetrahedron formation. A second obstacle test further confirmed the robustness of the reactive avoidance behavior. Throughout the mission, the primary objective of progressing towards the goal was maintained whenever it did not conflict with safety or formation constraints.
The velocity profiles showed smooth transitions during avoidance maneuvers. The control input $U_1$ (total thrust) and the roll angle $\phi$ exhibited expected variations corresponding to the evasion and reformation maneuvers, remaining within realistic physical limits for a quadrotor drone. These results collectively verify that the integrated NSB-PID control framework successfully enables a team of quadrotor drones to navigate cluttered environments cohesively and safely.
Conclusion
This article presented a holistic solution for the integrated obstacle avoidance and cooperative formation control problem for multi-quadrotor drone systems. By leveraging the Null-Space-Based behavioral method, we established a strict, conflict-free hierarchy between the critical tasks of collision avoidance, formation keeping, and goal navigation. The high-level NSB planner outputs a consistent desired velocity, which is then tracked by a robust, cascaded PID controller that translates it into motor commands for the underactuated quadrotor drone dynamics. The simulation results validate that this approach allows a drone team to effectively negotiate obstacles while preserving formation integrity and making continuous progress towards its mission objective. The framework is modular, allowing for the inclusion of additional tasks (e.g., visibility maintenance, payload management) at appropriate priority levels, making it a versatile foundation for advanced multi-quadrotor drone applications in complex, dynamic environments.