CN113280812A - 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 - Google Patents

一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 Download PDF

Info

Publication number
CN113280812A
CN113280812A CN202110519870.XA CN202110519870A CN113280812A CN 113280812 A CN113280812 A CN 113280812A CN 202110519870 A CN202110519870 A CN 202110519870A CN 113280812 A CN113280812 A CN 113280812A
Authority
CN
China
Prior art keywords
sins
lbl
difference
value
elevation
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.)
Granted
Application number
CN202110519870.XA
Other languages
English (en)
Other versions
CN113280812B (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.)
Sun Yat Sen University
Sun Yat Sen University Shenzhen Campus
Original Assignee
Sun Yat Sen 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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202110519870.XA priority Critical patent/CN113280812B/zh
Publication of CN113280812A publication Critical patent/CN113280812A/zh
Application granted granted Critical
Publication of CN113280812B publication Critical patent/CN113280812B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明公开了一种基于SINS/LBL紧组合的高程约束水下自适应导航方法及装置，通过使用SINS获取AUV位置信息，根据所述AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差；获取通过LBL声源到所述第i个应答器与LBL声源到参考应答器的斜距差，记为基于LBL斜距差；将所述基于SINS斜距差与所述基于LBL斜距差做差运算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观测值；将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应Kalman滤波方程可得到k时刻的状态估计值；用所述高程误差估计值对固有的SINS输出值的误差进行修正处理，获取修正误差后的SINS的导航信息。本发明相对于现有技术提高了水下导航定位的准确性。

Description

一种基于SINS/LBL紧组合的高程约束水下自适应导航方法及 装置
技术领域
本发明涉及水下多传感器组合导航的技术领域，特别是涉及一种 基于SINS/LBL紧组合的高程约束水下自适应导航方法及装置。
背景技术
随着水下导航器技术的不断发展，导航系统成为水下航行器研究 的主要技术核心，实现水下精确定位成为目前水下航行器定位导航系 统研究的一个重要分支。由于单一的导航自身的存在的不足，因而组 合导航技术已成为导航技术发展的重要方向之一，研究SINS与水声定 位技术的组合导航具有重要意义。
目前的组合导航系统一般都将惯导设备作为主要参考导航系统。 SINS体积小、功耗低，有着良好的隐蔽性，不受工作介质的影响，且 能够提供较为全面的导航信息，在短期内具有较高精度，数据更新频 率高。缺点是当其单独长期工作时稳定性很差，位置和速度误差会随 着时间积累，从而导致最终定位误差较大，甚至发散。尤其是高程通 道，纯惯导系统只要受到扰动，其高度通道就会变得极其不稳定。特 别是在水下工作时，由于水下环境复杂多变且不稳定，对导航系统的 稳定性带来了很大的挑战。
发明内容
本发明要解决的技术问题是：提供一种基于SINS/LBL紧组合的高 程约束水下自适应导航方法及装置，提高水下导航定位的准确性。
为了解决上述技术问题，本发明提供了一种基于SINS/LBL紧组合 的高程约束水下自适应导航方法，包括：
使用SINS获取AUV位置信息，根据所述AUV的位置信息和海底各 应答器的位置信息获取基于SINS斜距差；其中，所述海底各应答器包 括i个应答器和一个参考应答器，i的取值范围为1到3的正整数；
获取通过LBL声源到所述第i个应答器与LBL声源到参考应答器 的斜距差，记为基于LBL斜距差；
将所述基于SINS斜距差与所述基于LBL斜距差做差运算，获取所 述基于SINS与所述基于LBL的斜距差之差，记为外部观测值；
将所述外部观测值作为高程约束自适应的滤波计算的输入值，根 据k时刻的量测值，通过高程约束自适应Kalman滤波方程可得到k时 刻的状态估计值；
用所述高程误差估计值对固有的SINS输出值的误差进行修正处理， 获取修正误差后的SINS的导航信息。
进一步的，开始高程约束自适应Kalman滤波前，还包括：
从线性离散系统的状态方程和量测方程中获取满足预设条件的系 统激励噪音矩阵和量测噪音矩阵，再从满足第一预设条件的系统激励 噪音矩阵和量测噪音矩阵中提取相对应的系统噪声方差阵和量测噪音 方差阵，若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件， 则开始高程约束自适应Kalman滤波。
进一步的，使用SINS获取AUV位置信息，根据所述AUV的位置信 息和海底各应答器的位置信息获取基于SINS斜距差，具体为：
设SINS解算的AUV位置为(xS，yS，zS)，真实AUV位置为(x，y，z)，海 底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第四个应答器 (x0，y0，z0)为参考应答器，SINS解算的AUV位置与另外三个应答器的斜 距分别与第四个斜距作差，得到基于SINS的斜距差ρSINSi，其中，i＝ (1，2，3)，即：
Figure BDA0003062060380000031
将上式相对AUV真实位置进行Taylor线性化得：
ρSINSi＝Ri-R0+eixδx+eiyδy+eizδz
Figure BDA0003062060380000032
Figure BDA0003062060380000033
Figure BDA0003062060380000034
其中，Ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；R0表示声源与 参考应答器的斜距，有：
Figure BDA0003062060380000035
Figure BDA0003062060380000036
进一步的，获取通过LBL声源到所述第i个应答器和LBL声源到 参考应答器的斜距差，记为基于LBL斜距差，具体为：
LBL声源到所述第i个应答器的斜距：
Figure BDA0003062060380000037
其中，i＝(1，2，3)；
LBL声源到参考应答器的斜距：
Figure BDA0003062060380000038
基于LBL斜距差：
Figure BDA0003062060380000039
其中，δRi表示LBL斜距差的测量值误差，
Figure BDA00030620603800000310
表示LBL斜距差中存在 的噪音。
进一步的，将所述基于SINS斜距差与所述基于LBL斜距差做差运 算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观测 值，具体为：
Figure BDA0003062060380000041
将所述外部观测值δρi记为外部观测值。
进一步的，通过高程约束自适应Kalman滤波方程可得到k时刻的 状态估计值，具体为：
若系统噪声方差阵为非负定阵，量测噪声方差阵为正定阵，则：
一步状态预测方程：
Figure BDA0003062060380000042
一步估计误差协方差阵方程：
Figure BDA0003062060380000043
增益矩阵：
Figure BDA0003062060380000044
协方差矩阵估计：
Figure BDA0003062060380000045
状态估计及高程约束过程：
Figure BDA0003062060380000046
Figure BDA0003062060380000047
Figure BDA0003062060380000048
其中：
Figure BDA0003062060380000051
为k时刻的状态向量估计；
Figure BDA0003062060380000052
表示的是k时刻高程误差的估计值；
k时刻SINS解算的AUV位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据 为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值，即： ΔH(p，k)＝z(s，k)-z(p，k)
δHp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；
若高程误差估计值
Figure BDA0003062060380000053
超过阈值α·δHp，则要使用压力传感器 对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤 波估计。
进一步的，在使用本方法的同时还需要建立SINS/LBL紧组合导航 系统状态方程和量测方程，具体为：
SINS/LBL紧组合状态方程可描述为：
Figure BDA0003062060380000054
其中，XSINS、XLBL分别为SINS、LBL的状态变量，FSINS、FLBL分别为SINS、 LBL的状态矩阵，WSINS、WLBL分别为SINS、LBL的状态噪声。
SINS选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺 漂移作为状态量：
由SINS误差方程可确定FSINS，即：
Figure BDA0003062060380000055
LBL系统选择斜距差误差作为状态量，即：
XLBL＝[δR1 δR2 δR3]T
其中，δR为斜距差误差，可用一阶Markov过程表示。
LBL的量测矩阵为：
Figure BDA0003062060380000061
SINS/LBL紧组合的量测方程可描述为：
Figure BDA0003062060380000062
其中，ρSINS表示基于SINS的斜距差；ρLBL表示基于LBL的斜距差； ρSINS-ρLBL为基于SINS的斜距差与基于LBL的斜距差的差值；η为量 测噪声，H为量测矩阵。
进一步的，本发明还提供了一种基于SINS/LBL紧组合的高程约束 水下自适应导航装置，包括：第一斜距差模块、第二斜距差模块、斜 距差计算模块、数据处理模块和修正模块，
所述第一斜距差模块用于使用SINS获取AUV位置信息，根据所述 AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差；其 中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范 围为1到3的正整数；
所述第二斜距差模块用于获取通过LBL声源到所述第i个应答器 与LBL声源到参考应答器的斜距差，记为基于LBL斜距差；
所述斜距差计算模块用于将所述基于SINS斜距差与所述基于LBL 斜距差做差运算，获取所述基于SINS与所述基于LBL的斜距差之差， 记为外部观测值；
所述数据处理模块用于将所述外部观测值作为高程约束自适应的 滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应Kalman 滤波方程可得到k时刻的状态估计值；
所述修正模块用于用所述高程误差估计值对固有的SINS输出值的 误差进行修正处理，获取修正误差后的SINS的导航信息。
进一步的，一种终端设备，包括处理器、存储器以及存储在所述 存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执 行所述计算机程序时实现如权利要求1至7任意一项所述的基于 SINS/LBL紧组合的高程约束水下自适应导航方法。
进一步的，一种计算机可读存储介质，所述计算机可读存储介质 包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计 算机可读存储介质所在设备执行如权利要求1至7中任意一项所述的 基于SINS/LBL紧组合的高程约束水下自适应导航方法。
本发明实施例一种基于SINS/LBL紧组合的高程约束水下自适应导 航方法及装置，与现有技术相比，具有如下有益效果：
本发明提供的一种基于SINS/LBL紧组合的高程约束水下自适应导 航方法及装置，通过使用SINS获取AUV位置信息，根据所述AUV的位 置信息和海底各应答器的位置信息获取基于SINS斜距差；获取通过 LBL声源到所述第i个应答器与LBL声源到参考应答器的斜距差，记为 基于LBL斜距差；将所述基于SINS斜距差与所述基于LBL斜距差做差 运算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观 测值；将所述外部观测值作为高程约束自适应的滤波计算的输入值， 根据k时刻的量测值，通过高程约束自适应Kalman滤波方程可得到k 时刻的状态估计值；用所述高程误差估计值对固有的SINS输出值的误 差进行修正处理，获取修正误差后的SINS的导航信息。本发明相对于 现有技术提高了水下导航定位的准确性。
附图说明
图1是本发明提供的基于SINS/LBL紧组合的高程约束水下自适应 导航方法的一种实施例的流程示意图；
图2是本发明提供的基于SINS/LBL紧组合的高程约束水下自适应 导航装置的一种实施例的结构示意图。
具体实施方式
下面将结合本发明中的附图，对本发明实施例中的技术方案进行 清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施 例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术 人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于 本发明保护的范围。
由于本发明提供的高程约束水下自适应导航方法是基于SINS/LBL 紧组合导航系统模型，所述SINS/LBL紧组合导航系统模型建立的具体 实现步骤如步骤S1和步骤S2所示；
S1、建立LBL斜距差模型。
由于海洋环境的不确定性，声信号在海水中的传播受到许多影响， 因此对于斜距差的计算会造成一定误差。斜距差误差可以用一阶 Markov过程表示，即斜距差误差模型为：
Figure BDA0003062060380000081
Figure BDA0003062060380000082
其中，ΔRi表示LBL斜距差的真实值；δRi表示LBL斜距差的测量值误 差；
Figure BDA0003062060380000083
表示高斯白噪声；
Figure BDA0003062060380000084
分别表示相关时间和驱动白噪声。
S2、建立SINS/LBL紧组合导航系统状态方程和量测方程。
(1)状态方程的建立
SINS/LBL紧组合状态方程可描述为：
Figure BDA0003062060380000091
其中，XSINS、XLBL分别为SINS、LBL的状态变量，FSINS、FLBL分别为 SINS、LBL的状态矩阵，WSINS、WLBL分别为SINS、LBL的状态噪声。
SINS选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺 漂移作为状态量：
Figure RE-GDA0003130222090000084
其中，15项状态量分别对应捷联惯导的东向速度误差、北向速度误差、 天向速度误差、东向失准角、北向失准角、天向失准角、纬度误差、 经度误差、高度误差、加速度计的三个轴向偏差、陀螺仪的三个轴向 漂移。
由SINS误差方程可确定FSINS，即：
Figure BDA0003062060380000093
Figure BDA0003062060380000094
Figure BDA0003062060380000095
其中，矩阵F9×9中的元素为：
Figure BDA0003062060380000101
Figure BDA0003062060380000102
Figure BDA0003062060380000103
Figure BDA0003062060380000104
Figure BDA0003062060380000105
Figure BDA0003062060380000106
Figure BDA0003062060380000107
Figure BDA0003062060380000108
Figure BDA0003062060380000109
Figure BDA00030620603800001010
Figure BDA00030620603800001011
Figure BDA00030620603800001012
Figure BDA00030620603800001013
Figure BDA00030620603800001014
其中，
Figure BDA00030620603800001015
是载体的姿态变换矩阵；RN是参考椭球子午面的曲率半径；RE是垂直子午面内的曲率半径；Re是参考椭球的长轴半径；e是椭球的 第一偏心率。它们之间满足如下关系式：
RN＝Re(1-2e+3e sin2L)
RE＝Re(1+e sin2L)
LBL系统选择斜距差误差作为状态量，即：
XLBL＝[δR1 δR2 δR3]T
其中，δR为斜距差误差，可用一阶Markov过程表示。
LBL的量测矩阵为：
Figure BDA0003062060380000111
(2)量测方程的建立
SINS/LBL紧组合的量测方程可描述为：
Figure BDA0003062060380000112
其中，ρSINS表示基于SINS的斜距差；ρLBL表示基于LBL的斜距差； ρSINS-ρLBL为基于SINS的斜距差与基于LBL的斜距差的差值；η为量 测噪声，H为量测矩阵。
对量测矩阵H进行推导，由于惯导输出的位置为经纬高形式，则需 要将上式置换到大地坐标系后构造量测方程。δx，δy，δz到δL，δλ，δ置 换公式为：
δx＝δh cos L cosλ-(RE+h)sin L cosλδL-(RE+h)cos L sinλδλ
δy＝δh cos L sinλ-(RE+h)sin L sinλδL-(RE+h)cos L cos λδλ
δz＝δh sin L+[RE(1-e2)+h]cos LδL
量测矩阵可表示为：
H＝[03×6 H1(3×3) 03×6 -I3×3]
其中，I为单位向量，
Figure BDA0003062060380000121
矩阵中的各元素表达式如下：
ai1＝-(RE+h)sin L cosλeiX-(RE+h)sin L sinλeiy +[RE(1-e2)+h]cos L eiz
ai2＝-(RE+h)cos L cosλeiX-(RE+h)cos L cosλeiy
ai3＝cos L cosλeiX+sin L sinλeiy+sin L eiz
其中，i＝(1，2，3)，e为椭球的扁心率，RE为垂直子午面内的曲率半径。
基于上述步骤建立的SINS/LBL紧组合导航系统模型，主要用于本 发明所提供的一种基于SINS/LBL紧组合的高程约束水下自适应导航方 法及装置。
实施例1
参见图1，图1是本发明提供的基于SINS/LBL紧组合的高程约束 水下自适应导航方法的一种实施例的流程示意图，如图1所示，该方 法包括步骤101到步骤105，具体如下：
步骤101：使用SINS获取AUV位置信息，根据所述AUV的位置信 息和海底各应答器的位置信息获取基于SINS斜距差；其中，所述海底 各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3 的正整数。
本实施例中，设SINS解算的AUV位置为(xS，yS，zS)，真实AUV位置 为(x，y，z)，海底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第 四个应答器(x0，y0，z0)为参考应答器，SINS解算的AUV位置与另外三个 应答器的斜距分别与第四个斜距作差，得到基于SINS的斜距差ρSINSi， 其中，i＝(1，2，3)，即：
Figure BDA0003062060380000131
将上式相对AUV真实位置进行Taylor线性化得：
ρSINSi＝Ri-R0+eixδx+eiyδy+eizδz
Figure BDA0003062060380000132
Figure BDA0003062060380000133
Figure BDA0003062060380000134
其中，Ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；R0表示声源与 参考应答器的斜距，有：
Figure BDA0003062060380000135
Figure BDA0003062060380000136
步骤102：获取通过LBL声源到所述第i个应答器与LBL声源到参 考应答器的斜距差，记为基于LBL斜距差。
本实施例中，LBL声源到所述第i个应答器的斜距：
Figure BDA0003062060380000137
其中，i＝(1，2，3)；
LBL声源到参考应答器的斜距：
Figure BDA0003062060380000138
基于LBL斜距差：
Figure BDA0003062060380000139
其中，δRi表示LBL斜距差的测量值误差，
Figure BDA0003062060380000141
表示LBL斜距差中存在 的噪音。
步骤103：将所述基于SINS斜距差与所述基于LBL斜距差做差运 算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观测 值。
本实施例中，
Figure BDA0003062060380000142
将所述外部观测值δρi记为外部观测值。
步骤104：将所述外部观测值作为高程约束自适应的滤波计算的输 入值，根据k时刻的量测值，通过高程约束自适应Kalman滤波方程可 得到k时刻的状态估计值。
本实施例中，开始高程约束自适应Kalman滤波前，还包括：
从线性离散系统的状态方程和量测方程中获取满足预设条件的系 统激励噪音矩阵和量测噪音矩阵，线性离散系统的状态方程和量测方 程可以描述为：
Xk＝Φk，k-1Xk-1+Гk-1Wk-1
Zk＝HkXk+Vk
其中，Xk表示tk时刻需要估计的状态向量；Φk，k-1表示tk-1时刻到tk时 刻的一步转移矩阵；Гk-1表示系统噪声驱动矩阵；Zk表示量测向量；Hk为量测矩阵；Wk-1表示系统激励噪声矩阵；Vk表示量测噪声矩阵；工 程上通常假定Wk，Vk互不相关的均值为零的高斯白噪声；
从满足第一预设条件的系统激励噪音矩阵和量测噪音矩阵中提取 相对应的系统噪声方差阵和量测噪音方差阵，其中，第一预设条件为：
E[Wk]＝0，E[WkWj T]＝Qkδkj
E[Vk]＝0，E[VkVj T]＝Rkδkj
E[WkVj T]＝0
其中，Qk为系统噪声方差阵；Vk为量测噪声方差阵；δkj是Kroneckerδ函 数；
若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件，则 开始高程约束自适应Kalman滤波。其中，第二预设条件为系统噪声方 差阵为非负定阵，量测噪声方差阵为正定阵。
所述外部观测值作为高程约束自适应的滤波计算的输入值，给定 合适的初值
Figure BDA0003062060380000151
和P0以及α，根据k时刻的量测值Zk，通过高程约束自适 应Kalman滤波方程可得到k时刻的状态估计值，
一步状态预测方程：
Figure BDA0003062060380000152
一步估计误差协方差阵方程：
Figure BDA0003062060380000153
增益矩阵：
Figure BDA0003062060380000154
协方差矩阵估计：
Figure BDA0003062060380000155
状态估计及高程约束过程：
Figure BDA0003062060380000156
Figure BDA0003062060380000157
Figure BDA0003062060380000161
其中：
Figure BDA0003062060380000162
为k时刻的状态向量估计；
Figure BDA0003062060380000163
表示的是k时刻高程误差的估计值；
k时刻SINS解算的AUV位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据 为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值。即： ΔH(p，k)＝z(s，k)-z(p，k)；
δHp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；
若高程误差估计值
Figure BDA0003062060380000164
超过阈值α·δHp，则要使用压力传感器 对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤 波估计。
步骤105：用所述高程误差估计值对固有的SINS输出值的误差进 行修正处理，获取修正误差后的SINS的导航信息。
本实施例中，由于SINS工作时稳定性较差，位置和速度误差会随 着时间积累，从而导致最终的定位误差较大，甚至发散，因此用高程 误差估计值对固有的SINS输出值误差进行修正处理，得到修正后的 SINS导航信息。
参见图2，图2是本发明提供的基于SINS/LBL紧组合的高程约束 水下自适应导航装置的一种实施例的结构示意图，如图2所示，该装 置包括：
第一斜距差模块201用于使用SINS获取AUV位置信息，根据所述 AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差；其 中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范 围为1到3的正整数。
本实施例中，设SINS解算的AUV位置为(xS，yS，zS)，真实AUV位置 为(x，y，z)，海底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第 四个应答器(x0，y0，z0)为参考应答器，SINS解算的AUV位置与另外三个 应答器的斜距分别与第四个斜距作差，得到基于SINS的斜距差ρSINSi， 其中，i＝(1，2，3)，即：
Figure BDA0003062060380000171
将上式相对AUV真实位置进行Taylor线性化得：
ρSINSi＝Ri-R0+eixδx+eiyδy+eizδz
Figure BDA0003062060380000172
Figure BDA0003062060380000173
Figure BDA0003062060380000174
其中，Ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；R0表示声源与 参考应答器的斜距，有：
Figure BDA0003062060380000175
Figure BDA0003062060380000176
第二斜距差模块202用于获取通过LBL声源到所述第i个应答器 与LBL声源到参考应答器的斜距差，记为基于LBL斜距差。
本实施例中，LBL声源到所述第i个应答器的斜距：
Figure BDA0003062060380000177
其中，i＝(1，2，3)；
LBL声源到参考应答器的斜距：
Figure BDA0003062060380000181
基于LBL斜距差：
Figure BDA0003062060380000182
其中，δRi表示LBL斜距差的测量值误差，
Figure BDA0003062060380000183
表示LBL斜距差中存在 的噪音。
斜距差计算模块203用于将所述基于SINS斜距差与所述基于LBL 斜距差做差运算，获取所述基于SINS与所述基于LBL的斜距差之差， 记为外部观测值。
本实施例中，
Figure BDA0003062060380000184
Figure BDA0003062060380000185
将所述外部观测值δρi记为外部观测值。
数据处理模块204用于将所述外部观测值作为高程约束自适应的 滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应Kalman 滤波方程可得到k时刻的状态估计值。
本实施例中，开始高程约束自适应Kalman滤波前，还包括：
从线性离散系统的状态方程和量测方程中获取满足预设条件的系 统激励噪音矩阵和量测噪音矩阵，线性离散系统的状态方程和量测方 程可以描述为：
Xk＝Φk，k-1Xk-1+Гk-1Wk-1
Zk＝HkXk+Vk
其中，Xk表示tk时刻需要估计的状态向量；Φk，k-1表示tk-1时刻到tk时 刻的一步转移矩阵；Гk-1表示系统噪声驱动矩阵；Zk表示量测向量；Hk为量测矩阵；Wk-1表示系统激励噪声矩阵；Vk表示量测噪声矩阵；工 程上通常假定Wk，Vk互不相关的均值为零的高斯白噪声；
从满足第一预设条件的系统激励噪音矩阵和量测噪音矩阵中提取 相对应的系统噪声方差阵和量测噪音方差阵，其中，第一预设条件为：
E[Wk]＝0，E[WkWj T]＝Qkδkj
E[Vk]＝0，E[VkVj T]＝Rkδkj
E[WkVj T]＝0
其中，Qk为系统噪声方差阵；Vk为量测噪声方差阵；δkj是Kroneckerδ函 数；
若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件，则 开始高程约束自适应Kalman滤波。其中，第二预设条件为系统噪声方 差阵为非负定阵，量测噪声方差阵为正定阵。
所述外部观测值作为高程约束自适应的滤波计算的输入值，给定 合适的初值
Figure BDA0003062060380000191
和P0以及α，根据k时刻的量测值Zk，通过高程约束自适 应Kalman滤波方程可得到k时刻的状态估计值，
一步状态预测方程：
Figure BDA0003062060380000192
一步估计误差协方差阵方程：
Figure BDA0003062060380000193
增益矩阵：
Figure BDA0003062060380000194
协方差矩阵估计：
Figure BDA0003062060380000201
状态估计及高程约束过程：
Figure BDA0003062060380000202
Figure BDA0003062060380000203
Figure BDA0003062060380000204
其中：
Figure BDA0003062060380000205
为k时刻的状态向量估计；
Figure BDA0003062060380000206
表示的是k时刻高程误差的估计值；
k时刻SINS解算的AUV位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据 为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值。即： ΔH(p，k)＝z(s，k)-z(p，k)；
δHp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；
若高程误差估计值
Figure BDA0003062060380000207
超过阈值α·δHp，则要使用压力传感器 对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤 波估计。
修正模块205用于用所述高程误差估计值对固有的SINS输出值的 误差进行修正处理，获取修正误差后的SINS的导航信息。
本实施例中，由于SINS工作时稳定性较差，位置和速度误差会随 着时间积累，从而导致最终的定位误差较大，甚至发散，因此用高程 误差估计值对固有的SINS输出值误差进行修正处理，得到修正后的 SINS导航信息。
综上，本发明提供的一种基于SINS/LBL紧组合的高程约束水下自 适应导航方法及装置，通过使用SINS获取AUV位置信息，根据所述AUV 的位置信息和海底各应答器的位置信息获取基于SINS斜距差；获取通 过LBL声源到所述第i个应答器与LBL声源到参考应答器的斜距差， 记为基于LBL斜距差；将所述基于SINS斜距差与所述基于LBL斜距差 做差运算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外 部观测值；将所述外部观测值作为高程约束自适应的滤波计算的输入 值，根据k时刻的量测值，通过高程约束自适应Kalman滤波方程可得 到k时刻的状态估计值；用所述高程误差估计值对固有的SINS输出值的误差进行修正处理，获取修正误差后的SINS的导航信息，提高了水 下导航定位的准确性。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领 域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以 做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (10)

1.一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，包括：
使用SINS获取AUV位置信息，根据所述AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差；其中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3的正整数；
获取通过LBL声源到所述第i个应答器与LBL声源到参考应答器的斜距差，记为基于LBL斜距差；
将所述基于SINS斜距差与所述基于LBL斜距差做差运算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观测值；
将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应Kalman滤波方程可得到k时刻的状态估计值；
用所述高程误差估计值对固有的SINS输出值的误差进行修正处理，获取修正误差后的SINS的导航信息。
2.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，开始高程约束自适应Kalman滤波前，还包括：
从线性离散系统的状态方程和量测方程中获取满足预设条件的系统激励噪音矩阵和量测噪音矩阵，再从满足第一预设条件的系统激励噪音矩阵和量测噪音矩阵中提取相对应的系统噪声方差阵和量测噪音方差阵，若所述系统噪声方差阵和量测噪音方差阵满足第二预设条件，则开始高程约束自适应Kalman滤波。
3.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，使用SINS获取AUV位置信息，根据所述AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差，具体为：
设SINS解算的AUV位置为(xS，yS，zS)，真实AUV位置为(x，y，z)，海底四个应答器位置为(xj，yj，zj)，其中，j＝(0，1，2，3)，设第四个应答器(x0，y0，z0)为参考应答器，SINS解算的AUV位置与另外三个应答器的斜距分别与第四个斜距作差，得到基于SINS的斜距差ρSINSi，其中，i＝(1，2，3)，即：
Figure FDA0003062060370000021
将上式相对AUV真实位置进行Taylor线性化得：
ρSINSi＝Ri-R0+eixδx+eiyδy+eizδz
Figure FDA0003062060370000022
Figure FDA0003062060370000023
Figure FDA0003062060370000024
其中，Ri表示声源与第i个应答器的斜距，i＝(0，1，2，3)；R0表示声源与参考应答器的斜距，有：
Figure FDA0003062060370000025
Figure FDA0003062060370000026
4.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，获取通过LBL声源到所述第i个应答器和LBL声源到参考应答器的斜距差，记为基于LBL斜距差，具体为：
LBL声源到所述第i个应答器的斜距：
Figure FDA0003062060370000031
其中，i＝(1，2，3)；
LBL声源到参考应答器的斜距：
Figure FDA0003062060370000032
基于LBL斜距差：
Figure FDA0003062060370000033
其中，δRi表示LBL斜距差的测量值误差，
Figure FDA0003062060370000034
表示LBL斜距差中存在的噪音。
5.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，将所述基于SINS斜距差与所述基于LBL斜距差做差运算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观测值，具体为：
Figure FDA0003062060370000035
将所述外部观测值δρi记为外部观测值。
6.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，通过高程约束自适应Kalman滤波方程可得到k时刻的状态估计值，具体为：
若系统噪声方差阵为非负定阵，量测噪声方差阵为正定阵，则：
一步状态预测方程：
Figure FDA0003062060370000041
一步估计误差协方差阵方程：
Figure FDA0003062060370000042
增益矩阵：
Figure FDA0003062060370000043
协方差矩阵估计：
Figure FDA0003062060370000044
状态估计及高程约束过程：
Figure FDA0003062060370000045
Figure FDA0003062060370000046
Figure FDA0003062060370000047
其中：
Figure FDA0003062060370000048
为k时刻的状态向量估计；
Figure FDA0003062060370000049
表示的是k时刻高程误差的估计值；
k时刻SINS解算的AUV位置为(x(s，k)，y(s，k)，z(s，k))，其中，高程数据为z(s，k)；k时刻压力传感器输出的高程数据为z(p，k)，取二者差值，即：ΔH(p，k)＝z(s，k)-z(p，k)；
δHp为压力传感器的精度，α按经验取值为α∈[0.8，1.5]；
若高程误差估计值
Figure FDA00030620603700000410
超过阈值α·δHp，则要使用压力传感器对所述高程误差估计值进行约束；若未超过阈值，则继续下一时刻滤波估计。
7.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法，其特征在于，在使用本方法前还需要建立SINS/LBL紧组合导航系统状态方程和量测方程，具体为：
SINS/LBL紧组合状态方程可描述为：
Figure FDA0003062060370000051
其中，XSINS、XSINS分别为SINS、LBL的状态变量，FSINS、FLBL分别为SINS、LBL的状态矩阵，WSINS、WLBL分别为SINS、LBL的状态噪声。
SINS选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺漂移作为状态量：
由SINS误差方程可确定FSINS，即：
Figure FDA0003062060370000052
LBL系统选择斜距差误差作为状态量，即：
XLBL＝[δR1 δR2 δR3]T
其中，δR为斜距差误差，可用一阶Markov过程表示。
LBL的量测矩阵为：
Figure FDA0003062060370000053
SINS/LBL紧组合的量测方程可描述为：
Figure FDA0003062060370000061
其中，ρSINS表示基于SINS的斜距差；ρLBL表示基于LBL的斜距差；ρSINS-ρLBL为基于SINS的斜距差与基于LBL的斜距差的差值；η为量测噪声，H为量测矩阵。
8.一种基于SINS/LBL紧组合的高程约束水下自适应导航装置，其特征在于，包括：第一斜距差模块、第二斜距差模块、斜距差计算模块、数据处理模块和修正模块，
所述第一斜距差模块用于使用SINS获取AUV位置信息，根据所述AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差；其中，所述海底各应答器包括i个应答器和一个参考应答器，i的取值范围为1到3的正整数；
所述第二斜距差模块用于获取通过LBL声源到所述第i个应答器与LBL声源到参考应答器的斜距差，记为基于LBL斜距差；
所述斜距差计算模块用于将所述基于SINS斜距差与所述基于LBL斜距差做差运算，获取所述基于SINS与所述基于LBL的斜距差之差，记为外部观测值；
所述数据处理模块用于将所述外部观测值作为高程约束自适应的滤波计算的输入值，根据k时刻的量测值，通过高程约束自适应Kalman滤波方程可得到k时刻的状态估计值；
所述修正模块用于用所述高程误差估计值对固有的SINS输出值的误差进行修正处理，获取修正误差后的SINS的导航信息。
9.一种终端设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至7任意一项所述的基于SINS/LBL紧组合的高程约束水下自适应导航方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至7中任意一项所述的基于SINS/LBL紧组合的高程约束水下自适应导航方法。
CN202110519870.XA 2021-05-12 2021-05-12 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 Active CN113280812B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110519870.XA CN113280812B (zh) 2021-05-12 2021-05-12 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110519870.XA CN113280812B (zh) 2021-05-12 2021-05-12 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置

Publications (2)

Publication Number Publication Date
CN113280812A true CN113280812A (zh) 2021-08-20
CN113280812B CN113280812B (zh) 2023-06-02

Family

ID=77278894

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110519870.XA Active CN113280812B (zh) 2021-05-12 2021-05-12 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置

Country Status (1)

Country Link
CN (1) CN113280812B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
US20170347239A1 (en) * 2016-05-26 2017-11-30 Apple Inc. Assisted gnss velocity estimation
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
US20190088098A1 (en) * 2014-09-26 2019-03-21 Invensense, Inc. Using pressure sensors in a security system
CN111964684A (zh) * 2020-08-21 2020-11-20 运城学院 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN111982105A (zh) * 2020-08-21 2020-11-24 运城学院 一种基于sins/lbl紧组合的水下导航定位方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190088098A1 (en) * 2014-09-26 2019-03-21 Invensense, Inc. Using pressure sensors in a security system
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
US20170347239A1 (en) * 2016-05-26 2017-11-30 Apple Inc. Assisted gnss velocity estimation
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN111964684A (zh) * 2020-08-21 2020-11-20 运城学院 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN111982105A (zh) * 2020-08-21 2020-11-24 运城学院 一种基于sins/lbl紧组合的水下导航定位方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张涛;石宏飞;徐晓苏;: "基于SINS/LBL紧组合的AUV水下导航定位技术", 中国惯性技术学报 *
张涛;石宏飞;陈立平;刘强;: "基于UKF的SINS/LBL水下AUV紧组合定位技术", 中国惯性技术学报 *

Also Published As

Publication number Publication date
CN113280812B (zh) 2023-06-02

Similar Documents

Publication Publication Date Title
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN109443379B (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN109782323B (zh) 一种深海水下自主航行器导航定位及校准方法
CN109724599B (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN104457754A (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN103744098A (zh) 基于sins/dvl/gps的auv组合导航系统
CN106679662A (zh) 一种基于tma技术的水下机器人单信标组合导航方法
CN110940340A (zh) 基于小型uuv平台的多传感器信息融合方法
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
CN114265047B (zh) 一种大潜深auv的定位基阵联合标校方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN110873813B (zh) 一种水流速度估算方法、组合导航方法及装置
CN116026324B (zh) 用于水-空跨介质航行器的跨域导航系统和方法
CN113280812B (zh) 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置
CN112484721A (zh) 一种水下移动平台导航方法以及水下移动平台导航装置

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20240117

Address after: No. 66, Gongchang Road, Xinhu Street, Guangming District, Shenzhen, Guangdong 518000

Patentee after: Shenzhen, Zhongshan University

Patentee after: SUN YAT-SEN University

Address before: 510275 No. 135 West Xingang Road, Guangzhou, Guangdong, Haizhuqu District

Patentee before: SUN YAT-SEN University