車輛平行泊車系統(tǒng)路徑規(guī)劃與控制研究

劉 虎，楊 嘉，楊太旗，武 凱，李永超

（長(zhǎng)安大學(xué)工程機(jī)械學(xué)院，西安 710064）

0 引言

隨著汽車保有量持續(xù)增加，隨之而來(lái)的“泊車難”問(wèn)題也日益顯現(xiàn)，在停車規(guī)劃路徑時(shí)往往不考慮避障約束和曲率分析，造成路徑曲率不連續(xù)甚至發(fā)生碰撞事故。因此，分析和掌握避障路徑規(guī)劃和曲率優(yōu)化是解決自動(dòng)泊車系統(tǒng)的首要任務(wù)。

國(guó)內(nèi)外對(duì)平行泊車軌跡做了大量研究。Kim D等[1]設(shè)計(jì)了圓弧直線路徑，但路徑中存在曲率不連續(xù)點(diǎn)；王芳成[2]設(shè)計(jì)了四步驟平行泊車路徑，但需要多次停車轉(zhuǎn)向；張家旭等[3]采用回旋曲線設(shè)計(jì)泊車路徑，實(shí)現(xiàn)路徑曲率連續(xù)；張持等[4]和穆加彩等[5]用反正切函數(shù)擬合泊車路徑；姜立標(biāo)等[6]用B 樣條曲線規(guī)劃了泊車路徑，使得路徑曲率滿足要求，但對(duì)避障約束分析較少。

上述文獻(xiàn)雖然規(guī)劃了泊車路徑，但是沒(méi)有考慮到車輛避障約束和路徑曲率的優(yōu)化，因而在路徑安全性和曲率約束上存在一定問(wèn)題。

本文將從路徑規(guī)劃和曲率優(yōu)化兩個(gè)方面建立平行泊車路徑規(guī)劃體系?；谲囕v軌跡避障約束，分析了路徑中的碰撞約束，設(shè)置安全系數(shù)，保證車輛安全進(jìn)入泊車位。針對(duì)車輛路徑軌跡不連續(xù)的問(wèn)題，提出用五次多項(xiàng)式進(jìn)行路徑曲率優(yōu)化。最后，針對(duì)泊車路徑軌跡進(jìn)行分析，驗(yàn)證其準(zhǔn)確性和有效性。

1 泊車運(yùn)動(dòng)學(xué)模型及相關(guān)參數(shù)

車輛在泊車過(guò)程中處于低速，將車輛看成一個(gè)剛體，后軸無(wú)側(cè)向滑動(dòng)，將后軸中心點(diǎn)坐標(biāo)看做車輛的運(yùn)動(dòng)坐標(biāo)，建立車輛的運(yùn)動(dòng)學(xué)模型[7]。車輛后軸中心點(diǎn)的軌跡是由一段段圓弧組成，這是規(guī)劃泊車路徑的基礎(chǔ)。

本文以某款汽車為研究對(duì)象，其特性基本參數(shù)如表1所示。

表1 車輛參數(shù)表

2 泊車軌跡規(guī)劃及避障研究

本文采用兩圓弧相切法規(guī)劃泊車路徑，車輛在起始位置M2回正姿態(tài)，轉(zhuǎn)動(dòng)方向盤(pán)，以半徑R2運(yùn)行至M1后，停車左轉(zhuǎn)方向盤(pán)，以半徑R1運(yùn)行至M0點(diǎn)，最終順利停入泊車位[8]。整個(gè)泊車過(guò)程可以理解為以O(shè)1O2為圓心，半徑為R1R2的兩段相切圓弧組成的軌跡。以車位頂點(diǎn)A為坐標(biāo)原點(diǎn)建立坐標(biāo)系，車輛在泊車過(guò)程中路徑示意圖如圖1所示。

圖1 泊車兩圓弧相切軌跡示意圖

有幾何關(guān)系確定兩圓弧半徑和如下式所示：

式中：S0、H0分別為車輛起始點(diǎn)與目標(biāo)點(diǎn)之間X軸方向和Y軸方向上的距離。

當(dāng)確定了泊車起始點(diǎn)坐標(biāo)位置后，兩圓弧相切法確定的半徑R1和R2的和為定值。分析泊車過(guò)程中的碰撞約束，結(jié)合Rmin確定R1和R2的范圍，對(duì)R1和R2任意一個(gè)賦值，另外一個(gè)也隨之確定，從而泊車軌跡唯一確定。本文采用逆向方式分析泊車路徑，即將車輛泊車看做由M0駛出到達(dá)M2的過(guò)程，在此基礎(chǔ)上分析車輛運(yùn)動(dòng)過(guò)程中的避障約束，可以求得兩圓弧路徑的相關(guān)參數(shù)及泊車位寬度和長(zhǎng)度的極限值。

2.1 圓弧R1中的碰撞約束

圓弧R1中有2個(gè)碰撞約束，第一是車輛從泊車位M0起步時(shí)，右后點(diǎn)a與AB發(fā)生碰撞，此約束確定泊車位寬度最小值Wmin，碰撞位置如圖2所示；第二是駛出泊車位時(shí)，右前頂點(diǎn)b與頂點(diǎn)C發(fā)生碰撞，此約束確定位長(zhǎng)度的最小值Lmin和R1最大值R1max。碰撞位置如圖3所示。

圖2 第1種碰撞約束

圖3 第2種碰撞約束

當(dāng)車輛以最小轉(zhuǎn)彎半徑Rmin行駛時(shí)，此時(shí)滿足避障約束的泊車位寬度為極限最小值，由幾何關(guān)系推導(dǎo)得，右后頂點(diǎn)與車位下邊界不發(fā)生碰撞約束的條件如下式所示：

式中：Ra為車輛右后頂點(diǎn)轉(zhuǎn)彎半徑；Lw為車寬；W為泊車位寬度。

考慮車輛泊車入位時(shí)與邊界AB 的安全距離δ1，取δ1為0.2 m代入得Wmin=2.056 m。

對(duì)于第2類碰撞約束，當(dāng)車輛以最小轉(zhuǎn)彎半徑行駛時(shí)，此時(shí)滿足碰撞約束的泊車位長(zhǎng)度Lmin為極限最小值。設(shè)轉(zhuǎn)彎半徑圓心O1到車位上邊界CD的距離為L(zhǎng)ya，考慮安全距離δ2和δ3，取δ2和δ3均為0.2 m，泊車位長(zhǎng)度的最小值Lmin如下式所示：

式中：Rb為車輛右前點(diǎn)轉(zhuǎn)彎半徑；Lr為后懸。

由此計(jì)算得到的泊車位長(zhǎng)度為極限最小值，代入數(shù)據(jù)計(jì)算得Lmin=7.189 m。

根據(jù)實(shí)際泊車情況，在計(jì)算Wmin和Lmin的基礎(chǔ)上，適當(dāng)放大泊車位長(zhǎng)度，選取實(shí)際泊車位寬度為2.4 m，泊車位長(zhǎng)度為7.5 m。在計(jì)算實(shí)際泊車位長(zhǎng)度后，需要在滿足避障約束的前提下，計(jì)算圓弧1的半徑最大值R1max，圓弧半徑R1滿足下式：

式中：Lreal、Rbreal、Lyareal均為實(shí)際尺寸。

代入數(shù)據(jù)計(jì)算得圓弧1 的半徑最大值R1max為7.557 m。車輛在泊車過(guò)程中也受自身結(jié)構(gòu)的約束，轉(zhuǎn)彎半徑不能小于最小轉(zhuǎn)彎半徑Rmin，綜上，圓弧半徑R1的選取范圍為[R1min，R1max]。

2.2 圓弧R2中的碰撞約束

圓弧R2中也存在2種碰撞約束，第3種為車輛出庫(kù)后，后軸延長(zhǎng)線與車身交點(diǎn)p與前車位邊界點(diǎn)C發(fā)生碰撞，可以確定圓弧2的最小轉(zhuǎn)彎半徑R2min1，碰撞位置如圖4 所示。第4 類為車身左前點(diǎn)C 與道路邊界發(fā)生碰撞，可以確定車輛以圓弧2泊車時(shí)的最小轉(zhuǎn)彎半徑R2min2，碰撞位置如圖5所示。

圖4 第3類碰撞約束

圖5 第4類碰撞約束

對(duì)于第3類碰撞約束，由幾何關(guān)系得不發(fā)生碰撞的條件如下式所示：

式中：S 和L 分別為泊車起始點(diǎn)的橫縱坐標(biāo)；h 為起始位置車輛下邊界與車位邊界的距離。

本文取車輛泊車起始位置坐標(biāo)為（10.742,4.286），即S=10.742 m，h=1 m。代入公式計(jì)算得R2＞6.642 m，所以由第3類碰撞約束確定的R2min1為6.642 m。

對(duì)于第4類碰撞約束，道路邊界距離坐標(biāo)X軸的距離為H，車輛以圓弧2泊車時(shí)不與道路邊界發(fā)生碰撞的條件如下式所示：

式中：Rc為車輛左前點(diǎn)的轉(zhuǎn)彎半徑；H為道路邊界距離坐標(biāo)X軸的距離。

取δ4為0.2 m，道路邊界距離H 為6.2 m，代入上式計(jì)算得R2＞6.323 m。由第4 類碰撞約束確定的R2min2=6.323 m，圓弧2的半徑R2選取范圍如下式所示：

綜上所述，由以上4種避障約束分析，計(jì)算得到車輛的泊車位寬度和長(zhǎng)度，以及R1和R2的范圍。對(duì)R1和R2任意一個(gè)賦值，從而確定唯一的泊車軌跡。本文設(shè)定R1=7 m，R2=9.669 m，通過(guò)Matlab仿真得到起始點(diǎn)坐標(biāo)為（10.742,4.286）的兩圓弧泊車路徑如圖6所示。

圖6 兩圓弧軌跡仿真圖

3 五階多項(xiàng)式函數(shù)擬合路徑

由車輛運(yùn)動(dòng)過(guò)程分析可知，車輛在兩圓弧相切點(diǎn)存在停車轉(zhuǎn)向，該過(guò)程會(huì)使泊車過(guò)程不連續(xù)且加劇輪胎的磨損，在實(shí)際泊車過(guò)程中應(yīng)盡量避免這種情況[9]。本文選用五次多項(xiàng)式對(duì)平行泊車路徑進(jìn)行優(yōu)化。五次多項(xiàng)式的基本表達(dá)式如下：

式中：a5、a4、a3、a2、a1、a0分別為多項(xiàng)式的6個(gè)參數(shù)。6 個(gè)未知參數(shù)至少需要6 個(gè)方程來(lái)求解。五次多項(xiàng)式經(jīng)過(guò)目標(biāo)點(diǎn)（x0，y0），且在該點(diǎn)處一階二階導(dǎo)數(shù)均為0，過(guò)切點(diǎn)（x1，y1），過(guò)起始點(diǎn)（x2，y2），且在該點(diǎn)處斜率為0。由此得到的6個(gè)方程可以用來(lái)求解參數(shù)。

經(jīng)過(guò)仿真計(jì)算得，泊車軌跡如圖7所示，路徑曲率如圖8所示。從圖中可以看出，五階多項(xiàng)式較好擬合了兩圓弧相切軌跡，且滿足在起始點(diǎn)和終止點(diǎn)處車輛姿態(tài)的要求，可以進(jìn)行平穩(wěn)安全的泊車操作。

圖7 五次多項(xiàng)式擬合圖

圖8 五次多項(xiàng)式路徑曲率圖

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

本文采用兩圓弧軌跡規(guī)劃了平行泊車路徑，通過(guò)分析車輛泊車過(guò)程中存在的碰撞約束，確定了泊車位和圓弧軌跡的相關(guān)參數(shù)。針對(duì)圓弧軌跡中曲率突變的問(wèn)題，本文提出用五次多項(xiàng)式擬合路徑的方法，改善了路徑中存在的曲率突變。

最后根據(jù)仿真結(jié)果表明，五次多項(xiàng)式較好地?cái)M合了圓弧軌跡，在起點(diǎn)和終點(diǎn)處曲率均為0，表明車輛泊車車身姿態(tài)角擺正?；緷M足了車輛泊車要求，安全平穩(wěn)從起始位置泊車入位。而在起始位置車身姿態(tài)角傾斜方面，可以開(kāi)展傾斜初始位姿的平行泊車路徑規(guī)劃研究。