基于時(shí)差觀測(cè)量的三維定位坐標(biāo)解算

1、基于時(shí)差觀測(cè)量的三維定位坐標(biāo)解算分析的 GUI 仿真實(shí)現(xiàn)摘要：在各個(gè)領(lǐng)域有廣泛應(yīng)用，本項(xiàng)目為基于時(shí)差觀測(cè)量的三維定位坐標(biāo)解算研究。其中包括對(duì)中的相關(guān)觀測(cè)數(shù)據(jù)進(jìn)行建模和求解，對(duì)定位的誤差，精度分析設(shè)計(jì)相應(yīng)算法。文中給出一種基于時(shí)差觀測(cè)量的定位方法。通過(guò)觀測(cè)量得到一組非線性超定方程組，用 Newton 迭代法求解并對(duì)定位精度作簡(jiǎn)交互界面，使整個(gè)定位過(guò)程可以在 GUI 中單分析，利用語(yǔ)言設(shè)計(jì)出仿真實(shí)現(xiàn)。關(guān)鍵字： 時(shí)差觀測(cè)量， 定位，非線性超定方程組， Newton 法GUI Simulation of The Three-Dimenalitioning Coordinateysis Based on

2、 Time Difference ObservationCalculationAbstract:itioning technology is widely appd in various fields.This project is acalculating research of three-dimenalitioning coordinates based on the amount of timeobservation,including theitioning technology for ming and solving the relevantobservation data, t

3、heitioning error, preciysis and design correspondingalgorithm.A method of base sion itioning based on time difference observation is presented his pr. A group of nonlinear overdetermined system of equations is obtained by observation，and is solved by the Newton iteration method . ast,a software erfa

4、ce is designed by MATALB, so t the whole pros simulation can be showed he GUI.Keywords: Time difference observation，Base ，Nonlinear overdetermined system of equations，Newton method2sionitioning1.引言：移動(dòng)具有廣泛的用途，使得移動(dòng)的作用與商業(yè)利益并舉。隨著電子的突飛猛進(jìn)， 移動(dòng)定位對(duì)人們衣、食、住、行等各方面產(chǎn)生了巨大的影響。本項(xiàng)目是基于時(shí)差觀測(cè)量的三維定位坐標(biāo)解算研究，定位方法為：在無(wú)線自組織網(wǎng)的管理

5、下， 基主站通知各基從站和待定位的移動(dòng)節(jié)點(diǎn) R 在設(shè)置的時(shí)間窗口期間開(kāi)啟射頻電路， 基主站通過(guò)信道 u 發(fā)射定位脈沖序列， 移動(dòng)節(jié)點(diǎn) R 將該序列頻變后通過(guò)信道 v 發(fā)射出去。多個(gè)基從站對(duì)定位脈沖經(jīng)兩條路徑時(shí)差進(jìn)量。在系統(tǒng)坐標(biāo)中， 各基主站和基從站的具置事先都被精確確定，則移動(dòng)節(jié)點(diǎn) R 的坐標(biāo)服從橢圓曲線方程， 為一非線性函數(shù)。由多個(gè)基從站測(cè)量值得到一組超定非線性方程。對(duì)該方法將進(jìn)行模擬， 基于此定位方法進(jìn)行解算分析， 設(shè)計(jì)仿真程序描述該定位過(guò)程。模型分析建立模型圖 2.1 定位坐標(biāo)和路徑示意圖上圖所示為定位的一種情況(用 1 個(gè)主站+4 個(gè)從站實(shí)現(xiàn)定位)，從站個(gè)數(shù) n（n=4）可隨實(shí)際情況來(lái)

6、設(shè)置?？臻g直角坐標(biāo)系中，移動(dòng)點(diǎn) R 的坐標(biāo)向量待求。3記為已知主站 A 坐標(biāo)向量記為各從站坐標(biāo)向量 B1Bn坐標(biāo)矩陣記為或?qū)憺闉榱瞬僮鞣奖?，記則2.2 非線性方程組的建立根據(jù)時(shí)差觀測(cè)數(shù)據(jù)得到路徑和路徑距離之差，代表的任意一個(gè)。路徑和路徑示意圖如下：因?qū)嶋H測(cè)量的是存在誤差的，記式中為真實(shí)距離差，為誤差干擾?？赏扑愕木嚯x之和，記為，式中，用表示的距離；表示真實(shí)的的距離之和。4由以上條件，空間坐標(biāo)關(guān)系滿足如下方程組：結(jié)合式可依次簡(jiǎn)記為或記為顯然為該方程組為非線性超定方程組。3求解實(shí)現(xiàn)3.1線性方程組的迭代算法由以上分析得到的非線性方程組，一般為非線性超定方程組，很難直接精確求解。一般求解方法是首先將

7、方程組線性化，然后利用數(shù)值方法求得近似解。解非線性方程組的算法有有 Newton 法和 Bronyden 法，均屬于迭代算法。即從給定的初始值出發(fā)，按照一定規(guī)律產(chǎn)生一個(gè)迭代序列，直到該序列收斂到方程組的精確解。當(dāng)?shù)螖?shù)足夠多時(shí)，對(duì)應(yīng)迭代值即可作為方程組的近似解。Broyden 法是一種擬 Newton 法，步驟稍多，本質(zhì)上將非線性方程求解的割線法推廣到求 線性方程組。因在有些計(jì)算中 Newton 法對(duì)非線性方程組求偏導(dǎo)不容易實(shí)現(xiàn)的，而 Broyden 法克服了 Newton 法需要求導(dǎo)求逆的缺點(diǎn)。本文采用經(jīng)典的 Newton 迭代法。因?yàn)橛梢陨戏治霎a(chǎn)生的是橢圓方程組，如(6)式所示，各個(gè)方程

8、形式一致，各個(gè)未知項(xiàng)形式也一致，求偏導(dǎo)數(shù)是容易的。綜上使用 newton法可以快速收斂，是適合本方法的定位求解的。以下對(duì) Newton 法原理進(jìn)行簡(jiǎn)單分析，詳情可以參考相關(guān)算法資料。5記則非線性方程組為或若給出近似根在 rk 處進(jìn)行多元函數(shù)的 Taylor 展開(kāi)，取線性部分。則 F(r)可近似表示為令右端為零，得線性方程組的偏導(dǎo)，又稱為的 Jacobian 矩陣，求得后，方程組(7)的解其中為就可從更新為。3.2 超定方程組求解的最小二乘法上述 Newton 算法分析中，給出了非線性方程組的解的公式，針對(duì)的是方程個(gè)數(shù)等于未知數(shù)的情況來(lái)分析的，若方程式個(gè)數(shù)等于未知數(shù)個(gè)數(shù)，可直接求解解得而在本定位

9、求解中未知數(shù)個(gè)數(shù)為 3，方程式個(gè)數(shù) N3，方程個(gè)數(shù)大于未知量個(gè)數(shù)的方程組即為超定方程組。超定方程組一般是不存在解的方程，無(wú)法求出其精確解，只能求其最小二乘解。利用最小二乘法，得到其解的形式為如果矩陣可逆，可化簡(jiǎn)為式(11)。6最小二乘法解為其中 W 為權(quán)重矩陣為 N*N 的對(duì)角矩陣；;為對(duì)應(yīng)測(cè)量值的權(quán)重。F(R)由上面的分析已給出，為橢球方程組形式，dF(R)為 F(R)的偏導(dǎo)數(shù)，或稱為 jacobian 矩陣，接下來(lái)對(duì) jacobian 矩陣進(jìn)行求解分析。3.3 求解 jacobian 矩陣對(duì)(6)式給出的 F(R)的表達(dá)式進(jìn)行分析，因其各方程形式一致，現(xiàn)以式為例，求其偏導(dǎo)記向量則，同理得j

10、acobian 矩陣表示如下：初值可以根據(jù)實(shí)際情況有多種取法，這里取其中兩個(gè)的中心點(diǎn)坐標(biāo)設(shè)為初值 ，考慮誤差權(quán)重設(shè)置方法；不妨假設(shè)距離值 越大，誤差概率也隨之增大，則權(quán)重越小。這里定義誤差 為某一范圍的隨機(jī)數(shù)，其范圍大小 跟 成比例關(guān)系，7定義權(quán)重 為 的倒數(shù)；在實(shí)際應(yīng)用中可以做相應(yīng)修改。4仿真實(shí)現(xiàn)語(yǔ)言進(jìn)行編程，可在 Windows 操作系統(tǒng)下運(yùn)行。本選用了有強(qiáng)大的數(shù)學(xué)計(jì)算能力和豐富的數(shù)據(jù)處理工具，編程語(yǔ)言接近數(shù)學(xué)表達(dá)，操作簡(jiǎn)單。設(shè)計(jì) GUI 界面也很方便。另外，時(shí)不能編譯符號(hào)運(yùn)算，應(yīng)全部使用數(shù)值運(yùn)算。生成獨(dú)立可執(zhí)行程序1、總體框圖結(jié)構(gòu)分析圖 4.1.輸入數(shù)據(jù)初步處理框圖首先構(gòu)造仿真數(shù)據(jù)。如開(kāi)

11、頭分析所述實(shí)際中獲得的是路徑和路徑距離之差( 對(duì)應(yīng)圖中 Distn) ，由 n+1 個(gè)坐標(biāo)( 對(duì)應(yīng)圖中Basen+1)和移動(dòng)點(diǎn)坐標(biāo)(對(duì)應(yīng)圖中 r)計(jì)算得到。接著再進(jìn)一步進(jìn)行如下圖所示的建模和求解。圖 4.2 求解框圖8n+1 個(gè)坐標(biāo) Basen+1為已知，而實(shí)際測(cè)量得到的是存在誤差的，故在圖中 Distn加入了隨機(jī)噪聲 Noisen，下一步創(chuàng)建非線性方程組 Equan，使用Newton 法或Newton 都能夠就出近似解，記 r1 為近似解，r2 為解。兩種結(jié)果可以進(jìn)行對(duì)比和相互驗(yàn)證。因本程序加入的噪聲是均值噪聲，為減小噪聲的影響，在如框圖所示進(jìn)行 k次求解(每次加入的噪聲都是隨機(jī)數(shù))，然后對(duì)

12、求得的 k 個(gè)解取平均分別得到解 r1和 r2.為了更接近真實(shí)值，k 取值應(yīng)適當(dāng)大一點(diǎn)。另外，為了便于觀察分析，除了給出定位的近似解外，界面另外顯示一下信息：1、創(chuàng)建的 n 個(gè)超定方程組表達(dá)式；2、方程平均誤差，該值越小，則計(jì)算誤差越??；3、迭代次數(shù)和是否收斂的備注(針對(duì)于調(diào)用 Newton 法對(duì) r1 的求解)；4、各點(diǎn)坐標(biāo)示意圖。以便幫助對(duì)本定位算法進(jìn)行更好的分析和理解。2、程序思路分析及結(jié)果演示程序流程圖如下所示，9圖 4.3 程序流程圖10本程序運(yùn)行時(shí)界面如下：圖 4.4 求解結(jié)果演示圖 4.5 對(duì)應(yīng)坐標(biāo)演示11本程序直觀展現(xiàn)了橢圓曲線方程組求解定位坐標(biāo)的方法，定位誤差跟噪聲參數(shù)設(shè)置有

13、關(guān)，并用三維坐標(biāo)圖顯示各坐標(biāo)的位置關(guān)系。程序的求解誤差如上圖所示中噪聲系數(shù)為 0.01，若移動(dòng)點(diǎn)到主站和從站的距離值之和為 500，則變?yōu)榉秶鸀?5005 的一個(gè)隨機(jī)值，最大誤差距離為 5。圖 4.4 的六個(gè)方程式中可知觀測(cè)到的最大誤差距離(取 3 位有效數(shù)字)分別為 6.49，8.39，6.71，9.15，12.7；本次計(jì)算中得到近似解 r1 與實(shí)際點(diǎn) r 的誤差距離為 2.40，法得到的近似解 r2 對(duì)應(yīng)的誤差距離則為 1.66。其中這里進(jìn)行了 10 次重復(fù)求解，再對(duì)結(jié)果取平均，從而使結(jié)果的誤差更小，等同于均值濾波的原理。另外對(duì) r1 和 r2 兩者的誤差大小進(jìn)行對(duì)比分析，進(jìn)行了多次重復(fù)試

14、驗(yàn)，在數(shù)量較少時(shí)，則對(duì)應(yīng)構(gòu)造的方程組表達(dá)式也較少，r1 和 r2 的誤差范圍沒(méi)有明顯差別，當(dāng)添加子增加，r2 誤差范圍將比 r1 更小，顯然更加精確。從站，使方程組式經(jīng)測(cè)試，本程序可以仿真實(shí)現(xiàn)基于時(shí)差觀測(cè)構(gòu)造超定非線性方程組，并對(duì)其求得定位點(diǎn)的近似解，定位誤差較小。5.分析與總結(jié)本程序在項(xiàng)目后期進(jìn)行了多次的修改和測(cè)試，功能演示基本正常，對(duì)大范圍內(nèi)的定位解算良好，如下情況可能出現(xiàn)定位失?。?當(dāng)定位點(diǎn)與某一距離接近于零時(shí)，求解失?。?分布不合理，如幾個(gè)從站與主站過(guò)于緊密時(shí)，求解錯(cuò)誤。在絕大多數(shù)情況下，定位解算是正常，而權(quán)重的設(shè)定根據(jù)實(shí)際情況來(lái)，權(quán)重設(shè)置不合理時(shí)，可能出現(xiàn)某一個(gè)方程的權(quán)重特別大，結(jié)果

15、相當(dāng)于忽略了其他方程，只通過(guò)這個(gè)方程求解，是無(wú)法求出結(jié)果的，考慮到實(shí)際硬件設(shè)施的應(yīng)用是有一定限制的，還應(yīng)根據(jù)實(shí)際應(yīng)用來(lái)修程序設(shè)置的權(quán)重跟坐標(biāo)之間的距離有關(guān)，只能作為一個(gè)簡(jiǎn)單模擬，故各個(gè)的坐標(biāo)點(diǎn)的設(shè)定要合理。從定位的流程上來(lái)分析本定位方法的特點(diǎn)，與其他常用定位方法如 GPS 全球定位做一定的比較，可以看出在本方法中，計(jì)算距離數(shù)據(jù)是由各從站測(cè)量的時(shí)差得到的，不必測(cè)量設(shè)備發(fā)送和接收信號(hào)的時(shí)刻，避免考慮多個(gè)轉(zhuǎn)發(fā)信號(hào)的時(shí)12延，降低轉(zhuǎn)發(fā)干擾，可相應(yīng)簡(jiǎn)化移動(dòng)終端的硬件設(shè)備。與某些定位方法相比的是定位步驟多，需要選擇主站。構(gòu)造超定方程組至少需 5 個(gè)（包括 4 個(gè)方程）才能求解。本次研究的意義在于，通過(guò)設(shè)計(jì)

16、的 GUI 程序?qū)r(shí)差觀測(cè)進(jìn)行定位的方法進(jìn)行解算分析的基本演示，為硬件實(shí)現(xiàn)的可行性提供了參考。參考文獻(xiàn)：【1】. GPS 原理與設(shè)計(jì)M.電子工業(yè).2009 年【2】錦.科學(xué)計(jì)算和 C 程序集M.學(xué)苑. 1998 年【3】,.數(shù)值計(jì)算方法學(xué)習(xí)指導(dǎo)M. 科學(xué).2006 年【4】,.測(cè)時(shí)差定位精度分析與最優(yōu)布站J.火控技術(shù).2003年 01 期【5】高海艦.站組網(wǎng)時(shí)差測(cè)量定位精度算法研究J.系統(tǒng)工程與電子技術(shù).2005 年 04期【6】李,賜.一種新的穩(wěn)健的 TDOA 定位算法J.電子與信息學(xué)報(bào).2006 年 11期【7】.年 08 期迭代法及其應(yīng)用J. 重慶工學(xué)院學(xué)報(bào)(自然科學(xué)版). 2007線性

17、方程的【8】.實(shí)現(xiàn)J. 信息通信. 2011 年 06 期迭代法的13附主要函數(shù)代碼：(1)生成橢圓方程組矩陣函數(shù)function my_f=my_fun(x,a,temp,p)global ht fb;for it=1:(fb+ht)m_f(it)=sqrt(x(1)-a(1)2+(x(2)-a(2)2+(x(3)-a(3)2)+sqrt(x(1)-temp(it,1)2+(x(2)-temp(it,2)2+(x(3)-temp(it,3)2)-p(it);endmy_f=m_f;%其中 x 為迭代值向量，a 為主站坐標(biāo)向量（a1,a2,a3）mq 為坐標(biāo)陣(b1;b2;.bn);p 為%距

18、離向量。(2)對(duì)橢球方程求偏導(dǎo)的函數(shù)為function k=my_df(x,a,b)for i=1:3k(i)=(x(1)-a(1)2+(x(2)-a(2)2+x(3)-a(3)(-0.5)*(x(i)-a(i) +(x(1)-b(1)2+(x(2)-b(2)2+x(3)-b(3)(-0.5)*(x(i)-b(i);endk=k(1) k(2) k(3);(3)調(diào)用 my_df 求橢圓方程組的偏導(dǎo)(Jacobian 矩陣)函數(shù)function df=my_dfun(x,a,temp)global ht fb;for it=1:(fb+ht)dt(it,:)=my_df(x,a,temp(it,

19、:);enddf=dt;14(4)Newton 法迭代函數(shù)function x=newton(x0,eps,N,a0,mq,p)global con1;con1=0;%其中 x0 為迭代初值 eps 為精度要求 N 為最大迭代步數(shù) con1 用來(lái)結(jié)果是否收斂for i=1:Nf=my_fun(x0,a0,mq,p);df=my_dfun(x0,a0,mq);dx=(df*df)(-1)*df*f;x=x0-(dx);if norm(x-x0)epscon1=1;break;endx0=x;end(5)Newton 法函數(shù)與 Newton 函數(shù) newton.m 類似，作簡(jiǎn)單修改即可，開(kāi)頭為fu

20、nction x=newt_w(x0,eps,N,a0,temp,p,w);在 Newton 函數(shù)基礎(chǔ)上修改：其中將 dx=(df*df)(-1)*df*f;改為ct=w*w;%(w*w)dx=(df*ct*df)(-1)*df*ct*f;主函數(shù)的定位過(guò)程實(shí)現(xiàn)代碼：數(shù)據(jù)預(yù)處理：%計(jì)算距離矩陣 p 的函數(shù)(對(duì)應(yīng)上面框圖中的 distn)function t= f_mod(r,s)%求三維點(diǎn) r 與 s 的距離t=sqrt( r(1)-s(1) )2+( r(2)-s(2) )2+( r(3)-s(3) )2);15global fb ht；fb=4;%fb 定義為初始從站個(gè)數(shù)，ht 為增加個(gè)數(shù)%調(diào)用 f_mod 通過(guò)移動(dòng)點(diǎn)和坐標(biāo)生成其距離 ps_1=f_mod(r,a);s_2=ones(1,fb+ht);p=ones(1,fb+ht);for it=1:(fb+ht)s_2(it)=f_mod(r,mq(it,:);%s_2 數(shù)值p(it)=s_1+s_2(it);End定位過(guò)程仿真及求解：%初值 x_0 設(shè)置：取三個(gè)的坐標(biāo)中點(diǎn):%噪聲設(shè)置:通過(guò) rand 來(lái)模擬均值噪聲%求近似解：調(diào)用 Newton%權(quán)重設(shè)置：根據(jù)距離矩陣 p 的大小設(shè)置%求迭代解：調(diào)用 Newton_w%誤差消除：針對(duì)于含有均值誤差的解算結(jié)果，計(jì)算多次再取平均輸出得到結(jié)果更為%接近，%求解