应用消息传递和卡尔曼模型的无线传感网定位跟踪算法

赵 恒,袁正道,,孙 鹏,张园园,吴 胜

(1.河南开放大学 信息工程与人工智能学院,郑州 450002;2.郑州大学 电气与信息工程学院,郑州 450001;3.郑州轻工业大学 电子信息学院,郑州 450001;4.北京邮电大学 信息与通信工程学院,北京 100867)

0 引 言

在无线通信和物联网等新一代信息技术的浪潮下,无线传感网[1](Wireless Sensor Network,WSN)已经融入到生活和工业的各个角落。WSN系统将传感节点嵌入到目标物中,测量物体参数信息,发送给处理终端收集。但通常传感器节点采集到的只有目标物的状态参数,如果能结合定位,则能挖掘出更有价值的信息,因此无线定位也成为了WSN系统的关键技术之一[2]。WSN系统经常部署于室内、战场、密林等复杂环境,对定位系统的可靠性有较高要求。此外,待测目标物(如车辆、行人)会不断移动,如果能利用连续时间的位置信息(即跟踪定位),则能极大提升定位的准确度和可靠性。

目前较常用的短距离无线测距、定位方法有射频身份识别、超宽带[3](Ultra-wide Band,UWB)、WiFi、蓝牙等。定位算法的基础原理是利用信号强度[4]、到达时间(Time of Arrival,TOA)[5]测量目标物与已知位置节点(也称之为锚节点)的距离,通过三角定位、最小二乘估计(Least Square,LS)[6]、最小均方误差(Linear Minimum Mean Square Error,LMMSE)、质心算法[7]等方法计算待测目标物的坐标。从算法的角度看,以上算法均属于非贝叶斯类算法,忽略了测量误差和噪声的统计特性,未利用节点的先验信息和方差,在干扰较大的场景下会引起较大的性能损失[8]。

消息传递算法[9]是变分贝叶斯推理类方法的典型代表,近年来在图像处理、信道估计、接收机设计[10]、信号处理[11]等领域得到了广泛应用。

近年来国内外研究团队提出了多种基于因子图和变分消息传递的贝叶斯类定位算法[12]。文献[13]研究了基于贝叶斯测距和LS的定位算法,在测距阶段使用变分贝叶斯方法消除噪声干扰,在定位阶段使用LS方法估计坐标。文献[14]提出了一种基于概率图模型的单信标定位算法,能够在信标数据缺失的情况下联合其他位置的测量信息实现目标定位。上述两种方案利用了部分先验和方差信息,但是未考虑连续时间内的相关性。文献[15]研究了基于因子图的动态无线网络协作定位算法,利用无线传感网中锚节点和多个定位节点互通实现了高精度定位。但是该方案需要整个网络中所有节点参与,复杂度较高,很难在实际场景中得到应用。

为解决前述无线传感网中的节点定位和跟踪问题,本文提出了一种融合了消息传递算法和卡尔曼[16-17]模型的解决方案,并搭建了基于超宽带的测距跟踪系统,采集实际测距数据,对所提算法和现有算法进行实测对比验证。

本文的创新点可以归纳为如下两个方面：一是将定位与卡尔曼模型融合,建立统一的概率分解和因子图模型,利用连续时间内坐标的相关性实现了定位跟踪;二是对卡尔曼模型参数进行实时更新,使其能够适用于更通用的场景。本质上卡尔曼模型是将前一时刻定位结果作为本次定位的先验信息。本文所提方法是将定位和卡尔曼模型融合为一个整体,利用消息传递算法进行更新,每一步计算都包含了方差项,而方差是对估计值的不确定度,所形成的迭代算法更稳定。该算法的性能增益也在仿真和实测环境中得到了验证。

1 系统建模

假定t时刻待测节点观测到周围N+个锚节点的位置信息(xn,yn,zn),n=1：N+,该节点t-1时刻估计得到的位置为(x(t-1),y(t-1),z(t-1)),如图1所示。

图1 动态定位示意Fig.1 Schematic diagram of dynamic positioning

t时刻待测节点与第n个锚节点之间的欧氏距离计算为

(1)

根据待测节点的位置不会发生突变,可设定阈值剔除异常距离测量,假定剔除后的锚节点数量为N。设节点真实坐标为(x(t),y(t),z(t)),将其向量化为

α(t)≜(x(t),y(t),z(t))T

(2)

(3)

移项可得

(4)

将上式整理为观测模型(5)(文献中也称之为线性混合模型)：

r(t)=Φ(t)α(t)+ω(t)

(5)

式中：

(6)

矩阵Φ(t)∈(N-1)×3定义为

(7)

由于坐标x,y,z完全对等,此处以x为例进行计算,并不失一般性。假定连续的观测时间t内坐标具有相关性,表达式如下：

(8)

(9)

(10)

式中：U(a,b)表示上下界为[a,b]的均匀分布。

将式(5)中的矩阵运算,利用全概率公式和变量间的马尔可夫特性,将后验概率分布P(r,α,Φ,A,B)分解为似然函数与先验概率相乘的形式：

P(r,α,Φ,A,B)∝

P(α(0))P(A)P(B)=

(11)

(12)

表1 因式分解与函数表达式Tab.1 Factorization and corresponding function

将式(11)所示的因式分解和表1所示函数表达式用因子图形式表示,如图2所示。

图2 因式分解(11)对应因子图Fig.2 Factor graph corresponding to Equation (11)

2 算法推导

图3 坐标x的相关因子图Fig.3 Factor graph related to coordinate x

2.1 晶格部分消息更新

(13)

(14)

(15)

2.2 卡尔曼部分消息更新

(16)

(17)

(18)

(19)

(20)

在消息传递算法中,置信的物理意义为该变量的联合后验概率,即式(20)中得到了在t时刻待测节点坐标的估计。

2.3 卡尔曼参数估计部分消息更新

(21)

(22)

(23)

(24)

(25)

(26)

由于坐标x,y,z完全独立,则可以根据上述x坐标的估计步骤类比得到坐标y,z的更新方法。

2.4 消息迭代算法归纳

本节将前述因子图中所有消息迭代计算、初始化和迭代次序、结束条件等归纳为消息迭代算法。由于x,y,z坐标完全对等,在算法中仅以x坐标为例,坐标y,z可以类比得到。

自适应卡尔曼滤波定位算法伪代码如下：

Fort=1：Q

For Iter=1：TGlob

10 ∀n,由式(20)更新置信b(x(t));

11 ∀n,类比得到对坐标y,z的估计;

end For

end For

2.5 复杂度分析

本文所提基于消息传递和卡尔曼滤波的定位算法复杂度较低,每次迭代仅需要计算有限次的乘加运算和指数运算。针对t时刻单次迭代的复杂度分析如下：晶格部分消息更新所需乘-加运算次数为O(3N),N表示锚节点个数;卡尔曼滤波部分与锚节点无关,仅需要O(3)次乘-加运算;卡尔曼参数估计部分,假设参数A,B与最近的Q次观测相关,则其复杂度为O(3Q)。

3 仿真性能对比

建立一个长宽均为5 m、高为1 m的虚拟空间,空间的内壁上以1 m为间隔部署锚节点。为模拟真实环境,假定在安装锚节点时存在一些装配误差(误差服从[-0.05 0.05]m的均匀分布)。为测试跟踪算法的有效性,仿真中设待测目标沿x,y平面对角线匀速移动,同时在z轴上以正弦周期移动,运动速度为1 m/s。此外,假定目标物的移动速度还加入了服从[-0.05 0.05]m/s均匀分布的随机抖动。图4给出了虚拟空间和目标物示意图,图中星号代表在上下墙壁上部署的锚节点,蓝色轨迹代表待测目标物的运动曲线。

图4 锚节点部署位置和待测设备移动轨迹Fig.4 Anchor node position and the device moving path

(27)

式中：wn为白高斯噪声;测距信噪比为4～16 dB。仿真中选择有效锚节点个数为4～12,测量间隔为5次/s。为了验证算法的可靠性,仿真中模拟50次独立测量,然后对估计结果进行平均。为验证本文所提算法的有效性,仿真中还与文献中已有的质心算法、三点质心法、最小二乘法、最小均方误差(LMMSE)方法进行对比。算法的定位精度由均方误差(Mean Square Error,MSE)衡量,仿真中定义

(28)

图5展示了本文所提算法的定位MSE随测距信噪比的变化曲线,可以看出,本文所提自适应定位算法能够极大地提升定位精度。当有效锚节点N=4时,相对于最小二乘估计,本文所提自适应算法性能仅提升0.5 dB;而当N为6～8时,性能提升可达4 dB,充分验证了自适应卡尔曼预测的有效性。

图5 定位精度随信噪比变化曲线Fig.5 Positioning accuracy varies with SNR

为了更直观展示定位性能与锚节点个数的关系,图6给出了定位精度随N的变化曲线。从图6中可以看出,锚节点个数为4时,本文所提自适应算法对精度提升有限;当锚节点为6时,性能增益约为4 dB;当锚节点个数增加至8以上后,本文所提算法相比最小二乘估计和最小均方误差估计的性能增益稳定在2 dB。

图6 定位精度随有效锚节点个数变化曲线Fig.6 Positioning accuracy vs. anchor nodes

4 实测性能对比

为了验证本文所提算法的有效性,在室内搭建了基于超宽带的室内二维定位跟踪系统,利用实测数据进行测试。选择的硬件系统为广州联网科技的室内超宽带定位模组DWM-PG+,包含6个锚节点和1个待测节点。锚节点横向间隔2.5 m,纵向间隔2 m。为保证定位标签匀速运动,将定位标签安装于寻迹小车上,以约0.23 m/s的速度匀速沿黑色Z字形轨迹运动,如图7所示。

图7 基于超宽带的室内定位系统实测场地Fig.7 Field measure of positioning system based on UWB

定位标签的测距间隔为40 ms,即测距频率为25 Hz。每个锚节点独立测量与定位标签的距离,汇聚到E基站后通过串口上传至上位机。图8为定位系统的上位机软件,通过串口接收所有锚节点的测距结果,通过最小均方误差和卡尔曼滤波得到目标节点坐标并显示。为了计算各种算法估计坐标与真实坐标比较的均方误差值,对如图8所示的实测数据进行截取,仅保留图中3段直线位置数据,使得真实坐标(x(t),y(t)),t=1：T能够计算得到。

图8 定位跟踪系统上位机软件Fig.8 Host software of positioning and tracking system

(29)

经过5次实测计算取平均得到的MSE值如表2所示,可以看出,除质心类算法以外,其他算法的定位MSE均达到-5 dB以上,实测定位误差也均在厘米级,完全能够满足室内定位的需要。由于基站安装位置偏差、寻迹行走偏差、叠加噪声等因素,实测定位的MSE并未达到仿真中的-20 dB,但是从表2中仍能够看出本文所提定位算法能够有效提高定位跟踪精度。

表2 实测数据MSE对比Tab.2 MSE comparison of field measured data

从实测数据可以看出,由于本文所提方法利用卡尔曼滤波的结构发掘相邻时刻的定位信息,即将上一时刻的位置作为先验信息提供给本时刻的定位估计,能够获得额外的性能增益。以上结论也在本文的仿真和实测中得到了验证。

5 结 论

为解决WSN系统中的节点定位问题,本文提出了一种融合了卡尔曼模型和因子图的跟踪定位算法,充分发掘相邻观测坐标中所隐藏信息,作为定位的先验信息提供给本次位置估计。由于将定位、卡尔曼滤波和参数估计作为整体进行优化,相较于分开处理类方法,本文所提算法能够做到全局最优。在仿真中,本文建立虚拟环境对所提算法和文献中已有方法进行了对比,结果表明本文所提算法具有2 dB的性能增益。最后搭建室内实测环境,采集6个锚节点的实时测距结果并对比了所有算法的定位结果,实测数据也验证了本文所提算法的性能优势。