MATLAB源代码，可直接运行、得到结果，有中文注释
文章目录
程序概览
本程序实现三维快速扩展随机树（Rapidly-exploring Random Tree, RRT）与人工势场法（Artificial Potential Field, APF）联合路径规划，并结合到达角（Angle of Arrival, AOA）完成三维定位。RRT用于生成全局可行路径，APF对路径进行局部平滑与避障优化，随后利用多个锚节点的方位角和俯仰角信息估计目标三维位置。地图范围、障碍物、起终点、锚节点位置、RRT/APF规划参数及AOA量测噪声等均可自行修改，便于构建不同三维仿真场景。
运行结果
路径规划结果图：
路径规划轨迹与定位估计轨迹对比图：
各坐标分量随路径点序号变化曲线：
定位误差曲线：
命令行会输出统计结果：
MATLAB部分源代码
部分代码如下：
matlab
%% 三维RRT+APF路径规划与AOA定位算法
% 作者: matlabfilter
% 2026-09-19/Ver2
%
% 程序流程：
% 1. 完成RRT+APF全局采样与APF局部势场细化三维路径规划，将规划路径点作为运动轨迹真值；
% 2. 在每个路径点处模拟AOA量测；
% 3. 使用本脚本对应的定位模型估计路径点位置；
% 4. 使用普通figure窗口绘制规划轨迹、定位轨迹、坐标分量和误差曲线。
clear; clc; close all;
rng(0);
%% 参数设置
algorithmName = '三维RRT+APF路径规划与AOA定位算法';
measureName = 'AOA';
sigmaAngle = 0.010; % AOA角度噪声，单位rad
%% 路径规划
[rawPath, anchors, mapLimit, obstacles, planStats] = planRrtApf3D();
%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runAoaLocalization3D(rawPath, anchors, sigmaAngle);
%% 结果绘图与输出可前往付费专栏查看更多代码和介绍：
https://blog.csdn.net/callmeup/article/details/166008375?spm=1011.2415.3001.5331