原创代码，请勿翻卖文章目录
程序简介
本代码围绕多无人机三维编队轨迹规划与避障仿真展开，构建了一个包含球形障碍物和长方体障碍物的三维复杂飞行环境。程序采用 RRT* 算法完成领航无人机的全局路径搜索，并通过路径平滑处理生成更加连续、自然的飞行轨迹，使规划结果更适合用于无人机航迹展示、避障验证和编队控制仿真分析。
在完成领航机路径规划后，代码进一步引入多无人机编队保持机制，根据领航机的运动方向生成多架跟随机的三维编队轨迹，实现了从单机路径规划到多机协同飞行的扩展。最终程序输出 RRT* 探索树、原始路径、平滑路径以及多无人机三维编队轨迹图，整体可视化效果直观，适合用于无人机路径规划、三维避障、编队飞行控制等方向的教学演示、科研仿真和工程方案展示。
运行结果
本程序运行后将自动生成两幅三维可视化图像：
图1：RRT 规划结果 *
展示完整的探索树结构、原始折线路径与Catmull-Rom平滑后的路径对比，球形与箱形障碍物以半透明方式渲染，起点（蓝圆）与终点（红三角）清晰标注。
图2：多无人机编队三维轨迹
在图1平滑路径基础上，叠加显示1架领航机与3架跟随机的完整三维飞行轨迹，四机以不同颜色区分，编队队形在全程保持稳定的纵-横-垂偏移关系。
命令行输出：包括障碍物数量、RRT*迭代进度、原始路径节点数与总代价、平滑采样点数等关键信息。
MATLAB源代码
完整代码如下：
matlab
%% 多无人机三维编队轨迹规划仿真
% 算法: RRT* + Catmull-Rom 路径平滑 + Frenet 编队保持
% 作者：matlabfilter
% 2026-05-12
clear; clc; close all;
rng(0);
%% 全局参数
cfg.space_min = [0, 0, 0 ];
cfg.space_max = [50, 50, 30];
cfg.start = [2, 2, 2 ];
cfg.goal = [47, 47, 22];
cfg.max_iter = 4000;
cfg.step_size = 2.5;
cfg.goal_bias = 0.12;
cfg.rewire_r = 6.0;
cfg.goal_tol = 2.5;
cfg.smooth_pts = 300;
cfg.n_followers = 3;
cfg.formation_offsets = [
-4, 3, 0;
-4, -3, 0;
-8, 0, 2;
];
cfg.anim_speed = 3;
cfg.uav_colors = {[0.00 0.45 0.74]; [0.85 0.33 0.10]; [0.47 0.67 0.19]; [0.49 0.18 0.56]};
%% 构建三维障碍物
fprintf('构建三维障碍物环境...\n');
obs = struct('type',{},'center',{},'radius',{},'half_sz',{});
idx = 0;
spheres = [10,8,10,4.0; 20,15,8,3.5; 15,30,12,4.5; 30,10,18,3.0; 25,35,6,4.0; 35,20,15,3.5; 40,30,20,4.0; 18,22,20,3.0; 42,12,8,3.5; 28,42,18,4.0];
for i = 1:size(spheres,1)
idx = idx+1; obs(idx).type='sphere'; obs(idx).center=spheres(i,1:3); obs(idx).radius=spheres(i,4); obs(idx).half_sz=[];
end如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者