基于搜索的路径规划算法

基于搜索的路径规划算法是较为成熟和广泛使用的一种路径规划算法，常常被用于移动机器人或者游戏中的人物的路径规划。
主要分为下面几个部分进行介绍：

1. 图搜索基础
2. Dijkstra and A*算法
3. 跳点搜索算法（Jump Point Search）
4. 作业

图搜索基础

Configuration Space / 构形空间

Configuration Space / 构形空间是一种特殊的空间，机器人的每一种位姿都代表空间中的一个点，这些点的集合就称为构型空间，也叫C-space。为什么要提出这样一种奇怪的空间概念呢？我个人理解有以下几种原因：

1. 各种各样的机器人的形状不一，而在构型空间内机器人的位姿都简化成一个点，便于用统一的方法描述障碍物并且用统一的方法做碰撞检测
2. 各种各样的机器人的运动学方程都不一样，例如有的能侧移有的则不能，大大提高了路径规划的复杂度。而在构型空间内都内描述成一条连续的曲线，使得所有路径规划的问题都无需考虑运动学。

总之，C-space空间的提出有利于使路径规划问题简化，使复杂的、多样的问题变得简单且单一。这个空间的概念不是特别容易理解，下面举几个简单的例子便能理解了。

现在有一个机器人，只能在水平空间内平移而无法旋转，我们只用它的座标（x，y）便能描述它的位姿。于是这样一个机器人的C-space便是R2。

若这个机器人能够旋转，则增加了一个自由度，它的C-space便可以用（x，y，theta）来表达。当然更加高维的问题则需要用其他的数学工具去表达如se(3)等。

图搜索理论

首先，什么是图。在计算机科学中，一个图就是一些顶点的集合，这些顶点通过一系列边结对（连接）。顶点用圆圈表示，边就是这些圆圈之间的连线。顶点之间通过边连接。

图可以分为有向图和无向图，同时有的图还有权重，这部分概念和数据结构的内容一致可参考
https://www.cnblogs.com/w-wanglei/p/figure.html 。

图搜索主要的流程如下：

1. 创建一个容器来储存所有要访问的节点

2. 初始化容器，将起始位置放入

3. 进入循环

   1. 移除：根据规则访问一个节点并将其移出容器
   2. 扩展：获取改节点的所有邻节点
   3. 存储：把符合条件的邻节点放入容器中

4. 结束

下面看两种经典的图搜索方法，深度优先搜索和广度优先搜索。

深度优先搜索

如下图，从节点0出发首先搜索节点1，然后节点3，最后到节点4后走到尽头了才接着走节点3和2。

将如上过程转换成树的形式则如下图，可以清晰的看到深度优先搜索的特点，即一条路走到黑再走另外一条路。

广度优先搜索

和深度优先算法相反，广度优先算法则是一层层的搜索的过程。

更多的动图可以访问网站 https://visualgo.net/zh/dfsbfs?slide=4-2 帮助理解。

算法实现

在深入讲解两种搜索算法的实现前需要先提及两种较为常见的数据结构：队列和栈。其特点也非常简单，队列的特点是先进先出而栈的特点是先进后出。

下面根据这两种数据结构编写算法的基本流程。

深度优先搜索

1. 创建一个栈来储存所有要访问的节点

2. 初始化栈，将起始位置放入

3. 进入循环

   1. 移除：访问节点s后，将初始节点s移除
   2. 扩展：获取初始节点s的所有邻节点d、e、p。
   3. 存储：把未访问过的d、e、p节点放入容器中
   4. …一直循环下去直到所有点被访问

4. 结束

广度优先搜索

1. 创建一个队列来储存所有要访问的节点

2. 初始化队列，将起始位置放入

3. 进入循环

   1. 移除：访问节点s后，将初始节点s移除
   2. 扩展：获取初始节点s的所有邻节点d、e、p。
   3. 存储：把未访问过的d、e、p节点放入容器中
   4. …一直循环下去直到所有点被访问

4. 结束

两者的不同之处仅仅在于使用的数据结构不同，堆栈返回最后进入的则使整个搜索一直朝一条路走。而队列返回最先进入的节点则会使整个搜索呈现一层一层的搜索。

那在实际的地图上这两种方法的效果是怎么样的呢？

可以看到广度优先要搜索的区域很大，但是可以找到一条最优的路径，而深度优先则会找到很多的错误的路径，但是它的好处在于即使在未知全局地图的情况下也能找到一条路径尽管这条路径不是最优的。

启发式算法

除了上面两种图搜索的方法，还有一类经典的搜索算法较为常用叫做启发式搜索，也叫做贪心搜索。贪心算法的思路就是每一次迭代都朝着局部最优的值靠近。相比与DFS和BFS根据压入数据结构的先后来确定下一次访问的节点，贪心搜索则对每一个数据结构内的节点用一个函数去估计其代价，找到代价最小的即局部最优值去访问，其中这个预估代价的函数便称作启发函数。

例如，常见的启发函数有欧式距离和马式距离。

若采用距离来作为启发函数则每次都会朝着离目标点最近的方向去搜索，如下图。

可以看到，在没有障碍物的情况下该算法能非常快找到一条可行路径，但是我们再看一下有障碍物的情况。

可以看到在一开始的时候算法陷入了局部的极值，没有找到一条最好的路径反而走向了死胡同。

启发式算法的实现和之前的两种没有太大的区别，主要在于容器的数据结构不一样，这里采用优先级队列作为容器，定义不同的启发函数作为排序依据，每次弹出优先级最高（即最近）的节点并访问、迭代。

Dijkstra and A*算法

前面介绍的三种算法，广度优先和启发式搜索较为实用但是依旧存在着明显的缺点：广度优先搜索需要访问大量的节点才能找到最优路径，启发式搜索能很快的进行搜索但是容易陷入局部极值导致路径并非最优的。于是有人提出将这两种方法的有点结合起来，于是就有了Dijkstra and A*算法。

第一个问题是如何确保找到的路径是最优的，可以从之前的例子看到路径不只一条且有的是最优路径有的不是。在之前广度优先搜索中认为第一次搜索到的终点的路径最短但是如果机器人能斜着移动，如下图

若机器人能斜着移动，则左右前后平移和斜着移动的距离明显不一样两者之前的权重是不一样的。对于这种情况我们在图的边上加上权重，直走的权重为1而斜着走为2或者根号2。更通常的情况下则将这样的地图抽象成权重图。

每一个节点在访问的时候都会累计得到从起点到该节点的路径上所有的权重和，若之前这个节点被访问过，则比较两者的权重和大小，取小的从而保证路径是最优的。而每次从队列中取出的不再是先进入的而是权重和最小的。当然可以看到，如果机器人无法斜着运动，每一个节点的权重都是1，则算法退化为广度优先算法，因为权重和最小的和最先进入的是同一个节点。

接下来就可以详细的描述算法的流程。

1. 创建一个优先级队列来储存所有要访问的节点

2. 初始化优先级队列，将起始节点放入

3. 进入循环

   1. 如果优先级队列为空，则跳出循环

   2. 移除：移除权重和最小的节点，并访问它

   3. 标记改节点已经被访问了

   4. 如果该节点是目标节点则，跳出循环

      对于该n节点的所有邻居m进行循环

      1. 如果m的权重和为infinite
      2. m节点的权重和为n的权重和加上n到m路径的权重
      3. 把m压入到队列中
      4. 如果m的权重>n的权重和加上n到m路径的权重
      5. m节点的权重和为n的权重和加上n到m路径的权重

   5. 存储：把未访问过的d、e、p节点放入容器中

   6. …一直循环下去直到所有点被访问

4. 结束

详细的流程可以参考这个图，和之前最大的区别在于它的优先级队列的排序规则是之前路径的权重和。这样就能保证在各种图中搜索到一条最优的路径。

那么这样一个算法在地图中搜索的表现如何呢？

可以看到，比起广度优先算法以一种正方形的方式扩展Dijkstra算法是以圆形的方式扩展，更为合理且能保证找到一条最优的路径。但是依旧保留广度优先算法的最大的缺点——需要搜索大量的节点后才能得到一条路径，算法效率太低。

所以第二个问题是如何解决算法效率低下，其中一种思路就是将启发式搜索和Dijkstra算法结合。思路也很简单除暴，Dijkstra的优先级队列的排序规则是节点的权重和而启发式搜索的排序规则是距离目标点的距离。所以结合两者算法，优先级队列的规则设置为节点的权重和与距离目标点的距离两者的和。即
$f(n)=g(n)+h(n)$

A*和Dijkstra算法的流程一模一样,只是将排序的规则改成了上面的和的方式。那么这种算法在地图中搜索是什么情况？

可以看到改算法可以兼顾两者的优点，既访问较少的节点，目的性比较强又不至于陷入局部最优而找到一条不好的路径。

当然也有人提出将$f(n)=g(n)+h(n)$更改成加权和，通过修改两者的权重比来调整算法的贪心程度。
大家可以和如下图所示的调整权重来对比算法，可以在网站 http://qiao.github.io/PathFinding.js/visual/ 中进行尝试。

似乎所有的问题都解决了，但是在实际的工程实现中A*并不是总是能表现良好。其中如何选择启发函数便尤为重要，之前大多采用欧式距离作为启发函数实际上启发函数多种多样，那启发函数需要满足哪些要求呢？还有有没有一个理论最优的启发函数呢？

实际上只要满足m节点的启发函数值大于或等于实际上从m到目标点的最短路径即可。显然当启发函数和m到目标点的最短路径相等是最好的，也就是启发函数估计的和实际的一毛一样，当然能找到最好的路径了。但是现实是障碍物的千变万化，无法做到这一点，于是退而求其次要求启发函数在没有障碍物的环境中和实际的路径是一样的即可。那么找到一个最优的启发函数的问题转换成如何在无障碍物情况下找到一个最短路径了，这当然是一个很简单的事情，如下图。

于是得到最优的最短路径的计算方法如下：
$\begin{aligned} &d x=abs(node.x-goal.x)\\ &\mathrm{d} y=\operatorname{abs}(\text { node. } y-\text { goal. } y)\\ &h=(d x+d y)+(\sqrt{2}-2) * \min (d x, d y) \end{aligned}$

跳点法（Jump Point Search）

A*算法是目前较为成熟且使用的较多的一种路径搜索的算法，后来的学者又对其进行了很多的修改和完善使其适应不同的应用情况，而跳点法则是其中的佼佼者。

跳点法的提出主要目的在于解决如下的问题，在较为空旷的环境中搜索路径时常常有不只一条最优路径，而A*则无法区分这些路径导致所有的最优路径都会被搜索。但是实际情况是我们只需要其中的任意一条即可，而对其他的最优路径的搜索实则可以看成一种浪费资源的行为。如下图，有很多的路径，而我们只需要其中一条即可。

基于以上的考虑，有学者提出了跳点法的概念，他可以根据一种规则在许多的等价的路径中找出一条并一直跟随直到抵达目标点。

因此跳点法最关键的点在于如何确定这个规则，它的规则称为向前看规则（Look Ahead Rule），主要的规则如下：

1、若是通过平移的方式抵达该节点的且四周无障碍物，则只需要查询和抵达该节点的平移方向的唯一节点，如图。

为什么需要这样一个奇怪的规则呢？我们来仔细分析一下，从4->x，x按照A*的规则理论上要访问1，2，3，5，6，7，8节点，但是我们发现了一个漏洞。从4到1明显比4->x->1要短很多，既然我从4走到了x，说明1这个节点无需访问也会在其他的路径下走到1并取得更短的路径。这个过程相当于减枝的过程，抵达1的方法有很多而从4->x->1明显不是最短的，这个步骤我就直接减去了。同理，我们对2，3，4，6，7，8逐个分析可以得到，仅有从4->x->5这样一条路径是最短的。其他的节点都有其他的更短的方法抵达，因此我们跳到节点5。

2、若是通过斜移的方式抵达该节点且四周无障碍物，则访问如图三个节点。分析方法和第一种是一模一样的，对每个点分析从6到该点的最短路径，若最短路径需要经过x，则该点需要访问，反之则直接丢弃。

考虑完没有障碍物的简单情况，那么有障碍物则如何去取这个规则呢？

3、若是平移且障碍物在对角某处，则和第一种情况一样。

4、若是平移且障碍物在节点的侧面，则对应侧的对角节点也要压入优先级队列。可以通过之前同样的方法来分析，可以发现由于障碍物的出现，导致该对角节点的最短路径发生了改变也必须通过x了，所以也要访问了。

5、类似的，如图自行分析

知道了规则之后，我们来看JPS的具体流程

首先在初始位置我们需要访问右侧三个点，首先是水平方向的节点，然后由于没有障碍物继续方法其水平的点，最后一直调到障碍物上，结束水平跳跃。然后访问上方的点，同理会一直向上访问，直到地图边界然后结束，最后访问右上角的节点。对于右上角节点，又有三个节点需要被访问，和之前相似的步骤首先水平跳跃，遇到障碍物然后向上跳跃遇到边界，之后一直重复如上过程。

直到上图所示，发生了变化。在向右跳跃的过程中遇到了有障碍物的情况，这时候在该点有了两个需要访问的节点。

同样，首先访问右侧水平的节点，遇到边界结束。再次访问右下角的节点，然后不断重复，直到找到目标节点。

从上面的步骤中，我们可以提取出算法的步骤。而我们会发现和A*居然惊人的一致，仅仅在寻找当前节点的邻居节点时，JPS算法会按照Look Ahead rule，大大减少了压入队列的节点。

说了这么多，那么在地图中实际的搜索效果如何呢？

虽然jps能减少A*访问的节点，且找到的路径也是最优的，但是并不总是能取得很好的效果。例如在一个十分空旷的环境中，JPS往往需要查询很多节点才能找到路径，也就是说这个规则实际上削弱了算法的贪心程度，目的性不强导致效率在有些情况下变低了。

作业

最后的最后便是学习这一节课的作业了
待更新…