Improved model prediction control method for trajectory tracking of unmanned vehicle

Abstract

The invention discloses an improved model prediction control method for trajectory tracking of an unmanned vehicle, and belongs to the technical field of trajectory tracking control of unmanned vehicles, and the method comprises the steps: S1, building a three-degree-of-freedom nonlinear dynamic model of a vehicle; s2, making a linear dynamic model of the tire in combination with a magic tire formula; s3, formulating an objective function and constraint conditions of the model prediction control method; and S4, formulating a rule of a fuzzy controller, and enabling the unmanned vehicle to adaptively adjust a time domain coefficient according to the fuzzy rule and complete trajectory tracking. According to the method, the three-degree-of-freedom nonlinear dynamic model of the vehicle and the tire model based on the magic formula are established, the fuzzy control rule is introduced, and the improved MPC algorithm is designed, so that the unmanned vehicle can be adaptively adjusted according to the road, the preset tracking trajectory can be effectively tracked under the low-speed tracking working condition, the tracking error is reduced, and the tracking efficiency is improved. And the trajectory tracking precision is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of trajectory tracking control of unmanned vehicles, and in particular relates to an improved model predictive control method for trajectory tracking of unmanned vehicles. Background Art

[0002] With the continuous development and deepening of automation technology, the vehicle's autonomous control ability during driving has gradually improved, correspondingly reducing the need for continuous driver intervention. This development trend will significantly reduce traffic accidents caused by human operational errors and improve the level of road traffic safety.

[0003] Unmanned driving is an advanced intelligent transportation technology that mainly provides driving force for the vehicle's physical system through the fusion processing of multiple sensors, allowing the vehicle to operate autonomously and drive safely. Unmanned driving systems are usually composed of three modules: environmental perception, decision planning, and tracking control. Among them, the environmental perception module refers to the sensors installed on the unmanned vehicle, such as millimeter-wave radar, camera, lidar, ultrasonic radar, infrared sensor, global navigation satellite system, etc., to collect the vehicle's surrounding environment and its own location information and transmit it to the main control system. By processing these environmental and road information, it provides reliable input for the next decision planning. The decision planning module needs to first fuse multi-sensor information and then make task decisions based on driving needs. For unmanned vehicles, this goal is usually from the starting point to the destination. The tracking control module refers to the unmanned vehicle, which controls the vehicle's throttle, brake, and steering system according to the information fused by the sensors, so that the vehicle can drive according to the expected speed or expected trajectory output by the decision planning module, thereby realizing the vehicle's forward and turning actions. In the unmanned vehicle trajectory tracking control system, the main control algorithms currently used include pure tracking control (PurePursuit), robust control (Robust), Stanley control (Stanley), sliding mode control (SMC), fuzzy control (Fuzzy), linear quadratic regulator control (LQR), proportional integral differential control (PID) and model predictive control (MPC).

[0004] Among them, the model predictive control method (MPC) is a control method that can solve constrained problems. The algorithm simulates the operation of the system in the future time period by mathematically modeling the controlled object. In each sampling period, the controller uses the rolling optimization algorithm to predict the output in the future period based on the current system state information, and considers the corresponding constraints to calculate the optimal control sequence at the next moment. However, the controller only executes the first control action of this sequence, and recalculates the new optimal sequence in the next sampling period. By repeating this process, a rolling optimization process is formed. The existing model predictive control method has many advantages, but also has some disadvantages. When designing the cost function, the traditional MPC algorithm is easy to deviate from the tracking trajectory at the curve due to the fixed selection of the prediction time domain coefficient and the control time domain coefficient, resulting in a large tracking error. Therefore, it is impossible to timely evaluate the current driving position and driving state of the vehicle, and it cannot be adjusted adaptively according to the road.

[0005] Therefore, there is a need for an improved model predictive control method for unmanned vehicle trajectory tracking that can achieve adaptive adjustment according to road conditions and environmental information and improve trajectory tracking control accuracy. Summary of the invention

[0006] The purpose of the present invention is to provide an improved model predictive control method for unmanned vehicle trajectory tracking. By establishing a three-degree-of-freedom nonlinear dynamic model of the vehicle and a tire model based on the magic formula, introducing fuzzy control rules and designing an improved MPC algorithm, the unmanned vehicle can be adaptively adjusted according to the road, can effectively track the preset tracking trajectory under low-speed tracking conditions, reduce tracking errors, and improve trajectory tracking accuracy.

[0007] To achieve the above purpose, the technical solution adopted by the present invention is:

[0008] An improved model predictive control method for unmanned vehicle trajectory tracking comprises the following steps:

[0009] Step S1: establishing a three-degree-of-freedom nonlinear dynamic model of the vehicle;

[0010] Step S2: Formulate a linear dynamic model of the tire in combination with the magic tire formula;

[0011] Step S3: Formulate the objective function and constraints of the model predictive control method;

[0012] Step S4: Formulate the rules of the fuzzy controller, and the unmanned vehicle adaptively adjusts the time domain coefficients according to the fuzzy rules and completes trajectory tracking.

[0013] A further improvement of the technical solution of the present invention is that in step S1, establishing a three-degree-of-freedom nonlinear dynamic model of the vehicle includes the following steps:

[0014] Step S101: Make reasonable assumptions and simplify the vehicle;

[0015] Step S102: Establish an inertial coordinate system XOY and the vehicle's own coordinate system xoy, and establish a dynamic equilibrium equation at the vehicle's center of mass O, as shown in the following equation:

[0016]

[0017] In the formula, represents the velocity component of the vehicle in the X-axis direction in the inertial coordinate system, Represents the velocity component of the vehicle in the Y-axis direction in the inertial coordinate system, represents the angular velocity component of the vehicle in the inertial coordinate system, represents the vehicle velocity component function in the X-axis direction in the inertial coordinate system, Represents the vehicle velocity component function in the Y-axis direction in the inertial coordinate system;

[0018] Step S103: Convert the vehicle's own coordinate system and the inertial coordinate system. The conversion relationship is shown in the following equation:

[0019]

[0020] Step S104: Calculate the acceleration component of the vehicle's mass center O in the vehicle's own coordinate system xoy, as shown in the following formula:

[0021]

[0022] Step S105: Establish a nonlinear dynamics model for describing the dynamic behavior of the vehicle in three-dimensional space.

[0023] A further improvement of the technical solution of the present invention is that in step S105, a vehicle dynamics equation is established as shown below:

[0024]

[0025] In the formula, is the longitudinal acceleration of the vehicle, Indicates the lateral acceleration of the vehicle;

[0026] The longitudinal dynamic equation of the vehicle's center of mass along the x-axis is:

[0027]

[0028] The lateral dynamic equation of the vehicle's center of mass along the y-axis is:

[0029]

[0030] The yaw dynamic equation of the vehicle's center of mass along the z-axis is:

[0031]

[0032] The above three dynamic equations are integrated into a composite function form as shown below:

[0033]

[0034] In the formula, m represents the weight of the vehicle body, F xf The resultant longitudinal force of the tires on the front axle of the vehicle, F yf The resultant lateral force of the tires on the front axle, F xr Represents the resultant longitudinal force of the tires on the rear axle of the vehicle, F yr represents the resultant lateral force of the rear axle tire of the vehicle, a and b represent the distances of the front and rear wheels from the center of mass of the vehicle, respectively, and I z Represents the vehicle's moment of inertia about the z-axis.

[0035] A further improvement of the technical solution of the present invention is that in step S2, formulating a linear dynamic model of a tire in combination with the magic tire formula includes the following steps:

[0036] Step S201: Calculate the tire lateral force F using the magic formula y , and the relationship diagram between tire lateral force and sideslip angle is obtained by fitting;

[0037] Among them, the lateral force F of the tire is calculated by combining the magic formula y , as shown below:

[0038]

[0039] The calculation relationship of each parameter in the formula is as follows:

[0040]

[0041] Where a0, a1, ..., a 14 represents the model parameters, F z represents the vertical load of the tire, γ represents the camber angle, and α represents the side slip angle of the tire;

[0042] Step S202: Calculate the longitudinal force F of the tire using the magic formula x , and the relationship diagram between the tire longitudinal force and the slip rate is obtained by fitting;

[0043] Among them, the longitudinal force F of the tire is calculated by combining the magic formula x , as shown below:

[0044]

[0045] The calculation relationship of each parameter in the formula is as follows:

[0046]

[0047] Where a0, a1, …, a 10 represents the main parameters in the longitudinal force model, λ represents the tire slip rate, F z Indicates the vertical load of the tire;

[0048] Step S203: Under a small range assumption, the lateral force and longitudinal force of the tire are expressed in a linear manner;

[0049] Among them, the lateral force of the tire is linearly expressed as shown in the following formula:

[0050] F c =C c α

[0051] In the formula, C c Indicates the tire's cornering stiffness;

[0052] The longitudinal force of the tire is linearly expressed as shown below:

[0053] F l =C l s

[0054] In the formula, C l Indicates the longitudinal stiffness of the tire.

[0055] A further improvement of the technical solution of the present invention is that in step S3, formulating the objective function and constraint conditions of the model predictive control method includes the following steps:

[0056] Step S301: linearizing the vehicle nonlinear dynamics model established in step S1;

[0057] Step S302: discretizing the vehicle nonlinear dynamics model established in step S1;

[0058] Step S303: predicting the input and output quantities and designing a cost function;

[0059] Step S304: constrain the vehicle's own characteristics.

[0060] A further improvement of the technical solution of the present invention is that in step S301, a state space expression is established, as shown in the following formula:

[0061]

[0062] Where ζ(t) represents the state vector of the vehicle system, u(t) represents the control input, f(·) represents the nonlinear function describing the dynamics of the vehicle system, η(t) represents the output vector of the vehicle system, and C represents the output vector matrix, which is used to map the state vector ξ(t) to the output η(t) of the vehicle system.

[0063] Perform Taylor expansion at any point (ξ,u) and ignore higher-order terms:

[0064]

[0065] In the formula, A(t) and B(t) represent the state matrix and input matrix of the linearized system respectively.

[0066] A further improvement of the technical solution of the present invention is that in step S302, a discrete linear time-varying model is obtained by using the forward Euler method, as shown in the following formula:

[0067]

[0068] Where A(k) = I + TA(t), B(k) = TB(t); A(k) and B(k) represent the state matrix and input matrix of the discretized system, respectively, and η(k) represents the discretized form of the vehicle system output vector;

[0069] Let the new discrete linear time-varying form add the control increment, then:

[0070]

[0071] The new state space expression is obtained as shown below:

[0072]

[0073] In the formula, Δu(k)=u(k)-u(k-1); and They represent the incremental state matrix, incremental input matrix and incremental output matrix of the system after discretization respectively, and Δu(k) represents the control increment of the system after discretization.

[0074] A further improvement of the technical solution of the present invention is that in step S303, the input and output quantities are predicted and a cost function is designed, as shown in the following formula:

[0075]

[0076] In the formula, J(·) represents the cost function, represents the desired road trajectory, ΔU(k) ​​represents the control increment sequence, Q represents the weight matrix of the state quantity of the model predictive control, R represents the weight matrix of the control increment of the model predictive control, ε represents the relaxation factor, and ρ represents the weight coefficient of the relaxation factor.

[0077] A further improvement of the technical solution of the present invention is that in step S304, the following constraints are imposed on the sideslip angle of the vehicle:

[0078]

[0079] Constraints are imposed on the control variable front wheel steering angle u and its control increment Δu:

[0080]

[0081] In the formula, u min with u max Indicates the front wheel turning angle u = δ f The minimum and maximum boundary values ​​of Δu min With Δu min Indicates the front wheel steering angle increment Δδ f The minimum and maximum values ​​of the boundaries;

[0082] In each sampling period, the optimization problem of the objective function is transformed into a quadratic programming problem while satisfying the control constraints:

[0083]

[0084] In the formula, min J(·) represents the minimum constraint value of the cost function, ΔU min , Δu min and u min They represent the minimum lower bound of the control increment sequence ΔU(k), the incremental input Δu(k) and the control input u(k+i), ΔU max , Δu max and u max They represent the maximum upper bounds of the control increment sequence ΔU(k), the incremental input Δu(k), and the control input u(k+i), k=0,1,…,N c -1 indicates a time domain sequence.

[0085] A further improvement of the technical solution of the present invention is that in step S4, formulating the rules of the fuzzy controller includes the following steps:

[0086] Step S401: Establish the fuzzy controller output prediction time domain coefficient N p The input yaw angle change Δ and yaw angle The affiliation between:

[0087] Design the trimf membership function and adopt the fuzzy model. The trimf membership function relationship is shown as follows:

[0088]

[0089] Where f(θ(k)) represents the fuzzy trimf membership function, θ(k) represents the membership function parameter, The left endpoint indicates the point where the degree of membership starts to rise. The vertex represents a point with a membership degree of 1. The right endpoint represents the point where the degree of membership drops to 0;

[0090] Step S402: Establish the fuzzy controller output control time domain coefficient N c Changes with input yaw angle and yaw angle The affiliation between:

[0091] The Gauss2mf membership function is designed and the fuzzy model is adopted. The Gauss2mf membership function relationship is shown as follows:

[0092]

[0093] Where c1 and c2 represent the standard deviation of the Gauss2mf membership function, σ1 and σ2 represent the central value of the Gauss2mf membership function, ω1(θ(k)) and ω2(θ(k)) represent the fuzzy Gauss2mf membership function, and θ(k) represents the membership function parameter.

[0094] Due to the adoption of the above technical solution, the technical progress achieved by the present invention is:

[0095] The improved model predictive control method for trajectory tracking of unmanned vehicles of the present invention establishes a three-degree-of-freedom nonlinear dynamic model of the vehicle and a tire model based on the magic formula, and introduces fuzzy control rules at the same time, so that the sensor can detect the change of the yaw angle and the roll angle, so as to evaluate the current driving position and driving state of the vehicle. Once the trajectory deviation is detected, the dual time domain coefficients in the model predictive controller are adjusted and replaced accordingly; an improved MPC algorithm is designed to enable the unmanned vehicle to effectively track the preset tracking trajectory under low-speed tracking conditions, reduce the lateral error of tracking, improve the trajectory tracking accuracy, and neither turn too early nor respond too late. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] Figure 1 is a flow chart of the control method of the present invention;

[0097] Figure 2 It is a schematic diagram of a three-degree-of-freedom vehicle dynamics model in the present invention;

[0098] Figure 3 is a schematic diagram of the relationship between the tire lateral force and the sideslip angle in the present invention;

[0099] Figure 4 It is a schematic diagram of the relationship between the longitudinal force and the slip rate of the tire in the present invention;

[0100] Figure 5 It is a schematic diagram of the input-output relationship of the fuzzy controller in the present invention;

[0101] Figure 6 It is a schematic diagram of the input membership function of the fuzzy controller in the present invention;

[0102] Figure 7 It is a schematic diagram of the output membership function of the fuzzy controller in the present invention;

[0103] Figure 8 It is a 3D schematic diagram of the input and output relationship of the fuzzy controller in the present invention;

[0104] Fig. 9 It is a trajectory tracking comparison diagram of the MPC control method of the present invention with the adaptive time domain coefficient and the traditional MPC control algorithm when tracking double lane shifts;

[0105] Fig.10 It is a comparison diagram of the error curves when the MPC control method of the adaptive time domain coefficient in the present invention and the traditional MPC control algorithm track double lane shifting;

[0106] Fig.11 It is a comparison diagram of the yaw angle when the MPC control method of the adaptive time domain coefficient in the present invention and the traditional MPC control algorithm track the double-lane-shifting trajectory;

[0107] Fig.12 It is a comparison diagram of the yaw angular velocity when the MPC control method with adaptive time domain coefficients in the present invention and the traditional MPC control algorithm track the double-lane shift trajectory. DETAILED DESCRIPTION

[0108] The present invention is further described in detail below in conjunction with embodiments:

[0109] like Figure 1 As shown, the present invention provides an improved model predictive control method for unmanned vehicle trajectory tracking, which can enable the unmanned vehicle to autonomously track and run according to a preset trajectory, and specifically includes the following steps:

[0110] Step S1: Establishing a three-degree-of-freedom nonlinear dynamic model of the vehicle, specifically comprising the following steps:

[0111] Step S101: Make reasonable assumptions and simplify the vehicle:

[0112] (1) Assume that the front wheels of the vehicle have the same steering angle and the rear wheels have zero steering angle;

[0113] (2) Assume that the suspension system and the vehicle are rigid and ignore the motion of the suspension;

[0114] (3) Ignore the effect of tire self-aligning torque;

[0115] (4) Assume that the vehicle moves only in a plane parallel to the ground, and ignore the vehicle's vertical motion, pitch motion, and roll motion;

[0116] (5) Ignore the effect of load on tire cornering characteristics;

[0117] (6) Ignore air resistance;

[0118] (7) Ignoring the impact of environmental factors;

[0119] (8) Ignore the load transfer caused by the vehicle while driving;

[0120] Step S102: Figure 2 As shown, the inertial coordinate system XOY and the vehicle's own coordinate system xoy are established, and the dynamic equilibrium equation is established at the vehicle's center of mass O, as shown in the following formula:

[0121]

[0122] In the formula, represents the velocity component of the vehicle in the X-axis direction in the inertial coordinate system, Represents the velocity component of the vehicle in the Y-axis direction in the inertial coordinate system, represents the angular velocity component of the vehicle in the inertial coordinate system, represents the vehicle velocity component function in the X-axis direction in the inertial coordinate system, Represents the vehicle velocity component function in the Y-axis direction in the inertial coordinate system;

[0123] Step S103: Convert the vehicle's own coordinate system and the inertial coordinate system. The conversion relationship between the two is shown in the following formula:

[0124]

[0125] Step S104: Calculate the acceleration component of the vehicle's mass center O in the vehicle's own coordinate system xoy, as shown in the following formula:

[0126]

[0127] Step S105: Establish a nonlinear dynamic model of the vehicle to describe the dynamic behavior of the vehicle in three-dimensional space:

[0128] The vehicle dynamics equation is established as shown below:

[0129]

[0130] In the formula, is the longitudinal acceleration of the vehicle, Indicates the lateral acceleration of the vehicle;

[0131] The longitudinal dynamic equation of the vehicle's center of mass along the x-axis is:

[0132]

[0133] The lateral dynamic equation of the vehicle's center of mass along the y-axis is:

[0134]

[0135] The yaw dynamic equation of the vehicle's center of mass along the z-axis is:

[0136]

[0137] The above three dynamic equations are integrated into a composite function form as shown below:

[0138]

[0139] In the formula, m represents the weight of the vehicle body, F xf The resultant longitudinal force of the tires on the front axle of the vehicle, F yf The resultant lateral force of the tires on the front axle, F xr Represents the resultant longitudinal force of the tires on the rear axle of the vehicle, F yr represents the resultant lateral force of the rear axle tire of the vehicle, a and b represent the distances of the front and rear wheels from the center of mass of the vehicle, respectively, and I z represents the moment of inertia of the vehicle around the z-axis;

[0140] Step S2: Formulate a linear dynamic model of the tire in combination with the magic tire formula, which specifically includes the following steps:

[0141] Step S201: Calculate the tire lateral force F using the magic formula y , as shown below:

[0142]

[0143] The calculation relationship of each parameter in the formula is as follows:

[0144]

[0145] Where a0, a1, ..., a 14 represents the model parameters, F zrepresents the vertical load of the tire, γ represents the camber angle, and α represents the side slip angle of the tire;

[0146] Fit the relationship between tire lateral force and sideslip angle to get the relationship diagram between the two:

[0147] Specifically, the tire lateral force and sideslip angle data are obtained through vehicle dynamics experiments and the magic formula is used to calculate the tire lateral force. The relevant lateral force experimental data are obtained after adjusting the lateral force related formula parameters according to the tire parameters. Then, the relationship diagram between the tire lateral force and the sideslip angle is obtained through data fitting, such as Figure 3 As shown, Figure 3 In the figure, the horizontal axis represents the slip angle, and the vertical axis represents the lateral force. The lateral force is mainly caused by the deformation of the tire under the action of the lateral force. The slip angle is the angle between the tire speed direction and its actual rolling direction. The vertical load value changes with the ground adhesion coefficient. As the ground adhesion coefficient increases, the corresponding vertical load also increases. Under the same conditions, the lateral force of the tire will change with the change of the slip angle. Figure 3 It can be seen that when the slip angle is -6°≤α≤6°, the lateral force of the tire is approximately linearly related to the slip angle, and nonlinearly related in other ranges. In addition, when the slip angle is between 0° and 20°, the lateral force of the tire gradually increases with the increase of the vertical load. When the vertical load reaches the critical value, the lateral force no longer changes due to the increase of the load.

[0148] Step S202: Calculate the longitudinal force F of the tire using the magic formula x , as shown below:

[0149]

[0150] The calculation relationship of each parameter in the formula is as follows:

[0151]

[0152] Where a0, a1, …, a 10 represents the main parameters in the longitudinal force model, λ represents the tire slip rate, F z Indicates the vertical load of the tire;

[0153] The relationship between the tire longitudinal force and the slip rate is fitted to obtain the relationship diagram between the two:

[0154] Specifically, tire longitudinal force and slip rate data are obtained through vehicle dynamics experiments and the magic formula is used to calculate tire longitudinal force. After adjusting the longitudinal force related parameters of the formula according to tire parameters, relevant longitudinal force experimental data are obtained, and then the relationship diagram between tire longitudinal force and slip rate is obtained through data fitting, such as Figure 4 As shown, Figure 4It represents the curve of tire longitudinal force changing with slip rate under different vertical load conditions. Under different vertical load conditions, this relationship curve shows different characteristics:

[0155] 1) Low slip rate stage: When the slip rate is low, the longitudinal force increases approximately linearly with the increase of slip rate, which means that when the tire just starts to slip, increasing the slip rate will significantly increase the longitudinal force;

[0156] 2) Peak point: When the slip rate reaches a certain value, the longitudinal force reaches its maximum value. This peak point usually corresponds to the best traction or braking force, that is, the slip rate at which the tire can generate the maximum traction or braking force;

[0157] 3) High slip rate stage: After exceeding the peak point, the longitudinal force begins to decrease, which indicates that as the slip rate further increases, the adhesion between the tire and the ground weakens, resulting in a decrease in longitudinal force and entering a sliding or skidding state;

[0158] In summary, the change of longitudinal force has a coupling effect on the change of lateral force, which indirectly affects the relationship between the tire lateral force and the sideslip angle. Therefore, the tire lateral force can be adjusted by adjusting the sideslip angle, which indirectly affects the change of longitudinal force and thus controls the vehicle's driving trajectory.

[0159] Step S203: Under the small range assumption (reference Figure 3 and Figure 4 The approximate linear part in the tire is expressed in a linearized way:

[0160] The lateral force of the tire is linearly expressed as shown below:

[0161] F c =C c α

[0162] In the formula, C c Indicates the tire's cornering stiffness;

[0163] The longitudinal force of the tire is linearly expressed as shown below:

[0164] F l =C l s

[0165] In the formula, C l Indicates the longitudinal stiffness of the tire;

[0166] Step S3: Formulate the objective function and constraints of the model predictive control method, which specifically includes the following steps:

[0167] Step S301: Linearize the vehicle nonlinear dynamics model established in step S1:

[0168] Establish the state space expression as shown below:

[0169]

[0170] Where ζ(t) represents the state vector of the vehicle system, u(t) represents the control input, f(·) represents the nonlinear function describing the dynamics of the vehicle system, η(t) represents the output vector of the vehicle system, and C represents the output vector matrix, which is used to map the state vector ξ(t) to the output η(t) of the vehicle system.

[0171] Perform Taylor expansion at any point (ξ,u) and ignore higher-order terms:

[0172]

[0173] In the formula, A(t) and B(t) represent the state matrix and input matrix of the linearized system, respectively. Taylor expansion at any point (ξ,u) can simplify the analysis and design of nonlinear systems and realize trajectory tracking control of the vehicle.

[0174] Step S302: discretize the vehicle nonlinear dynamics model established in step S1:

[0175] The forward Euler method is used to obtain the discrete linear time-varying model, as shown below:

[0176]

[0177] Where A(k) = I + TA(t), B(k) = TB(t); A(k) and B(k) represent the state matrix and input matrix of the discretized system, respectively, and η(k) represents the discretized form of the vehicle system output vector;

[0178] Let the new discrete linear time-varying form add the control increment, then:

[0179]

[0180] The new state space expression is obtained as shown below:

[0181]

[0182] In the formula, Δu(k)=u(k)-u(k-1); and They represent the incremental state matrix, incremental input matrix and incremental output matrix of the system after discretization, and Δu(k) represents the control increment of the system after discretization;

[0183] Step S303: predict the input and output quantities and design a cost function, as shown in the following formula:

[0184]

[0185] In the formula, J(·) represents the cost function, represents the desired road trajectory, ΔU(k) ​​represents the control increment sequence, Q represents the weight matrix of the state quantity of the model predictive control, and R represents the weight matrix of the control increment of the model predictive control. In order to make the optimal control have a solution and avoid the local minimum, the relaxation factor ε is introduced, where ρ represents the weight coefficient of the relaxation factor.

[0186] Step S304: constrain the vehicle's own characteristics:

[0187] The following constraints are applied to the vehicle's sideslip angle:

[0188]

[0189] Constraints are imposed on the control variable u (front wheel steering angle) and its control increment Δu:

[0190]

[0191] In the formula, u min with u max Indicates the front wheel turning angle u = δ f The minimum and maximum boundary values ​​of Δu min With Δu min Indicates the front wheel steering angle increment Δδ f The minimum and maximum values ​​of the boundaries;

[0192] In each sampling period, the optimization problem of the objective function is transformed into a quadratic programming problem while satisfying the control constraints:

[0193]

[0194] In the formula, min J(·) represents the minimum constraint value of the cost function, ΔU min , Δu min and u min They represent the minimum lower bound of the control increment sequence ΔU(k), the incremental input Δu(k) and the control input u(k+i), ΔU max , Δu max and u max They represent the maximum upper bounds of the control increment sequence ΔU(k), the incremental input Δu(k), and the control input u(k+i), k=0,1,…,N c -1 indicates a time domain sequence;

[0195] Step S4: Formulate rules for the fuzzy controller so that the unmanned vehicle can adaptively adjust the time domain coefficients according to the fuzzy rules to complete trajectory tracking;

[0196] like Figure 5-Figure 8 As shown, the rules of the fuzzy controller are formulated, which specifically includes the following steps:

[0197] Step S401: Establish the fuzzy controller output prediction time domain coefficient N p Changes with input yaw angle and yaw angle The affiliation between:

[0198] Design the trimf membership function and adopt the fuzzy model. The trimf membership function relationship is shown as follows:

[0199]

[0200] Where f(θ(k)) represents the fuzzy trimf membership function, θ(k) represents the membership function parameter, The left endpoint indicates the point where the degree of membership starts to rise. The vertex represents a point with a membership degree of 1. The right endpoint represents the point where the degree of membership drops to 0;

[0201] Step S402: Establish the fuzzy controller output control time domain coefficient N c Changes with input yaw angle and yaw angle The affiliation between:

[0202] The Gauss2mf membership function is designed and the fuzzy model is adopted. The Gauss2mf membership function relationship is shown as follows:

[0203]

[0204] Where c1 and c2 represent the standard deviation of the Gauss2mf membership function, σ1 and σ2 represent the central value of the Gauss2mf membership function, ω1(θ(k)) and ω2(θ(k)) represent the fuzzy Gauss2mf membership function, and θ(k) represents the membership function parameter;

[0205] The vehicle yaw angle The domain is -20° to 20°, and the trimf membership function is used. The fuzzy subsets are FD (negative large), FZ (negative medium), ZX (positive small), ZH (positive medium), and ZD (positive large); the yaw angle changes The domain is -20° to 20°, and the Gauss2mf membership function is used. The fuzzy subsets are FD (negative large), FX (negative small), SH (suitable), ZX (positive small), and ZH (neutral).

[0206] The unmanned vehicle can adaptively adjust the size of the time domain coefficient according to the fuzzy rules during the tracking process by tracking the preset trajectory according to the improved model predictive control method designed by the present invention.

[0207] Under the condition of a vehicle speed of 36 km / h, the present invention is verified by combining an example, and the MPC controller with fixed time domain coefficients is used in comparison:

[0208] From the perspective of trajectory tracking accuracy, Fig. 9 As shown, the tracking double-lane-shifting trajectories of the MPC controller with adaptive time-domain coefficients (blue curve) of the present invention are compared with those of the MPC controller with fixed time-domain coefficients (black curve). The controller of the present invention is closer to the real reference trajectory (red curve) at the bend, showing higher trajectory tracking accuracy, while the traditional controller turns significantly in advance at the bend, deviating from the expected trajectory.

[0209] From the perspective of lateral error, Fig.10 As shown, the changes in lateral errors of the MPC controller with adaptive time domain coefficients of the present invention and the MPC controller with fixed time domain coefficients during the tracking process are compared, wherein the controller of the present invention has a smaller lateral error than the controller with fixed time domain coefficients as a whole, the maximum lateral error of the improved controller is 0.5 meters, while the maximum lateral error of the traditional controller is 0.75 meters. Therefore, the adaptive time domain coefficient of the present invention can effectively improve the accuracy of trajectory tracking, so that the lateral error is within an acceptable range, thereby ensuring the stability of vehicle driving and improving comfort.

[0210] From the perspective of yaw angular velocity, Fig.11 and Fig.12 As shown in the figure, the changes of yaw angle and yaw velocity of the MPC controller with adaptive time domain coefficients are compared with those of the MPC controller with fixed time domain coefficients. At the curve, the yaw velocity of the controller with fixed time domain coefficients is insufficient, and the vehicle cannot drive along the curve along the preset trajectory. Therefore, the degree of deviation from the trajectory is relatively large, and when returning to the center at the end of the curve, the yaw velocity changes unsteadily, which can easily cause the vehicle to become unstable. However, the yaw velocity of the controller of the present invention changes more steadily, and the vehicle turns more smoothly, which makes the switching of the vehicle driving mode smoother, effectively improves the speed tracking performance, and improves the robustness of the vehicle during driving.

[0211] Therefore, the MPC controller with adaptive time domain coefficients of the present invention can adaptively adjust according to the road under low-speed tracking conditions, effectively track the preset trajectory, reduce the lateral error of tracking, and improve the trajectory tracking accuracy, thereby improving the control performance and safety of the unmanned vehicle.

[0212] It is to be understood that the present invention is described by some embodiments, and it is known to those skilled in the art that various changes or equivalent substitutions may be made to these features and embodiments without departing from the spirit and scope of the present invention. In addition, under the teachings of the present invention, these features and embodiments may be modified to adapt to specific circumstances and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the scope of protection of the present invention.

Claims

1. An improved model predictive control method for unmanned vehicle trajectory tracking, characterized in that The following steps are involved: Step S1: establishing a three-degree-of-freedom nonlinear dynamic model of the vehicle; Step S2: Formulate a linear dynamic model of the tire in combination with the magic tire formula; Step S3: Formulate the objective function and constraints of the model predictive control method; Step S4: Formulate the rules of the fuzzy controller, and the unmanned vehicle adaptively adjusts the time domain coefficients according to the fuzzy rules and completes trajectory tracking.

2. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 1, characterized in that: In step S1, establishing a three-degree-of-freedom nonlinear dynamic model of the vehicle includes the following steps: Step S101: Make reasonable assumptions and simplify the vehicle; Step S102: Establish an inertial coordinate system XOY and the vehicle's own coordinate system xoy, and establish a dynamic equilibrium equation at the vehicle's center of mass O, as shown in the following equation: In the formula, represents the velocity component of the vehicle in the X-axis direction in the inertial coordinate system, Represents the velocity component of the vehicle in the Y-axis direction in the inertial coordinate system, represents the angular velocity component of the vehicle in the inertial coordinate system, represents the vehicle velocity component function in the X-axis direction in the inertial coordinate system, Represents the vehicle velocity component function in the Y-axis direction in the inertial coordinate system; Step S103: Convert the vehicle's own coordinate system and the inertial coordinate system. The conversion relationship is shown in the following equation: Step S104: Calculate the acceleration component of the vehicle's mass center O in the vehicle's own coordinate system xoy, as shown in the following formula: Step S105: Establish a nonlinear dynamics model for describing the dynamic behavior of the vehicle in three-dimensional space.

3. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 2, characterized in that: In step S105, the vehicle dynamics equation is established as shown below: In the formula, is the longitudinal acceleration of the vehicle, Indicates the lateral acceleration of the vehicle; The longitudinal dynamic equation of the vehicle's center of mass along the x-axis is: The lateral dynamic equation of the vehicle's center of mass along the y-axis is: The yaw dynamic equation of the vehicle's center of mass along the z-axis is: The above three dynamic equations are integrated into a composite function form as shown below: In the formula, m represents the weight of the vehicle body, F xf The resultant longitudinal force of the tires on the front axle of the vehicle, F yf The resultant lateral force of the tires on the front axle, F xr Represents the resultant longitudinal force of the tires on the rear axle of the vehicle, F yr represents the resultant lateral force of the rear axle tire of the vehicle, a and b represent the distances of the front and rear wheels from the center of mass of the vehicle, respectively, and I z Represents the vehicle's moment of inertia about the z-axis.

4. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 3, characterized in that: In step S2, formulating a linear dynamic model of the tire in combination with the magic tire formula includes the following steps: Step S201: Calculate the tire lateral force F using the magic formula y , and the relationship diagram between tire lateral force and sideslip angle is obtained by fitting; Among them, the lateral force F of the tire is calculated by combining the magic formula y , as shown below: The calculation relationship of each parameter in the formula is as follows: Where a0, a1, ..., a 14 represents the model parameters, F z represents the vertical load of the tire, γ represents the camber angle, and α represents the side slip angle of the tire; Step S202: Calculate the longitudinal force F of the tire using the magic formula x , and the relationship diagram between the tire longitudinal force and the slip rate is obtained by fitting; Among them, the longitudinal force F of the tire is calculated by combining the magic formula x , as shown below: The calculation relationship of each parameter in the formula is as follows: Where a0, a1, …, a 10 represents the main parameters in the longitudinal force model, λ represents the tire slip rate, F z Indicates the vertical load of the tire; Step S203: Under a small range assumption, the lateral force and longitudinal force of the tire are expressed in a linear manner; Among them, the lateral force of the tire is linearly expressed as shown in the following formula: F c =C c a In the formula, C c Indicates the tire's cornering stiffness; The longitudinal force of the tire is linearly expressed as shown below: F l =C l s In the formula, C l Indicates the longitudinal stiffness of the tire.

5. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 4, characterized in that: In step S3, formulating the objective function and constraints of the model predictive control method includes the following steps: Step S301: linearizing the vehicle nonlinear dynamics model established in step S1; Step S302: discretizing the vehicle nonlinear dynamics model established in step S1; Step S303: predicting the input and output quantities and designing a cost function; Step S304: constrain the vehicle's own characteristics.

6. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 5, characterized in that: In step S301, a state space expression is established, as shown in the following formula: Where ζ(t) represents the state vector of the vehicle system, u(t) represents the control input, f(·) represents the nonlinear function describing the dynamics of the vehicle system, η(t) represents the output vector of the vehicle system, and C represents the output vector matrix, which is used to map the state vector ξ(t) to the output η(t) of the vehicle system. Perform Taylor expansion at any point (ξ,u) and ignore higher-order terms: In the formula, A(t) and B(t) represent the state matrix and input matrix of the linearized system respectively.

7. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 6, characterized in that: In step S302, the forward Euler method is used to obtain a discrete linear time-varying model, as shown in the following formula: Where A(k) = I + TA(t), B(k) = TB(t); A(k) and B(k) represent the state matrix and input matrix of the discretized system, respectively, and η(k) represents the discretized form of the vehicle system output vector; Let the new discrete linear time-varying form add the control increment, then: The new state space expression is obtained as shown below: In the formula, Δu(k)=u(k)-u(k-1); and They represent the incremental state matrix, incremental input matrix and incremental output matrix of the system after discretization respectively, and Δu(k) represents the control increment of the system after discretization.

8. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 7, characterized in that: In step S303, the input and output quantities are predicted and a cost function is designed, as shown in the following formula: In the formula, J(·) represents the cost function, represents the desired road trajectory, ΔU(k) ​​represents the control increment sequence, Q represents the weight matrix of the state quantity of the model predictive control, R represents the weight matrix of the control increment of the model predictive control, ε represents the relaxation factor, and ρ represents the weight coefficient of the relaxation factor.

9. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 8, characterized in that: In step S304, the following constraints are imposed on the sideslip angle of the vehicle: Constraints are imposed on the control variable front wheel steering angle u and its control increment Δu: In the formula, u min with u max Indicates the front wheel turning angle u = δ f The minimum and maximum values ​​of the boundaries, Δu min With Δu min Indicates the front wheel steering angle increment Δδ f The minimum and maximum values ​​of the boundaries; In each sampling period, the optimization problem of the objective function is transformed into a quadratic programming problem while satisfying the control constraints: In the formula, min J(·) represents the minimum constraint value of the cost function, ΔU min , Δu min and u min They represent the minimum lower bound of the control increment sequence ΔU(k), the incremental input Δu(k), and the control input u(k+i), ΔU max , Δu max and u max They represent the maximum upper bounds of the control increment sequence ΔU(k), the incremental input Δu(k), and the control input u(k+i), k=0,1,…,N c -1 indicates a time domain sequence.

10. The improved model predictive control method for unmanned vehicle trajectory tracking according to claim 9, characterized in that: In step S4, formulating the rules of the fuzzy controller includes the following steps: Step S401: Establish the fuzzy controller output prediction time domain coefficient N p Changes with input yaw angle and yaw angle The affiliation between: Design the trimf membership function and adopt the fuzzy model. The trimf membership function relationship is shown as follows: Where f(θ(k)) represents the fuzzy trimf membership function, θ(k) represents the membership function parameter, The left endpoint indicates the point where the degree of membership starts to rise. The vertex represents a point with a membership degree of 1. The right endpoint represents the point where the degree of membership drops to 0; Step S402: Establish the fuzzy controller output control time domain coefficient N c Changes with input yaw angle and yaw angle The affiliation between: The Gauss2mf membership function is designed and the fuzzy model is adopted. The Gauss2mf membership function relationship is shown as follows: Where c1 and c2 represent the standard deviation of the Gauss2mf membership function, σ1 and σ2 represent the central value of the Gauss2mf membership function, ω1(θ(k)) and ω2(θ(k)) represent the fuzzy Gauss2mf membership function, and θ(k) represents the membership function parameter.

Images