無人駕駛汽車離散優(yōu)化的軌跡規(guī)劃算法

張垚， 彭育輝

(福州大學機械工程及自動化學院， 福建 福州 350108)

0 引言

隨著2020年2月《智能汽車創(chuàng)新發(fā)展戰(zhàn)略》的發(fā)布， 汽車無人駕駛技術(shù)研究得到了學者和行業(yè)的普遍關(guān)注. 軌跡規(guī)劃是汽車無人駕駛的關(guān)鍵模塊， 依據(jù)感知系統(tǒng)得到行駛環(huán)境的周圍信息， 生成一條保證行駛安全、 時空平滑、 可行的汽車行駛軌跡.

目前被廣泛應(yīng)用的基于離散優(yōu)化的軌跡規(guī)劃算法， 是針對行駛環(huán)境進行離散的. 其采用數(shù)值積分和微分方程描述車輛運動， 產(chǎn)生有限的候選軌跡， 由代價函數(shù)從候選軌跡中選擇出最優(yōu)軌跡， 實時性好. Chu等[1]基于離散優(yōu)化思想， 提出以三次多式作為車輛運動基元的算法， 能夠安全地避開靜態(tài)障礙物， 但沒有考慮動態(tài)障礙物和多車道場景. 周惠子等[2]通過加權(quán)碰撞檢測的方法， 實現(xiàn)了多車道避障行駛， 但仍未考慮移動障礙物. 文獻[3-4]在此基礎(chǔ)上考慮了移動障礙車輛， 提出一種新的安全代價函數(shù)， 但是僅考慮了同向行駛的障礙車輛， 具有一定的局限性. Werling等[5]以五次多項式作為運動基元， 同時引入最優(yōu)控制的方法進行規(guī)劃， 能夠避開低速行駛的障礙物， 但行駛時會導致方向盤頻繁的轉(zhuǎn)動. 由于使用特定的參數(shù)曲線描述車輛的運動軌跡， 限制了車輛的運動性能[6]. 文獻[7-9]使用采樣和優(yōu)化相結(jié)合的軌跡規(guī)劃方法， 使規(guī)劃出來的軌跡更加平滑. 上述研究對行駛環(huán)境進行離散時， 需選擇適合的采樣精度以平衡規(guī)劃路徑的最優(yōu)性和候選路徑的計算量， 且研究場景大多局限于單向車道場景. 相比于單向車道， 車輛在雙向兩車道執(zhí)行避障、 超車等行為時需要考慮旁邊車道相向行駛的車輛， 規(guī)劃難度更高.

針對以上問題， 本研究提出一種面向雙向兩車道的無人車軌跡規(guī)劃算法. 該算法使用人工勢場法描述車輛與周圍環(huán)境的相互關(guān)系， 基于分層采樣的方法求解初始路徑以平衡路徑的最優(yōu)性和計算量； 將同車道車輛以及旁邊車道相向行駛的車輛投影到ST圖上， 基于多目標A*搜索算法規(guī)劃出初始速度剖面并進行優(yōu)化， 完成對旁邊車道車輛的避讓. 最后， 通過Python進行計算機仿真， 驗證算法的有效性和可靠性.

1 軌跡規(guī)劃算法

算法分為路徑規(guī)劃和速度規(guī)劃兩部分. 路徑規(guī)劃時， 基于道路中心線構(gòu)建s-ρ坐標系， 將無人車與周圍障礙物映射到s-ρ坐標系上. 通過分層采樣構(gòu)建路徑Lattice圖， 搜索出初始路徑， 然后對初始路徑進行平滑優(yōu)化. 一旦生成了一條平滑的路徑， 障礙物就會被投影到ST圖上， 在ST圖上搜索出初始速度剖面后再進行平滑優(yōu)化. 結(jié)合路徑和速度曲線， 將獲得規(guī)劃的平滑軌跡.

1.1 初始路徑生成

1.1.1構(gòu)建路徑Lattice圖

對于雙向兩車道， 將右側(cè)車道的中心線作為參考路徑， 基于參數(shù)化樣條曲線方法構(gòu)建s-ρ坐標系[5]. 在s-ρ坐標系下對車輛行駛的狀態(tài)空間進行離散采樣， 縱向采樣間距Δs根據(jù)車速進行設(shè)定. 與文獻[5]不同的是， 本研究使用采樣層的組合構(gòu)建路徑Lattice圖(如圖1所示)， 以規(guī)劃較長的路徑.

圖1 路徑Lattice圖Fig.1 Lattice diagram of the path

采樣點之間的連接方式視為運動基元， 以三次多項式作為運動基元， 函數(shù)表達式為：

ρ=a0+a1s+a2s2+a3s3(s∈[s0,se])

(1)

通過起點狀態(tài)(s0,ρ0,θ0)和終點狀態(tài)(se,ρe,θe)的位姿約束， 得到4個邊界條件可以唯一確定運動基元.

1.1.2路徑代價函數(shù)設(shè)計

路徑Lattice圖中每條邊的代價值由路徑代價函數(shù)確定， 即：

Cvalue=Csmooth+Cref+Cobs

(2)

式中：Csmooth表示路徑的光滑度項， 用曲率反映；Cref表示路徑與參考線的橫向偏移懲罰項；Cobs表示障礙物的碰撞風險項.

對每一段曲線均勻采樣N個點， 用近似曲率平方的積分表示光滑度函數(shù)：

(3)

相對安全的行駛策略是車輛應(yīng)盡可能沿著右側(cè)車道的中心線行駛， 將Cref表示為N個路徑點的和， 即：

(4)

式中：ρr表示參考線在s-ρ坐標系下的橫向坐標.

周圍環(huán)境對無人車的行駛安全影響極大， 結(jié)合人工勢場法[10-11]， 分別對道路、 障礙車輛建模， 構(gòu)建無人車行駛的碰撞風險代價. 碰撞風險項定義為：

(5)

式中：Eroad表示道路邊界勢能；Eobsj表示第j個障礙車輛的勢能；nobs表示環(huán)境中障礙物的數(shù)量.

道路邊界是防止車輛駛出道路， 越靠近道路邊界位置其勢場值越大， 設(shè)計的道路邊界勢場函數(shù)為：

(6)

其中：Vroad表示邊界勢能值；ρroad， 1、ρroad， 2分別表示左右道路邊界在s-ρ坐標系下的橫向距離. 對于雙向兩車道， 道路勢場分布情況如圖2所示.

圖2 道路勢能分布Fig.2 Road potential energy distribution

通過建立障礙物勢能模型使無人車與障礙物保持一定的安全距離. 采用類高斯函數(shù)模擬障礙物勢能， 勢能分布函數(shù)為：

(7)

其中：Vobs表示障礙物勢場系數(shù)；σjs表示縱向安全參數(shù)；σjρ表示橫向安全參數(shù).

障礙車輛停放在s-ρ坐標系下(-1, 40)處的勢場分布情況如圖3所示.

圖3 障礙車輛勢能分布Fig.3 Potential energy distribution of obstacle vehicles

1.1.3路徑搜索

路徑代價函數(shù)式(2)確定了路徑Lattice圖中每一條邊的代價值， 采用圖搜索算法即可搜索出代價最小的路徑. 為了平衡路徑規(guī)劃的實時性和最優(yōu)性， 提出分層采樣的方法， 即先以較大的橫向采樣間隔構(gòu)建路徑Lattice圖， 搜索出一條較優(yōu)的路徑， 如圖4(a)中粉紫色的路徑. 以此路徑為中心線用較小的橫向采樣間隔再次構(gòu)建路徑Lattice圖， 搜索出初始路徑， 如圖4(b)中藍色的路徑. 與直接用較小橫向間隔的采樣方法相比， 分層采樣的方法求解路徑的計算量更小.

圖4 分層采樣規(guī)劃Fig.4 Stratified sampling planning

1.2 路徑優(yōu)化

初始路徑的曲率剖面具有階躍變化現(xiàn)象， 平滑性較差， 還需進一步優(yōu)化. 采用數(shù)值優(yōu)化的方法， 對路徑以縱向等距離Δs離散成Ns個路徑點(si,ρi)， 把ρ=[ρ0,ρ1, …,ρn]T作為優(yōu)化的決策變量， 通過最小化路徑點橫向距離ρ的變化率對路徑進行優(yōu)化， 目標函數(shù)定義為：

(8)

式中： 前三項與車輛在s-ρ坐標系下的航向角以及其變化率相關(guān)， 用來平滑路徑； 第四項表示對初始路徑的跟隨；ρinit表示初始路徑采樣點. 橫向距離ρ的相關(guān)導數(shù)由采樣路徑點進行差分近似， 即：

(9)

優(yōu)化時還要滿足車輛運動模型確定的曲率約束、 避免碰撞約束以及道路邊界約束. 其中， 由車輛的阿克曼轉(zhuǎn)向幾何關(guān)系， 確定車輛能夠通過的最大路徑曲率κlim， 將曲率約束表示為：

建立避免碰撞約束關(guān)系式時， 采用3個半徑為rc的圓沿著車輛的縱軸分布表示無人車的覆蓋面積， 其中一個圓的圓心與車輛后軸的中點重合， 兩個圓心之間的距離為d， 如圖5所示.車輛后軸中心點的坐標(si,ρi)由定位模塊通過坐標轉(zhuǎn)換得出， 計算可得其余兩個圓的圓心坐標， 將約束表示為：

圖5 三圓碰撞檢測Fig.5 Three circle collision detection

(11)

式中： (sj，ρj)表示第j個障礙物的s-ρ坐標；robs表示障礙物的擴展半徑.

同時， 車輛在行駛過程中不能超出道路的邊界， 因此形成不等式約束：

ρmin≤ρi≤ρmax

(12)

式(12)中，ρmax和ρmin分別是道路的左邊界和右邊界相對于中心線的橫向偏移. 從圖6中初始路徑(藍色)與優(yōu)化路徑(暗紅色)的對比， 可以看出優(yōu)化路徑的曲率是平滑的.

圖6 初始路徑與優(yōu)化路徑對比Fig.6 Comparison of initial path and optimized path

1.3 初始速度剖面生成

速度剖面規(guī)劃時引入ST圖[8]， 目標是規(guī)劃未來8 s或80 m的速度曲線. 先將ST圖柵格化， 將本車道以及旁邊車道的障礙物投影到ST圖上， ST圖中的可行域變成非凸區(qū)域. 然后， 基于提出的多目標A*算法在可行域中搜索出初始的速度剖面.

ST圖柵格化如圖7所示， 以時間間隔Δt=0.5 s和縱向距離間隔Δs=2 m， 將ST圖劃分成網(wǎng)格， 四邊形區(qū)域表示障礙車輛占據(jù)的時域， 黃綠色點是目標點， 紅色線表示以最大速度勻速行駛的速度剖面線. 在紅色線右側(cè)、 黃色目標點下方未被障礙車輛占據(jù)的時域為可行域.

圖7 初始速度剖面規(guī)劃Fig.7 Initial velocity profile planning

與A*算法[12]的節(jié)點擴展方式不同， 本研究以網(wǎng)格頂點為節(jié)點， 從原點進行節(jié)點擴展. 考慮到車輛最大速度的限制， 以及車輛正常行駛時沒有倒車行為， 父節(jié)點可以擴展為6個子節(jié)點, 如圖8所示. 節(jié)點速度及其導數(shù)可根據(jù)采樣間隔Δt， Δs進行差分近似計算. 近似計算與式(9)類似， 擴展節(jié)點的成本函數(shù)為：

圖8 節(jié)點擴展方式Fig.8 Node expansion method

Cspeed=g(s)+h(s)

(13)

其中：g(s)為從父節(jié)點到當前節(jié)點的代價函數(shù);h(s)為當前節(jié)點到目標節(jié)點集的啟發(fā)函數(shù).g(s)的具體算式為：

ω3(s?i)2+ω4(dsi+δ)-1

(14)

式中: 第一項代表相對于參考速度vref偏移的懲罰項， 使規(guī)劃速度不會偏離參考速度太遠； 第二項和第三項分別表示加速度和jerk的懲罰項， 通過抑制其快速變化使速度剖面更加平滑； 最后一項代表障礙物成本， 其中dsi表示節(jié)點到所有障礙物的最小距離，δ=0.001是為了消除奇異值.

式(13)中的h(s)與A*算法的不同在于速度規(guī)劃時設(shè)有多個目標點， 因此啟發(fā)函數(shù)的設(shè)計需要考慮到每個目標點， 即：

(15)

式中：m表示目標點的個數(shù)； (si,ti)表示當前節(jié)點； (sj,tj)表示目標節(jié)點.

多目標A*算法的搜索過程與A*算法類似， 當擴展節(jié)點的si大于規(guī)劃的最大縱向距離80 m或擴展節(jié)點的ti大于規(guī)劃的最大時間8 s， 則搜索結(jié)束. 查找出代價最小的目標節(jié)點， 通過節(jié)點回溯構(gòu)成速度剖面， 規(guī)劃的速度剖面如圖7藍色折線所示.

1.4 速度剖面優(yōu)化

初始速度剖面由多條直線段連接而成， 不滿足平滑性要求和車輛動力學約束， 需對初始速度剖面進行優(yōu)化， 優(yōu)化結(jié)果如圖9所示.

圖9 速度剖面優(yōu)化Fig.9 Speed profile profile optimization

首先， 對初始速度剖面以更小的時間間隔σ進行重采樣， 即：

ti=t0+iσ(i=0, 1, …,Nt)

(16)

然后計算時間戳ti對應(yīng)的縱向距離si， 通過固定時間戳把s=[s0,s1, …,sn]T作為優(yōu)化的決策變量， 將優(yōu)化問題的目標函數(shù)定義為：

(17)

優(yōu)化時需要滿足車輛的動力學約束和避免碰撞約束. 規(guī)劃的速度不能超出速度限制vmax、 道路限速vlim、 加速度限制amax和加加速度限制jerkmax， 故動力學約束定義為：

(18)

建立避免碰撞約束關(guān)系式時， 通過對ST圖的處理， 將初始速度剖面附近的可行域變成一個凸集(如圖9中綠色區(qū)域). 對于每個決策變量si， 其范圍約束為：

si≤si+1，sd, i≤si≤sh, i(i=0, 1, …,Nt-1)

(19)

式中: 第一個不等式限制了車輛的倒車行為；sh, i、sd, i分別表示凸集的上界和下界.

上述所有的約束都是線性的， 決策變量的可行域是一個凸集， 目標函數(shù)是二次凸函數(shù)， 優(yōu)化問題變成一個中等規(guī)模的二次規(guī)劃問題, 可應(yīng)用二次規(guī)劃求解器快速求解.

2 仿真結(jié)果與分析

為驗證本算法的有效性， 基于Python構(gòu)建寬為3.75 m的雙向兩車道道路模型， 道路包括直線路段和彎道路段， 并在道路上設(shè)置若干靜止和移動的障礙車輛， 針對典型的駕駛場景進行測試. 在仿真器中， 無人車用藍色矩形表示， 其最大速度為20 m·s-1， 最大加速度為2.5 m·s-2， 最大加加速度(jerk)為5 m·s-3， 曲率限制為0.1 m-1. 障礙車輛用其它顏色矩形表示， 時間信息由顏色的深度來表示， 其中較深的顏色表示較晚的時間步長. 算法使用Python腳本編寫， 運行在英特爾酷睿i7-5500U (2.4 GHz)的筆記本電腦上.

2.1 避障場景

無人車在雙向車道中避障時， 需要警惕旁邊車道的車輛， 場景如圖10所示. 本車道前方有2個靜止的車輛， 旁邊車道有一相向行駛的車輛， 車速為4 m·s-1. 無人車初始速度為8 m·s-1， 參考速度為12 m·s-1.

圖10(a)顯示了無人車按本規(guī)劃方法行駛的軌跡， 無人車安全地避開了障礙車輛. 圖10(b)將本方法與文獻[5]的軌跡規(guī)劃方法(稱為方法一)對比， 結(jié)果顯示， 無人車按本方法規(guī)劃軌跡行駛的曲率均方差為0.007 9， 相比于方法一的0.010 3減小了23.30%. 圖10(c)中橙色的曲線為無人車的速度變化曲線， 藍色曲線為參考速度曲線. 可以看出, 無人車選擇了加速避開障礙物， 這是因為移動車輛車速較慢， 速度規(guī)劃時選擇加速通過可能發(fā)生碰撞的區(qū)域. 而無人車的加速度和jerk的變化均沒有超過閾值.

圖10 避障場景Fig.10 Obstacle avoidance scene

2.2 超車場景

在雙向兩車道路上超車， 而相鄰車道上有迎面而來的車輛是最具挑戰(zhàn)性和危險性的駕駛場景， 如圖11所示. 此場景中， 無人車初始速度為8 m·s-1， 前車速度為 4 m·s-1, 迎面而來的車輛速度為12 m·s-1.

圖11(a)顯示了無人車與障礙車輛的運動軌跡. 圖11(b)為兩種方法行駛軌跡的曲率對比， 無人車按本方法規(guī)劃軌跡行駛的曲率均方差為0.009 5， 相比于方法一的0.013 3減小了28.57%. 圖11(c)為無人車的速度、 加速度和jerk的變化曲線. 圖中顯示， 無人車先減速， 直到迎面駛來的車輛通過， 然后加速超過前車并趨向于參考速度. 這是由于迎面駛來的車輛車速較快， 速度規(guī)劃時無人車選擇減速對其避讓， 當其通過時， 無人車加速超過前面的車輛并趨向于參考速度.

2.3 跟隨場景

車輛跟隨也是雙向兩車道最常見的駕駛行為之一， 如圖12所示. 此場景中， 無人車前方以及旁邊車道的障礙車輛車速為12 m·s-1， 無人車當前車速為16 m·s-1.

圖12(a)顯示了無人車跟隨前車行駛的軌跡， 在車輛行駛?cè)^程中無人車始終與前車保持一定的安全距離. 圖12(b)對比了兩種方法行駛軌跡的曲率. 結(jié)果顯示， 無人車按本方法規(guī)劃軌跡行駛的曲率均方差為0.004 1， 相比于方法一的0.004 4減小了6.82%. 圖12(c)為無人車的速度、 加速度和jerk的變化曲線. 圖中顯示， 無人車緩慢減速到與前車相同的速度， 且其加速度和jerk的變化都在約束范圍內(nèi).

圖12 跟隨場景Fig.12 Follow the scene

為了體現(xiàn)本算法的實時性， 在3個測試場景中， 各抽取10次本算法的運行時間， 如表1所示. 本算法的平均運行時間在0.22 s左右， 實時性好.

表1 算法運行時間

3 結(jié)語

基于離散優(yōu)化的方法， 提出一種針對雙向兩車道的新軌跡解耦規(guī)劃算法. 通過解耦方法降低軌跡規(guī)劃的復雜度， 且使用ST圖增加了軌跡的安全性. 在路徑代價函數(shù)的設(shè)計中結(jié)合人工勢場法， 引導無人車安全、 合理地避開障礙物. 使用分層采樣的方法對環(huán)境進行離散， 并采用多目標A*算法規(guī)劃初始速度剖面， 有效地加快了算法的求解速度. 該算法同樣適用于單向多車道道路. 仿真實驗結(jié)果表明， 所提算法能夠在短時間內(nèi)產(chǎn)生安全的軌跡； 與傳統(tǒng)軌跡規(guī)劃算法對比， 所規(guī)劃軌跡的曲率更加平滑.