Trajectory tracking control method and device for multi-axle heavy-load articulated vehicle

Abstract

The invention relates to a trajectory tracking control method and device for a multi-axle heavy-load articulated vehicle, and belongs to the field of trajectory tracking control in the unmanned driving field. The method comprises the following steps: constructing a dynamics and kinematics coupling model of the multi-axle heavy-load articulated vehicle; estimating the dynamic characteristics of the vehicle and the position of the vehicle based on a layered unscented Kalman filtering method, and respectively obtaining a state parameter estimation value and a position parameter estimation value of the vehicle; a vehicle error state space model is constructed based on the state parameter estimation value and the position parameter estimation value of the vehicle and the state space expression of the reference trajectory by taking the steering angle of each shaft as a control quantity, and the vehicle error state space model is discretized; building a prediction model based on the discretized vehicle error state space model; and performing tracking control on the trajectory of the multi-axle heavy-load articulated vehicle by using the prediction model. According to the invention, the track of the multi-axle heavy-load articulated vehicle can be accurately tracked, and the stability of the multi-axle heavy-load articulated vehicle in the driving process is ensured.

Description

Technical Field

[0001] The present invention relates to a trajectory tracking control method and device for a multi-axle heavy-duty articulated vehicle, belonging to the field of trajectory tracking control in the field of driverless driving. Background Art

[0002] In the field of modern industrial transportation, multi-axle heavy-duty articulated vehicles have become indispensable transportation tools in large industrial parks such as mining areas, tunnels, and ports due to their strong load-bearing capacity, high transportation efficiency, and good environmental adaptability. At the same time, with the continuous breakthrough of driverless technology, multi-axle heavy-duty articulated vehicles are also developing towards intelligence. Among them, trajectory tracking technology is the key technology to achieve driverless driving and has been widely applied in passenger cars. However, during the actual driving process of multi-axle heavy-duty articulated vehicles, due to their large mass, long body, and complex structure, when using classical control algorithms such as traditional model predictive control (MPC) and linear quadratic regulator (LQR), there will be a large deviation between the actual driving trajectory and the reference trajectory during trajectory tracking, and even unstable phenomena such as rollover and skidding may occur, affecting stability and safety. Therefore, there is an urgent need for a trajectory tracking control method for multi-axle heavy-duty articulated vehicles to solve the problems of large errors during trajectory tracking and low stability during the driving process in the prior art. Summary of the Invention

[0003] The purpose of the present invention is to provide a trajectory tracking control method and device for a multi-axle heavy-duty articulated vehicle to solve the problems of large errors during trajectory tracking and low stability during the driving process in the prior art for multi-axle heavy-duty articulated vehicles.

[0004] To achieve the above purpose, the solution of the present invention includes:

[0005] A trajectory tracking control method for a multi-axle heavy-duty articulated vehicle of the present invention includes the following steps:

[0006] Step 1): Construct a dynamic and kinematic coupling model of the multi-axle heavy-duty articulated vehicle;

[0007] Step 2): Based on the coupling model, use hierarchical unscented Kalman filtering to estimate the vehicle dynamic characteristics and vehicle position, and respectively obtain the estimated values of the vehicle state parameters and position parameters;

[0008] Step 3): Taking the steering angles of each axis as control variables, construct a vehicle error state space model based on the estimated values of the vehicle state parameters, position parameters, and the state space expression of the reference trajectory, and discretize the vehicle error state space model;

[0009] Step 4), construct a prediction model based on the discretized vehicle error state space model, and use the prediction model to perform trajectory tracking control on the multi-axle heavy-duty articulated vehicle.

[0010] Furthermore, the dynamic model of the multi-axle heavy-duty articulated vehicle is determined by the following formula:

[0011]

[0012] Among them, the multi-axle heavy-duty articulated vehicle includes a front vehicle and a rear vehicle; m f represents the mass of the front vehicle; m r represents the mass of the rear vehicle; β f represents the sideslip angle of the front vehicle's center of mass; β r represents the sideslip angle of the rear vehicle's center of mass; ω1 represents the yaw angular velocity of the front vehicle's center of mass; ω2 represents the yaw angular velocity of the rear vehicle's center of mass; u1 represents the longitudinal velocity of the front vehicle; u2 represents the longitudinal velocity of the rear vehicle; a1 represents the distance from the center of mass of the front vehicle to the first axle; a2 represents the distance from the center of mass of the rear vehicle to the sixth axle; b1 represents the distance from the center of mass of the front vehicle to the second axle; b2 represents the distance from the center of mass of the rear vehicle to the fifth axle; c1 represents the distance from the center of mass of the front vehicle to the third axle; c2 represents the distance from the center of mass of the rear vehicle to the fourth axle; d1 represents the distance from the center of mass of the front vehicle to the articulation point; d2 represents the distance from the center of mass of the rear vehicle to the articulation point; δ i represents the rotation angle of the i-th axle; F yi represents the lateral force acting on the i-th axle; i = 1, 2, 3, 4, 5, 6; F T represents the force at the articulation point; I zf represents the moment of inertia of the front vehicle about the Z axis; I zr represents the moment of inertia of the rear vehicle about the Z axis; is the articulation angle;

[0013] Among them, the first axle, the second axle, and the third axle are the three axles of the front vehicle; the fourth axle, the fifth axle, and the sixth axle are the three axles of the rear vehicle.

[0014] Furthermore, the kinematic model of the multi-axle heavy-duty articulated vehicle includes: the kinematic model of the front vehicle of the multi-axle heavy-duty articulated vehicle and the kinematic model of the rear vehicle of the multi-axle heavy-duty articulated vehicle;

[0015] Among them, the kinematic model of the front vehicle is determined by the following formula:

[0016]

[0017] The kinematic model of the rear vehicle is determined by the following formula:

[0018]

[0019] Among them, X fIndicates the lateral position of the leading vehicle; Y f Indicates the longitudinal position of the leading vehicle; X r Indicates the lateral position of the trailing vehicle; Y r Indicates the longitudinal position of the trailing vehicle; ψ f Indicates the heading angle of the leading vehicle; ψ r Indicates the heading angle of the trailing vehicle.

[0020] Furthermore, F yi needs to satisfy the linear relationship shown in the following formula:

[0021] F yi = k i α i

[0022] where k i represents the cornering stiffness of the i-th axle; α i represents the cornering angle of the tire of the i-th axle.

[0023] Furthermore, step 2) includes: at the first layer, sending the steering angle δi of each axle, the lateral acceleration aF of the leading vehicle, the lateral acceleration aR of the trailing vehicle, and the acting force F at the articulation point T to the dynamic model of the multi-axle heavy-duty articulated vehicle to calculate the estimated values of the vehicle's state parameters; then sending the estimation results of the first layer to the second layer, and based on the kinematic model of the multi-axle heavy-duty articulated vehicle, calculating the estimated values of the vehicle's position parameters;

[0024] where the lateral acceleration aF of the leading vehicle is determined by the following formula:

[0025]

[0026] The lateral acceleration aR of the trailing vehicle is determined by the following formula:

[0027]

[0028] where the estimated values of the vehicle's state parameters include: the estimated value β of the centroid slip angle of the leading vehicle fg , the estimated value β of the centroid slip angle of the trailing vehicle rg , the estimated value ω of the centroid yaw rate of the leading vehicle 1g and the estimated value ω of the centroid yaw rate of the trailing vehicle 2g ; the estimated values of the vehicle's position parameters include: the estimated value X of the lateral position of the leading vehicle of the multi-axle heavy-duty articulated vehicle fg , the estimated value Y of the longitudinal position of the leading vehicle fg , the estimated value X of the lateral position of the trailing vehicle of the multi-axle heavy-duty articulated vehicle rg and the estimated value Y of the longitudinal position of the trailing vehicle rg .

[0029] Further, the steering angle δi of each axis is obtained through an angle sensor; the longitudinal speed u1 of the vehicle in front and the longitudinal speed u2 of the vehicle behind are obtained through a speed sensor; and the acting force F at the hinge point is obtained through a force sensor. T 。

[0030] Further, step 3) includes: constructing a system state space expression according to the estimated values of the state parameters of the vehicle and the estimated values of the position parameters of the vehicle.

[0031] The control quantity of the system state space expression is the steering angle of each axis; the state space expression of the vehicle error state space model is obtained by subtracting the state space expression of the reference trajectory from the system state space expression.

[0032] Further, the forward Euler method is used to discretize the vehicle error state space model.

[0033] Further, in step 4), based on the discretized vehicle error state space model, a prediction model is constructed using a model predictive control algorithm.

[0034] A trajectory tracking control device for a multi-axis heavy-duty articulated vehicle according to the present invention includes a processor, and the processor executes a computer program to implement the method steps of the trajectory tracking control method for a multi-axis heavy-duty articulated vehicle as described above.

[0035] The beneficial effects of the present invention are as follows: As a pioneering invention, the present invention provides a trajectory tracking control method and device for a multi-axis heavy-duty articulated vehicle. First, by constructing a dynamic and kinematic coupling model of the multi-axis heavy-duty articulated vehicle, the state information and position information of the multi-axis heavy-duty articulated vehicle can be mastered simultaneously. Then, based on the hierarchical unscented Kalman filter method, the dynamic characteristics and position of the vehicle are estimated to obtain the estimated values of the state parameters and position parameters of the vehicle respectively, which can reduce the estimation error and improve the estimation accuracy. Subsequently, with the steering angle of each axis as the control quantity, a vehicle error state space model is constructed based on the estimated values of the state parameters of the vehicle, the estimated values of the position parameters, and the state space expression of the reference trajectory, and the vehicle error state space model is discretized. A prediction model is constructed based on the discretized vehicle error state space model, and the prediction model is used to track and control the trajectory of the multi-axis heavy-duty articulated vehicle, which can comprehensively consider the vehicle state and vehicle position information, realize accurate prediction and optimal control of the next moment, and ensure stability, accuracy, and robustness under complex working conditions. Description of the Drawings

[0036] Figure 1 is a schematic flow chart of a trajectory tracking control method for a multi-axis heavy-duty articulated vehicle provided by an embodiment of the present invention;

[0037] Figure 2It is a dynamic model of a multi-axle heavy-duty articulated vehicle provided by an embodiment of the present invention;

[0038] Figure 3 It is a schematic diagram of a hierarchical unscented Kalman filter estimation module provided by an embodiment of the present invention. Detailed implementation manners

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

[0040] The concept of the present invention is as follows: If only the dynamic model of the multi-axle heavy-duty articulated vehicle is used, the lateral and longitudinal position information of the vehicle cannot be obtained. If only the kinematic model is used, the state of the vehicle cannot be accurately obtained. Therefore, by establishing a coupled model of three-degree-of-freedom dynamics and kinematics, the position and state of the multi-axle heavy-duty articulated vehicle can be mastered, and then the coupled model and the reference trajectory are used to achieve an accurate and stable trajectory tracking effect for the multi-axle heavy-duty articulated vehicle.

[0041] Specifically: Construct a coupled model of dynamics and kinematics of a multi-axle heavy-duty articulated vehicle; Estimate the dynamic characteristics and position of the vehicle based on the hierarchical unscented Kalman filter method to obtain the estimated values of the state parameters and position parameters of the vehicle respectively; Use the steering angles of each axle as control variables, and construct a vehicle error state space model based on the state space expressions of the estimated values of the vehicle state parameters, position parameters and the reference trajectory, and discretize the vehicle error state space model; Construct a prediction model based on the discretized vehicle error state space model; Use the prediction model to perform trajectory tracking control on the multi-axle heavy-duty articulated vehicle.

[0042] An embodiment of a trajectory tracking control method for a multi-axle heavy-duty articulated vehicle:

[0043] Figure 1 It is a schematic flow chart of a trajectory tracking control method for a multi-axle heavy-duty articulated vehicle provided by an embodiment of the present invention. As Figure 1 shown, the control method includes the following steps S101 to step S104.

[0044] Step S101, construct a coupled model of dynamics and kinematics of a multi-axle heavy-duty articulated vehicle.

[0045] Figure 2 It is a dynamic model of a multi-axle heavy-duty articulated vehicle provided by an embodiment of the present invention. As Figure 2 shown, the multi-axle heavy-duty articulated vehicle takes 6 axles as an example for illustrative purposes. In practical applications, the number of axles can be more, and the present invention does not make special limitations in this regard.

[0046] Continuing as Figure 2 shown, the multi-axle heavy-duty articulated vehicle takes the hinge point FT Taking [a certain point] as the demarcation point, it is divided into the front vehicle and the rear vehicle. The first axis y1, the second axis y2, and the third axis y3 are the three axes belonging to the front vehicle, and the fourth axis y4, the fifth axis y5, and the sixth axis y6 are the three axes belonging to the rear vehicle.

[0047] Among them, the dynamic model of the multi-axle heavy-duty articulated vehicle is a three-degree-of-freedom dynamic model. The three degrees of freedom include the lateral movement of the whole vehicle, the yaw movement of the front vehicle, and the yaw movement of the rear vehicle; the kinematic model of the multi-axle heavy-duty articulated vehicle includes the kinematic model of the front vehicle and the kinematic model of the rear vehicle of the multi-axle heavy-duty articulated vehicle.

[0048] Since the multi-axle heavy-duty articulated vehicle is divided into the front vehicle and the rear vehicle, therefore, the lateral movement of the whole vehicle can be divided into the lateral movement of the front vehicle and the lateral movement of the rear vehicle.

[0049] The state parameters of the vehicle can be obtained by using the dynamic model of the multi-axle heavy-duty articulated vehicle, and the position parameters of the vehicle can be obtained by using the kinematic model of the multi-axle heavy-duty articulated vehicle.

[0050] Among them, the state parameters include the side slip angle β of the front vehicle's center of mass f , the side slip angle β of the rear vehicle's center of mass r , the yaw angular velocity ω1 of the front vehicle's center of mass, and the yaw angular velocity ω2 of the rear vehicle's center of mass; the position information parameters include the position X of the front vehicle in the lateral direction f , the position Y of the front vehicle in the longitudinal direction f , the position X of the rear vehicle in the lateral direction r , and the position Y of the rear vehicle in the longitudinal direction r .

[0051] First, the dynamic model of the multi-axle heavy-duty articulated vehicle will be introduced below, and then the kinematic model of the multi-axle heavy-duty articulated vehicle will be introduced.

[0052] The three-degree-of-freedom dynamic model of the multi-axle heavy-duty articulated vehicle can be determined by the following formula (1):

[0053]

[0054] Among them, m f represents the mass of the front vehicle; m r represents the mass of the rear vehicle; u1 represents the longitudinal speed of the front vehicle; u2 represents the longitudinal speed of the rear vehicle; a1 represents the distance from the center of mass of the front vehicle to the first axis; a2 represents the distance from the center of mass of the rear vehicle to the sixth axis; b1 represents the distance from the center of mass of the front vehicle to the second axis; b2 represents the distance from the center of mass of the rear vehicle to the fifth axis; c1 represents the distance from the center of mass of the front vehicle to the third axis; c2 represents the distance from the center of mass of the rear vehicle to the fourth axis; d1 represents the distance from the center of mass of the front vehicle to the articulation point; d2 represents the distance from the center of mass of the rear vehicle to the articulation point; δ i represents the rotation angle of the i-th axis; F yiDenote the lateral force acting on the i-th axis; i = 1, 2, 3, 4, 5, 6; F T Denote the acting force at the hinge point F T ; I zf Denote the moment of inertia of the front vehicle about the Z-axis; I zr Denote the moment of inertia of the rear vehicle about the Z-axis; is the hinge angle.

[0055] Since the vehicle model of the multi-axle heavy-duty articulated vehicle is a symmetric structure, the distance from the first axis of the front vehicle to the center of mass of the front vehicle is equal to the distance from the third axis of the rear vehicle to the center of mass of the rear vehicle (i.e., a1 = a2), the distance from the second axis of the front vehicle to the center of mass of the front vehicle is equal to the distance from the second axis of the rear vehicle to the center of mass of the rear vehicle (i.e., b1 = b2), the distance from the third axis of the front vehicle to the center of mass of the front vehicle is equal to the distance from the first axis of the rear vehicle to the center of mass of the rear vehicle (i.e., c1 = c2), and the distance from the center of mass of the front vehicle to the hinge point is equal to the distance from the center of mass of the rear vehicle to the hinge point (i.e., d1 = d2).

[0056] Among them, the first axis of the front vehicle is also called the first axis, the second axis of the front vehicle is also called the second axis, the third axis of the front vehicle is also called the third axis, the first axis of the rear vehicle is also called the fourth axis, the second axis of the rear vehicle is also called the fifth axis, and the third axis of the rear vehicle is also called the sixth axis.

[0057] In formula (1), the expression in the first row is the lateral movement of the front vehicle, the expression in the second row is the yaw movement of the front vehicle, the expression in the third row is the lateral movement of the rear vehicle, and the expression in the fourth row is the yaw movement of the rear vehicle.

[0058] For the longitudinal speed u1 of the front vehicle and the longitudinal speed u2 of the rear vehicle, they can be collected by a speed sensor; for the acting force F T at the hinge can be collected by a force sensor; for the distance a1 from the center of mass of the front vehicle to the first axis, the distance a2 from the center of mass of the rear vehicle to the sixth axis, the distance b1 from the center of mass of the front vehicle to the second axis, the distance b2 from the center of mass of the rear vehicle to the fifth axis, the distance c1 from the center of mass of the front vehicle to the third axis, the distance c2 from the center of mass of the rear vehicle to the fourth axis, the distance d from the center of mass of the front vehicle to the hinge point, and the distance d2 from the center of mass of the rear vehicle to the hinge point, they can all be collected by a distance sensor; for the hinge angle can be collected by an angle sensor; the moment of inertia I zf of the front vehicle about the Z-axis, the moment of inertia I zr of the rear vehicle about the Z-axis, the mass m f of the front vehicle, and m r the mass of the rear vehicle are all known quantities.

[0059] For the acquisition of the lateral force F yi acting on each axis, it can be directly obtained by a force sensor.

[0060] Since the traveling speed of multi-axle heavy-duty articulated vehicles is relatively low in actual application scenarios, as an alternative implementation, the lateral force F acting on each axle yi needs to satisfy a linear relationship, which is expressed by the following formula (2):

[0061] F yi = k i α i (2)

[0062] where k i represents the cornering stiffness of the i-th axle; α i represents the tire slip angle of the i-th axle.

[0063] Since the cornering stiffness of each axle is known and the tire slip angles of each axle can be obtained through angle sensors, the lateral force F acting on each axle yi is known.

[0064] Therefore, by substituting the corresponding physical quantities collected by various sensors and the known physical quantities into formula (1), the state parameters of the multi-axle heavy-duty articulated vehicle can be obtained, that is, the front vehicle's center-of-mass slip angle β f , the rear vehicle's center-of-mass slip angle β r , the front vehicle's center-of-mass yaw rate ω1 and the rear vehicle's center-of-mass yaw rate ω2 can be obtained.

[0065] Since the vehicle dynamics model includes the kinematic model of the front vehicle of the multi-axle heavy-duty articulated vehicle and the kinematic model of the rear vehicle of the multi-axle heavy-duty articulated vehicle, the kinematic model of the front vehicle can be determined by the following formula (3):

[0066]

[0067] The kinematic model of the rear vehicle can be determined by the following formula (4):

[0068]

[0069] where X f represents the lateral position of the front vehicle; Y f represents the longitudinal position of the front vehicle; X r represents the lateral position of the rear vehicle; Y r represents the longitudinal position of the rear vehicle; ψ f represents the heading angle of the front vehicle; ψ r represents the heading angle of the rear vehicle.

[0070] The heading angle ψ f of the front vehicle and the heading angle ψ rThey are all easily observable quantities that can be obtained through angle sensors. The longitudinal speed u1 of the leading vehicle and the longitudinal speed u2 of the trailing vehicle can be collected by speed sensors.

[0071] The construction of the dynamic and kinematic coupling model of a multi-axle heavy-duty articulated vehicle is reflected in that the state parameters of the vehicle can be obtained through the dynamic model, and the position information of the vehicle can be obtained by substituting the state parameters of the vehicle into the kinematic model.

[0072] Specifically, the heading angle ψ of the leading vehicle obtained by the angle sensor f and the heading angle ψ of the trailing vehicle r , the longitudinal speed u1 of the leading vehicle and the longitudinal speed u2 of the trailing vehicle obtained by the speed sensor, and the sideslip angle β of the center of mass of the leading vehicle obtained according to formula (1) f , the sideslip angle β of the center of mass of the trailing vehicle r are substituted into formula (3) to obtain the lateral position X f and the longitudinal position Y f of the leading vehicle of the multi-axle heavy-duty articulated vehicle.

[0073] The heading angle ψ of the leading vehicle obtained by the angle sensor f and the heading angle ψ of the trailing vehicle r , the longitudinal speed u1 of the leading vehicle and the longitudinal speed u2 of the trailing vehicle obtained by the speed sensor, and the sideslip angle β of the center of mass of the leading vehicle obtained according to formula (1) f , the sideslip angle β of the center of mass of the trailing vehicle r are substituted into formula (4) to obtain the lateral position X r and the longitudinal position Y r of the trailing vehicle of the multi-axle heavy-duty articulated vehicle.

[0074] To achieve more precise trajectory control, it is necessary to convert the three-degree-of-freedom dynamic model into a three-degree-of-freedom equation, and then construct a state-space expression based on the three-degree-of-freedom equation. The process of constructing the state-space expression includes step S101A and step S101B.

[0075] Specifically, step S101A: Based on the small-angle assumption, combined with the motion constraint equations of the leading and trailing vehicles and formula (2), the three-degree-of-freedom dynamic model (i.e., formula (1)) is converted into a three-degree-of-freedom equation.

[0076] Among them, the three-degree-of-freedom equation can refer to the following formula (5):

[0077]

[0078] Among them, F yi satisfies formula (2).

[0079] Step S101B: Rewrite the three-degree-of-freedom equation into a state-space expression.

[0080] Among them, the state-space expression is represented by ; then The expression of is as formula (6) below.

[0081]

[0082] Among them, x is the state variable, and the expression is x = [β f , ω1, β r , ω2] T ; u is the input variable, and the expression is u = [δ1, δ2, δ3, δ4, δ5, δ6] T .

[0083] Among them, G is the coefficient of the state variable x, and the expression of G is as formula (7) below.

[0084]

[0085] Among them, H is the coefficient of the input variable u, and the expression of H is as formula (8) below.

[0086]

[0087] Among them, M is the coefficient matrix remaining after extracting the state variable during calculation, and the expression of M satisfies formula (9) below.

[0088]

[0089] In formula (7), a 11 satisfies formula (10) below.

[0090] a 11 = k1 + k2 + k3 (10)

[0091] In formula (7), a 12 satisfies formula (11) below.

[0092]

[0093] In formula (7), a 13 satisfies formula (12) below.

[0094] a 13 = k4 + k5 + k6 (12)

[0095] In formula (7), a 14 satisfies formula (13) below.

[0096]

[0097] In formula (7), a 21 satisfies the following formula (14).

[0098] a 21 = a1k1 - b1k2 - c1k3 (14)

[0099] In formula (7), a 21 satisfies the following formula (15).

[0100]

[0101] In formula (7), a 23 satisfies the following formula (16).

[0102] a 23 = d1(k4 + k5 + k6) (16)

[0103] In formula (7), a 24 satisfies the following formula (17).

[0104]

[0105] In formula (7), a 33 satisfies the following formula (18).

[0106] a 33 = k4a2 + k5b2 - k6c2 - d2(k4 + k5 + k6) (18)

[0107] In formula (7), a 34 satisfies the following formula (19).

[0108]

[0109] Step S102: Based on the dynamic and kinematic coupling model, use the hierarchical unscented Kalman filter to estimate the vehicle dynamic characteristics and the vehicle position, and respectively obtain the estimated values of the vehicle state parameters and the position parameters.

[0110] Figure 3 is a schematic diagram of a hierarchical unscented Kalman filter estimation module provided by an embodiment of the present invention. As Figure 3 shown, the parameters that are easy to measure, such as the lateral acceleration aF of the vehicle in front, the lateral acceleration aR of the vehicle behind, and the force FT at the hinge, are defined as the observation parameters and the prediction parameters. In the first layer, the steering angles δi of each axis, the lateral acceleration aF of the vehicle in front, the lateral acceleration aR of the vehicle behind, and the force F TThe dynamic model of a multi-axle heavy-duty articulated vehicle is sent to calculate the estimated values of the vehicle's state parameters; then the estimation results of the first layer are sent to the second layer, and based on the kinematic model of the multi-axle heavy-duty articulated vehicle, the estimated values of the vehicle's position parameters are calculated.

[0111] Among them, the expression of the lateral acceleration aF of the front vehicle is The expression of the lateral acceleration aR of the rear vehicle is The estimated values of the state parameters include: the estimated value β of the centroid side slip angle of the front vehicle fg and the estimated value β of the centroid side slip angle of the rear vehicle rg the estimated value ω of the centroid yaw angular velocity of the front vehicle 1g and the estimated value ω of the centroid yaw angular velocity of the rear vehicle 2g ; the estimated values of the position parameters include: the estimated value X of the position of the front vehicle in the lateral direction fg the estimated value Y of the position of the front vehicle in the longitudinal direction fg the estimated value X of the position of the rear vehicle in the lateral direction rg and the estimated value Y of the position of the rear vehicle in the longitudinal direction rg .

[0112] Among them, the hierarchical unscented Kalman filtering method is an existing means, and the principle of the hierarchical unscented Kalman filtering method is specifically introduced below. First, obtain the state model x of the system k and the observation model z k , and the observation model z k is used to combine the predicted state with the actual observation value to correct the state estimation result.

[0113] Among them, the state model x of the system k is expressed by the following formula (20).

[0114] x k = f(x k-1 ) + w k-1 (20)

[0115] Among them, f(x k-1 ) is the state transition matrix, and w k-1 is the system noise. k is the time step, and its value range is an integer from 1, 2, 3,..., N.

[0116] Among them, the observation model z k can be expressed by the following formula (21).

[0117] z k = h(x k ) + v k (21)

[0118] Among them, h(x k ) represents the observation matrix; vk represents the observation noise.

[0119] The state model x of the system k and the observation model z k actually have an equal number of values. Let k = 1, 2, 3....N, then the state model ranges from 0 to N - 1, and the observation model ranges from 1 to N. The two only differ in time indices, reflecting the difference moments.

[0120] First, calculate 2n + 1 Sigma sampling points and their corresponding weights through unscented transformation. For convenience of representation, use X k-1 to represent these 2n + 1 Sigma sampling points and their corresponding weights, then X k-1 This set can be represented by the following formula (22).

[0121]

[0122] where, X k-1 represents the set of Sigma points generated through unscented transformation at time step k - 1; represents the state vector of the jth Sigma point, which is a sampling point generated based on the current state mean and covariance at time step k - 1; W j represents the weight corresponding to the jth Sigma point; n is the state space dimension.

[0123] Combined with the Sigma points, the selection of high - order information can be calculated according to the following formula (23) and the following formula (24):

[0124]

[0125] where, represents the ιth Sigma point; represents the ι + nth Sigma point; The a in represents the augmented state vector at time step k - 1. The state variable and the noise term are combined into the augmented state, aiming to more comprehensively describe the probability distribution of the system. a serves as an identifier; and are symmetrically distributed around to jointly capture the positive and negative direction uncertainties of the covariance matrix; the value range of l is: ι = 1, 2, 3……n.

[0126] where, W j satisfies the following formula (25).

[0127]

[0128] where, the value range of j is: j = 1, 2, 3……n; W0 To generate Sigma points, when W 0 ≥ 0, the Sigma points are far from the initial state, and when W 0 ≤ 0, the Sigma points are close to the initial state.

[0129] Calculate the gain matrix K k . Among them, the gain matrix K k needs to satisfy the following formula (26).

[0130]

[0131] Among them, is the prior estimate value of x k ; is the prior estimate value of z k .

[0132] Calculate the posterior estimate value of the state quantity Among them, the posterior estimate value of the state quantity can be confirmed by formula (27).

[0133]

[0134] Calculate the covariance P k . Among them, the covariance P k can be confirmed by the following formula (28).

[0135]

[0136] Among them, is the prior error covariance.

[0137] Step S103: With the steering angles of each axis as the control quantities, construct a vehicle error state space model based on the estimated values of the vehicle's state parameters, position parameters, and the state space expression of the reference trajectory, and discretize the vehicle error state space model.

[0138] Among them, the reference trajectory information includes the reference value of the front vehicle's center of mass sideslip angle β fc , the reference value of the rear vehicle's center of mass sideslip angle β rc , the reference value of the front vehicle's center of mass yaw rate ω 1c , the reference value of the rear vehicle's center of mass yaw rate ω 2c , the reference value of the front vehicle's lateral position X fc , the reference value of the front vehicle's longitudinal position Y fc , the reference value of the rear vehicle's lateral position X rc and the reference value of the rear vehicle's longitudinal position Y rc .

[0139] Specifically, step S103 includes the following steps S103A to S103E.

[0140] In step S103A, first, a system state space expression is constructed using the state parameter estimation value, the position parameter estimation value, and the rotation angles of each axis.

[0141] Specifically, the system state space expression is represented by and the system state space expression can be calculated by the following formula (29).

[0142]

[0143] where ξ(t) = [β fg , ω 1g , β rg , ω 2g , X fg , Y fg , X rg , Y rg ; u(t) = [δ1, δ2, δ3, δ4, δ5, δ6]. That is, the state variables of the system state space expression are the state parameter estimation value and the position parameter estimation value; the input variables of the system state space expression are the rotation angles of each axis.

[0144] In step S103B, a state space expression of the reference trajectory is constructed according to the reference trajectory information

[0145] where the state space expression of the reference trajectory can be determined by the following formula (30).

[0146]

[0147] where ξ r (t) is the reference value of the state parameter, that is, the reference value of the front vehicle's center of mass sideslip angle β fc , the reference value of the rear vehicle's center of mass sideslip angle β rc , the reference value of the front vehicle's center of mass yaw rate ω 1c , the reference value of the rear vehicle's center of mass yaw rate ω 2c , the reference value of the front vehicle's lateral position X fc , the reference value of the front vehicle's longitudinal position Y fc , the reference value of the rear vehicle's lateral position X rc and the reference value of the rear vehicle's longitudinal position Y rc ; u r (t) represents the reference input variable, that is, the reference value of the rotation angle of each axis, that is, the reference value of the rotation angle of the first axis δ 1c , the reference value of the rotation angle of the second axis δ 2c, the reference value δ of the rotation angle of the third axis 3c , the reference value δ of the rotation angle of the fourth axis 4c , the reference value δ of the rotation angle of the fifth axis 5c and the reference value δ of the rotation angle of the sixth axis 6c .

[0148] Step S103C: Linearize the state - space expression of the reference trajectory to obtain the state - space expression of the linear reference trajectory.

[0149] Among them, the state - space expression of the reference trajectory can be linearized using the Taylor expansion formula , or other methods can also be used to linearize the state - space expression of the reference trajectory . The present invention does not make special limitations on this. Subsequently, taking the linearization of the state - space expression of the reference trajectory using the Taylor expansion formula as an example, an exemplary description is given.

[0150] Specifically, at the reference point, use Taylor series expansion, ignore the high - order terms. The Taylor series expansion can refer to the following formula (31):

[0151]

[0152] Step S103D: Construct a vehicle error state - space model using the system state - space expression and the state - space expression of the linear reference trajectory.

[0153] Among them, the vehicle error state - space model can be represented by , and can be determined by the following formula (32).

[0154]

[0155] Among them, A is the coefficient of ; B is the coefficient of .

[0156] The expression of A can refer to the following formula (33):

[0157]

[0158] The expression of B can refer to the following formula (34):

[0159]

[0160] Step S103E: Discretize the vehicle error state - space model.

[0161] Among them, the forward Euler method can be used to discretize the vehicle error state space model ; other methods can also be used to discretize the vehicle error state space model for discretization processing. The present invention does not make special limitations on this. Subsequently, taking the discretization of the vehicle error state space model by using the forward Euler method as an example, an exemplary description will be given.

[0162] The discretized vehicle error state space model is represented by , then it can be determined by the following formula (35).

[0163]

[0164] Among them, A1 is the coefficient of , and B1 is the coefficient of .

[0165] A1 can be determined by the following formula (36):

[0166] A1 = I + λA (36)

[0167] B1 can be determined by the following formula (37).

[0168] B1 = λB (37)

[0169] Among them, I is the identity matrix and λ is the sampling period.

[0170] S104. Construct a prediction model based on the discretized vehicle error state space model, and use the prediction model to perform trajectory tracking control on the multi-axis heavy-duty articulated vehicle.

[0171] Among them, a prediction model can be constructed based on the discretized vehicle error state space model by using the model predictive control algorithm (Model Predictive Control, MPC).

[0172] The theoretical process of MPC is as follows:

[0173] Construct a new state space expression x(k + 1|t).

[0174] Set a new state matrix x(k|t).

[0175] The new state matrix x(k|t) can refer to the following formula (38).

[0176]

[0177] It should be noted that x(k|t) is the new state matrix, which contains both state variables and input variables. It is a transformation of the state-space expression using to derive a new state equation for predictive control. t represents the current time, marking the starting point of prediction, and k is the time offset. Specifically, it is the state prediction at time k for time t.

[0178] Substituting Equation (38) into Equation (35), a new state-space expression x(k + 1|t) can be obtained. The new state-space expression x(k + 1|t) can be referred to as Equation (39) below.

[0179]

[0180] where η(k|t) is the system output within the prediction horizon; is the newly constructed output quantity, and its expression consists of an identity matrix and a zero matrix.

[0181] According to the basic principle of model predictive control, assuming the current time is k, the prediction horizon N p and the control horizon N u are defined, such that when N p ≥N u , the state variables within the future N p time period can be expressed by Equation (40) below, and the control quantity can be expressed by Equation (41) below:

[0182] x(k + 1|k), x(k + 2|k), ……, x(k + N p |k) (40)

[0183]

[0184] Define the system input quantity at time k, i.e., the system control increment at the previous time, as ΔU(k). Then, the expression of ΔU(k) can be referred to as Equation (42) below.

[0185]

[0186] Define the system output at time k as y(k). Then, the expression of y(k) can be referred to as Equation (43) below.

[0187] y(k) = [η(k + 1|k), η(k + 2|k),......, η(k + N p |k)] T (43)

[0188] The expression of the system output Y(k) at future time can refer to the following formula (44), and the system output Y(k) at future time is the prediction model.

[0189] Y(k) = ψ(k)x(k) + I(k)ΔU(k) (44)

[0190] Among them, ψ(k) is a new coefficient matrix and can be expressed by the following formula (45).

[0191]

[0192] Among them, the expression of I(k) can refer to the following formula (46).

[0193]

[0194] In formula (44), x(k) is the current state quantity of the system. It can be clearly seen from formula (44) that within the prediction time domain, both the state quantity and the output quantity can be calculated through the current state quantity x(k) of the system and the system control increment ΔU(k) within the control time domain, realizing the prediction function.

[0195] Set the optimization objective function. The optimization objective function can refer to the following formula (47).

[0196]

[0197] Among them, Q and R are weight matrices, ρ is a weight coefficient, and ε is a relaxation factor.

[0198] The reference output value in the reference trajectory can refer to the following formula (48):

[0199] η ref (k + υ|t) = [ω1(k + υ|t), ω2(k + υ|t), X f (k + υ|t), X r (k + υ|t), Y f (k + υ|t), Y r (k + υ|t)] T (48)

[0200] Convert the above formula into the standard quadratic programming form, and the standard quadratic programming form can refer to the following formula (49):

[0201]

[0202] Among them, F = [2·(ψ(t)·x(t) - Y ref (t)) T ·Q·I t , 0].

[0203] According to the above formula, solve the control increment sequence in the control time domain, and use the first element as the actual control quantity at the next moment to input into the system. Repeat this operation to achieve the tracking control of the vehicle trajectory.

[0204] The embodiment of the present invention provides a trajectory tracking control method for a multi-axle heavy-duty articulated vehicle. First, by constructing a dynamic and kinematic coupling model of the multi-axle heavy-duty articulated vehicle, the state information and position information of the multi-axle heavy-duty articulated vehicle can be grasped simultaneously. Then, based on the hierarchical unscented Kalman filtering method, the vehicle dynamic characteristics and vehicle position are estimated to obtain the estimated values of the vehicle state parameters and position parameters respectively, which can reduce the estimation error and improve the estimation accuracy. Subsequently, taking the steering angles of each axle as the control quantities, a vehicle error state space model is constructed based on the state space expressions of the vehicle state parameter estimated values, position parameter estimated values, and reference trajectory, and the vehicle error state space model is discretized. Based on the discretized vehicle error state space model, a prediction model is constructed, and the prediction model is used to track and control the trajectory of the multi-axle heavy-duty articulated vehicle, which can comprehensively consider the vehicle state and vehicle position information, achieve accurate prediction and optimal control of the next moment, and ensure stability, accuracy, and robustness under complex working conditions.

[0205] An embodiment of a trajectory tracking control device for a multi-axle heavy-duty articulated vehicle:

[0206] The embodiment of the present invention provides a trajectory tracking control device for a multi-axle heavy-duty articulated vehicle, including a processor, and the processor executes a computer program to implement the method steps of the trajectory tracking control method for the multi-axle heavy-duty articulated vehicle.

[0207] Among them, regarding the "method steps of the trajectory tracking control method for the multi-axle heavy-duty articulated vehicle", reference can be made to the relevant descriptions in the foregoing "embodiment of a trajectory tracking control method for a multi-axle heavy-duty articulated vehicle", and details are not described herein again.

[0208] The trajectory tracking control device for a multi-axle heavy-duty articulated vehicle provided by the embodiment of the present invention can achieve the same beneficial effects as the foregoing trajectory tracking control method for a multi-axle heavy-duty articulated vehicle, and details are not described herein again.

Claims

1. A trajectory tracking control method for a multi-axle heavy-load articulated vehicle, characterized in that: The steps include: Step 1), construct a dynamic and kinematic coupling model of a multi-axle heavy-load articulated vehicle; Step 2), based on the coupling model, using a hierarchical unscented Kalman filter to estimate the vehicle dynamic characteristics and the vehicle position, and obtain the vehicle state parameter estimation value and the position parameter estimation value respectively; Step 3), taking the steering angle of each axis as the control quantity, constructing a vehicle error state space model based on the state parameter estimation value of the vehicle, the position parameter estimation value and the state space expression of the reference trajectory, and discretizing the vehicle error state space model; Step 4) constructing a prediction model based on the discretized vehicle error state space model, and using the prediction model to track and control the trajectory of the multi-axle heavy-load articulated vehicle.

2. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 1, characterized in that: The dynamic model of the multi-axle heavy-load articulated vehicle is determined by the following formula: Wherein, the multi-axle heavy-load articulated vehicle comprises a front vehicle and a rear vehicle; f Indicates the mass of the preceding vehicle; m r Indicates the mass of the following vehicle; β f represents the side slip angle of the center of mass of the front vehicle; β r represents the sideslip angle of the center of mass of the rear vehicle; ω1 represents the yaw rate of the center of mass of the front vehicle; ω2 represents the yaw rate of the center of mass of the rear vehicle; u1 represents the longitudinal speed of the front vehicle; u2 represents the longitudinal speed of the rear vehicle; a1 represents the distance from the center of mass of the front vehicle to the first axis; a2 represents the distance from the center of mass of the rear vehicle to the sixth axis; b1 represents the distance from the center of mass of the front vehicle to the second axis; b2 represents the distance from the center of mass of the rear vehicle to the fifth axis; c1 represents the distance from the center of mass of the front vehicle to the third axis; c2 represents the distance from the center of mass of the rear vehicle to the fourth axis; d1 represents the distance from the center of mass of the front vehicle to the hinge point; d2 represents the distance from the center of mass of the rear vehicle to the hinge point; δ i represents the rotation angle of the i-th axis; F yi represents the lateral force acting on the i-th axis; i = 1, 2, 3, 4, 5, 6; F T I represents the force at the hinge point; zf Indicates the moment of inertia of the front vehicle around the Z axis; I zr It represents the moment of inertia of the rear vehicle around the Z axis; is the articulation angle; Among them, the first axis, the second axis and the third axis are the three axes of the front vehicle; the fourth axis, the fifth axis and the sixth axis are the three axes of the rear vehicle.

3. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 2, characterized in that: The kinematic model of the multi-axle heavy-load articulated vehicle includes: a kinematic model of a front vehicle of the multi-axle heavy-load articulated vehicle and a kinematic model of a rear vehicle of the multi-axle heavy-load articulated vehicle; The kinematic model of the preceding vehicle is determined by the following formula: The kinematic model of the following vehicle is determined by the following formula: Among them, X f Indicates the lateral position of the vehicle in front; Y f Indicates the longitudinal position of the vehicle ahead; X r Indicates the lateral position of the rear vehicle; Y r Indicates the longitudinal position of the rear vehicle; ψ f Indicates the heading angle of the front vehicle; ψ r Indicates the heading angle of the following vehicle.

4. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 3, characterized in that: The F yi The linear relationship shown in the following formula must be satisfied: F yi =k i a i Among them, k i represents the cornering stiffness of the i-th axis; α i Represents the sideslip angle of the tire at the i-th axle.

5. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 3, characterized in that: The step 2) comprises: at the first level, the steering angle δi of each axle, the lateral acceleration aF of the front vehicle, the lateral acceleration aR of the rear vehicle, and the force F at the hinge point are calculated. T The first layer is sent to the dynamic model of the multi-axle heavy-load articulated vehicle to calculate the estimated value of the vehicle's state parameters; the estimation result of the first layer is then sent to the second layer to calculate the estimated value of the vehicle's position parameters based on the kinematic model of the multi-axle heavy-load articulated vehicle; The lateral acceleration aF of the front vehicle is determined by the following formula: The lateral acceleration aR of the rear vehicle is determined by the following formula: Among them, the estimated values ​​of the vehicle's state parameters include: the estimated value of the sideslip angle of the center of mass of the front vehicle β fg , the estimated value of the sideslip angle of the rear vehicle's center of mass β rg , the estimated value of the yaw rate of the center of mass of the front vehicle ω 1g And the estimated value of the yaw rate of the rear vehicle's center of mass ω 2g The estimated values ​​of the vehicle's position parameters include: the estimated value of the lateral position of the front vehicle of a multi-axle heavy-load articulated vehicle X fg , the estimated value Y of the longitudinal position of the front vehicle fg , the estimated value of the lateral position of the rear vehicle of a multi-axle heavy-duty articulated vehicle X rg and the estimated longitudinal position Y of the rear vehicle rg .

6. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 5, characterized in that: The steering angle δi of each axis is obtained through the angle sensor; the longitudinal speed u1 of the front vehicle and the longitudinal speed u2 of the rear vehicle are obtained through the speed sensor; the force F at the hinge point is obtained through the force sensor T .

7. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 5, characterized in that: Step 3) includes: constructing a system state space expression according to the estimated value of the state parameter of the vehicle and the estimated value of the position parameter of the vehicle; The control quantity of the system state space expression is the steering angle of each axis; the vehicle error state space model is obtained by subtracting the state space expression of the reference trajectory from the system state space expression.

8. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to claim 5, characterized in that: The vehicle error state space model is discretized using the forward Euler method.

9. The trajectory tracking control method for a multi-axle heavy-load articulated vehicle according to any one of claims 1 to 8, characterized in that: In step 4), a prediction model is constructed using a model predictive control algorithm based on a discretized vehicle error state space model.

10. A trajectory tracking control device for a multi-axle heavy-load articulated vehicle, comprising a processor, characterized in that: The processor executes a computer program to implement the method steps of the trajectory tracking control method for a multi-axle heavy-load articulated vehicle as described in any one of claims 1-9.

Images