A*算法与人工势场法结合的路径规划(附MATLAB源码)

路径规划666

已于 2025-01-01 14:30:43 修改

阅读量2k

点赞数 27

分类专栏：路径规划文章标签：算法 matlab 开发语言

于 2025-01-01 14:13:14 首次发布

本文链接：https://blog.csdn.net/LJGHZ/article/details/144867160

版权

路径规划专栏收录该内容

12 篇文章

订阅专栏

A*算法与人工势场法（APF）结合实现路径规划

路径规划是机器人、无人机及自动驾驶等领域中的一个重要问题。本文结合了经典的 A* 算法与 人工势场法（Artificial Potential Field, APF），实现了一种改进的路径规划方法。下面将从代码结构、功能实现和关键算法进行详细介绍。

一、背景知识

A*算法：
- A*算法是一种基于图的搜索算法，用于在一个栅格地图中找到从起点到终点的最短路径。
- 通过评价函数 f(n)=g(n)+h(n)f(n) = g(n) + h(n)，其中 g(n)g(n) 是起点到当前节点的实际代价，h(n)h(n) 是当前节点到目标的估计代价（启发函数）。
人工势场法（APF）：
- APF是一种基于力场的路径规划方法，将目标点视为“引力源”、障碍物视为“斥力源”，机器人在引力和斥力的合力作用下移动。
- 优势是计算简单、实时性强，但容易陷入局部最优。

结合A*算法与APF方法，本文代码先通过A*算法找到一条粗略路径，然后利用APF对路径进行优化，进一步避开动态障碍物并生成更平滑的路径。

二、代码功能概述

本文代码主要分为以下几个部分：

地图初始化与绘制：
- 定义一个20*20的栅格地图，加载地图数据并标记障碍物、起点和终点。
A*算法实现：
- 计算起点到终点的初步路径，储存路径点及其代价。
人工势场法路径优化：
- 基于A*生成的路径，结合动态障碍物的位置调整路径。
可视化与性能评估：
- 动态显示路径规划过程，计算路径长度与规划时间。

三、代码结构解析

1. 地图初始化与绘制

m = 20;   % 地图行数
n = 20;   % 地图列数
load MAP; % 加载地图数据

% 起点和终点定义
start_node = [1, 1];
target_node = [20, 20];

% 绘制栅格地图框架
figure('Name', 'A-APF Path Planning', 'Color', 'w');
draw_grid(m, n, start_node, target_node, obs);

代码通过加载 MAP 文件定义障碍物，并使用 fill 函数绘制地图。
起点和终点用红色星标表示，障碍物用红色方块填充。

2. **A*算法实现**

A*算法的核心包括以下步骤：

OpenList 和 CloseList：分别存储待探索节点和已探索节点。
代价计算：
- gg：从起点到当前节点的代价。
- hh：当前节点到目标的曼哈顿距离。
- f=g+hf = g + h：总代价。
路径扩展：
- 按代价从OpenList中选择代价最小的节点进行扩展。
- 判断是否到达终点。

以下是主要代码片段：

while flag   % 循环直到找到目标
    % 计算子节点
    child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList);

    % 更新OpenList
    for i = 1:size(child_nodes,1)
        g = openList_cost(min_idx, 1) + norm(parent_node - child_node);
        h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
        f = g + h;
        % 比较更新代价或添加新节点
    end

    % 更新CloseList
    closeList(end+1,: ) =  openList(min_idx,:);
end

3. 人工势场法路径优化

基于A*生成的初步路径，使用APF对其优化，主要步骤包括：

引力计算：
- 目标点对机器人产生吸引力，吸引力大小与目标距离成正比。
```
F_att = [Eta_att*dist(N,1)*unitVector(N,1), Eta_att*dist(N,1)*unitVector(N,2)];
```

斥力计算：

静态障碍物和动态障碍物产生斥力，斥力随距离增大而减小。

if dist(j,1) >= d0
    F_rep_ob(j,:) = [0,0];
else
    F_rep_ob1_abs = Eta_rep_ob * (1/dist(j,1)-1/d0) * dist(N,1) / dist(j,1)^2;
    F_rep_ob(j,:) = F_rep_ob1;
end

动态障碍物处理：
- 障碍物位置实时更新，基于相对速度计算斥力。

位置更新：

车辆按照总合力方向前进。

F_sum = [F_rep(1,1)+F_att(1,1),F_rep(1,2)+F_att(1,2)];
Pi(1,1:2) = Pi(1,1:2) + len_step * UnitVec_Fsum(i,:);

4. 动态可视化

每次迭代实时绘制路径、障碍物及车辆位置。

rectangle('Position',[Pi(1)-0.5-0.25,Pi(2)-0.5-0.25,0.5,0.5],'Curvature',1,'FaceColor','b');
pause(0.1);
cla;

四、实验结果

通过运行代码，可以观察到以下现象：

A*路径生成：
- 初步路径通过A*生成，绕过了主要的静态障碍物。
APF路径优化：
- 车辆按照人工势场的总力方向调整路径，避开动态障碍物。
路径平滑与动态避障：
- 最终路径更加平滑，车辆成功避开动态障碍物到达目标点。

五、性能分析

路径长度：

代码动态计算路径总长度：

length = length + norm(Path(i+1,:) - Path(i,:));
disp(['The length =',num2str(length)]);

规划时间：
- 总体规划时间由 tic 和 toc 函数统计。
优缺点：
- 优点：结合A*和APF，兼顾全局路径搜索与局部优化。
- 缺点：复杂场景下，动态障碍物处理仍可能陷入局部最优。

六、总结

本文通过将A*算法与人工势场法相结合，解决了路径规划中的全局与局部优化问题。代码不仅实现了基本功能，还加入了动态障碍物处理及路径平滑优化。未来可以尝试改进势场函数或引入机器学习方法进一步提升性能。

代码获取：A*算法与人工势场法（APF）结合实现路径规划A*算法与人工势场法（APF）结合实现路径规划A*算法与人工势场法结合的路径规划(附MATLAB源码)-CSDN博客路径规划是机器人、无人机及自动驾驶等领域中的一个重要问题。本文结合了经典的 A* 算法与人工势场法（Artificial Potential Field, APFhttps://mbd.pub/o/bread/Z52Xk5pw