基于改進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.