基于tau矢量場制導的多無人機協同standoff跟蹤方法

楊祖強,方　舟,李　平,2

(1.浙江大學 航空航天學院,浙江 杭州 310027; 2. 浙江大學 控制科學與工程學院,浙江 杭州 310027)

?

基于tau矢量場制導的多無人機協同standoff跟蹤方法

楊祖強1,方舟1,李平1,2

(1.浙江大學 航空航天學院,浙江 杭州 310027; 2. 浙江大學 控制科學與工程學院,浙江 杭州 310027)

摘要:針對有時間約束的多無人機(UAV)協同standoff跟蹤需求,研究基于四維矢量場的多UAV協同制導方法.利用本征tau-G制導策略能夠依照期望時間對各運動狀態進行同步規劃的特性,構建tau制導矢量場,并在此基礎上提出綜合性多UAV協同standoff跟蹤方法.該方法用tau矢量場導引各UAV的位置在期望時間準確收斂于目標圓,利用tau-G策略調整UAV之間的相位間隔,應用序列二次規劃對跟蹤參數進行優化,并采用人工勢場法進行協同避碰避障.仿真結果表明,基于tau矢量場制導的協同standoff跟蹤方法計算負荷低,跟蹤偏差小、制導策略可飛性好,飛行安全性高,能夠更好地滿足多UAV協同standoff跟蹤的應用需求.

關鍵詞：廣義tau理論；tau矢量場制導；standoff跟蹤；多機協同

多無人機(unmanned aerial vehicle,UAV)協同standoff跟蹤[1]是一類基本的多機協同任務.在該任務中,多架UAV以目標為圓心,按照一定相位間隔環繞目標做圓周運動.期望的圓周軌跡被稱為目標圓,圓周半徑為standoff距離.多UAV協同standoff跟蹤既可保證機載傳感器對目標有效覆蓋,又能降低任務風險,因而在目標定位、交通管制等領域有廣闊的應用前景.

目前已有的standoff跟蹤方法包括“優秀舵手”法[2],路徑塑造[3],非線性模型預測控制[4],參考點制導[5],矢量場制導等.矢量場制導在standoff跟蹤問題中應用最廣,主要包括Lyapunov矢量場制導(lyapunov vector field guidance,LVFG)[1, 6]、Tangent矢量場制導 (tangent field guidance,TVFG)[7]、T+LVFG[8]等.然而,已有的矢量場制導方法通常導引UAV在時間t→∞時漸進收斂于目標圓,而在協同standoff跟蹤中則期望各UAV能夠在期望時間按照相位間隔收斂.同時,已有方法在跟蹤制導時通常先用矢量場將UAV導引至目標圓,再調整相位間隔,放棄了收斂過程中的相位調整機會,延長了跟蹤形成的時間.最后,目前缺乏包含參數優化、矢量場制導、相位調整和避障等功能的綜合性協同standoff跟蹤方法.

為彌補已有矢量場制導方法的缺陷,應將時間信息融入矢量場中進行四維制導.近年來,生物啟發的tau理論在四維運動規劃領域日益受到關注[9].Tau理論在塘鵝捕魚[10],鴿子著陸[11]等動物運動規劃的研究基礎上提出,該理論認為視覺線索tau(τ)提供了在動物運動規劃中具有重要作用的接觸時間[12].Lee[13]將tau廣義化為運動物體由當前運動狀態轉換到目標狀態所需的接觸時間,提出了廣義tau理論,使tau可用于任何有目標的運動制導.應用廣義tau理論的仿生知識,研究者提出了本征tau-G制導策略(Intrinsic tau-G guidance strategy,tau-G策略)[13].該策略可嚴格依照時間約束對位置、速度、加速度進行同步規劃,并可保證運動狀態的平穩過渡[14]；同時,tau-G策略形式簡單,易于優化；因而在四維制導任務中優勢顯著.目前tau-G策略已被應用于UAV的運動規劃中,如剎車與著陸[15],鳥類停駐的模擬[16]等.

為克服已有方法的缺陷,本文利用tau-G策略構建tau矢量場,該矢量場可按照期望時間將各UAV導引至目標圓,實現制導維度的擴展.基于tau矢量場制導(tau vector field guidance,τVFG),本文提出了一種多機協同standoff跟蹤方法.該方法利用τVFG生成UAV的制導速度；在位置收斂過程中,同步采用tau-G策略規劃縱向飛行速度以調整UAV的相位間隔；并利用人工勢場法實現各UAV與障礙之間的避碰.

1多機協同standoff跟蹤問題

圖1　3架UAV協同standoff跟蹤示例Fig.1　Cooperative standoff tracking by three UAVs

為了保證對目標的持續standoff跟蹤,本文將跟蹤過程分為2段：在期望時間td內導引各UAV位置收斂于目標圓并調整相位至期望間隔的階段為跟蹤形成段；當跟蹤形成后,保持對目標的持續standoff跟蹤為跟蹤保持段.

在導航坐標系下,UAV的運動學模型為

(1)

式中:U=[ux,uy,uz]為UAV的3軸空速矢量,即矢量場為UAV提供的制導策略,W=[wx,wy,wz]為風速,ψ為UAV的航跡角.

制導策略需滿足的約束條件為

(2)

式中:vamax和vamin分別為最大和最小空速,ωmax為最大轉彎角速率,λmax為最大爬升/下滑率,最小轉彎半徑約束Rmin=vamin/ωmax已包含于式(2)中.

2tau制導矢量場的構建

2.1本征tau-G制導策略

在廣義tau理論中,tau變量定義為

(3)

Tau-G策略基于廣義tau理論提出,已有生物學研究表明,該策略在人和動物的運動平衡控制[17]、音樂演奏[18]等行為的規劃中發揮制導作用.Tau-G策略的內容為：若真實運動間距χ與虛擬的本征制導運動G(t)的tau變量在閉合過程中保持非零常數比率kχ,則2種運動的間距將在到達時間T同時閉合.kχ被稱為耦合系數,G(t)為重力作用下的自由落體運動,其與τG(t)的表達式為

(4)

(5)

圖2　kχ的不同取值對tau-G策略制導的影響Fig.2　Tau-G guidance with different k values

2.2tau制導矢量場

圖3　相對坐標矢量的分解Fig.3　Decomposition of relative position vector

(6)

(7)

式(7)僅根據UAV的初始位置進行制導,但由于UAV的避障動作、速度控制誤差和風擾等因素的影響,prt、prz不會嚴格按照式(7)收縮,甚至會嚴重偏離期望收斂過程.此時若不根據實際位置和當前時刻對r0、Δh0進行修正,則難以保證UAV的位置在td時刻收斂于目標圓.為保證矢量場能夠為各時刻、各位置的UAV提供合適的制導矢量,在時刻t需根據實際位置求得r0和Δh0為

(8)

將式(7)、(8)代入收縮項τ(pr)可得

(9)

設vs=‖S(pr)‖,為使UAV盡量按照應飛速度運行,即‖U‖=vd,則vs應滿足方程：

(10)

(11)

1) 在不超過vamax的情況下增大應飛速度vd；

2) 調整τVFG的耦合系數集[kt, kz]T,使其取值靠近0.5,以減小pr收縮過程中的最大速度.

綜上所述,τVFG制導矢量的表達式為

(12)

當t→td時,τ(pr)→0,在目標圓上僅S(pr)發揮作用,UAV以應飛速度繞目標旋轉.τVFG是時變的矢量場,其在制導中融入了時間信息,可為各時刻、各位置的UAV提供合適的制導矢量.

(13)

(14)

(15)

圖4　基于τVFG的無人機standoff跟蹤移動目標Fig.4　Standoff tracking of moving target based on τVFG

不同時刻的τVFG和UAV飛行軌跡示例如圖 4所示,其中UAV初始位置為[-400,-400]m,應飛速度vd=30m/s,目標初始位置為[0,0]m,以[3,3]m/s的速度勻速運動,期望跟蹤形成時間td=50s.由圖 4和式(12)可知,在tau矢量場制導的作用下,無論UAV位于目標圓內部還是外部,都能在時間td收斂于目標圓并環繞目標飛行,有效彌補了LVFG等方法不能嚴格滿足時間約束的缺陷.在standoff跟蹤形成后的跟蹤保持段,應當設定任務的期望時間td為較小值,以及時閉合UAV與目標圓之間的間距,保持對運動目標的跟蹤.

3多機協同standoff跟蹤方法

3.1多UAV協同相位調整

在多UAV協同跟蹤任務中,為使多架UAV按一定相位間隔分布在目標圓上,當旋轉項S(pr)不能提供合適的相位調整角速率時,需要在S(pr)上增加相位調整補償速度.

設i、j2架UAV間的相位間隔為θij,期望間隔為θdij,偏差Δθij=θdij-θij.與τVFG相似,采用tau-G制導策略描述t→td時,θij→θdij的過程,間距閉合所需的角速率為

(16)

式中:Δθ0ij為相位調整初始時刻相位偏差,kθ ij為相位間隔調整的耦合系數.在時刻t,Δθ0ij根據Δθij求得.

(17)

3.2多UAV協同避碰避障

多UAV在位置收斂和相位調整過程中,可能會發生沖突或遇到障礙,因此需要對沖突進行檢測,并對制導策略U進行修正,以保證飛行安全.

在多UAV協同跟蹤過程中,各UAV不能被當作質點,因此在沖突檢測中應當考慮UAV的形狀和體積.本文采用為UAV分配球形安全飛行區的方法[19]進行沖突檢測,如圖 5所示,安全區的半徑Rsafe應大于等于UAV的安全飛行距離,當其他UAV或障礙進入安全區(滿足式(18)),將會產生沖突報警；[xc,yc,zc]為闖入者相對于UAV本機中心的位置.該檢測方法可提供明確的安全警報,且檢測效率高,目前在航跡規劃領域已經得到了廣泛應用.簡便起見,本文中假設障礙物在UAV飛行高度的水平面內投影為圓形.

(18)

圖5　基于人工勢場法的UAV避障策略Fig.5　Obstacle avoidance based on Artificial Potential Field

為消解跟蹤過程中存在的沖突,本文采用人工勢場法導引UAV繞開沖突位置.該沖突消解方法采用虛擬勢場填充多UAV的工作空間,并利用勢函數求出障礙和其他UAV對本機作用的虛擬合力矢量,而后根據虛擬力的作用調整τVFG矢量航向的方法進行避障,即將制導策略U繞z軸旋轉Δψ變為U′.本文所采用的勢場僅考慮斥力,當UAV與障礙或其它飛機的距離d≤Ravoid時,斥力Frep發生作用；反之斥力為0；‖Frep‖的表達式為

(19)

式中:μ為角度調整增益,α為較小正數,以避免d→Rsafe時‖Frep‖→∞.

在障礙勢場的作用下,UAV所受到的虛擬力Frep及避障策略如圖 5,其方向沿障礙距UAV最近的點與UAV中心的連線指向UAV.將Frep分解為沿UAV運動方向的徑向力Famp和法向力Fort,其中‖Famp‖決定了航向角修正量Δψ的大小(Δψ∈(-π/2,π/2)),Fort的方向決定了Δψ的方向.修正量Δψ的表達式為

(20)

UAV距障礙越近,Δψ也越大；當UAV遠離障礙直至d>Ravoid時,Δψ為0.在虛擬力的作用下,UAV可以避免在跟蹤與相位調整過程中發生碰撞.存在障礙的τVFG示例如圖 6所示,從中可以觀察出障礙對矢量場制導方向的影響.

圖6　存在障礙的τVFGFig.6　 τVFG case with obstacle

3.3多機協同standoff跟蹤算法完整流程

在協同跟蹤過程中,各UAV需快速響應跟蹤,盡快運行至目標圓附近,且制導策略應符合UAV的應飛能力.由圖 2可知,通過對τVFG的耦合系數矢量k=[kt,kz,kθij]T的選取可以獲得期望的運動狀態間距閉合過程；因此在跟蹤的初始時刻,各UAV首先根據自身情況求解如下局部優化問題：

S.t.0

(21)

式中:Ji表示若以UAVi的位置、相位間隔閉合過程中的最大速度運行,達到目標狀態所需的平均時間,約束條件包括耦合系數的取值范圍和最大速度、轉彎角速率、爬升/下滑率的限制.由式(13)、(15)對τVFG制導策略的穩定性分析可知,對耦合系數矢量k的優化不會影響τVFG能夠嚴格按照時間約束導引運動狀態間距平穩閉合的特性；且式(21)所示優化問題僅考慮跟蹤響應的快速性和UAV的動力學約束,不會對算法的一般性帶來影響.本文采用序列二次規劃方法對k進行優化求解.在standoff跟蹤過程中,除‖U‖≥vamax外,不對k進行重新選取.

在每個控制周期,各UAV首先根據τVFG計算矢量場制導策略,而后根據角度間隔情況對旋轉項進行補償,最后判斷是否存在沖突并進行消解.

綜上所述，包含τVFG、多UAV協同相位調整、協同避碰避障的多機協同standoff跟蹤方法的完整流程如下：

1初始化各UAV的τVFG耦合系數矢量k

2 for 每個控制周期 do

3for 每一架UAV (i=1…N)do

4根據pri和時刻t依式(12)計算Ui

5根據與相鄰UAV的角度間隔依(17)調整Si(pr)

6根據距障礙和其它UAV距離計算Δψi,并旋轉Ui

7end for

8end for

4仿真結果與分析

目前針對多UAV協同standoff跟蹤問題尚未有標準測試用例,因此本文采用包含運動目標協同跟蹤,相位間隔調整,避障等任務的仿真對基于τVFG的standoff跟蹤算法性能進行驗證,并與目前standoff跟蹤中應用最廣泛的LVFG進行對比.

在仿真任務中,3架同型UAV協同對三維運動目標進行跟蹤,各UAV應飛速度vd=50 m/s,初始位置分別為p1=[-400,-400,20] m,p2=[400,-400,40] m,p3=[400,400,200] m,其余指標包括：vamax=90 m/s,vamin=10 m/s,ωmax=5 °/s,λmax=5 m/s.運動目標初始位置為pt=[0,0,0],在水平面內運動速度vt和運動方向ψt的變化如表 1所示,垂向運動速度為vtz=2 m/s.目標圓半徑為Rd=300 m,高度Hd=0 m,UAV之間期望相位間隔θd12=-2π/3,θd32=2π/3,安全間距Rsafe=25 m,跟蹤形成期望時間td=100 s,跟蹤保持段td=5 s.全部仿真實驗采用Matlab/Simulink R2013a設計,在有2.6GHz Core i5-3230M CPU和4GB RAM的計算機完成.

表1　目標運動速度和方向

圖7　τVFG作用下的UAV航跡Fig.7　UAV paths guided by τVFG

表2不同制導方法性能對比

Tab.2Performance comparison between the methods based onτVFG and LVFG

制導方法tc/(10-4s)perr/mθerr/(°)τVFG4.160.280.34LVFG6.886.201.27

圖8　各UAV距目標距離隨時間變化情況Fig.8　Distances between UAVs and moving target

圖9　各UAV相位間隔Fig.9　Phase differences between different UAVs

在跟蹤過程中各UAV的飛行速度vUAV如圖 10所示,因為UAV1和UAV3承擔了角度間隔調整的任務,同時目標在不斷變速運動,因而其速度并未保持應飛速度vd.由圖 10(a)、(b)對比可知,τVFG所提供的制導速度始終在vamax和vamin之間,而LVFG的制導速度則超出了UAV的飛行能力而被限幅.

各UAV與障礙之間的最小距離(d01,d02,d03)如表 3所示,τVFG可導引UAV安全地完成跟蹤任務,距障礙的距離大于安全飛行間距Rsafe=25 m,而LVFG則不能保證UAV的飛行安全,UAV1、UAV2均與障礙發生碰撞.在飛行過程中,各UAV之間均保持了安全飛行距離,未發生碰撞,此處不再贅述.

圖10　各UAV飛行速度Fig.10　Velocities of UAVs

制導方法d01/md02/md03/mτVFG27.130.527.8LVFG7.24.229.5

5結語

本文針對已有矢量場制導方法無法滿足standoff跟蹤中時間約束的缺陷,將時間信息融入矢量場中,利用廣義tau理論的tau-G制導策略構建了四維tau矢量場制導(τVFG),可導引UAV的位置嚴格按照期望時間收斂于目標圓.在多UAV協同standoff跟蹤應用中,用tau-G策略調整UAV之間的相位間隔,采用人工勢場法進行協同避碰避障,并運用序列二次規劃對τVFG和相位調整進行參數優化.與LVFG對比的仿真結果表明,本文所提出的方法具在計算負荷、跟蹤偏差、制導策略的可飛性、飛行安全性等方面均明顯占優,能夠更好地滿足多UAV協同standoff跟蹤的任務需求,同時也擴展了廣義tau理論在UAV領域的應用范圍.

在未來的工作中,將在τVFG中考慮包括UAV初始航向在內的更多約束,使τVFG應用于更多UAV協同跟蹤任務場景中.

參考文獻(References)：

[1] FREW E W, LAWRENCE D A, MORRIS S. Coordinated standoff tracking of moving targets using lyapunov guidance vector fields [J]. Journal of Guidance, Control, and Dynamics, 2008, 31(2): 290-306．

[2] WISE R A, RYSDYK R T. UAV coordination for autonomous target tracking [C]∥ Proceedings of the AIAA Guidance, Navigation, and Control Conference, Keystone: AIAA, 2006: 21-24．

[3] OH H, TURCHI D, KIM S, et al. Coordinated standoff tracking using path shaping for multiple UAVs [J]. IEEE Transactions on Aerospace and Electronic Systems, 2014, 50(1): 348-363．

[4] KIM S, OH H, and TSOURDOS A. Nonlinear model predictive coordinated standoff tracking of a moving ground vehicle [J]. Journal of Guidance, Control, and Dynamics, 2013, 36(2): 557-566．

[5] 王樹磊,魏瑞軒,郭慶,等. 面向協同standoff跟蹤問題的無人機制導律 [J]. 航空學報,2014,35(6): 1684-1693.

WANG Shu-lei, WEI Rui-xuan, GUO Qing, et.al. UAV guidance law for coordinated standoff target tracking [J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(6): 1684-1693．

[6] LAWRENCE D A. Lyapunov Vector Fields for UAV Flock Coordination [C]∥ 2nd AIAA “Unmanned Unlimited” Conf. and Workshop & Exhibit.[S.l]: AIAA, 2003.

[7] CHEN H, CHANG K, AGATE C S. A dynamic path planning algorithm for UAV tracking [C]∥ Proceedings of SPIE Signal Processing, Sensor Fusion, and Target Recognition XVIII. Orlando: SPIE, 2009: 1-10．

[8] CHEN H, CHANG K, AGATE C S. UAV path planning with tangent-plus-Lyapunov vector field guidance and obstacle avoidance [J]. IEEE Transactions on Aerospace and Electronic Systems, 2013, 49(2): 840-856．

[9] LEE D N. A theory of visual control of braking based on information about time-to-collision [J]. Perception, 1976, 5(4): 437-459．

[10] LEE D N, REDDISH P E. Plummeting gannets: a paradigm of ecological optics [J]. Nature, 1981, 293(1): 293-294．

[11] LEE D N, DAVIES M N, GREEN P R. Visual control of velocity of approach by pigeons when landing [J]. Journal of Experimental Biology, 1993, 180(1): 85-104．

[12] HECHT H ,SAVELSBERGH G J. Time-to-contact [M]. Amsterdam: Elsevier, 2004．

[13] LEE D N. General tau theory: evolution to date [J]. Perception, 2009, 38(6): 837-858．[14] 張書濤,張震,錢晉武. 基于Tau理論的機器人抓取運動仿生軌跡規劃 [J]. 機械工程學報,2014,50(13): 42-51.

ZHANG Shu-tao, ZHANG Zhen, QIAN Jin-wu. Bio-inspired trajectory planning for robot catching movements based on the tau theory [J]. Journal of Mechanical Engineering,2014, 50(13): 42-51．

[15] KENDOUL F. Four-dimensional guidance and control of movement using time-to-contact: Application to automated docking and landing of unmanned rotorcraft systems [J]. The International Journal of Robotics Research, 2014, 33(2): 237-267．

[16] ZHANG Z, XIE P, MA O. Bio-inspired trajectory generation for UAV perching movement based on tau theory [J]. International Journal of Advanced Robotic Systems, 2014, 11(1): 1-13．

[17] BOUSSON K ,MACHADO P F F. 4D trajectory generation and tracking for waypoint-based aerial navigation [J]. WSEAS Transactions on Systems & Control, 2013, 8(3): 105-119．

[18] SCHOGLER B, PEPPING G J, LEE D N. TauG-guidance of transients in expressive musical performance [J]. Experimental Brain Research, 2008, 189(3): 361-372．

[19] GOSS J, RAJVANSHI R, SUBBARAO K. Aircraft conflict detection and resolution using mixed geometric and collision cone approaches [C]∥ AIAA Guidance, Navigation, and Control Conference and Exhibit. Providence: AIAA, 2004: 1-20．

Cooperative standoff tracking for multi-UAVs based on tau vector field guidance

YANG Zu-qiang1, FANG Zhou1, LI Ping1,2

(1.SchoolofAeronauticsandAstronautics,ZhejiangUniversity,Hangzhou310027,China;2.CollegeofControlScienceandEngineering,Hangzhou310027,China)

Abstract:A four dimensional (4D) vector field guidance (VFG) method was presented for time-constrained cooperative standoff tracking of multiple unmanned aerial vehicles (UAVs). The tau VFG (τVFG) was proposed utilizing the 4D synchronous guidance capability of the intrinsic tau gravity (tau-G) guidance strategy. A comprehensive standoff tracking method was designed for multiple UAVs with the help of τVFG. The τVFG was applied to guide UAVs to approach the standoff circle exactly at the desired time, and tau-G guidance was also adopted for phase intervals adjustment. Tracking parameters were optimized by sequential quadratic programming, and conflicts were resolved by artificial potential fields. Simulation results show that the τVFG-based method performs better in cooperative standoff tracking tasks with a lower computation load, smaller tracking errors, better flyability and higher flight safety.

Key words:general tau theory; tau vector field guidance; standoff tracking; multi-UAV coordination

收稿日期：2015-10-17.浙江大學學報(工學版)網址： www.journals.zju.edu.cn/eng

基金項目：國家自然科學基金資助項目(61004066);浙江省自然科學基金資助項目(LY15F030005);浙江省公益性技術研究計劃資助項目(2016C33246).

作者簡介：楊祖強(1989-),男，博士生，從事多無人機協同規劃方法，飛行器控制與仿真槳研究.ORCID:0000-0001-5272-0390.E-mail:gaayzq@zju.edu.cn

通信作者：方舟，男，副教授.ORCID:0000-0002-0733-958X.E-mail:zfang@zju.edu.cn

DOI:10.3785/j.issn.1008-973X.2016.05.024

中圖分類號：V 249

文獻標志碼：A

文章編號：1008-973X(2016)05-0984-09