【三维飞行器】RRT路径规划与TOA定位仿真系统，MATLAB例程，路径起终点、障碍物、TOA锚点等均可设置。附下载链接

集成了路径规划与无线定位的无人机导航仿真平台。采用RRT（Rapidly-exploring Random Tree）算法实现三维空间的自主路径规划，并通过TOA（Time of Arrival）定位技术对规划轨迹进行定位精度验证，形成从路径生成到定位评估的完整闭环仿真系统。
完整代码分享，可用于研究、讨论，请勿翻卖

程序介绍

技术架构

输入层：环境参数、算法配置、定位参数
  ↓
规划层：RRT采样扩展、碰撞检测、路径优化
  ↓
定位层：TOA测距、最小二乘定位、误差分析
  ↓
输出层：三维可视化、性能指标、统计报告

核心功能模块

1. 三维RRT路径规划模块

功能描述：在已知障碍物分布的三维空间中，基于随机采样策略生成从起点到终点的可行路径。

技术特性：

• 采样策略：混合随机采样与目标偏向采样（Bias Sampling）
• 扩展机制：固定步长扩展，保证轨迹平滑性
• 碰撞检测：基于线性插值的多点采样检测算法
• 终止条件：基于欧氏距离的目标到达判定

算法参数：

参数	数值	说明
工作空间	200×200×200 m³	三维欧氏空间
步长	3 m	扩展时的固定距离
目标阈值	2 m	到达判定的距离容限
最大迭代	15000	防止算法不收敛
目标概率	0.1	偏向采样的概率权重

2. TOA无线定位模块

基于多基站到达时间差测量，采用最小二乘法求解三维空间位置。基站（锚点）的数量、具体位置等均可在代码中修改。

定位算法：

• 采用伪逆矩阵求解超定线性方程组
• 通过多余观测提高定位鲁棒性
• 对每个路径点独立进行位置估计

3. 性能评估与可视化模块

实时监控指标：

• 迭代次数与节点增长速率
• 当前树结构到目标的最小距离
• 算法收敛特性

离线分析指标：

• 路径长度（米）
• 路径点数量
• 定位误差统计（均值、最大值、最小值、RMSE）
• 三轴坐标偏差分析

定位参数设置

时间噪声（range_err）：

• 当前值：1e-9秒（对应0.3米距离误差）
• 实际系统：1e-8至1e-7秒更符合工程实际
• 影响：直接决定定位精度

锚节点配置：

• 最少需求：4个（三维定位的理论最小值）
• 当前配置：6个（超定系统，提高鲁棒性）
• 部署原则：尽量包围目标区域，避免共面

运行结果

路径规划结果：

规划的路径真值与定位结果：

定位误差曲线：

定位误差的统计特性：

MATLAB源代码

部分代码如下：

%% RRT路径规划，用于三维空间上的无人机路径规划与避障，含性能分析与可视化
% 作者:matlabfilter
% 2026-03-14/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];
% TOA定位设置
c = 3e8;                % 光速
range_err = 1e-9;       % 时间噪声

% 设置锚节点(放在地图顶部)
anchorNum = 6;
baseP = [
    0   0   200;
    200 0   0;
    0   200 200;
    200 200 0;
    100 0   200;
    0   100 0;
];
%% 数据记录变量
tree.pos = startPos;
tree.parent = 0;
distToGoalIter = [];  % 每步到目标的距离
nodeCountIter = [];   % 节点增长情况


    %% 轨迹对比图
figure
plot3(rawPath(:,1),rawPath(:,2),rawPath(:,3),'b','LineWidth',2);
hold on
plot3(estPath(:,1),estPath(:,2),estPath(:,3),'r--','LineWidth',2);
plot3(baseP(:,1),baseP(:,2),baseP(:,3),'ko','MarkerFaceColor','k');
axis equal
xlabel('X');ylabel('Y');zlabel('Z');
legend('路径规划的轨迹（视为真值）','TOA定位轨迹','锚节点','location','best');
title('UAV轨迹与TOA定位结果')


% 创建一个新的图形窗口
figure;
pathIdx = 1:size(rawPath,1);
subplot(3, 1, 1); % 1行3列，选择第1个
plot(pathIdx, rawPath(:,1), 'r-', 'LineWidth', 1.5);
hold on
plot(estPath(:,1),'b--','LineWidth',2);
legend('路径规划结果','定位结果');
xlabel('路径点序号', 'FontSize', 12);
ylabel('X坐标 (m)', 'FontSize', 12);
grid on;
title('X坐标变化', 'FontSize', 13);
subplot(3, 1, 2); % 1行3列，选择第2个
plot(pathIdx, rawPath(:,2), 'r-', 'LineWidth', 1.5);
hold on
plot(estPath(:,2),'b--','LineWidth',2);
xlabel('路径点序号', 'FontSize', 12);
ylabel('Y坐标 (m)', 'FontSize', 12);
grid on;
title('Y坐标变化', 'FontSize', 13);
subplot(3, 1, 3); % 1行3列，选择第3个
plot(pathIdx, rawPath(:,3), 'r-', 'LineWidth', 1.5);
hold on
plot(estPath(:,3),'b--','LineWidth',2);
xlabel('路径点序号', 'FontSize', 12);
ylabel('Z坐标 (m)', 'FontSize', 12);
grid on;
title('Z坐标变化', 'FontSize', 13);
sgtitle('路径规划与TOA定位的XYZ三轴位移曲线')



%% 定位误差
figure
plot(posErr,'LineWidth',2);
grid on
xlabel('轨迹点序号');
ylabel('定位误差 (m)');
title('TOA定位误差');

fprintf('TOA定位统计\n')
fprintf('平均误差: %.3f m\n',mean(posErr))
fprintf('最大误差: %.3f m\n',max(posErr))
fprintf('最小误差: %.3f m\n',min(posErr))
fprintf('RMSE: %.3f m\n',sqrt(mean(posErr.^2)))

完整代码：https://download.youkuaiyun.com/download/callmeup/92733431

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