原创代码，禁止翻卖
文章目录
程序简介
程序采用 "虚拟领航者 + 人工势场" 双层控制架构：
- 上层（编队层）：由领航者（UAV1）按预设轨迹飞行，其余3架跟随者根据固定的编队偏移量计算各自的期望位置，实现菱形队形保持。
- 下层（避障层）：利用人工势场法（APF）对每架无人机独立计算障碍排斥力，叠加到编队跟踪引力上，驱使无人机自主绕障。
领航者轨迹
matlab
leader_traj = @(t) [v_lead*t, 1.5*sin(0.15*t), 5 + 0.5*sin(0.2*t)];领航者在 X 方向以 1.5 m/s 匀速推进，Y 方向做低频正弦摆动，Z 方向在 5 m 高度上下小幅波动，模拟真实任务中的曲线飞行场景。
运动学模型
采用一阶积分器（双重积分器的简化形式），控制量直接作用于速度：
matlab
vel_new = vel(i,:) + u_total * dt; % 速度积分
pos(i,:) = pos(i,:) + vel(i,:) * dt; % 位置积分同时对速度和控制量分别施加硬限幅（ v m a x = 4 v_{max}=4 vmax=4 m/s， u m a x = 5 u_{max}=5 umax=5 m/s²），防止数值发散并模拟执行机构饱和约束。
关键参数说明
| 参数 | 值 | 含义 |
|---|---|---|
K_att |
2.0 | 位置跟踪引力增益，越大跟踪越紧但超调越大 |
K_damp |
1.5 | 速度阻尼增益，抑制震荡 |
K_rep |
80 | 障碍排斥力增益，越大绕障越激进 |
d0 |
4.0 m | 排斥力作用范围，需大于障碍半径+安全裕量 |
r_safe |
1.2 m | 碰撞判定安全距离（仅用于评估，不进入控制） |
运行结果
本文基于 人工势场法（APF）+ 虚拟领航者编队控制 策略，对4架无人机在复杂三维障碍环境下的编队飞行与自主避障进行仿真，共输出5张图表：
三维飞行轨迹图
展示领航者（UAV1）与3架跟随者在含5个球形障碍物场景中的完整飞行路径。图中以不同颜色区分各无人机轨迹，△标记起点，■标记终点，半透明球体为障碍物，白色虚线为编队快照连线，直观反映编队在绕障过程中的队形保持能力。
编队跟踪误差曲线
记录各无人机与其期望编队位置之间的欧氏距离随时间的变化。可观察到：在接近障碍物时误差明显增大（避障排斥力干扰引力项），绕过障碍物后误差迅速收敛恢复，体现了算法的鲁棒性与自恢复能力。竖向虚线标注各障碍物遭遇时刻。
最近障碍物距离曲线
实时记录每架无人机到最近障碍物表面的距离，并以红色虚线标注安全半径（1.2 m）。全程各机均保持在安全距离以上，验证避障算法的有效性，未发生碰撞。
控制输入幅值（三轴）
分 X / Y / Z 三个子图展示各无人机的加速度控制量随时间的变化，红色虚线为限幅边界（±5 m/s²），可直观看出遭遇障碍时控制量的峰值响应特征。
避障各阶段编队误差对比柱状图
将仿真过程划分为"避障前 / 避障中 / 避障后"三个阶段，对4机平均编队误差进行统计对比，量化评估避障机动对编队精度的影响程度及恢复质量。
命令行输出：
MATLAB源代码
部分代码如下：
matlab
%% 复杂障碍环境下多无人机编队避障与协同控制仿真
% 方法：人工势场法 (APF) + 虚拟领航者编队控制
% 输出：三维轨迹 / 编队误差 / 最近障碍物距离 / 控制输入
% 推荐版本：2018b及以上
% 作者:matlabfilter（可提供代码定制、讲解服务）
% 2026年5月10日 Ver2：修复避障后误差计算错误的问题
clear; clc; close all;
rng(0)
%% 基本参数设置
N = 4; % 无人机数量（1 领航者 + 3 跟随者）
dt = 0.05; % 时间步长 (s)
T = 70; % 仿真总时长 (s)
steps = round(T/dt);
% --- 领航者轨迹参数 ---
v_lead = 1.5; % 领航者匀速 (m/s)
% --- 编队几何（相对领航者的期望偏移，局部坐标系）---
% 队形：菱形
formation_offset = [
0, 0, 0; % UAV1 = 领航者
-3, -3, 0; % UAV2
-3, 3, 0; % UAV3
-6, 0, 0; % UAV4
];
% --- 控制增益 ---
K_att = 2.0; % 跟随引力增益
K_damp = 1.5; % 阻尼增益
K_form = 3.0; % 编队保持增益完整代码：https://blog.csdn.net/callmeup/article/details/160949545?spm=1011.2415.3001.5331
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者