一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法
【專利摘要】本發(fā)明提供一種用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，用于自主泊車系統(tǒng)檢測到可用泊車空間而將車輛自動地停泊于所述泊車空間中，包括：檢測目標(biāo)車位信息，確定泊車情景；確定待停泊車輛的初始狀態(tài)和目標(biāo)狀態(tài)；建立車輛運(yùn)動學(xué)微分方程；對車輛狀態(tài)變量和控制變量進(jìn)行分段，并按照一定的時(shí)間步長對每一分段進(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)停車位日趨緊張和狹小。對于新 手駕駛員，泊車通常是一個(gè)難題，特別是對于車位過于狹窄的情況，駕駛員往往難以很好地 控制汽車進(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ī)劃路徑中假定了車輛僅 向一個(gè)方向運(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ā)生事故。另外不利的是，其未檢測離散后的兩個(gè)車 輛狀態(tài)之間是否發(fā)生碰撞，可能造成車輛沿規(guī)劃軌跡行駛時(shí)發(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)行分段，并按照一定的時(shí)間步長對每一分段進(jìn) 行等距采樣，得到待優(yōu)化變量；
[0014] (5)米用拉格朗日插值法將車輛狀態(tài)變量每個(gè)米樣點(diǎn)上的微分表不為該米樣點(diǎn)所 在分段上各個(gè)采樣點(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ì) 化的時(shí)間步長對擬合后的車輛狀態(tài)變量進(jìn)行采樣，得到細(xì)化的車輛狀態(tài)序列，檢測所述細(xì) 化的車輛狀態(tài)序列中的每個(gè)車輛狀態(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ù)為Ν，每一分段包含等距的Μ個(gè)采樣點(diǎn)，每一分段時(shí) 間長度為(M-l)h，變量x、y、v、0、ω離散后的每一分段的Μ個(gè)采樣點(diǎn)中，兩端的采樣點(diǎn)與 相鄰的分段共用，變量a離散后的每一分段包含Μ個(gè)獨(dú)立的采樣點(diǎn)，則所述待優(yōu)化變量為：
[0027]
[0028] 其中，h表示時(shí)間步長;Xi，i = 0，1，…，（M-l )Ν表示變量X離散后第i個(gè)采樣點(diǎn)的值； yi，i = 0，l，···，(M-l)N表示變量y離散后第i個(gè)采樣點(diǎn)的值;vi，i=0，l，···，(M-l)N表示變量v 離散后第i個(gè)采樣點(diǎn)的值；01，1 = 〇，1，一，（1-10表示變量Θ離散后第i個(gè)采樣點(diǎn)的值， ?^:1.? (M - 1.)謂表示變量零離散后第i個(gè)采樣點(diǎn)的值；ω i，i = 〇，1，…，(M-l )N 表示變量ω離散后第i個(gè)采樣點(diǎn)的值;aP，p = 0，1，…，MN_1表示變量a離散后第p個(gè)采樣點(diǎn)的 值。
[0029] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，采用拉格朗日插值法將 車輛狀態(tài)變量x、y、v、9、f每個(gè)采樣點(diǎn)上的微分表示為該采樣點(diǎn)所在分段上Μ個(gè)采樣點(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)變量在時(shí)刻t n,m處的值， Y mm表示車輛狀態(tài)變量在時(shí)刻tn,m處的系數(shù)，S' n,m表示車輛狀態(tài)變量在時(shí)刻tn,m處的導(dǎo)數(shù)， tn,m=h*[(M-l) *n+m ]表示第η個(gè)分段內(nèi)第m個(gè)采樣點(diǎn)處的時(shí)刻。
[0034] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，所述待優(yōu)化變量的邊界 約束為：
[0035]
[0036] 其中，hmax表不時(shí)間步長的最大限值，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個(gè)采樣點(diǎn)的值， aP表不變量散后第p個(gè)米樣點(diǎn)的值。
[0037] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，所述待優(yōu)化變量的不等 式約束為：
[0038]
[0039] 其中，(^表示待停泊車輛處于第i個(gè)狀態(tài)時(shí)被抽象成的四邊形，匕表示第j個(gè)障礙物 被抽象成的四邊形，J表示障礙物的數(shù)量，Pik表示四邊形匕的第k個(gè)角點(diǎn)，C 1>k表示四邊形Q 的第k個(gè)角點(diǎn)，SKnPM)表示I^,k與四邊形(^所形成的四個(gè)三角形的面積和，SA表示四邊形 如勺面積，表示C 1>k與四邊形匕所形成的四個(gè)三角形的面積和，SP謙示四邊形匕的 面積，α為大于1的安全系數(shù)。
[0040] 所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，待停泊車輛的運(yùn)動范圍 約束為：
[0041]
[0042] 其中，表示待停泊車輛處于第i個(gè)狀態(tài)時(shí)被抽象成的四邊形的第k個(gè)角點(diǎn)的X 軸坐標(biāo)，！^表示待停泊車輛處于第i個(gè)狀態(tài)時(shí)被抽象成的四邊形的第k個(gè)角點(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ī)劃時(shí)間；
[0046] 優(yōu)化目標(biāo)為時(shí)間最短，即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í)際車輛抽象為一個(gè)矩形的車輛模型，該車輛模型符合阿克曼轉(zhuǎn)向 原理。車輛具有長度L和寬度W。車輛的位置以實(shí)際車輛后軸中心的位置(x，y)來表示。后軸 中心到車尾距離為Lr，前軸中心到車頭距離為L f，前后軸間距為Lm。車輛后軸中心速度為V， 車輛前輪轉(zhuǎn)角為寧，車輛朝向與全局坐標(biāo)系X軸夾角為Θ。
[0058]如圖3所示，一個(gè)典型的垂直泊車位兩邊都停泊有車輛，將其抽象為四邊形丹和內(nèi)， 通過感知系統(tǒng)測得其四個(gè)角點(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ā)明時(shí)的狀態(tài)，一般的，初始狀態(tài)車速為〇，即ν ζ = 〇。1??為所規(guī)劃路徑 中車輛的第i個(gè)狀態(tài)，其四個(gè)角點(diǎn)以
[0059]符合阿克曼轉(zhuǎn)向原理的前輪轉(zhuǎn)向四輪車輛模型轉(zhuǎn)向時(shí)具有一個(gè)轉(zhuǎn)向中心，并且位 于后軸延伸線上。在低速情況下，可以忽略輪胎的滑移，車輛運(yùn)動學(xué)微分方程可以表示為：
[0060]
[0061]車輛狀態(tài)變量由表示，控制變量由（a，ω)表示。車輛狀態(tài)變量和控 制變量在時(shí)間t上是連續(xù)的，在一系列時(shí)刻上對其采樣則形成車輛的一系列狀態(tài)。對車輛狀 態(tài)變量和控制變量進(jìn)行分段，初始地，設(shè)定分段數(shù)為N，例如N= 10，每一分段包含等距的5個(gè) 米樣點(diǎn)，每一分段時(shí)間長度為4h，變量x、y、v、0、爭、ω離散后的每一分段的5個(gè)米樣點(diǎn)中，兩 端的采樣點(diǎn)與相鄰的分段共用，變量a離散后的每一分段包含5個(gè)獨(dú)立的采樣點(diǎn)；得到待優(yōu) 化變量為：
[00621
[0063]每一個(gè)采樣點(diǎn)上的微分可以使用拉格朗日插值法表示為該分段上5個(gè)采樣點(diǎn)的函 數(shù)：
[0064]
[0065] 其中，sn,m，η = 0，1，…，N-l;m = 0，1，…，4為狀態(tài)變量s即_5暴巧爲(wèi)在時(shí)刻tn,m 處的值，為狀態(tài)變量s在時(shí)刻tn,m處的導(dǎo)數(shù)，tn,m = h*(4*n+m)表示第η個(gè)分段內(nèi)第m個(gè)采 樣點(diǎn)處的時(shí)刻。
[0066] 愈、&氣_均為時(shí)間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)角角速度在正反兩個(gè)方向都有 一個(gè)最大值。同時(shí)出于泊車過程的安全性考慮，車輛運(yùn)動范圍（x，y)以及車輛速度和加速度 應(yīng)該進(jìn)行限定。時(shí)間步長h和分段數(shù)N以及采樣點(diǎn)數(shù)5共同決定了泊車用時(shí)，通常泊車不應(yīng)耗 費(fèi)太長時(shí)間，因此對時(shí)間步長h進(jìn)行限制。同時(shí)，規(guī)劃結(jié)果中車輛第一個(gè)和最后一個(gè)狀態(tài)應(yīng) 分別等于車輛初始狀態(tài)和目標(biāo)狀態(tài)。綜上所述，待優(yōu)化變量的邊界約束如下：
[0069]
[0070] 其中，hmax表不時(shí)間步長的最大限值，乂11)、：711)、￥11 )、&11)、911)、吩您_、<^11)分別表不變量 叉、5^、&、9、擎、(〇的下限值山1 )、5^、￥111)、&111)、9111)、爭油.、《111 )分別表示變量叉、7、￥、&、9、屮、（〇 的上限值。
[0071] 車輛沿著規(guī)劃路徑泊車過程中不與障礙物發(fā)生碰撞是最重要的。使用面積法可以 判斷一個(gè)點(diǎn)是否位于一個(gè)四邊形之內(nèi)：當(dāng)點(diǎn)位于四邊形之內(nèi)時(shí)，該點(diǎn)與該四邊形的四條邊 所組成的四個(gè)三角形面積和等于四邊形的面積；當(dāng)點(diǎn)位于四邊形之外時(shí)，該點(diǎn)與該四邊形 的四條邊所組成的四個(gè)三角形面積和大于四邊形的面積。如果對于車輛的每一個(gè)狀態(tài)， {Pj,k|」=1，2士=1，2,3,4}都在其四邊形之外，并且對于每一個(gè)障礙物，車輛的每一個(gè)狀態(tài) 的四個(gè)角點(diǎn){(^，(^，(^，(^丨都在其四邊形之外屬可以判定車輛的每一個(gè)狀態(tài)都是安 全無碰撞的。因此，待優(yōu)化變量的不等式約束為：
[0072]
[0073] 其中，SKnPu)表示IV與四邊形匕所形成的四個(gè)三角形的面積和，SA表示四邊形 如勺面積，表示C1>k與四邊形匕所形成的四個(gè)三角形的面積和，SP謙示四邊形匕的 面積;α為大于1的安全系數(shù)，例如1.〇5，α越大，車輛與障礙物的安全間距越大。
[0074] 設(shè)定優(yōu)化目標(biāo)為時(shí)間最短，即目標(biāo)函數(shù)為：
[0075] Tf = N*4h (6)
[0076] 車輛在泊車過程中具有一個(gè)限定的活動范圍，如其不能過分地侵入另一條車道從 而妨礙其他車輛通行，同時(shí)也給自身帶來安全隱患。另外道路兩側(cè)可能為墻壁一類的空間 禁區(qū)，因此應(yīng)當(dāng)給泊車路徑規(guī)劃施加運(yùn)動范圍約束。泊車過程中，車輛的四個(gè)角點(diǎn)不可逾越 限定的運(yùn)動范圍，因此車輛的運(yùn)動范圍約束為：
[0077]
[0078] 其中，％表示車輛的第i個(gè)狀態(tài)時(shí)第k個(gè)角點(diǎn)的X軸坐標(biāo)，1??表示車輛的第i個(gè) 狀態(tài)時(shí)第k個(gè)角點(diǎn)的Y軸坐標(biāo)，和ycik可以由車輛狀態(tài)參數(shù)和車輛的幾何參數(shù)計(jì)算得 出。
[0079]使用非線性規(guī)劃求解器，例如IP0PT、SN0PT，求解以上得到的帶約束非線性規(guī)劃問 題：
[0080]
[0081]當(dāng)泊車環(huán)境過于苛刻時(shí)，求解器無法求得符合約束的解，此時(shí)判定泊車路徑規(guī)劃 失敗。否則，求解得到的結(jié)果即為代表時(shí)間最短泊車路徑的4N+1個(gè)車輛狀態(tài)，使用分段拉格 朗日插值法進(jìn)行擬合即可得到車輛狀態(tài)變量和控制變量在任意時(shí)刻的值。為防止時(shí)間步長 h過大而導(dǎo)致所規(guī)劃出的兩個(gè)車輛狀態(tài)間存在碰撞而未被檢測到，以0.01秒為細(xì)化的時(shí)間 步長，得到更加細(xì)化的車輛狀態(tài)序列。檢測細(xì)化的車輛狀態(tài)序列中的每一個(gè)車輛狀態(tài)是否 發(fā)生碰撞，若否，則判定泊車路徑規(guī)劃成功。否則，則增加分段數(shù)，例如令分段數(shù)為2N，重新 進(jìn)行路徑規(guī)劃。當(dāng)重復(fù)3次以后如果仍然存在碰撞則判定泊車路徑規(guī)劃失敗。
[0082] 最后當(dāng)泊車路徑規(guī)劃成功時(shí)，泊車控制器對動力系統(tǒng)、制動系統(tǒng)和轉(zhuǎn)向系統(tǒng)進(jìn)行 實(shí)時(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)行分段，并按照一定的時(shí)間步長對每一分段進(jìn)行等 距采樣，得到待優(yōu)化變量； (5) 采用拉格朗日插值法將車輛狀態(tài)變量每個(gè)采樣點(diǎn)上的微分表示為該采樣點(diǎn)所在分 段上各個(gè)采樣點(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ì)化的 時(shí)間步長對擬合后的車輛狀態(tài)變量進(jìn)行采樣，得到細(xì)化的車輛狀態(tài)序列，檢測所述細(xì)化的 車輛狀態(tài)序列中的每個(gè)車輛狀態(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ù)為Ν，每一分段包含等距的Μ個(gè)采樣點(diǎn)，每一分段時(shí)間長度 為(M-l)h，變量^7、10、爭、(〇離散后的每一分段的1個(gè)采樣點(diǎn)中，兩端的采樣點(diǎn)與相鄰的 分段共用，變量a離散后的每一分段包含Μ個(gè)獨(dú)立的采樣點(diǎn)，則所述待優(yōu)化變量為：其中，h表示時(shí)間步長;11，1=0，1，...，（1-1州表示變量1離散后第1個(gè)采樣點(diǎn)的值;71，1 =0，1，. . .，（M-1)N表示變量y離散后第i個(gè)采樣點(diǎn)的值;Vi，i = 0，l，. . .，（M-1)N表示變量v 離散后第i個(gè)采樣點(diǎn)的值；9i，i = 〇，l，. . .，（M-1)N表示變量Θ離散后第i個(gè)采樣點(diǎn)的值，表示變量f離散后第i個(gè)采樣點(diǎn)的值；Wi，i=〇，l，. N表示變量ω離散后第i個(gè)采樣點(diǎn)的值;ap，p = 〇，l，. . .，MN_1表示變量a離散后第p個(gè)采樣點(diǎn) 的值。5. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，其特征在 于，采用拉格朗日插值法將車輛狀態(tài)變量x、y、v、9、f每個(gè)采樣點(diǎn)上的微分表示為該采樣點(diǎn) 所在分段上Μ個(gè)采樣點(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)變量在時(shí)刻tn,?處的值，γ n,m表 示車輛狀態(tài)變量在時(shí)刻tmm處的系數(shù)，表示車輛狀態(tài)變量在時(shí)刻tn,m處的導(dǎo)數(shù)，tn,m = h* [(M-l)*n+m]表示第η個(gè)分段內(nèi)第m個(gè)采樣點(diǎn)處的時(shí)刻。6. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，其特征在 于，所述待優(yōu)化變量的邊界約束為：其中，hmax表示時(shí)間步長的最大限值，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個(gè)采樣點(diǎn)的值，&[)表 示變量a離散后第p個(gè)采樣點(diǎn)的值。7. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，其特征在 于，所述待優(yōu)化變量的不等式約束為：其中，(:康示待停泊車輛處于第i個(gè)狀態(tài)時(shí)被抽象成的四邊形，匕表示第j個(gè)障礙物被抽 象成的四邊形，J表示障礙物的數(shù)量，Pik表示四邊形匕的第k個(gè)角點(diǎn)，C1>k表示四邊形G的第k 個(gè)角點(diǎn)，表示P>k與四邊形匕所形成的四個(gè)三角形的面積和，SA表示四邊形Q的面 積，表示C 1>k與四邊形匕所形成的四個(gè)三角形的面積和，SP謙示四邊形匕的面積， α為大于1的安全系數(shù)。8. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，其特征在 于，待停泊車輛的運(yùn)動范圍約束為：其中表示待停泊車輛處于第i個(gè)狀態(tài)時(shí)被抽象成的四邊形的第k個(gè)角點(diǎn)的X軸坐 標(biāo)，^&表示待停泊車輛處于第i個(gè)狀態(tài)時(shí)被抽象成的四邊形的第k個(gè)角點(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ī)劃時(shí)間； 優(yōu)化目標(biāo)為時(shí)間最短，即min Tf。10. 根據(jù)權(quán)利要求4所述的用于多種泊車場景的車輛自主泊車路徑規(guī)劃方法，其特征在 于，Μ為整數(shù)且4 < MS 8。
【文檔編號】B60W30/06GK105857306SQ201610240384
【公開日】2016年8月17日
【申請日】2016年4月14日
【發(fā)明人】梅濤, 葉林銓, 祝輝, 梁華為, 袁勝, 王少平
【申請人】中國科學(xué)院合肥物質(zhì)科學(xué)研究院