基于平滑變結構-卡爾曼濾波的目標定位方法

許劍錕，金國棟，譚力寧，許劍鋒，薛遠亮

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

0 引言

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

平滑變結構濾波(Smooth Variable Structure Filter，SVSF)是一種次優(yōu)估計算法，它運用變結構和滑?？刂聘拍睿瑢⒐烙嬛迪拗圃诖嬖谧涌臻g內，使得預估值逐步逼近真實值，對系統(tǒng)受到大擾動、噪聲不符合高斯分布等情況具有較好魯棒性[10-11]。

為了克服卡爾曼濾波對噪聲高斯特性的限制，本文設計了平滑變結構和卡爾曼組合濾波算法，有效提高了大擾動、噪聲不確定情況下目標定位的精度和算法魯棒性。

1 測向交叉定位原理及數學模型

微小型無人機在對目標定位過程中，一般采用測向交叉的目標定位方法，即無人機在多點對目標進行觀測，觀測直線交點為目標位置點，如圖1所示。

圖1 測向交叉目標定位Fig.1 Target location by direction cross

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

1.1 視軸方位角計算

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

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

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

(1)

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

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

(2)

1.2 濾波方程建立

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

xk+1=f(xk)+wk

(3)

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

根據圖2，由目標點和無人機之間的位置關系得系統(tǒng)量測方程為

zk=h(xk)+vk

(4)

(5)

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

2 平滑變結構-卡爾曼濾波算法

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

2.1 平滑變結構濾波

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

圖3 平滑變結構濾波示意圖Fig.3 Schematic diagram of smooth variable structure filter

首先，進行一步預測，即

(6)

(7)

(8)

第二步，求解SVSF增益并進行更新，即

(9)

(10)

(11)

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

(12)

飽和函數的定義為

(13)

式中，vki表示vk中第i個元素，ψkii為ψk中第i個主對角線元素，二者相除構成飽和運算列向量的第i個元素，i=1,…,s,s為vk的維數。

2.2 平滑變結構-卡爾曼組合濾波

由于1.2節(jié)構建的量測方程具有較強的非線性，采用泰勒展開的方式存在截斷誤差會影響濾波精度，因此，本文采用UKF與SVSF的組合濾波。UKF算法已很成熟，不再介紹。

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

圖4 平滑變結構-卡爾曼組合濾波流程圖Fig.4 Flow chart of SVSF-KF

在計算量相當的情況下，UKF的無跡變換(UT)的結果比泰勒展開的近似更為準確。因此，對組合濾波預測階段統(tǒng)一使用UKF的方法，式(6)～(8)可變?yōu)?/p>

(14)

(15)

(16)

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

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

(17)

(18)

SVSF增益及量測更新仍按照式(9)～(11)計算。當采用UKF增益時，則按照標準UKF算法計算。

2.3 基于殘差自適應的組合濾波方法

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

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

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

(19)

Pk=Pk|k-1-KPzzKT

(20)

式中，K為UKF增益。

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

(21)

3 實驗驗證

3.1 第一組實驗：大擾動情況

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

實驗1參數設置:視軸角噪聲與估計模型一致，未受到擾動，整個測量過程中，視軸角誤差服從均值為0°、1倍標準差的正態(tài)分布。

實驗2參數設置:視軸角噪聲與估計模型一致，但受到了大擾動，在120～150采樣點時間段內，視軸角標準差由1倍變?yōu)?倍，其他設置與實驗1相同。

實驗3和實驗4參數設置與實驗2參數設置基本相同，將大擾動時間段內測量標準差由3倍分別變?yōu)?倍和5倍，以檢驗不同擾動強度下算法性能。

實驗采用均方根誤差(RMSE)衡量目標定位的精度。為了驗證算法的性能，每個實驗進行1000次蒙特卡羅實驗，每次實驗參數相同。RMSE定義如下

(22)

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

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

圖5 不同擾動下誤差曲線Fig.5 Error curves under different disturbances

表1 不同擾動下定位精度Table 1 Accuracy under different disturbances

3.2 第二組實驗：噪聲模型估計不準確情況

第二組實驗驗證算法在噪聲模型估計不準確情況下的性能，以第一組實驗1收斂結果為對比基準。實驗基本設置同第一組實驗，采樣間隔不變，飛行時間增長，采樣點增加到400個，量測噪聲服從以下非高斯分布

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

(23)

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

實驗中μ設置為0.35，同樣進行1000次的蒙特卡羅實驗，每次實驗參數相同，實驗結果如圖6所示。

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

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

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

4 結束語

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