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

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

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

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

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

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

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

反步技術(shù)適用于非線性系統(tǒng)，成為常用的方法之一。該技術(shù)采用逆向設(shè)計思路，使得控制器設(shè)計更為直觀和便捷，減輕了復(fù)雜的數(shù)學(xué)運算和系統(tǒng)建模的負(fù)擔(dān)，從而進(jìn)一步簡化了控制器的設(shè)計過程。

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

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

1 工況背景與問題描述

1.1 擺放與回收工況

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

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

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

1.2 微分同胚變換

對于給定的2 個微分流形H和J，微分流形之間存在可逆映射W:H→J，其逆映射表示為W-1:J→H。當(dāng)映射W和W-1都是連續(xù)可微的，則映射W就被稱為微分同胚。從其定義，可以看出微分同胚是將系統(tǒng)的狀態(tài)方程從一個坐標(biāo)空間轉(zhuǎn)換為另一個坐標(biāo)空間。

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

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

其中：qi是轉(zhuǎn)換前的變量。

將式（2）改寫為

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

1.3 智慧交通錐單機模型

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

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

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

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

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

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

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

1.4 智慧交通錐編隊模型

本文的智慧交通錐編隊模型，其結(jié)構(gòu)如圖3 所示。

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

在全局坐標(biāo)系下的跟蹤誤差定義[24]為：

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

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

結(jié)合方程（3）的結(jié)論，對式（11）求導(dǎo)可得

根據(jù)上述推導(dǎo)過程，可得到智慧交通錐系統(tǒng)的動態(tài)誤差方程，

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

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

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

考慮鎮(zhèn)定eyi，選擇角速度ωi為：

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

可得

因此，系統(tǒng)是漸進(jìn)穩(wěn)定的。可得到本文的控制器如下：

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

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

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

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

3 數(shù)值仿真與實驗

3.1 數(shù)值仿真

在本節(jié)中，通過MATLAB 數(shù)值仿真驗證所提方法的優(yōu)越性。考慮由3 個智慧錐所組成的編隊系統(tǒng)，其通信拓?fù)浣Y(jié)構(gòu)如圖4 所示。

圖4 控制器設(shè)計流程

從圖4 中可以看出：智慧錐2 跟隨智慧錐1 (領(lǐng)航者)，智慧錐3 跟隨智慧錐2 (領(lǐng)航者)，以此類推，第i個智慧錐僅需要知道第i-1個智慧錐的位置信息，因此，根據(jù)不同工況的需求，可將智慧交通錐的編隊擴(kuò)展到更多數(shù)量的工況應(yīng)用中。考慮2 種目標(biāo)工況：

工況1：智慧錐從應(yīng)急車道出發(fā)形成斜線編隊并到達(dá)警告區(qū)，完成擺放任務(wù)。

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

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

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

其中：k1和k2是大于0 的常數(shù)。

初始位置分別設(shè)定如下：

工況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°。

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

表1 智慧交通錐參數(shù)表

圖5 智慧錐擺放工況軌跡

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

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

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

圖8 智慧錐擺放工況距離

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

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

圖9 智慧錐回收工況軌跡

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

圖10 智慧錐回收工況速度

圖11 智慧錐回收工況距離

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

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

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

3.2 ROS 實驗與結(jié)果分析

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

圖13 智慧錐實驗平臺

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

1) 將樹莓派與PC 機連接在同一局域網(wǎng)內(nèi)，建立多機通訊網(wǎng)絡(luò)。

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

3) 通過Launch 文件啟動參數(shù)優(yōu)化節(jié)點。

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

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

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

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

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

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

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

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

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

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

圖19 智慧錐回收工況速度

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

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

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

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

4 結(jié) 論

為了解決交通錐擺放和回收問題，本文將反步法設(shè)計理念應(yīng)用至智慧交通錐動態(tài)誤差方程中，闡述了該方法的設(shè)計原理與應(yīng)用條件，并將單個智慧交通錐的軌跡跟蹤問題拓展到了3 個智慧交通錐的編隊控制問題中。根據(jù)系統(tǒng)的通信拓?fù)浣Y(jié)構(gòu)以及不同工況的需求，可將智慧交通錐的編隊擴(kuò)展到更多數(shù)量的工況應(yīng)用中，具有較好的應(yīng)用前景。此外，基于微分同胚變化的編隊方法相比于傳統(tǒng)的反步法，可以將智慧錐的線速度和角速度控制在給定范圍內(nèi)，且變化平穩(wěn)。在所設(shè)計的控制算法作用下，整個智慧交通錐系統(tǒng)可保持目標(biāo)編隊到達(dá)擺放和前進(jìn)回收位置。因此，該方法可為交通錐的擺放與回收問題提供一種新的控制方法。

考慮到智慧錐應(yīng)用環(huán)境的復(fù)雜多變性以及對編隊安全性能的高要求，在未來的研究中，如何在本文控制器的基礎(chǔ)上消除環(huán)境中不確定性對系統(tǒng)穩(wěn)定性的影響將成為一個重要的研究方向。