A slope estimation method based on fuzzy control fusion sensor and vehicle speed signal

Abstract

The present invention discloses a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal, aiming to solve the problems of many input parameters and calibration amount, high cost and the like of the existing road slope estimation method. The method comprises the following steps: (1) signal preprocessing, including low-pass filtering of sensor signal and differential of vehicle speed signal; (2) signal initialization, including calculation of zero drift value of gravity acceleration and pitch angular velocity; (3) calculation of two slope measurement values; (4) output fusion weight coefficient based on the designed fuzzy controller; (5) selecting different methods to calculate the optimal road slope estimation value according to the motion state of the vehicle turning or going straight. The present invention obtains the acceleration and angular velocity signals in the three directions of XYZ and the vehicle speed signal obtained by the six-axis sensor, and then realizes the calculation of the optimal slope estimation value based on fuzzy control, thereby reducing the cost and calibration amount of slope sensor development and improving the robustness of the slope estimation algorithm.

Description

Technical Field

[0001] The invention relates to a road slope recognition method for automobiles, in particular to a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal. Background Art

[0002] High-precision environmental perception technology is an important part of building a modern automobile intelligent transportation system. Under complex and changeable road conditions, it can quickly and accurately estimate road slope information, which has important practical significance for vehicle stability control, improving fuel economy, and smooth shifting control of automatic transmissions.

[0003] The current slope estimation algorithms can be divided into four categories in principle: vehicle kinematics-based methods, GPS and GIS-based geographic information query methods, vehicle dynamics-based methods, and sensor information fusion-based methods. Among them, the acceleration sensor signal based on the vehicle kinematics method has serious high-frequency noise, and the acceleration estimation is inaccurate, resulting in low slope estimation accuracy; the GPS information relied on by the GPS and GIS-based geographic information query method is easily lost in special environments such as tunnels, and the slope information cannot be obtained; the vehicle dynamics-based method relies on the accuracy of the driving force signal. Without the use of sensors, the vehicle driving force is difficult to observe, and this method requires more input parameters. The method based on sensor information fusion can use weighting coefficients to assign weights to the slope values ​​calculated by two different methods, which has become a current research hotspot.

[0004] Some existing patents, such as the invention patent with patent number CN 114633755 A, propose a method for estimating the slope of a car road based on information fusion. The invention fuses the slope values ​​obtained based on the vehicle dynamics model and the kinematics model through two weighting coefficients to obtain the final slope estimation value. Although the invention performs a fusion weighted calculation on the slope values ​​obtained by dynamics and kinematics, it requires more input parameters and both weighting coefficients need to be calibrated. Summary of the invention

[0005] The present invention aims to solve the shortcomings of the prior art, such as many input parameters and calibration quantities, many sensors, high cost and poor robustness, and proposes a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal. The method obtains fusion weighting coefficients based on fuzzy control, and weights the slope value calculated based on kinematics and the slope value obtained by integrating the pitch angular velocity sensor signal. The slope estimation method is adjusted according to the turning or straight driving state of the vehicle, thereby improving the accuracy and robustness of the slope estimation algorithm.

[0006] In order to solve the above technical problems, the present invention is implemented by adopting the following technical solutions:

[0007] A slope estimation method based on fuzzy control fusion sensor and vehicle speed signal includes the following steps:

[0008] S1: Sensor signal and vehicle speed signal preprocessing:

[0009] The six original signals of longitudinal acceleration ax, lateral acceleration ay, vertical acceleration az, roll angular velocity Gx, pitch angular velocity Gy, and yaw angular velocity Gz obtained by the six-axis sensor are low-pass filtered using formula (1);

[0010] X out (k)=(1-f)·U(k)+f·X out (k-1) (1)

[0011] Where, X out (k)——The output of the low-pass filter at the kth step

[0012] f——low-pass filter coefficient, ∈[0,1]

[0013] U(k)——The original signal obtained by the sensor at the kth step

[0014] The vehicle speed signal V obtained on the CAN line x Perform differential to obtain the vehicle acceleration signal for:

[0015]

[0016] Where, m is the number of groups participating in the mean of vehicle speed difference

[0017] T v ——Interval step length of vehicle speed difference

[0018] dt——control step length

[0019] S2: Calculate the gravity acceleration g and the pitch angular velocity zero drift value Gy_bias within the initialization time T0 using formula (3);

[0020]

[0021] Where, T0——initialization time

[0022] S3: Based on vehicle acceleration and the longitudinal acceleration signal ax, based on the vehicle kinematic equation shown in equation (4), the slope measurement value i1 is obtained as:

[0023]

[0024] Considering the dynamic slope limit when the road slope changes, the slope measurement value i2 is obtained;

[0025] S4: According to the acceleration signals in the three directions of XYZ, the gravity acceleration g calculated in step S2, and the pitch angular velocity signal, a fuzzy controller is established to output the fusion weighting coefficient K.

[0026] S5: Calculate the optimal estimated slope value α according to the vehicle's turning or straight-moving motion state.

[0027] In the preferred technical solution, the dynamic slope limitation when the road slope changes in step S3 refers to:

[0028] When the slope of the road on which the vehicle is traveling changes, the change in the gravity acceleration along the slope component Δa xv satisfy:

[0029]

[0030] In the formula, α and Δα represent the true road slope, the estimated road slope, and the slope estimation error, respectively. β and Δβ represent the actual change of road slope, the integral value of pitch angular velocity and the integral error every T1 time, respectively;

[0031] Considering that the error Δα and the error Δβ are very small, the upper and lower limit thresholds e of the sensor angular velocity integral error Δβ are calibrated β,L 、e β,H , after expanding formula (5), we can get Δa xv The upper and lower limits of are:

[0032]

[0033] Next, Δa xv Substituting the upper and lower limits of into formula (4), the slope measurement value i2 considering the slope limit can be obtained as:

[0034]

[0035] In the formula,

[0036] In the preferred technical solution, the step S4 includes the following steps:

[0037] S41: every T2 time, count the count value Count of all ax, ay and az signals acquired by the sensor within T2 time that satisfy formula (8), and the pitch angle θ obtained by integrating the pitch angular velocity Gy within time T2;

[0038]

[0039] Where ΔG is the threshold for judging that the acceleration signal is unreliable.

[0040] S42: The count value Count and the pitch angle θ are used as input variables, and the fusion weight coefficient K is used as the output variable to establish a fuzzy controller. The domain of the count value Count is defined as [0, T2 / dt], and the language variables are set to small S, medium M, and large B; the domain of the pitch angle is defined as [-Gy limit *T2,Gy limit *T2], where Gy limit In order to obtain the maximum limit of the pitch angular velocity of the sensor, the language is set to small S, medium M, and large B; the domain of the fusion weight coefficient K is defined as [0,1], and the language is set to minimum ZS, small S, medium M, large B, and maximum ZB; the fuzzy control rule of the fusion weight coefficient K shown in Table 1 is established, and the fusion weight coefficient K is obtained by looking up Table 1 based on the count value Count and the pitch angle θ calculated in step S41.

[0041] Table 1 Fuzzy control rules table integrating weight coefficient K

[0042]

[0043] In the preferred technical solution, the specific steps of calculating the optimal estimated value α of the slope in step S5 are:

[0044] S51: Perform short-time discrete integration on the yaw rate signal Gz processed in step S1 to obtain a yaw angle signal. When the relationship shown in formula (9) is satisfied, the vehicle is determined to be in a turning state. At this time, α1 output by formula (10) is used as the optimal estimated value of the slope; otherwise, the vehicle is determined to be in a straight-ahead state, and the process proceeds to step S72;

[0045]

[0046] Where, T z ——Total step length of short-time discrete integration of yaw angular velocity

[0047] P straight ——Steering state judgment threshold

[0048]

[0049] S52: At this time, the vehicle is in a straight-moving state, and α2 output by formula (11) is used as the optimal estimated value of the slope;

[0050]

[0051] Compared with the prior art, the advantages of the present invention are:

[0052] 1. The slope estimation method based on fuzzy control fusion sensor and vehicle speed signal of the present invention has only six-axis sensor signals and vehicle speed signals obtained on the CAN line as input parameters, with few input parameters and low algorithm complexity;

[0053] 2. The slope estimation method based on fuzzy control fusion sensor and vehicle speed signal of the present invention obtains the fusion weight coefficient based on fuzzy control, avoids the complexity of engineering calibration of key parameters, and improves the robustness of the algorithm;

[0054] 3. The slope estimation method based on fuzzy control fusion sensor and vehicle speed signal described in the present invention performs weight allocation on the slope value based on kinematic calculation and the slope value obtained by integrating the pitch angular velocity sensor signal, and adjusts the slope estimation method according to the turning or straight driving state of the vehicle, thereby improving the accuracy of the slope estimation algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The present invention will be further described below in conjunction with the accompanying drawings and embodiments:

[0056] Figure 1 It is a flow chart of a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal according to the present invention;

[0057] Figure 2 It is a flow chart of dynamic slope limitation when the road slope changes in a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal according to the present invention;

[0058] Figure 3 The present invention provides a design flow chart of a fuzzy controller in a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of well-known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.

[0060] The present invention will be further described below in conjunction with the accompanying drawings.

[0061] See also Figure 1 The present invention provides a slope estimation method based on fuzzy control fusion sensor and vehicle speed signal, which specifically includes the following steps:

[0062] S1: Sensor signal and vehicle speed signal preprocessing:

[0063] Formula (1) is used to perform low-pass filtering on the six original signals of longitudinal acceleration ax, lateral acceleration ay, vertical acceleration az, roll angular velocity Gx, pitch angular velocity Gy, and yaw angular velocity Gz obtained by the MPU6050 six-axis sensor;

[0064] X out (k)=(1-f)·U(k)+f·X out (k-1) (1)

[0065] In the formula, X out (k)——The output of the low-pass filter at the kth step

[0066] f——low-pass filter coefficient, ∈[0,1]

[0067] U(k)——The original signal obtained by the sensor at the kth step

[0068] The vehicle speed signal V obtained on the CAN line x Perform differential to obtain the vehicle acceleration signal for:

[0069]

[0070] Where, m is the number of groups participating in the mean of vehicle speed difference

[0071] T v ——Interval step length of vehicle speed difference

[0072] dt——control step length

[0073] S2: Calculate the gravity acceleration g and the pitch angular velocity zero drift value Gy_bias within the initialization time T0 using formula (3);

[0074]

[0075] Where, T0——initialization time

[0076] S3: Based on vehicle acceleration and the longitudinal acceleration signal ax, based on the vehicle kinematic equation shown in equation (4), the slope measurement value i1 is obtained as:

[0077]

[0078] Considering the dynamic slope limit when the road slope changes, the slope measurement value i2 is obtained;

[0079] S4: According to the acceleration signals in the three directions of XYZ, the gravity acceleration g calculated in step S2, and the pitch angular velocity signal, a fuzzy controller is established to output the fusion weighting coefficient K.

[0080] S5: Calculate the optimal estimated value α of the slope according to different motion states of the vehicle.

[0081] See also Figure 2 The dynamic slope limitation when the road slope changes in step S3 refers to:

[0082] When the slope of the road on which the vehicle is traveling changes, the change in the gravity acceleration along the slope component Δa xv satisfy:

[0083]

[0084] In the formula, α and Δα represent the actual road slope, the optimal slope estimate, and the slope estimation error, respectively. β and Δβ represent the actual change of road slope, the integral value of pitch angular velocity and the integral error every T1 time period respectively;

[0085] Considering that the error Δα and the error Δβ are very small, the upper and lower limit thresholds e of the sensor angular velocity integral error Δβ are calibrated β,L 、e β,H , after expanding formula (5), we can get Δa xv The upper and lower limits of are:

[0086]

[0087] Next, Δa xv Substituting the upper and lower limits of into formula (4), the slope measurement value i2 considering the slope limit can be obtained as:

[0088]

[0089] In the formula,

[0090] See also Figure 3 The method of establishing the fuzzy controller output fusion weighting coefficient K comprises the following steps:

[0091] S41: every T2 time, count the count value Count of all ax, ay and az signals acquired by the sensor within T2 time that satisfy formula (8), and the pitch angle θ obtained by integrating the pitch angular velocity Gy within time T2;

[0092]

[0093] Where ΔG is the threshold for judging that the acceleration signal is unreliable.

[0094] S42: The count value Count and the pitch angle θ are used as input variables, and the fusion weight coefficient K is used as the output variable to establish a fuzzy controller. The domain of the count value Count is defined as [0, T2 / dt], and the language variables are set to small S, medium M, and large B; the domain of the pitch angle is defined as [-Gy limit *T2,Gy limit *T2], where Gy limit In order to obtain the maximum limit of the pitch angular velocity of the sensor, the language is set to small S, medium M, and large B; the domain of the fusion weight coefficient K is defined as [0,1], and the language is set to minimum ZS, small S, medium M, large B, and maximum ZB; the fuzzy control rule of the fusion weight coefficient K shown in Table 1 is established, and the fusion weight coefficient K is obtained by looking up Table 1 based on the count value Count and the pitch angle θ calculated in step S41.

[0095] Table 1 Fuzzy control rules table integrating weight coefficient K

[0096]

[0097] The specific steps of calculating the optimal estimated value α of the slope are:

[0098] S51: Perform short-time discrete integration on the yaw angular velocity signal Gz processed in step S1 to obtain a yaw angle signal. When the relationship shown in formula (9) is satisfied, the vehicle is determined to be in a turning state. Considering that the pitch angular velocity obtained by the MPU6050 six-axis sensor in the turning state has an unstable zero drift value, the pitch angle obtained by the pitch angular velocity integration is unreliable at this time. Therefore, the slope limit of the gravity acceleration along the slope component in formula (6) is unreliable. At this time, α1 output by formula (10) is used as the optimal estimated value of the slope; otherwise, the vehicle is determined to be in a straight state, and the process goes to step S72;

[0099]

[0100] Where, T z ——Total step length of short-time discrete integration of yaw angular velocity

[0101] P straight ——Steering state judgment threshold

[0102]

[0103] S52: At this time, the vehicle is in a straight-moving state, and α2 output by formula (11) is used as the optimal estimated value of the slope;

[0104]

[0105] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A slope estimation method based on fuzzy control fusion sensor and vehicle speed signal, It is characterized in that The following steps are involved: S1: Sensor signal and vehicle speed signal preprocessing: The six original signals of longitudinal acceleration ax, lateral acceleration ay, vertical acceleration az, roll angular velocity Gx, pitch angular velocity Gy, and yaw angular velocity Gz obtained by the six-axis sensor are low-pass filtered using formula (1); X out (k)=(1-f)·U(k)+f·X out (k-1) (1) Where, X out (k)——The output of the low-pass filter at the kth step f——low-pass filter coefficient, ∈[0,1] U(k)——The original signal obtained by the sensor at the kth step The vehicle speed signal V obtained on the CAN line x Perform differential to obtain the vehicle acceleration signal for: Where, m is the number of groups participating in the mean of vehicle speed difference T v ——Interval step length of vehicle speed difference dt——control step length S2: Calculate the initialization time T using formula (3) 0 The gravity acceleration g and pitch angular velocity zero drift value Gy_bias; Where, T 0 ——Initialization time S3: Based on vehicle acceleration and the longitudinal acceleration signal ax, based on the vehicle kinematic equation shown in equation (4), the slope measurement value i is obtained 1 for: Considering the dynamic slope limit when the road slope changes, the slope measurement value i is obtained 2 ; S4: According to the acceleration signals in the three directions of XYZ, the gravity acceleration g calculated in step S2 and the pitch angular velocity signal, a fuzzy controller is established to output a fusion weighting coefficient K; S5: Calculate the optimal estimated slope value α according to the vehicle's turning or straight-moving motion state.

2. A slope estimation method based on fuzzy control fusion sensor and vehicle speed signal according to claim 1, It is characterized in that The dynamic slope limitation when the road slope changes in step S3 refers to: When the slope of the road on which the vehicle is traveling changes, the change in the gravity acceleration along the slope component Δa xv satisfy: In the formula, α and Δα represent the true road slope, the estimated road slope, and the slope estimation error, respectively. β and Δβ represent the actual change of road slope, the integral value of pitch angular velocity and the integral error every T1 time, respectively; Considering that the error Δα and the error Δβ are very small, the upper and lower limit thresholds e of the sensor angular velocity integral error Δβ are calibrated β,L 、e β,H , after expanding formula (5), we can get Δa xv The upper and lower limits of are: Next, Δa xv Substituting the upper and lower limits of into formula (4), we can obtain the slope measurement value i considering the slope limit 2 for: In the formula, 3. A slope estimation method based on fuzzy control fusion sensor and vehicle speed signal according to claim 1, It is characterized in that The step S4 includes the following steps: S41: every T2 time, count the count value Count of all ax, ay and az signals acquired by the sensor within T2 time that satisfy formula (8), and the pitch angle θ obtained by integrating the pitch angular velocity Gy within time T2; Where, ΔG is the threshold for judging that the acceleration signal is unreliable; S42: The count value Count and the pitch angle θ are used as input variables, and the fusion weight coefficient K is used as the output variable to establish a fuzzy controller. The domain of the count value Count is defined as [0, T2 / dt], and the language variables are set to small S, medium M, and large B; the domain of the pitch angle is defined as [-Gy limit *T2,Gy limit *T2], where Gy limit To obtain the maximum limit of the pitch angular velocity for the sensor, the language is set to small S, medium M, and large B; the domain of the fusion weight coefficient K is defined as [0,1], and the language is set to minimum ZS, small S, medium M, large B, and maximum ZB; the fuzzy control rules of the fusion weight coefficient K are established as shown in Table 1 below: Table 1 Fuzzy control rules table integrating weight coefficient K The fusion weighting coefficient K is obtained by looking up Table 1 based on the count value Count and the pitch angle θ calculated in step S41.

4. A slope estimation method based on fuzzy control fusion sensor and vehicle speed signal according to claim 1, It is characterized in that The specific steps of calculating the optimal estimated value α of the slope in step S5 are: S51: Perform short-time discrete integration on the yaw rate signal Gz processed in step S1 to obtain a yaw angle signal. When the relationship shown in formula (9) is satisfied, the vehicle is determined to be in a turning state. At this time, the α output by formula (10) 1 as the optimal estimated value of the slope; otherwise, the vehicle is determined to be in a straight-moving state, and the process proceeds to step S72; Where, T z ——Total step length of short-time discrete integration of yaw angular velocity P straight ——Steering state judgment threshold S52: At this time, the vehicle is in a straight-moving state, and the α output by formula (11) 2 As the optimal estimate of slope;

Images