[go: up one dir, main page]

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 PDF

Info

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
Application number
CN202310339625.XA
Other languages
Chinese (zh)
Other versions
CN116067370A (en
Inventor
曾维湘
杨文林
王蕴婷
司徒伟伦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong Intelligent Unmanned System Research Institute Nansha
Original Assignee
Guangdong Intelligent Unmanned System Research Institute Nansha
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangdong Intelligent Unmanned System Research Institute Nansha filed Critical Guangdong Intelligent Unmanned System Research Institute Nansha
Priority to CN202310339625.XA priority Critical patent/CN116067370B/en
Publication of CN116067370A publication Critical patent/CN116067370A/en
Application granted granted Critical
Publication of CN116067370B publication Critical patent/CN116067370B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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

IMU gesture resolving method, IMU gesture resolving equipment and storage medium
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:
Figure SMS_1
Figure SMS_2
Figure SMS_3
wherein,,
Figure SMS_4
is the main error of gyroscope, +.>
Figure SMS_5
For random constant drift, ++>
Figure SMS_6
For white noise in the gyroscope measurement signal, < >>
Figure SMS_7
Representing random constant drift +.>
Figure SMS_8
Derivative of>
Figure SMS_9
Is the main error of the accelerometer, +.>
Figure SMS_10
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
Figure SMS_11
Said navigational coordinate system->
Figure SMS_12
Coordinate system of system and carrier
Figure SMS_13
Coordinate transformation matrix between systems>
Figure SMS_14
The expression is as follows:
Figure SMS_15
=
Figure SMS_16
the construction of the acceleration output model comprises the steps of, in particular,
accelerometer on carrierCoordinate system
Figure SMS_17
Acceleration signal ∈>
Figure SMS_18
Can be written as:
Figure SMS_19
wherein the method comprises the steps of
Figure SMS_20
Representing absolute value of gravitational constant, +.>
Figure SMS_21
Measurement noise for accelerometer, +.>
Figure SMS_22
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:
Figure SMS_23
wherein the method comprises the steps of
Figure SMS_24
For the carrier coordinate system->
Figure SMS_25
Is->
Figure SMS_26
The system angular velocity matrix is represented as follows:
Figure SMS_27
；
defining system state vectors
Figure SMS_28
State vector dimension->
Figure SMS_29
The differential equation to obtain the system state update is thus as follows:
Figure SMS_30
wherein the gyroscope angular velocity estimation signal
Figure SMS_31
Expressed as gyroscope actual measurement signal +.>
Figure SMS_32
And gyroscope error->
Figure SMS_33
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 +.>
Figure SMS_34
Discretizing the system differential equation is as follows:
Figure SMS_35
wherein the method comprises the steps of
Figure SMS_36
Process noise covariance matrix +.>
Figure SMS_37
Wherein->
Figure SMS_38
Is a statistical mathematical expectation; error covariance matrix->
Figure SMS_39
，
Figure SMS_40
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
Figure SMS_41
The following are provided:
Figure SMS_42
，
discretizing the measurement equation to obtain
Figure SMS_43
The method comprises the steps of carrying out a first treatment on the surface of the Measuring noise covariance matrix
Figure SMS_44
。
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
Figure SMS_45
And->
Figure SMS_46
Error covariance matrix initial +.>
Figure SMS_47
。
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
Figure SMS_48
Moment start calculation +.>
Figure SMS_49
Square root of>
Figure SMS_50
Volume point->
Figure SMS_51
：
Figure SMS_52
Figure SMS_53
Wherein the method comprises the steps of
Figure SMS_54
Representing a set of volumetric points; for function->
Figure SMS_55
Substituting the volume point to obtain the estimated value +.>
Figure SMS_56
；
Calculating state predictors
Figure SMS_57
Predicted State covariance +.>
Figure SMS_58
:
Figure SMS_59
Figure SMS_60
Wherein the method comprises the steps of
Figure SMS_61
，
Figure SMS_62
Correction values representing a process noise covariance matrix;
the measurement update is performed, in particular,
calculation of
Figure SMS_63
Square root of>
Figure SMS_64
Is>
Figure SMS_65
，
Figure SMS_66
Figure SMS_67
Volume point of propagation measurement
Figure SMS_68
Calculating the estimated value of the measurement +.>
Figure SMS_69
：
Figure SMS_70
Figure SMS_71
Calculating a measurement error covariance and a cross covariance:
Figure SMS_72
Figure SMS_73
Figure SMS_74
a correction value representing a measurement noise covariance matrix;
meanwhile, the jacobian matrix of the measurement equation can be obtained:
Figure SMS_75
。
in some embodiments, the kalman gain is modified by an adaptive factor based on a prediction residual two-stage function, specifically,
using adaptive factors
Figure SMS_76
Correcting a gain matrix of the Kalman filter:
Figure SMS_77
wherein the method comprises the steps of
Figure SMS_78
，
Figure SMS_79
Is usually taken as +.>
Figure SMS_80
；
Figure SMS_81
Learning statistics representing prediction residual, +.>
Figure SMS_82
=
Figure SMS_83
The prediction residual is represented as such,
Figure SMS_84
covariance matrix representing prediction residual, tr ()'s represent the trace of the matrix;
updating
Figure SMS_85
System state at time and error covariance matrix:
Figure SMS_86
Figure SMS_87
；
the noise covariance matrix is adaptively adjusted by a Sage-Husa noise estimator, which, in particular,
Figure SMS_88
Figure SMS_89
wherein the method comprises the steps of
Figure SMS_90
，
Figure SMS_91
Is forgetting factor, and has a value range of +.>
Figure SMS_92
。
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,
Figure SMS_93
Figure SMS_94
。
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:
Figure SMS_95
Figure SMS_96
Figure SMS_97
wherein,,
Figure SMS_98
is the main error of gyroscope, +.>
Figure SMS_99
For random constant drift, ++>
Figure SMS_100
For white noise in the gyroscope measurement signal, < >>
Figure SMS_101
Representing random constant drift +.>
Figure SMS_102
Derivative of>
Figure SMS_103
Is the main error of the accelerometer, +.>
Figure SMS_104
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
Figure SMS_105
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 +.>
Figure SMS_106
The expression is as follows:
Figure SMS_107
Figure SMS_108
wherein R is a temporary variable.
Acceleration signal of acceleration in carrier coordinate system b system
Figure SMS_109
Can be written as:
Figure SMS_110
wherein,,
Figure SMS_111
representing the absolute value of the gravitational constant, in this embodiment +.>
Figure SMS_112
，
Figure SMS_113
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:
Figure SMS_114
wherein the method comprises the steps of
Figure SMS_115
For the carrier coordinate system->
Figure SMS_116
Is->
Figure SMS_117
The system angular velocity matrix is represented as follows:
Figure SMS_118
defining system state vectors
Figure SMS_119
State vector dimension->
Figure SMS_120
The differential equation to obtain the system state update is thus as follows:
Figure SMS_121
wherein the gyroscope angular velocity estimation signal
Figure SMS_122
Can be expressed as a gyroscope actual measurement signal
Figure SMS_123
And gyroscope error->
Figure SMS_124
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 +.>
Figure SMS_125
Discretizing the system differential equation is as follows:
Figure SMS_126
wherein the method comprises the steps of
Figure SMS_127
Process noise covariance matrix +.>
Figure SMS_128
Wherein
Figure SMS_129
Is a statistical mathematical expectation.
Based on the state equation, an error covariance matrix is obtained
Figure SMS_130
，
Figure SMS_131
Representing covariance.
Further, based on the output model of acceleration, a measurement equation is obtained
Figure SMS_132
The following are provided:
Figure SMS_133
；
discretizing the measurement equation
Figure SMS_134
Thereby obtaining the measurement noise covariance matrix
Figure SMS_135
。
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
Figure SMS_136
Simultaneously selecting noise covariance matrix initial value +.>
Figure SMS_137
And->
Figure SMS_138
Error covariance matrix initial +.>
Figure SMS_139
. In this example, the process noise covariance matrix initial value
Figure SMS_140
Measuring noise covariance matrix initial value +.>
Figure SMS_141
Error covariance matrix initial +.>
Figure SMS_142
Can be set as follows:
Figure SMS_143
Figure SMS_144
Figure SMS_145
wherein the method comprises the steps of
Figure SMS_146
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
Figure SMS_147
Moment start calculation +.>
Figure SMS_148
Square root of>
Figure SMS_149
Volume point->
Figure SMS_150
：
Figure SMS_151
Figure SMS_152
Wherein the method comprises the steps of
Figure SMS_153
Representing a set of volumetric points; for function->
Figure SMS_154
Substituting the volume point to obtain the estimated value +.>
Figure SMS_155
；
Calculating state predictors
Figure SMS_156
Predicted State covariance +.>
Figure SMS_157
：
Figure SMS_158
Figure SMS_159
Wherein the method comprises the steps of
Figure SMS_160
，
Figure SMS_161
A correction value representing the process noise covariance matrix.
(2) The measurement update is performed, in particular,
calculation of
Figure SMS_162
Square root of>
Figure SMS_163
Is>
Figure SMS_164
：
Figure SMS_165
Figure SMS_166
Volume point of propagation measurement
Figure SMS_167
Calculating the estimated value of the measurement +.>
Figure SMS_168
：
Figure SMS_169
Figure SMS_170
Calculating a measurement error covariance and a cross covariance:
Figure SMS_171
Figure SMS_172
Figure SMS_173
a correction value representing a measurement noise covariance matrix;
meanwhile, the jacobian matrix of the measurement equation can be obtained:
Figure SMS_174
。
using adaptive factors
Figure SMS_175
Correcting a gain matrix of the Kalman filter:
Figure SMS_176
wherein the method comprises the steps of
Figure SMS_177
，
Figure SMS_178
Is usually taken as +.>
Figure SMS_179
The present embodiment is set to 1.
Figure SMS_180
Learning statistics representing prediction residual, +.>
Figure SMS_181
=
Figure SMS_182
The prediction residual is represented as such,
Figure SMS_183
the covariance matrix representing the prediction residual, tr (), represents the trace of the matrix.
Updating
Figure SMS_184
System state at time and error covariance matrix:
Figure SMS_185
Figure SMS_186
。
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:
Figure SMS_187
Figure SMS_188
wherein the method comprises the steps of
Figure SMS_189
，
Figure SMS_190
Is usually taken as amnesia factor->
Figure SMS_191
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:
Figure SMS_192
. Wherein->
Figure SMS_193
Is the 2-norm of the quaternion vector, i.e. for the quaternion vector +.>
Figure SMS_194
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:
Figure SMS_195
Figure SMS_196
。
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
Figure QLYQS_1
Correcting a gain matrix of the Kalman filter:
Figure QLYQS_2
wherein the method comprises the steps of
Figure QLYQS_3
，
Figure QLYQS_4
Is usually taken as +.>
Figure QLYQS_5
；
Figure QLYQS_6
Learning statistics representing prediction residual, +.>
Figure QLYQS_7
=
Figure QLYQS_8
The prediction residual is represented as such,
Figure QLYQS_9
covariance matrix representing prediction residual, tr ()'s represent the trace of the matrix;
wherein,,
Figure QLYQS_10
is the cross covariance at time k+1, < ->
Figure QLYQS_11
Measurement error covariance at time k+1, < >>
Figure QLYQS_12
Jacobian matrix, which is the measurement equation at time k+1,>
Figure QLYQS_13
measurement noise covariance matrix correction value for time k+1,>
Figure QLYQS_14
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:
Figure QLYQS_15
Figure QLYQS_16
Figure QLYQS_17
wherein,,
Figure QLYQS_18
is the main error of gyroscope, +.>
Figure QLYQS_19
For random constant drift, ++>
Figure QLYQS_20
For white noise in the gyroscope measurement signal, < >>
Figure QLYQS_21
Representing random constant drift +.>
Figure QLYQS_22
Derivative of>
Figure QLYQS_23
Is the main error of the accelerometer, +.>
Figure QLYQS_24
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
Figure QLYQS_25
Navigation coordinate System->
Figure QLYQS_26
Is associated with the carrier coordinate system->
Figure QLYQS_27
Coordinate transformation matrix between systems>
Figure QLYQS_28
The expression is as follows:
Figure QLYQS_29
=
Figure QLYQS_30
an acceleration output model is constructed, in particular,
accelerometer in carrier coordinate system
Figure QLYQS_31
Acceleration signal ∈>
Figure QLYQS_32
Can be written as:
Figure QLYQS_33
wherein the method comprises the steps of
Figure QLYQS_34
Representing absolute value of gravitational constant, +.>
Figure QLYQS_35
Measurement noise for accelerometer, +.>
Figure QLYQS_36
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:
Figure QLYQS_37
wherein the method comprises the steps of
Figure QLYQS_38
For the carrier coordinate system->
Figure QLYQS_39
Is->
Figure QLYQS_40
The system angular velocity matrix is represented as follows:
Figure QLYQS_41
；
defining system state vectors
Figure QLYQS_42
State vector dimension
Figure QLYQS_43
The differential equation to obtain the system state update is thus as follows:
Figure QLYQS_44
wherein the gyroscope angular velocity estimation signal
Figure QLYQS_45
Represented as actual measured signals of gyroscopes
Figure QLYQS_46
And gyroscope error->
Figure QLYQS_47
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 +.>
Figure QLYQS_48
Pairing systemThe system differential equation discretization has:
Figure QLYQS_49
wherein the method comprises the steps of
Figure QLYQS_50
Process noise covariance matrix +.>
Figure QLYQS_51
Wherein->
Figure QLYQS_52
Is a statistical mathematical expectation; error covariance matrix->
Figure QLYQS_53
，
Figure QLYQS_54
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
Figure QLYQS_55
The following are provided:
Figure QLYQS_56
，
discretizing the measurement equation to obtain
Figure QLYQS_57
The method comprises the steps of carrying out a first treatment on the surface of the Measuring noise covariance matrix
Figure QLYQS_58
。
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
Figure QLYQS_59
And->
Figure QLYQS_60
Error covariance matrix initial +.>
Figure QLYQS_61
。
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
Figure QLYQS_62
Calculating error covariance matrix from moment on>
Figure QLYQS_63
Square root of>
Figure QLYQS_64
Volume point->
Figure QLYQS_65
：
Figure QLYQS_66
Figure QLYQS_67
Figure QLYQS_68
Wherein the method comprises the steps of
Figure QLYQS_69
Representing a set of volumetric points; for function->
Figure QLYQS_70
Substituting the volume point to obtain the estimated value +.>
Figure QLYQS_71
；
Calculating state predictors
Figure QLYQS_72
Predicted State covariance +.>
Figure QLYQS_73
:
Figure QLYQS_74
Figure QLYQS_75
Wherein the method comprises the steps of
Figure QLYQS_76
，
Figure QLYQS_77
Correction values representing a process noise covariance matrix;
the measurement update is performed, in particular,
calculation of
Figure QLYQS_78
Square root of>
Figure QLYQS_79
Is>
Figure QLYQS_80
，
Figure QLYQS_81
Figure QLYQS_82
Volume point of propagation measurement
Figure QLYQS_83
Calculating the estimated value of the measurement +.>
Figure QLYQS_84
：
Figure QLYQS_85
Figure QLYQS_86
Calculating a measurement error covariance and a cross covariance:
Figure QLYQS_87
Figure QLYQS_88
Figure QLYQS_89
a correction value representing a measurement noise covariance matrix;
meanwhile, the jacobian matrix of the measurement equation can be obtained:
Figure QLYQS_90
。
7. the IMU pose resolution method of claim 6, wherein updating is performed
Figure QLYQS_91
System state at time and error covariance matrix:
Figure QLYQS_92
Figure QLYQS_93
；
Figure QLYQS_94
error covariance matrix at time k+1,>
Figure QLYQS_95
for the predicted state covariance at time k+1, < >>
Figure QLYQS_96
A state predicted value at time k+1;
the noise covariance matrix is adaptively adjusted by a Sage-Husa noise estimator, which, in particular,
Figure QLYQS_97
Figure QLYQS_98
wherein the method comprises the steps of
Figure QLYQS_99
，
Figure QLYQS_100
Is forgetting factor, and has a value range of +.>
Figure QLYQS_101
。
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,
Figure QLYQS_102
Figure QLYQS_103
。
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.
CN202310339625.XA 2023-04-03 2023-04-03 IMU gesture resolving method, IMU gesture resolving equipment and storage medium Active CN116067370B (en)

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 (5)

* Cited by examiner, † Cited by third party
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
CN118200726B (en) * 2024-03-01 2024-10-01 圆周率科技(常州)有限公司 Image processing method and device and shooting equipment
CN117879540B (en) * 2024-03-12 2024-06-18 西南应用磁学研究所(中国电子科技集团公司第九研究所) Magnetic compass sensor self-adaptive signal filtering method based on improved Kalman filtering
CN117972637B (en) * 2024-03-28 2024-06-21 天津大学 Angular velocity data fusion method and angular velocity data fusion device for angular vibration table

Citations (1)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
US10976163B2 (en) * 2015-11-10 2021-04-13 Thales Defense & Security, Inc. Robust vision-inertial pedestrian tracking with heading auto-alignment
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
WO2018026544A1 (en) * 2016-07-22 2018-02-08 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 Volumetric Quaternion Estimation
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

Patent Citations (1)

* Cited by examiner, † Cited by third party
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
CN104884902B (en) For three axle magnetometer and the method and apparatus of the data fusion of three axis accelerometer
CN109163721A (en) Attitude measurement method and terminal device
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
CN105716610B (en) A kind of attitude of carrier and course calculating method and system of Geomagnetic Field Model auxiliary
CN106813679B (en) Method and device for estimating attitude of moving object
CN110887480A (en) Flight attitude estimation method and system based on MEMS sensor
CN112859139B (en) Gesture measurement method and device and electronic equipment
CN107402007A (en) A kind of method for improving miniature AHRS modules precision and miniature AHRS modules
CN110440797A (en) Vehicle attitude estimation method and system
CN113155129A (en) Holder attitude estimation method based on extended Kalman filtering
CN115855048A (en) Multi-sensor fusion pose estimation method
CN110873563B (en) Cloud deck attitude estimation method and device
CN110645976A (en) Attitude estimation method of mobile robot and terminal equipment
CN116608853B (en) Carrier dynamic posture estimation method, device and storage medium
CN106931965B (en) A method and device for determining the attitude of a terminal
JP2015094631A (en) Position calculation apparatus and position calculation method
CN113009816B (en) Method and device for determining time synchronization error, storage medium and electronic device
JP2019082328A (en) Position estimation device
CN113447019A (en) INS/CNS integrated navigation method, system, storage medium and equipment
CN114964214B (en) Extended Kalman filtering attitude calculation method of attitude heading reference system
CN117029810A (en) Mahony gesture measurement method based on iterative EKF
CN112859138B (en) Gesture measurement method and device and electronic equipment

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