一種基于等步長分層拓展的混合A*路徑規(guī)劃方法

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

（湖南大學(xué) 機(jī)械與運(yùn)載工程學(xué)院，湖南 長沙 410082）

0 引言

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

1 混合A*算法

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

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

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

2 改進(jìn)混合A*算法

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

2.1 搜索空間表示

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

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

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

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

圖2 車輛運(yùn)動(dòng)學(xué)模型Fig.2 Vehicle kinematics model

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

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

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

式中：k——拓展層數(shù)，k= 1, 2, …,M；f——正負(fù)號標(biāo)志，向前拓展時(shí)f取1，向后拓展時(shí)f取-1。

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

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

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

2.3 啟發(fā)式函數(shù)設(shè)計(jì)

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

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

2.4 代價(jià)函數(shù)設(shè)計(jì)

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

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

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

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

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

圖5 RS曲線終點(diǎn)擬合示意Fig.5 Endpoint fitting diagram of RS curve

3 路徑后處理方法

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

3.1 路徑優(yōu)化

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

式中：Δxj——各路點(diǎn)之間的位移矢量，Δxj=xj-xj-1；——各路點(diǎn)之間切向角的變化量；α,β,γ——各項(xiàng)權(quán)重系數(shù)，可通過多次試驗(yàn)選取。

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

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

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

圖8 路點(diǎn)航向角計(jì)算示意Fig.8 Diagram of road point heading angle calculation

得到路點(diǎn)航向角信息后，再結(jié)合每個(gè)路點(diǎn)的坐標(biāo)信息，即可求得每兩個(gè)路點(diǎn)之間插值用三次樣條曲線的各項(xiàng)系數(shù)，繼而得到各路點(diǎn)之間的插值點(diǎn)坐標(biāo)，具體求解方式可參考文獻(xiàn)[4]。插值前后路徑對比如圖9所示。

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

4 試驗(yàn)驗(yàn)證

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

表1 試驗(yàn)相關(guān)參數(shù)Tab.1 Related parameters of the experiment

4.1 仿真對比試驗(yàn)

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

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

表2 仿真試驗(yàn)數(shù)據(jù)對比Tab.2 Comparison of simulation experiment data

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

4.2 實(shí)車試驗(yàn)

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

圖11 試驗(yàn)車輛Fig.11 Test vehicle

圖12 實(shí)際場景實(shí)車試驗(yàn)驗(yàn)證Fig.12 Actual scene of experiment verification

5 結(jié)語

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