当前位置：服务支持 > 软件文章 > 二维A星加APF路径规划与AOA-TDOA融合定位源码

二维A星加APF路径规划与AOA-TDOA融合定位源码

阅读数 7

文章目录

程序简介概览路径规划模型定位模型
运行结果真实截图
MATLAB源代码
可扩展方向

程序简介

概览

本程序是路径规划与定位代码。在路径规划端引入 APF 细化，以便在保持可达性的同时优化轨迹形状。完成A*+APF二维栅格路径规划后，将规划路径点作为运动轨迹真值，在每个路径点处模拟AOA + TDOA定位，最终输出定位结果。

路径规划模型

本程序采用 A* 与 APF 串联的混合规划结构。首先利用 A* 在二维栅格地图上完成全局搜索，得到一条严格避障的引导路径；随后对引导路径进行稠密化，并基于人工势场法引入三类局部作用力：

相邻路径点之间的平滑力，用于减弱折线感；
指向终点的吸引力，用于保持整体前进方向；
障碍物附近的斥力，用于主动远离矩形障碍物边界。

每次 APF 修正都会重新检查与前后路径段的碰撞关系，仅接受仍然满足避障约束的更新，因此可以在不破坏全局可达性的前提下优化路径形状。

定位模型

AOA-TDOA 融合定位部分与原版本保持一致。程序将 AOA 角度残差与 TDOA 距离差残差拼接为同一个最小二乘问题，再用 Gauss-Newton 迭代完成联合定位。

运行结果

运行程序，会完成二维A*+APF路径规划与AOA-TDOA融合定位算法仿真。程序先在二维平面上生成避障路径，再沿路径点执行 AOA + TDOA 定位，并输出轨迹、坐标分量和定位误差统计。

真实截图

程序运行后会通过普通 figure; 命令生成图像窗口，主要包括：

路径规划结果图：

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

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

定位误差曲线：

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

MATLAB源代码

部分代码如下：

代码块

matlab

自动换行

复制代码

123456789101112131415

%% 二维A*+APF路径规划与AOA-TDOA融合定位算法% 作者: matlabfilter（V同号，可联系获取例程，代码定制、讲解）% 2026-06-11/Ver2
clear; clc; close all;rng(0);

%% 参数设置
algorithmName = '二维A*+APF路径规划与AOA-TDOA融合定位算法';
measureName = 'AOA + TDOA';
sigmaAngle = 0.012;        % AOA角度噪声，单位rad
sigmaTdoaRange = 0.45;     % TDOA距离差噪声，单位m
maxGnIter = 14;            % Gauss-Newton最大迭代次数

%% 路径规划[rawPath, anchors, mapLimit, obstacles, planStats] = planAstarApf2D();

%% 沿规划轨迹进行定位仿真[estPath, posErr, iterUsed] = runAoaTdoaLocalization2D(rawPath, anchors, 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);

      复制成功

完整代码获取：