基于預添加虛擬力的改進人工勢場算法

張佳尚,陳志華

(南京理工大學 瞬態物理國家重點實驗室， 南京 210094)

1 引言

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

2 人工勢場法

2.1 基本原理

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

2.2 人工勢場數學模型

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

引力場與引力模型：

(1)

(2)

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

排斥勢場與斥力模型：

(3)

(4)

(5)

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

合勢場與合力模型：

Utol=Uatt+Urep

(6)

(7)

其中：Utol為合勢場強度；Ftol為機器人所受的合力。

3 改進方法

傳統人工勢場法常面臨的問題有：

1) 易陷入局部極小點[14]。即當勢場中出現除目標點外其他合力為零的點，使機器人在到達該點后無法再向目標點移動的現象。

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

傳統人工勢場法存在著缺陷，在應用中會面臨很多問題導致機器人無法安全到達目標點，要想在實際中運用必須進行改進，在本文中提出了將傳統人工勢場算法結合預添加虛擬力的方法改良傳統人工勢場法的缺陷，具體改進方法見下文。

3.1 解決局部極小點問題的方法

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

但丁韙良也是京師同文館的教習，其譯文表述必須借助中國譯員的協助，方能將浸淫傳統理學、只識文言文的清朝大員納入目標讀者群的序列。這就給了本土文化的主導意識形態控制譯者聲音的可乘之機。畢竟以培養中國譯員為目標的京師同文館本身即清政府的官辦機構。

第1步：當利用傳統人工勢場算法達到設置的最大迭代次數但規劃的軌跡未達到目標點時判斷目標點處的合勢場是否為零，若為零則進行第2步。若不為零，則可得知系統陷入目標點不可達問題，直接結束此進程，進入解決目標點不可達問題的子程序。

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

第3步：若出現幾個預選方向到達的下一個極小點仍是局部極小點而存在其他預選方向可到達的下一個極小點是目標點則將再次陷入局部極小點的幾條路徑重新執行第2步。在舍棄不合理的預添加虛擬力的方向后規劃出可以到達目標點的幾條軌跡。然后分別計算每條預添加虛擬力方向的預規劃路徑的路程，選取其中路程最短的方向確定為最優虛擬力方向，若出現兩條路程相同的路徑選取預添加虛擬力方向與用傳統人工勢場法規劃出的陷入局部極小點的路徑間夾角最小的一條路徑，若為對稱路徑選擇改進算法先得到的一條路徑，然后結束算法進程。

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

3.2 解決目標點不可達問題的方法

針對目標點在障礙物附近時可能存在目標不可達的問題，提出的解決辦法如下：

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

4 仿真實驗

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

4.1 單障礙物陷入局部極小點的場景

為驗證在環境中存在一個障礙物的情況下機器人陷入局部極小點時改進算法的優化效果，設計了兩組仿真實驗。

第1組仿真實驗設置機器人的起始點位置坐標為(10，10)，目標點位置坐標為(360，360).仿真環境中存在一個圓形障礙物，其坐標為(250，250)，半徑為30 cm，障礙物影響范圍為15 cm。

圖2為運用傳統人工勢場法進行路徑規劃得到的二維勢場中的軌跡圖，圖2中藍色圓點為起始點，綠色圓點為目標點，紅線為規劃的軌跡，大小不一的箭頭為勢場線。從仿真結果可以看出運用傳統方法規劃的軌跡陷入了局部極小點，未能完成到達目標點的任務。

圖2 傳統APF解決第一組實驗時勢場中的軌跡圖

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

第2組仿真實驗設置機器人的起始點位置坐標為 (80，200)，目標點位置坐標為(370，200)。仿真環境中存在一個由3個矩形障礙物拼接而成的C型障礙物，障礙物影響范圍為15 cm。

圖3 改進算法解決第一組實驗時的軌跡圖

圖4 改進算法解決第一組實驗時勢場中的軌跡圖

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

圖6 改進算法解決第二組實驗的軌跡圖

圖7 改進算法解決第二組實驗時勢場中的軌跡圖

4.2 目標點不可達的場景

為驗證運用傳統人工勢場法進行路徑規劃陷入目標點不可達問題時改進算法的優化效果，設計了一組仿真實驗。障礙物的環境情況為起始點與目標點之間有障礙物相隔，但目標點在障礙物影響范圍內。仿真時設置的機器人的起始點位置坐標為(10，10)，目標點位置坐標為(337，350).仿真環境中存在一個圓形障礙物，其坐標為(300，330)，半徑為40 cm，障礙物影響范圍為20 cm。

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

圖8 傳統APF解決問題時勢場中的軌跡圖

圖9 改進算法解決問題時勢場中的軌跡圖

4.3 多障礙物陷入局部極小點的場景

為驗證當環境中存在多障礙物情況下改進算法的性能，設計了兩組多障礙物場景的仿真實驗。

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

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

圖10 利用傳統APF時勢場中的軌跡圖

圖11 利用改進算法時勢場中的軌跡圖

為驗證改進算法在更加復雜的障礙物環境下的優化效果，設計了第二組仿真實驗。實驗設置機器人的起始點位置坐標為(10，10)，目標點位置坐標為(460，460)。仿真環境中存在15個不同形狀的障礙物，障礙物的影響范圍都為15 cm。

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

圖12 利用傳統APF時的軌跡圖

圖13 利用改進算法時的軌跡圖

5 結論

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