CN110225454A - 一种置信度传递的分布式容积卡尔曼滤波协作定位方法 - Google Patents

Abstract

本发明公开了一种基于置信度传递的分布式容积卡尔曼滤波协作定位方法。其中，该方法包括：无线传感器网络中移动代理节点初始状态满足高斯分布，并将移动代理节点协作定位建模为时变因子图中多变量节点的边缘后验分布估计问题；其次，提出了高斯参数化置信度传递与重构策略，以及分布式容积卡尔曼滤波方法，计算因子图上每个变量节点的后验分布，进而获得每个移动代理节点定位结果。本发明公开协作定位方法中相邻代理节点之间只需传递高斯参数化置信度，具有良好的扩展性和鲁棒性，较低的通信开销和计算复杂度，提升整体网络定位精度和效率。

Description

一种置信度传递的分布式容积卡尔曼滤波协作定位方法

技术领域

本发明涉及无线传感器网络中移动节点协作定位技术领域，尤其涉及一种置信度传递的分布式容积卡尔曼滤波协作定位方法。

背景技术

目前，在无人驾驶导航技术和移动机器人技术快速发展背景下，对位置信息的需求逐渐增加，利用无线传感器网络协作定位技术实现移动节点定位受到了众多学者的广泛关注与研究。经典的和积无线网络定位算法采用非参数置信度传递策略实现代理节点协作定位，为了保证较好的定位精度需要应用大量随机采样粒子，导致较高的计算复杂度，同样也会占用大量的传感器资源。由于移动节点之间消息传递需要发送表示位置信息的所有粒子，较高的通信开销导致较高的能量消耗，缩短传感器和电池的使用寿命。采用分布式扩展卡尔曼滤波算法实现协作定位，由于需要求解雅可比矩阵，较高的计算复杂度限制其实际应用，并且非线性函数近似策略引入了模型误差导致定位精度有限；其次，分布式不敏卡尔曼滤波算法中参数和权值的确定都会影响最终的定位结果，并且其协方差不能始终保证正定导致定位效果不稳定甚至定位失败。因而，需要开发出具有良好扩展性和稳定性，较低通信开销和计算复杂度的分布式协作定位算法。

发明内容

本发明的目的是提供一种置信度传递的分布式容积卡尔曼滤波协作定位方法，能够有效解决高斯系统下的分布式移动代理节点协作定位问题，提升了移动代理节点定位精度，较低的通信开销和计算复杂度适用于无线传感器网络中的移动节点协作定位。

本发明采用的技术方案为：

一种置信度传递的分布式容积卡尔曼滤波协作定位方法，包括以下步骤：

(1)构建分布式移动代理节点协作定位网络模型，初始化网络参数、置信度迭代计算次数，已知参考节点位置，移动代理节点状态先验消息满足高斯分布；

(2)每个移动代理节点计算自身状态预测消息，包括高斯均值向量和误差协方差矩阵；

(3)构建实时量测网络，获得相邻代理和参考节点之间距离量测，对获得的实时量测消息进行存储；

(4)执行置信度传递与迭代计算，相邻移动代理节点之间传递高斯参数化置信度，并采用重构策略获得组合高维均值向量和协方差矩阵表示组合先验消息；

(5)根据步骤(4)获得高维组合均值向量和协方差矩阵，计算高维组合容积点，并将组合容积点带入非线性量测函数计算容积点传播，获得组合量测消息和量测预测均值；

(6)根据步骤(5)中获得的高维组合容积点和组合量测消息以及量测预测均值，计算量测预测协方差矩阵和组合状态消息与量测信息的交叉协方差矩阵；

(7)计算容积卡尔曼滤波增益，移动代理节点状态估计均值和误差协方差矩阵更新，获得移动代理节点高斯参数化置信度；返回步骤(4)执行高斯参数化置信度传递与迭代计算，完成预定迭代计算次数后返回步骤(2)进入下一轮递归计算。

所述步骤(1)中，初始化网络参数，t＝0时刻，已知参考节点位置集合{x_a′}，移动代理节点状态{x_a,0}先验消息满足高斯分布J(x_a，0)～(μ_a，0，C_a，0)，其中，t∈{0,...,T}表示运行步长，T表示运行步长总和，N(·)表示高斯分布缩写，f(x_a,0)表示先验消息，μ_a,0和C_a,0是已知高斯分布均值向量和协方差矩阵，(a,a′)∈A表示一个移动代理节点和一个参考节点，A是网络中所有节点集合；已知移动代理节点过程噪声和量测噪声满足零均值高斯分布分别表示为其中，u_a,t是已知过程噪声，是已知过程噪声方差，I是单位矩阵，v_a,k；t是量测噪声，是已知量测噪声方差，k是相邻节点，因子图中置信度传递迭代计算次数设置为n∈{1,...,N}，N表示正整数。

所述步骤(2)中移动代理节点计算t时刻自身状态预测消息f(x_a,t|t-1)，首先，获得t-1时刻的移动代理节点状态后验高斯分布均值向量和误差协方差矩阵其次，计算2d_a个容积点ξ⁽ⁱ⁾和表达式如下：

其中，d_a是节点均值向量的维度，e_i表示矩阵第i列元素，表示d_a维的单位矩阵，n是置信度迭代计算次数，移动代理节点通过状态转移函数x_a,t＝f(x_a,t-1,u_a,t)计算传播后的容积点表达式如下：i∈1,...,2d_a；进而可以计算移动代理节点预测消息包括均值向量μ_a,t|t-1和误差协方差矩阵C_a,t|t-1表达式如下：

其中，Q_t-1是已知过程噪声方差矩阵，所有移动代理节点完成状态预测和预测误差协方差计算，获得预测均值向量μ_a,t|t-1和误差协方差矩阵C_a,t|t-1表示t时刻移动代理节点先验分布消息f(x_a,t)。

所述步骤(3)中，移动代理节点获得相邻代理和参考节点之间距离量测表达式如下：y_a,k,t＝h(x_a,t,x_k,t,v_a,k；_t)，其中，x_a,t和x_k,t是代理节点和相邻节点状态，k∈M_a,t表示相邻节点，M_a,t表示相邻节点集合，也是网络节点集合A的子集表示为量测模型中h(·)是非线性函数计算表达式如下：其中，||·||表示欧式距离范数，和表示节点状态中的位置向量；进而，可以获得组合量测消息计算表达式如下：Y_a,t＝H(X_a,t)+V_a,t，其中组合状态向量组合量测值 表示相邻节点状态向量，是t时刻代理节点获得相邻节点距离量测，N_a,t＝(1,2,...,N)表示t时刻与代理节点a相链接的相邻节点个数。

所述步骤(4)中，相邻代理节点之间传递高斯参数化置信度表示为均值向量和误差协方差矩阵其中，n表示迭代计算次数，k→a表示t时刻相邻代理节点之间传递消息；并采用消息重构策略获得组合高维均值向量和协方差矩阵来表示组合先验消息f^(n-1)(X_a,t)，其中，高维组合均值向量和高维组合误差协方差矩阵表达式如下：

其中，diag(·)表示块对角矩阵。

所述步骤(5)中，根据获得的组合均值向量和误差协方差矩阵计算2D_a个组合容积点和表达式如下：

其中，D_a表组合均值向量的维度，d_a表示均值向量的维度，d_k表示相邻节点均值向量的维度，表示矩阵第i列元素，表示D_a维的单位矩阵；随后，将容积点带入非线性量测函数计算容积点传播并计算量测预测均值表达式如下：

所述步骤(6)中，根据组合容积点组合量测消息量测预测均值计算组合量测协方差矩阵和组合状态与量测的交叉协方差矩阵表达式如下：

其中，R_a,t是已知组合量测方差，X表示状态标识符,Y表示量测标识符；由于组合均值包含相邻节点位置消息k∈M_a,t，生成的组合容积点也包含了相邻节点位置消息，因而是一个耦合交叉协方差矩阵，在计算单个移动代理节点状态增益时需要对矩阵进行解耦合处理，从而提取出关于移动代理节点状态的交叉协方差矩阵，对D_a维交叉协方差矩阵进行解耦合，得到d_a维解耦交叉协方差矩阵

所述步骤(7)中，根据量测协方差矩阵和解耦交叉协方差矩阵计算迭代过程中的滤波增益表达式如下：根据t时刻量测消息Y_a,t计算代理状态估计均值和误差协方差矩阵表达式如下：

移动代理节点获得均值向量和误差协方差矩阵表示为自身高斯参数化置信度，随后返回步骤(4)进入下一次高斯参数化置信度传递与迭代计算，完成预定迭代计算次数后返回步骤(2)进入下一轮递归。

本发明针对传统分布式移动代理节点协作定位方法存在计算复杂度较高和定位精度较差等特点，提出了一种基于置信度传递的分布式容积卡尔曼滤波协作定位方法，该方法具有以下优势，首先，代理节点实现协作定位过程中，相邻代理节点只需传递均值向量和协方差矩阵，具有较低的通信开销和计算复杂度，适用于无线传感器网络中的移动节点协作定位。其次，代理节点协作定位采用本发明方法可以有效的避免了分布式扩展卡尔曼滤波算法因求解雅可比矩阵带来的计算复杂度急剧增加问题，更能有效避免采用分布不敏卡尔曼滤波算法因协方差非正定位导致定位失败问题。并且综合在定位精度、定位效率、计算复杂度和通信开销等方面均优于传统非参数置信度传递策略实现的分布式协作定位方法，在实际应用中更具有可行性。

附图说明

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

图1为本发明的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明包括以下步骤：

(1)构建分布式移动代理节点协作定位网络模型，初始化网络参数、置信度迭代次数，已知参考节点位置，移动代理节点状态先验消息满足高斯分布；首先，初始化网络参数，t＝0时刻，已知参考节点位置集合{x_a′}，移动代理节点状态{x_a,0}先验消息满足高斯分布f(x_a，0)～N(μ_a，0，C_a，0)，其中，t∈{0,...,T}表示运行步长，T表示运行步长总和，N(·)表示高斯分布缩写，f(x_a,0)表示先验消息，μ_a,0和C_a,0是已知高斯分布均值向量和协方差矩阵，(a,a′)∈A表示一个移动代理节点和一个参考节点，A是网络中所有节点集合。已知移动代理节点过程噪声和量测噪声满足零均值高斯分布分别表示为其中，u_a,t是已知过程噪声，是已知过程噪声方差，I是单位向量，v_a,k；_t是量测噪声，是已知量测噪声方差，k是相邻节点，因子图中置信度传递迭代计算次数设置为n∈{1,...,N}，N表示正整数。

(2)每个移动代理节点计算自身状态预测消息，包括高斯均值向量和误差协方差矩阵；移动代理节点计算自身状态预测消息f(x_a,t|t-1)，首先，获得t-1时刻移动代理节点状态后验高斯分布均值和误差协方差矩阵其次，计算2d_a个容积点ξ⁽ⁱ⁾和表达式如下：

其中，d_a是节点均值向量的维度，e_i表示矩阵第i列元素，表示d_a维的单位矩阵，n是置信度迭代计算次数，移动代理节点通过状态转移函数x_a,t＝f(x_a,t-1,u_a,t)计算传播后的容积点表达式如下：i∈1,...,2d_a；进而可以计算移动代理节点预测均值向量μ_a,t|t-1和预测误差协方差矩阵C_a,t|t-1表达式如下：

其中，Q_t-1是已知过程噪声方差矩阵，所有移动代理节点完成状态预测和预测误差协方差计算，获得预测均值向量μ_a,t|t-1和误差协方差矩阵C_a,t|t-1表示t时刻移动代理节点先验分布消息f(x_a,t)。

(3)构建实时量测网络，获得相邻移动代理节点和参考节点之间距离量测，对获得的实时量测消息进行存储；移动代理节点获得相邻移动代理节点和参考节点之间噪声量测距离表达式如下：y_a,_k,t＝h(x_a,t,x_k,t,v_a,k；_t)，其中，x_a,t和x_k,t是代理节点和相邻节点状态，k∈M_a,t表示相邻节点，M_a,t是相邻节点集合，也是网络集合A的子集表示为量测模型中h(·)是非线性函数计算表达式如下：其中，||·||表示欧式距离范数，和表示节点状态中的位置向量；进而，可以获得组合量测消息计算表达式如下：Y_a,_t＝H(X_a,t)+V_a,t，其中， 组合状态向量组合量测值其中，表示相邻节点状态向量，是t时刻代理节点获得相邻节点距离量测，N_a,_t＝(1,2,...,N)表示t时刻与代理节点a相链接的相邻节点个数，所述的步骤(2)与步骤(3)中各个子步骤之间可以并行处理。

(4)执行置信度传递与迭代计算，相邻代理节点之间传递高斯参数化置信度，并采用重构策略获得组合高维均值向量和协方差矩阵表示组合先验消息；相邻代理节点之间传递高斯参数化置信度表示为均值向量和误差协方差矩阵其中，n表示迭代计算次数，k→a表示t时刻相邻代理节点之间传递消息；并采用消息重构策略获得组合高维均值向量和协方差矩阵来表示组合先验消息f^(n-1)(X_a,t)，其中，高维组合均值向量和高维组合误差协方差矩阵表达式如下：

其中，diag(·)表示块对角矩阵。

(5)根据步骤(4)获得高维组合均值向量和协方差矩阵，计算高维组合容积点并将组合容积点带入非线性函数计算容积点传播，获得量测预测均值；根据获得的组合均值向量和误差协方差矩阵计算2D_a个组合容积点和表达式如下：

其中，D_a表组合均值的维度，d_a表示的均值向量的维度，d_k表示相邻节点均值向量的维度，表示矩阵第i列元素，表示D_a维的单位矩阵；随后，将容积点带入非线性量测函数计算容积点传播并计算量测预测均值表达式如下：

(6)根据步骤(5)中获得的高维组合容积点和组合量测消息，计算量测预测协方差矩阵和组合状态消息与量测信息的交叉协方差矩阵；获得组合容积点组合量测消息量测预测均值计算组合量测预测协方差矩阵和组合状态与量测的交叉协方差矩阵表达式如下：

其中，R_a,t是已知组合量测方差，X表示组合状态标识符,Y表示组合量测标识符；由于组合均值包含相邻节点位置消息k∈M_a,t，生成的组合容积点也包含了相邻节点位置消息，因而是耦合交叉协方差矩阵，在计算单个节点状态增益时需要对进行解耦合处理，从而提取出关于代理节点状态的交叉协方差矩阵，对D_a维交叉协方差矩阵进行解耦合，得到d_a维解耦交叉协方差矩阵

(7)计算容积卡尔曼滤波增益，计算移动代理节点状态估计均值和误差协方差矩阵更新，获得移动代理节点高斯参数化置信度；根据量测协方差矩阵和解耦交叉协方差计算迭代过程中的滤波增益表达式如下：根据t时刻量测消息Y_a,t计算代理状态估计均值和误差协方差矩阵表达式如下：

移动代理节点获得均值向量和误差协方差矩阵表示为自身高斯参数化置信度，随后返回步骤(4)进入下一次高斯参数化置信度传递与迭代计算，完成预定迭代计算次数后返回步骤(2)进入下一轮递归计算。

本发明针对传统分布式移动代理节点协作定位方法存在计算复杂度较高和定位精度较差等特点，提出了一种基于置信度传递的分布式容积卡尔曼滤波协作定位方法，该方法具有以下优势，首先，代理节点实现协作定位过程中，相邻代理节点只需传递均值向量和协方差矩阵，具有较低的通信开销和计算复杂度，适用于无线传感器网络中的移动节点协作定位。其次，代理节点协作定位采用本发明方法可以有效的避免了分布式扩展卡尔曼滤波算法因求解雅可比矩阵带来的计算复杂度急剧增加问题，更能有效避免采用分布不敏卡尔曼滤波算法因协方差非正定位导致定位失败问题。并且综合在定位精度、定位效率、计算复杂度和通信开销等方面均优于传统非参数置信度传递策略实现的分布式协作定位方法，在实际应用中更具有可行性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：包括以下步骤：

(1)构建分布式移动代理节点协作定位网络模型，初始化网络参数、置信度迭代计算次数，已知参考节点位置，移动代理节点状态先验消息满足高斯分布；

(2)每个移动代理节点计算自身状态预测消息，包括高斯均值向量和误差协方差矩阵；

(3)构建实时量测网络，获得相邻代理和参考节点之间距离量测，对获得的实时量测消息进行存储；

(4)执行置信度传递与迭代计算，相邻移动代理节点之间传递高斯参数化置信度，并采用重构策略获得组合高维均值向量和协方差矩阵表示组合先验消息；

(5)根据步骤(4)获得高维组合均值向量和协方差矩阵，计算高维组合容积点，并将组合容积点带入非线性量测函数计算容积点传播，获得组合量测消息和量测预测均值；

(6)根据步骤(5)中获得的高维组合容积点和组合量测消息以及量测预测均值，计算量测预测协方差矩阵和组合状态消息与量测信息的交叉协方差矩阵；

(7)计算容积卡尔曼滤波增益，移动代理节点状态估计均值和误差协方差矩阵更新，获得移动代理节点高斯参数化置信度；返回步骤(4)执行高斯参数化置信度传递与迭代计算，完成预定迭代计算次数后返回步骤(2)进入下一轮递归计算。

2.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(1)中，初始化网络参数，t＝0时刻，已知参考节点位置集合{x_a′}，移动代理节点状态{x_a,0}先验消息满足高斯分布f(x_a,0)～N(μ_a,0,C_a,0)，其中，t∈{0,...,T}表示运行步长，T表示运行步长总和，N(·)表示高斯分布缩写，f(x_a,0)表示先验消息，μ_a,0和C_a,0是已知高斯分布均值向量和协方差矩阵，(a,a′)∈A表示一个移动代理节点和一个参考节点，A是网络中所有节点集合；已知移动代理节点过程噪声和量测噪声满足零均值高斯分布分别表示为其中，u_a,t是已知过程噪声，是已知过程噪声方差，I是单位矩阵，v_a,k；t是量测噪声，是已知量测噪声方差，k是相邻节点，因子图中置信度传递迭代计算次数设置为n∈{1,...,N}，N表示正整数。

3.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(2)中移动代理节点计算t时刻自身状态预测消息f(x_a,t|t-1)，首先，获得t-1时刻的移动代理节点状态后验高斯分布均值向量和误差协方差矩阵其次，计算2d_a个容积点ξ⁽ⁱ⁾和表达式如下：

其中，d_a是节点均值向量的维度，e_i表示矩阵第i列元素，表示d_a维的单位矩阵，n是置信度迭代计算次数，移动代理节点通过状态转移函数x_a,t＝f(x_a,t-1,u_a,t)计算传播后的容积点表达式如下：进而可以计算移动代理节点预测消息包括均值向量μ_a,t|t-1和误差协方差矩阵C_a,t|t-1表达式如下：

其中，Q_t-1是已知过程噪声方差矩阵，所有移动代理节点完成状态预测和预测误差协方差计算，获得预测均值向量μ_a,t|t-1和误差协方差矩阵C_a,t|t-1表示t时刻移动代理节点先验分布消息f(x_a,t)。

4.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(3)中，移动代理节点获得相邻代理和参考节点之间距离量测表达式如下：y_a,k,t＝h(x_a,t,x_k,t,v_a,k；t)，其中，x_a,t和x_k,t是代理节点和相邻节点状态，k∈M_a,t表示相邻节点，M_a,t表示相邻节点集合，也是网络节点集合A的子集表示为量测模型中h(·)是非线性函数计算表达式如下：其中，||·||表示欧式距离范数，和表示节点状态中的位置向量；进而，可以获得组合量测消息计算表达式如下：Y_a,t＝H(X_a,t)+V_a,t，其中组合状态向量组合量测值 表示相邻节点状态向量，是t时刻代理节点获得相邻节点距离量测，N_a,t＝(1,2,...,N)表示t时刻与代理节点a相链接的相邻节点个数。

5.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(4)中，相邻代理节点之间传递高斯参数化置信度表示为均值向量和误差协方差矩阵其中，n表示迭代计算次数，k→a表示t时刻相邻代理节点之间传递消息；并采用消息重构策略获得组合高维均值向量和协方差矩阵来表示组合先验消息f^(n-1)(X_a,t)，其中，高维组合均值向量和高维组合误差协方差矩阵表达式如下：

其中，diag(·)表示块对角矩阵。

6.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(5)中，根据获得的组合均值向量和误差协方差矩阵计算2D_a个组合容积点和表达式如下：

其中，D_a表组合均值向量的维度，d_a表示均值向量的维度，d_k表示相邻节点均值向量的维度，表示矩阵第i列元素，表示D_a维的单位矩阵；随后，将容积点带入非线性量测函数计算容积点传播并计算量测预测均值表达式如下：

7.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(6)中，根据组合容积点组合量测消息量测预测均值计算组合量测协方差矩阵和组合状态与量测的交叉协方差矩阵表达式如下：

其中，R_a,t是已知组合量测方差，X表示状态标识符,Y表示量测标识符；由于组合均值包含相邻节点位置消息k∈M_a,t，生成的组合容积点也包含了相邻节点位置消息，因而是一个耦合交叉协方差矩阵，在计算单个移动代理节点状态增益时需要对矩阵进行解耦合处理，从而提取出关于移动代理节点状态的交叉协方差矩阵，对D_a维交叉协方差矩阵进行解耦合，得到d_a维解耦交叉协方差矩阵

8.如权利要求1所述的基于置信度传递的分布式容积卡尔曼滤波协作定位方法，其特征在于：所述步骤(7)中，根据量测协方差矩阵和解耦交叉协方差矩阵计算迭代过程中的滤波增益表达式如下：根据t时刻量测消息Y_a,t计算代理状态估计均值和误差协方差矩阵表达式如下：

移动代理节点获得均值向量和误差协方差矩阵表示为自身高斯参数化置信度，随后返回步骤(4)进入下一次高斯参数化置信度传递与迭代计算，完成预定迭代计算次数后返回步骤(2)进入下一轮递归。