异步时钟下基于信息物理融合的水下潜器协同定位算法

闫敬 张立 罗小元 濮彬 关新平

水下信息物理融合系统是集控制、通信和计算于一体的水下智能系统,包括探测与采集、传播与组网、控制与融合等过程.为构建水下信息物理融合系统,需要在特定水域部署静态传感器和动态潜器,其中潜器不仅可作为移动锚节点对传感器数据进行中继转发,也可根据任务需要动态调整姿态以达到网络灵活性的提升.为确保潜器上述功能的实现,需通过传感器的协同来确定潜器的精确位置.

现有定位技术大致分为距离相关技术和距离无关技术两类[1−2].前者定位精度较高、受制因素较少,是本文着力研究的定位技术.目前,一些学者已经对距离相关定位技术进行了研究,并从不同角度设计了协同定位算法.这些算法大多利用到达时间[3]、到达时间差[4]、到达角度[5]和接收信号强度[6]进行距离测量.例如Liu等[7]基于到达时间差设计了多潜器协作的定位算法,实现了移动潜器群的精确定位.Zhou等[8]考虑时空相关性,提出了基于移动预测和到达时间的协同定位算法.Luo等[9]对传感器节点的被动移动进行分析,提出了混合网络下的协同定位算法.贺华成等[10]研究了钻井平台在工作海况下的定位精度和功率消耗.文献[11]考虑区域无解问题,设计了两类传感器节点协作的水下定位算法.文献[12]考虑传感器不能直接获取潜器距离信息约束,设计了直接与间接测量下的融合定位算法.上述定位算法假设节点间的时钟是同步传播的.然而,相比于陆地无线电波通信,声波仍是水下通信最主要的载体[13−14].水声通信具有典型的弱通信特性,例如北斗与全球定位系统并不能直接应用于水下,且受高噪声以及多径干扰等不稳定因素影响,水下节点间的时钟同步难以精确实现.上述异步时钟等弱通信特性使得陆地良好通信条件下的定位算法并不能直接应用于水下.此外,潜器的动力学模型相对于陆地机器人更加复杂.

针对异步时钟下的定位问题,Carroll等[15]提出了基于穷举法的水下请求式异步定位算法.上述算法虽能消除时钟偏移的影响,但是在定位过程中需要不断穷举,使得算法复杂度较高.此外,Liu等[16]设计了水下移动环境下的同步定位算法.Mortazavi等[17−18]对时钟同步与定位进行联合求解,提出了多阶段协同定位算法.同时,文献[19]采用移动预测的方式,设计了异步时钟下的定位算法.根据对象的不同,文献[15−19]分别构建了定位优化问题,为求解上述优化问题,需对非线性测量方程进行近似线性化处理.然而,这种近似线性化易引入模型误差,使其定位精度受限.

本文提出一种基于信息物理融合的水下潜器协同定位算法.设计基于传感器与潜器交互通信的异步定位策略,并分别提出基于扩展卡尔曼滤波(Extended Kalman filter,EKF)与无迹卡尔曼滤波(Unscented Kalman filter,UKF)的协同定位算法.与文献[15,19]类似,本文忽略时钟漂移的影响,仅考虑时钟偏移对定位的影响.主要创新点如下:1)在异步时钟下,建立通信时延与位置的关系,进而定义了潜器协同定位问题;与文献[7−11]不同,本文所提的异步定位策略可以有效消除异步时钟对水下定位的影响.2)提出基于扩展卡尔曼与无迹卡尔曼滤波的协同定位算法.同时,对上述定位算法的有界性以及克拉美罗下界(Cramr-Rao lower bound,CRLB)进行了分析.与文献[15]中的穷举算法相比,本文所提算法复杂度更低,同时与文献[15−19]相比,基于无迹卡尔曼滤波的协同定位算法,不需要将非线性测量方程进行近似线性化处理,其定位精度更高.

1 系统架构与定位策略概述

1.1 水下信息物理融合系统架构

考虑如图1所示的系统架构,其各部分功能如下:

图1 水下信息物理融合系统架构Fig.1 Architecture of underwater cyber-physical system

1)浮标.漂浮在水面,可通过北斗导航或全球定位系统获取自身位置及时钟同步信息,主要作用是协助水下传感器节点进行自定位.

2)传感器.悬浮于水下,可通过浮标的协助获取自身位置,但受噪声等因素影响,时钟并不同步,主要作用是通过信息物理融合获取水下潜器的位置信息.

3)潜器.可看作是合作目标,可通过向传感器广播请求来启动整个定位过程.假设水下潜器配备压力传感器,以直接获取自身深度(即z轴)信息.

1.2 定位策略概述

文献[20]指出,潜器在滚动和俯仰自由度上较稳定.为此,将动力学简化为4自由度方程,其中u,v,w和r分别表示潜器进退、横移、潜浮、偏航角的速度.潜器动力学模型[14]表示如下:

其中,mu,mv,mw分别表示潜器进退、横移、潜浮的质量;Tr表示偏航的惯性力;Fu,Fv,Fw分别表示进退、横移、潜浮作用在潜器上的外力;Ir表示潜器偏航的力矩;W表示潜浮的浮力;为相关系数.

水下潜器的运动轨迹可表示为X1,k=[x(k),y(k),z(k),ϕ(k)],其运动学方程描述如下:

其中,x,y和z分别表示潜器在惯性坐标系北、东和下三个方位的位置;ϕ表示偏航旋转角度;ρ1∈R+为更新步长;惯性坐标系和载体坐标系之间的映射关系可通过欧拉角变换进行转换.定义Xk=[X1,k,X2,k]T,其中X2,k=[u(k),v(k),w(k),r(k)].进一步,结合式(1)和式(2),可得潜器的整体模型.

其中,f表示k−1与k时刻状态的非线性关系;为高斯白噪声,噪声协方差为.

在定位过程中,每个传感器节点有对应的ID号,且彼此知道感知范围内节点的ID.传感器节点可编号为1,2,···,N,且第n个传感器节点的位置可表示为(xn,yn,zn),其中潜器的位置可表示为(x,y,z).为此,定位过程(见图2)可以归纳为如下4步:

图2 传感器与潜器的信息协同过程Fig.2 Cooperation process for the sensors and vehicle

步骤1.在初始时刻ts,s,潜器向传感器节点发送定位请求,其中发起消息包含传感器节点的发送顺序,由n=1,2,···,N表示.随后,潜器进入监听模式,等待来自传感器节点的消息有序传回.

步骤2.在时刻,传感器节点n接收到潜器消息后,节点n进入监听模式并对来自节点1,2,···,n−1的所有消息进行解码.记录到达时间为,其中m=1,2,···,n−1.当节点n接收完节点n−1的消息后,进入发送模式并在时间tn,n发出它自己的消息.节点n发出的时间戳信息包含,以及tn,n.

步骤3.潜器在时刻接收到来自传感器节点n的回复,其中n=1,2,···,N.在接收到传感器节点N消息后,潜器完成本次定位交互过程,其中收集到的时间戳可表示为,.

步骤4.基于上述时间戳测量,潜器结合水下声速,将时间戳信息转化为距离信息,进而利用协同定位算法确定水平位置(x,y).最后,将水下水平位置与深度位置进行结合,确定潜器的位置(x,y,z),整个定位过程结束.

考虑如下的异步时钟模型[15]:T=t+a,其中T表示测量时间,t表示真实时间,a为时钟偏移.

算法目的:构建传播时延与位置之间的交互关联;定义潜器协同定位问题,并根据潜器整体模型(3),提出基于扩展卡尔曼与无迹卡尔曼滤波的协同定位算法,以实现潜器(x,y)位置的准确获取.

2 水下潜器协同定位

2.1 传播时间与位置关系构建

为抵消时钟异步的影响,获取时间差如下:

其中,n=1,2,···,N,m=1,2,···,n−1. 可看出上述时间戳差值都是在相同的节点时钟上做差的,因此上述计算可以消去时钟偏差的影响.

水下潜器与传感器节点n、传感器节点n与传感器节点m之间的距离可分别表示为

假设测量值具有相同精度,且方差为δ2mea.为此,结合式(4)～(6)可获得相应的定位位置与时间测量值的关系.

其中,γs,n=ls,n/c,γm,n=lm,n/c,c为水声传播速度.vs,n和vm,n为测量噪声.根据图2,在定位过程中获取的对应时间戳的传播次数及式(4)和式(5)可以推导出vs,n与vm,n的协方差可以分别为与.

水下潜器异步时钟定位问题可以表示为

注1.相比于超短基线定位方法,本文的定位方法有如下不同:在超短基线定位中,潜器与传感器(或浮标)直接交互,没有考虑传感器间的信息交互,在上述思想下定位问题可定义为,而本文给出的定位问题也考虑了锚节点间的协作,可通过锚节点的协作增加定位的可靠性与精度.

2.2 协同定位算法设计

2.2.1 基于扩展卡尔曼滤波的协同定位算法

基于式(4)～(6),定位时间差可转化为如下距离测量:ZZZk={cTs,n,cTm,n|n=1,···,N.m=1,···,n−1}.由于Zk与潜器的位置(x,y,z)T有关,再根据式(6)～(8)将位置与距离测量构建关联如下:

其中,h(Xk)= [2ls,1(k),···,2ls,n(k),l1,2(k)+ls,1(k)−ls,2(k),···,ls,m(k)+lm,n(k)−ls,n(k)]T;假设vk是方差为Rk的零均值测量噪声.

考虑到穷举法在求解水下时钟异步协同定位时存在算法复杂度较高的问题,本部分用扩展卡尔曼滤波[21]进行定位迭代,进而估计出优化问题的最优解,其中k时刻潜器的位置估计可由表示.

步骤1.在k=1时进行初始化,得E[X0],.

步骤2.基于式(3)对潜器位置进行状态预测

步骤3.基于式(6)和式(9),可知在k时刻测量方程关于状态位置Xk的雅克比矩阵为.与其类似,状态方程f关于Xk−1的雅克比矩阵可定义为.进而,在k时刻估计协方差矩阵表示为Pe,k,同时状态预测协方差矩阵Pe,k|k−1更新为

步骤4.估计协方差更新为

卡尔曼增益Ke,k为

状态更新方程为

算法1.扩展卡尔曼协同定位算法

2.2.2 基于无迹卡尔曼滤波的协同定位算法

上述算法需求解雅克比矩阵,易引入模型误差且求解繁琐.为此,采用无迹卡尔曼滤波[22]进行协同定位,其中k时刻潜器的位置估计可由表示.

步骤1.在k=1时,进行如下初始化:.

步骤2.基于状态估计Xu,k−1和误差协方差矩阵Pu,k−1,选择Sigma点如下:

其中,χj,k−1是Xu,k−1的第j+1 列,j=0,···,2L;;其中L=8;表示矩阵的第列.

步骤3.根据状态方程(3)预测下一时刻状态χj,k|k−1=f(χj,k−1),并加权处理后得

协方差矩阵更新为

然后由式(9)计算自身位置测量值的无迹变换.

步骤4.测量协方差矩阵和状态值测量交互协方差矩阵更新为

步骤5.通过以下更新规则来更新卡尔曼增益、协方差和估计状态.

算法2.无迹卡尔曼协同定位算法

3 性能分析

3.1 算法有界性分析

虽然一般卡尔曼估计均方误差(Mean squared error,MSE)已被证明有界[23],但在对水下异步时钟定位进行稳定性分析上有所不同.由文献[24]可知,滤波算法有界性分析需要在线性化后的系统矩阵Fe,k−1和测量矩阵He,k的基础上进行.基于此,可导出伪系统矩阵Fu,k−1和测量矩阵Hu,k.考虑到扩展卡尔曼滤波与无迹卡尔曼滤波的证明类似,下文以无迹卡尔曼滤波为例证明.Hu,k可以直接计算得到,.进一步,伪系统矩阵Fu,k−1可定义为Fu,k−1=,其中,.

与文献 [23]类似,引入对角矩阵αk=diag(α1,k,α2,k,···,αn,k) 和βk=diag(β1,k,β2,k,···,βn,k)对状态和观测进行补偿,则式(3)和式(9)可更新为

进而,可得出预测误差协方差、更新误差协方差和无迹卡尔曼滤波增益如下:

为给出估计有界性证明,参考如下引理[25].

引理1.假设ζk是随机过程,且存在V(ζk)和正实数vmin,vmax且0<λ≤1,这样对于∀k,有

若满足上式则该随机过程以均方为界表示为

引理2.如果矩阵A>0且C>0,可知A−1>B(BTAB+C)−1BT.

基于上述引理,给出如下有界性定理.

定理1.基于状态方程(3)和测量方程(9),并结合算法2,可得出结论如下:

1)存在不为零的实数αmin,βmin,fmin,hmin,αmax,βmax,fmax和hmax,使得对于任何k≥0满足如下关系:以及;

证明.由文献[26]得,预测和估计误差分别定义为.进而,预选李亚普洛夫函数如下：

综合式(27)和式(28)得到

其中

由式(24)知,

结合式(26),(27),(29),可得

由引理2可知,

从式(31)可知,0<λ<1,根据定理的两个条件可得

则式(30)可改写为

最终可知

由式(33)推导,根据引理1,可知Xu,k+1|k均方根有界,根据式(22)可变形为

两边同时求期望,可得

3.2 克拉美罗下界(CRLB)

令Mk={X0,X1,···,Xk},Nk={ ZZZ0, ZZ1,···,ZZk}.P(Mk,Nk)对应的联合概率密度为(Mk,Nk),则克拉美罗估计误差的形式为

进一步,Jk可定义为

其中,

在高斯白噪声假设下,由式(3)和式(9)可知

进而可得

综上,式(35)可更新为

4 仿真分析

使用4个传感器对潜器定位.设定状态方差为Q=0.01I,初始协方差矩阵为P=0.2I,测量方差为R=diag(3,3,3,3,4.5,4.5,4.5,4.5,4.5,4.5).潜器初始位置为(500m,200m,−100m).传感器位置分别为(600m,100m,−100m),(400m,300m,−110m),(600m,300m,−90m)以及 (550m,150m,−100m).

1)传感器与潜器信息物理交互过程分析

图3给出了同步测量(文献[7−8,11])与异步测量(本文)方式下的距离比较.为更加直观显示距离精度,图4给出了同步与异步测量下的距离误差.从图3和图4可以看出,本文所提的异步定位策略可以有效消除异步时钟对水下定位的影响.

2)协同定位算法精度比较

从图5可以看出,基于无迹卡尔曼滤波的定位算法精度更高.为更清晰地表述上述判断,图6给出了上述3种定位算法下的均方误差(MSE),可以看出扩展卡尔曼与无迹卡尔曼滤波下的定位精度比穷举法下定位精度高,基于扩展卡尔曼滤波的定位算法精度没有无迹卡尔曼滤波下定位精度高.图7给出了两种滤波下的MSE,可看出无迹卡尔曼滤波更接近CRLB,其定位精度更高.

3)协同定位算法仿真时间比较

图8给出了三种方法下的定位过程持续时间.可以看出,相比于扩展卡尔曼与无迹卡尔曼滤波算法,基于穷举法的定位算法需要更长的时间达到所需要的定位精度.同时,基于扩展卡尔曼与无迹卡尔曼滤波的定位算法仿真时间相差不大,但是基于无迹卡尔曼滤波的定位算法定位精度更高(见图6和图7).

4)误差统计分析

以无迹卡尔曼滤波定位算法为例,设定三种干扰场景.场景2下的干扰强度大于场景1下的干扰强度,而场景3下的干扰强度大于场景2下的干扰强度.考虑上述场景,图9和图10分别给出了潜器的定位轨迹与定位误差.可以看出,在上述场景下潜器尽管可以完成定位任务,然而场景1下的定位精度最高,而场景3下的定位精度最差,即干扰强度的增大降低了定位的精度.图11给出了测量距离误差的统计特性,可以看出干扰越小距离误差分布越接近于零.

5 结论

针对异步时钟下的潜器定位问题,提出了一种基于信息物理融合的水下潜器协同定位算法.建立了通信时延与位置的关系,并定义了潜器协同定位问题.为求解上述优化问题,提出了基于扩展卡尔曼与无迹卡尔曼滤波的协同定位算法,并对上述定位算法的有界性以及克拉美罗下界进行了分析.最后,通过仿真对比验证了本文所提方法的有效性.

图3 同步测量与异步测量距离比较Fig.3 Distance comparison with synchronous and asynchronous measurements

图4 同步测量与异步测量距离误差比较Fig.4 Distance error comparison with synchronous and asynchronous measurements

图5 穷举法、扩展卡尔曼和无迹卡尔曼定位轨迹Fig.5 Trajectories with exhaustive,EKF-based and UKF-based methods

图6 穷举法、扩展卡尔曼与无迹卡尔曼MSE比较Fig.6 Comparison of MSE for exhaustive,EKF and UKF based methods

图7 扩展卡尔曼与无迹卡尔曼MSE与CRLB比较Fig.7 Comparison of MSE and CRLB for UKF and EKF-based methods

图8 算法仿真定位用时比较Fig.8 Localized trajectories of underwater vehicle different interference intensities

图9 不同干扰强度下潜器定位轨迹Fig.9 Positioning trajectories of submersible under different interference intensities

图10 不同干扰强度下潜器定位误差Fig.10 Positioning errors of underwater vehicle under different interference intensities

图11 误差统计分析Fig.11 Analysis of statistical error