平行泊車系統(tǒng)軌跡規(guī)劃與運(yùn)動控制方法

陳文強(qiáng)

(福建漢特云智能科技有限公司，福建 福州 350007)

隨著汽車保有量的不斷攀升，城市交通中泊車難的問題日益突出。自動泊車系統(tǒng)(automatic parking system，APS)作為自動駕駛輔助技術(shù)，集車輛感知、路徑規(guī)劃和運(yùn)動控制于一體，能有效解決停車對駕駛員造成的困擾[1]。

在泊車規(guī)劃方面，Chen等[2]基于改進(jìn)遺傳算法和時(shí)間增強(qiáng)的A*(Hybrid A Star)算法解決高密度停車場路徑規(guī)劃問題，有效提高了泊車效率和安全性。Jang等[3]將軌跡規(guī)劃問題視為由車輛動力學(xué)模型和可行駛區(qū)域約束組成的約束優(yōu)化問題，基于內(nèi)點(diǎn)法求解平行停車軌跡[3]。郭奕璀[4]基于模型預(yù)測控制(model predictive control，MPC)的動態(tài)窗口法進(jìn)行泊車速度規(guī)劃，實(shí)現(xiàn)對路徑的局部動態(tài)調(diào)整，同時(shí)滿足運(yùn)動學(xué)約束[4]。在運(yùn)動控制方面，Chen等[5]提出了一種新的自適應(yīng)偽譜(NAP)方法，基于時(shí)間-能量最優(yōu)控制模型求解停車時(shí)間和能量的最小化，仿真結(jié)果表明所提方法相比內(nèi)點(diǎn)法和分段高斯偽譜法具有更高的計(jì)算效率和精度[5]。馮欣陽等[6]選用指數(shù)趨近率設(shè)計(jì)滑?？刂破?，利用連續(xù)函數(shù)代替符號函數(shù)來降低抖振影響，仿真表明車輛能夠連續(xù)、無突變地完成泊車操作，抖振抑制效果較好[6]。國內(nèi)外學(xué)者在泊車路徑規(guī)劃與運(yùn)動控制方面有諸多成果，但多數(shù)僅對泊車軌跡規(guī)劃或縱/橫向控制進(jìn)行針對性研究，缺少一套從泊車軌跡規(guī)劃到控制的完整、可行的解決方案。

本研究針對平行泊車場景提出泊車軌跡規(guī)劃與運(yùn)動控制策略：在規(guī)劃層通過幾何法及三次樣條插值進(jìn)行泊車路徑規(guī)劃，并基于二次規(guī)劃求解滿足泊車過程約束的最優(yōu)速度曲線；在控制層建立車輛運(yùn)動學(xué)模型，設(shè)計(jì)PID縱向控制器和MPC橫向控制器，實(shí)現(xiàn)對期望軌跡的精確跟蹤，最后進(jìn)行Prescan/Simulink聯(lián)合仿真驗(yàn)證。

1 車輛建模

考慮泊車過程為低速行駛，無需考慮車輛穩(wěn)定性等動力學(xué)因素，因此基于運(yùn)動學(xué)模型構(gòu)建規(guī)劃與控制的車輛模型，如圖1所示。

圖1 運(yùn)動學(xué)模型Fig.1 Kinematic model

建立XOY慣性坐標(biāo)系， 則車輛運(yùn)動學(xué)模型可表示為[7]：

(1)

其中，(x,y)為車輛后軸中心定位點(diǎn)，L為車輛前后軸中心之間的軸距，v為車輛后軸中心沿縱向方向的速度，θ為車輛當(dāng)前橫擺角，δf為前輪轉(zhuǎn)角，R為轉(zhuǎn)彎半徑。

2 泊車軌跡規(guī)劃

2.1 泊車路徑規(guī)劃

采用圓弧切直線法[8]進(jìn)行平行泊車路徑規(guī)劃，并基于三次樣條插值實(shí)現(xiàn)曲率連續(xù)。

2.1.1 平行泊車路徑規(guī)劃

圓弧切直線法的數(shù)學(xué)模型如圖2所示。

圖2 圓弧切直線平行泊車Fig.2 Circular arc tangent straight line parallel parking

圖2中L、W分別為車位長、寬，D為當(dāng)前車道寬度，R1與R2為兩段圓弧的半徑，S為車輛初始位置到目標(biāo)點(diǎn)的水平距離，H為初始位置處車輛后軸中心至目標(biāo)點(diǎn)的垂直距離，θ為圓弧段對應(yīng)的圓心角。δ1、δ2為安全距離，用于泊車目標(biāo)點(diǎn)及最小平行泊車位[8]。

根據(jù)圖2所示，建立其數(shù)學(xué)模型為：

(2)

合并式(2)，可得：

(3)

獲取距離S及H后，令半徑R1=R2=Rmin，結(jié)合式(2)求解參數(shù)S1和θ，實(shí)現(xiàn)基于最小轉(zhuǎn)彎半徑的圓弧切直線平行泊車路徑規(guī)劃。

2.1.2 基于三次樣條插值的路徑優(yōu)化

基于幾何方法生成的泊車路徑存在曲率不連續(xù)，因此采用3次樣條插值保證路徑曲率連續(xù)。其定義如下[9]：

假設(shè)有n+1個(gè)控制點(diǎn)：

(4)

在單個(gè)區(qū)間[xi,xi+1]內(nèi)，三次多項(xiàng)式表示為：

fi(x)=ai+bi(x-xi)+

ci(x-xi)2+di(x-xi)3

(5)

式中,i=0,1,···,n-1，ai、bi、ci、di為待求的4n個(gè)未知數(shù)。最終求解得到包含n段三次樣條曲線的擬合曲線。

2.2 泊車速度規(guī)劃

針對泊車場景進(jìn)行速度規(guī)劃時(shí)需滿足位姿、距離等約束，故考慮基于最優(yōu)的方法實(shí)現(xiàn)速度規(guī)劃。本研究提出基于二次規(guī)劃(quadratic programming，QP)方法進(jìn)行泊車速度規(guī)劃[10]。

采用5次多項(xiàng)式曲線作為泊車行駛距離-時(shí)間(s-t)曲線表達(dá)式，構(gòu)建最優(yōu)問題求解曲線系數(shù)，其一階導(dǎo)數(shù)即為規(guī)劃的速度曲線。假設(shè)單次規(guī)劃總時(shí)長為T，則s-t曲線表示為：

φ(t)=u0+u1t+u2t2+u3t3+u4t4+u5t5

(6)

式中μi為曲線系數(shù)，t∈[0,T]，令：

(7)

對式(6)中各未知系數(shù)求解即可求得規(guī)劃時(shí)長T內(nèi)的s-t曲線，一階求導(dǎo)后即為所需要的最優(yōu)速度曲線。 求解過程如下：

首先，構(gòu)建代價(jià)函數(shù)。

所構(gòu)建的代價(jià)函數(shù)表達(dá)式為[10]：

(8)

式中,T為規(guī)劃總時(shí)長，εi(i=1,2,3,4)為權(quán)重系數(shù)，vmax為泊車過程的最大期望速度。C1項(xiàng)保證舒適性，即速度、加減速、加速度變化率平穩(wěn)變化；C2項(xiàng)保證時(shí)間因素和經(jīng)濟(jì)性，即盡可能提高泊車行駛速度。

接著，將上述代價(jià)函數(shù)轉(zhuǎn)化為標(biāo)準(zhǔn)QP形式：

C(t)=C1(t)+C2(t)=

M-ε4·H4·M

(9)

式中，H1、H2、H3、H4矩陣如下：

最后，設(shè)計(jì)過程約束。

(1)速度、位置約束

假設(shè)規(guī)劃的泊車路徑總距離為S′：

(10)

(2)單調(diào)約束

考慮行駛距離s與時(shí)間t為單調(diào)遞增的關(guān)系，相鄰采樣點(diǎn)間應(yīng)滿足：

λi·Mi-λi+1·Mi+1≤0

(11)

(3)最大速度約束

考慮泊車場景限速，考慮最大速度約束：

(12)

通過求解上述QP問題可得到最優(yōu)s-t曲線，對其進(jìn)行一階求導(dǎo)后即為規(guī)劃的速度曲線。該方法中所規(guī)劃的s-t速度曲線中總時(shí)長T可由基于規(guī)則的梯形規(guī)劃算法[11]獲取參考值，該方法在此不進(jìn)行贅述。

3 泊車運(yùn)動控制

3.1 縱向控制器設(shè)計(jì)

考慮到車輛具有高度非線性及參數(shù)時(shí)變特性，這將導(dǎo)致難以建立精確的縱向動力學(xué)模型[12]。因此，本研究選用位置式PID控制和基于速度誤差直接輸出期望油門/制動控制指令[13]。

位置式PID控制的離散化計(jì)算公式為[14]：

u(k)=KPe(k)+KI∑e(k)+

KD(e(k)-e(k-1))

(13)

式中，u(k)為期望油門或制動值；e(k)為速度偏差；∑e(k)為偏差累積和；KP、KI、KD分別為驅(qū)/制動PID系數(shù)。

式(13)的e(k)根據(jù)驅(qū)/制動模式定義為：

(14)

式中，v(k)為車輛當(dāng)前速度；vdes(k)為目標(biāo)點(diǎn)參考速度。

3.2 橫向控制器設(shè)計(jì)

MPC控制是一種滾動求解多約束優(yōu)化問題的控制方法，魯棒性較強(qiáng)[15]。本研究采用非線性MPC設(shè)計(jì)泊車橫向控制器。

(1)預(yù)測模型

低速泊車場景下無需考慮車輛穩(wěn)定性等動力學(xué)因素，采用運(yùn)動學(xué)模型進(jìn)行MPC設(shè)計(jì)在保證較高精度的同時(shí)具備復(fù)雜度低、實(shí)時(shí)性高的優(yōu)點(diǎn)。因此，本研究選用運(yùn)動學(xué)模型作為預(yù)測模型并進(jìn)行離散化處理，同時(shí)MPC以控制增量作為待優(yōu)化變量，離散化后模型為：

(15)

(2)建立最優(yōu)控制問題

設(shè)計(jì)目標(biāo)函數(shù)如下[16]：

(16)

約束條件：

(17)

式中：第一項(xiàng)考慮預(yù)測時(shí)域內(nèi)的位置偏差ey，保證其路徑跟蹤能力；第二項(xiàng)考慮橫擺角偏差eφ，保證車輛位姿狀態(tài)；第三項(xiàng)考慮控制量變化Δu，保證控制平穩(wěn)。Q、R、P為權(quán)重矩陣，Np為預(yù)測時(shí)域，Nc為控制時(shí)域，Δδmax為允許最大前輪轉(zhuǎn)角增量，δmax為允許最大前輪轉(zhuǎn)角。

在每個(gè)周期內(nèi)利用fmincon求解器完成上述最優(yōu)控制問題求解，得到控制時(shí)域Nc內(nèi)最優(yōu)控制增量序列：

(18)

取式(18)中最優(yōu)控制增量序列的第一個(gè)元素，求得當(dāng)前時(shí)刻最優(yōu)控制量為：

δ*(k)=δ(k-1)+Δu*(k)

(19)

4 仿真與分析

為驗(yàn)證所提出的針對平行泊車場景的自動泊車系統(tǒng)軌跡規(guī)劃與運(yùn)動控制策略，本研究首先基于Matlab對軌跡規(guī)劃算法進(jìn)行驗(yàn)證，之后基于Prescan/Simulink搭建平行泊車場景及聯(lián)合仿真模型，驗(yàn)證軌跡規(guī)劃到運(yùn)動控制算法的正確性。仿真所用部分參數(shù)見表1。

表1 仿真部分參數(shù)

4.1 基于Matlab驗(yàn)證平行泊車軌跡規(guī)劃

假設(shè)車輛后軸中心至目標(biāo)點(diǎn)垂直距離H=1.40 m，至目標(biāo)停車點(diǎn)的水平距離S=8.30 m。路徑規(guī)劃仿真結(jié)果如圖3所示。

圖3 平行泊車路徑規(guī)劃及插值仿真結(jié)果Fig.3 Parallel parking path planning and interpolation simulation results

從結(jié)果可見，規(guī)劃的泊車路徑滿足無碰撞約束，且經(jīng)過3次樣條插值后實(shí)現(xiàn)曲率連續(xù)。進(jìn)一步根據(jù)上述路徑規(guī)劃結(jié)果，計(jì)算得到用于QP速度規(guī)劃的平行泊車總路程S′=8.20 m，同時(shí)根據(jù)梯形速度規(guī)劃算法得到用于QP速度規(guī)劃的s-t曲線總時(shí)長T=15 s(向上取整)。利用上述兩個(gè)條件，基于QP二次規(guī)劃的速度規(guī)劃仿真結(jié)果如圖4所示。

圖4 基于QP的最優(yōu)速度規(guī)劃仿真結(jié)果Fig.4 Simulation results of optimal speed planning based on QP

從圖4可以看出，由QP求解的最優(yōu)泊車速度曲線滿足泊車過程的距離、最大速度等約束，同時(shí)保證了速度平穩(wěn)變化。

4.2 基于Prescan/Simulink聯(lián)合仿真驗(yàn)證

本研究利用Prescan軟件建立自動泊車仿真場景，基于Prescan/Simulink聯(lián)合仿真驗(yàn)證所提的自動泊車規(guī)劃及控制策略的可行性。所用的仿真參數(shù)同表1，給定車輛一側(cè)至停車位邊界距離h=1.40 m，聯(lián)合仿真過程如圖5所示。

圖5 水平泊車整體仿真過程Fig.5 Overall simulation process of horizontal parking

運(yùn)行時(shí)首先基于車輛狀態(tài)及車位信息規(guī)劃平行泊車路徑，其次計(jì)算得到用于QP速度規(guī)劃的泊車總路程S′=8.60 m以及s-t曲線總時(shí)長T=16 s，根據(jù)上述兩個(gè)條件通過QP求解最優(yōu)速度曲線，最終通過PID及MPC進(jìn)行控制。結(jié)果如圖6所示。

圖6 水平泊車軌跡規(guī)劃及跟蹤控制結(jié)果Fig.6 Horizontal parking trajectory planning and tracking control results

由Prescan/Simulink聯(lián)合仿真結(jié)果可知：所規(guī)劃的圓弧切直線平行泊車路徑與車位邊界無碰撞，速度曲線滿足最大速度約束及平滑變化等要求，速度控制過程中最大誤差為0.043 m/s，路徑跟蹤過程最大誤差為0.072 m，滿足低速泊車控制要求，驗(yàn)證了所提出的平行泊車系統(tǒng)軌跡規(guī)劃與運(yùn)動控制策略的可行性。

5 結(jié)語

本研究針對平行泊車場景提出了一種完整可行的泊車軌跡規(guī)劃與運(yùn)動控制策略，并通過Prescan/Simulink聯(lián)合仿真平臺對所提算法進(jìn)行了驗(yàn)證，仿真結(jié)果表明所提出的平行泊車系統(tǒng)軌跡規(guī)劃與運(yùn)動控制策略能夠使車輛精確、平穩(wěn)地泊入平行車位，對于平行泊車系統(tǒng)設(shè)計(jì)具有一定的參考意義。