復雜環境下多無人機目標跟蹤與避障聯合航跡規劃

趙軍民, 何浩哲, 王少奇, 聶聰, 焦迎杰

(1.西北工業大學 航天學院, 陜西 西安 710072; 2.西安現代控制技術研究所, 陜西 西安 710065)

0 引言

經過幾十年的發展,無人機(UAV)技術日趨成熟,可以在復雜環境執行多樣化任務[1-2]。在偵察監視、對抗打擊、城市反恐和海上搜救等諸多任務場景中,UAV都需要對目標進行持續跟蹤[3-5]。隨著UAV間通信技術和協同控制技術的不斷進步,采用多架UAV協同執行目標跟蹤任務,可以增大對目標的觀測面積,提高對目標狀態的估計精度,提升對復雜環境的適應能力,因此受到廣泛關注[6-8]。

對于固定翼UAV,在多個UAV協同執行目標跟蹤任務時,常令它們按照一定的相位關系在目標上方具有指定半徑的Standoff圓上進行盤旋跟蹤,這種方式也稱為Standoff跟蹤。文獻[9]針對多UAV對地面目標進行Standoff跟蹤的航跡規劃問題,設計了兩個Lyapunov導引向量場(LGVF),控制各架UAV以期望相位關系收斂到目標上方的Standoff圓上,繞目標進行跟蹤,這種方法只適用于無障礙物存在的理想環境。

UAV在低空執行任務時,常存在影響飛行的障礙物,且具有密集性、非凸性、動態性和不確定性[10],因此在多UAV進行目標跟蹤時,需要進行避障航跡規劃。人工勢場(APF)法由于具有計算量小和航跡光滑等優點,已成為最常用的UAV避障航跡規劃算法之一[11]。但是由于在APF法中沒有障礙包絡的設定,依賴調整力場生成航跡,當力場參數調整不當時UAV有可能進入障礙內部,導致避障失敗[12]。在進行多UAV目標跟蹤和避障聯合航跡規劃時,有學者提出了基于擾動流場的航跡規劃方法[13-14],將目標跟蹤速度作為初始匯流速度,通過修正匯流速度得到的擾動流場速度即可作為UAV速度指令。還有學者對目標跟蹤與避障分別進行實時航跡規劃,再將兩種速度指令直接相加,得到UAV最終的速度指令[15]。然而,這種方法未考慮到避障任務事關UAV自身安全,比目標跟蹤任務具有更高的優先性,將兩種速度指令直接相加可能會減小UAV避障速度分量,導致在遇到密集型障礙物時UAV與障礙物距離過近,甚至進入障礙物內部出現避障失敗的情況。

針對無人系統的多任務行為控制問題,Arrichiello等提出零空間方法來綜合不同任務行為的控制指令[16]。零空間是指執行任務時不需要用到的維度空間,例如UAV在水平面內進行巡航飛行時,高度空間便是零空間。該方法強調將無人系統需要執行的任務進行劃分,并確定優先級。通過將低優先級任務向高優先級任務零空間進行投影的方式來得到處理后的低優先級任務行為,然后與高優先級任務行為進行相加。這樣可以保證高優先級任務行為不受低優先級任務行為的影響,同時部分執行低優先級任務行為[17]。

本文針對多個固定翼UAV在密集障礙環境中協同跟蹤地面目標的任務場景,提出一種基于零空間方法的目標跟蹤與避障聯合航跡規劃算法。首先,基于LGVF方法對多UAV進行Standoff跟蹤實時航跡規劃,得到使多個UAV能以期望相位關系收斂到Standoff圓上的速度指令,同時設計避障人工勢場函數,進行UAV避障實時航跡規劃,得到避障速度指令;然后,基于零空間方法進行目標跟蹤與避障聯合航跡規劃,通過將目標跟蹤速度指令向避障任務零空間投影后再與避障速度指令相加的方法得到UAV最終的速度指令。仿真結果表明,本文提出的聯合航跡規劃算法可使多UAV協同跟蹤地面目標并有效避障,提升了UAV執行目標跟蹤任務時的避障能力和安全性。

1 問題描述

多UAV對地面目標進行Standoff跟蹤時,一般在基準高度的二維平面上飛行,UAV最大速度高于目標移動速度。

UAV的運動模型為

(1)

式中:[x,y]T為UAV在慣性坐標系下的水平位置;vu、φu和ωu分別為UAV速度、航向角以及偏航角速度。

同時,對于固定翼UAV,需考慮以下約束[18]:

(2)

式中:vmax和vmin分別表示UAV的最大速度和最小速度;ωmax表示UAV的最大偏航角速度。

Standoff跟蹤的示意圖如圖1所示,其中Oxyz表示東北天坐標系,Otxtytzt表示以目標質心為原點、坐標軸方向與東北天坐標系相同的目標坐標系。UAV1、UAV2和UAV3共3架UAV均勻分布在Standoff圓上,對目標進行盤旋跟蹤。

圖1 多UAV Standoff跟蹤示意圖

多UAV在協同執行目標跟蹤任務時需要進行機間通信,以獲取機間相對運動信息和對目標的探測信息。多UAV的通信網絡可通過通信拓撲圖來描述。對單個地面目標進行跟蹤時使用的UAV數量通常不多于4架(一般為2～4架)[19],因此本文采用全連通的無向圖作為多UAV的通信拓撲,即任意兩個UAV之間可以相互收發信息。

對于飛行環境中的障礙物,其外形往往是不規則的。但對于多UAV目標跟蹤問題,可按規則外形包絡處理,如圓形(二維空間中)[20]、球形或橢球形(三維空間中)[21]。本文UAV在二維平面飛行,因此可將障礙物視為圓形。記障礙物中心坐標為[xobs,yobs]T,半徑為rmin,則UAV與障礙物中心的水平距離為

(3)

以障礙物中心作為避障人工勢場的中心,設定人工勢場的最大作用半徑為rmax。圖2為障礙物及避障人工勢場的示意圖,當robs≤rmax時,避障人工勢場產生作用;當robs≤rmin時,UAV與障礙物發生碰撞。

圖2 障礙物及人工勢場示意圖

在多UAV協同跟蹤地面運動目標時,一旦進入障礙物人工勢場的有效作用區域,則UAV既需要進行目標跟蹤航跡規劃,還需要進行避障航跡規劃。目標跟蹤與避障聯合航跡規劃是指通過對UAV同時進行目標跟蹤航跡規劃和避障航跡規劃得到速度指令,使UAV安全避開障礙物并持續跟蹤目標。

2 目標跟蹤與避障航跡規劃

通過目標跟蹤航跡規劃和避障航跡規劃分別得到UAV目標跟蹤與避障的實時速度指令,是在復雜環境下進行聯合航跡規劃的基礎。本節以LGVF方法作為多UAV目標跟蹤的航跡規劃算法,得到UAV跟蹤目標的速度指令,并構建避障人工勢場函數,利用APF方法進行UAV避障航跡規劃,得到UAV的避障速度指令。

2.1 基于LGVF的目標跟蹤航跡規劃

(4)

為保證這N架UAV對目標的最佳觀測配置,它們在Standoff圓上需要滿足以下相位關系[22]:

(5)

式中:θij為兩個相鄰UAVi和UAVj在對峙圓上的相位差。其中,UAVi在對峙圓上的相位θi是指從目標指向UAVi的向量與Otxt軸正向的夾角,其大小從Otxt軸正向逆時針計起,范圍為[0,2π)。θij的具體計算方法為

(6)

(7)

它們在Standoff圓上的相位關系如圖3所示。

圖3 UAV在Standoff圓上的相位關系

具體地,設UAVi(i=1,2,3)與目標的相對距離Lyapunov函數為

(8)

式中:R為Standoff圓半徑。相位差Lyapunov函數為

(9)

(10)

(11)

(12)

式中:v0為恒定的速度大小;k1和k3為相位系數。

為保證目標跟蹤速度指令式(10)的大小與式(12)期望速度大小一致,應有

(13)

式(10)便是基于LGVF方法進行多UAV協同目標跟蹤實時航跡規劃得到的UAV速度指令,將式(11)、式(12)和式(13)代入式(10),便可以得到各UAV目標跟蹤速度指令的具體值。

2.2 基于APF的避障航跡規劃

APF方法的核心是設計合適的障礙物模型和人工勢場函數[23]。在問題描述中障礙物模型的基礎上,構建如下避障人工勢場函數J(p)[24]:

(14)

式中:p=[x,y]T表示UAV水平位置;b0和c0為常數,分別決定人工勢場的幅值和變化速度。

(15)

式中:r=[x-xobs,y-yobs]T表示UAV與障礙物的相對位置。

若環境中存在多個密集障礙物,則UAV同時處于多個障礙物人工勢場的有效作用區域時,根據可加性原理,其受到的總虛擬排斥力為多個虛擬排斥力之和,即

(16)

式中:Ftotal(p)表示UAV受到的總虛擬排斥力;m表示對UAV產生虛擬斥力的障礙物總數。

以Ftotal(p)作為UAV的避障速度指令vavoidance,即

(17)

3 基于零空間方法的目標跟蹤與避障聯合航跡規劃

針對多UAV協同目標跟蹤與避障問題,將目標跟蹤與避障作為UAV需要執行的不同任務。避障關乎UAV安全,因此與目標跟蹤相比具有更高的任務優先級。本文目標跟蹤與避障聯合航跡規劃方法是分別進行這兩種航跡規劃得到兩個UAV速度指令,再基于零空間方法將這兩個速度指令進行聯合,得到UAV最終速度指令。

設避障任務函數為

(18)

式(18)對時間的導數為

(19)

式中:J1為避障任務函數Jacobi矩陣;vu為UAV速度矢量。

利用最小二乘法得到式(19)中vu的最小二乘解為

(20)

將式(19)代入上式,有

(21)

(22)

(23)

由式(23)可以看出,vNS不影響避障任務函數δ1,因此它是速度vu投影到避障任務零空間上的速度分量。

將式(21)代入vNS定義中,有

(24)

(25)

圖4 基于零空間方法的聯合航跡規劃示意圖

多UAV在障礙物存在的復雜環境中協同執行地面目標跟蹤任務的航跡規劃流程可總結為:如果UAV需要進行避障,則通過本文提出的聯合航跡規劃方法得到目標跟蹤與避障聯合速度指令;反之,則通過LGVF方法得到目標跟蹤速度指令。各個UAV的航跡規劃流程如圖5所示。

圖5 UAV航跡規劃流程示意圖

4 仿真驗證

4.1 仿真條件

考慮一類典型任務場景:3架UAV組成編隊在密集障礙物存在的環境中協同跟蹤地面運動目標。設定UAV運動約束為vmax=21 m/s,vmin=19 m/s,ωmax=0.2 rad/s。3架UAV初始位置如表1所示,目標初始位置為(100 m,100 m)。Standoff圓半徑R、UAV2速度大小常量v0、UAV1相位系數k1和UAV3相位系數k3,它們的具體取值如表2所示。分別對密集障礙環境中非機動目標和機動目標的跟蹤任務進行仿真,以驗證本文方法的合理性和有效性。

表1 UAV初始位置

表2 LGVF方法各參數取值

4.2 非機動目標跟蹤任務的聯合航跡規劃仿真

設地面目標從初始位置以速度(10 m/s,0 m/s)做勻速直線運動,環境中存在3個障礙物,位置如表3 所示,它們具有相同的障礙物半徑rmin=50 m和人工勢場最大作用半徑rmax=150 m。

表3 障礙物位置

在該任務場景中對本文提出的基于零空間方法的目標跟蹤與避障聯合航跡規劃方法進行仿真。結果表明,3架UAV可以避開密集障礙物,在Standoff圓上協同跟蹤非機動目標,如圖6(a)～圖6(e)所示。

圖6 協同跟蹤非機動運動目標的仿真結果

由圖6(a)和圖6(b)可以看出,3架UAV安全避開了3個密集障礙物。UAV1和UAV2在120 s左右收斂到Standoff圓上,UAV3在220 s左右收斂到Standoff圓上,如圖6(c)所示。3架UAV在Standoff圓上的相位差在250 s左右振蕩收斂到期望值,如圖6(d)所示。3架UAV在收斂到期望相位過程中出現相位差振蕩,是因為式(9)中相位差Lyapunov函數的導數并不能嚴格小于0。這是因為LGVF方法中得到的UAV目標跟蹤期望速度大小即式(12)實際應為UAV相對于目標的期望速度大小,而真實的目標跟蹤期望速度大小應為該速度大小與目標速度大小之和。但由于UAV存在最大速度約束,LGVF方法采取式(12)作為UAV期望速度大小。圖6(e)表明3架UAV之間的距離始終在安全范圍內,能夠避免發生碰撞。

4.3 機動目標跟蹤任務的聯合航跡規劃仿真

為驗證本文方法在機動目標跟蹤任務場景中的效果,考慮地面目標運動過程存在機動運動,具體如表4所示。環境中存在8個障礙物,前3個障礙物的位置和參數與4.2節相同,其余5個障礙物的位置和參數如表5所示。

表4 機動目標運動過程

表5 障礙物位置和參數

仿真結果表明,3架UAV可以避開密集障礙物,在Standoff圓上協同跟蹤機動目標,如圖7(a)～圖7(d)所示。

圖7 協同跟蹤機動運動目標的仿真結果

由圖7(a)可以看出,整個仿真過程中目標運動形式改變,3架UAV始終可以有效避開障礙物,持續跟蹤目標。由于避障時UAV速度指令不等于LGVF產生的目標跟蹤速度指令,圖7(b)中3架UAV與目標水平距離在UAV需要避障時出現波動,在避障完成后收斂到期望值。最終UAV1、UAV2和UAV3分別在335 s、360 s和400 s收斂到Standoff圓上。圖7(c)中UAV在Standoff圓上相位差在收斂到期望值過程中出現波動,同樣是由UAV進行避障引起的。避障完成后,UAV相位差振蕩收斂到期望值附近。由圖7(d)可以看出,3架UAV之間的距離始終都在安全范圍內,避免了機間碰撞。

4.4 障礙物半徑rmin和APF最大作用半徑rmax對算法性能的影響

在4.3節仿真條件基礎上,設障礙物7和障礙物8的半徑依次同時增大到100 m和150 m,其余參數不變。仿真得到不同障礙物半徑下3架UAV與障礙物7中心和障礙物8中心的最小距離分別如表6 和表7所示。

表6 不同rmin下各UAV與障礙物7中心的最小距離

表7 不同rmin下各UAV與障礙物8中心的最小距離

由表6和表7可以看出,在不同的障礙物半徑下,3架UAV與2個障礙物中心的距離都大于障礙物半徑,表明UAV可以避開障礙物對目標進行持續跟蹤。隨著rmin的增大,3架UAV與障礙物中心的距離也依次增大。這是因為APF的最大作用半徑rmax相同時,由式(15)可知,在APF作用范圍內的相同位置,rmin增大引起UAV避障速度增大,即UAV在障礙物法向方向的速度增大。

考慮UAV與障礙物邊緣的距離等于UAV與障礙物中心的距離減去障礙物半徑。由表6和表7可以看出,隨著rmin的增大,UAV與障礙物邊緣的距離減小,當rmin=150 m時該距離最小為2.04 m,UAV安全風險增大。這是因為rmin增大而rmax保持不變,在進入APF作用范圍后,UAV與障礙物中心距離的增幅小于rmin的增幅。因此,當障礙物半徑增大時,應增大APF最大作用半徑,使UAV在距離障礙物邊緣更遠的位置開始避障。

下面進一步分析rmax對算法性能的影響。在4.3節仿真條件基礎上,設障礙物7和障礙物8的半徑保持80 m不變,依次同時增大其rmax到220 m和300 m,其余參數不變。仿真得到不同rmax下3架UAV與障礙物7中心和障礙物8中心的最小距離分別如表8和表9所示,UAV在Standoff圓上的相位差如圖8所示。

表8 不同rmax下各UAV與障礙物7中心的最小距離

表9 不同rmax下各UAV與障礙物8中心的最小距離

圖8 不同rmax下UAV在Standoff圓上相位差

由表8和表9可以看出,rmax取不同值時,UAV與障礙物中心的距離始終大于障礙物半徑,UAV可以避障。增大rmax,APF作用范圍變大,UAV會在更大區域范圍內產生避障速度,UAV與障礙物中心的距離也隨之增大。由于障礙物半徑保持不變,UAV與障礙物邊緣的距離也隨之增大,UAV避障性能提升。由圖8可以看出,rmax取值160 m和220 m時,在避障過程中UAV相位差波動相對較小,而rmax取值300 m時,UAV相位差波動變大,目標跟蹤性能受到影響。這同樣是因為APF作用范圍變大,UAV避障過程變長,UAV需要在更大區域范圍內按照零空間方法得到的速度指令飛行,而不能按照目標跟蹤速度指令飛行。因此,在rmin已知時,選取rmax需要綜合考慮UAV避障性能和目標跟蹤性能,根據經驗,rmax取值一般是rmin的2～3倍。

4.5 仿真對比

為進一步說明本文基于零空間的目標跟蹤與避障聯合航跡規劃方法的優勢,將本文方法與已有的兩種方法進行對比。這兩種方法是將速度指令直接相加的聯合航跡規劃方法和基于擾動流場的聯合航跡規劃方法,下文分別簡稱為直接相加方法和擾動流場方法。在4.2節仿真條件下對這兩種方法進行仿真,直接相加方法和擾動流場方法的仿真結果分別如圖9和圖10所示。同時,計算整個仿真過程中本文方法和以上兩種方法下各UAV與障礙物中心的最小距離,結果如表10所示。

表10 3種方法下各UAV與障礙物中心的最小距離

圖9 直接相加方法仿真結果

圖10 擾動流場方法仿真結果

由圖9和圖10可以看出,直接相加方法和擾動流場方法均可以使多UAV繞開障礙物,對目標進行持續跟蹤。但是將圖6(d)、圖9(d)和圖10(d)進行對比可發現,在UAV收斂到期望相位位置的過程中,本文方法下UAV之間實際相位差在期望相位差附近的振蕩程度小于擾動流場方法下的振蕩程度,并與直接相加方法下的振蕩程度一致。這是因為本文方法和直接相加方法都是通過式(12)得到UAV目標跟蹤期望速度大小,而擾動流場方法則是通過擾動矩陣乘以式(12)得到期望速度大小。因此與本文方法相比,擾動流場方法下UAV目標跟蹤期望速度相對較小,導致其相位差Lyapunov函數的導數相對較大,具有更大的相位差振蕩程度。同時,由表10 可以看出,本文方法下3架UAV與障礙物的最小距離均大于直接相加方法下的距離,且略小于擾動流場方法下的距離。這是因為本文方法和擾動流場方法增大了UAV在障礙物法向方向上的速度分量,因此避障效果更好。

綜合以上分析可發現,擾動流場方法增大了UAV與障礙物的距離,同時也使UAV相位差振蕩程度變大,多UAV實際相位分布與期望分布差距較大;直接相加方法避障性能較差,但是具有相對較小的相位差振蕩程度。本文方法兼顧了UAV目標跟蹤相位協同分布和避障性能,在增大UAV與障礙物距離時依然能保證UAV在Standoff圓上的相位差具有較小振蕩。因此,綜合比較多UAV目標跟蹤協同效果和避障效果,本文所提方法具有最優性能。

5 結論

本文提出了一種密集障礙環境下多UAV目標跟蹤與避障聯合航跡規劃方法,針對3架UAV協同跟蹤地面運動目標的典型任務進行了仿真驗證。得出主要結論如下:

1) 構建的障礙物模型和避障人工勢場函數可用于實現UAV避障航跡規劃。

2) 基于零空間方法對目標跟蹤速度指令和避障速度指令進行綜合,能夠使多UAV在避開密集障礙物的同時,對目標進行持續跟蹤。

3) 與現有方法相比,本文方法在保證UAV的Standoff圓相位差在期望相位差附近具有較小振蕩的前提下增大了UAV與障礙物的距離,使UAV具有更好的目標跟蹤協同能力和避障能力,證明了本文方法的有效性。