基于改進(jìn)人工勢(shì)場(chǎng)的智能車動(dòng)態(tài)避障算法

于慧，郭宗和，秦志昌

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

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

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

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

針對(duì)人工勢(shì)場(chǎng)算法中的不足，本文通過引入智能車和目標(biāo)點(diǎn)的相對(duì)距離因子，克服傳統(tǒng)人工勢(shì)場(chǎng)目標(biāo)不可達(dá)的現(xiàn)象；通過局部極小值檢測(cè)，并增設(shè)虛擬子目標(biāo)點(diǎn)，避免陷入局部極小值；通過在斥力勢(shì)場(chǎng)函數(shù)中引入道路邊界勢(shì)場(chǎng)，保證車輛在道路中間行駛；通過在斥力勢(shì)場(chǎng)函數(shù)中引入障礙物相對(duì)速度勢(shì)場(chǎng)，解決車輛在動(dòng)態(tài)障礙物環(huán)境中容易發(fā)生碰撞的問題。

1 傳統(tǒng)人工勢(shì)場(chǎng)法

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

(a)障礙物斥力 (b)目標(biāo)點(diǎn)引力圖1 人工勢(shì)場(chǎng)受力示意圖Fig.1 Force diagram of artificial potential field

引力勢(shì)場(chǎng)表現(xiàn)為目標(biāo)點(diǎn)對(duì)智能車的吸引能力，主要由智能車與目標(biāo)點(diǎn)間的距離決定，當(dāng)智能車在目標(biāo)點(diǎn)附近時(shí)，距離較小，引力勢(shì)能較弱；當(dāng)智能車在起始點(diǎn)附近時(shí)，距離較大，引力勢(shì)能較強(qiáng)，以此吸引智能車向著目標(biāo)位置運(yùn)動(dòng)[14]。目標(biāo)點(diǎn)處的引力勢(shì)場(chǎng)其值最小，近似山底，起始點(diǎn)處的引力勢(shì)場(chǎng)其值最大，近似山頂。

引力勢(shì)場(chǎng)函數(shù)為

(1)

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

引力函數(shù)Fatt(q)是引力勢(shì)場(chǎng)對(duì)智能車與目標(biāo)點(diǎn)相對(duì)距離的導(dǎo)數(shù)的負(fù)值，其方向由智能車當(dāng)前所處位置指向目標(biāo)點(diǎn)位置。引力函數(shù)的表達(dá)式為

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

-katt(q-qg)。

(2)

斥力勢(shì)場(chǎng)體現(xiàn)障礙物對(duì)智能車的排斥能力，主要受障礙物與智能車之間距離的影響，當(dāng)智能車在障礙物附近時(shí)，距離較小，斥力勢(shì)能較大，以促使智能車避開障礙物；當(dāng)智能車遠(yuǎn)離障礙物時(shí)，距離較大，斥力勢(shì)能較小。當(dāng)距離大于障礙物最大影響范圍時(shí)，斥力勢(shì)場(chǎng)將不再起任何作用[15]。每個(gè)障礙物的斥力勢(shì)場(chǎng)近似高峰，斥力勢(shì)場(chǎng)函數(shù)的表達(dá)式如下：

Urep(q)=

(3)

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

斥力函數(shù)Frep為斥力勢(shì)場(chǎng)函數(shù)Urep(q)對(duì)智能車與障礙物相對(duì)距離的導(dǎo)數(shù)的負(fù)值， 其表達(dá)式為

(4)

汽車在空間中的運(yùn)動(dòng)受到引力勢(shì)場(chǎng)和多個(gè)障礙物斥力勢(shì)場(chǎng)共同作用，結(jié)合人工勢(shì)場(chǎng)法的理論和勢(shì)場(chǎng)函數(shù)公式，可得到智能車受到的合力勢(shì)場(chǎng)為

(5)

式中n為障礙物的個(gè)數(shù)。

將斥力函數(shù)和引力函數(shù)進(jìn)行累加，可以得到智能車的合力函數(shù)，即

(6)

智能車在合力場(chǎng)中的受力如圖2所示。障礙物1和障礙物2分別產(chǎn)生斥力Frep1和Frep2，目標(biāo)點(diǎn)產(chǎn)生引力Fatt，合力Ftotal引導(dǎo)智能車避讓障礙物向目標(biāo)位置運(yùn)動(dòng)。

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

利用MATLAB進(jìn)行智能車的仿真模擬，驗(yàn)證通過傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行避障路徑規(guī)劃的效果。在仿真設(shè)置中，紅色方框表示起點(diǎn)，位置設(shè)為(0,0)；綠色倒三角表示目標(biāo)點(diǎn)，其位置為(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)；斥力勢(shì)場(chǎng)增益因子krep=15，斥力正比例增益因子katt=5，障礙物的最大影響半徑ρo=7，分別建立斥力勢(shì)場(chǎng)和引力勢(shì)場(chǎng)，運(yùn)用傳統(tǒng)的人工勢(shì)場(chǎng)法進(jìn)行路徑規(guī)劃，仿真結(jié)果如圖3所示。

圖3 傳統(tǒng)人工勢(shì)場(chǎng)法仿真結(jié)果Fig.3 Simulation results of traditional artificial potential field method

由圖3可以看出，采用傳統(tǒng)人工勢(shì)場(chǎng)法規(guī)劃最優(yōu)路徑時(shí)，存在目標(biāo)點(diǎn)不可達(dá)以及局部最優(yōu)等問題，即行駛路徑最終停止在障礙物(8.1,7.9)前，未到達(dá)設(shè)定目標(biāo)點(diǎn)(10，10)。為解決人工勢(shì)場(chǎng)法存在的目標(biāo)點(diǎn)不可達(dá)以及局部最優(yōu)等問題，并使其更好地應(yīng)用于智能車運(yùn)動(dòng)規(guī)劃，本文對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行改進(jìn)。

2 傳統(tǒng)人工勢(shì)場(chǎng)法的改進(jìn)

2.1 局部極小值問題的改進(jìn)方法

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

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

當(dāng)智能車受到的斥力與引力大小相等、方向相反時(shí)，會(huì)產(chǎn)生局部極小值現(xiàn)象。將斥力方向旋轉(zhuǎn)一定的角度可以解決局部極小值問題[13]，但在未陷入局部極小值的情況下，旋轉(zhuǎn)斥力方向會(huì)產(chǎn)生一定的側(cè)向力，可能導(dǎo)致規(guī)劃出的路徑與最優(yōu)路徑有一定的偏差；故本文提出檢測(cè)局部最小值，并增加虛擬目標(biāo)點(diǎn)的方式，打破引力與斥力之間的平衡，從而避免產(chǎn)生局部極小值現(xiàn)象。其詳細(xì)步驟如下：

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

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

(7)

式中：θatt為引力的角度;θg為引力旋轉(zhuǎn)后的角度；xg、yg為虛擬目標(biāo)點(diǎn)的坐標(biāo);x0、y0為車輛當(dāng)前位置。

3)當(dāng)智能車逃離局部最優(yōu)后，再切換為原來的目標(biāo)點(diǎn)，保證車輛能順利到達(dá)目標(biāo)位置。

圖5 增設(shè)虛擬目標(biāo)點(diǎn)示意圖Fig.5 Schematic diagram of adding virtual target points

2.2 目標(biāo)不可達(dá)問題的改進(jìn)方法

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

圖6 目標(biāo)不可達(dá)分析示意圖Fig.6 Unreachable target analysis diagram

針對(duì)障礙物在目標(biāo)點(diǎn)附近而使智能車無法到達(dá)目標(biāo)點(diǎn)的現(xiàn)象，本文對(duì)斥力勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn)，通過增加一個(gè)距離因子(q-qg)m來保證目標(biāo)位置處的合力為零，新的斥力場(chǎng)函數(shù)為

(8)

對(duì)式(8)進(jìn)行負(fù)梯度運(yùn)算可得改進(jìn)后的斥力公式為

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

(9)

式中：

(10)

(11)

Frep1方向由障礙物位置指向車輛所處當(dāng)前位置，F(xiàn)rep2的方向由汽車當(dāng)前所處位置指向目標(biāo)點(diǎn)位置。m是斥力修正因子(m>0)，m取值不同，智能車受到的斥力大小不同，以下分為3種情況進(jìn)行討論。

(1) 0

(2)m=1時(shí)，

(3)m>1時(shí)，

智能車q趨向目標(biāo)點(diǎn)qg時(shí)，前兩種情況的斥力Frep都大于0，很有可能出現(xiàn)目標(biāo)不可達(dá)現(xiàn)象，因而不可取。第三種情況斥力的合力Frep為0，智能車在合力的作用下駛向目標(biāo)點(diǎn)。

綜上所述，在m>1時(shí)，斥力場(chǎng)函數(shù)是可行的，因此本文在仿真時(shí)取m=2，確保智能車能夠順利到達(dá)目標(biāo)位置。

為檢驗(yàn)改進(jìn)人工勢(shì)場(chǎng)算法的有效性，對(duì)優(yōu)化后的人工勢(shì)場(chǎng)法進(jìn)行仿真實(shí)驗(yàn)。圖7為改進(jìn)后的仿真結(jié)果，可以看出，改進(jìn)后的規(guī)劃路徑能到達(dá)目標(biāo)位置，說明該方法在解決局部最優(yōu)和目標(biāo)不可達(dá)問題上具有一定的效果。

圖7 改進(jìn)型人工勢(shì)場(chǎng)算法仿真結(jié)果Fig.7 Simulation results of improved artificial potential field algorithm

3 速度斥力勢(shì)場(chǎng)和道路邊界斥力勢(shì)場(chǎng)的動(dòng)態(tài)避障路徑規(guī)劃及仿真

真實(shí)的駕駛環(huán)境中，智能車無可避免地會(huì)在道路上受到人工車輛等障礙物的影響，這些障礙物以一定的速度進(jìn)行運(yùn)動(dòng)[18]。圖8為使用改進(jìn)后的人工勢(shì)場(chǎng)法在動(dòng)態(tài)環(huán)境下的仿真過程，其中黑色為本車，紅色為障礙車。

(a)起始時(shí)刻

由圖8可以看出，采用人工勢(shì)場(chǎng)法的智能車在遇到動(dòng)態(tài)障礙車輛時(shí)進(jìn)行了避障，沒有在最佳時(shí)間完成避障動(dòng)作，隨即發(fā)生了碰撞。這是由于動(dòng)態(tài)障礙物是以一定的速度進(jìn)行運(yùn)動(dòng)的，而傳統(tǒng)的人工勢(shì)場(chǎng)法忽視了障礙物的運(yùn)動(dòng)情況，特別是速度因素，因此避障過程中障礙物可能會(huì)運(yùn)動(dòng)到已經(jīng)規(guī)劃好的路徑上，使智能車發(fā)生碰撞，導(dǎo)致車輛避障失敗。

為解決這一問題，本文在原斥力勢(shì)場(chǎng)中增加速度斥力勢(shì)場(chǎng)，速度斥力勢(shì)場(chǎng)函數(shù)為

(12)

對(duì)速度斥力勢(shì)場(chǎng)求導(dǎo)數(shù)的負(fù)值得到速度斥力函數(shù)，速度斥力函數(shù)的表達(dá)式為

(13)

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

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

道路邊界斥力勢(shì)場(chǎng)為

(14)

道路邊界的斥力函數(shù)是對(duì)道路邊界斥力勢(shì)場(chǎng)求導(dǎo)數(shù)的負(fù)值，其公式為

(15)

對(duì)引力勢(shì)場(chǎng)、針對(duì)目標(biāo)不可達(dá)而改進(jìn)的斥力勢(shì)場(chǎng)，以及新增設(shè)的速度勢(shì)場(chǎng)和道路邊界勢(shì)場(chǎng)進(jìn)行疊加，得到智能車的合力勢(shì)場(chǎng)，如圖10所示。

圖10 動(dòng)態(tài)障礙物環(huán)境下智能車受力示意圖Fig.10 Force diagram of intelligent vehicle in dynamic obstacle environment

智能車的合力勢(shì)場(chǎng)為

(16)

智能車受到的合力為

(17)

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

從圖11中可以看出，在超車場(chǎng)景下，采用改進(jìn)的人工勢(shì)場(chǎng)法可以使智能車較好地實(shí)現(xiàn)動(dòng)態(tài)超車路徑規(guī)劃，順利抵達(dá)目標(biāo)位置，并且規(guī)劃的路徑較為平順、連續(xù)，能夠滿足后續(xù)橫向控制對(duì)規(guī)劃路徑的曲率要求。因此，將考慮障礙物運(yùn)動(dòng)信息和道路邊界信息的人工勢(shì)場(chǎng)法應(yīng)用于動(dòng)態(tài)避障路徑規(guī)劃領(lǐng)域具有一定的可行性。

4 結(jié)論

1)針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法可能陷入局部最優(yōu)和目標(biāo)不可達(dá)的現(xiàn)象，采用設(shè)置臨時(shí)目標(biāo)點(diǎn)以及在斥力勢(shì)場(chǎng)函數(shù)中增加智能車與目標(biāo)點(diǎn)間的距離因子這兩種方法，有效解決了以上問題。

(a)起始時(shí)刻

2) 針對(duì)改進(jìn)后的人工勢(shì)場(chǎng)使智能車在動(dòng)態(tài)障礙物環(huán)境下容易發(fā)生碰撞的問題，增加了速度勢(shì)場(chǎng)和道路邊界勢(shì)場(chǎng)。仿真結(jié)果表明，采用改進(jìn)后的人工勢(shì)場(chǎng)能較好地完成智能車在行駛環(huán)境中的動(dòng)態(tài)避障。