消毒机器人路径规划新突破：APF-GFARRT*算法详解

消毒机器人路径规划新突破：APF-GFARRT*算法详解

消毒机器人在疫情防控中发挥着重要作用，但其在复杂环境中的路径规划一直是个难题。本文提出了一种改进的RRT算法（APF-GFARRT），通过引入人工势场引导和模糊自适应步长调整，显著提高了消毒机器人在密集环境中的路径规划效率。

近年来，机器人技术在疫情防控中发挥了重要作用，特别是在消毒任务中。然而，现有的路径规划算法在复杂环境中存在诸多不足，如计算量大、容易陷入局部最优等。针对这些问题，本文提出了一种改进的RRT算法（APF-GFARRT），通过引入人工势场引导和模糊自适应步长调整，显著提高了消毒机器人在密集环境中的路径规划效率。

算法模型

基本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算法扩展的示意图*

改进算法的实现

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

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

实验和分析

本节将通过将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。在这些地图中，起点用圆表示，终点用菱形表示

狭窄通道测试

由于缺乏有效的引导，在狭窄通道中，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和其他算法的路径长度收敛曲线*

密集障碍测试

在地图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 不同算法路径在迭代过程中成本的变化

结论和未来工作

目前，面对严峻的全球公共卫生问题，消毒机器人作为一种防疫方法正逐渐在主要公共场所广泛使用。本研究致力于提高消毒机器人的路径规划效率，通过改进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中完成。未来，为了更好地测试算法及其性能，应进一步研究并在自然消毒机器人系统中进行验证。

本文原文来自腾讯云开发者社区