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

关注

原创最新推荐文章于 2026-11-07 19:20:20 发布 253 阅读 AI 写同款· GEO 优化 ›

5个4000比特大状态置换算法的设计与分析下面是 5 个概念性、实验性的 4000 位可逆置换设计。它们都保证是双射，但未经过密码分析，不可直接用于生产系统。风格分别覆盖：三维 SPN、Feistel、可逆元胞自动机、ARX、宽轨迹/Keccak 式结构。这 5 个设计分别代表了 SPN、Feistel、元胞自动机、ARX、宽轨迹 Keccak 五种不同风格，覆盖了主流大状态置换的构造思路。再次强调：它们只是实验性概念，若要用于实际协议，必须经过严格安全性分析。阅读详情

在这里插入图片描述

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

程序详解

概览

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

路径规划模型

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

量测模型

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

运行结果

路径规划结果图：
在这里插入图片描述

路径规划轨迹与定位估计轨迹对比图：
在这里插入图片描述

各坐标分量随路径点序号变化曲线：
在这里插入图片描述

定位误差曲线：
在这里插入图片描述

命令行会输出的结果：
在这里插入图片描述

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;