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

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

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