Semitrailer trajectory tracking model predictive control method and system based on articulated angle hybrid state estimation and steering delay compensation

Abstract

This invention provides a model predictive control method and system for semi-trailer trajectory tracking based on articulation angle hybrid state estimation and steering delay compensation. Applied to the field of intelligent vehicle control, the method includes: constructing a kinematic discrete model and a dynamic discrete model of the semi-trailer; acquiring historical time-series data collected by the semi-trailer's sensors and calculating virtual measured values ​​of the articulation angle; constructing a state estimation vector based on the vehicle state and inputting it into the dynamic discrete model to calculate the next state prediction value; combining the measured values ​​collected by the semi-trailer's sensors and the virtual measured values ​​of the articulation angle through filtering and fusion to obtain the optimal estimated state of the current articulation angle; obtaining the total delay time of the semi-trailer and constructing an augmented state input dynamic discrete model to obtain an augmented dynamic discrete model; constructing a trajectory tracking predictive control optimization problem for the semi-trailer and solving it to obtain the optimal steering control command. This invention significantly improves the accuracy and robustness of articulation angle estimation.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle control technology, and in particular to a model predictive control method and system for semi-trailer trajectory tracking based on articulation angle hybrid state estimation and steering delay compensation. Background Technology

[0002] A semi-trailer is a vehicle consisting of a tractor and a trailer connected by an articulated mechanism. Compared to ordinary passenger cars, semi-trailers are characterized by their longer body, larger turning radius, significant trailer following lag, wide range of load variations, and complex operating conditions. During turning, lane changing, reversing, and tracking curved paths, it is necessary not only to control the tractor's lateral position and heading attitude but also to consider the trailer's following trajectory and articulated state; otherwise, problems such as amplified path deviation, increased trailer tail sway, and even increased risk of folding may occur.

[0003] Existing semi-trailer trajectory tracking methods can be broadly categorized into kinematic model-based methods, dynamic model-based methods, and methods combining both. Kinematic model-based methods have a relatively simple structure and low computational cost, making them suitable for low-speed or geometry-driven operating conditions. However, their control accuracy tends to decrease when tire yaw, load variations, actuator delays, and model mismatches occur. Dynamic model-based methods can more fully reflect lateral velocity, yaw response, tire yaw, and articulated coupling characteristics, but they have a higher state dimension, more parameters, and require higher online state measurability and real-time performance.

[0004] In semi-trailer systems, the articulation angle is a key state variable describing the relative attitude relationship between the tractor and the trailer. The articulation angle not only determines the lateral movement trend of the trailer but also directly affects trailer trajectory prediction, lateral stability analysis, hazardous condition identification, and controller constraint settings. However, due to cost constraints, engineering vehicles are often not equipped with high-precision articulation angle sensors, or even if such sensors are installed, they are easily limited by installation space, cost, and adaptability to operating conditions, making it difficult to measure this state variable directly and stably over a long period.

[0005] Regarding the problem of obtaining the articulation angle, existing technologies mostly employ state observers based on vehicle models for estimation. These methods typically utilize measurable signals such as vehicle speed, yaw rate, steering angle, position, and heading to construct state observers and achieve online estimation of the articulation angle. However, because semi-trailer models are highly sensitive to tire stiffness, speed range, load, and road adhesion conditions, observers relying solely on analytical models are susceptible to model mismatch, leading to accumulated estimation errors. On the other hand, with the development of temporal neural networks, virtual measurement methods based on gated recurrent units can learn the evolution of the articulation angle from historical input sequences. However, purely data-driven methods are dependent on the training data distribution, and under conditions deviating from the training data distribution, estimation accuracy and stability are prone to decline.

[0006] Besides the articulation angle estimation problem, steering execution delay is also a common issue in semi-trailer trajectory tracking control. This delay can originate from the communication link between the host controller and the actuator, or from the steering system, mechanical transmission components, and the establishment process of tire lateral force. For vehicles with long lengths and high inertia, if the control system directly solves for steering commands based solely on the currently measured state without considering the time delay of the commands actually acting on the vehicle, significant phase lag will occur, leading to increased tracking errors, exacerbated steering oscillations, and even reduced system stability margin.

[0007] While several delay compensation methods have been proposed in existing technologies, such as forward state prediction, delay augmentation modeling, and aiming control, most solutions are primarily geared towards passenger cars or single-body vehicles, failing to fully consider the coupling relationships between the articulated structure of semi-trailers, trailer following dynamics, and control constraints. If the articulation angle estimation, delay compensation, and trajectory tracking control are separated, the state information received by the controller often misaligns with the actual effective state of the controlled object, hindering closed-loop performance improvement.

[0008] Therefore, existing semi-trailer trajectory tracking technologies have at least the following shortcomings: First, key state variables such as the articulation angle lack online estimation methods that balance accuracy and robustness; second, steering execution delay is not uniformly compensated in the control closed loop; and third, there is insufficient coupling between state estimation, delay compensation, and trajectory tracking control, making it difficult to simultaneously ensure tracking accuracy, control smoothness, and lateral stability. Based on this, it is necessary to propose a semi-trailer trajectory tracking control method with a clear structure that can uniformly handle the problems of articulation angle estimation and delay compensation. Summary of the Invention

[0009] This invention provides a model predictive control method and system for semi-trailer trajectory tracking based on articulation angle hybrid state estimation and steering delay compensation. It solves the technical problems in the prior art, such as the lack of balance between accuracy and robustness of key state variables such as articulation angle, the failure to uniformly compensate for steering execution delay in the control closed loop, and the difficulty in simultaneously balancing tracking accuracy, control smoothness and lateral stability.

[0010] According to a first aspect of the present invention, a model predictive control method for semi-trailer trajectory tracking based on articulation angle hybrid state estimation and steering delay compensation is provided. The method includes: Obtain the semi-trailer reference trajectory and vehicle state information, construct an approximate kinematic model and a simplified dynamic model of the semi-trailer, and discretize the approximate kinematic model and the simplified dynamic model based on the reference trajectory information to obtain a kinematic discrete model and a dynamic discrete model, respectively. Acquire historical time-series data collected by the semi-trailer's sensors, input it into the articulation angle virtual measurement generation unit, and output the virtual measurement value of the articulation angle; Based on the vehicle state information, a state estimation vector of the tractor is constructed and input into the dynamic discrete model to calculate the next state prediction value. The measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the next state prediction value are filtered and fused to obtain the current optimal articulation angle estimation state. Obtain the total delay time of the semi-trailer and calculate its corresponding equivalent delay steps, construct an augmented state with input delay, input the augmented state into the dynamic discrete model, and obtain the augmented dynamic discrete model. Based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, a trajectory tracking predictive control optimization problem for the semi-trailer is constructed. The optimal steering control command is then obtained and transmitted to the semi-trailer control system.

[0011] Furthermore, the process of constructing the kinematic discrete model includes the following steps: A combined kinematic model of the tractor and trailer is established with the center point of the rear axle of the tractor as the reference point. In the joint kinematic model, the Frenet coordinate system is introduced to represent the error of the reference trajectory. Under the condition of small angle approximation, the error of the reference trajectory is simplified and a continuous-time approximate kinematic model is obtained. The approximate kinematic model is discretized by the sampling period to obtain the discrete kinematic model; The kinematic discrete model is used as input for the control state vector at the current sampling time and outputs the control state vector at the next sampling time.

[0012] Furthermore, the construction process of the dynamic discrete model includes the following steps: A simplified continuous-time dynamics model is constructed based on the lateral force balance and yaw moment balance relationship between the tractor and the trailer. The simplified dynamic model is discretized by the sampling period to obtain the discrete dynamic model; The dynamic discrete model is used to input the tractor state vector at the current sampling time and output the tractor state vector at the next sampling time.

[0013] Furthermore, historical time-series data collected by the semi-trailer's sensors is acquired, input into the articulation angle virtual measurement generation unit, and outputs virtual articulation angle measurement values, including: The historical time-series data is constructed into a time-series input sequence; Input the timing sequence into the hinge angle virtual measurement generation unit, and output the virtual measurement value of the hinge angle; The hinge angle virtual measurement generation unit includes a gated loop unit network; The time-series input sequence includes an input feature vector composed of vehicle speed, front wheel angle, yaw rate, lateral error, and heading error at multiple consecutive acquisition times.

[0014] Furthermore, the measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the predicted values ​​for the next step are filtered and fused to obtain the optimal estimated state of the current articulation angle, including: The next state prediction value is calculated by using a dynamic discrete model and the corresponding error covariance matrix is ​​determined. The measured value and the virtual measured value of the hinge angle are used to form a dual measurement vector. The Kalman gain is calculated based on the error covariance matrix. The next state prediction value and the dual measurement vector are then weighted and fused based on the Kalman gain to obtain the optimal estimated state of the hinge angle and update the error covariance matrix. The input to the dynamic discrete model includes the process noise covariance matrix, and the Kalman gain is calculated based on the measurement noise covariance matrix. Weight coordination between dynamic discrete model prediction and dual measurement vector virtualization is achieved by changing the process noise covariance matrix and the measurement noise covariance matrix.

[0015] Furthermore, the total delay time is the total time from when the semi-trailer control system issues a steering control command to when the wheels actually turn to the target angle, and the equivalent delay steps are the ratio of the total delay time to the control cycle. The augmented state is obtained by concatenating the equivalent number of historical steering control inputs with the tractor state vector.

[0016] Furthermore, based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, a trajectory tracking predictive control optimization problem for the semi-trailer is constructed, specifically including the following steps: The initial state is constructed based on the optimal estimated state of the hinge angle and the equivalent number of historical steering control inputs with delay steps. The initial state is input into the augmented dynamic discrete model, and the extended control state vector is output. The prediction time domain and control time domain are set, and an optimization objective function is constructed based on the extended control state vector. The optimization objective function includes a state tracking error term, a control input term, and a control increment term. At the same time, constraints are set for the front wheel steering angle, steering angle change rate, hinge angle, lateral error, yaw rate, and hinge rate. The state tracking error term includes the control state vector state tracking error term and the tractor state vector state tracking error term.

[0017] According to a second aspect of the present invention, the present invention provides a semi-trailer trajectory tracking model predictive control system based on articulation angle hybrid state estimation and steering delay compensation, comprising: The semi-trailer modeling module is used to acquire the semi-trailer reference trajectory and vehicle state information, construct an approximate kinematic model and a simplified dynamic model of the semi-trailer, and discretize the approximate kinematic model and the simplified dynamic model based on the reference trajectory information to obtain a kinematic discrete model and a dynamic discrete model, respectively. The virtual measurement module, connected to the semi-trailer modeling module, is used to acquire historical time-series data collected by the semi-trailer sensors, input the virtual hinge angle measurement generation unit, and output the virtual hinge angle measurement value. The articulation angle hybrid estimation module is connected to the virtual calculation module. It is used to construct the state estimation vector of the tractor based on the vehicle state information, and input it into the dynamic discrete model to calculate the next state prediction value. It filters and fuses the measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the next state prediction value to obtain the current optimal articulation angle estimation state. The delay compensation module, connected to the articulation angle hybrid estimation module, is used to obtain the total delay time of the semi-trailer and calculate its corresponding equivalent delay steps, construct an augmented state with input delay, and input the augmented state into the dynamic discrete model to obtain the augmented dynamic discrete model. The trajectory tracking predictive control optimization module, connected to the delay compensation module, is used to construct the trajectory tracking predictive control optimization problem of the semi-trailer based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, solve for the optimal steering control command, and transmit it to the semi-trailer control system.

[0018] According to a third aspect of the present invention, an electronic device is provided, including a memory, a processor, and a computer program; wherein the computer program is stored in the memory and configured to be executed by the processor to implement the method as described in the first aspect.

[0019] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, characterized in that a computer program is stored thereon; the computer program is executed by a processor to implement the method as described in the first aspect.

[0020] The beneficial effects of this invention are: 1. A hybrid state estimation method based on virtual measurement and filtering fusion using gated cyclic unit network was designed. This method utilizes the data-driven network's ability to extract temporal features while retaining the physical constraints of the dynamic model, thereby improving the accuracy and robustness of hinge angle estimation.

[0021] 2. By using kinematic and dynamic models in parallel, we can use the kinematic model to achieve low-speed geometric tracking and error construction, and use the dynamic model to describe lateral response, yaw characteristics and articulation dynamics, thereby improving the applicability of the model and the system's expressive ability.

[0022] 3. By incorporating the steering execution input delay into the control closed loop and constructing an augmented state model without explicit input delay through input delay augmentation, the phase lag effect caused by input delay is reduced, thereby improving trajectory tracking accuracy and system stability.

[0023] 4. The state estimation, delay compensation and trajectory tracking control are designed as a unified integrated system architecture. The delay compensation state further corrects the controller's initial value, which helps to improve dynamic consistency and feasibility.

[0024] It should be understood that the description in the Summary of the Invention is not intended to limit the key or essential features of the embodiments of the present invention, nor is it intended to restrict the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0025] The above and other features, advantages, and aspects of the various embodiments of the present invention will become more apparent from the accompanying drawings and the following detailed description. The drawings are provided for a better understanding of the invention and are not intended to limit the invention. In the drawings, the same or similar reference numerals denote the same or similar elements, wherein: Figure 1 The flowchart of the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation provided by an embodiment of the present invention is shown. Figure 2This figure shows the overall structure block diagram of the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation provided by an embodiment of the present invention; Figure 3 A semi-hanging chart model diagram provided in an embodiment of the present invention is shown; Figure 4 A schematic diagram of the hinge angle hybrid estimation module provided in an embodiment of the present invention is shown; Figure 5 A simulation diagram comparing the reference trajectory provided in this embodiment of the invention with the actual trajectory of the vehicle is shown. Figure 6 The graphs showing the changes in lateral error and heading error of the tractor over time according to an embodiment of the present invention are shown. Figure 7 A comparison chart of the actual and estimated values ​​of the hinge angle provided in an embodiment of the present invention is shown; Figure 8 A comparison diagram of the steering command and actual steering response provided in an embodiment of the present invention is shown; Figure 9 A block diagram of a semi-trailer trajectory tracking model prediction control system based on articulation angle hybrid state estimation and steering delay compensation provided in an embodiment of the present invention is shown. Figure 10 A block diagram of an electronic device according to an embodiment of the present invention is shown. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0027] Furthermore, the term "and / or" in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. Additionally, the character " / " in this article generally indicates that the preceding and following related objects have an "or" relationship.

[0028] This invention provides a model predictive control method for semi-trailer trajectory tracking based on articulation angle hybrid state estimation and steering delay compensation. See [link to relevant documentation]. Figures 1 to 4 This includes the following steps: S1. Obtain the semi-trailer reference trajectory and vehicle state information, construct an approximate kinematic model and a simplified dynamic model of the semi-trailer, and discretize the approximate kinematic model and the simplified dynamic model based on the reference trajectory information to obtain a kinematic discrete model and a dynamic discrete model, respectively; wherein, the vehicle state information includes the longitudinal velocity, yaw rate, vehicle position, heading angle and front wheel steering angle of the tractor.

[0029] Specifically, the process of building the model is as follows: (1) Discrete kinematic model A combined kinematic model of the tractor and trailer is established with the center point of the rear axle of the tractor as the reference point. In the joint kinematic model, the Frenet coordinate system is introduced to represent the error of the reference trajectory. Under the condition of small angle approximation, the error of the reference trajectory is simplified and a continuous-time approximate kinematic model is obtained. The approximate kinematic model is discretized by sampling period to obtain a discrete kinematic model. The discrete kinematic model is used as input for the control state vector at the current sampling time and outputs the control state vector at the next sampling time.

[0030] Specifically, see the following steps: Establish a combined kinematic model of the tractor and trailer, taking the rear axle center point of the tractor as the reference point, and assuming the longitudinal velocity of the tractor is v and the front wheel rotation angle of the tractor is θ. The wheelbase of the tractor is The equivalent wheelbase of the trailer is The heading angle of the tractor is The trailer heading angle is Then the hinge angle can be expressed as The kinematic model of the tractor can be represented by the following kinematic equations: ; ; ; in, and These represent the rates of change of the position of the center point of the rear axle of the tractor in the X and Y directions of the global coordinate system, respectively. This indicates the rate of change of the tractor's heading angle.

[0031] Rate of change of heading angle of trailer It can be represented as: ; The corresponding dynamic equation for the hinge angle is: ,in, This indicates the hinge angular rate.

[0032] To characterize the error of the trajectory tracking task, a reference trajectory (i.e., a preset trajectory route) is obtained, and the Frenet error coordinate system of the reference trajectory is introduced. Let the tangential direction angle of the tractor's reference trajectory at the matching point be . The curvature of the reference trajectory is The lateral error of the tractor reference point relative to the reference trajectory is The heading error is Then, in the error coordinate system: ; ; in, This represents the rate of change of the lateral error. This represents the rate of change of heading error.

[0033] Constructing the control state vector of the kinematic model Under the small-angle approximation condition, the rates of change can be expressed as: ; ; ; From the above three formulas, we can obtain an approximate kinematic model for continuous time: ; in, and Represents the coefficient matrix of the kinematic model. , , This represents the longitudinal velocity of the tractor at time t. , This represents the rate of change of the control state vector. This indicates the angle of the tractor's front wheels as it changes over time. A known feedforward term is introduced for the curvature of the reference trajectory. This represents the curvature of the reference trajectory at the matching point corresponding to time t. When the reference trajectory uses parametric equations... , When given, the curvature can be further expressed as: ; in, This represents the velocity component of the reference trajectory along the X direction at time t. This represents the acceleration component of the reference trajectory along the X direction at time t. This represents the velocity component of the reference trajectory along the Y direction at time t. This represents the acceleration component of the reference trajectory along the Y direction at time t.

[0034] Using sampling period Discretize the approximate kinematic model to obtain the discrete kinematic model: ; Where the subscript k represents the discrete sampling time, This represents the control state vector at the k-th sampling time. , This represents the curvature at the reference trajectory matching point at the k-th sampling time. For the front wheel steering angle control command of the tractor , This represents the lateral error at the k-th acquisition time. This represents the heading error at the k-th data acquisition time. This represents the hinge angle at the k-th acquisition time. , , , This represents the longitudinal speed of the tractor at the k-th sampling time.

[0035] In summary, the kinematic discrete model is mainly used to calculate geometric features such as heading error and lateral error.

[0036] (2) Discrete dynamic model A simplified continuous-time dynamics model is constructed based on the lateral force balance and yaw moment balance relationship between the tractor and the trailer. The simplified dynamic model is discretized by sampling period to obtain a discrete dynamic model; the discrete dynamic model is used to input the tractor state vector at the current sampling time and output the tractor state vector at the next sampling time.

[0037] Specifically, it includes the following steps: To more accurately reflect the lateral dynamic response of the semi-trailer, a simplified dynamic model of the tractor and trailer is constructed based on the aforementioned approximate kinematic model.

[0038] Let the lateral speed of the tractor be... The yaw rate of the tractor is r, and the dynamic state vector is defined as follows: ; Under conditions of small sideslip angle and small articulation angle, based on the lateral force balance and yaw moment balance relationship between the tractor and trailer, a simplified continuous-time dynamic model can be constructed, as shown in the following formula: ; ; ; in, This indicates the rate of change of the lateral speed of the tractor unit. This indicates the rate of change of the yaw rate of the tractor unit. Indicates the hinge angular acceleration. For the lateral stiffness of the front axle of the tractor, For the lateral stiffness of the rear axle of the tractor, The distance from the center of gravity of the tractor to the front axle. The distance from the center of gravity to the rear axle. For the quality of the tractor, Let the moment of inertia be about the center of mass. The hinge angle recovery stiffness coefficient, This is the hinge angle damping coefficient. This is the coupling gain coefficient from the steering input to the articulated dynamics. , , , , and These are constant coefficients obtained after linearization from the equivalent mass distribution of the trailer, the geometric parameters of the articulation point, the axle lateral slip characteristics of the trailer, and the coupling relationship between the tractor and trailer. and The coupling effects of the articulation angular rate term and the articulation angle term on the lateral velocity channel of the tractor are characterized respectively. and The coupling effects of the articulation rate term and the articulation angle term on the yaw rate channel of the tractor are characterized respectively. and The coupling effects of the lateral velocity and yaw rate of the tractor on the dynamic channel of the articulation angle are characterized, respectively.

[0039] The above equation is transformed into state-space form and discretized. The sampling period during discretization is also the same. The discrete dynamic model can be obtained as follows: ; in, Represents the tractor state vector. , This represents the hinge angular velocity at the k-th acquisition time. This represents the yaw rate at the k-th acquisition time. This represents the lateral velocity of the tractor at the k-th data collection time. This represents the steering angle of the tractor's front wheel at the k-th sampling time. Let represent the discrete lumped disturbance term at the k-th sampling time, which is obtained by discretizing the model uncertainty and external disturbance term in the continuous-time model. and The coefficient matrix of the discrete model can be represented by the following formula: , .

[0040] In this invention, the kinematic discrete model is used to construct the error in the Frenet coordinate system of the reference trajectory and generate the control state vectors of lateral error and heading error, and to provide the input features related to the path geometry for the subsequent articulation angle virtual measurement generation unit.

[0041] The established dynamic discrete model is used to characterize the lateral response, yaw dynamics, and articulation dynamics of the tractor, and serves as the main model for state one-step prediction, articulation angle filtering fusion, steering input delay augmentation, and trajectory tracking predictive control optimization.

[0042] In summary, this invention constructs a dynamic discrete model to describe the dynamic response of a vehicle, such as lateral velocity, yaw rate, and articulation angle, thereby improving the accuracy of online estimation of the articulation angle and lateral state of the semi-trailer and providing a consistent state basis for subsequent delay compensation and predictive control.

[0043] S2. Acquire historical time-series data collected by the semi-trailer sensors, input it into the articulation angle virtual measurement generation unit, and output the virtual measurement value of the articulation angle.

[0044] Specifically, the historical time-series data is constructed into a time-series input sequence; Input the timing sequence into the hinge angle virtual measurement generation unit, and output the virtual measurement value of the hinge angle; The hinge angle virtual measurement generation unit includes a gated loop unit network; The time-series input sequence includes an input feature vector composed of vehicle speed, front wheel angle, yaw rate, lateral error, and heading error at multiple consecutive acquisition times.

[0045] In one specific embodiment, the following steps are included: Construct a time-series input sequence based on the historical time-series data collected by the vehicle status acquisition module, i.e., the sensors, over a recent period. : ; Where N represents the window length of the time-series input sequence. This represents the input feature vectors obtained at different acquisition times. .

[0046] Input time sequence Input the gated recurrent unit network to obtain the virtual measurement value of the hinge angle at the current moment. See the following formula: ; in, This represents the set of network parameters after training. This represents the nonlinear mapping function determined by the trained gated recurrent unit network, used to map the historical time series input sequence. Mapped to the virtual measured value of the hinge angle at the current sampling time. The recursive update relationship within the gated cyclic unit network can be expressed by the following formula: ; ; ; ; ; in, Indicates resetting the gate output. This indicates an update to the gate output. Indicates a hidden state. Indicates the candidate hidden state. This represents the Sigmoid function. Represents the Hadamard product. , , , , , , Represents the network weight matrix. , , , Both represent network bias terms.

[0047] S3. Based on the vehicle state information, construct the state estimation vector of the tractor and input it into the dynamic discrete model to calculate the next state prediction value. Filter and fuse the measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the next state prediction value to obtain the current optimal articulation angle estimation state, thus realizing the online estimation of the semi-trailer articulation angle and the vehicle's lateral state.

[0048] The next state prediction value is calculated by using a dynamic discrete model and the corresponding error covariance matrix is ​​determined. The measured value and the virtual measured value of the hinge angle are used to form a dual measurement vector. The Kalman gain is calculated based on the error covariance matrix. The next state prediction value and the dual measurement vector are then weighted and fused based on the Kalman gain to obtain the optimal estimated state of the hinge angle and update the error covariance matrix. The input to the dynamic discrete model includes the process noise covariance matrix, and the Kalman gain is calculated based on the measurement noise covariance matrix. Weight coordination between dynamic discrete model prediction and dual measurement vector virtualization is achieved by changing the process noise covariance matrix and the measurement noise covariance matrix.

[0049] In one specific embodiment, the current state estimation vector of the tractor is constructed, that is, the tractor state vector at the current sampling time. ,in, This represents the lateral velocity at time k. This represents the yaw rate at the k-th acquisition time. This represents the hinge angular velocity at time k. This represents the hinge angle at the k-th moment.

[0050] The state estimation vector is determined by the filtered and updated state estimation result from the previous sampling time and the current sensor data. The next state prediction value is the tractor state vector corresponding to the next sampling time. When using the dynamic discrete model for the next state prediction, the state estimation value from the previous sampling time is input. The front wheel steering angle input applied to the vehicle system at the previous sampling time. The predictive relationship can be found in the following formula: ; ; in, This represents the predicted state value at the k-th sampling time, obtained based on the information at time k-1. This represents the filtered and updated state estimate at the (k-1)th sampling time. This represents the front wheel steering angle input applied to the vehicle system at the (k-1)th sampling time. Let be the error covariance matrix. This is the updated error covariance matrix from the previous sampling time. Let be the process noise covariance matrix.

[0051] Measured values ​​of the yaw rate of the tractor were collected using sensors. Combined with virtual measurement of hinge angle Construct two measurement vectors and establish measurement equations. : ; in, H represents the measurement matrix. , This represents the measurement noise vector, which follows a zero-mean Gaussian distribution.

[0052] This invention constructs a Kalman gain to avoid the problems of single dynamic discrete models being greatly affected by parameter mismatch and pure data-driven methods lacking physical constraints. Specifically, the Kalman gain is calculated using the following formula. : ; in, This represents the measurement noise covariance matrix.

[0053] Weighted fusion is performed using Kalman gain, and the estimated state value is updated using the following formula: ; in, This represents the dynamic state estimate obtained at the k-th sampling time by combining the state prediction value and the current measurement information.

[0054] Finally, the error covariance matrix is ​​updated using the following formula to prepare for the next step of filtering and fusion: ; in, This represents the error covariance matrix after the measurement update is completed at the k-th sampling time, used to characterize the estimation uncertainty of the updated state estimate.

[0055] By adjusting the process noise covariance matrix With measurement noise covariance matrix It can achieve weight coordination between model prediction and virtual measurement. and It can be preset based on sensor calibration results, historical experimental data statistics, or simulation error statistics, or adjusted online according to vehicle operating conditions and virtual measurement errors. When the value increases... or reduce When the current measurement is increased, the filtering update process places more trust in the current measurement information; when the current measurement is decreased... or increase At that time, the filtering update process places more trust in the prediction results of the dynamic model.

[0056] The optimal estimate of the current hinge angle is obtained through the above prediction and update process, including the estimated values ​​of lateral velocity, yaw rate, hinge angle rate, and hinge angle.

[0057] The optimal estimated state of the current articulation angle obtained in this step is not independent of the subsequent delay compensation process, but rather enters the delay augmented state as part of the vehicle's current articulation state. Because steering control commands have an execution delay, the controller needs to predict the vehicle state at the actual moment the control command takes effect when calculating the current control quantity. If the articulation angle state is ignored, it is difficult to accurately describe the attitude change of the trailer relative to the tractor. Therefore, this application uses the articulation angle estimation result to constitute the current dynamic state, and constructs an input delay augmented model based on this, thereby achieving coupling between articulation state estimation and steering delay compensation, as detailed in the following steps.

[0058] S4. Obtain the total delay time of the semi-trailer and calculate its corresponding equivalent delay steps, construct an augmented state with input delay, input the augmented state into the dynamic discrete model, and obtain the augmented dynamic discrete model.

[0059] Total delay time refers to the total input response delay of the semi-trailer steering actuator, that is, the total time from when the control system issues a steering control command, the steering motor or hydraulic mechanism executes the action, to when the wheels actually turn to the target angle. It is the time difference between the control input and the actual system response, defined as... The control cycle is Then the equivalent delay steps .

[0060] Steering input delay compensation is implemented based on a dynamic discrete model. However, the kinematic discrete model is used to provide control state vectors such as lateral and heading errors in the Frenet coordinate system of the reference trajectory, and is not used as the main model for delay augmentation prediction. To handle steering execution input delay, an input delay augmentation method based on the dynamic discrete model is adopted, incorporating the delayed input as part of the extended state into the prediction model. This is based on the tractor state vector. That is, constructing augmented states from the original dynamic state vector. : ; The augmented state is obtained by concatenating the equivalent number of historical steering control inputs with the tractor state vector, which are delayed by a certain number of steps. This indicates that the historical steering control input has been sent to the steering actuator at the (k-1)th sampling time, but is still stored in the delay queue due to input delay. This means that the historical steering commands that have not yet taken effect in the future are directly appended to the original dynamic state vector to form an extended control state. Selected as the front wheel steering angle control command for the tractor By incorporating delayed instructions into the state, an augmented dynamic discrete model without explicit input delay can be established: ; in, This represents the extended control state vector. and This represents the delay compensation coefficient matrix. , .

[0061] S5. Based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, construct the trajectory tracking predictive control optimization problem of the semi-trailer, solve the optimal steering control command, and transmit it to the semi-trailer control system.

[0062] First, the initial state is constructed based on the optimal estimated state of the hinge angle and the equivalent number of historical steering control inputs with delay steps. The initial state is input into the augmented dynamic discrete model, and the extended control state vector is output.

[0063] Specifically, the hinge angle estimate obtained in S3 is the hinge angle estimate in the optimal hinge angle estimate state. and the corresponding estimated hinge angular rate As the current articulated state, and compared with the estimated lateral velocity of the tractor. yaw rate estimate Together they constitute the current estimated state value of the dynamics: .

[0064] The current dynamically estimated state value is used as the initial state of the extended control state vector, and combined with the steering input delay queue. Construct the initial state for the predictive control optimization problem: .

[0065] Therefore, the optimal estimated state of the articulation angle participates in the prediction of future states as part of the initial state of the predictive control optimization problem, and is subsequently used to solve the optimal steering control command through the influence of the articulation angle state tracking error term, the articulation angle rate state tracking error term, and the articulation angle constraint.

[0066] Using the extended control state vector, which includes the optimal estimated state of the articulation angle and the steering delay input sequence, as the control object, a model predictive control optimization objective function is constructed based on the augmented dynamics discrete model. This objective function is composed of state tracking error terms for the tractor's lateral error, heading error, lateral velocity, yaw rate, articulation rate, and articulation angle, as well as control input terms and control increment terms. Specifically, the state tracking error terms for lateral error and heading error are collectively referred to as the control state vector state tracking error terms, and the state tracking error terms for lateral velocity, yaw rate, articulation rate, and articulation angle are collectively referred to as the tractor's state vector state tracking error terms. Simultaneously, constraints are set for front wheel steering angle, front wheel steering angle change rate, articulation angle, lateral error, yaw rate, and articulation rate to limit the system's operating boundaries and trajectory tracking accuracy requirements. The optimal control sequence is obtained by solving the constrained optimization problem online. The first control variable in the optimal control sequence is selected as the steering control command for the current control cycle. This command is sent to the steering actuator to act on the semi-trailer system. The actual response state of the semi-trailer is collected as the vehicle state information for the next control cycle, thereby realizing closed-loop high-precision tracking control of the semi-trailer reference trajectory.

[0067] This invention sets the prediction time domain and control time domain The prediction time domain is the time range for predicting the state of multiple consecutive control cycles in the future, while the control time domain is the time range for optimizing the control input of multiple consecutive control cycles in the future. and According to the sampling period Vehicle lateral response time, steering execution equivalent delay steps And the real-time computing capabilities of the vehicle controller are determined.

[0068] in, It must be no less than This ensures that the prediction range covers the actual effective time of the control input. To reduce the computational load of online optimization. In one implementation, You can take 10~30. A value of 3 to 10 is acceptable.

[0069] Using the extended control state vector as the controlled object, the following optimization objective function is constructed. : ; in, This represents the prediction obtained at the k-th sampling time. Extended control state vector at each sampling time. Indicates the first The reference extended control state vector at each sampling time, This represents the result of optimization at the k-th sampling time. Steering control input for each prediction step, This represents the control input increment between adjacent prediction steps, satisfying... , This represents the state tracking error weight matrix. This represents the control input weight matrix. This represents the control increment weight matrix. The objective function contains three additive terms: the state tracking error term, the control input term, and the control increment term. Expanding these terms, we have: ; in, , , , , , These are the weighting coefficients for the tractor's lateral error, heading error, lateral velocity, yaw rate, articulation rate, and articulation angle. This represents the lateral error at the (k+i)th acquisition time, based on the Kth acquisition time. This represents the heading error obtained at the (k+i)th acquisition time based on the Kth acquisition time. This represents the lateral velocity obtained at the (k+i)th acquisition time based on the Kth acquisition time. This represents the yaw rate of the tractor at the (k+i)th acquisition time, obtained based on the Kth acquisition time. This represents the hinge angular velocity obtained at the (k+i)th acquisition time based on the Kth acquisition time. This represents the hinge angle obtained at the (k+i)th acquisition time based on the Kth acquisition time. This represents the control command for the front wheel angle of the tractor obtained at the (k+i)th acquisition time based on the Kth acquisition time. This represents the change in the front wheel steering angle control command of the tractor obtained at the (k+i)th acquisition time based on the Kth acquisition time. To control the input weights, To control the incremental weights.

[0070] In addition, the objective function is subject to the following constraints: ; in, and These represent the minimum and maximum values ​​of the front wheel steering angle control command for the tractor, respectively. These represent the minimum and maximum values ​​of the change in the front wheel steering angle control command of the tractor, respectively. and These represent the minimum and maximum values ​​of the hinge angle, respectively. and These represent the minimum and maximum values ​​of the heading error, respectively. and These represent the minimum and maximum yaw rates of the tractor unit, respectively. and Let represent the minimum and maximum values ​​of the articulation angular rate, respectively. Equations (1) to (6) above correspond to the front wheel steering angle constraint, steering angle change rate constraint, articulation angle constraint, lateral error constraint, yaw rate constraint, and articulation angular rate constraint, respectively.

[0071] After solving the above optimization problem online, the optimal steering control input sequence in the predictive control time domain is obtained: ; in, ; This represents the optimal control input sequence obtained at the k-th sampling time. This represents the optimal steering control input for the (k+i)th prediction step, predicted at the kth sampling time.

[0072] Using a rolling optimization approach, the first control variable in the optimal control input sequence is taken as the current control command: ; ; in, The input indicates the front wheel steering angle, which is the optimal steering control command. The optimal steering control command is sent to the steering execution module and applied to the semi-trailer system. The actual response result of the semi-trailer system is collected as the vehicle state for the next control cycle, thereby realizing high-precision tracking control of the semi-trailer on the reference trajectory.

[0073] It should be noted that the optimization variable in this application is the steering control input sequence. The optimization objective is not a single steering angle, but rather to minimize the comprehensive performance index that includes state errors such as lateral error, heading error, lateral velocity, yaw rate, articulation rate, and articulation angle, while constraining the amplitude and rate of change of the control input, thereby obtaining the optimal steering control command that balances trajectory tracking accuracy, articulation stability, and control smoothness.

[0074] In one specific embodiment, the following simulation analysis was performed to verify the effectiveness of the method of the present invention: like Figure 5 As shown, under different control configurations, the hybrid state estimation and delay compensation MPC combination scheme adopted in this invention reduces the root mean square error of the lateral error by 88.2% and the maximum lateral error by 89.2% compared with the scheme combining basic estimation and conventional MPC. This indicates that the present invention can significantly improve the tracking accuracy of the semi-trailer on the reference trajectory.

[0075] like Figure 6 As shown, after introducing hybrid state estimation and delay compensation (MPC), the fluctuation amplitudes of lateral and heading errors are significantly reduced, the error convergence speed is significantly improved, and the closed-loop tracking process is more stable. Compared with the comparative scheme, the heading error and articulation angle related estimation error are significantly reduced, indicating that the present invention can effectively improve the accuracy of vehicle state acquisition and the stability of trajectory tracking.

[0076] like Figure 7 As shown, the result of the hybrid state estimation of this invention is closer to the true value of the hinge angle. Compared with the traditional Kalman filtering method, the root mean square error of the hinge angle estimation by the hybrid state estimation method is reduced by 87.0%; compared with the estimation method using gated recurrent unit network alone, the root mean square error is further reduced by 12.4%, indicating that this invention can achieve high-precision online estimation of the articulation angle of a semi-trailer.

[0077] like Figure 8As shown, after adopting delay compensation, the actual steering response's adherence to control commands is significantly improved, and the response deviation caused by input lag is significantly reduced. Compared with the conventional MPC scheme, the delay-compensated MPC scheme reduces the root mean square error of the lateral error by 78.4%, the root mean square error of the heading error by 84.0%, and the maximum lateral error by 78.5%, indicating that the delay compensation design of this invention can effectively mitigate the adverse effects of steering execution delay on trajectory tracking performance.

[0078] In summary, by combining articulation angle hybrid state estimation, steering delay compensation, and model predictive control, this invention can improve the trajectory tracking accuracy, state estimation accuracy, and lateral stability of semi-trailers under conditions where direct articulation angle measurement is lacking and steering execution delay exists.

[0079] Based on the above technical solution, the following beneficial effects are achieved: 1. A hybrid state estimation method based on virtual measurement and filtering fusion using gated cyclic unit network was designed. This method utilizes the data-driven network's ability to extract temporal features while retaining the physical constraints of the dynamic model, thereby improving the accuracy and robustness of hinge angle estimation.

[0080] 2. By using kinematic and dynamic models in parallel, we can use the kinematic model to achieve low-speed geometric tracking and error construction, and use the dynamic model to describe lateral response, yaw characteristics and articulation dynamics, thereby improving the applicability of the model and the system's expressive ability.

[0081] 3. By incorporating the steering execution input delay into the control closed loop and constructing an augmented state model without explicit input delay through input delay augmentation, the phase lag effect caused by input delay is reduced, thereby improving trajectory tracking accuracy and system stability.

[0082] 4. The state estimation, delay compensation and trajectory tracking control are designed as a unified integrated system architecture. The delay compensation state further corrects the controller's initial value, which helps to improve dynamic consistency and feasibility.

[0083] This invention also provides a semi-trailer trajectory tracking model prediction control system 900 based on articulation angle hybrid state estimation and steering delay compensation, see [link to relevant documentation]. Figure 9 ,include: The semi-trailer modeling module 910 is used to acquire the semi-trailer reference trajectory and vehicle state information, construct an approximate kinematic model and a simplified dynamic model of the semi-trailer, and discretize the approximate kinematic model and the simplified dynamic model based on the reference trajectory information to obtain a kinematic discrete model and a dynamic discrete model, respectively. The virtual measurement module 920 is connected to the semi-trailer modeling module 910 and is used to acquire historical time-series data collected by the semi-trailer sensors, input the articulation angle virtual measurement generation unit, and output the articulation angle virtual measurement value. The articulation angle hybrid estimation module 930 is connected to the virtual calculation module 920. It is used to construct the state estimation vector of the tractor based on the vehicle state information, and input it into the dynamic discrete model to calculate the next state prediction value. It filters and fuses the measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the next state prediction value to obtain the current optimal articulation angle estimation state. The delay compensation module 940 is connected to the articulation angle hybrid estimation module 930. It is used to obtain the total delay time of the semi-trailer and calculate its corresponding equivalent delay steps, construct an augmented state with input delay, and input the augmented state into the dynamic discrete model to obtain the augmented dynamic discrete model. The trajectory tracking predictive control optimization module 950, connected to the delay compensation module 940, is used to construct the trajectory tracking predictive control optimization problem of the semi-trailer based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, and solve the optimal steering control command to transmit it to the semi-trailer control system.

[0084] For other details, please refer to the methods described above; they will not be repeated here.

[0085] It should be noted that, for the sake of simplicity, the foregoing method embodiments are all described as a series of actions. However, those skilled in the art should understand that the present invention is not limited to the described order of actions, because according to the present invention, some steps can be performed in other orders or simultaneously. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are all optional embodiments, and the actions and modules involved are not necessarily essential to the present invention.

[0086] The acquisition, storage, and application of user personal information involved in the technical solution of this invention all comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0087] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working process of the described module can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0088] Figure 10A schematic block diagram of an electronic device 1000 that can be used to implement embodiments of the present invention is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0089] Electronic device 1000 includes a computing unit 1001, which can perform various appropriate actions and processes according to a computer program stored in ROM 1002 or a computer program loaded into RAM 1003 from storage unit 1008. RAM 1003 may also store various programs and data required for the operation of electronic device 1000. The computing unit 1001, ROM 1002, and RAM 1003 are interconnected via bus 1004. I / O interface 1005 is also connected to bus 1004.

[0090] Multiple components in electronic device 1000 are connected to I / O interface 1005, including: input unit 1006, such as keyboard, mouse, etc.; output unit 1007, such as various types of displays, speakers, etc.; storage unit 1008, such as disk, optical disk, etc.; and communication unit 1009, such as network card, modem, wireless transceiver, etc. Communication unit 1009 allows electronic device 1000 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0091] The computing unit 1001 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the computing unit 1001 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 1001 performs the various methods and processes described above. For example, in some embodiments, the methods may be implemented as computer software programs tangibly contained in a machine-readable medium, such as storage unit 1008. In some embodiments, part or all of the computer program may be loaded into and / or installed on the electronic device 1000 via ROM 1002 and / or communication unit 1009. When the computer program is loaded into RAM 1003 and executed by the computing unit 1001, one or more steps of the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation described above may be performed. Alternatively, in other embodiments, the computing unit 1001 may be configured, by any other suitable means (e.g., by means of firmware), to perform the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation.

[0092] It should be understood that the various forms of processes shown above can be used to reorder, add, or delete steps. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein.

[0093] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A model predictive control method for semi-trailer trajectory tracking based on articulation angle hybrid state estimation and steering delay compensation, characterized in that, include: Obtain the semi-trailer reference trajectory and vehicle state information, construct an approximate kinematic model and a simplified dynamic model of the semi-trailer, and discretize the approximate kinematic model and the simplified dynamic model based on the reference trajectory information to obtain a kinematic discrete model and a dynamic discrete model, respectively. Acquire historical time-series data collected by the semi-trailer's sensors, input it into the articulation angle virtual measurement generation unit, and output the virtual measurement value of the articulation angle; Based on the vehicle state information, a state estimation vector of the tractor is constructed and input into the dynamic discrete model to calculate the next state prediction value. The measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the next state prediction value are filtered and fused to obtain the current optimal articulation angle estimation state. Obtain the total delay time of the semi-trailer and calculate its corresponding equivalent delay steps, construct an augmented state with input delay, input the augmented state into the dynamic discrete model, and obtain the augmented dynamic discrete model. Based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, a trajectory tracking predictive control optimization problem for the semi-trailer is constructed. The optimal steering control command is then obtained and transmitted to the semi-trailer control system.

2. The semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation according to claim 1, characterized in that, The process of constructing the kinematic discrete model includes the following steps: A combined kinematic model of the tractor and trailer is established with the center point of the rear axle of the tractor as the reference point. In the joint kinematic model, the Frenet coordinate system is introduced to represent the error of the reference trajectory. Under the condition of small angle approximation, the error of the reference trajectory is simplified and a continuous-time approximate kinematic model is obtained. The approximate kinematic model is discretized by the sampling period to obtain the discrete kinematic model; The kinematic discrete model is used as input for the control state vector at the current sampling time and outputs the control state vector at the next sampling time.

3. The semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation according to claim 1, characterized in that, The process of constructing the dynamic discrete model includes the following steps: A simplified continuous-time dynamics model is constructed based on the lateral force balance and yaw moment balance relationship between the tractor and the trailer. The simplified dynamic model is discretized by the sampling period to obtain the discrete dynamic model; The dynamic discrete model is used to input the tractor state vector at the current sampling time and output the tractor state vector at the next sampling time.

4. The semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation according to claim 1, characterized in that, The system acquires historical time-series data collected by the semi-trailer's sensors, inputs this data into the articulation angle virtual measurement generation unit, and outputs virtual articulation angle measurement values, including: The historical time-series data is constructed into a time-series input sequence; Input the timing sequence into the hinge angle virtual measurement generation unit, and output the virtual measurement value of the hinge angle; The hinge angle virtual measurement generation unit includes a gated loop unit network; The time-series input sequence includes an input feature vector composed of vehicle speed, front wheel angle, yaw rate, lateral error, and heading error at multiple consecutive acquisition times.

5. The semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation according to claim 1, characterized in that, The measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the predicted values ​​for the next step are filtered and fused to obtain the optimal estimated state of the current articulation angle, including: The next state prediction value is calculated by using a dynamic discrete model and the corresponding error covariance matrix is ​​determined. The measured value and the virtual measured value of the hinge angle are used to form a dual measurement vector. The Kalman gain is calculated based on the error covariance matrix. The next state prediction value and the dual measurement vector are then weighted and fused based on the Kalman gain to obtain the optimal estimated state of the hinge angle and update the error covariance matrix. The input to the dynamic discrete model includes the process noise covariance matrix, and the Kalman gain is calculated based on the measurement noise covariance matrix. Weight coordination between dynamic discrete model prediction and dual measurement vector virtualization is achieved by changing the process noise covariance matrix and the measurement noise covariance matrix.

6. The semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation according to claim 3, characterized in that, The total delay time is the total time from when the semi-trailer control system issues a steering control command to when the wheels actually turn to the target angle, and the equivalent delay steps are the ratio of the total delay time to the control cycle. The augmented state is obtained by concatenating the equivalent number of historical steering control inputs with the tractor state vector.

7. The semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation according to claim 1, characterized in that, Based on the control state vector output by the kinematic discrete model, the optimal estimate of the articulation angle, and the augmented dynamic discrete model, a trajectory tracking predictive control optimization problem for a semi-trailer is constructed, specifically including the following steps: The initial state is constructed based on the optimal estimated state of the hinge angle and the equivalent number of historical steering control inputs with delay steps. The initial state is input into the augmented dynamic discrete model, and the extended control state vector is output. The prediction time domain and control time domain are set, and an optimization objective function is constructed based on the extended control state vector. The optimization objective function includes a state tracking error term, a control input term, and a control increment term. At the same time, constraints are set for the front wheel steering angle, steering angle change rate, hinge angle, lateral error, yaw rate, and hinge rate. The state tracking error term includes the control state vector state tracking error term and the tractor state vector state tracking error term.

8. A semi-trailer trajectory tracking model predictive control system based on articulation angle hybrid state estimation and steering delay compensation, used to implement the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation as described in any one of claims 1 to 7, characterized in that, include: The semi-trailer modeling module is used to acquire the semi-trailer reference trajectory and vehicle state information, construct an approximate kinematic model and a simplified dynamic model of the semi-trailer, and discretize the approximate kinematic model and the simplified dynamic model based on the reference trajectory information to obtain a kinematic discrete model and a dynamic discrete model, respectively. The virtual measurement module, connected to the semi-trailer modeling module, is used to acquire historical time-series data collected by the semi-trailer sensors, input the virtual hinge angle measurement generation unit, and output the virtual hinge angle measurement value. The articulation angle hybrid estimation module is connected to the virtual calculation module. It is used to construct the state estimation vector of the tractor based on the vehicle state information, and input it into the dynamic discrete model to calculate the next state prediction value. It filters and fuses the measured values ​​collected by the semi-trailer sensors, the virtual measured values ​​of the articulation angle, and the next state prediction value to obtain the current optimal articulation angle estimation state. The delay compensation module, connected to the articulation angle hybrid estimation module, is used to obtain the total delay time of the semi-trailer and calculate its corresponding equivalent delay steps, construct an augmented state with input delay, and input the augmented state into the dynamic discrete model to obtain the augmented dynamic discrete model. The trajectory tracking predictive control optimization module, connected to the delay compensation module, is used to construct the trajectory tracking predictive control optimization problem of the semi-trailer based on the control state vector output by the kinematic discrete model, the optimal estimated state of the articulation angle, and the augmented dynamic discrete model, solve for the optimal steering control command, and transmit it to the semi-trailer control system.

9. An electronic device, characterized in that, It includes a memory, a processor, and a computer program; wherein the computer program is stored in the memory and configured to be executed by the processor to implement the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, It stores a computer program; the computer program is executed by a processor to implement the semi-trailer trajectory tracking model predictive control method based on articulation angle hybrid state estimation and steering delay compensation as described in any one of claims 1 to 7.

Images