基于高效模板法与动态窗口法的服务机器人全覆盖路径规划方法①

陈超勇 熊禾根 陶 永* 邹 遇 任 帆 江 山

(*武汉科技大学冶金装备及其控制教育部重点实验室 武汉 430081)(**武汉科技大学机械传动与制造工程湖北省重点实验室 武汉 430081)(***北京航空航天大学机械工程及自动化学院 北京 100191)(****北京航空航天大学生物医学工程高精尖创新中心 北京 100191)

0 引 言

随着科学技术的进步和社会的发展，服务机器人逐渐进入人们的生活。清扫机器人作为服务机器人的典型代表，能够节省人力减轻劳动负担，已成为智能机器人研究的热点[1]。清扫机器人的核心技术是全覆盖路径规划，通过多传感器融合来估计自身位姿和检测环境信息，然后寻找一条在工作区域内从起始点到终点且经过所有可达区域的连续无碰撞路径，并要保证所规划路径的最优性或合理性[2]。全覆盖路径规划的研究在排雷机器人[3]、割草机[4]、水下机器人[5]、喷漆机器人[6]等方面同样具有重大的应用价值。

目前，全覆盖路径规划技术总体可分为随机式全覆盖和规划式全覆盖[7]。随机式全覆盖即机器人直行，发生碰撞后随机旋转角度继续直行，该方法虽然简单，但是存在重复率高、效率低的缺点[8]。因此，国内外的研究人员主要对规划式全覆盖进行研究。Bochkarev等人[9]以高度最小为准则将地图分割成2部分，在每个区域内利用往复运动实现全覆盖，但该方法仅作用于非凸多边形区域。Gajjar等人[10]提出了一种基于改进Boustrophedon单元分解法的全覆盖路径规划算法，为栅格地图定义了障碍代价函数，并随着机器人的移动自动更新。Ma等人[11]提出了一种基于生物激励神经网络的全覆盖路径规划算法，采用高分辨率栅格定位、低分辨率栅格路径规划的策略，提高了算法的执行效率。Wang等人[12]将蚁群算法和禁忌搜索算法进行融合规划最佳覆盖路径，取得了不错的效果。为了快速逃离死区，曹翔等人[13]提出了一种基于栅格信度函数的全覆盖规划算法。针对扫地机器人局部区域覆盖过程中存在遗漏区域未覆盖的问题，李楷等人[14]提出了一种基于回溯法的全覆盖规划算法。

以上方法在已知的静态环境中能发挥作用，但无法处理存在动态障碍的环境。最新的研究和应用表明基于传感器的方法对动态环境有着较好的效果。Li等人[15]结合有限状态机(finite state machine, FSM)和滚动窗口法实现了在未知环境中的全覆盖。Liu等人[16]提出了一种适用于多机器人系统的全覆盖算法，该算法大大减少了区域覆盖所需的时间。Zhou等人[17]提出了一种基于滚动窗口法和距离变换法的全覆盖路径规划算法，使用传感器检测动态环境并更新局部栅格地图，算法性能优于生物激励神经网络法。梁嘉俊等人[18]提出了一种改进的势场栅格算法，解决了人工势场法中清洁机器人在障碍物附近震荡而无法到达目标点、临近的障碍物之间不能发现路径等问题。但是基于滚动窗口法的全覆盖算法受到其窗口大小的限制，对于全局地图的整体路径规划表现并不好。而在改进势场栅格法中，机器人面对复杂的环境容易陷入死区，影响覆盖效率。

针对以上算法存在的问题，本文提出一种基于高效模板法与动态窗口法(dynamic window approach, DWA)相结合的服务机器人全覆盖路径规划方法。高效模板法通过预定义的模板与先验地图匹配，对整个环境进行全覆盖路径规划，同时引入回溯法解决高效模板法中机器人陷入死区的问题。运用2D激光雷达传感器信息与全局覆盖路径信息构建环境检测的动态窗口，遇到运动障碍物时调用动态窗口法实现局部的路径规划。清扫服务机器人的实验结果表明该方法简单有效，实现了动态环境下的全覆盖路径规划。

1 栅格法环境建模

在清扫机器人开始清洁任务之前，必须建立其工作空间的地图，即环境建模。栅格法可以很好地描述任何形式的障碍物，并且产生的二值信息特征便于计算机的存储和更新，所以选用栅格法进行环境建模具有显著的优势。

清扫机器人的工作区域存在很多障碍物，在地图中以灰色图形表示障碍物。栅格法将机器人的工作空间分为大小相同的栅格，其中灰色栅格表示被障碍物占据，白色栅格表示自由空间，如图1所示。

图1 栅格化地图

以2维的栅格地图为例建立其数学模型。图2显示的是一个2维的栅格地图，工作区域被分成了9个栅格，在该地图中，机器人的移动方向不再是任意方向，而是用八叉树所表示的8个行进方向。每个栅格都存储着一个值，这些值就组成了一个2维数组，此2维数组即工作环境的栅格模型。将障碍栅格赋值为1，可行域栅格赋值为0，则工作区域栅格模型如式(1)所示。

图2 工作环境示意图

(1)

2 基于回溯法的高效模板算法

模板算法是一种利用模板对静态已知环境进行遍历的算法，因其简单易实现的优点被广泛使用。1995年Hofner等人[19]提出模板算法，但文章中提出的模板算法不能应用于有障碍的环境，只对于该文中列举的一些环境具有实用性。随后De Carvalho等人[20]对模板算法进行改进，成功应用在有障碍的环境中，但是在一些特殊情况如遇到存在凹陷的障碍物时会陷入无法处理的死循环状态。针对现有模板算法存在效率低、陷入死区无法逃离等问题，本文提出了一种基于回溯法的高效模板算法，极大地改善了原算法中存在的问题。

原模板算法共引入5个模板，TM(toward marker)，UT(u turn)，SS(side shift)，UTI(u turn interlaced)和SCOO(steer clear of obstacle)，如图3所示。原模板算法简单易实现，但也存在一些缺点。例如：5种模板之间没有确立明显的优劣关系，导致在某些情形下会出现2种模板皆可使用的状况，这使得算法陷入“僵局”；SS模板产生了较高的重复路径；SCOO模板的避障方式容易使机器人遗漏掉障碍物的边缘区域；对于机器人陷入死区没有相应的解决策略。

图3 原模板示意图

为了解决原模板算法中存在的问题，提高算法的效率，本文重新定义了4种高效模板并引入回溯法来逃离死区。4种高效模板分别是TU(toward up)、TD(toward down)、TL(toward left)以及TR(toward right)，如图4所示。TU、TD、TL、TR模板表示上下左右4个方向，在进行遍历时，每个模板都有着确定的优先级顺序，本文制定的模板优先级从高到低依次为TL、TD、TU、TR。相较于原模板算法，高效模板算法仅通过4种模板就可以完成地图的全遍历，不需要单独的避障模板就可以避开中间障碍物，制定的优先级策略更是保证了遍历路径的规律性和有效性。

图4 高效模板示意图

高效模板的执行原理如下，对栅格地图进行区域遍历之前，要确定遍历的起始位置，一般选为地图的边界处。按照模板的优先级首先判断TL模板是否与当前环境匹配，即查看Left方向是否存在障碍物，如果Left方向是自由区域，则执行TL模板沿着Left方向移动直到被障碍物、已遍历区域或环境边界等阻挡。在环境与TL模板不匹配的情况下，会选择次优模板TD，若Down方向不存在障碍物，则执行模板TD沿着Down方向进行遍历。同时在移动的过程中会时刻检测TL与环境的匹配情况，若Left方向不存在障碍物，算法将会停止执行TD模板转而执行TL模板。TU、TR模板的执行则排在TL与TD之后。因此，高效模板在进行地图遍历的过程中，将按照指定的优先级顺序进行遍历，并且高优先级模板具有抢断低优先级模板执行的能力。

上述提出的4种高效模板对于静态区域的覆盖虽然具有良好的性能，但是无法解决机器人陷入死区的问题。原模板算法中的SS模板仅能解决机器人陷入角落而侧面有可行区域这一种情况，对于更为复杂的情况如周围是由已遍历区域和障碍物组成的死区，SS模板并不能逃离出去。因此，本文在高效模板中引入回溯法解决死区问题，以实现一次完成所有可行区域的全覆盖。

回溯法中至关重要的一点是回溯列表的构建。回溯列表是根据高效模板在执行过程中积累的信息构建的，在地图的遍历过程中，如果将地图中的所有未遍历自由栅格作为回溯点存储到回溯列表中，将导致回溯列表占用的内存极大。因此，本文为回溯列表中存放的回溯点制定了2条准则。准则1：距离最短原则。回溯点与死区之间的距离要尽可能小，以此降低二者之间的重复覆盖。准则2：靠边原则。回溯点应位于未遍历子区域的边界角落，以及减少机器人到达该点后产生新的未遍历子区域和回溯点。通过以上2条准则，可以大大减少回溯列表中回溯点的数量。

回溯列表中存储的是符合准则1、2的未遍历自由栅格，在机器人遍历过程中，陷入到4种高效模板都无法处理的死区位置，则启动回溯法来逃离死区，前往未遍历区域继续执行区域覆盖。选择回溯列表中的哪一个回溯点作为未遍历区域的起始点成为了影响回溯机制效率的主要因素。为了提高回溯法的效率，本文将机器人陷入的死区位置与回溯点之间的欧式距离作为贪心算法的选择策略，采用贪心算法在回溯列表中选择距离死区位置最近的回溯点，其数学描述为

(2)

(3)

其中，ns表示当前状态选择的回溯点，lback表示回溯列表，n表示lback中的回溯点，ndp表示死区位置，d(n,ndp)表示死区位置与回溯点之间的欧式距离。

图5和图6分别是对原模板算法和高效模板算法进行地图全覆盖的描述图。在图5所示的原模板算法中，UT模板的主导地位使得机器人的遍历方向是沿着障碍物的边缘向左侧推进，从而导致了机器人陷入了更靠近角落的死区A。而在图6所示的高效模板算法中，机器人按照TL、TD、TU、TR的优先级顺序执行模板，它倾向于沿着障碍物边缘移动，避免陷入障碍物与墙壁边界的角落位置，达到一个离未覆盖区域更近的C点。最后，由SS模板沿边移动到B点产生路径AB，由回溯法从C点移动到B点产生路径CB，而CB路径长度明显小于AB路径长度，这意味着高效模板算法的重复覆盖率更低，并且回溯法能够处理任何位置的死区问题。因此，相较于原模板算法，使用高效模板算法对静态区域进行覆盖的整体效率更高。

图5 原模板算法

图6 高效模板算法

在高效模板中引入回溯法可以使清扫机器人逃离死区，实现所有可行区域的全覆盖，完整的基于回溯法的高效模板算法流程如下所示。

输入：环境模型M。

输出：一条遍历所有自由栅格的全局路径L。

步骤1栅格化环境模型，确定遍历的初始位置；

步骤2依照地图中的环境特征匹配对应的高效模板进行运动，直到陷入无法逃离的死区位置；

步骤3根据环境信息更新回溯列表lback；

步骤4以死区位置和回溯点之间的欧式距离为贪心策略，利用贪心算法选择未覆盖区域的回溯点，并移动到该回溯点。

步骤5重复步骤2～4，直至回溯列表lback中元素为空，结束算法。

3 动态环境下全覆盖路径规划方法

3.1 动态窗口法

动态窗口法(DWA)将局部路径规划问题描述为速度矢量空间上的约束优化问题，是一种有效的局部避障算法[21]。DWA算法的主要思想是，根据机器人速度、加速度以及环境障碍约束，将速度矢量空间(v,ω)限制在一个动态窗口中，对此窗口内的线速度v和角速度ω进行多组采样；然后，预测每个采样速度在下一个周期内对应的机器人运动轨迹；最后，引入一个评价函数对预测的运动轨迹进行评估，选择得分最高的轨迹对应的速度来控制机器人运动。DWA算法的核心在于动态窗口的建立以及评价函数的选择，下面根据清扫机器人自身性能和环境的约束建立速度采样动态窗口。

清扫机器人受硬件性能的约束，其最大、最小速度都被限制在一定范围内，所以清扫机器人允许的极限速度矢量集合Vs为：

Vs={(v,ω)|v∈[vmin,vmax],

ω∈[ωmin,ωmax]} (4)

式中，vmax、vmin为清扫机器人的最大、最小线速度，ωmax、ωmin为清扫机器人的最大、最小角速度。

为了保护自身的安全，清扫机器人要与障碍物保持一定距离，并且在碰到障碍物前能够及时停止，因此，清扫机器人的安全速度矢量集合Va可根据下式确定：

(5)

根据清扫机器人的最大加减速能力，可以计算出某时刻采样速度在下一个周期Δt内的变化范围，用Vd表示：

(6)

将满足Vs、Va、Vd的速度矢量空间构建成动态窗口，可得到最终速度搜索空间Vr：

Vr=Vs∩Va∩Vd

(7)

对于采样的速度空间Vr，每一组采样速度对应的预测轨迹都满足约束条件，需引入评价函数评估轨迹的优劣性。最优轨迹应使清扫机器人在避开障碍物的情况下更快更准地向目标移动，因此本文采用的评价函数如下：

G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)

+γ·vel(v,ω))

(8)

式中，heading(v,ω)为清扫机器人轨迹终点的航向与目标点的夹角，dist(v,ω)为轨迹与障碍物的最小距离，vel(v,ω)为采样速度，α、β、γ为权重比。

3.2 基于高效模板法与DWA算法的全覆盖路径规划方法

前面提出了一种基于回溯法的高效模板算法，该算法能够实现已知地图下所有可行区域的全覆盖，但对于环境中未记录或突然出现的障碍物没有应对策略。因此，本节将高效模板算法的静态规划能力与DWA算法的局部避障能力结合起来，提出一种适用动态环境下的全覆盖路径规划方法。该方法可让清扫机器人在已知环境中静态规划出全局覆盖路径后再进行清扫，即使遇到环境中的动态障碍物也能顺利避开，大大提高了清扫机器人的工作效率。

本节将结合如图7所示的算法流程图，详细描述提出的全覆盖路径规划方法的具体实现步骤。

图7 基于高效模板法与DWA算法的全覆盖路径规划流程图

(1)建立清扫机器人工作区域的环境模型。利用激光雷达传感器扫描环境，生成包含障碍物的代价地图，然后根据机器人的实际尺寸确定划分的栅格大小，用于全局的遍历路径规划。

(2)高效模板算法对先验静态地图规划出可行的全局覆盖路径。首先确定清扫机器人在栅格地图的起始位置S，然后根据地图信息匹配相应的模板，并按照TL、TD、TU、TR的优先级执行模板，最后得到一条能够遍历所有自由栅格的全局总路径L。

(3)总路径分解。按照关键拐点将全局总路径L分解成若干条子路径集合P，这些子路径皆由简单的直线组成，易于清扫机器人的执行，而子路径的首末端点则组成了子目标点集合Q。P和Q的数学描述分别如式(9)、(10)所示。

P={L1,L2,L3, …,Ln}

(9)

Q={O0,O1,O2, …,On}

(10)

其中n的值与全局总路径L的长短有关，O0和On分别为全局总路径L的起点和终点，当清扫机器人到达On点时表示遍历完成。

全局总路径L分解得到子路径集合P以及子目标点集合Q，如图8所示。

图8 总路径分解图

(4)将子目标集合Q中的第一个点O0设为清扫机器人的工作起始点，则O0的下一个点Oi作为机器人在当前位置需要到达的子目标点。在步骤2、3中，已通过高效模板算法得到全局遍历路径L并将其分解为子路径集合P，因此集合P中的子路径Li作为机器人在当前位置规划出的子路径。以速度v0驱动机器人沿着子路径方向移动，并根据机器人目前姿态和相关速度约束构建的动态窗口。动态窗口是连续的，实时接收传感器数据并在地图上更新动态障碍物信息。当沿着子路径Li方向的速度v0不满足动态窗口要求(即Li与障碍物将发生碰撞)时，DWA算法会重新在动态窗口中采样，选择最优轨迹对应的速度v驱动机器人进行局部路径规划避开障碍物，其原理如图9所示。

图9 DWA局部路径规划避障图

在图9中，DWA算法的关键在于动态窗口实时检测环境信息，并判断子路径Li在动态窗口中是否与动态障碍物发生碰撞，若是，则规划局部路径进行避障，否则继续沿着子路径Li移动，这样的优点在于避免了用DWA算法进行长距离的全局规划，可完美发挥其局部规划的能力。

(5)当清扫机器人到达子目标点Oi时，将当前位置设为Oi，令i值加1，作为下一个子目标点。

(6)判断清扫机器人当前位置是否已到达集合Q中最终目标点On，若否，则继续迭代移动到一下个子目标点。若是，则算法结束，表示清扫机器人已经完成了全覆盖的清洁任务。

以上是本文所提的基于高效模板与DWA算法全覆盖路径规划方法的全过程，大致可以分为全局静态规划与局部动态规划。高效模板算法在进行全遍历的同时也解决了清扫机器人陷入死区的问题，生成的简单路径易于机器人执行，提高了算法的效率。与DWA算法的融合有效改善了当前全覆盖算法对未知动态障碍物无法避障的能力，增加了机器人的安全性能。接下来将通过仿真和实验联合验证本文所提全覆盖算法的正确性和有效性。

4 仿真与实验

4.1 实验平台

本文自主搭建的双轮差速清扫机器人实验平台如图10所示。机器人底盘的左右主动轮分别由带有减速器和编码器的直流电机独立驱动，构成双轮差分系统，前后再辅以2个从动万向轮。在机器人平台的中间层安装有2维激光雷达传感器、STM32控制板、电源模块以及Intel NUC迷你主机，上层放置1块用于人机交互的显示屏。在本实验平台中，上位机通过RS232串口与STM32控制板进行通讯，控制左右电机的转速并获取编码器的里程信息，而激光雷达的数据则直接传给上位机处理。实验中清扫机器人规格及运动参数如表1所示。

图10 清扫机器人平台

表1 清扫机器人参数配置表

4.2 全覆盖路径规划仿真与实验分析

本文在如图11所示的实验环境验证所提出的基于高效模板与DWA算法的全覆盖路径规划方法。该实验场地由多条走廊和过道组成，可行区域面积约为230 m2，且整个场地被墙体隔断成多个连通区域，具有代表性和普遍性。实验中将通过行人来模拟地图中的高动态障碍物，以验证算法的局部动态性能。

图11 多走廊和过道组成的大面积实验环境

本文使用安装在机器人上的2维激光雷达传感器扫描图11中的环境，通过SLAM[22]技术建立环境的2维栅格地图，如图12所示。图12中数字标记的区域是门或电梯口等地方，限于机器人尺寸，本文将这7处区域从可行区域中除去，作为清扫机器人的工作空间。

图12 实验场景的SLAM地图

Stage是机器人操作系统(ROS)下的一个小型仿真器，能够模拟激光雷达数据且具备物理引擎，因此，本文将stage模拟器作为仿真平台。将SLAM地图导入stage平台中，配置机器人相关参数，得到仿真环境如图13所示，图中位于地图下方的方块表示机器人的初始位置，位于地图中间的方块模拟地图中未记录的动态障碍物。选取地图中间的一个通道来验证算法的局部动态性能，假定模板算法与本文所提算法规划的静态覆盖路径同为MN。从图14可以看出，当清扫机器人前方突然出现地图中未记录的障碍物时，模板算法直接撞向障碍物，而本文提出的算法根据传感器信息检测到障碍物，依靠DWA算法重新规划路径，并调整速度避开该障碍物。

图13 Stage仿真地图

图14 模板算法与本文算法动态性能对比图

本文所提算法对静态地图规划的全局覆盖路径如图15所示。根据关键拐点将其拆解成若干条子目标路径，然后清扫机器人按照子目标路径进行清扫任务，图16是机器人工作时的覆盖轨迹图。图16(b)、(d)分别模拟了行人从门内、电梯口突然出现的情况，由图16(c)、(e)可知机器人检测到了行人，立即调用DWA算法规划出了一条局部路径来绕开行人，随后回归到全局子路径上。图16(f)中机器人陷入死区，此时回溯法起作用，选择最近的回溯点并移动到该点，如图16(g)所示。

图15 静态全局覆盖路径

图16 清扫机器人覆盖轨迹图

通常情况下，覆盖率和重复率是衡量全覆盖算法性能的重要指标。覆盖率是机器人已覆盖面积占总面积的百分比，重复率是指机器人执行覆盖任务时，重复覆盖面积占总面积的百分比。表2是本文算法在仿真环境下的覆盖实验结果，通过表中的数据可知在发生2次避障的情况下，本文所提的全覆盖算法的覆盖率高达98.3%，重复率仅为2.6%，该结果表明本文算法在高动态环境下的全覆盖路径规划中有着良好的表现。

表2 清扫机器人覆盖实验结果

以上是在stage仿真器中的测试结果，下面进行实际场景中清扫机器人的工作演示。图17是清扫机器人在图11环境中的实验结果图，图中的覆盖轨迹以及避障情况与仿真结果一致，再次验证本文所提算法的正确性和有效性。

图17 清扫机器人实验结果图

5 结 论

本文提出了基于高效模板算法和DWA算法的清扫服务机器人的全覆盖路径规划方法。该方法首先提出基于回溯法的高效模板算法，解决了原模板算法中效率低、无法逃离死区的问题；然后利用高效模板算法对先验静态地图规划全局覆盖路径，将全局覆盖路径分解成子目标路径后与传感器信息融合构成动态窗口，使机器人沿着子目标路径进行清扫任务，窗口检测到地图上未记录的动态障碍物时调用DWA算法规划局部路径进行避障；最后，通过仿真和实验验证了本文算法在动态环境下全覆盖路径规划的正确性和有效性。目前，本文提出的全覆盖路径规划方法在逃离死区时会产生一定的重复路径，以后会针对该问题进一步提高算法的性能。