低速無人車編隊控制研究?

李 通 劉元盛 鈕文良 鐘啟學 郭笑笑

（北京聯合大學 北京 100101）

1 引言

在過去的數十年來，大家的注意力漸漸地從單一的受不完整約束的移動機器人的無人駕駛［1～2］轉移到多車輛的編隊無人駕駛控制。這是因為多車輛的協作編隊行駛可以大大提高公路的效率和安全性。

有很多的編隊控制方法，基于行為法［3］，領航跟隨法［4～9］，虛擬結構法［10］，基于路徑跟蹤的方法［11～12］。其中最直觀、最受關注的是領航跟隨法。這個方法有兩種控制器：l-l控制器和l-ψ控制器。在跟隨車輛的跟隨控制中還有橫向控制［13］、縱向控制［14］、橫向縱向綜合控制［15］三種。

在文獻［4～5］中分別使用車載傳感器信息和基于圖像處理的方式進行領航跟隨算法的實現，在以上兩種方法中都是使用傳感器信息來進行計算從而產生跟隨速度的輸入量。但是在使用圖像的方式進行領航跟隨編隊實現的時候，會發現它的可靠性會比較差，受光線影響非常大，盡管有文獻進行過改進，增加一個紅外成像探頭進行紅外圖像識別但是在熱源干擾強烈的環境下仍會誤檢。在文獻［9］中，在領航跟隨算法l-ψ控制器中，跟隨車輛與領航車輛所保持的距離和角度是固定不變的，且可以通過領航車輛的實時位姿進行解算，可以實時精確地計算出跟隨車輛應該處于的狀態信息。于是將這個狀態信息轉化成一個虛擬車輛，進而使實際跟隨車輛去跟隨這個虛擬車輛。但在該文獻中只是完成了編隊控制器的設計并沒有進行避障算法的設計。并且在前人所做的大量的研究上很少有在實際車輛上進行實驗的，大部分都是使用計算機仿真進行實驗，也有人使用很小尺寸的玩具車進行實驗。

在本文中將利用文獻［9］中所設計的跟蹤虛擬跟隨車輛的控制器進行編隊控制，同時采用簡化的VFH 算法進行避障。本文所采用的簡化的VFH 算法是在經典的VFH、VFH+、VFH*算法［16～18］的基礎上進行改進的方法，易于實現。同時采用基于行為的方法進行全局路徑規劃。在行駛路線上激光雷達沒有檢測到障礙物時就進行隊形保持行為；如果檢測到障礙物并且判斷出可以避開障礙物就進行避障行為。并在實際車輛進行編隊控制和避障實驗，使用無線路由器進行編隊車輛間的信息交互，實驗結果表明了本算法的有效性。

2 全局路徑規劃

本節將利用基于行為的方法進行全局路徑規劃?；谛袨榈木庩犓惴ㄊ菍⒕庩牽刂品纸鉃橐粋€個小的行為，如隊形保持、避障、駛向目標等。利用一種有效的行為協調機制進行行為選擇。

有限狀態機（如圖1 所示）集合S中包含了隊形保持、避障、駛向目標。在收集各個車輛的運行狀態反饋x1、xn后，進入F 中進行狀態解算并將結果發送給各車輛。進行決定是進行隊形保持還是避障狀態。

圖1 有限狀態機

用于車間通信的設備本文采用的是一個有效通信距離在100m 的無線路由器如圖2 所示，使用UDP協議進行數據傳輸［19～20］。

圖2 無線路由器

3 跟蹤控制器設計

本文采用領航-跟隨的編隊方法進行編隊控制。由領航車輛的位姿信息計算出跟隨車所對應的虛擬車輛的位姿信息（如圖3 所示），進而將編隊控制器轉換為對虛擬車輛的跟蹤。本節主要目的是設計出跟隨車輛的跟隨控制量跟隨車輛的線速度、跟隨車輛的角速度。

各車輛均滿足純滾動無摩擦的非完整約束，各車輛的運動學動態模型均為其中：x(t)，y(t)，θ(t)?R，表示車輛質心的坐標和方向；v(t)，ω(t)?R表示線速度和角速度；d 為質心到軸心的距離。

圖3 領航、跟隨車在全局坐標系下位姿關系

圖中的h 點表示傳感器所安裝的位置，因為考慮到實際車體較長，為了提高模型精度，必須考慮車體長度。h 點與質心的距離為l。圖2 中表示的是一對領航跟隨車的位姿關系。可得h點的位姿：

根據領航跟隨所需保持的距離σd和角度φd，可以得到虛擬車輛Rv的位姿信息：

為了設計出實際的跟隨車輛Rf中的h 點與所對應的虛擬車輛Rv上的質心坐標的跟蹤誤差e 方程組為零并且導數也為零，才能保證穩定。

對式（4）求導可得：

由于式（5）過于復雜，現在為了簡化計算，將以跟隨車輛Rf的質心為原點，以Rf的前進方向為z1軸的正方向建立坐標系。如圖4所示。

圖4 領航、跟隨車在以跟隨車坐標系下位姿關系

根據新的坐標系可以得到如下誤差方程：

為了使得式（6）的導數值最小選擇如下控制率：

其中k1、k2為正常量；vl、ωl為領航車輛的線速度和角速度。這個控制器的穩定性的證明是利用反步法和李雅普諾夫穩定性理論得到的，證明過程在文獻［9］有詳細的介紹，這里不再贅述。

4 編隊避障控制器設計

4.1 編隊避障控制

本節主要目標是設計出整車隊形的避障策略，以此來保證在編隊車輛正常行駛過程中不會與路徑上的障礙物發生碰撞。

在編隊行駛過程中，任意一臺車發現了障礙物就變換成縱列直線隊形，跟蹤領航車進行的軌跡避開障礙物。

4.2 單輛車避障的局部路徑規劃

上一節通過李雅普諾夫函數設計出了基于領航跟隨的隊形保持控制器。該控制器是使各個實際車輛分別跟蹤與之對應的相對應于領航車輛一定距離和角度的虛擬車輛來保持隊形，從而本身就具有一定的隊形內避碰的功能。

為了能夠更加穩定可靠地通過障礙物或者在無法通過的情況下停車，又在編隊隊形內車輛間距離過近并且有碰撞趨勢的情況下，利用本節方法可以實現緊急停車。本文參考比較成熟的避障算法VFH、VFH+、VFH*，采用一種簡化的避障方法。

由于在規劃好了全局路徑之后，障礙物的分布比較簡單且大致可以預測。

i車在行駛路線τi上發現了障礙物A（如圖5所示）此時目標方向決策如下。

圖5 路徑上出現障礙物A

1）活動窗口創建

本車所用激光雷達精度很高，誤差遠比聲吶傳感器低得多，所以在接收到雷達反射數據后可以很輕易地判斷出是否為障礙物。若發現障礙物就在柵格圖中對應位置的柵格元填上1，表示不可通行。因為是多線激光雷達，所以在不同層次的雷達光束上可以探測到障礙物后面的更高的障礙物，亦既在柵格圖中y軸方向的柵格位置。如圖6所示。

扇區具有任意的角分辨率α，為了使扇區數(n=360°/α)為整數也為了能使得扇區精度更高，此處的α取值為1。并且此處只考慮車的前向180°部分，所以此時實際扇區數(n=180°/α)。對于每個扇區k(k=0、1、2、3…)都可以很方便地計算出所對應角度ρ=kα。同理由于k是個整數值可得k=INT(βij/α)。其中βij為以車輛激光雷達為原點的坐標系中，在柵格中位置坐標為(xi，yj)的柵格元相對于坐標原點的角度值。

圖6 柵格化后的障礙物分布

2）障礙物與車輛描述

本文通過沿障礙物邊緣柵格區域膨脹一塊區域為rr+s=ds?rr（如圖7 所示），其中ds安全系數，rr為車身的寬度。

圖7 障礙物與膨脹區域

此時車輛就可以看成一個質點，用一個小柵格表示，圖中黑色數字1 部分就是雷達實際所探測到的障礙物，淺色數字1就是所膨脹部分。

3）行駛方向決策

在障礙物膨脹后的柵格圖中進行行駛方向決策，首先要進行可行駛區域規劃。在柵格圖中將各個無障礙區域標記出后，構建掩膜直方圖（圖8 所示），并且根據車輛的運動特性，將當前不可到達的區域在直方圖中置為1。

圖8 掩膜直方圖

圖9 可行駛區域決策

從掩膜直方圖中可以看出有三個可行駛區域，此時本文選擇距離目標點最近的那個可行駛區域的角平分線方向。

對于障礙物比較稀少連續的可行駛扇區超過了smax，本文采用smax=15。則在此時的決策方向為從距離目標點最近的一個扇區開始到smax之間的角平分線方向。

5 實驗結果與分析

5.1 實驗平臺

本實驗所采用的實驗平臺為北京聯合大學的低速無人智能車項目組的旋風巴士如圖10 所示。本車輛上裝備了導航設備、激光雷達、攝像機、工控機，在領航車還加裝了用于編隊車輛間的信息傳遞的無線路由器。本實驗所采用的無線路由器的有效通信距離在100m 內，所以在編隊隊形內能保證通信的穩定性。

圖10 “旋風巴士”無人駕駛智能車實驗平臺

5.2 實驗結果與分析

在本次驗證實驗中編隊隊形采用水平編隊。車輛間距σd取2.5m，保持角度φd取180°，實驗地點北京聯合大學校區內。

本次實驗中的線路上有一段直線行駛的路線，在線路中加入一個標準高度的錐桶作為障礙物，以此來判斷編隊車輛能否安全的繞開障礙物。

在實驗結果中能看出編隊隊形在避障上的動作安全有效。在障礙物前根據雷達數據以及簡化的VFH算法計算出有效的決策角度。圖11中進行方向決策，在成功地避開了障礙物之后，整個編隊能夠跟隨領航車輛回歸到預定路線。與圖11 對應的角度決策直方圖如圖12 中的虛線為預定的軌跡，中間的灰色圓圈所框出來的是障礙物，左邊第一條灰色實線為在障礙物膨脹后的第一個可行駛扇區，最右邊的是最大連續扇區，所以決策方向為這兩個扇區的角平分線。

圖11 可行駛方向決策圖

圖12 障礙物分布圖

實際隊形行駛軌跡如圖13 所示，圖中黑色圓圈內為障礙物，黃色曲線為領航車輛其余兩條白色曲線為跟隨車輛。在障礙物前后方向可以看出來編隊隊形在直線行駛的時候隊形保持的有效性。

圖13 編隊車輛的實際行駛軌跡

6 結語

本文研究了低速無人駕駛智能車的編隊與避障，在實車試驗中證明了算法的有效性。車輛之間能夠有效地保持設定的距離和角度，并且在遇到障礙物時可以及時有效的避開。

在完成編隊與避障的同時，本文也有很多需要改進提升的地方。在本文中只考慮了簡單的少量的車輛的編隊控制，對于大量的、高速的車輛的編隊控制還有很多要改進的地方，對于龐大的編隊群體來說一個穩定可靠的通信環境也是至關重要的，本文只是采用了一個無線路由器進行車輛間的通信，因此編隊規模和車輛間的距離都要受限于路由器的通信范圍，將來需要在這方面進行改進。