`完整代码，附下载链接。有中文注释，包运行成功
文章目录
程序简介
程序实现三维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源代码
部分代码：
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等效测距噪声，单位m
sigmaAngle = 0.010; % AOA角度噪声，单位rad
sigmaTdoaRange = 0.45; % TDOA距离差噪声，单位m
maxGnIter = 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 = [0 200 0 200 0 200];
startPos = [10 10 10];
goalPos = [180 180 180];
gridRes = 10;
% 障碍物：[x y z width height depth]
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];
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([gridSize 3]);
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);
if closedMap(ci(1), ci(2), ci(3))
continue;
end
closedMap(ci(1), ci(2), ci(3)) = true;
if isequal(ci, goalIdx)
foundPath = true;
break;
end
curXYZ = gridIndexToXyz(ci, mapLimit, gridRes);
for k = 1:size(moves, 1)
ni = ci + moves(k, :);
if any(ni < 1) || any(ni > gridSize)
continue;
end
if occupied(ni(1), ni(2), ni(3)) || closedMap(ni(1), ni(2), ni(3))
continue;
end
newXYZ = gridIndexToXyz(ni, mapLimit, gridRes);
if checkCollision3D(curXYZ, newXYZ, obstacles)
continue;
end
moveCost = norm((ni - ci) * gridRes);
newG = gMap(ci(1), ci(2), ci(3)) + moveCost;
if newG < 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>
end
end
end
if ~foundPath
error('三维A*未找到可行路径，请调整gridRes、障碍物或起终点。');
end
pathIdx = goalIdx;
ci = goalIdx;
while ~isequal(ci, startIdx)
pi = squeeze(parentMap(ci(1), ci(2), ci(3), :))';
if all(pi == 0)
error('路径回溯失败，父节点为空。');
end
pathIdx = [pi; pathIdx]; %#ok<AGROW>
ci = pi;
end
rawPath = zeros(size(pathIdx, 1), 3);
for k = 1:size(pathIdx, 1)
rawPath(k, :) = gridIndexToXyz(pathIdx(k, :), mapLimit, gridRes);
end
rawPath(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*';
end
function idx = 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, [1 1 1]), gridSize);
end
%% 更多函数完整代码：
https://download.csdn.net/download/callmeup/93461820
如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我