A semi-trailer state estimation method based on grey wolf optimization square root unscented Kalman filter

Abstract

The application relates to a semi-trailer state estimation method based on grey wolf optimization square root unscented Kalman filtering, which is applied to a semi-trailer vehicle-mounted controller and comprises the following steps: S1, acquiring running signals such as front wheel rotation angle, vehicle speed, yaw angular velocity and lateral acceleration through a vehicle-mounted sensor and a CAN bus; S2, pre-processing the running signals, and combining a semi-trailer dynamics model to construct a state estimation equation; S3, performing state prediction and measurement updating by adopting square root unscented Kalman filtering; S4, adaptively optimizing a process noise covariance matrix and a measurement noise covariance matrix by using a grey wolf optimization algorithm; and S5, outputting semi-trailer lateral speed, yaw angular velocity, hinged angle and hinged angular velocity estimation results and sending the estimation results to a stability control system. The method can improve the accuracy and robustness of semi-trailer state estimation under complex working conditions.

Description

Technical Field

[0001] This application relates to the field of vehicle state estimation and semi-trailer stability control technology, specifically to a semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filtering. Background Technology

[0002] In recent years, semi-trailers have become the main type of vehicle for road transportation, characterized by their high load capacity, high center of gravity, large body size, and the articulated connection between the tractor and trailer via a saddle. Compared to ordinary single-unit vehicles, semi-trailers are typical articulated multi-unit vehicles, with a strong coupling relationship between their yaw, lateral, and articulated movements. Under conditions such as high-speed steering, emergency obstacle avoidance, low-traction road surfaces, or load changes, they are more prone to instability phenomena such as sideslip, folding, and fishtailing.

[0003] In semi-trailer stability control, state variables such as tractor lateral velocity, yaw rate, articulation angle, and articulation velocity are crucial for the controller to determine the vehicle's stability. While yaw rate and lateral acceleration can be measured by inertial sensors, and front wheel steering angle, wheel speed, braking force, or driving force can be obtained through onboard sensors and the CAN bus, the tractor lateral velocity, articulation angle, and articulation velocity are often difficult to measure directly and accurately due to limitations imposed by sensor cost, installation location, measurement noise, and the operating environment. Therefore, joint real-time estimation of multiple key state variables of the semi-trailer using onboard state estimation algorithms is a prerequisite for the reliable operation of the semi-trailer stability control system.

[0004] Among existing vehicle state estimation algorithms, unscented Kalman filtering is suitable for nonlinear systems and avoids the complex Jacobian matrix calculations found in extended Kalman filtering. However, traditional unscented Kalman filtering typically uses fixed process noise covariance matrices and measurement noise covariance matrices in actual vehicle operation, making it difficult to adapt to noise variations caused by different vehicle speeds, steering inputs, load states, and road conditions. Furthermore, the accumulation of noise and numerical errors during filtering iterations can disrupt the symmetry and positive definiteness of the covariance matrix, leading to reduced estimation accuracy or even filter divergence.

[0005] Therefore, it is necessary to provide a semi-trailer state estimation method that can be implemented in an on-board controller, enabling the construction of state estimation equations based on on-board measurable signals, improving numerical stability through square root unscented Kalman filtering, and adaptively adjusting the noise covariance matrix through the Grey Wolf optimization algorithm, thereby improving the accuracy and robustness of semi-trailer state estimation under complex working conditions. Summary of the Invention

[0006] The purpose of this application is to provide a semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filtering, which solves the problems of fixed noise covariance matrix, insufficient adaptability to complex working conditions, and unstable estimation results caused by non-positive definite covariance matrix in traditional unscented Kalman filtering for semi-trailer state estimation.

[0007] To achieve the above objectives, this application provides a semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filtering. The method is applied to a semi-trailer onboard controller and specifically includes:

[0008] The vehicle acquires operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration through onboard sensors and the CAN bus;

[0009] The operating signals are preprocessed, and state estimation equations are constructed in conjunction with the semi-trailer dynamics model;

[0010] A square root unscented Kalman filter is used for state prediction and measurement update.

[0011] The gray wolf optimization algorithm is used to adaptively optimize the process noise covariance matrix and the measurement noise covariance matrix;

[0012] The system outputs the estimated results of the lateral speed, yaw rate, articulation angle, and articulation rate of the tractor vehicle and sends them to the stability control system.

[0013] Furthermore, in one embodiment, the vehicle acquires operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration via onboard sensors and the CAN bus, including:

[0014] The front wheel steering angle is obtained through a steering angle sensor, the vehicle speed and wheel speed are obtained through a wheel speed sensor or vehicle controller, and the yaw rate and lateral acceleration are obtained through an inertial measurement unit.

[0015] The system reads tire force, braking force, driving force, and vehicle operating status signals output by the braking system, electric drive system, or chassis domain controller via the CAN bus.

[0016] The acquired vehicle operation signals are used as input signals for estimating the on-board state of the semi-trailer.

[0017] Furthermore, in one embodiment, the preprocessing of the operating signal and the construction of the state estimation equation in conjunction with the semi-trailer dynamics model include:

[0018] The operating signals are time-synchronized, filtered, and their validity is judged to eliminate abnormal or failed signals.

[0019] A nonlinear dynamic model of a semi-trailer is established based on the lateral motion, yaw motion, and articulation motion relationships between the tractor and trailer.

[0020] By combining the tire lateral force model with the vehicle operation signal input into the semi-trailer nonlinear dynamics model, a state-space equation for on-board estimation is constructed.

[0021] Furthermore, in one embodiment, the state prediction and measurement update using square root unscented Kalman filtering includes:

[0022] Initialize the state estimate, process noise covariance matrix, measurement noise covariance matrix, and square root factor of the state error covariance matrix;

[0023] The Sigma point is selected based on the square root factor of the state error covariance matrix, and the state is predicted according to the semi-trailer state estimation equation.

[0024] Calculate the predicted observations based on the predicted state quantities, update the measurements by combining them with the actual observations, and update the square root factor of the state error covariance matrix.

[0025] Furthermore, in one embodiment, the adaptive optimization of the process noise covariance matrix and the measurement noise covariance matrix using the gray wolf optimization algorithm includes:

[0026] The parameters in the process noise covariance matrix and the measurement noise covariance matrix are used as the position variables of individual gray wolves;

[0027] The fitness function is constructed based on the innovation sequence of the square root unscented Kalman filter, and the fitness value of each gray wolf in the gray wolf population is calculated.

[0028] Determined based on fitness value Wolf, wolves and wolves, and based on Wolf, wolves and The wolf's position is updated to reflect the positions of other individual gray wolves, and the optimized process noise covariance matrix and measurement noise covariance matrix parameters are obtained.

[0029] Furthermore, in one embodiment, the output of the tractor's lateral speed, yaw rate, articulation angle, and estimated articulation rate results, and the transmission of these to the stability control system, includes:

[0030] The optimized process noise covariance matrix and measurement noise covariance matrix are fed back to the square root unscented Kalman filter;

[0031] The lateral velocity, yaw rate, articulation angle, and articulation rate estimation results of the tractor are output in real time by the square root unscented Kalman filter.

[0032] The estimation results are sent to the semi-trailer stability control system via the CAN bus or the internal communication interface of the on-board controller.

[0033] The beneficial effects of the technical solutions provided in this application include:

[0034] This application uses semi-trailer-mounted sensors and CAN bus signals as the starting point of its method. It incorporates real-vehicle-obtainable signals such as front wheel steering angle, vehicle speed, yaw rate, lateral acceleration, braking force, or tire force into the vehicle state estimation process, enabling the method to be deployed in the semi-trailer's onboard controller. Compared to performing state estimation only in an offline simulation environment, this application better aligns with the information acquisition methods of real-vehicle control systems, providing real-time, continuous, and readily available state inputs for subsequent stability control.

[0035] This application synchronizes, filters, and determines the validity of vehicle operation signals, and constructs state estimation equations based on a semi-trailer dynamics model. It establishes a correspondence between measurable vehicle operation signals and key state variables that are difficult to measure directly, such as the lateral velocity, yaw rate, articulation angle, and articulation velocity of the tractor unit. Therefore, without adding expensive dedicated measuring devices, it can achieve joint estimation of multiple key state variables of the semi-trailer, reducing sensor deployment costs and engineering implementation difficulties.

[0036] This application employs square-root unscented Kalman filtering for state prediction and measurement updates, using the square root factor of the state error covariance matrix instead of the covariance matrix in the filtering recursion. This approach reduces the impact of matrix square root, inversion, and numerical accumulation errors on the filtering process, improving the problem of reduced state estimation accuracy or even filter divergence due to non-positive definiteness of the covariance matrix, thereby enhancing the stability and robustness of semi-trailer state estimation under complex driving conditions.

[0037] This application utilizes the Grey Wolf optimization algorithm to adaptively optimize the process noise covariance matrix and the measurement noise covariance matrix. A fitness function is constructed based on the filtered innovation sequence, and the noise covariance parameters are dynamically updated during the iteration process. This avoids the problem in traditional unscented Kalman filtering where the fixed values ​​of the Q and R matrices are difficult to adapt to changes in operating conditions, reduces the impact of noise uncertainty and parameter sensitivity on the estimation results, and improves the estimation accuracy of state variables such as hinge angle, hinge angular velocity, and yaw rate.

[0038] This application sends the optimized state estimation results to the semi-trailer stability control system, enabling the system to make yaw stability control, articulation stability control, or lateral stability control decisions based on more accurate vehicle state information. For articulated multi-body vehicles like semi-trailers, accurately obtaining the articulation angle and angular velocity helps to identify risks of folding, fishtailing, and yaw instability in advance, thereby improving vehicle driving safety.

[0039] In summary, this application, based on onboard acquireable signals, integrates a semi-trailer dynamics model, square root unscented Kalman filtering, and the gray wolf optimization algorithm to realize a key state estimation method for semi-trailers deployed in real vehicles. This method balances engineering feasibility, estimation accuracy, and adaptability to complex operating conditions, and can provide reliable state information for semi-trailer stability control systems. Attached Figure Description

[0040] Figure 1 This is a flowchart of the semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filtering, as described in an embodiment of this application.

[0041] Figure 2 A simplified kinematic model diagram of the semi-trailer used in this application.

[0042] Figure 3 This is the location update graph for implementing the Grey Wolf algorithm in this application.

[0043] Figure 4 This is a block diagram of the gray wolf optimized square root unscented Kalman filter algorithm in an embodiment of this application.

[0044] Figure 5 This is a block diagram of the overall process of semi-trailer state estimation in an embodiment of this application. Detailed Implementation

[0045] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present application.

[0046] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0047] In one aspect, embodiments of this application provide a method for estimating the state of a semi-trailer based on gray wolf optimized square root unscented Kalman filtering.

[0048] In one embodiment, see Figure 1 As shown, the above method includes:

[0049] S1. The vehicle obtains operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration through on-board sensors and the CAN bus;

[0050] S2. Preprocess the operating signals and construct the state estimation equations in conjunction with the semi-trailer dynamics model;

[0051] S3. Use square root unscented Kalman filtering for state prediction and measurement update;

[0052] S4. Adaptively optimize the process noise covariance matrix and measurement noise covariance matrix using the Grey Wolf optimization algorithm;

[0053] S5 outputs the estimated results of the lateral speed, yaw rate, articulation angle, and articulation rate of the tractor vehicle, and sends them to the stability control system.

[0054] This application embodiment acquires vehicle operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration through onboard sensors and the CAN bus, and constructs state estimation equations in conjunction with a semi-trailer dynamics model. This enables the onboard controller to achieve joint estimation of multiple state variables when key state variables such as articulation angle, articulation rate, and tractor lateral speed are difficult to measure directly. By employing a square root unscented Kalman filter, with the square root factor of the state error covariance matrix participating in the filter recursion, the problem of reduced estimation accuracy or even filter divergence caused by the non-positive definite covariance matrix in traditional unscented Kalman filtering during iteration can be effectively improved, thus enhancing the numerical stability of the state estimation process. Furthermore, the Grey Wolf optimization algorithm is used to adaptively optimize the process noise covariance matrix and the measurement noise covariance matrix, allowing the filter parameters to be dynamically adjusted according to changes in vehicle operating status and complex working conditions, compensating for the insufficient adaptability of a fixed noise covariance matrix. This method can improve the accuracy and robustness of estimating key states such as lateral velocity, yaw rate, articulation angle, and articulation velocity of semi-trailers, providing more reliable state information for the semi-trailer stability control system, thereby improving the driving stability and safety of semi-trailers.

[0055] Furthermore, in one embodiment, step S1 above includes:

[0056] S101. The vehicle obtains operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration through on-board sensors and the CAN bus.

[0057] The front wheel steering angle can be obtained by a steering angle sensor, while vehicle speed and wheel speed can be obtained by wheel speed sensors or the vehicle controller. Yaw rate and lateral acceleration can be obtained by an inertial measurement unit. Parameters such as road gradient and road surface friction coefficient can be estimated using multiple sensors, including accelerometers, wheel speed sensors, and gyroscopes, in conjunction with a global navigation system and high-precision maps.

[0058] S102. Read tire force, braking force, driving force, and vehicle operating status signals output by the braking system, electric drive system, or chassis domain controller via the CAN bus. For semi-trailers equipped with electric drive axles or electronic braking systems, brake pressure, drive motor torque, estimated tire force, and vehicle operating status signals can also be obtained via the CAN bus.

[0059] S103. The acquired vehicle operating signal is used as a measurable input to the semi-trailer state estimation method for subsequent semi-trailer dynamics model calculation and filter measurement update. In this embodiment, the operating signal is not limited to the direct output of a single sensor, but can also be obtained by fusing multiple on-board sensors.

[0060] Furthermore, in one embodiment, step S2 above includes:

[0061] S201. Perform time synchronization, filtering, and validity assessment on the collected vehicle operation signals. For signals with abnormal jumps, frame drops, or exceeding the physical range, methods such as maintaining the previous valid value, amplitude limiting, or re-initialization can be used to improve the reliability of vehicle status estimation.

[0062] S202. Construct state estimation equations based on the semi-trailer dynamics model. The semi-trailer can be simplified into a three-axle articulated vehicle model with two axles for the tractor and one axle for the trailer. The specific simplified kinematic model is as follows: Figure 2 As shown.

[0063] The dynamic equations for the semi-trailer are as follows:

[0064]

[0065]

[0066]

[0067]

[0068] The articulated motion relationship between the tractor and the trailer can be expressed as:

[0069]

[0070]

[0071]

[0072] In the formula, For the quality of the tractor, For trailer quality, This is the distance from the center of gravity of the tractor to the front axle; This is the distance from the center of gravity of the tractor to the rear axle; This is the distance from the trailer's center of gravity to the articulation point. This is the distance from the trailer's center of gravity to the trailer axle. This is the distance from the center of gravity of the tractor vehicle to the articulation point.

[0073] The longitudinal speed of the tractor; The lateral speed of the tractor; The longitudinal speed of the trailer; The lateral speed of the trailer; The yaw rate of the tractor unit; The yaw rate of the tractor unit; This refers to the longitudinal force on the front axle tires of the tractor. The lateral force on the front axle tires of the tractor unit; The lateral force on the rear axle tires of the tractor unit; The lateral force on the trailer axle tires; Force applied at the hinge point; The moment of inertia of the tractor unit; The moment of inertia of the trailer; It is the hinge angle.

[0074] S203. Combining the tire lateral force model, the vehicle operating signal is input into the semi-trailer nonlinear dynamics model. In one embodiment, the tire lateral force can be calculated using the Dugoff tire model, and its longitudinal and lateral forces are expressed as follows:

[0075]

[0076] In the formula, Represents the longitudinal slip ratio; , For the longitudinal stiffness and lateral stiffness of the tire; Represents the tire slip angle; For vertical loads; These are nonlinear parameters relating to slip ratio and sideslip angle; Let be the road surface adhesion coefficient, then The function expression is as follows:

[0077]

[0078] in, The expression is as follows:

[0079]

[0080] S204. Based on the above dynamic model and tire model, construct the state-space equations for vehicle estimation.

[0081] Based on the above semi-trailer dynamics model, the state-space equation can be obtained as follows:

[0082]

[0083] State vector: Observation vector:

[0084] in, and These are process noise and observation noise, respectively. , , These represent the lateral forces of the tires on each axle; The turning angle of the front wheels of the tractor; It is lateral acceleration; This refers to the yaw rate; Represents state variables; This represents a control variable.

[0085] The above state estimation equations can be used to correlate on-board measurable signals with key state variables that are difficult to measure directly on a semi-trailer.

[0086] Furthermore, in one embodiment, step S3 above includes:

[0087] S301, Estimated initialization state value of the vehicle controller Process noise covariance matrix Measurement noise covariance matrix and the square root factor of the state error covariance matrix ;in:

[0088]

[0089] In the formula, Decomposed for Jolisky, It is the initial square root factor of the covariance and is a lower triangular matrix.

[0090] S302. Select the Sigma point based on the square root factor of the state error covariance matrix, and predict the state of the Sigma point according to the semi-trailer state estimation equation.

[0091] The method for selecting the Sigma point is as follows:

[0092]

[0093] The weights corresponding to each sampling point are:

[0094]

[0095] In the formula: For the weight parameters, to ensure the positive semidefiniteness of the variance matrix, they are usually taken as... =0; This indicates the degree to which the Sigma point deviates from the expected value; generally, a very small value is chosen. This indicates the distribution; for a Gaussian distribution, 2 is usually the optimal value. The dimension of the state variables; This represents the weight of the i-th Sigma point when calculating the mean of the predicted state; This indicates that the weights of the i-th Sigma point are used to calculate the covariance matrix.

[0096] The state prediction process can be represented as:

[0097]

[0098]

[0099] S303. Calculate the predicted observations based on the predicted state quantities, and complete the measurement update and square root factor update in conjunction with the actual observations.

[0100] The specific calculation methods for the observations and the square root factor are as follows:

[0101] Substituting the predicted Sigma points into the observation equation, we obtain the predicted observation values ​​corresponding to each Sigma point. The predicted observation mean was obtained by weighting the values. .

[0102]

[0103]

[0104] Calculate the square root factor of the prediction observation error covariance matrix based on the predicted observation values, the predicted observation mean, the covariance weights, and the measurement noise covariance matrix:

[0105]

[0106] In the formula, Let be the predicted observation value corresponding to the i-th Sigma point; To predict the observed mean; The mean weight is the value corresponding to the i-th Sigma point; R represents the covariance weights corresponding to the i-th Sigma point; R is the observation noise covariance matrix. The square root factor of the observation error covariance matrix; Decompose QR; This is the function to update the Cholesky factor.

[0107] The measurement equation update process is as follows:

[0108] 1) Calculate the cross-covariance matrix between the state variables and the observations based on the predicted state Sigma point, the predicted state mean, the predicted observations, and the predicted observation mean:

[0109]

[0110] In the formula, Let be the predicted state value corresponding to the i-th Sigma point; To predict the mean of the state; The cross-covariance matrix between state variables and observations;

[0111] 2) Calculate the Kalman gain matrix based on the cross-covariance matrix and the square root factor of the prediction observation error covariance matrix:

[0112]

[0113] In the formula, This is the Kalman gain matrix.

[0114] The specific steps for updating the state estimate and the square root covariance factor are as follows:

[0115] 1) Construct an innovation sequence based on the difference between the actual observations and the predicted observation mean, and use the Kalman gain matrix to correct the predicted state mean, thus obtaining the updated state estimate:

[0116]

[0117] 2) Construct the square root covariance correction term based on the Kalman gain matrix and the square root factor of the prediction observation error covariance matrix:

[0118]

[0119] 3) Obtain the square root factor of the updated state error covariance matrix using the Cholesky factor update function:

[0120]

[0121] In the above formula, The actual observations of the sensor at time k; For the new information sequence; The observed updated state estimate; M is the square root covariance correction term; The square root factor of the predicted state error covariance matrix; This is the square root factor of the updated state error covariance matrix, used for generating and updating the Sigma point at the next time step.

[0122] This application uses the square root factor of the state error covariance matrix to replace the covariance matrix in the filtering recursion, and uses the square root factor update method to update the square root factor of the state error covariance matrix. This can reduce the impact of the matrix square root sum and inversion process on numerical stability and reduce the risk of reduced estimation accuracy or filtering divergence caused by the non-positive definiteness of the covariance matrix.

[0123] Furthermore, in one embodiment, the adaptive optimization of the process noise covariance matrix and the measurement noise covariance matrix using the gray wolf optimization algorithm in step S4 above includes:

[0124] The S401 Gray Wolf algorithm optimizes parameters by simulating two core stages: the gray wolf's encirclement and pursuit of prey and the attack on prey. First, during the encirclement and pursuit stage, the positional difference between the gray wolf and the prey (the optimal solution) is calculated using a distance formula, and then... wolves follow , , The wolf updates its search direction based on its position. During the attack phase, it narrows the search range by using random coefficients to approach the optimal solution. The following is a mathematical modeling method for the encirclement and pursuit phase.

[0125] 1) Encirclement and pursuit phase

[0126] When hunting, gray wolves first form an encirclement and gradually approach. The mathematical model for this is:

[0127]

[0128] in, Indicates the distance between the gray wolf and its prey. This represents the position of the prey after the t-th iteration, i.e., the position of the optimal solution for the objective. Let C and A represent the position of the gray wolf after the t-th iteration, and let A be the coefficient vectors. The calculation formula is:

[0129]

[0130] Among them, convergence factor During the iteration process, the number of iterations decreases linearly with the increase of the iteration number t, and the range is [0,2]. and Let T be a random variable in the interval [0,1]; T is the maximum number of iterations.

[0131] 2) Hunting Phase

[0132] The second stage of the gray wolf's hunt is the attack hunt, which begins with... , , The wolves locate their prey and coordinate their hunt. At the end of the iteration, at this point... , , The wolf's position is considered one of the three optimal solutions. Simultaneously, calculate... wolves and , , The distance of the wolf:

[0133]

[0134] in, , , From respectively Wolf Pointing , , The vector of the wolf , , represent , , The wolf's current location This represents the position of the current solution.

[0135] During the algorithm iteration process, the ideal solution, i.e., the actual location of the prey, is unknown. Wolf position update passed , , The wolf guides the way; the specific formula is as follows:

[0136]

[0137]

[0138] in, , , Represent , , The wolf's updated location The position represents the updated optimal solution, and the algorithm proceeds to the next iteration using the Grey Wolf algorithm. The position update graph is shown below. Figure 3 As shown.

[0139] S402. Specific implementation steps for adaptive optimization of process noise covariance matrix and measurement noise covariance matrix based on Grey Wolf Optimization Algorithm:

[0140] 1) Parameter settings. Set the gray wolf population size to 50 and initialize the population position; in addition, according to the above state equation, Q is a 4-dimensional matrix and R is a 2-dimensional matrix, so set the variable dimension to 6; the maximum number of iterations is 200.

[0141] 2) Determine the fitness function. The following fitness function is established based on the mean square error of the state information:

[0142]

[0143] in, The fitness function; The actual observations at time k are represented; the fitness function is constructed using the new information sequence, and the fitness function is larger when the estimation error is smaller.

[0144] 3) Calculate the fitness value of all gray wolves in the population. Sort them from largest to smallest, and find the three largest values ​​as the fitness values. , , The fitness value of the wolf is recorded, along with its location information.

[0145] 4) Update location. Based on... , , Wolf's current location information is updated and iterated. , , The population's location information is updated by taking its mean as the new location and replacing part of the original population location with its mean.

[0146] 5) Calculate the population fitness value after updating the position. Find the three largest values ​​in the new round and update them. , , The wolf's fitness value and location information.

[0147] Through the above process, the Grey Wolf optimization algorithm obtains the optimized sum parameters and feeds them back to the square root unscented Kalman filter to achieve adaptive dynamic optimization of the process noise covariance matrix and the measurement noise covariance matrix.

[0148] Furthermore, in one embodiment, the step S5 above, which outputs the lateral velocity, yaw rate, articulation angle, and estimated articulation rate of the tractor vehicle and sends them to the stability control system, includes:

[0149] S501. The optimized process noise covariance matrix and measurement noise covariance matrix are fed back to the square root unscented Kalman filter, enabling the filter to dynamically adjust the noise parameters according to different operating conditions.

[0150] After receiving the sum parameters from the Grey Wolf Optimization Algorithm, the S502 and square root unscented Kalman filters output the estimated results of the tractor's lateral speed, yaw rate, articulation angle, and articulation rate in real time.

[0151] S503. The state estimation results are sent to the semi-trailer stability control system via the CAN bus or the internal communication interface of the on-board controller. The stability control system can determine the vehicle's yaw stability, articulation stability, and lateral stability based on the state estimation results, and further use them for active safety control functions such as differential braking, electric drive axle drive force distribution, yaw moment control, or anti-folding control.

[0152] This application combines onboard sensor signals from the semi-trailer with CAN bus information, establishes state estimation equations using a semi-trailer dynamics model, and integrates square-root unscented Kalman filtering with the Grey Wolf optimization algorithm to achieve real-time estimation of key state variables such as the tractor's lateral velocity, yaw rate, articulation angle, and articulation velocity. This method improves upon the problems of fixed noise parameters and easily non-positive definite covariance matrix in traditional unscented Kalman filtering, enhancing the accuracy and robustness of state estimation under complex operating conditions. It provides reliable state information for semi-trailer stability control, further ensuring vehicle driving stability and safety.

[0153] It should be noted that the sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0154] The terms "comprising" and "having," and any variations thereof, in the specification, claims, and accompanying drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such process, method, product, or apparatus. The terms "first," "second," and "third," etc., are used to distinguish different objects, etc., and do not indicate a sequence, nor do they limit "first," "second," and "third" to different types.

[0155] In the description of the embodiments of this application, terms such as "exemplary," "for example," or "for instance" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as "exemplary," "for example," or "for instance" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of terms such as "exemplary," "for example," or "for instance" is intended to present the relevant concepts in a concrete manner.

[0156] In the description of the embodiments of this application, unless otherwise stated, " / " means "or". For example, S / B can mean S or B. The "and / or" in the text is merely a description of the relationship between related objects, indicating that there can be three relationships. For example, S and / or B can mean: S exists alone, S and B exist simultaneously, and B exists alone. In addition, in the description of the embodiments of this application, "multiple" means two or more.

[0157] In some processes described in the embodiments of this application, multiple operations or steps are included in a specific order. However, it should be understood that these operations or steps may not be executed in the order they appear in the embodiments of this application, or they may be executed in parallel. The sequence number of the operation is only used to distinguish different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed sequentially or in parallel, and these operations or steps may be combined.

[0158] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method.

[0159] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A method for semi-trailer state estimation based on gray wolf optimized square root unscented Kalman filter, characterized in that, The method is applied to a semi-trailer on-board controller, and the method includes: S1. The vehicle obtains operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration through on-board sensors and the CAN bus; S2. Preprocess the operating signals and construct the state estimation equations in conjunction with the semi-trailer dynamics model; S3. Use square root unscented Kalman filtering for state prediction and measurement update; S4. Adaptively optimize the process noise covariance matrix and measurement noise covariance matrix using the Grey Wolf optimization algorithm; S5 outputs the estimated results of the lateral speed, yaw rate, articulation angle, and articulation rate of the tractor vehicle, and sends them to the stability control system.

2. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 1, characterized in that, The vehicle acquires operating signals such as front wheel steering angle, vehicle speed, yaw rate, and lateral acceleration via onboard sensors and the CAN bus, including: 1) The front wheel steering angle is obtained through a steering angle sensor, and the vehicle speed and wheel speed are obtained through a wheel speed sensor or the vehicle controller; 2) Obtain yaw rate and lateral acceleration through an inertial measurement unit; 3) Read the tire force, braking force, driving force and vehicle operating status signals output by the braking system, electric drive system or chassis controller via CAN bus.

3. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 1, characterized in that, The preprocessing of the operating signals and the construction of state estimation equations based on the semi-trailer dynamics model include: 1) Perform time synchronization, filtering, and validity assessment on the operating signals, and eliminate abnormal or failed signals; 2) Based on the lateral motion relationship, yaw motion relationship, and articulated motion relationship between the tractor and the trailer, a nonlinear dynamic model of the semi-trailer is established; 3) Combining the tire lateral force model, the vehicle operation signal is input into the semi-trailer nonlinear dynamics model to construct the state-space equation for on-board estimation.

4. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 3, characterized in that, In the state-space equation, the state variables to be estimated include at least the lateral velocity of the tractor, the yaw rate of the tractor, the articulation angle and the articulation rate, the observations include at least the yaw rate and the lateral acceleration, and the inputs include at least the front wheel steering angle and the tire force or braking force.

5. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 1, characterized in that, The state prediction and measurement update using square root unscented Kalman filtering includes: 1) Initialize the state estimate, process noise covariance matrix, measurement noise covariance matrix, and square root factor of the state error covariance matrix; 2) Select the Sigma point based on the square root factor of the state error covariance matrix, and predict the state of the Sigma point according to the semi-trailer state estimation equation; 3) Calculate the predicted observations based on the predicted state quantities, and update the measurements by combining them with the actual observations; 4) Update the square root factor of the state error covariance matrix using the square root factor update method.

6. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 5, characterized in that, The square root unscented Kalman filter replaces the state error covariance matrix with the square root factor of the state error covariance matrix in the filter recursion, thereby reducing the risk of reduced state estimation accuracy or filter divergence when the covariance matrix is ​​not in positive chronology.

7. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 1, characterized in that, The adaptive optimization of the process noise covariance matrix and measurement noise covariance matrix using the Grey Wolf optimization algorithm includes: 1) Use the parameters in the process noise covariance matrix and the measurement noise covariance matrix as the position variables of individual gray wolves; 2) Construct the fitness function based on the innovation sequence of the square root unscented Kalman filter; 3) Calculate the fitness value of each individual gray wolf in the gray wolf population, and determine the appropriate criteria based on the fitness value. Wolf, wolves and Wolf; 4) Based on Wolf, wolves and The wolf's position is updated to reflect the positions of other individual gray wolves, and the optimized process noise covariance matrix and measurement noise covariance matrix parameters are obtained.

8. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 7, characterized in that, The fitness function is constructed from the filtered innovation sequence. When the estimation error decreases, the fitness function value increases. The Grey Wolf optimization algorithm dynamically optimizes the process noise covariance matrix and the measurement noise covariance matrix based on the fitness function value.

9. The semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filter as described in claim 1, characterized in that, The output of the tractor's lateral speed, yaw rate, articulation angle, and estimated articulation rate results is sent to the stability control system, including: 1) Feed the optimized process noise covariance matrix and measurement noise covariance matrix back to the square root unscented Kalman filter; 2) The lateral velocity, yaw rate, articulation angle, and articulation rate estimation results of the tractor are output in real time by the square root unscented Kalman filter; 3) The estimation results are sent to the semi-trailer stability control system via the CAN bus or the internal communication interface of the vehicle controller.

10. A semi-trailer state estimation system, characterized in that, The system is used to execute the semi-trailer state estimation method based on gray wolf optimized square root unscented Kalman filtering as described in any one of claims 1-9, and the system comprises: 1) Signal acquisition module, used to acquire semi-trailer running signals through on-board sensors and CAN bus; 2) Signal preprocessing module, used to synchronize, filter, and determine the validity of the running signal; 3) State equation construction module, used to construct state estimation equations in conjunction with the semi-trailer dynamics model; 4) Square root unscented Kalman filter module, used for state prediction and measurement update; 5) The Grey Wolf optimization module is used for adaptive optimization of the process noise covariance matrix and the measurement noise covariance matrix; 6) Status output module, used to output the critical status estimation results of the semi-trailer and send them to the stability control system.

Images