集群無人機(jī)隊(duì)形重構(gòu)及虛擬仿真驗(yàn)證

盧燕梅，宗群，張秀云，魯瀚辰，張睿隆

天津大學(xué) 電氣自動化與信息工程學(xué)院，天津 300072

四旋翼無人機(jī)具有成本較低、設(shè)備簡單、飛行靈活等特點(diǎn)，近些年來廣泛應(yīng)用于軍事和民用領(lǐng)域，如目標(biāo)偵察、應(yīng)急救援、農(nóng)業(yè)植保、無人機(jī)燈光表演等。隨著任務(wù)復(fù)雜度的增加，單架無人機(jī)往往難以滿足任務(wù)需求，因此無人機(jī)集群控制及其應(yīng)用由此成為當(dāng)下的一個研究熱點(diǎn)，多無人機(jī)集群能夠提高執(zhí)行任務(wù)的效率、靈活性和容錯能力。無人機(jī)編隊(duì)隊(duì)形控制方法是實(shí)現(xiàn)多無人機(jī)編隊(duì)安全飛行的前提和必要手段[1]，其中集群無人機(jī)隊(duì)形重構(gòu)是集群無人機(jī)所要考慮的一個重要問題，主要是一定數(shù)量的無人機(jī)根據(jù)特定要求變換隊(duì)形，需要研究無人機(jī)路徑規(guī)劃算法和軌跡姿態(tài)控制算法等。

對于集群無人機(jī)隊(duì)形重構(gòu)的路徑規(guī)劃，主要目標(biāo)是讓每架無人機(jī)都能從初始位置無碰撞地到達(dá)最終位置，保證隊(duì)形重構(gòu)過程中代價最小或能耗最優(yōu)[2]。這其中既包含目標(biāo)分配問題，即每架無人機(jī)被分配到的目標(biāo)隊(duì)形的某一特定位置；又包含軌跡生成問題，保證無碰撞地到達(dá)目標(biāo)位置。許多算法對上述兩個問題進(jìn)行了解耦，即先進(jìn)行目標(biāo)分配，分配好目標(biāo)位置之后再進(jìn)行起始點(diǎn)到分配好的目標(biāo)位置之間的軌跡生成，其中目標(biāo)分配問題多利用匈牙利算法[3]進(jìn)行解決。分配好目標(biāo)位置之后，在軌跡生成部分，常見算法包括圖搜索算法A*[4]及其變體[5]、自適應(yīng)隨機(jī)搜索算法[6]、模型預(yù)測控制[7]、混合整數(shù)規(guī)劃[8-9]等。以上算法在單一無人機(jī)規(guī)劃時效果較好，但是在多無人機(jī)軌跡規(guī)劃時普遍存在計(jì)算難度加大、規(guī)劃時間增長、規(guī)劃效率難以滿足實(shí)際應(yīng)用時的實(shí)時性要求等缺點(diǎn)，因此，需要探索計(jì)算簡便、效率高的多無人機(jī)路徑規(guī)劃算法。

在無人機(jī)集群由同構(gòu)無人機(jī)構(gòu)成的情況下，每個目標(biāo)可以任意分配給各架無人機(jī)，則稱之為無標(biāo)號問題。若集群中包含異構(gòu)無人機(jī)或某些特定情境下針對某一目標(biāo)位置只能被分配給特定無人機(jī)(如快遞運(yùn)輸)，則稱之為帶標(biāo)號問題。Kumar等針對無標(biāo)號問題提出了許多算法，其中CAPT(Concurrent Assignment and Planning of Trajectories)算法[10]對多架無人機(jī)進(jìn)行并行的目標(biāo)分配和軌跡規(guī)劃?？紤]到將目標(biāo)分配和軌跡生成問題解耦計(jì)算的問題復(fù)雜度，該算法將上述2個問題耦合，提出了包括集中式CAPT(C-CAPT)和分布式CAPT(D-CAPT)2種實(shí)現(xiàn)思路，實(shí)現(xiàn)了無障礙物情況下的多機(jī)無碰撞全局路徑規(guī)劃，極大降低了計(jì)算的復(fù)雜程度，提高了實(shí)時性和實(shí)用性。隨后Kumar團(tuán)隊(duì)的Tang等基于帶標(biāo)號問題，在CAPT算法的基礎(chǔ)上相繼提出了OMP+CHOP(Optimal Motion Plans+ Circular HOlding Patterns)算法[11]、HOOP(HOld or take Optimal Plan)算法[12]，針對無障礙的擁擠環(huán)境，提出在無碰撞時采取CAPT算法、將要碰撞時采取圓形保持軌道進(jìn)行軌跡設(shè)計(jì)的思路，擴(kuò)展了CAPT算法的研究情景和研究領(lǐng)域。

對于無人機(jī)隊(duì)形重構(gòu)的控制問題，主要目標(biāo)是控制無人機(jī)追蹤預(yù)設(shè)軌跡，保證在一定外界干擾的情況下仍不發(fā)生偏移。若干擾有界，滑模控制算法在四旋翼無人機(jī)控制中應(yīng)用廣泛[13]，在文獻(xiàn)[14]中研究了一種終端滑模控制算法，可以很容易地消除飛行中擾動影響，但是主要缺點(diǎn)是控制律不連續(xù)引起的抖振。高階滑?？刂扑惴ㄈ缏菪统菪？刂瓶梢援a(chǎn)生連續(xù)控制信號，抵消連續(xù)擾動，文獻(xiàn)[15]利用內(nèi)-外環(huán)結(jié)構(gòu)設(shè)計(jì)了一種超螺旋滑模控制器。文獻(xiàn)[16]則提出了利用連續(xù)律來控制無人機(jī)姿態(tài)的新策略，但前提條件必須是擾動邊界已知。文獻(xiàn)[17]提出了一種新的連續(xù)多變量積分滑模控制器。結(jié)合連續(xù)標(biāo)稱控制器和改進(jìn)的超螺旋控制器來實(shí)現(xiàn)對干擾的有效抑制?；跀_動上界未知問題，引入自適應(yīng)控制算法，文獻(xiàn)[18]設(shè)計(jì)了一種自適應(yīng)終端滑?？刂撇呗裕瑢?shí)現(xiàn)了有限時間收斂。文獻(xiàn)[19]提出的自適應(yīng)終端滑模有限時間控制算法能有效滿足姿態(tài)跟蹤控制的快速性需求。無人機(jī)控制算法大多針對單輸入單輸出系統(tǒng)進(jìn)行設(shè)計(jì)，文獻(xiàn)[20]設(shè)計(jì)多變量有限時間積分滑?？刂破鳎ＷC了對期望參考輸入的高精度快速跟蹤控制。采用多變量控制算法可以避免對多輸入多輸出系統(tǒng)設(shè)計(jì)時進(jìn)行解耦[21]，從而一定程度上提高控制效果如減少滑?？刂扑惴ǖ亩墩瘳F(xiàn)象。

本文考慮集群無人機(jī)隊(duì)形重構(gòu)問題，首先基于四旋翼無人機(jī)軌跡姿態(tài)數(shù)學(xué)模型，利用CAPT算法進(jìn)行集群無人機(jī)路徑規(guī)劃，實(shí)現(xiàn)對多無人機(jī)的并行位置分配和最優(yōu)軌跡生成；其次，分別考慮干擾影響下的無人機(jī)軌跡及姿態(tài)控制系統(tǒng)，提出姿態(tài)解算及新型連續(xù)多變量積分滑模控制器設(shè)計(jì)算法，實(shí)現(xiàn)對無人機(jī)軌跡的有限時間精確跟蹤；最后，搭建Gazebo-ROS無人機(jī)仿真平臺，在該平臺下實(shí)現(xiàn)了12架四旋翼無人機(jī)隊(duì)形重構(gòu)過程的整體效果仿真，驗(yàn)證所提算法的有效性和實(shí)用性。

1 四旋翼無人機(jī)軌跡姿態(tài)模型

本文所采用的四旋翼無人機(jī)模型示意圖如圖1所示。xB、yB、zB為無人機(jī)機(jī)體坐標(biāo)系，xW、yW、zW為無人機(jī)慣性坐標(biāo)系。Fi(i=1,2,3,4)及Mi(i=1,2,3,4)分別代表4個電機(jī)產(chǎn)生的升力及力矩。充分考慮四旋翼無人機(jī)的固有特性，無人機(jī)在飛行過程中的動力學(xué)因素及受力平衡關(guān)系，其軌跡姿態(tài)數(shù)學(xué)模型為[22]

(1)

(2)

(3)

(4)

式中：p=[x,y,z]T∈R3，v=[vx,vy,vz]T∈R3，Θ=[φ,θ,ψ]T∈R3分別為無人機(jī)慣性坐標(biāo)系下的位置、速度和歐拉角；Ω=[ωx,ωy,ωz]T∈R3表示機(jī)體坐標(biāo)系下的姿態(tài)角速率；g=9.8 m/s2為地球重力加速度，ez=[0,0,1]T為常值向量；m為四旋翼無人機(jī)質(zhì)量；τf為無人機(jī)的總升力；τ為無人機(jī)x,y,z這3個方向的控制力矩；I=diag(Ix,Iy,Iz)為無人機(jī)慣性常值矩陣；Δ1(t)、Δ2(t)為綜合干擾。包括模型參數(shù)不確定及外界干擾，旋轉(zhuǎn)矩陣R和變換矩陣W定義為

R=

(5)

(6)

式中：cθ=cosθ,sθ=sinθ，下同?？刂屏縖τf，τ1,τ2,τ3]T與電機(jī)轉(zhuǎn)速ω=[ω1,ω2,ω3,ω4]T之間的關(guān)系為

(7)

圖1 無人機(jī)模型示意圖

本文控制目標(biāo)為：針對無人機(jī)軌跡姿態(tài)模型式(1)～式(4)，設(shè)計(jì)集群無人機(jī)路徑規(guī)劃算法和軌跡姿態(tài)跟蹤控制器，實(shí)現(xiàn)集群無人機(jī)安全、迅速的隊(duì)形重構(gòu)。

2 集群無人機(jī)路徑規(guī)劃

本節(jié)將針對集群無人機(jī)路徑規(guī)劃問題，利用CAPT算法及無人機(jī)軌跡平滑方法，實(shí)現(xiàn)多無人機(jī)隊(duì)形重構(gòu)過程中的并行目標(biāo)分配和軌跡生成。

2.1 CAPT算法

CAPT算法[10]是Kumar團(tuán)隊(duì)提出的基于無標(biāo)號問題的解決方案，可以解決集群由同構(gòu)個體組成且目標(biāo)位置可以任意分配的運(yùn)動規(guī)劃問題。本文研究的集群無人機(jī)隊(duì)形重構(gòu)問題，只要求形成特定的隊(duì)形形狀，而不關(guān)心每架無人機(jī)的具體位置，可以轉(zhuǎn)化成無標(biāo)號問題，因此可以采用CAPT算法求解。

考慮集群無人機(jī)隊(duì)形重構(gòu)問題，假設(shè)集群無人機(jī)規(guī)模與無人機(jī)目標(biāo)位置數(shù)量相同為N，第i架無人機(jī)的初始位置為xi∈Rn，第j個目標(biāo)位置gj∈Rn。解決無人機(jī)隊(duì)形重構(gòu)問題需要對無人機(jī)目標(biāo)位置進(jìn)行分配同時產(chǎn)生到達(dá)各個目標(biāo)的安全軌跡。

(8)

定義增廣分配矩陣Φ≡φ?In，?代表求取克羅克內(nèi)積。

CAPT算法的目的在于規(guī)劃軌跡p(t)→X(t)(t∈[0，T])，T為所有無人機(jī)到達(dá)最終位置的終止時間，將在后文進(jìn)行說明。運(yùn)動軌跡p(t)滿足始端約束條件p(0)=X(0)和終端約束條件ΦTp(T)=G。

假設(shè)初始位置與目標(biāo)位置、各無人機(jī)之間的距離滿足

(9)

CAPT算法將無標(biāo)號問題看做最優(yōu)化問題進(jìn)行求解，確定最優(yōu)的分配方案和軌跡，最優(yōu)化目標(biāo)函數(shù)為

(10)

問題的解由直線軌跡組成，且各無人機(jī)走過的距離的平方之和最小。即最優(yōu)分配方案為

(11)

其中分配問題φ*可以用匈牙利算法進(jìn)行求解。

所有的機(jī)器人都是在一定時間之內(nèi)到達(dá)目標(biāo)位置，所有無人機(jī)到達(dá)最終位置的終止時間T為

(12)

p*(t)=(1-δ(t))X(0)+δ(t)ΦG

(13)

2.2 無人機(jī)軌跡平滑方法

為了滿足四旋翼無人機(jī)的動力學(xué)方程，需要對軌跡進(jìn)行平滑，Mellinger和Kumar[22]證明了最小化位置狀態(tài)的四階導(dǎo)可以實(shí)現(xiàn)對于軌跡的平滑追蹤。代價函數(shù)為

(14)

問題(14)的最優(yōu)軌跡與式(10)所求直線軌跡相同，δ(t)為高階時間多項(xiàng)式。

本文中為了實(shí)現(xiàn)對軌跡的精確平滑追蹤，同時對位置、速度和加速度進(jìn)行控制，由文獻(xiàn)[23]中的最小化三次可微運(yùn)動的方法，將δ(t)表示為5階 時間多項(xiàng)式函數(shù)。利用文獻(xiàn)[23]中給出的不同終端約束條件下的最優(yōu)狀態(tài)軌跡參數(shù)表達(dá)式，在固定位置、固定速度和固定加速度的情況下，將位置狀態(tài)求取p(t)→X(t)(t∈[0,T])進(jìn)一步擴(kuò)展成求取包括位置、速度、加速度在內(nèi)的狀態(tài)軌跡s(k)=[pk,vk,ak]，其中：pk、vk、αk分別為X、Y、Z坐標(biāo)系中單架無人機(jī)在某一時刻某一坐標(biāo)維度的位置、速度和加速度。注意此時只針對X-Y-Z坐標(biāo)系中的單架無人機(jī)軌跡進(jìn)行求取，即求得的是單架無人機(jī)在某一時刻某一坐標(biāo)維度的位置、速度和加速度狀態(tài)信息。

狀態(tài)軌跡參數(shù)為

(15)

定初始狀態(tài)向量s(0)=[p0,v0,a0]。終端狀態(tài)向量s(T)=[pf,vf,af]，則

(16)

最優(yōu)狀態(tài)軌跡為

(17)

在本文研究的集群無人機(jī)隊(duì)形重構(gòu)問題中，采用“懸停-運(yùn)動-懸?！钡倪\(yùn)動模式，給定s(0)=[p0,0,0]和s(T)=[pf,0,0]，代入式(15)～式(17)中，得到最優(yōu)狀態(tài)軌跡：

(18)

(19)

3 四旋翼無人機(jī)軌跡姿態(tài)控制

基于第2節(jié)路徑規(guī)劃所設(shè)計(jì)的無人機(jī)軌跡，本節(jié)將分別針對無人機(jī)位置環(huán)及姿態(tài)環(huán)，設(shè)計(jì)有限時間多變量滑?？刂萍白藨B(tài)解算算法，實(shí)現(xiàn)干擾影響下的無人機(jī)位置-姿態(tài)跟蹤控制。

圖2 無人機(jī)位置-姿態(tài)控制原理圖

3.1 位置環(huán)控制器設(shè)計(jì)

定義位置誤差e1=p-pref，基于式(1)和式(2)， 可得

(20)

本文所設(shè)計(jì)的控制器包括2部分：標(biāo)稱控制器un及補(bǔ)償控制器ud，即

u=un+ud

(21)

其中，標(biāo)稱控制器un用于保證無綜合干擾(外加干擾及模型參數(shù)不確定綜合)影響下系統(tǒng)的穩(wěn)定跟蹤控制；而補(bǔ)償控制器ud則用于保證對綜合干擾的抑制。故位置環(huán)實(shí)際控制輸入為

τf=mu(Rez)

(22)

下面將分別介紹標(biāo)稱控制器un和補(bǔ)償控制器ud的設(shè)計(jì)過程。

步驟1標(biāo)稱控制器設(shè)計(jì)

針對標(biāo)稱系統(tǒng)：

(23)

引理1[16]針對積分鏈系統(tǒng)：

(24)

定義k1,k2,…,kn>0使sn+knsn-1+…+k2s+k1是Hurwitz的，且存在ε∈(0,1)使得對于每個r∈(1-ε,1)，如果采用如下反饋控制器：

(25)

式中:r1,r2,…,rn滿足

(26)

且rn=r,rn+1=1，那么系統(tǒng)式(24)是有限時間收斂到平衡點(diǎn)的。

(27)

式中:V(x0)為V(x)的初值；Treach為收斂時間。

考慮標(biāo)稱系統(tǒng)式(23)，設(shè)計(jì)標(biāo)稱控制器為

(28)

式中：k1,k2>0。

將式(28)代入標(biāo)稱系統(tǒng)(23)可得

(29)

故由引理1及式(29)可知，標(biāo)稱控制器式(28) 能夠保證系統(tǒng)式(23)的有限時間收斂。然而系統(tǒng)式(23)未考慮外界干擾和系統(tǒng)內(nèi)部不確定等綜合干擾的影響，因此，下面將給出連續(xù)補(bǔ)償控制器設(shè)計(jì)過程，用于對系統(tǒng)的綜合干擾進(jìn)行處理。

步驟2補(bǔ)償控制器設(shè)計(jì)

設(shè)計(jì)積分滑模面：

(30)

式中：e2(0)為e2(t)的初值。由式(30)可以看出，當(dāng)t=0時，滑模面s=0，即系統(tǒng)初始狀態(tài)就位于滑模面上，避免了趨近滑模的過程，能夠保證整個過程的魯棒性。

對積分滑模面(30)求導(dǎo)，可得

(31)

ueq=un-d(t)

(32)

代入系統(tǒng)(20)可得

(33)

通過比較式(33)及式(23)可得，當(dāng)系統(tǒng)狀態(tài)到達(dá)滑模面s后，綜合干擾影響下的系統(tǒng)即轉(zhuǎn)化為標(biāo)稱控制系統(tǒng)，基于所設(shè)計(jì)的標(biāo)稱控制器(28)即可實(shí)現(xiàn)系統(tǒng)的有限時間穩(wěn)定。因此，此時的控制目標(biāo)轉(zhuǎn)變?yōu)椋涸O(shè)計(jì)補(bǔ)償控制器ud，保證系統(tǒng)狀態(tài)有限時間內(nèi)到達(dá)滑模面s。

對式(31)再次求導(dǎo)，可得

(34)

根據(jù)式(31)和式(34)，設(shè)計(jì)虛擬控制?為

(35)

式中：α>2L為正常數(shù)。

而真實(shí)控制器為

(36)

由式(35)和式(36)可以看出，不連續(xù)量出現(xiàn)在虛擬控制輸入?中，而真正的補(bǔ)償控制輸入ud是連續(xù)的，因此能夠有效減少抖振，提高控制精度。

定理1考慮綜合干擾影響下的跟蹤控制系統(tǒng)式(20)，在假設(shè)1成立的條件下，如果控制器設(shè)計(jì)為式(21)，其中標(biāo)稱控制器un設(shè)計(jì)如式(28)所示，補(bǔ)償控制器ud設(shè)計(jì)如式(36)所示，那么存在一系列常數(shù)00,k2>0，α>2L，使得系統(tǒng)狀態(tài)跟蹤誤差是有限時間收斂到零的。

(37)

定義以下Lyapunov函數(shù)：

(38)

式中：γ為待設(shè)計(jì)正常數(shù)。式(38)可重新書寫為

(39)

(40)

(41)

(42)

由式(42)可知，如果矩陣A>0那么矩陣P1>0。由于

(43)

因此，式(41)可以寫為

(44)

由此可見，Lyapunov函數(shù)V是正定的。對V求導(dǎo)，可得

(45)

(46)

(47)

如果參數(shù)α,γ滿足下列條件：

(48)

(49)

基于式(44)和式(49)，式(46)可以寫為

(50)

3.2 姿態(tài)解算算法

四旋翼無人機(jī)無法通過控制力矩直接對飛行軌跡進(jìn)行控制，需要對式(21)中位置控制器得到的控制指令進(jìn)行姿態(tài)解算，轉(zhuǎn)化為期望的飛行姿態(tài)。本文中采用懸?？刂扑枷耄粉櫰谕恢胮ref和期望偏航角ψref=ψ0，姿態(tài)解算算法[25]為

(51)

式中：φref、θref、ψref分別為期望的滾轉(zhuǎn)角、俯仰角和偏航角。且期望加速度accref=-gez+u+Δ1(t)，accrefj(j=1,2,3)表示向量accref的第j個元素。

3.3 姿態(tài)控制器設(shè)計(jì)

(52)

等同于位置環(huán)控制器設(shè)計(jì)，可進(jìn)行姿態(tài)跟蹤控制器ua的設(shè)計(jì)。姿態(tài)控制器包括2部分：

ua=una+uda

式中:標(biāo)稱控制器una用于保證無綜合干擾(外加干擾及模型參數(shù)不確定綜合)影響下系統(tǒng)的穩(wěn)定跟蹤控制；而補(bǔ)償控制器uda則用于保證對綜合干擾的抑制。標(biāo)稱控制器及補(bǔ)償控制器分別設(shè)計(jì)為

(53)

最終得到姿態(tài)環(huán)控制力矩為

τ=IW-1ua

(54)

3.4 仿真校驗(yàn)

為驗(yàn)證設(shè)計(jì)的軌跡姿態(tài)控制算法的有效性，在MATLAB環(huán)境下對單架無人機(jī)進(jìn)行仿真，仿真采樣頻率設(shè)置為0.001，仿真過程中各參數(shù)選取如下：

1) 四旋翼無人機(jī)物理參數(shù)：質(zhì)量m=0.5 kg，慣性參數(shù)Ix=2.3×10-3kg·m2，Iy=2.4×10-3kg·m2，Iz=2.6×10-3kg·m2，旋翼轉(zhuǎn)動中心到無人機(jī)中心的距離d=0.225 m，旋翼的升力系數(shù)kF=5.55×10-8N·m· s2/rad2，旋翼的扭矩系數(shù)kM=2.5×10-9N·s2/rad2。

2) 期望軌跡pref=[sint,cost,sint]，期望偏航角ψref=0 °，所受干擾為Δ1=Δ2=[0.1sint，0.1cost，0.1(sint+cost)]，初始值p0=[2,-3,2]T,v0=[0,0,0]T，Θ0=[0.2,-0.3,0.2]T，Ω0=[0,0,0]T。

3) 控制器參數(shù)：位置控制器k1=5,k2=4,r1=1/2,r2=5/7,α=5。姿態(tài)控制器k3=12,k4=5,r3=1/2,r4=5/7,β=4。

仿真結(jié)果如圖3～圖5所示。由圖3和圖4可知，在綜合干擾影響的情況下，位置跟蹤誤差及姿態(tài)跟蹤誤差均可以在有限時間內(nèi)收斂到零。

圖3 位置跟蹤誤差

圖4 姿態(tài)跟蹤誤差

總控制量[τf，τ1，τ2，τ3]T的仿真結(jié)果如圖5所示，可以看到控制輸入連續(xù)且穩(wěn)定，系統(tǒng)在一定外界干擾的情況下仍能保持穩(wěn)定，且取得良好的軌跡姿態(tài)跟蹤效果。由圖3～圖5的仿真結(jié)果可以有效驗(yàn)證所提控制算法的有效性。

圖5 控制輸入

4 仿真平臺搭建與效果驗(yàn)證

4.1 仿真平臺基本結(jié)構(gòu)

集群無人機(jī)隊(duì)形重構(gòu)即一定數(shù)量的無人機(jī)根據(jù)特定任務(wù)需求，從一個隊(duì)形變換形成另一個新的隊(duì)形形狀。需要考慮的問題涉及目標(biāo)位置分配、路徑規(guī)劃、避撞要求、姿態(tài)軌跡跟蹤控制等多個方面。要對上述問題分別考慮控制策略，并以一種直觀可靠的方式對控制算法進(jìn)行驗(yàn)證和效果對比，就需要進(jìn)行綜合虛擬仿真實(shí)驗(yàn)。

本文采用的Gazebo仿真軟件，是基于ROS操作系統(tǒng)的一款三維物理仿真軟件，與ROS操作系統(tǒng)配套使用，可實(shí)現(xiàn)完整的“建模-仿真-可視化”的仿真流程，與MATLAB仿真相比，Gazebo仿真更貼近真實(shí)物理情景，仿真的實(shí)時性和真實(shí)性更強(qiáng)，同時能夠克服MATLAB仿真曲線表示缺乏直觀性的缺點(diǎn)，實(shí)現(xiàn)仿真結(jié)果的三維立體視景演示。Gazebo軟件中可以搭建近似于現(xiàn)實(shí)世界的仿真場景，構(gòu)建多種傳感器仿真模型，搭建包括無人機(jī)在內(nèi)的各種機(jī)器人模型，包含多個物理仿真引擎，由此可以為機(jī)器人模型添加現(xiàn)實(shí)世界的物理性質(zhì)，使仿真對象滿足貼近真實(shí)的運(yùn)動學(xué)和動力學(xué)物理要求。

仿真平臺的基本框架如圖6所示。其中ROS作為機(jī)器人開源的元操作系統(tǒng)，主要承擔(dān)對于控制器算法的實(shí)現(xiàn)，通過編寫路徑規(guī)劃算法、無人機(jī)控制算法等C++代碼，實(shí)現(xiàn)對無人機(jī)的位置、姿態(tài)等狀態(tài)的控制；Gazebo仿真軟件作為一個可視化的圖形用戶界面，需要在其中搭建仿真場景、傳感器模型、仿真無人機(jī)等，通過Gazebo-ROS bridge與ROS系統(tǒng)建立連接，實(shí)現(xiàn)無人機(jī)模型狀態(tài)的獲取與發(fā)送，可視化地演示集群無人機(jī)隊(duì)形重構(gòu)的整體流程效果。平臺各個部分描述見4.2節(jié)和4.3節(jié)。

圖6 Gazebo-ROS仿真平臺架構(gòu)

4.2 搭建仿真場景

Gazebo仿真軟件是一個功能完善的3D機(jī)器人仿真軟件，通過Gazebo搭建無人機(jī)仿真的整體場景，包括搭建仿真環(huán)境和無人機(jī)模型，添加傳感器、執(zhí)行器等各種插件，實(shí)現(xiàn)ROS與Gazebo的連接等環(huán)節(jié)，主要操作流程如圖7所示。其中URDF文件是Gazebo-ROS中的一種用于描述機(jī)器人組件的XML文件，該文件的主要功能就是描述機(jī)器人各個部分(link)與關(guān)節(jié)(joint)的屬性，創(chuàng)建機(jī)器人模型。.world文件是對仿真環(huán)境的描述，通過.launch文件啟動并加載仿真場景，由此實(shí)現(xiàn)Gazebo中仿真模型的整體搭建。詳細(xì)操作步驟參考Gazebo軟件官方說明[26]，此處不再贅述。

圖7 Gazebo仿真流程

4.3 集群無人機(jī)飛行控制的實(shí)現(xiàn)

本文中基于無人機(jī)燈光秀場景，設(shè)計(jì)實(shí)現(xiàn)對集群無人機(jī)進(jìn)行隊(duì)形重構(gòu)的整體流程演示，每架無人機(jī)都由相同的控制系統(tǒng)框架進(jìn)行控制，總體仿真演示方案如圖8所示，其核心包括軌跡規(guī)劃模塊和軌跡跟蹤模塊，其中軌跡規(guī)劃模塊包括隊(duì)形設(shè)計(jì)和路徑規(guī)劃2個子模塊，隊(duì)形設(shè)計(jì)子模塊針對無人機(jī)表演的隊(duì)形形狀進(jìn)行隊(duì)形設(shè)計(jì)，路徑規(guī)劃子模塊為每架無人機(jī)從起始點(diǎn)到目標(biāo)隊(duì)形最終點(diǎn)的軌跡進(jìn)行規(guī)劃設(shè)計(jì)；軌跡跟蹤模塊，主要利用本文第3節(jié)中的軌跡姿態(tài)控制器，通過內(nèi)環(huán)的姿態(tài)控制和外環(huán)的位置控制，確保每架無人機(jī)都能夠安全地、無碰撞地追蹤給定軌跡朝著各自目標(biāo)位置飛行。

圖8 集群無人機(jī)隊(duì)形重構(gòu)仿真方案

4.4 仿真驗(yàn)證

以12架四旋翼無人機(jī)為例，對上述提出的路徑規(guī)劃算法及軌跡姿態(tài)控制器的有效性進(jìn)行數(shù)值仿真驗(yàn)證。12架四旋翼無人機(jī)集群飛行的飛行示意圖如圖9所示，兩隊(duì)形之間的詳細(xì)隊(duì)形重構(gòu)過程如圖10所示。圖中標(biāo)注實(shí)時追蹤2架無人機(jī)(無人機(jī)6和無人機(jī)7)的實(shí)時位置變化。由圖9可以看出仿真實(shí)現(xiàn)了連續(xù)的隊(duì)形重構(gòu)過程，顯示了各隊(duì)形重構(gòu)后無人機(jī)的位置變化情況，其中由隊(duì)形1到隊(duì)形2的變換過程中無人機(jī)6的位置幾乎不變，體現(xiàn)了隊(duì)形位置最優(yōu)分配保證效率最高的思想。由圖10可以看出在兩隊(duì)形重構(gòu)過程中，無人機(jī)間均保持一定的飛行距離，確保相互之間不會發(fā)生碰撞。總結(jié)可得基于本文的控制算法，利用CAPT算法進(jìn)行集群無人機(jī)路徑規(guī)劃，可以實(shí)現(xiàn)多無人機(jī)的連續(xù)、安全、無碰撞的隊(duì)形重構(gòu)過程。

圖9 12架無人機(jī)隊(duì)形重構(gòu)

5 結(jié) 論

1) 研究了路徑規(guī)劃CAPT算法和無人機(jī)軌跡平滑方法，針對無人機(jī)集群隊(duì)形重構(gòu)的具體場景需求，實(shí)現(xiàn)多無人機(jī)安全迅速的并行目標(biāo)分配和軌跡生成。

2) 針對無人機(jī)軌跡姿態(tài)跟蹤問題，提出了一種有限時間多變量積分滑?？刂扑惴?，實(shí)現(xiàn)了對無人機(jī)期望軌跡的有限時間精確跟蹤。

3) 基于Gazebo-ROS搭建了無人機(jī)仿真平臺，基于實(shí)際物理引擎及多種動力學(xué)插件實(shí)現(xiàn)了集群無人機(jī)隊(duì)形重構(gòu)“建模-仿真-可視化”的一體化虛擬仿真演示，對上述控制算法及路徑規(guī)劃算法進(jìn)行了有效性驗(yàn)證。