融合風險勢場的離散優化局部路徑規劃方法研究

魏 凱,劉樹偉,李 剛

(遼寧工業大學 汽車與交通工程學院,遼寧 錦州 121001)

0 引言

隨著電動汽車逐漸向智能化領域發展,汽車的智能化已成為必然的發展趨勢[1]。當車輛在行駛過程中檢測到障礙物時,局部路徑規劃能夠實時規劃出一條安全可行的路徑,引導車輛完成對障礙物的規避,保證車輛的行駛安全,是智能汽車的關鍵技術之一。局部路徑規劃算法可分為四類:基于采樣的方法、基于搜索的方法、勢場法以及離散優化的方法?；诓蓸拥姆椒ㄊ窃跔顟B空間內隨機采樣節點,通過增加不同的運動約束最終生成規劃路徑[2]。該方法采樣隨機,收斂速度較慢,代表算法為快速隨機擴展樹法(rapidly random tree,RRT)與概率圖法(probabilistic road map,PRM)?；谒阉鞯姆椒ㄊ窃跂鸥竦貓D內遍歷節點間所有路徑,最終搜索出一條無碰撞的最短路徑[3]。該方法無法適應復雜地圖且計算量較大,實時性較差,代表算法為A?算法與Dijkstra 算法。以上兩種方法因其算法的局限性,并未考慮車輛的動力學特性且無法滿足實時性要求,更多應用于移動機器人規劃領域。勢場法基于勢場理論,引入引力場與斥力場的概念,根據綜合的場強分布完成規劃[4]。該方法能夠更加準確地反映規劃場景內障礙物的風險影響,彌補傳統算法中對于路徑安全性的分析。離散優化的方法通過自主設計代價函數評價候選路徑,能夠綜合考慮車輛動力學以及路徑的安全性、平滑性等指標,更加適用于智能汽車領域[5]。

針對勢場法以及離散優化方法的應用,國內外研究人員已經做了大量的研究工作。文獻[6]應用人工勢場法對自動駕駛汽車進行自適應運動規劃,利用虛擬勢場對車輛進行風險感知,適應不同障礙物的位置及速度變化,并通過硬件在環測試驗證了方法的可行性。文獻[7]提出建立基于道路縱向、橫向和時間3 個維度的預測風險場,并使用多項式曲線和二次規劃方法對軌跡進行優化。文獻[8]通過建立成本函數對候選路徑進行評價,并對評價結果進行加權處理,最終能夠在局部障礙圖中搜索出最優路徑,避免車輛與障礙物發生碰撞。文獻[9]提出一種綜合路徑采樣和離散優化的混合軌跡規劃方法,通過對側向運動的采樣生成多條軌跡,能夠適應不同速度的復雜動態環境,并通過實車測試驗證了方法的可行性。文獻[10]提出建立s-ρ坐標系生成候選路徑,能夠完成對復雜障礙物的規避,但是并沒有考慮動態障礙物的影響。文獻[11]基于碰撞風險評估對車輛進行局部路徑規劃,對不同障礙物建立風險場,通過對候選路徑進行風險評估,增強規劃路徑的安全性,但是并未考慮到道路本身對車輛的風險影響。

針對智能汽車在結構化道路內的緊急避障場景,本文提出一種融合風險勢場的離散優化局部路徑規劃方法?；趧輬隼碚摻⒌缆泛驼系K物的風險分布模型,利用風險勢場的場強分布描述規劃場景中的風險源;針對靜態和動態障礙物,結合風險勢場的場強與離散高斯方法設計路徑安全代價函數,評價候選路徑的安全性;綜合考慮路徑的安全性、偏移量和連續性,建立統一多目標代價函數對候選路徑進行評價。仿真結果表明,本文提出的局部路徑規劃方法能夠實時為車輛規劃出一條安全、可行的路徑,引導車輛完成對障礙物的規避,且針對多種復雜場景都具備良好的穩定性。

1 候選路徑

1.1 路徑參考線

將車輛路徑分割為多個路徑片段,每個路徑片段為一個局部路徑規劃周期。在一個路徑片段內有多個離散路徑點,將這些離散點擬合成路徑線,并以此參考線為基準進行局部路徑規劃算法的設計。三次樣條曲線計算簡單、實時性好,曲線穩定且收斂性有保證。因此,本文使用三次樣條曲線方程擬合路徑離散點,將路徑線作為局部路徑規劃的參考中心線,如圖1 所示?；￠L參數化后的三次樣條曲線方程為

圖1 路徑參考線Fig.1 Path guides

式中,a3x、a2x、a1x、a0x、a3y、a2y、a1y和a0y為曲線系數,si表示第i個路徑點的弧長值。根據弧長s,即可直接計算路徑參考線任意一點的坐標值(x0,y0),此時車輛的航向角和曲率為

式中,x0′、x0′′和y0′、y0′′分別為x0(s) 和y0(s) 對弧長s的一階導數和二階導數。

1.2 車輛定位

當車輛在彎道行駛時,路徑參考線為曲線,且車輛在行駛過程中軌跡跟蹤存在誤差,所以車輛的位置與路徑參考線之間一定存在一個橫向偏移量d0,在對車輛進行定位時這是不可忽略的。如果使用大地笛卡爾坐標系來計算車輛與路徑參考線之間的橫向距離,公式復雜且增加數據運算量。因此,本文引入局部Frenet 坐標系,用以描述當前路徑片段內車輛對于路徑參考線的相對位置關系,如圖2 所示。所建立的局部Frenet 坐標系以路徑參考線為基準,沿參考線的切線方向建立s軸,正方向為車輛當前的前進方向,數值表示路徑的長度;縱軸為參考線切線的法線方向,垂直于橫軸,即為d軸,數值表示車輛相對于參考線的橫向偏移量。通過局部Frenet 坐標系,可以將車輛每個時刻的位置信息分解為s軸方向和d軸方向的狀態,在進行局部路徑規劃時,減少處理車輛定位信息的工作量。在局部Frenet 坐標系內,使用二次最小化與牛頓法相結合的方法[12],可以準確計算得到車輛當前位置距離路徑基準線的橫向偏移量d0,根據坐標值(s0,d0) 即可清晰地表示出車輛的定位信息。

圖2 車輛定位Fig.2 Location of vehicle

1.3 邊界條件

在當前規劃周期內,利用局部Frenet 坐標系并基于路徑采樣的方法,可以生成一組有限的候選路徑簇。所有候選路徑的起點都相同,即為規劃周期開始時刻車輛的當前位置;路徑的終點位置各不相同,但都與路徑參考線的相切角相同。由于路徑參考線是由三次樣條曲線方程表示,為了保證路徑的連續性,避免路徑出現曲率突變,本文依然使用三次樣條曲線方程來表示候選路徑,則候選路徑簇可表示為

式中,s∈[sstart,send],sstart為候選路徑起點處所對應的弧長,send為候選路徑終點處所對應的弧長,a3、a2、a1為曲線系數。

建立候選路徑的邊界條件,其中路徑起點為車輛當前位置,終點處的send與起點處的sstart的差值Δs即為候選路徑投影到路徑參考線上的弧長;路徑終點處的橫向偏移量d取值范圍根據當前道路寬度設定,且保證每條路徑之間間距相等。所有路徑起點處的切線方向與當前車輛前進方向保持一致,終點處路徑的切線方向與參考線的切線方向保持一致。根據以上條件構建路徑邊界條件為

結合式(4)與式(5),將路徑終點的橫向采樣范圍等分,等間距地改變終點位置的橫向偏移量d,即可得到當前規劃周期內所有候選路徑的曲線方程。

對計算得到的候選路徑進行編號C1,C2,…,Cn,方便后續代價函數對候選路徑的評估與統計,如圖3 所示。此時規劃出來的候選路徑是基于局部Frenet 坐標系而產生的,在局部路徑規劃結束后,需將路徑信息從局部Frenet 坐標系再轉化至大地笛卡兒坐標系,傳輸至下層控制層進行軌跡跟蹤控制。根據坐標系之間的相對幾何關系,計算得到最終的規劃路徑信息。

圖3 候選路徑Fig.3 The candidate paths

2 風險勢場

為了能夠準確描述周圍環境對智能汽車局部路徑規劃的影響,本文使用風險勢場來描述車輛行駛環境內的風險源,用場強分布表示規劃場景內的風險[13]。所構建風險勢場由道路、靜態障礙物與動態障礙物組成,可表示為

式中,Erisk表示當前規劃周期內風險勢場的總場強,ER、ES和ED分別表示道路風險勢場、靜態障礙物風險勢場和動態障礙物風險勢場。將這三個風險勢場疊加,通過場強分布可以直觀地判斷出各個位置車輛的安全風險大小。

2.1 道路風險勢場

在構建道路風險勢場時,主要分析車道線、車道中心線以及道路邊界這三類關鍵位置。在車輛正常行駛過程中,道路兩側邊界位置的風險最高;車道中心線的風險相對較低,且當前車道中心線處的風險最低;而車道線可以約束車輛行駛于車道內,避免發生車道偏移,其風險應高于車道內部的風險,因此采用階躍函數構建道路的風險分布模型。以同向雙車道為例進行分析,將道路中線作為參考線建立局部Frenet 坐標系,在道路特征不變的前提下,其風險影響僅與橫向偏移量d有關,具體模型可表示為

式中:d1和d2分別為左、右兩車道中心線處的橫向偏移量;p為車道線處的風險值,取值與車道線類型有關,若為雙黃線,則車輛不可越線,取值為1,若為白色虛線,則車輛可以越線行駛,取值為0.3。

在構建道路風險勢場時,假設車輛越靠近道路兩側邊界位置其風險越高,且最大風險值為1;而車輛沿著當前車道的中心線行駛時風險最小,且最小風險值為0。在無障礙物的情況下,車輛應盡可能地沿著當前車道的中心線行駛,避免發生車道偏移;而在有障礙物的情況下,車輛應及時轉向,在規避障礙物之后回到原車道繼續行駛,同時在避障過程中避免與道路兩側邊界相撞。根據假設可知,車輛沿著車道中心線行駛時的風險小于車輛換道的風險,使得在無障礙物的情況下,車輛能夠保持在原車道行駛。根據式(7)所建立的風險分布模型,構建道路風險勢場并進行歸一化處理

式中,D為車道寬度,P(d) 為道路風險分布模型。此風險勢場是在局部Frenet 坐標系下構建,因此適用于任何形狀曲率的道路,且面對交叉路口等道路交匯處依然適用,可將兩條道路分別建模后將勢場進行疊加加以表示。

無障礙物情況下的道路風險勢場如圖4 所示,場景為同向雙車道,車道寬度取3.5 m,長度取40 m。由圖可以看出,風險勢場的場強分布僅與橫向偏移量有關。在道路兩側邊界位置取得最大值,數值為1;而在車道中心線位置取得最小值,數值為0,且車道線處風險高于車道內區域。

圖4 道路風險勢場示意圖Fig.4 Diagram of road risk potential field

2.2 障礙物風險勢場

2.2.1 靜態障礙物風險勢場

在結構化道路環境內,靜態障礙物主要為路面上靜止的車輛、臨時增加的路障等。面對靜態障礙物,車輛越靠近它,越容易與其發生碰撞,相對應的風險也就越高。由于靜態障礙物本身不具備速度,場強分布表現為各向同性,即無論從哪個方向靠近它,場強的分布都是均勻的。所分析的風險勢場是在局部Frenet 坐標系下進行的,設靜態障礙物中心點坐標為O(s0,d0) 且質量分布均勻,則它所形成的風險勢場在(s,d)處的場強可表示為

式中,k1為比例系數,r=(s-s0,d-d0) 表示與障礙物中心點的距離矢量。靜態障礙物風險勢場的場強變化趨勢為距離矢量的冪函數形式,同時在計算距離時增加σs與σd兩個風險分布因子,表示風險勢場在s軸和d軸方向上的分布范圍。

設靜態障礙物的中心點坐標為(0,0),其質量均勻分布,風險分布因子σs=σd=1,則它形成的風險勢場如圖5 所示。由圖可知,障礙物中心點處場強最大,場強方向由中心點均勻地向四周輻射,且沿場強方向,場強減小速度最快。隨著與障礙物中心點的距離增大,場強逐漸減小,表明障礙物所形成的風險在降低,直至距離足夠遠,減小至0。

圖5 靜態障礙物風險勢場示意圖Fig.5 Diagram of static obstacle risk potential field

2.2.2 動態障礙物風險勢場

將動態障礙物與靜態障礙物相比較,其共同點在于風險影響也具有方向性,而不同點在于,動態障礙物本身具備速度,不同的速度方向及大小所造成的風險是不同的,具有各向異性。針對動態障礙物的特點,在局部Frenet 坐標系下,將障礙物的速度v分解為s軸方向和d軸方向的速度分量,通過選取不同的分布因子σs和σd,可以準確表示不同速度方向所產生的風險影響。動態障礙物O(s0,d0) 所形成的風險勢場在(s,d)處的場強可表示為

式中,k2為比例系數,r=(s-s0,d-d0) 表示與障礙物中心點的距離矢量,θ為r與障礙物速度v之間的夾角,exp(vcosθ)描述場強隨速度大小、方向的變化趨勢。在相同速度大小下,越小,cosθ越大,場強也就越大,且當θ=0 時達到最大,而θ=π 時場強最小,障礙物所形成的風險也最低。在相同夾角下,cosθ> 0 時,速度越大風險也就越大,而cosθ< 0 時則相反。

假設動態障礙物質點坐標為(0,0),具有沿s軸正方向的速度v,則它所形成的風險勢場如圖6所示,其中分布因子σs=3,σd=1。由圖可知,沿s軸正方向的場強輻射范圍明顯大于沿s軸負方向。風險勢場在中心點處場強最大,且數值最大為1;由于障礙物具有s軸向的速度,沿速度方向形成的整體場強分布遠大于背離速度方向。

3 最優路徑

局部路徑規劃的目的是根據當前的環境信息,為車輛規劃出一條安全的、連續的、可行的路徑,能夠躲避道路上的障礙物,保障車輛的正常行駛。所以,本文基于離散優化的方法,設計了一種統一多目標代價函數用以評價候選路徑,綜合考慮路徑的安全性、偏移量以及連續性,分別建立相應的代價函數,對路徑評價結果進行加權處理,最終評價結果最低的路徑即為最優路徑。代價函數表達式為

式中,f(i) 表示第i條候選路徑的總代價函數,fs、fe、fc分別表示路徑安全代價函數、路徑偏移代價函數和路徑連續代價函數,代價函數的評價結果在[0,1]內,ws、we和wc為各代價函數的權重系數,ws+we+wc=1。

3.1 路徑安全代價函數

3.1.1 靜態障礙物安全代價函數

建立安全代價函數的主要目的為避免車輛與障礙物發生碰撞,保證車輛避障過程的安全性。傳統的碰撞檢測方法是將候選路徑上的所有車輛點與所有障礙物點進行遍歷檢測,發生碰撞則結果為1;若未發生碰撞則結果為0。該方法中較小的坐標計算誤差即可造成完全相反的檢測結果,誤差較大,無法完全體現避障過程中障礙物所形成的風險影響;規劃算法執行頻率較快,該方法計算量大且耗時較大,無法滿足復雜環境下的局部路徑規劃要求。本文結合障礙物風險勢場的場強分布,設計一種全新的路徑安全代價函數。對當前規劃周期內的道路和障礙物建立風險勢場,將路徑點位置的場強作為安全性評價指標,場強越大,則表明風險越大,路徑安全性越低。

在分析候選路徑的安全性時,必須考慮車輛的形狀。為減少計算量同時保證結果的精確,本文采用四圓分解策略,將車輛分解為4 個半徑為rvehicle的圓,使用這4 個包絡圓將車輛完全覆蓋,如圖7 所示。圖中虛線表示車輛的后軸,其中,候選路徑的參考點為后軸中心點。

圖7 四圓分解Fig.7 Four-circle decomposition

針對多車道、多障礙物的復雜路況,考慮到存在傳感器的檢測誤差以及車輛行駛的跟蹤誤差,需要對風險勢場結果做進一步處理。本文利用高斯卷積方法構建路徑安全代價函數,在對某一條候選路徑的安全性進行評價的同時,充分考慮相鄰路徑對其產生的影響[14]。路徑安全代價函數可表示為

式中,fs1(i) 為靜態障礙物安全代價函數,Erisk(Ci) 為第i條候選路徑在風險勢場中的場強結果,gs1(j) 為離散高斯卷積函數,σs1為標準差。當評價結果顯示相鄰的幾條候選路徑的風險都過高,則表示此局部區域內發生碰撞的風險較大,即使某一條候選路徑的結果較低,仍然認為其安全風險較高;同理,當某局部區域內相鄰幾條路徑的風險都較低,那么最優路徑很有可能就在此區域內產生。

路徑安全代價函數的分析如圖8 所示,圖8(a)為當前規劃場景,車輛在右側主車道行駛,前方遇到靜態障礙物,此時有候選路徑11 條,編號C1～C11。圖8(b)為候選路徑在風險勢場的場強結果,其中C1～C4在左側超車道內,C6～C11在原主車道內。路徑C7～C9與場強最大,結果為1,同時路徑C11在道路邊界處,結果同樣為1。由圖8(c)可以看出,由于C10周圍的路徑風險很高,在經過離散高斯處理之后其最終結果也為1,而非原車道上的路徑風險普遍較低,其中C2評價結果最低,安全性最高,為最優路徑。

圖8 路徑安全代價函數Fig.8 The cost function of path security

3.1.2 動態障礙物安全代價函數

當車輛面對動態障礙物時,自車與障礙物都處于運動狀態,此時分析候選路徑的安全性,不能僅僅依據當前時刻障礙物的運動狀態。應通過對動態障礙物的當前運動狀態和歷史移動軌跡分析其運動意圖,根據預測狀態進行路徑安全性的分析。利用車載傳感器獲得當前時刻動態障礙物的位置及速度信息,以及歷史移動軌跡,運用拉格朗日插值法構建動態障礙物的運動預測模型[15]。通過傳感器數據記錄動態障礙物的先后3 個位置為為(ti,xi,yi),(ti+1,xi+1,yi+1),(ti+2,xi+2,yi+2),將這三組數據帶入拉格朗日插值函數形成該障礙物的運動預測模型,根據此模型可以快速預測出此后任意時刻障礙物的運動位置(tn,xn,yn) 。根據預測位置以及道路環境構建當前規劃周期的風險勢場,結合勢場的場強分布與離散高斯卷積建立動態障礙物安全代價函數

式中,fs2(i) 為動態障礙物安全代價函數,Erisk(Ci) 為候選路徑在風險勢場中的場強分布結果,gs2(j) 為動態障礙物的離散高斯卷積函數,σs2為標準差。

將靜態障礙物安全代價函數與動態障礙物安全代價函數的評價結果進行加權求和,即得到路徑安全代價函數

式中,ws1和ws2為權重系數。當規劃周期內只有靜態障礙物時,ws1=1,ws2=0; 當只有動態障礙物時,ws1=0,ws2=1;而場景內同時存在靜態障礙物與動態障礙物時,ws1=ws2=0.5。

3.2 路徑偏移代價函數

當車輛完成對障礙物的規避之后,應盡快回到原車道上,跟蹤路徑參考線繼續行駛。在對候選路徑進行評價時,候選路徑相對于路徑參考線的偏移量也需作為重要的參考指標。建立路徑偏移代價函數

式中,Δs為候選路徑簇在路徑參考線上的投影弧長值,Δs=send-sstart,di為候選路徑的橫向偏移量,dmax為整個候選路徑簇的最大橫向偏移量。

3.3 路徑連續代價函數

在進行局部路徑規劃,特別是面對動態障礙物時,每一時刻障礙物的位置都會在發生變化,候選路徑也會隨之進行更新;同時自車也是實時前進的狀態,對于相鄰兩個規劃周期之間路徑的連續性,也成為評價候選路徑的關鍵指標。如果兩個相鄰規劃周期內路徑的偏差過大,會導致車輛在短時間內產生較大的轉角,增大車輛失穩的概率,車輛的舒適性與穩定性都無法得到保障。所以,為防止出現路徑突變的情況,保證規劃路徑平滑且連續,建立路徑連續代價函數

式中,di為當前規劃周期內候選路徑的橫向偏移量,dpre為上一個規劃周期內最優路徑的橫向偏移量,dmax為整個候選路徑簇的最大橫向偏移量。fc(i) 的值越小,表示路徑之間的連續性越好,整體規劃路徑更加平滑。

3.4 權重系數自適應調節

在離散優化局部路徑規劃方法中,各代價函數權重系數是綜合路徑安全性、連續性等性能指標的重要參數,不同的取值將直接影響最終最優路徑的選擇結果。路徑安全代價函數反映候選路徑的安全風險,保證路徑不發生碰撞,是局部路徑規劃的根本目標,在總代價函數中應占較大比重;路徑偏移代價函數能保證車輛以較短的路徑完成避障,并回到路徑參考線方向;路徑連續代價函數則保證路徑連續光滑,防止出現路徑突變。在規劃周期內,各代價函數之間也相互影響,擁有較低安全風險的路徑,其偏移量與連續性往往難以保證。本文采用自適應權重調節策略,將當前規劃周期內所有路徑的安全代價函數評價結果進行升序整理,分成兩半分別計算平均路徑安全代價函數評價結果將規劃周期內所有候選路徑的安全代價函數評價結果與相比較并進行分類。

①fs(i) ≤

當前路徑的安全風險低于較優群體的平均安全風險值,說明此路徑在候選路徑簇中安全性較好,發生碰撞的概率較低。此時應更多地考慮路徑的偏移量與連續性,ws應取值較小,為0.5;而we與wc應相對占比較大,各為0.25。

此時路徑處于整個候選路徑簇的一般位置,路徑安全性介于兩個均值之間,則權重系數應盡量平衡各個代價函數所占比重,ws取0.6,we與wc各取0.2。

③fs(i) ≥

路徑的安全代價函數評價結果高于較差路徑群體的平均值,說明此路徑安全性很不理想,此時應增大安全代價函數在總代價函數中的占比,更多的考慮路徑的安全性,ws取0.8,we與wc各取0.1。

采用這種自適應權重調節策略,使得權重系數不再固定不變,而是根據代價函數的評價結果進行自適應調節,增強代價函數的可行性,提高最優路徑的綜合性能。

4 仿真結果與分析

為驗證本文所提出的局部路徑規劃方法的可行性,應用軟件進行仿真驗證。利用MATLAB 軟件的Driving Scenario Designer 模塊構建仿真所需交通環境,所構建場景包含直道、彎道和多車道等多種常見道路環境,同時設置有多個不同狀態的靜態、動態障礙物,在軟件中對局部路徑規劃算法進行編程,驗證方法的可行性。

4.1 仿真參數設置

在使用Driving Scenario Designer 模塊構建仿真場景時,車輛為交通參與者,是具有特定長度、寬度和高度的長方體箱形對象,如圖9 所示。在仿真環境中,車輛長度設置為3 m,寬度設置為2 m,后軸比為0.25。本文仿真場景為結構化道路上的緊急避障場景,所以設置車輛最大速度為10 m/s,仿真采樣時間為0.01 s。

圖9 仿真車輛設置Fig.9 Simulated vehicle settings

4.2 仿真場景一

仿真場景一主要驗證算法在直線道路上面對靜態障礙物的連續避障規劃能力,如圖10 所示。圖10(a)表示初始狀態,其中車輛A 為控制車輛,即自車,起始坐標為(0,0);其他為靜態障礙車輛B、C、D,速度為0,靜止在道路內。場景內點劃線為道路中心線,即為規劃算法中的路徑參考線。圖10(b)表示車輛完整的規劃軌跡,由圖可以看出,車輛能夠完成對靜態障礙物的連續規避,同時能夠在避障之后迅速回到原路徑方向,沿著路徑參考線繼續行駛,符合局部路徑規劃的要求。

當t=5.89 s 時,車輛已經檢測到前方障礙車輛B 并開始進行局部路徑規劃,選取規劃片段進行分析,如圖11 所示。其中圖11(a)為此時刻規劃場景的局部放大圖,當車輛檢測到障礙車輛B時,會生成一組局部候選路徑C1,C2,…,C11;圖11(b)為候選路徑的代價函數評價結果,由圖可以看出,位于道路中心線左側的候選路徑安全代價函數結果為1,安全性較差。最終路徑C9的路徑安全代價函數評價結果最低,同時總評價結果也為最低,為此規劃片段內的最優路徑。

圖11 規劃片段分析Fig.11 Analysis of planning section

分析規劃路徑的曲率變化,如圖12 所示,其中虛線表示路徑參考線,即為無障礙物時的路徑,實線為規劃路徑。在整個規劃過程中,路徑曲率始終維持在-0.02 m-1與0.02 m-1之間,在對障礙物B、C、D 進行連續避障時達到曲率變化的最大值。路徑整體曲率連續且波動范圍較小,保證了車輛在避開障礙物的同時,有較好的穩定性。

圖12 路徑曲率Fig.12 Curvature of path

4.3 仿真場景二

場景二主要模擬行車過程中的換道超車場景,如圖13 所示,場景內有兩條車道,上方為超車道,下方為主車道。車輛A 為控制車輛,即為自車,行駛于主車道上,速度為10 m/s;車輛B 為障礙車輛,也行駛于主車道上,速度為5 m/s。場景內間虛線為車道線,點劃線為主車道中心線,即為規劃算法中的路徑參考線。由圖13 可以直觀地看出車輛A 在換道超車過程中的完整軌跡。

圖13 仿真場景示意圖Fig.13 Schematic diagram of simulation scene

當t=11.36 s 時,車輛已經檢測到動態障礙車輛B 并開始進行局部路徑規劃,選取避障規劃片段進行分析,如圖14 所示。圖14(a)為當前規劃場景的車輛追逐圖視角,圖14(b)表示各個候選路徑的代價函數評價結果,由圖中結果可以得出,原車道上的候選路徑整體評價結果較大,性能較差,而左側超車道上的路徑整體結果較好,其中路徑的路徑安全代價函數結果最小,同時總體評價結果也為最小,為最優路徑。

圖14 規劃片段分析Fig.14 Analysis of planning section

分析路徑的曲率變化,如圖15 所示,其中虛線表示路徑參考線,即為無障礙物時的路徑,實線為規劃路徑。由圖可以看出,路徑曲率最大為0.035 m-1,最小為-0.02 m-1,且在自車剛檢測到障礙車輛并開始進行局部規劃時達到最大曲率值,而在車輛進行變道超車過程中曲率一直穩定于 -0.02～0.02 m-1之間。在面對動態障礙物時,路徑曲率連續,并且在變道超車過程中曲率保持穩定,驗證了算法的可靠性。

圖15 路徑曲率Fig.15 Curvature of path

4.4 仿真場景三

搭建復雜仿真場景三,驗證算法在彎道下同時面對靜態和動態障礙物的規劃能力,如圖16 所示。道路是一條連續的“S”形彎道,一共有3 個車道,中間車道為主車道。車輛A 為控制車輛,即自車,行駛于中間車道,其他為障礙車輛。其中車輛C、D 靜止于中間車道,B 車在右側車道沿車道中心線行駛,E 車在中間車道行駛,但是速度低于自車。圖16(b)表示車輛完整的規劃軌跡。

圖16 仿真場景示意圖Fig.16 Schematic diagram of simulation scene

選取t=5.80 s 時的規劃片段分析,如圖17 所示。其中同時包含了靜態和動態障礙物,C 車靜止于中間車道內,而B 車行駛于右側車道。道路場景為三車道,因此為了能夠規劃出更好的路徑,擴大候選路徑的約束范圍,此時共有19 條候選路徑。由圖17(b)可以看出,位于右側車道的候選路徑受到動態障礙車輛B 的影響,整體評價結果較高,不僅安全性較差,而且路徑連續性的評價結果也較高;位于左側車道內的路徑整體評價結果較低,其中路徑C5的安全代價函數與連續代價函數結果最低,同時總體評價結果也為候選路徑組內最低,為最優路徑。

圖17 規劃片段分析Fig.17 Analysis of planning section

分析此場景下車輛的路徑曲率變化,如圖18所示,其中虛線表示路徑參考線,即為無障礙物時的路徑,實線為規劃路徑。由圖可知,曲率變化范圍為-0.04～0.08 m-1。在路徑長度為0～200 m時,車輛同時面對靜態、動態障礙車輛,曲率變化幅度為0.08 m-1;而在路徑長度為200～400 m 時,車輛同時面對兩個速度不同的動態障礙車輛,此時曲率變化幅度為0.06 m-1。在復雜的多車道彎道工況下,且同時面對多個靜態、動態障礙車輛時,局部環境內路徑曲率依然能夠保持穩定狀態,波動幅度較小,能夠實時為車輛規劃出路徑完成對靜態、動態障礙物的規避,驗證了算法的有效性。

圖18 路徑曲率Fig.18 Curvature of path

4.5 仿真場景四

仿真場景四主要模擬交叉路口場景,驗證算法在面對橫向移動障礙物時的規劃能力,如圖19所示。車輛A 為控制車輛,即自車,在地圖中由左向右沿道路中心線行駛;車輛B 為障礙車輛,在地圖中由下向上沿道路中心線行駛。圖19(b)表示車輛完整的規劃軌跡。

圖19 仿真場景示意圖Fig.19 Schematic diagram of simulation scene

自車與障礙物車輛各自沿著道路中心線行駛,當t=5.33 s 時,自車檢測到障礙物車輛B,并根據其運動狀態開始進行局部路徑規劃。選取此時刻的規劃片段進行分析,如圖20 所示。由圖20(b)可以看出,位于障礙車輛B 前進方向的候選路徑與位于后方的候選路徑在代價函數的評價結果上相差很大,前者安全性整體偏低,而后者明顯安全代價函數評價結果較小,同時連續性也更好。其中路徑的安全代價函數結果最低,同時連續性也較好,總體評價結果為所有候選路徑中最低,為最優路徑。

圖20 規劃片段分析Fig.20 Analysis of planning section

分析此場景下車輛的路徑曲率變化,如圖21所示,其中虛線表示路徑參考線,即為無障礙物時的路徑,實線為規劃路徑,曲率整體的變化范圍為-0.04～0.04 m-1。由于障礙車輛也始終處于運動狀態,且速度方向與自車速度方向垂直,所以局部路徑規劃的范圍長度相較換道超車場景更短,曲率變化幅度增大。但是考慮到規劃時間較超車場景縮短,并且曲率能夠快速恢復穩定,依然能夠在車輛安全避開橫向移動障礙車輛的同時,保持車輛行駛的穩定性。

圖21 路徑曲率Fig.21 Curvature of path

5 結論

本文以結構化道路下智能汽車的局部路徑規劃為研究對象,提出融合風險勢場的離散優化局部路徑規劃方法。應用離散優化構建代價函數對路徑的綜合性能進行評價,將路徑規劃轉化為優化問題;使用風險勢場描述場景對路徑的潛在風險,結合高斯卷積設計安全代價函數,增強路徑的安全性。仿真結果表明,所提出的局部路徑規劃方法能夠實時為車輛規劃出安全且連續的路徑,引導車輛完成對障礙物的規避,保證車輛的正常行駛,同時在面對多種復雜交通場景依然具有良好的可行性,方法魯棒性較好,為進一步優化智能汽車的局部路徑規劃研究提供了依據。