基于粒子群算法的平行泊車軌跡規(guī)劃

陳樂

(長安大學汽車學院，陜西西安 710064)

0 引言

隨著汽車“智能化、電動化、網(wǎng)聯(lián)化”的提出，智能汽車逐漸走進了人們的視野。隨著科技的發(fā)展，智能汽車的定義越來越復(fù)雜，包含的內(nèi)容逐漸增多。根據(jù)汽車工程協(xié)會(society of automotive engineerings，SAE)對自動駕駛的等級劃分，目前大多數(shù)車輛都處于L2或L3等級，這兩種等級都是特指在一定封閉環(huán)境下的自動化行為。自動泊車技術(shù)(auto-parking system，APS)是智能汽車技術(shù)的重要組成部分[1]，也是比較適合目前科技水平的一項智能化技術(shù)。自動泊車技術(shù)主要通過使用車載傳感器和自動轉(zhuǎn)向技術(shù)實現(xiàn)車輛自動泊車功能，提高泊車效率的同時也可以減少因泊車導致的交通事故。國內(nèi)外諸多大型實驗室和汽車公司都對泊車策略進行研究。Paromtchik等[2]采用正弦曲線對泊車軌跡進行擬合；Lee等[3]采用模糊控制實現(xiàn)自動泊車功能；宋金澤等[4]采用微分平坦理論對泊車路徑進行分析與規(guī)劃；李紅等[5]采用多約束方程對泊車軌跡進行規(guī)劃，并采用MATLAB非線性求解器進行參數(shù)求解。近年來，隨著智能算法研究的深入，越來越多的智能算法廣泛應(yīng)用在自動泊車軌跡規(guī)劃中[6]。

自動泊車技術(shù)主要分為車位識別、路徑規(guī)劃、路徑控制3個單元[7]。車位識別單元主要通過傳感器模塊和信息融合算法對泊車環(huán)境進行分析，主要實現(xiàn)尋找合適泊車環(huán)境和檢測障礙物的功能，一般采用超聲波傳感器，如今越來越多的實驗室采用廣角攝像頭采集泊車信息。路徑規(guī)劃單元是泊車系統(tǒng)的核心單元，通過建立車輛運動學模型，分析車輛轉(zhuǎn)彎時方向盤的轉(zhuǎn)角與車速、轉(zhuǎn)彎半徑、車位角的關(guān)系，同時對周圍潛在碰撞區(qū)域進行分析，根據(jù)分析結(jié)果，擬合出可行且安全的泊車軌跡。路徑控制單元是車輛能否按照理想軌跡實施泊車的關(guān)鍵。

目前自動泊車系統(tǒng)的研究方法主要分為2種：一種是基于路徑規(guī)劃進行泊車操作[8-11]；另一種是通過駕駛員的泊車經(jīng)驗對其建立模糊規(guī)則，設(shè)計模糊控制器實現(xiàn)自動泊車[12-17]。本文以最終車位角和初始車位角之和最小為目標，擬規(guī)劃出可行且安全的泊車軌跡。

1 泊車路徑規(guī)劃

1.1 汽車運動學模型

由于整個泊車過程車速很低，大約不超過5 m/s，但需要較大的轉(zhuǎn)向角度，所以不考慮汽車的滑移和側(cè)傾，不采用較復(fù)雜的動力學模型，根據(jù)Ackerman原理,建立簡單的運動學模型:

xr2+(yr+Lcot α)2=(Lcot α)2,

(1)

式中：xr、yr分別為汽車后軸中點的橫、縱坐標，L為汽車的軸距，α為前軸中點的等效轉(zhuǎn)角。

在泊車過程中，若方向盤轉(zhuǎn)角固定，α基本為定值，由式(1)可知，泊車的路徑會逐漸形成一個圓形軌跡，并且該泊車軌跡與泊車的速度無關(guān)。但在實際泊車過程中，方向盤的轉(zhuǎn)速和泊車速度往往會對泊車軌跡產(chǎn)生影響[18]。為了簡化泊車過程，忽略泊車中的速度變化，假設(shè)整個泊車過程以3 m/s的速度勻速進行，并且方向盤勻速轉(zhuǎn)動。為了使方向盤的轉(zhuǎn)動連續(xù)，不發(fā)生原地轉(zhuǎn)向和轉(zhuǎn)向突變的情況，規(guī)劃的軌跡曲率必須限制在一定范圍內(nèi)。

本文選用改進的反正切擬合函數(shù)作為軌跡函數(shù)的基礎(chǔ)函數(shù)，考慮泊車過程的車輛約束和環(huán)境約束，用粒子群算法對未知參數(shù)進行優(yōu)化求解。

1.2 泊車路徑規(guī)劃

泊車路徑規(guī)劃，即以汽車后軸中點作為軌跡規(guī)劃點，以該點的軌跡作為泊車的軌跡。為計算方便，將車輛簡化為矩形ABCD，以停車位的左下角點c作為坐標原點，建立大地坐標系xOy，如圖1所示。圖1中：Lr為車輛后軸到車輛后端的距離，L為車輛的軸距，Lf為車輛前軸到車輛前端的距離，l為車輛寬度，L1為車位長，L2為車位寬，θ為車輛任意位置的方位角，S0為汽車后軸初始位置在x軸方向與車位邊bc的距離，h0為汽車縱軸線初始位置在y軸方向與車位邊cd的距離。

圖1 平行泊車參數(shù)示意圖

泊車軌跡的基礎(chǔ)函數(shù)由修正的反正切函數(shù)構(gòu)成

y=a1arctan(a2x+a3)(a4x3+a5x2+a6x+a7)+a8,

(2)

式中：y為泊車軌跡的縱坐標，x為泊車軌跡的橫坐標，a1～a8為軌跡方程中的8個待優(yōu)化參數(shù)。

由式(2)得車輛在任意位置的一階導數(shù)、二階導數(shù)、方位角和曲率分別為:

由于車輛后軸中點的位置與汽車外輪廓點A、B、C、D相對固定，所以可根據(jù)幾何關(guān)系和每個時刻的后軸中點坐標(x，y)推導出各個汽車外輪廓點坐標。A、B、C、D點的坐標分別為：

2 車輛及環(huán)境約束

為找到最切合實際的泊車路線，將整個泊車過程模型化，建立多約束非線性模型，根據(jù)汽車實際車身限制約束和環(huán)境約束規(guī)劃泊車路線，確定含有未知參數(shù)的目標函數(shù)作為優(yōu)化參考，用粒子群算法在可行解中找到近似優(yōu)化解，得到較好的泊車路徑。

2.1 車輛約束

1)軌跡曲率半徑

汽車轉(zhuǎn)向時存在最小轉(zhuǎn)向半徑Rmin，要求泊車軌跡的曲率

ρ≤1/Rmin。

2)轉(zhuǎn)向角速度

在自動泊車過程中，方向盤的轉(zhuǎn)向角速度通常存在最大值，要求方向盤的轉(zhuǎn)向角速度

ω≤ωmax。

2.2 環(huán)境約束

環(huán)境約束的精確程度直接影響求解結(jié)果，過多的約束條件可能造成求解結(jié)果太少，進而不能找到近似優(yōu)化解，而約束條件太少或者不準確又導致求解結(jié)果錯誤。將整個泊車約束分為起始位置約束、避障約束、最終位置約束3部分。

1)起始位置約束

在起始位置階段，泊車位置要求汽車后軸中心在圖1(S0，h0)處，且方位角θ為0，即車輛縱軸線與圖1中x軸方向平行，用公式表示為：

(3)

由于θ=0是一個確切的值，這對于優(yōu)化的結(jié)果要求過于苛刻，為了得到更多可行解，并且要在一定范圍內(nèi)求解最優(yōu)解，對式(3)進行改進,得：

式中ε1、ε2為約束參數(shù)。

2)避障約束

避障約束是在泊車過程中，對車輛躲避周圍環(huán)境固定障礙進行限制的約束，防止泊車過程中發(fā)生事故，導致泊車失敗。避障約束相對來說較為復(fù)雜。根據(jù)車輛自身轉(zhuǎn)向限制，找出極限位置下的特殊狀態(tài)點作為邊界，以此來增加避障約束的條件。

圖2表示泊車初始時對a點避障的極限位置。其中：

①以a點為圓心，以安全距離Rs為半徑作圓，稱此圓為a點的安全圓(圖2中小圓)。

YC≥yq-Lrsinθ，x∈[xq，S0]。

當車輛繞開a點，進入車位中，如圖3所示，車輛C點可能與車位cd邊發(fā)生碰撞，有避障約束公式：

YC≥ys，x∈[xs+Lr，xq]，

式中xs、ys為車輛與車位邊bc、cd的安全距離。

圖2 車輛對a點避障的極限位置 圖3 車輛對cd邊的避障要求

3)最終位置約束

(4)

同式(3)一樣，根據(jù)反正切函數(shù)的特性，對式(4)進行改進，得：

式中ε3、ε4為約束參數(shù)。

綜合考慮上述約束，以泊車軌跡方程參數(shù)a1，a2，a3，a4，a5，a6，a7，a8為變量，以車輛到達路徑終點時車輛縱軸線與cd邊夾角最小為目標建立多約束非線性模型：

min|θend+θso|，

(5)

式中：θend為最終車位角，θso為初始車位角。

對式(5)的優(yōu)化問題，存在2種約束條件，即以等式形式存在和以不等式形式存在的約束條件。只有同時滿足等式條件和不等式條件的解，才能作為可行解。

在可行解中，根據(jù)目標函數(shù)的要求，求解最大或最小值，該最大或最小值的解作為本優(yōu)化問題的最終解。

以等式形式存在的式(5)的約束條件

(6)

以不等式形式存在的式(5)的約束條件

(7)

3 仿真實例及求解

3.1 仿真參數(shù)

車輛參數(shù)見表1，根據(jù)式(5)～(7)建立泊車方程，求解{a1，a2，a3，a4，a5，a6，a7，a8}的值。根據(jù)參考文獻[19]，車位標準尺寸為：車位長7 m、寬2.4 m，道路寬4 m。根據(jù)不同的車輛起始位置優(yōu)化不同的泊車路線，車輛后軸中點各起始點坐標如表2所示。

表1車輛參數(shù)

L/mLr/mRmin/mLf/ml/m速度/(m·s-1)2.451.065.350.861.65

表2 車輛后軸中點各起始點坐標 m

3.2 仿真求解方法

采用粒子群算法(particle swarm optimization, PSO)進行求解。粒子群算法又稱為鳥群覓食算法。PSO從隨機解出發(fā)，通過約束條件和適應(yīng)值大小篩選可行解，并且重復(fù)迭代尋找最優(yōu)解。PSO的優(yōu)點在于精度高，收斂快，規(guī)則簡單，因此該算法被廣泛使用。根據(jù)其規(guī)則的不同，PSO分為帶罰函數(shù)的PSO、自適應(yīng)PSO等。

本文考慮到式(6)(7)中存在較多的約束條件，求解過程比較復(fù)雜，可以采用帶罰函數(shù)的PSO，進而將有約束的優(yōu)化問題轉(zhuǎn)化為無約束的問題。約束函數(shù)的某種組合組成的“懲罰”項，加在原來的目標函數(shù)上迫使迭代點逼近可行域。

對于優(yōu)化問題

(8)

式中：gi(x)0為約束條件，m為約束條件的個數(shù)，A為x的約束條件，i為約束條件的個數(shù)。

由于不等式約束gi(x)≥0，等價于等式約束min(0,gi(x))=0。因此可將不等式約束問題轉(zhuǎn)化為等式約束問題

(9)

(10)

式中：F(x，M)為罰函數(shù)；M為罰因子，為正常數(shù)；Mp(x)為罰項。

當M充分大時，F(xiàn)(x，M)的最優(yōu)解能逼近約束問題的最優(yōu)解。因此，罰函數(shù)F(x，M)對可行點不實行懲罰，而對非可行點給予很大的懲罰，從而將求解約束極值的問題轉(zhuǎn)化為求解無約束極值問題。

3.3 仿真求解步驟

圖4 粒子群算法求解過程

PSO的求解過程如圖4所示。

1)建立仿真初始化函數(shù)。根據(jù)式(8)和表1、2及道路參數(shù)建立初始化函數(shù)，設(shè)置相關(guān)參數(shù)值。

2)建立泊車方程。根據(jù)實際泊車數(shù)據(jù)，對比得到曲率相近的反正切函數(shù)，以此作為泊車軌跡方程，對其進行參數(shù)化，并由后軸中心點的軌跡推算汽車輪廓點的軌跡，建立輪廓軌跡方程。

3)建立約束條件函數(shù)并設(shè)置相關(guān)罰函數(shù)。根據(jù)式(6)(7)對泊車過程的車輛約束和環(huán)境約束建立約束條件函數(shù)，并對不符合約束條件的情況，根據(jù)式(9)(10)對其設(shè)置罰函數(shù)，以此優(yōu)化模型。

4)設(shè)置目標函數(shù)。根據(jù)式(5)，以車輛到達路徑終點時車輛縱軸線與cd邊夾角最小設(shè)置目標函數(shù)。

5)設(shè)置PSO參數(shù)。對粒子群的粒子數(shù)、迭代次數(shù)、衰減因子進行設(shè)置，確保粒子算法的計算精度。

6)算法求解過程。初始化粒子群，即隨機分配各個取值區(qū)間的參數(shù)值，帶入目標函數(shù)中求解，并帶入約束函數(shù)中，根據(jù)滿足約束條件的個數(shù)，給各個粒子群設(shè)置不同的適應(yīng)度。根據(jù)適應(yīng)度的大小，選擇最優(yōu)的粒子作為最優(yōu)選擇，并且在最優(yōu)選擇附近對粒子群進行衰減更新，以此尋找最優(yōu)的粒子。

7)通過Matlab進行編程，然后迭代求解，結(jié)果如表3所示。

表3 泊車工況參數(shù)

4 結(jié)果分析

車輛4種起始位置泊車軌跡如圖5所示。

通過PSO算法求解的參數(shù)，可以較好地完成泊車任務(wù)。O1為a點的安全圓，O2為cd邊的安全圓。有

式中：μ為安全閾值，可以根據(jù)車型和車位尺寸進行取值，本文安全閾值為0.3 m。

當車輛行駛至a點附近時，由于有安全閾值的存在，只要泊車軌跡上的點至O1的距離大于RO1,就可以保證車輛在轉(zhuǎn)彎時不會碰撞a點。同理，在車輛行駛到cd邊附近時，由于有安全閾值的存在，只要泊車軌跡上的點至O2的距離小于RO2，就可以保證車輛在泊車結(jié)束時，始終不會碰撞到cd邊。

從圖5中可以看出：4種泊車軌跡都符合該要求，所以泊車過程中，4種軌跡都可很好的避開a點和cd邊,保證了泊車過程中的行駛安全。

圖6為該泊車路徑的曲率，從圖6中可以看出：整個泊車過程中，只有第2個起始位置在行駛到3.8 m時超過了曲率的限制，其余路徑的曲率都小于1/Rmin=0.18 m-1，說明該規(guī)劃方法存在一定的依賴性，對于不同的起始位置，規(guī)劃的泊車路徑可能存在不符合事實的情況，這也是后期需要改進的地方；4種軌跡開始曲率都很小，幾乎可以忽略不計，避免了泊車初始階段出現(xiàn)原地轉(zhuǎn)向的情況，圖6a)的最大曲率與極限曲率仍然存在一定余量，余量越大說明車輛轉(zhuǎn)動角度的變化越小，所以圖6a)的車位初始位置視為最佳理想的初始位置，即：S0=10 m，h0=4 m。

a)起始點1 b) 起始點2 c) 起始點3 d) 起始點4圖6 4種起始位置的泊車曲率

5 結(jié)語

1)規(guī)劃的泊車路徑可以很好的避開其他車輛和道路邊界，整個轉(zhuǎn)向過程較平穩(wěn)，無原地轉(zhuǎn)向的情況出現(xiàn)，增加了泊車的平順性。

2)由于反正切函數(shù)的特性，導致停車狀態(tài)和起始位置狀態(tài)不能較好的滿足現(xiàn)實停車的要求，也導致粒子群算法在迭代過程中很難得出一個較優(yōu)解，所以對約束進行了修改，將一個確切的值改為范圍區(qū)間，這不僅符合現(xiàn)實停車的不規(guī)則性，也有利于對更多較好解進行篩選。

3)本文從3個自動泊車模塊中的路徑規(guī)劃模塊聯(lián)合Matlab對規(guī)劃出來的路徑進行了驗證，后續(xù)還需要選擇合適的跟蹤控制模型對該路徑的可行性進行仿真驗證，可以在理論支持的基礎(chǔ)上做出硬件，搭建實車平臺，進行實車驗證。