Method and system for estimating the angle of a main suspension based on physical constraints and selective reset

Abstract

The application discloses a method and system for estimating a main-trailer included angle based on physical constraints and selective resetting, and belongs to the technical field of automobile electronics. The method comprises the following steps: acquiring a main vehicle speed, a main vehicle heading angle, and left and right wheel speed signals of a trailer; establishing a kinematic model of an articulated vehicle, calculating a trailer yaw rate observation value, and adopting a Kalman filter to define a state vector which at least comprises a trailer heading angle and an error compensation item of a wheel speed observation; establishing a prediction equation and an observation equation to estimate the trailer heading angle in real time; when the vehicle is in a stable straight driving state and a current road adhesion coefficient is higher than a preset threshold, performing a first state resetting: only the trailer heading angle in the state vector is forced to be assigned as a current main vehicle heading angle, and a covariance matrix is reset as an initial value which is dynamically set; and the main-trailer included angle is obtained by subtracting the main vehicle heading angle from the trailer heading angle obtained after the first state resetting, so that the accumulation path of the wheel speed integral error is cut off, and the estimation accuracy is improved.

Description

Technical Field

[0001] This invention relates to the field of automotive electronics technology, and in particular to a method and system for estimating the main and auxiliary armature angles based on physical constraints and selective reset. Background Technology

[0002] The main trailer angle of articulated vehicles (such as semi-trailers) is a key parameter for vehicle stability control, reversing assistance, and intelligent driving functions (including platoon lateral stability, wind resistance energy saving and cooperative braking redundancy in 1+1 formation). Accurately and in real time obtaining this angle has important engineering value.

[0003] Existing methods for estimating the articulation angle between the trailer and the trailer can be broadly categorized into three types. The first type involves adding an angle sensor, which offers high accuracy but requires additional hardware on the trailer side, resulting in high costs and difficulty adapting to logistics scenarios with frequent trailer changes. The second type relies on visual or lidar-based environmental perception solutions, which are significantly affected by lighting conditions, rain, and fog, and have poor generalization capabilities. For example, some patents utilize lidar to collect point cloud data and then apply a Kalman filter algorithm to obtain the articulation angle; others use point cloud images to construct an extended Kalman filter model for trailer attitude detection. The third type is the pure wheel speed integration method, which uses the trailer's existing ABS wheel speed sensors to calculate the wheel speed difference and then integrates it to obtain the angle. This method is extremely low-cost but suffers from severe integration drift—wheel speed measurement noise, zero bias, and proportional errors accumulate continuously during integration, reaching several degrees or even tens of degrees after prolonged operation. Furthermore, its accuracy drops sharply on low-adhesion surfaces or under braking conditions.

[0004] To suppress drift, existing techniques have attempted to apply Extended Kalman Filters (EKFs) to angle estimation for articulated vehicles. For example, some studies have designed EKFs based on nonlinear vehicle models to estimate the steering and articulation angles of tractor-trailer combinations; some patents have constructed EKF models to output trailer attitude data. However, such schemes still exhibit non-negligible cumulative drift after long-term operation. This is because the EKF's state correction relies on observational information. In the absence of an external absolute reference, small fluctuations continuously inject into the heading angle estimation through integral effects, causing drift to persist.

[0005] Intuitively, resetting the estimator using the physical constraint that the angle between the main and auxiliary components is always zero during stable straight-line driving seems to fundamentally cut off the drift accumulation path. However, this approach has not been adopted in the field for a long time. The reason is that there is a general understanding in Kalman filtering theory that artificially changing or "hard-resetting" the filter state may destroy the smoothness and consistency of the estimation. Academic research also shows that large state updates will degrade linearization accuracy, and discontinuous adjustment methods are prone to causing estimation jitter. Therefore, the mainstream technical approach in this field tends to adopt a "gradual" approach when facing the drift problem—optimizing the noise covariance, increasing the state dimension, or introducing external sensors—rather than using "state reset". In addition, although a few patents mention covariance reset or state preservation when stationary, none of them involve using the high-frequency condition of straight-line driving for state reset. Summary of the Invention

[0006] To address the problems in the prior art, this invention provides a method and system for estimating the main mounting angle based on physical constraints and selective reset.

[0007] On the one hand, a method for estimating the main mounting angle based on physical constraints and selective reset is provided, the method comprising:

[0008] S1: Get the main vehicle speed and the main vehicle heading angle ;

[0009] S2: Obtain the wheel speed signals of the left and right wheels of the trailer;

[0010] S3: Establish the kinematic model of the articulated vehicle, trailer heading angle The rate of change satisfies:

[0011] ;

[0012] Where, α The main hanging angle, This refers to the trailer wheelbase;

[0013] S4: Calculate the observed value of the trailer's yaw rate based on the speed difference between the left and right wheels of the trailer and the wheel track of the trailer;

[0014] S5: Employ a Kalman filter, defining its state vector to include at least the trailer heading angle. In addition to the error compensation term for wheel speed observation; a prediction equation is established based on the state vector and kinematic model, and an observation equation is established using the observed values ​​of the trailer yaw rate to estimate the trailer heading angle in real time. ;

[0015] S6: Real-time detection of vehicle driving conditions. When the vehicle is detected to be in a stable straight-line driving state and the current road surface adhesion coefficient is higher than a preset threshold, a forced reset logic is triggered, executing the first state reset: only the trailer heading angle in the state vector is reset. The value is forcibly assigned to the current driver's heading angle. While keeping the error compensation term unchanged, the initial value of the covariance matrix is ​​dynamically set according to the current vehicle speed, and the covariance matrix of the Kalman filter is reset to the dynamically set initial value.

[0016] S7: Adjust the yaw angle of the main vehicle Subtract the trailer heading angle obtained after the first state reset to obtain the main trailer angle. .

[0017] Furthermore, the error compensation term includes the additive zero bias of the wheel speed observation. and proportional error factor At least one of them.

[0018] Furthermore, the detection logic for the stable straight-line driving state is as follows: the following conditions are met simultaneously and the duration exceeds a preset time threshold: the absolute value of the yaw rate of the main vehicle is less than the angular velocity threshold, the absolute value of the speed difference between the left and right wheels of the trailer is less than the wheel speed difference threshold, and the speed of the main vehicle is greater than the preset minimum driving speed threshold.

[0019] Furthermore, the road surface adhesion coefficient is estimated in real time by comparing the residual between the observed value of the trailer yaw rate and the value calculated by the kinematic model: the larger the residual, the lower the road surface adhesion coefficient is determined. When the estimated road surface adhesion coefficient is lower than the preset low adhesion threshold, the first state reset is prohibited.

[0020] The method of dynamically setting the initial value of the covariance matrix based on the current vehicle speed includes: the higher the vehicle speed, the smaller the initial variance assigned to the trailer's heading angle state.

[0021] Furthermore, the trailer wheelbase in S3 The wheelbase is obtained through pre-input or online calibration. Online calibration includes estimating the trailer wheelbase online using a recursive least squares method based on kinematic models and wheel speed observation data during vehicle turning. And the next time the first state reset is triggered, the updated trailer wheelbase will be used. Reinitialize the kinematic model parameters.

[0022] Furthermore, in S4, the trailer wheel track is obtained through online calibration: when the vehicle is in a stable straight-line driving state, the measured difference between the left and right wheel speeds of the trailer at the current moment is obtained, and the deviation between the measured difference and the theoretical value of zero is used as the error input. The trailer wheel track is estimated and corrected online by recursive least squares method, and the corrected trailer wheel track is fed back in real time to calculate the observed value of trailer yaw rate.

[0023] Furthermore, it also includes an adaptive noise covariance adjustment step: real-time estimation of the road surface adhesion coefficient, and adjustment of the observation noise covariance of the trailer yaw rate observation in the Kalman filter according to the road surface adhesion coefficient. When the road surface adhesion coefficient is lower than a preset threshold, the observation noise covariance is increased.

[0024] Furthermore, it also includes a second state reset: when the vehicle is detected to be stationary, the trailer heading angle state variable in the Kalman filter is kept unchanged, and the covariance matrix of the Kalman filter is reset to a preset initial value.

[0025] Furthermore, it also includes anomaly detection and fault tolerance steps: when the trailer's wheel speed signal is abnormal or communication is interrupted, it automatically switches to an open-loop estimation mode that only uses kinematic model prediction, and relies on the first state reset to suppress error divergence; when the tractor vehicle speed is obtained... and the main vehicle heading angle In case of anomalies, increase the process noise covariance of the Kalman filter.

[0026] On the other hand, a master-mounted hanger angle estimation system based on physical constraints and selective reset is provided to implement the aforementioned master-mounted hanger angle estimation method based on physical constraints and selective reset. The system includes:

[0027] The positioning and attitude measurement unit, installed on the main vehicle, is used to obtain the vehicle speed. and the main vehicle heading angle ;

[0028] Wheel speed signal acquisition module, used to acquire wheel speed signals of the left and right wheels of the trailer;

[0029] The controller is electrically connected to the positioning and attitude measurement unit and the wheel speed signal acquisition module, respectively. The controller includes:

[0030] The kinematic model module is used to build the kinematic model of the articulated vehicle and the trailer heading angle. The rate of change satisfies:

[0031] ;

[0032] Where, α The main hanging angle, This refers to the trailer wheelbase;

[0033] The trailer yaw rate observation calculation module is used to calculate the trailer yaw rate observation based on the speed difference between the left and right wheels of the trailer and the wheel track of the trailer.

[0034] The Kalman filter estimation module is equipped with a Kalman filter. The state vector of the Kalman filter includes at least the trailer heading angle and the error compensation term for wheel speed observation. The Kalman filter estimation module is used to establish a prediction equation based on the state vector and the kinematic model of the articulated vehicle, establish an observation equation based on the observed value of the trailer yaw rate, and estimate the trailer heading angle in real time.

[0035] The working condition detection and state reset module is used to detect the vehicle's driving conditions in real time. When it is detected that the vehicle is in a stable straight driving state and the current road surface adhesion coefficient is higher than the preset threshold, the forced reset logic is triggered to execute the first state reset: only the trailer heading angle in the state vector is forcibly assigned to the current master vehicle heading angle, while keeping the error compensation term unchanged, and the initial value of the covariance matrix is ​​dynamically set according to the current vehicle speed, and the covariance matrix of the Kalman filter is reset to the dynamically set initial value.

[0036] The main trailer angle calculation module is used to subtract the main vehicle heading angle from the trailer heading angle obtained after the first state reset to obtain the main trailer angle.

[0037] The beneficial effects of the technical solution provided by this invention are as follows: By utilizing the absolute physical constraint that the main trailer angle is zero during stable straight-line driving, selective reset is performed on the estimator—only the trailer heading angle is reset while maintaining the memory of the wheel speed error compensation term, fundamentally cutting off the accumulation path of wheel speed integral error. Compared with pure wheel speed integration and conventional EKF (without reset), this invention can control the peak fluctuation range of the estimation error to an extremely low level, significantly improving the estimation accuracy.

[0038] All hardware is either existing equipment on the main vehicle or mandatory configuration by national standards for trailers, eliminating the need for additional environmental perception sensors such as LiDAR and cameras. This results in extremely low system costs, making it particularly suitable for logistics scenarios where trailers are frequently changed. Furthermore, it operates reliably in all weather conditions, independent of environmental factors such as lighting, weather, or trailer appearance.

[0039] By calibrating trailer wheelbase and track width online, the system can automatically adapt to the geometric parameters of different trailers. Through road adhesion coefficient estimation and adaptive noise covariance adjustment, the weight of wheel speed observations can be automatically reduced on low-adhesion roads. Through reset suppression mechanisms and multiple anomaly detection and degradation modes, usable estimates can still be output even when some sensors fail. Furthermore, the algorithms employed, such as EKF recursion, operational condition detection, and RLS online calibration, can all run in real-time on the embedded onboard controller, resulting in low computational burden. Attached Figure Description

[0040] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0041] Figure 1 This is a schematic diagram of the hardware layout of a master-mounted bracket angle estimation system based on physical constraints and selective reset provided by the present invention;

[0042] Figure 2 This is a schematic diagram of the kinematic model and wheel speed observation of the articulated vehicle provided by the present invention;

[0043] Figure 3 This is a flowchart of the extended Kalman filter fusion process provided by the present invention;

[0044] Figure 4 This is a flowchart of the detection and state reset for straight-line driving and stationary conditions provided by the present invention;

[0045] Figure 5 This is a flowchart of the online calibration process for trailer wheelbase provided by the present invention.

[0046] Reference numerals: 1-GNSS antenna; 2-RTK-GNSS receiver and six-axis inertial measurement unit; 3-traction pin; 4-ISO7638 electrical interface. Detailed Implementation

[0047] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

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

[0049] Example 1: A method for estimating the main mounting angle based on physical constraints and selective reset, comprising the following steps:

[0050] Step (1): Obtain the speed of the main vehicle (tractor). and the main vehicle heading angle .

[0051] Step (2): Obtain the wheel speed signals of the left and right wheels of the trailer.

[0052] Step (3): Establish the kinematic model of the articulated vehicle, and determine the heading angle of the main vehicle. With trailer heading angle The difference between the two is defined as the main mounting angle. The distance from the towing pin to the center of the trailer's rear axle is the trailer wheelbase. Ignoring tire slip angle, the rate of change of trailer heading angle satisfies the following kinematic relationship:

[0053] ;

[0054] This model forms the basis of the Kalman filter prediction equation in this invention.

[0055] Step (4): Calculate the observed yaw rate of the trailer:

[0056] ;

[0057] in, These represent the speeds of the left and right wheels of the trailer, respectively, and B represents the wheel track of the trailer.

[0058] This observation directly reflects the measurement results of the trailer's yaw motion. Under ideal conditions (no slippage, no error), It should be equal to the value calculated by the kinematic model. However, in actual operating conditions, this observation includes wheel speed sensor noise, additive bias, proportional error, and possible wheel slippage. This invention uses it as the observation input to the extended Kalman filter, rather than directly performing open-loop integration, to avoid the unconditional accumulation of errors.

[0059] Step (5): Design of extended Kalman filter.

[0060] This embodiment employs an extended Kalman filter (EKF) to handle the nonlinear sinusoidal terms in the kinematic model. Furthermore, it optimally fuses the trailer wheel speed observations with the kinematic model to estimate the trailer heading angle in real time.

[0061] (1) Definition of state vector

[0062] To achieve joint estimation of the observation errors of trailer heading angle and wheel speed, the state vector of the extended Kalman filter is defined as a three-dimensional vector:

[0063] ;

[0064] The meanings of each state variable are as follows:

[0065] The trailer's heading angle is the core state to be estimated. The additive zero bias (unit: rad / s) for wheel speed observation is used to compensate for additive deviations caused by constant drift of wheel speed sensors, installation asymmetry, etc. This is a dimensionless proportional error compensation factor for wheel speed observation, used to compensate for multiplicative deviations caused by differences in tire rolling radius, wheel speed sensor calibration coefficient errors, etc.

[0066] (2) Prediction equation (time update)

[0067] Based on the aforementioned state vector and the kinematic relationship of the articulated vehicle, a prediction equation is established to describe the evolution of the state over time. The prediction equation is obtained by discretizing the kinematic model of the articulated vehicle. Sampling period. The selection of [a specific component] needs to consider both computational load and dynamic response requirements. In this embodiment, [a specific component] is selected. (i.e., a 100 Hz update frequency), consistent with the sampling frequency of the wheel speed signal. The discretized prediction equation is:

[0068] ;

[0069] in, , , For the current moment The prior estimate; , , For the previous moment The posterior estimate; The current vehicle speed (unit: m / s) is provided in real time by the positioning and attitude measurement unit. Trailer wheelbase (unit: m), the distance from the towing pin to the center of the trailer's rear axle, can be pre-input or obtained through online calibration; The heading angle of the main vehicle at the current moment (unit: rad) is provided in real time by the positioning and attitude measurement unit.

[0070] In the above prediction equations, the change in the trailer's heading angle is determined by kinematic relationships. Additive zero bias. and proportional error factor Then a random walk model is adopted, which assumes that the value changes slowly in a short period of time and the predicted value remains unchanged from the posterior estimate of the previous time step.

[0071] (3) Observation equation (measurement update):

[0072] The prediction equation provides a priori deductions of the state, but due to model parameter errors and the existence of unmodeled dynamics, relying solely on the prediction equation will cause the estimation results to gradually deviate from the true value over time. Therefore, the trailer wheel speed difference is introduced as external observation information, and an observation equation is established to correct the predicted state. Establish a mathematical relationship with the internal state variables of the filter. The observation equation takes the form:

[0073] ;

[0074] in, The current observation is the measurement input of the filter. The observed value of the trailer's yaw rate at the current moment is calculated from the speed difference between the left and right wheels of the trailer, and is obtained through wheel speed sensors; The wheel speed proportional error compensation factor at the current moment is the filter state variable; The current vehicle speed is measured in real time by the positioning and attitude measurement unit. The trailer wheelbase is obtained through pre-entry or online calibration. The heading angle of the main vehicle at the current moment is measured in real time by the positioning and attitude measurement unit; The prior estimate of the trailer heading angle at the current moment is output by the filter prediction step; The current wheel speed additive zero-bias compensation term is the filter state variable; To observe the noise, we assume it follows a zero-mean Gaussian distribution, with its covariance denoted as . .

[0075] The physical meaning of this observation equation is: under ideal conditions (no slippage, no sensor error), the observed value... It should be equal to the trailer yaw rate calculated by the kinematic model. However, due to the existence of multiplicative and additive errors in reality, (1+) is introduced. ) and It needs to be corrected.

[0076] (4) Recursive results of the extended Kalman filter

[0077] Based on the aforementioned prediction and observation equations, the Extended Kalman Filter (EPF) achieves optimal state estimation through a recursive approach. The EPF employs a cyclic recursive structure of "prediction-correction," alternating between the prediction and observation equations within each sampling period. The prediction equation, based on the articulated vehicle kinematics model, utilizes the state estimate from the previous moment and the current driver motion input to perform prior deductions of the trailer heading angle and its error compensation term. The observation equation, on the other hand, incorporates external measured information—the difference in trailer wheel speeds—to correct the prediction results. The two equations are weighted and fused through Kalman gain: when the confidence level of wheel speed observations is high, the filter relies more heavily on the observed values ​​to quickly respond to dynamic changes; when the confidence level of wheel speed observations is low (e.g., on low-adhesion surfaces), the filter relies more on model predictions to maintain estimation stability.

[0078] like Figure 3As shown, the above recursive structure specifically includes the following five core steps.

[0079] 1: Calculate the Jacobian matrix

[0080] Since both the prediction equation and the observation equation are nonlinear equations, their Jacobian matrices with respect to the state vectors must be calculated first to achieve local linearization of the system.

[0081] State transition function Defined by the prediction equation, its Jacobian matrix F is:

[0082] ;

[0083] Observation function Defined by the observation equation, its Jacobian matrix H is:

[0084] ;

[0085] 2: Forecast (Time Update)

[0086] After calculating the Jacobian matrix, the prediction step is performed, utilizing the posterior state estimate from the previous time step. and posterior covariance matrix Calculate the prior state estimate at the current time. and prior covariance matrix :

[0087] ;

[0088] ;

[0089] in, This is an estimate of the posterior state from the previous time step; Estimate the prior state at the current moment; Let be the posterior covariance matrix of the previous time step; Let be the prior covariance matrix at the current moment;

[0090] The process noise covariance matrix represents the uncertainty of the prediction model. In this embodiment, we take... ; Jacobian matrix The transpose of .

[0091] 3: Calculate the Kalman gain

[0092] After obtaining the prior estimate in the prediction step, the Kalman gain needs to be calculated to determine the correction weight of the observation residuals in subsequent state updates.

[0093] Using the prior covariance matrix Observation of the Jacobian matrix and observation noise covariance Calculate the Kalman gain matrix at the current time. :

[0094] ;

[0095] in: The Kalman gain matrix has a dimension of 3×1, and its components determine the correction weights of the observation residuals to the corresponding state variables. To observe the noise covariance, here is a one-dimensional scalar, representing the noise variance of the wheel speed observations; To observe the Jacobian matrix The transpose of .

[0096] 4: Update (Measurement Update)

[0097] After obtaining the Kalman gain, an update step is performed to correct the prior state using actual observations, thus obtaining the posterior state estimate.

[0098] Using actual observations Compared with predictive observations calculated based on prior states The residuals between the two states are used to correct the prior state, resulting in the posterior state estimate for the current time step. and posterior covariance matrix :

[0099] = +K ;

[0100] ;

[0101] in, This is the posterior state estimate for the current moment, i.e., the optimal estimate after fusing wheel speed observations; The predicted observations are calculated based on the prior state and the observation equation; - () represents the observation residual; It is a 3×3 identity matrix; The updated posterior covariance matrix reflects the level of uncertainty in the state estimation.

[0102] 5: Status Output

[0103] After completing the state update, the required variables are extracted from the posterior state vector as the estimated output for this moment.

[0104] Extracting the first component from the posterior state estimation vector yields the optimal estimate of the trailer's heading angle at the current moment:

[0105] ;

[0106] This estimate will be used for subsequent calculations of the main mounting angle.

[0107] (5) Filter initialization

[0108] The above recursive process requires an initial state and an initial covariance matrix as starting conditions. The initialization settings for this embodiment are given below.

[0109] At system startup, k=0, the state vector and covariance matrix need to be initialized. In this embodiment, the initial state vector is set as follows:

[0110] ;

[0111] That is, it is assumed that the initial angle between the main vehicle and the trailer is zero, the trailer heading angle is equal to the main vehicle heading angle, and the initial values ​​of zero bias and proportional error factor are set to zero.

[0112] The initial covariance matrix is ​​set to a diagonal matrix:

[0113] ;

[0114] in, Let be the initial variance of the trailer's heading angle. The initial variance is additive and zero-biased. The initial variance of the proportional error factor.

[0115] As the filter runs recursively, the state estimate will gradually converge to the true value, and the covariance matrix will also be dynamically adjusted according to the quality of the observation information.

[0116] Step (6): Operating condition detection and forced alignment reset.

[0117] The extended Kalman filter in step (5) effectively suppresses noise and tracks the dynamic changes in the trailer's heading angle in the short term by fusing kinematic model predictions with wheel speed observations. However, due to the non-zero mean of wheel speed observation noise, the truncation error in model linearization, and the convergence inertia of state estimation itself, even with additive zero bias in the filter... and proportional error factor Online estimation was performed for the trailer heading angle. Even after long-term operation, slow but irreversible cumulative drift will still occur. The reason is that, in the absence of an external absolute reference, the convergence process of the zero-biased estimate is itself a random walk process. Small fluctuations in this process will be continuously injected into the heading angle estimate through the feedback channel of the observation equation, forming a cumulative effect.

[0118] To address the aforementioned issues, this embodiment introduces a condition-triggered forced alignment reset mechanism. The core idea of ​​this mechanism is to utilize specific transient conditions that naturally exist during vehicle operation and have definite physical meaning to perform a deterministic reset of the internal state of the Kalman filter, thereby periodically cutting off the cumulative path of drift.

[0119] (1) Testing of stable straight-line driving conditions

[0120] When the vehicle is traveling in a stable straight line, according to the kinematic principles of articulated vehicles, the tractor and trailer are on the same straight line in steady state, and the angle α between the tractor and trailer must be approximately zero, i.e., the trailer heading angle. With the main vehicle's heading angle They are equal. This relationship is a physical fact determined by the vehicle's mechanical connection structure, rather than a probabilistic inference, and therefore can serve as an absolute basis for forcibly calibrating the estimator.

[0121] In this embodiment, the detection of stable straight-line driving conditions requires that all of the following conditions be met simultaneously, and the duration for which all conditions are met simultaneously exceeds a preset time threshold. (In this embodiment, 0.5 s is used):

[0122] ;

[0123] Among them, the angular velocity threshold Take 0.02 rad / s as the wheel speed difference threshold. Take 0.1 m / s as the minimum vehicle speed threshold. The speed is set to 3 m / s. The above threshold can be adjusted appropriately based on vehicle type and sensor accuracy.

[0124] (2) Real-time estimation of road surface adhesion coefficient

[0125] The road surface adhesion coefficient μ is an important criterion for determining the confidence level of wheel speed observations. This embodiment estimates the road surface adhesion coefficient online by comparing the residuals between the observed trailer yaw rate and the values ​​calculated by the kinematic model. :

[0126] ;

[0127] in, = The yaw rate of the trailer calculated from the kinematic model;

[0128] ϵ is the sensitivity coefficient, typically 0.5; ϵ is a small value used to prevent division by zero, typically... rad / s; These are the lower and upper limits of the adhesion coefficient, typically 0.2 and 1.0 respectively; sat(⋅) is the limiting function, restricting the output to [ Within the range.

[0129] The physical basis for this estimate is that when the road surface adhesion is good, the observed wheel speed and the model calculation should be highly consistent with each other, with small residuals. The value approaches 1.0; when road surface adhesion is poor and wheel slippage occurs, the deviation between wheel speed observation and model calculation increases. The corresponding reduction.

[0130] (3) First state reset (selective reset)

[0131] When the vehicle is detected to be traveling in a stable straight line, and the currently estimated road adhesion coefficient is... When the adhesion threshold is higher than a preset threshold (e.g., 0.4), a first state reset is triggered, and the following operations are performed:

[0132] Only the trailer heading angle in the state vector The value is forcibly assigned to the current driver's heading angle. The accumulated error in heading angle estimation is eliminated by directly using the physical constraint of α=0.

[0133] Preserve additive zero bias in the state vector and proportional error factor The design remains unchanged. The purpose of this design is that these two compensation terms reflect the systematic error characteristics of the wheel speed sensor (such as installation deviation, tire wear, etc.), which are stable in the short term and should not be erroneously zeroed out by forced correction of the heading angle. Retaining them avoids the process of filter re-convergence after reset, ensuring the continuity of the estimation.

[0134] The initial value of the covariance matrix is ​​dynamically set based on the current vehicle speed, and the covariance matrix of the Kalman filter is reset to this dynamically set initial value. Specifically, the higher the vehicle speed, the smaller the initial variance assigned to the trailer's heading angle state. For example, in this embodiment, when the vehicle speed v ≤ 10 km / h, the initial variance is taken as... When the vehicle speed v > 10 km / h, take The initial variances of the additive zero bias and the proportional error factor remain unchanged from their calibration values. , The dynamically set initial covariance matrix is ​​denoted as... .

[0135] Therefore, the mathematical representation of the first state reset is:

[0136] , ;

[0137] The physical significance of the aforementioned reset operation lies in the fact that, at the deterministic moment of stable straight-line driving, the state estimate is directly changed by assignment using the absolute physical constraint of α=0, which is equivalent to performing a "zeroing" operation on the drift accumulation process. Since straight-line driving conditions occur frequently in normal transportation (such as highway cruising and urban road driving), this reset mechanism can intervene periodically, thereby reducing the long-term drift rate by more than two orders of magnitude. At the same time, retaining the error compensation term means that the filter does not need to reconverge the sensor error, ensuring the smoothness of the estimation; dynamically setting the covariance avoids overly conservative estimation behavior at high speeds or overly aggressive estimation behavior at low speeds.

[0138] (4) Reset the suppression mechanism

[0139] To avoid incorrect corrections due to wheel slippage on low-adhesion surfaces, this embodiment introduces a reset suppression mechanism. When the estimated road adhesion coefficient... When the adhesion threshold is below a preset threshold (e.g., 0.3), the system automatically prohibits the first-state reset even if all kinematic conditions for stable straight-line driving are met. This is because on low-adhesion surfaces (ice, snow, slippery surfaces) or in wheel slippage conditions, the linear relationship between wheel speed and vehicle speed is disrupted. In this case, forcibly correcting the trailer's heading angle using the tractor's heading angle may introduce larger errors. Through this suppression mechanism, the system only uses the physical constraints of straight-line driving for reset when the wheel speed observation is reliable (high adhesion), ensuring the robustness of the algorithm.

[0140] (5) Second state reset (static condition)

[0141] When the vehicle is stationary, the trailer heading angle should remain constant. Although this condition does not provide an absolute reference value for the heading angle, the physical constraint that "the state should not change" can be used to prevent the filter from experiencing covariance divergence or state drift due to a lack of effective observations during long periods of stationary operation.

[0142] In this embodiment, the detection conditions for a static operating condition are: the absolute value of the vehicle speed < 0.1 m / s, and the duration exceeds 1.0 s. When a static operating condition is detected, a second state reset is performed: the trailer heading angle state variable in the Kalman filter remains unchanged, and only the covariance matrix is ​​reset to a preset initial value P0 (fixed value). Its mathematical expression is:

[0143] , ;

[0144] The purpose of this operation is to prevent the covariance matrix from continuously increasing (diverging) during a long period of vehicle stationary operation due to a lack of effective observation information, which would affect the convergence speed of the estimation during subsequent driving.

[0145] like Figure 4 As shown, the above-mentioned working condition detection and state reset process is executed in parallel with the EKF recursion in each sampling period, forming a closed loop of "estimation-detection-reset-re-estimation". Through this mechanism, the present invention achieves fundamental suppression of error divergence with a minimal hardware configuration that only utilizes the tractor positioning and attitude measurement unit and the trailer's existing ABS wheel speed sensors, further improving the system's robustness under complex working conditions.

[0146] Step (7): Output the main hanging angle.

[0147] Main vehicle heading angle Compared with the filtered and reset estimated trailer heading angle Subtracting the two values ​​yields the main mounting angle, which is then normalized (converted to the range of [−180∘,+180∘] or [0∘,360∘]) and used as the final output for use by the upper-level control system.

[0148] To verify the technical effectiveness of this invention, a real-vehicle comparative test was conducted on a dry asphalt road surface. Test conditions: vehicle speed 30 km / h, driving on a continuous S-shaped curve for 30 minutes. Using the output of a high-precision angle sensor installed at the towing pin as the true value reference, the estimation accuracy of the following three schemes was compared:

[0149] It should be noted that, due to the continuous S-curve conditions used in this test, the vehicle's steering direction frequently changes, resulting in significant periodic fluctuations in the estimation errors of both the pure integral and non-reset EKF methods. To accurately reflect this dynamic characteristic, the "30-minute fluctuation range" in the table below refers to the peak-to-peak value of the estimation error over the entire test period (i.e., the difference between the 98th percentile and the 2nd percentile), used to measure the overall uncertainty and divergence tendency of the estimation results.

[0150] Pure wheel speed integration (Comparison Group 1) 2.3° 7.5° Approximately 15° No EKF reset (Comparison Group 2) 1.3° 3.8° Approximately 3.8° This invention (EKF+ state reset) 0.9° 2.8° <0.7°

[0151] Test results show that:

[0152] The pure wheel speed integral scheme exhibits significant periodic fluctuations in error under continuous S-curve conditions, with a peak-to-peak fluctuation range of approximately 15° over 30 minutes, indicating that it is highly susceptible to severe divergence due to steering conditions. The non-reset EKF scheme reduces the fluctuation range to approximately 3.8° by fusing kinematic models and online zero-bias estimation, but significant periodic oscillations still exist, indicating that model fusion alone cannot completely suppress error accumulation. This invention further suppresses peak-to-peak fluctuations to below 0.7° by introducing a forced alignment reset mechanism under working conditions, while keeping MAE and maximum error at extremely low levels (0.9° and 2.8°, respectively), achieving high-precision and high-stability angle estimation over a long period.

[0153] It should be noted that the trailer wheelbase The wheelbase is a core parameter in the kinematic prediction equation, and its accuracy directly affects the calculation accuracy of the trailer heading angle change rate, thus affecting the reliability of the extended Kalman filter prediction step. In practical applications, a tractor may tow trailers of different specifications, with significant differences in wheelbase between different trailers; even for the same trailer, changes in tire rolling radius due to load distribution can cause slight changes in its equivalent wheelbase parameter. Requiring the driver to manually input the wheelbase parameter each time a trailer is changed not only increases the operational burden but also makes it difficult to guarantee the accuracy of the input. Therefore, this embodiment provides an online trailer wheelbase calibration method that automatically estimates and updates the wheelbase using the kinematic relationship during vehicle turning. This value enables the system to adaptively adapt to different trailers.

[0154] (1) Determine the triggering conditions

[0155] Online calibration of the trailer wheelbase needs to be performed under conditions where the vehicle experiences significant yaw motion, because the sine term in the kinematic model only becomes relevant during cornering. Only then will it have a sufficiently large amplitude so that the effect of the wheelbase parameter on the yaw rate can be reliably observed. If calibrated under straight-line driving conditions, α≈0. The yaw rate calculated by the kinematic model approaches zero, the wheelbase parameter is almost unobservable in the observation equation, and the calibration results will be severely affected by noise.

[0156] In this embodiment, the calibration trigger condition is set as: absolute value of the yaw rate of the main vehicle | | Greater than the preset threshold And the duration exceeds .in, Take 0.05 rad / s, A threshold of 0.5 s is used. This threshold ensures that the vehicle has entered a stable turning state, and the yaw motion component in the kinematic relationship is sufficiently significant.

[0157] (2) Calibration principle and error function construction

[0158] During the vehicle's turning process, the extended Kalman filter outputs an estimate of the trailer's heading angle in real time. Combined with the main vehicle's heading angle The main mounting angle can be calculated. Based on the kinematic model of the articulated vehicle, the theoretical value (model-estimated value) of the trailer's yaw rate is:

[0159] ;

[0160] On the other hand, the yaw rate observation provided by the speed difference between the left and right wheels of the trailer is:

[0161] ;

[0162] Under ideal conditions where there is no significant wheel slippage and the sensors are functioning correctly, the two yaw rates mentioned above should be equal. This depends on the currently used trailer wheelbase parameters. Compared to the actual wheelbase If a bias exists, a systematic error will occur between the model's calculated values ​​and the observed values. Based on this, an error function is constructed:

[0163] ;

[0164] The goal of calibration is to adjust the wheelbase estimate. This minimizes the sum of squares of the error function.

[0165] (3) Online estimation based on recursive least squares method

[0166] To achieve online real-time calibration, this embodiment uses the recursive least squares (RLS) method to calibrate the trailer wheelbase. Perform recursive estimation.

[0167] First, the error function is rewritten into a linear parameterized form suitable for RLS. The observations are defined. Regression quantity The parameter to be estimated is θ = 1 / Then the model relationship can be expressed as:

[0168]

[0169] The recursive least squares method with a forgetting factor is used, and the recursive formula is as follows:

[0170] ;

[0171] ;

[0172] ;

[0173] in: The RLS gain determines the magnitude of the correction to the parameter estimate by the current observation; The covariance of the parameter estimate reflects the uncertainty of the current estimate; λ is the forgetting factor, with a value range of 0 < λ ≤ 1, and in this embodiment, λ = 0.98. The role of the forgetting factor is to assign exponentially decaying weights to historical data, enabling the estimator to adapt to gradual changes in wheelbase parameters (such as equivalent changes caused by tire wear); This is the observed yaw rate at the current moment; This represents the regression value at the current moment; The parameter estimates at the current moment are used to obtain the wheelbase estimate. (k) = 1 / RLS initialization: During system startup, It can be set to a preset nominal wheelbase. The reciprocal of, that is Initial covariance It can be set to a large value (such as 10000) to indicate a low confidence level in the initial estimate, enabling the algorithm to converge quickly in the initial stage.

[0174] (4) Smoothing and application of calibration results

[0175] Since wheel speed observation noise and road surface irregularities may cause instantaneous fluctuations in the RLS estimation results, this embodiment performs first-order low-pass filtering smoothing on the calibration output:

[0176] ;

[0177] Among them, the smoothing coefficient We set it to 0.05 to balance response speed and smoothing effect.

[0178] Smoothed wheelbase estimate The prediction equations of the extended Kalman filter are updated in real time, replacing the original ones. Parameters. Furthermore, when the first state reset (forced alignment during straight-line driving) is subsequently triggered, the filter will reinitialize the kinematic model based on the updated wheelbase parameters to ensure that the model parameters remain consistent with physical reality.

[0179] Calibration quality assessment and protection mechanism: To improve the robustness of the system, this embodiment also introduces a quality assessment of the calibration results. The wheelbase parameter update is paused and the current value is maintained when any of the following conditions are met:

[0180] RLS parameter covariance It has not yet converged to below the preset threshold (e.g.) >0.1), indicating that the estimate has not yet stabilized; the current estimate is... Exceeding a reasonable range (e.g., less than 3 m or greater than 15 m) may lead to estimation distortion due to sensor malfunction or extreme operating conditions; estimated road surface adhesion coefficient. A value below 0.4 indicates that there may be significant wheel slippage, and the underlying assumptions of the kinematic model no longer hold.

[0181] like Figure 5 As shown, the online calibration process for trailer wheelbase runs in parallel with the main estimation process, and the wheelbase parameters are updated only when a turning condition is detected and the quality assessment conditions are met.

[0182] The above online calibration method can automatically acquire and continuously correct trailer wheelbase parameters during normal vehicle operation without manual intervention, achieving plug-and-play adaptive capability for different trailers.

[0183] It should also be noted that in the observation equation, the calculation of the trailer yaw rate observation depends on the trailer wheelbase parameter B. If this parameter has an error, it will directly affect the accuracy of the observation. Therefore, this embodiment provides an online calibration method for trailer wheelbase. Unlike trailer wheelbase calibration, trailer wheelbase calibration needs to be performed under stable straight-line driving conditions. Under these conditions, the trailer does not yaw, and the theoretical value of the left and right wheel speed difference should be zero. If the actual measured wheel speed difference... If the value is non-zero, it mainly originates from the estimation error of the wheelbase parameter. The same recursive least squares (RLS) framework as described above is used for online estimation, with the difference being:

[0184] Triggering condition: The first alignment condition (straight-line driving) defined in step (6) is adopted; Observations and regressions: Observations Regression quantity Parameters to be estimated: .

[0185] The RLS recursive formula and smoothing filtering strategy are consistent with the calibration described above, and will not be repeated here. Obtain the wheelbase estimate. Then, it is updated in real time to the calculation in step (4). Updates will be paused when the estimated value exceeds a reasonable range (e.g., 1.5m~2.5m) or the road surface adhesion coefficient is too low.

[0186] It should also be noted that in the above extended Kalman filter, the observation noise covariance... Wheel speed is typically set to a fixed constant. However, in actual driving, the quality of wheel speed observations changes significantly with road conditions. On dry, high-friction surfaces, the linear relationship between wheel speed and vehicle speed is good, and the confidence level of wheel speed observations is high. But on low-friction surfaces such as ice, snow, or wet surfaces, or when the vehicle is under emergency braking, the wheels are prone to longitudinal slippage. In these situations, the relationship between wheel speed and vehicle speed is disrupted, and the yaw rate calculated from the wheel speed difference will contain a large error.

[0187] If wheel speed observations are still adopted with a fixed confidence level under such working conditions, the estimation accuracy will decrease or even diverge. Therefore, an optional adaptive mechanism is provided to adjust the observation noise covariance in real time based on the road surface adhesion coefficient. This allows the filter to automatically reduce the weight of wheel speed observations on low-adhesion surfaces, relying more on kinematic model predictions.

[0188] (1) Estimation of road surface adhesion coefficient (same as in step (6)).

[0189] (2) Adaptive mapping of observation noise covariance

[0190] The estimated adhesion coefficient is mapped to the observation noise covariance:

[0191]

[0192] in: The nominal observed noise covariance was obtained by calibration on a dry, high-adhesion road surface. The amplification factor controls the maximum amplification of noise covariance under low-adhesion road surfaces; a typical value is 1.5. The attenuation coefficient controls the steepness of the change in noise covariance with the adhesion coefficient; a typical value is 5.0.

[0193] The working characteristic of this mapping relationship is: when →1 (high adhesion), →0, → Wheel speed observations participate in fusion with nominal confidence; when →0.2 (low adhesion), Increase Significantly amplified, the Kalman gain decreases during the update step of the extended Kalman filter, thereby automatically reducing the correction weight of wheel speed observations, making the filter rely more on kinematic model predictions, and effectively suppressing observation distortion caused by slippage.

[0194] (3) Collaboration with other modules

[0195] The adaptive mechanism runs as a background module, calculating in each sampling period. And update This is used in the EKF update step of step (5). Additionally, the obtained adhesion coefficient is estimated. It can also be used for the reset suppression judgment in step (6) and the above calibration quality assessment to achieve information sharing.

[0196] Example 2

[0197] A master-mounted hanger angle estimation system based on physical constraints and selective reset is provided to implement the master-mounted hanger angle estimation method based on physical constraints and selective reset in Embodiment 1, such as... Figure 1 As shown, a dual GNSS antenna 1 is mounted on the top of the tractor (main vehicle), and an RTK-GNSS receiver and a six-axis inertial measurement unit (IMU) 2 are mounted in the middle of the chassis, together forming a positioning and attitude measurement unit. This positioning and attitude measurement unit can output the absolute position of the main vehicle, the main vehicle's heading angle ψt, the main vehicle's speed v, and the yaw rate ωt in real time, with a sampling frequency of 100 Hz. In this embodiment, the vehicle speed, heading angle, and yaw rate are mainly used as inputs for subsequent algorithms.

[0198] The trailer is equipped with ABS wheel speed sensors, which transmit the wheel speed signals of the left and right wheels of the trailer via the ISO 7638 electrical interface 4 between the main trailer and the trailer. and The signal is transmitted to the wheel speed signal acquisition module. This interface is a standard interface, requiring no additional hardware on the trailer side, achieving zero deployment on the trailer side. The wheel speed signal sampling frequency is also 100 Hz.

[0199] The main vehicle controller is electrically connected to the positioning and attitude measurement unit and the wheel speed signal acquisition module. The controller includes: a kinematic model module, a trailer yaw rate observation calculation module, a Kalman filter estimation module, a working condition detection and state reset module, and a main-trailer angle calculation module.

[0200] The kinematic model module is used to build the kinematic model of the articulated vehicle, including the trailer heading angle. The rate of change satisfies:

[0201] ;

[0202] Where, α The main hanging angle, is the trailer wheelbase, and is the distance from the towing pin 3 to the center of the trailer rear axle.

[0203] The trailer yaw rate observation calculation module is used to calculate the trailer yaw rate based on the speed of the left and right wheels of the trailer. And the trailer wheel track B, calculate the observed value of the trailer yaw rate:

[0204] ;

[0205] The Kalman filter estimation module is equipped with a Kalman filter. The state vector of the Kalman filter includes at least the error compensation terms for the trailer heading angle and wheel speed observations. The Kalman filter estimation module is used to establish prediction equations based on the state vector and the kinematic model of the articulated vehicle, establish observation equations based on the observed values ​​of the trailer yaw rate, and estimate the trailer heading angle in real time.

[0206] The operating condition detection and state reset module is used to detect the vehicle's driving conditions in real time. When it is detected that the vehicle is in a stable straight driving state and the current road surface adhesion coefficient is higher than the preset threshold, the forced reset logic is triggered to execute the first state reset: only the trailer heading angle in the state vector is forcibly assigned to the current master vehicle heading angle, while keeping the error compensation term unchanged. The initial value of the covariance matrix is ​​dynamically set according to the current vehicle speed, and the covariance matrix of the Kalman filter is reset to the dynamically set initial value.

[0207] The main trailer angle calculation module is used to subtract the main vehicle heading angle from the trailer heading angle obtained after the first state reset to obtain the main trailer angle.

[0208] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0209] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for estimating the main mounting angle based on physical constraints and selective reset, characterized in that, The method includes: S1: Get the main vehicle speed and the main vehicle heading angle ; S2: Obtain the wheel speed signals of the left and right wheels of the trailer; S3: Establish the kinematic model of the articulated vehicle, trailer heading angle The rate of change satisfies: Where, α The main hanging angle, This refers to the trailer wheelbase. S4: Calculate the observed value of the trailer's yaw rate based on the speed difference between the left and right wheels of the trailer and the wheel track of the trailer; S5: Employ a Kalman filter, defining its state vector to include at least the trailer heading angle. In addition to the error compensation term for wheel speed observation; a prediction equation is established based on the state vector and kinematic model, and an observation equation is established using the observed values ​​of the trailer yaw rate to estimate the trailer heading angle in real time. ; S6: Real-time detection of vehicle driving conditions. When the vehicle is detected to be in a stable straight-line driving state and the current road surface adhesion coefficient is higher than a preset threshold, a forced reset logic is triggered, executing the first state reset: only the trailer heading angle in the state vector is reset. The value is forcibly assigned to the current driver's heading angle. While keeping the error compensation term unchanged, the initial value of the covariance matrix is ​​dynamically set according to the current vehicle speed, and the covariance matrix of the Kalman filter is reset to the dynamically set initial value. S7: Adjust the yaw angle of the main vehicle Subtract the trailer heading angle obtained after the first state reset to obtain the main trailer angle. .

2. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, The error compensation term includes the additive zero bias of wheel speed observation. and proportional error factor At least one of them.

3. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, The detection logic for the stable straight-line driving state is as follows: The following conditions must be met simultaneously and the duration must exceed a preset time threshold: the absolute value of the yaw rate of the main vehicle is less than the angular rate threshold, the absolute value of the speed difference between the left and right wheels of the trailer is less than the wheel speed difference threshold, and the speed of the main vehicle is greater than the preset minimum driving speed threshold.

4. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, The road surface adhesion coefficient is estimated in real time by comparing the residual between the observed value of the trailer yaw rate and the value calculated by the kinematic model: the larger the residual, the lower the road surface adhesion coefficient is determined. When the estimated road surface adhesion coefficient is lower than the preset low adhesion threshold, the first state reset is prohibited. The method of dynamically setting the initial value of the covariance matrix based on the current vehicle speed includes: the higher the vehicle speed, the smaller the initial variance assigned to the trailer's heading angle state.

5. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, The trailer wheelbase in S3 The wheelbase is obtained through pre-input or online calibration. Online calibration includes estimating the trailer wheelbase online using a recursive least squares method based on kinematic models and wheel speed observation data during vehicle turning. And the next time the first state reset is triggered, the updated trailer wheelbase will be used. Reinitialize the kinematic model parameters.

6. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, The trailer wheel track in S4 is obtained through online calibration: when the vehicle is in a stable straight driving state, the measured difference between the left and right wheel speeds of the trailer at the current moment is obtained. The deviation between the measured difference and the theoretical value of zero is used as the error input. The trailer wheel track is estimated and corrected online by recursive least squares method. The corrected trailer wheel track is fed back in real time to calculate the observed value of trailer yaw rate.

7. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, It also includes an adaptive noise covariance adjustment step: real-time estimation of the road surface adhesion coefficient, and adjustment of the observation noise covariance of the trailer yaw rate observation in the Kalman filter based on the road surface adhesion coefficient. When the road surface adhesion coefficient is lower than a preset threshold, the observation noise covariance is increased.

8. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, It also includes a second state reset: when the vehicle is detected to be stationary, the trailer heading angle state variable in the Kalman filter is kept unchanged, and the covariance matrix of the Kalman filter is reset to a preset initial value.

9. The method for estimating the main mounting angle based on physical constraints and selective reset according to claim 1, characterized in that, It also includes anomaly detection and fault tolerance steps: when the trailer's wheel speed signal is abnormal or communication is interrupted, it automatically switches to an open-loop estimation mode that only uses kinematic model prediction, and relies on the first state reset to suppress error divergence; when the tractor's speed is obtained... and the main vehicle heading angle In case of anomalies, increase the process noise covariance of the Kalman filter.

10. A master-mounted bracket angle estimation system based on physical constraints and selective reset, characterized in that, The system is used to implement the main mounting angle estimation method based on physical constraints and selective reset as described in any one of claims 1 to 9, the system comprising: The positioning and attitude measurement unit, installed on the main vehicle, is used to obtain the vehicle speed. and the main vehicle heading angle ; Wheel speed signal acquisition module, used to acquire wheel speed signals of the left and right wheels of the trailer; The controller is electrically connected to the positioning and attitude measurement unit and the wheel speed signal acquisition module, respectively. The controller includes: The kinematic model module is used to build the kinematic model of the articulated vehicle and the trailer heading angle. The rate of change satisfies: Where, α The main hanging angle, This refers to the trailer wheelbase. The trailer yaw rate observation calculation module is used to calculate the trailer yaw rate observation based on the speed difference between the left and right wheels of the trailer and the wheel track of the trailer. The Kalman filter estimation module is equipped with a Kalman filter. The state vector of the Kalman filter includes at least the trailer heading angle and the error compensation term for wheel speed observation. The Kalman filter estimation module is used to establish a prediction equation based on the state vector and the kinematic model of the articulated vehicle, establish an observation equation based on the observed value of the trailer yaw rate, and estimate the trailer heading angle in real time. The working condition detection and state reset module is used to detect the vehicle's driving conditions in real time. When it is detected that the vehicle is in a stable straight driving state and the current road surface adhesion coefficient is higher than the preset threshold, the forced reset logic is triggered to execute the first state reset: only the trailer heading angle in the state vector is forcibly assigned to the current master vehicle heading angle, while keeping the error compensation term unchanged, and the initial value of the covariance matrix is ​​dynamically set according to the current vehicle speed, and the covariance matrix of the Kalman filter is reset to the dynamically set initial value. The main trailer angle calculation module is used to subtract the main vehicle heading angle from the trailer heading angle obtained after the first state reset to obtain the main trailer angle.

Images