无人车路径算法---（5）基于搜索的路径规划算法 III （Astar/ D*/ D* Lite）

上篇博客中介绍了Greedy以及Astar算法，本篇博客将介绍关于Astar算法的工程化应用以及基于Astar思想衍生出的一系列图搜索算法
其中Astar算法原理的相关介绍以及代码实现参见上篇博客， 链接：https://blog.csdn.net/weixin_40884570/article/details/121505255

---

1. Astar算法的工程化应用

根据Astar算法节点的cost公式定义：
$f(n)=g(n)+h(n)$
其中， $h(n)$也就是我们常说的heuristic function 表示了任意节点n到终点的最小估计值。根据搜索算法所对应的场景选择一个合适的heuristic function在实际应用中是一件很重要的事情。

1.1 heuristic function数值大小的影响

heuristic function的选用常决定着Astar算法的行为：

1. 当$h(n)$选择为0时，Astar算法即退化成了Dijkstra算法，可以搜索出最短路径，但搜索效率会有所下降
2. 当$h(n)$小于等于当前节点到达终点的实际cost时，Astar也可以保证搜索出最短路径，$h(n)$数值越小，所需扩展的节点数也越多，这也会导致搜索效率较低
3. 当$h(n)$完全等于当前节点到达终点的实际cost时，Astar将会严格搜索出最短路径，同时不会扩展多余的节点。这种情况会使得搜索非常高效，因此这种情况虽然要求严苛，但是确实会使搜索非常快速
4. 当$h(n)$数值有时大于当前节点到达终点的实际cost时，Astar无法保证搜索出的路径是最短路径，但是搜索的效率却可以很高
5. 当$h(n)$值很大，大到$g(n)$的值可以忽略的时候，算法则退化成了Greedy Best-First-Search

上述的介绍核心可以概括为，在工程化应用中，我们需要的是速度还是精度？
举一些例子：
1.当进行全局规划时，为了获得optimal的全局规划路径，我们通常会精度优先，选用较小的heuristic function保证路径为最短路径
2.当进行局部规划时，为了保证流畅性，我们可能会需要快速生成绕障路径，因此往往会选用稍大一些的heuristic function
3. 当无人车或者机器人算力有限或者地图较大时，我们也会选用速度有先，保证尽量少扩展不必要的节点
总之，heuristic function的选择与实际的工程应用场景有着千丝万缕的关系，也往往受限于cpu的性能，路径搜索的时间限制，地图的大小，以及地图中的语义信息重要程度。

1.2 heuristic function 计算方式的选择

当然，根据搜索方式的不同以及地图数据结构的不同，在选用distance heuristic function的计算方式时，前人也提出了一些建议（主要是学习了某斯坦福大佬的经验。。。。。）

• 当选用二维栅格地图且地图仅允许四邻域扩展时，选用曼哈顿距离(Manhattan distance)（$L_1$）
• 当选用二维栅格地图且地图允许八邻域扩展时，选用对角线距离（Diagonal distance）($L_{\infty }$)
• 当选用二维栅格地图且允许任意方向的扩展时，可选用欧氏距离（Euclidean distance）($L_2$),当然，此时也可以尝试其他的地图表达方式来实现搜索，因为可以任意方向扩展（譬如拓扑地图一类的）
• 当选用六边形栅格地图切允许六个方向扩展时，选用曼哈顿距离（此处无人车路径规划使用较少。。。可以不关注）

Manhattan distance:

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return  dx + dy

Diagonal Distance

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
// D = 1, D2 = 1, 为 Chebyshev distance
// D = 1, D2 = sqrt(2), 为 octile distance

Euclidean Distance

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return sqrt(dx * dx + dy * dy)

1.3 Breaking ties

假设我们的搜索场景：在空旷的场地内，搜索起点到终点，我们会发现，由于障碍物等较少，我们会产生很多f值相同的节点，根据我们上述介绍的Astar扩展原理，此时优先队列会分别pop这些节点进行扩展。这就导致了搜索效率被严重影响。因此采用了一个较为trick的方法，来打破这种平衡。
主要有两种方式来打破此平衡，

1. 给heuristic function增加一个系数来打破平衡：
   $h(n)=h(n)\times （1+p）$
   其中， p的经验值是 p < (扩展一步的最小代价值 / 最大路径长度期望值)。 值得注意的是，此中方法会破坏"admissibility", 但是正如上述部分说的，我们需要根据实际场景和需求去决定速度还是精度优先级更高

2. 给heuristic function增加一个确定性的随机数
   以下方法是通过增加随机数值来选择尽量沿着直线的搜索路径

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

上述代码计算了起点到终点向量与当前点到终点向量的叉积， ($\vec{a} \times \vec{b} = |a| \cdot |b| \cdot sin{\theta} $), 当向量方向角差别较大时，叉积自然更大，通过这种方式来实现选择与起点终点相连直线趋势相近的路径。

以下两幅图可以用来对比扩展的节点区别[1]

第一幅图是未采用breaking ties的节点拓展，可以看出其中存在很多无效节点拓展
第二幅图是采用了breaking ties的节点拓展，效率对比起来明显高了很多

2. 基于Astar衍生的算法

当前衍生的算法中比较出名的主要有以下几种：

1. Weighted A*
2. LPA*
3. D*
4. D Lite*

下面简单的介绍下D以及DLite 的算法流程

更加详细的介绍可以参考CMU的这篇课件，链接如下：
http://www.cs.cmu.edu/~motionplanning/lecture/AppH-astar-dstar_howie.pdf

2.1 Dstar算法

Dstar通常分为以下三个主要步骤：

1. 从终点利用类似于Dijkstra的算法开始倒叙搜索，直至搜索到起点
2. 从起点开始跟线，当判断路径movement_cost变化时，需要modify-cost,将受影响的states重新放入open_list中
3. 通过引入lower-state以及raise-state的概念来重新修正路径，因此达到根据环境变化动态调整路径的目的

主要算法变量的一些说明：

• $x,y$: 机器人状态位置
• $b(x)=y$: $x$的父辈是 $y$
• $c(x,y)$: 从状态$y$ 到状态$x$的movement_cost
• $t(x)$: 状态$x$的tag(由NEW，OPEN，CLOSED)组成
• $h(x)$: 从终点到当前位置的 path cost
• $k(x)$: 维护算法的open_list是一个优先队列(priority-queue)，决定队列数值大小的是$k(x)$，其中$k(x)$表示open_list中state $x$的最小path_cost

Modify-Cost

c(X,Y)=cval
if t(X) =CLOSED then INSERT (X,h(X))
Return GET-MIN ( )

---

Lower State

• $k(x)=h(x)$
• 将路径movement_cost降低的信息传播出去，例如障碍物消失或发现了新的前往终点的路径等
• 对于此状态 x