智能汽車避障風(fēng)險評估及軌跡規(guī)劃＊

王斌

（無錫商業(yè)職業(yè)技術(shù)學(xué)院，無錫 214153）

1 前言

交通事故的原因是多方面的，其中違反交通規(guī)則導(dǎo)致的交通事故占主導(dǎo)地位[1]。無人駕駛汽車嚴(yán)格遵守交通規(guī)則，反應(yīng)時間極短，可根據(jù)路況規(guī)劃最優(yōu)路徑，為減少甚至避免交通事故提供了新思路。

無人駕駛汽車的避撞系統(tǒng)由環(huán)境感知、軌跡預(yù)測、風(fēng)險評估、軌跡規(guī)劃、軌跡跟隨等過程構(gòu)成，其核心部分是危險評估和軌跡規(guī)劃。風(fēng)險評估指根據(jù)交通車（本文中指道路上其他車輛）駕駛意圖和本車狀態(tài)對碰撞危險進行評估，決定本車避撞行為（轉(zhuǎn)向、制動、保持狀態(tài)等），根據(jù)其原理，風(fēng)險評估可分為基于車輛狀態(tài)的風(fēng)險評估[2]、基于駕駛意圖的風(fēng)險評估[3]、基于交互的風(fēng)險評估[4]等。軌跡規(guī)劃指根據(jù)交通車等障礙物的分布情況和狀態(tài)，為本車規(guī)劃出符合動力約束的避撞軌跡，根據(jù)原理不同，軌跡規(guī)劃可以分為基于特定函數(shù)的軌跡規(guī)劃[5]、基于搜索的軌跡規(guī)劃[6]、基于優(yōu)化的軌跡規(guī)劃。

本文將汽車的確定性軌跡和概率性估計相結(jié)合，在確保風(fēng)險評估真實性的同時提高了風(fēng)險評估速度，提出了改進的快速搜索隨機樹算法，能夠規(guī)劃出平滑的安全避撞軌跡。

2 碰撞風(fēng)險評估

碰撞風(fēng)險評估主要包括駕駛員意圖識別、汽車軌跡預(yù)測和碰撞概率檢測。駕駛員意圖識別內(nèi)容復(fù)雜，不是本文研究的重點，因此在本文中假設(shè)駕駛員意圖已知。

2.1 汽車軌跡預(yù)測

基于汽車狀態(tài)的軌跡預(yù)測方法在短時間內(nèi)精度很高，但是預(yù)測時間長的情況下誤差很大甚至完全錯誤；基于駕駛員意圖的軌跡預(yù)測方法在長時間內(nèi)精度較高，短時間預(yù)測敏感度不高。因此本文將兩種方法進行融合。

2.1.1 基于汽車狀態(tài)的軌跡預(yù)測

基于汽車狀態(tài)的軌跡預(yù)測包括運動學(xué)和動力學(xué)兩種，本文使用運動學(xué)模型。常用的運動學(xué)模型包括恒速模型、恒加速模型、恒角加速度模型等。對于本車，本文選用恒角加速度模型；對于交通車，使用本車傳感器能夠測得交通車的位置、角度和速度，因此交通車選用恒加速模型。使用本車的車載傳感器測量本車和交通車的運動狀態(tài)，即可基于汽車狀態(tài)對車輛軌跡進行預(yù)測。

2.1.2 基于駕駛員意圖的軌跡預(yù)測

駕駛員的駕駛意圖可以分為車道保持和換道兩種，兩種駕駛意圖的軌跡預(yù)測原理相同，但是換道時的軌跡預(yù)測相對復(fù)雜。由文獻(xiàn)[7]可知，五次多項式可以很好地擬合汽車的換道曲線，首先建立求解換道曲線的坐標(biāo)系：以本車所在位置為坐標(biāo)原點，以本車當(dāng)前速度方向為X軸正方向，如圖1所示。

圖1 汽車換道軌跡

記目標(biāo)車道曲線為y=ax2+bx+c，換道軌跡曲線為y=a5x5+a4x4+a3x3+a2x2+a1x+a0。記當(dāng)前汽車縱向速度為vx，橫擺角速度為w，換道曲線與目標(biāo)車道線相切點為(xD,yD)，則根據(jù)初始點和目標(biāo)點的約束條件，解得。其中，式中，xD取不同值時預(yù)測結(jié)果也不同，如圖2所示。

圖2給出的汽車換道軌跡中，曲率過大時不滿足汽車動力學(xué)約束，曲率過小時換道持續(xù)時間過長，與實際情況不符。考慮汽車的動力學(xué)約束、換道時間和最小轉(zhuǎn)彎半徑約束就可以去除所有可能換道軌跡中與實際不符、無法滿足要求的軌跡，對剩余軌跡，建立指標(biāo)函數(shù)進行評價：

圖2 可能的汽車換道軌跡

式中，aytra為軌跡tra的側(cè)向加速度；aym為所有滿足約束條件軌跡中最大的側(cè)向加速度；ltra為軌跡tra的長度；lm為所有滿足約束條件軌跡的最大長度；Δtra為汽車已行駛軌跡與換道軌跡的偏差；Δm為最大偏差；w1、w2、w3分別為3個因素的權(quán)重系數(shù)。

根據(jù)式（1）在權(quán)重相等條件下選擇的最優(yōu)路徑見圖2。

當(dāng)本車行駛區(qū)域發(fā)現(xiàn)障礙物時，首先判斷當(dāng)前行駛狀態(tài)是否安全，若安全則繼續(xù)行駛，否則進行換道和制動的評估：優(yōu)先選擇制動，若制動無法滿足安全要求，則搜索安全換道軌跡或低風(fēng)險換道軌跡進行換道，最優(yōu)軌跡即為前文介紹的軌跡；若不存在安全換道軌跡，則考慮換道同時制動，此時最優(yōu)軌跡仍為前文預(yù)測的軌跡；若換道同時制動依然危險，則選擇緊急制動，此時為車道保持情況。

2.1.3 基于駕駛員意圖的軌跡預(yù)測

基于汽車狀態(tài)的軌跡預(yù)測ym在短時間內(nèi)精度很高，而基于駕駛員意圖的軌跡預(yù)測yb在長時間時保持較高精度，因此本文使用加權(quán)融合的方法將兩者軌跡預(yù)測方法融合在一起，即yf=w(t)ym+(1-w(t))yb，其中w(t)為權(quán)重函數(shù)，由于在1 s內(nèi)ym精度高而1 s后ym精度嚴(yán)重下降，因此w(t)取為隨三次樣條曲線變化的值，如圖3所示。

圖3 權(quán)值變化曲線

2.2 碰撞概率計算

為了確保碰撞風(fēng)險評估的真實性，本文考慮了汽車行駛的不確定性因素。

為了節(jié)省碰撞風(fēng)險判斷時間，首先進行確定性判斷，若本車和交通車距離大于安全臨界距離，則兩者之間絕對安全，若本車和交通車距離小于危險臨界距離，則兩車發(fā)生碰撞，此兩種情況都無需進行非確定性判斷。若本車和交通車的距離處于兩者之間時進行非確定性判斷，計算各種避撞措施下的碰撞概率。

在縱向上，縱向距離大于車寬就不會發(fā)生碰撞，因此縱向只定義危險臨界距離Δddan，此距離由車寬決定，是一個定值。在橫向上，安全臨界距離ΔLsaf由汽車當(dāng)前速度vi決定，即ΔLsaf=1.2vi，危險臨界距離ΔLdan由車身長度Ls決定，即ΔLdan=Ls。當(dāng)本車與交通車距離在兩者之間時考慮非確定因素。

結(jié)合本文使用的動力學(xué)模型，給出非確定性非線性系統(tǒng)的推導(dǎo)過程，即對于非線性系統(tǒng)（其中w(t)為高斯白噪聲），將其線性化并離散化，得

式中，為恒角加速度模型的雅克比矩陣為k時刻系統(tǒng)狀態(tài)均值；Σk為k時刻系統(tǒng)狀態(tài)標(biāo)準(zhǔn)差。

記初始時刻的狀態(tài)為則k時刻的狀態(tài)可以由式（2）得到。換道時含有不確定因素預(yù)測的軌跡如圖4所示。

圖4 含不確定因素的預(yù)測軌跡

在任意時刻，已知兩物體位置的統(tǒng)計特性就能夠計算它們的碰撞概率。記本車位置和航向角為Xv=(xv,yv,φv)，交通車位置和航向角為X0=(x0,y0,φ0)T，本車和交通車預(yù)測軌跡包絡(luò)區(qū)域分別為Sv和S0。使用概率密度函數(shù)計算兩車碰撞概率，記Xv、X0的高斯概率密度函數(shù)分別為pv(x,y,φ)、p0(x,y,φ)，則碰撞概率為[8]：

使用蒙特卡洛模擬法對上式進行求解，得到其解為：

式中，n為數(shù)據(jù)的采樣點個數(shù)，Ic(Svi,Soj)為自己定義的函數(shù)，其表達(dá)式為即當(dāng)兩區(qū)域Sv、So有重合區(qū)域時，此函數(shù)值為1，當(dāng)兩區(qū)域不重合時，此函數(shù)值為0。

3 道路避障行為決策

本文的道路避障決策從安全性方面考慮，選取車道行駛和交叉路口行駛兩種典型的工況進行分析。

3.1 車道行駛避障行為決策

在車道行駛時，駕駛行為包括保持當(dāng)前狀態(tài)、制動、緊急制動、換道、換道同時制動，智能汽車的駕駛行為決策根據(jù)周圍環(huán)境實時改變，即換道過程中可以采取制動措施，因此，駕駛行為可以分為保持當(dāng)前狀態(tài)、制動、緊急制動、換道。選擇駕駛行為的前提是進行危險等級劃分。圖5所示情景為典型的交通場景，以此為例進行危險等級劃分。

本車保持當(dāng)前狀態(tài)與Veh1、Veh2、Veh3發(fā)生碰撞的概率分布分別記為Pk1、Pk2、Pk3，本車換道與3輛車的碰撞概率分別記為Ps1、Ps2、Ps3，本車緊急制動與3輛車的碰撞概率分別記為Pb1、Pb2、Pb3，本文需要計算本車與交通車在一段時間的碰撞概率，因此以上概率均表示一個數(shù)據(jù)序列。記Pkmax=max(max(Pk1),max(Pk2),max(Pk3))，Pbmax、Psmax具有相同定義，Pcolls、Pcolld分別為碰撞概率的下限和上限閾值，則汽車危險等級劃分為：若Pkmax＜Pcolls，此時汽車是安全的，取Pcolls=0.2；若Pkmax＞Pcolld，此時汽車是危險的，取Pcolld=0.6；若Pkmax在兩者之間，汽車是不安全的。

根據(jù)當(dāng)前汽車的危險等級制定避障措施，決策過程如圖6所示。

圖6 車道行駛駕駛行為決策

3.2 交叉路口避障行為決策

在交叉路口，交通路況復(fù)雜，應(yīng)按照紅綠燈指示進行行為決策，若沒有紅綠燈則使用保守的避障決策，本文以雙向四車道交叉路口為例，如圖7所示，本車與交通車的軌跡沖突情況如表1所示，其中“沖突”指交通車和本車在此意圖下軌跡會發(fā)生沖突，要進行避障軌跡規(guī)劃，“不”指車輛軌跡不沖突，可按當(dāng)前狀態(tài)繼續(xù)行駛。

表1 交叉路口軌跡沖突判斷

首先判斷是否有交通車接近交叉路口，若有則判斷軌跡是否沖突，不沖突時按當(dāng)前狀態(tài)前進，若沖突，則判斷交通車到達(dá)路口時間和本車通過路口時間是否滿足閾值。記所有交通車最先到達(dá)路口時間為Tarr，本車通過路口時間為Treq，若Tarr＞Treq+1 s則不考慮交通車，本車按當(dāng)前狀態(tài)行駛，若不滿足時間閾值條件，則判斷本車采取制動或是換道措施，判斷方法與車道行駛時一致。

4 避障軌跡規(guī)劃

本文使用改進的快速搜索隨機樹算法進行最優(yōu)避障路徑的規(guī)劃。

4.1 避障規(guī)劃對快速搜索隨機樹算法的要求

快速搜索隨機樹算法是1998年提出的[9-10]，結(jié)合智能汽車避障軌跡規(guī)劃，快速搜索隨機樹算法存在以下缺陷：節(jié)點搜索完全隨機，形成的軌跡震蕩大、平滑性差，可能難以滿足汽車動力學(xué)要求；節(jié)點搜索的完全隨機性使算法收斂速度慢，難以滿足避障路徑規(guī)劃的時間要求；軌跡規(guī)劃的隨機性使相鄰周期內(nèi)規(guī)劃出的軌跡存在偏差，使汽車發(fā)生抖動。

汽車的軌跡規(guī)劃對算法要求為：智能汽車通過傳感器對周圍環(huán)境識別，識別范圍內(nèi)障礙物數(shù)量有限；本車面對的障礙物主要是高速動態(tài)的交通車；軌跡規(guī)劃實時更新，必須考慮算法的收斂時間；規(guī)劃得到的軌跡要滿足汽車運動學(xué)和動力學(xué)約束。

4.2 改進的快速搜索隨機樹算法

針對算法缺陷和汽車軌跡規(guī)劃的要求，本文從多個方面對快速搜索隨機樹算法進行了改進。

引入目標(biāo)偏向策略。由于快速搜索隨機樹算法使用的是節(jié)點隨機搜索，為了提高算法收斂速度，本文使用此方法增加目標(biāo)點的采樣概率。此策略從算法開始即使用五次多項式判斷初始點xinit與目標(biāo)點xgoal之間是否存在可行軌跡。若存在且軌跡不經(jīng)過障礙物和危險區(qū)，則將軌跡離散點添加到隨機樹上，軌跡規(guī)劃完畢；若軌跡經(jīng)過障礙物或危險區(qū)，則將障礙物前的軌跡離散點添加到隨機樹上，對每一個新增的節(jié)點xnew重復(fù)上述操作，直至找到目標(biāo)點，這種軌跡搜索方式，不僅提高了算法的收斂速度，而且可以保證軌跡的平滑度。

改進節(jié)點連接策略。節(jié)點連接就是找出合適的節(jié)點xnear與產(chǎn)生的采樣節(jié)點xrand連接，傳統(tǒng)的快速搜索隨機樹算法根據(jù)距離最短選擇節(jié)點xnear，但是在汽車軌跡規(guī)劃中要考慮汽車的轉(zhuǎn)向能力，要求采樣節(jié)點xrand與xnear及xnear父節(jié)點夾角為鈍角，基于此，將節(jié)點連接策略改進為：找出與采樣點xrand距離最近的5個節(jié)點xnear，依據(jù)距離依次判斷夾角是否滿足條件，選擇首先滿足角度要求的點作為xnear，若均不滿足則選擇夾角最小的節(jié)點作為xnear，而后找到xrand關(guān)于xnear的對稱點作為新的采樣點。

引入節(jié)點修剪策略。節(jié)點修剪即刪除可行軌跡中的多余節(jié)點，達(dá)到減少軌跡波動的目的。在隨機樹上找到從初始點到目標(biāo)點的節(jié)點序列后，從初始點開始依次連接后續(xù)節(jié)點，若連接線不經(jīng)過障礙物，則它們之間的節(jié)點可以刪除，當(dāng)經(jīng)過障礙物時，則以碰撞點的父節(jié)點為新起點重復(fù)上述修剪過程。節(jié)點修剪完畢后判斷節(jié)點間角度是否為鈍角，若不滿足角度要求，則通過增加節(jié)點滿足角度要求。

軌跡平滑性方法?？焖匐S機樹方法得到的軌跡為折線，所以本文使用B樣條曲線對節(jié)點進行擬合得到平滑的軌跡。B樣條曲線具有以下特性：n階B樣條曲線具有(n-1)階連續(xù)性，所以曲線平滑性好，適用于汽車軌跡；增加或減少1個節(jié)點，只對(n+1)段曲線產(chǎn)生影響，便于軌跡的局部修改；B樣條曲線可構(gòu)造滿足過定點并與指定曲線相切的要求。B樣條曲線的以上特性使其能夠擬合出平滑軌跡，并符合汽車運動學(xué)和動力學(xué)要求。試驗發(fā)現(xiàn)，5階B樣條曲線更適合擬合汽車軌跡[11]。

最優(yōu)軌跡的選擇。一般情況下，規(guī)劃出的避障軌跡有多條，考慮軌跡的安全性、可實現(xiàn)性及其長度，軌跡安全性通過其與障礙物的距離表征，可實現(xiàn)性考慮軌跡的側(cè)向加速度，基于上述3個方面給出選擇最優(yōu)軌跡的評價指標(biāo)：

式中，Dmin為所有軌跡與障礙物的最短距離；DTra(t)為軌跡Tra與障礙物的距離；AyTra(t)為軌跡Tra的側(cè)向加速度；Aymax為所有軌跡中側(cè)向加速度的最大值；LTra為軌跡Tra的長度；Lmax為所有軌跡中長度的最大值；本文取w1=w2=1，w3=0.5。

軌跡的實時規(guī)劃。汽車行駛環(huán)境是高度變化的，且軌跡規(guī)劃的長度非常有限，因此軌跡規(guī)劃需要實時更新。在軌跡更新時，為了充分利用上一周期的規(guī)劃結(jié)果，保留當(dāng)前最優(yōu)軌跡的下一節(jié)點作為新周期根節(jié)點的子節(jié)點，并保留此節(jié)點到達(dá)目標(biāo)的所有節(jié)點。若此軌跡依然對新周期適用，則繼續(xù)按此軌跡行駛；否則放棄導(dǎo)致碰撞的節(jié)點，重新進行軌跡規(guī)劃。

軌跡跟隨控制，就是要求汽車精準(zhǔn)地按照規(guī)劃的軌跡行駛，常用的軌跡跟隨控制方法有最優(yōu)預(yù)瞄控制、模型預(yù)測控制，從算法復(fù)雜度和控制效果看，本文選用最優(yōu)預(yù)瞄控制方法，具體原理可參考文獻(xiàn)[12]。

5 仿真驗證

5.1 仿真介紹

本文使用的仿真軟件為PanoSim，為了對避撞和軌跡規(guī)劃方法進行驗證，本文設(shè)計了干擾換道工況和交叉路口工況。

在計算預(yù)測軌跡不確定性時，需要知道本車和交通車的協(xié)方差矩陣，根據(jù)傳感器精度及現(xiàn)有研究成果，本車初始協(xié)方差陣為08×8，交通車協(xié)方差陣為diag(0.52,0.52,0.22,0.22,0.22,0.22)。

5.2 仿真結(jié)果分析

干擾換道工況：本車行駛速度為50 km/h，前方60 m處交通車Veh1以20 km/h行駛，相鄰車道Veh2在本車前20 m以30 km/h行駛，行駛到1.4 s時本車預(yù)測到以當(dāng)前狀態(tài)行駛不安全（Pcoll=0.224），此時換道或緊急制動均不危險，按照換道優(yōu)先原則，此時進行換道軌跡規(guī)劃如圖8a所示，本車換道中4.4 s時Veh1突然緊急換道并略有加速，本車按此時狀態(tài)前進會發(fā)生危險，如圖8a所示，圖中本車與Veh1軌跡發(fā)生交叉且時間無法錯開。

通過對緊急制動和換道進行危險評估，本車發(fā)現(xiàn)采用換道同時以-1 m/s2減速可以避開Veh1、Veh2，得到Veh1、Veh2、本車最終的預(yù)測軌跡如圖8b所示。

圖8 干擾換道仿真結(jié)果

從以上仿真結(jié)果可以看出：本車能夠根據(jù)交通車駕駛意圖準(zhǔn)確預(yù)測其他交通車輛的運動軌跡；結(jié)合確定因素和非確定因素，本車能夠及時判斷出危險等級；當(dāng)車輛不安全或危險時，本車能夠采取適當(dāng)措施（制動或換道）并進行軌跡規(guī)劃，及時避開障礙物。

交叉路口工況：本車以30 km/h速度在交叉路口左轉(zhuǎn)，交通車以30 km/h直線行駛，以當(dāng)前狀態(tài)行駛，本車和交通車在離開交叉路口時會發(fā)生碰撞，如圖9a所示。本車采用制動方式（加速度-1 m/s2）進行避障，碰撞概率、最優(yōu)軌跡規(guī)劃如圖9b、圖9c所示。

從圖9b可以看出，本車在行駛過程中，當(dāng)與交通車的碰撞概率上升到不安全閾值時，汽車采取避障措施，措施有效，使得碰撞概率下降，汽車風(fēng)險等級回到安全狀態(tài)，所以圖中的碰撞概率先上升后下降。

圖9 交叉路口仿真結(jié)果

由以上兩種工況的仿真結(jié)果可以看出，本文提出的智能汽車風(fēng)險評估及避障軌跡規(guī)劃方法能夠準(zhǔn)確預(yù)測交通車軌跡，結(jié)合確定因素和不確定因素準(zhǔn)確判斷出風(fēng)險等級，能夠及時找出合適的避障措施并進行軌跡規(guī)劃，規(guī)劃出的軌跡安全、平滑，滿足汽車運動學(xué)和動力學(xué)約束。

6 結(jié)論

本文將汽車狀態(tài)和駕駛意圖相結(jié)合預(yù)測交通車軌跡，保證了軌跡的短期和長期精度；同時考慮汽車碰撞的確定性因素和不確定因素，保證了風(fēng)險評估的準(zhǔn)確性和快速性；給出的道路行駛和交叉路口避障行為決策策略準(zhǔn)確有效，可以避免碰撞發(fā)生；使用改進的快速搜索隨機樹算法能夠快速規(guī)劃出一條最優(yōu)路徑，此路徑符合汽車運動學(xué)和動力學(xué)約束。

[1]胡啟洲,高寧波,葉茂.基于支持向量機的道路交通事故數(shù)據(jù)統(tǒng)計模型研究[J].中國安全科學(xué)學(xué)報,2013,23(6):39.

[2]Zhang Y,Antonsson E K,Grote K.A new Threat Assessment Measure for Collision Avoidance Systems[C]//Intelligent Transportation Systems Conference,2006.IEEE,2006:968-975.

[3]鄭華榮.考慮周邊車輛駕駛意圖的換道危險預(yù)警研究[D].武漢：武漢理工大學(xué),2013.

[4]Lawitzky A,Althoff D,Passenberg C F,et al.Interactive Scene Prediction for Automotive Applications[C]//Intelligent Vehicles Symposium.IEEE,2013:1028-1033.

[5]Ren D B,Zhang J Y.Trajectory Planning and Yaw Rate Tracking Control for Lane Changing of Intelligent Vehicle on Curved Road[J].Science China Technological Sciences,2011,54(3):630-642.

[6]劉多能.基于人機智能融合的移動機器人路徑規(guī)劃方法研究[D].長沙：國防科學(xué)技術(shù)大學(xué),2011.

[7]Yao W,Zhao H,Davoine F,et al.Learning Lane Change Trajectories from On-road Driving Data[C]//Intelligent Vehicles Symposium.IEEE,2012:885-890.

[8]Stellet J E,Schumacher J,Branz W,et al.Uncertainty Propagation in Criticality Measures for Driver Assistance[C]//Intelligent Vehicles Symposium.IEEE,2015:1187-1194.

[9]徐聯(lián)杰,劉檢華,何永熹,等.一種基于改進快速搜索隨機樹算法的管路自動布局方法[J].圖學(xué)學(xué)報,2016,37(1):1-10.

[10]莫棟成,劉國棟.改進的快速探索隨機樹雙足機器人路徑規(guī)劃算法[J].計算機應(yīng)用,2013,33(1):199-201.

[11]任重,楊燦軍,陳鷹.軌跡規(guī)劃中的B樣條插值算法[J].機電工程,2001,18(5):38-39.

[12]沈峘,凌銳,李舜酩,等.基于預(yù)瞄最優(yōu)曲率模型的大曲率轉(zhuǎn)向控制方法[J].中國機械工程,2012,23(17):2111-2116.