一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法
【專利摘要】本發(fā)明提供一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,用于自主泊車系統(tǒng)檢測到可用泊車空間而將車輛自動地停泊于所述泊車空間中,包括:檢測目標(biāo)車位信息,確定泊車情景;確定待停泊車輛的初始狀態(tài)和目標(biāo)狀態(tài);建立車輛運(yùn)動學(xué)微分方程;對車輛狀態(tài)變量和控制變量進(jìn)行分段,并按照一定的時間步長對每一分段進(jìn)行等距采樣,得到待優(yōu)化變量;分別形成待優(yōu)化變量的等式約束、邊界約束、不等式約束;根據(jù)車輛在泊車過程中的運(yùn)動范圍限制,形成待停泊車輛的運(yùn)動范圍約束;確定優(yōu)化目標(biāo),建立目標(biāo)函數(shù);采用非線性規(guī)劃求解器,得到泊車路徑的最優(yōu)解。本發(fā)明適用于多種泊車場景,設(shè)計(jì)合理,能夠提供豐富的信息控制車輛自主泊車,安全系數(shù)高。
【專利說明】
一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及車輛自主泊車技術(shù)領(lǐng)域,具體是一種用于多種泊車場景的車輛自主泊 車路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 近年來,隨著國內(nèi)汽車保有量的迅速增長,城市內(nèi)停車位日趨緊張和狹小。對于新 手駕駛員,泊車通常是一個難題,特別是對于車位過于狹窄的情況,駕駛員往往難以很好地 控制汽車進(jìn)行快速準(zhǔn)確的泊車,由泊車引發(fā)的事故幾率大大升高。
[0003] 自主泊車系統(tǒng)可以幫助駕駛員準(zhǔn)確安全地泊車,這種系統(tǒng)使用一種或多種傳感器 檢測出車位大小及其位置,然后規(guī)劃出一條可行的泊車路徑,最后自動地控制車輛的轉(zhuǎn)向 系統(tǒng)、制動系統(tǒng)和動力系統(tǒng)遵循規(guī)劃出的路徑完成泊車。在自主泊車系統(tǒng)中,路徑規(guī)劃是關(guān) 鍵技術(shù)之一。安全無碰撞、路徑可行是其最基本、最重要的要求,在此基礎(chǔ)上快速而舒適的 泊車路徑也是自主泊車系統(tǒng)需要的。除此之外,若泊車路徑規(guī)劃結(jié)果能提供更為豐富的信 息給執(zhí)行系統(tǒng),將更有利于對所規(guī)劃路徑的跟蹤。
[0004] 中國發(fā)明專利CN102975715A中提供一種汽車在任意姿態(tài)下進(jìn)行平行泊車路徑規(guī) 劃的方法,該方法遍歷連接車輛起點(diǎn)與終點(diǎn)的點(diǎn)陣擬合出的樣條曲線,然后從中尋找一條 符合車輛運(yùn)動學(xué)約束和避撞約束的路徑。其中不利的是,該方法規(guī)劃路徑中假定了車輛僅 向一個方向運(yùn)動,不符合泊車過程需要多次調(diào)整的實(shí)際需要,因此規(guī)劃成功率不高。
[0005] 申請?zhí)枮?01210547981.2的專利中提供一種用于自動平行停泊系統(tǒng)確定車輛路 徑的方法,該方法可以提供單循環(huán)轉(zhuǎn)向操縱或兩循環(huán)轉(zhuǎn)向操縱的平行泊車路徑規(guī)劃。申請 號為201080064605.7的專利中提供一種用于使汽車前進(jìn)地泊入豎直泊車位的方法。以上方 法不利的是,其方法僅適用于一種泊車位的泊車路徑規(guī)劃。另外不利的是,其方法規(guī)劃結(jié)果 中不提供速度、加速度等信息,不利于對所規(guī)劃路徑的跟蹤。
[0006] 申請?zhí)枮?01510737989.9的專利中提供了一種基于全聯(lián)立求解策略的車輛-環(huán)境 一體化建模的動態(tài)優(yōu)化框架,有效消除了不同車位形狀對軌跡規(guī)劃策略造成的影響。其中 不利的是,該方法未對車輛的活動范圍進(jìn)行約束,其所規(guī)劃軌跡可能使得車輛侵入其他車 道而妨礙其他車道上的車輛行駛甚至發(fā)生事故。另外不利的是,其未檢測離散后的兩個車 輛狀態(tài)之間是否發(fā)生碰撞,可能造成車輛沿規(guī)劃軌跡行駛時發(fā)生碰撞。
【發(fā)明內(nèi)容】
[0007] 本發(fā)明的目的在于提供一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,以 解決現(xiàn)有技術(shù)中存在的不足。
[0008] 本發(fā)明的技術(shù)方案為:
[0009] -種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,該方法用于自主泊車系統(tǒng) 檢測到可用泊車空間而將車輛自動地停泊于所述泊車空間中,包括以下步驟:
[0010] (1)檢測目標(biāo)車位信息,確定泊車情景;
[0011] (2)根據(jù)所述目標(biāo)車位信息和泊車情景,確定待停泊車輛的初始狀態(tài)和目標(biāo)狀態(tài);
[0012] (3)基于前輪轉(zhuǎn)向四輪車輛的阿克曼模型,建立車輛運(yùn)動學(xué)微分方程;
[0013] (4)對車輛狀態(tài)變量和控制變量進(jìn)行分段,并按照一定的時間步長對每一分段進(jìn) 行等距采樣,得到待優(yōu)化變量;
[0014] (5)米用拉格朗日插值法將車輛狀態(tài)變量每個米樣點(diǎn)上的微分表不為該米樣點(diǎn)所 在分段上各個采樣點(diǎn)的函數(shù),聯(lián)立所述函數(shù)與車輛運(yùn)動學(xué)微分方程,使所述車輛運(yùn)動學(xué)微 分方程轉(zhuǎn)化為代數(shù)方程,形成待優(yōu)化變量的等式約束;
[0015] (6)根據(jù)車輛運(yùn)動的物理限制和泊車的安全要求,形成待優(yōu)化變量的邊界約束;
[0016] (7)根據(jù)目標(biāo)車位周圍障礙物,公式化避撞要求,形成待優(yōu)化變量的不等式約束;
[0017] (8)根據(jù)車輛在泊車過程中的運(yùn)動范圍限制,形成待停泊車輛的運(yùn)動范圍約束;
[0018] (9)確定優(yōu)化目標(biāo),建立目標(biāo)函數(shù);
[0019] (10)采用非線性規(guī)劃求解器,得到泊車路徑的最優(yōu)解。
[0020] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,還包括以下步驟:
[0021] 采用拉格朗日插值法對所述泊車路徑的最優(yōu)解中的車輛狀態(tài)變量進(jìn)行擬合,以細(xì) 化的時間步長對擬合后的車輛狀態(tài)變量進(jìn)行采樣,得到細(xì)化的車輛狀態(tài)序列,檢測所述細(xì) 化的車輛狀態(tài)序列中的每個車輛狀態(tài)是否發(fā)生碰撞,若是,則增加分段數(shù),重復(fù)步驟(4)~ (10)重新進(jìn)行路徑規(guī)劃。
[0022] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,步驟(1)中,所述目標(biāo)車 位信息包括目標(biāo)車位的朝向、位置、長度和寬度以及目標(biāo)車位周圍障礙物位置;所述泊車情 景包括垂直泊車、斜向泊車和平行泊車。
[0023] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,步驟(3)中,所述車輛運(yùn) 動學(xué)微分方程為:
[0024]
[0025] 其中,X表示車輛后軸中心在笛卡爾坐標(biāo)系中的橫坐標(biāo),y表示車輛后軸中心在笛 卡爾坐標(biāo)系中的縱坐標(biāo),v表示車輛后軸中心的移動速度,Θ表示車輛朝向與笛卡爾坐標(biāo)系X 軸的夾角,Ψ表示車輛前輪轉(zhuǎn)角,a表示車輛后軸中心的加速度,ω表示車輛前輪轉(zhuǎn)角的角 速度,Lm表示車輛前軸與后軸之間的距離;
[0026] 步驟(4)中,所述車輛狀態(tài)變量為x、y、v、0、警,所述車輛控制變量為a、ω,對車輛 狀態(tài)變量和控制變量進(jìn)行分段,設(shè)分段數(shù)為Ν,每一分段包含等距的Μ個采樣點(diǎn),每一分段時 間長度為(M-l)h,變量x、y、v、0、ω離散后的每一分段的Μ個采樣點(diǎn)中,兩端的采樣點(diǎn)與 相鄰的分段共用,變量a離散后的每一分段包含Μ個獨(dú)立的采樣點(diǎn),則所述待優(yōu)化變量為:
[0027]
[0028] 其中,h表示時間步長;Xi,i = 0,1,…,(M-l )Ν表示變量X離散后第i個采樣點(diǎn)的值; yi,i = 0,l,···,(M-l)N表示變量y離散后第i個采樣點(diǎn)的值;vi,i=0,l,···,(M-l)N表示變量v 離散后第i個采樣點(diǎn)的值;01,1 = 〇,1,一,(1-10表示變量Θ離散后第i個采樣點(diǎn)的值, ?^:1.? (M - 1.)謂表示變量零離散后第i個采樣點(diǎn)的值;ω i,i = 〇,1,…,(M-l )N 表示變量ω離散后第i個采樣點(diǎn)的值;aP,p = 0,1,…,MN_1表示變量a離散后第p個采樣點(diǎn)的 值。
[0029] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,采用拉格朗日插值法將 車輛狀態(tài)變量x、y、v、9、f每個采樣點(diǎn)上的微分表示為該采樣點(diǎn)所在分段上Μ個采樣點(diǎn)的函 數(shù):
[0030]
[0031] 聯(lián)立所述函數(shù)與車輛運(yùn)動學(xué)微分方程,使所述車輛運(yùn)動學(xué)微分方程轉(zhuǎn)化為代數(shù)方 程,形成待優(yōu)化變量的等式約束:
[0032] s' n,m*h-f(tn,m)*h = 0,η = 0,1,…,N_1 ;m = 0,1,…,Μ_1
[0033] 其中,s代表車輛狀態(tài)變量x、y、v、0、f,Sn,m表示車輛狀態(tài)變量在時刻t n,m處的值, Y mm表示車輛狀態(tài)變量在時刻tn,m處的系數(shù),S' n,m表示車輛狀態(tài)變量在時刻tn,m處的導(dǎo)數(shù), tn,m=h*[(M-l) *n+m ]表示第η個分段內(nèi)第m個采樣點(diǎn)處的時刻。
[0034] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,所述待優(yōu)化變量的邊界 約束為:
[0035]
[0036] 其中,hmax表不時間步長的最大限值,111)、:711)、¥11 )、&11)、911)、中!15、〇11 )分別表不變量叉、 5^、8、0、爭、(〇的下限值,叉111)、5^1)、¥ 111)、&111)、9111)、-論、〇 111)分別表示變量叉、5^、&、0、釋、:(〇的 上限值;身2}表示待停泊車輛的初始狀態(tài),表示待停泊車 輛的目標(biāo)狀態(tài);1 1、71、~91、購、《1分別表示變量1^0、-、《離散后第1個采樣點(diǎn)的值, aP表不變量散后第p個米樣點(diǎn)的值。
[0037] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,所述待優(yōu)化變量的不等 式約束為:
[0038]
[0039] 其中,(^表示待停泊車輛處于第i個狀態(tài)時被抽象成的四邊形,匕表示第j個障礙物 被抽象成的四邊形,J表示障礙物的數(shù)量,Pik表示四邊形匕的第k個角點(diǎn),C 1>k表示四邊形Q 的第k個角點(diǎn),SKnPM)表示I^,k與四邊形(^所形成的四個三角形的面積和,SA表示四邊形 如勺面積,表示C 1>k與四邊形匕所形成的四個三角形的面積和,SP謙示四邊形匕的 面積,α為大于1的安全系數(shù)。
[0040] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,待停泊車輛的運(yùn)動范圍 約束為:
[0041]
[0042] 其中,表示待停泊車輛處于第i個狀態(tài)時被抽象成的四邊形的第k個角點(diǎn)的X 軸坐標(biāo),!^表示待停泊車輛處于第i個狀態(tài)時被抽象成的四邊形的第k個角點(diǎn)的Y軸坐標(biāo); xlb、xub分別表示車輛狀態(tài)變量X的下限值和上限值,ylb、y ub分別表示車輛狀態(tài)變量y的下限 值和上限值。
[0043] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,目標(biāo)函數(shù)為:
[0044] Tf = N*(M-l)h
[0045] 其中,Tf表示待停泊車輛的路徑規(guī)劃時間;
[0046] 優(yōu)化目標(biāo)為時間最短,即minTf。
[0047] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,Μ為整數(shù)且4 8。
[0048] 本發(fā)明的有益效果為:
[0049] 由上述技術(shù)方案可知,本發(fā)明適用于多種泊車場景的路徑規(guī)劃,包括垂直泊車、斜 向泊車和平行泊車,可以提供符合車輛運(yùn)動學(xué)約束和避撞約束的泊車路徑,規(guī)劃結(jié)果安全 可行,還可以提供速度、加速度等控制信息以便于對所規(guī)劃路徑的跟蹤,設(shè)計(jì)合理,能夠提 供豐富的信息控制車輛自主泊車,安全系數(shù)高。
【附圖說明】
[0050] 圖1是應(yīng)用本發(fā)明實(shí)施例的自主泊車系統(tǒng)的框圖;
[0051] 圖2是本發(fā)明實(shí)施例的車輛幾何示意圖;
[0052]圖3是本發(fā)明實(shí)施例的垂直泊車路徑規(guī)劃示意圖;
[0053] 圖4是本發(fā)明實(shí)施例的斜向泊車路徑規(guī)劃示意圖;
[0054] 圖5是本發(fā)明實(shí)施例的平行泊車路徑規(guī)劃示意圖。
【具體實(shí)施方式】
[0055] 下面結(jié)合附圖和具體實(shí)施例進(jìn)一步說明本發(fā)明。
[0056] 如圖1所示,自主泊車系統(tǒng)包括感知系統(tǒng)1、控制器2和車輛的轉(zhuǎn)向系統(tǒng)31、制動系 統(tǒng)32、動力系統(tǒng)33。感知系統(tǒng)1包含一種或多種傳感器,例如基于超聲波的傳感器、基于視覺 的傳感器或者基于激光的傳感器,其可以檢測到周圍障礙物的信息,并且檢測出泊車位信 息,設(shè)定車輛目標(biāo)狀態(tài),然后向控制器2發(fā)送以上信息。控制器2接收感知系統(tǒng)1發(fā)送的障礙 物信息、目標(biāo)車位信息和目標(biāo)狀態(tài)信息,然后依據(jù)本發(fā)明的方法對泊車問題建模并求解,最 后執(zhí)行規(guī)劃出的路徑。轉(zhuǎn)向系統(tǒng)31、制動系統(tǒng)32和動力系統(tǒng)33可以接收和執(zhí)行來自控制器2 的控制命令并向控制器2發(fā)送反饋信息。例如,轉(zhuǎn)向系統(tǒng)31可以接收方向盤轉(zhuǎn)角命令或車輛 前輪轉(zhuǎn)角命令并執(zhí)行相應(yīng)的方向盤轉(zhuǎn)角或車輛前輪轉(zhuǎn)角;制動系統(tǒng)32可以接收制動百分比 命令并執(zhí)行相應(yīng)的制動;動力系統(tǒng)33可以接收發(fā)動機(jī)扭矩命令或車速命令并輸出相應(yīng)的發(fā) 動機(jī)扭矩或車速。
[0057]如圖2所示,將實(shí)際車輛抽象為一個矩形的車輛模型,該車輛模型符合阿克曼轉(zhuǎn)向 原理。車輛具有長度L和寬度W。車輛的位置以實(shí)際車輛后軸中心的位置(x,y)來表示。后軸 中心到車尾距離為Lr,前軸中心到車頭距離為L f,前后軸間距為Lm。車輛后軸中心速度為V, 車輛前輪轉(zhuǎn)角為寧,車輛朝向與全局坐標(biāo)系X軸夾角為Θ。
[0058]如圖3所示,一個典型的垂直泊車位兩邊都停泊有車輛,將其抽象為四邊形丹和內(nèi), 通過感知系統(tǒng)測得其四個角點(diǎn)出,1^ = 1,2;1^ = 1,2,3,4}。:(%為'%%零^:為車輛目標(biāo) 狀態(tài),是感知系統(tǒng)通過識別泊車位的類型以后設(shè)定出的,一般的,目標(biāo)狀態(tài)車速為0,即 Vf = 0,車輛朝向與車位朝向相同,8卩0f = 90°。(?]?1?為車輛初始狀態(tài),是車輛開始 應(yīng)用本發(fā)明時的狀態(tài),一般的,初始狀態(tài)車速為〇,即ν ζ = 〇。1??為所規(guī)劃路徑 中車輛的第i個狀態(tài),其四個角點(diǎn)以
[0059]符合阿克曼轉(zhuǎn)向原理的前輪轉(zhuǎn)向四輪車輛模型轉(zhuǎn)向時具有一個轉(zhuǎn)向中心,并且位 于后軸延伸線上。在低速情況下,可以忽略輪胎的滑移,車輛運(yùn)動學(xué)微分方程可以表示為:
[0060]
[0061]車輛狀態(tài)變量由表示,控制變量由(a,ω)表示。車輛狀態(tài)變量和控 制變量在時間t上是連續(xù)的,在一系列時刻上對其采樣則形成車輛的一系列狀態(tài)。對車輛狀 態(tài)變量和控制變量進(jìn)行分段,初始地,設(shè)定分段數(shù)為N,例如N= 10,每一分段包含等距的5個 米樣點(diǎn),每一分段時間長度為4h,變量x、y、v、0、爭、ω離散后的每一分段的5個米樣點(diǎn)中,兩 端的采樣點(diǎn)與相鄰的分段共用,變量a離散后的每一分段包含5個獨(dú)立的采樣點(diǎn);得到待優(yōu) 化變量為:
[00621
[0063]每一個采樣點(diǎn)上的微分可以使用拉格朗日插值法表示為該分段上5個采樣點(diǎn)的函 數(shù):
[0064]
[0065] 其中,sn,m,η = 0,1,…,N-l;m = 0,1,…,4為狀態(tài)變量s即_5暴巧爲(wèi)在時刻tn,m 處的值,為狀態(tài)變量s在時刻tn,m處的導(dǎo)數(shù),tn,m = h*(4*n+m)表示第η個分段內(nèi)第m個采 樣點(diǎn)處的時刻。
[0066] 愈、&氣_均為時間t的函數(shù),即可以表示為i = l|_的形式。聯(lián)立公式(1) 和(2),公式(1)中形如l = 的微分方程轉(zhuǎn)化為如下的代數(shù)方程,形成待優(yōu)化變量的等 式約束:
[0067] s7n,m*h-f (tn,m)*h = 0, (η = 0,1··· ,Ν-1 ;ι? = 0,1···,4) (3)
[0068] 車輛運(yùn)動過程中存在限制,即其前輪轉(zhuǎn)角和前輪轉(zhuǎn)角角速度在正反兩個方向都有 一個最大值。同時出于泊車過程的安全性考慮,車輛運(yùn)動范圍(x,y)以及車輛速度和加速度 應(yīng)該進(jìn)行限定。時間步長h和分段數(shù)N以及采樣點(diǎn)數(shù)5共同決定了泊車用時,通常泊車不應(yīng)耗 費(fèi)太長時間,因此對時間步長h進(jìn)行限制。同時,規(guī)劃結(jié)果中車輛第一個和最后一個狀態(tài)應(yīng) 分別等于車輛初始狀態(tài)和目標(biāo)狀態(tài)。綜上所述,待優(yōu)化變量的邊界約束如下:
[0069]
[0070] 其中,hmax表不時間步長的最大限值,乂11)、:711)、¥11 )、&11)、911)、吩您_、<^11)分別表不變量 叉、5^、&、9、擎、(〇的下限值山1 )、5^、¥111)、&111)、9111)、爭油.、《111 )分別表示變量叉、7、¥、&、9、屮、(〇 的上限值。
[0071] 車輛沿著規(guī)劃路徑泊車過程中不與障礙物發(fā)生碰撞是最重要的。使用面積法可以 判斷一個點(diǎn)是否位于一個四邊形之內(nèi):當(dāng)點(diǎn)位于四邊形之內(nèi)時,該點(diǎn)與該四邊形的四條邊 所組成的四個三角形面積和等于四邊形的面積;當(dāng)點(diǎn)位于四邊形之外時,該點(diǎn)與該四邊形 的四條邊所組成的四個三角形面積和大于四邊形的面積。如果對于車輛的每一個狀態(tài), {Pj,k|」=1,2士=1,2,3,4}都在其四邊形之外,并且對于每一個障礙物,車輛的每一個狀態(tài) 的四個角點(diǎn){(^,(^,(^,(^丨都在其四邊形之外屬可以判定車輛的每一個狀態(tài)都是安 全無碰撞的。因此,待優(yōu)化變量的不等式約束為:
[0072]
[0073] 其中,SKnPu)表示IV與四邊形匕所形成的四個三角形的面積和,SA表示四邊形 如勺面積,表示C1>k與四邊形匕所形成的四個三角形的面積和,SP謙示四邊形匕的 面積;α為大于1的安全系數(shù),例如1.〇5,α越大,車輛與障礙物的安全間距越大。
[0074] 設(shè)定優(yōu)化目標(biāo)為時間最短,即目標(biāo)函數(shù)為:
[0075] Tf = N*4h (6)
[0076] 車輛在泊車過程中具有一個限定的活動范圍,如其不能過分地侵入另一條車道從 而妨礙其他車輛通行,同時也給自身帶來安全隱患。另外道路兩側(cè)可能為墻壁一類的空間 禁區(qū),因此應(yīng)當(dāng)給泊車路徑規(guī)劃施加運(yùn)動范圍約束。泊車過程中,車輛的四個角點(diǎn)不可逾越 限定的運(yùn)動范圍,因此車輛的運(yùn)動范圍約束為:
[0077]
[0078] 其中,%表示車輛的第i個狀態(tài)時第k個角點(diǎn)的X軸坐標(biāo),1??表示車輛的第i個 狀態(tài)時第k個角點(diǎn)的Y軸坐標(biāo),和ycik可以由車輛狀態(tài)參數(shù)和車輛的幾何參數(shù)計(jì)算得 出。
[0079]使用非線性規(guī)劃求解器,例如IP0PT、SN0PT,求解以上得到的帶約束非線性規(guī)劃問 題:
[0080]
[0081]當(dāng)泊車環(huán)境過于苛刻時,求解器無法求得符合約束的解,此時判定泊車路徑規(guī)劃 失敗。否則,求解得到的結(jié)果即為代表時間最短泊車路徑的4N+1個車輛狀態(tài),使用分段拉格 朗日插值法進(jìn)行擬合即可得到車輛狀態(tài)變量和控制變量在任意時刻的值。為防止時間步長 h過大而導(dǎo)致所規(guī)劃出的兩個車輛狀態(tài)間存在碰撞而未被檢測到,以0.01秒為細(xì)化的時間 步長,得到更加細(xì)化的車輛狀態(tài)序列。檢測細(xì)化的車輛狀態(tài)序列中的每一個車輛狀態(tài)是否 發(fā)生碰撞,若否,則判定泊車路徑規(guī)劃成功。否則,則增加分段數(shù),例如令分段數(shù)為2N,重新 進(jìn)行路徑規(guī)劃。當(dāng)重復(fù)3次以后如果仍然存在碰撞則判定泊車路徑規(guī)劃失敗。
[0082] 最后當(dāng)泊車路徑規(guī)劃成功時,泊車控制器對動力系統(tǒng)、制動系統(tǒng)和轉(zhuǎn)向系統(tǒng)進(jìn)行 實(shí)時控制以執(zhí)行跟蹤所規(guī)劃出的泊車路徑。
[0083] 本發(fā)明適用于多種泊車場景,圖4和圖5分別示意了車輛斜向泊車和平行泊車情況 下應(yīng)用本發(fā)明所得到的泊車路徑。
[0084] 應(yīng)當(dāng)注意的是,本發(fā)明并非限定泊車位周圍障礙物為其他車輛,也可以是其他障 礙物,如可能為車位地鎖,同樣適用于本發(fā)明應(yīng)用場景。另外應(yīng)當(dāng)注意的是,本發(fā)明并未限 定泊車位周圍障礙物數(shù)量為2,其他數(shù)量的障礙物也適用于本發(fā)明應(yīng)用場景。
[0085] 以上所述實(shí)施方式僅僅是對本發(fā)明的優(yōu)選實(shí)施方式進(jìn)行描述,并非對本發(fā)明的范 圍進(jìn)行限定,在不脫離本發(fā)明設(shè)計(jì)精神的前提下,本領(lǐng)域普通技術(shù)人員對本發(fā)明的技術(shù)方 案作出的各種變形和改進(jìn),均應(yīng)落入本發(fā)明的權(quán)利要求書確定的保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,該方法用于自主泊車系統(tǒng)檢 測到可用泊車空間而將車輛自動地停泊于所述泊車空間中,其特征在于,包括以下步驟: (1) 檢測目標(biāo)車位信息,確定泊車情景; (2) 根據(jù)所述目標(biāo)車位信息和泊車情景,確定待停泊車輛的初始狀態(tài)和目標(biāo)狀態(tài); (3) 基于前輪轉(zhuǎn)向四輪車輛的阿克曼模型,建立車輛運(yùn)動學(xué)微分方程; (4) 對車輛狀態(tài)變量和控制變量進(jìn)行分段,并按照一定的時間步長對每一分段進(jìn)行等 距采樣,得到待優(yōu)化變量; (5) 采用拉格朗日插值法將車輛狀態(tài)變量每個采樣點(diǎn)上的微分表示為該采樣點(diǎn)所在分 段上各個采樣點(diǎn)的函數(shù),聯(lián)立所述函數(shù)與車輛運(yùn)動學(xué)微分方程,使所述車輛運(yùn)動學(xué)微分方 程轉(zhuǎn)化為代數(shù)方程,形成待優(yōu)化變量的等式約束; (6) 根據(jù)車輛運(yùn)動的物理限制和泊車的安全要求,形成待優(yōu)化變量的邊界約束; (7) 根據(jù)目標(biāo)車位周圍障礙物,公式化避撞要求,形成待優(yōu)化變量的不等式約束; (8) 根據(jù)車輛在泊車過程中的運(yùn)動范圍限制,形成待停泊車輛的運(yùn)動范圍約束; (9) 確定優(yōu)化目標(biāo),建立目標(biāo)函數(shù); (10) 采用非線性規(guī)劃求解器,得到泊車路徑的最優(yōu)解。2. 根據(jù)權(quán)利要求1所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,還包括以下步驟: 采用拉格朗日插值法對所述泊車路徑的最優(yōu)解中的車輛狀態(tài)變量進(jìn)行擬合,以細(xì)化的 時間步長對擬合后的車輛狀態(tài)變量進(jìn)行采樣,得到細(xì)化的車輛狀態(tài)序列,檢測所述細(xì)化的 車輛狀態(tài)序列中的每個車輛狀態(tài)是否發(fā)生碰撞,若是,則增加分段數(shù),重復(fù)步驟(4)~(10) 重新進(jìn)行路徑規(guī)劃。3. 根據(jù)權(quán)利要求1所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,步驟(1)中,所述目標(biāo)車位信息包括目標(biāo)車位的朝向、位置、長度和寬度以及目標(biāo)車位周 圍障礙物位置;所述泊車情景包括垂直泊車、斜向泊車和平行泊車。4. 根據(jù)權(quán)利要求1所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,步驟(3)中,所述車輛運(yùn)動學(xué)微分方稈為:其中,X表示車輛后軸中心在笛卡爾坐標(biāo)系中的橫坐標(biāo),y表示車輛后軸中心在笛卡爾 坐標(biāo)系中的縱坐標(biāo),v表示車輛后軸中心的移動速度,Θ表示車輛朝向與笛卡爾坐標(biāo)系X軸的 夾角,費(fèi)表示車輛前輪轉(zhuǎn)角,a表示車輛后軸中心的加速度,ω表示車輛前輪轉(zhuǎn)角的角速度, Lm表示車輛前軸與后軸之間的距離; 步驟(4)中,所述車輛狀態(tài)變量為x、y、v、0、零,所述車輛控制變量為a、ω,對車輛狀態(tài)變 量和控制變量進(jìn)行分段,設(shè)分段數(shù)為Ν,每一分段包含等距的Μ個采樣點(diǎn),每一分段時間長度 為(M-l)h,變量^7、10、爭、(〇離散后的每一分段的1個采樣點(diǎn)中,兩端的采樣點(diǎn)與相鄰的 分段共用,變量a離散后的每一分段包含Μ個獨(dú)立的采樣點(diǎn),則所述待優(yōu)化變量為:其中,h表示時間步長;11,1=0,1,...,(1-1州表示變量1離散后第1個采樣點(diǎn)的值;71,1 =0,1,. . .,(M-1)N表示變量y離散后第i個采樣點(diǎn)的值;Vi,i = 0,l,. . .,(M-1)N表示變量v 離散后第i個采樣點(diǎn)的值;9i,i = 〇,l,. . .,(M-1)N表示變量Θ離散后第i個采樣點(diǎn)的值,表示變量f離散后第i個采樣點(diǎn)的值;Wi,i=〇,l,. N表示變量ω離散后第i個采樣點(diǎn)的值;ap,p = 〇,l,. . .,MN_1表示變量a離散后第p個采樣點(diǎn) 的值。5. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,采用拉格朗日插值法將車輛狀態(tài)變量x、y、v、9、f每個采樣點(diǎn)上的微分表示為該采樣點(diǎn) 所在分段上Μ個采樣點(diǎn)的函數(shù):聯(lián)立所述函數(shù)與車輛運(yùn)動學(xué)微分方程,使所述車輛運(yùn)動學(xué)微分方程轉(zhuǎn)化為代數(shù)方程, 形成待優(yōu)化變量的等式約束: S/n,m*h-f(tn,m)*h = 0,n = 0,l,= . . . ,Μ-1 其中,S代表車輛狀態(tài)變量X、y、V、θ、零,Sn,m表示車輛狀態(tài)變量在時刻tn,?處的值,γ n,m表 示車輛狀態(tài)變量在時刻tmm處的系數(shù),表示車輛狀態(tài)變量在時刻tn,m處的導(dǎo)數(shù),tn,m = h* [(M-l)*n+m]表示第η個分段內(nèi)第m個采樣點(diǎn)處的時刻。6. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,所述待優(yōu)化變量的邊界約束為:其中,hmax表示時間步長的最大限值,111)、711)、¥11 )、311)、911)、^|%、<^11)分別表示變量1、7、¥、 a、9、φ、ω 的下限值,11^、5^、¥111)、3111)、0111)、爭_、《 111)分別表示變量1、7、¥、3、9、<|>、(〇的上限 值;(?,b , θΖ? φ J表示待停泊車輛的初始狀態(tài),C%, )? V& Θ&御)表示待停泊車輛的 目標(biāo)狀態(tài);11、71、^、91、%、《1分別表示變量^7、 ¥、0、,《離散后第1個采樣點(diǎn)的值,&[)表 示變量a離散后第p個采樣點(diǎn)的值。7. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,所述待優(yōu)化變量的不等式約束為:其中,(:康示待停泊車輛處于第i個狀態(tài)時被抽象成的四邊形,匕表示第j個障礙物被抽 象成的四邊形,J表示障礙物的數(shù)量,Pik表示四邊形匕的第k個角點(diǎn),C1>k表示四邊形G的第k 個角點(diǎn),表示P>k與四邊形匕所形成的四個三角形的面積和,SA表示四邊形Q的面 積,表示C 1>k與四邊形匕所形成的四個三角形的面積和,SP謙示四邊形匕的面積, α為大于1的安全系數(shù)。8. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,待停泊車輛的運(yùn)動范圍約束為:其中表示待停泊車輛處于第i個狀態(tài)時被抽象成的四邊形的第k個角點(diǎn)的X軸坐 標(biāo),^&表示待停泊車輛處于第i個狀態(tài)時被抽象成的四邊形的第k個角點(diǎn)的Y軸坐標(biāo);xlb、 Xub分別表示車輛狀態(tài)變量X的下限值和上限值,yib、yub分別表示車輛狀態(tài)變量y的下限值和 上限值。9. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,目標(biāo)函數(shù)為: Tf = N*(M-l)h 其中,Tf表示待停泊車輛的路徑規(guī)劃時間; 優(yōu)化目標(biāo)為時間最短,即min Tf。10. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法,其特征在 于,Μ為整數(shù)且4 < MS 8。
【文檔編號】B60W30/06GK105857306SQ201610240384
【公開日】2016年8月17日
【申請日】2016年4月14日
【發(fā)明人】梅濤, 葉林銓, 祝輝, 梁華為, 袁勝, 王少平
【申請人】中國科學(xué)院合肥物質(zhì)科學(xué)研究院