基于采樣的無人車路徑規劃算法仿真與驗證

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

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

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

1 路徑規劃

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

本文應用frenet坐標系，將問題分解為S、L應用5次多項式進行求解，表達式為：

要求解這個方程組需要一些初始配置和目標配置，以橫向路徑規劃為例，初始配置為，即t0時刻車輛的橫向偏移、橫向速度和橫向加速度。

橫向軌跡評估函數考慮了橫向偏移、碰撞、橫縱向加速度變化率等，計算每一條候選軌跡代價值。經過將橫縱向軌跡合并，轉換到笛卡爾坐標下進行碰撞檢測，刪除發生碰撞的軌跡，剩余軌跡中代價最小的即為最優路徑。

2 算法仿真

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

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

圖1 Simulink框圖

圖2 無障礙物仿真驗證

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

圖3 有障礙物仿真驗證

3 實車驗證

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

驗證過程，激光雷達感知到障礙物信息，傳遞給規劃節點，規劃避障的最優路徑和合理的行駛速度，然后將路點信息和速度信息發送給控制模塊，控制車輛行進。

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

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

圖4 最優軌跡選擇

4 結論

圖5 實車避障

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