一种鲁棒式低功耗TDOA定位方法及鲁棒式中值过滤器

摘要

本发明属于协同定位数据处理技术领域，公开了一种鲁棒式低功耗TDOA定位方法及鲁棒式中值过滤器，对某个未知节点的多个邻居锚节点进行组合，使用三边定位算法得到多个候选位置组成的候选点集，并对所述多个候选位置按照X,Y,Z轴的值升序进行排列；从候选点集中将偏离较远的候选位置删除，对剩余的候选点求均值得到最终定位结果。本发明通过提出一种鲁棒式的两步加权最小二乘定位算法，避免实际测量中，信号传播偏离，通信错误所造成的定位系统的错误，从而影响其他功能的正常运行，提高整个系统的鲁棒性和可应用性；实验结果表明，本发明提出的算法其平均误差，定位错误点数量都要优于原始两步加权最小二乘定位算法。

说明书

技术领域

本发明属于协同定位数据处理技术领域，尤其涉及一种鲁棒式低功耗TDOA定位方法及鲁棒式中值过滤器。

背景技术

目前，协同定位技术在路径规划，交通运输，仓库存储、执法、人员安全、车联网、移动互联网等应用中占有重要的地位，尤其是在一些GNSS信号无法到达，或者需要高精度定位的室内任务应用场景中，协作式定位成为了这些基于位置信息应用场景及其系统的基础，并且定位的精度与稳定性决定了整个系统的健壮性和可用性。

目前协同定位模式主要分为两种，即无需测距(Range-Free)定位方式与基于测距(Range-Based)的定位方式。无需测距定位算法是指在定位过程中无需距离和角度信息，仅根据网络连通性等信息实现，常用于无线传感网络(WSN)中，代表性定位算法有DV-HOP，质心定位算法(Centroid LocalizationAlgorithm,CLA)，近似三角形内点测试法(Approximate Point-In-Triangulation Test,APIT)等。基于测距(Range-Based)技术的定位算法，通过测量节点间点到点的距离(TOA、TDOA、RSSI等)或者角度信息(AOA)，使用三角测量法、最大似然估计法、半定规划法来计算节点位置。

基于测距的协作定位系统主要有两部分组成，首先是测距技术，测距误差是整个定位误差产生的根本，其硬件特征以及使用环境都决定了测距精确度；其次定位算法与优化在定位系统中也占有一定的重要性，低算法复杂度，具有快速收敛的优化算法，复杂环境下的算法鲁棒性都是协作定位算法中需要考虑的方面，也是设计出符合硬件使用的算法的基本原则。

测距技术：

目前主要测距技术有红外线测距，WIFI与蓝牙信号，ZigBee技术，UWB技术等。红外线(IrDA)室内测距虽然有相对较高的精度，但仅能视距传输和距离较短两大缺陷使其局限性较大，甚至将标签放入口袋内以及任何一点遮挡就不能正常工作(例如传统电视遥控器)。蓝牙(Bluetooth)信号测距技术与WIFI信号测距技术是一种短距离低功耗的无线传输技术，主要通过接收信号强度(RSSI)进行小范围定位。设备体积小、易集成、高速率、低功耗，且传输不受非视距影响，但在复杂的空间环境中受噪声等信号干扰严重，稳定性差，精度至多只有米级。ZigBee是一种与蓝牙类似的新兴短距离、低速率无线网络技术，介于射频识别和蓝牙之间，通过一个己知位置的参考节点和数千个只需要很少的能量的微小传感器之间相互协调通信以实现全部定位。低功耗、低成本、自组织、短时延、高容量、工作效率高，但受多径效应和移动的影响较大。超宽带(Ultra-Wideband,UWB)精准实时定位系统(Real-Time Locating Systems,RTLS)采用了UWB无载波脉冲通信技术，与其他定位技术有着极大的差异。

目前的定位技术如Bluetooth,ZigBee,Wi-Fi,RFID等大多是根据接收信号强弱(RSSI)来判断物体位置，易受干扰影响，误差大、精度低。UWB RTLS系统利用纳秒至微秒级的非正弦波窄脉冲传输数据，与其他窄带系统相比，是一种新的共享频谱使用方式，在10米左右的范围内可实现数百Mbit/s甚至数Gbit/s的数据传输速率，系统简单、成本低、兼容性强，适用于各种需求。

同时，频谱带宽大、功耗低、对信道衰落不敏感、穿透性强、抗多径干扰效果好、低截获能力(安全性高)等特点也使得UWB在无线通信及室内精准实时定位领域占据一席之地，成为无线室内定位技术的新宠。不同技术方案总结如表1所示。

表1不同测距技术总结

定位技术：

目前主流基于测距的定位技术包括集中式和非集中式。经典的MDS-MAP算法是一种集中式定位算法，易于实现且计算复杂度较低。马震在文献中提出了MDS-MAP(D)算法，以网络分簇为基础，利用网络的连接关系，在不需要高精度测距技术支持的条件下对节点坐标进行估计的算法，分析表明该算法有效降低了计算复杂度。Minhan Shon在文献中针对非规则型集群网络的定位问题提出了基于分簇的CMDS算法。但即使对无线定位网络进行分簇，依旧需要集中式的相对坐标转绝对坐标的坐标配准过程。

非集中式定位技术主要包括：到达时间(Time of Arrival，TOA)，到达时间差(Time Difference ofArrival，TDOA)，到达频率差(Frequency ofArrival，FDOA)，到达角(Angle ofArrival，AOA)和到达增益比(Gain Ratio ofArrival，GROA)等。AOA方法是一种基于角度技术的定位方法。它允许节点通过为每个节点设置天线阵列来估计邻居之间的相对方向，并使用三角测量方法来获得节点的精确坐标。RSSI方法应用已知的数学模型来描述距离的路径损耗衰减。接收节点测量接收的射频信号的强度，该信号与原始信号进行比较，以获得通信路径的传播损耗，通过使用经验模型和理论公式将该传播损耗转换为两个节点之间的距离，最后可以使用经典几何关系或最大似然法来确定节点的位置。

在TOA方法中，可以利用从发送节点到接收节点测量射频信号的传播时间直接估计节点之间的距离，并配合使用三边测量法或最大似然算法来计算节点的位置信息，该方法需要所有节点之间时间同步，否则会造成误差。TDOA方法不需要知道信号的传输时间，该方法原理是信号在不同时间到达接收节点，节点之间的距离可以通过信号到达时间差来获得。

与TOA定位算法类似，TDOA方法也采用三边测量法、最大似然法算法或者两步加权最小二乘法来获得节点的精确坐标。常用的基于测距的定位技术特征如表2所示。

表2基于测距的定位技术特征总结

通过上述分析，现有技术存在的问题及缺陷为：

(1)现有技术中，通过单次三边定位不能减少测量环境中信号干扰的随机性对定位结果造成的大误差。

(2)现有技术实际测量中，信号传播偏离，通信错误易造成的定位系统的错误，从而影响其他功能的正常运行，应用性受限。

解决以上问题及缺陷的难度为：由于室内定位系统往往需要集成到小体积硬件中。由于电池容量的限制，这些小型设备上通常搭载低功耗低成本处理设备，而信号误差或者错误的抑制往往需要迭代优化，这样计算所带来的延迟也会导致定位结果不准确。闭式解算法可以在有限的循环次数内对位置进行求解，但其算法难度在于单次求解不具有鲁棒性，对大误差或者错误数据无法进行排除。

解决以上问题及缺陷的意义为：很多室内自动化设备，例如小型无人车，无人机和机器人等，需要位置信息作目标跟踪，路径规划等相关任务。定位算法的鲁棒性和收敛性影响着相关任务的进行。同时这些自动化设备又无法承担大量的迭代优化的计算开销。闭式解算法不受收敛问题影响，解决这种类型算法的鲁棒性问题能够更好的使用在低成本低功耗设备中，用于各种自动化设备。

本发明主要对基于测距的非集中式协同定位系统进行研究，针对DWM1000模块其功能特征，在基于测距TDOA定位技术以及两步加权最小二乘法定位求解算法基础之上，针对线性化处理技术中应用环境容易造成定位算法产生较大误差的问题，提出一种具有鲁棒式定位算法，提高TDOA定位技术在实际使用中的可应用性。

发明内容

针对现有技术存在的问题，本发明提供了一种鲁棒式低功耗TDOA定位方法及鲁棒式中值过滤器。

本发明是这样实现的，一种鲁棒式低功耗TDOA定位方法，应用于终端，所述鲁棒式低功耗TDOA定位方法包括：

对某个未知节点的多个邻居锚节点进行组合，根据TDOA测量值使用两步加权最小二乘定位算法，将多个候锚节点的组成的定位结果解出，构建位置解算候选点集。

分别计算候选节点集的中位数，带入到中值过滤器中计算位置组合后的定位结果偏离值，从候选点集中将偏离较远的候选位置删除，对剩余的候选点求均值得到最终定位结果。

进一步，所述组合后三边定位得到的候选点集为：

进一步，所述三边定位算法包括：

输入：邻居节点neighbors，组合数量comb_num，定位锚节点数量anchor_num

输出：定位源位置估计值数组；

步骤1，随机在邻居节点neighbors集合中选取anchor_num个节点；

步骤2，根据锚节点选择策略，得到锚节点选取约束，对随机选取的锚节点位置进行约束判断；

步骤3，循环步骤1与步骤2，直到选取满足组合数量comb_num个锚节点对；

步骤4，利用上述两步加权最小二乘法，计算位置估计值；

步骤5，返回所有位置估计值集合。

进一步，所述步骤2锚节点选择策略中，候选点集中的点服从正态分布；

其中

由于

本发明另一目的在于提供一种实施所述鲁棒式低功耗TDOA定位方法的鲁棒式中值过滤器。

本发明另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：

对某个未知节点的多个邻居锚节点进行组合，使用三边定位算法得到多个候选位置组成的候选点集，并对所述多个候选位置按照X,Y,Z轴的值升序进行排列；

从候选点集中将偏离较远的候选位置删除，对剩余的候选点求均值得到最终定位结果。

本发明另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：

对某个未知节点的多个邻居锚节点进行组合，使用三边定位算法得到多个候选位置组成的候选点集，并对所述多个候选位置按照X,Y,Z轴的值升序进行排列；

从候选点集中将偏离较远的候选位置删除，对剩余的候选点求均值得到最终定位结果。

本发明另一目的在于提供一种实施所述鲁棒式低功耗TDOA定位方法的的无人机。

本发明另一目的在于提供一种实施所述鲁棒式低功耗TDOA定位方法的室内任务应用场景的机器人。

结合上述的所有技术方案，本发明所具备的优点及积极效果为：

为了提高协同定位算法的可应用性，很多非集中式的定位算法被提出。本发明基于两部加权最小二乘的TDOA定位算法，提出一种鲁棒式定位模式，减少测量环境中信号干扰的随机性对定位结果造成的大误差现象；由于两部加权最小二乘采用线性化求解模式，该算法具有闭式求解过程但同时对测距误差较为敏感；通过提出一种鲁棒式的定位算法，避免实际测量中，信号传播偏离，通信错误所造成的定位系统的错误，从而影响其他功能的正常运行，提高整个系统的鲁棒性和可应用性；原始的两步加权最小二乘算法在定位过程中，由于测距通信数据错误，突发性网络延迟，网络数据包丢失等情况，其定位结果存在很大的误差，甚至会影响需要位置信息的主系统正常工作。而在低功耗的设备上，由于处理器的性能有限，无法承担较高迭代次数，因此，实验结果表明，在有限的迭代次数中，本发明提出的算法其平均误差，定位错误点数量都要优于原始算法。

本发明假定目标位置均要均匀地部署在100x100的正方形空间中。仿真结果是通过1000次Monte Carlo运行获得的。测距误差分为非视距误差和视距误差，非视距误差均值为0.2，标准差为0.2，视距误差均值为0.02，标准差为0.01，非视距测量值生成的概率为0.1。

在视距下条件下，本发明的算法平均定位误差为0.0317，最小定位误差为0.0128，最大定位误差为0.213，在少量非视距测距加入的条件下，平均定位误差为0.0539，最大定位误差为0.245，最小定位误差为0.0143。算法时间复杂度为O(N(2WLS))，组合定位循环次数为

本发明假定目标位置均要均匀地部署在100x100的正方形空间中。仿真结果是通过1000次Monte Carlo运行获得的。测距误差分为非视距误差和视距误差，非视距误差均值为0.2，标准差为0.2，视距误差均值为0.02，标准差为0.01，非视距测量值生成的概率为0.1。实验采用的电脑CPU为Intel core i53470，内存8gb。分别对原始两步加权最小二乘法和本发明的改进算法进行对比。

在视距下条件下，原始两步加权最小二乘法定位算法平均定位误差为0.0717，最小定位误差为0.0108，最大定位误差为1.186，在少量非视距测距加入的条件下，平均定位误差为0.0795，最大定位误差为1.245，最小定位误差为0.0157。算法时间复杂度为O(2N)。平均每次执行时间为0.0003s。

在视距下条件下，本发明的算法平均定位误差为0.0317，最小定位误差为0.0128，最大定位误差为0.213，在少量非视距测距加入的条件下，平均定位误差为0.0539，最大定位误差为0.245，最小定位误差为0.0143。算法时间复杂度为O(N(2WLS))，组合定位循环次数为

附图说明

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

图1是本发明实施例提供的组合定位方法示意图。

图2是本发明实施例提供的两步加权最小二乘实验结果图。

其中，图2(a)为所有定位源X轴定位误差；图2(b)为所有定位源Y轴定位误差；图2(c)为所有定位源Z轴定位误差；图2(d)为多次实验定位平均误差。

图3是本发明实施例提供的两步加权最小二乘实验结果图。

其中，图3(a)为所有定位源X轴定位误差；图3(b)为所有定位源Y轴定位误差；图3(c)为所有定位源Z轴定位误差；图3(d)为多次实验定位平均误差。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种鲁棒式低功耗TDOA定位方法及鲁棒式中值过滤器，下面结合附图对本发明作详细的描述。

本发明提供一种鲁棒式低功耗TDOA定位方法，应用于终端，所述鲁棒式低功耗TDOA定位方法包括：

对某个未知节点的多个邻居锚节点进行组合，使用三边定位算法得到多个候选位置组成的候选点集，并对所述多个候选位置按照X,Y,Z轴的值升序进行排列；

从候选点集中将偏离较远的候选位置删除，对剩余的候选点求均值得到最终定位结果。

下面结合具体实施例及原理分析对本发明作进一步描述。

实施例

(一)TDOA定位模型

(1)测距模型

假设测量时总共用到m个锚节点的测量数据，定义第i个锚节点传感器相对位置为s

不失一般性，假设第一个锚节点传感器s1为参考节点，因此第i个锚节点传感器与第1个锚节点传感器的到达时间差(TDOA)为

其中，

r＝r

这里，r＝[r

在考虑锚节点位置误差情况下，定位过程中锚节点位置为：

s＝s

这里，s＝[s

(2)闭式求解过程

通过构建线性化方程，可将上述测量模型进行求解，得到定位估计值，首先对TDOA等式进行处理，两边同时平方可得：

通过加入噪声，可将上述等式写为：

其中：

由此，可以利用加权最小二乘法对未知数u

其中，利用测量模型，权值矩阵为：

这里：

由此可以得到

通过上述算法，可以得到初步的包含误差的定位源位置估计值，下一步需要继续利用定位源位置和临时变量之间的关系，对位置估计值进行优化：

利用第一步的位置估计值，权值矩阵为：

因此，优化后的

通过

其中，

表3两步加权最小二乘法定位优化算法

(二)鲁棒式中值过滤器

由于使用线性化处理方法，其定位准确度随着测量噪声值的增加而快速恶化。为了增强算法鲁棒性，减少高等测量误差对定位结果的影响，需要对多个邻居锚节点测量值进行组合，得到组合后的定位结果，提高定位精度，其原理如图1组合定位方法所示。

图1中黑色圆点表示某个未知节点的邻居锚节点(初始的或升级而来的)，依次地组合这些锚节点，使用三边定位算法，最终得到一系列候选位置，在图中由三角形点表示。

图1中最右侧的三角形点与其他三角形的距离较远，可以看成是较大定位误差的结果，其主要原因可能是定位得到该位置的四个锚节点的分布较差，或者其中某段间距的测距误差较大甚至异常等。因此需要从候选点集中将这些偏离较远的候选位置删除，对剩余的候选点求均值得到最终定位结果。

现假设组合三边定位得到的候选点集为：

candidate_list＝{(x

理论上，得到的候选点大部分会集中在一起，而当存在少量异常数据或者某些组合的定位误差较大时，得到的候选点与其他大部分候选点的距离将较远，所以可以假设点集中的点服从正态分布。

其中

定义

表4随机组合定位算法

假设

证明：

假设测量次数为偶数时，可以把测量值写为：

u

这里

具体算法如表4所示，通过输入邻居锚节点，得到一组定位源估计值，通过后续对离群点进行过滤，可以得到最终位置估计值。

下面结合仿真实验结果及结论对本发明作进一步描述。

为了验证算法的性能，从两方面进行实验，首先保证实验具有相同参数的设置，对比改进前的定位算法；其次通过随机模拟无人机集群的运动状态，探究在随机环境下，多次测量中，改进后算法的稳定性和适用性。

参数设置方面参考文献，利用高斯矩阵，对测量误差和锚节点位置误差进行随机，其测量误差在±σ2diag[1,1,1,2,2,2,10,10,10,20,20,20,3,3,3]；sigma在0.001-1之间，为了仿真无人机集群下的定位场景，设定无人机数量为30，锚节点数量设置为100，与锚节点通信范围为30。

误差计算方面，通过对比多次测量的均方根误差(RMSE，Mean Squared Error)可以体现出算法的性能，均方根误差是指参数估计值与参数真值之差平方的期望值的平方根；RMSE可以评价数据的偏离程度，RMSE的值越小，说明预测模型描述实验数据具有更好的精确度

两步加权最小二乘实验结果：

在上述参数下，测量误差是最值得关注的要素之一，由于噪声分布的特征，由于原始定位算法特征，会使得某些节点定位性能很差。实验结果如图2(a)到图2(d)所示。由于误差的随机性，在受到较大测量误差影响情况下，原始定位优化算法在编号为3的无人机的定位结果上，产生了很大的误差，其中y轴坐标与z轴坐标误差约为1.2左右(如图2(b)，图2(c)纵坐标数据高的实线所示)；在正常情况下，原始两步加权最小二乘法定位算法也具有较好的性能，85％以上的定位源定位误差在0.20以内；但由于算法本身性质，其抗干扰能力一般，相比于精度不高的三边定位算法，其抗干扰能力不如这类算法；因此从图2(d)中可以看出，在上述参数配置情况下，两步最小二乘法在具有中等误差情况下，依旧具有良好的定位性能与定位精度。其中，图2(a)所有定位源X轴定位误差；图2(b)所有定位源Y轴定位误差；图2(c)所有定位源Z轴定位误差图2(d)多次实验定位平均误差。

鲁棒式两步加权最小二乘法实验结果：

虽然两步加权最小二乘法具有平均误差最小的优势，但在某些测量值误差较大的情况下，会存在严重定位误差节点，例如无人机节点，其存在1m左右的误差。在无人机集群定位系统中，由于环境的不可预知，需要提高定位算法对测量误差的容错性和算法本身的鲁棒性。

在相同参数下，经过多次实验，本发明提出的随机组合两步加权最小二乘定位算法实验结果如图3(a)-图3(d)所示，从图图3(a)-图3(d)中可以看出，算法在所有无人机节点中，编号13，17和20的无人机X轴误差高于平均误差，其误差结果在20左右，其Y轴与Z轴误差在15之内，在实验范围之内，定位结果偏离最严重的无人机定位误差均达到一般水准，相比于原始算法中某些无人机节点存在0.1左右的误差，改进后的定位算法在容错性方面有着显著的提升。

其中，图3(a)所有定位源X轴定位误差；图3(b)所有定位源Y轴定位误差。图3(c)所有定位源Z轴定位误差；图3(d)多次实验定位平均误差。

在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。

应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。