A星算法优化启发函数

Posted 2023-03-31 小巨同学

基于Python语言对A星算法进行优化：(视频中会拿python与matlab作对比)

源码地址：https://github.com/Grizi-ju/ros_program/blob/main/path_planning/Astar.py
B站详解视频：https://www.bilibili.com/video/BV1FL4y1M7PM?spm_id_from=333.999.0.0
基于开源算法：https://github.com/Grizi-ju/PythonRobotics/tree/master/PathPlanning/AStar
（个人认为，用哪种语言不重要，重要的是改进点及改进思路）

将从以下5个点进行改进：
1、启发函数——曼哈顿距离等
2、权重系数——动态加权等
3、搜索邻域——基于8邻域搜索改进
4、搜索策略——双向搜索、JPS策略等
5、路径平滑处理——贝塞尔曲线、B样条曲线等

一、基础代码详解

本人已经做了详细的中文注释，代码地址：https://github.com/Grizi-ju

算法预处理：
1、将地图栅格化，每一个正方形格子的中央成为节点（索引index）
2、确定起始点和目标点
3、定义open_set列表与closed_set列表，open_set中存放待考察的节点，closed_set中存放已经考察过的节点
4、初始时，定义起点为父节点，存入closed_set
5、父节点周围共有8个节点，定义为子节点，并存入open_set中，成为待考察对象

二、启发函数改进（理论）

A星算法评价函数为f(n)=g(n)+h(n)，其中h(n)为启发函数，启发函数的改进对算法行为影响很大
启发函数的作用：指引正确的扩展方向
其中：
f(n) 是节点 n的评价函数
g(n)是在状态空间中从初始节点到节点 n的实际代价
h(n)是从节点n到目标节点的最佳路径的估计代价。
g(n)是已知的，所以在这里主要是 h(n) 体现了搜索的启发信息。换句话说，g(n)代表了搜索的广度的优先趋势。

0、Dijkstra

如果h(n)=0,那么只有g(n)实际上是有用的,这时A*算法退化成迪杰斯特拉算法,它能保证一定可以找到一条最优路径

Dijkstra和贪心算法的缺点：
1.Dijkstra算法很好地找到了最短路径，但它浪费了时间去探索那些没有前途的方向。
2.贪婪的最好的第一次搜索在有希望的方向上探索，但它可能找不到最短的路径。

A*算法结合了这两种方法

1、曼哈顿距离

标准的启发函数是曼哈顿距离（Manhattan distance）

h = np.abs(n1.x - n2.x) + np.abs(n1.y - n2.y)     #  Manhattan

2、欧几里得距离（欧氏距离）

如果单位可以沿着任意角度移动（而不是网格方向），那么也许应该使用直线距离：

h = math.hypot(n1.x - n2.x, n1.y - n2.y)       #  Euclidean

3、对角线距离（切比雪夫距离）

如果在地图中允许对角运动，那么需要一个不同的启发函数

dx = np.abs(n1.x - n2.x)                        #  Diagnol distance 
dy = np.abs(n1.y - n2.y)
min_xy = min(dx,dy)
h = dx + dy + (math.sqrt(2) - 2) * min_xy   

参考论文：《一种面向非结构化环境的改进跳点搜索路径规划算法》等