A control method for a semi-trailer tractor based on switching MPC and LQR methods

Abstract

The present invention relates to the field of autonomous driving technology, and specifically to a semi-trailer tractor control method based on switching MPC and LQR methods. By constructing different motion models and controllers, an algorithm is used to control the output through a finite state machine. In view of the complex road environment and loading and unloading requirements of commercial vehicles, when driving at medium and high speeds, such as on urban roads, the LQR control part and the commercial semi-trailer tractor vehicle model suitable for medium and high speed driving are called. When driving at slow speeds, such as parking, the MPC control part and the commercial semi-trailer tractor vehicle model suitable for slow speed driving are called. By switching models and control methods, the control task is completed by adapting to different control backgrounds. The method of the present invention is different from the control method currently only applicable to the four-wheel single-vehicle structure of passenger cars. By switching models, the complex problems caused by the special double-vehicle articulated structure of commercial semi-trailer tractors are solved, and a better control effect is achieved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a semi-trailer tractor control method based on switching MPC and LQR methods. Background Art

[0002] Commercial semi-trailer tractors, due to their high efficiency and low fuel consumption per ton-kilometer, have become the mainstay of medium- and long-distance road transport. However, with the increasing complexity of road conditions and traffic volume, the requirements for the stability and safety of commercial semi-trailer tractors in long-distance transport are becoming increasingly stringent. Autonomous driving technology can effectively reduce driver fatigue and improve long-distance transport safety, and has a wide range of application scenarios.

[0003] While autonomous driving control algorithms for passenger cars are well-established, those for commercial semi-trailer tractors are relatively scarce. Due to the articulated structure of the tractor and trailer, autonomous driving control methods for passenger cars are not well-suited for commercial semi-trailer tractors. Summary of the Invention

[0004] The purpose of the present invention is to provide a control method for a semi-trailer tractor based on switching MPC and LQR methods, which can achieve comprehensive control in complex environments by switching control methods and is suitable for the special double-vehicle articulated structure of commercial semi-trailer tractors.

[0005] To achieve the above object, the present invention provides a control method for a semi-trailer tractor based on switching between MPC and LQR methods, comprising the following steps:

[0006] Step 1: Construct a kinematic model suitable for slow motion;

[0007] Step 2: Build a dynamic model suitable for medium and high speed motion;

[0008] Step 3: Set up an MPC controller for slow motion.

[0009] Step 4: Set up the LQR controller for medium and high speed motion;

[0010] Step 5: Set up the finite state machine;

[0011] Step 6: Set up the algorithm for vehicle motion control.

[0012] Optionally, the kinematic model consists of a tractor and a trailer, which are connected by an articulated joint, with the tractor performing active movement and the trailer performing under-actuated passive movement. Since the forward and backward movements of the semi-trailer tractor are not symmetrical, it is also divided into a forward model and a backward model.

[0013] Optionally, the dynamic model simplifies the two coaxial tires into one, which is a rigid body with two wheels in the front and one wheel in the rear. The dynamic model needs to meet the following assumptions: the tractor and trailer are rigidly articulated; the vehicle will not make large-angle turns; and lateral disturbances are ignored.

[0014] Optionally, the MPC controller uses the current system state and a preset control input sequence to predict future system behavior through a prediction model, and solves an optimization problem in a finite time domain at each sampling moment to achieve the control function. The MPC controller has a forward motion mode and a backward motion mode. The forward motion mode uses the rear axle position of the tractor as the control output, and the backward motion mode uses the rear axle position of the trailer as the control output. Which mode to use is determined by the finite state machine.

[0015] Optionally, the control logic of the LQR controller is as follows: First, the state space equation of the linear system is defined, that is, the relationship between the state vector x(t) and the control input u(t) is determined, specifically through the differential equation expressed by the state matrix A and the control matrix B To describe it, and then calculate the error between the reference path matching point and the current coordinates of the vehicle to obtain the specific error state equation Subsequently, the error state equation is discretized, and the Riccati equation is solved under the constraint of the cost function to obtain the optimal control coefficient k and the control output.

[0016] Optionally, the finite state machine is responsible for switching, starting and stopping the controller according to external control signals.

[0017] Optionally, the steps of the algorithm are as follows:

[0018] Step 6.1: Initialize vehicle parameters;

[0019] Step 6.2: Control information input;

[0020] Step 6.3: Finite state machine decision;

[0021] Step 6.4: Run the MPC controller / LQR controller to solve the motion control output and output the signal to the lower-level execution system.

[0022] The present invention provides a control method for a semi-trailer tractor based on switching MPC and LQR methods. By constructing different motion models and controllers, an algorithm is used to control the output through a finite state machine. In view of the complex road environment and loading and unloading requirements of commercial vehicles, when driving at medium and high speeds, such as on urban roads, the LQR control part and the commercial semi-trailer tractor vehicle model suitable for medium and high speed driving are called. When driving at slow speeds, such as parking, the MPC control part and the commercial semi-trailer tractor vehicle model suitable for slow speed driving are called. By switching models and control methods, the control task is completed by adapting to different control backgrounds. The method of the present invention is different from the current control method that is only applicable to the four-wheel single vehicle structure of passenger cars. By switching models, the complex problems caused by the special double-vehicle articulated structure of commercial semi-trailer tractors are solved, and a better control effect is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0024] Figure 1 The figure is a flowchart of the steps of a semi-trailer tractor control method based on switching MPC and LQR methods of the present invention.

[0025] Figure 2 It is a schematic diagram of the forward model of the present invention.

[0026] Figure 3 It is a schematic diagram of the backward model of the present invention.

[0027] Figure 4 It is a schematic diagram of the medium- and high-speed dynamics model of the present invention.

[0028] Figure 5 It is a structural flow diagram of the control algorithm of the present invention. DETAILED DESCRIPTION

[0029] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0030] See also Figure 1 The present invention provides a control method for a semi-trailer tractor based on switching MPC and LQR methods, comprising the following steps:

[0031] S1: Construct a kinematic model suitable for slow motion;

[0032] S2: Construct a dynamic model suitable for medium and high speed motion;

[0033] S3: Set up an MPC controller for slow motion;

[0034] S4: Set up the LQR controller for medium and high speed motion;

[0035] S5: Set up the finite state machine;

[0036] S6: Setting up algorithms for vehicle motion control.

[0037] The following is a further explanation of the execution steps and related terms:

[0038] Since its introduction in the late 1970s, the Model Predictive Control (MPC) algorithm has experienced rapid development from theoretical research to industrial application. Its basic principle is to use a system dynamic model to predict future behavior. During each control cycle, based on the current system state and historical control inputs, the model predicts the system output for a period of time in the future and constructs an optimization problem to solve the optimal control sequence. This optimization process typically involves minimizing the error between the predicted output and the desired output, while considering multiple constraints such as input, output, and state. By implementing this optimization strategy in a rolling manner—applying only the first control action in the calculated optimal control sequence to the actual system and repeating this process in the next control cycle—the MPC algorithm achieves precise control of complex motion processes.

[0039] LQR (Linear Quadratic Regulator), also known as linear quadratic regulator, is an important method for designing state feedback controllers in modern control theory. Its principle is based on the least squares optimization problem, and its main goal is to find a state feedback controller that makes the linear system perform optimally under a given performance indicator. This performance indicator is usually expressed as a cost function or price function, which is a quadratic function of the system state vector and the control input. By adjusting the weighting matrices Q and R of the state vector and the control input, the trade-off between the system state and the control input can be balanced, thereby minimizing the performance indicator while ensuring system stability. In short, LQR uses dynamic programming and state space expressions to solve the Riccati equation to obtain the optimal controller gain matrix, and then design the optimal control strategy.

[0040] Kinematic model for slow motion in step S1:

[0041] Slow motion is generally applicable to parking and starting phases. It is characterized by slow speeds, large front wheel turning angles, and significant changes in vehicle posture. Due to the unique dual-vehicle articulated structure of a semi-trailer tractor, forward and backward motion are asymmetrical, necessitating separate forward and backward models.

[0042] 1. Forward Model

[0043] like Figure 2 The figure shows the schematic diagram of the forward model. f ,y f ) is the center coordinate of the front axle of the tractor, (x r ,y r ) is the center coordinate of the rear axle of the tractor, (x t ,y t ) is the coordinate of the center of the trailer rear axle, (x0, y0) is the coordinate of the hinge point, is the articulation angle, α is the difference between the virtual steering angle and the articulation angle, λ is the virtual steering angle of the trailer, R0 is the turning radius of the tractor, R1 is the turning radius of the trailer, v0 is the longitudinal speed of the tractor, v1 is the longitudinal speed of the trailer, θ0 is the heading angle of the tractor, θ1 is the heading angle of the trailer, δ f is the front wheel turning angle.

[0044] When moving forward, the tractor is the dominant vehicle and the rate of change of the actual coordinates of the rear axle center of the tractor is The rate of change of the actual heading angle of the tractor and trailer As output, the vehicle input is v0 and δ f , then the forward kinematics model formula is

[0045]

[0046] The actual coordinates of the trailer's rear axle center are

[0047]

[0048] 2. Backward Model

[0049] Figure 3 The figure is a schematic diagram of the backward model. The symbols of the backward model have the same meaning as those of the forward model. In the backward movement, although the movement is still dominated by the tractor, the key control target changes to the trailer. The rate of change of the actual coordinate of the rear axle center of the trailer is The rate of change of the actual heading angle of the tractor and trailer As output, the vehicle input is still v0 and δ f , and at this time there is The backward kinematics model formula is

[0050]

[0051] The actual coordinates of the rear axle center of the tractor are

[0052]

[0053] In step S2, a dynamic model suitable for medium and high speed motion is used.

[0054] Medium and high speed motion is generally applicable to vehicle high-speed sections or open urban roads, with a speed generally not less than 15km / h. It is characterized by high vehicle speed, small front wheel turning angle, and small vehicle posture change. Since the tire lateral force has a greater impact when turning at medium and high speeds, while the lateral disturbances such as crosswind and side slope have a smaller impact, a dynamic model that considers the tire lateral force is established, ignoring the roll motion of the semi-trailer tractor. At the same time, a small angle assumption is adopted, that is, the front wheel turning angle and articulation angle are very small, and a linear tire model is used. Its mechanical motion diagram is as follows Figure 4 As shown:

[0055] Among them F xf 、F yf 、F xr 、F yr 、F xt 、F yt They are the force on the tractor's front wheel x-axis, the force on the tractor's front wheel y-axis, the force on the tractor's rear wheel x-axis, the force on the tractor's rear wheel y-axis, the force on the trailer's rear wheel x-axis, and the force on the trailer's rear wheel y-axis; v x1 、v y1 、v x2 、v y2 is the orthogonal decomposition of the tractor speed and the trailer speed; ω1 and ω2 are the angular velocities of the tractor and the trailer; point p is the articulation point; λ is the articulation angle; δ is the front wheel steering angle; are the heading angles of the tractor and trailer; a1, b1, a2, b2 are the distances from the front and rear axles of the tractor to their respective centers of mass; d is the distance from the articulation point to the rear axle of the tractor.

[0056] The longitudinal dynamics of the semi-trailer tractor are as follows:

[0057]

[0058] The lateral dynamics of the semi-trailer tractor are as follows:

[0059]

[0060] The yaw dynamics equation of the tractor is as follows:

[0061]

[0062] The trailer yaw dynamics equation is as follows:

[0063]

[0064] In the above equation, m1 is the mass of the tractor, m2 is the weight of the trailer and the cargo it carries, and I z1 is the moment of inertia of the tractor, I z2 is the moment of inertia of the trailer and its cargo.

[0065] The tire lateral force considers the linear tire model, as follows:

[0066]

[0067] C yf 、C yr 、C yt It is the lateral stiffness of the front and rear wheels of the tractor and the rear wheels of the trailer.

[0068] Step S3, MPC controller for slow motion

[0069] First, the model is linearized. The forward model and the backward model can be summarized as a state space expression

[0070]

[0071] Where X = [x r ,y r ,θ0,θ1] T Or X=[x t ,y t ,θ0,θ1] T Corresponding to the forward and backward models respectively, the function f() is the system state transfer function. For the reference path point set ρ ref For each point, it must have complete control information, which is recorded as X r =[x r_r ,y r_r ,θ 0-r ,θ 1_r ] T or X r =[x t_r ,y t-r ,θ 0-r ,θ 1_r ] T Corresponding to the forward and backward models respectively, the output is u r =[v 0_r ,δ f_r ] T , then

[0072]

[0073] To linearize the model state space expression, rTaylor expansion and ignoring higher-order terms, and then substituting the reference path expression back, we can get the error state space expression

[0074]

[0075] make The error state space expression can be simplified as

[0076]

[0077] For the forward model, A and B are

[0078]

[0079] For the backward model, A and B are

[0080]

[0081] in Then discretize the error state equation and get

[0082]

[0083] Where T is the sampling time.

[0084] Divide the maximum positive and negative angle into 51 possible angles a i , divide the prediction time domain into 50 prediction time t j , calculate the cost function, set the cost function J M for

[0085]

[0086] Where y is the system error weighting term, the expression is y = Q T X, Q T is the weight coefficient, which can be expressed as

[0087]

[0088] The weight coefficient can be flexibly adjusted according to the visual control effect.

[0089] After calculating the cost function for all possible angles, record (a i |t j ) makes J M (a i ) is the smallest, output the first item of the sequence (a i |t1) is the control execution information.

[0090] Step S4: LQR controller suitable for medium and high speed motion

[0091] Similarly, the LQR controller requires a certain reference trajectory as the tracking target, and the reference path point set ρ ref Must have complete control information, denoted as X r =[x r ,y r ,θ0,θ1] T , the output is u r =[v0,δ f ] T ,have

[0092]

[0093] It is linearized and the error state space equation is obtained. To distinguish it from the above MPC controller, the trajectory tracking error vector is defined as e x1 , e y1 They represent the longitudinal and lateral position deviations of the tractor’s center of mass, Represent the heading angle deviation of the tractor and the trailer respectively, and the error equation can be obtained:

[0094]

[0095] in Select u=[v x1 ,ω1] T As the control input, the error state equation can be obtained

[0096]

[0097] in

[0098]

[0099]

[0100] Discretize the error state equation and get

[0101] q e (t+1)=A t q e (t)+ B tu(t)+Γ(t)

[0102] Among them A t =I+AT s ,B t =I+BT s , T s is the sampling interval.

[0103] Set the cost function J required for optimal control L for

[0104]

[0105] Where P is the system error state weighted term coefficient, R is the input weighted term coefficient, and the weight coefficient can be flexibly adjusted according to the visual control effect.

[0106] Divide the vehicle speed v into 120 parts from 0km / h to 120km / h, and call the Riccati solver module to calculate the cost function J L The optimal control coefficient k under the constraint forms a speed control comparison table so that each speed is compared with a certain k,

[0107] v→k, v∈(0,1,2,…,120)

[0108] Using the interpolation table function, the current vehicle speed v0 is input during control and the optimal output u is obtained through interpolation table lookup.

[0109] u=-k(V0)q e (t)

[0110] There is no need to re-solve the Riccati equation. This can enhance the real-time performance of the control and ensure the safety of the vehicle at high speeds.

[0111] Step S5: Finite state machine.

[0112] A finite state machine is a mathematical model that describes the finite set of states an object can enter during its lifetime, as well as the event-triggered transitions between these states. It consists of states, events, transition rules, and actions. When a specific event occurs, the machine transitions to another state and may execute related actions based on the current state and the rules.

[0113] The state machine uses the DV signal to determine the controller state. The DV signal consists of three bits: the first bit determines whether to start the MPC controller or the LQR controller; the second bit determines whether the MPC controller is in forward or reverse mode, and whether the LQR controller is running for the first time or not; and the third bit determines whether the control is started. Details are shown in Table 1.

[0114] Table 1 DV signal decision table

[0115]

[0116] In step S6, an algorithm is set to control the vehicle motion. The algorithm structure is shown as follows: Figure 5 The specific steps are as follows:

[0117] Step 1: Initialize vehicle parameters. Set the tractor mass m t , trailer mass m r , tractor front wheel cornering stiffness C yf, rear wheel composite cornering stiffness C yr , trailer rear wheel cornering stiffness C yt . Set the LQR control table to empty.

[0118] Step 2: Control information input. Receive the current vehicle motion state variables: current vehicle speed V0, tractor front wheel angle δ f , tractor heading angle θ0, trailer heading angle θ1, articulation angle Coordinate of the rear axle of the tractor (x r ,y r ), trailer rear axle coordinate (x t ,y t ). Input the reference path point set ρ of the upper layer planning ref The input module decision variable DV is used to express the current vehicle state and the start and stop of the controller.

[0119] Step 3: Finite state machine decision. Based on the DV, the control information flow is determined, and the controller mode to be started or the control mode to be exited is determined.

[0120] Step 4: Run the controller to solve the motion control output and output the signal to the lower execution system. If the MPC controller is started, the maximum positive and negative angles are divided into 51 possible angles a i , divide the prediction time domain into 50 prediction time t j , calculate the cost function J M (a i ,t j ), and record the sequence with the minimum cost function, and output its first item (a i ,t1); If the LQR controller is started, when the LQR control table is empty, the cost function J is solved by the discretized error state equation L (q e (t),u(t)) constraints, and obtain an offline LQR control table. When this table is not empty, the control output u=-k(V0)q is obtained by looking up the table. e (t).

[0121] In summary, the present invention has the following beneficial effects:

[0122] 1. The present invention can better complete the comprehensive control of the complex road environment and loading and unloading requirements of commercial vehicles by switching control methods.

[0123] 2. Unlike most current control algorithms that are only applicable to the four-wheel single-vehicle structure of passenger cars, the control algorithm provided by the present invention can be applied to the special double-vehicle articulated structure of commercial semi-trailer tractors, and solves the complex control problems caused by this structure by switching models, achieving better control effects.

[0124] The above disclosure is only a preferred embodiment of the present invention, and certainly cannot be used to limit the scope of the rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above embodiment and equivalent changes made in accordance with the claims of the present invention are still within the scope of the invention.

Claims

1. A control method for a semi-trailer tractor based on switching MPC and LQR methods, characterized in that: The following steps are involved: Step 1: Construct a kinematic model suitable for slow motion; Step 2: Build a dynamic model suitable for medium and high speed motion; Step 3: Set up an MPC controller for slow motion. The MPC controller uses the current system state and a preset control input sequence to predict future system behavior through a prediction model. It solves an optimization problem within a finite time domain at each sampling moment to achieve control functions. The MPC controller has a forward motion mode and a backward motion mode. The forward motion mode uses the position of the tractor's rear axle as the control output, and the backward motion mode uses the position of the trailer's rear axle as the control output. Which mode to use is determined by a finite state machine. Step 4: Set up the LQR controller for medium and high speed motion; The control logic of the LQR controller is as follows: First, the state space equation of the linear system is defined, that is, the relationship between the state vector x(t) and the control input u(t) is determined, specifically through the differential equation expressed by the state matrix A and the control matrix B. To describe it, and then calculate the error between the reference path matching point and the current coordinates of the vehicle to obtain the specific error state equation Then, the error state equation is discretized and the Riccati equation is solved under the constraint of the cost function to obtain the optimal control coefficient k and the control output; Step 5: Set up the finite state machine; Step 6: Set up the algorithm for vehicle motion control.

2. The control method for a semi-trailer tractor based on switching between MPC and LQR methods according to claim 1, characterized in that: The kinematic model consists of a tractor and a trailer. The tractor and the trailer are connected by an articulated joint. The tractor performs active motion, and the trailer performs under-actuated passive motion. Since the forward and backward motions of the semi-trailer tractor are not symmetrical, it is also divided into a forward model and a backward model.

3. The control method for a semi-trailer tractor based on switching between MPC and LQR methods according to claim 2, characterized in that: The dynamic model simplifies the two coaxial tires into one, which is a rigid body with two wheels at the front and one wheel at the rear. The following assumptions must be met for the dynamic model to be valid: the tractor and trailer are rigidly articulated; the vehicle does not make large-angle turns; and lateral disturbances are ignored.

4. The control method for a semi-trailer tractor based on switching between MPC and LQR methods according to claim 3, characterized in that: The finite state machine is responsible for switching, starting and stopping the controller according to external control signals.

5. The control method for a semi-trailer tractor based on switching between MPC and LQR methods according to claim 4, characterized in that: The steps of the algorithm are as follows: Step 6.1: Initialize vehicle parameters; Step 6.2: Control information input; Step 6.3: Finite state machine decision; Step 6.4: Run the MPC controller / LQR controller to solve the motion control output and output the signal to the lower-level execution system.

Images