基于改进D*Lite⁃APF算法的巡检机器人路径规划

摘" 要： 针对巡检机器人在动态场景下路径规划存在非全局最优、路径不平滑及局部避障效果不佳的问题，提出一种将改进D*Lite算法和人工势场法融合的算法。首先优化D*Lite算法启发代价函数，提升规划效率，并引入Dubins曲线平滑生成的全局路径；其次改进人工势场法势场函数并添加随机半径扰动点，解决局部碰撞问题，提高避障性能；最后将两种优化算法有效融合，实现全局规划和局部避障。仿真实验结果表明，相较于单一D*Lite算法，融合算法在路径长度、时间花销、路径拐点及扩展节点数方面均表现更优，能在确保全局路径最优的情况下有效避障。

关键词： 巡检机器人； 路径规划； D*Lite； Dubins曲线； 人工势场法； 避障

中图分类号： TN99⁃34" " " " " " " " " " " " " " " 文献标识码： A" " " " " " " " " " " "文章编号： 1004⁃373X（2024）05⁃0155⁃05

Inspection robot path planning based on improved D*Lite⁃APF algorithm

HU Liqi1， ZENG Wei1， CHEN Caihua1， ZHANG Peng2， WANG Yiru1， LI Tong1

（1. Department of Artificial Intelligence， College of Computer Science and Cyber Security， Chengdu University of Technology， Chengdu 610059， China;

2. Department of Communication Engineering， School of Mechanical and Electrical Engineering， Chengdu University of Technology， Chengdu 610059， China）

Abstract： In view of the non⁃global optimality， unsmooth path and poor local obstacle avoidance effect in the path planning of inspection robots in dynamic scenes， an algorithm that integrates the improved D*Lite algorithm and the artificial potential field method is proposed. The D*Lite algorithm is optimized to heed the cost function， improve the planning efficiency， and introduce the global path generated by Dubins curve smoothing. The artificial potential field normal potential field function is improved and random radius disturbance points are added to avoid the local collision and improve the obstacle avoidance performance. The two optimization algorithms are integrated effectively to realize global planning and local obstacle avoidance. Simulation results show that， in comparison with the single D*Lite algorithm， the fusion algorithm performs better in path length， time cost， path inflection point and number of extension nodes， and can avoid obstacles effectively under the condition of ensuring the optimal global path.

Keywords： inspection robot; path planning; D*Lite; Dubins curve; artificial potential field method; obstacle avoidance

0" 引" 言

机器人路径规划是一种在已知或未知环境中寻找从任何指定位置到给定目标位置的无碰撞路径的技术[1]。其目的是在安全、能耗和时间约束下，通过避开静态及动态障碍物，计算从起始点到目标点的最短无碰撞路径[2]。

根据环境信息掌握情况，路径规划可分为全局路径规划和局部路径规划[3]。常用的全局规划算法包括：A*算法、蚁群算法、D*Lite算法等[4⁃6]。标准A*算法有较好的路径规划速度，但规划路径较长，且不适合动态路径规划。D*Lite算法以增量方式确定路径，很好地适应了动态环境中的路径规划问题，但其规划效果还有待提升[7]。文献[8]提出的改进D*Lite算法能在动态障碍物混合环境中实现自动有效寻路，在寻路成功率方面性能优越，但避障准确率还有待提升。文献[9]通过改进估值及启发函数优化D*Lite算法，减少扩展节点及拐点数量，提高了算法搜索效率。全局规划算法能在未知环境中有效规划路径，但在复杂障碍物环境局部避障中，未很好考虑局部因素，可能出现规划路径非全局最优，存在冗余节点等问题，导致巡检时间花销过高，规划效率低。而针对局部避障准确率等问题，一些局部规划算法被提出，如动态窗口法和人工势场法（Artificial Potential Field， APF）等[10]。动态窗口法是一种有效的局部路径规划方法，但其高度依赖于全局参考，难以实现未知环境路径规划[11]。传统APF算法在规划过程中可能陷入局部最优。为此，文献[12]提出改进的APF方法，根据不同障碍物和道路边界对势函数进行不同赋值，以提高连续性和灵敏性，但在多障碍物离散分布时可能存在换道不平顺、目标不可达等问题。局部路径规划算法很好地解决了局部避障的问题，但在动态环境下的处理能力较弱，且未考虑多障复杂分布的情况，存在目标不可达、易陷入局部最佳等问题。

综上，仅靠单一全局或局部规划算法无法较好解决复杂动态场景下的实时、高精度避障等问题，因此可考虑集成两种路径规划方法有效解决。本文针对巡检机器人在复杂动态环境下路径规划存在规划路径非全局最优、路径不平滑及局部避障效果不佳等问题进行深入研究，提出了一种融合改进D*Lite算法和人工势场法的路径规划方法。在复杂动态场景下，融合算法能在全局路径规划的同时，实时分析局部目标点并做出合理的避障操作，可极大地提高巡检小车路径规划的准确性和规划效率。

1" 改进D*Lite算法

传统D*Lite算法是一种采用逆向搜索策略的全局路径规划算法，其核心是结合增量式搜索和局部修正实时更新路径[12]。但搜索策略会将路径的偏转角度限制在固定的角度，导致在动态巡检环境中规划路径存在冗余拐点，路径不平滑，无法应用于实际的工作场景。针对上述D*Lite算法存在的问题，本文提出如下两点改进。

1.1" 改进启发代价函数

启发函数的选择较大程度影响了D*Lite算法性能和路径规划质量，使用原始启发函数找寻最优路径可能会降低搜索效率。本文引入切比雪夫距离计算方法来改进启发代价函数，使启发值准确化。切比雪夫距离计算方法公式如下：

[hm（s）=D·maxxa-xb，ya-yb]" " （1）

式中：[（xa，ya）]为实时节点[a]坐标；[（xb，yb）]为规划目标点[b]坐标；常数[D]表示栅格地图中相邻栅格物理距离。通过引入节点与目标点的相对距离值，改进启发代价函数，使启发值准确化，在路径规划中可避免盲目试错，寻径耗时长，有效优化D*Lite算法的路径规划效果，改进如下：

[Δx=xa-xbΔy=ya-yb]" （2）

[h（s）=D·Δx+Δy-2·min（Δx，Δy）Dxy·Δx2+Δy2] （3）

式中：[Δx]、[Δy]分别为节点[a]与目标点[b]的相对横向距离值及相对纵向距离值；[Dxy]为斜穿格子距离代价系数；[D·Δx+Δy-2·min（Δx，Δy）]为直穿格子的代价值；[Dxy·Δx2+Δy2]为斜穿格子的代价值。

1.2" 平滑处理

Dubins曲线是在指定起点和目标点方向下连接二维平面的最短路径，呈现序列CCC或CSC（[C]为圆弧段，[S]为直线线段）[13]。本文采用Dubins曲线中最短可接受路径CSC曲线处理冗余转折点，使得起始点和目标点的距离始终能满足弧面曲率最小。基于原始Dubins曲线计算方法，设定最小拐弯半径为[Rmin]，每条路径的曲率半径[rc]为[ρi]，且曲率[Kmin=1rc]。根据曲率公式，所规划路径需满足以下约束条件：

[mini=1nKmin，" " ∀rc≥Rmin] （4）

[Kmin=ρ″i1+ρ′i223] （5）

通过该约束确保路径曲率半径不小于搜索单元的最小转弯半径，使得规划路径总曲率最小且路径更加平滑。如图1所示，节点[Sa]和[Sb]连接形成的线段[S]、以[Sc1]为中心的弧段路径[lc1]及以[Sc2]为中心的弧段路径[lc2]共同构成了由[Sstart]到[Send]的Dubins曲线。相比原始未经平滑处理的路径（图中黑色折线），经过曲线平滑处理后所得路径（图中弧线）更加平滑。

2" 改进人工势场法

人工势场法是一种基于势能的局部避障路径搜索算法，其工作原理是通过计算机器人所处位置与目标位置的势场之和，使机器人沿着势场负梯度方向移动，从而实现机器人路径规划[13]。人工势场法广泛应用于实时避障，但该算法仍存在目标不可达、易陷入局部最优解等问题，亟待解决。针对人工势场法存在的上述问题，本文提出如下两点改进。

2.1" 添加随机扰动

当规划路径存在局部凹形区域时，APF目标点吸引力[Fatt（p）]与障碍物斥力[Frep（p）]大小相等、方向相反、陷入局部最优、优化停滞。本文通过添加随机次目标吸引扰动点解决该问题，扰动点方向为目标点吸引力与障碍物斥力中垂线方向。其中，APF吸引力为引力场函数[Uatt（p）]的负梯度，其函数如下所示：

[Uatt（p）=12k（p-pgoal）2]" " "（6）

[Fatt（p）=-gradUatt（p）=-k（p-pgoal）]" " " " （7）

式中：[k]为常数，表示尺度因子；[p-pgoal]表示巡检小车所处位置与目标点之间的距离。取距离巡检小车物理长度如下：

[dgoal=p-pobsn] （8）

式中：[p-pobs]为障碍物与巡检小车停滞时物理距离；[n]为距离尺度。将[dgoal]代入目标点吸引力函数中，可推出随机扰动点引力[F'att（p）]如下所示：

[U'att（p）=12kp-pgoal22]" " " " " " （9）

[F'att（p）=-gradU'att（p）=-14k（p-pobs）] （10）

此时巡检小车受力打破平衡态，由局部极值停滞转为正常执行工作状态。

2.2" 修改势场函数

当巡检小车距离目标点较远时，吸引力过大，障碍物相对排斥力小，易出现局部碰撞，故对引力势场函数进行分段，添加虚拟距离阈值[pdis]，以避免碰撞发生，如下式所示：

[U'att（p）=12k（p-pgoal）2，" " p-pgoallt;pdispdiskp-pgoal-12kp2dis，" " p-pgoal≥pdis]" （11）

当障碍物距离目标点较近时，易出现斥力[Frep（p）]大于目标点吸引力[Fatt（p）]，导致目标点不可达甚至越过目标点。故加入目标点尺度校准[（p-pgoal）n]改进原始斥力场函数[Urep（p）]，避免出现上述现象，公式如下：

[Urep（p）=12kp1p-pobsmin-1ρ0，" " p-pobslt;ρ00，" " p-pobs≥ρ0] （12）

[U'rep（p）=12kp1pn-pobsmin-1ρ02（pn-pgoal）n，" " " " " " pn-pobslt;ρ00，" " " " pn-pobs≥ρ0]" "（13）

式中：[kp]为斥场函数尺度因子；[p-pobsmin]为巡检小车所处位置与障碍物间的最短距离；[ρ0]是一个常数；[n]是大于零的任意常数，用于引入巡检小车与目标相对距离，保证整个势场仅在目标点[pgoal]全局最小。当距离目标点越小，[（p-pgoal）n]逐渐减小，斥力场[U'rep（p）]随之减小，即障碍物只对一定范围内物体产生斥力，超出范围则不受斥力影响。

2.3" 融合算法

改进D*Lite算法有较好的全局搜索能力，但在重规划过程中存在避障不佳的问题，而引入人工势场法可有效解决。为充分利用两种算法的优势，提出路径规划融合策略，具体实现步骤如下：

步骤1：获取已知环境信息，利用栅格法构建环境模型并标记障碍与可通行空间。

步骤2：利用改进D*Lite算法规划出一条从起点到目标点的全局路径，根据全局路径上的路径点，选择每隔[i]个单位路径点作为一个局部目标点，记录每个局部目标点的坐标。

步骤3：当环境中出现新障碍物时，在地图中更新障碍物位置信息，由当前寻路栅格切换为避障栅格，将由D*Lite正常规划切换为改进后的局部势场法执行实时避障，绕开障碍物后，回到全局规划路径并沿着当前路径继续前进。若到达当前局部目标点，则切换到下一目标点。

步骤4：判断到达的当前目标点是否为全局目标点，若是，则路径规划完成，任务结束；若否，则继续执行步骤3。

通过执行以上步骤1～步骤4，本文所提出的路径规划融合算法能有效地完成路径规划工作。

3" 相关实验

实验环境建模采用栅格法，仿真地图环境尺寸置为20×20单位，其中白色栅格表示可通行区域，黑色栅格表示随机障碍物区域，起始点为物理坐标在左上，终止点为右下。

3.1" 改进D*Lite算法仿真实验

3.1.1" 遍历节点数对比

在改进启发代价函数前后，D*Lite算法路径规划如图2所示。

从仿真实验结果可以看出，改进切比雪夫距离启发值后，D*Lite算法规划路径节点数明显减少，规划效率提高。

3.1.2" 路径平滑对比

平滑处理前后规划路径对比如图3所示。为模拟动态环境，实验设置不同障碍比，分别为0.15和0.35。

通过实验对比发现，平滑处理后路径相较于处理前冗余点数量减少，寻迹路程更短且路径更平滑。此外，在图3b）组预设凹形障碍物区域，执行任务时该区域出现寻迹死锁现象，规划失败。由此表明：改进的单一D*Lite算法虽有效减少了冗余节点，缩短了寻迹路程，但在预设凹形障碍物的情况下，局部极值问题并不能解决。

3.2" 改进人工势场法仿真实验

为验证改进APF避障效果，设置两组实验。实验一验证添加随机扰动点是否能解决局部极值问题；实验二验证优化势场函数是否能解决目标不可达问题。实验起点、终点坐标均随机设定，但为对比算法改进前后规划路径的差异，均提前预设凹形区域。实验起始点均置为（2，2），对比结果如图4所示。

图4a）中传统势场法使得巡检小车陷入局部极值，任务失败。改进势场法图4b）通过在小车[p-pobs2]半径范围添加随机扰动点，打破相持状态，使得合力未在到达目标点位置时不为零，巡检小车到达目标终点。图4c）中由于目标点位置与凹形障碍物距离过近，斥力与吸引力合力为零，巡检小车停滞不前，出现目标不可达。图4d）中，当目标点与障碍物距离越近，斥力场逐渐减小，巡检小车绕过障碍物，顺利到达目标点位置。通过仿真结果可知，采用改进后的势场法进行局部路径规划能有效解决局部避障相关问题。

3.3" 融合算法仿真实验

为验证融合算法综合性能，将算法与单一D*Lite算法进行三组仿真实验。三组障碍比分别设置为0.25、0.35及0.45，对比结果如图5所示。

当障碍比为0.25时，图5a）面对局部侧方障碍物遮挡时出现死锁现象，发生碰撞；图5b）通过局部随机扰动点，成功跳出局部死锁区域，到达目标点位置。当障碍比为0.35时，图5c）在凹形区域位置与障碍物发生局部碰撞；图5d）融合算法规划路径成功，逸出局部极值区域。障碍比增加至0.45时，图5e）在凹形区域出现连续碰撞现象，规划任务失败；图5f）通过融合算法优势，避过凹形死区，到达目标点。通过实验对比可知，相较单一规划算法，融合算法避障表现性能更优，避障成功率较大。

为对算法融合前后性能表现进行量化分析，记录了D*Lite和融合算法在不同障碍比下的路径长度、冗余拐点数、遍历节点数及时间花销，如表1所示。

对比实验数据发现，在不同障碍比下，融合算法相较于单一D*Lite算法路径长度约缩短12.3%，冗余拐点数约缩短35.51%，遍历节点数约缩短59.77%，时间花销约缩短51.04%。融合算法性能指标表现均优于单一算法，达到预期效果，证明了该融合算法的有效性和优越性。

4" 结" 语

本文提出了一种高效、性能更优的融合规划算法，旨在解决巡检机器人在动态场景下路径规划非全局最优、无法实时有效避障的问题。融合算法将优化后的全局规划D*Lite算法和局部避障人工势场法有机结合起来，相互配合搜索到的路径在时间计算、路径长度和平滑性等方面均优于单一算法，既保证了动态环境避障的准确率，又极大提升了全局路径规划效率。

注：本文通讯作者为曾维。

参考文献

[1] RAVANKAR A A， RAVANKAR A， EMARU T， et al. HPPRM： hybrid potential based probabilistic roadmap algorithm for improved dynamic path planning of mobile robots [J]. IEEE access， 2020， 8： 221743⁃221766.

[2] CHEN H C. Monocular vision⁃based obstacle detection and avoidance for a multicopter [J]. IEEE access， 2019， 7： 167869⁃167883.

[3] CAMPBELL S， O′ MAHONY N， CARVALHO A， et al. Path planning techniques for mobile robots： A review [C]// International Conference on Mechatronics and Robotics Engineering. Heidelberg， Germany： Springer International Publishing， 2021： 12⁃16.

[4] LIKHACHEV M， FERGUSON D， GORDON G， et al. Anytime search in dynamic graphs [J]. Artificial intelligence， 2008， 172（14）： 1613⁃1643.

[5] 宋宇，顾海蛟，程超.基于改进蚁群算法的无人机航迹规划研究[J].现代电子技术，2022，45（4）：123⁃127.

[6] LI J， JI M， LIU L， et al. Fusion of improved A* algorithm and dynamic window approach for path planning [C]// 2022 China Automation Congress （CAC）. New York： IEEE， 2022： 390⁃394.

[7] SUN X， WANG G， FAN Y， et al. Collision avoidance control for unmanned surface vehicle with COLREGs compliance [J]. Ocean engineering， 2023， 267： 113263.

[8] JIN J， ZHANG Y， ZHOU Z， et al. Conflict⁃based search with D* lite algorithm for robot path planning in unknown dynamic environments [J]. Computers and electrical engineering， 2023， 105： 108473.

[9] XIE K， QIANG J， YANG H. Research and optimization of D⁃Start Lite algorithm in track planning [J]. IEEE access， 2020， 8： 161920⁃161928.

[10] LIU L， WANG X， YANG X， et al. Path planning techniques for mobile robots： Review and prospect [J]. Expert systems with applications， 2023， 227： 120254.

[11] CHANG L， SHAN L， JIANG C， et al. Reinforcement based mobile robot path planning with improved dynamic window approach in unknown environment [J]. Autonomous robots， 2021， 45（1）： 51⁃76.

[12] HUANG T， HUANG D， QIN N， et al. Path planning and control of a quadrotor UAV based on an improved APF using parallel search [J]. International journal of aerospace engineering， 2021（1）： 1⁃14.

[13] GHADIRY W， HABIBI J， AGHDAM A G， et al. Optimal path tracking with Dubins′ vehicles [J]. IEEE systems journal， 2020， 15（1）： 466⁃477.

[14] 连云霞，樊永生，余红英，等.改进D*Lite算法在虚拟士兵路径规划中的应用[J].现代电子技术，2018，41（6）：23⁃27.

[15] 李靖，杨帆，韩艳芬，等.基于动态引力场的机器人路径规划研究[J].现代电子技术，2020，43（11）：41⁃46.