CN114325577A

CN114325577A - 一种非视距定位误差修正方法与装置

Info

Publication number: CN114325577A
Application number: CN202210020888.XA
Authority: CN
Inventors: 樊瑞新; 杜歆
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2022-01-06
Filing date: 2022-01-06
Publication date: 2022-04-12

Abstract

本发明公开了一种非视距定位误差修正方法与装置。本发明首先利用移动节点与多个基站之间的测距值与预测值作差，得到残差；再根据残差为各测距值赋予权重，然后通过加权最小二乘法求解定位方程组，得到出移动节点的坐标；最后执行卡尔曼滤波，得到滤波后的移动节点坐标，以及下一轮的移动节点坐标预测值。该方法仅利用测距结果，不需要接收机记录信号的波形，可避免信号特征随环境改变而变化的问题，具有计算代价小、硬件要求低的特点，可应用于室内无线定位等场合。

Description

一种非视距定位误差修正方法与装置

技术领域

本发明涉及无线室内定位技术，尤其涉及一种用于无线室内定位的非视距误差修正方法与装置。

背景技术

室内无线定位系统由基站和移动节点组成。基站位置已知，当测量出移动节点到多个基站的距离，即可构造定位方程，求解得到移动节点的位置。而要测量移动节点到基站的距离，需要让两者相互发送信号，并记录下信号的发射时间和接收时间，进而估计出信号飞行时间(Time of Flight，TOF)，乘上电磁波的速度即得到距离。

然而在室内环境中，节点之间的信号传输路径可能被障碍物遮挡，信号以较低的速度穿过障碍物，或者经其他障碍物反射到达接收机。这被称为非视距(Non Line ofSight，NLOS)现象。NLOS会导致TOF的估计值偏大，进而使得距离的估计值偏大，严重影响定位性能，因此，对NLOS进行检测和修正能够有效提高定位精度。

目前的NLOS检测和修正方法主要基于接收信号的特征。在NLOS和视距(Line ofSight，LOS)条件下，接收信号在信号强度、信号持续时间等参数上有所不同，通过采集数据并构造分类器，即可对路径是否为NLOS做出判定。分类器还可进一步预测出NLOS造成的测距误差大小，从而实现误差的修正。

现在的NLOS检测和修正存在的主要问题有：(1)接收机需要准确记录信号波形，从而提取特征，这在低成本的定位装置中无法实现；(2)接收信号特征受环境和接收机本身特性影响较大，在场景改变时分类器可能失效。

发明内容

针对现有技术不足，本发明提出了一种非视距定位误差修正方法与装置。

为实现上述技术目的，本发明的技术方案为：一种非视距定位误差修正方法，该方法包含如下步骤：

S101：构造卡尔曼滤波器并初始化；

S102：通过卡尔曼滤波器进行状态预测，预测得到移动节点的坐标；

S103：由步骤S102得到的预测坐标，根据勾股定理计算出移动节点到各基站的距离预测值；

S104：测量移动节点到每个基站的距离，得到距离测量值；

S105：将步骤S104得到的距离测量值与步骤S103得到的距离预测值相减，得到距离残差；

S106：根据步骤S105得到的距离残差，计算得到各个测距值的权重；

S107：根据各基站的坐标以及步骤S104中的距离测量值构造定位方程组；

S108：使用步骤S106得到的各个测距值的权重为步骤S107构造的定位方程组赋权得到优化目标函数，并用加权最小二乘法求解该优化目标函数，得到移动节点坐标的测量值；

S109：将步骤S108得到的坐标测量值作为观测量，通过卡尔曼滤波器进行误差预测、添加卡尔曼增益、状态校正和误差校正，得到滤波后的状态向量；该状态量包含移动节点坐标，即作为修正后的移动节点定位结果。

进一步地，所述步骤S103中，已知N个基站坐标，第i个基站的坐标记为(x_a(i)，y_a(i))，由步骤S102给出的移动节点的预测坐标

根据勾股定理计算出移动节点到各基站的距离预测值，公式如下：

进一步地，所述步骤S104中优选飞行时间法测量移动节点到每个基站的距离。

进一步地，所述步骤S105中距离残差res(i)的计算公式如下：

其中，d_m(i)为步骤S104得到的距离测量值。

进一步地，所述步骤S106中，计算权重的公式如下：

其中，r为判定阈值，取视距条件下的测距误差标准差的2～3倍。

进一步地，当距离残差为正时，测距值偏大，判定为非视距并赋予一个小于1的权重；反之，判定为视距并赋予最高权重1；距离残差越大，非视距误差越大，则权重越低。

进一步地，所述定位方程组的公式如下：

其中，(x，y)是未知量，即移动节点的坐标。

进一步地，步骤S108中目标优化函数的公式如下：

进一步地，还包括：重复上述步骤S102-S109，对每轮的定位误差做出修正，得到更准确的定位结果。

本发明提出了一种非视距定位误差修正装置，包括一个或多个处理器，用于上述的非视距定位误差修正方法。

本发明提出了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，用于实现上述的非视距定位误差修正方法。

本发明的有益效果为：本发明方法仅利用移动节点和基站间的测距结果，不需要接收机记录信号的波形，可避免信号特征随环境改变而变化的问题，具有计算代价小、硬件要求低、应用范围广的特点，尤其适用于室内无线定位等场合。

附图说明

图1为定位系统示意图；

图2为本发明提出的非视距定位误差修正方法的流程图；

图3为本发明所用激活函数示意图；

图4为使用本发明提出的方法对定位误差修正结果的示意图；

图5为本发明装置的示意图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本发明的一些方面相一致的装置和方法的例子。

在本发明使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

下面结合附图和具体实施方式对本发明作进一步详细描述。在不冲突的情况下，下述的实施例及实施方式中的特征可以相互组合。

室内无线定位系统由基站和移动节点组成，如图1所示。基站(圆圈)位置已知，移动节点(三角形)在场地内运动。当测量出移动节点到多个基站的距离，即可构造定位方程，求解得到移动节点的位置。而要测量移动节点到基站的距离，需要让两者相互发送信号，并记录下信号的发射时间和接收时间，进而估计出信号飞行时间(Time of Flight，TOF)，乘上电磁波的速度即得到距离。在室内环境下，移动节点到基站的路径(虚线)可能被障碍物(黑色方块)遮挡，这被称为非视距(Non Line of Sight，NLOS)现象。NLOS会导致TOF的估计值偏大，进而使得距离的估计值偏大，严重影响定位性能。

最小二乘法常用于求解超定线性方程组。而加权最小二乘法在最小二乘法的基础上，为每个方程赋予一个权重。在定位系统中，当移动节点估计出到多个基站的距离后，即可构造出一个方程组，用加权最小二乘法求出移动节点坐标，而权重可根据测距的误差来确定。

卡尔曼滤波用来解决离散时间线性系统的状态估算。随着计算机技术的迅速发展和对卡尔曼滤波器的深入研究，卡尔曼滤波技术现在已经广泛应用于各个领域内的信号于数据处理中。卡尔曼滤波器由下列五个方程表示：

状态预测：

误差预测：

卡尔曼增益：

状态校正：

误差校正：

其中，下标k表示当前计算结果，下标(k-1)表示上一轮计算结果；X为状态向量，

和

分别表示其上一轮的估计值、本轮预测值和校正后的值。P为均方误差矩阵，P_k-1、

和P_k分别表示其上一轮的估计值、本轮预测值和校正后的值。A为状态转移矩阵；Q为过程噪声矩阵；R为测量噪声矩阵；C为测量关系矩阵；K_k为卡尔曼增益；Y_k为测量向量。

加权最小二乘法和卡尔曼滤波器，目前已经在无线定位系统中普遍使用。当测距误差为高斯分布的随机变量，使用加权最小二乘法和卡尔曼滤波器可以取得最优的定位效果。但当存在NLOS路径时，使用上述方法无法消除由NLOS造成的误差。

本发明提出了一种基于残差以及加权最小二乘法的非视距定位误差修正方法，图2为本发明的方法流程图，具体包括以下步骤：

步骤S101中，构造一个卡尔曼滤波器，用于对移动节点的位置做滤波。其中，状态向量为X_k＝[x_k y_k v_xk v_yk]^T，其中x_k和y_k代表移动节点的x和y坐标，v_xk和v_yk代表x和y方向的速度。测量关系矩阵C＝[I O]，状态转移矩阵

其中，I为对角矩阵，Δt为两次定位的时间间隔。过程噪声矩阵

其中a为加速度估计值；测量噪声矩阵

其中

和

为LOS情况下定位结果的方差。X_k的初始值取X₀＝[x₀ y₀ 0 0]^T，其中(x₀，y₀)为使用最小二乘法得到的初始定位结果；均方误差矩阵P的初始值取

步骤S102中，通过卡尔曼滤波器进行状态预测，它的作用是根据上一轮得到的移动节点状态向量

预测得到本轮移动节点的状态

其前两项即移动节点的预测坐标

步骤S103中，N个基站坐标已知，第i个基站的坐标记为(x_a(i)，y_a(i))。利用步骤102的预测坐标，根据勾股定理计算出移动节点到各基站的距离预测值

公式如下：

步骤S104中，基于飞行时间法(Time of Flight，TOF)测量移动节点到每个基站的距离，得到距离测量值d_m(i)，i＝1，2，...N。

步骤S105中，将步骤S104得到的距离测量值，与步骤S103得到的距离预测值相减，得到距离残差res(i)：

步骤S106中，根据步骤S105得到的距离残差res(i)，利用下面的激活函数计算得到各个测距值的权重w(i)：

其中，r为判定阈值。该激活函数的作用是：当距离残差为正且较大，说明本次测距值偏大，因此判定为NLOS并赋予一个小于1的权重，且残差值越大，说明NLOS误差越大，则权重越低；反之，判定为LOS并赋予最高权重1。r可以取视距条件(Line of Sight，LOS)下的测距误差标准差σ的2～3倍，根据正态分布规律，在LOS条件下95％以上的情况下测距误差都会处在(-2σ，+2σ)范围内，从而权重为1。

步骤S107中，根据各基站的坐标以及步骤S104中的距离测量值，构造出定位方程组，公式如下：

其中，(x，y)是未知量，即移动节点的坐标。

步骤S108中，使用步骤S106得到的各个测距值的权重，为步骤S107构造的定位方程组赋权得到优化目标函数，其中第i个方程赋予权重w(i)，得到如下后的优化目标函数如下：

再用最小二乘法求解该优化目标函数，得到移动节点坐标的测量值(x_m，y_m)。

步骤S109中，将步骤S108得到的坐标测量值作为观测量，通过卡尔曼滤波器进行误差预测、添加卡尔曼增益、状态校正和误差校正，其中，状态校正中的向量Y_k＝[x_m y_m]^T，即步骤S107得到的坐标测量值构成向量。计算得到滤波后的状态向量

其前两项

表示移动节点坐标，作为修正后的移动节点定位结果。

步骤S110中，，即重复上述步骤S102-S109，即可对每轮的定位误差做出修正，得到较为准确的定位结果。

实施例1

本发明方法经实验证明能有效修正非视距造成的定位误差。实验场景为超宽带室内定位。如图1所示，4个基站组成一个长9.45m，宽5.40m的矩形，移动节点沿y＝1.20m的直线向x轴正方向移动。障碍物摆放在坐标(5m，1.8m)处。根据超宽带测距实验结果，LOS条件下的测距误差的标准差为σ＝0.02m。因此，激活函数公式(4)中参数取值为：r＝2σ＝0.04m，函数图像如图3所示。卡尔曼滤波参数为Δt＝0.1，a＝1m/s²，

实验结果如图4所示。受到障碍物的遮挡，在6m处的一段区间内，修正前的定位结果明显偏离真实值。而使用本发明提出的修正方法，定位结果与真实值更为接近。从数值上看，在不进行修正的情况下，NLOS造成的定位误差为0.30m，在使用本发明提出的修正方法，定位误差减小到0.05m。

与前述非视距定位误差修正方法的实施例相对应，本发明还提供了非视距定位误差修正装置的实施例。

参见图5，本发明实施例提供的一种非视距定位误差修正装置，包括一个或多个处理器，用于实现上述实施例中的非视距定位误差修正方法。

本发明非视距定位误差修正装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图5所示，为本发明非视距定位误差修正装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图5所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。

上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的非视距定位误差修正方法。

所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是任意具备数据处理能力的设备，例如所述设备上配备的插接式硬盘、智能存储卡(Smart Media Card，SMC)、SD卡、闪存卡(Flash Card)等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。

以上仅为本发明具体实施方式，不能以此来限定本发明的范围，本技术领域内的一般技术人员根据本创作所作的均等变化，以及本领域内技术人员熟知的改变，都应仍属本发明涵盖的范围。