In recent years, drones have found extensive and profound applications in both military and civilian fields due to their unique advantages, such as small size and high maneuverability. Coordinated operations involving multiple drones represent the primary mode of future drone combat deployment. A well-commanded and orderly mixed formation of various drone types can maximize operational effectiveness. A prerequisite for achieving this effectiveness is the ability of a drone formation to assemble into a specified formation, reach a designated position, and achieve a target velocity under real-world motion constraints. Research into drone formation tasks under these constraints necessitates the optimization of standard second-order consensus control algorithms.
As a key technology for drone formation control, formation control algorithms based on consensus theory have attracted significant attention due to their low communication load requirements and good node scalability. For second-order systems, the standard second-order consensus control algorithm is described by the following equations governing the dynamics of agent \(i\):
$$ \dot{s}_i = v_i $$
$$ \dot{v}_i = u_i $$
$$ u_i = \sum_{j \in \mathcal{N}_i} a_{ij} \left[ (s_j – s_i) + \gamma (v_j – v_i) \right] $$
where \(s_i = [x_i, y_i, z_i]^T\) and \(v_i = [\dot{x}_i, \dot{y}_i, \dot{z}_i]^T\) represent the position and velocity of drone \(i\), respectively. \(u_i\) is the control input, \(\mathcal{N}_i\) is the set of neighbors communicating with node \(i\), \(a_{ij}\) are elements of the weighted adjacency matrix \(\mathbf{A}\), and \(\gamma > 0\) is the velocity coupling coefficient. While this algorithm achieves consensus, meaning \( (s_j(t) – s_i(t)) \to 0\) and \( (v_j(t) – v_i(t)) \to 0\) as \(t \to \infty\), the final consensus position depends on the initial states of all agents and, crucially, all positions converge to the same point. This is unsuitable for drone formation tasks which require a specific geometric pattern, not a single cluster.
To address this fundamental issue, the concept of a virtual reference point is integrated, drawing inspiration from the virtual structure method. A formation information vector \(r^d_i = [x^d_{ri}, y^d_{ri}, z^d_{ri}]^T\) defines the desired position of drone \(i\) relative to the virtual reference point in a formation-fixed coordinate frame (b-frame). This frame is aligned with the reference point’s desired heading \(\psi_r\). The transformation from the inertial ground frame (I-frame) to the b-frame is given by the rotation matrix \(\mathbf{C}_I^b\):
$$ \mathbf{C}_I^b = \begin{bmatrix}
\sin\psi_r & \cos\psi_r & 0 \\
-\cos\psi_r & \sin\psi_r & 0 \\
0 & 0 & 1
\end{bmatrix} $$
The desired position \(s^I_i\) and velocity \(v^I_i\) for drone \(i\) in the I-frame are:
$$ s^I_i = s^r_i + \mathbf{C}_b^I r^d_i $$
$$ v^I_i = v^r_i $$
where \(s^r_i\) and \(v^r_i\) are the position and velocity of the virtual reference point. We then define the position and velocity coordination variables as \(\tilde{s}_i = s_i – s^I_i\) and \(\tilde{v}_i = v_i – v^I_i\). The control objective is to drive these coordination errors to zero, which implies the drone formation converges to the desired pattern moving with the reference state.
1. Second-Order Consensus Formation Control Algorithm with Virtual Reference Point
Building upon the standard consensus algorithm and incorporating formation geometry, we propose the following control law, termed the Second-Order Consensus Formation Control Algorithm with Virtual Reference Point:
$$ \dot{s}_i = v_i $$
$$ \dot{v}_i = u_i^2 $$
$$ u_i^2 = \sum_{j \in \mathcal{N}_i} a_{ij}\gamma\left[(v_j – v_i) + \alpha(v^r_i – v_i)\right] + \sum_{j \in \mathcal{N}_i} a_{ij}\left[(s_j – s_i + r_{ij}) + \alpha(s^r_i – s_i + r^I_i)\right] $$
Here, \(\alpha > 0\) is the reference state coupling coefficient, \(r_{ij} = r^I_j – r^I_i\) is the desired relative position vector between drones \(i\) and \(j\) in the I-frame, and \(r^I_i = \mathbf{C}_b^I r^d_i\). Let \(\tilde{\mathbf{S}} = \mathbf{S} – \mathbf{R}^I – \mathbf{S}^r\) and \(\tilde{\mathbf{V}} = \mathbf{V} – \mathbf{V}^r\) be the stacked vectors of coordination errors. The closed-loop system dynamics can be written in matrix form:
$$ \begin{bmatrix} \dot{\tilde{\mathbf{S}}} \\ \dot{\tilde{\mathbf{V}}} \end{bmatrix} = \begin{bmatrix} \mathbf{0}_{n \times n} & \mathbf{I}_n \\ -(\mathbf{L} + \alpha \mathbf{D}) & -(\gamma\mathbf{L} + \gamma\alpha\mathbf{D}) \end{bmatrix} \begin{bmatrix} \tilde{\mathbf{S}} \\ \tilde{\mathbf{V}} \end{bmatrix} = \Gamma_1 \begin{bmatrix} \tilde{\mathbf{S}} \\ \tilde{\mathbf{V}} \end{bmatrix} $$
where \(\mathbf{L}\) is the Laplacian matrix of the communication graph and \(\mathbf{D}\) is the in-degree matrix. For an undirected, connected communication graph with \(\gamma > 0\) and \(\alpha > 0\), the matrix \(\Gamma_1\) has all eigenvalues with negative real parts, guaranteeing asymptotic stability. The drone formation converges to the desired state:
$$ s_i(t) \to s^r_i(t) + r^I_i, \quad s_j(t) – s_i(t) \to r_{ij}, \quad v_i(t) \to v_j(t) \to v^r_i $$
This solves the problem of convergence to a single point and decouples the final state from the initial conditions, which is essential for practical drone formation control.
2. Motion-Constrained Second-Order Consensus Formation Control Algorithm
In real applications, drones are subject to motion constraints. Their airspeed \(V_i\) and linear acceleration \(a_i\) are bounded:
$$ v_{i}^{min} \le V_i \le v_{i}^{max}, \quad a_{min}(i) \le a_i \le a_{max}(i) $$
A naive application of the control law \(u_i^2\) could demand accelerations or velocities beyond these limits. To incorporate these constraints, we introduce a saturation function \(SAT(\cdot)\) to limit the control input (acceleration command). The Motion-Constrained Second-Order Consensus Formation Control Algorithm is:
$$ \dot{s}_i = v_i $$
$$ \dot{v}_i = \hat{u}_i^4 $$
$$ \hat{u}_i^4 = SAT(\hat{u}_i^2) $$
where \(\hat{u}_i^2\) is the unconstrained control input from the previous algorithm. The saturation function is defined as:
$$ SAT(u^b_i) = \begin{cases}
c_{max}(t), & u^b_i > c_{max}(t) \\
u^b_i, & c_{min}(t) \le u^b_i \le c_{max}(t) \\
c_{min}(t), & u^b_i < c_{min}(t)
\end{cases} $$
The time-varying bounds \(c_{max}(t)\) and \(c_{min}(t)\) are computed based on the current velocity and the drone’s dynamic limits to ensure feasibility:
$$ c_{max}(t) = \min\left\{ a_{max}(i),\ \frac{(v_{i}^{max} – \beta v_{i}^{max}) – V_i(t)}{\Delta t} \right\} $$
$$ c_{min}(t) = -\min\left\{ a_{max}(i),\ \frac{V_i(t) – [v_{min}(i) + \beta v_{min}(i)]}{\Delta t} \right\} $$
where \(\beta\) is a velocity margin coefficient and \(\Delta t\) is the control update period. The saturation function can be equivalently expressed as \(SAT(\hat{\mathbf{u}}_2) = \mathbf{F}(t)\hat{\mathbf{u}}_2\), where \(\mathbf{F}(t)\) is a positive definite, diagonal matrix with time-varying entries \(0 < \epsilon_{ik}(t) \le 1\). The closed-loop dynamics become:
$$ \begin{bmatrix} \dot{\tilde{\mathbf{S}}} \\ \dot{\tilde{\mathbf{V}}} \end{bmatrix} = \begin{bmatrix} \mathbf{I}_{3n} & \mathbf{0} \\ \mathbf{0} & \mathbf{F}(t) \end{bmatrix} (\Gamma_1 \otimes \mathbf{I}_3) \begin{bmatrix} \tilde{\mathbf{S}} \\ \tilde{\mathbf{V}} \end{bmatrix} = \Gamma_2(t) \begin{bmatrix} \tilde{\mathbf{S}} \\ \tilde{\mathbf{V}} \end{bmatrix} $$
Analysis shows that the eigenvalues \(\lambda_i[\Gamma_2(t)]\) satisfy \(\lambda_i(\Gamma_1) \le \lambda_i[\Gamma_2(t)] \le \epsilon \lambda_i(\Gamma_1) < 0\), where \(0 < \epsilon \le 1\). Therefore, the system remains asymptotically stable even with input saturation, and the drone formation still converges to the desired state as in Eq. (18), albeit potentially at a slower rate due to the constraints pushing the system eigenvalues closer to the imaginary axis.
3. Channel Controller Design for Drone Formation
The overall control architecture for each drone in the formation is decomposed into separate channels. For altitude and heading control, which typically involve set-point commands, simpler first-order consensus with PID control is sufficient.
Altitude Channel:
The altitude error is \(\tilde{z}_i = z^r – z_i + z^I_{ri}\). The altitude rate command is:
$$ \dot{z}_i^{cmd} = k_{zp} \tilde{z}_i + k_{zi} \int_0^t \tilde{z}_i \, d\tau + k_{zd} \frac{d\tilde{z}_i}{dt} $$
where \(k_{zp}, k_{zi}, k_{zd}\) are PID gains.
Heading Channel:
The heading error is \(\tilde{\psi}_i = \psi_r – \psi_i\). The heading rate command is:
$$ \dot{\psi}_i^{cmd} = k_{\psi p} \tilde{\psi}_i + k_{\psi i} \int_0^t \tilde{\psi}_i \, d\tau + k_{\psi d} \frac{d\tilde{\psi}_i}{dt} $$
where \(k_{\psi p}, k_{\psi i}, k_{\psi d}\) are PID gains. The desired heading \(\psi_r\) is derived from the virtual reference point’s velocity: \(\psi_r = \text{atan2}(\dot{y}^r, \dot{x}^r)\).
Velocity (East & North) Channels:
These channels employ the core Motion-Constrained Consensus Algorithm. The control inputs (acceleration commands) for the east (\(\ddot{x}_i\)) and north (\(\ddot{y}_i\)) channels are computed using the saturated consensus law. The per-axis constraints are derived from the total speed and acceleration limits. For example, the eastward acceleration limit is:
$$ \ddot{x}_{i}^{max} = \sqrt{ a_{max}^2(i) – \ddot{y}_i^2 – \ddot{z}_i^2 } $$
The final velocity commands are obtained by integrating the acceleration commands over the control period \(\Delta t\):
$$ \dot{x}_i(t+\Delta t) = \dot{x}_i(t) + \int_t^{t+\Delta t} \ddot{x}_i^{cmd}(\tau) \, d\tau $$
$$ \dot{y}_i(t+\Delta t) = \dot{y}_i(t) + \int_t^{t+\Delta t} \ddot{y}_i^{cmd}(\tau) \, d\tau $$
The control inputs to the drone’s autopilot are thus: \(\mathbf{U}_i = [z_i^{cmd}, \dot{x}_i^{cmd}, \dot{y}_i^{cmd}, \psi_i^{cmd}]^T\).
4. Simulation Results and Comparative Analysis
To validate the proposed algorithms, extensive simulations of a five-drone formation assembly task were conducted. The desired formation in the b-frame is defined by the matrix \(\mathbf{R}^d\), and the communication topology is a spanning tree. Key simulation parameters are listed below.
| Parameter | Value |
|---|---|
| Number of Drones | 5 |
| Desired Formation \(\mathbf{R}^d\) (m) | \([0,0,0; 5,-5,0; -5,-5,0; 10,-10,0; -10,-10,0]^T\) |
| Desired Speed \(v^r\) (m/s) | \([4, 0, 0]^T\) |
| Desired Altitude \(z^r\) (m) | 5 |
| Desired Heading \(\psi_r\) | 90° |
| Speed Constraint \([v^{min}, v^{max}]\) (m/s) | \([0, 5.0]\) |
| Acceleration Constraint \(a_{max}\) | \(5g\) |
| Coefficients \(\alpha, \gamma\) | 1, 20 |
4.1 Validation of the Algorithm with Virtual Reference Point
We first compare our proposed Algorithm (Eq. 16) with a baseline algorithm from literature (Eq. 15 in the source material) that only incorporates the reference velocity. Two different initial configurations were tested: one aligned along the East axis (Formation \(S_1\)) and one aligned along the North axis (Formation \(S_2\)). The convergence performance is summarized below.
| Simulation Case | Control Algorithm | Initial Formation | Convergence Time (s) | Position Error at 120s (Max, m) | Speed Error at 120s (Max, m/s) |
|---|---|---|---|---|---|
| I | Baseline (Ref. [10]) | \(S_1\) (East) | 107.78 | 95.66 | 0.44 |
| II | Baseline (Ref. [10]) | \(S_2\) (North) | 109.80 | 118.68 | 0.44 |
| III | Proposed (Eq. 16) | \(S_1\) (East) | 95.18 | 0.35 | 0.0087 |
| IV | Proposed (Eq. 16) | \(S_2\) (North) | 98.77 | 0.37 | 0.0037 |
The results clearly show that the baseline algorithm causes the drone formation to converge to a single point (large position errors relative to the desired formation) and fails to reach the desired speed of 4 m/s. In contrast, our proposed algorithm successfully converges to the specified diamond-shaped formation with very small position and speed errors, regardless of the initial configuration, verifying its independence from initial conditions.
4.2 Validation of the Motion-Constrained Algorithm
Next, we evaluate the effectiveness and efficiency of the motion-constrained algorithm (Eq. 23, labeled as Simulation V). We compare it against three other approaches:
1. The unconstrained version of our algorithm (Eq. 16, Simulation VI).
2. The baseline algorithm with the same saturation function applied (Simulation VII).
3. A consensus algorithm using the hyperbolic tangent (tanh) function for constraint, as used in some literature (Eq. 22, Simulation VIII).
| Simulation | Control Algorithm | Convergence Time (s) | Iterations to Converge | Final Position Error (Max, m) | Final Speed Error (Max, m/s) |
|---|---|---|---|---|---|
| V | Proposed Constrained (Eq. 23) | 95.38 | 4769 | 0.40 | 0.0215 |
| VI | Proposed Unconstrained (Eq. 16) | 93.97 | 4699 | 0.40 | 0.0656 |
| VII | Baseline + Saturation | 129.63 | 6482 | 0.40 | 0.0454 |
| VIII | Proposed + tanh Constraint | 201.53 | 10077 | 0.47 | 0.0936 |
The analysis of the results yields key insights:
1. All algorithms eventually achieve accurate drone formation assembly (small final errors).
2. Comparing V and VI: The motion-constrained algorithm is only about 1.5% slower than its unconstrained counterpart, demonstrating that the saturation function effectively enforces constraints with minimal impact on convergence speed.
3. Comparing V with VII and VIII: Our motion-constrained algorithm converges 26.4% faster than the saturated baseline algorithm and 52.7% faster than the tanh-constrained version. This validates the superior efficiency and rapid convergence of the proposed optimization.
4. The tanh function, while bounded, introduces nonlinear distortion even for small inputs within normal ranges, which slows down the consensus process. The saturation function preserves the linear relationship within the bounds, leading to faster convergence for the drone formation task.
5. Conclusion
This paper addresses the optimization of standard second-order consensus control for drone formation tasks under practical motion constraints. The core contributions are twofold. First, a second-order consensus formation control algorithm incorporating a virtual reference point is designed. This algorithm solves the critical problems associated with the standard protocol by ensuring the drone formation converges asymptotically to a pre-defined geometric pattern at a specified reference velocity, independent of initial conditions. Second, a saturation function is intelligently integrated to derive a motion-constrained version of the algorithm. This guarantees that all control commands remain within the feasible acceleration and velocity limits of individual drones, a necessity for real-world deployment. The stability and convergence of both algorithms are rigorously proven.
Based on this optimized consensus framework, a comprehensive multi-channel controller is designed for altitude, heading, and velocity control of the drone formation. Extensive simulation studies on formation assembly tasks demonstrate the algorithms’ effectiveness. Comparative simulations show that the proposed algorithm successfully converges to the desired formation from arbitrary initial states, and that it does so significantly faster than alternative methods employing the same saturation function or different constraint-handling techniques like the hyperbolic tangent function. The proposed optimization provides a practical, efficient, and robust solution for executing essential drone formation maneuvers such as assembly, maintenance, and transformation under real-world operational constraints.