基于聯(lián)合仿真的平行泊車控制策略

秦國(guó)慶，馮櫻

（湖北汽車工業(yè)學(xué)院 汽車工程學(xué)院，湖北 十堰 442002）

汽車數(shù)量的持續(xù)增長(zhǎng)使城市內(nèi)車位空間更加緊張，要求駕駛員擁有熟練的駕駛技巧才能在有限的空間內(nèi)順利泊車，否則容易引發(fā)車輛碰撞事故。而自動(dòng)泊車系統(tǒng)的應(yīng)用能夠大大降低泊車事故率，提高泊車安全性。國(guó)內(nèi)外學(xué)者針對(duì)自動(dòng)泊車系統(tǒng)的研究主要集中在車位識(shí)別、路徑規(guī)劃和路徑跟蹤控制器的設(shè)計(jì)問(wèn)題等方面。在車位識(shí)別技術(shù)上，沈崢楠［1］通過(guò)多傳感器融合建立了停車位識(shí)別模型，利用模糊推理自動(dòng)識(shí)別規(guī)則和不規(guī)則車位類型；江浩斌等［2］為解決自動(dòng)泊車系統(tǒng)車位檢測(cè)精度低的問(wèn)題，通過(guò)多個(gè)超聲波雷達(dá)和視覺(jué)傳感器數(shù)據(jù)融合，實(shí)現(xiàn)車位邊界的高精度探測(cè)。針對(duì)泊車路徑規(guī)劃方法，張持等［3］采用兩段相切圓弧的方式規(guī)劃平行泊車路徑，方法簡(jiǎn)單、計(jì)算效率高，但切點(diǎn)處存在曲率突變；杜文豪［4］在2段相切圓弧的基礎(chǔ)上提出多段式路徑規(guī)劃方法，有利于提高跟蹤精度、減小泊車位尺寸；黃江等［5］針對(duì)自動(dòng)泊車系統(tǒng)路徑規(guī)劃問(wèn)題，利用五次多項(xiàng)式對(duì)路徑做平滑處理，得到曲率連續(xù)且符合避障約束的泊車軌跡；李紅等［6-7］提出基于B樣條理論的平行泊車路徑規(guī)劃方法，改善了泊車路徑的跟蹤效果，但計(jì)算較為復(fù)雜；李茂月等［8］為避免自動(dòng)泊車過(guò)程中出現(xiàn)泊車死區(qū)，提出在車位外部汽車自動(dòng)向前行駛至前進(jìn)水平線的軌跡計(jì)算方法。對(duì)于泊車控制器的設(shè)計(jì)問(wèn)題，Li 等［9-10］使用模糊控制邏輯建立了自動(dòng)泊車系統(tǒng)控制器模型；郭孔輝等［11］針對(duì)泊車過(guò)程中車速控制失誤導(dǎo)致車輛偏離泊車路徑現(xiàn)象，提出了結(jié)合定點(diǎn)跟蹤的非時(shí)間參考路徑跟蹤控制策略，提高了泊車容錯(cuò)率；姜立標(biāo)等［12］結(jié)合非時(shí)間參考路徑跟蹤控制和終端滑?？刂品椒?，設(shè)計(jì)出基于趨近律的非時(shí)間參考終端滑模路徑跟蹤控制器；胡遠(yuǎn)志等［13］分析了泊車過(guò)程中遇到動(dòng)態(tài)障礙物的問(wèn)題，提出基于模型預(yù)測(cè)控制的自動(dòng)泊車控制方法?？梢钥闯觯F(xiàn)有泊車路徑存在曲率突變、難以跟蹤或者求解復(fù)雜度高、實(shí)時(shí)性較差的缺點(diǎn)；同時(shí)控制器跟蹤效果差導(dǎo)致泊車安全性降低，容易發(fā)生刮蹭等問(wèn)題。文中采用曲率連續(xù)且計(jì)算高效的等速偏移和正弦函數(shù)疊加曲線進(jìn)行泊車路徑規(guī)劃，用模型預(yù)測(cè)控制算法跟蹤期望路徑，并加入車位識(shí)別控制方法，設(shè)計(jì)泊車系統(tǒng)控制策略?；赑rescan 和Simulink 進(jìn)行聯(lián)合仿真，驗(yàn)證控制器的有效性。

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

車輛在泊車過(guò)程中車速不超過(guò)10 km·h?1，保持低速運(yùn)動(dòng)，不需要考慮輪胎的側(cè)偏特性，所以采用車輛運(yùn)動(dòng)學(xué)模型，如圖1所示。車輛后軸中心的軌跡可以體現(xiàn)車輛泊車過(guò)程的軌跡特征，以后軸中心點(diǎn)為參考點(diǎn)建立車輛的運(yùn)動(dòng)學(xué)方程：

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

式中：xr、yr分別為車輛后軸中心點(diǎn)橫、縱坐標(biāo)；x?r、y?r分別為后軸中點(diǎn)沿x、y軸方向的速度；φ為車輛橫擺角；δf為等效前輪轉(zhuǎn)角；vf、vr分別為車輛前后軸中點(diǎn)速度；L為軸距。

2 自動(dòng)泊車系統(tǒng)設(shè)計(jì)

自動(dòng)泊車系統(tǒng)分為車位識(shí)別、路徑規(guī)劃和路徑跟蹤控制。車輛進(jìn)入泊車環(huán)境后，先進(jìn)行車位識(shí)別，當(dāng)識(shí)別到有效車位再根據(jù)周圍環(huán)境狀況建立坐標(biāo)系進(jìn)行路徑規(guī)劃，在路徑跟蹤控制器的作用下執(zhí)行泊車操作。如果車位不滿足需求，車輛會(huì)繼續(xù)行駛搜索下個(gè)車位。

2.1 最小車位尺寸及路徑約束分析

最小車位尺寸大小和路徑約束分析是泊車路徑規(guī)劃的前提，滿足泊車約束的最小車位尺寸，見(jiàn)圖2。采用逆向行駛的方法，假設(shè)車輛位于車位內(nèi)部，O為圓心，以最小轉(zhuǎn)彎半徑Rmin駛出車位，所需要的最小車位空間即最小車位尺寸。為確保泊車的安全性，要避免車輛右后頂點(diǎn)C與車位右邊界碰撞、右前頂點(diǎn)B與車位左前端點(diǎn)a碰撞。根據(jù)圖2中的幾何關(guān)系，解出最小車位長(zhǎng)度Lp和寬度Wp：

圖2 最小車位示意圖

式中：δfmax為最大前輪轉(zhuǎn)角；W為車身寬度；R1、R2分別為車輛右前頂點(diǎn)B和右后頂點(diǎn)C的最小轉(zhuǎn)彎半徑；S為車輛邊界與車位線邊界間預(yù)留安全距離，取0.2 m。設(shè)車寬為1.7 m，前懸為0.77 m，后懸為0.62 m，軸距為2.46 m，前輪最大轉(zhuǎn)角為36.7°。由式（2）～（4）計(jì)算出最小車位尺寸為5.67 m×2.2 m，但車輛在泊車結(jié)束時(shí)前輪處于最大轉(zhuǎn)向角位置，會(huì)出現(xiàn)原地轉(zhuǎn)向的情況。因此需要調(diào)整車位的長(zhǎng)度，最終確定車位尺寸大小為6.5 m×2.2 m。

確定最小車位尺寸后，為確保泊車過(guò)程中車輛行駛的安全性，避免主車與車位邊界發(fā)生碰撞，對(duì)泊車過(guò)程中存在的障礙約束進(jìn)行分析。假設(shè)主車車身的4 個(gè)頂點(diǎn)分別為A（xA，yA）、B（xB，yB）、C（xC，yC）、D（xD，yD）；停車位4 個(gè)頂點(diǎn)分別為a、b、c、d，以車位左下角c點(diǎn)為坐標(biāo)原點(diǎn)，車位邊界線bc、cd分別為x軸、y軸建立大地坐標(biāo)系。泊車過(guò)程中主車可能發(fā)生碰撞的情況，如圖3所示。由圖3可知，為防止車輛泊車過(guò)程中發(fā)生碰撞，泊車路徑需要滿足下列避障約束：

圖3 泊車過(guò)程車身位置分析

1）主車泊車靠近車位過(guò)程中，要防止主車右側(cè)邊緣線BC與車位左前頂點(diǎn)a接觸，即

2）主車大部分進(jìn)入車位開(kāi)始車身回正時(shí)，需防止主車右前頂點(diǎn)B與車位前邊界線ab碰撞，即

3）主車在車位內(nèi)要避免右前頂點(diǎn)B與車位右邊界bc發(fā)生刮擦，即

4）主車在車位內(nèi)倒退過(guò)程中，要避免主車右后頂點(diǎn)C與車位右邊界線bc發(fā)生碰撞，即

5）主車在車位中要防止右后頂點(diǎn)C觸碰到車位后邊界線cd，即

6）到達(dá)泊車終點(diǎn)后，主車整體處于車位中，即

路徑曲線曲率過(guò)大或者曲率變化過(guò)快可能會(huì)導(dǎo)致車輛無(wú)法實(shí)現(xiàn)有效跟蹤。因此，還要求路徑曲率小于車輛所能達(dá)到的最大曲率，且泊車過(guò)程中，車輪轉(zhuǎn)向角速度小于最大轉(zhuǎn)向角速度。

2.2 路徑規(guī)劃

自動(dòng)泊車系統(tǒng)的路徑規(guī)劃是結(jié)合主車和車位的位置關(guān)系以及車輛運(yùn)動(dòng)學(xué)原理，規(guī)劃一條滿足泊車安全性、易跟蹤性和停車規(guī)范性等要求的路徑曲線。等速偏移和正弦函數(shù)疊加曲線具備曲率連續(xù)、計(jì)算簡(jiǎn)單等優(yōu)點(diǎn)，能夠很好地滿足泊車條件［14］，路徑表達(dá)式為

式中：a、b、c為待定常系數(shù)。假設(shè)泊車路徑起始點(diǎn)和終止點(diǎn)坐標(biāo)分別為（x1，y1）和（x2，y2），可得：

為確保在路徑起、止點(diǎn)位置，車身與車位平行且前輪轉(zhuǎn)角剛好處于回正狀態(tài)，要求路徑首點(diǎn)和末點(diǎn)的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)均為0，即

通過(guò)上述路徑約束條件，泊車路徑規(guī)劃將轉(zhuǎn)換為泊車起始點(diǎn)、終止點(diǎn)位置坐標(biāo)求解問(wèn)題。綜合考慮車位尺寸以及泊車安全性問(wèn)題，先確定泊車終點(diǎn)位置為（0.82，1.1），應(yīng)用MATLAB 的Fmincon 函數(shù)求解泊車起點(diǎn)位置。設(shè)泊車起始點(diǎn)位置（x1，y1）為優(yōu)化變量，為使車輛的轉(zhuǎn)向過(guò)程盡可能平穩(wěn)，規(guī)定優(yōu)化目標(biāo)是使車輛在路徑曲線中點(diǎn)位置的橫擺角最小化，目標(biāo)函數(shù)為

在滿足相應(yīng)約束條件下求出最優(yōu)解，確定泊車起始位置為（7.7，3.5），得到泊車路徑曲線如圖4a所示。為了驗(yàn)證所規(guī)劃路徑是否滿足避障要求，假設(shè)車輛能夠無(wú)偏差地跟蹤泊車路徑，用矩形近似代替車身外形，用MATLAB 繪制出泊車過(guò)程中車身輪廓包絡(luò)線，結(jié)果如圖4b 所示。在理想跟蹤狀態(tài)下，車輛整個(gè)泊車過(guò)程中不會(huì)與車位邊緣發(fā)生碰撞，泊車路徑曲率變化如圖4c 所示，可以看出：曲率在-0.279～0.279連續(xù)變化，不超過(guò)車輛所能達(dá)到的最大曲率0.303，并且在起始點(diǎn)和終止點(diǎn)處的曲率為0，且車輪處于回正狀態(tài)。因此，該路徑能夠作為有效泊車路徑。

圖4 泊車仿真曲線

2.3 路徑跟蹤控制器

模型預(yù)測(cè)控制是利用所建立的系統(tǒng)模型、系統(tǒng)當(dāng)前的狀態(tài)和未來(lái)的控制量去預(yù)測(cè)系統(tǒng)未來(lái)的輸出，滾動(dòng)求解帶約束的優(yōu)化問(wèn)題來(lái)實(shí)現(xiàn)控制目的，具備預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋矯正的特點(diǎn)［15］。對(duì)于泊車過(guò)程，采用車輛的運(yùn)動(dòng)學(xué)模型設(shè)計(jì)控制算法，此非線性系統(tǒng)可表示為

式中：u為系統(tǒng)輸入；ξ為狀態(tài)量。對(duì)于確定的期望路徑，可以得到每時(shí)刻的系統(tǒng)參考狀態(tài)量ξr和控制量ur，在f(ξr,ur)處對(duì)式（15）進(jìn)行泰勒展開(kāi)，僅保留低階項(xiàng)，可得：

聯(lián)合式（15）～（16），得到線性化的車輛運(yùn)動(dòng)學(xué)模型，并進(jìn)行近似離散化處理，可得：

式中：T為采樣周期。將狀態(tài)量和控制量組合作為新的狀態(tài)量ξ(k|t)，得到新的狀態(tài)空間表達(dá)式：

為了使車輛能夠快速且平穩(wěn)地跟蹤規(guī)劃路徑，定義目標(biāo)函數(shù)：

式中：Np、Nc為預(yù)測(cè)時(shí)域和控制時(shí)域；Q和R為權(quán)重矩陣；ρ為權(quán)重系數(shù)；ε為松弛因子。受車輛參數(shù)限制，對(duì)前輪轉(zhuǎn)角和前輪轉(zhuǎn)角增量范圍設(shè)置如下：

在每個(gè)控制周期內(nèi)對(duì)目標(biāo)函數(shù)求解，得到控制時(shí)域內(nèi)一系列控制增量，將第1個(gè)解作為實(shí)際控制增量輸入系統(tǒng)，后續(xù)控制過(guò)程重復(fù)上述步驟。

3 仿真分析

Prescan 作為無(wú)人駕駛仿真軟件，支持基于視覺(jué)傳感器、超聲波雷達(dá)、激光雷達(dá)等多種ADAS 系統(tǒng)開(kāi)發(fā)應(yīng)用。基于Prescan 仿真平臺(tái)搭建泊車場(chǎng)景，通過(guò)傳感器將車輛狀態(tài)信息傳遞給Simulink中的泊車控制器，控制器將控制量輸入Prescan 車輛模型，實(shí)時(shí)更新泊車場(chǎng)景中的車輛狀態(tài)，信息交互如圖5所示。

圖5 Prescan和Simulink信息交互圖

3.1 車位識(shí)別仿真驗(yàn)證

進(jìn)入泊車場(chǎng)景后，車輛勻速向右行駛，超聲波雷達(dá)會(huì)在道路右側(cè)先后探測(cè)到障礙車Ⅰ和障礙車Ⅱ，障礙車之間設(shè)置平行車位，如圖6所示。

圖6 車位識(shí)別示意圖

當(dāng)主車到達(dá)位置1 時(shí)，接近車Ⅰ尾部，車輛右側(cè)的超聲波傳感器探測(cè)距離會(huì)發(fā)生驟降，系統(tǒng)判斷出該車位已被占用；當(dāng)主車?yán)^續(xù)前進(jìn)至位置2 時(shí)，逐漸遠(yuǎn)離車Ⅰ，超聲波傳感器探測(cè)距離會(huì)出現(xiàn)上升沿突變，如果此時(shí)探測(cè)距離r大于2 m，系統(tǒng)認(rèn)定為空位，該位置被定為車位線后端。當(dāng)主車到達(dá)位置3 時(shí)，接近車Ⅱ，超聲波傳感器探測(cè)范圍發(fā)生下降沿突變，系統(tǒng)將該位置定為車位線前端，車位前端至后端車輛的行駛距離Lp，即為車位長(zhǎng)度。如果車位尺寸大于平行泊車最小車位要求，則該車位被判定為有效車位，車輛將繼續(xù)行駛一段距離到達(dá)位置4（規(guī)劃的泊車起始位置）后停車，進(jìn)入泊車環(huán)節(jié)，車位識(shí)別過(guò)程中傳感器探測(cè)距離變化見(jiàn)圖7。車位識(shí)別過(guò)程中，主車始終保持低速直線運(yùn)動(dòng)，跟蹤誤差可忽略，故僅對(duì)泊車過(guò)程中產(chǎn)生的誤差進(jìn)行分析。

圖7 傳感器探測(cè)距離變化曲線

3.2 泊車仿真驗(yàn)證

考慮到泊車過(guò)程中車速的波動(dòng)，分別設(shè)置恒定車速1 m·s-1、2 m·s-1以及變車速（0.5sint+1）m·s-1不同泊車工況，泊車起點(diǎn)坐標(biāo)為（7.7,3.5），泊車終點(diǎn)坐標(biāo)為（0.82,1.1），分別對(duì)3 種泊車工況進(jìn)行Prescan 和Simulink聯(lián)合仿真。為驗(yàn)證泊車過(guò)程的安全性，對(duì)泊車期間主車與障礙約束點(diǎn)的最小距離進(jìn)行分析，如圖8所示的障礙點(diǎn)1和2。l1、l2分別為主車與障礙點(diǎn)的最小間距值，當(dāng)間距大于0 時(shí)，則說(shuō)明主車能安全通過(guò)，且間距越大，安全性越高。泊車過(guò)程中最小間距l(xiāng)1、l2具體數(shù)值見(jiàn)表1。

圖8 泊車過(guò)程中障礙約束點(diǎn)分析示意圖

表1 障礙約束點(diǎn)處最小間距值 m

由表1 可知，3 種工況下主車在障礙點(diǎn)1、2 處與障礙約束點(diǎn)最小間距值均大于0，滿足防碰撞要求。工況1中2處的最小間距值均大于0.2 m，整體上安全性最高；而工況2的危險(xiǎn)性相對(duì)偏高，在2處與障礙點(diǎn)的最小間距僅0.15 m，可能是由于車輛泊車結(jié)束時(shí)姿態(tài)傾斜導(dǎo)致。車速提高后，泊車安全性會(huì)有所降低。

泊車過(guò)程中控制器跟蹤效果見(jiàn)圖9。由圖9a可知，路徑曲線整體上過(guò)渡平滑，在3 種泊車工況下，車輛都能夠順利地跟蹤參考路徑，控制器有效。由圖9b可得，工況1和工況2的橫向跟蹤誤差峰值分別為0.006 m和0.019 m，表明跟蹤誤差會(huì)隨著車速的增大而增大；工況3的跟蹤誤差最大值為0.016 m，與工況1 相比變化不大，說(shuō)明車速小范圍波動(dòng)的情況下，該控制器都具有較強(qiáng)的魯棒性。由圖9c 可知，泊車過(guò)程中前輪轉(zhuǎn)角均不超過(guò)允許最大前輪轉(zhuǎn)角，轉(zhuǎn)向過(guò)程保持穩(wěn)定、無(wú)突變；3種工況下泊車終點(diǎn)處的前輪轉(zhuǎn)角都不超過(guò)3°，較小轉(zhuǎn)角忽略不計(jì)，認(rèn)為此時(shí)車輪已經(jīng)回正。從圖9d 可看出，3 種工況下泊車終點(diǎn)處車輛橫擺角均不大于2°，認(rèn)為車身最終與車位保持平行，停放姿態(tài)較好。

圖9 不同工況下泊車過(guò)程中控制器跟蹤效果圖

4 結(jié)論

采用逆向行駛方法推算最小車位尺寸，對(duì)泊車過(guò)程中易發(fā)生碰撞的情況進(jìn)行分析，在考慮路徑約束的條件下，采用等速偏移和正弦函數(shù)疊加曲線規(guī)劃出轉(zhuǎn)向平穩(wěn)且曲率較小的路徑。根據(jù)MPC 算法設(shè)計(jì)路徑跟蹤控制器，在不同工況下進(jìn)行Prescan 和Simulink 聯(lián)合仿真，該控制器都滿足安全性要求、具有較高的跟蹤精度。未來(lái)將基于傳感器融合技術(shù)進(jìn)行多種車位識(shí)別，針對(duì)不同車位設(shè)計(jì)泊車控制算法，進(jìn)行泊車系統(tǒng)實(shí)車試驗(yàn)。