垂直泊車路徑跟蹤控制研究

徐遠征，吳長水

（上海工程技術大學 機械與汽車工程學院，上海 201620）

0 引言

城市停車位緊張、車位空間往往較小，泊車時所造成的剮蹭事故則不可避免地會時有發(fā)生。為提高泊車過程的安全性，自動泊車系統(tǒng)已成為了當前車企及研究機構的研究重點。一套完整的自動泊車系統(tǒng)可分為感知、規(guī)劃以及運動控制三部分，而運動控制的好壞將直接決定最終的泊車效果及成功率，因此對其展開研究就有著較高的應用價值。通常來說，運動控制主要分為2部分：縱向控制以及橫向控制。其中，縱向控制主要為針對車速的控制，穩(wěn)定的車速對于泊車過程至關重要。橫向控制則主要通過方向盤控制車身的橫向運動，其控制精度將對路徑跟蹤效果產(chǎn)生較大影響。

目前，已有多位學者對自動泊車運動控制算法進行了研究。張家旭等人針對水平泊車，設計了一種基于滑?？刂频牟窜嚶窂礁櫩刂品椒?，仿真結果證明了所提出方法相較于傳統(tǒng)的PID控制方法具有更好的抗干擾性及控制精度。黃曉峰綜合考慮泊車過程中的各性能指標，分別采用前饋加反饋控制以及模糊控制方式，對方向盤轉角和車速進行跟蹤控制，在確保泊車控制穩(wěn)定的基礎上保證了一定程度的舒適性。Petrov等人則就泊車過程中轉角連續(xù)抖動問題，針對不同的泊車工況，提出了一種基于飽和控制反饋的泊車控制方法，并通過仿真驗證了所提出方法的可行性。

本文以垂直泊車為研究對象，提出了一種基于模糊PID和 模型預測控制（Model Predict Control，MPC）算法的車輛聯(lián)合控制跟蹤算法，并通過Simulink與Carsim進行聯(lián)合仿真，驗證了所提出算法的有效性。

1 泊車運動學模型及路徑規(guī)劃

1.1 車輛運動學建模

自動泊車運動過程中車速較低，此時可以忽略輪胎的側向滑移，整個車身結構可視為剛體，因此本文將車輛簡化為矩形，選用車輛運動學模型進行建模。車輛運動學模型如圖1所示。

圖1 車輛運動學模型Fig.1 Vehicles kinematics model

由圖1可知，本文以垂直車位左上角點為原點建立全局坐標系，此時車輛位置坐標由車輛后軸中心在全局坐標系下的坐標（，）確定，車身的角度由車輛在全局坐標系下的航向角確定。本文建立的車輛運動學模型如式（1）所示：

其中，為車速；為軸距；為車輛前輪等效轉向角。研究可知只需輸入車輛速度及前輪的等效轉向角，便可求得當前車輛在全局坐標系下的位姿（，，）。

1.2 垂直泊車路徑規(guī)劃

本文使用快速探索隨機樹（Rapidly-exploring Randomized Tree，RRT）算法進行垂直泊車路徑規(guī)劃，RRT算法是一種使用增長樹的搜索算法，其優(yōu)勢在于無需對泊車空間進行幾何劃分，算法靈活性較高。RRT算法偽代碼詳見如下。

路徑規(guī)劃的具體步驟為：

（1）根據(jù)起始點x、目標點x、循環(huán)次數(shù)、拓展步長以及環(huán)境等變量初始化各項參數(shù)。

（2）在環(huán)境中生成隨機采樣點x，然后遍歷樹上已有的節(jié)點，通過_函數(shù)選擇離其最近的樹節(jié)點作為擴展節(jié)點x。

（3）通過拓展函數(shù)_生成新節(jié)點x，并將其索引、邊界添加到樹中。

（4）判斷樹是否拓展到距離x的某一范圍內，若是，則結束拓展、并輸出路徑，否則進行下一輪循環(huán)。

路徑規(guī)劃結束后，往往由于路徑不夠平滑，無法滿足車輛運動學約束。本文使用（）曲線進行路徑優(yōu)化。曲線是在曲線上的一種改進、即假定車輛轉向時始終按照最小轉彎半徑進行轉向，且曲線中的各段路徑均為圓弧及直線的組合。RRT路徑優(yōu)化過程如圖2所示。優(yōu)化方法為：從起始點開始，從小到大遍歷RRT樹上點的索引，若2個點間存在無碰撞的路徑，則用曲線替換原有的路徑，而后再以當前點為起始點進行新一輪的優(yōu)化，直到優(yōu)化結束。

圖2 RRT路徑優(yōu)化Fig.2 RRT path optimization

2 基于模糊PID算法的車輛縱向控制

2.1 模糊PID算法架構

模糊PID控制算法將傳統(tǒng)的PID控制算法和模糊算法相結合，可通過經(jīng)驗化的模糊規(guī)則實時調整PID算法的參數(shù)，以此彌補傳統(tǒng)PID算法不適用于非線性系統(tǒng)的不足，本文設計的模糊PID算法框圖如圖3所示。由圖3可知，模糊控制算法的輸入為目標車速以及車輛模型返回的實際車速，輸出為車輛模型所需的加速度。算法運行時，可循環(huán)比較目標車速與實際車速之間的的差值以及差值的變化率E，通過模糊控制算法求得K、K、K的補償值更新PID控制器的參數(shù)，再由PID控制算法計算出期望加速度，進而控制車輛運動。

圖3 模糊PID算法框圖Fig.3 Block diagram of fuzzy PID algorithm

2.2 模糊控制算法

模糊控制器主要由模糊化、模糊推理以及清晰化三個模塊組成，如圖4所示。圖4中，模糊化模塊通過輸入隸屬度函數(shù)將清晰的論域轉化為模糊集合；模糊推理模塊是通過設定的模糊規(guī)則推理出輸出變量的隸屬度；清晰化模塊則根據(jù)輸出變量的隸屬度函數(shù)及論域進行反模糊化，計算得到輸出變量的精確值。

圖4 模糊控制算法框圖Fig.4 Block diagram of fuzzy control algorithm

2.3 模糊化

本文將各參數(shù)的模糊子集定義為：｛負大，負中，負小，零，正小，正中，正大｝，對應符號為｛NB，NM，NS，Z，PS，PM，PB｝。參考實際調試過程中各參數(shù)的實際變化范圍，對于輸入?yún)?shù)，本文設定輸出值與參考值之間的誤差值的論域為［-1，1］，誤差值變化率E的論域為［15，15］。對于輸出值，本文設定輸出的K的論域為［03，03］，K的論域為［006，006］，K的論域為［3，3］。若上述參數(shù)的實際值超過變化范圍，則取邊界值進行計算。

常用的隸屬度函數(shù)有：三角形函數(shù)、梯形函數(shù)、高斯分布函數(shù)等。參考文獻［9］的方法以及實際調試效果，本文將各變量中間的隸屬度函數(shù)設定為三角形函數(shù)，兩側的隸屬度函數(shù)設定為高斯分布函數(shù)，如圖5所示。

圖5 各變量隸屬度函數(shù)設置Fig.5 Membership function setting of each variable

2.4 清晰化

本文根據(jù)泊車過程的工程實踐經(jīng)驗分別對K、K、K設置了相應的模糊規(guī)則表，根據(jù)誤差及誤差的變化率即可推理得到K、K、K的隸屬度。

常用的清晰化方法有：最大隸屬度法、重心法以及加權平均法，綜合考慮計算復雜度以及實際控制效果，本文采用加權平均法進行清晰化。各個輸出參數(shù)的輸出曲面如圖6所示。

圖6 輸出參數(shù)曲面圖Fig.6 Output parameters surface diagrams

3 基于MPC算法的車輛橫向控制

3.1 MPC算法架構

模型預測控制算法通常可分為預測模型、滾動優(yōu)化以及反饋校正三部分。本文根據(jù)垂直泊車的橫向控制需求，設計得到的MPC算法架構如圖7所示。圖7中，輸入為車輛當前的位姿以及車速，輸出為目標車速以及方向盤轉角的控制信號，其中目標車速作為模糊PID算法的參考值。

圖7 MPC算法架構Fig.7 MPC algorithm architecture

3.2 預測模型

低速狀態(tài)下輪胎滑移量可忽略，本文使用車輛低速運動學模型作為預測模型。由于上文建立的低速車輛運動學模型為非線性時變系統(tǒng)，無法確?？刂葡到y(tǒng)的實時性，因此本文對其進行線性化處理。

在控制車輛橫向運動時需跟蹤車輛的實時位姿，本文選取系統(tǒng)的輸入為（，），將系統(tǒng)的狀態(tài)變量用（，，）表示。根據(jù)路徑規(guī)劃的任意參考點的參考狀態(tài)χ（x，y，θ）以及對應的參考輸入u（v，δ），推導可得到以為采樣時間的離散化誤差模型，推導得到的該模型的數(shù)學表述見式（2）：

為提高模型效率，令A＝A，B＝B，得到的預測模型可根據(jù)時刻的系統(tǒng)狀態(tài)以及N各時域的預測輸入預測N各時域內的輸出。此時推得的數(shù)學公式為：

其中，

3.3 目標函數(shù)及約束條件

為使控制過程更為平穩(wěn)，本文使用目標函數(shù)為：

其中，N為預測步長；N為控制步長；為權重系數(shù)；為松弛因子；和分別為狀態(tài)量和控制量的權重。

設定采樣時間0.02 s，為了使泊車期間車輛速度及航向角變化平穩(wěn)，本文設定的速度及前輪轉角的控制量及控制增量約束如式（6）所示：

4 仿真驗證

4.1 仿真參數(shù)設置

本文在Matlab／Simulink搭建相應的算法，并在Carsim中配置相應的車輛參數(shù)，實現(xiàn)Carsim與Simulink的聯(lián)合仿真。本文搭建的仿真環(huán)境如圖8所示，所用的仿真參數(shù)見表1。

圖8 聯(lián)合仿真模型Fig.8 Co-simulation model

表1 仿真參數(shù)Tab.1 Simulation parameters

4.2 實驗結果分析

垂直泊車路徑跟蹤控制的整體效果如圖9所示。圖9中，實線部分為車輛實際運動軌跡。圖10顯示了車輛各執(zhí)行器的相應情況。泊車開始后，車輛首先掛檔，向右打方向盤行駛到點2，在此過程中車速緩慢上升。然后向左打方向盤行駛到點3，此時車速緩慢下降至0 km／h。之后車輛掛檔，向右打方向盤開始倒車，行駛到點4后回正方向盤，并適時調整其角度，使車輛沿直線泊入車位至緩慢停止。最后回正方向盤，檔位切換為檔，泊車結束。

圖9 規(guī)劃路徑與實際路徑對比Fig.9 Comparison of planned path and actual path

圖10 執(zhí)行器響應Fig.10 Actuators response

綜上可知，實際路徑與規(guī)劃路徑較為吻合且變化趨勢一致，車輛最終的泊車位姿與目標位姿偏差不大，且在泊車過程中沒有碰撞、壓線等情況的發(fā)生。整個泊車過程中方向盤轉角不超過450°，滿足車輛轉向約束。車輛車速變化較為平緩，車速控制在2 km／h以內，驗證了模糊PID算法的有效性。故本文所提出的垂直泊車跟蹤控制算法能夠對規(guī)劃的路徑進行較好的跟蹤，并控制車輛以較小的誤差按照該路徑泊入目標車位。

圖11為規(guī)劃車輛位姿與實際車輛位姿的誤差。由圖11可知，車輛實際的偏航角與路徑規(guī)劃的偏航角誤差的絕對值控制在0.057 m以內，規(guī)劃路徑與實際路徑的橫向偏差的絕對值控制在0.08 m以內。分析圖9可知，上述誤差并不會使車輛與外界環(huán)境發(fā)生碰撞，考慮到Carsim車輛模型模擬了真實的泊車環(huán)境，在泊車過程中存在較多的環(huán)境干擾，上述誤差范圍區(qū)間較為合理。因此，本文建立的垂直泊車路徑跟蹤控制算法具有良好的跟蹤控制效果。

圖11 偏航角、橫向角誤差Fig.11 Error of yaw angle and lateral angle

5 結束語

垂直泊車車位狹小，易造成剮蹭碰撞等危險情況的發(fā)生，往往對車輛控制精度具有較高的要求。本文以垂直泊車為研究對象，提出了一種基于模糊PID算法與模型預測控制算法的自動泊車聯(lián)合控制算法。聯(lián)合仿真結果表明：本文提出的跟蹤控制算法精度較高，可以控制車輛在允許的誤差范圍內沿規(guī)劃的路徑行駛。此外，該方法對水平泊車、斜列泊車也都具有一定的參考價值。