基于控制障碍函数（Control Barrier Function, CBF）的无人机编队三维动态避障路径规划，MATLAB代码

针对三维空间环境下无人机编队协同航行与动态障碍物规避问题，本文提出一种基于控制障碍函数（Control Barrier Function, CBF）与二次规划（Quadratic Programming, QP）的路径规划算法。该算法以编队保持为核心约束，将动态避障、机间防碰撞、空间边界约束等安全条件转化为凸优化约束条件，通过在线求解二次规划问题生成无人机编队的安全控制指令。算法实现了无人机在三维受限空间内的稳定编队保持、多动态障碍物自主规避、机间无碰撞协同航行，具备计算实时性、约束满足性与工程实用性。仿真结果验证了该算法在三维动态环境下无人机编队路径规划的有效性与可靠性。

一、引言

随着无人机技术在巡检、侦察、编队表演等领域的广泛应用，多无人机协同航行成为研究热点。三维空间环境中，无人机编队不仅需要保持预设队形精准航行，还需实时规避随机移动的动态障碍物，同时避免无人机之间发生碰撞，对路径规划算法的实时性、安全性与协同性提出了更高要求。

传统人工势场法、RRT*等路径规划算法在三维编队场景中存在易陷入局部最优、约束处理困难、实时性差等问题。控制障碍函数能够将安全约束转化为可直接嵌入控制器的条件，结合二次规划可实现约束条件下的最优控制求解，适用于多约束、动态环境下的无人机编队控制。基于此，本文设计一种适用于三维空间的无人机编队动态避障路径规划算法，实现编队保持、动态避障、机间防碰撞的一体化协同控制。

二、系统模型与问题描述

2.1 无人机运动模型

采用三维单积分器运动模型 描述无人机动力学特性，简化模型兼顾计算实时性与控制精度，适配实时路径规划：
x˙i=ui\dot{\boldsymbol{x}}_i = \boldsymbol{u}_ix˙i=ui

其中，xi=[xi,yi,zi]T∈R3\boldsymbol{x}i = [x_i, y_i, z_i]^T \in \mathbb{R}^3xi=[xi,yi,zi]T∈R3 为第 iii 架无人机的三维位置坐标；ui=[uix,uiy,uiz]T∈R3\boldsymbol{u}i = [u{ix}, u{iy}, u_{iz}]^T \in \mathbb{R}^3ui=[uix,uiy,uiz]T∈R3 为无人机的三维控制速度指令。

2.2 编队模型

采用虚拟结构法构建无人机编队体系，将编队中心点作为虚拟领航点，所有跟随无人机相对领航点保持固定相对位置：

1. 定义编队中心点 xc\boldsymbol{x}_cxc，由所有无人机位置取均值得到；
2. 预设每架无人机相对中心点的三维相对位置 Δxi\Delta \boldsymbol{x}_iΔxi；
3. 无人机目标位置满足：xi,goal=xc+Δxi\boldsymbol{x}_{i,goal} = \boldsymbol{x}_c + \Delta \boldsymbol{x}_ixi,goal=xc+Δxi。
   该模型保证编队在航行、避障过程中始终保持预设队形，提升编队稳定性。

2.3 障碍物模型

三维空间内障碍物建模为球体，具备独立动态运动特性：

1. 静态属性：定义障碍物三维坐标 xobs,j\boldsymbol{x}{obs,j}xobs,j、半径 robsr{obs}robs，安全距离为障碍物半径与安全裕度之和；
2. 动态属性：障碍物沿球面随机方向匀速运动，定时随机变向，触碰空间边界时发生速度反弹，模拟真实动态障碍环境；
3. 约束条件：障碍物之间设置安全距离，避免障碍物发生重叠碰撞。

2.4 问题描述

在三维边界约束空间内，实现无人机编队三大核心功能：

1. 编队保持：无人机相对虚拟领航点保持固定三维相对位置；
2. 动态避障：实时规避随机移动的球形动态障碍物；
3. 机间防碰撞：无人机之间保持安全距离，无碰撞航行；
4. 目标抵达：编队整体精准航行至预设三维目标点。

三、核心算法设计

本文算法以控制障碍函数 为约束核心，结合二次规划 实现最优控制求解，分为编队控制层 、避障约束层 、优化求解层三层架构。

3.1 基于控制障碍函数的安全约束构建

控制障碍函数的核心作用是将安全条件转化为可求解的凸约束，保证系统始终运行在安全集合内。

3.1.1 动态障碍物避障约束

针对第 jjj 个动态障碍物，定义三维避障障碍函数：
hobs,j(xc)=∥xc−xobs,j∥−Dsafeh_{obs,j}(\boldsymbol{x}c) = \|\boldsymbol{x}c - \boldsymbol{x}{obs,j}\| - D{safe}hobs,j(xc)=∥xc−xobs,j∥−Dsafe

其中，∥⋅∥\|\cdot\|∥⋅∥ 为三维欧几里得范数；DsafeD_{safe}Dsafe 为编队与障碍物之间的安全距离。

约束条件要求 hobs,j(xc)≥0h_{obs,j}(\boldsymbol{x}_c) \geq 0hobs,j(xc)≥0，即编队中心点始终处于障碍物安全范围外。

3.1.2 无人机机间防碰撞约束

针对任意两架无人机 iii 和 kkk，定义机间避障障碍函数：
huav,ik(xi,xk)=∥xi−xk∥−dsafeh_{uav,ik}(\boldsymbol{x}_i,\boldsymbol{x}_k) = \|\boldsymbol{x}_i - \boldsymbol{x}k\| - d{safe}huav,ik(xi,xk)=∥xi−xk∥−dsafe

其中，dsafed_{safe}dsafe 为无人机之间的安全距离，约束条件要求 huav,ik≥0h_{uav,ik} \geq 0huav,ik≥0，杜绝机间碰撞。

3.1.3 三维空间边界约束

定义三维空间边界 x∈[xmin,xmax]x \in [x_{min},x_{max}]x∈[xmin,xmax]、y∈[ymin,ymax]y \in [y_{min},y_{max}]y∈[ymin,ymax]、z∈[zmin,zmax]z \in [z_{min},z_{max}]z∈[zmin,zmax]，构建边界约束函数，保证无人机始终在规划空间内运动。

3.2 基于二次规划的最优控制求解

将编队控制目标与安全约束整合为二次规划问题，在线实时求解无人机控制速度：

3.2.1 优化目标函数

目标为最小化控制速度与期望编队速度的偏差，保证编队平稳航行：
min⁡u12uTIu−ucTu\min_{\boldsymbol{u}} \frac{1}{2}\boldsymbol{u}^T \boldsymbol{I} \boldsymbol{u} - \boldsymbol{u}_c^T \boldsymbol{u}umin21uTIu−ucTu

其中，I\boldsymbol{I}I 为三维单位矩阵；uc\boldsymbol{u}_cuc 为编队中心点向目标点运动的期望速度。

3.2.2 约束条件

1. 避障约束：−Aobsu≤αhobs-\boldsymbol{A}{obs} \boldsymbol{u} \leq \alpha h{obs}−Aobsu≤αhobs；
2. 机间防碰撞约束：−Auavu≤αhuav-\boldsymbol{A}{uav} \boldsymbol{u} \leq \alpha h{uav}−Auavu≤αhuav；
3. 速度限幅约束：umin≤u≤umax\boldsymbol{u}{min} \leq \boldsymbol{u} \leq \boldsymbol{u}{max}umin≤u≤umax。
   α\alphaα 为避障强度系数，用于调节约束响应速度。

3.2.3 求解与控制输出

采用二次规划求解器实时求解优化问题，得到无人机三维控制速度，通过龙格-库塔数值积分更新无人机三维位置，保证控制精度与平滑性。

3.3 动态障碍物运动控制

为模拟真实三维动态环境，障碍物采用球面随机运动+边界反弹策略：

1. 随机变向：每隔固定步数，生成球面随机角度（天顶角、方位角），计算三维随机速度；
2. 边界反弹：障碍物触碰三维空间边界时，对应轴速度反向，避免越界；
3. 障碍间避障：检测障碍物之间距离，小于安全距离时速度反向，防止重叠。

四、代码实现功能与模块设计

基于MATLAB实现三维无人机编队动态避障路径规划算法，代码模块化设计，核心功能模块如下：

clc; clear all; close all; warning off;

% 1. 无人机配置（三维）
UAV_NUM = 6;                  % 无人机数量
car_position = [-2, -3, -2, -1, -3,-1; 
                -2, -2, -3, -2.5, -0.5,0; 
                0, 1, -1, 0.5, -0.5, 1];   % 新增Z轴初始位置

% 2. 障碍物初始配置（三维，独立随机移动）
OBSTACLE_NUM = 6;             % 障碍物数量
obstacle_position = [0, 2, 3, 4, 1, 3; 
                     -1, 2, 4, 5, -2, 2.5;
                     5, 1.5, -1, 2, 0.5, 6];  % 新增Z轴初始位置

% 3. 独立动态障碍物核心参数（每个障碍物随机不同）
obstacle_v_max = 1;        % 最大速度
obstacle_dir_change_T = 10;   % 每隔N步随机变向
bound_x = [-4, 9];            % X边界
bound_y = [-4, 9];            % Y边界
bound_z = [-4, 9];            % 新增Z边界
obstacle_radius = 0.6;        % 障碍物半径（三维球体）
safe_margin = 0.5;            % 安全裕度

% 4. 固定仿真参数（三维适配）
goal_position = [7; 7; 3];    % 目标点（新增Z轴）
car_radius = 0.1;             % 无人机半径
center_radius = 0.05;         % 中心点半径
safe_margin = 0.5;            % 安全裕度
Time = 25;                    % 总时间
T = 0.05;                     % 步长
alpha = 0.9;                  % 避障强度
speed_limit = 1;              % 无人机速度限幅
goal_threshold = 0.2;         
obstacle_safe_dist = 2*(obstacle_radius + safe_margin); % 障碍物间安全距离
uav_safe_dist = 0.6;          % 无人机之间安全距离（互不干扰）

% ====================== 初始化核心参数 ======================
center_position = mean(car_position(1:3, :), 2);  % 三维中心点
relative_goal_position = goal_position - center_position;
car_goal_position = car_position(1:3,:) + relative_goal_position; % 无人机各自终点（三维）
last_car_position = car_position;

% 为每个障碍物分配独立随机方向、速度、计数器（三维）
obstacle_vel = zeros(3, OBSTACLE_NUM);  % 三维速度
obstacle_dir_timer = randi([5, obstacle_dir_change_T], 1, OBSTACLE_NUM);

% 安全距离（三维）
distances = sqrt((car_position(1, :) - center_position(1)).^2 + ...
                 (car_position(2, :) - center_position(2)).^2 + ...
                 (car_position(3, :) - center_position(3)).^2);
max_radius = max(distances);
safe_distance = max_radius + obstacle_radius + safe_margin;
relative_center_position = car_position(1:3, :) - center_position;

4.1 参数初始化模块

1. 系统参数：定义无人机数量、障碍物数量、仿真时间、步长、速度限幅等核心参数；
2. 三维坐标初始化：设置无人机、障碍物、目标点的三维初始位置；
3. 安全参数：配置障碍物半径、无人机安全距离、空间边界、避障强度系数等约束参数。

4.2 动态障碍物更新模块

1. 独立运动控制：每个障碍物拥有独立的运动计时器、速度与方向，实现完全随机动态移动；
2. 三维约束处理：完成边界反弹、障碍间碰撞检测与规避；
3. 实时位置更新：迭代更新障碍物三维坐标，为避障算法提供实时环境信息。

4.3 编队控制与优化求解模块

1. 编队中心点计算：实时解算无人机编队虚拟领航点位置；
2. 约束构建：集成动态避障、机间防碰撞、速度限幅约束；
3. 二次规划求解：调用quadprog函数求解最优三维控制速度；
4. 位置更新：采用四阶龙格-库塔法积分更新无人机三维位置，保证运动平滑。

4.4 三维可视化模块

1. 3D主场景：绘制三维坐标系、无人机（彩色球体）、动态障碍物（红色半透明球体）、目标点、三维飞行轨迹；
2. 数据曲线：实时绘制X/Y/Z三轴速度曲线、编队保持误差曲线、无人机到目标点距离曲线；
3. 动态刷新：实时更新所有实体位置与轨迹，直观展示编队避障全过程。

五、结论

本文提出的基于控制障碍函数与二次规划的三维无人机编队动态避障路径规划算法，成功实现了三维动态环境下多无人机的协同安全航行。算法通过控制障碍函数将各类安全约束转化为优化条件，结合二次规划实现最优控制输出，具备实时性强、约束满足性好、编队稳定性高等优点。MATLAB仿真结果表明，该算法能够有效驱动无人机编队保持预设队形，自主规避随机动态障碍物，精准抵达目标点，可为三维空间多无人机协同作业提供可靠的路径规划解决方案。

---

六、完整MATLAB见下方名片