【MATLAB例程】三维RRT+APF路径规划与AOA-TDOA融合定位算法。附代码的下载链接

完整代码，附链接。有中文注释，欢迎交流

程序简介

概览

本程序实现三维快速扩展随机树（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 在三维连续空间中完成全局采样搜索，得到一条可行的避障路径；随后对这条路径进行稠密化，并基于人工势场法在每个中间路径点上叠加：

1. 邻接路径点形成的平滑力；
2. 指向终点的吸引力；
3. 来自长方体障碍物的斥力。

APF 更新只在前后路径段仍然无碰撞时才会被接受，因此既保留了 RRT 的全局可达性，也改善了原始采样路径的锯齿和突变。

量测模型

AOA-TDOA 融合定位部分与原版本保持一致。程序将 AOA 角度残差与 TDOA 距离差残差拼接为同一个最小二乘问题，再用 Gauss-Newton 迭代完成联合定位。

运行结果

路径规划结果图：

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

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

定位误差曲线：

命令行会输出路径长度、路径点数、规划迭代次数、平均定位误差、最大定位误差、最小定位误差和 RMSE 等统计结果。

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.youkuaiyun.com/download/callmeup/93461979

如需帮助，或有导航、定位滤波相关的代码定制需求，可联系我