All-condition road slope estimation method and device and storage medium

Abstract

The invention discloses an all-working-condition road surface gradient estimation method and device and a storage medium. The method comprises the steps that vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model are obtained; constructing a kinematic system state equation and a kinematic system observation equation; constructing a dynamic system state equation and a dynamic system observation equation; performing abnormal point detection processing on the vehicle measurement data by using a box graph method to obtain target vehicle data; calculating a first road slope value under the vehicle longitudinal kinematics model by using a Kalman filtering method; calculating a second road surface slope value under the vehicle longitudinal dynamic model by using a recursive least square method; and carrying out data fusion by utilizing an interactive multi-model method to obtain a full-working-condition road surface slope value which is used for adjusting vehicle driving parameters. According to the invention, all-condition road slope estimation is realized, and the accuracy is improved. The method can be widely applied to the technical field of vehicle engineering.

Description

Technical Field

[0001] The present invention relates to the field of vehicle engineering technology, and in particular to a method, device and storage medium for estimating the slope of a road surface under all working conditions. Background Art

[0002] With the advancement of vehicle intelligence, road slope information can provide reference data for vehicle control, optimize engine fuel consumption and vehicle driving comfort, and prevent vehicle rollover during driving. Traditional road slope detection methods use a single pendulum angular displacement sensor or a single vehicle dynamics model to measure slope data. However, the measurement process of the single pendulum angular displacement sensor is affected by the high-frequency transient motion characteristics and driving conditions of the vehicle, resulting in a low measurement signal-to-noise ratio and inaccurate slope values. The vehicle dynamics model relies on accurate model parameters, and high-frequency noise disturbances and vehicle acceleration resistance lead to low model calculation accuracy.

[0003] In summary, the technical problems existing in the relevant technologies need to be improved. Summary of the invention

[0004] The embodiments of the present invention provide a method, device and storage medium for estimating the slope of a road surface under all working conditions, which effectively improve the accuracy.

[0005] On the one hand, an embodiment of the present invention provides a method for estimating the slope of a road surface under all working conditions, comprising the following steps:

[0006] Obtain vehicle measurement data, vehicle longitudinal kinematics model and vehicle longitudinal dynamics model;

[0007] According to the longitudinal kinematic model of the vehicle, constructing a kinematic system state equation and a kinematic system observation equation;

[0008] According to the longitudinal dynamics model of the vehicle, constructing a state equation of the dynamics system and an observation equation of the dynamics system;

[0009] Using a box plot method to perform outlier detection processing on the vehicle measurement data to obtain target vehicle data;

[0010] Calculating a first road surface slope value under the longitudinal kinematic model of the vehicle by using a Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation;

[0011] Calculating a second road surface slope value under the vehicle longitudinal dynamics model by using a recursive least squares method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation;

[0012] According to the first road surface slope value and the second road surface slope value, data fusion is performed using an interactive multi-model method to obtain a full-condition road surface slope value, and the full-condition road surface slope value is used to adjust vehicle driving parameters.

[0013] In some embodiments, constructing a kinematic system state equation and a kinematic system observation equation according to the vehicle longitudinal kinematic model includes:

[0014] Constructing a kinematic system state vector according to the longitudinal kinematic model of the vehicle;

[0015] According to the acceleration sensor measurement equation, the slope value equation is constructed;

[0016] Constructing the kinematic system state equation according to the kinematic system state vector and the slope value equation;

[0017] Performing matrix transformation on the kinematic system state equation to obtain a kinematic matrix state equation;

[0018] The kinematic system observation equation is constructed according to the kinematic matrix state equation.

[0019] In some embodiments, constructing a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamics model includes:

[0020] Calculate the vehicle driving force based on the current output torque of the engine, the final reduction ratio of the transmission, the transmission ratio of the transmission, the transmission efficiency from the turbine to the driving wheel, and the tire radius;

[0021] Calculate air resistance based on air density, air resistance coefficient, car frontal area and relative air flow speed;

[0022] Calculate rolling resistance based on vehicle mass, rolling resistance coefficient, gravitational acceleration and slope angle;

[0023] Calculating slope resistance according to the vehicle mass, the gravitational acceleration and the slope angle;

[0024] Calculating the acceleration resistance according to the vehicle mass, vehicle acceleration and the constant coefficient of the rotational mass inertia moment converted into the translational mass inertia force;

[0025] Constructing a vehicle longitudinal dynamics equation according to the vehicle longitudinal dynamics model, the vehicle driving force, the air resistance, the rolling resistance, the slope resistance and the acceleration resistance;

[0026] Performing Taylor expansion on the vehicle longitudinal dynamics equation to obtain a vehicle longitudinal dynamics discrete model;

[0027] Constructing a dynamic system state vector according to the vehicle longitudinal dynamics discrete model, the vehicle mass and the slope angle;

[0028] constructing a state space equation of the dynamic system according to the equivalent slope and the state vector of the dynamic system;

[0029] Simplifying the state vector of the dynamic system to obtain a dynamic simplified state vector;

[0030] Constructing a system state differential equation according to the state space equation of the dynamic system and the dynamic simplified state vector;

[0031] The system state differential equation is discretized by using the forward Euler method to obtain a discretized difference equation;

[0032] According to the discretized difference equation, a dynamic discrete Kalman filter equation is constructed;

[0033] Constructing a discrete state equation according to the discretized difference equation and the dynamic discrete Kalman filter equation;

[0034] According to the discrete state equation, construct the state equation of the dynamic system;

[0035] The observation equation of the dynamic system is constructed according to the discrete state equation and the observation matrix.

[0036] In some embodiments, the process of performing outlier detection processing on the vehicle measurement data using a box plot method to obtain target vehicle data includes:

[0037] Construct a box plot, wherein the box plot includes a minimum value, a lower quartile, a median, an upper quartile, and a maximum value;

[0038] constructing an outlier detection equation based on the vehicle measurement data and the box plot;

[0039] Performing differentiation processing on the outlier detection equation to obtain an outlier detection differential equation;

[0040] Simplifying the outlier detection differential equation to obtain a simplified outlier detection equation;

[0041] The simplified outlier detection equation is transformed to obtain the target vehicle data.

[0042] In some embodiments, the calculating the first road slope value under the longitudinal kinematic model of the vehicle using a Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation includes:

[0043] Constructing a kinematic Kalman filter equation according to the kinematic system state equation and the kinematic system observation equation;

[0044] Calculating a predicted state quantity and a predicted error covariance matrix according to the target vehicle data;

[0045] Calculating the Kalman gain according to the prediction error covariance matrix;

[0046] Calculating a target estimated state quantity according to the predicted state quantity and the Kalman gain;

[0047] Calculating a target estimation error covariance according to the prediction error covariance matrix and the Kalman gain;

[0048] The first road surface slope value is calculated according to the kinematic Kalman filter equation, the target estimated state quantity and the target estimated error covariance.

[0049] In some embodiments, the calculating the second road slope value under the vehicle longitudinal dynamics model by using a recursive least squares method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation includes:

[0050] Performing least squares transformation on the longitudinal dynamics equation of the vehicle to obtain a least squares formal model;

[0051] constructing a cost function according to the least squares formal model;

[0052] Solving the cost function to obtain a cost function value;

[0053] constructing an initial recursive least squares equation system according to the cost function value;

[0054] Constructing a target recursive least squares equation system according to the forgetting factor and the initial recursive least squares equation system;

[0055] Constructing a recursive least squares mass estimation model according to the vehicle longitudinal dynamics equation;

[0056] Constructing a recursive least squares vehicle mass estimation model equation group according to the target recursive least squares equation group and the recursive least squares mass estimation model;

[0057] Calculating a vehicle mass estimation value according to the target vehicle data and the recursive least squares vehicle mass estimation model equation group;

[0058] The second road surface slope value is calculated according to the estimated value of the vehicle mass, the state equation of the dynamic system and the observation equation of the dynamic system.

[0059] In some embodiments, the method of performing data fusion using an interactive multi-model method according to the first road surface slope value and the second road surface slope value to obtain the road surface slope value for all working conditions includes:

[0060] Construct system model state equations and system model measurement equations;

[0061] Set up the transition probability matrix;

[0062] Calculating the model prediction probability according to the transition probability matrix;

[0063] Calculating the mixing probability between models based on the model prediction probabilities;

[0064] Calculating a model mixing state estimate based on the inter-model mixing probability;

[0065] Calculating a model mixture covariance estimate based on the model mixture state estimate;

[0066] Calculating a state prediction value according to the system model state equation and the system model measurement equation;

[0067] Calculating a prediction error covariance based on the state prediction value and the model mixture covariance estimate;

[0068] Calculating residuals according to the system model measurement equation;

[0069] Calculating a residual covariance matrix based on the residuals;

[0070] Constructing a likelihood function based on the prediction error covariance and Gaussian distribution;

[0071] Calculating a Kalman filter gain matrix according to the prediction error covariance and the residual covariance matrix;

[0072] Calculating a state estimate according to the Kalman filter gain matrix;

[0073] Calculating an interaction error covariance matrix based on the state estimate;

[0074] Calculating motion state probability according to the interaction error covariance matrix;

[0075] Calculate the total probability according to the motion state probability;

[0076] Calculating the model probability according to the likelihood function, the total probability and the model probability normalization factor;

[0077] According to the model probability, constructing an interactive estimated state value equation and an interactive covariance equation;

[0078] The full-condition road surface slope value is calculated according to the interactive estimated state value equation, the interactive covariance equation, the first road surface slope value and the second road surface slope value.

[0079] On the other hand, an embodiment of the present invention provides a device for estimating the slope of a road surface under all working conditions, comprising:

[0080] The first module is used to obtain vehicle measurement data, vehicle longitudinal kinematics model and vehicle longitudinal dynamics model;

[0081] The second module is used to construct a kinematic system state equation and a kinematic system observation equation according to the longitudinal kinematic model of the vehicle;

[0082] The third module is used to construct a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamics model;

[0083] The fourth module is used to perform abnormal point detection processing on the vehicle measurement data using a box plot method to obtain target vehicle data;

[0084] A fifth module is used to calculate a first road surface slope value under the longitudinal kinematic model of the vehicle by using a Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation;

[0085] A sixth module, for calculating a second road surface slope value under the vehicle longitudinal dynamics model by using a recursive least squares method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation;

[0086] The seventh module is used to perform data fusion using an interactive multi-model method according to the first road surface slope value and the second road surface slope value to obtain a road surface slope value for all working conditions, and the road surface slope value for all working conditions is used to adjust vehicle driving parameters.

[0087] In another aspect, an embodiment of the present invention provides a computer device, comprising:

[0088] at least one processor;

[0089] at least one memory for storing at least one program;

[0090] When the at least one program is executed by the at least one processor, the at least one processor implements the described method.

[0091] On the other hand, an embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method described above is implemented.

[0092] The beneficial effects of the present invention are as follows:

[0093] The embodiment of the present invention first obtains vehicle measurement data, a vehicle longitudinal kinematic model and a vehicle longitudinal dynamic model, then constructs a kinematic system state equation and a kinematic system observation equation according to the vehicle longitudinal kinematic model, constructs a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamic model, and uses a box plot method to perform abnormal point detection processing on the vehicle measurement data to obtain target vehicle data, and then uses a Kalman filter method to calculate a first road surface slope value under the vehicle longitudinal kinematic model according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation, and uses a recursive least squares method to calculate a second road surface slope value under the vehicle longitudinal dynamic model according to the target vehicle data, the dynamic system state equation and the dynamic system observation equation, and finally uses an interactive multi-model method to perform data fusion according to the first road surface slope value and the second road surface slope value to obtain a full-condition road surface slope value, so that the full-condition road surface slope estimation can be achieved by combining the Kalman filter method and the recursive least squares method, thereby improving the accuracy.

[0094] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0096] Figure 1 This is a flow chart of a method for estimating the slope of a road surface under all working conditions according to an embodiment of the present invention;

[0097] Figure 2 A schematic diagram of a vehicle longitudinal kinematics model according to an embodiment of the present invention;

[0098] Figure 3 A schematic diagram of a vehicle longitudinal dynamics model according to an embodiment of the present invention;

[0099] Figure 4 A schematic diagram of a box plot according to an embodiment of the present invention;

[0100] Figure 5 A schematic diagram of an iterative calculation process of a Kalman filter method according to an embodiment of the present invention;

[0101] Figure 6 A schematic diagram of an interactive multi-model method process according to an embodiment of the present invention;

[0102] Figure 7 This is an overall flow chart of slope estimation using a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model according to an embodiment of the present invention;

[0103] Figure 8 This is a schematic diagram of the structure of a device for estimating the slope of a road surface under all working conditions according to an embodiment of the present invention;

[0104] Fig. 9 The figure is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0105] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below in conjunction with the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the embodiments of the present application. They are only examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the attached claims.

[0106] It is understood that the terms "first", "second", etc. used in this application can be used to describe various concepts in this article, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another concept. For example, without departing from the scope of the embodiment of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the words "if" and "if" as used herein can be interpreted as "at the time of" or "when" or "in response to determination".

[0107] The terms "at least one", "multiple", "each", "any", etc. used in this application, at least one includes one, two or more, multiple includes two or more, each refers to each of the corresponding multiple, and any refers to any one of the multiple.

[0108] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of this application and are not intended to limit this application.

[0109] Before describing the embodiments of the present application in detail, some nouns and terms involved in the embodiments of the present application are first described. The nouns and terms involved in the embodiments of the present application are subject to the following explanations.

[0110] Kalman filtering: It is an algorithm that uses the linear system state equation to optimally estimate the system state through the system input and output observation data. Since the observation data includes the influence of noise and interference in the system, the optimal estimation can also be regarded as a filtering process.

[0111] Box-plot: also known as box plot, box-whisker plot, box-type plot or box-and-whisker plot, is a statistical chart used to show the dispersion of a set of data. It is named because of its box-like shape. It is also often used in various fields, and is common in quality management. It is mainly used to reflect the characteristics of the original data distribution, and can also compare the distribution characteristics of multiple groups of data. The method of drawing a box-and-whisker plot is: first find the upper edge, lower edge, median and two quartiles of a set of data; then, connect the two quartiles to draw a box; then connect the upper edge and lower edge to the box, and the median is in the middle of the box.

[0112] Interacting Multiple Model (IMM): is an algorithm with adaptive characteristics that can effectively adjust the probability of each model. It is especially suitable for positioning and tracking maneuvering targets.

[0113] In the related technology, with the in-depth advancement of vehicle intelligence and automation, and the increasing accuracy and real-time requirements of vehicle dynamics control, longitudinal slope information is an important input parameter affecting the electronic control of the vehicle chassis, which directly affects the engine fuel consumption and the comfort of vehicle driving. In the advanced driver assistance system, the system will monitor the vertical force of the vehicle tires and the slope direction in real time, and issue warnings in time to prevent the vehicle from rolling over. Therefore, it is particularly important to accurately and real-timely obtain the road slope value to improve the safety of vehicle driving and the fuel economy of vehicle driving. At present, most of the methods for road slope detection use GPS or single pendulum angular displacement sensors to measure and calculate slope data, or simply rely on vehicle dynamics models. The sensor-based measurement process is easily affected by the high-frequency transient motion characteristics and driving conditions of the vehicle, resulting in a low measurement signal-to-noise ratio, and the obtained slope value is not accurate enough; the identification method based on the vehicle dynamics model relies more on the accuracy of the model parameters, and its disturbance is mostly high-frequency noise. The system model does not consider the influence of wind speed resistance, and the initial abnormal data of the system is not eliminated during the calculation process, resulting in the final slope estimation value. The accuracy is not high. The traditional vehicle dynamics model does not take into account the situation when the car is accelerating, and does not consider the acceleration resistance that needs to be overcome when its mass accelerates. As a result, the established vehicle dynamics model has a large error in the estimated value of the slope, low accuracy, and low robustness.

[0114] In view of this, this embodiment determines the system state equation and system observation equation for estimating the road slope under all working conditions by constructing a vehicle longitudinal kinematic model and a longitudinal dynamic model; the state input variable of the state equation constructed based on the vehicle longitudinal kinematic model includes the vehicle longitudinal speed, and the state input variable of the state equation of the system model includes the vehicle longitudinal speed based on the vehicle longitudinal dynamic model considering the influence of the acceleration resistance. At the same time, data cleaning is first performed according to the initial value measured by the system observation equation, and the abnormal data is eliminated by using the box plot method; the road slope based on the vehicle longitudinal kinematic model is estimated according to the Kalman filter; the vehicle mass based on the vehicle dynamic model is identified and estimated by the recursive least squares method with a forgetting factor, and then the slope value under the model is estimated; the data of the two models are processed by an interactive multi-model algorithm, and the total estimated state value and covariance are obtained through input interaction, filter update, probability update, and weighted fusion output interaction in turn, so as to realize the estimation of the road slope value under all working conditions. The process has strong real-time performance, strong robustness, and high accuracy.

[0115] A full-condition road slope estimation method provided in an embodiment of the present application relates to the field of vehicle engineering technology. A full-condition road slope estimation method provided in an embodiment of the present application can be applied to a terminal, can also be applied to a server, and can also be software running in a terminal or a server. In some embodiments, the terminal can be a smart phone, a tablet computer, a laptop computer, a desktop computer, a smart speaker, a smart watch, and a vehicle-mounted terminal, etc., but is not limited to this; the server side can be configured as an independent physical server, or it can be configured as a server cluster or distributed system composed of multiple physical servers, and can also be configured as a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, CDN, and big data and artificial intelligence platforms. The server can also be a node server in a blockchain network; the software can be an application that implements a full-condition road slope estimation method, etc., but is not limited to the above forms.

[0116] The present application can be used in many general or special computer system environments or configurations. For example: personal computers, server computers, handheld or portable devices, tablet devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, distributed computing environments including any of the above systems or devices, etc. The present application can be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present application can also be practiced in distributed computing environments, in which tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules can be located in local and remote computer storage media including storage devices.

[0117] The following is a detailed explanation of the embodiments of the present application in conjunction with the accompanying drawings:

[0118] Figure 1 is an optional flow chart of a method for estimating the slope of a road surface under all working conditions provided in an embodiment of the present application. Figure 1 The method may include but is not limited to steps S101 to S107.

[0119] Step S101, obtaining vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model;

[0120] Step S102: constructing a kinematic system state equation and a kinematic system observation equation according to the longitudinal kinematic model of the vehicle;

[0121] Step S103, constructing a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamics model;

[0122] Step S104, using the box plot method to perform outlier detection processing on the vehicle measurement data to obtain target vehicle data;

[0123] Step S105, calculating the first road surface slope value under the vehicle longitudinal kinematics model by using the Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation;

[0124] Step S106, calculating the second road slope value under the vehicle longitudinal dynamics model by using a recursive least squares method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation;

[0125] Step S107: According to the first road surface slope value and the second road surface slope value, an interactive multi-model method is used to perform data fusion to obtain a road surface slope value for all working conditions, and the road surface slope value for all working conditions is used to adjust vehicle driving parameters.

[0126] Steps S101 to S107 shown in the embodiment of the present application realize the estimation of road slope under all working conditions and improve the accuracy.

[0127] In step S101 of some embodiments, vehicle measurement data may be acquired through sensors, and a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model may be acquired through a vehicle model library. Vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model may also be acquired through other methods, but are not limited thereto. For example, required related parameters such as vehicle engine torque may be acquired through a controller area network (CAN) bus.

[0128] In some embodiments, in step S102, constructing the kinematic system state equation and the kinematic system observation equation according to the longitudinal kinematic model of the vehicle may include but is not limited to the following steps:

[0129] According to the longitudinal kinematics model of the vehicle, the kinematics system state vector is constructed;

[0130] According to the acceleration sensor measurement equation, the slope value equation is constructed;

[0131] According to the state vector of the kinematic system and the slope value equation, the state equation of the kinematic system is constructed;

[0132] Perform matrix transformation on the state equation of the kinematic system to obtain the kinematic matrix state equation;

[0133] According to the kinematic matrix state equation, the observation equation of the kinematic system is constructed.

[0134] In some embodiments, discrete Kalman filtering can be used to fuse the acceleration sensor signal with the vehicle speed information through the vehicle kinematic model, and the vehicle speed information and sensor information are used as system observables. At the same time, the ramp value is calculated through the state transfer equation. The acceleration data read out from the sensor is affected by the acceleration of the car and is also related to the current road slope. In general, the longitudinal acceleration measurement value of a vehicle contains two aspects of information: the derivative of the longitudinal vehicle speed in the direction of travel and the influence of the gravity component; when a vehicle is driving on a sloped road, the acceleration sensor measures the vehicle's motion acceleration along the ramp component. The sum of the gravitational acceleration constant along the slope component gsinα. When a car is driving on a slope road, the longitudinal kinematic model of the vehicle is as follows: Figure 2 As shown, the kinematic system state vector can be constructed according to the longitudinal kinematic model of the vehicle, wherein the expression of the kinematic system state vector at time k is: x(k) = [v k a x (k)sinα(k)] T According to the measurement principle of the acceleration sensor, the expression of the kinematic system state vector can be converted into: Where x(k) is the state vector of the kinematic system at time k, v k is the vehicle speed at time k, a x is the value measured by the acceleration sensor, α is the slope angle, g is the gravitational acceleration, is the vehicle acceleration. Then, according to the acceleration sensor measurement equation, the slope value equation is constructed, where the slope value equation is expressed as: It is generally believed that vehicle speed, vehicle acceleration and slope angle are continuous measurements that cannot change suddenly. Within the Δt time, the kinematic system state equation can be constructed based on the kinematic system state vector and the slope value equation, where the expression of the kinematic system state equation is: In the formula, v k is the vehicle speed at time k, v k-1 is the vehicle speed at time k-1, is the acceleration sensor measurement value at time k, is the acceleration sensor measurement value at time k-1, sin α k is the slope angle at time k, sinα k-1 is the slope angle at time k-1, Δt is the time interval, is the velocity observation noise, is the acceleration observation noise, is the slope value observation noise. Then the kinematic system state equation is transformed into a matrix to obtain the kinematic matrix state equation, where the expression of the kinematic matrix state equation is: Where, X k is the kinematic matrix state value at time k, X k-1 is the kinematic matrix state value at time k-1, and w is the system observation noise. The vehicle speed of the system can be obtained by reading the data in the CAN signal. The kinematic system observation equation can be constructed based on the kinematic matrix state equation, where the expression of the kinematic system observation equation is: k =H×x k +ζ, where Z k is the observation value of the kinematic system, H is the observation matrix, and ζ is the observation noise.

[0135] In some embodiments, in step S103, constructing the dynamic system state equation and the dynamic system observation equation according to the vehicle longitudinal dynamics model may include but is not limited to the following steps:

[0136] Calculate the vehicle driving force based on the current output torque of the engine, the final reduction ratio of the transmission, the transmission ratio of the transmission, the transmission efficiency from the turbine to the driving wheel, and the tire radius;

[0137] Calculate air resistance based on air density, air resistance coefficient, car frontal area and relative air flow speed;

[0138] Calculate rolling resistance based on vehicle mass, rolling resistance coefficient, gravitational acceleration and slope angle;

[0139] Calculate the slope resistance based on the vehicle mass, gravity acceleration and slope angle;

[0140] The acceleration resistance is calculated based on the vehicle mass, vehicle acceleration and the constant coefficient of the rotational mass inertia moment converted into the translational mass inertia force;

[0141] Construct the vehicle longitudinal dynamics equation based on the vehicle longitudinal dynamics model, vehicle driving force, air resistance, rolling resistance, slope resistance and acceleration resistance;

[0142] Taylor expansion is performed on the vehicle longitudinal dynamics equation to obtain the vehicle longitudinal dynamics discrete model;

[0143] Construct the state vector of the dynamic system according to the vehicle longitudinal dynamics discrete model, vehicle mass and slope angle;

[0144] According to the equivalent slope and the state vector of the dynamic system, the state space equation of the dynamic system is constructed;

[0145] Simplify the state vector of the dynamic system to obtain a dynamic simplified state vector;

[0146] According to the state space equation of the dynamic system and the dynamic simplified state vector, the system state differential equation is constructed;

[0147] The forward Euler method is used to discretize the system state differential equation to obtain the discretized difference equation;

[0148] According to the discretized difference equation, the dynamic discrete Kalman filter equation is constructed;

[0149] Construct discrete state equations based on discretized difference equations and dynamic discrete Kalman filter equations;

[0150] According to the discrete state equation, construct the state equation of the dynamic system;

[0151] According to the discrete state equation and observation matrix, the observation equation of the dynamic system is constructed.

[0152] In some embodiments, the longitudinal dynamics model of the vehicle during driving is built based on the vehicle dynamics theory; when the lateral force is not considered, there are driving force and driving resistance in the direction of vehicle driving. The driving force is the active torque generated by the engine of the power unit and transmitted to both sides of the wheels through the transmission device, and the driving resistance is the sum of the driving resistances suffered by the vehicle, including: air resistance, rolling resistance, slope resistance and acceleration resistance. The vehicle driving force can be calculated based on the current output torque of the engine, the main reduction ratio of the transmission, the transmission ratio of the transmission, the transmission efficiency from the turbine to the driving wheel, and the tire radius. The calculation formula of the vehicle driving force is: In the formula, F qd is the vehicle driving force, T tq is the current output torque of the engine, which can be calculated by reading relevant data from the engine messages EEC1, EEC3, and EC1. g is the main reduction ratio of the transmission, i o is the transmission ratio of the transmission, η t is the transmission efficiency from the turbine to the driving wheel, and R is the tire radius. The air resistance is calculated based on the air density, air resistance coefficient, vehicle frontal area and relative air flow speed. The air resistance calculation formula is: In the formula, F air is air resistance, ρ is air density, C d is the air resistance coefficient, which is generally taken as a function of the Reynolds number Re. When the vehicle speed is high, the dynamic pressure is high and the viscous friction of the corresponding gas is small, C dDoes not change with Re, A is the frontal area of ​​the car, that is, the projection area of ​​the car in the driving direction, v is the relative speed of air flow, and is the driving speed of the car when there is no wind. The rolling resistance is calculated based on the vehicle mass, rolling resistance coefficient, gravity acceleration and slope angle. The calculation formula of rolling resistance is: F fb =Mgf 0 cosα, where F fb is the rolling resistance, M is the mass of the vehicle, g is the acceleration of gravity, f 0 is the rolling resistance coefficient, and α is the slope angle.

[0153] The slope resistance is calculated based on the vehicle mass, gravity acceleration and slope angle. The slope resistance calculation formula is: F g =Mg sinα, where F g is the slope resistance. The acceleration resistance is calculated based on the vehicle mass, vehicle acceleration and the constant coefficient of the rotational mass inertia moment converted into the translational mass inertia force. The calculation formula of the acceleration resistance is: In the formula, F j is the acceleration resistance, δ is the constant coefficient of the rotation mass inertia moment converted into the translation mass inertia force, is the time derivative of vehicle speed, is the vehicle acceleration. The vehicle longitudinal dynamics model is as follows Figure 3 As shown, the vehicle longitudinal dynamics equation can be constructed based on the vehicle longitudinal dynamics model, vehicle driving force, air resistance, rolling resistance, slope resistance and acceleration resistance, where the expression of the vehicle longitudinal dynamics equation is: In the formula, is the derivative of the vehicle position with respect to time at time k, v k is the vehicle speed. More specifically, the vehicle longitudinal dynamics equation can also be expressed as:

[0154] Then, the vehicle longitudinal dynamics equation is Taylor expanded to obtain the vehicle longitudinal dynamics discrete model, where the expression of the vehicle longitudinal dynamics discrete model is: k+1 =v k +G(p k ,F k ,v k ,k)T s +ε 1 (k), where v k+1 is the vehicle speed at time k+1, G(*) is a nonlinear unknown function consisting of vehicle dynamics uncertainty, completely unknown system parameters and system disturbances, and p k For kT s The vehicle position at the time, F kFor kT s The vehicle driving force at time T s is the sampling interval, ε 1 (k) is the discrete error. According to the vehicle longitudinal dynamics discrete model, vehicle mass and slope angle, the dynamic system state vector is constructed, where the expression of the dynamic system state vector is: k =(v k ,M k ,α k ), where y k is the state value of the dynamic system, M k is the mass of the vehicle at time k, α k is the slope angle at time k. According to the equivalent slope and the state vector of the dynamic system, the state space equation of the dynamic system is constructed, and f 0 =tanβ, the expression of the state space equation of the dynamic system is: In the formula, β is the equivalent slope, is the derivative of the vehicle mass, is the derivative of the slope angle. The dynamic system state vector is simplified to obtain the dynamic simplified state vector, where the expression of the dynamic simplified state vector is: k =(v k ,α k ). According to the state space equation of the dynamic system and the simplified dynamic state vector, the system state differential equation is constructed, where the expression of the system state differential equation is:

[0155] The forward Euler method is then used to discretize the system state differential equation to obtain a discretized difference equation, where the expression of the discretized difference equation is: According to the discretized difference equation, the dynamic discrete Kalman filter equation is constructed, where the expression of the dynamic discrete Kalman filter equation is: Where Δt is the time interval. According to the discretized difference equation and the dynamic discrete Kalman filter equation, the discrete state equation is constructed, where the expression of the discrete state equation is: In the formula, u k is the system input at time k. The road resistance of the vehicle can be regarded as a constant in the calculation process. According to the discrete state equation, the state equation of the dynamic system is constructed, where the expression of the state equation of the dynamic system is: In the formula, u k-1 is the system input at time k-1, w k is the system process noise. According to the discrete state equation and the observation matrix, the observation equation of the dynamic system is constructed, where the expression of the observation equation of the dynamic system is:k =H×y k +ξ k , where H = [1 0], J is the observation matrix, z k is the observed value of the dynamic system, ξ k is the system observation noise. It can be understood that the system process noise and the system observation noise are independent of each other and both obey Gaussian distribution. When the vehicle mass is known, the subsequent Kalman filter method will be continuously iterated and updated to finally realize the estimation of the real-time slope value.

[0156] In some embodiments, in step S104, the vehicle measurement data is processed for outlier detection using a box plot method to obtain target vehicle data, which may include but is not limited to the following steps:

[0157] Construct a box plot, which includes the minimum value, lower quartile, median, upper quartile, and maximum value;

[0158] Based on the vehicle measurement data and box plots, an outlier detection equation is constructed;

[0159] Performing differentiation processing on the outlier detection equation to obtain the outlier detection differential equation;

[0160] The outlier detection differential equation is simplified to obtain the simplified outlier detection equation;

[0161] The simplified equation for outlier detection is transformed to obtain the target vehicle data.

[0162] In some embodiments, in the process of data estimation of the vehicle longitudinal kinematics model and the vehicle longitudinal dynamics model, system errors, mechanical errors and cumulative errors may inevitably be introduced. Therefore, the collected data needs to be firstly eliminated through the box plot method to make the estimation process more robust. The box plot is a way to convert data into visual graphics to detect outliers. Figure 4As shown, it includes the minimum value, the lower quartile (Q1), the median (Q2), the upper quartile (Q3) and the maximum value, among which the lower quartile can be called the first quartile, the median is called the second quartile, and the upper quartile can be called the third quartile. If there are abnormal data in the box plot, all data larger than the maximum value or smaller than the minimum value are abnormal values. After sorting the data from small to large, the 25% of the sorted data is Q1, the 50% of the sorted data is Q2, and the 75% of the sorted data is Q3. The box plot method first needs to define the limits of the abnormal values. The upper limit is Q3+1.5×IQR and the lower limit is Q1-1.5×IQR, where IQR is the interquartile range, IQR=Q3-Q1. When the data is above the upper limit or below the lower limit, the data is judged as an abnormal value. The selection of data in this effective interval can eliminate the interference of abnormal values ​​and increase the adaptability of data processing to uncertain factors, that is, it has robustness.

[0163] A box plot can be constructed first, where the box plot includes the minimum value, lower quartile, median, upper quartile and maximum value. Then, an outlier detection equation is constructed based on the vehicle measurement data and the box plot, where the expression of the outlier detection equation is: In the formula, F yc is the vehicle measurement data, which may include the vehicle driving force, M is the vehicle mass, is the vehicle acceleration, ρ is the air density, C d is the air resistance coefficient, A is the frontal area of ​​the car, v is the relative speed of air flow, g is the acceleration of gravity, and f 0 is the rolling resistance coefficient, and α is the slope angle. The outlier detection equation is differentiated to obtain the outlier detection differential equation, where the expression of the outlier detection differential equation is: It is understandable that the slope of the road changes randomly, and the slope of the road has little effect on the acceleration during vehicle driving, so the result of the slope derivative of acceleration is approximately zero, that is, During the calculation cycle of the system model, the rate of change of vehicle speed is much smaller than the rate of change of acceleration. The outlier detection differential equation can be simplified to obtain a simplified outlier detection equation, where the expression of the simplified outlier detection equation is: The simplified equation for outlier detection is transformed to obtain the target vehicle data, where the calculation formula for the target vehicle data is: It can be understood that the target vehicle data may include the vehicle mass.

[0164] In some embodiments, in step S105, the first road surface slope value under the vehicle longitudinal kinematic model is calculated using the Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation, which may include but is not limited to the following steps:

[0165] According to the state equation of the kinematic system and the observation equation of the kinematic system, the kinematic Kalman filter equation is constructed;

[0166] According to the target vehicle data, the predicted state quantity and the predicted error covariance matrix are calculated;

[0167] Calculate the Kalman gain based on the prediction error covariance matrix;

[0168] Calculate the target estimated state quantity according to the predicted state quantity and Kalman gain;

[0169] Calculate the target estimation error covariance based on the prediction error covariance matrix and the Kalman gain;

[0170] A first road surface slope value is calculated according to a kinematic Kalman filter equation, a target estimated state quantity and a target estimated error covariance.

[0171] In some embodiments, the iterative calculation process of the Kalman filter method is as follows Figure 5 As shown, the kinematic Kalman filter equation can be constructed based on the kinematic system state equation and the kinematic system observation equation, where the expression of the kinematic Kalman filter equation is: In the formula, x k is the state quantity at time k, x k-1 is the state quantity at time k-1, A is the process matrix, B is the control matrix, u k-1 is the control variable at time k-1, w k-1 is the process noise, z k is the observed quantity, H is the observation matrix, v k-1 is the observation noise. It can be understood that the process noise and the observation noise are independent of each other and conform to the normal distribution, that is, p(w)~N(0,Q), p(v)~N(0,R), where Q is the process excitation noise covariance matrix and R is the observation noise covariance matrix. Then, according to the target vehicle data, the predicted state quantity and the prediction error covariance matrix are calculated, where the calculation formula of the predicted state quantity is: In the formula, is the predicted state quantity at time k, is the predicted state quantity at time k-1. The calculation formula of the prediction error covariance matrix is: In the formula, is the prediction error covariance matrix at time k, P k-1is the target estimation error covariance at time k-1. Then, the Kalman gain is calculated based on the prediction error covariance matrix, where the calculation formula of the Kalman gain is: According to the predicted state quantity and Kalman gain, the target estimated state quantity is calculated, where the calculation formula of the target estimated state quantity is: In the formula, is the target estimated state at time k. Then, according to the prediction error covariance matrix and Kalman gain, the target estimation error covariance is calculated, where the calculation formula of the target estimation error covariance is: Where P k is the target estimation error covariance at time k, and I is the unit matrix. Finally, the first road slope value is calculated based on the kinematic Kalman filter equation, the target estimation state quantity and the target estimation error covariance. It can be understood that the control matrix B = 0 is lacking in the system input variables based on the longitudinal kinematic model of the vehicle. The advantage of this is that the entire system model is less disturbed by the deviation of the external input quantity. The state matrix and observation matrix of the system model are known. After specifying the initial values ​​of the system state variables and the values ​​of the process excitation noise covariance matrix and the observation noise covariance matrix, the slope value under the longitudinal kinematic model of the vehicle is estimated through continuous iterative updates.

[0172] In some embodiments, in step S106, the second road slope value under the vehicle longitudinal dynamics model is calculated using a recursive least squares method according to the target vehicle data, the dynamics system state equation, and the dynamics system observation equation, which may include but is not limited to the following steps:

[0173] Perform the least squares transformation on the longitudinal dynamics equation of the vehicle to obtain the least squares formal model;

[0174] According to the least squares formal model, construct the cost function;

[0175] Solve the cost function to obtain the cost function value;

[0176] According to the cost function value, construct an initial recursive least squares equation system;

[0177] According to the forgetting factor and the initial recursive least squares equations, a target recursive least squares equations system is constructed;

[0178] According to the vehicle longitudinal dynamics equation, a recursive least squares mass estimation model is constructed;

[0179] According to the target recursive least squares equation group and the recursive least squares mass estimation model, a recursive least squares vehicle mass estimation model equation group is constructed;

[0180] Calculate the estimated value of vehicle mass according to the target vehicle data and the recursive least squares vehicle mass estimation model equation group;

[0181] The second road surface slope value is calculated based on the estimated value of the vehicle mass, the state equation of the dynamic system and the observation equation of the dynamic system.

[0182] In some embodiments, the actual noise during the sensor measurement of the system model is generally large, and the direct estimation of the vehicle mass is greatly affected by the acceleration and driving force noise, which ultimately leads to a low reliability of the estimated value. Therefore, the recursive least squares method with a forgetting factor can be used to estimate the vehicle mass; the main idea is to continuously correct the previous parameter values ​​through the newly obtained parameter values ​​to obtain the optimal value of the parameter. The vehicle longitudinal dynamics model can be first organized into the least squares form: y = h T θ, where y is the system output vector, h is the system input sample set, and θ is the parameter set to be identified. 0 =tanβ, β is the equivalent slope, and the vehicle longitudinal dynamics equation is transformed by the least square method to obtain the least square method formal model, where the expression of the least square method formal model is: It can be understood that, corresponding to the least squares form, The selection of unknown parameters of the linear model in the standard least squares method is to ensure that the sum of the squares of the differences between the actual observed values ​​and the calculated values ​​is minimized. For a linear system, a cost function can be constructed based on the least squares formal model, where the expression of the cost function is: In the formula, is the parameter estimation, and φ is the system observable. Solve the cost function to get the cost function value, where the expression of the cost function value is: Since the two parameters to be estimated are time-varying, if the above formula is iteratively and recursively updated with new data obtained each time, the calculation result will be more efficient. The initial recursive least squares equation system can be constructed according to the cost function value, where the expression of the initial recursive least squares equation system is: In the formula, is the parameter estimate calculated based on the previous k observations, K k is the gain vector, y k is the output of the system, φ k is the observable quantity of the system, P k is the updated covariance matrix, whose initial value is obtained by experience, and I is the identity matrix.

[0183] In the practical application of the recursive least squares algorithm, as the recursion moves forward, Pk will decay rapidly, and K k It will also decay rapidly, making the correction effect of new data on parameter estimation smaller and smaller until the effect is lost. The main reason for this phenomenon is that all data are given the same degree of trust in the identification process. In order to solve this problem, the concept of forgetting factor is introduced, so that the correction effect of new data on parameter estimation obtained in the identification process will become stronger, while the correction effect of old data on parameter estimation will become weaker. The target recursive least squares system of equations can be constructed based on the forgetting factor and the initial recursive least squares system of equations, where the expression of the target recursive least squares system of equations is: In the formula, ω k is the forgetting factor at time k, usually satisfying ω k ∈(0,1), the larger the value, the higher the identification accuracy, but the slower the convergence speed, and vice versa, the identification accuracy decreases, but the convergence speed becomes faster. Then, according to the vehicle longitudinal dynamics equation, a recursive least squares mass estimation model is constructed, where the expression of the recursive least squares mass estimation model is: make represents the output of the system, Represents the observation matrix of the system, and M is the parameter to be estimated. According to the target recursive least squares equations and the recursive least squares mass estimation model, the recursive least squares vehicle mass estimation model equations can be constructed, where the expression of the recursive least squares vehicle mass estimation model equations is: In the formula, is the optimal estimate of the vehicle mass at time k, is the optimal estimate of the vehicle mass at time k-1, y k is the output of the system at time k, E k is the observation matrix of the system at time k. The real-time value of the vehicle mass can be identified and estimated through the recursive least squares vehicle mass estimation model equation group. Finally, the vehicle mass estimation value is calculated based on the target vehicle data and the recursive least squares vehicle mass estimation model equation group, and the second road slope value is calculated based on the vehicle mass estimation value, the dynamic system state equation and the dynamic system observation equation.

[0184] In some embodiments, in step S107, the interactive multi-model method is used to perform data fusion according to the first road surface slope value and the second road surface slope value to obtain the road surface slope value of all working conditions, which may include but is not limited to the following steps:

[0185] Construct system model state equations and system model measurement equations;

[0186] Set up the transition probability matrix;

[0187] According to the transition probability matrix, calculate the model prediction probability;

[0188] Calculate the mixing probability between models based on the model prediction probability;

[0189] Calculate the model mixing state estimate based on the mixing probability between models;

[0190] Based on the model mixture state estimate, the model mixture covariance estimate is calculated;

[0191] Calculate the state prediction value according to the system model state equation and the system model measurement equation;

[0192] Calculate the prediction error covariance based on the state prediction value and the model mixture covariance estimate;

[0193] Calculate the residual according to the system model measurement equation;

[0194] Based on the residuals, calculate the residual covariance matrix;

[0195] Construct the likelihood function based on the prediction error covariance and Gaussian distribution;

[0196] Calculate the Kalman filter gain matrix based on the prediction error covariance and residual covariance matrix;

[0197] Calculate the state estimate value according to the Kalman filter gain matrix;

[0198] Based on the state estimates, the interaction error covariance matrix is ​​calculated;

[0199] According to the interaction error covariance matrix, the motion state probability is calculated;

[0200] Calculate the total probability based on the motion state probability;

[0201] Calculate the model probability based on the likelihood function, total probability and model probability normalization factor;

[0202] According to the model probability, construct the interactive estimated state value equation and the interactive covariance equation;

[0203] The road surface slope value for all working conditions is calculated according to the interactive estimation state value equation, the interactive covariance equation, the first road surface slope value and the second road surface slope value.

[0204] In some embodiments, the accuracy of slope estimation based on the vehicle dynamics model depends on the accuracy of the system model, and the parameters in the system model are greatly affected by high-frequency noise; the disadvantage of slope estimation based on the vehicle kinematics model is that the measured value of the sensor is greatly affected by static deviation. Therefore, the present invention adopts an interactive multi-model algorithm with a Markov switching coefficient, uses different motion models to match different motion states of the target respectively, and multiple models work in parallel. Through the Markov chain, the Kalman filter algorithm is used to realize the update of the target state estimate and model probability. Combining the Kalman filter algorithm and the interactive multi-model algorithm to construct an adaptive model weight switching can adapt to the real-time vehicle state, automatically reduce the interference of error factors under different operating environments, and then select the optimal estimation model to improve the accuracy of slope estimation. In the interactive multi-model method, it is assumed that the system satisfies the finite state Markov chain, and it can switch between different mathematical models, and complete the interaction process between each sub-model by re-input initialization of the sub-filter in the filtering recursive process.

[0205] The process of interactive multi-model method is as follows Figure 6 As shown, using interactive multi-model filtering, the system model state equation and the system model measurement equation can be constructed first, where the expression of the system model state equation is: The expression of the system model measurement equation is: k =H k X k +ω k , where Indicates the valid mode m corresponding to time k-1 j The system state transfer matrix, Indicates the effective mode m at time k j The system noise excitation matrix, H k Indicates the effective mode m at time k j The measurement matrix, v k-1 is the measurement noise, ω k is process noise. The mean of the measurement noise and the mean of the process noise are both Gaussian white noise with a value of zero. k-1 is the variance matrix corresponding to the measurement noise, R k is the variance matrix corresponding to the process noise. The interactive multi-model mainly includes four stages: input interaction, filter update, probability update and output interaction. The transition probability between each model obeys the Markov process, and the transition probability matrix can be set. The expression of the transition probability matrix is: In the formula, p ijis the transition probability from model i to model j, i, j = 1, 2, ..., r. In the input interaction step, the estimates of each model are mixed to generate the initial conditions of the model input. Input interaction is actually a process of redistributing the estimation results of each filter. In this process, the state estimation value of the target at the previous moment (k-1 moment) is and the model probability of each sub-filter Get the mixed estimated state and covariance And use it as the initial state. The model prediction probability can be calculated according to the transition probability matrix, where the calculation formula for the prediction probability of model j is: According to the model prediction probability, the model mixing probability is calculated, where the calculation formula for the model mixing probability from model i to model j is: In the formula, is the probability of model i at time k-1, p ij is the transition probability from model i to model j. Then, according to the mixing probability between models, the model mixing state estimation is calculated, where the calculation formula for the mixing state estimation of model j is: According to the model mixed state estimate, the model mixed covariance estimate is calculated, where the calculation formula for the mixed covariance estimate of model j is: In the formula, r is the number of models, is the state update of model i at k-1, is the covariance matrix of model i in k-1.

[0206] In the filter update stage, the purpose of input interaction is to initialize the filter input value at time k based on the estimation results of each sub-filter at time k-1, and obtain the mixed estimated input information at time k-1; filter update uses the input interaction value obtained by input interaction, and combines each sub-model and measurement information, uses Kalman filtering to independently filter and update the state of each model at time k, and selects a suitable filter based on the characteristics of each sub-model. The state prediction value can be calculated based on the system model state equation and the system model measurement equation, where the calculation formula of the state prediction value is: In the formula, Represents the Kalman filter estimate at time k-1. According to the state prediction value and the model mixed covariance estimate, the prediction error covariance is calculated, where the calculation formula of the prediction error covariance is: In the formula, r k is the system mode variable, P k-1|k-1 represents the estimated error covariance matrix at time k-1, is the variance matrix of the measurement noise in the system state equation corresponding to time k-1, Represents the prediction error at time k-1.

[0207] Then, the residual is calculated according to the system model measurement equation, where the residual calculation formula is: According to the residual, the residual covariance matrix is ​​calculated, where the calculation formula of the residual covariance matrix is: In the formula, is the variance matrix of the process noise in the system state equation corresponding to time k. According to the prediction error covariance and Gaussian distribution, the likelihood function is constructed, where, and r k =j The expression of the likelihood function corresponding to is: Furthermore, if the Gaussian distribution is satisfied, the likelihood function is expressed as: The Kalman filter gain matrix is ​​calculated based on the prediction error covariance and the residual covariance matrix, where the calculation formula of the Kalman filter gain matrix is: According to the Kalman filter gain matrix, the state estimation value is calculated, where the calculation formula for the state estimation value obtained by filter j at time k is: According to the state estimation value, the interaction error covariance matrix is ​​calculated, where the calculation formula of the interaction error covariance matrix obtained by filter j at time k is: In the formula, represents the previous prediction value of the state corresponding to filter j at time k, represents the previous covariance of the state corresponding to filter j at time k, is the corresponding filter gain matrix, is the corresponding new information sequence, represents the state estimate obtained by filter j at time k, Represents the interaction error covariance matrix obtained by filter j at time k.

[0208] In the probability update phase, the model probability The value of changes with the change of the target motion state. It indicates the effectiveness of each sub-model in matching the current motion state. The model probability can be calculated by the likelihood function and needs to be continuously updated. If the sub-model accurately represents the current motion law, the filter's new information sequence has a mean of zero and a variance of Gaussian white noise, then the likelihood function corresponding to the model at time k is The motion state probability can be calculated based on the interaction error covariance matrix, where the motion state probability of model j matching the current motion state at time k is calculated as: Then, the total probability is calculated based on the motion state probability, where the calculation formula for the total probability is: The model probability is calculated based on the likelihood function, the total probability and the model probability normalization factor, where the calculation formula of the model probability is: In the formula, c is the model probability normalization factor,

[0209] In the output interaction stage, the output interaction is the process of weighted output of the estimated values ​​of each sub-filter. On the basis of the estimated values ​​of each filter, after the estimated results of each sub-model in the filter update stage and the sub-model probability in the model probability update stage are calculated, the total interactive estimated state value is obtained by weighted summation. and the interaction covariance P k According to the model probability, the interactive estimated state value equation and the interactive covariance equation can be constructed, where the expression of the interactive estimated state value equation is: The expression of the interaction covariance equation is: Finally, the full-condition road surface slope value is calculated according to the interactive estimation state value equation, the interactive covariance equation, the first road surface slope value and the second road surface slope value. Exemplarily, the correct probability of the model is calculated by interactive multi-model as a weighting coefficient, and the model output state is weighted. The system state is estimated by effective weighted fusion, the matching probability of each sub-model is adjusted in real time, and the filter update and model probability update are performed. The fusion output result is always tracked by the sub-model output with a small error according to the model transfer matrix to obtain the full-condition road surface slope value, wherein the full-condition road surface slope value is used to adjust the vehicle driving parameters so that the vehicle can drive normally, thereby improving the safety of vehicle driving.

[0210] In some embodiments, the overall process of estimating the slope using the vehicle longitudinal kinematics model and the vehicle longitudinal dynamics model is as follows: Figure 7As shown in the figure, the vehicle longitudinal kinematic model and the vehicle longitudinal dynamic model can be constructed first, and then the state equation and observation equation under the vehicle longitudinal kinematic model are established, and the state equation and observation equation under the vehicle longitudinal dynamic model are established. Then, based on the box plot method, the measured data is converted into a graphical method for abnormal point detection, and the abnormal data is eliminated. Then, the Kalman filter is used to estimate the slope value under the kinematic model, and the slope based on the vehicle kinematic model is estimated. At the same time, the recursive least squares method with a forgetting factor is used to identify the vehicle mass based on the vehicle dynamics model, and then the Kalman filter is continuously selected to realize the real-time estimation of the road slope value. Finally, the model output state is weightedly calculated based on the interactive multi-model, the matching probability of each sub-model is adjusted in real time, and the system state is estimated by effective weighted fusion. The total estimated state value and covariance are obtained by weighted fusion according to the model transfer matrix to calculate the road slope value of all working conditions.

[0211] In some embodiments, due to the fact that the vehicle is inevitably affected by the high-frequency transient characteristics of the vehicle and the driving conditions under all working conditions, the residual value of the data collected by a single sensor is large, or the traditional single model is used without eliminating abnormal data, resulting in inaccurate estimation of the slope value. Based on this, the longitudinal kinematic model and longitudinal dynamic model of the driving vehicle are used in this embodiment, and the collected data are firstly eliminated by using the box plot method for abnormal processing; the road slope value is estimated for the longitudinal kinematic model data of the vehicle after eliminating the abnormal data according to the Kalman filter; the recursive least squares method with forgetting factor is used to identify and estimate the vehicle mass after elimination, so as to further estimate the road slope value; and then combined with the interactive multi-model, the interactive mixed estimation of the Kalman filters corresponding to the two models is calculated by using the model probability and the model conversion probability, and the new estimated value and model probability are calculated according to the input mixed estimation and the measured data, and the probability weighted sum of the state estimation values ​​of all filters is used as the final estimated value. It overcomes the error of system state estimation by a single model and improves the robustness of the estimation of road slope values ​​under all working conditions. It has the characteristics of strong real-time performance, high accuracy and strong robustness.

[0212] The beneficial effects of implementing the embodiments of the present invention include: the embodiments of the present invention first obtain vehicle measurement data, a vehicle longitudinal kinematic model, and a vehicle longitudinal dynamic model, then construct a kinematic system state equation and a kinematic system observation equation based on the vehicle longitudinal kinematic model, construct a dynamic system state equation and a dynamic system observation equation based on the vehicle longitudinal dynamic model, and use a box plot method to perform abnormal point detection processing on the vehicle measurement data to obtain target vehicle data, and then use the Kalman filter method to calculate the first road surface slope value under the vehicle longitudinal kinematic model based on the target vehicle data, the kinematic system state equation, and the kinematic system observation equation, and use the recursive least squares method to calculate the second road surface slope value under the vehicle longitudinal dynamic model based on the target vehicle data, the dynamic system state equation, and the dynamic system observation equation, and finally use the interactive multi-model method to perform data fusion based on the first road surface slope value and the second road surface slope value to obtain the road surface slope value of all working conditions, so that the Kalman filter method and the recursive least squares method can be combined to realize the estimation of the road surface slope of all working conditions, thereby improving the accuracy.

[0213] like Figure 8 As shown, an embodiment of the present invention further provides a device for estimating the slope of a road surface under all working conditions, comprising:

[0214] The first module 801 is used to obtain vehicle measurement data, a vehicle longitudinal kinematic model and a vehicle longitudinal dynamic model;

[0215] The second module 802 is used to construct a kinematic system state equation and a kinematic system observation equation according to the longitudinal kinematic model of the vehicle;

[0216] The third module 803 is used to construct a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamics model;

[0217] The fourth module 804 is used to perform outlier detection processing on the vehicle measurement data using a box plot method to obtain target vehicle data;

[0218] The fifth module 805 is used to calculate the first road surface slope value under the vehicle longitudinal kinematic model by using the Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation;

[0219] The sixth module 806 is used to calculate the second road slope value under the vehicle longitudinal dynamics model by using a recursive least square method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation;

[0220] The seventh module 807 is used to perform data fusion using an interactive multi-model method based on the first road surface slope value and the second road surface slope value to obtain a road surface slope value for all working conditions, and the road surface slope value for all working conditions is used to adjust vehicle driving parameters.

[0221] The contents of the above method embodiments are all applicable to the present device embodiments. The functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0222] like Fig. 9 As shown, an embodiment of the present invention further provides a computer device, including:

[0223] at least one processor 901;

[0224] At least one memory 902, used to store at least one program;

[0225] When at least one program is executed by at least one processor, the at least one processor implements Figure 1 The method shown.

[0226] The contents of the above method embodiments are all applicable to the present device embodiments. The functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0227] The embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, Figure 1 The method shown.

[0228] The contents of the above method embodiments are all applicable to the present storage medium embodiments. The functions specifically implemented by the present storage medium embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0229] The preferred embodiments of the present invention are described above with reference to the accompanying drawings, but the scope of the rights of the present invention is not limited thereto. Any modification, equivalent substitution and improvement made by a person skilled in the art without departing from the scope and essence of the present invention should be within the scope of the rights of the present invention.

Claims

1. A method for estimating the slope of a road surface under all working conditions, characterized in that: The following steps are involved: Obtain vehicle measurement data, vehicle longitudinal kinematics model and vehicle longitudinal dynamics model; According to the longitudinal kinematic model of the vehicle, constructing a kinematic system state equation and a kinematic system observation equation; According to the longitudinal dynamics model of the vehicle, constructing a state equation of the dynamics system and an observation equation of the dynamics system; Using a box plot method to perform outlier detection processing on the vehicle measurement data to obtain target vehicle data; Calculating a first road surface slope value under the longitudinal kinematic model of the vehicle by using a Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation; Calculating a second road surface slope value under the vehicle longitudinal dynamics model by using a recursive least squares method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation; According to the first road surface slope value and the second road surface slope value, data fusion is performed using an interactive multi-model method to obtain a full-condition road surface slope value, and the full-condition road surface slope value is used to adjust vehicle driving parameters.

2. The method according to claim 1, characterized in that The step of constructing a kinematic system state equation and a kinematic system observation equation according to the vehicle longitudinal kinematic model includes: Constructing a kinematic system state vector according to the longitudinal kinematic model of the vehicle; According to the acceleration sensor measurement equation, the slope value equation is constructed; Constructing the kinematic system state equation according to the kinematic system state vector and the slope value equation; Performing matrix transformation on the kinematic system state equation to obtain a kinematic matrix state equation; The kinematic system observation equation is constructed according to the kinematic matrix state equation.

3. The method according to claim 1, characterized in that The step of constructing a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamics model includes: Calculate the vehicle driving force based on the current output torque of the engine, the final reduction ratio of the transmission, the transmission ratio of the transmission, the transmission efficiency from the turbine to the driving wheel, and the tire radius; Calculate air resistance based on air density, air resistance coefficient, car frontal area and relative air flow speed; Calculate rolling resistance based on vehicle mass, rolling resistance coefficient, gravitational acceleration and slope angle; Calculating slope resistance according to the vehicle mass, the gravitational acceleration and the slope angle; Calculating the acceleration resistance according to the vehicle mass, vehicle acceleration and the constant coefficient of the rotational mass inertia moment converted into the translational mass inertia force; Constructing a vehicle longitudinal dynamics equation according to the vehicle longitudinal dynamics model, the vehicle driving force, the air resistance, the rolling resistance, the slope resistance and the acceleration resistance; Performing Taylor expansion on the vehicle longitudinal dynamics equation to obtain a vehicle longitudinal dynamics discrete model; Constructing a dynamic system state vector according to the vehicle longitudinal dynamics discrete model, the vehicle mass and the slope angle; constructing a state space equation of the dynamic system according to the equivalent slope and the state vector of the dynamic system; Simplifying the state vector of the dynamic system to obtain a dynamic simplified state vector; Constructing a system state differential equation according to the state space equation of the dynamic system and the dynamic simplified state vector; The system state differential equation is discretized by using the forward Euler method to obtain a discretized difference equation; According to the discretized difference equation, a dynamic discrete Kalman filter equation is constructed; Constructing a discrete state equation according to the discretized difference equation and the dynamic discrete Kalman filter equation; According to the discrete state equation, construct the state equation of the dynamic system; The observation equation of the dynamic system is constructed according to the discrete state equation and the observation matrix.

4. The method according to claim 1, characterized in that: The process of performing outlier detection processing on the vehicle measurement data using a box plot method to obtain target vehicle data includes: Construct a box plot, wherein the box plot includes a minimum value, a lower quartile, a median, an upper quartile, and a maximum value; constructing an outlier detection equation based on the vehicle measurement data and the box plot; Performing differentiation processing on the outlier detection equation to obtain an outlier detection differential equation; Simplifying the outlier detection differential equation to obtain a simplified outlier detection equation; The simplified outlier detection equation is transformed to obtain the target vehicle data.

5. The method according to claim 1, characterized in that The method of calculating the first road surface slope value under the longitudinal kinematic model of the vehicle by using the Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation includes: Constructing a kinematic Kalman filter equation according to the kinematic system state equation and the kinematic system observation equation; Calculating a predicted state quantity and a predicted error covariance matrix according to the target vehicle data; Calculating the Kalman gain according to the prediction error covariance matrix; Calculating a target estimated state quantity according to the predicted state quantity and the Kalman gain; Calculating a target estimation error covariance according to the prediction error covariance matrix and the Kalman gain; The first road surface slope value is calculated according to the kinematic Kalman filter equation, the target estimated state quantity and the target estimated error covariance.

6. The method according to claim 3, characterized in that The method of calculating the second road surface slope value under the vehicle longitudinal dynamics model by using a recursive least square method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation includes: Performing least squares transformation on the longitudinal dynamics equation of the vehicle to obtain a least squares formal model; constructing a cost function according to the least squares formal model; Solving the cost function to obtain a cost function value; constructing an initial recursive least squares equation system according to the cost function value; Constructing a target recursive least squares equation system according to the forgetting factor and the initial recursive least squares equation system; Constructing a recursive least squares mass estimation model according to the vehicle longitudinal dynamics equation; Constructing a recursive least squares vehicle mass estimation model equation group according to the target recursive least squares equation group and the recursive least squares mass estimation model; Calculating a vehicle mass estimation value according to the target vehicle data and the recursive least squares vehicle mass estimation model equation group; The second road surface slope value is calculated according to the estimated value of the vehicle mass, the state equation of the dynamic system and the observation equation of the dynamic system.

7. The method according to claim 1, characterized in that The method of performing data fusion using an interactive multi-model method according to the first road surface slope value and the second road surface slope value to obtain a road surface slope value for all working conditions includes: Construct system model state equations and system model measurement equations; Set up the transition probability matrix; Calculating the model prediction probability according to the transition probability matrix; Calculating the mixing probability between models based on the model prediction probabilities; Calculating a model mixing state estimate based on the inter-model mixing probability; Calculating a model mixture covariance estimate based on the model mixture state estimate; Calculating a state prediction value according to the system model state equation and the system model measurement equation; Calculating a prediction error covariance based on the state prediction value and the model mixture covariance estimate; Calculating residuals according to the system model measurement equation; Calculating a residual covariance matrix based on the residuals; Constructing a likelihood function based on the prediction error covariance and Gaussian distribution; Calculating a Kalman filter gain matrix according to the prediction error covariance and the residual covariance matrix; Calculating a state estimate according to the Kalman filter gain matrix; Calculating an interaction error covariance matrix based on the state estimate; Calculating motion state probability according to the interaction error covariance matrix; Calculate the total probability according to the motion state probability; Calculating the model probability according to the likelihood function, the total probability and the model probability normalization factor; According to the model probability, constructing an interactive estimated state value equation and an interactive covariance equation; The full-condition road surface slope value is calculated according to the interactive estimated state value equation, the interactive covariance equation, the first road surface slope value and the second road surface slope value.

8. A device for estimating the slope of a road surface under all working conditions, characterized in that: include: The first module is used to obtain vehicle measurement data, vehicle longitudinal kinematics model and vehicle longitudinal dynamics model; The second module is used to construct a kinematic system state equation and a kinematic system observation equation according to the longitudinal kinematic model of the vehicle; The third module is used to construct a dynamic system state equation and a dynamic system observation equation according to the vehicle longitudinal dynamics model; The fourth module is used to perform abnormal point detection processing on the vehicle measurement data using a box plot method to obtain target vehicle data; A fifth module is used to calculate a first road surface slope value under the longitudinal kinematic model of the vehicle by using a Kalman filter method according to the target vehicle data, the kinematic system state equation and the kinematic system observation equation; A sixth module, for calculating a second road surface slope value under the vehicle longitudinal dynamics model by using a recursive least squares method according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation; The seventh module is used to perform data fusion using an interactive multi-model method according to the first road surface slope value and the second road surface slope value to obtain a road surface slope value for all working conditions, and the road surface slope value for all working conditions is used to adjust vehicle driving parameters.

9. A computer device, characterized in that: include: at least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the method according to any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images