运动规划之搜索算法：前端规划、后端轨迹生成到状态求解( 七 ) _算法

通过以上方法，添加一个启发函数h’用于评估从任意位置到达邻近导航点/中继点（waypoints）的代价。最终的启发式函数可以是：
h(n) = h'(n, w1) + distance(w1, w2), h'(w2, goal)
网格地图中的启发式算法
在网格地图中，有一些众所周知的启发式函数计算方法：
1.曼哈顿距离2.对角线距离3.欧几里得距离4.欧几里德距离平方

曼哈顿距离
标准的启发式函数是曼哈顿距离（Manhattan distance）。考虑代价函数并找到从一个位置移动到邻近位置的最小代价D 。因此，h是曼哈顿距离的D倍：
``` H(n) = D* (abs ( n.x – goal.x ) + abs ( n.y – goal.y ) ) ```

文章插图
对角线距离
如果在地图中允许对角运动那么需要考虑对角线距离。（4 east, 4 north）的曼哈顿距离将变成8D 。然而，可以简单地移动（4 northeast）代替，所以启发函数应该是4D 。这个函数使用对角线，假设直线和对角线的代价都是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 。
欧几里德距离
如果单位可以沿着任意角度移动（而不是网格方向），那么应该使用直线距离：
``` H(n) = D* sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2) ```

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

文章插图
欧几里德距离平方
还有一个方法是，使用距离的平方替代距离，避免进行平方根开方运算，从而减少计算消耗：
``` 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：

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

文章插图
为了解决这个问题，我们可以为启发函数添加一个较小的启发因子。启发因子对于每个结点必须是确定的唯一的，而且它必须让f值体现区别。因为A将会对f值进行堆排序，让f值不同，意味着只有一个f值会被检测。
一种添加启发因子的方式是稍微改变h的衡量单位。如果我们减少衡量单位，那么当我们朝着目标移动的时候f将逐渐增加。很不幸，这意味着A倾向于扩展到靠近初始点的结点，而不是靠近目标的结点。我们可以稍微的微调h的衡量单位（甚至是0.1%），A就会倾向于扩展到靠近目标的结点。