CN110057365B - 一种大潜深auv下潜定位方法 - Google Patents
一种大潜深auv下潜定位方法 Download PDFInfo
- Publication number
- CN110057365B CN110057365B CN201910365822.2A CN201910365822A CN110057365B CN 110057365 B CN110057365 B CN 110057365B CN 201910365822 A CN201910365822 A CN 201910365822A CN 110057365 B CN110057365 B CN 110057365B
- Authority
- CN
- China
- Prior art keywords
- auv
- depth
- submergence
- information
- positioning
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 238000004891 communication Methods 0.000 claims abstract description 67
- 238000005259 measurement Methods 0.000 claims abstract description 62
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 39
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 15
- 230000005540 biological transmission Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 41
- 230000001360 synchronised effect Effects 0.000 claims description 24
- 230000009189 diving Effects 0.000 claims description 17
- 125000004432 carbon atom Chemical group C* 0.000 claims description 11
- 238000005562 fading Methods 0.000 claims description 8
- 150000003839 salts Chemical class 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 239000000654 additive Substances 0.000 claims description 3
- 230000000996 additive effect Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 239000013535 sea water Substances 0.000 claims description 3
- 230000006978 adaptation Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 14
- 238000004088 simulation Methods 0.000 description 5
- 238000009434 installation Methods 0.000 description 3
- 238000011084 recovery Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 239000003643 water by type Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明涉及水下导航领域,具体涉及一种大潜深AUV下潜定位方法。通过AUV搭载水下通信节点、捷联惯导***和温盐深传感器;构建纯距离误差估计滤波模型;水面母船向AUV发送NED坐标系位置信息及时间信息;AUV记录捷联惯导***定位信息及温盐深传感器信息;通过传输时间和温盐深传感器信息得到AUV到母船的测距信息;构建强跟踪UKF算法;融合水面母船水平定位信息、测距信息和捷联惯导***定位信息和深度信息,跟踪深潜过程中AUV在NED坐标系下的水平定位误差以及载体坐标系下AUV的测速误差,对捷联惯导***校正得到精确AUV下潜定位信息。本发明能够扩展AUV捷联惯导***的水下在线校正手段,有效降低定位误差修正时间成本和设备成本,增强AUV的海洋环境适配能力。
Description
技术领域
本发明涉及水下导航领域,具体涉及一种大潜深AUV下潜定位方法。
背景技术
AUV上的捷联惯导***的水下重调问题长久以来备受学界关注。
对于大潜深AUV而言,由于捷联惯导***在下潜阶段无法获得卫星导航定位信息和DVL测速信息的及时修正,不得不在纯惯导模式下工作,致使AUV潜至指定深度时,捷联惯导***产生数百甚至上千米的定位误差。如AUTOSUB 6000配备的Ixsea Oceano PHINS捷联惯导***,在纯惯导模式下的定位精度为0.3m/s,若大潜深AUV以2节速度下潜至6000m深度,捷联惯导***此时将产生至少1800m的定位误差,亟需水下声学定位***予以校正。
目前,常用的水下声学定位***有长基线定位***LBL和超短基线定位***USBL,LBL***虽然精度较高,但基阵的布放和回收都会耗费大量船时;而USBL***虽然安装布设方便,但其水面辅助导航设备不但价格昂贵而且对安装精度要求极高,与这两种声学定位***不同,本发明针对大潜深AUV在深潜过程中的捷联惯导***误差校正问题,提出了一种大潜深AUV下潜定位方法。
发明内容
本发明的目的在于提供一种大潜深AUV下潜定位方法,以扩展AUV捷联惯导***的水下在线校正手段,有效降低捷联惯导***在大潜深AUV布放段定位误差修正的时间成本和设备成本,增强大潜深AUV的海洋环境适配能力。
本发明实施例提供一种大潜深AUV下潜定位方法,包括:
步骤一:通过大潜深AUV搭载水下通信节点、捷联惯导***以及温盐深传感器,得到大潜深AUV的基本信息;
步骤二:构建大潜深AUV的纯距离误差估计滤波模型;
步骤三:根据同步水声通信协议,水面母船向大潜深AUV发送水面母船各时刻在NED坐标系下的水平位置信息以及相应的时间信息;
步骤四:根据大潜深AUV水声通信***接收到的步骤三所述时间信息,大潜深AUV记录此时的捷联惯导***定位信息以及此时的温盐深传感器输出的温度、导电率和深度信息;
步骤五:根据同步水声通信协议,通过传输时间和温盐深传感器信息,大潜深AUV计算得到自身到母船的测距信息;
步骤六:构建强跟踪UKF算法模型;
步骤七:根据强跟踪UKF算法,融合水面母船水平定位信息、大潜深AUV同母船的测距信息和大潜深AUV捷联惯导***定位信息、大潜深AUV深度信息,跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下AUV的纵向和横向测速误差,对捷联惯导***输出进行校正,得到精确的大潜深AUV下潜定位信息;
本发明还包括这样一些结构特征:
所述步骤一中,所述步骤一方法的具体步骤为:
1)通过大潜深AUV搭载水下通信节点即水下modem,水面母船搭载水面通信节点即水面modem,将水声通信采用同步协议,使水面modem与水下modem时间同步;
2)通过大潜深AUV搭载捷联惯导***,实时输出大潜深AUV在NED坐标系下的定位信息以及在载体坐标系下的速度信息;
3)通过大潜深AUV搭载温盐深传感器,实时输出大潜深AUV潜深信息、大潜深AUV所处位置的温度信息以及大潜深AUV所处位置的海水导电率即盐度信息;
所述步骤二中,所述方法的具体步骤为:
1)设短时间内捷联惯导***的位置误差和速度误差均为常值;
2)建立大潜深AUV的纯距离误差估计滤波模型的离散***方程为:
Xk=ΦkXk-1+wk
其中,
上式中,分别为大潜深AUV在NED导航坐标系下惯导***北向和东向的位置误差,分别为大潜深AUV在载体坐标系下纵向和横向测速误差,T为采样时间,wk~N(0,Q),Q为wk的4×4协方差矩阵,ψ、θ、γ分别为大潜深AUV的艏向、纵倾角和横倾角;
3)将母船同大潜深AUV的测距信息rk和大潜深AUV温盐深输出的测深信息dk作为量测信息,设观测噪声噪声为加性白噪声,建立大潜深AUV纯距离误差估计滤波模型的观测方程为:
zk=h(Xk)+vk
其中,
所述步骤三中,所述方法的具体步骤为:
1)水面母船采集各时刻卫星导航的定位信息;
2)水面母船生成自身在NED坐标系下的水平定位数据信息;
3)根据同步水声通信协议,水面母船将上述的定位信息和相应母船水声通信***时间发送至大潜深AUV;
所述步骤四中,所述方法的具体步骤为:
1)根据大潜深AUV搭载的水下通信节点即水下modem所采用的串口通信,通过中断方式与大潜深AUV的导航计算机通信;
2)大潜深AUV导航计算机响应水声通信中断后,通过采用串口通信,以轮询方式采集大潜深AUV捷联惯导***的定位信息以及温盐深传感器输出的温度、导电率和深度信息信息;
所述步骤五中,所述方法的具体步骤为:
1)根据同步水声通信协议,大潜深AUV计算得到母船水声通信***时间Ts;
2)根据同步水声通信协议,大潜深AUV计算得到自身水声通信***时间Tr;
3)根据上述结果,大潜深AUV计算得到同步水声通信传输时间T=Tr-Ts;
4)根据温盐深传感器信息,大潜深AUV计算得到水声传播速度c;
5)根据公式rk=(Tr-Ts)c,大潜深AUV计算得到自身同母船的测距信息rk;
所述步骤六中,所述方法的具体步骤为:
1)设强跟踪UKF的初始状态为:
上式中,04×1、04×4分别为4维零矢量和4维全零方阵;
5)根据母船和大潜深AUV的测距信息r和大潜深AUV温盐深传感器输出的测深信息d,得到强跟踪UKF的观测信息zk为:
6)计算量测信息残差为:
7)计算残差协方差矩阵为:
上式中,ρ为遗忘因子,且0<ρ≤1,通常ρ=0.95;
8)基于UT变换计算***协方差矩阵Q通过确定性观测模型h的传播Qzz;根据和Q,求取Sigma点集{ζi},(i=1,…,L);计算Sigma点{ζi}通过确定性观测模型h的传播ξi=h(ζi),最后计算Qzz为:
9)计算残差中非正交噪声矢量协方差为:
Nk=Vk-R-Qzz;
10)基于UT变换计算k-1时刻状态协方差矩阵Pk,k-1的时间更新通过确定性观测模型h的传播Pzk,k-1;根据和Pk,k-1求取Sigma点集{χi},(i=1,…,L);计算Sigma点{χi}通过确定性观测模型h的传播δi=h(χi);根据计算状态协方差矩阵Pk,k-1的时间更新通过确定性观测模型h的传播Pzk,k-1;
12)状态协方差矩阵的强跟踪时间更新为:
14)计算滤波增益为:
15)滤波状态量测更新为:
16)滤波状态协方差矩阵量测更新为:
所述步骤七中,所述方法的具体步骤为:
1)根据强跟踪UKF算法,估计深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差,以及载体坐标系下大潜深AUV的纵向和横向测速误差;
2)根据直接校正方法,补偿大潜深AUV捷联惯导***的定位误差和测速误差,跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下大潜深AUV的纵向和横向测速误差,对捷联惯导***输出进行校正,得到精确的大潜深AUV下潜定位信息;
相比于现有技术,本发明的有益效果在于:
1.本发明基于强跟踪UKF构建大潜深AUV下潜定位方法,有效克服了母船与AUV间测距信息不确定性大、形式化非线性距离观测模型易产生模型失配等问题;
2.本发明仅仅利用通信声纳的测距信息,即可准确跟踪全海深AUV下潜段的定位误差和速度误差,避免了LBL声学信标的布放与回收的高昂船时成本而且不需要配备辅助USBL定位的高精度航姿与定位设备,避免了导航设备的繁复安装;
3.仿真结果表明,该方法能够有效补偿大潜深AUV下潜过程中捷联惯导***的定位与测速误差,只需要声学测距信息辅助,就可使捷联惯导***在大潜深AUV下潜过程中的东向和北向定位精度达到米级,与卫星导航***定位精度相当。
附图说明
图1为本发明基于强跟踪UKF的大潜深AUV下潜定位信号流示意图;
图2为本发明大潜深AUV与水面母船位置关系图;
图3为本发明定位数据信息示意图;
图4为本发明强跟踪UKF算法流程图;
图5为本发明基于UT变换计算强跟踪UKF渐消因子矩阵流程图;
图6为按照本发明进行定位误差修正的AUV下潜仿真路径示意图;
图7(a)为理论轨迹与纯惯导航迹跟踪轨迹示意图;
图7(b)为理论轨迹与修正后航迹跟踪轨迹示意图;
图8为本发明大潜深AUV纯惯导北向定位误差示意图;
图9为本发明大潜深AUV纯惯导东向定位误差示意图;
图10为本发明大潜深AUV纯惯导xb轴测速误差示意图;
图11为本发明大潜深AUV纯惯导yb轴测速误差示意图;
图12为本发明距离信息辅助的大潜深AUV惯导北向定位误差示意图;
图13为本发明距离信息辅助的大潜深AUV惯导东向定位误差示意图;
图14为本发明距离信息辅助的大潜深AUV惯导xb轴测速误差示意图;
图15为本发明距离信息辅助的大潜深AUV惯导yb轴测速误差示意图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图对本发明做进一步描述:
本发明的技术方案是这样实现的:
具体实施方式一:本实施方式的大潜深AUV下潜定位方法,包括:
步骤一:大潜深AUV搭载水下通信节点(水下modem)、温盐深和捷联惯导***;
步骤二:构建大潜深AUV纯距离误差估计滤波模型;
步骤三:基于同步水声通信,水面母船向AUV发送母船各时刻在北-东-地(NED)导航坐标系下的水平位置信息和相应的时间信息;
步骤四:AUV记录自身水声通信***接收信息时刻的捷联惯导***定位信息和温盐深传感器输出的深度、温度和导电率(获取盐度)信息;
步骤五:基于同步水声通信传输时间和温盐深传感器信息,AUV计算自身到母船的测距信息;
步骤六:构建强跟踪UKF算法;
步骤七:基于强跟踪UKF算法,融合水面母船水平定位信息、AUV同母船的测距信息和AUV捷联惯导***定位信息、AUV深度信息,跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差,对捷联惯导***输出进行校正,得到精确的AUV下潜定位信息。
本发明的一个应用情形可以是搭载了水下通信节点(水下modem)、温盐深、捷联惯导***的大潜深AUV,在下潜阶段无法基于卫星导航***、DVL等导航传感器修正捷联惯导***误差,捷联惯导***只能在纯惯导模式下运行,定位误差快速积累。如图1所示,以捷联惯导***的北向和东向定位误差与测速误差为状态变量,构建大潜深AUV纯距离误差估计滤波模型;母船基于同步水声通信***,定时向AUV发送自身在北-东-地(NED)导航坐标系下的水平位置信息和信息发送时刻的时间信息;AUV上搭载的水声modem解算母船信息的同时,触发AUV导航计算机记录母船信息到达时刻的捷联惯导***定位信息、和温盐深传感器输出的深度、温度和导电率(获取盐度)信息;利用同步水声通信传输时间和温盐深传感器信息,AUV计算自身到母船的测距信息;针对大潜深AUV纯距离误差估计滤波模型特点,构建强跟踪UKF算法,基于强跟踪UKF算法,融合水面母船水平定位信息、AUV同母船的测距信息和AUV捷联惯导***定位信息、AUV深度信息,跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差,对捷联惯导***输出进行校正,得到精确的AUV下潜定位信息。
具体实施方式二:本实施方式与具体实施方式一不同的是:步骤一具体为:
步骤一一:大潜深AUV搭载水下通信节点(水下modem),水面母船搭载水面通信节点(水面modem),水声通信采用同步模式,即水面modem与水下modem时间同步;
步骤一二:大潜深AUV搭载捷联惯导***,实时输出AUV在NED坐标系下的定位信息和载体坐标系下的速度信息;
步骤一三:大潜深AUV搭载温盐深,可实时输出AUV潜深信息、AUV所处位置的温度信息和海水导电率(获取盐度信息)。
本发明的一个应用情形可以是如图2所示,水面母船搭载水面通信节点(水面modem),大潜深AUV搭载水下通信节点(水下modem),为便于母船与AUV的通信和观测,AUV采用螺旋下潜方式,大潜深AUV搭载的捷联惯导***可以实时输出AUV在NED坐标系下的定位信息和测速信息和载体坐标系下的速度信息,同时大潜深AUV搭载温盐深,可实时输出AUV潜深信息、AUV所处位置的温度信息和海水导电率(获取盐度信息)。
具体实施方式三:本实施方式与具体实施方式一不同的是:步骤二具体为:
步骤二一:假定短时间内捷联惯导***的位置误差和速度误差均为常值;
步骤二二:建立大潜深AUV纯距离误差估计滤波模型的离散***方程
Xk=ΦkXk-1+wk
其中
式中为NED导航坐标系下惯导***的位置误差,为AUV载体坐标系下,纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差,T为采样时间,wk~N(0,Q),Q为wk的4×4协方差矩阵,ψ、θ、γ分别为AUV的艏向、纵倾和横倾角。
步骤二三:将母船同AUV的测距信息rk和AUV温盐深输出的测深信息dk作为量测信息,设观测噪声噪声为加性白噪声,建立大潜深AUV纯距离误差估计滤波模型的观测方程
具体实施方式四:本实施方式与具体实施方式一不同的是:步骤三具体为:
步骤三一:水面母船采集各时刻卫星导航定位信息;
步骤三二:水面母船生成自身在NED导航坐标系下的水平定位数据信息;
步骤三三:基于同步水声通信,水面母船将定位信息和相应母船水声通信***时间发送给AUV。
本发明的一个应用情形可以是大潜深AUV采用北-东-地(NED)导航坐标系,如图2所示,母船基于同步水声通信***定时向大潜深AUV发送的定位数据信息包括命令行、数据包长度、水面母船信息发送历元Ts以及水面母船的东向和北向位置坐标信息。如图3所示,定位数据信息共5个信息字段。其中“20”为定位数据信息的指令码占两个字节;“16”为定位数据信息的字节总数占2个字节;水面母船信息发送历元Ts为定位数据信息发送时刻,其与水面母船的东向和北向位置坐标信息字段均占用4个字节。大潜深AUV根据信息接受时刻Tr与水面母船历元Ts的时间差与水声速度c相乘后得到水声传输距离rk,即rk=(Tr-Ts)c。
具体实施方式五:本实施方式与具体实施方式一不同的是:步骤四具体为:
步骤四一:AUV搭载的水下通信节点(水下modem)采用串口通信,基于中断方式与AUV的导航计算机通信;
步骤四二:AUV导航计算机响应水声通信中断后,采用串口通信,以轮询方式,采集AUV捷联惯导***的定位信息和温盐深传感器输出的深度、温度和导电率(获取盐度)信息;
具体实施方式六:本实施方式与具体实施方式一不同的是:步骤五具体为:
步骤五一:基于同步水声通信协议,AUV解算母船水声通信***时间Ts;
步骤五二:AUV解算自身水声通信***时间Tr;
步骤五三:AUV计算同步水声通信传输时间T=Tr-Ts;
步骤五四:AUV根据温盐深传感器信息计算水声传播速度c;
步骤五五:AUV按公式计算自身同母船的测距信息rk
rk=(Tr-Ts)c
具体实施方式七:本实施方式与具体实施方式一不同的是:步骤六具体为:
步骤六一:设强跟踪UKF的初始状态为:
其中04×1、04×4分别为4维零矢量和4维全零方阵。
步骤六五:基于母船同AUV的测距信息r和AUV温盐深输出的测深信息d,获取强跟踪UKF的观测信息zk;
步骤六八:基于UT变换计算***协方差矩阵Q通过确定性观测模型h的传播Qzz。首先,根据和Q,求取Sigma点集{ζi},(i=1,…,L);然后,计算Sigma点{ζi}通过确定性观测模型h的传播ζi=h(ζi),最后计算Qzz。
步骤六九:计算残差中非正交噪声矢量协方差
Nk=Vk-R-Qzz
步骤六十:基于UT变换计算k-1时刻状态协方差矩阵Pk,k-1的时间更新通过确定性观测模型h的传播Pzk,k-1。首先根据和Pk,k-1求取Sigma点集{χi},(i=1,…,L);其次计算Sigma点{χi}通过确定性观测模型h的传播δi=h(χi);最后根据
步骤六十二:状态协方差矩阵的强跟踪时间更新;
步骤六十三:以和为观测方程参数,基于UT变换计算量测信息预测值;首先根据和Pk,k-1求取Sigma点集{ζi},(i=1,…,L);其次计算Sigma点{ζi}通过确定性观测模型h的传播ξi=h(ζi);最后根据
和
步骤六十五:滤波状态量测更新:
步骤六十六:滤波状态协方差矩阵量测更新:
算法流程图如图4和图5所示,其中图5为图4中基于UT变换计算强跟踪UKF渐消因子矩阵的流程图。
具体实施方式八:本实施方式与具体实施方式一不同的是:步骤七具体为:
步骤七一:采用如图6所示的仿真路径,基于强跟踪UKF算法,估计深潜过程中AUV在NED导航系下的北向和东向水平定位误差,以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差,在没有距离信息辅助的条件下,纯惯导***航迹跟踪路径如图7(a)的“+”曲线所示,如图7(b)中“+”曲线所示所示,校正后的惯导***航迹与理论航迹基本重合,如图8至图11所示分别为纯惯导***的定位误差和测速误差;
步骤七二:基于直接校正方法,补偿AUV捷联惯导***的定位误差和测速误差,跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差,对捷联惯导***输出进行校正,得到精确的AUV下潜定位信息,仿真结果如图12至图15所示,由图可见,在声学测距信息辅助下,大潜深AUV测速误差大大降低,定位误差小于10米与卫星导航***定位误差相当。
本发明涉及水下导航领域,具体涉及一种大潜深AUV下潜定位方法。本发明针对大潜深AUV在深潜过程中捷联惯导***定位误差快速积累,校正手段贫乏的问题,提出了一种大潜深AUV下潜定位方法,包括:大潜深AUV搭载水下通信节点(水下modem)、温盐深和捷联惯导***;构建大潜深AUV纯距离误差估计滤波模型;基于同步水声通信,水面母船向AUV发送母船各时刻在北-东-地(NED)导航坐标系下的水平位置信息和相应的时间信息;基于同步水声通信传输时间和温盐深传感器信息,AUV计算自身到母船的测距信息;AUV记录自身水声通信***接收信息时刻的捷联惯导***定位信息和深度信息;基于强跟踪UKF算法,融合水面母船水平定位信息、AUV同母船的测距信息和AUV捷联惯导***定位信息、AUV深度信息,分别跟踪深潜过程中AUV在NED导航系下的北向和东向水平定位误差以及载体下AUV的纵向(沿AUV的纵轴x轴)和横向(沿AUV的横轴y轴)测速误差,对捷联惯导***输出进行校正,得到精确的AUV下潜定位信息。本发明基于纯距离信息,对大潜深AUV在下潜阶段捷联惯导***的定位误差进行修正,实现大潜深AUV的下潜定位。
本发明基于强跟踪UKF构建大潜深AUV下潜定位方法,有效克服了母船与AUV间测距信息不确定性大、形式化非线性距离观测模型易产生模型失配等问题,该专利方法仅仅利用通信声纳的测距信息,即可准确跟踪全海深AUV下潜段的定位误差和速度误差,避免了LBL声学信标的布放与回收的高昂船时成本而且不需要配备辅助USBL定位的高精度航姿与定位设备,避免了导航设备的繁复安装,仿真结果表明,该方法能够有效补偿大潜深AUV下潜过程中捷联惯导***的定位与测速误差,只需要声学测距信息辅助,就可使捷联惯导***在大潜深AUV下潜过程中的东向和北向定位精度达到米级,与卫星导航***定位精度相当。
Claims (6)
1.一种大潜深AUV下潜定位方法,其特征在于,包括:
步骤一:通过大潜深AUV搭载水下通信节点、捷联惯导***以及温盐深传感器,得到大潜深AUV的基本信息;
步骤二:构建大潜深AUV的纯距离误差估计滤波模型;
1)设短时间内捷联惯导***的位置误差和速度误差均为常值;
2)建立大潜深AUV的纯距离误差估计滤波模型的离散***方程为:
Xk=ΦkXk-1+wk
其中,
上式中,分别为大潜深AUV在NED导航坐标系下惯导***北向和东向的位置误差,分别为大潜深AUV在载体坐标系下纵向和横向测速误差,T为采样时间,wk~N(0,Q),Q为wk的4×4协方差矩阵,ψ、θ、γ分别为大潜深AUV的艏向、纵倾角和横倾角;
3)将母船同大潜深AUV的测距信息rk和大潜深AUV温盐深传感器输出的深度信息dk作为量测信息,设观测噪声为加性白噪声,建立大潜深AUV纯距离误差估计滤波模型的观测方程为:
zk=h(Xk)+vk
其中,
步骤三:根据同步水声通信协议,水面母船向大潜深AUV发送水面母船各时刻在NED坐标系下的水平位置信息以及相应的时间信息;
步骤四:根据大潜深AUV水声通信***接收到的步骤三所述时间信息,大潜深AUV记录此时的捷联惯导***定位信息以及此时的温盐深传感器输出的温度、导电率和深度信息;
步骤五:根据同步水声通信协议,通过传输时间和温盐深传感器信息,大潜深AUV计算得到自身到母船的测距信息;
步骤六:构建强跟踪UKF算法模型;
1)设强跟踪UKF的初始状态为:
上式中,04×1、04×4分别为4维零矢量和4维全零方阵;
5)根据母船和大潜深AUV的测距信息r和大潜深AUV温盐深传感器输出的深度信息d,得到强跟踪UKF的观测信息zk为:
6)计算量测信息残差为:
7)计算残差协方差矩阵为:
上式中,ρ为遗忘因子,且0<ρ≤1,ρ=0.95;
8)基于UT变换计算***协方差矩阵Q通过确定性观测模型h的传播Qzz;根据和Q,求取Sigma点集{ζi},(i=1,…,L);计算Sigma点{ζi}通过确定性观测模型h的传播ξi=h(ζi),最后计算Qzz为:
9)计算残差中非正交噪声矢量协方差为:
Nk=Vk-R-Qzz;
10)基于UT变换计算k-1时刻状态协方差矩阵Pk,k-1的时间更新通过确定性观测模型h的传播Pzk,k-1;根据和Pk,k-1求取Sigma点集{χi},(i=1,…,L);计算Sigma点{χi}通过确定性观测模型h的传播δi=h(χi);根据计算状态协方差矩阵Pk,k-1的时间更新通过确定性观测模型h的传播Pzk,k-1;
12)状态协方差矩阵的强跟踪时间更新为:
14)计算滤波增益为:
15)滤波状态量测更新为:
16)滤波状态协方差矩阵量测更新为:
步骤七:根据强跟踪UKF算法,融合水面母船水平定位信息、大潜深AUV同母船的测距信息和大潜深AUV捷联惯导***定位信息、大潜深AUV深度信息,跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下AUV的纵向和横向测速误差,对捷联惯导***输出进行校正,得到精确的大潜深AUV下潜定位信息。
2.根据权利要求1所述的一种大潜深AUV下潜定位方法,其特征在于,所述步骤一中,所述步骤一方法的具体步骤为:
1)通过大潜深AUV搭载水下通信节点即水下modem,水面母船搭载水面通信节点即水面modem,将水声通信采用同步协议,使水面modem与水下modem时间同步;
2)通过大潜深AUV搭载捷联惯导***,实时输出大潜深AUV在NED坐标系下的定位信息以及在载体坐标系下的速度信息;
3)通过大潜深AUV搭载温盐深传感器,实时输出大潜深AUV的深度信息、大潜深AUV所处位置的温度信息以及大潜深AUV所处位置的海水导电率即盐度信息。
3.根据权利要求2所述的一种大潜深AUV下潜定位方法,其特征在于:所述步骤三中,所述方法的具体步骤为:
1)水面母船采集各时刻卫星导航的定位信息;
2)水面母船生成自身在NED坐标系下的水平定位数据信息;
3)根据同步水声通信协议,水面母船将上述的定位信息和相应母船水声通信***时间发送至大潜深AUV。
4.根据权利要求3所述的一种大潜深AUV下潜定位方法,其特征在于:所述步骤四中,所述方法的具体步骤为:
1)根据大潜深AUV搭载的水下通信节点即水下modem所采用的串口通信,通过中断方式与大潜深AUV的导航计算机通信;
2)大潜深AUV导航计算机响应水声通信中断后,通过采用串口通信,以轮询方式采集大潜深AUV捷联惯导***的定位信息以及温盐深传感器输出的温度、导电率和深度信息。
5.根据权利要求4所述的一种大潜深AUV下潜定位方法,其特征在于:所述步骤五中,所述方法的具体步骤为:
1)根据同步水声通信协议,大潜深AUV计算得到母船水声通信***时间Ts;
2)根据同步水声通信协议,大潜深AUV计算得到自身水声通信***时间Tr;
3)根据上述结果,大潜深AUV计算得到同步水声通信传输时间T=Tr–Ts;
4)根据温盐深传感器信息,大潜深AUV计算得到水声传播速度c;
5)根据公式rk=(Tr-Ts)c,大潜深AUV计算得到自身同母船的测距信息rk。
6.根据权利要求5所述的一种大潜深AUV下潜定位方法,其特征在于:所述步骤七中,所述方法的具体步骤为:
1)根据强跟踪UKF算法,估计深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差,以及载体坐标系下大潜深AUV的纵向和横向测速误差;
2)根据直接校正方法,补偿大潜深AUV捷联惯导***的定位误差和测速误差,跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下大潜深AUV的纵向和横向测速误差,对捷联惯导***输出进行校正,得到精确的大潜深AUV下潜定位信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910365822.2A CN110057365B (zh) | 2019-05-05 | 2019-05-05 | 一种大潜深auv下潜定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910365822.2A CN110057365B (zh) | 2019-05-05 | 2019-05-05 | 一种大潜深auv下潜定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110057365A CN110057365A (zh) | 2019-07-26 |
CN110057365B true CN110057365B (zh) | 2022-06-21 |
Family
ID=67322096
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910365822.2A Active CN110057365B (zh) | 2019-05-05 | 2019-05-05 | 一种大潜深auv下潜定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110057365B (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110471096A (zh) * | 2019-09-11 | 2019-11-19 | 哈尔滨工程大学 | 一种分布式海底飞行节点群体定位方法 |
CN110865333B (zh) * | 2019-11-19 | 2021-08-24 | 浙江大学 | 洋流影响下水下滑翔机单信标无源声学定位方法 |
CN111578936B (zh) * | 2020-05-09 | 2022-08-02 | 东南大学 | 基于imm-ukf的惯性/超短基线多参数标定方法 |
CN111829512B (zh) * | 2020-06-08 | 2024-04-09 | 中国航天空气动力技术研究院 | 一种基于多传感器数据融合的auv导航定位方法及*** |
CN111982117B (zh) * | 2020-08-17 | 2022-05-10 | 电子科技大学 | 一种基于深度学习的auv光学引导与测向方法 |
CN112432645B (zh) * | 2020-09-30 | 2023-04-07 | 河北汉光重工有限责任公司 | 深海潜航器下潜路径规划方法以及导航位置误差估计方法 |
CN113624211B (zh) * | 2021-10-12 | 2022-01-25 | 自然资源部第一海洋研究所 | Ladcp与usbl组合观测装置及其使用方法 |
CN114234964B (zh) * | 2021-11-05 | 2024-06-14 | 河北汉光重工有限责任公司 | 一体化自主式水下潜器在冰层下的定位方法及*** |
CN114966711B (zh) * | 2022-05-20 | 2023-01-24 | 国家深海基地管理中心 | 一种面向载人潜水器的海水深度确定方法及*** |
CN114993302B (zh) * | 2022-05-27 | 2024-05-28 | 中国人民解放军海军工程大学 | 基于多柔性节点的水下智能定位***和方法 |
CN115167485A (zh) * | 2022-06-23 | 2022-10-11 | 国家深海基地管理中心 | 一种自主水下航行器和载人潜水器的协同作业*** |
CN115409160B (zh) * | 2022-09-29 | 2023-04-28 | 中国科学院声学研究所 | 一种基于定深数据的全海深温度剖面反演方法及*** |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102495420A (zh) * | 2011-12-13 | 2012-06-13 | 大连海事大学 | 一种水下目标精确定位***及方法 |
CN104331087A (zh) * | 2014-10-24 | 2015-02-04 | 浙江大学 | 一种鲁棒的水下传感器网络目标跟踪方法 |
CN106017467A (zh) * | 2016-07-28 | 2016-10-12 | 中国船舶重工集团公司第七0七研究所 | 一种基于多水下应答器的惯性/水声组合导航方法 |
KR101755307B1 (ko) * | 2016-05-11 | 2017-07-10 | 한국해양과학기술원 | 수중 이동체의 위치 측정 오차 보정 방법 |
CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
CN109447133A (zh) * | 2018-10-11 | 2019-03-08 | 东南大学 | 一种基于svr算法的位置信息野值的剔除方法 |
-
2019
- 2019-05-05 CN CN201910365822.2A patent/CN110057365B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102495420A (zh) * | 2011-12-13 | 2012-06-13 | 大连海事大学 | 一种水下目标精确定位***及方法 |
CN104331087A (zh) * | 2014-10-24 | 2015-02-04 | 浙江大学 | 一种鲁棒的水下传感器网络目标跟踪方法 |
KR101755307B1 (ko) * | 2016-05-11 | 2017-07-10 | 한국해양과학기술원 | 수중 이동체의 위치 측정 오차 보정 방법 |
CN106017467A (zh) * | 2016-07-28 | 2016-10-12 | 中国船舶重工集团公司第七0七研究所 | 一种基于多水下应答器的惯性/水声组合导航方法 |
CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
CN109447133A (zh) * | 2018-10-11 | 2019-03-08 | 东南大学 | 一种基于svr算法的位置信息野值的剔除方法 |
Non-Patent Citations (3)
Title |
---|
Range-only navigation algorithm for positioning of deep-diving AUV;Zhang Qiang等;《2017 IEEE International Conference on Cybernetics and Intelligent Systems (CIS) and IEEE Conference on Robotics, Automation and Mechatronics (RAM)》;20180201;第2326-8239页 * |
Range-Only Positioning of a Deep-Diving Autonomous Underwater Vehicle From a Surface Ship;Stephen D. McPhail等;《IEEE Journal of Oceanic Engineering》;20091020;第34卷(第4期);第669-677页 * |
小型AUV水下导航***关键技术研究;张强;《中国优秀博硕士学位论文全文数据库(博士) 信息科技辑》;20120515(第5期);第I136-100页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110057365A (zh) | 2019-07-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110057365B (zh) | 一种大潜深auv下潜定位方法 | |
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
Eustice et al. | Experimental results in synchronous-clock one-way-travel-time acoustic navigation for autonomous underwater vehicles | |
CN109765523B (zh) | 基于自适应akf的单应答器斜距水声定位方法及*** | |
Hegrenas et al. | Model-aided inertial navigation for underwater vehicles | |
Eustice et al. | Recent advances in synchronous-clock one-way-travel-time acoustic navigation | |
CN103697910B (zh) | 自主水下航行器多普勒计程仪安装误差的校正方法 | |
Mandt et al. | Integrating DGPS-USBL position measurements with inertial navigation in the HUGIN 3000 AUV | |
CN112284384B (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN113819906A (zh) | 一种基于统计相似度量的组合导航鲁棒滤波方法 | |
CN107677272A (zh) | 一种基于非线性信息滤波的auv协同导航方法 | |
CN110274591B (zh) | 深潜载人潜水器adcp辅助sins导航方法 | |
CN105910603B (zh) | 一种通信延迟下的多auv协同导航滤波方法 | |
CN110940332B (zh) | 考虑航天器轨道动态效应的脉冲星信号相位延迟估计方法 | |
Bo et al. | Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
CN110865333B (zh) | 洋流影响下水下滑翔机单信标无源声学定位方法 | |
CN111982105A (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
Wang et al. | A novel adaptive sliding observation-based cooperative positioning algorithm under factor graph framework for multiple UUVs | |
CN114777812A (zh) | 一种水下组合导航***行进间对准与姿态估计方法 | |
CN116772903B (zh) | 基于迭代ekf的sins/usbl安装角估计方法 | |
Zhang et al. | A passive acoustic positioning algorithm based on virtual long baseline matrix window | |
CN111982126B (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 |
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 |