路径规划-A*算法

A*算法原理

A*算法通过包含启发信息的代价函数来搜索最优路径，代价函数()由两部分组成：起点沿着已生成的路径到达当前节点的开销()和当前节点到终点的预估开销ℎ()，

可由公式(1)表示： () = () + ℎ() (1)

A*算法建立并维护两个列表（open列表和close列表）

存放已经探测到但还未访问节点的Open列表和存放已经访问过节点的Close列表。

路径上每个节点都有一个指向父节点的指针，通过跟踪指针向前回溯即可找到最佳路径。

A*算法流程

A*算法主要是将节点划分为未考察的、待考察的以及已考察的三大类。刚开始时的(所有)节点都是未考察的，而待考察的节点则全都被放在open-table中，所有已经被考察过的节点全都放在close-table中。因此，明显的 open-table、close-table 表起初都是空的。

算法细节如下：

：重置数据；如：需要重置start-node 的 g-value = 0；h-value = -func(........)；

：将 start-node 入 open-table；

// 第一层循环逻辑开始

：取出open-table 中 f-value 值最小的节点作为 -node，并做如下处理。(刚开始时，f-value 最小肯定是 start-node，因为表中只有这个节点)

：如果 -node 不存在，则寻路失败退出

：如果 -node 为 end-node，则寻路成功退出，并将 end-node 的前驱节点置为 -node，然后返回由 end-node 逆序倒推其前驱节点直到 start-node 的所有节点组成的路径返回

：将 -node 入 close-table；

// 第二层循环开始

：逐个遍历 -node 的 -node，并做如下处理

：如果 -node 在闭表中，则不做任何处理

：如果 -node 不在开表中，则更新其 g-value、h-value 值；并将该 -node 的前驱节点置为 -node 节点，最后再将 -node 入 open-table

：如果 -node 已在开表中，则比较如果从当前的 -node 到该 -node 的 g-value 是否会比 -node 此时的 g-value 值更优(即：更小)。

如果不会更优，则不做任何处理；

如果会更优，则将 -node 的 g-value 值更新为从当前 -node 到 -node 的 g-value 值，并将 -node 的前驱更改为当前的 -node节点

：回到 继续处理下一个邻接节点

：只要 open-table 表非空，则继续回到 处理

A*算法缺点

标准 A*算法在扩展节点时比较盲目，可能将多余节点加入 Open 表，增加了维护 Open 表代价的同时扩大了搜索空间，导致算法在大范围空间下搜索效率不够理想。

改进A*算法[1]

本文采用双向搜索机制，同时开展从起点向终点的正向搜索和从终点向起点的反向搜索，逐步生成路径节点向中间靠拢；在扩展过程中依据方向信息对候选的节点进行筛选，过滤掉一些无效节点，提升搜索效率；最后对生成的路径进行修饰，删除冗余点，提升路径质量。

其中 S 是起始 位置，m 是被扩展到的节点，D 是目标位置。路径规划期望路径长度尽可能短，即路径方向尽可能贴合起点指向终点的方向。是当前节点到终点的方向 与期望方向的角度偏差，由公式(2)表示，||越小，节点 m 被加入 Open 表的可 能性越大。

--------------------------------------------------------------------------------------------------------------------------------