基于回旋曲線的垂直泊車路徑規劃

張明海 丁寧 曹立波 顏京才 李旭升

（1.湖南大學，汽車車身先進設計制造國家重點實驗室，長沙 410082；2.毫末智行科技有限公司保定分公司，保定 071000）

主題詞：自動泊車 路徑規劃 直線—圓弧—回旋曲線

1 前言

隨著汽車保有量的不斷增加，城市交通嚴重擁堵，泊車環境越來越擁擠、復雜，技術不足或缺乏經驗的駕駛員難以將車輛安全、準確、快速地泊入車位。因此，開發安全舒適的自動泊車系統對泊車安全具有重要意義[1]。

作為自動泊車系統的組成部分，路徑規劃模塊是確保泊車過程安全舒適的關鍵[2]。曲線插值是常用的泊車路徑規劃方法，能夠將路徑用函數表達，具有運算耗時短、規劃路徑基本平滑可控的優點，缺點是缺乏靈活性。直線和圓弧組合是一種經典的方法，杜賓斯（Dubins）曲線[3]和Reeds-Shepp 曲線[4]可以生成兩點間的最短路徑，包括直線段和圓弧段，前者對應無倒車的情況，后者對應有倒車的情況。一些學者在此基礎上進行了拓展或改進：Kim等[5]提出通過組合不同的圓弧和直線在狹窄環境中搜索所有可行路徑，該方法較實用，且停車路徑可以通過最小化成本函數進行優化；Sung?woo 等[6]將圓弧定位法應用于平行停車，提出了一種完全基于幾何的方法，該方法最大的缺點是在直線和圓弧的連接點處曲率不連續，為了精確地跟蹤路徑，車輛必須在連接點停下，重新調整前輪方向，這將導致不必要的時間消耗和轉向系統磨損。為了避免這些問題，學者們提出了一些解決方法，其中回旋曲線在處理路徑平滑性方面具有獨特優勢。Vorobieva等[7-8]采用雙圓弧規劃平行泊車路徑，然后使用回旋曲線進行平滑處理，解決了泊車過程中原地轉向的問題。張家旭等[9]基于直線-圓弧組合方式設計平行泊車路徑，并通過對固定回旋曲線的平移和翻轉變換完成曲率連續的平行泊車路徑規劃，但使用回旋線時移動了圓弧端點。

本文采用幾何方法，結合曲線路徑最短和回旋曲線曲率平滑且符合車輛轉彎特性的優點，對垂直車位的自動泊車路徑規劃開展研究。考慮開始泊車時航向角不為零的情況，首先使用直線圓弧組合的方式設計基礎的一步和多步泊車路徑，然后使用回旋線對圓弧段進行優化，可以有效消除路徑中曲率突變的不平滑點，使整個泊車路徑曲率連續，避免原地轉向，以實現不同初始位置和姿態角條件下的垂直泊車路徑規劃。

2 垂直泊車基礎路徑規劃

2.1 汽車運動學模型

為保證泊車過程的準確性，需要對車身輪廓運動軌跡進行計算，在給予一定裕度的情況下，將車身輪廓等效為矩形，如圖1所示，圖中L為軸距，Lf、Lr分別為前、后懸，W為車寬。

圖1 汽車簡化模型

由于泊車過程車速較低，可以忽略車輛側傾和輪胎滑移的情況。本文采用基于阿克曼轉向原理的運動學模型，將車視為平面剛體，選取后軸中心作為參考點，具有x、y、φ3個自由度，其中(x,y)為車輛坐標，φ為航向角，是車身與x軸正方向的夾角，逆時針為正，如圖2所示。

圖2 阿克曼轉向模型

車輛的狀態方程為：

式中，v為車速；δ為等效前輪轉角。

根據最大等效前輪轉角δmax和軸距L，可求得后軸中心對應的最小轉彎半徑Rmin：

2.2 單步泊車路徑規劃

車輛開始泊車時，首先通過全景環視系統檢測車位角點P1、P4，在車位左角點P1處建立坐標系。此時后軸中心位于S1點，以S1為規劃起點，坐標可根據P1、P4點得到，車身與車位的夾角φ即為初始航向角，垂直停車位尺寸為l×w，如圖3 所示。車輛先從S1點沿X軸前進或后退至S2點，再按最小轉彎半徑Rmin行駛至S3點，切換倒擋按Rmin后退至S4點，最后沿直線后退至S5點泊入車位。圖3中曲線為車輛后軸中心的軌跡，O0為的圓心，對應的圓心角為θ0，O1為的圓心，對應的圓心角為θ1。

圖3 單步垂直泊車示意

圖4 單步泊車碰撞風險點示意

由圖4中幾何關系可得O1的坐標為：

圖3中O0的Y坐標為：

在圖3中，對于ΔO0O1N，有：

由此可得O0的X坐標為：

S2、S3、S4的坐標可以通過式（4）～（7）求得：

實際泊車時，起始點可能相對于S2點偏前，須先后退一段距離，如圖5所示。此外，初始航向角φ一般不為0°，泊車過程需先通過一段圓弧，將車輛調整到與X軸平行的位置，再按照以上方法進行泊車，圖6 所示為初始航向角不為零的泊車路徑。

圖5 起始點偏右時泊車路徑示意

圖6 初始角度φ≠0°的泊車路徑示意

2.3 多步泊車路徑規劃

單步泊車所需空間較大，垂直泊車場景常遇到泊車空間不足的情況，比如道路較窄或對向車位停有車輛。如圖7 所示，車輛輪廓左前點A1與上邊界會發生碰撞。此時單步泊車難以實現，需采用多步泊車的方法完成。從圖7中可以看出，引起碰撞的原因是一步泊車路徑中S3點的Y坐標過大，因此，通過約束YS3即可避免碰撞。

圖7 車輛與上邊界碰撞示意

圖8 為車輛在S3點以及行駛至A點的泊車示意，為避免碰撞，車輛左前點A1與上邊界需保留安全距離，將J與A1的距離定為安全距離Lsafe2。

圖8 多步垂直泊車空間約束

由圖8中幾何關系可得：

由圖9和圖10中幾何關系有：

圖9 多步泊車路徑示意

圖10 S4點計算示意

設直線EF的解析式為y1=kx+b，則：

由式（19）～（22）可得到θ2、θ3的值以及O2、O3、S5、S6的坐標。

3 垂直泊車路徑規劃算法優化

前文設計的路徑規劃方法由直線和圓弧組成，可以完成泊車任務。但由于曲率不連續，要使車輛沿規劃的路徑行駛，必須在直線和圓弧連接處停車原地轉向，這會造成轉向機構及輪胎的磨損，還會延長泊車時間。因此，需要對第2節中的路徑進行優化。回旋曲線可以很好地描述車輛行駛時轉向的行為，如果車輛以恒定的速度運動，并以恒定的角速度轉向，則后軸中心駛過的軌跡即為回旋線。回旋線具有曲率連續變化的特點[10]，滿足泊車路徑對曲率平滑的要求。本文使用回旋曲線對基礎路徑進行優化。

3.1 回旋曲線

回旋曲線是曲率K隨其弧長s線性變化的曲線，可表示為：

式中，σ為曲率變化率。

車輛從起點O開始，初始狀態為q0=(x0，y0，φ0，δ0)=(0，0，0，0)，x0、y0、φ0、δ0分別為車輛的初始x坐標、y坐標、車身航向角、曲率，以恒定的縱向速度和恒定的轉向速度向左行駛，直至點E，轉向盤到達最大角度，此時轉彎半徑為Rmin，車輛后軸中心駛過的軌跡如圖11所示。距起始點s處車輛的姿態信息為：

圖11 回旋曲線

式中，A為回旋曲線參數，A2=RL=1/σ；Cf、Sf為菲涅爾積分：

圓心C的坐標為：

由圖11可得：

為充分利用車輛的最大轉向角并縮小泊車空間，回旋曲線長度S應盡可能小。回旋曲線長度與車速及轉向速度有關：

式中，tmin為轉向盤從0°轉到最大角度所需時間；vδ為最大轉向速度；vmax為泊車過程中的最大車速。

所用回旋線參數定為A2min=RminSmin，R1由式（25）～式（28）計算，αc為回旋線對應的角度。

3.2 連續曲率路徑優化

第2節規劃的路徑中，存在的問題是直線與圓弧處曲率突變（從0 突變到1/R），且圓弧段起始曲率為1/R，這會給泊車過程帶來困難。希望車輛每次前進/后退的路徑曲率都是從0開始，并以0結束，且每一段路徑曲率都是連續的。優化的思路是結合回旋線，將每一段圓弧都調整為“回旋線-圓弧-回旋線”（CAC）組合的曲線。

定義CAC 組合曲線包括：參數為A的回旋線，長度S，初始曲率為0，末端曲率為1/R；半徑為R，角度為θ的圓弧；參數為A的回旋線，長度L，初始曲率為1/R，末端曲率為0。

針對第2節的結果，將最小半徑Rmin替換為R1，使用回旋曲線將半徑為R1的圓弧過渡到半徑為Rmin的圓弧。此外，需將圓弧以S2為中心旋轉角度μ，從而使優化后的航向角連續，如圖12所示。

圖12 回旋曲線優化的泊車路徑示意

對于不同的圓心角度α，轉化為回旋曲線-圓弧-回旋曲線（CAC 序列），考慮以下2 種情況：當α≥2αc時，使用完整的CAC 序列，如圖13a 所示；當α<2αc時，使用2段回旋線（CC 序列）進行優化，如圖13b 所示。回旋線對應的參數Anew、Lnew將有所變化：

圖13 回旋曲線優化圓弧的2種情況

式中，Δφ=(φend-φstart)為汽車轉過的角度。

4 仿真驗證

本文針對某品牌SUV 汽車展開試驗，汽車相關參數如表1 所示，采用尺寸為2.5 m×6 m 的垂直車位。通過MATLAB對垂直泊車進行仿真。

表1 汽車主要參數

圖14 所示為汽車單步泊車規劃的路徑，圖14a中，車輛的初始坐標為(-2,2)，初始航向角為0°；圖14b中，初始坐標為(-3,2)，初始航向角為-5°。對于不同的初始位置和角度，本文的方法可以規劃出直線-圓弧-回旋曲線組合的安全平滑泊車路徑，但所需泊車空間較大。

圖14 單步泊車仿真結果

為確保泊車過程的安全性并提高路徑規劃的適用性，根據實際泊車場景，將橫向泊車距離限制為6 m。圖15 為在不同的初始坐標和初始航向角下，規劃的多步泊車路徑均能避免碰撞且平滑。

圖15 多步泊車仿真結果

5 結束語

本文對垂直車位自動泊車場景，規劃了具有不同初始航向角的平滑泊車路徑。首先，根據車輛初始姿態以及泊車空間，考慮運動學約束和碰撞約束，設計基于直線-圓弧組合的單步和多步垂直泊車基礎路徑規劃方法。然后，對于連接點曲率不平滑的問題，結合回旋曲線對基礎路徑規劃結果進行優化，實現了泊車路徑的曲率平滑，有效避免了泊車過程中的原地轉向。基于MATLAB 的仿真結果表明，對于不同的初始航向角，該方法能規劃出安全無碰撞的平滑軌跡，驗證了安全性和可行性。