基于采樣的無人車路徑規(guī)劃算法仿真與驗(yàn)證

付 行，聶文福，王 凱，任文峰

（陜西重型汽車有限公司，陜西 西安 710200）

無人駕駛局部路徑規(guī)劃主要研究在結(jié)構(gòu)化道路下依據(jù)參考線的規(guī)劃和非結(jié)構(gòu)化道路空間搜索路徑規(guī)劃。以參考線為基準(zhǔn)的方法，通過橫縱向平移產(chǎn)生多條候選軌跡，進(jìn)而評(píng)估軌跡優(yōu)劣性，選擇最優(yōu)軌跡［1-4］。A*及其變種算法通過啟發(fā)式搜索可用于區(qū)域性路徑規(guī)劃［5］。本文對(duì)基于采樣的算法研究，分別完成了路徑規(guī)劃、碰撞檢測(cè)、路徑評(píng)估和最優(yōu)選擇。同時(shí)，為了測(cè)試算法，在Prescan中搭建仿真場(chǎng)景對(duì)多種工況進(jìn)行了仿真。還進(jìn)行了實(shí)車測(cè)試，結(jié)果表明，該算法能夠有效避障，并生成平滑、穩(wěn)定的可行駛路徑。

1 路徑規(guī)劃

對(duì)末狀態(tài)進(jìn)行采樣，求解車輛當(dāng)前位置到采樣狀態(tài)的解是局部路徑規(guī)劃的核心問題［3］，將路徑生成表述為邊界值問題，通過逐步優(yōu)化迭代，得出最優(yōu)的5次多項(xiàng)式系數(shù)，該方法最大的問題是需要建立龐大的查詢系數(shù)表［4］。將采樣狀態(tài)進(jìn)行橫縱向分解，用高次多項(xiàng)式表達(dá)S縱向、L橫向之間的關(guān)系，求解多項(xiàng)式參數(shù)，最后轉(zhuǎn)換到笛卡爾坐標(biāo)系。該方法在坐標(biāo)變換過程中考慮了參考線的曲率，所以受參考線曲率影響較大。

本文應(yīng)用frenet坐標(biāo)系，將問題分解為S、L應(yīng)用5次多項(xiàng)式進(jìn)行求解，表達(dá)式為：

要求解這個(gè)方程組需要一些初始配置和目標(biāo)配置，以橫向路徑規(guī)劃為例，初始配置為，即t0時(shí)刻車輛的橫向偏移、橫向速度和橫向加速度。

橫向軌跡評(píng)估函數(shù)考慮了橫向偏移、碰撞、橫縱向加速度變化率等，計(jì)算每一條候選軌跡代價(jià)值。經(jīng)過將橫縱向軌跡合并，轉(zhuǎn)換到笛卡爾坐標(biāo)下進(jìn)行碰撞檢測(cè)，刪除發(fā)生碰撞的軌跡，剩余軌跡中代價(jià)最小的即為最優(yōu)路徑。

2 算法仿真

搭建Prescan仿真場(chǎng)景測(cè)試環(huán)境，編譯后生成對(duì)應(yīng)的Simulink框圖，如圖1所示。Dynamics_Simple是車輛運(yùn)動(dòng)控制模塊，SELF_Demux 是車輛自身信息的輸出模塊，Lidar_1_Demux是激光雷達(dá)感知模塊。按照消息類型分別添加Subcribe、Publish模塊用于接收和發(fā)送信息。圖1中的MATLAB Function1是S函數(shù)，主要完成算法的主體部分，計(jì)算出轉(zhuǎn)向盤轉(zhuǎn)角信息。Subsystem是依據(jù)當(dāng)前速度和接收到的期望速度V，計(jì)算油門開度和制動(dòng)力度。

仿真驗(yàn)證如圖2a所示，在沒有障礙物的情況下，左側(cè)的黑點(diǎn)為主車，右側(cè)的黑點(diǎn)為障礙物，3條實(shí)線為車道隔離線，藍(lán)色的小點(diǎn)是規(guī)劃的軌跡，在軌跡規(guī)劃沒有碰到障礙物的時(shí)候，紅色的點(diǎn)組成的連線為最優(yōu)路徑軌跡。從驗(yàn)證仿真圖可以看出，最優(yōu)的路徑軌跡為紅色的點(diǎn)連接成的軌跡，也就是車道的中心線是最優(yōu)的軌跡路徑。圖2b是Prescan仿真驗(yàn)證的3D顯示結(jié)果，從圖2b中可以看出主車為藍(lán)色車輛，前面的白車為障礙物，但是在本實(shí)驗(yàn)中藍(lán)色車輛距離白色車輛距離較遠(yuǎn)，還沒有形成障礙物，試驗(yàn)證明，主車一直沿著車道中心線運(yùn)動(dòng)，達(dá)到了預(yù)期的效果。

圖1 Simulink框圖

圖2 無障礙物仿真驗(yàn)證

存在障礙物的情況下，如圖3a所示，左側(cè)的黑點(diǎn)為主車，右側(cè)的黑點(diǎn)為障礙物，紅色的點(diǎn)組成的連線為避過障礙物之后的最優(yōu)路徑軌跡。圖3b是Prescan仿真3D顯示效果圖，后面的藍(lán)色車為主車，前面的白色車為障礙物，在有障礙物時(shí)，主車明顯的車頭向左轉(zhuǎn)動(dòng)，在有意避開障礙物。試驗(yàn)證明，主車沿著紅色的軌跡運(yùn)行，成功避開了障礙物（白色車輛），然后繼續(xù)沿著車道中心線運(yùn)行，試驗(yàn)驗(yàn)證達(dá)到了預(yù)期效果。

圖3 有障礙物仿真驗(yàn)證

3 實(shí)車驗(yàn)證

為了進(jìn)一步研究該算法，借助我公司重型越野無人平臺(tái)對(duì)其進(jìn)行了實(shí)車驗(yàn)證。該平臺(tái)搭載了多線激光雷達(dá)、GPS慣導(dǎo)設(shè)備。驗(yàn)證環(huán)境選擇我公司車輛測(cè)試場(chǎng)，分別設(shè)置了起點(diǎn)、終點(diǎn)，用于對(duì)停止點(diǎn)規(guī)劃，同時(shí)在道路內(nèi)設(shè)置了假人障礙物，用于驗(yàn)證規(guī)劃路徑能剔除碰撞路徑，選擇最優(yōu)路徑行駛。測(cè)試開始之前，首先在道路中心采集了GPS軌跡，用于指引車輛前行。其次，對(duì)于道路的采樣，橫向軌跡的采樣需要涵蓋多種橫向運(yùn)動(dòng)狀態(tài)。最后，借助QT_GUI顯示車輛行駛環(huán)境和規(guī)劃路徑，通過算法對(duì)最優(yōu)路徑的選擇，調(diào)整設(shè)置參數(shù)。在本次驗(yàn)證中設(shè)計(jì)了7個(gè)末狀態(tài)橫向偏移量：-1.5m，-1.0m，-0.5m，0.0m，0.5m，1m和1.5m以及4個(gè)到達(dá)這些橫向偏移量的縱向位移，分別為：10m，20m，40m，60m。

驗(yàn)證過程，激光雷達(dá)感知到障礙物信息，傳遞給規(guī)劃節(jié)點(diǎn)，規(guī)劃避障的最優(yōu)路徑和合理的行駛速度，然后將路點(diǎn)信息和速度信息發(fā)送給控制模塊，控制車輛行進(jìn)。

在避障過程中，GUI顯示最優(yōu)路徑的選擇先由遠(yuǎn)距離中心線轉(zhuǎn)換為近距離中心線，進(jìn)行了減速動(dòng)作，再轉(zhuǎn)換為近距離右偏1.5m，如圖4所示，柱體是靜態(tài)障礙物，選擇向右側(cè)偏1.5m，速度有所減緩的紅色線為最優(yōu)路徑。接著車輛跟隨軌跡向右轉(zhuǎn)，避開障礙物，隨即切換為近距離右偏1m，使轉(zhuǎn)向盤稍有回正，車身避開障礙物后，最優(yōu)路徑選擇為遠(yuǎn)距離中心線，加速行駛。圖5為實(shí)車進(jìn)行的避障情形。

經(jīng)過實(shí)車驗(yàn)證，該算法能夠在巡航、避障、停車等工況下給出合理可行的行駛路徑，滿足無人車控制約束條件。

圖4 最優(yōu)軌跡選擇

4 結(jié)論

圖5 實(shí)車避障

軌跡規(guī)劃是自動(dòng)駕駛決策模塊的重要組成部分，高效生成平滑、合理的規(guī)劃軌跡是實(shí)現(xiàn)自動(dòng)駕駛的關(guān)鍵技術(shù)之一。本文針對(duì)自動(dòng)駕駛車輛的軌跡規(guī)劃建模問題展開研究，將Frenet 坐標(biāo)系用于解決自動(dòng)駕駛車輛軌跡規(guī)劃問題，簡(jiǎn)化了建模過程，利用5次多項(xiàng)式分別建立了自動(dòng)駕駛車輛橫、縱向軌跡規(guī)劃模型，然后通過損失函數(shù)的計(jì)算確定每條軌跡的代價(jià)值，碰撞檢測(cè)去除與障礙物碰撞的軌跡，選擇一條代價(jià)最小的軌跡，最終得到自動(dòng)駕駛車輛在結(jié)構(gòu)化道路上的最優(yōu)運(yùn)動(dòng)軌跡。