matlab扩展的欧几里得算法_【优化求解】A*算法解决三维路径规划问题

A*算法

A算法是一种典型的启发式搜索算法，建立在Dijkstra算法的基础之上，广泛应用于游戏地图、现实世界中，用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数，如式(1)所示。

f(n)=g(n)+h(n)(1)

其中，f(n)是算法在搜索到每个节点时，其对应的启发函数。它由两部分组成，第一部分g(n)是起始节点到当前节点实际的通行代价，第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时，都选取f(n)值最小的那个节点作为最优路径上的下一个节点。

在实际应用中，若以最短路程为优化目标，h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0，表示没有利用任何当前节点与终点的信息，A*算法就退化为非启发的Dijkstra算法，算法搜索空间随之变大，搜索时间变长。

A*算法步骤如下，算法维护两个集合：P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点；Q表维护那些已加入最优路径树上的节点。

(1)P表、Q表置空，将起点S加入P表，其g值置0，父节点为空，路网中其他节点g值置为无穷大。

(2)若P表为空，则算法失败。否则选取P表中f值最小的那个节点，记为BT，将其加入Q表中。判断BT是否为终点T，若是，转到步骤(3)；否则根据路网拓扑属性和