小車位環境下的自動泊車路徑規劃與跟蹤控制

馮欣陽，李春書，唐風敏

河北工業大學 機械工程學院，天津 300401

隨著我國汽車保有量的增加，一些問題也隨之顯現，泊車可用空間的減小便是其中之一。狹小車位環境下的泊車需要豐富的駕駛經驗及嫻熟的駕駛技術，無形之中增加了駕駛人員的操作難度和操作體驗。目前，智能車輛的自動泊車研究是許多研究者們關注的問題之一[1-3]。自動泊車包括環境感知、泊車控制、人機交互等技術。

泊車控制主要有兩種方法：人工智能方法和運動狀態規劃及軌跡跟蹤。人工智能方法通過智能算法使控制器在泊車過程中模擬駕駛員的操作行為[3]。文獻[4-9]通過蟻群算法、神經網絡、模糊控制等智能算法，實現泊車操作；由于實際泊車環境的復雜性，駕駛員需要面對不同的泊車環境做出不同反應，增加了人工智能方法的實現難度。一些研究者提出基于路徑規劃及路徑跟蹤的泊車控制方法[10-18]。研究者們對于泊車路徑規劃的研究大多采用曲線組合或多項式插值的方式[11-14]。此外，有些研究者也采用貝塞爾曲線、B樣條曲線設計泊車路徑[15-16]。路徑跟蹤采用滑模控制、自適應控制、模型預測控制等多種跟蹤控制方法[12-18]。

上述泊車路徑規劃的研究均是針對一次泊入到位路徑進行設計實現泊車的；若車位長度較小，不能實現一次泊入到位，車輛需要在車位內進行位姿調整。為方便描述，將需要在車位內進行位姿調整的車位稱為小車位。目前，一些研究者已針對小車位環境自動泊車的路徑規劃進行研究。文獻[19]通過建立預定義模態集，采用遍歷策略，計算最優參數，對泊入段、調整段路徑進行規劃；但模態集的建立需要考慮多種情況，計算量較大。文獻[20]通過B 樣條曲線建立泊入路徑及調整路徑；該調整路徑曲率連續，但是B樣條曲線復雜程度高，求解過程較為困難。文獻[21]根據車位長度及車輛當前航向角，通過多次揉庫，實現狹小車位內的姿態調整；該方法耗時長、計算成本高。文獻[22]通過三段式路徑規劃實現平行車位內的位姿調整；調整路徑通過前進圓弧路段實現位姿調整，但該方法在換向時，曲率不連續且所需車位較大。文獻[23]利用曲線組合設計了泊入路徑和調整路徑，采用相切圓弧實現車位內的倒車位姿調整，其調整路徑存在曲率突變的不足。

為使泊車路徑曲率連續無突變且計算方便，利用回旋曲線曲率連續的特點，將回旋曲線、直線、圓弧曲線組合設計形成平滑的泊入段路徑和調整段路徑；在非時間參考變量的前提下結合滑模控制原理，設計路徑跟蹤控制器，實現泊車路徑的跟蹤控制。

1 平行車位泊車路徑規劃

1.1 小車位環境及路徑分析

當車輛側向泊入平行車位時，停泊時的標準位姿通常如參考文獻[24]所描述的狀態，如圖1所示。ab和dc分別代表車輛前端和后端；Lf、L、Lr、Wa分別代表前懸、軸距、后懸、車寬；Lp、Wp分別代表車位的長度與寬度；Db、Dr分別代表車輛后端、右端與車位障礙物邊界之間的安全距離。

圖1 車輛泊車位姿示意圖Fig.1 Diagram of vehicle standard position and pose

如圖2 所示，建立大地坐標系OXY，O為固定原點，障礙物邊界點A與原點O重合，泊車初始位姿如圖2 中I 位置所示，車輛的初始位置、初始航向角分別為C1、θ0，選擇車輛后軸的中心點作為路徑跟蹤的參考點。

圖2 小車位下的泊入路徑Fig.2 Parking path under small parking space

泊入路徑由回旋曲線C1-C2、圓弧曲線C2-C3、回旋曲線C3-C4、直線C4-C5、回旋曲線C5-C6、圓弧曲線C6-P組成；車輛的極限位置為圖2中III位置所示。

調整路徑由前進路段和后退路段組成，前進路段由回旋曲線P-P1、回旋曲線P1-P2、圓弧曲線P2-P3、回旋曲線P3-P4、回旋曲線P4-P5、圓弧曲線P5-P6、回旋曲線P6-P7組成；P-P1、P1-P2為兩段反向回旋曲線，P3-P4、P4-P5為兩段反向回旋曲線；后退路段由直線P7-P8組成；調整路徑如圖3所示。

圖3 小車位內調整路徑Fig.3 Adjust path in small parking space

設Rmin為車輛最小轉彎半徑，泊車路徑的最大曲率不超過ρmax=1Rmin；在泊車過程中為避免車身與障礙物發生碰撞，需要滿足以下條件：（1）泊入階段，車輛右前頂點b與A點不發生碰撞（記為Cons G）；車輛右側bc與A點不發生碰撞。（2）調整階段，車輛右前頂點b與車位BC邊界不發生碰撞；車輛前端ab與車位AB邊界的安全距離為Ds。

1.2 泊入路徑的設計

設回旋曲線的曲率變化率為k，初始點為固定原點O，初始方位角為零，回旋曲線上曲率為ρ的i點坐標及方位角αi為：

設圓弧曲線的半徑為R，圓心角為θ，起始點為固定原點O，初始方位角為零，圓弧曲線上對應圓心角為θ的j點坐標為：

xj=Rsinθ（4）

yj=R(1-cosθ) （5）

回旋曲線C1-C2曲率變化率為kC12，C1-C2曲線構造原理如圖4 所示，由式（1）、（2）得到初始回旋曲線O-C2*，C2*處的方位角為αC2*由式（3）求得，初始曲線如I所示。將曲線O-C2*逆時針旋轉π+θ0，如位置II所示，沿向量OC1平移至O和C1重合，得到回旋曲線C1-C2，如位置III所示。

圖4 C1-C2 曲線構造原理Fig.4 Construction principle of C1-C2 curve

圓弧曲線C2-C3的曲率為ρC23，圓心角為θC23，初始圓弧曲線O-C3*由式（4）和（5）求得，將其逆時針旋轉αC2*+θ0+π，沿向量OC2平移至O與C2重合，得到圓弧曲線C2-C3，C3點處的方位角為αC3=θ0+αC2*+θC23。

同理，其余回旋曲線和圓弧曲線也類似求得。回旋曲線C3-C4的曲率變化率為kC34，初始回旋曲線為OC3′，C3′處的方位角為αC3′。設P點的方位角為θ1。圓弧曲線C6-P的曲率為ρC6P，圓心角為θC6P，初始圓弧曲線為O-C6*；回旋曲線C5-C6的曲率變化率為kC56，初始回旋曲線為O-C6′，C6′處的方位角為αC6′；連接C4、C5得到直線C4-C5。

上述圓弧曲線的半徑均取Rmin，滿足θ1+θC6P+αC6′=αC3+αC3′。

泊入運動中允許的最大車速vmax1=3 km/h，曲率變化率的最大值kmax1=wmax2，wmax代表前輪最大轉向角速度。

泊入路徑中直線C4-C5長度LC45最小為優化目標，泊入路徑規劃轉換為非線性規劃：式中，第一個不等式表示車輛右前頂點b與A點不發生碰撞；第二個不等式為直線C4-C5存在性條件；第三個不等式表示車輛bc側與A點不發生碰撞；第四個等式表示直線C4-C5分別與回旋曲線C5-C6、回旋曲線C3-C4曲線相切。

1.3 調整段路徑設計

為了方便設計，將圓弧曲線P2-P3分為P2-Q和Q-P3兩段圓弧，Q點切線水平，如圖3（b）所示。

回旋曲線P6-P7的曲率變化率為kP67，初始回旋曲線為O-P6*，P6*處的方位角為αP6*；圓弧曲線P5-P6的曲率為ρP56，圓心角為θP56，初始圓弧曲線為O-P5*；回旋曲線P4-P5的曲率變化率為kP45，初始回旋曲線為O-P5′，P5′處的方位角為αP5′；回旋曲線P3-P4的曲率變化率為kp34，初始回旋曲線為O-P3*，P3*處的方位角為αP3*；圓弧曲線Q-P3的圓心角為θ33，曲率為ρQP3，初始圓弧曲線為O-P3′；回旋曲線P-P1的曲率變化率為kPP1，初始回旋曲線為O-P*，P*處的方位角為αP*；回旋曲線P1-P2的曲率變化率為kP12，初始回旋曲線為O-P2*，P2*處的方位角為αP2*；圓弧曲線P2-Q的圓心角為θ32，曲率為ρP2Q，初始圓弧曲線為O-P2′。

連接P7-P8，P8點為圖1所示參考點的位置。

圓弧曲線P5-P6的半徑取為Rmin。在調整路徑中，為保證車輛能夠跟隨路徑，允許的最大車度vmax2=0.75 km/h，P6-P7選擇曲率變化率的最大值kmax2=2wmax。將P與P7點的橫向距離LXPP7最小作為優化目標，調整路徑規劃轉換為非線性規劃：

式中，第一個等式限定P點的位置關系；第二個不等式表示圓弧路徑P2-P3存在性條件；第三個不等式表示車輛b點與障礙物BC側不發生碰撞；第四個不等式表示車輛ab側與障礙物AB側不發生碰撞。

2 小車位下的泊車路徑的跟蹤控制

在泊車過程中車輛以低速行駛，根據阿克曼轉向原理，車輛的運動學模型為：

式中，(x,y)代表參考點坐標，v代表車速，φ代表車輛航向角，δ代表車輛的前輪轉向角。

傳統路徑跟蹤的方法是將路徑視作時間參考函數，若車輛跟蹤過程中出現誤差，這種誤差會隨著時間逐漸增加，甚至會直接導致車輛失去跟蹤能力[25]。引入f=-x做為非時間參考量，車輛的運動學方程可表示為：

圖5 路徑跟蹤偏差示意圖Fig.5 Diagram of path tracking deviation

結合式（8）、（9）、（10），基于非時間參考變量的跟蹤偏差為：

3 仿真分析

車輛的基本參數如表1所示。

表1 車輛基本參數Table 1 Basic vehicle parameters

泊車路徑跟蹤效果如圖6所示。泊車過程中，汽車外輪廓與障礙物邊界之間的相對位置如圖7所示。

圖7 汽車外輪廓包絡圖Fig.7 Envelope diagram of automobile outline

由圖6、7 可知，車輛行駛路徑與規劃路徑基本重合，汽車外輪廓的包絡在行駛過程中未與周邊障礙物產生碰撞，滿足車輛避障約束及泊車要求等。

圖6 路徑跟蹤效果圖Fig.6 Diagram of path tracking

泊車過程中的縱向誤差如圖8所示。圖8（a）、（b）、（c）分別表示泊入路徑、前進調整路段和后退路段的縱向誤差。

圖8 路徑跟蹤過程中的縱向誤差Fig.8 Longitudinal error in path tracking

由圖8可知，泊入路段中的最大縱向誤差為0.005 4 m；調整路徑中的前進路段的最大縱向誤差為0.002 2 m，后退路段最大縱向誤差為0.002 1 m，泊車終止點的縱向誤差為0.002 1 m。

泊車過程中的航向誤差如圖9所示。圖9（a）、（b）、（c）分別代表泊入路段、前進調整路段和后退調整路段的航向誤差。

由圖9可知，泊入路徑中的最大航向誤差為0.114 6°；調整路徑中的前進路段中的最大航向誤差為0.401 1°，后退路段最大航向誤差為0.383 9°，泊車終止點的航向角誤差為0.383 9°。

圖9 路徑跟蹤過程中的航向誤差Fig.9 Course angle error in path tracking

仿真過程中車輛的實際前輪轉角如圖10所示。圖10（a）、（b）、（c）分別為泊入路段、前進調整路段、后退調整路段在不同函數作用下的前輪轉角。

圖10 不同函數作用下的前輪轉角Fig.10 Front wheel rotation angle under different functions

由圖10知，車輛在路徑跟隨過程中，前輪轉角無突變，車輛行駛平滑；連續函數作用下的滑模控制器相比符號函數作用下的滑模控制器較大的抑制了高頻抖振。

由圖6～10可知，在滑模控制器的作用下，車輛與周圍障礙物未發生碰撞，最大縱向誤差為0.005 4 m，最大航向誤差為0.401 1°，跟蹤誤差較小；全程前輪轉角變化平滑，抖振抑制效果良好。

4 結論

對于小車位環境泊車難的問題，設計了泊入路徑和調整路徑，將回旋曲線、直線、圓弧曲線進行組合規劃，實現了不同曲率路徑之間的平滑過渡，該規劃方法具有計算簡便、曲率無突變的優點。基于非時間參考誤差設計滑模控制器，將指數趨近率中的符號函數替換為連續函數，改善了泊車過程中的高頻抖振問題。仿真結果表明，該路徑規劃方法滿足小車位泊車的要求，前輪轉角無突變、跟蹤誤差小，較好地解決了小車位泊車難、位姿調整難的問題。