CN113959434B - 一种可调节的sins、dvl、usbl组合导航方法 - Google Patents

一种可调节的sins、dvl、usbl组合导航方法 Download PDF

Info

Publication number
CN113959434B
CN113959434B CN202111108406.8A CN202111108406A CN113959434B CN 113959434 B CN113959434 B CN 113959434B CN 202111108406 A CN202111108406 A CN 202111108406A CN 113959434 B CN113959434 B CN 113959434B
Authority
CN
China
Prior art keywords
sins
dvl
usbl
state
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111108406.8A
Other languages
English (en)
Other versions
CN113959434A (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.)
Hebei Hanguang Heavy Industry Ltd
Original Assignee
Hebei Hanguang Heavy Industry Ltd
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 Hebei Hanguang Heavy Industry Ltd filed Critical Hebei Hanguang Heavy Industry Ltd
Priority to CN202111108406.8A priority Critical patent/CN113959434B/zh
Publication of CN113959434A publication Critical patent/CN113959434A/zh
Application granted granted Critical
Publication of CN113959434B publication Critical patent/CN113959434B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提出了一种可调节的SINS、DVL、USBL组合导航方法，能够根据各种实际情况调整运行模式，实现优势互补，提高整个***的工作适应性，完成各种情况下的组合导航。本发明在SINS、DVL、USBL组合导航的实际使用中，通过对不同的情况采用可调节的灵活组合导航方式，获得惯导误差的准确估计。该组合导航可以实现优势互补，提高整个***的工作适应性，完成各种情况下的组合导航，可调节的灵活组合导航方式有效减小了惯导误差，提高了组合导航***的精度。

Description

一种可调节的SINS、DVL、USBL组合导航方法
技术领域
本发明涉及导航技术领域，具体涉及一种可调节的SINS、DVL、USBL组合导航方法。
背景技术
目前常用的几种导航设备包括捷联惯导***(SINS)、多普勒测速仪(DVL)以及超短基线定位***(USBL)。
其中，捷联惯导***(SINS)由陀螺、加速度计、导航计算机等组成可实现自主导航的惯导导航***，利用加速度计的量测通过其导航计算机的导航计算得到载体的位置和速度信息，利用陀螺仪的量测输出载体的姿态和航向信息，能够为水下载体提供包括姿态、速度和位置信息，但其导航误差随时间积累，***导航精度下降，不能单独长时间工作；多普勒测速仪(DVL)是依据多普勒效应测量载体速度的导航设备，通过多普勒效应测量DVL相对于水底的速度，能够提供高精度连续速度信息，是常用的辅助导航设备，然而SINS、DVL组合导航***器测速误差易受水温、海水盐度以及波束宽度等因素的影响，存在DVL声波无法到达水底，或当水底地质是淤泥类等吸波性强的物质时，DVL声波无法返回的问题；超短基线定位***(USBL)是根据声学定位原理提供载体的三维位置信息，其定位精度较高、操作简单、便携性强、成本较低，该***由基阵和应答器组成，其中应答器的位置精确已知，通过测量应答器相对于基阵的相对位置信息(包括角度信息和斜距信息)获取USBL的位置信息，可作为辅助导航***，用来抑制惯导***误差的发散，然而易受海洋环境的影响，会出现数据跳变或者数据丢失的现象，SINS、USBL组合不易实现远距离的组合导航，一般只能在特定水域工作。
综上，现有的导航***工作适应性不强，不能完成各种情况下的导航。
发明内容
有鉴于此，本发明提出了一种可调节的SINS、DVL、USBL组合导航方法，能够根据各种实际情况调整运行模式，实现优势互补，提高整个***的工作适应性，完成各种情况下的组合导航。
为实现上述目的，本发明的技术方案为：
本发明的一种可调节的SINS、DVL、USBL组合导航方法，通过采集SINS的信息和全球定位***的信息实现初始对准；
对准完成后同步采集SINS的位置和速度、DVL的速度、USBL的位置数据进行组合导航；SINS、DVL以及USBL实时判断各自输出信息是否有效及输出信息有效时精度是否高于设定值，根据具体情况选择不同的滤波方式，具体如下：
DVL和USBL量测数据均有效，且精度均高于设定值时，采用集中滤波形式进行SINS、DVL、USBL组合导航；其中，量测量为DVL的速度和SINS的速度之差以及USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；集中滤波进行数据融合时对数据进行统一计算，实现对状态的最优估计；
当DVL和USBL量测数据均有效，且至少一个量测数据精度不高于设定值时，转变成联邦滤波形式进行SINS、DVL和SINS、USBL组合导航；联邦滤波采用主滤波器和子滤波器形式，在主滤波器中将两个子滤波器的输入信息和主惯导的量测进行信息融合得到状态估计值，反馈姿态、速度和位置并把估计状态值置零；
当DVL量测数据有效，USBL量测数据无效时，转变成SINS、DVL组合导航，量测量为DVL的速度和SINS的速度之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；
当DVL量测数据无效，USBL量测数据有效时，转变成SINS、USBL组合导航，量测量为USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；
当DVL和USBL量测数据均无效时，不进行组合导航，转变成SINS导航，等待DVL或USBL量测再次有效时进入组合导航状态。
其中，所述组合导航方法中，组合导航的状态变量包括姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏；不同条件下的滤波器的状态变量均为15维，如下所示；
其中，分别代表惯性导航失准角误差中的东向失准角、北向失准角、天向失准角误差，δVE、δVN、δVU分别代表惯性导航速度误差中东向速度误差、北向速度误差和天向速度误差；δL、δλ、δh分别代表惯性导航位置误差中的纬度误差、经度误差和高度误差；εE、εN、εU分别代表x轴陀螺、y轴陀螺和z轴陀螺的随机常值漂移；/>分别代表x轴加速度计、y轴加速度计和z轴加速度计的随机常值零偏；
Kalman滤波***的***方程为：
其中，X为***误差状态变量，W为***噪声变量，F为***状态转移矩阵，G为***噪声转换矩阵；
W＝[wgx wgy wgz wax way waz]T
其中，wgx、wgy、wgz分别为x轴陀螺、y轴陀螺和z轴陀螺的噪声；wax、way、waz和分别为x轴加速度计，y轴加速度计和z轴加速度计的噪声；
其中，是载体系到导航系的姿态变换矩阵；
***状态转移矩阵中各子矩阵表示如下：
其中，RMh为地球的子午圈主曲率半径，RNh为地球的卯酉圈主曲率半径，ωie为地球自转角速度，L为当地的纬度，分别为惯导的东速、北速和天速，/>分别为惯导地理坐标系下三个方向的比力；
量测方程为：
Z＝HX+V；
***状态方程及量测方程的离散化为：
Xk＝φk,k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
式中，Xk为k时刻的状态向量，也就是被估计矢量，Xk-1为k-1时刻的状态向量，φk,k-1为k-1时刻到k时刻的一步状态转移矩阵，Γk-1是***噪声输入矩阵，Wk-1为k-1时刻的***噪声；Zk为k时刻的测量序列，Hk为k时刻的测量矩阵，Vk为k时刻的测量噪声序列；
状态转移矩阵：φk,k-1＝I+Fk-1Δt+…
其中，I是信息矩阵，为P-1矩阵；
利用标准卡尔曼滤波方程计算状态的最优估计如下：
状态一步预测向量：Xk/k-1＝φk,k-1Xk-1；
滤波增益：
状态估值计算：Xk＝Xk/k-1+Kk(Zk-HkXk/k-1)；
一步预测均方误差矩阵：
估计均方误差方程：
其中，Rk是量测噪声序列的方差阵，Qk-1是量测噪声序列的方差阵。
其中，采用联邦滤波时，通过第一子滤波器、第二子滤波器以及主滤波器实现；
其中，第一子滤波器的量测量为DVL的速度和SINS的速度之差，第二子滤波器的量测量为USBL的位置和SINS的位置之差，采用第一子滤波器实现SINS、DVL组合滤波，采用第二子滤波器实现SINS、USBL组合滤波，通过第一子滤波器获得的状态估计值和估计均方误差阵分别为X1和P1，通过子滤波器2获得的状态估计值和估计均方误差阵分别为X2和P2。计算融合估计误差方差阵，公式为其中，P1 -1为P1的逆矩阵，/>为P2的逆矩阵。计算主滤状态估计值，公式为/>从Xg中反馈姿态、速度和位置并把状态估计值X1和X2置零。
其中，采用集中滤波时，Z和H如下所示：
其中，采用联邦滤波时，第一子滤波器实现SINS、DVL组合滤波，第二子滤波器实现SINS、USBL组合滤波时，Z和H分别如下所示：
其中，当USBL无效时，采用SINS、DVL组合滤波时，Z和H如下所示：
其中，当DVL无效时，采用SINS、USBL组合滤波时，Z和H如下所示：
其中，不同滤波条件下，误差反馈校正均相同，具体为：
姿态误差校正：
位置误差校正：[L λ h]T＝[L′ λ′ h′]T-[δL δλ δh]T
速度误差校正：[VE VN VU]T＝[V′E V′N V′U]T-[δVE δVN δVU]T
其中，为姿态误差校正前一时刻的姿态变换矩阵；L′、λ′、h′为位置误差校正前一时刻的纬度、经度和高度，V′E、V′N、V′U为速度误差校正前一时刻的东向速度、北向速度和天向速度。
有益效果：
本发明可调节的SINS、DVL、USBL组合导航方法，在SINS、DVL、USBL组合导航的实际使用中，通过对不同的情况采用可调节的灵活组合导航方式，获得惯导误差的准确估计。该组合导航可以实现优势互补，提高整个***的工作适应性，完成各种情况下的组合导航，可调节的灵活组合导航方式有效减小了惯导误差，提高了组合导航***的精度。
附图说明
图1为本发明组合导航流程示意图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明的一种可调节的SINS、DVL、USBL组合导航方法，流程图如图1所示，包括如下步骤：
通过采集SINS的信息和全球定位***(GPS)的信息实现初始对准；
对准完成后同步采集SINS的位置和速度、DVL的速度、USBL的位置数据进行多种情况下的组合导航，SINS、DVL、USBL可以实时判断自己输出信息的有效位(分为：有效、无效)及输出信息有效时相应精度是否高于设定值(高于设定值为高精度，不高于设定值为低精度)，根据具体情况选择不同的滤波方式。具体如下：
正常情况下，DVL和USBL量测数据均有效，且高于设定值情况下，采用集中滤波形式进行SINS、DVL、USBL组合导航。量测量为DVL的速度和SINS的速度之差以及USBL的位置和SINS的位置之差，进行SINS、DVL、USBL的组合导航，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零。
当DVL和USBL量测数据均有效，且至少一个量测数据精度不高于设定值时，转变成联邦滤波形式进行SINS、DVL和SINS、USBL组合导航，在主滤波器中将两个子滤波器的输入信息和主惯导的量测进行信息融合得到状态估计值，反馈姿态、速度和位置并把估计状态值置零。
当DVL量测数据有效，USBL故障导致量测数据无效时，转变成SINS、DVL组合导航；当采用SINS、DVL组合导航时，量测量为DVL的速度和SINS的速度之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零。
当DVL故障导致量测数据无效，USBL量测数据有效，转变成SINS、USBL组合导航；当采用SINS、USBL组合导航时，量测量为USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零。
具体地，本实施例采用联邦滤波时，子滤波器1的量测量为DVL的速度和SINS的速度之差，子滤波器2的量测量为USBL的位置和SINS的位置之差，采用子滤波器1实现SINS、DVL组合，采用子滤波器2实现SINS、USBL组合，通过子滤波器1获得的状态估计值和估计均方误差阵分别为X1和P1，通过子滤波器2获得的状态估计值和估计均方误差阵分别为X2和P2。计算融合误差估计方差阵、主滤波器状态估计值，从主滤波器状态估计值中反馈姿态、速度和位置、并把X1和X2置零。
当DVL和USBL量测数据均无效时，不进行组合导航，转变成SINS(纯惯性)导航，纯惯性解算能够维持一段时间的精度，等待DVL或USBL量测再次有效时进入组合导航状态。
本发明具体过程为：
***上电启动通过GPS辅助完成初始对准后，根据捷联惯性导航***长期工作时的误差特点，选择姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏作为组合导航的状态变量。不同条件下的滤波器的状态变量均为15维，如下所示；
其中，分别代表惯性导航失准角误差中的东向失准角、北向失准角、天向失准角误差，δVE、δVN、δVU分别代表惯性导航速度误差中东向速度误差、北向速度误差和天向速度误差；δL、δλ、δh分别代表惯性导航位置误差中的纬度误差、经度误差和高度误差；εE、εN、εU分别代表x轴陀螺、y轴陀螺和z轴陀螺的随机常值漂移；/>分别代表x轴加速度计、y轴加速度计和z轴加速度计的随机常值零偏。
建立Kalman滤波***的***方程：
其中，X为***误差状态变量，W为***噪声变量，F为***状态转移矩阵，G为***噪声转换矩阵。
W＝[wgx wgy wgz wax way waz]T
其中，wgx、wgy、wgz分别为x轴陀螺、y轴陀螺和z轴陀螺的噪声。wax、way、waz和分别为x轴加速度计，y轴加速度计和z轴加速度计的噪声。
其中，是载体系到导航系的姿态变换矩阵。
***状态转移矩阵中各子矩阵表示如下：
上各式中，RMh、RNh分别为地球的子午圈主曲率半径，卯酉圈主曲率半径，ωie为地球自转角速度，L为当地的纬度，分别为惯导的东速、北速和天速，分别为惯导地理坐标系下三个方向的比力。
建立量测方程：
Z＝HX+V；
建***状态方程及量测方程的离散化：
Xk＝φk,k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
式中，Xk为k时刻的状态向量，也就是被估计矢量，Xk-1为k-1时刻的状态向量，φk,k-1为k-1时刻到k时刻的一步状态转移矩阵，Γk-1是***噪声输入矩阵，Wk-1为k-1时刻的***噪声；Zk为k时刻的测量序列，Hk为k时刻的测量矩阵，Vk为k时刻的测量噪声序列。
状态转移矩阵：φk,k-1＝I+Fk-1Δt+…
其中，I是信息矩阵，为P-1矩阵；
利用标准卡尔曼滤波方程计算状态的最优估计如下：
状态一步预测向量；Xk/k-1＝φk,k-1Xk-1；
滤波增益:
状态估值计算:Xk＝Xk/k-1+Kk(Zk-HkXk/k-1)；
一步预测均方误差矩阵:
估计均方误差方程:
其中，Rk是量测噪声序列的方差阵，Qk-1是量测噪声序列的方差阵。
采用集中滤波时，量测量为DVL的速度和SINS的速度之差以及USBL的位置和SINS的位置之差，进行SINS、DVL、USBL的组合导航，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零。Z和H如下所示：
采用联邦滤波时，子滤波器1的量测量为DVL的速度和SINS的速度之差，子滤波器2量测量为USBL的位置和SINS的位置之差，采用子滤波器1实现SINS、DVL组合，采用子滤波器2实现SINS、USBL组合，通过子滤波器1获得的状态估计值和估计均方误差阵分别为X1和P1，通过子滤波器2获得的状态估计值和估计均方误差阵分别为X2和P2。计算融合估计误差方差阵，公式为其中，P1 -1为P1的逆矩阵，/>为P2的逆矩阵。计算主滤状态估计值，公式为/>从Xg中反馈姿态、速度和位置并把状态估计值X1和X2置零。Z和H分别如下所示：
当采用SINS、DVL组合滤波时，量测量为DVL的速度和SINS的速度之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零。Z和H如下所示：
当采用SINS、USBL组合滤波时，量测量为USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零。Z和H如下所示：
不同情况下，误差反馈校正均相同，如下所示：
姿态误差校正：
位置误差校正：[L λ h]T＝[L′ λ′ h′]T-[δL δλ δh]T
速度误差校正：[VE VN VU]T＝[V′E V′N V′U]T-[δVE δVN δVU]T
其中，为姿态误差校正前一时刻的姿态变换矩阵；L′、λ′、h′为位置误差校正前一时刻的纬度、经度和高度，V′E、V′N、V′U为速度误差校正前一时刻的东向速度、北向速度和天向速度。
当DVL和USBL量测数据均无效时，纯惯性解算能够维持一段时间的精度，等待DVL或USBL量测再次有效时进入组合导航状态。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种可调节的SINS、DVL、USBL组合导航方法，其特征在于，通过采集SINS的信息和全球定位***的信息实现初始对准；
对准完成后同步采集SINS的位置和速度、DVL的速度、USBL的位置数据进行组合导航；SINS、DVL以及USBL实时判断各自输出信息是否有效及输出信息有效时精度是否高于设定值，根据具体情况选择不同的滤波方式，具体如下：
DVL和USBL量测数据均有效，且精度均高于设定值时，采用集中滤波形式进行SINS、DVL、USBL组合导航；其中，量测量为DVL的速度和SINS的速度之差以及USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；集中滤波进行数据融合时对数据进行统一计算，实现对状态的最优估计；
当DVL和USBL量测数据均有效，且至少一个量测数据精度不高于设定值时，转变成联邦滤波形式进行SINS、DVL和SINS、USBL组合导航；联邦滤波采用主滤波器和子滤波器形式，在主滤波器中将两个子滤波器的输入信息和主惯导的量测进行信息融合得到状态估计值，反馈姿态、速度和位置并把估计状态值置零；
当DVL量测数据有效，USBL量测数据无效时，转变成SINS、DVL组合导航，量测量为DVL的速度和SINS的速度之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；
当DVL量测数据无效，USBL量测数据有效时，转变成SINS、USBL组合导航，量测量为USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；
当DVL和USBL量测数据均无效时，不进行组合导航，转变成SINS导航，等待DVL或USBL量测再次有效时进入组合导航状态；
所述组合导航方法中，组合导航的状态变量包括姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏；不同条件下的滤波器的状态变量均为15维，如下所示；
其中，分别代表惯性导航失准角误差中的东向失准角、北向失准角、天向失准角误差，δVE、δVN、δVU分别代表惯性导航速度误差中东向速度误差、北向速度误差和天向速度误差；δL、δλ、δh分别代表惯性导航位置误差中的纬度误差、经度误差和高度误差；εE、εN、εU分别代表x轴陀螺、y轴陀螺和z轴陀螺的随机常值漂移；/>分别代表x轴加速度计、y轴加速度计和z轴加速度计的随机常值零偏；
Kalman滤波***的***方程为：
其中，X为***误差状态变量，W为***噪声变量，F为***状态转移矩阵，G为***噪声转换矩阵；
W＝[wgx wgy wgz wax way waz]T
其中，wgx、wgy、wgz分别为x轴陀螺、y轴陀螺和z轴陀螺的噪声；wax、way、waz和分别为x轴加速度计，y轴加速度计和z轴加速度计的噪声；
其中，是载体系到导航系的姿态变换矩阵；
***状态转移矩阵中各子矩阵表示如下：
其中，RMh为地球的子午圈主曲率半径，RNh为地球的卯酉圈主曲率半径，ωie为地球自转角速度，L为当地的纬度，分别为惯导的东速、北速和天速，/>分别为惯导地理坐标系下三个方向的比力；
量测方程为：
Z＝HX+V；
***状态方程及量测方程的离散化为：
Xk＝φk,k-1Xk-1+Γk-1Wk-1
Zk＝HkXk+Vk
式中，Xk为k时刻的状态向量，也就是被估计矢量，Xk-1为k-1时刻的状态向量，φk,k-1为k-1时刻到k时刻的一步状态转移矩阵，Γk-1是***噪声输入矩阵，Wk-1为k-1时刻的***噪声；Zk为k时刻的测量序列，Hk为k时刻的测量矩阵，Vk为k时刻的测量噪声序列；
状态转移矩阵：φk,k-1＝I+Fk-1Δt+…
其中，I是信息矩阵，为P-1矩阵；
利用标准卡尔曼滤波方程计算状态的最优估计如下：
状态一步预测向量：Xk/k-1＝φk,k-1Xk-1；
滤波增益：
状态估值计算：Xk＝Xk/k-1+Kk(Zk-HkXk/k-1)；
一步预测均方误差矩阵：
估计均方误差方程：
其中，Rk是量测噪声序列的方差阵，Qk-1是量测噪声序列的方差阵；
采用集中滤波时，Z和H如下所示：
2.如权利要求1所述的组合导航方法，其特征在于，采用联邦滤波时，通过第一子滤波器、第二子滤波器以及主滤波器实现；
其中，第一子滤波器的量测量为DVL的速度和SINS的速度之差，第二子滤波器的量测量为USBL的位置和SINS的位置之差，采用第一子滤波器实现SINS、DVL组合滤波，采用第二子滤波器实现SINS、USBL组合滤波，通过第一子滤波器获得的状态估计值和估计均方误差阵分别为X1和P1，通过子滤波器2获得的状态估计值和估计均方误差阵分别为X2和P2；计算融合估计误差方差阵，公式为其中，P1 -1为P1的逆矩阵，/>为P2的逆矩阵；计算主滤状态估计值，公式为/>从Xg中反馈姿态、速度和位置并把状态估计值X1和X2置零。
3.如权利要求1或2所述的组合导航方法，其特征在于，采用联邦滤波时，第一子滤波器实现SINS、DVL组合滤波，第二子滤波器实现SINS、USBL组合滤波时，Z和H分别如下所示：
4.如权利要求1所述的组合导航方法，其特征在于，当USBL无效时，采用SINS、DVL组合滤波时，Z和H如下所示：
5.如权利要求1所述的组合导航方法，其特征在于，当DVL无效时，采用SINS、USBL组合滤波时，Z和H如下所示：
6.如权利要求1、2、3、4或5所述的组合导航方法，其特征在于，不同滤波条件下，误差反馈校正均相同，具体为：
姿态误差校正：
位置误差校正：[L λ h]T＝[L' λ' h']T-[δL δλ δh]T
速度误差校正：[VE VN VU]T＝[V'E V'N V'U]T-[δVE δVN δVU]T
其中，为姿态误差校正前一时刻的姿态变换矩阵；L'、λ'、h'为位置误差校正前一时刻的纬度、经度和高度，V'E、V'N、V'U为速度误差校正前一时刻的东向速度、北向速度和天向速度。
CN202111108406.8A 2021-09-22 2021-09-22 一种可调节的sins、dvl、usbl组合导航方法 Active CN113959434B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111108406.8A CN113959434B (zh) 2021-09-22 2021-09-22 一种可调节的sins、dvl、usbl组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111108406.8A CN113959434B (zh) 2021-09-22 2021-09-22 一种可调节的sins、dvl、usbl组合导航方法

Publications (2)

Publication Number Publication Date
CN113959434A CN113959434A (zh) 2022-01-21
CN113959434B true CN113959434B (zh) 2024-06-14

Family

ID=79462327

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111108406.8A Active CN113959434B (zh) 2021-09-22 2021-09-22 一种可调节的sins、dvl、usbl组合导航方法

Country Status (1)

Country Link
CN (1) CN113959434B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116026324B (zh) * 2023-02-10 2023-06-20 北京大学 用于水-空跨介质航行器的跨域导航***和方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6449559B2 (en) * 1998-11-20 2002-09-10 American Gnc Corporation Fully-coupled positioning process and system thereof
CN102829777B (zh) * 2012-09-10 2015-09-16 江苏科技大学 自主式水下机器人组合导航***及方法
CN107024226B (zh) * 2016-02-01 2021-03-16 北京自动化控制设备研究所 一种基于惯导/dvl/usbl组合的惯导误差估计方法
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法
CN110006433A (zh) * 2019-04-22 2019-07-12 哈尔滨工程大学 海底油气管检测机器人的组合导航定位***及方法
CN111829512B (zh) * 2020-06-08 2024-04-09 中国航天空气动力技术研究院 一种基于多传感器数据融合的auv导航定位方法及***
CN112556697A (zh) * 2020-12-08 2021-03-26 江苏科技大学 一种基于联邦结构的浅耦合数据融合导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种基于集中滤波的SINS/DVL/USBL水下组合导航算法;张亚文 等;《导航定位与授时》;20170131;全文 *
联邦滤波器的SINS/GNS/DVS水下组合导航;刘明雍;周志远;赵涛;;火力与指挥控制;20091215(第12期);全文 *

Also Published As

Publication number Publication date
CN113959434A (zh) 2022-01-21

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
WO2020062791A1 (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN106443746B (zh) 一种低成本双天线gnss/ahrs组合测姿方法
CN109782323B (zh) 一种深海水下自主航行器导航定位及校准方法
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN102829777B (zh) 自主式水下机器人组合导航***及方法
CN102508275B (zh) 多天线gps/gf-ins深度组合定姿方法
US6860023B2 (en) Methods and apparatus for automatic magnetic compensation
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航***
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
CN102538792B (zh) 一种位置姿态***的滤波方法
CN102169184B (zh) 组合导航***中测量双天线gps安装失准角的方法和装置
CN106643709B (zh) 一种海上运载体的组合导航方法及装置
Wang et al. Quadratic extended Kalman filter approach for GPS/INS integration
CN104181574A (zh) 一种捷联惯导***/全球导航卫星***组合导航滤波***及方法
CN107797125B (zh) 一种减小深海探测型auv导航定位误差的方法
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN103712621B (zh) 偏振光及红外传感器辅助惯导***定姿方法
CN104931994A (zh) 一种基于软件接收机的分布式深组合导航方法及***
CN104061930B (zh) 一种基于捷联惯性制导和多普勒计程仪的导航方法
CN113959434B (zh) 一种可调节的sins、dvl、usbl组合导航方法
CN112525204B (zh) 一种航天器惯性和太阳多普勒速度组合导航方法
Allotta et al. Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements

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