基于運(yùn)動(dòng)學(xué)模型的平行泊車軌跡規(guī)劃研究

雷 超

(華南理工大學(xué)機(jī)械與汽車工程學(xué)院,廣東 廣州 510640)

自動(dòng)泊車系統(tǒng)關(guān)鍵技術(shù)主要包括車輛定位、車位檢測(cè)、軌跡規(guī)劃和軌跡跟蹤。本文主要針對(duì)自動(dòng)平行泊車軌跡規(guī)劃進(jìn)行研究。文獻(xiàn)[1]采用圓弧-直線組合的方法設(shè)計(jì)了狹小空間的泊車路徑，該泊車路徑存在曲率突變，跟蹤過程中需要停車轉(zhuǎn)向；文獻(xiàn)[2]、[3]采用反正切函數(shù)設(shè)計(jì)泊車路徑，解決了原地轉(zhuǎn)向問題，但由于曲線變化不靈活，需要較大車位空間。文獻(xiàn)[4]為解決平行泊車兩段圓弧路徑曲率不連續(xù)的問題，在圓弧路徑兩端使用回旋曲線進(jìn)行過渡連接使得曲率連續(xù)變化；文獻(xiàn)[5]、[6]和[7]分別使用貝塞爾曲線、五次多項(xiàng)式曲線和sigmoid曲線設(shè)計(jì)了曲率平滑的泊車路徑，但這些曲線缺乏靈活性，為了滿足避碰要求需要較大車位空間；文獻(xiàn)[8]采用B樣條曲線并考慮避碰約束、轉(zhuǎn)向性能約束等設(shè)計(jì)了曲率連續(xù)的泊車路徑，但是曲率變化不夠平緩。

為此，本文提出了一種基于車輛運(yùn)動(dòng)學(xué)模型的泊車軌跡規(guī)劃方法，通過該方法規(guī)劃得到的泊車路徑優(yōu)點(diǎn)在于路徑曲率連續(xù)且變化平順。

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

建立一個(gè)合理有效的車輛模型是進(jìn)行軌跡規(guī)劃和軌跡跟蹤控制的前提，文獻(xiàn)[9]中使用的是一種經(jīng)典的車輛運(yùn)動(dòng)學(xué)模型，其可以很好地模擬車輛低速行駛的運(yùn)動(dòng)狀態(tài)，如式(1)所示：

(1)

式中：x，y為車輛后軸中點(diǎn)坐標(biāo)；v為車輛后軸中點(diǎn)速度；φ為車輛航向角；δ為等效前輪轉(zhuǎn)角；L為軸距。圖1為車輛運(yùn)動(dòng)學(xué)模型示意圖。將車輛簡(jiǎn)化為一個(gè)矩形，其中A、B、C、D分別為車輛輪廓的4個(gè)頂點(diǎn)，Lf,Lr和W分別為車輛前懸長(zhǎng)、后懸長(zhǎng)和車寬。

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

將式(1)以T為周期進(jìn)行離散化可得如下離散化模型，即第(k+1)個(gè)離散點(diǎn)參數(shù)可以由第k個(gè)離散點(diǎn)參數(shù)表示：

(2)

式中：a為車速變化率。

本文建立的平行泊車車庫(kù)模型如圖2所示，以車庫(kù)左下角頂點(diǎn)為坐標(biāo)原點(diǎn)建立坐標(biāo)系O-XY，其中WA，HP，WP分別為道路寬度、車位寬度、車位長(zhǎng)度，虛線與實(shí)線之間的距離為本文設(shè)定的安全距離ε。

圖2 平行泊車車庫(kù)模型

2 平行泊車軌跡規(guī)劃

泊車是一個(gè)從低約束區(qū)的車庫(kù)外駛?cè)敫呒s束區(qū)的車庫(kù)內(nèi)的運(yùn)動(dòng)過程，而車輛泊入車位與駛出車位是一個(gè)可逆的過程，因此本文首先規(guī)劃出一條從高約束區(qū)的車庫(kù)內(nèi)駛出至低約束區(qū)的車庫(kù)外的泊車軌跡(逆泊車軌跡)，再通過逆變換得到泊車軌跡。該方法相較于傳統(tǒng)的由車庫(kù)外至車庫(kù)內(nèi)的泊車軌跡規(guī)劃方法，能帶來(lái)極大的簡(jiǎn)化。

圖3 平行泊車軌跡規(guī)劃過程分析

對(duì)于車輛而言，當(dāng)給定速度和轉(zhuǎn)角輸入后，車輛會(huì)相應(yīng)產(chǎn)生位移，車輛在行駛狀態(tài)(車速不為零)中進(jìn)行轉(zhuǎn)向駛過的路徑是曲率連續(xù)的，而如果出現(xiàn)中途停車轉(zhuǎn)向，那么前后路徑的曲率將會(huì)發(fā)生突變。本文在進(jìn)行軌跡規(guī)劃時(shí)利用這一特點(diǎn)設(shè)計(jì)了曲率連續(xù)變化的軌跡。

圖4 前輪等效轉(zhuǎn)角

圖5 后軸中點(diǎn)速度

(3)

在式(3)中，未知變量為最大等效前輪轉(zhuǎn)角δ1、M2M3段對(duì)應(yīng)的時(shí)間t1和M4M5段對(duì)應(yīng)的時(shí)間t2。當(dāng)這3個(gè)未知變量確定時(shí)，圖4和圖5所示的轉(zhuǎn)角、速度與時(shí)間的變化關(guān)系就確定了。由式(2)、逆泊車軌跡起點(diǎn)M1坐標(biāo)和轉(zhuǎn)角與速度曲線可以遞推得到逆泊車軌跡所有點(diǎn)位置，即可得到如圖3中所示的M1→M2→…→M6的運(yùn)動(dòng)軌跡。

給定這3個(gè)未知參數(shù)一個(gè)取值范圍，當(dāng)δ1，t1和t2取不同值進(jìn)行組合時(shí)，就可以得到不同的轉(zhuǎn)角和速度曲線，如圖4和圖5所示，從而得到不同的泊車路徑。當(dāng)δ1,t1和t2取值范圍合適時(shí)，可以遍歷得到所有可行的泊車路徑?？紤]車輛本身的最大轉(zhuǎn)向角度限制，本文將δ1的取值范圍設(shè)定為0～0.6 rad。經(jīng)過MATLAB仿真實(shí)驗(yàn)得出：當(dāng)t1和t2的取值大于5 s時(shí)，得到的路徑必會(huì)導(dǎo)致車輛與車庫(kù)邊界發(fā)生碰撞，因此將t1和t2的取值范圍設(shè)定為0～5 s。以0.05 rad和0.5 s的間隔遍歷轉(zhuǎn)角δ1以及時(shí)間t1和t2的取值范圍即可得到所有可行的泊車軌跡曲線簇，如圖6所示，通過約束分析并建立代價(jià)函數(shù)即可求出一條最優(yōu)軌跡。

車輛在運(yùn)動(dòng)過程中，為避免與障礙物發(fā)生碰撞，參考圖4，需要滿足的避碰和轉(zhuǎn)向性能約束有：1)車輛左前頂點(diǎn)A(xA,yA)應(yīng)避免與道路邊界發(fā)生碰撞；2)車輛右前頂點(diǎn)B(xB,yB)應(yīng)避免與泊車位右邊界發(fā)生碰撞；3)車輛右后頂點(diǎn)C(xC,yC)應(yīng)避免與泊車位底部邊界相碰；4)前輪轉(zhuǎn)角值應(yīng)不超過車輪最大轉(zhuǎn)角值。此外，車輛還受到幾何約束，車輛任意一點(diǎn)坐標(biāo)可由后軸中點(diǎn)坐標(biāo)和車輛航向角通過幾何關(guān)系表示。綜合考慮避碰約束、轉(zhuǎn)向性能約束和幾何約束，以終點(diǎn)位置與車庫(kù)邊界的橫向偏差、轉(zhuǎn)角δ1(對(duì)應(yīng)軌跡平緩程度)和泊車時(shí)間t的加權(quán)和的最小化為優(yōu)化目標(biāo)，建立式(4)所示優(yōu)化目標(biāo)函數(shù)J(δ1，t1，t2)，通過求解該函數(shù)即可從圖6所示的可行泊車軌跡曲線簇中得到一條最優(yōu)的軌跡。

圖6 泊車軌跡曲線簇

minJ(δ1，t1，t2)=

(4)

式中：k1和k2為加權(quán)系數(shù)；δmax為車輛最大等效前輪轉(zhuǎn)角；yM6為泊車起始點(diǎn)后軸中點(diǎn)的縱坐標(biāo)。

3 仿真分析

表1 軌跡規(guī)劃仿真參數(shù)

在相同的仿真條件下將所設(shè)計(jì)的平行泊車路徑與目前常見的雙圓弧路徑和三次B樣條路徑進(jìn)行對(duì)比。圖7為3種路徑規(guī)劃方法得到的平行泊車路徑對(duì)比，圖8為3種路徑對(duì)應(yīng)的曲率變化對(duì)比，從圖中可以看出，雙圓弧路徑曲率發(fā)生了突變，需要在泊車過程中進(jìn)行停車轉(zhuǎn)向。三次B樣條路徑曲率連續(xù)但變化不平穩(wěn)，波動(dòng)較大，不利于跟蹤控制。而本文所設(shè)計(jì)的平行泊車路徑不僅曲率連續(xù)變化，并且變化平緩，曲率絕對(duì)值最大值為0.228 m-1。圖9為本文規(guī)劃的平行泊車軌跡對(duì)應(yīng)的速度和等效前輪轉(zhuǎn)角變化曲線，轉(zhuǎn)角最大值為0.592 rad。圖10為車輛按照本文所設(shè)計(jì)路徑運(yùn)動(dòng)時(shí)車輛輪廓包絡(luò)線，從圖中可以看出所設(shè)計(jì)的路徑是滿足避碰約束條件的，可實(shí)現(xiàn)車輛平順、安全的泊入車位。為驗(yàn)證本文規(guī)劃方法對(duì)不同大小車輛的適用性，選用某C級(jí)車型作為對(duì)照組，車輛尺寸如下：軸距2.75 m，前后懸長(zhǎng)分別為0.966 m和1.095 m，車寬1.86 m。仿真結(jié)果如圖11和圖12所示，對(duì)照組曲率絕對(duì)值最大值為0.217 m-1,等效前輪轉(zhuǎn)角最大值為0.596 rad。從仿真結(jié)果來(lái)看，本文規(guī)劃方法對(duì)不同大小車輛均適用。

圖7 泊車路徑

圖8 泊車路徑曲率

圖9 車速和等效前輪轉(zhuǎn)角

圖10 車輛運(yùn)動(dòng)包絡(luò)線

圖11 泊車路徑對(duì)比

圖12 曲率、車速和轉(zhuǎn)角對(duì)比

4 結(jié)束語(yǔ)

本文根據(jù)泊車經(jīng)驗(yàn)設(shè)計(jì)了平行泊車轉(zhuǎn)角和速度隨時(shí)間的變化歷程，通過遍歷所設(shè)計(jì)的轉(zhuǎn)角和速度曲線獲取了所有可行的泊車軌跡曲線簇，并通過優(yōu)化求解得到其中最優(yōu)的泊車軌跡。從仿真結(jié)果來(lái)看，該方法綜合了雙圓弧路徑和B樣條路徑的優(yōu)化，既有B樣條路徑曲率連續(xù)的優(yōu)點(diǎn)，又有雙圓弧路徑曲率簡(jiǎn)單無(wú)波動(dòng)的優(yōu)點(diǎn)。