路径规划算法：A*

A*算法是启发式搜索，是一种尽可能基于现有信息的搜索策略，也就是说搜索过程中尽量利用目前已知的诸如迭代步数，以及从初始状态和当前状态到目标状态估计所需的费用等信息。

     A*算法可以选择下一个被检查的节点时引入了已知的全局信息，对当前结点距离终点的距离作出估计，作为评价该节点处于最优路线上的可能性的量度，这样可以首先搜索可能性大的节点，从而提高了搜索过程的效率。

     A*算法的基本思想如下：引入当前节点j的估计函数f*,当前节点j的估计函数定义为：

f*(j)= g(j)+h*(j)

    其中g(j)是从起点到当前节点j的实际费用的量度，h*(j)是从节点j到终点的最小费用的估计，可以依据实际情况，选择h*(j)的具体形式，h*(j)要满足一个要求：不能高于节点j到终点的实际最小费用。从起始节点点向目的节点搜索时，每次都搜索f*(j)最小的节点，直到发现目的节点。A*算法的核心是设计估价函数，设计估价函数h(j)有很多方法，下面介绍其中的两种。

估价函数1：欧几里德距离

  在和起点距离相等的中间节点集合里，与终点直线距离（欧几里德距离）越小的节点，方向夹角越小。

估价函数2：曼哈顿距离

   利用欧几里德距离计算估价函数的计算量很大，因此考虑将两点之间的曼哈顿距离作为估价函数。其中A点的经纬度为(Ax,Ay),B点的经纬度是(Bx,By),则A、B之间的Manhattan距离可以表示为:

f(j)=g(j)+(|Ax-Bx|+|Ay-By|)

A*算法在搜索中设置两个表:Open表和Close表。Open表保存了所有已生成而未被考察的节点，Close表中记录已被考察过的节点。算法中有一步是根据估计函数重排Open表中的节点，这样，循环的每一步只考虑Open表中状态最好（代价最小）的节点，如果被发现在Open表中存在同一个节点，就应比较两个节点代价的大小，如扩展得到的新节点代价大于已有的节点代价，可以放弃扩展得到的新节点，否则就以新节点代替原来的节点。如果在Close表中存在同一个节点号，则新新生成的路径的代价肯定大于原来路径的代价，可以淘汰新生成的节点。

算法步骤如下：

步1：生成空的Open表、Close表，将起点S放入Open表

步2：如果Open表为空，则失败退出。

步3：从Open表中，找出头节点U,作为当前节点,将它从OPEN表中移除。

步4：判断U是否为终点，如果是，则通过节点U的父指针，一直遍历到起点，找到到最优路径，算法结束。否则，转步5.

步5：扩展当前节点U,找到其扩展的后继节点集合V,遍历集合V中的节点，如果节点即不在Open表也不在Close表，计算该节点的估计值，将节点加入到OPEN表，设置父节点为U.如果节点在Open表、Close表，比较当前节点的代价g(v)和Open表、Close表中代价，如果当前节点的代价g(v)小，更新表中的代价和父节点指针。

步6：按估价值递增的顺序，对Open表中所有节点进行排序。

步7：转步2.