原创代码，仅供学习，请勿翻卖
文章目录
运行结果
MATLAB源代码
代码如下：
matlab
%% 多无人机编队协同控制与三维轨迹规划仿真
% 功能：
% 1. 构建多架无人机三维编队飞行场景
% 2. 领航无人机按照预设航路点完成轨迹规划
% 3. 跟随无人机通过编队控制保持期望相对位置
% 4. 引入障碍物斥力场，实现简单避障
% 5. 输出三维轨迹、编队误差、控制输入和性能统计结果
clear; clc; close all;
rng(1);
%% 参数设置
dt = 0.05; % 仿真步长
T = 80; % 仿真总时间
t = 0:dt:T;
N = length(t);
numUAV = 5; % 无人机数量
leaderID = 1; % 领航无人机编号
% 控制参数
kp_leader = 1.8; % 领航机位置控制增益
kv_leader = 1.2; % 领航机速度阻尼增益
kp_form = 2.4; % 编队位置控制增益
kv_form = 1.6; % 编队速度阻尼增益
k_obs = 4.0; % 障碍物避障斥力增益
obsInfluence = 8.0; % 障碍物影响半径
uMax = 6.0; % 最大控制加速度限制
vMax = 8.0; % 最大速度限制
% 领航机期望航路点
waypoints = [
0, 0, 10;
25, 0, 12;
45, 20, 15;
65, 10, 18;
85, 35, 16;
110, 30, 14
];
% 障碍物设置：[x, y, z, radius]
obstacles = [
35, 8, 13, 5;
60, 18, 16, 6;
82, 28, 15, 5
];完整代码：
https://download.csdn.net/download/callmeup/92836007
程序详解
多无人机编队协同控制与三维轨迹规划仿真程序介绍
本文程序构建了一个多无人机编队协同控制与三维轨迹规划仿真模型，用于验证领航---跟随式编队控制、三维航路点跟踪、障碍物规避以及无人机间安全距离保持等功能。仿真场景中共设置 5 5 5 架无人机，其中 UAV-1 作为领航无人机，其余无人机作为跟随无人机。领航无人机按照给定三维航路点序列完成轨迹跟踪任务，跟随无人机则依据相对于领航机的期望空间偏移量形成楔形/箭头形编队。程序同时引入球形障碍物模型和人工势场思想，使无人机在执行编队飞行任务时具备一定的避障能力和安全间隔保持能力。该代码采用 MATLAB 单文件形式实现，包含参数初始化、状态递推、控制律计算、避障斥力计算、误差统计和结果可视化等模块。
在运动学建模方面，每架无人机被简化为三维空间中的二阶积分系统。设第 i i i 架无人机在时刻 k k k 的位置、速度和控制输入分别为 p i ( k ) \mathbf{p}_i(k) pi(k)、 v i ( k ) \mathbf{v}_i(k) vi(k) 和 u i ( k ) \mathbf{u}_i(k) ui(k)，其中：
p i ( k ) = [ x i ( k ) y i ( k ) z i ( k ) ] T \mathbf{p}_i(k)= \begin{bmatrix} x_i(k) & y_i(k) & z_i(k) \end{bmatrix}^{T} pi(k)=[xi(k)yi(k)zi(k)]T
v ∗ i ( k ) = [ v ∗ x , i ( k ) v y , i ( k ) v z , i ( k ) ] T \mathbf{v}*i(k)= \begin{bmatrix} v*{x,i}(k) & v_{y,i}(k) & v_{z,i}(k) \end{bmatrix}^{T} v∗i(k)=[v∗x,i(k)vy,i(k)vz,i(k)]T
系统离散状态更新方程为：
v i ( k + 1 ) = v i ( k ) + u i ( k ) Δ t \mathbf{v}_i(k+1)=\mathbf{v}_i(k)+\mathbf{u}_i(k)\Delta t vi(k+1)=vi(k)+ui(k)Δt
p i ( k + 1 ) = p i ( k ) + v i ( k + 1 ) Δ t \mathbf{p}_i(k+1)=\mathbf{p}_i(k)+\mathbf{v}_i(k+1)\Delta t pi(k+1)=pi(k)+vi(k+1)Δt
其中， Δ t \Delta t Δt 为仿真步长。为保证飞行过程的物理合理性，程序对无人机速度和控制加速度进行限幅处理，即满足：
∣ v ∗ i ( k ) ∣ ≤ v ∗ max |\mathbf{v}*i(k)| \leq v*{\max} ∣v∗i(k)∣≤v∗max
∣ u ∗ i ( k ) ∣ ≤ u ∗ max |\mathbf{u}*i(k)| \leq u*{\max} ∣u∗i(k)∣≤u∗max
在领航无人机控制中，领航机根据当前位置与目标航路点之间的偏差生成期望速度方向。设当前目标航路点为 p w p \mathbf{p}_{wp} pwp，领航无人机当前位置为 p L ( k ) \mathbf{p}_L(k) pL(k)，则其期望运动方向可表示为：
d ∗ L ( k ) = p ∗ w p − p ∗ L ( k ) ∣ p ∗ w p − p L ( k ) ∣ \mathbf{d}*L(k)= \frac{\mathbf{p}*{wp}-\mathbf{p}*L(k)} {|\mathbf{p}*{wp}-\mathbf{p}_L(k)|} d∗L(k)=∣p∗wp−pL(k)∣p∗wp−p∗L(k)
对应的期望速度为：
v ∗ L , d ( k ) = α v ∗ max d L ( k ) \mathbf{v}*{L,d}(k)=\alpha v*{\max}\mathbf{d}_L(k) v∗L,d(k)=αv∗maxdL(k)
其中， α \alpha α 为速度比例系数。领航无人机采用比例---阻尼型控制律，其控制输入为：
u L ( k ) k p , L [ p ∗ w p − p ∗ L ( k ) ] + k ∗ v , L [ v ∗ L , d ( k ) − v ∗ L ( k ) ] + u ∗ o b s , L ( k ) \mathbf{u}L(k) k{p,L} \left[ \mathbf{p}*{wp}-\mathbf{p}*L(k) \right] + k*{v,L} \left[ \mathbf{v}*{L,d}(k)-\mathbf{v}*L(k) \right] + \mathbf{u}*{obs,L}(k) uL(k)kp,L[p∗wp−p∗L(k)]+k∗v,L[v∗L,d(k)−v∗L(k)]+u∗obs,L(k)
式中， k p , L k_{p,L} kp,L 和 k v , L k_{v,L} kv,L 分别表示领航机位置控制增益和速度阻尼增益， u o b s , L ( k ) \mathbf{u}_{obs,L}(k) uobs,L(k) 为障碍物避障斥力项。该控制律使领航无人机能够在跟踪预设航路点的同时，根据障碍物分布对轨迹进行局部调整。
对于跟随无人机，其核心任务是保持相对于领航机的期望编队位置。设第 i i i 架跟随无人机相对于领航机的期望偏移量为 δ i \boldsymbol{\delta}_i δi，则其期望位置为：
p i , d ( k ) = p L ( k ) + δ i \mathbf{p}_{i,d}(k)=\mathbf{p}_L(k)+\boldsymbol{\delta}_i pi,d(k)=pL(k)+δi
其中，编队保持控制项为：
u f o r m , i ( k ) k p , F [ p i , d ( k ) − p ∗ i ( k ) ] + k ∗ v , F [ v L ( k ) − v i ( k ) ] \mathbf{u}{form,i}(k) k{p,F} \left[ \mathbf{p}_{i,d}(k)-\mathbf{p}*i(k) \right] + k*{v,F} \left[ \mathbf{v}_L(k)-\mathbf{v}_i(k) \right] uform,i(k)kp,F[pi,d(k)−p∗i(k)]+k∗v,F[vL(k)−vi(k)]
式中， k p , F k_{p,F} kp,F 为跟随无人机编队位置控制增益， k v , F k_{v,F} kv,F 为速度阻尼增益。该项使跟随无人机在位置上趋近期望编队点，在速度上趋近领航无人机，从而实现多无人机系统的协同运动与构型保持。
为实现障碍物规避，程序将障碍物建模为球体。设第 j j j 个障碍物的中心位置为 c j \mathbf{c}_j cj，半径为 r j r_j rj，无人机当前位置为 p i \mathbf{p}_i pi，则无人机到障碍物表面的距离为：
d i j = ∣ p i − c j ∣ − r j d_{ij}=|\mathbf{p}_i-\mathbf{c}_j|-r_j dij=∣pi−cj∣−rj
当 0 < d i j < R o b s 0<d_{ij}<R_{obs} 0<dij<Robs 时，障碍物对无人机产生斥力，其表达式为：
u o b s , i j k o b s ( 1 d i j − 1 R o b s ) 1 d i j 2 p i − c j ∣ p i − c j ∣ \mathbf{u}{obs,ij} k{obs} \left( \frac{1}{d_{ij}}-\frac{1}{R_{obs}} \right) \frac{1}{d_{ij}^{2}} \frac{\mathbf{p}_i-\mathbf{c}_j} {|\mathbf{p}_i-\mathbf{c}_j|} uobs,ijkobs(dij1−Robs1)dij21∣pi−cj∣pi−cj
其中， R o b s R_{obs} Robs 为障碍物影响半径， k o b s k_{obs} kobs 为障碍物斥力增益。当无人机位于障碍物影响范围之外时，障碍物斥力为零。通过该斥力项，无人机在靠近障碍物时能够产生远离障碍物方向的控制分量，从而降低轨迹穿越障碍区域的风险。
在性能评价方面，程序采用编队误差衡量无人机对期望编队构型的保持能力。第 i i i 架无人机在时刻 k k k 的编队误差定义为：
e i ( k ) = ∣ p i ( k ) − [ p L ( k ) + δ i ] ∣ e_i(k)= \left| \mathbf{p}_i(k)- \left[ \mathbf{p}_L(k)+\boldsymbol{\delta}_i \right] \right| ei(k)=∣pi(k)−[pL(k)+δi]∣
系统平均编队误差为：
e ˉ ( k ) = 1 N u ∑ i = 1 N u e i ( k ) \bar{e}(k)= \frac{1}{N_u} \sum_{i=1}^{N_u} e_i(k) eˉ(k)=Nu1i=1∑Nuei(k)
进一步地，程序统计平均编队误差、最大编队误差、编队误差均方根和最终编队误差等指标：
e a v g = 1 N ∑ k = 1 N e ˉ ( k ) e_{avg}= \frac{1}{N} \sum_{k=1}^{N} \bar{e}(k) eavg=N1k=1∑Neˉ(k)
e m a x = max 1 ≤ k ≤ N e ˉ ( k ) e_{max}= \max_{1\leq k\leq N} \bar{e}(k) emax=1≤k≤Nmaxeˉ(k)
e R M S E = 1 N ∑ k = 1 N e ˉ 2 ( k ) e_{RMSE}= \sqrt{ \frac{1}{N} \sum_{k=1}^{N} \bar{e}^{2}(k) } eRMSE=N1k=1∑Neˉ2(k)
e f i n a l = e ˉ ( N ) e_{final}=\bar{e}(N) efinal=eˉ(N)
其中， e a v g e_{avg} eavg 用于反映整个飞行过程中的总体编队保持精度， e m a x e_{max} emax 用于描述编队控制过程中的最大偏差， e R M S E e_{RMSE} eRMSE 表征误差能量意义下的综合控制性能， e f i n a l e_{final} efinal 则用于评价仿真结束时编队构型的收敛状态。
程序运行后可输出多无人机三维飞行轨迹、XY 平面轨迹、编队保持误差、控制输入幅值以及高度变化曲线，并在命令行中给出主要性能统计结果。整体而言，该程序建立了一个较为完整的多无人机三维协同飞行仿真框架，能够直观展示领航---跟随式编队控制在航路点跟踪、构型保持和局部避障任务中的应用效果，可作为多无人机编队控制、协同轨迹规划和集群避障算法研究的基础仿真平台。
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者