基于動態引力場的機器人路徑規劃研究

李靖 楊帆 韓艷芬 陳虹

摘? 要： 人工勢場法對機器人進行路徑規劃時，存在障礙物使目標不可達、死鎖和局部極小點等問題，容易導致路徑規劃失敗，此外發現即使路徑規劃成功，由于斥力的存在，必定使得規劃路徑不是最短、最優的。針對上述問題提出一種動態引力場路徑規劃算法，該算法完全去掉斥力場，將引力場的大小及方向動態化，虛擬目標點動態化，不斷交替尋找最優或次優方向進行避障，動態地修正路徑，最后對路徑進行平滑優化，規劃出最短、最優的路徑。該方法解決了人工勢場法固有的缺陷，同時優化了規劃路徑的長度，仿真實驗結果說明該算法具有實用性和有效性。

關鍵詞： 機器人路徑規劃; 動態引力場; 動態路徑修正; 路徑長度優化; 路徑平滑優化; 路徑優化對比

中圖分類號： TN99?34; TP242? ? ? ? ? ? ? ? ? ? ?文獻標識碼： A? ? ? ? ? ? ? ? ? ? ?文章編號： 1004?373X（2020）11?0041?06

Research on robotic path planning based on dynamic gravitational field

LI Jing1， YANG Fan1， HAN Yanfen2， CHEN Hong2

（1. School of Electronic & Information Engineering， Hebei University of Technology， Tianjin 300401， China;

2. Tianjin Light Industry Vocational Technical College， Tianjin 300350， China）

Abstract： Some problems may occur when the artificial potential field method is applied to robot path planning， for example， target is unreachable due to obstacles， deadlock and local minimum point may occur， which can easily lead to failure of path planning. In addition， it is found that even if the path planning is successful， the planned path is definitely not the shortest and optimal due to the existence of repulsive force. In view of the above， a path planning algorithm based on dynamic gravitational field is proposed. In the algorithm， the repulsive force field is completely removed， both the size and direction of gravitational field and the virtual target point are changed dynamically to continuously search the optimal or suboptimal directions alternately for the obstacle avoidance， so that the path is corrected dynamically. In the end， the path is subjected to evenness optimization and the shortest and optimal path is planned. With this method， the inherent defects of the artificial potential field method are removed， and the length of the planning path is optimized. The simulation results show that the algorithm is practical and effective.

Keywords： robotic path planning; dynamic gravitational field; dynamic path correction; path length optimization; path evenness optimization; path optimization contrast

0? 引? 言

隨著機器人技術飛速發展，路徑規劃或障礙物規避問題成為機器人調度策略等控制領域研究的重點和熱點問題之一，是機器人順利完成各項作業任務的前提條件。機器人路徑規劃技術是根據規劃空間中障礙物信息，在滿足一定約束條件和評價指標的條件下，線下或線上為機器人規劃出從出發點到目標點合理避開障礙的滿意路徑。

機器人路徑規劃根據環境信息掌握情況[1]，可以分為全局規劃和局部規劃搜索算法。全局規劃算法主要應用于障礙物地圖信息已知的情況，啟發式算法運用居多，如遺傳算法[2]、粒子群算法[3]等。局部規劃搜索算法主要是根據實時環境信息確定的環境地圖，進行障礙物避障的路徑規劃方法，主要有人工勢場法[4?6]、模糊邏輯算法[7]、A*算法[8]等。此外，還有深度學習路徑規劃算法[9?10]，深度學習通過模擬大腦神經元網絡進行機器學習分析，對路徑規劃領域帶來了飛躍式發展。但其需要大數據集的支撐，模型復雜化，訓練、驗證、測試迭代導致時間復雜度大，且需要高要求的硬件支撐，對小樣本數據集無法進行無偏差的估計。因此，對于多機器人協同合作布置傳感器的一次性工作場景下，人工勢場法體現出了建模簡單、搜索路徑快捷且計算復雜度低的路徑規劃算法的優點，該算法借助虛擬力思想能搜索出比較平滑且安全的路徑。但人工勢場法會出現局部最小值及臨近目標點震蕩徘徊的現象，由于斥力的存在，其規劃的路徑一定不是最短、最優化路徑，且引力系數、斥力系數要根據不同的情況進行不同的設置，很難統一確定，在復雜的環境下會有局限性。

本文借助人工勢場法中虛擬力的思想，提出了動態引力場算法。借助引力場作用，將起點與目標點動態化，形成動態引力場，去掉斥力場作用，不但解決了傳統人工勢場法無法解決的四種情況，而且解決了由于斥力的存在，路徑不是最短的問題，可以得到最短、最優的路徑，且計算復雜度低，計算時間進一步減小。

1? 傳統的人工勢場法路徑規劃算法

1.1? 人工勢場法原理

1986年，Khatib將人工勢場算法引入到了機器人避障路徑規劃領域。人工勢場法[11]進行路徑規劃的基本思想是將機器人、障礙物、目標點簡化為一點，機器人在周圍環境中的運動抽象為在虛擬力場中的運動，即目標點對機器人產生引力場[Uatt]，障礙物對機器人產生斥力場[Urep]，機器人在斥力場和引力場的共同作用下向目標運動。

設機器人在空間中的位置為[X]，目標點位置為[Xg]，障礙物位置為[Xo]，引力勢函數和斥力勢函數可分別表示如下：

[Uatt（X）=12?k?X-Xg2] （1）

[Urep（X）=12?η?1ρ（X，Xo）-1ρo，? ? ρ（X，Xg）≤ρo0，? ? ρ（X，Xg）>ρo] （2）

式中：[k]和[η] 分別為引力和斥力增益系數;[ρ（X，Xg）]和[ρ（X，Xo）]分別為機器人到目標點與到障礙物的距離;[ρo] 為障礙物的影響半徑。障礙物對機器人的斥力和目標點對機器人的引力分別對應斥力場和引力場的負梯度，得到相應引力函數和斥力函數：

[Fatt（X）=-?Uatt（X）=k?ρ（X，Xg）] （3）

[Frep（X）=-?Urep（X）? ? ?=η?1ρ（X，Xo）-1ρo1ρ2（X，Xo），? ?ρ（X，Xo）≤ρo0，? ? ρ（X，Xo）>ρo] （4）

則機器人受到的合力為：

[Ftotal=Fatt+Frep] （5）

機器人在合力的作用下，朝目標點運動，其受力如圖1所示。

1.2? 傳統人工勢場法的缺陷

1.2.1? 目標不可達到

傳統人工勢場法在機器人還未到達目標點時，提前陷入局部勢場極小點而無法到達目標，形成目標不可到達的情況。主要體現在以下四種情況：

1） 搜索目標點正前方有障礙物的情況，即引力和斥力形成的合力運動方向被障礙物阻斷，使得機器人無法正常沿著合力的方向繼續前進，從而引起不可到達目標點的情況，如圖2a）所示。

2） 引力與斥力合力為0的情況，即引力和斥力大小相等方向相反，合力抵消為0，機器人無法找到運動方向，處于靜止狀態，從而使得目標不可達到，如圖2b）所示。

3） 臨近目標點斥力大于引力搜索失敗的情況，即目標點附近有大（多）障礙物時，可能引起障礙物產生的斥力大于引力，從而使得合力的方向偏向斥力，從而偏離目標點，甚至出現倒退現象，使得目標不可到達，如圖2c）所示。

4） 遇到陷阱會出現死鎖的情況，即合力的方向被障礙物阻斷，無法跳出陷阱，在陷阱內停止或死鎖，從而使得目標不可到達，如圖2d）所示。

1.2.2? 易陷入徘徊抖動狀態

在多個局部極小點附近徘徊抖動，當機器人處在多個局部極小點周圍時，若[ρ（X（n+m），X（n））≤ε]，則表明機器人在第[n]步到第[n+m+1]步中的[m]個位置上沒有實質性的位移，運動路徑出現了周期性徘徊，其中，[m=2，3，…]，[ε]為無窮小量，或者因為某種因素，合力方向突變，也容易導致徘徊抖動狀態。當機器人在運動過程中出現徘徊抖動現象時，盡管其最終可能到達目標點，但將嚴重影響路徑規劃質量，可行性極差，如圖3所示。

1.2.3? 規劃路徑非最短

為了解決傳統人工勢場法的固有缺陷，改進的人工勢場法主要通過構建合理的勢場函數，消除或減少局部極小點，從根本上解決局部最小值問題。在路徑搜索算法方面，主要是針對已經陷入局部極小點的情況，研究使用何種策略跳出局部極小點，使機器人重新回到正確的搜索狀態。但無論是修改勢場函數模型，還是在局部最小點處加入適當的跳出策略，斥力場依舊存在其中，只要斥力場存在，必定會使得機器人遠離障礙物邊緣行走，且斥力越大，偏離最大引力場的方向越多，路徑越長，如圖4所示，機器人并沒有貼合障礙物邊緣行走，從而出現使得路徑不是最短的情況，便有了繼續縮短路徑長度的空間。

2? 基于動態引力場的路徑規劃算法

2.1? 算法原理

2.1.1? 環境地圖柵格化[11]

根據機器人運動的有限場地大小，將工作環境進行柵格劃分，標定起點和目標點位置，如圖5所示。

根據人工勢場法原理中引力勢場確定每個柵格的勢場值，即：

[Uatt（X）=12?k?X-Xg2]

[Fatt（X）=-?Uatt（X）=k?ρ（X，Xg）]

2.1.2? 搜索區域

機器人在進行路徑搜索時有8個方位的搜索，即N，NE，E，SE，S，SW，W，NW，如圖6所示。

當搜索區域無障礙物時，將機器人搜索路徑轉化成相遇問題，假設起點與目標點均在做相向運動。首先，機器人在起點位置搜索相鄰8個方位，尋找目標最大引力勢場方向，并朝著此最大引力完全柵格移動，即朝著最大引力場方向進行移動，無需修改[θ]（偏移角）值。隨后，目標點根據新起點位置重新計算新起點最大引力勢場方向，并朝著最大引力完全柵格移動，使起點與目標點逐一進行相向運動，最大引力勢場根據起點和目標點位置的變化，不斷地被修正，形成動態引力勢場，當起點與目標點相遇（或距離小于一個步長，認為相遇）時，所記錄下來的運動路徑集便是規劃出的路徑。

當搜索區域有障礙物時，無論是起點還是目標點依舊搜索最大引力場柵格方向，若搜索到的最大引力場柵格有障礙物，將向次優引力場方向搜索，如該方向柵格無障礙物，便向次優引力勢場柵格移動，如圖6所示。此時最優引力勢場修正了[θ]角，起點與目標點逐一搜索路徑，進行相向運動，直到相遇（或距離小于一個步長，認為相遇）形成的運動路徑集便是規劃路徑。

2.1.3? 路徑平滑

為了進一步優化路徑長度，從起點節點開始，與其后不在同一直線上的節點依次相連，直到連接到某個節點時出現新生成的路徑上存在障礙物時停止，將起點節點與該節點的前一個節點相連作為新的路徑。將該節點作為起始節點繼續與其后不在同一直線上的節點相連。依此類推，直到連接到終點節點，如圖7所示，虛線為動態引力場規劃路徑，灰色路徑為最終平滑后的最短路徑。

2.2? 算法流程及分析

基于動態引力場的路徑規劃算法：首先要柵格化工作環境地圖，標定起點及目標點位置，通過引力勢場讓起點與目標點交替行駛一個步長，若最大引力場方向無障礙物，則沿著引力場方向運動，若最大引力場方向有障礙物，按照8個搜索方位，依次搜索次優方向，則對最大引力運動角修正[θ]角度，朝著次優引力場方向進行運動，直到起點與目標點相遇（或者起點與目標點小于一個步長，認為相遇），標定軌跡形成路徑集，并對軌跡進行路徑平滑的優化處理，從而得到最短、最優路徑，如圖8所示。

3? 算法仿真及結果分析

為了驗證算法的可行性，采用Matlab 2017軟件進行仿真分析。實驗的硬件平臺是Intel[?] CoreTM i5?4210U CPU @1.7 GHz 2.14 GHz處理器和4 GB RAM計算機。

3.1? 動態引力場算法仿真

為了檢驗動態引力場的機器人路徑規劃算法的性能，對人工勢場法的固有缺陷情況進行了對比仿真實驗。

3.1.1? 搜索目標點正前方有障礙物

人工勢場法中，當搜索到目標點正前方有障礙物，合力運動方向被障礙物阻擋，或者引力與斥力合力為0，無法到達目標點。而動態引力場法起點與目標點交替搜索，引力場在不斷被修正，形成動態引力場，且搜索遇到障礙物，將向次優方向移動，能夠沿著障礙物的邊沿，繞開障礙物做相向運動，相遇時形成搜索路徑。仿真如圖9所示，圖中將（0，0）設定為起點，圖中用正方形標注;（10，10）設定為目標點，圖中用三角形標注，根據具體場景分別設定障礙物，圖中用圓圈標注。仿真實驗結果表明，傳統的人工勢場法因合力的方向被障礙物阻檔，從而無法繼續規劃路徑，如圖9a）所示，而本文算法可以繞開障礙物，繼續進行路徑規劃，如圖9b）所示。

3.1.2? 易陷入徘徊抖動狀態

人工勢場法中，臨近目標點斥力大于引力，或存在多個障礙物產生的斥力場，機器人無法選擇正確的運動方向搜索失敗。動態引力場法只有引力的存在，只要起點與目標點不相遇，引力便不會是0。如圖10所示，可知臨近目標點時，傳統算法由于斥力和引力的共同作用導致路徑產生振蕩且最終沒有到達目標點，利用動態引力場和動態尋找次優方向很好地解決了遇到障礙物和臨近目標點振蕩以及到達不了目標點的情況。

人工勢場法中，遇到陷阱會出現死鎖的情況，動態引力場法當陷入陷阱時，在動態引力場的指引下，合理選擇引力場柵格，沿著障礙物邊緣避開障礙物運動，從而逃出陷阱。圖11是在圖10障礙物的基礎上增加（9.5，9.5）和（9，9）兩個障礙物，傳統算法會導致起點在（8.844，8.649 7）和（8.371 7，8.485 4）兩點之間循環運動，如圖11直線的加粗，紅色字體標注部分，最終搜索目標不成功。動態引力場能很好地解決傳統算法的死鎖而導致搜尋不到目標點的情況，具有良好的規劃效果。

3.2? 路徑優化算法仿真結果

通過模擬畫出障礙物不同的形狀信息，不同的位置信息的AB場景進行仿真，如圖12所示。設定（20，0）坐標點為起點，（0，20）坐標點為終點，運用基于動態引力場的路徑規劃算法對模擬現場進行路徑規劃，隨后并對路徑進行平滑優化處理，得到了最短、最優的路徑。圖12中虛線為本文提出的動態引力場規劃出來的路線，實線為本文路徑優化后的規劃路線。從圖中可看出，基于動態引力場的機器人路徑規劃算法可以有效地規劃出從起點到終點的最優路徑。

路徑優化前后路徑長度與花費時間對比結果見表1。由表1可知，運用本文路徑平滑優化后的路徑長度有縮短，規劃路徑所花費時間略有減少。本文算法中的路徑優化會根據具體場景的復雜情況不同，縮短的路徑不同，對于障礙物垂直于起點和目標點連線的情景，以及復雜度大的場景，效果更為明顯。

4? 結? 論

本文對傳統人工勢場法的原理、實現方法以及存在的問題進行了分析，在此基礎上提出了基于動態引力場的機器人路徑規劃算法，有效地解決了傳統人工勢場法對于障礙物、局部極小點和死鎖等目標不可達的問題。動態引力場法根據交替相向運動的起點與目標點，不斷更新引力勢場，繼而不斷地對路徑進行修正，最后通過路徑平滑進一步優化，形成最短且最優化路徑。在Matlab 2017仿真平臺上對改進前后的方法進行仿真，對搜索目標點正前方有障礙物、臨近目標點振蕩、陷入徘徊抖動狀態、死鎖缺陷情景進行了仿真對比，對路徑優化前后效果進行仿真對比。結果表明，改進后的方法能夠解決傳統人工勢場法的固有缺陷，通過對路徑進行平滑優化，在時間和長度上對路徑進行了進一步優化，很好地使機器人順利避開障礙物并最終到達目標點。

參考文獻

[1] 霍鳳財，遲金，黃梓健，等.移動機器人路徑規劃算法綜述[J].吉林大學學報（信息科學版），2018，36（6）：639?647.

[2] DAS P K， BEHERA H S， PANIGRAHI B K. Intelligent?based multi?robot path planning inspired by improved classical Q?learning and improved particle swarm optimization with perturbed velocity [J]. Engineering science and technology， 2016， 19（1）： 651?669.

[3] TIAN L， COLLINS C. An effective robot trajectory planning method using a genetic algorithm [J]. Mechatronics， 2004， 14（5）： 455?470.

[4] 張殿富，劉福.基于人工勢場法的路徑規劃方法研究及展望[J].計算機工程與科學，2013，35（6）：88?95.

[5] 彭艷，國文青，劉梅，等.基于切點優化人工勢場法的三維避障規劃[J].系統仿真學報，2014，26（8）：1758?1762.

[6] 王偉，王華.基于約束人工勢場法的彈載飛行器實時避障航跡規劃[J].航空動力學報，2014，29（7）：1738?1743.

[7] 李擎，張超，韓彩衛，等.動態環境下基于模糊邏輯算法的移動機器人路徑規劃[J].中南大學學報（自然科學版），2013，44（z2）：104?108.

[8] ZHAO Junwei， ZHAO Jianjun. Path planning of multi?UAVs concealment attack based on new A* method [C]// 2014 6th International Conference on Intelligent Human?machine Systems and Cybernetics. Hangzhou， China： IEEE， 2014： 401?404.

[9] 吳宗勝.室外移動機器人的道路場景識別及路徑規劃研究[D].西安：西安理工大學，2017.

[10] 卜祥津.基于深度強化學習的未知環境下機器人路徑規劃的研究[D].哈爾濱：哈爾濱工業大學，2018.

[11] 朱愛斌，劉洋洋，何大勇，等.解決路徑規劃局部極小問題的勢場柵格法[J].機械設計與研究，2017，33（5）：46?50.