消毒机器人路径规划：改进的RRT*算法

作者：Haotian Wang，Xiaolong Zhou，Jianyong Li，Zhilun Yang，Linlin Cao

编辑：东岸因为@一点人工一点智能公众号

针对消毒机器人在密集环境下利用快速搜索随机树（RRT）进行节点随机扩展时定位能力弱、搜索成功率低的问题，本文提出了一种基于人工势场引导采样和模糊自适应扩展的改进APF-GFARRT*（人工势场引导模糊自适应快速搜索随机树）算法。

针对RRT*算法中树生长固有的随机性，引入APF和RRT*相结合的方法，增强了采样过程的针对性。此外，在RRT*面临狭窄通道等密集和受限环境的情况下，采用模糊控制实现自适应步长调整。它加快了算法的收敛速度，提高了特定区域的搜索效率。

在MATLAB设计的专用环境中对该算法进行了验证和分析，并与现有的RRT、RRT*和APF-RRT*路径规划算法进行了比较。实验结果表明，改进算法具有良好的搜索速度，与其他三种算法相比，平均初始路径搜索时间缩短了46.52%。此外，改进后的算法收敛速度更快，平均迭代次数和平均最终路径代价显著减少约10.01%。

特别值得注意的是，该算法在特定环境下的适应性增强，与其他算法相比，增加了成功找到路径的机会，生成更合理、更平滑的路径。实验结果验证了该算法对类似问题具有实用性和可行性。

01 简介

近年来，机器人技术的不断进步在各个领域得到了广泛的应用。在全球新冠肺炎期间，机器人承担了体温测量、药物输送和消毒等任务，有效缓解了劳动力短缺的压力，降低了交叉感染的风险[1,2]。相关技术主要涉及实时定位、环境感知、路径规划和自主控制等技术，其中路径规划和自主避障一直是研究的重点[3,4]。

机器人的路径规划技术主要通过雷达、相机等传感器采集外部信息，然后根据这些环境信息建立可靠的移动路线[5,6,7]。

然而，在实际消毒任务中，路径规划的有效性受到多种因素的影响[8]。例如，当目标点周围存在障碍物时，会增加计算量，导致算法执行效率降低。类似地，现有算法在需要通过狭窄通道时可能会导致机器人失速或徘徊，无法有效规划路径。上述两种特殊情况使得现有的消毒机器人在某些特定环境下不能完全发挥作用。因此，优化和改进路径规划算法对提高消毒机器人的性能至关重要。

目前，基于网格搜索的路径规划算法（Dijkstra，A*）、基于虚拟势场的路径规划算法（APF）、基于概率抽样的路径规划算法（概率路线图法，RRT）和基于仿生智能的路径规划算法（模拟退火、遗传算法、粒子群优化算法）是移动机器人中应用最广泛的算法。

其中，A*算法具有较强的搜索能力，但在复杂环境下会受到网格建模的影响[9,10,11]。APF算法结构简单，实时性高，但在密集障碍区域可能出现路径振荡[12,13,14,15,16]。模拟退火算法可以摆脱局部最优，收敛到全局最优解，但受温度冷却速率的影响，其收敛速度较慢[17,18]。遗传算法很容易与其他优化算法或启发式算法相结合，以提高性能，但具有较高的计算复杂度[19,20]。PSO算法具有记忆性强、收敛速度快的特点，但也存在检测能力差、粒子多样性不足、易受局部极小值影响等明显缺点[21,22,23]。

因此，RRT算法采用全局均匀随机抽样策略快速扩展新节点，是需要快速路径规划的场景的最佳选择[24,25,26,27]。然而，随着任务复杂性的增加，研究人员发现了RRT的一些局限性，例如节点利用率低[28,29]和路径不稳定[30,31]。

针对这些不足，一些学者进行了深入的研究，提出了增强RRT算法的有效方法。结合双向搜索概念，Kuffner引入了RRT连接算法[32]。该算法使用启发式步骤减少在空白区域的无效搜索，最终节省整体搜索时间。在此基础上，Karaman提出了一种增强的RRT算法[33]。

RRT*包含树生长期间的“重新布线”过程，通过父节点重新选择和修剪操作优化树结构。经过多次迭代，RRT*收敛到渐近最优解。Gammell等人提出了基于RRT*的知情RRT*算法[34]。该算法将采样空间限制在一个椭圆区域内，随着路径长度的减小，采样区域逐渐减小，从而减少了对不必要区域的搜索。路线成本低于RRT*。

尽管许多研究人员在改进RRT（快速随机树）算法的采样和扩展方法，限制采样域方面取得了一定成效，但这些改进算法仍不能直接应用于消毒机器人系统。例如，RRT-Connect算法在构建双向树时指导不够明确，这可能导致更多的随机路径，尽管收敛速度快。RRT*引入“重连（rewire）”过程优化了搜索树的结构，但在具有狭窄通道和入口的密集障碍物中，重连过程受到限制，延长了获得全局最优解的时间。此外，尽管Informed-RRT*算法通过限制采样范围改善了搜索效率，但该算法的性能主要取决于椭圆区域参数的设置，需要在不同场景中持续调整以获得最佳结果，这在一定程度上增加了实际应用的复杂性。为了解决上述问题，本文提出了一种改进的基于APF引导采样的RRT*算法，并结合模糊自适应扩展，旨在更好地解决消毒机器人的路径规划问题。

本文其它结构如下：第2节详细阐述了提出的方法和相关知识。第3节全面概述了实验设计和实施结果，验证了提出方法的性能和优势。最后，第4节讨论了实验结果，并总结了研究问题。

02 算法模型

2.1 基本RRT*算法

基本RRT搜索过程类似于树在所有方向上扩展的生长过程，初始节点X_{init} 代表树的根。随机函数在自由空间内生成一个随机节点X_{rand} 。使用最近函数计算欧氏距离并选择最接近X_{rand}的节点X_{near} 。

随后，沿着从X_{near} 到X_{rand} 的方向发生以步长Step 进行扩展。算法使用生成的新节点X_{new} 进行碰撞检测。如果X_{new} 和X_{near} 之间的线段是无碰撞的，算法将X_{new} 添加到树T作为子节点。否则，算法将丢弃X_{new} ，当前迭代结束，进入下一次迭代。图1说明了展开新节点的过程。

图1 RRT扩展的示意图

RRT* 算法通过引入累积成本属性来增强基本的RRT算法，该属性记录了从起点到相应节点沿路径的所有边长度的总和。引入了一种新的策略，即重新选择X_{new} 的父节点，而不是使用X_{near}作为父节点。选择过程评估了以 X_{new} 为中心的一定半径内的所有节点，并选择累计成本最低的节点作为 X_{new} 的最佳父节点。确定了最佳父节点后，算法遍历剩余的树节点，计算每个节点到X_{new} 和根节点X_{init} 的路径成本。算法通过选择累计成本最小的路径重建树。图2说明了优化父节点和重构回溯的整个过程。

图2 RRT*算法扩展的示意图

2.2 改进算法的实现

与RRT算法相比，RRT*算法经历了上述两个关键处理步骤，极大地改进了附近节点的选择和路径优化。然而，在具有狭窄通道和入口的区域，它与RRT算法相比表现一致，并且容易出现局部生长困难。针对RRT*算法在特定区域的低探索效率以及搜索路径中仍存在的冗余节点，本文提出了改进的APF-GFARRT*（人工势场引导模糊自适应快速探索随机树）算法。

该算法包括两个关键模块：采样点引导模块和自适应步长调整模块。采样点引导模块使用APF并将目标点设置为（吸）引力采样点的潜在场引力点。另外，为了提高算法寻找第一条可执行路径的速度，引入了自适应步长调整模块，该模块应用模糊控制进行自适应步长调整，以加快算法的收敛速度。最后，模糊控制器引入收缩扩展因子，以自适应调整目标区域外的步长输出，减少对不必要区域的探索。

2.2.1 采样点引导模块

RRT 使用全局均匀随机采样策略扩展新节点，快速生成可行路径。然而，算法的随机性可能导致在密集环境中路径搜索盲目和低效。因此，引入采样点引导模块特别重要，以改善算法在狭窄通道和入口中的引导。

Khatib首先提出了经典的APF [16]。该方法的核心思想是将机器人周围环境建模为虚拟势场，在其中目标点产生连续引力，障碍物产生斥力。利用目标点的引力和障碍物的斥力规划路径表达如下：

总势场U 表示为引力场函数U_{att} 和斥力场函数U_{req} 的叠加：

引力场函数U_{att} ：

在这个方程中，K_{att} 是引力因子，它调节了引力场的势能的大小，目标点坐标是X_{target}。然后以下方程描述了斥力场函数U_{req}(x) ：

在这个方程中，\rho(X,X_{obs}) 表示机器人当前位置与障碍物位置之间的最短距离，\eta 是位置增益函数，p_0 是障碍物对机器人产生斥力的最大距离，在超过作用距离时，斥力将为0。假设搜索路径上实际障碍物的数量是m 。APF会对机器人产生叠加力，表达为：

人工势场具有简单的结构和快速的信息处理速度。为了提高节点扩展的目标定向性，在RRT*扩展过程中引入了人工势场。考虑到人工势场在路径搜索过程中容易受到局部约束，势场强度下降为0。因此，在搜索过程中只保留了引力场的影响，以避免APF-GFARRT*陷入局部最小点。

在获得随机点X_{rand} 的坐标信息后，采样点引导模块将目标点X_{target}设置为吸引点。采样点引导模块计算X_{rand} 接收的吸引水平和方向，生成相应的引导点，引导点将取代随机点。过程如下：

设置当前随机点的坐标X_{rand}=(x_{rand},y_{rand})，则相对于随机点的目标点的位移矢量为\vec{v}=(v_x,v_y) ，其中：

然后角度矢量\theta 可以使用反正切函数计算：

假设随机点不重叠目标点。在这种情况下，根据上述方程（2），潜在的引力函数与距离成反比，因此引力函数F_{att}(X) ：

假设两点之间的欧几里得距离为d ，引力因子为K_{attr} 。那么随机点的引力F_{attr} 为：

通过（6）-（9），我们可以在笛卡尔坐标系中计算吸引力F_{attr} 的分量：

最终引导点x_{guided} 的坐标可以使用以下方程计算：

其中，\xi 是控制引导点生成位置的引导系数。

设置好引力场之后，生成引导点的过程如图3所示。尽管采样点仍然具有一定程度的随机性，在势场的影响下，随机树更倾向于朝向目标点方向生长。

图3 (a) 计算引力 (b) 引导点生成

2.2.2 自适应步长调整模块

在RRT*中，树生长的长度由步长参数决定，过大或过小都会影响搜索性能。为了平衡路径搜索过程的深度和广度，本小节引入模糊控制以自适应调整步长。

在设计模糊控制器时需要考虑两个主要因素：采样点附近的障碍物密度（ObsDensity）以及当前节点到目标点的距离（PointDist）。首先，障碍物密度作为其中一个输入变量用于评估环境复杂度。对于密集环境的区域，应相应减小步长以便在狭小的空间中进行精确搜索。相反，在障碍物较少的区域，步长可以增加，以提高搜索效率。其次，当前点到目标点的距离作为另一个控制参数，步长根据距离大小进行调整。自适应步长调整的示意图如下图4所示。

图4 自适应步长调整：(a) 开阔区域；(b) 狭窄区域

在模糊系统中，ObsDensity和PointDist输入配置为相同的模糊子集{NL，NM，ZE，PM，PL}。在归一化后，它们的值范围分别为(0, 1)和(0, 10)。此外，为AdaptiveStep输出分配了五个成员函数（MF）状态，PL、PS、ZE、NS和NL。这里，“L”、“M”和“S”分别代表“大”、“中”和“小”。成员函数采用对称的、均匀分布的三角形和高斯形状。输入和输出成员函数的形状如图5所示。随后，设计了25条模糊规则，如表1所示。模糊推理和去模糊化采用Mamdani方法和Centroid方法，增强了模糊系统的自适应调整性能。

图5 输入和输出的函数设置

表1 AdaptiveStep的模糊控制规则

在RRT*迭代过程中，即使成功找到第一条可行路径，全局均匀采样策略仍然不会改变，这可能导致在低价值区域重复采样，使得难以快速收敛。为了解决这个问题，设计了一个缩放因子\alpha ，它有效地调整了模糊控制器的输出范围。这个因子的模糊子集包括{VB，B，M，S，V，VS}，并且仅设置输出域与其变化有关。\alpha 的表达式如下：

这里，k_1 是一个比例常数，D_{line} 代表从采样点到直线L （由起点和目标点定义的直线）的垂直距离。表达式如下：

\alpha的作用是加快算法的收敛速度。具体来说，它通过计算D_{line} 来判断采样点是否位于低价值区域。如果D_{line} 超过了设定的阈值，\alpha 将改变模糊控制器的输出范围，从而影响adaptiveStep 随着D_{line} 增加的快速减小，并有效阻止了在重构过程中随机树探索低价值区域。

至此，主要控制器的设计已经完成，并且在不同的输入条件下适应步长的变化如图6所示。

图6 模糊控制系统中自适应步长变化示意图

通过结合采样点引导模块和自适应步长调整模块，本文得到了基于引导采样和模糊自适应扩展的改进APF-GFARRT*算法。路径规划算法的整体流程如图7所示。

图7 基于自适应步长的APF-GFARRT*路径规划算法流程图

伪代码（算法1）描述了APF-GFARRT*搜索路径的过程

主要步骤如下：

初始化（第1-2行）：初始化整个程序，设置起点X_{init} 、目标点X_{target} 和其他参数，并创建一个以X_{init} 为根节点的空树T。

主循环（循环迭代，第3-28行）：

➢第4-6行：生成随机点X_{rand} ，计算引力apf\_force ，并获取引导点的坐标。

➢第7-8行：计算D_{line} 并在树T中搜索节点X_{near} 。

➢第9-24行：调用CollisionFree 函数检查从X_{near} 到x_{guided} 的直线是否穿过障碍物。

➢第10-11行：调用函数计算引导点处的障碍物密度级别，并根据模糊规则输出adaptiveStep 。

➢第12-17行：检查是否已经搜索到第一条可行路径，如果findPath 为1，则当前处于路径优化阶段，根据缩放因子\alpha 调整扩展步长。然后扩展X_{new} 。

➢第18-23行：选择父节点并更新树结构。

➢第25-27行：检查新节点是否到达目标点或附近区域，如果是，将目标点添加到树结构中，并回溯最终路径。

返回结果（第29行）：如果达到最大迭代次数且未找到路径，则返回无路径。

03 实验和分析

本节将通过将APF-GFARRT*算法与现有的RRT、RRT*和APF-RRT* [35]算法在相同的2D环境中进行比较实验来验证APF-GFDARRT*算法的性能。模拟平台和配置包括MATLAB R2022a，64位Windows 10以及AMD Ryzen3 5600处理器，CPU频率为3.6 GHz，内存为8 GB。

在实验过程中，将最大迭代次数设置为1000，吸引因子为50，并将步长固定为2.5 M。在APF-GFARRT*算法中，通过将最小步长设置为1 M，进行关键调整，以避免由于步长过小导致不必要的冗余节点。为了保持一致性和可比性，在实验中保持其他相关参数不变。另外，所有实验环境的地图大小均标准化为[100, 100]，起点和终点分别定义为[0, 0]和[90, 90]。考虑到消毒机器人的碰撞体积，机器人被简化为一个材料点，并对障碍物进行了适度膨胀处理。图8显示了两个地图的障碍物分布以及起点和终点的位置。

图8 两个不同的环境地图模型：(a) 地图1；(b) 地图2。在这些地图中，起点用圆表示，终点用菱形表示

3.1 狭窄通道测试

由于缺乏有效的引导，在狭窄通道中，RRT和RRT*算法可能受到限制，它们的扩展可能会崩溃或者明显变慢。为了验证诸如APF-GFRRT*等算法在受限环境中的可靠性，在地图1中设置了一个狭窄通道，并进行了100组实验。实验结果如图9所示，实验数据总结在表2中。

图9 不同算法在同一地图（地图1）中生成路径：(a) RRT；(b) RRT*；(c) APF-RRT*；(d) APF-GFARRT*

表2 在地图1下算法的比较

在图9中，红色曲线代表最终路径，蓝色线代表在随机树扩展过程中产生的单个分支点。通过比较图9中的四个规划结果，可以得出结论，APF-GFARRT*在寻找通往狭窄区域的路径方面表现最佳。此外，图9d表明改进的算法在狭窄通道附近有更密集的采样点分布。这是因为添加了采样引导模块具有更强的定向性，随机树更倾向于更向目标点生长。

为了验证所提出的算法在克服狭窄区域约束方面的有效性，在地图1环境中设置了两个受限通道，并使用四种算法进行规划。结果如图10所示：

图10 不同算法在相同障碍环境中生成路径：(a) RRT；(b) RRT*；(c) APF-RRT*；(d) APF-GFARRT*

与图9相比，图10表明在地图中添加约束区域时，随机树的增长速率显著降低，并产生了更多的分支。此外，从表3中的数据可以得出，RRT和RRT*算法的成功概率也有所降低。同时，平均路径成本也增加。相反，添加人工势场引导后，APF-RRT*和APF-GFRRT*的采样点更倾向于目标点，但从图10c中可以看出，APF-RRT*仍然在狭窄通道处停滞，成功率降低。而具有模糊自适应步长调整策略的APF-GFRRT*的成功率保持在85%以上。此外，在扩展到目标点的过程中，随机树更有方向性和更快速，如图10d所示。

表3 在相同障碍环境下算法的比较

从图11中的路径长度收敛曲线中可以看出，改进的算法仍然具有更短的路径长度，并且在较少的迭代次数下收敛到渐进最优路径，这验证了算法的可靠性。

图11 APF-GFRRT*和其他算法的路径长度收敛曲线

3.2 密集障碍测试

在地图2中设置了密集的障碍，以验证APF-GFARRT*算法在密集环境中的可靠性。每个算法进行了两百次实验来记录数据，例如首次发现路径所花费的时间，实验过程中的平均路径节点数和平均路径成本。表4提供了各种算法的数据比较分析。

表4 算法对比

Figure 12a显示了RRT算法的路径结果。RRT*算法的规划结果显示在图12b中。APF-RRT*算法的迭代实验结果显示在图12c中，改进后的APF-GFARRT*算法的规划效果显示在图12d中。观察图12d，可以发现改进后的APF-GFARRT*算法在左下角障碍物较少、地形更加开阔的区域采用了更大的扩展步长，而在地图中心密集障碍物区域，步长则相应调整减小（程序将最小步长设置为1m以防止过小步长导致扩展）。与其他算法相比，改进后的算法引入自适应调整模块后的适应能力有所增强，路径节点数量显著减少。将图12c与图12d进行比较，APF-GFARRT*算法通过调节扩展因子限制了超出指定采样区域的扩展速率。该算法侧重于初始路径附近区域的采样优化，导致最终路径更加顺畅，需要更小的路径成本。从图12中其他算法的规划结果对比可以发现，APF-GFARRT*算法在相同迭代次数下具有更好的最终规划效果。

图12 不同算法在同一地图（地图2）中生成路径：(a) RRT；(b) RRT*；(c) APF-RRT*；(d) APF-GFARRT*

根据表4中的数据，可以得出结论，RRT和RRT*在搜索过程中都有失败的可能性，成功率分别为73%和84%。原因是这两种算法都使用全局随机采样，在复杂环境中寻找可行路径的大规模采样成本较高。此外，密集障碍物产生许多狭小区域，严重影响了RRT和RRT*算法的搜索速率，首次找到路径所需的平均时间分别为6.8701秒和6.6804秒。

在比较分析中，原始RRT算法的平均路径成本和平均路径节点数分别为160.6656和81.1864，而加入了“重连”机制后，这两个数值分别降至142.4439和46.3722。借助人工势场导向策略的APF-RRT*算法将平均路径长度和平均路径节点数降至138.9890和45.8793，进一步提升了搜索性能。与传统的RRT、RRT*和APF-RRT*算法相比，APF-GFARRT*算法花费的时间最短，仅为3.5721秒。受益于人工势场采样引导，APF-GFARRT*算法具有更出色的密集环境搜索速率，并配备了自适应步长调整，在最终降低了平均路径成本和平均路径节点数到了更低的水平：128.35和29.2977。

RRT*、APF-RRT*和APF-GFARRT*算法均为渐进最优，因此在相同迭代次数下，它们的平均路径成本没有太大差异。然而，APF-GFARRT*算法在找到第一条可行路径方面更快。从图13中可以看出，尽管在路径的后期阶段成本没有太大差异，但APF-GFARRT*算法在500次迭代后的路径长度比RRT*算法在1500次迭代后更小，接近最优路径。这验证了所提出的采样引导模块和自适应步长调整模块有效提高了算法的收敛速率，使得在较少的迭代次数下找到更接近最优解的路径。

图13 不同算法路径在迭代过程中成本的变化。

04 结论和未来工作

目前，面对严峻的全球公共卫生问题，消毒机器人作为一种防疫方法正逐渐在主要公共场所广泛使用。本研究致力于提高消毒机器人的路径规划效率，通过改进RRT*算法，提出了APF-GFARRT*算法，该算法集成了APF、模糊控制和受限采样域的概念。该算法的优势主要体现在以下几个方面。

APF-GFARRT*算法在密集环境中表现良好，平均可行路径搜索时间仅为3.5721秒，远远优于RRT和RRT*，相比之下，与RRT相比节约了近48%的时间，与RRT*相比节约了47%的时间资源。与原始的RRT和RRT*对比表明，APF-GFARRT*的平均路径成本分别降低了20%和10%。此外，与RRT和RRT*相比，APF-GFARRT*算法还能够显著减少路径中的冗余节点，分别减少了约64%和37%。

以上研究证实，APF-GFARRT*在密集环境中更有效地找到路径，并能够快速找到具有较低最终路径成本和较少节点的可行路径。由于时间和容量的限制，此模拟实验仅在MATLAB中完成。未来，为了更好地测试算法及其性能，应进一步研究并在自然消毒机器人系统中进行验证。