改進(jìn)人工勢場法的移動(dòng)機(jī)器人局部路徑規(guī)劃

王迪，李彩虹，郭娜，高騰騰，劉國名

(山東理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院,山東 淄博 255049)

路徑規(guī)劃是移動(dòng)機(jī)器人領(lǐng)域的重要組成部分，主要分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃是機(jī)器人在環(huán)境信息已知的情況下進(jìn)行路徑規(guī)劃，可以找到最優(yōu)解，但一般是離線規(guī)劃，不能實(shí)時(shí)執(zhí)行；局部路徑規(guī)劃是機(jī)器人在環(huán)境信息未知或部分可知的情況下，在移動(dòng)過程中利用傳感器、紅外線等檢測環(huán)境信息進(jìn)行的實(shí)時(shí)路徑規(guī)劃。由于缺乏全局環(huán)境信息，傳感器的檢測范圍也有限，因此規(guī)劃的路徑可能不是最優(yōu)的，甚至找不到可行路徑。在大場景的運(yùn)行區(qū)域里，通常是將全局路徑規(guī)劃和局部路徑規(guī)劃結(jié)合起來使用。常用的路徑規(guī)劃算法有蟻群算法[1]、神經(jīng)網(wǎng)絡(luò)[2]、遺傳算法[3]、人工勢場法、A*算法[4]、模糊控制[5]等等，其中人工勢場法具有計(jì)算量小、結(jié)構(gòu)簡單、路徑平滑的特點(diǎn)，得到了廣泛應(yīng)用。但人工勢場法存在局部極小點(diǎn)的缺陷，即移動(dòng)機(jī)器人在障礙物前因受力平衡而靜止不動(dòng)或在某一范圍內(nèi)震蕩，從而無法到達(dá)目標(biāo)點(diǎn)[6]。為了解決這一問題，國內(nèi)外學(xué)者提出了很多解決方法。Chen等[7]加入混沌優(yōu)化算法，將勢場函數(shù)作為混沌優(yōu)化的目標(biāo)函數(shù)，通過混沌搜索實(shí)現(xiàn)了機(jī)器人相應(yīng)的運(yùn)動(dòng)步長和運(yùn)動(dòng)方向；魏先勇等[8]構(gòu)造了新斥力勢場函數(shù)消除局部極小點(diǎn)，并用量子遺傳算法對(duì)最優(yōu)或次優(yōu)個(gè)體進(jìn)行選擇，進(jìn)行路徑優(yōu)化；Abdalla等[9]將人工勢場法與模糊控制相結(jié)合，利用粒子群算法對(duì)模糊邏輯控制器的隸屬度函數(shù)進(jìn)行了優(yōu)化，并將其用于移動(dòng)機(jī)器人的運(yùn)動(dòng)控制，克服了局部極小點(diǎn)問題；Zhang等[10]將人工勢場法與滾動(dòng)窗法相結(jié)合，解決局部極小點(diǎn)問題；溫素芳等[11]提出障礙物影響范圍分層的思想；He等[12]提出動(dòng)態(tài)橢圓影響區(qū)域，建立了危險(xiǎn)區(qū)域模型；賈丙佳等[13]將鄰近障礙物整體化，避免機(jī)器人進(jìn)入局部極小點(diǎn)區(qū)域[13]；張鵬彬等[14]在障礙物一側(cè)設(shè)置中間目標(biāo)點(diǎn)；徐飛[15]在改進(jìn)的人工勢場法中采用扇區(qū)劃分建立虛擬目標(biāo)點(diǎn)的方法；紀(jì)迪[16]采用多方向沿墻走的方法，從五個(gè)方向中選出合適的方向跳出局部極小點(diǎn)陷阱；Lee等[17]在無障礙物的環(huán)境中設(shè)置虛擬目標(biāo)點(diǎn)來克服局部極小點(diǎn)和路徑無效率問題；韓知玖等[18]通過添加虛擬力解決局部極小點(diǎn)問題。這些方法能夠解決某種程度上的局部極小點(diǎn)問題，但大多只適用于簡單環(huán)境下。

本文針對(duì)局部極小點(diǎn)陷阱導(dǎo)致機(jī)器人無法到達(dá)目標(biāo)點(diǎn)的問題，提出了基于虛擬目標(biāo)點(diǎn)和環(huán)境判斷參數(shù)的改進(jìn)人工勢場法。在該方法中，加入近似封閉障礙物環(huán)境的判斷，在機(jī)器人即將逃出局部極小點(diǎn)而陷入死循環(huán)時(shí)設(shè)置虛擬目標(biāo)點(diǎn)，引導(dǎo)機(jī)器人逃出近似封閉的障礙物環(huán)境，避免機(jī)器人陷入陷阱區(qū)域，順利到達(dá)目標(biāo)點(diǎn)。

1 人工勢場法

基于人工勢場的移動(dòng)機(jī)器人路徑規(guī)劃算法是Khatib提出的[19]。基本思想是在機(jī)器人工作的環(huán)境中建立一個(gè)人工勢場，目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生引力，障礙物對(duì)機(jī)器人產(chǎn)生斥力，由引力和斥力疊加起來的合力對(duì)機(jī)器人產(chǎn)生引導(dǎo)作用，使機(jī)器人到達(dá)目標(biāo)點(diǎn)。為了解決路徑規(guī)劃過程中的目標(biāo)不可達(dá)問題，本文采取添加目標(biāo)點(diǎn)與機(jī)器人距離的改進(jìn)斥力勢場[20]，使目標(biāo)點(diǎn)處的勢場為全局最小點(diǎn)。人工勢場定義為

U=Uatt+Urep，

(1)

式中：U為勢場和；Uatt為目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生的引力場；Urep為障礙物對(duì)機(jī)器人產(chǎn)生的斥力場。定義目標(biāo)點(diǎn)對(duì)機(jī)器人的引力和障礙物對(duì)機(jī)器人的斥力分別對(duì)應(yīng)引力場和斥力場的負(fù)梯度，根據(jù)偏導(dǎo)得出人工勢場對(duì)機(jī)器人的作用力F為

F=Fatt+Frep，

(2)

式中：F為合力；Fatt為目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生的引力；Frep為障礙物對(duì)機(jī)器人產(chǎn)生的斥力。

引力勢場函數(shù)定義為

(3)

式中：Xg=(xg,yg)為目標(biāo)點(diǎn)的坐標(biāo);X=(x,y)為機(jī)器人的坐標(biāo)；Uatt為目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生的引力場；k為引力增益系數(shù)，‖X-Xg‖為機(jī)器人到目標(biāo)點(diǎn)的歐幾里得距離。目標(biāo)點(diǎn)對(duì)機(jī)器人的引力為引力勢場函數(shù)的負(fù)梯度：

Fatt=-k‖X-Xg‖。

(4)

斥力勢場函數(shù)定義為

(5)

式中：m為斥力增益系數(shù)；d為機(jī)器人與障礙物的距離；d0為障礙物影響距離；da為機(jī)器人與目標(biāo)點(diǎn)之間的距離。障礙物對(duì)機(jī)器人的斥力為

(6)

(7)

(8)

式中：分量Frep1由障礙物指向機(jī)器人；分量Frep2由機(jī)器人指向目標(biāo)點(diǎn)。添加目標(biāo)點(diǎn)與機(jī)器人距離，從力的角度上，增強(qiáng)了引力，避免目標(biāo)點(diǎn)附近有障礙物時(shí)，斥力過大，機(jī)器人無法到達(dá)目標(biāo)點(diǎn)的情況。人工勢場法受力分析如圖1所示。

圖1 人工勢場法受力分析Fig.1 Force diagram of the artificial potential method

引力勢場和斥力勢場構(gòu)成的人工勢場等勢圖如圖2所示。在勢場的作用下，機(jī)器人沿著梯度下降的方向移動(dòng)。目標(biāo)點(diǎn)處的勢場為0，是全局最小點(diǎn)。

圖2 人工勢場法勢場圖Fig.2 The potential field diagram of the artificial potential field method

2 改進(jìn)策略

2.1 虛擬目標(biāo)點(diǎn)法

利用人工勢場法進(jìn)行路徑規(guī)劃，當(dāng)機(jī)器人所受的引力和斥力和為0時(shí)，存在局部極小點(diǎn)問題，導(dǎo)致機(jī)器人在某一區(qū)域內(nèi)震蕩或靜止不動(dòng)，陷于陷阱區(qū)域，無法到達(dá)目標(biāo)點(diǎn)。本文通過設(shè)置虛擬目標(biāo)點(diǎn)和環(huán)境判斷參數(shù)來解決這個(gè)問題。

當(dāng)機(jī)器人在移動(dòng)過程中進(jìn)入陷阱區(qū)域，采取設(shè)置虛擬目標(biāo)點(diǎn)的策略，即在機(jī)器人所受斥力分量Frep1左轉(zhuǎn)90°的方向上的合適位置，設(shè)置虛擬目標(biāo)點(diǎn)。忽略目標(biāo)點(diǎn)產(chǎn)生的引力和障礙物產(chǎn)生的斥力，在虛擬目標(biāo)點(diǎn)的引導(dǎo)下，使機(jī)器人向前移動(dòng)一步，移動(dòng)后撤銷虛擬目標(biāo)點(diǎn)，計(jì)算當(dāng)前步驟下的合力及下一步坐標(biāo)。重復(fù)以上步驟，直到機(jī)器人逃出局部極小點(diǎn)區(qū)域。

采用虛擬目標(biāo)點(diǎn)法逃出局部極小點(diǎn)的示意圖如圖3所示。圖3中箭頭表示機(jī)器人下一步的移動(dòng)方向。當(dāng)前機(jī)器人所處位置為點(diǎn)A，因?yàn)橐εc斥力的合力兩者方向相反導(dǎo)致震蕩，陷入局部極小點(diǎn)。機(jī)器人在所受斥力分量Frep1左轉(zhuǎn)90°方向上的點(diǎn)B處，設(shè)置虛擬目標(biāo)點(diǎn)，經(jīng)虛擬目標(biāo)點(diǎn)牽引到達(dá)點(diǎn)B。在點(diǎn)B處，機(jī)器人在合力的作用下，將往點(diǎn)A方向移動(dòng)。經(jīng)判定，機(jī)器人陷入了局部極小點(diǎn)陷阱，在點(diǎn)A和點(diǎn)B之間發(fā)生震蕩。根據(jù)改進(jìn)措施，在點(diǎn)B處機(jī)器人所受斥力分量Frep1左轉(zhuǎn)90°方向上的點(diǎn)C處，設(shè)置虛擬目標(biāo)點(diǎn)。經(jīng)虛擬目標(biāo)點(diǎn)的引導(dǎo)，機(jī)器人到達(dá)C處。重復(fù)此步驟，直到機(jī)器人逃出局部極小點(diǎn)區(qū)域，最終到達(dá)目標(biāo)點(diǎn)。

圖3 虛擬目標(biāo)點(diǎn)法示意圖Fig.3 Schematic diagram of the virtual target method

在斥力分量Frep1左轉(zhuǎn)90°的方向上設(shè)置虛擬目標(biāo)點(diǎn)，是本文解決局部極小點(diǎn)問題、逃離陷阱區(qū)域的中心思想。這種方法可以解決機(jī)器人在一字型障礙物、U型障礙物環(huán)境下進(jìn)行路徑規(guī)劃時(shí)陷入的局部極小點(diǎn)情況，但是在近似封閉的障礙物環(huán)境下，機(jī)器人仍然無法逃出局部極小點(diǎn)區(qū)域。

2.2 近似封閉環(huán)境的判斷

在近似封閉的障礙物環(huán)境下，在設(shè)置虛擬目標(biāo)的基礎(chǔ)上，加入環(huán)境判斷參數(shù)：避障閾值、虛擬目標(biāo)標(biāo)志位和距離閾值。判斷參數(shù)見表1。假設(shè)機(jī)器人移動(dòng)了j步，此時(shí)的位置坐標(biāo)為(xj,yj)，則機(jī)器人移動(dòng)了j-2步和j-1步的位置坐標(biāo)分別為(xj-2,yj-2)、(xj-1,yj-1)。

當(dāng)機(jī)器人陷入局部極小點(diǎn)區(qū)域時(shí)，啟用局部極小點(diǎn)策略，即設(shè)置虛擬目標(biāo)點(diǎn)。對(duì)連續(xù)采用虛擬目標(biāo)點(diǎn)法的次數(shù)進(jìn)行統(tǒng)計(jì)，并且記錄機(jī)器人當(dāng)前所在位置的前兩步路徑規(guī)劃設(shè)置虛擬目標(biāo)點(diǎn)的情況。判斷機(jī)器人是否進(jìn)入近似封閉的障礙物環(huán)境的步驟如下：

1)判斷連續(xù)設(shè)置虛擬目標(biāo)點(diǎn)避障的次數(shù)是否超過避障閾值Nmin，否則轉(zhuǎn)到第4步；

表1 近似封閉環(huán)境的判斷參數(shù)Tab.1 The judgment parameter of approximately closed environment

2)判斷機(jī)器人是否在上一步移動(dòng)時(shí)設(shè)置了虛擬目標(biāo)點(diǎn)，而下一步移動(dòng)時(shí)沒用，若不符合此情況，轉(zhuǎn)到第4步；

3)判斷機(jī)器人與障礙物的距離是否小于dmin。若小于則在上一步設(shè)置虛擬目標(biāo)點(diǎn)的方向上的合適位置設(shè)置虛擬目標(biāo)點(diǎn)，機(jī)器人只受虛擬目標(biāo)點(diǎn)的影響；

4)根據(jù)實(shí)際運(yùn)行情況更新Fl、Fp、Fn的狀態(tài)；

5)根據(jù)合力計(jì)算機(jī)器人的下一步移動(dòng)位置；

6)如果設(shè)置了虛擬目標(biāo)點(diǎn)，則撤銷虛擬目標(biāo)點(diǎn)；

7)重復(fù)以上步驟。

如果滿足前3個(gè)步驟的條件，則判定機(jī)器人陷入近似封閉的障礙物環(huán)境中。此外，設(shè)置避障閾值是為了區(qū)分是否是簡單的局部極小點(diǎn)情況；設(shè)置虛擬目標(biāo)點(diǎn)的狀態(tài)是檢測移動(dòng)機(jī)器人是否即將退出局部極小點(diǎn)策略；設(shè)置機(jī)器人與障礙物的距離閾值dmin是判斷機(jī)器人在退出局部極小點(diǎn)策略時(shí)是否避開障礙物，同時(shí)為了逃出近似封閉的障礙物環(huán)境時(shí)，及時(shí)調(diào)整策略，避免機(jī)器人繞行。機(jī)器人在近似封閉的障礙物環(huán)境下逃出局部極小點(diǎn)區(qū)域的示意圖如圖4所示。

圖4 近似封閉環(huán)境下的機(jī)器人逃出局部極小點(diǎn)區(qū)域示意圖Fig.4 Schematic diagram of a robot escaping from a local minima in an approximately enclosed environment

采用虛擬目標(biāo)點(diǎn)和環(huán)境判斷參數(shù)，對(duì)人工勢場法進(jìn)行改進(jìn)，使機(jī)器人進(jìn)行路徑規(guī)劃時(shí)能夠逃出大部分的陷阱區(qū)域。算法流程如圖5所示。

圖5 改進(jìn)人工勢場法流程Fig.5 The flow chart of the improved artificial potential field method

3 仿真與分析

基于MATLABR2016a平臺(tái)，在能夠發(fā)生局部極小點(diǎn)陷阱區(qū)域的障礙物環(huán)境下，利用改進(jìn)的人工勢場法對(duì)移動(dòng)機(jī)器人局部路徑規(guī)劃進(jìn)行了仿真。每種環(huán)境都加入了虛擬目標(biāo)點(diǎn)，并分別利用環(huán)境判斷參數(shù)進(jìn)行運(yùn)行情況的對(duì)比。仿真圖中等勢線由目標(biāo)點(diǎn)產(chǎn)生的引力場與障礙物產(chǎn)生的斥力場疊加構(gòu)成，紅色標(biāo)志為遇到局部極小點(diǎn)情況設(shè)置的虛擬目標(biāo)點(diǎn)，紫色標(biāo)志為近似封閉的障礙物環(huán)境下設(shè)置的虛擬目標(biāo)點(diǎn)。

1)一字型障礙物環(huán)境下的路徑規(guī)劃仿真

機(jī)器人在一字型障礙物環(huán)境下的局部路徑規(guī)劃仿真如圖6所示。由圖6可以看出，無論是否加入近似封閉的障礙物環(huán)境判斷，機(jī)器人都可以逃離陷阱區(qū)域，解決局部極小值問題。在圖6(b)中加入環(huán)境判斷參數(shù)，并沒有檢測到近似封閉的障礙物環(huán)境，因此規(guī)劃路徑與圖6(a)基本類似。

(a)未加入環(huán)境判斷 (b) 加入環(huán)境判斷圖6 一字型障礙物環(huán)境下的路徑規(guī)劃Fig.6 Path planning in linear obstacle environment

2)U型障礙物環(huán)境下的路徑規(guī)劃仿真

機(jī)器人在U型障礙物環(huán)境下的局部路徑規(guī)劃仿真如圖7所示。由圖7可以看出，未加入環(huán)境判斷和加入環(huán)境判斷的改進(jìn)的人工勢場法，在虛擬目標(biāo)點(diǎn)的牽引下都可以逃離陷阱區(qū)域。盡管圖7(b)加入環(huán)境判斷后會(huì)把環(huán)境誤判斷為近似封閉的環(huán)境，但是路徑規(guī)劃效果與圖7(a)沒有明顯差別。

(a)未加入環(huán)境判斷 (b) 加入環(huán)境判斷圖7 U型障礙物環(huán)境下的路徑規(guī)劃Fig.7 Path planning in the U-shaped obstacle environment

3)近似封閉的障礙物環(huán)境下的路徑規(guī)劃仿真

機(jī)器人在近似封閉的障礙物環(huán)境下的局部路徑規(guī)劃仿真如圖8和圖9所示。在圖8(a)中，僅僅采用虛擬目標(biāo)點(diǎn)法，未加入環(huán)境參數(shù)判斷，機(jī)器人無法在近似封閉的障礙物環(huán)境下逃出局部極小點(diǎn)；而在圖8(b)中，加入近似封閉障礙物環(huán)境判斷以后，機(jī)器人可以順利到達(dá)目標(biāo)點(diǎn)。圖8(a)中，機(jī)器人在M處的合力基本指向目標(biāo)點(diǎn)，在位置變化上不確認(rèn)為局部極小點(diǎn)，退出局部極小點(diǎn)策略。在合力的作用下，機(jī)器人移動(dòng)至點(diǎn)N處，在引力與斥力合力的引導(dǎo)下移動(dòng)至點(diǎn)L處。經(jīng)判斷，機(jī)器人在點(diǎn)L處陷入局部極小點(diǎn)陷阱，啟用了虛擬目標(biāo)點(diǎn)法，在障礙物環(huán)境內(nèi)部按此路線繞圈，無法到達(dá)目標(biāo)點(diǎn)。而采用了加入近似封閉的障礙物環(huán)境判斷的虛擬目標(biāo)點(diǎn)法，在點(diǎn)M處判定機(jī)器人在合力的引導(dǎo)下退出局部極小點(diǎn)策略，但是經(jīng)過判斷，點(diǎn)M處屬于近似封閉的障礙物環(huán)境，故在未退出局部極小點(diǎn)策略時(shí)在設(shè)置虛擬目標(biāo)點(diǎn)的方向繼續(xù)設(shè)置虛擬目標(biāo)點(diǎn)，從而在虛擬目標(biāo)點(diǎn)的引領(lǐng)下逃出局部極小點(diǎn)區(qū)域，順利到達(dá)目標(biāo)點(diǎn)，路徑規(guī)劃效果如圖8(b)所示。圖9展示了另一種近似封閉的障礙物環(huán)境下的運(yùn)行情況，利用改進(jìn)的人工勢場法進(jìn)行局部路徑規(guī)劃，可以得出與圖8中相同的結(jié)論。

(a)未加入環(huán)境判斷 (b) 加入環(huán)境判斷圖8 近似封閉環(huán)境下的路徑規(guī)劃Fig.8 Path planning in approximately closed environment

(a)未加入環(huán)境判斷 (b) 加入環(huán)境判斷圖9 5字型障礙物環(huán)境下的路徑規(guī)劃Fig.9 Path planning in the environment of 5-shaped obstacles

4)混合障礙物環(huán)境下的路徑規(guī)劃仿真

機(jī)器人在兩種混合障礙物環(huán)境下的路徑規(guī)劃仿真如圖10所示。混合障礙物的運(yùn)行環(huán)境包括離散的障礙物、近似封閉的障礙物、一字型障礙物，在運(yùn)行環(huán)境里，改進(jìn)的人工勢場法同時(shí)運(yùn)用了虛擬目標(biāo)點(diǎn)法和近似封閉環(huán)境判斷。從圖10可以看出，盡管加入環(huán)境判斷后，機(jī)器人有時(shí)不能準(zhǔn)確地判斷環(huán)境，但是仍然可以在多種形狀的障礙物構(gòu)成的復(fù)雜環(huán)境下成功到達(dá)目標(biāo)點(diǎn)，而且路徑比較平滑。

(a)環(huán)境1 (b) 環(huán)境2圖10 混合障礙物環(huán)境下的路徑規(guī)劃Fig.10 Path planning in mixed obstacle environment

4 結(jié)束語

1)基于虛擬目標(biāo)點(diǎn)的改進(jìn)人工勢場法的移動(dòng)機(jī)器人局部路徑規(guī)劃，可以判斷移動(dòng)機(jī)器人是否進(jìn)入近似封閉的復(fù)雜障礙物環(huán)境中。

2)本文算法設(shè)置的虛擬目標(biāo)點(diǎn)給予了機(jī)器人逃離局部極小點(diǎn)時(shí)的方向引導(dǎo)，解決了部分因障礙物環(huán)境復(fù)雜而陷入局部極小點(diǎn)、無法到達(dá)目標(biāo)點(diǎn)的問題。

3)盡管一定程度上本文算法會(huì)將障礙物環(huán)境誤判斷成近似封閉的障礙物環(huán)境，使得計(jì)算量增加，但是路徑規(guī)劃的成功率得到了提高。