CN111380517B - 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 - Google Patents
一种基于usbl软件接收机的sins/usbl深组合导航定位方法 Download PDFInfo
- Publication number
- CN111380517B CN111380517B CN202010144235.3A CN202010144235A CN111380517B CN 111380517 B CN111380517 B CN 111380517B CN 202010144235 A CN202010144235 A CN 202010144235A CN 111380517 B CN111380517 B CN 111380517B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- sins
- usbl
- navigation
- axis
- 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 30
- 238000005259 measurement Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 51
- 230000008569 process Effects 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000001360 synchronised effect Effects 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
- 238000009434 installation Methods 0.000 claims description 2
- XEBWQGVWTUSTLN-UHFFFAOYSA-M phenylmercury acetate Chemical compound CC(=O)O[Hg]C1=CC=CC=C1 XEBWQGVWTUSTLN-UHFFFAOYSA-M 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 101150073669 NCAN gene Proteins 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 230000005764 inhibitory process Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 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)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,它属于组合导航及水声定位技术领域。本发明解决了当USBL测量信息中断或者发生错误时,传统组合方式的容错能力有限的问题。在不需要先验信息的情况下,本发明利用SINS的动态信息辅助无源工作模式下的USBL载波跟踪环路和码跟踪环路,获得准确径向速度信息和时延信息,提高了USBL在干扰信号或者是弱信号下的***容错能力,提高了组合***的性能。本发明可以应用于组合导航及水声定位领域。
Description
技术领域
本发明属于组合导航及水声定位技术领域,具体涉及一种基于USBL(超短基线定位***)软件接收机的SINS(捷联惯导***)/USBL深组合导航定位方法。
背景技术
传统的SINS/USBL组合导航建立在对SINS和USBL位置输出信息或时延、时延差(斜距、斜距差)测量信息进行数据融合的基础上,通过建立SINS误差方程和USBL定位误差方程或测距测向误差方程,实现对SINS导航误差的修正,在USBL***外部对定位结果或测距测向结果进行补偿,而忽略了利用SINS动态信息来深入辅助USBL***内部的信号跟踪环节。当USBL测量信息中断或者发生错误时,传统的组合方式都只能提供有限的容错能力,从而影响组合***的性能。
发明内容
本发明的目的是为解决当USBL测量信息中断或者发生错误时,传统组合方式的容错能力有限的问题,而提出了一种基于USBL软件接收机的SINS/USBL深组合导航定位方法。
本发明为解决上述技术问题采取的技术方案是:一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,该方法包括以下步骤:
步骤一、将USBL的声学基阵倒置安装在水下航行器上,SINS的陀螺组件和加速度计组件固联在水下航行器上,水面布放一个同步信标;信标在导航坐标系下的位置通过GPS获得;
所述导航坐标系是指“东北天”地理坐标系,“东北天”地理坐标系以水下航行器的质心为原点on,xn轴指向地理东向,yn轴指向地理北向,zn轴垂直于xnonyn平面指向天向,xn轴、yn轴和zn轴构成右手坐标系;
步骤二、建立声学基阵坐标系,并确定各基元在声学基阵坐标系下的位置;
所述声学基阵坐标系是指以USBL的声学基阵中心为原点oa,沿声学基阵平面指向水下航行器的艏向方向为ya轴,za轴垂直于声学基阵平面向上,xa轴与ya轴、za轴构成右手坐标系;
步骤三、根据水下航行器在SINS计算的导航坐标系下的姿态信息,将基元在声学基阵坐标系下的位置转换到SINS计算的导航坐标系下;
步骤四、根据基元在SINS计算导航坐标系下的位置,计算声信号在各基元与信标间的传播时延以及传播时延差的预测值;
步骤五、将基元在SINS计算导航坐标系下的速度沿径向分解,得到径向速度的预测值,即得到各个基元相对于信标在径向方向的运动速度大小;
步骤六、将步骤四计算出的传播时延、传播时延差的预测值以及步骤五计算出的径向速度的预测值输入至USBL软件接收机的信号跟踪环路,辅助USBL软件接收机对载波频率和码相位的跟踪,得到声信号在各基元与信标间的传播时延、传播时延差的测量值以及径向速度的测量值;
步骤七、建立以传播时延的预测值与测量值之差、传播时延差的测量值与预测值之差以及径向速度的预测值与测量值之差为观测量,以SINS误差以及SINS对USBL的辅助误差为状态变量的扩展卡尔曼滤波***;
步骤八、根据扩展卡尔曼滤波***输出的状态估计值修正SINS输出,并重置扩展卡尔曼滤波***输出的状态;
重置扩展卡尔曼滤波器的状态后,再重复步骤三至步骤八的过程,不断的对SINS的输出进行修正。
本发明的有益效果是:本发明提出了一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,在不需要先验信息的情况下,本发明利用SINS的动态信息辅助无源工作模式下的USBL载波跟踪环路和码跟踪环路,获得准确径向速度信息和时延信息,提高了USBL在干扰信号或者是弱信号下的***容错能力,提高了组合***的性能。
附图说明
图1为SINS/USBL深组合导航***结构图;
图2为SINS辅助USBL载波跟踪环路原理图;
图3为SINS辅助USBL码跟踪环路原理图;
图4为声学基阵坐标系、载体坐标系和导航坐标系的转换原理图。
具体实施方式
具体实施方式一:如图1和图4所示,本实施方式所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,该方法包括以下步骤:
步骤一、将USBL的声学基阵倒置安装在水下航行器上,SINS的陀螺组件和加速度计组件固联在水下航行器上,水面布放一个同步信标;信标在导航坐标系下的位置通过GPS获得;
所述导航坐标系是指“东北天”(“ENU”)地理坐标系,“东北天”地理坐标系以水下航行器的质心为原点on,xn轴指向地理东向,yn轴指向地理北向,zn轴垂直于xnonyn平面指向天向,xn轴、yn轴和zn轴构成右手坐标系;
步骤二、建立声学基阵坐标系,并确定各基元在声学基阵坐标系下的位置;
所述声学基阵坐标系是指以USBL的声学基阵中心为原点oa,沿声学基阵平面指向水下航行器的艏向方向为ya轴,za轴垂直于声学基阵平面向上,xa轴与ya轴、za轴构成右手坐标系;
步骤三、根据水下航行器在SINS计算的导航坐标系下的姿态信息,将基元在声学基阵坐标系下的位置转换到SINS计算的导航坐标系下;
步骤四、根据基元在SINS计算导航坐标系下的位置,计算声信号在各基元与信标间的传播时延以及传播时延差的预测值;
步骤五、将基元在SINS计算导航坐标系下的速度沿径向分解,得到径向速度的预测值,即得到各个基元相对于信标在径向方向的运动速度大小;
步骤六、将步骤四计算出的传播时延、传播时延差的预测值以及步骤五计算出的径向速度的预测值输入至USBL软件接收机的信号跟踪环路,辅助USBL软件接收机对载波频率和码相位的跟踪,得到声信号在各基元与信标间的传播时延、传播时延差的测量值以及径向速度的测量值;
步骤七、建立以传播时延的预测值与测量值之差、传播时延差的测量值与预测值之差以及径向速度的预测值与测量值之差为观测量,以SINS误差以及SINS对USBL的辅助误差为状态变量的扩展卡尔曼滤波***;
步骤八、根据扩展卡尔曼滤波***输出的状态估计值修正SINS输出,并重置扩展卡尔曼滤波***输出的状态;
重置扩展卡尔曼滤波器的状态后,再重复步骤三至步骤八的过程,不断的对SINS的输出进行修正。
本发明中的导航坐标系包括SINS计算的导航坐标系和真实导航坐标系(即步骤一建立的导航坐标系),任何未经限定的导航坐标系均是指真实导航坐标系。SINS计算的导航坐标系是指:根据SINS计算的水下航行器姿态而复现的导航坐标系。
本实施方式中的声学基阵是由多个声信号接收换能器(基元)均匀分布的一个圆柱形设备。
在本发明的整个组合导航***中,只有水声信号预处理和惯性器件是硬件部分,其他部分都由软件实现,能够将软件接收机的灵活性与SINS辅助USBL的优势相结合,不仅便于***的实现和测试,也为***性能的仿真验证提供了便利。
本发明在USBL软件接收机的基础上,利用SINS提供的动态信息辅助载波跟踪环路,可以降低多普勒的动态范围,减小跟踪环路的带宽,达到抑制噪声和信号干扰的目的;SINS的动态辅助信息进入码跟踪环路能够提高对多路径效应的抑制作用和校正能力。在不需要先验信息的情况下,SINS的动态信息能够辅助无源工作模式下的USBL码跟踪环路,获得准确时延信息,提高了USBL在干扰信号或者是弱信号下的***容错能力。SINS为USBL接收机提供的动态辅助信息能够增强USBL在远距离作业、高动态、强干扰和信号中断等恶劣环境中的鲁棒性,同时提高SINS/USBL组合导航性能。
利用SINS的动态信息辅助USBL水声定位***对接收信号的跟踪,提高USBL定位***的时延测量和径向速度测量精度,扩大USBL的有效作用范围。
其中:i为基元序号,i=1,2,3,4,r为基元1与3的间距或基元2与4的间距(基元1与3的间距同基元2与4的间距相等);为各基元在声学基阵坐标系的xa轴方向上的位置,为各基元在声学基阵坐标系的ya轴方向上的位置,为各基元在声学基阵坐标系的za轴方向上的位置,上角标T代表转置。
具体实施方式三:本实施方式与具体实施方式二不同的是:所述步骤三的具体过程为:
水下航行器在SINS计算的导航坐标系下的姿态信息包括航向角A、俯仰角K和横滚角ψ;
建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴xb的正方向沿水下航行器的横轴指向右,坐标轴yb的正方向沿水下航行器的纵轴指向前,坐标轴zb的正方向沿水下航行器的立轴指向上,载体坐标系的定义满足右手定则;
声学基阵坐标系与载体坐标系三个坐标轴的角度安装偏差分别为α、β和γ,声学基阵坐标系原点相对于载体坐标系原点的位置偏差为:(ΔXb、ΔYb和ΔZb均为载体坐标系下分量),ΔXb、ΔYb和ΔZb为中的分量;
其中,I为单位矩阵;
记
具体实施方式四:本实施方式与具体实施方式三不同的是:所述步骤四的具体过程为:
其中,为信标在真实导航坐标系的xn轴方向位置,为信标在真实导航坐标系的yn轴方向位置,为信标在真实导航坐标系的zn轴方向位置,为第i个基元在真实导航坐标系的xn轴方向位置,为第i个基元在真实导航坐标系的yn轴方向位置,为第i个基元在真实导航坐标系的zn轴方向位置;
所述有效声速为信标和接收基阵的几何距离与声信号在两点间的传播时间的比值。
具体实施方式五:本实施方式与具体实施方式四不同的是:所述步骤五中,各个基元相对于信标在径向方向的运动速度大小为:
其中:为第i个基元相对于信标在径向方向的运动速度大小,为第i个基元在计算导航坐标系的xn′轴方向位置,为第i个基元在计算导航坐标系的yn′轴方向位置,为第i个基元在计算导航坐标系的zn′轴方向位置,为水下航行器在计算导航坐标系的xn′轴方向速度,为水下航行器在计算导航坐标系的yn′轴方向速度,为水下航行器在计算导航坐标系的zn′轴方向速度;
其中,记中间变量cosζx、cosζy和cosζz为:
具体实施方式六:结合图2说明本实施方式。本实施方式与具体实施方式五不同的是:所述步骤六的具体过程为:
SINS对应USBL软件接收机第i个基元接收通道的载波跟踪环路动态辅助信息为:
式中,faid为SINS辅助频率,c为有效声速,f0为信标广播信号中心频率;
其中,是接收信号相位与本地参考信号相位的相位误差,为相位误差的导数,为低通滤波器输出的频率误差δf的导数,p和q是与具体跟踪环路有关的参数;δfp为δf与SINS辅助频率faid误差δfaid之和,为δfp的导数,为δfaid的导数;在SINS动态辅助USBL接收机时只需考虑一个基元接收通道的载波环跟踪过程。
结合图3,SINS对应的USBL软件接收机第i个基元接收通道的码跟踪环路动态辅助信息为
具体实施方式七:本实施方式与具体实施方式六不同的是:所述步骤七的具体过程为:
失准角误差φ为:φ=[φx φy φz]T,SINS速度误差δν为:δν=[δvx δvy δvz]T(速度误差是由SINS解算的计算导航坐标系下的水下航行器运动速度与真实导航坐标系下的速度真值的差),SINS位置误差δp为:δp=[δL δλ δh]T(位置误差是SINS解算的水下航行器在地球坐标系下的位置与真实位置的差值),陀螺漂移误差ε为:ε=[εx εy εz]T,加速度计偏置误差为:其中,φx、φy和φz为φ中的分量;
则
其中,X为扩展卡尔曼滤波***的状态变量;
扩展卡尔曼滤波***的状态方程为
Xk+1=Fk+1/kXk+wk+1 (21)
其中,Xk为k时刻的状态变量,Xk+1为k+1时刻的状态变量,Fk+1/k为状态转移矩阵,由捷联惯性导航***的误差方程以及式(14)、式(15)、式(16)和式(19)获得,wk+1为扩展卡尔曼滤波***过程噪声序列,通常为高斯白噪声形式;
扩展卡尔曼滤波***的观测量Z为:
观测方程为
Zk+1=Hk+1Xk+1+vk+1 (23)
其中,Zk+1为k+1时刻的观测量,vk+1为扩展卡尔曼滤波***观测噪声序列,通常为高斯白噪声形式,Hk+1为k+1时刻的观测矩阵。
SINS解算的水下航行器位置表示为纬度L、经度λ和高度h的球坐标形式,再将纬度L、经度λ和高度h转换到地球直角坐标系下;
地球直角坐标系以地心为原点,xe轴指向本初子午线与赤道交点,ye轴指向90°经线与赤道交点,ze轴与xe轴、ye轴构成右手坐标系,则
式中,x′e、y′e和z′e分别为SINS解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置,RN为地球卯酉圈主曲率半径,Re为地球半径,e为地球偏心率,a和b分别是椭圆长轴和短轴半径;
上式的微分转换关系为:
地球直角坐标系到SINS计算导航坐标系的转换矩阵为:
记中间变量矩阵A为:
结合式(7)和式(8),得到式(9)的全微分形式:
记
式(12)的全微分为
Civv=[cosζx cosζy cosζz] (32)
观测矩阵Hk+1为
式(33)中,C1tt=1 0 0 0,C31tt=-1 0 1 0。
具体实施方式八:本实施方式与具体实施方式七不同的是:所述步骤八中,根据扩展卡尔曼滤波***输出的状态估计值修正SINS输出,并重置扩展卡尔曼滤波***输出的状态,其具体过程为:
根据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时刻的***观测噪声协方差矩阵和观测矩阵;上角标-1代表矩阵的逆;
根据Pk+1=(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1;
所述扩展卡尔滤波***是指在最小均方误差准则下,将非线性的状态方程和观测方程进行线性近似处理,对状态变量进行估计。
所述重置滤波状态变量是指,在修正捷联惯性导航输出后,理论上认为此时捷联惯导输出的导航信息不存在误差,因此滤波状态变量为零。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程,而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式的变化或变动,这里无法对所有的实施方式予以穷举,凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。
Claims (3)
1.一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,其特征在于,该方法包括以下步骤:
步骤一、将USBL的声学基阵倒置安装在水下航行器上,SINS的陀螺组件和加速度计组件固联在水下航行器上,水面布放一个同步信标;信标在导航坐标系下的位置通过GPS获得;
所述导航坐标系是指“东北天”地理坐标系,“东北天”地理坐标系以水下航行器的质心为原点on,xn轴指向地理东向,yn轴指向地理北向,zn轴垂直于xnonyn平面指向天向,xn轴、yn轴和zn轴构成右手坐标系;
步骤二、建立声学基阵坐标系,并确定各基元在声学基阵坐标系下的位置;
所述声学基阵坐标系是指以USBL的声学基阵中心为原点oa,沿声学基阵平面指向水下航行器的艏向方向为ya轴,za轴垂直于声学基阵平面向上,xa轴与ya轴、za轴构成右手坐标系;
其中:i为基元序号,i=1,2,3,4,将基元1与3的间距表示为r1,将基元2与4的间距表示为r2,令r1=r2=r;为各基元在声学基阵坐标系的xa轴方向上的位置,为各基元在声学基阵坐标系的ya轴方向上的位置,为各基元在声学基阵坐标系的za轴方向上的位置,上角标T代表转置;
步骤三、根据水下航行器在SINS计算的导航坐标系下的姿态信息,将基元在声学基阵坐标系下的位置转换到SINS计算的导航坐标系下;
所述步骤三的具体过程为:
水下航行器在SINS计算的导航坐标系下的姿态信息包括航向角A、俯仰角K和横滚角ψ;
建立载体坐标系obxbybzb,载体坐标系的坐标原点ob位于水下航行器的质心,坐标轴xb的正方向沿水下航行器的横轴指向右,坐标轴yb的正方向沿水下航行器的纵轴指向前,坐标轴zb的正方向沿水下航行器的立轴指向上,载体坐标系的定义满足右手定则;
其中,I为单位矩阵;
记
步骤四、根据基元在SINS计算导航坐标系下的位置,计算声信号在各基元与信标间的传播时延以及传播时延差的预测值;
所述步骤四的具体过程为:
其中,为信标在真实导航坐标系的xn轴方向位置,为信标在真实导航坐标系的yn轴方向位置,为信标在真实导航坐标系的zn轴方向位置,为第i个基元在真实导航坐标系的xn轴方向位置,为第i个基元在真实导航坐标系的yn轴方向位置,为第i个基元在真实导航坐标系的zn轴方向位置;
步骤五、将基元在SINS计算导航坐标系下的速度沿径向分解,得到径向速度的预测值,即得到各个基元相对于信标在径向方向的运动速度大小;
所述各个基元相对于信标在径向方向的运动速度大小为:
其中:为第i个基元相对于信标在径向方向的运动速度大小,为第i个基元在计算导航坐标系的xn′轴方向位置,为第i个基元在计算导航坐标系的yn′轴方向位置,为第i个基元在计算导航坐标系的zn′轴方向位置,为水下航行器在计算导航坐标系的xn′轴方向速度,为水下航行器在计算导航坐标系的yn′轴方向速度,为水下航行器在计算导航坐标系的zn′轴方向速度;
其中,记中间变量cosζx、cosζy和cosζz为:
步骤六、将步骤四计算出的传播时延、传播时延差的预测值以及步骤五计算出的径向速度的预测值输入至USBL软件接收机的信号跟踪环路,辅助USBL软件接收机对载波频率和码相位的跟踪,得到声信号在各基元与信标间的传播时延、传播时延差的测量值以及径向速度的测量值;
所述步骤六的具体过程为:
SINS对应USBL软件接收机第i个基元接收通道的载波跟踪环路动态辅助信息为:
式中,faid为SINS辅助频率,c为有效声速,f0为信标广播信号中心频率;
其中,是接收信号相位与本地参考信号相位的相位误差,为相位误差的导数,为低通滤波器输出的频率误差δf的导数,p和q是与具体跟踪环路有关的参数;δfp为δf与SINS辅助频率faid误差δfaid之和,为δfp的导数,为δfaid的导数;
SINS对应的USBL软件接收机第i个基元接收通道的码跟踪环路动态辅助信息为
步骤七、建立以传播时延的预测值与测量值之差、传播时延差的测量值与预测值之差以及径向速度的预测值与测量值之差为观测量,以SINS误差以及SINS对USBL的辅助误差为状态变量的扩展卡尔曼滤波***;
步骤八、根据扩展卡尔曼滤波***输出的状态估计值修正SINS输出,并重置扩展卡尔曼滤波***输出的状态;
重置扩展卡尔曼滤波器的状态后,再重复步骤三至步骤八的过程,不断的对SINS的输出进行修正。
2.根据权利要求1所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,其特征在于,所述步骤七的具体过程为:
失准角误差φ为:φ=[φx φy φz]T,SINS速度误差δν为:δν=[δvx δvy δvz]T,SINS位置误差δp为:δp=[δL δλ δh]T,陀螺漂移误差ε为:ε=[εx εy εz]T,加速度计偏置误差为:其中,φx、φy和φz为φ中的分量;
则
其中,X为扩展卡尔曼滤波***的状态变量;
扩展卡尔曼滤波***的状态方程为
Xk+1=Fk+1/kXk+wk+1 (21)
其中,Xk为k时刻的状态变量,Xk+1为k+1时刻的状态变量,Fk+1/k为状态转移矩阵,wk+1为扩展卡尔曼滤波***过程噪声序列;
扩展卡尔曼滤波***的观测量Z为:
观测方程为
Zk+1=Hk+1Xk+1+vk+1 (23)
其中,Zk+1为k+1时刻的观测量,vk+1为扩展卡尔曼滤波***观测噪声序列,Hk+1为k+1时刻的观测矩阵。
3.根据权利要求2所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法,其特征在于,所述步骤八中,根据扩展卡尔曼滤波***输出的状态估计值修正SINS输出,并重置扩展卡尔曼滤波***输出的状态,其具体过程为:
根据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 |
---|---|---|---|
CN202010144235.3A CN111380517B (zh) | 2020-03-04 | 2020-03-04 | 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010144235.3A CN111380517B (zh) | 2020-03-04 | 2020-03-04 | 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111380517A CN111380517A (zh) | 2020-07-07 |
CN111380517B true CN111380517B (zh) | 2021-10-29 |
Family
ID=71215248
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010144235.3A Active CN111380517B (zh) | 2020-03-04 | 2020-03-04 | 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111380517B (zh) |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1361409A (zh) * | 2000-12-23 | 2002-07-31 | 林清芳 | 增强型导航定位之方法及其*** |
CN101806884A (zh) * | 2010-04-23 | 2010-08-18 | 哈尔滨工程大学 | 基于超短基线的深海信标绝对位置精确定位方法 |
KR20140006347A (ko) * | 2012-07-04 | 2014-01-16 | 한국생산기술연구원 | 이동체의 위치 검출 시스템 및 방법 |
CN104316045A (zh) * | 2014-11-06 | 2015-01-28 | 东南大学 | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 |
CN104457754A (zh) * | 2014-12-19 | 2015-03-25 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
CN104931994A (zh) * | 2015-06-12 | 2015-09-23 | 南京理工大学 | 一种基于软件接收机的分布式深组合导航方法及*** |
CN106483498A (zh) * | 2015-09-01 | 2017-03-08 | 北京自动化控制设备研究所 | 一种sinsusbl紧耦合算法 |
CN106767793A (zh) * | 2017-01-19 | 2017-05-31 | 东南大学 | 一种基于sins/usbl紧组合的auv水下导航定位方法 |
CN109828296A (zh) * | 2019-03-08 | 2019-05-31 | 哈尔滨工程大学 | 一种ins/usbl非线性紧组合综合校正方法 |
CN110132281A (zh) * | 2019-05-21 | 2019-08-16 | 哈尔滨工程大学 | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 |
-
2020
- 2020-03-04 CN CN202010144235.3A patent/CN111380517B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1361409A (zh) * | 2000-12-23 | 2002-07-31 | 林清芳 | 增强型导航定位之方法及其*** |
CN101806884A (zh) * | 2010-04-23 | 2010-08-18 | 哈尔滨工程大学 | 基于超短基线的深海信标绝对位置精确定位方法 |
KR20140006347A (ko) * | 2012-07-04 | 2014-01-16 | 한국생산기술연구원 | 이동체의 위치 검출 시스템 및 방법 |
CN104316045A (zh) * | 2014-11-06 | 2015-01-28 | 东南大学 | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 |
CN104457754A (zh) * | 2014-12-19 | 2015-03-25 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
CN104931994A (zh) * | 2015-06-12 | 2015-09-23 | 南京理工大学 | 一种基于软件接收机的分布式深组合导航方法及*** |
CN106483498A (zh) * | 2015-09-01 | 2017-03-08 | 北京自动化控制设备研究所 | 一种sinsusbl紧耦合算法 |
CN106767793A (zh) * | 2017-01-19 | 2017-05-31 | 东南大学 | 一种基于sins/usbl紧组合的auv水下导航定位方法 |
CN109828296A (zh) * | 2019-03-08 | 2019-05-31 | 哈尔滨工程大学 | 一种ins/usbl非线性紧组合综合校正方法 |
CN110132281A (zh) * | 2019-05-21 | 2019-08-16 | 哈尔滨工程大学 | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 |
Non-Patent Citations (3)
Title |
---|
Technology of SINS/USBL highly tightly coupled integrated navigation based on phase difference measurements;Y.Y.Zhu;《2019 DGON Inertial Sensors and Systems》;20191230;第1-14页 * |
一种基于超短基线的高精度多目标水声定位技术研究;郑翠娥等;《海军工程大学学报》;20070415(第02期);第12-16页 * |
超短基线定位***安装误差校准技术研究;郑翠娥等;《计算机工程与应用》;20070311(第08期);第171-173页 * |
Also Published As
Publication number | Publication date |
---|---|
CN111380517A (zh) | 2020-07-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
CN105891863B (zh) | 一种基于高度约束的扩展卡尔曼滤波定位方法 | |
CN106767793A (zh) | 一种基于sins/usbl紧组合的auv水下导航定位方法 | |
CN109613583B (zh) | 基于单星与地面站测向及联合测时差的无源目标定位方法 | |
CN104316045A (zh) | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 | |
CN108444476B (zh) | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 | |
CN101788296A (zh) | 一种sins/cns深组合导航***及其实现方法 | |
CN115200574B (zh) | 一种地球椭球模型下的极区横向组合导航方法 | |
CN101949703A (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN111380519B (zh) | 一种超短基线/捷联惯性导航松组合的导航误差校正方法 | |
CN111580144A (zh) | 一种mins/gps超紧组合导航***设计方法 | |
CN105547289A (zh) | 一种水下航行器组合导航***及导航信息融合方法 | |
CN112540371A (zh) | 一种近底多波束坐标转换处理方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN110207698B (zh) | 一种极区格网惯导/超短基线紧组合导航方法 | |
CN113155134B (zh) | 一种基于惯性信息辅助的水声信道跟踪与预测方法 | |
CN112083425B (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
CN111380520B (zh) | 一种引入径向速度的sins/usbl松组合导航定位方法 | |
CN111380517B (zh) | 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 | |
CN113176539B (zh) | 一种水声信号噪声多级抑制与稳健定位***及定位方法 | |
CN116482735A (zh) | 一种受限空间内外高精度定位方法 | |
CN111721284B (zh) | 一种无源模式下的sins/usbl组合导航定位方法 | |
CN113820733B (zh) | 一种基于定向天线和多普勒信息的运动载体导航方法和装置 | |
RU2428659C2 (ru) | Способ спутниковой коррекции гироскопических навигационных систем морских объектов |
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 |