多自主車輛隊列跟隨控制器設計

楊 祺， 劉士榮

（杭州電子科技大學自動化學院，杭州 310018）

隨著機動車保有量的日益增加，交通擁堵、交通事故及能源環境的矛盾進一步激化，作為解決此矛盾的重要手段之一，智能交通系統的研究日益迫切。自主車隊系統作為該系統的重要子系統，其目的是通過傳感、通信以及控制技術使多自主車輛在領頭車輛的帶領下以車隊的形式行駛，隊列則是自主車隊系統中最常見的隊形。自主車隊內車輛保持合理的行車間距行駛，不僅可以大幅度提高車輛密度，增加道路的車流量，還能夠減小風阻對車輛的影響，從而降低油耗[1]。自主車輛以隊列形式進行運輸也可在危險、重復程度高的場景中代替人類駕駛員，如在礦山開采、物流運輸等運輸場景中可實現24 h不間斷運輸。

多自主車輛隊列跟隨控制重在環境感知與控制器設計的研究，而控制器的設計往往依賴于周圍環境信息的獲取，因此能夠準確地獲取周圍的環境信息是自主車輛隊列控制的前提條件。在環境感知研究中，文獻[2-3]利用視覺信息中豐富的障礙物特征紋理信息從中識別出不同的障礙物，但該方法受光線與相機抖動的影響較大。Maral等[4]、Olmeda等[5]分別提出了用雙目相機和紅外相機來克服此缺陷，但檢測視野和檢測范圍較小。毫米波雷達在縱向方向檢測距離較長且精度較高[6]，但對目標橫向運動數據不精確，導致其感知范圍狹隘。激光雷達具有精度高、幾乎不受光照影響的優點，目前廣泛應用于自主車輛的環境感知領域。

在控制器設計方面，Marino等[7]采用嵌套PID控制方法，將一個PID控制器嵌套進另一個PID控制結構中，雖然比單個PID控制器提高了精度，但也增加了需要調節的參數數量。Wang等[8]利用多個模糊控制器對自主車輛的角速度進行控制，有效地減小了角度誤差。文獻[9]針對含擾動的多智能體系統，設計了基于雙不變集的事件觸發分布式預測控制策略。

在上述環境感知方面的研究中，以單一車載傳感器的研究為主，自主車輛的環境感知能力不可避免地會受到每一種傳感器缺點的制約。在上述控制器設計的研究中，主要針對通信正常或無通信的情況進行研究，缺少對通信回路在正常情況下突然中斷情況的討論。為解決這些問題，本文采用激光與視覺傳感器作為感知周圍環境信息的車載傳感器，分別采用激光測距和視覺傳感器提取環境信息并將兩種傳感器數據進行融合。針對自主車隊內車輛間是否有通信，分別設計了常規和應急兩種控制器，并在ROS平臺上進行了仿真，仿真結果表明所設計的自主車輛隊列控制器無論在通信正常還是通信中斷時均可以實現自主車輛隊列安全行駛。

1 基于虛擬結構的領航-跟隨策略

自主車輛編隊控制問題的研究來源于多機器人任務規劃及協作問題的研究，本文采用兩輪差分移動機器人作為研究對象，并采用基于虛擬結構的領航-跟隨法作為跟隨策略。

1.1 運動學模型

兩輪差分移動機器人擁有兩個標準驅動輪和一個無動力的小腳輪，配備激光、單目相機等傳感器設備，可用里程計計算出自身姿態位置。如圖1所示，機器人將自身作為局部坐標系的原點建立局部坐標系。每個機器人在全局坐標系下的位姿可表示為

其中： i 為機器人的編號； xi和 yi分別表示機器人在笛卡爾坐標系 x 軸和 y 軸方向上的位置； θi表示機器人當前前進方向與 x 軸正方向的夾角。第 i 個機器人在 x 、 y 軸方向上的線速度及角速度可表示為，和，因此有

其中： vi表示第 i 個機器人的線速度； ωi表示第 i 個機器人的角速度。

機器人線速度和角速度與兩個差分輪旋轉速度的關系可以表示為

其中： r 為差分驅動輪的半徑； d 為兩個驅動輪之間的距離。差分驅動輪旋轉速度與機器人線速度及角速度的關系為

聯合式（2）、式（3）可以得到典型的差分驅動機器人運動學模型：

圖 1 機器人坐標系示意圖Fig. 1 Schematic diagram of robot coordinate system

1.2 基于虛擬結構的領航-跟隨策略

領航-跟隨法主要分為l-l方式和l-φ方式[10]，本文采用l-φ領航-跟隨法作為編隊控制策略，即自主車輛將前車作為領航車輛，通過保持與前車之間的相對距離和相對角度使隊形達到穩定，并在l-φ控制策略基礎上引入了虛擬機器人的概念。自主車輛通過轉換矩陣求出虛擬機器人位姿，并調整自身的速度來逼近在剛體中相對應的虛擬節點，確保二者之間的位置和方向誤差無限趨近于零。

圖2示出了領航者機器人、虛擬機器人和跟隨機器人之間的關系。在保持既定隊形時，虛擬機器人的位姿將和領航者保持著一個固定的變換關系：

其中：lP 、vP 分別表示領航機器人和虛擬機器人的位姿；表示從領航機器人姿態映射到虛擬機器人姿態的旋轉矩陣；vPBORG表示虛擬機器人和領航機器人之間坐標偏移矢量。

圖 2 領航機器人，虛擬機器人和跟隨機器人的關系示意圖Fig. 2 Schematic diagram of leader robat, virtual robot and follower robot

2 基于激光和視覺傳感器的環境信息感知

多自主車輛隊列的行駛環境復雜多變，使得它必須具備感知周圍環境的能力，包括領航車輛的位姿、障礙物位置等。當通信正常時，可通過通信獲取部分信息；當通信中斷時，需要通過車載傳感器感知周圍環境。本文選用激光和視覺傳感器作為自主車輛的車載傳感器，通過信息融合的方式從激光和視覺數據中獲取控制器所需要的領航車輛相對位置信息。

2.1 基于激光傳感器的環境信息提取

選用激光傳感器作為主要車載傳感器之一，利用了激光傳感器實時性好、探測范圍廣、在夜間與在白天的效果相差無幾等優點。基于激光傳感器提取環境信息的步驟如下：

（1）對激光數據進行篩選。本文只考慮影響車輛行駛的障礙物，因此只保留高度與車身相似的障礙物信息。

（2）采用DBSCAN[11]算法對障礙物數據進行聚類。該算法是一種基于空間密度的數據聚類方法，允許使用索引結構進行密度估計，因此數據集可以拓展至大型數據庫[12]，適合處理數據量龐大的激光測距傳感器數據。

（3）求取各聚類點集的凸包，即求能夠包絡點集中所有點的最小凸多邊形。

（4）求各凸包的最小外接矩形。不同點集獲取的凸包結構不同，很難對所有點集進行統一分析。本文采用旋轉卡殼法[13]求凸包的最小外接矩形，用該矩形擬合障礙物的物理形狀，并將矩形的中心作為障礙物所在位置，記為 Cl(xl,yl) 。

2.2 基于視覺傳感器的環境信息提取

激光信息可以獲取周圍障礙物的物理特征及與自身的相對距離，但無法分辨出哪個障礙物是領航車輛。采用Camshift算法[14]通過視覺搜索、跟蹤領航車輛，并將搜索框中心作為搜索到前車的所在位置。當目標區域的尺度發生變化時，該算法能夠自適應調整窗口大小繼續跟蹤。算法步驟如下：

（1）在初始幀中選擇包含領航車輛的目標區域，對搜索窗口進行初始化；

（2）對初始幀中的目標區域進行顏色直方圖統計；

（3）若當前幀圖像不為空，計算它的顏色概率分布圖（即反向投影圖）；

（4）根據反向投影圖和搜索窗口，調用Meanshift算法，得到收斂窗口；Cc(xc,yc) ，作為前車的中心點；

（5）Camshift算法通過Meanshift迭代得到的收斂窗口所反映的跟蹤目標信息對下一幀目標搜索的窗口大小進行調整，并將該收斂窗口的中心記為

（6）在下一幀圖像中，用步驟（5）得到的收斂窗口作為搜索窗口，并跳轉到步驟（3）繼續執行。

2.3 視覺與激光傳感器的信息融合

自主車輛行駛時需要不斷感知周圍環境中的障礙物，用 o bj(l,θ) 表示障礙物信息， l 和 θ 分別表示自主車輛與障礙物之間的相對距離和角度， Cl(xl,yl) 為激光數據聚類結果的最小外接矩形中心點，將該點到自主車輛的距離作為障礙物與自主車輛之間的相對 距離 l 。

同樣，可求該點 Cl(xl,yl) 在自主車輛局部坐標系下 的角度，記作 θl。

自主車輛在跟隨領航車輛行駛時，需要通過車載傳感器獲取領航車輛的相對位置。通過式（7）和式（8）可以獲得周圍障礙物與自主車輛的相對位置，但此時自主車輛無法知道哪一個障礙物是領航車輛。

通過Camshift算法在當前幀中找到領航車輛，領航車輛的中心點坐標為 Cc(xc,yc) ，該點以像素為單位，需轉換成自主車輛局部坐標系中的相對角度信息。圖像平面坐標系與自主車輛局部坐標系之間的關系如圖3所示，其中 xroryr為自主車輛所在坐標系， yc是投影在圖像坐標系中領航車輛的中心點，wimage是目標中心點投影在圖像平面的寬度，即該圖像平面中邊緣到中心點沿著 y 坐標系方向的像素個數， θfov是相機的成像角度。由式（9）可求得領航車輛在當前幀中的坐標點轉換成自主車輛局部坐標系中的相對角度：

通過式（10）在 n 個障礙物中找到偏離角度與 θ最接近的障礙物的角度：

其中： θli為激光數據中第 i 個障礙物的相對角度信息； n 為當前環境中障礙物數目。偏離角度為 θobj的障礙物即為領航車輛。

圖 3 圖像平面坐標系與自主車輛局部坐標轉換關系Fig. 3 Transformation relationship between image plane coordinate and local coordinate system of autonomous vehicles

3 自主車輛跟隨控制器設計

3.1 基于李雅普諾夫穩定性的跟隨控制器設計

自主車輛可以通過自身搭載的GPS傳感器獲取自身所在位置的全局坐標信息，并通過慣性傳感器、磁羅盤等車載傳感器獲取自身朝向角度信息。當車隊成員之間通信正常時，跟隨車輛可通過通信獲得前車的位姿信息，并通過式（6）計算虛擬車輛位姿信息。將跟隨車輛與虛擬車輛的位姿信息分別用(xf,yf,θf) 和 ( xv,yv,θv) 表 示，建 立 如 圖4所 示 誤 差 模型，用 e 表示位姿誤差，通過幾何原理，可以得出以下關系：

圖 4 跟隨車輛與虛擬車輛的位姿關系Fig. 4 Posture relation between follower and virtual vehicle

將跟隨車輛和領航車輛的速度分解到 x 軸和 y軸方向可得

隊列保持穩定跟隨的條件為

式中： δ 為誤差常數； δP表示位置誤差常數； δo表示姿態角誤差常數，表示誤差在一個有限時間 T 內趨近 于誤差常數。對誤差模型 e 求導可得

式 中： vf和 vv分 別 表示跟隨 車 輛 和虛擬車 輛 的 線速度； ωf和 ωv分別表示跟隨車輛和虛擬車輛的角速度。選用的控制率為

其中： k1、 k2為正系數。為證明其穩定性，本文選取李雅普諾夫函數為

可 知 此 時 V ≥0 ，當 且 僅 當 xe=ye=0 ， θe=0 時，V =0 。對式（18）求導可得

將式（15）代入式（19）可得

將式（16）和式（17）代入式（20）得

可知 當且僅當 ， 時， ，故系統漸近穩定。

3.2 基于模糊控制的跟隨控制器設計

當車輛間無通信時，車輛仍可通過車載傳感器獲取自身位姿信息，但跟隨車輛無法獲得前車的位姿信息，無法計算朝向角誤差 θe，故無法使用3.1節設計的控制器。此時跟隨車輛將式（9）所得相對角度作為與前車的偏離角度，并結合式（10）在激光數據聚類結果中找到與前車匹配的障礙物信息，將式（7）所得各個障礙物的相對距離信息作為跟隨車輛與領航車輛之間的相對距離記為 l 。根據圖5所示前后車輛 位姿關系，可得到位姿誤差如下：

式中： le和 θe分別表示距離誤差和角度誤差； l0為跟隨車輛與領航車輛之間需要保持的相對距離。

圖 5 領航車輛與跟隨車輛的位姿關系Fig. 5 Posture relation between leader and follower

在此引入模糊控制器對自主車輛進行控制，將 le和 θe作為 模 糊 控 制 的 輸 入，將 跟 隨車輛的速度 ν 和角速度 ω 作為輸出。

輸入、輸出的模糊集劃分如表1和表2所示，其中輸入 le、 θe的論域分別取[0, 3 m]、 [ -π/2,π/2] ，輸出 ν 、 ω 的 論 域 分 別 取 [ 0m/s,3m/s] 、 [-0.2rad/s,0.2rad/s] 。

表 1 輸入變量模糊集劃分Table 1 Partition table of input variable fuzzy sets

表 2 輸出變量模糊集劃分Table 2 Partition table of output variable fuzzy sets

采用圖6所示隸屬度函數對兩個輸入變量進行模糊化。由于真實路況中實際誤差無法為0，因此將圖6（a）、6（b）兩個輸入模糊集中的ZE函數分別設為z形函數和梯形函數，表示系統在誤差允許范圍內保持穩定行駛，避免了由于較小的誤差導致自主車輛不停地調整位姿影響乘客舒適感。當誤差較大時，自主車輛需要快速地調整速度，圖6（a）中的VF和圖6（b）圖中的NL、PL所占比重需要快速增大，故將圖6（a）的VF函數和圖6（b）的NL選用s形隸屬度函數，將圖6（b）的PL選用z形隸屬度函數。

圖 6 輸入變量模糊隸屬函數Fig. 6 Fuzzy membership function of input variables

采用圖7所示隸屬度函數對兩個輸出變量進行模糊化。為了使輸出更平穩、連貫，采用高斯隸屬度函數作為兩個輸出變量的隸屬度函數。模糊控制的規則表往往由經驗豐富的專家給出，用來反映輸入到輸出的隸屬度關系。本文模糊控制的核心是當 le過大或過小時，模糊控制器可以輸出一個較大或較小的 ν ；當 θe過大或過小時，模糊控制器輸出一個較大或較小的 ω 來保證自主車輛在此控制器下能夠穩定跟隨在虛擬位姿的位置。采用2個輸入、2個輸出、6個模糊級別分別對 ν 和 ω 進行模糊控制，模糊規則形式如下：

圖 7 輸出變量模糊隸屬函數Fig. 7 Fuzzy membership functions of output variables

輸出變量 ν 的模糊規則形式

輸出變量 ω 的模糊規則形式

采用重心法使輸出結果去模糊化:

值得注意的是，在跟隨車輛與前車之間無通信的情況下，本文設計的控制器是短時間內使用的備用控制器，無法保證長時間的安全行駛。一旦周圍的其他車輛進入到跟隨車輛與前車之間，跟隨車輛將完全丟失前車信息。因此若在一定時間內未恢復通信，則跟隨車輛需離開道路，停靠在安全區域等待維修。

4 仿真實驗

為了驗證兩種控制器的有效性和穩定性，在ROS indigo版本和GAZEBO 7.0版本中采用3輛P3DX移動機器人作為仿真對象進行仿真實驗。選用的二維激光測距傳感器的掃描范圍為 - 180°～180°，角度分辨 率 為 0 .5°，選 用 的 相 機 分 辨 率 為 6 40×480 。式（16）、式（17）中控制器參數 k1=1.5 ， k2=0.4 。

實驗中，領航車輛的控制率由式（24）給出，模擬自主車隊變車道的過程。該過程中線速度、角速度均發生變化，是實際駕駛情況中較復雜的一種。3輛車的初始位置分別為（0，0）、（0，-2）、（0，-4），朝向角均為90°，即朝向y軸正方向。

圖8所示為兩種控制器自主車隊變車道的軌跡圖。可以看出，在領航車輛變道時，跟隨車輛均可以較好地跟隨。

圖9所示為兩種控制器3輛自主車輛的線速度變化曲線。可以看出，兩種控制器都能很好地使跟隨車輛的速度趨近于領航車輛，且基于李雅普諾夫法設計的控制器可以使速度變化更為平緩，但收斂時間要長于模糊控制器。

圖10所示為兩種控制器3輛自主車輛的角速度變化曲線。可以看出，兩種控制器都可以使角速度很好地收斂。

圖11示出了李雅普諾夫法控制器誤差模型中誤 差 xe和 模 糊 控 制 器 誤 差 模 型 中 de的 變 化 曲 線，圖12示出了李雅普諾夫法控制器誤差模型中誤差 ye和模糊控制器誤差模型中 θe的變化曲線，可以看出兩種控制器的誤差均能收斂到誤差范圍之內。

圖 8 兩種控制器的軌跡圖Fig. 8 Trajectory diagrams of two controllers

圖 9 兩種控制器線速度圖Fig. 9 Velocity diagrams of two controllers

圖 10 兩種控制器的角速度圖Fig. 10 Angular velocity diagrams of two controllers

xe de圖 11 李雅普諾夫法控制器誤差 （a）和模糊法控制器誤差 （b）xe deFig. 11 Error of Lyapunov controller (a)and error of the fuzzy controller(b)

ye θe圖 12 李雅普諾夫法控制器誤差 （a）和模糊法控制器誤差 （b）ye θeFig. 12 Error of Lyapunov controller (a) and Error of fuzzy controller (b)

表 3 各項評價指標Table 3 Table of evaluation indicators

5 結論與展望

采用激光測距和視覺傳感器作為感知周圍環境的車載傳感器，通過信息融合的方式獲取領航車輛位置信息，有效地彌補了單一傳感器存在的缺陷。分別提出了基于李雅普諾夫穩定性和模糊控制策略在通信正常和無通信兩種情況下的控制器設計，有效地解決了自主車隊成員間通信回路中斷的問題，提升了自主車隊系統對通信回路的魯棒性，并通過仿真實驗證明了本文設計的自主車輛隊列跟隨控制器的穩定性和有效性。

目前在多自主車輛隊列控制中，仍有諸多方面值得深入學習和研究，例如，在自主車隊列避撞方面，在行駛過程中，應充分考慮其他交通參與者對車隊帶來的影響，避免可能發生的碰撞；在自主車隊隊列重構方面，在自主車隊列行駛時，存在隊內成員需要離開車隊或是其他自主車輛需加入車隊，因此需要隊列優化重構策略對成員入隊/出隊進行控制。