基于入位基準線的避死區自動泊車路徑規劃

李茂月 陳 月 徐光岐

哈爾濱理工大學機械動力工程學院，哈爾濱，150080

0 引言

發展自動泊車技術，不僅可以減小駕駛員的泊車難度，而且對解決“一位難求”問題也有顯著效果。文獻[1]設計了一種泊車輔助器，可幫助駕駛員方便安全地泊車；文獻[2]結合基于幾何學改進的最小半徑法和不等半徑路徑規劃方法使泊車過程得到了優化；文獻[3]采用多段圓弧曲線設計泊車路徑，并采用回旋曲線連接曲率不連續的多段圓弧；文獻[4]進行了基于軌跡跟蹤控制器的泊車控制研究；文獻[5]采用B樣條曲線對泊車路徑曲率進行平滑處理；文獻[6]通過B樣條曲線建立了平行車位的泊車避障約束函數；文獻[7]提出了結合定點跟蹤的非時間參考的路徑跟蹤控制策略；文獻[8]應用微分平坦理論并融合動力學約束對自動平行泊車軌跡進行了研究；文獻[9]采用Bezier曲線對泊車軌跡進行擬合，但未解決泊車原地轉向問題；文獻[10]基于微分平坦與樣條理論進行了自主泊車軌跡規劃研究。

為了使駕駛員更好地觀察車位狀態，防止有小障礙物阻礙自動泊車的進行，本文研究了與車位齊平的位置處停車的自動泊車系統。自動泊車系統開啟后，駕駛員通過調節攝像頭的角度來尋找空車位，當提示車位大小合適時，按下確認鍵后系統將開始自動計算出汽車與車位的位姿關系，駕駛員同時選擇前進水平線的位置，以避免泊車死區(泊車死區是指由于汽車與車位位置較近、角度較大等原因造成的汽車不能泊入車位的區域)，進行確認后汽車將結合位姿關系規劃泊車軌跡，按下執行鍵后，汽車開始自動進行泊車動作。該泊車動作主要分為三部分：向前行駛至前進水平線、倒車行駛至入位基準線、倒車入位。

1 自動泊車過程軌跡規劃

自動泊車系統的工作流程如圖1所示。當汽車準確計算出與空車位的位姿關系后，會確定出汽車在泊車過程中必需經過的幾個位置點，根據三次插值樣條理論[11]控制泊車路徑的軌跡形狀[12]，泊車路線便被確定且具有唯一性。很明顯，車位長度越小，泊車系統需要進行更多次數的糾正才能使汽車完全停入車位。如果外圍車輛停放復雜，汽車需要進行糾正才能避免泊車死區的存在。因此，針對前一種情況，可以優化汽車泊車所需要的最小車位長度；針對后一種情況，需要確定出泊車死區范圍，從而避開該區域，以保證汽車能夠完成自動泊車動作。

圖1 自動泊車系統工作流程Fig.1 Automatic parking system workflow

本文研究的自動泊車過程包括車位外部的路徑規劃過程和車位內部的路徑規劃過程。前者的路徑規劃過程稱為外部路徑規劃，包括外部前進過程和外部后退過程；后者的路徑規劃過程稱為內部路徑規劃。本文主要對前者進行論述。

1.1 外部路徑規劃

自動泊車的第一步是向前行駛至前進水平線，然后由前進水平線倒車入位，由于前進水平線位于非泊車死區內，因此可以保證汽車一定能夠泊入車位。將本文所需要的一些車位線和軌跡線進行命名，如圖2所示。

圖2 自動泊車系統與車位命名示意圖Fig.2 Schematic diagram naming of the automatic parking system and parking spaces

圖2中，汽車初始位置為汽車探測空車位時所停止的位置；倒車準備位置為汽車行駛至最前端的位置，并由此位置開始倒車入位；前車軸中心點記為點A，表示汽車前端；后車軸中心點記為點B，表示汽車后端；以空車位的左下角為坐標系原點建立直角坐標系OXY。當汽車泊車系統探測空車位時，可以得到的已知數據有A點坐標(XA0，YA0)、B點坐標(XB0，YB0)、姿態角α，前進水平線為汽車檢測空車位后自動判斷的一條軌跡線，可以根據實際前方路況進行手動調節，故其與X軸的距離也為已知值。

1.2 前進水平線的選取

前進水平線與車位外邊線平行且具有一定間距，該間距可以通過手動選取來進行調節。前進水平線的作用是為了避開某些阻擋在泊車線路上的障礙物。前進水平線的調節范圍為：距車位外邊線最小距離的前進水平線與入位基準線相交后形成的右上側區域，因此當手動調節軌跡框向右向后調節至最小值時，繼續調節將不能使軌跡框向右向后移動。

在汽車與車位的位姿關系確定結束后，中央顯示屏內將顯示前攝像頭所采集的圖像信息，且會在圖像內出現汽車的前進軌跡。如圖3所示，實線表示汽車的輪廓路徑，虛線即為前進水平線，且由于位姿關系已確定，該三條線與車位外邊線處于平行狀態。通過手動選取前進水平線的位置及前端停止位置，可以改變汽車自動向前行駛的路徑。

圖3中前進路徑沒有被阻擋，表示汽車停止位置良好，不需要再進行前進水平線的調整。圖4表示待泊汽車的停止位置太過靠右，汽車的前進路徑軌跡被右側車輛阻擋，導致不能向前自動行駛，因此需要進行前進水平線的調整。對于圖4所示的情況，第一步需要將前進水平線向左移動，即外移至圖5所示位置，該位置的前進路徑右側將不會被右側的車輛阻擋，但左側將會被車輛1阻擋，因此還需要將前進路徑的前端向后移動至圖6所示位置(當前位置與后移位置間有最小閾值)，此時汽車的整個行車路徑都不會被阻擋，可以完成自動向前行駛和避死區的動作過程。

圖4 前進路徑右側被阻擋Fig.4 The right side of the path is blocked

圖5 左移后前進路徑前端被阻擋Fig.5 The front end of the forward path is blockedafter moving left

圖6 下移后前進路徑沒有被阻擋Fig.6 The path is not blocked after moving down

1.3 外部前進過程糾正方法與軌跡計算

根據阿克曼轉向幾何原理[13]，可以確定出汽車的轉彎圓心。當左/右打滿汽車轉向盤時，汽車上任何一點均繞該幾何圓心運行，對汽車的前后車軸的中心點繞圓心的軌跡半徑進行標定，對汽車與車位的其他名稱與符號進行匯總，如表1所示。

表1 汽車及車位的參數與符號

圖2所示為最簡單的外部糾正，即汽車由初始位置直接向前行駛至倒車準備位置，此時不需要進行角度糾正，只需向前行駛即可。除了上述情況外，由于汽車在初始位置處，車頭可能偏左，也可能偏右，本文以車頭偏右為例進行分析(向左情況相似)，存在下述3種情況：

(1)情況1。汽車的前后車軸中點均位于前進線左側，如圖7所示，即均在前進水平線上側，則計算汽車所需要的轉彎半徑，控制轉向盤的左轉角度，行駛至前進水平線。

該情況下，汽車需經過起始點A1和前進水平線起點A2，汽車與前進水平線夾角為α，即以O1點為圓心進行轉向需要掃過的角度為α，前車軸中心點將由A1沿圓弧A1A2運動至A2，其中A1和A2的縱坐標分別為YA1、Y0，其值均已知，則可以根據下式計算出此時所需要的轉彎半徑：

(1)

點A1至點A2運動過程中，ω為繞O1掃過的角度，其值是變化的，軌跡方程為

(2)

其中，XA0=XA1，YA0=YA1，ω變化范圍為270°-α～270°。

(2)情況2。汽車的前車軸中點位于前進水平線右側，后車軸中點位于前進水平線左側，如圖8所示。此時需要先計算汽車左打滿轉向盤的情況下，沿圓弧A1A2行駛至與前進水平線平行狀態(其掃過的角度大小為α)，并計算出此時所在平行線與前進水平線的距離差LX，通過“先左打滿再右打滿”的方法，即汽車先繼續保持轉向盤左打滿狀態沿圓弧A2A3行駛，當行駛至點A3時，將轉向盤改為右打滿狀態，沿圓弧A3A4行駛，并繼續向前行駛一小段距離X3使前車軸中心點A、后車軸中心點B同時處于前進水平線上。

圖8 情況2示意圖Fig.8 Schematic for the case 2

(3)情況3。汽車的前后車軸中點均位于前進水平線右側，如圖9所示，即均在前進水平線下側，也需要經過先行駛至水平狀態再進行“先左打滿再右打滿”的轉向模式，計算方法與情況2類似。

圖9 情況3所述示意圖Fig.9 Schematic for the case 3

由于情況2和情況3的計算方法相同，因此可將軌跡線繪制于圖10進行計算。

圖10 軌跡計算標識圖Fig.10 Trajectory calculation identification diagram

這兩種情況(情況2和情況3)分為三階段：第一階段點A1至點A2，第二階段點A2至點A3，第三階段點A3至點A4。其中，第一階段點A1至點A2的計算方式與情況1相同，見式(1)，第二、第三兩個階段關于點A3對稱，因此僅需計算第三階段，即能得出第二階段的數據。

根據圖10的幾何關系得

(3)

其計算結果為

(4)

在上述泊車過程中，汽車經過的點為起始點A1，第一次水平切點A2，過渡中間點A3和第二次水平切點A4(即前進水平線起點)。

點A2的坐標為

點A3的坐標為

點A4的坐標為

點A5的坐標為

Y0+LX)

由上述各個點的坐標，確定各階段軌跡方程如下。

點A2至點A3運動過程中，ω為繞O1掃過的角度與α的差值，記為β，其值是變化的，軌跡方程為

(5)

其中，ω變化范圍為270°～270°+β。

點A3至點A4運動過程中，ω為繞O2掃過的角度，其值是變化的，軌跡方程為

(6)

其中，ω變化范圍為90°+β～90°。

點A4至點A5運動過程中，XX為汽車前進的距離，軌跡方程為

(7)

其中，XX的變化范圍為(0，X3)。

1.4 外部后退過程糾正方法與軌跡計算

汽車由前進水平線倒車至入位基準線的軌跡分析如圖11所示，當汽車的前后車軸中心點均位于圖中陰影部分時，汽車便一定可以泊入車位。當設定的前進水平線與車位外邊線的距離大于Ymin時，則汽車不會存在泊車死區。

圖11 前進水平線與車位外邊線距離示意圖Fig.11 Schematic diagram of the distance between theforward horizontal line and the outside line ofparking spaces

(8)

故自動泊車時需保證前進水平線與車位外邊線距離滿足Y0≥Ymin。

汽車由前進水平線至完全泊入車位時的軌跡如圖12所示，當汽車行駛至所設定的前進水平線后，汽車開始進行倒車動作，此后以車軸中心點B為軌跡點，將依次經過點B1、B2、B3、B4、B0，當倒車至點B1時汽車轉向盤右打滿，轉彎半徑為RB，沿圓弧B1B2倒車，進入入位基準線。該過程中，汽車必經過起始點B1和入位基準線起點B2。由于已知點A1的坐標和軸距，故點B1的坐標也已知，記為(XB1,YB1)，則點B2的坐標為(XB1-RBsinθ，YB1-RB(1-cosθ))。

圖12 一次泊車入位示意圖Fig.12 The diagram of one-time automatic parkinginto the parking space

從點B1運動至點B2的過程中，繞O3掃過的角度ω是變化的，點的軌跡方程為

(9)

其中，ω變化范圍為90°～90°+α。

如果車位長度足夠，當汽車進入入位基準線后倒車至點B3時，汽車與車位的位置狀態為汽車的右前角點與汽車的左后角點同時位于車位外邊線上，并且汽車的右前角點與車位外邊線及車位前邊線的交點重合，此時汽車保持轉向盤左打滿狀態，沿圓弧B3B4行駛至點B4，并后退一段距離至B0，以保證汽車整個車身完全進入車位內并與車位保持平行，結束泊車動作。

該過程中，汽車必經過起始點B1、入位基準線起點B2、入位基準線終點B3、第一折點B4。

其中，B3坐標為

B4坐標為

B0坐標為

由上述各個點的坐標，確定各階段軌跡方程如下。

點B2運動至點B3過程中，軌跡方程為

(10)

其中，x的變化范圍為點B2至點B3的橫坐標變化范圍。

點B3至點B4運動過程中，ω為繞O4掃過的角度，其值是變化的，軌跡方程為

(11)

其中，ω的變化范圍為270°+θ～270°。

點B4至點B0運動的過程中，軌跡方程為

(12)

其中，x′的變化范圍為(0，X4)。

由于汽車的前車軸中心點A與后車軸中心點B的軸距差為LZ，且其值已知，因此可以將點A和點B的運動方程全部表示出，汽車運動方式即可確定。

2 傾斜與垂直車位的軌跡規劃

傾斜車位屬于垂直車位的延伸模式，如圖13所示，虛線為垂直車位，實線為傾斜車位，傾斜車位時汽車與車位的各種位置情況都與垂直車位類似，這兩種車位的軌跡規劃類似于平行車位。

圖13 傾斜車位與垂直車位Fig.13 Tilted and vertical parking space

對于垂直車位，第一步仍然是為避開泊車死區而向前行駛至前進水平線，然后開始倒車至車位內，如圖14所示。

圖14 垂直車位的避死區路徑規劃Fig.14 Path planning to avoid the dead zone forvertical parking

垂直車位時的泊車軌跡如圖15所示，此時需要保證汽車轉過的角度θ為90°，并根據不同的前進水平線決定不同的后退距離。由于不同的前進水平線會對應不同的車位大小，因此該軌跡規劃方法并不能確定出最小車位的大小。但由于垂直車位需要對乘客的上下車提供足夠的預留空間，因此車位的寬度一般都能達到需求。

圖15 垂直車位路徑軌跡計算圖Fig.15 Vertical parking path trajectorycalculation diagram

為了求出垂直車位的泊車軌跡方程，建立圖15所示的坐標系，汽車的后車軸中心點將依次經過點B1、B2、B3、B4，完成泊車動作。

從點B1運動至點B2過程中，軌跡方程為

(13)

從點B2運動至點B3過程中，θ為繞O′掃過的角度，其值是變化的，軌跡方程為

(14)

其中，ω的變化范圍為(0，90°)。

從點B3運動至點B4過程中，軌跡方程為

(15)

其中，n的變化范圍為(LZB,a)。

但由于前進水平線的選取距離不同，所需的車位大小也不相同，因此有圖16所示的車位與不同前進水平線的關系，其車位的寬度W和高度H如下：

(16)

圖16 不同前進水平線對應不同的垂直車位Fig.16 Different forward horizontal lines correspondto different vertical parking spaces

如圖17所示，對于傾斜車位，由于傾斜角度不同，因此倒車路徑的終止時間點不同，從而泊車過程更易適應。另外，車輛停入傾斜車位時相鄰兩車的車門并不是相對的，因此能夠減小相鄰車位的間距，并在相同的空間內規劃出更多的車位。

圖17 傾斜車位(沒有停泊完全的垂直車位)Fig.17 Tilting parking spaces(not parking completelyvertical parking spaces)

3 仿真實例及實驗驗證

3.1 仿真實例及分析

由上述方法建立泊車軌跡模型，對車輛的運行軌跡使用CarSim軟件進行仿真，并將軌跡點導出后通過MATLAB軟件進行繪制和結果分析。其中車輛參數見表2，仿真的結果見圖18、圖19。

表2 車輛參數

(a)起點(0，3.997 6 m)至前進水平線5.25

(b)起點(0，4.650 0 m)至前進水平線5.25

(c)起點(0，3.997 6 m)至前進水平線6.00

(d)起點(0，4.650 0 m)至前進水平線6.00圖18 不同起點前進修正至不同前進水平線的前軸中點運動仿真圖Fig.18 The front axle center points motion simulation diagram from different starting points to different forward horizontal line

將車輛參數代入式(8)中，可得Ymin=1 464.07 mm，即車身縱向中心線離車位上邊線的最小距離應不小于上述值，且進行仿真時需保證前進水平線與車位外邊線距離大于此值。分別選取前進水平線的縱坐標為5.25 m和6.00 m(下文簡稱為前進水平線5.25和前進水平線6.00)進行分析。

圖19 不同起點前進修正至不同前進水平線的前軸中點運動軌跡Fig.19 The front axle center points trajectory from different starting points to different forward horizontal line

為證明該方法的適用性，選取三種泊車因素，保持其中兩項因素相同，改變另一項因素進行驗證。這三種泊車因素分別為：起始點位置與角度、前進水平線的選取位置、車位長度。將汽車的起止點選取為起點A(0，3.997 6 m)和起點B(0，4.650 0 m)。

對于前進水平線5.25，從起點A和起點B出發，分別進行運動過程仿真，如圖18a和圖18b所示，前車軸中心點的外部前進糾正軌跡分別見圖19中的曲線1和曲線2。對于前進水平線6.00，從起點A和起點B出發，分別進行運動過程仿真，如圖18c和圖18d所示，前車軸中心點的外部前進糾正軌跡分別見圖19中的曲線3和曲線4。

圖19中，曲線1表示起點(0，3.997 6 m)到達前進水平線5.25時的前車軸中點運動軌跡，曲線2表示起點(0，4.650 0 m)到達前進水平線5.25時的前車軸中點運動軌跡，曲線3表示起點(0，3.997 6 m)到達前進水平線6.00時的前車軸中點運動軌跡，曲線4表示起點(0，4.650 0 m)到達前進水平線6.00時的前車軸中點運動軌跡。

對上面兩個起點和兩條前進水平線的路徑利用CarSim軟件進行仿真后，將所得的數據點導入MATLAB軟件進行曲線繪制以及運動分析，結果表明：汽車在尋找空車位時所停位置與車位的位姿關系不會影響汽車的外部前進動作，汽車將自動行駛至前進水平線，且當前進水平線的縱坐標距離大于最小距離Ymin時，汽車將避過泊車死區，且一定能夠完成泊車入位動作。

對比本文所述的泊車死區問題，目前的自動泊車系統也有少量涉及，文獻[14]中的泊車可行區域如圖20所示，通過MATLAB仿真，當汽車后車軸中心點位于深色區域，汽車能夠倒車泊入車位，但該種泊車模式并不具備本文介紹的向前行駛修正到非泊車死區的功能，因此不具有主動避開泊車死區的能力。

圖20 自動泊車倒車可行區域(非泊車死區區域)[14]Fig.20 Automatic parking reversing feasible area (non-parking dead zone)[14]

對于前進水平線5.25和6.00，當汽車分別以這兩條線為起點開始倒車至車位內，其運動過程仿真分別如圖21a和圖21b所示，后車軸中點的運動軌跡和前車軸中點的運動軌跡分別如圖22中的曲線5～曲線8所示。

(a)前進水平線5.25倒車至車位

(b)前進水平線6.00倒車至車位圖21 前進水平線5.25和6.00倒車至車位運動仿真圖Fig.21 Simulation diagrams of forward horizontal line 5.25 and 6.00 reversing to the parking movement

圖22中，曲線5表示從前進水平線5.25起點倒車至車位時后車軸中點的運動軌跡，曲線6表示從前進水平線5.25起點倒車至車位時前車軸中點的運動軌跡，曲線7表示從前進水平線6.00起點倒車至車位時后車軸中點的運動軌跡，曲線8表示從前進水平線6.00起點倒車至車位時前車軸中點的運動軌跡。

圖22 不同前進水平線倒車至車位時前后車軸中心點的運動軌跡Fig.22 The trajectories of the center points of the front and rear axles with different forward horizontal lines

對上文所述的從兩條前進水平線起點倒車至車位內的過程進行分析，結果表明：汽車外部倒退過程的軌跡與前進水平線的位置有關，兩者形狀相同，但切入入位基準線的位置點不同；入位基準線僅有一條，且與汽車的外形參數有關，汽車在進入入位基準線后，進行內部倒退過程動作時，將有且僅有一條泊車軌跡，該泊車軌跡與車位長度有關。

針對單步平行泊車所需要的最短車位長度，文獻[13]基于B樣條理論的主動泊車方法，給出了最小車位長度的計算公式如下：

M=

(17)

將汽車參數值代入式(17)，得出所需車位最小長度為5.67 m，而按照本文規劃方法用CarSim仿真時的車位長度為5.00 m，可見本文所述的自動泊車軌跡規劃方法可以減小所需最小車位長度的11.82%。

3.2 實驗驗證

搭建了基于stm32的智能實驗車，如圖23所示，通過WiFi與PC端相連，PC端用于分析采集的車位信息，將處理的結果傳送回stm32開發板，由stm32開發板控制實驗車的運動。

圖23 模擬實驗車Fig.23 Simulation of experimental vehicles

3.2.1平行車位實驗驗證

通過坐標紙可以快速地將實驗車放在所期望的初始位置，例如將實驗車的初始位置放置在縱坐標為100 mm處，如圖24所示。模型實驗車參數如表3所示。

圖24 實驗車初始位置的縱坐標為100 mmFig.24 The ordinate of the experimental vehicle initial position is 100 mm

表3 模型實驗車參數mm

實驗中，將車速設定為120 cm/min，每次停止時均對電動機加入鎖止信號以防止實驗車因慣性而移動。以坐標紙的左下角定位原點，通過計算，得到實驗車所要經過的必須點，并控制電機驅動時長，來驗證實驗車能否泊入車位。其中，車位的上邊線的起止坐標點分別為(100 mm，0)和(500 mm，0)。實驗車前車軸中心點的初始位置為(300 mm，100 mm)，前進水平線選取為150 mm。將前車軸中心點的經過位置點、電機開啟時長分別標入表4，實驗車將自動向左前行駛至選取的前進水平線，如圖25所示。

表4 實驗車前進控制

圖25 實驗車向左前行駛至縱坐標為150 mm的前進水平線Fig.25 The experimental vehicle drives to the forward horizontal line towards the left with anordinate of 150 mm

當實驗車向前行駛到達選取的前進水平線后，開始執行向后倒車泊入車位動作，此時以實驗車的后車軸中心點為位置點，由于軸距為198 mm，即起點由(950 mm，150 mm)變為(752 mm，150 mm)，將后車軸中心點的經過位置點、電機開啟時長分別列入表5，倒車至入位基準線的位置如圖26所示，倒車直至完全泊入車位如圖27所示。

表5 實驗車倒車控制

圖26 實驗車倒車至入位基準線Fig.26 The experimental vehicle is reversing to the entry baseline

圖27 實驗車完全進入車位Fig.27 The experimental vehicle is completely in the parking space

3.2.2垂直車位實驗驗證

對于平行車位的向前自動行駛，已證明其可行性，因此對于垂直車位則不再進行自動前行部分的實驗，直接將實驗車的后車軸中心點放至在坐標紙的(660 mm，300 mm)處，車位外邊線的起止坐標點為(180 mm，0)和(420 mm，0)，按表6依次進行控制，將實驗車泊入空車位內。對于垂直車位的實驗如圖28所示。

通過控制實驗車的運行軌跡，可以保證實驗車泊入車位內，因此可以證明提出方法的可行性。

表6 實驗車倒車控制

(a)實驗車向后倒車運動 (b)右打滿向后運動

(c)打正方向向后倒車 (d)倒車至完全進入車位后停止圖28 模擬垂直車位自動泊車Fig.28 Analog vertical parking automatic parking

4 結論

(1)設計了一種汽車自動泊車系統的外部路徑規劃方法，使汽車通過前進行駛動作修正汽車與車位的位姿關系，從而避免泊車死區。

(2)規劃了一條以入位基準線為基礎的泊車線路，避免了自動泊車系統在泊車過程中不斷反饋信息進行路徑修正的方式，減少系統計算量。

(3)通過將垂直、傾斜車位與平行車位的路徑規劃進行類比，驗證了該自動泊車方式能夠適應不同的車位類型。

(4)仿真結果表明，優化泊車路徑在平行車位的應用中可以使所需車位長度減小11.82%。