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

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

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

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

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

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

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

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

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

(1)

(2)

(3)

(4)

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

R=

(5)

(6)

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

(7)

圖1 無人機模型示意圖

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

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

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

2.1 CAPT算法

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

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

(8)

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

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

假設初始位置與目標位置、各無人機之間的距離滿足

(9)

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

(10)

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

(11)

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

所有的機器人都是在一定時間之內到達目標位置，所有無人機到達最終位置的終止時間T為

(12)

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

(13)

2.2 無人機軌跡平滑方法

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

(14)

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

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

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

(18)

(19)

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

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

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

3.1 位置環(huán)控制器設計

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

(20)

本文所設計的控制器包括2部分：標稱控制器un及補償控制器ud，即

u=un+ud

(21)

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

τf=mu(Rez)

(22)

下面將分別介紹標稱控制器un和補償控制器ud的設計過程。

步驟1標稱控制器設計

針對標稱系統(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)是有限時間收斂到平衡點的。

(27)

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

考慮標稱系統(tǒng)式(23)，設計標稱控制器為

(28)

式中：k1,k2>0。

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

(29)

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

步驟2補償控制器設計

設計積分滑模面：

(30)

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

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

(31)

ueq=un-d(t)

(32)

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

(33)

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

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

(34)

根據式(31)和式(34)，設計虛擬控制?為

(35)

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

而真實控制器為

(36)

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

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

(37)

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

(38)

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

(39)

(40)

(41)

(42)

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

(43)

因此，式(41)可以寫為

(44)

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

(45)

(46)

(47)

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

(48)

(49)

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

(50)

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

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

(51)

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

3.3 姿態(tài)控制器設計

(52)

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

ua=una+uda

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

(53)

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

τ=IW-1ua

(54)

3.4 仿真校驗

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

1) 四旋翼無人機物理參數(shù)：質量m=0.5 kg，慣性參數(shù)Ix=2.3×10-3kg·m2，Iy=2.4×10-3kg·m2，Iz=2.6×10-3kg·m2，旋翼轉動中心到無人機中心的距離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。

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

圖3 位置跟蹤誤差

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

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

圖5 控制輸入

4 仿真平臺搭建與效果驗證

4.1 仿真平臺基本結構

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

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

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

圖6 Gazebo-ROS仿真平臺架構

4.2 搭建仿真場景

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

圖7 Gazebo仿真流程

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

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

圖8 集群無人機隊形重構仿真方案

4.4 仿真驗證

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

圖9 12架無人機隊形重構

5 結 論

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

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

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