CN116067370B - IMU gesture resolving method, IMU gesture resolving equipment and storage medium - Google Patents
IMU gesture resolving method, IMU gesture resolving equipment and storage medium Download PDFInfo
- Publication number
- CN116067370B CN116067370B CN202310339625.XA CN202310339625A CN116067370B CN 116067370 B CN116067370 B CN 116067370B CN 202310339625 A CN202310339625 A CN 202310339625A CN 116067370 B CN116067370 B CN 116067370B
- Authority
- CN
- China
- Prior art keywords
- covariance matrix
- error
- measurement
- gyroscope
- state
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 239000011159 matrix material Substances 0.000 claims abstract description 93
- 238000005259 measurement Methods 0.000 claims abstract description 56
- 238000001914 filtration Methods 0.000 claims abstract description 29
- 239000013598 vector Substances 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 22
- 230000001133 acceleration Effects 0.000 claims abstract description 19
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 230000003044 adaptive effect Effects 0.000 claims abstract description 13
- 230000004927 fusion Effects 0.000 claims abstract description 12
- 238000006243 chemical reaction Methods 0.000 claims abstract description 9
- 230000006870 function Effects 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000003068 static effect Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 2
- 230000000694 effects Effects 0.000 description 10
- 238000002474 experimental method Methods 0.000 description 7
- 238000010606 normalization Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 208000000044 Amnesia Diseases 0.000 description 1
- 208000031091 Amnestic disease Diseases 0.000 description 1
- 230000006986 amnesia Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012612 static experiment Methods 0.000 description 1
- 238000006467 substitution reaction 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/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/20—Instruments for performing navigational calculations
-
- 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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- 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
- 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/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Theoretical Computer Science (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Automation & Control Theory (AREA)
- Computing Systems (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Navigation (AREA)
Abstract
The invention discloses a method, equipment and a storage medium for resolving an IMU gesture, which comprise the following steps: constructing an error model of the gyroscope and the accelerometer; based on the conversion relation of the quaternion vector representation navigation coordinate system and the carrier coordinate system, constructing an acceleration output model; constructing a state equation of a filter according to the output signal of the gyroscope and the error model; constructing a measurement equation based on the acceleration output model; setting a system state initial value, a noise covariance matrix initial value and an error covariance matrix initial value; constructing a filter based on a volume Kalman filtering algorithm, introducing a Sage-Husa noise estimator to adaptively adjust a noise covariance matrix, and updating the volume Kalman filter based on an adaptive factor of a prediction residual error two-section function; and carrying out attitude calculation on the updated quaternion vector to obtain the pitch angle and roll angle information after fusion filtering. The invention improves the gesture resolving precision and the robustness of the filter.
Description
Technical Field
The present invention relates to the field of navigation positioning technologies, and in particular, to an IMU gesture resolving method and apparatus, and a storage medium.
Background
In the fields of robot control, aerospace, autopilot, target tracking and the like, measurement positioning is always the dominant research direction. Inertial Measurement Units (IMUs), which are typically composed of three-axis accelerometers and three-axis gyroscopes, are widely studied as the basic building blocks for various measurement positioning systems. The information of the roll angle, the pitch angle and the yaw angle of the IMU can be calculated through the information of the triaxial angular velocity measured by the gyroscope, and the information of the roll angle and the pitch angle of the IMU can be calculated through the information of the triaxial acceleration measured by the accelerometer. In many cases, it is not necessary to estimate heading state (yaw angle), so obtaining accurate roll angle, pitch angle information with a six-axis IMU is the focus of current research.
Because the signals measured by the gyroscope contain zero offset, noise and other interferences besides effective information, direct integration can accumulate errors to invalidate the attitude calculation result. In addition, although the accelerometer can acquire attitude information through transformation of acceleration, the attitude angle calculated by the accelerometer is not reliable in a highly dynamic scene due to the influence of external acceleration. In order to obtain accurate attitude information, a fusion process of the gyroscope and the accelerometer data is required.
The invention discloses a carrier dynamic attitude estimation method based on MEMS inertial sensors (invention patent application number: 201911262998.1, invention name: carrier dynamic attitude estimation method based on MEMS inertial sensors), which firstly processes respective data through noise models of accelerometers and gyroscopes, then judges the accelerometer value and the gyroscope value by a threshold method, and finally adopts EKF as a fusion algorithm to carry out data fusion.
The method disclosed by the patent introduces a higher order truncation error and jacobian matrix operation by adopting an Extended Kalman Filter (EKF) algorithm, so that the gesture calculation precision is reduced, meanwhile, the real-time estimation of noise statistics characteristics is lacked, the underestimation or overestimation of a process noise covariance matrix Q and a measured noise covariance matrix R can cause the filter to deviate from the optimal to the suboptimal, in addition, various parameters have deviation in the operation process, the performance of the filter can be influenced under certain conditions, and the robustness of the traditional Kalman filter is insufficient.
Disclosure of Invention
The invention provides an IMU gesture resolving method, IMU gesture resolving equipment and a storage medium, which improve gesture resolving precision and robustness of a filter.
In order to solve the technical problems, the first aspect of the invention discloses an IMU gesture resolving method, which comprises the following steps:
acquiring error parameters of a gyroscope and error parameters of an accelerometer, and constructing an error model of the gyroscope and the accelerometer;
based on the conversion relation of the quaternion vector representation navigation coordinate system and the carrier coordinate system, constructing an acceleration output model;
constructing a state equation of a filter according to the output signal of the gyroscope and the error model; constructing a measurement equation based on the acceleration output model;
setting a system state initial value, a noise covariance matrix initial value and an error covariance matrix initial value;
constructing a filter based on a volume Kalman filtering algorithm, introducing a Sage-Husa noise estimator to adaptively adjust a noise covariance matrix, and correcting Kalman gain based on an adaptive factor of a prediction residual error two-section function to update the volume Kalman filter;
and updating the state and measurement information at the next moment, and performing attitude calculation on the updated quaternion vector to obtain the pitch angle and roll angle information after fusion filtering.
In some embodiments, the error model of the gyroscope is:
wherein,,
is the main error of gyroscope, +.>
For random constant drift, ++>
For white noise in the gyroscope measurement signal, < >>
Representing random constant drift +.>
Derivative of>
Is the main error of the accelerometer, +.>
White noise in the signal is measured for the accelerometer.
In some embodiments, the quaternion-based vector represents the conversion relationship between the navigation coordinate system and the carrier coordinate system, specifically,
defining a unit quaternion vector
Said navigational coordinate system->
Coordinate system of system and carrier
Coordinate transformation matrix between systems>
The expression is as follows:
=
the construction of the acceleration output model comprises the steps of, in particular,
accelerometer on carrierCoordinate system
Acceleration signal ∈>
Can be written as:
wherein the method comprises the steps of
Representing absolute value of gravitational constant, +.>
Measurement noise for accelerometer, +.>
Is a coordinate transformation matrix.
In some embodiments, a state equation of the filter, in particular,
according to the differential equation of the output angular velocity and quaternion of the gyroscope:
wherein the method comprises the steps of
For the carrier coordinate system->
Is->
The system angular velocity matrix is represented as follows:
defining system state vectors
State vector dimension->
The differential equation to obtain the system state update is thus as follows:
wherein the gyroscope angular velocity estimation signal
Expressed as gyroscope actual measurement signal +.>
And gyroscope error->
Subtracting; the Kalman filter usually adopts a discrete state space model, adopts a Picard successive approximation method and adopts a first order approximation, and simultaneously introduces a sampling time interval +.>
Discretizing the system differential equation is as follows:
wherein the method comprises the steps of
Process noise covariance matrix +.>
Wherein->
Is a statistical mathematical expectation; error covariance matrix->
，/>
Representing covariance;
constructing a measurement equation based on the acceleration output model, specifically,
obtaining a measurement equation according to an output model of the accelerometer
The following are provided:
discretizing the measurement equation to obtain
The method comprises the steps of carrying out a first treatment on the surface of the Measuring noise covariance matrix
。
In some embodiments, a system state initial value, a noise covariance matrix initial value, and an error covariance matrix initial value are set, specifically,
measuring output data of the gyroscopes after a plurality of groups of IMUs are placed still, and taking an average value of the output data as an estimated initial value of state zero offset;
obtaining initial value of noise covariance matrix through static test
And->
Error covariance matrix initial +.>
。
In some embodiments, the volume kalman filter is updated, including a state update, a measurement update, an error covariance matrix update, and a noise covariance matrix update;
the status update is, in particular,
from the slave
Moment start calculation +.>
Square root of>
Volume point->
：
Wherein the method comprises the steps of
Representing a set of volumetric points; for function->
Substituting the volume point to obtain the estimated value +.>
；
Calculating state predictors
Predicted State covariance +.>
:
Wherein the method comprises the steps of
，/>
Correction values representing a process noise covariance matrix;
the measurement update is performed, in particular,
calculation of
Square root of>
Is>
，
Volume point of propagation measurement
Calculating the estimated value of the measurement +.>
：
Calculating a measurement error covariance and a cross covariance:
meanwhile, the jacobian matrix of the measurement equation can be obtained:
in some embodiments, the kalman gain is modified by an adaptive factor based on a prediction residual two-stage function, specifically,
using adaptive factors
Correcting a gain matrix of the Kalman filter:
wherein the method comprises the steps of
，/>
Is usually taken as +.>
；/>
Learning statistics representing prediction residual, +.>
=/>
The prediction residual is represented as such,
covariance matrix representing prediction residual, tr ()'s represent the trace of the matrix;
updating
System state at time and error covariance matrix:
the noise covariance matrix is adaptively adjusted by a Sage-Husa noise estimator, which, in particular,
wherein the method comprises the steps of
，/>
Is forgetting factor, and has a value range of +.>
。
In some embodiments, the updated quaternion vector is subjected to gesture calculation to obtain the information of the pitch angle and the roll angle after fusion filtering, specifically,
according to a second aspect of the invention, it is disclosed that the device comprises: a memory storing executable program code;
a processor coupled to the memory;
the processor invokes the executable program code stored in the memory to perform an IMU pose resolution method as described above.
According to a third aspect of the invention, it is disclosed that the storage medium comprises:
a memory storing executable program code;
a processor coupled to the memory;
the processor invokes the executable program code stored in the memory to perform an IMU pose resolving method as claimed in any preceding claim.
Compared with the prior art, the invention has the beneficial effects that:
(1) By using the volume Kalman filter as a basic data fusion algorithm, the high-order truncation error and Jacobian matrix operation introduced by expanding Kalman filter in the traditional calculation method are avoided, and the method has good filtering precision and expansibility for a high-dimensional nonlinear system and is more stable.
(2) Aiming at the prior unknown of the noise covariance matrix under most conditions, the self-adaptive adjustment of the noise covariance matrix by the Sage-Husa noise estimator ensures the semi-positive nature of the process noise covariance matrix Q and the positive nature of the measurement noise covariance matrix R, ensures the noise estimator to perform unbiased estimation as much as possible, and ensures the estimation precision.
(3) The gesture resolving efficiency of the traditional Kalman filtering can be reduced aiming at the interference generated by a complex environment, the Kalman gain is corrected by introducing the adaptive factor based on the prediction residual two-section function, the robustness of the filter is improved, and the effectiveness of gesture resolving under partial scenes is ensured.
Drawings
Fig. 1 is a schematic flow chart of an IMU pose resolving method provided by the present invention;
fig. 2 is a diagram showing a comparison of resolving effects of static experimental gestures of a desktop according to the IMU gesture resolving method provided by the present invention;
fig. 3 is a graph comparing weak vibration experimental gesture resolving effects of an IMU gesture resolving method provided by the present invention;
fig. 4 is a comparison chart of the mechanical arm random swing gesture resolving effect of the IMU gesture resolving method provided by the invention;
fig. 5 is a schematic structural diagram of an IMU pose solving apparatus according to the present invention.
Detailed Description
For a better understanding and implementation, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or modules is not necessarily limited to those steps or modules that are expressly listed or inherent to such process, method, article, or apparatus.
As shown in FIG. 1, the IMU gesture resolving method provided by the application avoids the high-order truncation error and Jacobian matrix operation introduced by expanding Kalman filtering in the traditional computing method by using the volume Kalman filtering as a basic data fusion algorithm, has good filtering precision and expansibility for a high-dimensional nonlinear system, and is more stable. Aiming at the prior unknown of the noise covariance matrix under most conditions, the self-adaptive adjustment of the noise covariance matrix by the Sage-Husa noise estimator ensures the semi-positive nature of the process noise covariance matrix Q and the positive nature of the measurement noise covariance matrix R, ensures the noise estimator to perform unbiased estimation as much as possible, and ensures the estimation precision. The gesture resolving efficiency of the traditional Kalman filtering can be reduced aiming at the interference generated by a complex environment, the Kalman gain is corrected by introducing the adaptive factor based on the prediction residual two-section function, the robustness of the filter is improved, and the effectiveness of gesture resolving under partial scenes is ensured.
Specifically, the method comprises the following steps:
s1, acquiring error parameters of a gyroscope and error parameters of an accelerometer, and constructing an error model of the gyroscope and the accelerometer.
The error parameters of the gyroscope include random constant drift and white noise in the measurement signal, while the error of the accelerometer includes white noise in the acceleration measurement signal. Based on the plurality of error parameters, an error model of the gyroscope and the accelerometer is constructed as follows:
wherein,,
is the main error of gyroscope, +.>
For random constant drift, ++>
For white noise in the gyroscope measurement signal, < >>
Representing random constant drift +.>
Derivative of>
Is the main error of the accelerometer, +.>
White noise in the signal is measured for the accelerometer.
And S2, constructing an acceleration output model based on the conversion relation of the quaternion vector representation navigation coordinate system and the carrier coordinate system.
Defining a unit quaternion vector as
The coordinate conversion relationship between the navigation coordinate system n and the carrier coordinate system b can be obtained by the quaternion coordinate conversion matrix +.>
The expression is as follows:
wherein R is a temporary variable.
Acceleration signal of acceleration in carrier coordinate system b system
Can be written as:
wherein,,
representing the absolute value of the gravitational constant, in this embodiment +.>
，/>
Is the measurement noise of the accelerometer.
S3, constructing a state equation of a filter according to the output signal of the gyroscope and the error model; constructing a measurement equation based on the acceleration output model;
the Kalman filtering adopted in the application generally adopts a state space model, and constructs a state equation of the filter according to a gyroscope output signal and an error model, and specifically comprises the following steps:
obtaining a differential equation of the output angular speed and the quaternion of the gyroscope:
wherein the method comprises the steps of
For the carrier coordinate system->
Is->
The system angular velocity matrix is represented as follows:
defining system state vectors
State vector dimension->
The differential equation to obtain the system state update is thus as follows:
wherein the gyroscope angular velocity estimation signal
Can be expressed as a gyroscope actual measurement signal
And gyroscope error->
And (5) subtracting. The Kalman filter usually adopts a discrete state space model, so that the Picard successive approximation method is adopted and the first order approximation is adopted, and the sampling time interval +.>
Discretizing the system differential equation is as follows:
wherein the method comprises the steps of
Process noise covariance matrix +.>
Wherein
Is a statistical mathematical expectation.
Based on the state equation, an error covariance matrix is obtained
，/>
Representing covariance.
Further, based on the output model of acceleration, a measurement equation is obtained
The following are provided:
discretizing the measurement equation
Thereby obtaining the measurement noise covariance matrix
。
And S4, setting a system state initial value, a noise covariance matrix initial value and an error covariance matrix initial value.
The system state initial value comprises a quaternion vector estimation initial value and a gyroscope zero offset estimation initial value. The initial value of zero offset estimation of the gyroscope is generally obtained by standing an IMU to measure output data of a plurality of groups of gyroscopes, namely, taking an average value of data obtained by measuring the triaxial gyroscopes as the initial value of zero offset estimation of the gyroscope. In the present embodiment, the initial value of the system state
Simultaneously selecting noise covariance matrix initial value +.>
And->
Error covariance matrix initial +.>
. In this example, the process noise covariance matrix initial value
Measuring noise covariance matrix initial value +.>
Error covariance matrix initial +.>
Can be set as follows:
wherein the method comprises the steps of
Representing a diagonal matrix.
S5, constructing a filter based on a volume Kalman filtering algorithm, introducing a Sage-Husa noise estimator to adaptively adjust a noise covariance matrix, correcting Kalman gain based on an adaptive factor of a prediction residual error two-section function, and updating the volume Kalman filter;
the volume Kalman filtering algorithm is adopted as a basic data fusion algorithm, so that high-order truncation errors and Jacobian matrix operation introduced by an extended Kalman filtering (EFK) algorithm are avoided, and the method has good filtering precision and expansibility for a high-dimensional nonlinear system, is more stable and has higher gesture resolving precision. In order to keep the filter at the optimal filtering effect in various scenes, an improved Sage-Husa noise estimator is introduced to adaptively adjust a noise covariance matrix. In order to improve the robustness of Kalman filtering, an adaptive factor based on a prediction residual two-segment function is introduced to correct Kalman gain.
The volume Kalman filtering update comprises state update, measurement update, error covariance matrix update and noise covariance matrix update;
(1) The status update is, in particular,
from the slave
Moment start calculation +.>
Square root of>
Volume point->
：
Wherein the method comprises the steps of
Representing a set of volumetric points; for function->
Substituting the volume point to obtain the estimated value +.>
；
Calculating state predictors
Predicted State covariance +.>
：
Wherein the method comprises the steps of
，/>
A correction value representing the process noise covariance matrix.
(2) The measurement update is performed, in particular,
calculation of
Square root of>
Is>
：
Volume point of propagation measurement
Calculating the estimated value of the measurement +.>
：
Calculating a measurement error covariance and a cross covariance:
meanwhile, the jacobian matrix of the measurement equation can be obtained:
using adaptive factors
Correcting a gain matrix of the Kalman filter:
wherein the method comprises the steps of
，/>
Is usually taken as +.>
The present embodiment is set to 1.
Learning statistics representing prediction residual, +.>
=/>
The prediction residual is represented as such,
the covariance matrix representing the prediction residual, tr (), represents the trace of the matrix.
Updating
System state at time and error covariance matrix:
according to the invention, the self-adaptive factor based on the prediction residual error two-section function is introduced to correct the Kalman gain, so that the interference generated by a complex environment is avoided, the gesture resolving efficiency of the traditional Kalman filtering is reduced, the robustness of the filter is improved, and the effectiveness of gesture resolving under partial scenes is ensured.
(3) The noise covariance matrix is updated specifically as follows:
wherein the method comprises the steps of
，/>
Is usually taken as amnesia factor->
The present example is set to 0.99.
The invention introduces an improved Sage-Husa noise estimator to the priori unknowable noise covariance matrix under most conditions, thereby ensuring the semi-positive nature of the process noise covariance matrix Q and the positive nature of the measurement noise covariance matrix R, and also enabling the noise estimator to perform unbiased estimation as much as possible and ensuring the estimation precision.
(4) Normalization process
Further, the normalization of the quaternion vector is affected by the existence of calculation errors in the filtering process, so that the normalization processing of the quaternion after each iteration comprises the following steps:
. Wherein->
Is the 2-norm of the quaternion vector, i.e. for the quaternion vector +.>
And (5) taking a mould.
And S6, updating the state and measurement information at the next moment, and carrying out attitude calculation on the updated quaternion vector to obtain the pitch angle and roll angle information after fusion filtering.
Specifically, after one iteration of the Kalman filtering algorithm, an updated system state can be obtained, the system state contains quaternion vectors, and attitude angles after fusion filtering, namely pitch angle and roll angle information, can be obtained through attitude conversion.
The pose of the quaternion vector is calculated as follows:
in order to verify the effectiveness of the present application, as shown in fig. 2, 3 and 4, three sets of comparative experiments, namely a tabletop static experiment, a weak vibration experiment and a mechanical arm random swing experiment, are set. Three gesture resolving methods are adopted in each group of comparison experiments: the performance of the traditional extended kalman filter algorithm (EKF), the traditional volumetric kalman filter algorithm (CKF), and the adaptive robust volumetric kalman filter Algorithm (ARCKF) as applied herein under different conditions was examined by comparison.
As can be seen from fig. 2, in the static desktop experiment, the accuracy of the calculation of the ARCKF pitch angle and roll angle is the highest, the error of CKF in the calculation of the roll angle is larger, and the calculation effect is unstable. Through error calculation, the Root Mean Square Error (RMSE) of ARCKF solution is 0.015 degrees and 0.0274 degrees respectively, and the solution accuracy is higher than that of the EKF algorithm and the CKF algorithm.
As can be seen from fig. 3, the ARCKF resolving effect performs best and the EKF resolving effect performs poorly in weak vibration experiments. The error calculation was performed such that the ARCKF-resolved RMSE was 0.1038 and 0.0979, respectively, and the EKF-resolved RMSE was 0.5383 and 0.4678, respectively, with the CKF filtering effect centered.
As can be seen from fig. 4, the ARCKF filtering effect also performs best in the mechanical arm random swing experiment. Through error calculation, the ARCKF calculated RMSE at the moment is 0.4998 degrees and 0.7228 degrees respectively, so that the method has good gesture calculating effect in various application scenes.
The processing method of the system may refer to the description of the above method, and will not be described herein.
As shown in fig. 5, the apparatus may include: a memory 51 storing executable program code;
a processor 52 coupled to the memory 51;
a transceiver 53 for communicating with other devices or communication networks, receiving or transmitting network messages;
a bus 54 for connecting the memory 51, the processor 52, and the transceiver 53 for internal communication.
The transceiver 53 receives the message transmitted over the network, and transmits the message to the processor 52 through the bus 54, and the processor 52 calls the executable program code stored in the memory 51 through the bus 54 to process the message, and transmits the processing result to the transceiver 53 through the bus 54 to send the processing result, thereby implementing the method provided by the embodiment of the present application.
Embodiments of the present application also provide a non-transitory machine-readable storage medium having stored thereon an executable program, which when executed by a processor, causes the processor to perform the processing method as provided in the above embodiments. A memory 51 storing executable program code;
a processor 52 coupled to the memory 51;
the processor 52 invokes executable program code stored in the memory 51 for performing the described time-sensitive implementation of the virtualized core network. The embodiment of the invention discloses a computer-readable storage medium storing a computer program for electronic data exchange, wherein the computer program causes a computer to execute the described IMU pose solving method.
Embodiments of the present invention disclose a computer program product comprising a non-transitory computer readable storage medium storing a computer program, and the computer program is operable to cause a computer to perform a method of IMU pose resolution as described.
The embodiments described above are illustrative only, and the modules illustrated as separate components may or may not be physically separate, and components shown as modules may or may not be physical modules, may be located in one place, or may be distributed over multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art will understand and implement the present invention without undue burden.
From the above detailed description of the embodiments, it will be apparent to those skilled in the art that the embodiments may be implemented by means of software plus necessary general hardware platforms, or of course by means of hardware. Based on such understanding, the foregoing technical solutions may be embodied essentially or in part in the form of a software product that may be stored in a computer-readable storage medium including Read-Only Memory (ROM), random access Memory (Random Access Memory, RAM), programmable Read-Only Memory (Programmable Read-Only Memory, PROM), erasable programmable Read-Only Memory (Erasable Programmable Read Only Memory, EPROM), one-time programmable Read-Only Memory (OTPROM), electrically erasable programmable Read-Only Memory (EEPROM), compact disc Read-Only Memory (Compact Disc Read-Only Memory, CD-ROM) or other optical disc Memory, magnetic disc Memory, tape Memory, or any other medium that can be used for computer-readable carrying or storing data.
Finally, it should be noted that: the embodiment of the invention discloses a time-sensitive implementation method and a system for a virtualized core network, which are disclosed by the embodiment of the invention only as a preferred embodiment of the invention, and are only used for illustrating the technical scheme of the invention, but not limiting the technical scheme; although the invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art will understand that; the technical scheme recorded in the various embodiments can be modified or part of technical features in the technical scheme can be replaced equivalently; such modifications and substitutions do not depart from the spirit and scope of the corresponding technical solutions.
Claims (10)
1. An IMU pose resolving method is characterized in that the method comprises the following steps:
acquiring error parameters of a gyroscope and error parameters of an accelerometer, and constructing an error model of the gyroscope and the accelerometer;
based on the conversion relation of the quaternion vector representation navigation coordinate system and the carrier coordinate system, constructing an acceleration output model;
constructing a state equation of a filter according to the output signal of the gyroscope and the error model; constructing a measurement equation based on the acceleration output model;
setting a system state initial value, a noise covariance matrix initial value and an error covariance matrix initial value;
constructing a filter based on a volume Kalman filtering algorithm, introducing a Sage-Husa noise estimator to adaptively adjust a noise covariance matrix, correcting Kalman gain based on an adaptive factor of a prediction residual error two-section function, and updating the volume Kalman filter;
updating the state and measurement information at the next moment, and performing attitude calculation on the updated quaternion vector to obtain the pitch angle and roll angle information after fusion filtering;
wherein, the Kalman gain is corrected by an adaptive factor based on a two-segment function of the prediction residual, specifically,
using adaptive factors
Correcting a gain matrix of the Kalman filter:
wherein the method comprises the steps of
，/>
Is usually taken as +.>
；/>
Learning statistics representing prediction residual, +.>
=/>
The prediction residual is represented as such,
covariance matrix representing prediction residual, tr ()'s represent the trace of the matrix;
wherein,,
is the cross covariance at time k+1, < ->
Measurement error covariance at time k+1, < >>
Jacobian matrix, which is the measurement equation at time k+1,>
measurement noise covariance matrix correction value for time k+1,>
is the predicted state covariance at time k+1.
2. The IMU pose solving method according to claim 1, wherein the error model of the gyroscope is:
wherein,,
is the main error of gyroscope, +.>
For random constant drift, ++>
For white noise in the gyroscope measurement signal, < >>
Representing random constant drift +.>
Derivative of>
Is the main error of the accelerometer, +.>
White noise in the signal is measured for the accelerometer.
3. The method of claim 2, wherein the conversion relation between the navigation coordinate system and the carrier coordinate system is represented based on a quaternion vector,
defining a unit quaternion vector
Navigation coordinate System->
Is associated with the carrier coordinate system->
Coordinate transformation matrix between systems>
The expression is as follows:
=
an acceleration output model is constructed, in particular,
accelerometer in carrier coordinate system
Acceleration signal ∈>
Can be written as:
wherein the method comprises the steps of
Representing absolute value of gravitational constant, +.>
Measurement noise for accelerometer, +.>
Is a coordinate transformation matrix.
4. An IMU pose solving method according to claim 3, wherein a state equation of the filter is constructed according to an output signal of the gyroscope and the error model, specifically,
according to the differential equation of the output angular velocity and quaternion of the gyroscope:
wherein the method comprises the steps of
For the carrier coordinate system->
Is->
The system angular velocity matrix is represented as follows:
defining system state vectors
State vector dimension
The differential equation to obtain the system state update is thus as follows:
wherein the gyroscope angular velocity estimation signal
Represented as actual measured signals of gyroscopes
And gyroscope error->
Subtracting; the Kalman filter usually adopts a discrete state space model, adopts a Picard successive approximation method and adopts a first order approximation, and simultaneously introduces a sampling time interval +.>
Pairing systemThe system differential equation discretization has:
wherein the method comprises the steps of
Process noise covariance matrix +.>
Wherein->
Is a statistical mathematical expectation; error covariance matrix->
，/>
Representing covariance;
a measurement equation is constructed based on the acceleration output model, specifically,
obtaining a measurement equation according to an output model of the accelerometer
The following are provided:
discretizing the measurement equation to obtain
The method comprises the steps of carrying out a first treatment on the surface of the Measuring noise covariance matrix
。
5. The method of claim 4, wherein the system state initial value, the noise covariance matrix initial value, and the error covariance matrix initial value are set, specifically,
measuring output data of the gyroscopes after the IMUs are placed still, and taking an average value of the output data as an estimated initial value of the state zero offset;
obtaining initial value of noise covariance matrix through static test
And->
Error covariance matrix initial +.>
。
6. The IMU pose solution method according to claim 5, wherein updating the volume kalman filter includes a state update, a measurement update, an error covariance matrix update, and a noise covariance matrix update;
the status update is performed, in particular,
from the slave
Calculating error covariance matrix from moment on>
Square root of>
Volume point->
：
Wherein the method comprises the steps of
Representing a set of volumetric points; for function->
Substituting the volume point to obtain the estimated value +.>
；
Calculating state predictors
Predicted State covariance +.>
:
Wherein the method comprises the steps of
，/>
Correction values representing a process noise covariance matrix;
the measurement update is performed, in particular,
calculation of
Square root of>
Is>
，
Volume point of propagation measurement
Calculating the estimated value of the measurement +.>
：
Calculating a measurement error covariance and a cross covariance:
meanwhile, the jacobian matrix of the measurement equation can be obtained:
7. the IMU pose resolution method of claim 6, wherein updating is performed
System state at time and error covariance matrix:
the noise covariance matrix is adaptively adjusted by a Sage-Husa noise estimator, which, in particular,
wherein the method comprises the steps of
，/>
Is forgetting factor, and has a value range of +.>
。
8. The method of claim 7, wherein the updated quaternion vector is subjected to gesture calculation to obtain the information of the pitch angle and the roll angle after fusion filtering, specifically,
9. an IMU pose resolving apparatus, comprising
A memory storing executable program code;
a processor coupled to the memory;
the processor invokes the executable program code stored in the memory to perform an IMU pose resolving method according to any of claims 1-8.
10. A storage medium having stored thereon a computer program which, when executed, implements an IMU pose resolving method according to any of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310339625.XA CN116067370B (en) | 2023-04-03 | 2023-04-03 | IMU gesture resolving method, IMU gesture resolving equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310339625.XA CN116067370B (en) | 2023-04-03 | 2023-04-03 | IMU gesture resolving method, IMU gesture resolving equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116067370A CN116067370A (en) | 2023-05-05 |
| CN116067370B true CN116067370B (en) | 2023-06-27 |
Family
ID=86180522
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310339625.XA Active CN116067370B (en) | 2023-04-03 | 2023-04-03 | IMU gesture resolving method, IMU gesture resolving equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116067370B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116382321B (en) * | 2023-06-05 | 2023-08-18 | 小神童创新科技(广州)有限公司 | Complete machine posture control system and method for electric wheelchair |
| CN116608853B (en) * | 2023-07-21 | 2023-09-29 | 广东智能无人系统研究院(南沙) | Carrier dynamic posture estimation method, device and storage medium |
| CN117879540B (en) * | 2024-03-12 | 2024-06-18 | 西南应用磁学研究所(中国电子科技集团公司第九研究所) | Magnetic compass sensor self-adaptive signal filtering method based on improved Kalman filtering |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109829938A (en) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | A kind of self-adapted tolerance volume kalman filter method applied in target following |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6859727B2 (en) * | 2003-01-08 | 2005-02-22 | Honeywell International, Inc. | Attitude change kalman filter measurement apparatus and method |
| ES2935067T3 (en) * | 2015-11-10 | 2023-03-01 | Thales Defense & Security Inc | Robust vision-inertial pedestrian tracking with heading autoalignment |
| CN105973238B (en) * | 2016-05-09 | 2018-08-17 | 郑州轻工业学院 | A kind of attitude of flight vehicle method of estimation based on norm constraint volume Kalman filtering |
| CN106291645B (en) * | 2016-07-19 | 2018-08-21 | 东南大学 | The volume kalman filter method coupled deeply suitable for higher-dimension GNSS/INS |
| 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 |
| CN106767837B (en) * | 2017-02-23 | 2019-10-22 | 哈尔滨工业大学 | Spacecraft attitude estimation method based on the estimation of volume quaternary number |
| CN108761512A (en) * | 2018-07-28 | 2018-11-06 | 南京理工大学 | A kind of adaptive CKF filtering methods of missile-borne BDS/SINS deep combinations |
| CN110455287A (en) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | Adaptive Unscented kalman particle filter method |
| CN110567455B (en) * | 2019-09-25 | 2023-01-03 | 哈尔滨工程大学 | Tightly-combined navigation method for quadrature updating volume Kalman filtering |
| CN111878064B (en) * | 2020-05-11 | 2024-04-05 | 中国科学院地质与地球物理研究所 | Gesture measurement method |
| CN112254718B (en) * | 2020-08-04 | 2024-04-09 | 东南大学 | Motion constraint assisted underwater integrated navigation method based on improved Sage-Husa self-adaptive filtering |
-
2023
- 2023-04-03 CN CN202310339625.XA patent/CN116067370B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109829938A (en) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | A kind of self-adapted tolerance volume kalman filter method applied in target following |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116067370A (en) | 2023-05-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116067370B (en) | IMU gesture resolving method, IMU gesture resolving equipment and storage medium | |
| CN112013836B (en) | Attitude heading reference system algorithm based on improved adaptive Kalman filtering | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| US11120562B2 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
| JP6255924B2 (en) | IC for sensor, sensor device, electronic device and mobile object | |
| CN113155129B (en) | Holder attitude estimation method based on extended Kalman filtering | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| CN106813679B (en) | Method and device for estimating attitude of moving object | |
| CN110887480A (en) | Flight attitude estimation method and system based on MEMS sensor | |
| CN107727114B (en) | Acceleration calibration method and system based on gyroscope, service terminal and memory | |
| CN111721288A (en) | Zero offset correction method and device for MEMS device and storage medium | |
| CN110873563B (en) | Cloud deck attitude estimation method and device | |
| CN116608853B (en) | Carrier dynamic posture estimation method, device and storage medium | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| CN115855048A (en) | Multi-sensor fusion pose estimation method | |
| CN108450007B (en) | High performance inertial measurement using redundant arrays of inexpensive inertial sensors | |
| CN111649747A (en) | IMU-based adaptive EKF attitude measurement improvement method | |
| CN113009816A (en) | Method and device for determining time synchronization error, storage medium and electronic device | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN106931965B (en) | Method and device for determining terminal posture | |
| JP2015094631A (en) | Position calculation device, and position calculation method | |
| CN114964214B (en) | Extended Kalman filtering attitude calculation method of attitude heading reference system | |
| CN110645976A (en) | Attitude estimation method of mobile robot and terminal equipment | |
| CN114323007A (en) | Carrier motion state estimation method and device | |
| CN115727871A (en) | Track quality detection method and device, electronic equipment and storage medium |
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 |