陳文強

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

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

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

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

1 車輛建模

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

圖1 運動學模型Fig.1 Kinematic model

建立XOY慣性坐標系， 則車輛運動學模型可表示為[7]：

(1)

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

2 泊車軌跡規(guī)劃

2.1 泊車路徑規(guī)劃

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

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

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

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

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

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

(2)

合并式(2)，可得：

(3)

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

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

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

假設有n+1個控制點：

(4)

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

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個未知數(shù)。最終求解得到包含n段三次樣條曲線的擬合曲線。

2.2 泊車速度規(guī)劃

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

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

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

(6)

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

(7)

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

首先，構建代價函數(shù)。

所構建的代價函數(shù)表達式為[10]：

(8)

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

接著，將上述代價函數(shù)轉化為標準QP形式：

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

M-ε4·H4·M

(9)

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

最后，設計過程約束。

(1)速度、位置約束

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

(10)

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

考慮行駛距離s與時間t為單調(diào)遞增的關系，相鄰采樣點間應滿足：

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

(11)

(3)最大速度約束

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

(12)

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

3 泊車運動控制

3.1 縱向控制器設計

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

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

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

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

(13)

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

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

(14)

式中，v(k)為車輛當前速度；vdes(k)為目標點參考速度。

3.2 橫向控制器設計

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

(1)預測模型

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

(15)

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

設計目標函數(shù)如下[16]：

(16)

約束條件：

(17)

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

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

(18)

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

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

(19)

4 仿真與分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結語

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