智能汽車泊車軌跡規劃應用

高 濤

（1.江蘇工程職業技術學院 航空與交通工程學院，江蘇 南通 226007；2.江蘇省智能網聯汽車工程技術研究開發中心，江蘇 南通 226007）

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

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

針對現有方法存在的問題，本文對Hybrid A*算法進行改進，優化啟發函數解決規劃路徑中的頻繁倒車與換向，考慮車輛安全，通過改進碰撞檢測算法，使規劃路徑中車輛輪廓與障礙物保持一定安全距離。通過仿真實驗，驗證了算法的有效性，改進后算法能夠實現智能汽車在局部泊車環境下的軌跡規劃。

1 Hybrid A＊算法

Hybrid A*算法融合A*算法和RS曲線考慮車輛動力學約束，搜索路徑滿足車輛非完整性約束。

Hybrid A*算法搜索的維數由（,）變為（,,,），其中（,,）為車輛位姿狀態，取±1，分別代表車輛正向行駛和倒車狀態。A*算法與Hybrid A*算法節點擴展方式如圖1所示，A*算法節點擴展為八鄰域方式，都是直線相連，子節點必定是柵格中心，Hybrid A*算法節點擴展與車輛行駛一致，即左轉前進、直行、右轉前進、左轉倒車、倒車、右轉倒車，6個方向來生成路徑，因此，路徑是受車輛轉彎半徑約束的圓弧和直線。

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

（1）初始化地圖和車輛信息：起始點、終點、障礙點、可行區域，車輛參數等；

（2）執行A*算法，計算柵格地圖下Costmap：利用A*算法，在柵格地圖中計算每個柵格到目標柵格的總代價，得到Costmap矩陣，用于指引Hybrid A*的擴展方向；

（3）執行Hybrid A*算法，搜索融合路徑：利用Hybrid A*算法、RS曲線作為Hybrid A*的啟發函數搜索泊車路徑。泊車路徑分兩段，一部分是擴展路徑，通過Costmap的指引搜出，一部分是RS曲線的路徑；

（4）生成路徑節點列表：將搜索出的節點信息記錄生成路徑節點列表信息（,,,）；

（5）路徑節點平滑處理：對生成的路徑節點進行平滑處理，得到滿足車輛行駛需要的平滑路徑。

2 啟發函數優化

啟發函數是影響Hybrid A*算法實時性與有效性的關鍵所在。為滿足車輛運行路徑最短且符合車輛動力學約束，在執行Hybrid A*算法搜索中，啟發函數以Euclidean距離為基礎，考慮實際行車環境增加相應的懲罰系數形成優化后的啟發函數。

在車輛實際行車環境中，不同的場景對車輛倒車與轉向行駛有不同的限制。例如，在城市快速路或高速公路禁止倒車，在停車場環境允許倒車，另外頻繁的轉向會增加車輛執行機構負擔，增大行車安全風險和影響乘坐舒適性。在執行Hybrid A*算法搜索中，在子節點擴展的代價計算時引入懲罰系數，根據不同場景的決策需求設置懲罰系數大小來限制車輛是否出現倒車，使規劃軌跡更符合實際場景。

從節點到擴展的代價計算式為

其中，norm（,）為節點到達擴展的Euclidean距離；為車輛行駛狀態參數，=1為正向行駛，=-1為倒車狀態；為汽車轉向角；為倒車行駛的懲罰系數；為車輛行駛方向改變的懲罰系數；為車輛轉向角懲罰系數；為轉向角改變懲罰系數。

由式（1）可知，=-1為倒車狀態時，倒車懲罰系數會引起代價增加，增大將減少倒車的發生，置為無窮大則禁止倒車；同理，車輛行駛方向改變的懲罰系數會調整車輛改變前進或倒車狀態的代價成本，避免車輛頻繁改變行駛方向；懲罰系數會因車輛轉彎產生轉向角調整代價；會調整車輛改變轉向角大小的代價成本，避免頻繁調整轉向角大小的問題。以上四個懲罰系數可根據實車狀態及柵格地圖狀態調試確定。

3 碰撞檢測策略改進

基于MATLAB2020b仿真平臺實施Hybrid A*算法，在進行路徑節點搜索中，為避免車輛與障礙碰撞，節點擴展的過程中需進行碰撞檢測，以確定車輛無碰撞安全行駛。碰撞檢測的原理示意如圖3所示，圖中矩形為車輛邊界的平面模型，多線段為障礙物邊界，車輛與障礙的碰撞檢測實際上轉化為一個簡單的數學問題，即判斷矩形與多線段是否存在交點，若存在即判定碰撞發生，若不存在則認為無碰撞。

算法在進行規劃的過程中，為了滿足路徑最短規劃軌跡，往往靠近障礙物，汽車實際行駛時需與障礙物保持一定距離以確保車輛安全無刮蹭。算法需要改進碰撞檢測策略，使車輛行駛過程中實時計算車輛輪廓與周圍障礙的最小距離。如何在不影響算法復雜程度情況下簡單實現車輛行駛中與障礙保持安全距離需要另辟蹊徑。在車輛進行碰撞檢測時，車輛平面模型外輪廓一周增加距離Safe dis，即安全距離。如圖4所示，虛線為增加安全距離Safe dis后的車輛平面模型。

4 Hybrid A＊算法仿真應用與分析

通過建模與仿真，對啟發函數優化及碰撞檢測策略的有效性、可靠性進行驗證。通過三個算例對典型的垂直倒車入庫、側方位停車和局部停車場環境進行停車軌跡規劃仿真。仿真規劃結果中粗實線包圍的白色區域為可行駛區域，矩形為車輛模型輪廓軌跡，三角形尖角指向車頭方向。生成車輛后軸中心軌跡線經擬合為粗實線顯示。車輛模型設定為車輛尺寸4.7 m×1.8 m，軸距2.73 m，車輛參數如表1所示。根據國家相關標準，垂直車庫尺寸6 m×2.6 m，平行車庫尺寸6 m×2.5 m。

4.1 垂直倒車入庫場景軌跡規劃算例

圖5為汽車完成倒車入庫典型場景軌跡規劃結果，表2為算例所使用的懲罰系數，仿真時安全距離Safe dis為0.1 m。

4.2 側方位停車場景軌跡規劃算例

如圖6所示為汽車完成側方位停車典型場景軌跡規劃結果，表2仍為算例所使用的懲罰系數，仿真時安全距離Safe dis為0.1 m。

4.3 停車場尋徑停車軌跡規劃算例

圖7為停車場典型場景下車輛尋徑規劃軌跡。表3為算例所使用的懲罰系數，為減少車輛倒車和頻繁改變方向，對各懲罰系數均進行設置，規劃路徑較為合理。仿真結果顯示，當安全距離Safe dis為0.15 m時車輛仍可有效安全地找到一條平滑的軌跡實現停車。

5 結論

停車場泊車環境下，智能汽車可以借助于Hybrid A*算法進行泊車軌跡規劃。基于Matlab 2020b平臺的Hybrid A*仿真算例分表明，在標準車位前或者局部停車場環境中，運用改進的啟發函數和碰撞檢測策略，智能汽車可實現安全停車。