CN112304337A - Motion angle estimation method and system based on gyroscope and accelerometer - Google Patents
Motion angle estimation method and system based on gyroscope and accelerometer Download PDFInfo
- Publication number
- CN112304337A CN112304337A CN202011141659.0A CN202011141659A CN112304337A CN 112304337 A CN112304337 A CN 112304337A CN 202011141659 A CN202011141659 A CN 202011141659A CN 112304337 A CN112304337 A CN 112304337A
- Authority
- CN
- China
- Prior art keywords
- gyroscope
- acceleration
- iir
- accelerometer
- bias
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- 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)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Gyroscopes (AREA)
Abstract
A method, system, and computer readable medium for motion angle estimation based on a gyroscope and an accelerometer are provided. The method comprises the following steps: acquiring the acceleration of an accelerometer at the current moment, a plurality of groups of angular velocity mean values of a gyroscope, a preset gyroscope correction coefficient and a preset accelerometer correction coefficient; acquiring a gyroscope deviation and an accelerometer deviation; calibrating the acceleration at the current moment according to the accelerometer deviation; performing IIR filtering on the gyroscope deviation by using a preset gyroscope correction coefficient and a plurality of groups of angular speed average values; acquiring the acceleration of the last moment after filtering; performing IIR filtering on the calibrated acceleration at the current moment by using a preset accelerometer correction coefficient and the filtered acceleration at the previous moment; and estimating a final motion angle according to the filtered gyroscope deviation and the filtered acceleration at the current moment. According to the method, the deviation and the acceleration of the gyroscope are corrected through the improved IIR filter, and the accurate estimation of the motion angle can be realized.
Description
Technical Field
The present application relates generally to the field of data processing, and more particularly, to a method, system and computer readable medium for motion angle estimation based on a gyroscope and an accelerometer.
Background
Along with popularization and continuous improvement of intelligent degree of smart phones and smart homes, people can purchase and select smart homes suitable for smart phones, sweeping robots and the like within an economic bearing range. Taking the sweeping robot as an example, in the working process of the sweeping robot, the positioning mode is very important, and whether the sweeping robot can sweep the ground completely is determined. The sweeping robot positioning and the construction of the environment map are core parts of path planning, and the estimation of the motion angle of the sweeping robot is the key of the positioning.
The motion angle estimation method of the sweeping robot can be divided into three categories: a photoelectric encoder based method, an electronic compass based method, a gyroscope based method. The method based on the photoelectric encoder is an angle detection device which converts an angle quantity input to a shaft into a corresponding electric pulse or a digital quantity by using a photoelectric conversion principle, but the method needs to rely on a mechanical device for conversion, is difficult to overcome slip, and can be inaccurate in angle measurement caused by uncertain factors such as wheel slip. The method based on the electronic compass detects the deflection angle between the electronic compass and the direction of the geomagnetic field by using the geomagnetic field, so that the method is easily influenced by electromagnetic interference. The gyroscope-based method is used for obtaining the motion angle of the sweeping robot by measuring the angular velocity of the sweeping robot, and can solve the problems of the former two methods. And the gyroscope sensor has the characteristics of low manufacturing cost, no influence of environmental factors, good real-time performance and the like, and is widely applied.
However, in practical situations, most of the cheap gyroscope sensors have large deviations, so that the finally estimated angle value has large errors from the actual value. In order to solve the error problem, a user needs to calibrate the gyroscope before using the sweeping robot or when the sweeping robot is started, so that the use experience of the user is greatly reduced. After the robot is used for a long time, the gyroscope still generates accumulated deviation, and the problem that the motion angle estimation result is not ideal and the like is still caused to the sweeping robot. Most of the existing motion attitude estimation algorithms adopt algorithms based on Kalman filtering or complementary filtering, but the Kalman filtering has large calculation amount and high requirement on hardware, and the realization is relatively complex.
In general, errors of the gyroscope and the accelerometer are classified into a systematic error and a random error. Systematic errors can usually be eliminated by calibration of the sensor; the random error (also called zero offset) of the gyroscope usually drifts randomly within a certain range, so that the zero offset of the gyroscope needs to be corrected. The accelerometer is sensitive, but its jitter and noise can produce accumulated errors, so that the calibrated accelerometer data needs to be corrected.
Therefore, a motion angle estimation method based on a gyroscope and an accelerometer needs to be able to better correct the gyroscope bias and the accelerometer data to achieve more accurate motion angle estimation.
Disclosure of Invention
The technical problem to be solved by the application is to provide a motion angle estimation method, system and computer readable medium based on a gyroscope and an accelerometer, which can better correct gyroscope deviation and accelerometer data and realize more accurate motion angle estimation.
In order to solve the above technical problem, the present application provides a motion angle estimation method based on a gyroscope and an accelerometer, including: acquiring the acceleration of an accelerometer at the current moment, a plurality of groups of angular velocity mean values of a gyroscope, a preset gyroscope correction coefficient and a preset accelerometer correction coefficient; acquiring a gyroscope bias and an accelerometer bias while the gyroscope and the accelerometer remain horizontally disposed and stationary; calibrating the acceleration at the current moment according to the accelerometer deviation, and obtaining the calibrated acceleration at the current moment; performing IIR filtering on the gyroscope deviation by using the preset gyroscope correction coefficient and the multiple groups of angular speed mean values, and obtaining filtered gyroscope deviation; acquiring the filtered acceleration at the previous moment, and if the filtered acceleration at the previous moment does not exist, taking the calibrated acceleration at the current moment as the filtered acceleration at the previous moment; performing IIR filtering on the calibrated acceleration at the current moment by using the preset accelerometer correction coefficient and the filtered acceleration at the previous moment, and obtaining the filtered acceleration at the current moment; and estimating a final motion angle according to the filtered gyroscope deviation and the filtered acceleration at the current moment.
In an embodiment of the application, the IIR filtering the gyroscope bias using the preset gyroscope correction coefficient and the multiple groups of angular velocity averages, and obtaining the filtered gyroscope bias includes:
IIR_gx_bias’＝α*gx_mean+(1-α)*IIR_gx_bias
IIR_gy_bias’＝α*gy_mean+(1-α)*IIR_gy_bias
IIR_gz_bias’＝α*gz_mean+(1-α)*IIR_gz_bias
wherein gx _ mean, gy _ mean, and gz _ mean are the multiple groups of angular velocity mean values of the gyroscope on the x-axis, the y-axis, and the z-axis, respectively, α is the preset gyroscope correction coefficient, IIR _ gx _ bias, IIR _ gy _ bias, and IIR _ gz _ bias are the gyroscope deviations on the x-axis, the y-axis, and the z-axis, and IIR _ gx _ bias ', IIR _ gy _ bias ', and IIR _ gz _ bias ' are the filtered gyroscope deviations on the x-axis, the y-axis, and the z-axis, respectively.
In an embodiment of the application, the performing, by using the preset accelerometer correction coefficient and the filtered acceleration at the previous time, an IIR filtering on the calibrated acceleration at the current time, and obtaining the filtered acceleration at the current time includes:
IIR_ax’＝(1-β)*cor_ax+β*IIR_ax
IIR_ay’＝(1-β)*cor_ay+β*IIR_ay
IIR_az’＝(1-β)*cor_az+β*IIR_az
wherein cor _ ax, cor _ ay, and cor _ az are the accelerations at the calibrated current time on the x-axis, the y-axis, and the z-axis, respectively, β is the preset accelerometer correction coefficient, IIR _ ax, IIR _ ay, and IIR _ az are the accelerations at the filtered previous time on the x-axis, the y-axis, and the z-axis, and IIR _ ax ', IIR _ ay ', and IIR _ az ' are the accelerations at the filtered current time on the x-axis, the y-axis, and the z-axis, respectively.
In an embodiment of the application, the estimating a final motion angle according to the filtered gyroscope bias and the filtered acceleration at the current time is performed by using any one of the following methods: quaternion, matrix rotation, and euler rotation.
In an embodiment of the application, a value range of the preset gyroscope correction coefficient is 0.08-0.12.
In an embodiment of the present application, a value range of the predetermined accelerometer correction coefficient is 0.44 to 0.66.
In an embodiment of the application, the calibrating the acceleration at the current time according to the accelerometer bias, and obtaining the calibrated acceleration at the current time includes:
cor_ax＝raw_ax*g*R-bias_ax
cor_ay＝raw_ay*g*R-bias_ay
cor_az＝raw_az*g*R-bias_az
wherein cor _ ax, cor _ ay, and cor _ az are the accelerations at the calibrated current time on the x-axis, the y-axis, and the z-axis, raw _ ax, raw _ ay, and raw _ az are the accelerations at the current time on the x-axis, the y-axis, and the z-axis, g is a gravitational acceleration constant, R is a preset unit conversion factor, and bias _ ax, bias _ ay, and bias _ az are the accelerometer deviations on the x-axis, the y-axis, and the z-axis, respectively.
In order to solve the above technical problem, the present application further provides a motion angle estimation device based on a gyroscope and an accelerometer, including: the first acquisition module is used for acquiring the acceleration of the accelerometer at the current moment, the average values of multiple groups of angular velocities of the gyroscope, a preset gyroscope correction coefficient and a preset accelerometer correction coefficient; a second acquisition module for acquiring a gyroscope bias and an accelerometer bias while the gyroscope and the accelerometer remain horizontally disposed and stationary; the acceleration calibration module is used for calibrating the acceleration at the current moment according to the accelerometer deviation and obtaining the calibrated acceleration at the current moment; the deviation filtering module is used for carrying out IIR filtering on the gyroscope deviation by using the preset gyroscope correction coefficient and the multiple groups of angular velocity mean values and obtaining the filtered gyroscope deviation; a third obtaining module, configured to obtain a filtered acceleration at a previous time, and if the filtered acceleration at the previous time does not exist, take the calibrated acceleration at the current time as the filtered acceleration at the previous time; the acceleration filtering module is used for carrying out IIR filtering on the calibrated acceleration at the current moment by using the preset accelerometer correction coefficient and the filtered acceleration at the previous moment, and obtaining the filtered acceleration at the current moment; and the estimation module is used for estimating a final motion angle according to the filtered gyroscope deviation and the filtered acceleration at the current moment.
In order to solve the above technical problem, the present application further provides a motion angle estimation system based on a gyroscope and an accelerometer, including: a memory for storing instructions executable by the processor; and a processor for executing the instructions to implement the method as described above.
To solve the above technical problem, the present application also provides a computer readable medium storing computer program code, which when executed by a processor implements the method as described above.
Compared with the prior art, the motion angle estimation method, the motion angle estimation system and the computer readable medium based on the gyroscope and the accelerometer correct the deviation and the acceleration of the gyroscope through the improved IIR filter, can better filter accelerometer data and gyroscope data, and realize accurate estimation of the motion angle with lower calculation amount.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the application and together with the description serve to explain the principle of the application. In the drawings:
fig. 1 is a schematic flow chart of a motion angle estimation method based on a gyroscope and an accelerometer according to an embodiment of the application.
Fig. 2 is a block diagram of a motion angle estimation apparatus based on a gyroscope and an accelerometer according to an embodiment of the present application.
Fig. 3 is a system block diagram illustrating a gyroscope and accelerometer based motion angle estimation system according to an embodiment of the present application.
Detailed Description
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings used in the description of the embodiments will be briefly introduced below. It is obvious that the drawings in the following description are only examples or embodiments of the application, from which the application can also be applied to other similar scenarios without inventive effort for a person skilled in the art. Unless otherwise apparent from the context, or otherwise indicated, like reference numbers in the figures refer to the same structure or operation.
As used in this application and the appended claims, the terms "a," "an," "the," and/or "the" are not intended to be inclusive in the singular, but rather are intended to be inclusive in the plural unless the context clearly dictates otherwise. In general, the terms "comprises" and "comprising" merely indicate that steps and elements are included which are explicitly identified, that the steps and elements do not form an exclusive list, and that a method or apparatus may include other steps or elements.
The relative arrangement of the components and steps, the numerical expressions, and numerical values set forth in these embodiments do not limit the scope of the present application unless specifically stated otherwise. Meanwhile, it should be understood that the sizes of the respective portions shown in the drawings are not drawn in an actual proportional relationship for the convenience of description. Techniques, methods, and apparatus known to those of ordinary skill in the relevant art may not be discussed in detail but are intended to be part of the specification where appropriate. In all examples shown and discussed herein, any particular value should be construed as merely illustrative, and not limiting. Thus, other examples of the exemplary embodiments may have different values. It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, further discussion thereof is not required in subsequent figures.
Flow charts are used herein to illustrate operations performed by systems according to embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed in the exact order in which they are performed. Rather, various steps may be processed in reverse order or simultaneously. Meanwhile, other operations are added to or removed from these processes.
The application provides a motion angle estimation method based on a gyroscope and an accelerometer, wherein the gyroscope and the accelerometer are used for estimating the motion angle of the same device. Fig. 1 is a schematic flow chart of a motion angle estimation method based on a gyroscope and an accelerometer according to an embodiment of the application. As shown in fig. 1, the motion angle estimation method based on a gyroscope and an accelerometer of the present embodiment includes the following steps:
102, acquiring a gyroscope deviation and an accelerometer deviation under the condition that the gyroscope and the accelerometer are kept horizontally placed and static;
104, performing IIR filtering on the gyroscope deviation by using a preset gyroscope correction coefficient and a plurality of groups of angular speed average values, and obtaining the filtered gyroscope deviation;
and step 107, estimating a final motion angle according to the filtered gyroscope deviation and the filtered acceleration at the current moment.
In step 101, a motion angle estimation system based on a gyroscope and an accelerometer acquires an acceleration of the accelerometer at a current time, a plurality of groups of angular velocity mean values of the gyroscope, a preset gyroscope correction coefficient, and a preset accelerometer correction coefficient. The gyroscope used in this embodiment is a three-axis gyroscope, and the measured data is three-axis data, that is, the angular velocity measured by the gyroscope is three-axis angular velocity. The accelerometer used in this embodiment is a triaxial accelerometer, and the measured data is triaxial data, that is, the acceleration measured by the accelerometer is triaxial acceleration.
The number of groups of the multiple groups of angular velocity mean values of the gyroscope can be determined by the setting of the gyroscope itself, and the application does not limit the number. The multiple groups of angular velocity mean values can be directly provided by the gyroscope, and the system directly acquires the multiple groups of angular velocity mean values from the gyroscope without calculation. In one example, the sets of angular rate averages for the gyroscope may be stored in a First-in-First-out (FIFO) memory of the gyroscope from which the system retrieves the sets of angular rate averages.
The preset gyroscope correction coefficient and the preset accelerometer correction coefficient can be set by a user according to an application scene and actual experience. In an embodiment of the present application, a value range of the preset gyroscope correction coefficient may be 0.08-0.12. Preferably, the preset gyro correction coefficient may be 0.1. In an embodiment of the present application, the value of the predetermined accelerometer correction coefficient may range from 0.44 to 0.66. Preferably, the preset accelerometer correction factor may be 0.55.
In step 102, the system acquires the gyroscope bias and the accelerometer bias while the gyroscope and accelerometer remain horizontally disposed and stationary. The gyroscope bias and the accelerometer bias in this embodiment refer to the amount of offset of the respective axes of the gyroscope and the accelerometer with the gyroscope and the accelerometer kept horizontally and stationary, wherein the gyroscope bias is also called gyroscope random error or zero bias.
In step 103, the system calibrates the acceleration at the current time according to the accelerometer bias, and obtains the calibrated acceleration at the current time. In an embodiment of the present application, the system may perform step 103 by:
cor_ax＝raw_ax*g*R-bias_ax
cor_ay＝raw_ay*g*R-bias_ay
cor_az＝raw_az*g*R-bias_az
where cor _ ax, cor _ ay, cor _ az are the calibrated accelerations at the current time on the x, y, and z axes, respectively. raw _ ax, raw _ ay, and raw _ az are accelerations at the current time on the x-axis, y-axis, and z-axis, respectively. g is a gravitational acceleration constant, and the value of g can be 9.80665. And R is a preset unit conversion factor and is used for converting different metering units, and a user can set the conversion factor according to actual needs. In one example, the preset unit conversion factor R may be set to 0.001. bias _ ax, bias _ ay, bias _ az are accelerometer biases on the x-axis, y-axis, and z-axis, respectively.
In step 104, the system uses the preset gyroscope correction coefficients and the multiple groups of angular velocity average values to perform IIR filtering on the gyroscope bias, and obtains the filtered gyroscope bias. In an embodiment of the present application, the IIR filtering performed by the system is first-order IIR filtering, and step 104 may be performed in the following manner:
IIR_gx_bias’＝α*gx_mean+(1-α)*IIR_gx_bias
IIR_gy_bias’＝α*gy_mean+(1-α)*IIR_gy_bias
IIR_gz_bias’＝α*gz_mean+(1-α)*IIR_gz_bias
wherein gx _ mean, gy _ mean and gz _ mean are the average values of multiple groups of angular velocities of the gyroscope on the x axis, the y axis and the z axis respectively; alpha is a preset gyroscope correction coefficient, the larger the alpha value is, the larger the proportion of the multiple groups of angular speed mean values of the gyroscope is, and the smaller the deviation ratio is; IIR _ gx _ bias, IIR _ gy _ bias and IIR _ gz _ bias are respectively gyroscope deviations on an x axis, a y axis and a z axis; IIR _ gx _ bias ', IIR _ gy _ bias ', and IIR _ gz _ bias ' are filtered gyroscope biases on the x-axis, y-axis, and z-axis, respectively.
In step 105, the system obtains the filtered acceleration at the previous time, and if the filtered acceleration at the previous time does not exist, that is, the current time is the initial time, the system performs the calculation of the subsequent step using the calibrated acceleration at the current time as the filtered acceleration at the previous time.
In step 106, the system performs IIR filtering on the calibrated acceleration at the current time by using a preset accelerometer correction coefficient and the filtered acceleration at the previous time, and obtains the filtered acceleration at the current time. In an embodiment of the present application, the IIR filtering performed by the system is first-order IIR filtering, and step 106 may be performed in the following manner:
IIR_ax’＝(1-β)*cor_ax+β*IIR_ax
IIR_ay’＝(1-β)*cor_ay+β*IIR_ay
IIR_az’＝(1-β)*cor_az+β*IIR_az
wherein cor _ ax, cor _ ay, cor _ az are the calibrated accelerations at the current time on the x-axis, the y-axis and the z-axis, respectively; beta is a preset accelerometer correction coefficient, the smaller the beta value is, the larger the proportion of the calibrated current acceleration is, and the smaller the proportion of the filtered acceleration at the previous moment is; IIR _ ax, IIR _ ay, and IIR _ az are filtered accelerations at previous time instants on the x-axis, the y-axis, and the z-axis, respectively, and IIR _ ax ', IIR _ ay ', and IIR _ az ' are filtered accelerations at current time instants on the x-axis, the y-axis, and the z-axis, respectively.
In step 107, the system estimates the final motion angle from the filtered gyroscope bias and the filtered acceleration at the current time. In an embodiment of the present application, the system may perform step 107 by any one of the following methods: quaternion, matrix rotation, and euler rotation. Preferably, the system uses quaternion to estimate the final motion angle from the filtered gyroscope bias and the filtered acceleration at the current time. In this embodiment, the step 107 is described in detail by taking a quaternion method as an example, and includes the following steps 107a to 107 c:
step 107a, the system expresses the filtered gyroscope bias by a quaternion, and the formula is as follows:
wherein the initial value of the quaternion is quat _ in0＝1.0，quat_in1＝0.0，quat_in2＝0.0，quat_in3＝0.0；gx_mean、gy_mean and gz _ mean are the average values of the angular velocities of the gyroscopes on the x-axis, the y-axis and the z-axis, respectively.
The system then updates the quaternion through a first order Longgustata differential protocol as follows:
quat_update_x＝Runge_Kutta_1st(gx_mean,deltaT)
quat_update_y＝Runge_Kutta_1st(gy_mean,deltaT)
quat_update_z＝Runge_Kutta_1st(gz_mean,deltaT)
where deltaT is the sampling period of the gyroscope.
And step 107b, the system corrects the gyroscope according to the filtered acceleration at the current moment. Due to null shift of the gyroscope, the measured gyroscope data is not prepared, the accelerometer has good static performance and no serious drift of the gyroscope, and can be used for correcting the attitude of the gyroscope, and the data of the accelerometer and the gyroscope are subjected to vector product to obtain an error, so that the gyroscope is corrected, and the formula is as follows:
wherein a _ quat0、a_quat1、a_quat2、a_quat3For the modified quaternion data, g _ quat, for the accelerometer obtained in step 1060、g_quat1、g_quat2、g_quat3G _ quat as the pre-correction gyroscope quaternion data0'、g_quat1'、g_quat2'、g_quat3'is the modified gyroscope's quaternion data.
Step 107c, the system estimates the final motion angle according to a quaternion method. The quaternion loses its normalization characteristic due to calculation errors, etc., so that the modulus of the quaternion is no longer equal to 1. In addition, the normalized quaternion is not stretched in the rotation space, and the rotation matrix satisfies orthogonality only by the rotation angle. Therefore, before calculating the motion angle, the quaternion needs to be normalized, and the normalization formula is as follows:
where | quat | ═ sqrt ((gyro _ quat)0')2+(gyro_quat1')2+(gyro_quat2')2+(gyro_quat3')2) Is the modulus of the quaternion;
normalized gyroscope quaternion data.
Then the system obtains the motion angle of the equipment according to the corresponding relation between the direction cosine matrix of the quaternion and the Euler angle, and the formula is as follows:
wherein g _ quat0'、g_quat1'、g_quat2'、g_quat3' is the modified gyroscope quaternion data from step 107b, and m, n, o are the final, accurate estimated motion angles of the device.
The quaternion method shown in steps 107a, 107b, and 107c described above can reduce the computational complexity by using quaternion rotation instead of matrix rotation and euler rotation, and can prevent the gimbal deadlock phenomenon of euler angles.
To sum up with the step 101-107, the method for estimating the motion angle based on the gyroscope and the accelerometer corrects the gyroscope deviation and the acceleration through the improved IIR filter, so that the accurate estimation of the motion angle can be realized with a lower calculation amount. In the prior art, a first-order low-pass IIR filter is mostly used, but the accelerometer data needs to be filtered by a high-frequency signal, which is just opposite to the gyroscope data which needs to be filtered by a low-frequency signal. Therefore, the prior art scheme can not achieve a good filtering effect only through a single low-pass IIR. Compared with the prior art, the motion angle estimation method based on the gyroscope and the accelerometer can avoid the problems and can better filter accelerometer data and gyroscope data.
The application also provides a motion angle estimation device based on the gyroscope and the accelerometer. Fig. 2 shows a block diagram of a motion angle estimation apparatus based on a gyroscope and an accelerometer according to an embodiment of the present application. As shown in fig. 2, the motion angle estimation apparatus 200 based on a gyroscope and an accelerometer includes a first acquisition module 201, a second acquisition module 202, an acceleration calibration module 203, an offset filtering module 204, a third acquisition module 205, an acceleration filtering module 206, and an estimation module 207.
The first obtaining module 201 is configured to obtain the acceleration of the accelerometer at the current time, multiple groups of angular velocity averages of the gyroscope, a preset gyroscope correction coefficient, and a preset accelerometer correction coefficient.
A second acquiring module 202 for acquiring the gyroscope bias and the accelerometer bias while the gyroscope and the accelerometer remain horizontally disposed and stationary.
And the acceleration calibration module 203 is configured to calibrate the acceleration at the current moment according to the accelerometer deviation, and obtain the calibrated acceleration at the current moment.
And the deviation filtering module 204 is configured to perform IIR filtering on the gyroscope deviation by using a preset gyroscope correction coefficient and multiple groups of angular velocity average values, and obtain the filtered gyroscope deviation.
A third obtaining module 205, configured to obtain the filtered acceleration at the previous time, and if the filtered acceleration at the previous time does not exist, take the calibrated acceleration at the current time as the filtered acceleration at the previous time.
And the acceleration filtering module 206 is configured to perform IIR filtering on the calibrated acceleration at the current time by using a preset accelerometer correction coefficient and the filtered acceleration at the previous time, and obtain the filtered acceleration at the current time.
And the estimation module 207 is configured to estimate a final motion angle according to the filtered gyroscope bias and the filtered acceleration at the current time.
The steps executed by the modules 201-207 can refer to the corresponding steps 101-107 in the embodiment of fig. 1, and will not be described herein.
The application also provides a motion angle estimation system based on a gyroscope and an accelerometer, comprising: a memory for storing instructions executable by the processor; and a processor for executing instructions to implement the gyroscope and accelerometer based motion angle estimation method as described above.
Fig. 3 illustrates a system block diagram of a gyroscope and accelerometer based motion angle estimation system according to an embodiment of the present application. The gyroscope and accelerometer based motion angle estimation system 300 may include an internal communication bus 301, a Processor (Processor)302, a Read Only Memory (ROM)303, a Random Access Memory (RAM)304, and a communication port 305. When applied on a personal computer, the gyroscope and accelerometer based motion angle estimation system may also comprise a hard disk 307. The internal communication bus 301 may enable data communication between components of the gyroscope and accelerometer based motion angle estimation system 300. Processor 302 may make the determination and issue a prompt. In some embodiments, processor 302 may be comprised of one or more processors. The communication port 305 may enable data communication of the gyroscope and accelerometer based motion angle estimation system 300 with the outside. In some embodiments, the gyroscope and accelerometer based motion angle estimation system 300 may send and receive information and data from a network through the communication port 305. The gyroscope and accelerometer based motion angle estimation system 300 may also include various forms of program storage units and data storage units, such as a hard disk 307, Read Only Memory (ROM)303 and Random Access Memory (RAM)304, capable of storing various data files used for computer processing and/or communications, as well as possible program instructions executed by the processor 302. The processor executes these instructions to implement the main parts of the method. The results processed by the processor are communicated to the user device through the communication port and displayed on the user interface.
The above-mentioned motion angle estimation method based on a gyroscope and an accelerometer may be implemented as a computer program, stored in the hard disk 307, and recorded in the processor 302 for execution, so as to implement any of the motion angle estimation methods based on a gyroscope and an accelerometer in the present application.
The present application also provides a computer readable medium having stored thereon computer program code which, when executed by a processor, implements a gyroscope and accelerometer based motion angle estimation method as described above.
The present application also provides a computer readable medium having stored thereon computer program code which, when executed by a processor, implements any of the gyroscope and accelerometer based motion angle estimation methods described above.
The gyroscope and accelerometer based motion angle estimation method, when implemented as a computer program, may also be stored in a computer readable storage medium as an article of manufacture. For example, computer-readable storage media can include but are not limited to magnetic storage devices (e.g., hard disk, floppy disk, magnetic strips), optical disks (e.g., Compact Disk (CD), Digital Versatile Disk (DVD)), smart cards, and flash memory devices (e.g., electrically Erasable Programmable Read Only Memory (EPROM), card, stick, key drive). In addition, various storage media described herein can represent one or more devices and/or other machine-readable media for storing information. The term "machine-readable medium" can include, without being limited to, wireless channels and various other media (and/or storage media) capable of storing, containing, and/or carrying code and/or instructions and/or data.
It should be understood that the above-described embodiments are illustrative only. The embodiments described herein may be implemented in hardware, software, firmware, middleware, microcode, or any combination thereof. For a hardware implementation, the processing units may be implemented within one or more Application Specific Integrated Circuits (ASICs), Digital Signal Processors (DSPs), Digital Signal Processing Devices (DSPDs), Programmable Logic Devices (PLDs), Field Programmable Gate Arrays (FPGAs), processors, controllers, micro-controllers, microprocessors, and/or other electronic units designed to perform the functions described herein, or a combination thereof.
Having thus described the basic concept, it will be apparent to those skilled in the art that the foregoing disclosure is by way of example only, and is not intended to limit the present application. Various modifications, improvements and adaptations to the present application may occur to those skilled in the art, although not explicitly described herein. Such modifications, improvements and adaptations are proposed in the present application and thus fall within the spirit and scope of the exemplary embodiments of the present application.
Also, this application uses specific language to describe embodiments of the application. Reference throughout this specification to "one embodiment," "an embodiment," and/or "some embodiments" means that a particular feature, structure, or characteristic described in connection with at least one embodiment of the present application is included in at least one embodiment of the present application. Therefore, it is emphasized and should be appreciated that two or more references to "an embodiment" or "one embodiment" or "an alternative embodiment" in various places throughout this specification are not necessarily all referring to the same embodiment. Furthermore, some features, structures, or characteristics of one or more embodiments of the present application may be combined as appropriate.
Aspects of the present application may be embodied entirely in hardware, entirely in software (including firmware, resident software, micro-code, etc.) or in a combination of hardware and software. The above hardware or software may be referred to as "data block," module, "" engine, "" unit, "" component, "or" system. The processor may be one or more Application Specific Integrated Circuits (ASICs), Digital Signal Processors (DSPs), digital signal processing devices (DAPDs), Programmable Logic Devices (PLDs), Field Programmable Gate Arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or a combination thereof. Furthermore, aspects of the present application may be represented as a computer product, including computer readable program code, embodied in one or more computer readable media. For example, computer-readable media may include, but are not limited to, magnetic storage devices (e.g., hard disk, floppy disk, magnetic strips … …), optical disks (e.g., Compact Disk (CD), Digital Versatile Disk (DVD) … …), smart cards, and flash memory devices (e.g., card, stick, key drive … …).
Similarly, it should be noted that in the preceding description of embodiments of the application, various features are sometimes grouped together in a single embodiment, figure, or description thereof for the purpose of streamlining the disclosure aiding in the understanding of one or more of the embodiments. This method of disclosure, however, is not intended to require more features than are expressly recited in the claims. Indeed, the embodiments may be characterized as having less than all of the features of a single embodiment disclosed above.
Although the present application has been described with reference to the present specific embodiments, it will be recognized by those skilled in the art that the foregoing embodiments are merely illustrative of the present application and that various changes and substitutions of equivalents may be made without departing from the spirit of the application, and therefore, it is intended that all changes and modifications to the above-described embodiments that come within the spirit of the application fall within the scope of the claims of the application.
Claims (10)
1. A method of motion angle estimation based on a gyroscope and an accelerometer, comprising:
acquiring the acceleration of an accelerometer at the current moment, a plurality of groups of angular velocity mean values of a gyroscope, a preset gyroscope correction coefficient and a preset accelerometer correction coefficient;
acquiring a gyroscope bias and an accelerometer bias while the gyroscope and the accelerometer remain horizontally disposed and stationary;
calibrating the acceleration at the current moment according to the accelerometer deviation, and obtaining the calibrated acceleration at the current moment;
performing IIR filtering on the gyroscope deviation by using the preset gyroscope correction coefficient and the multiple groups of angular speed mean values, and obtaining filtered gyroscope deviation;
acquiring the filtered acceleration at the previous moment, and if the filtered acceleration at the previous moment does not exist, taking the calibrated acceleration at the current moment as the filtered acceleration at the previous moment;
performing IIR filtering on the calibrated acceleration at the current moment by using the preset accelerometer correction coefficient and the filtered acceleration at the previous moment, and obtaining the filtered acceleration at the current moment; and
and estimating a final motion angle according to the filtered gyroscope deviation and the filtered acceleration at the current moment.
2. The method of claim 1, wherein the IIR filtering the gyroscope bias using the preset gyroscope correction coefficients and the plurality of sets of angular velocity means and obtaining the filtered gyroscope bias comprises:
IIR_gx_bias’＝α*gx_mean+(1-α)*IIR_gx_bias
IIR_gy_bias’＝α*gy_mean+(1-α)*IIR_gy_bias
IIR_gz_bias’＝α*gz_mean+(1-α)*IIR_gz_bias
wherein gx _ mean, gy _ mean, and gz _ mean are the multiple groups of angular velocity mean values of the gyroscope on the x-axis, the y-axis, and the z-axis, respectively, α is the preset gyroscope correction coefficient, IIR _ gx _ bias, IIR _ gy _ bias, and IIR _ gz _ bias are the gyroscope deviations on the x-axis, the y-axis, and the z-axis, and IIR _ gx _ bias ', IIR _ gy _ bias ', and IIR _ gz _ bias ' are the filtered gyroscope deviations on the x-axis, the y-axis, and the z-axis, respectively.
3. The method of claim 1, wherein the IIR filtering the calibrated acceleration at the current time using the preset accelerometer correction factor and the filtered acceleration at the previous time and obtaining the filtered acceleration at the current time comprises:
IIR_ax’＝(1-β)*cor_ax+β*IIR_ax
IIR_ay’＝(1-β)*cor_ay+β*IIR_ay
IIR_az’＝(1-β)*cor_az+β*IIR_az
wherein cor _ ax, cor _ ay, and cor _ az are the accelerations at the calibrated current time on the x-axis, the y-axis, and the z-axis, respectively, β is the preset accelerometer correction coefficient, IIR _ ax, IIR _ ay, and IIR _ az are the accelerations at the filtered previous time on the x-axis, the y-axis, and the z-axis, and IIR _ ax ', IIR _ ay ', and IIR _ az ' are the accelerations at the filtered current time on the x-axis, the y-axis, and the z-axis, respectively.
4. The method of claim 1, wherein the estimated final angle of motion from the filtered gyroscope bias and the filtered acceleration at the current time is estimated using any one of: quaternion, matrix rotation, and euler rotation.
5. The method of claim 1, wherein the predetermined gyroscope calibration factor is in a range of 0.08-0.12.
6. The method of claim 1, wherein the predetermined accelerometer correction factor has a value in the range of 0.44 to 0.66.
7. The method of claim 1, wherein the calibrating the acceleration at the current time based on the accelerometer bias and obtaining the calibrated acceleration at the current time comprises:
cor_ax＝raw_ax*g*R-bias_ax
cor_ay＝raw_ay*g*R-bias_ay
cor_az＝raw_az*g*R-bias_az
wherein cor _ ax, cor _ ay, and cor _ az are the accelerations at the calibrated current time on the x-axis, the y-axis, and the z-axis, raw _ ax, raw _ ay, and raw _ az are the accelerations at the current time on the x-axis, the y-axis, and the z-axis, g is a gravitational acceleration constant, R is a preset unit conversion factor, and bias _ ax, bias _ ay, and bias _ az are the accelerometer deviations on the x-axis, the y-axis, and the z-axis, respectively.
8. A gyroscope and accelerometer-based motion angle estimation apparatus, comprising:
the first acquisition module is used for acquiring the acceleration of the accelerometer at the current moment, the average values of multiple groups of angular velocities of the gyroscope, a preset gyroscope correction coefficient and a preset accelerometer correction coefficient;
a second acquisition module for acquiring a gyroscope bias and an accelerometer bias while the gyroscope and the accelerometer remain horizontally disposed and stationary;
the acceleration calibration module is used for calibrating the acceleration at the current moment according to the accelerometer deviation and obtaining the calibrated acceleration at the current moment;
the deviation filtering module is used for carrying out IIR filtering on the gyroscope deviation by using the preset gyroscope correction coefficient and the multiple groups of angular velocity mean values and obtaining the filtered gyroscope deviation;
a third obtaining module, configured to obtain a filtered acceleration at a previous time, and if the filtered acceleration at the previous time does not exist, take the calibrated acceleration at the current time as the filtered acceleration at the previous time;
the acceleration filtering module is used for carrying out IIR filtering on the calibrated acceleration at the current moment by using the preset accelerometer correction coefficient and the filtered acceleration at the previous moment, and obtaining the filtered acceleration at the current moment; and
and the estimation module is used for estimating a final motion angle according to the filtered gyroscope deviation and the filtered acceleration at the current moment.
9. A gyroscope and accelerometer based motion angle estimation system comprising:
a memory for storing instructions executable by the processor; and a processor for executing the instructions to implement the method of any one of claims 1-7.
10. A computer-readable medium having stored thereon computer program code which, when executed by a processor, implements the method of any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011141659.0A CN112304337B (en) | 2020-10-22 | 2020-10-22 | Motion angle estimation method and system based on gyroscope and accelerometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011141659.0A CN112304337B (en) | 2020-10-22 | 2020-10-22 | Motion angle estimation method and system based on gyroscope and accelerometer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112304337A true CN112304337A (en) | 2021-02-02 |
| CN112304337B CN112304337B (en) | 2022-09-16 |
Family
ID=74326954
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011141659.0A Active CN112304337B (en) | 2020-10-22 | 2020-10-22 | Motion angle estimation method and system based on gyroscope and accelerometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112304337B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113124861A (en) * | 2021-03-03 | 2021-07-16 | 深圳市星砺达科技有限公司 | Space-assisted electronic equipment positioning method and device, computer equipment and medium |
| CN113959464A (en) * | 2021-10-25 | 2022-01-21 | 武汉元生创新科技有限公司 | Gyroscope-assisted accelerometer field calibration method and system |
| CN114111749A (en) * | 2021-10-29 | 2022-03-01 | 江苏凯卓立液压设备有限公司 | System and method for judging and calculating hydraulic tail plate action by using gyroscope |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090070058A1 (en) * | 2007-08-14 | 2009-03-12 | American Gnc Corporation | Miniaturized smart self-calibration electronic pointing method and system |
| CN101672649A (en) * | 2009-10-20 | 2010-03-17 | 哈尔滨工程大学 | Mooring alignment method of optical fiber strapdown system for ship based on digital low-pass filtering |
| US20120278024A1 (en) * | 2011-04-27 | 2012-11-01 | Samsung Electronics Co., Ltd. | Position estimation apparatus and method using acceleration sensor |
| CN107515011A (en) * | 2017-07-07 | 2017-12-26 | 青岛海信移动通信技术股份有限公司 | A kind of bearing calibration of gyroscope and device |
| JP2018057527A (en) * | 2016-10-04 | 2018-04-12 | 日本電信電話株式会社 | Posture estimation device, method and program |
| CN110503791A (en) * | 2019-08-20 | 2019-11-26 | 中国人民解放军海军工程大学 | Highly enriched nuclear material storage safety monitoring system and method |
-
2020
- 2020-10-22 CN CN202011141659.0A patent/CN112304337B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090070058A1 (en) * | 2007-08-14 | 2009-03-12 | American Gnc Corporation | Miniaturized smart self-calibration electronic pointing method and system |
| CN101672649A (en) * | 2009-10-20 | 2010-03-17 | 哈尔滨工程大学 | Mooring alignment method of optical fiber strapdown system for ship based on digital low-pass filtering |
| US20120278024A1 (en) * | 2011-04-27 | 2012-11-01 | Samsung Electronics Co., Ltd. | Position estimation apparatus and method using acceleration sensor |
| JP2018057527A (en) * | 2016-10-04 | 2018-04-12 | 日本電信電話株式会社 | Posture estimation device, method and program |
| CN107515011A (en) * | 2017-07-07 | 2017-12-26 | 青岛海信移动通信技术股份有限公司 | A kind of bearing calibration of gyroscope and device |
| CN110503791A (en) * | 2019-08-20 | 2019-11-26 | 中国人民解放军海军工程大学 | Highly enriched nuclear material storage safety monitoring system and method |
Non-Patent Citations (4)
| Title |
|---|
| DENNIS E.ANDERSON 等: "Maximum voluntary joint torque as a function of joint angle and angular velocity: Model development and application to the lower limb", 《JOURNAL OF BIOMECHANICS》 * |
| HANNES WIND 等: "Comparison of joint angle, velocity and acceleration estimators for hydraulically actuated manipulators to a novel dynamical approach", 《CONTROL ENGINEERING PRACTICE》 * |
| 伏家杰: "基于四元数非线性滤波的四旋翼姿态测量***研究", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》 * |
| 郭维: "基于INS/GPS数据融合的大客车运动状态估计方法研究", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113124861A (en) * | 2021-03-03 | 2021-07-16 | 深圳市星砺达科技有限公司 | Space-assisted electronic equipment positioning method and device, computer equipment and medium |
| CN113959464A (en) * | 2021-10-25 | 2022-01-21 | 武汉元生创新科技有限公司 | Gyroscope-assisted accelerometer field calibration method and system |
| CN113959464B (en) * | 2021-10-25 | 2023-10-20 | 武汉元生创新科技有限公司 | Gyroscope-assisted accelerometer field calibration method and system |
| CN114111749A (en) * | 2021-10-29 | 2022-03-01 | 江苏凯卓立液压设备有限公司 | System and method for judging and calculating hydraulic tail plate action by using gyroscope |
| CN114111749B (en) * | 2021-10-29 | 2022-12-02 | 江苏凯卓立液压设备有限公司 | System and method for judging and calculating hydraulic tail plate action by using gyroscope |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112304337B (en) | 2022-09-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112304337B (en) | Motion angle estimation method and system based on gyroscope and accelerometer | |
| CN112013836B (en) | Attitude heading reference system algorithm based on improved adaptive Kalman filtering | |
| CN106679649B (en) | Hand motion tracking system and method | |
| US7844415B1 (en) | Dynamic motion compensation for orientation instrumentation | |
| US9417091B2 (en) | System and method for determining and correcting field sensors errors | |
| CN101726295B (en) | Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation | |
| CN110398245B (en) | Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit | |
| CN109084806B (en) | Scalar field MEMS inertial system calibration method | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| CN110567492A (en) | Low-cost MEMS inertial sensor system-level calibration method | |
| CN113091770A (en) | Zero offset compensation method for inertial measurement sensor | |
| CN106595669B (en) | Method for resolving attitude of rotating body | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN116147624A (en) | Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system | |
| CN110058324B (en) | Strapdown gravimeter horizontal component error correction method using gravity field model | |
| CN112284388B (en) | Unmanned aerial vehicle multisource information fusion navigation method | |
| CN109506674B (en) | Acceleration correction method and device | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN106931965B (en) | Method and device for determining terminal posture | |
| CN110864684A (en) | User posture measuring and calculating method | |
| CN110030991B (en) | High-speed rotation angle movement measuring method for flyer integrating gyroscope and magnetometer | |
| CN114964214B (en) | Extended Kalman filtering attitude calculation method of attitude heading reference system | |
| CN110672127A (en) | Real-time calibration method for array type MEMS magnetic sensor | |
| CN115523919A (en) | Nine-axis attitude calculation method based on gyro drift optimization | |
| CN108731675B (en) | Measuring method and measuring device for course variation of object to be positioned and electronic equipment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |