孫柯陽(yáng),王兆強(qiáng),梁威,劉斌山,孫崇智

（1.上海工程技術(shù)大學(xué)機(jī)械與汽車(chē)工程學(xué)院,上海 201620；2.甘肅省特種設(shè)備檢驗(yàn)檢測(cè)研究院,甘肅 蘭州 730050）

由于民用汽車(chē)擁有量的增長(zhǎng),在有限的生活空間內(nèi),司機(jī)在泊車(chē)過(guò)程中容易產(chǎn)生泊車(chē)事故,自動(dòng)泊車(chē)系統(tǒng)是解決此類(lèi)問(wèn)題的一種有效方法[1],通過(guò)傳感器與控制算法技術(shù)的融合,自動(dòng)泊車(chē)系統(tǒng)可以提升城市空間的利用效率,降低事故發(fā)生率及保障行人安全.

自動(dòng)泊車(chē)系統(tǒng)主要有環(huán)境感知、決策規(guī)劃及控制執(zhí)行[2-3]三大關(guān)鍵技術(shù).現(xiàn)已有許多與自動(dòng)泊車(chē)系統(tǒng)相關(guān)的研究,如：Vorobieva 等[4]分別基于單個(gè)和多個(gè)圓弧規(guī)劃不同的泊車(chē)路徑,且根據(jù)回旋線進(jìn)行平滑處理,然而此方法規(guī)劃的泊車(chē)路徑起點(diǎn)與終點(diǎn)處的曲率不為零,會(huì)造成車(chē)輛原地轉(zhuǎn)向；Zips 等[5]將運(yùn)動(dòng)學(xué)微分方程離散化制定靜態(tài)優(yōu)化問(wèn)題,通過(guò)迭代求解,縮短了泊車(chē)路徑,但此方法由于忽略了路徑曲率的連續(xù)性,因此泊車(chē)過(guò)程車(chē)輛行駛不平順；錢(qián)立軍等[6]基于最短泊車(chē)時(shí)間,提出最優(yōu)控制問(wèn)題,且使用高斯偽譜法提升了求解效率,然而此方法缺乏對(duì)控制器的研究,因此跟蹤精度低.針對(duì)上述研究中泊車(chē)路徑曲率不連續(xù)與缺乏控制器跟蹤控制的問(wèn)題,本文重點(diǎn)對(duì)泊車(chē)的路徑規(guī)劃、車(chē)輛跟蹤控制及控制器的優(yōu)化進(jìn)行研究.

1 動(dòng)力學(xué)模型建立

車(chē)輛是由車(chē)身、底盤(pán)及發(fā)動(dòng)機(jī)組成的復(fù)雜的非線性系統(tǒng),需要繁雜的非線性微分方程來(lái)表示車(chē)輛數(shù)學(xué)模型,然而此方法會(huì)增加系統(tǒng)的計(jì)算量及降低實(shí)時(shí)性,因此本文在簡(jiǎn)化車(chē)輛模型的基礎(chǔ)上,結(jié)合阿克曼轉(zhuǎn)向幾何建立了車(chē)輛動(dòng)力學(xué)模型,如圖1 所示.

圖1 車(chē)輛動(dòng)力學(xué)模型Fig.1 Vehicle dynamics model

圖1 中,R為質(zhì)心處轉(zhuǎn)彎半徑,Vf、Vr、V分別為前后輪速度及質(zhì)心速度,la和lb分別是前后軸到質(zhì)心處的距離,αf、αr、β、δ、φ、θ 分別是前輪側(cè)偏角、后輪側(cè)偏角、質(zhì)心側(cè)偏角、前輪等效轉(zhuǎn)角、橫擺角及航向角、Fyf和Fyr分別為前后輪所受側(cè)偏力.對(duì)圖中車(chē)輛模型進(jìn)行受力分析可得

式（1）中：m是整車(chē)質(zhì)量,I為轉(zhuǎn)動(dòng)慣量.當(dāng)輪胎側(cè)偏角很小時(shí),輪胎側(cè)偏力與側(cè)偏角呈線性關(guān)系,可表示為Fy=Cα,其中C是側(cè)偏剛度,因此前輪等效轉(zhuǎn)角較小時(shí),根據(jù)式（1）可得

對(duì)圖中速度關(guān)系進(jìn)行分析可得

由于車(chē)輛行駛過(guò)程中,輪胎側(cè)偏角較小,因此可以求得前后輪側(cè)角αf與αr的大小

根據(jù)公式（2）（3）和（4）,可以得到關(guān)于Vy和的狀態(tài)方程

2 泊車(chē)路徑規(guī)劃

2.1 B-spline 曲線

B-spline 曲線具有易于控制、節(jié)點(diǎn)間連接平滑且可以進(jìn)行局部修改曲線形狀的優(yōu)點(diǎn),因此本文采用B-spline 曲線用以規(guī)劃泊車(chē)曲線,n次B-spline 曲線的公式見(jiàn)式（6）

式（6）中：u∈[0,1]；n為樣條曲線的階數(shù)；Pi表示第i個(gè)控制點(diǎn)；Bi,n（u）表示第i個(gè)n次基函數(shù),如式（7）所示

由于三階B-spline 曲線的二階導(dǎo)數(shù)連續(xù),滿足泊車(chē)過(guò)程中,參考曲線曲率連續(xù)的要求,因此,公式（6）與公式（7）中n=3,可求得三階B-spline 曲線的表達(dá)式,如式（8）

其中P1、P2、P3與P4為曲線的控制點(diǎn),根據(jù)公式（8）即可得出參考路徑的斜率與曲率公式,如式（9）（10）

2.2 曲線控制點(diǎn)的計(jì)算

在計(jì)算控制點(diǎn)的過(guò)程中考慮碰撞約束且采用逆向泊車(chē)法對(duì)操作過(guò)程進(jìn)行研究,具體過(guò)程如圖2所示.

圖2 逆向泊車(chē)過(guò)程Fig.2 Reverse parking process

車(chē)輛從車(chē)位駛出總共分為三個(gè)階段：P1P3、P3P4 與P4P6.在P1P3 段,車(chē)輛行駛至虛線位置時(shí),車(chē)輛外輪廓點(diǎn)c與車(chē)位拐點(diǎn)B有一定的安全距離Lsafe2=Lsafe1=200 mm,此時(shí)車(chē)身轉(zhuǎn)動(dòng)β 度,可求得

根據(jù)β 的數(shù)值可以求得P1 與P3 之間坐標(biāo)差值ΔP13 為

為避免右側(cè)車(chē)身與拐點(diǎn)B發(fā)生碰撞,需要回正方向盤(pán),使車(chē)輛沿直線行駛至圖中點(diǎn)線位置,根據(jù)圖中關(guān)系可求得P3 與P4 之間坐標(biāo)差值ΔP34 為

在P4P6 階段,可以求得P5 與P6 之間坐標(biāo)差值ΔP56 為

由于參考曲線的曲率不能過(guò)大,因此取P3P4 的延長(zhǎng)線與P1 點(diǎn)切線的交點(diǎn)為P2 點(diǎn),同理可得P5點(diǎn),P2 與P3 之間坐標(biāo)差值ΔP23 為

P4 和P5 點(diǎn)之間的坐標(biāo)差值ΔP45=ΔP23,因此確定P1 點(diǎn)坐標(biāo)即可確定各個(gè)控制點(diǎn)的坐標(biāo).設(shè)P1=（x,y）,則各控制點(diǎn)的坐標(biāo)計(jì)算如下

3 基于LQR 與FLA 的控制器的設(shè)計(jì)與優(yōu)化

3.1 基于LQR 的控制器設(shè)計(jì)

自動(dòng)駕駛車(chē)輛在行駛過(guò)程中,由于外部環(huán)境與自身因素的影響,車(chē)輛實(shí)際運(yùn)動(dòng)路徑與參考路徑之間存在一定的誤差,見(jiàn)圖3.

圖3 橫向誤差示意Fig.3 Schematic diagram of lateral error

令φ=eφ+θr,根據(jù)公式（17）可以得到以下公式

實(shí)際道路曲率變化較為平緩,因此可以忽略的計(jì)算,將公式（18）帶入公式（5）可得

公式（19）可以簡(jiǎn)化為

分析公式（20）可知,控制器的目標(biāo)為選取合適的控制輸入u,使得實(shí)際路徑與規(guī)劃路徑之間的誤差e盡量小,因此可以將此類(lèi)問(wèn)題轉(zhuǎn)化為求解線性二次型調(diào)節(jié)器（linear quadratic regulator,LQR）問(wèn)題[7],即

式（21）中：X為誤差量err,Q=[q1,q2,q3,q4]和R=[r]分別為誤差與輸入對(duì)應(yīng)的權(quán)重矩陣,由于參考路徑是由大量離散化的參考點(diǎn)所組成,需要將=AX+Bu離散化為Xk+1=AXk+Buk,然后進(jìn)行求解代價(jià)函數(shù)的極小值,因此可得如下公式

式（22）中：ξ∈[t,t+dt],進(jìn)一步利用歐拉法處理公式（22）并化簡(jiǎn)可得

基于拉格朗日乘數(shù)法構(gòu)造如式（24）所示函數(shù),即可求得代價(jià)函數(shù)的極值

為了簡(jiǎn)化式（24）,可構(gòu)造Hamilton 方程[8],令可得

通過(guò)求解上述公式,最終可以得到一個(gè)Riccati 方程

進(jìn)一步可求得控制量uk的表達(dá)式

式（27）可以簡(jiǎn)寫(xiě)為u=-KX,其中,系數(shù)和R已知.

分析公式（20）可知,K取任意值時(shí),系統(tǒng)無(wú)法同時(shí)消除誤差及其變化量,因此,引入一個(gè)控制量δf,有,當(dāng)e˙rr=0時(shí),,因此可以選擇合適的控制量δf使得err=0,式err為

式（28）中：系數(shù)K1和K3由LQR 求出,當(dāng)橫向誤差ed=0 時(shí),對(duì)于橫擺角誤差eφ,根據(jù)圖1 進(jìn)行分析,由于質(zhì)心處轉(zhuǎn)彎半徑R遠(yuǎn)大于后軸到質(zhì)心的距離lb,因此,有eφ=-γ-αr,根據(jù)包含γ 角的虛線三角形的角度關(guān)系有β=γ+αr=-eφ,帶入eφ=φ-θr,可得θ=θr,車(chē)輛的航向角誤差總是為零.

3.2 基于FLA 的控制器優(yōu)化

通過(guò)對(duì)LQR 控制器的設(shè)計(jì)過(guò)程進(jìn)行研究,分析得出影響泊車(chē)系統(tǒng)性能的因素主要有泊車(chē)行駛速度及控制器的權(quán)重矩陣Q=[q1,q2,q3,q4]和R=[r],具體影響效果如圖4、圖5 所示.

圖4 不同速度下?tīng)顟B(tài)矩陣Fig.4 State matrix at different speeds

圖5 不同系數(shù)下控制效果Fig.5 Control effects under different coefficient

圖4 為不同速度下?tīng)顟B(tài)矩陣A的變化,對(duì)比圖4-a、b、c 和d 可以得出隨著速度的增大,狀態(tài)矩陣中的部分系數(shù)隨之減小,因此,在不同速度場(chǎng)景下泊車(chē)精度不同.圖5 為不同權(quán)重系數(shù)條件下車(chē)輛跟蹤五次多項(xiàng)式S 型曲線,車(chē)輛的跟蹤誤差與橫擺角誤差,對(duì)比圖5-a、b、c 和d 可以發(fā)現(xiàn)q1 與q4 作用下的控制器穩(wěn)定且跟蹤誤差與橫擺角誤差較小,相反的,q2 與q3 對(duì)控制器的精度影響較小且會(huì)降低控制器的穩(wěn)定性.對(duì)于權(quán)重系數(shù)R,當(dāng)R=0.1 與R=10 時(shí),控制器的跟蹤效果不佳,因此直接取R=1,即有較好的控制效果.

現(xiàn)階段可以通過(guò)不斷試驗(yàn)調(diào)整權(quán)重矩陣以達(dá)到良好的跟蹤效果,然而此方法人工成本較高,隨著計(jì)算機(jī)技術(shù)的發(fā)展,有各類(lèi)優(yōu)化算法被廣泛地應(yīng)用于權(quán)重系數(shù)的選取,如灰狼算法[9]、藍(lán)鯨算法[10]與神經(jīng)網(wǎng)絡(luò)[11],而智能控制算法中的模糊邏輯算法（Fuzzy Logic Algorithm,FLA）是基于人的經(jīng)驗(yàn)進(jìn)行設(shè)計(jì)的,因此,對(duì)于精度低,非線性與時(shí)變性的被控對(duì)象也有很好的控制效果,可通過(guò)模糊邏輯算法代替人工進(jìn)行參數(shù)調(diào)整.模糊邏輯算法的設(shè)計(jì)過(guò)程主要有三步：模糊化、模糊推理與解模糊[12].以誤差e為輸入,控制因子r為輸出,最終通過(guò)q=10r計(jì)算得出最終的權(quán)重系數(shù).首先,確定模糊詞匯為：負(fù)大（NB）、負(fù)小（NS）、零（ZO）,正小（PS）和正大（PB）,定義輸入量的論域?yàn)閇-0.6,0.6],輸出量的論域分別為[-1,1]和[-1,0.5],得到隸屬度函數(shù),見(jiàn)圖6.

圖6 各隸屬度函數(shù)Fig.6 Membership functions

為了后續(xù)的模糊推理,需要制定相應(yīng)的模糊規(guī)則庫(kù),研究圖5 中各系數(shù)作用下的控制規(guī)律可得到規(guī)則庫(kù),見(jiàn)表1 和表2.

表1 red 的規(guī)則庫(kù)Tab.1 The rule base of red

表2 reφ的規(guī)則庫(kù)Tab.2 The rule base of reφ

由模糊推理得到的值為模糊值,需要相應(yīng)的解模糊方法將模糊值轉(zhuǎn)化為實(shí)際值,本文使用加權(quán)平均法進(jìn)行處理,得到控制器所需的實(shí)際值如圖7 所示.

圖7 輸出精確值Fig.7 The exact value of output

3.3 基于Frobenius 范數(shù)的更新方法

由于實(shí)際泊車(chē)場(chǎng)景的復(fù)雜性,泊車(chē)過(guò)程中車(chē)輛在不同場(chǎng)景下行駛速度不同,且模糊邏輯算法計(jì)算過(guò)程是實(shí)時(shí)的,需要相應(yīng)的更新方法以降低計(jì)算量,提升對(duì)不同速度的泊車(chē)場(chǎng)景的適應(yīng)度,因此,本文將基于Frobenius 范數(shù)的相似度計(jì)算引入優(yōu)化過(guò)程中,通過(guò)對(duì)比范數(shù)的接近程度,可以確定兩個(gè)矩陣之間的差異.

以狀態(tài)矩陣A為例,矩陣A為當(dāng)前速度下的狀態(tài)矩陣,A1 為另一速度下的狀態(tài)矩陣,則有差異矩陣D=A-A1,則D的Frobenius 范數(shù)為,兩個(gè)狀態(tài)矩陣的相似度為

式（29）中：差異矩陣的范數(shù)值越小,狀態(tài)矩陣A和A1 之間的相似度越高,同理,權(quán)重矩陣Q的更新方法亦是如此,更新方法制定完成后需選擇相應(yīng)的更新閾值,當(dāng)相似度小于閾值,則更新權(quán)重矩陣與狀態(tài)矩陣,反之,則不更新矩陣,考慮到計(jì)算量與精度的要求,本文設(shè)置的更新閾值為0.8.

4 仿真與分析

為了驗(yàn)證本文規(guī)劃的參考路徑與控制器的有效性,使用MATLAB/Simulink 與Carsim 進(jìn)行聯(lián)合仿真測(cè)試,仿真采用的車(chē)輛參數(shù)為軟件內(nèi)部車(chē)輛模型,具體參數(shù)如表3 所示.

表3 車(chē)輛參數(shù)Tab.3 Vehicle parameters

選取車(chē)位尺寸為[7.5 m,2.5 m],計(jì)算得出以后軸中心為參考點(diǎn)的參考路徑與路徑曲率,如圖8 所示.

圖8 參考路徑Fig.8 The reference path

從圖8 可以看出規(guī)劃的泊車(chē)路徑滿足車(chē)輛避障約束且路徑曲率連續(xù),保證了泊車(chē)過(guò)程中方向盤(pán)轉(zhuǎn)動(dòng)連續(xù)且平順.由于仿真軟件以前軸中心為參考點(diǎn),因此取控制點(diǎn)P1 的坐標(biāo)為（2.91,0）,則各控制點(diǎn)的坐標(biāo)見(jiàn)表4.

表4 控制點(diǎn)坐標(biāo)Tab.4 Control point coordinates

引入更新機(jī)制后,在設(shè)計(jì)的程序中加入tic 函數(shù),得到更新前后程序運(yùn)算的時(shí)間,如圖9 所示.

圖9 程序運(yùn)算時(shí)間Fig.9 Program operation time

由圖9 可知,引入更新機(jī)制前,從整體上來(lái)看,程序運(yùn)算的時(shí)間在2.5 ms 上下波動(dòng),加入更新機(jī)制后,程序運(yùn)算時(shí)間約為0.2 ms,程序運(yùn)行效率提升了92%,因此,在降低人工調(diào)整參數(shù)成本的同時(shí),控制器的實(shí)時(shí)性有所提升.

設(shè)定速度為1 m/s,對(duì)優(yōu)化后控制器的性能進(jìn)行仿真驗(yàn)證,具體仿真結(jié)果如圖10 所示.

圖10 仿真結(jié)果Fig.10 Simulation results

結(jié)合圖10-a,可以發(fā)現(xiàn)跟蹤誤差最大約為0.15 m,在泊車(chē)起始階段與即將進(jìn)入車(chē)位的階段,車(chē)輛產(chǎn)生的跟蹤誤差較大,在最終駛?cè)胲?chē)位后,車(chē)輛與預(yù)定位置相差約0.13 m.分析泊車(chē)過(guò)程中產(chǎn)生較大跟蹤誤差的原因是設(shè)計(jì)模糊邏輯算法的過(guò)程中,模糊詞匯數(shù)量選取較少,導(dǎo)致控制器的跟蹤精度下降.對(duì)于橫擺角誤差,從圖10-b 中可以看出車(chē)輛橫擺角誤差最大約為0.08 rad,角度誤差較小,車(chē)輛停止時(shí)與車(chē)位的夾角約為0.03 rad,近似與車(chē)位平行.圖10-c 為車(chē)輛的前輪轉(zhuǎn)角,從圖中可以看出車(chē)輛的前輪轉(zhuǎn)角變化連續(xù)且較為平順,然而在最后階段前輪轉(zhuǎn)角出現(xiàn)小范圍的波動(dòng),分析后可知在泊車(chē)最后階段車(chē)輛調(diào)整車(chē)身角度的時(shí)候出現(xiàn)誤差.

圖11 為Carsim 中展示的動(dòng)態(tài)泊車(chē)過(guò)程,圖中可以看出,雖然行駛過(guò)程中出現(xiàn)跟蹤誤差,然而并未與障礙車(chē)輛出現(xiàn)碰撞,可以完成安全無(wú)碰撞的泊入車(chē)位的要求.

圖11 路徑跟蹤圖Fig.11 Path tracking

圖12 為優(yōu)化前后,在不同速度條件下控制器的跟蹤結(jié)果,對(duì)比圖12-a 與b 中的跟蹤誤差與橫擺角誤差,得出優(yōu)化后的跟蹤誤差降低了約30%,橫擺角誤差降低幅度較小,表明優(yōu)化后的控制器提升了在不同速度的泊車(chē)場(chǎng)景的適應(yīng)度.

圖12 控制器適應(yīng)度仿真結(jié)果Fig.12 Simulation results of controller fitness

5 小結(jié)

首先,針對(duì)兩段圓弧式參考曲線曲率不連續(xù)的問(wèn)題,使用B-spline 曲線計(jì)算出一條曲率連續(xù)的路徑,仿真結(jié)果表明規(guī)劃路徑曲率連續(xù),滿足泊車(chē)需求；其次,結(jié)合車(chē)輛動(dòng)力學(xué)模型與LQR 理論設(shè)計(jì)出線性二次型控制器,利用模糊邏輯算法優(yōu)化控制器,解決了人工調(diào)整參數(shù)耗時(shí)耗力的問(wèn)題；最后,引入相似度的理論,提升控制系統(tǒng)的實(shí)時(shí)性與不同速度條件下的適應(yīng)度,使用MATLAB 與Carsim 進(jìn)行聯(lián)合仿真測(cè)試,仿真結(jié)果表明,優(yōu)化后控制器的運(yùn)算速率由改進(jìn)前的2.5 ms 變?yōu)楦倪M(jìn)后的0.2 ms,運(yùn)算效率提升了92%,同時(shí)改進(jìn)后的控制器在不同速度下的跟蹤誤差相較于改進(jìn)前的降低了約30%,因此改進(jìn)后的控制器的適應(yīng)度提升了30%.