完整代码，附链接。有中文注释，欢迎交流
文章目录
程序简介
概览
本程序实现三维快速扩展随机树（Rapidly-exploring Random Tree, RRT）与人工势场法（Artificial Potential Field, APF）联合路径规划，并结合到达角（Angle of Arrival, AOA）和到达时间差（Time Difference of Arrival, TDOA）完成三维融合定位。RRT负责生成全局可行路径，APF进一步对路径进行局部优化和避障细化。地图范围、障碍物、起终点、锚节点位置、RRT/APF规划参数及量测噪声等均可自行修改，便于构建不同三维仿真场景。
路径规划模型
本程序采用 RRT 与 APF 串联的混合规划结构。首先利用 RRT 在三维连续空间中完成全局采样搜索，得到一条可行的避障路径；随后对这条路径进行稠密化，并基于人工势场法在每个中间路径点上叠加：
- 邻接路径点形成的平滑力；
- 指向终点的吸引力；
- 来自长方体障碍物的斥力。
APF 更新只在前后路径段仍然无碰撞时才会被接受，因此既保留了 RRT 的全局可达性，也改善了原始采样路径的锯齿和突变。
量测模型
AOA-TDOA 融合定位部分与原版本保持一致。程序将 AOA 角度残差与 TDOA 距离差残差拼接为同一个最小二乘问题，再用 Gauss-Newton 迭代完成联合定位。
运行结果
路径规划结果图：
路径规划轨迹与定位估计轨迹对比图：
各坐标分量随路径点序号变化曲线：
定位误差曲线：
命令行会输出路径长度、路径点数、规划迭代次数、平均定位误差、最大定位误差、最小定位误差和 RMSE 等统计结果。
MATLAB源代码
部分代码如下：
matlab
%% 三维RRT+APF路径规划与AOA-TDOA融合定位算法
% 作者: matlabfilter（V同号，可接代码定制、讲解）
% 2026-06-11/Ver2
%
% 程序流程：
% 1. 完成RRT+APF全局采样与APF局部势场细化三维路径规划，将规划路径点作为运动轨迹真值；
% 2. 在每个路径点处模拟AOA + TDOA量测；
% 3. 使用本脚本对应的定位模型估计路径点位置；
% 4. 使用普通figure窗口绘制规划轨迹、定位轨迹、坐标分量和误差曲线。
clear; clc; close all;
rng(0);
%% 参数设置
algorithmName = '三维RRT+APF路径规划与AOA-TDOA融合定位算法';
measureName = 'AOA + TDOA';
sigmaAngle = 0.010; % AOA角度噪声，单位rad
sigmaTdoaRange = 0.45; % TDOA距离差噪声，单位m
maxGnIter = 14; % Gauss-Newton最大迭代次数
%% 路径规划
[rawPath, anchors, mapLimit, obstacles, planStats] = planRrtApf3D();
%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runAoaTdoaLocalization3D(rawPath, anchors, sigmaAngle, sigmaTdoaRange, maxGnIter, mapLimit);
%% 结果绘图与输出
plotPlanningResult(rawPath, anchors, mapLimit, obstacles, algorithmName);
plotLocalizationResult(rawPath, estPath, anchors, obstacles, mapLimit, algorithmName);
plotCoordinateResult(rawPath, estPath, algorithmName);
plotErrorResult(posErr, algorithmName);
printSummary(rawPath, posErr, iterUsed, planStats, algorithmName, measureName);
%% 函数完整代码：
https://download.csdn.net/download/callmeup/93461979
如需帮助，或有导航、定位滤波相关的代码定制需求，可联系我