An IMU installation angle estimation method based on gravity projection and displacement recursion

Abstract

This invention provides an IMU mounting angle estimation method based on gravity projection and displacement recursion, comprising: establishing a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state; when the vehicle is stationary, verifying the validity of the accelerometer measurements through the consistency of gravity vector magnitude, and simultaneously establishing constraint equations with the gravity projection to obtain the roll and pitch angles; during the short-term straight-line movement phase of the vehicle, performing mechanical choreography to obtain the horizontal displacement in the IMU coordinate system, and solving for the heading mounting angle based on the geometric correlation between the horizontal displacement and the heading mounting angle. This invention achieves integrated estimation of the three mounting angles by solving the roll and pitch mounting angles in the stationary phase and the heading mounting angle after startup, adapting to the initial calibration scenario of vehicle inertial navigation, eliminating the need for external references such as GNSS and odometers, and significantly simplifying the implementation conditions of mounting angle calibration.

Description

Technical Field

[0001] This invention relates to the field of integrated navigation technology, and in particular to an IMU installation angle estimation method based on gravity projection and displacement recursion. Background Technology

[0002] As the core sensing component of an inertial navigation system (INS), the Inertial Measurement Unit (IMU) typically forms a GNSS-INS fusion navigation architecture with the Global Navigation Satellite System (GNSS). By fusing angular motion measurement data from gyroscopes and linear motion measurement data from accelerometers, it achieves real-time, high-precision calculation of vehicle attitude, position, and motion state, playing an irreplaceable role in scenarios such as autonomous driving and navigation of special vehicles. GNSS provides absolute position and velocity references, effectively suppressing the accumulation of errors in the IMU, while the IMU can maintain short-term navigation accuracy when GNSS signals are interrupted. The synergy between the two depends on the consistent matching of data—among which, the installation angles (roll, pitch, and yaw angles) between the IMU and the vehicle coordinate system are key parameters determining the fusion effect.

[0003] Mounting angle deviations can cause distortions in the projection of physical quantities output by the IMU across coordinate systems. For example, a heading mounting angle offset can cause a deviation between the heading angle measured by the IMU and the actual direction of vehicle movement, leading to a misalignment between GNSS position updates and IMU motion predictions. Roll and pitch mounting angle errors can distort the gravity and motion acceleration components sensed by the accelerometer, causing attitude drift, accumulated position estimation errors, and ultimately compromising the stability and accuracy of the GNSS-INS fusion system. Therefore, even if the vehicle system is equipped with a GNSS module as standard, mounting angle calibration is still required after the IMU is installed to ensure spatial consistency between the two data sets, laying the foundation for reliable navigation in complex environments.

[0004] Existing methods for estimating the installation angle of vehicle-mounted IMUs mainly fall into two categories: One type is the external reference fusion method. This method uses the GNSS already equipped in the vehicle system as the core reference, combined with additionally deployed wheel speed odometers, visual sensors, and other equipment. Through data fusion algorithms such as Kalman filtering and extended Kalman filtering, it establishes an error model between IMU measurements and external reference information (such as GNSS positioning results and odometer speed data), and then inversely calculates the roll, pitch, and yaw angles of installation. Its core logic is to utilize the absolute reference characteristics of external sensors to correct the relative measurement deviation of the IMU, thereby achieving high-precision estimation of the installation angle.

[0005] Another type is the IMU static self-calibration method. This method relies solely on the IMU's own static phase data. Based on the projection geometry of the accelerometer output and the gravity vector, it solves for the horizontal mounting angle (roll and pitch angles) through trigonometric function calculations. Specifically, the accelerometer output in a static state mainly reflects the projection components of gravity on each axis of the IMU coordinate system. By utilizing the directional characteristics of the gravity vector, a constraint equation for the horizontal mounting angle can be established, and then the solution can be completed. However, since there is no horizontal motion excitation when stationary, this type of method cannot obtain the heading mounting angle, and additional calibration is required by manually aligning the vehicle's heading with a reference direction.

[0006] Both of the above-mentioned existing technologies have obvious limitations: For external reference fusion methods, although GNSS is a standard module in vehicle navigation and positioning scenarios and does not require additional deployment, this method must rely on additional hardware devices such as wheel speed odometers, which increases the cost of vehicle hardware procurement, installation and debugging, and increases the integration complexity of the system. More importantly, its performance is heavily dependent on the quality of GNSS signals. In scenarios where GNSS signals are blocked or fail, such as tunnels, underground parking garages, and densely populated areas with tall buildings, external reference information is interrupted, and the installation angle estimation will completely fail, which cannot meet the calibration requirements of complex vehicle environments.

[0007] While the static self-calibration method for IMUs is low-cost and easy to operate, it has functional limitations. It can only solve for the roll and pitch angles, but cannot obtain the heading angle. The heading angle directly affects the vehicle's orientation positioning accuracy, and the lack of this parameter will result in insufficient completeness of the IMU's attitude calculation. In addition, the manual calibration of the heading angle is prone to human error and is inefficient, making it difficult to adapt to large-scale and automated vehicle IMU calibration scenarios.

[0008] In summary, existing technologies generally suffer from the following drawbacks: high hardware costs, poor adaptability to various scenarios or incomplete measurement parameters, and low levels of automation. These limitations make it difficult to meet the needs of practical applications. Summary of the Invention

[0009] This invention provides an IMU mounting angle estimation method based on gravity projection and displacement recursion to address the shortcomings of existing technologies, such as high hardware costs, poor scene adaptability, incomplete measurement parameters, and low automation. It enables the verification of the validity of accelerometer measurements through the consistency of gravity vector magnitude, preventing the calculation of roll and pitch mounting angles using erroneous accelerometer measurements.

[0010] In a first aspect, the present invention provides a method for estimating the IMU mounting angle based on gravity projection and displacement recursion, comprising: Establish a zero-bias estimation model for the vehicle's gyroscope and a correlation model between the accelerometer and gravity projection in a stationary state, respectively. When the vehicle is stationary, the accelerometer measurement is verified to be valid by the consistency of the gravity vector magnitude. The roll angle and pitch angle are obtained by combining the constraint equations with the gravity projection. During the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric relationship between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

[0011] According to the present invention, an IMU mounting angle estimation method based on gravity projection and displacement recursion is provided, which establishes a gyroscope zero-bias estimation model for a vehicle and an accelerometer-gravity projection correlation model under stationary conditions, including: A gyroscope measurement model is constructed, and the average value of the gyroscope output is calculated when the vehicle is stationary. The zero bias of each axis of the gyroscope is obtained, and the measurement data is compensated and discretized. Based on the correlation model of accelerometer and gravity projection under static conditions, the projection of gravity in the IMU frame is derived. By solving the constraint equations, the roll and pitch installation angles are calculated.

[0012] According to the present invention, an IMU mounting angle estimation method based on gravity projection and displacement recursion is provided. When the vehicle is stationary, the validity of the accelerometer measurement value is verified by the consistency of gravity vector magnitude, including: The validity of the data was verified by the consistency of the gravity vector magnitude. Under static conditions, the vector magnitude of the denoised accelerometer output is approximately equal to the magnitude of gravitational acceleration, i.e.:

[0013] Define normalization error:

[0014] like If the data is valid, it will be used for subsequent installation angle calculation; otherwise, data from the stationary phase will be collected again.

[0015] According to the present invention, an IMU installation angle estimation method based on gravity projection and displacement recursion is provided. This method, combined with the constraint equations of gravity projection, yields the roll angle and pitch angle, including: After the data verification is successful, the pitch angle The solution is achieved through the following formula:

[0016] Since the range of values ​​for the pitch angle is Within this interval, there is always Taking the square root of the above equation, we get:

[0017] The expressions for the sine and cosine of the pitch angle are:

[0018] The formula for calculating the pitch angle is derived through continuous derivation:

[0019] This formula requires no small-angle assumptions, is applicable to any installation orientation, and its output angle range is exactly [value missing]. This perfectly matches the range of values ​​for the pitch angle; The expressions for the sine and cosine of the roll angle are:

[0020] The formula for calculating the roll angle is derived through continuous derivation:

[0021] in The quadrant of the angle is automatically determined based on the sign of the input parameters, and the output angle range is exactly [value missing]. It can fully cover all possible values ​​of the roll angle.

[0022] According to the present invention, an IMU mounting angle estimation method based on gravity projection and displacement recursion is provided. During the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system, including: Let the actual horizontal acceleration of the vehicle be... According to the horizontal plane The rotational relationship of the shaft, the horizontal acceleration component measured by the IMU ( The mapping relationship between the actual acceleration of the vehicle and the acceleration of the vehicle is as follows:

[0023] In the formula, For example, the rotation matrix in the horizontal plane is used to transform the acceleration vector in the vehicle coordinate system to the IMU coordinate system. The influence of measurement noise is temporarily ignored here. The noise interference will be further suppressed later through the mean effect in the integration process. By performing a second integration on the horizontal acceleration measured by the IMU, the horizontal displacement in the IMU coordinate system can be obtained. Because when a vehicle travels in a straight line, the actual horizontal displacement is only along... The coordinate system transformation relationship for the axis and displacement components is consistent with that for acceleration, that is:

[0024] in, This represents the actual horizontal displacement of the vehicle. and It needs to be calculated by acceleration integration, and the zero bias of the accelerometer's horizontal axis must be deducted before integration to avoid the integral drift caused by the zero bias affecting the displacement accuracy.

[0025] The present invention provides a method for estimating the IMU mounting angle based on gravity projection and displacement recursion, which solves for the mounting angle based on the geometric relationship between horizontal displacement and heading angle, including: Starting from the horizontal displacement correlation formula, due to the actual horizontal displacement of the vehicle Dividing the two equations will eliminate the error. ,get:

[0026] Taking the arctangent function of both sides of the equation, we obtain the preliminary formula for calculating the heading angle:

[0027] Using the four-quadrant arctangent function The corrected formula for calculating the heading installation angle is as follows:

[0028] Output range is This precisely covers all possible values ​​of the heading installation angle.

[0029] Secondly, the present invention also provides an IMU installation angle estimation system based on gravity projection and displacement recursion, comprising: A module is established to create a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state, respectively. The calculation module is used to verify the validity of the accelerometer measurement value by the consistency of the gravity vector magnitude when the vehicle is stationary, and to obtain the roll angle and pitch angle by combining the constraint equations with the gravity projection. The estimation module is used to perform mechanical choreography during the short-term straight-line movement of the vehicle to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric relationship between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

[0030] Thirdly, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the IMU mounting angle estimation method based on gravity projection and displacement recursion as described above.

[0031] Fourthly, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the IMU mounting angle estimation method based on gravity projection and displacement recursion as described above.

[0032] The IMU installation angle estimation method based on gravity projection and displacement recursion provided by this invention achieves integrated estimation of the three installation angles by calculating the roll and pitch installation angles during the static phase and the heading installation angle after startup. It is suitable for the initial calibration scenario of vehicle inertial navigation, and does not require external references such as GNSS or odometers, which significantly simplifies the implementation conditions of installation angle calibration. Attached Figure Description

[0033] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0034] Figure 1 This is a flowchart illustrating the IMU installation angle estimation method based on gravity projection and displacement recursion provided by the present invention. Figure 2 This is a schematic diagram of the installation angle of the GNSS / INS integrated navigation vehicle IMU provided by the present invention; Figure 3 This is a schematic diagram showing the correlation between accelerometer measurements and gravity projection provided by the present invention; Figure 4 This is a schematic diagram showing the relationship between the inertial navigation recursive trajectory and the heading installation angle provided by the present invention; Figure 5 This is a schematic diagram of the structure of the IMU installation angle estimation system based on gravity projection and displacement recursion provided by the present invention; Figure 6 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0035] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0036] Figure 1 This is a flowchart illustrating the IMU installation angle estimation method based on gravity projection and displacement recursion provided in an embodiment of the present invention, as shown below. Figure 1 As shown, it includes: Step 100: Establish the gyroscope zero-bias estimation model and the accelerometer-gravity projection correlation model in a stationary state for the vehicle, respectively; Step 200: When the vehicle is stationary, verify the validity of the accelerometer measurement by the consistency of the gravity vector magnitude, and combine it with the constraint equations of the gravity projection to obtain the roll angle and pitch angle; Step 300: During the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric relationship between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

[0037] Specifically, this embodiment of the invention first establishes a gyroscope zero-bias estimation model. During the vehicle's stationary phase, the mean value of the gyroscope output is calculated to obtain the zero bias of each axis of the gyroscope, and the measurement data is compensated. Then, a correlation model between the accelerometer and the gravity projection is established in the stationary state. After denoising the accelerometer output, the rotation matrix from the vehicle frame to the IMU frame is constructed, and the projection of gravity in the IMU frame is derived; the roll and pitch installation angles are solved. When the vehicle is stationary, the validity of the accelerometer measurement value is verified by the consistency of the gravity vector magnitude. Constraint equations are solved simultaneously with the gravity projection to obtain the roll and pitch installation angles; finally, the heading installation angle is solved. During the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric correlation between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

[0038] This invention innovatively verifies the validity of accelerometer measurements by verifying the consistency of gravity vector magnitude when statically solving for roll and pitch installation angles. This prevents the use of erroneous accelerometer measurements to calculate roll and pitch installation angles. Previous studies have not taken this into account when solving for roll and pitch installation angles, which may lead to errors in the calculation of roll and pitch installation angles.

[0039] Based on the above embodiments, step 100 includes: A gyroscope zero-bias estimation model was established. The mean value of the gyroscope output was calculated during the vehicle's stationary phase to obtain the zero bias of each axis of the gyroscope and to compensate for the measurement data.

[0040] The zero bias of a MEMS gyroscope is one of the main error sources affecting the accuracy of inertial navigation. It is defined as the output offset (constant error) of the gyroscope when there is no rotational input. In the initial installation angle estimation of a vehicle, accurate estimation of the gyroscope's zero bias can effectively eliminate rotational errors in the static stage, laying the foundation for subsequent dynamic recursion. This section derives the zero bias estimation formula based on the gyroscope output when the vehicle is stationary.

[0041] (1) Gyroscope measurement model When the vehicle is stationary, the IMU has no rotational motion, and the ideal output of the gyroscope should be 0. However, due to manufacturing process errors and environmental interference in MEMS devices, the actual output includes zero bias and random noise. Its measurement model can be expressed as: (1) in: for The IMU gyroscope at the moment The raw output of the axis ( (corresponding to forward, right, and down directions respectively). For the gyroscope Zero offset of the shaft (constant error, does not change with time); The measurement noise of the gyroscope (random error, with a mean of 0 and a variance of 0) (Gaussian distribution).

[0042] The core assumption of this model is that the output of the gyroscope in a static state consists only of zero bias and noise, with no rotational angular velocity input. Therefore, noise can be suppressed and zero bias extracted by time averaging.

[0043] (2) Derivation of the zero bias estimation formula The core idea of ​​zero-bias estimation is: during the static observation period Inside (from) arrive The gyroscope output is integrated over time and averaged. Since the noise mean is 0, the cumulative effect of the noise is suppressed after integration, thus obtaining a zero-biased unbiased estimate.

[0044] For both sides of the measurement model Integral over the interval: (2) Analyze the two terms on the right side of the equation: 1. First item: Since it is a constant, the integral result is: ; 2. Second item: The mean is 0, when the observation duration is Over a sufficiently long time (usually 10 seconds), the integral result The impact of noise is negligible.

[0045] Substituting the above result into the integral, and dividing both sides by... The gyroscope's first... Estimation formula for zero axis offset: (3) in, Indicates the gyroscope's first The estimated value of the axis zero bias.

[0046] (3) Zero bias compensation and discretization implementation By compensating the original gyroscope output with the zero-bias estimate, the effective angular velocity output after removing the zero bias can be obtained. The compensation formula is as follows: (4) When the vehicle is stationary, the compensated output It should be approximately 0, containing only random noise, to verify the effectiveness of the zero-biased estimation.

[0047] In practical engineering applications, IMU data is acquired through discrete sampling, requiring the continuous integral formula to be discretized. Let the sampling frequency during the stationary phase be... (Sampling interval) ), then in Collected within the time period There are 10 data points. At this point, the discretization formula for the zero-biased estimate is: (5) in, Indicates the first The gyroscope at the sampling point The original output value of the axis. This discretization formula is easy to implement in embedded systems and is a commonly used zero-bias estimation method in engineering.

[0048] Establish a correlation model between the accelerometer and the gravity projection under stationary conditions. After denoising the accelerometer output, construct the rotation matrix from the vehicle frame to the IMU frame, and derive the gravity projection in the IMU frame.

[0049] like Figure 2 In the schematic diagram of the GNSS / INS integrated navigation vehicle-mounted IMU installation angle shown, the vehicle coordinate system is called the v system, and the carrier (IMU) coordinate system is called the b system. The XYZ axes of the v system point to the lower right front of the vehicle body, while the XYZ axes of the b system do not coincide with the lower right front of the vehicle body, but form an angle. Therefore, it is necessary to construct a rotation matrix from the vehicle system to the IMU system.

[0050] When the vehicle is stationary, the IMU has no translational acceleration, and the accelerometer output only reflects the projection of gravity in the IMU coordinate system (ignoring the influence of zero bias). This section derives the mathematical relationship between the accelerometer measurement and the gravity vector when the vehicle is stationary, based on the relationship between coordinate system rotation and gravity projection, providing a theoretical basis for the subsequent calculation of roll and pitch angles.

[0051] (1) Accelerometer measurement model (stationary state) When the vehicle is stationary, the translational acceleration of the IMU is 0, and the accelerometer output consists only of gravity projection and random noise. Its measurement model is as follows: (6) in: For a moment IMU accelerometer The raw output of the axis; For gravity in the IMU coordinate system The projected components of the axis (constant values, not changing with time). The measurement noise of the accelerometer (random error, with a mean of 0 and a variance of 0) (Gaussian distribution).

[0052] The core assumption of this model is that the output of the accelerometer in a stationary state is only related to the gravity projection and has no translational acceleration input. Therefore, noise can be suppressed by time averaging and the gravity projection component can be extracted.

[0053] (2) Accelerometer output noise reduction To suppress the impact of measurement noise on the extraction of gravity projection components, it is necessary to adjust the static duration. The accelerometer output within the model is time-averaged. The measurement model is then compared on both sides. Integrate over the interval and take the average: (7) Analyze the two terms on the right side of the equation: 1. First item: Since it is a constant, the integral result is: ; 2. Second item: The mean is 0, when For a sufficiently long time, The noise impact is negligible.

[0054] Therefore, the accelerometer output after noise reduction Approximately equal to gravity in the IMU coordinate system The projection components of the axis, namely: (8) in, These are the time averages of the forward, rightward, and downward outputs of the accelerometer, respectively. The denoised values ​​are used in the subsequent formula derivations.

[0055] (3) Coordinate system projection of the gravity vector According to the direction cosine matrix, the gravity vector in the vehicle coordinate system ( (system) and IMU coordinate system ( The projection relationships between systems follow standard vector transformation rules.

[0056] First, when the vehicle is stationary on a horizontal surface, gravity acts only along the vehicle's coordinate system. The force acts along the axis (downward). Therefore, the vector expression for gravity in the vehicle coordinate system is: (9) in, This is the magnitude of gravitational acceleration. This vector is only... The axle has a component, which is consistent with the force state of a vehicle on a horizontal ground (gravity and ground support force are balanced, and there is no horizontal component).

[0057] Next, we derive the projection of gravity onto the IMU coordinate system. The IMU accelerometer measures specific force, which is the non-gravitational external force per unit mass. In the IMU coordinate system, the specific force equation is: Absolute acceleration when the vehicle is stationary. The equation then simplifies to: (10) in, This is the accelerometer output in the IMU coordinate system after noise reduction and zero-bias calibration. It is the projection of the gravity vector into the IMU coordinate system. For example... Figure 3 In the schematic diagram showing the correlation between accelerometer measurements and gravity projection, the specific force output by the accelerometer is in the opposite direction to gravity. The specific force projection is obtained in the b-frame. , , That is the measurement from the accelerometer.

[0058] Based on the above formula, the component expressions of the gravity vector in the IMU coordinate system can be directly obtained: (11) Finally, based on the vector transformation relationship of the direction cosine matrix, the vector of gravity in the IMU coordinate system is... It can also be determined by its vector in the vehicle coordinate system. By rotation matrix (From the vehicle system to the IMU system) we get: (12) The above derivation establishes the measurement output of the IMU accelerometer and the mounting angle between the IMU and the vehicle (included in...). The direct mathematical connection between the two (in Chinese) laid the foundation for subsequent calculations of roll and pitch angles using static data.

[0059] (4) Expansion of the gravitational projection components The installation deviation of the IMU relative to the vehicle is described by three attitude angles, and the rotation sequence follows the commonly used engineering sequence of "yaw-pitch-roll". Agreement (conforming to the laws governing changes in vehicle motion posture): Heading installation angle : Relative Tie The rotation angle of the axis (downward); Pitch installation angle : Relative Tie Rotation angle of the axis (to the right); Roll installation angle : Relative Tie Rotation angle of the axis (forward).

[0060] Tie Vector transformation of the system is achieved through the composite direction cosine matrix. This matrix is ​​based on " The complete expression for the rotation order derivation is: (13) Will and Substituting into the vector transformation formula, we get: (14) The gravity projection components of each axis of the IMU coordinate system are obtained by unfolding. Because... Only The axis has a non-zero component (the third element is...). (The first two elements are 0), therefore matrix multiplication only requires calculating The third column and The product of, i.e.: (15) After processing, the relationship between the accelerometer output and the installation angle can be obtained: (16) The physical meaning of this expansion is: the accelerometer output (the projection of the supporting force opposite to gravity) is determined by the roll angle. Pitch angle With gravitational acceleration The decision was made jointly, and the specific analysis is as follows: 1. Forward ( (axis) output Only related to pitch angle Related, When the vehicle looks up ( When the vehicle tilts its head down, the forward accelerometer output is positive; when the vehicle tilts its head down (…), the forward accelerometer output is positive. When ), the forward accelerometer output is negative.

[0061] 2. To the right ( (axis) output : with roll angle and pitch angle All are related. When the vehicle tilts to the right ( When the vehicle tilts to the left, the right-hand accelerometer output is negative; when the vehicle tilts to the left... When ), the right-hand accelerometer output is positive.

[0062] 3. downward ( (axis) output : with roll angle and pitch angle All are related. ,because and The absolute values ​​of all of them are less than 1, therefore The absolute value is always less than And it decreases as the installation angle increases.

[0063] This expansion is the core equation for subsequent solutions to roll and pitch angles. By correlating accelerometer measurements with the installation angle, it enables a quantitative conversion from sensor data to attitude angles.

[0064] (5) Discretization implementation Similar to gyroscope bias estimation, accelerometer output denoising also requires a discretization formula. Assume data is collected during the stationary phase. If there are 10 data points, the discretization formula for the accelerometer output after denoising is: (17) in, Indicates the first The accelerometer at the sampling point is... The original output value of the axis. This formula is easy to implement in embedded systems. It can directly use the sampled data during the stationary phase to calculate the denoised accelerometer output, providing input for subsequent attitude angle calculations (note that this value is opposite to the direction of gravity projection).

[0065] Based on the above embodiments, step 200 includes: Solve for the roll and pitch installation angles. With the vehicle stationary, verify the validity of the accelerometer measurements by checking the consistency of the gravity vector magnitude. Solve for the roll and pitch installation angles by combining the constraint equations with the gravity projection.

[0066] Based on discretization processing , , The horizontal mounting angle can be solved using the geometric relationships of gravity projection. Since the IMU may be mounted at arbitrary angles (without the assumption of small angles), rigorous derivation of trigonometric functions is required to ensure estimation accuracy in large-angle scenarios.

[0067] First, the validity of the data is verified by the consistency of the gravity vector magnitude: in a static state, the vector magnitude of the accelerometer's noise-reduced output should be approximately equal to the magnitude of gravitational acceleration, i.e. (18) Define normalization error (19) like ( Typically, a value of 0.05 is used to determine if the data is valid (no significant interference or sensor malfunction) and can be used for subsequent installation angle calculations; otherwise, data from the stationary phase needs to be reacquired. This verification method requires no additional sensors, can assess data reliability in real time, and provides a quality screening mechanism for subsequent calculations.

[0068] After the data verification is successful, the pitch angle The solution can be achieved by summing the squares of the last two equations in equation (16): (20) Since the range of values ​​for the pitch angle is Within this interval, there is always Therefore, by directly taking the square root of the above equation, we get: (twenty one) The expressions for the sine and cosine of the pitch angle can be obtained as follows: (twenty two) The formula for calculating the pitch angle is derived through continuous derivation: (twenty three) This formula requires no small-angle assumptions, is applicable to any installation orientation, and its output angle range is exactly [value missing]. It perfectly matches the range of values ​​for the pitch angle.

[0069] The expressions for the sine and cosine of the roll angle are: (twenty four) The formula for calculating the roll angle is derived through continuous derivation: (25) in The system can automatically determine the quadrant of the angle based on the sign of the input parameters, and the output angle range is exactly [range missing]. It can fully cover all possible values ​​of the roll angle.

[0070] Based on the above embodiments, step 300 includes: To determine the heading installation angle, during the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric relationship between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

[0071] like Figure 4In the schematic diagram showing the relationship between the inertial navigation recursive trajectory and the heading installation angle, the heading installation angle... Defined as IMU coordinate system ( (system) relative to the vehicle coordinate system ( (system) around The rotation angle of the axis (downward) reflects the forward axis (of the IMU) rotation angle. ) and the vehicle's actual front axle ( Deviation in the horizontal plane. (And roll angle) Pitch angle Unlike in a static state, the gravitational vector acts only in the vertical direction, without an independent horizontal vector to assist it, and therefore cannot be directly solved using accelerometer data. This method utilizes the horizontal acceleration and angular velocity generated by the vehicle's short-term motion (such as straight-line travel) and combines this with the displacement recursion principle in inertial navigation to achieve estimation. This method relies solely on the IMU's own measurement data, requires no additional sensors, and is suitable for engineering application scenarios.

[0072] When the vehicle enters a short-duration motion phase (the duration of motion is denoted as...) If the vehicle is controlled to travel approximately in a straight line (without steering or lateral slip), then its actual direction of motion is along the vehicle coordinate system. Axis (forward), in the horizontal plane ( The acceleration of a plane is only along its direction. The axis has a component, and the angular velocity only revolves around the axis. Axial (downward) variation (angular velocity is 0 during uniform travel). Based on the geometric relationship of coordinate system rotation, the horizontal acceleration measured by the IMU and the subsequently recursively derived horizontal displacement need to be determined through the heading installation angle. It is related to the actual movement state of the vehicle.

[0073] Let the actual horizontal acceleration of the vehicle be... (only along) Axial acceleration, lateral acceleration According to the horizontal plane, The rotational relationship of the shaft, the horizontal acceleration component measured by the IMU ( The mapping relationship between the actual acceleration of the vehicle and the acceleration of the vehicle is as follows: (26) In the formula, This is a rotation matrix in the horizontal plane, which physically transforms the acceleration vector from the vehicle coordinate system to the IMU coordinate system. The influence of measurement noise is temporarily ignored here; noise interference will be further suppressed later through the mean effect during the integration process.

[0074] By performing a second integration on the horizontal acceleration measured by the IMU, the horizontal displacement in the IMU coordinate system can be obtained. Because when a vehicle travels in a straight line, the actual horizontal displacement is only along... axis( Lateral displacement The coordinate system transformation relationship of the displacement components is consistent with that of the acceleration, that is: (27) in, The actual horizontal displacement of the vehicle (along) axis); and It needs to be calculated by acceleration integration, and the zero bias of the accelerometer horizontal axis needs to be deducted before integration (the zero bias can be estimated by the average acceleration during the stationary phase) to avoid the integral drift caused by the zero bias affecting the displacement accuracy.

[0075] Starting from the horizontal displacement correlation formula, due to the actual horizontal displacement of the vehicle (Short-duration motion can ensure non-zero displacement, usually requiring...) To reduce the impact of noise, dividing the two equations will eliminate the problem. ,get: (28) Taking the arctangent function of both sides of the equation, we can obtain a preliminary formula for calculating the heading angle: (29) It should be noted that The function has limitations: its output range is limited to... (Only covers the first and fourth quadrants), cannot cover the heading installation angle. The full range. For example, (Second Quadrant) and (Fourth Quadrant) If the values ​​are the same, they will be misjudged as the same angle, causing the angle calculation to be inconsistent with the actual installation posture and destroying the physical meaning of the installation angle.

[0076] To solve the above problem, a four-quadrant arctangent function is required. Correction: This function checks the input parameters simultaneously. (Molecules) and The sign of the denominator directly determines the quadrant in which the angle lies. The corrected formula for calculating the heading installation angle is: (30) Output range is It precisely covers all possible values ​​of the heading installation angle, allowing for accurate differentiation. (First Quadrant) and (Second Quadrant) Different attitudes, such as (fourth quadrant), ensure the accuracy of the heading installation angle calculation when the vehicle is moving forward, fully meeting the engineering requirements of vehicle-mounted IMU calibration.

[0077] The IMU installation angle estimation system based on gravity projection and displacement recursion provided by the present invention will be described below. The IMU installation angle estimation system based on gravity projection and displacement recursion described below can be referred to in correspondence with the IMU installation angle estimation method based on gravity projection and displacement recursion described above.

[0078] Figure 5 This is a schematic diagram of the IMU installation angle estimation system based on gravity projection and displacement recursion provided in an embodiment of the present invention, as shown below. Figure 5 As shown, it includes: a setup module 51, a calculation module 52, and an estimation module 53, wherein: The establishment module 51 is used to establish the gyroscope zero-bias estimation model and the accelerometer-gravity projection correlation model in a stationary state, respectively; the calculation module 52 is used to verify the validity of the accelerometer measurement value by the consistency of the gravity vector magnitude when the vehicle is stationary, and to obtain the roll angle and pitch angle by solving the constraint equations together with the gravity projection; the estimation module 53 is used to perform mechanical arrangement during the short-time straight-line movement of the vehicle to obtain the horizontal displacement in the IMU coordinate system, and to solve the heading angle based on the geometric correlation between the horizontal displacement and the heading angle.

[0079] Figure 6 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 6 As shown, the electronic device may include: a processor 610, a communication interface 620, a memory 630, and a communication bus 640. The processor 610, communication interface 620, and memory 630 communicate with each other via the communication bus 640. The processor 610 can call logical instructions in the memory 630 to execute an IMU installation angle estimation method based on gravity projection and displacement recursion. This method includes: establishing a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state; verifying the validity of the accelerometer measurements by verifying the consistency of the gravity vector magnitude when the vehicle is stationary, and simultaneously establishing constraint equations with the gravity projection to obtain the roll angle and pitch angle; performing mechanical choreography during the short-term straight-line movement of the vehicle to obtain the horizontal displacement in the IMU coordinate system, and solving for the heading installation angle based on the geometric correlation between the horizontal displacement and the heading installation angle.

[0080] Furthermore, the logical instructions in the aforementioned memory 630 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0081] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the IMU installation angle estimation method based on gravity projection and displacement recursion provided by the above methods. The method includes: establishing a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state; when the vehicle is stationary, verifying the validity of the accelerometer measurement value through the consistency of gravity vector magnitude, and solving the constraint equations with the gravity projection to obtain the roll angle and pitch angle; during the short-time straight-line forward phase of the vehicle, performing mechanical choreography to obtain the horizontal displacement in the IMU coordinate system, and solving the heading installation angle based on the geometric correlation between the horizontal displacement and the heading installation angle.

[0082] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements the IMU installation angle estimation method based on gravity projection and displacement recursion provided by the above methods. The method includes: establishing a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state; when the vehicle is stationary, verifying the validity of the accelerometer measurement value through the consistency of gravity vector magnitude, and solving the constraint equations simultaneously with the gravity projection to obtain the roll angle and pitch angle; during the short-time straight-line forward phase of the vehicle, performing mechanical choreography to obtain the horizontal displacement in the IMU coordinate system, and solving the heading installation angle based on the geometric correlation between the horizontal displacement and the heading installation angle.

[0083] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0084] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0085] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An IMU installation angle estimation method based on gravity projection and displacement recursion, characterized in that, include: Establish a zero-bias estimation model for the vehicle's gyroscope and a correlation model between the accelerometer and gravity projection in a stationary state, respectively. When the vehicle is stationary, the accelerometer measurement is verified to be valid by the consistency of the gravity vector magnitude. The roll angle and pitch angle are obtained by combining the constraint equations with the gravity projection. During the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric relationship between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

2. The IMU installation angle estimation method based on gravity projection and displacement recursion of claim 1, wherein, A zero-bias estimation model for the vehicle's gyroscope and a correlation model between the accelerometer and gravity projection in a stationary state are established, including: A gyroscope measurement model is constructed, and the average value of the gyroscope output is calculated when the vehicle is stationary. The zero bias of each axis of the gyroscope is obtained, and the measurement data is compensated and discretized. Based on the correlation model of accelerometer and gravity projection under static conditions, the projection of gravity in the IMU frame is derived. By solving the constraint equations, the roll and pitch installation angles are calculated.

3. The IMU installation angle estimation method based on gravity projection and displacement recursion according to claim 1, characterized in that, When the vehicle is stationary, the accelerometer measurements are verified for validity by checking the consistency of the gravity vector magnitude, including: The validity of the data was verified by the consistency of the gravity vector magnitude. Under static conditions, the vector magnitude of the denoised accelerometer output is approximately equal to the magnitude of gravitational acceleration, i.e.: Define normalization error: like If the data is valid, it will be used for subsequent installation angle calculation; otherwise, data from the stationary phase will be collected again.

4. The IMU installation angle estimation method based on gravity projection and displacement recursion according to claim 3, characterized in that, By combining the constraint equations with the gravity projection, the roll angle and pitch angle are obtained, including: After the data verification is successful, the pitch angle The solution is achieved through the following formula: Since the range of values ​​for the pitch angle is Within this interval, there is always Taking the square root of the above equation, we get: The expressions for the sine and cosine of the pitch angle are: The formula for calculating the pitch angle is derived through continuous derivation: This formula requires no small-angle assumptions, is applicable to any installation orientation, and its output angle range is exactly [value missing]. This perfectly matches the range of values ​​for the pitch angle; The expressions for the sine and cosine of the roll angle are: The formula for calculating the roll angle is derived through continuous derivation: in The quadrant of the angle is automatically determined based on the sign of the input parameters, and the output angle range is exactly [value missing]. It can fully cover all possible values ​​of the roll angle.

5. The IMU installation angle estimation method based on gravity projection and displacement recursion according to claim 1, characterized in that, During the short-term straight-line movement of the vehicle, mechanical choreography is performed to obtain the horizontal displacement in the IMU coordinate system, including: Let the actual horizontal acceleration of the vehicle be... According to the horizontal plane The rotational relationship of the shaft, the horizontal acceleration component measured by the IMU ( The mapping relationship between the actual acceleration of the vehicle and the acceleration of the vehicle is as follows: In the formula, For example, the rotation matrix in the horizontal plane is used to transform the acceleration vector in the vehicle coordinate system to the IMU coordinate system. The influence of measurement noise is temporarily ignored here. The noise interference will be further suppressed later through the mean effect in the integration process. By performing a second integration on the horizontal acceleration measured by the IMU, the horizontal displacement in the IMU coordinate system can be obtained. Because when a vehicle travels in a straight line, the actual horizontal displacement is only along... The coordinate system transformation relationship for the axis and displacement components is consistent with that for acceleration, that is: in, This represents the actual horizontal displacement of the vehicle. and It needs to be calculated by acceleration integration, and the zero bias of the accelerometer's horizontal axis must be deducted before integration to avoid the integral drift caused by the zero bias affecting the displacement accuracy.

6. The IMU installation angle estimation method based on gravity projection and displacement recursion according to claim 5, characterized in that, Based on the geometric relationship between horizontal displacement and heading angle, the heading angle is solved, including: Starting from the horizontal displacement correlation formula, due to the actual horizontal displacement of the vehicle Dividing the two equations will eliminate the error. ,get: Taking the arctangent function of both sides of the equation, we obtain the preliminary formula for calculating the heading angle: Using the four-quadrant arctangent function The corrected formula for calculating the heading installation angle is as follows: Output range is This precisely covers all possible values ​​of the heading installation angle.

7. An IMU installation angle estimation system based on gravity projection and displacement recursion, characterized in that, include: A module is established to create a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state, respectively. The calculation module is used to verify the validity of the accelerometer measurement value by the consistency of the gravity vector magnitude when the vehicle is stationary, and to obtain the roll angle and pitch angle by combining the constraint equations with the gravity projection. The estimation module is used to perform mechanical choreography during the short-term straight-line movement of the vehicle to obtain the horizontal displacement in the IMU coordinate system. Based on the geometric relationship between the horizontal displacement and the heading installation angle, the heading installation angle is solved.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the IMU mounting angle estimation method based on gravity projection and displacement recursion as described in any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the IMU mounting angle estimation method based on gravity projection and displacement recursion as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the IMU mounting angle estimation method based on gravity projection and displacement recursion as described in any one of claims 1 to 6.

Images