原创代码，包运行成功，可联系我获取讲解、定制多无人机协同巡逻仿真程序实现了1架长机与3架僚机的菱形编队飞行控制。系统采用集中式调度与分布式执行架构，通过串级PID控制器实现三维轨迹跟踪。

文章目录

• 程序介绍
• 物理模型与坐标系
• 长机-僚机协同策略
• 串级 PID 控制器逻辑
• 关键特性总结
• 运行结果
• MATLAB源代码

程序介绍

物理模型与坐标系

程序内置了非线性六自由度（6-DOF）无人机动力学模型。通过旋转矩阵 RR 将机体坐标系下的推力转换至惯性坐标系，并考虑了空气动力阻尼（线性阻尼 kd_lin 与旋转阻尼 kd_ang），确保了仿真环境的逼真度。

长机-僚机协同策略

• 集中式路径规划
  
  ：地面站预设 leader_wps，长机根据当前位置与目标的距离自动判定并切换下一航点。
• 分布式队形反馈
  
  ：僚机的期望位置不仅取决于长机广播的坐标和固定的 formation_offset，还引入了 formation_gain。
• 计算公式
• 这种设计增强了僚机在长机剧烈转向时的跟随速度。

串级 PID 控制器逻辑

每一架无人机均独立运行一套级联控制算法：

• 外环（位置环）
  
  输入空间误差，输出期望加速度。
• 姿态解算层
  
  将期望加速度解算为目标横滚角（Roll）和俯仰角（Pitch）。
• 内环（姿态环）
  
  输入角度误差，输出控制力矩（Torque）。

关键特性总结

• 角度归一化
  
  使用 angle_wrap 处理航向角突变问题。
• 抗饱和处理
  
  对位置积分、姿态角、推力及力矩均设置了 clamp 限幅，防止物理上的不可实现性导致的数值发散。

运行结果

本程序模拟了 1架长机（Leader）+ 3架僚机（Follower） 的协同巡逻任务。系统运行后将生成三张关键的分析图表：

1. 多无人机三维轨迹图
   
   直观展示四架无人机如何维持菱形编队完成起飞、前飞、直角转向及返航降落的全过程。图中清晰标出了长机的规划航点（Waypoints）。

1. 轴向位置跟踪与误差图
   
   详细对比了各架无人机在 X、Y、Z 三个轴向上的实际位置与期望位置，展示了级联 PID 控制器在高动态转向时的鲁棒性。

1. 队形误差与推力对比图
• 左侧
  
  记录了三架僚机相对于长机的实时位置偏差（欧氏距离），验证了在分布式反馈增益作用下，编队误差能迅速收敛至厘米级。
• 右侧
  
  展示了各机电机的总推力输出，反映了物理限幅对系统的实际约束。

1. 命令行截图

MATLAB源代码

同款代码，也可直接下载：

如需帮助，或有导航、定位滤波相关的代码定制需求，可联系作者：