基于滑?？刂频淖詣硬窜囅到y(tǒng)路徑跟蹤研究

姜立標 楊 杰

(華南理工大學機械與汽車工程學院, 廣州 510640)

0 引言

隨著科技的不斷發(fā)展，汽車智能化已經(jīng)是當前必然趨勢。車輛作為一個復雜的非線性系統(tǒng)，隨著國內(nèi)外學者的不斷研究，許多規(guī)劃方法和新型的控制方法被應用到泊車運動控制中[1-10]。

現(xiàn)有研究中，路徑規(guī)劃方法大多使用圓弧直線組合路徑，泊車路徑曲率不連續(xù)，沒有考慮車輛轉(zhuǎn)向能力。軌跡跟蹤控制器同時控制車速和方向盤轉(zhuǎn)角，一旦車速控制誤差較大會直接影響軌跡跟蹤效果。在對設(shè)計的路徑規(guī)劃方法和控制器進行仿真分析時，簡化的車輛運動學模型會造成實際控制器控制效果不佳。

本文以某SUV為研究對象，結(jié)合國內(nèi)外自動泊車研究現(xiàn)狀，對路徑規(guī)劃方法中常見的曲率不連續(xù)問題進行研究，以解決規(guī)劃路徑曲率不連續(xù)、路徑規(guī)劃初始條件嚴格的問題，并提出基于B樣條曲線的路徑規(guī)劃方法和基于趨近律的非時間參考終端滑模路徑跟蹤控制方法。

1 車輛運動學模型

假設(shè)車輛泊車過程為低速運動過程，忽略車輪側(cè)向滑動,車輛在行駛中不出現(xiàn)側(cè)向力，基于阿克曼轉(zhuǎn)向原理建立車輛運動學模型如圖1所示。

圖1 車輛運動學模型Fig.1 Vehicle kinematics model

圖中，θ為車輛車身與坐標系X軸的夾角，φ為等效前輪轉(zhuǎn)角，l為車輛軸距，vf為車輛前軸中心點的速度，(xf,yf)為車輛前輪軸中心坐標，(xr,yr)為車輛后輪軸中心坐標。

為方便計算，將四輪車簡化為虛擬的兩輪車，如圖2所示。簡化后前后輪轉(zhuǎn)角的關(guān)系式為

cotα+cotβ=2cotφ

(1)

式中α——外輪轉(zhuǎn)角β——內(nèi)輪轉(zhuǎn)角

圖2 等效前輪轉(zhuǎn)角示意圖Fig.2 Equivalent front wheel angle diagram

由實際條件可知，在滿足阿克曼轉(zhuǎn)向原理條件下，車輛方向盤轉(zhuǎn)角和等效前輪轉(zhuǎn)角之間存在近似線性的轉(zhuǎn)換關(guān)系

φ≈kφ

(2)

由目標車輛的實際測量結(jié)果可得k為16.2。

由假設(shè)條件可知，車輛運動學模型本身符合非完整性約束條件,即

(3)

由圖1可知

(4)

式(4)對時間求導得

(5)

將式(5)代入式(3),可得

(6)

又由圖1可得

(7)

將式(7)代入式(6)可得

(8)

聯(lián)立式(5)、(7)、(8)得

(9)

為簡化表達式，使用向量q=(v,ω)T表示車輛的線速度和角速度，其中，v為后軸中心點的速度，其方向垂直于后軸軸線。同時令

(10)

所以，對于前輪轉(zhuǎn)向的車輛，以后軸中點為參考點，其運動學方程為

(11)

車輛位姿可由后輪中點在坐標系中的位置以及車輛航向角來表示，可以描述為p=(x,y,θ)T。則車輛的運動學方程可以表示為

(12)

假設(shè)車輛最大等效前輪轉(zhuǎn)角為φmax，后軸中點最小的轉(zhuǎn)彎半徑為ρmin，ρmin=tanφmax/l，則對應的角速度為ω≤v/ρmin。

2 基于B樣條的泊車路徑規(guī)劃

常規(guī)泊車情況指車身方向與道路方向平行，與路基或是車庫方向沒有夾角。在實際泊車過程中，受駕駛經(jīng)驗以及個人主觀因素的影響，駕駛員難以將車身方向調(diào)整至與道路方向完全一致，車身總會與道路方向存在一定夾角，傾斜車輛初始位姿更為常見，所以本文以傾斜車輛初始位姿為研究工況。

2.1 B樣條曲線

在實際工程應用中，曲線擬合可分為兩種：①曲線通過所有的樣本點，如三次參數(shù)樣條曲線和拋物樣條曲線等。②曲線不一定通過所有樣本點，只是較好地逼近樣本點。在兩種曲線中，考慮到局部修改的便捷性，方便設(shè)計者交互操作，第1種曲線不能滿足要求，第2種曲線多用于產(chǎn)品外形設(shè)計。因此Bezier提出了一種新的參數(shù)曲線表示方法，但Bezier曲線在實際應用中缺乏靈活性、控制性差、不易修改。為了克服上述問題，Shoenbery定義了B樣條曲線，其表達式為

(13)

式中Pi——目標曲線控制點

Ni,k(t)——B樣條基函數(shù)，是分段多項式

DeBoor和Cox定義的B樣條曲線基函數(shù)遞推表達式為

(14)

由式(14)可知，四階樣條曲線函數(shù)在其節(jié)點定義區(qū)間二階可導，能滿足泊車路徑中曲率連續(xù)以及曲率變化率連續(xù)的要求，因此在滿足方向盤轉(zhuǎn)角連續(xù)變化的約束下，為減少計算量，使用四階均勻B樣條曲線作為目標路徑的規(guī)劃基礎(chǔ)。根據(jù)樣條曲線基函數(shù)遞推公式，可求出目標曲線表達式，并將對應的曲線表達式寫成矩陣形式

(15)

將式(15)中控制點分解為橫坐標分量和縱坐標分量，即

(16)

2.2 運動學約束分析

(1)最大曲率約束

設(shè)計的路徑應該滿足車輛的等效偏轉(zhuǎn)角約束，其最大等效前輪偏轉(zhuǎn)角應該小于車輛允許的最大值，即

(17)

(2)速度約束

為避免車輛滑移等現(xiàn)象，泊車過程中的車速應該滿足低速運動的假設(shè)條件要求，即

|v|max≤|vmax|

(18)

(3)角速度約束

若參數(shù)超出實際車輛執(zhí)行能力，使后續(xù)路徑跟蹤效果極差。設(shè)計路徑與等效偏轉(zhuǎn)角速度相關(guān)時參數(shù)應該滿足其角速度約束條件，即

|ω|≤ωmax

(19)

以控制點為變量，由B樣條曲線表達式求解路徑曲線，將泊車結(jié)束時車身與車位的夾角，即車身偏轉(zhuǎn)角最小化作為優(yōu)化目標，結(jié)合車輛運動學約束、車輛機械結(jié)構(gòu)約束以及環(huán)境約束等建立非線性約束方程。

3 軌跡跟蹤控制器設(shè)計

3.1 非時間參考路徑跟蹤系統(tǒng)誤差分析

現(xiàn)有軌跡跟蹤算法大多將期望軌跡視為時間函數(shù)，在時間域中求解控制律。這種控制方法常將車速作為控制量之一。但是在車輛行駛過程中，車速受到駕駛員、車輛狀態(tài)以及行駛環(huán)境等多方面的影響，如果因為特殊情況造成車速控制不佳，使得基于時間參考的控制律失去繼續(xù)跟蹤期望軌跡的能力，最終將造成泊車失敗。產(chǎn)生上述問題是因為時間相對于路徑跟蹤問題過于“自由”，與系統(tǒng)運行狀態(tài)無關(guān)，從而導致系統(tǒng)的規(guī)劃和控制不協(xié)調(diào)[11]。因此，基于非時間參考的控制思想被提出，通過引入其他非時間參考量代替時間參考量，提高系統(tǒng)在不確定環(huán)境中的路徑跟蹤能力。

基于非時間參考的路徑跟蹤控制律需要定義一個非時間參考的標量f，要求f為隨時間單調(diào)遞增函數(shù)，即

f=F(t)

(20)

(21)

設(shè)計的泊車路徑是一條與時間無關(guān)的空間曲線，不包含與時間相關(guān)的變量。車輛跟蹤目標路徑的過程可以描述為：對任意給定的幾何路徑S(x,y,θ)=0，尋找反饋控制律，對于任意給定的ζ>0，存在ft，當f>ft時，使|(xr,yr,θr)-(x,y,θ)|≤ζ成立，其中(xr,yr,θr)是參考的目標路徑坐標和車身偏轉(zhuǎn)角。

在車輛倒車狀態(tài)下，在建立的坐標系中選擇車輛實際路徑在X軸方向上的投影作為非時間參考量，即F=-x(t)，則車輛跟蹤目標路徑誤差如圖3所示。由圖3可得

(22)

則車輛路徑跟蹤誤差函數(shù)可以表示為

(23)

圖3 路徑跟蹤誤差Fig.3 Path tracking error

令pe=(ye,θe)，則其導數(shù)可表示為

(24)

假設(shè)車輛以穩(wěn)定的車速運行，并且對目標路徑進行跟蹤，目標是在特定的有界等效前輪轉(zhuǎn)角輸入φ控制下，使得在任一的初始誤差下，系統(tǒng)能在該控制下pe=(ye,θe)有界并且不斷趨向于0，即

(25)

3.2 滑?？刂破髟O(shè)計

為方便表示，令系統(tǒng)參考狀態(tài)量為

(26)

則系統(tǒng)狀態(tài)量導數(shù)可以表示為

(27)

將式(8)代入式(27)，可得

(28)

假設(shè)等效前輪轉(zhuǎn)角為小角度輸入，φ≈tanφ，令

(29)

則系統(tǒng)誤差狀態(tài)方程表示為

(30)

式中f(x)、b(x)——非線性函數(shù)

u——系統(tǒng)輸入

在普通滑?？刂浦校ǔ＿x擇一個線性的滑動超平面

(31)

使得系統(tǒng)到達滑動模態(tài)后，各誤差漸近收斂為0，并且漸近收斂的速度可以通過選擇滑動平面參數(shù)矩陣任意調(diào)節(jié)，但狀態(tài)跟蹤誤差不會在有限時間內(nèi)收斂為0。

為了提高控制器效果，提出終端滑?？刂撇呗裕ㄟ^引入非線性弧面，使得滑模面上跟蹤誤差能夠在有限時間內(nèi)收斂到0。通過設(shè)計一種動態(tài)非線性滑模面方程實現(xiàn)終端滑?？刂?，切換函數(shù)為

(32)

其中p>q，p和q為正奇數(shù)，β>0。

基于滑?？刂坪徒K端滑?？刂频姆菚r間參考路徑跟蹤控制律都具備跟蹤目標路徑的能力，但對車身偏轉(zhuǎn)角狀態(tài)控制存在強烈的抖振現(xiàn)象，需提高趨近品質(zhì)。結(jié)合采用趨近律的方法對趨近運動軌跡進行限制，改善趨近運動的動態(tài)品質(zhì)。

根據(jù)趨近律滑?？刂品椒?，采用一種新型趨近律滑?？刂疲罨Ｃ娴膶?shù)為

(33)

其中

(34)

式中0<δ<1，η>0，ε>0 ，δ為fal(s,η,δ)在原點附近正負對稱線性段的區(qū)間長度，且fal(s,η,δ)為非連續(xù)函數(shù)。當|s|≤δ時，即s接近0時，式(33)第1項可以使得系統(tǒng)趨近速度變大，使得系統(tǒng)狀態(tài)在有限時間之內(nèi)快速到達滑動面，同時在滑動面附近fal(s,η,δ)是光滑連續(xù)的，控制輸入的高頻震動現(xiàn)象被有效削弱。而且式(33)的第2項也是光滑連續(xù)函數(shù)，當|s|>δ時，能保證系統(tǒng)狀態(tài)以較大的速度趨近滑動面并且起到平滑和限幅的作用，聯(lián)立式(29)、(30)、(33)、(34)可得

(35)

定義Lyapunov函數(shù)

(36)

對式(36)求導并代入式(35)可得

(37)

其中γ>0，因此可得

(38)

圖4 路徑規(guī)劃仿真結(jié)果Fig.4 Path planning simulation results

4 仿真驗證

在Matlab中輸入車輛的尺寸參數(shù)(表1)、車位信息以及初始位姿信息， 添加等式約束條件函數(shù)和不等式約束條件函數(shù)，在滿足約束條件下，令終止車身偏轉(zhuǎn)角最小為目標對控制點坐標進行優(yōu)化求解，則結(jié)果就是滿足約束條件的B樣條曲線路徑的控制點坐標。

表1 車輛相關(guān)參數(shù)Tab.1 Vehicle related parameters

4.1 路徑規(guī)劃

結(jié)合實際泊車經(jīng)驗，當車身傾斜程度過大時，駕駛員會自行調(diào)整方向盤使得車身偏轉(zhuǎn)角在一個合理的范圍之內(nèi)，所以θ0不會太大，因此假設(shè)|θ0|∈(0°,5°]。查閱對應車型的技術(shù)手冊，可知最大允許曲率為0.256 4 m-1，最大允許角速度0.593 4 rad/s。

采用尺寸為6.4 m×2.0 m的車位作為目標車位，車輛起始位姿為x0=6.80 m，y0=1.80 m，θ0=0.087 rad。

由圖4a B樣條泊車路徑可知，規(guī)劃的曲線很好地連接了車輛起始點和預設(shè)終點，并且車輛在沿著規(guī)劃路徑行駛過程中，車輛輪廓沒有與道路邊緣、車庫邊界等發(fā)生碰撞，實現(xiàn)了車輛避障的要求。

由圖4b車身偏轉(zhuǎn)角可知，初始位置車身存在0.087 rad的初始偏轉(zhuǎn)角，但隨著規(guī)劃的泊車路徑，車輛在預設(shè)的泊車終點實現(xiàn)車身偏轉(zhuǎn)角為0 rad，滿足泊車的位姿要求，最大車身偏轉(zhuǎn)角為0.777 6 rad。

由圖4c泊車路徑曲率可知，在起始點處，由于車身有一定的偏角，規(guī)劃的路徑利用了該偏角，路徑起始點存在一定的曲率，該處的曲率與車身起始的偏轉(zhuǎn)角有關(guān)，路徑曲率最大值為0.236 8 m-1，小于最大允許曲率0.256 4 m-1，滿足車輛轉(zhuǎn)向約束條件。

由圖4d前輪轉(zhuǎn)向角速度可知，在起始位置處前輪需要迅速地滿足設(shè)計曲率的要求，因此需要較大的角速度，在整個泊車過程中，最大角速度為0.528 1 rad/s，小于最大允許角速度0.593 4 rad/s，滿足角速度約束。由上述仿真結(jié)果可知，此次規(guī)劃的路徑能實現(xiàn)車輛單步平行泊車。

4.2 路徑跟蹤

為驗證基于趨近律的終端滑模路徑跟蹤控制器的跟蹤效果，在Simulink上搭建路徑跟蹤仿真實驗。設(shè)定跟蹤路徑方程為y=16sin(0.05x)+5，車輛初始位姿參數(shù)為(0，0，0.02)，車速為1 m/s，3種路徑跟蹤控制器仿真結(jié)果如圖5所示。

圖5 3種控制算法路徑跟蹤效果對比Fig.5 Comparison of path tracking effects of three control algorithms

由圖5可知，終端滑?？刂婆c非時間參考滑?？刂葡啾?，效果有所提升，在橫坐標約為-26 m處成功地跟蹤到參考路徑，車輛在轉(zhuǎn)彎較為劇烈地方的縱坐標跟蹤誤差減少；基于趨近率的終端滑?？刂婆c基于終端滑?？刂菩Ч嘟?，但車輛在轉(zhuǎn)彎較為劇烈處縱坐標跟蹤效果更優(yōu)，能更好地跟蹤到參考路徑。

由圖6可知，終端滑?？刂婆c非時間參考滑模控制相比，縱坐標跟蹤最小誤差為-0.063 m，最大誤差約為-0.147 m，平均誤差為-0.105 0 m，系統(tǒng)的穩(wěn)定平均誤差減少0.157 5 m，控制效果有明顯提升；基于趨近率的終端滑?？刂婆c基于終端滑?？刂菩Ч啾?，穩(wěn)定誤差較小，最小誤差約為-0.069 m，最大誤差約為-0.144 m，平均誤差約為-0.106 5 m。

圖6 縱坐標跟蹤誤差效果對比Fig.6 Comparison of vertical ordinate tracking error

由圖7可知，終端滑?？刂婆c非時間參考滑?？刂葡啾?，車身偏轉(zhuǎn)角跟蹤誤差最小誤差約為0 rad，最大誤差約為-0.044 rad，平均誤差約為-0.022 rad，約為-1.26°，控制效果相當，仍存在較強的抖振現(xiàn)象；基于趨近率的終端滑模控制與基于終端滑?？刂菩Ч啾?，車身偏轉(zhuǎn)角跟蹤最小誤差約為-0.017 7 rad，最大誤差約為-0.023 2 rad，穩(wěn)定誤差約為-0.020 5 rad，約為-1.174°，誤差范圍與前者相當。但系統(tǒng)車身偏轉(zhuǎn)角誤差的抖振現(xiàn)象明顯減弱，控制效果更優(yōu)。

圖7 車身偏轉(zhuǎn)角跟蹤誤差對比Fig.7 Comparison of body deflection angle tracking error

實際泊車過程中，車輛一般從靜止開始運動到達目標車速，存在速度波動情況，為驗證設(shè)計控制器的適用性，設(shè)計車速從0 m/s加速到目標車速并且其過程中存在速度波動，對該情況進行仿真。目標路徑方程為y=16sin(0.05x)+5,車輛初始位姿參數(shù)為(0，0，0.02)，仿真時間為200 s。

圖8 不同車速路徑跟蹤仿真結(jié)果Fig.8 Tracking simulation results of different vehicle speeds

由圖8可知，在不同目標速度并且存在波動的情況下，基于趨近律的終端滑模路徑跟蹤控制器能跟蹤到目標路徑，不同速度下跟蹤情況相似，跟蹤效果受到速度影響較小，與車輛位置有較大關(guān)系，說明速度變化劇烈的情況下，本文設(shè)計的控制器仍具有較好的跟蹤效果。

對于相同的參考路徑，3種路徑跟蹤控制器都能控制車輛運動模型跟蹤到目標路徑?；？刂平Y(jié)構(gòu)簡單，但控制效果一般，系統(tǒng)相對穩(wěn)定時，會存在較大的穩(wěn)定誤差，并且車身偏轉(zhuǎn)角有較強的抖振現(xiàn)象；相比前者，終端滑?？刂频目刂菩Ч兴纳?，穩(wěn)定誤差降低，但是車身偏轉(zhuǎn)角的抖振現(xiàn)象強烈，甚至還有所加強；本文設(shè)計的基于趨近律的終端滑?？刂破鞑粌H降低了穩(wěn)定誤差，而且車身偏轉(zhuǎn)角誤差的抖振現(xiàn)象也明顯減弱，控制效果更優(yōu)。

圖10 Simulink/CarSim聯(lián)合仿真結(jié)果Fig.10 Co-simulation result of Simulink/CarSim

4.3 Simulink/CarSim聯(lián)合仿真

在Simulink和CarSim中建立聯(lián)合仿真模型，包括路徑規(guī)劃、跟蹤控制、CarSim車輛模型S函數(shù)，仿真車速采用PID控制，不是本文重點，故不再贅述。聯(lián)合仿真流程如圖9所示。

圖9 聯(lián)合仿真流程圖Fig.9 Flow chart of co-simulation

結(jié)合日常泊車工況，設(shè)定期望車速為-1 m/s，x0=6.80 m，y0=1.80 m，θ0=0.087 rad。路徑規(guī)劃與跟蹤仿真結(jié)果如圖10所示。

由圖10a可知，在初始車身偏轉(zhuǎn)角不平行于車庫方向，車身偏轉(zhuǎn)角為5°的情況下，基于趨近律的終端滑?？刂破髂軌蚋鶕?jù)給出的參考路徑，控制目標車輛的方向盤轉(zhuǎn)角，使得車輛從起始位置到達目標位置。在路徑起始階段，車輛起始位姿與路徑起始處坐標以及該處切線方向一致，車輛能較好地跟蹤到參考路徑，當車輛停穩(wěn)后，車輛與目標位置的縱坐標誤差為0.027 8 m。

由圖10b可知，車輛能夠跟蹤到參考路徑下的參考車身偏轉(zhuǎn)角，泊車結(jié)束時誤差為0.005 rad。

由圖10c、10d可知，在控制器控制下，當車輛到達預設(shè)位置后實現(xiàn)停車，不隨時間的變化發(fā)生移動。

由圖10e可知，在控制器控制下，縱坐標正向誤差最大達到0.031 m，負向最大誤差為-0.028 m，在此過程中，存在抖振現(xiàn)象，并且當泊車過程結(jié)束時，存在誤差0.027 8 m。

由圖10f可知，跟蹤過程中存在抖振現(xiàn)象，在x=5.72 m處，車身偏轉(zhuǎn)角正向誤差最大，達到0.045 rad，x=1.68 m處，車身偏轉(zhuǎn)角負向誤差最大，達到-0.038 rad，當泊車結(jié)束時，存在0.005 rad的偏轉(zhuǎn)角誤差。

由圖11可知，在CarSim仿真結(jié)果后處理部分中的動畫顯示中，控制的目標車輛能夠從初始位置較好地沿著參考路徑進入目標車位，從整體的輪廓圖可以看出，車輛與周圍的車輛沒有發(fā)生碰撞，在停車時沒有與后車發(fā)生碰撞，所以設(shè)計的參考路徑是合理的。

圖11 CarSim仿真動畫顯示Fig.11 Simulation animation of CarSim

5 結(jié)論

(1)結(jié)合B樣條曲線方法建立了非線性泊車路徑優(yōu)化函數(shù)，并針對傾斜泊車起始位姿進行路徑規(guī)劃Simulink仿真，驗證了設(shè)計的路徑規(guī)劃方法具有較好的效果。

(2)結(jié)合非時間參考路徑跟蹤控制方法和終端滑模控制方法的優(yōu)點，提出了基于趨近律的終端滑模路徑跟蹤控制方法。通過跟蹤目標正弦路徑的仿真實驗對比，驗證了設(shè)計的控制器具有較好的路徑跟蹤控制效果。

(3)建立了路徑規(guī)劃和基于趨近律的終端滑模路徑跟蹤控制器Simulink模型以及目標車型的CarSim整車模型，聯(lián)合仿真結(jié)果表明，本文所提算法路徑規(guī)劃結(jié)果滿足車輛泊車避障和運動約束要求，所設(shè)計路徑跟蹤控制器控制能較好地跟蹤參考路徑，快速安全進入車位。