原创代码，禁止翻卖
文章目录
- 程序简介
- 领航者轨迹
- 运动学模型
- 关键参数说明
- 运行结果
- MATLAB源代码
程序简介
程序采用“虚拟领航者 + 人工势场”双层控制架构：
- 上层（编队层）：由领航者（UAV1）按预设轨迹飞行，其余3架跟随者根据固定的编队偏移量计算各自的期望位置，实现菱形队形保持。
- 下层（避障层）：利用人工势场法（APF）对每架无人机独立计算障碍排斥力，叠加到编队跟踪引力上，驱使无人机自主绕障。
领航者轨迹
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 高度上下小幅波动，模拟真实任务中的曲线飞行场景。
运动学模型
采用一阶积分器（双重积分器的简化形式），控制量直接作用于速度：
vel_new=vel(i,:)+u_total*dt;% 速度积分pos(i,:)=pos(i,:)+vel(i,:)*dt;% 位置积分同时对速度和控制量分别施加硬限幅（v m a x = 4 v_{max}=4vmax=4m/s，u m a x = 5 u_{max}=5umax=5m/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源代码
部分代码如下：
%% 复杂障碍环境下多无人机编队避障与协同控制仿真% 方法：人工势场法 (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
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
