CN116772903B - 基于迭代ekf的sins/usbl安装角估计方法 - Google Patents

基于迭代ekf的sins/usbl安装角估计方法 Download PDF

Info

Publication number
CN116772903B
CN116772903B CN202311031478.6A CN202311031478A CN116772903B CN 116772903 B CN116772903 B CN 116772903B CN 202311031478 A CN202311031478 A CN 202311031478A CN 116772903 B CN116772903 B CN 116772903B
Authority
CN
China
Prior art keywords
matrix
usbl
iterative
state
sins
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
CN202311031478.6A
Other languages
English (en)
Other versions
CN116772903A (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202311031478.6A priority Critical patent/CN116772903B/zh
Publication of CN116772903A publication Critical patent/CN116772903A/zh
Application granted granted Critical
Publication of CN116772903B publication Critical patent/CN116772903B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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
    • 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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Navigation (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明公开了一种基于迭代EKF（扩展卡尔曼滤波）的SINS/USBL安装角估计方法，属于水下机器人导航技术领域。本发明将迭代EKF算法用于SINS/USBL安装角估计方法中，整个过程主要步骤包括：模型构建、基于迭代EKF的状态更新、基于LM算法的预测协方差矩阵优化，量测噪声方差阵优化，系统噪声方差阵优化以及基于迭代EKF的量测更新。本发明可以解决由于水下机器人捷联惯性导航系统与超短基线定位系统同载体的坐标系不同，而导致水下机器人导航精度下降的问题，提高估计SINS/USBL安装角的能力。相比于传统SINS/USBL安装角估计方法，本方法具有较高的鲁棒性和估计精度。

Description

基于迭代EKF的SINS/USBL安装角估计方法
技术领域
本发明属于水下机器人导航定位技术领域，具体涉及基于迭代EKF的SINS/USBL安装角估计方法。
背景技术
随着人类对资源的需求愈加剧烈，陆地资源已经无法满足人类的发展需要，海洋中蕴含着巨大的资源，是一座宝库。但是人工探索海洋成本高，危险性大，而水下自主潜航器（Autonomous Underwater Vehicle, AUV）的出现可以代替人工，对海洋环境进行探索开发。
AUV的运行需要时刻了解自身的位置，速度，姿态信息，但在水下GPS信号无法有效传递，而捷联惯性导航系统（Strapdown Inertial Navigation System, SINS）和超短基线定位系统（Ultra-short Baseline, USBL）可以帮助机器人进行水下的导航定位。由于SINS和USBL的设备安装存在偏差，会导致导航系统的精度下降，甚至会威胁AUV的航行安全，需要对二者的安装误差角进行估计，来提高导航系统的有效性，因此准确估计安装误差角是研究SINS/USBL导航系统的重点。同时，水下机器人航行时的环境变化对安装角估计技术也会造成较大影响。
发明内容
针对上述问题，本发明提出基于迭代EKF（扩展卡尔曼滤波）的SINS/USBL安装角估计方法，进一步提高了算法的收敛性和估计精度。
为达到上述目的，本发明的技术方案如下：
基于迭代EKF的SINS/USBL安装角估计方法，该方法基于水上/水下载体、应答器、以及安装在所述载体上的一体化装置，所述一体化装置上安装有IMU（惯性测量单元）、SINS（捷联惯性导航系统）、USBL（超短基线系统）；其中USBL输出信息，/>和/>分别表示USBL输出的两个方位角信息，/>表示USBL输出的斜距信息；用上标/>表示导航坐标系，简称为/>系，应答器在/>系下的绝对位置/>通过长基线的方式获得；GPS实时测量的卫星天线安装点在/>系下的绝对位置/>已知；载体姿态/>由IMU测量得到， />、/>、/>分别表示横摇角、纵摇角和航向角；该方法具体包括如下步骤：
S1.对水下机器人导航系统模型进行构建；
S2.使用基于迭代的扩展卡尔曼滤波算法进行状态更新；
S3.使用LM算法优化调整迭代的扩展卡尔曼滤波算法的预测协方差矩阵；
S4.对量测噪声方差阵进行优化；
S5.对系统噪声方差阵进行优化；
S6.进行基于迭代的扩展卡尔曼滤波算法进行量测更新。
进一步地，步骤S1的具体方法是：
系统的状态方程为：
，
其中，是系统k时刻的状态，/>是系统k-1时刻的状态；/>为系统噪声矩阵，/>为系统噪声向量；/>为系统状态转移矩阵；/>；
将安装误差角与杆臂误差/>作为状态量，则状态方程表示为
，
其中，，/>、/>、/>分别表示x、y、z三轴下的安装误差角；，/>、/>、/>分别表示x、y、z三轴下的杆臂误差；
建立系统量测方程：
，
其中，为系统测量值，/>为量测的非线性函数矩阵,/>为量测噪声向量；
由于标定误差的观测方程有如下的非线性关系：
，
上式显示了载体位置、应答器位置/>、载体姿态/>、标定误差(/>)和USBL输出的方位角及斜距/>之间的关系；/>表示一体化装置的杆臂误差；用上标a表示声学基阵坐标系，简称a系，/>表示在a系下的三轴测量值；
通过USBL解算的IMU的位置表示为
，
，
其中，用下标b表示载体坐标系，简称b系，表示b系到/>系的姿态转移矩阵；/>是a系到b系的姿态转移矩阵；/>是根据USBL定位原理解算出应答器在a系中的相对位置；是根据USBL定位原理解算出应答器在u系中的相对位置；u系表示超短基线基座坐标系；考虑标定误差后，应答器在a系下的位置表示为
。
进一步地，步骤S2的具体方法是：
基于迭代的扩展卡尔曼滤波算法的时间更新公式如下：
，
其中，表示k-1时刻系统状态估计值，/>表示k时刻状态预测值；表示状态转移矩阵/>的一阶泰勒展开结果，/>为k-1时刻系统噪声方差阵；表示状态估计误差的均方误差阵；/>表示误差协方差矩阵的先验估计。
进一步地，步骤S3的具体方法是在步骤S2所述基于迭代的扩展卡尔曼滤波算法的每一次迭代过程中，寻找合适的阻尼参数来修正状态量的误差协方差矩阵，即对其进行修正的式子为：
，
，
其中，为修正后的状态量的误差协方差矩阵；I为单位矩阵；
然后再用这个经过修正后的状态量的误差协方差矩阵来参与后面的迭代更新。
进一步地，阻尼参数。
进一步地，步骤S4的具体方法是将Sage-Husa滤波原理中对量测噪声方差阵的优化思想引入到迭代的扩展卡尔曼滤波算法中，Sage-Husa自适应卡尔曼滤波算法的步骤如下：
，
，
，
其中，为k-1时刻量测噪声期望估计值；/>为k-1时刻量测噪声方差阵；为k时刻的观测矩阵；/>为残差；/>为量测噪声方差阵；/>为k时刻的量测噪声调节因子，且/>，/>为遗忘因子，且/>，上标T表示矩阵的转置。
进一步地，遗忘因子取值范围在之内。
进一步地，步骤S5所述对系统噪声方差阵进行优化，公式如下：
，
，
，
其中，为实际的运动状态协方差矩阵；/>为系统初始化的运动状态噪声协方差矩阵；/>为增量；/>表示卡尔曼滤波增益；/>为k时刻的观测矩阵；/>表示观测残差；/>为实际的误差协方差矩阵。
进一步地，步骤S6所述迭代的扩展卡尔曼滤波算法进行量测更新公式如下所示：
式中上角标i为量测更新迭代次数，、/>和/> 分别为第i次迭代的增益矩阵、状态估计值、状态估计协方差矩阵；/>表示第i+1次迭代的状态估计值；/>表示k时刻量测的非线性函数矩阵；对于非线性量测矩阵，/>为第i次迭代的量测矩阵，其表达式如下所示：
，
式中，表示量测非线性函数；/>表示状态向量。
与现有技术相比，本发明具有如下优点：
(1) 本发明针对SINS和USBL安装角估计问题，在传统EKF算法的基础上提出了迭代EKF方法，进一步提高了算法的估计精度;
(2) 本发明针对算法的预测协方差矩阵收敛速度不够的问题，引入了LM算法，提高了算法的收敛性和估计精度;
(3) 本发明针对滤波算法中噪声矩阵无法估计的问题，分别设计了量测噪声方差阵优化策略和系统噪声方差阵优化策略，进一步提高了复杂环境下系统的适应性。
附图说明
图1为本发明所描述的基于迭代EKF的SINS/USBL安装角估计方法流程图。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图1所示，本实施例的基于迭代EKF的SINS/USBL安装角估计方法，该方法基于水上/水下载体、应答器、以及安装在所述载体上的一体化装置，所述一体化装置上安装有IMU（微机电系统）、SINS、USBL；其中USBL输出信息，/>和/>分别表示USBL输出的两个方位角信息，/>表示USBL输出的斜距信息；用上标/>表示导航坐标系，简称为/>系，应答器在/>系下的绝对位置/>通过长基线的方式获得；GPS实时测量的卫星天线安装点在/>系下的绝对位置/>已知；载体姿态/>由IMU测量得到， />、/>、/>分别表示横摇角、纵摇角和航向角；该方法具体包括如下步骤：
S1.对水下机器人导航系统模型进行构建；
具体方法是：
系统的状态方程为：
，
其中，是系统k时刻的状态，/>是系统k-1时刻的状态；/>为系统噪声矩阵，/>为系统噪声向量；/>为系统状态转移矩阵；/>；
将安装误差角与杆臂误差/>作为状态量，则状态方程表示为
，
其中，，/>、/>、/>分别表示x、y、z三轴下的安装误差角；，/>、/>、/>分别表示x、y、z三轴下的杆臂误差；
建立系统量测方程：
，
其中，为系统测量值，/>为量测的非线性函数矩阵,/>为量测噪声向量；
由于标定误差的观测方程有如下的非线性关系：
，
上式显示了载体位置、应答器位置/>、载体姿态/>、标定误差(/>)和USBL输出的方位角及斜距/>之间的关系；/>表示一体化装置的杆臂误差；用上标a表示声学基阵坐标系，简称a系，/>表示在a系下的三轴测量值；
通过USBL解算的IMU的位置表示为
，
，
其中，用下标b表示载体坐标系，简称b系，表示b系到/>系的姿态转移矩阵；/>是a系到b系的姿态转移矩阵；/>是根据USBL定位原理解算出应答器在a系中的相对位置；是根据USBL定位原理解算出应答器在u系中的相对位置；u系表示超短基线基座坐标系；考虑标定误差后，应答器在a系下的位置表示为
。
S2.使用基于迭代的扩展卡尔曼滤波算法进行状态更新；更新公式如下：
，
其中，表示k-1时刻系统状态估计值，/>表示k时刻状态预测值；表示状态转移矩阵/>的一阶泰勒展开结果，/>为k-1时刻系统噪声方差阵；表示状态估计误差的均方误差阵；/>表示误差协方差矩阵的先验估计。
S3.使用LM算法优化调整迭代的扩展卡尔曼滤波算法的预测协方差矩阵；
本实施例中采用LM算法来优化迭代EKF算法。利用LM算法优化调整迭代EKF算法的预测协方差矩阵，以保证算法的估算结果误差具有收敛性。
主要思想就是在迭代EKE算法的每一次迭代过程中，寻找合适的阻尼参数来修正状态量的误差协方差矩阵。即对其进行修正的式子为：
，
，
其中，为修正后的状态量的误差协方差矩阵；I为单位矩阵；
然后再用这个经过修正后的状态量的误差协方差矩阵来参与后面的迭代更新。本实施例中阻尼参数/>。
S4.对量测噪声方差阵进行优化；
本实施例将Sage-Husa滤波原理中对量测噪声方差阵的优化思想引入到迭代的扩展卡尔曼滤波算法中，Sage-Husa自适应卡尔曼滤波算法的步骤如下：
，
，
，
其中，为k-1时刻量测噪声期望估计值；/>为k-1时刻量测噪声方差阵；为k时刻的观测矩阵；/>为残差；/>为量测噪声方差阵；/>为k时刻的量测噪声调节因子，且/>，/>为遗忘因子，且/>，上标T表示矩阵的转置。本实施例中遗忘因子取值范围在/>之内。
S5.对系统噪声方差阵进行优化；
考虑到系统状态噪声往往会随着运动位置的变化变得不确定。本实施例在上述量测噪声方差阵优化的基础上提出一种系统噪声方差阵的优化方法，优化公式如下：
，
，
，
其中，为实际的运动状态协方差矩阵；/>为系统初始化的运动状态噪声协方差矩阵；/>为增量；/>表示卡尔曼滤波增益；/>为k时刻的观测矩阵；/>表示观测残差；/>为实际的误差协方差矩阵。
S6.进行基于迭代的扩展卡尔曼滤波算法进行量测更新，更新公式如下所示：
，
式中上角标i为量测更新迭代次数，、/>和/> 分别为第i次迭代的增益矩阵、状态估计值、状态估计协方差矩阵；/>表示第i+1次迭代的状态估计值；/>表示k时刻量测的非线性函数矩阵；对于非线性量测矩阵，/>为第i次迭代的量测矩阵，其表达式如下所示：
，
式中，表示量测非线性函数；/>表示状态向量。
为了验证本发明方法的有效性，开展基于迭代EKF的SINS/USBL安装角估计方法实验。试验设备包括：试验船、IMU设备、USBL设备等。试验结果如表1和表2所示。
表1不同方法的安装角估计结果
表2不同方法的杆臂估计结果
其中表1为不同方法的安装角估计结果，从表1中可以看出，在安装角估计结果来看，本发明方法在x, y, z三个方向的安装角估计结果均优于EKF方法，本发明的安装角估计结果更接近真值。表2为不同方法的杆臂估计结果，从表2中可以看出，在杆臂估计结果来看，本发明方法在x, y, z三个方向的杆臂估计结果均优于EKF方法，本发明的杆臂估计结果更接近真值。
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (8)

1.一种基于迭代EKF的SINS/USBL安装角估计方法，该方法基于水上/水下载体、应答器、以及安装在所述载体上的一体化装置，所述一体化装置上安装有IMU、SINS、USBL；其中USBL输出信息[α β R]，α和β分别表示USBL输出的两个方位角信息，R表示USBL输出的斜距信息；用上标n表示导航坐标系，简称为n系，应答器在n系下的绝对位置通过长基线的方式获得；GPS实时测量的卫星天线安装点在n系下的绝对位置/>已知；载体姿态由IMU测量得到，γ、η、/>分别表示横摇角、纵摇角和航向角；其特征在于，该方法具体包括如下步骤：
S1.对水下机器人导航系统模型进行构建；
S2.使用基于迭代的扩展卡尔曼滤波算法进行状态更新；
S3.使用LM算法优化调整迭代的扩展卡尔曼滤波算法的预测协方差矩阵Pk；
S4.对量测噪声方差阵Rk进行优化；
S5.对系统噪声方差阵Qk进行优化；
S6.进行基于迭代的扩展卡尔曼滤波算法进行量测更新；
步骤S3的具体方法是在步骤S2所述基于迭代的扩展卡尔曼滤波算法的每一次迭代过程中，寻找合适的阻尼参数λi来修正状态量的误差协方差矩阵，即对其进行修正的式子为：
其中，为修正后的状态量的误差协方差矩阵；I为单位矩阵，Pk/k-1表示误差协方差矩阵的先验估计；
然后再用这个经过修正后的状态量的误差协方差矩阵来参与后面的迭代更新。
2.根据权利要求1所述的基于迭代EKF的SINS/USBL安装角估计方法，其特征在于，步骤S1的具体方法是：
系统的状态方程为：
Xk＝Fk-1Xk-1+Gk-1Wk-1
其中，Xk是系统k时刻的状态，Xk-1是系统k-1时刻的状态；Gk-1为系统噪声矩阵，Wk-1为系统噪声向量；Fk-1为系统状态转移矩阵，Fk-1＝06×6；
将安装误差角θ与杆臂误差PL作为状态量，则状态方程表示为
Xk＝[θ PL]＝[θx θy θz Lx Ly Lz]
其中，θ＝[θx θy θz]，θx、θy、θz分别表示x、y、z三轴下的安装误差角；PL＝[Lx Ly Lz]，Lx、Ly、Lz分别表示x、y、z三轴下的杆臂误差；
建立系统量测方程：
Zk＝h(Xk)+Vk
其中，Zk为系统测量值，h(Xk)为量测的非线性函数矩阵，Vk为量测噪声向量；
由于标定误差的观测方程有如下的非线性关系：
上式显示了载体位置应答器位置/>载体姿态Θ、标定误差(θ，PL)和USBL输出的方位角及斜距[α β R]之间的关系；PL表示一体化装置的杆臂误差；用上标a表示声学基阵坐标系，简称a系，xa，ya，za表示在a系下的三轴测量值；
通过USBL解算的IMU的位置表示为
其中，用下标b表示载体坐标系，简称b系，表示b系到n系的姿态转移矩阵；/>是a系到b系的姿态转移矩阵；/>是根据USBL定位原理解算出应答器在a系中的相对位置；/>是根据USBL定位原理解算出应答器在u系中的相对位置；u系表示超短基线基座坐标系；考虑标定误差后，应答器在a系下的位置表示为
3.根据权利要求l所述的基于迭代EKF的SINS/USBL安装角估计方法，其特征在于，步骤S2的具体方法是：
基于迭代的扩展卡尔曼滤波算法的时间更新公式如下：
其中，表示k-1时刻系统状态估计值，/>表示k时刻状态预测值；Φk/k-1表示状态转移矩阵Γk-1的一阶泰勒展开结果，Qk-1为k-1时刻系统噪声方差阵；Pk表示状态估计误差的均方误差阵；Pk/k-1表示误差协方差矩阵的先验估计。
4.根据权利要求l所述的基于迭代EKF的SINS/USBL安装角估计方法，其特征在于，阻尼参数λi＝0.85。
5.根据权利要求2所述的基于迭代EKF的SINS/USBL安装角估计方法，其特征在于，步骤S4的具体方法是将Sage-Husa滤波原理中对量测噪声方差阵的优化思想引入到迭代的扩展卡尔曼滤波算法中，Sage-Husa自适应卡尔曼滤波算法的步骤如下：
其中，为k-1时刻量测噪声期望估计值；/>为k-1时刻量测噪声方差阵；Hk为k时刻的观测矩阵；ek为残差；Rk为量测噪声方差阵；dk为k时刻的量测噪声调节因子，且dk＝(1-m)/(1-mk+1)，m为遗忘因子，且0＜m＜1，上标T表示矩阵的转置。
6.根据权利要求5所述的基于迭代EKF的SINS/USBL安装角估计方法，其特征在于，遗忘因子取值范围在[0.95，0.99]之内。
7.根据权利要求5所述的基于迭代EKF的SINS/USBL安装角估计方法，其特征在于，步骤S5所述对系统噪声方差阵Qk进行优化，公式如下：
其中，为实际的运动状态协方差矩阵；Q为系统初始化的运动状态噪声协方差矩阵；ΔQ为增量；K表示卡尔曼滤波增益；Hk为k时刻的观测矩阵；/>表示观测残差；/>为实际的误差协方差矩阵。
8.根据权利要求7所述的基于迭代EKF的SlNS/USBL安装角估计方法，其特征在于，步骤S6所述迭代的扩展卡尔曼滤波算法进行量测更新公式如下所示：
式中上角标i为量测更新迭代次数，和/>分别为第i次迭代的增益矩阵、状态估计值、状态估计协方差矩阵；/>表示第i+1次迭代的状态估计值；/>表示k时刻量测的非线性函数矩阵；对于非线性量测矩阵，/>为第i次迭代的量测矩阵，其表达式如下所示：
式中，h表示量测非线性函数；X表示状态向量。
CN202311031478.6A 2023-08-16 2023-08-16 基于迭代ekf的sins/usbl安装角估计方法 Active CN116772903B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311031478.6A CN116772903B (zh) 2023-08-16 2023-08-16 基于迭代ekf的sins/usbl安装角估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311031478.6A CN116772903B (zh) 2023-08-16 2023-08-16 基于迭代ekf的sins/usbl安装角估计方法

Publications (2)

Publication Number Publication Date
CN116772903A CN116772903A (zh) 2023-09-19
CN116772903B true CN116772903B (zh) 2023-10-20

Family

ID=88008320

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311031478.6A Active CN116772903B (zh) 2023-08-16 2023-08-16 基于迭代ekf的sins/usbl安装角估计方法

Country Status (1)

Country Link
CN (1) CN116772903B (zh)

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101820269A (zh) * 2010-04-08 2010-09-01 西北工业大学 迭代平方根中心差分卡尔曼粒子滤波方法
WO2012044964A2 (en) * 2010-10-01 2012-04-05 Hillcrest Laboratories, Inc. Apparatuses and methods for estimating the yaw angle of a device in a gravitational reference system using measurements of motion sensors and a magnetometer attached to the device
CN105654053A (zh) * 2015-12-29 2016-06-08 河海大学 基于改进约束ekf算法的动态振荡信号参数辨识方法
CN105928518A (zh) * 2016-04-14 2016-09-07 济南大学 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法
KR101755307B1 (ko) * 2016-05-11 2017-07-10 한국해양과학기술원 수중 이동체의 위치 측정 오차 보정 방법
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109782289A (zh) * 2018-12-26 2019-05-21 中国电子科技集团公司第二十研究所 一种基于基线几何结构约束的水下航行器定位方法
CN110098875A (zh) * 2019-05-07 2019-08-06 北京邮电大学 光纤通信系统中自适应均衡方法、装置、电子设备及介质
CN112003527A (zh) * 2020-07-22 2020-11-27 西安理工大学 用于异步电机转速辨识的迭代扩展卡尔曼滤波的改进方法
CN112034735A (zh) * 2020-08-31 2020-12-04 浙江大学 一种多auv水下协同作业的模拟实验平台
CN113325452A (zh) * 2021-05-25 2021-08-31 哈尔滨工程大学 一种三星无源融合定位体制机动目标跟踪方法
CN113671377A (zh) * 2020-05-14 2021-11-19 金华职业技术学院 一种基于改进ekf算法的锂电池soc估算方法
CN115307643A (zh) * 2022-08-24 2022-11-08 东南大学 一种双应答器辅助的sins/usbl组合导航方法
CN115371705A (zh) * 2022-08-31 2022-11-22 哈尔滨工程大学 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法
CN116380067A (zh) * 2023-03-08 2023-07-04 东南大学 一种适应于挑战环境下的无人艇旋转调制惯导系统及方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7643939B2 (en) * 2006-03-08 2010-01-05 Honeywell International Inc. Methods and systems for implementing an iterated extended Kalman filter within a navigation system
US9709404B2 (en) * 2015-04-17 2017-07-18 Regents Of The University Of Minnesota Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation
US11466990B2 (en) * 2016-07-22 2022-10-11 Regents Of The University Of Minnesota Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system
US20190204430A1 (en) * 2017-12-31 2019-07-04 Woods Hole Oceanographic Institution Submerged Vehicle Localization System and Method
FR3106885B1 (fr) * 2020-02-03 2021-12-24 Safran Procede d’aide à la navigation d’un porteur mobile

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101820269A (zh) * 2010-04-08 2010-09-01 西北工业大学 迭代平方根中心差分卡尔曼粒子滤波方法
WO2012044964A2 (en) * 2010-10-01 2012-04-05 Hillcrest Laboratories, Inc. Apparatuses and methods for estimating the yaw angle of a device in a gravitational reference system using measurements of motion sensors and a magnetometer attached to the device
CN105654053A (zh) * 2015-12-29 2016-06-08 河海大学 基于改进约束ekf算法的动态振荡信号参数辨识方法
CN105928518A (zh) * 2016-04-14 2016-09-07 济南大学 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法
KR101755307B1 (ko) * 2016-05-11 2017-07-10 한국해양과학기술원 수중 이동체의 위치 측정 오차 보정 방법
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109782289A (zh) * 2018-12-26 2019-05-21 中国电子科技集团公司第二十研究所 一种基于基线几何结构约束的水下航行器定位方法
CN110098875A (zh) * 2019-05-07 2019-08-06 北京邮电大学 光纤通信系统中自适应均衡方法、装置、电子设备及介质
CN113671377A (zh) * 2020-05-14 2021-11-19 金华职业技术学院 一种基于改进ekf算法的锂电池soc估算方法
CN112003527A (zh) * 2020-07-22 2020-11-27 西安理工大学 用于异步电机转速辨识的迭代扩展卡尔曼滤波的改进方法
CN112034735A (zh) * 2020-08-31 2020-12-04 浙江大学 一种多auv水下协同作业的模拟实验平台
CN113325452A (zh) * 2021-05-25 2021-08-31 哈尔滨工程大学 一种三星无源融合定位体制机动目标跟踪方法
CN115307643A (zh) * 2022-08-24 2022-11-08 东南大学 一种双应答器辅助的sins/usbl组合导航方法
CN115371705A (zh) * 2022-08-31 2022-11-22 哈尔滨工程大学 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法
CN116380067A (zh) * 2023-03-08 2023-07-04 东南大学 一种适应于挑战环境下的无人艇旋转调制惯导系统及方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
彭雅奇 ; 许承东 ; 李臻 ; 赵靖 ; .MIEKF优化算法在GPS定位测速中的应用.计算机仿真.2018,(第07期),全文. *
徐博 ; 郝芮 ; 王超 ; 张勋 ; .基于倒置声学基阵的INSUSBL组合导航算法研究.海洋技术学报.2017,(第05期),全文. *
徐博等.基于IEKF的SINS/USBL组合导航系统安装偏差标定算法.中国惯性技术学报.2022,第30卷(第4期),全文. *

Also Published As

Publication number Publication date
CN116772903A (zh) 2023-09-19

Similar Documents

Publication Publication Date Title
CN110057365B (zh) 一种大潜深auv下潜定位方法
CN110514203B (zh) 一种基于isr-ukf的水下组合导航方法
CN110132308B (zh) 一种基于姿态确定的usbl安装误差角标定方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN104567930A (zh) 一种能够估计和补偿机翼挠曲变形的传递对准方法
CN114061591B (zh) 一种基于滑动窗数据回溯的等值线匹配方法
CN112068440B (zh) 基于模型预测控制的auv回收对接动力定位控制方法
Xu et al. A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles
CN112284384A (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
Bo et al. Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation
CN111928850A (zh) 适用于极地冰架下环境的自主水下机器人的组合导航方法
Troni et al. Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles
CN114265047B (zh) 一种大潜深auv的定位基阵联合标校方法
Jameian et al. A robust and fast self-alignment method for strapdown inertial navigation system in rough sea conditions
CN108460210A (zh) 一种基于噪残差和协方差匹配的动力定位系统噪声特性实时估计方法
Xu et al. USBL positioning system based adaptive kalman filter in AUV
CN109974695B (zh) 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
CN115096302A (zh) 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端
CN114777812A (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN116772903B (zh) 基于迭代ekf的sins/usbl安装角估计方法
Yu In-situ calibration of transceiver alignment for a high-precision USBL system
Wang et al. AUV navigation based on inertial navigation and acoustic positioning systems
Chen The estimation of angular misalignments for ultra short baseline navigation systems. Part I: Numerical simulations
Zhang et al. A robust calibration method for the underwater transponder position based on Gauss-Newton iteration algorithm

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