基于偽譜拼接法的自動泊車路徑規(guī)劃

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

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

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

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

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

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

1.1 工況描述

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

為了解決在狹窄空間的入庫問題，把車輛行駛過程分成3個階段，如圖2所示。

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

第2階段：前進到C位置；

第3階段：掛倒擋行駛，完成入庫過程。

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

圖1 狹窄環(huán)境下入庫碰撞場景

圖2 狹窄環(huán)境下3次往復入庫無碰撞場景

1.2 車輛運動學模型

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

圖3 車輛運動模型

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

(1)

車輛前后輪坐標關系為

(2)

由式(1)(2)可得車輛的運動學約束為

(3)

對于車輛來說，其不僅受到運動學的約束，還受到其他的約束。例如，由于汽車轉(zhuǎn)向器設計的不同，汽車的轉(zhuǎn)向角應約束在一定的范圍之內(nèi)；由于泊車速度相對比較低，其值也應約束在一定的范圍之內(nèi)。

(4)

1.3 車輛與車庫建模

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

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

p=1,2,…

(5)

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

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

1.3.2車輛邊界建模及坐標轉(zhuǎn)化

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

(6)

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

圖5 車輛的近似輪廓

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

i=1,2,…,4k

(7)

其中(x,y,θ)隨時間變化關系由車輛運動學方程(1)表示。

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

1.3.3車庫建模

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

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

在世界坐標系中，任意一點(x,y)在車庫可行駛區(qū)域內(nèi)滿足以下約束條件(稱為車庫約束條件)：

(8)

1.3.4無碰撞約束條件

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

(9)

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

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

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

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

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

(10)

(11)

(12)

(13)

(14)

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

(15)

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

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

3 仿真分析

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

圖7 泊車過程仿真示意圖

圖8 泊車過程中車輛速度的變化

圖9 泊車過程中車輛位置的變化曲線

圖10 泊車過程中車輛航向角的變化

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

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

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

4 結(jié)束語

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