CN113280812B - 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 - Google Patents
一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 Download PDFInfo
- Publication number
- CN113280812B CN113280812B CN202110519870.XA CN202110519870A CN113280812B CN 113280812 B CN113280812 B CN 113280812B CN 202110519870 A CN202110519870 A CN 202110519870A CN 113280812 B CN113280812 B CN 113280812B
- Authority
- CN
- China
- Prior art keywords
- sins
- lbl
- difference
- elevation
- value
- 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 49
- 238000005259 measurement Methods 0.000 claims abstract description 55
- 238000001914 filtration Methods 0.000 claims abstract description 32
- 238000004364 calculation method Methods 0.000 claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims description 85
- 230000003044 adaptive effect Effects 0.000 claims description 18
- 230000005284 excitation Effects 0.000 claims description 10
- 239000013598 vector Substances 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 229940060587 alpha e Drugs 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 239000013535 sea water Substances 0.000 description 1
- 230000009466 transformation Effects 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
-
- 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与水声定 位技术的组合导航具有重要意义。
目前的组合导航系统一般都将惯导设备作为主要参考导航系统。 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),即:
将上式相对AUV真实位置进行Taylor线性化得:
ρSINSi=Ri-R0+eixδx+eiyδy+eizδz
其中,Ri表示声源与第i个应答器的斜距,i=(0,1,2,3);R0表示声源与 参考应答器的斜距,有:
进一步的,获取通过LBL声源到所述第i个应答器和LBL声源到 参考应答器的斜距差,记为基于LBL斜距差,具体为:
LBL声源到所述第i个应答器的斜距:
其中,i=(1,2,3);
LBL声源到参考应答器的斜距:
进一步的,将所述基于SINS斜距差与所述基于LBL斜距差做差运 算,获取所述基于SINS与所述基于LBL的斜距差之差,记为外部观测 值,具体为:
将所述外部观测值δρi记为外部观测值。
进一步的,通过高程约束自适应Kalman滤波方程可得到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];
进一步的,在使用本方法的同时还需要建立SINS/LBL紧组合导航 系统状态方程和量测方程,具体为:
SINS/LBL紧组合状态方程可描述为:
其中,XSINS、XLBL分别为SINS、LBL的状态变量,FSINS、FLBL分别为SINS、 LBL的状态矩阵,WSINS、WLBL分别为SINS、LBL的状态噪声。
SINS选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺 漂移作为状态量:
由SINS误差方程可确定FSINS,即:
LBL系统选择斜距差误差作为状态量,即:
XLBL=[δR1 δR2 δR3]T
其中,δR为斜距差误差,可用一阶Markov过程表示。
LBL的量测矩阵为:
SINS/LBL紧组合的量测方程可描述为:
其中,ρ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过程表示,即斜距差误差模型为:
S2、建立SINS/LBL紧组合导航系统状态方程和量测方程。
(1)状态方程的建立
SINS/LBL紧组合状态方程可描述为:
其中,XSINS、XLBL分别为SINS、LBL的状态变量,FSINS、FLBL分别为 SINS、LBL的状态矩阵,WSINS、WLBL分别为SINS、LBL的状态噪声。
SINS选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺 漂移作为状态量:
其中,15项状态量分别对应捷联惯导的东向速度误差、北向速度误差、 天向速度误差、东向失准角、北向失准角、天向失准角、纬度误差、 经度误差、高度误差、加速度计的三个轴向偏差、陀螺仪的三个轴向 漂移。
由SINS误差方程可确定FSINS,即:
其中,矩阵F9×9中的元素为:
RN=Re(1-2e+3e sin2L)
RE=Re(1+e sin2L)
LBL系统选择斜距差误差作为状态量,即:
XLBL=[δR1 δR2 δR3]T
其中,δR为斜距差误差,可用一阶Markov过程表示。
LBL的量测矩阵为:
(2)量测方程的建立
SINS/LBL紧组合的量测方程可描述为:
其中,ρ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为单位向量,
矩阵中的各元素表达式如下:
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),即:
将上式相对AUV真实位置进行Taylor线性化得:
ρSINSi=Ri-R0+eixδx+eiyδy+eizδz
其中,Ri表示声源与第i个应答器的斜距,i=(0,1,2,3);R0表示声源与 参考应答器的斜距,有:
步骤102:获取通过LBL声源到所述第i个应答器与LBL声源到参 考应答器的斜距差,记为基于LBL斜距差。
本实施例中,LBL声源到所述第i个应答器的斜距:
其中,i=(1,2,3);
LBL声源到参考应答器的斜距:
步骤103:将所述基于SINS斜距差与所述基于LBL斜距差做差运 算,获取所述基于SINS与所述基于LBL的斜距差之差,记为外部观测 值。
本实施例中,
将所述外部观测值δρ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滤波。其中,第二预设条件为系统噪声方 差阵为非负定阵,量测噪声方差阵为正定阵。
一步状态预测方程:
一步估计误差协方差阵方程:
增益矩阵:
协方差矩阵估计:
状态估计及高程约束过程:
其中:
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];
步骤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),即:
将上式相对AUV真实位置进行Taylor线性化得:
ρSINSi=Ri-R0+eixδx+eiyδy+eizδz
其中,Ri表示声源与第i个应答器的斜距,i=(0,1,2,3);R0表示声源与 参考应答器的斜距,有:
第二斜距差模块202用于获取通过LBL声源到所述第i个应答器 与LBL声源到参考应答器的斜距差,记为基于LBL斜距差。
本实施例中,LBL声源到所述第i个应答器的斜距:
其中,i=(1,2,3);
LBL声源到参考应答器的斜距:
斜距差计算模块203用于将所述基于SINS斜距差与所述基于LBL 斜距差做差运算,获取所述基于SINS与所述基于LBL的斜距差之差, 记为外部观测值。
将所述外部观测值δρ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滤波。其中,第二预设条件为系统噪声方 差阵为非负定阵,量测噪声方差阵为正定阵。
一步状态预测方程:
一步估计误差协方差阵方程:
增益矩阵:
协方差矩阵估计:
状态估计及高程约束过程:
其中:
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];
修正模块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滤波。
7.如权利要求1所述的一种基于SINS/LBL紧组合的高程约束水下自适应导航方法,其特征在于,在使用本方法前还需要建立SINS/LBL紧组合导航系统状态方程和量测方程,具体为:
SINS/LBL紧组合状态方程描述为:
SINS选择速度误差、姿态误差、位置误差、加速度计零偏、陀螺漂移作为状态量:
LBL系统选择斜距差误差作为状态量,即:
LBL的量测矩阵为:
SINS/LBL紧组合的量测方程描述为:
8.一种基于SINS/LBL紧组合的高程约束水下自适应导航装置,其特征在于,包括:第一斜距差模块、第二斜距差模块、斜距差计算模块、数据处理模块和修正模块,
所述第一斜距差模块用于使用SINS获取AUV位置信息,根据所述AUV的位置信息和海底各应答器的位置信息获取基于SINS斜距差;其中,所述海底各应答器包括i个应答器和一个参考应答器,i的取值范围为1到3的正整数;
所述第二斜距差模块用于获取通过LBL声源到所述第i个应答器与LBL声源到参考应答器的斜距差,记为基于LBL斜距差;
所述斜距差计算模块用于将所述基于SINS斜距差与所述基于LBL斜距差做差运算,获取所述基于SINS与所述基于LBL的斜距差之差,记为外部观测值;
所述数据处理模块用于将所述外部观测值作为高程约束自适应的滤波计算的输入值,根据k时刻的量测值,通过高程约束自适应Kalman滤波方程得到k时刻的状态估计值;k时刻SINS解算的AUV位置为,其中,高程数据为/>;k时刻压力传感器输出的高程数据为/>,取二者差值,即:/>;
所述修正模块用于用所述高程误差估计值对固有的SINS输出值的误差进行修正处理,获取修正误差后的SINS的导航信息。
9.一种终端设备,包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序,所述处理器执行所述计算机程序时实现如权利要求1至7任意一项所述的基于SINS/LBL紧组合的高程约束水下自适应导航方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质包括存储的计算机程序,其中,在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至7中任意一项所述的基于SINS/LBL紧组合的高程约束水下自适应导航方法。
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 CN113280812A (zh) | 2021-08-20 |
CN113280812B true 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 (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104457754A (zh) * | 2014-12-19 | 2015-03-25 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
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紧组合的水下导航定位方法及系统 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10593176B2 (en) * | 2014-09-26 | 2020-03-17 | Invensense, Inc. | Using pressure sensors in a security system |
US10171946B2 (en) * | 2016-05-26 | 2019-01-01 | Apple Inc. | Assisted GNSS velocity estimation |
-
2021
- 2021-05-12 CN CN202110519870.XA patent/CN113280812B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104457754A (zh) * | 2014-12-19 | 2015-03-25 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
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)
Title |
---|
基于SINS/LBL紧组合的AUV水下导航定位技术;张涛;石宏飞;徐晓苏;;中国惯性技术学报(第04期);500-504 * |
基于UKF的SINS/LBL水下AUV紧组合定位技术;张涛;石宏飞;陈立平;刘强;;中国惯性技术学报(第05期);638-642 * |
Also Published As
Publication number | Publication date |
---|---|
CN113280812A (zh) | 2021-08-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN109724599B (zh) | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 | |
CN109324330B (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
CN109782323B (zh) | 一种深海水下自主航行器导航定位及校准方法 | |
CN109737956B (zh) | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN111596333B (zh) | 一种水下定位导航方法及系统 | |
CN104457754A (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
CN110940340A (zh) | 基于小型uuv平台的多传感器信息融合方法 | |
CN111504324B (zh) | 一种噪声自适应滤波的水下组合导航方法 | |
CN106679662A (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及系统 | |
CN108871379B (zh) | 一种dvl测速误差在线标定方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN115979253A (zh) | 一种基于抗差滤波的水下机器人多传感器紧组合导航方法 | |
CN112015086B (zh) | 一种欠驱动水面船有限时间路径跟踪输出反馈控制方法 | |
CN115388884A (zh) | 一种智能体位姿估计器联合初始化方法 | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
CN114265047B (zh) | 一种大潜深auv的定位基阵联合标校方法 | |
Lee et al. | An integrated navigation system for autonomous underwater vehicles with two range sonars, inertial sensors and Doppler velocity log | |
CN104061930A (zh) | 基于捷联惯性制导和多普勒计程仪的导航方法 | |
CN114136310B (zh) | 一种惯性导航系统误差自主抑制系统及方法 | |
CN112083425B (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
CN113280812B (zh) | 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 |
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 |