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

本文針對(duì)局部極小點(diǎn)陷阱導(dǎo)致機(jī)器人無(wú)法到達(dá)目標(biāo)點(diǎn)的問(wèn)題，提出了基于虛擬目標(biāo)點(diǎn)和環(huán)境判斷參數(shù)的改進(jìn)人工勢(shì)場(chǎng)法。在該方法中，加入近似封閉障礙物環(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 人工勢(shì)場(chǎng)法

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

U=Uatt+Urep，

(1)

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

F=Fatt+Frep，

(2)

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

引力勢(shì)場(chǎng)函數(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)生的引力場(chǎng)；k為引力增益系數(shù)，‖X-Xg‖為機(jī)器人到目標(biāo)點(diǎn)的歐幾里得距離。目標(biāo)點(diǎn)對(duì)機(jī)器人的引力為引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度：

Fatt=-k‖X-Xg‖。

(4)

斥力勢(shì)場(chǎng)函數(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í)，斥力過(guò)大，機(jī)器人無(wú)法到達(dá)目標(biāo)點(diǎn)的情況。人工勢(shì)場(chǎng)法受力分析如圖1所示。

圖1 人工勢(shì)場(chǎng)法受力分析Fig.1 Force diagram of the artificial potential method

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

圖2 人工勢(shì)場(chǎng)法勢(shì)場(chǎng)圖Fig.2 The potential field diagram of the artificial potential field method

2 改進(jìn)策略

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

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

當(dāng)機(jī)器人在移動(dòng)過(guò)程中進(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)后撤銷(xiāo)虛擬目標(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)問(wèn)題、逃離陷阱區(qū)域的中心思想。這種方法可以解決機(jī)器人在一字型障礙物、U型障礙物環(huán)境下進(jìn)行路徑規(guī)劃時(shí)陷入的局部極小點(diǎn)情況，但是在近似封閉的障礙物環(huán)境下，機(jī)器人仍然無(wú)法逃出局部極小點(diǎn)區(qū)域。

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

在近似封閉的障礙物環(huán)境下，在設(shè)置虛擬目標(biāo)的基礎(chǔ)上，加入環(huán)境判斷參數(shù)：避障閾值、虛擬目標(biāo)標(biāo)志位和距離閾值。判斷參數(shù)見(jiàn)表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ù)是否超過(guò)避障閾值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í)沒(méi)用，若不符合此情況，轉(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)，則撤銷(xiāo)虛擬目標(biāo)點(diǎn)；

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

如果滿(mǎn)足前3個(gè)步驟的條件，則判定機(jī)器人陷入近似封閉的障礙物環(huán)境中。此外，設(shè)置避障閾值是為了區(qū)分是否是簡(jiǎn)單的局部極小點(diǎn)情況；設(shè)置虛擬目標(biāo)點(diǎn)的狀態(tài)是檢測(cè)移動(dòng)機(jī)器人是否即將退出局部極小點(diǎn)策略；設(shè)置機(jī)器人與障礙物的距離閾值dmin是判斷機(jī)器人在退出局部極小點(diǎn)策略時(shí)是否避開(kāi)障礙物，同時(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ì)人工勢(shì)場(chǎng)法進(jìn)行改進(jìn)，使機(jī)器人進(jìn)行路徑規(guī)劃時(shí)能夠逃出大部分的陷阱區(qū)域。算法流程如圖5所示。

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

3 仿真與分析

基于MATLABR2016a平臺(tái)，在能夠發(fā)生局部極小點(diǎn)陷阱區(qū)域的障礙物環(huán)境下，利用改進(jìn)的人工勢(shì)場(chǎng)法對(duì)移動(dòng)機(jī)器人局部路徑規(guī)劃進(jìn)行了仿真。每種環(huán)境都加入了虛擬目標(biāo)點(diǎn)，并分別利用環(huán)境判斷參數(shù)進(jìn)行運(yùn)行情況的對(duì)比。仿真圖中等勢(shì)線(xiàn)由目標(biāo)點(diǎn)產(chǎn)生的引力場(chǎng)與障礙物產(chǎn)生的斥力場(chǎng)疊加構(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可以看出，無(wú)論是否加入近似封閉的障礙物環(huán)境判斷，機(jī)器人都可以逃離陷阱區(qū)域，解決局部極小值問(wèn)題。在圖6(b)中加入環(huán)境判斷參數(shù)，并沒(méi)有檢測(cè)到近似封閉的障礙物環(huán)境，因此規(guī)劃路徑與圖6(a)基本類(lèi)似。

(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)的人工勢(shì)場(chǎng)法，在虛擬目標(biāo)點(diǎn)的牽引下都可以逃離陷阱區(qū)域。盡管圖7(b)加入環(huán)境判斷后會(huì)把環(huán)境誤判斷為近似封閉的環(huán)境，但是路徑規(guī)劃效果與圖7(a)沒(méi)有明顯差別。

(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ī)器人無(wú)法在近似封閉的障礙物環(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)部按此路線(xiàn)繞圈，無(wú)法到達(dá)目標(biāo)點(diǎn)。而采用了加入近似封閉的障礙物環(huán)境判斷的虛擬目標(biāo)點(diǎn)法，在點(diǎn)M處判定機(jī)器人在合力的引導(dǎo)下退出局部極小點(diǎn)策略，但是經(jīng)過(guò)判斷，點(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)的人工勢(shì)場(chǎng)法進(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所示?；旌险系K物的運(yùn)行環(huán)境包括離散的障礙物、近似封閉的障礙物、一字型障礙物，在運(yùn)行環(huán)境里，改進(jìn)的人工勢(shì)場(chǎng)法同時(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é)束語(yǔ)

1)基于虛擬目標(biāo)點(diǎn)的改進(jìn)人工勢(shì)場(chǎng)法的移動(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)、無(wú)法到達(dá)目標(biāo)點(diǎn)的問(wèn)題。

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