基于改進人工勢場的智能車動態避障算法

于慧，郭宗和，秦志昌

(山東理工大學 交通與車輛工程學院, 山東 淄博 255049)

無人駕駛汽車是智能交通系統的核心部分，也是汽車產業的未來發展方向，它涉及眾多先進領域，包括制造、控制、通信、傳感器、人工智能等，對國家的發展具有戰略意義。無人駕駛汽車可以盡可能地減少駕駛員的人工操作，減少人工因素而導致的道路事故，促進人類的出行更加安全、高效[1]。無人駕駛技術主要分為感知、決策、規劃、控制四大模塊，路徑規劃模塊是無人駕駛技術的關鍵部分。路徑規劃需要參照基本道路信息、車輛狀態等，規劃出能夠使汽車從當前位置行駛到目標位置的最優路徑[2]。

目前，人工勢場法、A星算法、蟻群算法、隨機樹法、曲線插值法等是動態避障路徑規劃過程中較為認可的算法[3]。文獻[4]結合粒子群算法和A星算法，可以進行任意方向的搜索，但該方法計算量較大，在復雜的環境中，計算時間增大、時效性差。文獻[5]采用蟻群算法，合理利用了信息素啟發因子和期望啟發因子，進而改善了揮發系數；機器人在路徑規劃過程中，使用了更加合理的動態避障策略，但是采用該算法獲得的路徑較為曲折，不易滿足車輛運動學約束。文獻[6]采用了一種改進型快速隨機搜索樹路徑規劃算法，該算法將車輛的非完整約束與雙向RRT結合，同時采用B樣條基本函數作為參考點，對規劃路徑進行逼近，生成一條適合跟蹤控制的平滑路徑；該方法在一定程度上提高了搜索效率，但是未考慮與障礙物之間的距離，若直接對該路徑進行跟蹤，容易發生碰撞。文獻[7]采用分段式 Bezier 曲線生成規劃路徑，該方法根據傳感器提供的起點和障礙物位置信息，計算得到Bezier曲線的控制點，但該方法對曲線具有形狀限制，所以不能保證路徑的可行性。

人工勢場法(artificial potential field，APF)是Khatib[8]提出的一種路徑規劃方法，其原理是將智能車在行駛環境中的運動轉化為車輛在抽象勢場中的運動，使智能車在復合勢場合力的作用下，行駛到目標點，并規劃出最佳的避障路徑。該算法適用于信息條件未知的環境，其計算簡單、規劃的路徑平滑,廣泛應用于智能車的實時避障。文獻[9]采用人工勢場法并改進斥力函數作用域，將車輛的避障過程劃分為多個時間片段，在每個時間段內對避障路徑進行優化，從而規劃出最優的避障路徑，避免了目標不可達問題。文獻[10]將人工勢場法中的引力勢場動態化，并且去掉斥力場，按照八個方位進行搜索，尋找引力最大方向；若該方向存在障礙物，則尋找次優方向，不斷地修正路徑，使機器人能成功躲避障礙物。文獻[11]用梯度下降法改進人工勢場，使合力函數為凸函數，從而找到產生局部最優現象的障礙物，并通過增設外部擾動方法使其逃離局部極小值點，使智能車順利到達目標位置。然而，智能車輛在實際運行中,遇到的障礙物往往是動態的,傳統的人工勢場法未考慮障礙物的運動信息，避障過程中可能出現智能車與障礙物發生碰撞的現象[12]。

針對人工勢場算法中的不足，本文通過引入智能車和目標點的相對距離因子，克服傳統人工勢場目標不可達的現象；通過局部極小值檢測，并增設虛擬子目標點，避免陷入局部極小值；通過在斥力勢場函數中引入道路邊界勢場，保證車輛在道路中間行駛；通過在斥力勢場函數中引入障礙物相對速度勢場，解決車輛在動態障礙物環境中容易發生碰撞的問題。

1 傳統人工勢場法

人工勢場法是廣泛應用于機器人、智能車領域中的一種路徑規劃算法，其原理是將智能車在行駛環境中的運動轉化為智能車在人為設定的抽象勢場中的運動，抽象勢場由引力、斥力兩大勢場組成。將引力勢場和斥力勢場進行疊加即為合力勢場，智能車在合力勢場的作用下行駛，行駛方向即為勢能下降的方向[13]。汽車在抽象勢場中的受力情況如圖1所示(箭頭代表受力方向)。

(a)障礙物斥力 (b)目標點引力圖1 人工勢場受力示意圖Fig.1 Force diagram of artificial potential field

引力勢場表現為目標點對智能車的吸引能力，主要由智能車與目標點間的距離決定，當智能車在目標點附近時，距離較小，引力勢能較弱；當智能車在起始點附近時，距離較大，引力勢能較強，以此吸引智能車向著目標位置運動[14]。目標點處的引力勢場其值最小，近似山底，起始點處的引力勢場其值最大，近似山頂。

引力勢場函數為

(1)

式中:katt表示引力勢場增益因子，為一正常數；ρ(q,qg)表示汽車與目標位置之間的歐氏距離|q-qg|。

引力函數Fatt(q)是引力勢場對智能車與目標點相對距離的導數的負值，其方向由智能車當前所處位置指向目標點位置。引力函數的表達式為

Fatt(q)=-?Uatt(q)=-kattρ(q,qg)=

-katt(q-qg)。

(2)

斥力勢場體現障礙物對智能車的排斥能力，主要受障礙物與智能車之間距離的影響，當智能車在障礙物附近時，距離較小，斥力勢能較大，以促使智能車避開障礙物；當智能車遠離障礙物時，距離較大，斥力勢能較小。當距離大于障礙物最大影響范圍時，斥力勢場將不再起任何作用[15]。每個障礙物的斥力勢場近似高峰，斥力勢場函數的表達式如下：

Urep(q)=

(3)

式中：krep表示斥力正比例增益因子；ρ(q,qo)表示汽車與障礙物間的歐氏距離|q-qo|；ρo為一常值，表示障礙物的斥力影響范圍，可以自行設置調整。

斥力函數Frep為斥力勢場函數Urep(q)對智能車與障礙物相對距離的導數的負值， 其表達式為

(4)

汽車在空間中的運動受到引力勢場和多個障礙物斥力勢場共同作用，結合人工勢場法的理論和勢場函數公式，可得到智能車受到的合力勢場為

(5)

式中n為障礙物的個數。

將斥力函數和引力函數進行累加，可以得到智能車的合力函數，即

(6)

智能車在合力場中的受力如圖2所示。障礙物1和障礙物2分別產生斥力Frep1和Frep2，目標點產生引力Fatt，合力Ftotal引導智能車避讓障礙物向目標位置運動。

圖2 智能車在合力場中的受力圖Fig.2 Force diagram of intelligent vehicle in resultant force field

利用MATLAB進行智能車的仿真模擬，驗證通過傳統人工勢場法進行避障路徑規劃的效果。在仿真設置中，紅色方框表示起點，位置設為(0,0)；綠色倒三角表示目標點，其位置為(10，10)；黑色正方形表示障礙物，位置依次為(1,1.3)、(3,2.6)、(3,6.1)、(4,4.6)、(5.5,5.9)、(6,2.1)、(8.1,7.9)；斥力勢場增益因子krep=15，斥力正比例增益因子katt=5，障礙物的最大影響半徑ρo=7，分別建立斥力勢場和引力勢場，運用傳統的人工勢場法進行路徑規劃，仿真結果如圖3所示。

圖3 傳統人工勢場法仿真結果Fig.3 Simulation results of traditional artificial potential field method

由圖3可以看出，采用傳統人工勢場法規劃最優路徑時，存在目標點不可達以及局部最優等問題，即行駛路徑最終停止在障礙物(8.1,7.9)前，未到達設定目標點(10，10)。為解決人工勢場法存在的目標點不可達以及局部最優等問題，并使其更好地應用于智能車運動規劃，本文對傳統人工勢場法進行改進。

2 傳統人工勢場法的改進

2.1 局部極小值問題的改進方法

在行駛過程中，可能會出現智能車、障礙物、目標點在同一直線上的情況，這種情況會使斥力和引力作用在一條直線上，大小相等、方向相反；還有一種情況，智能車受到兩個及以上障礙物的斥力作用，多個斥力的合力與引力大小相等、方向相反，達到合力為零的狀態，此時智能車無法繼續行駛。這兩種情況都會使智能車誤認為目前位置就是目標點，使車輛陷入局部極小值，導致避障失敗[16]，如圖4所示。

圖4 智能車陷入局部極小值分析示意圖Fig.4 Schematic diagram of intelligent vehicle falling into local minimum analysis

當智能車受到的斥力與引力大小相等、方向相反時，會產生局部極小值現象。將斥力方向旋轉一定的角度可以解決局部極小值問題[13]，但在未陷入局部極小值的情況下，旋轉斥力方向會產生一定的側向力，可能導致規劃出的路徑與最優路徑有一定的偏差；故本文提出檢測局部最小值，并增加虛擬目標點的方式，打破引力與斥力之間的平衡，從而避免產生局部極小值現象。其詳細步驟如下：

1)檢測智能車是否陷入局部極小值。其判斷條件為：引力和斥力差值為零并且夾角為180°。

2)設置虛擬目標點?？紤]到實際場景下常采用左側避障，故將實際目標點角度減去π/3，作為虛擬目標點的方向，同時以ρ0為步長來確定虛擬目標點(增設的虛擬目標點如圖5所示)位置，其公式為

(7)

式中：θatt為引力的角度;θg為引力旋轉后的角度；xg、yg為虛擬目標點的坐標;x0、y0為車輛當前位置。

3)當智能車逃離局部最優后，再切換為原來的目標點，保證車輛能順利到達目標位置。

圖5 增設虛擬目標點示意圖Fig.5 Schematic diagram of adding virtual target points

2.2 目標不可達問題的改進方法

如果障礙物在目標點附近，那么智能車在接近目標位置的過程中，引力的作用愈發減小，而斥力的作用愈發加大。車輛到達目標點時，其自身所受的引力作用較小，斥力作用較大，導致在目標位置合力不為零，斥力推動車輛繼續向前行駛，使得車輛無法到達目標點。之后，車輛會繼續駛出一定的距離，車身受到的引力作用增大，斥力作用減小，車輛會逐漸接近目標點。如此反復，智能車始終無法到達目標位置[17]，如圖6所示。

圖6 目標不可達分析示意圖Fig.6 Unreachable target analysis diagram

針對障礙物在目標點附近而使智能車無法到達目標點的現象，本文對斥力勢場函數進行改進，通過增加一個距離因子(q-qg)m來保證目標位置處的合力為零，新的斥力場函數為

(8)

對式(8)進行負梯度運算可得改進后的斥力公式為

Frep(q)=-?Urep(q)=

(9)

式中：

(10)

(11)

Frep1方向由障礙物位置指向車輛所處當前位置，Frep2的方向由汽車當前所處位置指向目標點位置。m是斥力修正因子(m>0)，m取值不同，智能車受到的斥力大小不同，以下分為3種情況進行討論。

(1) 0

(2)m=1時，

(3)m>1時，

智能車q趨向目標點qg時，前兩種情況的斥力Frep都大于0，很有可能出現目標不可達現象，因而不可取。第三種情況斥力的合力Frep為0，智能車在合力的作用下駛向目標點。

綜上所述，在m>1時，斥力場函數是可行的，因此本文在仿真時取m=2，確保智能車能夠順利到達目標位置。

為檢驗改進人工勢場算法的有效性，對優化后的人工勢場法進行仿真實驗。圖7為改進后的仿真結果，可以看出，改進后的規劃路徑能到達目標位置，說明該方法在解決局部最優和目標不可達問題上具有一定的效果。

圖7 改進型人工勢場算法仿真結果Fig.7 Simulation results of improved artificial potential field algorithm

3 速度斥力勢場和道路邊界斥力勢場的動態避障路徑規劃及仿真

真實的駕駛環境中，智能車無可避免地會在道路上受到人工車輛等障礙物的影響，這些障礙物以一定的速度進行運動[18]。圖8為使用改進后的人工勢場法在動態環境下的仿真過程，其中黑色為本車，紅色為障礙車。

(a)起始時刻

由圖8可以看出，采用人工勢場法的智能車在遇到動態障礙車輛時進行了避障，沒有在最佳時間完成避障動作，隨即發生了碰撞。這是由于動態障礙物是以一定的速度進行運動的，而傳統的人工勢場法忽視了障礙物的運動情況，特別是速度因素，因此避障過程中障礙物可能會運動到已經規劃好的路徑上，使智能車發生碰撞，導致車輛避障失敗。

為解決這一問題，本文在原斥力勢場中增加速度斥力勢場，速度斥力勢場函數為

(12)

對速度斥力勢場求導數的負值得到速度斥力函數，速度斥力函數的表達式為

(13)

在實際駕駛環境下，駕駛員會選擇道路邊界以內作為行駛路線。因此，本文利用道路邊界斥力勢場來規定智能車的行駛路線范圍，確定仿真的行駛區域。當智能車沿道路中間行駛時，車輛受到的道路邊界斥力較??；當智能車靠近道路邊緣時，車輛受到的道路邊界斥力較大。因此，使得智能車在沒有遇到障礙物時盡量沿著道路中心線行駛，避免了車輛越過道路邊界而引發的交通事故。道路邊界斥力示意圖如圖9所示。

圖9 道路邊界斥力示意圖Fig.9 Schematic diagram of road boundary repulsion

道路邊界斥力勢場為

(14)

道路邊界的斥力函數是對道路邊界斥力勢場求導數的負值，其公式為

(15)

對引力勢場、針對目標不可達而改進的斥力勢場，以及新增設的速度勢場和道路邊界勢場進行疊加，得到智能車的合力勢場，如圖10所示。

圖10 動態障礙物環境下智能車受力示意圖Fig.10 Force diagram of intelligent vehicle in dynamic obstacle environment

智能車的合力勢場為

(16)

智能車受到的合力為

(17)

為了驗證改進的人工勢場在動態障礙物環境中規劃路徑的可行性，在MATLAB中進行仿真分析。本文按照路徑規劃測試場景的要求，設置了一種動態超車場景，道路類型為一條單向行駛的平行雙車道，長度80 m，每個車道寬3.5 m，共設置2輛車，黑色車為本車，紅色車為以一定速度直線行駛的障礙車，其避障過程如圖11所示。圖11(f)中，紅色軌跡為智能車在人工勢場下最終規劃的路徑。

從圖11中可以看出，在超車場景下，采用改進的人工勢場法可以使智能車較好地實現動態超車路徑規劃，順利抵達目標位置，并且規劃的路徑較為平順、連續，能夠滿足后續橫向控制對規劃路徑的曲率要求。因此，將考慮障礙物運動信息和道路邊界信息的人工勢場法應用于動態避障路徑規劃領域具有一定的可行性。

4 結論

1)針對傳統人工勢場法可能陷入局部最優和目標不可達的現象，采用設置臨時目標點以及在斥力勢場函數中增加智能車與目標點間的距離因子這兩種方法，有效解決了以上問題。

(a)起始時刻

2) 針對改進后的人工勢場使智能車在動態障礙物環境下容易發生碰撞的問題，增加了速度勢場和道路邊界勢場。仿真結果表明，采用改進后的人工勢場能較好地完成智能車在行駛環境中的動態避障。