CN116182871A - 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 - Google Patents

一种基于二阶混合滤波的海缆探测机器人姿态估计方法 Download PDF

Info

Publication number
CN116182871A
CN116182871A CN202310460963.9A CN202310460963A CN116182871A CN 116182871 A CN116182871 A CN 116182871A CN 202310460963 A CN202310460963 A CN 202310460963A CN 116182871 A CN116182871 A CN 116182871A
Authority
CN
China
Prior art keywords
accelerometer
information
matrix
gyroscope
magnetometer
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
CN202310460963.9A
Other languages
English (en)
Other versions
CN116182871B (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 CN202310460963.9A priority Critical patent/CN116182871B/zh
Publication of CN116182871A publication Critical patent/CN116182871A/zh
Application granted granted Critical
Publication of CN116182871B publication Critical patent/CN116182871B/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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

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

Abstract

本发明公开了一种基于二阶混合滤波的海缆探测机器人姿态估计方法，属于水下机器人位姿估计技术领域，该方法基于陀螺输出的设备系下的三维角速度信息和加速度计输出的设备系下的三维角加速度信息以及外接磁力计输出设备系下的磁场信息，首先对陀螺及加速度计输出信息使用差分法进行预处理；然后使用预处理得到的陀螺和加速度计信息，进行对陀螺与加速度计的噪声自适应滤波处理，并使用加速度计修正俯仰角和横滚角；最后使用拉普拉斯滤波算法对磁力计数据进行去噪，并使用磁力计修正偏航角。本方法能解决水下机器人姿态估计和角度修正问题。

Description

一种基于二阶混合滤波的海缆探测机器人姿态估计方法
技术领域
本发明属于水下机器人位姿估计技术领域，具体涉及一种基于二阶混合滤波的海缆探测机器人姿态估计方法。
背景技术
近年来随着海底勘探，水下作业的重要性越发突出，水下机器人是水下作业的重要载体，且体积小，灵活性高，隐蔽性强，因此在水下作业，水底勘探等领域发挥着重大的作用。
水下机器人的运行和操作离不开精准实时的姿态信息，传统的姿态估计***的主要误差来源是不准确的角估计，因此想要获得精确地位姿信息，建立高精度的角估计***是重中之重，而角估计***的建立需要多种传感器的配合，微电子惯性测量单元（MEMS）可以获得水下机器人的姿态信息，但是噪声较大，其中陀螺仪动态响应快，测量精度高，但是存在积分漂移问题，同时加速度计测量时噪声很大，磁力计十分易受外界干扰。因此由单一传感器无法精确地获取水下机器人的姿态信息，所以需要一种能够融合多种传感器的姿态估计算法，能够综合多个传感器的优点，为水下机器人的运行提供准确的位姿信息。
加速度计和磁力计在修正角位置方面各有作用，但是当只以加速度计作为固定参考时，只能修正横滚角和俯仰角，对偏航角无法进行准确的修正，相反只以磁力计为固定参考时也只能对偏航角进行有效修正。
针对上述问题，本发明提出一种基于二阶混合滤波的姿态估计算法。首先，对陀螺仪与加速度计数据进行预处理，挑选出异常值，并从测量数据中剔除；其次，使用噪声自适应滤波算法消除陀螺仪与加速度计的噪声，并以加速度计为固定参考，修正横滚角和俯仰角；最后，使用拉普拉斯滤波算法对磁力计进行抗干扰处理，并以磁力计为固定参考修正偏航角。
发明内容
针对上述问题，本发明提出一种基于二阶混合滤波的海缆探测机器人姿态估计方法，以微机电***惯性测量单元（缩写MEMS IMU）的输出信息作为计算基础，融合陀螺仪、加速度计和磁力计的优点，并采用改进的算法，解决水下机器人姿态估计和角度修正问题。
为达到上述目的，本发明的技术方案如下：
一种基于二阶混合滤波的海缆探测机器人姿态估计方法，该方法基于已知量：
MEMS IMU陀螺输出的设备系下的三维角速度信息
Figure SMS_1
，其中
Figure SMS_2
分别表示x方向、y方向、z方向的角速度信息，上标T表示矩阵的转置；
MEMS IMU加速度计输出的设备系下的三维角加速度信息
Figure SMS_3
；其中
Figure SMS_4
分别表示x方向、y方向、z方向的加速度信息；/>
外接磁力计输出设备系下的磁场信息
Figure SMS_5
，其中
Figure SMS_6
分别表示x方向、y方向、z方向的磁场信息；
其特征在于，该方法具体包括如下步骤：
步骤1、对陀螺及加速度计输出信息
Figure SMS_7
使用差分法进行预处理；
步骤2、使用步骤1预处理得到的陀螺和加速度计信息，进行对陀螺与加速度计的噪声自适应滤波处理，并使用加速度计修正俯仰角和横滚角；
步骤3、使用拉普拉斯滤波算法对磁力计数据进行去噪，并使用磁力计修正偏航角。
进一步地，步骤1具体为：使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值，当输出数据超出该阈值时剔除，当输出数据在阈值之内时保留，经过处理后可得到一系列陀螺及加速度计信息，分别为：
Figure SMS_8
其中，
Figure SMS_9
表示经过处理后的k时刻的陀螺角速度信息，其中/>
Figure SMS_10
分别表示经过处理后的k时刻x方向、y方向、z方向的角速度信息；/>
Figure SMS_11
表示经过处理后的k时刻的加速度计的角加速度信息，其中/>
Figure SMS_12
分别表示经过处理后的k时刻x方向、y方向、z方向的加速度信息。
进一步地，步骤2具体为：首先使用四元数q表示角位置：
Figure SMS_13
其中第一矢量部分
Figure SMS_16
为实数，表示旋转角/>
Figure SMS_17
；/>
Figure SMS_19
分别表示x方向、y方向、z方向的旋转轴；所以/>
Figure SMS_15
与四元数q中的第二矢量部分q 1配合，表示围绕x方向旋转轴旋转的横滚角；/>
Figure SMS_18
与第三矢量部分q 2配合表示围绕y方向旋转轴旋转的俯仰角；与第四矢量部分q 3配合表示围绕z方向旋转轴旋转的偏航角；令向量/>
Figure SMS_20
，则/>
Figure SMS_21
与/>
Figure SMS_14
的关系由下式确定：
Figure SMS_22
其次对陀螺与加速度计的噪声进行自适应滤波处理，设置长度为N的滑动窗，噪声自适应滤波算法如下：
Figure SMS_23
其中，q k 为为k时刻的***状态，q k-1为k-1时刻的***状态；A为状态转移矩阵；q k,k-1为k时刻的状态预测值；
Figure SMS_28
为新息，是滑动窗内各态历经的白噪声序列；/>
Figure SMS_25
为/>
Figure SMS_31
时刻的新息；/>
Figure SMS_27
为加速度计测量值；/>
Figure SMS_33
为观测矩阵，使用加速度计和磁力计修正角位置时分别为/>
Figure SMS_36
；/>
Figure SMS_39
为计算量测噪声矩阵的信息协方差阵，/>
Figure SMS_34
为计算k-1时刻***噪声矩阵的信息协方差矩阵；/>
Figure SMS_37
为卡尔曼滤波增益；/>
Figure SMS_24
为一步预测均方差矩阵；/>
Figure SMS_30
为误差协方差矩阵，/>
Figure SMS_29
为上一时刻的误差协方差矩阵；/>
Figure SMS_32
为陀螺测量值与状态预测值之差；/>
Figure SMS_35
为计算***噪声矩阵的信息协方差矩阵；/>
Figure SMS_38
为量测噪声协方差矩阵；/>
Figure SMS_26
为***噪声协方差矩阵；
观测矩阵
Figure SMS_40
，使用加速度计进行一阶修正时为H 1，A与H 1由下式得出：
Figure SMS_41
其中I为单位矩阵；
Figure SMS_42
为旋转矩阵；D为算法执行间的时间间隔；
由于加速度计只能对俯仰角和横滚角进行有效修正，所以将四元数中表示偏航角的部分置零：
Figure SMS_43
其中
Figure SMS_44
表示经加速度计修正后的角位置；/>
Figure SMS_45
表示四元数
Figure SMS_46
中的第一矢量，第二矢量，第三矢量和第四矢量部分，迭代后得到经加速度计修正后的角位置/>
Figure SMS_47
。
进一步地，步骤3具体为：针对磁力计易受外界磁场干扰而噪声很大的问题，使用拉普拉斯滤波算法对磁力计数据进行去噪，具体步骤如下：
Figure SMS_48
其中，
Figure SMS_50
拉普拉斯滤波处理后的磁力计修正时的量测噪声矩阵；/>
Figure SMS_53
为无野值干扰下测量噪声的协方差矩阵；/>
Figure SMS_55
为对角阵；/>
Figure SMS_51
为辅助变量，/>
Figure SMS_52
；/>
Figure SMS_54
为磁力计修正时的卡尔曼滤波增益；/>
Figure SMS_56
为一个极小的常数，如/>
Figure SMS_49
；
使用磁力计修正偏航角过程是将四元数中表示横滚角和俯仰角的部分置零：
Figure SMS_57
其中
Figure SMS_58
表示经磁力计修正后的角位置；/>
Figure SMS_59
表示四元数
Figure SMS_60
中的第一矢量，第二矢量，第三矢量和第四矢量部分，迭代后得到经过磁力计修正后的角位置/>
Figure SMS_61
。
与现有技术相比，本发明具有如下优点：
(1) 本发明针对加速度计只能修正俯仰角与横滚角，磁力计只能修正偏航角的问题，提出一种二阶混合滤波算法，可以融合两个传感器的优点，并且通过二阶滤波分离磁异常对横滚角和俯仰角估计精度的影响，对全方位角进行精确修正。
(2) 本发明针对陀螺仪和加速度计测量时噪声很大的问题，引入了噪声自适应滤波算法，通过实时更新量测噪声矩阵，减小陀螺仪与加速度噪声，进一步提高了姿态估计的准确性。
(3) 本发明针对磁力计使用时易受外界磁场影响的问题，引入了拉普拉斯滤波算法，减小磁力计输出误差，提高了磁力计修正角位置时的精度。
附图说明
图1为本发明所描述的一种基于二阶混合滤波的姿态估计算法流程图；
图2为步骤1陀螺及加速度计信息差分预处理流程图；
图3为实验过程中采集的三轴陀螺仪和三轴加速度计原始输出信息；
图4为不同方法姿态估计结果对比曲线。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明的一种基于二阶混合滤波的海缆探测机器人姿态估计方法，该方法基于已知量：
MEMS IMU陀螺输出的设备系下的三维角速度信息
Figure SMS_62
，其中
Figure SMS_63
分别表示x方向、y方向、z方向的角速度信息，上标T表示矩阵的转置；
MEMS IMU加速度计输出的设备系下的三维角加速度信息
Figure SMS_64
；其中
Figure SMS_65
分别表示x方向、y方向、z方向的加速度信息；
外接磁力计输出设备系下的磁场信息
Figure SMS_66
，其中
Figure SMS_67
分别表示x方向、y方向、z方向的磁场信息；
该方法具体包括如下步骤：
步骤1、为了精确计算***和量测噪声方差阵
Figure SMS_68
，需要对陀螺及加速度计输出信息/>
Figure SMS_69
使用差分法进行预处理；使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值，当输出数据超出该阈值时剔除，当输出数据在阈值之内时保留，经过处理后可得到一系列陀螺及加速度计信息，分别为：
Figure SMS_70
其中，
Figure SMS_71
表示经过处理后的k时刻的陀螺角速度信息，其中/>
Figure SMS_72
分别表示经过处理后的k时刻x方向、y方向、z方向的角速度信息；/>
Figure SMS_73
表示经过处理后的k时刻的加速度计的角加速度信息，其中/>
Figure SMS_74
分别表示经过处理后的k时刻x方向、y方向、z方向的加速度信息。
本实施例中所述使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值的方法是，首先，当前时刻的陀螺仪信息和上时刻的陀螺仪信息做差；当前时刻的加速度计信息和上时刻的加速度计信息做差。其次，设定陀螺仪信息和加速度信息阈值（阈值为：t-10s时刻到t时刻内采样数据的方差值。原则：因为方差能够反应数据的波动程度，因此，差分处理后的数据和方差值对比，便能反应数据波动程度），将差值分别和阈值作比较；最后，当差值大于阈值时剔除该时刻的陀螺仪或者加速度信息，否则保留。
步骤2、使用步骤1预处理得到的陀螺和加速度计信息，进行对陀螺与加速度计的噪声自适应滤波处理，并使用加速度计修正俯仰角和横滚角；
首先使用四元数q表示角位置：
Figure SMS_75
其中第一矢量部分
Figure SMS_77
为实数，表示旋转角/>
Figure SMS_79
；/>
Figure SMS_81
分别表示x方向、y方向、z方向的旋转轴；所以/>
Figure SMS_78
与四元数q中的第二矢量部分q 1配合，表示围绕x方向旋转轴旋转的横滚角；/>
Figure SMS_80
与第三矢量部分q 2配合表示围绕y方向旋转轴旋转的俯仰角；与第四矢量部分q 3配合表示围绕z方向旋转轴旋转的偏航角；令向量/>
Figure SMS_82
，则/>
Figure SMS_83
与/>
Figure SMS_76
的关系由下式确定：
Figure SMS_84
其次对陀螺与加速度计的噪声进行自适应滤波处理，设置长度为N的滑动窗，噪声自适应滤波算法如下：
Figure SMS_85
其中，q k 为为k时刻的***状态，q k-1为k-1时刻的***状态；A为状态转移矩阵；q k,k-1为k时刻的状态预测值；
Figure SMS_90
为新息，是滑动窗内各态历经的白噪声序列；/>
Figure SMS_88
为/>
Figure SMS_94
时刻的新息；/>
Figure SMS_89
为加速度计测量值；/>
Figure SMS_95
为观测矩阵，使用加速度计和磁力计修正角位置时分别为/>
Figure SMS_91
；/>
Figure SMS_97
为计算量测噪声矩阵的信息协方差阵，/>
Figure SMS_98
为计算k-1时刻***噪声矩阵的信息协方差矩阵；/>
Figure SMS_101
为卡尔曼滤波增益；/>
Figure SMS_86
为一步预测均方差矩阵；/>
Figure SMS_92
为误差协方差矩阵，/>
Figure SMS_93
为上一时刻的误差协方差矩阵；/>
Figure SMS_99
为陀螺测量值与状态预测值之差；/>
Figure SMS_96
为计算***噪声矩阵的信息协方差矩阵；/>
Figure SMS_100
为量测噪声协方差矩阵；/>
Figure SMS_87
为***噪声协方差矩阵；
观测矩阵
Figure SMS_102
，使用加速度计进行一阶修正时为H 1，A与H 1由下式得出：
Figure SMS_103
其中I为单位矩阵；
Figure SMS_104
为旋转矩阵；D为算法执行间的时间间隔；
由于加速度计只能对俯仰角和横滚角进行有效修正，所以将四元数中表示偏航角的部分置零：
Figure SMS_105
其中
Figure SMS_106
表示经加速度计修正后的角位置；/>
Figure SMS_107
表示四元数
Figure SMS_108
中的第一矢量，第二矢量，第三矢量和第四矢量部分，迭代后得到经加速度计修正后的角位置/>
Figure SMS_109
。
步骤3、使用拉普拉斯滤波算法对磁力计数据进行去噪，并使用磁力计修正偏航角。
针对磁力计易受外界磁场干扰而噪声很大的问题，使用拉普拉斯滤波算法对磁力计数据进行去噪，具体步骤如下：
Figure SMS_110
其中，
Figure SMS_113
拉普拉斯滤波处理后的磁力计修正时的量测噪声矩阵；/>
Figure SMS_115
为无野值干扰下测量噪声的协方差矩阵；/>
Figure SMS_117
为对角阵；/>
Figure SMS_112
为辅助变量，/>
Figure SMS_114
；/>
Figure SMS_116
为磁力计修正时的卡尔曼滤波增益；/>
Figure SMS_118
为一个极小的常数，如/>
Figure SMS_111
；
使用磁力计修正偏航角过程是将四元数中表示横滚角和俯仰角的部分置零：
Figure SMS_119
其中
Figure SMS_120
表示经磁力计修正后的角位置；/>
Figure SMS_121
表示四元数
Figure SMS_122
中的第一矢量，第二矢量，第三矢量和第四矢量部分，迭代后得到经过磁力计修正后的角位置/>
Figure SMS_123
。
为了验证本发明方法的有效性，开展基于二阶混合滤波的姿态估计实验。使用9轴传感器（MPU9250）作为传感设备，其中包括：三轴陀螺仪、三轴加速度计和三轴磁力计。其中图3为实验过程中采集的三轴陀螺仪和三轴加速度计原始输出信息。图4是对应的扩展卡尔曼滤波方法和本发明方法的姿态估计曲线，从图中可以看出，本发明方法的姿态结果稳定可靠，优于扩展卡尔曼滤波方法。
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (4)

1.一种基于二阶混合滤波的海缆探测机器人姿态估计方法，该方法基于已知量：
MEMS IMU陀螺输出的设备系下的三维角速度信息
Figure QLYQS_1
，其中
Figure QLYQS_2
分别表示x方向、y方向、z方向的角速度信息，上标T表示矩阵的转置；
MEMS IMU加速度计输出的设备系下的三维角加速度信息
Figure QLYQS_3
；其中
Figure QLYQS_4
分别表示x方向、y方向、z方向的加速度信息；
外接磁力计输出设备系下的磁场信息
Figure QLYQS_5
，其中/>
Figure QLYQS_6
分别表示x方向、y方向、z方向的磁场信息；
其特征在于，该方法具体包括如下步骤：
步骤1、对陀螺及加速度计输出信息
Figure QLYQS_7
使用差分法进行预处理；
步骤2、使用步骤1预处理得到的陀螺和加速度计信息，进行对陀螺与加速度计的噪声自适应滤波处理，并使用加速度计修正俯仰角和横滚角；
步骤3、使用拉普拉斯滤波算法对磁力计数据进行去噪，并使用磁力计修正偏航角。
2.根据权利要求1所述的一种基于二阶混合滤波的海缆探测机器人姿态估计方法，其特征在于，步骤1具体为：使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值，当输出数据超出该阈值时剔除，当输出数据在阈值之内时保留，经过处理后可得到一系列陀螺及加速度计信息，分别为：
Figure QLYQS_8
其中，
Figure QLYQS_9
表示经过处理后的k时刻的陀螺角速度信息，其中/>
Figure QLYQS_10
分别表示经过处理后的k时刻x方向、y方向、z方向的角速度信息；/>
Figure QLYQS_11
表示经过处理后的k时刻的加速度计的角加速度信息，其中 />
Figure QLYQS_12
分别表示经过处理后的k时刻x方向、y方向、z方向的加速度信息。
3.根据权利要求1所述的一种基于二阶混合滤波的海缆探测机器人姿态估计方法，其特征在于，步骤2具体为：首先使用四元数q表示角位置：
Figure QLYQS_13
其中第一矢量部分
Figure QLYQS_15
为实数，表示旋转角/>
Figure QLYQS_17
；/>
Figure QLYQS_19
分别表示x方向、y方向、z方向的旋转轴；所以/>
Figure QLYQS_16
与四元数q中的第二矢量部分q 1配合，表示围绕x方向旋转轴旋转的横滚角；/>
Figure QLYQS_18
与第三矢量部分q 2配合表示围绕y方向旋转轴旋转的俯仰角；与第四矢量部分q 3配合表示围绕z方向旋转轴旋转的偏航角；令向量/>
Figure QLYQS_20
，则/>
Figure QLYQS_21
与/>
Figure QLYQS_14
的关系由下式确定：
Figure QLYQS_22
/>
其次对陀螺与加速度计的噪声进行自适应滤波处理，设置长度为N的滑动窗，噪声自适应滤波算法如下：
Figure QLYQS_23
其中，q k 为为k时刻的***状态，q k-1为k-1时刻的***状态；A为状态转移矩阵；q k,k-1为k时刻的状态预测值；
Figure QLYQS_28
为新息，是滑动窗内各态历经的白噪声序列；/>
Figure QLYQS_27
为/>
Figure QLYQS_31
时刻的新息；/>
Figure QLYQS_26
为加速度计测量值；/>
Figure QLYQS_33
为观测矩阵，使用加速度计和磁力计修正角位置时分别为
Figure QLYQS_29
；/>
Figure QLYQS_35
为计算量测噪声矩阵的信息协方差阵，/>
Figure QLYQS_34
为计算k-1时刻***噪声矩阵的信息协方差矩阵；/>
Figure QLYQS_38
为卡尔曼滤波增益；/>
Figure QLYQS_25
为一步预测均方差矩阵；/>
Figure QLYQS_32
为误差协方差矩阵，/>
Figure QLYQS_30
为上一时刻的误差协方差矩阵；/>
Figure QLYQS_36
为陀螺测量值与状态预测值之差；/>
Figure QLYQS_37
为计算***噪声矩阵的信息协方差矩阵；/>
Figure QLYQS_39
为量测噪声协方差矩阵；/>
Figure QLYQS_24
为***噪声协方差矩阵；
观测矩阵
Figure QLYQS_40
，使用加速度计进行一阶修正时为H 1，A与H 1由下式得出：
Figure QLYQS_41
/>
其中I为单位矩阵；
Figure QLYQS_42
为旋转矩阵；D为算法执行间的时间间隔；
由于加速度计只能对俯仰角和横滚角进行有效修正，所以将四元数中表示偏航角的部分置零：
Figure QLYQS_43
其中
Figure QLYQS_44
表示经加速度计修正后的角位置；/>
Figure QLYQS_45
表示四元数/>
Figure QLYQS_46
中的第一矢量，第二矢量，第三矢量和第四矢量部分，迭代后得到经加速度计修正后的角位置
Figure QLYQS_47
。
4.根据权利要求3所述的一种基于二阶混合滤波的海缆探测机器人姿态估计方法，其特征在于，步骤3具体为：针对磁力计易受外界磁场干扰而噪声很大的问题，使用拉普拉斯滤波算法对磁力计数据进行去噪，具体步骤如下：
Figure QLYQS_48
其中，
Figure QLYQS_49
拉普拉斯滤波处理后的磁力计修正时的量测噪声矩阵；/>
Figure QLYQS_50
为无野值干扰下测量噪声的协方差矩阵；/>
Figure QLYQS_51
为对角阵；/>
Figure QLYQS_52
为辅助变量，/>
Figure QLYQS_53
；/>
Figure QLYQS_54
为磁力计修正时的卡尔曼滤波增益；/>
Figure QLYQS_55
为一个常数；
使用磁力计修正偏航角过程是将四元数中表示横滚角和俯仰角的部分置零：
Figure QLYQS_56
其中
Figure QLYQS_57
表示经磁力计修正后的角位置；/>
Figure QLYQS_58
表示四元数/>
Figure QLYQS_59
中的第一矢量，第二矢量，第三矢量和第四矢量部分，迭代后得到经过磁力计修正后的角位置
Figure QLYQS_60
。/>
CN202310460963.9A 2023-04-26 2023-04-26 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 Active CN116182871B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310460963.9A CN116182871B (zh) 2023-04-26 2023-04-26 一种基于二阶混合滤波的海缆探测机器人姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310460963.9A CN116182871B (zh) 2023-04-26 2023-04-26 一种基于二阶混合滤波的海缆探测机器人姿态估计方法

Publications (2)

Publication Number Publication Date
CN116182871A true CN116182871A (zh) 2023-05-30
CN116182871B CN116182871B (zh) 2023-07-07

Family

ID=86434830

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310460963.9A Active CN116182871B (zh) 2023-04-26 2023-04-26 一种基于二阶混合滤波的海缆探测机器人姿态估计方法

Country Status (1)

Country Link
CN (1) CN116182871B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103822633A (zh) * 2014-02-11 2014-05-28 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN105261037A (zh) * 2015-10-08 2016-01-20 重庆理工大学 一种自适应复杂场景的运动目标检测方法
CN106500721A (zh) * 2016-09-27 2017-03-15 哈尔滨工程大学 一种水下机器人双冗余姿态检测***
CN108693547A (zh) * 2018-06-05 2018-10-23 河海大学 一种用于水下深潜器的导航***及精准三点定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103822633A (zh) * 2014-02-11 2014-05-28 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN105261037A (zh) * 2015-10-08 2016-01-20 重庆理工大学 一种自适应复杂场景的运动目标检测方法
CN106500721A (zh) * 2016-09-27 2017-03-15 哈尔滨工程大学 一种水下机器人双冗余姿态检测***
CN108693547A (zh) * 2018-06-05 2018-10-23 河海大学 一种用于水下深潜器的导航***及精准三点定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
杨澜: "基于多源信息融合的车辆航姿估计技术研究", 《中国优秀硕士论文全文数据库》 *
黄浩乾 等: "基于扰动观测器的AUV固定时间积分滑模控制方法", 《中国惯性技术学报》 *

Also Published As

Publication number Publication date
CN116182871B (zh) 2023-07-07

Similar Documents

Publication Publication Date Title
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
KR102017404B1 (ko) 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考***算法
Li et al. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems
US9417091B2 (en) System and method for determining and correcting field sensors errors
Del Rosario et al. Computationally efficient adaptive error-state Kalman filter for attitude estimation
JP5861235B2 (ja) 物体の向きをより正確に推定する方法および前記方法を実装した姿勢制御システム
CN111551174A (zh) 基于多传感器惯性导航***的高动态车辆姿态计算方法及***
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及***
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
CN110702113B (zh) 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法
CN108534772B (zh) 姿态角获取方法及装置
CN110887480A (zh) 基于mems传感器的飞行姿态估计方法及***
CN107860382B (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
CN110567492A (zh) 低成本mems惯性传感器***级标定方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN112683269A (zh) 一种附有运动加速度补偿的marg姿态计算方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN114608517B (zh) 一种应用于农用无人机植保作业的姿态解算方法
CN110207647B (zh) 一种基于互补卡尔曼滤波器的臂环姿态角计算方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN112985384A (zh) 一种抗干扰磁航向角优化***
CN112033405B (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