【基于ROS的A*算法实现路径规划】A* | ROS | 路径规划

【基于ROS的A算法实现路径规划】A | ROS | 路径规划 | Python

编程语言 2025-04-09 23:27:24 阅读次数: 0

### 记录一下使用Python实现ROS平台A*算法路径规划 ###

代码可自取：Xz/little_projecthttps://gitee.com/Xz_zh/little_project.git

一、思路分析

二、算法实现

三、路径规划实现

一、算法介绍

关于A*算法，这里给出一个实例进行分析。首先定义一些变量便于解释A*算法。

(xs ,ys)	起点
(xd ,yd)	终点
(xi ,yi)	查询的第i个点
hx，hy	曼哈顿距离参数

如上图所示，现在有一任务：需要找到从起点(xs ,ys)到终点(xd ,yd)的最短路径（黑色部分为障碍物）。首先根据A*算法的代价函数：

$f(n)=g(n)+h(n)$

从起点出发向四周扩散，记录每个点的位置及代价，每次从代价最小的点进行下一次查找。如图，此时起点的代价f(0) = g(0)+h(0)，由于是起点，可定义g(0)=0，h(0)=hx+hy(曼哈顿距离)，即f(0) = 3+4=7。接下来搜索下一节点。

此时计算第一个点的代价，g(1) = g(0) +1=1,h(1) = hx + hy =2 + 4 =6即f(1) = 1 + 6 =7。依此类推，会发现f(2) = 7=f(1)，搜索的第一个点和第二个点的代价相同，会继续进行搜索。

在此规则上，你会发现从起点到终点会有很多最短路径，这是由于巡点的规则和障碍物分布所致，导致这个最短路径有多条。

再看下一个例子：

在上一条件规则下，修改障碍分布，同时规定搜索的点也能斜着进行搜索，如图所示：

通过计算，不难得出g(3) = 1 ，h(3) = 2+3=5，f(3) = 6。其中f(3)>f(1)=f(2)，随后就会选择第三个点继续搜索，不断迭代上诉过程，直至到达终点，即找到最短路径，中间过程不在赘述。

二、思路分析

要求使用A*算法实现路径规划，可以将该任务分为三大部分内容。首先需要对输入的图片和起点终点进行处理，其次就是通过A*算法输出规划的路径，最后再对路径进行处理并实现可视化。

其中第一部分需要对输入的地图进行处理，将世界地图转化为栅格地图，如果是彩色地图还需要进行灰度处理并进行二值化处理（判断障碍点），存入相关信息。第二部分A*算法通过代价函数，遍历从起点到终点的路径点并找到f值最小的路径。最后需要将A*算法返回的栅格路径转化为世界地图坐标，再进行可视化。

伪代码：

# 初始化 ROS 节点
初始化 ROS 节点 "astar_planner"
订阅 "/map" 以获取 OccupancyGrid 地图
订阅 "/move_base_simple/goal" 以获取 Rviz 选取的目标点
发布 "/astar_path" 以发布路径
 
# 处理地图数据
函数 map_callback(msg):
    解析地图信息（宽度、高度、分辨率、原点）
    转换为二值栅格地图（障碍物=1, 可通行=0）
 
# 世界坐标 <-> 栅格坐标转换
函数 world_to_map(x, y):
    计算栅格坐标并限制范围
 
函数 map_to_world(map_x, map_y):
    计算世界坐标
 
# 处理选取的目标点
函数 clicked_point_callback(msg):
    解析 Rviz 选取的点，转换为栅格坐标
    如果没有起点，设置为起点
    如果已经有起点，设置终点并运行 A* 规划
    调用 publish_path() 发送路径
 
# A* 算法
函数 astar(grid_map, start, end):
    初始化 开放列表 open_list，关闭集合 closed_set
    创建起点、终点节点
    while open_list 不是空:
        取出 f 最小的节点 current_node
        如果 current_node == 终点:
            回溯生成路径
            返回路径
        遍历当前节点的四个邻居:
            如果邻居是障碍物或已在 closed_set，跳过
            计算 g, h, f
            如果 f 值比 open_list 中的同位置节点更优，加入 open_list
    返回 None（无可行路径）
 
# 发布路径
函数 publish_path(path):
    构建 Path 消息
    依次添加路径点，转换回世界坐标
    发布 "/astar_path"