基于最短路徑的變半徑泊車路徑規(guī)劃研究

王龍，朱凱

（1.重慶交通大學(xué)機(jī)電與汽車學(xué)院，重慶 404100；2.中國農(nóng)業(yè)大學(xué)工學(xué)院，北京 100083）

基于最短路徑的變半徑泊車路徑規(guī)劃研究

王龍1，朱凱2

（1.重慶交通大學(xué)機(jī)電與汽車學(xué)院，重慶 404100；2.中國農(nóng)業(yè)大學(xué)工學(xué)院，北京 100083）

自動泊車路徑的規(guī)劃屬于從低約束到高約束的問題，也屬于非完整約束問題。一般的路徑規(guī)劃總是不盡如人意，對泊車條件要求高。根據(jù)一般問題的解決從高約束到低約束的過程，在最短路徑的基礎(chǔ)上，先確定高約束階段泊車車輛進(jìn)入泊車位的路徑，計(jì)算出停車位長度與直線斜率的擬合公式，得到直線的斜率，再改變泊車半徑，確定低約束段的泊車路徑。利用車輛運(yùn)動學(xué)模型，對車輛位置和姿態(tài)的分析，碰撞點(diǎn)到車輛的距離計(jì)算，證明了路徑規(guī)劃的可行性，生成的泊車軌跡比較符合泊車實(shí)際情況。

最短路徑；變半徑；路徑規(guī)劃

王龍

現(xiàn)就讀于重慶交通大學(xué)車輛工程專業(yè)碩士，目前在清華大學(xué)汽車研究所實(shí)習(xí)，主要研究方向是自動泊車. 曾獲得一項(xiàng)實(shí)用新型專利發(fā)明以及發(fā)表了一篇論文。

引言

自動泊車輔助系統(tǒng)可以在泊車時自主進(jìn)行轉(zhuǎn)向運(yùn)動，駕駛員僅需控制油門、制動以及掛擋，并且該系統(tǒng)可以隨時接管方向盤，停止系統(tǒng)作用。自動泊車輔助系統(tǒng)減輕了駕駛員的操作，給駕駛員泊車帶來了第二種選擇，近年來不斷有外國的車型增添了這一功能。國內(nèi)也不斷有這方面的研究。

泊車的路徑規(guī)劃是自動泊車系統(tǒng)的重要一部分。當(dāng)前的自動泊車規(guī)劃主要是兩圓弧相切型和兩圓弧加一條斜直線型，且圓弧半徑是泊車車輛的最小轉(zhuǎn)彎半徑。文獻(xiàn)［1，2］中提出了基于最小轉(zhuǎn)彎半徑的兩圓弧加直線的最短路徑，并進(jìn)行了最短路徑的證明。文獻(xiàn)［3］提出了基于多段圓弧和直線的平行泊車路徑規(guī)劃方法，依然沒有擺脫最小轉(zhuǎn)彎半徑作為第二段的圓弧半徑。文獻(xiàn)［4］提出了改進(jìn)的最小半徑和不等半徑兩種幾何路徑規(guī)劃方法，并對這兩種方法進(jìn)行了綜合分析，得出兩者的結(jié)合不僅可以大大擴(kuò)大泊車起始區(qū)域的范圍，但是沒有提出具體的算法來規(guī)劃路徑。文獻(xiàn)［5，6，7，8］分別利用貝爾樣條曲線、回旋曲線、人機(jī)交互的模糊控制曲線來規(guī)劃泊車路線，這些方法計(jì)算復(fù)雜且對系統(tǒng)的控制性要求高，不容易工程實(shí)現(xiàn)。本文通過對最短路徑的研究，提出一個基于最短路徑的變半徑泊車路徑，該路徑規(guī)劃易于理論推導(dǎo)且比較符合車輛實(shí)際泊車軌跡。

1　車輛泊車時運(yùn)動學(xué)分析

首先對車輛進(jìn)行簡化，得到基本的尺寸模型，如圖1為車輛的簡化尺寸。并利用簡化模型建立車輛泊車時的運(yùn)動學(xué)模型。

從文獻(xiàn)［1-7］中可以得到在低速情況下，泊車車體可以作為一個剛體，在不考慮側(cè)偏的條件下，車輛的運(yùn)動過程是由多段的圓弧組成。由于后輪與車身運(yùn)動方向一致，后輪軌跡在車體運(yùn)動過程中能夠完全體現(xiàn)車體的運(yùn)動軌跡，所以可以將后輪軸線中心坐標(biāo)認(rèn)為是車體運(yùn)動坐標(biāo)?？梢杂梅较虮P轉(zhuǎn)角確定車輛的轉(zhuǎn)彎半徑，僅僅需要知道后輪軸線中心坐標(biāo)的位置和車身偏角，就可以確定車輛的運(yùn)動軌跡了。以此來建立模型。如圖2所示，即為車輛運(yùn)動學(xué)模型。

由圖2所示，泊車車輛的位置可以用3個狀態(tài)變量表示出來。這三個變量是后軸中心的坐標(biāo)（x ，y）車輛正向時和水平方向的夾角θ，即車輛的航向角。v表示車輛前軸中心點(diǎn)的速度，表示車輛前輪φ

方向與車輛方向的夾角，l 表示泊車車輛的軸長。通過對車輛運(yùn)動學(xué)的分析，可以列出車輛泊車時的運(yùn)動方程：

2　基于最短路徑的泊車路徑規(guī)劃

2.1最短路徑分析

根據(jù)泊車路徑要求可知，路徑需要由導(dǎo)數(shù)連續(xù)的曲線構(gòu)成，而在文獻(xiàn)［1］中說明了由以車輛最小轉(zhuǎn)彎半徑為半徑的圓弧和直線組成的路徑為泊車的最短路徑。若路徑為圓和直線組成，則根據(jù)車輛的運(yùn)動學(xué)的限制可知，路徑的連接應(yīng)由初始和終點(diǎn)的兩段圓弧及與兩段圓弧相切的直線組成，如圖3所示。

如圖3所示，車輛倒車過程可以描述為：車輛先以某一固定轉(zhuǎn)彎半徑向右側(cè)轉(zhuǎn)彎倒車，到達(dá)F1后，將方向盤回正，沿直線倒車到點(diǎn)F2后，將方向盤左打到某一固定角度后，逐漸將車身回正，當(dāng)車身回正到與道路平行，且車輛M點(diǎn)到達(dá)理想停車點(diǎn)M'后，將方向盤回到原點(diǎn)即可完成此次倒車。

假設(shè)車輛到達(dá)理想泊車點(diǎn)時，以泊車車輛的后軸中心點(diǎn)建立坐標(biāo)軸，如圖3所示。車輛泊車起始點(diǎn)M到理想泊車點(diǎn)M'的水平距離為S。泊車車輛以圓心O1，半徑R1轉(zhuǎn)動一個角度θ所行駛的距離為S1，泊車車輛從F1到F2行駛的距離為L1，泊車車輛以圓心O2，半徑R2轉(zhuǎn)動一個角度θ所行駛的距離為S2，泊車從起點(diǎn)M到泊車到達(dá)理想停車點(diǎn)M'走過的總路程S是=SS0+。L從+S圖3-1中的幾何關(guān)系可以得到：

R1+R2和 S 成正比，隨著 S 的增大而增大。

對R1+R2與 θ的關(guān)系進(jìn)行分析，假設(shè)

對 f（ θ）進(jìn)行求導(dǎo)，得到：

得到

將公式（2-4）帶入得到

對 S0（θ）進(jìn)行求導(dǎo)，得到S0'（θ）＞0，即隨著轉(zhuǎn)彎角度θ的增大，總路程不斷增大。

根據(jù)隨著轉(zhuǎn)動角度θ的增大，兩圓弧半徑和R1+R2也不斷增大的一一對應(yīng)正比關(guān)系，得到隨著兩圓弧半徑和R1+R2不斷增大，轉(zhuǎn)動角度θ也不斷增大，總路程S0不斷增大。當(dāng)泊車起始時以最小轉(zhuǎn)彎半徑Rmin倒車和到達(dá)理想泊車點(diǎn)前一段圓弧也是以最小轉(zhuǎn)彎半徑Rmin進(jìn) 行轉(zhuǎn)彎的，此時 R1+R2最小且等于2Rmin，轉(zhuǎn)動角度θ最小，總路程S0也最小。隨著 R1+R2的不斷增大，總路程S0也不斷增大。故當(dāng)車輛在起點(diǎn)與到達(dá)理想點(diǎn)前一段以最小轉(zhuǎn)彎半徑Rmin轉(zhuǎn)彎時，泊車的總路程最短。

2.2高約束階段路徑分析與規(guī)劃

用最小轉(zhuǎn)彎半徑進(jìn)行的規(guī)劃，對最小轉(zhuǎn)彎半徑要求比較高，即必須以最小轉(zhuǎn)彎半徑進(jìn)行泊車，對車輛輪胎磨損嚴(yán)重。對泊車環(huán)境適應(yīng)性差，實(shí)驗(yàn)過程中不成功的泊車次數(shù)比較多。對于這些問題，通過改變圓弧的半徑克服這些缺點(diǎn)。

如圖4所示，以泊車車輛的后軸中心點(diǎn)O作為參考點(diǎn)，泊車車輛在第二次轉(zhuǎn)彎時車輛的避碰點(diǎn)為車輛前端的邊緣點(diǎn)Q。此時車輛在轉(zhuǎn)彎的過程中，車輛整體作為一個剛體，泊車車輛的前邊緣點(diǎn)也在做著圓弧軌跡，此時車輛以后橋中心為基點(diǎn)做半徑為R2的圓弧運(yùn)動，泊車車輛的前邊緣點(diǎn)Q做半徑為R3的圓弧運(yùn)動。LS為后軸中心點(diǎn)O到泊車車輛前段邊緣點(diǎn)Q的直線距離。Lq為泊車車輛后軸中心到泊車車輛前端的直線距離。WL為泊車車輛的寬度。此時點(diǎn)O與點(diǎn)Q與點(diǎn)O2構(gòu)成一個固定的三角形。其中θ3為兩條直線LS與R2的夾角。Lh為泊車車輛后軸中心點(diǎn)O到泊車車輛后端的距離。D為泊車行使過程中的安全距離。Z為泊車過程中的障礙點(diǎn)?？梢郧蟮茫?/p>

根據(jù)三角形的余弦公式得到：

當(dāng)測量到車位長度大于一個值P1時，車輛只需要一次泊車。

為了保證泊車過程的平順性和舒適性，選用一個合適的轉(zhuǎn)彎半徑R2，要求R2大于最小半徑且滿足多次泊車時的最大轉(zhuǎn)彎半徑。通過計(jì)算可以得到泊車結(jié)束時的位置，再根據(jù)轉(zhuǎn)彎半徑R2得到第二段圓弧的軌跡。在一次泊車的條件下，可以將第二段圓弧路徑固化下來，轉(zhuǎn)彎角度θ 不變化，這時可以確定第二段圓弧的起點(diǎn)F2。

2.3轉(zhuǎn)動角度的分析

轉(zhuǎn)動角度的取值應(yīng)滿足在H、S 的范圍內(nèi)，可以進(jìn)行一次泊車，不發(fā)生碰撞。從實(shí)際泊車環(huán)境出發(fā)，轉(zhuǎn)動角度的取值應(yīng)盡量小，保證規(guī)劃的路線盡可能短，同時轉(zhuǎn)彎平順。上邊給出了轉(zhuǎn)彎角度的大致范圍，在最小取值內(nèi)小中求大，即尋求最小取值范圍內(nèi)的最大值。假設(shè)第一段轉(zhuǎn)彎半徑R1大于等于R2，那么在轉(zhuǎn)彎角度的范圍內(nèi)最小范圍內(nèi)只需要H最大，S最小，且滿足規(guī)劃路徑的其他條件。利用公式2-4得到：

即轉(zhuǎn)彎角度可以用下列式子求出：

假設(shè)直線段的表達(dá)式為y=a1x+b1。根據(jù)得到的 F2起點(diǎn)坐標(biāo)和a1可以求得b1最終得到直線的表達(dá)式?？梢杂?jì)算直線段到障礙原點(diǎn)的距離 。其應(yīng)滿足以下條件：

2.4低約束階段的分析與規(guī)劃

當(dāng)?shù)诙无D(zhuǎn)彎圓弧確定后，泊車規(guī)劃路線的直線段與第二段圓弧的相切點(diǎn)F2可以確定，且直線的傾斜程度（即斜率）可以通過公式（2-17）求得。

半徑R2的圓弧與直線L1相切，圓心到直線的距離等于半徑 。即：

得到：

最大轉(zhuǎn)彎半徑需要滿足2個條件。

1. 泊車車輛后端在第一段圓弧泊車過程中不會撞住障礙點(diǎn)Z且保持一個安全距離D。

2. 最大轉(zhuǎn)彎半徑要與直線L1相切

用公式表示就是：

鑒于初始點(diǎn)的最大值Smax不容易求取，可以采用先求取滿足直線段長度L1時的初始值S ，看是否符合最大轉(zhuǎn)彎半徑需要滿足的初始點(diǎn)最大值Smax，若滿足就選用此時的初始值S作為初始點(diǎn)的最大值；若未滿足，可用此時的初始值S不斷逼近初始點(diǎn)的最大值Smax，具體采用兩點(diǎn)中值法，利用Smin作為下線，不斷求取中值進(jìn)行條件比較，若符合條件，繼續(xù)進(jìn)行中值比較，一直到找到最優(yōu)解為止。

實(shí)際上只要起始點(diǎn)的位置大于Smin一個安全值即可，但是考慮到泊車車輛的舒適性和穩(wěn)定性需要找到一個更合適的起始點(diǎn)。經(jīng)過綜合考慮將起點(diǎn)設(shè)為Smin與Smax的中點(diǎn)Smid。公式為：

這樣做的好處是在保證舒適的泊車時，行駛的路線盡可能短。也可以增加駕駛舒適度的權(quán)重，將起始點(diǎn)設(shè)置在Smid與Smax之 間。但是沒有一個具體的衡量標(biāo)準(zhǔn)，沒法進(jìn)行完全的量化，所以需要根據(jù)實(shí)驗(yàn)才可以更好的選擇合適的起始點(diǎn)。

2.5多次泊車路線分析與規(guī)劃

當(dāng)測量的泊車位長度大于一次泊車的長度P1，需要進(jìn)行多次泊車，如圖6所示，對于多次泊車可以通過車輛較小的轉(zhuǎn)彎半徑不斷走Z字型，最終走到Fn， 且此時車輛的航向角為零。通過MATLAB實(shí)驗(yàn)仿真獲得泊車時到達(dá)檢測停車位時候的點(diǎn)F3的坐標(biāo)位置和半徑R2。

假設(shè)在多次泊車時第一段圓弧的半徑等于最大的第二段圓弧半徑，通過公式2-15和2-17確定第二段圓弧的轉(zhuǎn)彎角度θ，并得到第二段圓弧的圓心位置，最終規(guī)劃出高約束階段的二段泊車路徑規(guī)劃，然后進(jìn)行低約束階段的規(guī)劃，最終獲得多次泊車的路徑規(guī)劃。

3　實(shí)驗(yàn)仿真

3.1多次泊車的仿真數(shù)據(jù)

實(shí)驗(yàn)采用大眾途安型號的車輛參數(shù)進(jìn)行仿真，參數(shù)如下：車長4 395 mm，車寬為1 794 mm，前懸為880 mm，后懸為837 mm，車輛輪距為1 530 mm，軸距為2 678 mm，車輛行駛速度為km/h，最小轉(zhuǎn)彎半徑為5 600 mm，方向盤極限轉(zhuǎn)角為540°。

通過MATLAB仿真可以得到多次泊車時入庫時的半徑R2與停車位長度Pl 的關(guān)系，如圖7所示。仿真得到點(diǎn)F3的坐標(biāo)位置，如圖8所示：

3.2一次泊車路線及避碰區(qū)域仿真分析

H 的取值范圍采用停車場的規(guī)范，H 的取值范圍是（200，300），S的取值范圍為測到停車位長度時通過障礙物后的直線距離（大概為130 cm）與泊車位長度之和，故一次泊車時S 的取值范圍為（826，1 500）。根據(jù)公式（2-17）得到a1的值。

通過計(jì)算可以得到一次泊車時，R2=560 cm ，a1=0.5946 ，θ =30.2° ，D=20 cm 。在新坐標(biāo)下，第二段圓弧的圓心O2=（-592.4， -470） ，第二段圓弧起點(diǎn)F2=（-319.2， 19.3），直線段的方程式為：

第一段圓弧需要的最小起始點(diǎn)位置與側(cè)向距離有關(guān)，側(cè)向距離越大，需要的最小起始點(diǎn)位置越小，當(dāng)側(cè)向距離H0=300 cm時，此時最小起始點(diǎn)位置為（140， 210），最大起始點(diǎn)位置為（450.54，210），此時，直線L1=100 cm，為最小距離。

通過計(jì)算的試驗(yàn)參數(shù)對其進(jìn)行避障分析，得到如圖9、10的黑色避障區(qū)域，主要由泊車規(guī)劃運(yùn)行第一個圓弧時的泊車后端運(yùn)動的區(qū)域和運(yùn)行第二個圓弧時泊車前端運(yùn)動的區(qū)域組成。通過實(shí)驗(yàn)分析，發(fā)現(xiàn)可以很好的避開障礙物點(diǎn)Z且保持一個安全距離。

2.5多次泊車路線仿真及直線斜率計(jì)算

對于多次泊車規(guī)劃的計(jì)算分析，在停車位長度大于656 cm時，如圖11所示，通過擬合得到半徑R與停車位長度Pl的關(guān)系式 ：

通過計(jì)算分析，并用MATLAB仿真得到此時半徑R與直線斜率a 的關(guān)系，如圖12所示。停車位長度Pl與斜率a 的關(guān)系，如圖13所示：

通過檢測到的停車位長度，可以通過公式3-3獲得此時需要的斜率a 。

當(dāng)停車位長度pl ≤656 cm時，為了保證車輛順利泊車，此時S 的最小值定為786（656+130）cm。此時泊車車輛半徑不斷變化，取值范圍R2=（460，480）。在此范圍內(nèi)半徑R2與直線斜率a的關(guān)系如圖14所示。半徑R2與直線斜率a的關(guān)系公式：

通過計(jì)算和仿真分析，整個泊車過程不會發(fā)生碰撞。

4　結(jié)論

基于最短路徑的泊車路徑規(guī)劃保證了在泊車路徑較短的前提下，泊車需要的空間減小，泊車的舒適性增加，避免了泊車?yán)米钚∞D(zhuǎn)彎半徑進(jìn)行泊車，汽車盡量做純滾動運(yùn)動。

通過實(shí)驗(yàn)仿真給出了多次泊車時停車位長度與直線斜率的擬合公式和第二段圓弧的半徑與直線斜率的擬合公式，對其進(jìn)行避碰計(jì)算，結(jié)果完全符合泊車的實(shí)際需要，證明了其方法的可行性。

［1］ Scheuer A， Fraichard T H， Planning Continuouscurvature Paths for Carlike Robots［C］， IEEE Transactions on Intelligent Robots and Systems， 1996：1304-1311.

［2］呂鎮(zhèn)基.自動泊車軌跡規(guī)劃算法研究及視覺平臺結(jié)構(gòu)設(shè)計(jì)［D］.哈爾濱工業(yè)大學(xué)，2012.

［3］楊妮娜，梁華為，王少平.平行泊車的路徑規(guī)劃方法及其仿真研究［J］.電子測量技術(shù)，2011，01：42-45.

［4］王道斌，梁華為，楊妮娜，高航.兩種自主泊車路徑規(guī)劃方法的對比研究［J］.電子測量技術(shù)，2011，01：27-30+38.

［5］林蓁蓁，李慶，梁艷菊，陳大鵬.一種基于路徑規(guī)劃的自動平行泊車算法［J］.計(jì)算機(jī)應(yīng)用研究，2012，05：1713-1715.

［6］宋金澤.自主泊車系統(tǒng)關(guān)鍵技術(shù)研究［D］.國防科學(xué)技術(shù)大學(xué)，2009.

［7］劉鈺，馬艷麗，李濤.基于Bezier曲線擬合的自主平行泊車軌跡模型仿真［J］. 科技導(dǎo)報(bào)，2011，11：59-61.

［8］黃迅.基于回旋曲線的自動平行泊車系統(tǒng)研究［D］.導(dǎo)師：曹立波.湖南大學(xué)，2014.

專家推薦

田哲文：

文章結(jié)構(gòu)較為完整、有一定新意、研究的科學(xué)性好，有較強(qiáng)的實(shí)用性。

Variable Radius of Parking Path Planning based on Shortest Path

WANG Long1， ZHU Kai2
（1.College of mechanical and electrical engineering， Chongqing Jiaotong University . Chongqing404100，China； 2. College of engineering， China Agricultural University.Beijing 100083， China）

The automatic parking path planning belongs to from low to high constraints problems，also bound by a non-complete problem. Generally the path planning is not very desirable， for parking demanding conditions. According to resolve common problems from high to low constraint process，based on the shortest path， Firstly determine the parking path of parking vehicles enter the parking space in the highly constrained stage， calculate the length of parking spaces and the slope of the line fit formula to give a straight line slope， and then change parking radius of parking path in lower bound segment. Using the vehicle kinematic model， analysis of vehicle position and posture， calculate the distance of collision point to the vehicle to prove the feasibility of path planning， the generated parking trajectory more in line with the actual situation parking.

Shortest path； variable radius； path planning

N945.13

A

1005-2550（2015）06-0051-07

10.3969/j.issn.1005-2550.2015.05.010

2015-06-24