無人駕駛汽車平行泊車路徑規(guī)劃優(yōu)化與跟蹤

于蕾艷，王顯宇，吳寶貴，侯澤宇，吳永輝

(中國石油大學(華東)機電工程學院，山東 青島 266580)

應用于先進駕駛輔助系統(tǒng)(advanced driver assistance system，ADAS)的自動泊車技術(shù)一定程度上解決了“泊車難”問題.自動平行泊車關(guān)鍵技術(shù)包括路徑規(guī)劃方法和路徑跟蹤方法.在路徑規(guī)劃方法研究方面，路徑規(guī)劃類型包括多項式曲線、圓弧-直線、B樣條曲線、Sigmoid函數(shù)和反正切函數(shù)曲線規(guī)劃等.例如，直線—過渡曲線—圓弧段—過渡曲線—圓弧段五段式平行泊車路徑規(guī)劃滿足曲率連續(xù)要求，路徑跟蹤難度較小[1]；基于圓弧—直線組合曲線推導平行泊車可行泊車起始區(qū)域邊界，對固定回旋曲線旋轉(zhuǎn)和翻轉(zhuǎn)變換得到曲率連續(xù)的平行泊車路徑[2].在路徑跟蹤方法研究方面，采用純跟蹤算法、最優(yōu)控制方法、改進迭代學習算法、模糊算法、線性反饋控制和滑?？刂频萚3].例如，基于非光滑控制的路徑跟蹤控制方法在有限時間內(nèi)全局精確跟蹤參考路徑，具有抗擾動性和快速收斂性[4]的特點.但是，無人駕駛汽車路邊平行泊車工況的泊車路徑規(guī)劃優(yōu)化方法[5-6]、泊車路徑跟蹤方法還需深入研究，以綜合提升路邊平行泊車的各項性能.

筆者分析無人駕駛汽車路邊平行泊車工況的特點，選取合理的多項式曲線進行路徑規(guī)劃.建立路徑規(guī)劃曲線參數(shù)優(yōu)化設(shè)計數(shù)學模型，選取設(shè)計變量，分析泊車邊界條件、路徑曲率、避撞、泊車空間等要求，建立約束條件，選取合適的目標函數(shù).采用粒子群優(yōu)化算法得到路徑參數(shù)泊車起始位置橫坐標，再計算多項式待定系數(shù)，得到優(yōu)化、平滑、曲率連續(xù)的規(guī)劃路徑曲線.基于純跟蹤算法進行泊車路徑跟蹤，通過Matlab平臺仿真試驗，驗證泊車安全性、平穩(wěn)性和乘坐舒適性的提升效果.

1 路邊平行泊車工況描述

無人駕駛汽車路邊平行泊車工況如圖1所示，汽車1為路邊待泊車位前方已泊汽車，汽車2為待泊的無人駕駛汽車,v為車速，bf為車位寬度，Ls為汽車2完成泊車后與前車1的安全距離，l為軸距，lf為前懸長度，lr為后懸長度，L為車長，b為車寬.泊車過程為汽車2的人機交互系統(tǒng)接收到乘車人泊車指令后，采用視覺傳感器與超聲波雷達融合方式實現(xiàn)車位檢測.根據(jù)車位類型進行路徑規(guī)劃，基于線控底盤的運動控制單元控制車輛前輪轉(zhuǎn)角與車速實現(xiàn)路徑跟蹤.以待泊車位的右前方角點為坐標原點，以車位縱向為x軸，建立直角坐標系.以汽車2后軸中點為參考點，A點為泊車起始位置，B點為泊車目標位置，汽車2沿泊車路徑曲線AB駛?cè)胲囄唬瓿刹窜?

圖1 路邊平行泊車工況示意圖

2 基于粒子群優(yōu)化的泊車路徑優(yōu)化

2.1 泊車路徑曲線類型和階次選取

泊車路徑規(guī)劃采用多項式曲線.確定曲線階次時，曲線AB光滑的要求如下：路徑曲率連續(xù)要求路徑曲線二階連續(xù)；泊車過程中由于轉(zhuǎn)向電動機轉(zhuǎn)向性能約束和轉(zhuǎn)向系統(tǒng)慣性特性，轉(zhuǎn)向輪轉(zhuǎn)角不能突變，要求路徑曲線三階連續(xù)，路徑曲線階次最低為四次，常采用奇次階曲線.曲線階次越高，待定系數(shù)越多，計算難度越大.因此，兼顧泊車曲線光滑、降低計算難度要求，采用五次多項式曲線.

在圖1所示直角坐標系下，路徑曲線表達式為

y=a0+a1x+a2x2+a3x3+a4x4+a5x5，

(1)

式中：y為汽車2后軸中點縱坐標值，m；x為汽車2后軸中點橫坐標值，m；a0-a5為系數(shù).

2.2 泊車路徑的約束條件

設(shè)A點坐標為(xA，yA)，B點坐標為(xB，yB).路徑曲線在A、B兩個端點處滿足：

(2)

(3)

汽車準備泊車時與泊車完成時的車身姿態(tài)均與車道線平行，即泊車起始位置與泊車目標位置的車身航向角均為0，A、B點一階導數(shù)均為0，即

(4)

(5)

在泊車起始位置A點，汽車未開始轉(zhuǎn)向，轉(zhuǎn)向輪轉(zhuǎn)角為0，路徑曲率為0，曲率為

(6)

由式(4)求導得A點處路徑二階導數(shù)為0，即

(7)

在泊車目標位置B點完成泊車，轉(zhuǎn)向輪回正，轉(zhuǎn)向輪轉(zhuǎn)角為0，B點處路徑曲率為0，可得

(8)

泊車路徑的最大曲率κmax需要滿足約束條件，確保路徑可跟蹤.約束條件如下：κmax對應的最小曲率半徑1/κmax大于汽車最小轉(zhuǎn)向半徑Rmin，即

κmax<1/Rmin.

(9)

建立碰撞約束，為避免汽車與道路邊緣碰撞，泊車過程中汽車右前頂點B′要始終在道路邊緣左側(cè)，由B′坐標計算公式得到約束條件為

(10)

由圖1可知，B點橫坐標xB由汽車2的軸距l(xiāng)、前懸長度lf及與前車1的安全距離Ls確定，即

xB=-(l+lf+Ls).

(11)

Ls取 0.5 m，代入式(11)中，求得xB=-3.705 m.根據(jù)GB/T 51328—2018《城市綜合交通體系規(guī)劃標準》，城市非機動車道寬度不小于2.5 m，泊車完成后，汽車2在泊車位中央，yB=bf/2=1.25 m，B點坐標確定.

假設(shè)汽車2靠道路右側(cè)行駛，A點縱坐標簡化計算為yA=bf+b/2.A點橫坐標xA待優(yōu)化.在yA確定的情況下，xA決定了路徑長度，受限于汽車傳感器探測范圍，|xA|有上限值|xA|1.在滿足泊車要求的前提下，|xA|越小，泊車所需空間越小.由圖1可得xA的約束條件為

xA<-(l+lf+L+Ls).

(12)

泊車路徑曲線中的未知量為xA、a0、a1、a2、a3、a4、a5.先優(yōu)化得到xA，再通過6個等式約束方程(2)、(3)、(4)、(5)、(7)、(8)求解得到其他6個路徑參數(shù)值.

2.3 目標函數(shù)

路徑曲率影響泊車過程中的汽車平穩(wěn)性，路徑最大曲率κmax越小，跟蹤路徑所需轉(zhuǎn)向輪轉(zhuǎn)角就越小，泊車平穩(wěn)性越好.xA影響路徑長度，|xA|越小，泊車路徑越短，泊車效率越高.|xA|與κmax存在矛盾，|xA|越大，κmax越小.設(shè)計目標函數(shù)時，權(quán)衡|xA|與κmax二者的大小.目標函數(shù)為

min(η1κmax+η2|xA|)，

(13)

式中：η1和η2分別為κmax、|xA|的權(quán)重因子，由于κmax比|xA|小得多，取η1=63，η2=1.

綜上，建立了包括式(13)所示的目標函數(shù)，式(2)、(3)、(4)、(5)、(7)、(8)所示的6個等式約束和式(9)、(10)、(12)所示的3個不等式約束，設(shè)計變量為xA的優(yōu)化設(shè)計數(shù)學模型.

2.4 基于粒子群優(yōu)化的路徑參數(shù)優(yōu)化

粒子群優(yōu)化(particle swarm optimization，PSO)算法是一種精度高、收斂速度快、參數(shù)少、易實現(xiàn)的隨機搜索算法，通過集體協(xié)作使群體達到最優(yōu).優(yōu)化問題的每個潛在解視為搜索空間的一只鳥，稱之為“粒子”.每個粒子有1個目標函數(shù)決定的適度值和1個速度來決定飛行距離和方向.所有粒子追隨當前的最優(yōu)粒子在解空間中進行搜索[7].參數(shù)選取如下：

1)粒子群規(guī)模N一般在[20，100]范圍內(nèi)取值，粒子群規(guī)模越大，越易找到全局最優(yōu)解，但算法運行時間越長，取N=30個.

2)粒子維度D根據(jù)搜索空間維數(shù)取值，取D=1維.

3)以最小誤差要求滿足最大迭代次數(shù)M，取M= 50次.

4)學習因子c1和c2，c1影響局部最優(yōu)值，c2影響全局最優(yōu)值，取c1= 1.5，c2=2.5.

5)粒子最大速度vmax決定粒子飛行中可飛過的最大距離，vmax過大，粒子可能飛過最優(yōu)解的位置；vmax過小，粒子陷入局部最優(yōu).vmax=λxdmax，λ∈[0，1]，xdmax為粒子變化范圍，取vmax= 0.3.

6)慣性權(quán)重w是PSO最重要的參數(shù)，調(diào)節(jié)粒子對解空間的搜索范圍，增大w能提高全局搜索能力，減小w能提高局部搜索能力，合理的w是實現(xiàn)高效搜索和避免陷入局部最優(yōu)的關(guān)鍵，擇優(yōu)選取w=0.5.

Matlab軟件編程實現(xiàn)的PSO算法流程如圖2所示，其中：pbest為粒子搜尋過的最優(yōu)位置；gbest為整個粒子群搜尋過的最優(yōu)位置.經(jīng)9次迭代求解出最優(yōu)值，目標函數(shù)值為20.743.泊車路徑曲線起點橫坐標xA=-14.955 m，滿足路徑最大曲率κmax=0.092 m-1小于汽車最小轉(zhuǎn)向半徑對應曲率的要求.確定xA值之后，求解式(2)、(3)、(4)、(5)、(7)、(8)組成的方程組，得到路徑曲線參數(shù)值[a0，a1，a2，a3，a4，a5]=[0.091 7，-1.063 1，-0.358 0，-0.053 0，0.003 2，-0.000 07].PSO優(yōu)化后的泊車路徑五次多項式曲線表達式為

圖2 PSO算法流程圖

y=0.091 7-1.063 1x-0.358 0x2-

0.053 0x3-0.003 2x4-0.000 07x5.

(14)

基于PSO算法優(yōu)化的泊車路徑規(guī)劃結(jié)果如圖3所示.基于PSO算法優(yōu)化的五次多項式泊車路徑平滑且曲率連續(xù)，汽車一次性泊車入位，沒有出現(xiàn)原地轉(zhuǎn)向現(xiàn)象，減小了轉(zhuǎn)向系統(tǒng)負荷.路徑起始位置與目標位置曲率均為0，保證了汽車在路徑端點轉(zhuǎn)向輪回正.

圖3 基于PSO算法優(yōu)化的泊車路徑規(guī)劃結(jié)果

3 基于純跟蹤算法的泊車路徑跟蹤

純跟蹤(pure pursuit)算法是基于幾何學的汽車橫向控制方法[8]，算法簡單、易實現(xiàn)，在低速工況具有較好的跟蹤效果.純跟蹤算法模型如圖4所示，其中：P為預瞄點；ld為預瞄距離；R為汽車后軸中點轉(zhuǎn)向半徑；α為車身軸線與后軸中點A與預瞄點P連線的夾角；e為車身軸線與預瞄點的橫向偏差；δ為前輪等效轉(zhuǎn)角.

圖4 純跟蹤算法模型

基于汽車后軸中點位置在目標路徑上以預瞄距離長度匹配一個預瞄點，假設(shè)汽車后軸中點按照一定轉(zhuǎn)向半徑到達預瞄點，根據(jù)轉(zhuǎn)向半徑、預瞄距離及汽車坐標系下的預瞄角度確定前輪轉(zhuǎn)角，實現(xiàn)對預瞄點跟蹤.前輪等效轉(zhuǎn)角控制律為

(15)

ld越大，控制效果越平滑，但會在某些急劇轉(zhuǎn)角處產(chǎn)生轉(zhuǎn)向不足情況；ld越小，控制效果越精確，但可造成汽車控制不穩(wěn)定甚至震蕩.將ld設(shè)為關(guān)于車速的函數(shù)，即

ld=gv+la，

(16)

式中：g為調(diào)節(jié)系數(shù)，取g=2；la為預設(shè)預瞄距離，m，取la=0.5 m；車速v=3 m·s-1.

路徑起點設(shè)為(-14.955，3.330)m，泊車起始位置位于路徑起點A，航向角為0.基于純跟蹤算法的路徑跟蹤結(jié)果如圖5所示，汽車較準確地跟蹤基于PSO算法優(yōu)化的五次多項式曲線泊車規(guī)劃路徑.汽車在停止運動時的位置為(-3.949，1.249)m，規(guī)劃的路徑終點為(-3.705，1.250)m，位置誤差|xe|= 0.243 9 m，|ye|= 0.001 5 m.汽車與道路邊界最易碰撞處位于汽車右前頂點處，其縱坐標最小值為0.026 3 m，汽車運動時沒有發(fā)生碰撞現(xiàn)象.路邊平行泊車路徑跟蹤結(jié)果如圖6所示，汽車前輪等效轉(zhuǎn)角和航向角連續(xù)變化，保證了汽車泊車過程的平穩(wěn)性與乘坐舒適性.從圖6b可以看出：汽車運動過程中路徑跟蹤誤差范圍為[0，0.049 7]m，誤差較小，汽車能較準確地跟蹤規(guī)劃路徑.

圖5 路邊平行泊車路徑跟蹤過程

圖6 路邊平行泊車路徑跟蹤結(jié)果

4 結(jié) 論

1)基于粒子群優(yōu)化算法的優(yōu)化五次多項式曲線路邊平行泊車工況路徑規(guī)劃方法，綜合提高了泊車安全性、泊車效率、平穩(wěn)性和乘坐舒適性等性能.設(shè)計變量取泊車起始位置橫坐標，基于泊車邊界條件建立6個路徑曲線等式約束，基于路徑曲率、避撞、減少泊車空間等要求建立3個不等式約束條件，選取最大曲率和泊車起始位置橫坐標絕對值的加權(quán)和作為目標函數(shù)，建立路徑規(guī)劃曲線參數(shù)優(yōu)化設(shè)計的數(shù)學模型方法科學合理.利用粒子群優(yōu)化算法求解得到路徑參數(shù)泊車起始位置橫坐標，再計算路徑曲線的6個多項式待定系數(shù)，獲得了平滑、曲率連續(xù)的規(guī)劃路徑曲線.

2)無人駕駛汽車基于純跟蹤算法進行泊車路徑跟蹤，較精確地跟蹤優(yōu)化的五次多項式曲線路徑，與周圍障礙物無碰撞，滿足安全性要求.前輪轉(zhuǎn)角連續(xù)變化，滿足泊車過程汽車平穩(wěn)性及乘坐舒適性等要求.