基于骨架地图的全覆盖路径优化方法

穆莉莉,史 程,王 超,单卓佳,刘帅帅

(安徽理工大学 机械工程学院,安徽 淮南 232000)

扫地机器人是一种智能化的机器人产品,全覆盖路径规划是机器人中最为重要的内容之一,决定着清洁效果和工作效率[1],常用的全覆盖路径规划算法有栅格法、内螺旋法、模板规划法等,总体来说,扫地机器人覆盖率低、重复率高、工作效率低[2]。扫地机器人工作场景复杂,静动态障碍物多,面对多障碍物或突然出现的动态障碍物,如何获取最佳清扫路径,是目前国内外研究机构的研究热点[3-4]。对全覆盖路径规划算法的进一步优化提升是当前扫地机器人中的重要研究方向。

对全覆盖路径规划的优化有两种途径,一是可以从路径生成方法上优化,二是可通过地图优化及地图分割方式去规划出更加合理的路径。目前,基于内螺旋搜索的生物激励路径规划算法已经能够取得较好的效果,其重复率低、拐点数较少且适应性强[5]。另外,使用全局路径规划与局部实时避障路径规划相结合的导航技术能够提高路径规划的合理性[6]。

通过更加精细的地图分割和优化,也可提升路径规划的合理性,使扫地机器人的清扫工作更加高效。目前基于激光导航的扫地机器人已经可以建立较为精细的室内地图,这为全局路径规划提供了良好的基础[7]。如elek等[8]针对已知环境开发了一种覆盖算法,其想法来源于生成树覆盖算法(STC)。然而,该算法的线路含有过多的拐点,这给作业效率带来了影响;Le[9]等设计了一套全覆盖路径规划算法,该算法基于螺旋生成树原理并可降低覆盖率重复率。然而,该算法的规划路径长度较长,拐弯次数显著增多。当面临密集零散障碍区域时,机器人通常会陷入死区,导致效率显著下降。许伦辉等[10]基于分治思想对二维栅格地图进行分割,在分割后的区域分别进行路径规划,单一环境下可通过分区域规划路径的方式,降低全覆盖算法的复杂度,但该方法分割方式相对简单,对于不规则形状的工作环境很难做出有效分割;王红星[11]等针对凹多边形区域,提出一种区域覆盖算法,把凹多边形转化为凸多边形,进行区域划分后对每一个区域进行路径规划,在空旷的大场景下可规划出复杂度低的路径,但该算法不适用于有复杂障碍物的室内环境,容易受室内障碍物的影响从而无法进行地图分割。

本文提出一种基于骨架抽取的地图优化方法,通过降低全覆盖路径的复杂程度,提高扫地机器人的清洁率,降低扫地机器人的清洁重复率。

1 二维激光SLAM地图构建与路径规划

1.1 二维激光SLAM地图构建

结合实际工作环境以及机器人的算力,首先选择Gmapping算法来进行室内地图构建,在此基础上对其进行路径规划研究。Gmapping算法以RBPF方法为基础,对提议分布进行了改进,并且提出了选择性采样的方法。

1.1.1 RBPF

机器人通过观测信息和里程计数据来获得其位姿和地图,RBPF算法对概率密度函数进行因式分解,首先估计位姿,然后计算栅格图。即拆开先求其中某一个变量,在解决另一个变量之前,先解决此变量,计算方法如式(1)所示。

P(X1:t,m|Z1:t,u1:t-1)

=P(m|X1:t,Z1:t)P(X1:t|Z1:t,u1:t-1)

(1)

式(1)中,P(X1:t,m|Z1:t,u1:t-1)是定位部分,P(m|X1:t,Z1:t)P(X1:t|Z1:t,u1:t-1)是建图部分,用前者作为后者的条件从而可以得到栅格地图。

1.1.2 提议分布

为了获取下一个时刻扫地机器人的姿态,从提议分布中进行采样。当建议分布与目标分布非常接近时,可以直接在目标分布中进行采样,这样只需要使用较少的粒子就能获取机器人的位姿。具体方法为:首先通过机器人的运动模型得到粒子数量;然后通过激光雷达的观测信息给粒子加权,从而得最佳粒子;最后用较大权重的粒子对拟议分布进行模拟改进。通过高斯函数来建立提议分布,使用K个数据模拟峰值并生成高斯函数作为提议分布,如式(2)所示。

(2)

(3)

式(3)中,用有限K个数据积分近似转变成求和,然后进行权重计算。

1.1.3 选择性重采样

针对频繁采样过程导致粒子退化和粒子多样性减少问题,通过设置阈值控制重采样的次数来解决,当粒子权重比阈值大时,则进行重采样否则不执行重采样。

使用Gmapping算法对自主搭建的仿真环境进行建图,建图效果如图1所示。

1.2 路径规划

室内扫地机器人工作空间封闭,且区域多网格化,首先选用传统的内螺旋式路径规划方法进行效果测试,如图2所示。

图2 螺旋算法示意图

此处使用的地图为栅格地图,把遍历过的栅格定义为已知栅格,未遍历的栅格为自由栅格。为了让扫地机器人在运动中更加高效,将其行走过程分成三种行为向左旋转90°、直行、向右旋转90°,并优先级递减。根据这个优先级,扫地机器人会选择下一个要遍历的栅格,并标记已经遍历过的栅格。在扫地机器人工作时,如果它在前方遇到了边界,并且在左侧或右侧的栅格中,出现了障碍物或该区域已被清扫,则机器人会进入一个死角状态。在这种情况下,扫地机器人会倒退沿着原始路径,并按照预设的行走优先级进行搜索,直到所有的栅格都被完全清扫为止。这个过程类似于回溯法,以确保每个区域都被覆盖到,从而实现全覆盖清扫。

对仿真环境的栅格地图进行全覆盖路径规划,规划效果如图3所示。

图3 路径规划效果

由图3可知,全覆盖路径因为极个别的黑点存在,近似的将地图分为多个网格区域,改变了全覆盖路径规划的方法,产生很多拐点,增加了路径的复杂程度。

针对上述情况,本文提出一种地图优化算法,对激光雷达采集的地图首先进行滤波处理,根据扫地机器人的工作环境特点,设置阈值,有针对性地滤掉较小离散点,接着对处理后的地图进行骨架抽取处理,去除模糊边界,最后在该地图上进行路径规划。

2 地图优化算法

2.1 地图开运算处理

因为室内环境中会有动态物体存在,所以会使扫地机器人构建的室内二维地图有噪点存在。对路径规划有很大的干扰,因此需要首先对这些噪点进行滤波处理。

本文采用图像处理中的开运算方法来进行滤除。进行开运算前首先对地图进行二值化处理,把地图边界轮廓的像素单独提取出来。为了能够完整的提取出所有能反映地图轮廓的像素,通过OTSU算法确定图像二值化分割阈值。

设灰度图像(x,y)有N个像素点、L个灰度级。选取全局阈值Tg,将f(x,y)分为C0和C1两个类型,C0是由f(x,y)在灰度级[0,Tg]内的像素点所组成的,C1是由f(x,y)在灰度级[Tg+1,L]内的像素点所组成,由此C0和C1的类间方差可以表示如式(4)所示。

σ=μ0(m0-m)2+μ1(m1-m)2

(4)

式(4)中,μ0和μ1分别是C0和C1两类的概率,m0和m1分别是C0和C1两类的灰度均值,m是整幅图像的灰度均值,在L级的灰度范围内,采用遍历的方法,选取出当类间方差最大时所对应的阈值Tg,即为相应的全局阈值。

图像二值化处理后,进一步对图像孤立小点和小桥进行滤除,对地图进行开运算处理。即先对二值图像做腐蚀运算,再做膨胀运算。

用结构元素B对A做腐蚀运算,表示为A⊖B,其式如(5)所示。

A⊖B={Z|(B)z⊆A}

(5)

式(5)中,BZ={c│c=b+z,b∈B},指结构元素按照z=(z1,z2)在图像上平移,即在图像A上,B中坐标值(x,y)被(x+z1,y+z2)替代。腐蚀运算式的含义为:将结构元素B相对于A进行平移,只要结构元素都包含在A的前景像素点中,则这些位移Z的像素点的集合为腐蚀结果。

用结构元素B对图像A做膨胀运算,表示为A⊕B,其式如(6)所示。

A⊕B={Z|[(B)z∩A]A}

(6)

开运算式如式(7)所示。

A0B=(A⊖B)⊕B

(7)

腐蚀后的图像变化特征和选取的结构元素及其原点有关,当结构元素为3×3大小的矩形且选其中心为原点时,进行腐蚀处理后图像的前景会缩小一个像素宽,膨胀处理后图像背景会扩大一个像素宽,如图4所示对地图进行腐蚀,图4浅色部分即为选定的正方形结构元素,以此结构元素图4(b)对图4(a)进行腐蚀,从图4(a)的边界出发,由外向内对该像素群进行腐蚀操作,最后得到图4(c)所示的结果。

图4 腐蚀操作

因为是对图片全部像素进行腐蚀,所以此处使用明克夫斯基差的全局定义形式如式(8)所示。

fΘg=▽{fx+g^(x):x∈D[g^]}

(8)

式(8)中,g^为g对原点的反射,即先对g进行纵轴反射求得个g(-x),然后再对g(-x)进行横轴反射求得-g(-x)。

膨胀操作为腐蚀操作的对偶运算,采用全局膨胀方式进行,膨胀全局定义形式如式(9)所示。

f⊕g=Δ{fx+g(x):x∈D[g]}

(9)

式(9)中,g(x)为g对原点的反射,即先对g进行纵轴反射求得个g(-x),然后再对g(-x)进行横轴反射求得-g(-x)。

使用以上方法对图1所得到的地图进行腐蚀处理,为了使滤波的效果更好,采用多次腐蚀膨胀处理,迭代三次,得到最终的地图文件如图5所示。

图5 开运算结果

2.2 骨架抽取

经过滤波处理后,虽然过滤掉了大部分的噪点但是地图边界轮廓仍不够清晰,工作特性仍不够友好,因此对图5进行骨架抽取,以提升地图质量。白色像素定义为0,黑色像素定义为1。设图片中任一点像素值为X,首先判断其是否为黑色像素点,若为黑色像素则寻找其八邻域点,分别依次定义为P1～P8,顺序如图6所示。

图6 八邻域点

由图6可知,P2、P4、P6、P8分别为该像素的垂直邻域点,P1、P3、P5、P7分别为交叉邻域点,通过判断这两类邻域点像素值,来确定该像素x是否需要被处理。该像素的垂直邻域点和交叉邻域点分别代入式(10)和(11)中。

(10)

(11)

若该像素点满足2.1和2.2的条件,则被视为非骨骼点,被滤除。

图7中,每一个方格表示一个像素,图7(a)为原图,对图7(a)进行骨架抽取,结构如图7(b)所示,剥离了最外面一层的像素。

图7 骨架抽取

使用该算法对经过滤波处理的地图进行处理,得到如图8所示的结果,可见地图的轮廓更加清晰简洁,在骨架抽取的同时也过滤掉了一些不需要的噪点。

图8 抽取后地图

路径规划结果如图9所示,因为没有细小黑点的干扰与原始地图全覆盖路径规划的效果相比,拐点更少,覆盖率更高,提高了扫地机器人的清洁效率。

图9 路径规划效果

3 实验及结果分析

3.1 机器人实验平台搭建及建图

为了验证该方法的可行性和实际工作效率,自主搭建了机器人实验平台,如图10所示。机器人实验平台为差速运动结构,已通过建模的方法对其路径偏差进行纠正,底层运动控制芯片为stm32f103rct6,主控处理器选用Jetson TX1,内部搭载Ubuntu18.04操作系统,激光传感器使用RPLIDAR A1。

图10 实验机器人及实验室

首先对实验室进行地图构建,如下图11所示。可发现,原始地图中有较多黑色噪点。

图11 实验室原始地图

3.2 实验结果及分析

使用传统的内螺旋式全覆盖路径规划,规划结果如图12所示。

图12 原始地图规划结果

由图12可知,地图上有大量离散黑点。在机器人建图的时候,实验室内有人在周围走动,当障碍物在机器人经过的全过程中一直存在,则会被认定为障碍物,即使机器人离开之后,该障碍物依然存在于地图中。

对本算法处理后的地图进行路径规划,结果如图13所示,可见轮廓修整得更加平滑,且滤掉了地图上影响全局路径规划的细小噪点。由图易知,该路径更加规整,且覆盖范围更广,使用机器人按照上图算法分别运行30次,对测得数据取平均值,并整理如表1所示。

图13 骨架地图全覆盖路径规划

表1 实际清扫指标

由表1可知,在本文处理后的地图上进行路径规划,路径的长度平均减少了13%,而且全局路径的拐点数相对于原始地图减少了15.64%,因为使用的导航算法有局部路径规划实时避障,所以碰撞次数基本不变。本文优化后的地图,对小型障碍物和移动的物体进行滤除后,实际清洁率有了明显提高,由原有的91%提高至98%,满足室内清洁的需求。

4 结论

本文针对扫地机器人全覆盖路径算法路径拐点多,效率低的问题,提出了一种基于地图优化的扫地机器人全局路径规划算法,其通过抽取二维栅格地图轮廓的骨架,及使用迭代腐蚀膨胀对地图进行滤波处理,获得了更高效更平滑的覆盖路线。经实验验证,全局路径拐点次数明显减少了15.64%,实际清洁率提高至98%。