多約束平行泊車路徑規劃

侯曉陽 黃勇 孫思 季學武

（清華大學 汽車安全與節能國家重點實驗室）

多約束平行泊車路徑規劃

侯曉陽 黃勇 孫思 季學武

（清華大學 汽車安全與節能國家重點實驗室）

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

1 前言

隨著汽車保有量的增加，隨之帶來的問題也日益凸顯。其中，最為突出的就是停車難問題，而自動泊車系統可以很好地解決這一問題，因此其已成為汽車行業的研究重點。平行泊車理論研究的關鍵部分是路徑規劃階段，對此國內外學者已經進行了大量研究，研究方法多采用兩圓弧相切或圓弧切直線的路徑規劃方法，并且路徑規劃的結果需要滿足環境約束[1～5]。雖然這些路徑規劃方法可滿足平行泊車的基本需要，但是由于實際泊車過程中傳感器測量的誤差以及機械機構操作誤差等，會導致車輛無法按照預定軌跡泊車入位，而兩圓弧相切法與圓弧切直線法無法解決這一問題。

為解決上述問題，本文通過分析在誤差產生時無法正常泊車入位的約束條件，提出雙向與單向路徑規劃方法，在減少泊車位長度的同時保證了泊車過程順利進行。

2 自動平行泊車系統介紹

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

3 自動平行泊車路徑規劃

3.1 單向路徑規劃平行泊車

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

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

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

表1 前、后車輛與所泊車輛的參數

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

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

a.當車輛沿圓弧AB行駛時，由圖4a可知，車輛左后方G點的約束條件為：

式中，Gx為G在點X方向坐標。

b.當車輛沿圓弧AB行駛時，由圖4b可知，車輛右后方F點的約束條件為：

式中，Fy為F點在Y方向坐標。

當Fy取最小值時，

c.當車輛沿圓弧CD行駛時，由圖4c可知，車輛前方的約束條件為：

式中，Dx為D點在X方向坐標。

d.當車輛沿圓弧CD行駛時，由圖4d可知，車輛右前方H點的約束條件為：

當Hy取最小值時：

式中，Hy為H點在Y方向坐標。

由上述分析可以建立規劃軌跡的約束函數：

在實際求解車位長度中，車輛初始位置已知，車位尺寸未知，則以θ和Wa作為變量，以Le最小化作為目標，利用matlab的非線性優化功能求解式（10）。

3.2 雙向路徑規劃平行泊車

在工程實踐中，當使用兩圓弧相切法進行路徑規劃時，若取圓弧半徑為車輛的最小轉彎半徑，則由于電動助力轉向系統（EPS）控制器設置有過流保護模式，將導致轉向盤無法長時間處于最大轉角，因此在使用兩圓弧相切法進行路徑規劃時，一般不使用最小圓弧半徑進行車輛轉彎動作，這就導致所需最小泊車位長度明顯增加，而雙向路徑規劃可有效減小所需的泊車位長度。

圖5為雙向路徑規劃方案。取后軸中點為參考點，由于AB段的泊車軌跡無法繼續進行，所以當車輛在B點時不能繼續倒車入位。此時，假設車輛的駛過角為θ，根據幾何關系可得到圓弧AB段的圓心角為θ。首先車輛沿圓弧BC段行駛，并駛過對應圓心角為2θ的弧長后，左打轉向盤，車輛沿圓弧CD1段行駛，并駛過對應圓心角為θ的弧長后，掛倒擋右打轉向盤，駛過對應圓心角為β的弧長后，再左打轉向盤駛過對應圓心角為β的弧長到達D2點，最后轉向盤回正并在車位中調整車輛與前、后車間距。車輛在BC與D1D2段行駛時的轉彎半徑為R，在CD1段行駛時的轉彎半徑為R1。

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

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

當車輛沿圓弧BC與圓弧CD1行駛時，由圖4c可知，車輛前方約束條件為：

式中，D2x與Bx分別為D點與B點在x方向坐標。

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

由上述分析可建立二次規劃軌跡約束函數：

在實際求解車位長度中，車輛初始位置已知，車位尺寸未知，則以θ、β和Wa作為變量，以Le最小化作為目標，利用matlab的非線性優化功能進行求解式（13）。

4 仿真實例與工程應用

4.1 仿真實例與分析

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

表2 仿真參數

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

在路徑規劃時，如果未采用雙向路徑規劃平行泊車，則所需泊車位長度為7.3 m，如果采用雙向路徑規劃平行泊車，則所需泊車位長度為7 m，仿真結果如圖8和圖9所示。

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

綜上所述，單向路徑規劃對泊車空間要求較小，但是對EPS系統的要求較高；雙向路徑規劃對泊車空間要求較高，但是對EPS系統的要求較低。在實際運用中，可以根據EPS系統的使用限制選擇合適的泊車方法進行泊車。

4.2 工程實例

以景逸X5為試驗車，硬件系統以XC164為基礎，軟件系統以Dave與Keil為基礎，建立自動泊車系統，通過OBD接口中的CAN信號實時監測車輛狀態，獲取車輛運動信息，并實現與EPS系統通訊，如圖10所示。實際泊車過程中，利用超聲波傳感器測得車輛與側方障礙物距離并計算得到泊車起始點信息與泊車過程中拐點信息，由單片機計算出車輛行駛的實際距離并與理論距離進行比較來控制轉向盤轉動的時機，最后根據電子羅盤中航向角信息進行泊車過程路徑規劃。

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

表3 實車試驗結果 m

5 結束語

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

1 中汽協會行業信息部.2013年汽車工業產銷情況簡析.中國汽車工業協會官網,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 宋金澤.自主泊車系統關鍵技術研究：[學位論文].湖南：國防科技技術大學研究生院,2009.

（責任編輯文 楫）

修改稿收到日期為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

自動平行泊車 路徑規劃 約束函數

U471.1

A

1000-3703（2015）09-0048-05