Vehicle speed fusion estimation method and device based on GPS, IMU and wheel speed sensor signals

Abstract

The application discloses a vehicle speed fusion estimation method and device based on GPS, IMU and wheel speed sensor signals, introduces other system estimated road longitudinal slope values, compensates and filters vehicle longitudinal acceleration, adopts steering compensation, maximum and minimum wheel speed method and improved dynamic slope method to construct a three-layer architecture wheel speed processing algorithm to realize high robustness processing of wheel speed, based on kinematics principle, takes the vehicle speed output by the wheel speed processing algorithm as a measurement value of IMU Kalman filtering, reasonably models the measurement noise, realizes adaptive estimation of the vehicle speed, converts the signal under the navigation coordinate system to the vehicle body coordinate system through coordinate conversion, models the noise, and then processes the signal through the Kalman filtering algorithm, and finally, based on the optimal fusion theory of multi-sensor Kalman filtering, optimally fuses the vehicle speeds output by the GPS Kalman filtering and the IMU Kalman filtering, and realizes higher precision and stronger robustness of the vehicle speed estimation.

Description

Technical Field

[0001] This application relates to the field of vehicle control technology, and in particular to a vehicle speed fusion estimation method and apparatus based on GPS, IMU and wheel speed sensor signals. Background Technology

[0002] Longitudinal vehicle speed primarily refers to the longitudinal velocity at the vehicle's center of gravity. It is a crucial input for many automotive safety systems, including Anti-lock Braking System (ABS), Traction Control System (TCS), and Electronic Stability Control (ESC). The accuracy of speed estimation directly impacts vehicle control stability and driving safety. Under stable conditions of constant speed driving, the longitudinal speed obtained directly using the average wheel speed method is very close to the true longitudinal speed. However, under conditions triggered by safety systems such as ABS, TCS, or ESC, the longitudinal speed estimated using the average wheel speed method often deviates significantly from the true value. In such cases, other methods must be employed to obtain a reliable estimated speed to ensure vehicle safety. Furthermore, for two-wheel-drive vehicles, the speed estimated based on the driven wheel speeds is relatively close to the longitudinal speed and is accurate under most conditions. However, for four-wheel-drive vehicles, all wheels can potentially be drive wheels, thus placing higher demands on the speed estimation algorithm.

[0003] Currently, speed estimation algorithms for four-wheel drive vehicles are mainly divided into two categories: dynamic estimation algorithms and kinematic estimation algorithms.

[0004] Dynamics-based estimation algorithms primarily rely on vehicle dynamics models, utilizing methods such as Kalman filtering and sliding mode observers to construct state observers, thereby estimating vehicle speed. These algorithms depend on high-precision dynamic models and typically require a large number of signals to construct them. These signals, besides those from sensors, may also require additional estimation, such as tire forces. The introduction of numerous signals increases the burden on signal processing; furthermore, the failure of any single signal can cause the entire algorithm to collapse, thus these algorithms generally have poor robustness. However, their advantage lies in their ability to achieve co-estimation of other signals, such as lateral vehicle speed and vehicle slip angle.

[0005] Kinematic estimation algorithms are based on kinematic principles and process wheel speed signals and inertial measurement unit (IMU) signals. Common algorithms include the maximum wheel speed method, the average wheel speed method, the dynamic slope method, and the acceleration integration method. These algorithms are widely used because they are simple, reliable, and require only a small amount of signal for vehicle speed estimation. However, their simplicity also comes with drawbacks such as over-reliance on empirical processing, susceptibility to noise, and a tendency to accumulate errors. For example, the maximum wheel speed method uses the maximum wheel speed as the estimated vehicle speed under deceleration conditions. However, under low-adhesion conditions such as icy or snowy roads, the wheel speeds of all four wheels may deviate significantly from the actual vehicle speed, making the maximum wheel speed method inaccurate. The dynamic slope method addresses this problem to some extent by introducing convex point selection and dynamic slope calculation. However, convex points do not necessarily appear at peaks; due to signal errors and fluctuations, they may also appear at troughs, leading to severe distortion in the estimated vehicle speed. Therefore, the convex point selection problem must be improved. The acceleration integral method suffers from significant noise in the vehicle acceleration signal, including mixed slope deviations, zero bias, and random noise. Long-term use can result in large cumulative errors. While the problems with kinematic methods may not be prominent in traditional vehicles, the increasing demands on vehicle safety systems in recent years have led to higher requirements for the accuracy and reliability of vehicle speed estimation, necessitating further improvements.

[0006] In recent years, with the upgrading of vehicle systems, the number of sensors has gradually increased. Sensor signals from some sensors not found in traditional vehicles can now be used for vehicle speed estimation, a typical example being the Global Positioning System (GPS). Currently, GPS using Real-Time Kinematic (RTK) technology can achieve centimeter-level accuracy. However, GPS signals are inherently susceptible to environmental interference, and in some scenarios such as tunnels and urban canyons, signal quality degrades or even ceases. Therefore, relying solely on GPS cannot achieve reliable all-weather vehicle speed estimation. Given the limitations of the aforementioned methods, it is necessary to explore new vehicle speed estimation algorithms with wider applicability and higher estimation accuracy. Summary of the Invention

[0007] This application provides a vehicle speed fusion estimation method and device based on GPS, IMU and wheel speed sensor signals. Based on the multi-sensor optimal fusion theory and the construction of a hierarchical fusion algorithm for multi-source signals, it expands the applicability of vehicle speed estimation algorithms and achieves robust and high-precision vehicle speed estimation.

[0008] The first aspect of this application provides a vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals, comprising the following steps: compensating and filtering the longitudinal acceleration output by the IMU using the road longitudinal slope value to obtain the actual longitudinal acceleration of the vehicle; performing steering compensation on the wheel speed of the vehicle under steering conditions, converting the wheel speed to the vehicle's center of gravity; using the maximum and minimum wheel speed method to perform a comparison mechanism between the estimated vehicle speed derivative and the overall vehicle acceleration, and determining the first estimated vehicle speed according to the current operating conditions; calculating the first estimated vehicle speed using the dynamic slope method with three convex points and two slopes; and using a vehicle speed verification mechanism to verify the first estimated vehicle speed. The estimated vehicle speed is verified by using a convex point and slope verification mechanism to obtain the initial value and dynamic slope of the vehicle speed calculation. After integration, the second estimated vehicle speed is obtained. The GPS signal of the vehicle is subjected to coordinate transformation and noise modeling, and filtered using the Kalman filter algorithm to obtain the GPS output vehicle speed value. A Kalman filter is constructed based on the kinematic equations. The second estimated vehicle speed is used as the measurement value of the Kalman filter for filtering, and the measurement noise is modeled to obtain the IMU output vehicle speed value. The GPS output vehicle speed value and the IMU output vehicle speed value are optimally fused to obtain the final estimated vehicle speed.

[0009] Optionally, in one embodiment of this application, the actual longitudinal acceleration of the vehicle is:

[0010] a x = IMU -gsin(θ)

[0011] Among them, a x a represents the actual longitudinal acceleration of the vehicle. IMU θ represents the raw acceleration output by the IMU, and θ represents the longitudinal slope of the road.

[0012] Optionally, in one embodiment of this application, steering compensation is performed on the wheel speeds of the vehicle under steering conditions, converting the wheel speeds into speeds at the vehicle's center of gravity. The four-wheel wheel speeds after steering compensation are:

[0013]

[0014]

[0015]

[0016]

[0017] Among them, V ij ,∈{F,R},∈{R,L} are the compensated wheel speeds, W ij The initial wheel speed is given by δ, the front wheel steering angle is given by ψ, and the yaw angle is given by k. fk is the distance between the left and right wheels of the front axle. r This is the distance between the left and right wheels of the rear axle.

[0018] Optionally, in one embodiment of this application, a comparison mechanism between the estimated vehicle speed derivative and the overall vehicle acceleration is performed using the maximum and minimum wheel speed method, and a first estimated vehicle speed is determined based on the current operating conditions, including:

[0019] The current operating condition of the vehicle is determined based on the vehicle's actual longitudinal acceleration;

[0020] When the current operating condition is an acceleration condition, if the derivative of the estimated vehicle speed is greater than the actual longitudinal acceleration of the vehicle, then the first estimated vehicle speed is the minimum value among the integral value of the longitudinal acceleration and the wheel speeds of the four wheels; otherwise, the first estimated vehicle speed is the minimum value among the wheel speeds of the four wheels.

[0021] When the current operating condition is a deceleration condition, if the derivative of the estimated vehicle speed is less than the actual longitudinal acceleration of the vehicle, then the first estimated vehicle speed is the maximum value of the longitudinal acceleration integral value and the four wheel speeds; otherwise, the first estimated vehicle speed is the maximum value of the four wheel speeds.

[0022] When the current operating condition is a steady-state condition, the first estimated vehicle speed is the average of the four wheel speeds.

[0023] Optionally, in one embodiment of this application, under acceleration conditions, the first estimated vehicle speed is calculated using the dynamic slope method with three convex points and two slopes, and the first estimated vehicle speed is verified using a vehicle speed verification mechanism to obtain the second estimated vehicle speed, including:

[0024] When the vehicle speed stability flag is 1 and the wheel acceleration is greater than the acceleration threshold at the current moment, the current working condition is the trigger working condition, which means that the first estimated vehicle speed of the vehicle deviates from the actual vehicle speed. The dynamic slope method is entered, and the two sets of convex points and the corresponding time are updated. The value of convex point 1 is updated to the original value of convex point 2, and the value of convex point 2 is updated to the new convex point. The initial value of the current vehicle speed calculation is taken as convex point 2, and the dynamic slope is the longitudinal acceleration of the vehicle.

[0025] If the current working condition is not the triggering working condition, and the vehicle speed stability flag is 0 and the first estimated vehicle speed change rate of the vehicle exceeds the first preset threshold, or the vehicle speed confidence flag is 0, the current working condition is the deviation working condition. The initial value of the current vehicle speed calculation is the vehicle speed output by the dynamic slope method at the previous moment. The dynamic slope is the longitudinal acceleration of the vehicle.

[0026] If the current operating condition is not the aforementioned deviation condition and a convex point appears on the speed curve, then the current operating condition is a dynamic slope condition. Based on the two recorded convex points and the first estimated vehicle speed, the corresponding two slope segments are calculated and entered into the convex point slope threshold judgment. If both slope segments are less than the second preset threshold, the average of the two slope segments is taken as the dynamic slope. Otherwise, the longitudinal acceleration of the vehicle is used as the dynamic slope. The difference between the vehicle speed estimated by the dynamic slope method at the previous moment and the first estimated vehicle speed of the vehicle is judged. When the difference is less than the lower threshold, the convex point and time sequence number are updated, and the initial value of the vehicle speed calculation is taken as convex point 2. Otherwise, it is judged whether it is greater than the upper threshold. If it is, the average of the vehicle speed estimated by the dynamic slope method at the previous moment and the first estimated vehicle speed of the vehicle is taken as the new convex point as convex point 2, and convex point 2 is taken as the initial value of the vehicle speed calculation. Otherwise, the convex point is not updated, and the vehicle speed estimated by the dynamic slope method at the previous moment is taken as the initial value of the vehicle speed calculation.

[0027] If the current operating condition is not the triggering operating condition, the deviation operating condition, or the dynamic slope operating condition, then the current operating condition is the default operating condition, and no updates to the convex point and dynamic slope are performed.

[0028] The difference between the vehicle speed estimated by the dynamic slope method at the previous moment and the first estimated vehicle speed of the vehicle is judged. If it is less than the third threshold, the vehicle speed confidence flag is set to 1; otherwise, the vehicle speed confidence flag is set to 0. The vehicle speed is calculated based on the vehicle speed stability flag. When the vehicle speed stability flag is 0, the second estimated vehicle speed of the vehicle is calculated based on the initial value of the vehicle speed calculation and the dynamic slope. When the vehicle speed stability flag is 1, the first estimated vehicle speed of the vehicle is used as the second estimated vehicle speed of the vehicle.

[0029] Optionally, in one embodiment of this application, before fusing the GPS output vehicle speed value and the IMU output vehicle speed value, the method further includes: determining whether there is an anomaly in the vehicle's GPS signal; if there is an anomaly, directly using the IMU output vehicle speed value as the final estimated vehicle speed.

[0030] A second aspect of this application provides a vehicle speed fusion estimation device based on GPS, IMU, and wheel speed sensor signals, comprising: an acceleration compensation module for compensating and filtering the longitudinal acceleration output by the IMU using the road longitudinal slope value to obtain the actual longitudinal acceleration of the vehicle; and a wheel speed processing module for performing steering compensation on the wheel speed of the vehicle under steering conditions, converting the wheel speed to the vehicle's center of gravity, performing a comparison mechanism between the estimated vehicle speed derivative and the overall vehicle acceleration using the maximum and minimum wheel speed method, determining a first estimated vehicle speed based on the current operating conditions, calculating the first estimated vehicle speed using a dynamic slope method with three convex points and two slopes, and verifying the first estimated vehicle speed using a vehicle speed verification mechanism. A convex point and slope verification mechanism obtains the initial value and dynamic slope for vehicle speed calculation, and after integration, obtains the second estimated vehicle speed. A GPS Kalman filter module performs coordinate transformation and noise modeling on the vehicle's GPS signal, and uses the Kalman filter algorithm to filter it, obtaining the GPS output vehicle speed value. An IMU Kalman filter module constructs a Kalman filter based on kinematic equations, uses the second estimated vehicle speed as the measurement value of the Kalman filter for filtering, and models the measurement noise to obtain the IMU output vehicle speed value. A hierarchical fusion algorithm module performs optimal fusion of the GPS output vehicle speed value and the IMU output vehicle speed value to obtain the final estimated vehicle speed.

[0031] Optionally, in one embodiment of this application, it further includes: an anomaly module, used to determine whether there is an anomaly in the GPS signal of the vehicle, and when there is an anomaly in the GPS signal, directly using the vehicle speed value output by the IMU as the final estimated speed of the vehicle.

[0032] A third aspect of this application provides an electronic device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to perform a vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals as described in the above embodiments.

[0033] A fourth aspect of this application provides a computer-readable storage medium having a computer program stored thereon, which is executed by a processor to perform the vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals as described in the above embodiments.

[0034] This application discloses a vehicle speed fusion estimation method and apparatus based on GPS, IMU, and wheel speed sensor signals. It uses four-wheel wheel speed signals, IMU signals, and GPS signals as a foundation and employs a multi-sensor fusion method to estimate longitudinal vehicle speed. First, it introduces the road longitudinal slope value estimated by other systems to compensate for and filter the vehicle's longitudinal acceleration. A three-layer wheel speed processing algorithm is constructed using steering compensation, the maximum / minimum wheel speed method, and an improved dynamic slope method to achieve robust wheel speed processing. Based on kinematic principles, the vehicle speed output by the wheel speed processing algorithm is used as the measurement value of the IMU Kalman filter, and the measurement noise is reasonably modeled to achieve adaptive estimation. For the GPS signal, coordinate transformation converts the signal from the navigation coordinate system to the vehicle coordinate system, and noise is modeled before being processed by the Kalman filter algorithm. Finally, based on the optimal fusion theory of multi-sensor Kalman filtering, the vehicle speed outputs from the GPS Kalman filter and the IMU Kalman filter are optimally fused to achieve higher accuracy and stronger robustness in vehicle speed estimation.

[0035] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0036] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0037] Figure 1 This is a flowchart illustrating a vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals, according to an embodiment of this application.

[0038] Figure 2 This is a schematic diagram of an overall architecture for wheel speed processing according to an embodiment of this application;

[0039] Figure 3 This is a flowchart of a maximum and minimum wheel speed method according to an embodiment of this application;

[0040] Figure 4 This is a flowchart of a dynamic slope method according to an embodiment of this application;

[0041] Figure 5 This is a schematic diagram of a dynamic slope method without "pseudo-bumps" according to an embodiment of this application;

[0042] Figure 6 This is a schematic diagram illustrating the failure of the dynamic slope method when "pseudo-bumps" exist, according to an embodiment of this application.

[0043] Figure 7 This is a block diagram of a fusion algorithm provided according to an embodiment of this application;

[0044] Figure 8 This is an example diagram of a vehicle speed fusion estimation device based on GPS, IMU and wheel speed sensor signals according to an embodiment of this application;

[0045] Figure 9 A schematic diagram of the structure of the electronic device provided in the application embodiment. Detailed Implementation

[0046] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain this application, and should not be construed as limiting this application.

[0047] Figure 1 This is a flowchart of a vehicle speed fusion estimation method based on GPS, IMU and wheel speed sensor signals provided according to an embodiment of this application.

[0048] like Figure 1 As shown, the vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals includes the following steps:

[0049] In step S101, the longitudinal acceleration output by the IMU is compensated and filtered using the longitudinal slope value of the road to obtain the actual longitudinal acceleration of the vehicle.

[0050] Since the longitudinal acceleration output by the IMU includes gradient deviation, zero bias, and random noise, it must be processed accordingly before use. The actual longitudinal acceleration of the vehicle can be expressed as:

[0051] a x = IMU -gsin(θ)

[0052] Among them, a x a represents the actual longitudinal acceleration of the vehicle. IMU The acceleration is the raw acceleration output by the IMU, and θ is the longitudinal slope of the road (this includes the effect of the pitch angle, but since its effect on acceleration is the same as that of the slope, it is not distinguished here). Considering that the acceleration at this point still contains a lot of noise, a Kalman filter algorithm is used to process it. The basic principle of Kalman filtering is as follows:

[0053] For a linear system:

[0054] x(k)=x(k-1)+u(k)+(k)

[0055] y(k)=x(k)+ε()

[0056] In the formula, x represents the system state, y represents the system measurement value, A, B, and H represent the system matrix, input matrix, and measurement matrix, respectively, u represents the system input, w and ε represent the system noise and measurement noise that satisfy the Gaussian distribution, respectively, and k represents the calculation time.

[0057] The Kalman filter algorithm estimates the system state optimally through iterative iteration of the following set of equations:

[0058] x(k|k-1)=x(k-1|k-1)+u(k)

[0059] P(k|k-1)=P(k-1|k-1)A T +

[0060] K(k)=(k|k-1)H T [HP(k|k-1)H T +] -1

[0061] x(k|k)=(k|k-1)+(k)y(k)-x(k|k-1)

[0062] P(k|k)=[IK(k)H]P(k|k-1)

[0063] In the formula, P is the covariance, Q and R are the variances of system noise and measurement noise, respectively, K is the Kalman filter gain, and I is the identity matrix.

[0064] The acceleration signal processed as described above still contains some colored noise, zero bias, and other interference terms, but it has reached a usable level and can be used in subsequent algorithms.

[0065] In step S102, steering compensation is performed on the wheel speed of the vehicle under steering conditions. The wheel speed is converted to the vehicle's center of gravity. The maximum and minimum wheel speed method is used to perform a comparison mechanism between the estimated vehicle speed derivative and the vehicle acceleration. The first estimated vehicle speed is determined based on the current working conditions. The first estimated vehicle speed is calculated using the dynamic slope method with three convex points and two slopes. The first estimated vehicle speed is verified using a vehicle speed verification mechanism. The initial value of the vehicle speed calculation and the dynamic slope are obtained using the convex point and slope verification mechanism. After integration, the second estimated vehicle speed is obtained.

[0066] Wheel speed signals are a crucial foundation for kinematic estimation methods. Under stable conditions such as uniform straight-line driving, wheel speed can be directly used to obtain a relatively accurate longitudinal vehicle speed. However, under conditions of strong acceleration, forced braking, when TCS or ABS systems are triggered, or on icy or snowy roads, the vehicle's wheel speed often deviates significantly from the true vehicle speed, requiring further processing before it can be used for speed estimation.

[0067] To address the limitations of current maximum and minimum wheel speed methods (limited applicability, distortion of all four wheel speeds leading to inaccurate speed estimation), and the susceptibility of the dynamic slope method to uncertainties arising from bumps, which can cause errors in bump and slope calculations, this paper proposes a three-layer wheel speed processing architecture that integrates steering compensation, the maximum and minimum wheel speed method, and the dynamic slope method, taking into account conditions such as steering and large slippage. This architecture achieves robust vehicle speed estimation using only wheel speed and acceleration signals, particularly effective under extreme conditions such as icy roads, strong acceleration, and forced movement.

[0068] Specifically, the wheel speed processing algorithm employs a three-layer architecture: the first layer is wheel speed steering compensation, the second layer is an estimation algorithm based on the maximum and minimum wheel speed method, and the third layer is a dynamic slope method. These three layers are executed sequentially, as follows: Figure 2 The first layer of steering compensation is used to handle the deviation between wheel speed and center of gravity speed under steering conditions; the second layer, the maximum and minimum wheel speed method, preprocesses the four-wheel wheel speed signals after steering compensation, integrates them into one signal, and then enters the third layer, the dynamic slope method, which realizes the calculation of convex points and dynamic acceleration estimation (dynamic slope) by fusing wheel acceleration and whole vehicle acceleration signals, thereby achieving a highly robust estimation of longitudinal vehicle speed.

[0069] Firstly, wheel speed steering compensation is performed. Under steering conditions, the wheel speeds on both sides are not equal to the vehicle's center of gravity speed. To ensure the accuracy of subsequent processing, the wheel speed signal needs to be compensated for steering and converted into the speed at the vehicle's center of gravity (in this application, wheel speed refers to the value obtained by multiplying the radius by the wheel angular velocity, and the unit is the same as that of longitudinal vehicle speed):

[0070]

[0071]

[0072]

[0073]

[0074] Among them, V ij ,∈{F,R},∈{R,L} are the compensated wheel speeds, W ij The initial wheel speed is given by δ, the front wheel steering angle is given by ψ, and the yaw angle is given by k. f k is the distance between the left and right wheels of the front axle. r This is the distance between the left and right wheels of the rear axle.

[0075] Optionally, in one embodiment of this application, a comparison mechanism between the estimated vehicle speed derivative and the vehicle acceleration is performed using the maximum-minimum wheel speed method, and the first estimated vehicle speed is determined according to the current operating condition, including: determining the current operating condition of the vehicle based on the actual longitudinal acceleration of the vehicle; when the current operating condition is an acceleration condition, if the derivative of the estimated vehicle speed is greater than the actual longitudinal acceleration of the vehicle, then the first estimated vehicle speed is the minimum value among the longitudinal acceleration integral value and the four wheel speeds, otherwise, the first estimated vehicle speed is the minimum value among the four wheel speeds; when the current operating condition is a deceleration condition, if the derivative of the estimated vehicle speed is less than the actual longitudinal acceleration of the vehicle, then the first estimated vehicle speed is the maximum value among the longitudinal acceleration integral value and the four wheel speeds, otherwise, the first estimated vehicle speed is the maximum value among the four wheel speeds; when the current operating condition is a steady-state condition, the first estimated vehicle speed is the average value of the four wheel speeds.

[0076] In the maximum and minimum wheel speed method, three working conditions are processed according to the magnitude of acceleration. An improvement mechanism is introduced by comparing the estimated rate of change of vehicle speed with the vehicle acceleration. This alleviates the problem of severe wheel speed distortion in extreme working conditions such as large slippage to a certain extent, and ensures that the estimated vehicle speed does not deviate too much from the true value. For the vehicle acceleration, slope compensation and optimal filtering are also performed to ensure sufficient accuracy and quality.

[0077] Specifically, the compensated wheel speeds of the four wheels enter this part of the processing, such as... Figure 3 As shown. Generally speaking, smaller wheel speeds have higher reliability under acceleration conditions, larger wheel speeds have higher reliability under deceleration conditions, and all wheel speeds have high reliability under steady conditions. However, the above principles are not perfect. For example, smaller wheel speeds may be severely distorted under acceleration conditions. Therefore, this section introduces a mechanism for comparing the derivative of the estimated vehicle speed with the overall vehicle acceleration.

[0078] Under acceleration conditions, the minimum wheel speed is generally used. If the derivative of the estimated vehicle speed is greater than the vehicle acceleration, it indicates that the minimum wheel speed is likely to exceed the actual vehicle speed. In this case, the minimum value between the integral value of acceleration and the wheel speed signal is used to ensure that the estimated vehicle speed does not deviate too much from the true value. Under deceleration conditions, the maximum wheel speed is generally used. If the derivative of the estimated vehicle speed is less than the vehicle acceleration, it indicates that the maximum wheel speed is likely to be less than the actual vehicle speed. In this case, the maximum value between the integral value of acceleration and the wheel speed signal should be used.

[0079] Based on the above principles, this section constructs a maximum-minimum wheel speed method, which sets two thresholds: an acceleration threshold TH. A and time threshold TH t It is divided into three operating conditions based on the current acceleration:

[0080] 1) Acceleration mode

[0081] When the vehicle acceleration exceeds the acceleration threshold THA And the duration exceeding the acceleration threshold exceeds the time threshold TH. t At this point, the algorithm determines that the vehicle has entered an acceleration phase. Under this phase, if the derivative of the estimated vehicle speed is... Greater than the vehicle acceleration a x The algorithm considers the vehicle acceleration signal at this point to be highly reliable, and therefore takes the minimum value between the integral of the acceleration and the wheel speed signal, i.e., V. mm =min(V pre + x dT,V ij ), where V pre The vehicle speed is the estimated value from the previous moment, dT is the sampling time, and V is the speed of the vehicle. ij This is the wheel speed output by the steering compensation module. Otherwise, the algorithm takes the minimum wheel speed, i.e., V. mm =min(V ij ).

[0082] 2) Deceleration condition

[0083] When the vehicle acceleration is less than the acceleration threshold TH A The negative of the value, and the duration of the negative of the value less than the acceleration threshold exceeds the time threshold TH. t At this point, the algorithm determines that the vehicle has entered a deceleration condition. Under this condition, if the derivative of the estimated vehicle speed is... Less than the vehicle acceleration a x The algorithm considers the vehicle acceleration signal at this point to be highly reliable, and therefore takes the maximum value between the integral value of the acceleration and the wheel speed signal, i.e., V. mm =max(V pre + x dT,V ij Otherwise, the algorithm takes the maximum wheel speed, i.e., V. mm =max(V ij ).

[0084] 3) Steady-state operating conditions

[0085] If neither of the above two operating conditions is met, i.e., the vehicle acceleration a x The absolute value is less than the acceleration threshold TH A When the algorithm considers the vehicle to have entered a steady-state condition, the estimated vehicle speed is the average wheel speed, i.e., V. mm =ean(V ij ).

[0086] In the execution of the above algorithm, each time a certain working state is entered, a certain duration, namely the time threshold TH, is required. t Only then can it be transferred out, thereby limiting the frequent state switching caused by acceleration signal fluctuations and reducing the occurrence of vehicle speed estimation jitter.

[0087] Optionally, in one embodiment of this application, under acceleration conditions, the first estimated vehicle speed is calculated using a dynamic slope method with three convex points and two slopes, and the first estimated vehicle speed is verified using a vehicle speed verification mechanism to obtain the second estimated vehicle speed. This includes: when the vehicle speed stability flag is 1 and the wheel acceleration at the current moment is greater than the acceleration threshold, the current condition is a triggered condition, indicating that the first estimated vehicle speed deviates from the true vehicle speed. The dynamic slope method is then entered, and the two sets of convex points and their corresponding times are updated. The value of convex point 1 is updated to the original value of convex point 2, and the value of convex point 2 is updated to the new convex point. The initial value for the current vehicle speed calculation is taken from convex point 2. The dynamic slope is the vehicle's longitudinal acceleration. If the current operating condition is not a trigger condition, and the vehicle speed stability flag is 0 and the vehicle's first estimated speed change rate exceeds the first preset threshold, or the vehicle speed confidence flag is 0, then the current operating condition is a deviation condition. The initial value for the current vehicle speed calculation is the vehicle speed output by the dynamic slope method at the previous moment. The dynamic slope is the vehicle's longitudinal acceleration. If the current operating condition is not a deviation condition, and the speed curve shows a convex point, then the current operating condition is a dynamic slope condition. Based on the two recorded convex points and the vehicle's first estimated speed, the corresponding two slope segments are calculated and entered into the convex point slope threshold judgment. If both slope segments are less than the second preset threshold... If a threshold is set, the average of the two slopes is taken as the dynamic slope; otherwise, the vehicle's longitudinal acceleration is used as the dynamic slope. The difference between the vehicle speed estimated by the dynamic slope method at the previous moment and the vehicle's first estimated speed is evaluated. If the difference is less than the lower threshold, the convex point and time sequence are updated, and the initial value for speed calculation is convex point 2. Otherwise, it is checked whether the difference is greater than the upper threshold. If it is, the average of the vehicle speed estimated by the dynamic slope method at the previous moment and the vehicle's first estimated speed is taken as the new convex point 2, and convex point 2 is used as the initial value for speed calculation. Otherwise, the convex point is not updated, and the vehicle speed estimated by the dynamic slope method at the previous moment is used as the initial value for speed calculation. Calculate the initial value; if the current operating condition is not a trigger condition, deviation condition, or dynamic slope condition, then the current operating condition is the default condition, and no updates to the convex point and dynamic slope are performed; judge the difference between the vehicle speed estimated by the dynamic slope method at the previous moment and the vehicle's first estimated speed. If it is less than the third threshold, set the vehicle speed confidence flag to 1; otherwise, set the vehicle speed confidence flag to 0; calculate the vehicle speed based on the vehicle speed stability flag. When the vehicle speed stability flag is 0, calculate the vehicle's second estimated speed based on the initial value of the vehicle speed calculation and the dynamic slope. When the vehicle speed stability flag is 1, use the vehicle's first estimated speed as the vehicle's second estimated speed.

[0088] In the dynamic slope method, an additional convex point is added, and a three-convex-point, dual-slope calculation mechanism is adopted. An input vehicle speed verification mechanism is proposed, which does not use the input vehicle speed when it is severely distorted. A convex point verification and dynamic slope verification mechanism are proposed to solve a series of error problems caused by the uncertainty of the convex point (such as the convex point in the acceleration condition may appear at the peak, and the convex point in the deceleration condition may appear at the trough). This ensures that the convex point and dynamic slope are updated reasonably, effectively solving the pain points of the traditional dynamic slope method.

[0089] The estimated vehicle speed V output by the maximum and minimum wheel speed method mm It is a scalar signal that will enter the third layer for processing using the dynamic slope method. The overall framework of its acceleration part is as follows: Figure 4 As shown. The traditional dynamic slope method estimates vehicle speed by calculating the convex point (generally a "concave point" (i.e., "lower convex point") during acceleration and a "convex point" (i.e., "upper convex point") during deceleration; for convenience, this application will uniformly refer to them as "convex points") and the dynamic slope. Figure 5 As shown in the diagram. Taking acceleration as an example, it can be seen that the dynamic slope method detects a bump at each trough, updates the bump and dynamic slope, and calculates the vehicle speed using an integral method until the next bump is detected. However, this method has significant drawbacks. It requires the curve to have relatively perfect peaks and troughs, but in reality, due to factors such as brake pressure fluctuations and signal noise, bumps can appear at any position, such as... Figure 6 As shown. From Figure 6 As can be seen from the circle, the acceleration condition bump may even appear at the peak or in the middle. Obviously, such a bump is invalid, and the dynamic slope calculated from it is also wrong, which will lead to an incorrect estimate of the vehicle speed.

[0090] Therefore, the embodiments of this application have made the following improvements to the traditional dual-convex point dynamic slope method: adding a convex point and adopting a three-convex point, dual-slope calculation mechanism; proposing an input vehicle speed verification mechanism, which does not use the input vehicle speed when the input vehicle speed is severely distorted; and proposing a convex point verification and dynamic slope verification mechanism to ensure that the convex point and dynamic slope are updated reasonably, thereby making the estimated vehicle speed more accurate.

[0091] For this algorithm, the main difference between acceleration and deceleration conditions lies in the selection of the dynamic slope: acceleration uses a value greater than 0, while deceleration uses a value less than 0. The algorithm is mainly divided into four parts: initialization and condition classification, curve processing, vehicle speed calculation, and TCS / ABS exit judgment.

[0092] 1. Input and Operating Condition Classification

[0093] In this section, the input is the speed V output by the maximum and minimum wheel speed method at times k, k-1, and k-2. mm Then, through the acceleration of the whole vehicle a xThe process involves determining whether the vehicle is entering a deceleration or acceleration phase; the subsequent discussion will focus on the acceleration phase.

[0094] 2. Curve processing

[0095] Curve processing is the core of the dynamic slope method. In this part, the algorithm processes the vehicle speed V of the maximum and minimum wheel speed method. mm The curve is processed by classifying operating conditions using different thresholds, resulting in two core quantities of the dynamic slope: convex point and dynamic slope. Curve processing is divided into four categories, in the order of condition judgment: trigger condition, deviation condition, dynamic slope condition, and default condition.

[0096] Before introducing the operating conditions, let's first explain the meaning of several terms used in the following text:

[0097] Stable speed indicator: TCS system not triggered, wheel acceleration If V is always less than the threshold, then V is determined. mm If accurate, set the flag to 1. Otherwise, set it to 0.

[0098] Vehicle speed confidence flag: V mm If the absolute value of the rate of change exceeds a certain large threshold, then V is determined to be... mm Unreliable, significantly deviates from the actual vehicle speed; mark position 0. Otherwise, set to 1.

[0099] Initial value for vehicle speed calculation: Vehicle speed calculation starts from an initial value and is integrated based on the slope. This initial value is referred to as the initial value for vehicle speed calculation in this patent.

[0100] Dynamic slope: This refers to the slope term in the initial value and slope integral. The embodiments of this application use a three-convex point, thus having two slope segments, but only one slope segment is used in the calculation. The slope actually used in the calculation is called the dynamic slope.

[0101] 1) Triggering conditions

[0102] The trigger condition is the entry point for the dynamic slope method.

[0103] Entry conditions: The vehicle speed stability indicator is set to 1, and the wheel acceleration at this moment is... The threshold has been exceeded. This condition indicates that V... mm The vehicle begins to deviate from its actual speed, thus entering the dynamic slope method and updating the two sets of convex points and their corresponding times.

[0104] Bump Update: The value of bump 1 is updated to the original value of bump 2, and the value of bump 2 is updated to the new bump V. mm (-1), that is, V at time k-1 mm If this is the first time entering this operating condition, and there were no previous bumps, then both will be V. mm (-1).

[0105] Initial value for vehicle speed calculation: Take convex point 2.

[0106] Dynamic slope: vehicle acceleration a x .

[0107] 2) Deviation Conditions

[0108] Deviation conditions are primarily used for input speed verification. Values ​​that deviate significantly from the actual vehicle speed cannot be accurately estimated regardless of processing methods and must be excluded. For example, in acceleration conditions, the dynamic slope method takes the convex point of the input speed trough. However, in severely distorted input speeds, the input speed at the trough may also be much greater than the actual vehicle speed, which is unreasonable. Therefore, input speed verification is necessary.

[0109] Entry conditions: Triggering conditions are not met, vehicle speed stability flag is 0 and V mm The rate of change exceeds the threshold, or the vehicle speed confidence flag is 0. The vehicle speed confidence flag is 0 when entering this condition.

[0110] Bump Update: Do not update.

[0111] Initial value for vehicle speed calculation: Vehicle speed V output by the dynamic slope method at the previous moment. dyn (-1).

[0112] Dynamic slope: vehicle acceleration a x .

[0113] 3) Dynamic slope condition

[0114] In this operating condition, the embodiments of this application propose a three-convex-point, dual-slope calculation and verification mechanism. The addition of more convex points and slopes brings more information, and the robustness of the algorithm can be improved through reasonable threshold judgment. In the traditional dual-convex-point, single-slope method, if a convex point appears in an incorrect position, such as the peak of the acceleration condition, using that convex point will cause a large error. Moreover, this situation is very common due to factors such as wheel cylinder pressure fluctuations and signal noise. Such local convex points appearing at the peak of the acceleration condition are called "pseudo-convex points". The three-convex-point, dual-slope calculation and verification mechanism proposed in the embodiments of this application is precisely to eliminate the influence of "pseudo-convex points" on vehicle speed estimation, and reduces the occurrence of this problem through slope threshold judgment, vehicle speed difference judgment and other means.

[0115] Entry conditions: The deviation condition is not met and the curve shows a convex point. If the vehicle speed before and after the acceleration condition is greater than the vehicle speed at the current moment, then the vehicle speed at the current moment is a convex point; the opposite is true for the deceleration condition.

[0116] Dynamic slope: Based on the two recorded convex points (convex point 1 and convex point 2) and the current input vehicle speed (also a convex point), calculate the corresponding two slope segments and judge them against the convex point slope threshold. If both slope segments are less than the threshold, take the average of the two as the dynamic slope; otherwise, use the vehicle acceleration 'a'. x As a dynamic slope.

[0117] Initial value update for convex point and vehicle speed calculation: First, update the vehicle speed V estimated by the dynamic slope method at the previous time step. dyn (-1) and the vehicle speed V input at this moment mm The algorithm judges the difference between the values ​​of () and (). When the difference is less than the lower threshold, the algorithm considers the convex point to have high credibility and updates the convex point and time sequence number normally. That is, the value of convex point 1 is updated to convex point 2, and the value of convex point 2 is updated to V. mm (), the initial value for vehicle speed calculation is taken as convex point 2. Otherwise, it is checked whether it is greater than the upper threshold. If it is, the current convex point is considered to be a pseudo-convex point, and its value may deviate significantly from the actual vehicle speed, but it may also be a normal convex point. In this case, a compromise method is adopted, taking V. dyn (-1) and V mm The average value of () is used as the new convex point (convex point 2), and convex point 2 is taken as the initial value for vehicle speed calculation. If neither of the above two conditions is met, the convex point is not updated, and V is taken. dyn (-1) is used as the initial value for calculating vehicle speed.

[0118] 4) Default operating conditions

[0119] When the aforementioned three working conditions are not met, the algorithm considers the vehicle speed V calculated by the maximum and minimum wheel speed method to be... mm It is stable and highly reliable, without updating the bumps or slopes, and directly jumps to the vehicle speed calculation section.

[0120] 3. Vehicle speed calculation

[0121] In this section, the vehicle speed is calculated based on the initial speed value and dynamic slope calculated in the preceding process. First, the input vehicle speed V is... mm With V dyn The difference between (-1) is used to determine the vehicle speed. If it is less than the threshold, the input vehicle speed is considered to meet the usage conditions, and the vehicle speed confidence flag is set to 1. Then, the vehicle speed is calculated based on the vehicle speed stability flag. If it is 1, then V is considered to be... mm The data already demonstrates high reliability, with very stable wheel speeds, eliminating the need for dynamic slope calculations. Therefore, proceed directly to the next section. Otherwise, calculate the vehicle speed based on the initial speed value and the dynamic slope.

[0122] 4. TCS Exit Detection

[0123] If TCS is not triggered and the unupdated convex point time exceeds the duration threshold, the counter is incremented by 1. When the count reaches the threshold, the vehicle speed stability flag is set to 1, and the algorithm considers the input vehicle speed V to be stable.mm It is stable enough that no dynamic slope calculation is required.

[0124] In step S103, coordinate transformation and noise modeling are performed on the vehicle's GPS signal, and the Kalman filter algorithm is used to filter it to obtain the GPS output vehicle speed value. A Kalman filter is constructed based on the kinematic equations, and the second estimated vehicle speed is used as the measurement value of the Kalman filter for filtering. The measurement noise is modeled to obtain the IMU output vehicle speed value. The GPS output vehicle speed value and the IMU output vehicle speed value are optimally fused to obtain the final estimated vehicle speed.

[0125] A GPS Kalman filter module is constructed. Based on the different GPS calculation methods and the comparison between the rate of change of GPS vehicle speed and the vehicle acceleration, noise is reasonably modeled and calculated to achieve an adaptive filter estimation, thereby mitigating the problem of reduced output vehicle speed quality caused by GPS signal interference and other reasons.

[0126] GPS Kalman filtering first performs coordinate transformation and noise modeling on the GPS signal, and then uses the Kalman filtering algorithm to perform optimal filtering.

[0127] 1. Coordinate transformation

[0128] GPS signals include attitude angles, velocity in the navigation coordinate system, latitude and longitude, etc. Generally, GPS signals are in the NE-G coordinate system. To obtain the vehicle speed in the vehicle's coordinate system, a coordinate system transformation is required. First, the GPS vehicle speed signal in the NE-G navigation coordinate system is converted to the front right-downward vehicle coordinate system. This transformation can be obtained by rotating the vehicle in sequence using Euler angles: first, rotate the heading angle ψ around the Z-axis; second, rotate the pitch angle θ around the Y-axis; and finally, rotate the roll angle φ around the X-axis. The specific coordinate transformation matrix is ​​as follows:

[0129]

[0130] In the formula, This is the transformation matrix from the navigation coordinate system to the vehicle coordinate system.

[0131] Therefore, the vehicle speed in the front right lower body coordinate system is:

[0132]

[0133] In the formula, V b V represents the vehicle speed in the front right-downward coordinate system. n The vehicle speed is in the navigation coordinate system.

[0134] Considering that the vehicle coordinate system specified by SAE, ISO, etc. is front-left-top, a further transformation is required, from front-right-bottom to front-left-top. The transformation matrix is ​​as follows:

[0135]

[0136] Therefore, the vehicle speed in the front left upper vehicle coordinate system is:

[0137]

[0138] 2. Noise Modeling

[0139] GPS signals have very little noise, but accuracy can decrease when the signal is blocked. By properly modeling the noise, adaptive filtering can be implemented to reduce its impact.

[0140] Noise modeling first sets different thresholds based on the different GPS vehicle speed calculation methods, which are obtained by looking up a table, i.e.:

[0141] Cov1 GPS =(method)

[0142] In the formula, Cov1 GPS Here, denoted as noise covariance, 'method' represents the GPS vehicle speed calculation method, and 'f' is a lookup table function.

[0143] Furthermore, based on the relationship between GPS vehicle speed change rate and vehicle acceleration, other types of noise covariance calculation rules can be designed:

[0144]

[0145] In the formula, C1 and C2 are calibration constants, C3 is set to ensure calculation stability, and tanh is the hyperbolic tangent function.

[0146] The final covariance is the maximum of the two mentioned above:

[0147] Cov GPS =max(Cov1) GPS ,Cov2 GPS )

[0148] 3. Kalman Filter

[0149] Kalman filtering of GPS signals is relatively simple, and its system equations are set as follows:

[0150] x(k) = x(k-1) + w(k)

[0151] y(k)=x(k)+ε(k)

[0152] The optimal filtering can then be achieved by iterating through the Kalman filter equations described above.

[0153] The processed vehicle speed is used as the measurement value of the Kalman filter to construct an IMU Kalman filter. Noise is then reasonably modeled and calculated to achieve an adaptive filtering estimation. In extreme conditions such as large slippage, the noise calculation module outputs a larger measurement noise covariance, thus reducing the algorithm's dependence on the measured value and increasing its dependence on the predicted value (i.e., acceleration integral). Furthermore, because the acceleration is compensated for by slope and optimally filtered, it can be used for short periods, thereby promptly reducing the estimation error of the dynamic slope method. In addition, the introduction of Kalman filtering also makes the estimated vehicle speed curve smoother.

[0154] IMU Kalman filtering includes system filtering and noise modeling. The basic basis for vehicle speed estimation by the IMU Kalman filter module is the kinematic equation based on acceleration.

[0155]

[0156] In the formula, V x V represents the longitudinal vehicle speed. y Let y be the lateral vehicle speed, ψ be the yaw angle, ∈ be the system noise, and a be the lateral vehicle speed. bias Assuming zero bias for the sensor. Consider zero bias a. bias Relatively fixed, including:

[0157]

[0158] Therefore, the state transition equation under discrete conditions is:

[0159]

[0160] The system measurement equation is:

[0161] y(k)=V dyn +ε(k)

[0162] In the formula, V dyn The vehicle speed output by the wheel speed processing module is used here as the measurement value for the IMU Kalman filter. Optimal filtering can then be achieved through iterative Kalman filtering.

[0163] IMU Kalman filter noise modeling needs to consider two aspects: the original wheel speed W and the processed wheel speed, i.e., the speed V output by the dynamic slope method. Dyn The basic idea is that the wheel speed and the difference between the processed wheel speed and the vehicle speed of the previous cycle determine the noise covariance. If the difference is large, the noise is large. The calculation principle is as follows:

[0164] Cov IMU =C2*tanh(C1*(η*min(|WV last |)+|V Dyn -V last |))+C3

[0165] In the formula, η is the weighting coefficient, used to allocate the ratio of wheel speed to the processed wheel speed, C1 and C2 are calibration coefficients, and C3 is set to ensure calculation stability.

[0166] One of the innovations of this application is the integration of reasonable noise modeling with the processed wheel speed. Simply processing wheel speed results in an inconsistent and uneven output speed curve. Furthermore, under conditions of high slippage, such as strong acceleration, braking, or low-friction surfaces, even the dynamic slope method still introduces errors. Therefore, this application uses a Kalman filter algorithm, employing the processed wheel speed as the filter's measurement value. In extreme conditions such as high slippage, the noise calculation module outputs a larger measured noise covariance, thereby reducing the algorithm's dependence on the measured value and increasing its reliance on the predicted value (i.e., the acceleration integral), thus promptly reducing the estimation error of the dynamic slope method. In addition, the introduction of Kalman filtering also makes the estimated speed curve smoother.

[0167] After obtaining the vehicle speed values ​​output by GPS and IMU, a weighted fusion is performed. The fusion algorithm is a multi-sensor Kalman filter fusion algorithm in the sense of linear minimum variance. Consider a discrete linear time-varying system Σ(A,B,H) with l sensors:

[0168] x(k+1)=A(k)x(k)+B(k)u(k)+Γ(k)w(k)

[0169] y i (k)=H i (k)x(k)+v i (k), i = 1, 2, ..., l

[0170] In the formula, A, B, Γ, H i These are the system matrix, input matrix, process noise matrix, and measurement matrix, respectively, w, v i These are process noise and measurement noise, respectively.

[0171] Before constructing the fusion algorithm, several assumptions need to be made:

[0172] a) w(k) and v i (k) is correlated white noise, and:

[0173]

[0174]

[0175] In the formula, E is the expected value, δ is the Kroos function, Q and R are the process and measurement noise variances, and S... i For covariance.

[0176] b) The initial state x(0) is independent of w(k) and v i (k), and:

[0177] [x(0)]=μ0

[0178] E[(x(0)-μ0)(x(0)-μ0) T ] = P0

[0179] The goal of the algorithm is to base it on the measured value y i For i = 1, 2, ..., l, find the optimal Kalman filter in the sense of linear minimum variance. It meets the following performance requirements:

[0180] 1. Unbiasedness, that is...

[0181] 2. Optimality, i.e., finding a set of weight matrices α i =1,2,…,l, minimize the error variance of the fusion filter: tr[P o (k|k)=min{tr[P(k|k)}.

[0182] Let x i It is the optimal Kalman filter for the i-th sensor, that is:

[0183] x i (k|k)= i (k|k-1)+ i () i (k)- i (k)x(k|k-1))

[0184] In the formula, K i The Kalman gain of the i-th sensor can be obtained by the following formula:

[0185] P i (k|k-1)=P i (k-1|k-1)A T +

[0186]

[0187] P i (k|k)=[IK i (k)H i ]P i (k|k-1)

[0188] Among them, P i (k|k) and P i (k|k-1) are the variance matrices of the filtering and the first-step prediction error, respectively.

[0189] For ease of calculation, assume Si (k)=0,S ij If (k) = 0, meaning the sensor noise is uncorrelated, then the covariance of the i-th and j-th sensors can be defined as:

[0190] P ij (k|k)=[IK i (t)H i [AP] ij (k-1|k-1)A+ΓQ(k-1)Γ T ][IK j (k)H j ] T

[0191] The optimal multi-sensor fusion filter can then be given by the following formula:

[0192]

[0193] In the formula, α i This is the weight matrix of the i-th sensor, and it is calculated as follows:

[0194]

[0195] Among them, e=[I n ,…, n ] T It is an nl×n matrix.

[0196] The variance of the optimal multi-sensor fusion filter is:

[0197]

[0198] Algorithm fusion such as Figure 7 As shown, in the embodiments of this application, the IMU Kalman filter module and the GPS Kalman filter module are regarded as two sensors. The corresponding Kalman filters output the corresponding state estimates, covariance, and Kalman gain, which are then input into the fusion algorithm for weighted fusion to obtain the vehicle speed fusion estimate.

[0199] Optionally, in one embodiment of this application, before fusing the GPS output vehicle speed value and the IMU output vehicle speed value, the method further includes: determining whether there is an anomaly in the vehicle's GPS signal; if there is an anomaly, directly using the IMU output vehicle speed value as the final estimated vehicle speed.

[0200] In addition, considering that GPS signals are susceptible to interference and may be interrupted under certain operating conditions, the algorithm determines whether to use GPS signals based on the value of the GPS signal flag. If not, the output of the IMU Kalman filter module is used directly as the final output vehicle speed.

[0201] A multi-sensor Kalman filter algorithm based on linear minimum variance is employed to achieve optimal fusion of vehicle speed outputs from the IMU Kalman filter module and the GPS Kalman filter module. A switching criterion is designed based on GPS signal availability: when GPS is available, the fused vehicle speed is used to improve estimation accuracy; otherwise, the vehicle speed output from the IMU Kalman filter module is used, ensuring stable vehicle speed estimation under various operating conditions. By fusing multiple algorithms, the disadvantages of single traditional algorithms are overcome, achieving high-precision vehicle speed estimation under various operating conditions and exhibiting strong robustness. This provides effective and reliable vehicle speed signals for other advanced vehicle safety systems.

[0202] The vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals proposed in this application estimates longitudinal vehicle speed using four-wheel wheel speed signals, IMU signals, and GPS signals as a basis. First, the longitudinal road slope value estimated by other systems is introduced to compensate and filter the vehicle's longitudinal acceleration. A three-layer wheel speed processing algorithm is constructed using steering compensation, the maximum and minimum wheel speed method, and an improved dynamic slope method to achieve robust wheel speed processing. Based on kinematic principles, the vehicle speed output by the wheel speed processing algorithm is used as the measurement value of the IMU Kalman filter, and the measurement noise is reasonably modeled to achieve adaptive estimation. For the GPS signal, the signal in the navigation coordinate system is transformed to the vehicle coordinate system through coordinate transformation, and the noise is modeled before being processed by the Kalman filter algorithm. Finally, based on the optimal fusion theory of multi-sensor Kalman filtering, the vehicle speed outputs from the GPS Kalman filter and the IMU Kalman filter are optimally fused to achieve higher accuracy and stronger robustness in vehicle speed estimation.

[0203] Next, referring to the accompanying drawings, a vehicle speed fusion estimation device based on GPS, IMU, and wheel speed sensor signals is described according to an embodiment of this application.

[0204] Figure 8 This is an example diagram of a vehicle speed fusion estimation device based on GPS, IMU and wheel speed sensor signals according to an embodiment of this application.

[0205] like Figure 8 As shown, the vehicle speed fusion estimation device 10 based on GPS, IMU and wheel speed sensor signals includes: an acceleration compensation module 10, a wheel speed processing module 200, a GPS Kalman filter module 300, an IMU Kalman filter module 400 and a hierarchical fusion algorithm module 500.

[0206] The acceleration compensation module 100 compensates and filters the longitudinal acceleration output by the IMU using the road longitudinal slope value to obtain the actual longitudinal acceleration of the vehicle. The wheel speed processing module 200 compensates for the wheel speed under steering conditions, converting the wheel speed to the vehicle's center of gravity. It uses the maximum and minimum wheel speed method to compare the estimated vehicle speed derivative with the overall vehicle acceleration, determines the first estimated vehicle speed based on the current conditions, calculates the first estimated vehicle speed using the dynamic slope method with three convex points and two slopes, and verifies the first estimated vehicle speed using a vehicle speed verification mechanism. It uses the convex point and slope verification mechanism to obtain the initial value of the calculated vehicle speed and the dynamic slope, and integrates to obtain the second estimated vehicle speed. The GPS Kalman filter module 300 performs coordinate transformation and noise modeling on the vehicle's GPS signal, and uses the Kalman filter algorithm to filter it to obtain the GPS output vehicle speed value. The IMU Kalman filter module 400 is used to construct a Kalman filter based on kinematic equations. It uses the vehicle's second estimated speed as the measurement value for filtering and models the measurement noise to obtain the IMU output speed value. The hierarchical fusion algorithm module 500 is used to optimally fuse the GPS output speed value and the IMU output speed value to obtain the final estimated speed of the vehicle.

[0207] Optionally, in one embodiment of this application, it further includes: an anomaly module, used to determine whether there is an anomaly in the vehicle's GPS signal, and when there is an anomaly in the GPS signal, directly using the IMU output vehicle speed value as the final estimated vehicle speed.

[0208] It should be noted that the foregoing explanation of the vehicle speed fusion estimation method based on GPS, IMU and wheel speed sensor signals also applies to the vehicle speed fusion estimation device based on GPS, IMU and wheel speed sensor signals in this embodiment, and will not be repeated here.

[0209] The vehicle speed fusion estimation device based on GPS, IMU, and wheel speed sensor signals proposed in this application estimates longitudinal vehicle speed using four-wheel wheel speed signals, IMU signals, and GPS signals as a basis. First, the longitudinal road slope value estimated by other systems is introduced to compensate and filter the vehicle's longitudinal acceleration. A three-layer wheel speed processing algorithm is constructed using steering compensation, the maximum and minimum wheel speed method, and an improved dynamic slope method to achieve robust wheel speed processing. Based on kinematic principles, the vehicle speed output by the wheel speed processing algorithm is used as the measurement value of the IMU Kalman filter, and the measurement noise is reasonably modeled to achieve adaptive estimation. For the GPS signal, the signal in the navigation coordinate system is transformed to the vehicle coordinate system through coordinate transformation, and the noise is modeled before being processed by the Kalman filter algorithm. Finally, based on the optimal fusion theory of multi-sensor Kalman filtering, the vehicle speed outputs from the GPS Kalman filter and the IMU Kalman filter are optimally fused to achieve higher accuracy and stronger robustness in vehicle speed estimation.

[0210] Figure 9 A schematic diagram of the structure of an electronic device provided in an embodiment of this application. The electronic device may include:

[0211] The memory 901, the processor 902, and the computer program stored on the memory 901 and capable of running on the processor 902.

[0212] When the processor 902 executes the program, it implements the vehicle speed fusion estimation method based on GPS, IMU and wheel speed sensor signals provided in the above embodiments.

[0213] Furthermore, electronic devices also include:

[0214] Communication interface 903 is used for communication between memory 901 and processor 902.

[0215] The memory 901 is used to store computer programs that can run on the processor 902.

[0216] The memory 901 may include high-speed RAM memory, and may also include non-volatile memory, such as at least one disk storage device.

[0217] If the memory 901, processor 902, and communication interface 903 are implemented independently, then the communication interface 903, memory 901, and processor 902 can be interconnected via a bus to complete communication between them. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 9 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.

[0218] Optionally, in a specific implementation, if the memory 901, processor 902, and communication interface 903 are integrated on a single chip, then the memory 901, processor 902, and communication interface 903 can communicate with each other through an internal interface.

[0219] The processor 902 may be a central processing unit (CPU), an application specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of this application.

[0220] This embodiment also provides a computer-readable storage medium storing a computer program, characterized in that the program, when executed by a processor, implements the above-described vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals.

[0221] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0222] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "N" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0223] Any process or method described in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more N executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0224] It should be understood that the various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0225] Those skilled in the art will understand that all or part of the steps of the methods described in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it includes one or a combination of the steps of the method embodiments.

Claims

1. A vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals, characterized in that, Includes the following steps: The longitudinal acceleration output by the IMU is compensated and filtered using the longitudinal slope value of the road to obtain the actual longitudinal acceleration of the vehicle. Steering compensation is performed on the wheel speed of the vehicle under steering conditions. The wheel speed is converted to the vehicle's center of gravity. The maximum and minimum wheel speed method is used to perform a comparison mechanism between the estimated vehicle speed derivative and the vehicle acceleration. The first estimated vehicle speed is determined based on the current working conditions. The first estimated vehicle speed is calculated using the dynamic slope method with three convex points and two slopes. The first estimated vehicle speed is verified using a vehicle speed verification mechanism. The initial value of the vehicle speed calculation and the dynamic slope are obtained using the convex point and slope verification mechanism. After integration, the second estimated vehicle speed is obtained. The GPS signal of the vehicle is subjected to coordinate transformation and noise modeling, and filtered using the Kalman filter algorithm to obtain the GPS output vehicle speed value. A Kalman filter is constructed based on the kinematic equations, and the second estimated vehicle speed of the vehicle is used as the measurement value of the Kalman filter for filtering. The measurement noise is modeled to obtain the IMU output vehicle speed value. The GPS output vehicle speed value and the IMU output vehicle speed value are optimally fused to obtain the final estimated vehicle speed.

2. The method according to claim 1, characterized in that, The actual longitudinal acceleration of the vehicle is: in, This represents the vehicle's actual longitudinal acceleration. The raw acceleration output by the IMU. This refers to the longitudinal slope of the road.

3. The method according to claim 1, characterized in that, Steering compensation is applied to the wheel speeds of the vehicle under steering conditions, converting the wheel speeds into speeds at the vehicle's center of gravity. The four-wheel wheel speeds after steering compensation are: in, For the compensated wheel speed, The original wheel speed, For the front wheel steering angle, For the horizontal swing angle, This is the distance between the left and right wheels of the front axle. This is the distance between the left and right wheels of the rear axle.

4. The method according to claim 1, characterized in that, A comparison mechanism is used to estimate the derivative of the vehicle speed and the vehicle acceleration using the maximum and minimum wheel speed method, and the first estimated vehicle speed is determined based on the current operating conditions, including: The current operating condition of the vehicle is determined based on the vehicle's actual longitudinal acceleration; When the current operating condition is an acceleration condition, if the derivative of the estimated vehicle speed is greater than the actual longitudinal acceleration of the vehicle, then the first estimated vehicle speed is the minimum value among the integral value of the longitudinal acceleration and the wheel speeds of the four wheels; otherwise, the first estimated vehicle speed is the minimum value among the wheel speeds of the four wheels. When the current operating condition is a deceleration condition, if the derivative of the estimated vehicle speed is less than the actual longitudinal acceleration of the vehicle, then the first estimated vehicle speed is the maximum value of the longitudinal acceleration integral value and the four wheel speeds; otherwise, the first estimated vehicle speed is the maximum value of the four wheel speeds. When the current operating condition is a steady-state condition, the first estimated vehicle speed is the average of the four wheel speeds.

5. The method according to claim 1, characterized in that, Under acceleration conditions, the first estimated vehicle speed is calculated using the dynamic slope method with three convex points and two slopes, and the first estimated vehicle speed is verified using a vehicle speed verification mechanism to obtain the second estimated vehicle speed, including: When the vehicle speed stability flag is 1 and the wheel acceleration is greater than the acceleration threshold at the current moment, the current working condition is the trigger working condition, which means that the first estimated vehicle speed of the vehicle deviates from the actual vehicle speed. The dynamic slope method is then entered, and the two sets of convex points and the corresponding time are updated. The value of convex point 1 is updated to the original value of convex point 2, and the value of convex point 2 is updated to the new convex point. The initial value of the current vehicle speed calculation is taken as convex point 2, and the dynamic slope is the longitudinal acceleration of the vehicle. If the current working condition is not the triggering working condition, and the vehicle speed stability flag is 0 and the first estimated vehicle speed change rate of the vehicle exceeds the first preset threshold, or if the current working condition is not the triggering working condition and the vehicle speed confidence flag is 0, the current working condition is a deviation working condition. The initial value for calculating the current vehicle speed is the vehicle speed output by the dynamic slope method at the previous moment, and the dynamic slope is the longitudinal acceleration of the vehicle. If the current operating condition is not the aforementioned deviation condition and a convex point appears on the speed curve, then the current operating condition is a dynamic slope condition. Based on the two recorded convex points and the first estimated vehicle speed, the corresponding two slope segments are calculated and entered into the convex point slope threshold judgment. If both slope segments are less than the second preset threshold, the average of the two slope segments is taken as the dynamic slope; otherwise, the longitudinal acceleration of the vehicle is used as the dynamic slope. The difference between the vehicle speed estimated by the dynamic slope method at the previous moment and the first estimated vehicle speed is judged. When the difference is less than the lower threshold, the convex point and time sequence number are updated, and the initial value for vehicle speed calculation is taken as convex point 2; otherwise, it is judged whether it is greater than the upper threshold. If it is, the average of the vehicle speed estimated by the dynamic slope method at the previous moment and the first estimated vehicle speed is taken as the new convex point as convex point 2, and convex point 2 is taken as the initial value for vehicle speed calculation; otherwise, the convex point is not updated, and the vehicle speed estimated by the dynamic slope method at the previous moment is taken as the initial value for vehicle speed calculation. If the current operating condition is not the triggering operating condition, the deviation operating condition, or the dynamic slope operating condition, then the current operating condition is the default operating condition, and no updates to the convex point and dynamic slope are performed. The difference between the vehicle speed estimated by the dynamic slope method at the previous moment and the first estimated vehicle speed of the vehicle is judged. If it is less than the third threshold, the vehicle speed confidence flag is set to 1; otherwise, the vehicle speed confidence flag is set to 0. The vehicle speed is calculated based on the vehicle speed stability flag. When the vehicle speed stability flag is 0, the second estimated vehicle speed of the vehicle is calculated based on the initial value of the vehicle speed calculation and the dynamic slope. When the vehicle speed stability flag is 1, the first estimated vehicle speed of the vehicle is used as the second estimated vehicle speed of the vehicle.

6. The method according to claim 1, characterized in that, Before fusing the GPS output vehicle speed value and the IMU output vehicle speed value, the method further includes: Determine if there is any anomaly in the vehicle's GPS signal. If there is an anomaly, directly use the vehicle speed value output by the IMU as the final estimated speed of the vehicle.

7. A vehicle speed fusion estimation device based on GPS, IMU, and wheel speed sensor signals, characterized in that, include: The acceleration compensation module is used to compensate and filter the longitudinal acceleration output by the IMU using the longitudinal slope value of the road to obtain the actual longitudinal acceleration of the vehicle. The wheel speed processing module is used to perform steering compensation on the wheel speed of the vehicle under steering conditions, convert the wheel speed to the vehicle's center of gravity, perform a comparison mechanism between the estimated vehicle speed derivative and the overall vehicle acceleration using the maximum and minimum wheel speed method, determine the first estimated vehicle speed based on the current working conditions, calculate the first estimated vehicle speed using the dynamic slope method with three convex points and two slopes, verify the first estimated vehicle speed using a vehicle speed verification mechanism, obtain the initial value of the vehicle speed calculation and the dynamic slope using the convex point and slope verification mechanism, and obtain the second estimated vehicle speed after integration. The GPS Kalman filter module is used to perform coordinate transformation and noise modeling on the GPS signal of the vehicle, and to filter it using the Kalman filter algorithm to obtain the GPS output vehicle speed value. The IMU Kalman filter module is used to construct a Kalman filter based on kinematic equations, filter the second estimated vehicle speed as the measurement value of the Kalman filter, and model the measurement noise to obtain the IMU output vehicle speed value. The hierarchical fusion algorithm module is used to optimally fuse the GPS output vehicle speed value and the IMU output vehicle speed value to obtain the final estimated vehicle speed.

8. The apparatus according to claim 7, characterized in that, Also includes: An anomaly module is used to determine whether there is an anomaly in the vehicle's GPS signal, and when there is an anomaly in the GPS signal, directly use the vehicle speed value output by the IMU as the final estimated vehicle speed.

9. An electronic device, characterized in that, include: The memory, the processor, and the computer program stored in the memory and executable on the processor, the processor executing the program to implement the vehicle speed fusion estimation method based on GPS, IMU, and wheel speed sensor signals as described in any one of claims 1-6.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to implement the vehicle speed fusion estimation method based on GPS, IMU and wheel speed sensor signals as described in any one of claims 1-6.

Images