基于微分同胚變換的智慧交通錐反步編隊控制

張佳樂，焦生杰*,2，王玥琳，易小剛

（1.長安大學 公路養護裝備國家工程研究中心，西安 710064，中國；2.江蘇集萃道路工程技術與裝備研究所有限公司，徐州 221004，中國）

交通錐作為道路養護作業中必不可少的工具之一，其在引導車流，保證正常道路養護作業方面有著重要作用。目前，市場上主要有2 種擺放方式[1-2]：一種是人工擺放交通錐，另一種是車輛收集放置交通錐。其中，人工擺放耗時耗力，增加了工人擺放過程中發生交通事故的風險?，F有的交通錐放置設備無法實現交通錐斜線擺放以及曲率大的擺放要求，尤其在回收交通錐的過程對駕駛員有一定的技術要求。為了解決上述難題，智慧交通錐逐漸走入人們的視線中，其由一個標準的交通錐模型和一臺Irobot Roomba 機器人組成，同時配備了各種傳感器去收集周圍環境的數據，可進一步解決當前交通錐有序擺放和回收問題。因此，針對交通錐編隊擺放和回收問題展開研究具有理論和應用價值。

編隊控制是智慧交通錐系統控制中的一個關鍵問題。多移動機器人編隊控制出現了一些方法，如：行為法[3-4]、虛擬結構方法[5-6]、人工勢場法[7-8]、領航跟隨方法[9-11]。在行為方法中，將幾個期望行為分配給每個機器人，得到的控制動作是所有可能行為的加權組合，但是無法保證系統的穩定性，難以用數學模型準確表達。在虛擬結構方法中，將整組機器人視為剛體，可以保證編隊形狀的準確性，但不適合在群體規模增加的情況。在人工勢場法中，利用勢函數的梯度進行控制設計，通過選擇一個合適的勢函數，可以達到期望的整體行為，但由于該控制器有可能出現局部極小值問題，不能保證全局漸近收斂到目標編隊。在領航跟隨法中，跟隨機器人需要跟隨(虛擬)領隊，同時保持領隊所需的相對位置和方向，由于其易于實現，已被廣泛研究以解決編隊問題。

近年來，多機器人編隊控制問題在自動駕駛，消防救援以及無人機編隊等領域得到了廣泛的研究，包括傳統比例- 微分- 積分(proportion integration differentiation，PID)[12]，神經網絡[13-14]、滑?？刂疲╯liding mode control，SMC)[15]、模型預測控制(model predictive control，MPC)[16-17]、反步法(backstepping technique)[18-23]等。

為提高自動駕駛車隊的縱向編隊控制器的魯棒性，文獻[12]提出了一種車輛執行器時延和車車通信時延因素的縱向控制方法。同時推導出了車隊縱向編隊PID控制器參數的邊界條件，并利用Lyapunov 穩定性理論，驗證了車隊保持隊列的穩定性。CHU Zhongchu 等人[13]提出了一種自適應模糊神經網絡控制方案，通過阻抗學習來協調具有未知動力學和時變約束的多個機器人。針對無人機飛行過程中存在的參數和外界擾動不確定性導致編隊系統不穩定問題，曹志斌等人[14]設計了一種多四旋翼分布式神經自適應動態協同編隊控制方法。采用學習維數低、實時性強的最小參數神經網絡觀測器，實現對四旋翼位置回路和角度回路未知非線性干擾的快速平滑學習與補償。CHWA Dongkyoung 等人[15]使用SMC 算法的編隊控制方法可以處理較大的初始跟蹤誤差，但其過程復雜且計算量大。張平等人[16]研究了自動駕駛汽車在高速公路上超車過程中的軌跡規劃和跟蹤控制，采用集中式模型預測控制器，實現了超車過程的軌跡跟蹤。針對多移動機器人編隊系統存在的干擾問題，LIU Andong 等人[17]提出了一種基于擴展狀態觀測器(extended state observer，ESO)的分布式模型預測控制(distributed model predictive control，DMPC)方法。

反步技術適用于非線性系統，成為常用的方法之一。該技術采用逆向設計思路，使得控制器設計更為直觀和便捷，減輕了復雜的數學運算和系統建模的負擔，從而進一步簡化了控制器的設計過程。

為提高編隊性能，文獻[18]設計了基于反饋線性化的反步控制器。針對多四旋翼無人機的編隊保持和防碰撞及機間通信問題，楊明月等人[19]設計了一種基于虛擬結構和人工勢場相結合的編隊規劃控制算法，同時利用反步法實現無人機對期望軌跡的跟蹤功能。針對四旋翼無人飛行器(unmanned aerial vehicle，UAV)模型參數不確定和外部干擾情況下的精確軌跡跟蹤問題，文獻[20]提出了一種自適應積分反步控制策略。同時，文獻[21]利用基于反步控制技術解決了欠壓自動潛航器三維軌跡跟蹤中與運動參數跳躍相關的問題。此外，為解決欠驅動自主潛水器(autonomous underwater vehicle，AUV)模型參數中的不確定性問題，文獻[22]提出了一種軌跡跟蹤算法。該算法結合了徑向基函數(radial basis function，RBF)神經網絡算法和反步控制方法，采用狀態預測策略，并通過仿真結果驗證了該方法的有效性和魯棒性。針對輪式移動機器人在運動學模型下軌跡跟蹤誤差較大的實際應用問題，文獻[23]采用反步法設計了控制器，實現了對移動機器人位置誤差和角度誤差的精確控制。眾所周知，基于反步法設計的控制器會因為初始誤差的原因導致不現實的速度跳變超過其電機的最大限制，從而影響編隊的控制性能。

為解決道路養護過程中交通錐擺放和回收的問題，本研究基于相對位置的編隊控制，即選擇一個或多個智慧錐作為領隊，其余智慧交通錐為跟隨者。針對智慧錐質心與中心(即智慧交通錐驅動輪軸線的中心)未在同一位置的情況下，構建智慧錐的誤差動態方程。將微分同胚思想首次引入到反步設計中，并設計了一種新的映射函數，有效解決控制輸入限制問題。借助MATLAB 和機器人操作系統(robot operating system，ROS)實驗平臺對該控制方法進行了數值仿真和實驗驗證，以驗證該算法有效性和實用性。

1 工況背景與問題描述

1.1 擺放與回收工況

本文的應用場景為四車道高速公路封閉外側車道的養護作業區域，如圖1 所示。智慧交通錐編隊工況由3 個階段組成：第1 階段，起始位置(應急車道)；第2階段，進入編隊擺放區；第3 階段，編隊回收工作(返回至應急車道)。其中，編隊擺放由1 個警告區、2 個緩沖區、1 個工作區和1 個終止區組成。

圖1 智慧交通錐初始位置和工況示意

根據《道路交通標志和標線》 (GB5768)的相關規定，擺放智慧交通錐的順序應從警告區開始，逐步向終止區域推進。在回收時，順序與擺放相反進行，其中，擺放的智慧交通錐之間的布設間距不宜大于10 m，警告區、上游過渡區和工作區的布設間距不宜大于4 m。在編隊擺放之前，工作人員首先會將智慧交通錐從卡車上取下并移動至初始位置(應急車道)，具體操作通過遙控完成；然后，根據養護作業區域的要求，預先設置智慧交通錐的擺放標準位置；最后，當智慧交通錐接收到移動指令，便開始運動，直至到達警告區的擺放目標停止，即形成警告區的斜線編隊。這里的警戒區是安全事故發生最頻繁的位置，在本研究中作為主要擺放目標，其他區域則使用普通交通錐進行擺放。

1.2 微分同胚變換

對于給定的2 個微分流形H和J，微分流形之間存在可逆映射W:H→J，其逆映射表示為W-1:J→H。當映射W和W-1都是連續可微的，則映射W就被稱為微分同胚。從其定義，可以看出微分同胚是將系統的狀態方程從一個坐標空間轉換為另一個坐標空間。

根據微分同胚理論，選在一系列Wi以滿足：

其中：t∈R為時間，x∈Rn為系統的位置，λ為常數，Wi是微分同胚Xi上的一個局部微分同胚。針對fi(x,t)，提出以下映射函數：

其中：qi是轉換前的變量。

將式（2）改寫為

可知，當qi∈(-∞, +∞)，fi∈(-λ, +λ)；即fi(x,t)滿足不等式約束。

1.3 智慧交通錐單機模型

如圖2 所示，假設智慧交通錐在平面上運動，全局坐標系固定到笛卡爾空間，局部坐標系固連到智慧交通錐平臺上。其中，Eoi表示第i個智慧交通錐的幾何中心(i=1, 2, 3)，Eci表示第i個智慧交通錐的質心坐標。

圖2 智慧交通錐平面示意圖

由于智慧交通錐的質心與幾何中心不重合，可以得到Eo和Eci的關系，如下：

其中： (xi,yi) 為智慧交通錐的質心Eci的慣性坐標系OXY下的坐標位置；θi為方位角；(xoi,yoi)為智慧交通錐的幾何中心在慣性坐標系OXY下的坐標位置；di表示Eoi與Eci之間的距離。

考慮智慧交通錐在純滾動無滑動條件下運動，則運動學約束(非完整約束)為

根據運動學約束式(5)，可以得到智慧交通錐的運動學模型為

其中：vi和ωi分別表示智慧交通錐的線速度和角速度。

1.4 智慧交通錐編隊模型

本文的智慧交通錐編隊模型，其結構如圖3 所示。

圖3 智慧交通錐編隊示意圖

在全局坐標系下的跟蹤誤差定義[24]為：

其中：edx'i和edy'i分別為x方向和y方向的目標距離誤差值。則本文的目標是對控制輸入vi和ωi進行設計，以滿足下面條件：

通過幾何投影變換，可以得到全局和局部坐標系下的跟蹤誤差關系[25-26]。

結合方程（3）的結論，對式（11）求導可得

根據上述推導過程，可得到智慧交通錐系統的動態誤差方程，

其中：vi-1和ωi-1分別為第i-1 個智慧交通錐的線速度和角速度。

2 基于微分同胚變換的反步法控制器設計

在本節中，由于在編隊轉彎時每個智慧交通錐的方位角不會一直保持一致，則在后面的控制器設計過程中忽略方位角的影響。將方程(14)改寫為：

考慮鎮定eyi，選擇角速度ωi為：

根據式(19)，選擇線速度為

可得

因此，系統是漸進穩定的?？傻玫奖疚牡目刂破魅缦拢?/p>

其中：K1和K2是大于零的常數，vi和ωi不受限制，會因初始誤差的原因超過其電機輸入最大限制。

為了解決控制輸入限制問題，結合電機本身的硬件要求，給出以下速度限制：

將變換(3)引入到控制輸入限制中，則等式(22)改寫為

若速度vi和wi∈ (-∞, +∞)，則u1i∈ (vmin,vmax)和u2i∈ (ωmin,ωmax)，符合實際要求。與直接對電機輸入進行硬約束相比，采用微分同胚思想的方法可實現對控制輸入的動態調整，使得系統響應更加平滑，避免了直接施加硬約束可能導致的過渡振蕩和不穩定現象，有助于進一步提高控制系統的穩定性和控制性能。

3 數值仿真與實驗

3.1 數值仿真

在本節中，通過MATLAB 數值仿真驗證所提方法的優越性?？紤]由3 個智慧錐所組成的編隊系統，其通信拓撲結構如圖4 所示。

圖4 控制器設計流程

從圖4 中可以看出：智慧錐2 跟隨智慧錐1 (領航者)，智慧錐3 跟隨智慧錐2 (領航者)，以此類推，第i個智慧錐僅需要知道第i-1個智慧錐的位置信息，因此，根據不同工況的需求，可將智慧交通錐的編隊擴展到更多數量的工況應用中?？紤]2 種目標工況：

工況1：智慧錐從應急車道出發形成斜線編隊并到達警告區，完成擺放任務。

工況2：在接收到回收命令時，智慧錐則需從擺放位置返回到緊急車道，結束編隊任務(即，斜線隊形變成直線隊形)。

工況1：讓智慧錐1 跟蹤一個擺放目標(5, 0) m。

工況2：在工況1 的基礎上，讓智慧錐1 跟蹤一個回收目標(10, 0) m。

其中：k1和k2是大于0 的常數。

初始位置分別設定如下：

工況1：x1= 0 m，y1= 0 m，θ1= 0°，x2= -1.1 m，y2= 0 m，θ2= 0°，x3= -1.9 m，y3= 0 m，θ3= 45°；

工況2：x1= 5 m，y1= 0 m，θ1= 0°，x2= 4 m，y2= -1 m，θ2= 0°，x3= 3 m，y3= -2 m，θ3= 0°。

給出智慧錐仿真參數，如表1 所示。圖5 為智慧錐擺放工況的軌跡曲線圖。由圖5 可知：3 個智慧錐在本文所設計的控制器(式(25))作用下，依次從應急車道出發，保持相對穩定的目標編隊到達擺放位置。

表1 智慧交通錐參數表

圖5 智慧錐擺放工況軌跡

2 種工況仿真結果如圖6-圖12 所示。圖6-圖7為智慧錐擺放工況的速度、角速度曲線。圖8 描述了3個智慧錐之間的距離隨時間變化的曲線。

圖6 智慧錐擺放工況線速度

圖7 智慧錐擺放工況角速度

圖8 智慧錐擺放工況距離

由圖7a 可知：由于初始位置誤差的原因，角速度發生了明顯的速度跳躍現象超過了其電機的最大值(ωi∈-0.5～0.5 rad/s)，這在實際應用中是不允許的。相比于傳統的反步法，圖7b 將微分同胚變換思想引入到編隊設計中，可以將智慧錐的線速度和角速度控制在給定范圍內，且變化平穩，這也體現了本文控制算法相比于傳統算法的一個優勢。由圖8 可知：3 個智慧錐在x方向和y方向上的距離均在150 s 左右收斂于目標距離，即形成斜線的隊形。

與工況1 一樣， 圖9-圖11 分別為工況2 智慧錐的軌跡、速度、距離。

圖9 智慧錐回收工況軌跡

從圖10 可以看出：智慧錐在接收到回收指令后，依次從擺放位置返回至應急車道，整個過程依次有序，互不相交。從圖11-圖12 的結果可以看出智慧錐從警告區返回到應急車道位置的同時各個智慧錐之間的在x方向上的距離均收斂于期望距離1 m 和2 m，在y方向上均收斂于10 cm 以內。同時線速度、角速度分別收斂于0.1 m/s、0.1 rad/s 以內。綜上所述,智慧錐系統在所設計的控制算法作用下，能夠實現本文的工況要求，進一步驗證了本文算法的有效性。

圖10 智慧錐回收工況速度

圖11 智慧錐回收工況距離

圖12 5 臺智慧錐擺放和回收工況軌跡

為了更好地體現算法在1.1 節中所述的典型場景的可拓展性，本文進一步補充了5 臺智慧錐的仿真結果，如圖12 所示。

從圖12 可知：這5 臺智慧錐的軌跡在任何時候都不會相交，并且在追蹤擺放和回收固定目標的同時，保持了穩定的編隊形態。實驗結果表明，所設計的控制器對于更多的智慧錐仍然可行，具有良好的可拓展性。

3.2 ROS 實驗與結果分析

采用3 臺智慧交通錐進行實驗。各智慧錐的初始參數與3.1 節相同。圖13 為機器人實驗平臺框架圖，包括移動機器人平臺(Irobot)、控制器(Raspberry Pi)、傳感器(Real-time kinematic(RTK) GPS)、電源和標準交通錐模型。上位機作為系統的監控層，通過WiFi 與樹莓派進行交互。

圖13 智慧錐實驗平臺

樹莓派通過USB(universal serial bus)串口與Irobot、RTK GPS(inertial measurement unit)傳感器進行通信，其中，采用了RTK GPS 差分傳感器來實時獲取每臺智慧交通錐的位置、航向和速度信息。同時，選擇了KSXT 作為RTK GPS 報文的數據格式。根據KSXT報文信息，可以讀取到智慧交通錐的經度、緯度信息以及方位角信息，然后根據經緯度信息，將智慧交通錐的位置轉換到OXY平面坐標系中。在該坐標系中，原點的位置可以根據實際需求進行指定，其中X軸的正方向指向正東，Y軸的正方向指向正北。其具體的實驗操作步驟如下：

1) 將樹莓派與PC 機連接在同一局域網內，建立多機通訊網絡。

2) 通過SSH 遠程登陸服務，在PC 機上遠程登陸樹莓派，并啟動各智慧交通錐。

3) 通過Launch 文件啟動參數優化節點。

4) 啟動RTK GPS 串口信息讀取節點，并通過Launch 文件啟動軌跡展示節點，將二者輸出的經緯度、航向、速度等信息轉換到OXY平面坐標系。

5) 啟動智慧交通錐編隊控制器。

6) 擺放(回收)作業完畢后，停止程序，保存智慧錐的軌跡信息。

本次實驗在機器人操作系統(ROS)環境進行中，并利用Rosbag Record 命令進行編隊數據記錄，實驗過程的控制參數與仿真參數保持一致。圖14 分別給出了0、10、30、70 s 時擺放和回收工況下智慧錐的實驗過程圖。將數據進行可視化，如圖15-圖19 所示。

圖14 智慧錐擺放工況實驗場景

圖15 智慧錐擺放和回收工況軌跡圖

圖15為智慧交通錐編隊擺放和回收二維軌跡。圖16為擺放工況下智慧交通錐之間的距離誤差曲線。圖17 為智慧錐速度曲線。與編隊擺放工況一樣，圖18、圖19分別為智慧交通錐的距離誤差曲線圖和速度曲線圖。

圖16 智慧錐擺放工況距離誤差

圖17 智慧錐擺放工況速度、角速度

圖18 智慧錐回收工況距離誤差

圖19 智慧錐回收工況速度

從圖16 可知：所有跟隨智慧錐的線速度和角速度均收斂于領航智慧錐的線速度和角速度。其中線速度和角速度均在電機所允許的范圍內，即vi∈ (-0.5 m/s,+ 0.5 m/s)，ωi∈(-0.5 rad/s, + 0.5 rad/s)。

從圖18 可知：由于外部環境的變化(例如：地面不平整、風速以及遮擋物等因素)，導致RTK GPS 傳感器出現測量誤差。這些誤差導致智慧錐2 和智慧錐3之間的距離誤差在40～60 s 之間突然增加。然而，在基于微分同胚變換的反步法控制器作用下，智慧錐能夠在60 s 左右克服誤差，使得彼此之間的距離誤差ei均收斂到10 cm 左右。該結果表明控制器具有一定適應外部環境的能力。

由圖19 可知：將基于微分同胚變換的反步控制算法運用在多智慧錐的回收控制中，速度均可以滿足電機硬件需求，并收斂到期望隊形返回至應急車道。

綜上分析可知，實物實驗結果與數值仿真結果一致，進一步驗證了本文算法在實際場景中的可行性。

4 結 論

為了解決交通錐擺放和回收問題，本文將反步法設計理念應用至智慧交通錐動態誤差方程中，闡述了該方法的設計原理與應用條件，并將單個智慧交通錐的軌跡跟蹤問題拓展到了3 個智慧交通錐的編隊控制問題中。根據系統的通信拓撲結構以及不同工況的需求，可將智慧交通錐的編隊擴展到更多數量的工況應用中，具有較好的應用前景。此外，基于微分同胚變化的編隊方法相比于傳統的反步法，可以將智慧錐的線速度和角速度控制在給定范圍內，且變化平穩。在所設計的控制算法作用下，整個智慧交通錐系統可保持目標編隊到達擺放和前進回收位置。因此，該方法可為交通錐的擺放與回收問題提供一種新的控制方法。

考慮到智慧錐應用環境的復雜多變性以及對編隊安全性能的高要求，在未來的研究中，如何在本文控制器的基礎上消除環境中不確定性對系統穩定性的影響將成為一個重要的研究方向。