基于定位单元质量及多算法数据融合的抗NLoS误差定位方法

摘要

本发明公开了一种基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，包括如下步骤：待测终端获得N个锚节点反馈的测量值，包括TOA、RSSI以及信噪比SNR；利用Kalman滤波器处理TOA测量值，得到新的TOA测量值，并将得到的TOA测量值分为两组：第二TOA测量值和TDOA测量值；采用模式搜索法和CHAN算法分别对第二TOA测量值和TDOA测量值进行定位计算；确定定位单元的权值，分别对第二TOA测量值和TDOA测量值的定位结果进行数据融合；对第二TOA测量值的融合定位结果和TDOA测量值的融合定位结果进行融合。本发明充分利用了测量值带来的各类信息，通过数据融合的方法进行整合，可以得到更为精确的定位结果。

说明书

技术领域

本发明属于无线通信技术领域，具体涉及一种基于定位单元质量及多算法数据融合 的抗NLoS误差定位方法。

背景技术

1996年美国联邦通信委员会（FCC）首次颁布了E-911定位需求，并在1999年提 出：对67%的移动终端的定位精度不低于50m，95%的定位精度不低于150m。这一要 求的提出掀起了移动通信系统终端定位技术的研究的热潮。

非视距传播（Non-Line-of-Sight，NLoS）带来的误差是无线定位中主要的误差来源。 非视距传播是指信号在传播过程中，直射路径被障碍物阻挡，经过反射、衍射等方式到 达接收端，此时的到达时间（Time-of-Arrival，TOA）、到达时间差 （Time-Difference-of-Arrival，TDOA）、到达角度（Angle-of-Arrival，AOA）等都不能 真实反映发射端和接收端之间的距离关系。在实际环境中，特别是城市、山区等环境中， 非视距误差是广泛存在的，可能带来几十米到几百米的误差。

随着数据融合技术的走入研究人员的视野，将数据融合技术与移动通信系统终端定 位相结合，利用更多的信息、方法协同获得待测终端的位置，从而进一步提高定位精度 成为当前移动定位技术的研究热点之一。

目前在JDL的数据融合模型基础上提出的基于数据融合的定位技术主要有下面几 类：

第一类，使用多种测量值，融合其中包含的不同方面的有用信息以提高定位精度。 例如Thomas Kleine-Ostmann和Amy E.Bell在如下文献中提出的方法：

A Data Fusion Architecture for Enhanced Position Estimation in Wireless Networks, IEEE COMMUNICATIONS LETTERS,VOL.5,NO.8,pp.343-345,2001

该方法的特点是融合了到达时间和到达时间差两种测量值，提出一种数据融合的模 型，并给出了第四层融合的具体选择方案。

第二类，使用同一类测量值，但采用不同的定位算法，最终通过对不同定位算法得 出的位置进行最佳线性组合，获得更好的定位精度。例如王玲霞，张效义，袁佳在如下 文献中提出的方法：

基于数据融合的定位解算算法，计算机工程与设计，30（21），4921-4927,2009。

该方法只使用了TDOA一种测量值，其特点在于采用了Chan算法、Taylor算法以 及优化设计中的退火算法，提出了一种多算法协同定位模型，第四层融合是采用最佳线 性组合，将多种算法的定位结果融合在一起，以获得更精确定位结果。

第三类，基于定位单元质量的数据融合。例如Xiaoyong Yan，Huanyan Qian，Ya  Huang，Zhipeng Jiang在如下文献中提出的方法：

A high accuracy localization based on RSSI measurements,Consumer Electronics, Communications and Networks(CECNet),2011International Conference on.IEEE,pp. 1926-1929,2011.

该方法的特点在于，将构成定位单元的三个锚节点的共线度作为对定位单元质量的 度量，越接近共线的定位单元定位质量越差，在最终的定位结果中所占比例越小，以此 来提高最终的定位精度。

第四类，使用Kalman滤波器，利用滤波算法进一步提高定位精度。例如David Macii， Alessio Colombo，Paolo Pivato和Daniele Fontanelli在如下文献中提出的方法：

Data Fusion Technique for Wireless Ranging Performance Improvement，IEEE  TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT,VOL.62,NO.1,PP. 27-37,2013.

该算法的特点在于，使用了到达信号强度（Received Signal Strength Index，RSSI） 和Two-way Time-of-Flight（ToF）两种测量值，并使用Kalman滤波器对定位精度进行 提高。

以上算法都在一定程度上提高了移动终端的定位精度，然而都只是从一个方面使用 了数据融合技术，对信息的利用仍有提高的空间，可进一步提高定位的精度。同时这些 方法没有加入对NLoS误差的处理，在NLoS普遍存在的环境下，定位精度会大幅下降。

因此，需要一种基于定位单元质量及多算法数据融合的抗NLoS误差定位方法以解 决上述问题。

发明内容

发明目的：本发明针对现有技术中无线定位方法的缺陷，提供一种更为精确的基于 定位单元质量及多算法数据融合的抗NLoS误差定位方法。

技术方案：为解决上述技术问题，本发明的基于定位单元质量及多算法数据融合的 抗NLoS误差定位方法采用如下技术方案：

一种基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，包括以下步骤：

1）、利用待测终端获得定位单元反馈的测量值，所述定位单元包括N个锚节点，其中， N≥3，所述测量值包括TOA、RSSI和信噪比SNR；

2）、利用Kalman滤波器计算TOA测量值中的NLoS误差，在所述TOA测量值中减去 所述NLoS误差，得到减小了NLoS误差的TOA测量值，将所述减小了NLoS误差的 TOA测量值分为两组:一组为第二TOA测量值，另一组为TDOA测量值；

3）、利用模式搜索法对步骤2）得到的所述第二TOA测量值的进行定位计算，利用Chan 算法对所述TDOA测量值进行定位计算；

4）、基于定位单元与待测终端距离、定位单元的三点共线度和定位单元的可靠性确定所 述第i个定位单元的权值w_i，

$w_i=0.4w_i^d+0.2w_i^a+0.4w_i^{\mathrm{SNR}},$

式中，为基于定位单元与待测终端的距离得到的权值，为基于定位单元的三 点共线度得到的权值，为基于定位单元的可靠性得到的权值，

设第i个定位单元的定位结果为(x_i,y_i)，则融合后的定位结果(x,y)为：

$x=\underset{i}{\Sigma }{w}_i{x}_i,y=\underset{i}{\Sigma }{w}_i{y}_i$

分别对步骤3）得到的所述第二TOA测量值的定位结果和TDOA测量值的定位结果利 用上式进行融合，得到所述第二TOA测量值的融合定位结果和TDOA测量值的融合定 位结果；

5）、将步骤4）得到的所述第二TOA测量值的融合定位结果和TDOA测量值的融 合定位结果进行融合，设所述第二TOA测量值的融合定位结果为(x_TOA,y_TOA)，方差为 σ_TOA,x，σ_TOA,y；所述TDOA测量值的融合定位结果为(x_TDOA,y_TDOA)，方差为σ_TDOA,x， σ_TDOA,y，按下式进行对所述第二TOA测量值的融合定位结果和TDOA测量值的融合定 位结果进行融合，得到待测终端的定位结果(x',y')：

$x^{\prime }=\frac{{\sigma }_{\mathrm{TDOA},x}}{{\sigma }_{\mathrm{TDOA},x}+{\sigma }_{\mathrm{TOA},x}}{x}_{\mathrm{TOA}}+\frac{{\sigma }_{\mathrm{TOA},x}}{{\sigma }_{\mathrm{TDOA},x}+{\sigma }_{\mathrm{TOA},x}}{x}_{\mathrm{TDOA}}$

$y^{\prime }=\frac{{\sigma }_{\mathrm{TDOA},y}}{{\sigma }_{\mathrm{TDOA},y}+{\sigma }_{\mathrm{TOA},y}}{y}_{\mathrm{TOA}}+\frac{{\sigma }_{\mathrm{TOA},y}}{{\sigma }_{\mathrm{TDOA},y}+{\sigma }_{\mathrm{TOA},y}}{y}_{\mathrm{TDOA}}.$

更进一步的，步骤4）中基于定位单元与待测终端的距离得到的权值通过下式计 算得到：

使用对数常态模型来计算接收信号强度与距离之间的关系：

$\mathrm{PL}\left(d\right)=\mathrm{PL}\left(d_0\right)+10n\log \left(\frac{d}{d_0}\right)+e_n$

其中，PL(d)为距离dkm时的路径损耗，d₀为参考距离，n为路径损耗指数，e_n为 均值为0的高斯随机变量，

将N个所述锚节点三个一组分为个组，每一组根据步骤1）得到的RSSI测量值 获得一组距离(d_i1,d_i2,d_i3)，则基于定位单元与待测终端的距离的权值为：

$w_i^d=(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}})/\underset{i=1}{\overset{N}{\Sigma }}(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}}).$

更进一步的，步骤4）中基于定位单元三点共线度得到的权值通过下式计算得到： 设第i个定位单元构成的三角形的最小角为则基于定位单元三点共线度的权值为：

$w_i^a=q_i/\underset{i=1}{\overset{N}{\Sigma }}{q}_i$其中，$q_i=\frac{{3\alpha }_{\min }^i}{\pi }.$

更进一步的，步骤4）中基于定位单元的可靠性得到的权值通过下式计算得到：

将所述锚节点三个一组分为个组，每一组根据步骤1）得到的信噪比SNR测量 值获得一组信噪比(SNR_i1,SNR_i2,SNR_i3)，

$w_i^{\mathrm{SNR}}=({\mathrm{SNR}}_{i1}+{\mathrm{SNR}}_{i2}+{\mathrm{SNR}}_{i3})/\underset{i=1}{\overset{N}{\Sigma }}({\mathrm{SNR}}_{i1}+{\mathrm{SNR}}_{i2}+{\mathrm{SNR}}_{i3}).$

更进一步的，步骤2）所述的Kalman滤波器由下式表示：

状态方程：x[t+1]＝Φx[t]+Γω[t]

观测方程：y[t]＝Hx[t]+υ[t]

式中，Φ为状态转移矩阵，H为观测矩阵，ω[t]和υ[t]分别为过程噪声和测量误差， 都服从均值为0的高斯分布，且互不相关，其中，

$x\left[t\right]={\left(\begin{array}{cc}{\tau }_{\mathrm{LOS}}\left[t\right]& e_{\mathrm{NLOS}}\left[t\right]\end{array}\right)}^T,\Phi =\left(\begin{array}{cc}1& 0\\ 0& \beta \end{array}\right),H=\left(\begin{array}{cc}1& 0\end{array}\right),$

式中，τ_LOS[t]是锚节点与待测终端之间的视距传播时间，e_NLOS[t]为NLoS误差。

更进一步的，假设在t时刻，得到t-1时刻的估计值$\hat{x}[t-1|t-1]={\left(\begin{array}{cc}a& b\end{array}\right)}^T,$估计误差 的协方差矩阵$\hat{P}[t-1|t-1]=\left(\begin{array}{cc}0& 0\\ 0& p\end{array}\right),$则：

$\beta =\arg \min \left\{\right|e_{\mathrm{NLOS}}^{\prime }\left[t\right]-\mathrm{\beta b}+\frac{{\beta }^{2}p}{{\beta }^{2}p+{\sigma }_{\omega }^2}\left(y\right[t]-a-\mathrm{\beta b})\left|\right\},$

其中β是状态转移矩阵Φ中第二行第二列的未知元素。

所述Kalman滤波器的迭代过程为：

$\hat{x}\left[t\right|t]=\tilde{x}[t|t-1]+K\left[t\right]ϵ\left[t\right]$

$\tilde{x}\left[t\right|t-1]=\Phi \hat{x}[t-1|t-1]$

$ϵ\left[t\right]=y\left[t\right]-H\tilde{x}\left(t\right|t-1)$

$K\left[t\right]=\tilde{P}\left[t\right|t-1]H^T{\left[H\tilde{P}\right[t|t-1]H^T+R]}^{-1}$

$\tilde{P}\left[t\right|t-1]=\Phi \hat{P}[t-1|t-1]{\Phi }^T+\mathrm{\Gamma Q}{\Gamma }^{-1}$

$\hat{P}\left[t\right|t]=[I_n-K\left[t\right]H\left]\tilde{P}\right[t|t-1]$

在迭代过程中，当NLoS误差的估计值小于0时，则将其估计值设置为0。

更进一步的，步骤3）中，所述模式搜索法包括以下步骤：f(x)为待测终端到三个 锚节点的距离残差绝对值的和，

（1）给定初始点x⁽¹⁾∈Rⁿ，n个坐标方向初始步长δ，加速因子α≥1， 缩减率γ∈(0,1)和允许误差ε＞0，令y⁽¹⁾＝x⁽¹⁾，k=1，j=1；

（2）如果f(y^(j)+δe_j)＜f(y^(j))，则令y^(j+1)＝y^(j)+δe_j，转至步骤（4），否则转至步 骤（3）；

（3）如果f(y^(j)-δe_j)＜f(y^(j))，则令y^(j+1)＝y^(j)-δe_j，转至步骤（4），否则令 y^(j+1)＝y^(j)转至步骤（4）；

（4）如果j＜n，则j＝j+1，转到步骤（2），否则转至步骤（5）；

（5）如果f(y⁽ⁿ⁺¹⁾)＜f(x^(k))，则转至步骤（6），否则转至步骤（7）；

（6）令x^(k+1)＝y⁽ⁿ⁺¹⁾，y⁽¹⁾＝x^(k+1)+α(x^(k+1)-x^(k))，k＝k+1，j＝1，转到步骤（2）；

（7）如果δ≤ε则停止迭代，得到点x^(k)，否则，令δ＝γδ，y⁽¹⁾＝x^(k)，x^(k+1)＝x^(k)， k＝k+1，j＝1，转到步骤（2）。

更进一步的，步骤3）所述的Chan算法包括以下步骤：

设待测终端的坐标为(x,y)，N个锚节点的坐标为(x_i,y_i)（i=1,2，…，N），r_j为第j 锚节点与待测终端的距离，r_j为第j锚节点与待测终端的距离，第j锚节点和第i锚节点 与待测终端距离差为r_j,i＝r_j-r_i，

r_i²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y²

其中，将r_i²＝(r_i，1+r₁)²带入上式可得：

$r_{i,1}^2+2r_{i,1}{r}_1+{r_1}^2=K_i-2x_{i}x-2y_{i}y+x^2+y^2$

将r₁²＝(x₁-x)²+(y₁-y)²带入上式，整理可得：

$r_{i,1}^2+2r_{i,1}{r}_1=2x_{i,1}x-2y_{i,1}y+K_i-K_1$

其中，x_j,i＝x_j-x_i，y_j,i＝y_j-y_i，

有噪声的情况下，误差向量为：

$\psi =h-G_a{z}_a^0$

其中，$h=\frac{1}{2}\left(\begin{array}{c}{r}_{\mathrm{2,1}}^2-K_2+K_1\\ r_{\mathrm{3,1}}^2-K_3+K_1\\ ·\\ ·\\ ·\\ r_{N,1}^2-K_N+K_1\end{array}\right),$$G_a=-\left(\begin{array}{ccc}{x}_{\mathrm{2,1}}& y_{\mathrm{2,1}}& r_{\mathrm{2,1}}\\ x_{\mathrm{3,1}}& y_{\mathrm{3,1}}& r_{\mathrm{3,1}}\\ ·& ·& ·\\ ·& ·& ·\\ ·& ·& ·\\ x_{N,1}& y_{N,1}& r_{N,1}\end{array}\right),$$z_a={\left(\begin{array}{cc}{z}_p^T& r_1\end{array}\right)}^T,$其中，z_p＝[x y]^T， 带有上标0表示真实值，则误差矩阵为：

式中，r_i⁰是第i个锚节点到待测终端之间的真实距离，c为 光速，n为噪声矩阵，忽略可得ψ的协方差Ψ为：

Ψ＝E[ψψ^T]＝c²BQB

式中，Q为噪声的协方差矩阵，则z_a由下式计算得到：

$z_a\approx {\left(G_a^T{Q}^{-1}{G}_a\right)}^{-1}{G}_a^T{Q}^{-1}h$

利用上式得到z_a的估计值，得到待测终端位置的一个估计值，用以计算B的值，将 B的值代入下式，得到新的z_a，

$z_a={\left(G_a^T{\psi }^{-1}{G}_a\right)}^{-1}{G}_a^T{\psi }^{-1}h$

$\mathrm{cov}\left(z_a\right)={\left(G_a^T{\psi }^{-1}{G}_a\right)}^{-1}$

$h^{\prime }=\left(\begin{array}{c}{(z_{a,1}-x_1)}^2\\ {(z_{a,2}-y_1)}^2\\ z_{a,3}^2\end{array}\right),$$G_a^{\prime }=\left(\begin{array}{cc}1& 0\\ 0& 1\\ 1& 1\end{array}\right),$$z_a^{\prime }=\left(\begin{array}{c}{(x-x_1)}^2\\ {(y-y_1)}^2\end{array}\right)$

Ψ′＝4B′cov(z_a)B′，B′＝diag{x⁰-x₁,y⁰-y₁,r₁⁰}

$z_a^{\prime }={\left(G_a^{\prime T}{\psi }^{\prime -1}{G}_a^{\prime }\right)}^{-1}{G}_a^{\prime T}{\psi }^{\prime -1}{h}^{\prime }$

$z_p=\sqrt{z_a^{\prime }}+\left(\begin{array}{c}{x}_1\\ y_1\end{array}\right)$或$z_p=-\sqrt{z_a^{\prime }}+\left(\begin{array}{c}{x}_1\\ y_1\end{array}\right)$

z_p选取与所述z_a的估计值结果接近的值。

有益效果：本发明的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法 充分利用了测量值带来的各类信息，通过数据融合的方法进行整合，可以得到更为精确 的定位结果。

附图说明

图1是本发明的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法的流 程图；

图2是本发明的模式搜索法的流程图；

图3为山地环境下本发明与第三类已有方法的定位结果比较。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本 发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各 种等价形式的修改均落于本申请所附权利要求所限定的范围。

请参阅图1所示，本发明公开了一种基于定位单元质量及多算法数据融合的抗NLoS 误差定位方法，包括以下步骤：

1）、利用待测终端获得定位单元反馈的测量值，定位单元包括N个锚节点，其中，N≥ 3，测量值包括TOA、RSSI和信噪比SNR，这些测量值包含了待测终端与锚节点之间 的距离、接受到信号的质量即可信度等信息。

2）、利用Kalman滤波器计算TOA测量值中的NLoS误差，在TOA测量值中减去NLoS 误差，得到减小了NLoS误差的TOA测量值。非视距（Non-Line-of-Sight NLoS）误差 是引起较大定位误差的主要因素之一。实验证明，该方法对较大的NLoS误差追踪性能 较好，为了得到更好的滤波结果，可以重复使用该方法。将减小了NLoS误差的TOA 测量值分为两组:一组为第二TOA测量值，另一组变型为TDOA测量值；

其中，Kalman滤波器的各类矩阵如下设置：

状态方程：x[t+1]＝Φx[t]+Γω[t]

观测方程：y[t]＝Hx[t]+υ[t]

式中，Φ为状态转移矩阵，H为观测矩阵，ω[t]和υ[t]分别为过程噪声和测量误差， 都服从均值为0的高斯分布，且互不相关，其中，

$x\left[t\right]={\left(\begin{array}{cc}{\tau }_{\mathrm{LOS}}\left[t\right]& e_{\mathrm{NLOS}}\left[t\right]\end{array}\right)}^T,\Phi =\left(\begin{array}{cc}1& 0\\ 0& \beta \end{array}\right),H=\left(\begin{array}{cc}1& 0\end{array}\right),$

式中，τ_LOS[t]是锚节点与待测终端之间的视距传播时间，e_NLOS[t]为NLoS误差。

假设在t时刻，得到t-1时刻的估计值$\hat{x}[t-1|t-1]={\left(\begin{array}{cc}a& b\end{array}\right)}^T,$估计误差的协方差矩阵 $\hat{P}[t-1|t-1]=\left(\begin{array}{cc}0& 0\\ 0& p\end{array}\right),$则：

$\beta =\arg \min \left\{\right|e_{\mathrm{NLOS}}^{\prime }\left[t\right]-\mathrm{\beta b}+\frac{{\beta }^{2}p}{{\beta }^{2}p+{\sigma }_{\omega }^2}\left(y\right[t]-a-\mathrm{\beta b})\left|\right\},$

其中β是转移矩阵Φ中第二行第二列的未知元素。

Kalman滤波器的迭代过程为：

$\hat{x}\left[t\right|t]=\tilde{x}[t|t-1]+K\left[t\right]ϵ\left[t\right]$

$\tilde{x}\left[t\right|t-1]=\Phi \hat{x}[t-1|t-1]$

$ϵ\left[t\right]=y\left[t\right]-H\tilde{x}\left(t\right|t-1)$

$K\left[t\right]=\tilde{P}\left[t\right|t-1]H^T{\left[H\tilde{P}\right[t|t-1]H^T+R]}^{-1}$

$\tilde{P}\left[t\right|t-1]=\Phi \hat{P}[t-1|t-1]{\Phi }^T+\mathrm{\Gamma Q}{\Gamma }^{-1}$

$\hat{P}\left[t\right|t]=[I_n-K\left[t\right]H\left]\tilde{P}\right[t|t-1]$

在迭代过程中，当NLoS误差的估计值小于0时，这显然与实际情况不符，这种情 况下将其估计值置为0。

3）、利用模式搜索法对步骤2）得到的第二TOA测量值的进行定位计算，利用Chan 算法对TDOA测量值进行定位计算；

请参阅图2所示，模式搜索法包括以下步骤：f(x)为待测终端到三个锚节点的距离 残差绝对值的和，

（1）给定初始点x⁽¹⁾∈Rⁿ，n个坐标方向初始步长δ，加速因子α≥1， 缩减率γ∈(0,1)和允许误差ε＞0，令y⁽¹⁾＝x⁽¹⁾，k=1，j=1；

（2）如果f(y^(j)+δe_j)＜f(y^(j))，则令y^(j+1)＝y^(j)+δe_j，转至步骤（4），否则转至步 骤（3）；

（3）如果f(y^(j)-δe_j)＜f(y^(j))，则令y^(j+1)＝y^(j)-δe_j，转至步骤（4），否则令 y^(j+1)＝y^(j)转至步骤（4）；

（4）如果j＜n，则j＝j+1，转到步骤（2），否则转至步骤（5）；

（5）如果f(y⁽ⁿ⁺¹⁾)＜f(x^(k))，则转至步骤（6），否则转至步骤（7）；

（6）令x^(k+1)＝y⁽ⁿ⁺¹⁾，y⁽¹⁾＝x^(k+1)+α(x^(k+1)-x^(k))，k＝k+1，j＝1，转到步骤（2）；

（7）如果δ≤ε则停止迭代，得到点x^(k)，否则，令δ＝γδ，y⁽¹⁾＝x^(k)，x^(k+1)＝x^(k)， k＝k+1，j＝1，转到步骤（2）。

Chan算法包括以下步骤：

设待测终端的坐标为(x,y)，N个锚节点的坐标为(x_i,y_i)（i=1,2，…，N），r_j为第j 锚节点与待测终端的距离，r_j为第j锚节点与待测终端的距离，第j锚节点和第i锚节点 与待测终端距离差为r_j,i＝r_j-r_i，

r_i²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y²

其中，将r_i²＝(r_i,1+r₁)²带入上式可得：

$r_{i,1}^2+2r_{i,1}{r}_1+{r_1}^2=K_i-2x_{i}x-2y_{i}y+x^2+y^2$

将r₁²＝(x₁-x)²+(y₁-y)²带入上式，整理可得：

$r_{i,1}^2+2r_{i,1}{r}_1=2x_{i,1}x-2y_{i,1}y+K_i-K_1$

其中，x_j,i＝x_j-x_i，y_j,i＝y_j-y_i，

在有噪声的情况下，误差向量可写为：

$\psi =h-G_a{z}_a^0$

其中，$h=\frac{1}{2}\left(\begin{array}{c}{r}_{\mathrm{2,1}}^2-K_2+K_1\\ r_{\mathrm{3,1}}^2-K_3+K_1\\ ·\\ ·\\ ·\\ r_{N,1}^2-K_N+K_1\end{array}\right),$$G_a=-\left(\begin{array}{ccc}{x}_{\mathrm{2,1}}& y_{\mathrm{2,1}}& r_{\mathrm{2,1}}\\ x_{\mathrm{3,1}}& y_{\mathrm{3,1}}& r_{\mathrm{3,1}}\\ ·& ·& ·\\ ·& ·& ·\\ ·& ·& ·\\ x_{N,1}& y_{N,1}& r_{N,1}\end{array}\right),$$z_a={\left(\begin{array}{cc}{z}_p^T& r_1\end{array}\right)}^T,$其中，z_p＝[x y]^T， 带有上标0表示真实值，其中，n_i,1表示噪声，c为光速，可得：

式中，r_i⁰是第i个锚节点到待测终端之间的真实距离，c为 光速，n为噪声矩阵，一般来说，cn_i，1＜＜r_i⁰，所以可忽略可得ψ的协方差Ψ 为：

Ψ＝c²BQB

式中，Q为噪声的协方差矩阵，则z_a可由下式计算得到：

$z_a\approx {\left(G_a^T{Q}^{-1}{G}_a\right)}^{-1}{G}_a^T{Q}^{-1}h$

利用上式得到z_a的估计值，得到待测终端位置的一个估计值，用以估计B的值，将 B的值代入下式，得到新的z_a，

$z_a={\left(G_a^T{\psi }^{-1}{G}_a\right)}^{-1}{G}_a^T{\psi }^{-1}h$

$\mathrm{cov}\left(z_a\right)={\left(G_a^T{\psi }^{-1}{G}_a\right)}^{-1}$

$h^{\prime }=\left(\begin{array}{c}{(z_{a,1}-x_1)}^2\\ {(z_{a,2}-y_1)}^2\\ z_{a,3}^2\end{array}\right),$$G_a^{\prime }=\left(\begin{array}{cc}1& 0\\ 0& 1\\ 1& 1\end{array}\right),$$z_a^{\prime }=\left(\begin{array}{c}{(x-x_1)}^2\\ {(y-y_1)}^2\end{array}\right)$

ψ′＝4B′cov(z_a)B′，B′＝diag{x⁰-x₁，y⁰-y₁，r₁⁰}

$z_a^{\prime }={\left(G_a^{\prime T}{\psi }^{\prime -1}{G}_a^{\prime }\right)}^{-1}{G}_a^{\prime T}{\psi }^{\prime -1}{h}^{\prime }$

$z_p=\sqrt{z_a^{\prime }}+\left(\begin{array}{c}{x}_1\\ y_1\end{array}\right)$或$z_p=-\sqrt{z_a^{\prime }}+\left(\begin{array}{c}{x}_1\\ y_1\end{array}\right)$

z_p选取与所述z_a的估计值结果接近的值。

4）、在大多数情况下，锚节点构成的定位单元与待测移动端的距离越近，定位单元所构 成的三角形越接近正三角形，定位的精度也就越高。越高的SNR反映对应信号的可靠 性越高，所以平均SNR高的定位单元对待测终端的定位结果也就越可靠。本发明从这 三个方面对定位单元进行评估。基于定位单元与待测终端距离、定位单元的三点共线度 和定位单元的可靠性确定第i个定位单元的权值w_i，

$w_i=0.4w_i^d+0.2w_i^a+0.4w_i^{\mathrm{SNR}},$

式中，为基于定位单元与待测终端的距离得到的权值，为基于定位单元的三 点共线度得到的权值，为基于定位单元的可靠性得到的权值。

其中，基于定位单元与待测终端的距离得到的权值通过下式计算得到：

使用对数常态模型来计算接收信号强度与距离之间的关系：

$\mathrm{PL}\left(d\right)=\mathrm{PL}\left(d_0\right)+10n\log \left(\frac{d}{d_0}\right)+e_n$

其中，PL(d)为距离dkm时的路径损耗，d₀为参考距离，n为路径损耗指数，e_n为 均值为0的高斯随机变量，

将N个锚节点三个一组分为个组，每一组根据步骤1）得到的RSSI测量值获得 一组距离(d_i1,d_i2,d_i3)，则基于定位单元与待测终端的距离的权值为：

$w_i^d=(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}})/\underset{i=1}{\overset{N}{\Sigma }}(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}}).$

基于定位单元三点共线度得到的权值通过下式计算得到：

设第i个定位单元构成的三角形的最小角为则基于定位单元三点共线度的权值为：

$w_i^a=q_i/\underset{i=1}{\overset{N}{\Sigma }}{q}_i$其中，$q_i=\frac{{3\alpha }_{\min }^i}{\pi }.$

基于定位单元的可靠性得到的权值通过下式计算得到：

将锚节点三个一组分为个组，每一组根据步骤1）得到的信噪比SNR测量值获得一 组信噪比(SNR_i1,SNR_i2,SNR_i3)，

$w_i^{\mathrm{SNR}}=({\mathrm{SNR}}_{i1}+{\mathrm{SNR}}_{i2}+{\mathrm{SNR}}_{i3})/\underset{i=1}{\overset{N}{\Sigma }}({\mathrm{SNR}}_{i1}+{\mathrm{SNR}}_{i2}+{\mathrm{SNR}}_{i3}).$

设第i个定位单元的定位结果为(x_i,y_i)，则融合后的定位结果(x,y)为：

$x=\underset{i}{\Sigma }{w}_i{x}_i,y=\underset{i}{\Sigma }{w}_i{y}_i$

分别对步骤3）得到的第二TOA测量值的定位结果和TDOA测量值的定位结果利 用上式进行融合，得到第二TOA测量值的融合定位结果和TDOA测量值的融合定位结 果；

5）、将步骤4）得到的第二TOA测量值的融合定位结果和TDOA测量值的融合定 位结果进行融合，设第二TOA测量值的融合定位结果为(x_TOA,y_TOA)，方差为σ_TOA,x， σ_TOA,y；TDOA测量值的融合定位结果为(x_TDOA,y_TDOA)，方差为σ_TDOA,x，σ_TDOA,y，按下式 进行对第二TOA测量值的融合定位结果和TDOA测量值的融合定位结果进行融合，得 到待测终端的定位结果(x',y')：

$x^{\prime }=\frac{{\sigma }_{\mathrm{TDOA},x}}{{\sigma }_{\mathrm{TDOA},x}+{\sigma }_{\mathrm{TOA},x}}{x}_{\mathrm{TOA}}+\frac{{\sigma }_{\mathrm{TOA},x}}{{\sigma }_{\mathrm{TDOA},x}+{\sigma }_{\mathrm{TOA},x}}{x}_{\mathrm{TDOA}}$

$y^{\prime }=\frac{{\sigma }_{\mathrm{TDOA},y}}{{\sigma }_{\mathrm{TDOA},y}+{\sigma }_{\mathrm{TOA},y}}{y}_{\mathrm{TOA}}+\frac{{\sigma }_{\mathrm{TOA},y}}{{\sigma }_{\mathrm{TDOA},y}+{\sigma }_{\mathrm{TOA},y}}{y}_{\mathrm{TDOA}}.$

实施例1

本发明在山地环境下进行仿真，该环境下TOA测量值被NLoS（Non-Line-of-Sight 非视距）误差污染严重，可以明显的看出本算法在NLoS环境下的优越性。

信号在待测节点和锚节点i之间的传播时间可以用如下模型来描述：

t_i＝t_i,LoS+t_i,n+t_i,NLoS

其中t_i是测量得到的传播时间，t_i,LoS是信号在锚节点i与待测节点之间视距传播时需 要的时间，t_i,n是测量误差，服从均值为0，方差为σ_ω²的高斯分布，在误差中所占比例 较小，t_i,NLoS是NLoS传播造成的误差，与测量误差相互独立，可能服从平均分布、Delta 分布和指数分布，是误差的主要来源。本发明考虑服从指数分布的NLoS误差，给出其 概率密度函数为：

$f\left({\tau }_i\right)=\left(\begin{array}{cc}\frac{1}{{\tau }_{i,\mathrm{rms}}}\exp (-\frac{{\tau }_i}{{\tau }_{i,\mathrm{rms}}})& {\tau }_i>0\\ 0& {\tau }_i\le 0\end{array}\right)$

其中τ_i,rms为均方根时延扩展，表达式为T是τ_i,rms在d_i＝1km时的中值， d_i为待测节点与锚节点之间的真实距离，ε为0.5～1之间的数，ξ是服从对数正态分布的 随机变量，均值为0，方差为σ_ξ²，即η＝10lgξ～N(0,σ_ξ²)，σ_ξ取值在4～6dB之间。 下表列出来在不同环境下的参数。

典型环境参数

无线信道环境 T/μs ε σ_ξ/dB 繁华市区 0.90 0.5 4 一般市区 0.40 0.5 4 郊区 0.30 0.5 4 远郊 0.10 0.5 4 山区 0.50 1.0 6

选取山区环境，设待测终端位置为（50，-28），锚节点坐标为(1000,1000)，(-500,500)， (-1500,-1400)，(2000,-2000)，测量误差t_i,n的方差为0.04，发射端的发射功率为43dB，信道 模型中的损耗系数n=3.41。

分别用本发明和第三类方法进行定位，分别计算定位结果与真实位置的距离，即定 位误差，重复进行1000次试验，得到定位误差的累计分布函数（CDF），如图3所示， 从图中可明显看出本发明的定位结果优于第三类方法。