改進人工勢場法在未知環境中的路徑規劃研究

陳冠星，張志安，黃學功，華 洪，施振穩

(南京理工大學機械工程學院，江蘇 南京 210094)

0 引言

機器人的路徑規劃，是指機器人根據給定地圖環境信息或者傳感器探測信息尋找一條從起點到終點的無碰撞路徑[1]。路徑規劃在無人機[2]、無人車[3]等方面都有廣泛應用。

移動機器人的路徑規劃包括全局路徑規劃和局部路徑規劃[4]。局部路徑規劃可以應用在未知環境中，局部路徑規劃算法包括人工勢場法[5]、動態窗口法[6]等。在復雜未知環境中，人工勢場法因其實時性高、計算量小的優點得到廣泛應用[7-8]。但人工勢場法本身也存在缺陷：路徑震蕩和局部極小點。

對于路徑震蕩問題，文獻[9]采用過濾震蕩點法解決規劃路徑震蕩的問題，但是其需要提前規劃數個周期的路徑，計算量較大。本文提出采用角度過濾器的方法改善了規劃路徑震蕩的問題，在計算引力時采用不同增益系數的方法進一步改善了規劃路徑震蕩的問題，最終得到了較為光滑的路徑。對于局部極小點問題，文獻[10]針對U型陷阱問題添加了模糊控制的思想，提出了判斷U型障礙物的依據，但是當U型障礙物范圍較大時，這種方法會極大增加機器人的運動路徑；文獻[11]采用改進人工協調場的方法來改進局部極小點問題，但是沒有在更復雜的環境中進行實驗；文獻[12]針對傳統人工勢場法的缺點，使用勢場強度代替力矢量進行路徑規劃，利用遺傳信賴域算法搜索機器人周圍勢場強度最小點，并引入“填平勢場”解決局部極值點問題。在此，本文采用添加虛擬目標點的方法引導機器人走出陷阱區域，且對多種陷阱區域進行了仿真驗證。

1 人工勢場法

1.1 傳統人工勢場法

1986年Khatb首次提出了人工勢場法，其基本原理是將機器人所處環境模擬為一個人造勢場，機器人所處的環境中，人工勢場的勢場函數定義為

U(q)=Uatt(q)+Urep(q)

(1)

機器人所受的合力為

F(q)=Fatt(q)+Frep(q)

(2)

U(q)為總的勢場函數；Uatt(q)為目標點產生的引力勢場函數；Urep(q)為障礙物產生的斥力勢場函數；q為機器人在絕對坐標系中的位置；F(q)為機器人所受合力；Fatt(q)為目標點對機器人的引力；Frep(q)為障礙物對機器人的斥力。

人工勢場受力模型如圖1所示，目標點對機器人產生引力Fatt，障礙物對機器人產生斥力Frep。目標點所產生的引力會引導機器人朝著目標點運動，而障礙物產生的斥力避免了機器人與障礙物的碰撞，在引力和斥力的合力F作用下，機器人能夠無碰撞地到達目標點。

圖1 人工勢場受力模型

目標點產生的引力勢場為全局勢場，且勢場與機器人和目標點距離的平方成比例關系，目標點產生的勢場函數和引力一般定義為

(3)

Ka為引力增益系數；qtar為目標點在絕對坐標系中的位置；ρ(q,qtar)為機器人距離目標點的距離。根據引力函數，引力的方向為機器人指向目標點，且距離目標點越遠，引力越大。

障礙物產生的斥力勢場為局部勢場，只有在障礙物影響范圍內具有勢場，斥力勢場函數定義為

(4)

Kr為斥力增益系數；ρ(q,qobs)為機器人與障礙物的最近距離；ρd為障礙物的影響范圍。由函數可知，在障礙物的影響范圍內才具有勢場，機器人才會受到斥力。機器人所受的斥力函數為

Frep(q)=-Urep(q)=

(5)

1.2 傳統人工勢場法的缺點

機器人利用人工勢場法規劃路徑時，在某些情況下，規劃出來的路徑呈現震蕩狀，如同“鋸齒波”。例如，在機器人所受引力方向和斥力方向之間的夾角大于一定角度時，極易出現路徑震蕩。還有一種情況，在機器人經過狹窄通道時，規劃路徑也極其容易出現震蕩狀。如圖2所示，人工勢場法規劃路徑分別在“震蕩段1”(圖2中下方震蕩段)和“震蕩段2”(圖2中上方震蕩段)處出現震蕩，可以看出震蕩呈“鋸齒波”狀。

圖2 規劃路徑震蕩

此外，人工勢場法因為算法本身的原因，機器人在環境中運動時會陷入局部極小點，導致機器人在局部極值點附近徘徊不前。因為人工勢場中存在目標點產生的全局引力勢場以及障礙物產生的局部斥力勢場，機器人所受合力為勢場函數的梯度，此梯度即為勢能下降最快的方向，導致在局部極小點附近機器人所受合力一直指向局部極小點，因此，使機器人在局部極小點附近徘徊不前，在所受的力方面表現為機器人受到的引力與斥力共線反向。機器人陷入局部極小點狀態如圖3所示。

圖3 機器人陷入局部極小點

當障礙物為長邊界障礙物時，機器人更加容易陷入局部極小點。在機器人靠近局部極小點的過程中，機器人受到的引力方向和斥力方向之間的夾角大于一定角度時，人工勢場法規劃路徑是震蕩的，而且當機器人越靠近局部極小點，引力方向和斥力方向夾角越大，規劃路徑震蕩程度越劇烈。機器人會陷入局部極小點，同時規劃路徑劇烈震蕩，如圖4所示。

圖4 機器人陷入局部極小點

2 改進人工勢場法

2.1 解決規劃路徑震蕩

人工勢場法規劃路徑是由機器人所受合力方向決定的。如圖5所示，機器人的位置分別經過R1、R2和R3，合力分別為F1、F2和F3，合力方向不斷變化，導致機器人頻繁遠離和靠近障礙物。合力方向變化劇烈，是由于機器人受到的引力在斥力方向上的分力Fa2與斥力Frep的合力(Fa2-Frep)方向不斷變化，而合力在斥力垂直方向上的分力為Fa1，兩者絕對值的比值為比重系數k=|(Frep-Fa2)|/|Fa1|。當機器人受到的引力方向與斥力方向之間的夾角越大時，k越大，規劃路徑越震蕩。

圖5 機器人受力示意

圖2中“震蕩段1”和“震蕩段2”處的機器人受力曲線如圖6所示(受力為虛擬力，無單位)。由圖6可以發現，機器人所受引力基本上沒有變化，機器人受到的斥力劇烈變化導致機器人受到的合力方向劇烈變化。并且根據圖6可知，合力震蕩的幅值越來越大，這是因為，機器人受到的斥力和引力之間的夾角越來越大，導致k值越來越大，機器人受到的合力在斥力垂直方向上的分力所占的比重越來越小。

圖6 路徑震蕩時機器人受力曲線

為了減輕規劃路徑震蕩程度，本文提出利用角度過濾器計算機器人下一周期運動方向。角度過濾器通過對傳統人工勢場法計算出來的機器人轉動角度Θ乘以過濾系數f,得到最終的機器人轉動角度Θ′，角度過濾器過濾系數定義為

(6)

根據角度過濾器原理，機器人下一周期的運動方向向量按如下步驟計算：

a.記錄本周期機器人在絕對坐標系中的運動方向單位向量為V1，V1的方向角度(0～360°)為Θ1，通過傳統人工勢場算法計算得到下一周期的運動方向單位向量為V2，V2的方向角度為Θ2，計算可得V1和V2的夾角為Θ。為了方便描述，假設V1逆時針旋轉Θ角后達到V2,則Θ2=Θ1+Θ。

c.定義V1和下一周期運動方向單位向量V3之間的夾角為Θ′,則Θ′=fΘ，Θ3=Θ1+Θ′。

加入角度過濾器后的路徑規劃效果如圖7所示，與圖2所示傳統人工勢場法的路徑規劃效果對比，可以看出路徑已經平滑了很多。

圖7 加入角度過濾器的效果

改進人工勢場法規劃路徑的結果數據如表1所示，由表1可知，加入角度濾波器的路徑規劃相對于傳統人工勢場法來說，規劃路徑的總轉動角度大幅減少，規劃周期也有減少。

表1 路徑規劃結果

在添加角度過濾器的基礎上，針對規劃路徑震蕩的問題，提出在計算機器人受到的引力的分力Fa1和Fa2時采用不同增益系數k1和k2。根據前面定義的機器人引力函數和斥力函數，計算出機器人受到的引力Fa和斥力Frep。然后，如圖8所示，以機器人為坐標原點Or，機器人所受斥力反方向為Y軸正方向，以Y軸正方向順時針旋轉90°方向為X軸正方向，建立斥力坐標系OrXY。

圖8 機器人在斥力坐標系中的受力示意

把機器人受到的引力Fa在斥力坐標系中分解可得到Fa1和Fa2，Fa1就是引力在斥力方向上的分力，Fa2就是引力在斥力垂直方向上的分力，并設置引力在斥力坐標系中的增益系數k1和k2。按照傳統人工勢場法計算的引力進行分解，則k1=k2，機器人引力在斥力坐標系中表示為

(7)

(8)

如圖9所示，在加入角度過濾器的基礎上，計算引力時采用不同增益系數，與圖7進行對比，可以看出在某些位置處算法規劃路徑更加平滑。

圖9 進一步采用不同引力增益系數效果

在加入角度過濾器基礎上，計算引力時采用不同增益系數的數據結果如表2所示。由表2可知，相比于只添加角度過濾器，進一步采用不同增益系數使得規劃路徑總轉動角度進一步減少。

表2 采用不同增益系數的結果

2.2 解決局部極值點

人工勢場法因其算法本身在路徑規劃過程中會出現局部極小點，會導致路徑規劃失敗。在局部極小點處，機器人受到的引力方向和斥力方向成180°關系，尤其當環境未知，且環境中存在長邊界型障礙物、U型障礙物等復雜障礙物陷阱時，則更容易陷入局部極小點而難以脫離。因此，避免局部極小點的關鍵就是保證引力和斥力的夾角小于180°，本文采用添加虛擬目標點來脫離局部極小點。

2.2.1 虛擬目標點確定方法

在人工勢場法進行路徑規劃的過程中，可能因為多種類型的障礙物陷阱導致機器人陷入局部極小點，包括長邊界型障礙物和U型障礙物等。如圖10所示為長邊界型障礙物導致機器人陷入局部極小點，在局部極小點附近，機器人受到的引力方向和斥力方向的夾角接近180°，而且合力在斥力方向的垂直方向的分力會一直吸引機器人往局部極小點處靠近，因為添加了解決震蕩的措施，所以機器人一直在圍繞局部極小點運動。圖11所示為U型障礙物導致機器人陷入局部極小點，機器人陷入局部極小點的原因與上述原因相同。

圖10 長邊界型障礙物導致的局部極小點

圖11 U型陷阱導致的局部極小點

人工勢場法加入虛擬目標點的算法執行過程如圖12所示。首先判斷機器人是否到達目標點，如果機器人到達目標點，則完成路徑規劃，如果沒有到達目標點，則判斷機器人是否陷入陷阱區域。當機器人沒有陷入陷阱區域時，按照原來目標點進行路徑解算，當引力和斥力夾角大于170°時，本文默認機器人陷入陷阱區域，這時把陷入陷阱標志位flag設置為1。當機器人陷入陷阱區域，利用添加虛擬目標點的方法引導機器人走出陷阱區域。當機器人脫離了陷阱區域后，設置陷入陷阱標志位flag=0，繼續按照原來目標點進行路徑解算。

圖12 加入虛擬目標點后算法執行流程

有些文獻是根據機器人受力達到平衡時的特征來判定機器人陷入局部極小點的，如機器人繞局部極小點徘徊，但是這種判定會增加機器人的運動時間以及運動路徑。所以在引力方向和斥力方向的夾角大于170°(機器人距離局部極小點還有一段距離)時,不管機器人運動前方是否存在局部極小點，本文都默認機器人陷入局部極小點。

放置虛擬目標點的位置按2步計算。首先，如圖13所示，以機器人的全局坐標位置為原點Oq，以斥力方向為Y軸正方向，如果在機器人運動過程中機器人與障礙物的距離大于障礙物影響范圍ρd，則以障礙物距離機器人最近點指向機器人的方向為Y軸正方向。以Y軸正方向順時針旋轉90°方向為X軸正方向，建立局部坐標系OqXY，這時可得虛擬目標點在此局部坐標系Y軸上的坐標為Ty=-ρd。接下來求取虛擬目標點在局部坐標系X軸上的坐標Tx，此時根據機器人運動方向在局部坐標系中的表示vq=[vq,xvq,y]來判定Tx。如果vq,x≤0,則

圖13 添加虛擬目標點

Tx=-2ρd,記虛擬目標點選擇方向dir=-1，表示障礙物在機器人前進路徑的左側，否則，Tx=2ρd，記dir=1，表示障礙物在機器人前進路徑的右側。

2.2.2 虛擬目標點取消方法

當采用添加虛擬目標點的方法脫離陷阱區域時，要能夠判斷是否已經脫離陷阱區域。如果已經脫離陷阱區域，則使用原目標點進行路徑解算，否則，繼續添加虛擬目標點進行路徑解算。假設機器人開始采用添加虛擬目標點的規劃周期為n。則判斷機器人是否脫離陷阱區域的步驟為：

a.添加虛擬目標點，進行路徑解算，假設機器人現在處于第j個規劃周期，計算機器人相對于第j-1個周期的轉動角度βj，記機器人順時針轉動時，βj<0,當機器人逆時針轉動時，βj>0。

b.計算從第n個規劃周期到第j個規劃周期機器人轉動總角度βsum=∑jβ(j≥n)。同時計算機器人距離原目標點的距離ρ(qj,qtar)，當j≥n+m時，比較ρ(qj,qtar)和ρ(qj-m,qtar)的關系，這里m取3。如果ρ(qj,qtar)≥ρ(qj-m,qtar),令j=j+1，并跳到步驟a，否則跳到步驟c。

c.如果虛擬目標點選擇方向dir=-1，表示障礙物在機器人前進路徑的左側，當βsum>45°,結束添加虛擬目標點。如果dir=1，表示障礙物在機器人前進路徑的右側，當βsum<-45°,結束添加虛擬目標點。如果沒有達到結束添加虛擬目標點的條件，則令j=j+1，并跳到步驟a。

機器人在虛擬目標點的引導下順利通過U型障礙物如圖14所示。

圖14 機器人順利通過U型障礙物

3 仿真結果及分析

在未知復雜環境采用傳統人工勢場法進行路徑規劃如圖15所示。由圖15可以看出，機器人在局部極小點附近徘徊，導致機器人無法到達目標點。圖16表示采用改進算法引導機器人走出局部極小點，最終到達目標點。對比圖15和圖16可知,添加虛擬目標點能夠克服局部極小點的缺點。

圖15 傳統人工勢場法陷入復雜陷阱

圖16 機器人在虛擬目標點引導下走出障礙物

傳統人工勢場法和改進人工勢場法規劃路徑導致的機器人轉動角度的對比，如圖17所示，由圖17可以看出到達目標點后轉動角度值相差巨大。傳統人工勢場法和改進算法的數據結果如表3所示，由表3可知，采用改進的人工勢場法后，機器人總轉動角度極大減少，規劃周期也稍微減少。

圖17 改進算法和傳統算法結果對比

表3 改進算法解決震蕩的結果

圖18表示在另一復雜環境中，利用傳統人工勢場法進行路徑規劃，機器人陷于局部極小點，并且在靠近局部極小點的過程中，規劃路徑劇烈震蕩。圖19為利用改進算法進行路徑規劃，由于設定為未知環境，所以機器人會進入障礙物圍繞區域，但通過改進算法可以引導機器人走出障礙物區域，并尋求一條路徑到達目標點。

圖18 傳統人工勢場法進行路徑規劃

圖19 改進算法進行路徑規劃

機器人運動到圖18中a點時，傳統人工勢場法和改進算法的數據結果對比如表4所示。因為傳統人工勢場法會導致規劃路徑劇烈震蕩，所以機器人運動到a點為止的總轉動角度非常大，改進的算法可以明顯減輕路徑震蕩，機器人運動到a點的總轉動角度極大減少，且規劃周期也明顯減少。

表4 算法改進前后a點的結果

4 結束語

本文就人工勢場法存在的規劃路徑震蕩以及局部極小點問題進行了分析，并對這2個問題進行了改進。

a.通過本文分析可以知道，雖然人工勢場法理論上不會存在規劃路徑震蕩的問題，但是在實際應用中要對人工勢場法進行離散化處理，導致機器人受到的引力在斥力方向的分力與斥力大小關系不斷轉變，導致規劃路徑震蕩。因此，本文采用添加角度過濾器對機器人轉角進行了限制，且考慮了機器人避障安全性，并進一步采用不同增益系數來計算引力。結果表明，規劃路徑震蕩問題得到極大改善。

b.針對未知復雜環境下人工勢場法容易出現的局部極小點問題，本文提出采用虛擬目標點引導機器人走出復雜環境，最終到達目標點。