動態(tài)威脅環(huán)境下單無人機測向定位航跡優(yōu)化算法*

陳方正，郝紹杰

(中國電子科技集團公司第四十一研究所， 山東 青島 266555)

隨著無人機(Unmanned Aerial Vehicles，UAV)技術(shù)的發(fā)展，將UAV平臺作為機動觀測站，搭載測向設(shè)備對敵方目標進行探測定位越發(fā)受到重視。單站測向定位是指利用運動的單個觀測站在不同位置測得的目標方位角信息[1]，結(jié)合某種估計算法實現(xiàn)對目標的快速定位和跟蹤。測向手段既可以是傳統(tǒng)意義上的無線電測向，也可以是利用光電設(shè)備的測向[2]。利用UAV等單觀測站進行測向定位航跡優(yōu)化問題的本質(zhì)是研究如何通過優(yōu)化觀測平臺的運動軌跡來提升對目標預(yù)測的可觀測性問題。目前比較有代表性的方法包括，基于Fisher信息矩陣(Fisher Information Matrix, FIM)及其變形形式的最優(yōu)航跡規(guī)劃算法[3-6]，以方位角度變化率最大為目標的航跡優(yōu)化算法[7]，以定位誤差幾何因子(Geometrical Dilution Of Precision，GDOP)最小化為目標的最優(yōu)定位航跡優(yōu)化算法[8]，以濾波協(xié)方差最小為準則的航跡優(yōu)化算法[9]，以及將上述多種目標函數(shù)結(jié)合的航跡優(yōu)化算法[10]等。

然而這些研究大多基于理想空間條件下，即未考慮UAV等單觀測站定位航跡受威脅和障礙等約束條件的影響，限制了單站測向定位的實際應(yīng)用，因此近年來結(jié)合復(fù)雜的飛行環(huán)境，進行測向定位航跡優(yōu)化成為研究的熱點。He等[11]研究了在威脅障礙環(huán)境下機動單站對靜止和機動目標定位的最優(yōu)機動策略。吳昊等[12]考慮多種戰(zhàn)場威脅約束，通過打分函數(shù)引導(dǎo)路徑選擇，得到在約束條件下的UAV最優(yōu)定位航跡。由于定位問題的特殊性，事先并不能知道目標的確切位置，因此需要UAV等單觀測站邊定位邊規(guī)劃。然而以往關(guān)于單站測向定位航跡優(yōu)化的研究都是基于靜態(tài)環(huán)境下的單步最優(yōu)原則。在動態(tài)戰(zhàn)場環(huán)境下，單步最優(yōu)原則使得UAV的預(yù)判信息極為有限，可能導(dǎo)致UAV由于機動能力不足而與障礙發(fā)生碰撞。

因此本文以單架UAV作為測向定位平臺，針對動態(tài)戰(zhàn)場環(huán)境下的測向定位問題，基于動態(tài)窗口法(Dynamic Window Approach，DWA)思想，提出多步預(yù)測求解最優(yōu)航跡的算法，在線生成滿足UAV運動學(xué)約束的航跡，解決單架UAV在動態(tài)戰(zhàn)場環(huán)境下的測向定位航跡優(yōu)化問題。

1 問題描述

1.1 測向定位的問題描述

考慮到進行測向定位時目標與UAV之間的高度差異遠小于水平方向的距離，因此將UAV測向定位簡化為二維平面中的定位問題，如圖1所示。其中：oxy為地面慣性坐標系；XT為待定位目標，其坐標為[xT,yT]T；XU,k代表UAV，k為時間步，Vk為其速度矢量，ηk為其觀測的目標的方位角。UAV離散形式運動學(xué)模型可寫為：

XU,k+1=AXU,k+BUk

(1)

(2)

其中，Δt為狀態(tài)變化時間間隔。

對于測向定位問題，假設(shè)目標靜止，不考慮過程噪聲，選取XT為待估計的狀態(tài)變量，則目標離散狀態(tài)方程可寫為：

XT,k+1|k=Φk+1|kXT,k|k

(3)

其中

(4)

圖1 測向定位示意圖Fig.1 Illustration for bearing-only target localization

由圖1可知，UAV對目標的方位觀測方程可表示為：

(5)

式中，εk是均值為0，方差為σ2的高斯白噪聲序列。

1.2 測向定位在動態(tài)戰(zhàn)場環(huán)境下面臨的問題

典型戰(zhàn)場環(huán)境包括：敵方雷達探測威脅、火力威脅、地形威脅、建筑等障礙物威脅以及危險天氣威脅等各種威脅因素。以往關(guān)于定位航跡優(yōu)化的研究中都是針對靜態(tài)環(huán)境[11-12]，即威脅因素狀態(tài)固定，這與實際差異較大。本文中將威脅因素歸納為雷達探測威脅和障礙物威脅，研究在動態(tài)戰(zhàn)場環(huán)境下雷達突然開關(guān)機和障礙物機動時UAV的最優(yōu)測向定位航跡。

1.2.1 敵方雷達探測威脅

被敵方雷達捕獲會使UAV處于危險之中。雷達能否探測到UAV與其反射信號的信噪比有直接影響。根據(jù)文獻[12]對于確定的目標和雷達，其信噪比SN可表示為：

(6)

式中，下標i表示雷達編號，di為雷達與UAV之間的距離，dmax,i為雷達最大探測距離，bi為常數(shù)。UAV飛行過程中應(yīng)當盡量避免被雷達探測到。動態(tài)戰(zhàn)場環(huán)境下，探測雷達可能突然開機，而若此時UAV正好處于雷達探測范圍中，UAV飛行航跡應(yīng)能使SN盡快降低。

1.2.2 障礙躲避

UAV處于火力威脅、地形威脅、建筑等障礙物威脅以及危險天氣威脅之中時將會導(dǎo)致其墜毀，因此可以將上述威脅均等效為障礙物，UAV不能與其碰撞。障礙物既可以是固定的，也可以是不斷運動的。在二維環(huán)境下，本文將不同障礙物均等效為不同直徑的圓形固定障礙或者可動障礙，UAV在飛行過程中時刻需要對這些障礙進行躲避，避免與障礙物相撞，即滿足：

obj,k=Rj,k-rj>0

(7)

式中，j為障礙物編號，Rj,k為UAV與障礙物中心的距離，rj為障礙物半徑，obj,k為UAV與障礙物邊緣的距離。

2 基于DWA的多步預(yù)測方法

在以往關(guān)于單站測向定位航跡優(yōu)化的研究中多是基于單步最優(yōu)機動策略。如圖2所示，即根據(jù)觀測站的機動能力和單位時間運動距離，在可行航向區(qū)間內(nèi)離散若干航點，根據(jù)定位最優(yōu)準則，從這些航點中選擇最有利于快速定位的航向。然而這種方法在障礙環(huán)境中預(yù)判信息極為有限，不能充分利用觀測站的機動性提前躲避障礙，可能會導(dǎo)致UAV因機動能力不足而與障礙碰撞。

圖2 單步最優(yōu)機動策略示意圖Fig 2 Illustration for one-step optimal maneuver strategy

DWA算法的主要思想是在速度空間中選擇不同的線速度和角速度組合作為控制量，基于UAV運動學(xué)模型，計算在模擬預(yù)測時間內(nèi)可能的飛行航跡集合，通過對這些航跡進行評價，選擇最優(yōu)航跡的控制量作為下一步的控制目標。DWA因?qū)崟r性強和直接考慮機器人運動學(xué)約束等特性而被廣泛應(yīng)用于動態(tài)復(fù)雜環(huán)境下的機器人的障礙威脅規(guī)避[13]。DWA可以有效增加測向UAV的預(yù)判信息，解決復(fù)雜障礙環(huán)境下的UAV避碰和定位問題。

本文以FIM行列式最大作為UAV測向定位航跡優(yōu)化的準則，以提高對目標的定位精度?；贒WA多步預(yù)測的思想，需要解決對預(yù)測航跡的評價問題。文獻[14]推導(dǎo)了無過程噪聲條件下FIM的遞推計算方法：

(8)

式中，F(xiàn)IMk為Fisher信息矩陣，Rk為觀測噪聲序列εk的協(xié)方差矩陣，Hk為觀測方程h(·) 的雅可比矩陣，表達式為：

(9)

JFIM=det(FIMk+n)

(10)

敵方雷達威脅評價函數(shù)由式(11)計算，為UAV在整條預(yù)測航跡上被q個雷達探測的SN值之和。該評價函數(shù)使UAV更傾向于選擇不容易被發(fā)現(xiàn)的航跡，若UAV不在雷達探測范圍內(nèi)，則航跡不受影響。

(11)

圖3 預(yù)測碰撞時間計算示意圖Fig.3 Calculation of predicted collision time

障礙躲避采用與碰撞時間相關(guān)的評價函數(shù)[15]。如圖3所示，假設(shè)UAV可以通過自身傳感器估計障礙物的運動速度和方向[16]，則可以對其運動航跡進行預(yù)測，基于UAV和障礙物每一時刻的位置，通過式(7)可以判斷兩者發(fā)生碰撞的時間。對于UAV附近的多個障礙物，需依次進行判斷，并選擇最小碰撞時間作為該條預(yù)測航跡的避碰評價函數(shù)JT。在所有的預(yù)測航跡中，選擇預(yù)計碰撞時間最晚的航跡有利于UAV躲避障礙，因此以JT最大為避碰準則。

綜合考慮定位、雷達威脅和避碰因素，一條航跡的總評價函數(shù)為：

Jp=c1JFIM,p+c2JSN,p+c3JT,p

(12)

式中，c1=0.2，c2=0.3，c3=0.5為權(quán)重因子，p為預(yù)測航跡數(shù)。考慮到無人機避障和躲避雷達威脅是其生存進而完成定位任務(wù)的前提，因此無人機避障和雷達躲避的評價指標權(quán)重系數(shù)較大。多條航跡構(gòu)成目標函數(shù)集合，則

J=[J1,J2,…,Jp]T

(13)

3 基于DWA的單UAV測向定位航跡優(yōu)化算法

基于DWA的單UAV測向定位航跡優(yōu)化算法如算法1所示，其中P為擴展卡爾曼濾波(Extended Kalman Filter， EKF)過程中的估計誤差方差矩陣。

算法1 基于DWA的單UAV測向定位航跡優(yōu)化算法

算法1中，第2～12步通過基于DWA的UAV航跡優(yōu)化算法形成下一步控制指令。由于該控制指令是由對最優(yōu)航跡的評價得到的，因此包含了多步預(yù)判的信息。第13步UAV根據(jù)控制指令機動。第14步UAV基于新的測向值，使用EKF進行目標狀態(tài)估計；第15步UAV更新FIM。如此反復(fù)迭代直至UAV定位誤差收斂。

4 仿真實驗

4.1 無障礙條件下定位航跡對比

首先考察定位目標函數(shù)的有效性，飛行環(huán)境中不考慮障礙物和雷達威脅，此時JSN為0，JT為固定常數(shù)，UAV航跡優(yōu)化目標函數(shù)僅與JFIM有關(guān)。分析預(yù)測步數(shù)不同對航跡形狀和定位誤差收斂過程的影響，并與一般勻速直線航跡進行比較。每種條件進行50次Monte-Carlo仿真，每次測量平均定位誤差均方差(Root Mean Square Error, RMSE)為：

(14)

表1 不同航跡最終定位結(jié)果

圖4 平均定位誤差收斂曲線Fig 4 Convergence curve for RMSE

圖5 不同航跡對比Fig.5 Comparison of different trajectories

結(jié)合表1和圖4可知，當k≤200時，航跡1-1比航跡1-2 RMSE收斂速度快，但是最終定位精度卻最低。而優(yōu)化航跡1-3～1-6不但前期RMSE收斂速度快，并且能夠達到更高的定位精度，這說明評價函數(shù)JFIM對于提升UAV測向定位精度是有效的。圖4中，當k≥300時，n值越大，RMSE收斂加快，這說明增加預(yù)測步數(shù)n有利于進一步改善UAV的定位收斂過程。

4.2 動態(tài)復(fù)雜障礙條件下航跡對比

在仿真環(huán)境中引入障礙和雷達干擾，其中兩個固定障礙半徑均為r1=r2=1 km，位置分別為Xob,1=[2 km,2 km]T，Xob,2=[12 km,4 km]T，運動障礙半徑為r3=1 km，初始位置為Xob,3=[8 km,2 km]T，沿x軸做速度為Vob=12 m/s的勻速直線運動，雷達位置為Xradar=[6 km,2 km]T，最大探測半徑為dmax,1=4 km。其他仿真條件不變，每種條件同樣進行50次Monte-Carlo仿真，結(jié)果如表2所示，其中航跡2-2～2-4均達到收斂表1中相同的收斂精度，說明動態(tài)環(huán)境的引入沒有影響最終的定位性能。而航跡2-1由于與障礙發(fā)生碰撞未完成定位任務(wù)，因此無最終定位精度。

表2 不同航跡定位結(jié)果

不同時刻航跡如圖6所示。當k=150 時，各條航跡如圖6(a)所示。此時雷達未開機，不會對UAV航跡產(chǎn)生影響。對于航跡2-1，n=1為單步最優(yōu)原則。當UAV預(yù)測到將與障礙物碰撞時，由于其機動能力有限，無法產(chǎn)生有效規(guī)避，從而導(dǎo)致其最終與障礙物發(fā)生碰撞。對于航跡2-2～2-4，均未與固定障礙1發(fā)生碰撞，并且n越大，預(yù)知信息越多，航跡越平滑。當k=290時，各條航跡如圖6(b)所示。雷達突然開機時，UAV均在雷達探測范圍內(nèi)，此時UAV迅速轉(zhuǎn)向逃離雷達探測邊界，從而減小被雷達探測到的概率。在k=290～330范圍內(nèi)，航跡2-2～2-4依次與運動障礙相遇，由于UAV能夠?qū)φ系K物運動進行預(yù)判，因此3條航跡均能成功避開運動障礙物，整個運動過程未發(fā)生碰撞，如圖6(b)～(d)所示。

當定位結(jié)束時，如圖6(e)所示，各條航跡均收斂到目標附近，定位過程結(jié)束。比較航跡2-2～2-4發(fā)現(xiàn)，n值越大，UAV控制信息中包含的預(yù)測信息越多，航跡越平滑。

(a) k=150時刻航跡(a) Trajectories at k=150

(b) k=290時刻航跡(b) Trajectories at k=290

仿真結(jié)果表明，本文提出的算法能夠在雷達探測威脅和動態(tài)障礙環(huán)境的影響下依然保證較高的測向定位精度，實現(xiàn)了UAV在動態(tài)戰(zhàn)場環(huán)境下對目標的測向定位航跡優(yōu)化。

5 結(jié)論

本文基于DWA多步預(yù)測的思想，提出了一種應(yīng)用于動態(tài)戰(zhàn)場環(huán)境下的單站測向定位航跡優(yōu)化算法。該方法將測向定位目標函數(shù)由單步最優(yōu)原則擴展到對多步預(yù)測航跡的評價，同時考慮對雷達探測威脅和運動障礙的躲避，在線生成滿足UAV運動學(xué)約束的航跡。仿真結(jié)果表明，增加預(yù)測步數(shù)，一方面有助于提升測向定位的收斂速度，另一方面由于增加了UAV對障礙的預(yù)判信息，使其更早地對障礙物(包括運動障礙)進行規(guī)避。航跡優(yōu)化算法使UAV在復(fù)雜動態(tài)環(huán)境的影響下依然能夠完成高精度測向定位任務(wù)。