基于移动补偿的动态水下传感节点定位算法

随着海洋资源开发和环境监测等领域受重视程度的提升,水下传感网络以其海洋观测和探索的便利性逐渐受到研究人员的关注,发展水下传感网络的重要意义之一正是为水下节点提供优质的定位导航服务[1]。水下节点按照机动能力可以分为动态节点和静态节点两种,以自主水下潜航器(Autonomous Underwater Vehicle, AUV)为代表的动态节点以其优越的机动、隐蔽和多任务等特点正逐渐成为探索海洋的重要力量,精确的定位与导航信息对其的重要性不言而喻[2]。近年来,水下传感网络对于水下节点定位技术在时钟同步[3]、声速补偿[4-7]、定位协议[8-10]等方面取得了巨大的进展。但现有的常规水声定位算法应用于以多待定位节点定位为应用需求的水下传感网络中时,存在以下两点问题:一是待定位节点较多的情况下,所有的待定位节点依次发出定位请求和信标节点向各待定位节点返回定位结果会造成信道冲突。二是众多待定位节点的时钟同步问题较难解决。

对此,Cheng[11]等首次提出了静默定位(Underwater Positioning Scheme, UPS)算法,各信标节点依次发送定位信息,待定位节点只需监听并解析各信标发来的信息,凭借自身时钟测量到达时间差就可以得出自身位置。该算法无需节点间时钟同步且定位时长与通信负担仅与信标节点数量有关,很好地解决了上述问题。但该算法仅就均匀声速分布情况进行讨论,未考虑声速分布不均匀对定位效果的影响;此外算法在降阶法求解时引入了附加条件,使定位范围存在盲区。Patrick[12]等首次通过试验验证了UPS算法的可行性。Tan[13]等尝试增加辅助信标以缩小UPS算法的定位盲区,但并没有从解算方法上彻底解决此问题,且提高了应用成本和难度。

综上所述,经典静默定位方面的算法存在的问题可以归纳为以下几个方面:定位盲区问题没有被彻底地解决、未考虑声速分布不均匀因素和信标阵型不理想导致算法定位失效。针对上述三个问题,文献[14]提出了一种静态水下传感网络定位算法(Silent Location algorithm for Static underwater sensor network based on Ray Acoustics, SSL-RA)。结合高斯-牛顿法和声线跟踪技术,采用迭代-跟踪交替进行的方式,克服了静默定位模型与声线跟踪不兼容的问题,解决了UPS算法存在的定位盲区和未补偿声速的问题。同时采用Tikhonov正则化来解决算法迭代发散问题。由于该算法需要待定位节点依次发送定位信息,定位周期较长,当待定位节点移动时,其接收到各定位信号的位置相距较远,会引入较大的定位误差,因此该算法不适用于动态节点的准确定位。

对于此类问题,常见的解决方法包括几何方法[15-16]、反演方法[17]和卡尔曼滤波类方法[6,18-20]。其中,几何方法通过将信标节点与待定位节点移动轨迹建模为椭圆模型,通过解椭圆上的几何关系引入时延修正量,以消除待定位节点移动对定位精度的影响。但几何方法和反演方法都要求观测得到的时延信息为绝对传播时延,即通过应答式定位获得双程传播时延[15-17],因此仅适用于长基线球面交汇等较为简单的定位模型,不适用于几何关系较为复杂的静默定位类算法。卡尔曼滤波类方法中,文献[6,18-19]通过节点间时钟同步获得单程传播时延,并对其进行卡尔曼滤波处理以消除潜器运动带来的定位误差。Bahr[20]等通过将航位推算等设备提供的航向航速信息融合进卡尔曼滤波过程,补偿潜器运动对协同定位精度的影响。但是,水下传感网络定位的静默定位协议只能获得待定位节点接收到各定位信号时的自身时钟时刻,无法像以往定位方法一样获得绝对的传播时延,且定位协议较为特殊,因此上述方法不完全适用。

本文从理论和数值层面研究了待定位节点运动对SSL-RA算法定位精度的影响,针对静默类定位算法只能获得基于待定位节点自身时钟时刻获取信号到达时刻的特点,建立了其观测量随定位周期的变化模型,提出了一种结合移动补偿的动态水下待定位节点静默定位算法(Silent Location algorithm for Dynamic underwater sensor nodes based on Mobility Compensation, DSL-MC)。对于来自各个信标节点的定位信号,根据当前周期观测到的到达时刻和已有观测记录,通过卡尔曼滤波器对其随定位周期的变化状态进行估计,掌握其变化规律。对于当前周期内,根据其变化规律,以待定位节点接收到定位发起信号位置为基准,对各信标节点定位信号到达时刻进行补偿,实现周期内共点观测定位信号。随后利用此补偿结果,结合SSL-RA算法对待定位节点自身位置进行解算,得到其接收到第一个信标信号的位置。最后通过仿真分析,验证了所提算法的有效性。

2 静态水下传感网络定位算法

2.1 定位协议

假设待定位节点T的通信范围内存在N个信标节点Bi,i=1,2,…,N,其三维坐标分别表示为Pt=[xt,yt,zt]T和Pi=[xi,yi,zi]T,i=1,2,…,N。

信标节点B1周期性发起定位,广播包括信标节点B1当前的时钟信息、经纬度和深度等信息的定位信号,待定位节点T接收到来自信标节点B1的定位发起信号时,自身时钟时间为m1;信标节点B2在接收到来自信标节点B1的定位发起信号后,广播包括其当前的经纬度、深度以及处理延迟τ2等信息的定位信号,待定位节点T接收到来自信标节点B2的定位信号时,自身时钟时间为m2;后续的信标节点也依次按此方式工作。最终待定位节点T获得基于自身时钟的时间观测量{mi,i=1,2,…,N}和各信标节点的处理时间{τi,i=2,3,…,N}。定位协议如图1所示。

2.2 解算方法

水体中温度、盐度和压力等参数是不均匀分布的,进而水下声速呈不均匀分布。因定位区域相对海洋尺度较小,水体温度和盐度等参数在此区域内的水平方向变化较弱,故本文只考虑声速的垂直分布,暂不考虑声速的水平分布[21]。按照声线理论,声波传播的时间与声源和接收端位置之间的关系是关于声源与接收端位置的函数,对应关系由声线跟踪算法给出,即

式中,tp,q,p,q=t,1,2,…,N为声信号从节点p到节点q处的传播时间。在水下环境给定的情况下,任意信标节点位置Pi和待定位节点位置Pt对应唯一的传播时间ti,t。因此,求解待定位节点位置Pt的问题可以等效成搜寻最符合观测数据的一组传播时间{ti,t,i=1,2,…,N}集合问题。

以观测方程为核心构建目标函数,采用最小化问题表示此问题

式中,F(Pt)=[F2,F3,…,FN]T为估计向量,其中的元素Fi=ti,t-t1,t;R=[R2,R3,…,RN]T为观测向量,其中的元素

为声信号从信标B1发出,最后经由信标Bi到达待定位节点T与信标B1直接到待定位节点T的传播时间差;

为声信号从信标B1发出,最后经由节点Bi到达待定位节点T期间内,在各信标节点之间的传播时间。

采用高斯-牛顿迭代方法即可求解式(2)的最小二乘问题。实际应用中,由于各方面条件限制,经常会出现信标节点组成的定位阵型不理想等情况。为防止求逆矩阵奇异,导致算法发散,采用Tikhonov正则化解决此问题。状态向量的第k次迭代过程为

式中,αk为迭代步长,通过Armijo准则即可选取[22];η为正则化参数,合适的η可降低求逆矩阵条件数,令算法收敛;

为

时G(Pt)的雅克比矩阵,文献[14]已经给出了其详细表达式,此处不再赘述。

理想情况下,

时,

即为待定位节点的真实位置。为提高计算效率,设定迭代终止判决门限ε。目标函数若小于ε,则迭代终止,此时的

即认为是待定位节点估计位置。其中,ε表征算法对待定位节点位置坐标的估计精度,可根据实际需求选取。

对Tikhonov正则化过程进行改进,根据正则化效果,调整正则化参数。设置矩阵病态程度判决门限ξ,若当前条件数小于门限ξ,则无需进行正则化,即η=0;若当前条件数大于ξ,则需要进行正则化,即η≠0。若根据当前正则化参数更新的状态

使得目标函数

不变甚至升高,则正则化参数需适当增大κ倍,直到更新的状态

使得目标函数降低为止。这里正则化参数的变化参数κ根据经验取正数即可。

考虑到水下传感网络的深海应用场景,在信标节点以5 km边长的矩形分布情况下,一个定位周期至少需10 s时间。即使待定位节点以2节低速运动,则待定位节点接收到来自信标节点信号的时间最长将相距10 m以上,这无疑会对其位置估计精度产生一定的影响,因此本文对待定位节点运动进行补偿,提出了适用于动态水下传感节点的静默类定位算法。

3 本文提出的动态水下传感网络定位算法

待定位节点运动情况下,相邻定位周期内的定位场景如图2所示。其中,

为第n周期,待定位节点接收到信标Bi信号时的位置。可见随着待定位节点的运动,其接收到各信标信号的位置也不同,这必然会给SSL-RA算法的定位精度造成一定的影响,因此需引入移动补偿机制。

3.1 观测量状态估计

首先,对来自各信标的信号到达时刻的变化状态进行估计。假设第n周期,信标B1于

时刻广播定位信号。由于水下传感网络应用场景下的水下潜器一般运动速度和转向速度较小,因此可合理地假设在相邻定位周期之间,待定位节点的运动为低匀速运动。在声线跟踪精确度较高的前提下,声信号从各信标节点传播至待定位节点的时间仅与待定位节点的运动速度大小和航向有关。航向一定时,定位周期内声信号传播时间符合运输变化规律;转向时,航向与转向半径共同决定声信号传播时间变化规律,低航速下,待定位节点的加速度十分小,可以忽略不计[18]。因此可以假设各声信号传播时间

的变化符合匀加速(Constant Acceleration, CA)模型,构建状态方程有

其中,

为第n个定位周期各声信号传播时间

的变化状态;

为其转移矩阵;w为过程噪声。

将声信号传播时间

的定义代入式(4),可以得出

与

变化状态的对应关系,有

其中,

为定位周期,一般为常数,若定位周期不恒定,(n-1)δ项可转换成求和形式。可见,对于相邻定位周期,

与

的变化规律大致相同,区别只在于一个常数项。其对应的观测方程为

其中,

为其观测矩阵;u为观测噪声。

根据卡尔曼滤波模型有

其中,

为

时刻

状态的预测值;

为其协方差矩阵的预测值;

为

状态的估计;

为协方差矩阵的估计。

采用此卡尔曼滤波器,可以估计出每个观测值

的变化状态,以便后续对观测时刻进行补偿。

3.2 观测时刻补偿

若需消除待定位节点运动对定位算法产生的影响,就必须以同一位置为基准获得各观测量。为了与SSL-RA算法相对应,本文的做法是对于每个定位周期,利用上一节对各观测值变化状态的估计,将待定位节点的各观测量从

补偿至

位置处。在一个定位周期内,假设每个信标发射信号的到达时刻的变化也符合CA模型,其状态方程可以表示为

其中,

为第n个定位周期待定位节点于

位置接收到信标Bi信号时刻

的变化状态;

为其状态转移矩阵。值得注意的是,

且

向量的后两项与上一节卡尔曼滤波器估计出的

向量的后两项相等,这也正是上一节进行卡尔曼滤波的意义。

至此,待定位节点于每个周期对各信标信号到达时刻的测量值均已被补偿至对应周期的

位置处,消除了待定位节点运动对算法定位精度的影响。

3.3 算法流程

该算法流程如下:先根据前序周期的观测信息,对当前定位周期观测到的信号接收时刻

的变化状态进行估计,得出其变化规律;再将本周期各接收时刻

统一补偿至

位置,得到待定位节点于同一位置接收到各信标信号的时刻

最后利用得到的本周期一组

采用SSL-RA算法对待定位节点位置进行解算。

完整的DSL-MP算法流程如图3所示。

4 误差分析和仿真试验

4.1 待定位节点运动对SSL-RA算法的影响

设置四个信标节点的位置分别为(-2500,2500,3000) m、(-2500,-2500,2010) m、(2500,-2500,4000) m和(2500,2500,4050) m,待定位节点起始位置为(1000,500,100) m,为使显示效果更易于观察,暂设其向西以10 m/s的速度前行,同时以1 m/s的速度下潜。环境水深5000 m,声速剖面为夏季南中国海某处声速剖面,如图4所示。正则化参数η的初始值设置为10-7,κ设置为10。

目标函数

在待定位节点深度平面和X-Z垂直平面的分布及其在待定位节点附近的局部放大分别如图5和图6所示,其中等高线汇聚的位置即为SSL-RA算法估计出的待定位节点位置;星型标识位置为待定位节点真实起始位置;圆形连线标识表示待定位节点接收到各信标定位信号时的位置。

可以看出,在三维空间中,目标函数虽仍存在极小值点,但已偏离待定位节点运动轨迹上任意一点。若仍将SSL-RA算法迭代收敛得到的极小值点作为待定位节点的位置估计值,则其结果会存在较大的误差。

定义误差考查指标误差斜距比

式中,

为待定位节点位置的估计值;Pt为真实值。

本例中,估计位置与接收到各信标信号位置的距离分别为41.9301 m、20.0048 m、33.4048 m和59.7169 m。虽然待定位节点运动速度较大,但在不考虑其他定位精度影响因素的情况下,仅待定位节点运动造成的最小水平误差斜距比γ就达到了0.44%,这样的定位误差是不可忍受的。

下面考查待定位节点运动对SSL-RA算法定位精度的影响,即求取偏导数∂Pt/∂v=[∂xt/∂v,∂yt/∂v,∂zt/∂v],其中v=[νx,νy,νz]T表示待定位节点的三维空间运动速度。下面给出∂xt/∂v的推导过程及解析表达式。

对于偏导数∂xt/∂v,有

对式(2)取关于xt的偏导有

其中,Gi,i=1,2,…,N-1表示G(Pt)向量中第i个元素,偏导数

可以写成

由射线声学理论有

对式(2)取关于v的偏导有

其中,偏导数∂Gi/∂v可以写成

与∂ti,t/∂xt的推导过程类似,∂ti,t/∂v可以写成

将式(15)和式(18)代入式(14),有

偏导数∂Pt/∂v中另外两个元素∂yt/∂v和∂zt/∂v的求解方法与∂xt/∂v同理。

待定位节点速度的获取方式有很多种,包括配备惯性导航系统(Inertial Navigation System, INS)、定位结果差分以及卡尔曼滤波器等。设置待定位节点初始位置不变,向西以不同速度运动,本文SSL-RA算法和UPS算法受待定位节点运动的影响如图7所示。可以看出,随着待定位节点运动速度的增大,SSL-RA算法的位置估计精度迅速降低,并向UPS算法的估计精度逼近。这也说明了使用本文DSL-MC算法补偿待定位节点运动的必要性。

4.2 DSL-MC算法效果

按上节仿真条件,待定位节点起始位置不变,运动速度设置为向西2 m/s,信标节点B1广播定位信号的周期为20 s,其他条件不变。

设置观测量初始状态

使用DSL-MC算法对30个周期内待定位节点的位置进行估计。待定位节点对各信标信号到达时刻观测量的补偿误差如图8所示。可以看出,输入的初始状态中观测量变化速度与加速度的不准确,导致了算法起初对观测量补偿误差较大;随着时间的推移,补偿误差经过一段时间的震荡后,逐渐变小。渐渐地,DSL-MC算法的移动补偿部分成功地将各观测值补偿至

处,使用得到的补偿值对待定位节点进行位置估计,将较准确地得到待定位节点接收到信标B1信号的位置。

这30个周期中,使用经对准的观测量进行定位的DSL-MC算法和使用原始观测量进行定位的SSL-RA算法的定位误差对比如图9所示。结合图8可以看出,由于输入卡尔曼滤波器的观测量初始状态存在偏差,第一周期DSL-MC的移动补偿部分没有对用于解算位置的观测量发生影响,DSL-MC算法的定位误差并没有降低;但随着时间的推移,DSL-MC算法对观测量变化规律的掌握越来越全面,进而对观测量的补偿越来越准确,算法对待定位节点运动补偿的准确性也逐渐提升,定位误差相应地快速降低。而SSL-RA算法由于没有考虑待定位节点运动的影响,定位误差始终较大。

下面考察待定位节点运动速度对DSL-MC算法定位精度的影响。结合图8和图9可以看出,DSL-MC算法在10周期以后,观测量补偿误差波动幅度明显变小,进入稳定阶段,故此处取第10～20周期定位结果的平均值来考查算法的定位误差。设置待定位节点初始位置不变,以不同速度向西运动。以基于椭圆模型的运动补偿算法作为对比,SSL-RA算法与DSL-MC算法的定位误差如图10所示。可以看出,随着待定位节点运动速度的增大,SSL-RA算法的定位误差逐渐增大;基于椭圆模型的运动补偿算法的定位误差有所下降,但由于无法与几何关系较为复杂的静默定位协议完美匹配,仍存在一定的定位误差,无法完全消除待定位节点运动对定位精度的影响;而DSL-MC算法的运动补偿方法不受静默定位协议的几何关系影响,因此其定位误差并没有随待定位节点运动速度的增大而增大,始终保持较低的水平。可见,DSL-MC算法对待定位节点运动的补偿效果不受其运动速度的影响,且补偿效果较好。

5 结论

针对现有适用于水下传感网络的静默类定位算法在待定位节点运动情况下定位误差较大情况,本文分析了待定位节点运动引起定位误差的机制,建立了原始观测量随定位周期的变化模型,提出了一种结合移动补偿的动态水下传感节点定位算法。该算法通过对待定位节点接收到各信标节点信号时刻随定位周期的变化规律进行估计,并将各观测量补偿至对应周期内待定位节点接收到定位发起信号的位置处,实现周期内共点接收定位信号,再结合已有的SSL-RA算法完成待定位节点位置的解算。仿真分析表明,本文算法在卡尔曼滤波器进入稳定状态后,能够较好地补偿待定位节点运动对SSL-RA算法的影响。

[1] EROL-KANTARCI M, MOUFTAH H T, OKTUG S. A survey of architectures and localization techniques for underwater acoustic sensor networks[J]. IEEE Communications Surveys & Tutorials, 2011, 13(3): 487-502.

[2] PAULL L, SAEEDI S, SETO M, et al. AUV navigation and localization: A review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1): 131-149.

[3] RAMEZANI H, JAMALI-RAD H, LEUS G. Target localization and tracking for an isogradient sound speed profile[J]. IEEE Transactions on Signal Processing, 2013, 61(6): 1434-1446.

[4] LIU Jun, WANG Zhaohui, CUI Junhong, et al. A joint time synchronization and localization design for mobile underwater sensor networks[J]. IEEE Transactions on Mobile Computing, 2016, 15(3): 530-543.

[5] MORTAZAVI E, JAVIDAN R, DEHGHANI M J, et al. A robust method for underwater wireless sensor joint localization and synchronization[J]. Ocean Engineering, 2017, 137: 276-286.

[6] ZHENG Cuie, SUN Dajun, CAI Lin, et al. Mobile node localization in underwater wireless networks[J]. IEEE Access, 2018, 6: 17232-17244.

[7] 蔡守桂. 结合声速反演的水声传感网络定位误差修正方法研究[D]. 杭州: 浙江大学, 2019.

CAI Shougui. Research on positioning error correction in underwater acoustic sensor network with sound speed inversion[D]. Hangzhou: Zhejiang University, 2019.(in Chinese)

[8] ZHOU Zhong, PENG Zheng, CUI Junhong, et al. Scalable localization with mobility prediction for underwater sensor networks[J]. IEEE Transactions on Mobile Computing, 2011, 10(3): 335-348.

[9] 曹俊, 郑翠娥, 孙大军, 等. AUV直线航迹下的单信标测距定位[J]. 哈尔滨工程大学学报, 2017, 38(5): 733-738.

CAO Jun, ZHENG Cuie, SUN Dajun, et al. AUV positioning based on single-beacon ranging in straight-line trajectory[J]. Journal of Harbin Engineering University, 2017, 38(5): 733-738.(in Chinese)

[10] JIN Bonan,XU Xiaosu,ZHU Yongyun,et al.Single-source aided semi-autonomous passive location for correcting the position of an underwater vehicle[J]. IEEE Sensors Journal, 2019, 19(9): 3267-3275.

[11] CHENG Xiuzhen, SHU Haining, LIANG Qilian, et al. Silent positioning in underwater acoustic sensor networks[J]. IEEE Transactions on Vehicular Technology, 2008, 57(3): 1756-1766.

[12] CARROLL P, MAHMOOD K, ZHOU Shengli, et al. On-demand asynchronous localization for underwater sensor networks[J]. IEEE Transactions on Signal Processing, 2014, 62(13): 3337-3348.

[13] TAN H P, GABOR A F, EU Z A, et al. A wide coverage positioning system (WPS) for underwater localization[C]∥2010 IEEE International Conference on Communications. Cape Town, South Africa. IEEE, 2010: 1-5.

[14] 李浩铭, 鄢社锋, 徐立军, 等. 基于射线声学的水下传感网络静默定位算法[J]. 电子与信息学报, 2021, 43(3): 781-787.

LI Haoming, YAN Shefeng, XU Lijun, et al. A silent location algorithm for underwater sensor network based on ray acoustics[J]. Journal of Electronics and Information Technology, 2021, 43(3): 781-787.(in Chinese)

[15] CAO Jun,ZHENG Cuie,SUN Dajun,et al.Travel time processing for LBL positioning system[C]∥2016 IEEE/OES China Ocean Acoustics (COA). Harbin, China. IEEE, 2016: 1-5.

[16] 封金星, 丁士圻, 惠俊英. 水下运动目标长基线定位解算研究[J]. 声学学报, 1996, 21(5): 832- 837.

FENG Jinxing, DING Shiqi, HUI Junying. A solution of locating a moving object using long-base line acoustic system[J]. Acta Acustica, 1996, 21(5): 832- 837.(in Chinese)

[17] LI Zhao, DOSSO S E, SUN Dajun. Motion-compensated acoustic localization for underwater vehicles[J]. IEEE Journal of Oceanic Engineering, 2016, 41(4): 840- 851.

[18] 张居成. 深水长基线定位导航技术研究[D]. 哈尔滨: 哈尔滨工程大学, 2014.

ZHANG Jucheng. Research of deepwater LBL positoning and navigation technology[D]. Harbin: Harbin Engineering University, 2014.(in Chinese)

[19] WEBSTER S E, EUSTICE R M, SINGH H, et al. Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles[J]. The International Journal of Robotics Research, 2012, 31(8): 935-950.

[20] BAHR A,LEONARD J J,MARTINOLI A. Dynamic positioning of beacon vehicles for cooperative underwater navigation[C]∥2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura-Algarve, Portugal. IEEE, 2012: 3760-3767.

[21] 吴德明. 一种用于声线修正的迭代法[J]. 声学学报, 1992, 17(2): 104-110.

WU Deming. An iteration method for correcting the located coordinates of an underwater target[J]. Acta Acustica, 1992, 17(2): 104-110.(in Chinese)

[22] P.Bertsekas D.Nonlinenar programming[M].Belmont, MA, USA: Athena Scientific, 1999:22-108.