融合波前边缘检测与快速搜索随机树的自主探索方法

张淑珍，马玉祥，侯致远，查富生，何 镇

（1.兰州理工大学 机电工程学院，兰州 730000；2.哈尔滨工业大学 机器人技术与系统国家重点实验室，哈尔滨 150001）

移动机器人已广泛应用于工业、农业和军事等领域。从搜索救援到行星探测[1]，未知环境下移动机器人自主探索成为机器人领域的研究热点之一[2]。如何在时间短及路程少的条件下通过感知环境决定最佳探索目标点并移动至该点，进而完成移动机器人所在未知环境的遍历，得到足够的信息用于全局地图创建[3]，是自主探索及地图创建问题的核心，也是机器人实现真正自主，智能化的基础性技术。

Umari H.[4]提出将快速搜索随机树（Rapid Random Tree,RRT）用于边界检测，并设计出局部RRT 前沿点检测器与全局RRT 前沿点检测器混合的搜索策略。金毅康[5]在传统RRT 快速搜索随机树算法上，以其生长的端点作为广度优先搜索（Breadth First Search,BFS）的起点搜索边界点，从而提高边界的获取效率。Sun Xuehao[6]提出一种利用RRT 算法搜索边界点，以距离代价、障碍物代价为判断依据的性能指标函数，完成室外地面环境探索。RRT 算法偏向于覆盖未知区域，虽然能够实现探索边界检测的功能，但由于其生长具有随机性，狭窄区域通过性较差，存在着边界点提取不全面的问题，同时在提取边界点或是树枝生长过程中，需不断检查树枝与障碍物及边界之间的关系，一定程度上降低了边界点提取的效率。Keidar M.[7]提出了两种快速检测边界的方法，分别为快速前边缘检测（Fast Frontier Detector,FFD）与波前边缘检测（Wavefront Frontier Detector,WFD）算法，其中FFD方法只处理实时接收到的新激光数据，进行探索边界的提取，同样Sun Y.X.[8]等将栅格地图与激光数据结合起来探索前沿区域，这种依赖于激光雷达数据的算法对传感器硬件的精度有较高的要求，这在一定程度上限制了应用范围，并且由于激光点在极端范围内发散，此类算法提取激光数据构成的边界轮廓时将跨越未知区域，难以覆盖应被检测为边界的所有单元格。虽然Phillip[9]针对上述问题提出边界跟踪式边界检测（Frontier-Tracing Frontier Detection,FTFD）算法使用前一时刻边界和传感器包络的端点作为当前时刻寻找新边界的起点，提高了检测边界的准确性及完整性，但适用范围还是受限于激光雷达的精度。WFD 方法则是只处理已经探索过的栅格地图进行边界的提取，在建图前期具有较快的收敛速度，但随着地图的完备，需要遍历的栅格数量变得庞大，使得边界提取速度不断上升，针对该问题Phillip[9]提出的扩展波前边缘检测（Expanding-Wavefront Frontier Detector,EWFD）算法在WFD 算法的基础上对提取的边界加入时间标签，使用前一个时间步长的边界作为起点，在未知环境中搜索边界，减少了遍历地图栅格的数量，但其运行时间仍与已知区域大小成正比，且因时间标签的加入而耗费大量的内存空间。

本文针对已有探索候选点算法提取耗时久、不全面，探索目标点引导机器人大角度转向等问题，对传统WFD 算法进行改进并与快速搜索随机树方法结合，分别作为局部候选点与全局候选点的提取方法。同时将萤火虫算法轻简化后，用于提取众多探索候选点中的关键点，并在传统评价函数中引入角度代价，提出一种基于改进的WFD 与RRT 融合的自主探索方法。

1 自主探索及地图构建系统框架

未知环境下自主探索及地图构建主要包括探索目标点的提取和地图实时构建。自主探索算法提取探索目标点传递给定位导航算法，确定运动路线，采用SLAM 算法获取感知传感器的数据构建局部栅格地图，随着探索目标点的不断生成，引导机器人遍历环境，最终实现未知环境的全局地图构建。自主探索及地图构建系统如图1 所示。

图1 自主探索及地图构建系统框图Fig.1 System diagram of autonomous exploration and mapping

本系统利用ROS 平台[10]编写提取探索目标点模块中的相关节点，即改进的WFD 局部候选点提取、RRT 全局候选点提取、轻量化萤火虫聚类、改进的评价函数等。基于GMapping 算法接收激光雷达数据，用以构建二维栅格地图，探索过程中利用ROS 集成的move_base 功能包获取里程计信息和探索目标点，继而进行路径规划[11]，不断迭代直至探索完成。

2 探索候选点提取

2.1 基于改进波前边缘检测算法的探索候选点提取

Keidar M.[7]提出的传统WFD（Wavefront Frontier Detector）算法只遍历已探索的地图，以机器人当前的位置为初始栅格利用广度优先理论提取边界点，通过检测边界点周边相邻单元格，快速提取边界。该算法中存在三个队列与一个集合，分别为待检测队列Qc、边界点队列Qp、边界队列Qf、边界集合Sf{Qf1,Qf2,…,Qfn}，算法运行时将初始栅格Xr放入Qc中，搜索Qc中栅格的四邻域，若不存在未知栅格，表示该栅格不是边界点，将其四邻域放入Qc中，不断迭代，直至发现四邻域存在未知栅格的边界点，并将其放入Qp中，进而搜索Qp中边界点的邻域，将连续的边界点存入Qf中，构成一条边界，添加至Sf中。在建图初始阶段该算法确有较高的提取效率，但随已探索区域扩大，Qc中储存的元素增多，算法收敛时间不断增加，且WFD 算法提取边界时，需要为边界点附上标签以避免重复搜索同一边界点，因此加剧了内存占用量，对于未知不确定区域范围的搜索空间，如何在搜索区域扩大的情况下，保持较高的边界点提取效率，是提高探索效率的前提。针对该问题，本文对传统WFD 算法进行改进，通过限制搜索范围并对边界点归类进行简化操作以提高算法提取效率，将改进方法称为WFD_L（Wavefront Frontier Detector Limit），改进思想如下：

(1)在Qc队列元素筛选条件中加入距离条件。当搜索Qc中栅格的四邻域不存在未知栅格，且满足与初始栅格的欧式距离小于搜索阈值时，再将四邻域栅格加入Qc中，以减少储存元素的数量。图2 为距离限制条件示意图，图中F、B 为WFD 算法搜索边界点时所要遍历的栅格，R 为当前机器人位置即初始栅格，ε为搜索阈值，可以看出B 的四邻域栅格B1、B2、B3、B4的栅格状态皆为空闲，且，可将B1、B2、B3、B4存入Qc中，而F 的四邻域栅格虽然都不是未知栅格，但与初始栅格的距离大于ε，因此将F1、F2、F3、F4不存入cQ中。该策略使得WFD_L 算法在已探索区域增大的情况下仍然保持较低的收敛时间，提高边界点提取效率。

(2)去除搜索Qp中元素邻域，将连续的边界点存入Qfn中，并添加至Sf集合的步骤，减少算法的计算量，降低主控单元计算负荷。

基于WFD_L 算法的候选点提取步骤如下：

步骤1：以机器人当前位置为初始栅格，利用四邻域法判断该栅格周围栅格是否存在未知栅格，若不存在未知栅格则将其四邻域放入待处理队列中。

步骤2：若待处理队列中存在数据，则检测其中每一数据的四邻域是否存在未知栅格，若存在则将其对应的数据移动到边界点队列，若不存在未知栅格，进行步骤3。

步骤3：判断四邻域栅格与机器人当前位置的距离，当小于一定阈值时，放入待处理队列中。

步骤4：重复步骤2 与步骤3 直至待处理队列中无元素存在，则表明机器人当前位置处的边界点提取结束，返回边界点队列中的所有边界点。

如图3 所示为仿真环境下WFD 算法与WFD_L算法在探索地图时提取边界点所耗费时间对比，从图中可以看出，WFD 算法迭代一次的时间随着已知地图的不断扩大而持续增加，探索环境越大，则整体搜索效率随着WFD 算法提取时间的上升而降低。而WFD_L 在提取边界点时，通过距离条件改变栅格进入待处理队列的规则使得遍历范围确定，将时间固定在2 ms 左右，不随已知栅格的增多而上升。

图3 WFD 与WFD_L 提取边界点消耗时间Fig.3 Time consuming of extracting frontier points by WFD with WFD_L

2.2 引入全局视角的探索候选点提取

基于WFD_L 算法提取局部候选点具有较高的效率，但未考虑全局最优，因此需融合一种具有全局视角的边界点提取算法，避免机器人陷入局部陷阱。本文将搜索全局性、随机性较好的随机生成树数据结构（RRT）[12]用于全局候选点提取。

融合算法提取候选点的步骤如下：

步骤1：初始化RRT 树的根节点即机器人初始位置，使RRT 树枝生长，同时启用WFD_L 算法获取局部候选点，最大化维持探索候选点提取的效率。

步骤2：若局部候选点数量大于某一阈值时，将其数据信息传递给聚类模块进行处理，反之将RRT 生成的全局候选点信息传递给聚类模块。

步骤3：若全局候选点与局部候选点不存在或小于某一阈值时表示地图探索完成，停止自主探索并保存地图。

3 最佳探索目标点选取

传统最佳探索目标点提取，对所有边界点进行评价函数处理，得到指标最优的边界点作为探索目标点，增加了主控单元的计算负荷与算法收敛时间。为减少计算量，本文先对探索候选点进行聚类，再输入评价模块中，选出最佳探索目标点。

3.1 基于轻简化萤火虫算法的候选点聚类

常用的聚类算法有K 均值聚类、均值聚类及萤火虫算法等。K 均值聚类需要提前知道类别数量，均值聚类则是寻找每一部分圆形滑动窗口中密度最大的中心点，无法适用于线性分布居多且无种类之分的边界点。萤火虫算法[13]通过自定的目标函数决定自身亮度，在边界点聚类中可以将信息增益与路径代价作为评价标准，更好地剔除每一区域较差的探索候选点，适用于线性分布较多的探索候选点聚类。但该算法无法保证自主探索的效率，针对该问题本文将萤火虫算法嵌入自主探索系统，对大量探索候选点进行实时聚类时，建立目标函数并进行轻简化处理，轻简化思想如下：

(1)将边界点的信息增益与路径代价综合考虑，构建萤火虫算法的目标函数即自身光强I0如式(1)，其中0r表示当前候选点与机器人的距离，作为指数能够避免路径代价无限制地增大，γ为吸收系数，S 表示当前候选点的信息增益，其计算时以边界点为圆心，传感器测量距离为半径，统计圆内的所有未知栅格的数量。

(2)将轻简化萤火虫算法的迭代次数设定为1，并保持感知范围内最亮萤火虫的位置不变，避免多次迭代导致探索效率下降，得到局部最优个体后，将剩余边界点剔除，达到释放缓存的同时，降低评价模块的计算负荷。

轻简化萤火虫算法的运行步骤如下：

步骤1：计算群体中每一萤火虫（候选点）的自身光强I0如式(1)。

步骤2：计算影响范围内相比较的两个萤火虫之间的相对光强Ir，如式(2)，通过判断Ir的大小杀死二者中相对光强较小的萤火虫。

其中r表示相比较萤火虫之间的距离。

步骤3：重复步骤2 遍历搜索萤火虫，最后返回存活的萤火虫，作为聚类后的局部最优解。

如图4(a)所示红色边界点是未经过轻量化萤火虫算法聚类的边界点，数量密集庞大，对评价函数的计算造成较大的负荷，而图4(b)中黄色的边界点是经过轻量化萤火虫算法处理后的结果，在每一局部区域剔除影响因子较小的边界点，保留相对最优的边界点，在保证边界信息完整性前提下，减少了控制单元不必要的计算量，提升最佳目标点的选取效率。

图4 边界点聚类前后对比Fig.4 Comparison of before and after frontier point clustering

3.2 具有角度代价的评价函数

评价函数通过计算每一探索候选点的探索收益，选择最优值，确定最佳探索目标点。已有评价函数只包含信息增益与路径代价，可以选出未知邻域大且需行程少的探索点，但该点引导的运动趋势可能与机器人当前姿态呈现较大夹角，致使机器人导航过程中频繁旋转，加剧导航成本，因此本文引入角度代价，即以机器人坐标系为基准计算最佳目标点与机器人位置连线的倾角，并与机器人当前姿态做差值，解决机器人因大曲率转向导致探索低效的问题，本文称之为评价函数_IDA（Information Distance Angular）。计算步骤如下：

步骤1：获取机器人当前世界坐标系下的姿态，并计算机器人当前位置点与被评价候选点连线的倾斜角。

步骤2：分类讨论倾斜角与机器人姿态之间的关系，确定出候选点与机器人姿态的角度差值。

如图5 所示，直角坐标系O-xOy为机器人坐标系，P1,P2,P3,P4为各个象限中的任意候选点。处于机器人坐标系中第二、三象限的P2、P3点不能只通过与原点连线的斜率来计算相对机器人的偏转角，需采用其补角作为相对机器人的偏转角，因此将机器人姿态与候选点间的偏转角关系进行分类讨论。角度代价的计算公式为：

图5 目标点相对机器人坐标系位置关系Fig.5 Position relationship of target points relative to the robot coordinate system

(a)当候选点处于坐标系的第一与第四象限时δ=0 ；(b)当候选点处于第二象限时δ=1 ；(c)当候选点处于第三象限时δ=-1，其中(xi,yi)为候选点坐标，(xr,yr)为机器人坐标，yaw为只考虑机器人绕z轴的姿态信息，其角度范围为(0，π)与(0，-π)。

步骤3：对上述三种因素采用适宜的权重进行线性组合完成评价函数_IDA 的设计，评价函数计算公式为：

其中R为探索收益，μ,ε,τ为权重值，I为信息增益，d为距离代价，θ为角度代价。

4 仿真与实验

4.1 最佳目标点选取实验

为验证角度代价对提高机器人探索效率的效果，在40 m×40 m 大小的仿真环境下进行了两组实验，其中候选点提取方法统一为传统WFD 算法，目标点选取函数分别为传统评价函数与评价函数_IDA。

虽然传统评价函数能够在探索过程中不断产生目标点，直至结束，但由于该函数基于贪婪策略，选取每一目标点都是朝向未知区域最多，相对距离较近的边界点，而没有考虑机器人当前姿态，所以导致机器人在探索过程中多次大角度转向，即机器人转向目标点方位的角度大于π/2，从而降低探索效率，如图6(b)黄色框线内就是机器人大角度转向的运动行为。而评价函数_IDA 在考虑信息增益与路径代价的基础上选取目标点，控制机器人转向角度，减少机器人大角度转向，如图6(a)显示机器人的运动轨迹较为平滑，基本没有大角度转向运动行为，从而提高机器人在未知环境的探索效率。

图6 仿真实验Fig.6 Simulation experiments

从图7 可以看出，经过传统评价函数提取后，有50%的目标点引导机器人进行大角度转向；而经过评价函数_IDA 提取后，机器人大角度转向的概率为10.00%，相比于传统评价函数降低了38.89%。

图7 机器人转向角分布图Fig.7 Distribution of robot steering angle

为了消除实验结果的偶然性，本文基于两种评价函数在上述仿真环境下分别进行10 次目标点选取实验，从转向角、时间、路径三方面比对评价函数_IDA与传统评价函数的优劣，并计算对应平均值。如图8(a)(b)为机器人大角度转向占比与转向角数据对比图，图8(c)(d)为自主探索过程中耗费时间与行进路程对比图。

图8 仿真结果对比Fig.8 Comparison of simulation results

从图8(a)(b)可看出，传统评价函数选取的目标点引导机器人转向的平均角度为2.142 rad，大角度转向占比为56.18%，评价函数_IDA 选取的目标点引导机器人转向平均角度为1.044 rad，相比于传统评价函数减少了1.098 rad，大角度转向占比为19.29%，相比于传统评价函数降低了36.89%。从图8(c)(d)可看出，机器人考虑角度代价后，探索时间节省了42.30%，路径长度缩减了9.95%。综上所述，证明了加入角度代价的评价函数（评价函数_IDA）对于提升地图创建效率的有效性。

4.2 自主探索实验

为验证本文提出的自主探索算法的整体性能，且剔除结果的偶然性，在如图9(a)面积约为50 m2的真实环境下，使用如图9(b)控制单元为Jetson nano，搭配有镭神N10 激光雷达的样机进行了5 次实验。该实验将Keidar M.[7]提出的WFD 算法、金毅康[5]提出的RRT-BFS 算法与本文提出的WFD-RRT 算法从探索时间、路径长度两方面进行比对分析。

图10(a)为机器人在初始位置探索到的栅格地图，图10(b)为探索过程中生成的栅格地图，图10(c)为最终建图的结果，可以看出该算法能够引导机器人在未知环境中建立完整的地图，为防止剧烈碰撞，将机器人最大速度限制为0.1 m/s。从表1 可看出，WFD-RRT算法相较于WFD 算法，探索路径长度增加约2.95%，探索消耗时间降低约38.24%，这表明WFD-RRT 算法没有陷入局部陷阱而往复徘徊，反而通过降低候选点提取的计算量，减少地图创建时间，提高了自主探索效率；WFD-RRT 算法相较于RRT-BFS 算法，探索路径长度减少约32.13%，探索消耗时间降低约22.19%，总体来看，WFD-RRT 算法能够较明显地提升未知环境自主探索效率，在探索候选点提取、最佳目标点选择方面都有一定的优势，验证了WFD-RRT 算法的可行性与有效性。

表1 样机实验结果对比Tab.1 Comparison of prototype experimental results

图10 自主探索过程Fig.10 Process of autonomous exploration

5 结论

本文以提高自主探索效率为目标，针对候选点提取与聚类、目标点选取，提出一种基于WFD_L 与RRT融合的自主探索方法。通过仿真与实验测试结果，得出以下结论：

(1)提出一种考虑方向的评价函数，仿真实验得出评价函数_IDA 相比于传统评价函数选取的探索目标点，引导机器人大角度转向的概率下降36.89%，探索路径与探索耗时分别减少9.95%、42.30%。表明引入角度代价的评价函数_IDA 相比于传统评价函数能够提取出更高效的探索目标点。

(2)提出的WFD-RRT 算法，通过限制WFD 算法的搜索范围，并在传统评价函数中引入角度代价，使得探索候选点提取时间缩短、目标点选取更有效。在实验环境中，该算法的探索路径及探索耗时相比于RRT-BFS 算法分别降低了32.13%、22.19%，表明WFD-RRT 算法具有更加高效的探索效果。