CN111380519B - 一种超短基线/捷联惯性导航松组合的导航误差校正方法 - Google Patents

一种超短基线/捷联惯性导航松组合的导航误差校正方法 Download PDF

Info

Publication number
CN111380519B
CN111380519B CN202010144251.2A CN202010144251A CN111380519B CN 111380519 B CN111380519 B CN 111380519B CN 202010144251 A CN202010144251 A CN 202010144251A CN 111380519 B CN111380519 B CN 111380519B
Authority
CN
China
Prior art keywords
coordinate system
navigation
underwater vehicle
strapdown inertial
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
Application number
CN202010144251.2A
Other languages
English (en)
Other versions
CN111380519A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010144251.2A priority Critical patent/CN111380519B/zh
Publication of CN111380519A publication Critical patent/CN111380519A/zh
Application granted granted Critical
Publication of CN111380519B publication Critical patent/CN111380519B/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/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)
  • Navigation (AREA)

Abstract

一种超短基线/捷联惯性导航松组合的导航误差校正方法，它属于组合导航及水声定位技术领域。本发明解决了在基阵开角较大时，水下航行器的捷联惯性导航需要运动至超短基阵有效工作范围内进行误差校正，导致水下航行器的工作效率降低的问题。当超短基线定位***有实数解时，可以得到声学基阵与信标的相对深度和斜距的比值，即基阵开角的余弦值，以该值控制组合导航的滤波增益，能够减小基阵开角比较大时的低质量观测信息在状态更新中所占的比重。通过在外部对滤波器的增益进行控制，能有效抑制捷联惯性导航误差，并且保证了在基阵开角较大情况下的水下航行器的工作效率。本发明可以应用于组合导航及水声定位领域。

Description

一种超短基线/捷联惯性导航松组合的导航误差校正方法
技术领域
本发明属于组合导航及水声定位技术领域，具体涉及一种超短基线/捷联惯性导航松组合的滤波增益控制方法。
背景技术
随着基阵开角的增大，超短基线的定位误差变大，组合导航精度受到影响。换言之，传统的超短基线和捷联惯性导航的组合只能在以某一基阵开角为半锥角的圆锥内部及表面进行，水下航行器在其它位置区域时，只能依靠捷联惯性导航，而纯惯性导航存在着误差随时间积分累积的缺点，使得水下航行器需要每隔一段时间运动至超短基线的有效工作范围内，以此来校正惯性导航的累积误差，这样就大大降低了水下航行器的工作效率。
发明内容
本发明的目的是为解决在基阵开角较大时，水下航行器的捷联惯性导航需要运动至超短基阵有效工作范围内进行误差校正，导致水下航行器的工作效率降低的问题，而提出了一种超短基线/捷联惯性导航松组合的导航误差校正方法。
本发明为解决上述技术问题采取的技术方案是：一种超短基线/捷联惯性导航松组合的导航误差校正方法，该方法包括以下步骤：
步骤一、将超短基线声学基阵倒置安装在水下航行器上，捷联惯性导航的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标；信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、通过测量声信号在超短基线声学基阵与信标之间的单程传播时延及有效声速，在基阵坐标系下，计算信标的位置以及超短基线声学基阵与信标的相对深度，再计算相对深度与声信号在水下航行器和信标之间传播距离的比值，计算出的比值即为基阵开角的余弦值；
所述基阵坐标系是指以超短基线声学基阵中心为原点Oa，沿超短基线声学基阵平面指向水下航行器的艏向方向为Ya轴，Za轴垂直于基阵平面向上，Xa轴与Ya轴、Za轴构成右手坐标系；
步骤三、根据捷联惯性导航提供的水下航行器姿态信息以及基阵坐标系下和导航坐标系下信标的位置信息，获得在捷联惯性导航计算导航坐标系下，超短基线对水下航行器的定位结果
Figure BDA0002400161440000021
步骤四、根据超短基线对水下航行器的定位结果
Figure BDA0002400161440000022
和捷联惯性导航计算的水下航行器位置
Figure BDA0002400161440000023
建立扩展卡尔曼滤波器；
步骤五、根据步骤二计算出的基阵开角余弦值调整扩展卡尔曼滤波器的滤波增益，实现状态更新；
步骤六、根据步骤五更新后的状态值，修正捷联惯性导航的输出，并重置扩展卡尔曼滤波器的状态。
本发明的有益效果是：本发明提出了一种超短基线/捷联惯性导航松组合的导航误差校正方法，当超短基线定位***定位有效时，可以得到声学基阵与信标的相对深度和斜距的比值，即基阵开角的余弦值，以该值控制组合导航的滤波增益，能够减小基阵开角比较大时的低质量观测信息在状态更新中所占的比重。
通过在外部对滤波器的增益进行控制，能有效抑制捷联惯性导航误差，并且保证了在基阵开角较大情况下的水下航行器的工作效率。
附图说明
图1是超短基线定位有效时超短基线/捷联惯性导航松组合的导航误差校正方法的原理图。
具体实施方式
具体实施方式一：如图1所示，本实施方式所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，该方法包括以下步骤：
步骤一、将超短基线声学基阵倒置安装在水下航行器上，捷联惯性导航的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标；信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”(“ENU”)地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、通过测量声信号在超短基线声学基阵与信标之间的单程传播时延及有效声速，在基阵坐标系下，计算信标的位置以及超短基线声学基阵与信标的相对深度，再计算相对深度与声信号在水下航行器和信标之间传播距离的比值，计算出的比值即为基阵开角的余弦值；
所述基阵坐标系是指以超短基线声学基阵中心为原点Oa，沿超短基线声学基阵平面指向水下航行器的艏向方向为Ya轴，Za轴垂直于基阵平面向上，Xa轴与Ya轴、Za轴构成右手坐标系；
步骤三、根据捷联惯性导航提供的水下航行器姿态信息以及基阵坐标系下和导航坐标系下信标的位置信息，获得在捷联惯性导航计算导航坐标系下，超短基线对水下航行器的定位结果
Figure BDA0002400161440000031
所述捷联惯性导航计算导航坐标系是指：捷联惯性导航***通过解算水下航行器的姿态信息实现对“东北天”地理坐标系的复现；
步骤四、根据超短基线对水下航行器的定位结果
Figure BDA0002400161440000032
和捷联惯性导航计算的水下航行器位置，建立基于位置信息融合的扩展卡尔曼滤波器；
步骤五、根据步骤二计算出的基阵开角余弦值调整扩展卡尔曼滤波器的滤波增益，实现状态更新；
步骤六、根据步骤五更新后的状态值，修正捷联惯性导航的输出，并重置扩展卡尔曼滤波器的状态。
重置扩展卡尔曼滤波器的状态后，再重复步骤二至步骤五的过程，利用更新后的状态值修正捷联惯性导航的输出，不断的对捷联惯性导航的输出进行修正。
在任意基阵开角下，本发明都可以保证组合导航精度和航行器工作效率。
具体实施方式二：本实施方式与具体实施方式一不同的是：所述有效声速为声源和超短基线声学基阵的几何距离与声音在两点间传播时间的比值。
具体实施方式三：本实施方式与具体实施方式一不同的是：所述基阵开角是指在基阵坐标系下，超短基线声学基阵同信标的连线与Za轴正向的夹角。
具体实施方式四：本实施方式与具体实施方式一不同的是：所述步骤二中，在基阵坐标系下，信标的位置为[Xa′ Ya′ Za′]T，基阵开角的余弦值为cosθZ；
Figure BDA0002400161440000033
其中：R是声信号在水下航行器和信标之间的传播距离，Xa′为信标在Xa轴方向的位置，Ya′为信标在Ya轴方向的位置，Za′为信标在Za轴方向的位置；cosθX是信标在Xa轴方向的位置与R的比值，cosθY是信标在Ya轴方向的位置与R的比值；
cos2θX+cos2θY+cos2θZ＝1
其中：
Figure BDA0002400161440000041
c是水中声速，τ是超短基线声学基阵各个基元测量的单程传播时延均值，单程传播距离为水下航行器相对于信标的距离，τX和τY分别是两两基元的传播时延差沿基阵坐标系Xa轴和Ya轴的分量，dX和dY是对应的两基元在Xa轴和Ya轴方向上的位置差。
具体实施方式五：本实施方式与具体实施方式四不同的是：所述步骤三的具体过程为：
捷联惯性导航提供的水下航行器姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
基阵坐标系相对于载体坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与载体坐标系的三个坐标轴的角度偏差)，基阵坐标系原点相对于载体坐标系原点的位置偏差
Figure BDA0002400161440000047
为：
Figure BDA0002400161440000042
(ΔXb、ΔYb和ΔZb均为载体坐标系下分量)，ΔXb、ΔYb和ΔZb为
Figure BDA0002400161440000043
中的分量，上角标T代表转置；
载体坐标系到捷联惯性导航计算导航坐标系的转换矩阵
Figure BDA0002400161440000044
和基阵坐标系到载体坐标系的转换矩阵
Figure BDA0002400161440000045
分别为：
Figure BDA0002400161440000046
Figure BDA0002400161440000051
根据超短基线声学定位***解算的基阵坐标系下信标的位置
Figure BDA0002400161440000052
和GPS提供的信标的位置
Figure BDA0002400161440000053
得到
Figure BDA0002400161440000054
其中：
Figure BDA0002400161440000055
为捷联惯性导航计算导航坐标系下超短基线对水下航行器的定位结果；
捷联惯性导航计算的导航坐标系n'与真实导航坐标系n(建立的导航坐标系)的三个坐标轴存在角度误差φ：φ＝[φE φN φU]T，称φ为捷联惯性导航失准角误差，φE、φN和φU均为φ中的分量，捷联惯性导航计算的导航坐标系n'与真实导航坐标系n之间的转换矩阵
Figure BDA0002400161440000056
可近似为
Figure BDA0002400161440000057
其中：φ×为中间变量，I为单位矩阵；
Figure BDA0002400161440000058
真实导航坐标系下水下航行器的位置真值与捷联惯性导航计算导航坐标系n'下水下航行器的位置的差
Figure BDA0002400161440000059
为：
Figure BDA00024001614400000510
其中：
Figure BDA00024001614400000511
是载体坐标系与计算的导航坐标系n'之间的转换矩阵，n为超短基线声学定位***中的高斯白噪声序列。
本发明中，未特别限定的导航坐标系，均是指建立的导航坐标系。
α、β和γ分别是基阵坐标系的三个坐标轴相对于载体坐标系的三个坐标轴的角度偏差；
航向角为载体(水下航行器)艏向与地理北向的夹角，定义为北偏西为正，角度范围(-180°,180°]；横滚角γ为载体立轴zb与横轴xb轴所在铅垂面的夹角，载体向右倾斜时角度为正，角度范围(-180°,180°]；俯仰角θ为载体纵轴yb与其水平投影线之间的夹角，当载体抬头时为正，角度范围(-90°,90°]。
捷联惯性导航解算水下航行器在导航坐标系下的姿态、速度和位置。结合声学基阵的安装偏差校准结果和姿态信息，将超短基线解算的信标在声学基阵坐标系下的相对位置进行转换，得到水下航行器在计算导航坐标系下位置。
具体实施方式六：本实施方式与具体实施方式五不同的是：所述步骤四的具体过程为：
将捷联惯性导航输出的水下航行器位置表示为纬度L、经度λ和高度h的形式，再将纬度L、经度λ和高度h转换为地球直角坐标，其中，地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
Figure BDA0002400161440000061
式中，xe′、ye′和ze′分别为捷联惯性导航输出的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，RN为地球卯酉圈主曲率半径，
Figure BDA0002400161440000062
Re为地球半径，e为地球偏心率，
Figure BDA0002400161440000063
a和b分别是椭圆长轴和短轴半径；
上式的微分转换关系为：
Figure BDA0002400161440000064
地球直角坐标系到捷联惯性导航计算导航坐标系的转换矩阵为：
Figure BDA0002400161440000065
扩展卡尔曼滤波器的状态变量由捷联惯性导航失准角误差φ＝[φE φN φU]T，捷联惯性导航速度误差δν＝[δvE δvN δvU]T，捷联惯性导航位置误差δp＝[δL δλ δh]T，捷联惯性导航陀螺漂移误差ε＝[εx εy εz]T，捷联惯性导航加速度计偏置误差
Figure BDA0002400161440000078
组成；
Figure BDA0002400161440000079
其中：X为扩展卡尔曼滤波器的状态变量；δvE、δvN和δvU均为δν中的分量，δL、δλ和δh均为δp中的分量，εx、εy和εz均为ε中的分量，
Figure BDA00024001614400000710
和
Figure BDA00024001614400000711
均为
Figure BDA00024001614400000712
中的分量；
扩展卡尔曼滤波器的状态方程为：
Xk+1＝Fk+1/kXk+wk+1
其中：Xk为k时刻扩展卡尔曼滤波器的状态变量，Xk+1为k+1时刻扩展卡尔曼滤波器的状态变量，Fk+1/k为状态转移矩阵，由捷联惯性导航***的误差方程获得，wk+1为扩展卡尔曼滤波***过程噪声序列，通常为高斯白噪声形式；
捷联惯性导航输出的水下航行器位置[L λ h]T经坐标转换得到水下航行器在地球直角坐标系下的位置X′e(SINS)，则水下航行器位置X′e(SINS)在捷联惯性导航计算导航坐标系下的位置
Figure BDA0002400161440000071
为：
Figure BDA0002400161440000072
则扩展卡尔曼滤波的观测量Z为：
Figure BDA0002400161440000073
其中：
Figure BDA0002400161440000074
是
Figure BDA0002400161440000075
对应的位置误差，
Figure BDA0002400161440000076
与δp＝[δL δλ δh]T的关系为：
Figure BDA0002400161440000077
扩展卡尔曼滤波的观测方程Zk+1为：
Zk+1＝Hk+1Xk+1+vk+1
其中：vk+1为扩展卡尔曼滤波***观测噪声序列，通常为高斯白噪声形式，Hk+1为观测矩阵。
Figure BDA0002400161440000081
组合导航一般有反馈校正的闭环方式和输出修正的开环方式，前者工程实现复杂，当滤波器有故障时会直接影响捷联惯性导航的输出，而输出修正不涉及独立的导航***内部，容错能力较强，因此本发明采用输出修正方式。
具体实施方式七：本实施方式与具体实施方式六不同的是：所述步骤五的具体过程为：
在超短基线声学基阵定位无效时，没有量测信息进入扩展卡尔曼滤波器，此时，扩展卡尔曼滤波器只进行时间更新：
根据
Figure BDA0002400161440000082
计算k+1时刻的状态预测值
Figure BDA0002400161440000083
其中
Figure BDA0002400161440000084
为k时刻的状态估计值；
根据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；
根据
Figure BDA0002400161440000085
计算k+1时刻的状态估计值
Figure BDA0002400161440000086
所述基于位置信息的扩展卡尔滤波器是指以捷联惯性导航的姿态误差、速度误差、位置误差和陀螺漂移、加速度计偏置为状态变量，以超短基线和捷联惯性导航输出的水下航行器位置之差作为观测量对状态变量进行估计。
所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理。
所述卡尔滤波器是指在最小均方误差准则下的线性贝叶斯估计；卡尔滤波采用状态方程描述相邻时刻状态变量的关系，采用观测方程描述状态变量和观测量的关系。
所述调整滤波增益是指利用基阵开角的余弦值控制理论计算的滤波器增益，适当地减小在大基阵开角情况下的滤波增益，降低低质量的超短基线声学定位结果对状态变量估计精度的影响程度。
所述修正捷联惯性导航输出是指在捷联惯导原导航输出上减去状态变量的估计值，得到新的惯性导航输出。
所述重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。
|cosθz|的取值范围为[0 1]，在大开角的情况下，|cosθz|能够适当调整滤波增益，减小低质量的观测信息对滤波状态更新的影响，提高组合导航精度。
本发明中，扩展卡尔曼滤波器的状态方程是由捷联惯性导航***的误差方程构成，但不仅限于此，如在状态方程中加入超短基线的定位误差、测距误差和安装偏差等。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (7)

1.一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，该方法包括以下步骤：
步骤一、将超短基线声学基阵倒置安装在水下航行器上，捷联惯性导航的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标；信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、通过测量声信号在超短基线声学基阵与信标之间的单程传播时延及有效声速，在基阵坐标系下，计算信标的位置以及超短基线声学基阵与信标的相对深度，再计算相对深度与声信号在水下航行器和信标之间传播距离的比值，计算出的比值即为基阵开角的余弦值；
所述基阵坐标系是指以超短基线声学基阵中心为原点Oa，沿超短基线声学基阵平面指向水下航行器的艏向方向为Ya轴，Za轴垂直于基阵平面向上，Xa轴与Ya轴、Za轴构成右手坐标系；
步骤三、根据捷联惯性导航提供的水下航行器姿态信息以及基阵坐标系下和导航坐标系下信标的位置信息，获得在捷联惯性导航计算导航坐标系下，超短基线对水下航行器的定位结果
Figure FDA0003139446290000011
步骤四、根据超短基线对水下航行器的定位结果
Figure FDA0003139446290000012
和捷联惯性导航计算的水下航行器位置，建立扩展卡尔曼滤波器；
步骤五、根据步骤二计算出的基阵开角余弦值调整扩展卡尔曼滤波器的滤波增益，实现状态更新；
步骤六、根据步骤五更新后的状态值，修正捷联惯性导航的输出，并重置扩展卡尔曼滤波器的状态。
2.根据权利要求1所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，所述有效声速为声源和超短基线声学基阵的几何距离与声信号在两点间传播时间的比值。
3.根据权利要求1所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，所述基阵开角是指在基阵坐标系下，超短基线声学基阵同信标的连线与Za轴正向的夹角。
4.根据权利要求1所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，所述步骤二中，在基阵坐标系下，信标的位置为[X′a Y′a Z′a]T，基阵开角的余弦值为cosθZ；
Figure FDA0003139446290000021
其中：R是声信号在水下航行器和信标之间的传播距离，X′a为信标在Xa轴方向的位置，Y′a为信标在Ya轴方向的位置，Z′a为信标在Za轴方向的位置；cosθX是信标在Xa轴方向的位置与R的比值，cosθY是信标在Ya轴方向的位置与R的比值；
cos2θX+cos2θY+cos2θZ＝1
其中：
Figure FDA0003139446290000022
c是水中声速，τX和τY分别是两两基元的传播时延差沿基阵坐标系Xa轴和Ya轴的分量，dX和dY是对应的两基元在Xa轴和Ya轴方向上的位置差。
5.根据权利要求4所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，所述步骤三的具体过程为：
捷联惯性导航提供的水下航行器姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
基阵坐标系相对于载体坐标系的角度安装偏差分别为α、β和γ，基阵坐标系原点相对于载体坐标系原点的位置偏差
Figure FDA0003139446290000023
为：
Figure FDA0003139446290000024
ΔXb、ΔYb和ΔZb为
Figure FDA0003139446290000025
中的分量，上角标T代表转置；
载体坐标系到捷联惯性导航计算导航坐标系的转换矩阵
Figure FDA0003139446290000026
和基阵坐标系到载体坐标系的转换矩阵
Figure FDA0003139446290000031
分别为：
Figure FDA0003139446290000032
Figure FDA0003139446290000033
根据基阵坐标系下信标的位置
Figure FDA0003139446290000034
和GPS提供的信标的位置
Figure FDA0003139446290000035
得到
Figure FDA0003139446290000036
其中：
Figure FDA0003139446290000037
为捷联惯性导航计算导航坐标系下超短基线对水下航行器的定位结果；
捷联惯性导航计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在角度误差φ：φ＝[φE φN φU]T，称φ为捷联惯性导航失准角误差，φE、φN和φU均为φ中的分量，捷联惯性导航计算的导航坐标系n'与真实导航坐标系n之间的转换矩阵
Figure FDA0003139446290000038
为
Figure FDA0003139446290000039
其中：φ×为中间变量，I为单位矩阵；
Figure FDA00031394462900000310
真实导航坐标系下水下航行器的位置与捷联惯性导航计算导航坐标系n'下水下航行器的位置的差
Figure FDA00031394462900000311
为：
Figure FDA00031394462900000312
其中：
Figure FDA00031394462900000313
是载体坐标系与计算的导航坐标系n'之间的转换矩阵，n为超短基线声学定位***中的高斯白噪声序列。
6.根据权利要求5所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，所述步骤四的具体过程为：
将捷联惯性导航输出的水下航行器位置表示为纬度L、经度λ和高度h的形式，再将纬度L、经度λ和高度h转换为地球直角坐标，其中，地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
Figure FDA0003139446290000041
式中，x′e、y′e和z′e分别为捷联惯性导航输出的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，RN为地球卯酉圈主曲率半径，
Figure FDA0003139446290000042
Re为地球半径，e为地球偏心率，
Figure FDA0003139446290000043
a和b分别是椭圆长轴和短轴半径；
上式的微分转换关系为：
Figure FDA0003139446290000044
地球直角坐标系到捷联惯性导航计算导航坐标系的转换矩阵为：
Figure FDA0003139446290000045
扩展卡尔曼滤波器的状态变量由捷联惯性导航失准角误差φ＝[φE φN φU]T，捷联惯性导航速度误差δν＝[δvE δvN δvU]T，捷联惯性导航位置误差δp＝[δL δλ δh]T，捷联惯性导航陀螺漂移误差ε＝[εx εy εz]T，捷联惯性导航加速度计偏置误差
Figure FDA0003139446290000046
组成；
Figure FDA0003139446290000047
其中：X为扩展卡尔曼滤波器的状态变量；δvE、δvN和δvU均为δν中的分量，δL、δλ和δh均为δp中的分量，εx、εy和εz均为ε中的分量，
Figure FDA0003139446290000048
和
Figure FDA0003139446290000049
均为
Figure FDA00031394462900000410
中的分量；
扩展卡尔曼滤波器的状态方程为：
Xk+1＝Fk+1/kXk+wk+1
其中：Xk为k时刻扩展卡尔曼滤波器的状态变量，Xk+1为k+1时刻扩展卡尔曼滤波器的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波***过程噪声序列；
捷联惯性导航输出的水下航行器位置[L λ h]T经坐标转换得到水下航行器在地球直角坐标系下的位置X′e(SINS)，则水下航行器位置X′e(SINS)在捷联惯性导航计算导航坐标系下的位置
Figure FDA0003139446290000051
为：
Figure FDA0003139446290000052
则扩展卡尔曼滤波的观测量Z为：
Figure FDA0003139446290000053
其中：
Figure FDA0003139446290000054
是
Figure FDA0003139446290000055
对应的位置误差，
Figure FDA0003139446290000056
与δp＝[δL δλ δh]T的关系为：
Figure FDA0003139446290000057
扩展卡尔曼滤波的观测方程Zk+1为：
Zk+1＝Hk+1Xk+1+vk+1
其中：vk+1为扩展卡尔曼滤波***观测噪声序列，Hk+1为观测矩阵。
7.根据权利要求6所述的一种超短基线/捷联惯性导航松组合的导航误差校正方法，其特征在于，所述步骤五的具体过程为：
在超短基线声学基阵定位无效时，没有量测信息进入扩展卡尔曼滤波器，扩展卡尔曼滤波器只进行时间更新：
根据
Figure FDA0003139446290000058
计算k+1时刻的状态预测值
Figure FDA0003139446290000059
其中
Figure FDA00031394462900000510
为k时刻的状态估计值；
根据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；
根据
Figure FDA0003139446290000061
计算k+1时刻的状态估计值
Figure FDA0003139446290000062
CN202010144251.2A 2020-03-04 2020-03-04 一种超短基线/捷联惯性导航松组合的导航误差校正方法 Active CN111380519B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010144251.2A CN111380519B (zh) 2020-03-04 2020-03-04 一种超短基线/捷联惯性导航松组合的导航误差校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010144251.2A CN111380519B (zh) 2020-03-04 2020-03-04 一种超短基线/捷联惯性导航松组合的导航误差校正方法

Publications (2)

Publication Number Publication Date
CN111380519A CN111380519A (zh) 2020-07-07
CN111380519B true CN111380519B (zh) 2021-08-31

Family

ID=71222696

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010144251.2A Active CN111380519B (zh) 2020-03-04 2020-03-04 一种超短基线/捷联惯性导航松组合的导航误差校正方法

Country Status (1)

Country Link
CN (1) CN111380519B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112836889B (zh) * 2021-02-19 2024-07-19 鹏城实验室 路径优化方法、水下航行器以及计算机可读存储介质
CN113176539B (zh) * 2021-04-25 2022-09-09 哈尔滨工程大学 一种水声信号噪声多级抑制与稳健定位***及定位方法
CN116068540B (zh) * 2023-02-17 2023-09-12 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8130142B2 (en) * 2009-09-21 2012-03-06 Appareo Systems, Llc GNSS ultra-short baseline heading determination system and method
CN105509765B (zh) * 2014-09-23 2018-07-20 北京自动化控制设备研究所 一种惯性/dvl/usbl安装误差标定方法
CN105486313A (zh) * 2016-02-03 2016-04-13 东南大学 一种基于usbl辅助低成本sins***的定位方法
CN109737956B (zh) * 2018-12-17 2022-06-21 东南大学 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法

Also Published As

Publication number Publication date
CN111380519A (zh) 2020-07-07

Similar Documents

Publication Publication Date Title
CN111380519B (zh) 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航***初始对准方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
AU2020103939A4 (en) Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN109211269B (zh) 一种双轴旋转惯导***姿态角误差标定方法
CN107806874B (zh) 一种视觉辅助的捷联惯导极区初始对准方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN101788296A (zh) 一种sins/cns深组合导航***及其实现方法
CN109443385B (zh) 一种动中通天线的惯导安装误差自动标定方法
CN115200574A (zh) 一种地球椭球模型下的极区横向组合导航方法
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
CN110207698B (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN111025229B (zh) 一种水下机器人纯方位目标估计方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准***
CN104634348B (zh) 组合导航中的姿态角计算方法
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN112833878A (zh) 近地面多源天文自主导航方法
CN111380520B (zh) 一种引入径向速度的sins/usbl松组合导航定位方法
CN114061574B (zh) 一种基于位置不变约束及零速校正的采煤机定姿定向方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN113252029B (zh) 一种基于光学陀螺量测信息的天文导航姿态传递方法
CN109471103A (zh) 一种弹载双基sar数据融合定位误差修正方法
CN111380517B (zh) 一种基于usbl软件接收机的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