楊 強(qiáng) 袁希文 肖緋雄 羅佳祥,3

1西南交通大學(xué)牽引動(dòng)力國家重點(diǎn)實(shí)驗(yàn)室 成都 610031 2中車株洲電力機(jī)車研究所有限公司 株洲 412001 3西南交通大學(xué)電氣工程學(xué)院 成都 611756

0 引言

車輛隊(duì)列行駛較單車行駛具有降低燃油消耗和減小空氣污染，增加道路交通的容載量等優(yōu)點(diǎn)。隨著汽車智能駕駛技術(shù)的不斷發(fā)展完善，無人車輛隊(duì)列控制技術(shù)逐漸成為近年國內(nèi)外研究的熱門課題之一[1]。目前隊(duì)列控制方法主要有行為法[2，3]、人工勢場法[4，5]、虛擬結(jié)構(gòu)法[6，7]和領(lǐng)航-跟隨法[8-10]。領(lǐng)航-跟隨法是目前無人系統(tǒng)編隊(duì)控制中最常用的方法[1]。領(lǐng)航者是編隊(duì)中被指定的某一車輛，其他車輛通過獲取領(lǐng)航者的運(yùn)動(dòng)信息來規(guī)劃自身的運(yùn)動(dòng)軌跡并進(jìn)行控制從而達(dá)到追蹤領(lǐng)航者的目的[11]。領(lǐng)航-跟隨法使得隊(duì)列控制問題成為傳統(tǒng)軌跡跟蹤問題的自然延伸，在通過領(lǐng)航-跟隨法確定跟隨車需要跟蹤的特定位置后便可將汽車智能駕駛中的縱向控制與橫向控制[12-15]技術(shù)運(yùn)用于其上，從而實(shí)現(xiàn)車輛隊(duì)列控制。

王保防等[16]根據(jù)車式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型提出了一種基于軌跡跟蹤多機(jī)器人隊(duì)列控制方法，利用隊(duì)列結(jié)構(gòu)參數(shù)確定隊(duì)形，根據(jù)隊(duì)列軌跡和相關(guān)參數(shù)生成虛擬機(jī)器人；然后運(yùn)用反步法構(gòu)造車式移動(dòng)機(jī)器人軌跡跟蹤系統(tǒng)的李雅普諾夫函數(shù)，通過使該函數(shù)負(fù)定，得到跟隨機(jī)器人的軌跡跟蹤控制器。羅京等[17]針對多個(gè)兩輪差動(dòng)移動(dòng)機(jī)器人的隊(duì)列控制問題，提出了一種結(jié)合Polar Histogram避障法的領(lǐng)航-跟隨協(xié)調(diào)隊(duì)列控制算法。該算法在領(lǐng)航-跟隨l-φ隊(duì)列控制結(jié)構(gòu)的基礎(chǔ)上引入虛擬跟隨機(jī)器人，采用極坐標(biāo)系下基于障礙物密度的Polar Histogram避障法，引導(dǎo)多機(jī)器人隊(duì)列智能地避開障礙物。師五喜等[18]為控制多輪式機(jī)器人協(xié)作系統(tǒng)中跟隨者機(jī)器人的輸入速度v和加速度a不超過其額定值vm和am，設(shè)計(jì)了一種速度和加速度受限的領(lǐng)航-跟隨型編隊(duì)控制算法，此算法設(shè)計(jì)了2個(gè)一階濾波器，作為控制器的2個(gè)反饋信號，且此反饋信號及其導(dǎo)數(shù)的大小受限于2個(gè)濾波器參數(shù)。

上述文獻(xiàn)的共同特點(diǎn)是在領(lǐng)航-跟隨l-φ隊(duì)列控制結(jié)構(gòu)的基礎(chǔ)上引入虛擬跟隨車而變成領(lǐng)航-虛擬跟隨法，將隊(duì)列控制問題轉(zhuǎn)化為跟隨車對虛擬跟隨車的軌跡跟蹤控制，進(jìn)而設(shè)計(jì)了不同的算法進(jìn)行控制器的設(shè)計(jì)。然而這種使虛擬跟隨車參考航向角等價(jià)于領(lǐng)航車的航向角的方法，在曲線工況下存在模型誤差。

因此，本文提出了一種考慮跟隨車參考路徑點(diǎn)連線與橫軸的夾角作為參考航向角的改進(jìn)領(lǐng)航-虛擬跟隨方法規(guī)劃跟隨車位姿，提出基于運(yùn)動(dòng)學(xué)模型的預(yù)測方法規(guī)劃跟隨車速度。分別使用PD控制和誤差修正的Stanley方法[19]控制車輛油門、剎車達(dá)到跟隨車參考速度和跟隨車前輪轉(zhuǎn)向使其達(dá)到參考軌跡。最后使用CarSim/Simulink仿真驗(yàn)證了本文提出的方法較改進(jìn)前方法的隊(duì)列控制橫向跟蹤效果的提升。

1 車輛隊(duì)列運(yùn)動(dòng)規(guī)劃

車輛隊(duì)列運(yùn)動(dòng)規(guī)劃是在領(lǐng)航車位姿、速度確定的條件下，為跟隨車橫向控制設(shè)計(jì)參考軌跡使跟隨車與領(lǐng)航車保持特定的隊(duì)形；為跟隨車縱向控制規(guī)劃參考速度使領(lǐng)航車與跟隨車的距離穩(wěn)定在參考距離。本文通過改進(jìn)的領(lǐng)航-虛擬跟隨法規(guī)劃跟隨車軌跡，基于車輛運(yùn)動(dòng)學(xué)模型預(yù)測方法規(guī)劃跟隨車速度。從使用領(lǐng)航車信息進(jìn)行隊(duì)列運(yùn)動(dòng)規(guī)劃到隊(duì)列控制實(shí)現(xiàn)的過程如圖1所示。

圖1 車輛隊(duì)列控制原理框圖

1.1 改進(jìn)的領(lǐng)航-虛擬跟隨法

領(lǐng)航-虛擬跟隨法即設(shè)置與領(lǐng)航者一定距離和角度的虛擬車輛作為虛擬跟隨車，當(dāng)控制跟隨車與虛擬跟隨車的位置完全重合時(shí)便實(shí)現(xiàn)了車輛編隊(duì)。如圖2所示，可得虛擬跟隨車車輛位姿為

圖2 領(lǐng)航-虛擬跟隨法隊(duì)形結(jié)構(gòu)

式中：xl、yl分別為領(lǐng)航車前軸中心在全局坐標(biāo)系下橫坐標(biāo)、縱坐標(biāo)位置，θl為領(lǐng)航車航向角，xv、yv分別為虛擬跟隨車前軸中心在全局坐標(biāo)系下橫坐標(biāo)、縱坐標(biāo)位置，θv為虛擬跟隨車航向角，l、φ分別為跟隨距離和跟隨角度。

由式（1）可知，虛擬跟隨車在其軌跡中的參考航向角等于對應(yīng)位置領(lǐng)航車的航向角，但并不一定是其軌跡的切線與橫軸的夾角。如圖3所示，以跟隨車作圓周軌跡運(yùn)動(dòng)的特例來說明。領(lǐng)航車的參考航向角為圓周離散點(diǎn)切線與橫軸X的夾角θl。參考航向角設(shè)定的依據(jù)為剛體瞬時(shí)速度的方向沿運(yùn)動(dòng)軌跡的切線方向，以運(yùn)動(dòng)軌跡切線與橫軸的夾角作為車輛的參考航向角[20，21]。

圖3 領(lǐng)航-虛擬跟隨法軌跡

考慮圖3中領(lǐng)航車位置處于A點(diǎn)而虛擬跟隨車位置由式（1）確定而處于B點(diǎn)，由領(lǐng)航-虛擬跟隨法確定的參考航向角為θl，而跟隨車軌跡切線與橫軸的夾角為θl2，由于這2個(gè)角度不相等，跟隨車從理論上無法精確地跟隨參考軌跡，即領(lǐng)航-虛擬跟隨法在這種特定的情況下存在模型誤差。

因此，通過使虛擬跟隨車參考航向角等于2個(gè)時(shí)刻虛擬跟隨車位置的連線與橫軸的夾角以近似其位置切線與橫軸的夾角，給出改進(jìn)的領(lǐng)航-虛擬跟隨法表達(dá)式為

式中：xl(k)、yl(k)分別為此刻領(lǐng)航車前軸中心在全局坐標(biāo)系下橫坐標(biāo)、縱坐標(biāo)位置，θl(k)為此刻領(lǐng)航車航向角，xv(k)、yv(k)，xv(k-1)、yv(k-1)分別為此刻和上一時(shí)刻虛擬跟隨車前軸中心在全局坐標(biāo)系下橫坐標(biāo)、縱坐標(biāo)位置，θv(k)為跟隨車航向角，l、φ分別為跟隨距離和跟隨角度。

1.2 縱向速度規(guī)劃

如圖4所示，此時(shí)刻領(lǐng)航車的位置、航向角、速度和車輪旋轉(zhuǎn)角度已知，跟隨車的位置、航向角、車輪旋轉(zhuǎn)角度已知，速度為待求量。該模型假定車輛在未來一個(gè)短的時(shí)間內(nèi)以恒定速度運(yùn)動(dòng)且車輪轉(zhuǎn)角不變。在速度約束條件下，當(dāng)求解某一速度使得2車前軸中心距離與參考距離最接近，此速度即為跟隨車的參考速度。

圖4 領(lǐng)航車、跟隨車運(yùn)動(dòng)學(xué)模型預(yù)測方法幾何分析

由阿克曼轉(zhuǎn)向模型有

式中：δl為領(lǐng)航車車輪轉(zhuǎn)角，rl為領(lǐng)航車轉(zhuǎn)彎半徑，a為領(lǐng)航車軸距。

經(jīng)過時(shí)間t，車輛運(yùn)動(dòng)距離為

式中：sl為領(lǐng)航車位移（圖中虛線表示的一段圓?。瑅l為領(lǐng)航車速度。

由阿克曼轉(zhuǎn)向模型與平面幾何關(guān)系有

式中：lsl為領(lǐng)航車2時(shí)刻車輛后軸中心距離。

根據(jù)運(yùn)動(dòng)學(xué)模型，有

式中：θl為領(lǐng)航車運(yùn)動(dòng)變化角。

領(lǐng)航車t時(shí)刻后車輛后軸中心位置為

式中：xmlr、ymlr分別為領(lǐng)航車后軸中心當(dāng)前時(shí)刻在全局坐標(biāo)系下的橫坐標(biāo)、縱坐標(biāo)，xnlr、ynlr分別為領(lǐng)航車后軸中心未來時(shí)刻在全局坐標(biāo)系下的橫坐標(biāo)、縱坐標(biāo)，φml為當(dāng)前時(shí)刻領(lǐng)航車車身航向角。

領(lǐng)航車t時(shí)刻后車輛前軸中心位置為

式中：xnlf、ynlf分別為領(lǐng)航車前軸中心未來時(shí)刻在全局坐標(biāo)系下的橫坐標(biāo)、縱坐標(biāo)，φnl為未來時(shí)刻領(lǐng)航車車身航向角。

同理可得跟隨車t時(shí)刻后車輛前軸中心位置(xnff，ynff)。

2車前軸中心距離為

求解式(11)即得到跟隨車參考速度使得未來一個(gè)時(shí)刻后2車前軸中心距離與參考距離最接近。

式中：L為2車前軸中心距離，Lr為期望領(lǐng)航車與跟隨車所保持的參考距離，vf為待求的跟隨車速度，vmin和vmax為跟隨車所允許的最小速度與最大速度。

2 車輛隊(duì)列運(yùn)動(dòng)控制

跟隨車隊(duì)列運(yùn)動(dòng)控制器的設(shè)計(jì)包含轉(zhuǎn)向角橫向控制和速度縱向控制，2個(gè)方面的內(nèi)容，轉(zhuǎn)向角橫向控制為當(dāng)跟隨車的參考位姿由領(lǐng)航-虛擬跟隨法確定后，使用誤差修正的Stanley方法進(jìn)行橫向控制使跟隨車處于參考軌跡上。速度縱向控制為當(dāng)跟隨車參考速度由規(guī)劃得到后，使用標(biāo)定策略對車輛驅(qū)/制動(dòng)非線性環(huán)節(jié)擬合與PD控制跟隨車速度達(dá)到參考速度。

2.1 轉(zhuǎn)向角橫向控制

本文使用誤差修正的Stanley方法對跟隨車輛轉(zhuǎn)向進(jìn)行控制。如圖5所示，其基于前軸中心點(diǎn)與參考軌跡的幾何關(guān)系給出非線性的前輪轉(zhuǎn)向角控制量，其轉(zhuǎn)角表達(dá)式為

圖5 Stanley方法幾何分析

式中：δθe為航向角（近似等于車身與橫軸的夾角）誤差引起的前輪轉(zhuǎn)向角，可由當(dāng)橫向誤差為0時(shí)求得，用以消除航向角誤差；δe為橫向誤差（前軸中心與參考軌跡的最近距離）引起的前輪轉(zhuǎn)向角，可由當(dāng)航向角誤差為0時(shí)求得，用以消除橫向誤差；φ為跟隨車航向角；φr為參考航向角；v(t)為跟隨車速度；e(t)為橫向誤差；k為比例系數(shù)。

前輪轉(zhuǎn)向角為

若將Stanley方法直接應(yīng)用于車輛隊(duì)列控制問題中的跟隨車時(shí)會出現(xiàn)誤差距離不匹配的問題。Stanley方法的路徑點(diǎn)是通過索引車輛前軸中心點(diǎn)與參考軌跡的最近點(diǎn)，將其作為路徑點(diǎn)，將2點(diǎn)之間的距離作為誤差距離，車輪轉(zhuǎn)角將使得誤差距離趨于0。跟隨車的路徑點(diǎn)由虛擬跟隨車的位置決定，當(dāng)虛擬跟隨車與跟隨車位置如圖6所示時(shí)，Stanley方法中的橫向誤差若直接使用2車距離即圖5中e所示距離，相當(dāng)于將虛擬跟隨車與跟隨車的縱向距離誤差也進(jìn)行了考慮使得橫向誤差過大，會出現(xiàn)轉(zhuǎn)向過大的問題。此時(shí)計(jì)算轉(zhuǎn)角誤差應(yīng)當(dāng)使用2點(diǎn)距離在路徑點(diǎn)切線的垂直方向投影替代，即圖6中elat所示距離。

圖6 誤差修正的Stanley方法幾何分析

因此，跟隨車橫向控制轉(zhuǎn)角為

2.2 速度縱向控制

車輛隊(duì)列控制問題中跟隨車縱向控制的過程是在對車輛驅(qū)/制動(dòng)非線性環(huán)節(jié)擬合的基礎(chǔ)上，將由PD控制得到的車輛期望加速度轉(zhuǎn)化為油門/剎車信號控制車輛實(shí)際速度達(dá)到規(guī)劃得到的參考速度，以保持跟隨車與領(lǐng)航車的距離恒定。

首先通過開環(huán)標(biāo)定的策略擬合車輛驅(qū)/制動(dòng)非線性環(huán)節(jié)。其過程是在Carsim中選擇平直道路，在不同油門開度和剎車壓力輸入的情況下，對車輛速度、加速度數(shù)據(jù)進(jìn)行多次采集，并將數(shù)據(jù)擬合生成三維標(biāo)定表。標(biāo)定結(jié)果如圖7所示。使用車輛驅(qū)/制動(dòng)標(biāo)定表時(shí)是以期望加速度、車速作為輸入，以油門開度/制動(dòng)壓力作為輸出，其關(guān)系表示為

圖7 驅(qū)/制動(dòng)標(biāo)定表

式中：U為開環(huán)油門開度/制動(dòng)壓力；(Map)為標(biāo)定表；v為車輛速度；a為車輛期望加速度；kp為比例系數(shù)，取值為6；kD為微分系數(shù)，取值為0.05；error(t)為期望速度與車輛實(shí)際速度之差；vtarget為期望速度，v為車輛實(shí)際速度。

3 仿真實(shí)驗(yàn)與分析

在CarSim/Simulink中，分別使用領(lǐng)航-虛擬跟隨法與本文所提的參考航向角表達(dá)式修正后的領(lǐng)航-虛擬跟隨法對車輛進(jìn)行三角形編隊(duì)仿真。仿真所使用的車輛為Carsim數(shù)據(jù)庫中自帶的B級Hatchback轎車。其車身質(zhì)量為1 230 m/kg；質(zhì)心到前軸距離a為1 040 mm；質(zhì)心到后軸距離b為1 560 mm；質(zhì)心高度h為540 mm；輪距L為1 480 mm；車身繞z軸轉(zhuǎn)動(dòng)慣量IZ為1 343.1kg/m2；輪胎類型為185/65R15。

仿真的初始條件設(shè)置為：領(lǐng)航車輛行駛的軌跡分別為圓心位置（0 m，25 m）、半徑為25 m的圓周和由方程y=4sin(x/10)確定的正弦曲線。領(lǐng)航車初始位置為（0 m，0 m），速度為10 m/s勻速；跟隨車輛1、2初始位置分別為（-4.85 m，2.8 m）、（-4.85 m，-2.8 m），初始速度為10 m/s；2臺跟隨車前軸中心與領(lǐng)航車前軸中心參考距離都為5.6 m。

3.1 改進(jìn)前、后領(lǐng)航-虛擬跟隨法仿真結(jié)果對比

改進(jìn)前、后領(lǐng)航-虛擬跟隨法在圓周和正弦曲線下車輛隊(duì)列控制仿真的軌跡如圖8、圖9所示。軌跡圖中車輛位置為從0時(shí)刻開始每隔1.5 s取樣確定的。由圖可知，改進(jìn)前、后的領(lǐng)航-虛擬跟隨法均可以引導(dǎo)跟隨車使其與領(lǐng)航車形成預(yù)定的三角形編隊(duì)，但領(lǐng)航-虛擬跟隨法下跟隨車輛偏離參考軌跡的橫向距離較大，改進(jìn)后的領(lǐng)航-虛擬跟隨法下跟隨車輛偏離參考軌跡的橫向距離較小。圖10、圖11分別展示了圓周工況下改進(jìn)前、后領(lǐng)航-虛擬跟隨法下的3車車輪轉(zhuǎn)角與車輛速度。圖12、圖13分別展示了正弦曲線工況下改進(jìn)前、后領(lǐng)航-虛擬跟隨法下的3車車輪轉(zhuǎn)角與車輛速度。如圖14、圖15所示的改進(jìn)前、后領(lǐng)航虛擬跟隨法在圓周和正弦曲線下的橫向誤差，可知改進(jìn)后的領(lǐng)航-虛擬跟隨法使得跟隨車橫向跟蹤誤差得到了較大的降低。

圖8 圓周工況下車輛軌跡

圖9 正弦曲線工況下車輛軌跡

圖10 圓周工況下車輪轉(zhuǎn)角

圖11 圓周工況下車輛速度

圖12 正弦曲線工況下車輪轉(zhuǎn)角

圖13 正弦曲線工況下車輛速度

圖14 圓周工況下橫向跟蹤誤差

圖15 正弦曲線工況下橫向跟蹤誤差

3.2 仿真結(jié)果分析

改進(jìn)前、后的領(lǐng)航-虛擬跟隨法在圓周和正弦曲線下跟隨車橫向跟蹤誤差如表2、表3所示。其中，表2中誤差均值通過各時(shí)刻誤差絕對值求和平均后得到。表3中，因初始位置關(guān)系所造成的誤差最大值無意義因此不計(jì)，誤差最大值選取的是0.57以后的誤差絕對值的最大值。

表2 圓周工況下橫向跟蹤誤差

表3 正弦曲線工況下橫向跟蹤誤差 m

從結(jié)果來看使用本文所提的參考航向角公式修正后的領(lǐng)航-虛擬跟隨法在圓周曲線、正弦曲線工況下橫向誤差均值和最大值均較改進(jìn)前降低。在圓周工況下跟隨車1號、2號橫向誤差均值較改進(jìn)前分別降低了0.486 m與0.339 m，降低幅度為83.22%、69.61%；橫向誤差最大值較改進(jìn)前分別降低了0.445 m、0.339 m，降低幅度為74.92%、67.26%。在正弦曲線工況下跟隨車1號、2號橫向誤差均值較改進(jìn)前分別降低了0.231 m與0.232 m，降低幅度為73.80%、70.09%；橫向誤差最大值較改進(jìn)前分別降低了0.407 m、0.405 m，降低幅度為70.66%、70.31%。

改進(jìn)后的領(lǐng)航-虛擬跟隨法能使跟隨車橫向跟蹤誤差降低的原因是傳統(tǒng)的領(lǐng)航-虛擬跟隨法以領(lǐng)航車的位置通過幾何關(guān)系確定虛擬跟隨車位置后，直接以領(lǐng)航車的航向角等于虛擬跟隨車的航向角。航向角是車輛速度方向與橫軸的夾角，車輛速度方向?yàn)檐囕v運(yùn)動(dòng)軌跡的切線方向。當(dāng)虛擬跟隨車的位置確定即跟隨車行駛的參考路徑確定后，其參考航向角即已確定，此時(shí)強(qiáng)行令其等于領(lǐng)航車的航向角反而會產(chǎn)生模型的誤差，降低了橫向跟蹤的精度。改進(jìn)后的領(lǐng)航-虛擬跟隨法能夠較大地提高曲線工況下跟隨車橫向跟蹤精度。橫向誤差將影響到車輛對車道限界的遵守，跟隨車橫向跟蹤精度的提升有助于提高其運(yùn)行安全性。

4 結(jié)語

本文在領(lǐng)航-虛擬跟隨法的基礎(chǔ)上修改了虛擬跟隨車航向角的表達(dá)式，提出了基于車輛運(yùn)動(dòng)學(xué)模型的預(yù)測方法規(guī)劃跟隨車速度，分別通過誤差修正的Stanley方法和PD方法進(jìn)行橫向、縱向控制，通過Carsim/Simulink聯(lián)合仿真平臺對改進(jìn)前的領(lǐng)航-虛擬跟隨法和本文所提方法進(jìn)行了圓周和正弦曲線工況下的對比。結(jié)果顯示在圓周工況下跟隨車1號、2號橫向誤差均值改進(jìn)后較改進(jìn)前降低了83.22%、69.61%；橫向誤差最大值降低了74.92%、67.26%。在正弦曲線工況下跟隨車1號、2號橫向誤差均值改進(jìn)后較改進(jìn)前降低了73.80%、70.09%；橫向誤差最大值降低了70.66%、70.31%。相比較于傳統(tǒng)的領(lǐng)航-虛擬跟隨法，改進(jìn)的領(lǐng)航-虛擬跟隨法能夠較大地提高車輛隊(duì)列控制中跟隨車在曲線工況下的橫向跟蹤精度。