多約束平行泊車路徑規(guī)劃

侯曉陽(yáng) 黃勇 孫思 季學(xué)武

（清華大學(xué) 汽車安全與節(jié)能國(guó)家重點(diǎn)實(shí)驗(yàn)室）

多約束平行泊車路徑規(guī)劃

侯曉陽(yáng) 黃勇 孫思 季學(xué)武

（清華大學(xué) 汽車安全與節(jié)能國(guó)家重點(diǎn)實(shí)驗(yàn)室）

針對(duì)不能滿足平行泊車要求的狹小車位，提出了單向多次和雙向多次路徑規(guī)劃方法。在確定了兩種泊車軌跡的變量、碰撞約束函數(shù)以及規(guī)劃目標(biāo)后，利用matlab軟件的非線性約束函數(shù)的優(yōu)化功能求解泊車軌跡方程，并對(duì)兩種路徑規(guī)劃方法進(jìn)行仿真試驗(yàn)，結(jié)果表明，在相同的泊車環(huán)境下，兩種路徑規(guī)劃方法都可實(shí)現(xiàn)安全順利的泊車入位；通過(guò)實(shí)車試驗(yàn)進(jìn)一步驗(yàn)證了所提出的路徑規(guī)劃方法的安全性和有效性。

1 前言

隨著汽車保有量的增加，隨之帶來(lái)的問(wèn)題也日益凸顯。其中，最為突出的就是停車難問(wèn)題，而自動(dòng)泊車系統(tǒng)可以很好地解決這一問(wèn)題，因此其已成為汽車行業(yè)的研究重點(diǎn)。平行泊車?yán)碚撗芯康年P(guān)鍵部分是路徑規(guī)劃階段，對(duì)此國(guó)內(nèi)外學(xué)者已經(jīng)進(jìn)行了大量研究，研究方法多采用兩圓弧相切或圓弧切直線的路徑規(guī)劃方法，并且路徑規(guī)劃的結(jié)果需要滿足環(huán)境約束[1～5]。雖然這些路徑規(guī)劃方法可滿足平行泊車的基本需要，但是由于實(shí)際泊車過(guò)程中傳感器測(cè)量的誤差以及機(jī)械機(jī)構(gòu)操作誤差等，會(huì)導(dǎo)致車輛無(wú)法按照預(yù)定軌跡泊車入位，而兩圓弧相切法與圓弧切直線法無(wú)法解決這一問(wèn)題。

為解決上述問(wèn)題，本文通過(guò)分析在誤差產(chǎn)生時(shí)無(wú)法正常泊車入位的約束條件，提出雙向與單向路徑規(guī)劃方法，在減少泊車位長(zhǎng)度的同時(shí)保證了泊車過(guò)程順利進(jìn)行。

2 自動(dòng)平行泊車系統(tǒng)介紹

自動(dòng)平行泊車系統(tǒng)的工作流程如圖1所示。系統(tǒng)開(kāi)啟后，位于車輛前方的超聲波傳感器開(kāi)始尋找合適的泊車位；如果找到合適的泊車位，則根據(jù)車輛的位置關(guān)系建立泊車軌跡坐標(biāo)系，并根據(jù)周圍的環(huán)境以及車輛參數(shù)進(jìn)行路徑規(guī)劃；路徑規(guī)劃完成后，系統(tǒng)提示駕駛員進(jìn)行下一步操作；如果駕駛員采用自動(dòng)泊車，則系統(tǒng)會(huì)根據(jù)傳感器采集到的車速以及環(huán)境信息控制轉(zhuǎn)向盤完成泊車。首次泊車結(jié)束后，若車輛未能順利泊車入位，則根據(jù)車輛的位置信息進(jìn)行二次路徑規(guī)劃并泊車入位，直到達(dá)到最佳泊車位置。如果駕駛員不采用自動(dòng)泊車，則泊車系統(tǒng)自動(dòng)退出。此外，泊車系統(tǒng)還需保證在泊車過(guò)程中駕駛員能隨時(shí)接管轉(zhuǎn)向盤以保證泊車過(guò)程中的安全性。

3 自動(dòng)平行泊車路徑規(guī)劃

3.1 單向路徑規(guī)劃平行泊車

在實(shí)際泊車過(guò)程中，會(huì)出現(xiàn)無(wú)法按照理論上的路徑規(guī)劃過(guò)程完全地泊車入位情況，如圖2所示。由圖2可看出，車輛在圖示位置進(jìn)行倒車入位過(guò)程中，理論倒車軌跡為圓弧EN段與圓弧NA段，由于在實(shí)際操作過(guò)程中傳感器的測(cè)量誤差及駕駛員的操作誤差，車輛如果按照理論情況進(jìn)行泊車入位，將導(dǎo)致車輛在B點(diǎn)與后方障礙物發(fā)生碰撞，因此圓弧AB段泊車軌跡無(wú)法實(shí)現(xiàn)，車輛無(wú)法按照規(guī)劃好的泊車軌跡泊車入位，泊車過(guò)程無(wú)法完成。此時(shí)，需要進(jìn)一步的路徑規(guī)劃才能完成車輛泊車入位。

為解決上述自動(dòng)泊車問(wèn)題，提出了單向路徑規(guī)劃方法。單向路徑規(guī)劃采用平移庫(kù)模式，使車輛通過(guò)1次前進(jìn)就可順利進(jìn)入泊車位中。由圖2可知，由于AB段的泊車軌跡無(wú)法繼續(xù)進(jìn)行，所以當(dāng)車輛在B點(diǎn)時(shí)不能繼續(xù)倒車入位。假設(shè)車輛在B點(diǎn)時(shí)的駛過(guò)角為θ，此時(shí)可采用圖3所示路徑規(guī)劃方案進(jìn)行泊車。首先轉(zhuǎn)向盤右轉(zhuǎn)并掛前進(jìn)擋，車輛沿圓弧BC行駛到C點(diǎn)，然后轉(zhuǎn)向盤左轉(zhuǎn)使車輛沿圓弧CD行駛到D點(diǎn)，最后車輛掛入倒擋以調(diào)整車輛與前、后障礙物距離。

由于圖3中的路徑規(guī)劃方案并沒(méi)有考慮碰撞約束，因此需要在碰撞約束條件下分析該方案的可行性。記車輛后軸中點(diǎn)為E，車輛輪廓右后端、左后端、右前端、左前端分別為F、G、H、I。假設(shè)作為障礙物的前、后車輛與所泊車輛的參數(shù)（表1）相同，則：A點(diǎn)坐標(biāo)為（a，0）；O1點(diǎn)坐標(biāo)為（a，R）；B點(diǎn)坐標(biāo)為（a+Rsinθ，R（1-cosθ））；O2點(diǎn)坐標(biāo)為(a+2Rsinθ，R（2cosθ-1））；C點(diǎn)坐標(biāo)為（a+3Rsinθ，R（1-cosθ））；D點(diǎn)坐標(biāo)為（a+4Rsinθ，0）；O3點(diǎn)坐標(biāo)為（a+ 4Rsinθ，R）。

表1 前、后車輛與所泊車輛的參數(shù)

在單向路徑規(guī)劃時(shí)，車輛與周圍障礙物可能發(fā)生碰撞的情況有如下幾種：泊車入位時(shí)車輛左后端與后方障礙物發(fā)生碰撞；泊車入位時(shí)車輛右后端與右方障礙物發(fā)生碰撞；泊車入位時(shí)車輛前端與前方障礙物發(fā)生碰撞；泊車入位時(shí)車輛右前方與右側(cè)障礙物發(fā)生碰撞，如圖4所示。

為避免在泊車過(guò)程中車輛與周圍障礙物發(fā)生碰撞并能安全進(jìn)入車位，車輛的泊車軌跡需滿足以下要求：

a.當(dāng)車輛沿圓弧AB行駛時(shí)，由圖4a可知，車輛左后方G點(diǎn)的約束條件為：

式中，Gx為G在點(diǎn)X方向坐標(biāo)。

b.當(dāng)車輛沿圓弧AB行駛時(shí)，由圖4b可知，車輛右后方F點(diǎn)的約束條件為：

式中，F(xiàn)y為F點(diǎn)在Y方向坐標(biāo)。

當(dāng)Fy取最小值時(shí)，

c.當(dāng)車輛沿圓弧CD行駛時(shí)，由圖4c可知，車輛前方的約束條件為：

式中，Dx為D點(diǎn)在X方向坐標(biāo)。

d.當(dāng)車輛沿圓弧CD行駛時(shí)，由圖4d可知，車輛右前方H點(diǎn)的約束條件為：

當(dāng)Hy取最小值時(shí)：

式中，Hy為H點(diǎn)在Y方向坐標(biāo)。

由上述分析可以建立規(guī)劃軌跡的約束函數(shù)：

在實(shí)際求解車位長(zhǎng)度中，車輛初始位置已知，車位尺寸未知，則以θ和Wa作為變量，以Le最小化作為目標(biāo)，利用matlab的非線性優(yōu)化功能求解式（10）。

3.2 雙向路徑規(guī)劃平行泊車

在工程實(shí)踐中，當(dāng)使用兩圓弧相切法進(jìn)行路徑規(guī)劃時(shí)，若取圓弧半徑為車輛的最小轉(zhuǎn)彎半徑，則由于電動(dòng)助力轉(zhuǎn)向系統(tǒng)（EPS）控制器設(shè)置有過(guò)流保護(hù)模式，將導(dǎo)致轉(zhuǎn)向盤無(wú)法長(zhǎng)時(shí)間處于最大轉(zhuǎn)角，因此在使用兩圓弧相切法進(jìn)行路徑規(guī)劃時(shí)，一般不使用最小圓弧半徑進(jìn)行車輛轉(zhuǎn)彎動(dòng)作，這就導(dǎo)致所需最小泊車位長(zhǎng)度明顯增加，而雙向路徑規(guī)劃可有效減小所需的泊車位長(zhǎng)度。

圖5為雙向路徑規(guī)劃方案。取后軸中點(diǎn)為參考點(diǎn)，由于AB段的泊車軌跡無(wú)法繼續(xù)進(jìn)行，所以當(dāng)車輛在B點(diǎn)時(shí)不能繼續(xù)倒車入位。此時(shí)，假設(shè)車輛的駛過(guò)角為θ，根據(jù)幾何關(guān)系可得到圓弧AB段的圓心角為θ。首先車輛沿圓弧BC段行駛，并駛過(guò)對(duì)應(yīng)圓心角為2θ的弧長(zhǎng)后，左打轉(zhuǎn)向盤，車輛沿圓弧CD1段行駛，并駛過(guò)對(duì)應(yīng)圓心角為θ的弧長(zhǎng)后，掛倒擋右打轉(zhuǎn)向盤，駛過(guò)對(duì)應(yīng)圓心角為β的弧長(zhǎng)后，再左打轉(zhuǎn)向盤駛過(guò)對(duì)應(yīng)圓心角為β的弧長(zhǎng)到達(dá)D2點(diǎn)，最后轉(zhuǎn)向盤回正并在車位中調(diào)整車輛與前、后車間距。車輛在BC與D1D2段行駛時(shí)的轉(zhuǎn)彎半徑為R，在CD1段行駛時(shí)的轉(zhuǎn)彎半徑為R1。

圖5中，O1、O2、O3、O4、O5分別為各圓弧的圓心；A點(diǎn)坐標(biāo)為（a，0）；O1點(diǎn)坐標(biāo)為（a，R）；B點(diǎn)坐標(biāo)為（a+Rsinθ，R（1-cosθ））；O2點(diǎn)坐標(biāo)為（a+2Rsinθ，R（2cosθ-1））；C點(diǎn)坐標(biāo)為（a+3Rsinθ，R（1-cosθ））；O3點(diǎn)坐標(biāo)為（a+3Rsinθ+R1sinθ，R（1-cosθ)）+R1cosθ）D1點(diǎn)坐標(biāo)為（a+3Rsinθ+R1sinθ，R（1-cosθ）-R1（1-cosθ））；O4點(diǎn)坐標(biāo)為（a+3Rsinθ+R1sinθ-2Rsinβ，R）；D2點(diǎn)坐標(biāo)為（a+ 3Rsinθ+R1sinθ-2Rsinβ，0）；O5點(diǎn)坐標(biāo)為（a+3Rsinθ+R1sinθ，-Rcosθ-R1（1-cosθ））。

與前述相同，在碰撞分析時(shí)只需考慮二次路徑規(guī)劃時(shí)可能發(fā)生的碰撞，碰撞情況與單向路徑規(guī)劃時(shí)相同。除第3種碰撞情況外，其它3種情況的碰撞分析關(guān)系式與前述相同，為此僅對(duì)第3種碰撞情況進(jìn)行分析。

當(dāng)車輛沿圓弧BC與圓弧CD1行駛時(shí)，由圖4c可知，車輛前方約束條件為：

式中，D2x與Bx分別為D點(diǎn)與B點(diǎn)在x方向坐標(biāo)。

除此之外，根據(jù)車輛本身的約束條件，可以得到約束函數(shù)：

由上述分析可建立二次規(guī)劃軌跡約束函數(shù)：

在實(shí)際求解車位長(zhǎng)度中，車輛初始位置已知，車位尺寸未知，則以θ、β和Wa作為變量，以Le最小化作為目標(biāo)，利用matlab的非線性優(yōu)化功能進(jìn)行求解式（13）。

4 仿真實(shí)例與工程應(yīng)用

4.1 仿真實(shí)例與分析

利用軟件matlab進(jìn)行仿真試驗(yàn)，試驗(yàn)用車輛參數(shù)為：車長(zhǎng)4.4 m，車寬1.8 m，軸距2.7 m，后懸長(zhǎng)0.5 m，最小轉(zhuǎn)彎半徑4.2 m，障礙物寬度取為2.5 m。根據(jù)式（10）與式（13）得出仿真參數(shù)如表2所列。

表2 仿真參數(shù)

在未考慮車輛與前、后方障礙物所需間隙的情況下，如果未采用單向路徑規(guī)劃，得出的最小車位長(zhǎng)度為6.1 m，如果采用單向路徑規(guī)劃平行泊車進(jìn)行泊車入位，得出的最小車位長(zhǎng)度為5.8 m，仿真結(jié)果如圖6和圖7所示。

在路徑規(guī)劃時(shí)，如果未采用雙向路徑規(guī)劃平行泊車，則所需泊車位長(zhǎng)度為7.3 m，如果采用雙向路徑規(guī)劃平行泊車，則所需泊車位長(zhǎng)度為7 m，仿真結(jié)果如圖8和圖9所示。

由圖7和圖9可看出，在相同泊車環(huán)境下，車輛沿泊車軌跡進(jìn)入泊車位中，且與周圍障礙物無(wú)交點(diǎn)，表明單向路徑規(guī)劃方法與雙向路徑規(guī)劃方法均能在滿足車輛最小轉(zhuǎn)彎半徑的情況下使車輛安全入位。此外，雙向路徑規(guī)劃所需轉(zhuǎn)向盤轉(zhuǎn)角較小，因此可減少泊車系統(tǒng)EPS的工作負(fù)荷。

綜上所述，單向路徑規(guī)劃對(duì)泊車空間要求較小，但是對(duì)EPS系統(tǒng)的要求較高；雙向路徑規(guī)劃對(duì)泊車空間要求較高，但是對(duì)EPS系統(tǒng)的要求較低。在實(shí)際運(yùn)用中，可以根據(jù)EPS系統(tǒng)的使用限制選擇合適的泊車方法進(jìn)行泊車。

4.2 工程實(shí)例

以景逸X5為試驗(yàn)車，硬件系統(tǒng)以XC164為基礎(chǔ)，軟件系統(tǒng)以Dave與Keil為基礎(chǔ)，建立自動(dòng)泊車系統(tǒng)，通過(guò)OBD接口中的CAN信號(hào)實(shí)時(shí)監(jiān)測(cè)車輛狀態(tài)，獲取車輛運(yùn)動(dòng)信息，并實(shí)現(xiàn)與EPS系統(tǒng)通訊，如圖10所示。實(shí)際泊車過(guò)程中，利用超聲波傳感器測(cè)得車輛與側(cè)方障礙物距離并計(jì)算得到泊車起始點(diǎn)信息與泊車過(guò)程中拐點(diǎn)信息，由單片機(jī)計(jì)算出車輛行駛的實(shí)際距離并與理論距離進(jìn)行比較來(lái)控制轉(zhuǎn)向盤轉(zhuǎn)動(dòng)的時(shí)機(jī)，最后根據(jù)電子羅盤中航向角信息進(jìn)行泊車過(guò)程路徑規(guī)劃。

試驗(yàn)采用與表2相同的參數(shù)，試驗(yàn)結(jié)果如表3所列。由于在實(shí)際泊車過(guò)程中，車輛與前、后方障礙物留有間隙，因此所需車位長(zhǎng)度比理論值大。由表3可知，使用2種路徑規(guī)劃方案后可有效降低泊車位所需長(zhǎng)度。

表3 實(shí)車試驗(yàn)結(jié)果 m

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

本文通過(guò)采用單向路徑與雙向路徑規(guī)劃方法，有效解決了車輛無(wú)法按照預(yù)定軌跡泊車入位問(wèn)題。在確定了2種泊車軌跡的變量、碰撞約束函數(shù)以及規(guī)劃目標(biāo)后，利用matlab軟件的非線性約束函數(shù)的優(yōu)化功能求解泊車軌跡方程，并對(duì)2種軌跡方法進(jìn)行仿真試驗(yàn)，結(jié)果表明，在相同的泊車環(huán)境下，2種軌跡規(guī)劃方法都可實(shí)現(xiàn)安全順利的泊車入位。在實(shí)際工程運(yùn)用中，考慮到傳感器、單片機(jī)以及EPS系統(tǒng)在工作過(guò)程中會(huì)產(chǎn)生誤差，對(duì)實(shí)際車輛進(jìn)行路徑規(guī)劃時(shí)，應(yīng)根據(jù)車輛尺寸增加合適的安全余量。實(shí)踐表明，本文所述路徑規(guī)劃方法可在工程中實(shí)現(xiàn)。

1 中汽協(xié)會(huì)行業(yè)信息部.2013年汽車工業(yè)產(chǎn)銷情況簡(jiǎn)析.中國(guó)汽車工業(yè)協(xié)會(huì)官網(wǎng),2013.

2 I.E.Paromtchik,C.Laugier.Autonomous parallel parking and returning to traffic maneuvers.Intelligent Robotics and System,1997,3:21-23.

3 C.Laugier,Th.Fraichard.Sensor-based control architec?ture for a car-like vehicle.Intelligent Robotics and System, 1998,1:216-222.

4 Richard M.Murray,S.Shankar Sastry.Steering nonholonom?ic systems using sinusoids.IEEE proceeding of 29thConfer?ence on Decision and Control,1990:2097-2101.

5 Derrick H.Nguyen,Bernard Widrow.Neural Neural net?works for self-learning control sysyems.IEEE Intelligent Vehicle Symposium,1990:450-454.

6 Derrick H.Nguyen,Bernard Widrow.The truck back-upper an example of self-learning Inneural network.Proc.Int.Joint.Neural Networks,1989,7:357-363

7 I.E.Paromtchik,C.Laugier.Autonomous parallel parking of nonholonomomic Vehicle.Proceedings of the IEEE Intel?ligent Vehicle Symposium,1996:3117-3122

8 I.E.Paromtchik,C.Laugier.Motion generation and control for parking an autonomous vehicle.Proceedings of the IEEE international Conference on Robots and Automation,1996: 13-18

9 宋金澤.自主泊車系統(tǒng)關(guān)鍵技術(shù)研究：[學(xué)位論文].湖南：國(guó)防科技技術(shù)大學(xué)研究生院,2009.

（責(zé)任編輯文 楫）

修改稿收到日期為2015年5月27日。

Multi-route Planning for Parallel Parking System with Multi-constraints

Hou Xiaoyang,Huang Yong,Sun Si,Ji Xuewu
（State Key Laboratory of Automotive Safety and Energy,Tsinghua University）

For the narrow parking space which can not satisfy the requirement of parallel parking,we present two route planning methods:one-way multiple-route planning method and two-way multiple-route planning method.When variables,collision constraint function and planning objective of two parking trajectories are defined,we make use of optimization function of the nonlinear constraint function in MATAB to solve the parking trajectory equation,and these two route planning methods are simulated and tested,which show that,in the same parking environment,both of two route planning methods can make the car park safely with smaller parking length;safety and validity of the presented route planning methods are verified with vehicle test.

Auto parallel parking，Route planning，Constraint function

自動(dòng)平行泊車 路徑規(guī)劃 約束函數(shù)

U471.1

A

1000-3703（2015）09-0048-05