基于模糊純追蹤控制的自動泊車算法研究

陳 龍，羅 杰，楊 旭，王小龍

(1.武漢理工大學 自動化學院，湖北 武漢 430070；2.中國船舶重工集團公司第七二二研究所，湖北 武漢 430205)

近年來，汽車保有量在迅速增長，城市車位十分狹小，泊車讓大量駕駛員感到困惑。泊車輔助也應運而生，現(xiàn)階段倒車影像、全景環(huán)視系統(tǒng)逐漸普及[1]，但是仍需要駕駛員完成泊車工作。自動泊車系統(tǒng)無需人工干預，能夠快速、安全地使車輛駛入泊車位，可有效幫助駕駛員解決泊車問題。因此自動泊車系統(tǒng)的開發(fā)也成為汽車主動安全研究的重要方向之一，有著很好的應用前景[2]。

自動泊車系統(tǒng)一般由感知系統(tǒng)、決策系統(tǒng)和控制系統(tǒng)3部分構成。感知系統(tǒng)通過超聲波雷達[3]、激光雷達、攝像機[4]等傳感器獲取可用泊車位信息。在此基礎上，決策系統(tǒng)根據(jù)車輛固有的物理約束條件和碰撞約束條件，生成一條合理的泊車曲線?？刂葡到y(tǒng)通過實時控制車輛的車速和轉向角，達到對泊車曲線的跟蹤，完成泊車操作。現(xiàn)階段自動泊車的研究重點是泊車的路徑規(guī)劃與追蹤控制。

在路徑規(guī)劃方面，兩段最小轉彎半徑的圓弧組成的兩段式泊車路徑可使需求泊車位最小，易于開發(fā)，但是在兩圓弧相切位置，曲率發(fā)生了突變，需要車輛在該點停車轉向，會加大對輪胎的磨損。為此，學者們對兩段式路徑進行了優(yōu)化與改進。如穆加彩等[5]使用一種基于三階多項式與反正切函數(shù)的路徑擬合方式，使曲線與兩段式泊車路徑相仿，改善了曲線曲率的連續(xù)性。李紅等[6]基于B樣條理論，選取多個特征點作為泊車曲線的固定點，通過動態(tài)調整曲線的階數(shù)，使泊車路徑曲率連續(xù)。但是使用高階擬合或者動態(tài)多點計算的方法加大了計算量，考慮到工程適用性，筆者在兩段式泊車路徑上，使用回旋曲線進行優(yōu)化，解決兩段式路徑曲率突變問題的同時降低計算量。

在路徑跟蹤方面，純追蹤算法因其模型簡單，參數(shù)易調的特性，廣泛應用于智能車路徑跟蹤研究。其中，前視距離這一主要參數(shù)的整定極大地影響了路徑跟蹤效果。段建民等[7]根據(jù)橫向偏差來調整前向距離，使運動軌跡平滑，避免了橫向誤差較大時，轉角控制量突變的情況。唐小濤等[8]綜合考慮車輛速度與橫向偏差，設計模糊控制來自適應調整前視距離，增強系統(tǒng)對速度的適應性。黃沛琛等[9]使用BP神經(jīng)網(wǎng)絡實現(xiàn)前視距離的動態(tài)調整，仿真證明該方法對車輛轉向調頭的曲線跟蹤效果良好，且無震蕩現(xiàn)象。但上述研究目標路徑設計較為單一，且沒有結合航向偏差來設計合適的控制算法，導致對不同誤差輸入及目標路徑的跟蹤效果相差較大，難以適應由多種曲線生成的泊車路徑。為此，筆者對純追蹤模型進行改進，通過模糊控制，融合航向誤差與橫向誤差，進而動態(tài)調整前向距離，并應用在泊車系統(tǒng)中。

1 泊車路徑規(guī)劃

1.1 車輛運動學模型

對于前輪轉向車輛，后軸與車身的運動軌跡一致，因此將后軸中心作為車輛參考坐標。車輛泊車時處于低速狀態(tài)，忽略側向力的作用，車輛不會出現(xiàn)滑移與側向移動的狀態(tài)。基于阿克曼轉向機構的車輛運動學模型[10]如圖1所示，關系式如式(1)所示。

圖1 車輛運動學模型

(1)

式中：θ為車輛航向角；φ為等效前輪轉向角度；v為車輛縱向速度；l為車輛軸距；x,y分別為車輛后軸中心的橫縱坐標。在任何時刻，瞬時轉向軌跡與車體線速度方向相切，對應的后軸中心點的運動軌跡如式(2)所示。

(2)

其中，r為瞬時轉向半徑。由此可得：汽車低速泊車時，其運行軌跡是一段圓弧，且圓弧半徑與車速無關，而與轉向角度和軸距相關，這為軌跡規(guī)劃提供了理論依據(jù)。

1.2 基于回旋曲線優(yōu)化的泊車路徑規(guī)劃

回旋曲線是隨著曲線長度增加，曲率線性變化的一類曲線[11]，回旋曲線示意圖如圖2所示。若車輛行駛時具有恒定的縱向速度同時保持前輪轉角變化率一定，車輛后軸中心的運動軌跡即是一條回旋曲線，可以保證行駛曲率的連續(xù)性。基于上述性質，使用回旋曲線對兩段式泊車路徑進行優(yōu)化，既有利于后續(xù)的跟蹤控制，又能解決切點處曲率突變問題。

圖2 回旋曲線示意圖

在P點處曲率半徑為R，O點處曲率為零，則P點坐標及在P點處的航向角分別為：

(3)

VOROBIEVA等[12]使用上述性質，建立了適用任意初始位置的雙圓弧路徑，并使用了4段固定的回旋曲線將雙圓弧的兩側進行連接，實現(xiàn)曲率連續(xù)、滿足車輛運動學的泊車路徑?？紤]車輛約束條件和回旋曲線的性質，一般加入的回旋曲線長度約為0.5m，相比于雙圓弧路徑，會顯著增加需求車位的長度。為了適當減小需求車位長度，筆者使用兩段對稱的回旋曲線連接兩段圓弧，并將設計的曲線序列作為泊車路徑，如圖3所示。

圖3 規(guī)劃路徑示意圖

路徑起始點為K1，目標點為K5，由傳感器獲取位置。K1K2段，車輛轉角處于最大值；K2K3段是曲率由最大變化到0的回旋曲線，后半段曲線與前半段以K3為中心對稱。

由起始點到終止點的坐標變化量計算得到起始點縱坐標YK1及兩段圓弧圓心角β：

(4)

式中：lr為后懸長度;w為車輛寬度；D為車輛與車位的距離，由感知系統(tǒng)獲取。至此，路徑的各段曲線長度及關鍵點均可由式(3)和式(4)求出。

2 路徑跟蹤控制器設計

對于泊車系統(tǒng)而言，為了跟蹤規(guī)劃的泊車路徑，車輛的后軸中心坐標(x,y)需要快速趨近目標曲線，同時車輛的航向角θ也要與曲線各點的斜率保持一致。鑒于多個目標量同時控制的復雜性，筆者采用純追蹤的控制方法，通過車輛運動時的幾何關系將受控量進行降維，簡化泊車路徑跟蹤模型。

2.1 純追蹤模型的建立

在實際泊車系統(tǒng)中，控制器周期性地從傳感器接收信息，計算并發(fā)送控制量給執(zhí)行機構，所以必須對車輛運動學模型(式(1))進行離散化處理：

(5)

式中：(xn-1,yn-1,θn-1) 為第n-1個采樣周期時車輛的位姿狀態(tài)；(xn,yn,θn)為第n個采樣周期時通過更新的車輛位姿狀態(tài)；Δsn為車輛在第n-1個采樣周期到第n個采樣周期間行駛的距離，一般由后輪輪速脈沖傳感器得到。當車輛起始航向角已知的情況下，根據(jù)式(5),由轉角反饋量和輪速脈沖傳感器的信息推算出后軸中心的位置坐標(x,y)及車輛實時的航向角θ?；谕扑愕能囕v狀態(tài)及規(guī)劃的目標曲線，便可設計控制算法。

以直線的目標路徑為例，純追蹤模型示意圖如圖4所示。其中：O為瞬時轉向圓心；R為轉彎半徑；Ld為預定前視距離；p為橫向距離，代表車輛目前與目標路徑最近點的距離；γ為圓弧CD對應的圓心角。

圖4 純追蹤模型

在車輛行進過程中，根據(jù)固定的前視距離Ld在目標路徑S上找到唯一對應目標點D作為預瞄位置，圓弧CD即為理論運動路徑，通過控制車輛跟隨理論轉向角度，從而運動到指定路徑S上。根據(jù)幾何關系可得：

(6)

根據(jù)式(6)計算可得：

(7)

從而得到目標等效前輪轉角：

(8)

根據(jù)幾何關系，純追蹤模型將路徑的追蹤轉換為對目標點的追蹤，通過控制前輪轉角，快速減小位置誤差，同時逐漸減小航向誤差。

2.2 模糊純追蹤控制器的設計

由式(8)可知，前視距離Ld是純追蹤控制模型唯一可調參數(shù)，決定了控制效果的優(yōu)劣。一般而言，大的前視距離適合直線工況，跟蹤時間變長，不能完全跟隨路徑的細節(jié)；小的前視距離適合曲線工況，使車輛迅速地跟隨曲率較大的目標軌跡，但參數(shù)選擇過小會引起震蕩。由于泊車路徑由多段曲線構成，且曲線特性隨車型參數(shù)而發(fā)生改變，單一的前視距離不能滿足復雜的泊車工況。因此，利用橫向距離p和航向角度誤差θe來反映車輛與目標點的位置偏差與角度偏差，將其設置為模糊控制器輸入，動態(tài)改變前向距離以達到對復雜路徑的適應性?？刂葡到y(tǒng)框圖如圖5所示。

圖5 控制系統(tǒng)框圖

轉向角偏差、橫向距離偏差及輸出前視距離均分為7個模糊集合，橫向距離取[-40，40](cm)，航向偏差取[-90，90](°)，前視距離取[0.2，1.0](m)。以曲線左側橫向偏差為正，右側為負；航向偏差順時針為正，逆時針為負。建立模糊規(guī)則的原則為：①航向誤差較大時，快速減小前視距離，使車輛方向快速與曲線達到一致，進而減小位置誤差；②橫向誤差在逐漸減小時，關注系統(tǒng)的穩(wěn)定性，適當增大前視距離，防止系統(tǒng)震蕩。綜合考慮橫向距離和航向偏差不同情況下的重要性，構建前視距離輸出模糊規(guī)則，如表1所示。

表1 前視距離為輸出的模糊規(guī)則表

3 仿真與分析

3.1 路徑規(guī)劃仿真

使用仿真車輛為江淮iev7s，實驗車輛參數(shù)如表2所示，在MATLAB GUI中根據(jù)規(guī)劃方法進行仿真，檢驗路徑規(guī)劃的可行性。

路徑規(guī)劃仿真結果如圖6所示，最小車位長度為6.2 m，最小車位長度與車長比值為1.50。文獻[12]使用的仿真車輛的車長為4.084 m，單次泊車最小泊車位為6.8 m，比值為1.67。筆者規(guī)劃的泊車路徑使用了回旋曲線，保證了泊車過程的曲率連續(xù)，并且在一定程度上減小了需求車位長度，泊車終止坐標處車輛與車位平行，路徑平滑，達到路徑規(guī)劃的目標。

表2 實驗車輛參數(shù) m

圖6 路徑規(guī)劃仿真結果

3.2 路徑跟蹤仿真

泊車路徑會隨車輛參數(shù)的不同而發(fā)生調整，且在尋找車位的過程中，車輛的直線運動也是受泊車系統(tǒng)控制，故僅使用單一的泊車路徑對控制算法進行驗證是不夠的。一般使用直線、正弦曲線等路徑對跟蹤控制算法進行驗證是比較有代表性的，可以客觀地評價算法對路徑跟蹤的響應速度、跟蹤誤差等指標。仿真中車速均設定為5 km/h。

3.2.1 直線路徑

圖7 對直線路徑跟蹤情況

由圖7(a)可知，模糊純追蹤方法在x=1.2以后誤差趨于穩(wěn)定，能快速減小誤差，且軌跡較為平滑。由圖7(b)可知，Ld=0.8 m的純追蹤方法收斂速度稍慢，略微超調后，能很好地跟隨直線的工況；Ld=0.2 m時響應速度最快，但是會產(chǎn)生震蕩，達到穩(wěn)定的時間過長。

3.2.2 正弦曲線路徑

圖8 對正弦曲線路徑跟蹤情況

3.2.3 回旋曲線優(yōu)化的兩段式泊車路徑

圖9 對泊車曲線跟蹤情況

車輛模型泊入車位仿真如圖10所示，可知基于模糊純跟蹤控制的方法在起始位姿有較大誤差時可以很好地實現(xiàn)對泊車路徑的跟蹤，讓車輛無碰撞地泊入車位。

圖10 車輛模型泊入車位仿真

綜合以上情形可以看出：使用模糊純追蹤控制算法在多種工況下均能有效跟隨目標路徑，誤差較小，可以滿足泊車的要求，有較好的穩(wěn)定性。

4 結論

針對兩段式泊車曲線曲率不連續(xù)的缺陷，使用回旋曲線對其進行優(yōu)化，給出路徑計算方法，并通過仿真驗證了該泊車路徑的可行性。對于路徑跟隨模型的復雜性，使用純追蹤模型可將路徑追蹤模型簡化。由于純追蹤模型前向距離是一個固定值，無法很好跟隨復雜的泊車路徑工況，設計了模糊控制器來實現(xiàn)前向距離的動態(tài)調整，通過仿真驗證了該算法在指定泊車路徑跟蹤的可靠性，有效改善了泊車系統(tǒng)的泊車精度。同時,對直線及正弦曲線等目標曲線追蹤進行測試，證明了該路徑跟蹤方法有一定的道路自適應性。