CN117889851A - Gesture determination method, device, equipment and storage medium - Google Patents
Gesture determination method, device, equipment and storage medium Download PDFInfo
- Publication number
- CN117889851A CN117889851A CN202410086353.1A CN202410086353A CN117889851A CN 117889851 A CN117889851 A CN 117889851A CN 202410086353 A CN202410086353 A CN 202410086353A CN 117889851 A CN117889851 A CN 117889851A
- Authority
- CN
- China
- Prior art keywords
- current moment
- current
- determining
- state
- measurement
- 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 60
- 238000005259 measurement Methods 0.000 claims abstract description 105
- 238000001914 filtration Methods 0.000 claims abstract description 86
- 230000007613 environmental effect Effects 0.000 claims abstract description 7
- 230000001133 acceleration Effects 0.000 claims description 39
- 238000004364 calculation method Methods 0.000 claims description 22
- 238000004590 computer program Methods 0.000 claims description 16
- 230000003068 static effect Effects 0.000 claims description 13
- 230000006870 function Effects 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 7
- 238000004891 communication Methods 0.000 description 8
- 238000012545 processing Methods 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- User Interface Of Digital Computer (AREA)
Abstract
The invention discloses a gesture determination method, a gesture determination device, gesture determination equipment and a storage medium. Acquiring specific force data output by an accelerometer at the current moment and the gyro angular rate at the current moment; based on the gyro angular rate at the current moment and the current environmental temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state comprises a posture quaternion estimated posture at the current moment; determining the current motion state of the target carrier based on the specific force data, and determining the measurement and the measurement variance of Kalman filtering corresponding to the current motion state; based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; the Kalman filtering state includes an attitude quaternion error; and calculating the gesture and the gesture quaternion error based on the gesture quaternion, and determining the gesture data of the target carrier at the current moment. The attitude estimation under various dynamic conditions is realized, and the attitude estimation precision is effectively improved.
Description
Technical Field
The present invention relates to the field of MEMS inertial devices, and in particular, to a method, apparatus, device, and storage medium for determining a pose.
Background
MEMS (Micro-Electro-Mechanical System, micro-electromechanical system) inertial devices have been widely used in the fields of unmanned aerial vehicles, unmanned ships, robots, etc., as an important branch of inertial navigation technology. The MEMS inertial device comprises a MEMS gyroscope for providing attitude information and a MEMS accelerometer for measuring acceleration forces of the carrier. In order to compensate for output noise of the MEMS gyroscopes, three MEMS gyroscopes and three MEMS accelerometers are typically combined together to form a six-axis sensor, thereby enabling attitude determination.
However, the existing attitude determination method cannot meet the requirements of attitude estimation under various dynamic conditions, and has the problem of low attitude estimation precision.
Disclosure of Invention
The invention provides a gesture determining method, a gesture determining device, gesture determining equipment and a storage medium, which are used for solving the problems that the existing gesture determining method cannot meet the requirements of gesture estimation under various dynamic conditions and the gesture estimation precision is low.
According to an aspect of the present invention, there is provided a gesture determining method including:
acquiring specific force data output by an accelerometer corresponding to a target carrier at the current moment and gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment;
Based on the gyro angular rate at the current moment and the current environmental temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises a posture quaternion calculation posture at the current moment and a gyro zero offset intermediate value at the current moment;
determining the current motion state of the target carrier based on the specific force data, and determining the measurement and the measurement variance of Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state;
based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; wherein the Kalman filtering state includes an attitude quaternion error;
and calculating the gesture and gesture quaternion errors based on the gesture quaternion at the current moment, and determining the gesture data of the target carrier at the current moment.
According to another aspect of the present invention, there is provided an attitude determination apparatus including:
the data acquisition module is used for acquiring specific force data output by the accelerometer corresponding to the target carrier at the current moment and the gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment;
The state determining module is used for updating the Kalman filtering time based on the gyro angular rate at the current moment and the current environment temperature value to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises a posture quaternion calculation posture at the current moment and a gyro zero bias intermediate value at the current moment;
the first determining module is used for determining the current motion state of the target carrier based on the specific force data and determining the measurement and the measurement variance of the Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state;
the measurement updating module is used for carrying out Kalman filtering measurement updating based on the measurement of Kalman filtering, the measurement variance and the prediction state of the current moment to obtain a Kalman filtering state; wherein the Kalman filtering state includes an attitude quaternion error;
the second determining module is used for calculating the gesture and the gesture quaternion error based on the gesture quaternion of the current moment and determining the gesture data of the current moment of the target carrier.
According to another aspect of the present invention, there is provided a posture determining device including:
At least one processor; and
a memory communicatively coupled to the at least one processor; wherein,
the memory stores a computer program executable by the at least one processor to enable the at least one processor to perform the pose determination method of any of the embodiments of the invention.
According to another aspect of the present invention, there is provided a computer readable storage medium storing computer instructions for causing a processor to perform the pose determination method according to any of the embodiments of the present invention.
According to the technical scheme provided by the embodiment of the invention, specific force data output by the accelerometer corresponding to the target carrier at the current moment and the gyro angular rate at the current moment are obtained; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment; based on the gyro angular rate at the current moment and the current environmental temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises a posture quaternion calculation posture at the current moment and a gyro zero offset intermediate value at the current moment; determining the current motion state of the target carrier based on the specific force data, and determining the measurement and the measurement variance of Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state; based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; wherein the Kalman filtering state includes an attitude quaternion error; and calculating the gesture and gesture quaternion errors based on the gesture quaternion at the current moment, and determining the gesture data of the target carrier at the current moment. According to the technical scheme, the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment, and further, the Kalman filtering time is updated according to the gyro angular rate and the current environment temperature value, so that the predicted state at the current moment can be obtained, and the predicted state at the current moment comprises the attitude quaternion estimated attitude at the current moment; determining a current motion state based on the obtained specific force data, wherein the current motion state can be a preset motion state or a non-preset motion state, further determining measurement and measurement variance of Kalman filtering corresponding to the current motion state, and then performing Kalman filtering measurement update based on the measurement and measurement variance of Kalman filtering to obtain a Kalman filtering state containing an attitude quaternion error; and calculating the gesture and gesture quaternion errors based on the gesture quaternion at the current moment, so that the gesture data of the target carrier at the current moment can be determined. The method solves the problems that the existing gesture determining method cannot meet the requirements of gesture estimation under various dynamic conditions and the gesture estimation precision is low, and has the advantages that the gesture estimation can be performed under various dynamic conditions and the gesture estimation precision is effectively improved.
It should be understood that the description in this section is not intended to identify key or critical features of the embodiments of the invention or to delineate the scope of the invention. Other features of the present invention will become apparent from the description that follows.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a gesture determination method according to a first embodiment of the present invention;
fig. 2 is a flowchart of a gesture determining method according to a second embodiment of the present invention;
fig. 3 is a schematic structural view of an attitude determination apparatus according to a third embodiment of the present invention;
fig. 4 is a schematic structural view of an attitude determination apparatus according to a fourth embodiment of the present invention.
Detailed Description
In order that those skilled in the art will better understand the present invention, a technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in which it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.
It should be noted that the terms "first," "second," and the like in the description and the claims of the present invention and the above figures are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented in sequences other than those illustrated or otherwise described herein. Furthermore, 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 elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
The "epoch" and "time" referred to in the embodiments of the present invention are the same meaning.
Example 1
Fig. 1 is a flowchart of a posture determining method provided in a first embodiment of the present invention, which is applicable to a case of determining a posture of a carrier by an accelerometer and a gyro angular rate, and the method may be performed by a posture determining apparatus, which may be implemented in the form of hardware and/or software, and the posture determining apparatus may be configured in a posture determining device, for example, the posture determining device may be an unmanned vehicle, an unmanned plane, an unmanned ship, or a robot, etc. As shown in fig. 1, the method includes:
S101, acquiring specific force data output by an accelerometer corresponding to a target carrier at the current moment and gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment.
In this embodiment, the target carrier may include an unmanned aerial vehicle, an unmanned ship, a robot, and the like. The accelerometer is used to measure acceleration of the carrier, e.g. a MEMS accelerometer. And the gyro angular rate at the current moment is determined according to the gyro angular rate at the last updating moment and the gyro zero offset at the last updating moment.
Specifically, an attitude estimation sensor is mounted on a target carrier, and the attitude estimation sensor is a high-performance three-dimensional motion attitude measurement system based on MEMS technology and comprises a gyroscope, an accelerometer and other devices. And acquiring specific force data output by a accelerometer in the attitude estimation sensor corresponding to the target carrier at the current moment, and simultaneously acquiring a gyro angular rate corresponding to the current moment, wherein the gyro angular rate is the gyro angular rate at the current moment obtained by calculating the gyro angular rate at the last update moment and the gyro zero offset at the last update moment.
S102, based on the gyro angular rate at the current moment and the current environment temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises an attitude quaternion calculation attitude at the current moment and a gyro zero offset intermediate value at the current moment.
Specifically, based on the gyro angular rate at the current time and the current environmental temperature value, the kalman filtering time is updated, the attitude quaternion calculation attitude at the current time and the gyro zero bias intermediate value at the current time can be determined, and the prediction state at the current time can be determined based on the attitude quaternion calculation attitude at the current time and the gyro zero bias intermediate value at the current time.
Optionally, based on the gyro angular rate at the current moment, performing kalman filtering time update to obtain the attitude quaternion estimated attitude at the current moment, including: determining a first product of a first preset coefficient, an updating period, a gyro angular rate at the current moment and a first attitude quaternion at the last updating moment; and determining the attitude quaternion calculation attitude at the current moment according to the sum of the first attitude quaternion and the first product at the last updating moment. The updating mode is as follows:
wherein,calculating the gesture for the gesture quaternion of the current moment, and q k-1 For the attitude data at the last update time, 0.5 is a first preset coefficient, Δt is the update period, ω x 、ω y 、ω z All are gyro angular rates corresponding to the current moment.
S103, determining the current motion state of the target carrier based on the specific force data, and determining the measurement and the measurement variance of Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state.
In this embodiment, the current motion state of the target carrier includes a preset motion state and a non-preset motion state, where the preset motion state includes a stationary state and a uniform linear motion state.
Specifically, considering different motion states of the target carrier, the corresponding measurement and measurement variance of the kalman filter are different, so that in order to perform gesture estimation under various dynamic conditions, the accuracy of gesture estimation is improved, and the motion state of the target carrier at the current moment needs to be determined. And judging the specific force data output by the accelerometer through the time sliding window, and determining the motion state of the current target carrier. The width of the time sliding window may be set as needed, for example, the time sliding window is set to 1s. The method of judging the motion state of the carrier is not limited in this embodiment. The current motion state may be a preset motion state, i.e. a static state or a uniform linear motion state, or may be a non-preset motion state, so as to calculate the measurement and measurement variance of the corresponding kalman filter according to different motion states.
S104, based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; wherein the Kalman filtering state includes a pose quaternion error.
Specifically, based on the measurement and measurement variance of the Kalman filter obtained by calculation and the prediction state of the current moment, the Kalman filter measurement is updated, and the updating process is as follows:
wherein x is Kalman filtering state, x= [ delta q, delta b g ] T Vq is the attitude quaternion error, vb g And correcting data for zero offset of the gyroscope.For predicting state +.>The posture quaternion calculation posture including the current time>And the gyro zero offset intermediate value at the current moment +.>K is gain, H is preset matrix, Z is measurement.
S105, calculating the gesture and the gesture quaternion error based on the gesture quaternion at the current moment, and determining the gesture data of the target carrier at the current moment.
In this embodiment, the pose data includes a pose quaternion that is used to describe the pose of the target carrier.
Specifically, based on the attitude quaternion calculation attitude and the attitude quaternion error at the current moment, the attitude data of the target carrier at the current moment can be determined to describe the attitude of the target carrier. In this embodiment, the specific determination method is not limited.
Optionally, determining the attitude data of the target carrier at the current moment based on the attitude quaternion calculation attitude and the attitude quaternion error at the current moment includes: based on the sum of the calculated gesture of the gesture quaternion at the current moment and the gesture quaternion error, determining the calculated gesture as gesture data of the target carrier at the current moment, wherein the calculation formula is as follows:
Wherein q k Namely, the attitude data of the target carrier at the current moment.
According to the technical scheme provided by the embodiment of the invention, specific force data output by the accelerometer corresponding to the target carrier at the current moment and the gyro angular rate at the current moment are obtained; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment; based on the gyro angular rate at the current moment and the current environmental temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises a posture quaternion calculation posture at the current moment and a gyro zero offset intermediate value at the current moment; determining the current motion state of the target carrier based on the specific force data, and determining the measurement and the measurement variance of Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state; based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; wherein the Kalman filtering state includes an attitude quaternion error; and calculating the gesture and gesture quaternion errors based on the gesture quaternion at the current moment, and determining the gesture data of the target carrier at the current moment. According to the technical scheme, the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment, and further, the Kalman filtering time is updated according to the gyro angular rate, so that the predicted state at the current moment can be obtained, and the predicted state at the current moment comprises the attitude quaternion estimated attitude at the current moment; determining a current motion state based on the obtained specific force data, wherein the current motion state can be a preset motion state or a non-preset motion state, further determining measurement and measurement variance of Kalman filtering corresponding to the current motion state, and then performing Kalman filtering measurement update based on the measurement and measurement variance of Kalman filtering to obtain a Kalman filtering state containing an attitude quaternion error; and calculating the gesture and gesture quaternion errors based on the gesture quaternion at the current moment, so that the gesture data of the target carrier at the current moment can be determined. The method solves the problems that the existing gesture determining method cannot meet the requirements of gesture estimation under various dynamic conditions and the gesture estimation precision is low, and has the advantages that the gesture estimation can be performed under various dynamic conditions and the gesture estimation precision is effectively improved.
Optionally, an initial roll angle and an initial pitch angle are calculated according to initial output data of the accelerometer, and an initial attitude quaternion is determined according to the initial roll angle and the initial pitch angle. The initial output data of the accelerometer is the output data of the accelerometer when the carrier is in a static state. The initial roll angle and initial pitch angle are calculated as follows:
wherein roll represents the initial roll angle, pitch represents the initial pitch angle, f x 、f y 、f z The accelerometer output values are front, right, and bottom, respectively.
Optionally, the initial gyroscope zero bias is determined according to initial output data of the gyroscope, wherein the initial output data of the gyroscope is output data of the gyroscope when the carrier is in a static state.
Optionally, in the process of performing kalman filter measurement update based on the measurement and measurement variance of the kalman filter, a covariance matrix of the pose and zero bias variance can be obtained, and the process is as follows:
(6)
wherein P is a covariance matrix of the gesture and zero-bias variance, which is used for calculating the next epoch,the variance covariance matrix is a prediction state, wherein the prediction state comprises a predicted gesture and zero offset, and I is an identity matrix.
Example two
Fig. 2 is a flowchart of a gesture determining method according to a second embodiment of the present invention, where the gesture determining method is optimized and expanded based on the foregoing embodiment. The embodiment describes in detail that according to different motion states of the target carrier, the measurement and the measurement variance of the corresponding kalman filter are calculated, as shown in fig. 2, the method includes:
S201, acquiring specific force data output by an accelerometer corresponding to a target carrier at the current moment and gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment.
S202, based on the gyro angular rate at the current moment and the current environment temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises an attitude quaternion estimated attitude at the current moment and a gyro zero offset intermediate value at the current moment.
S203, determining the current motion state of the target carrier based on the specific force data; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state; if the current motion state is the preset motion state, S204 is executed, and if the current motion state is the non-preset motion state, S205 is executed.
S204, according to the specific force data, measuring of the Kalman filter is determined, and measuring variance of the Kalman filter is determined based on the accuracy of the accelerometer.
In this embodiment, when the current motion state of the target carrier is determined to be the preset motion state, that is, the target carrier is in a static state or a uniform linear motion state, considering that the accelerometer carried by the target carrier is dominated by gravity and is not interfered by other factors at this time, the measurement of the kalman filter can be determined according to the specific force data output by the accelerometer, as shown in the following formula:
Z＝f (7)
Wherein Z is the measurement of Kalman filtering, and f is the specific force data output by the accelerometer.
And determining the measurement variance R of the Kalman filter according to the preset accelerometer precision.
S205, determining measurement of the Kalman filter according to the specific force data and the external acceleration at the current moment, and determining measurement variance of the Kalman filter according to the measurement, the prediction state and the variance covariance of the prediction state; the external acceleration at the current moment is determined by a preset acceleration model, and the variance covariance of the prediction state is determined based on the variance covariance of the last epoch.
In this embodiment, the external acceleration data is interference data generated by measuring the accelerometer in a non-translational motion state of the carrier. The preset acceleration model is a model established for calculating external acceleration data. The metrology data includes accelerometer measured data excluding external acceleration data. The prediction state is updated by the kalman filter time. The variance covariance of the prediction state is determined based on the variance covariance of the last epoch.
Specifically, the measurement of the kalman filter and the measurement variance of the kalman filter are determined as follows:
Z＝f-[a k 0 0] T (8)
wherein a is k Is the external acceleration. r is the innovation of the kalman filter, wherein the innovation represents the error between the best predicted value and the measured value of the measurement predicted from the existing information. The subscript k denotes the kth value.
S206, based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; wherein the Kalman filtering state includes a pose quaternion error.
S207, calculating the gesture and gesture quaternion errors based on the gesture quaternion at the current moment, and determining the gesture data of the target carrier at the current moment.
According to the technical scheme provided by the embodiment II of the invention, the gesture estimation is carried out aiming at different motion states of the target carrier, and the interference of external acceleration is effectively reduced when the target carrier is in a non-preset motion state, so that the accuracy of the gesture estimation is improved, and the adaptability of the method is further effectively improved.
In some embodiments, the preset acceleration model is built based on a first order low pass white noise process; the external acceleration at the current moment is determined by a preset acceleration model in the following way: determining a second product of a second preset coefficient and the acceleration of the last epoch, wherein the acceleration of the last epoch is determined according to the innovation in the Kalman filtering mode; and determining the external acceleration at the current moment according to the sum of the second product and Gaussian white noise. Through the technical scheme, the external acceleration can be effectively determined, and the risk of overfitting is effectively avoided.
Specifically, when the carrier is determined to be in a non-preset motion state, a preset acceleration is modeled as a first-order low-pass white noise process, and the specific implementation mode is as follows:
a k ＝γ ·a k-1 +w a,k (11)
wherein a is k Is the external acceleration data at the current moment. Gamma is a second preset coefficient which is determined according to the actual situation and which generally satisfies the condition 0<γ<1。a k-1 The acceleration for the last epoch can be determined from the innovation. w (w) a,k Is white gaussian noise.
In some embodiments, based on the current ambient temperature value, performing a kalman filter time update to obtain a gyro zero bias intermediate value at the current time, including: and determining a zero offset intermediate value of the gyroscope at the current moment based on the product of the unit column vector and a preset quadratic polynomial function, wherein the value of a variable in the quadratic polynomial function is an ambient temperature value, and coefficients in the quadratic polynomial function are determined according to the characteristics of the gyroscope. By the technical scheme, the gyroscope zero offset is modeled as a function of temperature, so that the temperature interference is effectively reduced, and the attitude estimation accuracy is maintained.
Specifically, considering the influence of the zero bias temperature of the gyroscope, modeling is a temperature-dependent quadratic polynomial, and a specific formula is as follows:
Wherein,is zero offset intermediate value of gyro at current moment, t is current ambient temperature value, coefficient alpha 1 、α 2 Determined by the gyroscope characteristics.
In some embodiments, the gesture determination method further comprises: judging whether the measurement variance of the Kalman filtering meets a preset condition or not; if the preset condition is met, taking the sum of the gyro zero offset intermediate value and gyro zero offset correction data at the current moment as the gyro zero offset of the next epoch; the Kalman filtering state comprises gyro zero offset correction data; and if the preset condition is not met, taking the gyro zero offset at the current moment as the gyro zero offset of the next epoch. By the technical scheme, a foundation is laid for obtaining the gyro angular rate of the next epoch with higher precision.
In this embodiment, the preset condition is set according to the actual situation. For example, the measurement variance of the kalman filter state may be smaller than βi as a preset condition, where β may be set according to the actual situation, for example, taking 0.3, I as a unit array. The Kalman filtering state comprises attitude quaternion errors and gyro zero offset correction data.
Specifically, whether the measurement variance of the kalman filter state is smaller than βi is determined, where β may be set according to the actual situation, for example, 0.3 is taken, if the measurement variance of the kalman filter state is smaller than βi, the gyro zero offset of the next epoch is calculated, and the calculation mode is as follows:
Wherein b g For compensating the actual output of the gyroscope.
And if the condition is not met, taking the gyro zero offset at the current moment as the gyro zero offset of the next epoch.
Optionally, the gyro angle rate of the next epoch is calculated by calculating the gyro zero offset of the next epoch, as follows:
wherein ω is the next epoch gyro angular rate,the gyro angular rate of this epoch is obtained.
Example III
Fig. 3 is a schematic structural diagram of an attitude determination apparatus according to a third embodiment of the present invention. As shown in fig. 3, the apparatus includes a data acquisition module 31, a state determination module 32, a first determination module 33, and a measurement update module 34 and a second determination module 35.
The data acquisition module 31 is configured to acquire specific force data output by an accelerometer corresponding to a target carrier at a current moment and a gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last update moment and the gyro zero offset at the last update moment; the state determining module 32 is configured to update a kalman filtering time based on a gyro angular rate at a current time and a current environmental temperature value, so as to obtain a predicted state at the current time, where the predicted state at the current time includes a posture quaternion estimated posture at the current time and a gyro zero bias intermediate value at the current time; a first determining module 33, configured to determine a current motion state of the target carrier based on the specific force data, and determine a measurement and a measurement variance of the kalman filter corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state; the measurement updating module 34 is configured to perform kalman filter measurement updating based on the measurement of the kalman filter, the measurement variance and the prediction state of the current moment, so as to obtain a kalman filter state; wherein the Kalman filtering state includes an attitude quaternion error; the second determining module 35 is configured to determine the pose data of the target carrier at the current moment based on the pose quaternion calculation pose and the pose quaternion error at the current moment.
The technical scheme provided by the third embodiment of the invention solves the problems that the existing gesture determining method cannot meet the requirements of gesture estimation under various dynamic conditions and the gesture estimation precision is low, and has the beneficial effects of being capable of carrying out gesture estimation under various dynamic conditions and effectively improving the gesture estimation precision.
Optionally, the first determining module 33 includes a state determining unit and a first determining unit;
wherein the state determining unit is used for determining the current motion state of the target carrier based on the specific force data; the first determining unit is used for determining measurement and measurement variance of Kalman filtering corresponding to the current motion state;
optionally, the first determining unit includes:
the first determining subunit is used for determining measurement of the Kalman filter according to the specific force data if the current motion state is a preset motion state, and determining measurement variance of the Kalman filter based on the precision of the accelerometer;
the second determining subunit is configured to determine measurement of the kalman filter according to the specific force data and the external acceleration at the current moment if the current motion state is a non-preset motion state, and determine measurement variance of the kalman filter according to the measurement, the prediction state and variance covariance of the prediction state; the external acceleration at the current moment is determined by a preset acceleration model, and the variance covariance of the prediction state is determined based on the variance covariance of the last epoch.
Optionally, the state determining module 32 includes a posture determining unit and an intermediate value determining unit.
The attitude determination unit is used for updating Kalman filtering time based on the gyro angular rate at the current moment to obtain an attitude quaternion calculation attitude at the current moment; and the intermediate value determining unit is used for updating the Kalman filtering time based on the current environment temperature value to obtain the zero offset intermediate value of the gyroscope at the current moment.
Optionally, the gesture determining unit includes:
the product determining subunit is used for determining a first product of a first preset coefficient, an updating period, a gyro angular rate at the current moment and a first posture quaternion at the last updating moment;
and the intermediate value determining subunit is used for determining the attitude quaternion calculation attitude at the current moment according to the sum of the first attitude quaternion and the first product at the last update moment.
Optionally, the preset acceleration model is built based on a first-order low-pass white noise process;
the external acceleration at the current moment is determined by a preset acceleration model in the following way:
determining a second product of a second preset coefficient and the acceleration of the last epoch, wherein the acceleration of the last epoch is determined according to the innovation in the Kalman filtering mode;
And determining the external acceleration at the current moment according to the sum of the second product and Gaussian white noise.
Optionally, the second determining module 35 is specifically configured to determine the pose data of the target carrier at the current moment based on the sum of the pose quaternion calculation pose and the pose quaternion error at the current moment.
Optionally, the intermediate value determining unit is specifically configured to determine a zero offset intermediate value of the gyroscope at the current moment based on a product of the unit column vector and a preset quadratic polynomial function, where a value of a variable in the quadratic polynomial function is an ambient temperature value, and a coefficient in the quadratic polynomial function is determined according to a characteristic of the gyroscope.
Optionally, the gesture determining apparatus further includes:
the condition judging module is used for judging whether the measurement variance of the Kalman filtering meets the preset condition;
the first zero offset determining module is used for taking the sum of the gyro zero offset intermediate value and the gyro zero offset correction data at the current moment as the gyro zero offset of the next epoch if the preset condition is met; the Kalman filtering state comprises gyro zero offset correction data;
and the second zero offset determining module is used for taking the gyro zero offset at the current moment as the gyro zero offset of the next epoch if the preset condition is not met.
The gesture determining device provided by the embodiment of the invention can execute the gesture determining method provided by any embodiment of the invention, and has the corresponding functional modules and beneficial effects of the executing method.
Example IV
Fig. 4 is a schematic structural view of an attitude determination apparatus according to a fourth embodiment of the present invention. The pose determination device may be an electronic device intended to represent various forms of digital computers, such as laptops, desktops, workstations, personal digital assistants, servers, blade servers, mainframes, and other suitable computers. Electronic equipment may also represent various forms of mobile devices, such as personal digital processing, cellular telephones, smartphones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions, are meant to be exemplary only, and are not meant to limit implementations of the inventions described and/or claimed herein.
As shown in fig. 4, the electronic device 10 includes at least one processor 11, and a memory, such as a Read Only Memory (ROM) 12, a Random Access Memory (RAM) 13, etc., communicatively connected to the at least one processor 11, in which the memory stores a computer program executable by the at least one processor, and the processor 11 may perform various appropriate actions and processes according to the computer program stored in the Read Only Memory (ROM) 12 or the computer program loaded from the storage unit 18 into the Random Access Memory (RAM) 13. In the RAM 13, various programs and data required for the operation of the electronic device 10 may also be stored. The processor 11, the ROM 12 and the RAM 13 are connected to each other via a bus 14. An input/output (I/O) interface 15 is also connected to bus 14.
Various components in the electronic device 10 are connected to the I/O interface 15, including: an input unit 16 such as a keyboard, a mouse, etc.; an output unit 17 such as various types of displays, speakers, and the like; a storage unit 18 such as a magnetic disk, an optical disk, or the like; and a communication unit 19 such as a network card, modem, wireless communication transceiver, etc. The communication unit 19 allows the electronic device 10 to exchange information/data with other devices via a computer network, such as the internet, and/or various telecommunication networks.
The processor 11 may be a variety of general and/or special purpose processing components having processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a Central Processing Unit (CPU), a Graphics Processing Unit (GPU), various specialized Artificial Intelligence (AI) computing chips, various processors running machine learning model algorithms, digital Signal Processors (DSPs), and any suitable processor, controller, microcontroller, etc. The processor 11 performs the respective methods and processes described above, such as the posture determining method.
In some embodiments, the pose determination method may be implemented as a computer program tangibly embodied on a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program may be loaded and/or installed onto the electronic device 10 via the ROM 12 and/or the communication unit 19. When a computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the gesture determination method described above may be performed. Alternatively, in other embodiments, the processor 11 may be configured to perform the pose determination method by any other suitable means (e.g., by means of firmware).
Various implementations of the systems and techniques described here above may be implemented in digital electronic circuitry, integrated circuit systems, field Programmable Gate Arrays (FPGAs), application Specific Integrated Circuits (ASICs), application Specific Standard Products (ASSPs), systems On Chip (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and/or combinations thereof. These various embodiments may include: implemented in one or more computer programs, the one or more computer programs may be executed and/or interpreted on a programmable system including at least one programmable processor, which may be a special purpose or general-purpose programmable processor, that may receive data and instructions from, and transmit data and instructions to, a storage system, at least one input device, and at least one output device.
A computer program for carrying out methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general purpose computer, special purpose computer, or other programmable data processing apparatus, such that the computer programs, when executed by the processor, cause the functions/acts specified in the flowchart and/or block diagram block or blocks to be implemented. The computer program may execute entirely on the machine, partly on the machine, as a stand-alone software package, partly on the machine and partly on a remote machine or entirely on the remote machine or server.
In the context of the present invention, a computer-readable storage medium may be a tangible medium that can contain, or store a computer program for use by or in connection with an instruction execution system, apparatus, or device. The computer readable storage medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. Alternatively, the computer readable storage medium may be a machine readable signal medium. More specific examples of a machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
To provide for interaction with a user, the systems and techniques described here can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to a user; and a keyboard and a pointing device (e.g., a mouse or a trackball) through which a user can provide input to the electronic device. Other kinds of devices may also be used to provide for interaction with a user; for example, feedback provided to the user may be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user may be received in any form, including acoustic input, speech input, or tactile input.
The systems and techniques described here can be implemented in a computing system that includes a background component (e.g., as a data server), or that includes a middleware component (e.g., an application server), or that includes a front-end component (e.g., a user computer having a graphical user interface or a web browser through which a user can interact with an implementation of the systems and techniques described here), or any combination of such background, middleware, or front-end components. The components of the system can be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: local Area Networks (LANs), wide Area Networks (WANs), blockchain networks, and the internet.
The computing system may include clients and servers. The client and server are typically remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a client-server relationship to each other. The server can be a cloud server, also called a cloud computing server or a cloud host, and is a host product in a cloud computing service system, so that the defects of high management difficulty and weak service expansibility in the traditional physical hosts and VPS service are overcome.
It should be appreciated that various forms of the flows shown above may be used to reorder, add, or delete steps. For example, the steps described in the present invention may be performed in parallel, sequentially, or in a different order, so long as the desired results of the technical solution of the present invention are achieved, and the present invention is not limited herein.
The above embodiments do not limit the scope of the present invention. It will be apparent to those skilled in the art that various modifications, combinations, sub-combinations and alternatives are possible, depending on design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of the present invention.
Claims (10)
1. A posture determining method, characterized by comprising:
acquiring specific force data output by an accelerometer corresponding to a target carrier at the current moment and gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last updating moment and the gyro zero offset at the last updating moment;
based on the gyro angular rate at the current moment and the current environmental temperature value, carrying out Kalman filtering time update to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises an attitude quaternion calculation attitude at the current moment and a gyro zero offset intermediate value at the current moment;
Determining the current motion state of the target carrier based on the specific force data, and determining the measurement and measurement variance of Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state;
based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment, carrying out Kalman filtering measurement update to obtain a Kalman filtering state; wherein the Kalman filtering state includes an attitude quaternion error;
and calculating the gesture based on the gesture quaternion of the current moment and the gesture quaternion error, and determining the gesture data of the target carrier at the current moment.
2. The method of claim 1, wherein said determining the measurement and measurement variance of the kalman filter corresponding to the current motion state comprises:
if the current motion state is the preset motion state, determining measurement of the Kalman filtering according to the specific force data, and determining measurement variance of the Kalman filtering based on the precision of an accelerometer;
If the current motion state is the non-preset motion state, determining measurement of the Kalman filtering according to the specific force data and external acceleration at the current moment, and determining measurement variance of the Kalman filtering according to the measurement, the prediction state and variance covariance of the prediction state; the external acceleration at the current moment is determined by a preset acceleration model, and the variance covariance of the prediction state is determined based on the variance covariance of the last epoch.
3. The method of claim 1, wherein the performing a kalman filter time update based on the gyro angular rate at the current time to obtain a posture quaternion estimated posture at the current time comprises:
determining a first preset coefficient, an updating period, a gyro angular rate at the current moment and a first product of a first attitude quaternion at the last updating moment;
and determining the attitude quaternion calculation attitude at the current moment according to the sum of the first attitude quaternion at the last update moment and the first product.
4. The method according to claim 2, wherein the preset acceleration model is built based on a first order low pass white noise process;
The external acceleration at the current moment is determined by a preset acceleration model in the following way:
determining a second product of a second preset coefficient and the acceleration of a previous epoch, wherein the acceleration of the previous epoch is determined according to the innovation in the Kalman filtering mode;
and determining the external acceleration at the current moment according to the sum of the second product and Gaussian white noise.
5. The method of claim 1, wherein the determining the pose data for the current time of the target carrier based on the pose quaternion derived pose and the pose quaternion error for the current time comprises:
and determining the attitude data of the target carrier at the current moment based on the sum of the attitude quaternion calculation attitude at the current moment and the attitude quaternion error.
6. The method of claim 1, wherein the performing a kalman filter time update based on the current ambient temperature value to obtain a gyro zero offset intermediate value at the current time comprises:
and determining a gyro zero offset intermediate value at the current moment based on the product of the unit column vector and a preset quadratic polynomial function, wherein the value of a variable in the quadratic polynomial function is the ambient temperature value, and the coefficient in the quadratic polynomial function is determined according to the characteristics of the gyroscope.
7. The method as recited in claim 6, further comprising:
judging whether the measurement variance of the Kalman filtering meets a preset condition or not;
if the preset condition is met, taking the sum of the gyro zero offset intermediate value and the gyro zero offset correction data at the current moment as the gyro zero offset of the next epoch; wherein the Kalman filtering state comprises the gyro zero offset correction data;
and if the preset condition is not met, taking the gyro zero offset at the current moment as the gyro zero offset of the next epoch.
8. An attitude determination apparatus, characterized by comprising:
the data acquisition module is used for acquiring specific force data output by the accelerometer corresponding to the target carrier at the current moment and the gyro angular rate at the current moment; the gyro angular rate at the current moment is determined according to the gyro angular rate at the last updating moment and the gyro zero offset at the last updating moment;
the state determining module is used for updating the Kalman filtering time based on the gyro angular rate at the current moment and the current environment temperature value to obtain a predicted state at the current moment, wherein the predicted state at the current moment comprises an attitude quaternion estimated attitude at the current moment and a gyro zero offset intermediate value at the current moment;
The first determining module is used for determining the current motion state of the target carrier based on the specific force data and determining the measurement and the measurement variance of the Kalman filtering corresponding to the current motion state; the current motion state comprises a preset motion state and a non-preset motion state, and the preset motion state comprises a static state and a uniform linear motion state;
the measurement updating module is used for carrying out Kalman filtering measurement updating based on the measurement of the Kalman filtering, the measurement variance and the prediction state of the current moment to obtain a Kalman filtering state; wherein the Kalman filtering state includes an attitude quaternion error;
and the second determining module is used for determining the attitude data of the current moment of the target carrier based on the attitude quaternion calculation attitude of the current moment and the attitude quaternion error.
9. A posture determining device, characterized in that the posture determining device comprises:
at least one processor; and
a memory communicatively coupled to the at least one processor; wherein,
the memory stores a computer program executable by the at least one processor to enable the at least one processor to perform the pose determination method according to any of claims 1-7.
10. A computer readable storage medium storing computer instructions for causing a processor to perform the pose determination method according to any of claims 1-7 when executed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410086353.1A CN117889851A (en) | 2024-01-22 | 2024-01-22 | Gesture determination method, device, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410086353.1A CN117889851A (en) | 2024-01-22 | 2024-01-22 | Gesture determination method, device, equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117889851A true CN117889851A (en) | 2024-04-16 |
Family
ID=90644376
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410086353.1A Pending CN117889851A (en) | 2024-01-22 | 2024-01-22 | Gesture determination method, device, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117889851A (en) |
-
2024
- 2024-01-22 CN CN202410086353.1A patent/CN117889851A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109163721B (en) | Attitude measurement method and terminal equipment | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN114018274B (en) | Vehicle positioning method and device and electronic equipment | |
| CN116067370B (en) | IMU gesture resolving method, IMU gesture resolving equipment and storage medium | |
| CN112683269B (en) | MARG attitude calculation method with motion acceleration compensation | |
| CN114179825B (en) | Method for obtaining confidence of measurement value through multi-sensor fusion and automatic driving vehicle | |
| CN108827301A (en) | A kind of improvement error quaternion Kalman filtering robot pose calculation method | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN111076722A (en) | Attitude estimation method and device based on self-adaptive quaternion | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| CN110487269A (en) | GPS/INS Combinated navigation method, device, storage medium and electronic equipment | |
| CN113218389A (en) | Vehicle positioning method, device, storage medium and computer program product | |
| CN110160530B (en) | Spacecraft attitude filtering method based on quaternion | |
| CN114964214B (en) | Extended Kalman filtering attitude calculation method of attitude heading reference system | |
| CN115727871A (en) | Track quality detection method and device, electronic equipment and storage medium | |
| CN117889851A (en) | Gesture determination method, device, equipment and storage medium | |
| CN114001730B (en) | Fusion positioning method, fusion positioning device, computer equipment and storage medium | |
| CN113804194B (en) | Positioning method, device and equipment of driving equipment and storage medium | |
| CN116448105B (en) | Pose updating method and device, electronic equipment and storage medium | |
| CN115164888B (en) | Error correction method and device, electronic equipment and storage medium | |
| CN116380056B (en) | Inertial positioning method, inertial positioning device, electronic equipment and storage medium | |
| CN118518101B (en) | Error correction method, system, equipment and product of inertial navigation system | |
| CN117806469A (en) | Automatic positioning method, device, equipment and medium | |
| CN117058430B (en) | Method, apparatus, electronic device and storage medium for field of view matching |
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 |