甘 能,趙克剛,裴 鋒,郭泉成,劉素芬

(1.華南理工大學(xué) 汽車(chē)零部件技術(shù)國(guó)家地方聯(lián)合工程實(shí)驗(yàn)室， 廣州 510640；2.廣汽集團(tuán)汽車(chē)工程研究院， 廣州 511495； 3.廣州南洋理工職業(yè)學(xué)院， 廣州 510900)

隨著汽車(chē)保有量的增長(zhǎng)，停車(chē)空間狹窄、停車(chē)難等問(wèn)題日漸凸顯。傳感器、信息技術(shù)、自動(dòng)化控制理論等的發(fā)展為汽車(chē)走向智能化奠定了基礎(chǔ)。自動(dòng)泊車(chē)作為智能車(chē)的重要應(yīng)用能有效解決入庫(kù)的問(wèn)題，越來(lái)越受到人們的關(guān)注。在泊車(chē)過(guò)程中通過(guò)超聲波雷達(dá)、攝像頭等一系列傳感器感知泊車(chē)的環(huán)境，規(guī)劃出一條合理有效的避障路線(xiàn)，可有效降低在狹小空間內(nèi)泊車(chē)的難度[1]。

目前，學(xué)者們對(duì)泊車(chē)技術(shù)進(jìn)行了大量的研究。文獻(xiàn)[2]采用BP神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)PID控制汽車(chē)自動(dòng)泊車(chē)；文獻(xiàn)[3]采用神經(jīng)網(wǎng)絡(luò)和模糊理論結(jié)合的方法進(jìn)行泊車(chē)研究；文獻(xiàn)[4-5]采用B樣條曲線(xiàn)設(shè)計(jì)泊車(chē)路徑；文獻(xiàn)[6]采用多段圓弧曲線(xiàn)進(jìn)行泊車(chē)路徑設(shè)計(jì)；文獻(xiàn)[7]建立泊車(chē)路徑規(guī)劃最優(yōu)控制問(wèn)題的數(shù)學(xué)模型，通過(guò)分段離散化為非線(xiàn)性問(wèn)題，求解出泊車(chē)路徑。

偽譜法[8-10]在20世紀(jì)90年代被引入最優(yōu)控制領(lǐng)域，該方法的優(yōu)點(diǎn)是收斂速度極快[11]。偽譜法包括高斯偽譜法(GPM)、切比雪夫偽譜法(CPM)、勒讓德偽譜法(LPM)等。Huntington[12]對(duì)上述偽譜法進(jìn)行了研究，并證明LPM和GPM這兩類(lèi)偽譜法在收斂速度和精度上具有優(yōu)勢(shì)。本文運(yùn)用Gauss偽譜法對(duì)汽車(chē)入庫(kù)和泊車(chē)的路徑進(jìn)行規(guī)劃計(jì)算。

1 狹窄空間下泊車(chē)路徑規(guī)劃數(shù)學(xué)模型

1.1 工況描述

如圖1所示，車(chē)輛行駛空間狹窄，車(chē)輛一次倒車(chē)入庫(kù)極易與周?chē)能?chē)輛發(fā)生碰撞。因此，在狹小的空間車(chē)輛需要往復(fù)多次才能順利停到車(chē)位。

為了解決在狹窄空間的入庫(kù)問(wèn)題，把車(chē)輛行駛過(guò)程分成3個(gè)階段，如圖2所示。

第1階段：車(chē)輛后退，從位置A行駛到位置B；

第2階段：前進(jìn)到C位置；

第3階段：掛倒擋行駛，完成入庫(kù)過(guò)程。

這樣，車(chē)輛在可行駛的狹小空間內(nèi)多次以不同的轉(zhuǎn)角前進(jìn)或后退，調(diào)整車(chē)輛的位置和姿態(tài)進(jìn)行倒車(chē)入庫(kù)，其路徑規(guī)劃可以用偽譜法進(jìn)行多階段建模和求解。

圖1 狹窄環(huán)境下入庫(kù)碰撞場(chǎng)景

圖2 狹窄環(huán)境下3次往復(fù)入庫(kù)無(wú)碰撞場(chǎng)景

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

前輪轉(zhuǎn)向的四輪車(chē)符合阿克曼轉(zhuǎn)向幾何原理，其運(yùn)動(dòng)模型通?？梢钥闯勺孕熊?chē)模型。車(chē)輛在泊車(chē)時(shí)，其速度很慢，受側(cè)偏和側(cè)傾等因素影響較小。忽略這些運(yùn)動(dòng)學(xué)特性，可以把車(chē)輛看成在線(xiàn)性空間內(nèi)運(yùn)動(dòng)。如圖3所示，以車(chē)輛后軸中心的坐標(biāo)(x,y)來(lái)表示，車(chē)輛前輪的坐標(biāo)為(xf,yf)，車(chē)輛前后軸的軸距為L(zhǎng),前輪的轉(zhuǎn)向角為δ，車(chē)輛朝向與全局坐標(biāo)的夾角(航向角)為θ，車(chē)輛前進(jìn)速度為v。建立汽車(chē)在全局坐標(biāo)系下位移與車(chē)速，橫擺角與前輪轉(zhuǎn)角的數(shù)學(xué)關(guān)系。其中x、y、θ為狀態(tài)變量，δ為控制變量。

圖3 車(chē)輛運(yùn)動(dòng)模型

車(chē)輛前輪轉(zhuǎn)角、航向角、車(chē)輛橫縱向速度之間的關(guān)系表示為

(1)

車(chē)輛前后輪坐標(biāo)關(guān)系為

(2)

由式(1)(2)可得車(chē)輛的運(yùn)動(dòng)學(xué)約束為

(3)

對(duì)于車(chē)輛來(lái)說(shuō)，其不僅受到運(yùn)動(dòng)學(xué)的約束，還受到其他的約束。例如，由于汽車(chē)轉(zhuǎn)向器設(shè)計(jì)的不同，汽車(chē)的轉(zhuǎn)向角應(yīng)約束在一定的范圍之內(nèi)；由于泊車(chē)速度相對(duì)比較低，其值也應(yīng)約束在一定的范圍之內(nèi)。

(4)

1.3 車(chē)輛與車(chē)庫(kù)建模

1.3.1P-Norm函數(shù)的介紹

P-Norm函數(shù)可以描繪各種圖形，通過(guò)選擇不同的參數(shù)形成不同的圖形邊界[13]，該函數(shù)為

p=1,2,…

(5)

對(duì)于式(5)，可以通過(guò)取不同的p值得到不同的形狀。其中：rx,ry分別為x方向和y方向的比例系數(shù)；(a,b)為不同圖形的中心坐標(biāo)。當(dāng)h(x,y)>0時(shí)，點(diǎn)在圖形的外面；當(dāng)h(x,y)<0時(shí)，點(diǎn)在圖形的里面；h(x,y)=0，點(diǎn)在圖形邊界上。

不同的p值可以讓圖形呈現(xiàn)不同的形狀。如圖4所示，當(dāng)p=1時(shí)函數(shù)可以創(chuàng)建菱形；p=2時(shí)可以創(chuàng)建圓或橢圓(rx=ry為圓，rx≠ry為橢圓)；當(dāng)p趨于無(wú)窮，圖形趨于矩形。

1.3.2車(chē)輛邊界建模及坐標(biāo)轉(zhuǎn)化

本文選擇某一型號(hào)車(chē)輛作為研究對(duì)象，其基本參數(shù)如表1所示。車(chē)輛的外形可以看成是一個(gè)矩形，在進(jìn)行車(chē)輛建模時(shí)，可以采用上述P-Norm函數(shù)[10]刻畫(huà)，選用適當(dāng)?shù)膒值刻畫(huà)車(chē)輛的外形。在車(chē)輛坐標(biāo)系(XC,O,YC)中，為刻畫(huà)車(chē)輛邊界，同時(shí)減少計(jì)算量，在式(5)中取p=4，則變?yōu)槭?6)，為車(chē)輛邊界刻畫(huà)函數(shù)，其中：rx、ry分別為車(chē)輛長(zhǎng)和寬的一半；dr是質(zhì)心到后軸的距離。則車(chē)輛邊界上每一個(gè)點(diǎn)(XC,YC)都滿(mǎn)足式(6)，刻畫(huà)出的車(chē)輛邊界見(jiàn)圖5。

(6)

圖4 不同p值下的函數(shù)圖形形狀

圖5 車(chē)輛的近似輪廓

由于智能車(chē)在不斷地運(yùn)動(dòng)，故車(chē)輛的邊界函數(shù)是一個(gè)變化的函數(shù)。在車(chē)輛坐標(biāo)系(XC,O,YC)中，車(chē)輛邊界上每一個(gè)點(diǎn)(Xci,Yci)可以通過(guò)轉(zhuǎn)換矩陣轉(zhuǎn)換成在世界坐標(biāo)系(x,o,y)上對(duì)應(yīng)的點(diǎn)(xci,yci)，其中車(chē)輛位置為(x,y),航向角為θ。車(chē)輛坐標(biāo)和世界坐標(biāo)的轉(zhuǎn)換矩陣為：

i=1,2,…,4k

(7)

其中(x,y,θ)隨時(shí)間變化關(guān)系由車(chē)輛運(yùn)動(dòng)學(xué)方程(1)表示。

表1 車(chē)輛的主要參數(shù)

1.3.3車(chē)庫(kù)建模

如圖6所示，對(duì)于垂直車(chē)庫(kù)函數(shù)而言，同樣使用P函數(shù)進(jìn)行建模，不同之處在于參數(shù)選擇，此處不一一列舉。車(chē)庫(kù)由最外面矩形h1和左右兩邊矩形(h2，h3)構(gòu)成。其中構(gòu)成3個(gè)矩形的P值為6，參數(shù)見(jiàn)表2。

圖6 垂直車(chē)庫(kù)函數(shù)

在世界坐標(biāo)系中，任意一點(diǎn)(x,y)在車(chē)庫(kù)可行駛區(qū)域內(nèi)滿(mǎn)足以下約束條件(稱(chēng)為車(chē)庫(kù)約束條件)：

(8)

1.3.4無(wú)碰撞約束條件

為使車(chē)輛在入庫(kù)時(shí)不與車(chē)庫(kù)邊界發(fā)生碰撞，則車(chē)輛邊界上任意一點(diǎn)(xci,yci)都滿(mǎn)足車(chē)庫(kù)約束函數(shù)，則無(wú)碰撞約束條件為：

(9)

2 多階段優(yōu)化問(wèn)題建立與求解

2.1 多階段最優(yōu)問(wèn)題

車(chē)輛在倒車(chē)入庫(kù)時(shí)，會(huì)受到路面空間、車(chē)輛本身尺寸和車(chē)輛運(yùn)動(dòng)學(xué)約束。車(chē)輛倒車(chē)入庫(kù)需要大角度的轉(zhuǎn)向，而車(chē)輛轉(zhuǎn)向角通常需在一定的范圍內(nèi)，如果進(jìn)行一次性倒車(chē)入庫(kù)，就會(huì)與周?chē)矬w發(fā)生碰撞，故在狹窄的環(huán)境下，采用多階段倒車(chē)入庫(kù)才能滿(mǎn)足車(chē)輛尺寸、道路空間和車(chē)輛運(yùn)動(dòng)學(xué)的要求。

進(jìn)行車(chē)輛多階段倒車(chē)入庫(kù)求解時(shí)，由于各階段的速度方向和各階段連接時(shí)刻車(chē)輛的速度變?yōu)榱悖什扇》蛛A段求解。在第1階段，車(chē)輛的速度為負(fù)；在第2階段，車(chē)輛是前進(jìn)行駛的，故速度為正；在第3階段，車(chē)輛的速度變?yōu)樨?fù)。實(shí)際泊車(chē)情況下，在每一階段結(jié)束時(shí)，車(chē)輛可進(jìn)行原地打方向，此時(shí)可以采用分階段求解。

對(duì)于在狹窄空間內(nèi)泊車(chē)這一多階段最優(yōu)問(wèn)題，選擇能量最低作為性能指標(biāo)。對(duì)于每一階段，車(chē)輛都應(yīng)滿(mǎn)足車(chē)輛運(yùn)動(dòng)學(xué)約束(如式(3))、車(chē)輛的物理約束(如式(4))、車(chē)庫(kù)邊界約束(如式(8))、無(wú)碰撞約束(如式(9))，以及下面介紹的階段約束。因此，總的最優(yōu)控制模型如下：

(10)

(11)

(12)

(13)

(14)

對(duì)于第3階段最優(yōu)控制問(wèn)題，還需要添加2個(gè)連接約束，使各階段狀態(tài)變量連續(xù)。

(15)

2.2 基于Gauss偽譜法求解多階段最優(yōu)控制問(wèn)題

針對(duì)前文所構(gòu)建的車(chē)輛入庫(kù)的多階段最優(yōu)控制問(wèn)題，首先可基于Gauss偽譜法的時(shí)域變換、狀態(tài)及控制變量近似、動(dòng)力學(xué)方程轉(zhuǎn)化為代數(shù)方程、性能目標(biāo)函數(shù)中積分項(xiàng)轉(zhuǎn)化為求和項(xiàng)等步驟將其轉(zhuǎn)化為非線(xiàn)性規(guī)劃問(wèn)題(nonlinear programming problem,NLP)。并應(yīng)用基于序列二次規(guī)劃算法開(kāi)發(fā)的NLP問(wèn)題求解器(如SNOPT、IPOPT)進(jìn)行數(shù)值求解，以獲得最優(yōu)泊車(chē)軌跡曲線(xiàn)。具體求解轉(zhuǎn)化過(guò)程可參考文獻(xiàn)[14]。

3 仿真分析

在PreScan軟件中對(duì)垂直泊車(chē)進(jìn)行實(shí)驗(yàn)仿真，分別得到了車(chē)輛位置(x,y)、航向角θ、車(chē)輛速度v、前輪轉(zhuǎn)角δ的實(shí)驗(yàn)結(jié)果。其中車(chē)輛位置是質(zhì)心在世界坐標(biāo)系的值，航向角是車(chē)輛車(chē)頭方向與x軸的夾角，逆時(shí)針為正。仿真結(jié)果如圖7～11所示。

圖7 泊車(chē)過(guò)程仿真示意圖

圖8 泊車(chē)過(guò)程中車(chē)輛速度的變化

圖9 泊車(chē)過(guò)程中車(chē)輛位置的變化曲線(xiàn)

圖10 泊車(chē)過(guò)程中車(chē)輛航向角的變化

圖11 泊車(chē)過(guò)程中車(chē)輛前輪轉(zhuǎn)角的變化

圖7為整體的路徑規(guī)劃結(jié)果，初始位置x0=-8 m,y0=7 m,θ0=π；末值位置xf=0 m,yf=2.5 m,θf(wàn)=π/2。由此得到狹窄空間內(nèi)的泊車(chē)路徑，該路徑分成了3段，符合實(shí)際狹窄空間內(nèi)的泊車(chē)結(jié)果。

由圖8可知，車(chē)輛速度先為負(fù)，然后變化為正，最后變化為負(fù)，對(duì)應(yīng)車(chē)輛3階段的速度變化。在圖9中顯示了車(chē)輛在世界坐標(biāo)系中的X和Y隨時(shí)間的變化。在0～8 s內(nèi)，X隨時(shí)間的變化而變大，Y先不變?nèi)缓鬁p小，該階段為圖7的第1階段，即車(chē)輛倒車(chē)；在8～10 s內(nèi)，車(chē)輛前進(jìn)，X減小，Y增大，對(duì)應(yīng)圖7的第2階段；在10～15 s為第3階段，車(chē)輛最終泊車(chē)到了(0,2.5)位置。符合泊車(chē)要求。圖10和圖11對(duì)應(yīng)車(chē)輛航向角和前輪轉(zhuǎn)角變化，同樣符合泊車(chē)要求。

4 結(jié)束語(yǔ)

為了解決狹窄空間內(nèi)泊車(chē)問(wèn)題，本文通過(guò)P-Norm函數(shù)刻畫(huà)車(chē)輛和車(chē)庫(kù)的邊界模型，構(gòu)建運(yùn)動(dòng)學(xué)方程，結(jié)合各階段的狀態(tài)來(lái)添加各種約束，建立了最優(yōu)控制模型。然后運(yùn)用拉格朗日插值，把微分狀態(tài)方程轉(zhuǎn)換成代數(shù)方程，利用高斯積分近似性能積分的積分項(xiàng)，最后運(yùn)用二次規(guī)劃法求解最優(yōu)控制問(wèn)題。實(shí)驗(yàn)結(jié)果表明，在狹窄的空間內(nèi)泊車(chē)時(shí)，運(yùn)用高斯偽譜法能較好地規(guī)劃出路徑，為多階段的最優(yōu)控制問(wèn)題提供參考。