完整代码附下载链接。有中文注释包运行成功文章目录程序简介三维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融合定位算法% 作者: matlabfilterV同号可接代码定制、讲解% 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融合定位算法;measureNameTOA AOA TDOA;sigmaToaRange0.55;% TOA等效测距噪声单位msigmaAngle0.010;% AOA角度噪声单位radsigmaTdoaRange0.45;% TDOA距离差噪声单位mmaxGnIter14;% 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];gridRes10;% 障碍物[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];startIdxxyzToGridIndex(startPos,mapLimit,gridRes,gridSize);goalIdxxyzToGridIndex(goalPos,mapLimit,gridRes,gridSize);occupiedbuildOccupancyGrid3D(gridSize,mapLimit,gridRes,obstacles);occupied(startIdx(1),startIdx(2),startIdx(3))false;occupied(goalIdx(1),goalIdx(2),goalIdx(3))false;closedMapfalse(gridSize);gMapinf(gridSize);parentMapzeros([gridSize3]);gMap(startIdx(1),startIdx(2),startIdx(3))0;openList[startIdx,0,heuristic3D(startIdx,goalIdx,gridRes)];movesneighborMoves3D();foundPathfalse;iterCount0;while~isempty(openList)iterCountiterCount1;[~,minId]min(openList(:,5));curopenList(minId,:);openList(minId,:)[];cicur(1:3);ifclosedMap(ci(1),ci(2),ci(3))continue;endclosedMap(ci(1),ci(2),ci(3))true;ifisequal(ci,goalIdx)foundPathtrue;break;endcurXYZgridIndexToXyz(ci,mapLimit,gridRes);fork1:size(moves,1)nicimoves(k,:);ifany(ni1)||any(nigridSize)continue;endifoccupied(ni(1),ni(2),ni(3))||closedMap(ni(1),ni(2),ni(3))continue;endnewXYZgridIndexToXyz(ni,mapLimit,gridRes);ifcheckCollision3D(curXYZ,newXYZ,obstacles)continue;endmoveCostnorm((ni-ci)*gridRes);newGgMap(ci(1),ci(2),ci(3))moveCost;ifnewGgMap(ni(1),ni(2),ni(3))gMap(ni(1),ni(2),ni(3))newG;parentMap(ni(1),ni(2),ni(3),:)ci;newFnewGheuristic3D(ni,goalIdx,gridRes);openList(end1,:)[ni,newG,newF];%#okAGROWendendendif~foundPatherror(三维A*未找到可行路径请调整gridRes、障碍物或起终点。);endpathIdxgoalIdx;cigoalIdx;while~isequal(ci,startIdx)pisqueeze(parentMap(ci(1),ci(2),ci(3),:));ifall(pi0)error(路径回溯失败父节点为空。);endpathIdx[pi;pathIdx];%#okAGROWcipi;endrawPathzeros(size(pathIdx,1),3);fork1:size(pathIdx,1)rawPath(k,:)gridIndexToXyz(pathIdx(k,:),mapLimit,gridRes);endrawPath(1,:)startPos;rawPath(end,:)goalPos;rawPathdensifyPath(rawPath,4);stats.iteriterCount;stats.nodeCountnnz(isfinite(gMap));stats.lengthpathLength(rawPath);stats.planner3D A*;endfunctionidxxyzToGridIndex(position,mapLimit,gridRes,gridSize)idxround([(position(1)-mapLimit(1))/gridRes,...(position(2)-mapLimit(3))/gridRes,...(position(3)-mapLimit(5))/gridRes])1;idxmin(max(idx,[111]),gridSize);end%% 更多函数完整代码https://download.csdn.net/download/callmeup/93461820如需帮助或有导航、定位滤波相关的代码定制需求可从个人主页左侧联系我
完整代码附下载链接。有中文注释包运行成功文章目录程序简介三维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融合定位算法% 作者: matlabfilterV同号可接代码定制、讲解% 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融合定位算法;measureNameTOA AOA TDOA;sigmaToaRange0.55;% TOA等效测距噪声单位msigmaAngle0.010;% AOA角度噪声单位radsigmaTdoaRange0.45;% TDOA距离差噪声单位mmaxGnIter14;% 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];gridRes10;% 障碍物[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];startIdxxyzToGridIndex(startPos,mapLimit,gridRes,gridSize);goalIdxxyzToGridIndex(goalPos,mapLimit,gridRes,gridSize);occupiedbuildOccupancyGrid3D(gridSize,mapLimit,gridRes,obstacles);occupied(startIdx(1),startIdx(2),startIdx(3))false;occupied(goalIdx(1),goalIdx(2),goalIdx(3))false;closedMapfalse(gridSize);gMapinf(gridSize);parentMapzeros([gridSize3]);gMap(startIdx(1),startIdx(2),startIdx(3))0;openList[startIdx,0,heuristic3D(startIdx,goalIdx,gridRes)];movesneighborMoves3D();foundPathfalse;iterCount0;while~isempty(openList)iterCountiterCount1;[~,minId]min(openList(:,5));curopenList(minId,:);openList(minId,:)[];cicur(1:3);ifclosedMap(ci(1),ci(2),ci(3))continue;endclosedMap(ci(1),ci(2),ci(3))true;ifisequal(ci,goalIdx)foundPathtrue;break;endcurXYZgridIndexToXyz(ci,mapLimit,gridRes);fork1:size(moves,1)nicimoves(k,:);ifany(ni1)||any(nigridSize)continue;endifoccupied(ni(1),ni(2),ni(3))||closedMap(ni(1),ni(2),ni(3))continue;endnewXYZgridIndexToXyz(ni,mapLimit,gridRes);ifcheckCollision3D(curXYZ,newXYZ,obstacles)continue;endmoveCostnorm((ni-ci)*gridRes);newGgMap(ci(1),ci(2),ci(3))moveCost;ifnewGgMap(ni(1),ni(2),ni(3))gMap(ni(1),ni(2),ni(3))newG;parentMap(ni(1),ni(2),ni(3),:)ci;newFnewGheuristic3D(ni,goalIdx,gridRes);openList(end1,:)[ni,newG,newF];%#okAGROWendendendif~foundPatherror(三维A*未找到可行路径请调整gridRes、障碍物或起终点。);endpathIdxgoalIdx;cigoalIdx;while~isequal(ci,startIdx)pisqueeze(parentMap(ci(1),ci(2),ci(3),:));ifall(pi0)error(路径回溯失败父节点为空。);endpathIdx[pi;pathIdx];%#okAGROWcipi;endrawPathzeros(size(pathIdx,1),3);fork1:size(pathIdx,1)rawPath(k,:)gridIndexToXyz(pathIdx(k,:),mapLimit,gridRes);endrawPath(1,:)startPos;rawPath(end,:)goalPos;rawPathdensifyPath(rawPath,4);stats.iteriterCount;stats.nodeCountnnz(isfinite(gMap));stats.lengthpathLength(rawPath);stats.planner3D A*;endfunctionidxxyzToGridIndex(position,mapLimit,gridRes,gridSize)idxround([(position(1)-mapLimit(1))/gridRes,...(position(2)-mapLimit(3))/gridRes,...(position(3)-mapLimit(5))/gridRes])1;idxmin(max(idx,[111]),gridSize);end%% 更多函数完整代码https://download.csdn.net/download/callmeup/93461820如需帮助或有导航、定位滤波相关的代码定制需求可从个人主页左侧联系我
