0 序

本文将从两个大块浅谈一下路径规划算法，第一部分是规划算法本身，第二部分是地图。
---- howe

1 前言

　　移动一个简单的物体（object）看起来很容易，而路径搜索却比较复杂。那为什么涉及到路径搜索就产生麻烦了呢？考虑以下情况：

我们的任务是：一个物体（unit）最初位于地图的底端并且尝试向顶部移动。

方法一: 物体扫描的区域中(粉红色部分)没有任何东西显示它不能向上移动，因此它持续向上移动。在靠近顶部时，它探测到一个障碍物然后改变移动方向。然后它沿着U形障碍物找到它的红色的路径（例如bug算法）。

方法二: 另一种路径搜索器（path finder / planner）将会扫描一个更大的区域（淡蓝色部分），但是它能做到，不让物体(unit)走向凹形障碍物，从而而找到一条更短的路径(蓝色路径）（例如贪心算法类）。

方法三: 地图处理
对于以上两种算法，除了路径规划算法本身，还有一种办法是对地图的处理，你可以把凹形出口标识为危险的(只有当目的地在里面时才进去)（这个方法的思路是对地图的预处理）如下图所示。

对于路径搜索器， 我们更加期望结果路径可以提前绕开，以相对较小的cost完成路径。

1.0 地图以及算法简介

　　在路径搜索算法中主要地图包括

             1. 栅格地图
             2. 概率图
             3. 特征地图
             4. 拓扑地图

• 栅格地图（Grid Map）则是把环境划分成一系列栅格，在数学视角下是由边联结起来的结点的集合，一个基于图块拼接的地图可以看成是一个栅格图，每个图块(tile)是一个结点，图块之间的连接关系如短线。
• 概率图（Cost Map）如果在栅格图的基础上，每一栅格给定一个可能值，表示该栅格被占据的概率，则该图为概率图。
• 特征地图（Feature Map）特征地图用有关的几何特征（如点、直线、面）表示环境。常见于vSLAM（视觉SLAM）技术中。它一般通过如GPS、UWB以及摄像头配合稀疏方式的vSLAM算法产生，优点是相对数据存储量和运算量比较小，多见于最早的SLAM算法中。
• 拓扑地图（Topological Map）是指地图学中一种统计地图, 一种保持点与线相对位置关系正确而不一定保持图形形状与面积、距离、方向正确的抽象地图。包括有有向图和无向图（字面意思）。

栅格地图

概率图

特征地图

拓扑地图-有向图

拓扑地图-无向图

以上几种地图分类，根据使用的坐标系不同又可以分为笛卡尔坐标系和Frenet坐标系

Frenet坐标系

笛卡尔坐标系

但是只要我们保证搜素算法/地图/障碍物都在同一坐标系，其实可以忽略坐标系的影响，所以对搜索算法有影响的只有地图类型，而且不是坐标系类型。

地图和算法的支持关系是多对多的，即有的算法可以被应用到不同的地图类型中去，但是有的算法只能被应用到特定场景中。例如

   1. 栅格地图 / 概率图
             1. Dijkstra
             2. BFS（Best-First-Search）
             3. A*
             4. hybrid A*
             5. D  *
             6. RRT
             7. RRT*
             8. 蚁群算法
             9. Rectangular Symmetry Reduction (RSR) 
             10. BUG
             11. Beam search
             12. Iterative Deepeningc
             13. Dynamic weighting
             14. Bidirectional search
             15. Dynamic A* and Lifelong Planning A *
             16. Jump Point Search
             17. Theta *
     2. 拓扑地图
              1. Dijkstra
              2. BFS（Best-First-Search）
              3.  A*
              4. CH
              5. HH
              6. CRP

首先，我先介绍使用栅格(grid map)图/概率图（cost map）的算法。稍后我将讨论其他类型的图和算法。

             1. Dijkstra
             2. BFS（Best-First-Search）
             3. A*

1.1 Dijkstra算法

　　Dijkstra算法算是贪心思想实现的，其可以适用与拓扑图或者栅格图，具体实现方法是，首先把起点到所有点的距离存下来找个最短的，然后松弛一次再找出最短的，所谓的松弛操作就是，遍历一遍看通过刚刚找到的距离最短的点作为中转站会不会更近，如果更近了就更新距离，这样把所有的点找遍之后就存下了起点到其他所有点的最短距离：

动画过程如下：

在栅格图中的实现效果如下：

1.2 BFS（Best-First-Search）算法

　　BFS（Best-First-Search）算法也是可以看作基于启发式的深度优先算法，其按照和Dijkstra类似的流程运行，不同的是它能够评估任意结点到目标点的代价（即启发式函数）。与选择离初始结点最近的结点不同的是，它选择离目标最近的结点。BFS不能保证找到一条最短路径。但是它比Dijkstra算法快的多，因为它用了一个启发式函数（heuristic ）能快速地导向目标结点。例如，如果目标位于出发点的南方，BFS将趋向于导向南方的路径。在下面的图中，越黄的结点代表越高的启发值（移动到目标的代价高），而越黑的结点代表越低的启发值（移动到目标的代价低）。这表明了与Dijkstra 算法相比，BFS运行得更快。

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

　　另一方面，BFS运行得较快，但是它找到的路径明显不是一条好的路径：

　　问题在于BFS是基于贪心策略的，它试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价，而忽略了当前已花费的代价，于是尽管路径变得很长，它仍然继续走下去。

　　结合两者的优点不是更好吗？1968年发明的A*算法就是把启发式方法（heuristic approaches）如BFS，和常规方法如Dijsktra算法结合在一起的算法。有点不同的是，类似BFS的启发式方法经常给出一个近似解而不是保证最佳解。然而，尽管A*基于无法保证最佳解的启发式方法，A*却能保证找到一条最短路径。

1.3 A *算法

　　A*是路径搜索中最受欢迎的选择，因为它相当灵活，并且能用于多种多样的情形之中。

　　和其它的图搜索算法一样，A*潜在地搜索图中一个很大的区域。和Dijkstra一样，A*能用于搜索最短路径。和BFS一样，A*能用启发式函数引导它自己。在简单的情况中，它和BFS一样快。

　　在凹型障碍物的例子中，A*找到一条和Dijkstra算法一样好的路径：

　　成功的秘决在于，它把Dijkstra算法（靠近初始点的结点）和BFS算法（靠近目标点的结点）的信息块结合起来。在讨论A*的标准术语中，g(n)表示从初始结点到任意结点n的代价，h(n)表示从结点n到目标点的启发式评估代价（heuristic estimated cost）。在上图中，yellow(h)表示远离目标的结点而teal(g)表示远离初始点的结点。当从初始点向目标点移动时，A*权衡这两者。每次进行主循环时，它检查f(n)最小的结点n，其中f(n) = g(n) + h(n)。

2 关于启发式函数的讨论

　　启发式函数h(n)告诉A*从任意结点n到目标结点的最小代价评估值。选择一个好的启发式函数是重要的。
启发式函数h(n)告诉A*从任意结点n到目标结点的最小代价评估值。选择一个好的启发式函数是重要的。

2.1 A*对启发式函数的使用

2.1.1 通过对启发式函数的调节，可以达成控制A*的行为：

• 一种极端情况，如果h(n)是0，则只有g(n)起作用，此时A*演变成Dijkstra算法，这保证能找到最短路径。
• 如果h(n)经常都比从n移动到目标的实际代价小（或者相等），则A*保证能找到一条最短路径。h(n)越小，A*扩展的结点越多，运行就得越慢。
• 如果h(n)精确地等于从n移动到目标的代价，则A *将会仅仅寻找最佳路径而不扩展别的任何结点，这会运行得非常快。尽管这不可能在所有情况下发生，但是你仍可以在一些特殊情况下让它们精确地相等。只要提供完美的信息，A*会运行得很完美，认识这一点很好。
• 如果h(n)有时比从n移动到目标的实际代价高，则A*不能保证找到一条最短路径，但它运行得更快。
• 另一种极端情况，如果h(n)比g(n)大很多，则只有h(n)起作用，A*就演变成了BFS算法。

所以我们得到一个很有趣的情况，那就是我们可以决定我们想要从A*中获得什么。理想情况下，我们想以最快地得到最短路径。如果我们的目标的引力太低，我们仍会得到最短路径，不过速度变慢了；如果我们的目标引力太高，那我们就放弃了最短路径，但A*运行得更快，所以最优路径和最快搜索在复杂情况下需要有一个取舍/平衡。

A*的这个特性非常有用。例如，你会发现在某些情况下，你希望得到一条好的路径，而不是一条完美的路径（我认为：游戏/无人车场景均适用该情况），为了权衡g(n)和h(n)，你可以修改任意一个。

2.1.2 同样通过对g(n)或者f(n)的调节，也可以对达成A*具体动作的控制

• 通过加上障碍物cost function到g(n)或者f(n)（这两个动作是一个意思），可以实现规划路径在障碍物中间。
• 通过加上车辆几何或者轨迹kappa平滑度cost function的到g(n)或者f(n)，可以实现规划出来的路径是平滑变化的。
• 通过加上到way point的cost function的到g(n)或者f(n)，规划出来的路径则倾向于走way points的方向。

注: 在学术上，如果启发式函数值是对实际代价的低估，A*算法被称为简单的A算法。

2.2 速度还是精确度？

　　A*基于改变启发式代价函数来改变它自己行为的能力。在速度和精确度之间取得折衷将会让我们的程序运行得更快。 在大部分情况下，我们并不真正需要得到最好的路径，仅需要近似的就足够了。而你需要什么则取决于当前场景发生着什么，或者运行的机器有多快。

　　举个例子，假设你有两种地形，平原和山地，在平原中的移动代价是1而在山地则是3。则A* 在平坦路面上走3步等于在崎岖道路上走一步。这时候A*将会搜素出一条绕行山地的路，但是有时候这并不是最好的路，如果我们期望走最短的路则通过调节山地的代价等于甚至小于平原即可。

　　速度和精确度之间的选择前不是全局固定不变的。你可以基于CPU的速度、用于路径搜索的时间片数、地图上物体的数量、物体的重要性、物体群落的大小、经过难度或者其他任何因素来进行动态的选择。

　　取得动态的折衷的一个方法是，建立一个启发式函数用于假定通过一个网格空间的最小代价是1，然后建立一个代价函数（cost ）用于测量：

g’(n) = 1 + alpha * ( g(n) – 1 )

　　如果alpha是0，则改进后的代价函数的值总是1。这种情况下，地形代价被完全忽略，A*工作变成简单地判断一个网格可否通过。如果alpha是1，则最初的代价函数将起作用，然后你得到了A*的所有优点。你可以设置alpha的值为0到1的任意值。

　　你也可以考虑对启发式函数的返回值做选择：绝对最小代价或者期望最小代价。例如，如果你的地图大部分地形代价为2，其它一些地方是代价为1的道路，那么你可以考虑让启发式函数不考虑道路，而只返回2*距离。

　　速度和精确度之间的选择并不是全局固定对。在地图上的某些区域，精确度是重要的，你可以基于此进行动态选择。例如，假设我们可能在某点停止重新计算路径或者改变方向，则在接近当前位置的地方，选择一条好的路径则是更重要的，对于在地图上的一个安全区域，最短路径也许并不十分重要，但是当从一个危险区域脱离对时候，轨迹的精度是最重要的。

2.3 计算的量级

　　A*类贪心算法的计算公式是 f(n) = g(n) + h(n)。为了对这两个值进行相加，这两个值必须使用相同的衡量单位，即需要在同一量级上计算。如果g(n)用km来衡量而h(n)用m来衡量，那么A*将会认为g或者h太大或者太小，因而你将不能得到正确的路径，同时你的A*算法将运行得更慢。因此统一量级是一件很重要的事，这里sigmoid函数的函数特性，将会是起到一定的帮助。

2.4 启发式函数设计的重要性

　　如果你的启发式函数精确地等于实际最优路径，如2.5.1部分的图中所示，你会看到此时A*扩展的结点将非常少。A*算法没一步发生的事情是：在每一结点它都计算f(n) = g(n) + h(n)。当h(n)精确地和g(n)匹配时，f(n)的值在沿着该路径时将不会改变。不在最佳路径上的所有结点的f值均大于正确路径上的f值。如果已经有较低f值的结点，A*将不考虑f值较高的结点，因此它肯定不会偏离最短路径。

　　构造精确启发函数的一种方法是预先计算任意一对结点之间最短路径的长度。有几种方法可以近似模拟这种启发函数：

1.  【降采样地图-预计算】在密集栅格图的基础上添加一个分辨率更大的稀疏栅格图。预计算稀疏图中任意两个栅格的最短路径。

2.  【waypoings-预计算】在密集栅格图上，预计算任意两个way points的的最短路径。

通过以上方法，添加一个启发函数h’用于评估从任意位置到达邻近导航点/中继点（waypoints）的代价。最终的启发式函数可以是：

h(n) = h'(n, w1) + distance(w1, w2), h'(w2, goal)

2.5 网格地图中的启发式算法

在网格地图中，有一些众所周知的启发式函数计算方法：

1.   曼哈顿距离
2.   对角线距离
3.   欧几里得距离
4.   欧几里德距离平方

2.5.1 曼哈顿距离

　　标准的启发式函数是曼哈顿距离（Manhattan distance）。考虑你的代价函数并找到从一个位置移动到邻近位置的最小代价D。因此，h是曼哈顿距离的D倍：

  ``` H(n) = D \  * (abs ( n.x – goal.x ) + abs ( n.y – goal.y ) ) ```

2.5.2 对角线距离

　　如果在你的地图中你允许对角运动那么你需要考虑对角线距离。（4 east, 4 north）的曼哈顿距离将变成8*D。然而，你可以简单地移动（4 northeast）代替，所以启发函数应该是4*D。这个函数使用对角线，假设直线和对角线的代价都是D：

H(n) = D * max(abs(n.x - goal.x), abs(n.y - goal.y))

　　如果对角线运动的代价不是D，但类似于D2 = sqrt(2) * D，则准确的计算方法如下：

          h_diagonal(n) = min(abs(n.x - goal.x), abs(n.y - goal.y))

          h_straight(n) = (abs(n.x - goal.x) + abs(n.y - goal.y))

          H(n) = D2\* h_diagonal(n) + D\* (h_straight(n) - 2\ *h_diagonal(n))) 

　　这里，我们计算h_diagonal(n)：沿着斜线可以移动的步数；h_straight(n)：曼哈顿距离；然后合并这两项，让所有的斜线步都乘以D2，剩下的所有直线步(注意这里是曼哈顿距离的步数减去2倍的斜线步数)都乘以D。

2.5.3 欧几里德距离

　　如果你的单位可以沿着任意角度移动（而不是网格方向），那么你也许应该使用直线距离：

  ``` H(n) = D* sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2) ```

　　然而，如果是这样的话，直接使用A*时将会遇到麻烦，因为代价函数g不匹配启发函数h。因为欧几里得距离比曼哈顿距离和对角线距离都短，你仍可以得到最短路径，不过A*将运行得更久一些：

2.5.4 欧几里德距离平方

　　还有一个方法是，使用距离的平方替代距离，避免进行平方根开方运算，从而减少计算消耗：

  ``` H(n) = D* ((n.x-goal.x)^2 + (n.y-goal.y)^2) ```

　　不过这样做会明显地导致衡量单位的问题。当A*计算f(n) = g(n) + h(n)，距离的平方将比g的代价大很多，并且你会因为启发式函数评估值过高而停止。对于更长的距离，这样做会靠近g(n)的极端情况而不再计算任何东西，A*退化成BFS：

2.6 启发函数的启发因子

　　导致A*搜索低性能的另外一个原因是启发函数的启发因子。当某些路径具有相同的f值的时候，它们都会被探索，比较函数无法打破比较平衡点，尽管我们这时候只需要搜索其中的一条，下图为没有添加启发因子的效果：

　　为了解决这个问题，我们可以为启发函数添加一个较小的启发因子。启发因子对于每个结点必须是确定的唯一的，而且它必须让f值体现区别。因为A*将会对f值进行堆排序，让f值不同，意味着只有一个f值会被检测。

1. 一种添加启发因子的方式是稍微改变h的衡量单位。如果我们减少衡量单位，那么当我们朝着目标移动的时候f将逐渐增加。很不幸，这意味着A*倾向于扩展到靠近初始点的结点，而不是靠近目标的结点。我们可以稍微的微调h的衡量单位（甚至是0.1%），A*就会倾向于扩展到靠近目标的结点。

     ``` heuristic  \  \ *= (1.0 + p) ```
   

其中这里的启发因子需要满足

        p < 移动一步的最小代价 / 期望的最长路径长度。

假设你不希望你的路径超过1000步（step），你可以使p = 1 / 1000。添加这个附加值的结果是，A*比以前搜索的结点更少了。如下图所示。

当存在障碍物时，当然仍要在它们周围寻找路径，但要意识到，当绕过障碍物以后，A*搜索的区域非常少：

1. Steven van Dijk的建议是：直接把h放到比较函数中。当f值相等时，比较函数将会通过比较两个节点h的大小实现启发因子的功能，打破比较平衡点。

2. Cris Fuhrman的建议的启发因子是：给每个节点加一个决定性的任意数，例如所在坐标系中位置的hash值

3. 最后一种方法类似于frenet坐标系的做法：对比起点到终点的直连线段的投影距离，计算方法入下：

dx1 = current.x - goal.x
dy1 = current.y - goal.y
dx2 = start.x - goal.x
dy2 = start.y - goal.y
cross = abs(dx1*dy2 - dx2*dy1)
heuristic += cross*0.001

　　其目的是：计算初始->目标向量和当前->目标向量的向量叉乘（cross-product）。当向量偏离方向后，其叉乘将会提供一个较大的启发因子。结果是，这段代码选择的路径稍微倾向于从初始点到目标点的直线。当没有障碍物时，A*不仅搜索很少的区域，而且它找到的路径看起来非常棒：

其动态过程如下

Reference List：

• https://blog.csdn.net/DinnerHowe/article/details/79649954
• http://theory.stanford.edu/~amitp/GameProgramming/
• https://github.com/bonjs/wayFinding

Other

• todo: Tobe continue.....