CN111380520A - 一种引入径向速度的sins/usbl松组合导航定位方法 - Google Patents

一种引入径向速度的sins/usbl松组合导航定位方法 Download PDF

Info

Publication number
CN111380520A
CN111380520A CN202010149299.2A CN202010149299A CN111380520A CN 111380520 A CN111380520 A CN 111380520A CN 202010149299 A CN202010149299 A CN 202010149299A CN 111380520 A CN111380520 A CN 111380520A
Authority
CN
China
Prior art keywords
coordinate system
sins
usbl
navigation
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.)
Granted
Application number
CN202010149299.2A
Other languages
English (en)
Other versions
CN111380520B (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 CN202010149299.2A priority Critical patent/CN111380520B/zh
Publication of CN111380520A publication Critical patent/CN111380520A/zh
Application granted granted Critical
Publication of CN111380520B publication Critical patent/CN111380520B/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)
  • Navigation (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

一种引入径向速度的SINS/USBL松组合导航定位方法，它属于组合导航及水声定位技术领域。本发明解决了传统SINS/USBL松组合方法忽略基元与信标之间径向速度的测量导致SINS速度误差的估计精度受限，以及在USBL定位无效时，依靠SINS单独完成导航任务带来的导航误差累积的问题。本发明将USBL的径向速度测量信息引入SINS/USBL松组合中，能够在USBL定位无效时，使SINS保持组合工作方式，减小SINS的误差积累；通过将径向速度作为滤波观测量，能够提高SINS/USBL松组合滤波系统对SINS速度误差的估计精度，从而改善整体导航性能。本发明可以应用于组合导航及水声定位领域。

Description

一种引入径向速度的SINS/USBL松组合导航定位方法
技术领域
本发明属于组合导航及水声定位技术领域，具体涉及一种引入径向速度的SINS/USBL松组合导航定位方法。
背景技术
传统的SINS(捷联惯导系统)/USBL(超短基线声学定位系统)松组合导航需要在USBL能够输出有效定位结果的条件下，以SINS和USBL计算的航行器位置之差作为组合导航滤波系统的观测量，该方法在USBL定位无效时，仅依靠SINS单独完成导航任务，容易使该段时间内的导航误差累积。径向速度与航行器的运动速度直接相关，描述了水下航行器与参考信标之间的距离变化率大小。USBL能够通过计算基元接收信号与发射信号间的多普勒频率，获得基元与信标之间的径向速度，而传统的SINS/USBL松组合导航忽略了USBL这一冗余测量，导致SINS/USBL松组合工作条件和SINS速度误差的估计精度受限。
发明内容
本发明的目的是为解决传统SINS/USBL松组合方法忽略基元与信标之间径向速度的测量导致SINS速度误差的估计精度受限，以及在USBL定位无效时，依靠SINS单独完成导航任务带来的导航误差累积的问题，而提出了一种引入径向速度的SINS/USBL松组合导航定位方法。
本发明为解决上述技术问题采取的技术方案是：一种引入径向速度的SINS/USBL松组合导航定位方法，该方法包括以下步骤：
步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；
步骤二、建立基阵坐标系，并确定信标在基阵坐标系下的位置
Figure BDA0002400157930000011
和各基元在基阵坐标系下的位置；
所述基阵坐标系是指以声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；
步骤三、根据信标在基阵坐标系下的位置来判断USBL是否定位有效；
若
Figure BDA0002400157930000021
成立，则USBL定位有效；否则，USBL定位无效；其中，R是声信号在水下航行器和信标之间的传播距离；
步骤四、根据信标在基阵坐标系下的位置，获得USBL在计算导航坐标系下对水下航行器的定位结果
Figure BDA0002400157930000022
将
Figure BDA0002400157930000023
称为水下航行器的位置测量值；
SINS输出的水下航行器在计算导航坐标系下的位置为
Figure BDA0002400157930000024
称
Figure BDA0002400157930000025
为水下航行器的位置预测值，利用
Figure BDA0002400157930000026
将各基元在基阵坐标系下的位置转换到计算导航坐标系下，获得各基元在计算导航坐标系下的位置；
步骤五、根据SINS计算的水下航行器速度以及各基元在计算导航坐标系下的位置，获得各基元相对信标的径向运动速度预测值
Figure BDA0002400157930000027
步骤六、USBL获得的各基元相对信标的径向运动速度测量值为
Figure BDA0002400157930000028
若步骤三中判断USBL定位有效，则将
Figure BDA0002400157930000029
与
Figure BDA00024001579300000210
的差以及
Figure BDA00024001579300000211
与
Figure BDA00024001579300000212
的差作为观测量，建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
若步骤三中判断USBL定位无效，则将
Figure BDA00024001579300000213
与
Figure BDA00024001579300000214
的差作为观测量建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
步骤七、根据SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型原理，利用模型输出的状态估计值修正SINS的输出，并重置扩展卡尔曼滤波器状态；
步骤八、重复步骤二至步骤七的过程，不断的对SINS的输出进行修正，获得修正后的导航输出结果。
本发明的有益效果是：本发明提出了一种引入径向速度的SINS/USBL松组合导航定位方法，本发明将USBL的径向速度测量信息引入SINS/USBL松组合中，能够在USBL定位无效时，使SINS保持组合工作方式，减小SINS的误差积累；通过将径向速度作为滤波观测量，能够提高SINS/USBL松组合滤波系统对SINS速度误差的估计精度，从而改善整体导航性能。
附图说明
图1是引入径向速度的SINS/USBL松组合导航定位数据融合原理图；
图2是基阵坐标系、载体坐标系和导航坐标系的示意图。
具体实施方式
具体实施方式一：如图1和图2所示，本实施方式所述的一种引入径向速度的SINS/USBL松组合导航定位方法，该方法包括以下步骤：
步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”(“ENU”)地理坐标系，该地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；
步骤二、建立基阵坐标系，并确定信标在基阵坐标系下的位置
Figure BDA0002400157930000031
和各基元在基阵坐标系下的位置；
所述基阵坐标系是指以声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；
步骤三、根据信标在基阵坐标系下的位置来判断USBL是否定位有效；
若
Figure BDA0002400157930000032
成立，则USBL定位有效；否则，USBL定位无效；其中，R是声信号在水下航行器和信标之间的传播距离；
步骤四、根据USBL计算的信标在基阵坐标系下的位置，获得USBL在计算导航坐标系下对水下航行器的定位结果
Figure BDA0002400157930000033
将
Figure BDA0002400157930000034
称为水下航行器的位置测量值；
SINS输出的水下航行器在计算导航坐标系下的位置为
Figure BDA0002400157930000035
称
Figure BDA0002400157930000036
为水下航行器的位置预测值，利用
Figure BDA0002400157930000037
将各基元在基阵坐标系下的位置转换到计算导航坐标系下，获得各基元在计算导航坐标系下的位置；
根据SINS(捷联惯导系统)计算的水下航行器姿态而复现的导航坐标系称为计算导航坐标系n′；
步骤五、根据SINS计算的水下航行器速度以及各基元在计算导航坐标系下的位置，获得各基元相对信标的径向运动速度预测值
Figure BDA0002400157930000038
步骤六、USBL获得的各基元相对信标的径向运动速度测量值为
Figure BDA0002400157930000041
若步骤三中判断USBL定位有效，则将
Figure BDA0002400157930000042
与
Figure BDA0002400157930000043
的差以及
Figure BDA0002400157930000044
与
Figure BDA0002400157930000045
的差作为观测量，建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
若步骤三中判断USBL定位无效，则将
Figure BDA0002400157930000046
与
Figure BDA0002400157930000047
的差作为观测量建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
步骤七、根据SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型原理，利用模型输出的状态估计值修正SINS的输出，并重置扩展卡尔曼滤波器状态；
步骤八、重复步骤二至步骤七的过程，不断的对SINS的输出进行修正，获得修正后的导航输出结果。
重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。
本实施方式中，超短基线声学基阵是由多个声信号接收换能器(基元)均匀分布的一个圆柱形设备。
具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中，信标在基阵坐标系下的位置
Figure BDA0002400157930000048
为：
Figure BDA0002400157930000049
其中：R是声信号在水下航行器和信标之间的传播距离，X′a为信标在xa轴方向的位置，Y′a为信标在ya轴方向的位置，Z′a为信标在za轴方向的位置；cosθX是信标在xa轴方向的位置与R的比值，cosθY是信标在ya轴方向的位置与R的比值；
Figure BDA00024001579300000410
其中：c是水中声速，τ是声学基阵各个基元测量的单程传播时延均值，单程传播距离为水下航行器相对于信标的距离，τX和τY分别是两两基元的传播时延差沿基阵坐标系xa轴和ya轴的分量，dX和dY是对应的两基元在xa轴和ya轴方向上的位置差；
各基元在基阵坐标系下的位置
Figure BDA00024001579300000411
为：
Figure BDA0002400157930000051
其中：i为基元序号，i＝1,2,3,4，r为基元1与3的间距或基元2与4的间距(基元1与3的间距同基元2与4的间距相等)；
Figure BDA0002400157930000052
为各基元在基阵坐标系的xa轴方向上的位置，
Figure BDA0002400157930000053
为各基元在基阵坐标系的ya轴方向上的位置，
Figure BDA0002400157930000054
为各基元在基阵坐标系的za轴方向上的位置，上角标T代表转置。
具体实施方式三：本实施方式与具体实施方式二不同的是：所述步骤四的具体过程为：
SINS提供的水下航行器在计算导航坐标系下的姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
基阵坐标系相对于载体坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与载体坐标系的三个坐标轴的角度偏差)，基阵坐标系原点相对于载体坐标系原点的位置偏差
Figure BDA0002400157930000055
为：
Figure BDA0002400157930000056
(ΔXb、ΔYb和ΔZb均为载体坐标系下分量)，ΔXb、ΔYb和ΔZb为
Figure BDA0002400157930000057
中的分量，上角标T代表转置；
载体坐标系到计算导航坐标系的转换矩阵
Figure BDA0002400157930000058
和基阵坐标系到载体坐标系的转换矩阵
Figure BDA0002400157930000059
分别为：
Figure BDA00024001579300000510
Figure BDA0002400157930000061
根据USBL解算的基阵坐标系下信标的位置
Figure BDA0002400157930000062
和GPS提供的信标在导航坐标系下的位置
Figure BDA0002400157930000063
得到
Figure BDA0002400157930000064
其中：
Figure BDA0002400157930000065
为USBL在计算导航坐标系下对水下航行器的定位结果；
计算导航坐标系n'与真实导航坐标系(建立的导航坐标系)的三个坐标轴存在角度误差φ：φ＝[φE φN φU]T，称φ为SINS的失准角误差，φE、φN和φU均为φ中的分量，计算导航坐标系n'与真实导航坐标系之间的转换矩阵
Figure BDA0002400157930000066
可近似为
Figure BDA0002400157930000067
其中：φ×为中间变量，I为单位矩阵；
Figure BDA0002400157930000068
水下航行器的真实位置
Figure BDA0002400157930000069
(是指水下航行器在真实导航坐标系下的位置)与
Figure BDA00024001579300000610
的差
Figure BDA00024001579300000611
为：
Figure BDA00024001579300000612
其中：n为超短基线声学定位系统中的高斯白噪声序列；
根据SINS输出的水下航行器在计算导航坐标系下的位置为
Figure BDA00024001579300000613
则USBL的第i个基元在计算导航坐标系中的位置
Figure BDA00024001579300000614
为：
Figure BDA00024001579300000615
本发明中，未特别限定的导航坐标系，均是指建立的导航坐标系。
α、β和γ分别是基阵坐标系的三个坐标轴相对于载体坐标系的三个坐标轴的角度偏差；
航向角为载体(水下航行器)艏向与地理北向的夹角，定义为北偏西为正，角度范围(-180°,180°]；横滚角γ为载体立轴zb与横轴xb轴所在铅垂面的夹角，载体向右倾斜时角度为正，角度范围(-180°,180°]；俯仰角θ为载体纵轴yb与其水平投影线之间的夹角，当载体抬头时为正，角度范围(-90°,90°]。
具体实施方式四：本实施方式与具体实施方式三不同的是：所述步骤五的具体过程为：
Figure BDA0002400157930000071
其中：
Figure BDA0002400157930000072
为第i个基元相对于信标在径向方向的运动速度预测值，
Figure BDA0002400157930000073
为第i个基元在计算导航坐标系的xn′轴方向位置，
Figure BDA0002400157930000074
为第i个基元在计算导航坐标系的yn′轴方向位置，
Figure BDA0002400157930000075
为第i个基元在计算导航坐标系的zn′轴方向位置，
Figure BDA0002400157930000076
为SINS计算的水下航行器在计算导航坐标系的xn′轴方向速度，
Figure BDA0002400157930000077
为SINS计算的水下航行器在计算导航坐标系的yn′轴方向速度，
Figure BDA0002400157930000078
为SINS计算的水下航行器在计算导航坐标系的zn′轴方向速度；
Figure BDA0002400157930000079
为信标在导航坐标系的xn轴方向位置，
Figure BDA00024001579300000710
为信标在导航坐标系的yn轴方向位置，
Figure BDA00024001579300000711
为信标在导航坐标系的zn轴方向位置，
Figure BDA00024001579300000712
其中，记中间变量cosζx、cosζy和cosζz为：
Figure BDA00024001579300000713
具体实施方式五：本实施方式与具体实施方式四不同的是：所述步骤六的具体过程为：
失准角误差φ为：φ＝[φE φN φU]T，SINS速度误差δν为：δν＝[δvx δvy δvz]T(速度误差是由SINS解算的计算导航坐标系下的水下航行器运动速度与真实导航坐标系下的速度真值的差)，SINS位置误差δp为：δp＝[δL δλ δh]T(位置误差是SINS解算的水下航行器在地球坐标系下的位置与真实位置的差值)，陀螺漂移误差ε为：ε＝[εx εy εz]T，加速度计偏置误差
Figure BDA0002400157930000081
为：
Figure BDA0002400157930000082
其中，δvx、δvy和δvz为δν中的分量，δL、δλ和δh为δp中的分量，εx、εy和εz为ε中的分量，
Figure BDA0002400157930000083
和
Figure BDA00024001579300000810
为
Figure BDA0002400157930000084
中的分量；
则
Figure BDA0002400157930000089
其中，X为扩展卡尔曼滤波器的状态变量；
扩展卡尔曼滤波器的状态方程为
Xk+1＝Fk+1/kXk+wk+1 (14)
其中，Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，由捷联惯性导航系统的误差方程获得，wk+1为扩展卡尔曼滤波器的过程噪声序列，通常为高斯白噪声形式；
当USBL定位有效时：
扩展卡尔曼滤波器的观测量Z为：
Figure BDA0002400157930000085
式中：
Figure BDA0002400157930000086
是
Figure BDA0002400157930000087
与水下航行器的真实位置
Figure BDA0002400157930000088
的差；
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (16)
其中，Zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波器的观测噪声序列，通常为高斯白噪声形式，Hk+1为k+1时刻的观测矩阵；
将SINS解算的水下航行器位置表示为纬度L、经度λ和高度h的球坐标形式，再将纬度L、经度λ和高度h转换到地球直角坐标系下；
地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
Figure BDA0002400157930000091
式中，x′e、y′e和z′e分别为SINS解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，RN为地球卯酉圈主曲率半径，
Figure BDA0002400157930000092
Re为地球半径，e为地球偏心率，
Figure BDA0002400157930000093
a和b分别是椭圆长轴和短轴半径；
上式的微分转换关系为：
Figure BDA0002400157930000094
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure BDA0002400157930000095
记中间变量矩阵A为：
Figure BDA0002400157930000096
Figure BDA0002400157930000097
与δp＝[δL δλ δh]T的关系为：
Figure BDA0002400157930000098
式(11)的全微分为
Figure BDA0002400157930000099
式中，
Figure BDA00024001579300000910
是SINS计算的水下航行器在计算导航坐标系下三个方向的速度微分。
Civv＝[cosζx cosζy cosζz] (24)
Figure BDA0002400157930000101
观测矩阵Hk+1为
Figure BDA0002400157930000102
当USBL定位无效时：
扩展卡尔曼滤波器的观测量Z为：
Figure BDA0002400157930000103
此时，观测方程的形式与公式(16)相同。
当USBL定位无效时，观测矩阵的形式为：
Figure BDA0002400157930000104
本实施方式在SINS/USBL中引入了径向速度作为滤波观测量，当USBL定位有效和定位无效的情况下分别对滤波的观测量进行选择，可以有效提高速度估计精度。而且，当USBL定位无效时，也能利用USBL测量的径向速度，使SINS也能处于组合工作状态，保证SINS/USBL组合工作能正常进行，减小导航误差累积，提高导航精度。
具体实施方式六：本实施方式与具体实施方式五不同的是：所述步骤七的具体过程为：
根据
Figure BDA0002400157930000105
计算k+1时刻的状态预测值
Figure BDA0002400157930000106
其中
Figure BDA0002400157930000107
为k时刻的状态估计值；
根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk计算k+1时刻的状态预测误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵；
根据
Figure BDA0002400157930000108
计算k+1时刻的滤波器增益Kk+1，其中Rk+1和Hk+1分别是k+1时刻的系统观测噪声协方差矩阵和观测矩阵；上角标-1代表矩阵的逆；
根据Pk+1＝(I-Kk+1Hk+1)Pk计算k+1时刻的状态估计误差协方差矩阵Pk+1；
根据
Figure BDA0002400157930000109
计算k+1时刻的状态估计值
Figure BDA00024001579300001010
根据
Figure BDA0002400157930000111
修正SINS的输出，并重置扩展卡尔曼滤波器状态。
本发明是基于一个同步信标，但本发明不仅限于此，多个同步信标或应答器均可采用本发明方法。
本发明是基于USBL4元基阵，但本发明不仅限于此，大于或等于3个接收基元的USBL定位系统均可采用本方法。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (6)

1.一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，该方法包括以下步骤：
步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；
步骤二、建立基阵坐标系，并确定信标在基阵坐标系下的位置
Figure FDA0002400157920000011
和各基元在基阵坐标系下的位置；
所述基阵坐标系是指以声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；
步骤三、根据信标在基阵坐标系下的位置来判断USBL是否定位有效；
若
Figure FDA0002400157920000012
成立，则USBL定位有效；否则，USBL定位无效；其中，R是声信号在水下航行器和信标之间的传播距离；
步骤四、根据信标在基阵坐标系下的位置，获得USBL在计算导航坐标系下对水下航行器的定位结果
Figure FDA0002400157920000013
将
Figure FDA0002400157920000014
称为水下航行器的位置测量值；
SINS输出的水下航行器在计算导航坐标系下的位置为
Figure FDA0002400157920000015
称
Figure FDA0002400157920000016
为水下航行器的位置预测值，利用
Figure FDA0002400157920000017
将各基元在基阵坐标系下的位置转换到计算导航坐标系下，获得各基元在计算导航坐标系下的位置；
步骤五、根据SINS计算的水下航行器速度以及各基元在计算导航坐标系下的位置，获得各基元相对信标的径向运动速度预测值
Figure FDA0002400157920000018
步骤六、USBL获得的各基元相对信标的径向运动速度测量值为
Figure FDA0002400157920000019
若步骤三中判断USBL定位有效，则将
Figure FDA00024001579200000110
与
Figure FDA00024001579200000111
的差以及
Figure FDA00024001579200000112
与
Figure FDA00024001579200000113
的差作为观测量，建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
若步骤三中判断USBL定位无效，则将
Figure FDA00024001579200000114
与
Figure FDA00024001579200000115
的差作为观测量建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
步骤七、根据SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型原理，利用模型输出的状态估计值修正SINS的输出，并重置扩展卡尔曼滤波器状态；
步骤八、重复步骤二至步骤七的过程，不断的对SINS的输出进行修正，获得修正后的导航输出结果。
2.根据权利要求1所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤二中，信标在基阵坐标系下的位置
Figure FDA0002400157920000021
为：
Figure FDA0002400157920000022
其中：R是声信号在水下航行器和信标之间的传播距离，X′a为信标在xa轴方向的位置，Y′a为信标在ya轴方向的位置，Z′a为信标在za轴方向的位置；cosθX是信标在xa轴方向的位置与R的比值，cosθY是信标在ya轴方向的位置与R的比值；
Figure FDA0002400157920000023
其中：c是水中声速，τX和τY分别是两两基元的传播时延差沿基阵坐标系xa轴和ya轴的分量，dX和dY是对应的两基元在xa轴和ya轴方向上的位置差；
各基元在基阵坐标系下的位置
Figure FDA0002400157920000024
为：
Figure FDA0002400157920000025
其中：i为基元序号，i＝1,2,3,4，r为基元1与3的间距或基元2与4的间距；
Figure FDA0002400157920000026
为各基元在基阵坐标系的xa轴方向上的位置，
Figure FDA0002400157920000027
为各基元在基阵坐标系的ya轴方向上的位置，
Figure FDA0002400157920000028
为各基元在基阵坐标系的za轴方向上的位置，上角标T代表转置。
3.根据权利要求2所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤四的具体过程为：
SINS提供的水下航行器在计算导航坐标系下的姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
基阵坐标系相对于载体坐标系的角度安装偏差分别为α、β和γ，基阵坐标系原点相对于载体坐标系原点的位置偏差
Figure FDA0002400157920000031
为：
Figure FDA0002400157920000032
ΔXb、ΔYb和ΔZb为
Figure FDA0002400157920000033
中的分量，上角标T代表转置；
载体坐标系到计算导航坐标系的转换矩阵
Figure FDA0002400157920000034
和基阵坐标系到载体坐标系的转换矩阵
Figure FDA0002400157920000035
分别为：
Figure FDA0002400157920000036
Figure FDA0002400157920000037
根据USBL解算的基阵坐标系下信标的位置
Figure FDA0002400157920000038
和GPS提供的信标在导航坐标系下的位置
Figure FDA0002400157920000039
得到
Figure FDA00024001579200000310
其中：
Figure FDA00024001579200000311
为USBL在计算导航坐标系下对水下航行器的定位结果；
计算导航坐标系n'与真实导航坐标系的三个坐标轴存在角度误差φ：φ＝[φE φNφU]T，称φ为SINS的失准角误差，φE、φN和φU均为φ中的分量，计算导航坐标系n'与真实导航坐标系之间的转换矩阵
Figure FDA00024001579200000312
为
Figure FDA00024001579200000313
其中：φ×为中间变量，I为单位矩阵；
Figure FDA0002400157920000041
水下航行器的真实位置
Figure FDA0002400157920000042
与
Figure FDA0002400157920000043
的差
Figure FDA0002400157920000044
为：
Figure FDA0002400157920000045
其中：n为超短基线声学定位系统中的高斯白噪声序列；
根据SINS输出的水下航行器在计算导航坐标系下的位置为
Figure FDA0002400157920000046
则USBL的第i个基元在计算导航坐标系中的位置
Figure FDA0002400157920000047
为：
Figure FDA0002400157920000048
4.根据权利要求3所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤五的具体过程为：
Figure FDA0002400157920000049
其中：
Figure FDA00024001579200000410
为第i个基元相对于信标在径向方向的运动速度预测值，
Figure FDA00024001579200000411
为第i个基元在计算导航坐标系的xn′轴方向位置，
Figure FDA00024001579200000412
为第i个基元在计算导航坐标系的yn′轴方向位置，
Figure FDA00024001579200000413
为第i个基元在计算导航坐标系的zn′轴方向位置，
Figure FDA00024001579200000414
为SINS计算的水下航行器在计算导航坐标系的xn′轴方向速度，
Figure FDA00024001579200000415
为SINS计算的水下航行器在计算导航坐标系的yn′轴方向速度，
Figure FDA00024001579200000416
为SINS计算的水下航行器在计算导航坐标系的zn′轴方向速度；
Figure FDA00024001579200000417
为信标在导航坐标系的xn轴方向位置，
Figure FDA00024001579200000418
为信标在导航坐标系的yn轴方向位置，
Figure FDA00024001579200000419
为信标在导航坐标系的zn轴方向位置，
Figure FDA00024001579200000420
其中，记中间变量cosζx、cosζy和cosζz为：
Figure FDA0002400157920000051
5.根据权利要求4所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤六的具体过程为：
失准角误差φ为：φ＝[φE φN φU]T，SINS速度误差δν为：δν＝[δvx δvy δvz]T，SINS位置误差δp为：δp＝[δL δλ δh]T，陀螺漂移误差ε为：ε＝[εx εy εz]T，加速度计偏置误差
Figure FDA0002400157920000052
为：
Figure FDA0002400157920000053
其中，δvx、δvy和δvz为δν中的分量，δL、δλ和δh为δp中的分量，εx、εy和εz为ε中的分量，
Figure FDA0002400157920000054
和
Figure FDA0002400157920000055
为
Figure FDA0002400157920000056
中的分量；
则
Figure FDA0002400157920000057
其中，X为扩展卡尔曼滤波器的状态变量；
扩展卡尔曼滤波器的状态方程为
Xk+1＝Fk+1/kXk+wk+1 (14)
其中，Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波器的过程噪声序列；
当USBL定位有效时：
扩展卡尔曼滤波器的观测量Z为：
Figure FDA0002400157920000058
式中：
Figure FDA0002400157920000059
Figure FDA00024001579200000510
是
Figure FDA00024001579200000511
与水下航行器的真实位置
Figure FDA00024001579200000512
的差；
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (16)
其中，Zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波器的观测噪声序列，Hk+1为k+1时刻的观测矩阵；
当USBL定位无效时：
扩展卡尔曼滤波器的观测量Z为：
Figure FDA0002400157920000061
此时，观测方程的形式与公式(16)相同。
6.根据权利要求5所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤七的具体过程为：
根据
Figure FDA0002400157920000062
计算k+1时刻的状态预测值
Figure FDA0002400157920000063
其中
Figure FDA0002400157920000064
为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 FDA0002400157920000065
计算k+1时刻的状态估计值
Figure FDA0002400157920000066
根据
Figure FDA0002400157920000067
修正SINS的输出，并重置扩展卡尔曼滤波器状态。
CN202010149299.2A 2020-03-04 2020-03-04 一种引入径向速度的sins/usbl松组合导航定位方法 Active CN111380520B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010149299.2A CN111380520B (zh) 2020-03-04 2020-03-04 一种引入径向速度的sins/usbl松组合导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010149299.2A CN111380520B (zh) 2020-03-04 2020-03-04 一种引入径向速度的sins/usbl松组合导航定位方法

Publications (2)

Publication Number Publication Date
CN111380520A true CN111380520A (zh) 2020-07-07
CN111380520B CN111380520B (zh) 2022-02-11

Family

ID=71222660

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010149299.2A Active CN111380520B (zh) 2020-03-04 2020-03-04 一种引入径向速度的sins/usbl松组合导航定位方法

Country Status (1)

Country Link
CN (1) CN111380520B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116068540A (zh) * 2023-02-17 2023-05-05 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20120096626A (ko) * 2011-02-23 2012-08-31 대양전기공업 주식회사 수중운동체의 위치 추정 방법 및 수중운동체의 위치 추정 장치
CN106483498A (zh) * 2015-09-01 2017-03-08 北京自动化控制设备研究所 一种sinsusbl紧耦合算法
CN106546956A (zh) * 2016-10-25 2017-03-29 中国海洋石油总公司 一种超短基线接收基阵基元位置的精确标定方法
CN109737956A (zh) * 2018-12-17 2019-05-10 东南大学 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20120096626A (ko) * 2011-02-23 2012-08-31 대양전기공업 주식회사 수중운동체의 위치 추정 방법 및 수중운동체의 위치 추정 장치
CN106483498A (zh) * 2015-09-01 2017-03-08 北京自动化控制设备研究所 一种sinsusbl紧耦合算法
CN106546956A (zh) * 2016-10-25 2017-03-29 中国海洋石油总公司 一种超短基线接收基阵基元位置的精确标定方法
CN109737956A (zh) * 2018-12-17 2019-05-10 东南大学 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
B.WANG等: "SINS/USBL INTEGRATED NAVIGATION FAULT-TOLERANT METHOD WITH CHI-SQUARE TEST", 《2018 25TH SAINT PETERSBURG INTERNATIONAL CONFERENCE ON INTEGRATED NAVIGATION SYSTEMS (ICINS)》 *
DAJUN SUN等: "An Underwater Acoustic Positioning Algorithm for Compact Arrays With Arbitrary Configuration", 《IEEE JOURNAL OF SELECTED TOPICS IN SIGNAL PROCESSING》 *
孙大军等: "水声定位导航技术的发展与展望", 《中国科学院院刊》 *
徐博等: "基于倒置声学基阵的INSUSBL组合导航算法研究", 《海洋技术学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116068540A (zh) * 2023-02-17 2023-05-05 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法
CN116068540B (zh) * 2023-02-17 2023-09-12 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法

Also Published As

Publication number Publication date
CN111380520B (zh) 2022-02-11

Similar Documents

Publication Publication Date Title
CN101788296B (zh) 一种sins/cns深组合导航系统及其实现方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN110926468B (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN107015259B (zh) 采用多普勒测速仪计算伪距/伪距率的紧组合方法
CN113311436B (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN101261130B (zh) 一种船用光纤捷联惯导系统传递对准精度评估方法
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN115200574A (zh) 一种地球椭球模型下的极区横向组合导航方法
CN111829512A (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN111380519B (zh) 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN111380520B (zh) 一种引入径向速度的sins/usbl松组合导航定位方法
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN115542363B (zh) 一种适用于垂直下视航空吊舱的姿态测量方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN113176539B (zh) 一种水声信号噪声多级抑制与稳健定位系统及定位方法
CN114705188A (zh) 一种基于格网水流速度辅助的惯性/多普勒中水层导航方法
CN111380517B (zh) 一种基于usbl软件接收机的sins/usbl深组合导航定位方法
RU2428659C2 (ru) Способ спутниковой коррекции гироскопических навигационных систем морских объектов
CN116068540B (zh) 一种声学多普勒测速径向波束角校正方法
CN117470234B (zh) 基于Psi角误差模型的舰船跨极区滤波切换方法
CN108957508A (zh) 车载pos离线组合估计方法和装置

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