CN111380517A - 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 - Google Patents

一种基于usbl软件接收机的sins/usbl深组合导航定位方法 Download PDF

Info

Publication number
CN111380517A
CN111380517A CN202010144235.3A CN202010144235A CN111380517A CN 111380517 A CN111380517 A CN 111380517A CN 202010144235 A CN202010144235 A CN 202010144235A CN 111380517 A CN111380517 A CN 111380517A
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.)
Granted
Application number
CN202010144235.3A
Other languages
English (en)
Other versions
CN111380517B (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 CN202010144235.3A priority Critical patent/CN111380517B/zh
Publication of CN111380517A publication Critical patent/CN111380517A/zh
Application granted granted Critical
Publication of CN111380517B publication Critical patent/CN111380517B/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)

Abstract

一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，它属于组合导航及水声定位技术领域。本发明解决了当USBL测量信息中断或者发生错误时，传统组合方式的容错能力有限的问题。在不需要先验信息的情况下，本发明利用SINS的动态信息辅助无源工作模式下的USBL载波跟踪环路和码跟踪环路，获得准确径向速度信息和时延信息，提高了USBL在干扰信号或者是弱信号下的系统容错能力，提高了组合系统的性能。本发明可以应用于组合导航及水声定位领域。

Description

一种基于USBL软件接收机的SINS/USBL深组合导航定位方法
技术领域
本发明属于组合导航及水声定位技术领域，具体涉及一种基于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码跟踪环路原理图；
图中，δτ是接收信号码相位对应的时延测量τ与本地参考信号码相位对应时延参考
Figure BDA0002400158700000021
的时延差；
图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的有效作用范围。
具体实施方式二：如图2所示。本实施方式与具体实施方式一不同的是：所述步骤二中，各基元在声学基阵坐标系下的位置
Figure BDA0002400158700000041
为：
Figure BDA0002400158700000042
其中：i为基元序号，i＝1,2,3,4，r为基元1与3的间距或基元2与4的间距(基元1与3的间距同基元2与4的间距相等)；
Figure BDA0002400158700000043
为各基元在声学基阵坐标系的xa轴方向上的位置，
Figure BDA0002400158700000044
为各基元在声学基阵坐标系的ya轴方向上的位置，
Figure BDA0002400158700000045
为各基元在声学基阵坐标系的za轴方向上的位置，上角标T代表转置。
具体实施方式三：本实施方式与具体实施方式二不同的是：所述步骤三的具体过程为：
水下航行器在SINS计算的导航坐标系下的姿态信息包括航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
声学基阵坐标系与载体坐标系三个坐标轴的角度安装偏差分别为α、β和γ，声学基阵坐标系原点相对于载体坐标系原点的位置偏差
Figure BDA0002400158700000051
为：
Figure BDA0002400158700000052
(ΔXb、ΔYb和ΔZb均为载体坐标系下分量)，ΔXb、ΔYb和ΔZb为
Figure BDA0002400158700000053
中的分量；
载体坐标系到计算导航坐标系的转换矩阵
Figure BDA0002400158700000054
和声学基阵坐标系到载体坐标系的转换矩阵
Figure BDA0002400158700000055
分别为：
Figure BDA0002400158700000056
Figure BDA0002400158700000057
则第i个基元在SINS计算的导航坐标系n′下的位置
Figure BDA0002400158700000058
表示为
Figure BDA0002400158700000059
其中，
Figure BDA00024001587000000510
是SINS计算的水下航行器的位置在计算导航坐标系中的直角坐标形式，
Figure BDA00024001587000000511
为载体坐标系到SINS计算的导航坐标系n'的转换矩阵；
SINS计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在失准角误差φ＝[φx φy φz]T，SINS计算的导航坐标系n'到真实导航坐标系n的转换矩阵
Figure BDA00024001587000000512
可近似为：
Figure BDA00024001587000000513
其中，I为单位矩阵；
Figure BDA00024001587000000514
第i个基元在SINS计算的导航坐标系下的位置
Figure BDA00024001587000000515
与在真实导航坐标系(建立的导航坐标系)下位置
Figure BDA0002400158700000061
之间的误差
Figure BDA0002400158700000062
为
Figure BDA0002400158700000063
式中，ΔXi是
Figure BDA0002400158700000064
与水下航行器在真实导航坐标系下位置
Figure BDA0002400158700000065
的差；
记
Figure BDA0002400158700000066
Figure BDA0002400158700000067
是在计算导航坐标系中第i个基元相对水下航行器质心的位置，
Figure BDA0002400158700000068
是
Figure BDA0002400158700000069
沿计算导航坐标系xn′轴、yn′轴和zn′轴分量。
具体实施方式四：本实施方式与具体实施方式三不同的是：所述步骤四的具体过程为：
声信号在第i个基元与信标之间的传播时延预测值
Figure BDA00024001587000000610
为：
Figure BDA00024001587000000611
式中，c是测量的有效声速，
Figure BDA00024001587000000612
是信标在真实导航坐标系中的位置，由GPS提供的位置信息转换得到，||·||是矩阵二范数的表示；
Figure BDA00024001587000000613
其中，
Figure BDA00024001587000000614
为信标在真实导航坐标系的xn轴方向位置，
Figure BDA00024001587000000615
为信标在真实导航坐标系的yn轴方向位置，
Figure BDA00024001587000000616
为信标在真实导航坐标系的zn轴方向位置，
Figure BDA00024001587000000617
为第i个基元在真实导航坐标系的xn轴方向位置，
Figure BDA00024001587000000618
为第i个基元在真实导航坐标系的yn轴方向位置，
Figure BDA00024001587000000619
为第i个基元在真实导航坐标系的zn轴方向位置；
声信号在基元3和基元1之间传播时延差的预测值为
Figure BDA00024001587000000620
声信号在基元4和基元2之间传播时延差的预测值为
Figure BDA00024001587000000621
Figure BDA00024001587000000622
所述有效声速为信标和接收基阵的几何距离与声信号在两点间的传播时间的比值。
具体实施方式五：本实施方式与具体实施方式四不同的是：所述步骤五中，各个基元相对于信标在径向方向的运动速度大小为：
Figure BDA0002400158700000071
其中：
Figure BDA0002400158700000072
为第i个基元相对于信标在径向方向的运动速度大小，
Figure BDA0002400158700000073
为第i个基元在计算导航坐标系的xn′轴方向位置，
Figure BDA0002400158700000074
为第i个基元在计算导航坐标系的yn′轴方向位置，
Figure BDA0002400158700000075
为第i个基元在计算导航坐标系的zn′轴方向位置，
Figure BDA0002400158700000076
为水下航行器在计算导航坐标系的xn′轴方向速度，
Figure BDA0002400158700000077
为水下航行器在计算导航坐标系的yn′轴方向速度，
Figure BDA0002400158700000078
为水下航行器在计算导航坐标系的zn′轴方向速度；
其中，记中间变量cosζx、cosζy和cosζz为：
Figure BDA0002400158700000079
具体实施方式六：结合图2说明本实施方式。本实施方式与具体实施方式五不同的是：所述步骤六的具体过程为：
SINS对应USBL软件接收机第i个基元接收通道的载波跟踪环路动态辅助信息为：
Figure BDA00024001587000000710
式中，faid为SINS辅助频率，c为有效声速，f0为信标广播信号中心频率；
结合图2，SINS辅助下的USBL软件接收机第i个基元接收通道的载波环跟踪相位误差
Figure BDA00024001587000000711
的导数和频率误差δf的导数表示为：
Figure BDA00024001587000000712
Figure BDA00024001587000000713
其中，
Figure BDA0002400158700000081
是接收信号相位
Figure BDA0002400158700000082
与本地参考信号相位
Figure BDA0002400158700000083
的相位误差，
Figure BDA0002400158700000084
为相位误差
Figure BDA0002400158700000085
的导数，
Figure BDA0002400158700000086
为低通滤波器输出的频率误差δf的导数，p和q是与具体跟踪环路有关的参数；δfp为δf与SINS辅助频率faid误差δfaid之和，
Figure BDA0002400158700000087
为δfp的导数，
Figure BDA0002400158700000088
为δfaid的导数；在SINS动态辅助USBL接收机时只需考虑一个基元接收通道的载波环跟踪过程。
结合图3，SINS对应的USBL软件接收机第i个基元接收通道的码跟踪环路动态辅助信息为
Figure BDA0002400158700000089
Figure BDA00024001587000000810
其中，
Figure BDA00024001587000000811
为SINS的径向速度辅助信息，
Figure BDA00024001587000000812
为SINS的时延辅助信息；
结合图3，SINS辅助下的USBL软件接收机第i个基元接收通道的码跟踪环路时延误差
Figure BDA00024001587000000813
的导数表示为：
Figure BDA00024001587000000814
式中，
Figure BDA00024001587000000815
为
Figure BDA00024001587000000816
的导数，
Figure BDA00024001587000000817
为SINS的径向速度辅助误差，
Figure BDA00024001587000000818
为
Figure BDA00024001587000000819
的导数，Qi是环路热噪声；
根据δf、
Figure BDA00024001587000000820
faid和δτ，得到USBL软件接收机第i个基元接收通道的声信号在对应基元和信标间的传播时延测量值为
Figure BDA00024001587000000821
传播时延差测量值为
Figure BDA00024001587000000822
和
Figure BDA00024001587000000823
径向速度测量值为
Figure BDA00024001587000000824
具体实施方式七：本实施方式与具体实施方式六不同的是：所述步骤七的具体过程为：
失准角误差φ为：φ＝[φx φy φz]T，SINS速度误差δν为：δν＝[δvx δvy δvz]T(速度误差是由SINS解算的计算导航坐标系下的水下航行器运动速度与真实导航坐标系下的速度真值的差)，SINS位置误差δp为：δp＝[δL δλ δh]T(位置误差是SINS解算的水下航行器在地球坐标系下的位置与真实位置的差值)，陀螺漂移误差ε为：ε＝[εx εy εz]T，加速度计偏置误差
Figure BDA00024001587000000825
为：
Figure BDA00024001587000000826
其中，φx、φy和φz为φ中的分量；
则
Figure BDA0002400158700000091
其中，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为：
Figure BDA0002400158700000092
观测方程为
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轴构成右手坐标系，则
Figure BDA0002400158700000093
式中，x′e、y′e和z′e分别为SINS解算的水下航行器在地球直角坐标系的xe轴、ye轴和ze轴方向的位置，RN为地球卯酉圈主曲率半径，
Figure BDA0002400158700000094
Re为地球半径，e为地球偏心率，
Figure BDA0002400158700000095
a和b分别是椭圆长轴和短轴半径；
上式的微分转换关系为：
Figure BDA0002400158700000101
地球直角坐标系到SINS计算导航坐标系的转换矩阵为：
Figure BDA0002400158700000102
记中间变量矩阵A为：
Figure BDA0002400158700000103
结合式(7)和式(8)，得到式(9)的全微分形式：
Figure BDA0002400158700000104
式中，[dxn′ dyn′ dzn′]T是SINS计算的水下航行器位置与真实位置在三个轴方向的位置误差；
Figure BDA0002400158700000105
是
Figure BDA0002400158700000106
中的坐标分量。
记
Figure BDA0002400158700000107
Figure BDA0002400158700000108
式(12)的全微分为
Figure BDA0002400158700000109
式中，
Figure BDA00024001587000001010
是SINS计算的水下航行器在导航坐标系下三个方向的速度误差。
Civv＝[cosζx cosζy cosζz] (32)
Figure BDA0002400158700000111
观测矩阵Hk+1为
Figure BDA0002400158700000112
式(33)中，C1tt＝1 0 0 0，C31tt＝-1 0 1 0。
具体实施方式八：本实施方式与具体实施方式七不同的是：所述步骤八中，根据扩展卡尔曼滤波系统输出的状态估计值修正SINS输出，并重置扩展卡尔曼滤波系统输出的状态，其具体过程为：
根据
Figure BDA0002400158700000113
计算k+1时刻的状态预测值
Figure BDA0002400158700000114
其中
Figure BDA0002400158700000115
为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 BDA0002400158700000121
计算k+1时刻的状态估计值
Figure BDA0002400158700000122
根据
Figure BDA0002400158700000123
修正SINS的输出，并重置扩展卡尔曼滤波系统输出的状态。
所述扩展卡尔滤波系统是指在最小均方误差准则下，将非线性的状态方程和观测方程进行线性近似处理，对状态变量进行估计。
所述重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (8)

1.一种基于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的输出进行修正。
2.根据权利要求1所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤二中，各基元在声学基阵坐标系下的位置
Figure FDA0002400158690000021
为：
Figure FDA0002400158690000022
其中：i为基元序号，i＝1,2,3,4，r为基元1与3的间距或基元2与4的间距；
Figure FDA0002400158690000023
为各基元在声学基阵坐标系的xa轴方向上的位置，
Figure FDA0002400158690000024
为各基元在声学基阵坐标系的ya轴方向上的位置，
Figure FDA0002400158690000025
为各基元在声学基阵坐标系的za轴方向上的位置，上角标T代表转置。
3.根据权利要求2所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤三的具体过程为：
水下航行器在SINS计算的导航坐标系下的姿态信息包括航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
声学基阵坐标系与载体坐标系三个坐标轴的角度安装偏差分别为α、β和γ，声学基阵坐标系原点相对于载体坐标系原点的位置偏差
Figure FDA0002400158690000026
为：
Figure FDA0002400158690000027
ΔXb、ΔYb和ΔZb为
Figure FDA0002400158690000028
中的分量；
载体坐标系到计算导航坐标系的转换矩阵
Figure FDA0002400158690000029
和声学基阵坐标系到载体坐标系的转换矩阵
Figure FDA00024001586900000210
分别为：
Figure FDA00024001586900000211
Figure FDA0002400158690000031
则第i个基元在SINS计算的导航坐标系n′下的位置
Figure FDA0002400158690000032
表示为
Figure FDA0002400158690000033
其中，
Figure FDA0002400158690000034
是SINS计算的水下航行器的位置在计算导航坐标系中的直角坐标形式，
Figure FDA0002400158690000035
为载体坐标系到SINS计算的导航坐标系n'的转换矩阵；
SINS计算的导航坐标系n'与真实导航坐标系n的三个坐标轴存在失准角误差φ＝[φxφy φz]T，SINS计算的导航坐标系n'到真实导航坐标系n的转换矩阵
Figure FDA0002400158690000036
为：
Figure FDA0002400158690000037
其中，I为单位矩阵；
Figure FDA0002400158690000038
第i个基元在SINS计算的导航坐标系下的位置
Figure FDA0002400158690000039
与在真实导航坐标系下位置
Figure FDA00024001586900000310
之间的误差
Figure FDA00024001586900000311
为
Figure FDA00024001586900000312
式中，ΔXi是
Figure FDA00024001586900000313
与水下航行器在真实导航坐标系下位置
Figure FDA00024001586900000314
的差；
记
Figure FDA00024001586900000315
Figure FDA00024001586900000316
是在计算导航坐标系中第i个基元相对水下航行器质心的位置，
Figure FDA00024001586900000317
是
Figure FDA00024001586900000318
沿计算导航坐标系xn′轴、yn′轴和zn′轴分量。
4.根据权利要求3所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤四的具体过程为：
声信号在第i个基元与信标之间的传播时延预测值
Figure FDA00024001586900000319
为：
Figure FDA0002400158690000041
式中，c是测量的有效声速，
Figure FDA0002400158690000042
是信标在真实导航坐标系中的位置，||·||是矩阵二范数的表示；
Figure FDA0002400158690000043
其中，
Figure FDA0002400158690000044
为信标在真实导航坐标系的xn轴方向位置，
Figure FDA0002400158690000045
为信标在真实导航坐标系的yn轴方向位置，
Figure FDA0002400158690000046
为信标在真实导航坐标系的zn轴方向位置，
Figure FDA0002400158690000047
为第i个基元在真实导航坐标系的xn轴方向位置，
Figure FDA0002400158690000048
为第i个基元在真实导航坐标系的yn轴方向位置，
Figure FDA0002400158690000049
为第i个基元在真实导航坐标系的zn轴方向位置；
声信号在基元3和基元1之间传播时延差的预测值为
Figure FDA00024001586900000410
声信号在基元4和基元2之间传播时延差的预测值为
Figure FDA00024001586900000411
Figure FDA00024001586900000412
5.根据权利要求4所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤五中，各个基元相对于信标在径向方向的运动速度大小为：
Figure FDA00024001586900000413
其中：
Figure FDA00024001586900000414
为第i个基元相对于信标在径向方向的运动速度大小，
Figure FDA00024001586900000415
为第i个基元在计算导航坐标系的xn′轴方向位置，
Figure FDA00024001586900000416
为第i个基元在计算导航坐标系的yn′轴方向位置，
Figure FDA00024001586900000417
为第i个基元在计算导航坐标系的zn′轴方向位置，
Figure FDA00024001586900000418
为水下航行器在计算导航坐标系的xn′轴方向速度，
Figure FDA00024001586900000419
为水下航行器在计算导航坐标系的yn′轴方向速度，
Figure FDA00024001586900000420
为水下航行器在计算导航坐标系的zn′轴方向速度；
其中，记中间变量cosζx、cosζy和cosζz为：
Figure FDA0002400158690000051
6.根据权利要求5所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤六的具体过程为：
SINS对应USBL软件接收机第i个基元接收通道的载波跟踪环路动态辅助信息为：
Figure FDA0002400158690000052
式中，faid为SINS辅助频率，c为有效声速，f0为信标广播信号中心频率；
SINS辅助下的USBL软件接收机第i个基元接收通道的载波环跟踪相位误差
Figure FDA00024001586900000514
的导数和频率误差δf的导数表示为：
Figure FDA0002400158690000053
Figure FDA0002400158690000054
其中，
Figure FDA00024001586900000515
是接收信号相位
Figure FDA00024001586900000516
与本地参考信号相位
Figure FDA00024001586900000517
的相位误差，
Figure FDA0002400158690000055
为相位误差
Figure FDA00024001586900000518
的导数，
Figure FDA0002400158690000056
为低通滤波器输出的频率误差δf的导数，p和q是与具体跟踪环路有关的参数；δfp为δf与SINS辅助频率faid误差δfaid之和，
Figure FDA0002400158690000057
为δfp的导数，
Figure FDA0002400158690000058
为δfaid的导数；
SINS对应的USBL软件接收机第i个基元接收通道的码跟踪环路动态辅助信息为
Figure FDA0002400158690000059
Figure FDA00024001586900000510
其中，
Figure FDA00024001586900000511
为SINS的径向速度辅助信息，
Figure FDA00024001586900000512
为SINS的时延辅助信息；
SINS辅助下的USBL软件接收机第i个基元接收通道的码跟踪环路时延误差
Figure FDA00024001586900000513
的导数表示为：
Figure FDA0002400158690000061
式中，
Figure FDA0002400158690000062
为
Figure FDA0002400158690000063
的导数，
Figure FDA0002400158690000064
为SINS的径向速度辅助误差，
Figure FDA0002400158690000065
为
Figure FDA0002400158690000066
的导数，Qi是环路热噪声；
根据δf、
Figure FDA0002400158690000067
faid和δτ，得到USBL软件接收机第i个基元接收通道的声信号在对应基元和信标间的传播时延测量值为
Figure FDA0002400158690000068
传播时延差测量值为
Figure FDA0002400158690000069
和
Figure FDA00024001586900000610
径向速度测量值为
Figure FDA00024001586900000611
7.根据权利要求6所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤七的具体过程为：
失准角误差φ为：φ＝[φx φy φz]T，SINS速度误差δν为：δν＝[δvx δvy δvz]T，SINS位置误差δp为：δp＝[δL δλ δh]T，陀螺漂移误差ε为：ε＝[εx εy εz]T，加速度计偏置误差
Figure FDA00024001586900000612
为：
Figure FDA00024001586900000613
其中，φx、φy和φz为φ中的分量；
则
Figure FDA00024001586900000614
其中，X为扩展卡尔曼滤波系统的状态变量；
扩展卡尔曼滤波系统的状态方程为
Xk+1＝Fk+1/kXk+wk+1 (21)
其中，Xk为k时刻的状态变量，Xk+1为k+1时刻的状态变量，Fk+1/k为状态转移矩阵，wk+1为扩展卡尔曼滤波系统过程噪声序列；
扩展卡尔曼滤波系统的观测量Z为：
Figure FDA00024001586900000615
观测方程为
Zk+1＝Hk+1Xk+1+vk+1 (23)
其中，Zk+1为k+1时刻的观测量，vk+1为扩展卡尔曼滤波系统观测噪声序列，Hk+1为k+1时刻的观测矩阵。
8.根据权利要求7所述的一种基于USBL软件接收机的SINS/USBL深组合导航定位方法，其特征在于，所述步骤八中，根据扩展卡尔曼滤波系统输出的状态估计值修正SINS输出，并重置扩展卡尔曼滤波系统输出的状态，其具体过程为：
根据
Figure FDA0002400158690000071
计算k+1时刻的状态预测值
Figure FDA0002400158690000072
其中
Figure FDA0002400158690000073
为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 FDA0002400158690000074
计算k+1时刻的状态估计值
Figure FDA0002400158690000075
根据
Figure FDA0002400158690000076
修正SINS的输出，并重置扩展卡尔曼滤波系统输出的状态。
CN202010144235.3A 2020-03-04 2020-03-04 一种基于usbl软件接收机的sins/usbl深组合导航定位方法 Active CN111380517B (zh)

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 true CN111380517A (zh) 2020-07-07
CN111380517B 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)

* Cited by examiner, † Cited by third party
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 哈尔滨工程大学 一种基于询问应答模式的水下高速目标高精度自主声学导航方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
Y.Y.ZHU: "Technology of SINS/USBL highly tightly coupled integrated navigation based on phase difference measurements", 《2019 DGON INERTIAL SENSORS AND SYSTEMS》 *
郑翠娥等: "一种基于超短基线的高精度多目标水声定位技术研究 ", 《海军工程大学学报》 *
郑翠娥等: "超短基线定位系统安装误差校准技术研究 ", 《计算机工程与应用》 *

Also Published As

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

Similar Documents

Publication Publication Date Title
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN105891863B (zh) 一种基于高度约束的扩展卡尔曼滤波定位方法
CN109613583B (zh) 基于单星与地面站测向及联合测时差的无源目标定位方法
CN106767793A (zh) 一种基于sins/usbl紧组合的auv水下导航定位方法
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN101788296A (zh) 一种sins/cns深组合导航系统及其实现方法
CN115200574B (zh) 一种地球椭球模型下的极区横向组合导航方法
CN101246012B (zh) 一种基于鲁棒耗散滤波的组合导航方法
CN111380519B (zh) 一种超短基线/捷联惯性导航松组合的导航误差校正方法
CN105547289A (zh) 一种水下航行器组合导航系统及导航信息融合方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN106767928A (zh) 一种自适应快速传递对准方法
CN103471614A (zh) 一种基于逆坐标系的极区传递对准方法
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN107894240A (zh) 一种用于水下无人航行器在极区导航的初始粗对准方法
CN110207698A (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN111380520B (zh) 一种引入径向速度的sins/usbl松组合导航定位方法
CN111380517B (zh) 一种基于usbl软件接收机的sins/usbl深组合导航定位方法
CN103616026A (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN109724627A (zh) 一种基于多普勒计程仪和星敏感器辅助的极区传递对准方法

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