A full-working-condition road surface slope estimation method and device and a storage medium

Abstract

The application discloses a full-condition road slope estimation method and device and a storage medium, and the method comprises the following steps: acquiring vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model; constructing a kinematics system state equation and a kinematics system observation equation; constructing a dynamics system state equation and a dynamics system observation equation; performing abnormal point detection processing on the vehicle measurement data by using a box chart 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 slope value under the vehicle longitudinal dynamics model by using a recursive least square method; performing data fusion by using an interactive multiple model method to obtain a full-condition road slope value, and the full-condition road slope value is used for adjusting vehicle driving parameters. The application realizes full-condition road slope estimation and improves the accuracy. The application can be widely applied to the technical field of vehicle engineering.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of vehicle engineering, and in particular to a full-condition road slope estimation method and device and a storage medium. BACKGROUND

[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 angle displacement sensor or a single vehicle dynamics model to measure slope data. However, the measurement process of the single pendulum angle displacement sensor is affected by the high-frequency transient motion characteristics of the vehicle and the driving conditions, resulting in low signal-to-noise ratio and inaccurate measurement of the slope value. The vehicle dynamics model relies on accurate model parameters, and high-frequency noise disturbance and vehicle acceleration resistance result in low model calculation accuracy.

[0003] In summary, the technical problems in the related art need to be improved. SUMMARY

[0004] The embodiments of the present application provide a full-condition road slope estimation method, device and storage medium, which effectively improve the accuracy.

[0005] In one aspect, the embodiments of the present application provide a full-condition road slope estimation method, comprising the following steps:

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

[0007] According to the vehicle longitudinal kinematics model, a kinematics system state equation and a kinematics system observation equation are constructed;

[0008] According to the vehicle longitudinal dynamics model, a dynamics system state equation and a dynamics system observation equation are constructed;

[0009] The box plot method is used to detect and process abnormal points of the vehicle measurement data to obtain target vehicle data;

[0010] According to the target vehicle data, the kinematics system state equation and the kinematics system observation equation, a first road slope value under the vehicle longitudinal kinematics model is calculated by using the Kalman filtering method;

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

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

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

[0014] constructing a kinematics system state vector according to the vehicle longitudinal kinematics model;

[0015] constructing a slope value equation according to an acceleration sensor measurement equation;

[0016] constructing the kinematics system state equation according to the kinematics system state vector and the slope value equation;

[0017] performing matrix conversion on the kinematics system state equation to obtain a kinematics matrix state equation;

[0018] constructing the kinematics system observation equation according to the kinematics matrix state equation.

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

[0020] calculating a vehicle driving force according to a current output torque of an engine, a main reduction ratio of a transmission, a transmission ratio of the transmission, a transmission efficiency between a turbine and a driving wheel, and a tire radius;

[0021] calculating an air resistance according to an air density, an air resistance coefficient, an air flow relative speed, and an air flow area of the vehicle;

[0022] calculating a rolling resistance according to a total vehicle mass, a rolling resistance coefficient, a gravitational acceleration, and a slope angle;

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

[0024] calculating an acceleration resistance according to the total vehicle mass, a vehicle acceleration, and a constant coefficient for converting a rotational mass inertia force couple into a 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 dynamics system state vector according to the vehicle longitudinal dynamics discrete model, the total vehicle mass and the slope angle;

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

[0029] simplifying the dynamics system state vector to obtain a dynamics simplified state vector;

[0030] constructing a system state differential equation according to the dynamics system state space equation and the dynamics simplified state vector;

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

[0032] constructing a dynamics discrete Kalman filtering equation according to the discretized difference equation;

[0033] constructing a discrete state equation according to the discretized difference equation and the dynamics discrete Kalman filtering equation;

[0034] constructing the dynamics system state equation according to the discrete state equation;

[0035] constructing the dynamics system observation equation according to the discrete state equation and an observation matrix.

[0036] In some embodiments, the abnormal point detection processing of the vehicle measurement data by using the box plot method to obtain the target vehicle data comprises:

[0037] constructing a box plot, the box plot comprising a minimum value, a lower quartile, a median, an upper quartile and a maximum value;

[0038] constructing an abnormal value detection equation according to the vehicle measurement data and the box plot;

[0039] differential processing the abnormal value detection equation to obtain an abnormal value detection differential equation;

[0040] simplifying the abnormal value detection differential equation to obtain an abnormal value detection simplified equation;

[0041] converting the abnormal value detection simplified equation to obtain the target vehicle data.

[0042] In some embodiments, the first road slope value under the vehicle longitudinal kinematics model is calculated by using the Kalman filtering method according to the target vehicle data, the kinematics system state equation and the kinematics system observation equation, comprising:

[0043] constructing a kinematic Kalman filtering 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 a Kalman gain according to the predicted error covariance matrix;

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

[0047] calculating a target estimated error covariance according to the predicted error covariance matrix and the Kalman gain;

[0048] calculating the first road slope value according to the kinematic Kalman filtering 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 according to the target vehicle data, the dynamics system state equation and the dynamics system observation equation comprises:

[0050] performing a least square method conversion on the vehicle longitudinal dynamics equation to obtain a least square method form model;

[0051] constructing a cost function according to the least square method form model;

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

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

[0054] constructing a target recursive least square equation set according to a forgetting factor and the initial recursive least square equation set;

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

[0056] constructing a recursive least square whole vehicle mass estimation model equation set according to the target recursive least square equation set and the recursive least square mass estimation model;

[0057] calculating a whole vehicle mass estimation value according to the target vehicle data and the recursive least square whole vehicle mass estimation model equation set;

[0058] calculating the second road slope value according to the whole vehicle mass estimation value, the dynamics system state equation and the dynamics system observation equation.

[0059] In some embodiments, the data fusion is performed by using an interacting multiple model method according to the first road slope value and the second road slope value to obtain a full-condition road slope value, including:

[0060] constructing a system model state equation and a system model measurement equation;

[0061] setting a transition probability matrix;

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

[0063] calculating an inter-model mixing probability according to the model prediction probability;

[0064] calculating a model mixing state estimation according to the inter-model mixing probability;

[0065] calculating a model mixing covariance estimation according to the model mixing state estimation;

[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 according to the state prediction value and the model mixing covariance estimation;

[0068] calculating a residual according to the system model measurement equation;

[0069] calculating a residual covariance matrix according to the residual;

[0070] constructing a likelihood function according to the prediction error covariance and a Gaussian distribution;

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

[0072] calculating a state estimation value according to the Kalman filter gain matrix;

[0073] calculating an interaction error covariance matrix according to the state estimation value;

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

[0075] calculating a full probability according to the motion state probability;

[0076] calculating a model probability according to the likelihood function, the full probability, and a model probability normalization factor;

[0077] constructing an interaction estimation state value equation and an interaction covariance equation according to the model probability;

[0078] According to the interaction state value equation, the interaction covariance equation, the first road slope value and the second road slope value, the all-condition road slope value is calculated.

[0079] In another aspect, an embodiment of the present application provides an all-condition road slope estimation device, comprising:

[0080] A first module is configured to acquire vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model.

[0081] A second module is configured to construct a kinematics system state equation and a kinematics system observation equation according to the vehicle longitudinal kinematics model.

[0082] A third module is configured to construct a dynamics system state equation and a dynamics system observation equation according to the vehicle longitudinal dynamics model.

[0083] A fourth module is configured to perform outlier detection processing on the vehicle measurement data by using a box plot method to obtain target vehicle data.

[0084] A fifth module is configured to calculate a first road slope value under the vehicle longitudinal kinematics model by using a Kalman filtering method according to the target vehicle data, the kinematics system state equation and the kinematics system observation equation.

[0085] A sixth module is configured to calculate a 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.

[0086] A seventh module is configured to perform data fusion by using an interactive multiple model method according to the first road slope value and the second road slope value to obtain an all-condition road slope value, wherein the all-condition road slope value is used to adjust vehicle driving parameters.

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

[0088] at least one processor;

[0089] at least one memory configured to store 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 method.

[0091] In another aspect, an embodiment of the present application provides a computer readable storage medium, wherein the computer readable storage medium stores a computer program, and the computer program is executed by a processor to implement the method.

[0092] The application has the following beneficial effects:

[0093] The embodiment of the application first acquires vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model, then constructs a kinematics system state equation and a kinematics system observation equation according to the vehicle longitudinal kinematics model, constructs a dynamics system state equation and a dynamics system observation equation according to the vehicle longitudinal dynamics model, detects and processes abnormal points of the vehicle measurement data by using a box plot method to obtain target vehicle data, calculates a first road slope value under the vehicle longitudinal kinematics model by using a Kalman filtering method according to the target vehicle data, the kinematics system state equation and the kinematics system observation equation, calculates a 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, and finally performs data fusion by using an interactive multiple model method according to the first road slope value and the second road slope value to obtain a full-condition road slope value, so that the full-condition road slope estimation can be realized by combining the Kalman filtering method and the recursive least square method, and the accuracy is improved.

[0094] Additional features and advantages of the application will be set forth in the descriptions to follow, and in part will become apparent to those skilled in the art upon examination of the following or can be learned by practice of the application. The objects and other advantages of the application can be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings. BRIEF DESCRIPTION OF DRAWINGS

[0095] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the embodiment description. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained from these drawings without any creative effort.

[0096] Figure 1 A flow chart of a full-condition road slope estimation method according to an embodiment of the present application;

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

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

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

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

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

[0102] Figure 7 A schematic diagram of an overall process for slope estimation by a vehicle longitudinal kinematic model and a vehicle longitudinal dynamic model for an embodiment of the present application;

[0103] Figure 8 A schematic diagram of the structure of a full-working-condition road surface slope estimation device for an embodiment of the present application;

[0104] Figure 9 A schematic diagram of the hardware structure of a computer device for an embodiment of the present application. DETAILED DESCRIPTION

[0105] In order to make the objects, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and not intended to limit the present application. When the following description refers to the accompanying drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with 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 appended claims.

[0106] It can be understood that the terms "first", "second", and the like as used herein can be used to describe various concepts, but unless specifically stated, 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 embodiments of the present application, the first information can also be referred to as the second information, and similarly, the second information can also be referred to as the first information. Depending on the context, the word "if" as used herein can be interpreted as "when" or "upon" or "in response to determining".

[0107] The terms "at least one", "multiple", "each", "any", and the like used herein include one, two or more than two, multiple includes two or more than two, 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 commonly understood by one of ordinary skill in the art to which the present application belongs. The terms used herein are only for the purpose of describing the embodiments of the present application and are not intended to limit the present application.

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

[0110] Kalman filtering: it is an algorithm for optimal estimation of system state by using linear system state equation and 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-and-whisker plot, box plot or box-and-whisker plot, is a statistical chart used to display the dispersion of a set of data. It is named because of its shape like a box. It is often used in various fields, commonly used in quality management. It is mainly used to reflect the characteristics of the distribution of original data, and can also compare the distribution characteristics of multiple sets of data. The drawing method of box 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 the lower edge with the box, and the median is in the middle of the box.

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

[0113] In the related art, with the in-depth promotion of vehicle intelligence and automation, and the increasing accuracy and real-time requirement of vehicle dynamics control, longitudinal slope information is an important input parameter for vehicle chassis electronic control, which directly affects engine fuel consumption and vehicle driving comfort. In the advanced auxiliary driving system, the system will monitor the vertical force of the vehicle tire and the slope direction in real time, and timely issue a warning to prevent vehicle rollover. Therefore, accurately and timely obtaining the road slope value is particularly important for improving the safety and fuel economy of vehicle driving. In the current method for detecting road slope, most of them use GPS or pendulum type angular displacement sensor to measure and calculate the slope data, or simply rely on vehicle dynamics model. The measurement process based on sensor is easily affected by the high-frequency transient motion characteristics and driving conditions of the vehicle, resulting in low signal-to-noise ratio and inaccurate slope value. The identification method based on vehicle dynamics model relies more on the accuracy of model parameters, the disturbance is mostly high-frequency noise, the system model does not consider the influence of wind speed resistance, and the initial abnormal data is not removed in the calculation process, resulting in low accuracy of the final slope estimation value. The traditional vehicle dynamics model does not consider the case of vehicle acceleration, does not consider the acceleration resistance when the mass needs to overcome the acceleration, resulting in a large error in the estimation value of the vehicle dynamics model and low accuracy and robustness.

[0114] Therefore, the embodiment constructs a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model to determine the system state equation and the system observation equation of the road slope estimation under all working conditions. The state input variables of the state equation constructed based on the vehicle longitudinal kinematics model include the vehicle longitudinal speed, and the state input variables of the state equation of the system model considering the influence of acceleration resistance are constructed based on the vehicle longitudinal dynamics model. At the same time, the initial value is measured according to the system observation equation to perform data cleaning, and the box plot method is used to remove abnormal data. The road slope based on the vehicle longitudinal kinematics model is estimated according to Kalman filtering. The whole vehicle mass based on the vehicle dynamics model is identified and estimated by recursive least squares with forgetting factor, and then the slope value under the model is estimated. The data of the two models are processed by using the interactive multiple model algorithm, and the total estimated state value and covariance are obtained by input interaction, filtering update, probability update, weighted fusion output interaction, 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] The embodiment of the present application provides a kind of full working condition road surface slope estimation method, it is related to vehicle engineering technical field.The full working condition road surface slope estimation method provided in the embodiment of the present application can be applied in terminal, also can be applied in server, it can also be software running in terminal or server.In some embodiments, terminal can be smart phone, tablet computer, notebook computer, desktop computer, smart speaker, smart watch and vehicle terminal etc., but not limited to this;Server end can be configured as independent physical server, can also be configured as the server cluster or distributed system of multiple physical servers, can also be configured as the cloud server of providing cloud service, cloud database, cloud computing, cloud function, cloud storage, network service, cloud communication, middleware service, domain name service, security service, CDN and basic cloud computing services such as big data and artificial intelligence platform, server can also be a node server in blockchain network;Software can be the application of realizing a kind of full working condition road surface slope estimation method etc., but not limited to the above form.

[0116] The present application can be used in many general or special computer system environments or configurations. For example: personal computer, server computer, handheld device or portable device, tablet device, multi-processor system, microprocessor-based system, set-top box, programmable consumer electronics, network PC, small computer, mainframe computer, distributed computing environment 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 a distributed computing environment, in which tasks are performed by remote processing devices connected by 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 embodiments of the present application will be specifically explained below in conjunction with the drawings:

[0118] Figure 1 is an optional flow chart of the full working condition road surface slope estimation method provided by the embodiment of the present application, Figure 1 The method in the above embodiment can include but not limited to steps S101 to S107.

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

[0120] Step S102, according to vehicle longitudinal kinematics model, construct kinematics system state equation and kinematics system observation equation;

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

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

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

[0124] Step S106, calculating a 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;

[0125] Step S107, performing data fusion by using an interactive multiple model method according to the first road slope value and the second road slope value to obtain a full-condition road slope value, and the full-condition road slope value is used to adjust vehicle driving parameters.

[0126] The steps S101 to S107 shown in the embodiments of the present application realize full-condition road slope estimation and improve the accuracy.

[0127] In step S101 of some embodiments, the vehicle measurement data can be acquired by a sensor, and the vehicle longitudinal kinematics model and the vehicle longitudinal dynamics model can be acquired by a vehicle model library. The vehicle measurement data, the vehicle longitudinal kinematics model and the vehicle longitudinal dynamics model can also be acquired by other ways, which are not limited thereto. For example, the related parameters such as vehicle engine torque can be collected by a controller area network (CAN).

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

[0129] constructing a kinematics system state vector according to the vehicle longitudinal kinematics model;

[0130] constructing a slope value equation according to the acceleration sensor measurement equation;

[0131] constructing the kinematics system state equation according to the kinematics system state vector and the slope value equation;

[0132] performing matrix conversion on the kinematics system state equation to obtain a kinematics matrix state equation;

[0133] constructing the kinematics system observation equation according to the kinematics matrix state equation.

[0134] In some embodiments, the signal of the acceleration sensor can be fused with the vehicle speed information through a vehicle kinematics model, the vehicle speed information and the sensor information are taken as system observations, and the slope value is calculated through the state transition equation. The acceleration data read on the sensor is affected by the driving acceleration of the automobile and is related to the current road slope. Generally, the measured value of the longitudinal acceleration of the vehicle contains two pieces of information: the derivative of the longitudinal speed in the driving direction and the influence of the gravity component; when the vehicle drives on a sloping road, the component of the acceleration sensor along the slope is the vehicle motion acceleration and the gravity acceleration constant along the slope component g sin a. When the automobile drives on a sloping road, the vehicle longitudinal kinematics model is as shown in Figure 2 According to the vehicle longitudinal kinematics model, the kinematics system state vector can be constructed, and the expression of the kinematics system state vector at the k time is x(k) = [v k a x (k) sin a(k)] T According to the measurement principle of the acceleration sensor, the expression of the kinematics system state vector can be converted to: wherein x(k) is the kinematics system state vector at the k time, v k is the vehicle speed at the k time, a x is the measured value of the acceleration sensor, a is the slope angle, g is the gravity acceleration, is the vehicle acceleration. Then, the slope value equation is constructed according to the acceleration sensor measurement equation, and the expression of the slope value equation is: It is generally considered that the vehicle speed, the vehicle acceleration and the slope angle are continuous measurement values that cannot be suddenly changed. Within the time interval At, the kinematics system state equation can be constructed according to the kinematics system state vector and the slope value equation, and the expression of the kinematics system state equation is: wherein v k is the vehicle speed at the k time, v k-1 is the vehicle speed at the k-1 time, is the measured value of the acceleration sensor at the k time, is the measured value of the acceleration sensor at the k-1 time, sin a k is the slope angle at the k time, sin a k-1 is the slope angle at the k-1 time, At is the time interval, is the speed observation noise, is the acceleration observation noise, The noise is represented by the slope value. Then, a matrix transformation is performed on the kinematic system state equation to obtain the kinematic matrix state equation, where the expression of the kinematic matrix state equation is: In the formula, X k Let X be the kinematic matrix state value at time k. k-1 Let Z be the kinematic matrix state value at time k-1, and w be the system observation noise. The vehicle speed of the system can be obtained by reading data from the CAN signal. Based on the kinematic matrix state equation, the kinematic system observation equation can be constructed, where the expression of the kinematic system observation equation is: Z k =H×x k +ζ, where, Z k Let H be the kinematic system observations, H be the observation matrix, and ζ be the observation noise.

[0135] In some embodiments, in step S103, constructing the state equations and observation equations of the dynamic system based on the vehicle's longitudinal dynamics model may include, but is not limited to, the following steps:

[0136] Calculate the vehicle's driving force based on the engine's current output torque, the transmission's final drive ratio, the transmission's gear ratio, the transmission efficiency between the turbine and the drive wheel, and the tire radius.

[0137] Calculate air resistance based on air density, drag coefficient, vehicle frontal area, and relative air velocity.

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

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

[0140] Calculate the acceleration drag by converting the total vehicle mass, vehicle acceleration, and the constant coefficient of the rotational mass inertial couple moment into the translational mass inertial force;

[0141] Based on the vehicle's longitudinal dynamics model, vehicle driving force, air resistance, rolling resistance, gradient resistance, and acceleration resistance, the vehicle's longitudinal dynamics equations are constructed.

[0142] Taylor expansion of the vehicle's longitudinal dynamics equations yields a discrete model of the vehicle's longitudinal dynamics.

[0143] Based on the vehicle's longitudinal dynamics discrete model, vehicle mass, and slope angle, construct the dynamic system state vector;

[0144] Based on the equivalent slope and the state vector of the dynamic system, construct the state-space equation of the dynamic system;

[0145] Simplifying the state vector of the dynamic system, a simplified state vector of the dynamic system is obtained;

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

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

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

[0149] According to the discretized difference equation and the dynamic discrete Kalman filtering equation, a discrete state equation is constructed;

[0150] According to the discrete state equation, a state equation of the dynamic system is constructed;

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

[0152] In some embodiments, the longitudinal dynamic model during vehicle driving is built based on vehicle dynamics theory; when the lateral force is not considered, the driving force and the driving resistance are applied in the driving direction of the vehicle. The driving force is the main driving torque generated by the engine of the power device and transmitted to both sides of the wheel through the transmission device, and the driving resistance is the sum of the driving resistances of the vehicle, including: air resistance, rolling resistance, slope resistance and acceleration resistance. The vehicle driving force can be calculated according to the current output torque of the engine, the main reduction ratio of the transmission, the transmission ratio of the transmission, the transmission efficiency between the turbine and the driving wheel, and the tire radius, wherein 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 the relevant data in the engine messages EEC1, EEC3 and EC1, i g is the main reduction ratio of the transmission, i o is the transmission ratio of the transmission, η t is the transmission efficiency between the turbine and the driving wheel, and R is the tire radius. The air resistance can be calculated according to the air density, the air resistance coefficient, the vehicle windward area and the air flow relative speed, wherein the calculation formula of the air resistance is: In the formula, F air is the air resistance, ρ is the 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 viscosity friction of the corresponding gas is small, C dA is the frontal area of the vehicle, i.e. the projected area of the vehicle in the direction of travel, v is the relative velocity of the air flow, which is the vehicle speed in the absence of wind. The rolling resistance is calculated according to the vehicle mass, the rolling resistance coefficient, the gravitational acceleration and the slope angle, wherein the calculation formula of the rolling resistance is: F fb = Mgf0cos a, wherein F fb is the rolling resistance, M is the vehicle mass, g is the gravitational acceleration, f0is the rolling resistance coefficient, and a is the slope angle.

[0153] The slope resistance is calculated according to the vehicle mass, the gravitational acceleration and the slope angle, wherein the calculation formula of the slope resistance is: F g = Mgsin a, wherein F g is the slope resistance. The acceleration resistance is calculated according to the vehicle mass, the vehicle acceleration and the constant coefficient of the moment of inertia couple of the rotating mass converted into the inertia force of the translational mass, wherein the calculation formula of the acceleration resistance is: wherein F j is the acceleration resistance, d is the constant coefficient of the moment of inertia couple of the rotating mass converted into the inertia force of the translational mass, is the derivative of the vehicle speed with respect to time, is the vehicle acceleration. The vehicle longitudinal dynamics model is shown in Figure 3 , which can be constructed according to the vehicle longitudinal dynamics model, the vehicle driving force, the air resistance, the rolling resistance, the slope resistance and the acceleration resistance, wherein the expression of the vehicle longitudinal dynamics equation is: wherein is the derivative of the vehicle position with respect to time at k moment, v k is the vehicle speed. Further, the expression of the vehicle longitudinal dynamics equation can also be expressed as:

[0154] Then, the Taylor expansion is performed on the vehicle longitudinal dynamics equation to obtain the vehicle longitudinal dynamics discrete model, wherein the expression of the vehicle longitudinal dynamics discrete model is: v k+1 = v k + G(p k , F k , v k , k)T s + e1(k), wherein v k+1 is the vehicle speed at k+1 moment, G(*) is a nonlinear unknown function composed of the vehicle dynamics uncertainty, the completely unknown system parameters and the system disturbance, p k is the vehicle position at kT s moment, F k is the vehicle driving force at kT s moment, and Ts Let ε1 be the sampling interval and εk be the discrete error. Based on the vehicle's longitudinal dynamics discrete model, vehicle mass, and slope angle, a dynamic system state vector is constructed, where the expression for the dynamic system state vector is: y1 k =(v k M k ,α k In the formula, y k M represents the state value of the dynamic system. k Let α be the total vehicle mass at time k. k Let f0 be the slope angle at time k. Based on the equivalent slope and the state vector of the dynamic system, the state-space equation of the dynamic system is constructed. We can let f0 = tanβ, and the expression of the state-space equation of the dynamic system is: In the formula, β is the equivalent slope. Let be the derivative of the total vehicle mass. Let y be the derivative of the slope angle. Simplifying the state vector of the dynamic system, we obtain the simplified dynamic state vector, where the expression for the simplified dynamic state vector is: y k =(v k ,α k Based on the state-space equations and simplified state vectors of the dynamic system, the system state differential equations are constructed, where the expression of the system state differential equations is:

[0155] The forward Euler method is then used to discretize the system state differential equations, resulting in discretized difference equations. The expression for the discretized difference equations is as follows: Based on the discretized difference equation, a dynamic discrete Kalman filter equation is constructed, where the expression of the dynamic discrete Kalman filter equation is: In the formula, Δt represents the time interval. Based on 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 Let k be the system input at time k. During the calculation, the road resistance of the vehicle can be considered a constant. Based on the discrete state equations, the dynamic system state equations are constructed, where the expression for the dynamic system state equations is: In the formula, u k-1 Let w be the system input at time k-1. k The system process noise is represented by the dynamic system observation equations, constructed based on the discrete state equations and the observation matrix. The expression for these equations is: z k =H×y k +ξ kIn the formula, H = [1 0], J is the observation matrix, and z k For the observations of the dynamic system, ξ k This represents the system observation noise. It is understood that the system process noise and the system observation noise are independent and both follow a Gaussian distribution. Given the vehicle mass, the Kalman filter method will continuously iterate and update, ultimately achieving a real-time estimation of the gradient value.

[0156] In some embodiments, step S104, which uses box plotting to perform outlier detection processing on the vehicle measurement data to obtain target vehicle data, may include, but is not limited to, the following steps:

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

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

[0159] The outlier detection equation is differentiated to obtain the outlier detection differential equation;

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

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

[0162] In some embodiments, during the data estimation process of the vehicle's longitudinal kinematics model and longitudinal dynamics model, systematic errors, mechanical errors, and cumulative errors are inevitably introduced. Therefore, it is necessary to first remove outliers from the collected data using box plots to make the estimation process more robust. Box plots are a method of converting data into visual graphics for outlier detection. Box plots are shown below. Figure 4 As shown, the box plot includes the minimum value, lower quartile (Q1), median (Q2), upper quartile (Q3), and maximum value. The lower quartile can be called the first quartile, the median the second quartile, and the upper quartile the third quartile. If there are outliers in the box plot, all data points greater than the maximum or smaller than the minimum are considered outliers. After sorting the data in ascending order, the 25th percentile of the sorted data is designated as Q1, the 50th percentile as Q2, and the 75th percentile as Q3. The box plot method first requires defining the boundaries of outliers: the upper boundary is Q3 + 1.5 × IQR, and the lower boundary is Q1 - 1.5 × IQR, where IQR is the interquartile range (IQR = Q3 - Q1). Data points above the upper boundary or below the lower boundary are considered outliers. Using this effective data range for selection can eliminate outlier interference and increase the data processing's adaptability to uncertainties, i.e., it has robustness.

[0163] A box plot can be constructed first, wherein the box plot comprises a minimum value, a lower quartile, a median, an upper quartile and a maximum value. Then, according to the vehicle measurement data and the box plot, an outlier detection equation is constructed, wherein the expression of the outlier detection equation is: In the formula, F yc is the vehicle measurement data, which can include a vehicle driving force, M is a total vehicle mass, is a vehicle acceleration, p is an air density, C d is an air resistance coefficient, A is an automobile frontal area, v is an air flow relative speed, g is a gravitational acceleration, f0 is a rolling resistance coefficient, and a is a slope angle. The outlier detection equation is differentiated to obtain an outlier detection differential equation, wherein the expression of the outlier detection differential equation is: It can be understood that the slope change of the road is random, and the influence of the road slope on the acceleration during the vehicle driving process is small, so the result of the slope derivation of the acceleration is approximately zero, i.e. The change rate of the vehicle speed is much smaller than the change rate of the acceleration in the calculation period of the system model, so The outlier detection differential equation can be simplified to obtain an outlier detection simplified equation, wherein the expression of the outlier detection simplified equation is: The outlier detection simplified equation is converted to obtain target vehicle data, wherein the calculation formula of the target vehicle data is: In the formula, it can be understood that the target vehicle data can include a total vehicle mass.

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

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

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

[0167] According to the predicted error covariance matrix, a Kalman gain is calculated;

[0168] According to the predicted state quantity and the Kalman gain, a target estimated state quantity is calculated;

[0169] According to the predicted error covariance matrix and the Kalman gain, a target estimated error covariance is calculated;

[0170] The first road surface slope value is calculated based on the kinematic Kalman filter equation, the target estimation state variables, and the target estimation 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 first based on the state equation and observation equation of the kinematic system. The expression of the kinematic Kalman filter equation is as follows: In the formula, x k Let x be the state variable at time k. k-1 Let A be the state variable at time k-1, A be the process matrix, B be the control matrix, and u be the state variable. k-1 w is the control variable at time k-1. k-1 For process noise, z k Let H be the observations, and v be the observation matrix. k-1 This represents observation noise. It is understood that process noise and observation noise are independent and follow a normal distribution, i.e., 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, based on the target vehicle data, the predicted state variables and prediction error covariance matrices are calculated. The formula for calculating the predicted state variables is: In the formula, Let be the predicted state variable at time k. Let be the predicted state variable at time k-1. The formula for calculating the prediction error covariance matrix is: In the formula, Let P be the prediction error covariance matrix at time k. k-1 Let the target estimation error covariance be given at time k-1. Then, based on the prediction error covariance matrix, the Kalman gain is calculated, where the formula for calculating the Kalman gain is: Based on the predicted state variables and the Kalman gain, the target estimated state variables are calculated, where the formula for calculating the target estimated state variables is: In the formula, Let k be the target estimated state variables at time k. Then, based on the prediction error covariance matrix and the Kalman gain, calculate the target estimation error covariance, where the formula for calculating the target estimation error covariance is: In the formula, P kwherein I is an identity matrix. Finally, the first road slope value is calculated according to the kinematic Kalman filtering equation, the target estimation state quantity and the target estimation error covariance. It can be understood that the system input variable is missing based on the vehicle longitudinal kinematic model, and therefore the control matrix B = 0. The advantage is that the entire system model is less disturbed by the deviation of the external input quantity. The state matrix and the observation matrix of the system model are known, and after the initial value of the system state variable and the values of the process excitation noise covariance matrix and the observation noise covariance matrix are specified, the downhill slope value under the vehicle longitudinal kinematic model is estimated through continuous iteration and updating.

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

[0173] The least square method is converted to the vehicle longitudinal dynamics equation to obtain a least square method form model;

[0174] According to the least square method form model, a cost function is constructed;

[0175] The cost function is solved to obtain a cost function value;

[0176] According to the cost function value, an initial recursive least square equation set is constructed;

[0177] According to the forgetting factor and the initial recursive least square equation set, a target recursive least square equation set is constructed;

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

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

[0180] According to the target vehicle data and the recursive least square whole vehicle mass estimation model equation set, a whole vehicle mass estimation value is calculated;

[0181] According to the whole vehicle mass estimation value, the dynamics system state equation and the dynamics system observation equation, the second road slope value is calculated.

[0182] In some embodiments, the actual noise during sensor measurements in the system model is generally high, and direct estimation of the vehicle mass is significantly affected by acceleration and driving force noise, ultimately leading to low reliability of the estimated value. Therefore, a recursive least squares method with a forgetting factor can be used to estimate the vehicle mass; the main idea is to continuously correct previous parameter values ​​with newly acquired parameter values ​​to obtain the optimal parameter values. The vehicle longitudinal dynamics model can first be rearranged into a least squares form: y = h T θ, where y is the system output vector, h is the system input sample set, and θ is the set of parameters to be identified. We can let f0 = tanβ, where β is the equivalent slope, and perform a least squares transformation on the vehicle longitudinal dynamics equations to obtain the least squares form model. The expression of the least squares form model is: Understandably, corresponding to the least squares form, In standard least squares, the selection of unknown parameters in a linear model is to minimize the sum of squares of the differences between the observed and calculated values. For a linear system, a cost function can be constructed based on the least squares model, where the expression for the cost function is: In the formula, Let φ be the system observable, and let φ be the parameter estimate. Solve the cost function to obtain its value, where the expression for the cost function value is: Since the two parameters to be estimated are time-varying, iteratively updating the above equation with each new data acquisition will yield more efficient results. An initial system of recursive least squares equations can be constructed based on the cost function value. The expression for the initial system of recursive least squares equations is: In the formula, It is a parameter estimate calculated based on the data from the previous k observations, K k It is the gain vector, y k It is the system output, φ k For observable measurements of the system, P k It is the updated covariance matrix, whose initial value is obtained empirically, and I is the identity matrix.

[0183] In practical applications of the recursive least squares algorithm, as the recursion progresses, P... k It will decay rapidly, and K kThe new data will also decay rapidly, so that the correction effect of the new data on the parameter estimation is smaller and smaller until the effect is lost, and the main reason for this phenomenon is that the same trust degree is given to all data in the identification process; in order to solve this problem, the concept of forgetting factor is introduced, so that the correction effect of the new data obtained in the identification process on the parameter estimation will be stronger, and the correction effect of the old data on the parameter estimation will be weaker. According to the forgetting factor and the initial recursive least squares equation set, the target recursive least squares equation set can be constructed, wherein the expression of the target recursive least squares equation set is: In the formula, ω k is the forgetting factor at k moment, ω k Generally satisfies ω k ∈(0, 1), the larger the value is, the higher the identification accuracy is, but the convergence speed is slower, on the contrary, the identification accuracy is reduced, but the convergence speed will be faster. According to the vehicle longitudinal dynamics equation, the recursive least squares mass estimation model is constructed, wherein the expression of the recursive least squares mass estimation model is: Let represent the output of the system, represent the observation matrix of the system, and M is the parameter to be estimated. According to the target recursive least squares equation set and the recursive least squares mass estimation model, the recursive least squares vehicle mass estimation model equation set can be constructed, wherein the expression of the recursive least squares vehicle mass estimation model equation set is: In the formula, is the optimal estimation value of the vehicle mass at k moment, is the optimal estimation value of the vehicle mass at k-1 moment, y k is the output of the system at k moment, and E k is the observation matrix of the system at k moment. Through the recursive least squares vehicle mass estimation model equation set, the real-time value of the vehicle mass can be identified and estimated. Finally, according to the target vehicle data and the recursive least squares vehicle mass estimation model equation set, the vehicle mass estimation value is calculated, and according to the vehicle mass estimation value, the dynamics system state equation and the dynamics system observation equation, the second road slope value is calculated.

[0184] In some embodiments, in step S107, according to the first road slope value and the second road slope value, data fusion is performed by using the interactive multiple model method to obtain the full-condition road slope value, which can include but is not limited to the following steps:

[0185] Constructing system model state equation and system model measurement equation;

[0186] Setting transition probability matrix;

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

[0188] calculating a model mixing probability according to the model prediction probability;

[0189] calculating a model mixing state estimation according to the model mixing probability;

[0190] calculating a model mixing covariance estimation according to the model mixing state estimation;

[0191] calculating a state prediction value according to a system model state equation and a system model measurement equation;

[0192] calculating a prediction error covariance according to the state prediction value and the model mixing covariance estimation;

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

[0194] calculating a residual covariance matrix according to the residual;

[0195] constructing a likelihood function according to the prediction error covariance and a Gaussian distribution;

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

[0197] calculating a state estimation value according to the Kalman filter gain matrix;

[0198] calculating an interaction error covariance matrix according to the state estimation value;

[0199] calculating a motion state probability according to the interaction error covariance matrix;

[0200] calculating a total probability according to the motion state probability;

[0201] calculating a model probability according to the likelihood function, the total probability, and a model probability normalization factor;

[0202] constructing an interaction estimation state value equation and an interaction covariance equation according to the model probability;

[0203] calculating a total working condition road slope value according to the interaction estimation state value equation, the interaction covariance equation, a first road slope value, and a second road slope value.

[0204] In some embodiments, the slope estimation accuracy based on the vehicle dynamics model depends on the accuracy of the system model, and each parameter in the system model is greatly affected by high-frequency noise; the slope estimation based on the vehicle kinematics model has the disadvantage that the measurement values of the sensor are greatly affected by static bias, and therefore the application adopts an interactive multiple model algorithm with Markov switching coefficients, uses different motion models to match different motion states of the target respectively, multiple models work in parallel, and the target state estimation value and model probability are updated by using the Kalman filtering algorithm through the Markov chain. The adaptive model weight switching constructed by combining the Kalman filtering algorithm and the interactive multiple model algorithm can adaptively and in real time the vehicle state, automatically reduce the error factor interference in different running environments, and then select the optimal estimation model to improve the slope estimation accuracy. In the interactive multiple model method, it is assumed that the system satisfies the finite state Markov chain, switching can be performed between different mathematical models, and the interaction process between each sub-model is completed by re-inputting and initializing the sub-filter in the filtering recursion process.

[0205] The process of the interactive multiple model method is shown in Figure 6 Using the interactive multiple model filtering, the system model state equation and the system model measurement equation can be constructed first, wherein the expression of the system model state equation is: The expression of the system model measurement equation is: k = H k X k + ω k , wherein, represents the system state transition matrix of the effective mode m j at k-1 moment, represents the system noise excitation matrix of the effective mode m j at k moment, k represents the measurement matrix of the effective mode m j at k moment, v k-1 is the measurement noise, ω k is the process noise, the mean value of the measurement noise and the mean value of the process noise are both Gaussian white noise with a value of zero, Q k-1 is the variance matrix corresponding to the measurement noise, and R k is the variance matrix corresponding to the process noise. The interactive multiple model mainly includes four stages of input interaction, filtering update, probability update and output interaction, the transition probability between each model obeys the Markov process, and the transition probability matrix can be set, wherein the expression of the transition probability matrix is: , wherein, 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 produce the initial condition of the model input. The input interaction is actually a process of redistributing the estimates of each filter, in which the state estimate of the target at the last time (k-1 time) is used as the initial condition of the input interaction and the model probability of each sub-filter to obtain the mixed estimate state and the 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 of the prediction probability of model j is: The inter-model mixing probability is calculated according to the model prediction probability, where the calculation formula of the inter-model mixing probability from model i to model j is: where, is the probability of model i at k-1 time, p ij is the transition probability from model i to model j. The model mixed state estimate is calculated according to the inter-model mixing probability, where the calculation formula of the mixed state estimate of model j is: The model mixed covariance estimate is calculated according to the model mixed state estimate, where the calculation formula of the mixed covariance estimate of model j is: where r is the number of models, is the state update of model i at k-1, is the covariance matrix of model i at k-1.

[0206] In the filter update phase, the purpose of the input interaction is to initialize the filter input value at k time based on the estimates of each sub-filter at k-1 time, and the mixed estimate input information at k-1 time is obtained; the filter update is to use the input interaction value obtained by the input interaction, combine each sub-model and the measurement information, and use the Kalman filter to independently filter and update the state of each model at k time, and select a suitable filter according to the characteristics of each sub-model. The state prediction value can be calculated according to the system model state equation and the system model measurement equation, where the calculation formula of the state prediction value is: where, represents the Kalman filter estimate value at k-1 time. The prediction error covariance is calculated according to the state prediction value and the model mixed covariance estimate, where the calculation formula of the prediction error covariance is: where r k is the system mode variable, P k-1|k-1 represents the estimation error covariance matrix at k-1 time, is the variance matrix of the measurement noise in the system state equation at k-1 time, represents the prediction error value at time k-1.

[0207] Then, according to the system model measurement equation, the residual is calculated, wherein the calculation formula of the residual is: According to the residual, the residual covariance matrix is calculated, wherein the calculation formula of the residual covariance matrix is: In the formula, is the variance matrix corresponding to the process noise in the system state equation at time k. According to the prediction error covariance and the Gaussian distribution, the likelihood function is constructed, wherein r k The expression of the likelihood function corresponding to j is: Further, if the Gaussian distribution is satisfied, the likelihood function is represented as: According to the prediction error covariance and the residual covariance matrix, the Kalman filter gain matrix is calculated, wherein the calculation formula of the Kalman filter gain matrix is: According to the Kalman filter gain matrix, the state estimation value is calculated, wherein the calculation formula of the state estimation value obtained by the filter j at time k is: According to the state estimation value, the cross-error covariance matrix is calculated, wherein the calculation formula of the cross-error covariance matrix obtained by the filter j at time k is: In the formula, represents the state one-step prediction value corresponding to the filter j at time k, represents the state one-step covariance corresponding to the filter j at time k, is the corresponding filter gain matrix, is the corresponding innovation sequence, represents the state estimation value obtained by the filter j at time k, represents the cross-error covariance matrix obtained by the filter j at time k.

[0208] In the probability update stage, the model probability changes with the change of the target motion state, which represents the effective degree of each sub-model matching the current motion state, and the model probability can be calculated by the likelihood function and needs to be updated constantly; if the sub-model accurately represents the current motion law, the innovation sequence of the filter is the Gaussian white noise with the mean value of zero and the variance of The likelihood function corresponding to the model at time k is The motion state probability can be calculated according to the cross-error covariance matrix, wherein the calculation formula of the motion state probability of the model j matching the current motion state at time k is: Then, the total probability is calculated according to the motion state probability, and a calculation formula of the total probability is as follows: According to the likelihood function, the total probability and the model probability normalization factor, the model probability is calculated, and a calculation formula of the model probability is as follows: In the formula, c is a model probability normalization factor,

[0209] In the output interaction stage, the output interaction is a process of weighted output of the estimated values of the sub-filters. After the estimated values of each sub-model in the filter updating stage and the sub-model probabilities calculated in the model probability updating stage, the total interaction estimated state value is obtained by weighted summation and superposition and the interaction covariance P k According to the model probability, the interaction estimated state value equation and the interaction covariance equation can be constructed, and an expression of the interaction estimated state value equation is as follows: An expression of the interaction covariance equation is as follows: Finally, the total road slope value is calculated according to the interaction estimated state value equation, the interaction covariance equation, the first road slope value and the second road slope value. Exemplarily, the model correct probability is calculated by the interactive multiple model, as a weighted coefficient, and the model output state is weighted and calculated, the system state is estimated by effective weighted fusion, the matching probability of each sub-model is adjusted in real time, the filter updating and the model probability updating are performed, and the fusion output result always tracks the sub-model output with small error according to the model transition matrix, so that the total road slope value is obtained. The total road slope value is used to adjust the vehicle driving parameter, so that the vehicle can normally drive, and the safety of vehicle driving is improved.

[0210] In some embodiments, the overall process of slope estimation by the vehicle longitudinal kinematics model and the vehicle longitudinal dynamics model is as follows Figure 7As shown, the vehicle longitudinal kinematic model and the vehicle longitudinal dynamics model can be first constructed, then the state equation and the observation equation under the vehicle longitudinal kinematic model are established, and the state equation and the observation equation under the vehicle longitudinal dynamics model are established, then the measurement data is converted into a graphical method for abnormal point detection by using the box plot method, the abnormal data is removed, then the slope value under the kinematic model is estimated by using the Kalman filter, and the slope based on the vehicle kinematic model is estimated, and the total vehicle mass based on the vehicle dynamics model is identified by using the recursive least squares method with a forgetting factor, and then the real-time estimation of the road slope value is realized by continuously selecting and replacing the Kalman filter. Finally, the model output state is weighted and calculated based on the interactive multiple model, the matching probability of each sub-model is adjusted in real time, the system state is estimated by using effective weighted fusion, and the total estimated state value and covariance are obtained by weighted fusion according to the model transition matrix, so as to calculate the road slope value in all working conditions.

[0211] In some embodiments, due to the influence of high-frequency transient characteristics of the vehicle and driving conditions during driving in all working conditions, the residual value of the data collected by a single sensor is large, or the abnormal data is not removed by using a traditional single model, resulting in inaccurate estimation of the slope value. Based on this, the longitudinal kinematic model and the longitudinal dynamics model of the driving vehicle are used in the present embodiment to remove the abnormal data by using the box plot method; the road slope value is estimated by using the Kalman filter for the vehicle longitudinal kinematic model data after removing the abnormal data; the total vehicle mass is identified and estimated by using the recursive least squares method with a forgetting factor for the removed data, so as to further estimate the road slope value; then the interactive multiple model is combined to calculate the interactive mixed estimation of the Kalman filter corresponding to the two models by using the model probability and the model transition probability, and the new estimated value and the model probability are calculated according to the input mixed estimation and the measurement data, and the probability weighted sum of the state estimation values of all filters is taken as the final estimated value. The error of the system state estimation by using a single model is overcome, the robustness of the estimation of the road slope value in all working conditions is improved, and the real-time performance, the high precision and the strong robustness are particularly strong.

[0212] The beneficial effects of implementing the embodiments of the present application include that the embodiments of the present application first acquire vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model, then construct a kinematics system state equation and a kinematics system observation equation according to the vehicle longitudinal kinematics model, construct a dynamics system state equation and a dynamics system observation equation according to the vehicle longitudinal dynamics model, and perform abnormal point detection processing on the vehicle measurement data by using a box plot method to obtain target vehicle data, and then calculate a first road slope value under the vehicle longitudinal kinematics model by using a Kalman filtering method according to the target vehicle data, the kinematics system state equation and the kinematics system observation equation, calculate a 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, and finally perform data fusion by using an interactive multiple model method according to the first road slope value and the second road slope value to obtain a full-condition road slope value, so as to realize full-condition road slope estimation in combination with the Kalman filtering method and the recursive least square method, and thus improve the accuracy.

[0213] As shown in Figure 8 The embodiments of the present application also provide a full-condition road slope estimation device, which comprises:

[0214] A first module 801 is configured to acquire vehicle measurement data, a vehicle longitudinal kinematics model and a vehicle longitudinal dynamics model.

[0215] A second module 802 is configured to construct a kinematics system state equation and a kinematics system observation equation according to the vehicle longitudinal kinematics model.

[0216] A third module 803 is configured to construct a dynamics system state equation and a dynamics system observation equation according to the vehicle longitudinal dynamics model.

[0217] A fourth module 804 is configured to perform abnormal point detection processing on the vehicle measurement data by using a box plot method to obtain target vehicle data.

[0218] A fifth module 805 is configured to calculate a first road slope value under the vehicle longitudinal kinematics model by using a Kalman filtering method according to the target vehicle data, the kinematics system state equation and the kinematics system observation equation.

[0219] A sixth module 806 is configured to calculate a 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] A seventh module 807 is configured to perform data fusion by using an interactive multiple model method according to the first road slope value and the second road slope value to obtain a full-condition road slope value, and the full-condition road slope value is used to adjust vehicle driving parameters.

[0221] The contents in the method embodiments are applicable to the device embodiments, the device embodiments specifically implement the functions same as the method embodiments, and achieve the beneficial effects same as the method embodiments.

[0222] As shown in Figure 9 , the embodiment of the application further provides a computer device, comprising:

[0223] at least one processor 901;

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

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

[0226] The contents in the method embodiments are applicable to the device embodiments, the device embodiments specifically implement the functions same as the method embodiments, and achieve the beneficial effects same as the method embodiments.

[0227] The embodiment of the application further provides a computer readable storage medium, the computer readable storage medium stores a computer program, and the computer program is executed by a processor to implement the method shown in Figure 1 .

[0228] The contents in the method embodiments are applicable to the storage medium embodiments, the storage medium embodiments specifically implement the functions same as the method embodiments, and achieve the beneficial effects same as the method embodiments.

[0229] The preferred embodiments of the application are described above with reference to the drawings, and the scope of the right of the application is not limited by this. Any modification, equivalent replacement and improvement made by the person skilled in the art without departing from the scope and essence of the application should be within the scope of the right of the application.

Claims

1. A method for estimating road slope under all working conditions, characterized in that, Includes the following steps: Acquire vehicle measurement data, vehicle longitudinal kinematics model, and vehicle longitudinal dynamics model; Based on the vehicle's longitudinal kinematics model, the state equation and observation equation of the kinematic system are constructed. Based on the vehicle's longitudinal dynamics model, construct the state equations and observation equations of the dynamic system. An outlier detection process is performed on vehicle measurement data using a box plot method to obtain target vehicle data. This process includes: constructing a box plot, which includes the minimum value, lower quartile, median, upper quartile, and maximum value; constructing an outlier detection equation based on the vehicle measurement data and the box plot; performing differentiation 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; and transforming the simplified outlier detection equation to obtain the target vehicle data. Based on the target vehicle data, the state equation of the kinematic system, and the observation equation of the kinematic system, the first road surface slope value under the longitudinal kinematic model of the vehicle is calculated using the Kalman filter method. Based on the target vehicle data, the dynamic system state equation and the dynamic system observation equation, the second road surface slope value under the vehicle longitudinal dynamic model is calculated using the recursive least squares method. Based on the first and second pavement slope values, data fusion is performed using the interactive multi-model method to obtain the pavement slope values ​​under all working conditions. This includes: constructing the system model state equation and system model measurement equation; setting the transition probability matrix; calculating the model prediction probability based on the transition probability matrix; calculating the inter-model mixing probability based on the model prediction probability; calculating the model mixed state estimate based on the inter-model mixing probability; calculating the model mixed covariance estimate based on the model mixed state estimate; calculating the state prediction value based on the system model state equation and system model measurement equation; calculating the prediction error covariance based on the state prediction value and the model mixed covariance estimate; calculating the residual based on the system model measurement equation; and calculating the residual covariance matrix based on the residual. Based on the prediction error covariance and Gaussian distribution, a likelihood function is constructed. The Kalman filter gain matrix is ​​calculated using the prediction error covariance and residual covariance matrices. The state estimate is calculated using the Kalman filter gain matrix. The interaction error covariance matrix is ​​calculated using the state estimate. The motion state probability is calculated using the interaction error covariance matrix. The total probability is calculated using the motion state probability. The model probability is calculated using the likelihood function, total probability, and model probability normalization factor. The interaction estimation state value equation and interaction covariance equation are constructed using the model probability. The full-condition road surface slope value is calculated using the interaction estimation state value equation, interaction covariance equation, first road surface slope value, and second road surface slope value. 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 the kinematic system state equation and kinematic system observation equation based on the vehicle longitudinal kinematic model includes: Based on the vehicle's longitudinal kinematics model, construct the kinematic system state vector; Based on the acceleration sensor measurement equation, construct the slope value equation; Based on the kinematic system state vector and the slope equation, construct the kinematic system state equation; Perform a matrix transformation on the state equations of the kinematic system to obtain the kinematic matrix state equations; Based on the state equation of the kinematic matrix, the observation equation of the kinematic system is constructed.

3. The method according to claim 1, characterized in that, The process of constructing the state equations and observation equations of the dynamic system based on the vehicle's longitudinal dynamics model includes: Calculate the vehicle's driving force based on the engine's current output torque, the transmission's final drive ratio, the transmission's gear ratio, the transmission efficiency between the turbine and the drive wheel, and the tire radius. Calculate air resistance based on air density, drag coefficient, vehicle frontal area, and relative air velocity. Calculate the rolling resistance based on the vehicle mass, rolling resistance coefficient, gravitational acceleration, and slope angle; Calculate the slope resistance based on the vehicle mass, the gravitational acceleration, and the slope angle; Calculate the acceleration drag by converting the total vehicle mass, vehicle acceleration, and rotational mass inertial moment into constant coefficients of the translational mass inertial force; Based on the vehicle longitudinal dynamics model, the vehicle driving force, the air resistance, the rolling resistance, the slope resistance, and the acceleration resistance, the vehicle longitudinal dynamics equations are constructed. Taylor expansion of the vehicle longitudinal dynamics equations yields a discrete model of the vehicle longitudinal dynamics. Based on the vehicle longitudinal dynamics discrete model, the vehicle mass, and the slope angle, construct the dynamic system state vector; Based on the equivalent slope and the state vector of the dynamic system, construct the state-space equation of the dynamic system; The state vector of the dynamic system is simplified to obtain the simplified dynamic state vector; Based on the state-space equations of the dynamic system and the simplified state vectors of the dynamic system, construct the system state differential equations; The system state differential equations are discretized using the forward Euler method to obtain discretized difference equations. Based on the discretized difference equation, construct the dynamic discrete Kalman filter equation; Based on the discretized difference equation and the dynamic discrete Kalman filter equation, a discrete state equation is constructed. Based on the discrete state equations, construct the state equations of the dynamic system; Based on the discrete state equations and the observation matrix, the observation equations of the dynamic system are constructed.

4. The method according to claim 1, characterized in that, The step of calculating the first road surface slope value under the longitudinal kinematic model of the vehicle using the Kalman filter method based on the target vehicle data, the state equation of the kinematic system, and the observation equation of the kinematic system includes: Based on the state equation and observation equation of the kinematic system, construct the kinematic Kalman filter equation; Based on the target vehicle data, calculate the predicted state variables and the prediction error covariance matrix; Calculate the Kalman gain based on the prediction error covariance matrix; Calculate the target estimated state variables based on the predicted state variables and the Kalman gain; The target estimation error covariance is calculated based on the prediction error covariance matrix and the Kalman gain. The first road surface slope value is calculated based on the kinematic Kalman filter equation, the target estimated state variables, and the target estimated error covariance.

5. The method according to claim 3, characterized in that, The step of calculating the second road surface slope value under the longitudinal dynamics model of the vehicle using the recursive least squares method based on the target vehicle data, the state equation of the dynamic system, and the observation equation of the dynamic system includes: The longitudinal dynamic equations of the vehicle are transformed using the least squares method to obtain the least squares form model; Based on the least squares form model, construct the cost function; The cost function is solved to obtain the cost function value; Based on the cost function value, construct an initial system of recursive least squares equations; Based on the forgetting factor and the initial recursive least squares equation set, construct the target recursive least squares equation set; Based on the vehicle longitudinal dynamics equations, a recursive least squares mass estimation model is constructed. Based on the target recursive least squares equation set and the recursive least squares mass estimation model, construct the recursive least squares vehicle mass estimation model equation set; Based on the target vehicle data and the recursive least squares vehicle mass estimation model equations, calculate the estimated vehicle mass. The second road surface slope value is calculated based on the estimated vehicle mass, the dynamic system state equation, and the dynamic system observation equation.

6. A road slope estimation device under all working conditions, characterized in that, include: The first module is used to acquire vehicle measurement data, vehicle longitudinal kinematics model, and vehicle longitudinal dynamics model; The second module is used to construct the state equation and observation equation of the kinematic system based on the longitudinal kinematic model of the vehicle. The third module is used to construct the state equations and observation equations of the dynamic system based on the vehicle's longitudinal dynamics model. The fourth module is used to perform outlier detection processing on vehicle measurement data using the box plot method to obtain target vehicle data. This includes: constructing a box plot, which includes the minimum value, lower quartile, median, upper quartile, and maximum value; constructing an outlier detection equation based on the vehicle measurement data and the box plot; performing differential 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; and transforming the simplified outlier detection equation to obtain the target vehicle data. The fifth module is used to calculate the first road surface slope value under the longitudinal kinematic model of the vehicle based on the target vehicle data, the state equation of the kinematic system, and the observation equation of the kinematic system using the Kalman filter method. The sixth module is used to calculate the second road surface slope value under the longitudinal dynamics model of the vehicle based on the target vehicle data, the state equation of the dynamic system, and the observation equation of the dynamic system using the recursive least squares method. The seventh module is used to fuse data based on the first and second road surface slope values ​​using an interactive multi-model method to obtain the road surface slope values ​​under all working conditions. This includes: constructing the system model state equation and system model measurement equation; setting the transition probability matrix; calculating the model prediction probability based on the transition probability matrix; calculating the inter-model mixing probability based on the model prediction probability; calculating the model mixed state estimate based on the inter-model mixing probability; calculating the model mixed covariance estimate based on the model mixed state estimate; calculating the state prediction value based on the system model state equation and system model measurement equation; calculating the prediction error covariance based on the state prediction value and the model mixed covariance estimate; calculating the residual based on the system model measurement equation; and calculating the residual covariance moment based on the residual. The following steps are performed: First, construct the likelihood function based on the prediction error covariance and Gaussian distribution. Second, calculate the Kalman filter gain matrix based on the prediction error covariance and residual covariance matrices. Third, calculate the state estimate based on the Kalman filter gain matrix. Fourth, calculate the interaction error covariance matrix based on the state estimate. Fifth, calculate the motion state probability based on the interaction error covariance matrix. Sixth, calculate the total probability based on the motion state probability. Seventh, calculate the model probability based on the likelihood function, total probability, and model probability normalization factor. Eighth, construct the interaction estimation state value equation and interaction covariance equation based on the model probability. Finally, calculate the full-condition road surface slope value based on the interaction estimation state value equation, interaction covariance equation, first road surface slope value, and second road surface slope value. The full-condition road surface slope value is used to adjust vehicle driving parameters.

7. 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 as described in any one of claims 1-5.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-5.

Images