本發(fā)明涉及無人機目標定位技術領域,尤其涉及一種基于無人機的運動目標ukf濾波方法。
背景技術:
對運動目標而言,想要實現(xiàn)高精度軌跡跟蹤能力,必須建立準確的目標運動模型(系統(tǒng)狀態(tài)方程)和系統(tǒng)量測模型。然而目標模型和量測模型的建立很大程度上取決于自身所選取的坐標系。目標模型和量測模型大多建立在大地直角坐標系中,其中目標模型有cv、ca、時間相關模型以及當前統(tǒng)計模型等。狀態(tài)變量為目標在大地直角坐標系中的位置、速度和加速度,量測值以目標定位算法求出的目標在大地直角坐標系中的坐標值為量測量。這樣帶來的后果一方面由于是間接計算出的結(jié)果導致濾波器使用時的量測值已經(jīng)夾雜了非常大的目標定位算法誤差,需要誤差的統(tǒng)計特性,不利于提高目標定位精度;另一方面在于增大了量測噪聲的不確定性,因為誤差值與目標相對無人機距離有關,并非一成不變。
為提高系統(tǒng)量測方程的準確性,需要建立以傳感器直接測量值(r,α,λ)t(目標斜距、方位角和俯仰角)為量測變量的非線性量測方程,系統(tǒng)量測噪聲方差陣直接從傳感器獲取,將大大提高量測模型的準確性。目前,針對這一問題的主要解決方法為混合坐標系求解法,以
技術實現(xiàn)要素:
本發(fā)明所要解決的技術問題是針對傳統(tǒng)目標跟蹤方案中定位模型和濾波器彼此分開所帶來的噪聲模型不準確導致系統(tǒng)精度不高的問題,提供一種基于無人機的運動目標ukf濾波方法。
本發(fā)明為解決上述技術問題采用以下技術方案:
一種基于無人機的運動目標ukf濾波方法,包含以下步驟:
步驟1),在無人機上設置紅外光電吊艙和雷達,其中,所述紅外光電吊艙用于測量運動目標相對無人機的距離,所述雷達用于測量運動目標相對無人機的方位角和俯仰角;
步驟2),利用無人機上的紅外光電吊艙、雷達對運動目標進行測量,獲得運動目標相對無人機的距離、方位角和俯仰角(r,α,λ)t,其中,r為運動目標相對無人機的距離,α為運動目標相對無人機的方位角,λ為運動目標相對無人機的俯仰角;
步驟3),根據(jù)運動學原理建立運動目標的勻加速數(shù)學模型,其系統(tǒng)狀態(tài)方程如下:
xk=φk,k-1xk-1+gk-1wk-1
其中,
xk為運動目標k時刻的狀態(tài)量,xk-1為運動目標k-1時刻的狀態(tài)量,
步驟4),令運動目標的非線性量測方程為zk=h(xk)+vk,其中,h(xk)為量測函數(shù),vk是k時刻量測零均值高斯白噪聲,方差為r,zk=[r,α,λ]t;
建立運動目標在大地直角坐標系與運動目標相對無人機傳感器基座坐標系之間的關系:
采用齊次坐標轉(zhuǎn)換方法,得到運動目標的大地直角坐標系坐標值(x,y,z)到基座坐標系坐標值(xb,yb,zb)的轉(zhuǎn)換矩陣
hs是無人機高度,rn是卯酉圈曲率半徑,λs是無人機緯度,αs是無人機經(jīng)度,
運動目標相對于傳感器基座坐標系坐標值(xb,yb,zb)與傳感器對目標量測值(r,α,λ)t之間的關系如下:
則結(jié)合轉(zhuǎn)換矩陣
步驟5),ukf濾波
步驟5.1),根據(jù)k-1時刻的運動目標的狀態(tài)量
步驟5.2),結(jié)合運動目標勻加速數(shù)學模型的系統(tǒng)狀態(tài)方程得到k時刻的運動目標狀態(tài)方程和協(xié)方差的一步預測值;
步驟5.3),計算運動目標的坐標值和量測值的一步預測值;
步驟5.4),計算自協(xié)方差陣
步驟5.5),更新運動目標坐標及協(xié)方差。
作為本發(fā)明基于無人機的運動目標ukf濾波方法進一步的優(yōu)化方案,所述步驟5.1)中,根據(jù)以下公式構造2n+1個對稱的sigma采樣點:
以上式中χ表示狀態(tài)量通過非線性量測函數(shù)h(xk)+vk的傳播,χ的下標表示采樣點順序及時刻;γ=ε2(n+κ)-n,ε、κ均為預設的閾值,其中,ε決定采樣點距離均值的遠近程度;κ≥0保證方差陣的半正定性。
作為本發(fā)明基于無人機的運動目標ukf濾波方法進一步的優(yōu)化方案,所述步驟5.2)中,根據(jù)以下公式得到k時刻的運動目標狀態(tài)方程和協(xié)方差的一步預測值:
作為本發(fā)明基于無人機的運動目標ukf濾波方法進一步的優(yōu)化方案,所述步驟5.3)中,由以下公式計算運動目標的坐標值和量測值的一步預測值:
其中,
作為本發(fā)明基于無人機的運動目標ukf濾波方法進一步的優(yōu)化方案,所述步驟5.4)中,由以下公式計算自協(xié)方差陣
式中,rk為k時刻量測噪聲的方差。
β為與狀態(tài)向量的先驗分布相關的參數(shù)。
作為本發(fā)明基于無人機的運動目標ukf濾波方法進一步的優(yōu)化方案,所述步驟5.5)中,由以下公式計算目標坐標及協(xié)方差更新:
本發(fā)明采用以上技術方案與現(xiàn)有技術相比,具有以下技術效果:
本發(fā)明直接以目標斜距、方位角和俯仰角三個參數(shù)為量測變量,在很大程度上降低了統(tǒng)計量測噪聲特性時的工作量,并提高了噪聲模型的準確性。與混合坐標系相比,系統(tǒng)狀態(tài)變量統(tǒng)一,不存在混合坐標系中狀態(tài)變量的轉(zhuǎn)換問題,結(jié)合無人機特性構建的目標非線性量測方程不涉及強非線性方程的一二階求導,大大降低了模型的復雜度。最終仿真結(jié)果驗證了該模型的快速收斂性能和高精度性能。
附圖說明
圖1是本發(fā)明的流程示意圖;
圖2是本發(fā)明中x軸位置誤差圖;
圖3是本發(fā)明中y軸位置誤差圖;
圖4是本發(fā)明中z軸位置誤差圖;
圖5是本發(fā)明中x軸速度誤差圖;
圖6是本發(fā)明中y軸速度誤差圖;
圖7是本發(fā)明中z軸速度誤差圖;
圖8是本發(fā)明中x軸加速度誤差圖;
圖9是本發(fā)明中y軸加速度誤差圖;
圖10是本發(fā)明中z軸加速度誤差圖。
具體實施方式
下面結(jié)合附圖對本發(fā)明的技術方案做進一步的詳細說明:
本發(fā)明主要為面向基于無人機的運動目標ukf濾波方法,該方案的設計流程圖如附圖1所示。該方法利用雷達、紅外光電吊艙等傳感器為量測手段獲得目標相對于傳感器的距離、方位角和俯仰角等量測信息,以該量測信息為輸入量結(jié)合無人機機載導航設備獲取的無人機自身信息建立非線性量測方程。與此同時,由目標運動模型獲得目標預測值,結(jié)合非線性量測方程與目標運動模型進行無跡卡爾曼濾波獲得當前時刻精度較高的目標狀態(tài)量。
本發(fā)明包含以下步驟:
步驟1),在無人機上設置紅外光電吊艙和雷達,其中,所述紅外光電吊艙用于測量運動目標相對無人機的距離,所述雷達用于測量運動目標相對無人機的方位角和俯仰角;
步驟2),利用無人機上的紅外光電吊艙、雷達對運動目標進行測量,獲得運動目標相對無人機的距離、方位角和俯仰角(r,α,λ)t,其中,r為運動目標相對無人機的距離,α為運動目標相對無人機的方位角,λ為運動目標相對無人機的俯仰角;
步驟3),根據(jù)運動學原理建立運動目標的勻加速數(shù)學模型,系統(tǒng)狀態(tài)方程如下:
xk=φk,k-1xk-1+gk-1wk-1(1)
其中,
xk為運動目標k時刻的狀態(tài)量,xk-1為運動目標k-1時刻的狀態(tài)量,φk,k-1為k-1時刻到k時刻的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,gk-1為k-1時刻系統(tǒng)噪聲矩陣,wk-1是k-1時刻系統(tǒng)零均值高斯白噪聲,方差為q,t為采樣周期,i3×3為三階單位矩陣。把目標加速度變換量當做系統(tǒng)噪聲干擾,提高了系統(tǒng)對機動目標的跟蹤能力。
步驟4),令運動目標的非線性量測方程為
zk=h(xk)+vk(2)
其中,h(xk)為量測函數(shù),vk是k時刻量測零均值高斯白噪聲,方差為r,zk=[r,α,λ]t為目標相對雷達的距離、方位角和俯仰角度信息。由于狀態(tài)變量是基于大地直角坐標系的,然而量測變量是基于傳感器基座坐標系,因此需要構建兩者之間的聯(lián)系關系式。根據(jù)下面公式計算以上兩個坐標系之間的關系式。
記目標在大地直角坐標系中的坐標為(x,y,z),目標在無人機傳感器基座坐標系中的坐標為(xb,yb,zb),
采用齊次坐標轉(zhuǎn)換方法,得到運動目標的大地直角坐標系坐標值(x,y,z)到基座坐標系坐標值(xb,yb,zb)的轉(zhuǎn)換矩陣為:
其中,
下面分別求取
其中,αs,λs,hs分別為載機的大地經(jīng)度、緯度和高度,e為地球參考橢球的第一偏心率,rn為卯酉圈曲率半徑。
其中,φas,θas,ψas分別為載機的橫滾角、俯仰角和偏航角。
其中,
運動目標相對于傳感器基座坐標系坐標值(xb,yb,zb)與傳感器對目標量測值(r,α,λ)t之間的關系如下:
則結(jié)合轉(zhuǎn)換矩陣
步驟5),ukf濾波
步驟5.1),計算采樣點
根據(jù)k-1時刻的目標狀態(tài)量
以上式中χ表示狀態(tài)量通過非線性量測函數(shù)h(xk)+vk的傳播,χ的下標表示采樣點順序及時刻;γ=ε2(n+κ)-n,ε、κ均為預設的閾值,其中,ε決定采樣點距離均值的遠近程度;κ≥0保證方差陣的半正定性。
步驟5.2),建立時間更新方程
由系統(tǒng)狀態(tài)方程(1)可得到k時刻的運動目標狀態(tài)更新方程和協(xié)方差一步預測值分別為:
步驟5.3),計算目標坐標值及量測量的一步預測值
由(9)式計算得到的采樣點及量測方程(2)獲得k-1時刻對于k時刻的一步預測采樣點χk/k-1、目標坐標值預測值
其中,
步驟5.4),將式(12)、(13)代入下式中求得自協(xié)方差陣
(14)式中,rk為k時刻量測噪聲的方差。
β為與狀態(tài)向量的先驗分布相關的參數(shù)。
利用式(14)、(15)求得卡爾曼濾波增益
步驟5.5),目標坐標及協(xié)方差更新
將(16)、(13)、(14)和(15)式求得的量代入以下(17)、(18)式得到狀態(tài)量及協(xié)方差更新方程
將步驟3)建立的運動目標狀態(tài)方程與步驟4)建立的系統(tǒng)量測方程代入到步驟5)中進行濾波處理,可得到k時刻較精確的目標狀態(tài)量,該算法可循環(huán)計算,由此可根據(jù)傳感器直接對目標的量測值得到各時刻目標的精確定位。
附圖2為用本發(fā)明ukf濾波后和用線性卡爾曼濾波后x軸方向位置誤差對比圖。
附圖3為分別用ukf濾波和用線性卡爾曼濾波后y軸方向位置誤差對比圖。
附圖4為分別用ukf濾波和用線性卡爾曼濾波后z軸方向位置誤差對比圖。
附圖5為分別用ukf濾波和用線性卡爾曼濾波后x軸方向速度誤差對比圖。
附圖6為分別用ukf濾波和用線性卡爾曼濾波后y軸方向速度誤差對比圖。
附圖7為分別用ukf濾波和用線性卡爾曼濾波后z軸方向速度誤差對比圖。
附圖8為分別用ukf濾波和用線性卡爾曼濾波后x軸方向加速度誤差對比圖。
附圖9為分別用ukf濾波和用線性卡爾曼濾波后y軸方向加速度誤差對比圖。
附圖10為分別用ukf濾波和用線性卡爾曼濾波后z軸方向加速度誤差對比圖。
由圖2~圖10可知,初始時刻,改進后的ukf濾波方法和卡爾曼濾波方法具有相同的定位跟蹤精度,但隨著時間的推移,誤差差異開始逐漸凸顯,且目標距離無人機越遠,差異越明顯,尤其在速度和加速度方面表現(xiàn)尤為突出,改進后的算法精度更高。
本技術領域技術人員可以理解的是,除非另外定義,這里使用的所有術語(包括技術術語和科學術語)具有與本發(fā)明所屬領域中的普通技術人員的一般理解相同的意義。還應該理解的是,諸如通用字典中定義的那些術語應該被理解為具有與現(xiàn)有技術的上下文中的意義一致的意義,并且除非像這里一樣定義,不會用理想化或過于正式的含義來解釋。
以上所述的具體實施方式,對本發(fā)明的目的、技術方案和有益效果進行了進一步詳細說明,所應理解的是,以上所述僅為本發(fā)明的具體實施方式而已,并不用于限制本發(fā)明,凡在本發(fā)明的精神和原則之內(nèi),所做的任何修改、等同替換、改進等,均應包含在本發(fā)明的保護范圍之內(nèi)。