多項(xiàng)式曲線優(yōu)化的垂直泊車路徑規(guī)劃與跟蹤

江銘，彭育輝，黃煒，，徐德強(qiáng)

(1. 福州大學(xué)機(jī)械工程及自動(dòng)化學(xué)院，福建 福州 350108; 2. 廈門金龍旅行車有限公司，福建 廈門 361006)

0 引言

自動(dòng)泊車技術(shù)不僅能有效改善交通狀況避免泊車事故的發(fā)生，還能提高泊車成功率，減輕駕駛員的泊車壓力，現(xiàn)已成為智能駕駛領(lǐng)域研究的重點(diǎn)內(nèi)容[1]. 現(xiàn)有主要的自動(dòng)泊車路徑規(guī)劃和跟蹤策略可分兩種，一是通過分析泊車過程中的環(huán)境數(shù)據(jù)和對應(yīng)駕駛員控制信息，應(yīng)用模糊邏輯學(xué)和神經(jīng)網(wǎng)絡(luò)理論反復(fù)學(xué)習(xí)泊車路徑并轉(zhuǎn)化為復(fù)雜的轉(zhuǎn)向策略實(shí)現(xiàn)自動(dòng)泊車. 但此類方法需制定模糊規(guī)則和調(diào)節(jié)控制參數(shù)，涉及大量經(jīng)驗(yàn)數(shù)據(jù)，且適應(yīng)性差無法應(yīng)對陌生泊車環(huán)境[2]； 二是根據(jù)汽車運(yùn)動(dòng)學(xué)限制和空間約束預(yù)先規(guī)劃出泊車過程路徑，通過追蹤算法實(shí)現(xiàn)泊車，此類方法具有較強(qiáng)工程實(shí)踐意義[3]. Glaser等[4]、 Song等[5]、 黃江等[6]、 張家旭等[7]分別采用直線-圓弧、 B樣條、 多項(xiàng)式曲線、 回旋曲線方法設(shè)計(jì)泊車路徑. 在提高路徑跟蹤精度方面，主要方法有PID控制[8]、 滑模控制[9]、 模型預(yù)測控制[10]、 模糊控制[11]等. 李茂月等[12]通過前向行駛過程修正待泊汽車與停車位之前的位姿關(guān)系，保證了泊車過程的安全性及避免進(jìn)入泊車死區(qū)，但所規(guī)劃路徑曲率不連續(xù). 張家旭等[13]通過高斯偽譜法求解泊車路徑規(guī)劃的最優(yōu)控制問題得到無碰撞泊車路徑，擴(kuò)大了泊車場景的應(yīng)用. 此外，趙林峰[14]基于反正切函數(shù)擬合泊車路徑，設(shè)計(jì)非時(shí)間參考路徑跟蹤控制模型，同時(shí)應(yīng)用自抗擾控制策略設(shè)計(jì)了轉(zhuǎn)向控制器，實(shí)現(xiàn)對汽車轉(zhuǎn)角的跟蹤.

垂直泊車作為自動(dòng)泊車系統(tǒng)廣泛使用的場景，當(dāng)前垂直泊車路徑規(guī)劃系統(tǒng)研究主要基于單步后退的規(guī)劃方法. 由于單步后退泊車存在無中間段姿態(tài)調(diào)整措施和出現(xiàn)大幅度原地轉(zhuǎn)向的局限，在很多場景下都無法直接通過單步后退泊車方法實(shí)現(xiàn)自動(dòng)泊車，因此研究雙步進(jìn)退的垂直泊車路徑規(guī)劃方法顯得尤為重要. 上述研究仍存在規(guī)劃路徑曲率不連續(xù)、 計(jì)算量較大和對汽車線控轉(zhuǎn)向系統(tǒng)要求高等問題，本研究基于多項(xiàng)式曲線優(yōu)化，對雙步進(jìn)退垂直場景下的泊車路徑規(guī)劃與跟蹤方法進(jìn)行研究. 構(gòu)建車輛在行駛過程中的運(yùn)動(dòng)學(xué)模型，分析其可行起始點(diǎn)范圍，并綜合考慮車輛位姿、 結(jié)構(gòu)和道路邊界約束，以多項(xiàng)式為基函數(shù)、 泊車終點(diǎn)姿態(tài)角最小為目標(biāo)，建立泊車多約束非線性函數(shù)規(guī)劃模型. 同時(shí)，利用帶罰函數(shù)的粒子群算法求解滿足曲率平滑的無碰撞泊車路徑，設(shè)計(jì)模糊神經(jīng)網(wǎng)絡(luò)控制器進(jìn)行路徑跟蹤. 在Simulink/Carsim聯(lián)合仿真平臺上對所提的泊車路徑規(guī)劃算法和跟蹤控制策略進(jìn)行仿真測試分析.

1 泊車可行起始點(diǎn)范圍

雙步進(jìn)退垂直泊車示意如圖1(a)所示. 雙步進(jìn)退垂直泊車工況下，車輛從起點(diǎn)T0到終點(diǎn)T4的泊車過程如圖1(b)所示.泊車過程可分為4個(gè)階段： 1)從起點(diǎn)T0沿直線段L1行駛到T1； 2)以點(diǎn)E1為圓心，R1為半徑，從T1行駛θ1角度到T2； 3)以點(diǎn)E2為圓心，R2為半徑，從T2行駛θ2角度到T3； 4)從T3行駛L2距離到終點(diǎn)T4.

圖1 雙步進(jìn)退垂直泊車

考慮汽車最小轉(zhuǎn)彎半徑約束和泊車空間避障條件，基于直線-圓弧路徑下計(jì)算垂直泊車可行起始點(diǎn)范圍.

泊車空間避障約束包括： 1) 車輛右后點(diǎn)C與車位邊界的碰撞約束，即車輛中軸點(diǎn)與右方道路邊界的距離應(yīng)大于極限值LK； 2) 車輛左前點(diǎn)A與道路邊界的碰撞約束，即T1-T2路徑下車輛左前點(diǎn)A縱坐標(biāo)的最大值應(yīng)小于道路寬度Lroad; 3) 車輛左后方與車位點(diǎn)b的碰撞約束，即T2-T3路徑下車輛左后點(diǎn)B在坐標(biāo)軸上的橫軸坐標(biāo)值大于-SW； 4) 車輛右后方與車位點(diǎn)a的碰撞約束，即T2-T3路徑下車輛右側(cè)方到車位點(diǎn)a的距離大于極限值.各幾何約束如下所示：

(1)

式中：CW為汽車寬度；L為汽車軸距；LFS為汽車前懸長度；LRS為汽車后懸長度；Rmin為汽車后軸中點(diǎn)的最小轉(zhuǎn)彎半徑；SW為泊車位寬度；YT0為泊車起始位置時(shí)汽車后軸中點(diǎn)縱軸坐標(biāo)值；XE2、YE2分別為點(diǎn)E2橫縱軸坐標(biāo)值.

考慮最小轉(zhuǎn)彎半徑約束以及直線段長度限制，可得：

R1≥Rmin;R2≥Rmin;L1≥0;L2≥0; 0<θ1<π; 0<θ2<π

(2)

綜上所述，雙步進(jìn)退垂直泊車可行起始點(diǎn)范圍是由式(1)和式(2)的約束條件共同組成的封閉區(qū)域. 利用Matlab求解，可得雙步進(jìn)退垂直泊車可行起始點(diǎn)范圍.

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

垂直泊車路徑規(guī)劃問題可描述為： 待泊車輛在起始位置和終止位置間尋找不超過車輛轉(zhuǎn)向極限及曲率連續(xù)平滑的曲線段，且沿該線段行駛時(shí)車輛不與道路邊界發(fā)生碰撞. 將雙步進(jìn)退垂直泊車過程模型化，尋找合適曲線為路徑基函數(shù). 經(jīng)過比較發(fā)現(xiàn)，垂直泊車曲線類似C型曲線，與反正切、 B樣條、 回旋和多項(xiàng)式曲線相似，但反正切曲線函數(shù)在路徑起始點(diǎn)不能滿足曲率為零或小曲率的要求，B樣條曲線函數(shù)和回旋曲線函數(shù)在求解過程中規(guī)劃參數(shù)較多，計(jì)算量較大且無法準(zhǔn)確地確定控制節(jié)點(diǎn)的數(shù)量和位置，而多項(xiàng)式曲線函數(shù)既能保證泊車路徑的曲率為連續(xù)變化，不存在車輛大幅度原地轉(zhuǎn)向的情況，又能減少求解規(guī)劃參數(shù)降低計(jì)算量. 因此本研究選取多項(xiàng)式曲線函數(shù)為路徑基函數(shù)，停車姿態(tài)角最小為目標(biāo)，建立多約束非線性模型，并應(yīng)用改進(jìn)的基于罰函數(shù)粒子群算法進(jìn)行求解. 垂直泊車路徑規(guī)劃問題可描述為：

min|θend|; s.t.(車輛位置約束; 汽車結(jié)構(gòu)幾何約束; 空間避障約束)

(3)

2.1 雙步進(jìn)退垂直泊車規(guī)劃路徑約束

2.1.1車輛位置約束

雙步進(jìn)退垂直泊車過程如圖1(b)所示，假設(shè)T1-T2和T2-T3過程汽車行駛路線表達(dá)式分別為：

Y=P1X4+P2X3+P3X2+P4X+P5；U=V1X4+V2X3+V3X2+V4X+V5

(4)

式中：P1～P5以及V1～V5為待求多項(xiàng)式參數(shù)；Y為汽車T1-T2段前進(jìn)曲線函數(shù)；U為汽車T2-T3段后退曲線函數(shù).

汽車行駛路徑需經(jīng)過起始位置點(diǎn)T1、 經(jīng)停位置點(diǎn)T2和終止位置T3，故建立如下約束表達(dá)：

(5)

且多項(xiàng)式曲線在起始位置點(diǎn)、 經(jīng)停位置點(diǎn)和終止位置點(diǎn)的導(dǎo)數(shù)已知，可建立如下約束：

(6)

2.1.2汽車結(jié)構(gòu)幾何約束

汽車沿路徑曲線行駛時(shí)受自身結(jié)構(gòu)限制，所以轉(zhuǎn)向角不超過一定范圍值，約束表達(dá)如下：

(7)

式中:ρ為泊車路徑曲率.

2.1.3空間避障約束

為保證泊車過程中車輛不與周圍固定障礙物發(fā)生碰撞，車輛的路徑函數(shù)需滿足以下要求.

1) 由圖2(a)可知，為避免行駛車輛的左前端點(diǎn)A和車輛左后端點(diǎn)B與道路邊界的碰撞，要求車輛端點(diǎn)A縱坐標(biāo)YA和車輛端點(diǎn)B縱坐標(biāo)YB的最大值應(yīng)小于道路寬度Lroad，即：

(8)

2) 由圖2(b)可知，為避免行駛車輛的右側(cè)輪廓與待泊車位右側(cè)a點(diǎn)發(fā)生碰撞，要求車輛輪廓線BC位于車位點(diǎn)a的上方，即：

(9)

3) 由圖2(c)可知，為避免行駛車輛的左后輪廓與待泊車位的左側(cè)發(fā)生碰撞，要求車輛端點(diǎn)B橫坐標(biāo)值XB應(yīng)大于-SW，即：

(10)

2.1.4目標(biāo)函數(shù)

在雙步進(jìn)退垂直泊車多約束非線性函數(shù)模型中，選擇停車姿態(tài)角最小為目標(biāo)函數(shù)，即雙步進(jìn)退垂直泊車過程目標(biāo)函數(shù)為：

(11)

2.2 基于罰函數(shù)粒子群算法求解路徑

粒子群算法是一種基于群集協(xié)同的隨機(jī)搜索算法，該算法主要通過群集中各單獨(dú)體內(nèi)的相關(guān)協(xié)助和資訊共享來篩選可行解[15]. 標(biāo)準(zhǔn)粒子群算法主要用于求解無約束的數(shù)學(xué)模型，考慮到本研究規(guī)劃路徑函數(shù)中的約束條件較多，包括： 車輛位置和結(jié)構(gòu)約束、 空間避障約束，其求解過程較為復(fù)雜. 因此，選用帶罰函數(shù)的粒子算法對該類問題進(jìn)行求解，將原本的帶約束限制的求解過程轉(zhuǎn)化為無約束限制求解，得到滿足各約束且目標(biāo)函數(shù)最小的多項(xiàng)式曲線中未知參數(shù)，其中所確定的目標(biāo)函數(shù)為： 停車姿態(tài)和車位垂直方向的夾角最小. 基于罰函數(shù)的粒子群算法流程如圖3所示.

圖3 粒子群算法求解流程

3 垂直泊車路徑跟蹤控制

泊車路徑跟蹤控制的核心是通過控制車輛的轉(zhuǎn)向輪轉(zhuǎn)角，使汽車后軸中心點(diǎn)和航向角能穩(wěn)定快速地趨近于目標(biāo)路徑點(diǎn)和期望航向角. 為更好模擬分析路徑規(guī)劃結(jié)果，本研究利用模糊神經(jīng)網(wǎng)絡(luò)跟蹤控制方法設(shè)計(jì)路徑跟蹤控制器進(jìn)行泊車過程跟蹤.

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

將待泊汽車視為平面剛體且忽略行駛過程中的側(cè)滑現(xiàn)象，選擇汽車后軸中點(diǎn)作為參考點(diǎn)，建立汽車的運(yùn)動(dòng)學(xué)方程用于描述垂直泊車過程中汽車的轉(zhuǎn)向角度、 速度與后輪中心坐標(biāo)點(diǎn)間的關(guān)系：

(12)

式中：φ為等效前輪轉(zhuǎn)角；θ為車身航向角.

3.2 基于模糊神經(jīng)網(wǎng)絡(luò)跟蹤控制器的設(shè)計(jì)

基于T-S型模糊神經(jīng)網(wǎng)絡(luò)的跟蹤控制算法框架如圖4所示. 利用車輛運(yùn)動(dòng)學(xué)模型得到當(dāng)前的位置坐標(biāo)點(diǎn)和航向角度，通過與期望軌跡進(jìn)行對比，求解規(guī)劃路徑與實(shí)際路徑的位置距離誤差ye和航向角度誤差θe. 將位姿誤差信息輸入到模糊控制器輸入層中. 輸入層參數(shù)的模糊子集劃分成{負(fù)大 負(fù)小 零 正小 正大}，輸出層為前輪轉(zhuǎn)向角度φ. 模糊神經(jīng)網(wǎng)絡(luò)通過學(xué)習(xí)輸入?yún)?shù)，包括： 位置距離偏差、 航向角度偏差與輸出參數(shù)車輛前輪轉(zhuǎn)向角度的關(guān)系，訓(xùn)練模糊隸屬度函數(shù)的最佳參數(shù)，結(jié)果如圖5所示.

圖4 跟蹤控制框圖

圖5 模糊神經(jīng)網(wǎng)絡(luò)訓(xùn)練結(jié)果

4 仿真分析

自動(dòng)泊車的場景和單步垂直泊車仿真模擬如圖6所示.

圖6 泊車場景及單步垂直泊車工況

復(fù)雜的自動(dòng)泊車場景如圖6(a)所示，場景中車道寬度為5.5 m，垂直泊車位尺寸為5.0 m × 2.3 m. 圖中空閑垂直泊車位為目標(biāo)車位，道路前方為車庫墻體. 在該泊車環(huán)境下，待泊車輛無法直接通過單步倒退垂直泊車的方式駛?cè)肽繕?biāo)車位. 單步倒退垂直泊車過程如圖6(b)所示, 基于逆向最小轉(zhuǎn)彎半徑的泊車方法模擬單步垂直泊車過程如圖6(c)所示. 待泊車輛從目標(biāo)車位起點(diǎn)出發(fā)，到達(dá)終點(diǎn)位置時(shí)會與車庫墻體碰撞，且在泊車過程中會與車道邊界線發(fā)生重疊.

為實(shí)現(xiàn)在該泊車環(huán)境下的泊車入庫，利用Simulink/CarSim聯(lián)合仿真對所提雙步進(jìn)退垂直方法進(jìn)行分析. 首先在Simulink中搭建基于罰函數(shù)粒子群的路徑規(guī)劃模塊和基于模糊神經(jīng)網(wǎng)絡(luò)控制的路徑跟蹤控制模塊，并導(dǎo)入含有泊車場景和車輛模型的Carsim模塊. 汽車主要參數(shù)包括： 汽車長度CL為4.453 m; 汽車寬度CW為1.683 m； 最小轉(zhuǎn)彎半徑Rmin為4.2 m; 汽車軸距L為2.676 m; 汽車前懸長度LFS為0.721 m; 汽車后懸長度LRS為1.056 m. 雙步進(jìn)退垂直泊車規(guī)劃控制結(jié)果如圖7、 8所示，所分析可行起始點(diǎn)范圍如圖9所示.

圖7 垂直泊車路徑規(guī)劃仿真結(jié)果

圖8 垂直泊車路徑跟蹤仿真結(jié)果

圖9 可行起始點(diǎn)范圍

圖7(a)采用罰函數(shù)粒子群算法得到多步垂直泊車路徑, 引導(dǎo)車輛由起始位置泊到車位內(nèi)，且在泊車過程車輛外輪廓包絡(luò)未與車位和道路邊界發(fā)生相交. 由圖7(b)和圖7(c)中傳統(tǒng)直線-圓弧規(guī)劃方法與本方法所得到泊車路徑的姿態(tài)角和曲率對比可知： 雖然兩種方法得到的泊車路徑曲率為-0.238～0.238 m-1，滿足汽車轉(zhuǎn)向結(jié)構(gòu)約束條件，但采用直線-圓弧方法進(jìn)行泊車路徑規(guī)劃，在直線段與圓弧段的交匯處會出現(xiàn)曲率突變的情況，汽車需進(jìn)行較大幅度的原地轉(zhuǎn)向; 而采用本算法所得曲率連續(xù)且平滑, 在曲率極大值和極小值之間有過度曲線無突變點(diǎn)，能夠避免在路徑跟蹤過程中因曲率突變帶來的誤差. 由圖8所示，采用基于模糊神經(jīng)網(wǎng)絡(luò)方法的路徑跟蹤控制器，在行駛過程中使待泊汽車后軸中心點(diǎn)的行駛路徑航向角貼合所規(guī)劃路徑最終泊車停車位. 泊車過程中，最大位置跟蹤誤差小于0.08 m，最大位姿角跟蹤誤差小于3°，滿足路徑跟蹤精度. 從而驗(yàn)證所提雙步進(jìn)退垂直泊車路徑規(guī)劃和跟蹤策略的有效性.

5 結(jié)語

針對垂直泊車路徑規(guī)劃問題，提出一種基于多項(xiàng)式曲線的雙步進(jìn)退路徑規(guī)劃方法. 綜合考慮汽車位置約束、 結(jié)構(gòu)約束和道路邊界約束，以多項(xiàng)式為基函數(shù)、 泊車終點(diǎn)姿態(tài)角最小為目標(biāo)，通過求解多約束非線性函數(shù)模型獲得規(guī)劃路徑. 通過路徑規(guī)劃跟蹤的計(jì)算機(jī)仿真，結(jié)果表明, 所提的泊車路徑規(guī)劃策略能獲得曲率平滑連續(xù)的路徑，并有效規(guī)避碰撞，是一種有效的垂直泊車路徑規(guī)劃方法.