微小初始角度的多段式泊車路徑規(guī)劃 *

畢 錕， 范 英

(太原科技大學(xué) 交通與物流學(xué)院， 山西 太原 030024)

0 引 言

隨著我國汽車保有量不斷增加， 停車空間越來越小， 增加了泊車過程中交通事故的發(fā)生幾率， 也對駕駛員的操作水平提出了更高的要求.

自動泊車是增加駕駛員舒適性和安全性的解決方案. 自動泊車系統(tǒng)的一個關(guān)鍵部分是泊車路徑規(guī)劃， 對此， 國內(nèi)外很多學(xué)者進(jìn)行了大量的研究. Su B等[1]提出了一種基于端點(diǎn)區(qū)域化的二次平行自動停車方法,建立了避障約束條件的泊車路徑函數(shù)， 并用遺傳算法以泊車路徑函數(shù)為目標(biāo)函數(shù)進(jìn)行了優(yōu)化， 但其估算最小車位會使結(jié)果造成一些誤差； Vorobieva H等[2]提出了一種使用回旋曲線將圓弧路徑轉(zhuǎn)換為連續(xù)曲率路徑的微小車位乘用車自動平行泊車系統(tǒng)； Wang L X等[3]提出了一種使用兩個不同半徑切向圓弧的從任意初始角度自動平行泊車的算法， 該方法可以從任意初始角度實現(xiàn)泊車， 但泊車起始區(qū)域過大， 不適合實際運(yùn)用； Ge Y R等[4]提出了一種從任意起始位姿經(jīng)過中間過渡位姿到達(dá)目標(biāo)位姿的自動平行泊車運(yùn)動規(guī)劃算法； 陳無畏等[5]規(guī)劃了一種雙向的3段弧泊車路徑， 并用遺傳算法對其進(jìn)行了優(yōu)化； 胡文等[6]基于熟練駕駛員泊車經(jīng)驗， 設(shè)計了一種新的自動泊車規(guī)劃算法， 但需要較多駕駛員泊車經(jīng)驗樣本； 錢立軍等[7]提出了泊車過程中方向盤勻速轉(zhuǎn)動的策略， 對比分析五段式路徑和兩段式路徑在泊車過程中的車速、 轉(zhuǎn)角、 油門控制量、 剎車控制量隨時間的變化關(guān)系， 證明五段式泊車過程更加連貫， 但未考慮方向盤轉(zhuǎn)角隨時間變化等因素； 王啟明[8]和胡勤明等[9]為了解決泊車軌跡的曲率突變問題， 提出了多段式泊車路徑， 并對其進(jìn)行了優(yōu)化處理； 郭夜啼[10]提出了一種多段式曲線擬合的平行泊車路徑規(guī)劃方法， 采用不同的起始點(diǎn)進(jìn)行泊車仿真， 但沒有考慮車輛存在一定初始角度時的泊車問題.

以上文獻(xiàn)都是在平行初始狀態(tài)下規(guī)劃的泊車路徑， 沒有考慮到在實際泊車環(huán)境中待泊車車輛總會與目標(biāo)車位存在初始泊車角度的問題. 為了解決車輛初始泊車狀態(tài)與車位存在一定角度而導(dǎo)致泊車失敗的問題， 提出了一種多段式平行泊車路徑規(guī)劃方法， 以實現(xiàn)汽車與車位存在一定初始角度時， 仍能保證其不與周邊環(huán)境發(fā)生碰撞且泊車入位， 并通過MATLAB仿真驗證了該方法的可行性.

1 車輛低速運(yùn)動學(xué)模型

在泊車過程中， 由于車速較低， 可忽略車輪的滑移和側(cè)偏. 假定車輪運(yùn)動狀態(tài)均為滾動， 根據(jù)阿克曼轉(zhuǎn)向原理， 構(gòu)建車輛運(yùn)動學(xué)模型[11]， 如圖 1 所示.

在參考坐標(biāo)系中， 車輛繞旋轉(zhuǎn)中心O以R為半徑轉(zhuǎn)動，l為軸距，δ為車輪中心點(diǎn)等效轉(zhuǎn)角，θ為車身轉(zhuǎn)向角，vf為前輪中心點(diǎn)速度，vr為后輪中心點(diǎn)速度， 汽車前軸中心點(diǎn)坐標(biāo)為(xf,yf)， 后軸中心點(diǎn)坐標(biāo)為(xr,yr).

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

由于忽略了車輛輪胎的滑移現(xiàn)象， 后輪運(yùn)動方向與車輛垂直的方向速度為0， 所以有

(1)

根據(jù)幾何關(guān)系推導(dǎo)后軸中心點(diǎn)和前軸中心點(diǎn)關(guān)系得

(2)

前后軸中心速度滿足關(guān)系

(3)

取車輛后軸中心點(diǎn)速度v為整車速度， 可以得到以后軸中心為參考點(diǎn)的運(yùn)動學(xué)模型[12]

(4)

2 碰撞分析

為了保證車輛能夠安全地停進(jìn)車庫， 在規(guī)劃泊車路徑之前， 需要對車輛和泊車環(huán)境進(jìn)行防碰撞分析， 這也是實現(xiàn)無碰撞泊車的關(guān)鍵[13]. 為了便于避障分析， 將車輛簡化成一個長方形， 如圖 2 所示. 其中， 點(diǎn)A，B，C，D分別是車輛的4個頂點(diǎn)在地面的投影點(diǎn)； 點(diǎn)a，b，c，d分別為車位在地面的投影點(diǎn)； 點(diǎn)O1為第一段圓弧軌跡的轉(zhuǎn)動中心； 點(diǎn)O2為第二段圓弧軌跡的轉(zhuǎn)動中心； 點(diǎn)P，Q分別為后軸中心點(diǎn)和前軸中心點(diǎn)； 點(diǎn)E為后軸的延長線與車輛右側(cè)邊界的交點(diǎn)在地面的投影點(diǎn)； 點(diǎn)F為車輛前軸延長線與車輛右側(cè)邊界的交點(diǎn)在地面的投影點(diǎn)； 點(diǎn)M和點(diǎn)N分別為車輛后軸和前軸延長線與車輛左側(cè)邊界的交點(diǎn)在地面的投影點(diǎn).根據(jù)駕駛員泊車經(jīng)驗可知， 車輛在平行泊車過程中， 最容易發(fā)生碰撞的是車輛的點(diǎn)B，C，D和E.

圖 2 微小初始角度平行泊車碰撞分析示意圖

2.1 車輛第一段曲線運(yùn)動軌跡碰撞分析

2.1.1E點(diǎn)碰撞分析

車輛方向盤向右打泊車過程中， 車輛以點(diǎn)O1為圓心，R1為半徑轉(zhuǎn)動， 車輛右側(cè)邊界最容易和車位右側(cè)發(fā)生碰撞.將點(diǎn)A，B，C，D，E與圓心O1連接起來， 根據(jù)幾何關(guān)系， 可以發(fā)現(xiàn)連線O1E最短， 說明車輛右側(cè)最容易發(fā)生碰撞的點(diǎn)是點(diǎn)E.以車位左側(cè)最上方的邊界點(diǎn)a(xa,ya)為原點(diǎn)， 建立X-Y平面直角坐標(biāo)系， 以后軸中心點(diǎn)P為參考， 分析點(diǎn)E與車位右側(cè)的碰撞過程.E1為車輛與車位的碰撞臨界點(diǎn)， 將輪胎整個運(yùn)動過程視作純滾動運(yùn)動， 根據(jù)阿克曼轉(zhuǎn)向原理， 轉(zhuǎn)動中心點(diǎn)O1在后軸延長線上， 所以有

(5)

假設(shè)車輛右側(cè)點(diǎn)E與車位右側(cè)邊界臨界碰撞時， 后軸中心點(diǎn)P坐標(biāo)為(XP,YP)， 車輛的航向角為θ.

第一段曲線轉(zhuǎn)動中心與車輛后軸中心的坐標(biāo)關(guān)系為

(6)

根據(jù)幾何關(guān)系可以知道兩條直角邊的長度分別為

(7)

CE=L-Lr.

(8)

車輛不發(fā)生碰撞的條件為

(9)

根據(jù)以上公式可以得出不發(fā)生碰撞的條件為

(R1-0.5W)2≥(XP+R1sinθ-Ll)2+

(YP-R1cosθ)2,

(10)

式中：Ll為目標(biāo)車位的長；W為車寬.

2.1.2B點(diǎn)碰撞分析

第一段圓弧軌跡泊車過程中， 車輛的B點(diǎn)容易與目標(biāo)車位右側(cè)車位上邊界發(fā)生碰撞， 為了避免發(fā)生碰撞， 需要滿足約束條件

O1B≥O1d.

(11)

根據(jù)幾何關(guān)系可得

(12)

將式(7), 式(12)代入式(11)中可知需要滿足

(YP-R1cosθ)2.

(13)

2.1.3D點(diǎn)碰撞分析

為了防止車頭與車位對面道路邊界障礙物或者道路另一側(cè)行駛的車輛發(fā)生碰撞， 需要就此建立車輛約束條件， 避免車輛越過對面道路邊界， 保證車輛泊車安全. 在平行泊車過程中， 車輛左側(cè)最容易與對面道路邊界發(fā)生碰撞的點(diǎn)是點(diǎn)D.D1為車輛與道路邊界的碰撞臨界點(diǎn)，H為車位對面道路的寬度， 不發(fā)生碰撞的條件為

O1D+YO1≤H.

(14)

根據(jù)幾何關(guān)系可得

O1D2=O1M2+MD2,

(15)

O1M=R1+0.5W,

(16)

MD=L-Lr.

(17)

將式(13)， 式(14)代入式(12)中可以得到

(18)

將式(18)和點(diǎn)O1的縱坐標(biāo)代入式(14)可以得出，D點(diǎn)不發(fā)生碰撞的條件為

(19)

2.2 車輛第二段曲線運(yùn)動軌跡碰撞分析

2.2.1C點(diǎn)碰撞分析

車輛方向盤向右打的狀態(tài)下， 泊車到達(dá)合適位置后， 向左轉(zhuǎn)動方向盤， 由此執(zhí)行整個泊車過程的第二段曲線運(yùn)動. 車輛以點(diǎn)O2為圓心，R2為半徑轉(zhuǎn)動， 車輛在這段運(yùn)動過程中， 最容易發(fā)生碰撞的是車輛右側(cè)邊界的兩個頂點(diǎn)B和C.在泊車入位過程中， 汽車車身右側(cè)頂點(diǎn)C容易與車位右頂點(diǎn)d發(fā)生碰撞，C1為車輛與車位的碰撞臨界點(diǎn)，Ll為目標(biāo)車位的長，Lw為車位寬，Lf為前懸，Lr為后懸， 第二段曲線轉(zhuǎn)動中心與車輛后軸中心的坐標(biāo)關(guān)系為

(20)

防止發(fā)生碰撞的約束條件是

O2C≤O2d.

(21)

因為

O2C1=O2d，

(22)

根據(jù)以上各點(diǎn)坐標(biāo)， 可以得到無碰撞約束條件為

(23)

2.2.2B點(diǎn)碰撞分析

在平行泊車最后階段， 車輛繞轉(zhuǎn)動中心O2轉(zhuǎn)動， 車輛尾部頂點(diǎn)B容易與車位的底部邊界發(fā)生碰撞， 根據(jù)幾何關(guān)系可以得出

O2B2=O2E2+BE2,

(24)

O2E=R2+0.5W.

(25)

將式(25)代入到式(24)中得

(26)

滿足車尾B點(diǎn)不與車位底部發(fā)生碰撞的條件為

O2B≤YO2+Lw.

(27)

可以得到， 需要滿足

(28)

2.3 最小安全車位

為了防止車輛在停車過程中與停車位周圍環(huán)境發(fā)生碰撞和保證車輛順利泊進(jìn)車位， 不但要求泊車入位過程的路徑規(guī)劃水準(zhǔn)， 還要求停車位足夠大， 能容許車輛停進(jìn)去. 顯然， 停車位的尺寸必須大于車輛的尺寸. 但是在實際泊車過程中， 必須根據(jù)車輛的性能和避碰約束條件來具體分析車輛能夠順利泊車入位的最小停車位大小.

根據(jù)以上的車輛碰撞分析， 停車位與車輛和碰撞點(diǎn)之間的相對位置如圖 3 所示， 車輛的右前頂點(diǎn)C可能在C1點(diǎn)與車位發(fā)生碰撞， 車輛的右后頂點(diǎn)B可能在點(diǎn)B1與車位發(fā)生碰撞， 為了確保泊車安全， 設(shè)定車輛輪廓與車位邊界之間的安全距離分別為Δl1， Δl2， Δd1和Δd2. 可以得出停車位的長和寬分別為

(29)

圖 3 最小安全車位

3 泊車起始區(qū)域劃分

自動泊車的起始區(qū)域也是整個泊車過程的關(guān)鍵內(nèi)容之一， 起始區(qū)域的好壞很大程度上決定著泊車的成功與否. 為了順利實現(xiàn)泊車過程的第一步， 提高泊車成功率， 本文根據(jù)倒車入庫過程， 車輛右側(cè)邊緣BC和車輛左前頂點(diǎn)D的碰撞分析， 對泊車的起始區(qū)域進(jìn)行了劃分.

通過多段式泊車路徑進(jìn)行可行泊車起始區(qū)域分析， 當(dāng)車輛在第二段直線行駛時， 根據(jù)車輛結(jié)構(gòu)參數(shù)和泊車路徑可知第二段直線終點(diǎn)P3的坐標(biāo)為

(30)

直線P2P3的表達(dá)式為

y=tanθ(x-xP3)+yP3，

(31)

則車輛右側(cè)頂點(diǎn)B的運(yùn)動軌跡方程為

(32)

為了避免車輛右側(cè)邊緣與車位頂點(diǎn)發(fā)生碰撞， 需要滿足點(diǎn)d與直線BC的距離不小于0， 即

(33)

車輛在沿圓弧P1P2行駛時， 左前頂點(diǎn)D不能越過車位對面的道路邊界， 即要滿足

RD+yO1≤H。

(34)

4 平行車位泊車路徑規(guī)劃

如圖 4 所示， 開始進(jìn)行五段式平行泊車，P0點(diǎn)為車輛后軸中心的泊車初始位置點(diǎn)， 車輛的初始航向角為θ， 泊車時車輪為回正狀態(tài).

圖 4 車輛后軸中點(diǎn)軌跡示意圖

針對泊車過程車輛的初始狀態(tài)可能與車位存在夾角導(dǎo)致泊車失敗的問題， 本文分別對 3種不同的車輛初始狀態(tài)進(jìn)行路徑規(guī)劃. 視逆時針方向為夾角的正方向，P0～P1為第一段泊車路徑，P1～P2為第二段泊車路徑，P2～P3為第三段泊車路徑，P3～P4為第四段泊車路徑，P4～P5為第五段泊車路徑.

根據(jù)第2節(jié)的幾何分析， 以路徑相關(guān)參數(shù)l1，l2，l3，R1，R2，θ為變量， 泊車入位為目標(biāo)， 建立多約束非線性軌跡方程， 其中避障約束為

(35)

本文以最小安全車位為目標(biāo)車位， 其尺寸為 6.4 m×2 m， 車位對面道路寬度H設(shè)置為 5.5 m， 依據(jù)以上的可行起始泊車區(qū)域分析， 選取泊車的起始點(diǎn)坐標(biāo)， 根據(jù)多約束非線性軌跡方程編寫MATLAB程序， 求解路徑相關(guān)參數(shù)l1,l2,l3,R1,R2,θ的值.由圖 4 可知， 目標(biāo)車輛從P0點(diǎn)開始泊車， 第一段路徑為直線， 方向盤保持回正狀態(tài)向后倒車， 行駛至P1點(diǎn)后， 將方向盤向右打， 以R1為半徑向后倒車， 至車輛后軸中點(diǎn)到達(dá)點(diǎn)P2后， 將方向盤向左打至回正狀態(tài)， 繼續(xù)向后倒車， 行駛至P3點(diǎn)， 然后將方向盤向左打， 以R2為半徑倒車， 當(dāng)車輛后軸中心點(diǎn)到達(dá)P4點(diǎn)后回正方向盤繼續(xù)倒車， 到達(dá)最終泊車位置時， 停止泊車.

5 仿真驗證

為了驗證該泊車路徑規(guī)劃算法的可行性， 以某SUV車型為例， 建立避障約束條件， 采用解非線性優(yōu)化問題方法求解各段泊車路徑， 在MATLAB上實現(xiàn)了自動平行泊車路徑規(guī)劃算法的編程， 該SUV車的相關(guān)參數(shù)如表1 所示.

表1 某SUV泊車相關(guān)參數(shù)

在MATLAB上搭建泊車仿真環(huán)境， 其中車位長為6.4 m， 寬為2 m， 泊車通道寬為5.5 m， 仿真車輛的最大泊車速度vm為1m/s， 根據(jù)車輛初始泊車狀態(tài)與車位可能存在的角度進(jìn)行仿真驗證， 車輛初始偏角為-30°， 0°， 30°時的仿真結(jié)果分別如圖 5 所示.

由圖 5 可以看出， 車輛在3種不同初始角度的泊車過程中， 都沒有與周圍泊車環(huán)境發(fā)生碰撞， 且成功實現(xiàn)泊車.

(a) 初始角度為-30°

(b) 初始角度為0°

(c) 初始角度為30°

6 結(jié) 論

考慮到車輛初始泊車狀態(tài)與停車位可能存在一定角度， 結(jié)合實際的泊車過程， 分析泊車過程中可能產(chǎn)生的碰撞問題， 確定了最小安全車位尺寸， 劃分了可行泊車區(qū)域， 規(guī)劃出了一種由圓弧直線組成的五段式平行泊車路徑， 該路徑可以從實際可能存在的任意初始角度進(jìn)行平行泊車. 仿真結(jié)果表明， 該算法能夠從任意初始角度成功生成車輛的平行停車路徑， 并能準(zhǔn)確地泊車入位.