`完整代码，附下载链接。有中文注释，包运行成功
文章目录
- 程序简介
- 三维A*路径规划
- TOA量测
- AOA量测
- TDOA量测
- 融合定位
- 运行结果
- MATLAB源代码
程序简介
程序实现三维A*避障路径规划与到达时间（Time of Arrival, TOA）、到达角（Angle of Arrival, AOA）和到达时间差（Time Difference of Arrival, TDOA）融合定位，并对三维轨迹及定位误差进行分析。地图范围、障碍物、起终点、锚节点位置、栅格分辨率及各类量测噪声等参数均可自行修改，便于构建不同三维仿真场景。
三维A*路径规划
程序首先将三维空间划分为规则栅格，并采用A*算法从起点搜索到终点。搜索过程中，每个节点最多可以向周围26个方向扩展，同时综合考虑已经走过的路径长度以及当前位置到终点的距离。
在节点扩展时，程序会判断新节点和连接路径是否穿过三维长方体障碍物，从而保证最终得到一条能够绕开障碍物的三维可行路径。
TOA量测
到达时间（Time of Arrival, TOA）主要提供目标与各个锚节点之间的距离信息。程序根据目标真实位置模拟TOA量测，并加入一定的测距噪声，用于后续融合定位。
AOA量测
到达角（Angle of Arrival, AOA）主要提供目标相对于锚节点的方向信息。三维情况下同时使用方位角和俯仰角，因此可以从不同方向对目标位置进行约束。
程序还对角度误差进行了处理，避免角度跨越正负180°时出现数值跳变。
TDOA量测
到达时间差（Time Difference of Arrival, TDOA）使用一个锚节点作为参考，通过比较目标到不同锚节点之间的距离差来提供定位信息。
TOA提供距离约束，AOA提供方向约束，TDOA提供距离差约束，三种信息相互补充，可以提高三维定位的稳定性。
融合定位
程序将TOA、AOA和TDOA三类量测误差统一处理，并采用带阻尼的Gauss-Newton迭代方法不断修正目标位置，直到得到较稳定的三维位置估计结果。
整个程序中的地图范围、障碍物、起终点、锚节点位置、栅格大小以及各类量测噪声均可以自行修改，方便测试不同场景下的路径规划和融合定位效果。
运行结果
三维A*路径规划结果图：
路径规划轨迹与TOA-AOA-TDOA融合定位轨迹对比图：
三轴坐标分量对比曲线：
定位误差曲线：
命令行会输出规划算法、定位量测类型、路径长度、路径点数、规划迭代次数、规划节点数、平均定位误差、最大定位误差、最小定位误差、RMSE和平均GN迭代次数等统计结果。
MATLAB源代码
部分代码：
%% 三维A*路径规划与TOA-AOA-TDOA融合定位算法% 作者: matlabfilter（V同号，可接代码定制、讲解）% 2026-09-17/Ver1%% 程序流程：% 1. 使用三维体素A*完成无人机避障路径规划；% 2. 将规划路径点作为真实轨迹，模拟TOA + AOA + TDOA量测；% 3. 采用阻尼Gauss-Newton最小二乘进行多源融合定位；% 4. 使用普通figure窗口绘制路径规划、定位轨迹、三轴坐标和误差曲线。clear;clc;close all;rng(0);%% 参数设置algorithmName='三维A*路径规划与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]=planAstar3D();%% 沿规划轨迹进行定位仿真[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]=planAstar3D()mapLimit=[020002000200];startPos=[101010];goalPos=[180180180];gridRes=10;% 障碍物：[x y z width height depth]obstacles=[30020158040;604010158060;1008050304040;14012080255035;80120100353030];anchors=[000;20000;02000;2002000;00200;2000200;0200200;200200200;1001000;100100200];gridSize=[round((mapLimit(2)-mapLimit(1))/gridRes)+1,...round((mapLimit(4)-mapLimit(3))/gridRes)+1,...round((mapLimit(6)-mapLimit(5))/gridRes)+1];startIdx=xyzToGridIndex(startPos,mapLimit,gridRes,gridSize);goalIdx=xyzToGridIndex(goalPos,mapLimit,gridRes,gridSize);occupied=buildOccupancyGrid3D(gridSize,mapLimit,gridRes,obstacles);occupied(startIdx(1),startIdx(2),startIdx(3))=false;occupied(goalIdx(1),goalIdx(2),goalIdx(3))=false;closedMap=false(gridSize);gMap=inf(gridSize);parentMap=zeros([gridSize3]);gMap(startIdx(1),startIdx(2),startIdx(3))=0;openList=[startIdx,0,heuristic3D(startIdx,goalIdx,gridRes)];moves=neighborMoves3D();foundPath=false;iterCount=0;while~isempty(openList)iterCount=iterCount+1;[~,minId]=min(openList(:,5));cur=openList(minId,:);openList(minId,:)=[];ci=cur(1:3);ifclosedMap(ci(1),ci(2),ci(3))continue;endclosedMap(ci(1),ci(2),ci(3))=true;ifisequal(ci,goalIdx)foundPath=true;break;endcurXYZ=gridIndexToXyz(ci,mapLimit,gridRes);fork=1:size(moves,1)ni=ci+moves(k,:);ifany(ni<1)||any(ni>gridSize)continue;endifoccupied(ni(1),ni(2),ni(3))||closedMap(ni(1),ni(2),ni(3))continue;endnewXYZ=gridIndexToXyz(ni,mapLimit,gridRes);ifcheckCollision3D(curXYZ,newXYZ,obstacles)continue;endmoveCost=norm((ni-ci)*gridRes);newG=gMap(ci(1),ci(2),ci(3))+moveCost;ifnewG<gMap(ni(1),ni(2),ni(3))gMap(ni(1),ni(2),ni(3))=newG;parentMap(ni(1),ni(2),ni(3),:)=ci;newF=newG+heuristic3D(ni,goalIdx,gridRes);openList(end+1,:)=[ni,newG,newF];%#ok<AGROW>endendendif~foundPatherror('三维A*未找到可行路径，请调整gridRes、障碍物或起终点。');endpathIdx=goalIdx;ci=goalIdx;while~isequal(ci,startIdx)pi=squeeze(parentMap(ci(1),ci(2),ci(3),:))';ifall(pi==0)error('路径回溯失败，父节点为空。');endpathIdx=[pi;pathIdx];%#ok<AGROW>ci=pi;endrawPath=zeros(size(pathIdx,1),3);fork=1:size(pathIdx,1)rawPath(k,:)=gridIndexToXyz(pathIdx(k,:),mapLimit,gridRes);endrawPath(1,:)=startPos;rawPath(end,:)=goalPos;rawPath=densifyPath(rawPath,4);stats.iter=iterCount;stats.nodeCount=nnz(isfinite(gMap));stats.length=pathLength(rawPath);stats.planner='3D A*';endfunctionidx=xyzToGridIndex(position,mapLimit,gridRes,gridSize)idx=round([(position(1)-mapLimit(1))/gridRes,...(position(2)-mapLimit(3))/gridRes,...(position(3)-mapLimit(5))/gridRes])+1;idx=min(max(idx,[111]),gridSize);end%% 更多函数完整代码：
https://download.csdn.net/download/callmeup/93461820
如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我
