基于TDOA原理計算信號源位置的算法探討

1、基于tdoa原理計算信號源位置的算法探討（c語言）云南省無線電監(jiān)測中心 唐皓 吳季達(dá) 魯東生摘要 目前，在無線電監(jiān)測工作中小型監(jiān)測站以其成本低、體積小、便于維護(hù)等眾多優(yōu)勢得到廣泛應(yīng)用，已逐漸成為行業(yè)趨勢。但是，眾多小型監(jiān)測站目前僅具備監(jiān)測功能，不具備測向功能，給未知信號源的定位帶來諸多不便，因此基于tdoa（time difference of arrival）定位方法的研究便顯得尤為重要。本文中主要討論基于未知信號源到不同監(jiān)測站的時間差，計算未知信號源位置的方法，并用c語言實現(xiàn)。關(guān)鍵詞 tdoa、雙曲線、經(jīng)緯度、時差定位概述tdoa（time difference of arrival）是通

2、過測量無線電信號到達(dá)不同監(jiān)測地點的天線單元時間差，來對發(fā)射無線電信號的發(fā)射源進(jìn)行定位的技術(shù)。根據(jù)平面解析幾何原理，我們知道與平面上兩個定點的距離之差的絕對值為定值的點的軌跡是雙曲線。在實際中，無線電波在空中以光速在空中傳播，當(dāng)有兩個監(jiān)測站搜到未知無線電信號時（若發(fā)射源不在兩監(jiān)測站的中心線上）則信號源一定在以兩個監(jiān)測站為交點的雙曲線上，當(dāng)有三個或三個以上的監(jiān)測站都能收到該信號時，平面中的雙曲線就會有交點，則未知信號源一定在其中的一個交點上。如果我們能找出該交點，并輸出該點的經(jīng)緯度信息，那么就可以確定未知信號源的實際位置。假設(shè)在每個小站都能接收gps時鐘，那么每個監(jiān)測站的時鐘將是同步的，再通過每個

3、監(jiān)測站對每一時隙監(jiān)測到的時域數(shù)據(jù)打上時間戳，并將多個打好時間戳的數(shù)據(jù)返回一臺服務(wù)器，那么就可以通過相關(guān)運算核對波形后找到時間差。用時間差乘與在其環(huán)境中實際的光速則得到了未知信號源到達(dá)兩個監(jiān)測站的距離差。目前由于各個廠商對各自設(shè)備的底層數(shù)據(jù)及接口大多都不開放，無法得到統(tǒng)一的數(shù)據(jù)結(jié)構(gòu)并進(jìn)行相關(guān)運算的程序開發(fā)。因此，本文假設(shè)了以下條件進(jìn)行討論：1、 假設(shè)有三個監(jiān)測站能收到未知信號源的無線信號；2、 假設(shè)已通過相關(guān)算法得到了較為準(zhǔn)確時間差，此時可以計算出未知信號源到達(dá)各個監(jiān)測站的距離差；3、 假設(shè)地球是正球體；4、 假設(shè)未知信號源為全向發(fā)射；圖1如圖1示，當(dāng)確定到達(dá)每兩個監(jiān)測站的距離差后就可以畫出一對

4、雙曲線，同理可以畫出3對雙曲線，由于三對雙曲線中任意兩對都有共同的焦點，因此在平面內(nèi)必有交點。一、關(guān)鍵問題分析1、計算地球上任意兩點間的距離在直角坐標(biāo)系中建立方程組并求解，不難求出若干條雙曲線的交點，若采用此方法，實際應(yīng)用中就會遇到經(jīng)緯度信息與直角坐標(biāo)的變換，而且整體過程對單位的控制不好把握，相對繁瑣。考慮到地球為近似球體，如果直接采用經(jīng)緯度信息作為方程變量，則輸出交點也為經(jīng)緯度信息，也便于c語言實現(xiàn)，因此使用球坐標(biāo)系建模計算比較可行。地球是一個近乎標(biāo)準(zhǔn)的橢球體，其赤道半徑為6,378.140 km，極半徑為6,356.755 km，平均半徑6,371.004 km。如果我們假設(shè)地球是一個完美

5、的球體，那么其半徑就是地球的平均半徑，記為r，如圖2示。如果以0度經(jīng)線為基準(zhǔn)，那么根據(jù)地球表面任意兩點的經(jīng)緯度就可以計算出這兩點間的地表距離（這里忽略地球表面地形對計算帶來的誤差，對于就方圓幾十公里范圍內(nèi)的監(jiān)測站使用，正球體計算帶來的誤差極?。D2設(shè)第一點a的經(jīng)緯度為(lona, lata)，第二點b的經(jīng)緯度為(lonb, latb)，按照0度經(jīng)線的基準(zhǔn)，東經(jīng)取經(jīng)度的正值(longitude)，西經(jīng)取經(jīng)度負(fù)值(-longitude)，北緯取90-緯度值(90-latitude)，南緯取90+緯度值(90+latitude)，則經(jīng)過上述處理過后的兩點被計為(mlona, mlata)和(mlo

6、nb, mlatb)。那么根據(jù)三角推導(dǎo)，可以得到計算兩點距離的如下公式：s=sinmlatasinmlatacosmlona-mlonb+cosmlatacosmlatb (1)distance= (rarccos(s)/180 (2)注：s的單位為度，distance的單位為米，下文中a、b兩點的distance簡寫為da-b。2、求解雙曲線交點雙曲線，即與平面上兩個定點距離之差的絕對值為定值的點的軌跡。對未知信號源的定位至少需要3個監(jiān)測站，如果我們僅考慮3個站的情況，以任意兩個監(jiān)測站為交點可以畫出1對雙曲線，此時共有3對雙曲線。當(dāng)3對雙曲線中的任意3條相交時就是未知信號源可能存在的點。由于

7、6取3的組合有20種可能，且任意1對雙曲線不可能與剩余4條中的1條共交一個點，所以3對雙曲線其中3條相交于同一點的最大可能為20-43=8個，因此在程序中需要8個數(shù)組存放可能存在的8組相互近似的交點。利用程序解方程組時通常是設(shè)定一個極小值a，將方程化為f(x,y)=0形式，用點（x1,y1）不停地遞增或遞減一個步進(jìn)值step，當(dāng)代入該點的兩個式子之差的絕對值小于極小值a時，便認(rèn)為存在交點（x1,y1），將其統(tǒng)統(tǒng)存放于一個結(jié)果數(shù)組。|f1(x1,y1)- f2(x1,y1)-distance|a (3) 由于在程序中使用經(jīng)緯度做變量進(jìn)行計算，且以距離“米”為單位控制計算精度，因此在程序中需要有用

8、經(jīng)緯度來表示距離的對應(yīng)關(guān)系。我們知道在地球赤道附近經(jīng)度每差1度，實際距離相隔約111 km，即n=111 000，當(dāng)使用error/n表示步長step時，能使精度隨緯度升高而提高。當(dāng)在每次計算中取極小值a為“error/n/3.3”時，整個計算的精度將控制在誤差error以內(nèi)，error值由人為定值。3、合并相似點 根據(jù)(3)式步進(jìn)得到的點可能有很多，本文稱這些點為相似點，其共有特征是都能使(3)式成立，為此，需要進(jìn)一步甄別，依前述可知，3個站點至多可能有8個交點，于是應(yīng)當(dāng)根據(jù)某種算法把8組相互近似的交點區(qū)別清楚。本文根據(jù)同組相似點應(yīng)當(dāng)相關(guān)性較大，反之即為差異性較小，于是令(xi,yi)表征結(jié)

9、果數(shù)組中的元素，i為正整數(shù)。依下述公式：distancexi,yi,xi-1,yi-12*error (4) 此時，以經(jīng)緯度兩點距離公式(1)、(2)進(jìn)行計算，當(dāng)兩點間距離小于2*error時，可以默認(rèn)為該點為同一個點的近似值，單位是米。當(dāng)(xi,yi)中有滿足(4)式的點時，將其逐一分組，分別存放在8個數(shù)組中，然后根據(jù)(5)式對其求均值，以此值作為該組的代表點，最后可得至多8個交點。p(x,y)=(x_i )/(i),(y_i )/(i) (5)二、仿真模型1、常量定義如下：常量名數(shù)值單位說明r6 371 004米地球半徑（近似球體）n111 000米地球赤道上每一經(jīng)度對應(yīng)的距離2、變量定義

10、如下：常量名數(shù)值單位說明error100米計算的誤差精度，人工輸入d20公里掃描區(qū)域邊界拓展寬度，人工輸入steperror/n/3.3度定義了變量x、y的增量x_n待計算度存放可能存在的交點的緯度y_n待計算度存放可能存在的交點的經(jīng)度3、題設(shè)a地球是球形的，采用球坐標(biāo)系建模，認(rèn)為在方圓20公里內(nèi)近地面是平面；b三個監(jiān)測站的經(jīng)緯度分別為：a(x1,y1)，b(x2,y2)，c(x3,y3)； cr1、r2、r3為真假判決值，取1時表示條件成立；ds1_2、s1_3、s2_3分別為未知信號源任意兩站點間的距離差，使用式（1）、（2）進(jìn)行計算；f未知信源記為點o，未知信號源的緯度為x，經(jīng)度為y；g

11、o點信號到達(dá)a點與b點的時間差為td1，距離差為sd1，o點信號到達(dá)b點與c點的時間差為td2，距離差為sd2，o點信號到達(dá)a點與c點的時間差為td3，距離差為sd3；h. a= error/3，b=2*error；三、算法設(shè)計1、計算地球上任意兩條雙曲線的交點在程序中如用變量“x”表示緯度，用“y”表示經(jīng)度，“s”表示距離，則兩點的距離差可以表示為（其中/1801/57.2958），依式（1）有：s=fabs(r*acos(sin(x2/57.2958)*sin(x1/57.2958)+cos(x2/57.2958)*cos(x1/57.2958)*cos(y2-y1)/57.2958).求

12、雙曲線交點本文采用窮舉法，以step為步進(jìn)值，遍歷可行域，該域在三站點經(jīng)緯度的最值之左右上下各拓展d的矩形區(qū)域，在此域?qū)ふ曳吓袥Q條件的點，然后將這些點存于結(jié)果數(shù)組。為保證下次判決有效，每次判決結(jié)束后都要對r值清零。流程圖如下：跳出循環(huán)導(dǎo)入監(jiān)測站點的經(jīng)緯度值、未知信號到任意兩站點的時間差，變量初始化計算站點經(jīng)緯度的最大（最?。┲祒,y=min(x,y)yx,y=max(x,y)？nr1=1do-ado-b sd1 =a？do-bdo-c sd2=a？do-ado-c sd3 =a？ynnyyr2=1r3=1r1 & r2 & r3=1?ynn(xi,yi)=(x,y)i+r1=0r2=0r3=

13、0x（y）+=step窮舉法掃描雙曲線交點流程圖計算雙曲線交點的程序段：if(fabs(fabs(r*acos(sin(x/57.2958)*sin(x1/57.2958)+cos(x/57.2958)*cos(x1/57.2958)*cos(y-y1)/57.2958)-fabs(r*acos(sin(x/57.2958)*sin(x2/57.2958)+cos(x/57.2958)*cos(x2/57.2958)*cos(y-y2)/57.2958)-s1_2)(error/3) r1=1;if(fabs(fabs(r*acos(sin(x/57.2958)*sin(x1/57.2958)

14、+cos(x/57.2958)*cos(x1/57.2958)*cos(y-y1)/57.2958)-fabs(r*acos(sin(x/57.2958)*sin(x3/57.2958)+cos(x/57.2958)*cos(x3/57.2958)*cos(y-y3)/57.2958)-s1_3)(error/3) r2=1;if(fabs(fabs(r*acos(sin(x/57.2958)*sin(x2/57.2958)+cos(x/57.2958)*cos(x2/57.2958)*cos(y-y2)/57.2958)-fabs(r*acos(sin(x/57.2958)*sin(x3/5

15、7.2958)+cos(x/57.2958)*cos(x3/57.2958)*cos(y-y3)/57.2958)-s2_3)(error/3)r3=1;if(r1&r2&r3) r1=0;r2=0;r3=0;。/ 下一步執(zhí)行語句 2、合并相似點程序段 按照（4）、（5）兩式，進(jìn)行程序設(shè)計，為保證(xi,yi)中所有元素都能被本組，需要附加一個if語句進(jìn)行判斷，若下一個待判決的元素任然存在，則將其附為下一組的首元素，然后繼續(xù)執(zhí)行判別語句，否則，則繼續(xù)執(zhí)行分組甄別的while循環(huán)語句，實際上此時后面的while語句都不執(zhí)行了，而那最后一個元素也被分組了。這樣就防止出現(xiàn)最后一個元素被遺漏的情況發(fā)生

16、。輸入交點數(shù)組(xi,yi)(d(xi+1,yi+1),(xi,yi)=0）?(x1,y1)=(xi+1,yi+1);i-;i=0？(x2,y2)=(xi+1,yi+1);i-;nyyn(d(xi+1,yi+1),(xi,yi)=0)?(x7,y7)=(xi+1,yi+1);i-;i=0？(x8,y8)=(xi+1,yi+1);i-;nyyn(x1, y1)(x7, y7)輸出交點(x1, y1)至 (x8, y8)的經(jīng)緯度相似點分組 程序段如下：while(i=0&fabs(r*acos(sin(x_ii/57.2958)*sin(x_ii-1/57.2958) + cos(x_ii/57.

17、2958)*cos(x_ii-1/57.2958)*cos(y_ii-y_ii-1)/57.2958)=0) /若第一組元素取完后，x_i，y_i數(shù)組中仍有元素，繼續(xù)執(zhí)行以下代碼 x_1i1=x_ii;y_1i1=y_ii;i-;i1+=1;對數(shù)組中的元素求平均值，程序段如下：double average(double array,int m)/求數(shù)組平均值 int k;double aver;double sum;sum=array0;if(m0)for(k=1;km;k+)sum=sum+arrayk;aver=sum/m;elseaver=0;return(aver);四、驗證仿真結(jié)果選

18、取昆明地區(qū)的三個固定監(jiān)測站1、 省監(jiān)測中心站（24.9889 102.6570）2、 五華山監(jiān)測站（25.049358 102.706879）3、 機(jī)場監(jiān)測站 （25.012774 102.74032）設(shè)未知信號源位置為（24.979197 102.714763）三個固定監(jiān)測站點及“未知信號源”實際位置圖3示：圖3利用google earth測距功能測出信號源與監(jiān)測站的距離分別為：s1=5933m; s2=7838m; s3=4532m; s1_2=1905m;s1_3=1401ms2_3=3306m 將以上變量輸入程序，取error為100米，掃描半徑為20公里，運行程序，輸出見圖4：圖4從

19、輸出可知，在方圓約20公里的范圍內(nèi)，共存在兩個三線相交點，分別為：計算信號源位置1（24.979363 102.714758）計算信號源位置2（25.033645 102.677175）在實際操作中可以根據(jù)監(jiān)測到最大值的監(jiān)測站應(yīng)靠近發(fā)射源（假設(shè)發(fā)射源為全向發(fā)射）的原則來甄別計算點中的真實點。在本例中假設(shè)已經(jīng)通過以上原則判定計算真值為“計算信號源位置1（24.979363 102.714758）”，如圖5示：圖5圖6 放大圖圖6如上可知，計算誤差為19.87米五、結(jié)論本文敘述了一種tdoa定位算法，算法以可調(diào)的窮舉方式偵測可行域內(nèi)的點，巧妙地避開了經(jīng)緯度坐標(biāo)與直角坐標(biāo)系的映射問題，轉(zhuǎn)而使用經(jīng)緯度坐標(biāo)系下的兩點間的距離，便于整體算法在數(shù)據(jù)單位上的統(tǒng)一。