基于改進(jìn)人工勢場法的自動駕駛路徑規(guī)劃方法

馬慶祿，黃光浩

(重慶交通大學(xué)交通運(yùn)輸學(xué)院，重慶400074)

1 引言

近年來，自動駕駛技術(shù)的成熟使得無人車逐漸走入大眾的視野。想要保證其安全平穩(wěn)的運(yùn)行，精確的路徑規(guī)劃是必要的。移動機(jī)器人的路徑規(guī)劃是其重要的分支之一，而自動駕駛車輛是移動機(jī)器人的一種實(shí)體形式，因此許多學(xué)者將移動機(jī)器人路徑規(guī)劃的方法應(yīng)用于自動駕駛車輛的規(guī)劃之中。

應(yīng)用較多的算法主要有基于搜索的方法：A*算法[1]、基于采樣的方法：PRM概率圖算法和RRT快速搜索隨機(jī)樹[2-3]、基于離散優(yōu)化的方法：貝塞爾曲線和三次樣條插值曲線[4-5]、還有人工勢場法[6]、智能優(yōu)化算法如遺傳算法[7]等。其中人工勢場法是路徑規(guī)劃研究中比較成熟和實(shí)時性較好的規(guī)劃方法，其原理簡單、計算量不大、對于未知環(huán)境的應(yīng)對能力較好，規(guī)劃中實(shí)時考慮周圍障礙物的情況且便于控制。但也存在目標(biāo)不可達(dá)問題和容易陷入局部極小值的情況，這會導(dǎo)致復(fù)雜環(huán)境下路徑規(guī)劃失敗。

針對人工勢場法可能存在的問題，眾多學(xué)者對其進(jìn)行了改進(jìn)。Sato提出利用拉普拉斯方程求解勢能，改進(jìn)的勢場方程有效地消除了極小值點(diǎn)[8]。韓永等在原有的基于相對位置的勢場函數(shù)基礎(chǔ)上，通過構(gòu)造相對速度引力勢場和相對速度斥力勢場使機(jī)器人能適應(yīng)未知動態(tài)的環(huán)境[9]。Saeid Fazli等人采用“沿墻走”方法來解決障礙物附近目標(biāo)不可達(dá)問題以及復(fù)雜環(huán)境下路徑規(guī)劃問題[10]。李東方等通過調(diào)整引力增益系數(shù)、斥力增益系數(shù)、障礙物的影響范圍以及機(jī)器人的運(yùn)動頻率等參數(shù)以獲得最優(yōu)路徑[11]。任彥等提出當(dāng)機(jī)器人陷入局部極小值時對機(jī)器人施加附加虛擬控制力以跳出局部極小點(diǎn)[12]。石為人等提出通過建立基于聲吶傳感器的機(jī)器人模型，分別計算不同方向所受的斥力大小，當(dāng)機(jī)器人遇到極小值點(diǎn)時將該區(qū)域障礙物連接為整體快速遠(yuǎn)離該區(qū)域[13]。羅乾又等提出設(shè)置虛擬目標(biāo)點(diǎn)的方法，對機(jī)器人所受合力為0時通過實(shí)際目標(biāo)點(diǎn)與虛擬目標(biāo)點(diǎn)的共同作用下擺脫局部極小值[14]。修彩靖等在考慮道路邊界的引力約束以及車輛約束后，利用高斯組合隸屬函數(shù)來優(yōu)化引力函數(shù)，證明勢場法適用于在結(jié)構(gòu)化環(huán)境下運(yùn)行的無人車[15]。程志等通過引入機(jī)器人實(shí)時前進(jìn)向量來跳出局部極小值點(diǎn)，引入虛擬目標(biāo)點(diǎn)以逃離障礙物陷阱[16]。溫素芳等對于目標(biāo)點(diǎn)周圍存在障礙物提出縮小其影響范圍來減小斥力抵達(dá)目標(biāo)點(diǎn)，將障礙物的影響范圍分層討論以逃離局部極小值點(diǎn)[17]。翟紅生等利用量子粒子群算法優(yōu)化勢場函數(shù)中的增益系數(shù)，有效解決了復(fù)雜動態(tài)環(huán)境下隨機(jī)障礙物的避障問題[18]。梁獻(xiàn)霞等通過扇區(qū)劃分的方法構(gòu)造虛擬障礙物，使得局部極小值點(diǎn)的機(jī)器人在虛擬障礙物和目標(biāo)點(diǎn)以及真實(shí)障礙物三者力的作用下擺脫該區(qū)域[19]。魏立新等通過將模糊控制算法和改進(jìn)的人工勢場法結(jié)合，解決了U型陷阱和動態(tài)障礙物問題[20]。羅強(qiáng)等提出了做機(jī)器人對于障礙物勢場范圍的切線來確定虛擬的逃逸力，通過搜索法來判斷障礙物之間的可行區(qū)域來逃離極小值點(diǎn)[21]。王洪斌等將改進(jìn)的二次A*搜索算法與人工勢場法相結(jié)合，實(shí)現(xiàn)了機(jī)器人動態(tài)規(guī)劃路徑的長度與平穩(wěn)度較優(yōu)[22]。

本文介紹了傳統(tǒng)人工勢場法原理，分析在傳統(tǒng)人工勢場法進(jìn)行路徑規(guī)劃時出現(xiàn)目標(biāo)不可達(dá)和局部極小點(diǎn)情況的問題。在改進(jìn)的引力勢場函數(shù)上通過設(shè)定距離閾值來避免引力過大的導(dǎo)致不能避障問題，在改進(jìn)的斥力勢場函數(shù)上，通過引入斥力調(diào)節(jié)因子并將車輛與障礙物的距離分段討論，使得自動駕駛車輛能夠順利逃離局部極小點(diǎn)抵達(dá)目標(biāo)。

2 傳統(tǒng)人工勢場法原理

Khatib[6]于1986年提出人工勢場法，基本原理是將自動駕駛車輛運(yùn)行區(qū)域設(shè)置為由一個引力勢場和若干個斥力勢場組成的虛擬的力場環(huán)境。預(yù)抵達(dá)的目標(biāo)點(diǎn)周圍的引力勢場對車輛產(chǎn)生吸引力從而“拉動”車輛駛向目標(biāo)點(diǎn)，吸引力的大小與車輛至目標(biāo)點(diǎn)的距離成正比，即距離越遠(yuǎn)吸引力越大距離越近吸引力逐漸減小。行駛過程中遇到的人、其車輛等障礙物會對車輛產(chǎn)生排斥力從而“推動”車輛遠(yuǎn)離障礙物達(dá)到避障的效果，排斥力的大小與車輛至障礙物的距離成反比，即距離越遠(yuǎn)排斥力越小距離越近排斥力逐漸增大。自動駕駛車輛在目標(biāo)點(diǎn)與障礙物所產(chǎn)生引力場與斥力場的合力場下能夠有效地避開障礙物抵達(dá)目標(biāo)位置。自動駕駛車輛行駛過程中的受力示意圖如圖1所示。

圖1 自動駕駛車輛在勢場中的受力示意圖

2.1 引力場與斥力場函數(shù)

(1)

(2)

式中，S為自動駕駛車輛現(xiàn)在的位置(x，y)，Starget為預(yù)設(shè)目標(biāo)點(diǎn)的位置(xt，yt)，λatt為引力勢場的常數(shù)。λrep為斥力勢場常數(shù)，Sbarrier為區(qū)域內(nèi)障礙物位置(xb，yb)，障礙物對自動駕駛車輛影響的最大距離為d0。對引力勢場函數(shù)和斥力勢場函數(shù)分別求解負(fù)梯度，即引力與斥力對距離求導(dǎo)可得引力場函數(shù)和斥力場函數(shù)公式見式(3)和(4)

Fatt(S)=-?Uatt(S)=-λatt|S-Starg et|

(3)

Frep(S)

(4)

2.2 合力場函數(shù)

自動駕駛車輛在接近目標(biāo)點(diǎn)的過程中，目標(biāo)點(diǎn)只有一個但障礙物不止一個，若存在n個障礙物則車輛會受到n個斥力勢場疊加起來的作用?？梢娨χ挥袉蝹€目標(biāo)點(diǎn)產(chǎn)生，斥力則會由n個障礙物產(chǎn)生。整個行駛區(qū)域內(nèi)的總勢能為引力勢能與斥力勢能之和，車輛受到的合力為所受的引力和斥力之和，根據(jù)上述公式可求得總勢能函數(shù)與車輛所受合力為

(5)

2.3 人工勢場法存在的問題

1)目標(biāo)不可達(dá)問題。當(dāng)距目標(biāo)點(diǎn)很近的范圍內(nèi)存在障礙物時，此時由式(4)可知障礙物的斥力隨著距離的減小而變得很大，由式(3)可得目標(biāo)點(diǎn)的引力隨距離的減小變得很小，這就會造成自動駕駛車輛受到的勢場合力一直指向目標(biāo)方向的反方向，從而使得車輛停滯無法前進(jìn)到達(dá)指定的目標(biāo)點(diǎn)。

2)局部極小值問題。引力與斥力的合力決定自動駕駛車輛的移動方向，當(dāng)車輛在某一點(diǎn)所受到的合力為零或者合力方向與引力和斥力共線時，車輛將無法確定下一步的前進(jìn)方向，可能會出現(xiàn)停止或者徘徊的情況。下圖2中展示了兩種導(dǎo)致局部極小值問題的障礙物分布類型，分別是“U”型障礙物、“L”型障礙物與目標(biāo)點(diǎn)共線的情況。

圖2 自動駕駛車輛在勢場中陷入局部最小值的幾種情況

3 改進(jìn)的勢場法

3.1 引力場函數(shù)的改進(jìn)

由式(3)可知目標(biāo)點(diǎn)的引力是和車輛與目標(biāo)點(diǎn)的距離成正比，也就是說當(dāng)二者距離較遠(yuǎn)時，此時引力相當(dāng)大，周圍障礙物的斥力相比引力而言會較小，這可能導(dǎo)致車輛受到很強(qiáng)的引力來不及躲避障礙物與其相撞。對于引力過大的問題，對引力場和引力函數(shù)進(jìn)行改進(jìn)，公式為

(6)

(7)

式中α為人為設(shè)定的距離閾值，也可以理解為目標(biāo)點(diǎn)對車輛的影響距離。當(dāng)車輛位置與目標(biāo)點(diǎn)位置距離大于設(shè)定的距離閾值α?xí)r，引力則為常數(shù)。當(dāng)車輛位置與目標(biāo)點(diǎn)位置距離小于設(shè)定的距離閾值α?xí)r，可認(rèn)為車輛行駛到目標(biāo)點(diǎn)影響距離以內(nèi)，此時引力同未改進(jìn)前一樣與二者距離成正比。

3.2 斥力場函數(shù)的改進(jìn)

對于目標(biāo)不可達(dá)問題和局部極小值問題，需要對傳統(tǒng)人工勢場法中的斥力勢場函數(shù)進(jìn)行改進(jìn)，保證車輛在遇到上述幾種局部極小值的情況下能夠抵達(dá)并穩(wěn)定在目標(biāo)點(diǎn)，即整個勢場中目標(biāo)點(diǎn)是全局最小值。常用的方法是將車輛與目標(biāo)點(diǎn)的相對距離的n次方引入到原有的斥力勢場函數(shù)中，即(S-Starget)n為斥力函數(shù)調(diào)節(jié)因子。對于障礙物的影響距離進(jìn)行分段，在不同的距離閾值采用不同的斥力，使得目標(biāo)點(diǎn)位置的斥力為零，車輛穩(wěn)定于目標(biāo)點(diǎn)。

斥力增益系數(shù)λrep為正數(shù)，障礙物對車輛的影響距離d0，車輛與障礙物距離大于d0時不受此障礙物的影響，這里在S0與障礙物邊界之間增加一個距離閾值d0/2。

當(dāng)S-Sbarrier∈(d0，+∞)時，車輛處于安全區(qū)域不會與障礙物碰撞此時斥力勢場和斥力都為0，S-Sbarrier∈(d0/2，d0)時，車輛處于控制區(qū)域此時障礙物產(chǎn)生斥力作用于車輛且線性收斂，S-Sbarrier∈(0，d0/2)時，車輛處于危險區(qū)域，當(dāng)車輛進(jìn)入即將發(fā)生碰撞的危險區(qū)，所受的斥力要保證其盡快遠(yuǎn)離該區(qū)域。

(8)

對斥力勢場求負(fù)梯度，由式(8)計算得到斥力，其公式為

(9)

(10)

式中n可以取大于0的任意常數(shù)，F(xiàn)rep的兩個分力為Frep1和Frep2，二者都是是障礙物對車輛的斥力分力，F(xiàn)rep1受力方向從障礙物指向車輛，F(xiàn)rep2受力方向從車輛指向目標(biāo)點(diǎn)。斥力函數(shù)調(diào)節(jié)因子(S-Starget)n中n的取值影響到自動駕駛車輛的受力情況，故對其進(jìn)行討論以分析不同取值下Frep1和Frep2的大小。

1)n∈(0，1]

(11)

(12)

當(dāng)自動駕駛車輛靠近目標(biāo)位置時，無論是S-Sbarrier∈(d0/2，d0)的控制區(qū)還是S-Sbarrier∈(0，d0/2)的危險區(qū)，此時障礙物對車輛的斥力Frep1都趨于0。目標(biāo)點(diǎn)對車輛的引力Frep2在S-Sbarrier∈(d0/2，d0)的控制區(qū)內(nèi)趨于無窮大，在S-Sbarrier∈(0，d0/2)的危險區(qū)內(nèi)趨于0。當(dāng)n在(0，1]內(nèi)，車輛處于控制區(qū)時在Frep2和Fatt的共同作用下靠近目標(biāo)位置，處于危險區(qū)時只在Fatt作用下靠近目標(biāo)位置。

2)n∈(1，+∞]

(13)

(14)

當(dāng)自動駕駛車輛靠近目標(biāo)位置時，無論是S-Sbarrier∈(d0/2，d0)的控制區(qū)還是S-Sbarrier∈(0，d0/2)的危險區(qū)，此時障礙物對車輛的斥力Frep1、Frep2都趨于0。此時車輛只在Fatt作用下靠近目標(biāo)位置。

4 Matlab仿真分析

本文選取重慶交通大學(xué)南岸校區(qū)內(nèi)障礙物環(huán)境為研究區(qū)域，起點(diǎn)為重慶交通大學(xué)1號門，目標(biāo)點(diǎn)為菁園小區(qū)，黑色方框內(nèi)為路徑規(guī)劃區(qū)域，將區(qū)域內(nèi)建筑物障礙物全部抽象為圓形障礙物，計算勢場力，試圖規(guī)劃出一條無碰撞的自動駕駛車輛路徑，研究區(qū)域如圖3。

圖3 重慶交通大學(xué)南岸校區(qū)1號門-菁園小區(qū)

利用 Matlab 2016a軟件針對傳統(tǒng)的人工勢場法的存在的問題進(jìn)行仿真，以下仿真結(jié)果是在障礙物均為靜態(tài)的前提下進(jìn)行的，步驟如下：

1)仿真環(huán)境設(shè)置為x軸為8、y軸為9的二維區(qū)域，起始位置設(shè)為(0，0)，目標(biāo)位置為(7.2，8)。引力勢場增益常數(shù)λatt=1000斥力勢場增益常數(shù)λrep=15。自動駕駛車輛的步長l=0.1，障礙物的影響距離p0=1。

2)計算的車輛和各個建筑物障礙，和目標(biāo)點(diǎn)之間的與X軸之間的夾角(統(tǒng)一規(guī)定角度為逆時針方向)

3)根據(jù)車輛當(dāng)前位置與目標(biāo)和障礙物的角度，分別計算引力和斥力在x和y軸方向的分力。

4)根據(jù)引力與斥力在x軸方向的分力的合力，在y軸方向的分力的合力，確定車輛下一步的x坐標(biāo)和y坐標(biāo)，更新位置坐標(biāo)、與障礙物和目標(biāo)點(diǎn)的角度，記錄車輛走過的點(diǎn)。

5)判斷是否車輛達(dá)到目標(biāo)點(diǎn)，若抵達(dá)則步驟結(jié)束，否則將重新轉(zhuǎn)入步驟3)。

傳統(tǒng)勢場法下路徑規(guī)劃在不遇到復(fù)雜障礙物的前提下可以順利達(dá)到目標(biāo)點(diǎn)，本文實(shí)驗(yàn)則遇到了局部極小值致車輛無法抵達(dá)目標(biāo)點(diǎn)的情況，見圖4(a)，對勢場函數(shù)進(jìn)行改進(jìn)后見圖4(b)。

圖4 傳統(tǒng)勢場法與改進(jìn)勢場法下自動駕駛車輛路徑規(guī)劃圖

圖4中黑色實(shí)心圓代表影響路徑規(guī)劃的主要障礙物，淡藍(lán)色實(shí)心圓代表對路徑規(guī)劃無影響的次要障礙物。紅色方形代表起始點(diǎn)，藍(lán)色三角形代表目標(biāo)點(diǎn)，紅色實(shí)線代表傳統(tǒng)勢場下車輛的運(yùn)動軌跡，藍(lán)色實(shí)線代表改進(jìn)后車輛的運(yùn)動軌跡，傳統(tǒng)人工勢場法下，自動駕駛車輛沒有成功繞開障礙物，陷入了局部極小值最終沒能抵達(dá)目標(biāo)點(diǎn)處。改進(jìn)的人工勢場下，車輛成功繞過障礙物避免了陷入局部極小值到達(dá)目標(biāo)點(diǎn)位置。

對傳統(tǒng)勢場函數(shù)與改進(jìn)后勢場函數(shù)分別求路徑過程中總勢場值見圖5。

圖5 傳統(tǒng)勢場法與改進(jìn)勢場法勢場曲線圖

圖5為與兩種方法所對應(yīng)的勢場曲線圖，圖5(a)對應(yīng)傳統(tǒng)勢場法，可以看到，當(dāng)車輛運(yùn)行到第81步的時候，總勢場陷入了局部極小值，因此車輛在此處停滯不前；圖6(b)對應(yīng)改進(jìn)人工勢場法，可以看到運(yùn)行到第141步時，雖出現(xiàn)了局部震蕩但總勢場沒有出現(xiàn)局部極小點(diǎn)，因此最終可以到達(dá)目標(biāo)點(diǎn)。

自動駕駛車輛路徑規(guī)劃目的是為了規(guī)劃出安全無碰撞且相對平滑的路徑，因此這里引入路徑規(guī)劃的評價模型對傳統(tǒng)勢場法與改進(jìn)后勢場法所規(guī)劃出的路徑進(jìn)行量化評價[23]。

(15)

式中，V為評價值，V越大，表明規(guī)劃路徑的質(zhì)量越高；W1、W2、W3為權(quán)重系數(shù)，且三者和為1；C、T、L、X分別為碰撞系數(shù)、轉(zhuǎn)點(diǎn)數(shù)量、直線行駛長度以及尋路時間，這些參數(shù)的含義如下。

對于車輛規(guī)劃而言，基本的要求就是不與障礙物發(fā)生碰撞，因此規(guī)定C=1為無碰撞，C=0時視為撞到障礙物路徑規(guī)劃失敗。路徑平滑度通常需要參考轉(zhuǎn)折點(diǎn)數(shù)，轉(zhuǎn)點(diǎn)數(shù)量越多說明路徑規(guī)劃的平滑度越差，反之則平滑度越好。路徑的長度可以確保的情況下，直線行駛長度越長，則說明規(guī)劃出的路徑無需車輛保持較高的靈敏性，反之則要求車輛保持較好的轉(zhuǎn)向性。路徑規(guī)劃時間的大小可以一定程度反映資源的消耗，路徑時間越短規(guī)劃的結(jié)果一般越優(yōu)。

應(yīng)用規(guī)劃路徑評價模型來評價傳統(tǒng)勢場法與改進(jìn)后勢場法規(guī)劃出的路徑，結(jié)果見表1。

表1 兩種勢場法評價對比

由表1，可以明顯的看出傳統(tǒng)與改進(jìn)后兩條規(guī)劃的路徑都是無碰撞的，改進(jìn)后直線行駛長度L明顯變長，說明規(guī)劃路徑中更多是直線規(guī)劃，降低了對車輛自身機(jī)動性的要求，轉(zhuǎn)折點(diǎn)數(shù)量進(jìn)一步減小說明路徑的平滑度提升，尋路時間相較于傳統(tǒng)勢場法較長是由于改進(jìn)后的路徑抵達(dá)了目標(biāo)點(diǎn)。最終路徑評價值V 顯示，改進(jìn)勢場函數(shù)的路徑較傳統(tǒng)勢場在評價值方面提高了64%，說明基于勢場函數(shù)的改進(jìn)模型大大提高了路徑規(guī)劃的質(zhì)量。

5 結(jié)論

針對傳統(tǒng)人工勢場法中當(dāng)車輛距離目標(biāo)點(diǎn)較遠(yuǎn)時引力過大斥力過小而無法避開障礙物的情況下，通過引入車輛與目標(biāo)位置之間的距離閾值α，大于閾值α?xí)r引力為常數(shù)，小于閾值α?xí)r引力與距離成正比以解決避障問題。對于傳統(tǒng)勢場法中的目標(biāo)不可達(dá)和局部極小值問題，本文對于斥力函數(shù)進(jìn)行了改進(jìn)，在改進(jìn)的斥力函數(shù)中加入斥力調(diào)節(jié)因子，并對障礙物的影響范圍分進(jìn)行段討論，通過設(shè)定障礙物分段影響距離點(diǎn)計算不同的斥力。最后以重慶交通大學(xué)南岸校區(qū)1號門為起點(diǎn)菁園小區(qū)為目標(biāo)點(diǎn)，所在區(qū)域進(jìn)行實(shí)地路徑規(guī)劃，利用Matlab2016a對自動駕駛車輛仿真的結(jié)果表明改進(jìn)后的勢場法有效，畫出了車輛規(guī)劃的路徑，并對傳統(tǒng)勢場法與改進(jìn)后勢場法進(jìn)行量化評價，結(jié)果表明改進(jìn)后勢場函數(shù)在評價值上提高了64%，明顯提高了路徑規(guī)劃的質(zhì)量。