CN110850363B - 一种基于实时定位轨迹数据进行动态滤波优化的方法 - Google Patents

Abstract

本发明提供了一种基于实时定位轨迹数据进行动态滤波优化的方法，包括：步骤1，读入实时解算出的定位坐标数据；步骤2，读取历史解算出的定位坐标数据；步骤3，若历史解算数据少于4条，则转至步骤4，否则转至步骤5；步骤4，将新解算出的数据和已解算的最多偶数个历史数据融合形成长度为奇数的滑动窗口数据；步骤5，将新解算的数据和最近解算出的四条数据进行融合，形成滑动窗口数据；步骤6，对获得的窗口数据进行平滑处理；步骤7，滤波过后将新解算的数据加入历史数据集，将窗口内平滑处理过后的历史数据更新到历史数据集；步骤8，清空窗口数据，进行下一步的定位数据解算，解算完成后，转至步骤1，读取下一条数据。

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-Series Estimation[J].IEEE Transactions on Aerospace&Electronic Systems,2007,AES-12(2):187-194.

发明内容

发明目的：现有基于TOF和TDOA等数据的经典定位算法在真实环境中由于种种干扰性能显著下降，尤其是实时追踪过程中无法形成平滑连续轨迹的问题，在平滑滤波过程中，依据时序信息和轨迹坐标的先验知识，在保证实时性和高效性的前提下，增强轨迹的平滑性和连续性。

为了解决上述技术问题，本发明公开了一种复杂环境下基于实时定位轨迹数据进行动态滤波优化的方法，该方法可以用于仓库管理、定位导航、机器人追踪、港口实时定位跟踪等应用中，包括以下步骤：

步骤1，读取一条实时解算的定位数据；

步骤2，读取历史解算坐标数据；

步骤3，如果历史解算坐标数据少于4条，则转至步骤4，否则转至步骤5；

步骤4，将新解算出的数据和已解算的最多偶数个历史数据融合形成长度为奇数的滑动窗口数据；

步骤5，将新解算的数据和最近解算出的四条数据进行融合形成滑动窗口数据，转至步骤6；

步骤6，利用高斯滤波器对获得的滑动窗口数据进行平滑处理；

步骤7，滤波过后，将从步骤1中得到的数据加入历史数据集，同时将滑动窗口内平滑处理过后的历史数据更新到历史数据集；

步骤8，清空滑动窗口数据，进行下一步的定位数据解算，解算完成后，转至步骤1，读取下一条数据，数据窗口向后移。

步骤1中，读入的实时解算的定位数据data的形式为：

data＝((x₁，y₁)，(x₂，y₂)，...，(x_n，y_n))，

其中n表示数据集中历史数据的个数，x_i，y_i分别表示第i个解算出的定位数据的对应的横坐标、纵坐标，1＜＝i＜＝n。

步骤2中的读取历史数据，即将之前的解算结果读取进来，为了保证滤波的低延迟，最多仅需读入四条最近解算数据即可，其数据形式为：

Window＝((x_i，y_i)，(x_i+1，y_i+1)，(x_i+2，y_i+2)，(x_i+3，y_i+3)，(x_i+4，y_i+4))，

x_i，y_i分别表示第i个数据对应的横坐标、纵坐标，其中window的前四个数据为历史数据，最后一个数据为新解算的结果。

步骤3中，根据轨迹坐标的先验知识，对已解算出的数据个数做判断，确定是否有足够的历史数据可以来做滤波优化，如果历史时序信息不足，则需要借助已解算出的信息进行辅助滤波：

如果span≤Ntemp_after，则判定有足够的历史数据来做滤波优化；

其中，span为预先设置的窗口长度，Ntemp_after为当前已解算出的数据个数。根据窗口长度和已有历史数据个数的关系，进行自适应的窗口调整。

步骤4中，所需的历史解算数据有可能不足4条，基于此，为了保证滤波的效果，需要将之前的最多偶数条数据与新数据融合成长度为奇数的窗口进行滤波，否则不进行相应优化。基于此，需要对以下两个条件做出判断，如果所需的窗口数据个数多于已解算出的数据个数且并且当前解算出的数据个数为奇数，那么将对所有已解算的数据进行高斯滤波操作，滤波完成后，在Fn中更新对应的数据，其中Fn为已解算出的数据的集合。其中最多偶数条的个数不会大于4，算法的时效性得以保证。

步骤5中，如果历史解算的数据足够窗口长度的要求，为了保证时效性，那么仅需要选取最近邻的四条数据和当前数据组合起来进行滤波优化，首先进行条件判断，检查所需的窗口数据长度span是否小于等于已解算的数据个数Ntemp_after，如果满足该条件，则将新解算的数据和最近解算出的四条数据进行融合形成长度为5的滑动窗口数据，然后对这个窗口进行高斯滤波操作，然后对这个窗口进行高斯滤波操作，本发明所用的高斯滤波的算法基于一维数据，具体使用如下正态分布的概率密度函数：

其中σ、μ分别为正态分布的标准差和均值，在本发明中，将采用均值为0，标准差为1的标准正态分布，即σ＝1、μ＝0。由上述公式生成相应的高斯模板，因为采用的窗口长度为5，所以需要生成长度为5，标准差为1的模板，即生成高斯模板h＝[f(-2),f(-1),f(0),f(1),f(2)]，其中函数f(x)为正态分布概率密度函数。

为了实现函数中自适应窗口大小，高斯模板的生成由窗口大小span指定，即将函数中的变量x，以一维向量的形式代入正态分布的概率密度公式中，所述一维向量为：

(-span/2,-span/2+1,…,0,1,…,span/2),

以窗口长度span＝5举例，该向量就为(-2,-1,0,1,2)。由此生成向量形式的高斯模板，接下来需要对该模板做归一化，令高斯模板为h，归一化操作表示为：

其中h_i为最初计算出的高斯模板中的第i个数据。由此生成了归一化后长度为span的高斯模板。设用到的窗口数据为window，

window＝((x₁，y₁)，(x₂，y₂)，(x₃，y₃)，(x₄，y₄)，(x₅，y₅))，

进行滤波时，其实是对中间数据进行过滤和平滑，在本发明中，即为(x₃，y₃)，通过高斯模板更新该数据，具体计算方法为：

(x₃′，y₃′)＝h*window^T，

其中，x₃′，y₃′为高斯滤波后计算出的第三个窗口数据的平滑后的结果，h为前述计算的高斯模板，window^T是对窗口数据window作转置的结果，通过该方法会更新中间点的信息，由此每次将窗口向后移动一位，进而可以做到对所有数据点的平滑操作。

每次滤波完成后，在Fn中更新对应的窗口数据。

步骤6中，对之前步骤中得到的窗口数据进行高斯滤波操作后。原有的window向量经平滑处理后得到新的向量window’：

window’＝((x_i’，y_i’)，(x_i+1’，y_i+1’)，(x_i+2’，y_i+2’)，(x_i+3’，y_i+3’)，(x_i+4’，y_i+4’))

其中x_i’，y_i’分别为平滑处理后得到的第i个解算数据的横坐标、纵坐标。

步骤7中，将过滤得到的新的窗口数据更新到结算结果中，其中最新解算的结果已经依据时序信息经过了一次滤波处理，比起直接的解算结果，该处理使得轨迹更加平滑合理，将解算结果中可能出现的噪声干扰进行了剔除。同时将滤波过后将新解算的数据加入历史数据集，窗口内平滑处理过后的历史数据更新到历史数据集。

步骤8中，将滤波所需的窗口清空，为下一个解算数据的平滑处理做准备，窗口向后滑动，由此进行下一次的解算，转到步骤1。

有益效果：本发明的显著优点是对实时定位数据进行后处理平滑滤波优化时，利用滑动窗口的思想，自适应地组织历史数据集做滤波，而且仅需极少的先验坐标数据知识，即可达到很好的平滑效果。同时每次进行滤波操作时，只需最近的几个点的坐标数据，算法延迟很低，在4HZ的定位数据采样设备下，延迟最大仅为500ms，从而优化了整个定位系统的性能。适合处理人行走过程中实时轨迹追踪，能够保证轨迹的连续性和光滑性，显著改善定位系统的性能和效果。

附图说明

下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。

图1为本发明的整体流程图。

图2为本发明中根据先验坐标数据的判断是否需要自适应地调整窗口的大小。

图3a是原始坐标数据图像，图3b是根据原始坐标进行平滑处理后的结果。

图4a是原始坐标数据图像，图4b是根据原始坐标进行平滑处理后的结果。

具体实施方式

图1是本发明识别障碍物的流程图，包括8个步骤。

第一个步骤中，读入的定位数据的形式为：

data＝((x₁，y₁)，(x₂，y₂)，...，(x_n，y_n))，

其中n表示数据集中历史数据的个数，x_i，y_i分别表示第i个解算出的定位数据的对应的横坐标、纵坐标(1＜＝i＜＝n)。

第二个步骤中，读取历史数据，即将之前的解算结果读取进来，为了保证滤波的低延迟，最多仅需读入四条最近解算数据即可，其数据形式为：

window＝((x_i，y_i)，(x_i+1，y_i+1)，(x_i+2，y_i+2)，(x_i+3，y_i+3)，(x_i+4，y_i+4))，

x_i，y_i为第i个坐标对应的横坐标、纵坐标，其中window的前四个数据为历史数据，最后一个数据为新解算的结果。

第三个步骤中，根据轨迹坐标的先验知识，需要对已解算出的数据个数做判断，确定是否有足够的历史数据可以来做滤波优化，如果历史时序信息不足，则需要借助已解算出的信息进行辅助滤波。如果span≤Ntemp_after，则判定有足够的历史数据来做滤波优化；

其中，span为预先设置的窗口长度，Ntemp_after为当前已解算出的数据个数。

第四个步骤中，所需的历史解算数据有可能不足4条，基于此，为了保证滤波的效果，需要将之前的最多偶数条数据与新数据融合成长度为奇数的窗口进行滤波，否则不进行相应优化。因此需要对以下两个条件做出判断，如果所需的窗口数据个数多于已解算出的数据个数且并且当前解算出的数据个数为奇数，那么将对所有已解算的数据进行高斯滤波操作，滤波完成后，在Fn中更新对应的数据。

第五个步骤中，如果之前解算的数据足够满足窗口长度的要求，那么需要选取最近邻的四条历史坐标数据和当前数据组合起来进行滤波优化，首先需要进行条件判断，检查所需的窗口数据长度span是否小于等于已解算的数据个数Ntemp_after，如果满足该条件，则将新解算的数据和最近解算出的四条数据进行融合形成长度为5的滑动窗口数据，然后对这个窗口进行高斯滤波操作，然后对这个窗口进行高斯滤波操作，本发明所用的高斯滤波的算法基于一维数据，借助正态分布的概率密度函数：

其中σ、μ分别为正态分布的标准差和均值，在本发明中，将采用均值为0，标准差为1的标准正态分布，即σ＝1、μ＝0。由该公式生成相应的高斯模板，因为采用的窗口长度为5，所以需要生成长度为5，标准差为1的模板，即生成h＝[f(-2)，f(-1)，f(0)，f(1)，f(2)]，其中函数f(x)为前文说明的正态分布概率密度函数。为了实现函数中自适应窗口大小，高斯模板的生成由窗口大小span指定，即将函数中的变量x，以一维向量的形式代入，该一维向量为：

(-span/2，-span/2+1，…，0，1，…，span/2)，

以窗口长度span＝5举例，该向量就为(-2，-1，0，1，2)。由此生成向量形式的高斯模板，接下来需要对该模板做归一化，令高斯模板为h，归一化操作可表示为：

其中h_i为最初计算出的高斯模板中的第i个数据。由此生成了归一化后长度为span的高斯模板。设用到的窗口数据为window，

window＝((x₁，y₁)，(x₂，y₂)，(x₃，y₃)，(x₄，y₄)，(x₅，y₅))，

进行滤波时，其实是对中间数据进行过滤和平滑，在本发明中，即为(x₃，y₃)，通过高斯模板更新该数据，具体计算方法为：

(x₃′，y₃′)＝h*window^T，

其中，x₃′，y₃′为高斯滤波后计算出的第三个窗口数据的平滑后的结果，h为前述计算的高斯模板，window^T是对窗口数据window作转置的结果，通过该方法会更新中间点的信息，由此每次将窗口向后移动一位，进而可以做到对所有数据点的平滑操作。

每次滤波完成后，在Fn中更新对应的窗口数据。

步骤三、四、五历史数据窗口的自适应调整的流程图见图2。

第六个步骤中，对之前步骤中得到的窗口数据进行高斯滤波操作。原有的window向量经过平滑处理后得到：

window’＝((x_i’，y_i’)，(x_i+1’，y_i+1’)，(x_i+2’，y_i+2’)，(x_i+3’，y_i+3’)，(x_i+4’，y_i+4’))

其中x_i’，y_i’分别表示平滑处理后得到的第i个解算数据的横坐标、纵坐标。

第七个步骤中，将过滤得到的新的窗口数据更新到结算结果中，其中最新解算的结果已经依据时序信息经过了一次滤波处理，比起直接的解算结果，该处理使得轨迹更加平滑合理，将解算结果中可能出现的噪声干扰进行了剔除。同时将滤波过后将新解算的数据加入历史数据集，窗口内平滑处理过后的历史数据更新到历史数据集。

第八个步骤中，将滤波所需的窗口清空，为下一个解算数据的平滑处理做准备，窗口向后滑动，由此进行下一次的解算，转到步骤1。

实施例

为了验证本发发明的有效性，在实际环境部署场地并进行测试。测试场地为一个5m*7m左右的房间，房间左上角有一块玻璃，附近的信号会发生明显的反射。场地四周共部署了8个基站，基站的高度大概在3m左右。测试人员在场地内沿着边缘行走数周，运动轨迹接近于矩形。将此次采集的TDOA和TOF数据进行解算得到的坐标数据作为测试数据在本发明中进行平滑处理，其中各步骤的实现及参数细节如下：

步骤1，读入的定位坐标数据；

步骤2中，读取历史数据，即将之前的解算结果读取进来，为了保证滤波的低延迟，最多仅需读入四条最近解算数据即可；

步骤3中，根据轨迹坐标的先验知识，需要对已解算出的数据个数做判断，确定是否有足够的历史数据可以来做滤波优化，如果历史时序信息不足，则需要借助已解算出的信息进行辅助滤波；

步骤4中，所需的历史解算数据有可能不足4条，基于此，为了保证滤波的效果，需要将之前的最多偶数条数据与新数据融合成长度为奇数的窗口进行滤波，否则不进行相应优化；

步骤5中，如果之前解算的数据足够满足窗口长度的要求，那么需要选取最近邻的四条历史坐标数据和当前数据组合起来进行滤波优化；

步骤6中，对之前步骤中得到的窗口数据进行高斯滤波操作。原有的window向量经过平滑处理后得到优化过后的窗口向量。

步骤7中，将过滤得到的新的窗口数据更新到结算结果中，其中最新解算的结果已经依据时序信息经过了一次滤波处理，比起直接的解算结果，该处理使得轨迹更加平滑合理，将解算结果中可能出现的噪声干扰进行了剔除。同时将滤波过后将新解算的数据加入历史数据集，窗口内平滑处理过后的历史数据更新到历史数据集；

步骤8中，将滤波所需的窗口清空，为下一个解算数据的平滑处理做准备，窗口向后滑动，由此进行下一次的解算，转到步骤1。

在图3a和图3b中，展示了对于一组坐标数据进行滤波过后的中间结果对比图，其中，图3a是原始坐标数据图像，图3b是根据原始坐标进行平滑处理后的结果。

在图4a和图4b中，展示的是对于另一组不同轨迹下的坐标数据进行滤波过后的中间结果对比图，其中，图4a是原始坐标数据图像，图4b是根据原始坐标进行平滑处理后的结果。

经过以上在数据上的验证可以表明，本发明的在轨迹上获得的平滑性和时效性都表现出了令人满意的结果。

本发明提供了一种基于实时定位轨迹数据进行动态滤波优化的方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (2)

1.一种基于实时定位轨迹数据进行动态滤波优化的方法，其特征在于，包括如下步骤：

步骤1，读取一条实时解算的定位数据；

步骤2，读取历史解算坐标数据；

步骤3，如果历史解算坐标数据少于span-1条，则转至步骤4，否则转至步骤5；

步骤4，将新解算出的数据和已解算的最多偶数个历史数据融合形成长度为奇数的滑动窗口数据；

步骤5，将新解算的数据和最近解算出的四条数据进行融合形成滑动窗口数据，转至步骤6；

步骤6，利用高斯滤波器对获得的滑动窗口数据进行平滑处理；

步骤7，滤波过后，将从步骤1中得到的数据加入历史数据集，同时将滑动窗口内平滑处理过后的历史数据更新到历史数据集；

步骤8，清空滑动窗口数据，进行下一步的定位数据解算，解算完成后，转至步骤1，读取下一条数据，数据窗口向后移动；

步骤1中，读入的实时解算的定位数据data的形式为：

data＝((x₁，y₁)，(x₂，y₂)，...，(x_n，y_n))，

其中n表示历史数据的个数，x_i，y_i分别表示第i个解算出的定位数据对应的横坐标、纵坐标，1＜＝i＜＝n；

步骤2中的读取历史数据，即将之前的解算结果以向量window的形式读取进来，向量window形式为：

window＝((x_i，y_i)，(x_i+1，y_i+1)，(x_i+2，y_i+2)，(x_i+3，y_i+3)，(x_i+4，y_i+4))，

x_i，y_i分别表示第i个解算出的定位数据的对应的横坐标、纵坐标，其中window的前四个数据为历史数据，最后一个数据为新解算的结果；

步骤3中，对已解算出的历史数据个数做判断，确定是否有足够的历史数据来做滤波优化：

如果span≤Ntemp_after，则判定有足够的历史数据来做滤波优化；

其中，span为预先设置的窗口长度，Ntemp_after为当前已解算出的数据个数；

步骤4中，需要对以下两个条件做出判断：如果所需的窗口数据个数多于已解算出的数据个数，并且当前解算出的数据个数为奇数，则将对所有已解算的数据进行高斯滤波操作，滤波完成后，在Fn中更新对应的数据，Fn为已解算出的数据的集合；

步骤5中，将新解算的数据和最近解算出的四条数据进行融合形成长度为5的滑动窗口数据，然后对这个窗口进行高斯滤波操作，具体使用如下正态分布的概率密度函数：

其中σ、μ分别为正态分布的标准差和均值，由上述公式生成相应的高斯模板，因为采用的窗口长度为5，所以需要生成长度为5，标准差为1的模板，即生成高斯模板h＝[f(-2)，f(-1)，f(0)，f(1)，f(2)]，其中函数f(x)为正态分布概率密度函数；

为了实现函数中自适应窗口大小，高斯模板的生成由窗口大小span指定，即将函数中的变量x，以一维向量的形式代入正态分布的概率密度公式中，所述一维向量为：

(-span/2，-span/2+1，…，0，1，…，span/2)，

对高斯模板h做归一化，归一化操作表示为：

其中h_i为最初计算出的高斯模板中的第i个数据，由此生成了归一化后长度为span的高斯模板；

设窗口数据window表示为：

window＝((x₁，y₁)，(x₂，y₂)，(x₃，y₃)，(x₄，y₄)，(x₅，y₅))，

进行滤波时，是对中间数据进行过滤和平滑，即对第三个窗口数据(x₃，y₃)，通过高斯模板更新该数据，具体计算方法为：

(x₃′，y₃′)＝h*window^T，

其中，x₃′，y₃′为高斯滤波后计算出的第三个窗口数据的平滑后的结果，window^T是对窗口数据window作转置的结果；

每次滤波完成后，在Fn中更新对应的窗口数据。

2.根据权利要求1所述的方法，其特征在于，步骤6中，对原有的window向量经平滑处理后得到新的向量window’：

window’＝((x_i’，y_i’)，(x_i+1’，y_i+1’)，(x_i+2’，y_i+2’)，(x_i+3’，y_i+3’)，(x_i+4’，y_i+4’))

其中x_i’，y_i’分别为平滑处理后得到的第i个解算数据的横坐标、纵坐标。

Images