基于場景驅動與分層規(guī)劃的自主代客泊車系統(tǒng)研究

邱少林 錢立軍 陸建輝

合肥工業(yè)大學汽車與交通工程學院，合肥，230009

0 引言

近年來，高級駕駛輔助系統(tǒng)(advanced driver assistance systems, ADAS)和自動駕駛(autonomous driving, AD)技術一直是學術界和工業(yè)界研究和開發(fā)的熱點。因為視距角度和距離誤判的局限性以及停車位空間的限制，對駕駛員來說停車入位任務并不容易，所以泊車輔助系統(tǒng)是市場上最為必要的產品之一。幾乎所有商業(yè)化的泊車輔助系統(tǒng)都基于各種傳感器和基礎設施，并且具有被動安全輔助功能，如在停車時警告駕駛員障礙物的存在，即使采用智能泊車輔助系統(tǒng)(intelligent parking assistance systems, IPAS)等主動泊車輔助系統(tǒng)，泊車位也需由駕駛員確定。為了提高駕駛員的便利性和停車場的安全性，自主代客泊車系統(tǒng)(autonomous valet parking systems, AVPS)已經(jīng)得到了廣泛的研究[1-4]。

AVPS系統(tǒng)的開發(fā)主要涉及兩個重要的研究問題：一個是確定目標停車位和停車場中已停車輛的周圍環(huán)境；另一個是生成和跟蹤參考軌跡。通常使用車載傳感器檢測周圍環(huán)境，如相機、激光雷達，而要實現(xiàn)泊車過程中車輛的準確定位和環(huán)境信息的正確表達，目前車載傳感器的性能尚未達到令人滿意的水平，故關于車輛定位的研究都集中在基于地圖輔助的自動駕駛車輛定位[5-7]，也有許多研究涉及構建合適的數(shù)字地圖[5-9]。在一種典型的自動駕駛場景——智能化停車場中，停車場是具備提供地圖信息功能的基礎設施?；诨A設施的停車位占用率檢測系統(tǒng)的研究已經(jīng)持續(xù)了近十年[10-12]，這些系統(tǒng)的優(yōu)點是車輛不需要具備復雜檢測系統(tǒng)。然而，當車輛具有自動駕駛系統(tǒng)時，因為車輛已經(jīng)具有車載傳感器，所以不需要對所有車位進行占用檢測，因此，本文將占用檢測算法與基礎設施信息分開，并使用基于停車場幾何結構的簡單環(huán)境地圖。

目前研究成果中，生成參考路徑的方法主要有基于幾何的方法和基于最優(yōu)控制的實時路徑規(guī)劃方法。GMEZ-BRAVO等[13-14]提出基于連續(xù)曲率β-樣條曲線的路徑生成算法，為了保證參考路徑的無碰撞和可用性，該算法采用具有曲率和邊界約束的模糊控制邏輯。CHENG等[15]將路徑生成問題分為兩部分，分別采用不同的方法規(guī)劃，基于感知結果，每個部分選擇Reeds-Shepp曲線、連續(xù)曲率曲線或半連續(xù)曲率曲線，但沒有提出環(huán)境感知模塊的細節(jié)。YOON等[16]基于兩種啟發(fā)式算法提出A*算法，這兩種啟發(fā)式算法分別考慮有障礙物和無障礙物時車輛運動學特性。

本文從系統(tǒng)的角度對AVPS系統(tǒng)進行功能集成與仿真驗證。

1 場景定義

場景是ADAS系統(tǒng)和自動駕駛系統(tǒng)開發(fā)和測評的關鍵因素，是ADAS系統(tǒng)和自動駕駛系統(tǒng)進行功能需求分析和測試評價的基礎，同時也決定系統(tǒng)的工作范圍，因此，如何定義功能開發(fā)和測試場景一直是ADAS系統(tǒng)及其他智能車技術的研究重點。鑒于本文的研究目的，在此只做駕駛場景的定義，并基于定義的駕駛場景進行系統(tǒng)設計與驗證。

自主代客泊車(AVP)有望成為自動駕駛領域最早實現(xiàn)落地并轉化商業(yè)價值的應用場景之一。簡單來說，自主代客泊車就是指車輛到達商場、園區(qū)等某個相對封閉環(huán)境或分時租賃的還車范圍以后，用戶便可下車辦事，同時只需在手機端下達停車指令，車輛就可自動從下車地點規(guī)劃行駛路徑，開進停車場的指定車位停好；需要取車時，用戶也可通過手機端發(fā)出指令，一鍵召車。整個泊/取車過程沒有人的參與，是一個典型的限定區(qū)域內低速無人駕駛應用場景。

AVPS系統(tǒng)的最終目標是通過智能基礎設施和V2I通信提供AVP服務。AVP服務的駕駛場景由一系列駕駛操作組成，例如向前駕駛(圖1中的A到B)、臨時停車點(圖1中的B)、平行或垂直停車(圖1中B到C屬于平行泊車，B到D屬于垂直泊車)、完全停車(圖1中的C或D)[17]。

智能基礎設施由多臺服務器、分布式感知傳感器和V2I通信系統(tǒng)組成[18]。分布式感知傳感器(如相機、激光雷達和超聲波傳感器)用于檢測停放車輛并跟蹤正在運動的車輛。相應的信息被發(fā)送到一個局部狀態(tài)感知服務器，對車輛位置和車位占用進行估算并將結果發(fā)送到全局狀態(tài)感知服務器，如圖1所示。

圖1 AVPS場景定義Fig.1 AVPS scenario definition

2 環(huán)境模型層

停車場的優(yōu)點之一是車輛可以使用來自基礎設施的地圖信息。地圖數(shù)據(jù)可以通過許多不同的方式使用。環(huán)境模型代表環(huán)境地圖，對于AVPS系統(tǒng)，該地圖包括可用和被占用的停車位、道路標記以及諸如行人或其他車輛的障礙物信息。占用概率地圖是這種環(huán)境模型的常見表示形式。環(huán)境地圖通常通過集成來自激光雷達和相機傳感器的觀測數(shù)據(jù)來實現(xiàn)即時定位與地圖創(chuàng)建(simultaneous localization and mapping, SLAM)。本文旨在得到一個更簡單的場景，如車輛通過基礎設施(V2I)系統(tǒng)或俯瞰整個停車位的相機提供地圖。使用停車場的靜態(tài)地圖，并假定車輛的自定位是準確的。本文所建停車場環(huán)境模型主要包含3個占用柵格層(圖2)：①靜止障礙層包含靜止障礙物，如墻壁、障礙物和停車場的邊界；②道路標記層包含道路標記信息，包括停車位的道路標記；③停放車輛層包含已被占用停車位的信息。

(a)靜止障礙層 (b)道路標記層 (c)停放車輛層圖2 停車場環(huán)境分層模型Fig.2 Layered environment model of the parking lot

每一個地圖層都包含不同種類的障礙物，它們代表不同程度的危險。通過這種結構，每個層都可以獨立地處理、更新和維護。在每一層中，黑色單元代表已占用，而白亮單元代表可用。為簡單起見，將3個圖層合并為一個代價地圖，如圖3所示。代價地圖將車輛環(huán)境表示為二維占用網(wǎng)格。單元格中的每個網(wǎng)格都具有介于0和1之間的值，表示單元格的成本。障礙物成本較高，而可用空間成本較低。如果一個單元的成本高于占用閾值，則該單元被視為障礙物，如果成本低于可用閾值，則該單元是可用的。

圖3 停車場環(huán)境組合模型Fig.3 Combined environment model of the parking lot

3 路徑規(guī)劃層

自主代客泊車的路徑規(guī)劃問題可以看作是一個簡單的2D路徑規(guī)劃，因為停車場的坡度是可以忽略不計的(與道路行駛相比，泊車總路徑長度相對較短，可以忽略道路坡度而產生的誤差)，因此，2D路徑足以用于AVPS系統(tǒng)的參考路徑設計。為了生成參考路徑，應該考慮車輛運動的特性，而不是設計不可跟蹤的不合理路徑。如果轉向角是恒定的，并且車輛的速度足夠低而不使輪胎力飽和，則車輛的運動可近似看成沿后軸中心行駛的圓周運動。從而，停車過程中，車輛可以被視為具有離散控制輸入的Reeds-Shepp Car?？刂戚斎氚ǎ河珊愣ò霃较蜃?、向右轉向輸入組成的橫向運動，由向前、向后的恒定速度輸入組成的縱向運動。事實證明，48個不同的組合足以完全描述Reeds-Shepp Car的最短路徑[19]。停車場區(qū)域使用簡單的路徑生成算法，如Reeds-Sepp Car。因為停車場的大小和每個停車位之間的間隔是規(guī)范化的，這意味著使用標準停車場定義的路徑可以被用于幾乎所有停車場。

在48個Reeds-Shepp曲線中，曲線-曲線-直線(CCS)用于生成垂直停車的參考路徑，如圖4所示。在二維平面上，除了目標點外，還需要定義3個點，本文中，曲線與直線之間的點被定義為切點，兩條曲線之間的點被定義為中間點，連接在自由行駛路徑和曲線之間的點被定義為轉折點。通過這3個標準點，可完全描述參考路徑。為了定義這些點，必須計算半徑R1和R2、中心弧角θ和直線的距離dt。基于停車場環(huán)境地圖和目標停車位的位置、航向信息生成參考路徑。為避免與已停放車輛發(fā)生碰撞，應以最低安全邊界cmgn計算dt，計算公式如下：

(1)

(2)

dt=Lpf-dm

(3)

式中，WP為車位寬度；dm為縱向邊界；Vw為車輛寬度；R1為切點與中間點之間的圓弧半徑；Lpf為停車位前端與目標點之間的長度。

圖4 路徑生成示意圖Fig.4 Path generating schematic

dt定義后，基于停車位的尺寸計算θ，計算公式如下：

(4)

(5)

式中，cpath為圖4中停車位與預行駛路徑之間的安全邊界。

為了簡單起見，假設R1和R2是相同的，并由Ackerman的角度公式計算如下：

(6)

式中，L為車的軸距；δ為前輪轉向角。

則定義參考路徑所需的點確定如下：

(7)

式中，xGoal、yGoal為目標點的橫縱坐標；xtan、ytan為切點的橫縱坐標；xmid、ymid為中間點的橫縱坐標；xturn、yturn為轉折點的橫縱坐標。

4 行為層

規(guī)劃是將所有相關信息組織到分層結構中，每個連續(xù)的層負責更細分的任務。行為層位于頂部，負責通過提供一系列導航任務來激活和管理任務的不同部分[20]。行為層匯集來自系統(tǒng)所有相關部分的信息,包括以下各部分的信息：

(1)定位。行為層檢查定位模塊以估算車輛的當前位置。

(2)環(huán)境模型。感知和傳感器融合系統(tǒng)反映車輛周圍環(huán)境的地圖。

(3)車位檢測。分析地圖以確定最近的可用停車位。

(4)全局路徑搜索。路徑模塊基于地圖服務或從V2I基礎設施獲得的路網(wǎng)計算全局路徑。將全局路徑分解為一系列道路段，可以對每個連接的路徑段進行不同的規(guī)劃，例如，不同的泊車方式，其最后的泊車行為需要不同的速度曲線。推而廣之，分段規(guī)劃對不同車速限制、不同車道數(shù)量和不同道路標志的街道的導航也至關重要。

本文不是依靠車輛傳感器來構建環(huán)境地圖，而是通過車輛與基礎設施V2I通信從智能停車場獲取地圖。為簡單起見，假設地圖采用占據(jù)網(wǎng)格的形式，由V2I提供道路連接和可用停車位的位置。全局路徑規(guī)劃被描述為遍歷一系列路徑段以到達停車位。全局路徑規(guī)劃由路徑規(guī)劃列表構成，主要包含起始姿態(tài)(x0,y0,θ0)、終止姿態(tài)(xn,yn,θn)以及路段屬性(如限速信息)。以當前姿態(tài)并根據(jù)路徑規(guī)劃中關鍵點的創(chuàng)建行為來規(guī)劃輔助目標。行為請求用于從行為規(guī)劃器請求一系列導航任務，直至到達目的地。每個路徑段的車輛導航步驟如下：

(1)運動規(guī)劃。通過環(huán)境地圖規(guī)劃滿足車輛約束的可行路徑。

(2)路徑平滑。通過擬合樣條曲線來平滑參考路徑。

(3)軌跡生成。通過生成速度曲線將平滑路徑轉換為軌跡。

(4)車輛控制。基于所設計的橫向控制器執(zhí)行軌跡。

(5)終點檢測。檢查車輛是否到達目的地。

4.1 運動規(guī)劃

給定全局路徑的情況下，運動規(guī)劃用于規(guī)劃通過環(huán)境的路徑以到達每個輔助目標點，直到車輛到達最終目的地。每個路徑段的規(guī)劃路徑必須可行并且無碰撞。一條可行的路徑是指車輛在運動學和動力學約束條件下實現(xiàn)的路徑。AVPS系統(tǒng)涉及低速和低加速，可忽略由慣性效應引起的動力學約束。

路徑規(guī)劃算法通過構建連續(xù)且無碰撞車輛姿態(tài)樹來找到路徑。使用Reeds-Shepp轉向連接姿態(tài)，確保生成的路徑在運動學上可行。除規(guī)劃的參考路徑外，還應考慮車輛行駛的無碰撞區(qū)域，這些區(qū)域表示車輛的參考點不得越過代價地圖區(qū)域，以避免碰撞任何障礙物。路徑規(guī)劃器檢查并確保產生的車輛姿態(tài)不在這些區(qū)域上，規(guī)劃避開障礙物的路徑。

4.2 路徑平滑

路徑規(guī)劃器生成的參考路徑由Reeds-Shepp線段組成。兩條線段連接處的曲率可能不連續(xù)，導致轉向角度突然改變。為了避免這種不自然的運動以確保乘客的舒適度，路徑需要連續(xù)可微并且平滑[21]。本文采用的平滑路徑的方法是擬合三次樣條曲線，通過擬合參考路徑中所有輔助目標點的三次樣條曲線進行路徑平滑，生成控制器可以執(zhí)行的平滑路徑。樣條曲線的起始和結束方向與車輛的起始和結束方位角近似匹配。

4.3 軌跡生成

軌跡生成是將平滑路徑轉換為可基于速度曲線執(zhí)行的軌跡。按照三個階段的順序計算每個路徑的速度曲線：加速到設定的最大速度、保持最大速度和減速到終點速度。定義初始速度、最大速度和終點速度，得到包含平滑路徑上每個點參考速度的軌跡曲線。

4.4 車輛控制

AVPS系統(tǒng)最具挑戰(zhàn)性的問題之一是在給定的駕駛條件下對車輛進行建模。用于橫向控制的自行車模型已經(jīng)被廣泛用于公路駕駛[22-23]。假定側向力與滑移角成正比，并且所有的車輪轉角都很小，但停車時會產生較大的車輪轉角，而在現(xiàn)實中不容易識別出不同車輛的剛度系數(shù)等車輛參數(shù)。

對于AVPS系統(tǒng)，車輛以低速行駛，因此滑移角在本文中可被忽略。運動學模型可以用于前向運動和后向平行/垂直泊車，并且可以被寫成如下形式[24-25]：

(8)

式中，(x，y)為車輛的質心坐標；ψ為車輛的航向角；v為縱向速度。

此外，考慮轉向執(zhí)行機構的運動約束，即轉向盤轉角與前輪轉向角成正比，前輪轉向角和前輪轉向角速度有界，運動學約束如下：

(9)

式中，γ為轉向盤轉角；Rj為轉向系統(tǒng)的傳動比；ω為前輪轉向角速度。

基于式(8)和式(9)中的車輛模型和動態(tài)面控制(dynamic surface control,DSC)設計橫向控制器[17,26]。由于橫向控制器應該具有足夠的魯棒性，以補償由各種車輛類型和駕駛操縱引起的模型不確定性和干擾，且計算出的轉向角應該足夠平滑以滿足式(9)中的運動學約束，所以應用DSC技術設計橫向控制器。橫向控制器的設計流程如下。

定義誤差曲面為

Si=ey+dieψ=ey+di(ψd-ψ)

(10)

式中，下標i表示駕駛動作，即f是前進行駛，b是后退行駛；Si為誤差曲面；di為預瞄距離；eψ為橫擺角誤差；ψd為期望的橫擺角。

其中，橫向誤差ey定義為

圖5中，di和ψd的定義如下：

di=‖epd‖

ψd=∠epd

圖5 預瞄算法原理圖Fig.5 Preview algorithm schematic

應該注意的是，預瞄控制思想通過考慮等式(10)中的di和ψd(圖5)，被納入動態(tài)面控制中[25]。圖5中Pd(xd,yd)不是將ψd定義為切線角度，而是通過使用Pd和Ppd之間的角度以及預瞄距離di來定義。

對式(10)中Si取導數(shù)，結合式(8)有

(11)

(12)

為了滿足式(9)中的第二運動約束，即轉向執(zhí)行器的角速度極限，通過如下的低通濾波器對式(11)中與期望轉向角相關的項進行濾波：

(13)

式中，τ為濾波時間常數(shù)。

然后，基于式(9)，可得轉向盤轉角

γ=Rjarctangdes

(14)

5 仿真驗證

PreScan是一款可用于ADAS和智能車輛(IV)系統(tǒng)仿真測試的軟件工具。在 PreScan軟件中可以很方便地搭建用于驗證和評估ADAS和IV系統(tǒng)的車輛虛擬行駛環(huán)境以及相關傳感器，如毫米波雷達、激光雷達、超聲波雷達、相機、GPS和V2X通信。本文基于PreScan-MATLAB聯(lián)合仿真環(huán)境實現(xiàn)對AVPS系統(tǒng)的功能集成與有效性驗證。整個自主泊車過程可以分成以下步驟。

(1)基于智能停車場的V2I通信服務，AVPS系統(tǒng)接收停車場的環(huán)境地圖以及目標可用車位信息。

(2)基于環(huán)境地圖與目標車位信息生成輔助目標點，規(guī)劃全局路徑，并生成軌跡。

(3)車輛向前行駛跟蹤全局軌跡，同時超聲波雷達實時檢測，一旦超聲波檢測結果與V2I發(fā)送結果一致，則確認車位檢測成功。

(4)發(fā)現(xiàn)車位后車輛減速，建立大地直角坐標系，同時實現(xiàn)路徑規(guī)劃。全部計算完畢后，車輛開始倒行泊車。

(5)通過實時橫向控制器計算轉向角度，車輛自動泊車，泊車修正，泊車完成。

PreScan場景模型、聯(lián)合仿真模型及仿真結果如圖6～圖8所示。

(a)二維場景 (b)三維場景圖6 PreScan場景模型Fig.6 PreScan scenario model

圖7 PreScan-MATLAB聯(lián)合仿真模型Fig.7 PreScan-MATLAB co-simulation model

(a)創(chuàng)建環(huán)境模型 (b)生成輔助目標點

(c)路徑規(guī)劃 (d) 軌跡生成

(e)軌跡跟蹤 (f)泊車操作圖8 AVPS聯(lián)合仿真結果Fig.8 AVPS co-simulation results

6 結語

為了提高駕駛員泊車的便利性和停車場的安全性，結合V2I通信服務實現(xiàn)特定場景下的環(huán)境建模，利用Reeds-Shepp Car曲線進行路徑規(guī)劃并生成運動軌跡，基于預瞄控制理論與動態(tài)面控制技術實現(xiàn)車輛前向駕駛與后退泊車的控制，在PreScan-MATLAB聯(lián)合仿真環(huán)境下實現(xiàn)對所設計系統(tǒng)的功能集成與有效性驗證。結果表明該系統(tǒng)可在特定場景下實現(xiàn)自主代客泊車功能，后續(xù)將在功能樣車上做更加充分的驗證，同時，場景的普適性與動態(tài)避障也是重點研究的方面。