[發(fā)明專利]一種復(fù)雜環(huán)境下的TDOA定位追蹤方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910026517.0
• 申請(qǐng)日: 2019-01-11
• 公開(kāi)（公告）號(hào): CN109613477B
• 公開(kāi)（公告）日: 2020-08-04
• 發(fā)明（設(shè)計(jì)）人: 申富饒;周至;于斐彥;安俊逸;李俊
• 申請(qǐng)（專利權(quán)）人: 南京大學(xué)
• 主分類號(hào): G01S5/02
• 分類號(hào): G01S5/02
• 代理公司: 江蘇圣典律師事務(wù)所 32237
• 代理人: 胡建華;于瀚文

1.一種復(fù)雜環(huán)境下的TDOA定位追蹤方法，其特征在于，包括如下步驟：

步驟1，讀取一條含有N個(gè)以上基站的TDOA數(shù)據(jù)；

步驟2，進(jìn)行數(shù)據(jù)預(yù)處理，去除誤差較大的基站，剩余的基站為有效基站；

步驟3，對(duì)于剩余有效基站數(shù)大于等于N的數(shù)據(jù)進(jìn)行解算，將解算結(jié)果放入數(shù)組P；

步驟4，更換主基站，重復(fù)步驟3，直到該條TDOA數(shù)據(jù)中的所有基站都當(dāng)過(guò)一次主基站；

步驟5，如果數(shù)組P不為空，從中選擇最合適的點(diǎn)放入保存最終定位結(jié)果的數(shù)組T，并清空數(shù)組P；

步驟6，如果數(shù)組T中加入了新點(diǎn)，進(jìn)行平均濾波，將濾波結(jié)果放入軌跡數(shù)組F，用于顯示實(shí)時(shí)軌跡；

步驟7，如果數(shù)組T中沒(méi)有加入新點(diǎn)，根據(jù)軌跡數(shù)組F中的數(shù)據(jù)利用時(shí)間和速度對(duì)當(dāng)前坐標(biāo)進(jìn)行預(yù)測(cè)，將預(yù)測(cè)結(jié)果同時(shí)放入軌跡數(shù)組F和數(shù)組T中；

步驟8，該條TDOA數(shù)據(jù)處理完畢，回到步驟1，讀取下一條TDOA數(shù)據(jù)；

步驟1中，讀入的TDOA數(shù)據(jù)data的形式為：

data＝(di1,di2,di3,…,din)，

其中n表示該條TDOA數(shù)據(jù)中基站的個(gè)數(shù)，i表示主基站序號(hào)，1＝i＝n，dij表示當(dāng)前待求坐標(biāo)到主基站的距離和到序號(hào)為j的基站的距離之差，1＝j(luò)＝n，當(dāng)j＝i時(shí)，dij＝0；

步驟2中，設(shè)定當(dāng)前讀入TDOA數(shù)據(jù)與歷史數(shù)據(jù)之間的差值△data為：

△data＝(△di1,△di2,△di3,…,△din)，

其中△dij為dij和之前時(shí)刻較好歷史數(shù)據(jù)之間的差值，設(shè)定誤差閾值為t，如果△dijt，就從data中將dij刪除；

步驟3包括如下步驟：

步驟3-1，將TDOA數(shù)據(jù)進(jìn)行變換，使主基站成為序號(hào)為1的基站，變換后的數(shù)據(jù)data_形式為：

data_＝(d12,d13,d14,…,d1n)，

其中d1j表示當(dāng)前坐標(biāo)到主基站和基站j之間距離的差值，2＝j(luò)＝n，設(shè)定基站的坐標(biāo)為(x1,y1,z1)，(x2,y2,z2)，…，(xn,yn,zn)，其中(xi,yi,zi)表示基站i的三維坐標(biāo)，1＝i＝n，該坐標(biāo)已知；采用引入高度的Chan算法，引入的待定位物體的固定高度為h，則改進(jìn)后的Chan算法表示為：

Ki＝xi^2+yi^2+(zi-h)^2，

E＝0.5*[d12^2+K1-K2；d13^2+K1-K3；…；d1n^2+K1-Kn]，

xij＝xi-xj，

yij＝y(tǒng)i-yj，

G＝[-x21,-y21,d12；-x31,-y31,d13；…；-xn1,-yn1,d1n]，

Z_＝inv(G^T*inv(Q)*G)*G^T*inv(Q)*E，

ri＝sqrt((xi-Z_(1))^2+(yi-Z_(2))^2+(zi-h)^2)，

D＝diag{r2,r3,…,rn}，

X＝D*Q*D，

Z＝inv(G^T*inv(X)*G)*G^T*inv(X)*E，

其中Ki、E、xij、yij、G、Z_、ri、D、X表示引入的中間量；^2表示平方操作，inv表示矩陣的求逆操作，矩陣右上角的T表示矩陣轉(zhuǎn)置，Q表示誤差的協(xié)方差矩陣，sqrt表示開(kāi)方運(yùn)算，Z_(i)表示向量Z_的第i個(gè)分量，diag{r2,r3,…,rn}表示以r2，r3，…，rn依次為對(duì)角線元素的對(duì)角矩陣；最終求得的Z為含有三個(gè)元素的向量，表示為Z＝(x,y,r)，其中(x,y)即所求的坐標(biāo)估計(jì)值，r為該坐標(biāo)到主基站的距離；

步驟3-2，將步驟3-1得到的結(jié)果作為Talor序列方法的初始值代入進(jìn)行迭代，得到更精確的坐標(biāo)：引入固定高度信息，設(shè)定高度為h，坐標(biāo)初始值為(x0,y0)，則改進(jìn)后的Taylor序列方法表示為：

Ri＝sqrt((xi-x0)^2+(yi-y0)^2+(zi-h)^2)，

Rij＝Ri-Rj，

H＝[R21-(R2-R1)；R31-(R3-R1)；…；Rn1-(Rn-R1)]，

pi＝(xi-x0)/Ri，

qi＝(yi-y0)/Ri，

M＝[p1-p2,q1-q2；p1-p3,q1-q3；…；p1-pn,q1-qn]，

[△x,△y]＝inv(M^T*inv(Q)*M)*M^T*inv(Q)*H，

其中，Ri、Rij、H、pi、qi、M表示引入的中間量；(△x,△y)是本輪迭代中求出的坐標(biāo)偏差量，用其判斷迭代是否收斂；設(shè)閾值參數(shù)為t，收斂的判斷條件為：

|△x|+|△y|t，

如果本輪迭代沒(méi)有收斂，則用(x0+△x,y0+△y)取代(x0,y0)，重復(fù)上述過(guò)程，直到收斂或迭代次數(shù)達(dá)到預(yù)先設(shè)置的上限；若收斂，將最終計(jì)算出的結(jié)果放入數(shù)組P；若迭代次數(shù)達(dá)到上限后Taylor序列方法仍然沒(méi)有收斂，轉(zhuǎn)步驟3-3；

步驟3-3，如果步驟3-2中的Taylor迭代沒(méi)有收斂，則用保存最終定位結(jié)果的數(shù)組T中的最后一個(gè)點(diǎn)，即最近一次成功解算并通過(guò)篩選的結(jié)果作為初始值，代入Taylor序列方法進(jìn)行迭代，迭代的收斂判斷與步驟3-2相同，如果收斂，則將結(jié)果放入數(shù)組P，否則判定為解算失敗；

步驟4中，已知原始數(shù)據(jù)data的形式為data＝(di1,di2,di3,…,din)，它以基站i作為主基站，若要用基站j取代基站i成為主基站，則更換主基站后的數(shù)據(jù)data’為：

data’＝(di1-dij,di2-dij,…,din-dij)；

步驟5中，設(shè)定數(shù)組P中第k個(gè)坐標(biāo)為Pk，當(dāng)前主基站為j，根據(jù)Pk反推其TDOA數(shù)據(jù)datak，反推公式為：

ui＝sqrt((xi-Pk(1))^2+(yi-Pk(2))^2+(zi-h)^2)，

datak＝(uj-u1,uj-u2,…,uj-un)，

其中Pk(i)表示坐標(biāo)Pk的第i個(gè)分量，i取值為1或2，ui表示Pk到基站i之間的距離，uj表示Pk到基站j之間的距離，un表示Pk到基站n之間的距離，在得到反推數(shù)據(jù)datak后，計(jì)算其與原始數(shù)據(jù)data之間歐氏距離的平方，計(jì)算公式為：

wi＝data(i)-datak(i)，

distk＝w1^2+w2^2+…+wn^2，

其中wi表示原始數(shù)據(jù)data與反推數(shù)據(jù)datak在第i個(gè)分量上的差值，data(i)表示data的第i個(gè)分量，datak(i)表示datak的第i個(gè)分量，按上述公式計(jì)算(dist1,dist2,…,distm)，m為數(shù)組P中元素的個(gè)數(shù)，取其中最小值對(duì)應(yīng)的數(shù)組P中元素，將其作為本條TDOA數(shù)據(jù)的最終定位結(jié)果，放入數(shù)組T。