基于改進領航-虛擬跟隨法的車輛隊列控制

楊 強 袁希文 肖緋雄 羅佳祥,3

1西南交通大學牽引動力國家重點實驗室 成都 610031 2中車株洲電力機車研究所有限公司 株洲 412001 3西南交通大學電氣工程學院 成都 611756

0 引言

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

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

上述文獻的共同特點是在領航-跟隨l-φ隊列控制結構的基礎上引入虛擬跟隨車而變成領航-虛擬跟隨法，將隊列控制問題轉化為跟隨車對虛擬跟隨車的軌跡跟蹤控制，進而設計了不同的算法進行控制器的設計。然而這種使虛擬跟隨車參考航向角等價于領航車的航向角的方法，在曲線工況下存在模型誤差。

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

1 車輛隊列運動規劃

車輛隊列運動規劃是在領航車位姿、速度確定的條件下，為跟隨車橫向控制設計參考軌跡使跟隨車與領航車保持特定的隊形；為跟隨車縱向控制規劃參考速度使領航車與跟隨車的距離穩定在參考距離。本文通過改進的領航-虛擬跟隨法規劃跟隨車軌跡，基于車輛運動學模型預測方法規劃跟隨車速度。從使用領航車信息進行隊列運動規劃到隊列控制實現的過程如圖1所示。

圖1 車輛隊列控制原理框圖

1.1 改進的領航-虛擬跟隨法

領航-虛擬跟隨法即設置與領航者一定距離和角度的虛擬車輛作為虛擬跟隨車，當控制跟隨車與虛擬跟隨車的位置完全重合時便實現了車輛編隊。如圖2所示，可得虛擬跟隨車車輛位姿為

圖2 領航-虛擬跟隨法隊形結構

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

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

圖3 領航-虛擬跟隨法軌跡

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

因此，通過使虛擬跟隨車參考航向角等于2個時刻虛擬跟隨車位置的連線與橫軸的夾角以近似其位置切線與橫軸的夾角，給出改進的領航-虛擬跟隨法表達式為

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

1.2 縱向速度規劃

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

圖4 領航車、跟隨車運動學模型預測方法幾何分析

由阿克曼轉向模型有

式中：δl為領航車車輪轉角，rl為領航車轉彎半徑，a為領航車軸距。

經過時間t，車輛運動距離為

式中：sl為領航車位移（圖中虛線表示的一段圓?。?，vl為領航車速度。

由阿克曼轉向模型與平面幾何關系有

式中：lsl為領航車2時刻車輛后軸中心距離。

根據運動學模型，有

式中：θl為領航車運動變化角。

領航車t時刻后車輛后軸中心位置為

式中：xmlr、ymlr分別為領航車后軸中心當前時刻在全局坐標系下的橫坐標、縱坐標，xnlr、ynlr分別為領航車后軸中心未來時刻在全局坐標系下的橫坐標、縱坐標，φml為當前時刻領航車車身航向角。

領航車t時刻后車輛前軸中心位置為

式中：xnlf、ynlf分別為領航車前軸中心未來時刻在全局坐標系下的橫坐標、縱坐標，φnl為未來時刻領航車車身航向角。

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

2車前軸中心距離為

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

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

2 車輛隊列運動控制

跟隨車隊列運動控制器的設計包含轉向角橫向控制和速度縱向控制，2個方面的內容，轉向角橫向控制為當跟隨車的參考位姿由領航-虛擬跟隨法確定后，使用誤差修正的Stanley方法進行橫向控制使跟隨車處于參考軌跡上。速度縱向控制為當跟隨車參考速度由規劃得到后，使用標定策略對車輛驅/制動非線性環節擬合與PD控制跟隨車速度達到參考速度。

2.1 轉向角橫向控制

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

圖5 Stanley方法幾何分析

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

前輪轉向角為

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

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

因此，跟隨車橫向控制轉角為

2.2 速度縱向控制

車輛隊列控制問題中跟隨車縱向控制的過程是在對車輛驅/制動非線性環節擬合的基礎上，將由PD控制得到的車輛期望加速度轉化為油門/剎車信號控制車輛實際速度達到規劃得到的參考速度，以保持跟隨車與領航車的距離恒定。

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

圖7 驅/制動標定表

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

3 仿真實驗與分析

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

仿真的初始條件設置為：領航車輛行駛的軌跡分別為圓心位置（0 m，25 m）、半徑為25 m的圓周和由方程y=4sin(x/10)確定的正弦曲線。領航車初始位置為（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臺跟隨車前軸中心與領航車前軸中心參考距離都為5.6 m。

3.1 改進前、后領航-虛擬跟隨法仿真結果對比

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

圖8 圓周工況下車輛軌跡

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

圖10 圓周工況下車輪轉角

圖11 圓周工況下車輛速度

圖12 正弦曲線工況下車輪轉角

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

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

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

3.2 仿真結果分析

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

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

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

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

改進后的領航-虛擬跟隨法能使跟隨車橫向跟蹤誤差降低的原因是傳統的領航-虛擬跟隨法以領航車的位置通過幾何關系確定虛擬跟隨車位置后，直接以領航車的航向角等于虛擬跟隨車的航向角。航向角是車輛速度方向與橫軸的夾角，車輛速度方向為車輛運動軌跡的切線方向。當虛擬跟隨車的位置確定即跟隨車行駛的參考路徑確定后，其參考航向角即已確定，此時強行令其等于領航車的航向角反而會產生模型的誤差，降低了橫向跟蹤的精度。改進后的領航-虛擬跟隨法能夠較大地提高曲線工況下跟隨車橫向跟蹤精度。橫向誤差將影響到車輛對車道限界的遵守，跟隨車橫向跟蹤精度的提升有助于提高其運行安全性。

4 結語

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