1.2 Dijkstra和A*算法原理

Dijikstra算法原理

与BFS相比，添加了每个边的移动成本

• 策略：扩展/访问具有最小累积成本g(n)的节点
  • g(n)：从开始状态到当前节点的累积成本
  • 更新节点的所有未扩展邻居的累积成本g(m)
  • 已扩展/访问的节点保证从启动状态起具有最小的成本

伪代码：

• 维护一个优先级队列以存储要扩展的所有节点
• 优先级队列初始化并加入初始状态$X_s$
• 对图中的所有节点初始化$g(X_s)=0$, $g(n)=inf$
• 开始循环
  • 如果队列为空，说明没有找到路径，return FALSE; break;
  • 把队列中$g(n)$最小的节点n弹出
  • 标记节点n为已扩展
  • 如果节点n是目标点, return TRUE; break;
  • 寻找节点n的所有未拓展节点m
  • If g(m) = inf 更新节点累积成本
    • g(m)= g(n) + Cnm
    • 将节点m塞入队列
  • If g(m) > g(n) + Cnm 保证节点从启动状态起具有最小的成本
    • g(m)= g(n) + Cnm
  • end
• End Loop

优缺点

• 优点：
  • 搜索完整且得到的路径最优
• 缺点：
  • 只能看到到目前为止积累的成本（即统一成本），从而探索每个“方向”中的下一个状态
  • 没有关于目标位置的信息

A*算法

Dijikstra算法与贪心算法结合

• 从开始状态到目标状态通过节点n的最小估计成本：f(n)=g(n)+h(n)
• g(n)：从开始状态到当前节点的累积成本
• h(n)：从节点n到目标状态的估计最小成本（贪心算法启发式函数）
• 策略：扩展/访问具有最小累积成本f(n)的节点
  • 更新节点的所有未扩展邻居的累积成本g(m)
  • 已扩展/访问的节点保证从启动状态起具有最小的成本

伪代码：

• 维护一个优先级队列以存储要扩展的所有节点
• 初始化所有节点的启发式函数$h(n)$
• 优先级队列初始化并加入初始状态$X_s$
• 对图中的所有节点初始化$g(X_s)=0$, $g(n)=inf$
• 开始循环
  • 如果队列为空，说明没有找到路径，return FALSE; break;
  • 把队列中$f(n)$最小的节点n弹出
  • 标记节点n为已扩展
  • 如果节点n是目标点, return TRUE; break;
  • 寻找节点n的所有未拓展节点m
  • If g(m) = inf 更新节点累积成本
    • g(m)= g(n) + Cnm
    • 将节点m塞入队列
  • If g(m) > g(n) + Cnm 保证节点从启动状态起具有最小的成本
    • g(m)= g(n) + Cnm
  • end
• End Loop

可能出现的问题

如图所示最佳路径应该为S->A->G，g(n) = 4，但由于启发式函数h(n)的存在，S->A->G的f(n)=10，使得A*算法探索出的最短路径为f(n)=5的S->G。

所以我们需要所有节点的估计值小于实际最小目标成本（即目标成本）

如何设计启发式函数？
$h(n)\le h^{\ast }(n)$对所有的节点n成立，其中$h^{\ast }(n)$是从节点n到目标节点的真实成本，此时称启发式函数为容许(admissible)的.

常见的启发式函数：

• 欧式距离($\sqrt{x^2+y^2}$)：always admissible
• 曼哈顿距离($x+y$)：Depends admissible 比如四邻域的话可行，八邻域不可行
• $L_{\infty }$：always admissible
• 0：always admissible
  

Weighted A*

如果使用高估启发式方法（即$h(n)\ge h^{\ast }(n)$）

$f=g+\epsilon h，\epsilon >1$ 偏向更快地接近目标，但结果可能不是最优路径。

Weighted A* 用最优性换取了速度。
贪心算法、Weighted A* 和A* 三种算法对比：

工程实践经验

常见的栅格地图连接方法，四邻域和八邻域：

工程实现步骤

• 创建稠密栅格地图
• 链接存储在栅格地图中的占用状态
• 通过网格搜索发现邻居节点
• 执行 A* 搜索

C++中常用的优先级队列：

• std priority_queue
• std make_heap
• std multimap

最佳启发式函数

常见的启发式函数：欧式距离($\sqrt{x^2+y^2}$)、曼哈顿距离($x+y$)、$L_{\infty }$、0都不是最佳启发式函数。

如图为使用欧式距离作为启发式函数，虽然能得到最佳路径，但依然扩展了很多无用节点。

启发式函数要求$h(n)\le h^{\ast }(n)$，如果令$h(n)=h^{\ast }(n)$搜索结果会如何变化？
由于栅格地图是高度结构化的

如图所示，在八邻域连接情况下，从起点到终点的距离可以通过计算得到：
$$h^{\ast }=(dx+dy)+(\sqrt{2}-2)min(dx,dy)$$
令$h(n)=h^{\ast }(n)$，搜索结果可视化为：

可见$h(n)=h^{\ast }(n)$时也能得到最佳路径，大量减少了扩展的无用节点。

Tie breaker(打破平衡性)

目前存在的问题

• 许多路径具有相同的f值
• 使用A* 搜索的路径之间没有差异

Tie breaker：选择一种技巧使得A*有倾向性地搜索其中一条路径

左侧为没有使用Tie breaker，右侧图片使用Tie breaker

常用Tie breaker方法：

• 轻微地修改ℎ：
  $$\begin{gathered} h=h\times (1.0+p) \\ p<\frac{一步的最小成本}{预期最大路径成本} \end{gathered}$$
  此时$h(n)\le h^{\ast }(n)$不再满足，但p足够小不足以对最后结果产生影响。

• 在相同成本路径中设置偏向性，以下为三种常用方法：

  1. 当节点具有相同的$f$时，比较它们的$h$
  2. 对每一个节点加上事先确定好的随机的数值（由坐标构成的哈希表）
  3. 首选从起点到目标的直线路径（以下公式为二维空间的叉积公式，代表两个向量在一个坐标系中的面积）
     $$\begin{gathered} dx1=abs(node.x-goal.x) \\ dy1=abs(node.y-goal.y) \\ dx2=abs(start.x-goal.x) \\ dy2=abs(start.y-goal.y) \\ cross=abs(dx1\times dy2-dx2\times dy1) \\ h=h+cross\times 0.001 \end{gathered}$$

问题

由于首选从起点到目标的直线路径，在没有障碍物的情况下有比较好的表现，但在有障碍物遮挡的情况下可能会出现右图这种情况，虽然路径长度相同，但直观上来说，图中红线这种路径才是最短路径，且会对后续轨迹优化产生影响，Tie breaker只是一种加快路径搜索的工程方法，应看情况使用。