張佳尚,陳志華

(南京理工大學(xué) 瞬態(tài)物理國家重點(diǎn)實(shí)驗(yàn)室， 南京 210094)

1 引言

路徑規(guī)劃是保證移動機(jī)器人自主導(dǎo)航能夠穩(wěn)定運(yùn)行的關(guān)鍵技術(shù)之一，吸引了眾多學(xué)者對路徑規(guī)劃算法進(jìn)行研究。人工勢場法(artificial potential field,APF)是由Khatib提出的一種用于機(jī)器人路徑規(guī)劃的算法[1]。人工勢場法具有直觀，對運(yùn)算量要求不高，可以跟機(jī)器人的控制相結(jié)合[2]的特點(diǎn)，因此得到了廣泛的應(yīng)用。但傳統(tǒng)人工勢場法并不是完備的，在實(shí)際應(yīng)用中無法避開陷入局部極小點(diǎn)與目標(biāo)點(diǎn)不可達(dá)問題。解決傳統(tǒng)人工勢場法的易陷入局部極小點(diǎn)問題的改進(jìn)方法主要分成2個方向：一種是構(gòu)造合適的勢場函數(shù)以減小或避免局部極小點(diǎn)的出現(xiàn)；另一種是將人工勢場法與其他的算法結(jié)合[3]，從而改善傳統(tǒng)人工勢場法的缺陷?，F(xiàn)有的研究成果中運(yùn)用第一種方法的有：Liu等[4]提出的通過優(yōu)化斥力勢場函數(shù)，調(diào)整斥力分量在坐標(biāo)軸上的方向，來解決傳統(tǒng)人工勢場法的局部極小值缺陷。Qi等[5]通過引入動態(tài)調(diào)整系數(shù)，對傳統(tǒng)人工勢場法中的引力和斥力函數(shù)進(jìn)行改進(jìn)，使避障安全系數(shù)更高，路徑更平滑。針對目標(biāo)不可達(dá)問題，在引力函數(shù)中構(gòu)建新的引力勢場，能夠高效地解決無人機(jī)的路徑規(guī)劃避障問題。Zhang等[6]提出了一種在斥力函數(shù)中加入一個繞行力來改進(jìn)斥力函數(shù)的方法，同樣可以有效地解決局部極小點(diǎn)問題。運(yùn)用第2種方法的有：郭梟鵬[7]提出融合人工勢場法與快速擴(kuò)展隨機(jī)樹算法(rapidly-exploring random tree,RRT)的特性，通過RRT算法與人工勢場法優(yōu)勢互補(bǔ)同時解決各自的缺陷，通過多個算法的對比實(shí)驗(yàn)，證明了APF-RRT算法具有良好的適用性。本文的改進(jìn)方向是基于第2種改進(jìn)方法，通過將預(yù)添加虛擬力的算法與傳統(tǒng)人工勢場算法結(jié)合改良傳統(tǒng)人工勢場法的缺陷。通過設(shè)置的五組仿真實(shí)驗(yàn)結(jié)果與傳統(tǒng)人工勢場法的仿真結(jié)果對比，分別驗(yàn)證了在環(huán)境中存在一個與多個障礙物的情況下基于預(yù)添加虛擬力的改進(jìn)人工勢場算法的可行性與有效性。證明了改良算法能夠解決傳統(tǒng)人工勢場算法易陷入局部極小值與目標(biāo)點(diǎn)不可達(dá)的問題。相較于其他的第二種改良方向的方法，例如通過設(shè)置虛擬目標(biāo)點(diǎn)、人工勢場法結(jié)合蟻群算法[8]、人工勢場法結(jié)合粒子群算法[9-10]、人工勢場法結(jié)合快速擴(kuò)展隨機(jī)樹算法[11]等改良方向，本文提出的改良方法具有計算量小，規(guī)劃速度快，實(shí)時性好，規(guī)劃路徑更短等優(yōu)點(diǎn)。

2 人工勢場法

2.1 基本原理

人工勢場法是一種通過將障礙物和目標(biāo)點(diǎn)抽象為斥力與引力虛擬勢場的算法，讓目標(biāo)點(diǎn)處于谷底，距離目標(biāo)點(diǎn)越遠(yuǎn)的位置上引力勢場越高，同時為了避免碰撞，在障礙物四周添加排斥勢場[12-13]。圖1為利用傳統(tǒng)人工勢場法得到的勢場中的軌跡，其中藍(lán)色箭頭為勢場線，箭頭越長代表該位置的引力或斥力越大，紅線為規(guī)劃的軌跡。機(jī)器人距離目標(biāo)點(diǎn)越遠(yuǎn)，受到目標(biāo)點(diǎn)的引力越強(qiáng)，機(jī)器人在障礙物影響范圍內(nèi)離障礙物越近受到的排斥力越強(qiáng)，當(dāng)機(jī)器人不在障礙物影響范圍內(nèi)時不會受到排斥力。人工勢場法會根據(jù)機(jī)器人在勢場中的受力情況，沿著所受到的引力與斥力合力的方向自主規(guī)劃出一條能夠到達(dá)目標(biāo)點(diǎn)的最優(yōu)路徑。

2.2 人工勢場數(shù)學(xué)模型

人工勢場法模型分為引力場模型與斥力場模型。

引力場與引力模型：

(1)

(2)

其中：Uatt為引力勢場強(qiáng)度；Fatt為機(jī)器人所受的引力；katt為引力勢場增益系數(shù)；dg為機(jī)器人到目標(biāo)點(diǎn)的直線距離；grad(·)為求·的梯度；m為常數(shù)，通常取值為2；eg為機(jī)器人到目標(biāo)點(diǎn)的單位矢量。

排斥勢場與斥力模型：

(3)

(4)

(5)

其中：kUrep為第k個障礙物的排斥勢場強(qiáng)度；krep為排斥勢場增益系數(shù)；kdo為機(jī)器人到第k個障礙物的直線距離；dd為障礙物的影響范圍；keo為機(jī)器人到第k個障礙物的單位矢量；kFrep為第k個障礙物對機(jī)器人的排斥力；Frep為機(jī)器人在勢場中受到的所以排斥力的合力。

合勢場與合力模型：

Utol=Uatt+Urep

(6)

(7)

其中：Utol為合勢場強(qiáng)度；Ftol為機(jī)器人所受的合力。

3 改進(jìn)方法

傳統(tǒng)人工勢場法常面臨的問題有：

1) 易陷入局部極小點(diǎn)[14]。即當(dāng)勢場中出現(xiàn)除目標(biāo)點(diǎn)外其他合力為零的點(diǎn)，使機(jī)器人在到達(dá)該點(diǎn)后無法再向目標(biāo)點(diǎn)移動的現(xiàn)象。

2) 當(dāng)目標(biāo)點(diǎn)在障礙物影響范圍內(nèi)時可能存在目標(biāo)點(diǎn)不可達(dá)的問題[15]。即當(dāng)機(jī)器人接近目標(biāo)點(diǎn)時，其所受到的引力會逐漸減小，若此時目標(biāo)點(diǎn)恰好在障礙物的影響范圍內(nèi)，其所受到的排斥力會因機(jī)器人靠近目標(biāo)點(diǎn)的同時靠近障礙物而不斷增大且與引力方向相反，就會使機(jī)器人停在某一點(diǎn)(局部極小點(diǎn))或發(fā)生抖動，導(dǎo)致無法到達(dá)目標(biāo)點(diǎn)的現(xiàn)象。目標(biāo)點(diǎn)不可達(dá)問題可以看作是陷入局部極小點(diǎn)問題的特例，不同于其他陷入局部極小點(diǎn)問題的是此時的目標(biāo)點(diǎn)不再是全局極小點(diǎn)，使得機(jī)器人到達(dá)不了目標(biāo)點(diǎn)而陷入局部極小點(diǎn)。

傳統(tǒng)人工勢場法存在著缺陷，在應(yīng)用中會面臨很多問題導(dǎo)致機(jī)器人無法安全到達(dá)目標(biāo)點(diǎn)，要想在實(shí)際中運(yùn)用必須進(jìn)行改進(jìn)，在本文中提出了將傳統(tǒng)人工勢場算法結(jié)合預(yù)添加虛擬力的方法改良傳統(tǒng)人工勢場法的缺陷，具體改進(jìn)方法見下文。

3.1 解決局部極小點(diǎn)問題的方法

針對易陷入局部極小值點(diǎn)的問題，提出的解決方法如下：

但丁韙良也是京師同文館的教習(xí)，其譯文表述必須借助中國譯員的協(xié)助，方能將浸淫傳統(tǒng)理學(xué)、只識文言文的清朝大員納入目標(biāo)讀者群的序列。這就給了本土文化的主導(dǎo)意識形態(tài)控制譯者聲音的可乘之機(jī)。畢竟以培養(yǎng)中國譯員為目標(biāo)的京師同文館本身即清政府的官辦機(jī)構(gòu)。

第1步：當(dāng)利用傳統(tǒng)人工勢場算法達(dá)到設(shè)置的最大迭代次數(shù)但規(guī)劃的軌跡未達(dá)到目標(biāo)點(diǎn)時判斷目標(biāo)點(diǎn)處的合勢場是否為零，若為零則進(jìn)行第2步。若不為零，則可得知系統(tǒng)陷入目標(biāo)點(diǎn)不可達(dá)問題，直接結(jié)束此進(jìn)程，進(jìn)入解決目標(biāo)點(diǎn)不可達(dá)問題的子程序。

第2步：通過給原人工勢場預(yù)添加幾個指定方向的虛擬力(具體方法在下文給出)，得到機(jī)器人在勢場中受到的新合力，根據(jù)新合力繼續(xù)規(guī)劃路徑，使機(jī)器人能夠逃離局部極小點(diǎn)，到達(dá)下一個極小點(diǎn)(可能為目標(biāo)點(diǎn))且不碰撞障礙物，得到幾條可行的路徑。

第3步：若出現(xiàn)幾個預(yù)選方向到達(dá)的下一個極小點(diǎn)仍是局部極小點(diǎn)而存在其他預(yù)選方向可到達(dá)的下一個極小點(diǎn)是目標(biāo)點(diǎn)則將再次陷入局部極小點(diǎn)的幾條路徑重新執(zhí)行第2步。在舍棄不合理的預(yù)添加虛擬力的方向后規(guī)劃出可以到達(dá)目標(biāo)點(diǎn)的幾條軌跡。然后分別計算每條預(yù)添加虛擬力方向的預(yù)規(guī)劃路徑的路程，選取其中路程最短的方向確定為最優(yōu)虛擬力方向，若出現(xiàn)兩條路程相同的路徑選取預(yù)添加虛擬力方向與用傳統(tǒng)人工勢場法規(guī)劃出的陷入局部極小點(diǎn)的路徑間夾角最小的一條路徑，若為對稱路徑選擇改進(jìn)算法先得到的一條路徑，然后結(jié)束算法進(jìn)程。

預(yù)添加虛擬力方法中最重要問題的是如何選擇合適的時機(jī)去掉預(yù)添加虛擬力。在本文中選擇當(dāng)達(dá)到設(shè)定的最大迭代次數(shù)或新規(guī)劃的軌跡點(diǎn)與目標(biāo)點(diǎn)的連線之間沒有障礙物時，則去掉預(yù)添加虛擬力，此方法可有效避免與障礙物發(fā)生碰撞并減少冗余路徑，得到最優(yōu)路徑。若想使預(yù)添加虛擬力的方向更接近最優(yōu)，可以通過更加細(xì)化等分預(yù)添加虛擬力的角度的方法，并且等分的角度越小預(yù)添加虛擬力的方向就越精確，但是如果細(xì)化等分的角度過小，將會延長規(guī)劃多個方向路徑與計算路徑長度的時間，從而在一定程度上影響改進(jìn)人工勢場法避障的實(shí)時性。

3.2 解決目標(biāo)點(diǎn)不可達(dá)問題的方法

針對目標(biāo)點(diǎn)在障礙物附近時可能存在目標(biāo)不可達(dá)的問題，提出的解決辦法如下：

若通過上面3.1節(jié)中的第一步判斷出目標(biāo)點(diǎn)處的合勢場不為零，則可判斷為遇到了目標(biāo)點(diǎn)不可達(dá)問題。改進(jìn)算法選擇將在目標(biāo)點(diǎn)處給目標(biāo)點(diǎn)施加排斥勢場的障礙物在勢場中先去除掉，然后在原軌跡的基礎(chǔ)上規(guī)劃出一條能夠到達(dá)目標(biāo)點(diǎn)的預(yù)規(guī)劃路徑。再重新將障礙物放回原勢場中，若預(yù)規(guī)劃路徑與障礙物無碰撞，則可將預(yù)規(guī)劃路徑作為最優(yōu)路徑。若預(yù)規(guī)劃路徑與障礙物發(fā)生碰撞，則將預(yù)規(guī)劃路徑與障礙物影響范圍重合的部分路徑進(jìn)行重新規(guī)劃，此時運(yùn)用預(yù)添加虛擬力的方法規(guī)劃出重合部分的新路徑，將新路徑與原規(guī)劃路徑結(jié)合作為最終的規(guī)劃路徑。

4 仿真實(shí)驗(yàn)

以下運(yùn)用Matlab 2020b軟件進(jìn)行仿真，利用上述改進(jìn)方法對各種不同的障礙物環(huán)境下改進(jìn)算法的有效性進(jìn)行驗(yàn)證。在仿真實(shí)驗(yàn)中人工勢場各參數(shù)設(shè)置為：katt=0.001，m=2，krep=800，dd=15 cm。仿真結(jié)果圖中X坐標(biāo)與Y坐標(biāo)單位為厘米。

4.1 單障礙物陷入局部極小點(diǎn)的場景

為驗(yàn)證在環(huán)境中存在一個障礙物的情況下機(jī)器人陷入局部極小點(diǎn)時改進(jìn)算法的優(yōu)化效果，設(shè)計了兩組仿真實(shí)驗(yàn)。

第1組仿真實(shí)驗(yàn)設(shè)置機(jī)器人的起始點(diǎn)位置坐標(biāo)為(10，10)，目標(biāo)點(diǎn)位置坐標(biāo)為(360，360).仿真環(huán)境中存在一個圓形障礙物，其坐標(biāo)為(250，250)，半徑為30 cm，障礙物影響范圍為15 cm。

圖2為運(yùn)用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃得到的二維勢場中的軌跡圖，圖2中藍(lán)色圓點(diǎn)為起始點(diǎn)，綠色圓點(diǎn)為目標(biāo)點(diǎn)，紅線為規(guī)劃的軌跡，大小不一的箭頭為勢場線。從仿真結(jié)果可以看出運(yùn)用傳統(tǒng)方法規(guī)劃的軌跡陷入了局部極小點(diǎn)，未能完成到達(dá)目標(biāo)點(diǎn)的任務(wù)。

圖2 傳統(tǒng)APF解決第一組實(shí)驗(yàn)時勢場中的軌跡圖

圖3為運(yùn)用預(yù)添加虛擬力改進(jìn)算法后得到的軌跡圖，圖中黑色圓形為障礙物所在的位置，圖4為運(yùn)用預(yù)添加虛擬力改進(jìn)算法進(jìn)行路徑規(guī)劃得到的二維勢場中的軌跡圖。從圖3可以看出，運(yùn)用改進(jìn)算法規(guī)劃的軌跡能夠與障礙物保持足夠的距離，并沿著障礙物的排斥勢場邊緣規(guī)劃出平滑的路徑。結(jié)合圖3與圖4可知當(dāng)運(yùn)用傳統(tǒng)人工勢場法進(jìn)行軌跡規(guī)劃陷入局部極小值時，利用預(yù)添加虛擬力的改進(jìn)算法可以有效地使機(jī)器人逃離局部極小點(diǎn)，同時在保證不與障礙物發(fā)生碰撞的情況下降低冗余路程，規(guī)劃出一條能夠到達(dá)目標(biāo)點(diǎn)的軌跡，證明了提出的基于預(yù)添加虛擬力的改良方法在解決傳統(tǒng)人工勢場法易陷入局部極小值問題的可行性。

第2組仿真實(shí)驗(yàn)設(shè)置機(jī)器人的起始點(diǎn)位置坐標(biāo)為 (80，200)，目標(biāo)點(diǎn)位置坐標(biāo)為(370，200)。仿真環(huán)境中存在一個由3個矩形障礙物拼接而成的C型障礙物，障礙物影響范圍為15 cm。

圖3 改進(jìn)算法解決第一組實(shí)驗(yàn)時的軌跡圖

圖4 改進(jìn)算法解決第一組實(shí)驗(yàn)時勢場中的軌跡圖

圖5為運(yùn)用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃得到的二維勢場中的軌跡圖，從仿真結(jié)果可以看出傳統(tǒng)勢場法規(guī)劃的軌跡由于目標(biāo)點(diǎn)與起始點(diǎn)的連線與機(jī)器人受到的障礙物的排斥力處于同一水平線，沒有垂直方向的分力，所以在引力與斥力平衡后陷入了局部極小點(diǎn)，使得機(jī)器人未能到達(dá)目標(biāo)點(diǎn)。圖6為運(yùn)用改進(jìn)算法得到的軌跡圖，圖7為運(yùn)用改進(jìn)算法進(jìn)行路徑規(guī)劃時二維勢場中的軌跡圖。從仿真結(jié)果可以看出在機(jī)器人陷入局部極小點(diǎn)后，通過添加的虛擬力能夠擺脫局部極小點(diǎn)，在與障礙物保持一定距離避免碰撞的同時，沿著障礙物的輪廓走出障礙物凹陷部分，最終在撤去虛擬力后，在人工勢場力的作用下順利到達(dá)目標(biāo)點(diǎn)。仿真結(jié)果表明在復(fù)雜的障礙物環(huán)境下改進(jìn)算法依舊可以克服傳統(tǒng)人工勢場法易陷入局部極小點(diǎn)的缺陷。

圖6 改進(jìn)算法解決第二組實(shí)驗(yàn)的軌跡圖

圖7 改進(jìn)算法解決第二組實(shí)驗(yàn)時勢場中的軌跡圖

4.2 目標(biāo)點(diǎn)不可達(dá)的場景

為驗(yàn)證運(yùn)用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃陷入目標(biāo)點(diǎn)不可達(dá)問題時改進(jìn)算法的優(yōu)化效果，設(shè)計了一組仿真實(shí)驗(yàn)。障礙物的環(huán)境情況為起始點(diǎn)與目標(biāo)點(diǎn)之間有障礙物相隔，但目標(biāo)點(diǎn)在障礙物影響范圍內(nèi)。仿真時設(shè)置的機(jī)器人的起始點(diǎn)位置坐標(biāo)為(10，10)，目標(biāo)點(diǎn)位置坐標(biāo)為(337，350).仿真環(huán)境中存在一個圓形障礙物，其坐標(biāo)為(300，330)，半徑為40 cm，障礙物影響范圍為20 cm。

圖8為利用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃得到的仿真結(jié)果。從仿真圖可知軌跡陷入了目標(biāo)點(diǎn)不可達(dá)問題，出現(xiàn)此問題的原因是目標(biāo)點(diǎn)在障礙物影響范圍內(nèi)，機(jī)器人所受到的斥力會因靠近障礙物而增大，并與其受到的引力相平衡，使得規(guī)劃的軌跡未能順利到達(dá)目標(biāo)點(diǎn)。圖9為利用改進(jìn)算法時得到的勢場中的軌跡圖，從仿真結(jié)果可以看出由于引入了虛擬力的原因，機(jī)器人克服了障礙物的排斥力，逃離了局部極小點(diǎn)(合力為零點(diǎn))，成功到達(dá)了目標(biāo)點(diǎn)。證明了改進(jìn)算法在解決傳統(tǒng)人工勢場法易陷入目標(biāo)點(diǎn)不可達(dá)問題時的有效性。

圖8 傳統(tǒng)APF解決問題時勢場中的軌跡圖

圖9 改進(jìn)算法解決問題時勢場中的軌跡圖

4.3 多障礙物陷入局部極小點(diǎn)的場景

為驗(yàn)證當(dāng)環(huán)境中存在多障礙物情況下改進(jìn)算法的性能，設(shè)計了兩組多障礙物場景的仿真實(shí)驗(yàn)。

第一組仿真實(shí)驗(yàn)設(shè)置機(jī)器人的起始點(diǎn)位置坐標(biāo)為(10，10)，目標(biāo)點(diǎn)位置坐標(biāo)為(360，360).仿真環(huán)境中存在3個障礙物，其坐標(biāo)分別為(210，210)、(280，320)、(310，230)，半徑依次為30 cm、35 cm和30 cm，障礙物影響范圍都為15 cm。

圖10為利用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃得到的勢場中的軌跡圖，從圖中可以看出利用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃時會陷入局部極小點(diǎn)，未能完成路徑規(guī)劃的任務(wù)。圖11為利用改進(jìn)算法進(jìn)行路徑規(guī)劃得到的勢場中的軌跡圖，從仿真結(jié)果可知引入預(yù)添加虛擬力的人工勢場法能夠有效地避免陷入局部極小點(diǎn)，在不與障礙物發(fā)生碰撞的情況下，規(guī)劃出一條最優(yōu)路徑，證明了此改進(jìn)方法在多障礙物環(huán)境下應(yīng)用的可行性。

圖10 利用傳統(tǒng)APF時勢場中的軌跡圖

圖11 利用改進(jìn)算法時勢場中的軌跡圖

為驗(yàn)證改進(jìn)算法在更加復(fù)雜的障礙物環(huán)境下的優(yōu)化效果，設(shè)計了第二組仿真實(shí)驗(yàn)。實(shí)驗(yàn)設(shè)置機(jī)器人的起始點(diǎn)位置坐標(biāo)為(10，10)，目標(biāo)點(diǎn)位置坐標(biāo)為(460，460)。仿真環(huán)境中存在15個不同形狀的障礙物，障礙物的影響范圍都為15 cm。

圖12為利用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃后得到的軌跡圖，從圖中可以看出利用傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃時會因仿真軌跡圖軌跡末端左上角的圓形障礙物與右下角的方形障礙物的排斥合力作用與來自目標(biāo)點(diǎn)的引力平衡，而陷入局部極小點(diǎn)，未能規(guī)劃出到達(dá)目標(biāo)點(diǎn)的路徑。圖13為利用改進(jìn)算法進(jìn)行路徑規(guī)劃得到的軌跡圖，從仿真結(jié)果可以看出原人工勢場作用力在結(jié)合了東南方向的虛擬力后，可以逃離局部極小點(diǎn)，到達(dá)目標(biāo)點(diǎn)。證明了改良算法多在障礙物的復(fù)雜工況下依舊能夠改良傳統(tǒng)人工勢場法的缺陷，規(guī)劃出合理的軌跡。

圖12 利用傳統(tǒng)APF時的軌跡圖

圖13 利用改進(jìn)算法時的軌跡圖

5 結(jié)論

在本文中提出了將預(yù)添加虛擬力的方法與傳統(tǒng)人工勢場法結(jié)合的想法，對傳統(tǒng)人工勢場法進(jìn)行改進(jìn)。通過設(shè)計的5組Matlab仿真實(shí)驗(yàn)，驗(yàn)證了在環(huán)境中存在一個障礙物與多個障礙物的復(fù)雜環(huán)境情況下，改進(jìn)算法能夠逃離局部極小點(diǎn)與目標(biāo)點(diǎn)不可達(dá)問題，同時降低冗余路程，并在避免與障礙物發(fā)生碰撞的前提下規(guī)劃出一條到達(dá)目標(biāo)點(diǎn)的合理軌跡，證明了改進(jìn)算法可以有效地改良傳統(tǒng)人工勢場法的缺陷，具有一定的可行性。