A method for determining heading angle for IMU-GPS integrated navigation

Abstract

A method for determining the heading angle for IMU-GPS integrated navigation uses the rotation angle calculated by the gyroscope and the rotation angle calculated by the rotational speed difference as summing factors. Based on the real-time dynamic drift error of the gyroscope, the two summing factors are assigned real-time dynamically changing weight coefficients. This method establishes a representation of the heading angle by fusing the real-time dynamically changing weight coefficients and the weighted summation of the rotation angle calculated by the gyroscope and the rotation angle calculated by the rotational speed difference. The method for determining the heading angle for IMU-GPS integrated navigation of the present invention uses the rotation angle calculated by the gyroscope and the rotation angle calculated by the rotational speed difference, combined with the real-time dynamically changing weights established by the real-time dynamic attitude error of the gyroscope, to determine a representation of the real-time heading angle change. The overall technical solution is simple, efficient, and low-cost, offering broad alternative and practical applications.

Description

Technical Field

[0001] The present invention belongs to the fields of vehicle engineering, automatic driving and integrated navigation, and particularly relates to a method for determining a heading angle of an IMU-GPS integrated navigation. Background Art

[0002] In the fields of autonomous driving and advanced driver assistance, driving safety is paramount. A precise positioning system is essential for ensuring a precise understanding of the surrounding environment under various road conditions. A crucial step in autonomous driving technology is the navigation and positioning system, which keeps the vehicle informed of its location and surroundings at all times. However, relying solely on satellite navigation and positioning is insufficient. In tunnels, underground garages, urban canyons, and other road conditions with poor satellite positioning signals, the position output provided by satellite positioning is unreliable. In these situations, the system requires an inertial measurement unit (IMU) to measure the vehicle's trajectory and determine its position. Integrated navigation and Kalman fusion of information output provides precise positioning, even in the absence of satellite signals. However, achieving centimeter-level accuracy in these conditions requires high-precision inertial navigation components, which typically cost tens or even hundreds of thousands of yuan. This prohibitive cost for in-vehicle navigation systems prevents their application in mass production. In low-cost inertial navigation devices, accelerometers and gyroscopes are susceptible to environmental influences, resulting in high noise in their output. During the long-term heading angle calculation process, the accelerometer and gyroscope outputs must be continuously integrated. This integration process accumulates the errors, resulting in significant errors in the navigation information and inaccurate estimates.

[0003] The invention application with application number CN201310532435.6 discloses a “GNSS and MIMU combined navigation heading angle estimation method based on satellite navigation receiver”, the steps of which are as follows: (1) using a satellite navigation receiver to obtain visible satellite ephemeris information, the relative velocity between the receiver and the visible satellite, the relative acceleration vector and the carrier position and velocity information, and calculate the carrier acceleration vector; (2) establishing a specific force measurement equation of SINS in the Earth-centered Earth-fixed coordinate system, and using dynamic leveling to calculate the horizontal attitude angle based on the carrier acceleration vector measured by the satellite navigation receiver and the measurement value of the accelerometer in the MIMU; (3) according to the specific force measurement equation, using the acceleration vector measured by the satellite navigation receiver, the measurement value of the accelerometer, the calculated horizontal attitude angle, the velocity vector and the local gravity vector, to solve and obtain the heading angle information.

[0004] The invention application with application number: CN202010606149.X discloses "a heading angle calculation method based on front-end fusion", including an inertial navigation module, a front-end fusion module and a positioning algorithm module. The inertial navigation module outputs the original IMU data, and the positioning algorithm module calculates and processes the posture data. The front-end fusion module includes a Kalman filter unit and a motion model calculation unit. The front-end fusion module receives the posture data and the original IMU data, processes them to obtain the fused IMU data including the heading angle, angular velocity and linear acceleration, and outputs them to the positioning algorithm module. Its heading angle calculation method uses the posture data output by the positioning algorithm module as observation to realize the heading angle incremental Kalman filtering method; there is also a heading angle output method that uses a motion model for updating. This method can provide a more accurate heading angle estimate for the positioning algorithm, and can timely correct the occasional position loss problem of the positioning algorithm during alignment, thereby improving the reliability and stability of positioning. Summary of the Invention

[0005] In order to solve the above problems and make the heading angle more accurate when there is no satellite signal, the present invention provides a method for determining the heading angle of IMU-GPS integrated navigation, and its technical solution is as follows:

[0006] A method for determining a heading angle for IMU-GPS integrated navigation, characterized by comprising the following steps:

[0007] S1: The angle calculated by the gyroscope and the angle calculated by the speed difference are added as the factor.

[0008] Based on the real-time dynamic drift error of the gyroscope, the two summing factors are assigned real-time dynamic weight coefficients. Based on this, the heading angle change represented by the weighted sum of the real-time dynamic weight coefficients at each time step and the angle calculated by the gyroscope and the rotation angle calculated by the rotation speed difference is established;

[0009] S2: The heading angle at the first time step is determined based on the initial heading angle and the heading angle change at the first time step in the time series; the subsequent real-time heading angle is determined based on the heading angle at the previous time step and the heading angle change at the current time step.

[0010] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0011] The weight coefficients of the two summation factors that change dynamically in real time are established in a mutually exclusive relationship.

[0012] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0013] The two summation factors are given respective real-time dynamic weight coefficients based on the real-time dynamic drift error of the gyroscope, specifically:

[0014] Firstly, a first error coefficient based on dynamic drift is established according to the real-time dynamic drift error of the gyroscope;

[0015] Then, a second error coefficient based on the rotation angle difference is established according to the rotation angle calculated by the real-time gyroscope and the rotation angle calculated by the real-time rotation speed difference;

[0016] Finally, a weight coefficient expression is established according to the first error coefficient and the second error coefficient, and the weight coefficient setting of each summation factor is completed based on the weight coefficient.

[0017] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0018] The first error coefficient based on dynamic drift is established according to the real-time dynamic drift error of the gyroscope, specifically:

[0019] First, the dynamic drift of the gyroscope under experimental static conditions is determined in the laboratory, and an error benchmark is established based on this;

[0020] Then, according to the established error benchmark and in combination with the monitoring of the real-time dynamic drift of the gyroscope, a corresponding first error coefficient based on the dynamic drift is determined.

[0021] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0022] The second error coefficient based on the angle difference is established based on the angle calculated by the real-time gyroscope and the angle calculated by the real-time rotation speed difference, specifically as follows:

[0023] r=|angel_gyro-angel_wheel|,

[0024] Where,

[0025] r: second error coefficient;

[0026] angel_gyro: the rotation angle calculated by the gyroscope, unit: degree;

[0027] angel_wheel: The angle calculated by the speed difference, unit: degree.

[0028] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0029] The first error coefficient is determined according to the following formula:

[0030]

[0031] Where,

[0032] q: first error coefficient;

[0033] q_0: initial weight parameter value;

[0034] w z : Real-time dynamic drift of gyroscope;

[0035] w z0 : Dynamic drift of gyroscope under experimental static conditions.

[0036] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0037] The weight coefficient expression is established based on the first error coefficient and the second error coefficient, and the weight coefficient setting of each summation factor is completed based on the weight coefficient, specifically:

[0038] First, the following expression of the weight coefficient is established based on the first error coefficient and the second error coefficient:

[0039] K=q / (q+r),

[0040] Then, the weight coefficients of the summing factors are set according to the weight coefficients, thereby establishing the following value for the heading angle change:

[0041] angel_gyro_true=(1-K)×angel_gyro+K×angel_wheel,

[0042] Where,

[0043] K: weight coefficient;

[0044] q: first error coefficient;

[0045] r: second error coefficient;

[0046] angel_gyro: the rotation angle calculated by the gyroscope, unit: degree;

[0047] angel_wheel: the angle calculated by the speed difference, unit: degree;

[0048] angel_gyro_true: The current heading angle change, unit: degree.

[0049] A method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0050] The initial weight parameter value q_0 is determined according to the actual gyroscope performance.

[0051] The present invention provides a method for determining a heading angle for IMU-GPS integrated navigation. The method is based on a rotation angle calculated by a gyroscope and a rotation angle calculated by a rotation speed difference, and combines a real-time dynamic change weight established by the real-time dynamic attitude error of the gyroscope to determine a representation of the real-time heading angle change. The overall technical solution is simple, efficient, and low-cost, and has wide alternatives and practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 Schematic diagram of the heading angle determination steps of the present invention;

[0053] Figure 2 This is a schematic diagram of the working principle and rear wheel turning process of the present invention;

[0054] Figure 3 for Figure 2 A simplified abstract diagram of

[0055] Figure 4 This is a schematic diagram comparing the working principle and driving path of the underground garage in the process before and after using the technical solution of the present invention. DETAILED DESCRIPTION

[0056] Below, a method for determining a heading angle of an IMU-GPS integrated navigation system of the present invention is further described in detail based on the accompanying drawings and specific implementation methods.

[0057] like Figure 1 The method for determining the heading angle of an IMU-GPS integrated navigation system includes the following steps:

[0058] S1: The angle calculated by the gyroscope and the angle calculated by the speed difference are added as the factor.

[0059] Based on the real-time dynamic drift error of the gyroscope, the two summing factors are assigned real-time dynamic weight coefficients. Based on this, the heading angle change represented by the weighted sum of the real-time dynamic weight coefficients at each time step and the angle calculated by the gyroscope and the rotation angle calculated by the rotation speed difference is established;

[0060] S2: The heading angle at the first time step is determined based on the initial heading angle and the heading angle change at the first time step in the time series; the subsequent real-time heading angle is determined based on the heading angle at the previous time step and the heading angle change at the current time step.

[0061] in,

[0062] The weight coefficients of the two summation factors that change dynamically in real time are established in a mutually exclusive relationship.

[0063] in,

[0064] The two summation factors are given respective real-time dynamic weight coefficients based on the real-time dynamic drift error of the gyroscope, specifically:

[0065] Firstly, a first error coefficient based on dynamic drift is established according to the real-time dynamic drift error of the gyroscope;

[0066] Then, a second error coefficient based on the rotation angle difference is established according to the rotation angle calculated by the real-time gyroscope and the rotation angle calculated by the real-time rotation speed difference;

[0067] Finally, a weight coefficient expression is established according to the first error coefficient and the second error coefficient, and the weight coefficient setting of each summation factor is completed based on the weight coefficient.

[0068] in,

[0069] The first error coefficient based on dynamic drift is established according to the real-time dynamic drift error of the gyroscope, specifically:

[0070] First, the dynamic drift of the gyroscope under experimental static conditions is determined in the laboratory, and an error benchmark is established based on this;

[0071] Then, according to the established error benchmark and in combination with the monitoring of the real-time dynamic drift of the gyroscope, a corresponding first error coefficient based on the dynamic drift is determined.

[0072] in,

[0073] The second error coefficient based on the angle difference is established based on the angle calculated by the real-time gyroscope and the angle calculated by the real-time rotation speed difference, specifically as follows:

[0074] r=|angel_gyro-angel_wheel|,

[0075] Where,

[0076] r: second error coefficient;

[0077] angel_gyro: the rotation angle calculated by the gyroscope, unit: degree;

[0078] angel_wheel: The angle calculated by the speed difference, unit: degree.

[0079] The method for determining a heading angle of an IMU-GPS integrated navigation system according to the present invention is characterized in that:

[0080] The first error coefficient is determined according to the following formula:

[0081]

[0082] Where,

[0083] q: first error coefficient;

[0084] q_0: initial weight parameter value;

[0085] w z : Real-time dynamic drift of gyroscope;

[0086] w z0 : Dynamic drift of gyroscope under experimental static conditions.

[0087] in,

[0088] The weight coefficient expression is established based on the first error coefficient and the second error coefficient, and the weight coefficient setting of each summation factor is completed based on the weight coefficient, specifically:

[0089] First, the following expression of the weight coefficient is established based on the first error coefficient and the second error coefficient:

[0090] K=q / (q+r),

[0091] Then, the weight coefficients of the summing factors are set according to the weight coefficients, thereby establishing the following value for the heading angle change:

[0092] angel_gyro_true=(1-K)×angel_gyro+K×angel_wheel,

[0093] Where,

[0094] K: weight coefficient;

[0095] q: first error coefficient;

[0096] r: second error coefficient;

[0097] angel_gyro: the rotation angle calculated by the gyroscope, unit: degree;

[0098] angel_wheel: the angle calculated by the speed difference, unit: degree;

[0099] angel_gyro_true: The current heading angle change, unit: degree.

[0100] in,

[0101] The initial weight parameter value q_0 is determined according to the actual gyroscope performance.

[0102] Working principle and process

[0103] First, the design ideas of this technical solution are summarized as follows:

[0104] For gyro-based inertial navigation, the gyroscope measures angular velocity, and the angle needs to be obtained by integrating the angular velocity. However, due to the existence of the gyroscope's zero drift, the error will accumulate over time and become larger and larger, so the integrated angle will become more and more distorted. In order to solve this problem and accurately represent the heading angle, the angle calculated by the gyroscope is combined with the angle calculated by the rotational speed difference, and the main control factor is established by using the error of real-time dynamic drift. The difference between the angle calculated by the real-time gyroscope and the angle calculated by the rotational speed difference is used as auxiliary control, and a weight coefficient is established that can change in real time according to actual conditions. The auxiliary control is to verify the true accuracy of the drift and at the same time form an auxiliary control of the main control factor. Its overall establishment idea is based on the idea of ​​standard deviation, that is: based on a real-time dynamically changing value, a real-time standard deviation calculation is established (in this technical solution, it is specifically a representation and real-time calculation of a weight coefficient based on dynamic drift established with reference to the standard deviation). When the calculation result is relatively small, the true value calculation of the gyroscope calculated angle with higher credibility is automatically formed according to the establishment of the formula. When the calculation result is large, an adjusted calculation result is automatically formed according to the establishment of the formula.

[0105] Secondly, some basic knowledge involved in this technical solution is stated as follows:

[0106] For details on calculating the heading angle using the wheel speed difference model, see Figure 2 、 3 .

[0107] Combine Figure 2 、 Figure 3 , Figure 3 The AP direction is the heading at time t1, and the CQ direction is the heading at time t2. At time t1, the rear wheel position is shown by line segment AB, and the speeds of the left and right wheels are v respectively. 1左 、v 1右 , the rear wheel position at time t2 is line segment CD, and its speed is v 2左 、v 2右 , and They are the arc trajectories of the left wheel from time t1 to time t2, where the speed is the calibrated speed.

[0108] In a short period of time, it can be considered that: 1左 =v 2左 =v 左 、v 1右 =v 2右 =v 右 ,

[0109] According to the knowledge of differential calculus,

[0110] BC′ is a line parallel to DC,

[0111] ∠PNQ=∠AOC (reciprocal)

[0112] Therefore ∠ABC'=∠AOC=∠PNQ

[0113] Therefore, the heading angle can be calculated from the velocity:

[0114]

[0115] The heading angle obtained by the wheel speed difference can be obtained Regarding the angle calculated by the gyroscope:

[0116] In the vehicle system, the frequency of the odometer speed output is assumed to be t i The output angular velocity at this moment is w i ,in

[0117] t i ∈[t1,t2]

[0118] Then: The gyroscope angle obtained by integrating the gyroscope from time t1 to t2 is:

[0119]

[0120] Regarding the attitude drift error caused by the dynamic drift of the gyroscope, this attitude error can be regarded as a time-varying signal and determined by calculating the corresponding state equation and observation equation. This is also a common application and processing of basic knowledge points in this field, which will not be repeated here.

[0121] Finally, the technical solution based on the above is described as follows:

[0122] Some background information and notes are provided below: Gyroscope stability can be expressed as dynamic drift. This refers to the difference between the gyroscope's output and the true value after the fixed zero bias introduced by the startup is subtracted. A large absolute value indicates a significant difference between the gyroscope's output and the true value; conversely, a small difference indicates a small difference. The true value is determined by comparing the gyroscope's output with the satellite navigation system's output when the satellite signal is good, with the heading angle outputted by the satellite navigation system being the standard.

[0123] The definitions of the following formulas are shown in Table 1 below:

[0124] Table 1 Parameter Description

[0125]

[0126]

[0127] where ω z0 It refers to the dynamic drift of the gyroscope obtained through static testing in the laboratory. The dynamic drift in actual working conditions can be compared with this value to obtain the quality of the gyroscope output.

[0128] First, the weight parameter q is determined by dynamic drift

[0129]

[0130] The weight parameter r is obtained according to the gyroscope angle and the wheel speed difference angle, and then the weight K is obtained:

[0131] r=|angel_gyro-angel_wheel|,

[0132] K=q / (q+r),

[0133] The weight is used to correct the heading angle and output it.

[0134] angel_gyro_true=(1-K)*angel_gyro+K*angel_wheel,

[0135] new_yaw=last_yaw+angel_gyro_true,

[0136] The resulting new heading angle is the final, more accurate heading angle.

[0137] In the above formula, the larger q is, the larger k is; the larger r is, the smaller k is. Therefore, using the rear wheel speed differential angle to correct the gyroscope's heading angle output leverages the advantages of both. When the gyroscope is more accurate, the wheel speed differential angle contributes less, and when the wheel speed differential angle is more accurate, the gyroscope contributes less. Adaptively correcting and filtering the two ultimately yields a more accurate heading angle.

[0138] This solution can effectively compensate for the problem of large long-term error accumulation of the gyroscope integral angle, improve the heading angle accuracy of the combined navigation in the absence of satellite signals, and greatly improve the accuracy of navigation positioning.

[0139] Figure 4 The white frame shows a simulation of the road section inside the basement.

[0140] ①The purple path is the path with only gyroscope integration and no model

[0141] ③The blue path in the figure below is the path after using this model

[0142] It can be seen that if the model of the present invention is not added, the heading angle will become worse and worse due to the accumulation of errors output by the gyroscope itself during a long period of loss of lock in the underground garage. After adding the model of calculating the steering angle and correcting the heading based on the rear wheel speed difference, the path is corrected because the heading angle is calibrated.

[0143] Error statistics:

[0144] The table below compares the errors without and with rear wheel speed angle. "Error" is the difference between the estimated position and the actual position at the moment of leaving the basement, and "Error rate" is the ratio of "Error" to the total distance traveled when the vehicle was unlocked.

[0145] Table 2 Error statistics

[0146]

[0147] The example in the figure illustrates the phenomenon of error accumulation in the gyro-integrated angle. This long-term accumulation causes the error to gradually increase. The rear wheel speed angle is used as another observable quantity and adaptively filtered and fused with the gyro-integrated angle to correct the heading angle inside the basement, thereby improving the accuracy of navigation positioning inside the basement.

[0148] Conclusion: The heading angle can be filtered and fused by the angle calculated by the rear wheel speed and the gyro integrated angle to obtain a more accurate angle output. In summary, the present invention provides a heading angle determination method for IMU-GPS integrated navigation. This method is based on the angle calculated by the gyroscope and the angle calculated by the rotation speed difference, and combines the real-time dynamic weight established by the gyroscope's real-time dynamic attitude error to determine the representation of the real-time heading angle change. The overall technical solution is simple, efficient, and low-cost, and has wide alternative and practical applications.

Claims

1. A method for determining the heading angle of IMU-GPS integrated navigation, characterized in that The steps include: S1: The angle calculated by the gyroscope and the angle calculated by the speed difference are added as the factor. Based on the real-time dynamic drift error of the gyroscope, the two summing factors are assigned real-time dynamic weight coefficients. Based on this, the heading angle change represented by the weighted sum of the real-time dynamic weight coefficients at each time step and the angle calculated by the gyroscope and the rotation angle calculated by the rotation speed difference is established; S2: The heading angle at the first time step is determined based on the initial heading angle and the heading angle change at the first time step in the time series; the subsequent real-time heading angle is determined based on the heading angle at the previous time step and the heading angle change at the current time step. The weight coefficients of the two summation factors that change dynamically in real time are established in a mutually exclusive relationship. The two summation factors are given respective real-time dynamic weight coefficients based on the real-time dynamic drift error of the gyroscope, specifically: Firstly, a first error coefficient based on dynamic drift is established according to the real-time dynamic drift error of the gyroscope; Then, a second error coefficient based on the rotation angle difference is established according to the rotation angle calculated by the real-time gyroscope and the rotation angle calculated by the real-time rotation speed difference; Finally, a weight coefficient expression is established according to the first error coefficient and the second error coefficient, and the weight coefficient setting of each summation factor is completed based on the weight coefficient.

2. The method for determining a heading angle of an IMU-GPS integrated navigation system according to claim 1, wherein: The first error coefficient based on dynamic drift is established according to the real-time dynamic drift error of the gyroscope, specifically: First, the dynamic drift of the gyroscope under experimental static conditions is determined in the laboratory, and an error benchmark is established based on this; Then, according to the established error benchmark and in combination with the monitoring of the real-time dynamic drift of the gyroscope, a corresponding first error coefficient based on the dynamic drift is determined.

3. The method for determining a heading angle of an IMU-GPS integrated navigation system according to claim 1, wherein: The second error coefficient based on the angle difference is established based on the angle calculated by the real-time gyroscope and the angle calculated by the real-time rotation speed difference, specifically as follows: r=|angel_gyro-angel_wheel|, Where, r: second error coefficient; angel_gyro: the rotation angle calculated by the gyroscope, unit: degree; angel_wheel: The angle calculated by the speed difference, unit: degree.

4. The method for determining a heading angle of an IMU-GPS integrated navigation system according to claim 2, wherein: The second error coefficient based on the angle difference is established based on the angle calculated by the real-time gyroscope and the angle calculated by the real-time rotation speed difference, specifically as follows: r=|angel_gyro-angel_wheel|, Where, r: second error coefficient; angel_gyro: the rotation angle calculated by the gyroscope, unit: degree; angel_wheel: The angle calculated by the speed difference, unit: degree.

5. The method for determining a heading angle of an IMU-GPS integrated navigation system according to claim 2, wherein: The first error coefficient is determined according to the following formula: Where, q: first error coefficient; q_0: initial weight parameter value; w z : Real-time dynamic drift of gyroscope; w z0 : Dynamic drift of gyroscope under experimental static conditions.

6. The method for determining a heading angle of an IMU-GPS integrated navigation system according to claim 4, wherein: The first error coefficient is determined according to the following formula: Where, q: first error coefficient; q_0: initial weight parameter value; w z : Real-time dynamic drift of gyroscope; w z0 : Dynamic drift of gyroscope under experimental static conditions.

7. The method for determining a heading angle of an IMU-GPS integrated navigation system according to claim 6, wherein: The weight coefficient expression is established based on the first error coefficient and the second error coefficient, and the weight coefficient setting of each summation factor is completed based on the weight coefficient, specifically: First, the following expression of the weight coefficient is established based on the first error coefficient and the second error coefficient: K=q / (q+r), Then, the weight coefficients of the summing factors are set according to the weight coefficients, thereby establishing the following value for the heading angle change: angel_gyro_true=(1-K)×angel_gyro+K×angel_wheel, Where, K: weight coefficient; q: first error coefficient; r: second error coefficient; angel_gyro: the rotation angle calculated by the gyroscope, unit: degree; angel_wheel: the angle calculated by the speed difference, unit: degree; angel_gyro_true: The current heading angle change, unit: degree.

8. The method for determining the heading angle of an IMU-GPS integrated navigation system according to claim 5 or 6, wherein: The initial weight parameter value q_0 is determined according to the actual gyroscope performance.

Images