一种三维动态集群网络中基于两层滤波的容错协同定位方法

技术领域

本发明属于动态集群网络信息处理技术领域，尤其涉及一种三维动态集群网络中基于两层滤波的容错协同定位方法。

背景技术

目前，动态集群网络，包括移动机器人网络，车联网，无人机集群等。动态集群网络的核心问题可以归结为以下三个子问题：(1)我在哪？(2)我要去哪？(3)我怎么过去？而第一个子问题，集群网络节点的定位问题，又是后面两个子问题的基础。一种可能的方案是为集群网络中的每个节点安装GPS接收机，但一些集群网络例如移动机器人网络，通常部署在室内，太空，下水管道，水下等环境中，GPS信号容易受到多径干扰、遮挡等因素的影响，往往只有少部分节点能够得到精确的绝对位置信息。而且对于移动集群网络的某些应用，如编队、防碰撞等，只需要知道集群网络节点的相对位置信息即可。

利用集群网络节点之间通信交互的过程中获得的相对测量信息辅助网络节点定位是解决缺少蜂窝系统基站和在GPS缺失环境下高精度定位的一种新思路。相对测量信息包括相对距离测量，相对角度测量，相对速度测量等。本发明只考虑基于相对距离测量的定位。目前常用的测距技术主要有：射频信号强度指示模型RSSI，该技术费用低但测距精度不稳定；到达时间测量模型TOA以及超宽带(Ultra Wideband，UWB)。

基于测距的定位算法主要可分为两大类，单点定位和协同定位。三边定位或者多边定位是单点定位算法的基础，是完全分布式，算法简单，只需要局部通信即可完成，但该算法存在启动困难，过早停机，迭代误差方面的问题。基于刚性图的定位算法放松了定位单个点的需求，从而部分解决了锚节点稀疏带来的算法无法启动的问题。刚性图理论主要针对二维网络，对于三维网络的适用性较差。单点定位忽略了集群网络节点间潜在的合作定位的可能性，事实上，实际应用中许多不能通过单点定位的网络是可以通过节点间信息协同完成定位的。协同定位相较于单点定位的优势在于，可以利用集群网络节点间相对测量信息，给出集群网络节点相对位置的整体解。协同定位可以建模为求解章节2的非凸优化问题。求解该非凸优化问题的方法主要有基于半定规划(SDP，Semidefinite Programming)的算法和基于MDS(Multi-dimensional Scaling)的算法。基于SDP的求解方法，其基本思想是将原来的非凸优化问题放松为凸优化问题，进而通过内点法、模拟退火等算法求解，SDP算法最大的问题是计算复杂度过高，在理想的情况下计算复杂度依然达到O(n

文献提出的MDS-MAP是经典MDS算法，该算法用最短路径距离代替实际距离构建距离矩阵，在规则型网络中有效，但在不规则型网络存在较大定位误差。为了解决上述问题，Yi Shang在文献中，提出了分布式的MDS-MAP(P)算法，以及引入迭代求精的MDS-MAP(P,R)算法。

文献提出的dwMDS是一种迭代MDS算法，该算法利用SMACOF算法迭代求解压力函数。dwMDS仅要求节点与单跳邻居通信以及测距，相较于MDS-MAP算法降低了网络的通信量，提高了定位精度。

文献提出的vMDS算法，该算法将节点的相对速度加入到压力函数之中，构造了一个利用相对速度来辅助定位的MDS算法框架(vMDS)，求解压力函数也是利用的SMACOF算法。这里需要指出的是，SMACOF算法对初始输入非常敏感，一个合适的初始输入(不需要非常精确)会让算法迅速收敛，而一个随机的初始输入需要大量的迭代甚至无法收敛。

文献在利用SMACOF算法求解压力函数时，都没有给出一个确定初始输入的方法。经典MDS算法简单，对于一个连通网络可以很容易求解节点的相对坐标，但由于使用了最短路径距离代替节点间的实际距离，其精度不高；而SMACOF是一种迭代算法，能够得到很高的精度，但该算法对初始输入敏感。可以将经典MDS与SMACOF算法的优点相结合，利用经典MDS求解节点的相对坐标作为SMACOF算法的初始输入，进行迭代求精。针对MDS-MAP(P，R)中局部子图过多的问题，可以先将网络分簇，将大网划分为若干局部子网，由簇头节点负责收集簇内成员节点的距离信息，并解算该局部子网的相对坐标。马震在文献

CB-MDS算法首先利用FCM(fuzzy c-means)算法将认真无线电网络分簇，Fuzzy C-Means是一种聚类算法，该分簇方式可以获得较均匀的分簇结果，但却需要掌握全局信息。

上述三种基于分簇的算法，相对定位都只采用了经典MDS，而没有进行求精。这里需要指出的是，分簇的策略能够有效降低经典MDS因用最短路径距离代替实际距离产生的误差，然而这种误差仍然是不可忽视的，继续使用一些迭代求精的策略可以进一步减少误差。

现有的基于测距的协同定位算法主要针对无线环境较好的高精度定位，即一般假设节点间的观测距离与真实距离之间存在误差，而这种误差是零均值的高斯白噪声。但在实际环境中，并不是所有情况都能满足这一条件。容错定位算法是目前待解决的问题，这里所谓的错误条件主要指两个。第一是异常观测误差，节点间的观测距离比真实距离偏差较大。这种误差一方面可能是节点间距离的观测是在非视距(not line of sight，NLOS)环境下进行的，当参考节点与目标节点之间的LOS被阻挡时，电波发生超量延时，距离测量产生正均值的随机误差。另一方面可能是数据传输过程中产生的随机误差，目前集群网络(例如无人机集群)朝着低成本、小型化的方向发展，数据链路不稳定，距离数据在发送过程中可能产生传输误差。该误差与非视距观测的误差不同，即可能是比真实距离小，也可能比真实距离大，且与真实距离偏差较大。第二是链路中断，即视距内的节点也不能完成测距。具体的误差模型将在第二部分给出。由于协同定位算法能够充分利用集群网络节点间相对测量信息，给出集群网络节点相对位置的整体解，对于潜在的链路中断具有良好的鲁棒性。优势也会是劣势，若相对测量信息存在较大误差，则求解的整个网络节点的坐标信息都可能存在巨大偏差，甚至完全不可用，现有的容错定位算法研究，主要针对非视距环境(Nlos)，这些算法大致可以分为两类，一类是在硬件层面识别非视距测量，另一类是算法层面减弱非视距测量的影响，提高定位精度。

上述研究是在假设距离测量产生正均值的随机误差基础上进行的(观测距离比真实距离大)，而没有考虑数据传输过程中产生随机误差的情况，该种情况下，观测距离可能比真实距离小。

单点定位可以通过不同的锚节点组合得到待测节点的位置估计，进而通过一些聚类算法对随机误差进行排除，这是定位后校验。基于MDS的协同定位技术利用集群网络的协同信息给出整体的相对位置估计，这就使得很难根据后验信息对距离信息进行一致性校验。

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

(1)现有技术关于动态集群网络的协同定位问题，主要是针对二维无线环境，对于三维无线环境存在因技术计算复杂度高而不适用于计算能力弱的集群网络的问题；

(2)现有技术关于动态集群网络的协同定位问题，主要是针对存在非视距误差的无线环境，而没有针对数据在传输过程中可能产生的随机误差的无线环境；

(3)现有技术关于动态集群网络的协同定位问题，缺乏对随机误差环境中异常测距误差的识别，导致了协同定位结果精度低。

解决以上问题及缺陷的难度为：有效识别随机误差环境中的异常测距误差，提高三维动态集群网络的定位精度。

解决以上问题及缺陷的意义为：集群网络(例如无人机集群)朝着低成本、小型化的方向发展，数据链路不稳定，部署环境复杂，本发明主要分析容错条件下动态集群网络的协同定位。首先对NLOS误差进行了扩展，引入了随机误差。其次依据距离信息矩阵和集群网络的动态性构建了两层滤波机制，来过滤随机误差。本发明的主要贡献如下：

(1)构建了一种适合动态集群网络协同定位的框架。集群相对定位采用经典MDS与SMACOF算法相结合的方式求解节点相对坐标。该框架对于二维和三维空间同样适用。

(2)扩展了NLOS误差，引入了随机误差模型。随机误差主要包括异常观测误差与链路中断误差两类。

(3)为剔除动态集群网络中可能出现的异常观测误差设置了两层滤波机制。一是基于连续时间序列下运动状态的滤波，二是基于几何构型的滤波。

发明内容

针对现有技术存在的问题，本发明提供了一种三维动态集群网络中基于两层滤波的容错协同定位方法。

本发明是这样实现的，一种三维动态集群网络中基于两层滤波的容错协同定位方法，包括：首先拓展了无线环境中非视距误差，提出了随机误差的概念。其次构建了一种适合于三维动态集群网络协同定位的定位框架，在正常观测条件下该定位框架的归一均方根误差(NRMSE)接近于三维网络的克拉美罗下界(CRLB)。在存在随机误差的无线环境中，引入了基于连续时间序列下运动状态以及基于几何构型的两层滤波，提出了Filter-MDS。仿真实验表明，Filter-MDS能够有效的识别无线测距中存在的异常误差，提高集群网络在随机误差环境中的协同定位精度。

进一步，随机误差模型包括异常观测误差与链路中断误差；

所述异常观测误差包括：节点i与j之间的观测距离用

所述异常观测误差包括：发生观测误差时，节点i与j之间的观测距离用

链路中断误差，视距内的节点不能完成测距，发生误差的概率用p

进一步，构建适合于三维动态集群网络协同定位的定位框架的方法包括：

利用经典MDS求解节点的相对坐标作为SMACOF迭代算法的初始输入，迭代求精，稳定定位阶段利用t-1时刻的位置信息作为t时刻SMACOF迭代算法初始输入。

对于大规模的移动集群网路，采用分簇策略半分布式的框架，将大网划分为局部子网，降低计算与通信复杂度。

进一步，基于两层滤波的容错定位算法包括：

输入：1)t-1时刻的坐标估计

2)t时刻的观测距离

3)SMACOF算法的迭代终止条件ε，K

输出：t时刻的坐标估计

步骤1.根据

步骤2.D

步骤3.D

步骤4.初始化迭代器k＝0，

步骤5.Fork＝1,2…K

步骤5.1根据SMACOF算法更新

步骤5.2如果

步骤6.令

动态集群网络中有N个节点，m个锚节点，SMACOF算法的平均迭代次数是K

集群网络中的节点要将与邻居的测距信息发送给计算节点；网络中每个节点的平均邻居数量是K

进一步，所述三维网络的克拉美罗下界的误差分析方法包括：

节点间的测距误差

其中

再根据NRMSE的定义，得：

NRMSE的CRLB表示为：

本发明的另一目的在于提供一种实施所述三维动态集群网络中基于两层滤波的容错协同定位方法的三维动态集群网络中基于两层滤波的容错协同定位系统，所述基于两层滤波的容错协同定位系统包括：

随机误差获取模块，用于拓展无线环境中非视距误差，获取随机误差；

定位框架获取模块，用于构建适合于三维动态集群网络协同定位的定位框架，在正常观测条件下该定位框架的归一均方根误差相等于三维网络的克拉美罗下界；

无线测距中异常误差识别模块，在存在随机误差的无线环境中，引入基于连续时间序列下运动状态以及基于几何构型的两层滤波，进行基于两层滤波的容错定位算法运算，识别无线测距中存在的异常误差。

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

拓展无线环境中非视距误差，获取随机误差；

构建适合于三维动态集群网络协同定位的定位框架，在正常观测条件下该定位框架的归一均方根误差相等于三维网络的克拉美罗下界；

在存在随机误差的无线环境中，引入基于连续时间序列下运动状态以及基于几何构型的两层滤波，进行基于两层滤波的容错定位算法运算，识别无线测距中存在的异常误差。

本发明的另一目的在于提供一种实施所述三维动态集群网络中基于两层滤波的容错协同定位方法的无人机。

结合上述的所有技术方案，本发明所具备的优点及积极效果为：现有技术关于动态集群网络的协同定位问题的分析，主要是针对无线环境较好的二维网络。本发明针对三维动态集群网络的协同定位问题，提出了一种基于两层滤波的容错协同定位算法Filter-MDS。首先拓展了无线环境中非视距误差，提出了随机误差的概念。其次构建了一种适合于三维动态集群网络协同定位的定位框架，在正常观测条件下该定位框架的归一均方根误差(NRMSE)接近于三维网络的克拉美罗下界(CRLB)。在存在随机误差的无线环境中，引入了基于连续时间序列下运动状态以及基于几何构型的两层滤波，提出了Filter-MDS。仿真实验表明，Filter-MDS能够有效的识别无线测距中存在的异常误差，提高集群网络在随机误差环境中的协同定位精度。

相比于现有技术，本发明的优点进一步包括：本发明实验统计了Filter-MDS对异常误差的检测率。主要参数设置如下：通信半径R＝400，P

表4.Filter-MDS对异常误差的检测率

随着异常观测误差比例的增加，错误检测率有下降趋势，但检测率均超过99％，说明本发明设计的两层滤波机制能够有效过滤异常观测误差，增加协同定位的精度。

附图说明

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

图1是本发明实施例提供的链路中断对定位精度的影响图。

图2是本发明实施例提供的异常观测误差对定位精度的影响图。

图3是本发明实施例提供的自由反方向移动图。

图4是本发明实施例提供的自由相向移动图。

图5是本发明实施例提供的编队反方向加速移动图。

图6是本发明实施例提供的编队相向加速移动图。

图7是本发明实施例提供的无法构成三角形的两种情形图。

图7(a)

图8是本发明实施例提供的与CRLB的比较图。

图9是本发明实施例提供的不同最大迭代次数对NRMSE的影响图。

图10是本发明实施例提供的异常观测误差下Filter-MDS的NRMSE图。

图11是本发明实施例提供的异常观测误差下Filter-MDS的MAE图。

图12是本发明实施例提供的随机误差下Filter-MDS的NRMSE图。

图13是本发明实施例提供的随机误差下Filter-MDS的MAE图。

具体实施方式

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

针对现有技术存在的问题，本发明提供了一种三维动态集群网络中基于两层滤波的容错协同定位方法，下面结合附图对本发明作详细的描述。

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

实施例

1.系统模型

1.1动态集群网络定位问题描述

考虑具有N个节点的动态集群网络，该网络分布在p(p＝2或p＝3)维空间之中，p＜＜N。假设集群网络中有m个锚节点利用GPS系统或者相似的定位系统已经有了绝对位置信息，m＜＜N。剩余的n＝N-m个节点没有位置信息，称为未知节点。假设未知节点编号为1…n，则锚节点编号可表示为n+1,…,n+m。如果在一些应用中，只需要知道集群节点的相对位置信息，则不需要锚节点。在时刻t，节点i在p维空间的位置信息用列向量

1.2随机误差模型

本发明将NLOS误差模型拓展为随机误差模型，随机误差模型主要包括异常观测误差与链路中断误差。

第一类异常观测误差是节点间的观测距离比真实距离大很多(例如两节点的实际距离是30，而观测距离是90)，与传统的NLOS误差模型类似。发生该类型的观测误差时，节点i与j之间的观测距离用

第二类异常观测误差是节点间的观测距离比真实距离小很多(例如两节点的实际距离是30，而观测距离是10)，发生该类型的观测误差时，节点i与j之间的观测距离用

链路中断误差，即视距内的节点也不能完成测距，发生该类型误差的概率用p

1.3定位框架

实际上，知道了节点间的距离之后，估计节点相对坐标的问题可转化为如式(4)所示的优化问题：

优化问题(4)是一个非凸的问题，目前还没有一种低复杂度的算法能够求解出(4)的全局最优解。经典MDS是求解(4)的一种算法，该算法简单，适用于全连通网络，对于非全连通网络因用最短路径距离代替实际距离，误差较大。许多通过迭代求解优化问题(4)的算法被提出，基于优化函数(Majorizing Function)的非线性最小二乘法(SMACOF)是目前效率较高的算法，因为基于该算法的迭代过程可以保证压力函数(4)产生一个单调不增的序列。

(4)式可以改写为：

压力函数可以分解为三个部分。如式(6)所示，第一部分C，

这里需要指出的是，SMACOF算法对初始输入非常敏感，在本发明的仿真中，一个合适的初始输入(不需要非常精确)会让算法迅速收敛，而一个随机的初始输入需要大量的迭代甚至无法收敛。将经典MDS与SMACOF迭代算法的优点相结合。首次定位时，利用经典MDS求解节点的相对坐标作为SMACOF迭代算法的初始输入，迭代求精，稳定定位阶段利用t-1时刻的位置信息作为t时刻SMACOF迭代算法初始输入。本发明的定位框架(LocationFramework)如下表1所示。表1中ε和K

表1定位框架

对于大规模的移动集群网路，可以采用基于分簇的策略，这是一种半分布式的框架，将大网划分为局部子网，可显著降低计算与通信复杂度，同时适用于一些不规则型网络。

2.基于两层滤波的容错定位算法Filter-MDS

基于经典MDS+SMACOF的定位框架可以利用集群网络的协同信息给出整体的相对位置估计，这就很难根据后验信息对距离信息进行一致性校验。但在相对定位阶段，计算节点会收集成员节点间的距离信息构建距离信息矩阵D

2.1链路中断误差

协同定位相较于单点定位的优势在于可以综合集群尽可能多的信息，给出整体解。这种特性也使得协同定位对于网络中存在的链路中断具有更好的鲁棒性。在本发明的方案中，集群相对定位技术采用了经典MDS+SMACOF算法，这也是一种协同定位技术。下面通过仿真实验，来检测本发明的定位框架于链路中断的鲁棒性。

实验设置如下：节点数量50，随机部署在400*400*400的区域，计算结点数量1，锚节点数量6，节点间的通信与测距距离均为400m，测距误差服从

结果如下图1链路中断对定位精度的影响所示。随链路中断比例的增大，定位绝对误差的均值和标准差都有增大的趋势，但链路中断比例在50％以下时，增长幅度较小，由此可见，经典MDS+SMACOF的定位框架对于链路中断具有良好的鲁棒性。但需要指出的是，根据本发明的仿真实验，当链路中断比例达到60％以后，定位的绝对误差开始变得不稳定，特别是首次定位会存在巨大偏差。

这给本发明设计过滤异常误差的方案带来启示，若判断出两节点间的距离存在异常误差，可令

2.2异常观测误差

2.1.1异常观测误差对定位算法解的影响

协同定位的基本思路是综合节点间的观测信息，给出节点位置信息的统一解。其方法是首先构建节点间距离观测矩阵，然后求解该距离矩阵。若该距离矩阵某部分元素较真实值有巨大偏差，给出的统一解也会有巨大偏差。

为说明异常观测误差对协同定位算法解的影响，本发明对本发明的定位框架做如下仿真实验。实验设置如下：基本设置如链路中断误差实验，中断比例取15％，加入异常观测误差，异常观测误差参数设置如下表2所示。

表2.异常观测误差参数设置

图2异常观测误差对定位精度的影响的横坐标是指加入的异常观测误差比例，其中1％，1％是指P

2.1.2对异常观测误差的两层滤波

异常观测误差较真实值偏差大，容易检测并剔除，本发明为剔除动态集群网络中可能出现的异常观测误差设置了两层滤波机制。

a)基于连续时间序列下运动状态的滤波机制

集群网络的运动状态可分为自由移动与编队移动。假设节点i与节点j在t-1时刻的距离是

(a)自由移动状态

节点i在t-1时刻的位置是

综上，集群自由移动状态下，节点i与j之间的距离在相邻两个时刻的变化量是

(b)编队移动状态

节点i在t-1时刻的位置是

综上，集群编队移动状态下，节点i与j之间的距离在相邻两个时刻的变化量是Δd

在集中式定位模式下，基于连续时间序列下运动状态的滤波由计算节点执行。计算节点存储t-1时刻构建的成员节点距离矩阵D

基于运动状态的滤波算法

b)基于几何构型的滤波

基于连续时间序列下运动状态的滤波机制并不能将所有的异常误差全部过滤，考虑如下情况：

在t-1时刻，节点i与j之间的观测距离

本发明希望根据节点间的几何构型进一步检测异常观测误差。由于分布于三维空间中的集群网络，很难用二维空间中刚体理论来校验距离矩阵D

假设集群中有N个节点，对于

若出现

针对上述无法构成三角形的两种情形，本发明设置了投票机制来判断哪个观测距离可能出现异常误差。对于

基于几何构型的滤波算法

2.3链路中断误差与异常观测误差同时发生(随机误差环境)

在随机误差环境下的动态相对定位过程中，仅比较前后两个时刻节点间的距离状态，部分异常观测误差无法剔除。考虑如下情况：节点i与j在t-1时刻由于视距内链路中断，

基于多时刻运动状态的滤波算法

2.4Filter-MDS

综上，本发明只要在定位框架中加入两层滤波即构建了Filter-MDS。Filter-MDS的算法步骤如下所示。

Filter-MDS算法

输入：1)t-1时刻的坐标估计

2)t时刻的观测距离

3)SMACOF算法的迭代终止条件ε，K

输出：t时刻的坐标估计

步骤1.根据

步骤2.D

步骤3.D

步骤4.初始化迭代器k＝0，

步骤5.Fork＝1,2…K

步骤5.1根据SMACOF算法更新

步骤5.2如果

步骤6.令

2.5Filter-MDS复杂度分析

2.5.1计算复杂度分析

动态集群网络中有N个节点，m个锚节点，SMACOF算法的平均迭代次数是K

综上，Filter-MDS的计算复杂度是O(N

2.5.2通信复杂度分析

集群网络中的节点要将与邻居的测距信息发送给计算节点。假设网络中每个节点的平均邻居数量是K

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

(1)本发明在3-D空间中对Filter-MDS算法的性能进行仿真实验。推导了3-D空间中集中式的克拉美罗下界，作为评价定位框架性能的标准。

实验环境

实验软件是Windows10系统下Matlab2017a，具体实验环境参数如下表所示：

表3.实验环境参数

(2)实验设置

为检验Filter-MDS对异常误差的鲁棒性，做如下仿真实验。实验设置如下：节点数量50，初始随机部署在400*400*400的区域，计算节点数量1，锚节点数量6(3D网络坐标转换至少需要4个锚节点，设置6个主要是为了避免由于共面问题造成坐标转换过程中的巨大误差)。节点间通信与测距半径是R＝400，

其中σ

除另有说明，所有仿真数据均是取自200次蒙特卡洛仿真实验的均值。用归一化均方根误差(Normalized root mean square error，NRMSE)评价位置估计的准确度，NRMSE的定义如下：

根据本发明设定的集群网络的运动状态，随着时间的推移，节点的横纵坐标在不断改变，所以令

(3)本发明的定位框架在三维动态集群网络定位中的有效性

本发明的仿真实验主要是验证定位框架在三维动态集群网络定位的有效性，故在本发明的实验中观测距离不存在随机误差。

本发明的定位框架和MDS-MAP在不同通信半径R下的NRMSE如图8与CRLB的比较所示。图8黑色虚线是全连通网络的克拉美罗下界(Cramer-Rao lower bound，CRLB)，其值为0.0035。关于CRLB的推导，随着通信半径R的增加，本发明的定位框架和MDS-MAP的NRMSE都在下降，当R＝400时，本发明的定位框架的NRMSE是0.0047接近于CRLB，而MDS-MAP的NRMSE是0.0103。这说明了本发明的定位框架在三维动态集群网络定位中的有效性。当通信半径R超过600时，该集群网络逐渐变为全连通网络。

图9不同最大迭代次数对NRMSE的影响图展示了不同最大迭代次数K

间内收敛，但随时间的推移，算法的NRMSE波动性较大。总得来说，这是运算复杂度和位置估计的精确性与稳定性的折中。

(4)异常观测误差下Filter-MDS的NRMSE和MAE

本发明实验中本发明在节点观测距离中添加异常误差(保持链路中断比例P

图11异常观测误差下Filter-MDS的MAE图展示的是异常观测误差下Filter-MDS(Kback＝1)的MAE,其中X轴与Y轴分别表示计算节点在运动过程中的横坐标与纵坐标，运动时间t∈[1,100]，Z轴表示每个时刻集群网络定位结果的绝对误差均值(MAE)。首次定位时，由于没有先验信息，MAE是2.88m，稳定定位阶段MAE基本稳定在3m以下。

(5)随机误差下Filter-MDS的NRMSE和MAE

本发明实验中本发明在节点观测距离中添加随机误差，以验证Filter-MDS在随机误差环境下的鲁棒性。通信半径R＝400，P

图13随机误差下Filter-MDS的MAE图展示的是随机误差下Filter-MDS(Kback＝4)的MAE,其中X轴与Y轴分别表示计算节点在运动过程中的横坐标与纵坐标，运动时间t∈[1,100]，Z轴表示每个时刻集群网络定位结果的绝对误差均值(MAE)。前2次定位时，由于没有足够的先验信息，MAE均超过6m，稳定定位阶段MAE稳定在3m以下。

(6)异常观测误差检测率

本发明实验统计了Filter-MDS对异常误差的检测率。主要参数设置如下：通信半径R＝400，P

表4.Filter-MDS对异常误差的检测率

随着异常观测误差比例的增加，错误检测率有下降趋势，但检测率均超过99％，说明本发明设计的两层滤波机制能够有效过滤异常观测误差，增加协同定位的精度。

下面结合效果对本发明作进一步描述。

目前集群网络(例如无人机集群)朝着低成本、小型化的方向发展，数据链路不稳定，部署环境复杂，三维动态集群网络的容错协同定位是亟待解决的问题。本发明构建了适合三维动态集群网络协同定位的框架，拓展了非视距误差，引入了随机误差的概念，在随机误差环境中提出了基于连续时间序列下运动状态以及基于几何构型的两层滤波算法Filter-MDS，仿真实验表明该算法可以有效的过滤观测距离中存在的异常误差，有效保证三维动态集群网络在随机误差环境中的协同定位精度。

Filter-MDS的计算复杂度是O(N

在本发明中，提供的NRMSE的克拉美罗下界(三维空间)包括：

采用的是集中式克拉美罗下界，即集群网络是全连通的。CRLB的计算与文献相似，现有技术中，推导了二维空间的克拉美罗下界，本发明将其推广到了三维空间。节点间的测距误差

其中

由于求解的是集中式CRLB，集群网络全连通，所以

综上所述，NRMSE的CRLB可表示为：

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

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

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