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

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

Info

Publication number
CN111380518A
CN111380518A CN202010144245.7A CN202010144245A CN111380518A CN 111380518 A CN111380518 A CN 111380518A CN 202010144245 A CN202010144245 A CN 202010144245A CN 111380518 A CN111380518 A CN 111380518A
Authority
CN
China
Prior art keywords
coordinate system
sins
navigation
underwater vehicle
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
CN202010144245.7A
Other languages
English (en)
Other versions
CN111380518B (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 CN202010144245.7A priority Critical patent/CN111380518B/zh
Publication of CN111380518A publication Critical patent/CN111380518A/zh
Application granted granted Critical
Publication of CN111380518B publication Critical patent/CN111380518B/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

一种引入径向速度的SINS/USBL紧组合导航定位方法，本发明涉及水下组合导航定位方法。本发明的目的是为了提高现有传统SINS/USBL紧组合的速度精度和大开角下紧组合的稳定性。过程为：一、将USBL基阵和SINS的陀螺组件和加速度计组件安装在水下航行器上；二、确定基元在基阵坐标系下的位置；三、得到USBL测量的各基元相对信标的径向运动速度；四、得到基元在计算导航坐标系下的位置；五、计算传播时延和传播时延差；六、将航行器运动速度沿径向分解；七、建立状态方程和观测方程；八、修正SINS输出，重置扩展卡尔曼滤波器状态，重新执行步骤三～步骤八，直至导航结束。本发明用于组合导航及水声定位技术领域。

Description

一种引入径向速度的SINS/USBL紧组合导航定位方法
技术领域
本发明涉及SINS/USBL紧组合导航定位方法，属于组合导航及水声定位技术领域。
背景技术
传统的SINS/USBL紧组合建立在以传播时延(斜距)之差和时延差(斜距差)之差为观测量的基础上。径向速度是水下航行器与参考信标在视在方向的运动速度，描述了水下航行器与参考信标之间的距离变化率大小，径向速度与航行器的运动速度直接相关。USBL能够通过计算基元接收信号与发射信号间的多普勒频率，获得基元与信标之间的径向速度。通过引入径向速度既能够提高滤波系统对速度误差的估计精度，又能保证大开角下紧组合导航的稳定性，改善整体导航性能，而传统的SINS/USBL紧组合导航忽略了USBL计算的径向速度这一冗余测量，导致在传统的紧组合滤波系统中，速度误差的可观测性较弱，数据融合结果对SINS的速度误差修正效果相对较差，且降低了在大开角情况下，SINS/USBL紧组合的导航精度和稳定性。
发明内容
本发明的目的是为了提高现有传统SINS/USBL紧组合的速度误差估计精度和大开角情况下的SINS/USBL紧组合稳定性，而提出一种引入径向速度的SINS/USBL紧组合导航定位方法。
一种引入径向速度的SINS/USBL紧组合导航定位方法具体过程为：
步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；
所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；
水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；
所述声学基阵是由多个基元均匀分布的一个圆柱形设备；
所述多个为大于等于3个；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；
步骤三、USBL测量的声信号在各基元和信标间的传播时延为
Figure BDA0002400160560000021
传播时延差为
Figure BDA0002400160560000022
Figure BDA0002400160560000023
下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为
Figure BDA0002400160560000024
步骤四、根据SINS计算水下航行器的姿态、速度和位置；
结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置；
步骤五、结合声信号在航行器与信标之间传播的有效声速，根据SINS转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；
步骤六、将SINS解算的航行器运动速度沿径向分解；
步骤七、建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程；
步骤八、计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。
本发明的有益效果为：
本发明将USBL定位系统计算的水下航行器相对声学信标在径向方向的运动速度引入传统的SINS/USBL紧组合中，增强滤波系统中速度误差的可观测性，提高速度误差的估计精度，最终实现组合导航速度和位置精度的提高；同时提高SINS/USBL紧组合在大开角情况下的导航精度和稳定性。
附图说明
图1为SINS/USBL紧组合导航定位数据融合原理图；
图2为基阵坐标系、载体坐标系和导航坐标系的示意简图。
具体实施方式
具体实施方式一：本实施方式一种引入径向速度的SINS/USBL紧组合导航定位方法具体过程为：
步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；
所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；
水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；
所述声学基阵是由多个基元(声信号接收换能器)均匀分布的一个圆柱形设备；
所述多个为大于等于3个；
所述导航坐标系是指“东北天”(“ENU”)地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；
步骤三、USBL测量的声信号在各基元和信标间的传播时延为
Figure BDA0002400160560000031
传播时延差为
Figure BDA0002400160560000032
Figure BDA0002400160560000033
下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为
Figure BDA0002400160560000034
步骤四、根据SINS计算水下航行器的姿态、速度和位置(在这里把SINS当成一个黑匣子，它输出水下航行器的姿态、速度和位置)；
结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果(
Figure BDA0002400160560000035
和
Figure BDA0002400160560000036
)，得到基元在计算导航坐标系下的位置；
步骤五、结合声信号在航行器与信标之间传播的有效声速(声速剖面仪(SVP)测量)，根据SINS转换的基元在计算导航坐标系中的位置信息(式5)，计算声信号在各基元与信标间的传播时延和传播时延差；
步骤六、将SINS解算的航行器运动速度(SINS的输出)沿径向分解；
步骤七、建立基于传播时延、传播时延差和径向速度信息(公式9、11、12)融合的扩展卡尔曼滤波器状态方程和观测方程；
步骤八、计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零(所述重置滤波状态变量是指，在修正SINS输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。)，重新执行步骤三～步骤八，直至导航结束。
具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；具体过程为：
声学基阵坐标系是指以基阵中心为原点，y轴沿基阵平面指向水下航行器的艏向方向，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；
基元在声学基阵坐标系下的坐标为：
Figure BDA0002400160560000041
式(1)中，i为基元序号，I′为基元总数，
Figure BDA0002400160560000042
为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，
Figure BDA0002400160560000043
为基元i在声学基阵坐标系y轴下的坐标，
Figure BDA0002400160560000044
为基元i在声学基阵坐标系z轴下的坐标。
其它步骤及参数与具体实施方式一相同。
具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤三中USBL测量的声信号在各基元和信标间的传播时延为
Figure BDA0002400160560000045
传播时延差为
Figure BDA0002400160560000046
Figure BDA0002400160560000047
下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为
Figure BDA0002400160560000048
具体过程为：
将
Figure BDA0002400160560000049
和
Figure BDA00024001605600000410
表示为：
Figure BDA00024001605600000411
式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声。
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤四中根据SINS计算水下航行器的姿态、速度和位置(在这里把SINS当成一个黑匣子，它输出水下航行器的姿态、速度和位置)；
结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果(
Figure BDA00024001605600000412
和
Figure BDA00024001605600000413
)，得到基元在计算导航坐标系下的位置；具体过程为：
建立水下航行器坐标系obxbybzb，水下航行器坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，水下航行器坐标系的定义满足右手定则；
所述航行器姿态包括航向角A、俯仰角K和横滚角ψ，根据航行器姿态复现(对真实导航系的一个跟踪)的导航坐标系称计算导航坐标系n'。
声学基阵坐标系相对于水下航行器坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与水下航行器坐标系的三个坐标轴的角度偏差)，声学基阵坐标系原点相对于水下航行器坐标系原点的位置偏差
Figure BDA0002400160560000051
为：
Figure BDA0002400160560000052
其中，ΔXb、ΔYb和ΔZb为
Figure BDA0002400160560000053
中的分量，上角标T代表转置；
水下航行器坐标系到计算导航坐标系n'的转换矩阵
Figure BDA0002400160560000054
和声学基阵坐标系到水下航行器坐标系的转换矩阵
Figure BDA0002400160560000055
分别为：
Figure BDA0002400160560000056
Figure BDA00024001605600000512
则第i个基元在计算导航坐标系下的位置表示为
Figure BDA0002400160560000057
式中，
Figure BDA0002400160560000058
为第i个基元在声学基阵坐标系下的位置，
Figure BDA0002400160560000059
为SINS计算的水下航行器位置；
由于SINS的计算导航坐标系n'与导航坐标系n的三个坐标轴之间存在角度误差φ＝[φx φy φz]T，称失准角误差，计算导航坐标系n'与导航坐标系n之间的转换矩阵
Figure BDA00024001605600000510
可近似为
Figure BDA00024001605600000511
其中
Figure BDA0002400160560000061
式中，φ＝[φx φy φz]T为失准角误差(SINS姿态误差)，I为三阶单位阵；φx、φy、φz为φ中的分量；
第i个基元在SINS计算导航坐标系下的位置
Figure BDA0002400160560000062
与在真实导航坐标系(建立的导航坐标系)下位置值
Figure BDA0002400160560000063
之间的误差
Figure BDA0002400160560000064
为
Figure BDA0002400160560000065
式中，ΔXi是
Figure BDA0002400160560000066
与水下航行器在真实导航坐标系下位置
Figure BDA0002400160560000067
的差；记
Figure BDA0002400160560000068
Figure BDA0002400160560000069
是在计算导航坐标系中第i个基元相对水下航行器质心的位置。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤五中结合声信号在航行器与信标之间传播的有效声速(声速剖面仪(SVP)测量)，根据SINS转换的基元在计算导航坐标系中的位置信息(式5)，计算声信号在各基元与信标间的传播时延和传播时延差；具体过程为：
声信号在第i个基元与信标之间的传播时延预测值
Figure BDA00024001605600000610
为：
Figure BDA00024001605600000611
式中，c是测量的有效声速，
Figure BDA00024001605600000612
是信标在真实导航坐标系中的位置，由GPS提供的位置信息转换得到，||·||是矩阵二范数的表示；
Figure BDA00024001605600000613
其中，
Figure BDA00024001605600000614
为信标在真实导航坐标系的xn轴方向位置，
Figure BDA00024001605600000615
为信标在真实导航坐标系的yn轴方向位置，
Figure BDA0002400160560000071
为信标在真实导航坐标系的zn轴方向位置，
Figure BDA0002400160560000072
为第i个基元在计算导航坐标系的xn轴方向位置，
Figure BDA0002400160560000073
为第i个基元在计算导航坐标系的yn轴方向位置，
Figure BDA0002400160560000074
为第i个基元在计算导航坐标系的zn轴方向位置；
声信号的传播时延差为：
Figure BDA0002400160560000075
式中，
Figure BDA0002400160560000076
为SINS计算的第j个基元传播时延预测值，
Figure BDA0002400160560000077
为SINS计算的第1个基元传播时延预测值。
所述有效声速为信标和接收基阵的几何距离与声信号在两点(信标和接收基阵)间的传播时间的比值。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤六中将SINS解算的航行器运动速度(SINS的输出)沿径向分解；具体过程为：
将SINS解算的航行器运动速度沿径向分解为：
Figure BDA0002400160560000078
其中：
Figure BDA0002400160560000079
为第i个基元相对于信标在径向方向的运动速度大小，
Figure BDA00024001605600000710
为第i个基元在计算导航坐标系xn′轴方向位置，
Figure BDA00024001605600000711
为第i个基元在计算导航坐标系yn′轴方向位置，
Figure BDA00024001605600000712
为第i个基元在计算导航坐标系zn′轴方向位置，
Figure BDA00024001605600000713
为水下航行器在计算导航坐标系的xn′轴方向速度，
Figure BDA00024001605600000714
为水下航行器在计算导航坐标系的yn′轴方向速度，
Figure BDA00024001605600000715
为水下航行器在计算导航坐标系的zn′轴方向速度，
Figure BDA00024001605600000716
Figure BDA00024001605600000717
式中，cosζx、cosζy、cosζz为中间变量。
其它步骤及参数与具体实施方式一至五之一相同。
具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：所述步骤七中建立基于传播时延、传播时延差和径向速度信息(公式9、11、12)融合的扩展卡尔曼滤波器状态方程和观测方程；具体过程为：
所述信息融合扩展卡尔滤波器是指以失准角误差、SINS的速度误差、位置误差、陀螺漂移、加速度计偏置为状态变量，以SINS和USBL的传播时延之差、传播时延差之差以及径向速度之差作为观测量，建立描述系统的状态方程和观测方程。
组合导航一般有反馈校正的闭环方式和输出修正的开环方式，前者工程实现复杂，当滤波器有故障时会直接影响SINS的输出，而输出修正不涉及独立的导航系统内部，容错能力较强，因此本发明采用输出修正方式；
失准角误差φ＝[φx φy φz]T，SINS速度误差δν＝[δvx δvy δvz]T(速度误差是由SINS解算的计算导航坐标系下的水下航行器运动速度与真实速度的差)，SINS位置误差δp＝[δL δλ δh]T(位置误差是SINS解算的水下航行器在地球坐标系下的位置与真实位置的差值)，陀螺漂移误差ε＝[εx εy εz]T，加速度计偏置误差
Figure BDA0002400160560000081
则
Figure BDA0002400160560000082
其中，X为扩展卡尔曼滤波系统的状态变量；
扩展卡尔曼滤波系统的状态方程为
Xk+1＝Fk+1/kXk+wk+1 (15)
其中，Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，由SINS的误差方程获得，wk+1为扩展卡尔曼滤波系统过程噪声序列，通常为高斯白噪声形式；
扩展卡尔曼滤波系统的观测量Z为：
Figure BDA0002400160560000083
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (17)
其中，Zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波系统观测噪声序列，通常为高斯白噪声形式，Hk+1为k时刻的观测矩阵，
Figure BDA0002400160560000091
为声信号在第i个基元与信标之间的传播时延预测值；
观测矩阵Hk+1为
Figure BDA0002400160560000092
式中，Cipφ为中间变量，01×3为3维零向量，Cipp为中间变量，
Figure BDA0002400160560000093
为地球直角坐标系到SINS计算导航坐标系的转换矩阵，A为中间变量矩阵，01×6为6维零向量，Cjpφ为第j个基元对应的中间变量，j≠1，C1pφ为第1个基元对应的中间变量，Cjpp为第j个基元对应的中间变量，j≠1；C1pp为第1个基元对应的中间变量，Civv为中间变量，Cipv为中间变量。
其它步骤及参数与具体实施方式一至六之一相同。
具体实施方式八：本实施方式与具体实施方式一至七之一不同的是：所述观测矩阵Hk+1具体推算过程为：
SINS解算的水下航行器位置表示为纬度L、经度λ和高度h的球坐标形式，再将纬度L、经度λ和高度h转换到地球直角坐标系下；
地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
Figure BDA0002400160560000094
式中，x′e、y′e和z′e分别为SINS解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，RN为地球卯酉圈主曲率半径，
Figure BDA0002400160560000095
Re为地球半径，e为地球偏心率，
Figure BDA0002400160560000096
a和b分别是地球椭圆长轴和短轴半径；
式(18)的微分转换关系为：
Figure BDA0002400160560000101
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure BDA0002400160560000102
记中间变量矩阵A为：
Figure BDA0002400160560000103
结合式(7)和式(8)，得到式(9)的全微分形式：
Figure BDA0002400160560000104
式中，[dxn′ dyn′ dzn′]T是SINS计算的水下航行器位置与真实位置在三个轴方向的位置误差；
Figure BDA0002400160560000105
是
Figure BDA0002400160560000106
中的坐标分量；
Figure BDA0002400160560000107
为
Figure BDA0002400160560000108
的微分，c为有效声速，
Figure BDA00024001605600001012
是
Figure BDA00024001605600001010
沿计算导航坐标系xn′轴、yn′轴和zn′轴分量；
记
Figure BDA00024001605600001011
式中，Cipp、Cipφ为中间变量；
式(12)的全微分为
Figure BDA0002400160560000111
式中，
Figure BDA0002400160560000112
是SINS计算的水下航行器在导航坐标系下三个方向的速度误差；Civv、Cipv为中间变量；
Civv＝[cosζx cosζy cosζz] (26)
Figure BDA0002400160560000113
式中，
Figure BDA0002400160560000114
为水下航行器在计算导航坐标系下三个方向的速度；
观测矩阵Hk+1为
Figure BDA0002400160560000115
式中，Cipφ为中间变量，01×3为3维零向量，Cipp为中间变量，
Figure BDA0002400160560000116
为地球直角坐标系到SINS计算导航坐标系的转换矩阵，A为中间变量矩阵，01×6为6维零向量，Cjpφ为第j个基元对应的中间变量，j≠1，C1pφ为第1个基元对应的中间变量，Cjpp为第j个基元对应的中间变量，j≠1；C1pp为第1个基元对应的中间变量，Civv为中间变量，Cipv为中间变量。
其它步骤及参数与具体实施方式一至七之一相同。
具体实施方式九：本实施方式与具体实施方式一至八之一不同的是：所述步骤八中计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零(所述重置滤波状态变量是指，在修正SINS输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。)，重新执行步骤三～步骤八，直至导航结束；具体过程为：
计算SINS的导航误差，具体过程为：
根据
Figure BDA0002400160560000117
计算k+1时刻的状态预测值
Figure BDA0002400160560000118
式中，
Figure BDA0002400160560000119
为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；
式中，I为三阶单位阵；
根据
Figure BDA0002400160560000121
计算k+1时刻的状态估计值
Figure BDA0002400160560000122
根据
Figure BDA0002400160560000123
修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。
其它步骤及参数与具体实施方式一至八之一相同。
所述扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理；
所述卡尔滤波器是指在最小均方误差准则下的线性贝叶斯估计；
所述修正SINS输出是指在SINS输出的vn'、
Figure BDA0002400160560000124
上减去状态变量的对应的估计值(δv、δp)，在航向角A、俯仰角K和横滚角ψ上，将φ结合姿态算法进行修正，得到新的惯性导航输出；
所述SINS输出为航向角A、俯仰角K和横滚角ψ，航行器运动速度vn'、航行器位置
Figure BDA0002400160560000125
状态变量的估计值为
Figure BDA0002400160560000126
所述重置扩展卡尔曼滤波器状态变量是指在修正SINS输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。
所述信息融合扩展卡尔滤波器是指以SINS的姿态误差、速度误差、位置误差、陀螺漂移、加速度计偏置为状态变量，以USBL和SINS的传播时延之差、传播时延差之差以及径向速度之差作为观测量，建立描述系统的状态方程和观测方程。
本发明首次将USBL测量的水下航行器相对信标在径向方向的运动速度引入SINS/USBL紧组合，以增强组合导航滤波器中速度误差的可观测性，进一步提高组合导航精度。
采用以下实施例验证本发明的有益效果：
实施例一：
本实施例具体是按照以下步骤制备的：
1.将超短基线声学基阵和捷联惯性导航的陀螺组件、加速度计组件安装在水下航行器上。
2.附图2中基元1-5在基阵坐标系下的坐标为：
Figure BDA0002400160560000131
式(29)中，i为基元序号，
Figure BDA0002400160560000132
为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，
Figure BDA0002400160560000133
为基元i在声学基阵坐标系y轴下的坐标，
Figure BDA0002400160560000134
为基元i在声学基阵坐标系z轴下的坐标；
3.超短基线定位系统测量的声信号在各基元和信标间的传播时延为
Figure BDA0002400160560000135
传播时延差为
Figure BDA0002400160560000136
下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,3,4,5，径向速度为
Figure BDA0002400160560000137
将
Figure BDA0002400160560000138
和
Figure BDA0002400160560000139
表示为：
Figure BDA00024001605600001310
式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声；
4.航行器姿态包括航向角A、俯仰角K和横滚角ψ
声学基阵坐标系相对于水下航行器坐标系的角度安装偏差分别为α、β和γ(基阵坐标系的三个坐标轴与载体坐标系的三个坐标轴的角度偏差)，声学基阵坐标系原点相对于水下航行器坐标系原点的位置偏差
Figure BDA00024001605600001311
为：
Figure BDA00024001605600001312
其中，ΔXb、ΔYb和ΔZb为
Figure BDA0002400160560000141
中的分量，上角标T代表转置；
水下航行器坐标系到计算导航坐标系n'的转换矩阵
Figure BDA0002400160560000142
和声学基阵坐标系到水下航行器坐标系的转换矩阵
Figure BDA0002400160560000143
分别为：
Figure BDA0002400160560000144
Figure BDA00024001605600001414
则第i个基元在计算导航坐标系下的位置表示为
Figure BDA0002400160560000145
由于捷联惯性导航计算的导航坐标系n'与真实导航坐标系n存在角度误差φ＝[φx φy φz]T，二者之间的转换矩阵
Figure BDA0002400160560000146
可近似为
Figure BDA0002400160560000147
其中
Figure BDA0002400160560000148
因此基元在导航坐标系下的位置误差为
Figure BDA0002400160560000149
5.第i个基元在SINS计算导航坐标系下的位置
Figure BDA00024001605600001410
与在真实导航坐标系(建立的导航坐标系)下位置
Figure BDA00024001605600001411
之间的误差
Figure BDA00024001605600001412
为：
Figure BDA00024001605600001413
式(37)中，c是有效声速，||·||是矩阵二范数的表示，即
Figure BDA0002400160560000151
声信号的传播时延差预测值为：
Figure BDA0002400160560000152
6.捷联惯性导航计算的航行器运动速度在径向的分解为：
Figure BDA0002400160560000153
记
Figure BDA0002400160560000154
7.建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程。
惯性导航位置表示为纬度L、经度λ和高度h的形式，需要将其转换为直角坐标，则
Figure BDA0002400160560000155
式(42)中，RN为地球卯酉圈主曲率半径
Figure BDA0002400160560000156
其中Re为地球半径；e为地球偏心率，
Figure BDA0002400160560000157
其中a,b分别是椭圆长轴和短轴半径。
组合导航一般有反馈校正的闭环方式和输出修正的开环方式，前者工程实现复杂，当滤波器有故障时会直接影响捷联惯性导航的输出，而输出修正不涉及独立的导航系统内部，容错能力较强，因此本发明采用输出修正方式。
扩展卡尔曼滤波状态变量由捷联惯性导航姿态误差φ＝[φx φy φz]T，速度误差δν＝[δvx δvy δvz]T，位置误差δp＝[δL δλ δh]T，陀螺漂移误差ε＝[εx εy εz]T，加速度计偏置误差
Figure BDA0002400160560000158
组成，简记为
Figure BDA0002400160560000161
状态方程为
Xk+1＝Fk+1/kXk+wk+1 (44)
其中Fk+1/k为状态转移矩阵，由捷联惯性导航系统的误差方程获得。wk+1为高斯白噪声序列。
导航坐标系与地球坐标系的微分转换关系如下所示
Figure BDA0002400160560000162
记
Figure BDA0002400160560000163
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure BDA0002400160560000164
扩展卡尔曼滤波的观测量为
Figure BDA0002400160560000165
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (49)
其中vk+1为高斯白噪声序列。
结合式(36)，式(37)的全微分表示为：
Figure BDA0002400160560000171
记
Figure BDA0002400160560000172
式(40)的全微分为
Figure BDA0002400160560000173
其中
Civv＝[cosζx cosζy cosζz] (54)
Figure BDA0002400160560000174
观测矩阵Hk+1为
Figure BDA0002400160560000181
8.计算捷联惯性导航的导航误差，滤波解算过程如下。
根据
Figure BDA0002400160560000182
计算k+1时刻的状态预测值
Figure BDA0002400160560000183
根据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 BDA0002400160560000184
计算k+1时刻的状态估计值
Figure BDA0002400160560000185
根据
Figure BDA0002400160560000186
的计算结果修正捷联惯性导航输出，重置扩展卡尔曼滤波器状态，重新执行步骤3～步骤8，直至导航结束。
实施例2
本发明中，扩展卡尔曼滤波器的状态方程是由捷联惯性导航系统的误差方程构成，但不仅限于此，如在状态方程中加入超短基线的测时误差和安装偏差等。
本发明中，时延和时延差观测信息与斜距、斜距差信息意义等价。
本发明是基于一个同步信标，但不仅限于此，多个同步信标或应答器均可采用上述方法。
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (9)

1.一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述方法具体过程为：
步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；
所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；
水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；
所述声学基阵是由多个基元均匀分布的一个圆柱形设备；
所述多个为大于等于3个；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；
步骤三、USBL测量的声信号在各基元和信标间的传播时延为
Figure FDA0002400160550000011
传播时延差为
Figure FDA0002400160550000012
Figure FDA0002400160550000013
下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为
Figure FDA0002400160550000014
步骤四、根据SINS计算水下航行器的姿态、速度和位置；
结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置；
步骤五、结合声信号在航行器与信标之间传播的有效声速，根据SINS转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；
步骤六、将SINS解算的航行器运动速度沿径向分解；
步骤七、建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程；
步骤八、计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。
2.根据权利要求1所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤二中建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；具体过程为：
声学基阵坐标系是指以基阵中心为原点，y轴沿基阵平面指向水下航行器的艏向方向，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；
基元在声学基阵坐标系下的坐标为：
Figure FDA0002400160550000021
式(1)中，i为基元序号，I′为基元总数，
Figure FDA0002400160550000022
为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，
Figure FDA0002400160550000023
为基元i在声学基阵坐标系y轴下的坐标，
Figure FDA0002400160550000024
为基元i在声学基阵坐标系z轴下的坐标。
3.根据权利要求1或2所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤三中USBL测量的声信号在各基元和信标间的传播时延为
Figure FDA0002400160550000025
传播时延差为
Figure FDA0002400160550000026
Figure FDA0002400160550000027
下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为
Figure FDA0002400160550000028
具体过程为：
将
Figure FDA0002400160550000029
和
Figure FDA00024001605500000210
表示为：
Figure FDA00024001605500000211
式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声。
4.根据权利要求3所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤四中根据SINS计算水下航行器的姿态、速度和位置；
结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置；具体过程为：
建立水下航行器坐标系obxbybzb，水下航行器坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，水下航行器坐标系的定义满足右手定则；
所述航行器姿态包括航向角A、俯仰角K和横滚角ψ，根据航行器姿态复现的导航坐标系称计算导航坐标系n'。
声学基阵坐标系相对于水下航行器坐标系的角度安装偏差分别为α、β和γ，声学基阵坐标系原点相对于水下航行器坐标系原点的位置偏差
Figure FDA0002400160550000031
为：
Figure FDA0002400160550000032
其中，ΔXb、ΔYb和ΔZb为
Figure FDA0002400160550000033
中的分量，上角标T代表转置；
水下航行器坐标系到计算导航坐标系n'的转换矩阵
Figure FDA0002400160550000034
和声学基阵坐标系到水下航行器坐标系的转换矩阵
Figure FDA0002400160550000035
分别为：
Figure FDA0002400160550000036
Figure FDA0002400160550000037
则第i个基元在计算导航坐标系下的位置表示为
Figure FDA0002400160550000038
式中，
Figure FDA0002400160550000039
为第i个基元在声学基阵坐标系下的位置，
Figure FDA00024001605500000310
为SINS计算的水下航行器位置；
由于SINS的计算导航坐标系n'与导航坐标系n的三个坐标轴之间存在角度误差φ＝[φx φy φz]T，称失准角误差，计算导航坐标系n'与导航坐标系n之间的转换矩阵
Figure FDA00024001605500000311
近似为
Figure FDA00024001605500000312
其中
Figure FDA0002400160550000041
式中，φ＝[φx φy φz]T为失准角误差，I为三阶单位阵；φx、φy、φz为φ中的分量；
第i个基元在SINS计算导航坐标系下的位置
Figure FDA0002400160550000042
与在真实导航坐标系下位置值
Figure FDA0002400160550000043
之间的误差
Figure FDA0002400160550000044
为
Figure FDA0002400160550000045
式中，ΔXi是
Figure FDA0002400160550000046
与水下航行器在真实导航坐标系下位置
Figure FDA0002400160550000047
的差；记
Figure FDA0002400160550000048
式中，
Figure FDA0002400160550000049
是在计算导航坐标系中第i个基元相对水下航行器质心的位置。
5.根据权利要求4所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤五中结合声信号在航行器与信标之间传播的有效声速，根据SINS转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；具体过程为：
声信号在第i个基元与信标之间的传播时延预测值
Figure FDA00024001605500000410
为：
Figure FDA00024001605500000411
式中，c是测量的有效声速，
Figure FDA00024001605500000412
是信标在真实导航坐标系中的位置，||·||是矩阵二范数的表示；
Figure FDA00024001605500000413
其中，
Figure FDA00024001605500000414
为信标在真实导航坐标系的xn轴方向位置，
Figure FDA00024001605500000415
为信标在真实导航坐标系的yn轴方向位置，
Figure FDA00024001605500000416
为信标在真实导航坐标系的zn轴方向位置，
Figure FDA00024001605500000417
为第i个基元在计算导航坐标系的xn轴方向位置，
Figure FDA00024001605500000418
为第i个基元在计算导航坐标系的yn轴方向位置，
Figure FDA00024001605500000419
为第i个基元在计算导航坐标系的zn轴方向位置；
声信号的传播时延差为：
Figure FDA0002400160550000051
式中，
Figure FDA0002400160550000052
为SINS计算的第j个基元传播时延预测值，
Figure FDA0002400160550000053
为SINS计算的第1个基元传播时延预测值。
6.根据权利要求5所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤六中将SINS解算的航行器运动速度沿径向分解；具体过程为：
将SINS解算的航行器运动速度沿径向分解为：
Figure FDA0002400160550000054
其中：
Figure FDA0002400160550000055
为第i个基元相对于信标在径向方向的运动速度大小，
Figure FDA0002400160550000056
为第i个基元在计算导航坐标系xn′轴方向位置，
Figure FDA0002400160550000057
为第i个基元在计算导航坐标系yn′轴方向位置，
Figure FDA0002400160550000058
为第i个基元在计算导航坐标系zn′轴方向位置，
Figure FDA0002400160550000059
为水下航行器在计算导航坐标系的xn′轴方向速度，
Figure FDA00024001605500000510
为水下航行器在计算导航坐标系的yn′轴方向速度，
Figure FDA00024001605500000511
为水下航行器在计算导航坐标系的zn′轴方向速度；
Figure FDA00024001605500000512
式中，cosζx、cosζy、cosζz为中间变量。
7.根据权利要求6所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤七中建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程；具体过程为：
失准角误差φ＝[φx φy φz]T，SINS速度误差δν＝[δvx δvy δvz]T，SINS位置误差δp＝[δL δλ δh]T，陀螺漂移误差ε＝[εx εy εz]T，加速度计偏置误差
Figure FDA0002400160550000061
则
Figure FDA0002400160550000062
其中，X为扩展卡尔曼滤波系统的状态变量；
扩展卡尔曼滤波系统的状态方程为
Xk+1＝Fk+1/kXk+wk+1 (15)
其中，Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波系统过程噪声序列；
扩展卡尔曼滤波系统的观测量Z为：
Figure FDA0002400160550000063
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (17)
其中，Zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波系统观测噪声序列，Hk+1为k时刻的观测矩阵，
Figure FDA0002400160550000064
为声信号在第i个基元与信标之间的传播时延预测值；
观测矩阵Hk+1为
Figure FDA0002400160550000065
式中，Cipφ为中间变量，01×3为3维零向量，Cipp为中间变量，
Figure FDA0002400160550000066
为地球直角坐标系到SINS计算导航坐标系的转换矩阵，A为中间变量矩阵，01×6为6维零向量，Cjpφ为第j个基元对应的中间变量，j≠1，C1pφ为第1个基元对应的中间变量，Cjpp为第j个基元对应的中间变量，j≠1；C1pp为第1个基元对应的中间变量，Civv为中间变量，Cipv为中间变量。
8.根据权利要求7所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述观测矩阵Hk+1具体推算过程为：
SINS解算的水下航行器位置表示为纬度L、经度λ和高度h的球坐标形式，再将纬度L、经度λ和高度h转换到地球直角坐标系下；
地球直角坐标系以地心为原点，xe轴指向本初子午线与赤道交点，ye轴指向90°经线与赤道交点，ze轴与xe轴、ye轴构成右手坐标系，则
Figure FDA0002400160550000071
式中，x′e、y′e和z′e分别为SINS解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，RN为地球卯酉圈主曲率半径，
Figure FDA0002400160550000072
Re为地球半径，e为地球偏心率，
Figure FDA0002400160550000073
a和b分别是地球椭圆长轴和短轴半径；
式(18)的微分转换关系为：
Figure FDA0002400160550000074
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure FDA0002400160550000075
记中间变量矩阵A为：
Figure FDA0002400160550000076
结合式(7)和式(8)，得到式(9)的全微分形式：
Figure FDA0002400160550000077
式中，[dxn′ dyn′ dzn′]T是SINS计算的水下航行器位置与真实位置在三个轴方向的位置误差；
Figure FDA0002400160550000081
是
Figure FDA0002400160550000082
中的坐标分量；
Figure FDA0002400160550000083
为
Figure FDA0002400160550000084
的微分，c为有效声速，
Figure FDA0002400160550000085
是
Figure FDA0002400160550000086
沿计算导航坐标系xn′轴、yn′轴和zn′轴分量；
记
Figure FDA0002400160550000087
Figure FDA0002400160550000088
式中，Cipp、Cipφ为中间变量；
式(12)的全微分为
Figure FDA0002400160550000089
式中，
Figure FDA00024001605500000810
是SINS计算的水下航行器在导航坐标系下三个方向的速度误差；Civv、Cipv为中间变量；
Civv＝[cosζx cosζy cosζz] (26)
Figure FDA00024001605500000811
式中，
Figure FDA00024001605500000812
为水下航行器在计算导航坐标系下三个方向的速度；
观测矩阵Hk+1为
Figure FDA00024001605500000813
式中，Cipφ为中间变量，01×3为3维零向量，Cipp为中间变量，
Figure FDA00024001605500000814
为地球直角坐标系到SINS计算导航坐标系的转换矩阵，A为中间变量矩阵，01×6为6维零向量，Cjpφ为第j个基元对应的中间变量，j≠1，C1pφ为第1个基元对应的中间变量，Cjpp为第j个基元对应的中间变量，j≠1；C1pp为第1个基元对应的中间变量，Civv为中间变量，Cipv为中间变量。
9.根据权利要求8所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤八中计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束；具体过程为：
计算SINS的导航误差，具体过程为：
根据
Figure FDA0002400160550000091
计算k+1时刻的状态预测值
Figure FDA0002400160550000092
式中，
Figure FDA0002400160550000093
为k+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；
式中，I为三阶单位阵；
根据
Figure FDA0002400160550000094
计算k+1时刻的状态估计值
Figure FDA0002400160550000095
根据
Figure FDA0002400160550000096
修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。
CN202010144245.7A 2020-03-04 2020-03-04 一种引入径向速度的sins/usbl紧组合导航定位方法 Active CN111380518B (zh)

Priority Applications (1)

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

Applications Claiming Priority (1)

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

Publications (2)

Publication Number Publication Date
CN111380518A true CN111380518A (zh) 2020-07-07
CN111380518B CN111380518B (zh) 2021-10-29

Family

ID=71213534

Family Applications (1)

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

Country Status (1)

Country Link
CN (1) CN111380518B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111964684A (zh) * 2020-08-21 2020-11-20 运城学院 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN112083425A (zh) * 2020-09-14 2020-12-15 湖南航天机电设备与特种材料研究所 一种引入径向速度的sins/lbl紧组合导航方法
CN113075665A (zh) * 2021-03-24 2021-07-06 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质
CN113189676A (zh) * 2021-05-25 2021-07-30 哈尔滨工程大学 一种基于声学暗室的声学相位中心校准方法及系统
CN113933526A (zh) * 2021-10-14 2022-01-14 哈尔滨工程大学 一种体目标高精度三维运动速度测量方法
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紧耦合算法
WO2018145011A1 (en) * 2017-02-06 2018-08-09 Seabed Geosolutions B.V. Methods and systems for deployment of seismic autonomous underwater vehicles
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法
CN110207698A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种极区格网惯导/超短基线紧组合导航方法

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紧耦合算法
WO2018145011A1 (en) * 2017-02-06 2018-08-09 Seabed Geosolutions B.V. Methods and systems for deployment of seismic autonomous underwater vehicles
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法
CN110207698A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种极区格网惯导/超短基线紧组合导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LINGXIAO WANG，ET AL.: "AUV Navigation Based on Inertial Navigation and Acoustic Positioning Systems", 《OCEANS 2018 MTS》 *
徐博等: "基于倒置声学基阵的INSUSBL组合导航算法研究", 《海洋技术学报》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111964684A (zh) * 2020-08-21 2020-11-20 运城学院 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN111964684B (zh) * 2020-08-21 2023-11-17 运城学院 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN112083425A (zh) * 2020-09-14 2020-12-15 湖南航天机电设备与特种材料研究所 一种引入径向速度的sins/lbl紧组合导航方法
CN112083425B (zh) * 2020-09-14 2024-03-12 湖南航天机电设备与特种材料研究所 一种引入径向速度的sins/lbl紧组合导航方法
CN113075665A (zh) * 2021-03-24 2021-07-06 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质
CN113075665B (zh) * 2021-03-24 2023-06-20 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质
CN113189676A (zh) * 2021-05-25 2021-07-30 哈尔滨工程大学 一种基于声学暗室的声学相位中心校准方法及系统
CN113933526A (zh) * 2021-10-14 2022-01-14 哈尔滨工程大学 一种体目标高精度三维运动速度测量方法
CN116068540A (zh) * 2023-02-17 2023-05-05 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法
CN116068540B (zh) * 2023-02-17 2023-09-12 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法

Also Published As

Publication number Publication date
CN111380518B (zh) 2021-10-29

Similar Documents

Publication Publication Date Title
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN113311436B (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
CN109613583B (zh) 基于单星与地面站测向及联合测时差的无源目标定位方法
CN110514203B (zh) 一种基于isr-ukf的水下组合导航方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN104697520B (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN105547289A (zh) 一种水下航行器组合导航系统及导航信息融合方法
CN111380519B (zh) 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN111380520B (zh) 一种引入径向速度的sins/usbl松组合导航定位方法
CN115542363A (zh) 一种适用于垂直下视航空吊舱的姿态测量方法
CN115096302A (zh) 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端
CN115327587A (zh) 基于gnss定位信息的低轨卫星轨道误差修正方法及系统
CN113324539A (zh) 一种sins/srs/cns多源融合自主组合导航方法
KR101565259B1 (ko) 비정지 위성용 안테나의 구동 제어 방법 및 이를 위한 프로그램 기록매체
Gong et al. Deformation measuring methods based on inertial sensors for airborne distributed POS
CN111380517B (zh) 一种基于usbl软件接收机的sins/usbl深组合导航定位方法
RU2428659C2 (ru) Способ спутниковой коррекции гироскопических навигационных систем морских объектов
CN111473786A (zh) 一种基于局部反馈的两层分布式多传感器组合导航滤波方法
Li et al. A nonlinear two-filter smoothing estimation method based on DD2 filter for land vehicle 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