邊有鋼，張?zhí)锾?，謝和平，3，秦洪懋，楊澤宇

（1.湖南大學機械與運載工程學院，汽車車身先進設計制造國家重點實驗室，長沙 410082；2.湖南大學無錫智能控制研究院，無錫 214115；3.徐州徐工礦業(yè)機械有限公司，徐州 210009）

前言

車輛編隊行駛是智能交通系統（intelligent transportation systems，ITS）的一項創(chuàng)新技術［1］，具有優(yōu)化交通效率、節(jié)省能源消耗以及提高乘坐安全性和舒適性等優(yōu)點［2-4］。車輛隊列包括縱向控制和橫向控制兩大系統，現有大量研究主要集中在隊列縱向控制，相比之下，在某些交通場景下，如變道、轉向中涉及的橫向控制很少被研究［5-7］。

隊列路徑跟蹤控制是車隊系統的一個關鍵模塊。由于跟隨車傳感定位方案的多樣化，隊列路徑跟蹤控制方案也呈現多樣化特征，在基于前車信息的過彎工況下車輛隊列路徑跟蹤控制場景中，現有的主流控制方案有3 類［8-10］：基于結構化道路標識定位的跟蹤控制、基于前車歷史軌跡的跟蹤控制和前車直接跟隨的跟蹤控制。

基于結構化道路標識的隊列路徑跟蹤控制可以通過檢測道路標線或預埋磁釘實現自車橫向定位，利用車道保持、自適應巡航等技術實現車隊橫縱向控制。文獻［11］中基于攝像頭的圖像處理算法進行車道檢測，基于檢測信息開發(fā)橫向控制器，進而實現帶有預瞄功能的橫向控制。但該方法在隊列車輛距離很近時無法實現車道標線的準確識別；另外，車道標線可能不清晰或者存在被泥土或雨雪覆蓋甚至不存在的情況（如交叉路口、農村道路），應用場景受到很大限制。文獻［12］中使用磁標記系統進行車道保持控制，但利用預埋磁釘的方法只能實現車輛下方位置檢測和定位，沒有引入車輛前方信息，相較于基于前視道路標線的方法，該方法控制效果稍差；雖然這種方法被證明是可靠和魯棒的，但由于應用道路需要嵌入磁釘裝置，因此不具備實用性，一般只在碼頭、倉庫等特定場景下應用。

基于前車歷史軌跡的隊列路徑跟蹤控制，跟隨車根據前車絕對定位信息和車車（vehicle-tovehicle，V2V［13］）通信技術，記錄前車行駛路點并對前車軌跡進行重建，跟隨車的跟蹤性能與前車軌跡重建精度密切相關，傳感器噪聲和車間短距離跟蹤往往會降低重建路徑的可靠性和準確性［14］。文獻［15］中通過V2V 通信獲得前車軌跡采樣點信息，采用最小二乘法擬合得到由圓弧和直線段組成的跟隨期望軌跡。文獻［16］中基于V2V 通信獲得前車實時路徑，采用三次均勻B 樣條曲線插值方法得到平滑的后車目標路徑。文獻［17］中基于前車速度、自車與前車的距離以及記錄的前車路徑，使用二次規(guī)劃方法生成一個曲率連續(xù)的參考軌跡，該方法忽略了道路環(huán)境結構，考慮了自車動力學和運動學約束，能夠確保規(guī)劃軌跡的可行性和安全性。文獻［18］中設計了一種以隊列穩(wěn)定性為約束的線性前饋加反饋的控制器以保證安全車間距，在此基礎上，基于車載通信模塊傳輸的前車歷史運動軌跡，提出一種軌跡估計方法，實現對前車軌跡重建，所提方法在直線道路或道路曲率恒定的工況下具有良好的跟蹤效果，但是很難應用于道路曲率變化的工況。此類方法能夠實現對前車的穩(wěn)定跟隨，但是需要依賴絕對精確的定位信息，因此難以應用于定位信號不穩(wěn)定或受遮擋的場景。

前車直接跟隨的車輛隊列路徑跟蹤控制無需絕對定位信息，也不需要安裝特定的道路定位設施，僅通過車載傳感器或車車通信的方式獲取前車狀態(tài)信息，并基于自車與前車的相對狀態(tài)信息直接控制車輛與前車橫縱向保持特定距離行駛。相比于上述兩種方案，該方案取消了諸多應用限制，能夠適應更一般的非結構化道路應用場景。文獻［19］中提出了一種考慮車輛動力學特性的橫向隊列控制系統，該系統包括同時轉向和順序轉向兩種橫向控制策略，實現了一般道路曲率場景下的隊列變速度穩(wěn)定行駛。但由于跟隨車直接跟隨前車而不是跟隨圓弧路徑，導致傳感器一旦感知到與前車航向不同時，就會立即控制跟隨車轉向，從而導致過早轉向，最終導致“抄近道”的現象，并且隨著車隊規(guī)模的擴大，車間距的增加，車隊整體過彎橫向跟隨誤差也會不斷積累。文獻［20］中在車車間虛構一種可變剛度的連接桿，連接桿每塊切片的柔韌性隨著道路曲率的變化而變化，所提方法一定程度上緩解了內切現象。文獻［21］中提出針對三輪車隊列的橫縱向系統控制方法，車隊過彎行駛時，通過延長跟隨車跟蹤點的方式緩解內切行為，實現了恒定道路曲率下的車隊小誤差跟蹤。文獻［22］中設計了一種帶有航向誤差觀測器的跟隨車擴展前視點的跟隨策略，實現道路變曲率下抗內切跟蹤控制的同時考慮了航向狀態(tài)存在噪聲或難以量測的問題，但該方法未考慮系統位置狀態(tài)存在噪聲的情況。實際應用環(huán)境復雜，工況惡劣，系統除了存在一定噪聲外可能還會存在航向角狀態(tài)不易量測的情況。因此，本文提出了一種復雜工況下抗擾抗內切的車輛隊列協同路徑跟蹤控制方法，主要貢獻如下：

（1）提出了一種車輛隊列抗內切控制策略。采用圓弧跟蹤路徑代替一般直線跟蹤路徑的方法設計隊列跟馳策略，并將該策略融入到控制器設計，緩解隊列跟隨車輛過彎轉向過早進而導致整體跟馳誤差過大的現象。

（2）該控制方法兼顧了系統位置狀態(tài)存在的噪聲和航向狀態(tài)存在的噪聲或不易量測的情況。相較于文獻［22］在隊列路徑跟蹤控制中不僅考慮了系統航向狀態(tài)存在噪聲或不易量測的情況，還考慮了系統位置狀態(tài)存在噪聲的情況，基于卡爾曼濾波構建位置狀態(tài)濾波器，提高了所提方法在復雜工況下的適應性，進一步擴展了應用場景。

（3）該控制方法僅依靠車載傳感器（如攝像頭、激光雷達等）或車車通信獲取相鄰車輛相對狀態(tài)偏差信息，實現隊列高性能路徑跟蹤控制，擺脫了對結構化道路和絕對定位信息的依賴，可以作為現有方法的一種補充。

1 車輛隊列過彎控制問題建模

1.1 場景描述

車輛隊列過彎行駛，如圖1 所示，隊列中共有N+1 輛車，第一輛車（即領航車）用0 表示，跟隨車依次用1 到N表示。每輛跟隨車僅通過車載傳感器或V2V 通信獲得前車信息及與前車的位置和航向偏差信息，基于這些信息，跟隨車按期望跟車間距跟隨前車行駛。

圖1 車輛隊列過彎行駛示意圖

1.2 車輛建模

考慮跟隨車i的狀態(tài)Xi=[xi yi θi vi]T，前車i-1 的狀態(tài)Xi-1=[xi-1yi-1θi-1vi-1]T，其中，ωi-1、vi-1分別為前車i-1 的橫擺角速度、縱向速度。令si=sinθi，ci=cosθi，可構建車輛預測模型：

取狀態(tài)量ξi=[xi yi ci si]T，將式（1）寫成狀態(tài)空間形式：

其中：

考慮噪聲的車輛真實模型可進一步表示為

式中wi，k、μi，k+1為系統噪聲，一般采用高斯白噪聲模擬。

2 抗擾濾波器與觀測器設計

為進一步提高控制方法的魯棒性和普適性，本節(jié)基于卡爾曼濾波（Kalman filtering，KF）設計針對含有擾動的位置狀態(tài)（xi，yi）的狀態(tài)濾波器；類比文獻［23］中航向觀測器設計方法，設計航向狀態(tài)觀測器，在確保隊列穩(wěn)定跟馳的同時減少傳感器的使用量，降低實際應用過程中的成本支出。

2.1 位置濾波器設計

設計卡爾曼濾波包括預測和校正兩個步驟。

（1）預測

建立系統遞推估計模型：

估計誤差協方差矩陣為

式中：Pi，k-1為k-1 時刻優(yōu)化的誤差估計協方差矩陣；Qi為過程噪聲協方差矩陣。

（2）校正

計算卡爾曼增益：

式中Ri為量測噪聲協方差矩陣。對量測狀態(tài)和估計狀態(tài)進行數據融合：

更新誤差協方差矩陣：

若給定初值ξi，k-1、Pi，k-1和Ri，k-1，則能夠對縱向位置xi、橫向位置yi進行遞推估計優(yōu)化。

2.2 航向觀測器設計

基于龍伯格觀測器原理設計航向狀態(tài)觀測器，對于一般系統的觀測器框架如圖2 所示。仍采用式（2）的預測模型，根據系統輸入和輸出來估計系統航向狀態(tài)，構建的觀測器見式（9）。

圖2 觀測器框架圖

由式（11）可知，矩陣(A-HC)決定了觀測誤差是否收斂以及如何收斂，對于一個確定系統式（2），矩陣A、C的值均已知，因此，H矩陣是誤差能否收斂的關鍵，通過合理的配置H矩陣的參數，使得(A-HC)的極點在虛軸的左半開平面，實現ei收斂到零值，即實現觀測量收斂到真值。為確保系統可解，首先需要驗證模型可觀性，對于本系統即驗證：

3 抗內切控制器設計與穩(wěn)定性分析

3.1 抗內切跟車策略設計

首先建立跟隨車期望位置擴展點和實際位置擴展點，然后建立車輛系統模型。為了緩解一般的前車直接跟隨式方法中跟隨車轉向過早進而導致嚴重過彎內切的問題，采用以圓弧路徑代替直線路徑的策略構建跟隨車期望跟蹤點，對跟隨車實際位置及期望位置分別向前擴展di的距離，di為相鄰兩車的期望位置間距，并且根據擴展后的期望點和車輛擴展點進行系統橫縱向誤差動力學建模，如圖3所示。

圖3 抗內切過彎策略圖

為了引入航向角偏差控制量，并沒有直接使用跟隨車期望位置和實際位置進行跟蹤誤差模型構建，而是分別將跟隨車期望位置和實際位置分別向前擴展了直線期望車間距di的距離，基于擴展后得到的跟隨車期望位置的擴展位置和實際位置的擴展位置便能構建既包含車輛位置偏差信息又包含車輛航向偏差信息的跟蹤誤差動力學模型。

前車轉向曲率可表示為

跟隨車期望位置后軸中心線與前車后軸中心線的夾角為

式中：在直線路段，dr，i是以直線路徑計算的相鄰車輛期望跟車間距；在曲線路段，dr，i是以圓弧曲線計算的相鄰車輛期望跟車間距。

對式（13）求導：

在直線路段和彎曲路段，車間后軸中心實際直線期望間距為

跟隨車i橫縱向期望位置pd，i=[xd，i yd，i]T可表示為

可以看到，跟隨車期望位置依賴于前車位置pi-1=[xi-1yi-1]T和航向角φi-1以及期望間距di。將跟隨車期望位置向前擴展di的期望跟車距離得到跟隨車期望位置擴展位置的橫縱坐標為

將式（17）代入式（18）中并運用積化和差：

同理，得到i車向前擴展di的期望跟車距離：

3.2 跟蹤控制誤差模型構建

根據圖3，分別沿橫縱向定義如下誤差動力學：

對式（21）求導并運用積化和差：

3.3 控制器設計

針對式（23），根據系統相鄰車輛準確的狀態(tài)偏差，采用反饋線性化的方法可將控制器設計為

將式（25）代入式（23）中，得到系統動力學誤差狀態(tài)空間方程：

3.4 系統穩(wěn)定性分析

取k1，i，k2，i>0，可保證系統式（25）是大范圍漸近穩(wěn)定的。可選取李雅普諾夫函數：

當選取k1，i>0且k2，i>0時，可以確保＜0，即系統在平衡點是全局漸近穩(wěn)定的。

4 仿真與實車驗證

本節(jié)采用曲率固定和曲率連續(xù)變化共存的正弦路徑對抗擾抗內切的隊列協同路徑跟蹤控制方法進行仿真和實車測試驗證。

4.1 數值仿真驗證

仿真路徑按式（28）變化，如圖4 所示。仿真中車隊由4 輛車組成，領航車橫向采用純跟蹤控制器自動控制，縱向用PID 控制器控制以5 m/s 速度巡航行駛，每輛車車長為4.3 m，停車間距為1.5m，每輛車初始位置分別為（21.9 m，0.5 m）、（14.6 m，0.5 m）、（7.3 m，0.5 m）、（0，0.5 m），初始航向均為0。測試場景包括：場景1，采用文獻［24］中狀態(tài)反饋控制器的前車直接跟隨控制，作為對照組；場景2，位置狀態(tài)存在噪聲的抗內切控制，在仿真試驗中采用高斯白噪聲進行模擬；場景3，采用卡爾曼濾波器的抗位置噪聲抗內切控制；場景4，航向角狀態(tài)存在噪聲的抗內切控制，在仿真試驗中采用高斯白噪聲進行模擬；場景5，采用卡爾曼濾波器和龍伯格觀測器的抗擾抗內切控制。在測試噪聲對系統控制效果影響時，采用嚴格的控制變量法進行，即當位置狀態(tài)存在噪聲時，使用準確的航向角狀態(tài)；當航向角狀態(tài)存在噪聲時，使用準確的位置狀態(tài)，這樣能夠準確測試所提位置狀態(tài)濾波器和航向角觀測器的可行性和有效性。控制周期均為20 ms。

圖4 仿真路徑

4.1.1 場景1仿真驗證

采用文獻［24］中的隊列路徑跟蹤方法，即跟隨車直接跟隨前車，不對跟隨點做任何拓展處理。車輛相關控制參數如表1所示，仿真結果如圖5所示。

表1 仿真用車隊控制器參數

圖5 場景1仿真

從圖5 的仿真結果可以看出，對于一般的前車直接跟隨式隊列路徑跟蹤控制，即使初始位置具有一定誤差，在直線路段仍能收斂到穩(wěn)定狀態(tài)，但是在彎道路段則會出現較大跟蹤誤差，因此需要提出能夠有效降低隊列過彎跟蹤誤差的方法。

4.1.2 場景2仿真驗證

實際應用中車輛狀態(tài)可能包含噪聲，本節(jié)采用邊界為［-0.02 m，0.02 m］的高斯白噪聲對位置狀態(tài)噪聲進行模擬，采用3.3 節(jié)中抗內切控制器進行仿真試驗，控制增益為ki，1=ki，2=0.65，仿真結果如圖6所示。

圖6 場景2仿真

從圖6 的仿真效果可以看出，采用抗內切的控制器后，能夠有效緩解車輛過彎內切現象，即使道路曲率不為零且時刻發(fā)生變化，隊列依然能夠保持良好跟蹤性能，橫向跟蹤誤差以及縱向跟蹤速度依然保持在平衡狀態(tài)附近。另外，由于位置狀態(tài)含有噪聲導致跟蹤控制結果存在較大波動，因此需要對位置狀態(tài)進行優(yōu)化。

4.1.3 場景3仿真驗證

采用2.1 節(jié)中的KF 濾波器對隊列中車輛含有位置噪聲的位置狀態(tài)進行優(yōu)化估計，仿真中濾波器參數如表2所示，仿真結果如圖7所示。

表2 仿真用濾波器參數

圖7 場景3仿真

通過對比圖6和圖7可以發(fā)現，針對存在噪聲的位置狀態(tài)進行KF 濾波，能夠較好地濾除噪聲，使得反饋用的狀態(tài)更加接近真實值，進而實現良好的跟蹤控制效果。

4.1.4 場景4仿真驗證

采用邊界為［-0.5°，0.5°］的高斯白噪聲對航向角狀態(tài)噪聲進行模擬并進行仿真試驗，得到如圖8所示仿真結果。通過對比圖7和圖8可以發(fā)現，路徑跟蹤控制結果存在較大波動，控制性能較差，因此，需要對系統航向角狀態(tài)噪聲進行處理。

圖8 場景4仿真

4.1.5 場景5仿真驗證

在場景4 的基礎上，針對車輛航向狀態(tài)存在噪聲或難以量測的情況，采用2.2 節(jié)的觀測器所觀測的航向角進行仿真試驗，觀測器參數分別為hi，1=10，hi，2=10，hi，3=10，hi，4=10，仿真結果如圖9所示。

圖9 場景5仿真

通過對比圖7 和圖9 可以發(fā)現，采用2.2 節(jié)中的觀測器所觀測的航向值代替包含噪聲的航向值進行反饋控制，能夠實現良好的控制性能，通過對比圖10 中的橫擺角可以發(fā)現，觀測器估計的橫擺角與系統反饋的橫擺角相當，說明觀測器實現了對航向角的準確估計，同時一定程度上降低了傳感器使用成本，更加符合實際應用需求。

4.2 實車測試驗證

基于2 輛AGV 小車進行實車測試試驗，見圖11，每輛小車主要由上層計算單元和下層執(zhí)行單元兩部分組成。上層計算單元為研華ARK-3530F工 控機，采用i7-7700 處理器，6 核12 線程，具有2.4 GHz 的處理速度，負責處理傳感器數據并運行隊列路徑跟蹤控制算法，得出速度和前輪轉角并發(fā)送至下層，相關算法運行在Ubuntu 18.04 系統中的ROS 架構下，使用C++編寫。下層執(zhí)行單元即為AGV 底盤，均采用阿克曼轉向，通過CAN 總線接收上層下發(fā)的速度和轉角控制指令并執(zhí)行以及向上層反饋底盤實時速度和轉角信息。基于車輛底盤反饋的車輛速度和轉角信息，結合車輛運動學特性積分得到車輛狀態(tài)并通過車間狀態(tài)作差的方式模擬僅依靠車載傳感器或V2V 通信獲得相鄰車輛相對狀態(tài)信息。實車試驗中，車車間通過路由器建立的局域網實現信息傳遞，通信周期為10 ms。結合測試場地、微縮智能小車尺寸及最大運行車速等因素限制，對仿真試驗中的跟蹤路徑進行了縮小和調整，調整后的路徑按照式（29）變化，如圖12 所示。實車試驗中領航車為虛擬車，橫向仍采用純跟蹤控制器自動控制，縱向用PID 控制器控制以1.2 m/s 速度巡航行駛，每輛車車長為1 m，停車間距為1.5 m，每輛車初始位置分別為（10 m，0）、（7.5 m，0）、（5 m，0），初始航向均為0，控制周期為20 ms。

圖11 隊列實車測試實物

圖12 實車試驗路徑

4.2.1 場景1實車試驗

車輛相關控制參數如表3 所示，實車測試結果如圖13所示。

表3 實車試驗用車輛控制器參數

圖13 場景1實車試驗

采用文獻［24］中前車直接跟隨的控制器進行實車試驗，并將試驗結果作為對照組?？梢园l(fā)現過彎工況下隊列產生明顯內切現象，導致隊列產生較大跟蹤誤差。

4.2.2 場景2實車試驗

采用第3.3 節(jié)中的隊列抗內切跟隨控制器進行仿真試驗，控制增益為ki，1=ki，2=0.65，仿真結果如圖14所示。

圖14 場景2實車試驗

通過對比圖13和圖14實車測試結果可以看出，采用抗內切的控制器能夠明顯改善隊列過彎內切現象，但是由于位置狀態(tài)包含噪聲，導致控制量抖動加大，跟蹤位置也有少許波動。

4.2.3 場景5實車試驗

采用第2 節(jié)的濾波器和觀測器進行實車試驗，濾波器和觀測器參數如表4 所示，仿真結果如圖15所示。

表4 實車試驗用濾波器和觀測器參數

圖15 場景4實車試驗

通過對比圖14和圖15仿真結果可以看出，在對位置狀態(tài)進行卡爾曼濾波以及對存在擾動或不可測航向狀態(tài)進行觀測器觀測后，與場景2 實車試驗結果相比，隊列最大跟蹤誤差相當，但是跟蹤誤差均方值得到顯著改善，同時，系統控制量也更加平順，說明卡爾曼濾波器在濾除位置噪聲中發(fā)揮了顯著作用。另外，通過對比圖16（a）和圖16（b）可以發(fā)現，采用觀測器估計的航向值與系統狀態(tài)反饋的航向值相當，說明了所提觀測器在實車試驗中也能實現準確的航向估計。通過實車試驗進一步說明了所提方法的可行性和有效性。

圖16 實車試驗橫擺角

5 結論

本文針對前車直接跟隨式車輛隊列路徑跟蹤控制場景中過彎內切現象嚴重、狀態(tài)存在擾動或難以量測的問題，首先，采用圓弧跟蹤路徑代替直線跟蹤路徑的策略，緩解了過彎內切現象；其次，設計卡爾曼濾波器，濾除了位置狀態(tài)的噪聲；之后，構建龍伯格觀測器，實現了航向角準確觀測；然后，設計了基于狀態(tài)反饋的協同控制器并根據李雅普諾夫定理推導了系統穩(wěn)定的充分條件，指導參數設計；最后，進行了數值仿真與實車試驗。結果表明，本文設計的車輛隊列跟蹤控制器能夠減緩隊列路徑跟蹤過程中內切現象，一定程度上克服位置狀態(tài)存在擾動以及航向狀態(tài)存在擾動或不易量測的問題。