Dijkstra、RRT两类路径规划算法原理的直观理解

在路径规划的算法里，有两大类算法是很常用的，一类是基于搜索和图的Dijkstra算法，还有一类是基于采样的RRT算法。本文对其算法原理进行简单的理解，力图生动的展示枯燥的数学公式背后精彩的思想。

Dijkstra与Astar思想

巫师三游戏开图

首先先不管什么是Dijkstra，让我们开局巫师。

一般来说，像这种开放世界游戏，前期大部分地图都是未知的（黑的），需要我们边做任务边探索，等到后期就可以很方便的想去哪里就去哪里了。我们先把目光放到前期。
上面是巫师三的宏观地图，一共有七个地方，在刚开始游戏的时候我们在百果园，我们的目标是在没有瞬移的情况下找到从百果园去任意地方的最短路径，毕竟路上很无聊，打桩要争分夺秒。
我们可以根据地图做出如下的有向图（为了解释，有些顺序和数字不符合游戏）：

箭头上面的数字是从箭头起点位置到终点位置的难度。比如刚开始玩这个游戏，在百果园（1）打第一个boss皇家狮鹫比较困难，可能需要打20次才能到达城堡见皇上（2），所以从1到2的损耗是20，再比如7是陶森特，等级很低去那里难度很大，所以从1到7的损耗是100。这里为了方便，我们用$c(i,j)$表示损耗，$m(i,j)$表示最短消耗。

现在假设小薯和女朋友各开了一个存档，但是为了让女朋友有更好的游戏体验，小薯要比她进度快一点，让她少踩一些坑。

他从1出发，进行探索，首先看到1可以去2和7两个地方，其他地方还不能去，小薯两个地方都去了，而且发现从1到2消耗$c(1,2)=20$，$c(1,7)=100$。这个时候他告诉女朋友，要从1先走到2，从1到2的最短消耗为:
$$m(1,2)=20$$
那么这样就开了1和2两个点。

而2只能到3，所以小薯和女朋友一起走到了3，那么从1到3的最短消耗为:
$$m(1,3)=c(1,2)+c(2,3)=21$$

这个时候又出现了分岔路口，从3先去4还是先去5，小薯进行了探索，发现$c(3,4)=15,c(3,5)=40$,所以他告诉女朋友先从3去4，且也可以得到，从1到4的最短路径为
$$m(1,4)=c(1,2)+c(2,3)+c(3,4)=36$$
这样就开了1，2，3，4。

之后发现从4可以又可以去两个点，那么又要麻烦小薯，比如对于6来说，从1到6的最小损耗是$m(1,6)=43。$,但是对于5这个点，不仅可以从3到4到5，也可以从3直接到5，而且发现从3 到5比从3到4到5还简单，这样小薯会从这三条路线中选出最简单的（12346,12345,1235），告诉女朋友如何走。且：
$$m(1,5)=40$$

这样就开了1,2,3,4,5。依次类推，开完全部的图。

从巫师三到Dijkstra

如果理解了上述例子，那么Dijkstra就已经理解了，差的就是总结成算法。
小薯在上面的例子中其实起到了一个不断尝试的功能，这个在Dijkstra的算法里面叫open list。这个东西的功能就是计算从起点到当前能到达的所有地方的损耗（当然，已经确认最小损耗的就没必要了，因为女朋友已经按照你的指引走完了）。
而女朋友其实就是实践，也就是确定的最终路径，这个在Dijkstra的算法里面要放在closed list中。发现一个最短路径就添加一个，当然，如果之后发现了更短的，也需要更新。
基于上面的思想，下面给出Dijkstra伪代码：

• 解释如下：

正经算法解释	联系上文
输入：一个有向图	巫师三地图的简化图
输出：从起点到任意位置的最短路径	女朋友走过的路
１：将起点放入closed list
２：起点的最小损耗为０
３：对于所有非起始点，需要做同样的操作：
４：从起始点到其他所有点能走的都走一遍，并给出最小损耗，走不到的地方值设置成无限大	小薯开始替女朋友尝试
５：如果open list还有点	女朋友还没开完图
6：找到所有点中的最小的损失那个一个	告诉女朋友该去哪里
7：将其加入colsed list	女朋友行动
8：加入点后，对其他点也会产生影响，对其他点进行更新	女朋友开完这个点之后，又可以去好多地方了
9：对其他点的损失进行更新，如果比原来的短	小薯再次开工
10：更新该点的最短距离	重新更新，以备告诉女朋友最新的简单路线

Dijkstra到Astar

介绍完Dijkstra的基本原理，其实Astar就简单很多了，主要的区别就是在最小损失的计算上面，也就是例子中箭头上面的数字。比如我们想从1到5，我们可能要经过2，3这些点，那么在计算损失函数的时候，我们只计算了从起点到2，3这些点的实际损失，但是没有考虑到当前点到终点预计的损失。

比如在3这个点，因为5是猎魔人的家，考虑到从3直接可以直线走到5会比较快，所以预计损失会比较小（设置为10），而从3到4到5走的比较长，所以对4这个点，可能预估损失比较大（设置为20）。

考虑当前位置对于终点的估计值，这是一种启发式搜索。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。对于Astar，箭头上面的数值改为：
$$f(n)=g(n)+h(n)$$
其中$g(n)$为从起点到当前点的实际损失，$h(n)$为从当前点到终点的估计损失。

代码参考：

• Dijkstra算法及代码详解
• Dijkstra算法图文详解
• python3.6实现的A星算法

RRT思想

从一棵小树长成参天大树

一棵小树苗想要长成参天大树，需要发展更多的枝杈，枝杈没有规律的随机生长，如果遇到障碍物。比如墙，就换个方向生长。这其实就是RRT的思想。

从乌鸦窝到圣格列高里之桥

我们再以巫师三为例。

猎魔人要从乌鸦窝去圣格列高里之桥东边办点事情（嘿嘿）。由于比较急，加上猎魔人本来就没有耐心(假如他可以瞬移一段距离)，所以想尽快到达。他只知道终点和起点，具体怎么走第一次其实不太清楚。他随便瞬移，如果遇到不能走的地方，就回到上一个位置，然后继续朝着终点大概的方向瞬移，直到到达办事地点。

事后思考

事情办完让我们理性的思考一下刚才的步骤，并看一看伪代码：
输入：一张地图、起点和终点
结果：一条路径
从起点开始，随机找一个点，然后找到这个点附近最近的已经确定的路径的点，组成一个方向。在这个方向上前进一步，得到一个新的点，如果这个点没有碰到障碍，那么就把这个点纳入路径中，之后重复上述过程，直到到达终点。

RRT的基本思想比较简单，需要了解更多的是其举不胜举的变种：基于概率的RRT算法、RRT Connect、RRT*算法等等，会单独进行整理。

参考：

• RRT算法简介
• 基于采样的运动规划算法-RRT(Rapidly-exploring Random Trees)