文|大核有料

编辑|大核有料

在机器人领域，路径规划一直是研究的热点问题之一，近年来，国内外学者对路径规划算法进行了大量的分析与研究.

目前，主流的路径规划算法分为以下几类：基于采样的路径规划、基于搜索的路径规划、基于深度学习的路径规划和基于遗传算法的路径规划。

基于采样的路径规划算法是近年来在路径规划算法中较为常用的算法类型。其显著优势在于不需要对周边环境进行建模，在三维或更高维度的复杂环境上具有良好的适应性。

其中快速搜索随机树算法（Rapidly-exploringRandomTree，RRT）和概率路线图算法（ProbabilisticRoadmap，PRM）是其中最具代表性的算法。

目前这些算法大都是在已经具备了环境地图的前提下进行路径规划导航，而在没有已知地图的情况，这些算法在未知环境下进行数据的建模和路径规划则需要相当长的计算时间。

而现有的可视图算法可以解决未知环境下路径规划的问题，但并未兼顾局部网络过于密集时，计算量骤增，路径计算时间和导航时间过长的问题。

在此基础上本文提出了一种双层框架的可视路途径规划算法，充分利用双层框架快速规划的优势来进行地图构建和避障。

那么双层框架可视图路径规划算法，在移动机器人中，能否减少机器人在复杂环境中的状态误差呢？

«——【·算法基础·】——»

可视图：可视图在机器人路径规划和计算几何的领域应用广泛，通常是由欧几里得平面上的一组点和障碍物来构成。

图中每个节点表示一个点位置，每一条边表示两节点之间的可视连接。大多数现有的可视图都是将雷达扫描出的点云信息存储到本地网络中，然后采用平面映射的方式提取多边形的顶点。

但这样的算法具有如下的弊端：首先若局部网络过于稀疏则采样的精度会降低，若局部网络过于密集，多边形的形状可以很快的确定，但计算量也会骤增；

其次顶点和边的数量会影响基于可视图的路径搜索算法的复杂程度，地图中会有很多冗余的顶点、边和错综复杂的障碍，这使得路径搜索成为一项耗时的任务；

最后可视图中大量的顶点和边会减慢它的遍历速度，从而导致可视图的维护速度下降。

由于可视图的简单性和有效性，其在社会生产中得到了广泛的应用。

但是，构造可视图需要相当大的计算量，时间复杂度为O（n3），其中n为顶点的数量，目前有国内外的学者对降低构造可视图的计算量和时间复杂度进行了大量的研究：改变障碍物的形状、合并附近的障碍物以及忽略微小的障碍物等。

在3D的环境中只考虑直线障碍可以将构造可视图的时间复杂度降低到O（n2）。

跳点搜索算法：跳点搜索（JumpPointSearch，JPS）是目前最先进的路径搜索算法之一，它是A*算法的一种改进算法，在保留A*的主体框架下，针对A*算法拓展大量无意义节点的行为进行优化：筛选出有价值的节点，即跳点，删除无意义的冗余节点，然后再进行扩展。

JPS算法的跳点筛选规则可分为自然邻居筛选和强制邻居筛选规则。

自然邻居筛选如图1所示，箭头表示运动方向，图1（a）的箭头代表机器人是从4号栅格移动到目前x的位置上，移动路线可以为水平或竖直方向直线也可以为对角方向直线，在这两种情况下，可以直接删除所有灰色邻居栅格块，因为这些灰色邻居块可以从x的父节点直接到达而并不需要经过x。

而剩下不能直接到达的白色栅格块称作x的自然邻居。

强制邻居筛选如图2所示，当x与黑色栅格块的障碍物相邻时，如图2（a）所示的3号块不存在从4号不经过x直接到达的最短路径，所以定义3号为x的强制邻居。

«——【·可视图的双层模型架构与双向JPS·】——»

多边形障碍物的采集与提取：在可视算法中，障碍物用多边形描述。移动机器人通过传感器检测到障碍物的点云数据O，而连续的数据点可以连接成为一组多边形，生成的多边形表示如公式1所示：

移动机器人会运行地形可通行性分析模块来分析地形特征，本文使用了激光雷达和深度相机处理环境信息，并输出O。

多边形的提取过程采用图像处理技术，假设B是一张二值图像，二值图像中黑色像素点对应着实际可通行的点，白色像素点对应着障碍物的点。

二值图像B会以移动机器人的起始出发点为中心，首先将点云O投影到B上，同时以移动机器人的尺寸进行参考。

将O上的点做膨胀处理，接下来用均值滤波器把图像进行处理，可以得到灰度模糊图像B’，接着在B’中提取出边缘点并分析边缘点的拓扑分布，这样就得到了一组轮廓由密集的点组成的封闭多边形。

将得到的封闭多边形表示为公式2所示：

对于每一个封闭多边形都会进行缩小顶点的处理，这样做的好处是可以推断出障碍物的局部曲率。

由于实际中不可能有完全笔直的多边形障碍物，故在处理顶点时加入一个角度阈值，本文把阈值设置为2度，将小于此阈值角度的顶点删掉，最后根据获取的顶点提取多边形。

可视图的双层架构与动态更新：本文使用双层逻辑框架来构建实时可视图，分为局部层和全局层。

在每一帧的图像上，都会生成一个由移动机器人传感器捕捉到的障碍物点云数据O构成的局部层，接着将局部层数据合并整合到全局层上。由此可知构造一个可视图的算法复杂度为O（n2Logn），其中n为可视图的顶点个数。

基于此，本文通过计算发现，在本地构造局部层，占用的算力很小，所以采用增量的方式构造可视图，只更新目前移动机器人所处位置及其周围区域，再将其合并至整体全局层，可以很快的构造出可视图。

本文充分利用可视图的快速重新规划的优势来处理未知环境下的路径规划问题。可以被移动机器人观测到的边缘都会连接着障碍物或是墙壁，而不可见的区域包含的障碍物则很少。

基于此，移动机器人运动的过程中只会观测到小部分的障碍，所以调整可视图的计算代价很小，随着移动机器人的前进，上述调整会不断地重复。

局部层构建：传感器得到的点云数据O经过处理转换成多边形Plocal，根据Plocal的结果在局部层中首先构造一个简约版的可视图。

设elocal为局部层上可见边的集合，一般来说，在elocal中经常会出现连接多边形时边出现重合交错的情况，删掉其中多余没用的边，称之为“冗余边”，留下的边称之为“连接边”。

对留下的连接边会设置一个阈值来检测边的长度是否构成边，然而在elocal中，初步检测到的低于阈值的边都不会被直接删除。

这是因为暂时不能确定这些低于阈值的边是否真的只有这么长，由于移动机器人视野或噪声的原因，可能这条边的末端并非是多边形的顶点。

在模拟中，由于环境的复杂和地形的限制，移动机器人可观测到和遍历的空间同样也存在着巨大的限制，大多数的可见边都会被附近的多边形遮挡，导致最后出现在elocal中的连接边的数量会比预测的要少。

全局层更新：在局部层构建了简约版的可视图后，将局部层和全局层合并，将局部层获得的数据更新覆盖到全局层上。

本文把在全局层上的多边形定义为公式3，其中，eglobal是全局层中可见边的集合。

首先将Plocal和Pglobal中的顶点进行关联，然后在Pglobal中更新关联的顶点的位置信息，并采用稳健拟合更新法消除离群值，通过多次的迭代更新，最后过滤出数据帧上的相应顶点。

迭代会先从内嵌的顶点开始，每一次的迭代都会计算样本点的均值和协方差，根据计算结果来调整顶点的内嵌状态或外联状态。

如果在两次迭代中，顶点的状态保持内嵌状态不变，则迭代终止，同样会设置一个最大迭代次数以减少无限循环的问题，若满足了最大循环条件同样会退出迭代。

接着将Pglobal上的顶点值更新为迭代后内嵌状态下顶点的均值，而没有被关联的顶点会被删除。

最后将elocal中存放的边合并整合到eglobal中，若eglobal中存在某些边，则会更新这些已有边，若不存在，会作为新边添加至eglobal中。在算法1中描述了可视图更新的整个过程。

基于可视图的双向：JPS算法路径规划由于本文的可视图具有存储功能，可以将移动机器人的行驶地图保存为先验地图，所以在执行全局路径规划时本文的算法会首先将起始点设置为全局层上的顶点，然后执行地图检测。

若存在目标点的位置信息，则将目标点也设置成全局层的顶点，接着采用双向JPS算法，并扩展eglobal以此来寻找最短路径；若未检测到目标点，则执行单向的JPS算法进行路径规划。

如果全部采用单向的JPS算法，在规划移动机器人已经行驶过一次的环境或半未知环境的路径时，会浪费大量的时间和搜索成本，双向JPS会有效地解决这个问题。

所以本文将目标点嵌入到可视图中，将目标点与图中现有的顶点相结合，然后采用双向JPS算法进行路径搜索。

如图3所示，S为初始的起始点，P为目标点。

从起始点S开始寻找最优跳点S1，接下来P寻找到最优跳点P1，按照如此流程交替搜索直到两个方向跳点相遇，此时找到了一条可通行路径，但此时并不能保证该路径为最优路径，所以两个方向要继续查找一次，如果搜索重叠则代表该路径为最优路径。

实际算法流程图如图4所示。

«——【·仿真结果分析·】——»

在仿真实验中，每次可视图更新时路径规划算法会重新搜索路径以更新最短路径。

在所有的仿真环境下都会进行两部分实验，分别是在完全未知的环境下和半未知的环境下进行模拟导航。

在完全未知环境下导航时，会在移动机器人到达目标点时，重置可视图及算法参数，以确保每一次机器人的导航都是完全未知的环境下运行的。而第二种半未知的环境则是在运行中保证数据不也慢慢变得已知。

图5为室内走廊环境仿真实例，本文在五个环境下分别进行路径导航规划。

从0号出发点开始，向1号目标点导航，到达1号点后再以1号点为起始点向2号点出发，同理，3号点为最终目标点。

可以充分利用各个地图的复杂性来检验算法的性能，如图5的0号点和1号点：两个点中间有两条岔路，可以很好的考验算法是否会陷入局部最优解。

图5的右下角是在RVIZ下的完整仿真路径。

实验结果证明，本文的算法在完全未知和半未知的情况下都可以高效的实现路径规划。

算法结果分析：FARPlanner（以下简写为FAR）是目前实时使用可视图提取环境进行路径规划最先进的算法，该算法支持在无先验地图的情况下根据传感器的数据对地图及导航路径进行实时调整。

本文统计了在上述几种环境下将可视图局部层的点云信息存储到全局层并分类（障碍物点云或自由空间点云）的过程，且统计了接受处理点云数据的时间，与FAR进行对比。

结果如图6所示。

图6（a）-（e）显示了五种环境下每个数据帧接受和处理点云信息所消耗的时间。按顺序分别为：室内，森林，隧道，校园，17DR。

图6（f）显示了平均消耗时间，可以直观地看出，本文算法（以下简称为DV-BJPS）在处理每个数据帧的信息时花费时间较少。

将本文的算法，以下简称为DV-BJPS与两种基于搜索的算法A*、D*Lite、两种基于采样的算法RRT*、BIT*及FAR进行对比，其中RRT*是RRT算法中的经典算法，而BIT*则是目前基于RRT算法中优化最好，效果最好的算法。

将上述算法同样运行于上小节的环境中，同样是在未知和半未知的环境下两次运行，将得到的结果和DV-BJPS进行比对。

表1展示了不同算法在不同环境下导航花费的总时间，表2展示了不同算法搜索出路径花费的时间，其中星号代表着在一次路径查找后将重置算法和可视图；

没有星号的代表在导航之前都不会重置。对于基于搜索的算法，A*和D*Lite在其中属于佼佼者，然而当环境复杂且及其大的时候，这两个方法的计算量会显著增加，从而导致搜索路径的时间变长，进而使得移动机器人反应迟缓，总的花费时间变多。

从表2可以看到，A*搜索算法在室内环境相较于室外校园环境，路径搜索时间增长了一倍多，室外校园相较于更复杂的隧道环境，搜索时间更是增长了三倍左右。

D*Lite可以通过重用上一个规划路径的状态值来减少规划搜索时间，但当移动机器人进入死角或是死胡同时，许多状态值与之前会变得迥然不同，算法需要大量的迭代计算使得状态值重新收敛，计算时间成本剧增，所以在表2中可以发现从室内环境到室外环境D*Lite的搜索时间呈指数级上升。

本文的算法可以实现在未知环境下搜索出最优路径，表1显示本文的算法在室内环境下相较于A*导航花费时间减少了2.5%~8.9%，相较于D*Lite减少了5.3%~10.9%；在隧道环境下本文的算法较于A*导航花费时间10.4%~13.8%，较于D*Lite减少了29.6%~32.1%，均有优秀成效。

而在校园的环境，A*算法在导航的时间上要略优于本文的算法，从表2可知，本文的算法要比A*算法搜索时间要快了几个数量级。

对于基于采样的算法：RRT*和BIT*，他们相较于基于搜索的算法来说，搜索时容易出现随机性，所以搜索时间也会出现不一致的情况，这也会增加总的导航时间。

为了在未知的环境中进行路径规划，算法需要从自由空间和未知空间两部分提取样本。

每当部分的环境被观察到时，搜索的时间都会有所增加，这是因为从未知空间到自由空间的路径经常会被障碍物阻塞，很难找到通往目标点的可行性路径。

从表2可以发现，在同一种环境下设置了分别均设置两种路径规划方式：完全未知和半未知。

在隧道环境下，BIT*的搜索时间从未知环境到半未知环境下，路径搜索时间从9.45ms增加到了231.25ms，这同样也导致了在半未知的环境下比完全未知的环境下路程多花费48%的时间。

表1的结果可以看到本文的算法均优于基于采样的算法，其中在室内环境下相比于RRT*缩短了近一倍的时间。

对于可视图算法FAR，我们在室内环境下可以看到，使用FAR进行导航时，搜索空间都有不同程度的浪费，FAR更倾向于探索代价更低的区域。

可能的原因是它的代价函数只参考了当前节点本身的代价和欧氏距离的代价，这就导致机器人倾向于在一个方向上朝着总成本最低的节点行驶，即使该节点不能抵达目标点。

在隧道环境下，本文的算法生成的路径相比FAR短了10.4%，这是由于隧道的环境较为复杂，而双向JPS搜索算法在这种环境下有着巨大的优势。

对于比较近的无障碍导航，FAR与DV-BJPS则相差不大。

«——【·结语·】——»

基于双层框架可视图的路径规划算法，该算法可以有效地在未知及部分未知的环境中搜索到最优路径。

算法中，将采集到的障碍物点云数据整合到多边形中，并在动态更新的可视图中使用双层结构框架来降低本文数据计算复杂度。

针对复杂障碍物，提出了一种删除多边形冗余边的算法。

将可视图与双向JPS搜索算法结合，并与目前最先进的基于搜索的两种算法：A*和D*Lite；基于采样的两种算法；RRT*和BIT*以及基于可视图算法；FAR在五种环境的两种情况下进行了比较分析。

试验结果表明，基于双层框架可视图的双向JPS搜素算法在未知和半未知的环境中可以迅速地找到最优路径，并优于目前主流的路径规划算法。

实时的构建可视图要求机器人的状态准确无误差，但在某些特殊气候环境下移动机器人的状态会出现一些异常，这会导致生成的可视图不可靠，从而影响路径规划搜索，因此考虑特殊气候环境对机器人状态产生影响下的可视图构建是接下来研究的问题。