CN112066981B - 一种复杂环境下的三维定位追踪方法 - Google Patents

一种复杂环境下的三维定位追踪方法 Download PDF

Info

Publication number
CN112066981B
CN112066981B CN202010927152.1A CN202010927152A CN112066981B CN 112066981 B CN112066981 B CN 112066981B CN 202010927152 A CN202010927152 A CN 202010927152A CN 112066981 B CN112066981 B CN 112066981B
Authority
CN
China
Prior art keywords
base station
data
result
tdoa
axis
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010927152.1A
Other languages
English (en)
Other versions
CN112066981A (zh
Inventor
申富饶
赖碧兰
安俊逸
赵健
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University
Original Assignee
Nanjing University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University filed Critical Nanjing University
Priority to CN202010927152.1A priority Critical patent/CN112066981B/zh
Publication of CN112066981A publication Critical patent/CN112066981A/zh
Application granted granted Critical
Publication of CN112066981B publication Critical patent/CN112066981B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Mobile Radio Communication Systems (AREA)

Abstract

本发明提供了一种复杂环境下的三维定位追踪方法，包括：步骤1，读取TDOA数据和TOF数据；步骤2，解算三维结果步骤3，反推主基站的TOF数据和从基站的TDOA数据；步骤4，对MS数组进行二维Taylor迭代，将结果存入数组X；步骤5，通过最小二乘法解算，如果无实数解则将z轴结果置0，记为zz，否则解算出两个z轴实数解z(1)、z(2)；步骤6，将z(1)、z(2)分别通过最小二乘法反推出每个基站的TDOA数据，选出解算出的TDOA与实际TDOA数据误差最小的z，并将这个z记为zz；步骤7，进行三维Taylor解算，将结果放入数组Fn；步骤8，对Fn数组进行平均滤波；步骤9，读取下一条数据。

Description

一种复杂环境下的三维定位追踪方法
技术领域
本发明涉及室内定位技巧，特别是涉及一种复杂环境下的三维定位追踪方法。
背景技术
随着物联网和移动互联技术的不断发展，越来越多的服务和应用依赖于用户的位置信息，如何精确地对设备进行定位正成为当前研究的一大热点。在室外环境下，全球定位系统(GPS)以其精度较高、稳定性强和成本低廉的优势，取得了广泛的应用，但在室内环境下，由于卫星信号无法穿透建筑物，GPS的使用受到了严重的限制。而且，相比于室外，室内环境受多径效应、非视距传输(NLOS)的影响更为明显，还存在着环境的动态变化等问题，这些都增加了室内精确定位的挑战性。目前，国内外研究者们已经提出了基于信号衰减模型、TOA(Time-of-Arrival)、TDOA(Time-Difference-of-Arrival)和AOA(Angle-of-Arrival)等原理的室内定位解决方案，其中，TDOA以其不需要严格时间同步的优势，得到了广泛的重视和研究。
在众多TDOA定位方法中，Wade H.Foy依据Taylor展开提出的方法(简称Taylor序列方法)以其形式简单、精度较高的优点取得了广泛的应用。该方法在给定初始坐标附近进行Taylor展开并忽略二阶以上分量，然后通过迭代计算误差的局部最小二乘解来逐步优化坐标。该方法的精度依赖于输入的初始值，如果初始值接近真实解则结果较为精确，但在实际应用中，初始值很难选取，导致其精度较差，甚至可能无法收敛。而且迭代的特性使其需要较大的计算量。文献：Foy W H.Position-Location Solutions by Taylor-SeriesEstimation[J].IEEE Transactions on Aerospace&Electronic Systems，2007，AES-12(2):187-194.
为了克服基于Taylor展开的方法存在的依赖初始值进行迭代、无法保证收敛以及时间开销大的问题，Y.T.Chan等人提出了具有闭式解的双曲定位算法(简称Chan算法)。该算法通过引入第三个的变量，将非线性问题转化为线性问题，然后通过两次加权最小二乘法得到最终的定位结果。Chan算法计算速度快，结果也较为准确，但该算法的推导基于信道误差较小且服从零均值高斯分布的假设，该假设在真实环境下很难满足，因此精度会显著下降。文献：Chan Y T，Ho K C.A simple and efficient estimator for hyperboliclocation[J].IEEE Transactions on Signal Processing，2002，42(8):1905-1915.
发明内容
发明目的：解决Taylor序列方法、TOF定位等经典TDOA定位算法在真实环境下性能显著下降、追踪过程中无法形成连续平滑轨迹的问题，在TOF和TDOA算法中采用联合解算，提升定位结果的准确性，并加入数据扩充技术和坐标的先验知识，大幅减少无法解算的情况，同时通过合理的滤波和预测策略，增强轨迹的光滑性和连续性。
为了解决上述技术问题，本发明公开了一种复杂室内环境下基于TDOA和TOF的定位追踪方法，该方法可以用于仓库管理、定位导览、机器人追踪等应用中，包括如下步骤：
步骤1，读取一条含有n个及以上基站的到达时间差TDOA(Time Difference ofArrival，简称TDOA)数据和主基站的到达时间TOF(Time of Flight，简称TOF)数据；
步骤2，将TDOA数据和TOF数据利用极大似然法联立方程解算出2个三维解算结果，并将2个三维解算结果的x、y轴结果放入数组A；
步骤3，由数组A中的2个x、y轴解算结果分别反推出主基站的TOF数据和从基站的TDOA数据，并分别求该反推结果与实际主基站TOF和从基站TDOA数据差值的平方和，选出平方和结果最小1个的x、y轴结果，将结果存入MS数组；
步骤4，对MS数组进行二维Taylor迭代，得到优化的x、y轴结果，将结果存入数组X；
步骤5，根据数组X中的x、y轴结果，通过最小二乘法解算，如果无实数解，则认为z轴结果为0，如果有实数解，则解出两个z轴结果，分别记为z(1)、z(2)；
步骤6，将z(1)、z(2)分别通过最小二乘法反推出主基站的z轴结果，并分别求反推结果与实际主基站z轴结果差值的平方，选出平方结果最小的z，并将这个平方结果最小的z记为zz；
步骤7，将数组X和zz联合进行三维Taylor解算，将解算出的三维结果放入最终定位结果的数组Fn；
步骤8，对数组Fn进行平均滤波，将滤波结果放入轨迹数组fr，用于显示实时轨迹；
步骤9，该条数据处理完毕，回到步骤1，读取下一条数据。
步骤1中，读入的TDOA数据data的形式为：
data＝(x1，y1，z1，d1；
x2，y2，z2，d12；
...
xn，yn，zn，d1n)，
其中n表示该条TDOA数据中基站的个数，x1、y1、z1分别表示主基站的x、y、z轴坐标；1表示主基站，d1表示当前主基站的TOF数据，d1i表示当前待求坐标到主基站的距离和到序号为i的基站(简称基站i，2<＝i<＝n)的距离之差，即d1i＝d1-di。xi、yi、zi分别表示基站i的x、y、z轴坐标；该步骤要求基站数大于等于4，即n>＝4。
步骤2包括：
用引入三维的极大似然算法对坐标进行粗略估计。将TDOA数据和TOF数据做变换，变换后的TOF数据为tof_data，变换后的TDOA数据为tdoa_data：
tof_data＝d1，
tdoa_data＝(d12，d13，d14，...，d1n)，
其中d1j表示当前坐标到主基站和基站j之间距离的差值，2<＝j<＝n；
设定基站的坐标为(x1，y1，z1)，(x2，y2，z2)，…，(xn，yn，zn)，其中(xi，yi，zi)表示基站i的三维坐标，该坐标已知，则改进的极大似然算法表示为：
目标位置与基站的距离关系为：(d1+di1)^2＝(x-xi)^2+(y-yi)^2+(z-zi)^2，
设定：ki＝xi^2+yi^2+zi^2；s＝x^2+y^2+z^2，将上述等式转化为：
d1^2-k1＝-2(x1*x+y1*y+z1*z)+s，
di1^2+2di1*d1＝-2(xi1*x+yi1*y+zi1*z)+ki-k1，
设定Za＝[x，y，z，d1]'，则上式转化为：
φ＝h-G*Za，G＝-2*[x1，y1，z1，-s/(2d1)；x21，y21，z21，d12；...；xn1，yn1，zn1，d1n]，h＝[d1^2；d12^2+K1-K2；d13^2+K1-K3；...；d1n^2+K1-Kn]；
设定[d1，d21，...，dn1]'＝[r1，r21，...，rn1]'+e，得到：
误差φ＝2B*e+e^2≈2B*e，B＝diag(d1，d21，...，dn1)；
由于噪声未知，设定噪声为高斯噪声，得到高斯协方差矩阵为：
Q＝E[ee']＝diag[σ^1，σ^2，…，σ^n)]，
Q＝(0.5*ones(size(h，1))+0.5*eye(size(h，1)))*(delta^2)，
得到Φ＝E[φφ']＝4BQB'；
则得到损失函数为：(h-G*Za)'pinv(Φ)(h-G*Za)，
将损失函数最小化，得出的Za值才为所求结果，根据极大似然法得到：
Za＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*h；
因此得到Za的协方差矩阵：
cov(Za)＝E[ΔzΔz']＝pinv(G'*pinv(Φ)*G)，
Δz＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*B*e；
由于Za向量仅是初次使用最大似然估计的结果，并不能保证很好的精确度，再次进行最大似然估计，保证精度；将上述所得结果Za设为：
Za＝[t1，t2，t3，t4]'，t1＝x+e1，t2＝y+e2，t3＝z+e3，t4＝d1+e4，
使用POS1保存Za向量的前三维，即Za＝[t1，t2，t3]'，对t1，t2，t3，t4求平方得到新的损失函数为：
Figure BDA0002668801760000041
h1＝[(t1-x1)^2，(t2-y1)^2，(t3-z1)^2，t4^2]'，
G1＝[1 0 0；0 1 0；0 0 1；1 1 1]，
Za1＝[(x-x1)^2，(y-y1)^2，(z-z1)^2]'，
Figure BDA0002668801760000044
同理，得到:
Figure BDA0002668801760000042
B1＝diag(x-x1，y-y1，z-z1，d1)，
Figure BDA0002668801760000043
再次根据最大似然估计得到：
Za1＝argmin(h1-G1*Za1)'*pinv(Φ1)*(h1-G1*Za1)，
Za1＝pinv(G1'pinv(Φ1)*G1)*G1'pinv(Φ1)*h1，
最终得到POS2＝[x，y，z)]'＝[x1，y1，z1)]'±sqrt(Za1)；
其中x、y、z分别表示目标位置三维坐标；ki表示基站i的三维坐标平方和，1<＝i<＝n；s表示目标位置的三维坐标平方和，在第一次极大似然法求解中，Za表示目标位置坐标和TOF数据凑成的向量，φ表示结果存在的实际误差，h、G都为等式转化之后的矩阵，r1，r21，...，rn1分别对应表示d1，d21，...，dn1的实际精确值，e表示实际误差；B表示由d1，d21，...，dn1组成的对角矩阵，Q表示误差的协方差矩阵，Φ表示φ实际误差的协方差矩阵，cov(Za)表示Za的协方差矩阵，Za结果虽然是经过一次极大似然法得到的结果，仍然存在误差，Δz表示Za与实际目标位置之间、实际目标位置和主基站距离的值的误差，POS1用于表示第一次极大似然法解算之后得到的三维目标位置坐标；在第二次极大似然法求解中，POS2表示第二次求解的最终目标位置的坐标，Za1表示为POS2与POS1误差的平方，t1、t2、t3、t4为Za向量中的四个分量，e1、e2、e3、e4分别表示t1、t2、t3、t4与实际的x、y、z、d1的误差，φ1表示Za与实际结果的大概误差，h1、G1都为等式转化之后凑成的矩阵，B1表示以x-x1，y-y1，z-z1，d1依次为对角线元素的对角矩阵，Φ1表示误差φ1的协方差矩阵；pinv表示矩阵的求逆操作，矩阵右上角的'表示矩阵转置，sqrt表示开方运算，Za(i)表示向量Za的第i个分量，i＝1，2，3；diag{r2，r3，…，rn}表示以r2，r3，…，rn依次为对角线元素的对角矩阵；
最终求得的POS1和POS2为含有三个元素的向量，表示为POS1＝(t1，t2，t3)，POS2＝(x，y，z)，其中(t1，t2，t3)和(x，y，z)都为所求的坐标估计值。
步骤3中，将步骤3得到的两个三维解算结果POS1，POS2作为备选结果；分别选取这两个结果中的前两维，即x、y轴结果放入数组A中，即A保存了POS1，POS2的x、y轴值，用A中保存的两个x、y轴结果反推出误差最小的一组二维解算结果，反推公式为：
MSi＝[A(2*i+1)A(2*i+2)]；
R＝(d1，d12，d13，d14，...，d1n)；
tdoa_maini＝sqrt((MSi(1)-x1)^2+(MS1(2)-y1)^2)；
toa_tempi＝sqrt((MSi(1)-x1)^2+(MSi(2)-y1)^2)-R(1)；
Addi＝n*toa_tempi^2；
tdoa_tempi＝sqrt((MSi(1)-xj)^2+(MSi(2)-yj)^2)-tdoa_main1-R(j)；
Addi＝Addi+n*tdoa_tempi^2；
其中2<＝j<＝n，MSi为第i(i＝1，2)组备选的x、y轴结果，R表示主基站的TOF数据和从基站的TDOA数据集合。tdoa_maini表示第i组备选的二维结果反推出来的主基站的TOF结果。toa_tempi表示第i组备选的二维结果反推出来的主基站TOF与实际TOF数据的差值；而tdoa_tempi表示第i组备选的二维结果反推出来的从基站的TDOA数据与实际TDOA数据的差值；将toa_tempi的平方和与tdoa_tempi的平方和求和记为Addi，取Addi值最小的一组结果作为x、y轴的初步解算结果，并将结果放入数组MS。
步骤4中，将得到的MS(x，y)二维结果，作为初始位置(x0，y0)，即x0＝x，y0＝y，引入固定高度h，根据TDOA数据的性质得到等式为：
F(x，y)＝sqrt((x-xi)^2+(y-yi)^2+h^2)-sqrt((x-xj^2+(y-yj)^2+h^2)＝dij，
则改进后的Taylor序列方法表示为：
F(x0，y0)+pi*△x+qi*△y≈di+ei＝F(x，y)，
将上述等式转化为：
Gδ＝E+e，e＝[e1；e2；...；en]，δ＝[△x；△y]
ri＝sqrt((xi-x0)^2+(yi-y0)^2+h^2)，
rij＝ri-rj，
E＝[r21-(r2-r1)；r31-(r3-r1)；…；rn1-(rn-r1)]，
pi＝(xi-x0)/ri，
qi＝(yi-y0)/ri，
G＝[p1-p2，q1-q2；p1-p3，q1-q3；…；p1-pn，q1-qn]，
[△x，△y]＝inv(GT*inv(Q)*G)*GT*inv(Q)*E，
其中F(x，y)表示根据TDOA数据得出的函数，ri表示基站i到初始位置的距离，1<＝i<＝n，rij表示基站i到初始位置的距离与基站j到初始位置的距离的差值，E表示由ri1-(ri-r1)为分量组成的向量，2<＝i<＝n，pi表示F(x0，y0)对于x0的一阶偏导，qi表示F(x0，y0)对于y0的一阶偏导，根据等式F(x，y)变换，G表示由F(x，y)的一阶偏导组成的矩阵，δ表示解算的目标位置与实际目标位置的误差向量；Q与步骤1中相同，为误差的协方差矩阵；(△x，△y)是本轮迭代中求出的坐标偏差量，用其判断迭代是否收敛。设置阈值参数为t＝100，收敛的判断条件为：
|△x|+|△y|<t，
如果本轮迭代没有收敛，则将该点设置丢点标记，判定解算失败；如果收敛，将最终计算出的结果放入数组X；如果迭代次数达到上限后Taylor序列方法仍然没有收敛，则将该点设置丢点标记，判定解算失败，否则将结果放入X。
步骤5中，已知目前所得二维结果为X；通过X根据最小二乘法反推出z轴的结果，反推公式为：
D＝d1^2-((X(1)-x1)^2+(X(2)-y1)^2)，
z(1)＝z1+sqrt(D)，
z(2)＝z1-sqrt(D)，
其中D为目标位置z轴坐标与主基站的平方差，当中间参数D为负数时，将结果0直接存入zz中，即zz＝0，作为目标位置的z轴结果；当D大于等于0时，将最小二乘法所得的结果分别存入数组z中，即分别存入z(1)、z(2)中，并跳转至步骤6中。
步骤6中，根据z的结果，反推出各个基站的TDOA数据，反推公式为：
d(j)＝sqrt((X(1)-xj)^2+(X(2)-yj)^2+(z(i)-zj)^2)，
dis(j)＝d(j)-d1，
diff(i)＝0，diff(i)＝diff(i)+dis(j)-dj1
其中i表示数组z中第i个值，j表示第j个基站的坐标，d1为实际的TOF数据，d表示目标位置的x、y轴结果X与z轴可能的结果z(i)，i＝1，2，联合成三维坐标，计算这个坐标位置与基站j的距离，1<＝j<＝n；通过解算出来的目标位置与主基站的TOF数据求差值，反推得到基站j的TDOA数据，记为dis；diff(i)表示在使用z(i)作为目标位置的z轴结果时，反推出来所有基站的TDOA数据与其测量的实际TDOA数据差值的和，选择使diff(i)最小的z(i)作为目标位置的z轴结果，存入zz中。
步骤7中，将得到的目标定位解算结果X与zz作为初始值为(x0，y0，z0)，即x0＝X(1)，y0＝X(2)，z0＝zz，根据TDOA数据的性质得到公式：
F(x，y，z)＝sqrt((x-xi)^2+(y-yi)^2+(z-zi)^2)-sqrt((x-xj^2+(y-yj)^2+(z-zj)^2)＝dij，
则改进后的Taylor序列方法表示为：
F(x0，y0，z0)+pi*△x+qi*△y+si*△z≈di+ei＝F(x，y，z)，
将上述等式转化为：
Gδ＝E+e，e＝[e1；e2；...；en]，δ＝[△x；△y；△z]：
ri＝sqrt((xi-x0)^2+(yi-y0)^2+(zi-z0)^2)，
rij＝ri-rj，
E＝[r21-(r2-r1)；r31-(r3-r1)；…；rn1-(rn-r1)]，
pi＝(xi-x0)/ri，
qi＝(yi-y0)/ri，
si＝(zi-z0)/ri，
G＝[p1-p2，q1-q2，s1-s2；p1-p3，q1-q3，s1-s3；…；p1-pn，q1-qn，s1-sn]，
[△x，△y，△z]＝inv(GT*inv(Q)*G)*GT*inv(Q)*E，
其中F(x，y，z)表示根据TDOA数据得出的函数，ri表示基站i到初始位置的距离，1<＝i<＝n，rij表示基站i到初始位置的距离与基站j到初始位置的距离的差值，E表示由ri1-(ri-r1)为分量组成的向量，2<＝i<＝n，pi表示F(x0，y0)对于x0的一阶偏导；qi表示F(x0，y0)对于y0的一阶偏导；si表示F(x0，y0)对于z0的一阶偏导，根据等式F(x，y，z)变换，G表示由F(x，y，z)的一阶偏导组成的矩阵，δ表示解算的目标位置与实际目标位置的误差向量；Q与步骤1中相同，为误差的协方差矩阵；(△x，△y，△z)是本轮迭代中求出的坐标偏差量，用其判断迭代是否收敛。设置阈值参数为t＝100，收敛的判断条件为：
|△x|+|△y|+|△z|<t，
如果本轮迭代没有收敛，则将该点设置丢点标记，判定解算失败；如果收敛，将最终计算出的结果放入数组X；如果迭代次数达到上限后Taylor序列方法仍然没有收敛，则将该点设置丢点标记，判定解算失败，否则将结果放入数组Fn。
步骤8中，设定平均滤波的窗口大小为span，设定数组Fn中元素个数为k，则滤波公式为：
f＝(Fn(1)+Fn(2)+…+Fn(k))/k，其中k<span，
f＝(Fn(k)+Fn(k-1)+…+Fn(k-span+1))/span，其中k>＝span，
f即为滤波平滑处理后得到坐标，将其加入数组fr，Fn为根据步骤7的三维泰勒迭代之后得到的目标位置解算结果数组，span为滤波窗口大小，k为Fn数组中存储的解算结果的个数。
步骤9中，该条TDOA数据已经处理完毕，最终定位结果为Fn中最后一个坐标，滤波结果为f中最后一个坐标，f用于显示实时轨迹。此时需要读取下一条TDOA数据和TOF数据，处理下一时刻的信息。
有益效果：本发明的显著优点是在复杂的真实环境下，比如受到严重的反射和遮挡干扰时，能够大幅减少TDOA和TOF定位中因数据质量差而出现的无法解算的情况，并达到较高的精度，同时在目标追踪时，保证轨迹的连续性和光滑性，显著提升定位系统的性能。
附图说明
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
图1为本发明的整体流程图。
图2为本发明中TOF-TDOA解算的流程图。
图3a是TDOA数据序列通过本发明方法计算出的轨迹。
图3b是TOF数据序列通过本发明方法计算出的轨迹。
图3c是用Taylor序列方法计算出的散点图。
具体实施方式
本发明公开了一种复杂室内环境下基于TDOA和TOF的定位追踪方法，图1是本发明的整体流程图，图2为本发明中TOF-TDOA解算的流程图。本发明具体包含如下步骤：
步骤1，读取一条含有n个及以上基站的到达时间差TDOA(Time Difference ofArrival，简称TDOA)数据和主基站的到达时间TOF(Time of Flight，简称TOF)数据；
步骤2，将TDOA数据和TOF数据利用极大似然法联立方程解算出2个三维解算结果，并将2个三维解算结果的x、y轴结果放入数组A；
步骤3，由数组A中的2个x、y轴解算结果分别反推出主基站的TOF数据和从基站的TDOA数据，并分别求该反推结果与实际主基站TOF和从基站TDOA数据差值的平方和，选出平方和结果最小1个的x、y轴结果，将结果存入MS数组；
步骤4，对MS数组进行二维Taylor迭代，得到优化的x、y轴结果，将结果存入数组X；
步骤5，根据数组X中的x、y轴结果，通过最小二乘法解算，如果无实数解，则认为z轴结果为0，如果有实数解，则解出两个z轴结果，分别记为z(1)、z(2)；
步骤6，将z(1)、z(2)分别通过最小二乘法反推出主基站的z轴结果，并分别求反推结果与实际主基站z轴结果差值的平方，选出平方结果最小的z，并将这个平方结果最小的z记为zz；
步骤7，将数组X和zz联合进行三维Taylor解算，将解算出的三维结果放入最终定位结果的数组Fn；
步骤8，对数组Fn进行平均滤波，将滤波结果放入轨迹数组fr，用于显示实时轨迹；
步骤9，该条数据处理完毕，回到步骤1，读取下一条数据。
步骤1中，读入的TDOA数据data的形式为：
data＝(x1，y1，z1，d1；
x2，y2，z2，d12；
...
xn，yn，zn，d1n)，
其中n表示该条TDOA数据中基站的个数，x1、y1、z1分别表示主基站的x、y、z轴坐标；1表示主基站，d1表示当前主基站的TOF数据，d1i表示当前待求坐标到主基站的距离和到序号为i的基站(简称基站i，2<＝i<＝n)的距离之差，即d1i＝d1-di。xi、yi、zi分别表示基站i的x、y、z轴坐标；该步骤要求基站数大于等于4，即n>＝4。
步骤2包括：
用引入三维的极大似然算法对坐标进行粗略估计。将TDOA数据和TOF数据做变换，变换后的TOF数据为tof_data，变换后的TDOA数据为tdoa_data：
tof_data＝d1，
tdoa_data＝(d12，d13，d14，...，d1n)，
其中d1j表示当前坐标到主基站和基站j之间距离的差值，2<＝j<＝n；
设定基站的坐标为(x1，y1，z1)，(x2，y2，z2)，…，(xn，yn，zn)，其中(xi，yi，zi)表示基站i的三维坐标，该坐标已知，则改进的极大似然算法表示为：
目标位置与基站的距离关系为：(d1+di1)^2＝(x-xi)^2+(y-yi)^2+(z-zi)^2，
设定：ki＝xi^2+yi^2+zi^2；s＝x^2+y^2+z^2，将上述等式转化为：
d1^2-k1＝-2(x1*x+y1*y+z1*z)+s，
di1^2+2di1*d1＝-2(xi1*x+yi1*y+zi1*z)+ki-k1，
设定Za＝[x，y，z，d1]'，那么上式可以转化为：
φ＝h-G*Za，G＝-2*[x1，y1，z1，-s/(2d1)；x21，y21，z21，d12；...；xn1，yn1，zn1，d1n]，h＝[d1^2；d12^2+K1-K2；d13^2+K1-K3；...；d1n^2+K1-Kn]；
设定[d1，d21，...，dn1]'＝[r1，r21，...，rn1]'+e，可以得到：
误差φ＝2B*e+e^2≈2B*e，B＝diag(d1，d21，...，dn1)；
由于噪声未知，假设噪声为高斯噪声，得到高斯协方差矩阵为：
Q＝E[ee']＝diag[σ^1，σ^2，…，σ^n)]，
Q＝(0.5*ones(size(h，1))+0.5*eye(size(h，1)))*(delta^2)，
可得Φ＝E[φφ']＝4BQB'；
那么可以得到损失函数为：(h-G*Za)'pinv(Φ)(h-G*Za)，
将损失函数最小化，得出的Za值才为所求结果，根据极大似然法可以得到：
Za＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*h；
因此得到Za的协方差矩阵：
cov(Za)＝E[ΔzΔz']＝pinv(G'*pinv(Φ)*G)，
Δz＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*B*e；
由于Za向量仅是初次使用最大似然估计的结果，并不能保证很好的精确度，再次进行最大似然估计，保证精度；将上述所得结果Za设为:
Za＝[t1，t2，t3，t4]'，t1＝x+e1，t2＝y+e2，t3＝z+e3，t4＝d1+e4，
使用POS1保存Za向量的前三维，即Za＝[t1，t2，t3]'，对t1，t2，t3，t4求平方得到新的损失函数为：
φ1＝h1-G1*Za1，h1＝[(t1-x1)^2，(t2-y1)^2，(t3-z1)^2，t4^2]'，
G1＝[1 0 0；0 1 0；0 0 1；1 1 1]，
Za1＝[(x-x1)^2，(y-y1)^2，(z-z1)^2]'，
φ1＝[2(x-x1)*e1+e1^2，2(y-y1)*e2+e2^2，2(z-z1)*e3+e3^2，2d1*e4+e4^2]'≈[2(x-x1)*e1，2(y-y1)*e2，2(z-z1)*e3，2d1*e4]'，
同理，可得到:φ1＝2B1*ε+ε^2≈2B1*ε，B1＝diag(x-x1，y-y1，z-z1，d1)，
Φ1＝E[φ1φ1']＝4B1*cov(t)*B1，
再次根据最大似然估计得到:
Za1＝argmin(h1-G1*Za1)'*pinv(Φ1)*(h1-G1*Za1)＝pinv(G1'pinv(Φ1)*G1)*G1'pinv(Φ1)*h1，
最终得到POS2＝[x，y，z)]'＝[x1，y1，z1)]'±sqrt(Za1)；
其中x、y、z分别表示目标位置坐标；ki表示基站i的三维坐标平方和，1<＝i<＝n；s表示目标位置的三维坐标平方和，在第一次极大似然法求解中，Za表示目标位置坐标和TOF数据凑成的向量，φ表示结果存在的实际误差，h、G都为等式转化之后的矩阵，r1，r21，...，rn1表示d1，d21，...，dn1的实际精确值，e表示实际误差；B表示由d1，d21，...，dn1组成的对角矩阵，Q表示误差的协方差矩阵，Φ表示φ实际误差的协方差矩阵，cov(Za)表示Za的协方差矩阵，Za结果虽然是经过一次极大似然法得到的结果，仍然存在误差，Δz表示Za与实际目标位置之间、实际目标位置和主基站距离的值的误差，POS1用于表示第一次极大似然法解算之后得到的三维目标位置坐标；在第二次极大似然法求解中，POS2表示第二次求解的最终目标位置的坐标，Za1表示为POS2与POS1误差的平方，t1、t2、t3、t4为Za向量中的四个分量，e1、e2、e3、e4表示t1、t2、t3、t4与实际的x、y、z、d1的误差，φ1表示Za与实际结果的大概误差，h1、G1都为等式转化之后凑成的矩阵，B1表示以x-x1，y-y1，z-z1，d1依次为对角线元素的对角矩阵，Φ1表示误差φ1的协方差矩阵；pinv表示矩阵的求逆操作，矩阵右上角的'表示矩阵转置，sqrt表示开方运算，Za(i)表示向量Za的第i个分量，i＝1，2，3；diag{r2，r3，…，rn}表示以r2，r3，…，rn依次为对角线元素的对角矩阵；
最终求得的POS1和POS2为含有三个元素的向量，表示为POS1＝(t1，t2，t3)，POS2＝(x，y，z)，其中(t1，t2，t3)和(x，y，z)都为所求的坐标估计值。
步骤3中，将步骤3得到的结果两个个三维解算结果POS1，POS2作为备选结果；分别选取这两个个结果中的前两维，即x、y轴结果放入数组A中，即A保存了POS1，POS2的x、y轴值，用A中保存的两个x、y轴结果反推出误差最小的一组二维解算结果，反推公式为：
MSi＝[A(2*i+1)A(2*i+2)]；
R＝(d1，d12，d13，d14，...，d1n)；
tdoa_maini＝sqrt((MSi(1)-x1)^2+(MS1(2)-y1)^2)；
toa_tempi＝sqrt((MSi(1)-x1)^2+(MSi(2)-y1)^2)-R(1)；
Addi＝n*toa_tempi^2；
tdoa_tempi＝sqrt((MSi(1)-xj)^2+(MSi(2)-yj)^2)-tdoa_main1-R(j)；
Addi＝Addi+n*tdoa_tempi^2；
其中2<＝j<＝n，MSi为第i(i＝1，2)组备选的x、y轴结果，R表示主基站的TOF数据和从基站的TDOA数据集合。tdoa_maini表示第i组备选的二维结果反推出来的主基站的TOF结果。toa_tempi表示第i组备选的二维结果反推出来的主基站TOF与实际TOF数据的差值；而tdoa_tempi表示第i组备选的二维结果反推出来的从基站的TDOA数据与实际TDOA数据的差值；将toa_tempi的平方和与tdoa_tempi的平方和求和记为Addi，取Addi值最小的一组结果作为x、y轴的初步解算结果，并将结果放入数组MS。
步骤4中，将得到的MS(x，y)二维结果，作为初始位置(x0，y0)，即x0＝x，y0＝y，引入固定高度h，根据TDOA数据的性质可以得到等式为：
F(x，y)＝sqrt((x-xi)^2+(y-yi)^2+h^2)-sqrt((x-xj^2+(y-yj)^2+h^2)＝dij，
则改进后的Taylor序列方法表示为：
F(x0，y0)+pi*△x+qi*△y≈di+ei＝F(x，y)，
将上述等式转化为：
Gδ＝E+e，e＝[e1；e2；...；en]，δ＝[△x；△y]
ri＝sqrt((xi-x0)^2+(yi-y0)^2+h^2)，
rij＝ri-rj，
E＝[r21-(r2-r1)；r31-(r3-r1)；…；rn1-(rn-r1)]，
pi＝(xi-x0)/ri，
qi＝(yi-y0)/ri，
G＝[p1-p2，q1-q2；p1-p3，q1-q3；…；p1-pn，q1-qn]，
[△x，△y]＝inv(GT*inv(Q)*G)*GT*inv(Q)*E，
其中F(x，y)表示根据TDOA数据得出的函数，ri表示基站i到初始位置的距离，1<＝i<＝n，rij表示基站i到初始位置的距离与基站j到初始位置的距离的差值，E表示由ri1-(ri-r1)为分量组成的向量，2<＝i<＝n，pi表示F(x0，y0)对于x0的一阶偏导，qi表示F(x0，y0)对于y0的一阶偏导，根据等式F(x，y)变换，G表示由F(x，y)的一阶偏导组成的矩阵，δ表示解算的目标位置与实际目标位置的误差向量；Q与步骤1中相同，为误差的协方差矩阵；(△x，△y)是本轮迭代中求出的坐标偏差量，用其判断迭代是否收敛。设阈值参数为t＝100，收敛的判断条件为：
|△x|+|△y|<t，
如果本轮迭代没有收敛，则将该点设置丢点标记，判定解算失败；如果收敛，将最终计算出的结果放入数组X；如果迭代次数达到上限后Taylor序列方法仍然没有收敛，则将该点设置丢点标记，判定解算失败，否则将结果放入X。
步骤5中，已知目前所得二维结果为X；通过X根据最小二乘法反推出z轴的结果，反推公式为：
D＝d1^2-((X(1)-x1)^2+(X(2)-y1)^2)，
z(1)＝z1+sqrt(D)，
z(2)＝z1-sqrt(D)，
其中D为目标位置z轴坐标与主基站的平方差，当中间参数D为负数时，将结果0直接存入z中，即zz＝0；当D大于等于0时，将最小二乘法所得的结果分别存入数组z中，即z(1)、z(2)中，并跳转至步骤6中。
步骤6中，根据z的结果，反推出各个基站的TDOA数据，反推公式为：
d(j)＝sqrt((X(1)-xj)^2+(X(2)-yj)^2+(z(i)-zj)^2)，
dis(j)＝d(j)-d1，
diff(i)＝0，diff(i)＝diff(i)+dis(j)-dj1
其中i表示数组z中第i个值，j表示第j个基站的坐标，d1为实际的TOF数据，d表示目标位置的x、y轴结果X与z轴可能的结果z(i)，i＝1，2，联合成三维坐标，计算这个坐标位置与基站j的距离，1<＝j<＝n；通过解算出来的目标位置与主基站的TOF数据求差值，反推得到基站j的TDOA数据，记为dis；diff(i)表示在使用z(i)作为目标位置的z轴结果时，反推出来所有基站的TDOA数据与其测量的实际TDOA数据差值的和，选择使diff(i)最小的z(i)作为目标位置的z轴结果，存入zz中。
步骤7中，将得到的目标定位解算结果X与zz作为初始值为(x0，y0，z0)，即x0＝X(1)，y0＝X(2)，z0＝zz，根据TDOA数据的性质可以得到公式：
F(x，y，z)＝sqrt((x-xi)^2+(y-yi)^2+(z-zi)^2)-sqrt((x-xj^2+(y-yj)^2+(z-zj)^2)＝dij，
则改进后的Taylor序列方法表示为：
F(x0，y0，z0)+pi*△x+qi*△y+si*△z≈di+ei＝F(x，y，z)，
将上述等式转化为：
Gδ＝E+e，e＝[e1；e2；...；en]，δ＝[△x；△y；△z]：
ri＝sqrt((xi-x0)^2+(yi-y0)^2+(zi-z0)^2)，
rij＝ri-rj，
E＝[r21-(r2-r1)；r31-(r3-r1)；…；rn1-(rn-r1)]，
pi＝(xi-x0)/ri，
qi＝(yi-y0)/ri，
si＝(zi-z0)/ri，
G＝[p1-p2，q1-q2，s1-s2；p1-p3，q1-q3，s1-s3；…；p1-pn，q1-qn，s1-sn]，
[△x，△y，△z]＝inv(GT*inv(Q)*G)*GT*inv(Q)*E，
其中F(x，y，z)表示根据TDOA数据得出的函数，ri表示基站i到初始位置的距离，1<＝i<＝n，rij表示基站i到初始位置的距离与基站j到初始位置的距离的差值，E表示由ri1-(ri-r1)为分量组成的向量，2<＝i<＝n，pi表示F(x0，y0)对于x0的一阶偏导，qi表示F(x0，y0)对于y0的一阶偏导，si表示F(x0，y0)对于z0的一阶偏导，根据等式F(x，y，z)变换，G表示由F(x，y，z)的一阶偏导组成的矩阵，δ表示解算的目标位置与实际目标位置的误差向量；Q与步骤1中相同，为误差的协方差矩阵；(△x，△y，△z)是本轮迭代中求出的坐标偏差量，用其判断迭代是否收敛。设阈值参数为t＝100，收敛的判断条件为：
|△x|+|△y|+|△z|<t，
如果本轮迭代没有收敛，则将该点设置丢点标记，判定解算失败；如果收敛，将最终计算出的结果放入数组X；如果迭代次数达到上限后Taylor序列方法仍然没有收敛，则将该点设置丢点标记，判定解算失败，否则将结果放入数组Fn。
步骤8中，设定平均滤波的窗口大小为span，设定数组Fn中元素个数为k，则滤波公式为：
f＝(Fn(1)+Fn(2)+…+Fn(k))/k，其中k<span，
f＝(Fn(k)+Fn(k-1)+…+Fn(k-span+1))/span，其中k>＝span，
f即为滤波平滑处理后得到坐标，将其加入数组fr，Fn为根据步骤7的三维泰勒迭代之后得到的目标位置解算结果数组，span为滤波窗口大小，k为Fn数组中存储的解算结果的个数。
步骤9中，该条TDOA数据已经处理完毕，最终定位结果为Fn中最后一个坐标，滤波结果为f中最后一个坐标，f用于显示实时轨迹。此时需要读取下一条TDOA数据和TOF数据，处理下一时刻的信息。
实施例
为了验证提出方法的有效性，在实际环境部署场地并进行测试。测试场地为一个5m*7m左右的房间，房间左上角有一块玻璃，附近的信号会发生明显的反射。场地四周共部署了8个基站，基站的高度大概在3m左右。测试人员在场地内沿着边缘行走数周，运动轨迹接近于矩形。将此次采集的TDOA数据序列作为测试数据在本发明中进行计算，其中各步骤的实现及参数细节如下：
步骤1，读取一条含有n个及以上基站的到达时间差(Time Difference ofArrival，简称TDOA)数据和主基站的到达时间(Time of Flight，简称TOF)数据；
步骤2，利用TDOA数据和TOF数据联立方程作三维解算出2个三维结果，仅保存2个x、y轴结果；
步骤3，将2个三维结果分别进行反推计算出各基站TDOA数据和主基站的TOF数据。比较这些数据与实际测量TDOA数据和TOF数据差值，选取最小的一组x、y轴结果作为初步二维解算结果存入数组MS。
步骤4，对二维结果MS进行二维Taylor迭代。如果本轮迭代与上一轮迭代相比没有收敛，则将该点设置丢点标记，认为解算失败。若收敛，将最终计算出的结果放入数组X；若迭代次数达到上限后Taylor序列方法仍然没有收敛，则将该点设置丢点标记，认为解算失败，否则将结果放入X；
步骤5，如果X不为空，由X利用最小二乘法解算出两个z轴候选解算结果，分别记为z(1)、z(2)；
步骤6，通过将X数组作为x、y轴的值，z(1)、z(2)作为z轴的值，分别反推出各基站的TDOA数据。选出使得计算出的数据与实际差值最小的一个z值作为z轴的初步解算结果，记为zz；
步骤7，将X数组与zz合并为一个三维数组。进行三维Taylor迭代。如果本轮迭代与上一轮迭代相比没有收敛，则将该点设置丢点标记，认为解算失败。若收敛，将最终计算出的结果放入数组Fn；若迭代次数达到上限后Taylor序列方法仍然没有收敛，则将该点设置丢点标记，认为解算失败，否则将结果放入Fn；
步骤8，滤波的窗口大小span设为11，进行平均滤波；
步骤9，该条数据处理完毕，回到步骤1，读取下一条数据。
图3a和图3b分别是此TDOA数据和TOF数据序列通过本发明方法计算出的轨迹，图3c是该数据用Taylor序列方法计算出的散点图。不难看出，Taylor方法计算出的点较为分散，如果按时间顺序连接形成轨迹图，稳定性较差，存在严重的跳变现象。尤其是左上角，玻璃反射导致这部分数据质量较差，Taylor序列方法在这些数据上计算出的结果偏差明显增大，因此不具备在实际场景下使用的能力。而本发明得出的轨迹则十分接近真实轨迹，轨迹始终保持连续且较为平滑，定位精度也比较高，在10～20cm左右。左上角的数据通过本发明中的提出的优化策略，没有出现目标偏离和丢失现象，有力地证明了方法的有效性。
上述测试仅为多次测试中具有代表性的一次，其他环境、轨迹的下的测试都能够取得类似效果，定位的精度和轨迹的平滑连续性显著优于传统方法。
本发明提供了一种复杂环境下的三维定位追踪方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (8)

1.一种复杂环境下的三维定位追踪方法，其特征在于，包括如下步骤：
步骤1，读取一条含有n个及以上基站的到达时间差TDOA数据和主基站的到达时间TOF数据；
步骤2，将TDOA数据和TOF数据利用极大似然法联立方程解算出2个三维解算结果，并将2个三维解算结果的x、y轴结果放入数组A；
步骤3，由数组A中2个x、y轴三维解算结果分别反推出主基站的TOF数据和从基站的TDOA数据，并分别求反推结果与实际主基站TOF和从基站TDOA数据差值的平方和，选出平方和结果最小1个的x、y轴结果，将结果存入MS数组；
步骤4，对MS数组进行二维Taylor迭代，得到优化的x、y轴结果，将结果存入数组X；
步骤5，根据数组X中的x、y轴结果，通过最小二乘法解算，如果无实数解，则认为z轴结果为0，如果有实数解，则解出两个z轴结果，分别记为z(1)、z(2)；
步骤6，将z(1)、z(2)分别通过最小二乘法反推出主基站的z轴结果，并分别求反推结果与实际主基站z轴结果差值的平方，选出平方结果最小的z，并将这个平方结果最小的z记为zz；
步骤7，将数组X和zz联合进行三维Taylor解算，将解算出的三维结果放入最终定位结果的数组Fn；
步骤8，对数组Fn进行平均滤波，将滤波结果放入轨迹数组fr，用于显示实时轨迹；
步骤9，该条数据处理完毕，回到步骤1，读取下一条数据；
步骤1中，读入的TDOA数据data的形式为：
data＝(x1，y1，z1，d1；
x2，y2，z2，d12；
...
xn，yn，zn，d1n)，
中n表示该条TDOA数据中基站的个数，x1、y1、z1分别表示主基站的x、y、z轴坐标；1表示主基站，d1表示当前主基站的TOF数据，d1i表示当前待求坐标到主基站的距离和到序号为i的基站的距离之差，2<＝i<＝n，即d1i＝d1-di，xi、yi、zi分别表示基站i的x、y、z轴坐标；该步骤要求基站数大于等于4，即n>＝4；
步骤2包括：
将TDOA数据和TOF数据做变换，变换后的TOF数据为tof_data，变换后的TDOA数据为tdoa_data：
tof_data＝d1，
tdoa_data＝(d12，d13，d14，...，d1n)，
其中d1i表示当前坐标到主基站和基站i之间距离的差值，2<＝i<＝n；
设定基站的坐标为(x1，y1，z1)，(x2，y2，z2)，…，(xn，yn，zn)，其中(xi，yi，zi)表示基站i的三维坐标，该坐标已知，则改进的极大似然算法表示为：
目标位置与基站的距离关系为：(d1+di1)^2＝(x-xi)^2+(y-yi)^2+(z-zi)^2，
设定：ki＝xi^2+yi^2+zi^2；s＝x^2+y^2+z^2，将上述等式转化为：
d1^2-k1＝-2(x1*x+y1*y+z1*z)+s，
di1^2+2di1*d1＝-2(xi1*x+yi1*y+zi1*z)+ki-k1，
设定Za＝[x，y，z，d1]'，则上式转化为：
Figure FDA0003523509590000021
G＝-2*[x1，y1，z1，-s/(2d1)；x21，y21，z21，d12；...；xn1，yn1，zn1，d1n]，h＝[d1^2；d12^2+K1-K2；d13^2+K1-K3；...；d1n^2+K1-Kn]；
设定[d1，d21，...，dn1]'＝[r1，r21，...，rn1]'+e，得到：
误差φ＝2B*e+e^2≈2B*e，B＝diag(d1，d21，...，dn1)；
由于噪声未知，设定噪声为高斯噪声，得到高斯协方差矩阵为：
Q＝E[ee']＝diag[σ^1，σ^2，…，σ^n)]＝(0.5*ones(size(h，1))+0.5*eye(size(h，1)))*(delta^2)，
得到
Figure FDA0003523509590000022
则得到损失函数为：(h-G*Za)'pinv(Φ)(h-G*Za)，
将损失函数最小化，得出的Za值才为所求结果，根据极大似然法得到：
Za＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*h；
因此得到Za的协方差矩阵：
cov(Za)＝E[ΔzΔz']＝pinv(G'*pinv(Φ)*G)，
Δz＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*B*e；
再次进行最大似然估计，将上述所得结果Za设为：
Za＝[t1，t2，t3，t4]'，t1＝x+e1，t2＝y+e2，t3＝z+e3，t4＝d1+e4，
使用POS1保存Za向量的前三维，即Za＝[t1，t2，t3]'，对t1，t2，t3，t4求平方得到新的损失函数为：
Figure FDA0003523509590000031
h1＝[(t1-x1)^2，(t2-y1)^2，(t3-z1)^2，t4^2]'，
G1＝[1 0 0；0 1 0；0 0 1；1 1 1]，
Za1＝[(x-x1)^2，(y-y1)^2，(z-z1)^2]'，
Figure FDA0003523509590000032
2(y-y1)*e2+e2^2，2(z-z1)*e3+e3^2，2d1*e4+e4^2]'≈[2(x-x1)*e1，2(y-y1)*e2，2(z-z1)*e3，2d1*e4]'，
得到:
Figure FDA0003523509590000033
B1＝diag(x-x1，y-y1，z-z1，d1)，
Figure FDA0003523509590000034
再次根据最大似然估计得到：
Za1＝argmin(h1-G1*Za1)'*pinv(Φ1)*(h1-G1*Za1)＝pinv(G1'pinv(Φ1)*G1)*G1'pinv(Φ1)*h1，
最终得到POS2＝[x，y，z)]'＝[x1，y1，z1)]'±sqrt(Za1)；
其中x、y、z分别表示目标位置三维坐标；ki表示基站i的三维坐标平方和；s表示目标位置的三维坐标平方和，在第一次极大似然法求解中，Za表示目标位置坐标和TOF数据凑成的向量，φ表示结果存在的实际误差，h、G都为等式转化之后的矩阵，r1，r21，...，rn1分别对应表示d1，d21，...，dn1的实际精确值，e表示实际误差；B表示由d1，d21，...，dn1组成的对角矩阵，Q表示误差的协方差矩阵，Φ表示φ实际误差的协方差矩阵，cov(Za)表示Za的协方差矩阵，Δz表示Za与实际目标位置之间、实际目标位置和主基站距离的值的误差，POS1用于表示第一次极大似然法解算之后得到的三维目标位置坐标；在第二次极大似然法求解中，POS2表示第二次求解的最终目标位置的坐标，Za1表示为POS2与POS1误差的平方，t1、t2、t3、t4为Za向量中的四个分量，e1、e2、e3、e4分别表示t1、t2、t3、t4与实际的x、y、z、d1的误差，φ1表示Za与实际结果的大概误差，h1、G1都为等式转化之后凑成的矩阵，B1表示以x-x1，y-y1，z-z1，d1依次为对角线元素的对角矩阵，Φ1表示误差
Figure FDA0003523509590000035
的协方差矩阵；pinv表示矩阵的求逆操作，矩阵右上角的'表示矩阵转置，sqrt表示开方运算；diag{r2，r3，…，rn}表示以r2，r3，…，rn依次为对角线元素的对角矩阵；
最终求得的POS1和POS2为含有三个元素的向量，表示为POS1＝(t1，t2，t3)，POS2＝(x，y，z)，其中(t1，t2，t3)和(x，y，z)都为所求的坐标估计值。
2.根权利要求1所述的方法，其特征在于，步骤3中，将步骤2得到的两个三维解算结果POS1，POS2作为备选结果；分别选取这两个结果中的前两维，即x、y轴结果放入数组A中，即A保存了POS1，POS2的x、y轴值，用A中保存的两个x、y轴结果反推出误差最小的一组二维解算结果，反推公式为：
MSj＝[A(2*j1+1)A(2*j1+2)]；
R＝(d1，d12，d13，d14，...，d1n)；
tdoa_mainj＝sqrt((MSj(1)-x1)^2+(MS1(2)-y1)^2)；
toa_tempj＝sqrt((MSj(1)-x1)^2+(MSj(2)-y1)^2)-R(1)；
Addj＝n*toa_tempj^2；
tdoa_tempj＝sqrt((MSj(1)-xi)^2+(MSj(2)-yi)^2)-tdoa_main1-R(i)；
Addj＝Addj+n*tdoa_tempj^2；
其中MSj为第j1组备选的x、y轴结果，j1＝1，2；其中(x1，y1，z1)表示主基站的三维坐标；(xi，yi，zi)表示基站i的三维坐标，2<＝i<＝n；R表示主基站的TOF数据和从基站的TDOA数据集合数组；tdoa_mainj表示第j1组备选的二维结果反推出来的主基站的TOF结果；toa_tempj表示第j1组备选的二维结果反推出来的主基站TOF与实际TOF数据的差值；tdoa_tempj表示第j1组备选的二维结果反推出来的从基站的TDOA数据与实际TDOA数据的差值；将toa_tempj的平方和与tdoa_tempj的平方和求和记为Addj，取Addj值最小的一组结果作为x、y轴的初步解算结果，并将结果放入数组MS。
3.根据权利要求2所述的方法，其特征在于，步骤4中，将得到的MS(x，y)二维结果，作为初始位置(x0，y0)，即x0＝x，y0＝y，引入固定高度h，根据TDOA数据的性质得到等式为：
F(x，y)＝sqrt((x-xi)^2+(y-yi)^2+h^2)-sqrt((x-xj^2+(y-yj)^2+h^2)＝dij，
则改进后的Taylor序列方法表示为：
F(x0，y0)+pi*△x+qi*△y≈di+ei＝F(x，y)，
将上述等式转化为：
Gδ＝E+e，e＝[e1；e2；...；en]，δ＝[△x；△y]
r1＝sqrt((x1-x0)^2+(y1-y0)^2+h^2)，
ri＝sqrt((xi-x0)^2+(yi-y0)^2+h^2)，
rij＝ri-rj，
E＝[r21-(r2-r1)；r31-(r3-r1)；…；rn1-(rn-r1)]，
pi＝(xi-x0)/ri，
qi＝(yi-y0)/ri，
G＝[p1-p2，q1-q2；p1-p3，q1-q3；…；p1-pn，q1-qn]，
[△x，△y]＝inv(GT*inv(Q)*G)*GT*inv(Q)*E，
[x0 y0]＝[x0+△x y0+△y]
其中F(x，y)表示根据TDOA数据得出的函数，r1表示主基站到初始位置的距离，ri表示基站i到初始位置的距离，2<＝i<＝n，rij表示基站i到初始位置的距离与基站j到初始位置的距离的差值，E表示由ri1-(ri-r1)为分量组成的向量；根据等式F(x，y)变换，G表示由F(x，y)的一阶偏导组成的矩阵，δ表示解算的目标位置与实际目标位置的误差向量，Q为误差的协方差矩阵；(△x，△y)是本轮迭代中求出的坐标偏差量，设置阈值参数为t，收敛的判断条件为：
|△x|+|△y|<t，
如果迭代次数达到上限后Taylor序列方法仍然没有收敛，则判定目标位置的定位坐标解算失败；如果收敛，将最终计算出的结果放入数组X＝[x0，y0]。
4.根据权利要求3所述的方法，其特征在于，步骤5中，已知目前所得二维结果为X；通过X根据最小二乘法反推出z轴的结果，反推公式为：
D＝d1^2-((X(1)-x1)^2+(X(2)-y1)^2)，
z(1)＝z1+sqrt(D)，
z(2)＝z1-sqrt(D)，
其中D为目标位置z轴坐标与主基站的平方差，当中间参数D为负数时，将结果0直接存入zz中，即zz＝0，作为目标位置的z轴结果；当D大于等于0时，将最小二乘法所得的结果分别存入数组z中，即分别存入z(1)、z(2)中，并跳转至步骤6中。
5.根据权利要求4所述的方法，其特征在于，步骤6中，根据z的结果，反推出各个基站的TDOA数据，反推公式为：
d(i)＝sqrt((X(1)-xi)^2+(X(2)-yi)^2+(z(j2)-zi)^2)，
dis(i)＝d(i)-d1，
diff(j2)＝0，diff(j2)＝diff(j2)+dis(i)-di1
其中j2表示数组z中第j2个值，j2＝1，2；i表示第i个基站的坐标，d1为实际的TOF数据，d(i)表示计算坐标位置Xj2与基站i的距离，2<＝i<＝n；通过解算出来的目标位置与主基站的TOF数据求差值，反推得到基站i的TDOA数据，记为dis；diff(j2)表示在使用z(j2)作为目标位置的z轴结果时，反推出来所有基站的TDOA数据与其测量的实际TDOA数据差值的和，选择使diff(j2)最小的z(j2)作为目标位置的z轴结果，存入zz中。
6.根据权利要求5所述的方法，其特征在于，步骤7中，将得到的目标定位解算结果X与zz作为初始值为(x0，y0，z0)，即x0＝X(1)，y0＝X(2)，z0＝zz，根据TDOA数据的性质得到公式：
F(x，y，z)＝sqrt((x-xi)^2+(y-yi)^2+(z-zi)^2)-sqrt((x-xj^2+(y-yj)^2+(z-zj)^2)＝dij，
则改进后的Taylor序列方法表示为：
F(x0，y0，z0)+pi*△x+qi*△y+si*△z≈di+ei＝F(x，y，z)，
将上述等式转化为：
Gδ＝E+e，e＝[e1；e2；...；en]，δ＝[△x；△y；△z]：
ri＝sqrt((xi-x0)^2+(yi-y0)^2+(zi-z0)^2)，
rij＝ri-rj，
E＝[r21-(r2-r1)；r31-(r3-r1)；…；rn1-(rn-r1)]，
pi＝(xi-x0)/ri，
qi＝(yi-y0)/ri，
si＝(zi-z0)/ri，
G＝[p1-p2，q1-q2，s1-s2；p1-p3，q1-q3，s1-s3；…；p1-pn，q1-qn，s1-sn]，
[△x，△y，△z]＝inv(GT*inv(Q)*G)*GT*inv(Q)*E，
其中F(x，y，z)表示根据TDOA数据得出的函数，r1表示主基站到初始位置的距离，ri表示基站i到初始位置的距离，2<＝i<＝n，rij表示基站i到初始位置的距离与基站j到初始位置的距离的差值，E表示由ri1-(ri-r1)为分量组成的向量，根据等式F(x，y，z)变换，G表示由F(x，y，z)的一阶偏导组成的矩阵，δ表示解算的目标位置与实际目标位置的误差向量；(△x，△y，△z)是本轮迭代中求出的坐标偏差量，设置阈值参数为t，收敛的判断条件为：
|△x|+|△y|+|△z|<t，
如果迭代次数达到上限后Taylor序列方法仍然没有收敛，则判定目标位置的定位坐标解算失败；如果收敛，将最终计算出的结果放入数组Fn＝[x0，y0，z0]。
7.根据权利要求6所述的方法，其特征在于，步骤8中，设定平均滤波的窗口大小为span，设定数组Fn中元素个数为k，则滤波公式为：
f＝(Fn(1)+Fn(2)+…+Fn(k))/k，其中k<span，
f＝(Fn(k)+Fn(k-1)+…+Fn(k-span+1))/span，其中k>＝span，
f即为滤波平滑处理后得到坐标，将其加入数组fr，Fn为根据步骤7的三维泰勒迭代之后得到的目标位置解算结果数组，span为滤波窗口大小，k为Fn数组中存储的解算结果的个数。
8.根据权利要求7所述的方法，其特征在于，步骤9中，该条TDOA数据已经处理完毕，最终定位结果为Fn中最后一个坐标，滤波结果为f中最后一个坐标，f用于显示实时轨迹，此时需要读取下一条TDOA数据，处理下一时刻的信息。
CN202010927152.1A 2020-09-07 2020-09-07 一种复杂环境下的三维定位追踪方法 Active CN112066981B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010927152.1A CN112066981B (zh) 2020-09-07 2020-09-07 一种复杂环境下的三维定位追踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010927152.1A CN112066981B (zh) 2020-09-07 2020-09-07 一种复杂环境下的三维定位追踪方法

Publications (2)

Publication Number Publication Date
CN112066981A CN112066981A (zh) 2020-12-11
CN112066981B true CN112066981B (zh) 2022-06-07

Family

ID=73663755

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010927152.1A Active CN112066981B (zh) 2020-09-07 2020-09-07 一种复杂环境下的三维定位追踪方法

Country Status (1)

Country Link
CN (1) CN112066981B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107566065A (zh) * 2017-08-25 2018-01-09 中山大学深圳研究院 基于uwb的tof定位方法
CN110308419A (zh) * 2019-06-27 2019-10-08 南京大学 一种基于静态求解和粒子滤波的鲁棒tdoa定位方法
CN110657806A (zh) * 2019-09-30 2020-01-07 青岛联合创智科技有限公司 一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7667640B2 (en) * 2007-04-13 2010-02-23 Glowlink Communications Technology, Inc. Determining a geolocation solution of an emitter on earth using satellite signals
EP2353024B1 (fr) * 2008-12-05 2016-08-31 Thales Procede de geo-localisation d'un objet par multitelemetrie
US10261169B2 (en) * 2014-06-05 2019-04-16 Zebra Technologies Corporation Method for iterative target location in a multiple receiver target location system
CN109541529A (zh) * 2018-10-23 2019-03-29 北京凯乐比兴科技有限公司 一种基于uwb的idc机房的外来人员定位系统及方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107566065A (zh) * 2017-08-25 2018-01-09 中山大学深圳研究院 基于uwb的tof定位方法
CN110308419A (zh) * 2019-06-27 2019-10-08 南京大学 一种基于静态求解和粒子滤波的鲁棒tdoa定位方法
CN110657806A (zh) * 2019-09-30 2020-01-07 青岛联合创智科技有限公司 一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Performance comparison of TOA and TDOA based location estimation algorithms in LOS environment;Guowei Shen 等;《2008 5th Workshop on Positioning, Navigation and Communication》;20080502;第71-78页 *
TDOA-Based Joint Synchronization and Localization Algorithm for Asynchronous Wireless Sensor Networks;Tan Wang 等;《 IEEE Transactions on Communications 》;20200214;第68卷(第05期);第3107-3124页 *
超宽带室内定位算法研究与实现;赵杰磊;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20190715(第07期);第I136-304页 *
采用三次通信的TOF与TDOA联合定位算法;高健 等;《电子测量与仪器学报》;20200331;第34卷(第03期);第66-73页 *

Also Published As

Publication number Publication date
CN112066981A (zh) 2020-12-11

Similar Documents

Publication Publication Date Title
Kang et al. A high-accuracy TOA-based localization method without time synchronization in a three-dimensional space
CN109917333B (zh) 融合aoa观测量与tdoa观测量的无源定位方法
Li et al. Improved two-step constrained total least-squares TDOA localization algorithm based on the alternating direction method of multipliers
CN110933630A (zh) 基于超宽带通信的室内三维定位方法及装置
CN108872934B (zh) 一种基于非视距误差抑制的室内三维定位方法
CN109379711B (zh) 一种定位方法
CN110187333B (zh) 一种基于合成孔径雷达技术的rfid标签定位方法
CN110850363A (zh) 一种基于实时定位轨迹数据进行动态滤波优化的方法
Cui et al. Approximate closed-form TDOA-based estimator for acoustic direction finding via constrained optimization
Chen et al. TDOA/FDOA mobile target localization and tracking with adaptive extended Kalman filter
CN112066981B (zh) 一种复杂环境下的三维定位追踪方法
CN111444467B (zh) 基于实时定位轨迹数据进行局部线性插值和预测的方法
CN109613477B (zh) 一种复杂环境下的tdoa定位追踪方法
CN114051209B (zh) 一种基于智能反射面和场景几何模型的指纹定位方法
Landolsi et al. TOAI/AOA/RSS maximum likelihood data fusion for efficient localization in wireless networks
Lowrance et al. Direction of arrival estimation for robots using radio signal strength and mobility
Chang et al. Robust mobile location estimation using hybrid TOA/AOA measurements in cellular systems
Oh et al. Hybrid TDOA and AOA localization using constrained least squares
CN113970762A (zh) 一种多级干扰源定位方法及系统
CN110346757A (zh) 基于移动式测量站的抗多径时差定位方法、装置及系统
CN107484119B (zh) 一种用于移动通信系统的终端跟踪定位方法
李绍贤 et al. UWB positioning enhancement using Markov chain in indoor NLOS environment
Go et al. An efficient non-line-of-sight error mitigation method for TOA measurement in indoor environments
Nomura et al. Reference node selection for range-based localization using hierarchical clustering
CN113804199B (zh) 一种基于Chan氏算法和牛顿法的组合定位方法与系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant