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

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

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

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

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

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

1 泊車路徑規(guī)劃

1.1 車輛運(yùn)動(dòng)學(xué)模型

對于前輪轉(zhuǎn)向車輛，后軸與車身的運(yùn)動(dòng)軌跡一致，因此將后軸中心作為車輛參考坐標(biāo)。車輛泊車時(shí)處于低速狀態(tài)，忽略側(cè)向力的作用，車輛不會出現(xiàn)滑移與側(cè)向移動(dòng)的狀態(tài)?；诎⒖寺D(zhuǎn)向機(jī)構(gòu)的車輛運(yùn)動(dòng)學(xué)模型[10]如圖1所示，關(guān)系式如式(1)所示。

圖1 車輛運(yùn)動(dòng)學(xué)模型

(1)

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

(2)

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

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

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

圖2 回旋曲線示意圖

在P點(diǎn)處曲率半徑為R，O點(diǎn)處曲率為零，則P點(diǎn)坐標(biāo)及在P點(diǎn)處的航向角分別為：

(3)

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

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

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

由起始點(diǎn)到終止點(diǎn)的坐標(biāo)變化量計(jì)算得到起始點(diǎn)縱坐標(biāo)YK1及兩段圓弧圓心角β：

(4)

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

2 路徑跟蹤控制器設(shè)計(jì)

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

2.1 純追蹤模型的建立

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

(5)

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

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

圖4 純追蹤模型

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

(6)

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

(7)

從而得到目標(biāo)等效前輪轉(zhuǎn)角：

(8)

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

2.2 模糊純追蹤控制器的設(shè)計(jì)

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

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

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

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

3 仿真與分析

3.1 路徑規(guī)劃仿真

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

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

表2 實(shí)驗(yàn)車輛參數(shù) m

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

3.2 路徑跟蹤仿真

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

3.2.1 直線路徑

圖7 對直線路徑跟蹤情況

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

3.2.2 正弦曲線路徑

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

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

圖9 對泊車曲線跟蹤情況

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

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

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

4 結(jié)論

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