集成了路径规划与无线定位的无人机导航仿真平台。采用RRT（Rapidly-exploring Random Tree）算法实现三维空间的自主路径规划，并通过TOA（Time of Arrival）定位技术对规划轨迹进行定位精度验证，形成从路径生成到定位评估的完整闭环仿真系统。
完整代码分享，可用于研究、讨论，请勿翻卖
文章目录
程序介绍
技术架构
输入层：环境参数、算法配置、定位参数
↓
规划层：RRT采样扩展、碰撞检测、路径优化
↓
定位层：TOA测距、最小二乘定位、误差分析
↓
输出层：三维可视化、性能指标、统计报告核心功能模块
- 三维RRT路径规划模块
功能描述：在已知障碍物分布的三维空间中，基于随机采样策略生成从起点到终点的可行路径。
技术特性：
- 采样策略：混合随机采样与目标偏向采样（Bias Sampling）
- 扩展机制：固定步长扩展，保证轨迹平滑性
- 碰撞检测：基于线性插值的多点采样检测算法
- 终止条件：基于欧氏距离的目标到达判定
算法参数：
| 参数 | 数值 | 说明 |
|---|---|---|
| 工作空间 | 200×200×200 m³ | 三维欧氏空间 |
| 步长 | 3 m | 扩展时的固定距离 |
| 目标阈值 | 2 m | 到达判定的距离容限 |
| 最大迭代 | 15000 | 防止算法不收敛 |
| 目标概率 | 0.1 | 偏向采样的概率权重 |
- TOA无线定位模块
基于多基站到达时间差测量，采用最小二乘法求解三维空间位置。基站（锚点）的数量、具体位置等均可在代码中修改。
定位算法：
- 采用伪逆矩阵求解超定线性方程组
- 通过多余观测提高定位鲁棒性
- 对每个路径点独立进行位置估计
- 性能评估与可视化模块
实时监控指标：
- 迭代次数与节点增长速率
- 当前树结构到目标的最小距离
- 算法收敛特性
离线分析指标：
- 路径长度（米）
- 路径点数量
- 定位误差统计（均值、最大值、最小值、RMSE）
- 三轴坐标偏差分析
定位参数设置
时间噪声（range_err）：
- 当前值：1e-9秒（对应0.3米距离误差）
- 实际系统：1e-8至1e-7秒更符合工程实际
- 影响：直接决定定位精度
锚节点配置：
- 最少需求：4个（三维定位的理论最小值）
- 当前配置：6个（超定系统，提高鲁棒性）
- 部署原则：尽量包围目标区域，避免共面
运行结果
路径规划结果：
规划的路径真值与定位结果：
定位误差曲线：
定位误差的统计特性：
MATLAB源代码
部分代码如下：
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.csdn.net/download/callmeup/92733431
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者