A*算法原理概述

路径规划A*算法原理

路线规划理论概述

移动一个物体直观上很容易的，但是物品的路线规划是复杂的。如下图3-1所示，物体最初位于地图的底端，并尝试向顶部移动。物在扫描的区域中，没有任何东西显示物体不能向上移动，因此它持续向上移动。在靠近顶部时，它探测到一个障碍物然后改变移动方向。然后它沿着U形障碍物找到它的红色的路径。但是，路径规划将会扫描一个更大的区域（淡蓝色阴影部分），但是它能做到不让物体避开凹形障碍物，找到一条更短的路径(蓝色线）。

图3-1 物体规划前路线示意图
不带路径规划的运动可以在很多种情形下工作，同时可以扩展到更多的情形，但是路径规划是一种更常用的解决方法，路径规划可以提前作出路线规划，避免最后时刻发现问题。因此，研究人员提出路径搜索算法，用于规避上图的凹形障碍，示意图如下：

图3-2 路径规划路线示意图

Dijkstra算法

Dijkstra算法思想是从物体所在的初始点开始，遍历地图中的节点，迭代检查所有待检查节点集中的节点，并把和该节点最靠近的尚未检查的节点加入待检查节点集。该节点集从初始节点向外扩展，直到到达目标节点。Dijkstra算法保证能找到一条从初始点到目标点的最短路径，只要所有的边都有一个非负的代价值。在下图中，粉色的节点是初始节点，蓝色的节点是目标点，而类菱形的有色区域则是Dijkstra算法扫描过的区域。颜色最淡的区域是那些离初始点最远的，因而形成探测过程的边境：

图3-3 Dijkstra算法搜索示意图

最佳优先搜索算法

最佳优先搜索（BFS）算法评估任意节点到目标点的代价。与选择离初始节点最近的节点不同的是，它选择离目标最近的节点。最佳优先搜索算法不能保证找到一条最短路径。但是，较之Dijkstra算法，最佳优先搜索算法速度较快，因为它用了一个启发式函数快速地导向目标节点。例如，如果目标位于出发点的南方，BFS将趋向于导向南方的路径。在下面的图中，越黄的节点代表越高的启发式值（移动到目标的代价高），而越黑的节点代表越低的启发式值（移动到目标的代价低）。与Dijkstra 算法相比，BFS运行速度得更快。

图3-4 最佳优先搜索（BFS）算法搜索示意图
然而，这两个例子都仅仅是最简单的情况——地图中没有障碍物，最短路径是直线的。现在我们来考虑前边描述的凹型障碍物。Dijkstra算法运行得较慢，但确实能保证找到一条最短路径：

图3-5 最佳优先搜索（BFS）算法障碍物规避示意图
　另一方面，BFS运行得较快，但是它找到的路径明显不是一条好的路径，如下图所示。最佳优先搜索的缺点在于算法本身是基于贪心策略的，它试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价，而忽略了当前已花费的代价，于是尽管路径变得很长，它仍然继续走下去。

图3-6 最佳优先搜索（BFS）算法失败路径示意图

A*算法理论概述

A是路径搜索中最受欢迎的选择，因为它相当灵活，并且能用于多种多样的情形之中。和其它的图搜索算法一样，A算法潜在地搜索图中一个很大的区域。和Dijkstra一样，A能用于搜索最短路径。和BFS一样，A能用启发式函数，在简单的情况中，它和最佳搜索算法一样快。
在凹型障碍物的例子中，A找到一条和Dijkstra算法一样好的路径：

图3-7 凹型障碍物A算法路径
算法中的估价函数值代表从起始节点到当前节点的代价实际值，可能是距离或时间等指标的实际度量值，而值代表从当前节点到目标节点的代价估计值。当规定了地图节点之间的垂直、水平以及对角线代价后，在计算沿特定路径从起点到某一栅格节点的值时，可以依照路径的方向增加代价和求解。值可以用不同的方法估算，在网格地图中主要有曼哈顿距离、对角线距离和欧几里得距离三种，具体如下：
(1)曼哈顿距离
当机器人小车在地图中的运动方向只有正南、正北方向或者正东、正西方向时，可以选择曼哈顿距离作为估价函数。

图3-8 曼哈顿距离示意图

曼哈顿距离为两节点之间y轴方向（南北方向）上距离加上在x轴方向（东西方向）上距离，则启发式函数为：

假定直线行走一个网格的曼哈顿距离为D表示，则启发式函数为：
其中，c代表当前节点，goal代表目标节点。
(2)对角线距离
当机器人小车在地图中除了正南正北方向外，还允许对角运动时，此时可以选择对角线距离作为新的估价函数。

图3-9 对角线距离示意图
两节点之间的对角线距离为当前节点与目标点在南北方向和东西方向上距离的最大值，即：

假定机器人小车直线行走和对角行走的代价都为对角线距离D，则启发式函数为：

其中，c代表当前节点，goal代表目标节点。

如果对角线运动的代价不是D，而是D2=sqrt(2) * D，因此计算h_diagonal(n)，即沿着斜线可以移动的步数，h_straight(n)，即曼哈顿距离，然后合并这两项，让所有的斜线步都乘以D2，剩下的所有直线步(注意这里是曼哈顿距离的步数减去2倍的斜线步数)都乘以D。

则对角线的启发函数可以更新为：

(3)欧几里何距离
如果机器人小车在地图中可以沿着任意角度移动，而不仅仅是网格方向或对角线方向时，则应该选择欧几里得距离，也称为直线距离。两点之间的直线距离为：

假设直线行走和对角线行走的代价都为，则启发式函数为：

因为欧几里得距离比曼哈顿距离和对角线距离都短，使用欧几里何距离仍可以得到最短路径，不过A*算法将运行得更久一些。

参考：
http://theory.stanford.edu/~amitp/GameProgramming/
http://www-cs-students.stanford.edu/~amitp/gameprog.html#Paths