一種基于全聯(lián)立動態(tài)優(yōu)化框架的自主泊車軌跡優(yōu)化方法
【技術(shù)領域】
[0001] 本發(fā)明涉及無人駕駛系統(tǒng)中關(guān)鍵的自主泊車軌跡優(yōu)化技術(shù)。
【背景技術(shù)】
[0002] 今年來自主泊車（auto-parkingsystem，APS)技術(shù)發(fā)展迅猛，作為無人駕駛系統(tǒng) 的重要組成部分，其目的是輔助或替代駕駛員完成車輛的安全泊位。隨著車輛的增多，泊車 位空間的緊縮，駕駛員泊車更加困難，因此自主泊車技術(shù)快速發(fā)展起來。
[0003] 國內(nèi)外學者主要用以下兩種方法研究自主泊車：（1)基于模糊控制的方法：將技 術(shù)成熟的駕駛員的泊車經(jīng)驗整理為模糊規(guī)則，通過控制汽車轉(zhuǎn)向角和相對停車位的位置實 現(xiàn)泊車。然而控制過程缺乏連貫的規(guī)劃性，需要反復調(diào)整車速和轉(zhuǎn)向角，前后挪動需要較大 停車空間，難以量化。（2)基于路徑規(guī)劃的方法運用A*、D*等多種搜索算法尋找優(yōu)化路徑。 例如Dubins提出的最小轉(zhuǎn)向半徑圓、單方向從任意起始位置運動到任意目標位置的最短 路徑規(guī)劃等。由此發(fā)展而來的幾何法成為目前實現(xiàn)障礙環(huán)境下泊車的主要方法，即通過分 析泊車環(huán)境約束，結(jié)合最小轉(zhuǎn)向半徑圓和直線段得到分段連續(xù)的無碰路徑。然而幾何法在 處理自主泊車問題時有其固有的局限性：（1)幾何法下得到的無碰路徑不一定滿足小車的 運動學約束和物理約束，所以不一定可行。（2)幾何路徑不包含與時間相關(guān)的信息，所以需 要通過控制等方法進行再次規(guī)劃，將無碰路徑轉(zhuǎn)換為帶有時間信息的、車輛可跟蹤的軌跡。
[3] 幾何法只能得到最短泊車路徑，而無法實施如泊車時間最短、油耗最少等目標函數(shù)下的 優(yōu)化。（4)幾何法受限于車位形狀和泊車軌形，例如在垂直泊車時設計的是1/4圓弧和直線 段相接的軌形，在平行泊車中運用最小轉(zhuǎn)向半徑圓設計兩段圓弧相接的S形路徑，而不同 的軌形又需要研究不同的轉(zhuǎn)向策略。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是針對現(xiàn)有技術(shù)的不足，在聯(lián)立框架下研究自主泊車的最短時間軌 跡優(yōu)化問題。通過建立包括車輛與泊車環(huán)境在內(nèi)的行車系統(tǒng)模型，構(gòu)造最短時間動態(tài)優(yōu)化 命題，運用高性能非線性規(guī)劃求解算法得到同時滿足無碰、以及車輛物理約束的泊車軌跡。
[0005] 本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的：一種基于全聯(lián)立動態(tài)優(yōu)化框架的自 主泊車軌跡優(yōu)化方法，包括以下步驟：
[0006] (1)基于前輪驅(qū)動小車建立自主泊車過程車輛低速運動學模型，如式（1)所示；
[0007]
[0008] 其中，（x，y)表示小車后輪軸中心點的坐標，V表示前軸中心點縱向速度，a表示前 軸中心點縱向加速度，Φ表示小車前軸中心點轉(zhuǎn)向角；w表示小車前軸中心點轉(zhuǎn)向角速度； Θ表示車輛中心軸與水平方向的夾角；
[0009] (2)根據(jù)具體泊車車型輸入車體參數(shù)及車體運動中的物理極限約束，所述車體參 數(shù)包括：車輛前后輪軸距L;前懸長度Lp;后懸長度Lr;所述車體運動中的物理極限約束包 括：最高車速vmax、最大前輪轉(zhuǎn)角Φ_、最大加速度amax、最大角速度wmax等；其中，
[0010]
[0011] (3)確定待泊車位在泊車系統(tǒng)中的相對尺寸，以泊車位底部中點為原點，確定底部 與泊車入口車位線間的距離yl、底部寬度PW、以及車位傾斜度la。平行泊車、垂直泊車的車 位傾斜度為〇,斜式車位的車位傾斜度從0°變化到90°，角度越大越傾斜。
[0012] (4)建立基于MPCC(mathematicalprogramswithcomplementarity constraints)的車位避障模型，步驟如下：
[0013]自主泊車避障的條件約束如式（3)所示，含義是小車四角如果不在車位線上方則 必然被夾在車位線和車位底部之間的平行線區(qū)域內(nèi)：
[0014]
[0015] (4. 1)明確參數(shù)計算規(guī)則：
[0016]其中Xll，xlu表示車位平行線區(qū)域的兩邊界，根據(jù)步驟（3)中的車位底部寬度和傾 斜角度來確定。由步驟（3)中設定的地面坐標系可知：
[0017] (a)平行泊車模式下：LI= -pw/2 ;L2 =pw/2
[0018] (b)垂直泊車模式下：LI= -pw/2 ;L2 =pw/2
[0019] (c)斜式泊車模式下：LI=cot(la) (x+pw/2) ;L2 =cot(la) (x-pw/2)
[0020] i表示小車車身朝向X軸正方向時從右后輪逆時針環(huán)繞到左后輪的四角序號，根 據(jù)步驟（2)中的車體參數(shù)L，Lp，Lr可以計算出小車任意位姿下的車身四角坐標，如公式 (4)：
[0021]
[0022] (4. 2)將公式（3)中的條件約束轉(zhuǎn)化為帶有互補約束的MPCC可以處理的模型：
[0023] 通過引入非負輔助變量sP，sn，μ得到公式（5):
[0024]
[0025] (4. 2. 1)將公式（5)中的模型轉(zhuǎn)化為MPCC下的Reg(ε)模型：
[0026]
[0027]ε為任意小的正數(shù)，表征模型轉(zhuǎn)化等價的精度，越小則越逼近原來的車位條件避 障模型。％是小車距離車位平行線邊界的安全裕量。這種轉(zhuǎn)化下優(yōu)化目標不變?nèi)詾闀r間 最短：minTf〇
[0028] (4· 2· 2)也可以將公式（5)中模型轉(zhuǎn)化為PF(P)模型：
[0029]
[0030] P為懲罰因子，這種轉(zhuǎn)化通過改變目標函數(shù)簡化了約束條件，懲罰因子越大優(yōu)化 模型的解越符合原來的車位避障約束。
[0031] (4. 3)MPCC技術(shù)轉(zhuǎn)換的車位避障模型可以限制小車的四角在車位線之外，為了防 止車位兩拐角撞入車身，還需要增加一些約束。這里采用面積法進行判斷：若車位拐點位于 矩形小車之外，那么該點與矩形四角連成的三角形面積之和大于矩形的面積，否則二者面 積相等。
[0032] (5)由步驟（1)、（2)、（4)構(gòu)造的以最短時間為優(yōu)化目標的自主泊車軌跡優(yōu)化命題 如式⑶所示：
[0033]
[0034] 當泊車避障約束選擇MPCC-Reg模型，即公式（6)時，γ= 0;當選擇MPCC-PF模 型，即公式（7) _
[0035] 確定待泊車的初始位姿參數(shù)（X。，y。，Θ。，ν。，Φ。）和終止位姿參數(shù)（xtf，ytf，Θtf， vtf，Φ tf)〇
[0036] (6)對于步驟（5)中建立的軌跡優(yōu)化命題的求解分為兩步驟：
[0037] (6. 1)離散化：采用全聯(lián)立有限元正交配置的離散化方法：將步驟⑴中涉及的車 輛運動學模型變量（X，y，Θ，v，φ)通過選擇基于Radau正交配置點的Lagrange插值函數(shù) 進行離散化。其中（x，y，Θ，v，Φ)為模型的狀態(tài)變量，插值函數(shù)構(gòu)造如（9):
[0038]
[0039]K為插值階次，本發(fā)明選擇K=3,使離散化求解具有5階精度。（x，y，Θ)離散化 如（10):
[0040]
[0041]NE表示將優(yōu)化時間分成的有限元段數(shù)，Xlj、yij、Θ 別表示第i個有限元第j個 配置點上狀態(tài)變量的值。狀態(tài)變量的初值和終值條件為：
[0042]
[0043] 由于狀態(tài)變量可導，所以相鄰有限元連接處的節(jié)點上狀態(tài)變量值也應該連續(xù)，故 有下面的連續(xù)性條件：
[0044]
[0045] 控制變量為小車前輪軸中心縱向加速度和前輪轉(zhuǎn)角角速度（a，w)，其Lagrange插 值多項式如下：
[0046]
[0047] 離散化后如（14):
[0048]
[0049] 對于控制變量不要求在有限元節(jié)點處的連續(xù)性。
[0050] 相比于其他插值方法，Lagrange插值多項式的優(yōu)勢在于變量在各個配置點上的值 恰好等于其系數(shù)，BP
[0051]
[0052] 這樣軌跡動態(tài)優(yōu)化命題（8)離散化后的NLP命題形式如下：
[0053]
[0054] (6. 2)對于離散化后產(chǎn)生的大規(guī)模NLP問題（16)，調(diào)用基于內(nèi)點法的求解器ΙΡ0ΡΤ 來求解。一次性得到（x(ti,.j)，y(ti,.j)