无人机分布式通信与二阶一致性协议

简介#

本文基于 ROS2+PX4 无人机编队仿真环境和多机 Offboard 控制的基础，介绍分布式通信在无人机集群协同控制系统中的应用。通过Gazebo仿真环境和ROS2的分布式通信框架，结合PX4飞控的MAVLink协议，实现多无人机间的实时状态共享与协同控制。系统采用一致性算法，使无人机仅依赖局部邻居节点的信息即可达成编队队形保持、沿轨迹飞行等目标，为大规模无人机集群的协同任务提供一种高可靠性的编队间通信方式。

常见的无人机编队通信方式有集中式控制和分布式控制两种核心架构，主要区别在于决策层级、通信结构和系统鲁棒性。

集中式控制依赖单一中央节点（如地面站或主无人机）进行全局决策，所有无人机根据中央节点指令执行统一动作。其优势在于控制精度高、队形一致性佳，适合复杂任务。然而，这种通信方式存在单点故障风险，且单一节点通信压力大，扩展性较差。尤其是当中央节点出现故障或受到攻击时，编队控制的安全性会受到极大影响。

分布式控制则无中心节点，各无人机通过局部通信自主决策，仅需与邻近无人机交互信息。这种方式具有强鲁棒性、高扩展性和动态适应性，适合大规模编队，但协调复杂度较高，全局优化能力较弱。

本文基于六架无人机组成的编队，首先建立一阶集群协同控制律，然后拓展至更一般化的二阶模型（加速度输入），并通过LMI方法求解反馈增益矩阵，最终在Gazebo仿真中进行验证。文章末尾给出含常定编队偏置的严格误差动力学证明。

一阶集群协同控制#

系统建模#

考虑由 $N+1$ 架无人机组成的编队。编号为 $0$ 的无人机为领航者（leader），其余 $N$ 架编号为 $i=1,2,\dots,N$ 的无人机为跟随者（followers）。

每架无人机采用一阶运动学模型，其状态仅包含位置信息。设第 $i$ 架无人机的位置为 $p_i \in \mathbb{R}^3$ ，控制输入为速度指令 $v_i \in \mathbb{R}^3$ ，则：

\dot{p}_i = v_i

一阶一致性协议#

选取经典线性一致性协议，第 $i$ 架无人机的控制输入由其邻居状态偏差的加权和决定：

v_i = k_p \sum_{j \in \mathcal{N}_i} w_{ij}(p_j - p_i)

其中 $\mathcal{N}_i$ 为第 $i$ 架无人机的邻居集合， $w_{ij} \ge 0$ 为通信权重， $k_p > 0$ 为位置增益。

可以证明，对于连通图，各无人机的位置状态满足 $p_i(t) - p_j(t) \to 0$ （ $t \to \infty$ ），即所有跟随者渐近收敛到领航者的位置。这正是分布式一致性算法在集群编队中最基础的应用形式——仅依赖邻居位置偏差，即可实现全局位置同步。

二阶分布式控制集群系统#

理论建模#

单无人机系统建模#

我们考虑由 $N+1$ 架无人机组成的编队， $0$ 号无人机作为领航者（leader），选取为虚拟领航者（不实际参与飞行）。编号 $i = 1, 2, \dots, N$ 的无人机为跟随者（followers）。

每架无人机采用二阶动力学模型（包含位置与速度状态）。记第 $i$ 架无人机的状态为：

\xi_i = \begin{bmatrix} p_i \\ v_i \end{bmatrix} \in \mathbb{R}^6, \quad p_i \in \mathbb{R}^3, \quad v_i \in \mathbb{R}^3

其中 $p_i$ 为位置， $v_i$ 为速度。控制输入为二阶加速度指令 $u_i \in \mathbb{R}^3$ 。

每架无人机的二阶线性化模型可以写为矩阵形式：

\dot{\xi}_i = A \xi_i + B u_i \tag{1}

矩阵 $A \in \mathbb{R}^{6 \times 6}$ 与 $B \in \mathbb{R}^{6 \times 3}$ 为块矩阵，简单推导即可得：

\dot{\xi}_i = \begin{bmatrix} \dot{p}_i \\ \dot{v}_i \end{bmatrix} = \begin{bmatrix} v_i \\ u_i \end{bmatrix} = \begin{bmatrix} 0_{3\times3} & I_3 \\ 0_{3\times3} & 0_{3\times3} \end{bmatrix} \begin{bmatrix} p_i \\ v_i \end{bmatrix} + \begin{bmatrix} 0_{3\times3} \\ I_3 \end{bmatrix} u_i

\Rightarrow A = \begin{bmatrix} 0_{3\times3} & I_3 \\ 0_{3\times3} & 0_{3\times3} \end{bmatrix}, \quad B = \begin{bmatrix} 0_{3\times3} \\ I_3 \end{bmatrix} \tag{2}

线性一致性协议#

选取经典线性一致性协议，每架跟随者无人机（ $i=1,2,\dots,N$ ）的输入可以表示为邻居状态偏差的加权和：

u_i = K \sum_{j=0}^{N} w_{ij}(\xi_j - \xi_i) \tag{3}

其中 $K$ 为状态反馈增益矩阵。

将其写为拉普拉斯矩阵形式。定义权重矩阵 $W = (w_{ij})$ ，度矩阵 $D = \text{diag}(d_0, \dots, d_N)$ 其中 $d_i = \sum_j w_{ij}$ ，拉普拉斯矩阵 $L = D - W$ ，元素满足 $l_{ii} = \sum_{j=0}^N w_{ij}$ ， $l_{ij} = -w_{ij}$ （ $i \ne j$ ）， $w_{ii}=0$ 。

将式（3）展开：

\begin{aligned} u_i &= K \sum_{j=0}^{N} w_{ij}(\xi_j - \xi_i) \\ &= K\left( \sum_{j=0}^{N} w_{ij} \xi_j - \left(\sum_{j=0}^{N} w_{ij}\right)\xi_i \right) \\ &= K\left( \sum_{j=0, j \ne i}^{N} w_{ij} \xi_j + w_{ii}\xi_i - \left(\sum_{j=0}^{N} w_{ij}\right)\xi_i \right) \\ &= K\left( -\sum_{j=0, j \ne i}^{N} l_{ij} \xi_j + 0 - l_{ii}\xi_i \right) \\ &= -K \sum_{j=0}^{N} l_{ij} \xi_j \end{aligned}

因此：

u_i = -K \sum_{j=0}^{N} l_{ij} \xi_j, \quad i = 1, 2, \dots, N \tag{4}

误差系统定义#

定义每架跟随者相对于领航者（编号 $0$ ）的误差：

e_i = \xi_i - \xi_0, \quad i = 1, \dots, N \tag{5}

由单机动力学（式（1））可得：

\dot{e}_i = \dot{\xi}_i - \dot{\xi}_0 = A\xi_i + Bu_i - \dot{\xi}_0

代入控制律（式（4））：

\dot{e}_i = A\xi_i - BK\sum_{j=0}^{N} l_{ij} \xi_j - \dot{\xi}_0

对所有 $j \ge 1$ 有 $\xi_j = e_j + \xi_0$ ，拆分为 $j=0$ 和 $j \ge 1$ 两项：

\sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j + \left(l_{i0} + \sum_{j=1}^{N} l_{ij}\right)\xi_0

利用拉普拉斯矩阵的行和为零 $\sum_{j=0}^N l_{ij} = 0$ ，因此 $l_{i0} + \sum_{j=1}^N l_{ij} = 0$ ，故：

\sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j

代回得单个跟随者 $i$ 的误差动力学方程：

\dot{e}_i = A e_i - BK \sum_{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) \tag{6}

堆叠所有 $N$ 个跟随者：

e = \begin{bmatrix} e_1 \\ \vdots \\ e_N \end{bmatrix} \in \mathbb{R}^{6N}, \quad d = \mathbf{1}_N \otimes \left(A\xi_0 - \dot{\xi}_0\right) \in \mathbb{R}^{6N}, \quad \tilde{L} = [-l_{ij}]_{i,j=1}^N

将误差动力学方程写成矩阵形式：

\dot{e} = \left(I_N \otimes A + \tilde{L} \otimes BK\right) e + d \tag{7}

Lyapunov 函数与 LMI 条件#

选取 Lyapunov 函数 $V(e) = e^\top P e$ ，其中 $P = I_N \otimes X^{-1}$ ， $X \succ 0$ 为对称正定阵：

V(e) = e^\top \left(I_N \otimes X^{-1}\right) e \tag{8}

误差动力学方程的闭环系统（齐次部分）为：

\dot{e} = A_{cl} e, \quad A_{cl} = I_N \otimes A + \tilde{L} \otimes BK \tag{9}

计算 Lyapunov 导数：

\dot{V} = e^\top \left( A_{cl}^\top P + P A_{cl} \right) e

为保证渐近稳定性，需要 $\dot{V} \prec 0$ ，即：

A_{cl}^\top P + P A_{cl} \prec 0 \tag{10}

同余变换说明： 设 $S \in \mathbb{R}^{n \times n}$ 为对称矩阵， $M \in \mathbb{R}^{n \times n}$ 为可逆矩阵，定义 $\bar{S} = M S M^\top$ ，则满足 $S \succ 0 \iff \bar{S} \succ 0$ 和 $S \prec 0 \iff \bar{S} \prec 0$ 。对任意非零向量 $z$ ， $z^\top \bar{S} z = (M^\top z)^\top S (M^\top z)$ ，因 $M$ 可逆二者符号完全相同。

运用同余变换去掉 $X^{-1}$ ：

\bar{S} = (I_N \otimes X) \left( P A_{cl} + A_{cl}^\top P \right) (I_N \otimes X)

则 $P A_{cl} + A_{cl}^\top P \prec 0 \iff \bar{S} \prec 0$ 。展开 $\bar{S}$ ：

\bar{S} = I_N \otimes (AX + XA^\top) + \tilde{L} \otimes (BKX) + \tilde{L}^\top \otimes \big(X(BK)^\top\big) \tag{11}

使用变量替换 $Y = KX$ 消除双线性耦合：

BKX = BY, \quad X(BK)^\top = (BY)^\top

于是 LMI 条件为：

I_N \otimes (AX + XA^\top) + \tilde{L} \otimes (BY) + \tilde{L}^\top \otimes (BY)^\top \prec 0, \quad X \succ 0 \tag{12}

包含编队位置的控制律#

假设编队内存在期望相对位置，定义 $r_{ij}$ 为跟随者 $i$ 与 $j$ 之间的期望相对偏差。控制律修改为：

u_i = K \sum_{j=0}^{N} w_{ij} \big[(\xi_j - \xi_i) + r_{ij}\big] = -K \sum_{j=0}^{N} l_{ij} \xi_j + K \sum_{j=0}^{N} w_{ij} r_{ij} \tag{13}

重新定义误差项。 设 $r_{ij} = r_i - r_j$ （ $r_0 = 0$ ）， $r_i$ 为跟随者 $i$ 相对于领航者的期望编队位置偏置。定义新的误差为：

e_i = \xi_i - (\xi_0 + r_i), \quad i = 1, \dots, N

对误差求导，代入单机动力学（1）和控制律（13）：

\dot{e}_i = A\xi_i - BK \sum_{j=0}^{N} l_{ij} \xi_j + BK \sum_{j=0}^{N} w_{ij} r_{ij} - \dot{\xi}_0 - \dot{r}_i \tag{14}

化简和式 1（拉普拉斯项）。 对 $j \ge 1$ ，代入 $\xi_j = e_j + \xi_0 + r_j$ ， $\xi_0 = \xi_0 + r_0$ ：

\begin{aligned} \sum_{j=0}^{N} l_{ij} \xi_j &= l_{i0} \xi_0 + \sum_{j=1}^{N} l_{ij} (e_j + \xi_0 + r_j) \\ &= \sum_{j=1}^{N} l_{ij} e_j + \Big(l_{i0} + \sum_{j=1}^{N} l_{ij}\Big) \xi_0 + \sum_{j=1}^{N} l_{ij} r_j \end{aligned}

由拉普拉斯矩阵行和为零（ $\sum_{j=0}^N l_{ij} = 0$ ）得 $l_{i0} + \sum_{j=1}^N l_{ij} = 0$ ，从而：

\sum_{j=0}^{N} l_{ij} \xi_j = \sum_{j=1}^{N} l_{ij} e_j + \sum_{j=1}^{N} l_{ij} r_j \tag{15}

化简和式 2（编队加权项）。 利用 $r_{ij} = r_i - r_j$ 及 $w_{ii} = 0$ 、 $l_{ij} = -w_{ij}$ （ $i \ne j$ ）：

\begin{aligned} \sum_{j=0}^{N} w_{ij} r_{ij} &= \sum_{j=0}^{N} w_{ij}(r_i - r_j) = r_i \sum_{j=0}^{N} w_{ij} - \sum_{j=0}^{N} w_{ij} r_j \\ &= l_{ii} r_i - \sum_{j=0, j \ne i}^{N} w_{ij} r_j = l_{ii} r_i + \sum_{j=0, j \ne i}^{N} l_{ij} r_j = \sum_{j=0}^{N} l_{ij} r_j \end{aligned}

因此：

\sum_{j=0}^{N} w_{ij} r_{ij} = \sum_{j=0}^{N} l_{ij} r_j \tag{16}

代入误差方程。 将 $\xi_i = e_i + \xi_0 + r_i$ （ $i \ge 1$ ）、（15）、（16）代入（14），展开后注意 $BK \sum_{j=1}^N l_{ij} r_j$ 项与 $BK \sum l_{ij} r_j$ 中含 $j \ge 1$ 的部分相互抵消，且 $r_0 = 0$ 使 $BK l_{i0} r_0 = 0$ ，得：

\dot{e}_i = A e_i - BK \sum_{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) + (A r_i - \dot{r}_i) \tag{17}

常定偏置情形——LMI 条件不变。 若 $r_i$ 为常定位置偏置（最常见情况），即：

r_i = \begin{bmatrix} p_{r_i} \\ 0 \end{bmatrix}, \quad \dot{r}_i = 0, \quad A = \begin{bmatrix} 0 & I \\ 0 & 0 \end{bmatrix} \Rightarrow A r_i = \begin{bmatrix} 0 \\ 0 \end{bmatrix}

额外的偏置项 $(A r_i - \dot{r}_i)$ 消失，误差动力学化归为经典的齐次形式：

\dot{e}_i = A e_i - BK \sum_{j=1}^{N} l_{ij} e_j + (A\xi_0 - \dot{\xi}_0) \tag{18}

堆叠所有跟随者：

\dot{e} = \left(I_N \otimes A + \tilde{L} \otimes BK\right) e + \mathbf{1}_N \otimes (A\xi_0 - \dot{\xi}_0) \tag{19}

与式（7）完全一致。因此 存在编队内部相对位置（常定偏置）时，Lyapunov 分析与 LMI 条件（12）保持不变。

仿真实现#

求解 LMI 不等式#

将上述 LMI 条件在 MATLAB 的 YALMIP 工具中求解。选取的拓扑结构为： $0$ 号无人机为虚拟领航者， $1$ 号无人机订阅 $0$ 、 $2$ 、 $3$ 号无人机（相对编队位置 $(0, 0)$ ）， $2$ 号无人机订阅 $1$ 号（相对编队位置 $(-5, -10)$ ）， $3$ 号无人机订阅 $1$ 号（相对编队位置 $(5, -10)$ ）。

% ------------ 设置参数 ------------
N = 3;               % follower 数量
% 系统矩阵 A (6x6) 和 B (6x3)
A = [zeros(3) eye(3); zeros(3) zeros(3)];
B = [zeros(3); eye(3)];

% 构造 L_sub 和 Ltilde
L_sub = [ 3 -1 -1;
         -1  1  0;
         -1  0  1 ];
Ltilde = -L_sub;

% ------------- YALMIP 变量 -------------
n = size(A,1);      % =6
m = size(B,2);      % =3

X = sdpvar(n,n,'symmetric');    % X ≻ 0
Y = sdpvar(m,n,'full');         % Y = K*X (m x n)

% 构造大矩阵 S = I_N ⊗ (A X + X A') + Ltilde ⊗ (B Y) + Ltilde' ⊗ (B Y)'
S = kron(eye(N), A*X + X*A') + kron(Ltilde, B*Y) + kron(Ltilde', (B*Y)');

% LMI: S < 0, X > 0
eps = 1e-6;
Constraints = [S <= -eps*eye(N*n), X >= eps*eye(n)];
Constraints = [Constraints, X == X'];

% ------------- 求解 -------------
options = sdpsettings('verbose', 1, 'solver', 'sedumi');
Objective = [];
sol = optimize(Constraints, Objective, options);

if sol.problem == 0
    Xopt = value(X);
    Yopt = value(Y);
    K = Yopt * (Xopt \ eye(n));   % K = Y * X^{-1}
    disp('Found feasible K:');
    disp(K);
else
    disp('求解失败；返回信息：');
    yalmiperror(sol.problem)
end

matlab

本文的编队位置求解结果（读者需根据自己建模求解）：

Found feasible K:
    0.6983    0.0000    0.0000    2.1929    0.0000    0.0000
   -0.0000    0.6983    0.0000   -0.0000    2.1929    0.0000
    0.0000   -0.0000    0.6983   -0.0000   -0.0000    2.1929

plaintext

因此 $K_p = 0.6983$ ， $K_v = 2.1929$ 。

加速度积分#

PX4 采用典型的级联 PID 控制结构：位置控制器 → 速度控制器 → 加速度控制器 → 姿态控制器 → 角速率控制器 → 电机。

若直接使用二阶加速度输入，需通过重力补偿和姿态解算将世界坐标系下的加速度指令转换为机体姿态指令：

a_{body}^{des} = R_b^{w^{-1}} \left( a_{world}^{des} - g \right)

在 Offboard 模式下直接采用加速度控制在工程实践中并不多（例如 $a_z = 0$ 并不能直接悬停，需给予重力补偿的经验值）。

因此，本文将线性一致性协议得到的虚拟加速度指令通过积分变换生成期望的速度输入：

a_i(k) = k_p \sum_{j \in \mathcal{N}_i} \big(p_j(k) - p_i(k) + r_{ij}(k)\big) + k_v \sum_{j \in \mathcal{N}_i} \big(v_j(k) - v_i(k)\big) \tag{20}

v_i^{des}(k+1) = v_i^{des}(k) + a_i(k) \Delta t \tag{21}

再通过一阶滤波器进行平滑处理：

v_i^{cmd}(k+1) = \alpha v_i^{des}(k+1) + (1 - \alpha) v_i^{cmd}(k) \tag{22}

其中 $\alpha \in (0, 1)$ 为滤波因子，本文取 $0.5$ 。

在 $x$ 、 $y$ 方向的代码示例：

float ax = position_gain_ * sum_dx + velocity_gain_ * sum_dvx;
float ay = position_gain_ * sum_dy + velocity_gain_ * sum_dvy;

float vx_integrated = desired_velocity_[0] + ax * dt;
float vy_integrated = desired_velocity_[1] + ay * dt;

desired_velocity_[0] = filter_alpha_ * vx_integrated
                     + (1.0f - filter_alpha_) * desired_velocity_[0];
desired_velocity_[1] = filter_alpha_ * vy_integrated
                     + (1.0f - filter_alpha_) * desired_velocity_[1];

cpp

该方法保留了二阶一致性控制的动力学特性，同时兼容 PX4 的速度控制层，具有良好的工程可实现性与稳定性。

离散化滤波器推导#

连续时间的一阶低通滤波器方程为：

\tau \dot{v}_f(t) + v_f(t) = v(t)

其中 $v_f(t)$ 为滤波后的速度信号， $v(t)$ 为输入信号， $\tau$ 为时间常数。改写为：

\dot{v}_f(t) = \frac{1}{\tau}\big(v(t) - v_f(t)\big)

在离散条件下，采样周期为 $dt$ ：

\dot{v}_f(t) \approx \frac{v_f[k] - v_f[k-1]}{dt}

代入得：

\frac{v_f[k] - v_f[k-1]}{dt} = \frac{1}{\tau}\big(v[k] - v_f[k-1]\big)

两边乘以 $dt$ 并整理：

v_f[k] - v_f[k-1] = \frac{dt}{\tau}\big(v[k] - v_f[k-1]\big)

定义 $\alpha = \frac{dt}{\tau + dt}$ ，得：

v_f[k] = (1 - \alpha) v_f[k-1] + \alpha v[k] \tag{23}

与式（22）形式一致。

ROS2 中的坐标变换注意#

在 ROS2 + PX4 启动中，每架无人机实例启动时默认以自身启动点坐标为原点 $(0, 0)$ 。因此，在每次回调订阅位置时，需人为将坐标减去启动点的坐标，获得全局坐标后再代入控制律。代码示例见下节。

参考代码#

以下给出上文建模中 $1$ 号无人机（同时订阅虚拟领航者及 $2$ 、 $3$ 号邻居无人机）的完整 C++ 代码。 $2$ 、 $3$ 号无人机的代码类似，仅需修改订阅拓扑和编队偏置。

#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <vector>
#include <map>
#include <array>
#include <limits>
#include <chrono>

struct NeighborState {
    std::array<float, 3> position = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> velocity = {0.0f, 0.0f, 0.0f};
    bool valid = false;
};

class UAV1Controller : public rclcpp::Node {
public:
    UAV1Controller() : Node("uav1_controller") {
        rclcpp::QoS qos(10);
        qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
        qos.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);

        offboard_control_mode_publisher_ = create_publisher<
            px4_msgs::msg::OffboardControlMode>(
            "/px4_1/fmu/in/offboard_control_mode", qos);
        trajectory_setpoint_publisher_ = create_publisher<
            px4_msgs::msg::TrajectorySetpoint>(
            "/px4_1/fmu/in/trajectory_setpoint", qos);
        vehicle_command_publisher_ = create_publisher<
            px4_msgs::msg::VehicleCommand>(
            "/px4_1/fmu/in/vehicle_command", qos);

        own_odometry_sub_ = create_subscription<
            px4_msgs::msg::VehicleOdometry>(
            "/px4_1/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                own_position_ = {msg->position[0], msg->position[1],
                                 msg->position[2]};
                own_velocity_ = {msg->velocity[0], msg->velocity[1],
                                 msg->velocity[2]};
                own_state_valid_ = true;
            });

        uav2_sub_ = create_subscription<px4_msgs::msg::VehicleOdometry>(
            "/px4_2/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                neighbor_states_[2] = {
                    {msg->position[0] - 5.0f, msg->position[1] - 10.0f,
                     msg->position[2]},
                    {msg->velocity[0], msg->velocity[1], msg->velocity[2]},
                    true
                };
            });

        uav3_sub_ = create_subscription<px4_msgs::msg::VehicleOdometry>(
            "/px4_3/fmu/out/vehicle_odometry", qos,
            [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) {
                neighbor_states_[3] = {
                    {msg->position[0] + 5.0f, msg->position[1] - 10.0f,
                     msg->position[2]},
                    {msg->velocity[0], msg->velocity[1], msg->velocity[2]},
                    true
                };
            });

        position_gain_ = 0.6983f;
        velocity_gain_ = 2.1929f;
        filter_alpha_ = 0.5f;

        start_time_ = this->now();
        control_timer_ = create_wall_timer(std::chrono::milliseconds(20),
                                           [this]() { this->controlLoop(); });
        last_print_time_ = this->now();
        last_control_time_ = this->now();
        desired_velocity_ = {0.0f, 0.0f, 0.0f};
    }

private:
    NeighborState getVirtualLeaderState() {
        auto current_time = this->now();
        double t = (current_time - start_time_).seconds();

        NeighborState leader_state;
        leader_state.valid = true;
        leader_state.position[2] = 0.0f;
        leader_state.velocity[2] = 0.0f;

        if (t <= 10.0) {
            leader_state.velocity[0] = 3.0f;
            leader_state.position[0] = 3.0f * t;
            leader_state.velocity[1] = 0.3f * t;
            leader_state.position[1] = 0.5f * 0.3f * t * t;
        } else if (t <= 20.0) {
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 3.0f;
            leader_state.position[1] = 15.0f + 3.0f * (t - 10.0f);
        } else if (t <= 30.0) {
            float d = t - 20.0f;
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 3.0f - 0.3f * d;
            leader_state.position[1] = 45.0f + 3.0f * d
                                       - 0.5f * 0.3f * d * d;
        } else {
            leader_state.velocity[0] = 0.0f;
            leader_state.position[0] = 30.0f;
            leader_state.velocity[1] = 0.0f;
            leader_state.position[1] = 60.0f;
        }
        return leader_state;
    }

    void controlLoop() {
        NeighborState leader_state = getVirtualLeaderState();
        neighbor_states_[0] = leader_state;

        if (!own_state_valid_ || !neighbor_states_[2].valid
            || !neighbor_states_[3].valid) return;

        double dt = (this->now() - last_control_time_).seconds();
        last_control_time_ = this->now();

        px4_msgs::msg::OffboardControlMode offboard_msg;
        offboard_msg.position = false;
        offboard_msg.velocity = true;
        offboard_msg.acceleration = false;
        offboard_msg.timestamp = this->now().nanoseconds() / 1000;
        offboard_control_mode_publisher_->publish(offboard_msg);

        static int init_counter = 0;
        if (init_counter < 20) {
            px4_msgs::msg::TrajectorySetpoint setpoint{};
            setpoint.timestamp = get_clock()->now().nanoseconds() / 1000;
            setpoint.position = {std::numeric_limits<float>::quiet_NaN(),
                                std::numeric_limits<float>::quiet_NaN(),
                                std::numeric_limits<float>::quiet_NaN()};
            setpoint.velocity = {0.0f, 0.0f, 0.0f};
            setpoint.acceleration = {std::numeric_limits<float>::quiet_NaN(),
                                    std::numeric_limits<float>::quiet_NaN(),
                                    std::numeric_limits<float>::quiet_NaN()};
            trajectory_setpoint_publisher_->publish(setpoint);
            init_counter++;
            if (init_counter == 20) {
                px4_msgs::msg::VehicleCommand cmd;
                cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE;
                cmd.param1 = 1.0f; cmd.param2 = 6.0f;
                cmd.target_system = 2; cmd.target_component = 1;
                cmd.source_system = 1; cmd.source_component = 1;
                cmd.from_external = true;
                cmd.timestamp = this->now().nanoseconds() / 1000;
                vehicle_command_publisher_->publish(cmd);
            }
            return;
        }

        float sum_dx = (neighbor_states_[0].position[0] - own_position_[0])
                     + (neighbor_states_[2].position[0] - own_position_[0]
                        + 5.0f)
                     + (neighbor_states_[3].position[0] - own_position_[0]
                        - 5.0f);

        float sum_dy = (neighbor_states_[0].position[1] - own_position_[1])
                     + (neighbor_states_[2].position[1] - own_position_[1]
                        + 10.0f)
                     + (neighbor_states_[3].position[1] - own_position_[1]
                        + 10.0f);

        float sum_dvx = (neighbor_states_[0].velocity[0] - own_velocity_[0])
                      + (neighbor_states_[2].velocity[0] - own_velocity_[0])
                      + (neighbor_states_[3].velocity[0] - own_velocity_[0]);

        float sum_dvy = (neighbor_states_[0].velocity[1] - own_velocity_[1])
                      + (neighbor_states_[2].velocity[1] - own_velocity_[1])
                      + (neighbor_states_[3].velocity[1] - own_velocity_[1]);

        float ax = position_gain_ * sum_dx + velocity_gain_ * sum_dvx;
        float ay = position_gain_ * sum_dy + velocity_gain_ * sum_dvy;

        float vx_integrated = desired_velocity_[0] + ax * dt;
        float vy_integrated = desired_velocity_[1] + ay * dt;

        desired_velocity_[0] = filter_alpha_ * vx_integrated
                             + (1.0f - filter_alpha_) * desired_velocity_[0];
        desired_velocity_[1] = filter_alpha_ * vy_integrated
                             + (1.0f - filter_alpha_) * desired_velocity_[1];
        desired_velocity_[2] = 0.0f;

        px4_msgs::msg::TrajectorySetpoint setpoint;
        setpoint.timestamp = this->now().nanoseconds() / 1000;
        setpoint.velocity = {desired_velocity_[0], desired_velocity_[1], 0.0f};
        setpoint.position = {NAN, NAN, NAN};
        setpoint.acceleration = {NAN, NAN, NAN};
        trajectory_setpoint_publisher_->publish(setpoint);
    }

    // Publishers
    rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr
        offboard_control_mode_publisher_;
    rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr
        trajectory_setpoint_publisher_;
    rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr
        vehicle_command_publisher_;

    // Subscribers
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr
        own_odometry_sub_;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr uav2_sub_;
    rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr uav3_sub_;

    rclcpp::TimerBase::SharedPtr control_timer_;

    std::array<float, 3> own_position_ = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> own_velocity_ = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> desired_velocity_ = {0.0f, 0.0f, 0.0f};
    bool own_state_valid_ = false;

    std::map<int, NeighborState> neighbor_states_;
    float position_gain_, velocity_gain_, filter_alpha_;
    rclcpp::Time last_print_time_, last_control_time_, start_time_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<UAV1Controller>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

cpp

仿真效果#

开始时，在三架无人机的终端中使用 commander takeoff 手动启动并在 $2.5\text{m}$ 高度悬停，随后在 $xy$ 方向进行控制，使其按编队跟随虚拟领航者运动。

视频 1：二阶一致性控制律编队仿真效果

整体编队运动效果良好，存在一定超调但保持了稳定性，说明二阶控制律有效。

值得关注的是，利用分布式通信的独特优势，我们还可以做进一步的鲁棒性测试：在整体开始运动后，关掉第 $1$ 号无人机的仿真（模拟被击落场景），剩余的无人机仍然按固定路线飞行，不受影响。这正是分布式通信的核心可靠性所在——利用连通的通信拓扑网络实现数据交换，在实际应用中可以有效减少单点故障对编队整体的影响。

总结#

本文从一阶集群系统出发，逐步引入二阶动力学模型，搭建了一个基于分布式通信的无人机编队控制系统。从一阶位置一致性推导至二阶模型的误差动力学方程和 Lyapunov 稳定性条件，通过同余变换将稳定性条件转化为可求解的 LMI，在 MATLAB 中利用 YALMIP 工具箱求得反馈增益矩阵；同时将加速度形式的控制指令通过积分和滤波转化为速度指令，兼容 PX4 的级联 PID 控制架构，并对含常定编队偏置的情形进行了严格推导，验证了 LMI 条件不变。

分布式控制相比于集中式控制具有更高的鲁棒性和可扩展性，在复杂、突变的实际应用场景中具有显著优势。在低空经济、蜂群作战等场景中，这种通信控制一体化的架构将发挥越来越重要的作用。

本文只是一个初步的控制方案，在仿真中也发现了许多可以改进的地方——例如如何进一步抑制超调、如何适应非连通的通信拓扑切换、如何在真实机载电脑上进行计算等。关于如何将仿真算法部署到真实无人机上，可以参考 ROS2 真机实践：为机载电脑刷机和使用机载电脑控制 PX4 无人机（MAVROS2），了解从仿真到实机的完整流程。

最后，再次感谢大家的耐心阅读和批评指正！

简介#

相关数学知识#