MATLAB代码，三维空间上的RRT（快速扩展随机树）路径规划仿真工具，专门用于模拟无人机在复杂三维空间中的避障导航。利用目标偏向采样算法，在障碍物环境中快速生长出一条从起点到终点的可行航迹，并支持搜索过程的实时动态可视化。
原创代码，可直接运行

程序简介

代码为经典的 3D RRT（快速扩展随机树）算法，专门用于无人机在复杂三维空间中的路径规划与避障。
RRT (Rapidly-exploring Random Tree) 是一种基于采样的路径规划算法。它通过在搜索空间内随机采样，像树木生长一样快速扩展节点，从而在充满障碍物的空间中寻找一条可行路径。

核心功能：

• 三维空间建模：不同于平面规划，该算法在 $X,Y,Z$ 三个维度上进行采样和碰撞检测，完美契合无人机的飞行环境。
• 目标偏向采样 (Goal Bias)：通过 goalProb 参数，算法会以一定概率直接向终点方向扩展，极大地提高了搜索效率，避免了盲目搜索。
• 实时动态显示：程序在运行过程中会实时绘制树的生长过程（蓝色细线），用户可以直观看到算法是如何“绕过”障碍物的。
• 多维性能分析：规划完成后，程序会自动生成路径长度、节点增长趋势以及 $X/Y/Z$ 坐标变化曲线，方便进行算法评估。

运行结果

直接运行，可得以下结果。
规划的路线图（3D）：

路径的XYZ三轴坐标：

命令行窗口显示规划结果：

部分程序截图如下，可自由更改地图大小、起终点坐标、障碍物等：

MATLAB源代码

部分代码如下：

%% RRT路径规划，用于三维空间上的无人机路径规划与避障，含性能分析与可视化
% 作者:matlabfilter
% 2026-03-10/Ver1

clear; clc; close all;
rng(0);

%% 参数设置
mapLimit = [0 200 0 200 0 200]; % 地图大小 [xmin xmax ymin ymax zmin zmax]
startPos = [10, 10, 10]; % 起点坐标(x,y,z)
goalPos = [180, 180, 180]; % 终点(目标)的坐标(x,y,z)
stepSize = 3; % 步长
goalThreshold = 2; % 到达目标的阈值
maxIter = 15000; % 最大迭代次数
goalProb = 0.1; % 采样点偏向目标的概率

% 障碍物设置 [x, y, z, width, height, depth]
obstacles = [
    30, 0, 20, 15, 80, 40;
    60, 40, 10, 15, 80, 60;
    100, 80, 50, 30, 40, 40;
    140, 120, 80, 25, 50, 35;
    80, 120, 100, 35, 30, 30];

%% 数据记录变量
tree.pos = startPos;
tree.parent = 0;
distToGoalIter = [];  % 每步到目标的距离
nodeCountIter = [];   % 节点增长情况

%% 【图形布局】三维可视化
figure;
hold on
title('RRT 三维实时规划空间', 'FontSize', 14);
axis equal
xlim(mapLimit(1:2))
ylim(mapLimit(3:4))
zlim(mapLimit(5:6))
grid on
xlabel('X (m)', 'FontSize', 12);
ylabel('Y (m)', 'FontSize', 12);
zlabel('Z (m)', 'FontSize', 12);
view(45, 30); % 设置三维视角

% 绘制障碍物
for i = 1:size(obstacles, 1)
    drawBox(obstacles(i,:), [0.7 0.7 0.7], 0.3);
end

% 绘制起点和终点

完整代码：https://download.csdn.net/download/callmeup/92731399

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