改進人工勢場法的移動機器人避障軌跡研究

李二超，王玉華

蘭州理工大學 電氣工程與信息工程學院，蘭州 730050

隨著人工智能的快速發展，移動機器人被廣泛應用于工業、軍事以及醫學等領域。而路徑規劃算法被廣泛地應用在移動機器人的導航中，機器人如何從所處的環境中搜索一條從初始位置到終點的安全、無碰撞的最優路徑是路徑規劃的關鍵[1]。常用的算法有A*算法、蟻群算法（ant colony optimization，ACO）、Dijkstra算法、遺傳算法（genetic algorithm，GA）、快速隨機搜索樹法（rapidlyexploring random tree，RRT）、粒子群算法（particle swarm optimization，PSO）和人工勢場法（artificial potential field，APF）等[2]。其中，A*算法引入了啟發函數避免了路徑規劃問題中產生大量的無效搜索路徑；ACO是一種啟發式隨機搜索算法，存在計算量大、收斂速度慢等問題[3]；Dijkstra算法主要特點是由中心向外擴散，直到擴展到終點為止，但其遍歷計算節點較多，效率較低[4]GA得到的路徑符合現實中路徑選擇需求，但運算量大且耗時長，無法滿足實時性的需求[5]；RRT是一種基于采樣的概率算法，其特點主要是在空間中隨機生成新節點并在新節點上繼續向目標點延伸，但存在路徑不平滑，搜索路徑一直變化的情況[6]；PSO同GA都是迭代算法，相較于GA，PSO收斂速度較快，但粒子同一化后，收斂速度較慢，且收斂精度不夠。

人工勢場法在路徑規劃中只需要根據當前機器人的位姿以及障礙物來確定下一步的運動，算法本身具有計算量小、反應速度快、生成路徑平滑等優點被廣泛用于運動規劃系統的移動機器人中。Rostami等[7]在傳統人工勢場函數中加入調節因子來繞過障礙物從而克服局部最小值和目標不可達問題；Yao等[8]從強化學習的視角來解決復雜環境問題，將黑洞人工勢場和強化學習結合來解決局部穩定點問題，但在路徑規劃過程中會出現振蕩現象；趙明等[9]提出域來解決多目標點任務的局部穩定點問題，在機器人陷入局部極小值時通過域場逃離局部極小點，但未考慮多復雜環境下機器人能否到達目標點；何乃峰等[10]基于模擬退火算法，重新構建斥力勢場函數模型，從而設計逃離局部極小值控制器進行全局路徑，全局路徑平滑，但所規劃路徑并不是最優路徑；徐小強等[11]針對陷阱區域和局部極小值問題，提出提前預測人工勢場法，但對于較多障礙物的復雜環境而言，仍不能完整規劃路徑；王迪等[12]提出基于虛擬目標點和環境判斷的改進方法，實現移動機器人的局部路徑規劃，但在遇到陷入型障礙物時規劃出“沿壁走”的路徑存在消耗程距的問題；曹凱等[13]設計了一種由人工勢場引導RRT*進行路徑規劃的方法來解決障礙物密集、通道狹窄的環境，但面對陷入型環境問題時，偏向區域搜索會導致可行區域的收斂性，使路徑規劃采樣點不完整，無法完成路徑的完整性；高志偉等[14]提出將軌跡跟蹤和機器人動力學控制模型相結合，實現全局路徑規劃點作為局部目標點來得到能耗最優軌跡，但這種規劃方式面對更復雜環境時全局路徑規劃點和局部路徑規劃點的判斷無疑加大了計算時間和耗能。

針對上述算法的不足，本文提出了一種簡化障礙物預測碰撞人工勢場法（simplified obstacles and predict collision of artificial potential field method，SOPC-APF）。首先，加入預測碰撞思想，幫助機器人在未知環境中通過傳感器探測進行提前預判，避免機器人陷入局部最優；其次簡化機器人受障礙物影響范圍內的障礙物——即只考慮目標點一側障礙物斥力的影響；最后通過虛擬目標點的加入，采用改進斥力因子影響范圍的斥力函數引導機器人快速、平穩、無碰撞地到達目標點。最后在仿真平臺對SOPC-APF進行可行性的驗證。

1 APF算法基本原理

人工勢場法作為路徑規劃算法中廣泛應用的一種算法，最早由Khatib[15]所提出。在人工勢場算法中，將機器人的運動環境看作為一個虛擬力場，目標點和障礙物在機器人運動過程中產生引力和斥力，通過兩者的合力作為機器人運動的加速度來控制機器人向目標點移動。

假設移動機器人在二維有限空間內運動受力分析如圖1所示，且機器人在二維有限空間中的位置表示為q=(x,y)T，目標點位置表示為qtarg=(xT,yT)T，障礙物位置表示為qobs=(xo,yo)T。

圖1 傳統人工勢場法受力分析Fig.1 Force analysis of traditional APF

目標點對機器人產生的引力場為Uatt(q)，障礙物對機器人產生的斥力場為Urep(q)，即機器人最終所受到的合力場為Utotal(q)，則可表達為：

式中，α為斥力勢場增益系數，‖q-qobs‖為機器人與障礙物之間的歐幾里德距離，ρo為障礙物斥力場對機器人產生斥力作用的影響距離，若‖q-qobs‖小于該值，受斥力作用，反之不受作用。

引力勢場函數的負梯度為機器人所受到的引力為：

當機器人處于各障礙物影響距離內時，機器人所受到的合斥力為，即機器人所受到的合力Ftotal為：

2 SOPC-APF算法

針對APF算法存在的缺陷，本文提出了SOPC-APF算法，該算法能夠在未知場景中根據傳感器探測出的環境信息，提高APF算法的全局搜索能力，減小某些局部震蕩產生的局部路徑消耗。SOPC-APF算法首先對機器人前方的障礙物進行預測碰撞處理，從而簡化影響范圍內的受限障礙物（機器人當前位置與目標點之間存在的障礙物稱為受限障礙物）；在簡化后的受限障礙物中通過旋轉角度來設置最優的虛擬目標點；最后結合虛擬目標點的軌跡找尋真實目標點，從而生成一條路徑平滑且連續的最優路徑，從而使移動機器人沿著該全局優化路徑快速、平穩、安全地到達目標點。SOPC-APF算法的大體流程圖示意如圖2所示。

圖2 SOPC-APF流程圖Fig.2 Flow chart of SOPC-APF

針對移動機器人在多障礙物影響范圍內時受多個障礙物各個方向的影響出現合力為零或者機器人在某一范圍內來回振蕩的情況，本文提出一種簡化障礙物的思想，即移動機器人只考慮在目標點一側障礙物的斥力函數影響，如圖3所示。在簡化過程中，令移動機器人在當前位置Xj處剔除(O1,O5,O6)，若保留障礙物(O1,O5)，其產生的斥力F1,5對逃離(O2,O3,O4)組成的受限障礙物的斥力有較大影響，導致與目標點產生的引力發生不必要的振蕩，使后續的路徑規劃算法效率降低，所有在當前位置Xj處，考慮(O1,O5)是完全沒有必要的。

圖3 簡化障礙物圖Fig.3 Map of simplified obstacles

定義1機器人處于n個障礙物影響距離內時，機器人只受m(m∈n)個障礙物斥力作用，則移動機器人所即機器人所受到的合力受到的合斥力為Ftotal為：

2.1 碰撞預測

當移動機器人通過傳感器檢測出地圖存在陷阱區域時，如U型障礙物等復雜障礙物，傳統的人工勢場法由于算法自身的缺陷會出現震蕩或停止的現象，以致于不能完整地規劃出路徑。因此本節引入了預測距離和虛擬目標點來引導移動機器人繞開陷阱區域，規劃完整路徑。

2.1.1 預測距離

針對陷阱區域的提前預知，如U型，陷入型障礙物，傳統算法會在陷阱區域內發生震蕩或者停止，無法規劃出完整路徑。因此引出預測距離Rp，通過比較移動機器人到簡化障礙物之間的距離r0來判斷自己的下一步位置，若小于，通過偏轉角度來設置虛擬目標點；否則按照初始方向向目標點靠近。對于偏轉角度的實現以移動機器人當前位置為圓心，m個障礙物距離機器人最遠的障礙物為半徑偏轉角度γ來繞開m個障礙物。使每次旋轉3°直到簡化后的障礙物在機器人安全范圍之外停止旋轉。則偏轉角度可表示為：

2.1.2 虛擬目標點

通過偏轉角度γ，在m個障礙物中距離機器人最遠的障礙物為半徑的另一端設置虛擬目標點，在虛擬目標點的引導下，使機器人向虛擬目標點移動，到達虛擬目標點后主動撤銷虛擬目標點。重復以上步驟，直到機器人到達目標點。找尋最優虛擬目標點的偽代碼如算法1。

算法1設置虛擬目標點算法

2.2 改進斥力函數

在二維有限空間中，改進斥力場函數為：

改進受力分析如圖4所示。其中，FR1是m個障礙物對機器人的斥力分量，FR2是目標點對機器人的引力分量。

圖4 改進勢場函數受力圖Fig.4 Force analysis of improved APF

算法2改進斥力場函數算法

3 算法仿真及結果分析

為驗證SOPC-APF算法的可行性和路徑規劃的效果，在實驗平臺中進行仿真研究，研究過程中，將移動機器人視為一個質點，分別對傳統人工勢場法和改進人工勢場法在目標不可達、局部極小值點以及復雜環境下地圖中做出對比，從而驗證出SOPC-APF的優越性。

3.1 基礎地圖算法對比分析

若將初始參數設置如下：機器人初始位置Xo=( 0,0)，目標點位置Xg=(1 0,10)，引力系數ks=15，斥力1系數α=1.1，斥力2系數M=2，障礙物影響范圍ρo=2.5，預測碰撞距離Rp=2，步長L=0.1，障礙物半徑R=0.5。圖5所示為移動機器人-障礙物-目標點目標點不可達問題。

圖5 目標不可達問題Fig.5 Problem of unreachable goals

圖5（a）為三點共線問題，障礙物處于移動機器人和目標點兩點之間的連接線上，傳統算法在A點時斥力引力產生的合力在某一范圍震蕩，導致傳統算法無法逃出該范圍到達目標點；SOPC-APF在B點碰撞預測后發現障礙物，及時偏轉角度脫離合力震蕩范圍，從而到達目標點。圖5（b）中目標點處于障礙物的影響范圍內且距離障礙物較近，因此傳統算法會受到的斥力比較大，引力較小，會出現在目標點附近抖動現象；SOPC-APF則在B點處對障礙物的預判，從而通過偏轉角度設置虛擬目標點，到達虛擬目標點后重新規劃到達目標點的路徑。

圖6 局部極小點問題Fig.6 Problem of local minimum

設置參數如下：機器人初始位置Xo=(5 .5,2.5)；目標點位置Xg=(1 0.5,10.5)。圖7為陷入陷阱區域問題，機器人起點被障礙物包圍，傳統算法在到達A點處受到合斥力影響，在A點處產生振蕩，無法逃脫半包圍障礙物；SOPC-APF在起點處通過預測距離判斷出移動機器人與目標點之間已存在障礙物，則在全局障礙物中找出所有與該障礙物相連的障礙物，即受限障礙物，對受限障礙物及時旋轉角度設置虛擬目標點B，從而逃出半包圍障礙物，規劃處達到目標點路徑。

圖7 陷入問題Fig.7 Problem of trapping

仿真數據結果如表1所示，驗證了SOPC-APF對于目標不可達和局部極小值點問題路徑規劃的可行性和安全性。

表1 基礎地圖算法對比數據Table 1 Basic map algorithm compares data

3.2 復雜地圖算法對比分析

若將基礎參數設置如下：機器人初始位置Xo=( 0.5,0.5)，目標點位置Xg=(1 9.5,19.5)，引力系數ks=15，斥力1系數α=1.1，障礙物影響范圍ρo=2.5，預測碰撞距離Rp=4，步長L=0.2。在不同復雜障礙物下對本文算法、傳統人工勢場法、文獻[16]中改進的蟻群算法（記作A-ACO）、文獻[7]改進的人工勢場法（記作BAPF）、文獻[11]改進的人工勢場法（記作C-APF）進行多次比較，從而驗證本文算法可以克服人工勢場在復雜障礙物不適用等問題。傳統人工勢場法由于自身的缺陷，在復雜環境中會出現振蕩現象，無法成功完成路徑規劃；A-ACO算法屬于全局路徑規劃算法可以很好地避障從而完成路徑規劃，但相比較人工勢場法運行時間較長；B-APF在遇到障礙物時通過繞過受限障礙物來規劃路徑，這樣導致路徑過長；C-APF引入預測距離和機器人安全距離，在障礙物受限之前作出反應，通過設置虛擬目標點規劃出來的路徑較短；SOPC-APF在C-APF基礎上對受限障礙物進行簡化處理，在斥力函數中加入目標點和機器人的相對距離，通過障礙物影響范圍內M取值情況的分類，從而使機器人的規劃路徑最短且平滑。

3.2.1 隨機離散障礙物地圖

若障礙物環境是離散分布的，幾種算法在同一仿真環境下運行，設置M=0.5，R=0.65。仿真對比結果如圖8（a）所示。圖8（b）為SOPC-APF的引力和斥力能量圖，引力的大小趨于0，表示移動機器人已到達目標點，從圖8（b）中可知機器人抵達目標點步數不超過150步，且機器人合力方向較平穩，說明生成路徑較光滑。從表2數據對比可以看出，SOPC-APF相較于A-ACO算法速度提高99.05%，轉折數量減少了1個；相較于B-APF算法路徑縮短了37.78%，算法速度提高了67.95%；相較于C-APF算法轉折數量減少了1個。

圖8 離散障礙物中SOPC-APF路徑規劃Fig.8 SOPC-APF path planning in discrete obstacles

3.2.2 L型、離散障礙物地圖

若障礙物環境中存在L型障礙物和離散障礙物，幾種算法在同一仿真環境下運行，設置M=2，R=0.9。仿真對比結果如圖9（a）所示，傳統算法未達成功到達目標點。圖9（b）為SOPC-APF的引力和斥力能量圖。從表3數據對比可以看出，SOPC-APF相較于A-ACO算法路徑縮短了5.48%，速度提高了99.05%，轉折數量減少了6個；相較于B-APF算法路徑縮短了3.50%，算法速度提高了30.0%；相較于C-APF算法路徑縮短了2.13%。

圖9 L型、離散障礙物中SOPC-APF路徑規劃Fig.9 SOPC-APF path planning in L-shaped and discrete obstacles

表3 L型、離散障礙物路徑規劃算法數據對比Table 3 Comparison of path planning algorithms for L-shaped and discrete obstacles

3.2.3 U型、離散障礙物地圖

若障礙物環境中存在U型障礙物和離散障礙物，幾種算法在同一仿真環境下運行，設置M=1.5，R=1。仿真對比結果如圖10（a）所示，（b）為SOPC-APF的引力和斥力能量圖。從表4數據對比可以看出，SOPC-APF相較于A-ACO算法路徑縮短了2.05%，速度提高了99.06%，轉折數量減少了10個；B-APF算法在該地圖中由于陷入障礙物太深，無法通過B-APF算法繞出，因此路徑規劃失敗；相較于C-APF算法路徑縮短了2.72%。

圖10 U型、離散障礙物中SOPC-APF路徑規劃Fig.10 SOPC-APF path planning in U-shaped and discrete obstacles

3.2.4 復雜障礙物地圖

（1）若存在密集障礙物等復雜環境下，幾種算法在同一仿真環境下運行，設置M=1.5，R=0.5，Rp=6。仿真對比結果如圖11（a），圖11（b）為SOPC-APF的引力和斥力能量圖。

圖11 復雜障礙物中SOPC-APF路徑規劃Fig.11 SOPC-APF path planning in complex obstacles

從表5數據對比可以看出，SOPC-APF相較于AACO算法路徑縮短了4.83%，速度提高了99.06%，轉折數量減少了7個；B-APF算法路徑縮短了56.15%，速度提高了94.61%；C-APF算法由于該地圖過于復雜無法完整的規劃出路徑。

表5 復雜路徑規劃算法數據對比Table 5 Comparison of complex obstacles path planning algorithm data

（2）若移動機器人起點處于半包圍障礙物等復雜環境下，幾種算法在同一仿真環境下運行，設置M=1.5，R=0.5，Rp=4。仿真對比結果如圖12（a）所示，圖12（b）為對應的的引力和斥力能量圖。

圖12 復雜障礙物中SOPC-APF路徑規劃Fig.12 SOPC-APF path planning in complex obstacles

從表6數據對比可以看出，SOPC-APF相較于AACO算法路徑縮短了1.05%，速度提高了98.52%，轉折數量減少了8個；B-APF和C-APF算法由于陷入障礙物內部，無法完整地規劃出路徑。

表6 復雜路徑規劃算法數據對比Table 6 Comparison of complex obstacles pathplanning algorithm data

圖13為上述環境下移動機器人的合力方向的對比分析，而移動機器人面對目標點的角度問題也可以看作移動機器人相對路徑平穩性的評估，從對比圖中不難看出：在相同仿真環境下，傳統算法面對目標不可達、局部極小值以及陷入問題時出現的震蕩問題，使移動機器人路徑波動大，穩定性不理想；SOPC-APF在設定迭代次數內可到達目標點，且運行路徑較平滑，無震蕩現象出現。

圖13 合力方向對比分析Fig13 Comparative analysis of total force direction

通過以上仿真實驗結果得到如下結論：當環境復雜，存在陷阱區域、初始位置陷入環境以及密集障礙物時，SOPC算法可以安全地逃離或避開陷阱區域和穿越密集環境，且軌跡平滑；當地圖中存在較為簡單的目標不可達和局部極小點問題的障礙物環境時SOPC算法也可以規劃出相比人工勢場法更加簡潔的軌跡，且無震蕩軌跡發生。

4 結論

針對傳統人工勢場法目標不可達，易陷入局部最優，且不適用于復雜環境下等問題導致移動機器人無法規劃出完整路徑，因此本文提出一種改進——基于簡化障礙物預測碰撞人工勢場法。首先，在機器人移動前對前方路徑進行預測判斷；在預測到存在障礙物后，簡化受限障礙物，即機器人只受影響范圍內目標一側障礙物的斥力影響；隨后在簡化后的障礙物附近設置合理的虛擬目標點，經改進的斥力函數引導機器人在復雜環境中快速生成一條路徑平滑、平穩、無碰撞的路徑。

仿真實驗表明，SOPC-APF有效解決了人工勢場法不適用于多障礙物復雜環境的問題，以及傳統算法容易陷入陷阱區域和局部極小點問題。下一步將考慮對于動態環境和突發情況下，機器人如何進行實時避障，為路徑規劃的真實性和適用性做近一步的研究。