基于回旋曲線的垂直泊車路徑規(guī)劃

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

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

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

1 前言

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

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

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

2 垂直泊車基礎路徑規(guī)劃

2.1 汽車運動學模型

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

圖1 汽車簡化模型

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

圖2 阿克曼轉向模型

車輛的狀態(tài)方程為：

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

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

2.2 單步泊車路徑規(guī)劃

車輛開始泊車時，首先通過全景環(huán)視系統(tǒng)檢測車位角點P1、P4，在車位左角點P1處建立坐標系。此時后軸中心位于S1點，以S1為規(guī)劃起點，坐標可根據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 多步泊車路徑規(guī)劃

單步泊車所需空間較大，垂直泊車場景常遇到泊車空間不足的情況，比如道路較窄或對向車位停有車輛。如圖7 所示，車輛輪廓左前點A1與上邊界會發(fā)生碰撞。此時單步泊車難以實現，需采用多步泊車的方法完成。從圖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 垂直泊車路徑規(guī)劃算法優(yōu)化

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

3.1 回旋曲線

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

式中，σ為曲率變化率。

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

圖11 回旋曲線

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

圓心C的坐標為：

由圖11可得：

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

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

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

3.2 連續(xù)曲率路徑優(yōu)化

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

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

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

圖12 回旋曲線優(yōu)化的泊車路徑示意

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

圖13 回旋曲線優(yōu)化圓弧的2種情況

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

4 仿真驗證

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

表1 汽車主要參數

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

圖14 單步泊車仿真結果

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

圖15 多步泊車仿真結果

5 結束語

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