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

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

---

目录

一、思路分析

二、算法实现

三、路径规划实现

---

一、思路分析

        要求使用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"

---

二、算法实现

        A*算法是一种有序的搜索算法，常常用于优化问题求取最短路径。其特点主要在于对估价函数的定义上。A*算法主要通过其估价函数来计算各点之间的代价值，从而找到代价最小的路径即为最优路径。估价函数 f(n)用于评估从起点经过当前节点 nn 到目标节点的总代价，其公式为：

• ：从起点到当前节点 nn 的实际代价（已知值）。
• ：从当前节点 nn 到目标节点的启发式估计代价（预测值）。通常情况下利用曼哈顿函数计算。

这里关于A*算法的原理不在赘述。

        由于需要存储遍历点的上一节点和对应的f 值，这里可以利用类实现。每个点是Node类的对象，每个点有position(当前点的坐标)、parent(父节点坐标)、g、h、f(代价函数)。

为了方便计算h值，封装曼哈顿函数：

核心算法

三、路径规划实现

首先开启ROS核心

启动map_server加载PGM图像

运行A*算法代码

Rviz实现规划路径可视化