一種基于tdoa的快速定位方法
【專利摘要】本發(fā)明公開了一種基于TDOA的快速定位方法，其主要目的在于解決被測目標在近距、遠距多場景下依靠多個信號接收器探測的時間差進行位置求解的問題。本發(fā)明的主要步驟包括：坐標與距離方程的建立、中心距離的求解、目標坐標的求解、建立距離約束和球面關系約束，求解目標坐標的修正解。本發(fā)明可以解決水下航行器被動式探測、定位問題，也可用于無線電定位、超聲波定位、室內定位等其他基于TDOA的定位問題。相比于目前普遍采用的Chan算法，解決了近距、遠距算法不統(tǒng)一，存在模糊解的問題，精度和穩(wěn)定性均得到進一步提高。比較于Taylor算法，無需外部初值和遞歸運算，大大減小運算量，而精度和穩(wěn)定性不變。
【專利說明】
一種基于TDOA的快速定位方法
技術領域
[0001] 本發(fā)明涉及空間被動定位等應用，尤其是水下導航定位領域，具體地說是涉及一 種基于TD0A的快速定位方法。
【背景技術】
[0002] 衛(wèi)星導航和慣性導航日益成為民用和軍用領域的主要導航方式，而衛(wèi)星導航和慣 性導航的局限性也促使越來越多的其他方式的導航技術不斷發(fā)展。比如水下航行器由于高 度隱蔽的特點，不能主動定位，衛(wèi)星導航沒法在水下進行，慣性導航長時間工作會帶來誤差 積累，因此需要一種可靠的，精確的水下導航系統(tǒng)提供水下坐標的定位。
[0003] 基本ID0A(到達時間差）的定位技術是利用聲源信號到達各接收器的時間差，計算 出聲源位置的一種導航方法。由于只需要被動地接收聲源信號，計算過程在接收端完成，加 上水下聲信號傳播的高效性，因此十分適合航行器的空間定位和探測。該定位方法從最早 的無線電導航，羅蘭一 C系統(tǒng)，到如今日益發(fā)展的室內導航系統(tǒng)在原理上都是如此，都需要 解決非線性方程組的求解問題。該領域中，最廣為人知的是Chan算法，該算法分為兩種情況 對遠距和近距分別提供了計算方案，且會出現(xiàn)模糊解的情況。Taylor算法是目前最精準的 算法，但依賴一個初始值進行迭代，計算量大，一旦初始值誤差較大，會出現(xiàn)發(fā)散的現(xiàn)象。針 對目前主流算法的不足，本發(fā)明提出了一種新的定位計算方法，統(tǒng)一了遠近距的計算方式， 避免了模糊解的問題，更重要的是在簡單的步驟下，能達到Taylor算法的定位精度。對基于 ID0A的快速被動定位技術提供了計算支撐。

【發(fā)明內容】

[0004] 發(fā)明目的：本發(fā)明提供一種基于TD0A的快速定位方法，主要在于解決被測目標在 近距、遠距多場景下依靠多個信號接收器探測的時間差進行位置求解的問題。
[0005] 本發(fā)明的技術方案具體如下：
[0006] 一種基于TD0A的快速定位方法，包括以下步驟：
[0007] 步驟1:根據(jù)已知的各接收器采集的信號71(丨），1 = 0，1，2,3~1信號傳播速度
[0008] C，各接收器時延協(xié)方差矩陣Q，各接收器布放坐標(Xi，yi，Zi)，i = 0，l，2,3~N， [0009]獲取由目標位置發(fā)出的聲源信號；
[0010]步驟2:建立關于目標位置的方程組；
[0011]步驟3:對步驟2中的方程組進行消元，變化求取僅包含未知量中心距離H)的方程 組；
[0012] 步驟4:計算中心距離r〇;
[0013] 步驟5:根據(jù)步驟4得到的中心距離計算目標位置粗估計；
[0014] 步驟6:利用步驟4、5計算出的中心距離ro和目標位置粗估計坐標建立包含約束方 程的方程組；
[0015] 步驟7:根據(jù)步驟6的包含約束方程的方程組，重新計算得到更新的目標位置。
[0016] 所述步驟1具體為：
[0017] 將待測目標作為聲源發(fā)出聲信號x(t)，各接收器接收到N+1個信號，記為yi(t)，i =0，1，2,3^々表示各接收器的編號，其中0號接收器為參考接收器;將其他各接收器的信號 yi(t)，i = l，2,3-_N與參考接收器的信號yQ(t)作相關運算，得到N個互相關函數(shù)RlQ( T)，i = 1，2，3…N;對RlQ( t )進行峰值監(jiān)測，結合先驗知識排除模糊峰干擾，得到各接收器相對于參 考接收器接收信號x(t)的時間差TlQ，i = l，2,3"_N;進一步可以得到距離差riQ = CTlQ，i = l， 2,3---N〇
[0018]所述步驟2具體為：
[0019]根據(jù)空間幾何的位置關系，得到關于目標位置的方程組：
[0021]
是目標到參考接收器的距離，即為中心 距離；約:，i = 1，2，3...N是誤差;Ki = xi2+yi2+zi2，i = 0，1，2，3...N;
[0022]將上式寫成矩陣形式：
[0025]所述步驟3具體為：
[0026]對所述關于目標位置的方程組進行消元，消去x，y，z，得到以下形式的方程組：
[0028]其中ha人，ki，i = l，2,3^_N由矩陣行變換得到;具體計算公式為：

[0033] 這里需要注意的是，|A|乒0,即編號0，1，2,3的四個接收器不能共面;另外N彡4,即 接收器的數(shù)量至少5個，保證本算法方程有解;至此得到關于中心距離ro的方程組：
[0034] ga ? r〇 = ha7 2+07 2
[0036]所述步驟4具體為：
[0037]忽略誤差?'2，則r〇的最小二乘解為4 = (ga1' ? ga)-1? fe;;
[0038]所述步驟5具體為：
[0039]將所述步驟4求得的4代入目標位置方程，得到關于x，y，z的方程組：
[0042] 利用加權最小二乘法計算出目標位置：
[0043] (x，y，z)T= (GbT ?也―1 ? Gb)-1 ? GbT ?也―1 ? hb
[0044] 其中，協(xié)方差陣it = c2 ? B ? Q ? B，
表示目標到各接收器的 距離，利用G 6 A關系計算；
[0045] 所述步驟6具體為:對坐標解(x，y，z)進行修正，采用在步驟2建立的方程組基礎上 增加兩個約束方程再進行加權最小二乘，一個方程為中心約束方程：
[0046] r0 = 4
[0047] 另一個方程為球面關系約束方程：
[0048] r〇2= (x-xo)2+(y-yo)2+(z-zo)2
[0049] 將上式非線性方程線性化，得到線性化后的近似方程：
[0051 ] 將上述兩式和步驟2的方程組整理可得：
[0055] 利用加權最小二乘法計算出最終位置：
[0056] (x，y，z)T= (GcT ? V -1 ? Gc)-1 ? GcT ? V -1 ? he
[0058]將r〇2=(叉1〇)2+(71〇)2+(2-2()) 2非線性方程線性化，要解釋這個過程，先從二維空 間說明。對于二維問題，寫成ro2 = (x-xo)2+ (y-yo)2，將x，y，ro當一個三維空間，則上式為一 個錐面方程，方程組
^的加權最小二 乘解可以理解為到各平面和錐面的加權距離最小的點，所以把錐面上到粗估計點最近的切 面作為錐面的線性近似，該切面的法線向量為
，加權最小二乘解可以近 似為到各平面和切面的加權距離最小的點。現(xiàn)在把問題擴展至三維，即X，y，Z，rQ組成一個 四維空間，雖然不易直觀表述，但原理相同。
[0059] 考慮到計算難度，這里做幾點近似假設。首先死^?和供相互獨立，即協(xié)方差為 零。其次0破，^?呈反比例，在實驗中發(fā)現(xiàn)，球面關系約束在遠距環(huán)境下的效果不佳;而在近 距環(huán)境下效果很好。也就是說，遠距時，中心距離約束誤差相對較小，而粗估計求出的切面 作為近似時誤差較大;近距時，粗估計可信度高，球面關系約束可使得誤差減小，其加權值 應高于中心距離約束的加權值。故以中心距離和時延標準差之比作為加權值平衡的標準。 其效果也得到仿真實驗的驗證。
[0060] 本發(fā)明通過坐標與距離方程的建立、中心距離的求解、目標坐標的求解、建立距離 約束和球面關系約束，求解目標坐標的修正解等步驟，解決水下航行器被動式探測、定位問 題，也可用于無線電定位、超聲波定位、室內定位等其他基于TD0A的定位問題。與現(xiàn)有技術 相比，本發(fā)明具有如下優(yōu)點：
[0061] l)Chan算法對于遠距情況采用一次加權最小二乘法求解，近距情況在遠距情況的 計算基礎上增加了一次加權最小二乘，并進行一次修正。而對于遠距和近距的劃分界限并 沒有明確說明，即多遠距離屬于遠距，多近屬于近距沒有明確的界限。本發(fā)明統(tǒng)一了遠距和 近距的方法，無論遠距近距都適用，也就是說在使用場景上沒有距離區(qū)分判斷的環(huán)節(jié)； [0062] 2)本發(fā)明在最后的位置修正環(huán)節(jié)采用中心距離約束和球面關系約束，而不是Chan 算法的誤差修正，計算簡單，效果良好，更重要的是不會出現(xiàn)開方計算產生多個解。因此不 會像Chan算法那樣存在后續(xù)需要甄別模糊解的步驟；
[0063] 3)本發(fā)明在精度方面達到Taylor算法的效果，無論遠距還是近距。因此其效果是 優(yōu)于Chan算法的，尤其是遠距環(huán)境下，精度可以提高30%。而相比較于Taylor算法遞歸帶來 大量計算量，本發(fā)明的方法顯然計算更加簡單，方便。
[0064] 4)對于噪聲方差較大的情況，Taylor迭代發(fā)散的幾率很大，不宜采用，本發(fā)明的方 法由于不需要初始值，在這點上也是超過Tay lor算法的。
【附圖說明】
[0065]圖1為本發(fā)明的計算原理圖；
[0066]圖2為目標聲源到水聽器的幾何示意圖；
[0067]圖3為距離仿真實驗目標聲源和水聽器的布放示意圖；
[0068]圖4為本發(fā)明和其他三種算法在不同距離下仿真結果的均方根誤差圖；
[0069]圖5為本發(fā)明和其他三種算法在不同距離下仿真結果的誤差標準差圖；
[0070] 圖6為本發(fā)明和其他三種算法在不同大小噪聲下仿真結果的均方根誤差圖；
[0071] 圖7為本發(fā)明和其他三種算法在不同大小噪聲下仿真結果的誤差標準差圖。
【具體實施方式】
[0072] 下面結合具體實施例，進一步闡明本發(fā)明，應理解這些實施例僅用于說明本發(fā)明 而不用于限制本發(fā)明的范圍，在閱讀了本發(fā)明之后，本領域普通技術人員對本發(fā)明的各種 等價形式的修改均落于本申請所附權利要求所限定的范圍。
[0073]如圖1所示，本發(fā)明所述的一種基于TD0A的快速定位方法，包括以下步驟：
[0074]步驟1:根據(jù)已知的各接收器采集的信號71(〇，1 = 0，1，2,3~1信號傳播速度(：， 各接收器時延協(xié)方差矩陣Q，各接收器布放坐標(xi，yi，Zi)，i =0，1，2，3…N，獲取由目標位 置發(fā)出的聲源信號;具體為：
[0075]將待測目標作為聲源發(fā)出聲信號x(t)，各接收器接收到N+1個信號，記為yi(t)，i =0，1，2,3^々表示各接收器的編號，其中0號接收器為參考接收器;將其他各接收器的信號 yi(t)，i = l，2,3-_N與參考接收器的信號yQ(t)作相關運算，得到N個互相關函數(shù)RlQ( T)，i = 1，2，3…N;對RlQ( t )進行峰值監(jiān)測，結合先驗知識排除模糊峰干擾，得到各接收器相對于參 考接收器接收信號x(t)的時間差TlQ，i = l，2,3"_N;進一步可以得到距離差riQ = CTlQ，i = l， 2,3---N〇
[0076]步驟2:建立關于目標位置的方程組；
[0077]根據(jù)空間幾何的位置關系，得到關于目標位置的方程組：
[0079]
是目標到參考接收器的距離，即為中心 距離；約，i = 1，2，3...N是誤差;Ki = xi2+yi2+zi2，i = 0，l，2,3...N;
[0080] 將上式寫成矩陣形式：
[0083] 步驟3:對步驟2中的方程組進行消元，變化求取僅包含未知量中心距離ro的方程 組；
[0084] 對所述關于目標位置的方程組進行消元，消去x，y，z，得到以下形式的方程組：
[0086]其中ha人，ki，i = l，2,3^_N由矩陣行變換得到;具體計算公式為：

[0091] 這里需要注意的是，|A|乒0,即編號0，1，2,3的四個接收器不能共面；另外N>4， 即接收器的數(shù)量至少5個，保證本算法方程有解;至此得到關于中心距離ro的方程組：
[0092] ga ? r〇 = ha7 2+(I)/ 2
[0094] 步驟4:計算中心距離ro;
[0095] 忽略誤差①'2,則r〇的最小二乘解為々=(gV f/) .gt/' ./w:
[0096] 步驟5:根據(jù)步驟4得到的中心距離ro計算目標位置粗估計；
[0097]將所述步驟4求得的&代入目標位置方程，得到關于x，y，z的方程組：
[0100] 利用加權最小二乘法計算出目標位置粗估計：
[0101] (x，y，z)T= (GbT ?邊―1 ? Gb)-1 ? GbT ?邊―1 ? hb
[0102]其中，
，ri表示目標到各接收器的 距離，利用^ ~&關系計算。
[0103] 步驟6:利用步驟4、5計算出的中心距離ro和目標位置粗估計坐標建立包含約束方 程的方程組；
[0104] 對坐標解(x，y，z)進行修正，采用在步驟2建立的方程組基礎上增加兩個約束方程 再進行加權最小二乘，一個方程為中心約束方程：
[0105]
[0106] 另一個方程為球面關系約束方程：
[0107] r〇2= (x-xo)2+(y-yo)2+(z-zo)2
[0108] 將上式非線性方程線性化，要解釋這個過程，先從二維空間說明。對于二維問題， 上式寫成n)2=(x-x〇) 2+(y-y())2,將x，y，r〇當一個三維空間，貝lj上式為一個錐面方程，方程組
的加權最小二乘解可以理解為到各平面和錐面 的加權距離最小的點，所以把錐面上到粗估計點最近的切面作為錐面的線性近似，該切面 的法線向量為
，加權最小二乘解可以近似為到各平面和切面的加權距 離最小的點?，F(xiàn)在把問題擴展至三維，即X，y，Z，r〇組成一個四維空間，雖然不易直觀表述， 但原理相同，所以球面約束方程可以近似為如下近似方程：
[0110] 將上述兩式和第二步的方程組整理可得：
[0114] 利用加權最小二乘法計算出最終位置：
[0115] (x，y，z)T= (GcT ? V -1 ? Gc)-1 ? GcT ? V -1 ? he
[0117] 考慮到計算難度，這里做幾點近似假設。首先和灼相互獨立，即協(xié)方差為 零。其次^呈反比例，在實驗中發(fā)現(xiàn)，球面關系約束在遠距環(huán)境下的效果不佳;而在近 距環(huán)境下效果很好。也就是說，遠距時，中心距離約束誤差相對較小，而粗估計求出的切面 作為近似時誤差較大;近距時，粗估計可信度高，球面關系約束可使得誤差減小，其加權值 應高于中心距離約束的加權值。故以中心距離和時延標準差之比作為加權值平衡的標準。 其效果也得到仿真實驗的驗證。
[0118] 步驟7:根據(jù)步驟6的包含約束方程的方程組，重新計算得到更新的目標位置。
[0119] 如圖2、3所示，以水下航行器探測定位為例，當水下目標進入探測區(qū)域后，其發(fā)出 的特定頻率的噪聲被各接收器獲得，一般水下接收器為水聽器。由于各水聽器到目標位置 的距離不一樣，因此聲音信號到達各水聽器的時間也有所不同，各水聽器呈現(xiàn)出的是波形 相似，強度不同，時序上相差一定平移量的信號。利用互相關解算技術可以求出各水聽器波 形相對于參考水聽器波形的時序平移量，即時間差。作為算法的輸入量，我們還需要提前標 定好該區(qū)域的聲速值，各水聽器的測量誤差方差，以及各水聽器布放的具體坐標。將上述信 息輸入本發(fā)明方法，可得出目標位置的坐標。計算結果隨水聽器測量誤差的增大而增大，另 外遠距離的定位誤差要大于近距離的誤差，增加水聽器的數(shù)目可以提高定位精度。
[0120] 本發(fā)明在計算機上對各種輸入參數(shù)進行了 1000次仿真實驗，對Chan算法，SI算法， Taylor算法以及本發(fā)明方法進行了對比，統(tǒng)計計算結果的均方根誤差均值和標準差。仿真 分為兩組，第一組比較的是距離對精度的影響，第二組比較的是測量誤差對精度的影響。模 擬的時間差采用目標位置到各水聽器的距離加上一個均值為零的白噪聲得到。實驗中，聲 速設為1500m/s。本發(fā)明至少需要5個水聽器（1個參考水聽器+4個水聽器），仿真中采用1個 參考水聽器+6個水聽器組成基陣，具體坐標見表1。
[0121] 表1水聽器的布放位置
[0123]第一組仿真實驗比較的是距離對精度的影響。設定測量誤差為5X1(T3s，位置從(_ 4200,4600,100)起始到（350,50,100)結束的直線路徑，位置移動步長為99m。每移動一個步 長，以直線上該點為中心，邊長為70m的立方空間隨機選1000個點作為每一次的目標位置， 最后統(tǒng)計這1000個測試的均方根誤差和標準差，如圖3。路徑上共65個步長，仿真會得到由 遠至近65個計算結果。仿真結果如圖4、5。均方根誤差圖反映的是計算結果偏離實際位置的 大小。圖4中可以看出，Chan算法在遠距環(huán)境下誤差非常大，近距環(huán)境下效果不錯。相反，SI 算法在遠距環(huán)境效果不錯，近距環(huán)境下的效果則不如Chan算法。Taylor算法在遠距和近距 環(huán)境下誤差都最小，效果最好。而本發(fā)明方法在遠近距精度均優(yōu)于Chan和SI算法，接近于 Taylor算法，考慮到其計算簡便，無需初始值等方面優(yōu)勢，因此可以認為發(fā)明是具有明顯意 義的。誤差標準差圖反映的是計算結果的誤差波動大小，是算法穩(wěn)定性的體現(xiàn)。圖5是誤差 標準差對比圖，其特點和均方根誤差圖基本相似。
[0124] 第二組仿真實驗比較的是測量誤差對精度的影響。設定目標位置為以（3500，200， 0)為中心，200m為邊長的矩形區(qū)域隨機選點。實驗的幾種測量誤差見表2,每種誤差進行 1000次蒙特卡洛實驗，統(tǒng)計測試的均方根誤差和標準差，仿真結果如圖6、7。從圖6的均方根 誤差圖可以看出，隨著測量誤差的增大，定位誤差也增大。其中各測量誤差下定位誤差大小 依次為Chan算法，SI算法，本發(fā)明方法和Taylor算法。在測量誤差較低的時候，Chan算法的 誤差相對來講過大。另外，在測量誤差較大的時候，由于初始值誤差大，遞歸發(fā)散等原因， Taylor算法無法得到最終結果。相比較而言，本算法的穩(wěn)定性則較高，再考慮到其較高的精 度，因此本算法效果較好。
[0125] 表2.時延白噪聲標準差
[0127] 通過上述仿真實驗的驗證，我們可以得出結論，無論在遠距還是近距，小噪聲還是 大噪聲環(huán)境下，本發(fā)明方法在精度和穩(wěn)定性上均優(yōu)于Chan算法和SI算法。Taylor算法雖然 在大部分情況下表現(xiàn)優(yōu)異，但其精度和穩(wěn)定性和本發(fā)明方法相差無幾，而在大噪聲環(huán)境下 則波動嚴重，計算異常，這對于抗干擾性較差的系統(tǒng)來說是致命的?？紤]到Taylor算法還需 要外部提供初始值，迭代計算量大等因素，本發(fā)明方法簡單，快速，穩(wěn)定的特點彌補了較之 于Taylor算法高精度上的微弱劣勢。此外，對于處理能力較強的系統(tǒng)，也可以將本發(fā)明方法 的結果作為Taylor算法的初始值，由于初始值的精度和穩(wěn)定性更高，遞歸發(fā)散的概率降低， 其結果只會比現(xiàn)有Taylor算法的效果有過之而無不及。不過從圖4、6中可以看出，由于本發(fā) 明方法精度很高，因此效果提升微弱，可根據(jù)需求自行選擇。
【主權項】
1. 一種基于TDOA的快速定位方法，其特征在于，包括以下步驟： 步驟1:根據(jù)已知的各接收器采集的信號71(〖），1 = 0，1，2,3 - 1信號傳播速度(：，各接收 器時延協(xié)方差矩陣Q，各接收器布放坐標(xi，yi，Zi)，i = 0，1，2，3 - N，獲取由目標位置(x，y， z)發(fā)出的聲源信號； 步驟2:建立關于目標位置的方程組； 步驟3:對步驟2中的方程組進行消元，變化求取僅包含未知量中心距離ro的方程組； 步驟4:計算中心距離r〇; 步驟5:根據(jù)步驟4得到的中心距離計算目標位置粗估計； 步驟6:利用步驟4、5計算出的中心距離和目標位置粗估計坐標建立包含約束方程的 方程組； 步驟7:根據(jù)步驟6的包含約束方程的方程組，重新計算得到更新的目標位置。2. 根據(jù)權利要求1所述的基于ID0A的快速定位方法，其特征在于，所述步驟1具體為： 將待測目標作為聲源發(fā)出聲信號x(t)，各接收器接收到N+1個信號，記為 yi(t)，i = 0，1， 2,3…N表示各接收器的編號，其中0號接收器為參考接收器;將其他各接收器的信號71(〇， i = l，2,3~N與參考接收器的信號yo(t)作相關運算，得到N個互相關函數(shù)Ri〇⑴，i = l，2， 3…N;對RiQ(t)進行峰值監(jiān)測，結合先驗知識排除模糊峰干擾，得到各接收器相對于參考接 收器接收信號x(t)的時間差T iQ，i = l，2,3…N;進一步可以得到距離差riQ = cTiQ，i = l，2，3."N〇3. 根據(jù)權利要求1所述的基于ID0A的快速定位方法，其特征在于，所述步驟2具體為： 根據(jù)空間幾何的位置關系，得到關于目標位置的方程組：是目標到參考接收器的距離，即為中心距離； 灼，f = 1，2,3…JV是誤差;Ki = xi2+yi2+zi2，i = 0，l，2,3."N; 將上式寫成矩陣形式：所述步驟3具體為： 對所述關于目標位置的方程組進行消元，消去x，y，z，得到以下形式的方程組：其中ha'，①'，ki，i = l，2,3^_N由矩陣行變換得到;具體計算公式為：這里需要注意的是，|A|乒0,即編號0，1，2,3的四個接收器不能共面；另外N彡4,即接收 器的數(shù)量至少5個，保證本算法方程有解;至此得到關于中心距離ro的方程組： ga ? rt^lW。十*!^ 2所述步驟4具體為： 忽略誤差①/〗，則ro的最小二乘解為..g<7) ? gaT ? ka2 ； 所述步驟5具體為： 將所述步驟4求得的&代入目標位置方程，得到關于x，y，z的方程組：利用加權最小二乘法計算出目標位置：(x，y，z)T=(GbT ?邊―1 ? Gb)-1 ? GbT ?邊―1 ? hb 其中，協(xié)方差陣it = C2 ? B ? Q ? B，n表示目標到各接收器的距離， 利用n。+4關系計算； 所述步驟6具體為:對坐標解(x，y，z)進行修正，采用在步驟2建立的方程組基礎上增加 兩個約束方程再進行加權最小二乘，一個方程為中心約束方程： 另一個方程為球面關系約束方程： r〇2= (x-xo)2+(y-yo)2+(z-zo)2 將上式非線性方程線性化，得到線性化后的近似方程：將上述兩式和步驟2的方程組整理可得：利用加權最小二乘法計算出最終位置： (x，y，z)T= (GcT ? V -1 ? Gc)-1 ? GcT ? V -1 ? he
【文檔編號】G01S5/22GK106054134SQ201610340104
【公開日】2016年10月26日
【申請日】2016年5月20日
【發(fā)明人】徐曉蘇, 金博楠, 姚逸卿, 童金武, 吳梅, 閆琳宇
【申請人】東南大學