基于動態(tài)引力場的機(jī)器人路徑規(guī)劃研究

李靖 楊帆 韓艷芬 陳虹

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

關(guān)鍵詞： 機(jī)器人路徑規(guī)劃; 動態(tài)引力場; 動態(tài)路徑修正; 路徑長度優(yōu)化; 路徑平滑優(yōu)化; 路徑優(yōu)化對比

中圖分類號： TN99?34; TP242? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識碼： 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? 引? 言

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

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

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

1? 傳統(tǒng)的人工勢場法路徑規(guī)劃算法

1.1? 人工勢場法原理

1986年，Khatib將人工勢場算法引入到了機(jī)器人避障路徑規(guī)劃領(lǐng)域。人工勢場法[11]進(jìn)行路徑規(guī)劃的基本思想是將機(jī)器人、障礙物、目標(biāo)點簡化為一點，機(jī)器人在周圍環(huán)境中的運(yùn)動抽象為在虛擬力場中的運(yùn)動，即目標(biāo)點對機(jī)器人產(chǎn)生引力場[Uatt]，障礙物對機(jī)器人產(chǎn)生斥力場[Urep]，機(jī)器人在斥力場和引力場的共同作用下向目標(biāo)運(yùn)動。

設(shè)機(jī)器人在空間中的位置為[X]，目標(biāo)點位置為[Xg]，障礙物位置為[Xo]，引力勢函數(shù)和斥力勢函數(shù)可分別表示如下：

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

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

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

[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）

則機(jī)器人受到的合力為：

[Ftotal=Fatt+Frep] （5）

機(jī)器人在合力的作用下，朝目標(biāo)點運(yùn)動，其受力如圖1所示。

1.2? 傳統(tǒng)人工勢場法的缺陷

1.2.1? 目標(biāo)不可達(dá)到

傳統(tǒng)人工勢場法在機(jī)器人還未到達(dá)目標(biāo)點時，提前陷入局部勢場極小點而無法到達(dá)目標(biāo)，形成目標(biāo)不可到達(dá)的情況。主要體現(xiàn)在以下四種情況：

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

2） 引力與斥力合力為0的情況，即引力和斥力大小相等方向相反，合力抵消為0，機(jī)器人無法找到運(yùn)動方向，處于靜止?fàn)顟B(tài)，從而使得目標(biāo)不可達(dá)到，如圖2b）所示。

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

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

1.2.2? 易陷入徘徊抖動狀態(tài)

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

1.2.3? 規(guī)劃路徑非最短

為了解決傳統(tǒng)人工勢場法的固有缺陷，改進(jìn)的人工勢場法主要通過構(gòu)建合理的勢場函數(shù)，消除或減少局部極小點，從根本上解決局部最小值問題。在路徑搜索算法方面，主要是針對已經(jīng)陷入局部極小點的情況，研究使用何種策略跳出局部極小點，使機(jī)器人重新回到正確的搜索狀態(tài)。但無論是修改勢場函數(shù)模型，還是在局部最小點處加入適當(dāng)?shù)奶霾呗裕饬鲆琅f存在其中，只要斥力場存在，必定會使得機(jī)器人遠(yuǎn)離障礙物邊緣行走，且斥力越大，偏離最大引力場的方向越多，路徑越長，如圖4所示，機(jī)器人并沒有貼合障礙物邊緣行走，從而出現(xiàn)使得路徑不是最短的情況，便有了繼續(xù)縮短路徑長度的空間。

2? 基于動態(tài)引力場的路徑規(guī)劃算法

2.1? 算法原理

2.1.1? 環(huán)境地圖柵格化[11]

根據(jù)機(jī)器人運(yùn)動的有限場地大小，將工作環(huán)境進(jìn)行柵格劃分，標(biāo)定起點和目標(biāo)點位置，如圖5所示。

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

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

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

2.1.2? 搜索區(qū)域

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

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

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

2.1.3? 路徑平滑

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

2.2? 算法流程及分析

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

3? 算法仿真及結(jié)果分析

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

3.1? 動態(tài)引力場算法仿真

為了檢驗動態(tài)引力場的機(jī)器人路徑規(guī)劃算法的性能，對人工勢場法的固有缺陷情況進(jìn)行了對比仿真實驗。

3.1.1? 搜索目標(biāo)點正前方有障礙物

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

3.1.2? 易陷入徘徊抖動狀態(tài)

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

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

3.2? 路徑優(yōu)化算法仿真結(jié)果

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

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

4? 結(jié)? 論

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

參考文獻(xiàn)

[1] 霍鳳財，遲金，黃梓健，等.移動機(jī)器人路徑規(guī)劃算法綜述[J].吉林大學(xué)學(xué)報（信息科學(xué)版），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] 張殿富，劉福.基于人工勢場法的路徑規(guī)劃方法研究及展望[J].計算機(jī)工程與科學(xué)，2013，35（6）：88?95.

[5] 彭艷，國文青，劉梅，等.基于切點優(yōu)化人工勢場法的三維避障規(guī)劃[J].系統(tǒng)仿真學(xué)報，2014，26（8）：1758?1762.

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

[7] 李擎，張超，韓彩衛(wèi)，等.動態(tài)環(huán)境下基于模糊邏輯算法的移動機(jī)器人路徑規(guī)劃[J].中南大學(xué)學(xué)報（自然科學(xué)版），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] 吳宗勝.室外移動機(jī)器人的道路場景識別及路徑規(guī)劃研究[D].西安：西安理工大學(xué)，2017.

[10] 卜祥津.基于深度強(qiáng)化學(xué)習(xí)的未知環(huán)境下機(jī)器人路徑規(guī)劃的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2018.

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