[发明专利]一种复杂环境下的TDOA定位追踪方法

权利要求书

1.一种复杂环境下的TDOA定位追踪方法，其特征在于，包括如下步骤：

步骤1，读取一条含有N个以上基站的TDOA数据；

步骤2，进行数据预处理，去除误差较大的基站，剩余的基站为有效基站；

步骤3，对于剩余有效基站数大于等于N的数据进行解算，将解算结果放入数组P；

步骤4，更换主基站，重复步骤3，直到该条TDOA数据中的所有基站都当过一次主基站；

步骤5，如果数组P不为空，从中选择最合适的点放入保存最终定位结果的数组T，并清空数组P；

步骤6，如果数组T中加入了新点，进行平均滤波，将滤波结果放入轨迹数组F，用于显示实时轨迹；

步骤7，如果数组T中没有加入新点，根据轨迹数组F中的数据利用时间和速度对当前坐标进行预测，将预测结果同时放入轨迹数组F和数组T中；

步骤8，该条TDOA数据处理完毕，回到步骤1，读取下一条TDOA数据；

步骤1中，读入的TDOA数据data的形式为：

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

其中n表示该条TDOA数据中基站的个数，i表示主基站序号，1＝i＝n，dij表示当前待求坐标到主基站的距离和到序号为j的基站的距离之差，1＝j＝n，当j＝i时，dij＝0；

步骤2中，设定当前读入TDOA数据与历史数据之间的差值△data为：

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

其中△dij为dij和之前时刻较好历史数据之间的差值，设定误差阈值为t，如果△dijt，就从data中将dij删除；

步骤3包括如下步骤：

步骤3-1，将TDOA数据进行变换，使主基站成为序号为1的基站，变换后的数据data_形式为：

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

其中d1j表示当前坐标到主基站和基站j之间距离的差值，2＝j＝n，设定基站的坐标为(x1,y1,z1)，(x2,y2,z2)，…，(xn,yn,zn)，其中(xi,yi,zi)表示基站i的三维坐标，1＝i＝n，该坐标已知；采用引入高度的Chan算法，引入的待定位物体的固定高度为h，则改进后的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＝yi-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表示矩阵转置，Q表示误差的协方差矩阵，sqrt表示开方运算，Z_(i)表示向量Z_的第i个分量，diag{r2,r3,…,rn}表示以r2，r3，…，rn依次为对角线元素的对角矩阵；最终求得的Z为含有三个元素的向量，表示为Z＝(x,y,r)，其中(x,y)即所求的坐标估计值，r为该坐标到主基站的距离；

步骤3-2，将步骤3-1得到的结果作为Talor序列方法的初始值代入进行迭代，得到更精确的坐标：引入固定高度信息，设定高度为h，坐标初始值为(x0,y0)，则改进后的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)是本轮迭代中求出的坐标偏差量，用其判断迭代是否收敛；设阈值参数为t，收敛的判断条件为：

|△x|+|△y|t，

如果本轮迭代没有收敛，则用(x0+△x,y0+△y)取代(x0,y0)，重复上述过程，直到收敛或迭代次数达到预先设置的上限；若收敛，将最终计算出的结果放入数组P；若迭代次数达到上限后Taylor序列方法仍然没有收敛，转步骤3-3；

步骤3-3，如果步骤3-2中的Taylor迭代没有收敛，则用保存最终定位结果的数组T中的最后一个点，即最近一次成功解算并通过筛选的结果作为初始值，代入Taylor序列方法进行迭代，迭代的收敛判断与步骤3-2相同，如果收敛，则将结果放入数组P，否则判定为解算失败；

步骤4中，已知原始数据data的形式为data＝(di1,di2,di3,…,din)，它以基站i作为主基站，若要用基站j取代基站i成为主基站，则更换主基站后的数据data’为：

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

步骤5中，设定数组P中第k个坐标为Pk，当前主基站为j，根据Pk反推其TDOA数据datak，反推公式为：

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

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

其中Pk(i)表示坐标Pk的第i个分量，i取值为1或2，ui表示Pk到基站i之间的距离，uj表示Pk到基站j之间的距离，un表示Pk到基站n之间的距离，在得到反推数据datak后，计算其与原始数据data之间欧氏距离的平方，计算公式为：

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

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

其中wi表示原始数据data与反推数据datak在第i个分量上的差值，data(i)表示data的第i个分量，datak(i)表示datak的第i个分量，按上述公式计算(dist1,dist2,…,distm)，m为数组P中元素的个数，取其中最小值对应的数组P中元素，将其作为本条TDOA数据的最终定位结果，放入数组T。