基于有限狀態(tài)機(jī)的代客泊車決策規(guī)劃系統(tǒng)研究*

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

（1.武漢理工大學(xué)，現(xiàn)代汽車零部件技術(shù)湖北省重點(diǎn)實(shí)驗(yàn)室，武漢 430070；2.武漢理工大學(xué)，汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070；3.新能源與智能網(wǎng)聯(lián)汽車湖北工程技術(shù)中心，武漢 430070；4.東風(fēng)悅享科技有限公司，武漢 430205）

前言

代客泊車系統(tǒng)旨在幫助駕駛員解決尋找車位以及泊車等問(wèn)題［1-2］。駕駛員將車輛停泊在停車場(chǎng)外，在開(kāi)啟代客泊車功能后，車輛自動(dòng)行駛尋找并泊入車位，完成泊車流程。同時(shí)代客泊車系統(tǒng)應(yīng)包括召回功能，車輛接收召回點(diǎn)信息，完成泊出與自動(dòng)行駛過(guò)程，最終在召回點(diǎn)停止。代客泊車系統(tǒng)可以極大地減少駕駛員在停車過(guò)程中花費(fèi)的時(shí)間，提高車輛使用效率。

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

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

基于車場(chǎng)融合的方案則要求車端與場(chǎng)端均具有一定的感知、定位與規(guī)劃能力，其技術(shù)難度進(jìn)一步提升，同時(shí)也大大增加車輛開(kāi)發(fā)與停車場(chǎng)建設(shè)及維護(hù)的成本，因此，基于車場(chǎng)融合的方案研究較少且無(wú)商用案例。

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

1 代客泊車系統(tǒng)架構(gòu)

本文設(shè)計(jì)代客泊車系統(tǒng)功能包括代客泊車與定點(diǎn)召回，根據(jù)駕駛員需求，代客泊車系統(tǒng)以不同模式工作。使用分層有限狀態(tài)機(jī)，建立代客泊車系統(tǒng)整體架構(gòu)。

為使用同一系統(tǒng)架構(gòu)實(shí)現(xiàn)代客泊車與定點(diǎn)召回功能，使用有限狀態(tài)機(jī)設(shè)計(jì)各模塊，并根據(jù)不同情況對(duì)車輛狀態(tài)進(jìn)行切換。采用分層狀態(tài)機(jī)結(jié)構(gòu)，將代客泊車狀態(tài)機(jī)分為上層與下層狀態(tài)機(jī)，上層狀態(tài)機(jī)主要對(duì)代客泊車與定點(diǎn)召回功能中的行車和泊車狀態(tài)進(jìn)行切換；下層狀態(tài)機(jī)包括行車與泊車狀態(tài)機(jī)，其中行車狀態(tài)引導(dǎo)車輛到達(dá)車位附近，泊車狀態(tài)則引導(dǎo)車輛泊入或泊出車位，同時(shí)對(duì)車輛行車與泊車過(guò)程中的行為進(jìn)行決策。

1.1 上層狀態(tài)機(jī)設(shè)計(jì)

為實(shí)現(xiàn)代客泊車與定點(diǎn)召回功能，設(shè)計(jì)的代客泊車狀態(tài)框架如圖1 所示。其中車輛起始為初始狀態(tài)，隨后根據(jù)駕駛員選取的功能進(jìn)入不同的狀態(tài)，具體流程如下。

圖1 代客泊車系統(tǒng)狀態(tài)機(jī)框架

（1）代客泊車模式 駕駛員選擇代客泊車功能，車輛在接收到信號(hào)后，從初始狀態(tài)跳轉(zhuǎn)至行車狀態(tài)；在車輛行駛至行車泊車切換區(qū)域后，車輛減速停止，隨后進(jìn)入泊車狀態(tài)；在完成泊車后，車輛進(jìn)入結(jié)束狀態(tài)，完成代客泊車流程。

（2）定點(diǎn)召回模式 駕駛員在遠(yuǎn)程選擇定點(diǎn)召回功能并輸入召回點(diǎn)，車輛接收召回點(diǎn)信息后，從初始狀態(tài)跳轉(zhuǎn)至泊車狀態(tài)；完成泊出后，跳轉(zhuǎn)至行車狀態(tài)；行駛至召回點(diǎn)區(qū)域并完全停下后，車輛進(jìn)入結(jié)束狀態(tài)，完成定點(diǎn)召回流程。

上層狀態(tài)機(jī)各狀態(tài)之間跳轉(zhuǎn)的事件定義如表1所示，其中相關(guān)區(qū)域判定將在第3節(jié)中展開(kāi)敘述。

表1 上層狀態(tài)機(jī)事件表

1.2 行車狀態(tài)機(jī)設(shè)計(jì)

行車狀態(tài)是實(shí)現(xiàn)代客泊車功能與定點(diǎn)召回功能的重要模塊，用以引導(dǎo)車輛從起始位置行駛至車位附近。使用有限狀態(tài)機(jī)設(shè)計(jì)車輛在行車過(guò)程中的主要狀態(tài)。目前對(duì)于車輛行車過(guò)程狀態(tài)機(jī)研究較多［11-12］，但針對(duì)停車場(chǎng)復(fù)雜環(huán)境的研究較少，因此針對(duì)停車場(chǎng)場(chǎng)景，須設(shè)計(jì)對(duì)應(yīng)的行車狀態(tài)機(jī)和相應(yīng)的規(guī)則。

1.2.1 車輛環(huán)境建立

根據(jù)全局路徑以及感知系統(tǒng)得到的車輛周圍障礙物信息，首先對(duì)障礙物進(jìn)行分類。為提高車輛的安全性且兼顧行駛效率，首先將自車與障礙物轉(zhuǎn)化至Frenet 坐標(biāo)系下，F(xiàn)renet 坐標(biāo)系相比于全局坐標(biāo)系，能夠更加準(zhǔn)確獲取障礙物與自車的相對(duì)位置，防止在大曲率路徑中對(duì)障礙物進(jìn)行誤判斷，提升行車效率。

在行車狀態(tài)下，自車制動(dòng)、跟車、換道等行為判斷實(shí)際受自車周圍最近車輛的影響較大，而與自車相隔一輛或多輛汽車的車輛對(duì)自車的影響較小，因此，只考慮在自車道與左右相鄰車道的距離自車最近的障礙車輛或障礙物。在Frenet 坐標(biāo)系下將自車道與左右相鄰車道劃分為6部分：左前、左后、前、后、右前、右后，對(duì)應(yīng)區(qū)域內(nèi)距離自車最近車輛作為障礙車，如圖2所示，分類結(jié)果如圖3所示。

圖2 障礙物相對(duì)位置判斷

圖3 Frenet坐標(biāo)系下障礙物分類

將環(huán)境中障礙車按照該方式進(jìn)行實(shí)時(shí)分類，可以有效簡(jiǎn)化行車狀態(tài)所處理的信息量，為下一步行車狀態(tài)下的行為決策提供判斷準(zhǔn)則。

1.2.2 行車行為決策

車輛處于停車場(chǎng)環(huán)境時(shí)，由于周圍環(huán)境較為復(fù)雜，因此對(duì)車輛行車要求較高。為提高車輛的響應(yīng)速度，行車過(guò)程行為決策應(yīng)在滿足車輛安全性的同時(shí)，保證一定效率，因此設(shè)計(jì)行車模塊主要行為如下：常規(guī)行駛、跟車行駛、行車制動(dòng)以及換道行駛。行車模塊行為決策狀態(tài)轉(zhuǎn)移過(guò)程如圖4 所示，行車事件如表2所示。

圖4 行車狀態(tài)轉(zhuǎn)移圖

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

DE1：前車在跟車范圍內(nèi)時(shí)，事件DE1 為真，跟車范圍計(jì)算公式為

式中：τ為車間時(shí)距，s；v0為自車車速，m/s；D0為補(bǔ)償距離，m。

DE2：關(guān)于前車是否處于制動(dòng)范圍，使用TTC 時(shí)間進(jìn)行判斷，其計(jì)算公式為

式中：vego為自車車速；vtarget為目標(biāo)車車速；starget為目標(biāo)車在Frenet 坐標(biāo)系下對(duì)應(yīng)s值；sego為自車在Frenet坐標(biāo)系下對(duì)應(yīng)s值。當(dāng)TTCFront＞0 且TTCFront≤TTCT（TTCFront為前車TTC，TTCT為TTC 安全閾值）或與自車車距小于最短規(guī)劃距離時(shí)，事件DE2為真。

DE3：換道需求分為兩類。第1 類為主動(dòng)換道，在車輛當(dāng)前行駛車道與目標(biāo)車道不一致時(shí)觸發(fā)，目標(biāo)車道的選取根據(jù)目標(biāo)車位與召回點(diǎn)所處位置進(jìn)行選取。第2 類為被動(dòng)換道，當(dāng)自車前方跟車距離內(nèi)存在速度較慢或0 ＜TTCFront≤TTCT的障礙物，車輛同樣存在換道需求。

DE4：當(dāng)車輛與目標(biāo)車道的橫向偏差小于距離偏差閾值且航向偏差小于航向偏差閾值時(shí)，事件DE4為真。

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

DE6：目標(biāo)區(qū)域根據(jù)車輛功能分為行車泊車切換區(qū)域與召回點(diǎn)區(qū)域，當(dāng)車輛車頭進(jìn)入目標(biāo)區(qū)域時(shí)，則判定車輛進(jìn)入目標(biāo)區(qū)域，關(guān)于目標(biāo)區(qū)域的設(shè)計(jì)在第3節(jié)詳細(xì)闡述。

根據(jù)以上車輛行為和對(duì)應(yīng)事件真值，對(duì)車輛行車過(guò)程中行為進(jìn)行決策。本文中設(shè)計(jì)車輛行車狀態(tài)機(jī)，保證車輛在自車道行駛過(guò)程中的安全，同時(shí)在前車速度較慢且旁側(cè)車道安全情況下，允許車輛換道，使車輛在行車過(guò)程兼顧安全與效率。

1.3 泊車狀態(tài)機(jī)設(shè)計(jì)

為滿足車輛在泊車過(guò)程中的安全，設(shè)計(jì)車輛泊車狀態(tài)機(jī)轉(zhuǎn)移圖，如圖5 所示，泊車事件表如表3所示。

圖5 泊車狀態(tài)轉(zhuǎn)移圖

表3 泊車狀態(tài)事件

關(guān)于車輛碰撞風(fēng)險(xiǎn)（BE1）的判斷，將未來(lái)規(guī)劃時(shí)間τp內(nèi)，車輛輪廓在規(guī)劃路徑下掃掠所得到的區(qū)域作為障礙物風(fēng)險(xiǎn)區(qū)域，如圖6 所示。在該區(qū)域內(nèi)存在障礙物時(shí)，事件BE1置為1，反之置為0。

圖6 泊車風(fēng)險(xiǎn)區(qū)域

2 全局路徑規(guī)劃

本文對(duì)代客泊車系統(tǒng)的研究是基于高精度地圖和全局路徑中的參考線，因此獲取一條準(zhǔn)確、高效到達(dá)目標(biāo)車位的全局路徑是系統(tǒng)的基礎(chǔ)。目前常用基于搜索或基于勢(shì)場(chǎng)的方法求解最優(yōu)全局路徑［5-8］。代客泊車中全局路徑作用主要分為兩種，第1 種直接使用全局路徑引導(dǎo)車輛到達(dá)車位附近，第2 種是利用全局路徑作為參考線，進(jìn)行局部路徑規(guī)劃。第1種方案較為簡(jiǎn)便，但路徑固定。而第2種方案可更好地適應(yīng)停車場(chǎng)復(fù)雜多變的環(huán)境，實(shí)現(xiàn)對(duì)障礙物的規(guī)避。

利用Dijkstra 算法，根據(jù)高精度地圖所提供停車場(chǎng)道路帶權(quán)有向圖，求解出車輛起始點(diǎn)至目標(biāo)點(diǎn)的最短路徑道路ID，經(jīng)地圖引擎將結(jié)果發(fā)送至車輛，為之后的局部路徑規(guī)劃提供基礎(chǔ)，規(guī)劃結(jié)果如圖7所示。

圖7 全局路徑規(guī)劃

3 局部路徑規(guī)劃

3.1 行車路徑規(guī)劃

狀態(tài)柵格法是一種基于采樣和曲線插值的路徑規(guī)劃方法［13］。該方法具有采樣法和插值法的優(yōu)勢(shì)，能夠快速應(yīng)對(duì)動(dòng)態(tài)環(huán)境的變化，同時(shí)計(jì)算負(fù)載小于基于搜索或優(yōu)化的方法，在結(jié)構(gòu)化道路下的自動(dòng)駕駛車輛避障及換道規(guī)劃上得到了大量應(yīng)用［14］?？紤]到本文研究的代客泊車系統(tǒng)場(chǎng)景為具有結(jié)構(gòu)化道路的停車場(chǎng)，同時(shí)為降低計(jì)算平臺(tái)算力負(fù)擔(dān)，采用文獻(xiàn)［15］中基于事件觸發(fā)的狀態(tài)柵格法對(duì)車輛行車軌跡進(jìn)行規(guī)劃，車輛某時(shí)刻規(guī)劃路徑如圖8所示。

圖8 行車局部路徑規(guī)劃

3.2 泊入路徑規(guī)劃

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

圖9 泊車路徑規(guī)劃流程

3.2.1 前進(jìn)段路徑規(guī)劃

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

3.2.2 泊入段路徑規(guī)劃

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

最終以路徑長(zhǎng)度、最大曲率作為成本項(xiàng)建立路徑成本函數(shù)，計(jì)算不同路徑成本。在滿足道路約束與車輛運(yùn)動(dòng)學(xué)約束條件情況下，選取最低成本路徑作為最優(yōu)泊入路徑，如圖10 所示。前進(jìn)與泊入段速度規(guī)劃方法則使用文獻(xiàn)［17］中的方法。

圖10 泊入路徑規(guī)劃示意圖

3.2.3 行車泊車切換區(qū)域設(shè)計(jì)

由于停車場(chǎng)環(huán)境復(fù)雜多變，車輛在跳轉(zhuǎn)出行車狀態(tài)前可能處于換道或跟車等不確定狀態(tài)，導(dǎo)致車輛泊車起始位姿難以確定，因此使用前進(jìn)段路徑消除起始位姿對(duì)泊入路徑規(guī)劃的影響。而行車泊車切換區(qū)域位置對(duì)前進(jìn)段路徑曲率產(chǎn)生較大影響，故須保證車輛在區(qū)域中規(guī)劃出的前進(jìn)段路徑滿足車輛運(yùn)動(dòng)學(xué)約束以及道路邊界約束。

為保證車輛在行駛至車位附近后，在較大位姿范圍內(nèi)均可規(guī)劃出滿足車輛的運(yùn)動(dòng)學(xué)以及道路邊界約束的前進(jìn)段路徑。根據(jù)車輛進(jìn)入切換區(qū)域后的航向，分為平行于道路以及與道路存在一定夾角兩種情況。在不同位姿的情況下，對(duì)切換區(qū)域進(jìn)行設(shè)計(jì)，使車輛在限定的航向與位置下，在切換區(qū)域中均可規(guī)劃出滿足要求的前進(jìn)段路徑。

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

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

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

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

圖12 平行航向行車泊車切換區(qū)域示意圖

切換區(qū)域長(zhǎng)度Lp通過(guò)式（4）進(jìn)行計(jì)算：

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

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

車輛在進(jìn)入行車泊車切換區(qū)域前，可能由于障礙物等因素，處于換道避障等狀態(tài)，導(dǎo)致進(jìn)入切換區(qū)域后，車輛與道路產(chǎn)生一定角度，為保證能夠規(guī)劃出滿足要求的前進(jìn)段路徑，須根據(jù)車輛航向?qū)η袚Q區(qū)域進(jìn)行研究。首先計(jì)算出在滿足道路約束的條件下，車輛相對(duì)道路允許的最大航向角。在上述坐標(biāo)系下，根據(jù)車輛最小轉(zhuǎn)彎半徑，以逆時(shí)針為正，得到車輛所允許的最大正負(fù)航向αp與αn，如圖13 所示。同時(shí)可得車輛后軸中心在不同縱向道路位置下，車輛允許的極限航向，如圖14 所示。在相應(yīng)縱向位置的極限航向下，車輛以最小轉(zhuǎn)彎半徑轉(zhuǎn)向，仍可以保持在道路范圍內(nèi)。

圖13 車輛相對(duì)道路最大正負(fù)航向

圖14 車輛極限航向與縱向位置關(guān)系圖

根據(jù)上述車輛在不同位置下的允許極限航向，以車輛平行于道路航向所得到的右極限位置作為基準(zhǔn)，按照?qǐng)D15 所示流程（Δxi為橫向采樣距離），得到對(duì)應(yīng)縱向采樣點(diǎn)下右極限x值，由此得到在正負(fù)航向下滿足運(yùn)動(dòng)學(xué)條件的前進(jìn)段路徑的右極限邊界，如圖16和圖17所示，其中切換區(qū)域長(zhǎng)度見(jiàn)式（4）。

圖15 正負(fù)航向行車泊車切換區(qū)域設(shè)計(jì)流程

圖16 正航向行車泊車切換區(qū)域示意圖

圖17 負(fù)航向行車泊車切換區(qū)域示意圖

根據(jù)上述車輛在平行于道路以及與道路有正負(fù)夾角的情況下分別得到的行車泊車切換區(qū)域，將3種情況下的右極限位置取最小值，得到行車泊車切換區(qū)域。在該區(qū)域內(nèi)，車輛在滿足道路約束的極限航向下，均可規(guī)劃出前進(jìn)段路徑，引導(dǎo)車輛到達(dá)泊入段路徑起始點(diǎn)，順利泊入目標(biāo)車位。

當(dāng)車輛進(jìn)入切換區(qū)域，事件DE6 置為1，車輛進(jìn)入行車制動(dòng)狀態(tài)；完全停下后，事件E2 為1，跳轉(zhuǎn)進(jìn)入泊車狀態(tài)，根據(jù)規(guī)劃出的前進(jìn)路段與泊入路段泊入車位；完成泊入后，事件E4置為1，進(jìn)入結(jié)束狀態(tài)，完成代客泊車流程。

3.3 泊出路徑規(guī)劃

泊出路徑規(guī)劃主要應(yīng)用在定點(diǎn)召回模式下。根據(jù)召回點(diǎn)與當(dāng)前車位的關(guān)系，分為左泊出與右泊出，保證車輛在完成泊出后車輛航向角與召回點(diǎn)方向一致。車輛泊出路徑規(guī)劃方法與泊入狀態(tài)中泊入段路徑規(guī)劃方法一致，將規(guī)劃起止點(diǎn)對(duì)調(diào)即可，在此不再贅述。完成泊出后，事件E5置為1，車輛由泊車狀態(tài)跳轉(zhuǎn)進(jìn)入行車狀態(tài)。為保證車輛到達(dá)召回點(diǎn)準(zhǔn)確停止，需要對(duì)召回點(diǎn)區(qū)域進(jìn)行設(shè)計(jì)。其中召回點(diǎn)區(qū)域右邊界為召回點(diǎn)對(duì)應(yīng)x值，其長(zhǎng)度選取與行車泊車切換區(qū)域方法一致，寬度為道路寬度。泊出最優(yōu)路徑規(guī)劃與召回點(diǎn)區(qū)域如圖18所示。

圖18 泊出路徑規(guī)劃與召回點(diǎn)區(qū)域

當(dāng)車輛進(jìn)入召回點(diǎn)區(qū)域時(shí)，車輛跳轉(zhuǎn)進(jìn)入行車制動(dòng)狀態(tài)，減速停止后，事件E3 置為1，車輛由行車跳轉(zhuǎn)至結(jié)束狀態(tài)，完成定點(diǎn)召回流程。

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

4.1 試驗(yàn)平臺(tái)介紹

采用東風(fēng)悅享Sharing Van 1.0 Plus 公開(kāi)道路版作為試驗(yàn)平臺(tái)，該車搭載4 枚激光雷達(dá)以及GNSS、IMU 等定位設(shè)備，配備線控底盤，實(shí)現(xiàn)對(duì)前輪轉(zhuǎn)角、加速與制動(dòng)踏板的精確控制，滿足代客泊車的試驗(yàn)要求。車輛具體參數(shù)如表4 所示。采用文獻(xiàn)［19］中的方法，對(duì)行車與泊車過(guò)程中軌跡進(jìn)行跟蹤。

表4 Sharing Van 1.0 Plus基本參數(shù)

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

4.2.1 代客泊車試驗(yàn)

車輛起始停泊在停車場(chǎng)入口處，在起始位置與目標(biāo)車位之間，設(shè)置相應(yīng)靜態(tài)與動(dòng)態(tài)障礙物，隨后輸入目標(biāo)車位信息，起動(dòng)車輛。車輛在行車過(guò)程中順利完成避障，在到達(dá)泊車區(qū)域后，制動(dòng)減速至0，進(jìn)入泊入狀態(tài)，并順利完成垂直車位的泊入過(guò)程，如圖19所示。對(duì)應(yīng)局部路徑規(guī)劃如圖20所示，泊車過(guò)程中車輛速度規(guī)劃如圖21所示。

圖19 代客泊車實(shí)車試驗(yàn)

圖20 代客泊車路徑規(guī)劃結(jié)果

圖21 泊入狀態(tài)速度規(guī)劃

4.2.2 定點(diǎn)召回試驗(yàn)

車輛起始停泊在垂直車位中，起動(dòng)車輛，車輛完成泊出過(guò)程，隨后進(jìn)入行車狀態(tài)，在到達(dá)召回區(qū)域后，車輛制動(dòng)至停止，停泊在召回點(diǎn)前，完成定點(diǎn)召回功能，如圖22所示。

圖22 定點(diǎn)召回實(shí)車試驗(yàn)

4.2.3 試驗(yàn)結(jié)果分析

試驗(yàn)過(guò)程中，車輛狀態(tài)與試驗(yàn)時(shí)間的關(guān)系如圖23所示。代客泊車過(guò)程中，在1 s時(shí)車輛由初始狀態(tài)進(jìn)入行車狀態(tài)中常規(guī)行駛，在22 s 到達(dá)行車泊車切換區(qū)域進(jìn)行制動(dòng)，減速至靜止后，于30 s跳轉(zhuǎn)至泊車狀態(tài)中初始狀態(tài)，并隨后進(jìn)入泊入狀態(tài)，在67 s完成泊入，進(jìn)入結(jié)束狀態(tài)，完成代客泊車過(guò)程。

圖23 車輛狀態(tài)時(shí)間圖

定點(diǎn)召回試驗(yàn)中，車輛在1 s時(shí)由初始狀態(tài)進(jìn)入泊車狀態(tài)中泊車初始狀態(tài)，隨后進(jìn)入泊出狀態(tài)，完成泊出后，在28 s跳轉(zhuǎn)進(jìn)入行車狀態(tài)中常規(guī)行駛狀態(tài)，直至74 s 到達(dá)召回點(diǎn)區(qū)域進(jìn)行制動(dòng)，在82 s 時(shí)停止并進(jìn)入結(jié)束狀態(tài)，完成召回過(guò)程。

5 結(jié)論

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

最終對(duì)該代客泊車方案進(jìn)行實(shí)車試驗(yàn)。試驗(yàn)結(jié)果表明，所設(shè)計(jì)代客泊車方案能順利完成車輛代客泊車以及定點(diǎn)召回功能。相比于傳統(tǒng)代客泊車系統(tǒng)，本文提出的方案能夠顯著降低代客泊車中泊車對(duì)起始位姿的要求，將傳統(tǒng)泊車點(diǎn)擴(kuò)展為一較大的泊車區(qū)域，提高代客泊車成功率。該方案可以較好地移植至基于車端或場(chǎng)端的代客泊車系統(tǒng)中。