基于有限狀態機的代客泊車決策規劃系統研究*

胡杰，劉昊巖，張敏超，張志豪，朱 琪，陳銳鵬，駱 嫚

（1.武漢理工大學，現代汽車零部件技術湖北省重點實驗室，武漢 430070；2.武漢理工大學，汽車零部件技術湖北省協同創新中心，武漢 430070；3.新能源與智能網聯汽車湖北工程技術中心，武漢 430070；4.東風悅享科技有限公司，武漢 430205）

前言

代客泊車系統旨在幫助駕駛員解決尋找車位以及泊車等問題［1-2］。駕駛員將車輛停泊在停車場外，在開啟代客泊車功能后，車輛自動行駛尋找并泊入車位，完成泊車流程。同時代客泊車系統應包括召回功能，車輛接收召回點信息，完成泊出與自動行駛過程，最終在召回點停止。代客泊車系統可以極大地減少駕駛員在停車過程中花費的時間，提高車輛使用效率。

目前對代客泊車系統的方案研究主要分為車端、場端和車場融合［3］。其中基于場端代客泊車方案通過安裝在停車場內部的傳感器向車端發送障礙物信息，以避免突然出現的障礙物造成事故，該方案常用于地下以及環境復雜的停車場。文獻［4］中基于場端基礎設施V2I 發送的地圖以及障礙物信息，進行實時全局路徑規劃，車輛按照全局路徑行駛，并使用速度規劃使車輛在到達泊車點時速度為零，隨后開始泊車，針對垂直車位，進行泊車路徑規劃，使用Prescan-Matlab 仿真驗證方案的可行性。然而在實際過程中，可能由于避障、定位與控制誤差等原因導致車輛不在泊車點，從而無法規劃出泊車路徑。文獻［5］中則根據場端提供的障礙物信息，不斷對行車全局路徑進行重規劃，并根據全局路徑將車輛引導至泊車點，到達泊車點后，使用圓弧+直線對車輛垂直泊車路徑規劃，完成代客泊車流程。由于泊車路徑使用圓弧進行規劃，因此對車輛起始位置要求較高，若圓弧半徑小于車輛最小轉彎半徑，則會造成車輛泊車失敗。同時基于場端方案對于停車場要求較高，增加了停車場修建與維護成本。

基于車端的方法是僅根據車載的激光雷達或其他傳感器對障礙物進行感知，并根據自車規劃結果完成代客泊車［6-10］。由于該方案成本較低，因此目前相關研究較多。文獻［6］和文獻［7］中基于全局規劃路徑進行局部路徑規劃，引導車輛到達車位附近，使用hp 維譜法與Dubins 曲線對斜車位與垂直車位進行規劃，該泊車規劃對車輛起始泊車點位具有一定的任意性，但該種方法并沒有考慮車輛在實際過程中的障礙物避障超車行為，且仍對車輛起始位姿具有一定要求。文獻［8］中使用全局路徑規劃引導車輛到達泊車點。行車過程中，設計停車場環境中的不同場景如避障、跟車、泊車等決策樹，根據車輛所處環境，對車輛行為進行規劃，到達泊車點后，針對垂直車位進行路徑規劃，最終使用實車驗證其可行性。

基于車場融合的方案則要求車端與場端均具有一定的感知、定位與規劃能力，其技術難度進一步提升，同時也大大增加車輛開發與停車場建設及維護的成本，因此，基于車場融合的方案研究較少且無商用案例。

綜上所述，目前代客泊車主流方案是基于車端，通過全局路徑規劃，引導車輛到達泊車點，進行泊車，最終完成整套流程。但該方案僅滿足代客泊車功能，不能完成召回功能。同時大部分研究對于車輛在行駛過程中遇到的障礙物以及其他突發情況并沒有進行考慮［5-10］，不滿足車輛實際行駛要求。其次，對于車輛在行車與泊車狀態切換的時機要求較高，在實際場景中，車輛在到達泊車點附近時，泊車點周圍可能會出現動態或靜態障礙物，使車輛產生避障等行為，從而導致泊車規劃起始位姿不確定，致使泊車規劃失敗。為解決以上問題，提高代客泊車系統的穩定性以及泊車成功率，本文中提出了一種基于有限狀態機的露天停車場代客泊車決策規劃方法，基于高精度地圖，通過Dijkstra 算法得到全局路徑與對應的參考線；使用分層有限狀態機建立代客泊車系統中車輛的各種狀態，并根據當前功能以及車輛所處環境與定義規則對車輛狀態進行切換；使用事件觸發的五次多項式對車輛的行車路徑進行規劃；同時采用多段曲線規劃泊車路徑，設計行車泊車切換區域，保證車輛在較大位姿范圍下可以規劃出符合車輛約束的路徑，提高代客泊車的成功率。

1 代客泊車系統架構

本文設計代客泊車系統功能包括代客泊車與定點召回，根據駕駛員需求，代客泊車系統以不同模式工作。使用分層有限狀態機，建立代客泊車系統整體架構。

為使用同一系統架構實現代客泊車與定點召回功能，使用有限狀態機設計各模塊，并根據不同情況對車輛狀態進行切換。采用分層狀態機結構，將代客泊車狀態機分為上層與下層狀態機，上層狀態機主要對代客泊車與定點召回功能中的行車和泊車狀態進行切換；下層狀態機包括行車與泊車狀態機，其中行車狀態引導車輛到達車位附近，泊車狀態則引導車輛泊入或泊出車位，同時對車輛行車與泊車過程中的行為進行決策。

1.1 上層狀態機設計

為實現代客泊車與定點召回功能，設計的代客泊車狀態框架如圖1 所示。其中車輛起始為初始狀態，隨后根據駕駛員選取的功能進入不同的狀態，具體流程如下。

圖1 代客泊車系統狀態機框架

（1）代客泊車模式 駕駛員選擇代客泊車功能，車輛在接收到信號后，從初始狀態跳轉至行車狀態；在車輛行駛至行車泊車切換區域后，車輛減速停止，隨后進入泊車狀態；在完成泊車后，車輛進入結束狀態，完成代客泊車流程。

（2）定點召回模式 駕駛員在遠程選擇定點召回功能并輸入召回點，車輛接收召回點信息后，從初始狀態跳轉至泊車狀態；完成泊出后，跳轉至行車狀態；行駛至召回點區域并完全停下后，車輛進入結束狀態，完成定點召回流程。

上層狀態機各狀態之間跳轉的事件定義如表1所示，其中相關區域判定將在第3節中展開敘述。

表1 上層狀態機事件表

1.2 行車狀態機設計

行車狀態是實現代客泊車功能與定點召回功能的重要模塊，用以引導車輛從起始位置行駛至車位附近。使用有限狀態機設計車輛在行車過程中的主要狀態。目前對于車輛行車過程狀態機研究較多［11-12］，但針對停車場復雜環境的研究較少，因此針對停車場場景，須設計對應的行車狀態機和相應的規則。

1.2.1 車輛環境建立

根據全局路徑以及感知系統得到的車輛周圍障礙物信息，首先對障礙物進行分類。為提高車輛的安全性且兼顧行駛效率，首先將自車與障礙物轉化至Frenet 坐標系下，Frenet 坐標系相比于全局坐標系，能夠更加準確獲取障礙物與自車的相對位置，防止在大曲率路徑中對障礙物進行誤判斷，提升行車效率。

在行車狀態下，自車制動、跟車、換道等行為判斷實際受自車周圍最近車輛的影響較大，而與自車相隔一輛或多輛汽車的車輛對自車的影響較小，因此，只考慮在自車道與左右相鄰車道的距離自車最近的障礙車輛或障礙物。在Frenet 坐標系下將自車道與左右相鄰車道劃分為6部分：左前、左后、前、后、右前、右后，對應區域內距離自車最近車輛作為障礙車，如圖2所示，分類結果如圖3所示。

圖2 障礙物相對位置判斷

圖3 Frenet坐標系下障礙物分類

將環境中障礙車按照該方式進行實時分類，可以有效簡化行車狀態所處理的信息量，為下一步行車狀態下的行為決策提供判斷準則。

1.2.2 行車行為決策

車輛處于停車場環境時，由于周圍環境較為復雜，因此對車輛行車要求較高。為提高車輛的響應速度，行車過程行為決策應在滿足車輛安全性的同時，保證一定效率，因此設計行車模塊主要行為如下：常規行駛、跟車行駛、行車制動以及換道行駛。行車模塊行為決策狀態轉移過程如圖4 所示，行車事件如表2所示。

圖4 行車狀態轉移圖

表2中各事件的真值判斷如下。

DE1：前車在跟車范圍內時，事件DE1 為真，跟車范圍計算公式為

式中：τ為車間時距，s；v0為自車車速，m/s；D0為補償距離，m。

DE2：關于前車是否處于制動范圍，使用TTC 時間進行判斷，其計算公式為

式中：vego為自車車速；vtarget為目標車車速；starget為目標車在Frenet 坐標系下對應s值；sego為自車在Frenet坐標系下對應s值。當TTCFront＞0 且TTCFront≤TTCT（TTCFront為前車TTC，TTCT為TTC 安全閾值）或與自車車距小于最短規劃距離時，事件DE2為真。

DE3：換道需求分為兩類。第1 類為主動換道，在車輛當前行駛車道與目標車道不一致時觸發，目標車道的選取根據目標車位與召回點所處位置進行選取。第2 類為被動換道，當自車前方跟車距離內存在速度較慢或0 ＜TTCFront≤TTCT的障礙物，車輛同樣存在換道需求。

DE4：當車輛與目標車道的橫向偏差小于距離偏差閾值且航向偏差小于航向偏差閾值時，事件DE4為真。

DE5：以左車道為例，當左車道前后車輛皆滿足TTC＞TTCT，同時前后車輛與自車車距大于最小規劃距離，判定左車道允許換道。

DE6：目標區域根據車輛功能分為行車泊車切換區域與召回點區域，當車輛車頭進入目標區域時，則判定車輛進入目標區域，關于目標區域的設計在第3節詳細闡述。

根據以上車輛行為和對應事件真值，對車輛行車過程中行為進行決策。本文中設計車輛行車狀態機，保證車輛在自車道行駛過程中的安全，同時在前車速度較慢且旁側車道安全情況下，允許車輛換道，使車輛在行車過程兼顧安全與效率。

1.3 泊車狀態機設計

為滿足車輛在泊車過程中的安全，設計車輛泊車狀態機轉移圖，如圖5 所示，泊車事件表如表3所示。

圖5 泊車狀態轉移圖

表3 泊車狀態事件

關于車輛碰撞風險（BE1）的判斷，將未來規劃時間τp內，車輛輪廓在規劃路徑下掃掠所得到的區域作為障礙物風險區域，如圖6 所示。在該區域內存在障礙物時，事件BE1置為1，反之置為0。

圖6 泊車風險區域

2 全局路徑規劃

本文對代客泊車系統的研究是基于高精度地圖和全局路徑中的參考線，因此獲取一條準確、高效到達目標車位的全局路徑是系統的基礎。目前常用基于搜索或基于勢場的方法求解最優全局路徑［5-8］。代客泊車中全局路徑作用主要分為兩種，第1 種直接使用全局路徑引導車輛到達車位附近，第2 種是利用全局路徑作為參考線，進行局部路徑規劃。第1種方案較為簡便，但路徑固定。而第2種方案可更好地適應停車場復雜多變的環境，實現對障礙物的規避。

利用Dijkstra 算法，根據高精度地圖所提供停車場道路帶權有向圖，求解出車輛起始點至目標點的最短路徑道路ID，經地圖引擎將結果發送至車輛，為之后的局部路徑規劃提供基礎，規劃結果如圖7所示。

圖7 全局路徑規劃

3 局部路徑規劃

3.1 行車路徑規劃

狀態柵格法是一種基于采樣和曲線插值的路徑規劃方法［13］。該方法具有采樣法和插值法的優勢，能夠快速應對動態環境的變化，同時計算負載小于基于搜索或優化的方法，在結構化道路下的自動駕駛車輛避障及換道規劃上得到了大量應用［14］。考慮到本文研究的代客泊車系統場景為具有結構化道路的停車場，同時為降低計算平臺算力負擔，采用文獻［15］中基于事件觸發的狀態柵格法對車輛行車軌跡進行規劃，車輛某時刻規劃路徑如圖8所示。

圖8 行車局部路徑規劃

3.2 泊入路徑規劃

代客泊車的重點在于行車與泊車的切換以及對應車位的泊入路徑規劃。目前常用泊車規劃方法有幾何法、采樣法以及優化法［16-18］。由于車輛在從行車狀態跳轉至泊車狀態時，采用傳統方法對車輛泊車路徑規劃的初始位姿要求較高，同時受環境影響，到達泊車點之前，車輛在行車中的具體狀態無法確定，可能處于換道和避障等過程中。除此以外，定位與控制過程中存在誤差，導致車輛與理想位姿差異較大。為減小車輛對起始位姿的要求，使用采樣法與幾何法相結合，針對垂直車位，使用多段泊車路徑規劃。車輛在行車狀態下進入行車泊車切換區域后，進入行車狀態中制動模式，直至減速為零。若此時車輛仍位于切換區域，車輛進入泊車狀態。對于泊入路徑規劃，使用倒推法，針對垂直車位，首先確定車輛泊入段路徑，隨后在此基礎上，對車輛前進段路徑進行規劃。具體流程如圖9所示。

圖9 泊車路徑規劃流程

3.2.1 前進段路徑規劃

前進段路徑作用是連接車輛行車狀態結束點P1與泊入段路徑起始點P2。為保證規劃路徑滿足起始與目標位置的車輛后軸中心坐標、航向以及曲率約束，采用五次多項式連接P1與P2。為求解五次多項式中的系數，使用P1（x0，y0，θ0，k0）以及P2（x1，y1，θ1，k1）作為約束條件，即起止位置車輛后軸中心點的x、y坐標，車輛航向角θ以及曲率k，多項式系數使用式（3）求解：

3.2.2 泊入段路徑規劃

車輛由P2點規劃泊入段路徑時，采用圓弧+直線的方法，為保證泊入路徑的曲率連續性，且滿足車輛運動學要求與道路邊界條件，使用回旋曲線連接圓弧與直線。首先以車輛在車位中的理想停泊點為起始點，以車輛最小轉彎半徑以及道路邊界作為約束條件，使用不同轉彎半徑與圓弧角度對圓弧段路徑進行規劃，依據路徑的曲率連續條件，得到對應的回旋曲線路徑、直線段路徑以及有關P2點的集合。為保證路徑連續性，以車輛當前位置點作為P1，使用五次多項式分別連接P1與P2點集合中每一點，得到前進段路徑簇。

最終以路徑長度、最大曲率作為成本項建立路徑成本函數，計算不同路徑成本。在滿足道路約束與車輛運動學約束條件情況下，選取最低成本路徑作為最優泊入路徑，如圖10 所示。前進與泊入段速度規劃方法則使用文獻［17］中的方法。

圖10 泊入路徑規劃示意圖

3.2.3 行車泊車切換區域設計

由于停車場環境復雜多變，車輛在跳轉出行車狀態前可能處于換道或跟車等不確定狀態，導致車輛泊車起始位姿難以確定，因此使用前進段路徑消除起始位姿對泊入路徑規劃的影響。而行車泊車切換區域位置對前進段路徑曲率產生較大影響，故須保證車輛在區域中規劃出的前進段路徑滿足車輛運動學約束以及道路邊界約束。

為保證車輛在行駛至車位附近后，在較大位姿范圍內均可規劃出滿足車輛的運動學以及道路邊界約束的前進段路徑。根據車輛進入切換區域后的航向，分為平行于道路以及與道路存在一定夾角兩種情況。在不同位姿的情況下，對切換區域進行設計，使車輛在限定的航向與位置下，在切換區域中均可規劃出滿足要求的前進段路徑。

（1）車輛航向平行于道路

當車輛行駛到達車位附近后，前方不存在障礙物，車輛無須進行換道或避障等行為，以平行于道路航向行駛。此時，根據上述垂直車位泊入路徑規劃方法，首先為防止車輛起始位置超出道路邊界，須得到車輛在道路的上下起始極限位置。為保證車輛在平行于道路航向下車身不離開道路，車輛上下極限位置應為道路對應y值分別加減w/2，其中w為車身寬度，在以車位上邊界中心位置為原點的坐標系下，上下極限位置如圖11所示。

圖11 平行航向車輛上下極限位置

根據泊入段路徑簇中P2點的集合以及五次多項式的性質，為保證前進段路徑滿足車輛運動學約束，須保證前進段路徑最大曲率kmax小于車輛運動學允許最大曲率。在平行航向上下極限位置中縱向間隔0.5 m 進行采樣，以初始位置為基準，得到不同y值下，能夠規劃出滿足車輛運動學要求以及道路約束條件下的最大x值，最終得到在平行于道路航向的情況下，不同縱向位置所允許的右極限位置，如圖12所示。

圖12 平行航向行車泊車切換區域示意圖

切換區域長度Lp通過式（4）進行計算：

式中：vmax為停車場允許最大車速，m/s；amax為車輛最大減速度，m/s2；La為補償距離。

（2）車輛與道路存在夾角

車輛在進入行車泊車切換區域前，可能由于障礙物等因素，處于換道避障等狀態，導致進入切換區域后，車輛與道路產生一定角度，為保證能夠規劃出滿足要求的前進段路徑，須根據車輛航向對切換區域進行研究。首先計算出在滿足道路約束的條件下，車輛相對道路允許的最大航向角。在上述坐標系下，根據車輛最小轉彎半徑，以逆時針為正，得到車輛所允許的最大正負航向αp與αn，如圖13 所示。同時可得車輛后軸中心在不同縱向道路位置下，車輛允許的極限航向，如圖14 所示。在相應縱向位置的極限航向下，車輛以最小轉彎半徑轉向，仍可以保持在道路范圍內。

圖13 車輛相對道路最大正負航向

圖14 車輛極限航向與縱向位置關系圖

根據上述車輛在不同位置下的允許極限航向，以車輛平行于道路航向所得到的右極限位置作為基準，按照圖15 所示流程（Δxi為橫向采樣距離），得到對應縱向采樣點下右極限x值，由此得到在正負航向下滿足運動學條件的前進段路徑的右極限邊界，如圖16和圖17所示，其中切換區域長度見式（4）。

圖15 正負航向行車泊車切換區域設計流程

圖16 正航向行車泊車切換區域示意圖

圖17 負航向行車泊車切換區域示意圖

根據上述車輛在平行于道路以及與道路有正負夾角的情況下分別得到的行車泊車切換區域，將3種情況下的右極限位置取最小值，得到行車泊車切換區域。在該區域內，車輛在滿足道路約束的極限航向下，均可規劃出前進段路徑，引導車輛到達泊入段路徑起始點，順利泊入目標車位。

當車輛進入切換區域，事件DE6 置為1，車輛進入行車制動狀態；完全停下后，事件E2 為1，跳轉進入泊車狀態，根據規劃出的前進路段與泊入路段泊入車位；完成泊入后，事件E4置為1，進入結束狀態，完成代客泊車流程。

3.3 泊出路徑規劃

泊出路徑規劃主要應用在定點召回模式下。根據召回點與當前車位的關系，分為左泊出與右泊出，保證車輛在完成泊出后車輛航向角與召回點方向一致。車輛泊出路徑規劃方法與泊入狀態中泊入段路徑規劃方法一致，將規劃起止點對調即可，在此不再贅述。完成泊出后，事件E5置為1，車輛由泊車狀態跳轉進入行車狀態。為保證車輛到達召回點準確停止，需要對召回點區域進行設計。其中召回點區域右邊界為召回點對應x值，其長度選取與行車泊車切換區域方法一致，寬度為道路寬度。泊出最優路徑規劃與召回點區域如圖18所示。

圖18 泊出路徑規劃與召回點區域

當車輛進入召回點區域時，車輛跳轉進入行車制動狀態，減速停止后，事件E3 置為1，車輛由行車跳轉至結束狀態，完成定點召回流程。

4 實車試驗

4.1 試驗平臺介紹

采用東風悅享Sharing Van 1.0 Plus 公開道路版作為試驗平臺，該車搭載4 枚激光雷達以及GNSS、IMU 等定位設備，配備線控底盤，實現對前輪轉角、加速與制動踏板的精確控制，滿足代客泊車的試驗要求。車輛具體參數如表4 所示。采用文獻［19］中的方法，對行車與泊車過程中軌跡進行跟蹤。

表4 Sharing Van 1.0 Plus基本參數

4.2 實車試驗

4.2.1 代客泊車試驗

車輛起始停泊在停車場入口處，在起始位置與目標車位之間，設置相應靜態與動態障礙物，隨后輸入目標車位信息，起動車輛。車輛在行車過程中順利完成避障，在到達泊車區域后，制動減速至0，進入泊入狀態，并順利完成垂直車位的泊入過程，如圖19所示。對應局部路徑規劃如圖20所示，泊車過程中車輛速度規劃如圖21所示。

圖19 代客泊車實車試驗

圖20 代客泊車路徑規劃結果

圖21 泊入狀態速度規劃

4.2.2 定點召回試驗

車輛起始停泊在垂直車位中，起動車輛，車輛完成泊出過程，隨后進入行車狀態，在到達召回區域后，車輛制動至停止，停泊在召回點前，完成定點召回功能，如圖22所示。

圖22 定點召回實車試驗

4.2.3 試驗結果分析

試驗過程中，車輛狀態與試驗時間的關系如圖23所示。代客泊車過程中，在1 s時車輛由初始狀態進入行車狀態中常規行駛，在22 s 到達行車泊車切換區域進行制動，減速至靜止后，于30 s跳轉至泊車狀態中初始狀態，并隨后進入泊入狀態，在67 s完成泊入，進入結束狀態，完成代客泊車過程。

圖23 車輛狀態時間圖

定點召回試驗中，車輛在1 s時由初始狀態進入泊車狀態中泊車初始狀態，隨后進入泊出狀態，完成泊出后，在28 s跳轉進入行車狀態中常規行駛狀態，直至74 s 到達召回點區域進行制動，在82 s 時停止并進入結束狀態，完成召回過程。

5 結論

首先使用分層有限狀態機模型設計了代客泊車決策系統，分別設計代客泊車整體狀態機以及行車與泊車的行為決策狀態機；隨后基于高精度地圖，使用Dijkstra算法得到全局路徑；使用基于采樣的五次多項式對車輛行車軌跡進行規劃；采用多段泊車軌跡規劃，并設計車輛行車泊車切換區域，使泊車路徑規劃對起始位姿依賴較小，保證車輛順利泊入車位，完成代客泊車功能。同時對車輛泊出路徑以及召回點區域進行設計，實現定點召回功能。

最終對該代客泊車方案進行實車試驗。試驗結果表明，所設計代客泊車方案能順利完成車輛代客泊車以及定點召回功能。相比于傳統代客泊車系統，本文提出的方案能夠顯著降低代客泊車中泊車對起始位姿的要求，將傳統泊車點擴展為一較大的泊車區域，提高代客泊車成功率。該方案可以較好地移植至基于車端或場端的代客泊車系統中。