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

Abstract

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

Description

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

技术领域

本发明属于无线通信技术领域，具体涉及一种基于定位单元质量及多算法数据融合的抗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 inWireless Networks,IEEE COMMUNICATIONS LETTERS,VOL.5,NO.8,pp.343-345,2001

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

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

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

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

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

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

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

第四类，使用Kalman滤波器，利用滤波算法进一步提高定位精度。例如DavidMacii，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}{\mathrm{\Σ}}{w}_i{x}_i,y=\underset{i}{\mathrm{\Σ}}{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^{\′}=\frac{{\mathrm{\σ}}_{\mathrm{TDOA},x}}{{\mathrm{\σ}}_{\mathrm{TDOA},x}+{\mathrm{\σ}}_{\mathrm{TOA},x}}{x}_{\mathrm{TOA}}+\frac{{\mathrm{\σ}}_{\mathrm{TOA},x}}{{\mathrm{\σ}}_{\mathrm{TDOA},x}+{\mathrm{\σ}}_{\mathrm{TOA},x}}{x}_{\mathrm{TDOA}}$

$y^{\′}=\frac{{\mathrm{\σ}}_{\mathrm{TDOA},y}}{{\mathrm{\σ}}_{\mathrm{TDOA},y}+{\mathrm{\σ}}_{\mathrm{TOA},y}}{y}_{\mathrm{TOA}}+\frac{{\mathrm{\σ}}_{\mathrm{TOA},y}}{{\mathrm{\σ}}_{\mathrm{TDOA},y}+{\mathrm{\σ}}_{\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}{\mathrm{\Σ}}}(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}}).$

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

$w_i^a=q_i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{q}_i$ 其中， $q_i=\frac{{3\mathrm{\α}}_{\min }^i}{\mathrm{\π}}.$

更进一步的，步骤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}{\mathrm{\Σ}}}({\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}{\mathrm{\τ}}_{\mathrm{LOS}}\left[t\right]& e_{\mathrm{NLOS}}\left[t\right]\end{array}\right]}^T,\mathrm{\Φ}=\left[\begin{array}{cc}1& 0\\ 0& \mathrm{\β}\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],$ 则：

$\mathrm{\β}=\arg \min \left\{\right|e_{\mathrm{NLOS}}^{\′}\left[t\right]-\mathrm{\βb}+\frac{{\mathrm{\β}}^{2}p}{{\mathrm{\β}}^{2}p+{\mathrm{\σ}}_{\mathrm{\ω}}^2}\left(y\right[t]-a-\mathrm{\βb})\left|\right\},$

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

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

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

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

$\mathrm{\ϵ}\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]=\mathrm{\Φ}\hat{P}[t-1|t-1]{\mathrm{\Φ}}^T+\mathrm{\ΓQ}{\mathrm{\Γ}}^{-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，

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

$\mathrm{\ψ}=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\≈{\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{\mathrm{\ψ}}^{-1}{G}_a\right)}^{-1}{G}_a^T{\mathrm{\ψ}}^{-1}h$

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

$h^{\′}=\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^{\′}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 1& 1\end{array}\right],$ $z_a^{\′}=\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^{\′}={\left(G_a^{\′T}{\mathrm{\ψ}}^{\′-1}{G}_a^{\′}\right)}^{-1}{G}_a^{\′T}{\mathrm{\ψ}}^{\′-1}{h}^{\′}$

$z_p=\sqrt{z_a^{\′}}+\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]$ 或 $z_p=-\sqrt{z_a^{\′}}+\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}{\mathrm{\τ}}_{\mathrm{LOS}}\left[t\right]& e_{\mathrm{NLOS}}\left[t\right]\end{array}\right]}^T,\mathrm{\Φ}=\left[\begin{array}{cc}1& 0\\ 0& \mathrm{\β}\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],$ 则：

$\mathrm{\β}=\arg \min \left\{\right|e_{\mathrm{NLOS}}^{\′}\left[t\right]-\mathrm{\βb}+\frac{{\mathrm{\β}}^{2}p}{{\mathrm{\β}}^{2}p+{\mathrm{\σ}}_{\mathrm{\ω}}^2}\left(y\right[t]-a-\mathrm{\βb})\left|\right\},$

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

Kalman滤波器的迭代过程为：

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

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

$\mathrm{\ϵ}\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]=\mathrm{\Φ}\hat{P}[t-1|t-1]{\mathrm{\Φ}}^T+\mathrm{\ΓQ}{\mathrm{\Γ}}^{-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，

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

$\mathrm{\ψ}=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\≈{\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{\mathrm{\ψ}}^{-1}{G}_a\right)}^{-1}{G}_a^T{\mathrm{\ψ}}^{-1}h$

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

$h^{\′}=\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^{\′}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 1& 1\end{array}\right],$ $z_a^{\′}=\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^{\′}={\left(G_a^{\′T}{\mathrm{\ψ}}^{\′-1}{G}_a^{\′}\right)}^{-1}{G}_a^{\′T}{\mathrm{\ψ}}^{\′-1}{h}^{\′}$

$z_p=\sqrt{z_a^{\′}}+\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]$ 或 $z_p=-\sqrt{z_a^{\′}}+\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}{\mathrm{\Σ}}}(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}}).$

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

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

$w_i^a=q_i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{q}_i$ 其中， $q_i=\frac{{3\mathrm{\α}}_{\min }^i}{\mathrm{\π}}.$

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

将锚节点三个一组分为个组，每一组根据步骤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}{\mathrm{\Σ}}}({\mathrm{SNR}}_{i1}+{\mathrm{SNR}}_{i2}+{\mathrm{SNR}}_{i3}).$

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

$x=\underset{i}{\mathrm{\Σ}}{w}_i{x}_i,y=\underset{i}{\mathrm{\Σ}}{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^{\′}=\frac{{\mathrm{\σ}}_{\mathrm{TDOA},x}}{{\mathrm{\σ}}_{\mathrm{TDOA},x}+{\mathrm{\σ}}_{\mathrm{TOA},x}}{x}_{\mathrm{TOA}}+\frac{{\mathrm{\σ}}_{\mathrm{TOA},x}}{{\mathrm{\σ}}_{\mathrm{TDOA},x}+{\mathrm{\σ}}_{\mathrm{TOA},x}}{x}_{\mathrm{TDOA}}$

$y^{\′}=\frac{{\mathrm{\σ}}_{\mathrm{TDOA},y}}{{\mathrm{\σ}}_{\mathrm{TDOA},y}+{\mathrm{\σ}}_{\mathrm{TOA},y}}{y}_{\mathrm{TOA}}+\frac{{\mathrm{\σ}}_{\mathrm{TOA},y}}{{\mathrm{\σ}}_{\mathrm{TDOA},y}+{\mathrm{\σ}}_{\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({\mathrm{\τ}}_i\right)=\left\{\begin{array}{cc}\frac{1}{{\mathrm{\τ}}_{i,\mathrm{rms}}}\exp (-\frac{{\mathrm{\τ}}_i}{{\mathrm{\τ}}_{i,\mathrm{rms}}})& {\mathrm{\τ}}_i>0\\ 0& {\mathrm{\τ}}_i\≤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所示，从图中可明显看出本发明的定位结果优于第三类方法。

Claims (7)

1.一种基于定位单元质量及多算法数据融合的抗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^{SNR},$

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

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

$x=\underset{i}{\Σ}{w}_i{x}_i,y=\underset{i}{\Σ}{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^{\′}=\frac{{\mathrm{\σ}}_{TDOA,x}}{{\mathrm{\σ}}_{TDOA,x}+{\mathrm{\σ}}_{TOA,x}}{x}_{TOA}+\frac{{\mathrm{\σ}}_{TOA,x}}{{\mathrm{\σ}}_{TDOA,x}+{\mathrm{\σ}}_{TOA,x}}{x}_{TDOA}$

$y^{\′}=\frac{{\mathrm{\σ}}_{TDOA,y}}{{\mathrm{\σ}}_{TDOA,y}+{\mathrm{\σ}}_{TOA,y}}{y}_{TOA}+\frac{{\mathrm{\σ}}_{TOA,y}}{{\mathrm{\σ}}_{TDOA,y}+{\mathrm{\σ}}_{TOA,y}}{y}_{TDOA}.$

2.如权利要求1所述的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，其特征在于，步骤4)中基于定位单元与待测终端的距离得到的权值通过下式计算得到：

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

$PL\left(d\right)=PL\left(d_0\right)+10nlog\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}{\Σ}}(\frac{1}{d_{i1}}+\frac{1}{d_{i2}}+\frac{1}{d_{i3}}).$

3.如权利要求1所述的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，其特征在于，步骤4)中基于定位单元三点共线度得到的权值通过下式计算得到：设第i个定位单元构成的三角形的最小角为则基于定位单元三点共线度的权值为：

其中，

4.如权利要求1所述的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，其特征在于，步骤4)中基于定位单元的可靠性得到的权值通过下式计算得到：

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

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

5.如权利要求1所述的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，其特征在于，步骤2)所述的Kalman滤波器由下式表示：

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

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

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

x[t]＝[τ_LOS[t] e_NLOS[t]]^T，H＝[1 0]，

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

假设在t时刻，得到t-1时刻的估计值估计误差的协方差矩阵则：

$\mathrm{\β}=\arg min\left\{\right|e_{NLOS}^{\′}\[t\]-\mathrm{\β}b+\frac{{\mathrm{\β}}^{2}p}{{\mathrm{\β}}^{2}p+{\mathrm{\σ}}_{\mathrm{\ω}}^2}(y\[t\]-a-\mathrm{\β}b)\left|\right\},$

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

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

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

$\tilde{x}\[t|t-1\]=\mathrm{\Φ}\hat{x}\[t-1|t-1\]$

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

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

$\tilde{P}\[t|t-1\]=\mathrm{\Φ}\hat{P}\[t-1|t-1\]{\mathrm{\Φ}}^T+{\mathrm{\ΓQ\Γ}}^{-1}$

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

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

6.如权利要求1所述的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，其特征在于，步骤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)。

7.如权利要求1所述的基于定位单元质量及多算法数据融合的抗NLoS误差定位方法，其特征在于，步骤3)所述的Chan算法包括以下步骤：

设待测终端的坐标为(x,y)，其中第q个锚节点的坐标为(x_q,y_q)(q＝1,2，…，N)，r_q为第q锚节点与待测终端的距离，r_j为第j锚节点与待测终端的距离，第j锚节点和第q锚节点与待测终端距离差为r_j,q＝r_j-r_q，

r_q ²＝(x_q-x)²+(y_q-y)²＝K_q-2x_qx-2y_qy+x²+y²

其中，将带入上式可得：

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

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

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

其中，x_j,q＝x_j-x_q，y_j,q＝y_j-y_q，

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

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

其中，其中，z_p＝[x y]^T，带有上标0表示真实值，则误差矩阵为：

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

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

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

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

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

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

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

$h^{\′}=\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^{\′}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 1& 1\end{array}\right],z_a^{\′}=\left[\begin{array}{c}{(x-x_1)}^2\\ {(y-y_1)}^2\end{array}\right]$

${\mathrm{\Ψ}}^{\′}=4B^{\′}\mathrm{cov}\left(z_a\right)B^{\′},B^{\′}=diag\{x^0-x_1,y^0-y_1,r_1^0\}$

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

或

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