【MATLAB例程】三维快速扩展随机树（RRT）路径规划与到达角（AOA）定位算法｜三维避障+定位仿真。附下载链接

原创代码，包运行成功，请勿翻卖

文章目录

程序详解
运行结果
MATLAB源代码

程序详解

概览

程序为三维快速扩展随机树（Rapidly-exploring Random Tree, RRT）避障路径规划与到达角（Angle of Arrival, AOA）定位的仿真，带有对三维规划轨迹、定位轨迹及定位误差进行分析。地图范围、障碍物、起终点、锚节点位置、RRT规划参数及AOA量测噪声等均可自行修改，便于构建不同三维仿真场景。

路径规划模型

本程序采用RRT算法，在三维连续空间中随机采样、寻找最近树节点，并沿采样方向扩展固定步长。每一条新增线段都会与长方体障碍物做碰撞检测，最终回溯树节点得到三维避障路径。

量测模型

AOA量测使用锚节点到目标的方向角。二维情况下使用方位角，三维情况下在方位角基础上增加俯仰角，程序使用AOA线性最小二乘方程直接求解目标位置。

运行结果

路径规划结果图：

路径规划轨迹与定位估计轨迹对比图：

各坐标分量随路径点序号变化曲线：

定位误差曲线：

命令行会输出的结果：

MATLAB源代码

部分代码如下：

matlab 复制代码

%% 三维RRT路径规划与AOA定位算法
% 作者: matlabfilter
% 2026-09-19/Ver2
clear; clc; close all;
rng(0);

%% 参数设置
algorithmName = '三维RRT路径规划与AOA定位算法';
measureName = 'AOA';
sigmaAngle = 0.010;        % AOA角度噪声，单位rad

%% 路径规划
[rawPath, anchors, mapLimit, obstacles, planStats] = planRrt3D();

%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runAoaLocalization3D(rawPath, anchors, sigmaAngle);

%% 结果绘图与输出
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);

%% 本地函数

function [rawPath, anchors, mapLimit, obstacles, stats] = planRrt3D()
mapLimit = [0 200 0 200 0 200];
startPos = [10 10 10];
goalPos = [180 180 180];
stepSize = 7;
goalThreshold = 8;
maxIter = 18000;
goalProb = 0.18;
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];
anchors = [
      0   0   0;
    200   0   0;
      0 200   0;
    200 200   0;
      0   0 200;
    200   0 200;
      0 200 200;
    200 200 200;
    100 100   0;
    100 100 200];

tree.pos = startPos;
tree.parent = 0;
foundPath = false;

完整代码：
https://download.csdn.net/download/callmeup/93471698

如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我