基于二段多項式的窄空間平行泊車路徑規劃方法①

熊 瑩,毛雪松

1(武漢科技大學 計算機科學與技術學院,武漢 430065)

2(武漢科技大學 智能信息處理與實時工業系統湖北省重點實驗室,武漢 430065)

3(武漢科技大學 信息科學與工程學院/人工智能學院,武漢 430081)

相比于垂直泊車,平行泊車對駕駛員難度更大,特別當車位長與車身長比值較小的情況下,很難完成連續轉向的泊車任務.泊車的路徑規劃包括正向駛入和倒車入庫,其中正向駛入的方法要求車位較長.當車位較短時,通常使用倒車入庫的方法.當車身長接近車位長時,為減少泊車過程中的檔位切換,在倒車入庫前需確定最佳倒車起點位置與方向角(統稱為位姿).本文基于倒車入庫常用的二段泊車法,即在前進過程中預先調整車輛的方向角,再倒車進入泊車位的方法,提出使用二段五階多項式,分別表示前進路段和倒車路段,實現在有限空間范圍內的自動泊車.

自動泊車路徑規劃常用的方法包括幾何路徑法、基于地圖的搜索法以及基于模糊控制的方法等.其中幾何路徑法包括使用圓弧與直線連接生成路徑法[1,2]、B 樣條曲線法[3,4]、五階多項式法[5–7]等.這些方法沒有考慮前行過程中調整車輛方向角,而是假定倒車的起始位姿與車位橫向方向平行,因此需要更大的橫向空間.在車位長度較短的情況下,不預先調整方位角有可能找不到連續泊車路徑.圓弧與直線連接的方法產生的路徑存在曲率不連續,泊車過程中需要多次停車調整方向盤轉向角,輪胎磨損大的問題.使用B 樣條曲線產生的路徑,雖然能夠滿足曲率連續的要求,但由于方程的約束條件以及待求參數量很多,求解過程復雜且運算量大.

建立于地圖搜索的方法與車輛行駛路徑規劃方法類似,包括常用的A*、D*、RRT[8–11]等,這些方法將地圖分割為小的柵格,通過建立啟發函數在柵格中搜索到一條長度最優的無碰撞路徑,然后選擇使用曲線對該路徑平滑得到曲率連續路徑.首先,柵格的大小對算法的計算效率影響很大;其次,路徑搜索中沒有考慮車輛運動學特征,即使經過平滑,在空間狹窄的情況下,車輛也不一定能夠跟蹤所得到的路徑.基于控制的方法則根據駕駛員的駕駛經驗,整理出一套控制規則,控制泊車過程中的轉向角變化.這種方法在泊車過程中需要多次改變車輛的行駛速度和轉向角、多次切換行駛方向,對車的前后所需要的空間較大.

本文根據駕駛員泊車經驗,將泊車過程分解為前進調整方位角和倒車入庫兩個過程,對這兩個過程分別使用五階多項式規劃最優路徑,相比于前述文獻中的方法,本文實現在更短的空間內泊車,且實現方向盤的連續轉向.

1 仿真場景與車輛模型

考慮縱列駐車停車場位于車輛右側,停車位長5.5 m,寬2 m,相鄰停車位之間間隔20 cm,符合通常停車位的標準,如圖1所示.在車輛長度較大,即泊車位相對較小的情況下,為了避免車輛多次切換檔位,通常按圖1所示,在倒車入庫前調整自身的位置和方向角(統稱為位姿).在前進調整位姿與倒車入庫過程中,車輛不得觸碰相鄰車位,且不能觸碰自身停車位的路沿邊界.倒車入庫后要求車身與車位長邊平行,如果方向盤轉向角不為零,可以在泊車完成后增加一個調整方向盤的步驟.

圖1 平行泊車仿真場景

泊車過程中,車輛的速度很低,因此可以不考慮側滑等因素,車輛的運動采用自行車模型[12].

這里(x,y)表示車輛后軸中心位置處的坐標,θ為方向角,定義為車頭方向與x軸正方向之間的夾角,δ表示方向盤的轉向角,l表示前后輪之間的軸距,v和分別表示車的徑向速度和方向盤的轉動角速率,w表示方向盤的轉向速率.前3 個方程表示車輛的運動狀態更新,最后一個方程表示對方向盤轉向角的控制,該式本身為冗余項,需要根據搜索得出的無碰撞路徑,使用專門的轉向角控制算法來控制車輛行駛過程中的轉向角,例如Pure Pursuit[13]、Stanely[14]等算法.規劃無碰撞路徑算法中,考慮一定的安全裕度同時為簡化運動學計算,用矩形來定義汽車的外觀.

算法仿真中使用的車輛特征參數如表1所示.

表1 車輛特征參數

表1中,軸距表示前后軸之間的垂直距離,前懸為車頭中心到前軸中心的距離,后懸表示車尾中心到后軸中心的距離.泊車完成后,車位前后方自由空間長度總和為50 cm.

2 二段多項式泊車路徑規劃方法

2.1 5 階多項式連接相鄰路點

有很多文獻使用5 階多項式作為連接汽車起點(x0,y0)與終點(xf,yf)之間的路徑規劃的方法,所得到的路徑滿足起點和終點的約束條件,且二階連續,車輛控制算法對路徑的跟蹤精度高,但是起點與終點路徑并不總是能夠通過一條多項式曲線連接.如圖1所示,車輛行駛路徑包括前進路徑與后退路徑,雖然整條路徑是連續的,但不滿足二階連續,因此不能使用單個多項式來表示從起點到終點的路徑.對車輛的行駛路徑分段,保證每段路徑二階連續,且相鄰路徑之間滿足一階連續的條件,則稱每段路徑的端點為一個路點.圖1的路徑中包含3 個路點,分別稱為起點、中間點和終點.考慮車輛的方向角和轉向角,分別定義3 個點的位姿為(x0,y0,θ0,δ0)、(xi,yi,θi,δi)和(xf,yf,θf,δf).

從路徑上看,車輛的方向角θ即為路徑對x的導數dy/dx,轉向角δ與路徑之間滿足關系:

其中,l為車的軸距,κ為路徑的曲率,定義為:

在一階導數已知的情況下,它是一個僅與二階導數有關的量,因此根據曲線點的參數將車輛位姿修改為(x,y,y′,y′′).

由于Stanely 方法控制車輛方向盤轉向角需要計算前輪與路徑之間的最小距離,因此規劃前進路徑時,以前輪中心所能到達的位置為前進路徑的終點.倒車時,為減小控制誤差,使用基于參數的方法控制方向盤轉向角,從后輪中心所在位置處為起點計算第二條多項式路徑.將這兩個點均稱為泊車路徑的中間路點,分別記為(xi1,yi1,y′i1,y′′i1)和(xi2,yi2,y′i3,y′′i3).二段多項式法生成的前進路徑、倒車路徑以及各路點的表示如圖2所示.

圖2 前進與倒車路徑以及路點標識

2.2 多項式系數的求解

設連接路點的起點與終點多項式表示為:

將空間內無碰撞路徑搜索問題轉化為多項式系數的求解問題.

取泊車路徑的起始位置坐標為(x0,y0),位于待泊車位左后方,起始位置處的方向角和轉向角均為0,因此:

終點坐標為(xf,yf),滿足:

其中,C為車轉向角限定的常數.

泊車路徑包括前進與倒車兩段路徑,每段路徑均由一條多項式曲線連接兩個路點得到.式(5)確定前進路段的起點,式(6)確定倒車路段的終點.根據圖2,在中間點已知的情況下,即可根據式(4)計算連接起點和終點的兩段多項式.

將每段曲線兩個路點的位姿給出的6 個限定條件代入式(4),得出在r=5的情況下,可以找到連接路點起點和終點的唯一曲線.令:

圖3 高階系數修正的泊車路徑

將6 個限定條件分別代入式(7)～式(9),求出曲線方程的系數為:

這里,

式(10)和式(11)中的下標l0和lf分別表示路點的起點和終點.對于平行泊車,兩個路點上車的方向角之差不會很大,因此B矩陣不會出現奇異解.

2.3 中間點的搜索

圖2中間點(xi1,yi1)和(xi2,yi2)的搜索以及在中間點位置處的最佳方向角和轉向角的計算是泊車路徑規劃的關鍵.可以根據駕駛經驗,確定點(xi1,yi1)存在的區域范圍以及方向角范圍.為了使前進與后退過程中方向盤的連續轉向,可取點(xi1,yi1)處的轉向角δ為0,即y′′|x=xi1=0.

使用搜索算法找到點(xi1,yi1)以及方向角后,根據車體的位姿求出點(xi2,yi2)的位姿為:

作為倒車路段的起點.

2.4 邊界條件的數學模型

要求在前進過程中車的v1v2邊、v2v3邊不能與相鄰車位的a0b0邊相接觸,倒車過程中v1v2邊、v2v3邊不能與a2b2邊、a2d2邊接觸,另外v1v2邊、v1v0邊不能與c1d1邊接觸.轉化為數學模型,即要求前進路段:

其中,v2(x)、v2(y)分別表示頂點v2的x坐標和y坐標.

倒車路段要求:

2.5 目標函數的定義

按照兩個路段的曲率最優原則定義目標函數為:

其中,κ1和κ2分別為前進路段和倒車路段曲線各點的曲率.α為對目標函數貢獻的權重系數.

3 仿真結果

取待泊車位的4 個頂點坐標為(3,0)、(8.5,0)、(8.5,2)、(3,2),車的起始坐標位置為(2.5,3.5),方向角和轉向角均為0.泊車位置為(3.9,1.05),方向角為0,轉向角設為方向盤最大轉向范圍內的一個數,這里取34°.在x從9 米到14 米、y從4 米到6 米、θ從0 到π/4的范圍內搜索最優泊車路徑,得到的泊車路徑如圖4所示.由于通常期望目標函數中倒車的曲率貢獻值大一些,因此仿真中定義權重因子α=0.1.從泊車過程的行駛軌跡看,二段多項式法可以正確實現平行泊車,且泊車過程中轉向角連續變化.前進過程中適當調整車的方向角,可以使倒車過程更容易完成.

圖4 最優二段多項式泊車路徑以及泊車行駛軌跡

泊車時,期望將車盡可能停向車位內側,因此取終點位置處的y值分別為1.01 m、1.02 m、1.03 m、1.05 m等幾種情況,在xi1從9 m 到14 m、yi1從4 m 到6 m、θ從0 到π/4的范圍內搜索存在安全路徑的中間點.某一中間點處,可能有多個θ值對應安全路徑,在這些方向中選擇一個θ值,使路徑的曲率最小.存在安全路徑的中間點區域如圖5所示.從圖可以看出,車輛終點位置越靠近外側,中間點存在區域就越大.當xi1值較小時,安全路徑的中間點θ值較大,這與駕駛經驗吻合.

取yf=1.05 m,對應于圖5(d)各點處泊車路徑的曲率如圖6所示,可見對應于每一個xi1,存在一個最佳yi1和方向角.同樣對應于每一個yi1值,存在一個最優xi1值,使得曲率最低.另外,對于固定xi1或yi1,觀察θ對另一變量也具有下凸性,因此,在搜索區域范圍內式(15)定義的T函數具有下凸性,可使用常規優化算法在搜索空間內找到最優解.例如選取前輪中間點位姿xi1=18.0 m,yi1=10 m和θ=π/4,利用最速下降法搜索,設置收斂精度為2 cm,程序運行于Intel(R)Core(TM)i5-4460 CPU@3.2GHz,內存8 GB,得到最優解的時間約xi1=14.6 m,yi1=7.3 m和θ=0.79,所用時間為0.73 s.

圖5 泊車終點位置的縱向坐標對中間點存在區域的影響,箭頭指向該位置處的最佳方向角

圖6 對應于圖5(d)各點的安全路徑曲率

前向路徑上,使用Stanely 算法控制轉向角.當參考路徑為多項式時,低速情況下Stanely 具有良好的跟蹤性能.倒車路徑上,根據計算得到路徑的位置、一階和二階導數,計算出在每一點位置處的方向盤轉向角,將其保存為倒車控制文件(Speed Profile).仿真中,設置仿真時間步長參數為0.025 s,倒車速度為0.5 m/s,對路徑的跟蹤誤差不到0.2 cm,因此通過分段得到的路徑可以運用于車輛的平行泊車中.

本文通過二段曲線泊車法實現車位長度5.5 m 情況下的連續泊車,車位長與車身長的比值約為1.2,與相關文獻中已實現的車位長與車身長的比值對比,結果如表2所示.

表2 車位長/車身長的比較

4 結束語

本文提出了一種基于二段五階多項式的方法完成平行泊車,給出存在安全泊車路徑的中間點存在范圍,設計代價函數并驗證其下凸性.基于通常平行車位的尺寸以及B 型家用轎車尺寸,執行泊車仿真.仿真結果表明,通過調整中間點方位角的方法,可以在狹窄的空間范圍完成平行泊車任務.相比于過去假定倒車起點位置處車輛的方向與車位平行的方法,本文所提的方法更加符合駕駛員的駕駛習慣,實現更窄空間的平行泊車,可以為智能駕駛車輛泊車系統的開發提供通用的方案.