本發(fā)明涉及多平臺多傳感器數據融合領域，具體涉及一種動平臺傳感器系統(tǒng)偏差的估計方法，從而協(xié)調好基于合作目標的信息和基于非合作目標的信息。

背景技術：

目前已公開的空間配準文獻所研究的問題是針對探測傳感器(如雷達)的系統(tǒng)偏差估計，很少提到平臺姿態(tài)角的系統(tǒng)偏差估計。在超視距作戰(zhàn)過程中，由于目標距離很遠，很小的姿態(tài)偏差都可能導致極大的多源數據探測誤差。因此亟需研究平臺的姿態(tài)配準算法。

在多平臺多傳感器目標跟蹤系統(tǒng)中，由于平臺自身的姿態(tài)角系統(tǒng)偏差在坐標轉換過程中會嚴重的污染量測數據，使得后續(xù)的關聯(lián)、濾波、融合等運算異常，甚至產生虛假目標，很難發(fā)揮出多傳感器的優(yōu)勢[1]。目前，對系統(tǒng)偏差估計的算法主要包括：實時質量控制法[2]、最小二乘類估計算法[3]、精確極大似然類法[4]，以及基于kalman濾波的方法[5]、不敏濾波[6]、em算法[7]等，這些算法均適用于平臺姿態(tài)角偏差是固定值時的系統(tǒng)偏差估計。

而在空間配準技術中，根據目標類型的不同，主要分為基于合作目標的空間配準和基于非合作目標的空間配準兩類[8]。合作目標是指目標的真實位置是已知的，所以只需要一個傳感器就能獲取其位置。而非合作是指目標的真實位置是未知的，所以往往需要兩個或兩個以上的傳感器對其進行探測。文獻[9]研究了當同時出現(xiàn)合作目標與非合作目標時怎樣協(xié)調好基于合作目標的空間配準結果與非合作目標信息，但其只適用于僅有一個傳感器對合作目標有量測的場景，并且是針對僅傳感器有系統(tǒng)偏差的情況，而非平臺姿態(tài)角有偏差。

因此，需要設計一種當合作目標與非合作目標同時存在，僅平臺的姿態(tài)角存在系統(tǒng)偏差場景下的空間配準方法，從而使得配準精度既優(yōu)于單基于合作目標的空間配準，同時也優(yōu)于單基于非合作目標的空間配準，進而實現(xiàn)對目標更加精確的跟蹤。

以上內容所提及的參考文獻如下：

[1]保錚.傳感器信號的長時間積累[j].第七屆全國傳感器學術年會,南京,1999:9-15；

[2]burkej.thesagerealqualitycontrolfractionanditsinterfacewithbuicii/buiciii[r].[s.l.]：mitrecorporation，1966；

[3]sudanojj.aleastsquarealgorithmwithcovarianceweightingforcomputingthetranslationalandrotationalerrorsbetweentworadarsites[j].ieeeaes，1993，29(1)：383-387；

[4]zhouyifeng，henryl.anexactmaximumlikelihoodregistrationalgorithmfordatafusion[j].ieeetranssignalprocessing，1997，45(6)：1560-1572；

[5]kousugey，okadat.biasestimationoftwo3-dimensonalradarsusingkalmanfilter[j].4thint.workshoponadvancedmotioncontrol，1996，1(1)：377-382；

[6]w.li,h.leungandy.zhou.space-timeregistrationofradarandesmusingunscentedkalmanfilter.ieeetrans.onaes.vol.40(3),2004,824-836；

[7]zhenhualiandhenryleung.anexpectationmaximizationbasedsimultaneousregistrationandfusionalgorithmforradarnetworks.ieeeccece/ccgei,ottawa,may2006；

[8]宋文彬.傳感器數據空間配準算法研究進展[j].傳感器與微系統(tǒng),2012,31(8):5-8；

[9]宋文彬.基于合作目標與非合作目標的一體化空間配準新算法[j].電訊技術,2013,53(11):1422-1427。

技術實現(xiàn)要素：

本發(fā)明的目的在于針對上述現(xiàn)有技術中的問題，提供一種動平臺傳感器系統(tǒng)偏差的估計方法，協(xié)調好基于合作目標的信息和基于非合作目標的信息，實現(xiàn)對目標的精確跟蹤。

為了實現(xiàn)上述目的，本發(fā)明采用的技術方案包括以下步驟：

第一步、基于合作目標建立估計姿態(tài)系統(tǒng)偏差的狀態(tài)空間；

將傳感器載于平臺上，使二者的笛卡爾坐標系重合，取ecef坐標系為配準的坐標系，得到：xtrue＝xs+rtrl(α-δα,β-δβ,γ-δγ)x1；式中，xtrue為目標在ecef坐標系下的真實位置，xs為平臺在ecef坐標系下的真實坐標，rt為東北天坐標系到ecef坐標系的轉換矩陣，rl為機載平臺笛卡爾坐標系到東北天坐標系的轉換矩陣，x1為機載平臺笛卡爾坐標系下平臺對目標的量測，β,γ,α分別是平臺俯仰角、偏航角、滾動角的量測值，δβ,δγ,δα分別是平臺俯仰角、偏航角、滾動角的系統(tǒng)偏差；

第二步、基于非合作目標建立估計姿態(tài)系統(tǒng)偏差的狀態(tài)空間；

將傳感器載于平臺上，使二者的笛卡爾坐標系重合，取ecef坐標系為配準的坐標系，對運動的非合作目標，在不考慮傳感器的系統(tǒng)偏差及平臺的姿態(tài)角量測噪聲的情況下，兩平臺a,b在同一時刻，對目標的不含偏差量測相同，得到：

xsa+rtarla(αa-δαa,βa-δβa,γa-δγa)xpa

＝xsb+rtbrlb(αb-δαb,βb-δβb,γb-δγb)xpb

式中，xsa,xsb分別為平臺a,b在ecef坐標系下的坐標，rta,rtb分別為平臺a,b東北天坐標系到ecef坐標系的轉換矩陣，rla,rlb分別為平臺a,b機載平臺笛卡爾坐標系到東北天坐標系的轉換矩陣，xpa,xpb分別為平臺a,b在機載平臺笛卡爾坐標系下傳感器對目標的量測，(αa,βa,γa)和(αb,βb,γb)分別是平臺a,b的滾動角、俯仰角、偏航角的量測值，(δαa,δβa,δγa)和(δαb,δβb,δγb)分別是平臺a,b相應姿態(tài)角的系統(tǒng)偏差；

第三步、實現(xiàn)基于合作目標和非合作目標的交替配準。

所述的第一步中：

1.1)得到東北天坐標系到ecef坐標系的轉換矩陣rt為：

得到：

其中：

此處(xs,ys,zs)表示機載平臺中心在ecef坐標系中的位置，而(λs,ls,hs)則表示機載平臺中心在大地坐標系的位置，分別表示經度，緯度和高度；

1.2)得到機載平臺笛卡爾坐標系到東北天坐標系的轉換矩陣rl(α,β,γ)為：

其中，γ,α,β分別為平臺的偏航角，滾動角和俯仰角；

1.3)得到機載平臺笛卡爾坐標系到東北天坐標系的真實轉換矩陣rtl為：

其中，δγ,δα,δβ分別為平臺的偏航角系統(tǒng)偏差，滾動角系統(tǒng)偏差和俯仰角系統(tǒng)偏差。

將步驟1.3)得到的矩陣在δγ＝δβ＝δα＝0處進行一階泰勒展開，得到：

使得：

其中，δε＝(δα,δβ,δγ)^t是平臺在機體坐標系的定姿誤差，而γ¹是把機體坐標系的定姿誤差轉換到東北天坐標系的誤差轉換矩陣；

a.對δrl的各元素展開，得到：

b.根據機載平臺笛卡爾坐標系到東北天坐標系的真實轉換矩陣rtl及其一階泰勒展開式，計算得到各元素：

根據東北天坐標系到ecef坐標系的轉換矩陣rt，得到：

結合上式以及東北天坐標系到ecef坐標系的轉換矩陣rt，得到：

xtrue＝xs+rtrl(α,β,γ)x1+δx3＝xs+rtrl(α,β,γ)x1+γδε

其中，γ＝rt·γ¹是平臺定姿誤差到ecef坐標系的轉換矩陣，是機體坐標系定姿誤差轉換到東北天坐標系的誤差轉換矩陣與旋轉變換矩陣的復合。

所述的第二步中：

2.1)令i＝a,b，則平臺i東北天坐標系到ecef坐標系的轉換矩陣rti為：

其中，(λsi,lsi,hsi)則表示機載平臺i(i＝a,b)中心在大地坐標系的位置，分別表示經度，緯度和高度；

2.2)機載平臺i笛卡爾坐標系到東北天坐標系的轉換矩陣rli為：

其中，γi,αi,βi分別為平臺i的偏航角，滾動角和俯仰角。

將步驟2.2)的等式在δαa＝δβa＝δγa＝δαb＝δβb＝δγb＝0處進行一階泰勒展開，

得到：

其中，分別為：

整理得到：

簡化后得到：

γfhζ＝zfh；

其中，γfh＝[γa-γb]為線性化后的量測矩陣，具體為：

ζ＝[δαaδβaδγaδαbδβbδγb]^t為平臺三個姿態(tài)角的系統(tǒng)偏差，即狀態(tài)向量，zfh＝(xsa+rtarla(αa,βa,γa)xpa)-(xsb+rtbrlb(αb,βb,γb)xpb)為等效量測。

所述第三步實現(xiàn)交替配準的步驟為：

步驟3.1、若兩平臺a,b對同一個合作目標均有量測，則根據第一步關系式得到：

xtrue＝xsa+rtarla(αa,βa,γa)x1a+γaδεa＝xsb+rtbrlb(αb,βb,γb)x1b+γbδεb；

步驟3.2、將步驟3.1簡化為：

其中，

za＝xtrue-xsa-rtarla(αa,βa,γa)x1a，

zb＝xtrue-xsb-rtbrlb(αb,βb,γb)x1b，

ζ＝[δαaδβaδγaδαbδβbδγb]^t；

步驟3.3、若兩平臺a,b中只有平臺a對合作目標有量測，則根據第一步關系式得到：

xtrue＝xsa+rtarla(αa,βa,γa)x1a+γaδεa；

步驟3.4、將步驟3.3簡化，使之能與基于非合作目標實現(xiàn)交替配準：

其中，

za＝xtrue-xsa-rtarla(αa,βa,γa)x1a，

zb＝[000]^t，

ζ＝[δαaδβaδγaδαbδβbδγb]^t；

步驟3.5、進行基于合作目標的k時刻卡爾曼濾波，基于合作目標的狀態(tài)空間為：

步驟3.5.1、對狀態(tài)進行一步預測和對狀態(tài)的誤差協(xié)方差陣進行一步預測：

步驟3.5.2、對狀態(tài)的增益矩陣的計算：

步驟3.5.3、對狀態(tài)進行一步更新：

步驟3.5.4、對狀態(tài)誤差協(xié)方差陣進行一步更新：

其中，cov(υh,k)＝rh,k+1。

步驟3.6、進行基于非合作目標的k時刻卡爾曼濾波時，等于pkk等于pk+1|k+1，則基于非合作目標的狀態(tài)空間為：

步驟3.6.1、對狀態(tài)進行一步預測和對狀態(tài)的誤差協(xié)方差陣進行一步預測；

步驟3.6.2、對狀態(tài)的增益矩陣的計算：

步驟3.6.3、對狀態(tài)進行一步更新：

步驟3.6.4、對狀態(tài)誤差協(xié)方差陣進行一步更新：

其中，cov(υfh,k)＝rfh,k+1；

步驟3.7、k＝k+1，然后返回步驟3.1，但是此時的等于步驟3.6.2中的pk|k等于步驟3.6.3中的pk+1|k+1，如此實現(xiàn)所有時刻基于合作目標和基于非合作目標的交替配準。

若是多個合作目標，則先按照步驟3.5對所有的合作目標進行k時刻的參數估計；若是多個非合作目標，則先按照步驟3.6對所有的非合作目標進行k時刻的參數估計。

與現(xiàn)有技術相比，本發(fā)明具有如下的有益效果：解決了動平臺傳感器系統(tǒng)中，傳感器量測無偏差而平臺姿態(tài)角存在偏差的空間配準問題，考慮到地球曲率的影響，本發(fā)明基于公共坐標系為地心地固坐標系的三維場景，分析傳感器不存在量測偏差的前提下，平臺姿態(tài)角偏差的估計問題，根據目標類型的不同，設計了基于合作目標為單個或多個的姿態(tài)角配準，基于非合作目標為單個或多個的姿態(tài)角配準，以及兩類目標共存時的姿態(tài)角配準，從而得到同時用兩類目標的量測信息進行配準，并且能夠精確估計姿態(tài)角偏差，實現(xiàn)目標的精確跟蹤。本發(fā)明首先建立以地心地固坐標系為公共坐標系，用于合作目標空間配準的狀態(tài)空間，并針對一個平臺對該合作目標有量測還是兩個平臺對該合作目標均有量測，然后建立以地心地固坐標系為公共坐標系，用于非合作目標空間配準的狀態(tài)空間，最后建立基于合作目標配準和基于非合作配準的交替姿態(tài)角偏差估計，最終得到的估計精度優(yōu)于單基于合作目標量測信息進行空間配準的方法以及單基于非合作目標量測信息進行空間配準的方法。

附圖說明

圖1合作目標、非合作目標及兩平臺a,b的運動軌跡；

圖2平臺a姿態(tài)角偏差rmse圖：(a)、(b)分別為不同程度放大示意圖；

圖3平臺b姿態(tài)角偏差rmse圖：(a)、(b)分別為不同程度放大示意圖；

圖4本發(fā)明方法的整體操作框圖；

圖5本發(fā)明方法的具體流程圖。

具體實施方式

下面結合附圖對本發(fā)明做進一步的詳細說明。

本發(fā)明動平臺傳感器系統(tǒng)偏差的估計方法包括以下步驟：

步驟一：基于合作目標建立估計姿態(tài)系統(tǒng)偏差的狀態(tài)空間；

步驟二：基于非合作目標建立估計姿態(tài)系統(tǒng)偏差的狀態(tài)空間；

步驟三：實現(xiàn)基于合作目標和非合作目標的交替配準。

步驟一中基于合作目標建立估計姿態(tài)系統(tǒng)偏差的狀態(tài)空間，具體包括以下步驟：

步驟1.1：傳感器載于平臺之上，且二者的笛卡爾坐標系重合，考慮地球曲率影響，取地心地固坐標系(ecef坐標系)作為配準的坐標系。在不考慮傳感器量測系統(tǒng)偏差以及姿態(tài)角量測噪聲的情況下，平臺對目標不含偏差的量測位置與合作目標匯報的真實位置在ecef坐標系下相同，于是可列出等式

xtrue＝xs+rtrl(α-δα,β-δβ,γ-δγ)x1

其中，xtrue為目標在ecef坐標系下的真實位置，xs為平臺在ecef坐標系下的真實坐標，rt為東北天坐標系到ecef坐標系的轉換矩陣，rl為機載平臺笛卡爾坐標系到東北天坐標系的轉換矩陣，x1為機載平臺(傳感器)笛卡爾坐標系下平臺對目標的量測，β,γ,α分別是平臺俯仰角、偏航角、滾動角的量測值，δβ,δγ,δα分別是平臺俯仰角、偏航角、滾動角的系統(tǒng)偏差。

步驟1.1.1：東北天坐標系到ecef坐標系的轉換矩陣rt為：

此時步驟1.1可表示為：

其中：

此處(xs,ys,zs)表示機載平臺中心在ecef坐標系中的位置，而(λs,ls,hs)則表示機載平臺中心在大地坐標系的位置，分別表示經度，緯度和高度。

步驟1.1.2：機載平臺(傳感器)笛卡爾坐標系到東北天坐標系的轉換矩陣rl(α,β,γ)為

其中，γ,α,β分別為平臺的偏航角，滾動角和俯仰角。

步驟1.1.3：參考步驟1.1.2，機載平臺(傳感器)笛卡爾坐標系到東北天坐標系的真實轉換矩陣rtl為

其中，δγ,δα,δβ分別為平臺的偏航角系統(tǒng)偏差，滾動角系統(tǒng)偏差和俯仰角系統(tǒng)偏差。

步驟1.2：將步驟1.1.3在δγ＝δβ＝δα＝0處進行一階泰勒展開，得到

使得

其中，δε＝(δα,δβ,δγ)^t是平臺在機體坐標系的定姿誤差，而γ¹是把機體坐標系的定姿誤差轉換到東北天坐標系的誤差轉換矩陣。

步驟1.2.1：根據步驟1.2，對δrl的各元素展開，得到

步驟1.2.2：根據步驟1.1.3和步驟1.2，計算得到各元素，得到

步驟1.3：根據步驟1.1.1，得

步驟1.4：根據步驟1.1.1和步驟1.3，得

xtrue＝xs+rtrl(α,β,γ)x1+δx3＝xs+rtrl(α,β,γ)x1+γδε

其中γ＝rt·γ¹是平臺定姿誤差到ecef坐標系的轉換矩陣，是機體坐標系定姿誤差轉換到東北天坐標系的誤差轉換矩陣與旋轉變換矩陣的復合。

步驟二中基于非合作目標建立估計姿態(tài)系統(tǒng)偏差的狀態(tài)空間，具體步驟如下：

步驟2.1：傳感器載于平臺之上，且二者的笛卡爾坐標系重合，考慮地球曲率影響，取地心地固坐標系(ecef坐標系)作為配準的坐標系。在目標是運動的非合作目標的前提下對兩個平臺進行配準。配準的主要思路是，在不考慮傳感器的系統(tǒng)偏差及平臺的姿態(tài)角量測噪聲的情況下，兩平臺a,b在同一時刻，對目標的不含偏差量測相同，則可列出列出等式

其中，xsa,xsb分別為平臺a,b在ecef坐標系下的坐標，rta,rtb分別為平臺a,b東北天坐標系到ecef坐標系的轉換矩陣，rla,rlb分別為平臺a,b機載平臺(傳感器)笛卡爾坐標系到東北天坐標系的轉換矩陣，xpa,xpb分別為平臺a,b在機載平臺(傳感器)笛卡爾坐標系下傳感器對目標的量測，(αa,βa,γa)和(αb,βb,γb)分別是平臺a,b的滾動角、俯仰角、偏航角的量測值，(δαa,δβa,δγa)和(δαb,δβb,δγb)分別是平臺a,b相應姿態(tài)角的系統(tǒng)偏差。

步驟2.1.1：平臺i(i＝a,b)東北天坐標系到ecef坐標系的轉換矩陣rti為

其中，(λsi,lsi,hsi)則表示機載平臺i(i＝a,b)中心在大地坐標系的位置，分別表示經度，緯度和高度。

步驟2.1.2：機載平臺(傳感器)i(i＝a,b)笛卡爾坐標系到東北天坐標系的轉換矩陣rli為

其中，γi,αi,βi分別為平臺i的偏航角，滾動角和俯仰角。

步驟2.2：由于平臺的姿態(tài)偏差相對較小，參考步驟2.1.1和步驟2.1.2，將步驟2.1等式在δαa＝δβa＝δγa＝δαb＝δβb＝δγb＝0處進行一階泰勒展開，得

其中，分別為

步驟2.3：將步驟2.2進行整理，得

步驟2.4：將步驟2.3進行簡化，得

γfhζ＝zfh

其中，γfh＝[γa-γb]為線性化后的量測矩陣，具體為

ζ＝[δαaδβaδγaδαbδβbδγb]^t為平臺三個姿態(tài)角的系統(tǒng)偏差，即狀態(tài)向量，

zfh＝(xsa+rtarla(αa,βa,γa)xpa)-(xsb+rtbrlb(αb,βb,γb)xpb)為等效量測。

步驟三中，對基于合作目標的量測信息和基于非合作目標的量測信息用卡爾曼濾波交替估計平臺的姿態(tài)角偏差，具體步驟如下：

步驟3.1：若兩平臺(傳感器)a,b對同一個合作目標均有量測，則根據步驟一的建模過程，可分別得到

xtrue＝xsa+rtarla(αa,βa,γa)x1a+γaδεa＝xsb+rtbrlb(αb,βb,γb)x1b+γbδεb

步驟3.2：步驟3.1可簡化為

其中，

za＝xtrue-xsa-rtarla(αa,βa,γa)x1a

zb＝xtrue-xsb-rtbrlb(αb,βb,γb)x1b

ζ＝[δαaδβaδγaδαbδβbδγb]^t

步驟3.3：若兩平臺(傳感器)a,b中只有平臺(傳感器)a對合作目標有量測，則根據步驟一的建模過程，可得到

xtrue＝xsa+rtarla(αa,βa,γa)x1a+γaδεa

步驟3.4：但是為了能與基于非合作目標實現(xiàn)交替配準，步驟3.3需簡化為

其中，

za＝xtrue-xsa-rtarla(αa,βa,γa)x1a

zb＝[000]^t

ζ＝[δαaδβaδγaδαbδβbδγb]^t

步驟3.5：步驟一為了簡便起見，忽略了平臺傳感器的量測噪聲，下面在進行卡爾曼濾波的時候將噪聲考慮在內。依據步驟3.1至3.4，進行基于合作目標的k時刻的卡爾曼濾波，基于合作目標的狀態(tài)空間為

步驟3.5.1：對狀態(tài)進行一步預測和對狀態(tài)的誤差協(xié)方差陣進行一步預測

步驟3.5.2：對狀態(tài)的增益矩陣的計算

步驟3.5.3：對狀態(tài)進行一步更新

步驟3.5.4：對狀態(tài)誤差協(xié)方差陣進行一步更新

其中，cov(υh,k)＝rh,k+1。

步驟3.6：步驟二為了簡便起見，忽略了平臺傳感器的量測噪聲，下面在進行卡爾曼濾波的時候將平臺姿態(tài)角量測噪聲考慮在內。依據步驟二，進行基于非合作目標的k時刻的卡爾曼濾波，但是此時的等于步驟3.5.2中的pk|k等于步驟3.5.3中的pk+1|k+1，基于非合作目標的狀態(tài)空間為

步驟3.6.1：對狀態(tài)進行一步預測和對狀態(tài)的誤差協(xié)方差陣進行一步預測

步驟3.6.2：對狀態(tài)的增益矩陣的計算

步驟3.6.3：對狀態(tài)進行一步更新

步驟3.6.4：對狀態(tài)誤差協(xié)方差陣進行一步更新

其中，cov(υfh,k)＝rfh,k+1

步驟3.7：k＝k+1，然后返回步驟3.1，但是此時的等于步驟3.6.2中的pk|k等于步驟3.6.3中的pk+1|k+1。如此實現(xiàn)所有時刻的基于合作目標和基于非合作目標的交替配準。當然，若是多個合作目標，則先按照步驟3.5對所有的合作目標進行k時刻的參數估計，同樣的，若是多個非合作目標，則先按照步驟3.6對所有的非合作目標進行k時刻的參數估計，具體參見圖5。

參見圖1-3，本發(fā)明利用pc平臺仿真來實現(xiàn)對算法的驗證。仿真軟件為windows7旗艦版，matlabr2014a版本。仿真場景設置為：兩平臺對一個合作目標和一個非合作目標進行觀測，其中兩平臺對該合作目標均有量測，平臺a和b在大地坐標系下的初始位置(緯度，經度，高度)分別為(33.9°,108.2°,6000m)和(33.9°,107.9°,6000m)，在緯度、經度、高度的速度向量均為(-0.12km/s,-0.12km/s,0)，平臺a和b的姿態(tài)角真值分別為(25°,10°,20°)和(30°,15°,25°)，姿態(tài)角系統(tǒng)偏差分別為(0.5°,-0.95°,0.55°)和(0.45°,-0.7°,0.4°)，所有的角度量測噪聲標準差均為0.1°，合作目標和非合作目標在在大地坐標系下的初始位置(緯度，經度，高度)分別為(33°,107°,6000m)和(33.4°^,107.2°,6000m)，在緯度、經度、高度的速度向量分別為(0.1km/s,0.1km/s,0)和(0.1km/s,0,0)，采樣周期為1s，整個仿真時間為1000s，蒙特卡洛仿真次數為100次。圖1為兩個目標和兩個平臺在大地坐標系下的運動軌跡，從圖2(a)和圖2(b)可以看出利用平臺a對合作目標和非合作目標的量測信息可以得到最精確的姿態(tài)角偏差估計，從圖3(a)和圖3(b)可以看出利用平臺b對合作目標和非合作目標的量測信息可以得到最精確的姿態(tài)角偏差估計，精確的姿態(tài)角偏差估計可以實現(xiàn)對目標更準確的跟蹤。