王永勝，羅禹貢，古諺諶，陳 銳，齊蘊龍，江發(fā)潮

(1. 中國農(nóng)業(yè)大學工學院，北京 100083；2. 清華大學，汽車安全與節(jié)能國家重點實驗室，北京 100084)

前言

自主泊車系統(tǒng)在無需駕駛員操作或監(jiān)督的情況下，可對車輛完全接管，控制車輛完成自主導航與自動泊車過程。自主泊車系統(tǒng)也可稱為自動代客泊車(automated valet parking，AVP)系統(tǒng)[1]，它作為 L4級自動駕駛低速場景下的典型應(yīng)用，目前正成為產(chǎn)業(yè)界與學術(shù)界的研究熱點[2－3]。自主泊車與自動充電功能的結(jié)合，將是未來智能交通系統(tǒng)的發(fā)展趨勢之一[4－6]。SAE International 發(fā)布的電動汽車無線充電標準J2954 指出:WPT(wireless power transfer)在結(jié)合自動泊車與自動充電方面為自動駕駛提供了附加優(yōu)勢[6]。然而，接觸式自動充電(如自動充電機械臂)需要車輛較為精確地停在指定區(qū)域，而非接觸式自動充電(如無線充電)的效率與充電裝置的對接位置直接相關(guān)，較大的縱橫向位置偏差將嚴重降低充電效率[7－8]，因此有必要開展自主泊車與自動充電系統(tǒng)精準對接方面的研究。

目前，自動泊車方法的研究多以提高泊車過程中的控制精度為目標，在保證不侵入其他車位的同時準確入庫。文獻[9]～文獻[11]中針對初始階段或泊車過程中車輛偏離參考路徑的問題，利用多段泊車路徑規(guī)劃，通過前后多次往復運動將車輛調(diào)整至參考路徑。文獻[12]～文獻[14]中利用先進的預(yù)瞄方法與控制方法，提高了泊車的路徑跟蹤控制精度。但現(xiàn)有的研究并未關(guān)注泊車完成之后，車輛停止位置的偏差是否達到了需求精度，在未達到需求精度時如何降低該偏差。而解決此問題對保證自動充電系統(tǒng)的良好工作至關(guān)重要。

為滿足智能電動車輛在自動充電時，充電裝置對對接位置準確性的高要求，本文中提出為自主泊車加入精準調(diào)節(jié)功能，并相應(yīng)提出了基于“前進方向偏差與倒車橫向位置偏差(forward orientation and reverse lateral position deviations，F(xiàn)ORLPD)”的精準調(diào)節(jié)策略。首先，通過基于預(yù)瞄視角的兩點匹配和基于預(yù)瞄前視點匹配的雙向檢索策略，解決了單點預(yù)瞄方法在大曲率路徑下，檢索預(yù)瞄參考點時可能存在的檢索異常點與檢索失效問題。然后，基于FORLPD 精準調(diào)節(jié)策略，通過車輛前進運動過程中的航向角偏差、倒車運動過程中的橫向位置偏差，在車位寬度范圍內(nèi)實現(xiàn)小角度的轉(zhuǎn)向控制，保證了自主泊車完成時自動充電裝置的對接精度與對接成功率。

1 自主泊車架構(gòu)設(shè)計

本文提出的自主泊車系統(tǒng)架構(gòu)如圖1 所示，主要包含停車場拓撲地圖、自主泊車路徑規(guī)劃和決策控制3 大關(guān)鍵模塊。停車場拓撲地圖為路徑規(guī)劃提供可充電車位的幾何參數(shù)、高精度道路節(jié)點坐標與節(jié)點之間的拓撲關(guān)系。路徑規(guī)劃模塊則基于地圖與自車定位信息，規(guī)劃出車輛當前位置到指定車位的完整自主泊車路徑。自主泊車決策控制模塊通過分層調(diào)用相關(guān)子單元，計算當前功能下的轉(zhuǎn)向、擋位、驅(qū)動和制動控制指令，進而由車輛的控制執(zhí)行單元接收并執(zhí)行，最終實現(xiàn)自主泊車。

為保證自動充電裝置的精準對接，本文中為自主泊車擴展了精準調(diào)節(jié)功能，如圖2 所示。車輛運行模式被劃分為8 個階段，分別以S1 ～S8 表示。其中，常規(guī)的自主泊車流程為S1 ～S4 階段，精準調(diào)節(jié)流程為S5～S8 階段。在泊車停止(S4)階段后，若車輛橫、縱向偏差滿足了自動充電所需的精度要求，則系統(tǒng)終止；否則啟動精準調(diào)節(jié)，通過前進方向與倒車方向的車輛運動控制，利用有限次數(shù)的精準調(diào)節(jié)，保證自動充電裝置的對接精度與對接成功率。

為擴展精準調(diào)節(jié)功能，本文中將圖1 中的自主泊車控制系統(tǒng)設(shè)計為分層共用的架構(gòu)形式。系統(tǒng)首先依據(jù)參考路徑與自車定位信息決策車輛運行模式，進而根據(jù)當前車輛運行模式(如S3)，分別調(diào)用并組合分層系統(tǒng)中預(yù)瞄單元、橫向控制單元、縱向控制單元的相關(guān)子單元(如圖 1 中的 Y1、H2、Z1、Z2 和Z3)，以實現(xiàn)當前模式下的具體功能。

2 基于預(yù)瞄視角的單點預(yù)瞄方法

單點預(yù)瞄方法作為路徑跟蹤的基礎(chǔ)，應(yīng)用于自主泊車的S1 和S3 階段。雖然在小曲率路徑下，單點預(yù)瞄方法[15－16]能夠求得合理的預(yù)瞄參考點，但應(yīng)用于停車場內(nèi)的大曲率路徑時可能存在預(yù)瞄參考點求解不合理，甚至無法求解的特殊情況。因此本文在單點預(yù)瞄方法的基礎(chǔ)上引入了預(yù)瞄視角，確保在特殊情況下仍能求解合理的預(yù)瞄參考點，進而得到橫向位置偏差與航向角偏差。

2.1 基于預(yù)瞄視角的單點預(yù)瞄模型

首先，對基于預(yù)瞄視角的單點預(yù)瞄模型做以下定義，如圖3 所示。

圖2 自主泊車與精準調(diào)節(jié)流程

圖3 基于預(yù)瞄視角的單點預(yù)瞄模型

定義全局坐標系為XOY，車輛坐標系為xoy，將后軸中心o作為自車參考點。參考路徑的離散描述形式為(X，Y，Ψ，κ，S，N)，分別表示橫坐標、縱坐標、路徑航向角、路徑曲率、累積長度(從路徑起始點至當前點)和路點序號，其中路徑航向角定義為沿當前可行駛方向的路點切線方向與X軸正方向之間的夾角，范圍為[0，2π)。預(yù)瞄參考點P為(XP，YP，ΨP，κP，SP，NP)，點SP為參考路徑上距離點o最近的點，最小距離為dmin。LP為從點o沿x軸方向的預(yù)瞄距離，θP為預(yù)瞄視角，PV、PL與PR分別為預(yù)瞄前視點、預(yù)瞄左極限點和預(yù)瞄右極限點。Δy和Δψ分別為預(yù)瞄參考點相對自車參考點o的橫向位置偏差與航向角偏差。當車輛倒車運動時，預(yù)瞄范圍與相關(guān)點沿車輛坐標系x軸的負方向表示。

基于以上定義，可通過預(yù)瞄視角θP與預(yù)瞄距離LP限制預(yù)瞄的范圍，如圖3 中的陰影區(qū)域所示，即

式中:Lub為預(yù)瞄距離上界；Llb為預(yù)瞄距離下界；v為車速；tp為預(yù)瞄時間；LPL為點 PV到 PL的長度；LPR為點PV到PR的長度。

然后通過路點檢索與插值求解得到預(yù)瞄參考點P，將全局坐標系下的P點位姿[XP，YP，ΨP]，經(jīng)過旋轉(zhuǎn)與平移，轉(zhuǎn)化為車輛坐標系xoy下的P點位姿[xP，yP，ψP]，即

式中:XV為全局坐標系下自車參考點的X坐標；YV為全局坐標系下自車參考點的Y坐標；ΨV為全局坐標系下車輛的航向角，范圍為[0，2π)。

結(jié)合圖3 中的幾何關(guān)系，可知:

最后，可得到用于車輛橫向控制的Δy與Δψ，其解析式為

2.2 特殊情況分析

單點預(yù)瞄方法在大曲率路徑下，檢索預(yù)瞄參考點時可能存在檢索異常點與檢索失效的問題，如圖4 所示，圖中Pe點為檢索異常點。因此，提出基于預(yù)瞄視角的兩點匹配方法解決該問題。首先利用預(yù)瞄極限點PL與PR匹配距離路徑的最近點Prl與Prr，如式(8)所示；然后將兩者對應(yīng)的路點序號Nrl、Nrr與上一時刻的預(yù)瞄參考點的路點序號NP，k－1進行比較，選取路點序列最大的點作為當前時刻的預(yù)瞄參考點，如式(9)所示，以避免預(yù)瞄參考點前后跳變。

圖4 檢索時的異常點與失效情況

同時，以路點表示的參考路徑在預(yù)瞄參考點檢索時，現(xiàn)有算法通常從SP點開始檢索，直至找到預(yù)瞄參考點。但對于僅由兩點描述的直線路段，存在SP點在預(yù)瞄參考點P之前的情況，單一的前向檢索將導致檢索失效。而對于路點稠密的曲線路段，SP點至P點之間的檢索范圍較大，會產(chǎn)生過多的無效運算。因此，針對該問題，本文中使用預(yù)瞄前視點PV匹配最近路點作為起始檢索點SCP，如圖5 所示，在比較車輛坐標系下的SCP點橫坐標xSCP與LP之間的數(shù)值大小后，再決定檢索方向。

圖5 基于SCP 點匹配的雙向檢索示意圖

最后，預(yù)瞄參考點的檢索求解流程如圖6 所示。首先在預(yù)瞄參考點檢索階段確定檢索方向，然后找到預(yù)瞄參考點所在的前后路點Pi與Pj，最后在滿足預(yù)瞄視角的情況下，通過線性插值求解的交點，獲得預(yù)瞄參考點P，否則利用基于預(yù)瞄視角的兩點匹配方法得到預(yù)瞄參考點P。圖中，xSCP為SCP點在車輛坐標系下的橫坐標；NSCP為SCP點的路點序號；Ne為參考路徑末點的路點序號；ncr為參考的檢索點數(shù)量；nc為實際的檢索點數(shù)量；(xi，yi)為路點Pi的坐標；(xj，yj)為路點Pj的坐標。

3 自主泊車精準調(diào)節(jié)策略

車輛位姿的準確測量與估計和執(zhí)行機構(gòu)的精準響應(yīng)是實現(xiàn)自主泊車精準調(diào)節(jié)的前提。傳統(tǒng)的自動泊車因受到傳感器精度限制，需考慮車輛相對位姿的測量與估計方法；而自主泊車可以利用高精度地圖與定位獲得滿意的測量精度，因此本文對自主泊車的精準調(diào)節(jié)方法進行研究，以提高自動充電系統(tǒng)的對接精度與對接成功率。

考慮車位空間對精準調(diào)節(jié)的約束，將所提的FORLPD 精準調(diào)節(jié)策略應(yīng)用于如圖2 所示的S5 ～S8階段。該策略利用前進運動調(diào)節(jié)、倒車運動調(diào)節(jié)與精準停車3 個主要環(huán)節(jié)，在車位寬度范圍內(nèi)，調(diào)整車輛泊車入位完成后的車輛停止位置偏差。

首先，前進運動調(diào)節(jié)僅對航向角偏差進行控制，為倒車運動調(diào)節(jié)提供較好的車輛初始姿態(tài)，如圖7(a)所示。然后，倒車運動調(diào)節(jié)僅對橫向位置偏差進行控制，保證車輛的橫向位置精度，如圖7(b)所示。橫向控制采用了基于單點追蹤的預(yù)瞄方法，將預(yù)瞄參考點固定為參考路徑的末點。根據(jù)車位坐標系XPSOPSYPS下P點的位置和方向，將其轉(zhuǎn)換至全局坐標系中，利用式(4)～式(7)求解Δy與Δψ，用于精準調(diào)節(jié)的橫向控制。

圖6 基于預(yù)瞄視角的預(yù)瞄參考點求解流程圖

圖7 FORLPD 精準調(diào)節(jié)策略示意圖

精準停車控制根據(jù)制動距離調(diào)整車輛的縱向位置偏差。其中停車距離的指標選取尤為關(guān)鍵，如圖8 所示，o為自車參考點位置，xgogyg為參考路徑末點的坐標系，(xpg，ypg)為路徑末點坐標系下自車的坐標，dpg為自車至路徑末點的歐式距離，Spg為自車至路徑末點的剩余里程，Rc為開始檢測制動距離的半徑范圍。

圖8 停車距離判定指標

若僅使用歐式距離dpg，在直線參考路徑下可能導致制動時機滯后，而在曲線參考路徑下會使制動時機提前；若僅使用剩余里程Spg，則在路點稀疏的參考路徑下會導致里程跳變；若僅使用xpg，在直線參考路徑下精確，但對曲線參考路徑不適用。因此，依據(jù)實際情況選擇合理的停車距離指標才能獲得理想的制動啟動時機，實現(xiàn)縱向停車精度的有效調(diào)節(jié)，圖8 中的判定指標通過式(10)～式(12)計算。本文中僅研究垂直泊車的情況，精準停車階段的參考路徑為直線，因此選用xpg作為停車距離指標。

式中:(Xg，Yg)為全局坐標系下路徑末點坐標；Ψg為全局坐標系下路徑末點的航向角；NV為距離車輛參考點最近的路點序號；Ng為路徑末點的序號。

4 實車試驗

4.1 軟硬件平臺

為驗證自主泊車精準調(diào)節(jié)策略的有效性，結(jié)合本文的理論方法設(shè)計了自主泊車控制系統(tǒng)，系統(tǒng)框圖如圖9 所示。該系統(tǒng)由車輛運行模式?jīng)Q策模塊根據(jù)xpg、Spg和dpg判斷當前車輛運行模式；預(yù)瞄單元、橫向控制單元與縱向控制單元依據(jù)運行模式激活對應(yīng)模塊；其中，不同模式下橫向控制器的轉(zhuǎn)向角速度約束不同，縱向控制單元需要的部分反饋信號是指與縱向控制相關(guān)的信號，如輪缸壓力。橫向位置偏差控制器與航向角偏差控制器均采用PD 控制，縱向車速采用基于規(guī)則的控制。

圖9 自主泊車控制系統(tǒng)框圖

首先基于圖9 所示的軟件系統(tǒng)框圖，在Matlab/Simulink 環(huán)境下搭建自主泊車系統(tǒng)的軟件模型，并將模型編譯下載至MicroAutoBox 作為自主泊車控制器，試驗時通過ControlDesk 實現(xiàn)人機交互功能。然后搭建如圖10 所示的實車驗證平臺，該平臺在北汽新能源EU260 車型的基礎(chǔ)上，通過改造具備了線控轉(zhuǎn)向、驅(qū)動和制動能力。采用具有厘米級定位精度的RTK 定位方式，車端配置聯(lián)適R60S－U 接收機與雙衛(wèi)星天線，地面搭建聯(lián)適R30S 基站與電臺。車輛同時配備了SICK 單線激光雷達、MicroAutoBox(ds1401)、CAN case 和監(jiān)控平臺。試驗場地為GPS 信號良好的室外停車場，如圖11 所示。

圖10 實車試驗平臺

圖11 室外停車場

EU260 電動汽車相關(guān)參數(shù)如表1 所示，實車試驗時的預(yù)瞄參數(shù)如表2 所示。

表1 車輛相關(guān)參數(shù)

表2 預(yù)瞄模型參數(shù)

4.2 評價指標

針對精準停車階段的車輛停止位姿精度，設(shè)計評價指標。首先，建立精準調(diào)節(jié)參考路徑末點的坐標系，評價指標定義為Δxs、Δys，分別表示車輛停止位置的縱向、橫向偏差，如圖12(a)所示。然后，定義泊車入位、第1 次精準調(diào)節(jié)、第2 次精準調(diào)節(jié)之后的精準停車評價指標，分別為 Δxs1、Δys1、Δxs2、Δys2、Δxs3、Δys3，且將縱橫向偏差處于－0.15～0.15 m 范圍內(nèi)定義為精準泊車。最后定義自主泊車過程中的評價指標，如圖12(b)所示，通過匹配距離車輛后軸中心的最近路點，建立路點坐標系xroryr，車輛后軸中心在路點坐標系下的橫坐標和航向角分別定義為泊車過程中的橫向偏差Δyr與航向角偏差Δψr。

圖12 評價指標的定義

4.3 試驗結(jié)果分析

針對停車場內(nèi)的不同車位進行了11 次自主泊車實車試驗，整體的軌跡情況如圖13 所示，包含了C 型與人字型垂直泊車，以及左側(cè)與右側(cè)泊車的情況。

圖13 自主泊車實車試驗結(jié)果

為驗證系統(tǒng)架構(gòu)的合理性與系統(tǒng)工作流程的可行性，對5 號車位的自主泊車與精準調(diào)節(jié)過程進行分析。圖14 描述了車輛的位姿軌跡，可以看出所設(shè)計的系統(tǒng)能夠完整實現(xiàn)自主泊車與精準調(diào)節(jié)過程，但由于初始時車輛的位姿偏差較大，因此在前進路徑跟蹤階段車輛的跟蹤誤差較大。由圖中放大處可知，經(jīng)過一次精準調(diào)節(jié)，車輛的橫向位置偏差明顯減小。

圖14 5 號車位的自主泊車軌跡

圖15 描述了全局坐標系下，車輛的縱橫向位置與車身姿態(tài)隨時間t的動態(tài)變化情況。S1 ～S8分別對應(yīng)圖2 中的不同階段。圖16 描述了轉(zhuǎn)向盤轉(zhuǎn)角控制指令δ與車速v隨時間的變化情況，可以看出在S1 階段，通過較大的轉(zhuǎn)向盤轉(zhuǎn)角調(diào)節(jié)車輛橫向位置偏差，并最終趨于穩(wěn)定；在精準調(diào)節(jié)階段，結(jié)合圖15 可以看出，此時車輛的縱向坐標和航向角變化較小，通過小角度的轉(zhuǎn)向控制，能夠使車輛在狹小的車位空間中調(diào)整位姿。速度控制方面，在S1 階段距終點15 m 范圍內(nèi)，車輛開始以1 m/s的參考車速降速運行，但速度控制的波動較大。在S3 ～S8 階段，車輛的行駛路徑長度較短，以起步與制動過程為主，不易保持勻速，因此穩(wěn)定的低速控制方法有待提高。

圖15 車輛動態(tài)軌跡變化

圖16 轉(zhuǎn)向盤轉(zhuǎn)角控制指令與車速控制變化

圖17 描述了車輛橫向位置偏差Δyr與航向角偏差Δψr的變化情況，在S1 階段，橫向位置偏差與航向偏差分別達到了－0.58 m 與0.325 rad，導致車輛的控制效果并不理想，但最終在S2 階段收斂。泊車入位階段橫向位置偏差從－0.27 收斂至0.16 m，并經(jīng)過精準調(diào)節(jié)后達到0.10 m；航向偏差最大達－0.173 rad，并最終收斂至0.037 rad，表明泊車控制與精準調(diào)節(jié)橫向控制效果良好。圖中S2 階段模式發(fā)生切換，使得偏差輸出跳變?yōu)?，同時因參考路徑發(fā)生切換，導致模式切換后的偏差值存在稍許差異；S4 過渡至S5 時也存在相同情況。

圖17 自車的橫向位置偏差與航向角偏差

4.4 統(tǒng)計結(jié)果分析

為說明所提精準調(diào)節(jié)策略應(yīng)用于不同車位的有效性，基于4.1 節(jié)中提出的評價指標，將11 次不同車位的試驗結(jié)果進行了橫向?qū)Ρ?，不同車位試驗時的精準調(diào)節(jié)次數(shù)如表3 所示。可以看出其中未觸發(fā)精準調(diào)節(jié)模式，在泊車入位階段直接達到泊車精度的次數(shù)僅占55%，因此有必要加入精準調(diào)節(jié)功能。

表3 精準調(diào)節(jié)次數(shù)統(tǒng)計

圖18 描述了縱向位置偏差的變化情況，通過精準調(diào)節(jié)，縱向的精度均控制在0.15 m 的精準范圍內(nèi)。但對于初始縱向精度較好的試驗，存在縱向偏差稍許增大的情況，原因在于精準停車在勻速的假設(shè)條件下依據(jù)距離判斷，但實際當中難以實現(xiàn)極低速下的勻速控制，故進入制動的速度有所不同，導致高精度制動距離不易保證。

圖18 精準停車后的縱向位置偏差變化

圖19 描述了橫向位置偏差的變化情況，通過精準調(diào)節(jié)，該偏差均控制在0.15 m 范圍內(nèi)，且多次調(diào)節(jié)后的精度更高，說明FORLPD 精準調(diào)節(jié)策略對橫向位置偏差具有良好的調(diào)節(jié)作用。

圖19 精準停車后的橫向位置偏差變化

由圖18 和圖19 可以看出，通過精準調(diào)節(jié)，全部試驗均實現(xiàn)了偏差在0.15 m 范圍內(nèi)的精準泊車，有效保證了自動充電裝置的對接精度與對接成功率。為量化試驗指標，對表3 中觸發(fā)了精準調(diào)節(jié)的5 次試驗結(jié)果進行統(tǒng)計對比，如表4 所示。精準調(diào)節(jié)后的縱橫向位置偏差值相比調(diào)節(jié)前的偏差值在最大數(shù)值MAX、最小數(shù)值MIN、均方根值RMS 和總體標準差σ4 個方面均有所降低。其中，縱向位置偏差的RMS 從0.13 降至0.09 m，減小了 30.8%；橫向位置偏差的 RMS 從0.18 降至0.11 m，減小了38.9%?？梢钥闯鱿啾瘸Ｒ?guī)自主泊車，在加入精準調(diào)節(jié)功能后，車輛的泊車停止位置偏差能夠得到有效降低，使其達到精度要求。此外，縱向位置偏差的總體標準差σ從0.05 降至0.02 m；橫向位置偏差的總體標準差也從0.05 降至0.02 m，說明了偏差的離散程度均有所降低，精準調(diào)節(jié)的效果穩(wěn)定。

表4 泊車入位與精準調(diào)節(jié)的偏差對比

5 結(jié)論

針對自主泊車結(jié)合自動充電時，面臨自動充電裝置對對接裝置精度的高要求，本文中為自主泊車加入了精準調(diào)節(jié)功能，并提出了FORLPD 精準調(diào)節(jié)策略。相比常規(guī)自主泊車，在加入精準調(diào)節(jié)功能后，車輛的泊車停止位置偏差得到有效且穩(wěn)定地降低。

在單點預(yù)瞄方法的基礎(chǔ)上，分析了大曲率路徑下，檢索預(yù)瞄參考點時可能出現(xiàn)的異常點與失效情況，通過基于預(yù)瞄視角的兩點匹配與基于預(yù)瞄前視點的雙向檢索解決了該問題，提高了預(yù)瞄算法的完備性。