張家旭 趙 健 施正堂 楊 雄

(1吉林大學汽車仿真與控制國家重點實驗室, 長春 130011)(2中國第一汽車集團有限公司智能網(wǎng)聯(lián)開發(fā)院, 長春 130011)(3浙江亞太機電股份有限公司, 杭州 311200)

近幾年，無人駕駛汽車成為汽車行業(yè)研究的熱點，我國和歐美日等主要汽車廠商正在加速布局無人駕駛技術(shù)，而全自動泊車系統(tǒng)是無人駕駛汽車的重要組成部分，可以有效解決復雜狹窄泊車環(huán)境下的自動駕駛問題，提高泊車過程的便捷性和安全性.因此，現(xiàn)階段研究開發(fā)全自動泊車系統(tǒng)對布局無人駕駛技術(shù)具有重要意義[1-2].

全自動泊車系統(tǒng)包括環(huán)境感知、控制策略和人機交互3部分，而控制策略是全自動泊車系統(tǒng)的核心，目前國內(nèi)外學者和汽車廠商對全自動泊車系統(tǒng)控制策略的研究成果可以分為人工智能法和規(guī)劃-跟蹤控制法.文獻[3]將提高泊車精度和縮短泊車時間作為優(yōu)化目標，采用演化策略對基于模糊邏輯規(guī)則建立的泊車控制器參數(shù)進行優(yōu)化，文獻[4]則采用蟻群算法優(yōu)化基于模糊邏輯規(guī)則建立的泊車控制器參數(shù).文獻[5]將熟練駕駛員的平行泊車、垂直泊車和斜向泊車試驗數(shù)據(jù)作為訓練樣本，并基于遺傳算法優(yōu)化模糊泊車控制器的隸屬度函數(shù)，實現(xiàn)3種泊車應(yīng)用場景的全自動模糊控制.文獻[6]將熟練駕駛員的泊車試驗數(shù)據(jù)作為訓練樣本，建立基于自適應(yīng)神經(jīng)模糊推理系統(tǒng)的泊車控制策略，實現(xiàn)特定應(yīng)用場景的全自動泊車控制.人工智能法需要依靠大量的專家數(shù)據(jù)或控制經(jīng)驗數(shù)據(jù)建立控制規(guī)則庫，增加了工程實現(xiàn)難度.

相對于人工智能法，規(guī)劃-跟蹤控制法不依賴專家數(shù)據(jù)或控制經(jīng)驗，具有較強的工程實現(xiàn)價值.文獻[7]綜合考慮車輛避碰約束與非完整性約束，規(guī)劃出一簇平行泊車路徑和斜向泊車路徑，隨后采用模糊邏輯系統(tǒng)選出最優(yōu)泊車路徑.文獻[8]建立不規(guī)則泊車場景的軌跡規(guī)劃的數(shù)學模型，并通過高斯偽譜法將其轉(zhuǎn)化為非線性規(guī)劃問題，在統(tǒng)一的優(yōu)化框架下實現(xiàn)泊車路徑與泊車速度的并行規(guī)劃.文獻[9]采用圓弧-回旋曲線組合方式規(guī)劃曲率連續(xù)的平行泊車路徑，并基于參考泊車路徑的弧長直接將參考泊車路徑曲率映射為車輛的等效前軸轉(zhuǎn)角，實現(xiàn)參考泊車路徑的跟蹤控制.文獻[10]綜合考慮車輛運動學約束、車輛性能參數(shù)約束和泊車避障約束等，以泊車時間最短為性能指標，建立了泊車路徑規(guī)劃最優(yōu)控制問題的數(shù)學模型，并采用高斯偽譜法將泊車路徑規(guī)劃最優(yōu)控制問題離散化為非線性規(guī)劃問題，最后采用非線性規(guī)劃求解器SNOPT求解得到參考泊車路徑.文獻[11]基于B樣條理論和微分平坦理論規(guī)劃參考泊車路徑，并采用模型預測控制方法設(shè)計參考泊車路徑跟蹤控制律.文獻[12]綜合考慮可行性、安全性、平順性和工效性4大性能指標，基于車速無關(guān)的離散車輛運動學模型提出了平行泊車和垂直泊車最優(yōu)軌跡決策算法，并分別采用前饋加反饋的控制方式與模糊控制方式實現(xiàn)了方向盤轉(zhuǎn)角和車速的跟蹤控制.

本文采用規(guī)劃-跟蹤控制架構(gòu)，綜合考慮汽車運動學約束、避障約束和邊界條件約束，提出一種基于回旋曲線的垂直泊車軌跡規(guī)劃與跟蹤控制方法.首先，選擇垂直泊車軌跡的弧長作為關(guān)聯(lián)參考量，將垂直泊車軌跡規(guī)劃問題解耦成路徑規(guī)劃問題和速度規(guī)劃問題，進而將三維空間內(nèi)的規(guī)劃問題簡化為2個二維空間內(nèi)的規(guī)劃問題，降低垂直泊車軌跡規(guī)劃的難度.針對垂直泊車路徑規(guī)劃問題，基于回旋曲線規(guī)劃曲率連續(xù)的垂直泊車路徑；針對垂直泊車速度規(guī)劃問題，基于五次多項式曲線規(guī)劃滿足過程和邊界約束條件的垂直泊車速度.隨后，選擇垂直泊車軌跡的橫向位移量作為非時間參考量，將垂直泊車軌跡跟蹤控制問題解耦成垂直泊車路徑跟蹤控制問題和垂直泊車速度跟蹤控制問題，降低垂直泊車軌跡跟蹤控制的難度.考慮垂直泊車軌跡跟蹤控制系統(tǒng)的計算時延，以航跡推算算法計算輸出的當前車輛位姿和速度為基礎(chǔ)，采用預估模型預測下一采樣時刻的車輛位姿和速度，將其作為垂直泊車路徑跟蹤控制問題和垂直泊車速度跟蹤控制問題的狀態(tài)反饋量.針對多輸入的垂直泊車路徑跟蹤控制問題，基于2增益控制理論設(shè)計非時間參考泊車路徑跟蹤2增益控制律，提高垂直泊車路徑跟蹤控制的魯棒性.針對多輸出的泊車速度跟蹤控制問題，基于PID控制設(shè)計泊車速度跟蹤控制律，實現(xiàn)驅(qū)動系統(tǒng)和制動系統(tǒng)的平滑切換控制.最后，結(jié)合硬件在環(huán)試驗平臺對所提出的垂直泊車軌跡規(guī)劃與跟蹤控制方法的可行性和有效性進行驗證.

1 垂直泊車軌跡規(guī)劃

1.1 垂直泊車路徑規(guī)劃

圖1 垂直泊車路徑規(guī)劃

① 計算θ1，即

(1)

② 計算點G2的坐標(xG2,yG2)，即

(xG2,yG2)=(xG1+Rminsinθ1,yG1+Rmin(1-cosθ1))

(2)

③ 計算點G4的坐標(xG4,yG4)，即

(xG4,yG4)=(xG1,yG2-Rminsinθ2)

(3)

(xO1,yO1)=(xG1,yG1+Rmin)

(4)

(xO2,yO2)=(xG4+Rmin,yG4)

(5)

⑥ 計算S1，即

(6)

圖2 基于回旋曲線的泊車路徑平滑原理圖

由回旋曲線定義可知，原點O3到點H的回旋曲線長sH=1/(Rminc).由此，基于sH可得

(7)

(8)

(9)

式中，N為回旋曲線的近似階次.

由式(7)～(9)可得

(10)

式中

n1=xH-RminsinαH

(11)

n2=yH-Rmin(1-cosαH)

(12)

(13)

(14)

② 計算點G0的坐標(xG0,yG0)，即

(15)

(16)

(17)

(18)

(19)

⑥ 計算平滑之后曲線的端點G3的坐標(xG3,yG3)，即

(20)

⑦ 為了提高垂直泊車路徑規(guī)劃方法的實時性，離線計算并存儲回旋曲線的采樣序列，通過旋

圖3 垂直泊車路徑平滑

(21)

1.2 垂直泊車速度規(guī)劃

垂直泊車速度規(guī)劃的任務(wù)是設(shè)計滿足要求的垂直泊車速度曲線，使汽車從靜止開始沿著規(guī)劃好的路徑行駛到目標點，并安全平穩(wěn)地停在目標點.因此，垂直泊車速度曲線可以分為從零加速到恒定值的加速段、保持恒定值的勻速段和從恒定值減速到零的減速段.本節(jié)基于五次多項式曲線設(shè)計垂直泊車速度曲線的加速段和減速段，提高垂直泊車效率和舒適性.

由于通過對垂直泊車速度曲線的加速段垂直翻轉(zhuǎn)和水平移動可以得到垂直泊車速度曲線的減速段，因此本節(jié)僅需設(shè)計垂直泊車速度曲線的加速段.采用五次多項式對加速段的速度曲線、加速度曲線和加加速度曲線分別進行描述，即

v(t)=q0+q1t+q2t2+q3t3+q4t4+q5t5

(22)

a(t)=q1+2q2t+3q3t2+4q4t3+5q5t4

(23)

j(t)=2q2+6q3t+12q4t2+20q5t3

(24)

式中，v(t)、a(t)、j(t)分別為垂直泊車速度曲線加速段的速度、加速度、加加速度；q0、q1、…、q5為五次多項式的待確定系數(shù)；t為時間參數(shù).

考慮垂直泊車速度規(guī)劃的任務(wù)需求以及車輛制動系統(tǒng)和驅(qū)動系統(tǒng)的機械約束，以最小化垂直泊車速度曲線加速段所需的時間tf作為性能指標，將垂直泊車速度規(guī)劃問題轉(zhuǎn)化為如下的非線性優(yōu)化問題：

s.t.vstart=0

vend=vmax

astart=aend=0

|v|≤vmax

|a|≤amax

|j|≤jmax

(25)

式中，vstart和vend分別為垂直泊車速度曲線的起始速度和終止速度；astart和aend分別為垂直泊車速度曲線的起始加速度和終止加速度；vmax、amax、jmax分別為垂直泊車速度曲線的速度最大值、加速度最大值、加加速度最大值.

針對式(25)描述的非線性優(yōu)化問題，可以采用內(nèi)點法進行求解[14].

為了提高垂直泊車速度規(guī)劃方法的計算實時性，對垂直泊車速度曲線的加速段進行離線計算并采樣，通過垂直翻轉(zhuǎn)和水平移動變換得到垂直泊車速度曲線減速段的采樣點，并且將加速段和減速段的采樣點以表格的方式存儲在控制器內(nèi).由于垂直泊車速度曲線的加速段與減速段是固定不變的，二者所需要的垂直泊車路徑弧長也是固定不變的.因此，可以通過調(diào)整垂直泊車速度曲線的勻速段時間使得垂直泊車速度曲線與時間軸圍成的面積等于垂直泊車路徑弧長，即建立所規(guī)劃的垂直泊車速度與垂直泊車路徑之間的對應(yīng)關(guān)系.

2 垂直泊車軌跡跟蹤控制

圖4 垂直泊車軌跡跟蹤控制框圖

2.1 垂直泊車路徑跟蹤控制

垂直泊車路徑跟蹤控制的任務(wù)是設(shè)計控制律，使得車輛可以快速、精確、穩(wěn)定地跟蹤參考泊車路徑.由于車輛在垂直泊車過程中處于低速大轉(zhuǎn)角行駛狀態(tài)，可以忽略輪胎的側(cè)向滑動，進而可以采用如下車輛運動學方程描述垂直泊車過程中的車輛行駛狀態(tài)[15]：

(26)

式中，(x,y)為車輛后軸中點坐標；v為車速；φ為車輛方位角；δd為前軸等效轉(zhuǎn)角.

當車輛倒車時，選擇車輛后軸中點的橫坐標值的負數(shù)作為非時間參考量.考慮參數(shù)不確定性、外界干擾等因素產(chǎn)生的系統(tǒng)復合干擾，將式(26)轉(zhuǎn)化為如下包含系統(tǒng)復合干擾的泊車路徑跟蹤偏差方程：

(27)

式中，x1=yr-y；x2=-(tanφr-tanφ)；yr、ρr和φr分別為參考泊車路徑的縱坐標值、曲率和方位角；d為系統(tǒng)的復合干擾.

針對系統(tǒng)式(27)，構(gòu)造如下的變量變換：

(28)

式中，υ為虛擬控制輸入.

定義系統(tǒng)的性能輸出z為

z=[z1z2]T

(29)

基于式(29)定義的系統(tǒng)性能輸出，設(shè)計非時間參考泊車路徑跟蹤2增益控制律，并得到如下定理.

定理1針對系統(tǒng)式(27)，設(shè)計如下非時間參考泊車路徑跟蹤2增益控制律：

(30)

式中，γ為增益系數(shù)，且γ>0.當d≠0時，閉環(huán)系統(tǒng)是有限增益2穩(wěn)定的；當d=0時，閉環(huán)齊次系統(tǒng)是漸近穩(wěn)定的.

證明定義李雅普諾夫候選函數(shù)

(31)

對式(31)兩端求導數(shù)，即

(32)

選取υ=-z1/2，并將其代入式(32)，可得

(33)

增廣李雅普諾夫候選函數(shù)

(34)

對式(34)兩端求導數(shù)，即

(35)

將式(30)代入式(35)，可得

(36)

對式(36)兩側(cè)進行積分運算，可得

(37)

式中，T為積分時間.

(38)

當車輛前進時，選擇車輛后軸中點的橫坐標值作為非時間參考量.修正定義x2=tanφr-tanφ，基于上述方法得到控制律與式(30)相同.

2.2 垂直泊車速度跟蹤控制

垂直泊車速度跟蹤控制需要協(xié)調(diào)驅(qū)動系統(tǒng)和制動系統(tǒng)來實現(xiàn)參考泊車速度的快速、精確、穩(wěn)定的跟蹤控制.由于難以建立被控對象的精確數(shù)學，基于PID控制設(shè)計垂直泊車速度跟蹤控制律，即

uk+1=uk+KP(ek+1-ek)+KIek+1+
KD(ek+1-2ek+ek-1)

(39)

式中，uk+1和uk分別為k+1時刻和k時刻的控制量；KP、KI和KD分別為比例、積分和微分系數(shù)；ek+1、ek和ek-1分別為k+1時刻、k時刻和k-1時刻的速度偏差.

為了實現(xiàn)驅(qū)動系統(tǒng)和制動系統(tǒng)的平滑切換控制，當uk+1≥0時，uk+1作為驅(qū)動系統(tǒng)的基礎(chǔ)控制量；當uk+1<0時，uk+1作為制動系統(tǒng)的基礎(chǔ)控制量[18].

2.3 車輛位姿和速度預估

(40)

式中，Tr為后軸輪距.

圖5 航跡推算法示意圖

(41)

由此，可得Δx和Δy的表達式為

(42)

將式(40)描述的車輛方位角增量和式(42)描述的車輛后軸中點位置增量累加求和，可得一段時間內(nèi)的車輛位姿描述，即

(43)

假設(shè)車輛后軸中點的速度在k-1時刻到k時刻時間間隔保持不變，可得車輛后軸中點在k時刻的速度vk，即

(44)

由式(43)和(44)可知，已知k-1時刻的車輛位姿與k-1時刻到k時刻時間間隔的車輛右后輪和左前輪走過的距離，可以推算出k時刻的車輛位姿和車速.下面基于k時刻的車輛位姿、車速、前軸等效轉(zhuǎn)角期望值和實際值，預估k+1時刻的車輛位姿和車速.

(45)

將車輛轉(zhuǎn)向機構(gòu)視為一階慣性環(huán)節(jié)，并采用前向差分方法離散化，可得

(46)

(47)

由式(42)可得k+1時刻車輛后軸中點位置的估計值，即

(48)

由式(45)、(47)和(48)可知，已知k時刻的車輛位姿、車速、前軸等效轉(zhuǎn)角期望值和實際值，可以預估出k+1時刻的車輛位姿和車速.

3 硬件在環(huán)試驗驗證

采用圖6所示的硬件在環(huán)試驗平臺對所提出的垂直泊車軌跡規(guī)劃與跟蹤控制方法的可行性和有效性進行驗證.

圖6 硬件在環(huán)試驗平臺

如圖6所示，硬件在環(huán)試驗平臺由上位機、實時處理器1、實時處理器2和超聲波雷達臺架組成.實時處理器1上運行所提出的垂直泊車軌跡規(guī)劃與跟蹤控制模型，實時處理器2上運行車輛動力學模型和超聲波雷達模型，并且實時處理器1和實時處理器2通過光纖進行通信.上位機通過以太網(wǎng)與實時處理器1和實時處理器2相連，實現(xiàn)對實時處理器1和實時處理器2的管理.超聲波雷達臺架通過硬線與實時處理器1和實時處理器2相連，實時處理器2通過硬線激勵真實的超聲波雷達傳感器，實時處理器1通過硬線接收真實超聲波雷達感知到信息.綜上，通過上位機、實時處理器1、實時處理器2和超聲波雷達臺架組成垂直泊車軌跡規(guī)劃與跟蹤控制方法的虛擬仿真驗證環(huán)境.

在垂直泊車驗證工況中，設(shè)置車輛長度、寬度和軸距分別為4.155、1.645和2.405 m，車輛前懸和后懸長度分別為0.800和0.950 m，車輛最大泊車速度、最大泊車加速度和最大泊車加加速度分別為1 m/s、2 m/s2和3 m/s3，車輛后軸中點最小轉(zhuǎn)彎半徑為4.200 m，硬件在環(huán)試驗平臺仿真步長為1 ms，路面附著系數(shù)為1，垂直泊車位寬度和長度分別為2.300和4.155 m，垂直泊車起始點坐標為[-1.378, 2.000]m，試驗結(jié)果如圖7～圖10所示.

(a) 車輛后軸中點橫向位移

(b) 車輛方位角沿后軸中點

橫向位移變化

(c) 車輛后軸中點縱向位移

(d) 方向盤轉(zhuǎn)角

如圖8所示，基于圓弧-直線方法規(guī)劃的垂直泊車路徑最短，但是在圓弧段與直線段的結(jié)合點處存在曲率突變，車輛必須執(zhí)行原地轉(zhuǎn)向才能跟蹤基于圓弧-直線方法規(guī)劃的垂直泊車路徑；基于B樣條曲線規(guī)劃的垂直泊車路徑與基于回旋曲線規(guī)劃的垂直泊車路徑的曲率均是連續(xù)變化的，但基于回旋曲線規(guī)劃的垂直泊車路徑更短，使得垂直泊車時間更短.如圖7所示，基于2增益控制理論設(shè)計的非時間參考泊車路徑跟蹤2增益控制律可以使車輛精確地跟蹤目標路徑，并且在垂直泊車過程中無輪胎原地轉(zhuǎn)向現(xiàn)象.如圖9所示，基于PID控制方法設(shè)計的泊車速度跟蹤控制律可以實現(xiàn)目標泊車速度的快速、精確、穩(wěn)定的跟蹤控制.如圖10所示，藍色區(qū)域為垂直泊車位兩側(cè)的障礙物，黃色區(qū)域為目標路徑對應(yīng)的車輛外輪廓包絡(luò)，綠色區(qū)域為實際路徑對應(yīng)的車輛外輪廓包絡(luò)，由車輛外輪廓包絡(luò)和障礙物無相交可知：基于回旋曲線規(guī)劃的垂直泊車軌跡可以安全、精確地引導車輛進入泊車位，基于2增益控制理論設(shè)計的非時間參考泊車路徑跟蹤2增益控制律與基于PID控制方法設(shè)計的泊車速度跟蹤控制律可以在垂直泊車位寬度與車輛寬度之差大于0.655 m的前提下使車輛沿著規(guī)劃的垂直泊車軌跡安全、舒適、精確地停放在垂直泊車位.

(a) 目標路徑

(b) 泊車路徑曲率沿泊車路徑弧長變化

4 結(jié)論

1) 選擇垂直泊車軌跡的弧長作為關(guān)聯(lián)參考量，將垂直泊車軌跡規(guī)劃問題解耦成路徑規(guī)劃問題和速度規(guī)劃問題，降低了垂直泊車軌跡規(guī)劃的難度.

(a) 車速

(b) 車速沿后軸中點橫向位移變化

(c) 速度控制信號

(a) 目標路徑的車輛外輪廓包絡(luò)

(b) 實際路徑的車輛外輪廓包絡(luò)

針對垂直泊車路徑規(guī)劃問題，基于回旋曲線規(guī)劃曲率連續(xù)的垂直泊車路徑；針對垂直泊車速度規(guī)劃問題，基于五次多項式曲線規(guī)劃滿足過程和邊界約束條件的垂直泊車速度.

2) 將垂直泊車軌跡跟蹤控制問題解耦成垂直泊車路徑跟蹤控制問題和垂直泊車速度跟蹤控制問題.考慮垂直泊車軌跡跟蹤控制系統(tǒng)的計算時延，采用預估模型預測下一采樣時刻的車輛位姿和速度.針對垂直泊車路徑跟蹤控制問題，基于2增益控制理論設(shè)計非時間參考泊車路徑跟蹤2增益控制律.針對泊車速度跟蹤控制問題，基于PID控制設(shè)計泊車速度跟蹤控制律實現(xiàn)驅(qū)動系統(tǒng)和制動系統(tǒng)的平滑切換控制.

3) 結(jié)合硬件在環(huán)試驗平臺對所提出的垂直泊車軌跡規(guī)劃與跟蹤控制方法的可行性和有效性進行驗證，結(jié)果表明：所提出的垂直泊車軌跡規(guī)劃方法得到的泊車路徑曲率是連續(xù)的，并且所提出的垂直泊車軌跡跟蹤控制方法在泊車位寬度與車輛寬度之差大于0.655 m時可以安全、精確地引導車輛停放在泊車位.