CN111721284B - 一种无源模式下的sins/usbl组合导航定位方法 - Google Patents
一种无源模式下的sins/usbl组合导航定位方法 Download PDFInfo
- Publication number
- CN111721284B CN111721284B CN202010144829.4A CN202010144829A CN111721284B CN 111721284 B CN111721284 B CN 111721284B CN 202010144829 A CN202010144829 A CN 202010144829A CN 111721284 B CN111721284 B CN 111721284B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- usbl
- sins
- propagation delay
- matrix
- 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 28
- 238000004364 calculation method Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 75
- 238000005259 measurement Methods 0.000 claims description 43
- 230000004927 fusion Effects 0.000 claims description 11
- 238000009434 installation Methods 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- XEBWQGVWTUSTLN-UHFFFAOYSA-M phenylmercury acetate Chemical compound CC(=O)O[Hg]C1=CC=CC=C1 XEBWQGVWTUSTLN-UHFFFAOYSA-M 0.000 claims description 3
- 230000026676 system process Effects 0.000 claims description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 3
- 230000004907 flux Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 2
- 239000000654 additive Substances 0.000 description 1
- 230000000996 additive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000011541 reaction mixture Substances 0.000 description 1
- 230000001360 synchronised effect 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
- G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
一种无源模式下的SINS/USBL组合导航定位方法,涉及组合导航及水声定位技术领域,针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题,本发明引入深度计信息,设计USBL无源工作模式下的时钟误差的计算方法,补偿USBL单通道的时延误差,以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
Description
技术领域
本发明涉及组合导航及水声定位技术领域,具体为一种无源模式下的SINS/USBL组合导航定位方法。
背景技术
USBL水声定位***的无源工作模式是指声源时钟和测量***时钟不同步,使USBL声学接收基阵的各个通道测量的声信号传播时延均含有相同的时钟误差。在利用两两通道的时延差时,时钟误差的影响可以相互抵消,而单个通道的时延精度仍受时钟误差的影响,降低了USBL的测距和定位精度。传统的SINS/USBL紧组合导航都是建立在USBL有源工作模式下,以传播时延和时延差(斜距和斜距差)作为数据融合的基础,而对无源模式下的SINS/USBL紧组合导航研究较少。
发明内容
本发明的目的是:针对现有技术中无源模式下SINS/USBL紧组合导航定位精度低的问题,提出一种无源模式下的SINS/USBL组合导航定位方法。
本发明为了解决上述技术问题采取的技术方案是:
一种无源模式下的SINS/USBL组合导航定位方法,包括以下步骤:
步骤一:将USBL声学基阵倒置安装在水下航行器上,并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上,水面布设一个非同步信标,并设置GPS获取信标在导航坐标系下的位置信息;
步骤二:建立声学基阵坐标系,并确定基元在基阵坐标系下的位置;
步骤三:利用USBL测量声信号在信标和基元之间的传播时延及时延差,获得USBL传播时延量测值及USBL传播时延差量测值,并结合有效声速,解算航行器在基阵坐标系下相对于信标的方向角余弦,所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值;
步骤四:获取航行器的深度信息,并结合方向角余弦,判断航行器与信标的相对位置关系是否满足基阵开角条件,若满足,则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值,当出现连续两个采样时刻都满足基阵开角条件时,计算得到连续两个采样时刻的斜距预测值和传播时延预测值,并执行步骤五,若不满足基阵开角条件,则判断是否存在邻近时刻的时钟误差估计值,若存在,则沿用其估计值后执行步骤六,若不存在,则沿用步骤三中得到的USBL传播时延量测值,并执行步骤七,所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角;
步骤五:建立时钟误差模型,利用连续两个采样时刻的传播时延预测值与对应的USBL 测量的各基元传播时延均值之差,估计误差模型参数,计算时钟误差估计值;
步骤六:利用时钟误差估计值,补偿USBL测量的传播时延,得到新的USBL传播时延测量值;
步骤七:利用SINS计算水下航行器的姿态和位置,并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下;
步骤八:根据SINS转换的基元在导航坐标系中的位置信息,计算声信号在各基元与信标间的传播时延和传播时延差,获得SINS传播时延量测值和传播时延差量测值;
步骤九:根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值,建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器;
步骤十:利用信息融合扩展卡尔曼滤波器计算SINS误差,修正导航输出,重置扩展卡尔曼滤波器状态,重新执行步骤三至步骤十。
进一步的,所述步骤二中建立声学基阵坐标系的具体步骤为:
首先以基阵中心为原点,然后以沿基阵平面指向水下航行器的艏向方向为y轴,z轴垂直于基阵平面向上,x轴与y轴、z轴构成右手坐标系;
所述步骤八中导航坐标系为东北天地理坐标系,所述东北天地理坐标系为以水下航行器质心为原点,x轴指向地理东向,y轴指向地理北向,z轴垂直于xoy平面指向天向,构成右手坐标系。
进一步的,所述信息融合扩展卡尔滤波器以SINS的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量,以USBL和SINS的传播时延之差、传播时延差之差作为观测量,在最小均方误差准则下,建立描述***的状态方程和观测方程。
进一步的,所述基元在基阵坐标系下的位置为:
其中,i为基元序号,r为基元1,3或基元2,4的间距。
进一步的,所述步骤三中方向角余弦为:
进一步的,所述步骤六中利用相邻时刻的传播时延预测值与对应的USBL水声定位***测量的声信号传播时延均值之差,估计误差模型参数,计算时钟误差估计值,补偿USBL声学基阵的传播时延误差的具体步骤为:
其中,τi是基元i的时延测量真值,δτ是时钟误差真值,是补偿的时钟误差,ni是基元i的测量噪声,τ31是基元1,3的时延差真值,n31是基元1,3的时延差测量噪声,τ42是基元2,4的时延差真值,n42是基元4,2的时延差测量噪声。
进一步的,所述步骤七的具体步骤为:
首先结合基阵坐标系与载体坐标系的安装偏差校准结果,得到基元在导航坐标系下的位置,其中航行器姿态包括航向角A、俯仰角K和横滚角ψ,声学基阵与航行器对应的载体坐标系的角度安装偏差分别为α、β和γ,位置安装偏差为ΔX=[Δx Δy Δz]T,载体坐标系到导航坐标系的转换矩阵和声学基阵坐标系到载体坐标系的转换矩阵分别为:
则第i个基元在导航坐标系下的位置表示为
传播时延差为:
进一步的,所述步骤九的具体步骤为:
SINS位置为纬度L、经度λ和高度h的形式,将其转换为地球直角坐标,则
上式的微分形式为:
记
地球直角坐标系到SINS计算导航坐标系的转换矩阵为:
扩展卡尔曼滤波状态变量由SINS姿态误差φ=[φx φy φz]T,速度误差δν=[δvxδvy δvz]T,位置误差δp=[δL δλ δh]T,陀螺漂移误差ε=[εx εy εz]T,加速度计偏置误差组成,即
状态方程为
Xk+1=Fk+1/kXk+wk+1
其中,Fk+1/k为状态转移矩阵,由SINS的误差方程获得,wk+1为扩展卡尔滤波过程噪声矢量;扩展卡尔曼滤波的观测量为
观测方程为
Zk+1=Hk+1Xk+1+vk+1
其中,vk+1为高斯白噪声序列,
记
观测矩阵Hk+1为
进一步的,所述步骤十的具体步骤为:
步骤十二:根据Pk+1/k=Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k,其中Pk为k时刻的状态估计误差协方差矩阵,Qk为k时刻的***过程噪声协方差矩阵;
步骤十三:根据Kk+1=PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1,其中Rk+1和Hk+1分别是k+1时刻的***观测噪声协方差矩阵和观测矩阵;
步骤十四:根据Pk+1=(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1;
本发明的有益效果是:
本发明引入深度计信息,设计USBL无源工作模式下的时钟误差的计算方法,补偿USBL单通道的时延误差,以提高组合导航定位的精度。本发明能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
附图说明
图1为无源模式下USBL时钟误差估计原理图;
图2为航行器与信标在基阵坐标系下的相对位置关系图;
图3为基阵坐标系、载体坐标系和导航坐标系的示意简图。
具体实施方式
具体实施方式一:参照图具体说明本实施方式,本实施方式所述的一种无源模式下的 SINS/USBL组合导航定位方法,包括以下步骤:
步骤一:将USBL声学基阵倒置安装在水下航行器上,并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上,水面布设一个非同步信标,并设置GPS获取信标在导航坐标系下的位置信息;
步骤二:建立声学基阵坐标系,并确定基元在基阵坐标系下的位置;
步骤三:利用USBL测量声信号在信标和基元之间的传播时延及时延差,获得USBL传播时延量测值及USBL传播时延差量测值,并结合有效声速,解算航行器在基阵坐标系下相对于信标的方向角余弦,所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值;
步骤四:获取航行器的深度信息,并结合方向角余弦,判断航行器与信标的相对位置关系是否满足基阵开角条件,若满足,则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值,当出现连续两个采样时刻都满足基阵开角条件时,计算得到连续两个采样时刻的斜距预测值和传播时延预测值,并执行步骤五,若不满足基阵开角条件,则判断是否存在邻近时刻的时钟误差估计值,若存在,则沿用其估计值后执行步骤六,若不存在,则沿用步骤三中得到的USBL传播时延量测值,并执行步骤七。所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角。
步骤五:建立时钟误差模型,利用连续两个采样时刻的传播时延预测值与对应的USBL 测量的各基元传播时延均值之差,估计误差模型参数,计算时钟误差估计值;
步骤六:利用时钟误差估计值,补偿USBL测量的传播时延,得到新的USBL传播时延测量值;
步骤七:利用SINS计算水下航行器的姿态和位置,并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下;
步骤八:根据SINS转换的基元在导航坐标系中的位置信息,计算声信号在各基元与信标间的传播时延和传播时延差,获得SINS传播时延量测值和传播时延差量测值;
步骤九:根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值,建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器;
步骤十:利用信息融合扩展卡尔曼滤波器计算SINS误差,修正导航输出,重置扩展卡尔曼滤波器状态,重新执行步骤三至步骤十。
针对上述步骤需要说明的是:
本发明是连续导航定位的需要,直至导航结束,不然就一直重复。
所述声学基阵是由多个声信号接收换能器(基元)均匀分布的一个圆柱形设备。
所述基阵坐标系是指以基阵中心为原点,沿基阵平面指向水下航行器的艏向方向为y 轴,z轴垂直于基阵平面向上,x轴与y轴、z轴构成右手坐标系。
所述基阵开角是指在基阵坐标系原点与信标的连线与z轴正向的夹角。
所述导航坐标系是指“东北天(ENU)”地理坐标系。
所述“东北天(ENU)”地理坐标系是指以载体质心为原点,x轴指向地理东向,y轴指向地理北向,z轴垂直于xoy平面指向天向,构成右手坐标系。
所述有效声速为信标和接收基阵的几何距离与声信号在两点间的传播时间的比值。
所述信息融合扩展卡尔滤波器是指以捷联惯性导航的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量,以超短基线和捷联惯性导航的传播时延之差、传播时延差之差作为观测量,在最小均方误差准则下,建立描述***的状态方程和观测方程,对状态变量进行估计。
所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理。
所述重置滤波状态变量是指,在修正捷联惯性导航输出后,理论上认为此时捷联惯导输出的导航信息不存在误差,因此滤波状态变量为零。
本发明中所述的USBL声学基阵为多个声信号接收换能器均匀设置,声信号接收换能器一般大于或等于3个。
本发明首次研究无源模式SINS/USBL紧组合导航中USBL时钟误差的估计与补偿方法,以提高USBL的时延测量精度和SINS/USBL紧组合导航定位精度。
本发明时钟误差的估计精度虽受USBL基阵开角的影响,但能在有限的基阵开角范围内提高SINS/USBL紧组合导航定位精度。
针对每个步骤的具体说明如下:
步骤一:将超短基线声学基阵和捷联惯性导航的陀螺组件、加速度计组件以及深度计安装在水下航行器上,其中信标可以在水下,USBL基阵可以不倒置。
步骤二:结合附图2,图2中基元1-4在基阵坐标系下的坐标为:
式(1)中,i为基元序号,r为基元1,3或基元2,4的间距。图中a为基阵坐标系,b 为载体坐标系,n为导航坐标系。
步骤三:超短基线定位***测量的声信号在各基元和信标间的传播时延为传播时延差为和下标表示对应基元的传播时延之差。将表示为对应真值附加时钟误差和高斯白噪声干扰的形式,将和表示为附加高斯白噪声干扰的形式:
τi是基元i的时延测量真值,δτ是时钟误差真值,ni是基元i的测量噪声,τ31是基元1,3的时延差真值,n31是基元1,3的时延差测量噪声,τ42是基元2,4的时延差真值,n42是基元4,2的时延差测量噪声。
附图2的基阵坐标系中,航行器相对于信标的方向角余弦为
式(3)中,c为有效声速,d为对应基元间距。
步骤四:t1时刻深度计辅助下的预测斜距和预测传播时延分别为:
步骤五:重复步骤三,t2时刻深度计辅助下的预测斜距和预测传播时延分别为:
步骤六:USBL无源工作模式下的时钟误差δτ与USBL测量的声信号传播时延τ满足如下关系:
δτ=aτ+b (8)
步骤七:捷联惯性导航解算水下航行器在导航坐标系下的姿态、速度和位置。根据姿态信息,得到SINS对导航坐标系的复现,称为计算导航坐标系。结合基阵坐标系与载体坐标系的安装偏差校准结果,得到基元在计算导航坐标系下的位置。
建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴xb的正方向沿水下航行器的横轴指向右,坐标轴yb的正方向沿水下航行器的纵轴指向前,坐标轴zb的正方向沿水下航行器的立轴指向上,载体坐标系的定义满足右手定则;
其中航行器姿态包括航向角A、俯仰角K和横滚角ψ,声学基阵坐标系与水下航行器载体坐标系对应的三个轴的角度安装偏差分别为α、β和γ,坐标系原点在载体坐标系下的位置安装偏差为ΔX=[Δx Δy Δz]T。载体坐标系到计算导航坐标系的转换矩阵和声学基阵坐标系到载体坐标系的转换矩阵分别为:
则第i个基元在计算导航坐标系下的位置表示为
其中
步骤八声信号在第i个基元与信标之间的传播时延为:
||·||是矩阵二范数的表示,即
其中,为信标在真实导航坐标系的xn轴方向位置,为信标在真实导航坐标系的yn轴方向位置,为信标在真实导航坐标系的zn轴方向位置,为第i个基元在计算导航坐标系的xn轴方向位置,为第i个基元在计算导航坐标系的yn轴方向位置,为第 i个基元在计算导航坐标系的zn轴方向位置;
声信号的传播时延差为:
步骤九:建立基于传播时延、传播时延差融合的扩展卡尔曼滤波器状态方程和观测方程。
首先SINS位置为纬度L、经度λ和高度h的形式,将其转换为地球直角坐标,地球直角坐标系以地心为原点,xe轴指向本初子午线与赤道交点,ye轴指向90°经线与赤道交点,ze轴与xe轴、ye轴构成右手坐标系,则
上式的微分形式为:
记
地球直角坐标系到SINS计算导航坐标系的转换矩阵为:
扩展卡尔曼滤波状态变量由SINS姿态误差φ=[φx φy φz]T,速度误差δν=[δvxδvy δvz]T,位置误差δp=[δL δλ δh]T,陀螺漂移误差ε=[εx εy εz]T,加速度计偏置误差组成,即
状态方程为
Xk+1=Fk+1/kXk+wk+1 (24)
其中Fk+1/k为状态转移矩阵,由SINS的误差方程获得。wk+1为扩展卡尔滤波过程噪声矢量;
扩展卡尔曼滤波的观测量为
观测方程为
Zk+1=Hk+1Xk+1+vk+1 (26)
其中vk+1为高斯白噪声序列。
结合式(16),式(17)的全微分表示为:
观测矩阵Hk+1为
步骤十:计算捷联惯性导航的导航误差,滤波解算过程如下。
根据Pk+1/k=Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k,其中Pk为k时刻的状态估计误差协方差矩阵,Qk为k时刻的***过程噪声协方差矩阵。
根据Kk+1=PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1,其中Rk+1和Hk+1分别是k+1时刻的***观测噪声协方差矩阵和观测矩阵。
根据Pk+1=(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1。
需要注意的是,具体实施方式仅仅是对本发明技术方案的解释和说明,不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的,仍应落入本发明的保护范围内。
Claims (10)
1.一种无源模式下的SINS/USBL组合导航定位方法,其特征在于包括以下步骤:
步骤一:将USBL声学基阵倒置安装在水下航行器上,并将SINS的陀螺组件、加计组件以及深度计固联在水下航行器上,水面布设一个非同步信标,并设置GPS获取信标在导航坐标系下的位置信息;
步骤二:建立声学基阵坐标系,并确定基元在基阵坐标系下的位置;
步骤三:利用USBL测量声信号在信标和基元之间的传播时延及时延差,获得USBL传播时延量测值及USBL传播时延差量测值,并结合有效声速,解算航行器在基阵坐标系下相对于信标的方向角余弦,所述有效声速为信标和接收基阵的几何距离与声信号在两点间传播时间的比值;
步骤四:获取航行器的深度信息,并结合方向角余弦,判断航行器与信标的相对位置关系是否满足基阵开角条件,若满足,则计算得到信标与航行器的斜距预测值和声信号在信标与基阵中心之间的传播时延预测值,当出现连续两个采样时刻都满足基阵开角条件时,计算得到连续两个采样时刻的斜距预测值和传播时延预测值,并执行步骤五,若不满足基阵开角条件,则判断是否存在邻近时刻的时钟误差估计值,若存在,则沿用其估计值后执行步骤六,若不存在,则沿用步骤三中得到的USBL传播时延量测值,并执行步骤七,所述基阵开角为基阵坐标系原点与信标的连线与z轴正向的夹角;
步骤五:建立时钟误差模型,利用连续两个采样时刻的传播时延预测值与对应的USBL测量的各基元传播时延均值之差,估计误差模型参数,计算时钟误差估计值;
步骤六:利用时钟误差估计值,补偿USBL测量的传播时延,得到新的USBL传播时延测量值;
步骤七:利用SINS计算水下航行器的姿态和位置,并根据得到的姿态和位置将基元在基阵坐标系下的相对位置转换到SINS使用的导航坐标系下;
步骤八:根据SINS转换的基元在导航坐标系中的位置信息,计算声信号在各基元与信标间的传播时延和传播时延差,获得SINS传播时延量测值和传播时延差量测值;
步骤九:根据USBL传播时延测量值并结合步骤八中SINS传播时延量测值和传播时延差量测值,建立基于USBL和SINS传播时延量测值之差、传播时延差量测值之差的信息融合扩展卡尔曼滤波器;
步骤十:利用信息融合扩展卡尔曼滤波器计算SINS误差,修正导航输出,重置扩展卡尔曼滤波器状态,重新执行步骤三至步骤十。
2.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法,其特征在于所述步骤二中建立声学基阵坐标系的具体步骤为:
首先以基阵中心为原点,然后以沿基阵平面指向水下航行器的艏向方向为y轴,z轴垂直于基阵平面向上,x轴与y轴、z轴构成右手坐标系;
所述步骤八中导航坐标系为东北天地理坐标系,所述东北天地理坐标系为以水下航行器质心为原点,x轴指向地理东向,y轴指向地理北向,z轴垂直于xoy平面指向天向,构成右手坐标系。
3.根据权利要求1所述的一种无源模式下的SINS/USBL组合导航定位方法,其特征在于所述信息融合扩展卡尔滤波器以SINS的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量,以USBL和SINS的传播时延之差、传播时延差之差作为观测量,在最小均方误差准则下,建立描述***的状态方程和观测方程。
6.根据权利要求5所述的一种无源模式下的SINS/USBL组合导航定位方法,其特征在于所述步骤六中利用相邻时刻的传播时延预测值与对应的USBL水声定位***测量的声信号传播时延均值之差,估计误差模型参数,计算时钟误差估计值,补偿USBL声学基阵的传播时延误差的具体步骤为:
9.根据权利要求8所述的一种无源模式下的SINS/USBL组合导航定位方法,其特征在于所述步骤九的具体步骤为:
SINS位置为纬度L、经度λ和高度h的形式,将其转换为地球直角坐标,则
上式的微分形式为:
记
地球直角坐标系到SINS计算导航坐标系的转换矩阵为:
扩展卡尔曼滤波状态变量由SINS姿态误差φ=[φx φy φz]T,速度误差δν=[δvx δvyδvz]T,位置误差δp=[δL δλ δh]T,陀螺漂移误差ε=[εx εy εz]T,加速度计偏置误差组成,即
状态方程为
Xk+1=Fk+1/kXk+wk+1
其中,Fk+1/k为状态转移矩阵,由SINS的误差方程获得,wk+1为扩展卡尔滤波过程噪声矢量;扩展卡尔曼滤波的观测量为
观测方程为
Zk+1=Hk+1Xk+1+vk+1
其中,vk+1为高斯白噪声序列,
记
观测矩阵Hk+1为
10.根据权利要求9所述的一种无源模式下的SINS/USBL组合导航定位方法,其特征在于所述步骤十的具体步骤为:
步骤十二:根据Pk+1/k=Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k,其中Pk为k时刻的状态估计误差协方差矩阵,Qk为k时刻的***过程噪声协方差矩阵;
步骤十三:根据Kk+1=PkHT k+1(Hk+1PkHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1,其中Rk+1和Hk+1分别是k+1时刻的***观测噪声协方差矩阵和观测矩阵;
步骤十四:根据Pk+1=(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010144829.4A CN111721284B (zh) | 2020-03-04 | 2020-03-04 | 一种无源模式下的sins/usbl组合导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010144829.4A CN111721284B (zh) | 2020-03-04 | 2020-03-04 | 一种无源模式下的sins/usbl组合导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111721284A CN111721284A (zh) | 2020-09-29 |
CN111721284B true CN111721284B (zh) | 2021-08-31 |
Family
ID=72564072
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010144829.4A Active CN111721284B (zh) | 2020-03-04 | 2020-03-04 | 一种无源模式下的sins/usbl组合导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111721284B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113155134B (zh) * | 2021-04-27 | 2022-05-03 | 哈尔滨工程大学 | 一种基于惯性信息辅助的水声信道跟踪与预测方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106546956A (zh) * | 2016-10-25 | 2017-03-29 | 中国海洋石油总公司 | 一种超短基线接收基阵基元位置的精确标定方法 |
CN109029460A (zh) * | 2018-08-03 | 2018-12-18 | 国家深海基地管理中心 | 深海运载器对水面监控平台测距的导航方法、***及装置 |
CN109738902A (zh) * | 2019-03-06 | 2019-05-10 | 哈尔滨工程大学 | 一种基于同步信标模式的水下高速目标高精度自主声学导航方法 |
CN110082706A (zh) * | 2019-04-23 | 2019-08-02 | 哈尔滨工程大学 | 一种基于时延差和相位差且适用于时钟异步的水下单信标导航方法 |
CN110132281A (zh) * | 2019-05-21 | 2019-08-16 | 哈尔滨工程大学 | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 |
-
2020
- 2020-03-04 CN CN202010144829.4A patent/CN111721284B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106546956A (zh) * | 2016-10-25 | 2017-03-29 | 中国海洋石油总公司 | 一种超短基线接收基阵基元位置的精确标定方法 |
CN109029460A (zh) * | 2018-08-03 | 2018-12-18 | 国家深海基地管理中心 | 深海运载器对水面监控平台测距的导航方法、***及装置 |
CN109738902A (zh) * | 2019-03-06 | 2019-05-10 | 哈尔滨工程大学 | 一种基于同步信标模式的水下高速目标高精度自主声学导航方法 |
CN110082706A (zh) * | 2019-04-23 | 2019-08-02 | 哈尔滨工程大学 | 一种基于时延差和相位差且适用于时钟异步的水下单信标导航方法 |
CN110132281A (zh) * | 2019-05-21 | 2019-08-16 | 哈尔滨工程大学 | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 |
Non-Patent Citations (1)
Title |
---|
詹飞等.众核处理架构在水下航行器相位编码脉冲回波检测中的应用.《声学学报》.2018,第43卷(第4期),445-452. * |
Also Published As
Publication number | Publication date |
---|---|
CN111721284A (zh) | 2020-09-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN111595348B (zh) | 一种自主水下航行器组合导航***的主从式协同定位方法 | |
CN110146076B (zh) | 一种无逆矩阵自适应滤波的sins/dvl组合定位方法 | |
CN105823480A (zh) | 基于单信标的水下移动目标定位算法 | |
CN106679662B (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
CN108444476B (zh) | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 | |
CN111829512B (zh) | 一种基于多传感器数据融合的auv导航定位方法及*** | |
CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及*** | |
CN110389318B (zh) | 一种基于立体六元阵的水下移动平台定位***及方法 | |
CN105547289A (zh) | 一种水下航行器组合导航***及导航信息融合方法 | |
CN111380519B (zh) | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 | |
CN104501809B (zh) | 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN113155134B (zh) | 一种基于惯性信息辅助的水声信道跟踪与预测方法 | |
CN111721284B (zh) | 一种无源模式下的sins/usbl组合导航定位方法 | |
CN110207698B (zh) | 一种极区格网惯导/超短基线紧组合导航方法 | |
CN112083425B (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
CN111735455A (zh) | 基于改进的高斯距离迭代算法对接回收组合导航方法 | |
Liu et al. | SINS/DVL integrated system with current and misalignment estimation for midwater navigation | |
CN108759864A (zh) | 基于单应答器的水下捷联惯导***初始对准方法 | |
CN111380520B (zh) | 一种引入径向速度的sins/usbl松组合导航定位方法 |
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 |