一种基于到达时差和到达频差的定位方法、装置及设备

摘要

本发明公开了一种基于到达时差和到达频差的定位方法、装置及设备，所述方法包括：建立基于到达时差和到达频差的定位模型；获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；列出概率密度函数对应的极大似然估计表达式；根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值。本发明将到达时差测量噪声、到达频差测量噪声以及测量节点位置噪声引入在内，同时通过迭代方式进一步提升目标节点位置和速度的估计精度，能提高在高噪声和远场环境下对目标节点定位的精确性和鲁棒性。

权利要求

1.一种基于到达时差和到达频差的定位方法，其特征在于，包括：建立基于到达时差和到达频差的定位模型；

基于所述定位模型，获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；

根据所述到达时差测量噪声向量的概率密度函数、所述到达频差测量噪声向量的概率密度函数以及所述测量节点位置噪声的概率密度函数获得总的概率密度函数，列出所述总的概率密度函数对应的极大似然估计表达式；

根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；

对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值。

2.根据权利要求1所述的定位方法，其特征在于，所述到达时差测量噪声向量、所述到达频差测量噪声向量以及所述测量节点位置噪声向量均是零均值高斯噪声向量。

3.根据权利要求1所述的定位方法，其特征在于，所述根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值之前，还包括：将所述极大似然估计表达式转化为凸函数的形式。

4.根据权利要求1所述的定位方法，其特征在于，所述对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值，包括：将所述位置初始估计值确定为当前位置估计值，将所述速度初始估计值确定为当前速度估计值；

对所述当前位置估计值和所述当前速度估计值进行迭代优化，每次优化基于所述当前位置估计值和所述当前速度估计值得到修正速度估计值，基于所述修正速度估计值得到修正位置估计值，将所述修正位置估计值确定为当前位置估计值，将所述速度估计值确定为当前速度估计值，直到对所述当前位置估计值和所述当前速度估计值进行迭代优化的次数达到预设迭代次数；

将所述修正位置估计值确定为最终位置估计值；

将所述修正速度估计值确定为最终速度估计值。

5.根据权利要求4所述的定位方法，其特征在于，所述基于所述当前位置估计值和所述当前速度估计值得到修正速度估计值，采用的是加权最小二乘法方法。

6.根据权利要求4所述的定位方法，其特征在于，所述基于所述修正速度估计值得到修正位置估计值，采用的是半正定规划方法。

7.一种基于到达时差和到达频差的定位装置，其特征在于，包括：建模模块、检测模块、预处理模块、估计模块、优化模块；

所述建模模块用于建立基于到达时差和到达频差的定位模型；

所述检测模块用于基于所述定位模型，获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；

所述预处理模块用于根据所述到达时差测量噪声向量的概率密度函数、所述到达频差测量噪声向量的概率密度函数以及所述测量节点位置噪声的概率密度函数获得总的概率密度函数，列出所述总的概率密度函数对应的极大似然估计表达式；

所述估计模块用于根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；

所述优化模块用于对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值。

8.根据权利要求7所述的定位装置，其特征在于，所述定位装置还包括：问题转化模块，用于将所述极大似然估计表达式转化为凸函数的形式。

9.根据权利要求7所述的定位装置，其特征在于，所述优化模块还包括：速度修正单元，用于基于所述当前位置估计值和所述当前速度估计值，得到修正速度估计值；

位置修正单元，用于基于所述修正速度估计值，得到修正位置估计值。

10.一种计算机设备，其特征在于，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述指令由所述处理器加载并执行以实现如权利要求1至6任一项所述定位方法。

说明书

一种基于到达时差和到达频差的定位方法、装置及设备

技术领域

[0001] 本发明涉及无线传感器网络节点定位领域，具体涉及一种基于到达时差和到达频差的定位方法、装置及设备。

背景技术

[0002] 在人员难以企及的深远地区以及有毒有害、高压高温等恶劣野外环境中，无线传感器网络被大量使用以实现多种不同的功能。在追踪、监控、网络通信等各种无线传感器应用中，节点的位置信息很重要，因而对待测节点的位置及速度进行测量具有重要意义。在实际的应用中，尤其是军事领域，被动定位技术如到达时差(TDOA)、到达频差(FDOA)等被广泛使用。而基于应用的相关要求，高精度、高鲁棒性是所有方法必须具备的特性之一。

[0003] 由于测量所用传感器精度的有限性，加上野外环境中大量的干扰、阴影遮蔽或阻挡，少量的误差也会对测量结果带来很大的影响。同时，由于野外布设的随机性与节点自身的移动性，测量节点自身的位置误差同样需要引入到计算中。现有采用两步加权最小二乘法对问题直接线性近似的方式引入了较大的误差，在高噪声环境下测量精度迅速降低；而采用半正定规划方法相关理论对位置和速度同时计算的方式，忽略了位置和速度两者的耦合关系，在高噪声和远场条件下的测量精度依然不理想，因而在实际应用中受限。

发明内容

[0004] 针对现有技术在高噪声和远场环境中对目标节点定位精度较低的问题，本发明提供一种基于到达时差和到达频差的定位方法、装置及设备。

[0005] 为解决上述技术问题，本发明采用的技术方案是：

[0006] 第一方面，提供一种基于到达时差和到达频差的定位方法，包括：

[0007] 建立基于到达时差和到达频差的定位模型；

[0008] 基于所述定位模型，获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；

[0009] 根据所述到达时差测量噪声向量的概率密度函数、所述到达频差测量噪声向量的概率密度函数以及所述测量节点位置噪声的概率密度函数获得总的概率密度函数，列出所述总的概率密度函数对应的极大似然估计表达式；

[0010] 根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；

[0011] 对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值。

[0012] 进一步地，所述到达时差测量噪声向量、所述到达频差测量噪声向量以及所述测量节点位置噪声向量均是零均值高斯噪声向量。

[0013] 进一步地，所述根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值之前，还包括：

[0014] 将所述极大似然估计表达式转化为凸函数的形式。

[0015] 进一步地，所述对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值，包括：

[0016] 将所述位置初始估计值确定为当前位置估计值，将所述速度初始估计值确定为当前速度估计值；

[0017] 对所述当前位置估计值和所述当前速度估计值进行迭代优化，每次优化基于所述当前位置估计值和所述当前速度估计值得到修正速度估计值，基于所述修正速度估计值得到修正位置估计值，将所述修正位置估计值确定为当前位置估计值，将所述速度估计值确定为当前速度估计值，直到对所述当前位置估计值和所述当前速度估计值进行迭代优化的次数达到预设迭代次数；

[0018] 将所述修正位置估计值确定为最终位置估计值；

[0019] 将所述修正速度估计值确定为最终速度估计值。

[0020] 进一步地，所述基于所述当前位置估计值和所述当前速度估计值得到修正速度估计值，采用的是加权最小二乘法方法。

[0021] 进一步地，所述基于所述修正速度估计值得到修正位置估计值，采用的是半正定规划方法。

[0022] 第二方面，提供一种基于到达时差和到达频差的定位装置，包括：建模模块、检测模块、预处理模块、估计模块、优化模块；

[0023] 所述建模模块用于建立基于到达时差和到达频差的定位模型；

[0024] 所述检测模块用于基于所述定位模型，获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；

[0025] 所述预处理模块用于根据所述到达时差测量噪声向量的概率密度函数、所述到达频差测量噪声向量的概率密度函数以及所述测量节点位置噪声的概率密度函数获得总的概率密度函数，列出所述总的概率密度函数对应的极大似然估计表达式；

[0026] 所述估计模块用于根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；

[0027] 所述优化模块用于对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值。

[0028] 进一步地，所述定位装置还包括：

[0029] 问题转化模块，用于将所述极大似然估计表达式转化为凸函数的形式。

[0030] 进一步地，所述优化模块还包括：

[0031] 速度修正单元，用于基于所述当前位置估计值和所述当前速度估计值，得到修正速度估计值；

[0032] 位置修正单元，用于基于所述修正速度估计值，得到修正位置估计值。

[0033] 第三方面，提供一种计算机设备，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述指令由所述处理器加载并执行以实现所述定位方法。

[0034] 本发明的技术方案带来的有益效果是：

[0035] 本发明将TDOA测量噪声、FDOA测量噪声以及测量节点自身的位置噪声考虑在内，同时考虑位置和速度的耦合关系，采用加权最小二乘法和半正定规划方法迭代的方式轮流对位置和速度进行优化，能够提高在高噪声和远场环境中定位的精确性和鲁棒性，特别适合在实际定位领域中应用。

附图说明

[0036] 为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图做简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

[0037] 图1是本发明实施例一种基于到达时差和到达频差的定位方法流程图。

[0038] 图2是本发明实施例一种基于到达时差和到达频差的定位方法的迭代方法流程图。

[0039] 图3是本发明实施例仿真实验中测量节点的位置和速度配置图。

[0040] 图4A是本发明实施例仿真实验，测量节点位置噪声固定，近场源位置的均方根误差随系统测量噪声强度变化关系示意图；

[0041] 图4B是本发明实施例仿真实验，测量节点位置噪声固定，近场源速度的均方根误差随系统测量噪声强度变化关系示意图；

[0042] 图4C是本发明实施例仿真实验，TDOA/FDOA系统测量噪声固定，近场源位置的均方根误差随测量节点位置噪声强度变化关系示意图；

[0043] 图4D是本发明实施例仿真实验，TDOA/FDOA系统测量噪声固定，近场源速度的均方根误差随测量节点位置噪声强度变化关系示意图；

[0044] 图5A是本发明实施例仿真实验，测量节点位置噪声固定，中场源位置的均方根误差随系统测量噪声强度变化关系示意图；

[0045] 图5B是本发明实施例仿真实验，测量节点位置噪声固定，中场源速度的均方根误差随系统测量噪声强度变化关系示意图；

[0046] 图5C是本发明实施例仿真实验，TDOA/FDOA系统测量噪声固定，中场源位置的均方根误差随测量节点位置噪声强度变化关系示意图；

[0047] 图5D是本发明实施例仿真实验，TDOA/FDOA系统测量噪声固定，中场源速度的均方根误差随测量节点位置噪声强度变化关系示意图；

[0048] 图6A是本发明实施例仿真实验，测量节点位置噪声固定，远场源位置的均方根误差随系统测量噪声强度变化关系示意图；

[0049] 图6B是本发明实施例仿真实验，测量节点位置噪声固定，远场源速度的均方根误差随系统测量噪声强度变化关系示意图；

[0050] 图6C是本发明实施例仿真实验，TDOA/FDOA系统测量噪声固定，远场源位置的均方根误差随测量节点位置噪声强度变化关系示意图；

[0051] 图6D是本发明实施例仿真实验，TDOA/FDOA系统测量噪声固定，远场源速度的均方根误差随测量节点位置噪声强度变化关系示意图；

[0052] 图7是本发明实施例一种基于到达时差和到达频差的定位装置结构框图。

[0053] 图8是本发明实施例一种基于到达时差和到达频差的定位装置结构框图。

具体实施方式

[0054] 为了使本技术领域的人员更好的理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分的实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

[0055] 本发明实施例提供的定位方法将测量节点自身的位置误差以高斯噪声的形式引入，并包含在问题整体的概率密度函数中，获得将测量节点位置误差考虑在内的极大似然表达式，在此基础上将所述极大似然表达式利用半正定规划方法进行近似转变为凸函数，使用CVX工具包对该问题进行求解，得到目标节点的速度初始估计值和位置初始估计值，再使用加权最小二乘法方法(WLS，Weighted Least Square)和半正定规划方法(SDP，Semidefinite Program)迭代的方式对位置和速度进行优化。

[0056] 本发明实施例提供的定位方法流程图如图1所示，具体包括：

[0057] S1：建立基于到达时差和到达频差的定位模型；

[0058] 假设定位系统中有N个传感器(或发射器，或基站)作为测量节点，第i个传感器的真实位置(未受到噪声影响的力量测量位置)和真实速度(未受到噪声影响的理论测量速度)分别用 和 表示。在本实施例中传感器的数量设置为5,5个传感器的三维坐标和速度如图3所示，本实施例方式分别定位一个近场信号源、一个中场信号源和一个远场信号源，近场信号源的三维位置为[285,325,275]Tm；中场信号源的三维位置为[600,650,550]Tm；远场信号源的三维位置为[1500,1200,1000]Tm；所述近场信号源、所述中场信号源以及所述远场信号源的速度均为[-20,15,40]Tm/s；将所述近场信号源或所述中场信号源或所述远场信号源作为将要定位的目标节点，并将所述目标节点的位置和速度分别用x和 表示。

[0059] 对于第i个传感器，将目标节点到其的距离定义为ri，将目标节点到其的距离变化率定义为 则有 在定位系统中，以第1个传感器作为基准，定位得到的所述第i个传感器与所述第1个传感器和所述目标节点之间的距离差可以表示为：

[0060]

[0061] 所述第i个传感器与所述第1个传感器和所述目标节点之间的距离变化率差可以表示为：

[0062]

[0063] 其中，||.||为欧几里得距离，为ni1为距离差值测量噪声， 为距离变化率差值测量噪声。

[0064] 若所述第i个传感器与所述第1个传感器的到达时差TDOA和到达频差FDOA分别用ti1和fi1表示，则由N-1个TDOA构成的到达时差向量可以表示为Td＝[t21,t31,…tN1]T，由N-1个FDOA构成的到达频差向量可以表示为Fd＝[f21,f31,…fN1]T，相应的，距离差向量和距离变化率差向量分别可表示为：

[0065] d＝[d21,d31,…dN1]T，

[0066] 那么，所述到达时差ti1和所述距离差di1之间、所述到达频差fi1和所述距离变化率差 之间、所述距离差向量d与所述到达时差向量Td之间以及所述距离变化率差向量 与所述到达频差向量Fd之间的关系可以表示为：

[0067]

[0068]

[0069] 其中，c为信号传输的速度，f0为载波频率。

[0070] 即式(4)可表述为：所述第i个传感器与所述第1个传感器和所述目标节点的距离差值di1，等价于所述第i个传感器与所述第1个传感器的TDOA值乘以比例系数c；所述第i个传感器与所述第1个传感器和所述目标节点的变化率差值 等价于第i个传感器与第1个传感器的FDOA值乘以比例系数c/f0。那么，ni1可以代表所述到达时差测量噪声， 可以代表所述到达频差测量噪声。接下来，定义如下表达式：

[0071]

[0072] 则式(4)中的d和 可以另写为：

[0073]

[0074] 其中，G＝[-1N-1,IN-1]，IN-1为N-1维单位矩阵，1N-1表示N维全1列向量)，n＝[n2,1,n3,1,…nN,1]T代表由N-1个到达时差测量噪声构成的到达时差测量噪声向量，由N-1个到达频差测量噪声构成的到达频差测量噪声向量。

[0075] 若用si表示所述第i个传感器的误差位置，则所述第i个传感器的误差位置si和真实位置 之间有如下关系：

[0076]

[0077] 其中，ψi是第i个传感器节点的位置噪声即所述测量节点位置噪声，其方差为(σi为第i个传感器节点位置误差的标准差)，ID为D*D的单位矩阵，D表示空间的维数(D＝2or 3)，ψ＝[ψ1,ψ2,…ψN]T表示由N-1个测量节点位置噪声构成的测量节点位置噪声向量。

[0078] S2：获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；

[0079] 由步骤S1可知，所述到达时差测量噪声向量n可以表示为[n2,1,n3,1,…nN,1]T，所述到达频差测量噪声向量 可以表示为 所述测量节点位置噪声向量可以表示为[ψ1,ψ2,…ψN]T。

[0080] 进一步地，将所述到达时差测量噪声、所述到达频差测量噪声及所述测量节点位置噪声均视为零均值的高斯噪声，则所述到达时差测量噪声向量、所述到达频差测量噪声向量及所述测量节点位置噪声均为零均值的高斯噪声向量。则n的协方差矩阵可以表示为Qt＝E[nnT]、 的协方差矩阵可以表示为 ψ的协方差矩阵可以表示为其中， diag(A)表示取矩阵A主对角线上的
元素。

[0081] S3：列出概率密度函数对应的极大似然估计表达式；

[0082] 进一步地，将所述到达时差测量噪声向量的概率密度函数、所述到达频差测量噪声向量的概率密度函数以及所述测量节点位置噪声的概率密度函数相加得到总的概率密度函数。

[0083] 假设n、及ψ三个噪声向量信号相互独立，将以上三个零均值已知方差的高斯分布噪声相加后，所得变量依然为零均值高斯分布，方差为所述三个噪声的方差之和。

[0084] 采用极大似然估计方法，对所述总的概率密度函数取对数并舍去常数项，得到概率密度函数对应的极大似然估计等价表达式：

[0085]

[0086] 进一步地，根据式(5)可以得到所述到达时差测量噪声向量的等价表达式可以表示为n＝d-Gr，所述到达频差测量噪声向量的等价表达式可以表示为

[0087] 进一步地，利用n的等价表达式、的等价表达式以及协方差矩阵Qt和Qf，将所述极大似然估计等价表达式(7)直接变形为矩阵形式，得到概率密度函数对应的极大似然估计表达式：

[0088]

[0089] 其中，A1＝A[IN,0N,N]，A2＝A[0N,N,IN]，A＝[-1N-1IN-1]，0N,N表示N维全0方阵，IN表示N维单位矩阵， S＝[s1,…,sN]。

[0090] S4：根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；

[0091] 为求解目标节点的位置初始估计值和速度初始估计值，需将步骤S3中的所述极大似然估计表达式(8)转换为凸函数的形式，首先将所述极大似然估计表达式(8)展开，定义Θ＝θθT,用Θ替换掉展开式中的θθT，将所述极大似然估计表达式(8)的目标函数转换为如下形式：

[0092]

[0093] 其中,tr(.)表示矩阵的迹，可以发现式(9)的目标函数是Θ和θ的线性函数。令Y＝XTX，式(8)的问题约束也可以利用柯西不等式等方法处理为：

[0094] Θi,i＝Y(1,1)-2Y(1,i+2)+Y(i+2,i+2),    (10)

[0095]

[0096] Θi,j≥||Y(1,1)-Y(1,i+2)-Y(1,j+2)

[0097] +Y(i+2，j+2)||,1≤i≤j≤N,      (12)

[0098]

[0099] 由于上述目标函数是凸函数，但是约束条件中的θθT项仍是非凸的，为求解极大似然估计问题，需先将约束条件转换为凸函数形式。

[0100] 进一步地，使用半正定规划松弛方法，把非凸项Θ＝θθT松弛为Θ≥θθT，把非凸项Y＝XTX松弛为Y≥XTX，将Θ和Y转换为线性矩阵不等式的形式：

[0101]

[0102] 进一步地，综合式(9)至式(14)，式(8)的极大似然估计表达式可转换为半正定规划问题：

[0103]

[0104]

[0105] Θi,j≥‖Y(1,1)-Y(1,i+2)-Y(1,j+2)+Y(i+2,j+2)‖,1≤i

[0106]

[0107]

[0108] 其中，η1与η2是为了保证问题收敛定义的两个惩罚因子：

[0109]

[0110]

[0111] 进一步地，使用CVX最优化工具包对所述半正定规划问题(15)求解，得到所述位置初始估计值 和所述速度初始估计

[0112] 由于极大似然估计具有非线性以及非凸特性，单独采用半正定规划方式获取的值精确度较低，而且由于FDOA系统中位置和速度测量的耦合关系，本发明实施例在所述位置初始估计值和所述速度初始估计值的基础上进行优化，进一步提升目标节点位置估计和速度估计的精度。

[0113] S5：对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值；

[0114] 将所述位置初始估计值确定为当前位置估计值，将所述速度初始估计值确定为当前速度估计值；

[0115] 对所述当前位置估计值和所述当前速度估计值进行迭代优化，每次优化后得到修正位置估计值和修正速度估计值，直到对所述当前位置估计值和所述当前速度估计值进行迭代优化的次数达到预设迭代次数；

[0116] 将对所述当前位置估计值和所述当前速度估计值进行迭代优化的次数达到预设迭代次数的所述修正位置估计值确定为最终位置估计值；

[0117] 将对所述当前位置估计值和所述当前速度估计值进行迭代优化的次数达到预设迭代次数的所述修正速度估计值确定为最终速度估计值。

[0118] 进一步地，所述对所述当前位置估计值和所述当前速度估计值进行迭代优化，每次优化后得到修正估计值和修正速度估计值的步骤如图2所示，具体如下：

[0119] S501：首先对所述当前速度估计值进行修正，基于所述当前位置估计值和所述当前速度估计值，使用加权最小二乘法方法，可获得目标节点的修正速度估计值：

[0120]

[0121] 其中，

[0122]

[0123]

[0124]

[0125] S502：然后对所述当前位置估计值进行修正，基于得到的所述修正速度估计值，采用半正定规划方法得到修正位置估计值；

[0126] 进一步地，所述采用半正定规划方法得到修正位置估计值的步骤包括：

[0127] 用所述修正速度估计值 代替式(8)中的X(:,2)，可以获得目标节点位置的极大似然估计表达式：

[0128]

[0129] s.t.θi＝||X(:,1)-X(:,i+2)||,i＝1,…,N,

[0130]

[0131] 其中，

[0132] 使用与式(9)至式(14)相同的方式，将所述极大似然估计表达式(22)转换为凸函数的形式，利用柯西不等式对问题约束进行处理，通过半正定规划松弛方法将式(22)转化为半正定规划问题：

[0133]

[0134] s.t.Θi,i＝Y(1,1)-2Y(1,i+1)+Y(i+1,i+1),

[0135]

[0136] Θi,j≥‖Y(1,1)-Y(1,i+1)-Y(1,j+1)+Y(i+1,j+1)‖,1≤i

[0137]

[0138] θi≥||X(:,1)-X(:,i+2)||,

[0139]

[0140] 通过使用CVX最优化工具包对式(23)求解，可以得到所述修正位置估计值[0141] S503：判断当前迭代次数是否小于预设迭代次数；

[0142] S504：若当前迭代次数小于所述预设迭代次数，则将所述修正位置估计值确定为所述当前位置估计值，所述修正速度估计值确定为所述当前速度估计值，并返回执行步骤S501、步骤S502及步骤S503，继续进行迭代计算；

[0143] S505：若当前迭代次数达到所述预设迭代次数，停止迭代。

[0144] 进一步地，S501步骤中加权矩阵W1值可以通过如下计算过程获得：

[0145] 将式(6)式子变形为 将式(2)中x用 替代， 用si替代，并使用一阶多元泰勒级数展开，可以得到：

[0146]

[0147] 其中，

[0148]

[0149]

[0150]

[0151] 在式(25)和式(26)中，目标节点的速度 用 替代，同时令：

[0152]

[0153] ∈＝[∈2,…,∈N]T   (29)

[0154] 且定义如下表达式：

[0155] F1＝[b2,…,bN]T,    (30)

[0156]

[0157] 则式(29)可以改写为：

[0158]

[0159] 加权矩阵W1可表示为：

[0160] W1＝E(∈∈T),     (33)

[0161] 假设Δx，ψ和 彼此是不相关的，式(33)进一步可以表示为：

[0162]

[0163] 其中，Qx＝E(ΔxΔxT)。

[0164] 进一步地，S502步骤中加权矩阵W2值的可以通过如下计算过程获得：

[0165] 将式(6)变形为 式(2)中 用 替代， 用si替代，并使用一阶多元泰勒级数展开，可以得到：

[0166]

[0167] 其中，

[0168]

[0169]

[0170]

[0171] 式(36)和式(37)中，x用 代替，令：

[0172]

[0173] γ＝[γ2,…,γN]T,   (40)

[0174] 同时定义如下表达式：

[0175]

[0176]

[0177] 则式(40)可以改写为：

[0178]

[0179] 根据式(43)，可以获得加权矩阵W2的表达式：

[0180] W2＝E(γγT),   (44)

[0181] 假设 ψ和 彼此是不相关的，式(44)进一步可以写成：

[0182]

[0183] 其中，

[0184] 为验证算法的性能，本发明实施例设计几组仿真实验。仿真分别定位一个近场信号源、一个中场信号源以及一个远场信号源，信号源的位置和速度如前所述，5个移动的传感器作为测量节点，所述5个传感器的位置和速度配置如图3所示。

[0185] 在仿真实验中，用Proposed代表本发明实施例所述定位方法，将Proposed方法与其它方法的性能进行了对比。其余的方法分别为：利用加权最小二乘法对问题直接进行线性近似的两步加权最小二乘方法2WLS；使用迭代方式轮流对目标节点的位置和速度估计进行处理，但并未将测量节点自身的位置误差考虑在整个问题的计算之中的方法Zou；以及克拉美罗界CRLB。性能评估参数使用均方根误差 其中，M是蒙特卡洛次数(本发明实施例中M＝1000)，u是目标节点真实位置或真实速度， 是第j次估算位置或估算速度。

[0186] 由于所述到达时差测量噪声和所述到达频差测量噪声均是零均值的高斯噪声，相应地，所述到达时差测量噪声向量的方差矩阵可以表示为Qt＝E[nnT]＝δ2Σ，所述到达频差测量噪声向量的方差矩阵可以表示为 其中，δ表示到达时差或到达频差测量噪声水平(即系统测量噪声水平)，Σ的主对角线上的元素为1，其它非主对角线上的元素均为0.5；测量节点位置噪声向量的方差矩阵可以表示为Qw＝σ2ID，其中，σ为测量节点位置噪声水平，ID可以是二维或三维的单位矩阵，本发明实施例使用三维单位矩阵；
迭代次数设为2。模拟在不同噪声水平δ和σ变化的环境下几种定位方法的性能曲线。

[0187] 图4A至图4D是定位近场信号源时几种定位方法的性能曲线。图4A和图4B是当所述测量节点位置噪声水平σ固定为0.1，对应位置和速度的均方根误差随系统测量噪声水平δ变化时的曲线图；不难发现，所述2WLS方法存在门限效应，当所述系统测量噪声水平较低时，所述2WLS方法计算误差与克拉美罗界CRLB接近，但当所述系统测量噪声水平δ达到某个特定值后，其精度开始急剧下降。图4C和图4D是当所述系统测量噪声水平δ固定为0.1，对应位置和速度的均方根误差随所述测量节点位置噪声水平σ变化时的曲线图，此时，所述2WLS方法和所述Zou方法均存在门限效应，当所述测量节点位置噪声水平σ达到一定值后，所述Zou的方法计算的精度也迅速下降。而本发明实施例所述方法即使在所述系统测量噪声水平和所述测量节点位置噪声水平较高时，其计算误差均无限逼近CRLB。

[0188] 图5A至图5D是定位中场信号源时几种定位方法的性能曲线。图5A和图5B是当所述测量节点位置噪声水平σ固定为0.1，对应位置和速度的均方根误差随所述系统测量噪声水平δ变化时的曲线图；5C和图5D是当所述系统测量噪声水平δ固定为0.1，对应位置和速度的均方根误差随所述测量节点位置噪声水平σ变化时的曲线图。在这两种情况下，所述2WLS方法和所述Zou方法均存在门限效应，随着噪声水平的不断上升，稳定性急剧下降，特别当所述测量节点位置噪声水平增大时，其计算误差远远大于CRLB，计算精度迅速降低。而本发明实施例所述方法，其计算误差仍然接近CRLB，保持高的计算精度。

[0189] 图6A至图6D是定位远场信号源时几种定位方法的性能曲线。图6A和图6B当所述测量节点位置噪声水平σ固定为0.1，对应位置和速度的均方根误差随所述系统测量噪声水平δ变化时的曲线图；图6C和图6D是当所述系统测量噪声水平δ固定为0.1，对应位置和速度的均方根误差随所述测量节点位置噪声水平σ变化时的曲线图。此时所述2WLS方法、所述Zou方法及本发明实施例所述方法差异凸显，所获得的位置和速度精度较低，而本发明实施例所述方法计算的位置和速度误差依然与CRLB接近。

[0190] 综上所述，所述2WLS方法和所述Zou方法在噪声强度较低或近场环境下保证了良好的定位精度，一旦噪声过高或处于远场环境下，则会存在严重的“门限效应”；当噪声强度达到某个水平时，定位误差会急剧增加。而本发明实施例所述定位方法即使在高噪声和远场环境下，仍然表现良好，其误差与接近CRLB。因此，本发明实施例所述定位方法具有高的精确度和鲁棒性。

[0191] 图7是本发明实施例提供的一种基于到达时差和到达频差的定位装置的结构框图，为简化起见，仅示出了与文中所述的主题相关的模块，总体的定位装置可具有许多其它构造和可使用许多其它类型的装备。如图7所示，所述装置包括：建模模块601、检测模块602、预处理模块603、估计模块604、优化模块605、问题转化模块606；

[0192] 所述建模模块601用于建立基于到达时差和到达频差的定位模型；

[0193] 所述检测模块602用于基于所述定位模型，获得到达时差测量噪声向量、到达频差测量噪声向量以及测量节点位置噪声向量；

[0194] 所述预处理模块603用于根据所述到达时差测量噪声向量的概率密度函数、所述到达频差测量噪声向量的概率密度函数以及所述测量节点位置噪声的概率密度函数获得总的概率密度函数，列出所述总的概率密度函数对应的极大似然估计表达式；

[0195] 所述估计模块604用于根据所述极大似然估计表达式，得到目标节点的位置初始估计值和速度初始估计值；

[0196] 所述优化模块605用于对所述位置初始估计值和所述速度初始估计进行迭代优化，获得目标节点的最终位置估计值和最终速度估计值；

[0197] 所述问题转化模块606用于将所述极大似然估计表达式转化为凸函数的形式。

[0198] 进一步地，所述优化模块605的结构框图如图8所示，包括：

[0199] 速度修正单元6051，用于基于所述当前位置估计值和所述当前速度估计值，得到修正速度估计值；

[0200] 位置修正单元6052，用于基于所述修正速度估计值，得到修正位置估计值。

[0201] 本发明实施例还提供一种计算机设备，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述指令由所述处理器加载并执行以实现所述定位方法。

[0202] 在本发明所提供的实施例中，应该理解，所揭露的装置/设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。

[0203] 上述说明已经充分揭露了本发明的具体实施方式。需要指出的是，熟悉该领域的技术人员对本发明的具体实施方式所做的任何改动均不脱离本发明的权利要求书的范围。相应地，本发明的权利要求的范围也并不仅仅局限于前述具体实施方式。