基于泰勒估計的迭代定位算法研究

基于泰勒估計的迭代定位算法研究

1飛行物的幾何距離l通常的目標飛行物定位方法是基于多基雷達的測量方法。每個雷達都可以測量自身的坐標Ri(xi,yi,zi)以及它到飛行物距離ri(i=1,…,n),其中n為雷達的總數(shù)。通過一組雷達位置坐標和飛行物到各雷達的距離,我們可以確定目標的空間飛行物的坐標s(x,y,z)。由于每個雷達在測量自身坐標和飛行物到各雷達的距離都存在測量誤差,這給精確定位帶來了困難。如何選取合適的方法進行精確定位是目前對飛行物進行精確定位一個難點。在本文中,假定距離誤差服從正態(tài)分布N(0,σt),坐標誤差服從正態(tài)分布N(0,σr),我們給出了定位精度與兩者之間的關(guān)系模型。2飛行物的確定由于采用歐氏距離,每一部雷達Ri都可以獲得一組數(shù)據(jù):Ri(xi,yi,zi)與ri。理論上利用其中任意三組數(shù)據(jù)即可對飛行物坐標進行定位。不妨設(shè)其中任意三部雷達為:R1(x1,y1,z1)、R2(x2,y2,z2)、R3(x3,y3,z3);測得的飛行物距離分別為:r1、r2、r3。則可以得到下列方程組:解此方程組,一般可以得到兩組值,對此,我們對確定飛行物的過程作以下分析:(1)由于不共線的三部雷達總可以唯一的確定一個平面α,通過歐氏坐標變換,使得平面α與地平面β重合,從而可以使雷達的坐標分量zi為0,也即雷達均為地面雷達。(2)以雷達R1的自身坐標為心,以r1為半徑作球,在空間上方得到半球面γ1,對雷達R2作相似的處理,得到半球面γ2,則γ1與γ2的交點軌跡為一條半圓周l1,易知l1與地平面垂直,且兩雷達R1、R2所測得的飛行物位于l1上。(3)以雷達R3為頂點,以r3為母線作圓錐,在空間上得到一個半圓錐,當且僅當r3>max{R3到直線R1R2距離,R3到l1所在平面距離}時,該半圓錐的底面半圓周與l1有一個交點s,則該交點s即為飛行物的位置。事實上,位于地平面以下的s關(guān)于地平面對稱的點s*也滿足上述所有的條件,然而卻不符合實際情況,這就是我們選擇歐氏變換把雷達平面變?yōu)榈仄矫娴脑?。通過以上分析,可以得出,若要確定飛行物的坐標,至少需要三部雷達。3定位誤差分析3.1一階歷史-可加標移動商為了較易得到距離誤差Δri與飛行物坐標向量誤差(Δx,Δy,Δz)之間的關(guān)系,我們采用控制變量法,假定雷達自身坐標Ri(xi,yi,zi)不變。設(shè)飛行物的真實坐標為s(x,y,z),三部雷達通過測量所確定的飛行物的坐標為s(x0,y0,z0),令Δx=x-x0,Δy=y-y0,Δz=z-z0,則易知向量(Δx,Δy,Δz)為飛行物定位誤差。記雷達Ri到飛行物的真實距離為ri+Δri,那么可以得到如下方程組:記fi(x,y,z)=(x-xi)2+(y-yi)2+(z-zi)2,(i=1,2,3)。則給定初值s(x0,y0,z0),作出fi(x,y,z)在s(x0,y0,z0)附近的一階泰勒(Taylor)展式為:fi(x,y,z)=fi(x0,y0,z0)+2(x0-xi)(x-x0)+2(y0-yi)(y-y0)+2(z0-zi)(z-z0)+ο(Δs)…?,其中Δs=(Δx,Δy,Δz)。當(x,y,z)→(x0,y0,z0)時,fi(x0,y0,z0)→r2ii2,略去高階無窮小項ο(Δs),對?式化簡后可得到:(x0-xi)Δx+(y0-yi)Δy+(z0-zi)Δz=riΔri-(Δri)2。記其系數(shù)矩陣由于在一般情況下,三部雷達與飛行物四點不共面,因此可知矩陣A為非退化矩陣,從而存在可逆矩陣A-1,把上述化簡后的方程組化為矩陣表達式為:(ΔxΔyΔz)T=A-1(r1Δr1-(Δr1)2r2Δr2-(Δr2)2r3Δr3-(Δr3)2)T,略去無窮小項(Δri)2后,該矩陣可以簡化為:(ΔxΔyΔz)T=A-1(r1r2r3)(Δr1Δr2Δr3)T,由此觀之,距離誤差與飛行物定位誤差之間為近似線性關(guān)系,由數(shù)值分析逼近理論,記cond(A)=‖A‖*‖A-1‖(‖·‖表示“·”的給定定義下的范數(shù))表示矩陣A的狀態(tài)數(shù),該狀態(tài)數(shù)反應(yīng)了定位誤差(ΔxΔyΔz)T的上界,若雷達R1、R2、R3在同一個圓周上的分布是均勻的,通過實際數(shù)據(jù)檢測,當給定矩陣A的一個很小的撓動時,(ΔxΔyΔz)T的值變化不大,因此可以認為距離誤差與飛行物定位誤差之間的線性關(guān)系是比較符合實際情形的。3.2飛行物坐標位置解的誤差設(shè)地面三個站點橫坐標真實值為x1、x2、x3,坐標誤差分別為δx1,δx2,δx3,令QX=[x1,x2,x3]T,δQX=[δx1,δx2,δx3]T,觀測值為QXS=[x1s,x2s,x3s]T則有QXS=QX+δQX。同理可得QYS=QY+δQY,QZS=QZ+δQZ。令QS=[QX,QY,QZ]T,δQ=[δQX,δQY,δQZ]T分別代表三維坐標觀測值向量矩陣及三維坐標值的測量誤差矩陣。易知有QS=Q+δQ。在實際進行飛行物坐標求解時,使用的地面雷達坐標實際上是QS而不是Q,這樣飛行物坐標位置解的誤差中就包括由于δQ而引入的誤差。令飛行物三維坐標的測量解算誤差向量為ε=[εx,εy,εz]T,則有ε=s0-s,式中s0引入地面誤差后計算獲得的空中目標三維位置矢量,s是飛行物的位置向量真實值。令s=[x,y,z],當引入各雷達坐標測量誤差后,可以推得f(s,Q)=0,為了得到由于引入δQ而對飛行物位置向量求解精度影響的模型,對f(s,Q)=0式在點(s0,Qs)處附近進行泰勒(Taylor)展式,并線性化處理有:f(s,Q)≈f+L(s?r0)+?f?QδQ|(s0,Qs)f(s,Q)≈f+L(s-r0)+?f?QδQ|(s0,Qs),計算容易得到:L=2A且其中(x1sy1s,z1s),(x2sy2s,z2s),(x3sy3s,z3s)分別為三個雷達的自身坐標觀測值。進一步推導得s=s0-L-1MδQ|(s0,Qs)注意s是飛行物的實際位置向量,而s0是前述方法測量解算獲得的飛行物三維位置向量。則據(jù)ε=[εx,εy,εz]T和s=s0-L-1MδQ|(s0,Qs)可得飛行物三維坐標的測量解誤差向量為ε≈L-1MδQ,至此便得到了采用向量矩陣表示的誤差模型,在模型中我們不難發(fā)現(xiàn)誤差ε的大小與L-1M有直接關(guān)系。當坐標誤差對矩陣M的擾動不是很大時,我們可以近似認為M≈-2A=L從而有L-1M=-I(其中I為3階單位矩陣),ε≈-δQ即:ε與δQ是正比例關(guān)系。當坐標誤差對矩陣M的擾動很大時,會導致ε的值較大,此時飛行物的坐標會有較大誤差,飛行物的定位就不精確。3.3距離和坐標系誤差通過上面的計算我們知道不論是距離誤差還是坐標誤差,它們對空中目標定位的影響都與系數(shù)矩陣L,M有密切的聯(lián)系。如果距離誤差或者坐標誤差對L,M或L-1的擾動比較大,就會產(chǎn)生較大的誤差,當距離和坐標誤差對L,M及L-1擾動較小時,距離誤差對空中目標的定位影響更大。因而我們要盡量減小距離和坐標誤差對L,M及L-1的擾動。觀察L,M的表達式知道只有當三個雷達在其所在的平面上非常均勻時,距離誤差和坐標誤差對L,M及L-1的擾動會比較小。因而我們一般要求三個雷達在其所確定的圓周上分布均勻且這三個雷達所確定的圓周半徑不能太小(否則也可能會產(chǎn)生較大的誤差),這樣飛行物誤差會比較小。4飛行物坐標值的計算設(shè)雷達總數(shù)為n,由以上分析可知,我們可考慮把每三部雷達作為一個分組(假定三部雷達均勻的分布在同一圓周上),每一組雷達均可確定一個飛行物的測量坐標,這樣至多會得到C3nn3組飛行物坐標數(shù)據(jù),對這C3nn3組數(shù)據(jù)取算術(shù)平均即可得到飛行物最終定位值。對每組三部雷達R1、R2、R3,我們作出如下分析:設(shè)飛行物坐標為s(x,y,z),雷達Ri的自身坐標為(xi,yi,zi),其到飛行物s的測量距離為ri。構(gòu)造函數(shù)組gi(x,y,z)=(x?xi)2+(y?yi)2+(z?zi)2?????????????????????????√?ri?gi(x,y,z)=(x-xi)2+(y-yi)2+(z-zi)2-ri?其中i=1,2,3。令g=(g1g2g3)T,則g為關(guān)于x、y、z的三維三元向量函數(shù),且g(x,y,z)=0。解此方程組即可得到飛行物坐標值向量(x,y,z)。利用牛頓法解非線性方程組的方法,記X=(x,y,z),給定初值X0=(x0,y0,z0),函數(shù)g(X)在X0處附近的一階泰勒(Tayloy)展式為:g(X)=g(X0)+g′(X0)(X-X0)+ο(X-X0),其中ο(X-X0)為X-X0的高階無窮小量,g′(X0)為g′(X)=???????g1?x?g2?x?g3?x?g1?y?g2?y?g3?y?g1?z?g2?z?g3?z??????g′(X)=(?g1?x?g1?y?g1?z?g2?x?g2?y?g2?z?g3?x?g3?y?g3?z)在X=X0時的值。則由g(X)=0可得到g(X0)+g′(X0)(X-X0)≈0,解此方程可得X=X0-[g′(X0)]-1*g(X0),然后令X1=X利用上式進行迭代可得X2=X1-[g′(X1)]-1*g(X1),再對X2進行相同的處理得X3,……,一直重復此過程得到無窮序列X0,X1,X2,…Xn…,其中Xn=Xn-1-[g′(Xn-1)]-1*g(Xn-1),當|Xn-Xn-1|<ε(n→∞)(其中ε為給定任意小正數(shù))時,序列{Xn}收斂到某一值X,此X值即為所求的飛行物坐標值。例如:在地面上有三個雷達坐標為:(6650,1430,0),(8705,1430,0),(8705,3030,0),測得空中目標的距離分別為:40251,41899,41744