【MATLAB程序】二维A*+APF路径规划与TOA-AOA-TDOA融合定位仿真，附代码下载链接

原创代码，请勿翻卖。包运行成功，不包讲解，可联系我获取代码定制、讲解等

程序简介

程序为二维 A*+APF 路径规划与 TOA-AOA-TDOA 融合定位仿真。程序先在含障碍物的二维地图中利用 A* 算法生成全局避障路径，并通过人工势场法对路径进行平滑优化；随后，以规划路径点作为真实轨迹，模拟 TOA、AOA 和 TDOA 多源量测，并采用 Gauss-Newton 迭代方法完成目标位置估计。程序最终绘制规划轨迹、定位轨迹、坐标分量和定位误差曲线，并输出路径长度、定位误差和 RMSE 等统计结果，可用于路径规划与多源融合定位算法验证。

运行结果

路径规划结果图：

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

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

定位误差曲线：

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

MATLAB源代码

部分代码如下：

%% 二维A*+APF路径规划与TOA-AOA-TDOA融合定位算法
% 作者V: matlabfilter（代码获取、定制、讲解）
% 2026-06-14/Ver2
clear;clc;close all;