多約束雙向自動(dòng)平行泊車軌跡規(guī)劃

李 紅 郭孔輝，2 宋曉琳

1.湖南大學(xué)汽車車身先進(jìn)設(shè)計(jì)制造國家重點(diǎn)實(shí)驗(yàn)室，長沙，410082 2.吉林大學(xué)汽車仿真與控制國家重點(diǎn)實(shí)驗(yàn)室，長春，130022

0 引言

隨著汽車數(shù)量的逐年增加，車輛運(yùn)行空間越來越小，狹小的泊車空間及有限的視野范圍給泊車增加了難度，導(dǎo)致泊車過程中事故發(fā)生率不斷提高。自動(dòng)泊車系統(tǒng)為近年正在研發(fā)的新技術(shù)，該技術(shù)借助監(jiān)視測距系統(tǒng)，控制車輛自動(dòng)安全進(jìn)入目標(biāo)車位中，減輕了駕駛員泊車操作負(fù)擔(dān)，降低了泊車事故發(fā)生，國內(nèi)外各大汽車公司及專家針對泊車策略做了大量研究工作：Paromtchik等［1］采用回歸算法，通過正弦曲線尋找安全的泊車路線，但其泊車空間要求較大；Ryu等［2］、Nejad等［3］、Lee等［4］、Chang等［5］分別基于模糊邏輯控制對泊車系統(tǒng)進(jìn)行了研究；劉鈺等［6］采用Bezier曲線對泊車軌跡進(jìn)行了擬合；宋金澤等［7］應(yīng)用微分平坦理論進(jìn)行了軌跡規(guī)劃。泊車策略研究可分為兩種：①經(jīng)驗(yàn)法，即利用模糊控制、神經(jīng)網(wǎng)絡(luò)等算法模擬駕駛員操縱行為，控制汽車進(jìn)入車位；②路徑規(guī)劃法，即根據(jù)汽車碰撞約束等條件規(guī)劃車輛泊車路徑，控制車輛跟蹤預(yù)定路徑進(jìn)入目標(biāo)車位。

1 自動(dòng)泊車系統(tǒng)工作流程

自動(dòng)泊車系統(tǒng)工作流程如圖1所示：自動(dòng)泊車系統(tǒng)開啟后，車輛沿道路緩慢行駛，通過傳感器感知泊車環(huán)境信息，檢測停車空間及環(huán)境障礙，若停車空間過小不能滿足要求，則繼續(xù)行駛；若找到滿足最小車位要求的車位，建立泊車軌跡坐標(biāo)系，根據(jù)環(huán)境障礙信息及車輛參數(shù)信息規(guī)劃泊車路徑，若規(guī)劃成功，則提示駕駛員是否采取自動(dòng)泊車，若不成功則繼續(xù)行駛尋找下一個(gè)車位；泊車路徑規(guī)劃成功后，若駕駛員采用自動(dòng)泊車，泊車系統(tǒng)根據(jù)傳感器采集到的車速及環(huán)境障礙信息通過控制方向盤完成泊車，若駕駛員不采用自動(dòng)泊車，則自動(dòng)泊車系統(tǒng)關(guān)閉。另外，自動(dòng)泊車系統(tǒng)還須保證駕駛員可隨時(shí)中斷自動(dòng)泊車系統(tǒng)并接管車輛以保證泊車過程的安全性。

圖1 自動(dòng)泊車系統(tǒng)工作流程

2 泊車軌跡規(guī)劃

自動(dòng)平行泊車軌跡規(guī)劃問題可描述如下：車輛的起點(diǎn)與終點(diǎn)之間找到一條曲率不大于1／Rmin（Rmin為車輛最小轉(zhuǎn)彎半徑）的平滑曲線，當(dāng)車輛沿該軌跡運(yùn)動(dòng)時(shí)不與周圍物體發(fā)生碰撞并完全進(jìn)入泊車車位中。泊車方式可根據(jù)車位方向分為平行泊車、垂直泊車和斜向泊車，根據(jù)泊車復(fù)雜程度可分為后退式泊車和多次進(jìn)退式泊車，當(dāng)泊車空間相對狹小后退式泊車方式不能使車輛安全進(jìn)入車位時(shí)，需采用多次進(jìn)退式泊車方法使車輛安全進(jìn)入車位，因此對進(jìn)退式平行泊車方式進(jìn)行研究。

2.1 平移式雙向泊車軌跡規(guī)劃

平行式雙向泊車軌跡采用平行移庫模式，使車輛通過兩次后退一次前進(jìn)平行移入車位中，如圖2所示，車輛掛入倒擋，方向盤右轉(zhuǎn)，車輛沿圓弧FG行駛，然后方向盤左轉(zhuǎn)，車輛沿圓弧GH行駛，車身與車位平行時(shí)停車，完成第一次平移，然后掛入前進(jìn)擋，方向盤右轉(zhuǎn)，車輛沿圓弧HL行駛，然后方向盤左轉(zhuǎn)，車輛沿圓弧LM行駛，車身與車位平行時(shí)停車，完成第二次平移，最后掛入倒擋以調(diào)整車輛與前、后車位車輛的距離。

圖2 平移式雙向泊車示意圖

當(dāng)后軸為非轉(zhuǎn)向軸時(shí)，由后軸中點(diǎn)軌跡及軌跡各點(diǎn)切線可確定車輛車身方位角，由車身結(jié)構(gòu)參數(shù)可得到車身各點(diǎn)軌跡曲線，故選擇車輛后軸中點(diǎn)軌跡為研究目標(biāo)，圖2中，O1、O2、O3、O4分別為圓弧FG、GH、HL、LM圓心。AoBoCoDo分別為車輛終點(diǎn)位置，A1B1C1D1為車位。

記E為后軸中點(diǎn)，A、B、C、D分別為車輛輪廓右后端、右前端、左前端、左后端，F(xiàn)為車輛后軸中點(diǎn)軌跡泊車起始點(diǎn)。已知車輛長La，寬W，軸距為Lo，前懸架長Lf，后懸架長Lr，車輛最小轉(zhuǎn)彎半徑Rmin，泊車初始位置時(shí)后軸中點(diǎn)E分別與車位線A1D1縱向距離so，與車位線C1D1側(cè)向距離ho，車位寬度A1D1及道路寬度為h，r1、r2、r3、r4，θ1、θ2、θ3、θ4分別為圓弧FG、GH、HL、LM的半徑和圓心角，Lc和Lk分別為車位長和寬，由圖2可知：θ1＝θ2，θ3＝θ4。選擇r1、r2、r3、r4、θ1、θ3為軌跡曲線變量，D1點(diǎn)為坐標(biāo)原點(diǎn)，由幾何關(guān)系知：

（1）F點(diǎn)坐標(biāo)為（so，ho）；

（2）O1點(diǎn)坐標(biāo)（x1，y1）＝（so，ho－r1）；

（3）O2點(diǎn) 坐 標(biāo) （x2，y2）＝ （x1－ （r1＋r2）sinθ1，y1＋ （r1＋r2）cosθ1）；

（4）O3點(diǎn)坐標(biāo)（x3，y3）＝（x2，y2－r2－r3）；

（5）O4點(diǎn) 坐 標(biāo) （x4，y4）＝ （x3＋ （r3＋r4）sinθ3，y3＋ （r3＋r4）cosθ3）；

（6）C1點(diǎn)坐標(biāo)（C1x，C1y）＝（Lc，0）；

（7）B1點(diǎn)坐標(biāo)（B1x，B1y）＝（Lc，－Lk）；

（8）H點(diǎn)坐標(biāo)（Hx，Hy）＝（x2，y2－r2）；

（9）M點(diǎn)坐標(biāo)（Mx，My）＝（x4，y4－r4）；

泊車過程中，車輛與周圍障礙物碰撞情況如下：①車輛左前端與左側(cè)道路邊界或?qū)γ嫘旭傑囕v碰撞；②車輛右側(cè)與前車位碰撞；③ 完成第一次平移時(shí)車輛后端與后方障礙物碰撞；④第二次平移時(shí)車輛前端與右側(cè)障礙物碰撞；⑤完成第二次平移時(shí)車輛前端與前方障礙物碰撞；⑥泊車結(jié)束時(shí)車輛未完全進(jìn)入車位，如圖3所示。

圖3 平移式雙向泊車碰撞示意圖

為保證車輛無碰撞地完全進(jìn)入車位，車輛軌跡需滿足以下要求。

（1）當(dāng)車輛沿圓弧FG行駛時(shí)，由圖3a可知，要求C點(diǎn)軌跡半徑O1C滿足：

（2）由圖3b可知，車輛A點(diǎn)軌跡半徑O1A及沿圓弧GH行駛時(shí)車輛B點(diǎn)軌跡半徑O2B需滿足：

（3）當(dāng)車輛到達(dá)H點(diǎn)時(shí)，由圖3c知，車輛后端D點(diǎn)x向坐標(biāo)Dx需滿足：

（4）當(dāng)車輛到達(dá)點(diǎn)L時(shí)，由圖3d知，車輛在點(diǎn)L處B點(diǎn)y向坐標(biāo)需滿足：

（5）當(dāng)車輛沿圓弧LM行駛，到達(dá)M點(diǎn)時(shí)，由圖3e、圖3f知，需滿足：

（6）車輛最小轉(zhuǎn)彎半徑約束要求：r1≥Rmin，r2≥Rmin，r3≥Rmin，r4≥Rmin。

由上述分析建立軌跡約束函數(shù)：

當(dāng)求解車輛可實(shí)現(xiàn)泊車的最小車位長度時(shí)，車輛起始位置已知，車位尺寸未知，則以r1、r2、r3、r4、θ1、θ3、Lk為變量，以Lc最小化為目標(biāo)，利用MATLAB的非線性優(yōu)化功能求解式（8）；當(dāng)進(jìn)行泊車軌跡規(guī)劃時(shí)，車輛起始位置及車位尺寸已知，為減小泊車過程中轉(zhuǎn)向的繁忙程度，以方向盤轉(zhuǎn)角最小化為目標(biāo)，以r1、r2、r3、r4最小值的最大化為目標(biāo)，求解式（8）。

2.2 自由式雙向泊車軌跡規(guī)劃

自由式雙向泊車軌跡如圖4所示，車輛掛入倒擋，方向盤右轉(zhuǎn)，車輛沿圓弧FG行駛，然后方向盤左轉(zhuǎn)，沿圓弧GH行駛，車身與車位具有一定夾角時(shí)掛入前進(jìn)擋，右轉(zhuǎn)方向盤沿圓弧HL行駛，車身與車位平行時(shí)停車，最后再掛入倒擋調(diào)整車輛與前后車位車輛的距離。

選擇車輛后軸中點(diǎn)軌跡為研究目標(biāo)，由后軸中點(diǎn)軌跡切線得到車身方位角，結(jié)合車輛結(jié)構(gòu)參數(shù)可得到車輛各端點(diǎn)軌跡，如圖4所示，O1、O2、O3分別為圓弧FG、GH、HL的圓心，AoBoCoDo分別為車輛泊車終點(diǎn)，A1B1C1D1為車位。

圖4 自由式雙向泊車示意圖

記E為后軸中點(diǎn)，A、B、C、D分別為車輛輪廓右后端、右前端、左前端、左后端，F(xiàn)為車輛后軸中點(diǎn)軌跡泊車起始點(diǎn)。已知車輛軸距Lo，前懸架長Lf，后懸架長Lr，車寬W，車輛最小轉(zhuǎn)彎半徑Rmin，泊車初始位置時(shí)后軸中點(diǎn)分別與車位線A1D1縱向距離so，車位線C1D1側(cè)向距離ho，車位寬度A1D1，道路寬度為h，r1、r2、r3、θ1、θ2、θ3分別為圓弧FG、GH、HL半徑與圓心角，Lc和Lk分別為車位長與寬，由圖4知θ1＝θ2＋θ3。選擇r1、r2、r3、θ1、θ3為變量，選擇D1點(diǎn)為坐標(biāo)原點(diǎn)，由圖4幾何關(guān)系可得：

（1）F點(diǎn)坐標(biāo)為（so，ho）；

（2）O1點(diǎn)坐標(biāo)（x1，y1）＝（so，ho－r1）；

（3）O2點(diǎn) 坐 標(biāo) （x2，y2）＝ （x1－ （r1＋r2）sinθ1，y1＋ （r1＋r2）cosθ1）；

（4）O3點(diǎn) 坐 標(biāo) （x3，y3）＝ （x2＋ （r2＋r3）sinθ3，y2－ （r2＋r3）cosθ3）；

（5）C1點(diǎn)坐標(biāo)（C1x，C1y）＝（Lc，0）；

（6）B1點(diǎn)坐標(biāo)（B1x，B1y）＝（Lc，－Lk）；

泊車過程中，車輛與周圍障礙物碰撞情況如下：①車輛左前端與左側(cè)道路邊界或?qū)γ嫘旭傑囕v碰撞；②車輛右側(cè)與前車位碰撞；③ 車輛左后端與后方障礙物碰撞；④車輛右后點(diǎn)與后側(cè)障礙物碰撞；⑤車輛前端與前方障礙物碰撞；⑥車輛最終未能完全進(jìn)入車位。

為避免車輛與周圍障礙物發(fā)生碰撞，且能完全進(jìn)入車位，車輛軌跡需滿足以下要求。

（1）當(dāng)車輛沿圓弧FG行駛時(shí)，由圖5a可知，車輛C點(diǎn)軌跡半徑O1C需滿足：

（2）當(dāng)車輛沿圓弧FG和GH行駛時(shí)，由圖5b可知，沿圓弧FG行駛時(shí)車輛A點(diǎn)軌跡半徑O1A和沿圓弧GH行駛時(shí)車輛B點(diǎn)軌跡半徑O2B需滿足：

（3）當(dāng)車輛沿圓弧GH行駛到達(dá)H點(diǎn)時(shí)，由圖5c、圖5d知，D點(diǎn)x向坐標(biāo)及A點(diǎn)y向坐標(biāo)需滿足：

車輛沿圓弧GH行駛時(shí)A點(diǎn)與D點(diǎn)軌跡半徑O2A和O2D的表達(dá)式為

（4）當(dāng)車輛沿圓弧HL行駛，由圖5e、圖5f知，L點(diǎn)處B點(diǎn)x向坐標(biāo)Bx、L點(diǎn)y向坐標(biāo)Ly需滿足：

圖5 自由式雙向泊車碰撞示意圖

（5）車輛最小轉(zhuǎn)彎半徑約束要求：r1≥Rmin，r2≥Rmin，r3≥Rmin。

由上述分析建立軌跡約束函數(shù)：

求解車輛所需最小車位長度，車輛起始位置已知，車位尺寸未知，則以r1、r2、r3、θ1、θ3、Lk為變量，以Lc最小化為目標(biāo)，求解非線性約束函數(shù)式（16）；進(jìn)行泊車軌跡規(guī)劃時(shí)，車輛起始位置及車位尺寸已知，為減小泊車過程中轉(zhuǎn)向的繁忙程度，以方向盤轉(zhuǎn)角最小化為目標(biāo)，即以r1、r2、r3最小值的最大化為目標(biāo)，求解式（16）。

3 仿真實(shí)例及分析

車輛參數(shù)如下：軸距Lo＝2.5m，前懸架長Lf＝0.9m，后懸架長Lr＝0.8m，車寬W＝1.6m，最小轉(zhuǎn)彎半徑Rmin＝4.7m。

有限的停車場所空間隨著車輛數(shù)量的增加顯得逐漸擁擠，若泊車過程中車輛所需車位較小，不但可提高停車場所的空間利用率，還可提高車輛泊車過程中的安全性，因此本文對平移式平行泊車和自由式平行泊車兩種泊車方式所需最小車位進(jìn)行了研究，參考文獻(xiàn)［8］選擇道路寬度為4m，車輛處于起始位置時(shí)，so＝8m，ho＝2m，根據(jù)上述分析對兩種泊車方式所需最小車位進(jìn)行求解。由求解結(jié)果可知平移式雙向泊車方法可實(shí)現(xiàn)的最小車位長5.86m寬2.10m，自由式雙向泊車方法可實(shí)現(xiàn)的最小車位長5.28m寬1.93m，兩種方法各自最小車位泊車軌跡參數(shù)見表1，泊車軌跡見圖6、圖7。

當(dāng)其他參數(shù)不變，車位尺寸固定，車位長6.2m、寬2.5m時(shí)，由前述軌跡規(guī)則方法求解泊車軌跡參數(shù)，求解結(jié)果見表2，泊車軌跡見圖8、圖9。

表1 最小車位軌跡參數(shù)

圖6 平移式雙向最小車位平行泊車軌跡

圖7 自由式雙向最小車位平行泊車軌跡

表2 軌跡規(guī)劃軌跡參數(shù)

圖8 車位尺寸固定時(shí)平移式雙向平行泊泊車軌跡

由圖6～圖9可知，在相同泊車環(huán)境中，車輛沿著兩條軌跡經(jīng)過一退一進(jìn)進(jìn)入車位，且與周圍障礙物無交點(diǎn)，表明了兩種方法均能在滿足車輛最小轉(zhuǎn)彎半徑約束條件下使車輛安全進(jìn)入車位；對比圖6與圖7知，與自由式雙向平行泊車方法相比，平移式雙向平行泊車方法所需車位空間大，平移式泊車方法所需最小泊車車位長5.86m寬2.10m，自由式泊車方法所需最小車位長5.28m寬1.93m；由表2可知，相同泊車環(huán)境及車位尺寸時(shí)，自由式泊車方法所需方向盤轉(zhuǎn)角較小，因此可減小泊車系統(tǒng)轉(zhuǎn)向電機(jī)的工作負(fù)荷。綜上可知，自由式泊車方法更具安全性與實(shí)用性。另外，兩種軌跡規(guī)劃方法僅規(guī)劃了車輛一次后退與一次前進(jìn)的軌跡，在實(shí)際泊車過程中，當(dāng)車輛完成一退一進(jìn)過程后，還需一次前進(jìn)或后退來調(diào)節(jié)車輛與前后車位的距離，該操作相對簡單，因此未進(jìn)行研究。

圖9 車位尺寸固定時(shí)自由式雙向平行泊車軌跡

4 結(jié)論

（1）泊車空間過小不能滿足車輛最小轉(zhuǎn)彎半徑約束及環(huán)境避障約束無法實(shí)現(xiàn)后退式平行泊車時(shí)，需采用進(jìn)退式泊車即雙向平行泊車。分析泊車軌跡特點(diǎn)，提出兩種雙向平行泊車軌跡：平移式雙向平行泊車軌跡與自由式雙向平行泊車軌跡。確定兩種泊車軌跡規(guī)劃的變量、規(guī)劃目標(biāo)及碰撞約束函數(shù)。

（2）利用MATLAB軟件非線性約束函數(shù)優(yōu)化功能求解泊車軌跡方程，并對兩種泊車軌跡進(jìn)行仿真。結(jié)果表明相同泊車環(huán)境下，兩種軌跡規(guī)劃方法均可實(shí)現(xiàn)小車位安全泊車，通過比較可知，自由式雙向平行泊車軌跡規(guī)劃法在滿足車輛參數(shù)約束及碰撞約束的條件下能實(shí)現(xiàn)相對更小的泊車車位；對同一泊車環(huán)境進(jìn)行軌跡規(guī)劃，自由式雙向平行泊車方法能更輕便地完成泊車。

（3）考慮傳感器測量精度及車輛系統(tǒng)的響應(yīng)滯后特性，對實(shí)車進(jìn)行軌跡規(guī)劃時(shí)，應(yīng)給車輛尺寸增加合適的安全余量后對車輛進(jìn)行泊車軌跡規(guī)劃，車輛在軌跡圓弧節(jié)點(diǎn)處受方向盤轉(zhuǎn)向電機(jī)轉(zhuǎn)速及泊車車速的影響，需適當(dāng)增加車位尺寸以提高泊車系統(tǒng)的泊車安全性。另外，車輛沿軌跡行駛完全進(jìn)入車位后，還需根據(jù)前后雷達(dá)自動(dòng)調(diào)整車輛與前后障礙距離。由于傳感器測量精度及車輛動(dòng)態(tài)特性，在泊車過程中車輛位置估算存在一定誤差，軌跡規(guī)劃研究結(jié)束后還需對車輛方位估算進(jìn)行研究，為泊車系統(tǒng)路徑跟蹤控制研究作準(zhǔn)備。

［1］Paromtchik I E，Laugier C.Autonomous Parallel Parking and Returning to Traffic Maneuvers［C］／／Proceedings of the 1997IEEE／RSJ International Conference on Intelligent Robot and Systems.Grenoble，F(xiàn)rance，1997，3：21－23.

［2］Ryu Y W，Oh S Y，Kim S Y.Robust Automatic Parking without Odometry Using an Evolutionary Fuzzy Logic Controller［J］.International Journal of Control，Automation and Systems，2008，6（3）：434－443.

［3］Nejad M K.Fuzzy Logic Based Autonomous Parallel Parking of a Car－like Mobile Robot［D］.Montreal：Concordia University，2006.

［4］Lee J Y，Lee J J.Multiple Designs of Fuzzy Control for Car Parking Using Evolutionary Algorithm［C］／／Proceedings of International Conference on Mechatronics.Kumamoto，Japan，2007：1－6.

［5］Chang S J，Li T H S.Design and Implementation of Fuzzy Parallel－parking Control for a Car－type Mobile Robot［J］.Journal of Intelligent and Robotic Systems，2004，34（2）：175－194.

［6］劉鈺，馬艷麗，李濤.基于Bezier曲線擬合的自主平行泊車軌跡模型仿真［J］.科技導(dǎo)報(bào)，2011，29（11）：59－61.Liu Yu，Ma Yanli，Li Tao.Parallel Parking Path Generation Based on Bezier Curve Fitting［J］.Science ＆ Technology Review，2011，29（11）：59－61.

［7］宋金澤，戴斌，單恩忠，等.融合動(dòng)力學(xué)約束的自主平行泊車軌跡生成方法［J］.中南大學(xué)學(xué)報(bào)（自然科學(xué)版），2009，40（1）：135－141.Song Jinze，Dai Bin，Shan Enzhong，et al.Trajectory Planning Approach of Autonomous Parallel Parking with Kinodynamic Constraints［J］.Journal of Central South University（Science and Technoloty），2009，40（1）：135－141.

［8］北京市市政設(shè)計(jì)研究院.CJJ37－90，城市道路設(shè)計(jì)規(guī)范［S］.北京：中國建筑工業(yè)出版社，1991.