A method for online estimation and compensation of wheel speed sensor error of vehicle integrated navigation system

Abstract

The application discloses a kind of vehicle-mounted integrated navigation system wheel speed meter error online estimation and compensation method, this method under the premise of not changing original integrated navigation algorithm architecture, wheel speed meter double-loop update mode is designed, wheel speed meter error is estimated online using low-dimensional filter in error estimation loop, compensate wheel speed meter output in filter loop in combination with error estimation result, improve the precision of integrated navigation of inertial navigation system / wheel speed meter.The method realizes wheel speed meter error online estimation based on wheel speed meter double-loop update mode and dimensionality reduction filter, does not affect the original integrated navigation algorithm architecture, small amount of calculation, high estimation accuracy, after error estimation and compensation, the integrated navigation positioning performance under the condition that GNSS signal is blocked can be improved, and higher engineering application value is shown.

Description

Technical Field

[0001] This application belongs to the technical field of vehicle-mounted integrated navigation systems, and particularly relates to an online estimation and compensation method for wheel speedometer errors in vehicle-mounted integrated navigation systems. Background Technology

[0002] Integrated navigation systems are indispensable modules in vehicle-assisted and autonomous driving, responsible for providing real-time, high-precision position, velocity, and attitude information to the vehicle. A typical in-vehicle integrated navigation system comprises three modules: an inertial navigation system (INS), a global navigation satellite system (GNSS), and a wheel speed sensor module. In-vehicle inertial navigation generally uses micro-electro-mechanical systems (MEMS), which are small, low-cost, and have a high data update rate, and do not rely on external information. However, due to the relatively low accuracy of the internal gyroscopes and accelerometers, they cannot operate independently for extended periods. Therefore, data fusion between the INS and GNSS modules is necessary to ensure high-precision navigation performance. However, in urban road conditions, GNSS signals are easily interfered with or blocked, resulting in poor or unusable GNSS positioning quality. In such cases, a combination of wheel speed sensors and MEMS INS is required to improve positioning and orientation accuracy under GNSS signal obstruction conditions.

[0003] Wheel speed meters (WSMs) mainly suffer from two types of errors: calibration coefficient error and orientation installation error. The former is easily affected by vehicle tire deformation, while the latter is easily affected by vehicle body structural deformation. Therefore, these two errors are difficult to accurately calibrate in advance and must be estimated and compensated online in real time. Existing literature generally uses the extended state estimation method to estimate wheel speed meter errors, adding the wheel speed meter error to the integrated navigation filter and estimating all state variables together. This method has a high filter dimension, resulting in a large computational load, and the severe coupling between state variables can easily lead to incorrect error allocation, thus affecting the accuracy of filter estimation. Therefore, to improve the estimation accuracy of wheel speed meter errors in vehicle-mounted integrated navigation systems, it is necessary to study a simple, easy-to-implement, computationally inexpensive, and non-disruptive online estimation and compensation method that improves the efficiency and accuracy of online wheel speed meter error estimation, thereby improving the positioning performance of vehicle-mounted integrated navigation systems under GNSS signal obstruction conditions. Summary of the Invention

[0004] This application proposes an online estimation and compensation method for wheel speedometer errors in a vehicle-mounted integrated navigation system. This method is applicable to the online estimation and compensation of wheel speedometer errors in high-precision vehicle positioning and orientation systems, and is particularly suitable for applications requiring high-precision positioning using a combination of low-cost inertial navigation and wheel speedometers even under conditions of satellite signal interference. The technical solution adopted in this application is as follows:

[0005] A method for online estimation and compensation of wheel speedometer error in a vehicle-mounted integrated navigation system, the method comprising the following steps:

[0006] Step 1: Start the vehicle-mounted integrated navigation system, complete the initialization of the integrated navigation position and attitude, and update the position, velocity and attitude through MEMS inertial navigation and GNSS integrated navigation;

[0007] Step 2: Update the speed and position of the wheel speedometer based on the attitude of the integrated navigation system and the information output by the wheel speedometer;

[0008] Step 3: Based on the position of the wheel speedometer and the position of the integrated navigation system, construct the measurement information for wheel speedometer error estimation;

[0009] Step 4: Based on the measurement information of the wheel speed gauge error estimation, use the Kalman filter algorithm to realize the online estimation of the wheel speed gauge scale coefficient error and the azimuth installation error;

[0010] Step 5: Use the online estimation results of the wheel speed gauge error to compensate for the speed and position update process of the wheel speed gauge in real time.

[0011] Furthermore, in step 2, updating the speed and position of the wheel speedometer includes updating the error estimation loop and updating the filtering loop.

[0012] Furthermore, the calculation formula for the error estimation loop update is as follows:

[0013]

[0014]

[0015] The calculation formula for the filter loop update is as follows:

[0016]

[0017] Among them, v OD This represents the raw speed output of the wheel speed meter, Δk represents the scale coefficient error of the wheel speed meter, and Δψ represents the orientation installation error of the wheel speed meter. This represents the attitude transformation matrix of the inertial navigation system from the right-front-up coordinate system to the east-north-sky coordinate system. This indicates the eastward, northward, and upward speeds of the wheel speed gauge error estimation loop. L represents the east, north, and sky speeds of the wheel speed meter filter circuit. OD-E ,λ OD-E L represents the latitude and longitude of the wheel speed gauge error estimation loop. OD-F ,λ OD-F R represents the latitude and longitude of the wheel speed gauge filter circuit. x ,R y t represents the radius of the Earth's meridian and circumference. k and t k+1 dt represents the previous time and the current time, and dt represents the time update period.

[0018] Furthermore, if the GNSS signal is good, the position and velocity of the integrated INS / GNSS navigation will be assigned to the position and velocity of the wheel speedometer in the filter loop after the INS / GNSS integrated navigation is completed.

[0019]

[0020] The subscript KF indicates the result of the integrated navigation. L KF ,λ KF These represent the eastward speed, northward speed, latitude, and longitude of the integrated navigation system, respectively.

[0021] Furthermore, in step 3, the calculation formula for the measurement information of the wheel speed gauge error estimation is as follows:

[0022]

[0023]

[0024] Z Δk =Δd×cos(ΔH) (11)

[0025] Z Δψ =Δd×sin(ΔH) (12)

[0026] Where, Δp E and Δp N p represents the eastward and northward position errors of the wheel speed gauge error estimation loop. E and p N The displacements are eastward and northward, Δd represents the horizontal positioning error of the wheel speedometer error estimation loop, d represents the distance between the vehicle's current position and initial position, ΔH represents the direction angle corresponding to the horizontal positioning error of the wheel speedometer error estimation loop, and Z represents the displacement. Δk and Z Δψ These represent the measurement information for the wheel speed gauge scale coefficient error and the azimuth installation error, respectively.

[0027] Furthermore, in step 4, the calculation formula for the online estimation model of the wheel speedometer based on Kalman filtering is as follows:

[0028]

[0029] Where X represents the state variable, Z represents the measurement variable, F represents the system matrix, H represents the measurement matrix; W and V represent the system noise and measurement noise, respectively, both of which are white noise.

[0030] Furthermore, the online estimation model is discretized, and the calculation formula is as follows:

[0031]

[0032] Where I2 represents the second-order identity matrix, Φ k / k-1 Let represent the system state transition matrix, t represent the state transition step size, and Q represent the system noise matrix.

[0033] Furthermore, in step 4, the initial values ​​of the Kalman filter are set as follows:

[0034]

[0035] Where R k Let X0 represent the initial value of the state variable, P0 represent the initial value of the state covariance matrix, and Q represent the system noise matrix.

[0036] Compared with the prior art, the beneficial effects of this application are as follows:

[0037] (1) The online estimation method for wheel speed meter error of the vehicle-mounted integrated navigation system proposed in this application designs a dual-loop update mode for the wheel speed meter, decouples the error estimation loop and the filtering loop, avoids the mutual influence of various errors in the dynamic estimation process of wheel speed meter error, and makes it easy to achieve error compensation.

[0038] (2) The online estimation method for wheel speed meter error of vehicle-mounted integrated navigation system proposed in this application is simple to implement and does not affect the original filtering algorithm architecture and calculation process. It realizes the online estimation of wheel speed meter scale coefficient error and orientation installation error through wheel speed meter position error and low-dimensional Kalman filter. The calculation amount is small and the estimation accuracy is high. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of this application, 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 application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 This is a flowchart illustrating the online estimation and compensation method.

[0041] Figure 2 The simulation estimation curve for the wheel speed gauge scale coefficient error;

[0042] Figure 3 Simulation estimation curve for the azimuth installation error of the wheel speed gauge;

[0043] Figure 4 This is the true estimation curve of the wheel speed gauge scale coefficient error;

[0044] Figure 5 The true estimation curve of the wheel speed gauge azimuth installation error;

[0045] Figure 6 The curve showing the eastward position error during vehicle-mounted testing;

[0046] Figure 7 This is a comparison curve of the northward position error in the vehicle-mounted test. Detailed Implementation

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

[0048] The principle of the online estimation and compensation method in this application is as follows: The wheel speed meter outputs the velocity along the y-axis (vertical axis) in the carrier coordinate system. When using the wheel speed meter output for velocity and position updates, it is necessary to combine it with the attitude matrix for decomposition to obtain the velocity components in the navigation coordinate system, and then obtain the position through integration.

[0049] When the wheel speed gauge has a scale coefficient error, it is equivalent to an error in the longitudinal velocity:

[0050]

[0051] Considering that the pitch and roll angles of the vehicle system are relatively small in most cases, when there is an azimuth installation error in the wheel speedometer, it is equivalent to errors in the longitudinal and lateral speeds caused by incorrect decomposition of the heading:

[0052]

[0053] Considering that Δψ is relatively small, sin(Δψ)≈Δψ, cos(Δψ)≈1, the wheel speed gauge error model can be obtained by simplifying and combining the above two formulas:

[0054]

[0055] According to the simplified wheel speed gauge error model, the error in the wheel speed gauge's scale coefficient will cause a longitudinal position error along the driving trajectory, and the error in the orientation installation will cause a lateral position error along the driving trajectory. The magnitude of the error is proportional to the driving distance.

[0056] This application designs a dual-loop mode for wheel speed meter updates. The error estimation loop does not compensate for the scale coefficient and orientation installation error when updating the wheel speed meter position. During vehicle operation, the position of the error estimation loop can be compared with the accurate position of the INS / GNSS integrated navigation to obtain the eastward position error Δp. E and northward position error Δp N Based on this, the azimuth error H1 = arctan(Δp) can be calculated. E / Δp N Similarly, according to the eastward displacement p E and northward displacement p N The azimuth angle H2 = arctan(p) of the current position relative to the initial position can be calculated. E / p N The difference between the two yields the decomposed angle ΔH of the wheel speed gauge position error:

[0057] ΔH=H2-H1 (26)

[0058] Based on the characteristics of the wheel speed gauge scale coefficient error and the orientation installation error, the correspondence between the wheel speed gauge error and the position error can be obtained:

[0059]

[0060] Directly using the above correspondence formula to calculate wheel speed gauge error may be affected by various noises, reducing estimation accuracy. Therefore, the above correspondence formula is adjusted to a state-space model, corresponding to the expression of state variable X. When modeling the error, the wheel speed gauge error is considered as a constant value model. Considering that the wheel speed gauge error has certain random and slow variation characteristics, the state noise matrix Q can be appropriately set, and then online recursive estimation of the wheel speed gauge error can be achieved through Kalman filtering.

[0061] Furthermore, considering that the position error caused by wheel speedometer error is proportional to the vehicle's travel range, the filter's estimation accuracy is low when the initial travel range is short. In this case, error compensation should not be performed; compensation should only be performed after the vehicle has traveled beyond a certain range. The accuracy index of the high-precision vehicle-mounted integrated navigation system in the inertial navigation / wheel speedometer combination is that the error should not exceed 2m when the vehicle travels 1km. If the 2m positioning error is equally distributed among the wheel speedometer scale coefficient error and the orientation installation error, the corresponding error estimation accuracy should meet 1000ppm and 3.5 arcminutes. If the maximum vehicle travel range is 500m, the position error caused by wheel speedometer errors of 1000ppm and 3.5 arcminutes is 0.5m, which meets the measurement accuracy requirements. Therefore, the wheel speedometer error compensation threshold in step 5 is set to 500m.

[0062] The present application will now be further described with reference to the accompanying drawings.

[0063] Figure 1 This is a flowchart illustrating the online estimation and compensation method. The online estimation method shown employs a dual-loop update mode for the wheel speedometer, decoupling the error estimation loop and the filtering loop. It consists of only five steps, making it simple to implement without affecting the original filtering algorithm architecture and calculation process. Online estimation of the wheel speedometer scale coefficient error and azimuth installation error is achieved through the wheel speedometer position error and a low-dimensional Kalman filter, resulting in low computational complexity and high estimation accuracy.

[0064] In step 1, after the vehicle-mounted integrated navigation system is powered on, the GNSS module begins satellite acquisition and calculation. Once positioning and orientation are completed, the initial position and dual-antenna heading are assigned to the inertial navigation system. The pitch and roll angles of the inertial navigation system are obtained using the following formulas:

[0065]

[0066] Where θ0 and γ0 are the initial pitch and roll angles, and g is the local gravitational acceleration. and This is the average value of the x and y accelerometer outputs during position initialization. Additionally, the initial latitude L0 and initial longitude λ0 are recorded.

[0067] In step 4, the wheel speed gauge calibration coefficient error and azimuth installation error are estimated online using the Kalman filter algorithm. The calculation formula is as follows:

[0068]

[0069] Where R k P represents the measurement noise matrix. k The covariance matrix represents the state variables.

[0070] In step 5, after obtaining the online estimation results of the wheel speedometer scale factor error and the azimuth installation error, if the distance d of the vehicle relative to the initial position exceeds the preset wheel speedometer error compensation threshold, then update the scale factor error Δk and the azimuth installation error Δψ of the wheel speedometer in the filtering loop;

[0071] On the basis of the above calculations, calculate the speed and position of the wheel speedometer in the filtering loop. If the GNSS signal quality deteriorates or positioning is unavailable during subsequent navigation, use the speed and position information of the wheel speedometer filtering loop as measurements, and combine them with the MEMS pure inertial navigation results for integrated navigation, thereby improving the positioning performance.

[0072] To verify the correctness of the online estimation method for wheel speedometer errors proposed in this application, a simulation verification experiment was first designed. A "square" motion trajectory was generated by a trajectory generator. Various errors were added according to the basic performances of the MEMS inertial navigation, GNSS module, and wheel speedometer, and then the simulation estimation results of the wheel speedometer errors were obtained through a simulation program.

[0073] Figure 2 is the simulation estimation curve for the wheel speedometer scale factor error, Figure 3 The simulation estimation results of the wheel speedometer azimuth installation error are given. The set true value of the wheel speedometer scale factor error is 1000 ppm, and the set true value of the azimuth installation error is 3 arc minutes. The simulation results show that the estimated result of the scale factor error is 980 ppm, and the estimated result of the azimuth installation error is 2.8 arc minutes. The estimation accuracy is relatively high, and the curve fluctuates less during the estimation process, proving that the online estimation method for wheel speedometer errors proposed in this application is accurate and feasible.

[0074] The zero-bias stabilities of the MEMS gyroscope and accelerometer used in this inertial navigation system are 10° / h and 100 μg respectively. The GNSS module uses the UM482 chip, and the wheel speedometer uses an optical encoder disk fixed and installed in an external form through the wheel hub. The number of pulses per full circle of the encoder disk is 4096. The total duration of the vehicle test is about 20 minutes, the driving route is a real urban road, the running modes include straight-line acceleration and deceleration, turning, etc., the heading motion covers the entire range of 0 - 360°, and the maximum speed is about 30 m / s.

[0075] Figure 4 and Figure 5 The online estimation results of the wheel speedometer scale factor error and the azimuth installation error obtained from the vehicle test of the specific application of this application are given. After the test vehicle has traveled more than 500 m, error estimation is started. The scale factor error is about -400 ppm, and the azimuth installation error is about 21.3 arc minutes. Moreover, the fluctuations of both are small, which basically coincides with the simulation results, proving that the online estimation results of the wheel speedometer scale factor error and the azimuth installation error during the vehicle test are credible.

[0076] To further verify the accuracy of the online wheel speedometer error estimation results in the vehicle-mounted test, the collected raw data can be processed offline. During processing, a straight section of the vehicle is selected, GNSS positioning information is ignored in the software, and inertial navigation / wheel speedometer integrated navigation is forced. The position output after integration is observed. Using the original inertial navigation / GNSS integrated navigation position output as the reference true value, the position error is calculated, thereby verifying the accuracy of the online estimation and compensation of wheel speedometer error.

[0077] Figure 6 and Figure 7 A comparison chart of the eastward and northward position errors obtained after offline processing in the vehicle-mounted test of this application is provided. During the implementation, the inertial navigation / GNSS integrated navigation was switched to the inertial navigation / wheel velocity sensor integrated navigation at 600-800 seconds of navigation time. This stage lasted 200 seconds, the driving direction was basically east-west, and the driving distance was about 2800m. Figure 6 and Figure 7 The dashed curves in the figure represent the position error before wheel speedometer error compensation. It can be seen that the maximum position error in the east direction is approximately 3m, and the maximum position error in the north direction is approximately 15m. The larger north direction position error is related to a significant azimuth installation error in the wheel speedometer. After error compensation, the wheel speedometer filter circuit, combined with inertial navigation, yields a maximum east direction position error of 0.8m and a maximum north direction position error of 2.6m. Based on the propagation law of wheel speedometer error, the estimated accuracy of the wheel speedometer scale coefficient error is estimated to be 280ppm, the estimated accuracy of the azimuth installation error is 3.1 arcminutes, and the compensated overall positioning error is approximately 1‰, demonstrating the accuracy of the online estimation and compensation of wheel speedometer error proposed in this application.

[0078] While specific embodiments of this application have been described above, those skilled in the art should understand that these are merely illustrative examples, and the scope of protection of this application is defined by the appended claims. Those skilled in the art can make various changes or modifications to these embodiments without departing from the principles and essence of this application, but all such changes and modifications fall within the scope of protection of this application.

Claims

1. A method for online estimation and compensation of wheel speedometer error in a vehicle-mounted integrated navigation system, characterized in that, The method includes the following steps: Step 1: Start the vehicle-mounted integrated navigation system, complete the initialization of the integrated navigation position and attitude, and update the position, velocity and attitude through MEMS inertial navigation and GNSS integrated navigation; Step 2: Update the speed and position of the wheel speedometer based on the attitude of the integrated navigation system and the information output by the wheel speedometer; Step 3: Based on the position of the wheel speedometer and the position of the integrated navigation system, construct the measurement information for wheel speedometer error estimation; Step 4: Based on the measurement information of the wheel speed gauge error estimation, use the Kalman filter algorithm to realize the online estimation of the wheel speed gauge scale coefficient error and the azimuth installation error; Step 5: Use the online estimation results of the wheel speed gauge error to compensate for the speed and position update process of the wheel speed gauge in real time; In step 4, the calculation formula for the online estimation model of wheel speedometer based on Kalman filtering is as follows: Where X represents the state variable, Z represents the measurement variable, F represents the system matrix, and H represents the measurement matrix; W and V represent the system noise and measurement noise, respectively, both of which are white noise; the online estimation model is discretized, and the calculation formula is as follows: Where I2 represents the second-order identity matrix, Φ k / k-1 Let represent the system state transition matrix, t represent the state transition step size, and Q represent the system noise matrix.

2. The method according to claim 1, characterized in that, In step 2, updating the speed and position of the wheel speedometer includes updating the error estimation loop and updating the filtering loop.

3. The method according to claim 2, characterized in that, The calculation formula for the error estimation loop update is as follows: The calculation formula for the filter loop update is as follows: Among them, v OD This represents the raw speed output of the wheel speed meter, Δk represents the scale coefficient error of the wheel speed meter, and Δψ represents the orientation installation error of the wheel speed meter. This represents the attitude transformation matrix of the inertial navigation system from the right-front-up coordinate system to the east-north-sky coordinate system. This indicates the eastward, northward, and upward speeds of the wheel speed gauge error estimation loop. L represents the east, north, and sky speeds of the wheel speed meter filter circuit. OD-E ,λ OD-E L represents the latitude and longitude of the wheel speed gauge error estimation loop. OD-F ,λ OD-F R represents the latitude and longitude of the wheel speed gauge filter circuit. x ,R y t represents the radius of the Earth's meridian and circumference. k and t k+1 dt represents the previous time and the current time, and dt represents the time update period.

4. The method according to claim 3, characterized in that, If the GNSS signal is good, after completing the INS / GNSS integrated navigation, the position and velocity of the integrated navigation will be assigned to the position and velocity of the wheel speedometer in the filter loop: The subscript KF indicates the result of the integrated navigation. L KF ,λ KF These represent the eastward speed, northward speed, latitude, and longitude of the integrated navigation system, respectively.

5. The method according to claim 1, characterized in that, In step 3, the calculation formula for the measurement information of the wheel speed gauge error estimation is as follows: Z Δk =Δd×cos(ΔH) (11) Z Δψ =Δd×sin(ΔH) (12) Where, Δp E and Δp N p represents the eastward and northward position errors of the wheel speed gauge error estimation loop. E and p N The displacements are eastward and northward, Δd represents the horizontal positioning error of the wheel speedometer error estimation loop, d represents the distance between the vehicle's current position and initial position, ΔH represents the direction angle corresponding to the horizontal positioning error of the wheel speedometer error estimation loop, and Z represents the displacement. Δk and Z Δψ These represent the measurement information for the wheel speed gauge scale coefficient error and the azimuth installation error, respectively.

6. The method according to claim 1, characterized in that, In step 4, the initial values ​​of the Kalman filter are set as follows: Where R k Let X0 represent the initial value of the state variable, P0 represent the initial value of the state covariance matrix, and Q represent the system noise matrix.

Images