A星算法优化

A*算法：

结合了Dijkstra算法和最佳优先搜索的特点。A*算法维护一个优先队列，通过一个估价函数来确定节点的优先级，这个估价函数通常是实际已花费的代价加上到目标的预估代价（启发函数）。

算法实现步骤

1.建立搜索区域：将地图划分为一个二维数组或网络，每个格子代表一个节点；

2.设置起始节点和目标节点： 确定起始节点和目标节点在搜索区域里的位置；

3.计算节点评估值： 每个节点都有评估值，评估值F(n)=G(n)+H(n)，F(n)表示节点n的评价函数；G(n)表示从起点到该节点的实际移动代价，H(n)为启发函数，表示从节点到目标节点的估算成本；

4.扩展节点： 从起始节点开始，依次扩展其相邻的节点，并计算他们的评估值；

5.选择最优节点： 在扩展的节点中，选择评估值最小的节点作为下一个要扩展的节点；

6.重复4.5步骤： 直到找到目标节点或扩展完所有可能的节点；

7.回溯路径： 从目标节点开始，通过节点的父节点回溯，找到从起始到目标节点的最短路径。

一、启发函数
评估值F(n)=G(n)+H(n)，其中，H(n)为启发函数，启发函数的改进对算法行为影响很大。启发函数作用是指引正确的扩展方向。H(n)体现了搜索的启发信息。
如果H(n)=0，那么只有G(n)实际上是有用的，这时，A星算法退化为Dijkstra算法。
启发函数H(n)一般为：
1.曼哈顿距离： h(n)=|x_n-x_goal| + |y_n-y_goal| h = np.abs(n1.x - n2.x) + np.abs(n1.y - n2.y)

2.欧几里得距离： h(n)=$\sqrt{x^2+y^2}$

3.对角线距离：
dx = np.abs(n1.x - n2.x)
dy = np.abs(n1.y - n2.y)
min_xy = min(dx,dy)
h = dx + dy + (math.sqrt(2) - 2) * min_xy

二、权重系数改进
f(n)=g(n)+w(n)*h(n)
对启发函数前的系数进行调整。
三、搜索领域
修改代码中的get_motion_model()模块；
四、贝塞尔曲线平滑处理

混合A*
算法只考虑了x和y两个维度，混合A*算法在此基础上引入了表示朝向的θ维度。

相比于A* 算法，混合A*引入了Reeds-Shepp曲线生成机制。该曲线由一系列直线和圆弧组成，生成的路径符合车辆的运动学约束，能够生成平滑的路径。

参考博客：
https://blog.csdn.net/weixin_52029211/article/details/123412402?spm=1001.2014.3001.5501
https://blog.csdn.net/weixin_52029211/article/details/123598667?spm=1001.2014.3001.5502
https://blog.csdn.net/weixin_52029211/article/details/123712811?spm=1001.2014.3001.5502
https://blog.csdn.net/weixin_52029211/article/details/123898439?spm=1001.2014.3001.5502