逃離傳統(tǒng)勢(shì)場(chǎng)法局部穩(wěn)定點(diǎn)策略?

耿雙樂(lè) 羅順心

（上海理工大學(xué)光電信息與計(jì)算機(jī)工程學(xué)院 上海 200082）

1 引言

人工勢(shì)場(chǎng)法是當(dāng)前移動(dòng)機(jī)器人路徑規(guī)劃中最常用的算法之一。人工勢(shì)場(chǎng)法［2］是由Khatib在1986年提出的一種局部路徑規(guī)劃算法。人工勢(shì)場(chǎng)法無(wú)需通過(guò)復(fù)雜的建立環(huán)境模型，只需傳感器感知與障礙物信息就可完成避障。人工勢(shì)場(chǎng)法通過(guò)引入物理上勢(shì)場(chǎng)概念，并通過(guò)勢(shì)場(chǎng)梯度得到引力和斥力。通過(guò)梯度下降法求得引力斥力值。由于無(wú)需復(fù)雜建模過(guò)程，并具有良好的避障功能，所以在移動(dòng)機(jī)器人路徑規(guī)劃領(lǐng)域運(yùn)用廣泛。然而傳統(tǒng)勢(shì)場(chǎng)法也存在兩種固有的缺陷。一種是復(fù)雜環(huán)境下或者某區(qū)域位置合力為零的情況下會(huì)陷入局部最優(yōu)狀態(tài)。一種是目標(biāo)點(diǎn)周?chē)嬖谡系K物時(shí)，又無(wú)法到達(dá)目標(biāo)點(diǎn)［3］。

傳統(tǒng)勢(shì)場(chǎng)法在復(fù)雜環(huán)境中容易陷入局部最優(yōu)狀態(tài)。文獻(xiàn)［4～6］分別通過(guò)SA算法結(jié)合、模擬退火算法、自適應(yīng)過(guò)程去解決局部最優(yōu)問(wèn)題。此外加入虛擬中間點(diǎn)［7，11］方式也可逃離局部目標(biāo)點(diǎn)。同時(shí)對(duì)于動(dòng)態(tài)障礙物環(huán)境下的避障［8，12，14］也引入速度勢(shì)場(chǎng)概念完成動(dòng)態(tài)避障。本文通過(guò)利用周期距離檢測(cè)，結(jié)合柵格法局部建模方式完成逃離局部最優(yōu)點(diǎn)逃離。傳統(tǒng)勢(shì)場(chǎng)法中的目標(biāo)點(diǎn)不可達(dá)問(wèn)題，最常用是S.S.GE在文獻(xiàn)［3］提出的加入相對(duì)距離因子方法。此種方法是從數(shù)學(xué)函數(shù)方面進(jìn)行改進(jìn)。人工勢(shì)場(chǎng)在路徑規(guī)劃中將移動(dòng)機(jī)器人當(dāng)做無(wú)約束性質(zhì)點(diǎn)［15］考慮，無(wú)需復(fù)雜建模。

2 傳統(tǒng)勢(shì)場(chǎng)原理

勢(shì)場(chǎng)法將整個(gè)運(yùn)動(dòng)空間抽象成一個(gè)勢(shì)場(chǎng)，目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人會(huì)形成一個(gè)引力場(chǎng)，運(yùn)動(dòng)空間內(nèi)所存在的障礙物又會(huì)在一定范圍內(nèi)對(duì)機(jī)器人形成一個(gè)斥力場(chǎng)。勢(shì)場(chǎng)能是人工勢(shì)場(chǎng)最原始數(shù)學(xué)模型。通過(guò)梯度下降方法，分別求得目標(biāo)點(diǎn)產(chǎn)生的引力和障礙物產(chǎn)生的斥力。因此機(jī)器人所在的空間即為引力場(chǎng)和斥力場(chǎng)所疊加的合力場(chǎng)。

圖1 引力勢(shì)場(chǎng)分布圖

圖2 斥力勢(shì)場(chǎng)分布圖

機(jī)器人始終沿著勢(shì)場(chǎng)梯度下降的方向運(yùn)動(dòng)。將斥力場(chǎng)的負(fù)梯度作為斥力虛擬力，引力場(chǎng)的負(fù)梯度作為引力虛擬力。因此，移動(dòng)機(jī)器人在空間中運(yùn)動(dòng)受到引力和斥力的合力作用。設(shè)機(jī)器人在運(yùn)動(dòng)空間中實(shí)時(shí)位置為 X=[x，y]，下式（1）、（2）中構(gòu)建了引力場(chǎng)和斥力場(chǎng)關(guān)于機(jī)器人位置變化的函數(shù)。

其中Uatt表示移動(dòng)機(jī)器人所受到目標(biāo)位置的引力場(chǎng)，katt是自定義的引力場(chǎng)系數(shù)， ||X-XG表示機(jī)器人與目標(biāo)點(diǎn)之間的歐幾里得距離。XG表示目標(biāo)點(diǎn)的位置。η為可自定義的斥力場(chǎng)系數(shù)，ρo為自定義的障礙物斥力場(chǎng)影響范圍，Xob為障礙物所在的空間位置。那么目標(biāo)與障礙物所產(chǎn)生的負(fù)梯度即引力和斥力如下式（3）、（4）：

傳統(tǒng)勢(shì)場(chǎng)法根據(jù)傳感器裝置不斷檢測(cè)機(jī)器人與障礙物和目標(biāo)點(diǎn)的距離信息，并通過(guò)人工勢(shì)場(chǎng)算法轉(zhuǎn)化為引力和斥力進(jìn)行規(guī)劃。人工勢(shì)場(chǎng)在合力為零或者復(fù)制環(huán)境中移動(dòng)機(jī)器人可能會(huì)陷入局部最優(yōu)穩(wěn)定區(qū)域無(wú)法逃離。本文通過(guò)周期距離檢測(cè)方式，檢測(cè)局部穩(wěn)定區(qū)域，并結(jié)合人工勢(shì)場(chǎng)參數(shù)，建立合適柵格模型運(yùn)用啟發(fā)式搜索算法進(jìn)行逃離。

3 掃描周期距離檢測(cè)穩(wěn)定區(qū)域

人工勢(shì)場(chǎng)是根據(jù)力的作用導(dǎo)向目標(biāo)點(diǎn)。因此會(huì)存在合力為零狀態(tài)。常見(jiàn)的陷入局部最優(yōu)有三種狀態(tài)如圖3～圖5所示。

圖3 單障礙物陷入局部最優(yōu)圖示

圖4 多障礙作用下陷入局部最優(yōu)狀態(tài)

圖5 復(fù)雜障礙物下陷入局部穩(wěn)定區(qū)域

如圖3中，由于移動(dòng)機(jī)器人和障礙物、目標(biāo)點(diǎn)都在同一延長(zhǎng)線上，此時(shí)會(huì)在某一位置下合力為零。圖4多障礙物狀態(tài)下在某一位置區(qū)域，多障礙物斥力方向相反可互相抵消，可能發(fā)生局部最優(yōu)。圖5中，在多復(fù)雜環(huán)境下，大型無(wú)規(guī)則障礙無(wú)法看做質(zhì)點(diǎn)處理。此時(shí)在復(fù)雜環(huán)境中陷入局部震蕩區(qū)域。

陷入局部最優(yōu)環(huán)境下的移動(dòng)機(jī)器人都是在某一區(qū)域范圍內(nèi)停止不前或者震蕩。因此可通過(guò)時(shí)間周期距離檢測(cè)方式檢測(cè)。具體過(guò)程如下：

1）根據(jù)移動(dòng)機(jī)器人移動(dòng)步長(zhǎng)L，設(shè)定震蕩區(qū)域圓半徑R=2L；

2）設(shè)定平均檢測(cè)時(shí)間t，每隔一段時(shí)間周期進(jìn)行一次局部最優(yōu)檢測(cè)觀察機(jī)器人是否陷入或者逃離局部穩(wěn)定點(diǎn)；

3）記錄移動(dòng)機(jī)器人當(dāng)前實(shí)時(shí)坐標(biāo)位置為Xnow；

4）每隔一段掃描周期后檢測(cè)移動(dòng)機(jī)器人掃描周期后的更新位置為Xafter。計(jì)算掃描周期前后兩者位置的距離，與震蕩位置區(qū)域半徑R1進(jìn)行大小比較。當(dāng)|Xnow-Xafter|＜R1，表示陷入局部區(qū)域。當(dāng)|Xnow-Xafter| ≥ R1，表示未陷入震蕩區(qū)域或者已經(jīng)逃離。

4 柵格法建模

4.1 柵格模型介紹

柵格法是一種全局路徑規(guī)劃的方法。通過(guò)對(duì)環(huán)境建模生成具有二值信息的單元柵格地圖。單元柵格選取越大，精確度越低，但環(huán)境模型簡(jiǎn)單計(jì)算量較小。單元柵格選取越小，精確度越高，但是計(jì)算量較大不利于實(shí)時(shí)計(jì)算。因此在規(guī)劃中完全運(yùn)用柵格建模，不太具有可行性。在計(jì)算中通常用0和1表示柵格地圖。其中一個(gè)設(shè)置為障礙物，另外一值為自由空間區(qū)域。柵格地圖如圖6所示。

圖6 柵格地圖

圖6 所示黑色表示障礙物，白色區(qū)域?yàn)樽杂煽臻g區(qū)域。在計(jì)算機(jī)中存儲(chǔ)矩陣信息如圖7所示。

圖7 地圖存儲(chǔ)信息矩陣

4.2 柵格法處理復(fù)雜障礙物

移動(dòng)機(jī)器人在實(shí)際運(yùn)行環(huán)境中會(huì)存在大量無(wú)規(guī)則形狀。因此通過(guò)柵格法環(huán)境建模時(shí)，需要對(duì)無(wú)規(guī)則障礙物進(jìn)行“膨化”處理。“膨化處理”分為兩種情況：一種是障礙物的面積大小小于柵格大小；一種是障礙物面積大于柵格大小。如果障礙物面積小于單元方格面積，此時(shí)單元方格內(nèi)非完全障礙物。此時(shí)通過(guò)“膨化處理”，將整個(gè)單元格都作為障礙物方格。如果障礙物面積大于單個(gè)柵格面積，此時(shí)柵格地圖將障礙物分為多個(gè)柵格，對(duì)剩余柵格繼續(xù)做“膨化”處理。兩種情況下的“膨化”處理過(guò)程，如圖8～圖10所示。

圖8 障礙物面積小于單元柵格面積膨化處理圖

圖9 障礙物面積大于單元柵格面積未“膨化”處理

圖10 障礙物面積大于單元柵格面積“膨化”處理

4.3 啟發(fā)式算法在柵格模型下搜索路徑

啟發(fā)式算法是一種利用估價(jià)函數(shù)進(jìn)行搜索路徑的算法。估價(jià)函數(shù)通常分為真帶代價(jià)函數(shù)和預(yù)估代價(jià)函數(shù)。代價(jià)函數(shù)表示如下表示：

g(n)是當(dāng)前位置到達(dá)一下節(jié)點(diǎn)的真實(shí)代價(jià)，h(n)表示當(dāng)前位置點(diǎn)到目標(biāo)點(diǎn)的預(yù)估代價(jià)值。路徑規(guī)劃中，代價(jià)在柵格地圖中表示為距離代價(jià)。通過(guò)柵格建模地圖下，可通過(guò)啟發(fā)式算法搜索路徑，路徑搜索原理如下：

1）初始化兩張狀態(tài)列表，一張是open列表，一張為closed列表。兩張表一般記錄位置信息。初始路徑時(shí)，將起點(diǎn)加入open列表當(dāng)中去。此時(shí)closed列表為空，不記錄任何信息；

2）查看起始點(diǎn)位置相鄰的方格，并可行路徑柵格和可能到達(dá)目標(biāo)點(diǎn)的柵格放入open列表當(dāng)中去（柵格地圖每一位置會(huì)存在四個(gè)行駛方向，或者加對(duì)角線可行路徑為八個(gè)方向）。同時(shí)將上一柵格地點(diǎn)作為當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)，稱作parent。父節(jié)點(diǎn)用來(lái)尋求最終行駛路徑。

3）此時(shí)將起始位置點(diǎn)從open列表中移除，并加入closed列表中。加入closed列表表明此柵格節(jié)點(diǎn)不在作為后續(xù)路徑選擇節(jié)點(diǎn)。

4）從open列表中選取下個(gè)位置的柵格節(jié)點(diǎn)。此時(shí)通過(guò)計(jì)算估價(jià)函數(shù)，分別計(jì)算出實(shí)際代價(jià)值和估計(jì)代價(jià)值之和。最后選取估計(jì)函數(shù)值最小節(jié)點(diǎn)，作為下一節(jié)點(diǎn)。同時(shí)將此節(jié)點(diǎn)從open列表中刪除，放入closed列表中，表示此柵格節(jié)點(diǎn)不在作為下一路徑節(jié)點(diǎn)選項(xiàng)。

5）不斷重復(fù)以上過(guò)程，最終會(huì)將目標(biāo)點(diǎn)加入到open列表中去。

在柵格環(huán)境中，啟發(fā)式搜索算法是通過(guò)估價(jià)函數(shù)進(jìn)行路徑搜索，并不會(huì)陷入局部最優(yōu)狀態(tài)。在全局路徑規(guī)劃上，用柵格建模下采用啟發(fā)式算法搜索路徑計(jì)算量較大。因此可通過(guò)人工勢(shì)場(chǎng)法先規(guī)劃，并通過(guò)時(shí)間距離檢測(cè)方法實(shí)時(shí)檢測(cè)移動(dòng)機(jī)器人是否陷入局部最優(yōu)點(diǎn)。當(dāng)路徑規(guī)劃完成時(shí)未檢測(cè)到移動(dòng)機(jī)器人陷入局部最優(yōu)，此時(shí)通過(guò)人工勢(shì)場(chǎng)正常完成規(guī)劃。當(dāng)檢測(cè)到移動(dòng)機(jī)器人陷入局部最優(yōu)點(diǎn)，此時(shí)根據(jù)檢測(cè)到實(shí)時(shí)位置，生成二維柵格地圖，并利用啟發(fā)式搜索算法完成路徑搜索。基本原理流程如圖11所示。

圖11 混合算法流程圖

4.4 局部最優(yōu)狀態(tài)下仿真分析

人工勢(shì)場(chǎng)在三種常陷入局部最優(yōu)狀態(tài)時(shí)，通過(guò)柵格建模運(yùn)用啟發(fā)式搜索算法進(jìn)行路徑搜索到達(dá)目標(biāo)點(diǎn)。如圖12～圖14所示，以下三種障礙物情形下，通過(guò)傳統(tǒng)人工勢(shì)場(chǎng)法無(wú)法完成規(guī)劃，都存在合力為零，或者環(huán)境過(guò)于復(fù)雜陷入局部穩(wěn)定震蕩區(qū)域。

圖12 單障礙物情況下陷入局部最優(yōu)

圖13 多障礙物情況下陷入局部最優(yōu)

圖14 多障礙物情況下陷入局部最優(yōu)

圖12 狀態(tài)下由于移動(dòng)機(jī)器人和障礙物、目標(biāo)點(diǎn)均處于同一延長(zhǎng)線上，因此當(dāng)傳統(tǒng)勢(shì)場(chǎng)法引力和斥力都處于同一方向延長(zhǎng)線，因此在接近障礙物時(shí)，移動(dòng)機(jī)器人所受合力為零，陷入局部最優(yōu)。在圖13狀態(tài)下，由于受到多種障礙物，并且存在合力為零的情況，因此也會(huì)陷入局部最優(yōu)點(diǎn)。圖14是常見(jiàn)的U星障礙物情況下，移動(dòng)機(jī)器人陷入局部震蕩無(wú)法逃離。

因此在移動(dòng)機(jī)器人通過(guò)周期時(shí)間距離檢測(cè)到陷入局部最優(yōu)時(shí)，此時(shí)生成二維柵格地圖，在柵格模型下運(yùn)用啟發(fā)式搜索算法搜索路徑到達(dá)目標(biāo)點(diǎn)。柵格地圖下啟發(fā)式搜索結(jié)果如圖15～圖18所示。

傳統(tǒng)勢(shì)場(chǎng)法由于無(wú)法掌握全局環(huán)境信息，在受單一數(shù)學(xué)模型下的虛擬力控制時(shí)很容易陷入局部最優(yōu)。局部最優(yōu)狀況在實(shí)際應(yīng)用場(chǎng)景中較少出現(xiàn)。人工勢(shì)場(chǎng)由于無(wú)需復(fù)雜建模，并且具有較好的避障結(jié)果。因此整體路徑規(guī)劃下，采用人工勢(shì)場(chǎng)法。實(shí)時(shí)通過(guò)時(shí)間周期距離檢測(cè)是否陷入局部最優(yōu)。當(dāng)未檢測(cè)到局部最優(yōu)時(shí)，即整個(gè)規(guī)劃過(guò)程都是通過(guò)人工勢(shì)場(chǎng)完成規(guī)劃。當(dāng)陷入局部最優(yōu)狀態(tài)時(shí)，此時(shí)通過(guò)生成柵格地圖運(yùn)用啟發(fā)式搜索算法到達(dá)目標(biāo)點(diǎn)。此混合規(guī)劃，可避免單一運(yùn)用人工勢(shì)場(chǎng)法無(wú)法完成整體規(guī)劃。

圖15 單障礙物局部最優(yōu)下到達(dá)目標(biāo)點(diǎn)

圖16 多障礙物局部最優(yōu)下到達(dá)目標(biāo)點(diǎn)

圖17 U型障礙物下陷入局部震蕩區(qū)域到達(dá)目標(biāo)點(diǎn)

圖18 復(fù)雜環(huán)境下到達(dá)目標(biāo)點(diǎn)

5 結(jié)語(yǔ)

本文提出在傳統(tǒng)傳統(tǒng)勢(shì)場(chǎng)在可能存在幾種障礙物情，通過(guò)時(shí)間周期距離檢測(cè)方法實(shí)時(shí)檢測(cè)移動(dòng)機(jī)器人是否現(xiàn)在局部最優(yōu)點(diǎn)，當(dāng)未檢測(cè)到移動(dòng)機(jī)器人陷入局部最優(yōu)時(shí)，則通過(guò)人工勢(shì)場(chǎng)法完成路徑規(guī)劃。當(dāng)檢測(cè)出移動(dòng)機(jī)器人陷入局部最優(yōu)時(shí)，則在局部最優(yōu)區(qū)域生成柵格地圖，通過(guò)啟發(fā)式搜索算法完成路徑規(guī)劃。