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

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

0 引言

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

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

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

雙步進(jìn)退垂直泊車(chē)示意如圖1(a)所示. 雙步進(jìn)退垂直泊車(chē)工況下，車(chē)輛從起點(diǎn)T0到終點(diǎn)T4的泊車(chē)過(guò)程如圖1(b)所示.泊車(chē)過(guò)程可分為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)退垂直泊車(chē)

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

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

(1)

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

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

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

(2)

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

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

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

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

(3)

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

2.1.1車(chē)輛位置約束

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

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

(4)

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

汽車(chē)行駛路徑需經(jīng)過(guò)起始位置點(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汽車(chē)結(jié)構(gòu)幾何約束

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

(7)

式中:ρ為泊車(chē)路徑曲率.

2.1.3空間避障約束

為保證泊車(chē)過(guò)程中車(chē)輛不與周?chē)潭ㄕ系K物發(fā)生碰撞，車(chē)輛的路徑函數(shù)需滿足以下要求.

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

(8)

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

(9)

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

(10)

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

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

(11)

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

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

圖3 粒子群算法求解流程

3 垂直泊車(chē)路徑跟蹤控制

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

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

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

(12)

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

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

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

圖4 跟蹤控制框圖

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

4 仿真分析

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

圖6 泊車(chē)場(chǎng)景及單步垂直泊車(chē)工況

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

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

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

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

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

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

5 結(jié)語(yǔ)

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