2024IEEE 《基于二次规划的安全关键型多智能体系统的控制》四旋翼 无人机 MATLAB 代码复现（文献+代码）协同控制 规划 无人机 研究了基于二次规划的安全关键型多智能体系统的控制问题。 每个被控智能体被建模为一个积分器和一个不确定非线性驱动系统的级联连接。 其中，积分器表示位置-速度关系，驱动系统描述实际速度对速度参考信号的动态响应。 采用输入输出稳定性的概念来表征驱动系统的基本速度跟踪能力。 由于致动器动力学的不确定性，标准的QP避碰算法可能不可行。 即使可行，由于可能违反活动约束的满秩条件，解也可能是非lipschitz的。 此外，控制积分器与不确定作动器动力学之间的相互作用可能导致显著的非鲁棒性问题。 基于当前非线性控制理论和数值优化方法的发展，本文首先提出了一种新的可行集重构技术和一种改进的QP算法，用于可行性、鲁棒性和局部Lipschitz连续性。 然后，为了保证闭环多智能体系统的安全，我们提出了一种非线性小增益分析来处理固有的相互作用。 最后用数值方法进行了验证

当无人机群开始玩碰碰车：QP安全控制实战笔记

实验室最近搞了个大新闻——四台无人机在3x3米的空间里群魔乱舞还不撞机。这背后其实是基于二次规划（QP）的避碰算法在撑腰。不过说真的，复现那篇IEEE论文时差点被可行性问题搞崩心态，今天就带大家看看怎么用MATLAB调教这群铁鸟。

先看个刺激的：

% 紧急避让场景模拟
drones = QuadcopterSwarm(4);
drones.setTrajectory(@(t) [sin(t), cos(t)]); % 作死设定交叉航线
safety_controller = SafetyQP(drones);
sim_result = simulate(safety_controller, 0:0.1:10);
animateCollisionNearMiss(sim_result); % 这个动画看得手心冒汗

运行后你会看到四架无人机在即将相撞的瞬间突然像被无形大手拨开——这就是重构可行集的魔法。但别急着爽，先看看我们踩过的坑。

核心问题1：约束打架怎么办？

传统QP处理动态障碍容易翻车，特别是当无人机群突然改变队形时。论文里那个可行集重构，用MATLAB写出来长这样：

function [A,b] = rebuildConstraints(agents, alpha)
    % alpha是安全系数
    n = numel(agents);
    A = []; b = [];
    for i = 1:n-1
        for j = i+1:n
            rij = agents(i).position - agents(j).position;
            dij = norm(rij) - (agents(i).radius + agents(j).radius);
            A_ij = [rij'/norm(rij), -rij'/norm(rij)];
            b_ij = dij - alpha*dij^2; % 关键在这！非线性安全边界
            A = [A; A_ij];
            b = [b; b_ij];
        end
    end
end

注意第9行的二次项alpha*dij²，这个骚操作把原本线性的安全边界变成了弹性区域。实测当alpha=0.3时，就算两个铁头娃对向冲刺，约束也不会突然消失。

控制律里的猫腻

2024IEEE 《基于二次规划的安全关键型多智能体系统的控制》四旋翼 无人机 MATLAB 代码复现（文献+代码）协同控制 规划 无人机 研究了基于二次规划的安全关键型多智能体系统的控制问题。 每个被控智能体被建模为一个积分器和一个不确定非线性驱动系统的级联连接。 其中，积分器表示位置-速度关系，驱动系统描述实际速度对速度参考信号的动态响应。 采用输入输出稳定性的概念来表征驱动系统的基本速度跟踪能力。 由于致动器动力学的不确定性，标准的QP避碰算法可能不可行。 即使可行，由于可能违反活动约束的满秩条件，解也可能是非lipschitz的。 此外，控制积分器与不确定作动器动力学之间的相互作用可能导致显著的非鲁棒性问题。 基于当前非线性控制理论和数值优化方法的发展，本文首先提出了一种新的可行集重构技术和一种改进的QP算法，用于可行性、鲁棒性和局部Lipschitz连续性。 然后，为了保证闭环多智能体系统的安全，我们提出了一种非线性小增益分析来处理固有的相互作用。 最后用数值方法进行了验证

速度跟踪环节藏着魔鬼细节。论文把动力学拆成了积分器+驱动系统，对应到代码里：

classdef SafetyQP < handle
    properties
        kp = 1.2;  % 别乱改这个！血泪教训
        safe_dist = 0.5;
        Q = diag([10,10,1]); % 代价函数权重
    end
    
    methods
        function u = solveQP(obj, states)
            % 构造H矩阵时要注意物理单位
            H = blkdiag(obj.Q, obj.Q, obj.Q, obj.Q); 
            % 后面省略20行约束构造...
            [u,~,exitflag] = quadprog(H,f,A,b,Aeq,beq);
            if exitflag <= 0
                error('翻车警告：QP无解！'); 
            end
        end
    end
end

重点在Q矩阵的第三个元素设为1——因为高度方向机动性差，权重太大容易导致姿态失稳。有个师弟不信邪改成10，结果无人机直接表演海豚跳。

非线性小增益的工程实现

理论部分看着头疼？其实代码里就一个while循环：

while max(gain_ratios) > 1
    % 动态调整安全距离
    safe_dist = safe_dist * 0.95;
    updateAllConstraints();
    % 重新计算增益比
    gain_ratios = computeInteractionGains(); 
end

这个增益比监控循环像极了老司机踩刹车：系统开始震荡就稍微放松安全距离，直到整个机群达成微妙平衡。不过注意别降得太猛，0.95这个衰减系数是试了三十多组参数才确定的。

效果验证的骚操作

论文里的漂亮曲线都是怎么来的？分享个压箱底的画图技巧：

figure('Position',[100,100,800,600])
subplot(2,2,1);
plotCollisionSpheres(sim_result); % 三维轨迹球
view(135,30) % 这个视角最能唬人

subplot(2,2,2);
animateControlInputs(sim_result); % 控制量动图

% 灵魂拷问：最接近距离统计
min_dists = computeMinPairwiseDistances(sim_result);
disp(['最近距离：',num2str(min(min_dists)),'米'])

运行完记得检查控制台输出——如果最近距离小于无人机直径，赶紧检查是不是约束矩阵构造时把正负号搞反了（别问我是怎么知道的）。

后记

真正部署时发现个魔幻现象：有时候QP求解器会突然抽风报无解，但加上随机扰动又好了。后来发现是雅可比矩阵在某些奇异位形下秩亏，解决方法居然是在约束里加了个微小旋转：

A_ij = [rij'/norm(rij), -rij'/norm(rij)] + 1e-6*randn(1,8); 

这1e-6的噪声让数值稳定性直接起飞。所以说啊，理论再完美，到了代码层还是得有点engineering hack精神。