CN113099383B - 一种auv辅助的三维水下节点自定位方法 - Google Patents

一种auv辅助的三维水下节点自定位方法 Download PDF

Info

Publication number
CN113099383B
CN113099383B CN202110379287.3A CN202110379287A CN113099383B CN 113099383 B CN113099383 B CN 113099383B CN 202110379287 A CN202110379287 A CN 202110379287A CN 113099383 B CN113099383 B CN 113099383B
Authority
CN
China
Prior art keywords
auv
node
positioning
underwater
tdoa
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110379287.3A
Other languages
English (en)
Other versions
CN113099383A (zh
Inventor
郝琨
薛奇鑫
赵璐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin Chengjian University
Original Assignee
Tianjin Chengjian University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin Chengjian University filed Critical Tianjin Chengjian University
Priority to CN202110379287.3A priority Critical patent/CN113099383B/zh
Publication of CN113099383A publication Critical patent/CN113099383A/zh
Application granted granted Critical
Publication of CN113099383B publication Critical patent/CN113099383B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • HELECTRICITY
    • H04ELECTRIC COMMUNICATION TECHNIQUE
    • H04WWIRELESS COMMUNICATION NETWORKS
    • H04W4/00Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02Services making use of location information
    • H04W4/023Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • HELECTRICITY
    • H04ELECTRIC COMMUNICATION TECHNIQUE
    • H04WWIRELESS COMMUNICATION NETWORKS
    • H04W4/00Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02Services making use of location information
    • H04W4/025Services making use of location information using location based information parameters
    • H04W4/027Services making use of location information using location based information parameters using movement velocity, acceleration information
    • HELECTRICITY
    • H04ELECTRIC COMMUNICATION TECHNIQUE
    • H04WWIRELESS COMMUNICATION NETWORKS
    • H04W64/00Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/006Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明属于水下传感器网络节点定位技术领域,特别涉及一种AUV辅助的三维水下节点自定位方法。该方法包括搭建水下传感器网络;构建水下节点自定位测量模型;节点估计自身位置坐标;定位误差优化等步骤。本发明提供的AUV辅助的三维水下节点自定位方法是将AUV与TDOA定位和多普勒频移差定位进行联合,待定位节点通过接收AUV在不同时刻发射的信号获得TDOA和多普勒频移差测量数据,建立关于节点位置的极大似然估计,利用加权最小二乘法求解节点位置的初始值,再采用高斯‑牛顿迭代法优化定位误差。本方法能够显著提升节点的自定位精度,当AUV位置和速度误差以及TDOA和多普勒频移差测量误差较小时,节点的位置估计具有较高的精度。

Description

一种AUV辅助的三维水下节点自定位方法
技术领域
本发明属于水下传感器网络节点定位技术领域,特别是涉及一种AUV辅助的三维水下节点自定位方法。
背景技术
随着陆地资源的日益消耗,开发海洋的热潮在世界各地兴起。基于水下传感器网络(Underwater Sensor Network,UWSN)的水下节点定位作为探索海洋的重要手段,已然受到各国的重视。UWSN在水下部署了多种类具备不同功能的传感器节点,这些节点可以从海洋及其附近区域中收集数据并与水面监测船和岸基监控中心进行信息交互,为实时监测水下环境提供了观测平台。在当下的UWSN应用中,快速、精确的估计节点的位置是保障UWSN完成各项监测任务的前提和核心之一,准确的节点位置信息有助于提高路由协议效率、校准网络节点数据、优化网络拓扑设计及均衡网络不同区域的能耗。另外,在水下目标打捞、自然灾害预警、紧急搜索救援等海洋工程中,也都离不开水下节点定位的支持。
近十年来,具备较强灵活性和机动性的AUV(Autonomous Underwater Vehicle,自主水下航行器)引起了民用、军用等海洋领域的广泛关注,并且已经研究和测试了众多用于海洋实际应用的AUV,这为高效的监测UWSN中节点的状态提供了极大的便利。然而,水下节点所处的海洋环境有着极其苛刻的特性,水声信道固有的诸如高传输时延、有限的带宽、多普勒效应及严重多径等特点;洋流和海洋生物的影响会使已部署的节点产生无法预测的漂移,从而导致自身位置出现偏差,这些问题都给水下节点定位带来了极大的挑战。因此,设计一种精确估计节点位置信息的方法以充分满足实际应用需求显得极其重要。
现有的UWSN节点定位方法可根据定位过程中是否需要测量节点间的距离或角度分为两大类,分别为基于测距的定位和无需测距的定位。在基于测距的定位中,常用的测量方法包括信号到达时间(TOA)、信号到达时间差(TDOA)、接收信号强度(RSSI)、信号到达角度(AOA)、多普勒频移(Doppler Shift)等。为了提高水下环境中节点的定位精度,估计节点的位置信息时可以联合多种类的测量参数获取与节点位置相关的附加信息,从而对节点进行定位。即主要采用TOA、TDOA、AOA、多普勒频移测量单独进行定位或将不同测量方法相互组合进行定位。但目前尚未发现以AUV为辅助的,联合TDOA和多普勒频移差测量的三维水下传感器网络节点自定位的方法。
发明内容
为了解决上述问题,本发明的目的在于提供一种AUV辅助的三维水下节点自定位方法,针对UWSN中的节点自定位提出,将AUV与TDOA定位和多普勒频移差定位进行联合,能够显著提升节点的自定位精度。
为了达到上述目的,本发明提供的AUV辅助的三维水下节点自定位方法是采用下述技术方案实现的:
AUV辅助的三维水下节点自定位方法包括按顺序进行的以下步骤:
步骤1:搭建适用于水下传感器节点自定位的网络模型;所述的网络模型包括待定位的节点u和AUV;选择使用AUV作为特殊锚节点辅助水下节点进行定位,第i个时刻AUV的位置信息ai和速度信息vi(i=1,2,3,…,N)由自身装备的导航仪、内置的深度计和测速仪给出;AUV周期性地发射信号,信号中包含AUV位置和速度的附加信息;
步骤2:构建水下节点自定位测量模型;部署在水中的节点u能够接收AUV发射的信号;利用接收到的信号进行自定位,所述的自定位测量模型利用的测量参数为节点u获得AUV在不同位置发射信号的TDOA和多普勒频移差测量,通过上述测量参数建立测量模型;在实际中,节点u的真实位置参数u0=[x,y,z]T和AUV的真实位置
Figure BDA0003012262170000021
速度参数
Figure BDA0003012262170000022
均未知,需要估计未知参数
Figure BDA0003012262170000023
才能得到节点u的位置估计;故需建立关于未知参数Ω的极大似然估计;将极大似然估计函数转化为最小化代价函数
Figure BDA00030122621700000216
求解;
步骤3:通过构建关于节点u位置参数的伪线性方程,将TDOA和多普勒频移差联合定位问题转化为加权最小二乘估计问题;使用带有测量误差的测量向量ξ和带有偏差的AUV位置向量η估计节点u的位置;
步骤4:优化定位误差:分析并简化已有的定位模型,使用步骤3得到的节点位置估计作为高斯-牛顿迭代的初始值,节点u通过迭代以精确估计自身的位置。
上述技术方案中,所述步骤1具体包括以下步骤:
步骤1.1:当监测船行驶至监测区域时,向海水中投放辅助定位装置AUV,成为水下传感器网络部署在水下的移动锚节点;由于GPS无法应用在水下,AUV需要浮上水面与卫星交互并接收GPS信号确定自身的位置后下潜至监测区域,再按照程序预先设定的轨迹移动并周期性的发射信号,AUV在第i个时刻发射信号时的真实但未知的位置为
Figure BDA0003012262170000024
真实但未知的速度为
Figure BDA0003012262170000025
其中i=1,2,3,…,N;AUV的监测范围能够覆盖整个UWSN区域;
步骤1.2:AUV在水下移动的过程中,水下洋流会对其位置和速度产生影响,随着时间的流逝,AUV的位置和速度会产生误差,AUV存在误差的位置和速度分别为ai=[xi,yi,zi]T和vi=[vix,viy,viz]T;将AUV在N个位置发射信号的位置参量记为
Figure BDA0003012262170000026
将AUV在不同位置发射信号的实际位置记为
Figure BDA0003012262170000027
Figure BDA0003012262170000028
和ai之间的关系表示为:
Figure BDA0003012262170000029
其中,Δai为AUV在第i个位置时的位置误差;将AUV在N个位置发射信号时的速度参量记为
Figure BDA00030122621700000210
将AUV在不同位置发射信号的实际速度记为
Figure BDA00030122621700000211
Figure BDA00030122621700000212
和vi之间的关系表示为:
Figure BDA00030122621700000213
其中,Δvi为AUV在第i个位置时的速度误差;
步骤1.3:将AUV的位置参量和速度参量记为
Figure BDA00030122621700000214
η=η0+Δη (21)
式中,η=[aT,vT]T,Δη=[ΔaT,ΔvT]T,假设误差Δη服从零均值高斯分布,其协方差矩阵为Qη
上述技术方案中,所述步骤2具体包括以下步骤:
步骤2.1:建立TDOA和多普勒频差测量模型:AUV在第i(i=1,2,3,…,N)个位置发射信号时与节点u之间的真实距离为
Figure BDA00030122621700000215
节点u接收到AUV在第i个位置发射的信号与初次接收到AUV的信号之间的TDOA测量τi1表示为
Figure BDA0003012262170000031
式中,i=2,3,…,N,c为水下平均声速,
Figure BDA0003012262170000032
为TDOA真实值,Δτi1为TDOA测量误差,该误差服从均值为0,方差为
Figure BDA0003012262170000033
的高斯分布;
节点u获得AUV在第i个位置发射信号的多普勒频移记为
Figure BDA0003012262170000034
节点u获得的AUV在第i个位置发射的信号与初次接收到AUV的信号之间的多普勒频移差测量fi1表示为
Figure BDA0003012262170000035
式中,fc为AUV发射信号的载波频率,
Figure BDA0003012262170000036
为多普勒频移差的真实值,Δfi1为多普勒频移差的测量误差,并且测量误差服从零均值高斯分布,误差方差为
Figure BDA0003012262170000037
步骤2.2:通过分析TDOA和多普勒频移差之间的关系,将两者转化为信号传播距离差和距离差变化率表示,可得到
Figure BDA0003012262170000038
Figure BDA0003012262170000039
式中,Δdi1
Figure BDA00030122621700000310
分别表示距离差和距离差变化率的测量误差,
Figure BDA00030122621700000311
为信号传播距离差的真实值,
Figure BDA00030122621700000312
为传播距离差变化率的真实值。整理以上两式得
d=d0+Δd (26)
Figure BDA00030122621700000313
式中,d=[d21,d31,…,dN1]T
Figure BDA00030122621700000314
Δd=[Δd21,Δd31,…,ΔdN1]T
Figure BDA00030122621700000315
以及
Figure BDA00030122621700000316
将以上两式合并得到
ξ=ξ0+Δξ (28)
式中,
Figure BDA00030122621700000317
为测量误差构成的向量,其服从均值为零的高斯分布,协方差矩阵为E=[ΔξΔξT]=Qξ
步骤2.3:建立关于未知参数
Figure BDA00030122621700000318
的极大似然估计:TDOA和多普勒频移差测量参数ξ和AUV位置、速度参数η与未知参数Ω的似然函数为
Figure BDA0003012262170000041
式中,K为归一化常数,
Figure BDA0003012262170000042
最大化似然函数即得到未知参数Ω的估计值,此处将似然函数转化为最小化代价函数,表示为
Figure BDA0003012262170000043
上述技术方案中,所述步骤3具体包括以下步骤:
步骤3.1:构建关于节点u位置参数的伪线性方程:对
Figure BDA0003012262170000044
Figure BDA0003012262170000045
进行变换,得到
Figure BDA0003012262170000046
Figure BDA0003012262170000047
使用包含测量误差的误差项
Figure BDA0003012262170000048
Figure BDA0003012262170000049
替代测量参数的真实值,将上式表示为
Figure BDA00030122621700000410
Figure BDA00030122621700000411
定义
Figure BDA00030122621700000412
将上述等式整理为矩阵方程形式,即
Figure BDA00030122621700000413
式中,
Figure BDA00030122621700000414
Figure BDA00030122621700000415
Figure BDA00030122621700000416
Figure BDA0003012262170000051
将上述等式写为
AΔξ+BΔη=b-Yθ0 (36);
步骤3.2:将TDOA和多普勒频移差联合定位的问题转化为加权最小二乘估计问题:通过求解上述等式,得到θ0的加权最小二乘估计,表示为
Figure BDA0003012262170000052
式中,W1=E[(AΔξ+BΔη)(AΔξ+BΔη)T]-1=(AQξAT+BQηBT)-1为加权矩阵,
Figure BDA0003012262170000053
的协方差矩阵近似为
Figure BDA0003012262170000054
由上式得到节点u位置坐标的初始估计,即
Figure BDA0003012262170000055
上述技术方案中,所述步骤4具体包括以下步骤:
步骤4.1:当用于辅助定位的AUV存在位置和速度偏差时,节点u的位置的Fisher信息矩阵FIM与辅助定位的AUV位置精确但TDOA和多普勒频移差测量值具有增量协方差矩阵
Figure BDA0003012262170000056
的情况下的FIM相同,在已知AUV位置和速度参数的情况下,将TDOA和多普勒频移差的测量方程写为
Figure BDA0003012262170000057
式中,
Figure BDA0003012262170000058
是表示在参量ξ0中用存在误差的AUV的位置、速度参数η代替AUV的真实位置参数η0,等式的右边为新的TDOA和多普勒频移差的测量误差,该误差同样服从零均值高斯分布;此时,对于所述步骤2中子步骤2.3提出的估计未知参数Ω的问题简化为仅估计节点u位置参数的问题,即写为
Figure BDA0003012262170000059
步骤4.2:利用高斯-牛顿法优化定位误差:以步骤3中得到的节点u的位置初始估计
Figure BDA00030122621700000510
作为高斯-牛顿迭代的初始值,根据公式
Figure BDA00030122621700000511
求出逼近节点u真实位置的位置估计;
式中,
Figure BDA00030122621700000512
为第j(j=0,1,2…K)次高斯-牛顿迭代的估计结果,
Figure BDA00030122621700000513
为第j次迭代的误差修正量;W是在AUV位置和速度误差较小的情况下的加权矩阵,表示为
Figure BDA00030122621700000514
为雅各比矩阵;
步骤4.3:在经过K次高斯-牛顿迭代后,可解算出节点u位置的最佳估计
Figure BDA0003012262170000061
本发明提供的AUV辅助的三维水下节点自定位方法是针对UWSN中的节点自定位提出,将AUV与TDOA定位和多普勒频移差定位进行联合,灵活性较强的AUV能够在指定区域内以预先设定的路径移动并且周期性的发射信号,待定位节点通过接收AUV在不同时刻发射的信号获得TDOA和多普勒频移差测量数据,建立关于节点位置的极大似然估计,利用加权最小二乘法求解节点u位置的初始值,并采用高斯-牛顿迭代法计算节点的精确位置。本方法能够显著提升节点的自定位精度,当AUV位置和速度误差以及TDOA和多普勒频移差测量误差较小时,节点的位置估计具有较高的精度。
本发明适用于水下环境中运行的网络,节点有效实现自定位能够为感知、监测水域安全提供保障,可被人类应用于海洋探索、开发的各个领域。
附图说明
图1是本发明提供的AUV辅助的三维水下节点自定位方法的网络模型示意图。
图2是本发明提供的AUV辅助的三维水下节点自定位方法的节点自定位测量模型示意图。
图3是本发明提供的AUV辅助的三维水下节点自定位方法流程图。
具体实施方式
下面结合附图和具体实施例对本发明提供的AUV辅助的三维水下节点自定位方法进行详细说明。
如图1、图2、图3所示,本发明提供的AUV辅助的三维水下节点自定位方法包括按顺序进行的下列步骤:
步骤1:搭建适用于水下传感器节点自定位的网络模型;所述的网络模型包括待定位的节点u和AUV;为了较少传感器节点的能量损耗、延长节点的使用寿命,选择使用AUV作为特殊锚节点(移动锚)辅助水下节点进行定位,第i个时刻AUV的位置信息ai和速度信息vi(i=1,2,3,…,N)由自身装备的导航仪、内置的深度计和测速仪给出;AUV周期性地发射信号,信号中包含AUV位置和速度的附加信息。具体包括以下步骤:
步骤1.1:当监测船行驶至监测区域时,向海水中投放辅助定位装置AUV,成为水下传感器网络部署在水下的移动锚节点;由于GPS无法应用在水下,AUV需要浮上水面与卫星交互并接收GPS信号确定自身的位置后下潜至监测区域,再按照程序预先设定的轨迹移动并周期性的发射信号,AUV在第i个时刻发射信号时的真实但未知的位置为
Figure BDA0003012262170000062
真实但未知的速度为
Figure BDA0003012262170000063
其中i=1,2,3,…,N;AUV的监测范围能够覆盖整个UWSN区域;
步骤1.2:AUV在水下移动的过程中,水下洋流会对其位置和速度产生影响,随着时间的流逝,AUV的位置和速度会产生误差,AUV存在误差的位置和速度分别为ai=[xi,yi,zi]T和vi=[vix,viy,viz]T;将AUV在N个位置发射信号的位置参量记为
Figure BDA0003012262170000064
将AUV在不同位置发射信号的实际位置记为
Figure BDA0003012262170000065
Figure BDA0003012262170000066
和ai之间的关系表示为:
Figure BDA0003012262170000067
其中,Δai为AUV在第i个位置时的位置误差;将AUV在N个位置发射信号时的速度参量记为
Figure BDA0003012262170000068
将AUV在不同位置发射信号的实际速度记为
Figure BDA0003012262170000069
Figure BDA00030122621700000610
和vi之间的关系表示为:
Figure BDA00030122621700000611
其中,Δvi为AUV在第i个位置时的速度误差;
步骤1.3:将AUV的位置参量和速度参量记为
Figure BDA0003012262170000071
η=η0+Δη (41)
式中,η=[aT,vT]T,Δη=[ΔaT,ΔvT]T,假设误差Δη服从零均值高斯分布,其协方差矩阵为Qη
步骤2:构建水下节点自定位测量模型;部署在水中的节点u能够接收AUV发射的信号;利用接收到的信号进行自定位,所述的自定位测量模型利用的测量参数为节点u获得AUV在不同位置发射信号的TDOA和多普勒频移差测量,通过上述测量参数建立测量模型;在实际中,节点u的真实位置参数u0=[x,y,z]T和AUV的真实位置
Figure BDA0003012262170000072
速度参数
Figure BDA0003012262170000073
均未知,需要估计未知参数
Figure BDA0003012262170000074
才能得到节点u的位置估计;故需建立关于未知参数Ω的极大似然估计;将极大似然估计函数转化为最小化代价函数
Figure BDA00030122621700000715
求解。具体包括以下步骤:
步骤2.1:建立TDOA和多普勒频差测量模型:AUV在第i(i=1,2,3,…,N)个位置发射信号时与节点u之间的真实距离为
Figure BDA0003012262170000075
节点u接收到AUV在第i个位置发射的信号与初次接收到AUV的信号之间的TDOA测量τi1表示为
Figure BDA0003012262170000076
式中,i=2,3,…,N,c为水下平均声速,
Figure BDA0003012262170000077
为TDOA真实值,Δτi1为TDOA测量误差,该误差服从均值为0,方差为
Figure BDA0003012262170000078
的高斯分布;
节点u获得AUV在第i个位置发射信号的多普勒频移记为
Figure BDA0003012262170000079
节点u获得的AUV在第i个位置发射的信号与初次接收到AUV的信号之间的多普勒频移差测量fi1表示为
Figure BDA00030122621700000710
式中,fc为AUV发射信号的载波频率,
Figure BDA00030122621700000711
为多普勒频移差的真实值,Δfi1为多普勒频移差的测量误差,并且测量误差服从零均值高斯分布,误差方差为
Figure BDA00030122621700000712
步骤2.2:通过分析TDOA和多普勒频移差之间的关系,将两者转化为信号传播距离差和距离差变化率表示,得到
Figure BDA00030122621700000713
Figure BDA00030122621700000714
式中,Δdi1
Figure BDA0003012262170000081
分别表示距离差和距离差变化率的测量误差,
Figure BDA0003012262170000082
为信号传播距离差的真实值,
Figure BDA0003012262170000083
为传播距离差变化率的真实值。整理以上两式得
d=d0+Δd (46)
Figure BDA0003012262170000084
式中,d=[d21,d31,…,dN1]T
Figure BDA0003012262170000085
Δd=[Δd21,Δd31,…,ΔdN1]T
Figure BDA0003012262170000086
以及
Figure BDA0003012262170000087
将以上两式合并得到
ξ=ξ0+Δξ (48)
式中,
Figure BDA0003012262170000088
为测量误差构成的向量,其服从均值为零的高斯分布,协方差矩阵为E=[ΔξΔξT]=Qξ
步骤2.3:建立关于未知参数
Figure BDA0003012262170000089
的极大似然估计:由于TDOA和多普勒频移差测量误差以及AUV的位置和速度误差均满足均值为零的高斯分布,因此,TDOA和多普勒频移差测量参数ξ和AUV位置、速度参数η与未知参数Ω的似然函数为
Figure BDA00030122621700000810
式中,K为归一化常数,
Figure BDA00030122621700000811
最大化似然函数即得到未知参数Ω的估计值,此处将似然函数转化为最小化代价函数,表示为
Figure BDA00030122621700000812
步骤3:由于TDOA和多普勒频移差观测方程具有高度非线性,直接对最小化代价函数
Figure BDA00030122621700000820
求解很难得到解析解;通过构建关于节点u的位置参数的伪线性方程,将TDOA和多普勒频移差联合定位问题转化为加权最小二乘估计问题;使用带有测量误差的测量向量ξ和带有偏差的AUV位置向量η估计节点u的位置。具体包括以下步骤:
步骤3.1:构建关于节点u位置参数的伪线性方程:对
Figure BDA00030122621700000813
Figure BDA00030122621700000814
进行变换,得到
Figure BDA00030122621700000815
Figure BDA00030122621700000816
使用包含测量误差的误差项
Figure BDA00030122621700000817
Figure BDA00030122621700000818
替代测量参数的真实值,将上式表示为
Figure BDA00030122621700000819
Figure BDA0003012262170000091
定义
Figure BDA0003012262170000092
将上述等式整理为矩阵方程形式,即
Figure BDA0003012262170000093
式中,
Figure BDA0003012262170000094
Figure BDA0003012262170000095
Figure BDA0003012262170000096
Figure BDA0003012262170000097
将上述等式写为
AΔξ+BΔη=b-Yθ0 (56);
步骤3.2:将TDOA和多普勒频移差联合定位的问题转化为加权最小二乘估计问题:通过求解上述等式,得到θ0(包含节点u的位置)的加权最小二乘估计,表示为
Figure BDA0003012262170000098
式中,W1=E[(AΔξ+BΔη)(AΔξ+BΔη)T]-1=(AQξAT+BQηBT)-1为加权矩阵,
Figure BDA0003012262170000099
的协方差矩阵近似为
Figure BDA00030122621700000910
由上式得到节点u位置坐标的初始估计,即
Figure BDA00030122621700000911
步骤4:优化定位误差:分析并简化已有的定位模型,使用步骤3得到的节点位置估计作为高斯-牛顿迭代的初始值,节点u通过迭代以精确估计自身的位置。具体包括以下步骤:
步骤4.1:当用于辅助定位的AUV存在位置和速度偏差时,节点u的位置的Fisher信息矩阵FIM(Fisher information matrix,FIM)与辅助定位的AUV位置精确但TDOA和多普勒频移差测量值具有增量协方差矩阵
Figure BDA0003012262170000101
的情况下的FIM相同,由于AUV上配备了导航仪、深度计和测速仪,故AUV可以提供相对准确的位置和速度数据。那么,可在已知AUV位置和速度参数的情况下,将TDOA和多普勒频移差的测量方程写为
Figure BDA0003012262170000102
式中,
Figure BDA0003012262170000103
是表示在参量ξ0中用存在误差的AUV的位置、速度参数η代替AUV的真实位置参数η0,等式的右边为新的TDOA和多普勒频移差的测量误差,该误差同样服从零均值高斯分布。此时,对于所述步骤2中子步骤2.3提出的估计未知参数Ω的问题简化为仅估计节点u位置参数的问题,即写为
Figure BDA0003012262170000104
步骤4.2:利用高斯-牛顿法优化定位误差:以步骤3中得到的节点u的位置初始估计
Figure BDA0003012262170000105
作为高斯-牛顿迭代的初始值,根据公式
Figure BDA0003012262170000106
求出逼近节点u真实位置的位置估计;
式中,
Figure BDA0003012262170000107
为第j(j=0,1,2…K)次高斯-牛顿迭代的估计结果,
Figure BDA0003012262170000108
为第j次迭代的误差修正量;W是在AUV位置和速度误差较小的情况下的加权矩阵,表示为
Figure BDA0003012262170000109
Figure BDA00030122621700001010
为雅各比矩阵;
步骤4.3:在经过K次高斯-牛顿迭代后,解算出节点u位置的最佳估计
Figure BDA00030122621700001011

Claims (1)

1.一种AUV辅助的三维水下节点自定位方法,其特征在于:所述的AUV辅助的三维水下节点自定位方法包括以下步骤:
步骤1:搭建适用于水下传感器节点自定位的网络模型;所述的网络模型包括待定位的节点u和AUV;选择使用AUV作为特殊锚节点辅助水下节点进行定位,第i个时刻AUV的位置信息ai和速度信息vi(i=1,2,3,…,N)由自身装备的导航仪、内置的深度计和测速仪给出;AUV周期性地发射信号,信号中包含AUV位置和速度的附加信息;
步骤2:构建水下节点自定位测量模型;部署在水中的节点u能够接收AUV发射的信号;利用接收到的信号进行自定位,所述的自定位测量模型利用的测量参数为节点u获得AUV在不同位置发射信号的TDOA和多普勒频移差测量,通过上述测量参数建立测量模型;在实际中,节点u的真实位置参数u0=[x,y,z]T和AUV的真实位置
Figure FDA0003908695440000011
速度参数
Figure FDA0003908695440000012
均未知,需要估计未知参数
Figure FDA0003908695440000013
才能得到节点u的位置估计;故需建立关于未知参数Ω的极大似然估计;将极大似然估计函数转化为最小化代价函数
Figure FDA00039086954400000115
求解;
步骤3:通过构建关于节点u位置参数的伪线性方程,将TDOA和多普勒频移差联合定位问题转化为加权最小二乘估计问题;使用带有测量误差的测量向量ξ和带有偏差的AUV位置向量η估计节点u的位置;
步骤4:优化定位误差:分析并简化已有的定位模型,使用步骤3得到的节点位置估计作为高斯-牛顿迭代的初始值,节点u通过迭代以精确估计自身的位置;
所述步骤1具体包括以下子步骤:
步骤1.1:当监测船行驶至监测区域时,向海水中投放辅助定位装置AUV,成为水下传感器网络部署在水下的移动锚节点;由于GPS无法应用在水下,AUV需要浮上水面与卫星交互并接收GPS信号确定自身的位置后下潜至监测区域,再按照程序预先设定的轨迹移动并周期性的发射信号,AUV在第i个时刻发射信号时的真实但未知的位置为
Figure FDA0003908695440000014
真实但未知的速度为
Figure FDA0003908695440000015
其中
Figure FDA00039086954400000116
AUV的监测范围能够覆盖整个UWSN区域;
步骤1.2:AUV在水下移动的过程中,水下洋流会对其位置和速度产生影响,随着时间的流逝,AUV的位置和速度会产生误差,AUV存在误差的位置和速度分别为ai=[xi,yi,zi]T和vi=[vix,viy,viz]T;将AUV在N个位置发射信号的位置参量记为
Figure FDA0003908695440000016
将AUV在不同位置发射信号的实际位置记为
Figure FDA0003908695440000017
Figure FDA0003908695440000018
和ai之间的关系表示为:
Figure FDA0003908695440000019
其中,Δai为AUV在第i个位置时的位置误差;将AUV在N个位置发射信号时的速度参量记为
Figure FDA00039086954400000110
将AUV在不同位置发射信号的实际速度记为
Figure FDA00039086954400000111
Figure FDA00039086954400000112
和vi之间的关系表示为:
Figure FDA00039086954400000113
其中,Δvi为AUV在第i个位置时的速度误差;
步骤1.3:将AUV的位置参量和速度参量记为
Figure FDA00039086954400000114
η=η0+Δη (1)
式中,η=[aT,vT]T,Δη=[ΔaT,ΔvT]T,假设误差Δη服从零均值高斯分布,其协方差矩阵为Qη
所述步骤2具体包括以下子步骤:
步骤2.1:建立TDOA和多普勒频差测量模型:AUV在第i(i=1,2,3,…,N)个位置发射信号时与节点u之间的真实距离为
Figure FDA0003908695440000021
节点u接收到AUV在第i个位置发射的信号与初次接收到AUV的信号之间的TDOA测量τi1表示为
Figure FDA0003908695440000022
式中,i=2,3,…,N,c为水下平均声速,
Figure FDA0003908695440000023
为TDOA真实值,Δτi1为TDOA测量误差,该误差服从均值为0,方差为
Figure FDA0003908695440000024
的高斯分布;
节点u获得AUV在第i个位置发射信号的多普勒频移记为
Figure FDA0003908695440000025
节点u获得的AUV在第i个位置发射的信号与初次接收到AUV的信号之间的多普勒频移差测量fi1表示为
Figure FDA0003908695440000026
式中,fc为AUV发射信号的载波频率,
Figure FDA0003908695440000027
为多普勒频移差的真实值,Δfi1为多普勒频移差的测量误差,并且测量误差服从零均值高斯分布,误差方差为
Figure FDA0003908695440000028
步骤2.2:通过分析TDOA和多普勒频移差之间的关系,将两者转化为信号传播距离差和距离差变化率表示,得到
Figure FDA0003908695440000029
Figure FDA00039086954400000210
式中,Δdi1
Figure FDA00039086954400000211
分别表示距离差和距离差变化率的测量误差,
Figure FDA00039086954400000212
为信号传播距离差的真实值,
Figure FDA00039086954400000213
为传播距离差变化率的真实值;整理以上两式得
d=d0+Δd (6)
Figure FDA00039086954400000214
式中,d=[d21,d31,…,dN1]T
Figure FDA00039086954400000215
Δd=[Δd21,Δd31,…,ΔdN1]T
Figure FDA00039086954400000216
以及
Figure FDA00039086954400000217
将以上两式合并得到
ξ=ξ0+Δξ (8)
式中,
Figure FDA0003908695440000031
为测量误差构成的向量,其服从均值为零的高斯分布,协方差矩阵为E=[ΔξΔξT]=Qξ
步骤2.3:建立关于未知参数
Figure FDA0003908695440000032
的极大似然估计:TDOA和多普勒频移差测量参数ξ和AUV位置、速度参数η与未知参数Ω的似然函数为
Figure FDA0003908695440000033
式中,K为归一化常数,
Figure FDA0003908695440000034
最大化似然函数即得到未知参数Ω的估计值,此处将似然函数转化为最小化代价函数,表示为
Figure FDA0003908695440000035
所述步骤3具体包括以下子步骤:
步骤3.1:构建关于节点u位置参数的伪线性方程:对
Figure FDA0003908695440000036
Figure FDA0003908695440000037
进行变换,得到
Figure FDA0003908695440000038
Figure FDA0003908695440000039
使用包含测量误差的误差项
Figure FDA00039086954400000310
Figure FDA00039086954400000311
替代测量参数的真实值,将上式表示为
Figure FDA00039086954400000312
Figure FDA00039086954400000313
定义
Figure FDA00039086954400000314
将上述等式整理为矩阵方程形式,即
Figure FDA00039086954400000315
式中,
Figure FDA00039086954400000316
Figure FDA0003908695440000041
Figure FDA0003908695440000042
Figure FDA0003908695440000043
将上述等式写为
Figure FDA0003908695440000044
步骤3.2:将TDOA和多普勒频移差联合定位的问题转化为加权最小二乘估计问题:通过求解上述等式,得到
Figure FDA0003908695440000045
的加权最小二乘估计,表示为
Figure FDA0003908695440000046
式中,W1=E[(AΔξ+BΔη)(AΔξ+BΔη)T]-1=(AQξAT+BQηBT)-1为加权矩阵,
Figure FDA0003908695440000047
的协方差矩阵近似为
Figure FDA0003908695440000048
由上式得到节点u位置坐标的初始估计,即
Figure FDA0003908695440000049
所述步骤4具体包括以下子步骤:
步骤4.1:当用于辅助定位的AUV存在位置和速度偏差时,节点u的位置的Fisher信息矩阵FIM与辅助定位的AUV位置精确但TDOA和多普勒频移差测量值具有增量协方差矩阵
Figure FDA00039086954400000410
的情况下的FIM相同,在已知AUV位置和速度参数的情况下,将TDOA和多普勒频移差的测量方程写为
Figure FDA00039086954400000411
式中,
Figure FDA00039086954400000412
是表示在参量ξ0中用存在误差的AUV的位置、速度参数η代替AUV的真实位置参数η0,等式的右边为新的TDOA和多普勒频移差的测量误差,该误差同样服从零均值高斯分布;此时,对于所述步骤2中子步骤2.3提出的估计未知参数Ω的问题简化为仅估计节点u位置参数的问题,即写为
Figure FDA00039086954400000413
步骤4.2:利用高斯-牛顿法优化定位误差:以步骤3中得到的节点u的位置初始估计
Figure FDA00039086954400000414
作为高斯-牛顿迭代的初始值,根据公式
Figure FDA0003908695440000051
求出逼近节点u真实位置的位置估计;
式中,
Figure FDA0003908695440000052
为第j(j=0,1,2…K)次高斯-牛顿迭代的估计结果,
Figure FDA0003908695440000053
为第j次迭代的误差修正量;W是在AUV位置和速度误差较小的情况下的加权矩阵,表示为
Figure FDA0003908695440000054
为雅各比矩阵;
步骤4.3:在经过K次高斯-牛顿迭代后,解算出节点u位置的最佳估计
Figure FDA0003908695440000055
CN202110379287.3A 2021-04-08 2021-04-08 一种auv辅助的三维水下节点自定位方法 Active CN113099383B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110379287.3A CN113099383B (zh) 2021-04-08 2021-04-08 一种auv辅助的三维水下节点自定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110379287.3A CN113099383B (zh) 2021-04-08 2021-04-08 一种auv辅助的三维水下节点自定位方法

Publications (2)

Publication Number Publication Date
CN113099383A CN113099383A (zh) 2021-07-09
CN113099383B true CN113099383B (zh) 2022-12-30

Family

ID=76675223

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110379287.3A Active CN113099383B (zh) 2021-04-08 2021-04-08 一种auv辅助的三维水下节点自定位方法

Country Status (1)

Country Link
CN (1) CN113099383B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563763B (zh) * 2022-01-21 2022-10-21 青海师范大学 基于归零神经动力学的水下传感器网络节点测距定位方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108445446A (zh) * 2018-02-06 2018-08-24 中国人民解放军战略支援部队信息工程大学 一种无源测速定位方法及装置

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104316045B (zh) * 2014-11-06 2017-06-16 东南大学 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
US10267895B1 (en) * 2015-03-11 2019-04-23 National Technology & Engineering Solutions Of Sandia, Llc Doppler-assisted sensor fusion
US20160341812A1 (en) * 2015-05-21 2016-11-24 Santa Monica Semiconductor Single transaction aoa/toa/doppler tagging tracking and locating
CN107390171B (zh) * 2017-08-03 2020-10-09 厦门大学 基于toa测距和多普勒效应的水下传感器节点定位方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108445446A (zh) * 2018-02-06 2018-08-24 中国人民解放军战略支援部队信息工程大学 一种无源测速定位方法及装置

Also Published As

Publication number Publication date
CN113099383A (zh) 2021-07-09

Similar Documents

Publication Publication Date Title
CN101900558B (zh) 集成声纳微导航的自主式水下机器人组合导航方法
CN111273298B (zh) 基于波浪滑翔器组网技术的水下声学目标定位与跟踪方法
Liu et al. Suave: Swarm underwater autonomous vehicle localization
Liu et al. Time-difference-of-arrival-based localization methods of underwater mobile nodes using multiple surface beacons
Hegrenæs et al. Underwater transponder positioning and navigation of autonomous underwater vehicles
KR100906362B1 (ko) 2개의 기준점에 대한 거리정보와 저정밀도 관성센서를 이용한 무인잠수정 선단의 의사 lbl 수중항법시스템
CN102288170A (zh) 一种水下航行器内电子罗盘的校正方法
CN112346102A (zh) 一种水下声学定位导航授时系统
CN106054135A (zh) 一种基于周期移动时间窗的被动水声定位方法
CN111578944B (zh) 一种基于单信标的水下滑翔机定位方法
CN110294080A (zh) 一种利用超短基线实现水下精确作业的方法
CN113099383B (zh) 一种auv辅助的三维水下节点自定位方法
Jiang et al. Improvement of the position estimation for underwater gliders with a passive acoustic method
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
CN114200401A (zh) 一种基于网格划分的水下机器人自定位系统及自定位方法
CN108627802B (zh) 多信源海洋物联网定位方法
Zhang et al. Ocean current-aided localization and navigation for underwater gliders with information matching algorithm
Zhang et al. Research on position correction method for AUV large depth navigation based on ranging positioning
Schneider et al. Self-adapting under-ice integrated communications and navigation network
Garcia et al. Underwater robot localization using magnetic induction: Noise modeling and hardware validation
CN116609815A (zh) 一种海底航行器智能定位系统及方法
Sunitha et al. Localization of nodes in underwater wireless sensor networks
CN107255810B (zh) 基于单信标测距定位双精度加权融合的航向误差补偿方法
Wang et al. Wireless sensor networks for underwater localization: A survey
Kaveripakam et al. Enhancement of precise underwater object localization

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant