High-frequency sampling motion error compensation method and system

Abstract

The invention discloses a high-frequency sampling motion error compensation method and a high-frequency sampling motion error compensation system. The method comprises the following steps: step 1, acquiring original data of an inertial measurement unit at high frequency; 2, grouping the collected data, and performing high-frequency cone motion compensation and paddling effect compensation on every two data; and step 3, combining results of pairwise compensation calculation after grouping into low-frequency data. On one hand, high-frequency compensation of coning errors and paddling errors is realized, so that the navigation system is suitable for complex maneuvering conditions; on the other hand, 200 interruption is used, IMU error compensation and navigation calculation can be completed within interruption time, frequent stack operation is reduced, and stability of the system is facilitated.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation, and particularly to a motion error compensation method and system with high-frequency sampling. Background Art

[0002] In inertial navigation, when the carrier has high-frequency angular vibration or compound motion, the traditional first-order integration algorithm will generate non-negligible mathematical errors. Therefore, coning motion and paddling motion compensation are required to correct the rotational non-commutativity and motion coupling effects.

[0003] Currently, in existing inertial navigation technologies, single-sample or double-sample methods are often used to compensate for coning motion and paddling motion. From the perspective of the accuracy of the compensation algorithm, the algorithm error drift coefficient of single-sample compensation is about 1 / 12, and that of double-sample compensation is about 1 / 960, resulting in certain compensation algorithm errors in medium- and high-precision applications. Although the algorithm error drift coefficient of cross-sample compensation is reduced to 1.3E-27, it requires a very large amount of calculation, which is not conducive to engineering implementation. Summary of the Invention

[0004] The technical problem to be solved by the embodiments of the present invention is to provide a motion error compensation method and system with high-frequency sampling, so as to reduce the system operation amount while obtaining a high-precision compensation effect.

[0005] To solve the above technical problem, the embodiments of the present invention propose a motion error compensation method with high-frequency sampling, including: Step 1: High-frequency collect the original data of the inertial measurement unit; Step 2: Group the collected data and perform high-frequency coning motion compensation and paddling effect compensation pairwise; Step 3: Combine the results of pairwise compensation calculations after grouping into low-frequency data.

[0006] Correspondingly, the embodiments of the present invention also provide a motion error compensation system with high-frequency sampling, including: A collection unit: High-frequency collect the original data of the inertial measurement unit; A compensation unit: Group the collected data and perform high-frequency coning motion compensation and paddling effect compensation pairwise; A synthesis unit: Combine the results of pairwise compensation calculations after grouping into low-frequency data.

[0007] The beneficial effects of the present invention are as follows: The present invention realizes high-frequency compensation of 2000Hz IMU raw data using a 200Hz interrupt, avoiding the complex calculation amount of cross-shaped compensation. The present invention decomposes it into 5 similar double-shaped compensations plus an accumulation and splicing operation, thus greatly reducing the calculation amount without sacrificing compensation accuracy. On the one hand, the present invention realizes high-frequency compensation of coning error and paddling error so that the navigation system is applicable to complex maneuvering situations. On the other hand, using a 200Hz interrupt, IMU error compensation and navigation solution can be completed within the interrupt time, reducing frequent operations on the stack and being beneficial to the stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] Figure 1 is a schematic flowchart of the high-frequency sampling motion error compensation method according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0009] It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The present invention will be further described in detail below with reference to the drawings and specific embodiments.

[0010] In the embodiments of the present invention, if there are directional indications (such as up, down, left, right, front, back...), they are only used to explain the relative positional relationship and motion conditions between components in a specific posture (as shown in the drawings). If the specific posture changes, the directional indications will also change accordingly.

[0011] In addition, in the present invention, the descriptions involving "first", "second", etc. are only for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features.

[0012] Please refer to Figure 1 , the high-frequency sampling motion error compensation method according to an embodiment of the present invention includes steps 1 to 3.

[0013] Step 1: High-frequency acquisition of the original data of the inertial measurement unit. In Step 1, the data acquisition strategy used is as follows: A 200Hz interrupt is adopted, that is, the inertial update period of the inertial measurement unit is 200Hz, which also means that the output frequencies of the angular increment and velocity increment are 200Hz. The gyro data of the inertial measurement unit is acquired through a synchronous handshake method, with an acquisition frequency of 2000Hz. At the same time, the accelerometer data of the inertial measurement unit is acquired using the rising edge of the same frequency, basically ensuring the synchronization of the gyro output data and the accelerometer output data under the condition of 2000Hz. The FPGA temporarily stores the acquired gyroscope and accelerometer data in the FIFO. In the embedded software, the interrupt is 200Hz. After entering an interrupt, 10 consecutive acquisitions are performed, and the 10 inertial measurement unit data acquired by the FPGA at high frequency are read at one time and assigned to global variables.

[0014] Step 2: Group the acquired data and perform high-frequency coning motion compensation and rowing effect compensation pairwise. The specific calculations are as follows: The acquired high-frequency angular rate data is divided into five groups. That is, the 10 data are numbered from 1 to 10 respectively, and the i-th and the (i + 1)-th data form a group, where i = 1, 3, 5, 7, 9. A total of 5 groups are decomposed.

[0015] The following formula calculations are performed on each group of data: ; ; ; ; ; ; ; ; ; Among them, is the i-th angular increment data, is the (i + 1)-th angular increment data, is the sum of the angular increment data of the current group, is the i-th velocity increment data, is the (i + 1)-th velocity increment data, is the sum of the velocity increment data of the current group, is the vector cross product operation, 、 、 、 、 、 、 It is an intermediate variable in the iterative calculation process.

[0016] Step 3: Combine the results of pairwise compensation calculations after grouping into low-frequency data. The timing of this calculation is as follows: After the fifth group of data is calculated according to Step 2, we get , , , , and then perform the following combined calculation. After completing the iteration of the fifth group of data, the core intermediate variables , , , have completed the accumulation and calculation.

[0017] ; ; Among them, is the 5ms angular increment information after conical compensation, is the 5ms speed increment information after oar compensation.

[0018] After obtaining the 5ms angular increment and speed increment information, the relevant cumulative variables need to be cleared for the next calculation of the 10 sampled data of 5ms. The variables to be cleared are , , , .

[0019] The high-frequency sampling motion error compensation system of the embodiment of the present invention includes an acquisition unit, a compensation unit, and a synthesis unit.

[0020] Acquisition unit: High-frequency acquisition of the original data of the inertial measurement unit.

[0021] Compensation unit: Group the acquired data and perform high-frequency conical motion compensation and oar effect compensation pairwise.

[0022] Synthesis unit: Combine the results of pairwise compensation calculations after grouping into low-frequency data.

[0023] As an implementation method, the acquisition unit uses the method of 200Hz interruption to high-frequency acquire the original data of the inertial measurement unit, and continuously acquires the data of the inertial measurement unit 10 times after entering an interruption.

[0024] As an implementation method, the compensation unit divides the high-frequency angular rate data in the acquired original data into five groups, that is, numbers 10 data from 1 to 10 respectively, and the i-th and (i + 1)-th data are in a group, and a total of 5 groups are decomposed. i = 1, 3, 5, 7, 9; Perform the following formula calculation on each group of data: ; ; ; ; ; ; ; ; ; wherein, is the i-th angular increment data, is the (i + 1)-th angular increment data, is the sum of the angular increment data of the current group, is the i-th velocity increment data, is the (i + 1)-th velocity increment data, is the sum of the velocity increment data of the current group, is the vector cross product operation, , , , , , , are intermediate variables in the iterative calculation process.

[0025] As an implementation manner, after the compensation unit calculates the corresponding , , , according to the 5th group of data, the synthesis unit performs the following combined calculation on the data calculated by the compensation unit: ; ; wherein, is the 5ms angular increment information after conical compensation, is the 5ms velocity increment information after oar compensation.

[0026] As an implementation manner, after the synthesis unit obtains the 5ms angular increment and velocity increment information, the relevant cumulative variables are cleared for the next calculation of the 10 sampling data of 5ms. Among them, the variables to be cleared are , , , .

[0027] Through the collaboration of hardware capabilities and algorithmic intelligence, the present invention reconstructs real motion from noise and can be applied to precise hovering of drones (compensating for attitude jitter caused by high-frequency vibration of rotors), high-dynamic carrier navigation (high-frequency angular motion compensation), etc. In high-precision fields such as autonomous driving and aerospace, the present invention can achieve centimeter-level positioning and milliradian-level attitude accuracy.

[0028] Although the embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A motion error compensation method with high-frequency sampling, characterized in that, Including: Step 1: High-frequency acquisition of the original data of the inertial measurement unit; Step 2: Group the acquired data and perform high-frequency coning motion compensation and oar effect compensation pairwise; Step 3: Combine the results of pairwise compensation calculations after grouping into low-frequency data; In Step 1, the original data of the inertial measurement unit is acquired at high frequency in the way of 200Hz interruption, and the data of the inertial measurement unit is continuously acquired 10 times after entering one interruption; In Step 2, the high-frequency angular rate data in the acquired original data is divided into five groups, that is, the 10 data are numbered from 1 to 10 respectively, and the i-th and the (i + 1)-th data are in one group, and a total of 5 groups are decomposed, where i = 1, 3, 5, 7, 9; The following formula calculations are performed on each group of data: ； ； ； ； ； ； ； ； ； Among them, is the i-th angular increment data, is the (i + 1)-th angular increment data, is the sum of the current group of angular increment data, is the i-th speed increment data, is the (i + 1)-th speed increment data, is the sum of the current group of speed increment data, is the vector cross product operation, , , , , , , are intermediate variables in the iterative calculation process.

2. The high-frequency sampling motion error compensation method according to claim 1, wherein In Step 3, after the corresponding , , , and are calculated according to Step 2 for the fifth group of data, the following combined calculation is then performed: , , , ​ ； ； Among them, is the 5ms angular increment information after conical compensation, is the 5ms speed increment information after oar compensation.

3. The high-frequency sampling motion error compensation method according to claim 2, wherein, After obtaining the angular increment and speed increment information for 5 ms, the relevant cumulative variables are cleared so that the 10 sampled data for the next 5 ms can be calculated again. Among them, the variables to be cleared are , , , .

4. A motion error compensation system with high-frequency sampling, characterized in that, Including: Acquisition unit: High-frequency acquisition of the original data of the inertial measurement unit; Compensation unit: Group the acquired data and perform high-frequency coning motion compensation and oar effect compensation pairwise; Synthesis unit: Combine the results of pairwise compensation calculations after grouping into low-frequency data; The acquisition unit acquires the original data of the inertial measurement unit at high frequency in the way of 200Hz interruption, and the data of the inertial measurement unit is continuously acquired 10 times after entering one interruption; The compensation unit divides the high-frequency angular rate data in the acquired original data into five groups, that is, the 10 data are numbered from 1 to 10 respectively, and the i-th and the (i + 1)-th data are in one group, and a total of 5 groups are decomposed, where i = 1, 3, 5, 7, 9; The following formula calculations are performed on each group of data: ； ； ； ； ； ； ； ； ； Wherein, is the i-th angular increment data, is the (i + 1)-th angular increment data, is the sum of the current group of angular increment data, is the i-th velocity increment data, is the (i + 1)-th velocity increment data, is the sum of the current group of velocity increment data, is the vector cross product operation, and and and and and and are intermediate variables in the iterative calculation process.

5. The high-frequency sampling motion error compensation system according to claim 4, wherein, After the synthesis unit calculates the corresponding , , , according to the fifth group of data in the compensation unit, the following combined calculation is performed on the data calculated by the compensation unit: ； ； Among them, is the 5ms angular increment information after conical compensation, is the 5ms speed increment information after oar stroke compensation.

6. The high-frequency sampling motion error compensation system according to claim 5, wherein After the synthesis unit obtains the angular increment and velocity increment information of 5 ms, the relevant cumulative variables are cleared so that the 10 sampling data of the next 5 ms can be calculated again. Among them, the variables to be cleared are , , , .

Images