詹瑞典,黃經(jīng)偉,張學(xué)習(xí),肖 淳,侯 帥,蔡述庭
1.廣東工業(yè)大學(xué) 先進(jìn)制造學(xué)院,廣州 510006
2.廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院,廣州 510006
隨著人工智能的發(fā)展,自動(dòng)駕駛為解決交通擁堵和事故等問(wèn)題提供了一種新途徑[1-4]。自動(dòng)泊車(chē)系統(tǒng)作為自動(dòng)駕駛的一部分,已經(jīng)能夠有效實(shí)現(xiàn)汽車(chē)自動(dòng)泊車(chē)功能,但都是基于側(cè)方位倒車(chē)和垂直倒車(chē)方法實(shí)現(xiàn)的。然而,對(duì)于電動(dòng)汽車(chē)而言,許多充電口都安裝在車(chē)頭,使用垂直倒車(chē)方法可能會(huì)導(dǎo)致充電線(xiàn)太短無(wú)法充電或拉線(xiàn)過(guò)長(zhǎng)存在安全隱患等問(wèn)題。在未來(lái)幾十年,電動(dòng)汽車(chē)代替燃油汽車(chē)成為主流已是必然[5-7]。因此,設(shè)計(jì)一款針對(duì)電動(dòng)汽車(chē)的自動(dòng)泊車(chē)系統(tǒng)至關(guān)重要。另一方面,現(xiàn)有的先進(jìn)控制算法對(duì)單片機(jī)系統(tǒng)運(yùn)算速度和存儲(chǔ)空間都具有極高的要求,極有可能出現(xiàn)運(yùn)算爆炸和存儲(chǔ)爆炸的問(wèn)題,這將極大限制它在汽車(chē)工業(yè)中的應(yīng)用。泊車(chē)過(guò)程是一個(gè)低速行駛過(guò)程,對(duì)于系統(tǒng)的實(shí)效性要求并不高,本文采用PID方法控制汽車(chē)的泊車(chē)過(guò)程,克服“計(jì)算爆炸”問(wèn)題,并有效地實(shí)現(xiàn)車(chē)頭泊入。
本文針對(duì)上述問(wèn)題提出基于車(chē)頭泊入的自動(dòng)泊車(chē)新方法。該方法主要包括對(duì)車(chē)輛進(jìn)行運(yùn)動(dòng)學(xué)建模,根據(jù)汽車(chē)與車(chē)庫(kù)的相對(duì)位置,基于幾何推導(dǎo)進(jìn)行路徑規(guī)劃,并利用PID 控制算法減少系統(tǒng)計(jì)算量使之更符合工程應(yīng)用。在文獻(xiàn)[8-9]中,主要使用Matlab平臺(tái)建立車(chē)輛運(yùn)動(dòng)學(xué)模型,并設(shè)計(jì)路徑規(guī)劃及跟蹤控制器進(jìn)行仿真分析,該方法不能夠全面地對(duì)自動(dòng)泊車(chē)過(guò)程進(jìn)行運(yùn)動(dòng)仿真分析。有鑒于此,本文使用Matlab和CarSim聯(lián)合仿真,通過(guò)車(chē)輛運(yùn)動(dòng)學(xué)模型和實(shí)車(chē)參數(shù)在CarSim中搭建實(shí)驗(yàn)?zāi)P蛙?chē),聯(lián)合Matlab設(shè)計(jì)泊車(chē)路徑跟蹤控制器進(jìn)行聯(lián)合仿真實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果驗(yàn)證了本文設(shè)計(jì)的控制方法的有效性。
將泊車(chē)過(guò)程設(shè)置為一個(gè)極低速的(通常低于5 km/h)運(yùn)動(dòng)過(guò)程,此時(shí)車(chē)輪低速滾動(dòng),車(chē)輛一般不會(huì)發(fā)生側(cè)向滑動(dòng),可忽略側(cè)向力,因而沒(méi)有車(chē)輪側(cè)偏角。在實(shí)際工程中,通常對(duì)汽車(chē)運(yùn)動(dòng)模型進(jìn)行相應(yīng)的簡(jiǎn)化,建立車(chē)輛運(yùn)動(dòng)學(xué)模型。本文基于阿克曼轉(zhuǎn)向原理[10]建立車(chē)輛運(yùn)動(dòng)學(xué)模型,如圖1所示。
圖1 汽車(chē)運(yùn)動(dòng)學(xué)模型Fig.1 Car kinematics model
圖1 中,2d為輪距,L為車(chē)輛軸距,φ表示車(chē)輛等效的轉(zhuǎn)角,φl(shuí)為左輪轉(zhuǎn)角,φr為右輪轉(zhuǎn)角。為了方便計(jì)算,將四輪車(chē)簡(jiǎn)化為兩輪車(chē),根據(jù)阿克曼轉(zhuǎn)向原理可得:
由實(shí)際條件可知,在滿(mǎn)足阿克曼轉(zhuǎn)向原理?xiàng)l件下,車(chē)輛方向盤(pán)轉(zhuǎn)角和等效阿克曼前輪角之間存在近似線(xiàn)性的轉(zhuǎn)換關(guān)系:
其中,θsw為方向盤(pán)角度,k為轉(zhuǎn)換系數(shù),由實(shí)際測(cè)量結(jié)果可得,k≈16.2。以后軸中心為參考點(diǎn)的汽車(chē)運(yùn)動(dòng)學(xué)方程為:
其中,xr、yr分別為后軸中心對(duì)應(yīng)的x坐標(biāo)和y坐標(biāo),v為車(chē)輛中心速度,θ為車(chē)輛的橫擺角。
泊車(chē)系統(tǒng)通過(guò)傳感器感知泊車(chē)環(huán)境信息規(guī)劃泊車(chē)路徑,若路徑符合要求則進(jìn)行泊車(chē)操作,否則系統(tǒng)關(guān)閉。
車(chē)頭泊入的泊車(chē)方式分為垂直泊入和斜向泊入,本文對(duì)垂直泊入進(jìn)行規(guī)劃研究。
車(chē)庫(kù)模型設(shè)置為長(zhǎng)5 200 mm、寬為2 500 mm 的矩形,模型車(chē)體設(shè)置為長(zhǎng)4 542 mm、寬1 786 mm的矩形。
設(shè)計(jì)泊車(chē)過(guò)程共3個(gè)階段,分別如下:
汽車(chē)前進(jìn)階段軌跡:汽車(chē)由泊車(chē)中途停車(chē)點(diǎn)至車(chē)頭抵達(dá)車(chē)庫(kù)階段,汽車(chē)以最小轉(zhuǎn)彎半徑駛?cè)胲?chē)庫(kù),整個(gè)階段方向盤(pán)處于打滿(mǎn)狀態(tài),此時(shí)中途停車(chē)點(diǎn)與車(chē)庫(kù)入口的垂直距離最短,即最極限位置。
汽車(chē)入庫(kù)階段軌跡:可由上一階段的后軸中心位置和航向角θ以及完全入庫(kù)汽車(chē)最終位置的后軸中心規(guī)劃出路徑。
汽車(chē)后退階段軌跡:泊車(chē)中途停車(chē)點(diǎn)應(yīng)保證汽車(chē)的姿態(tài)不變,在后退階段對(duì)應(yīng)的圓與前進(jìn)的第一階段對(duì)應(yīng)的圓應(yīng)該保持相切關(guān)系;根據(jù)汽車(chē)的初始位置、航向角和前進(jìn)階段求得的圓,然后求出后退階段對(duì)應(yīng)的圓、中途停車(chē)點(diǎn)(即兩個(gè)圓的相切點(diǎn))和相應(yīng)的方向盤(pán)角度。
汽車(chē)前進(jìn)階段分為兩段軌跡:第一段軌跡為汽車(chē)自中途停車(chē)點(diǎn)前進(jìn)至車(chē)庫(kù);第二段為入庫(kù)軌跡。
2.2.1 前進(jìn)至車(chē)庫(kù)階段
取停車(chē)位左下角的點(diǎn)為坐標(biāo)原點(diǎn),長(zhǎng)為x軸,寬為y軸,汽車(chē)與x軸的夾角為航向角θ,逆時(shí)針為正。
為確保汽車(chē)可以完全駛?cè)胲?chē)庫(kù),可以設(shè)置汽車(chē)駛至車(chē)庫(kù)時(shí)的姿態(tài)如圖2所示。
圖2 車(chē)庫(kù)入口汽車(chē)角度示意圖Fig.2 Car angle of garage entrance
由幾何關(guān)系得此時(shí)汽車(chē)的航向角θ為:
其中,CW為車(chē)寬,CL為車(chē)長(zhǎng),PW為車(chē)庫(kù)寬,d取300 mm。
再求出此時(shí)后軸中心的坐標(biāo):
其中,Clf為汽車(chē)前懸長(zhǎng)度,L為軸距,則此時(shí)前進(jìn)階段對(duì)應(yīng)圓心坐標(biāo)為:
其中,Rmin為最小轉(zhuǎn)彎半徑。
2.2.2 入庫(kù)階段
由第2.2.1 小節(jié)設(shè)計(jì)的汽車(chē)開(kāi)始入庫(kù)時(shí)的姿態(tài),此時(shí)方向盤(pán)處于極限打滿(mǎn)狀態(tài),想要確保汽車(chē)可以完全入庫(kù)只需保證汽車(chē)入庫(kù)對(duì)應(yīng)的軌跡圓的半徑大于最小轉(zhuǎn)彎半徑即可。設(shè)計(jì)汽車(chē)完全入庫(kù)時(shí)的后軸中心處于車(chē)庫(kù)入口的中心點(diǎn),則汽車(chē)入庫(kù)的軌跡示意圖如圖3所示。
圖3 入庫(kù)示意圖Fig.3 Driving into garage
由圖3可得此時(shí)圓心和半徑存在關(guān)系:
假設(shè)汽車(chē)初始位置為(xf,yf),航向角為θ,前進(jìn)階段對(duì)應(yīng)的圓心為(x2,y2),后退階段對(duì)應(yīng)的圓心為(x1,y1),半徑為R1,如圖4所示。
圖4 后退軌跡示意圖Fig.4 Reversing trajectory
由幾何關(guān)系可知:
由上述方程可求出(x1,y1)和R1。
此時(shí)兩圓心的斜率為:
則兩個(gè)圓的交點(diǎn)坐標(biāo)為:
綜上,泊車(chē)過(guò)程前兩段軌跡各自所需的方向盤(pán)轉(zhuǎn)角為:
整個(gè)泊車(chē)過(guò)程的后軸中心規(guī)劃路徑如圖5所示。
圖5 泊車(chē)軌跡規(guī)劃圖Fig.5 Parking trajectory planned
設(shè)計(jì)合理的路徑規(guī)劃是泊車(chē)成功的前提,而控制車(chē)輛跟蹤規(guī)劃好的泊車(chē)路徑是成功泊車(chē)的保證。
汽車(chē)常用的跟蹤控制方法主要有PID、MPC和LQR三種算法[11-13]。MPC 和LQR 算法根據(jù)狀態(tài)方程在一定的時(shí)域上求得最優(yōu)解,如圖6所示。
圖6 MPC與LQR的工作時(shí)域Fig.6 Working time domain of MPC and LQR
PID 算法通過(guò)上一時(shí)刻的實(shí)際值與期望值的差值來(lái)調(diào)節(jié)控制量,因此雖然控制效果LQR 與MPC 整體優(yōu)于PID,但相應(yīng)的計(jì)算量LQR與MPC也遠(yuǎn)遠(yuǎn)高于PID[14-15]。對(duì)于泊車(chē)系統(tǒng)而言,整個(gè)過(guò)程是一個(gè)低速狀態(tài),實(shí)效性的要求并沒(méi)有高速行駛狀態(tài)高,因此使用PID控制可以保證跟蹤效果并減少計(jì)算量,降低對(duì)單片機(jī)的要求,使系統(tǒng)更符合工程應(yīng)用。
本文選用的汽車(chē)模型為自動(dòng)擋汽車(chē),且泊車(chē)過(guò)程為低速行駛過(guò)程,為了保證泊車(chē)過(guò)程的舒適性和減少泊車(chē)過(guò)程的誤差,設(shè)置泊車(chē)過(guò)程的最大車(chē)速為2 km/h,而自動(dòng)擋汽車(chē)怠速可達(dá)7 km/h。泊車(chē)過(guò)程主要控制的參數(shù)為:(1)制動(dòng)輪缸壓力;(2)方向盤(pán)角度。
如圖7 所示,汽車(chē)真實(shí)位置與參考位置的橫向誤差為:
圖7 泊車(chē)位置偏差分析圖Fig.7 Analysis of deviation of parking position
實(shí)際方向盤(pán)轉(zhuǎn)角與期望轉(zhuǎn)角誤差為:
可將誤差設(shè)置為:
其中,k為系數(shù)。
則PID控制器為:
仿真驗(yàn)證已經(jīng)成為產(chǎn)品開(kāi)發(fā)必不可少的一部分,傳統(tǒng)泊車(chē)實(shí)驗(yàn)需要準(zhǔn)備樣本車(chē)、硬件平臺(tái)、實(shí)驗(yàn)場(chǎng)地等,實(shí)際實(shí)驗(yàn)中可能由于算法不完善和操作失誤等問(wèn)題會(huì)對(duì)實(shí)驗(yàn)器材造成損壞,甚至導(dǎo)致不必要的人員受傷。隨著車(chē)輛仿真軟件功能的不斷集成,強(qiáng)大的仿真軟件已經(jīng)可以代替部分實(shí)物測(cè)試。
本文基于CarSim車(chē)輛動(dòng)力學(xué)仿真軟件建立目標(biāo)車(chē)型模型和模擬泊車(chē)環(huán)境,并結(jié)合路徑規(guī)劃方法和PID路徑跟蹤控制器,搭建Simulink 模型進(jìn)行聯(lián)合仿真,驗(yàn)證路徑規(guī)劃方法的合理性以及控制器的控制效果。
在Matlab 軟件上搭建路徑規(guī)劃模塊和跟蹤控制模塊,并與設(shè)置好的CarSim 聯(lián)立。CarSim 通過(guò)數(shù)據(jù)接口將模型傳輸?shù)侥繕?biāo)目錄的mdl文件中,以S 函數(shù)的形式添加到Simulink模塊庫(kù)中。Simulink通過(guò)調(diào)用該S函數(shù),與CarSim軟件進(jìn)行數(shù)據(jù)交互,完成聯(lián)合仿真,如圖8所示。
圖8 聯(lián)合仿真示意圖Fig.8 Co-simulation illustration
聯(lián)合仿真中CarSim提供了完善準(zhǔn)確的汽車(chē)運(yùn)動(dòng)模型,并設(shè)計(jì)了相應(yīng)的泊車(chē)環(huán)境,而Simulink/Matlab 便于計(jì)算分析,兩者結(jié)合可以讓仿真快速準(zhǔn)確完成并生成相應(yīng)的參數(shù)圖表、仿真過(guò)程的動(dòng)畫(huà)等,能夠有效避免使用單一軟件導(dǎo)致的效果不佳等問(wèn)題。
參考市面上的車(chē)型參數(shù),設(shè)置實(shí)驗(yàn)?zāi)P蛙?chē)主要參數(shù)如表1所示。
表1 汽車(chē)模型主要參數(shù)Table 1 Main parameters of car model
泊車(chē)過(guò)程為低速過(guò)程,對(duì)于自動(dòng)擋汽車(chē)只用控制制動(dòng)輪缸壓力來(lái)控制速度,CarSim輸入輸出見(jiàn)表2。
表2 CarSim輸入輸出Table 2 CarSim input and output
Simulink模型由三個(gè)主要模塊組成,分別是跟蹤控制模塊、路徑規(guī)劃模塊、CarSim 的S 函數(shù)模塊,如圖9所示。其中,路徑規(guī)劃模塊包括坐標(biāo)軸的轉(zhuǎn)換、初始位置參數(shù)的獲取以及兩段軌跡的規(guī)劃數(shù)據(jù)。
圖9 聯(lián)合仿真模型Fig.9 Co-simulation model
跟蹤控制器根據(jù)路徑規(guī)劃得到的期望信息和汽車(chē)的實(shí)時(shí)信息,通過(guò)控制算法計(jì)算得到輸出制動(dòng)輪缸壓力、方向盤(pán)角度及擋位,進(jìn)而反饋給CarSim的S函數(shù),保證汽車(chē)完成泊車(chē)任務(wù)。路徑規(guī)劃和跟蹤控制流程圖如圖10所示。
圖10 路徑規(guī)劃和跟蹤控制示意圖Fig.10 Path planning and tracking control
由上述內(nèi)容可知整個(gè)泊車(chē)過(guò)程分為以下幾個(gè)步驟:
(1)將方向盤(pán)轉(zhuǎn)至θsw1,并倒車(chē)至中途停車(chē)點(diǎn);
(2)將方向盤(pán)轉(zhuǎn)至θsw2,前進(jìn)至車(chē)庫(kù)入口;
(3)汽車(chē)駛?cè)胲?chē)庫(kù)并控制方向盤(pán)逐漸將車(chē)身擺至與車(chē)庫(kù)平行。
通過(guò)CarSim 與Simulink 聯(lián)合仿真實(shí)驗(yàn)驗(yàn)證路徑跟蹤控制模型的有效性,實(shí)驗(yàn)結(jié)果如圖11所示。
由圖11 可以看出,在本文所設(shè)計(jì)的跟蹤控制模型的作用下,整個(gè)泊車(chē)過(guò)程中模型車(chē)能較好地沿著規(guī)劃路徑完成整個(gè)泊車(chē)過(guò)程。
圖11 仿真動(dòng)畫(huà)Fig.11 Simulation animation
泊車(chē)過(guò)程中汽車(chē)的主要參數(shù)變化如圖12所示。
由圖12 可以看出,聯(lián)合仿真過(guò)程中模型車(chē)的方向盤(pán)轉(zhuǎn)角和車(chē)速變化均可以通過(guò)PID 得到有效控制。其中方向盤(pán)轉(zhuǎn)角變化均勻且具有較好的角度維持性,車(chē)速能夠保持低速狀態(tài)(低于2 km/h)且制動(dòng)輪缸壓力小于1.6 MPa(一般汽車(chē)制動(dòng)輪缸壓力要求不大于8 MPa)。汽車(chē)運(yùn)動(dòng)軌跡與規(guī)劃軌跡對(duì)比圖如圖13所示。
圖12 泊車(chē)過(guò)程中汽車(chē)的主要參數(shù)變化Fig.12 Change of car main parameters during parking
由圖13 可以看出,在聯(lián)合仿真下,設(shè)計(jì)的PID 控制器能夠很好地跟蹤參考路徑行駛至目標(biāo)位置。在倒車(chē)階段,車(chē)輛起始位置與路徑起始位置一致,車(chē)輛能較好地跟蹤到參考路徑,中途發(fā)生偏差也可以調(diào)整回來(lái),汽車(chē)的行駛路徑與參考路徑基本一致,誤差較?。辉谇斑M(jìn)至車(chē)庫(kù)入口階段,由于泊車(chē)過(guò)程中汽車(chē)實(shí)際停車(chē)點(diǎn)與參考停車(chē)點(diǎn)的偏差較大,而且受到最小轉(zhuǎn)彎半徑的影響偏差較上一階段大,但仍基本符合規(guī)劃路徑;汽車(chē)駛?cè)胲?chē)庫(kù)階段,此時(shí)需要逐漸擺正汽車(chē)車(chē)身和方向盤(pán),汽車(chē)方向盤(pán)需要一定時(shí)間調(diào)整,因此無(wú)法完全符合規(guī)劃要求,但整個(gè)泊車(chē)過(guò)程中模型車(chē)能較好地跟蹤理想泊車(chē)路徑。本文設(shè)計(jì)的入庫(kù)車(chē)輛的頂點(diǎn)與車(chē)庫(kù)頂點(diǎn)的距離為30 cm,泊車(chē)過(guò)程最大偏差不超過(guò)15 cm,仿真結(jié)果偏差仍在可接受范圍內(nèi)。
圖13 汽車(chē)運(yùn)動(dòng)軌跡與規(guī)劃軌跡對(duì)比圖Fig.13 Comparison of car trajectory and planned trajectory
針對(duì)電動(dòng)汽車(chē)泊車(chē)系統(tǒng)設(shè)計(jì)了一款車(chē)頭泊入的泊車(chē)方式,包括倒車(chē)、前進(jìn)和入庫(kù)三段規(guī)劃路徑,同時(shí)結(jié)合PID 控制跟蹤路徑,減少了常用泊車(chē)算法的計(jì)算量,具有較好的工程實(shí)踐意義?;谲?chē)輛運(yùn)動(dòng)學(xué)模型和實(shí)車(chē)參數(shù)在CarSim 中搭建了實(shí)驗(yàn)?zāi)P蛙?chē),聯(lián)合Simulink 建立了仿真實(shí)驗(yàn)平臺(tái),設(shè)計(jì)了路徑跟蹤控制模型并進(jìn)行了仿真實(shí)驗(yàn)。從聯(lián)合仿真結(jié)果可以看出:通過(guò)控制模型的輸入、輸出變量,可以實(shí)現(xiàn)模型車(chē)對(duì)規(guī)劃路徑的良好跟蹤,證明了本文設(shè)計(jì)的路徑跟蹤控制方法的有效性。