CN113108790A - Robot IMU angle measurement method and device, computer equipment and storage medium - Google Patents
Robot IMU angle measurement method and device, computer equipment and storage medium Download PDFInfo
- Publication number
- CN113108790A CN113108790A CN202110528453.1A CN202110528453A CN113108790A CN 113108790 A CN113108790 A CN 113108790A CN 202110528453 A CN202110528453 A CN 202110528453A CN 113108790 A CN113108790 A CN 113108790A
- Authority
- CN
- China
- Prior art keywords
- data
- filtering
- angle
- angular velocity
- calculating
- 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
- 238000000691 measurement method Methods 0.000 title claims description 16
- 238000001914 filtration Methods 0.000 claims abstract description 104
- 230000001133 acceleration Effects 0.000 claims abstract description 96
- 230000004927 fusion Effects 0.000 claims abstract description 41
- 238000000034 method Methods 0.000 claims abstract description 28
- 238000004364 calculation method Methods 0.000 claims abstract description 21
- 238000012545 processing Methods 0.000 claims abstract description 20
- 230000000295 complement effect Effects 0.000 claims abstract description 14
- 239000013598 vector Substances 0.000 claims description 37
- 238000004590 computer program Methods 0.000 claims description 23
- 238000005070 sampling Methods 0.000 claims description 17
- 238000005259 measurement Methods 0.000 claims description 8
- 230000010354 integration Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 7
- 238000010606 normalization Methods 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Abstract
The embodiment of the invention discloses a robot IMU angle measuring method, a robot IMU angle measuring device, computer equipment and a storage medium. The method comprises the following steps: calculating fitting parameters; acquiring angular velocity data acquired by a gyroscope and acceleration data acquired by an acceleration sensor; filtering the angular velocity data and the acceleration data to obtain a filtering result; performing zero offset removing processing on the filtering result, and fusing the filtering result with the fitting parameters to obtain a fusion result; calculating a yaw angle according to the fusion result; and calculating a roll angle and a pitch angle according to the angular speed data and the acceleration data. By implementing the method of the embodiment of the invention, the yaw angle can be independently calculated, the influence of X-axis and Y-axis data on the yaw angle is avoided, the accuracy of the yaw angle is ensured, the complementary filtering algorithm is applied to the calculation of the roll angle and the pitch angle, the precision is improved, the requirement of an indoor mobile robot can be completely met, and the problem that the high precision and the low cost of the IMU cannot be compatible is solved.
Description
Technical Field
The invention relates to an indoor mobile robot, in particular to an IMU angle measuring method and device of the robot, computer equipment and a storage medium.
Background
In recent years, indoor mobile robots have been developed rapidly, have the characteristics of high autonomy, high adaptability, intelligent positioning, navigation and the like, and are widely applied to places such as hospitals, factories, schools, airports, hotels, restaurants and the like.
An IMU (Inertial measurement unit) provides attitude angle, angular velocity and acceleration for positioning and navigation of an indoor mobile robot, however, the accuracy of the IMU is greatly affected by the surrounding environment, and the requirement for positioning and navigation of the indoor mobile robot needs to be met by special algorithm processing or using a high-cost MEMS (Micro-Electro-Mechanical System) sensor, so that the IMU commonly used in the market at present generally has the problem that the high accuracy and the low cost cannot be compatible.
Therefore, it is necessary to design a new method to solve the problem that the high precision and low cost of IMU are incompatible.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a robot IMU angle measuring method, a robot IMU angle measuring device, a computer device and a storage medium.
In order to achieve the purpose, the invention adopts the following technical scheme: the robot IMU angle measurement method comprises the following steps:
calculating fitting parameters;
acquiring angular velocity data acquired by a gyroscope and acceleration data acquired by an acceleration sensor;
filtering the angular velocity data and the acceleration data to obtain a filtering result;
performing zero offset removing processing on the filtering result, and fusing the filtering result with the fitting parameters to obtain a fusion result;
calculating a yaw angle according to the fusion result;
and calculating a roll angle and a pitch angle according to the angular velocity data and the acceleration data.
The further technical scheme is as follows: the calculating fitting parameters comprises:
and acquiring full-range data of the gyroscope on the high-precision turntable, and performing linear fitting through an upper computer to obtain fitting parameters.
The further technical scheme is as follows: the filtering the angular velocity data and the acceleration data to obtain a filtering result includes:
by using M1Filtering the angular velocity data and the acceleration data to obtain a filtering result, (X1+ X2+ … +, XN)/N, wherein M is the filtering result, and X1, X2 and XN are sampling data; and N is the sampling times, and the sampling data comprises angular velocity data and acceleration data.
The further technical scheme is as follows: the zero offset removing processing is performed on the filtering result, and the filtering result is fused with the fitting parameter to obtain a fusion result, and the method comprises the following steps:
by using M2＝M1K-B carries out zero offset removal processing on the filtering result and is fused with the fitting parameters to obtain a fusion result, wherein M2For the fusion result, M1And K is a fitting parameter and B is zero-offset data after filtering.
The further technical scheme is as follows: the calculating of the yaw angle according to the fusion result comprises the following steps:
according to
Calculating a yaw angle, where θ is the yaw angle, t represents the integration time, w represents the angular velocity within the fused result,
representing the yaw angle at the previous moment.
The further technical scheme is as follows: the calculating of the roll angle and the pitch angle according to the angular velocity data and the acceleration data comprises:
and carrying out quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating a roll angle and a pitch angle.
The further technical scheme is as follows: and performing quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating a roll angle and a pitch angle, wherein the method comprises the following steps:
normalizing the acceleration data;
calculating a vector error between an acceleration vector under the coordinate of the computer body and an attitude vector calculated at the last attitude;
carrying out angular velocity compensation on the angular velocity data according to the vector error to obtain a compensation result;
updating the quaternion of the compensation result to obtain a quaternion;
and converting the quaternion into an Euler angle, and calculating a roll angle and a pitch angle.
The invention also provides an IMU angle measuring device of the robot, which comprises:
the parameter calculation unit is used for calculating fitting parameters;
the data acquisition unit is used for acquiring angular velocity data acquired by the gyroscope and acceleration data acquired by the acceleration sensor;
the filtering unit is used for filtering the angular velocity data and the acceleration data to obtain a filtering result;
the fusion unit is used for performing zero offset removing processing on the filtering result and fusing the filtering result with the fitting parameters to obtain a fusion result;
the yaw angle calculation unit is used for calculating a yaw angle according to the fusion result;
and the angle calculation unit is used for calculating a roll angle and a pitch angle according to the angular speed data and the acceleration data.
The invention also provides computer equipment which comprises a memory and a processor, wherein the memory is stored with a computer program, and the processor realizes the method when executing the computer program.
The invention also provides a storage medium storing a computer program which, when executed by a processor, is operable to carry out the method as described above.
Compared with the prior art, the invention has the beneficial effects that: according to the invention, the yaw angle is calculated independently by filtering and calibrating the angular velocity data acquired by the MEMS sensor, namely the gyroscope and the acceleration data acquired by the acceleration sensor, so that the influence of X-axis and Y-axis data on the yaw angle is avoided, the accuracy of the yaw angle is ensured, the calculation of the roll angle and the pitch angle utilizes a complementary filtering algorithm, the precision is improved, the requirement of an indoor mobile robot can be completely met, the cost of the whole hardware is low, and the problem that the high precision and the low cost of the IMU cannot be compatible is solved.
The invention is further described below with reference to the accompanying drawings and specific embodiments.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
Fig. 1 is a schematic view of an application scenario of a robot IMU angle measurement method according to an embodiment of the present invention;
fig. 2 is a schematic flow chart of an IMU angle measurement method of a robot according to an embodiment of the present invention;
fig. 3 is a sub-flow diagram of a robot IMU angle measurement method according to an embodiment of the present invention;
FIG. 4 is a schematic block diagram of an IMU angle measurement apparatus of a robot provided in an embodiment of the present invention;
fig. 5 is a schematic block diagram of an angle calculation unit of the robot IMU angle measurement apparatus according to an embodiment of the present invention;
FIG. 6 is a schematic block diagram of a computer device provided by an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It is also to be understood that the terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used in the specification of the present invention and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
It should be further understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.
Referring to fig. 1 and fig. 2, fig. 1 is a schematic view of an application scenario of a robot IMU angle measurement method according to an embodiment of the present invention. Fig. 2 is a schematic flow chart of a robot IMU angle measurement method according to an embodiment of the present invention. The robot IMU angle measurement method is applied to a server. The server can be in a single chip microcomputer form integrated on the robot, data interaction is carried out between the server and the sensor, the sensor comprises a gyroscope and an acceleration sensor, and filtering, parameter fusion, yaw angle calculation, roll angle calculation and pitch angle calculation are carried out through angular velocity data collected by the gyroscope and acceleration data collected by the acceleration sensor, so that the measurement of the IMU angle is realized. The IMU module applied to the indoor mobile robot can meet the requirement of low cost, and meanwhile, the positioning and navigation precision of the robot can be realized.
Fig. 2 is a schematic flow chart of a robot IMU angle measurement method according to an embodiment of the present invention. As shown in fig. 2, the method includes the following steps S110 to S160.
And S110, calculating fitting parameters.
In this embodiment, the fitting parameters refer to parameters of an angular velocity linear fitting process.
Specifically, full-range data of the gyroscope is collected on the high-precision rotary table, and linear fitting is carried out through the upper computer to obtain fitting parameters.
Specifically, full-range data acquisition of gyroscope data is carried out on the high-precision rotary table, linear fitting is carried out through the upper computer, and then fitting parameters are written into the flash of the single chip microcomputer.
And S120, acquiring angular velocity data acquired by the gyroscope and acceleration data acquired by the acceleration sensor.
In this embodiment, acceleration data and angular velocity data of the X-axis, Y-axis, and Z-axis are acquired by the ICM-42688-P inertial measurement unit.
And S130, filtering the angular velocity data and the acceleration data to obtain a filtering result.
In the present embodiment, the filtering result refers to a result of filtering angular velocity data and a result of filtering acceleration data.
In particular, with M1Filtering the angular velocity data and acceleration data (X1+ X2+ … +, XN)/NWave to obtain a filtering result, wherein M is the filtering result, and X1, X2 and XN are sampling data; n is the number of sampling times, and the sampling data includes angular velocity data and acceleration data, that is, the angular velocity data is also filtered by the above equation, and the acceleration data is also filtered by the above equation.
The digital low-pass filtering is realized by using a software algorithm, other hardware cost is not needed, only one calculation process is used, and the reliability is high.
And S140, performing zero offset removing processing on the filtering result, and fusing the filtering result with the fitting parameters to obtain a fusion result.
In this embodiment, the fusion result refers to a result obtained by performing zero offset removal processing on the filtered acceleration data and angular velocity data, and fusing the result with the fitting parameters.
In particular, with M2＝M1K-B carries out zero offset removal processing on the filtering result and is fused with the fitting parameters to obtain a fusion result, wherein M2For the fusion result, M1And K is a fitting parameter, B is zero offset data, and the zero offset data refers to a numerical value of zero offset of the gyroscope.
And S150, calculating a yaw angle according to the fusion result.
In this embodiment, the yaw angle refers to a result obtained by digitally filtering and integrating Z-axis angular velocity data in the fusion result.
In particular, according to
Calculating a yaw angle, where θ is the yaw angle, t represents the integration time, w represents the angular velocity within the fused result,
representing the yaw angle at the previous moment.
In the embodiment, the yaw angle is independently calculated, so that the influence of X-axis and Y-axis data on the yaw angle is avoided, and the accuracy of the yaw angle is ensured.
And S160, calculating a roll angle and a pitch angle according to the angular speed data and the acceleration data.
In this embodiment, the roll angle refers to an included angle between a horizontal axis and a horizontal axis of a chassis of the robot; the pitch angle refers to the included angle between the x axis of a robot coordinate system and the horizontal plane, and the robot coordinate system refers to a coordinate system loaded on the robot chassis.
Specifically, quaternion complementary filtering is performed on the angular velocity data and the acceleration data, and a roll angle and a pitch angle are calculated.
The roll angle and the pitch angle are calculated by using a complementary filtering algorithm, so that the precision is improved, and the requirement of the indoor mobile robot can be completely met.
In an embodiment, referring to fig. 3, the step S160 may include steps S161 to S165.
And S161, normalizing the acceleration data.
In this embodiment, in order to correspond to the unit quaternion, the three-dimensional vector of the acceleration data needs to be converted into the unit vector, and the normalized value will be [ -1, 1]According to the formula:
x ° -x/normal, y ° -y/normal, z ° -z/normal, where normal is the normalization coefficient, x, y, z are the acceleration data of the three axes, and x °, y °, and z ° are the values after the three-axis normalization.
And S162, calculating the vector error between the acceleration vector under the body coordinate and the attitude vector calculated at the last attitude.
In this embodiment, the vector error is a vector product of the current normalized acceleration value and the last attitude vector calculated from the previous attitude, where the last attitude vector calculated from the previous attitude is the last normalized acceleration value.
In particular toThe error between vectors is represented by a vector product, according to the formula:
wherein
Representing the error between the normalized acceleration vector and the last attitude vector,
is the acceleration vector after the normalization,
is the last attitude vector.
And S163, performing angular velocity compensation on the angular velocity data according to the vector error to obtain a compensation result.
In this embodiment, the compensation result refers to data obtained by performing compensation calibration on angular velocity data.
Specifically, according to the formula: gyroout is gyro + (Gain g error), where gyroout is angular velocity data after calibration, gyro is original angular velocity data, Gain is calibration Gain, the compensation effect is adjusted by setting the magnitude of the Gain, and g error is a quantized form of vector error.
And S164, performing quaternion updating on the compensation result to obtain a quaternion.
In this embodiment, the quaternion refers to a result obtained by performing quaternion calculation and updating on the compensation result.
According to the formula: q (T + T) ═ q (T) + T Ω (T) × q (T), where q (T + T) is an updated quaternion, indicating that the quaternion is updated from time T to time (T + T), T is time indicating that time T has elapsed from time T to time (T + T), q (T) is a quaternion value at time T, and Ω (T) is an angular velocity increment at time T, that is, an increment at time T of the angular velocity data after calibration.
And S165, converting the quaternion into an Euler angle, and calculating a roll angle and a pitch angle.
In particular, the rootAccording to the formula:
θ＝a sin(2*(q0*q2-q3*q1) Therein), wherein
And θ is the Euler angle, q0、q1、q3And q is4Is the composition data of the attitude quaternion, i.e., the quaternion in step S165.
According to the robot IMU angle measurement method, the yaw angle is calculated independently by filtering and calibrating the angular velocity data acquired by the MEMS sensor, namely the gyroscope and the acceleration data acquired by the acceleration sensor, so that the influence of X-axis and Y-axis data on the yaw angle is avoided, the accuracy of the yaw angle is ensured, a complementary filtering algorithm is applied to the calculation of the roll angle and the pitch angle, the precision is improved, the requirement of an indoor mobile robot can be met completely, the cost of the whole hardware is low, and the problem that the high precision and the low cost of the IMU cannot be compatible is solved.
Fig. 4 is a schematic block diagram of a robot IMU angle measuring apparatus 300 according to an embodiment of the present invention. As shown in fig. 4, the present invention also provides a robot IMU angle measuring apparatus 300 corresponding to the above robot IMU angle measuring method. The robot IMU angle measuring apparatus 300 includes a unit for performing the above-described robot IMU angle measuring method, and the apparatus may be configured in a server. Specifically, referring to fig. 4, the robot IMU angle measuring apparatus 300 includes a parameter calculating unit 301, a data obtaining unit 302, a filtering unit 303, a fusing unit 304, a yaw angle calculating unit 305, and an angle calculating unit 306.
A parameter calculating unit 301 for calculating fitting parameters; a data acquisition unit 302, configured to acquire angular velocity data acquired by a gyroscope and acceleration data acquired by an acceleration sensor; a filtering unit 303, configured to filter the angular velocity data and the acceleration data to obtain a filtering result; a fusion unit 304, configured to perform zero offset removal on the filtering result, and fuse the filtering result with the fitting parameter to obtain a fusion result; a yaw angle calculation unit 305 for calculating a yaw angle from the fusion result; and an angle calculating unit 306 for calculating roll angle and pitch angle according to the angular velocity data and the acceleration data.
In an embodiment, the parameter calculation unit 301 is configured to collect full-scale data of a gyroscope on a high-precision turntable, and perform linear fitting through an upper computer to obtain a fitting parameter.
In an embodiment, the filtering unit 303 is configured to employ M1Filtering the angular velocity data and the acceleration data to obtain a filtering result, (X1+ X2+ … +, XN)/N, wherein M is the filtering result, and X1, X2 and XN are sampling data; and N is the sampling times, and the sampling data comprises angular velocity data and acceleration data.
In an embodiment, the fusion unit 304 is configured to adopt M2＝M1K-B carries out zero offset removal processing on the filtering result and is fused with the fitting parameters to obtain a fusion result, wherein M2For the fusion result, M1And K is a fitting parameter and B is zero-offset data after filtering.
In an embodiment, the yaw angle calculation unit 305 is configured to calculate the yaw angle based on
Calculating a yaw angle, where θ is the yaw angle, t represents the integration time, w represents the angular velocity within the fused result,
representing the yaw angle at the previous moment.
In an embodiment, the angle calculating unit 306 is configured to perform quaternion complementary filtering on the angular velocity data and the acceleration data, and calculate a roll angle and a pitch angle.
In one embodiment, as shown in fig. 5, the angle calculation unit 306 includes a normalization sub-unit 3061, an error calculation sub-unit 3062, a compensation sub-unit 3063, an update sub-unit 3064, and a conversion sub-unit 3065.
A normalizing subunit 3061 for normalizing the acceleration data; an error calculation subunit 3062, configured to calculate a vector error between the acceleration vector in the coordinate of the machine body and the attitude vector calculated in the previous attitude; a compensation subunit 3063, configured to perform angular velocity compensation on the angular velocity data according to the vector error to obtain a compensation result; an updating subunit 3064, configured to perform quaternion updating on the compensation result to obtain a quaternion; a conversion subunit 3065, configured to convert the quaternion into euler angles, and calculate roll angles and pitch angles.
It should be noted that, as will be clear to those skilled in the art, the detailed implementation process of the above-mentioned robot IMU angle measuring apparatus 300 and each unit may refer to the corresponding description in the foregoing method embodiments, and for convenience and brevity of description, no further description is provided herein.
The above described robotic IMU angle measuring device 300 may be implemented in the form of a computer program which may be run on a computer apparatus as shown in fig. 6.
Referring to fig. 6, fig. 6 is a schematic block diagram of a computer device according to an embodiment of the present application. The computer device 500 may be a server, wherein the server may be an independent server or a server cluster composed of a plurality of servers.
Referring to fig. 6, the computer device 500 includes a processor 502, memory, and a network interface 505 connected by a system bus 501, where the memory may include a non-volatile storage medium 503 and an internal memory 504.
The non-volatile storage medium 503 may store an operating system 5031 and a computer program 5032. The computer programs 5032 include program instructions that, when executed, cause the processor 502 to perform a robotic IMU angle measurement method.
The processor 502 is used to provide computing and control capabilities to support the operation of the overall computer device 500.
The internal memory 504 provides an environment for the execution of the computer program 5032 in the non-volatile storage medium 503, which computer program 5032, when executed by the processor 502, causes the processor 502 to perform a method of robotic IMU angle measurement.
The network interface 505 is used for network communication with other devices. Those skilled in the art will appreciate that the configuration shown in fig. 6 is a block diagram of only a portion of the configuration associated with the present application and does not constitute a limitation of the computer device 500 to which the present application may be applied, and that a particular computer device 500 may include more or less components than those shown, or may combine certain components, or have a different arrangement of components.
Wherein the processor 502 is configured to run the computer program 5032 stored in the memory to implement the following steps:
calculating fitting parameters; acquiring angular velocity data acquired by a gyroscope and acceleration data acquired by an acceleration sensor; filtering the angular velocity data and the acceleration data to obtain a filtering result; performing zero offset removing processing on the filtering result, and fusing the filtering result with the fitting parameters to obtain a fusion result; calculating a yaw angle according to the fusion result; and calculating a roll angle and a pitch angle according to the angular velocity data and the acceleration data.
In an embodiment, when the processor 502 implements the step of calculating the fitting parameter, the following steps are specifically implemented:
and acquiring full-range data of the gyroscope on the high-precision turntable, and performing linear fitting through an upper computer to obtain fitting parameters.
In an embodiment, when the processor 502 implements the step of filtering the angular velocity data and the acceleration data to obtain the filtering result, the following steps are specifically implemented:
by using M1Filtering the angular velocity data and the acceleration data to obtain a filtering result (X1+ X2+ … +, XN)/N, whereinM is the filtering result, X1, X2, and XN are the sampled data; and N is the sampling times, and the sampling data comprises angular velocity data and acceleration data.
In an embodiment, when the processor 502 implements the step of performing zero offset removal on the filtering result and fusing the filtering result with the fitting parameter to obtain a fused result, the following steps are specifically implemented:
by using M2＝M1K-B carries out zero offset removal processing on the filtering result and is fused with the fitting parameters to obtain a fusion result, wherein M2For the fusion result, M1And K is a fitting parameter and B is zero-offset data after filtering.
In an embodiment, when the processor 502 implements the step of calculating the yaw angle according to the fusion result, the following steps are specifically implemented:
according to
Calculating a yaw angle, where θ is the yaw angle, t represents the integration time, w represents the angular velocity within the fused result,
representing the yaw angle at the previous moment.
In an embodiment, when the processor 502 implements the step of calculating the roll angle and the pitch angle according to the angular velocity data and the acceleration data, the following steps are specifically implemented:
and carrying out quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating a roll angle and a pitch angle.
In an embodiment, when the processor 502 implements the steps of performing quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating the roll angle and the pitch angle, the following steps are specifically implemented:
normalizing the acceleration data; calculating a vector error between an acceleration vector under the coordinate of the computer body and an attitude vector calculated at the last attitude; carrying out angular velocity compensation on the angular velocity data according to the vector error to obtain a compensation result; updating the quaternion of the compensation result to obtain a quaternion; and converting the quaternion into an Euler angle, and calculating a roll angle and a pitch angle.
It should be understood that in the embodiment of the present Application, the Processor 502 may be a Central Processing Unit (CPU), and the Processor 502 may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field Programmable Gate Arrays (FPGAs) or other Programmable logic devices, discrete Gate or transistor logic devices, discrete hardware components, and the like. Wherein a general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
It will be understood by those skilled in the art that all or part of the flow of the method implementing the above embodiments may be implemented by a computer program instructing associated hardware. The computer program includes program instructions, and the computer program may be stored in a storage medium, which is a computer-readable storage medium. The program instructions are executed by at least one processor in the computer system to implement the flow steps of the embodiments of the method described above.
Accordingly, the present invention also provides a storage medium. The storage medium may be a computer-readable storage medium. The storage medium stores a computer program, wherein the computer program, when executed by a processor, causes the processor to perform the steps of:
calculating fitting parameters; acquiring angular velocity data acquired by a gyroscope and acceleration data acquired by an acceleration sensor; filtering the angular velocity data and the acceleration data to obtain a filtering result; performing zero offset removing processing on the filtering result, and fusing the filtering result with the fitting parameters to obtain a fusion result; calculating a yaw angle according to the fusion result; and calculating a roll angle and a pitch angle according to the angular velocity data and the acceleration data.
In an embodiment, when the step of calculating the fitting parameter is implemented by the processor executing the computer program, the following steps are specifically implemented:
and acquiring full-range data of the gyroscope on the high-precision turntable, and performing linear fitting through an upper computer to obtain fitting parameters.
In an embodiment, when the processor executes the computer program to implement the step of filtering the angular velocity data and the acceleration data to obtain a filtering result, the following steps are specifically implemented:
by using M1Filtering the angular velocity data and the acceleration data to obtain a filtering result, (X1+ X2+ … +, XN)/N, wherein M is the filtering result, and X1, X2 and XN are sampling data; and N is the sampling times, and the sampling data comprises angular velocity data and acceleration data.
In an embodiment, when the processor executes the computer program to implement the zero offset removing processing on the filtering result and fuse the filtering result with the fitting parameter to obtain a fused result, the following steps are specifically implemented:
by using M2＝M1K-B carries out zero offset removal processing on the filtering result and is fused with the fitting parameters to obtain a fusion result, wherein M2For the fusion result, M1And K is a fitting parameter and B is zero-offset data after filtering.
In an embodiment, when the processor executes the computer program to implement the step of calculating the yaw angle according to the fusion result, the following steps are specifically implemented:
according to
Calculating a yaw angle, where θ is the yaw angle, t represents the integration time, w represents the angular velocity within the fused result,
representing the yaw angle at the previous moment.
In an embodiment, when the processor executes the computer program to implement the step of calculating the roll angle and the pitch angle according to the angular velocity data and the acceleration data, the processor specifically implements the following steps:
and carrying out quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating a roll angle and a pitch angle.
In an embodiment, when the processor executes the computer program to implement the steps of performing quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating the roll angle and the pitch angle, the following steps are specifically implemented:
normalizing the acceleration data; calculating a vector error between an acceleration vector under the coordinate of the computer body and an attitude vector calculated at the last attitude; carrying out angular velocity compensation on the angular velocity data according to the vector error to obtain a compensation result; updating the quaternion of the compensation result to obtain a quaternion; and converting the quaternion into an Euler angle, and calculating a roll angle and a pitch angle.
The storage medium may be a usb disk, a removable hard disk, a Read-Only Memory (ROM), a magnetic disk, or an optical disk, which can store various computer readable storage media.
Those of ordinary skill in the art will appreciate that the elements and algorithm steps of the examples described in connection with the embodiments disclosed herein may be embodied in electronic hardware, computer software, or combinations of both, and that the components and steps of the examples have been described in a functional general in the foregoing description for the purpose of illustrating clearly the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus and method may be implemented in other ways. For example, the above-described apparatus embodiments are merely illustrative. For example, the division of each unit is only one logic function division, and there may be another division manner in actual implementation. For example, various elements or components may be combined or may be integrated into another system, or some features may be omitted, or not implemented.
The steps in the method of the embodiment of the invention can be sequentially adjusted, combined and deleted according to actual needs. The units in the device of the embodiment of the invention can be merged, divided and deleted according to actual needs. In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit.
The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a storage medium. Based on such understanding, the technical solution of the present invention essentially or partially contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a terminal, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention.
While the invention has been described with reference to specific embodiments, the invention is not limited thereto, and various equivalent modifications and substitutions can be easily made by those skilled in the art within the technical scope of the invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (10)
1. The robot IMU angle measurement method is characterized by comprising the following steps:
calculating fitting parameters;
acquiring angular velocity data acquired by a gyroscope and acceleration data acquired by an acceleration sensor;
filtering the angular velocity data and the acceleration data to obtain a filtering result;
performing zero offset removing processing on the filtering result, and fusing the filtering result with the fitting parameters to obtain a fusion result;
calculating a yaw angle according to the fusion result;
and calculating a roll angle and a pitch angle according to the angular velocity data and the acceleration data.
2. The robotic IMU angle measurement method of claim 1, wherein the calculating fitting parameters comprises:
and acquiring full-range data of the gyroscope on the high-precision turntable, and performing linear fitting through an upper computer to obtain fitting parameters.
3. The robotic IMU angle measurement method of claim 1, wherein the filtering the angular velocity data and acceleration data to obtain filtered results comprises:
by using M1Filtering the angular velocity data and the acceleration data to obtain a filtering result, (X1+ X2+ … +, XN)/N, wherein M is the filtering result, and X1, X2 and XN are sampling data; and N is the sampling times, and the sampling data comprises angular velocity data and acceleration data.
4. The method of claim 3, wherein the applying zero offset to the filtered results and fusing the filtered results with the fitting parameters to obtain fused results comprises:
by using M2＝M1K-B carries out zero offset removal processing on the filtering result and is fused with the fitting parameters to obtain a fusion result, wherein M2For the fusion result, M1And K is a fitting parameter and B is zero-offset data after filtering.
5. The method of claim 4, wherein said calculating a yaw angle from said fused results comprises:
according to
Calculating a yaw angle, where θ is the yaw angle, t represents the integration time, w represents the angular velocity within the fused result,
representing the yaw angle at the previous moment.
6. The robotic IMU angle measurement method of claim 1, wherein the calculating roll and pitch angles from the angular velocity data and acceleration data comprises:
and carrying out quaternion complementary filtering on the angular velocity data and the acceleration data, and calculating a roll angle and a pitch angle.
7. The method of IMU angular measurement of claim 6, wherein said performing quaternion complementary filtering on said angular velocity data and acceleration data and calculating roll and pitch angles comprises:
normalizing the acceleration data;
calculating a vector error between an acceleration vector under the coordinate of the computer body and an attitude vector calculated at the last attitude;
carrying out angular velocity compensation on the angular velocity data according to the vector error to obtain a compensation result;
updating the quaternion of the compensation result to obtain a quaternion;
and converting the quaternion into an Euler angle, and calculating a roll angle and a pitch angle.
8. IMU angle measurement device of robot, its characterized in that includes:
the parameter calculation unit is used for calculating fitting parameters;
the data acquisition unit is used for acquiring angular velocity data acquired by the gyroscope and acceleration data acquired by the acceleration sensor;
the filtering unit is used for filtering the angular velocity data and the acceleration data to obtain a filtering result;
the fusion unit is used for performing zero offset removing processing on the filtering result and fusing the filtering result with the fitting parameters to obtain a fusion result;
the yaw angle calculation unit is used for calculating a yaw angle according to the fusion result;
and the angle calculation unit is used for calculating a roll angle and a pitch angle according to the angular speed data and the acceleration data.
9. A computer device, characterized in that the computer device comprises a memory, on which a computer program is stored, and a processor, which when executing the computer program implements the method according to any of claims 1 to 7.
10. A storage medium, characterized in that the storage medium stores a computer program which, when executed by a processor, implements the method according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110528453.1A CN113108790A (en) | 2021-05-14 | 2021-05-14 | Robot IMU angle measurement method and device, computer equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110528453.1A CN113108790A (en) | 2021-05-14 | 2021-05-14 | Robot IMU angle measurement method and device, computer equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113108790A true CN113108790A (en) | 2021-07-13 |
Family
ID=76722158
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110528453.1A Pending CN113108790A (en) | 2021-05-14 | 2021-05-14 | Robot IMU angle measurement method and device, computer equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113108790A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114323007A (en) * | 2021-12-30 | 2022-04-12 | 西人马帝言(北京)科技有限公司 | Carrier motion state estimation method and device |
| CN114947627A (en) * | 2022-08-01 | 2022-08-30 | 深圳市云鼠科技开发有限公司 | Determination method, device, equipment and storage medium for initializing IMU of sweeper |
| CN115268442A (en) * | 2022-07-27 | 2022-11-01 | 湖州丽天智能科技有限公司 | Automatic deviation rectifying method and system for photovoltaic cleaning robot and cleaning robot |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150012839A (en) * | 2013-07-26 | 2015-02-04 | 삼성중공업 주식회사 | A method for attitude reference system of moving unit and an apparatus using the same |
| CN107202578A (en) * | 2017-05-10 | 2017-09-26 | 陕西瑞特测控技术有限公司 | A kind of strapdown vertical gyroscope calculation method based on MEMS technology |
| US20170350721A1 (en) * | 2015-10-13 | 2017-12-07 | Shanghai Huace Navigation Technology Ltd | Method of Updating All-Attitude Angle of Agricultural Machine Based on Nine-Axis MEMS Sensor |
| CN108061855A (en) * | 2017-11-30 | 2018-05-22 | 天津大学 | A kind of globular motor rotor position detecting method based on MEMS sensor |
| CN109001787A (en) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | A kind of method and its merge sensor of solving of attitude and positioning |
| CN109540135A (en) * | 2018-11-09 | 2019-03-29 | 华南农业大学 | The method and device that the detection of paddy field tractor pose and yaw angle are extracted |
| CN110672099A (en) * | 2019-09-09 | 2020-01-10 | 武汉元生创新科技有限公司 | Course correction method and system for indoor robot navigation |
| CN111964676A (en) * | 2020-08-26 | 2020-11-20 | 三一机器人科技有限公司 | Attitude heading calculation method and device, electronic equipment and computer readable storage medium |
| WO2020253854A1 (en) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | Mobile robot posture angle calculation method |
-
2021
- 2021-05-14 CN CN202110528453.1A patent/CN113108790A/en active Pending
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150012839A (en) * | 2013-07-26 | 2015-02-04 | 삼성중공업 주식회사 | A method for attitude reference system of moving unit and an apparatus using the same |
| US20170350721A1 (en) * | 2015-10-13 | 2017-12-07 | Shanghai Huace Navigation Technology Ltd | Method of Updating All-Attitude Angle of Agricultural Machine Based on Nine-Axis MEMS Sensor |
| CN107202578A (en) * | 2017-05-10 | 2017-09-26 | 陕西瑞特测控技术有限公司 | A kind of strapdown vertical gyroscope calculation method based on MEMS technology |
| CN108061855A (en) * | 2017-11-30 | 2018-05-22 | 天津大学 | A kind of globular motor rotor position detecting method based on MEMS sensor |
| CN109001787A (en) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | A kind of method and its merge sensor of solving of attitude and positioning |
| CN109540135A (en) * | 2018-11-09 | 2019-03-29 | 华南农业大学 | The method and device that the detection of paddy field tractor pose and yaw angle are extracted |
| WO2020253854A1 (en) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | Mobile robot posture angle calculation method |
| CN110672099A (en) * | 2019-09-09 | 2020-01-10 | 武汉元生创新科技有限公司 | Course correction method and system for indoor robot navigation |
| CN111964676A (en) * | 2020-08-26 | 2020-11-20 | 三一机器人科技有限公司 | Attitude heading calculation method and device, electronic equipment and computer readable storage medium |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114323007A (en) * | 2021-12-30 | 2022-04-12 | 西人马帝言(北京)科技有限公司 | Carrier motion state estimation method and device |
| CN115268442A (en) * | 2022-07-27 | 2022-11-01 | 湖州丽天智能科技有限公司 | Automatic deviation rectifying method and system for photovoltaic cleaning robot and cleaning robot |
| CN114947627A (en) * | 2022-08-01 | 2022-08-30 | 深圳市云鼠科技开发有限公司 | Determination method, device, equipment and storage medium for initializing IMU of sweeper |
| CN114947627B (en) * | 2022-08-01 | 2022-11-22 | 深圳市云鼠科技开发有限公司 | Determination method, device, equipment and storage medium for initializing IMU of sweeper |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113108790A (en) | Robot IMU angle measurement method and device, computer equipment and storage medium | |
| CN109696183B (en) | Calibration method and device of inertia measurement unit | |
| CN108458714B (en) | Euler angle solving method without gravity acceleration in attitude detection system | |
| US10551939B2 (en) | Attitude detecting device | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN109724602A (en) | A kind of attitude algorithm system and its calculation method based on hardware FPU | |
| CN105588567A (en) | Automatic magnetometer correction assisted AHRS (altitude and heading reference system) and method | |
| CN110440827B (en) | Parameter error calibration method and device and storage medium | |
| CN109827571B (en) | Double-accelerometer calibration method under turntable-free condition | |
| CN114216456B (en) | Attitude measurement method based on fusion of IMU and robot body parameters | |
| CN107664498A (en) | A kind of posture fusion calculation method and system | |
| CN112066985B (en) | Initialization method, device, medium and electronic equipment for combined navigation system | |
| JP7025215B2 (en) | Positioning system and positioning method | |
| CN116067370B (en) | IMU gesture resolving method, IMU gesture resolving equipment and storage medium | |
| Hoang et al. | Pre-processing technique for compass-less madgwick in heading estimation for industry 4.0 | |
| Hoang et al. | Measurement optimization for orientation tracking based on no motion no integration technique | |
| CN106595669B (en) | Method for resolving attitude of rotating body | |
| CN108592902B (en) | Positioning equipment, positioning method and system based on multiple sensors and mechanical arm | |
| Carratù et al. | IMU self-alignment in suspensions control system | |
| CN109866217B (en) | Robot mileage positioning method, device, terminal equipment and computer storage medium | |
| CN110567493A (en) | Magnetometer calibration data acquisition method and device and aircraft | |
| CN113483765A (en) | Satellite autonomous attitude determination method | |
| CN106931965B (en) | Method and device for determining terminal posture | |
| CN110954081A (en) | Quick calibration device and method for magnetic compass | |
| CN116125789A (en) | Gesture algorithm parameter automatic matching system and method based on quaternion |
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 |