馬慶祿,黃光浩
(重慶交通大學(xué)交通運(yùn)輸學(xué)院,重慶400074)
近年來,自動駕駛技術(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)。
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 自動駕駛車輛在勢場中的受力示意圖
(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)
自動駕駛車輛在接近目標(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)
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)可知目標(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)前一樣與二者距離成正比。
對于目標(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)位置。
本文選取重慶交通大學(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ì)量。
針對傳統(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ì)量。