具有時空約束的無人車集群構(gòu)型變換方法

蘇致遠(yuǎn)，李永樂，徐友春，章永進(jìn)

(陸軍軍事交通學(xué)院，天津300161)

0 引言

無人車輛集群是具備不同功能的無人車聚集執(zhí)行統(tǒng)一任務(wù)的重要形式，通過編成特定的隊形，實現(xiàn)進(jìn)攻、防御、監(jiān)視等多樣化任務(wù).在任務(wù)執(zhí)行過程中，車輛集群需要根據(jù)任務(wù)需求在規(guī)定時間或空間內(nèi)進(jìn)行構(gòu)型變換，特定約束下的構(gòu)型變換方法是集群的一個重點研究方向.

現(xiàn)有集群控制主要采用圖論法、領(lǐng)航跟隨法、基于行為法、勢場法和虛擬結(jié)構(gòu)法等[1-2].圖論法可以方便描述編隊成員之間的拓?fù)潢P(guān)系，判定編隊控制的收斂性；領(lǐng)航跟隨法是最常見的集群控制方法，跟隨者根據(jù)領(lǐng)航者的行為做出響應(yīng)并滿足一致性；利用基于行為法，每個智能體都可以根據(jù)環(huán)境及反饋實現(xiàn)基本的行為控制；虛擬結(jié)構(gòu)法根據(jù)事先定義的虛擬結(jié)構(gòu)為每個智能體分配目標(biāo)，實現(xiàn)隊形保持；人工勢場法通過構(gòu)造勢場函數(shù)實現(xiàn)隊形保持與障礙規(guī)避，但可能陷入局部最小的情況.以上方法描述了集群組織的基本框架，針對具體的任務(wù)，尤其是狀態(tài)切換任務(wù)，還需要單獨設(shè)計切換過程.

在集群構(gòu)型變換方面，大部分研究為無人機(jī)集群的隊形變換[3-5]，廣泛采用分層進(jìn)化路徑規(guī)劃方法、最優(yōu)化與生物進(jìn)化相結(jié)合的方法[6-7].與無人機(jī)隊形變換不同，車輛具有非完整性約束，在碰撞規(guī)避、路徑規(guī)劃等方面更加復(fù)雜.本文針對無人車集群的構(gòu)型變換問題，采用圖論[8]方法描述構(gòu)型關(guān)聯(lián)和構(gòu)型變換關(guān)系，以3輛車最小變換單元為研究對象，采用基于數(shù)值優(yōu)化的路徑規(guī)劃和基于曲線插值的速度規(guī)劃方法設(shè)計跟隨車的運(yùn)動，并通過仿真和實車實驗證明所提方法的有效性.

1 構(gòu)型描述與變換建模

1.1 編隊組織與構(gòu)型變換方法

本文無人車集群構(gòu)建了車載自組織網(wǎng)絡(luò)，在自組網(wǎng)通信范圍內(nèi)，車輛之間可以雙向通信，網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)可以表示為G=(V,E,R).其中：V為網(wǎng)絡(luò)中節(jié)點i的集合，V={0 ,1,2,…,N-1}，節(jié)點數(shù)量為N，按照 0～N-1編號；E表示邊eij的集合，E={eij=(i,j):i,j∈V} ，邊eij連接節(jié)點i和節(jié)點j；R表示智能體間的位置關(guān)系，R={rij=(θ,d):i,j∈V} ，rij表示節(jié)點j相對于節(jié)點i的位置關(guān)系，用極坐標(biāo)的形式表示，θ為相對角度，d為相對距離.

考慮N輛車一路縱向編隊(從前向后編號為0,1,2,…,N-1)，跟隨車的跟隨目標(biāo)為前車和引導(dǎo)車[9]，跟隨車通過車間通信獲取前車和引導(dǎo)車的信息，通過傳感器檢測前向車輛信息；引導(dǎo)車具有車隊監(jiān)控和管理能力，接收車隊所有車輛的信息.編隊信息流拓?fù)浣Y(jié)構(gòu)如圖1所示，圖中水平直線箭頭表示車輛傳感器感知，曲線箭頭表示車間通信信息傳輸.

圖1 車隊信息流拓?fù)浣Y(jié)構(gòu)Fig.1 Information flow topology of platoon

根據(jù)以上論述，車輛跟隨關(guān)系用鄰接矩陣方式描述為

對于大規(guī)模構(gòu)型變換場景，為使整個變換過程可控且減少車輛間運(yùn)動沖突，提出一種分組變換方法，如圖2所示.將每個組視為一個整體，先以組為單位執(zhí)行全局的隊形變換，然后進(jìn)行組內(nèi)隊形變換，或先進(jìn)行局部再進(jìn)行全局隊形變換.在隊形變換中，3輛車可作為最基本的變換單元，故將3輛車編隊的隊形變換作為基本問題，多于3輛車的編隊可以仿照此方法以3輛車為1組，分步進(jìn)行隊形變換.車輛集群的基本隊形有：一路縱隊、前三角隊形、橫隊等，其中縱隊是長距離行進(jìn)的主要隊形，前三角隊形是多種隊形變換的基本隊形，故以縱隊到前三角隊形的變換為基本構(gòu)型變換動作，其他變換以此為依據(jù)實施.

圖2 大規(guī)模構(gòu)型變換Fig.2 Large-scale formation reconfiguration

1.2 環(huán)境約束

在編隊運(yùn)行中，有兩種隊形變換場景：

(1)不受環(huán)境或空間約束，車隊在收到隊形變換指令后立即進(jìn)行隊形變換.

(2)具有空間約束，所有車輛必須在經(jīng)過必經(jīng)點后開始或到達(dá)目標(biāo)點前完成隊形變換.

如圖3所示，兩種場景的區(qū)別是：場景1中，兩輛跟隨車可以同時進(jìn)行隊形變換動作；場景2中，跟隨車必須經(jīng)過障礙(必經(jīng)點)才能進(jìn)行隊形變換；場景2在通過障礙后可以轉(zhuǎn)化為場景1，也可以按照通過障礙的先后順序依次進(jìn)行隊形變換.因此，本文基于第2種普適場景，研究最優(yōu)的隊形變換策略.

圖3 隊形變換場景Fig.3 Environment of formation reconfiguration

2 構(gòu)型變換

通過分析構(gòu)型變換場景和變換過程，將大規(guī)模集群的構(gòu)型變換簡化為由3輛車組成基本單元的隊形變換過程.在變換中需要解決兩方面的問題：一是跟隨車須在約束時間內(nèi)通過約束空間到達(dá)目標(biāo)位置，二是在構(gòu)型變換中無車輛碰撞并達(dá)到目標(biāo)狀態(tài).針對以上問題，分別從路徑規(guī)劃和速度規(guī)劃角度進(jìn)行設(shè)計.

2.1 路徑規(guī)劃

根據(jù)構(gòu)型變換的任務(wù)特點，進(jìn)行局部路徑規(guī)劃時已知車輛的初始位姿信息及目標(biāo)位姿信息，需要生成一條從起點到終點的平滑路徑，該過程可以將車輛抽象為質(zhì)點，考慮其運(yùn)動學(xué)約束，并滿足無碰撞條件.

車輛狀態(tài)包含車輛位置(x,y)、航向θ和速度v，控制量為加速度αx和方向盤轉(zhuǎn)角，方向盤轉(zhuǎn)角決定行駛曲率ρ，故將行駛曲率作為直接輸入u.在空間約束條件下，為消除構(gòu)型變換中速度變化的影響，將式(1)轉(zhuǎn)換為與路線長度s相關(guān)的函數(shù)[10]，任意變量ξ對時間t的導(dǎo)數(shù)可以按式(2)轉(zhuǎn)化成與行駛距離s和速度v的關(guān)系式.

根據(jù)以上轉(zhuǎn)換關(guān)系，可得到x,y,θ,ρ關(guān)于s的表達(dá)式為

式中：0≤s≤sf，sf為軌跡曲線長度.

以上轉(zhuǎn)化具有降維效果，目標(biāo)軌跡由ρ(s)唯一確定.由于模型的非線性約束，采用多項式方程對曲率函數(shù)進(jìn)行參數(shù)化，即

式中：τ為多項式階次；ρ0為軌跡曲線起點的曲率；k1,k2,…,kτ為多項式系數(shù).車輛在曲線上任一點的姿態(tài)為

參數(shù)化后，可以用P(k1,k2,…,kτ,s)表示控制空間，通過求解P就可以得到滿足要求的軌跡.在隊形變換過程中，應(yīng)該滿足車輛的運(yùn)動學(xué)約束和安全性約束，行駛曲線曲率應(yīng)小于最大曲率，在不同速度下車輛的橫擺角速度應(yīng)不大于臨界值，即車輛的行駛曲線曲率ρ及其一階導(dǎo)數(shù)應(yīng)滿足

由車輛初始狀態(tài)X0(x0,y0,θ0,ρ0)和目標(biāo)狀態(tài)X1(x1,y1,θ1,ρ1)決定的曲線軌跡不唯一，根據(jù)未知變量的數(shù)量，定義曲率方程為關(guān)于曲線長度的三次多項式方程，即

式(6)可保證方程具有唯一解，但解析法很難求得精確解，故用迭代法求近似解.當(dāng)曲率方程各點處的值滿足曲率及其導(dǎo)數(shù)約束時，該結(jié)果滿足規(guī)劃要求；若不滿足曲率及其導(dǎo)數(shù)約束，則需增大初始點和目標(biāo)點的變換距離，從而使構(gòu)型變換路線更加平滑.

2.2 速度規(guī)劃

多車構(gòu)型變換完成后，如果目標(biāo)位置車輛速度不一致，容易誘發(fā)車輛位置震蕩，構(gòu)型不穩(wěn)定，因此在目標(biāo)位置附近跟隨車應(yīng)具有相同速度和加速度.此外，速度規(guī)劃應(yīng)考慮編隊車輛間的速度匹配，避免發(fā)生碰撞.

編隊內(nèi)跟隨車的位置由引導(dǎo)車決定，在空間或時間約束下，跟隨車速度可表示成路徑和時間的函數(shù)，這里的時間指從速度規(guī)劃開始至到達(dá)指定狀態(tài)的時間.通常方法是建立時間和里程的直方圖，通過限制起點和終點的約束條件，求解一個五次多項式為速度規(guī)劃的目標(biāo).設(shè)初始條件為Y0=(s0,v0,a0)T，目標(biāo)狀態(tài)為Y1=(s1,v1,a1)T，其中，s0、v0、a0分別為初始里程、速度、加速度，s1、v1、a1分別為目標(biāo)里程、速度、加速度.由以上條件建立里程和時間函數(shù)為

由此得到里程、速度和加速度的函數(shù)關(guān)系為

初始時間t=0時，設(shè)初始條件為Y0=(0,v0,0)T，到時間t=t1時，目標(biāo)狀態(tài)為

將以上初始條件和目標(biāo)條件帶入式(8)，得到函數(shù)系數(shù)為

根據(jù)以上關(guān)系，可以求解任意滿足初始條件和目標(biāo)條件的函數(shù).速度規(guī)劃還要具有無碰撞特性，無碰撞的曲線才是可行的速度規(guī)劃曲線，若存在碰撞或較大碰撞可能性，則需要重新規(guī)劃速度曲線.

2.3 碰撞檢測與速度重規(guī)劃

常用的二維碰撞檢測算法有軸對稱包圍盒法、圓形碰撞法、光線投射法、分離軸定理算法、地圖格子劃分法和像素檢測法等.圓形碰撞法具有應(yīng)用簡單、運(yùn)算速度快的特點，應(yīng)用廣泛.根據(jù)構(gòu)型變換中車輛的運(yùn)行特點，碰撞僅可能發(fā)生在后車追趕前車，且存在軌跡交叉或重合的情況下，碰撞檢測可用圖4(a)所示的圓形碰撞模型描述，考慮構(gòu)型變換過程中需要保持安全車間距，在前后包絡(luò)圓的基礎(chǔ)上向外擴(kuò)展一定的安全距離，得到新的碰撞檢測模型，如圖4(b)所示.在構(gòu)型變換后車追趕前車時，若前車后方包絡(luò)圓與后車前部包絡(luò)圓相交，即碰撞檢測距離(從后車前軸中心到前車后軸中心的距離)小于安全距離2R2，則認(rèn)為變換過程危險.

根據(jù)以上路徑規(guī)劃和速度規(guī)劃方法，結(jié)合碰撞檢測的安全邊界設(shè)置，可以得出路徑規(guī)劃和速度規(guī)劃的邊界條件，即在相鄰車輛接近過程中，當(dāng)其縱向位置偏差小于車長時，其碰撞檢測距離Dc2大于安全半徑R2時，車輛間不會發(fā)生碰撞.因此，完成路徑規(guī)劃后，速度規(guī)劃應(yīng)滿足：當(dāng)兩車接近時，車輛的碰撞檢測距離不小于2R2.當(dāng)速度規(guī)劃結(jié)果不滿足此要求時，需推遲后車開始追趕的時間，重新計算滿足要求的速度規(guī)劃方案.

圖4 碰撞檢測模型Fig.4 Collision detection model

3 仿真與實車實驗

3.1 數(shù)值仿真

按照以上路徑規(guī)劃和速度規(guī)劃方法，進(jìn)行算法仿真，比較兩種路徑和速度規(guī)劃方案.方案1是兩輛跟隨車分別在通過障礙之后，進(jìn)行隊形變換的路徑和速度規(guī)劃；方案2路徑規(guī)劃同方案1，但跟隨車2在跟隨車1通過必經(jīng)點(障礙物)后開始速度規(guī)劃.相比方案1，方案2能夠以較短的時間追趕上跟隨車1.

假設(shè)車輛長度L=5 m，寬度W=1.6 m，跟車距離為10 m，引導(dǎo)車速度5 m/s，由一路隊形變?yōu)榍叭顷犘我笤?0 m范圍內(nèi)完成，橫向偏移量為2 m，安全半徑R2設(shè)置為車寬大小.方案1的隊形變換仿真結(jié)果如圖5所示，在縱向距離y=0時開始進(jìn)行構(gòu)型變換，當(dāng)縱向距離達(dá)到40 m時，構(gòu)型變換完成.

圖5 構(gòu)型變換結(jié)果Fig.5 Formation reconfiguration result

圖6 跟隨車距離變化Fig.6 Curves of distance between two following vehicles

圖6顯示了跟隨車在構(gòu)型變換期間的車間距和碰撞檢測距離.碰撞檢測距離指示跟隨車2的前軸中心至跟隨車1的后軸中心的距離，其最小值出現(xiàn)在跟隨車1完成構(gòu)型變換，跟隨車2正在變換的狀態(tài)下，最小值大于碰撞安全距離，故不存在碰撞風(fēng)險.圖7顯示了構(gòu)型變換過程中跟隨車的速度增量：跟隨車1速度波動較小，并與引導(dǎo)車保持穩(wěn)定的縱向距離；跟隨車2在設(shè)定的追趕時間內(nèi)最大速度增量約為3.5 m/s，且速度變化平穩(wěn).

圖7 跟隨車速度增量Fig.7 Curves of speed increment of two following vehicles

方案2的一組仿真結(jié)果如圖8和圖9所示.

圖8 跟隨車距離變化Fig.8 Curves of distance between two following vehicles

圖9 跟隨車速度增量Fig.9 Curves of speed increment of two following vehicles

與方案1相比，方案2中跟隨車2在較早時間點開始加速，造成最小碰撞距離出現(xiàn)時間早于方案1，碰撞檢測距離最小值小于方案1，最大速度增量小于方案1.方案2的最小碰撞檢測距離為3.158 m，小于安全距離3.2 m，雖然跟隨車1已完成構(gòu)型變換，但仍存在碰撞風(fēng)險，故優(yōu)先采用方案1.

3.2 實車實驗

仿真實驗優(yōu)選了更安全的構(gòu)型變換方案，本節(jié)再對較優(yōu)方案進(jìn)行實車驗證，進(jìn)一步證明其可行性.實驗平臺為3輛長城H9越野車，車輛搭載激光雷達(dá)、慣性導(dǎo)航系統(tǒng)及車間通信系統(tǒng).激光雷達(dá)能夠檢測車輛的位置和運(yùn)動信息，慣性導(dǎo)航系統(tǒng)測量本車位姿信息，車間通信系統(tǒng)能夠在車隊內(nèi)轉(zhuǎn)發(fā)車輛位置和狀態(tài)信息，并進(jìn)行車隊管理.

構(gòu)型變換實施流程為：① 車輛編隊按照縱向一路編隊形式運(yùn)行；②到達(dá)構(gòu)型變換區(qū)域，指揮員下達(dá)構(gòu)型變換指令，車間通信與車隊管理系統(tǒng)將構(gòu)型變換任務(wù)下達(dá)各車；③各車按照下達(dá)的任務(wù)實施構(gòu)型變換.

圖10為隊形變換過程中各跟隨車的速度曲線，在構(gòu)型變換過程中跟隨車速度響應(yīng)存在一定滯后，最大滯后時間約0.4 s，主要原因是車輛系統(tǒng)的響應(yīng)延遲.圖11為跟隨車相對位置變化曲線，由于車輛速度波動，車輛實際位置相比理論位置有一定偏差，縱向最大滯后約0.72 m，橫向最大滯后約0.06 m.盡管存在一定偏差，但構(gòu)型變換任務(wù)能夠在規(guī)定條件下完成，證明了所提出構(gòu)型變換方法的可行性.

圖10 構(gòu)型變換中各跟隨車速度曲線Fig.10 Speed curve of two following vehicles in formation reconfiguration

圖11 跟隨車1、2橫向和縱向的目標(biāo)和實際位置Fig.11 Target and actual position of two following vehicles in lateral and longitudinal direction

4 結(jié)論

提出一種時空約束下無人車集群構(gòu)型變換方法，設(shè)計構(gòu)型變換流程，利用基于軌跡預(yù)測的路徑規(guī)劃和基于曲線插值的速度規(guī)劃方法滿足構(gòu)型變換的空間和時間約束.仿真分析兩種構(gòu)型變換策略的優(yōu)缺點，通過實車試驗證明所提構(gòu)型變換方法的可行性.本文研究有助于無人車集群的快速發(fā)展和運(yùn)用.