基于改進(jìn)β樣條理論的自主泊車路徑規(guī)劃

吳飛龍，郭世永

(青島理工大學(xué) 機(jī)械與汽車工程學(xué)院，山東 青島 266520)

隨著汽車保有量的增加，城市道路交通壓力日益增加，駕駛員行車與停車的環(huán)境也變得越來越復(fù)雜[1]，泊車作為駕駛行為的重要組成部分，已經(jīng)成為安全事故高發(fā)的駕駛環(huán)節(jié).自主泊車技術(shù)可以減小駕駛員在面對(duì)狹小空間泊車時(shí)的難度，同時(shí)也提高了駕駛員舒適性和泊車安全性[2].自主泊車又稱為代客泊車或一鍵泊車，既可以做到讓駕駛員可以在指定地點(diǎn)處召喚停車位上的車輛，又能讓當(dāng)前駕駛的車輛停入指定或隨機(jī)的停車位.其關(guān)鍵技術(shù)主要包括車位識(shí)別、路徑規(guī)劃、路徑跟蹤控制(執(zhí)行控制)[3-4].

自主泊車的路徑規(guī)劃包括全局路徑規(guī)劃和局部空間路徑規(guī)劃[5].國(guó)內(nèi)外學(xué)者已對(duì)此進(jìn)行了大量的研究，文獻(xiàn)[6]基于改進(jìn)后的五階多項(xiàng)式泊車路徑，通過針對(duì)性地設(shè)計(jì)罰函數(shù)，采用遺傳算法計(jì)算得到最佳泊車路徑和最小泊車空間.文獻(xiàn)[7]提出一種經(jīng)回旋曲線優(yōu)化的泊車路徑,解決了兩段式路徑規(guī)劃在切點(diǎn)處曲率不連續(xù)問題，并給出相應(yīng)的路徑計(jì)算方法.文獻(xiàn)[8]通過建立前進(jìn)和后退的模糊規(guī)則，來控制汽車的轉(zhuǎn)向角，從而實(shí)現(xiàn)平行泊車的軌跡規(guī)劃.文獻(xiàn)[9]考慮車位的幾何約束，采用基于最小轉(zhuǎn)彎半徑的圓弧曲線與直線連接的方式，來求得不同工況下的泊車路徑.文獻(xiàn)[10]基于模糊控制理論，設(shè)計(jì)了三段式的平行泊車控制算法.文獻(xiàn)[11]以最短路徑為目標(biāo)，采用遺傳算法對(duì)垂直泊車路徑進(jìn)行優(yōu)化，可擴(kuò)大泊車初始區(qū)域.文獻(xiàn)[12]綜合考慮平行泊車碰撞情況,以泊車終點(diǎn)處車輛航向角最小化為目標(biāo)，建立了含避障約束、車輛轉(zhuǎn)向盤轉(zhuǎn)角及角速度約束等多個(gè)非線性約束的泊車路徑函數(shù)，基于β樣條理論獲得曲率連續(xù)、變化緩慢的泊車路徑.

筆者提出一種基于改進(jìn)β樣條理論法的自主泊車路徑規(guī)劃方法，建立泊車運(yùn)動(dòng)學(xué)模型、自身物理?xiàng)l件約束、路徑避障約束、車輛轉(zhuǎn)向盤轉(zhuǎn)角及角速度約束，根據(jù)實(shí)際泊車場(chǎng)景，對(duì)3種工況下的平行泊車模式進(jìn)行仿真分析，驗(yàn)證提出的路徑規(guī)劃算法的可行性.

1 車輛運(yùn)動(dòng)學(xué)模型

由于泊車過程中車速較低，輪胎側(cè)滑可以忽略不計(jì)，因此采用純滾動(dòng)車輛運(yùn)動(dòng)學(xué)模型[5].如圖1所示，其中：l為軸距；W為車身寬度；f、r分別為前懸、后懸長(zhǎng)度；P為車輛后軸中心；O′為瞬時(shí)轉(zhuǎn)向中心；θ為車輛航向角；φ為前輪等效擺角.

圖1 車輛運(yùn)動(dòng)學(xué)模型

根據(jù)運(yùn)動(dòng)學(xué)模型，可以得到泊車的運(yùn)動(dòng)學(xué)微分方程：

(1)

式中：v為車速；a為加速度；ω為前輪擺角角速度.

車輛在泊車過程中，除了要考慮車輛運(yùn)動(dòng)學(xué)方程約束，還要考慮自身物理?xiàng)l件約束，其對(duì)狀態(tài)量和控制量的約束如下：

(2)

式中：vm、am分別為泊車時(shí)的最大車速和最大加速度；φm、ωm分別為最大前輪擺角及其對(duì)應(yīng)角速度.

2 平行泊車路徑規(guī)劃

泊車把握一個(gè)原則:不管是分幾步泊車，首先要保證車輛在末位置不僅要滿足車位底邊的碰撞約束，還要使航向角盡可能小(有利于后續(xù)的泊車步驟).先設(shè)定末位置迭代航向角，泊車軌跡如圖2所示，dx、dy為相應(yīng)的安全閾值.首先設(shè)置航向角為0°，是否可以規(guī)劃一條無碰撞路徑，如果沒有，緩慢增大航向角，直到找到一條滿足碰撞約束的路徑.

圖2 泊車軌跡

2.1 碰撞點(diǎn)的預(yù)判

通常情況下，在車輛剛要駛?cè)胲囄磺?見圖3a)，存在2個(gè)碰撞點(diǎn)：一是車頭外側(cè)A點(diǎn)與周圍物體的碰撞；二是車輛內(nèi)側(cè)CD邊與車位頂點(diǎn)O的碰撞.在車輛剛好駛?cè)胲囄坏杰囕v完全完成泊車的過程中(見圖3b)，存在3個(gè)碰撞點(diǎn)，分別是車頭內(nèi)側(cè)的D點(diǎn)、車尾兩側(cè)的碰撞B點(diǎn)和C點(diǎn).

圖3 泊車碰撞示意圖

2.2 三段式路徑規(guī)劃算法設(shè)計(jì)

為使車輛行駛路徑的曲率及曲率變化率較小，采用三段式的泊車路徑，即圓弧+切直線+圓弧模式.泊車動(dòng)態(tài)圖如圖4所示.

圖4 泊車動(dòng)態(tài)圖

紅圓為車輛頂點(diǎn)C走過的圓弧，設(shè)定車輛的末位姿，即車輛完全駛?cè)胲囄磺一卣?，車輛BC邊、CD邊與車位的安全距離分別為dx和dy,滿足車輛B、C點(diǎn)的碰撞約束.由圖4可以確定B、C點(diǎn)的坐標(biāo)分別為(XB,YB)、(XC,YC).假設(shè)車長(zhǎng)為L(zhǎng)，可求得車輛航向角，后軸中點(diǎn)P的坐標(biāo)(XP,YP)，即

(3)

由P點(diǎn)坐標(biāo)可得圓心O1的坐標(biāo)(XO1,YO1),其坐標(biāo)值為

(4)

式中：rmin為車輛最小轉(zhuǎn)彎半徑.

由后軸中心點(diǎn)坐標(biāo)可求得車輛D點(diǎn)坐標(biāo)(XD,YD)，即

(5)

由此可求得車輛D點(diǎn)軌跡圓的半徑R2以及圓心O1到車位右頂點(diǎn)O的距離LO1O.若LO1O-R2≥d，d為安全閾值,則滿足車輛點(diǎn)D與車位右邊的碰撞約束，否則不滿足，需進(jìn)行下一步計(jì)算.

計(jì)算圓心O1到車位右邊距離為L(zhǎng)O1O+d的點(diǎn),如圖5所示，假設(shè)E點(diǎn)恰好為此臨界點(diǎn).O點(diǎn)坐標(biāo) (XO,YO)已知，分別求線段O1E與y軸平行半徑的夾角θ1和線段O1D到與y軸平行半徑的夾角θ2，即

圖5 泊車空間約束示意圖

(6)

由此可得角θerror=θ1-θ2.此差值映射到以后軸中心P為軌跡的圓(藍(lán)色的圓)上，也就是說P點(diǎn)的軌跡圓的可行駛范圍是圓心角為[θP,θP+θerror].求得P點(diǎn)軌跡圓在角θP+θerror的x軸坐標(biāo)值X*作為判斷是否滿足碰撞約束的閾值，即

X*=XO1+R1cos(3π/2+θP+θerror).

(7)

泊車初始位置如圖6所示，車輛與庫右邊的安全閾值dh確定(y坐標(biāo)確定)，沿后軸中心線向右迭代泊車起始點(diǎn)不同的x坐標(biāo)，計(jì)算第1段圓弧和切線方程.判斷是否滿足第2碰撞約束.

圖6 泊車初始位置

泊車軌跡規(guī)劃如圖7所示，車輛泊車起始位置后軸中心P1坐標(biāo) (XP1,YP1),由圓心O1坐標(biāo)和圓心O1′坐標(biāo)(XO1′,YO1′)得與切線相交中點(diǎn)L的坐標(biāo)(XL,YL),可進(jìn)一步得到線段LO1的長(zhǎng)度LLO1,θa、θb、θe以及切點(diǎn)K的坐標(biāo) (XK,YK)如下：

圖7 泊車軌跡規(guī)劃圖

(8)

式中：R1為后軸中心P的軌跡圓半徑.

圓O1與圓O1′半徑相等，即R1=R1′，同理可得切點(diǎn)H的坐標(biāo) (XH,YH)，即

(9)

最終可得切線HK的方程.

從圖7可以看出，車輛CD邊與庫頂點(diǎn)O的碰撞可能在圓弧階段或者切線階段，因此，應(yīng)當(dāng)判斷其臨界點(diǎn).經(jīng)分析，當(dāng)過O點(diǎn)作垂直于切線的直線，當(dāng)與切線的交點(diǎn)恰為切線與圓的切點(diǎn)時(shí)，為臨界點(diǎn).易求得交點(diǎn)Q的坐標(biāo)為(XQ,YQ)，后軸的延長(zhǎng)線與車身一側(cè)的交點(diǎn)為J.當(dāng)XH≥XQ時(shí)，碰撞約束的判斷條件為L(zhǎng)OQ≥0.5W+d；當(dāng)XH

程序流程圖如圖8所示.

圖8 泊車程序流程

三段式路徑規(guī)劃算法既能根據(jù)車庫大小和車輛尺寸，通過迭代自適應(yīng)找到泊車起始區(qū)域和最佳起始泊車點(diǎn)，同時(shí)又保證第1次入庫車輛航向角相對(duì)最小，并且能形成一條完整的無碰撞泊車路徑，以便后續(xù)泊車動(dòng)作，但路徑曲率的連續(xù)性不夠好，且曲率變化率相對(duì)較快，難以控制，曲率的變化不足以滿足車輛運(yùn)動(dòng)學(xué)以及EPS(electric power stee-ring)的性能要求.

3 基于改進(jìn)β樣條曲線的路徑優(yōu)化

路徑優(yōu)化需滿足以下3個(gè)目標(biāo)：① 優(yōu)化之后依然是無碰撞路徑；② 優(yōu)化后的路徑曲率沿樣條必須有界；③ 其曲率的變化必須滿足車輛運(yùn)動(dòng)學(xué)以及EPS的性能要求.β樣條曲線是階導(dǎo)數(shù)連續(xù)的分段曲線(如一階和二階)，即使在相鄰段的節(jié)點(diǎn)處也是如此，這種曲線由一組控制頂點(diǎn)所確定，這些頂點(diǎn)按順序連接就構(gòu)成了一個(gè)控制多邊形.曲線趨向于模擬控制多邊形的整體形狀.

現(xiàn)讓控制多邊形由控制點(diǎn)序列 [v-2,v-1,v0,v1,…,vm]所組成，每個(gè)點(diǎn)由它的笛卡爾坐標(biāo)所確定.曲線將由m-2段組成，第i段曲線記為Qi(u),如圖9所示，它由4個(gè)控制頂點(diǎn) (Vi+r,r=-2,-1,0,1)所確定，這段曲線上的任何點(diǎn)都是這些控制點(diǎn)的加權(quán)平均值，第i段曲線上點(diǎn)的坐標(biāo)為

圖9 樣條線和控制點(diǎn)

(10)

式中：br為權(quán)重系數(shù)，是評(píng)估在任何參數(shù)u范圍內(nèi)以及每個(gè)形狀參數(shù)β1、β2下的標(biāo)量函數(shù)，形狀參數(shù)決定了曲線模擬控制多邊形的方式.

有研究[13]表明，當(dāng)需要平滑的曲率變化時(shí)，三次樣條曲線是最佳選擇.在這種情況下，β1=0,β2=0，權(quán)重系數(shù)定義為

(11)

3.1 控制點(diǎn)的選擇

β樣條曲線最重要的特性之一是保凸性，這一特性保證了曲線經(jīng)過的表面以控制多邊形為界,如圖10所示.

圖10 圓弧上的控制點(diǎn)

如果控制點(diǎn)在一段圓弧上面，可以推導(dǎo)出結(jié)論：控制點(diǎn)之間的距離是有規(guī)律的，一段β樣條曲線段的控制點(diǎn)在半徑為R的圓弧C上，如圖10所示，如果控制點(diǎn)之間的距離為δ，那么δ≤πR/6,β樣條曲線段在圓C′的外面，那么圓C′的半徑為

(12)

進(jìn)一步通過試驗(yàn)表明：曲線段的曲率半徑上界為R,下界為R′[13].因此，如果曲線段的曲率半徑的值要保持在R與R′之間，那么，δ應(yīng)該滿足：

(13)

這個(gè)約束也保證了曲線段在圓弧之間.

優(yōu)化后的軌跡如圖11所示，Ra、Rb分別為R1的最大值和最小值；Rc、Rd分別為R2的最大值和最小值.

圖11 軌跡優(yōu)化圖

在R1段圓弧上取控制點(diǎn)，由β樣條曲線的保凸性，原路徑無碰撞，那優(yōu)化之后的路徑也無碰撞.在R2段圓弧取控制點(diǎn)，Rd為避免碰撞的最小轉(zhuǎn)彎半徑，因此可得

(14)

δ≤min(δ1,δ2,δ3),

(15)

式中：δ1為第1個(gè)控制點(diǎn)與第2個(gè)控制點(diǎn)間的距離；δ2為第2個(gè)控制點(diǎn)與第3個(gè)控制點(diǎn)間的距離；δ3為第3個(gè)控制點(diǎn)與第4個(gè)控制點(diǎn)間的距離，后面以此類推.

為保證生成的樣條可被執(zhí)行，第1段及第2段圓弧生成的樣條曲線的曲率半徑要大于車輛自身的最小轉(zhuǎn)彎半徑Rmin，即

(16)

3.2 轉(zhuǎn)速的約束

盡管β樣條曲線生成的路徑是曲率連續(xù)的，但

曲率的變化應(yīng)該足夠滿足有效路徑跟蹤的平順性的要求，通過試驗(yàn)表明：按β樣條曲線進(jìn)行平行泊車的控制策略，如圖12所示，其曲率的變化滿足以下線性關(guān)系：

圖12 樣條曲線曲率

(17)

式中：ρ為曲率；ρ1為S0至S1間的曲率；ρ2為S4至S5間的曲率；s為弧長(zhǎng)；k1、k2、k3為曲率系數(shù).

如果v表示汽車行駛的線速度，其按照樣條曲線行駛的曲率改變的速度滿足：

(18)

式中：[t0,t1]、[t2,t3]、[t4,t5]為曲率改變的時(shí)間段.

曲率變化的速率為

(19)

(20)

根據(jù)式(18)和式(20)，轉(zhuǎn)向機(jī)構(gòu)要正確遵循曲率的變化，δ需滿足以下約束：

(21)

式中：k1、k2、k3分別取6.6、3.2、6.6.

δ的選擇及轉(zhuǎn)角變化如圖13所示.圖13a表明該控制策略沒有遵循式(21)，因而導(dǎo)致路徑跟蹤的精度不高.由于汽車不能遵循轉(zhuǎn)向角的變化(見圖13b)，由圖13c可見，通過保持δ的值，并減小速度v的值(δ5,δ6,δ7的值減小)，路徑跟蹤的精度很高.由圖13e可以看出：通過保持速度v的值而增加δ的值直到它滿足式(21)的要求，其表現(xiàn)出來的控制效果也較好.

圖13 δ的選擇及轉(zhuǎn)角變化

3.3 δ的選擇

當(dāng)β樣條曲線生成后，δ的值應(yīng)該被使用，根據(jù)式(14)-(16)和式(21)，δ的值必須滿足：

max(δ5，δ6，δ7)=δvel≤δ≤δcol=min(δ1,δ2,δ3,δ4),

(22)

式中：δvel、δcol分別為控制點(diǎn)間距的上界和下界.

δvel被選擇后，要保證δvel選擇滿足約束δf≤δcol，δf為滿足約束條件后的控制點(diǎn)間距.

考慮到(δ1,δ2,δ3,δ4)主要與幾何約束有關(guān)，根據(jù)式(21)，(δ5,δ6,δ7)主要取決于車輛的速度和轉(zhuǎn)向系統(tǒng)的速度，這可能會(huì)造成δvel>δcol，換言之，它可能會(huì)得到一種連續(xù)曲率的控制策略，但是該策略不是無碰撞的.在這種情況下，為了獲得一個(gè)無碰撞的策略，δ應(yīng)選為δ=δcol.然而，根據(jù)式(21)，如果該準(zhǔn)則被應(yīng)用，由于在這個(gè)速度下，車輛轉(zhuǎn)向系統(tǒng)不能很好地執(zhí)行曲率的變化，原始速度的值一定會(huì)被改變.因此在這種情況下，為了使車輛盡可能正確地沿著樣條曲線路徑行駛，速度v的值應(yīng)該減小.因此，由式(21)可知，速度v的上界為

(23)

然后vmax=min(v1,v2,v3)被選為新的v值.

4 仿真分析

由Matlab/Simulink搭建仿真平臺(tái)，建立平行泊車多種場(chǎng)景驗(yàn)證基于改進(jìn)β樣條理論法規(guī)劃自主泊車路徑的有效性，采用某大眾車為研究對(duì)象，車輛參數(shù)及約束參數(shù)如下：l為2.578 m；f為0.918 m；r為1.076 m；W為1.769 m；vm、am分別為3 m·s-1、0.72 m·s-2；φm、ωm分別為0.56 rad、0.56 rad·s-1.規(guī)定車輛沿車道線水平向右為速度正方向.

1)平行泊車工況1,此工況下設(shè)定車道寬度為3.5 m,車位寬度為4.0 m,選(-1.70,-5.00)m作為后軸中心起始位置，此時(shí)，車身與兩側(cè)車道線保持平行，汽車終止的條件為完成泊車后，后軸中心點(diǎn)在車位中心線上，且車身與車位保持平行，如圖14所示，車輛滿足上述泊車要求，在泊車過程中并未發(fā)生碰撞.泊車工況1狀態(tài)量與控制量如圖15所示，狀態(tài)量和控制量均滿足約束條件.

圖14 泊車工況1路徑示意圖

圖15 泊車工況1狀態(tài)量與控制量

2)平行泊車工況2是停車場(chǎng)內(nèi)常見的平行泊車位，該工況下設(shè)定車位的寬度為1.9 m,車位的長(zhǎng)度位7.0 m,該工況下選(0.16,-1.00)m作為汽車后軸中心的起始位置,汽車初始位置時(shí)，車身與車位保持平行，汽車完成泊車的條件如下：汽車完成泊車時(shí)，汽車后軸中心點(diǎn)在車位中心線上,且車身與車位保持平行，如圖16所示.汽車很好地一次性連續(xù)地完成泊車動(dòng)作，且形成一條無碰撞的連續(xù)曲率路徑.泊車工況2狀態(tài)量與控制量如圖17所示，該工況下，車輛的狀態(tài)量和控制量均滿足約束條件.

圖16 泊車工況2路徑示意圖

圖17 泊車工況2狀態(tài)量與控制量

3)平行泊車工況3，該工況與工況2類似，均屬于常見的平行泊車位，該工況與工況2的區(qū)別在于汽車的起始位置不同，車位的長(zhǎng)度不同，此工況下設(shè)定(-0.86,1.60)m為泊車后軸中心的起始位置，汽車在初始位置時(shí)，車身與車位保持平行，汽車的終止條件與工況2完全一致，如圖18所示，由于汽車初始位置離車位很近，車位相對(duì)狹窄，汽車不能一次性完成泊車動(dòng)作，車輛在初始位置以及進(jìn)入車位內(nèi)都需經(jīng)過多次前進(jìn)和后退的調(diào)整，最終才完成整個(gè)泊車過程，但這一過程中均未發(fā)生任何碰撞現(xiàn)象.泊車工況3速度、加速度、前輪擺角角速度曲線如圖19所示，該工況下部分控制量未滿足約束條件，前輪擺角角速度超過最大值約束限制，這是由于車輛在車位內(nèi)部調(diào)整時(shí)，車位相對(duì)狹窄導(dǎo)致車輛短暫的在原地轉(zhuǎn)向.

圖18 泊車工況3路徑示意圖

圖19 泊車工況3速度、加速度、前輪擺角角速度曲線

改進(jìn)前后2種工況下車輛橫向位移對(duì)比如圖20所示.

圖20 改進(jìn)前后車輛橫向位移對(duì)比

由圖20a可以看出：工況2下，在車輛快要進(jìn)入車位的過程中，優(yōu)化后的泊車軌跡橫向位移比優(yōu)化前小，且泊車軌跡更平滑.由圖20b可以看出：工況3下，在車輛進(jìn)入車位后的過程中，優(yōu)化后的泊車軌跡橫向位移的變化要略小于優(yōu)化前.結(jié)合圖20和表1可知，3種工況下，整個(gè)泊車過程中，優(yōu)化后的路徑總長(zhǎng)要小于優(yōu)化前，且泊車結(jié)束后后軸中心點(diǎn)與車位中心線的橫向偏差優(yōu)化后都要明顯小于優(yōu)化前.

表1 改進(jìn)前后泊車性能對(duì)比

5 結(jié) 論

1)提出的泊車路徑規(guī)劃算法能指導(dǎo)車輛完成自主泊車，并最終到達(dá)指定的泊車位，且在自主泊車的過程中無碰撞發(fā)生.

2)提出的基于改進(jìn)β樣條理論法規(guī)劃的路徑曲率連續(xù)，曲率變化平緩，滿足車輛運(yùn)動(dòng)學(xué)以及EPS的性能要求，并且泊車效率高.

3)提出的算法泊車路徑總和減小了3%左右，泊車結(jié)束后后軸中心點(diǎn)與車位中心線的橫向偏差也大大減小.

4)能實(shí)現(xiàn)狹窄泊車位內(nèi)的自主泊車路徑規(guī)劃，但與真實(shí)的復(fù)雜泊車場(chǎng)景還有一定的差距，后續(xù)還需要考慮真實(shí)場(chǎng)景下的動(dòng)態(tài)避障的規(guī)劃.