王瑞 孫濤 賴杰

摘要：路徑規(guī)劃算法研究是自動泊車系統(tǒng)中最常見的問題之一。為解決平行泊車場景下，車輛泊車入位較難的問題，采用反向推導法，理論分析泊車過程并推導了路徑規(guī)劃函數(shù)公式，在確定泊車起點后，能夠安全引導車輛進入泊車位。首先建立車輛模型，在確定碰撞約束和最優(yōu)目標函數(shù)后，推導了車輛三階段單向行駛泊車路徑;然后針對單向泊車路徑規(guī)劃中要求車位長度較長的問題，提出四階段行駛路徑規(guī)劃方法;最后，根據(jù)實際車輛及車位信息，利用Matlab軟件與實驗車平臺，分別驗證了該路徑規(guī)劃方法的有效性。實驗結果表明，采用雙向四階段泊車方式相比單向三階段泊車方式，對車位長度的要求縮短了0.26m。

關鍵詞：路徑規(guī)劃;平行泊車;車輛模型;軟件仿真

DOI： 10. 11907/rjdk.192711

開放科學（資源服務）標識碼（OSID）：

中圖分類號：TP312

文獻標識碼：A

文章編號：1672-7800（2020）004-0119-06

Research and Verification of Parallel Parking Path Planning Algorithm

WANC; Rui. SUN Tao . LAI Jie

（School of Mechanical Engineering ， Univer.sity of .Shanghaifor Science and Technology ， Shanghai 200093，China ）Abstract： Research on path planning algorithms is one of the most common problems in vehicle parking systems. This research is ap-plied to the path planning probleru in the parallel parking scenario. The reverse derivation method is used to theoretically analy ze theparking process and derive the path planning function formula. After the parking starting point is determined. the vehicle can be safelyguided into the parking space. This paper first estahlishes a vehicle model. After determining the collision constraints and the optimalobjective function. the f'ormula derives the three-phase one-way parking path of' the vehicle. Then， the problem of the long parkingspace required in the one-way parking path planning is proposed. Four-stage driving path planning method. Finally， according to theactu al vehicle information and parking space information . the parking path trajectory is simulated using Matlab software. Finally ， a testvehicle is used as a platform to establish a software and hardware test platform for real vehicle verification. The results shoW the effec-tiveness of the proposed method for path derivation. The test results show that the two-way four-stage parking method requires 0.26mshorter parking space than the one-way three-stage parking method.Key Words ： path planning ; parallel parking ; vehicle model; software siiu ulation

O 引言

根據(jù)公安部教管局統(tǒng)計，截至2019年上半年，全國汽車保有量達到2.5億輛，私家車達到1.98億輛…。隨著車輛保有量近年來不斷攀升，城市交通擁堵和停車難的問題也尤為突出。據(jù)統(tǒng)計，在一線城市，司機30%的駕駛時間花在停車、找車位上[2]。采用白主泊車系統(tǒng)能較好地解決泊車難、泊車效率低這一問題。

泊車系統(tǒng)功能開發(fā)主要要求如下：控制單元必須實時監(jiān)控車輛運行狀態(tài)，并檢測停車環(huán)境的變化。當檢測到的泊車位滿足泊車要求，即檢測到的泊車位長度大于泊車所需的最小長度時，系統(tǒng)將提示駕駛員。如果駕駛員接受停車位，系統(tǒng)將切換到自動停車模式，接管車輛轉向，自動控制停車速度，完成停車過程[3]。目前對于白動泊車方面的研究主要分為兩大類。[4]：一類是基于人工智能的控制方法，通過系統(tǒng)的白學習能力學習駕駛員的泊車經驗，利用神經網(wǎng)絡或模糊控制實現(xiàn)泊車;另一類是基于對路徑規(guī)劃的研究，利用車輛感知到的車位信息及車輛運動狀態(tài)預先規(guī)劃出一條泊車路徑，通過控制車輛行駛軌跡實現(xiàn)泊車過程。如文獻[3]通過建立車輛動力學模型，運用數(shù)學方法設置泊車環(huán)境及尺寸條件，基于模糊控制理論方法白學習駕駛員倒車經驗，設計控制器操縱并穩(wěn)定車輛轉向，最后在Matlah中驗證整個泊車過程;文獻[4]運用反正切函數(shù)及改進的反正切函數(shù)對兩段網(wǎng)弧式泊車路徑進行逼近擬合，并在Matlab中進行泊車仿真驗證。自動泊車研究的關鍵問題是路徑規(guī)劃，大部分研究是將泊車路徑簡化為兩段圓弧[5]，而本文對泊車路徑規(guī)劃問題的研究基于三段式泊車路徑。汽車從尋找車位起始點至泊車人位結束，車輛行駛的3個階段為：車輛由起始位置行駛至泊車起點的路徑、第一段圓弧路徑以及第二段圓弧路徑。基于兩段圓弧式的泊車路徑能解決一般泊車路徑的規(guī)劃問題[6]，但在實際泊車過程中，由于車輛傳感器的執(zhí)行誤差或系統(tǒng)執(zhí)行上的誤差累積，車輛可能在未執(zhí)行完規(guī)劃路徑時，后邊緣邊界就會發(fā)出碰撞預警，使車輛無法繼續(xù)行駛。針對這一問題，要考慮被迫停車后的二次路徑規(guī)劃甚至多次路徑規(guī)劃，直到車輛成功停入泊車位。

1 泊車路徑規(guī)劃

1.1 車輛模型建立

在開發(fā)泊車系統(tǒng)功能時，主要是研究車輛泊車軌跡與速度、轉向角等參數(shù)之間的關系[7]。汽車在泊車過程中的軌跡主要包括車身輪廓上關鍵點與后軸中點的運動軌跡。為了方便對以上參數(shù)的研究，首先基于阿克曼轉角建立車輛動力學模型，在不考慮側向加速度以及車輪打滑的情況下，根據(jù)泊車過程中的汽車位置和運動方向建立坐標系。構建車輛模型如圖1所示。

其中，a、b、c、d4點為車輛矩形輪廓的4個邊界點，A（Xr，Yr），B（Xf，Yf）為汽車前后軸的中點坐標。

在整個泊車過程中，車輛行駛速度都較慢，車輛后軸方向速度接近于零[8]。位移和速度的關系表現(xiàn)為：

其中車輛前軸中心點B（Xf，Yf）與后軸中心點A（Xr，yr）之間的關系可表示為：由圖1可知，前軸中心點B的X、Y方向速度公式為：根據(jù)以上方程，經過一系列公式推導可得：通過積分可得：

由上式可知，后軸中心的運動軌跡是圓弧，也即是說在不考慮汽車打滑和側向加速度的情況下，車輛低速行駛。車輛后輪軌跡只與車的軸距AL及轉向角 有關，與車輛泊車速度無關[9]。

1.2關鍵點轉彎半徑求解

在車輛轉向行駛時，路面對車輛輪胎產生橫向阻力和摩檫力，會加快對輪胎的磨損。在車輛研發(fā)設計時，應保證汽車在轉向時車輪僅作滾動運動，顯然車身走過的圓弧線會交于一點，該交點0稱為轉向中心[10]。泊車動作是一種低速運動，阿克曼轉向系統(tǒng)下的4個車輪分別圍繞左、右轉向系統(tǒng)的瞬時旋轉中心滾動且不打滑。首先，計算出不同轉彎半徑，以E為后軌道中心，對車輛與關鍵點的轉彎半徑進行計算。

建立的阿克曼轉向幾何模型如圖2所示，用于計算汽車的轉彎半徑。由圖2可知，汽車后軸中心的轉彎半徑R為：汽車車頭有角的轉彎半徑Rn為：汽車車后左角的轉彎半徑 為：

汽車車后右角的轉彎半徑 為：

1.3泊車路線推導

車輛在有泊車需求時，通過車位檢測尋找車位，之后進入泊車初始位置。設泊車初始位置為A（Xr0，Yr0），以圖3中泊車場景為例，首先將方向盤向右轉一定角度（設角度大小為W，），使其以01為同心、R1為轉彎半徑作轉向運動;然后方向盤向左轉 角度，使其以Oz為同心、R2為轉彎半徑作轉向運動;到達泊車終點時，回正汽車方向盤，此時車身航向角剛好為零[11]。

但車輛在按照既定路徑規(guī)劃行駛時，因受到空間限制，有可能會與周圍車輛及車位障礙物發(fā)生碰撞：①泊車過程中，車輛右前端與前方障礙物發(fā)生碰撞;②泊車過程中，車輛后端與后方障礙物發(fā)生碰撞;③泊車過程中，車輛右后端與右側車位邊界障礙物發(fā)生碰撞[12]，如圖4所示。

為方便描述，將圖4（a）、（b）、（c）所示碰撞約束分別稱為應滿足的第一約束條件、第二約束條件和第三約束條件。

根據(jù)以上約束，對車輛每段運動軌跡進行推導。第三階段泊車路線如圖5所示。

為確保最后泊車成功，對整個泊車軌跡推導采用反向推導法。首先確定泊車終點位置，然后依次確定第三階段和第二階段運動軌跡。QWER代表車位矩形的4個邊緣點，假設車輛在車位停好后，車輛后端與車庫邊緣距離為d。

根據(jù)圖5，最優(yōu)軌跡路線推導可列出如下約束關系式：將公式（7）帶人公式（l0），可得：由第一約束條件知：OQ≥RFr。

將公式（II）帶入第一約束條件中，得到：

根據(jù)第三約束條件，可知有一個最大值pman，p

據(jù)此，由公式（11）計算得到d的值，然后可以得出符合約束條件下車輛右輪轉彎半徑和車輛方向盤轉角w2的大小[13]。

第二階段泊車路線如圖6所示。

由圖6可知：

車輛在泊車起點時，由于汽車側向速度為零，可得：由式（12）-式（14）得到：

綜上可得w2與d值大小，再將一個確定的o帶入公式（1）即可得到w1。顯然，在起點坐標X最接近x值時，可得到一個最優(yōu)的o和w1對于o和w1.的確定，主要是基于實際倒車起點最接近最優(yōu)倒車起點，每次實際泊車過程中的起點與反向推導法推導出來的泊車起點肯定有一定誤差，因此只要控制實際倒車起點與最優(yōu)倒車起點的誤差值最小即可[14]。

差值可表示為：

該階段需要滿足的約束條件如下：

（1）汽車方向盤轉角必須小于最大方向盤轉角，即

（2）汽車右前邊角不能碰到車位邊界，即 （△L為車輛軌跡距離車位左上角邊界的最小值）。

如圖5所示，在確定泊車起始點位置后，可首先將方向盤左打一定角度設為 ，設由點2到點3曲線運動時車輛方向盤轉角為 ，汽車運動到點2時，汽車車身與X軸的夾角為 ，則可得到不同的平行泊車軌跡。在眾多停車軌跡中，尋求最優(yōu)停車路徑。

在實際工程應用過程中，在成功識別車位后，車輛按照路徑規(guī)劃情況進行泊車入位，可能由于執(zhí)行傳感器精度不足或平行車位較短，在第三階段泊車路徑規(guī)劃還未執(zhí)行完成時，車輛就發(fā)出后方碰撞預警[15]，如圖7所示。針對這種情況，提出更多一階段的路徑規(guī)劃，即雙向泊車路徑規(guī)劃，此時車輛需要向前行駛一段距離，以準確進入車位。

類似于三階段路徑推導方法，在考慮避障約束條件下，首先確定泊車終點位置。在約束條件下，為了盡量加大車輛與后方車位邊界的距離，應使點3、4之間的水平位移盡量小。設水平距離為：

由此可知，由點3到點4，該階段車輛應采用最大的方向盤轉角 。

最后聯(lián)立以上兩式。

點2到點3軌跡的確定可參考三階段泊車路徑規(guī)劃思想，在碰撞約束條件下進行最優(yōu)泊車路徑選擇，由式（13）得到V2和f9：，接著使實際倒車起點與規(guī)劃最優(yōu)起點橫坐標差值AX最小，計算得到，和的值，即為所求結果。

2仿真實例與實車驗證

2.1路徑規(guī)劃仿真

針對已有車輛信息，基于實際車輛參數(shù)，分別對三階段和四階段路徑規(guī)劃進行平行泊車仿真。在Matlab仿真平臺中，利用GUI模塊T具，制作一個簡易的上位機，在窗口中輸入車輛信息和車位約束條件后，即可對完整的泊車過程進行仿真。若輸入的約束條件不能滿足泊車要求，則無法顯示泊車過程，意味著泊車失敗。分別采用三階段和四階段路徑規(guī)劃方式進行平行泊車仿真驗證。如圖8、圖9所示，當車位長度為6.3m時，在單向三階段路徑規(guī)劃下可以順利泊車入位。當長度減少為6m時，進行單向路徑規(guī)劃時后方有碰撞隱患，無法繼續(xù)執(zhí)行下去，泊車過程失敗。此時采用雙向軌跡路徑規(guī)劃可以成功泊車人位。

綜上所述，在同一泊車環(huán)境中，針對于長度不同的平行車位，車輛可按照推導軌跡安全進入泊車位，表明采用單向三階段路徑規(guī)劃方法與雙向四階段路徑規(guī)劃方法均能滿足車輛泊車入位要求，并且采用雙向四階段路徑規(guī)劃對泊車車位長度要求較低。

2.2實驗驗證

以某品牌SUV為實驗平臺，如圖10所示。首先搭建軟硬件系統(tǒng)平臺，系統(tǒng)計算平臺由工控機、控制器等設備組成。軟件系統(tǒng)以OT與ROS平臺為基礎，建立白動泊車軟件操作平臺，通過OBD接口中的CAN信號實時監(jiān)測車輛狀態(tài)，獲取車輛運動信息，并實現(xiàn)與汽車底盤系統(tǒng)的通訊。

工控機中集成了豐富的1/0接口，其內部運行程序主要是傳感器數(shù)據(jù)處理和路徑規(guī)劃等。實驗平臺選擇DSPACE公司的MicroAutoBox作為控制器，其包含豐富的CAN總線接口和模擬信號輸入輸出接口，能夠運行控制程序并與底層控制系統(tǒng)相連，將控制信號發(fā)送到車輛底盤系統(tǒng)。

軟件系統(tǒng)選擇ROS（Rohot Operating Systein）系統(tǒng)作為操作平臺，ROS也被稱為機器人操作系統(tǒng)，但在車輛無人駕駛技術功能開發(fā)中，也會更多地用到這個平臺。與嵌入式操作系統(tǒng)相比，ROS系統(tǒng)對進程間通信的實現(xiàn)更加容易。另外，其強大的調試工具也是智能駕駛開發(fā)者更傾向于選擇使用它的一個重要原因[16]。

實際泊車過程如圖12所示，車輛參數(shù)如表1所示，實車實驗結果如表2所示。

通過仿真及實車驗證，兩者實驗結果雖然存在一定誤差，但大致能得出相同結論。在相同的泊車環(huán)境下，實驗結果表明，采用雙向四階段泊車方式相比單向三階段泊車方式對車位長度的要求縮短了0.26m，雙向泊車規(guī)劃方法有效減小了所需車位長度，從而放寬了泊車約束條件。因此，在平行泊車場景下，車輛能夠適應的泊車條件也更為寬松。

3 總結與展望

本文通過研究平行泊車場景下的路徑規(guī)劃問題，基于反向推導法推導了平行泊車路徑規(guī)劃函數(shù)公式，詳細分析了整個泊車過程碰撞約束條件。通過明確泊車軌跡中的碰撞約束，確定了約束函數(shù)及規(guī)劃目標，進而推導得出整個泊車路徑。對兩種軌跡方法進行仿真及實車實驗，結果表明，在相同的泊車環(huán)境下，雙向泊車規(guī)劃方法有效減小了所需車位長度。因此，在車位長度進一步減小的情況，可基于該思想推導出更多階段的路徑規(guī)劃方法，對應泊車過程相對也會更加繁瑣。值得注意的是，在實際應用中，考慮傳感器及其它硬件設備T作誤差，推導路徑規(guī)劃方法時，應適當增加車輛在車位內的安全距離余量[17]。另外，路徑規(guī)劃的工程實現(xiàn)還要考慮許多其它因素，如車輛行駛的平穩(wěn)性以及對車輛傳感器磨損影響程度等。在泊車實驗時發(fā)現(xiàn)，本文提出的泊車方法中，在上一階段轉向下一階段路徑的銜接處，車輛行駛出現(xiàn)了中間停頓的情況。因此，針對該問題，算法仍存在需要改進的地方。

參考文獻：

[1]搜狐網(wǎng)交管局：2019年上半年全國汽車保有量達2.5億輛[EB/OL]. http：肌www.sohu.com/a/327997780_242272.

[2]新浪科技.李彥宏：AI雖然不能產生萬物但是可以喚醒萬物[EB/OL l.https： //tech.sina.cn/i/gn/201 9-07-03/detail-ihvtcitk9339362.d.html：l vt=4.

[3]SHIDIAN M. HAOBIN J.Research on automatic parking systemshased on parking scene recognition[J].IEEE Access， 2017，7：44-45.

[4]李紅，王文軍，李克強基于B樣條理論的平行泊車路徑規(guī)劃[J]中國公路學報，2016，29（9）：143-151.

[5]穆加彩，張振東，管登詩，等.自動泊車系統(tǒng)路徑規(guī)劃與控制研究[J]軟件導刊，2017（5）：117-121.

[5]PAROMTCHIK I E，LAUGIER C.Autonomous parallel parking andreturning to traffic maneuvers[J]. Intelligent Rnbotics andSvstem，1997.3：21-23.

[3]趙海蘭，高松，孫賓賓，等全自動平行泊車路徑規(guī)劃方法研究[J].科學技術與工程，2017（7）：268-274.

[7]李大森.自主泊車系統(tǒng)關鍵技術研究[D].廣州：華南理工大學，2014

[8]PIAO C H.ZHANG L. LL S，et al. Research on path planning of auto-matic p arking system[J] Advanced Materials Research. 2012， 590：416-420.

[9]宋金澤.自主泊車系統(tǒng)關鍵技術研究[D].長沙：國防科技大學，2009

[10]王道斌，梁華為，楊妮娜，等.兩種自主泊車路徑規(guī)劃方法的對比研究[J]電子測量技術，2011，34（1）：27-30，38.

[II]LIU S， AN X. SHANG E.et al.A path planning method for assistantparallel car-parking[C].Computational Intelligence and Design（IS一CID）， 2012 Fifth International Symposium on. IEEE Computer Soci-etv.2012.

[12]郭夜啼多段式曲線擬合的平行泊車路徑規(guī)劃研究[D]蘭州：蘭州交通大學，2019.

[13]侯曉陽.基于路徑規(guī)劃的平行泊車系統(tǒng)研究[D].北京：清華大學，2015.

[14]吳中偉乘用車自動泊車系統(tǒng)路徑規(guī)劃與仿真分析[D]廣州：華南理工大學，2018.

[15]VOROBIEVA H，CLASER S， MINOIL： -ENACHE N，et al. automaticparallel parking in tiny spots： path planning and control[J]. IEEETransactions on Intelligent Transportation Svstems. 2015. 16（1）：396-410

[16]高瑞金.基于毫米波雷達和視覺信息融合的自動泊車系統(tǒng)研究[D].上海：上海交通大學，2018

[17]侯曉陽，黃勇，孫思，等.多約束平行泊車路徑規(guī)劃[J].汽車技術，

2015（ 9）：52-56

（責任編輯：黃健）

收稿日期：2019-12-05

作者簡介：王瑞（1992-），男，上海理工大學機械工程學院碩士研究生，研究方向為智能駕駛路徑規(guī)劃及電子控制;孫濤（1974-），男，博

士，上海理工大學機械工程學院副教授、碩士生導師，研究方向為汽車系統(tǒng)動力學及電子控制;賴杰（1984-），男，上海理工

大學機械工程學院教授級高級工程師，研究方向為汽車智能駕駛。