智能汽車(chē)泊車(chē)軌跡規(guī)劃應(yīng)用

高 濤

（1.江蘇工程職業(yè)技術(shù)學(xué)院 航空與交通工程學(xué)院，江蘇 南通 226007；2.江蘇省智能網(wǎng)聯(lián)汽車(chē)工程技術(shù)研究開(kāi)發(fā)中心，江蘇 南通 226007）

城市化發(fā)展過(guò)程中，交通擁堵、事故率高、環(huán)境污染和能源短缺等因素制約著城市的快速發(fā)展，而自動(dòng)駕駛技術(shù)被普遍認(rèn)為是破解這些難題的關(guān)鍵。自動(dòng)駕駛技術(shù)四大核心技術(shù)分別是環(huán)境感知、精確定位、路徑規(guī)劃、線控執(zhí)行。路徑規(guī)劃在智能汽車(chē)運(yùn)動(dòng)控制中占據(jù)核心地位，路徑規(guī)劃算法的效率將直接影響智能汽車(chē)的尋路效率及實(shí)施規(guī)劃能力。其中，運(yùn)動(dòng)軌跡規(guī)劃是對(duì)車(chē)輛運(yùn)行軌跡進(jìn)行預(yù)測(cè)規(guī)劃。

現(xiàn)階段大量研究主要集中在基于采樣的方法、基于搜索的方法和基于生物啟發(fā)的方法等方向。基于圖搜索的A*算法能求解最優(yōu)路徑且實(shí)時(shí)性好，但傳統(tǒng)圖搜索算法得到的路徑不能滿足車(chē)輛非完整性約束。針對(duì)該問(wèn)題，DOLGOV等提出一種滿足車(chē)輛運(yùn)動(dòng)學(xué)約束的混合A*（Hybrid A*）路徑搜索算法；HU等使用混合A*算法作為路徑搜索方法、Pythagorean-Hodograph三次曲線作為路徑后處理方法，但是路徑不能與障礙保持足夠的安全距離；文獻(xiàn)[6]使用A*搜索結(jié)果與路線規(guī)劃算法（Reeds-Shepp, RS）共同作為啟發(fā)式數(shù)值，并使用Reeds-Shepp曲線進(jìn)行節(jié)點(diǎn)擴(kuò)展，加快了搜索的速度，但容易導(dǎo)致路徑中頻繁出現(xiàn)倒車(chē)、變換行駛方向等不合理的情況出現(xiàn)。

針對(duì)現(xiàn)有方法存在的問(wèn)題，本文對(duì)Hybrid A*算法進(jìn)行改進(jìn)，優(yōu)化啟發(fā)函數(shù)解決規(guī)劃路徑中的頻繁倒車(chē)與換向，考慮車(chē)輛安全，通過(guò)改進(jìn)碰撞檢測(cè)算法，使規(guī)劃路徑中車(chē)輛輪廓與障礙物保持一定安全距離。通過(guò)仿真實(shí)驗(yàn)，驗(yàn)證了算法的有效性，改進(jìn)后算法能夠?qū)崿F(xiàn)智能汽車(chē)在局部泊車(chē)環(huán)境下的軌跡規(guī)劃。

1 Hybrid A＊算法

Hybrid A*算法融合A*算法和RS曲線考慮車(chē)輛動(dòng)力學(xué)約束，搜索路徑滿足車(chē)輛非完整性約束。

Hybrid A*算法搜索的維數(shù)由（,）變?yōu)椋?span id="j5i0abt0b" class="emphasis_bold">,,,），其中（,,）為車(chē)輛位姿狀態(tài)，取±1，分別代表車(chē)輛正向行駛和倒車(chē)狀態(tài)。A*算法與Hybrid A*算法節(jié)點(diǎn)擴(kuò)展方式如圖1所示，A*算法節(jié)點(diǎn)擴(kuò)展為八鄰域方式，都是直線相連，子節(jié)點(diǎn)必定是柵格中心，Hybrid A*算法節(jié)點(diǎn)擴(kuò)展與車(chē)輛行駛一致，即左轉(zhuǎn)前進(jìn)、直行、右轉(zhuǎn)前進(jìn)、左轉(zhuǎn)倒車(chē)、倒車(chē)、右轉(zhuǎn)倒車(chē)，6個(gè)方向來(lái)生成路徑，因此，路徑是受車(chē)輛轉(zhuǎn)彎半徑約束的圓弧和直線。

Hybrid A*算法流程如圖2所示，具體步驟如下：

（1）初始化地圖和車(chē)輛信息：起始點(diǎn)、終點(diǎn)、障礙點(diǎn)、可行區(qū)域，車(chē)輛參數(shù)等；

（2）執(zhí)行A*算法，計(jì)算柵格地圖下Costmap：利用A*算法，在柵格地圖中計(jì)算每個(gè)柵格到目標(biāo)柵格的總代價(jià)，得到Costmap矩陣，用于指引Hybrid A*的擴(kuò)展方向；

（3）執(zhí)行Hybrid A*算法，搜索融合路徑：利用Hybrid A*算法、RS曲線作為Hybrid A*的啟發(fā)函數(shù)搜索泊車(chē)路徑。泊車(chē)路徑分兩段，一部分是擴(kuò)展路徑，通過(guò)Costmap的指引搜出，一部分是RS曲線的路徑；

（4）生成路徑節(jié)點(diǎn)列表：將搜索出的節(jié)點(diǎn)信息記錄生成路徑節(jié)點(diǎn)列表信息（,,,）；

（5）路徑節(jié)點(diǎn)平滑處理：對(duì)生成的路徑節(jié)點(diǎn)進(jìn)行平滑處理，得到滿足車(chē)輛行駛需要的平滑路徑。

2 啟發(fā)函數(shù)優(yōu)化

啟發(fā)函數(shù)是影響Hybrid A*算法實(shí)時(shí)性與有效性的關(guān)鍵所在。為滿足車(chē)輛運(yùn)行路徑最短且符合車(chē)輛動(dòng)力學(xué)約束，在執(zhí)行Hybrid A*算法搜索中，啟發(fā)函數(shù)以Euclidean距離為基礎(chǔ)，考慮實(shí)際行車(chē)環(huán)境增加相應(yīng)的懲罰系數(shù)形成優(yōu)化后的啟發(fā)函數(shù)。

在車(chē)輛實(shí)際行車(chē)環(huán)境中，不同的場(chǎng)景對(duì)車(chē)輛倒車(chē)與轉(zhuǎn)向行駛有不同的限制。例如，在城市快速路或高速公路禁止倒車(chē)，在停車(chē)場(chǎng)環(huán)境允許倒車(chē)，另外頻繁的轉(zhuǎn)向會(huì)增加車(chē)輛執(zhí)行機(jī)構(gòu)負(fù)擔(dān)，增大行車(chē)安全風(fēng)險(xiǎn)和影響乘坐舒適性。在執(zhí)行Hybrid A*算法搜索中，在子節(jié)點(diǎn)擴(kuò)展的代價(jià)計(jì)算時(shí)引入懲罰系數(shù)，根據(jù)不同場(chǎng)景的決策需求設(shè)置懲罰系數(shù)大小來(lái)限制車(chē)輛是否出現(xiàn)倒車(chē)，使規(guī)劃軌跡更符合實(shí)際場(chǎng)景。

從節(jié)點(diǎn)到擴(kuò)展的代價(jià)計(jì)算式為

其中，norm（,）為節(jié)點(diǎn)到達(dá)擴(kuò)展的Euclidean距離；為車(chē)輛行駛狀態(tài)參數(shù)，=1為正向行駛，=-1為倒車(chē)狀態(tài)；為汽車(chē)轉(zhuǎn)向角；為倒車(chē)行駛的懲罰系數(shù)；為車(chē)輛行駛方向改變的懲罰系數(shù)；為車(chē)輛轉(zhuǎn)向角懲罰系數(shù)；為轉(zhuǎn)向角改變懲罰系數(shù)。

由式（1）可知，=-1為倒車(chē)狀態(tài)時(shí)，倒車(chē)懲罰系數(shù)會(huì)引起代價(jià)增加，增大將減少倒車(chē)的發(fā)生，置為無(wú)窮大則禁止倒車(chē)；同理，車(chē)輛行駛方向改變的懲罰系數(shù)會(huì)調(diào)整車(chē)輛改變前進(jìn)或倒車(chē)狀態(tài)的代價(jià)成本，避免車(chē)輛頻繁改變行駛方向；懲罰系數(shù)會(huì)因車(chē)輛轉(zhuǎn)彎產(chǎn)生轉(zhuǎn)向角調(diào)整代價(jià)；會(huì)調(diào)整車(chē)輛改變轉(zhuǎn)向角大小的代價(jià)成本，避免頻繁調(diào)整轉(zhuǎn)向角大小的問(wèn)題。以上四個(gè)懲罰系數(shù)可根據(jù)實(shí)車(chē)狀態(tài)及柵格地圖狀態(tài)調(diào)試確定。

3 碰撞檢測(cè)策略改進(jìn)

基于MATLAB2020b仿真平臺(tái)實(shí)施Hybrid A*算法，在進(jìn)行路徑節(jié)點(diǎn)搜索中，為避免車(chē)輛與障礙碰撞，節(jié)點(diǎn)擴(kuò)展的過(guò)程中需進(jìn)行碰撞檢測(cè)，以確定車(chē)輛無(wú)碰撞安全行駛。碰撞檢測(cè)的原理示意如圖3所示，圖中矩形為車(chē)輛邊界的平面模型，多線段為障礙物邊界，車(chē)輛與障礙的碰撞檢測(cè)實(shí)際上轉(zhuǎn)化為一個(gè)簡(jiǎn)單的數(shù)學(xué)問(wèn)題，即判斷矩形與多線段是否存在交點(diǎn)，若存在即判定碰撞發(fā)生，若不存在則認(rèn)為無(wú)碰撞。

算法在進(jìn)行規(guī)劃的過(guò)程中，為了滿足路徑最短規(guī)劃軌跡，往往靠近障礙物，汽車(chē)實(shí)際行駛時(shí)需與障礙物保持一定距離以確保車(chē)輛安全無(wú)刮蹭。算法需要改進(jìn)碰撞檢測(cè)策略，使車(chē)輛行駛過(guò)程中實(shí)時(shí)計(jì)算車(chē)輛輪廓與周?chē)系K的最小距離。如何在不影響算法復(fù)雜程度情況下簡(jiǎn)單實(shí)現(xiàn)車(chē)輛行駛中與障礙保持安全距離需要另辟蹊徑。在車(chē)輛進(jìn)行碰撞檢測(cè)時(shí)，車(chē)輛平面模型外輪廓一周增加距離Safe dis，即安全距離。如圖4所示，虛線為增加安全距離Safe dis后的車(chē)輛平面模型。

4 Hybrid A＊算法仿真應(yīng)用與分析

通過(guò)建模與仿真，對(duì)啟發(fā)函數(shù)優(yōu)化及碰撞檢測(cè)策略的有效性、可靠性進(jìn)行驗(yàn)證。通過(guò)三個(gè)算例對(duì)典型的垂直倒車(chē)入庫(kù)、側(cè)方位停車(chē)和局部停車(chē)場(chǎng)環(huán)境進(jìn)行停車(chē)軌跡規(guī)劃仿真。仿真規(guī)劃結(jié)果中粗實(shí)線包圍的白色區(qū)域?yàn)榭尚旭倕^(qū)域，矩形為車(chē)輛模型輪廓軌跡，三角形尖角指向車(chē)頭方向。生成車(chē)輛后軸中心軌跡線經(jīng)擬合為粗實(shí)線顯示。車(chē)輛模型設(shè)定為車(chē)輛尺寸4.7 m×1.8 m，軸距2.73 m，車(chē)輛參數(shù)如表1所示。根據(jù)國(guó)家相關(guān)標(biāo)準(zhǔn)，垂直車(chē)庫(kù)尺寸6 m×2.6 m，平行車(chē)庫(kù)尺寸6 m×2.5 m。

4.1 垂直倒車(chē)入庫(kù)場(chǎng)景軌跡規(guī)劃算例

圖5為汽車(chē)完成倒車(chē)入庫(kù)典型場(chǎng)景軌跡規(guī)劃結(jié)果，表2為算例所使用的懲罰系數(shù)，仿真時(shí)安全距離Safe dis為0.1 m。

4.2 側(cè)方位停車(chē)場(chǎng)景軌跡規(guī)劃算例

如圖6所示為汽車(chē)完成側(cè)方位停車(chē)典型場(chǎng)景軌跡規(guī)劃結(jié)果，表2仍為算例所使用的懲罰系數(shù)，仿真時(shí)安全距離Safe dis為0.1 m。

4.3 停車(chē)場(chǎng)尋徑停車(chē)軌跡規(guī)劃算例

圖7為停車(chē)場(chǎng)典型場(chǎng)景下車(chē)輛尋徑規(guī)劃軌跡。表3為算例所使用的懲罰系數(shù)，為減少車(chē)輛倒車(chē)和頻繁改變方向，對(duì)各懲罰系數(shù)均進(jìn)行設(shè)置，規(guī)劃路徑較為合理。仿真結(jié)果顯示，當(dāng)安全距離Safe dis為0.15 m時(shí)車(chē)輛仍可有效安全地找到一條平滑的軌跡實(shí)現(xiàn)停車(chē)。

5 結(jié)論

停車(chē)場(chǎng)泊車(chē)環(huán)境下，智能汽車(chē)可以借助于Hybrid A*算法進(jìn)行泊車(chē)軌跡規(guī)劃。基于Matlab 2020b平臺(tái)的Hybrid A*仿真算例分表明，在標(biāo)準(zhǔn)車(chē)位前或者局部停車(chē)場(chǎng)環(huán)境中，運(yùn)用改進(jìn)的啟發(fā)函數(shù)和碰撞檢測(cè)策略，智能汽車(chē)可實(shí)現(xiàn)安全停車(chē)。