CN110864684A - User posture measuring and calculating method - Google Patents
User posture measuring and calculating method Download PDFInfo
- Publication number
- CN110864684A CN110864684A CN201911210177.3A CN201911210177A CN110864684A CN 110864684 A CN110864684 A CN 110864684A CN 201911210177 A CN201911210177 A CN 201911210177A CN 110864684 A CN110864684 A CN 110864684A
- Authority
- CN
- China
- Prior art keywords
- vector data
- user
- magnetometer
- angular velocity
- magnetic field
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 230000001133 acceleration Effects 0.000 claims abstract description 53
- 230000003068 static effect Effects 0.000 claims abstract description 51
- 238000012544 monitoring process Methods 0.000 claims abstract description 5
- 238000001914 filtration Methods 0.000 claims description 24
- 230000000295 complement effect Effects 0.000 claims description 11
- 230000005484 gravity Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000003247 decreasing effect Effects 0.000 claims description 3
- 230000008901 benefit Effects 0.000 abstract description 5
- 239000011159 matrix material Substances 0.000 description 22
- 230000036544 posture Effects 0.000 description 21
- 238000005259 measurement Methods 0.000 description 6
- 230000009471 action Effects 0.000 description 5
- 230000004927 fusion Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 230000002829 reductive effect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000007499 fusion processing Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000036961 partial effect Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000012546 transfer 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Gyroscopes (AREA)
Abstract
A user gesture reckoning method, comprising: acquiring acceleration vector data of a user, which is acquired by an accelerometer, and monitoring the variable quantity of a modulus value of the acceleration vector data in a preset time period; judging whether the gyroscope is in a static or uniform state or not according to the variable quantity of the modulus value, if so, collecting angular velocity vector data of the gyroscope at the current moment, and recording the angular velocity vector data as static drift vector data; eliminating static drift vector data in angular velocity vector data of a user, which is acquired by a gyroscope; performing auxiliary calibration on the magnetometer by adopting the angular velocity vector data after the static drift vector data is eliminated; setting a confidence parameter, wherein the confidence parameter is used for measuring the accuracy of the accelerometer and the magnetometer; and according to the confidence parameters, fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer to obtain a quaternion corresponding to the user posture at the current moment. The method has the advantages of convenient and fast calibration, real-time performance and small course angle drift.
Description
Technical Field
The invention relates to the field of inertial navigation, in particular to a user posture measuring and calculating method.
Background
Acquiring pose information of equipment is one of key technologies in the field of consumer electronics. The attitude angle of the device may be obtained using inertial sensors (e.g., accelerometers, gyroscopes, magnetometers, etc.). The existing attitude measurement and calculation method based on the inertial sensor has the problems that the time is prolonged and the course angle drift is increased, and even an algorithm for calibrating the magnetometer by using ellipsoid fitting has the defects of incapability of calibrating in real time, incapability of coping with environmental changes, high requirement on user action and the like.
In the related art, the gyroscope, the accelerometer and the magnetometer are fused through kalman filtering or complementary filtering, however, the method adopts a plane calibration method or a splay calibration method for calibrating the magnetometer, and if the method is directly used after a calibration action is performed, the method cannot cope with the change of the environment in real time, thereby causing the accumulation of errors.
Disclosure of Invention
Technical problem to be solved
The invention provides a user posture measuring and calculating method aiming at the prior technical problems, which is used for at least partially solving one of the technical problems.
(II) technical scheme
The invention provides a user posture measuring and calculating method on one hand, which comprises the following steps: acquiring acceleration vector data of a user, which is acquired by an accelerometer, and monitoring the variable quantity of a modulus value of the acceleration vector data in a preset time period; judging whether the gyroscope is in a static or uniform state or not according to the variable quantity of the modulus, if so, collecting angular velocity vector data of the gyroscope at the current moment, and recording the angular velocity vector data as static drift vector data; eliminating static drift vector data in angular velocity vector data of a user, which is acquired by a gyroscope; performing auxiliary calibration on the magnetometer by adopting the angular velocity vector data after the static drift vector data is eliminated; setting a confidence parameter for measuring the accuracy of the accelerometer and the magnetometer; and according to the confidence parameters, fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer to obtain a quaternion corresponding to the user posture at the current moment.
Optionally, the performing auxiliary calibration on the magnetometer by using the angular velocity vector data after the static drift vector data is eliminated includes: and based on the angular velocity vector data after the static drift vector data is eliminated, performing auxiliary calibration on the magnetometer by adopting an extended Kalman filtering algorithm.
Optionally, the fusing the acceleration vector data of the user, the angular velocity vector data of the user, and the magnetic field vector data measured by the magnetometer according to the reliability parameter includes: converting gravity vector data and magnetic field vector data under the terrestrial coordinate system into a sensor coordinate system to obtain reference acceleration vector data and reference magnetic field vector data; cross-multiplying the reference acceleration vector data and the acceleration vector data of the user to obtain a first control error; cross-multiplying the reference magnetic field vector data by the magnetic field vector data measured by the magnetometer to obtain a second control error; setting the size of the confidence parameter, and adjusting the weight of the first control error and the second control error; and according to the weight, combining quaternions corresponding to the user postures at the historical moment, fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer to obtain quaternions corresponding to the current user postures.
Optionally, the determining whether the gyroscope is in a stationary state or a constant speed state according to the variation of the modulus includes: judging whether the variable quantity is smaller than a preset threshold value, if so, counting once by using a static counter, and if not, resetting the counting times of the static counter and judging again; and repeating the operation until the counting frequency of the static counter reaches a preset frequency, and judging that the gyroscope is in a static or uniform speed state at the moment.
Optionally, the performing auxiliary calibration on the magnetometer by using the angular velocity vector data after the static drift vector data is eliminated further includes: and setting control parameters, wherein the control parameters are used for determining the weight occupied by the angular velocity vector data after the static drift vector data is eliminated for auxiliary calibration in the process of calibrating the magnetometer.
Optionally, before the fusing the acceleration vector data of the user, the angular velocity vector data of the user, and the magnetic field vector data measured by the magnetometer, the method further includes: and judging whether the magnetometer is calibrated or not, and fusing the magnetic field vector data measured by the calibrated magnetometer with the acceleration vector data of the user and the angular velocity vector data of the user.
Optionally, the determining whether the magnetometer is calibrated includes: acquiring a section of magnetic field data measured by the magnetometer, and calculating the root mean square error of other magnetic field data and the real value data by taking the data of the last point of the section of magnetic field data as real value data; and judging whether the root mean square error is smaller than a preset threshold value, if so, finishing the calibration of the magnetometer.
Optionally, when the user is in a stationary state or a constant speed state, setting the trust parameter to a constant value; and gradually decreasing the value of the confidence parameter when the acceleration of the user is gradually increased.
Optionally, the fusing the acceleration vector data of the user, the angular velocity vector data of the user, and the magnetic field vector data measured by the magnetometer in combination with the quaternion corresponding to the user posture at the historical time to obtain the quaternion corresponding to the current user posture includes: using a formula
Integrating after iterative operation to obtain a quaternion corresponding to the current user posture; wherein,
representing the user's pose of the iteration at the previous time corresponds to a quaternion,Sωtvector data representing the angular velocity of the user measured by the gyroscope at the current time,
and the differential of the quaternion corresponding to the user posture at the current moment is shown, and t represents the moment.
Optionally, a complementary filtering algorithm is adopted to fuse the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer; the method further comprises the following steps: and initializing the extended Kalman filtering algorithm and the complementary filtering algorithm.
(III) advantageous effects
The user posture measuring and calculating method provided by the invention at least has the following beneficial effects:
1. based on angular velocity vector data acquired by the gyroscope and subjected to static drift vector data elimination, the extended Kalman filtering is adopted to perform auxiliary calibration on the magnetometer, so that dynamic calibration of a magnetic field is realized, the method has the advantages of speed and precision, no need of executing complex calibration actions, and convenience and real-time calibration.
2. By setting the confidence parameters, the acceleration vector data and the magnetic field vector data are screened in the nine-axis data fusion process, so that the error caused by sudden change of the environment where the user is located is reduced, and the method has the advantage of small course angle drift.
3. The method for calibrating the magnetometer based on the assistance of the gyroscope can be applied to more scenes such as VR equipment, wearable equipment, automobile navigation and the like, is more convenient and faster compared with other dynamic or static magnetic field calibration methods, and can break through some limitations in use.
Drawings
FIG. 1 schematically illustrates a schematic diagram of a user gesture estimation method provided by an embodiment of the invention;
FIG. 2 is a flow chart of a method for measuring and calculating user gestures according to an embodiment of the present invention;
fig. 3 schematically illustrates a flowchart of a method for determining a static or uniform speed state of a gyroscope according to an embodiment of the present invention;
FIG. 4 schematically illustrates an exponential function plot of a simulated trust parameter provided by an embodiment of the present invention;
fig. 5 schematically illustrates a schematic diagram of nine-axis data fusion provided by an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to specific embodiments and the accompanying drawings.
The embodiment of the invention provides a user posture measuring and calculating method, which is based on the principle shown in figure 1, and is characterized in that in the magnetometer calibration process, the angular velocity vector data acquired based on a gyroscope and subjected to static drift vector data elimination is subjected to auxiliary calibration, and in the nine-axis data fusion process, acceleration vector data and magnetic field vector data are screened by setting trust parameters, so that quaternion corresponding to the user posture is obtained.
Fig. 2 schematically shows a flowchart of a user gesture calculating method provided by an embodiment of the present invention. As shown in fig. 2, the method may include operations S201 to S206, for example.
S201, initializing the extended Kalman filtering algorithm and the complementary filtering algorithm.
And when subsequent magnetometer calibration and nine-axis data fusion are performed, an extended Kalman filtering algorithm and a complementary filtering algorithm can be used respectively. Because the extended kalman filter and the complementary filter algorithm are iterative, the initial iterative values of various variables in the algorithm are initialized, including: hard magnetic interference matrix, soft magnetic interference matrix, rotation matrix, quaternion vector, gyroscope drift error vector, etc.
S202, acquiring acceleration vector data of a user, acquired by an accelerometer, monitoring the variable quantity of a module value of the acceleration vector data in a preset time period, judging whether the gyroscope is in a static or uniform state or not according to the variable quantity of the module value, and if so, acquiring angular velocity vector data of the gyroscope at the current moment and recording the angular velocity vector data as static drift vector data.
The dominant error of a gyroscope is the static drift error, which is a time-varying value. In order to eliminate triaxial angular velocity vector data (unit rad/s, recorded as g ═ g) collected by a gyroscopexgygz]T) In the static drift error, it is necessary to obtain angular velocity vector data of the gyroscope in a static or constant velocity state, and the data is recorded as an offset of the gyroscope (recorded as ∈ ═ epsilon [ ∈ [ ])xεyεz]T). The specific process flow is shown in fig. 3.
Firstly, the dynamic acquisition of triaxial acceleration vector data (unit m/s) measured by an accelerometer is needed2) And monitoring the module value variation (recorded as delta | g |) of the triaxial acceleration vector data in a preset time period, namely calculating the difference value between the module value at the current moment and the module value at the previous moment.
And secondly, judging whether the gyroscope is in a static or uniform speed state according to the variable quantity of the modulus value. Specifically, it is determined whether the variation of the module value at one time is smaller than a preset threshold (the threshold may be a value (0.001) much smaller than G (9.81m/s 2)), if so, the static counter is used to count once, the static counter is incremented by 1, if not, the count number of the static counter is cleared, and it is determined again whether the variation of the module value at the next time is smaller than the preset threshold. And repeating the operation until the counting frequency of the static counter reaches the preset frequency (the counting frequency of the counter is full), and judging that the gyroscope is in a static or uniform speed state at the moment.
In the above steps, low-pass filtering is required when the acceleration module value change and the gyroscope drift are recorded, and if the respective low-pass filtering parameters are α and β, the iterative formulas are respectively
Δ|g|k＝(1-α)Δ|g|k-1+α(Δ|g|k-Δ|g|k-1)
εk＝(1-β)εk-1+βεk
Where k represents a certain time and k-1 represents the time immediately preceding the time k.
And S203, eliminating static drift vector data in the angular velocity vector data of the user collected by the gyroscope.
After the offset epsilon of the gyroscope in a static or uniform speed state of the gyroscope is obtained, the epsilon is subtracted from the angular velocity vector data measured by the gyroscope, and then the calibration value of the gyroscope can be obtained.
And S204, performing auxiliary calibration on the magnetometer by adopting the angular velocity vector data after the static drift vector data is eliminated.
In the foregoing operation, the largest error source of the gyroscope is eliminated, and at this time, the calibrated gyroscope can be used to assist in calibrating the magnetometer.
Specifically, by the relationship between the angular velocity matrix corresponding to the angular velocity vector data of the gyroscope and the rotation matrix I, a theoretical value recurrence formula of the magnetometer can be calculated:
Bk+1＝(I+[ωk]×Δt)Bk
wherein, [ omega ]k]×Is the angular velocity matrix, Δ t is the sampling interval, BkA matrix of magnetic field vector data correspondences for the magnetometer at time k, Bk+1The magnetic field vector data for the magnetometer at time k +1 corresponds to the matrix.
Corresponding the magnetic field vector data to 6 independent components (W) in a matrix B, a hard magnetic interference matrix V and a soft magnetic interference matrix W11W22W33W12W13W23) Combined into a Kalman filtering state quantity x, to obtain a state transfer function
The specific form of (1):
Bcal，k＝(I+[ωcal，k-1]×Δt)Bcal，k-1-[εg]×ΔtBcal，k-1
Welement，k＝Welemment，k-1
Vk＝Vk-1
wherein, [ epsilon ]g]×For being stationary by gyroscopes3 x 3 matrix of drift, WelementIs a column vector of 6 independent components in W, V is a hard magnetic interference matrix, which is a 3 × 1 column vector itself, k represents time, BcalFor the calibrated magnetic field vector data correspondence matrix, ωcalThe vector data for the calibrated angular velocity corresponds to a matrix.
In the course of calibrating magnetometer, in order to define the weight occupied by angular velocity vector data after eliminating static drift vector data to make auxiliary calibration, a control parameter can be set
The variance matrix Q of the above equation of statek-1Can be that
Wherein,
for controlling the degree of utilization of the gyroscope. When the sampling rate of the gyroscope is low or the drift phenomenon is serious, the gyroscope takes a larger value, the proportion of the value obtained by the calculation of the state equation in the filtering process is reduced, and the filtering result depends on the measurement value of the magnetometer more. SigmagThe white noise can be preset according to experience for the gyroscope to measure. 0x*yIs a zero matrix.
The measurement equation for a magnetometer is
Bp，k＝WkBcal，k+Vk+εm
Wherein, BpAs direct measurement of the magnetometer, BcalFor the calibration values of the magnetometer, W and V are respectively the soft and hard magnetic interference matrices, εmFor measuring noise, the state quantity is
So equation B is measuredp，k＝h(Xk)+VkIs non-linear, so its Jacobian matrix is required to make itAnd (6) linearization is carried out. The state quantity is xk＝[BxByBzW11W22W33W12W13W23V1V2V3]TTo measurement equation Bp，k＝WkBcal，k+Vk+εmSolving a first-order partial derivative to obtain:
Hk＝[Wk|k-1，hWk，I3×3]
wherein HkIs a Jacobian matrix of measurement equations, Wk|k-1Still a soft magnetic interference matrix, I3×3Is a third order unit matrix.
Based on the magnetometer calibration process described above, arbitrary actions can be performed to calibrate without requiring strict calibration around the "8" word motion.
S205, setting a trust parameter, which is used to measure the accuracy of the accelerometer and the magnetometer.
For accelerometers, the main errors affecting the algorithm are derived from non-gravitational acceleration, and for magnetometers, the calibrated magnetic field values may also be subject to errors in the presence of significant nearby disturbances. Therefore, to reduce the impact of adverse conditions, embodiments of the present invention may employ a confidence parameter to measure the respective accuracy, with the value of the confidence parameter being gradually decreased as the acceleration of the user gradually increases from zero.
For acceleration, consider three cases:
1. in a static or uniform speed state, the accelerometer only has the action of gravity acceleration, and the confidence parameter is a constant value and is the maximum value.
2. In the low acceleration state, other acceleration disturbances other than the gravity acceleration occur, but the disturbance is not large, the influence on the algorithm is not large, and the algorithm should reduce the dependence on the accelerometer appropriately, so that the trust parameter is slightly reduced.
3. The high acceleration state, which is the largest error source, should reduce the dependence on the accelerometer infinitely, and even approach 0 when the non-gravitational acceleration is extremely large.
The embodiment of the invention uses an exponential function to simulate the relation between the trust parameter and the actual condition, records the normalized acceleration vector as a, gamma as a coefficient and Kp0Is the proportional element (constant) of the PI controller in the complementary filtering, the exponential function KpCan be expressed as:
Kp＝Kp0exp(-γΔ|a|)，Δa＝|a|-1
get Kp0When the value is 1 and γ is 2, the exponential function is shown in fig. 4.
The same is true for the magnetic field calibration value, because the magnetic field intensity after calibration is certain, the confidence parameter function of the calibration magnetic information can be obtained by adjusting gamma and 1 in the formula.
And S206, fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer according to the confidence parameters to obtain a quaternion corresponding to the user posture at the current moment.
Because the magnetic field calibration needs a short time, when the algorithm is started for data fusion, if the magnetic field information output by the EKF is directly used, the algorithm is sometimes misaligned, so that the EKF output should be temporarily shielded when the EKF is started, but the EKF is kept running. And meanwhile, the magnetic field calibration value output by the EKF is judged. If available, the magnetic field calibration values output by the EKF are input into subsequent complementary filtering, if not available, the magnetometer inputs to the complementary filtering are truncated.
The dynamic judgment method comprises the following steps: acquiring a section of magnetic field data measured by the magnetometer (a window with a fixed length can be taken to slide forwards on the magnetic field intensity calculated in each iteration), and calculating the root mean square error between other magnetic field data and the real value data by taking the data of the last point of the section of magnetic field data as real value data; and judging whether the root mean square error is smaller than a preset threshold value, if so, finishing the calibration of the magnetometer. The longer the window length is, the more strict the determination condition is; the shorter the window length, the faster the RMSE can follow the actual motion, also losing some of the accuracy. The RMSE threshold, window length, may be set empirically.
After the calibration is judged to be finished, data fusion is carried out, firstly, gravity vector data and magnetic field vector data under the terrestrial coordinate system are converted into a sensor coordinate system, and reference acceleration vector data and reference magnetic field vector data are obtained; then, cross-multiplying the reference acceleration vector data and the acceleration vector data of the user to obtain a first control error; the magnetic field vector data measured by the reference magnetic field vector data magnetometer is cross-multiplied to obtain a second control error; and finally, setting the magnitude of the confidence parameter, adjusting the weights of the first control error and the second control error, and fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer according to the weights and the quaternion corresponding to the user posture at the historical moment to obtain the quaternion corresponding to the current user posture.
The attitude angle can be obtained by quaternion iteration, and the angular velocity vector measured by the gyroscope is set asSω＝[0 ωxωyωz]Then the derivative of the discrete form quaternion is
Wherein,
representing the user's pose of the iteration at the previous time corresponds to a quaternion, which represents the rotation from the earth system to the carrier system, superscript ^ indicates that it is the "true value" of the a posteriori,Sωtvector data representing the angular velocity of the user measured by the gyroscope at the current time,
and the differential of the quaternion corresponding to the user posture at the current moment is shown, and t represents the moment. Thus, when Δ t is sufficiently small, the quaternion at the current time can be obtained by integrating its differential:
the following description is made with reference to a specific schematic diagram, and the fusion principle of the sensor is shown in FIG. 5, wherein KpNumerically equal to the above mentioned trust parameter. Referring to fig. 5, in which,Sωt、Sat、Smtrespectively representing angular velocity vector data, acceleration vector data and magnetic field vector data under a sensor coordinate system at the moment t,
represents the corrected angular velocity vector data,
and
representing reference vector data from a terrestrial coordinate system through a rotation matrix
Theoretical prediction of rotation to the sensor coordinate system, their correlation withSat、SmtPerforming cross multiplication to obtain control errors, respectively passing through PI controllers, wherein KpNumerically equal to the above mentioned trust parameter.Eg＝[0 0 1]TIs a gravity vector (reference vector) in the terrestrial coordinate system,Eb＝[0 bybz]Tthe magnetic field vector (reference vector) in the earth coordinate system is obtained by rotating the magnetometer measurement values into the earth coordinate system, then combining the x-axis component and the y-axis into the y-axis, and assuming that the x-axis is 0. q/| q | | denotes normalization to a quaternion.
The embodiment of the invention verifies the user posture measuring and calculating method, and the user posture measuring and calculating method rotates 100 circles on a desktop in situ, the time length is 10min, and the course angle only drifts 0.08 degrees; worn on the arm, jogged on a standard 400 m playground, and the drift speed of the course angle is only 0.09 degree/min. Both course angles have a small drift angle.
In summary, the embodiment of the present invention provides a user posture measuring and calculating method, which is based on angular velocity vector data acquired by a gyroscope after static drift vector data is eliminated, and performs auxiliary calibration on a magnetometer by using extended kalman filtering, so as to implement dynamic calibration of a magnetic field, have both speed and precision, and do not need to perform a complicated calibration operation. By setting the trust parameters, the acceleration vector data and the magnetic field vector data are screened in the nine-axis data fusion process, and errors caused by sudden change of the environment where the user is located are reduced. The method for calibrating the magnetometer based on the assistance of the gyroscope can be applied to more scenes such as VR equipment, wearable equipment, automobile navigation and the like, is more convenient and faster compared with other dynamic or static magnetic field calibration methods, and can break through some limitations in use.
The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are only exemplary embodiments of the present invention, and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (10)
1. A user posture measuring and calculating method is characterized by comprising the following steps:
acquiring acceleration vector data of a user, which is acquired by an accelerometer, and monitoring the variable quantity of a modulus value of the acceleration vector data in a preset time period;
judging whether the gyroscope is in a static or uniform state or not according to the variable quantity of the modulus, if so, collecting angular velocity vector data of the gyroscope at the current moment, and recording the angular velocity vector data as static drift vector data;
eliminating static drift vector data in angular velocity vector data of a user, which is acquired by a gyroscope;
performing auxiliary calibration on the magnetometer by adopting the angular velocity vector data after the static drift vector data is eliminated;
setting a confidence parameter for measuring the accuracy of the accelerometer and the magnetometer;
and according to the confidence parameters, fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer to obtain a quaternion corresponding to the user posture at the current moment.
2. The method of claim 1, wherein said assisted calibration of a magnetometer using angular velocity vector data after removal of static drift vector data comprises:
and based on the angular velocity vector data after the static drift vector data is eliminated, performing auxiliary calibration on the magnetometer by adopting an extended Kalman filtering algorithm.
3. The method of claim 1, wherein said fusing the acceleration vector data of the user, the angular velocity vector data of the user, and the magnetic field vector data measured by the magnetometer according to the confidence parameter comprises:
converting gravity vector data and magnetic field vector data under the terrestrial coordinate system into a sensor coordinate system to obtain reference acceleration vector data and reference magnetic field vector data;
performing cross multiplication on the reference acceleration vector data and the acceleration vector data of the user to obtain a first control error; cross-multiplying the reference magnetic field vector data by the magnetic field vector data measured by the magnetometer to obtain a second control error;
setting the size of the confidence parameter, and adjusting the weight of the first control error and the second control error;
and according to the weight and in combination with the quaternion corresponding to the user posture at the historical moment, fusing the acceleration vector data of the user, the angular velocity vector data of the user and the magnetic field vector data measured by the magnetometer to obtain the quaternion corresponding to the current user posture.
4. The method of claim 1, wherein the determining whether the gyroscope is in a static state or a constant speed state according to the variation of the modulus value comprises:
judging whether the variation is smaller than a preset threshold value, if so, counting once by using a static counter, and if not, resetting the counting times of the static counter and judging again;
and repeating the operation until the counting times of the static counter reach preset times, and judging that the gyroscope is in a static or uniform speed state at the moment.
5. The method of claim 1 or 2, wherein said assisted calibration of a magnetometer using angular velocity vector data after removal of static drift vector data further comprises:
and setting control parameters, wherein the control parameters are used for determining the weight occupied by the angular velocity vector data after the static drift vector data is eliminated for auxiliary calibration in the process of calibrating the magnetometer.
6. The method of claim 1, further comprising, prior to said fusing acceleration vector data of said user, angular velocity vector data of said user, and magnetic field vector data measured by said magnetometer:
and judging whether the magnetometer is calibrated or not, and fusing the magnetic field vector data measured by the calibrated magnetometer with the acceleration vector data of the user and the angular velocity vector data of the user.
7. The method of claim 6, wherein said determining whether the magnetometer is calibrated comprises:
acquiring a section of magnetic field data measured by the magnetometer, and calculating the root mean square error of other magnetic field data and the real value data by taking the data of the last point of the section of magnetic field data as real value data;
and judging whether the root mean square error is smaller than a preset threshold value, if so, finishing the calibration of the magnetometer.
8. A method according to claim 3, characterized by setting the trust parameter to a constant value when the user is stationary or in a uniform state;
gradually decreasing the value of the confidence parameter as the acceleration of the user gradually increases.
9. The method of claim 3, wherein the fusing the acceleration vector data of the user, the angular velocity vector data of the user, and the magnetic field vector data measured by the magnetometer in combination with the quaternion corresponding to the user posture at the historical time to obtain the quaternion corresponding to the current user posture comprises:
using a formula
Integrating after iterative operation to obtain a quaternion corresponding to the current user posture; wherein,
representing the user's pose of the iteration at the previous time corresponds to a quaternion,Sωtvector data representing the angular velocity of the user measured by the gyroscope at the current time,
and the differential of the quaternion corresponding to the user posture at the current moment is shown, and t represents the moment.
10. The method of claim 2, wherein complementary filtering algorithms are employed to fuse the user's acceleration vector data, the user's angular velocity vector data, and the magnetometer-measured magnetic field vector data;
the method further comprises the following steps: and initializing the extended Kalman filtering algorithm and the complementary filtering algorithm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911210177.3A CN110864684A (en) | 2019-11-29 | 2019-11-29 | User posture measuring and calculating method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911210177.3A CN110864684A (en) | 2019-11-29 | 2019-11-29 | User posture measuring and calculating method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110864684A true CN110864684A (en) | 2020-03-06 |
Family
ID=69658092
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911210177.3A Pending CN110864684A (en) | 2019-11-29 | 2019-11-29 | User posture measuring and calculating method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110864684A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111504254A (en) * | 2020-04-28 | 2020-08-07 | 深圳市云鼠科技开发有限公司 | Sweeper angle measuring and calculating method based on six-axis gyroscope |
| CN112304340A (en) * | 2020-12-24 | 2021-02-02 | 北京轻威科技有限责任公司 | Attitude calculation method and device based on nine-axis IMU and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140278183A1 (en) * | 2013-03-13 | 2014-09-18 | Invensense, Inc. | Heading confidence interval estimation |
| CN106403952A (en) * | 2016-08-29 | 2017-02-15 | 中国人民解放军火箭军工程大学 | Method for measuring combined attitudes of Satcom on the move with low cost |
| CN109883451A (en) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | A kind of adaptive gain complementary filter method and system for the estimation of pedestrian orientation |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
-
2019
- 2019-11-29 CN CN201911210177.3A patent/CN110864684A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140278183A1 (en) * | 2013-03-13 | 2014-09-18 | Invensense, Inc. | Heading confidence interval estimation |
| CN106403952A (en) * | 2016-08-29 | 2017-02-15 | 中国人民解放军火箭军工程大学 | Method for measuring combined attitudes of Satcom on the move with low cost |
| CN109883451A (en) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | A kind of adaptive gain complementary filter method and system for the estimation of pedestrian orientation |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
Non-Patent Citations (3)
| Title |
|---|
| KE HAN ET AL.: "Extended Kalman Filter-Based Gyroscope-Aided Magnetometer Calibration for Consumer Electronic Devices", 《IEEE SENSORS JOURNAL》 * |
| 范冰飞: "人体运动姿态传感器抗磁场和大加速度干扰的方法及其步态分析应用研究", 《中国博士学位论文全文数据库》 * |
| 陈亮等: "基于梯度下降法和互补滤波的航向姿态参考系统", 《电子设计工程》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111504254A (en) * | 2020-04-28 | 2020-08-07 | 深圳市云鼠科技开发有限公司 | Sweeper angle measuring and calculating method based on six-axis gyroscope |
| CN112304340A (en) * | 2020-12-24 | 2021-02-02 | 北京轻威科技有限责任公司 | Attitude calculation method and device based on nine-axis IMU and storage medium |
| CN112304340B (en) * | 2020-12-24 | 2021-04-06 | 北京轻威科技有限责任公司 | Attitude calculation method and device based on nine-axis IMU and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107560613B (en) | Robot indoor track tracking system and method based on nine-axis inertial sensor | |
| JP3947531B2 (en) | Acceleration error correction method and apparatus, and inertial navigation system using the same | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| JP6922641B2 (en) | Angular velocity derivation device and angular velocity derivation method | |
| CN108827299A (en) | A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter | |
| US10101174B2 (en) | Air data aided inertial measurement unit | |
| CN103791905A (en) | Attitude estimation method and apparatus | |
| Yousuf et al. | Sensor fusion of INS, odometer and GPS for robot localization | |
| Ariffin et al. | Low cost MEMS gyroscope and accelerometer implementation without Kalman Filter for angle estimation | |
| EP3388788B1 (en) | Inertially-aided air data computer altitude rate | |
| CN106813679B (en) | Method and device for estimating attitude of moving object | |
| CN110132271B (en) | Adaptive Kalman filtering attitude estimation algorithm | |
| EP3527948B1 (en) | Air data aided inertial measurement unit | |
| CN112304337B (en) | Motion angle estimation method and system based on gyroscope and accelerometer | |
| JP3726884B2 (en) | Attitude estimation apparatus and method using inertial measurement apparatus, and program | |
| TW201833557A (en) | Determination of a spatial orientation | |
| CN110864684A (en) | User posture measuring and calculating method | |
| CN108871319B (en) | Attitude calculation method based on earth gravity field and earth magnetic field sequential correction | |
| EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| CN109506674B (en) | Acceleration correction method and device | |
| CN112033405B (en) | Indoor environment magnetic anomaly real-time correction and navigation method and device | |
| CN110954103B (en) | Method and system for estimating dynamic attitude of vehicle body based on MEMS sensor | |
| CN114964214B (en) | Extended Kalman filtering attitude calculation method of attitude heading reference system | |
| CN116641697A (en) | Method and system for dynamically measuring attitude while drilling |
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 | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20200306 |