【MATLAB例程】三维RRT+APF避障路径规划与到达角（AOA）定位算法｜三维路径优化与定位仿真例程

Evand J2026-10-04 23:49

MATLAB源代码，可直接运行、得到结果，有中文注释

文章目录

程序概览
运行结果
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