陳鑫鵬，徐 彪，胡滿江，秦兆博，邊有鋼

（湖南大學 機械與運載工程學院，湖南 長沙 410082）

0 引言

智能車輛融合了先進傳感器技術、高精度定位技術和先進決策控制技術，可有效提高車輛運行效率、提升駕駛安全性、減小駕駛員的駕駛負擔，是當下汽車技術發(fā)展前沿[1]。路徑規(guī)劃作為智能車輛的核心技術之一，其主要任務是為車輛提供一條通往目的地的安全、無碰撞路徑[2]。根據不同場景，智能車輛的路徑規(guī)劃可分為結構化道路的路徑規(guī)劃和非結構化道路的路徑規(guī)劃[3]。結構化道路（如高速公路、城市主干道等）包含明顯的車道線、道路標識和車道邊界等信息，可以直接為智能車輛提供參考路徑，極大地降低了路徑規(guī)劃的復雜程度[4]。相較于結構化道路場景，非結構化道路場景（如礦場、港口、停車場和園區(qū)等）無車道線信息、地形場景開闊且道路邊界復雜，因此，其智能車輛路徑規(guī)劃具有較大難度。目前，針對非結構化道路場景的路徑規(guī)劃方法主要分為基于采樣的方法和基于圖搜索的方法兩大類。基于采樣的方法主要包括PRM算法[5]、RRT算法[6]及其衍生算法[7-9]，此類方法具有搜索速度快、不需要對環(huán)境進行具體建模等優(yōu)點，但由于其隨機采樣的特性導致搜索的路徑迂回曲折，難以得到穩(wěn)定的最優(yōu)或次優(yōu)路徑。基于圖搜索的方法主要有Dijkstra算法、A*算法和D*算法等[10]，采用此類方法搜索得到的路徑雖能保證最優(yōu)性，但難以滿足車輛的運動學約束[11]。針對這一問題，Dolgov等人提出一種考慮車輛運動學性能的混合A*算法[12]，通過對車輛前輪轉角進行離散，拓展得到滿足車輛運動學約束的子節(jié)點。但該方法采用的子節(jié)點拓展方式效率較低，且搜索得到的路徑不夠平滑，仍然難以滿足智能車輛實際應用需求。對此，本文在傳統混合A*算法基礎上進行改進，提出一種新的子節(jié)點拓展方式，并對啟發(fā)式函數（Heuristic）與代價函數進行合理設計，以提高其路徑搜索效率與路徑平滑性；同時對搜索得到的路徑進行進一步平滑與插值，從而保證路徑的最優(yōu)性。

1 混合A*算法

傳統A*算法可在柵格化的地圖內進行路徑搜索，首先建立一個open集和一個closed集，每搜索一步都利用評估函數f(s)=g(s)+h(s)（其中g(s)為代價函數，h(s)為啟發(fā)式函數）對open集中的節(jié)點進行排序，并將評估代價最小的節(jié)點移到closed集中；然后以此節(jié)點作為父節(jié)點進行子節(jié)點拓展，從而構建出一棵搜索樹。當終點加入搜索樹后再通過路徑回溯，即可獲得一條從給定起點到達終點的最優(yōu)路徑，但由于其子節(jié)點只能朝父節(jié)點周圍8個柵格中心拓展，故得到的路徑無法給具有非完整約束的車輛執(zhí)行。

混合A*算法沿用了傳統A*算法的思想，與傳統A*算法不同的是，其子節(jié)點位置是根據拓展步長、車輛最小轉彎半徑以及前輪轉角離散數量而確定的，因此可以訪問柵格中的任意位置，從而保證了最終搜索得到的路徑能夠滿足車輛運動學要求。圖1示出傳統A*算法與混合A*算法子節(jié)點拓展示意。

圖1 A*算法與混合A*算法子節(jié)點拓展示意Fig.1 Node expansion diagram of A* and hybrid-A*algorithms

2 改進混合A*算法

傳統混合A*算法采用等步長單層拓展方式，其搜索效果受前輪轉角離散數量和搜索步長影響較大。前輪轉角離散數量過大或搜索步長過小，都會降低搜索效率；前輪轉角離散數量過小或搜索步長過大，則會降低路徑的平滑性與安全性。為了同時保證算法搜索效率和路徑平滑性，本文提出一種新的子節(jié)點拓展方式。其采用等步長分層拓展的方法，既能保證搜索得到的路徑平滑性與安全性，又能大大提高路徑搜索效率。同時，通過合理設計啟發(fā)函數和代價函數，避免了節(jié)點沿著不必要的區(qū)域或方向進行拓展，進一步提高了路徑搜索的實時性。

2.1 搜索空間表示

車輛在平面中的狀態(tài)可表示為(x,y,φ,d)，其中(x,y)表示車輛所處位置，φ表示航向角，d表示車輛行駛方向（d=0為前進，d=1為后退）。采用該種車輛狀態(tài)表達方式即創(chuàng)造了一個復雜的四維搜索空間，必須對其進行離散才能使用混合A*算法進行路徑搜索。具體計算方法為

式中：x——(x,y)連續(xù)車輛位置信息；——離散車輛位置信息；——離散車輛航向角信息；ξ——位置離散分辨率；γ——角度離散分辨率；o——選取的坐標原點。

2.2 子節(jié)點拓展方式

結合圖2所示的車輛運動學自行車模型，本文所采用的子節(jié)點拓展方式具體步驟如下：

圖2 車輛運動學模型Fig.2 Vehicle kinematics model

（1）確定前輪轉角離散數量N。為了保證子節(jié)點能朝車輛直行方向拓展，N必須為奇數。將前輪轉角范圍[-θmax,θmax]進行均勻離散，得到每個離散位置的前輪轉角為

（2）確定從每個離散前輪轉角處向前或向后拓展的子節(jié)點數量Mi。由于倒車行駛不符合人類駕駛習慣，故本文將向后拓展的節(jié)點數量Mi設置為1，保證每次向后拓展的距離較短；而對于向前拓展，為保證車輛盡可能地保持直行，故減少不必要的轉彎，子節(jié)點數量Mi可由式(4)計算得到：

（3）與傳統混合A*算法的單步拓展方式不同，改進混合A*算法從每個父節(jié)點Np(xp,yp,φp,dp)開始進行等步長分層拓展，各子節(jié)點Ni(xi,yi,φi,di)的計算公式如下：

式中：k——拓展層數，k= 1, 2, …,M；f——正負號標志，向前拓展時f取1，向后拓展時f取-1。

（4）對拓展得到的每個車輛位姿進行碰撞檢測，若車輛在該節(jié)點位姿處與障礙物碰撞，則停止在該前輪轉角處繼續(xù)向外層拓展節(jié)點，以保證各子節(jié)點與父節(jié)點之間的路徑無碰撞。

圖3示出選取N=7，9和11時的子節(jié)點拓展效果。由圖可知，采用此子節(jié)點拓展方式既能完成長距離的直線探索，又能保證在必要位置進行不同程度的轉彎。

圖3 節(jié)點拓展效果示意Fig.3 Schematic diagram of node expansion effect

2.3 啟發(fā)式函數設計

啟發(fā)式函數對于A*類算法至關重要，由啟發(fā)式函數計算得到的啟發(fā)值越接近實際值，路徑搜索效率則越高。傳統A*算法以歐式距離（即兩點間直線距離）或曼哈頓距離（即兩點在標準坐標系上的絕對軸距總和）作為啟發(fā)式函數，由于忽略了障礙物約束，導致對實際路徑成本估計不準確。本文采用Dijkstra算法以在二維障礙物地圖中計算得到的每個柵格點與終點的距離作為啟發(fā)值，如圖4（b）所示，顏色越深，表明啟發(fā)值越大，即離目標位置的距離越遠，相較于圖4（a）以歐式距離計算得到的啟發(fā)值，其更加符合實際情況。

圖4 不同啟發(fā)函數對比Fig.4 Comparison of different heuristic functions

2.4 代價函數設計

為了使搜索得到的路徑能讓車輛盡可能地保持直行，減少不必要的轉彎和后退，本文考慮在代價函數中加入對轉彎和后退節(jié)點的懲罰，各子節(jié)點Ni(xi,yi,φi,di)的實際代價計算公式如下：

式中：Gp——父節(jié)點Np的代價；Gi——子節(jié)點Ni的代價；α1,α2,α3——權重系數，可通過多次試驗后選取。

根據所設計的代價函數，通過g1,i對后退的搜索給予懲罰，并考慮了節(jié)點拓展時的步長代價，保證前向路徑搜索的優(yōu)先性；通過g2,i對路徑點航向角改變給予懲罰，保證了路徑平滑性；通過g3,i對路徑點方向切換給予懲罰，能夠有效避免路徑出現頻繁前進后退切換現象。

2.5 終點區(qū)域曲線擬合

由于采用2.2節(jié)所述的子節(jié)點拓展方式無法精確搜索到目標位姿，需要進行曲線擬合，故本文采用Reeds-Shepp曲線（簡稱“RS曲線”）[13]進行終點區(qū)域曲線擬合。如圖5所示，當選擇的待拓展節(jié)點距終點距離小于一定閾值時，以當前節(jié)點位姿作為起點位姿，使用RS曲線擬合到目標位姿；若擬合路徑碰撞檢測通過則停止搜索，否則繼續(xù)進行子節(jié)點拓展。

圖5 RS曲線終點擬合示意Fig.5 Endpoint fitting diagram of RS curve

3 路徑后處理方法

使用圖搜索方法搜索得到的路徑存在曲率不連續(xù)、包含不必要轉彎和路點間距不均勻的問題，本文對其做進一步的平滑與插值。首先采用梯度下降法對搜索得到的路徑進行非線性優(yōu)化，以提高路徑的平滑度，改善路徑曲率連續(xù)性；然后采用三次樣條曲線對優(yōu)化后的路徑做進一步的插值處理，得到稠密且均勻的路點，以滿足實際車輛控制需求。

3.1 路徑優(yōu)化

設混合A*搜索得到的初始路點序列為xo,j=(xj,yj),j∈[1,N]，優(yōu)化的初始解為xj=xo,j。固定起始點和目標點，對中間各點采用梯度下降法進行優(yōu)化，目標函數定義為

式中：Δxj——各路點之間的位移矢量，Δxj=xj-xj-1；——各路點之間切向角的變化量；α,β,γ——各項權重系數，可通過多次試驗選取。

目標函數的三項分別考慮了路徑平滑性、曲率約束和優(yōu)化路徑與原路徑的偏離程度，各項梯度的求解可參考文獻[12]。值得注意的是，采用本方法優(yōu)化得到的路徑可能會與障礙物發(fā)生碰撞。若出現這種情況，則參考初始路點xo,j，對優(yōu)化后發(fā)生碰撞的路點進行固定，并再次進行優(yōu)化處理，同時加大目標函數中的第三項權重系數，以減小優(yōu)化后路徑與原路徑的偏差，直到路徑碰撞檢測通過后停止。

圖6示出某非結構化道路場景下路徑優(yōu)化前后對比，圖7示出為對應路徑優(yōu)化前后曲率對比?？梢钥吹剑瑑?yōu)化后路徑具備更好的平滑性和曲率連續(xù)性，滿足車輛的實際行駛要求。

圖6 優(yōu)化前后路徑對比Fig.6 Contrast of paths before and after optimization

圖7 優(yōu)化前后曲率對比Fig.7 Contrast of curvatures before and after optimization

3.2 路徑插值

為了使優(yōu)化后的路點之間更加稠密，以滿足車輛控制需求，本文采用三次樣條曲線對路點進行插值。在此之前，由于路徑優(yōu)化過程導致搜索得到各路點航向角信息丟失，此時需要重新進行計算。如圖8所示，保持起點、終點和其他固定點的角度不變，中間點xj則是以前后兩點（xj-1和xj+1）連線方向與x軸正向的夾角作為該點的航向角φj。

圖8 路點航向角計算示意Fig.8 Diagram of road point heading angle calculation

得到路點航向角信息后，再結合每個路點的坐標信息，即可求得每兩個路點之間插值用三次樣條曲線的各項系數，繼而得到各路點之間的插值點坐標，具體求解方式可參考文獻[4]。插值前后路徑對比如圖9所示。

圖9 插值前后路徑對比Fig.9 Comparison of paths before and after interpolation

4 試驗驗證

通過仿真和實車試驗，驗證本文所提等步長分層拓展混合A*算法的有效性和實用性。首先，基于Matlab平臺搭建了若干個仿真場景，對本文所提方法與傳統混合A*算法進行了對比試驗，以驗證算法的優(yōu)越性。然后，基于機器人操作系統（robot operating system, ROS）進行C++算法開發(fā)，通過實車采集停車場地圖，得到實際的非結構化道路場景。在該場景下進行實車路徑規(guī)劃試驗，進一步驗證本文所提方法的可靠性。試驗相關參數見表1。

表1 試驗相關參數Tab.1 Related parameters of the experiment

4.1 仿真對比試驗

如圖10所示，在80 m×40 m的仿真地圖中對本文提出方法與傳統混合A*算法進行仿真對比試驗，試驗結果數據如表2所示。可以看出，本文方法相較于傳統混合A*算法可有效減少拓展節(jié)點數量，同時，closed集中存放的節(jié)點數量降低了一個數量級。根據A*算法原理可知，采用本文方法進行路徑搜索時的循環(huán)次數也降低了一個數量級，從而大大提高了路徑搜索效率，使算法具備更好的實時性。

圖10 路徑搜索結果對比Fig.10 Comparison of the path search results

表2 仿真試驗數據對比Tab.2 Comparison of simulation experiment data

由表2可知，采用本文所提改進算法較傳統混合A*算法，路徑搜索時間縮短了84.6 %。此外，除了圖10所示的試驗場景，本文還針對其他不同的場景進行了仿真對比試驗，試驗結果顯示平均路徑搜索時間縮短了51.6%，進一步驗證了該方法相較于傳統混合A*算法的優(yōu)越性。

4.2 實車試驗

為了進一步驗證所提出方法的可靠性，本文針對實際的非結構化道路場景進行了實車試驗。如圖11所示，本實驗所使用的試驗車輛為改造的乘用轎車，配備有激光雷達、毫米波雷達以及慣導等感知和定位設備。圖12（a）為測試的實際停車場場景（大小約為100 m×30 m），在該地圖場景中設置終點進行路徑規(guī)劃。圖12（c）為搜索得到的初始路徑，總拓展節(jié)點數量為551個；圖12（d）為優(yōu)化后的最終路徑，采用Intel i5-8300H型處理器進行計算，得到的路徑搜索與優(yōu)化總用時為37 ms。試驗結果表明，本文所提方法能夠在非結構化道路場景中以較短時間搜索得到一條安全、平滑且滿足車輛運動學約束的車輛可行駛路徑。

圖11 試驗車輛Fig.11 Test vehicle

圖12 實際場景實車試驗驗證Fig.12 Actual scene of experiment verification

5 結語

本文對非結構化道路智能車輛的路徑規(guī)劃方法進行了研究，提出了一種基于等步長分層拓展的混合A*路徑規(guī)劃方法。首先，通過改進傳統混合A*算法的子節(jié)點拓展方式，保證路徑安全性的同時提高了節(jié)點拓展效率；通過合理設計啟發(fā)式函數與代價函數，進一步減少了不必要的節(jié)點拓展。然后，采用梯度下降法對搜索得到路徑進行優(yōu)化處理，得到一條平滑舒適的車輛可行駛路徑。最后利用三次樣條曲線對稀疏路徑點進行插值，得到稠密且均勻的路點，以滿足實際車輛控制需求。仿真對比試驗和實車試驗結果表明，本文所提出的方法能顯著提高傳統A*搜索算法的搜索效率，同時保證了最終生成路徑的安全性、平滑性以及車輛的可行駛性。本文主要解決了非結構化道路場景下的智能車輛路徑規(guī)劃問題，但未對車輛沿路徑行駛時的速度進行規(guī)劃，因此，后續(xù)將對非結構化道路場景下的智能車輛速度規(guī)劃方法展開研究。