動態威脅環境下單無人機測向定位航跡優化算法*

陳方正，郝紹杰

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

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

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

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

1 問題描述

1.1 測向定位的問題描述

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

XU,k+1=AXU,k+BUk

(1)

(2)

其中，Δt為狀態變化時間間隔。

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

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 測向定位在動態戰場環境下面臨的問題

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

1.2.1 敵方雷達探測威脅

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

(6)

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

1.2.2 障礙躲避

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

obj,k=Rj,k-rj>0

(7)

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

2 基于DWA的多步預測方法

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

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

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

本文以FIM行列式最大作為UAV測向定位航跡優化的準則，以提高對目標的定位精度。基于DWA多步預測的思想，需要解決對預測航跡的評價問題。文獻[14]推導了無過程噪聲條件下FIM的遞推計算方法：

(8)

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

(9)

JFIM=det(FIMk+n)

(10)

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

(11)

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

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

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

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

(12)

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

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

(13)

3 基于DWA的單UAV測向定位航跡優化算法

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

算法1 基于DWA的單UAV測向定位航跡優化算法

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

4 仿真實驗

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

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

(14)

表1 不同航跡最終定位結果

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

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

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

4.2 動態復雜障礙條件下航跡對比

在仿真環境中引入障礙和雷達干擾，其中兩個固定障礙半徑均為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仿真，結果如表2所示，其中航跡2-2～2-4均達到收斂表1中相同的收斂精度，說明動態環境的引入沒有影響最終的定位性能。而航跡2-1由于與障礙發生碰撞未完成定位任務，因此無最終定位精度。

表2 不同航跡定位結果

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

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

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

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

仿真結果表明，本文提出的算法能夠在雷達探測威脅和動態障礙環境的影響下依然保證較高的測向定位精度，實現了UAV在動態戰場環境下對目標的測向定位航跡優化。

5 結論

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