基于双层地图的分段式路径规划*

光兴屿，蒋 林,b

(武汉科技大学 a.冶金装备及其控制教育部重点实验室；b.机器人与智能系统研究院，武汉 430081)

0 引言

移动机器人的导航是移动机器人研究的重要组成部分之一，而路径规划一直是移动机器人研究方向上的热点与难点，寻求一条路径最短、收敛最快、能够避开障碍的最优路径，对解决机器人导航问题具有重要意义。

按照对机器人工作环境信息的掌握程度，机器人路径规划可以分为全局路径规划和局部路径规划两部分[1]。其中全局路径规划是指已知关于机器人工作环境的所有信息、机器人位姿信息与终点，根据环境地图按照一定的算法搜索一条最优或者近似最优的无碰撞路径，根据对地图构建的方法，分为栅格法[2]、拓扑法[3]等。局部路径规划是一种边探测工作环境边进行路径规划的方法[4]，可与全局路径规划配合使用，用于动态环境中的避障，局部路径规划主要有、遗传算法[5]、人工势场法[6]、滚动窗口算法[7]等。

现有的全局路径规划算法在路径规划中一般通过加入膨胀系数来解决在规划经过门的路径时，机器人会过于靠近障碍物的问题[8]，但当膨胀系数过低时，机器人离障碍物不会足够远，而当膨胀系数过高时，则会导致机器人无法通过一些比机器人自身略宽的门。

本文通过结合对栅格地图的预处理，通过加入了一层门点图层，对传统的全局路径规划进行优化，使其在实际环境中能够更快的到达终点。

1 全局路径规划介绍

机器人的全局路径规划可以分为两部分，第一部分是对环境地图的构 建，第二部分是路径规划方法。

1.1 占用栅格地图的建立

地图的构建就是将机器人的工作区域转化为机器人可以识别的数学模型，而栅格法是将机器人移动空间上建立二维的直角坐标系，并以固定尺寸对机器人的工作环境进行分割。

占用栅格的基本思想就是用一系列随机变量来表示地图，每个随机变量是一个二值数据，表示该位置是否被占用，占用栅格地图构建算法对以上随机变量进行近似后验估计。

在通常的尺度地图中，对于一个点，它要么有(Occupied状态，下面用1来表示)障碍物，要么没有(Free状态，下面用0来表示)障碍物(概率值)。在占据栅格地图中，对于一个点，用p(s=100 )来表示它是Free状态的概率，用p(s=0)来表示它是Occupied状态的概率。通过引入两者的比值Odd(s)来作为点的状态[9]：

(1)

对于一个点，新来了一个测量值(Measurement，z～{0,1})之后需要更新它的状态。

(2)

具体实现通过贝叶斯公式实现：

(3)

同时对两边进行对数运算：

(4)

含有测量值的项就只剩下了第一项，这个比值即为测量值的模型，一个点状态数值越大，就表示越肯定他是occupied状态，相反数值越小，就表示越肯定它是free状态。

在最终建成的地图中，地图上的栅格被分为三种，一是障碍物栅格，无法通过，对此类栅格赋值为99，二是未知栅格，对此类栅格赋值为-1，三是可行驶区域栅格，对此类栅格赋值为0，这样机器人就可以根据栅格的灰度值区分障碍物和可通行区域。相比于拓扑法，栅格法具有简单易于实现等优点，是目前解决路径规划问题最常用的环境建模方法[10]。

1.2 A*算法介绍

A*算法在机器人全局路径寻优和规划方面应用非常广泛。A*算法是一种启发式搜索算法，通过在传统的Dijkstra算法的基础上引入了启发式函数，使其相较于Dijkstra路径搜寻效率更高，是一种在静态路径规划求解最短路径最有效的方法[11]，其估价函数为[12]f(n)=g(n)+h(n)。其中h(n)为启发函数，值为n到目标点的估计代价，一般选取两点间的曼哈顿距离作为代价值，如下式所示：

h(n)=abs(xi-xj)+abs(yi-yj)

(5)

其中,点(xi,yi)为当前节点n，点(xj,yj)为目标节点。g(n)为从初始节点到当前节点n的实际代价，栅格地图中任意节点都有8个相邻节点，如图1所示。

图1 每一步所增加的代价值

规划的路径通过前后左右4个栅格则g(n)=g(n)+1，若通过斜向的4个栅格则g(n)=g(n)+1.4，从而得到g(n)的值。

1.3 A*算法实现流程

A*算法具体的实现流程图如图2所示。

图2 A*算法流程图

图中符号定义如下：

N为路径规划中已规划过的某一个节点；g(n)为初始节点s到n的实际代价；h(n)为启发函数，n到目标点的启发代价；f(n)—g(n)+h(n)，为节点n到目标点的估价函数；OPEN LIST为保存所有已生成而未考察的节点；CLOSED LIST为记录已访问过的所有节点。

1.4 实际环境中存在的缺陷

在实际的是室内环境中，情况会变得复杂。在大部分情况下A*算法在栅格地图上规划出的路径是理论上的最佳路径，但在规划经过门路径时，理论上的最佳路径经常需要机器人靠近障碍物，但在依靠激光雷达进行定位的机器人上，因为激光雷达在探测近距离时存在盲区,导致移动机器人在接近障碍物的时候，经常无法准确的进行定位，从而在原地旋转，导致导航的耗时增加，从而影响了导航的效率，而加入了膨胀系数后，虽然一定程度上缓解了上述问题，却会导致很多时候移动机器人在面对明显比自己要宽的门时无法通过，并且规划的路径还存在转折次数过多和转折角过大等问题。为了解决上述问题，本文提出了一种基于多层地图的分段式导航算法，通过对原始的栅格地图加入一次预处理的流程，并加入一层门点图层，以完成对路径规划的优化。

2 基于双层地图的分段式路径规划

2.1 对栅格地图的预处理

栅格地图具有简单易维护的特点，但因其结构的局限性，只能通过数字(1～100)来表示障碍物的代价值，而无法表示障碍物以外的物体类型，因而在原图层的基础上再引入一张图层，来表示地图上门的位置。首先，需要得到一张完整的环境的栅格地图，并对地图进行二值化处理，从而提取出地图上障碍物的形状。将二值化后的地图视为一个由0与255组成的列表，其中障碍物栅格的灰度为0，其余栅格灰度为255。并对地图上的障碍物进行空心化处理。分别从4个方向对二值化后的栅格地图进行缩进处理，C即为需要删除的点，1为障碍物栅格，0为灰度为255的栅格，空白处为任意栅格，如图3所示。

图3 栅格地图缩进示意图

最后将分别4张缩放所得到的地图进行叠加，得到最终的空心化后的栅格地图。

接下来进行对门点图层的制作。对空心化后的栅格地图进行处理。取空心化后栅格地图上灰度为0的任意点x，设点x的坐标为[i,j]，将，通过下列公式：

img[i,j+1]+img[i-1,j+1]+img[i-1,j]+
img[i-1,j-1]+img[i,j-1]+img[i+1,j-1]+
img[i+1,j]+img[i+1,j+1]=255

(6)

判断，式中的img[i,j]即为图像img中坐标为[i,j]的像素点的灰度值。再对符合条件的点进行配对，将最近的4个点进行匹配为一组4个元素的列表，通过设定一个阈值，即为每个列表中若有任意两点的距离大于阈值，则删除此列表，从而筛选出正确的门点，再将每个列表中的4个点分别连接，并将四边形填充为灰度0，从而得到门点图层。

2.2 对路径的优化

通过将表示门的图层与原始地图同时载入，可以将表示门的图层用于移动机器人的导航。首先，需要对门点的坐标进行处理，在现实环境中，门在地图中的朝向不确定，通过门的4个点的坐标来对原有的路径进行分段。设定地图中代表门的4个点的坐标分别为(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)。因为大部分情况下门的形状可以视为一个长方形来处理，且大部分情况下长方形的长边都是可以通过的边，而短边是不能通过的，因此，取四边形[(x1,y1),(x2,y2),(x3,y3),(x4,y4)]最短的两条边，假设所求得最短的两条边分别为[(x1,y1),(x2,y2)]与[(x3,y3),(x4,y4)]，设(x5,y5)与(x6,y6)分别为长方形的两条短边的中点，再通过公式：

(7)

可求得目标点的斜率k，再通过设定距离L，可以求得与点(x7,y7)在方向k上相距离分别为L与-L的两个点，即为所求目标点，其中点(x7,y7)为列表中4个点的中点,所需目标点由下列公式求得：

(8)

如图4所示。

图4 对门点的处理

在传统A*算法规划的路径上先计算规划出的路径在门点图层上的代价值，若当前规划路径在门点图层上的代价值为0，则表示该路径并没有经过门，这不需要启用路径优化，若该路径在门点图层上的代价值大于99，即当前规划出的路径经过了门，需要启用分段路径规划机制，并对当前的全局路径规划进行重新规划。通过对每个门周围的目标点距离为L的范围进行判断，若范围内存在轨迹，则将目标点添加至列表List中，并通过计算List中离目标点最近的各点在轨迹中的排序，对列表中的点依次进行排序，并对路径进行重新规划，将起点和中点分别加入列表的第一位和末位，并分别通过A*算法，将各个点按列表的顺序连接起来，生成新的路径，从而完成对路径的优化。

3 仿真分析

本次仿真实验的仿真环境基于开源的机器人仿真软件gazebo，机器人的控制平台则选用了Ubuntu16.04+开源机器人操作系统Ros Kinetic，建立的仿真环境如图5所示。

图5 仿真环境

通过基于2D激光雷达的Slam-Gmapping[14]算法建立出的环的栅格地图如图6所示。

图6 栅格地图

对所建好的栅格地图空心化后，如图7所示。

图7 空心化后的栅格地图

最终所获得的门点图层如图8所示。

图8 门点图层

通过Ros下的map_server节点将原始栅格地图与门点地图同时载入，其中原始栅格地图载入的话题名为/map，而门点图层载入的话题名为/map1，在可视化图形界面Rviz下显示如图9所示。

图9 Rviz界面

我们通过ros下的navigation下的导航包导航出的路径与我们优化后的路径进行对比，其中路径A为未优化的路径，而路径B为我们优化后的路径，对比如图10所示。

图10 结果比对

可见相比于原始路径A，优化后的路径B，在通过门的时候，不存在过于靠近障碍物的问题，从而避免了进入激光雷达的盲区导致的机器人定位丢失的问题。

4 试验验证与分析

4.1 试验软硬件设计

本次试验使用课题组自主研发的机器人平台，机器人采用单激光雷达进行定位建图与导航。所用的激光雷达传感器型号为SICK LMS111，激光雷达的探测角度为180°，角度分辨率为0.5°。控制器则采用了型号为intel nuc6i7KYK的mini PC，mini PC的cpu型号为intel core i7-6770HQ，内存8g。试验所用的软件平台为ubuntu16.04+ROS kinetic。试验用的移动机器人平台如图11所示。

图11 实验用的移动机器人

4.2 试验的结果及分析

通过纸箱子的摆放来模拟房间中的门，试验所用的室内环境如图12所示。

图12 实验环境

通过基于ros的google cartographer[15]算法对房间建图,对所得到的栅格地图进行处理，如图13所示。

图13 对栅格地图进行处理

将所得到的门点图层载入后，任意规划一条经过门的路径，最终得到的路径与未经过如图14所示。

(a) 全局路径规划 (b) 优化后的轨迹 图14 优化后的轨迹与传统A*算法实验对比

图14a中的轨迹为传统A*算法全局路径规划，图14b中的轨迹为经过优化后的轨迹，由实验结果可知，当规划经过门的路径时，算法检测到路径在门点图层上的代价值大于99，即路径经过门，从而对路径进行了重新规划。可以很明显的看到，生成的路径虽然相比原始路径要长，但对于机器人更加安全，机器人不会出现离障碍物过近的情况。

5 结论

针对机器人移动机器人通过传统A*算法路径规划经过门时因规划的路径靠近障碍物，导致定位丢失的问题，提出了基于双层地图的路径规划而的方法。传统的A*算法规划的路径距离最短(前提是路径存在)，但路径常沿障碍物边缘前进，在经过门时会因为激光雷达的近角盲区导致定位丢失甚至发生危险。基于多层地图的分段路径规划可以使移动机器人在经过门时不会离障碍物过避免了移动机器人因为离障碍物过近而无法定位的问题。首先对已经建立的栅格地图进行空心化，从而提取出栅格地图上的门的坐标，并对门进行填充，制作成门点图层，再将原始栅格地图与门点图层同时用于移动机器人的定位，当移动机器人规划的路径需要经过门点时，则启动路径优化机制，对原始路径进行优化。仿真实验与现实环境中的实验表明基于多层地图的分段路径规划规划出的路径可以使移动机器人不会离障碍物过近，降低了移动机器人在导航时丢失位置的几率，具有跟高的品质。