許劍錕，金國(guó)棟，譚力寧，許劍鋒，薛遠(yuǎn)亮

(火箭軍工程大學(xué)，西安 710000)

0 引言

無人機(jī)高精度目標(biāo)定位在軍事偵察、精準(zhǔn)打擊、搶險(xiǎn)救災(zāi)等方面具有迫切的應(yīng)用需求[1-2]。目前常用的目標(biāo)定位方法是基于空間幾何關(guān)系，利用無人機(jī)姿態(tài)、位置信息，以及光電平臺(tái)獲取的無人機(jī)與目標(biāo)相對(duì)位置關(guān)系，通過齊次坐標(biāo)轉(zhuǎn)換進(jìn)行目標(biāo)位置解算[3-4]。受設(shè)備制造水平、應(yīng)用環(huán)境等影響，該方法引入了大量測(cè)量誤差，需要使用優(yōu)化或?yàn)V波的方法來抑制噪聲影響，提高目標(biāo)定位精度[2-4]。卡爾曼濾波(KF)及其衍生算法是便于計(jì)算機(jī)實(shí)現(xiàn)的遞推最優(yōu)估計(jì)方法[5]。穆紹碩等[6]提出了一種基于擴(kuò)展卡爾曼濾波(EKF)的算法進(jìn)行迭代優(yōu)化，比采用地球橢球模型的算法精度有了明顯提高；鄭藝等[7]提出滑動(dòng)后向遞推的EKF算法，與傳統(tǒng)濾波向前遞推結(jié)合，增加數(shù)據(jù)的反復(fù)利用,降低濾波誤差；徐誠(chéng)等[8]采用無跡卡爾曼濾波(UKF)處理視軸角和目標(biāo)位置之間的非線性關(guān)系，減少了因泰勒展開截?cái)嘣斐傻恼`差；唐大全等[9]提出了一種由極大似然估計(jì)法確定迭代條件的自適應(yīng)迭代無跡卡爾曼濾波(Adaptive Iterative Unscented Kalman Filter，AIUKF)算法，提高了濾波收斂效率。以上方法對(duì)傳統(tǒng)卡爾曼濾波算法都做出了一定改進(jìn)，提高了濾波性能，但都是在高斯噪聲模型的前提下，在非高斯噪聲情況下，濾波估計(jì)誤差會(huì)增大，甚至?xí)V波發(fā)散。

平滑變結(jié)構(gòu)濾波(Smooth Variable Structure Filter，SVSF)是一種次優(yōu)估計(jì)算法，它運(yùn)用變結(jié)構(gòu)和滑?？刂聘拍睿瑢⒐烙?jì)值限制在存在子空間內(nèi)，使得預(yù)估值逐步逼近真實(shí)值，對(duì)系統(tǒng)受到大擾動(dòng)、噪聲不符合高斯分布等情況具有較好魯棒性[10-11]。

為了克服卡爾曼濾波對(duì)噪聲高斯特性的限制，本文設(shè)計(jì)了平滑變結(jié)構(gòu)和卡爾曼組合濾波算法，有效提高了大擾動(dòng)、噪聲不確定情況下目標(biāo)定位的精度和算法魯棒性。

1 測(cè)向交叉定位原理及數(shù)學(xué)模型

微小型無人機(jī)在對(duì)目標(biāo)定位過程中，一般采用測(cè)向交叉的目標(biāo)定位方法，即無人機(jī)在多點(diǎn)對(duì)目標(biāo)進(jìn)行觀測(cè)，觀測(cè)直線交點(diǎn)為目標(biāo)位置點(diǎn)，如圖1所示。

圖1 測(cè)向交叉目標(biāo)定位Fig.1 Target location by direction cross

無人機(jī)在發(fā)現(xiàn)目標(biāo)后，通過跟蹤算法和伺服系統(tǒng)，可以將目標(biāo)鎖定在攝像機(jī)視場(chǎng)中心，再利用攝像機(jī)的方位角、高低角和無人機(jī)的姿態(tài)角、位置坐標(biāo)等數(shù)據(jù)，機(jī)載計(jì)算機(jī)或地面站即可解算出目標(biāo)的位置。

1.1 視軸方位角計(jì)算

合理的輔助坐標(biāo)系能夠方便計(jì)算。定義輔助坐標(biāo)系如下：無人機(jī)站心坐標(biāo)系，又稱地理坐標(biāo)系，簡(jiǎn)記為N系，坐標(biāo)原點(diǎn)為全球定位系統(tǒng)(GPS)接收機(jī)中心，采用北東地表達(dá)方式；載機(jī)慣性坐標(biāo)系，簡(jiǎn)記為I系，坐標(biāo)原點(diǎn)為慣性測(cè)量單元(IMU)中心，載機(jī)航向角φ、俯仰角θ、橫滾角γ，代表該坐標(biāo)系相對(duì)站心坐標(biāo)系的3個(gè)姿態(tài)角；攝像機(jī)坐標(biāo)系，簡(jiǎn)記為C系，坐標(biāo)原點(diǎn)為攝像機(jī)光心，Z軸與攝像機(jī)光軸重合，指向目標(biāo)，光軸指向角以方位角η和高低角?表示。

以站心坐標(biāo)為基準(zhǔn)定義視軸角(α,β)，α為視軸矢量與站心坐標(biāo)系Z軸夾角，稱為視軸高低角；β為視軸矢量在水平面投影與X軸的夾角，稱為視軸方向角。

假定目標(biāo)被鎖定在視場(chǎng)中心，則其在C系坐標(biāo)可表示為tC=(00f),其中，f為攝像機(jī)焦距。根據(jù)坐標(biāo)齊次轉(zhuǎn)換理論，目標(biāo)在站心坐標(biāo)系下的坐標(biāo)可表示為

(1)

根據(jù)式(1)和圖2，視軸角可表示為

圖2 視軸角計(jì)算示意圖Fig.2 The calculation of LOS angle

(2)

1.2 濾波方程建立

無人機(jī)發(fā)現(xiàn)目標(biāo)后，在目標(biāo)上空進(jìn)行盤旋飛行，進(jìn)行多次拍攝測(cè)量。假設(shè)無人機(jī)對(duì)目標(biāo)共進(jìn)行K次測(cè)量，第k次測(cè)量時(shí)，無人機(jī)的位置為(xUAV,yUAV,zUAV)，目標(biāo)位置為(xT,yT,zT)。采用狀態(tài)空間模型法構(gòu)建狀態(tài)方程，選取離散狀態(tài)變量xk=(xTyTzT)k，則系統(tǒng)狀態(tài)方程為

xk+1=f(xk)+wk

(3)

式中:f(·)為狀態(tài)轉(zhuǎn)移函數(shù)；wk為過程噪聲，wk～N(0,Q)，Q為噪聲協(xié)方差矩陣。對(duì)于固定目標(biāo)，f(xk)=xk，Q=0n×n，0n×n代表n×n的零矩陣，n為狀態(tài)向量的維數(shù)。

根據(jù)圖2，由目標(biāo)點(diǎn)和無人機(jī)之間的位置關(guān)系得系統(tǒng)量測(cè)方程為

zk=h(xk)+vk

(4)

(5)

其中，vk為觀測(cè)噪聲，vk～N(0,R)，且與系統(tǒng)噪聲相互獨(dú)立，R為量測(cè)噪聲協(xié)方差矩陣。

2 平滑變結(jié)構(gòu)-卡爾曼濾波算法

卡爾曼濾波是一種實(shí)時(shí)性高的最優(yōu)估計(jì)算法，在目標(biāo)定位方面有著廣泛的應(yīng)用，但在系統(tǒng)受到大擾動(dòng)或者噪聲非高斯分布時(shí)，易出現(xiàn)精度下降甚至發(fā)散的問題。平滑變結(jié)構(gòu)濾波是一種次優(yōu)估計(jì)算法，其優(yōu)勢(shì)是在應(yīng)對(duì)模型不準(zhǔn)確和未知噪聲時(shí)有較好的魯棒性和穩(wěn)定性。本文將卡爾曼濾波和平滑變結(jié)構(gòu)濾波進(jìn)行融合，設(shè)計(jì)了平滑變結(jié)構(gòu)-卡爾曼組合濾波算法，保持了卡爾曼濾波的精度優(yōu)勢(shì)，改善了濾波的魯棒性。

2.1 平滑變結(jié)構(gòu)濾波

SVSF算法基本思想是，通過使用SVSF增益，將估計(jì)狀態(tài)限制在存在子空間內(nèi)，沿著真值軌跡來回切換，如圖3所示。

圖3 平滑變結(jié)構(gòu)濾波示意圖Fig.3 Schematic diagram of smooth variable structure filter

首先，進(jìn)行一步預(yù)測(cè)，即

(6)

(7)

(8)

第二步，求解SVSF增益并進(jìn)行更新，即

(9)

(10)

(11)

平滑有界層寬度矩陣ψk的定義為

(12)

飽和函數(shù)的定義為

(13)

式中，vki表示vk中第i個(gè)元素，ψkii為ψk中第i個(gè)主對(duì)角線元素，二者相除構(gòu)成飽和運(yùn)算列向量的第i個(gè)元素，i=1,…,s,s為vk的維數(shù)。

2.2 平滑變結(jié)構(gòu)-卡爾曼組合濾波

由于1.2節(jié)構(gòu)建的量測(cè)方程具有較強(qiáng)的非線性，采用泰勒展開的方式存在截?cái)嗾`差會(huì)影響濾波精度，因此，本文采用UKF與SVSF的組合濾波。UKF算法已很成熟，不再介紹。

由式(12)ψk的定義可以看出，其大小直接與系統(tǒng)誤差和系統(tǒng)模型有關(guān)。因此，可以利用ψk來評(píng)判系統(tǒng)當(dāng)前模型不確定性和噪聲大小。為了兼顧卡爾曼濾波的精度，本文設(shè)計(jì)以切換增益的策略，構(gòu)建SVSF和卡爾曼組合濾波。當(dāng)ψk小于預(yù)設(shè)值ψmin，則當(dāng)前系統(tǒng)模型較準(zhǔn)確、噪聲較小，此時(shí)采用卡爾曼增益，獲取最優(yōu)估計(jì)保證濾波精度；當(dāng)ψk大于預(yù)設(shè)值ψmin，當(dāng)前系統(tǒng)模型不夠準(zhǔn)確、噪聲較大，則采用SVSF增益，獲取次優(yōu)估計(jì)保證濾波穩(wěn)定性。組合濾波流程如圖4所示。其中，ψmin為經(jīng)驗(yàn)數(shù)據(jù)，可利用統(tǒng)計(jì)學(xué)方法從歷史數(shù)據(jù)中獲取。

圖4 平滑變結(jié)構(gòu)-卡爾曼組合濾波流程圖Fig.4 Flow chart of SVSF-KF

在計(jì)算量相當(dāng)?shù)那闆r下，UKF的無跡變換(UT)的結(jié)果比泰勒展開的近似更為準(zhǔn)確。因此，對(duì)組合濾波預(yù)測(cè)階段統(tǒng)一使用UKF的方法，式(6)～(8)可變?yōu)?/p>

(14)

(15)

(16)

同時(shí)，為了提高平滑有界層寬度計(jì)算精度，將UKF量測(cè)協(xié)方差代入式(12)得

停藥反跳：服用氫化可的松、強(qiáng)的松等治療風(fēng)濕、類風(fēng)濕性關(guān)節(jié)炎時(shí)，吃維生素E既可助激素一臂之力，增強(qiáng)療效，又可防止和減少激素撤停時(shí)疾病“反跳現(xiàn)象”的發(fā)生。

(17)

(18)

SVSF增益及量測(cè)更新仍按照式(9)～(11)計(jì)算。當(dāng)采用UKF增益時(shí)，則按照標(biāo)準(zhǔn)UKF算法計(jì)算。

2.3 基于殘差自適應(yīng)的組合濾波方法

在組合濾波算法中，無論是采用SVSF增益還是UKF增益，Rk都將影響增益大小，在先驗(yàn)估計(jì)不準(zhǔn)確的情況下，會(huì)造成濾波精度下降甚至發(fā)散。因此,需對(duì)噪聲協(xié)方差矩陣進(jìn)行實(shí)時(shí)在線調(diào)整,使噪聲協(xié)方差矩陣可以根據(jù)系統(tǒng)狀態(tài)進(jìn)行自適應(yīng)調(diào)節(jié)。

根據(jù)已有文獻(xiàn)可知，在Qk和Rk非同時(shí)變化時(shí)，可以利用新息或者殘差序列對(duì)Rk進(jìn)行實(shí)時(shí)估計(jì)[12-13]。文獻(xiàn)[12]利用新息序列實(shí)現(xiàn)了對(duì)量測(cè)噪聲的實(shí)時(shí)跟蹤。然而，此方法存在兩項(xiàng)相減，可能會(huì)使Rk失去正定性。參考文獻(xiàn)[13]，本文使用殘差序列對(duì)Rk進(jìn)行在線估計(jì)。

由殘差定義，根據(jù)開窗估計(jì)法和誤差傳播理論，可得卡爾曼濾波Rk的在線估計(jì)為

(19)

Pk=Pk|k-1-KPzzKT

(20)

式中，K為UKF增益。

將式(18)和式(20)代入式(19)，得組合濾波算法Rk的實(shí)時(shí)估計(jì)為

(21)

3 實(shí)驗(yàn)驗(yàn)證

3.1 第一組實(shí)驗(yàn)：大擾動(dòng)情況

第一組實(shí)驗(yàn)主要驗(yàn)證算法在系統(tǒng)受到大擾動(dòng)情況下的性能。實(shí)驗(yàn)基本條件設(shè)置為：靜止目標(biāo)真實(shí)位置為(350 m,450 m,8 m)，無人機(jī)在發(fā)現(xiàn)目標(biāo)后，盤旋上升飛行，飛行半徑r=100 m,圓心坐標(biāo)為(500 m,600 m,400 m)，初始航高為400 m，盤旋飛行角速度為0.02 rad/s、上升速度為1 m/s，無人機(jī)自身定位誤差服從均值為0°、標(biāo)準(zhǔn)差為5°的正態(tài)分布，無人機(jī)繞飛一周，均勻選取180個(gè)采樣點(diǎn)。系統(tǒng)方程如式(3)和式(4)，Q矩陣為零矩陣，R=diag(1,1)。目標(biāo)初始狀態(tài)估計(jì)為(330 m,480 m,0 m)，初始狀態(tài)協(xié)方差設(shè)置為P0=diag(100,100,100)。

實(shí)驗(yàn)1參數(shù)設(shè)置:視軸角噪聲與估計(jì)模型一致，未受到擾動(dòng)，整個(gè)測(cè)量過程中，視軸角誤差服從均值為0°、1倍標(biāo)準(zhǔn)差的正態(tài)分布。

實(shí)驗(yàn)2參數(shù)設(shè)置:視軸角噪聲與估計(jì)模型一致，但受到了大擾動(dòng)，在120～150采樣點(diǎn)時(shí)間段內(nèi)，視軸角標(biāo)準(zhǔn)差由1倍變?yōu)?倍，其他設(shè)置與實(shí)驗(yàn)1相同。

實(shí)驗(yàn)3和實(shí)驗(yàn)4參數(shù)設(shè)置與實(shí)驗(yàn)2參數(shù)設(shè)置基本相同，將大擾動(dòng)時(shí)間段內(nèi)測(cè)量標(biāo)準(zhǔn)差由3倍分別變?yōu)?倍和5倍，以檢驗(yàn)不同擾動(dòng)強(qiáng)度下算法性能。

實(shí)驗(yàn)采用均方根誤差(RMSE)衡量目標(biāo)定位的精度。為了驗(yàn)證算法的性能，每個(gè)實(shí)驗(yàn)進(jìn)行1000次蒙特卡羅實(shí)驗(yàn)，每次實(shí)驗(yàn)參數(shù)相同。RMSE定義如下

(22)

式中:(x′k,y′k,z′k)為第k次目標(biāo)位置估計(jì)值；(xT,yT,zT) 為目標(biāo)真實(shí)位置。

仿真結(jié)果如圖5和表1所示。通過圖5和表1可以看出，在噪聲模型估計(jì)準(zhǔn)確、未受到大擾動(dòng)的情況下，在180個(gè)采樣點(diǎn)時(shí)間內(nèi)基本穩(wěn)定收斂，3種算法的濾波效果相近，都有較高的精度。在增加了大擾動(dòng)的情況下，各算法都有一定的精度下降。3倍噪聲擾動(dòng)時(shí)UKF和AIUKF的魯棒性也可以使濾波保持較高精度，但隨著擾動(dòng)增大，卡爾曼濾波魯棒性下降；本文算法在5倍正常噪聲下，精度下降僅11.89%，其他2種算法都超過了50%，這是因?yàn)閁KF和AIUKF在受到大擾動(dòng)的情況下，仍然是假定噪聲統(tǒng)計(jì)特性沒有變化，所以精度下降較多；AIUKF由于迭代時(shí)仍然將大擾動(dòng)影響的測(cè)量作為正常噪聲的測(cè)量值，使得偏差更大，所以精度下降最快；本文算法能夠?qū)υ肼曔M(jìn)行在線識(shí)別，同時(shí)，SVSF增益直接與模型的不確定性、量測(cè)噪聲水平有關(guān)，在受到大擾動(dòng)時(shí)，能夠?qū)⒐烙?jì)值限定在真實(shí)值存在子空間內(nèi)，所以，依然有較高精度。

圖5 不同擾動(dòng)下誤差曲線Fig.5 Error curves under different disturbances

表1 不同擾動(dòng)下定位精度Table 1 Accuracy under different disturbances

3.2 第二組實(shí)驗(yàn)：噪聲模型估計(jì)不準(zhǔn)確情況

第二組實(shí)驗(yàn)驗(yàn)證算法在噪聲模型估計(jì)不準(zhǔn)確情況下的性能，以第一組實(shí)驗(yàn)1收斂結(jié)果為對(duì)比基準(zhǔn)。實(shí)驗(yàn)基本設(shè)置同第一組實(shí)驗(yàn)，采樣間隔不變，飛行時(shí)間增長(zhǎng)，采樣點(diǎn)增加到400個(gè)，量測(cè)噪聲服從以下非高斯分布

vk～(1-μ)N(0,R)+μU(0,1)

(23)

式中:μ為因子系數(shù);N(0,R) 表示均值為0、協(xié)方差矩陣為R的正態(tài)分布;U(0,1) 表示0～1之間的均勻分布。

實(shí)驗(yàn)中μ設(shè)置為0.35，同樣進(jìn)行1000次的蒙特卡羅實(shí)驗(yàn)，每次實(shí)驗(yàn)參數(shù)相同，實(shí)驗(yàn)結(jié)果如圖6所示。

圖6 非高斯分布噪聲濾波效果Fig.6 Non-Gaussian noise filtering

通過1000次蒙特卡羅實(shí)驗(yàn)，本文算法定位誤差為穩(wěn)定在2.95 m左右，而另外2種算法則呈現(xiàn)了發(fā)散的趨勢(shì)。

在噪聲滿足高斯分布的情況下，卡爾曼濾波器隨著觀測(cè)數(shù)據(jù)的增多，會(huì)得到越來越精確的結(jié)果。但是在噪聲不滿足高斯分布時(shí)，卡爾曼濾波器仍然以處理高斯噪聲方式進(jìn)行濾波，必然會(huì)產(chǎn)生估計(jì)誤差，其影響程度由系統(tǒng)誤差決定，并且會(huì)累積，最終導(dǎo)致濾波發(fā)散[14]。而本文算法使用平滑有界寬度ψk度量系統(tǒng)誤差狀態(tài)，它不僅包含狀態(tài)向量預(yù)協(xié)方差、量測(cè)先驗(yàn)誤差，還包含量測(cè)轉(zhuǎn)移矩陣、量測(cè)后驗(yàn)誤差和收斂率等。本文算法使用兩種增益：一方面使得估計(jì)狀態(tài)始終在真實(shí)狀態(tài)軌跡周圍，保持了魯棒性；另一方面利用了卡爾曼最優(yōu)估計(jì)的特點(diǎn)取得較高精度。

4 結(jié)束語(yǔ)

本文介紹了小型無人機(jī)測(cè)向交叉目標(biāo)定位原理和濾波模型建立，分析了基于卡爾曼濾波算法的優(yōu)勢(shì)和不足，針對(duì)卡爾曼濾波對(duì)噪聲分布要求高、魯棒性差的問題，本文提出了一種卡爾曼和平滑變結(jié)構(gòu)組合濾波算法。利用仿真實(shí)驗(yàn)，在不同噪聲情況下，進(jìn)行了對(duì)比驗(yàn)證。實(shí)驗(yàn)表明，本文算法具有良好的精度和魯棒性，具有一定的工程參考價(jià)值。下一步，將利用無人機(jī)進(jìn)行實(shí)際飛行驗(yàn)證算法性能，并將其擴(kuò)展到移動(dòng)目標(biāo)定位中。