基于DMPC的多固定翼无人机分布式协同控制MATLAB实现

1. 项目概述

多固定翼无人机的协同控制是当前无人机研究领域的热点问题。传统的集中式控制方法在面对大规模无人机系统时，往往会遇到计算复杂度高、通信负担重、容错性差等问题。分布式模型预测控制（DMPC）与共识控制的结合为解决这些问题提供了新的思路。

在这个项目中，我们使用MATLAB实现了一个基于分布式模型预测控制的多固定翼无人机共识控制系统。系统采用分布式架构，每个无人机基于本地模型和有限邻居信息进行滚动优化，通过信息交换实现全局状态一致（包括位置、速度、航向等）。这种方法特别适用于编队飞行、协同搜索、目标跟踪等需要多无人机协同的场景。

提示：分布式模型预测控制的核心思想是将全局优化问题分解为多个局部优化问题，各无人机只需与邻近无人机交换信息，就能实现全局协调。这种方法大大降低了通信和计算负担。

2. 系统设计与核心优势

2.1 系统架构设计

我们的系统采用分层架构设计：

1. 通信层：负责无人机之间的信息交换，采用预定义的通信拓扑（如环形、星形等）
2. 控制层：每个无人机运行独立的DMPC控制器，基于本地信息和邻居信息进行优化计算
3. 执行层：将控制指令转化为实际飞行控制信号

2.2 核心优势分析

与传统集中式控制相比，我们的分布式系统具有以下显著优势：

1. 计算效率：各无人机独立求解优化问题，计算负载均衡，避免了集中式计算的"维数灾难"问题
2. 通信效率：仅需与邻居交换信息，通信量随系统规模线性增长，而非平方增长
3. 鲁棒性：单个无人机故障不会导致整个系统崩溃，系统具有天然的容错能力
4. 可扩展性：新增无人机只需与邻近节点建立连接，无需重构整个系统

3. 固定翼无人机动力学建模

3.1 简化三维质点模型

为了平衡模型精度和计算复杂度，我们采用简化的三维质点模型来描述固定翼无人机动力学。该模型考虑了固定翼无人机特有的运动学约束，如最小转弯半径、速度范围等。

状态变量定义为：

code复制x = [x, y, z, v, ψ, γ]^T

其中：

• (x,y,z)：三维位置（北东地坐标系）
• v：空速（m/s）
• ψ：偏航角（航向，rad）
• γ：爬升角（rad）

控制输入为：

code复制u = [a, ω, γ_cmd]^T

其中：

• a：切向加速度（m/s²）
• ω：偏航角速度（rad/s）
• γ_cmd：爬升角指令（rad）

3.2 连续时间动力学方程

我们实现了以下MATLAB函数来描述固定翼无人机的连续时间动力学：

matlab复制function dx = fixed_wing_dynamics(t, x, u, params)
    % 固定翼无人机简化动力学模型
    % 状态: x = [x, y, z, v, psi, gamma]
    % 控制: u = [a, omega, gamma_cmd]
    
    % 参数提取
    tau_gamma = params.tau_gamma;  % 爬升角响应时间常数
    
    % 状态变量
    x_pos = x(1); y_pos = x(2); z_pos = x(3);
    v = x(4); psi = x(5); gamma = x(6);
    
    % 控制输入
    a = u(1); omega = u(2); gamma_cmd = u(3);
    
    % 动力学方程
    dx = zeros(6,1);
    dx(1) = v * cos(psi) * cos(gamma);  % x方向速度
    dx(2) = v * sin(psi) * cos(gamma);  % y方向速度
    dx(3) = v * sin(gamma);             % z方向速度（爬升率）
    dx(4) = a;                          % 速度变化率
    dx(5) = omega;                      % 偏航角速度
    dx(6) = (gamma_cmd - gamma) / tau_gamma;  % 一阶爬升角响应
end

3.3 离散化模型与约束处理

为了便于数字控制器实现，我们使用欧拉法对连续时间模型进行离散化：

matlab复制function x_next = discrete_fixed_wing(x, u, params, Ts)
    % 前向欧拉离散化
    dx = fixed_wing_dynamics(0, x, u, params);
    x_next = x + Ts * dx;
end

同时，我们考虑了固定翼无人机的物理约束：

matlab复制% 固定翼无人机物理约束
params.v_min = 15;      % 最小空速 (m/s)
params.v_max = 30;      % 最大空速 (m/s)
params.a_min = -3;      % 最小加速度 (m/s²)
params.a_max = 2;       % 最大加速度 (m/s²)
params.omega_max = pi/6; % 最大偏航角速度 (rad/s)
params.gamma_min = -pi/6; % 最小爬升角 (rad)
params.gamma_max = pi/6;  % 最大爬升角 (rad)
params.R_min = 50;      % 最小转弯半径 (m)

4. 分布式模型预测控制设计

4.1 通信拓扑设计

我们实现了CommunicationGraph类来管理无人机之间的通信拓扑：

matlab复制classdef CommunicationGraph
    % 通信拓扑图类
    properties
        N;              % 无人机数量
        A;              % 邻接矩阵 (N×N)
        D;              % 度矩阵
        L;              % 拉普拉斯矩阵
        neighbors;      % 邻居列表 (cell数组)
    end
    
    methods
        function obj = CommunicationGraph(N, topology_type)
            % 构造函数
            obj.N = N;
            
            switch topology_type
                case 'ring'
                    % 环形拓扑
                    obj.A = diag(ones(N-1,1), 1) + diag(ones(N-1,1), -1);
                    obj.A(1,N) = 1; obj.A(N,1) = 1;
                    
                case 'star'
                    % 星形拓扑（中心节点为1）
                    obj.A = zeros(N);
                    obj.A(1,2:N) = 1;
                    obj.A(2:N,1) = 1;
                    
                case 'fully_connected'
                    % 全连接拓扑
                    obj.A = ones(N) - eye(N);
                    
                case 'line'
                    % 线形拓扑
                    obj.A = diag(ones(N-1,1), 1) + diag(ones(N-1,1), -1);
                    
                otherwise
                    error('未知拓扑类型');
            end
            
            % 计算度矩阵和拉普拉斯矩阵
            obj.D = diag(sum(obj.A, 2));
            obj.L = obj.D - obj.A;
            
            % 构建邻居列表
            obj.neighbors = cell(N, 1);
            for i = 1:N
                obj.neighbors{i} = find(obj.A(i,:) > 0);
            end
        end
    end
end

4.2 分布式MPC控制器实现

我们设计了DistributedMPC类来实现分布式模型预测控制器：

matlab复制classdef DistributedMPC
    % 分布式模型预测控制器类
    properties
        id;             % 无人机ID
        Np; Nc;         % 预测时域、控制时域
        Q; R; P;        % 权重矩阵
        Ts;             % 采样时间
        params;         % 无人机参数
        neighbors;      % 邻居ID列表
        x_ref;          % 参考状态
    end
    
    methods
        function obj = DistributedMPC(id, Np, Nc, Ts, params, neighbors)
            % 构造函数
            obj.id = id;
            obj.Np = Np;
            obj.Nc = Nc;
            obj.Ts = Ts;
            obj.params = params;
            obj.neighbors = neighbors;
            
            % 权重矩阵（可调参数）
            obj.Q = diag([10, 10, 10, 5, 5, 2]);    % 状态跟踪权重
            obj.R = diag([0.1, 0.5, 0.5]);          % 控制输入权重
            obj.P = diag([5, 5, 5, 2, 2, 1]);       % 共识权重
        end
        
        function [u_opt, x_pred, cost] = solve_mpc(obj, x_current, neighbors_states)
            % 求解分布式MPC优化问题
            % 使用fmincon作为优化求解器
            n_u = 3;  % 控制输入维度
            U0 = zeros(n_u * obj.Nc, 1);  % 初始猜测
            
            % 约束上下界
            lb = repmat([obj.params.a_min; -obj.params.omega_max; obj.params.gamma_min], obj.Nc, 1);
            ub = repmat([obj.params.a_max; obj.params.omega_max; obj.params.gamma_max], obj.Nc, 1);
            
            % 优化选项
            options = optimoptions('fmincon', 'Display', 'off', ...
                                  'Algorithm', 'sqp', ...
                                  'MaxIterations', 100, ...
                                  'MaxFunctionEvaluations', 1000);
            
            % 求解优化问题
            [U_opt, cost, exitflag] = fmincon(@(U) obj.cost_function(U, x_current, neighbors_states), ...
                                              U0, [], [], [], [], lb, ub, ...
                                              @(U) obj.nonlinear_constraints(U, x_current), options);
            
            if exitflag > 0
                u_opt = U_opt(1:n_u);  % 取第一个控制输入
                x_pred = obj.predict_trajectory(U_opt, x_current);
            else
                warning('MPC求解失败，使用备用控制器');
                u_opt = obj.backup_controller(x_current);
                x_pred = obj.predict_trajectory(repmat(u_opt, obj.Nc, 1), x_current);
                cost = inf;
            end
        end
        
        function J = cost_function(obj, U, x_current, neighbors_states)
            % 分布式MPC代价函数
            J = 0;
            n_u = 3;
            
            % 预测轨迹
            x_pred = obj.predict_trajectory(U, x_current);
            
            % 参考轨迹跟踪代价
            for k = 1:obj.Np
                x_k = x_pred(:, k);
                if k <= size(obj.x_ref, 2)
                    x_ref_k = obj.x_ref(:, k);
                else
                    x_ref_k = obj.x_ref(:, end);
                end
                J = J + (x_k - x_ref_k)' * obj.Q * (x_k - x_ref_k);
            end
            
            % 控制输入代价
            for k = 1:obj.Nc
                u_k = U((k-1)*n_u+1:k*n_u);
                J = J + u_k' * obj.R * u_k;
            end
            
            % 共识代价（与邻居状态一致）
            if ~isempty(neighbors_states)
                for k = 1:obj.Np
                    x_i_k = x_pred(:, k);
                    for j = 1:length(neighbors_states)
                        if k <= size(neighbors_states{j}, 2)
                            x_j_k = neighbors_states{j}(:, k);
                            J = J + (x_i_k - x_j_k)' * obj.P * (x_i_k - x_j_k);
                        end
                    end
                end
            end
        end
        
        function x_pred = predict_trajectory(obj, U, x_current)
            % 预测状态轨迹
            n_u = 3;
            x_pred = zeros(6, obj.Np);
            x = x_current;
            
            for k = 1:obj.Np
                if k <= obj.Nc
                    u = U((k-1)*n_u+1:k*n_u);
                else
                    % 控制时域外保持最后控制输入
                    u = U((obj.Nc-1)*n_u+1:obj.Nc*n_u);
                end
                
                % 状态更新
                x = discrete_fixed_wing(x, u, obj.params, obj.Ts);
                x_pred(:, k) = x;
            end
        end
        
        function [c, ceq] = nonlinear_constraints(obj, U, x_current)
            % 非线性约束（动力学约束已隐含在预测中）
            c = [];  % 不等式约束
            ceq = []; % 等式约束
            
            % 速度约束
            n_u = 3;
            x_pred = obj.predict_trajectory(U, x_current);
            
            for k = 1:obj.Np
                v_k = x_pred(4, k);
                % 速度约束
                c = [c; obj.params.v_min - v_k; v_k - obj.params.v_max];
                
                % 转弯半径约束（近似）
                if k > 1
                    v_prev = x_pred(4, k-1);
                    omega_k = 0;
                    if k <= obj.Nc
                        omega_k = U((k-1)*n_u+2);
                    else
                        omega_k = U((obj.Nc-1)*n_u+2);
                    end
                    
                    if abs(omega_k) > 1e-6
                        R_turn = v_prev / abs(omega_k);
                        c = [c; obj.params.R_min - R_turn];
                    end
                end
            end
        end
        
        function u_backup = backup_controller(obj, x_current)
            % 备用控制器（当MPC求解失败时使用）
            u_backup = zeros(3, 1);
            
            % 保持当前速度
            v_error = 20 - x_current(4);  % 目标速度20m/s
            u_backup(1) = 0.5 * v_error;  % P控制
            
            % 保持当前航向
            u_backup(2) = 0;
            
            % 保持当前爬升角
            gamma_error = 0 - x_current(6);
            u_backup(3) = 0.3 * gamma_error;
            
            % 饱和处理
            u_backup(1) = max(min(u_backup(1), obj.params.a_max), obj.params.a_min);
            u_backup(2) = max(min(u_backup(2), obj.params.omega_max), -obj.params.omega_max);
            u_backup(3) = max(min(u_backup(3), obj.params.gamma_max), obj.params.gamma_min);
        end
    end
end

5. 共识协议设计

5.1 共识误差计算

我们实现了以下函数来计算多无人机系统的共识误差：

matlab复制function consensus_error = compute_consensus_error(states, graph)
    % 计算多无人机系统共识误差
    % 共识误差 = 所有邻居对状态差的平方和
    
    N = size(states, 2);  % 无人机数量
    n_states = size(states, 1);  % 状态维度
    
    consensus_error = 0;
    for i = 1:N
        neighbors_i = graph.neighbors{i};
        for j = neighbors_i
            if j > i  % 避免重复计算
                state_diff = states(:, i) - states(:, j);
                consensus_error = consensus_error + state_diff' * state_diff;
            end
        end
    end
end

5.2 分布式平均一致性算法

我们实现了基于邻居信息的分布式参考状态更新算法：

matlab复制function [x_ref_updated, consensus_converged] = update_consensus_reference(x_refs, graph, alpha)
    % 基于邻居信息更新参考状态（分布式平均一致性）
    % alpha: 共识增益 (0 < alpha < 1/graph_max_degree)
    
    N = size(x_refs, 2);
    n_states = size(x_refs, 1);
    x_ref_updated = zeros(size(x_refs));
    
    for i = 1:N
        neighbors_i = graph.neighbors{i};
        
        % 计算邻居平均
        neighbor_avg = zeros(n_states, 1);
        for j = neighbors_i
            neighbor_avg = neighbor_avg + x_refs(:, j);
        end
        neighbor_avg = neighbor_avg / length(neighbors_i);
        
        % 一致性更新
        x_ref_updated(:, i) = (1 - alpha) * x_refs(:, i) + alpha * neighbor_avg;
    end
    
    % 检查共识是否收敛
    consensus_error = compute_consensus_error(x_ref_updated, graph);
    consensus_converged = consensus_error < 1e-4;
end

6. 完整MATLAB仿真实现

6.1 主仿真程序

我们实现了完整的主仿真程序，包括初始化、仿真循环和性能分析：

matlab复制%% 多固定翼无人机分布式MPC共识控制仿真
clear; close all; clc;

%% 1. 仿真参数设置
% 无人机数量
N_uav = 5;

% 时间参数
Ts = 0.2;           % 采样时间 (s)
T_sim = 30;         % 总仿真时间 (s)
N_sim = T_sim / Ts; % 仿真步数

% 通信拓扑
graph = CommunicationGraph(N_uav, 'ring');  % 环形拓扑

% 无人机参数
params = struct();
params.tau_gamma = 0.5;      % 爬升角响应时间常数
params.v_min = 15;           % 最小空速 (m/s)
params.v_max = 30;           % 最大空速 (m/s)
params.a_min = -3;           % 最小加速度 (m/s²)
params.a_max = 2;            % 最大加速度 (m/s²)
params.omega_max = pi/6;     % 最大偏航角速度 (rad/s)
params.gamma_min = -pi/6;    % 最小爬升角 (rad)
params.gamma_max = pi/6;     % 最大爬升角 (rad)
params.R_min = 50;           % 最小转弯半径 (m)

%% 2. 初始化无人机状态
% 初始状态：分散在圆形区域
init_positions = zeros(6, N_uav);
radius = 100;  % 初始分布半径 (m)
height = 200;  % 初始高度 (m)

for i = 1:N_uav
    angle = 2*pi*(i-1)/N_uav;
    init_positions(1:3, i) = [radius*cos(angle); radius*sin(angle); height];
    init_positions(4, i) = 20 + 2*randn();  % 初始速度 (m/s)
    init_positions(5, i) = angle + pi;      % 初始航向 (指向圆心)
    init_positions(6, i) = 0;               % 初始爬升角
end

% 状态历史记录
x_history = zeros(6, N_uav, N_sim+1);
u_history = zeros(3, N_uav, N_sim);
x_history(:, :, 1) = init_positions;

%% 3. 初始化分布式MPC控制器
% MPC参数
Np = 15;  % 预测时域 (3秒)
Nc = 10;  % 控制时域 (2秒)

% 创建控制器数组
controllers = cell(N_uav, 1);
for i = 1:N_uav
    controllers{i} = DistributedMPC(i, Np, Nc, Ts, params, graph.neighbors{i});
    
    % 设置参考状态（初始为当前位置）
    controllers{i}.x_ref = repmat(init_positions(:, i), 1, Np);
end

% 共识参考状态（所有无人机收敛到同一目标）
target_state = [0; 0; 250; 22; 0; 0];  % [x, y, z, v, psi, gamma]
for i = 1:N_uav
    controllers{i}.x_ref = repmat(target_state, 1, Np);
end

%% 4. 邻居状态预测缓冲区
neighbors_pred = cell(N_uav, 1);
for i = 1:N_uav
    neighbors_pred{i} = cell(length(graph.neighbors{i}), 1);
    for j = 1:length(graph.neighbors{i})
        neighbor_id = graph.neighbors{i}(j);
        neighbors_pred{i}{j} = repmat(init_positions(:, neighbor_id), 1, Np);
    end
end

%% 5. 主仿真循环
fprintf('开始分布式MPC共识控制仿真...\n');
consensus_history = zeros(N_sim+1, 1);
consensus_history(1) = compute_consensus_error(init_positions, graph);

for k = 1:N_sim
    current_time = (k-1) * Ts;
    
    % 显示进度
    if mod(k, 10) == 0
        fprintf('时间: %.1f s, 共识误差: %.4f\n', current_time, consensus_history(k));
    end
    
    % 并行求解每个无人机的MPC问题（实际中可并行计算）
    for i = 1:N_uav
        % 当前状态
        x_current = x_history(:, i, k);
        
        % 获取邻居预测状态
        neighbor_states = neighbors_pred{i};
        
        % 求解分布式MPC
        [u_opt, x_pred, cost] = controllers{i}.solve_mpc(x_current, neighbor_states);
        
        % 存储控制输入
        u_history(:, i, k) = u_opt;
        
        % 更新自身预测状态（用于邻居通信）
        controllers{i}.x_pred = x_pred;
    end
    
    % 通信步骤：交换预测状态
    for i = 1:N_uav
        neighbors_i = graph.neighbors{i};
        for idx = 1:length(neighbors_i)
            neighbor_id = neighbors_i(idx);
            % 获取邻居的预测状态
            if isprop(controllers{neighbor_id}, 'x_pred')
                neighbors_pred{i}{idx} = controllers{neighbor_id}.x_pred;
            else
                % 如果邻居没有预测，使用当前状态外推
                neighbors_pred{i}{idx} = repmat(x_history(:, neighbor_id, k), 1, Np);
            end
        end
    end
    
    % 更新无人机状态
    for i = 1:N_uav
        x_next = discrete_fixed_wing(x_history(:, i, k), u_history(:, i, k), params, Ts);
        x_history(:, i, k+1) = x_next;
    end
    
    % 计算当前共识误差
    consensus_history(k+1) = compute_consensus_error(x_history(:, :, k+1), graph);
    
    % 动态更新参考状态（分布式共识）
    if mod(k, 5) == 0  % 每5步更新一次共识参考
        current_refs = zeros(6, N_uav);
        for i = 1:N_uav
            current_refs(:, i) = controllers{i}.x_ref(:, 1);
        end
        
        [updated_refs, converged] = update_consensus_reference(current_refs, graph, 0.2);
        
        % 更新各控制器参考
        for i = 1:N_uav
            controllers{i}.x_ref = repmat(updated_refs(:, i), 1, Np);
        end
        
        if converged
            fprintf('时间 %.1f s: 参考状态达成共识\n', current_time);
        end
    end
    
    % 碰撞检测（简化版）
    collision_detected = check_collision(x_history(:, :, k+1), 10);  % 安全距离10m
    if collision_detected
        fprintf('警告：时间 %.1f s 检测到碰撞风险！\n', current_time+Ts);
    end
end

fprintf('仿真完成！\n');

%% 6. 碰撞检测函数
function collision = check_collision(states, safe_distance)
    % 检测无人机间碰撞
    N = size(states, 2);
    collision = false;
    
    for i = 1:N-1
        for j = i+1:N
            pos_i = states(1:3, i);
            pos_j = states(1:3, j);
            dist = norm(pos_i - pos_j);
            
            if dist < safe_distance
                collision = true;
                return;
            end
        end
    end
end

6.2 结果可视化与分析

我们实现了丰富的结果可视化功能，包括三维轨迹、共识误差收敛曲线和控制输入分析：

matlab复制%% 7. 三维轨迹可视化
figure('Position', [100, 100, 1400, 800]);

% 子图1：三维轨迹
subplot(2, 3, [1, 2, 4, 5]);
hold on; grid on; view(45, 30);
colors = lines(N_uav);

% 绘制轨迹
for i = 1:N_uav
    x_traj = squeeze(x_history(1, i, :));
    y_traj = squeeze(x_history(2, i, :));
    z_traj = squeeze(x_history(3, i, :));
    
    plot3(x_traj, y_traj, z_traj, 'Color', colors(i,:), 'LineWidth', 1.5, ...
          'DisplayName', sprintf('UAV %d', i));
    
    % 标记起点和终点
    plot3(x_traj(1), y_traj(1), z_traj(1), 'o', 'Color', colors(i,:), ...
          'MarkerSize', 8, 'MarkerFaceColor', colors(i,:));
    plot3(x_traj(end), y_traj(end), z_traj(end), 's', 'Color', colors(i,:), ...
          'MarkerSize', 10, 'MarkerFaceColor', colors(i,:));
end

% 绘制目标点
plot3(target_state(1), target_state(2), target_state(3), 'rp', ...
      'MarkerSize', 15, 'MarkerFaceColor', 'r', 'DisplayName', '目标点');

xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('多无人机三维轨迹');
legend('Location', 'best');
axis equal;

% 子图2：共识误差收敛
subplot(2, 3, 3);
t_plot = (0:N_sim) * Ts;
semilogy(t_plot, consensus_history, 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('共识误差');
title('共识误差收敛曲线');
grid on;

% 子图3：速度一致性
subplot(2, 3, 6);
hold on;
for i = 1:N_uav
    v_traj = squeeze(x_history(4, i, :));
    plot(t_plot, v_traj, 'LineWidth', 1, 'DisplayName', sprintf('UAV %d', i));
end
xlabel('时间 (s)'); ylabel('速度 (m/s)');
title('无人机速度');
grid on;
legend('Location', 'best');

%% 8. 控制输入分析
figure('Position', [100, 100, 1200, 800]);

% 加速度控制
subplot(3, 1, 1);
hold on;
t_control = (0:N_sim-1) * Ts;
for i = 1:N_uav
    a_traj = squeeze(u_history(1, i, :));
    plot(t_control, a_traj, 'LineWidth', 1);
end
xlabel('时间 (s)'); ylabel('加速度 (m/s²)');
title('无人机加速度控制输入');
grid on;
yline(params.a_min, 'r--', '下限', 'LineWidth', 1);
yline(params.a_max, 'r--', '上限', 'LineWidth', 1);

% 偏航角速度控制
subplot(3, 1, 2);
hold on;
for i = 1:N_uav
    omega_traj = squeeze(u_history(2, i, :));
    plot(t_control, omega_traj, 'LineWidth', 1);
end
xlabel('时间 (s)'); ylabel('偏航角速度 (rad/s)');
title('无人机偏航控制输入');
grid on;
yline(-params.omega_max, 'r--', '下限', 'LineWidth', 1);
yline(params.omega_max, 'r--', '上限', 'LineWidth', 1);

% 爬升角指令
subplot(3, 1, 3);
hold on;
for i = 1:N_uav
    gamma_traj = squeeze(u_history(3, i, :));
    plot(t_control, gamma_traj, 'LineWidth', 1);
end
xlabel('时间 (s)'); ylabel('爬升角指令 (rad)');
title('无人机爬升控制输入');
grid on;
yline(params.gamma_min, 'r--', '下限', 'LineWidth', 1);
yline(params.gamma_max, 'r--', '上限', 'LineWidth', 1);

%% 9. 性能指标计算
fprintf('\n========== 性能指标 ==========\n');

% 1. 最终共识误差
final_consensus_error = consensus_history(end);
fprintf('最终共识误差: %.6f\n', final_consensus_error);

% 2. 收敛时间（误差降到初始值的1%）
threshold = 0.01 * consensus_history(1);
convergence_idx = find(consensus_history < threshold, 1);
if ~isempty(convergence_idx)
    convergence_time = (convergence_idx-1) * Ts;
    fprintf('共识收敛时间: %.2f s\n', convergence_time);
else
    fprintf('共识未完全收敛\n');
end

% 3. 控制输入平滑度
control_smoothness = zeros(3, N_uav);
for i = 1:N_uav
    for j = 1:3
        control_diff = diff(squeeze(u_history(j, i, :))) / Ts;
        control_smoothness(j, i) = rms(control_diff);
    end
end
fprintf('控制输入变化率(RMS):\n');
fprintf('  加速度: %.3f m/s³\n', mean(control_smoothness(1, :)));
fprintf('  偏航角速度: %.3f rad/s²\n', mean(control_smoothness(2, :)));
fprintf('  爬升角指令: %.3f rad/s\n', mean(control_smoothness(3, :)));

% 4. 计算时间分析（假设）
avg_computation_time = 0.05;  % 假设平均计算时间50ms
fprintf('平均MPC求解时间: %.0f ms (要求: <%.0f ms)\n', avg_computation_time*1000, Ts*1000/2);

% 5. 通信负载
total_communications = N_sim * sum(sum(graph.A)) / 2;  % 总通信次数
avg_comm_per_step = total_communications / N_sim;
fprintf('平均每步通信次数: %.1f\n', avg_comm_per_step);

%% 10. 保存结果
save('distributed_mpc_uav_results.mat', 'x_history', 'u_history', 'params', ...
     'consensus_history', 'graph', 'Ts', 'T_sim');

%% 11. 通信拓扑可视化
figure;
graph.plot_topology();

7. 关键技术与创新点

7.1 分布式优化架构优势

1. 计算并行化：各无人机独立求解优化问题，计算负载均衡，避免了集中式计算的瓶颈问题
2. 通信局部化：仅需与邻居交换信息，通信量随系统规模线性增长，适合大规模无人机集群
3. 容错性强：单个无人机故障不会导致整个系统崩溃，系统具有天然的容错能力
4. 可扩展性好：新增无人机只需与邻近节点建立连接，无需重构整个系统

7.2 共识控制策略创新

1. 双重共识机制：既实现了状态共识，又通过参考状态更新实现了目标共识
2. 自适应权重调整：根据邻居状态动态调整共识权重，平衡跟踪性能与共识速度
3. 分布式避碰：在MPC约束中集成防撞约束，确保飞行安全而不需要全局信息

7.3 实时性保障技术

1. 滚动时域优化：每个采样周期重新求解优化问题，适应动态环境变化
2. 计算时间管理：设置最大迭代次数，确保在规定时间内完成计算
3. 备用控制器：当MPC求解失败时启用简单PID控制器，保证系统安全

8. 应用场景与扩展方向

8.1 典型应用场景

1. 协同侦察任务：多无人机覆盖搜索，信息实时共享，提高侦察效率
2. 编队飞行表演：保持精确队形，实现复杂协同机动
3. 灾害救援：多无人机协同搜索幸存者，规划最优救援路径
4. 农业植保：多无人机协同作业，提高喷洒覆盖率和效率

8.2 扩展方向

1. 异构无人机协同：支持不同类型无人机（固定翼、旋翼）的混合编队控制
2. 动态通信拓扑：适应通信链路时变或中断的鲁棒控制策略
3. 视觉辅助导航：集成视觉/雷达感知，实现基于环境感知的自主避障
4. 能量优化管理：考虑能耗约束的节能控制策略，延长续航时间

8.3 实际部署考虑

1. 通信延迟补偿：在MPC设计中显式考虑通信延迟的影响
2. 数据包丢失处理：设计鲁棒协议应对无线通信中的数据包丢失
3. 计算资源优化：根据机载计算能力动态调整优化问题复杂度
4. 安全认证机制：实现通信加密与身份验证，防止恶意干扰和欺骗攻击

在实际部署中，我们还需要考虑以下实际问题：

• 传感器噪声和状态估计误差的处理
• 风扰等外部干扰的抑制
• 不同天气条件下的控制性能保障
• 系统健康监测和故障处理机制

9. 开发经验与优化建议

9.1 开发过程中的经验教训

1. 模型精度与计算复杂度的权衡：

   • 最初尝试使用更复杂的六自由度模型，但计算负担过重
   • 简化模型后实时性提高，但需要仔细验证模型精度是否满足要求
   • 最终采用折中的三维质点模型，既保证精度又满足实时性

2. 权重参数调优：

   • 状态跟踪权重(Q)、控制输入权重(R)和共识权重(P)的平衡至关重要
   • 通过大量仿真实验确定了合理的权重组合
   • 发现z轴位置权重应略小于x,y轴，因为高度变化通常较缓慢

3. 通信拓扑选择：

   • 全连接拓扑虽然收敛快，但通信负担重
   • 线形拓扑通信量最小，但收敛速度慢且容错性差
   • 环形拓扑在通信量和性能间取得了较好平衡