CN112083425A - 一种引入径向速度的sins/lbl紧组合导航方法 - Google Patents

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

Info

Publication number
CN112083425A
CN112083425A CN202010961099.7A CN202010961099A CN112083425A CN 112083425 A CN112083425 A CN 112083425A CN 202010961099 A CN202010961099 A CN 202010961099A CN 112083425 A CN112083425 A CN 112083425A
Authority
CN
China
Prior art keywords
sins
underwater vehicle
transponder
calculated
earth
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
CN202010961099.7A
Other languages
English (en)
Other versions
CN112083425B (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.)
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Original Assignee
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
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 Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials filed Critical Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority to CN202010961099.7A priority Critical patent/CN112083425B/zh
Publication of CN112083425A publication Critical patent/CN112083425A/zh
Application granted granted Critical
Publication of CN112083425B publication Critical patent/CN112083425B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • 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
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/87—Combinations of sonar systems
    • G01S15/872—Combination of several systems for attitude determination

Abstract

本发明公开了一种引入径向速度的SINS/LBL紧组合导航方法，在LBL水声定位系统的应答器阵列中，每个应答器的应答信号频率不同，应答信号的理论频率和实际接收频率之差为多普勒频率，它反映了水下航行器与应答器在径向方向的相对运动关系，即径向速度；在传统的SINS/LBL紧组合中引入SINS和LBL计算的径向速度作为扩展卡尔曼滤波器的观测量，与速度相关的直接观测量的引入增加了速度误差的可观测性，提高了速度误差的估计精度和组合导航精度。

Description

一种引入径向速度的SINS/LBL紧组合导航方法
技术领域
本发明属于组合导航及水声定位技术领域，尤其涉及一种引入径向速度的SINS/LBL紧组合导航方法。
背景技术
LBL(Long Base Line，长基线LBL)水声定位系统是由安装在海底的基线长度为几千米的应答器阵列和安装在载体上的问答机组成，其定位原理是利用载体上的问答机与海底应答器阵列之间的距离信息来求解水下航行器位置。LBL因其作用范围广、定位精度高已广泛应用于水下潜器。
LBL的应答器或信标的绝对地理位置在定位解算前已精确标定，传统SINS/LBL紧组合的滤波观测量是SINS(Strapdown Inertial Navigation Systems，捷联式惯性导航系统SINS)计算的水下航行器与应答器之间的距离和LBL测量的声信号在水下航行器与应答器的传播距离之差(斜距之差)，或是增加SINS和LBL计算的水下航行器与不同应答器之间的距离差之差为观测量(斜距差之差)，来估计SINS的导航误差，修正SINS输出的导航结果，提高导航精度。由于LBL定位至少需要3个应答器，当水下航行器运动至应答器阵列有效作用范围之外时，LBL不能计算水下航行器的位置，SINS/LBL松组合导航无法更新计算，紧组合能够解决SINS/LBL松组合中因LBL定位无效而降低组合导航精度的问题。而在传统的紧组合中，位置误差和速度误差分别是观测量(斜距和斜距差)的一阶导数和二阶导数，速度误差的可观测性较弱，估计精度低。
发明内容
本发明的目的在于提供一种引入径向速度的SINS/LBL紧组合导航方法，以解决传统SINS/LBL紧组合导航中速度误差可观测性弱导致估计精度低的问题。
本发明独立权利要求的技术方案解决了上述发明目的中的一个或多个。
本发明是通过如下的技术方案来解决上述技术问题的：一种引入径向速度的SINS/LBL紧组合导航方法，包括：
步骤1：在水下航行器上安装SINS，在海底布放LBL水声定位系统的应答器阵列，并精确标定每个应答器的位置；
步骤2：在LBL水声定位系统中，根据声信号在水下航行器与应答器之间的传播时延，计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差；
根据实际接收到的应答信号以及设置的参考信号，计算出水下航行器与应答器在径向方向的相对运动速度；
步骤3：在SINS中，计算出水下航行器在导航坐标系下的姿态和速度以及水下航行器在地球坐标系下的位置，并将水下航行器在地球坐标系下的位置转换成地球直角坐标；
根据所述步骤1中已标定的应答器的位置计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差；
步骤4：根据所述步骤3中SINS计算的水下航行器在导航坐标系下的速度计算出水下航行器与应答器在径向方向的相对运动速度；
步骤5：由SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差构成扩展卡尔曼滤波器的状态变量；根据所述状态变量建立状态方程；
由所述步骤2中LBL水声定位系统计算出的传播距离、传播距离之差和相对运动速度，步骤3中SINS计算出的传播距离和传播距离之差以及步骤4中计算出的相对运动速度构成扩展卡尔曼滤波器的观测量；
根据所述状态变量和观测量建立观测方程；
步骤6：根据所述步骤5中的状态方程和观测方程估计出SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差；
步骤7：根据所述步骤6中估计出的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差对所述SINS输出的姿态、速度、位置、陀螺漂移和加速度计偏置进行修正，得到修正后的姿态、速度、位置、陀螺漂移和加速度计偏置；
步骤8：重置所述扩展卡尔曼滤波器的状态变量，循环执行步骤2～7，直到水下航行器作业结束。
本发明所述导航方法，在LBL水声定位系统的应答器阵列中，每个应答器的应答信号频率不同，应答信号的理论频率和实际接收频率之差为多普勒频率，它反映了水下航行器与应答器在径向方向的相对运动关系，即径向速度；在传统的SINS/LBL紧组合中引入SINS和LBL计算的径向速度(即LBL计算的水下航行器与应答器在径向方向的相对运动速度以及SINS计算的水下航行器与应答器在径向方向的相对运动速度)作为扩展卡尔曼滤波器的观测量，与速度相关的直接观测量的引入增加了速度误差的可观测性，提高了速度误差的估计精度和组合导航精度。
进一步地，所述步骤1中，应答器阵列的组数大于或等于1组，每组所述应答器阵列的应答器数量大于或等于3个。
进一步地，所述步骤2中，水下航行器与第i个应答器之间的传播距离的计算表达式为：
Figure BDA0002680574100000031
声信号到达第i个应答器和第j个应答器的传播距离之差的计算表达式为：
Figure BDA0002680574100000032
其中，
Figure BDA0002680574100000033
为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，c为水中声速，τi为声信号在水下航行器与第i个应答器之间的单程传播时延，τj为声信号在水下航行器与第j个应答器之间的单程传播时延，
Figure BDA0002680574100000034
为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，i≠j；
水下航行器与第i个应答器在径向方向的相对运动速度的计算表达式为：
Figure BDA0002680574100000035
其中，
Figure BDA0002680574100000036
为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，fc为参考信号频率，fd为参考信号频率fc与实际接收到的应答信号频率之差。
进一步地，所述步骤3中，水下航行器的地球直角坐标为：
Figure BDA0002680574100000037
其中，Xs＝[xs ys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，
Figure BDA0002680574100000038
RN为地球子午圈半径，Re为地球半径，e为地球偏心率，
Figure BDA0002680574100000039
a和b分别为地球椭圆长轴和短轴半径，h为SINS计算的水下航行器在地球坐标系下的高度，L为SINS计算的水下航行器在地球坐标系下的纬度，λ为SINS计算的水下航行器在地球坐标系下的经度，SINS计算的水下航行器在地球坐标系下的位置包括高度、纬度和经度；
水下航行器与第i个应答器之间的传播距离的计算表达式为：
Figure BDA0002680574100000041
声信号到达第i个应答器和第j个应答器的传播距离之差的计算表达式为
Figure BDA0002680574100000042
其中，
Figure BDA0002680574100000043
为SINS计算的水下航行器与第i个应答器之间的传播距离，Xi＝[xi yizi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，
Figure BDA0002680574100000044
为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure BDA0002680574100000045
为SINS计算的水下航行器与第j个应答器之间的传播距离，i≠j。
进一步地，所述步骤4中，水下航行器与应答器在径向方向的相对运动速度的计算表达式为：
Figure BDA0002680574100000046
其中，
Figure BDA0002680574100000047
为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度，Xs＝[xs ys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，Xi＝[xi yi zi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，vE为SINS计算的水下航行器在导航坐标系下的东向速度，vN为SINS计算的水下航行器在导航坐标系下的北向速度，vU为SINS计算的水下航行器在导航坐标系下的天向速度，SINS计算的水下航行器在导航坐标系下的速度包括东向速度、北向速度和天向速度。
进一步地，所述步骤5中，状态方程为：
Xk+1＝Fk+1/kXk+wk+1
其中，Xk、Xk+1分别为k时刻、k+1时刻的状态变量，Fk+1/k为状态一步转移矩阵，wk+1为系统过程噪声；状态变量XT＝[φT (δν)T (δp)T εT ▽T]，φ为SINS的姿态误差，φ＝[φEφN φU]T，φE、φN和φU分别为SINS的俯仰角误差、横滚角误差和航向角误差，δν为SINS的速度误差，δν＝[δvE δvN δvU]T，δvE、δvN和δvU分别为SINS的东向速度误差、北向速度误差和天向速度误差，δp为SINS的位置误差，δp＝[δL δλ δh]T，δL、δλ和δh分别为SINS的纬度误差、经度误差和高度误差，ε为SINS的陀螺漂移误差，ε＝[εx εy εz]T，εx、εy和εz分别为SINS的陀螺在载体坐标系x、y、z轴的漂移误差，▽为SINS的加速度计偏置误差，▽＝[▽x ▽y▽z]T，▽x、▽y和▽z分别为SINS的加速度计在载体坐标系x、y、z轴的偏置误差。
进一步地，所述步骤5中，观测方程为：
Zk+1＝Hk+1Xk+1+vk+1
其中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，Xk+1为k+1时刻的状态变量，vk+1为系统观测噪声，观测量
Figure BDA0002680574100000051
Figure BDA0002680574100000052
为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，
Figure BDA0002680574100000053
为SINS计算的水下航行器与第i个应答器之间的传播距离，
Figure BDA0002680574100000054
为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure BDA0002680574100000055
为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure BDA0002680574100000056
为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，
Figure BDA0002680574100000057
为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度；
k+1时刻的观测矩阵
Figure BDA0002680574100000058
Figure BDA0002680574100000059
Figure BDA0002680574100000061
Figure BDA0002680574100000062
Figure BDA0002680574100000063
其中，
Figure BDA0002680574100000064
RN为地球子午圈半径，Re为地球半径，e为地球偏心率，
Figure BDA0002680574100000065
a和b分别为地球椭圆长轴和短轴半径，h为SINS计算的水下航行器在地球坐标系下的高度，L为SINS计算的水下航行器在地球坐标系下的纬度，λ为SINS计算的水下航行器在地球坐标系下的经度，0i×3为i×3维零向量，Xs＝[xs ys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，Xi＝[xi yi zi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，vE为SINS计算的水下航行器在导航坐标系下的东向速度，vN为SINS计算的水下航行器在导航坐标系下的北向速度，vU为SINS计算的水下航行器在导航坐标系下的天向速度。
进一步地，所述步骤7中，修正后的姿态、速度、位置、陀螺漂移和加速度计偏置分别为：qk+1＝Δq·qk，
Figure BDA0002680574100000071
Figure BDA0002680574100000072
其中，qk、qk+1分别为k时刻、k+1时刻的姿态四元数，Δq为k时刻到k+1时刻的姿态变化四元数，Δq由
Figure BDA0002680574100000073
计算得到，
Figure BDA0002680574100000074
为k+1时刻状态变量估计值的第1到3个，v'为修正后的速度向量，v为SINS计算的水下航行器在导航坐标系下的速度向量，
Figure BDA0002680574100000075
为k+1时刻状态变量估计值的第4到6个，p'为修正后的位置向量，p为SINS计算的水下航行器在地球坐标系下的位置向量，
Figure BDA0002680574100000076
为k+1时刻状态变量估计值的第7到9个，ε'为修正后的陀螺漂移向量，ε为修正前的陀螺漂移向量，
Figure BDA0002680574100000077
为k+1时刻状态变量估计值的第10到12个，▽'为修正后的加速度计偏置向量，▽为修正前的加速度计偏置向量，
Figure BDA0002680574100000078
为k+1时刻状态变量估计值的第13到15个。
有益效果
与现有技术相比，本发明所提供的一种引入径向速度的SINS/LBL紧组合导航方法，在LBL水声定位系统的应答器阵列中，每个应答器的应答信号频率不同，应答信号的理论频率和实际接收频率之差为多普勒频率，它反映了水下航行器与应答器在径向方向的相对运动关系，即径向速度；在传统的SINS/LBL紧组合中引入SINS和LBL计算的径向速度作为扩展卡尔曼滤波器的观测量，与速度相关的直接观测量的引入增加了速度误差的可观测性，提高了速度误差的估计精度和组合导航精度。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一个实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例中水下航行器在4个应答器下的SINS/LBL紧组合导航工作模式例图；
图2是本发明实施例中水下航行器在1个应答器下的径向速度变化简图；
其中，△表示声信号发射和接收换能器，□表示SINS，数字标号1,2,3,4均表示应答器。
具体实施方式
下面结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明所提供的一种引入径向速度的SINS/LBL紧组合导航方法，包括以下步骤：
1、在水下航行器上安装SINS，在海底布放LBL水声定位系统的应答器阵列，并精确标定每个应答器的位置。
如图1所示，R1、R2、R3、R4分别表示声信号发射和接收换能器到四个不同应答器之间的距离，在水下航行器上安装SINS，SINS的陀螺组件和加速度计组件固定设在水下航行器上，声信号发射和接收换能器也固设于水下航行器上，应答器阵列的组数大于或等于1组，每组应答器阵列的应答器数量大于或等于3个，如图1所示，该组应答器阵列的应答器数量为4，基线长度在几百米至几千米的范围内，基线长度是指相邻两个应答器之间的几何距离。每个应答器的位置通过GPS或USBL(超短基线定位系统)等设备精确标定，标定的应答器的位置用地球直角坐标来表示，第i个应答器的地球直角坐标Xi＝[xi yi zi]T，xi,yi,zi分别为第i个应答器的地球直角坐标的分量。
2、在LBL水声定位系统中，根据声信号在水下航行器与应答器之间的传播时延，计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差。
如图2所示，Ts是采样时间间隔，R1是水下航行器上的声信号发射和接收换能器在位置1与第1个应答器的距离，R1'是水下航行器上的声信号发射和接收换能器在位置2与第1个应答器的距离，v1是水下航行器在采样时间间隔内的平均径向速度，平均径向速度为该段时间内相对距离的变化率。在水下航行器上的声信号发射和接收换能器发出声信号，该声信号传输至应答器时具有传播时延，根据该传播时延计算出水下航行器与每个应答器之间的传播距离以及声信号到达两个不同应答器的传播距离之差，计算表达式如下式：
Figure BDA0002680574100000082
式(1)和(2)中，
Figure BDA0002680574100000083
为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，c为水中声速，τi为声信号在水下航行器与第i个应答器之间的单程传播时延，τj为声信号在水下航行器与第j个应答器之间的单程传播时延，
Figure BDA0002680574100000091
为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，i≠j。水中声速c为水下航行器和应答器之间的几何距离与声信号在水下航行器和应答器之间的传播时间的比值。
应答器在接收到声信号后，会反馈一个应答信号给声信号发射和接收换能器，每个应答器发出的应答信号的频率不同，声信号发射和接收换能器实际接收到的应答信号与该应答信号的理论频率(即设置的参考信号的频率)也不同，根据声信号发射和接收换能器实际接收到的应答信号以及设置的参考信号，计算出水下航行器与应答器在径向方向的相对运动速度，具体计算表达式为：
Figure BDA0002680574100000092
式(3)中，
Figure BDA0002680574100000093
为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，fc为参考信号频率，fd为参考信号频率fc与实际接收到的应答信号频率之差。
3、在SINS中，计算出水下航行器在导航坐标系下的姿态和速度以及水下航行器在地球坐标系下的位置，并将水下航行器在地球坐标系下的位置转换成地球直角坐标。
姿态和速度为导航坐标系下的向量，位置为地球坐标系下的球坐标。姿态包括航向角A、俯仰角K和横滚角ψ，速度包括东向速度vE、北向速度vN和天向速度vU，位置包括经度λ、纬度L和高度h。导航坐标系是指东北天(ENU)地理坐标系，即以水下航行器的质心为原点，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向地理天向，xn轴、yn轴和zn轴构成右手坐标系。地球直角坐标系是指以地心为原点，x轴指向本初子午线与赤道交点，z轴沿地轴方向，x、y、z构成右手坐标系。航向角是指水下航行器艏向在当地水平面的投影线与地理北向的夹角，定义北偏西为正；俯仰角是指水下航行器纵轴与其水平投影线的夹角，定义抬头为正；横滚角是指水下航行器的立轴与纵轴所在铅垂面的夹角，定义右倾为正；天向速度是指水下航行器沿地理天向的运动速度；东向速度是指水下航行器沿地理东向的运动速度；北向速度是指水下航行器沿地理北向的运动速度。
根据SINS计算水下航行器在导航坐标系下的姿态和速度以及水下航行器在地球坐标系下的位置为现有技术，可参考严恭敏等著《捷联惯导算法与组合导航原理》，西北工业大学出版社；还可参考秦永元著《惯性导航》(第二版)，科学出版社，以及秦永元等著《卡尔曼滤波与组合导航原理》，西北工业大学出版社。
采用式(4)将水下航行器在地球坐标系下的位置转换成地球直角坐标：
Figure BDA0002680574100000101
式(4)中，Xs＝[xs ys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，
Figure BDA0002680574100000102
RN为地球子午圈半径，Re为地球半径，e为地球偏心率，
Figure BDA0002680574100000103
a和b分别为地球椭圆长轴和短轴半径，h为SINS计算的水下航行器在地球坐标系下的高度，L为SINS计算的水下航行器在地球坐标系下的纬度，λ为SINS计算的水下航行器在地球坐标系下的经度。
根据步骤1中已标定的应答器的位置计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差，如式(5)和(6)所示：
Figure BDA0002680574100000104
Figure BDA0002680574100000105
式(5)和(6)中，
Figure BDA0002680574100000106
为SINS计算的水下航行器与第i个应答器之间的传播距离，Xi＝[xi yi zi]T，Xi为第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，
Figure BDA0002680574100000107
为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure BDA0002680574100000108
为SINS计算的水下航行器与第j个应答器之间的传播距离，i≠j。
4：根据步骤3中SINS计算的水下航行器在导航坐标系下的速度计算出水下航行器与应答器在径向方向的相对运动速度，具体计算表达式为：
Figure BDA0002680574100000109
记：
Figure BDA0002680574100000111
式(7)和(8)中，
Figure BDA0002680574100000112
为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度，vE为SINS计算的水下航行器在导航坐标系下的东向速度，vN为SINS计算的水下航行器在导航坐标系下的北向速度，vU为SINS计算的水下航行器在导航坐标系下的天向速度。
5、由SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差构成扩展卡尔曼滤波器的状态变量，根据状态变量建立状态方程。
即状态变量XT＝[φT (δν)T (δp)T εT ▽T]，φ为SINS的姿态误差，φ＝[φE φNφU]T，φE、φN和φU分别为SINS的俯仰角误差(又称为东向失准角误差)、横滚角误差(又称为北向失准角误差)和航向角误差(又称为天向失准角误差)，δν为SINS的速度误差，δν＝[δvE δvN δvU]T，δvE、δvN和δvU分别为SINS的东向速度误差、北向速度误差和天向速度误差，δp为SINS的位置误差，δp＝[δL δλ δh]T，δL、δλ和δh分别为SINS的纬度误差、经度误差和高度误差，ε为SINS的陀螺漂移误差，ε＝[εx εy εz]T，εx、εy和εz分别为SINS的陀螺在载体坐标系x、y、z轴的漂移误差，▽为SINS的加速度计偏置误差，▽＝[▽x ▽y ▽z]T，▽x、▽y和▽z分别为SINS的加速度计在载体坐标系x、y、z轴的偏置误差。载体坐标系是以水下航行器质心为原点，x轴方向指向水下航行器右舷，y轴指向水下航行器的艏向，z轴指向水下航行器的立轴方向建立的坐标系，x，y，z符合右手坐标系定则。
因此，状态方程为：
Xk+1＝Fk+1/kXk+wk+1 (9)
式(9)中，Xk、Xk+1分别为k时刻、k+1时刻的状态变量，Fk+1/k为状态一步转移矩阵，wk+1为系统过程噪声，wk+1通常视为高斯白噪声序列，Fk+1/k的获取为现有技术。
由步骤2中LBL水声定位系统计算出的传播距离、传播距离之差和相对运动速度，步骤3中SINS计算出的传播距离和传播距离之差以及步骤4中计算出的相对运动速度构成扩展卡尔曼滤波器的观测量，根据状态变量和观测量建立观测方程。
即观测量
Figure BDA0002680574100000121
Figure BDA0002680574100000122
为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，
Figure BDA0002680574100000123
为SINS计算的水下航行器与第i个应答器之间的传播距离，
Figure BDA0002680574100000124
为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure BDA0002680574100000125
为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure BDA0002680574100000126
为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，
Figure BDA0002680574100000127
为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度。
因此，观测方程为：
Zk+1＝Hk+1Xk+1+vk+1 (10)
式(10)中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，Xk+1为k+1时刻的状态变量，vk+1为系统观测噪声，vk+1通常视为高斯白噪声序列。
对式(4)左右两边同时求导得：
Figure BDA0002680574100000128
记：
Figure BDA0002680574100000129
式(5)的全微分表示为：
Figure BDA0002680574100000131
记：
Figure BDA0002680574100000132
对式(6)两边同时微分得：
Figure BDA0002680574100000133
式(7)的全微分为：
Figure BDA0002680574100000134
其中，CVVi＝[cosζx cosζy cosζz]，
Figure BDA0002680574100000135
则，观测矩阵
Figure BDA0002680574100000141
6、根据步骤5中的状态方程和观测方程估计出SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差。
扩展卡尔滤波器是指将非线性的状态方程和观测方程进行线性近似处理，在最小均方误差准则下估计状态变量。
根据卡尔曼滤波的递推方程估计状态变量，状态变量的估计为现有技术，可参考严恭敏等著《捷联惯导算法与组合导航原理》，西北工业大学出版社，以及秦永元等著《卡尔曼滤波与组合导航原理》，西北工业大学出版社。递推计算过程如下：
根据
Figure BDA0002680574100000142
计算k+1时刻的状态估计值或预测值
Figure BDA0002680574100000143
根据Pk+1/k＝Fk+1/kPkFT k+1/k+Qk，计算k+1时刻的状态估计误差协方差矩阵Pk+1/k，其中Pk为k时刻的状态估计误差协方差矩阵，Qk为k时刻的系统过程噪声协方差矩阵。
根据Kk+1＝Pk+1/kHT k+1(Hk+1Pk+1/kHT k+1+Rk+1)-1计算k+1时刻的滤波器增益Kk+1，其中Rk+1是k+1时刻的系统观测噪声协方差矩阵。
根据Pk+1＝(I-Kk+1Hk+1)Pk+1/k计算k+1时刻的状态估计误差协方差矩阵Pk+1，I为对角线元素为1的单位矩阵。
根据
Figure BDA0002680574100000144
计算k+1时刻的状态估计值
Figure BDA0002680574100000145
7、根据步骤6中估计出的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差对所述SINS输出的姿态、速度、位置、陀螺漂移和加速度计偏置进行修正，得到修正后的姿态、速度、位置、陀螺漂移和加速度计偏置。
修正后的姿态、速度、位置、陀螺漂移和加速度计偏置分别为：
qk+1＝Δq·qk，
Figure BDA0002680574100000146
Figure BDA0002680574100000147
其中，qk、qk+1分别为k时刻、k+1时刻的姿态四元数，Δq为k时刻到k+1时刻的姿态变化四元数，Δq由
Figure BDA0002680574100000148
计算得到，
Figure BDA0002680574100000149
为k+1时刻状态变量估计值的第1到3个，姿态四元数经修正好后转换成对应的俯仰角、横滚角和航向角(转换过程可参考严恭敏著《捷联惯导算法与组合导航原理》)，v'为修正后的速度向量，v为SINS计算的水下航行器在导航坐标系下的速度向量，
Figure BDA0002680574100000151
为k+1时刻状态变量估计值的第4到6个，p'为修正后的位置向量，p为SINS计算的水下航行器在地球坐标系下的位置向量，
Figure BDA0002680574100000152
为k+1时刻状态变量估计值的第7到9个，ε'为修正后的陀螺漂移向量，ε为修正前的陀螺漂移向量，
Figure BDA0002680574100000153
为k+1时刻状态变量估计值的第10到12个，▽'为修正后的加速度计偏置向量，▽为修正前的加速度计偏置向量，
Figure BDA0002680574100000154
为k+1时刻状态变量估计值的第13到15个。
8、重置扩展卡尔曼滤波器的状态变量，循环执行步骤2～7，直到水下航行器作业结束。
重置状态变量(即滤波状态变量)是指在修正SINS导航输出后，理论上认为此时SINS的导航输出不存在误差，将下一时刻的状态变量置零，即滤波器估计的是历史时刻所有导航误差的累加效果。重置扩展卡尔曼滤波器的状态变量，即
Figure BDA0002680574100000155
以上所揭露的仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或变型，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种引入径向速度的SINS/LBL紧组合导航方法，其特征在于，包括：
步骤1：在水下航行器上安装SINS，在海底布放LBL水声定位系统的应答器阵列，并精确标定每个应答器的位置；
步骤2：在LBL水声定位系统中，根据声信号在水下航行器与应答器之间的传播时延，计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差；
根据实际接收到的应答信号以及设置的参考信号，计算出水下航行器与应答器在径向方向的相对运动速度；
步骤3：在SINS中，计算出水下航行器在导航坐标系下的姿态和速度以及水下航行器在地球坐标系下的位置，并将水下航行器在地球坐标系下的位置转换成地球直角坐标；
根据所述步骤1中已标定的应答器的位置计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差；
步骤4：根据所述步骤3中SINS计算的水下航行器在导航坐标系下的速度计算出水下航行器与应答器在径向方向的相对运动速度；
步骤5：由SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差构成扩展卡尔曼滤波器的状态变量；根据所述状态变量建立状态方程；
由所述步骤2中LBL水声定位系统计算出的传播距离、传播距离之差和相对运动速度，步骤3中SINS计算出的传播距离和传播距离之差以及步骤4中计算出的相对运动速度构成扩展卡尔曼滤波器的观测量；
根据所述状态变量和观测量建立观测方程；
步骤6：根据所述步骤5中的状态方程和观测方程估计出SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差；
步骤7：根据所述步骤6中估计出的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差对所述SINS输出的姿态、速度、位置、陀螺漂移和加速度计偏置进行修正，得到修正后的姿态、速度、位置、陀螺漂移和加速度计偏置；
步骤8：重置所述扩展卡尔曼滤波器的状态变量，循环执行步骤2～7，直到水下航行器作业结束。
2.如权利要求1所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤1中，应答器阵列的组数大于或等于1组，每组所述应答器阵列的应答器数量大于或等于3个。
3.如权利要求1所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤2中，水下航行器与第i个应答器之间的传播距离的计算表达式为：
Figure FDA0002680574090000021
声信号到达第i个应答器和第j个应答器的传播距离之差的计算表达式为：
Figure FDA0002680574090000022
其中，
Figure FDA0002680574090000023
为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，c为水中声速，τi为声信号在水下航行器与第i个应答器之间的单程传播时延，τj为声信号在水下航行器与第j个应答器之间的单程传播时延，
Figure FDA0002680574090000024
为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，i≠j；
水下航行器与第i个应答器在径向方向的相对运动速度的计算表达式为：
Figure FDA0002680574090000025
其中，
Figure FDA0002680574090000026
为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，fc为参考信号频率，fd为参考信号频率fc与实际接收到的应答信号频率之差。
4.如权利要求1所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤3中，水下航行器的地球直角坐标为：
Figure FDA0002680574090000027
其中，Xs＝[xs ys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，
Figure FDA0002680574090000028
RN为地球子午圈半径，Re为地球半径，e为地球偏心率，
Figure FDA0002680574090000029
a和b分别为地球椭圆长轴和短轴半径，h为SINS计算的水下航行器在地球坐标系下的高度，L为SINS计算的水下航行器在地球坐标系下的纬度，λ为SINS计算的水下航行器在地球坐标系下的经度，SINS计算的水下航行器在地球坐标系下的位置包括高度、纬度和经度；
水下航行器与第i个应答器之间的传播距离的计算表达式为：
Figure FDA00026805740900000210
声信号到达第i个应答器和第j个应答器的传播距离之差的计算表达式为
Figure FDA0002680574090000031
其中，
Figure FDA0002680574090000032
为SINS计算的水下航行器与第i个应答器之间的传播距离，Xi＝[xi yi zi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，
Figure FDA0002680574090000033
为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure FDA0002680574090000034
为SINS计算的水下航行器与第j个应答器之间的传播距离，i≠j。
5.如权利要求1所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤4中，水下航行器与应答器在径向方向的相对运动速度的计算表达式为：
Figure FDA0002680574090000035
其中，
Figure FDA0002680574090000036
为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度，Xs＝[xsys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，Xi＝[xi yi zi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，vE为SINS计算的水下航行器在导航坐标系下的东向速度，vN为SINS计算的水下航行器在导航坐标系下的北向速度，vU为SINS计算的水下航行器在导航坐标系下的天向速度，SINS计算的水下航行器在导航坐标系下的速度包括东向速度、北向速度和天向速度。
6.如权利要求1所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤5中，状态方程为：
Xk+1＝Fk+1/kXk+wk+1
其中，Xk、Xk+1分别为k时刻、k+1时刻的状态变量，Fk+1/k为状态一步转移矩阵，wk+1为系统过程噪声；状态变量
Figure FDA0002680574090000037
φ为SINS的姿态误差，φ＝[φEφN φU]T，φE、φN和φU分别为SINS的俯仰角误差、横滚角误差和航向角误差，δν为SINS的速度误差，δν＝[δvE δvN δvU]T，δvE、δvN和δvU分别为SINS的东向速度误差、北向速度误差和天向速度误差，δp为SINS的位置误差，δp＝[δL δλ δh]T，δL、δλ和δh分别为SINS的纬度误差、经度误差和高度误差，ε为SINS的陀螺漂移误差，ε＝[εx εy εz]T，εx、εy和εz分别为SINS的陀螺在载体坐标系x、y、z轴的漂移误差，
Figure FDA0002680574090000041
为SINS的加速度计偏置误差，
Figure FDA0002680574090000042
Figure FDA0002680574090000043
和
Figure FDA0002680574090000044
分别为SINS的加速度计在载体坐标系x、y、z轴的偏置误差。
7.如权利要求6所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤5中，观测方程为：
Zk+1＝Hk+1Xk+1+vk+1
其中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，Xk+1为k+1时刻的状态变量，vk+1为系统观测噪声，观测量
Figure FDA0002680574090000045
Figure FDA0002680574090000046
为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，
Figure FDA0002680574090000047
为SINS计算的水下航行器与第i个应答器之间的传播距离，
Figure FDA0002680574090000048
为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure FDA0002680574090000049
为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，
Figure FDA00026805740900000410
为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，
Figure FDA00026805740900000411
为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度；
k+1时刻的观测矩阵
Figure FDA00026805740900000412
Figure FDA00026805740900000413
CVVi＝[cosζx cosζy cosζz]，
Figure FDA00026805740900000414
Figure FDA0002680574090000051
Figure FDA0002680574090000052
其中，
Figure FDA0002680574090000053
RN为地球子午圈半径，Re为地球半径，e为地球偏心率，
Figure FDA0002680574090000054
a和b分别为地球椭圆长轴和短轴半径，h为SINS计算的水下航行器在地球坐标系下的高度，L为SINS计算的水下航行器在地球坐标系下的纬度，λ为SINS计算的水下航行器在地球坐标系下的经度，0i×3为i×3维零向量，Xs＝[xs ys zs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，Xi＝[xi yi zi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，vE为SINS计算的水下航行器在导航坐标系下的东向速度，vN为SINS计算的水下航行器在导航坐标系下的北向速度，vU为SINS计算的水下航行器在导航坐标系下的天向速度。
8.如权利要求1-7中任一项所述的SINS/LBL紧组合导航方法，其特征在于：所述步骤7中，修正后的姿态、速度、位置、陀螺漂移和加速度计偏置分别为：
qk+1＝Δq·qk，
Figure FDA0002680574090000055
Figure FDA0002680574090000056
其中，qk、qk+1分别为k时刻、k+1时刻的姿态四元数，Δq为k时刻到k+1时刻的姿态变化四元数，Δq由
Figure FDA0002680574090000057
计算得到，
Figure FDA0002680574090000058
为k+1时刻状态变量估计值的第1到3个，v'为修正后的速度向量，v为SINS计算的水下航行器在导航坐标系下的速度向量，
Figure FDA0002680574090000061
为k+1时刻状态变量估计值的第4到6个，p'为修正后的位置向量，p为SINS计算的水下航行器在地球坐标系下的位置向量，
Figure FDA0002680574090000062
为k+1时刻状态变量估计值的第7到9个，ε'为修正后的陀螺漂移向量，ε为修正前的陀螺漂移向量，
Figure FDA0002680574090000063
为k+1时刻状态变量估计值的第10到12个，
Figure FDA0002680574090000064
为修正后的加速度计偏置向量，
Figure FDA0002680574090000065
为修正前的加速度计偏置向量，
Figure FDA0002680574090000066
为k+1时刻状态变量估计值的第13到15个。
CN202010961099.7A 2020-09-14 2020-09-14 一种引入径向速度的sins/lbl紧组合导航方法 Active CN112083425B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010961099.7A CN112083425B (zh) 2020-09-14 2020-09-14 一种引入径向速度的sins/lbl紧组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010961099.7A CN112083425B (zh) 2020-09-14 2020-09-14 一种引入径向速度的sins/lbl紧组合导航方法

Publications (2)

Publication Number Publication Date
CN112083425A true CN112083425A (zh) 2020-12-15
CN112083425B CN112083425B (zh) 2024-03-12

Family

ID=73737800

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010961099.7A Active CN112083425B (zh) 2020-09-14 2020-09-14 一种引入径向速度的sins/lbl紧组合导航方法

Country Status (1)

Country Link
CN (1) CN112083425B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112815949A (zh) * 2021-02-02 2021-05-18 中国科学院沈阳自动化研究所 一种适用水下回收过程的超短组合导航方法
CN116337115A (zh) * 2023-05-31 2023-06-27 山东大学 一种基于声呐标校惯性传感器的方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6427122B1 (en) * 2000-12-23 2002-07-30 American Gnc Corporation Positioning and data integrating method and system thereof
US20030078706A1 (en) * 2000-03-03 2003-04-24 Larsen Mikael Bliksted Methods and systems for navigating under water
CN104316045A (zh) * 2014-11-06 2015-01-28 东南大学 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110132281A (zh) * 2019-05-21 2019-08-16 哈尔滨工程大学 一种基于询问应答模式的水下高速目标高精度自主声学导航方法
CN110836981A (zh) * 2019-11-11 2020-02-25 南京世海声学科技有限公司 一种分层水流高分辨径向声学多普勒频率测量方法
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030078706A1 (en) * 2000-03-03 2003-04-24 Larsen Mikael Bliksted Methods and systems for navigating under water
US6427122B1 (en) * 2000-12-23 2002-07-30 American Gnc Corporation Positioning and data integrating method and system thereof
CN104316045A (zh) * 2014-11-06 2015-01-28 东南大学 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110132281A (zh) * 2019-05-21 2019-08-16 哈尔滨工程大学 一种基于询问应答模式的水下高速目标高精度自主声学导航方法
CN110836981A (zh) * 2019-11-11 2020-02-25 南京世海声学科技有限公司 一种分层水流高分辨径向声学多普勒频率测量方法
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
张宏: "多普勒计程仪工作原理和测速精度的初探", 科技论坛 *
张涛;石宏飞;徐晓苏;: "基于SINS/LBL紧组合的AUV水下导航定位技术", 中国惯性技术学报, no. 04 *
张涛;石宏飞;陈立平;刘强;: "基于UKF的SINS/LBL水下AUV紧组合定位技术", 中国惯性技术学报, no. 05 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112815949A (zh) * 2021-02-02 2021-05-18 中国科学院沈阳自动化研究所 一种适用水下回收过程的超短组合导航方法
CN112815949B (zh) * 2021-02-02 2023-11-24 中国科学院沈阳自动化研究所 一种适用水下回收过程的超短组合导航方法
CN116337115A (zh) * 2023-05-31 2023-06-27 山东大学 一种基于声呐标校惯性传感器的方法及系统
CN116337115B (zh) * 2023-05-31 2023-08-29 山东大学 一种基于声呐标校惯性传感器的方法及系统

Also Published As

Publication number Publication date
CN112083425B (zh) 2024-03-12

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN106990424B (zh) 一种双天线gps测姿方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN111239787A (zh) 一种集群自主协同中的gnss动态卡尔曼滤波方法
CN113311436A (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
Hide et al. GPS and low cost INS integration for positioning in the urban environment
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN115327588A (zh) 一种基于网络rtk的无人自动化作业特种车高精度定位方法
CN110793518A (zh) 一种海上平台的定位定姿方法及系统
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN108151765A (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
CN109813316A (zh) 一种基于地形辅助的水下载体紧组合导航方法
CN109471102B (zh) 一种惯组误差修正方法
CN111380520B (zh) 一种引入径向速度的sins/usbl松组合导航定位方法
CN112051598A (zh) 一种基于双重校正的车载gnss/ins组合导航方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法
CN115031724A (zh) Sins/dvl紧组合系统dvl波束故障处理方法
CN113670302A (zh) 一种运动效应影响下的惯性/超短基线组合导航方法
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