原创代码，请勿翻卖
文章目录
- 程序简介
- 路径规划模型
- 量测模型
- 运行结果
- MATLAB源代码
程序简介
本程序实现三维RRT避障路径规划与TOA、AOA、TDOA融合定位，并对三维轨迹及定位误差进行分析。地图范围、三维障碍物、起终点、锚节点位置、RRT规划参数及量测噪声等均可自行修改，便于构建不同三维仿真场景。
路径规划模型
本程序采用RRT算法，在三维连续空间中随机采样、寻找最近树节点，并沿采样方向扩展固定步长。每一条新增线段都会与长方体障碍物做碰撞检测，最终回溯树节点得到三维避障路径。
量测模型
TOA量测提供绝对距离，AOA量测提供方向角，TDOA量测提供相对距离差。三维AOA会同时使用方位角和俯仰角。程序将三类残差按各自噪声标准差归一化后统一迭代求解。
运行结果
运行代码后，程序会完成三维RRT路径规划与TOA-AOA-TDOA多源紧耦合融合定位仿真。程序先在三维空间中生成避障路径，再沿路径点模拟TOA + AOA + TDOA量测，并计算定位估计轨迹和误差统计。
运行结果如下。
路径规划结果图：
路径规划轨迹与定位估计轨迹对比图：
各坐标分量随路径点序号变化曲线：
定位误差曲线：
命令行会输出路径长度、路径点数、规划迭代次数、平均定位误差、最大定位误差、最小定位误差和RMSE等统计结果。
MATLAB源代码
部分代码如下：
%% 三维RRT路径规划与TOA-AOA-TDOA融合定位算法% 作者: matlabfilter% 2026-09-12/Ver2%% 程序流程：% 1. 完成RRT三维采样路径规划，将规划路径点作为运动轨迹真值；% 2. 在每个路径点处模拟TOA + AOA + TDOA量测；% 3. 使用本脚本对应的定位模型估计路径点位置；% 4. 使用普通figure窗口绘制规划轨迹、定位轨迹、坐标分量和误差曲线。clear;clc;close all;rng(0);%% 参数设置algorithmName='三维RRT路径规划与TOA-AOA-TDOA融合定位算法';measureName='TOA + AOA + TDOA';sigmaToaRange=0.55;% TOA等效测距噪声，单位msigmaAngle=0.010;% AOA角度噪声，单位radsigmaTdoaRange=0.45;% TDOA距离差噪声，单位mmaxGnIter=14;% Gauss-Newton最大迭代次数%% 路径规划[rawPath,anchors,mapLimit,obstacles,planStats]=planRrt3D();%% 沿规划轨迹进行定位仿真[estPath,posErr,iterUsed]=runToaAoaTdoaLocalization3D(rawPath,anchors,sigmaToaRange,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);%% 本地函数function[rawPath,anchors,mapLimit,obstacles,stats]=planRrt3D()mapLimit=[020002000200];startPos=[101010];goalPos=[180180180];stepSize=7;goalThreshold=8;maxIter=18000;goalProb=0.18;obstacles=[30020158040;604010158060;1008050304040;14012080255035;80120100353030];anchors=[000;20000;02000;2002000;00200;2000200;0200200;200200200;1001000;100100200];tree.pos=startPos;tree.parent=0;foundPath=false;foriterCount=1:maxIterifrand<goalProb qRand=goalPos;elseqRand=[mapLimit(1)+rand*(mapLimit(2)-mapLimit(1)),...mapLimit(3)+rand*(mapLimit(4)-mapLimit(3)),...mapLimit(5)+rand*(mapLimit(6)-mapLimit(5))];end[~,nearIdx]=min(sqrt(sum((tree.pos-qRand).^2,2)));qNear=tree.pos(nearIdx,:);direction=qRand-qNear;distVal=norm(direction);ifdistVal<epscontinue;endqNew=qNear+stepSize*direction/distVal;ifany(qNew<[mapLimit(1)mapLimit(3)mapLimit(5)])||...any(qNew>[mapLimit(2)mapLimit(4)mapLimit(6)])continue;endifcheckCollision3D(qNear,qNew,obstacles)continue;endtree.pos(end+1,:)=qNew;%#ok<AGROW>tree.parent(end+1)=nearIdx;%#ok<AGROW>ifnorm(qNew-goalPos)<goalThreshold&&~checkCollision3D(qNew,goalPos,obstacles)tree.pos(end+1,:)=goalPos;tree.parent(end+1)=size(tree.pos,1)-1;foundPath=true;break;endendif~foundPatherror('RRT未找到可行路径，请增大maxIter或调整障碍物参数。');end完整代码：
https://download.csdn.net/download/callmeup/93431931
如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我
