融合人工势场法的动态快速行进树路径规划算法

摘 要：针对快速行进树算法（FMT*）由于随机采样导致的冗余探索问题以及不适用于动态环境的问题，提出一种融合人工势场法的动态快速行进树路径规划算法（APF-Dynamic FMT*），该算法设计了一种基于人工势场法的采样点引导函数，该函数根据环境信息动态的调整采样点生成范围，减少冗余探索。同时，该算法设计了一种路径树动态调整机制，当现有路径受到环境改变的影响时，能在未受影响的剩余路径树的基础上重新规划出新的优秀路径，适用于解决动态环境下的路径规划问题。仿真实验结果表明，APF-Dynamic FMT*算法在消耗相同计算资源的同时，显著提高了路径规划的成功率与路径质量，且当现有路径受动态环境影响后，能够高效地重新规划出可通行的优秀路径。

关键词：FMT*算法；人工势场法；动态路径规划

中图分类号：TP391 文献标志码：A 文章编号：1001-3695（2024）09-025-2745-06

doi：10.19734/j.issn.1001-3695.2024.01.0004

Dynamic fast marching tree algorithm with integrated artificial potential fields

Wu Xupeng1，2，Jia Xiaolin1，2，Gu Yajun1，2

（1.RFID & IOT Laboratory，School of Computer Science & Technology，Southwest University of Science & Technology，Mianyang Sichuan 621010，China;2.Mobile Internet of Things & Radio Frequency Identification Technology Key Laboratory of Mianyang，Mianyang Sichuan 621010，China）

Abstract：Addressing the issues of redundant exploration and inapplicability in dynamic environments inherent in fast mar-ching tree algorithm（FMT*），this paper proposed the APF-Dynamic FMT* algorithm，which integrated artificial potential field method.This algorithm designed a sampling point guidance function based on artificial potential field method，which could dynamically adjust the sampling point generation range according to the environment information to reduce redundant exploration.Additionally，this algorithm designed a dynamic path tree adjustment mechanism，when the existing path was affected by the environment change，it could re-plan a new excellent path on the basis of the remaining path tree that is not affected，which was suitable for solving the path planning problem in dynamic environment.The results verify that the APF-Dynamic FMT* algorithm can significantly improve the success rate and path quality of path planning while consuming the same computational resources，and when the existing path is affected by the dynamic environment，it can efficiently re-plan the passable excellent path.

Key words：FMT* algorithm;artificial potential field method;dynamic path planning

0 引言

路径规划在机器人导航、自动驾驶汽车以及众多自动化系统中扮演着至关重要的角色。它涉及在环境中寻找一条从起始点到目标点的有效路径，同时考虑优化路径的长度、安全性和可行性［1，2］。

概率路线图快速探索随机树（rapidly exploring random trees，RRT）与快速行进树算法（fast marching tree，FMT*）都是基于采样的路径规划算法［3，4］。RRT算法虽然路径规划效率较高，但已被证明无法规划出最优的路径［5］。在高维空间中，传统路径规划算法往往因计算复杂度高而效率不佳，为了克服这一限制，Janson等人［6］于2013年提出了FMT*算法。FMT*算法是一种基于采样的渐近最优算法。该算法通过有效地利用采样点来构建一棵覆盖搜索空间的树，从而高效地找到从起点到终点的路径。FMT*算法在多种应用场景下展现了出色的性能，但在特定条件下，如复杂地形或动态环境中，它仍然面临一些挑战［7～9］。

针对FMT*算法的研究主要集中在如何提高其在复杂环境中的适应性和效率上。为了提高FMT*算法的路径规划效率，Starek等人［10］提出了一种双向拓展的FMT*算法（bidirectional fast marching Tree，BFMT*），缩短了搜索时间，但增加了计算资源消耗；Gao等人［11］将BFMT*与A*算法结合，形成了一种双向搜索的启发式路径规划算法（heuristic bidirectional fast marching tree，HBFMT*），提高了探索效率，但传统启发函数使用的样本信息不够充分，有时会影响计算效率。Wu等人［12］提出APF-IRRT*算法（artificial potential field-informed RRT*），通过引入人工势场方法来提高移动机器人路径规划的效率和质量，但在动态环境中，一旦环境变化影响了现有路径，需要从头开始重新规划路径，效率较低。

针对FMT*算法在采样点数量较少时路径规划成功率较低、路径不够优秀且无法应对动态环境的问题，提出一种融合人工势场法的动态快速行进树路径规划算法（dynamic fast marching tree algorithm with integrated artificial potential field，APF-Dynamic FMT*）。APF-Dynamic FMT*与APF-IRRT*算法在利用人工势场法改善路径规划效率与质量方面有着共同的理念。然而，APF-Dynamic FMT*算法在此基础上进一步针对动态环境的路径规划问题进行优化，展现出针对动态环境适应性的创新和进步。该算法结合了人工势场法的环境适应性和FMT*算法的路径规划策略，显著提高了在采样点数量较少时的路径规划成功率与路径质量，同时采用路径树动态调整机制，在动态环境中能高效地规划出新路径。通过仿真实验验证了APF-Dynamic FMT*算法的有效性以及在动态环境下的可用性。

1 问题描述

本文主要考虑二维路径规划问题，FMT*算法的核心理念是在一组预先生成的采样点上实施动态递归探索，通过设定的连接半径和邻域筛选策略来构建出一棵高效的路径树。在FMT*算法中，采样点是随机生成并均匀分布于整个搜索空间的，如图1所示，其中矩形代表障碍物，蓝色点是起点，绿色点是目标点，红色小点是采样点（见电子版），这种均匀分布的采样方法会导致较多的冗余探索，影响探索效率。

在FMT*算法的路径规划过程中，如图2所示，路径树会均匀地向各个方向扩展。然而，由于某些采样点距离较远，它们无法有效地被整合到路径树中，这导致了计算资源的不必要浪费。

由于FMT*算法在规划开始之前对整个搜索空间进行一次性采样，并在这些采样点上构建一棵树。在动态环境中，障碍物的位置可能随时间改变，这使得预先采样的点可能变得不再有效或安全，因而需要重新采样和构建路径。并且FMT*算法没有应对环境变化的响应机制，在动态环境下，一旦环境发生变化，需要从起点开始重新规划，包括重新采样和构建路径树。这一过程消耗了大量的计算资源与时间，特别是在环境频繁发生变化的情况下，这会大大降低算法的实用性。

综上所述，FMT*算法进行路径规划时，采样点随机生成导致的冗余探索，计算资源浪费等问题是导致FMT*算法在采样点数量较少时路径规划成功率低、路径不够优秀的主要原因，且由于FMT*算法通常针对静态环境设计，难以适应环境的实时变化［13］。在已有的环境基础上，如果向环境中动态添加障碍物以阻挡现有路径，由于FMT*算法无法对实时的环境变化作出应对，不适用于动态环境［14］。

2 APF-Dynamic FMT*算法

针对FMT算法在动态环境下的局限性，本文提出了APF-Dynamic FMT算法。该算法设计了一种基于人工势场法的采样点引导函数SampleAPF（n），不同于FMT*的随机采样策略，SampleAPF（n）能够根据环境的障碍物占比信息动态调整采样点的生成范围。同时，APF-Dynamic FMT算法设计了一种路径树动态调整机制以实时监测环境变化（如障碍物的新增），若新的障碍物出现阻挡了现有路径，能够快速规划出新的优秀路径。

2.1 基于人工势场法的采样点生成函数

由于FMT*算法采用随机生成采样点的策略，导致大量冗余探索和计算资源的浪费。FMT*算法所生成的采样点中，如图3所示，图中绿色椭圆虚线所圈住的范围内的采样点与FMT*算法路径规划的成功率、路径质量高度相关，但位于绿色椭圆虚线圈外范围的采样点，对路径规划的影响程度低且数量占比较大，将其定义为冗余采样点。

设定一组实验，设定采样点数量为100，在障碍物占比分别为0%、10%、20%、30%、40%、50%的地图环境下，进行路径规划，统计冗余采样点的占比数据。如表1所示，随着环境障碍物占比的增加，冗余采样点的占比由74%降至54%，虽有小幅降低，但仍有大量冗余采样点，这导致了大量的冗余探索。

为解决由于采样点随机生成所导致的冗余探索问题，APF-Dynamic FMT*算法设计了一种基于人工势场法的采样点引导函数SampleAPF（n）以调整采样点的生成范围，该函数可以根据环境信息动态地调整采样点生成范围，适用于不同障碍物比例的情况，该函数返回一个具有q∈n（n为采样点数量）个点的集合，具体实现细节如算法1所示，主要使用以下函数：

a）random（0，J（q））生成一个取值为（0，J（q））的随机值，J（q）表示对采样点q的限制势力。

b）U（q）表示采样点q的合力势绝对值，用于判断采样点q是否满足生成条件。

算法1 SampleAPF（n）

输入：采样点数量n。

输出：一个有n个采样点的集合Sample（n）。

1 N=n，m=0，Sample（n）←{}

2 for m≤N do

3 Generate a random sample point q

4 θ=random（0，J（q））

4 if U（q）≤θ then

5 Sample（n）←Sample（n）∪{q}

6 m+=1

7 end if

8 end for

9 return Sample（n）

SampleAPF（n）返回一个有n个采样点的集合，首先根据目标位置和障碍物位置，构建一个势能场，如图4所示，势能场将地图范围内的起点与目标视为引力极，障碍物视为斥力极。抽象定义引力极产生的引力势为采样点位置与引力极位置的距离相关函数；斥力极产生的斥力势为采样点位置与斥力极位置的距离相关函数。

假设地图中新生成一个采样点q，SampleAPF（n）将根据合力势绝对值U（q）判断该采样点是否满足生成条件，相关公式如式（1）所示。

4 仿真实验分析

4.1 采样点分布对比

设定实验环境为30×50的矩形地图如图7所示，图中灰色线条为地图边界，灰色矩形为障碍物，蓝色点为起点，绿色点为目标点，红色小圆点为采样点。相比于FMT*算法，在相同数量采样点的情况下，APF-Dynamic FMT*算法所生成的采样点大部分位于以起点与目标点连线为长轴的椭圆内，且越靠近起点与目标点的连线范围内采样点越密集，而FMT*算法所生成的采样点则随机分布于整个地图范围。

由于FMT*算法在空间中随机生成采样点，导致大部分与目标方向无关的冗余采样点需要被探索，这占用了大量的计算资源。而APF-Dynamic FMT*算法生成的采样点绝大部分分布在具有探索价值的范围内，较少生成与目标方向无关的采样点。如图8所示，在相同数量采样点的情况下，FMT*算法中有大量与目标点方向无关的采样点被探索，且有部分采样点一直未被探索，这导致部分计算资源浪费，影响了路径规划效率。而APF-Dynamic FMT*算法中大部分被探索的采样点都具有一定的探索价值，并且APF-Dynamic FMT*算法进行路径规划的过程中未被探索到的采样点数量远少于FMT*算法，因此采用APF-Dynamic FMT*算法具有更高的采样点利用率。

4.2 路径规划效果对比

以下是APF-Dynamic FMT*与FMT*算法在仿真实验中的路径规划效果对比分析。如图9～11所示，两种算法在采样点数量同为100、200、300的情况下均能成功规划一条可行路径，但是FMT*所规划出的路径长度过长，且路径不平滑，冗余拐点较多。相比于FMT*算法，APF-Dynamic FMT*算法所规划出的路径长度明显更短且路径更加平滑，更加趋向于最优路径。

由于APF-Dynamic FMT*算法与FMT*算法基于采样点探索的特性，采样点分布的有效性影响了路径规划的成功率。如图12所示，当出现面积较大障碍物挡住采样点之间的联通路线时，由于采样点分布不合理，可能会导致FMT*算法路径规划失败，所以采样点的分布会对路径规划的成功率有较大影响。

在环境一致的情况下，分别采用APF-Dynamic FMT*与FMT*、RRT、APF-IRRT*算法的路径规划成功率随采样点数量的变化曲线如图13所示，可以得知在较少采样点的情况下采用APF-Dynamic FMT*算法进行路径规划的成功率明显高于其余算法，且APF-Dynamic FMT*算法的路径规划成功率能够更快地收敛于95%以上的高成功率并且保持稳定，采用APF-Dynamic FMT*算法进行路径规划有着更高的成功率，解决了FMT*算法在较少采样点数量时路径规划成功率低的问题。

由于APF-Dynamic FMT*、FMT*、RRT与APF-IRRT*算法基于采样点探索的特性，在相同空间内，采样点数量越多，所生成的路径相对越短且越平滑，但相对而言所消耗的计算资源就越大，所以采样点的数量设定并不是越多越好，而是需要随着采样点数量的增加更快地收敛于最优路径。APF-Dynamic FMT*与FMT*算法和RRT算法路径规划所得的路径长度随采样点数量的变化曲线，如图14所示，随着采样点数量的递增，FMT*算法规划所得路径长度起伏较大，受采样点数量影响较大，在采样点数量达到400以上才逐渐收敛于最短路径长度且保持相对稳定；由于采样点数量不足，RRT算法规划所得路径长度起伏较大，且未收敛于最优路径；APF-IRRT*算法相较于RRT算法有不错的提升，但收敛于最优路径的速度较为缓慢；APF-Dynamic FMT*算法规划所得路径长度整体相对稳定且更快地收敛于最短路径长度，受采样点数量的影响较小，具有不错的稳定性，相比于FMT*算法规划所得路径更为优秀，解决了FMT*算法在较少采样点数量时规划所得路径不够优秀的问题。

4.3 APF-Dynamic FMT*算法在动态环境下路径规划效果

APF-Dynamic FMT*算法具有路径树动态调整机制，环境发生变化时，算法会判断当前路径是否被障碍物所阻挡。若有障碍物阻碍了当前已规划出的路径时，路径树动态调整机制会实时调整当前的路径树，剔除掉被障碍物覆盖的采样点以及被障碍物阻挡的路径树，并在现存路径树的基础上进行路径规划。如图15所示，在采样点数量设定为300的情况下，初次进行路径规划成功后，在地图中连续三次添加圆形障碍物障碍物（如图中灰色圆形）阻碍当前路径后，APF-Dynamic FMT*算法均能在未重新采样的前提下规划出一条可通行路径，证明了该算法在动态环境下的可用性。

为验证APF-Dynamic FMT*算法在动态环境下重新规划出新路径的高效性，与文献［15］提出的一种动态RRT路径规划算法（replanning algorithm for rapidly-exploring random trees，replanning-RRT）进行对比实验。replanning-RRT算法能删除RRT算法在动态环境中失效部分并保留其他部分，且在已生成的扩张树的基础上继续生长，直到规划出新的路径。设定实验环境为动态二维地图环境，采样点数量为500，在首次路径规划成功后，向地图中逐次添加障碍物以阻挡现有路径，记录重新规划出新路径的平均时间。实验结果如图16所示，随着添加障碍物次数的增加，replanning-RRT算法的平均路径规划时间增幅较大，且没有达到稳定的趋势，而APF-Dynamic FMT*算法的平均路径规划时间的增幅较小，且逐渐趋向稳定，证明了其在动态环境中的高效性。

5 结束语

为了解决FMT*算法在采样点数量较少时路径规划效率低以及不适用于动态路径规划等问题，本文提出一种融合人工势场法的动态快速行进树路径规划算法（APF-Dynamic FMT*）。APF-Dynamic FMT*设计了一种基于人工势场法的采样点引导函数SampleAPF（n），该函数可以根据环境信息动态的调整采样点生成范围，适用于不同障碍物比例的情况，这减少了对空间的冗余探索，提高了采样点的利用率。同时，APF-Dynamic FMT*算法设计了一种路径树动态调整机制，监测环境是否发生改变，当环境发生改变时，算法会判断当前路径是否被障碍物所阻挡。若有障碍物阻碍了当前已规划出的路径时，算法会实时调整当前的路径树，剔除掉被障碍物覆盖的采样点以及被障碍物阻挡的路径树，并在剩余路径树的基础上进行路径规划。仿真实验结果表明，APF-Dynamic FMT*算法在消耗相同计算资源的同时，显著提高了路径规划的成功率与路径质量。在动态环境中，能够高效地再次规划出可通行的优秀路径。

参考文献：

［1］刘澳霄，周永录，刘宏杰.基于改进人工势场法的医疗配送机器人路径规划［J］.计算机应用研究，2024，41（3）：842-847.（Liu Aoxiao，Zhou Yonglu，Liu Hongjie.Path planning of medical delivery robot based on improved artificial potential field method［J］.Application Research of Computers，2024，41（3）：842-847.

［2］张艳菊，吴俊，程锦倩，等.多搬运任务下考虑碰撞避免的AGV路径规划［J］.计算机应用研究，2024，41（5）：1462-1469.（Zhang Yanju，Wu Jun，Cheng Jinqian，et al.AGV path planning considering collision avoidance of multiple handling tasks［J］.Application Research of Computers，2024，41（5）：1462-1469.

［3］Chen Jiagui，Zhao Yun，Xu Xing.Improved RRT-connect based path planning algorithm for mobile robots［J］.IEEE Access，2021，9：145988-145999.

［4］司徒华杰，雷海波，庄春刚.动态环境下基于人工势场引导的RRT路径规划算法［J］.计算机应用研究，2021，38（3）：714-717，724.（Situ Huajie，Lei Haibo，Zhuang Chungang.Artificial potential field based RRT algorithm for path planning in dynamic environment［J］.Application Research of Computers，2021，38（3）：714-717，724.）.

［5］Fareh R，Baziyad M，Rabie T，et al.Enhancing path quality of real-time path planning algorithms for mobile robots：a sequential linear paths approach［J］.IEEE Access，2020，8：167090-167104.

［6］Janson L，Pavone M.Fast marching trees：a fast marching sampling-based method for optimal motion planning in many dimensions-exten-ded version［EB/OL］.（2013-06-15）.https：//arxiv.org/abs/1306.3532.

［7］Fan Jiaming，Chen Xia，Liang Xiao.UAV trajectory planning based on bi-directional APF-RRT* algorithm with goal-biased［J］.Expert Systems with Applications，2023，213：119137.

［8］Li Qiongqiong，Xu Yiqi，Bu Shenqiang，et al.Smart vehicle path planning based on modified PRM algorithm［J］.Sensors，2022，22（17）：6581.

［9］Xu Jing，Song Kechen，Zhang Defu，et al.Informed anytime fast ma-rching tree for asymptotically optimal motion planning［J］.IEEE Trans on Industrial Electronics，2020，68（6）：5068-5077.

［10］Starek J A，Gomez J V，Schmerling E，et al.An asymptotically-optimal sampling-based algorithm for bi-directional motion planning［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2015：2072-2078.

［11］Gao Wenxiang，Tang Qing，Yao Jin，et al.Heuristic bidirectional fast marching tree for optimal motion planning［C］//Proc of the 3rd Asia-Pacific Conference on Intelligent Robot Systems.Piscataway，NJ：IEEE Press，2018：71-77.

［12］Wu Daohua，Wei Lisheng，Wang Guanling，et al.APF-IRRT*：an improved informed rapidly-exploring random trees-star algorithm by introducing artificial potential field method for mobile robot path planning［J］.Applied Sciences，2022，12（21）：10905-10905.

［13］Janson L，Schmerling E，Clark A，et al.Fast marching tree：a fast marching sampling-based method for optimal motion planning in many dimensions［J］.The International journal of robotics research，2015，34（7）：883-921.

［14］Wang Kuan，Xu Jing，Song Kechen，et al.Informed anytime bi-directional fast marching tree for optimal motion planning in complex cluttered environments［J］.Expert Systems with Applications，2023，215：119263.

［15］Ferguson D，Kalra N，Stentz A.Replanning with RRTs［C］//Proc of IEEE International Conference on Robotics and Automation.Pisc-ataway，NJ：IEEE Press，2006：1243-1248.

收稿日期：2024-01-06

修回日期：2024-03-08

基金项目：国家自然科学基金面上项目（61471306）；四川省自然科学基金资助项目（2022NSFSC0548）；四川省重点研发计划资助项目（2020YFS0360）；四川省教育厅教改资助项目（JG2021-1414）

作者简介：吴旭鹏（1999—），男，四川眉山人，硕士，CCF会员，主要研究方向为移动物联网技术、移动机器人导航技术；贾小林（1975—），男（通信作者），四川南江人，教授，博导，博士，CCF高级会员，主要研究方向为射频识别（RFID）技术、物联网（IoT）技术、计算机应用系统（my_jiaxl@163.com）；顾娅军（1979—），女，四川绵阳人，讲师，硕士，主要研究方向为计算机应用技术、物联网（IoT）技术.