CN116182871A - Sea cable detection robot attitude estimation method based on second-order hybrid filtering - Google Patents
Sea cable detection robot attitude estimation method based on second-order hybrid filtering 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
The invention discloses a sea cable detection robot attitude estimation method based on second-order hybrid filtering, which belongs to the technical field of underwater robot attitude estimation, and is based on three-dimensional angular velocity information under a gyroscope output device system, three-dimensional angular acceleration information under an accelerometer output device system and magnetic field information under an external magnetometer output device system, and comprises the steps of preprocessing gyroscope and accelerometer output information by using a difference method; then, using the information of the gyroscope and the accelerometer obtained by preprocessing to carry out noise self-adaptive filtering processing on the gyroscope and the accelerometer, and using the accelerometer to correct pitch angle and roll angle; finally, the magnetometer data is denoised by using a Laplace filtering algorithm, and the yaw angle is corrected by using the magnetometer. The method can solve the problems of attitude estimation and angle correction of the underwater robot.
Description
Technical Field
The invention belongs to the technical field of underwater robot pose estimation, and particularly relates to a submarine cable detection robot pose estimation method based on second-order hybrid filtering.
Background
In recent years, with the exploration of the sea bottom, the importance of the underwater operation is more and more remarkable, and the underwater robot is an important carrier for the underwater operation, has small volume, high flexibility and strong concealment, and therefore plays a great role in the fields of the underwater operation, the underwater exploration and the like.
The operation and the operation of the underwater robot are not separated from accurate real-time attitude information, the main error source of the traditional attitude estimation system is inaccurate angle estimation, so that the accurate attitude information is required to be obtained, the establishment of the high-precision angle estimation system is important, the establishment of the angle estimation system requires the cooperation of a plurality of sensors, a microelectronic inertial measurement unit (MEMS) can obtain the attitude information of the underwater robot, but the noise is large, the dynamic response of a gyroscope is fast, the measurement precision is high, but the problem of integral drift exists, meanwhile, the noise is large when the accelerometer is used for measurement, and the magnetometer is very easy to be interfered by the outside. Therefore, the single sensor cannot accurately acquire the attitude information of the underwater robot, so that an attitude estimation algorithm capable of integrating multiple sensors is needed, the advantages of the multiple sensors can be integrated, and accurate attitude information is provided for the operation of the underwater robot.
The accelerometer and the magnetometer have functions in correcting angular positions, but only the roll angle and the pitch angle can be corrected when the accelerometer is used as a fixed reference, the yaw angle cannot be accurately corrected, and on the contrary, only the yaw angle can be effectively corrected when the magnetometer is used as a fixed reference.
Aiming at the problems, the invention provides a gesture estimation algorithm based on second-order hybrid filtering. Firstly, preprocessing a gyroscope and acceleration data, selecting abnormal values, and removing the abnormal values from measurement data; secondly, using a noise self-adaptive filtering algorithm to eliminate noise of a gyroscope and an accelerometer, and correcting a roll angle and a pitch angle by using the accelerometer as a fixed reference; finally, the magnetometer is subjected to anti-interference processing by using a Laplace filtering algorithm, and the yaw angle is corrected by using the magnetometer as a fixed reference.
Disclosure of Invention
Aiming at the problems, the invention provides a sea cable detection robot attitude estimation method based on second-order hybrid filtering, which takes output information of a micro-electromechanical system inertial measurement unit (abbreviated as MEMS IMU) as a calculation basis, combines advantages of a gyroscope, an accelerometer and a magnetometer, and solves the problems of underwater robot attitude estimation and angle correction by adopting an improved algorithm.
In order to achieve the above purpose, the technical scheme of the invention is as follows:
a submarine cable detection robot attitude estimation method based on second-order hybrid filtering is based on the known quantity:
three-dimensional angular velocity information under MEMS IMU gyro output equipment system
, wherein 
Respectively representxDirection(s),yDirection(s),zAngular velocity information of direction, superscriptTRepresenting a transpose of the matrix;
three-dimensional angular acceleration information under MEMS IMU accelerometer output device system
； wherein 
Respectively representxDirection(s),yDirection(s),zAcceleration information of the direction; />
Magnetic field information under external magnetometer output device system
, wherein 
Respectively representxDirection(s),yDirection(s),zMagnetic field information of the direction;
the method is characterized by comprising the following steps of:
step 1, gyro and accelerationOutput information of the dial gauge
Preprocessing by using a difference method;
step 2, using the information of the gyroscope and the accelerometer obtained by the preprocessing in the step 1 to carry out noise self-adaptive filtering processing on the gyroscope and the accelerometer, and using the accelerometer to correct pitch angle and roll angle;
and 3, denoising magnetometer data by using a Laplace filtering algorithm, and correcting a yaw angle by using the magnetometer.
Further, the step 1 specifically comprises the following steps: setting the average value of output data of the gyroscope and the accelerometer as a threshold value by using a difference method, removing the output data when the output data exceeds the threshold value, retaining the output data when the output data is within the threshold value, and processing the output data to obtain a series of gyroscope and accelerometer information, wherein the gyroscope and the accelerometer information are respectively as follows:
wherein ,
representing the processedkGyro angular velocity information of moment of time, wherein +.>
Respectively represent the processedkTime of dayxDirection(s),yDirection(s),zAngular velocity information of the direction; />
Representing the processedkAngular acceleration information of accelerometer at moment of time, wherein +.>
Respectively represent the processedkTime of dayxDirection(s),yDirection(s),zAcceleration information of the direction.
Further, the step 2 specifically comprises: first using quaternionsqRepresenting the angular position:
wherein the first vector part
Is real, represents rotation angle +.>
；/>
Respectively representxDirection(s),yDirection(s),zA directional rotation axis; therefore->
And quaternionqThe second vector part of (2)q 1 Fit, meaning aroundxA roll angle for rotating the direction rotation shaft; />
And a third vector partq 2 Fitting means surroundingyA pitch angle rotated by the direction rotation shaft; and a fourth vector partq 3 Fitting means surroundingzA yaw angle rotated by the direction rotation shaft; let vector->
Then->
And->
The relationship of (2) is determined by the following equation:
secondly, carrying out self-adaptive filtering processing on noise of the gyroscope and the accelerometer, setting a sliding window with the length of N, and adopting a noise self-adaptive filtering algorithm as follows:
wherein ,q k is as followskThe state of the system at the moment in time,q k-1 is thatk-A system state at time 1;Ais a state transition matrix;q k,k-1 is thatkA state prediction value of the moment;
is new, is a white noise sequence which is experienced by each state in the sliding window; />
Is->
A moment of innovation; />
Is an accelerometer measurement; />
For the observation matrix, the angular position is corrected using an accelerometer and a magnetometer, respectively +.>
；/>
Information covariance matrix for calculating measurement noise matrix, < +.>
To calculatek-1, an information covariance matrix of a system noise matrix at moment; />
Is Kalman filtering gain; />
A one-step prediction mean square error matrix; />
Is error covariance matrix +.>
The error covariance matrix of the last moment; />
The difference between the measured value of the gyroscope and the predicted value of the state; />
An information covariance matrix for calculating a system noise matrix; />
Measuring a noise covariance matrix; />
A covariance matrix of system noise;
observation matrix
The first order correction by using the accelerometer isH 1 ，AAnd (3) withH 1 The following equation is used to obtain:
wherein IIs a unit matrix;
is a rotation matrix;Dfor the time interval between algorithm executions;
because the accelerometer can only effectively correct pitch angle and roll angle, the part of the quaternion representing the yaw angle is set to zero:
wherein 
Representing the angular position corrected by the accelerometer;/>
representing quaternions
The first vector, the second vector, the third vector and the fourth vector part, and obtaining the angular position corrected by the accelerometer after iteration>
。
Further, the step 3 specifically comprises: aiming at the problem that the magnetometer is easy to be interfered by an external magnetic field and has large noise, the data of the magnetometer is denoised by using a Laplace filtering algorithm, and the specific steps are as follows:
wherein ,
measuring noise matrix after the Laplace filtering treatment when the magnetometer is corrected; />
The covariance matrix is the covariance matrix of the measurement noise under the interference of no outlier; />
Is a diagonal array; />
As an auxiliary variable, +.>
；/>
Kalman filtering gain for magnetometer correction; />
Is a very small constant, e.g.>
；
The correction of yaw angle using magnetometers is to zero out the portion of the quaternion that represents roll and pitch angles:
wherein 
Representing the magnetometer corrected angular position; />
Representing quaternions
The first vector, the second vector, the third vector and the fourth vector part, and obtaining the angular position corrected by the magnetometer after iteration>
。
Compared with the prior art, the invention has the following advantages:
(1) Aiming at the problems that an accelerometer can only correct pitch angle and roll angle and a magnetometer can only correct yaw angle, the invention provides a second-order hybrid filtering algorithm which can integrate the advantages of two sensors, and the influence of magnetic anomalies on the estimation precision of the roll angle and the pitch angle is separated through second-order filtering, so that the full azimuth angle is accurately corrected.
(2) According to the invention, aiming at the problem of high noise during measurement of the gyroscope and the accelerometer, a noise self-adaptive filtering algorithm is introduced, and the accuracy of attitude estimation is further improved by updating the measurement noise matrix in real time to reduce the noise of the gyroscope and the accelerometer.
(3) Aiming at the problem that the magnetometer is easily influenced by an external magnetic field when in use, the invention introduces a Laplace filtering algorithm, reduces the output error of the magnetometer and improves the accuracy of correcting the angular position of the magnetometer.
Drawings
FIG. 1 is a flow chart of an attitude estimation algorithm based on second order hybrid filtering described in the present invention;
FIG. 2 is a flow chart of differential preprocessing of gyroscope and accelerometer information in step 1;
FIG. 3 is raw output information of a three-axis gyroscope and a three-axis accelerometer acquired during an experiment;
fig. 4 is a graph showing the comparison of the results of the posture estimation in different methods.
Detailed Description
The present invention is further illustrated in the following drawings and detailed description, which are to be understood as being merely illustrative of the invention and not limiting the scope of the invention.
The invention discloses a submarine cable detection robot attitude estimation method based on second-order hybrid filtering, which is based on the known quantity:
three-dimensional angular velocity information under MEMS IMU gyro output equipment system
, wherein 
Respectively representxDirection(s),yDirection(s),zAngular velocity information of direction, superscriptTRepresenting a transpose of the matrix;
three-dimensional angular acceleration information under MEMS IMU accelerometer output device system
； wherein 
Respectively representxDirection(s),yDirection(s),zAcceleration information of the direction;
magnetic field information under external magnetometer output device system
, wherein 
Respectively representxDirection(s),yDirection(s),zMagnetic field information of the direction;
the method specifically comprises the following steps:
step 1, for accurate calculation of system and measurement of noise variance matrix
Information about the gyroscope and the accelerometer needs to be output>
Preprocessing by using a difference method; setting the average value of output data of the gyroscope and the accelerometer as a threshold value by using a difference method, removing the output data when the output data exceeds the threshold value, retaining the output data when the output data is within the threshold value, and processing the output data to obtain a series of gyroscope and accelerometer information, wherein the gyroscope and the accelerometer information are respectively as follows:
wherein ,
representing the processedkGyro angular velocity information of moment of time, wherein +.>
Respectively represent the processedkTime of dayxDirection(s),yDirection(s),zAngular velocity information of the direction; />
Representing the processedkAngular acceleration information of accelerometer at moment of time, wherein +.>
Respectively represent the processedkTime of dayxDirection(s),yDirection(s),zAcceleration information of the direction.
The method for setting the average value of the output data of the gyroscope and the accelerometer to be the threshold value by using the difference method in the embodiment is that firstly, the gyroscope information at the current moment and the gyroscope information at the last moment are subjected to difference; the accelerometer information at the current time and the accelerometer information at the previous time are poor. Secondly, setting a gyroscope information threshold value and an acceleration information threshold value (the threshold value is a variance value of sampling data from t-10s time to t time, and the principle is that the variance can reflect the fluctuation degree of the data, so that the data after differential processing can reflect the fluctuation degree of the data by comparing the variance value with the variance value), and comparing the difference value with the threshold value respectively; and finally, eliminating the gyroscope or the acceleration information at the moment when the difference value is larger than the threshold value, otherwise, keeping the gyroscope or the acceleration information.
Step 2, using the information of the gyroscope and the accelerometer obtained by the preprocessing in the step 1 to carry out noise self-adaptive filtering processing on the gyroscope and the accelerometer, and using the accelerometer to correct pitch angle and roll angle;
first using quaternionsqRepresenting the angular position:
wherein the first vector part
Is real, represents rotation angle +.>
；/>
Respectively representxDirection(s),yDirection(s),zA directional rotation axis; therefore->
And quaternionqThe second vector part of (2)q 1 Fit, meaning aroundxA roll angle for rotating the direction rotation shaft; />
And a third vector partq 2 Fitting means surroundingyA pitch angle rotated by the direction rotation shaft; and a fourth vector partq 3 Fitting means surroundingzA yaw angle rotated by the direction rotation shaft;let vector->
Then->
And->
The relationship of (2) is determined by the following equation:
secondly, carrying out self-adaptive filtering processing on noise of the gyroscope and the accelerometer, setting a sliding window with the length of N, and adopting a noise self-adaptive filtering algorithm as follows:
wherein ,q k is as followskThe state of the system at the moment in time,q k-1 is thatk-A system state at time 1;Ais a state transition matrix;q k,k-1 is thatkA state prediction value of the moment;
is new, is a white noise sequence which is experienced by each state in the sliding window; />
Is->
A moment of innovation; />
Is an accelerometer measurement; />
For the observation matrix, the angular position is corrected using an accelerometer and a magnetometer, respectively +.>
；/>
Information covariance matrix for calculating measurement noise matrix, < +.>
To calculatek-1, an information covariance matrix of a system noise matrix at moment; />
Is Kalman filtering gain; />
A one-step prediction mean square error matrix; />
Is error covariance matrix +.>
The error covariance matrix of the last moment; />
The difference between the measured value of the gyroscope and the predicted value of the state; />
An information covariance matrix for calculating a system noise matrix; />
Measuring a noise covariance matrix; />
A covariance matrix of system noise;
observation matrix
The first order correction by using the accelerometer isH 1 ，AAnd (3) withH 1 The following equation is used to obtain:
wherein IIs a unit matrix;
is a rotation matrix;Dfor the time interval between algorithm executions;
because the accelerometer can only effectively correct pitch angle and roll angle, the part of the quaternion representing the yaw angle is set to zero:
wherein 
Representing the angular position corrected by the accelerometer; />
Representing quaternions
The first vector, the second vector, the third vector and the fourth vector part, and obtaining the angular position corrected by the accelerometer after iteration>
。
And 3, denoising magnetometer data by using a Laplace filtering algorithm, and correcting a yaw angle by using the magnetometer.
Aiming at the problem that the magnetometer is easy to be interfered by an external magnetic field and has large noise, the data of the magnetometer is denoised by using a Laplace filtering algorithm, and the specific steps are as follows:
wherein ,
measurement at correction of magnetometer after Laplace filteringA noise matrix; />
The covariance matrix is the covariance matrix of the measurement noise under the interference of no outlier; />
Is a diagonal array; />
As an auxiliary variable, +.>
；/>
Kalman filtering gain for magnetometer correction; />
Is a very small constant, e.g.>
；
The correction of yaw angle using magnetometers is to zero out the portion of the quaternion that represents roll and pitch angles:
wherein 
Representing the magnetometer corrected angular position; />
Representing quaternions
The first vector, the second vector, the third vector and the fourth vector part, and obtaining the angular position corrected by the magnetometer after iteration>
。
In order to verify the effectiveness of the method, an attitude estimation experiment based on second-order hybrid filtering is carried out. As the sensing apparatus, a 9-axis sensor (MPU 9250) is used, which includes: three-axis gyroscopes, three-axis accelerometers and three-axis magnetometers. Wherein fig. 3 is the original output information of the three-axis gyroscope and the three-axis accelerometer acquired in the experimental process. Fig. 4 is a corresponding gesture estimation curve of the extended kalman filtering method and the method of the present invention, and it can be seen from the figure that the gesture result of the method of the present invention is stable and reliable, which is superior to the extended kalman filtering method.
It should be noted that the foregoing merely illustrates the technical idea of the present invention and is not intended to limit the scope of the present invention, and that a person skilled in the art may make several improvements and modifications without departing from the principles of the present invention, which fall within the scope of the claims of the present invention.
Claims (4)
1. A submarine cable detection robot attitude estimation method based on second-order hybrid filtering is based on the known quantity:
three-dimensional angular velocity information under MEMS IMU gyro output equipment system
, wherein 
Respectively representxDirection(s),yDirection(s),zAngular velocity information of direction, superscriptTRepresenting a transpose of the matrix;
three-dimensional angular acceleration information under MEMS IMU accelerometer output device system
； wherein 
Respectively representxDirection(s),yDirection(s),zAcceleration information of the direction;
magnetic field information under external magnetometer output device system
, wherein />
Respectively representxDirection(s),yDirection(s),zMagnetic field information of the direction;
the method is characterized by comprising the following steps of:
step 1, outputting information to a gyroscope and an accelerometer
Preprocessing by using a difference method;
step 2, using the information of the gyroscope and the accelerometer obtained by the preprocessing in the step 1 to carry out noise self-adaptive filtering processing on the gyroscope and the accelerometer, and using the accelerometer to correct pitch angle and roll angle;
and 3, denoising magnetometer data by using a Laplace filtering algorithm, and correcting a yaw angle by using the magnetometer.
2. The submarine cable detection robot attitude estimation method based on second-order hybrid filtering according to claim 1, wherein step 1 is specifically: setting the average value of output data of the gyroscope and the accelerometer as a threshold value by using a difference method, removing the output data when the output data exceeds the threshold value, retaining the output data when the output data is within the threshold value, and processing the output data to obtain a series of gyroscope and accelerometer information, wherein the gyroscope and the accelerometer information are respectively as follows:
wherein ,
representing the processedkGyro angular velocity information of moment of time, wherein +.>
Respectively represent the processedkTime of dayxDirection(s),yDirection(s),zAngular velocity information of the direction; />
Representing the processedkAngular acceleration information of accelerometer at moment of time, wherein +.>
Respectively represent the processedkTime of dayxDirection(s),yDirection(s),zAcceleration information of the direction.
3. The submarine cable detection robot attitude estimation method based on second-order hybrid filtering according to claim 1, wherein step 2 is specifically: first using quaternionsqRepresenting the angular position:
wherein the first vector part
Is real, represents rotation angle +.>
；/>
Respectively representxDirection(s),yDirection(s),zA directional rotation axis; therefore->
And quaternionqThe second vector part of (2)q 1 Fit, meaning aroundxA roll angle for rotating the direction rotation shaft; />
And a third vector partq 2 Fitting means surroundingyA pitch angle rotated by the direction rotation shaft; and a fourth vector partq 3 Fitting means surroundingzA yaw angle rotated by the direction rotation shaft; let vector->
Then->
And->
The relationship of (2) is determined by the following equation:
secondly, carrying out self-adaptive filtering processing on noise of the gyroscope and the accelerometer, setting a sliding window with the length of N, and adopting a noise self-adaptive filtering algorithm as follows:
wherein ,q k is as followskThe state of the system at the moment in time,q k-1 is thatk-A system state at time 1;Ais a state transition matrix;q k,k-1 is thatkA state prediction value of the moment;
is new, is a white noise sequence which is experienced by each state in the sliding window; />
Is->
A moment of innovation; />
Is an accelerometer measurement; />
For the observation matrix, the angular positions were corrected using accelerometers and magnetometers, respectively
；/>
Information covariance matrix for calculating measurement noise matrix, < +.>
To calculatek-1, an information covariance matrix of a system noise matrix at moment; />
Is Kalman filtering gain; />
A one-step prediction mean square error matrix; />
Is error covariance matrix +.>
The error covariance matrix of the last moment; />
The difference between the measured value of the gyroscope and the predicted value of the state; />
An information covariance matrix for calculating a system noise matrix; />
Measuring a noise covariance matrix; />
A covariance matrix of system noise;
observation matrix
The first order correction by using the accelerometer isH 1 ，AAnd (3) withH 1 The following equation is used to obtain:
wherein IIs a unit matrix;
is a rotation matrix;Dfor the time interval between algorithm executions;
because the accelerometer can only effectively correct pitch angle and roll angle, the part of the quaternion representing the yaw angle is set to zero:
wherein 
Representing the angular position corrected by the accelerometer; />
Representing quaternion +.>
The first vector, the second vector, the third vector and the fourth vector part, and obtaining the angular position corrected by the accelerometer after iteration
。
4. The submarine cable detection robot attitude estimation method based on second-order hybrid filtering according to claim 3, wherein the step 3 is specifically: aiming at the problem that the magnetometer is easy to be interfered by an external magnetic field and has large noise, the data of the magnetometer is denoised by using a Laplace filtering algorithm, and the specific steps are as follows:
wherein ,
measuring noise matrix after the Laplace filtering treatment when the magnetometer is corrected; />
The covariance matrix is the covariance matrix of the measurement noise under the interference of no outlier; />
Is a diagonal array; />
As an auxiliary variable, +.>
；/>
Kalman filtering gain for magnetometer correction; />
Is a constant;
the correction of yaw angle using magnetometers is to zero out the portion of the quaternion that represents roll and pitch angles:
wherein 
Representing the magnetometer corrected angular position; />
Representing quaternion +.>
The first vector, the second vector, the third vector and the fourth vector part of the magnetic sensor are iterated to obtain the corrected angular position of the magnetic sensor
。/>
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310460963.9A CN116182871B (en) | 2023-04-26 | 2023-04-26 | Sea cable detection robot attitude estimation method based on second-order hybrid filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310460963.9A CN116182871B (en) | 2023-04-26 | 2023-04-26 | Sea cable detection robot attitude estimation method based on second-order hybrid filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116182871A true CN116182871A (en) | 2023-05-30 |
| CN116182871B CN116182871B (en) | 2023-07-07 |
Family
ID=86434830
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310460963.9A Active CN116182871B (en) | 2023-04-26 | 2023-04-26 | Sea cable detection robot attitude estimation method based on second-order hybrid filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116182871B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103822633A (en) * | 2014-02-11 | 2014-05-28 | 哈尔滨工程大学 | Low-cost attitude estimation method based on second-order measurement update |
| CN105261037A (en) * | 2015-10-08 | 2016-01-20 | 重庆理工大学 | Moving object detection method capable of automatically adapting to complex scenes |
| CN106500721A (en) * | 2016-09-27 | 2017-03-15 | 哈尔滨工程大学 | A kind of underwater robot dual redundant attitude detection system |
| CN108693547A (en) * | 2018-06-05 | 2018-10-23 | 河海大学 | A kind of navigation system for underwater bathyscaph and accurate three-point positioning method |
-
2023
- 2023-04-26 CN CN202310460963.9A patent/CN116182871B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103822633A (en) * | 2014-02-11 | 2014-05-28 | 哈尔滨工程大学 | Low-cost attitude estimation method based on second-order measurement update |
| CN105261037A (en) * | 2015-10-08 | 2016-01-20 | 重庆理工大学 | Moving object detection method capable of automatically adapting to complex scenes |
| CN106500721A (en) * | 2016-09-27 | 2017-03-15 | 哈尔滨工程大学 | A kind of underwater robot dual redundant attitude detection system |
| CN108693547A (en) * | 2018-06-05 | 2018-10-23 | 河海大学 | A kind of navigation system for underwater bathyscaph and accurate three-point positioning method |
Non-Patent Citations (2)
| Title |
|---|
| 杨澜: "基于多源信息融合的车辆航姿估计技术研究", 《中国优秀硕士论文全文数据库》 * |
| 黄浩乾 等: "基于扰动观测器的AUV固定时间积分滑模控制方法", 《中国惯性技术学报》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116182871B (en) | 2023-07-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102017404B1 (en) | How to update the full posture angle of agricultural machinery based on 9 axis MEMS sensor | |
| CN108827299B (en) | Aircraft attitude calculation method based on improved quaternion second-order complementary filtering | |
| CN108225308B (en) | Quaternion-based attitude calculation method for extended Kalman filtering algorithm | |
| CN112013836B (en) | Attitude heading reference system algorithm based on improved adaptive Kalman filtering | |
| 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 (en) | Method for estimating the orientation of an object more accurately and attitude control system implementing the method | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN111721288B (en) | Zero offset correction method and device for MEMS device and storage medium | |
| CN110702113B (en) | Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor | |
| CN108534772B (en) | Attitude angle acquisition method and device | |
| CN110887480A (en) | Flight attitude estimation method and system based on MEMS sensor | |
| CN107860382B (en) | Method for measuring attitude by applying AHRS under geomagnetic anomaly condition | |
| CN110567492A (en) | Low-cost MEMS inertial sensor system-level calibration method | |
| CN111189474A (en) | Autonomous calibration method of MARG sensor based on MEMS | |
| CN106595669B (en) | Method for resolving attitude of rotating body | |
| Sokolović et al. | INS/GPS navigation system based on MEMS technologies | |
| CN114608517B (en) | Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation | |
| CN110207647B (en) | Arm ring attitude angle calculation method based on complementary Kalman filter | |
| CN111649747A (en) | IMU-based adaptive EKF attitude measurement improvement method | |
| CN108871319B (en) | Attitude calculation method based on earth gravity field and earth magnetic field sequential correction | |
| CN112985384A (en) | Anti-interference magnetic course angle optimization system | |
| CN112033405B (en) | Indoor environment magnetic anomaly real-time correction and navigation method and device |
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 |