【MATLAB例程】二维A*（A star）+APF（人工势场）路径规划与AOA定位算法|基于A*与人工势场融合路径规划的二维AOA定位仿真

原创代码，包运行成功

程序简介

二维 A*+APF 路径规划与 AOA 定位联合仿真程序。整体流程是先在二维栅格地图中设置起点、终点、矩形障碍物和锚节点，然后利用 A* 算法生成初始避障路径，再通过人工势场法对路径进行平滑和局部优化，最终将优化后的路径点作为运动轨迹真值，在各路径点处模拟 AOA 方位角量测，并利用多锚节点角度约束完成二维位置估计。

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

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

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

运行结果

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

路径规划结果图：

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

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

定位误差曲线：

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

MATLAB源代码

部分代码如下：

%% 二维A*+APF路径规划与AOA定位算法
% 作者: matlabfilter（V同号，例程获取、代码定制、讲解）
% 2026-06-11/Ver2
%
% 程序流程：
%   1. 完成A*+APF二维栅格路径规划，将规划路径点作为运动轨迹真值；
%   2. 在每个路径点处模拟AOA量测；
%   3. 使用本脚本对应的定位模型估计路径点位置；
%   4. 使用普通figure窗口绘制规划轨迹、定位轨迹、坐标分量和误差曲线。

clear; clc; close all;
rng(0);

%% 参数设置
algorithmName = '二维A*+APF路径规划与AOA定位算法';
measureName = 'AOA';
sigmaAngle = 0.012;        % AOA角度噪声，单位rad

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

%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runAoaLocalization2D(rawPath, anchors, sigmaAngle);

%% 结果绘图与输出
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);

%% 本地函数

完整代码也可通过下方链接获取：
https://download.csdn.net/download/callmeup/92977662

扩展方向

1. 扩展到三维路径规划与三维 AOA 定位
2. 加入动态障碍物与实时重规划机制
3. 优化用于定位的锚节点布局与定位精度评估

如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我