An IMU installation angle estimation method based on velocity increment and confidence check

Abstract

The application provides an IMU installation angle estimation method based on speed increment and confidence check, comprising: establishing a gyroscope zero offset estimation model, performing mean value calculation on the gyroscope output in the vehicle static stage, obtaining the gyroscope axis zero offset and compensating the measurement data; establishing a correlation model of the accelerometer and the gravity projection in the static state, deducing the gravity projection in the IMU system, and solving the roll and pitch installation angles by simultaneously solving the constraint equations; when the vehicle acceleration is large, starting the inertial navigation recursion, solving the speed change, using the included angle of the speed change to solve the heading installation angle of the IMU at each epoch; through sequence convergence judgment and installation angle confidence check, taking the mean value of the latter half of the converged sequence as the final heading installation angle, and obtaining the complete estimation of the three-axis installation angle. The application solves the heading installation angle through the speed increment and the attitude constraint, realizes the integrated estimation of the three installation angles, and adapts to the initial calibration scene of the vehicle-mounted inertial navigation.

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 velocity increment and confidence verification. Background Technology

[0002] In vehicle-mounted integrated navigation systems, the inertial measurement unit (INS), as the core sensing element, typically forms a loosely or tightly coupled navigation architecture with the Global Navigation Satellite System (GNSS). By fusing the angular velocity information output by the gyroscope with the specific force information acquired by the accelerometer, the system can calculate the vehicle's attitude, velocity, and position information in real time, providing crucial support for applications such as intelligent driving and high-precision positioning. The GNSS-INS fusion scheme is widely adopted because it leverages the complementary advantages of both systems: satellite navigation provides long-term stable absolute position correction, suppressing the time accumulation error of the inertial navigation system; while the inertial navigation system, when satellite signal lock is lost, achieves short-term high-precision recursion through mechanical orchestration.

[0003] In this fusion system, spatial alignment between the IMU coordinate system and the vehicle coordinate system is a prerequisite for ensuring the effectiveness of data fusion. Any discrepancy in the installation angle between the two will directly lead to distortion of sensor observations during projection. Specifically, heading installation errors will cause the trajectory direction calculated by inertial propagation to be inconsistent with the actual driving direction, thus disrupting the temporal matching between position updates and motion predictions; pitch and roll installation errors will affect the decomposition of the gravity vector in accelerometer observations, leading to attitude calculation offsets and position drift, ultimately reducing the output quality and reliability of the entire integrated navigation system. Therefore, after the IMU is installed, its installation angle relative to the vehicle coordinate system must be accurately obtained through calibration to ensure spatial consistency in subsequent navigation calculations.

[0004] Currently, research on the estimation of the installation angle of vehicle-mounted IMUs has formed several mainstream technical paths, which can be mainly summarized into the following two categories: (1) Calibration method based on multi-source information fusion This type of method utilizes existing or additional sensing devices on the vehicle, such as wheel speed sensors, vision systems, or lidar, to establish an error observation equation based on GNSS observations. Through data fusion techniques such as Kalman filtering, the IMU output and reference sensor information are jointly estimated to achieve gradual convergence of the installation angle. The advantage of this method is that it can utilize dynamic information from the vehicle's normal driving process for estimation. However, its effective operation is highly dependent on the continuity and accuracy of external reference data, which not only increases the complexity of system hardware configuration and calibration but also renders the method ineffective in scenarios where GNSS is unavailable due to the lack of reference information.

[0005] (2) Self-calibration method based on motion constraints To reduce reliance on external sensors, another approach uses only IMU (Instrument Measure) data for mounting angle estimation. This method typically calculates roll and pitch mounting angles based on accelerometer output while the vehicle is stationary. Then, it uses the vehicle's linear acceleration from rest to motion to construct a planar trajectory through inertial integration, thereby estimating the heading mounting angle. This method requires no additional hardware and theoretically can achieve complete three-axis mounting angle calculations. However, it is strictly dependent on a specific motion pattern of "zero-speed start + linear acceleration." In scenarios with weak initial acceleration, significant noise interference, or when the vehicle is already moving and requires recalibration, this method faces issues of decreased accuracy or operational limitations, making it difficult to implement in routine applications.

[0006] In summary, current mainstream IMU installation angle calibration methods generally suffer from several practical bottlenecks: one type of solution faces challenges of insufficient system complexity and scenario adaptability due to its reliance on external sensors and GNSS signals; another type, while possessing independence, is stringent in its requirements for vehicle motion state settings, has a rigid process, and cannot flexibly adapt to diverse driving conditions. Therefore, existing technologies struggle to balance accuracy, robustness, and operability in practical deployments, necessitating the development of a calibration scheme with stronger generalization capabilities and practicality. Summary of the Invention

[0007] This invention provides an IMU installation angle estimation method based on speed increment and confidence level verification, which solves the defects of existing technologies such as high hardware cost, poor scene adaptability or dependence on specific startup conditions and cumbersome operation. The method realizes the installation angle verification by confidence level, which can determine whether the installation angle calculated each time is reliable based on the simple calculation of the installation angle, and directly discard the calculated installation angle when the confidence level is low.

[0008] In a first aspect, the present invention provides an IMU installation angle estimation method based on velocity increment and confidence level verification, 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 a large vehicle acceleration is detected, the inertial navigation system is activated to recursively solve for the velocity change. Using the angle between the velocity changes, the heading angle of the IMU is solved at each epoch. By determining the sequence convergence and verifying the confidence of the installation angle, the mean of the second half of the converged sequence is taken as the final heading installation angle, thus obtaining a complete estimate of the three-axis installation angle.

[0009] According to the present invention, an IMU mounting angle estimation method based on velocity increment and confidence level verification is provided, which establishes a gyroscope zero-bias estimation model for the 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.

[0010] According to the present invention, an IMU mounting angle estimation method based on velocity increment and confidence level verification is provided. When a large vehicle acceleration is detected, the inertial navigation system is initiated to recursively solve for the velocity change. Using the angle between the velocity changes, the heading mounting angle of the IMU is solved at each epoch, including: Determine the core estimation principle of inertial navigation recursion; Based on the core estimation principle, the coordinate system transformation relationship of the horizontal velocity change is obtained; The heading installation angle is solved by the coordinate system transformation relationship of the horizontal velocity change.

[0011] According to the present invention, an IMU installation angle estimation method based on velocity increment and confidence level verification is provided, which determines the core estimation principle of inertial navigation recursion, including: Complete roll installation angle With pitch installation angle After solving, the heading installation angle is ignored. The horizontal reference coordinate system for eliminating roll and pitch tilt deviations is defined as follows: Tie, Vehicle coordinate system The horizontal plane is parallel, with rotational deviation only in the heading direction. ; Establish IMU carrier coordinate system Set to the horizontal reference coordinate system The direction cosine matrix of the system This matrix is ​​derived solely from the roll installation angle obtained by solving. Pitch installation angle The decision is made without a heading angular component, based on the direction cosine matrix. After performing coordinate system transformation on the IMU measured vectors, we obtain:

[0012] Set acceleration threshold When the horizontal reference coordinate system The resultant acceleration amplitude under the following conditions satisfies At that time, start the process. By integral recursion, we obtain The velocity change vector in the system ; Define the vehicle coordinate system To the horizontal reference coordinate system The direction cosine matrix contains only the heading angle. The simplified direction cosine matrix is ​​denoted as: .

[0013] The present invention provides an IMU installation angle estimation method based on velocity increment and confidence level verification. Based on the core estimation principle, it obtains the coordinate system transformation relationship of the horizontal velocity change, including: When the vehicle is traveling in a straight line, the vehicle coordinate system The actual horizontal velocity change vector only along Since the axis has a component, and the lateral velocity change component is 0, the velocity change vector of the vehicle system can be represented as:

[0014] in, The actual change in the vehicle's forward speed satisfies And when the vehicle moves forward ; Horizontal reference coordinate system The conversion relationship between the velocity change vector of the vehicle system and the velocity change vector of the vehicle system is as follows:

[0015] In the formula, Horizontal reference coordinate system The velocity change vector.

[0016] The present invention provides an IMU installation angle estimation method based on velocity increment and confidence level verification, which solves for the heading installation angle by the coordinate system transformation relationship of the horizontal velocity change, including: The direction cosine matrix containing only the heading angle With respect to the velocity change vector of the vehicle system Substituting into the transformation formula, we can perform matrix multiplication:

[0017] Take the horizontal plane Expanding the components of the velocity change on the shaft yields explicit expressions for each component:

[0018] The vehicle meets the requirements during the straight-line driving phase. , being a non-zero scalar, solve for the heading installation angle. The expressions for sine and cosine:

[0019] Combining the trigonometric identities of the arctangent functions in the four quadrants, we obtain the formula for calculating the heading angle: .

[0020] According to the present invention, an IMU installation angle estimation method based on velocity increment and confidence verification is provided. Through sequence convergence determination and installation angle confidence verification, the mean of the latter half of the converged sequence is taken as the final heading installation angle, resulting in a complete estimation result of the three-axis installation angles, including: After completing the epoch-by-epoch calculation of the heading installation angle, the convergence of the continuously calculated heading installation angle sequence is determined. After the heading installation angle sequence converges, the installation angle confidence is verified. Perform final value calculation of heading installation angle and integration of three-axis installation angle.

[0021] According to the present invention, an IMU installation angle estimation method based on velocity increment and confidence verification is provided. After completing the epoch-by-epoch calculation of the heading installation angle, the convergence of the continuously calculated heading installation angle sequence is determined. After the heading installation angle sequence converges, the installation angle confidence verification is performed, including: Extract the heading installation angle calculation values ​​from 10 consecutive epochs to form a sequence, and determine the heading angle adjacent epoch deviation threshold. If the sequence simultaneously satisfies the following two criteria, then the heading installation angle sequence is considered convergent: Heading angle difference constraint for any adjacent epochs:

[0022] Overall amplitude fluctuation constraint of the sequence:

[0023] Determine the horizontal attitude perturbation threshold and heading and turning disturbance threshold The quantitative constraints are determined as follows:

[0024] In the formula, The total disturbance value of the horizontal attitude angle within 10 epochs represents the comprehensive fluctuation of the vehicle's roll and pitch attitude. , For the first Epoch in horizontal reference coordinate system The roll and pitch angles are obtained through recursion. The total heading angle over 10 epochs represents the degree of change in the vehicle's heading attitude. For the first Epoch Horizontal Reference Coordinate System The effective output value of the gyroscope Z-axis is as follows. The epoch sampling interval; A quantitative calculation model for the confidence level of the installation angle calculation results is constructed based on quantitative constraints. The reliability of the calculated installation angle value is comprehensively evaluated, and the calculation formula is as follows:

[0025] In the formula, The installation angle confidence level has a range of values. ; , The weighting coefficients are positive coefficients that match the dimensions of the angle threshold. They are tuned based on the influence of horizontal attitude disturbances and heading disturbances on the installation angle calculation in actual engineering scenarios, satisfying the following requirements. , Installation angle confidence The larger the value, the less the installation angle is affected by driving conditions, and the higher the reliability of the solution. Conversely, the smaller the value, the more the solution is affected by disturbances, and the lower the reliability. Set the confidence threshold for the installation angle If satisfied If the installation angle calculation result is not met, the result is deemed invalid and the set of calculated values ​​is discarded and will not be included in subsequent optimal value calculations; if the following conditions are met... If the result is positive, the installation angle calculation is considered valid, and the calculated value is retained and used in subsequent optimal value calculations.

[0026] According to the present invention, an IMU installation angle estimation method based on velocity increment and confidence level verification is provided, which performs final value calculation of the heading installation angle and integration of the three-axis installation angles, including: The heading and installation angle solutions for 10 convergent epochs are subjected to sliding window mean filtering. The mean of the heading and installation angles for the last 5 epochs is taken as the final optimal estimate of the heading and installation angle. The solution formula is as follows:

[0027] The roll installation angle is accurately calculated based on the gravity projection method. Pitch installation angle And the optimal estimate of the heading installation angle obtained after completing the convergence determination and confidence verification. Complete the estimation of the three-axis mounting angles of the IMU coordinate system relative to the vehicle body coordinate system. The three-axis mounting angle vector is represented as: .

[0028] Secondly, the present invention also provides an IMU installation angle estimation system based on velocity increment and confidence level verification, 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 detect when the vehicle acceleration is large, and then initiate the inertial navigation system to recursively solve for the velocity change. Using the angle between the velocity changes, the heading installation angle of the IMU is solved at each epoch. The estimation module is used to obtain the complete estimation result of the three-axis installation angle by taking the mean of the second half of the converged sequence as the final heading installation angle through sequence convergence determination and installation angle confidence verification.

[0029] The IMU mounting angle estimation method based on velocity increment and confidence verification provided by this invention achieves integrated estimation of the three mounting angles by calculating the roll and pitch mounting angles when stationary and calculating the heading mounting angle by velocity increment and attitude constraints. 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 mounting angle calibration. Attached Figure Description

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

[0031] Figure 1 This is a flowchart illustrating the IMU installation angle estimation method based on velocity increment and confidence verification 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 of the dynamic calculation and verification process for the installation angle of the vehicle-mounted IMU provided by the present invention; Figure 5 This is a schematic diagram of the structure of the IMU installation angle estimation system based on velocity increment and confidence verification 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

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

[0033] Figure 1 This is a flowchart illustrating the IMU installation angle estimation method based on velocity increment and confidence level verification provided in this embodiment of the invention. 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 a large vehicle acceleration is detected, the inertial navigation system is started to recursively solve for the velocity change. Using the angle between the velocity changes, the heading installation angle of the IMU is solved at each epoch. Step 300: Through sequence convergence determination and installation angle confidence verification, the mean of the second half of the converged sequence is taken as the final heading installation angle to obtain the complete estimation result of the three-axis installation angle.

[0034] 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 gyroscope axis, and the measurement data is compensated. Then, a correlation model between the accelerometer and the gravity projection in the stationary state is established. The projection of gravity in the IMU frame is derived, and the constraint equations are solved to obtain the roll and pitch installation angles. When a large vehicle acceleration is detected, the inertial navigation recursion is initiated to solve for the velocity change. Using the angle between the velocity changes, the heading installation angle of the IMU is solved at each epoch. Finally, through sequence convergence judgment and installation angle confidence verification, the mean value of the latter half of the converged sequence is taken as the final heading installation angle, obtaining a complete estimate of the three-axis installation angles.

[0035] Based on the above embodiments, step 100 includes: First, a gyroscope zero-bias estimation model is established. The mean value of the gyroscope output is calculated when the vehicle is stationary to obtain the zero bias of each axis of the gyroscope and compensate for the measurement data.

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

[0037] (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).

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

[0039] (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.

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

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

[0042] (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.

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

[0044] Then, a correlation model between the accelerometer and the gravity projection under static conditions is established. The projection of gravity in the IMU frame is derived, and the constraint equations are solved to obtain the roll and pitch installation angles.

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

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

[0047] (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).

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

[0049] (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 Long enough time, The noise impact is negligible.

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

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

[0052] 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).

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

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

[0055] (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).

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

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

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

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

[0060] (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).

[0061] (6) Solve for roll and pitch installation angles Pitch angle The solution can be achieved by summing the squares of the last two equations in equation (14): (18) Take the absolute value after taking the square root: (19) Divide the first equation of combination (16) with equation (19), and use... Within the range From the properties of, we can obtain: (20) This formula can be solved directly using the arctangent function, without requiring small angle assumptions, and is applicable to any installation orientation.

[0062] Roll angle The solution can be obtained by dividing the last two equations of equation (16): (twenty one) because The four-quadrant arctangent function needs to be used to ensure quadrant correctness: (twenty two) in The quadrant is automatically determined based on the input sign, and the output range exactly covers all possible values ​​of the roll angle.

[0063] Based on the above embodiments, step 200 includes: When a large vehicle acceleration is detected, the inertial navigation recursion is initiated to solve for the velocity change. Using the angle between the velocity changes, the heading installation angle of the IMU is solved at each epoch.

[0064] 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 other angles, the heading angle is a rotation angle in the horizontal plane and cannot be solved by gravity projection in a stationary state.

[0065] (1) Estimating the core principles Complete roll installation angle With pitch installation angle After solving the problem, first ignore the heading installation angle. The influence of roll and pitch deviations is eliminated, and a horizontal reference coordinate system is defined as follows: Tie, Vehicle coordinate system The horizontal plane is parallel, with rotational deviation only in the heading direction. .

[0066] Establish IMU carrier coordinate system Set to the horizontal reference coordinate system The direction cosine matrix of the system is denoted as This matrix is ​​derived solely from the roll installation angle obtained by solving. Pitch installation angle It is determined that there is no heading angle component. Based on this transformation matrix, the coordinate system transformation of the IMU measured vectors is completed, resulting in: (twenty three) To avoid problems such as weak velocity changes and insufficient accuracy in calculating the heading angle due to excessively small acceleration amplitude, an acceleration threshold is set. When the horizontal reference coordinate system The resultant acceleration amplitude under the following conditions satisfies At that time, start the process. By integral recursion, we obtain The velocity change vector in the system is denoted as .

[0067] Define the vehicle coordinate system To the horizontal reference coordinate system The direction cosine matrix, which contains only the heading angle. The simplified direction cosine matrix is ​​denoted as: (twenty four) Its core physical characteristic is: heading angle It is around vertically downwards The rotation angle of the axis only changes the angle in the horizontal plane. The component mapping relationship of the axis, for the vertical direction The components of the axis have no effect, therefore the third row and third column of the matrix are always equal. .

[0068] (2) Coordinate system transformation relationship of horizontal velocity change When a vehicle is traveling in a straight line, the vehicle coordinate system The actual horizontal velocity change vector only along Since the axis has a component, and the lateral velocity change component is 0, the velocity change vector of the vehicle system can be expressed as: (25) in, The actual change in the vehicle's forward speed satisfies And when the vehicle moves forward Horizontal reference coordinate system The conversion relationship between the velocity change vector of the vehicle system and the velocity change vector of the vehicle system is as follows: (26) In the formula, Horizontal reference coordinate system The velocity change vector.

[0069] (3) Heading installation angle The solution formula The direction cosine matrix containing only the heading angle With respect to the velocity change vector of the vehicle system Substituting into the transformation formula, we can perform matrix multiplication: (27) Take the horizontal plane Expanding the components of the velocity change on the shaft yields explicit expressions for each component: (28) The vehicle meets the requirements during the straight-line driving phase. It is a non-zero scalar. Solve for the heading installation angle. The expressions for sine and cosine: (29) Combining the trigonometric identities of the arctangent functions in the four quadrants, we obtain the formula for calculating the heading angle: (30) The output angle range of this formula is: It precisely covers all possible values ​​of the heading installation angle, and can be based on , The sign of the angle is automatically determined to determine the quadrant, avoiding the traditional... The quadrant ambiguity problem of functions is solved with accurate and unambiguous results.

[0070] Based on the above embodiments, step 300 includes: By determining the sequence convergence and verifying the confidence of the installation angle, the mean of the second half of the converged sequence is taken as the final heading installation angle, thus obtaining a complete estimate of the three-axis installation angle.

[0071] (1) Convergence determination of heading installation angle sequence After completing the epoch-by-epoch calculation of the heading installation angle, in order to filter out the influence of measurement noise and random disturbances on the calculation results and improve the estimation accuracy and stability of the heading installation angle, the convergence of the continuously calculated heading installation angle sequence is determined.

[0072] Take the calculated heading and installation angle values ​​of 10 consecutive epochs to form a sequence. ,in Set the heading angle deviation threshold between adjacent epochs for the current epoch. This threshold can be set according to the actual engineering scenario. The heading installation angle sequence is considered convergent when it simultaneously meets the following two criteria: 1. Heading angle difference constraint for any adjacent epochs: (31) 2. Constraints on overall sequence amplitude fluctuations: (32) (2) Installation angle confidence test After the heading installation angle sequence converges, the confidence level of the installation angle needs to be further verified. Quantitative indicators are constructed for the degree of horizontal attitude disturbance and the degree of heading turn, respectively, to quantitatively evaluate the reliability of the calculated installation angle. Various low-confidence conditions during vehicle operation are eliminated to ensure the reliability of the speed change recursion and the heading installation angle calculation results. The verification logic of the two types of quantitative indicators has different focuses and is independent of each other.

[0073] Set horizontal attitude perturbation threshold This threshold can be taken as... Set the heading and turning disturbance threshold. This threshold can be taken as... The specific quantitative constraints are as follows: (33) In the formula, The total disturbance value of the horizontal attitude angle within 10 epochs represents the comprehensive fluctuation of the vehicle's roll and pitch attitude. , For the first Epoch in horizontal reference coordinate system The roll and pitch angles are obtained through recursion. The total heading angle over 10 epochs represents the degree of change in the vehicle's heading attitude. For the first Epoch Horizontal Reference Coordinate System The effective output value of the gyroscope Z-axis is as follows. The epoch sampling interval.

[0074] The smaller the value, the less significant the fluctuation in the vehicle's roll and pitch attitude within those 10 epochs. The lateral smoothness and longitudinal slope of the road surface on which the vehicle travels remain stable, without any attitude disturbances in the pitch and roll directions such as road bumps, climbing, or descending. This can significantly reduce the installation angle calculation interference error caused by uneven road surfaces. The smaller the value, the less noticeable the turning or cornering action of the vehicle within those 10 epochs. The vehicle maintains an almost straight-line driving state, which can effectively avoid the installation angle calculation deviation caused by heading deflection.

[0075] Based on the above two quantitative indicators, a quantitative calculation model for the confidence level of the installation angle calculation results is constructed to comprehensively evaluate the reliability of the installation angle calculation value. The calculation formula is as follows: (34) In the formula, The installation angle confidence level has a range of values. ; , The weighting coefficients are positive coefficients that match the dimensions of the angle threshold. They can be tuned according to the influence of horizontal attitude disturbances and heading disturbances on the installation angle calculation in actual engineering scenarios, satisfying the following requirements. , Installation angle confidence The larger the value, the less the installation angle is affected by driving conditions, and the higher the reliability of the solution; conversely, the smaller the value, the more the solution is affected by disturbances, and the lower the reliability.

[0076] Set the confidence threshold for the installation angle If satisfied If the installation angle calculation result is not met, the result is deemed invalid and the set of calculated values ​​is discarded and will not be included in subsequent optimal value calculations; if the following conditions are met... If the result is positive, the installation angle calculation is considered valid, and the calculated value is retained and used in subsequent optimal value calculations.

[0077] (3) Calculation of final value of heading installation angle and integration of three-axis installation angle After both the convergence determination and the confidence verification of the heading and installation angle sequence pass, to further suppress the influence of random measurement noise, the calculated heading and installation angle values ​​of the 10 convergent epochs are subjected to sliding window mean filtering. The mean of the heading angles in the last 5 epochs of the sequence is taken as the final optimal estimate of the heading and installation angles. The calculation formula is as follows: (35) Combining the roll installation angle obtained accurately based on the gravity projection method mentioned above Pitch installation angle And the optimal estimate of the heading installation angle obtained after completing the convergence determination and confidence verification in this section. Finally, the complete estimation of the three-axis mounting angles of the IMU coordinate system relative to the vehicle body coordinate system is achieved. The three-axis mounting angle vector can be expressed as: (36) like Figure 4 The flowchart shown below illustrates the dynamic calculation and verification process for the vehicle-mounted IMU installation angle. The overall process is as follows: Initialization and startup: The system first performs installation angle calculation initialization, and after completing parameter configuration and state preparation, it enters the vehicle stationary state determination.

[0078] Static horizontal mounting angle calculation: If the vehicle is determined to be stationary, the roll and pitch horizontal mounting angles are calculated using the gravity projection method; if the vehicle is not stationary, the process returns to the initialization phase and waits for the stationary trigger condition.

[0079] Dynamic operating condition triggering and processing: After the horizontal installation angle is calculated, it is determined whether the vehicle has entered a moving state. If the vehicle is not moving, it enters a waiting state; if the vehicle is moving, it is further determined whether the acceleration exceeds the threshold: if it exceeds the threshold, the inertial navigation speed recursion is initiated; if it does not exceed the threshold, it continues to wait.

[0080] Heading installation angle sequence calculation and verification: The heading installation angle sequence is calculated based on the inertial navigation velocity recursion result, and then the sequence convergence judgment and confidence verification are performed in sequence: If the sequence does not converge or the verification fails, the heading installation angle sequence is recalculated; if the verification passes, the final heading installation angle is output.

[0081] Thus, without relying on any external auxiliary sensors, the fully autonomous and high-precision estimation of the IMU's three-axis mounting angle was achieved solely through data measured by the IMU's own accelerometer and gyroscope.

[0082] The IMU installation angle estimation system based on velocity increment and confidence verification provided by the present invention will be described below. The IMU installation angle estimation system based on velocity increment and confidence verification described below can be referred to in correspondence with the IMU installation angle estimation method based on velocity increment and confidence verification described above.

[0083] Figure 5 This is a schematic diagram of the structure of the IMU installation angle estimation system based on velocity increment and confidence verification 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: Module 51 is used to establish the gyroscope zero-bias estimation model and the accelerometer-gravity projection correlation model in a stationary state for the vehicle, respectively. The calculation module 52 is used to start the inertial navigation system to recursively solve for the velocity change when the vehicle acceleration is large, and to solve for the heading installation angle of the IMU in each epoch using the angle of the velocity change. The estimation module 53 is used to obtain the complete estimation result of the three-axis installation angle by taking the mean of the second half of the converged sequence as the final heading installation angle through sequence convergence determination and installation angle confidence verification.

[0084] Figure 6 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 6As 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 logic instructions in the memory 630 to execute an IMU mounting angle estimation method based on velocity increment and confidence verification. This method includes: establishing a gyroscope zero-bias estimation model for the vehicle and an accelerometer-gravity projection correlation model in a stationary state; when a large vehicle acceleration is detected, inertial navigation is initiated to recursively solve for the velocity change; using the angle between the velocity changes, the heading mounting angle of the IMU is solved at each epoch; through sequence convergence determination and mounting angle confidence verification, the average of the latter half of the converged sequence is taken as the final heading mounting angle, resulting in a complete estimation result of the three-axis mounting angle.

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

[0086] On the other hand, 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 velocity increment and confidence verification 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 a large vehicle acceleration is detected, inertial navigation is initiated to recursively solve for the velocity change, and the heading installation angle of the IMU is solved at each epoch using the angle between the velocity changes; through sequence convergence determination and installation angle confidence verification, the average of the second half of the converged sequence is taken as the final heading installation angle to obtain a complete estimation result of the three-axis installation angle.

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

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

[0089] 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. A method for estimating the installation angle of an IMU based on velocity increment and confidence level verification, 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 a large vehicle acceleration is detected, the inertial navigation system is activated to recursively solve for the velocity change. Using the angle between the velocity changes, the heading angle of the IMU is solved at each epoch. By determining the sequence convergence and verifying the confidence of the installation angle, the mean of the second half of the converged sequence is taken as the final heading installation angle, thus obtaining a complete estimate of the three-axis installation angle.

2. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 1, characterized in that, 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 velocity increment and confidence verification according to claim 1, characterized in that, When a large vehicle acceleration is detected, the inertial navigation system is initiated to recursively solve for the velocity change. Using the angle between these velocity changes, the IMU's heading angle is calculated at each epoch, including: Determine the core estimation principle of inertial navigation recursion; Based on the core estimation principle, the coordinate system transformation relationship of the horizontal velocity change is obtained; The heading installation angle is solved by the coordinate system transformation relationship of the horizontal velocity change.

4. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 3, characterized in that, The core estimation principle of inertial navigation recursion is determined, including: Complete roll installation angle With pitch installation angle After solving, the heading installation angle is ignored. The horizontal reference coordinate system for eliminating roll and pitch tilt deviations is defined as follows: Tie, Vehicle coordinate system The horizontal plane is parallel, with rotational deviation only in the heading direction. ; Establish IMU carrier coordinate system Set to the horizontal reference coordinate system The direction cosine matrix of the system This matrix is ​​derived solely from the calculated roll installation angle. Pitch installation angle The decision is made without a heading angular component, based on the direction cosine matrix. After performing coordinate system transformation on the IMU measured vectors, we obtain: Set acceleration threshold When the horizontal reference coordinate system The resultant acceleration amplitude satisfies At that time, start the process. By integral recursion, we obtain The velocity change vector in the system ; Define the vehicle coordinate system To the horizontal reference coordinate system The direction cosine matrix contains only the heading angle. The simplified direction cosine matrix is ​​denoted as: 。 5. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 4, characterized in that, Based on the core estimation principle, the coordinate system transformation relationship of the horizontal velocity change is obtained, including: When the vehicle is traveling in a straight line, the vehicle coordinate system The actual horizontal velocity change vector only along Since the axis has a component, and the lateral velocity change component is 0, the velocity change vector of the vehicle system can be represented as: in, The actual change in the vehicle's forward speed satisfies And when the vehicle moves forward ; Horizontal reference coordinate system The conversion relationship between the velocity change vector of the vehicle system and the velocity change vector of the vehicle system is as follows: In the formula, Horizontal reference coordinate system The velocity change vector.

6. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 5, characterized in that, The heading angle is determined by the coordinate system transformation relationship of the horizontal velocity change, including: The direction cosine matrix containing only the heading angle With respect to the velocity change vector of the vehicle system Substituting into the transformation formula, we can perform matrix multiplication: Take the horizontal plane Expanding the components of the velocity change on the shaft yields explicit expressions for each component: The vehicle meets the requirements during the straight-line driving phase. , being a non-zero scalar, solve for the heading installation angle. The expressions for sine and cosine: Combining the trigonometric identities of the arctangent functions in the four quadrants, we obtain the formula for calculating the heading angle: 。 7. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 1, characterized in that, By determining the sequence convergence and verifying the confidence level of the mounting angle, the mean of the latter half of the converged sequence is taken as the final heading mounting angle, thus obtaining a complete estimate of the three-axis mounting angles, including: After completing the epoch-by-epoch calculation of the heading installation angle, the convergence of the continuously calculated heading installation angle sequence is determined. After the heading installation angle sequence converges, the installation angle confidence is verified. Perform final value calculation of heading installation angle and integration of three-axis installation angle.

8. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 7, characterized in that, After completing the epoch-by-epoch calculation of the heading and installation angles, the convergence of the continuously calculated heading and installation angle sequence is determined. Once the heading and installation angle sequence converges, the confidence level of the installation angles is verified, including: Extract the heading installation angle calculation values ​​from 10 consecutive epochs to form a sequence, and determine the heading angle adjacent epoch deviation threshold. If the sequence simultaneously satisfies the following two criteria, then the heading installation angle sequence is considered convergent: Heading angle difference constraint for any adjacent epochs: Overall amplitude fluctuation constraint of the sequence: Determine the horizontal attitude perturbation threshold and heading and turning disturbance threshold The quantitative constraints are determined as follows: In the formula, The total disturbance value of the horizontal attitude angle within 10 epochs represents the comprehensive fluctuation of the vehicle's roll and pitch attitude. , For the first Epoch in horizontal reference coordinate system The roll and pitch angles are obtained through recursion. The total heading angle over 10 epochs represents the degree of change in the vehicle's heading attitude. For the first Epoch Horizontal Reference Coordinate System The effective output value of the gyroscope Z-axis is as follows. The epoch sampling interval; A quantitative calculation model for the confidence level of the installation angle calculation results is constructed based on quantitative constraints. The reliability of the calculated installation angle value is comprehensively evaluated, and the calculation formula is as follows: In the formula, The installation angle confidence level has a range of values. ; , The weighting coefficients are positive coefficients that match the dimensions of the angle threshold. They are tuned based on the influence of horizontal attitude disturbances and heading disturbances on the installation angle calculation in actual engineering scenarios, satisfying the following requirements. , Installation angle confidence The larger the value, the less the installation angle is affected by driving conditions, and the higher the reliability of the solution. Conversely, the smaller the value, the more the solution is affected by disturbances, and the lower the reliability. Set the confidence threshold for the installation angle If satisfied If the installation angle calculation result is not met, the result is deemed invalid and the set of calculated values ​​is discarded and will not be included in subsequent optimal value calculations; if the following conditions are met... If the result is positive, the installation angle calculation is considered valid, and the calculated value is retained and used in subsequent optimal value calculations.

9. The IMU installation angle estimation method based on velocity increment and confidence verification according to claim 8, characterized in that, The final value calculation of the heading installation angle and the integration of the three-axis installation angles are performed, including: The heading and installation angle solutions for 10 convergent epochs are subjected to sliding window mean filtering. The mean of the heading and installation angles for the last 5 epochs is taken as the final optimal estimate of the heading and installation angle. The solution formula is as follows: The roll installation angle is accurately calculated based on the gravity projection method. Pitch installation angle And the optimal estimate of the heading installation angle obtained after completing the convergence determination and confidence verification. Complete the estimation of the three-axis mounting angles of the IMU coordinate system relative to the vehicle body coordinate system. The three-axis mounting angle vector is represented as: 。 10. An IMU installation angle estimation system based on velocity increment and confidence level verification, 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 detect when the vehicle acceleration is large, and then initiate the inertial navigation system to recursively solve for the velocity change. Using the angle between the velocity changes, the heading installation angle of the IMU is solved at each epoch. The estimation module is used to obtain the complete estimation result of the three-axis installation angle by taking the mean of the second half of the converged sequence as the final heading installation angle through sequence convergence determination and installation angle confidence verification.

Images