基于狹窄垂直車位的多段式路徑規劃與仿真分析

蔡 云，王 廣，張又水，吳澳琦，陳 銳

基于狹窄垂直車位的多段式路徑規劃與仿真分析

蔡 云1,2，王 廣1,2，張又水1,2，吳澳琦1,2，陳 銳3

（1.西華大學 汽車與交通學院，四川 成都 610039；2.汽車測控與安全四川省重點實驗室，四川 成都 610039；3.成都路行通信息技術有限公司，四川 成都 610041）

自動泊車系統（APS）目前受到廣泛的關注，但大部分研究忽略了車輛初始航向角偏差對整體泊車效果的影響，對此，論文研究了狹窄垂直車位場景下車輛初始航向角不為零的多段式路徑規劃方法。首先基于車輛最小轉彎半徑建立了車輛后軸中心運動軌跡方程，通過五次多項式對路徑規劃進行優化，實現路徑曲率更連續、泊車更平順和舒適。CarSim/Simulink聯合仿真驗證表明，五次多項式泊車路徑優化方法可行，在滿足泊車安全的基礎上，泊車位所需實際寬度減少10.4%，長度減少15.3%。

垂直泊車；路徑規劃；五次多項式；模型預測控制；聯合仿真；CarSim/Simulink

隨著高級駕駛輔助系統（Advanced Driving Assistant System, ADAS）在汽車市場的逐漸普及，作為ADAS重要組成部分的自動泊車系統（Auto- matic Parking System, APS）也得到了更多的關注[1]。我國汽車保有量的逐年增加與泊車位供應不平衡之間的問題愈發突出，狹窄車位逐漸增多，泊車環境日益復雜，交通事故頻發。開發基于狹窄垂直車位的自動泊車系統，一方面可以將駕駛員從繁瑣的泊車操縱中解脫出來，降低交通安全事故發生概率，促進自動駕駛技術的發展[2]；另一方面可以大幅提升土地資源利用率，緩解當今“車多位少”的局面。國內外很多學者對路徑規劃技術做了研究[3]，包括logistic曲線法[4]、回旋曲線法[5-7]、B樣條曲線法[8]、Bezier曲線[9]、三角函數曲線法[10]、ReedsShepp（RS）曲線法[11]以及多項式曲線法[12]等。雖然針對自動泊車系統研究取得了不俗的進展，但是當前的大部分泊車路徑規劃算法未考慮車輛初始航向角帶來的誤差影響，同時針對交通事故更易發生的狹窄垂直車位泊車工況研究較少。因此，本文對狹窄垂直車位的多段式路徑規劃問題開展研究，采用最小轉彎半徑設計了泊車路徑，運用五次多項式進行路徑優化，并聯合CarSim/Simulink搭建垂直自動泊車仿真實驗平臺來驗證所提出的狹窄垂直車位多段式路徑規劃算法的合理性和可靠性。

1 自動泊車系統簡介及運動學模型

自動泊車系統工作流程如圖1所示。駕駛員選擇自動泊車功能后，自動泊車系統基于環境感知的車位進行路徑規劃，然后控制汽車進行軌跡跟蹤，控制汽車安全泊入停車位[13]。泊車完成后轉向盤回正并掛駐車擋，自動泊車系統關閉。

汽車輪廓外形對泊車成功率有直接影響。由于實際汽車外形繁雜多樣，在給予一定余量的條件下，設計以最大長度和最大寬度的矩形來代替汽車外形[14]。簡化后如圖2所示。本文仿真與計算的參數設定如表1所示。

圖1 自動泊車系統工作流程圖

圖2 汽車模型簡化圖

表1 汽車主要參數表 單位：m

參數數值 車長L4.634 車寬W1.855 軸距Lz2.661 前懸Lf0.785 后懸Lr1.070

選擇停車時后軸的中心點作為車輛參考點，在低速泊車工況下，根據阿克曼（Ackeman）轉向幾何理論，在低速工況下將車輛視為平面剛體，即只存在方向，方向和橫擺3個自由度。汽車在坐標系下的后軸中心坐標為（r,r），航向角為，汽車運動學模型為

式中，z為軸距；r為車速；f為等效前輪轉角。

2 狹窄垂直泊車位的路徑規劃策略

在自動泊車過程中，由于車輛后軸中心軌跡能完全體現整車的軌跡特征[15]，故采用后軸中心代表整車泊車軌跡。采取一步式泊入的路徑規劃方法所需的路徑較長，泊車區域較大，當車輛處于狹窄垂直車位工況時，道路條件很難滿足要求。為了使自動泊車系統更好地應用于實際生活中，提高乘客舒適度。針對始航向角不為零時的狹窄垂直車位工況，提出基于車輛最小轉彎半徑的圓弧-直線-圓弧的多段式路徑規劃設計。

2.1 初始航向角不為零時的路徑規劃分析

在泊車過程中，以車輛前進方向為軸方向，以車位右頂點為坐標原點，垂直方向軸設立坐標系。設汽車后軸中心點為1（x1，y1），泊車位寬度為P，泊車開始后，在第一段試探性圓弧泊入車庫的過程中，當后軸中心點到達2時，若繼續后退，車輛最右側與原點會有碰撞風險，如圖3所示。此時選擇回正方向盤前進一段距離后再右打方向盤安全泊入車庫中，其后軸中心軌跡如圖4所示。

圖3 第一段泊車路徑幾何圖

圖4 航向角不為零時泊車路徑軌跡圖

汽車相對于泊車位的初始航向角為

=arctan(b/b) （2）

為方便計算，將整個坐標系逆時針旋轉角度，旋轉后坐標系11，假設泊車起始點1坐標為(x1,y1)，汽車從1開始以最小半徑開始泊車，根據車輛自身參數以及幾何關系可以得出：

0=arctan(r/(+/2)) （3）

根據汽車一般性泊車運動規律，易證車身最右側在第一段泊車過程中與原點距離最近。設防撞閾值S=0.2 m，可計算出點1(x1,y1)的坐標為

由1可以得出2點的坐標為

當車輛到達2點后，此時調整方向盤轉角為零，沿著直線路徑到3點，設直線路徑長度為d，則d為

d=(-(O1+(b/2))/sin(π/2-1+) （7）

式中，d為23的直線距離，此時3點的坐標為

圓弧34對應的角度2為

2=π/2-1+（9）

根據2的值可得到路徑的2圓心坐標：

最后計算出4點坐標：

2.2 垂直泊車路徑改進

由于圓弧-直線組合路徑的曲率可能發生突變導致車輛原地轉向，那么基于圓弧-直線-圓弧的路徑規劃大概率會導致路徑跟蹤控制失敗。為了確?；谧钚∞D彎半徑規劃的路徑契合車輛連續轉向的特征，同時又可以實現轉向和避障約束，考慮到具有連續曲率的五次多項式[16]曲線能更好地滿足路徑的平滑度和靈活性，且運算量增加較少，故選用五次多項式對圓弧直線段進行擬合，使其曲率連續優化并滿足車輛轉向約束。

首先，針對五次多項式，曲線的公式可表達為

=55+44+33+22+1+0（12）

然后，分析車輛起點和終點的位姿狀態，根據路徑規劃起始點1和航向角可得

同理，已知終點4的坐標和航向角，則有

最后，為了保證曲線有良好的擬合效果同時確定避障約束，根據曲線經過2、3點，可得

根據式（12）—（18），可以得到擬合的五次多項式曲線，該曲線即為垂直泊車的規劃路徑。

為使生成路徑更好地滿足狹窄垂直車位場景下的車輛轉向半徑約束，將3的坐標沿當前航向角方向適當延長一段距離d1，則延長后的坐標為

同時將4向下延長一段距離d2，此時延長后4的坐標為

由于不同的中間點會生成曲率不同的曲線，將2進行適當修正并設為中間點：

式中，Δ為修正值。

由車輛運動學模型和初始航向角可知，目標車輛在1點的斜率k1=，在4點的斜率k4=π/2，因tan(π/2)=+∞，所以通過將整個坐標系逆時針旋轉π/4以方便計算，轉換之后起點和終點的斜率為

k1=-π/4+（22）

k4=1 （23）

旋轉坐標系后對應點的坐標如圖5所示。

圖5 逆時針旋轉45°的坐標軸示意圖

由圖5幾何關系可知

同樣，為了得到旋轉之前的坐標系下連續的可行曲線，再將坐標系順時針旋轉π/4，公式如下：

將上述參數代入到MATLAB中進行編程，以車輛初始航向角5°為例，求出五次多項式優化后的曲線圖，如圖6所示。

圖6 五次多項式路徑優化與原路徑對比示意圖

圖6中的實線為五次多項式優化后的曲線。圖7為優化后的曲線避撞約束驗證示意圖；圖8為優化后生成的曲線曲率圖，其最大值為0.07m-1左右，滿足車輛最小轉向半徑約束，同時曲率變化連續，保證了泊車過程中的平穩性和順暢性。運用五次多項式優化后的曲線更加平滑，避免了車輛的反復轉向和輪胎的過度磨損。

圖7 五次多項式路徑規劃避障約束驗證示圖

圖8 五次多項式規劃路徑曲線曲率示意圖

2.3 垂直泊車跟蹤控制

在自動泊車工況中，通過控制車輛的方向盤轉角與車速，使車輛沿期望的路徑行駛稱為跟蹤控制。目前主要使用的跟蹤算法有預瞄控制[17]、自適應控制[18]、純跟蹤控制[19]、模糊控制[20]、線性二次型調節器（Linear Quadratic Regulator, LQR）[21]和模型預測控制（Model Predictive Con- trol, MPC）[22]等。本文選擇MPC法對自動垂直泊車系統進行軌跡跟蹤。MPC的基本原理是根據模型科學預測系統在某一時間段內的表現進行優化控制的過程，其具有預測模型、滾動優化和反饋校正三個特點[23]，控制流程如圖9所示。

圖9 MPC控制流程圖

將式（1）的汽車運動學方程看作輸出為r=[r,r]和狀態量r=[rrr]的控制系統，其一般形式為

式中，r=[rrr]T,r=[r,r]T。

將函數泰勒數級展開，通過顯示歐拉法（For- ward Euler, FE）可得到離散系統狀態方程：

(+1)=k()+k() （27）

設采樣周期為，為當前采樣時刻，+1為下一采樣時刻。將性能評價函數設為

式中，、為權重系統矩陣；為松弛因子。

為了保證車輛能沿著規劃路徑行進，對控制量和控制增量進行約束，表示為

cmaxmin（29）

Δcmaxmin（30）

MPC的基本原理即是在滿足控制約束式（28）和式（29）的基礎下使性能評價函數式最小。一般將優化問題轉為標準二次規劃問題求解。即目標函數轉化為

((),(-1),Δ())=

[Δ()T,]Tt [Δ()T,]+t [Δ()T,] （31）

3 仿真驗證

3.1 仿真設置

基于CarSim/Simulink軟件對所規劃的泊車路徑以及控制算法進行聯合仿真。在CarSim中根據表1的參數建立車輛和泊車位模型并鏈接到Simu- link，接著在Simulink中設立基于五次多項式的路徑規劃模塊與MPC的軌跡跟蹤模塊，搭建完上述模塊后進行聯合仿真。參考《城市道路路內停車泊位設置規范》（GA/T 850—2021），城市內垂直停車位長度一般為6 m，寬度為2.5 m，在仿真中將狹窄垂直泊車位長寬設為（5.2×2.3）m。在CarSim中設置泊車場景如圖10所示。

Simulink中的聯合仿真模型如圖11所示，包括控制模塊、信號反饋模塊、參數計算模塊，以及參數顯示模塊。車輛參數沿用表1所列出的相關參數。在CarSim輸出車輛初始位置、航向角、速度等信息，通過 Simulink模塊計算后將得到的速度，控制制動壓力以及方向盤轉角輸入到，得出車輛的實時運動狀態到CarSim中，最后觀察泊車效果。顯示模塊顯示實際所需的泊車位寬度、長度，以及泊車時間（車輛開始倒車時進行計時）。

圖11 CarSim與Simulink聯合仿真示意圖

3.2 仿真結果分析

為驗證初始航向角不為零時的算法有效性，選取CarSim自帶車型D-class-SUV為模型，選擇表1所列參數驗證航向角不為零時的算法有效性。以航向角5°為例得到仿真動畫如圖12所示。

圖12 航向角為5°時的泊車動畫示意圖

車輛泊車的動畫效果如圖13(a)所示，車輛的后軸中心參考路徑曲線與后軸中心實際路徑曲線基本吻合，說明車輛軌跡跟蹤效果良好，車輛可沿規劃路徑泊入車位中。車輛前輪轉角如圖13(b)所示，其在最大限制轉角限制范圍內且最后基本回正，過程中無劇烈轉向、乘車舒適性較好。航向角如圖13(c)所示，從5°至最后穩定于90°，表明車輛已垂直泊入停車位。泊車參數如圖13(d)所示，基于最小轉彎半徑的實際泊車位所需寬度為2.24 m，所需長度為5.08 m，泊車時間為11.25 s。對比《城市道路路內停車泊位設置規范》（GA/T 850—2021）小型車位標準尺寸，采用此種泊車方案停車位實際寬度減少10.4%，長度減少15.3%，能較好地適應狹窄垂直車位工況。

4 結論

本文對初始航向角不為零時的狹窄垂直車位泊車工況進行了研究，基于最小轉彎半徑設計了圓弧-直線-圓弧的多段式路徑規劃，針對圓弧直線組合路徑曲率突變的情況，采用平滑度和靈活性較好的五次多項式對規劃的路徑進行了優化，聯合Carsim/Simulink對規劃路徑進行了仿真驗證。結果表明經過五次多項式優化后的路徑曲率連續且無碰撞，泊車過程車輛無劇烈轉向，優化效果較好。較城市內泊車設置規范GA/T 850—2021要求，此泊車方案停車位寬度和長度均有較為明顯減少，能更好地滿足城市中常見的車輛初始航向角不為零的狹窄垂直車位泊車場景。

[1] PADEN B,CAP M,YONG S Z,et al.A Survey of Motion Planning and Control Techniques for Self- driving Urban Vehicles[J].IEEE Transactions on Intel- ligent Vehicles, 2016,1(1):33-55.

[2] BANZHAF H, NIENHUSER D, KNOOP S, et al. The Future of Parking: A Survey on Automated Valet Par- king with an Outlook on High Density Parking[C]// 2017 IEEE Intelligent Vehicles Symposium.Piscata- way:IEEE,2017,1827-1834.

[3] GONZáLEZ D,PéREZ J,MILANéS V,et al.A Review of Motion Planning Techniques for Automated Vehi- cles[J].IEEE Transactions on Intelligent Transportation Systems,2015,17(4):1135-1145.

[4] UPADHYAY S,RATNOO A.Continuous-curvature Path Planning with Obstacle Avoidance Using Four Parame- ter Logistic Curves[J].IEEE Robotics and Automation Letters,2016,1(2):609-616.

[5] KIM D J,CHUNG CC.Automated Perpendicular Par- king System with Approximated Clothoid Based Local Path Planning[J].IEEE Control Systems Letters, 2021,5(6):1940-1945.

[6] 張家旭,趙健,施正堂,等.基于回旋曲線的垂直泊車軌跡規劃與跟蹤控制[J].東南大學學報(自然科學版),2020,50(1):182-191.

[7] VOROBIEVA H, GLASER S, MINOIU-ENACHE N, et al.Automatic Parallel Parking in Tiny Spots:Path Planning and Control[J].IEEE Transactions on Intelli- gent Transportation Systems,2014,16(1):396-410.

[8] 李紅,郭孔輝,基于樣條理論的自動垂直泊車軌跡規劃[J].湖南大學學報(自然科學版),2012,39(7):25-30.

[9] MOON J, BAE I, CHA J, et al. A Trajectory Planning Method Based on Forward Path Generation and Backward Tracking Algorithm for Automatic Parking Systems[C]//17th International IEEE Conference on Intelligent Transportation Systems.Piscataway:IEEE, 2014:719-724.

[10] 王啟明,宗高強,胥津銘.多段式自動泊車最優路徑規劃與仿真分析[J].系統仿真學報,2022,34(2):385-395.

[11] KIM J M, LIM KI, KIM J H.Auto Parking Path Planning System Using Modified Reeds-shepp Curve Algorithm[C]//2014 11th International Conference on Ubiquitous Robots and Ambient Intelligence.Piscata- way:IEEE,2014:311-315.

[12] 高強,陸洲,段晨東.汽車垂直泊車路徑規劃與路徑跟蹤研究[J].汽車工程,2021,43(7):987-994,1012.

[13] WU Z,SUN W,WANG M,et al. Psdet:Efficient and Universal Parking Slot Detection[C]//2020 IEEE Intelligent Vehicles Symposium.Piscataway:IEEE,2020: 290-297.

[14] LI B,WANG K,SHAO Z.Time-optimal Maneuver Plan- ning in Automatic Parallel Parking Using a Simultan- eous Dynamic Optimization Approach[J].IEEE Tran- sactions on Intelligent Transportation Systems,2016, 17(11):3263-3274.

[15] MORALES D P, KERMORGANT O, QUIJADA S D, et al.Laser-based Control Law for Autonomous Paral- lel and Perpendicular Parking[C]//2018 Second IEEE International Conference on Robotic Computing. Piscataway:IEEE,2018:64-71.

[16] ZHANG S, SIMKANI M, ZADEH M H. Automatic Vehicle Parallel Parking Design Using Fifth Degree Polynomial Path Planning[C]//2011 IEEE Vehicular Technology Conference (VTC Fall).Piscataway:IEEE, 2011:1-4.

[17] 熊周兵,楊威,丁可.基于預瞄模糊控制的自動泊車算法研究[J].重慶理工大學學報(自然科學版),2017,31 (2):14-22.

[18] 周鵬.基于自適應算法的平行泊車路徑規劃及跟蹤控制[D].哈爾濱:哈爾濱理工大學,2020.

[19] 陳龍,羅杰,楊旭.基于模糊純追蹤控制的自動泊車算法研究[J].武漢理工大學學報(信息與管理工程版), 2019,41(3): 316-321.

[20] 楊向彬.基于Fuzzy-PID控制的平行泊車仿真[J].汽車實用技術,2019,44 (12):132-135.

[21] 李翔.無人車運動控制與自主泊車路徑規劃方法[D]. 杭州:浙江大學, 2021.

[22] 龔建偉,姜巖,徐威.無人駕駛車輛模型預測控制[M].2版.北京:北京理工大學出版社,2020.

[23] TASHIRO T. Vehicle Steering Control with MPC for Target Trajectory Tracking of Autonomous Reverse Parking[C]//2013 IEEE International Conference on Control Applications.Piscataway:IEEE,2013:247-251.

Multi-stage Path Planning and Simulation Analysis Based on Narrow Vertical Parking Space

CAI Yun1,2, WANG Guang1,2, ZHANG Youshui1,2, WU Aoqi1,2, CHEN Rui3

( 1.School of Automobile and Transportation, Xihua University, Chengdu 610039, China;2.Vehicle Measurement, Control and Safety Key Laboratory of Sichuan Province, Chengdu 610039, China;3.Chengdu Luxingtong Information Technology Company Limited, Chengdu 610041, China )

Automated parking system (APS) is a hot topic in current research. However, most studies ignore the effect of the initial vehicle heading angle deviation on the overall parking effect, this paper investigates a multi-stage path planning method with a non-zero initial vehicle heading angle in a narrow vertical parking space scenario. Firstly, based on the minimum turning radius of the vehicle, the trajectory equation of the vehicle rear axle center motion is established, and the path planning is optimized by the fifth polynomial to achieve more continuous path curvature, smoother and more comfortable parking. CarSim/Simulink joint simulation validation shows that the five polynomial parking path optimization method is feasible, and the actual width required for the parking space is reduced by 10.4% and the length is reduced by 15.3% on the basis of satisfying the parking safety.

Vertical parking;Path planning;Quintuple polynomial;Model predictive control;Co-Simulation;CarSim/Simulink

U471.15

A

1671-7988(2023)10-22-08

10.16638/j.cnki.1671-7988.2023.010.005

蔡云（1979—），男，碩士，副教授，研究方向為智能汽車感知、規劃與控制技術，E-mail:29682131@qq.com。

基于北斗雙模高精度定位的智能車聯大數據安全綜合服務平臺開發及推廣示范（2020ZHCG0077）。