Motion error compensation method and system for high-frequency sampling

Abstract

The present invention discloses a high-frequency sampling motion error compensation method and system. The method comprises: step 1: acquiring raw data from an inertial measurement unit at high frequency; step 2: grouping the acquired data and performing high-frequency coning motion compensation and paddling effect compensation on the data in pairs; and step 3: combining the grouped pairwise compensation results into low-frequency data. The present invention achieves high-frequency compensation for coning and paddling errors, making the navigation system suitable for complex maneuvers. Furthermore, by utilizing 200 interrupts, IMU error compensation and navigation solutions can be completed within the interrupt time, reducing frequent stack operations and improving system stability.

Description

Technical Field

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

[0002] In inertial navigation, when the carrier has high-frequency angular vibration or complex motion, the traditional first-order integration algorithm will produce 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, existing inertial navigation technologies often use single- or double-pattern compensation methods to compensate for coning and paddling motion. In terms of compensation algorithm accuracy, the error drift coefficient for single-pattern compensation is approximately 1 / 12, and for double-pattern compensation, approximately 1 / 960. This results in a certain degree of compensation algorithm error in medium- and high-precision applications. While cross-pattern compensation reduces the error drift coefficient to 1.3E-27, it requires a significant amount of computation, making it difficult to implement in engineering applications. 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 amount of system calculation while obtaining a high-precision compensation effect.

[0005] In order to solve the above technical problems, an embodiment of the present invention proposes a motion error compensation method for high-frequency sampling, comprising:

[0006] Step 1: Collect raw data from the inertial measurement unit at high frequency;

[0007] Step 2: Group the collected data and perform high-frequency cone motion compensation and paddling effect compensation on each pair;

[0008] Step 3: Combine the results of the pairwise compensation calculation after grouping into low-frequency data.

[0009] Accordingly, an embodiment of the present invention further provides a high-frequency sampling motion error compensation system, comprising:

[0010] Acquisition unit: collects raw data from the inertial measurement unit at high frequency;

[0011] Compensation unit: groups the collected data and performs high-frequency cone motion compensation and paddling effect compensation on two-by-two data.

[0012] Synthesis unit: merges the results of the pairwise compensation calculation after grouping into low-frequency data.

[0013] The beneficial effects of the present invention are as follows: the present invention realizes high-frequency compensation of 2000Hz IMU raw data by using 200Hz interruption, thereby avoiding the complex calculation amount of cross-pattern compensation. The present invention decomposes it into 5 similar double-word compensations plus an accumulation splicing operation, which greatly reduces the amount of calculation without losing the 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 suitable for complex maneuvering situations; on the other hand, by using 200 interruption, IMU error compensation and navigation solution can be completed within the interruption time, reducing the frequent operation of the stack, which is beneficial to the stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 4 is a flow chart of a motion error compensation method for high-frequency sampling according to an embodiment of the present invention. DETAILED DESCRIPTION

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

[0016] In the embodiments of the present invention, if there are directional indications (such as up, down, left, right, front, back, etc.), they are only used to explain the relative position relationship and movement status of the various components under a specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0017] In addition, the terms "first," "second," and so on, used in this disclosure are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features being referred to. Therefore, features specified as "first" or "second" may explicitly or implicitly include at least one of these features.

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

[0019] Step 1: Collect raw data from the inertial measurement unit at high frequency. In step 1, the data collection strategy used is: adopt a 200Hz interrupt, that is, the inertial measurement unit's inertial update period is 200Hz, which means the output frequency of the angular increment and velocity increment is 200Hz. The gyroscope data of the inertial measurement unit is collected through synchronous handshake at a frequency of 2000Hz. At the same time, the accelerometer data of the inertial measurement unit is collected using the rising edge of the same frequency, basically ensuring the synchronization of the gyroscope output data and the accelerometer output data under 2000Hz conditions. The FPGA temporarily stores the collected gyroscope and accelerometer data in the FIFO. In the embedded software, the interrupt is 200Hz. After entering an interrupt, it will collect 10 consecutive times. The 10 times of inertial measurement unit data collected by the FPGA at high frequency are read at one time and assigned to the global variable.

[0020] Step 2: Group the collected data and perform high-frequency cone motion compensation and paddling effect compensation on each of them. The specific calculation is as follows:

[0021] The collected high-frequency angular rate data is divided into five groups. That is, 10 data are numbered from 1 to 10, and the i-th and i+1-th data are grouped together, i = 1, 3, 5, 7, 9. A total of 5 groups are decomposed.

[0022] The following formula is calculated for each set of data:

[0023] ;

[0024] ;

[0025] ;

[0026] ;

[0027] ;

[0028] ;

[0029] ;

[0030] ;

[0031] ;

[0032] in, is the i-th angle increment data, is the i+1th angular increment data, is the current group angle increment data and, is the i-th speed increment data, is the i+1th speed increment data, The current group speed increment data and, is the vector cross product operation, 、 、 、 、 、 、 It is an intermediate variable in the iterative calculation process.

[0033] Step 3: Combine the results of the two-by-two compensation calculations after grouping into low-frequency data. The timing of this calculation is: after the fifth group of data is calculated according to step 2, 、 、 、 , and then perform the following combined calculation. After completing the iteration of the fifth set of data, the core intermediate variables 、 、 、 The cumulative sum calculation has been completed.

[0034] ;

[0035] ;

[0036] in, It is the 5ms angular increment information after cone compensation. It is the 5ms speed increment information after paddling compensation.

[0037] After obtaining the 5ms angular increment and velocity increment information, the relevant accumulated variables need to be cleared to allow for the next 5ms 10 sampling data to be recalculated. The variables to be cleared are 、 、 、 .

[0038] 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.

[0039] Acquisition unit: collects raw data from the inertial measurement unit at high frequency.

[0040] Compensation unit: Groups the collected data and performs high-frequency cone motion compensation and paddling effect compensation on two by two.

[0041] Synthesis unit: merges the results of the pairwise compensation calculation after grouping into low-frequency data.

[0042] As an implementation manner, the acquisition unit acquires raw data of the inertial measurement unit at a high frequency using a 200 Hz interruption method, and continuously acquires data of the inertial measurement unit 10 times after entering an interruption.

[0043] As an implementation method, the compensation unit divides the high-frequency angular rate data in the collected raw data into five groups, that is, 10 data are numbered from 1 to 10, the i-th and i+1-th data are grouped together, and a total of 5 groups are decomposed. i=1, 3, 5, 7, 9;

[0044] The following formula is calculated for each set of data:

[0045] ;

[0046] ;

[0047] ;

[0048] ;

[0049] ;

[0050] ;

[0051] ;

[0052] ;

[0053] ;

[0054] in, is the i-th angle increment data, is the i+1th angular increment data, is the current group angle increment data and, is the i-th speed increment data, is the i+1th speed increment data, The current group speed increment data and, is the vector cross product operation, 、 、 、 、 、 、 It is an intermediate variable in the iterative calculation process.

[0055] As an embodiment, the synthesis unit calculates the corresponding 、 、 、 Finally, the data obtained by the compensation unit is combined and calculated as follows:

[0056] ;

[0057] ;

[0058] in, It is the 5ms angle increment information after cone compensation. It is the 5ms speed increment information after paddling compensation.

[0059] As an implementation method, after the synthesis unit obtains the 5ms angle increment and velocity increment information, the relevant accumulated variables are cleared to enable the next 5ms 10 sampling data to be recalculated, wherein the cleared variables are 、 、 、 .

[0060] This invention, through the synergy of hardware capabilities and algorithmic intelligence, reconstructs real motion from noise. It can be applied to precise drone hovering (compensating for attitude jitter caused by high-frequency rotor vibration) and high-dynamic vehicle navigation (compensating for high-frequency angular motion). In high-precision fields such as autonomous driving and aerospace, this invention can achieve centimeter-level positioning and milliradian-level attitude accuracy.

[0061] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A motion error compensation method for high-frequency sampling, characterized in that: include: Step 1: Collect raw data from the inertial measurement unit at high frequency; Step 2: Group the collected data and perform high-frequency cone motion compensation and paddling effect compensation on each pair; Step 3: Combine the results of the pairwise compensation calculation after grouping into low-frequency data; In step 1, the raw data of the inertial measurement unit is collected at a high frequency using a 200 Hz interruption method. After entering an interruption, the data of the inertial measurement unit is collected continuously for 10 times. In step 2, the high-frequency angular rate data in the collected raw data are divided into five groups, that is, 10 data are numbered from 1 to 10, the i-th and i+1-th data are grouped together, and a total of 5 groups are decomposed, i=1, 3, 5, 7, 9; The following formula is calculated for each set of data: ； ； ； ； ； ； ； ； ； in, is the i-th angle increment data, is the i+1th angular increment data, is the current group angle increment data and, is the i-th speed increment data, is the i+1th speed increment data, The current group speed increment data and, is the vector cross product operation, 、 、 、 、 、 、 It is an intermediate variable in the iterative calculation process.

2. The high-frequency sampling motion error compensation method according to claim 1, wherein: In step 3, the corresponding 、 、 、 Then, perform the following combined calculation: ； ； in, It is the 5ms angle increment information after cone compensation. It is the 5ms speed increment information after paddling compensation.

3. The high-frequency sampling motion error compensation method according to claim 2, wherein: After obtaining the 5ms angular increment and velocity increment information, the relevant accumulated variables are cleared to allow the next 5ms 10 sampling data to be recalculated. The cleared variables are 、 、 、 .

4. A high-frequency sampling motion error compensation system, characterized in that: include: Acquisition unit: collects raw data from the inertial measurement unit at high frequency; Compensation unit: groups the collected data and performs high-frequency cone motion compensation and paddling effect compensation on two-by-two data. Synthesis unit: combines the results of the two-by-two compensation calculations after grouping into low-frequency data; The acquisition unit uses a 200Hz interruption method to collect the raw data of the inertial measurement unit at a high frequency. After entering an interruption, the data of the inertial measurement unit is collected continuously for 10 times. The compensation unit divides the high-frequency angular rate data in the collected raw data into five groups, that is, 10 data are numbered from 1 to 10, the i-th and i+1-th data are grouped together, and a total of 5 groups are decomposed, i=1, 3, 5, 7, 9; The following formula is calculated for each set of data: ； ； ； ； ； ； ； ； ； in, is the i-th angle increment data, is the i+1th angular increment data, is the current group angle increment data and, is the i-th speed increment data, is the i+1th speed increment data, The current group speed increment data and, is the vector cross product operation, 、 、 、 、 、 、 It is an intermediate variable in the iterative calculation process.

5. The high-frequency sampling motion error compensation system according to claim 4, wherein: The synthesis unit calculates the corresponding 、 、 、 Finally, the data obtained by the compensation unit is combined and calculated as follows: ； ； in, It is the 5ms angle increment information after cone compensation. It is the 5ms speed increment information after paddling compensation.

6. The high-frequency sampling motion error compensation system according to claim 5, wherein: After the synthesis unit obtains the 5ms angle increment and velocity increment information, the relevant accumulated variables are cleared to allow the next 5ms 10 sampling data to be recalculated. The variables that are cleared are 、 、 、 .

Images