Method, device, equipment and storage medium for fusion of IMU and rigid body posture

Through the pose fusion method of IMU and rigid body, the measurement data of IMU and rigid body is processed using the fusion algorithm, which solves the problem of large pose data error during high-speed motion and improves the measurement accuracy.

CN113984051BActive Publication Date: 2024-11-12SHENZHEN REALIS MULTIMEDIA TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202111208097.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2020-04-30
Publication Date
2024-11-12
Estimated Expiration
2040-04-30

AI Technical Summary

Technical Problem

When measuring the high-speed motion of the rigid body, the position data of the IMU fused with the optical positioning device has a large error, resulting in the inability to obtain accurate rigid position data.

Method used

By determining the positional relationship between the IMU and the rigid body, the measured attitude data of the IMU and the rigid body are obtained, and the fusion algorithm is used to fuse the attitude data to be fused with the candidate attitude data of the rigid body to be candidate attitude data to obtain accurate fusion attitude data.

Benefits of technology

The error between the measured posture data and the real posture data is reduced, and the accuracy of the measured posture data is improved.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN113984051B_ABST
    Figure CN113984051B_ABST
Patent Text Reader

Abstract

The present invention relates to the field of measurement, and discloses a method, device, equipment and storage medium for fusing an IMU with a rigid body, which is used to solve the problem of large error between the measured posture data of the rigid body and the real posture data when measuring the posture data of the rigid body. The posture fusion method of the IMU and the rigid body includes: determining the positional relationship between the inertial measurement unit IMU and the rigid body; when the rigid body moves with the IMU, obtaining the measured posture data of the IMU, and calculating the posture data to be fused of the IMU through the measured posture data of the IMU: in a preset world coordinate system, obtaining the measured posture data of the rigid body, and determining the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body and the positional relationship between the IMU and the rigid body; using a fusion algorithm to fuse the posture data to be fused of the IMU with the candidate posture data of the rigid body, and obtaining the fused posture data of the rigid body.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of measurement, and in particular to a method, device, equipment and storage medium for fusing an IMU with a rigid body. Background Art

[0002] Inertial measurement unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Generally, an IMU is equipped with three-axis gyroscopes and three-directional accelerometers, which are used to measure the angular velocity and acceleration of a rigid body in three-dimensional space, and calculate the attitude of the rigid body. Therefore, the use of IMU is very wide, from aerospace to automobiles and consumer electronics. In addition, IMU has high-frequency data acquisition capabilities. For example, the acquisition frequency of the ADIS16 series of Analog Devices (ADI) can reach 200Hz, and some models can even reach 400Hz. In some application scenarios, it is necessary to measure the posture data of a rigid body during high-speed movement. At this time, the optical positioning device and IMU fusion method can be used.

[0003] However, in specific practical application scenarios, when a rigid body moves at high speed, if some camera devices fail to capture the position of the rigid body or only capture the offset position of the rigid body, the error between the measured posture data of the rigid body and the actual posture data will be too large, and accurate rigid body data cannot be obtained. Summary of the invention

[0004] The main purpose of the present invention is to solve the problem of large error between measured posture data of a rigid body and actual posture data when measuring the posture data of a rigid body.

[0005] The first aspect of the present invention provides a method for fusing an inertial measurement unit (IMU) with a rigid body, comprising: determining the positional relationship between an inertial measurement unit (IMU) and a rigid body; when the rigid body moves with the IMU, obtaining measurement attitude data of the IMU, and calculating the attitude data to be fused of the IMU through the measurement attitude data of the IMU; in a preset world coordinate system, obtaining the measurement attitude data of the rigid body, and determining candidate attitude data of the rigid body based on the measurement attitude data of the IMU, the measurement attitude data of the rigid body, and the positional relationship between the IMU and the rigid body; and using a fusion algorithm to fuse the attitude data to be fused of the IMU with the candidate attitude data of the rigid body to obtain fused attitude data of the rigid body.

[0006] Optionally, in a first implementation method of the first aspect of the present invention, when the rigid body carries the IMU for movement, the measurement attitude data of the IMU is obtained, and the attitude data to be fused of the IMU is calculated through the measurement attitude data of the IMU, including: obtaining standard attitude data of the IMU, the standard attitude data including standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise; when the rigid body carries the IMU for movement, the measurement attitude data of the IMU is obtained, and the attitude data to be fused is determined according to the measurement attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes acceleration to be fused and angular velocity to be fused.

[0007] Optionally, in a second implementation of the first aspect of the present invention, when the rigid body carries the IMU for movement, the measured attitude data of the IMU is obtained, and the attitude data to be fused is determined based on the measured attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes acceleration to be fused and angular velocity to be fused, including: obtaining the measured attitude data of the IMU when the rigid body carries the IMU for movement, wherein the measured attitude data of the IMU includes measured acceleration and measured angular velocity; calculating the acceleration to be fused according to a first preset formula, the standard attitude data and the measured attitude data of the IMU, wherein the first preset formula is: Among them, a t represents the measured acceleration, a t(real) represents the acceleration to be fused, represents the standard acceleration bias at time t, Indicates the gravity direction component of the IMU attitude at time t, n a is the standard acceleration noise; the angular velocity to be fused is calculated according to the second preset formula, the standard posture data and the measured posture data of the IMU, and the second preset formula is: Among them, w t Denotes the measured angular velocity, w t(real) represents the angular velocity to be fused, represents the standard angular velocity bias at time t, n w represents standard acceleration noise; the acceleration to be fused and the angular velocity to be fused are combined to obtain the attitude data to be fused.

[0008] Optionally, in a third implementation of the first aspect of the present invention, obtaining the measurement posture data of the rigid body in a preset world coordinate system, and determining the candidate posture data of the rigid body according to the measurement posture data of the IMU, the measurement posture data of the rigid body, and the positional relationship between the IMU and the rigid body includes: obtaining the measurement posture data of the rigid body and the measurement posture data of the IMU, extracting the 0th frame IMU measurement posture data and the nth frame IMU measurement posture data from the measurement posture data of the IMU, extracting the 0th frame rigid body to world measurement posture data and the nth frame rigid body to world measurement posture data from the measurement posture data of the rigid body; in the preset world coordinate system, calculating the 0th frame IMU to world measurement posture data and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates according to the relationship equation, wherein the relationship equation is:

[0009]

[0010] in, Represents the measured posture data of the rigid body to the world in the nth frame, Represents the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates, [R i ] n Indicates the nth frame of IMU measurement attitude data, Represents the measured posture data of the rigid body to the world at frame 0, [R i ]0 represents the IMU measurement attitude data of the 0th frame, Indicates the measured attitude data from IMU to the world in frame 0, represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame, and E is used to indicate the unit matrix; the attitude data of the IMU is converted into the attitude data of the rigid body according to the attitude conversion formula to obtain the candidate attitude data of the rigid body, and the attitude conversion formula is:

[0011]

[0012] in, Represents the candidate posture data of the rigid body after transformation, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame. Represents the inverse transformation of the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates.

[0013] Optionally, in a fourth implementation of the first aspect of the present invention, the use of a fusion algorithm to fuse the to-be-fused posture data of the IMU with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body includes: extracting the posture quaternion in the candidate posture data of the rigid body, and respectively obtaining the candidate position data and candidate displacement data of the rigid body, the posture quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates; calculating the fused position of the rigid body according to the position fusion formula in the fusion algorithm, the measured posture data of the IMU, and the to-be-fused posture data of the IMU, and the position fusion formula is:

[0014]

[0015] in, Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1, Indicates the candidate position of the rigid body in the preset world coordinate system at time k, α is the position fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, represents the candidate displacement of the rigid body at time k, Δt k represents the time interval, Δt = t k+1 -t k , a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, It represents the gravity direction component of the IMU attitude at time t, Indicates that when the IMU velocity and the acceleration to be fused are projected into the preset world coordinate system, [t k , t k+1 ] The displacement generated by the acceleration to be fused in the time period; the fusion posture of the rigid body is calculated according to the posture fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused. The posture fusion formula is:

[0016]

[0017] in, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k+1, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k, represents the multiplication symbol of quaternion, β represents the attitude fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, is the attitude quaternion of the IMU at time k, w t represents the measured angular velocity, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise, indicates [t k , t k+1 ] period; merging the fusion position of the rigid body with the fusion posture of the rigid body to determine the fusion posture data of the rigid body.

[0018] The second aspect of the present invention provides a posture fusion device for an IMU and a rigid body, comprising: a determination module, used to determine the positional relationship between an inertial measurement unit IMU and a rigid body; an acquisition and calculation module, used to acquire the measurement posture data of the IMU when the rigid body carries the IMU for movement, and calculate the posture data to be fused of the IMU through the measurement posture data of the IMU; an acquisition and determination module, used to acquire the measurement posture data of the rigid body in a preset world coordinate system, and determine the candidate posture data of the rigid body according to the measurement posture data of the IMU, the measurement posture data of the rigid body and the positional relationship between the IMU and the rigid body; a fusion module, used to fuse the posture data to be fused of the IMU with the candidate posture data of the rigid body using a fusion algorithm to obtain the fused posture data of the rigid body.

[0019] Optionally, in a first implementation method of the second aspect of the present invention, the acquisition and calculation module includes: an acquisition unit, used to acquire standard posture data of the IMU, the standard posture data including standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise; a determination unit, when the rigid body carries the IMU for movement, used to acquire the measured posture data of the IMU, and determine the posture data to be fused based on the measured posture data of the IMU and the standard posture data, wherein the posture data to be fused includes the acceleration to be fused and the angular velocity to be fused.

[0020] Optionally, in a second implementation of the second aspect of the present invention, the determination unit is specifically used to: obtain the measured attitude data of the IMU when the rigid body carries the IMU for movement, wherein the measured attitude data of the IMU includes measured acceleration and measured angular velocity; calculate the acceleration to be fused according to a first preset formula, the standard attitude data and the measured attitude data of the IMU, wherein the first preset formula is: Among them, a t represents the measured acceleration, a t(real) represents the acceleration to be fused, represents the standard acceleration bias at time t, Indicates the gravity direction component of the IMU attitude at time t, na is the standard acceleration noise; the angular velocity to be fused is calculated according to the second preset formula, the standard posture data and the measured posture data of the IMU, and the second preset formula is: Among them, w t Denotes the measured angular velocity, w t(real) represents the angular velocity to be fused, represents the standard angular velocity bias at time t, n w , represents standard acceleration noise; the acceleration to be fused and the angular velocity to be fused are combined to obtain the attitude data to be fused.

[0021] Optionally, in a third implementation of the second aspect of the present invention, the acquisition and determination module is specifically used to: acquire the measurement posture data of the rigid body and the measurement posture data of the IMU, extract the 0th frame IMU measurement posture data and the nth frame IMU measurement posture data from the measurement posture data of the IMU, and extract the 0th frame rigid body to world measurement posture data and the nth frame rigid body to world measurement posture data from the measurement posture data of the rigid body; in a preset world coordinate system, calculate the 0th frame IMU to world measurement posture data and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates according to the relationship equation, wherein the relationship equation is:

[0022]

[0023] in, Represents the measured posture data of the rigid body to the world in the nth frame, Represents the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates, [R i ] n Indicates the nth frame of IMU measurement attitude data, Represents the measured posture data of the rigid body to the world at frame 0, [R i ]0 represents the IMU measurement attitude data of the 0th frame, Indicates the measured attitude data from IMU to the world in frame 0, represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame, and E is used to indicate the unit matrix; the attitude data of the IMU is converted into the attitude data of the rigid body according to the attitude conversion formula to obtain the candidate attitude data of the rigid body, and the attitude conversion formula is:

[0024]

[0025] in, Represents the candidate posture data of the rigid body after transformation, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame. Represents the inverse transformation of the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates.

[0026] Optionally, in a fourth implementation of the second aspect of the present invention, the acquisition and determination module is specifically used to: extract the attitude quaternion in the candidate attitude data of the rigid body, and respectively obtain the candidate position data and candidate displacement data of the rigid body, the attitude quaternion of the IMU and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates; calculate the fusion position of the rigid body according to the position fusion formula in the fusion algorithm, the measured attitude data of the IMU, and the attitude data to be fused of the IMU, and the position fusion formula is:

[0027]

[0028] in, Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1, Indicates the candidate position of the rigid body in the preset world coordinate system at time k, α is the position fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, represents the candidate displacement of the rigid body at time k, Δt k represents the time interval, Δt = t k+1 -t k , a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, It represents the gravity direction component of the IMU attitude at time t, Indicates that when the IMU velocity and the acceleration to be fused are projected into the preset world coordinate system, [t k , t k+1 ] The displacement generated by the acceleration to be fused in the time period; the fusion posture of the rigid body is calculated according to the posture fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused. The posture fusion formula is:

[0029]

[0030] in, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k+1, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k, represents the multiplication symbol of quaternion, β represents the attitude fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, is the attitude quaternion of the IMU at time k, w t represents the measured angular velocity, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise, indicates [t k , t k+1 ] period; merging the fusion position of the rigid body with the fusion posture of the rigid body to determine the fusion posture data of the rigid body.

[0031] The third aspect of the present invention provides an IMU and rigid body posture fusion device, comprising: a memory and at least one processor, the memory storing instructions, the memory and the at least one processor being interconnected through lines; the at least one processor calls the instructions in the memory to enable the IMU and rigid body posture fusion device to execute the above-mentioned IMU and rigid body posture fusion method.

[0032] A fourth aspect of the present invention provides a computer-readable storage medium, which stores instructions. When the computer-readable storage medium is run on a computer, it enables the computer to execute the above-mentioned IMU and rigid body posture fusion method.

[0033] In the technical solution provided by the present invention, the positional relationship between the inertial measurement unit IMU and the rigid body is determined; when the rigid body carries the IMU for movement, the measured posture data of the IMU is obtained, and the posture data to be fused of the IMU is calculated through the measured posture data of the IMU; in a preset world coordinate system, the measured posture data of the rigid body is obtained, and the candidate posture data of the rigid body is determined according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body; the posture data to be fused of the IMU is fused with the candidate posture data of the rigid body by using a fusion algorithm to obtain the fused posture data of the rigid body. In an embodiment of the present invention, by obtaining the measured posture data of the IMU when the rigid body carries the IMU for movement, and determining the candidate posture data of the rigid body according to the conversion relationship between the IMU and the rigid body, and finally using a fusion function to fuse the measured posture data of the IMU with the candidate posture data of the rigid body, the fused posture data of the rigid body is obtained, thereby reducing the error between the measured posture data of the rigid body and the real posture data, and improving the accuracy of the measured posture data. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a schematic diagram of an embodiment of a method for fusing the posture of an IMU and a rigid body in an embodiment of the present invention;

[0035] Figure 2 It is a schematic diagram of another embodiment of the method for fusing the posture of an IMU and a rigid body in an embodiment of the present invention;

[0036] Figure 3 It is a schematic diagram of an embodiment of a position and posture fusion device of an IMU and a rigid body in an embodiment of the present invention;

[0037] Figure 4 It is a schematic diagram of another embodiment of the position and posture fusion device of IMU and rigid body in an embodiment of the present invention;

[0038] Figure 5 It is a schematic diagram of an embodiment of an IMU and a rigid body posture fusion device in an embodiment of the present invention. DETAILED DESCRIPTION

[0039] The embodiments of the present invention provide a method, device, equipment and storage medium for fusing the posture of an IMU and a rigid body. The method obtains the measured posture data of the IMU when the rigid body carries the IMU for movement, and determines the candidate posture data of the rigid body according to the conversion relationship between the IMU and the rigid body. Finally, the measured posture data of the IMU is fused with the candidate posture data of the rigid body using a fusion function to obtain the fused posture data of the rigid body, thereby reducing the error between the measured posture data and the actual posture data of the rigid body and improving the accuracy of the measured posture data.

[0040] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "including" or "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0041] For ease of understanding, the specific process of the embodiment of the present invention is described below. Figure 1 , an embodiment of the method for fusing the posture of an IMU and a rigid body in an embodiment of the present invention includes:

[0042] 101. Determine the positional relationship between the inertial measurement unit IMU and the rigid body;

[0043] It is understandable that the execution subject of the present invention may be an IMU and a rigid body posture fusion device, or a terminal or a server, which is not specifically limited here. The embodiment of the present invention is described by taking a server as an execution subject as an example.

[0044] The server determines the positional relationship between the inertial measurement unit IMU and the rigid body.

[0045] It should be noted that the inertial measurement unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Usually an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometers measure the acceleration data of the rigid body in the three independent axes of the carrier coordinate system, while the gyroscopes measure the angular velocity data of the carrier relative to the navigation coordinate system. The IMU attitude data of the rigid body can be measured, including the angular velocity and acceleration of the rigid body in the three axes in three-dimensional space. IMU has different models, and the acquisition frequency used by each model may be different. The model of IMU is not limited in this application.

[0046] A rigid body is a solid of finite size that can undergo negligible deformation regardless of whether it is affected by external forces. Negligible deformation here means that the distance between particles inside the rigid body does not change.

[0047] When detecting the posture data of a rigid body, the IMU needs to be fixed on the rigid body. When the rigid body carries the IMU to move, the IMU will detect the measured posture data of the IMU, and then perform the subsequent steps. The movement of the rigid body carrying the IMU here can be uniform linear motion or variable speed motion. The type of movement performed by the rigid body carrying the IMU is not limited in this application.

[0048] 102. When the rigid body moves with the IMU, the measured attitude data of the IMU is obtained, and the attitude data to be fused of the IMU is calculated through the measured attitude data of the IMU;

[0049] When the rigid body moves with the IMU, the server obtains the measured attitude data of the IMU and calculates the attitude data to be fused of the IMU through the measured attitude data of the IMU.

[0050] It can be understood that when a rigid body carries an IMU for movement, the IMU can obtain the acceleration and angular velocity during movement by measuring, and calculate the IMU's attitude data to be fused based on the standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise originally set by the IMU.

[0051] It should be further explained that the standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise here are all known. Generally, IMU hardware manufacturers will test the IMU equipment before leaving the factory and determine the standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise of the IMU through a large amount of test data. Therefore, in this application, the values ​​of the standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise are not limited, and different IMUs have different values.

[0052] 103. In a preset world coordinate system, obtain measurement posture data of the rigid body, and determine candidate posture data of the rigid body according to the measurement posture data of the IMU, the measurement posture data of the rigid body, and the positional relationship between the IMU and the rigid body;

[0053] The server obtains the measured posture data of the rigid body in a preset world coordinate system, and determines candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body.

[0054] It should be noted that due to the different positions of the IMU and the rigid body, when the rigid body carries the IMU for movement, the coordinate systems of the two are different. Therefore, it is necessary to convert the coordinate format of the IMU's posture to the posture of the rigid body, that is, to use the acquired measured posture data to calculate the transformation matrix of the IMU from the IMU coordinate system to the preset world coordinate system, and realize the calibration of the two in the coordinate format, thereby obtaining the posture conversion relationship between the IMU and the rigid body, and then when the posture of the IMU is integrated and calibrated with the posture of the rigid body, more accurate posture data of the rigid body can be obtained more quickly.

[0055] 104. Use the fusion algorithm to fuse the IMU's to-be-fused posture data with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

[0056] The server uses a fusion algorithm to fuse the IMU's to-be-fused posture data with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

[0057] After the server obtains the IMU's to-be-fused posture data and the candidate posture data of the rigid body, it is necessary to further fuse the two to obtain the fused posture data of the rigid body. The fused posture data of the rigid body includes the position data of the rigid body and the posture data of the rigid body. The factors affecting the position data of the rigid body include the candidate position data of the rigid body, the candidate displacement data, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates. The position data of the rigid body can be calculated based on the above factors and the position fusion formula; the factors affecting the posture data of the rigid body include the posture quaternion of the rigid body, the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, and the fused posture data of the IMU. The posture data of the rigid body can be calculated based on the above factors and the posture fusion formula. The position data of the rigid body is combined with the posture data of the rigid body to obtain the fused posture data of the rigid body.

[0058] In an embodiment of the present invention, the measured posture data of the IMU is obtained when the rigid body carries the IMU for movement, and the candidate posture data of the rigid body is determined according to the conversion relationship between the IMU and the rigid body. Finally, the measured posture data of the IMU is fused with the candidate posture data of the rigid body using a fusion function to obtain the fused posture data of the rigid body, thereby reducing the error between the measured posture data of the rigid body and the actual posture data, and improving the accuracy of the measured posture data.

[0059] See also Figure 2 Another embodiment of the method for fusing the IMU and the rigid body's posture in the embodiment of the present invention includes:

[0060] 201. Determine the positional relationship between the inertial measurement unit IMU and the rigid body;

[0061] The server determines the positional relationship between the inertial measurement unit IMU and the rigid body. Among them, the inertial measurement unit is a device for measuring the three-axis attitude angle and acceleration of an object. Usually an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometers measure the acceleration data of the rigid body in the three independent axes of the carrier coordinate system, while the gyroscopes measure the angular velocity data of the carrier relative to the navigation coordinate system. The IMU attitude data of the rigid body can be measured, including the angular velocity and acceleration of the rigid body in three axes in three-dimensional space. IMU has different models, and the acquisition frequency used by each model may be different. The model of IMU is not limited in this application.

[0062] A rigid body is a solid of finite size whose deformation is negligible regardless of whether it is affected by external forces. The negligible deformation here means that the distance between particles inside the rigid body will not change.

[0063] When detecting the posture data of a rigid body, the IMU needs to be fixed on the rigid body. When the rigid body carries the IMU to move, the IMU will display the measured posture data of the IMU, and then perform the subsequent steps. The movement of the rigid body carrying the IMU here can be uniform linear motion or variable speed motion. The type of movement performed by the rigid body carrying the IMU is not limited in this application.

[0064] 202. Obtain standard attitude data of the IMU, where the standard attitude data includes standard acceleration bias, standard angular velocity bias, standard acceleration noise, and standard angular velocity noise;

[0065] The server obtains the standard posture data of the IMU, and the standard posture data includes standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise.

[0066] Here, the standard attitude data of the IMU is a known parameter, among which the standard attitude data includes: standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise. Generally, IMU hardware manufacturers will test the IMU equipment before leaving the factory and determine the standard acceleration bias, standard angular velocity bias, standard acceleration noise and standard angular velocity noise of the IMU through a large amount of test data.

[0067] 203. When the rigid body moves with the IMU, the measured attitude data of the IMU is obtained, and the attitude data to be fused is determined by the measured attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused;

[0068] When the rigid body moves with the IMU, the server obtains the measured attitude data of the IMU, and determines the attitude data to be fused based on the measured attitude data of the IMU and the standard attitude data, where the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused. Specifically:

[0069] The server obtains the measured attitude data of the IMU when the rigid body carries the IMU for movement, wherein the measured attitude data of the IMU includes the measured acceleration and the measured angular velocity; the server calculates the acceleration to be fused according to the first preset formula, the standard attitude data and the measured attitude data, and the first preset formula is: Among them, a t represents the measured acceleration, a t(real) represents the acceleration to be fused, represents the standard acceleration bias at time t, Indicates the gravity direction component of the IMU attitude at time t, n a is the standard acceleration noise; the server calculates the angular velocity to be fused according to the second preset formula, the standard posture data and the measured posture data, and the second preset formula is: Among them, w t Denotes the measured angular velocity, w t(real) represents the angular velocity to be fused, represents the standard angular velocity bias at time t, n w Represents standard acceleration noise; the server combines the acceleration to be fused and the angular velocity to be fused to obtain the attitude data to be fused.

[0070] When the rigid body carries the IMU to move, the measured attitude data of the IMU is obtained. The measured attitude data here includes the measured acceleration and measured angular velocity output by the IMU. When the rigid body carries the IMU to move, it will be affected by the weight of the rigid body and the acceleration noise, resulting in a smaller measured acceleration. Therefore, the server calculates the theoretical real acceleration, which is the acceleration to be fused, through the first preset formula. Similarly, when calculating the angular velocity of the rigid body, it will also be affected by the weight of the rigid body and the angular velocity noise, resulting in a smaller measured angular velocity. Therefore, the server calculates the theoretical real angular velocity, which is the angular velocity to be fused, through the second preset formula.

[0071] According to a large amount of measured attitude data, the standard acceleration noise n a And standard acceleration noise n w All obey Gaussian distribution, among which the standard acceleration noise obeys the distribution: The standard angular velocity noise follows the distribution: Here σ a With σ w is the variance of Gaussian white noise, and its specific value depends on the IMU model. In addition, the standard acceleration bias and standard angular velocity bias is modeled as a random walk, where the random walk of the standard acceleration bias is Obey Gaussian distribution The random walk of the standard angular velocity bias is Obey Gaussian distribution Here and They are the variance of the random walk Gaussian white noise of the standard acceleration bias and the standard angular velocity bias, respectively. The specific value depends on the IMU model.

[0072] It is understandable that when the IMU and the rigid body are not moving or the movement is not obvious, the measured attitude data obtained at this time is unchanged and can be considered as invalid attitude data. The measured attitude data obtained when the IMU and the rigid body are in motion is valid attitude data.

[0073] 204. In a preset world coordinate system, obtain measurement posture data of the rigid body, and determine candidate posture data of the rigid body according to the measurement posture data of the IMU, the measurement posture data of the rigid body, and the positional relationship between the IMU and the rigid body;

[0074] The server obtains the measured posture data of the rigid body in the preset world coordinate system, and determines the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body. Specifically:

[0075] The server first obtains the measurement posture data of the rigid body and the measurement posture data of the IMU, extracts the 0th frame IMU measurement posture data and the nth frame IMU measurement posture data from the IMU measurement posture data, and extracts the 0th frame rigid body to world measurement posture data and the nth frame rigid body to world measurement posture data from the rigid body measurement posture data; then the server calculates the 0th frame IMU to world measurement posture data and the IMU rotation matrix from the IMU coordinates to the rigid body coordinates in the preset world coordinate system according to the relationship equation, where the relationship equation is:

[0076]

[0077] in, Represents the measured posture data of the rigid body to the world in the nth frame, Represents the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates, [R i ] n Indicates the nth frame of IMU measurement attitude data, Represents the measured posture data of the rigid body to the world at frame 0, [R i ]0 represents the IMU measurement attitude data of the 0th frame, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame, and E is used to indicate the unit matrix; finally, the server converts the IMU attitude data into the attitude data of the rigid body according to the attitude conversion formula to obtain the candidate attitude data of the rigid body. The attitude conversion formula is:

[0078]

[0079] in, Represents the candidate posture data of the rigid body after transformation, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame. Represents the inverse transformation of the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates.

[0080] It is understandable that when the coordinate format is converted using the measured posture data of the rigid body, the measured posture data includes the measured posture data of multiple frames, wherein the 0-frame IMU measured posture data is the position initially measured by the IMU, the n-frame IMU measured posture data is the position of the IMU at the nth frame, and the 0-frame rigid body to world measured posture data is the position initially measured by the rigid body. The server calculates the rotation matrix of the IMU measured posture data from the IMU coordinates to the rigid body coordinates, and calculates the 0-frame IMU to world measured posture data through the relational equation, wherein the relational equation is:

[0081] in, Represents the measured posture data of the rigid body to the world in the nth frame, Represents the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates, [R i ] n [R i ] n Indicates the nth frame of IMU measurement attitude data, Represents the measured posture data of the rigid body to the world at frame 0, [R i ]0 represents the IMU measurement attitude data of the 0th frame, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame, and E is used to indicate the unit matrix.

[0082] Specifically: The server substitutes the IMU measurement posture data of the 0th frame, the IMU measurement posture data of the nth frame, the measurement posture data of the rigid body to the world of the 0th frame, and the measurement posture data of the rigid body to the world of the nth frame into the relationship equation, and uses the least squares method to obtain when the sum of the squares of the differences between the data on both sides of the equal sign of the relationship equation is the smallest. Know Among them, the least squares formula Q includes:

[0083]

[0084] Further: After the server calculates the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates, and the measured attitude data from the IMU to the world in the 0th frame, it can substitute the two into the attitude conversion formula to determine the conversion formula when the IMU measured attitude data is converted from the IMU coordinates to the rigid body coordinates, where the attitude conversion formula is:

[0085]

[0086] in, is the inverse transformation of the rotation matrix of the IMU attitude data from the IMU coordinates to the rigid body coordinates, It is the posture data of the rigid body after transformation.

[0087] 205. The fusion algorithm is used to fuse the IMU's to-be-fused posture data with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

[0088] The server uses a fusion algorithm to fuse the IMU's to-be-fused posture data with the rigid body's candidate posture data to obtain the rigid body's fused posture data. Specifically:

[0089] First, the server extracts the attitude quaternion from the candidate attitude data of the rigid body, and obtains the candidate position data and candidate displacement data of the rigid body, the attitude quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates; secondly, the server calculates the fusion position of the rigid body according to the position fusion formula in the fusion algorithm, the measured attitude data of the IMU, and the attitude data to be fused of the IMU. The position fusion formula is:

[0090]

[0091] in, Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1, Indicates the candidate position of the rigid body in the preset world coordinate system at time k, α is the position fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, represents the candidate displacement of the rigid body at time k, Δt k represents the time interval, Δt = t k+1 -t k , a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, It represents the gravity direction component of the IMU attitude at time t, Indicates that when the IMU velocity and the acceleration to be fused are projected into the preset world coordinate system, [t k , t k+1 ] The displacement generated by the acceleration to be fused in the time period; then the server calculates the fusion posture of the rigid body according to the posture fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused. The posture fusion formula is:

[0092]

[0093] in, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k+1, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k, represents the multiplication symbol of quaternion, β represents the attitude fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, is the attitude quaternion of the IMU at time k, w t represents the measured angular velocity, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise, indicates [t k , t k+1 ] period; finally, the server merges the fusion position of the rigid body with the fusion posture of the rigid body to determine the fusion posture data of the rigid body.

[0094] After the server obtains the IMU's to-be-fused posture data and the candidate posture data of the rigid body, it needs to further fuse the two to obtain the fused posture data of the rigid body. First, the server performs position fusion of the rigid body. The server calculates the IMU's measured speed through the IMU's measured acceleration. t The speed can be obtained by integration. Because the speed value is small due to noise and gravity when the IMU is measuring, the speed of the IMU at time k is calculated by this formula:

[0095]

[0096] In the formula, a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, Represents the gravity direction component of the IMU posture at time t. And the obtained measured attitude parameters are substituted into the position fusion formula to determine the position and attitude data of the rigid body. It should be noted that α is the position fusion coefficient, that is, the ratio of the candidate displacement of the rigid body at time k to the measured displacement of the IMU at time k. The value is determined according to the IMU inter-frame displacement integration accuracy. The value in this application is 0.15.

[0097] After the server obtains the fused position data, it fuses the attitude data. The server first obtains the attitude quaternion of the rigid body in the preset world coordinate system at time k Then get the attitude quaternion of the IMU at time k Substitute the two and other acquired posture data into the posture fusion formula to determine the posture data of the rigid body. It should be noted that β is the posture fusion coefficient, which is set according to the accuracy of optical posture calculation and IMU posture data, and the value in the present invention is 0.75.

[0098] In an embodiment of the present invention, the measured posture data of the IMU is obtained when the rigid body carries the IMU for movement, and the candidate posture data of the rigid body is determined according to the conversion relationship between the IMU and the rigid body. Finally, the measured posture data of the IMU is fused with the candidate posture data of the rigid body using a fusion function to obtain the fused posture data of the rigid body, thereby reducing the error between the measured posture data of the rigid body and the actual posture data, and improving the accuracy of the measured posture data.

[0099] The above describes the method for fusion of IMU and rigid body posture in the embodiment of the present invention. The following describes the device for fusion of IMU and rigid body posture in the embodiment of the present invention. Figure 3 In one embodiment of the present invention, an IMU and a rigid body posture fusion device comprises:

[0100] A determination module 301 is used to determine the positional relationship between the inertial measurement unit IMU and the rigid body;

[0101] The acquisition and calculation module 302 is used to acquire the measured posture data of the IMU when the rigid body moves with the IMU, and calculate the posture data to be fused of the IMU through the measured posture data of the IMU;

[0102] The acquisition and determination module 303 is used to acquire the measured posture data of the rigid body in a preset world coordinate system, and determine the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body;

[0103] The fusion module 304 is used to fuse the to-be-fused posture data of the IMU with the candidate posture data of the rigid body by using a fusion algorithm to obtain fused posture data of the rigid body.

[0104] In an embodiment of the present invention, the measured posture data of the IMU is obtained when the rigid body carries the IMU for movement, and the candidate posture data of the rigid body is determined according to the conversion relationship between the IMU and the rigid body. Finally, the measured posture data of the IMU is fused with the candidate posture data of the rigid body using a fusion function to obtain the fused posture data of the rigid body, thereby reducing the error between the measured posture data of the rigid body and the actual posture data, and improving the accuracy of the measured posture data.

[0105] See also Figure 4Another embodiment of the IMU and rigid body posture fusion device in the embodiment of the present invention includes:

[0106] A determination module 301 is used to determine the positional relationship between the inertial measurement unit IMU and the rigid body;

[0107] The acquisition and calculation module 302 is used to acquire the measured posture data of the IMU when the rigid body moves with the IMU, and calculate the posture data to be fused of the IMU through the measured posture data of the IMU;

[0108] The acquisition and determination module 303 is used to acquire the measured posture data of the rigid body in a preset world coordinate system, and determine the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body;

[0109] The fusion module 304 is used to fuse the to-be-fused posture data of the IMU with the candidate posture data of the rigid body by using a fusion algorithm to obtain fused posture data of the rigid body.

[0110] Optionally, the acquisition and calculation module 302 includes:

[0111] The acquisition unit 3021 is used to acquire the standard posture data of the IMU, wherein the standard posture data includes a standard acceleration bias, a standard angular velocity bias, a standard acceleration noise, and a standard angular velocity noise;

[0112] The determination unit 3022 is used to obtain the measured attitude data of the IMU when the rigid body moves with the IMU, and determine the attitude data to be fused according to the measured attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused.

[0113] Optionally, the determining unit 3022 may also be specifically configured to:

[0114] Obtaining the measured attitude data of the IMU when the rigid body carries the IMU in motion, wherein the measured attitude data of the IMU includes measured acceleration and measured angular velocity;

[0115] The acceleration to be fused is calculated according to the first preset formula, the standard posture data and the measured posture data of the IMU. The first preset formula is: Among them, a t represents the measured acceleration, a t(real) represents the acceleration to be fused, represents the standard acceleration bias at time t, Indicates the gravity direction component of the IMU attitude at time t, n a is the standard acceleration noise;

[0116] The angular velocity to be fused is calculated according to the second preset formula, the standard posture data and the IMU measured posture data. The second preset formula is: Among them, w t Denotes the measured angular velocity, w t(real) represents the angular velocity to be fused, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise;

[0117] The acceleration to be fused and the angular velocity to be fused are combined to obtain the attitude data to be fused.

[0118] Optionally, the obtaining and determining module 303 may also be specifically used for:

[0119] Obtaining the measurement posture data of the rigid body and the measurement posture data of the IMU, extracting the 0th frame IMU measurement posture data and the nth frame IMU measurement posture data from the measurement posture data of the IMU, and extracting the 0th frame rigid body-to-world measurement posture data and the nth frame rigid body-to-world measurement posture data from the measurement posture data of the rigid body;

[0120] In the preset world coordinate system, the measured posture data from the IMU to the world in the 0th frame and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates are calculated according to the relationship equation, where the relationship equation is:

[0121]

[0122] in, Represents the measured posture data of the rigid body to the world in the nth frame, Represents the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates, [R i ] n Indicates the nth frame of IMU measurement attitude data, Represents the measured posture data of the rigid body to the world at frame 0, [R i ]0 represents the IMU measurement attitude data of the 0th frame, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame, and E is used to indicate the unit matrix;

[0123] According to the attitude conversion formula, the attitude data of the IMU is converted into the attitude data of the rigid body to obtain the candidate attitude data of the rigid body. The attitude conversion formula is:

[0124]

[0125] in, Represents the candidate posture data of the rigid body after transformation, Indicates the measured attitude data from IMU to the world in frame 0, It represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the IMU measured attitude data of the 0th frame. Represents the inverse transformation of the rotation matrix of the IMU measured attitude data from the IMU coordinates to the rigid body coordinates.

[0126] Optionally, the fusion module 304 may also be specifically used for:

[0127] Extract the attitude quaternion from the candidate attitude data of the rigid body, and obtain the candidate position data and candidate displacement data of the rigid body, the attitude quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates;

[0128] The fusion position of the rigid body is calculated according to the position fusion formula in the fusion algorithm, the measured attitude data of the IMU, and the attitude data to be fused by the IMU. The position fusion formula is:

[0129]

[0130] in, Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1, Indicates the candidate position of the rigid body in the preset world coordinate system at time k, α is the position fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, represents the candidate displacement of the rigid body at time k, Δt k represents the time interval, Δt = t k+1 -t k , a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, It represents the gravity direction component of the IMU attitude at time t, Indicates that when the IMU velocity and the acceleration to be fused are projected into the preset world coordinate system, [t k , t k+1 ] is the displacement generated by the acceleration to be fused in the time period;

[0131] According to the attitude fusion formula in the fusion algorithm, the measured attitude data of the IMU, and the attitude data to be fused, the fusion attitude of the rigid body is calculated. The attitude fusion formula is:

[0132]

[0133] in, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k+1, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k, represents the multiplication symbol of quaternion, β represents the attitude fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, is the attitude quaternion of the IMU at time k, w t represents the measured angular velocity, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise, indicates [t k , t k+1 ] The rotation change produced by the IMU during the period;

[0134] The fusion position of the rigid body is combined with the fusion posture of the rigid body to determine the fusion posture data of the rigid body.

[0135] In an embodiment of the present invention, the measured posture data of the IMU is obtained when the rigid body carries the IMU for movement, and the candidate posture data of the rigid body is determined according to the conversion relationship between the IMU and the rigid body. Finally, the measured posture data of the IMU is fused with the candidate posture data of the rigid body using a fusion function to obtain the fused posture data of the rigid body, thereby reducing the error between the measured posture data of the rigid body and the actual posture data, and improving the accuracy of the measured posture data.

[0136] above Figure 3 and Figure 4 The position and posture fusion device of the IMU and the rigid body in the embodiment of the present invention is described in detail from the perspective of modular functional entity. The position and posture fusion device of the IMU and the rigid body in the embodiment of the present invention is described in detail from the perspective of hardware processing.

[0137] Figure 5: is a structural schematic diagram of an IMU and rigid body posture fusion device provided by an embodiment of the present invention. The IMU and rigid body posture fusion device 500 may have relatively large differences due to different configurations or performances, and may include one or more processors (central processing units, CPU) 510 (for example, one or more processors) and a memory 520, and one or more storage media 530 (for example, one or more massive storage devices) storing application programs 533 or data 532. Among them, the memory 520 and the storage medium 530 can be short-term storage or permanent storage. The program stored in the storage medium 530 may include one or more modules (not shown in the figure), and each module may include a series of instruction operations in the IMU and rigid body posture fusion device 500. Furthermore, the processor 510 can be configured to communicate with the storage medium 530 to execute a series of instruction operations in the storage medium 530 on the IMU and rigid body posture fusion device 500.

[0138] The IMU and rigid body posture fusion device 500 may also include one or more power supplies 540, one or more wired or wireless network interfaces 550, one or more input and output interfaces 560, and / or one or more operating systems 531, such as Windows Serve, Mac OS X, Unix, Linux, FreeBSD, etc. Those skilled in the art will appreciate that Figure 5 The structure of the IMU and rigid body posture fusion device shown does not constitute a limitation of the IMU and rigid body posture fusion device, and may include more or fewer components than shown in the figure, or a combination of certain components, or a different arrangement of components.

[0139] The present invention also provides a computer-readable storage medium, which may be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium. Instructions are stored in the computer-readable storage medium. When the instructions are executed on a computer, the computer executes the steps of the method for fusion of the IMU and the rigid body posture.

[0140] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0141] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk and other media that can store program codes.

[0142] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for fusing IMU and rigid body posture, characterized in that: The IMU and rigid body posture fusion method includes: Determine the positional relationship between the inertial measurement unit IMU and the rigid body; When the rigid body carries the IMU for movement, the measured posture data of the IMU is obtained, and the posture data to be fused of the IMU is calculated through the measured posture data of the IMU; Acquire the measurement posture data of the rigid body and the measurement posture data of the IMU, extract the 0th frame IMU measurement posture data and the nth frame IMU measurement posture data from the measurement posture data of the IMU, and extract the 0th frame rigid body to world measurement posture data and the nth frame rigid body to world measurement posture data from the measurement posture data of the rigid body; In a preset world coordinate system, according to a preset relationship equation, the measured posture data of the IMU to the world in the 0th frame and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates are calculated, and the posture data of the IMU is converted into the posture data of the rigid body according to a preset posture conversion formula to obtain candidate posture data of the rigid body; Extracting the attitude quaternion from the candidate attitude data of the rigid body, and respectively obtaining the candidate position data and candidate displacement data of the rigid body, the attitude quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates; Calculate the fusion position of the rigid body according to the position fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused of the IMU; Calculate the fusion posture of the rigid body according to the posture fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused; Combining the fusion position of the rigid body with the fusion posture of the rigid body to determine the fusion posture data of the rigid body; The position fusion formula is: in, Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1, Indicates the candidate position of the rigid body in the preset world coordinate system at time k, α is the position fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, represents the candidate displacement of the rigid body at time k, Δt k represents the time interval, Δt = t k+1 -t k , a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, It represents the gravity direction component of the IMU attitude at time t, Indicates that when the IMU velocity and the acceleration to be fused are projected into the preset world coordinate system, [t k ,t k+1 ] is the displacement generated by the acceleration to be fused in the time period; The posture fusion formula is: in, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k+1, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k, represents the multiplication symbol of quaternion, β represents the attitude fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, is the attitude quaternion of the IMU at time k, w t represents the measured angular velocity, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise, indicates [t k ,t k+1 ] The rotation change produced by the IMU during the period.

2. The method for fusion of IMU and rigid body posture according to claim 1, characterized in that: When the rigid body moves with the IMU, obtaining the measured attitude data of the IMU, and calculating the to-be-fused attitude data of the IMU through the measured attitude data of the IMU includes: Acquire standard posture data of the IMU, wherein the standard posture data includes standard acceleration bias, standard angular velocity bias, standard acceleration noise, and standard angular velocity noise; When the rigid body moves with the IMU, the measured attitude data of the IMU is obtained, and the attitude data to be fused is determined according to the measured attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused.

3. The method for fusion of IMU and rigid body posture according to claim 2, characterized in that: When the rigid body carries the IMU for movement, the measured attitude data of the IMU is obtained, and the attitude data to be fused is determined according to the measured attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused, including: Acquire the measured attitude data of the IMU when the rigid body carries the IMU for movement, wherein the measured attitude data of the IMU includes measured acceleration and measured angular velocity; The acceleration to be fused is calculated according to a first preset formula, the standard posture data and the measured posture data of the IMU, wherein the first preset formula is: Among them, a t represents the measured acceleration, a t(real) represents the acceleration to be fused, represents the standard acceleration bias at time t, Indicates the gravity direction component of the IMU attitude at time t, n a is the standard acceleration noise; The angular velocity to be fused is calculated according to a second preset formula, the standard posture data and the measured posture data of the IMU, wherein the second preset formula is: Among them, w t Denotes the measured angular velocity, w t(real) represents the angular velocity to be fused, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise; The acceleration to be fused and the angular velocity to be fused are combined to obtain the posture data to be fused.

4. An IMU and rigid body posture fusion device, characterized in that: The IMU and rigid body posture fusion device comprises: A determination module, used to determine the positional relationship between the inertial measurement unit IMU and the rigid body; An acquisition and calculation module, when the rigid body carries the IMU for movement, is used to acquire the measured posture data of the IMU, and calculate the posture data to be fused of the IMU through the measured posture data of the IMU; An acquisition and determination module is used to acquire the measurement posture data of the rigid body in a preset world coordinate system, and determine the candidate posture data of the rigid body according to the measurement posture data of the IMU, the measurement posture data of the rigid body, and the positional relationship between the IMU and the rigid body; A fusion module, used for fusing the to-be-fused posture data of the IMU with the candidate posture data of the rigid body using a fusion algorithm to obtain fused posture data of the rigid body; The position fusion formula is: in, Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1, Indicates the candidate position of the rigid body in the preset world coordinate system at time k, α is the position fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, represents the candidate displacement of the rigid body at time k, Δt k represents the time interval, Δt = t k+1 -t k , a t represents the measured acceleration, represents the speed of the IMU at time k, represents the standard acceleration bias at time t, n a is the standard acceleration noise, It represents the gravity direction component of the IMU attitude at time t, Indicates that when the IMU velocity and the acceleration to be fused are projected into the preset world coordinate system, [t k ,t k+1 ] is the displacement generated by the acceleration to be fused in the time period; The posture fusion formula is: in, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k+1, Represents the attitude quaternion of the rigid body in the preset world coordinate system at time k, represents the multiplication symbol of quaternion, β represents the attitude fusion coefficient, Represents the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, is the attitude quaternion of the IMU at time k, w t represents the measured angular velocity, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise, indicates [t k ,t k+1 ] The rotation change produced by the IMU during the period.

5. The IMU and rigid body position and posture fusion device according to claim 4, characterized in that: The acquisition and calculation module includes: An acquisition unit, used to acquire standard posture data of the IMU, wherein the standard posture data includes a standard acceleration bias, a standard angular velocity bias, a standard acceleration noise, and a standard angular velocity noise; A determination unit is used to obtain the measured attitude data of the IMU when the rigid body moves with the IMU, and determine the attitude data to be fused according to the measured attitude data of the IMU and the standard attitude data, wherein the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused.

6. The IMU and rigid body position and posture fusion device according to claim 5, characterized in that: The determining unit is specifically used for: Acquire the measured attitude data of the IMU when the rigid body carries the IMU for movement, wherein the measured attitude data of the IMU includes measured acceleration and measured angular velocity; The acceleration to be fused is calculated according to a first preset formula, the standard posture data and the measured posture data of the IMU, wherein the first preset formula is: Among them, a t represents the measured acceleration, a t(real) represents the acceleration to be fused, represents the standard acceleration bias at time t, Indicates the gravity direction component of the IMU attitude at time t, n a is the standard acceleration noise; The angular velocity to be fused is calculated according to a second preset formula, the standard posture data and the measured posture data of the IMU, wherein the second preset formula is: Among them, w t Denotes the measured angular velocity, w t(real) represents the angular velocity to be fused, represents the standard angular velocity bias at time t, n w represents the standard acceleration noise; The acceleration to be fused and the angular velocity to be fused are combined to obtain the posture data to be fused.

7. An IMU and rigid body posture fusion device, characterized in that: The IMU and rigid body posture fusion device comprises: a memory and at least one processor, the memory stores instructions, and the memory and the at least one processor are interconnected through a line; The at least one processor calls the instructions in the memory so that the IMU and rigid body posture fusion device executes the IMU and rigid body posture fusion method as described in any one of claims 1-3.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the IMU and rigid body posture fusion method as described in any one of claims 1 to 3 is implemented.