原创代码，请勿翻卖

文章目录

• 程序简介
• 运行结果
• MATLAB源代码

程序简介

本代码围绕多无人机三维编队轨迹规划与避障仿真展开，构建了一个包含球形障碍物和长方体障碍物的三维复杂飞行环境。程序采用 RRT* 算法完成领航无人机的全局路径搜索，并通过路径平滑处理生成更加连续、自然的飞行轨迹，使规划结果更适合用于无人机航迹展示、避障验证和编队控制仿真分析。

在完成领航机路径规划后，代码进一步引入多无人机编队保持机制，根据领航机的运动方向生成多架跟随机的三维编队轨迹，实现了从单机路径规划到多机协同飞行的扩展。最终程序输出 RRT* 探索树、原始路径、平滑路径以及多无人机三维编队轨迹图，整体可视化效果直观，适合用于无人机路径规划、三维避障、编队飞行控制等方向的教学演示、科研仿真和工程方案展示。

运行结果

本程序运行后将自动生成两幅三维可视化图像：

图1：RRT 规划结果*
展示完整的探索树结构、原始折线路径与Catmull-Rom平滑后的路径对比，球形与箱形障碍物以半透明方式渲染，起点（蓝圆）与终点（红三角）清晰标注。

图2：多无人机编队三维轨迹
在图1平滑路径基础上，叠加显示1架领航机与3架跟随机的完整三维飞行轨迹，四机以不同颜色区分，编队队形在全程保持稳定的纵-横-垂偏移关系。

命令行输出：包括障碍物数量、RRT*迭代进度、原始路径节点数与总代价、平滑采样点数等关键信息。

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

完整代码：https://blog.csdn.net/callmeup/article/details/161015749?fromshare=blogdetail&sharetype=blogdetail&sharerId=161015749&sharerefer=PC&sharesource=callmeup&sharefrom=from_link

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者