CN112304340B

CN112304340B - Attitude calculation method and device based on nine-axis IMU and storage medium

Info

Publication number: CN112304340B
Application number: CN202011542749.0A
Authority: CN
Inventors: 黄永鑫; 路晗; 方珺
Original assignee: Beijing Zvr Technology Co ltd
Current assignee: Beijing Zvr Technology Co ltd
Priority date: 2020-12-24
Filing date: 2020-12-24
Publication date: 2021-04-06
Anticipated expiration: 2040-12-24
Also published as: CN112304340A

Abstract

The attitude calculation method comprises the steps of calibrating internal parameters of three internal modules by utilizing original data output by a gyroscope, an accelerometer and a magnetometer of the nine-axis IMU, and converting the original data into calibration data by means of calibrating the internal parameters; carrying out data fusion on calibration data of a gyroscope and an accelerometer, and outputting preliminary attitude data of an IMU; in a magnetic force and attitude data correction algorithm based on a magnetic correction time corresponding period and an attitude correction time corresponding period, the gyroscope drift is corrected by using a gravity vector and a magnetic field vector with the highest weight value, and the final attitude data of the IMU is output. The IMU attitude is corrected through mutual correction of magnetic force and attitude data in a long and short period, adverse effects of magnetic field interference on attitude calculation are restrained, accurate and stable output of the IMU attitude is achieved, and application requirements of the IMU device in an indoor complex magnetic field environment are met.

Description

Attitude calculation method and device based on nine-axis IMU and storage medium

Technical Field

The application relates to the technical field of navigation and positioning, in particular to a method and a device for attitude calculation based on a nine-axis IMU and a storage medium.

Background

With the development of information technology and economic construction, higher requirements are provided for navigation technology in the fields of transportation, industry, agriculture, surveying and mapping and the like, and the original data of an accelerometer (Acc), a gyroscope (Gyro) and a magnetometer (Mag) output by a nine-axis IMU (Inertial measurement unit) are comprehensively analyzed and calculated by an IMU attitude calculation method, so that the attitude data of three-degree-of-freedom rotation of the IMU device is stably output to an upper computer by an algorithm, and the method can be widely applied to the fields of navigation positioning and the like.

In the traditional attitude calculation method, the real-time attitude information of the IMU can be obtained by calculation by means of the three-axis angular velocity data of the gyroscope. However, the result of the attitude calculation is biased because of a certain drift due to the simple dependence of the accumulation operation. Therefore, for a nine-axis IMU device, attitude correction by means of an accelerometer and a magnetometer is required periodically or aperiodically in order to expect output of accurate attitude data.

However, a real indoor scene has a complex magnetic field environment, the magnetometer of the IMU device is easily affected by interference, when the magnetometer outputs inaccurate data, the conventional attitude calculation method is obviously affected, and when the interference is serious, correction failure and attitude data error are directly caused.

Disclosure of Invention

In view of this, an object of the present application is to provide a method, an apparatus, and a storage medium for resolving an attitude based on a nine-axis IMU, which can correct an attitude of the IMU by mutually correcting magnetic force and attitude data in a long-and-short period, and suppress adverse effects of magnetic field interference on the attitude resolution, thereby achieving accurate and stable output of the attitude of the IMU and meeting application requirements on an IMU device in an indoor complex magnetic field environment.

In a first aspect, the present application provides an attitude calculation method based on a nine-axis IMU, including:

respectively obtaining calibrated gyroscope calibration data, accelerometer calibration data and magnetometer calibration data based on the obtained original data of the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU;

performing data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data;

determining a set of magnetic field vector-weight values characterizing the temporal and spatial statistics of the magnetic field;

converting accelerometer calibration data and magnetometer calibration data into an acceleration vector and a magnetic field vector for vector description;

when the preset magnetic force correction moment is reached, correcting the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction moment to obtain a corrected magnetic field vector-weight value set;

and when a preset attitude correction moment is reached, correcting the preliminary attitude data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present to obtain final attitude data, wherein the corresponding period of the attitude correction moment is integral multiple of the corresponding period of the magnetic force correction moment.

Preferably, the data fusion of the obtained calibration data of the gyroscope and the calibration data of the accelerometer to obtain the preliminary attitude data includes:

calculating candidate preliminary attitude data at the current sampling moment according to the preliminary attitude data calculated at the last sampling moment and the acquired gyroscope calibration data at the current sampling moment;

if the nine-axis IMU is in a non-static state, updating the preliminary attitude data based on the candidate preliminary attitude data to obtain updated preliminary attitude data;

if the nine-axis IMU is in a static state, determining a gravity vector at the current sampling moment in an IMU initial coordinate system according to accelerometer calibration data at the current sampling moment;

determining an error vector between the gravity vector at the current sampling moment and an initial gravity vector according to the initial gravity vector predetermined under the IMU initial coordinate system;

calculating to obtain fused preliminary attitude data by adopting a data fusion algorithm according to the candidate preliminary attitude data and the error vector; updating the preliminary attitude data based on the fused preliminary attitude data to obtain updated preliminary attitude data; the IMU initial coordinate system is a coordinate system determined by the initial time of the nine-axis IMU after the gyroscope, the accelerometer and the magnetometer are calibrated.

Preferably, the set of magnetic field vector-weight values is determined by:

obtaining a plurality of initial set elements based on each initial magnetic field vector and the weight value corresponding to the initial magnetic field vector;

when the difference value of the newly added magnetic field vector and any one initial magnetic field vector is within a preset conical range, determining the accumulated weight value of the initial magnetic field vector corresponding to the newly added magnetic field vector;

when the difference value of the newly added magnetic field vector and any one initial magnetic field vector is not in the preset conical range, determining the newly added magnetic field vector and the weight value corresponding to the newly added magnetic field vector as a newly added set element;

and updating the magnetic field vector-weight value set based on the accumulated weight value of the initial magnetic field vector corresponding to the newly added magnetic field vector or the newly added set element.

Preferably, the set of magnetic field vector-weight values is determined by:

when detecting that the number of elements of all the set elements in the magnetic field vector-weight value set is greater than a set number threshold value, deleting part of target elements;

the method comprises the steps of sorting all set elements according to the sequence of the corresponding weight values from small to large, and selecting a target number of set elements from the minimum weight value to form part of target elements, wherein the target number is the number difference between the element number of all set elements and a set number threshold, and the set elements comprise initial set elements or the sum of the initial set elements and the newly added set elements.

Preferably, the set of magnetic field vector-weight values is determined by the following formula:

wherein P represents a set of magnetic field vector-weight values,

denotes the ith magnetic field vector, ki denotes the weight value corresponding to the ith magnetic field vector, and N denotes the magnetic field vector-weightThe number of all magnetic field vectors in the weight set,

represents the magnetometer calibration data at time t,

and representing a quaternion corresponding to the preliminary attitude data under the IMU coordinate system at the time t.

Preferably, when a preset magnetic force correction time is reached, the magnetic field vector-weight value set is corrected according to the preliminary attitude data and a magnetic field vector obtained at the current magnetic force correction time, and the corrected magnetic field vector-weight value set includes:

determining a magnetic field vector corresponding to the largest weight value in the magnetic field vector-weight value set when the nine-axis IMU is in a static state;

determining a magnetic field vector under an IMU coordinate system based on the magnetic field vector at the current magnetic force correction moment and the attitude quaternion of the gyroscope in the preliminary attitude data;

comparing the magnetic field vector under the IMU coordinate system with the magnetic field vector corresponding to the maximum weight value in the magnetic field vector-weight value set to obtain an angle difference value between the magnetic field direction detected by the magnetometer and the stable magnetic field direction at the current magnetic force correction moment;

if the angle difference value is within a preset angle difference range, adding a magnetic field vector under an IMU coordinate system at the current magnetic force correction moment into the magnetic field vector-weight value set;

and if the angle difference value is not within the preset angle difference range, not adding the magnetic field vector under the IMU coordinate system at the current magnetic force correction moment into the magnetic field vector-weight value set.

Preferably, when the preset attitude correction time is reached, correcting the preliminary attitude data according to a magnetic field vector with a highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a current stationary state to obtain final attitude data, including:

updating the attitude quaternion of the nine-axis IMU based on the magnetic field vector corresponding to the maximum weight value and the acceleration vector determined by the accelerometer at the current attitude correction moment;

and determining the updated attitude quaternion of the nine-axis IMU as final attitude data.

In a second aspect, an embodiment of the present application provides an attitude resolver based on a nine-axis IMU, including:

the internal reference calibration module is used for respectively obtaining calibrated gyroscope calibration data, accelerometer calibration data and magnetometer calibration data based on the obtained original data of the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU;

the data processing module is used for carrying out data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data;

the set determining module is used for determining a magnetic field vector-weight value set representing the magnetic field time statistics and the magnetic field space statistics;

the data conversion module is used for converting the accelerometer calibration data and the magnetometer calibration data into an acceleration vector and a magnetic field vector for vector description;

the first correction module is used for correcting the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction moment when the preset magnetic force correction moment is reached, so as to obtain a corrected magnetic field vector-weight value set;

and the second correction module is used for correcting the preliminary attitude data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present when a preset attitude correction moment is reached, so as to obtain final attitude data, wherein the period corresponding to the attitude correction moment is an integral multiple of the period corresponding to the magnetic force correction moment.

In a third aspect, the present application further provides an electronic device, including: a processor, a memory and a bus, the memory storing machine-readable instructions executable by the processor, the processor and the memory communicating over the bus when the electronic device is operating, the machine-readable instructions when executed by the processor performing the steps of the nine-axis IMU-based attitude solution method as described above.

In a fourth aspect, the present application also provides a computer-readable storage medium having a computer program stored thereon, where the computer program is executed by a processor to perform the steps of the nine-axis IMU-based attitude solution method as described above.

The application provides a method, a device and a storage medium for solving an attitude based on a nine-axis IMU, wherein the method for solving the attitude comprises the following steps: firstly, calibrating internal parameters of three internal modules by utilizing original data output by a gyroscope, an accelerometer and a magnetometer of a nine-axis IMU in an internal reference calibration algorithm, and converting the original data into calibration data by virtue of the calibrated internal parameters; and finally, correcting the drift of the gyroscope by using a gravity vector and a magnetic field vector with the highest weight value in a magnetic force and attitude data correction algorithm based on a magnetic correction time corresponding period and an attitude correction time corresponding period, and outputting the final attitude data of the IMU. Therefore, the IMU attitude can be corrected through mutual correction of the magnetic force and the attitude data with long and short periods, adverse effects of magnetic field interference on attitude calculation are inhibited, accurate and stable output of the IMU attitude is further realized, and application requirements of the IMU device in an indoor complex magnetic field environment are met.

In order to make the aforementioned objects, features and advantages of the present application more comprehensible, preferred embodiments accompanied with figures are described in detail below.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained from the drawings without inventive effort.

Fig. 1 is a flowchart of an attitude solution method based on a nine-axis IMU according to an embodiment of the present application;

fig. 2 is a flowchart illustrating a method for sequentially performing internal reference calibration by a gyroscope and an accelerometer according to an embodiment of the present disclosure;

fig. 3 is a flowchart of a six-axis fusion calculation method based on a complementary filtering algorithm according to an embodiment of the present disclosure;

fig. 4 is an organizational structure block diagram of a posture solution method based on a nine-axis IMU according to an embodiment of the present application;

fig. 5 is a flowchart of a magnetic force and attitude data correction algorithm based on a long and short cycle according to an embodiment of the present application;

FIG. 6 is a schematic diagram of a visual result of the attitude of the IMU device before the IMU device approaches the high magnetic field during an experiment;

FIG. 7 is a schematic diagram of a visual result of the attitude of the IMU device after the IMU device approaches the high magnetic field during the experiment;

FIG. 8 is a schematic structural diagram of an attitude resolver based on a nine-axis IMU according to an embodiment of the present disclosure;

fig. 9 is a schematic structural diagram of an electronic device according to an embodiment of the present application.

Detailed Description

In order to make the objects, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all the embodiments. The components of the embodiments of the present application, generally described and illustrated in the figures herein, can be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, presented in the accompanying drawings, is not intended to limit the scope of the claimed application, but is merely representative of selected embodiments of the application. Every other embodiment that can be obtained by a person skilled in the art without making creative efforts based on the embodiments of the present application falls within the protection scope of the present application.

The IMU is a common positioning sensing device, and in general, an IMU device is fixed on a certain rigid body, and final attitude data of the IMU (or the rigid body) is calculated and output in real time by using IMU original data output by the IMU device and combining an attitude calculation method for navigation positioning calculation. The attitude here represents only three-dimensional rotation amount information in the space, and does not include translation amount information, and the attitude in the navigation field is named as: pitch angle, yaw angle, and roll angle.

The nine-axis IMU comprises a three-axis gyroscope (Gyro) module, a three-axis accelerometer (Acc) module and a three-axis magnetometer (Mag) module, and the three internal modules can stably output three-axis acceleration data Ax, Ay and Az, three-axis angular velocity data Gx, Gy and Gz and three-axis magnetic field intensity data Mx, My and Mz which are recorded as IMU original data relative to respective coordinate systems (a three-axis gyroscope coordinate system, a three-axis accelerometer coordinate system and a three-axis magnetometer coordinate system).

In the IMU attitude calculation method under the normal condition, IMU original data is used as input quantity, tasks such as IMU internal reference calibration, IMU attitude calculation, IMU attitude correction and the like are sequentially completed, and finally IMU attitude data are stably output in real time. In practice, there is a fixed offset of the three coordinate systems. In the internal reference calibration algorithm, after internal reference calibration is completed by using IMU original data, three coordinate systems can be unified into one coordinate system by using calibration parameters, and the coordinate system is marked as an IMU coordinate system. Three axes of an IMU coordinate system are vertical, and the direction of the coordinate system is determined by the gravity direction and the stable geomagnetic north direction in a calibration environment.

The existing nine-axis IMU attitude calculation method is mainly a data fusion attitude calculation method realized based on algorithms such as Mahony complementary filtering, Madgwick algorithm, extended Kalman filtering and the like. Although the methods are different, the basic ideas are all consistent: on one hand, the initial attitude of the IMU is obtained by performing accumulated calculation on the original angular velocity data of the gyroscope, and on the other hand, the accumulated error in the IMU attitude calculation is periodically or aperiodically eliminated through the earth gravity direction and the earth magnetic field direction, so that the correction is completed. However, these nine-axis IMU attitude calculation methods only aim at an external environment with a stable magnetic field, and although they also have a certain magnetic interference resistance, when facing a complex unstable magnetic field environment, the calculated IMU attitude data is difficult to satisfy, and as a result, they have no reliability, and thus they cannot be practically applied.

For example, in an indoor environment, a magnet is placed near the IMU device, the IMU device is powered on and the magnet is removed, and the IMU attitude data solved using these algorithms yields significantly erroneous results. The reason for the analysis is that the attitude solution methods disclosed above lack a targeted design for the application of the complex magnetic field link, and have no characteristics that can directly distinguish the geomagnetic field from various disturbances. Even if the current magnetic field strength is the same as the geomagnetic field strength, the direction of the magnetic field is not consistent with the direction of the geomagnetic field. The earth magnetic field direction cannot be determined and the IMU attitude correction algorithm based thereon fails, resulting in errors.

Besides, the existing nine-axis IMU attitude calculation method is not suitable for indoor application scenarios. First, the strength of the earth's magnetic field is weak, only less than 60 μ T. In an indoor magnetic field environment, the magnetic field strength near the interference source is much stronger than the earth magnetic field strength. For example, near a notebook, the value can reach 3000 μ T, 500 μ T desktop, 1000 μ T mobile phone, 4000 μ T wireless transmitting device; secondly, the direction vectors of the indoor magnetic field are different everywhere; the magnetic field of the indoor environment is usually formed by superposition of a plurality of interference source effects, and the magnetic field in the indoor environment is abnormally complicated by the magnetizing body and the object with the shielding effect. Therefore, the existing nine-axis IMU attitude calculation method can be only applied to outdoor navigation scenes more and is not suitable for indoor application scenes.

Based on the above, the embodiment of the application provides an attitude calculation method based on a nine-axis IMU, belongs to the field of navigation positioning, can be suitable for a complex magnetic field environment, has a certain anti-magnetic interference capability, and can solve the application problem of indoor navigation. The embodiment of the application defines a magnetic field vector-weighted value set, designs a magnetic force and attitude data correction algorithm based on a long period and a short period (corresponding period of attitude correction time and corresponding period of magnetic correction time), can ensure that reliable and accurate stable magnetometer calibration data are provided, realizes the fusion and mutual correction of magnetometer calibrated data and IMU initial attitude data, inhibits the drift of a yaw angle, ensures stable and accurate final attitude data output of an IMU device, and fully meets the application requirements of a complex magnetic field environment (especially an indoor environment).

Referring to fig. 1, fig. 1 is a flowchart of an attitude solution method based on a nine-axis IMU according to an embodiment of the present disclosure.

First, the technical concept of the present application is introduced: the initial attitude data of the IMU device includes the suppression of two other axial drift errors except the yaw angle, but has no suppression correction effect on the yaw angle axial drift error. Furthermore, the embodiment of the application corrects the yaw angle axial deviation of the attitude of the IMU device by means of magnetometer calibration data.

However, in the earth surface environment, especially in the indoor environment, metal objects generally exist, and under the action of the magnetization effect and the shielding effect, the indoor magnetic field environment is extremely complex, the indoor magnetic interference phenomenon is very serious, and the magnetic interference is caused by the magnetic field or the magnetic field change which is distributed unevenly in the space. Therefore, the magnetometer calibration data output by the working magnetometer is not stable magnetic field data, and contains complex and serious magnetic interference, which can easily cause the failure of the calibration work.

To address this problem, we statistically process magnetometer calibration data: and extracting stable magnetic field vector data in the magnetometer calibration data, effectively and accurately correcting the device attitude by means of the stable magnetic field vector data, and outputting final attitude data.

Specifically, the method comprises the following steps: the embodiment of the application defines a magnetic field vector-weighted value set, adopts magnetometer calibration data validity detection, designs a long and short correction period, corrects magnetic force and attitude data based on the long and short periods, can ensure that reliable and accurate stable magnetometer calibration data are provided, realizes fusion and correction of magnetometer calibrated data and IMU initial attitude data, inhibits drift of a yaw angle, ensures stable and accurate IMU final attitude data output, and fully meets application requirements of a complex magnetic field environment (especially an indoor environment).

Specifically, as shown in fig. 1, an attitude solution method provided in an embodiment of the present application includes:

and S110, respectively obtaining calibrated gyroscope calibration data, accelerometer calibration data and magnetometer calibration data based on the obtained original data of the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU.

In step S110, raw data of the gyroscope, the accelerometer, and the magnetometer of the nine-axis IMU is obtained, and then processed based on the obtained raw data to obtain calibrated data, that is, calibration data of the gyroscope, calibration data of the accelerometer, and calibration data of the magnetometer.

Firstly, calibrating internal parameters of three internal modules by using raw data output by a gyroscope, an accelerometer and a magnetometer of a nine-axis IMU in an internal reference calibration algorithm, and converting the raw data into calibration data by virtue of the calibrated internal parameters.

In particular, each of the three internal modules of the gyroscope, accelerometer and magnetometer in the IMU has an internal reference model of the form:

wherein x is_I，y_I，z_IThe data is the original data of the current module; x is the number of_O，y_O，z_OThe calibrated data of the current module is obtained; b is_x，B_y，B_zZero offset for the current module; s_x，S_y，S_zIs the scale factor of the current module; k is a radical of_ij(i = x, y, z; j = x, y, z; i ≠ j) is the installation error coefficient for the current module. The zero offset, the scale factor and the installation error coefficient of each module in the gyroscope, the accelerometer and the magnetometer are all internal parameters to be calibrated.

Referring to fig. 2, fig. 2 is a flowchart of a method for sequentially executing internal reference calibration by a gyroscope and an accelerometer according to an embodiment of the present application, and the specific process at the internal reference calibration stage is as follows: and calibrating the internal reference of the Acc module, continuously acquiring a plurality of frame data of the Acc module by virtue of a rotary table, and calibrating the internal reference of the module by using a six-position discrete calibration method. And calibrating the internal parameters of the Gyro module, continuously acquiring a plurality of frame data of Acc and Gyro by means of a turntable, and calibrating the internal parameters of the module by using a discrete calibration method.

In addition, internal reference calibration of the Mag module is carried out, an IMU device is manually swung, a plurality of frame data of the Mag are continuously collected, internal reference of the module is calibrated by using a manual splay winding method, in a normal operation stage, an internal reference result calibrated in the internal reference calibration stage is adopted, the original IMU data is calculated and processed by combining the formula, and data after IMU calibration are output.

And S120, performing data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data.

In step S120, a data fusion attitude solution method is usually implemented based on data fusion algorithms such as Mahony complementary filtering, Madgwick algorithm, extended kalman filtering, and the like.

Specifically, data fusion is carried out on gyroscope calibration data and accelerometer calibration data by using a data fusion algorithm to complete attitude primary calculation, primary attitude data of the IMU is output, and the essence of the primary attitude data is that attitude data is obtained by using gyroscope accumulation calculation, and attitude data deviation is irregularly corrected by means of gravity vectors.

Furthermore, accumulated calculation is carried out on the gyroscope calibration data, the attitude of the IMU device at each moment can be directly obtained, but the IMU device attitude obtained through calculation is influenced by an accumulated error effect, and larger drift errors can be generated along with the time lapse. The essence of the fusion algorithm is that the gravity vector of the accelerometer in a static state is used for partially correcting the attitude of the IMU device, restraining the drift errors of the other two axial directions except the yaw angle, and outputting the initial attitude data of the IMU device. In the method, data fusion is performed on gyroscope calibration data and accelerometer calibration data by using a data fusion algorithm (such as a Mahony complementary filter algorithm) such as a complementary filter algorithm, so that attitude preliminary solution is completed, and IMU preliminary attitude quaternion data is output. Wherein the suppression of two axial drift errors other than yaw angle is done by means of the gravity vector of the accelerometer at rest time.

Referring to fig. 3, fig. 3 is a flowchart of a six-axis fusion calculation method based on a complementary filtering algorithm according to an embodiment of the present application, as shown in fig. 3: firstly, judging whether the IMU is in a static state by using Acc calibration data, when the IMU is in the static state, converting the Acc calibration data from an IMU coordinate system corresponding to the current sampling time to an IMU initial coordinate system, obtaining a gravity vector at the current sampling time in the IMU initial coordinate system through parameter conversion calculation, calculating and obtaining an error between the gravity vector and a preset initial gravity vector to obtain an error vector, and executing a complementary filtering algorithm by using the error vector and quaternion data after the Gyro calibration at the time; when the IMU is in a non-static state, the complementary filtering algorithm is executed by directly utilizing quaternion data after the Gyro calibration; and finally, outputting IMU initial attitude data by a complementary filtering algorithm.

S130, determining a magnetic field vector-weight value set representing the time statistics and the space statistics of the magnetic field.

Here, the time statistic means: analyzing all magnetic field vectors in the nine-axis IMU motion time, and judging whether the magnetic field vectors are added into the magnetic field vector-weight value set or not; the spatial statistics refer to: and comparing all the magnetic field vectors added into the set in the space direction, and recording the occurrence times of the magnetic field vectors in different directions in the form of different weight values.

In step S130, a magnetic field vector-weight value set is defined, and the analysis of the magnetic field vector on the time scale interfered by external factors is converted into a set operation for maintaining a statistic, and the optimal magnetic field vector is the magnetic field vector with the largest corresponding weight value and is the most stable magnetic field vector in the current test environment, so that the reliability and accuracy of the magnetic field vector are ensured in the statistical sense, and the global stability and the magnetic interference resistance of the IMU attitude calculation data are ensured.

And S140, converting the accelerometer calibration data and the magnetometer calibration data into an acceleration vector and a magnetic field vector for vector description.

Here, the accelerometer calibration data and the acceleration vector are different descriptions of the same object, and similarly, the magnetometer calibration data and the magnetic field vector are also different descriptions of the same object.

Specifically, the accelerometer calibration data is a set of triaxial specific acceleration value data; from the viewpoint of mathematical vectors, the data in the above set can be understood as a component vector of a total vector in three directions, and the total vector is an acceleration vector.

It should be understood that the accelerometer calibration data referred to below is equivalent to an acceleration vector and the magnetometer calibration data is equivalent to a magnetic field vector.

S150, when the preset magnetic force correction moment is reached, correcting the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction moment to obtain a corrected magnetic field vector-weight value set.

In step S150, the magnetic force correction time corresponding period is a short correction period, and the magnetic force correction time is a certain time of the magnetic force correction time corresponding period. Here, a correction operation is periodically performed, the correction target being to update the set of magnetic field vector-weight values by means of the preliminary attitude quaternion and the current magnetic field vector of the IMU device.

Specifically, comparing a magnetic field vector in an IMU coordinate system with a magnetic field vector corresponding to a maximum weight value in a magnetic field vector-weight value set to obtain an angle difference between a magnetic field direction detected by the magnetometer and a stable magnetic field direction at the current magnetic force correction time; if the angle difference value is within the preset angle difference range, adding the magnetic field vector under the IMU coordinate system at the current magnetic force correction moment into the magnetic field vector-weight value set; and if the angle difference value is not within the preset angle difference range, not adding the current magnetic force correction moment IMU coordinate system into the magnetic field vector-weight value set.

And S160, when a preset posture correction time is reached, correcting the preliminary posture data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present to obtain final posture data, wherein the corresponding period of the posture correction time is integral multiple of the corresponding period of the magnetic force correction time.

In step S160, the period corresponding to the attitude correction time is a long correction period, where a correction operation is periodically performed, and the correction target is to update the attitude quaternion of the IMU device by means of the magnetic field vector-weight value set, thereby completing the correction of the preliminary attitude data.

Here, the attitude quaternion of the nine-axis IMU is updated based on the magnetic field vector corresponding to the largest weight value and the gravity vector determined by the accelerometer at the current attitude correction time, and the updated attitude quaternion of the nine-axis IMU is determined as final attitude data.

It should be added that the magnetic field vector-weight value set records several magnetic field directions with the highest probability in all historical moments, and finally, when the attitude is corrected, the magnetic field vector with the highest probability (with the highest weight value) is selected, that is, the magnetic field direction and the magnetic field vector which are most stable for a long time in the current environment are selected, so that the long-term stable correction effect on the preliminary attitude data can be ensured.

Specifically, referring to fig. 4, fig. 4 is an organization structure block diagram of a posture calculation method based on a nine-axis IMU according to an embodiment of the present application, and as shown in fig. 4, an overall flow of the posture calculation method based on the nine-axis IMU is as follows: firstly, calibrating internal parameters of three internal modules by utilizing original data output by a gyroscope, an accelerometer and a magnetometer of a nine-axis IMU in an internal reference calibration algorithm, and converting the original data into calibration data by virtue of the calibrated internal parameters; then in a six-axis fusion calculation algorithm, aiming at the calibration data of the gyroscope and the calibration data of the accelerometer, performing data fusion by using a data fusion algorithm to complete the primary attitude calculation, outputting the primary attitude data of the IMU, wherein the primary attitude data is obtained by using the accumulative calculation of the gyroscope, and the attitude data deviation is irregularly corrected by means of the gravity vector; and finally, in a magnetic force and attitude data correction algorithm based on a long period and a short period (a period corresponding to the attitude correction time and a period corresponding to the magnetic correction time), correcting the drift of the gyroscope by utilizing a gravity vector and an optimal magnetic field vector (a magnetic field vector with the highest weight value), and outputting the final attitude data of the IMU.

Therefore, the IMU attitude is corrected through mutual correction of the magnetic force and the attitude data with the long and short periods in the embodiment of the application, the adverse effect of magnetic field interference on attitude calculation can be inhibited, the IMU attitude is accurately and stably output, and the application requirement of the IMU device in an indoor complex magnetic field environment is met.

In the embodiment of the present application, as a preferred embodiment, the step S120 includes:

calculating candidate preliminary attitude data at the current sampling moment according to the preliminary attitude data calculated at the last sampling moment and the acquired gyroscope calibration data at the current sampling moment; if the nine-axis IMU is in a non-static state, updating the preliminary attitude data based on the candidate preliminary attitude data to obtain updated preliminary attitude data; if the nine-axis IMU is in a static state, determining a gravity vector at the current sampling moment in an IMU initial coordinate system according to accelerometer calibration data at the current sampling moment; determining an error vector between the gravity vector at the current sampling moment and an initial gravity vector according to the initial gravity vector predetermined under the IMU initial coordinate system; calculating to obtain fused preliminary attitude data by adopting a data fusion algorithm according to the candidate preliminary attitude data and the error vector; updating the preliminary attitude data based on the fused preliminary attitude data to obtain updated preliminary attitude data; the IMU initial coordinate system is a coordinate system determined by the initial time of the nine-axis IMU after the gyroscope, the accelerometer and the magnetometer are calibrated.

Here, the preliminary pose data calculated at the last sampling time is known pose data, and the embodiment of the present application is directly used.

Step S120 is to perform data fusion based on the complementary filtering algorithm, so as to obtain the initial pose data of the IMU. The specific process can be seen in fig. 3.

In the embodiment of the present application, as a preferred embodiment, step S130 determines the magnetic field vector-weight value set by:

obtaining a plurality of initial set elements based on each initial magnetic field vector and the weight value corresponding to the initial magnetic field vector; when the difference value of the newly added magnetic field vector and any one initial magnetic field vector is within a preset conical range, determining the accumulated weight value of the initial magnetic field vector corresponding to the newly added magnetic field vector; when the difference value of the newly added magnetic field vector and any one initial magnetic field vector is not in the preset conical range, determining the newly added magnetic field vector and the weight value corresponding to the newly added magnetic field vector as a newly added set element; and updating the magnetic field vector-weight value set based on the accumulated weight value of the initial magnetic field vector corresponding to the newly added magnetic field vector or the newly added set element.

Here is the add operation of the collection element: when the effective magnetic field vector is required

When added to the set P, first, all set elements and magnetic field vectors in the set P are traversed

Comparing; if the magnetic field vector is found

With magnetic field vector

When the difference is within a preset cone range (minimum cone setting range), the magnetic field vector is adjusted

With magnetic field vector

Is expressed as the same magnetic field vector

Corresponding weight value k of_iAdding 1 in an accumulated way; if any magnetic field vector meeting the difference requirement is not searched, adding an element in the corresponding set P

。

Specifically, step S130 determines the set of magnetic field vector-weight values by:

Here is a delete operation of a collection element: when detecting that the number of the set elements in the set P exceeds a set number threshold N_PmaxAnd then sorting the set elements in the set P according to the weight values, and selecting a target number of set elements with smaller weight values from the set P to delete from the set elements with the lowest weight values, wherein the target number is the number difference between the element numbers of all the set elements and a set number threshold.

Specifically, step S130 determines the set of magnetic field vector-weight values by the following formula:

wherein P represents a set of magnetic field vector-weight values,

denotes the ith magnetic field vector, k_iRepresents the weight value corresponding to the ith magnetic field vector, N represents the number of all the magnetic field vectors in the magnetic field vector-weight value set,

represents the magnetometer calibration data at time t,

The IMU coordinate system is a coordinate system determined by calibrating the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU, and the IMU coordinate system at the time t is a coordinate system determined by calibrating the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU at the time t.

Here, a set of magnetic field vectors-weight values is defined: define set P as the magnetic field vector-weight value set:

wherein,

denotes the ith magnetic field vector, k_iIs the weight value corresponding to the ith magnetic field vector, N represents the number of all the magnetic field vectors in the magnetic field vector-weight value set,

the data can be calibrated by the magnetometer at the time t in the process of set maintenance

Combining quaternions corresponding to the preliminary attitude data under the IMU coordinate system at the corresponding t moment

And calculating to obtain the description of the ith magnetic field vector under an IMU coordinate system:

wherein the magnetic field vector with the largest weight value in the set P is recorded as

。

Therefore, a magnetic field vector-weight value set is defined, analysis of a magnetic field vector on a time scale interfered by external factors is converted into maintenance of a set operation with statistics, and the optimal magnetic field vector is the magnetic field vector with the maximum corresponding weight value, so that the reliability and accuracy of the magnetic field vector are ensured in a statistical sense, and the global stability and the magnetic interference resistance of IMU attitude calculation data are ensured.

It is to be added that the embodiment of the present application performs the magnetic field vector correction operation and the preliminary attitude data correction operation only when the correction condition is satisfied. Furthermore, before performing the correction operation, it is necessary to determine whether the IMU attitude satisfies the correction condition, please refer to fig. 5.

Specifically, the IMU state is detected by means of accelerometer calibration data, and when the IMU state is determined to be a static state by comparison with the gravity vector, the correction condition is said to be satisfied.

Here, there is a static initial gravity vector (a vector of 3 times 1) under the initial coordinate system of the IMU, the accelerometer has three axial acceleration values, the acceleration values are converted under the initial coordinate system of the IMU to obtain a gravity vector at the current sampling time under the initial coordinate system of the IMU, the gravity vector at the current sampling time is compared with the initial gravity vector under the initial coordinate system of the IMU, and when an error vector between two gravity vectors is within an error allowable range, it is determined that the nine-axis IMU at this time is in a static state.

As shown in fig. 5, before the magnetic field vector correction operation and the attitude data correction operation are performed, validity detection of the magnetometer calibration data is required. Is arranged at

At any moment, the quaternion corresponding to the IMU preliminary attitude data maintained by the attitude calculation algorithm is

. Suppose that

The moment satisfies the correction condition, and the calibration data of the gyroscope is

Magnetometer calibration data of

The accelerometer calibration data is

. First, combine

Judging that the correction condition needs to be met; then, calibrating the magnetometer

Combined with attitude quaternion at corresponding time

Calculated in IMUDescription of the magnetic field vector in the initial coordinate system:

. Then, will

Magnetic field vector element with largest weight value in sum set P

And comparing to obtain the angle difference. If the angle difference is within a set range, judging the magnetic field vector at the current time t

If not, judging the magnetic field vector at the current time t

Is invalid data. Furthermore, the validity detection of the magnetometer calibration data can effectively judge whether the current magnetometer calibration data is magnetic interference data, so that the magnetic interference data is preliminarily eliminated.

In the embodiment of the present application, as a preferred embodiment, the step S150 includes:

determining a magnetic field vector corresponding to the largest weight value in the magnetic field vector-weight value set when the nine-axis IMU is in a static state; determining a magnetic field vector under an IMU coordinate system based on the magnetic field vector at the current magnetic force correction moment and the attitude quaternion of the gyroscope in the preliminary attitude data; comparing the magnetic field vector under the IMU coordinate system with the magnetic field vector corresponding to the maximum weight value in the magnetic field vector-weight value set to obtain an angle difference value between the magnetic field direction detected by the magnetometer and the stable magnetic field direction at the current magnetic force correction moment; if the angle difference value is within a preset angle difference range, adding a magnetic field vector under an IMU coordinate system at the current magnetic force correction moment into the magnetic field vector-weight value set; and if the angle difference value is not within the preset angle difference range, not adding the magnetic field vector under the IMU coordinate system at the current magnetic force correction moment into the magnetic field vector-weight value set.

Specifically, as shown in fig. 5, this step is mainly a magnetic field vector correction operation. When the preset magnetic force correction moment is reached, firstly detecting whether the IMU meets the correction condition, if not, continuing the execution, and if so, continuing the execution; magnetometer calibration data for current frame

Carrying out validity detection on the magnetometer calibration data, if the magnetometer calibration data are detected to be invalid, not continuing to execute, and if the magnetometer calibration data are valid, continuing to execute; correcting the current magnetic field vector-weight value set P, firstly calibrating the magnetometer calibration data of the current frame

Corresponding magnetic field vector

Adding to the set P, and then performing a set element deleting operation to delete the magnetic field vector with the lowest weight value in the set if necessary.

In this way, by periodically performing the magnetic field vector correction operation, the magnetic field vector-weight value set P can be effectively maintained, and the stability of the recorded magnetic field data from beginning to end can be ensured.

In the embodiment of the present application, as a preferred embodiment, step S160 includes:

determining a magnetic field vector corresponding to the largest weight value in the magnetic field vector-weight value set when the nine-axis IMU is in a static state; updating the attitude quaternion of the nine-axis IMU based on the magnetic field vector corresponding to the maximum weight value and the acceleration vector determined by the accelerometer at the current attitude correction moment; and determining the updated attitude quaternion of the nine-axis IMU as final attitude data.

Specifically, as shown in fig. 5, this step is mainly an attitude data correction operation. When reaching the preset posture correction moment, firstly detecting whether the IMU posture meets the correction requirementIf the positive condition is not met, the execution is not continued, and if the positive condition is met, the execution is continued; selecting the magnetic field vector with the largest weight value from the current set of magnetic field vector-weight values P, i.e. selecting the magnetic field vector with the largest weight value

(ii) a By using

The determined steady magnetic field vector and the current static state are calibrated by the accelerometer

The determined acceleration vector, i.e. gravity vector, constructs a new attitude to obtain a corresponding attitude quaternion of

(ii) a Using calculated attitude quaternion

Replacement of attitude quaternion for currently maintained IMU devices

Namely, the process of correcting the attitude by means of the magnetic field vector is completed.

As described above, by periodically executing the attitude data correction operation, the deviation of the device attitude affected by the accumulated error effect in the yaw angle axial direction is corrected, the utilization of the magnetic field vector-weight value set ensures the stable and reliable source of the magnetic field vector, further ensures the effectiveness and accuracy of the operation of periodically correcting the device attitude by means of the magnetic field vector, and enhances the global stability and the magnetic interference resistance of the IMU attitude calculation data.

Further, the maintained attitude data after the above-mentioned multi-step correction process

I.e. the final output node of the attitude calculation algorithmAnd obtaining the final posture data.

The following examples are given in the experiments:

before the nine-axis IMU device is electrified, a strong magnetic field does not exist near the initial position; after the nine-axis IMU device is operated, the device is brought near a magnet (strong magnetic field) for a short time of movement, and then the nine-axis IMU device is released back to the initial position. The experimental results are as follows: when the IMU device is static before moving, quaternion of IMU attitude data is expressed as (0.7281, -0.0042, -0.06854, 0.0038); when the IMU device moves close to the magnetic field and then is placed at the home position, quaternion of IMU attitude data is expressed as (0.7223, -0.0038, -0.6906, -0.0350), specifically, as shown in FIG. 6 and FIG. 7, FIG. 6 is a schematic diagram of a visual diagram result of the attitude of the IMU device before the IMU device is close to the strong magnetic field in an experiment, and FIG. 7 is a schematic diagram of a visual diagram result of the attitude of the IMU device after the IMU device is close to the strong magnetic field in the experiment; according to the strong magnetic interference resistance test result graph of the moving IMU in the experiment, the front and back attitude change of the IMU device is small and is in an allowable small range. Experiments show that the method is suitable for solving the nine-axis IMU attitude in the complex magnetic field environment, can effectively inhibit the magnetic field interference influence of the type, and stably outputs the accurate pose of the nine-axis IMU.

The specific process is as follows: calibrating internal modules of the nine-axis IMU device respectively: a gyroscope, an accelerometer, and internal references to a magnetometer, wherein: calibrating the internal reference of the accelerometer internal reference model by using a six-position discrete calibration method through a turntable; directly calibrating the internal reference of the gyroscope internal reference model by using a turntable and a discrete calibration method; and calibrating the internal reference of the internal reference model of the magnetometer by using a manual eight-character winding method. The calibration method is not limited, and the calibration can be completed by using a universal mature algorithm.

Specifically, in a six-axis fusion calculation method based on a complementary filtering algorithm, a Mahony complementary filtering algorithm is used, an accelerometer is used for correcting a gyroscope, and two axial drifts except a yaw angle are restrained; standing the product, testing the drift velocity of the gyroscope in the yaw angle direction, and setting the long and short correction periods of magnetic force and attitude data correction according to the drift velocity of the gyroscope and the product performance requirements; when the preset magnetic force correction moment is reached, correcting the magnetic field vector-weight value set based on the preliminary attitude data and a magnetic field vector (mathematical vector description form of magnetometer calibration data) obtained at the current magnetic correction moment, and deleting the magnetic field vector with the lowest weight value in the set if necessary; and when the preset attitude correction moment is reached, selecting a magnetic field vector with the highest weight value based on the magnetic field vector-weight value set, carrying out yaw angle axial offset correction on the initial attitude data to obtain corrected final attitude data, and outputting the final attitude data.

According to the attitude calculation method based on the nine-axis IMU, calibrated gyroscope calibration data, accelerometer calibration data and magnetometer calibration data are obtained respectively based on the obtained original data of the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU; then, performing data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data; predetermining a magnetic field vector-weight value set representing magnetic field time statistics and space statistics; and converting the accelerometer calibration data and the magnetometer calibration data into an acceleration vector and a magnetic field vector for vector description; further, when a preset magnetic force correction moment is reached, correcting the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction moment to obtain a corrected magnetic field vector-weight value set; and when a preset attitude correction moment is reached, correcting the preliminary attitude data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present to obtain final attitude data, wherein the corresponding period of the attitude correction moment is integral multiple of the corresponding period of the magnetic force correction moment. Therefore, the IMU attitude can be corrected through the magnetic force and the attitude data with the long and short periods, the adverse effect of magnetic field interference on attitude calculation is restrained, the IMU attitude is accurately and stably output, and the application requirement of the IMU device in an indoor complex magnetic field environment is met.

Based on the same inventive concept, the embodiment of the present application further provides a posture calculation device based on a nine-axis IMU corresponding to the posture calculation method based on a nine-axis IMU, and as the principle of solving the problem of the device in the embodiment of the present application is similar to that of the method in the embodiment of the present application, the implementation of the device can refer to the implementation of the method, and repeated details are omitted.

Referring to fig. 8, fig. 8 is a schematic structural diagram of an attitude calculation apparatus based on a nine-axis IMU according to an embodiment of the present disclosure. As shown in fig. 8, the attitude resolver 800 includes:

the internal reference calibration module 810 is configured to obtain calibrated gyroscope calibration data, accelerometer calibration data and magnetometer calibration data based on the obtained raw data of the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU;

the data processing module 820 is configured to perform data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data;

a set determination module 830 for determining a set of magnetic field vector-weight values characterizing the magnetic field temporal statistics and the spatial statistics;

a data conversion module 840, configured to convert the accelerometer calibration data and the magnetometer calibration data into an acceleration vector and a magnetic field vector for vector description;

a first correction module 850, configured to correct the magnetic field vector-weight value set according to the preliminary attitude data and a magnetic field vector obtained at a current magnetic force correction time when a preset magnetic force correction time is reached, so as to obtain a corrected magnetic field vector-weight value set;

and a second correction module 860, configured to correct the preliminary attitude data according to a magnetic field vector with a highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer currently in a stationary state when a preset attitude correction time is reached, so as to obtain final attitude data, where a period corresponding to the attitude correction time is an integral multiple of a period corresponding to the magnetic force correction time.

In this embodiment, as a preferred embodiment, when the data processing module 820 is configured to perform data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data, the data processing module 820 is specifically configured to:

In the embodiment of the present application, as a preferred embodiment, the set determining module 830 is configured to determine the magnetic field vector-weight value set by:

In the embodiment of the present application, as a preferred embodiment, the set determining module 830 is configured to determine the set of magnetic field vectors and weight values by the following formula:

wherein P represents a set of magnetic field vector-weight values,

represents the magnetometer calibration data at time t,

In this embodiment, as a preferred embodiment, when the first correcting module 850 is configured to correct the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction time when a preset magnetic force correction time is reached, the first correcting module 850 is specifically configured to:

In this embodiment, as a preferred embodiment, when the second correction module 860 is configured to correct the preliminary attitude data according to a magnetic field vector with a highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer currently in a stationary state when a preset attitude correction time is reached, to obtain final attitude data, the second correction module 860 is specifically configured to:

The attitude calculation device based on the nine-axis IMU comprises an internal reference calibration module, a data processing module, a set determination module, a data conversion module, a first correction module and a second correction module, wherein the internal reference calibration module is used for respectively obtaining calibrated gyroscope calibration data, accelerometer calibration data and magnetometer calibration data based on the obtained original data of the gyroscope, the accelerometer and the magnetometer of the nine-axis IMU; the data processing module is used for carrying out data fusion on the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data; the set determination module is used for determining a magnetic field vector-weight value set representing the time statistics and the space statistics of the magnetic field; the data conversion module is used for converting the accelerometer calibration data and the magnetometer calibration data into an acceleration vector and a magnetic field vector for vector description; the first correction module is used for correcting the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction moment when the preset magnetic force correction moment is reached, so as to obtain a corrected magnetic field vector-weight value set; and the second correction module is used for correcting the preliminary attitude data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present when a preset attitude correction moment is reached, so as to obtain final attitude data, wherein the corresponding period of the attitude correction moment is integral multiple of the corresponding period of the magnetic force correction moment. Therefore, the IMU attitude can be corrected through the magnetic force and the attitude data with the long and short periods, the adverse effect of magnetic field interference on attitude calculation is restrained, the IMU attitude is accurately and stably output, and the application requirement of the IMU device in an indoor complex magnetic field environment is met.

Referring to fig. 9, fig. 9 is a schematic structural diagram of an electronic device according to an embodiment of the present disclosure. As shown in fig. 9, the electronic device 900 includes a processor 910, a memory 920, and a bus 930.

The memory 920 stores machine-readable instructions executable by the processor 910, when the electronic device 900 operates, the processor 910 communicates with the memory 920 through the bus 930, and when the machine-readable instructions are executed by the processor 910, the method for calculating an attitude based on a nine-axis IMU shown in fig. 1 may be executed, the method for calibrating an internal reference by a gyroscope and an accelerometer shown in fig. 2 may be executed in sequence, the method for calculating a six-axis fusion based on a complementary filtering algorithm shown in fig. 3, the method for calculating an attitude based on a nine-axis IMU shown in fig. 4, and the method for correcting magnetic force and attitude data based on a long period and a short period shown in fig. 5 may be implemented in a method embodiment, which is not described herein again.

An embodiment of the present application further provides a computer-readable storage medium, where a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the method for calculating an attitude based on a nine-axis IMU shown in fig. 1 may be executed, the method for calibrating an internal reference by a gyroscope and an accelerometer shown in fig. 2 may be executed in sequence, the method for calculating a six-axis fusion based on a complementary filtering algorithm shown in fig. 3, the method for calculating an attitude based on a nine-axis IMU shown in fig. 4, and the method for correcting magnetic force and attitude data based on a long-short period shown in fig. 5 may be implemented in a specific manner, which is described in the method embodiment and is not described herein again.

It is clear to those skilled in the art that, for convenience and brevity of description, the specific working processes of the above-described systems, apparatuses and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.

In the several embodiments provided in the present application, it should be understood that the disclosed system, apparatus and method may be implemented in other ways. The above-described embodiments of the apparatus are merely illustrative, and for example, the division of the units is only one logical division, and there may be other divisions when actually implemented, and for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of devices or units through some communication interfaces, and may be in an electrical, mechanical or other form.

The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.

In addition, functional units in the embodiments of the present application 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 functions, if implemented in the form of software functional units and sold or used as a stand-alone product, may be stored in a non-volatile computer-readable storage medium executable by a processor. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: various media capable of storing program codes, such as a usb disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk.

Finally, it should be noted that: the above-mentioned embodiments are only specific embodiments of the present application, and are used for illustrating the technical solutions of the present application, but not limiting the same, and the scope of the present application is not limited thereto, and although the present application is described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: any person skilled in the art can modify or easily conceive the technical solutions described in the foregoing embodiments or equivalent substitutes for some technical features within the technical scope disclosed in the present application; such modifications, changes or substitutions do not depart from the spirit and scope of the exemplary embodiments of the present application, and are intended to be covered by the scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.

Claims

1. An attitude calculation method based on a nine-axis IMU (inertial measurement Unit), which is characterized by comprising the following steps:

wherein the set of magnetic field vector-weight values is determined by:

updating the magnetic field vector-weight value set based on the accumulated weight value of the initial magnetic field vector corresponding to the newly added magnetic field vector or the newly added set element;

when a preset magnetic force correction moment is reached, correcting the magnetic field vector-weight value set according to the preliminary attitude data and a magnetic field vector obtained at the current magnetic force correction moment to obtain a corrected magnetic field vector-weight value set, including:

if the angle difference value is not within the preset angle difference range, not adding the magnetic field vector under the IMU coordinate system at the current magnetic force correction moment into the magnetic field vector-weight value set;

when a preset attitude correction moment is reached, correcting the preliminary attitude data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present to obtain final attitude data, wherein the corresponding period of the attitude correction moment is an integral multiple of the corresponding period of the magnetic force correction moment;

when a preset attitude correction moment is reached, correcting the preliminary attitude data according to a magnetic field vector with a highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present to obtain final attitude data, wherein the method comprises the following steps of:

2. The attitude calculation method according to claim 1, wherein the data fusion of the obtained gyroscope calibration data and accelerometer calibration data to obtain preliminary attitude data comprises:

determining an error vector between a gravity vector at the current sampling moment and an initial gravity vector according to the initial gravity vector predetermined under the IMU initial coordinate system;

3. The attitude calculation method according to claim 1, wherein the set of magnetic field vector-weight values is determined by:

4. The attitude calculation method according to claim 3, wherein the set of magnetic field vector-weight values is determined by the following equation:

wherein P represents a set of magnetic field vector-weight values,

represents the ith magnetic field vector, ki represents the weight value corresponding to the ith magnetic field vector, N represents the number of all the magnetic field vectors in the magnetic field vector-weight value set,

；

；

represents the magnetometer calibration data at time t,

5. A nine-axis IMU based attitude solution, the attitude solution comprising:

wherein the set determination module is configured to determine the set of magnetic field vector-weight values by:

when the first correction module is used for correcting the magnetic field vector-weight value set according to the preliminary attitude data and the magnetic field vector obtained at the current magnetic force correction time when the preset magnetic force correction time is reached, the first correction module is used for:

the second correction module is used for correcting the preliminary attitude data according to a magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and an acceleration vector of the accelerometer in a static state at present when a preset attitude correction moment is reached, so as to obtain final attitude data, wherein the period corresponding to the attitude correction moment is an integral multiple of the period corresponding to the magnetic force correction moment;

when the second correction module is used for correcting the preliminary attitude data according to the magnetic field vector with the highest weight value in the corrected magnetic field vector-weight value set and the acceleration vector of the accelerometer in the static state at present when the preset attitude correction moment is reached, and the final attitude data is obtained, the second correction module is used for:

6. An electronic device, comprising: a processor, a memory and a bus, the memory storing machine-readable instructions executable by the processor, the processor and the memory communicating over the bus when the electronic device is operating, the processor executing the machine-readable instructions to perform the steps of the nine-axis IMU-based attitude solution method of any of claims 1-4.

7. A computer readable storage medium having stored thereon a computer program for performing, when executed by a processor, the steps of the nine-axis IMU-based attitude solution method according to any one of claims 1-4.