（4-2） 轨迹规划算法和优化：A*算法

4.2  A*算法

A*算法（A星算法）是一种常用于图搜索和路径规划的启发式搜索算法，用于找到从起点到目标点的最短路径。该算法综合了最短路径搜索和贪婪搜索的优点，在现实中具有较高的效率和广泛的应用。

4.2.1  A*算法介绍

A*算法的基本思想是通过估计从起点到目标点的总代价（通常记为f(n)）来选择每一步的移动，该总代价由如下两个部分组成：

1. 实际已经花费的代价（g(n)）：从起点到当前节点n的实际路径代价。
2. 启发式估计的剩余代价（h(n)）：从当前节点n到目标节点的估计路径代价。这是A算法的关键之一，启发式函数要足够接近实际代价，但又要高效计算。如果启发式函数等于0，A算法就相当于Dijkstra算法。

实现A*算法的核心步骤如下所示。

（1）初始化：将起点加入开放列表&