CN116182871A - 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 - Google Patents
一种基于二阶混合滤波的海缆探测机器人姿态估计方法 Download PDFInfo
- 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
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 47
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000001514 detection method Methods 0.000 title claims abstract description 11
- 238000012937 correction Methods 0.000 claims abstract description 12
- 238000012545 processing Methods 0.000 claims abstract description 12
- 230000001133 acceleration Effects 0.000 claims abstract description 11
- 238000007781 pre-processing Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 62
- 238000005259 measurement Methods 0.000 claims description 18
- 230000007704 transition Effects 0.000 claims description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000004377 microelectronic Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
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)的输出信息作为计算基础,融合陀螺仪、加速度计和磁力计的优点,并采用改进的算法,解决水下机器人姿态估计和角度修正问题。
为达到上述目的,本发明的技术方案如下:
一种基于二阶混合滤波的海缆探测机器人姿态估计方法,该方法基于已知量:
其特征在于,该方法具体包括如下步骤:
步骤2、使用步骤1预处理得到的陀螺和加速度计信息,进行对陀螺与加速度计的噪声自适应滤波处理,并使用加速度计修正俯仰角和横滚角;
步骤3、使用拉普拉斯滤波算法对磁力计数据进行去噪,并使用磁力计修正偏航角。
进一步地,步骤1具体为:使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值,当输出数据超出该阈值时剔除,当输出数据在阈值之内时保留,经过处理后可得到一系列陀螺及加速度计信息,分别为:
其中,表示经过处理后的k时刻的陀螺角速度信息,其中/>分别表示经过处理后的k时刻x方向、y方向、z方向的角速度信息;/>表示经过处理后的k时刻的加速度计的角加速度信息,其中/>分别表示经过处理后的k时刻x方向、y方向、z方向的加速度信息。
进一步地,步骤2具体为:首先使用四元数q表示角位置:
其中第一矢量部分为实数,表示旋转角/>;/>分别表示x方向、y方向、z方向的旋转轴;所以/>与四元数q中的第二矢量部分q 1配合,表示围绕x方向旋转轴旋转的横滚角;/>与第三矢量部分q 2配合表示围绕y方向旋转轴旋转的俯仰角;与第四矢量部分q 3配合表示围绕z方向旋转轴旋转的偏航角;令向量/>,则/>与/>的关系由下式确定:
其次对陀螺与加速度计的噪声进行自适应滤波处理,设置长度为N的滑动窗,噪声自适应滤波算法如下:
其中,q k 为为k时刻的***状态,q k-1为k-1时刻的***状态;A为状态转移矩阵;q k,k-1为k时刻的状态预测值;为新息,是滑动窗内各态历经的白噪声序列;/>为/>时刻的新息;/>为加速度计测量值;/>为观测矩阵,使用加速度计和磁力计修正角位置时分别为/>;/>为计算量测噪声矩阵的信息协方差阵,/>为计算k-1时刻***噪声矩阵的信息协方差矩阵;/>为卡尔曼滤波增益;/>为一步预测均方差矩阵;/>为误差协方差矩阵,/>为上一时刻的误差协方差矩阵;/>为陀螺测量值与状态预测值之差;/>为计算***噪声矩阵的信息协方差矩阵;/>为量测噪声协方差矩阵;/>为***噪声协方差矩阵;
由于加速度计只能对俯仰角和横滚角进行有效修正,所以将四元数中表示偏航角的部分置零:
进一步地,步骤3具体为:针对磁力计易受外界磁场干扰而噪声很大的问题,使用拉普拉斯滤波算法对磁力计数据进行去噪,具体步骤如下:
使用磁力计修正偏航角过程是将四元数中表示横滚角和俯仰角的部分置零:
与现有技术相比,本发明具有如下优点:
(1) 本发明针对加速度计只能修正俯仰角与横滚角,磁力计只能修正偏航角的问题,提出一种二阶混合滤波算法,可以融合两个传感器的优点,并且通过二阶滤波分离磁异常对横滚角和俯仰角估计精度的影响,对全方位角进行精确修正。
(2) 本发明针对陀螺仪和加速度计测量时噪声很大的问题,引入了噪声自适应滤波算法,通过实时更新量测噪声矩阵,减小陀螺仪与加速度噪声,进一步提高了姿态估计的准确性。
(3) 本发明针对磁力计使用时易受外界磁场影响的问题,引入了拉普拉斯滤波算法,减小磁力计输出误差,提高了磁力计修正角位置时的精度。
附图说明
图1为本发明所描述的一种基于二阶混合滤波的姿态估计算法流程图;
图2为步骤1陀螺及加速度计信息差分预处理流程图;
图3为实验过程中采集的三轴陀螺仪和三轴加速度计原始输出信息;
图4为不同方法姿态估计结果对比曲线。
具体实施方式
下面结合附图和具体实施方式,进一步阐明本发明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明的一种基于二阶混合滤波的海缆探测机器人姿态估计方法,该方法基于已知量:
该方法具体包括如下步骤:
步骤1、为了精确计算***和量测噪声方差阵,需要对陀螺及加速度计输出信息/>使用差分法进行预处理;使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值,当输出数据超出该阈值时剔除,当输出数据在阈值之内时保留,经过处理后可得到一系列陀螺及加速度计信息,分别为:
其中,表示经过处理后的k时刻的陀螺角速度信息,其中/>分别表示经过处理后的k时刻x方向、y方向、z方向的角速度信息;/>表示经过处理后的k时刻的加速度计的角加速度信息,其中/>分别表示经过处理后的k时刻x方向、y方向、z方向的加速度信息。
本实施例中所述使用差分法将陀螺仪和加速度计输出数据的均值设定为阈值的方法是,首先,当前时刻的陀螺仪信息和上时刻的陀螺仪信息做差;当前时刻的加速度计信息和上时刻的加速度计信息做差。其次,设定陀螺仪信息和加速度信息阈值(阈值为:t-10s时刻到t时刻内采样数据的方差值。原则:因为方差能够反应数据的波动程度,因此,差分处理后的数据和方差值对比,便能反应数据波动程度),将差值分别和阈值作比较;最后,当差值大于阈值时剔除该时刻的陀螺仪或者加速度信息,否则保留。
步骤2、使用步骤1预处理得到的陀螺和加速度计信息,进行对陀螺与加速度计的噪声自适应滤波处理,并使用加速度计修正俯仰角和横滚角;
首先使用四元数q表示角位置:
其中第一矢量部分为实数,表示旋转角/>;/>分别表示x方向、y方向、z方向的旋转轴;所以/>与四元数q中的第二矢量部分q 1配合,表示围绕x方向旋转轴旋转的横滚角;/>与第三矢量部分q 2配合表示围绕y方向旋转轴旋转的俯仰角;与第四矢量部分q 3配合表示围绕z方向旋转轴旋转的偏航角;令向量/>,则/>与/>的关系由下式确定:
其次对陀螺与加速度计的噪声进行自适应滤波处理,设置长度为N的滑动窗,噪声自适应滤波算法如下:
其中,q k 为为k时刻的***状态,q k-1为k-1时刻的***状态;A为状态转移矩阵;q k,k-1为k时刻的状态预测值;为新息,是滑动窗内各态历经的白噪声序列;/>为/>时刻的新息;/>为加速度计测量值;/>为观测矩阵,使用加速度计和磁力计修正角位置时分别为/>;/>为计算量测噪声矩阵的信息协方差阵,/>为计算k-1时刻***噪声矩阵的信息协方差矩阵;/>为卡尔曼滤波增益;/>为一步预测均方差矩阵;/>为误差协方差矩阵,/>为上一时刻的误差协方差矩阵;/>为陀螺测量值与状态预测值之差;/>为计算***噪声矩阵的信息协方差矩阵;/>为量测噪声协方差矩阵;/>为***噪声协方差矩阵;
由于加速度计只能对俯仰角和横滚角进行有效修正,所以将四元数中表示偏航角的部分置零:
步骤3、使用拉普拉斯滤波算法对磁力计数据进行去噪,并使用磁力计修正偏航角。
针对磁力计易受外界磁场干扰而噪声很大的问题,使用拉普拉斯滤波算法对磁力计数据进行去噪,具体步骤如下:
使用磁力计修正偏航角过程是将四元数中表示横滚角和俯仰角的部分置零:
为了验证本发明方法的有效性,开展基于二阶混合滤波的姿态估计实验。使用9轴传感器(MPU9250)作为传感设备,其中包括:三轴陀螺仪、三轴加速度计和三轴磁力计。其中图3为实验过程中采集的三轴陀螺仪和三轴加速度计原始输出信息。图4是对应的扩展卡尔曼滤波方法和本发明方法的姿态估计曲线,从图中可以看出,本发明方法的姿态结果稳定可靠,优于扩展卡尔曼滤波方法。
需要说明的是,以上内容仅仅说明了本发明的技术思想,不能以此限定本发明的保护范围,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰均落入本发明权利要求书的保护范围之内。
Claims (4)
1.一种基于二阶混合滤波的海缆探测机器人姿态估计方法,该方法基于已知量:
其特征在于,该方法具体包括如下步骤:
步骤2、使用步骤1预处理得到的陀螺和加速度计信息,进行对陀螺与加速度计的噪声自适应滤波处理,并使用加速度计修正俯仰角和横滚角;
步骤3、使用拉普拉斯滤波算法对磁力计数据进行去噪,并使用磁力计修正偏航角。
3.根据权利要求1所述的一种基于二阶混合滤波的海缆探测机器人姿态估计方法,其特征在于,步骤2具体为:首先使用四元数q表示角位置:
其中第一矢量部分为实数,表示旋转角/>;/>分别表示x方向、y方向、z方向的旋转轴;所以/>与四元数q中的第二矢量部分q 1配合,表示围绕x方向旋转轴旋转的横滚角;/>与第三矢量部分q 2配合表示围绕y方向旋转轴旋转的俯仰角;与第四矢量部分q 3配合表示围绕z方向旋转轴旋转的偏航角;令向量/>,则/>与/>的关系由下式确定:
其次对陀螺与加速度计的噪声进行自适应滤波处理,设置长度为N的滑动窗,噪声自适应滤波算法如下:
其中,q k 为为k时刻的***状态,q k-1为k-1时刻的***状态;A为状态转移矩阵;q k,k-1为k时刻的状态预测值;为新息,是滑动窗内各态历经的白噪声序列;/>为/>时刻的新息;/>为加速度计测量值;/>为观测矩阵,使用加速度计和磁力计修正角位置时分别为;/>为计算量测噪声矩阵的信息协方差阵,/>为计算k-1时刻***噪声矩阵的信息协方差矩阵;/>为卡尔曼滤波增益;/>为一步预测均方差矩阵;/>为误差协方差矩阵,/>为上一时刻的误差协方差矩阵;/>为陀螺测量值与状态预测值之差;/>为计算***噪声矩阵的信息协方差矩阵;/>为量测噪声协方差矩阵;/>为***噪声协方差矩阵;
由于加速度计只能对俯仰角和横滚角进行有效修正,所以将四元数中表示偏航角的部分置零:
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)
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 | 河海大学 | 一种用于水下深潜器的导航***及精准三点定位方法 |
-
2023
- 2023-04-26 CN CN202310460963.9A patent/CN116182871B/zh active Active
Patent Citations (4)
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)
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 |