Parking trajectory tracking control method based on model predictive control and PID control
By combining model prediction control and PID control, the problem of ignoring longitudinal speed control in existing parking technology is solved, and the precise path tracking of the vehicle during parking is achieved and the vehicle speed stability is improved, and the comfort and stability of automatic parking is improved. sex.
Patent Information
- Application Number
- CN202410947318.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-16
- Publication Date
- 2024-11-12
AI Technical Summary
Existing parking technology ignores longitudinal speed control in path tracking control, resulting in the inability to accurately track the desired path, which may lead to collision accidents and vehicle comfort.
Using a method based on model prediction control and PID control, the front wheel angle and vehicle speed are calculated through the microcontroller, and the vehicle controller determines the steering wheel angle and throttle opening or braking pressure to realize the horizontal and vertical control of the vehicle, and the model prediction algorithm is used. Establish a vehicle kinematics model, perform optimization control sequence solutions, and adjust parameters in combination with fuzzy PID control to ensure that the vehicle speed is stable near the expected value.
Improves the comfort and stability of automatic parking, reduces vehicle speed fluctuations, ensures that the vehicle can accurately track the path and performs effective longitudinal speed control.
Smart Images

Figure CN118928365A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a parking trajectory tracking control method, in particular to a parking trajectory tracking control method based on model predictive control and PID control. Background Art
[0002] There are two main methods for path tracking control in existing parking technologies. One is a control algorithm based on the target path, and the other is an intelligent control method based on experience. The first method first plans a parking path that meets the vehicle kinematics and EPS performance requirements and avoids obstacles based on the parking environment information detected by the on-board sensors. Then, a control method for the vehicle parking motion is designed. By controlling the vehicle's heading angle or speed, the vehicle can track the reference path with high accuracy, thereby completing the entire parking process. The second method does not require the design of a reference path, but instead designs a parking controller by analyzing the parking behavior of experienced drivers. The parking environment information detected by the on-board sensors and the current vehicle status and position are fed back to the controller. The controller generates a control strategy in real time based on the feedback information to control the vehicle to park. The experience-based intelligent control method has high requirements for the design of the controller, and requires the collection and processing of a large amount of experimental data, which is difficult to implement.
[0003] When performing tracking control, most control methods currently only consider the lateral control of the vehicle and ignore the control of the longitudinal speed of the vehicle. When the difference between the expected speed and the actual speed is large, the control effect of path tracking will be greatly different from the expected path. The path control of parking requires relatively precise control. If the deviation is too large, the vehicle cannot be parked correctly in the target parking space, and even a collision accident may occur. In addition, it will also have a certain impact on the comfort of the vehicle. Summary of the invention
[0004] The technical problem to be solved by the present invention is to provide a parking trajectory tracking control method based on model predictive control and PID control.
[0005] The technical solution adopted by the present invention to solve the above technical problems is: a parking trajectory tracking control method based on model predictive control and PID control, the control method is: a microcontroller calculates the required front wheel angle and vehicle speed, and then transmits them to the vehicle controller, the vehicle controller determines the steering wheel angle according to the front wheel angle, determines the throttle opening and brake pressure according to the vehicle speed, and controls the vehicle's actuator according to the steering wheel angle, the throttle opening and the brake pressure. The microcontroller is a controller based on a model predictive control algorithm, and the model predictive control algorithm includes: firstly, establishing a prediction model to predict future states and outputs; then solving the optimal solution of the system according to an objective function with constraints to obtain an optimal control sequence in the prediction time domain; then acting the first element of the control sequence on the controlled object;
[0006] (1) The method for establishing the prediction model is as follows:
[0007] Establish the vehicle kinematic model and select the rear axle center O r As a reference point, the vehicle position coordinates are (x, y);
[0008] The kinematic equation of the vehicle is:
[0009]
[0010] Where (x, y) is the vehicle position coordinate; υ is the vehicle speed; φ is the equivalent front wheel turning angle; θ is the heading angle; LL f -L r is the vehicle wheelbase;
[0011] Consider the vehicle kinematic model as the control quantity and a control system with state variables χ = [xyθ];
[0012] The vehicle kinematic model is approximately linearized using the Taylor formula. The error model expression after linearization is:
[0013]
[0014] υ r is the vehicle speed at reference point r; is the equivalent front wheel turning angle of the reference point r; θ r is the heading angle of the reference point r;
[0015] Construct a new state vector:
[0016]
[0017] Get the new state space expression:
[0018]
[0019] The output equation is:
[0020]
[0021] E is a unit vector;
[0022] Y = ψζ(k) + ΘΔU;
[0023]
[0024] N P is the prediction time domain in the model prediction algorithm; (2) the objective function is:
[0025]
[0026] is the Kronecker product, Q is the output weight matrix, and R is the control increment weight matrix;
[0027] (3) The constraints are:
[0028]
[0029] U min and U max They are respectively the minimum and maximum values of the control quantity in the control time domain.
[0030] Preferably, in the parking trajectory tracking control method based on model predictive control and PID control, the method for determining the throttle opening and the brake pressure according to the vehicle speed is:
[0031] (1) System initialization;
[0032] Set the desired speed v d ;
[0033] Set the initial parameters of the PID controller, including the proportional coefficient k p , integration coefficient k i and the differential coefficient k d ;
[0034] Initialize the input and output variable domain and fuzzy sets of the fuzzy controller;
[0035] (2) Vehicle speed error calculation:
[0036] Measure actual vehicle speed v r ;
[0037] Calculate the vehicle speed error e=v d -v r ;
[0038] Calculate the speed error change rate e c =(e cu -e pr ) / Δt, where e cu is the current error, e pr is the last error, Δt is the sampling time interval;
[0039] (3) Fuzzy PID control:
[0040] a. Fuzzification:
[0041] The speed error e and error change rate e c Fuzzification, mapping to the domain of fuzzy sets;
[0042] b. Fuzzy rule reasoning:
[0043] According to the preset fuzzy control rules, based on e and e c The value of , infers the adjustment amount Δk of the PID control parameter p , Δk i and Δk d ; The fuzzy control rule is: if the error is large and the error change rate is also large, increase the proportional coefficient to quickly reduce the error;
[0044] c. Defuzzification:
[0045] The inferred Δk p , Δk i and Δk d Defuzzification, converting into specific numerical values;
[0046] d.PID parameter adjustment:
[0047] Update the parameters of the PID controller:
[0048]
[0049] e.PID calculation:
[0050] The updated PID parameters are used to calculate the throttle opening adjustment amount or the brake pressure adjustment amount.
[0051] Preferably, the parking trajectory tracking control method based on model predictive control and PID control also includes:
[0052] a.Throttle control:
[0053] If v r <v d , then the throttle actuator is controlled according to the throttle opening adjustment amount calculated by PID;
[0054] b. Braking control:
[0055] If v r >v d , then the brake system is controlled according to the brake pressure adjustment calculated by PID
[0056] The beneficial effects of the present invention are as follows: the present invention adopts a model predictive control algorithm to perform lateral control on a vehicle, and uses a PID algorithm to control the longitudinal speed of parking, thereby reducing the fluctuation of vehicle speed and maintaining it near the desired speed, thereby improving the comfort and stability of automatic parking. The control method of the present invention can not only perform lateral control on a parked vehicle, but also perform longitudinal speed control. BRIEF DESCRIPTION OF THE DRAWINGS
[0057] Figure 1is a principle block diagram of the parking trajectory tracking control method of the present invention;
[0058] Figure 2 It is a schematic diagram of the kinematic coordinates of the vehicle;
[0059] Figure 3 This is a principle block diagram of the method for determining the throttle opening and brake pressure according to the vehicle speed in the present invention.
[0060] Figure 4 It is a tracking error diagram formed by simulation using the parking trajectory tracking control method of the present invention. DETAILED DESCRIPTION
[0061] The present invention will now be described in further detail in conjunction with the accompanying drawings and preferred embodiments. These drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.
[0062] A parking trajectory tracking control method based on model predictive control and PID control, such as Figure 1 As shown, the control method is: the microcontroller calculates the required front wheel angle and vehicle speed, and then transmits it to the vehicle controller. The vehicle controller determines the steering wheel angle according to the front wheel angle, determines the throttle opening and brake pressure according to the vehicle speed, and controls the vehicle's actuator according to the steering wheel angle, throttle opening and brake pressure. The microcontroller is a controller based on the model predictive control algorithm. The model predictive control algorithm includes: first, establishing a prediction model to predict future states and outputs; then solving the optimal solution of the system according to the objective function with constraints to obtain the optimal control sequence in the prediction time domain; then the first element of the control sequence acts on the controlled object. In a preferred embodiment, the optimal control quantity of the system in the current state can also be output through the feedback correction process.
[0063] (1) The method for establishing the prediction model is as follows:
[0064] During the parking process, the vehicle's roll and tire slip can be ignored due to the low speed, and the wheels are considered to only roll and turn. Based on the Ackerman steering principle, the rectangle enclosed by the maximum length and width of the vehicle is used as the vehicle contour boundary, such as Figure 2 As shown in the figure, the vehicle kinematic model is established. Since the movement direction of the rear wheel of the vehicle is always the same as the movement direction of the vehicle body, the rear axle center O is selected r As a reference point, the vehicle position coordinates are (x,y).
[0065] according to Figure 2 , we get the following differential equation:
[0066]
[0067] The kinematic equation of the vehicle is obtained from formula (1):
[0068]
[0069] In formulas (1) and (2), (x, y) is the vehicle position coordinate; υ is the vehicle speed; φ is the equivalent front wheel steering angle; θ is the heading angle; LL f -L r is the vehicle wheelbase.
[0070] Formula (2) can be regarded as the control quantity For a control system with a state variable χ = [xy θ], any reference point on the reference trajectory is represented by r.
[0071] Rewrite equation (2) as:
[0072]
[0073] The above vehicle kinematic model is nonlinear, so it is necessary to make linear changes to the above model in order to describe the vehicle motion state in the form of state space equations, thereby realizing lateral tracking control.
[0074] The original model is approximately linearized using Taylor's formula, and the linear model is:
[0075]
[0076] Combining equations (3) and (4), we can get the error model expression after linearization:
[0077]
[0078] In the control process, sampling is performed continuously in the time domain. The control process is a discrete process, so the above linear continuous model needs to be transformed into a linear discrete form:
[0079]
[0080] Where T is the sampling time and E is the unit vector.
[0081] Based on MPC control theory, the control increment is represented by the difference between the current control amount and the control amount in the previous sampling period. Therefore, it is necessary to further improve the model and construct a new state vector:
[0082]
[0083] Get the new state space expression:
[0084]
[0085] The output equation is:
[0086]
[0087] E is a unit vector;
[0088] After constructing the above linear error model, the model is used to predict the state quantity and output quantity. The derivation process is shown in equation (12) and equation (13) respectively:
[0089]
[0090] In formula (12) and formula (13), N P is the prediction time domain in the model prediction algorithm. Combining the above derivation, we can get the output equation:
[0091] Y = ψζ(k) + ΘΔU (14);
[0092] in:
[0093]
[0094]
[0095] (2) Constructing the objective function
[0096] In the actual control process, the control increment in the system control time domain is unknown, and a reasonable optimization objective function needs to be designed, and then the optimization objective is solved to calculate the control increment in the control time domain. The standard form of the quadratic programming of the optimization objective function set by the present invention is:
[0097]
[0098] Where: Q is the output weight matrix, R is the control increment weight matrix. In the control process, it is hoped that there is only one independent variable, the control increment, so the formula (15) is transformed as follows:
[0099]
[0100] In formula (16), is the Kronecker product, Q is the output weight matrix, and R is the control increment weight matrix;
[0101] The objective function is obtained:
[0102]
[0103] (3) Constraint Analysis
[0104] During the actual driving process, the vehicle is restricted by the vehicle's kinematic characteristics. In order to ensure that the vehicle drives smoothly along the desired path, it is necessary to constrain the control quantity and control increment.
[0105] The control quantity increment constraint is:
[0106] ΔU min ≤ΔU≤ΔU max (18);
[0107] In formula (18), U min and U max They are respectively the minimum and maximum values of the control quantity in the control time domain.
[0108] For the constraints of the control quantity, it is necessary to perform formula recursion and then express it using the control quantity increment. It is known that the control quantity at a certain moment can be composed of the control quantity at the previous moment and the control quantity increment at this moment, that is:
[0109]
[0110] In formula (19), N C is the control time domain. Rewrite equation (19) as:
[0111]
[0112] Simplifying formula (20), we can get the relationship between the control quantity and the control increment:
[0113] U=U t +M I ΔU (20);
[0114] Using the constraints of the control quantity, the constraint function is rewritten into the form of control quantity increment:
[0115]
[0116] Formula (21) can be simplified as:
[0117]
[0118] U min and U max They are respectively the minimum and maximum values of the control quantity in the control time domain.
[0119] Finally, the first item of the obtained control sequence is used as the incremental input of the current control quantity, thereby realizing the tracking control of the vehicle trajectory.
[0120] like Figure 3 As shown in the figure, the method for determining the throttle opening and brake pressure according to the vehicle speed is:
[0121] (1) System initialization:
[0122] Set the desired speed v d ;
[0123] Set the initial parameters of the PID controller, including the proportional coefficient k p , integration coefficient k i and the differential coefficient k d ;
[0124] Initialize the input and output variable domain and fuzzy sets of the fuzzy controller.
[0125] (2) Vehicle speed error calculation:
[0126] Measure actual vehicle speed v r ;
[0127] Calculate the vehicle speed error e=v d –v r ;
[0128] Calculate the speed error change rate e c =(e cu –e pr ) / Δt, where e cu is the current error, e pr is the last error, Δt is the sampling time interval;
[0129] (3) Fuzzy PID control:
[0130] a. Fuzzification:
[0131] The speed error e and error change rate e c Fuzzification, mapping to the domain of fuzzy sets.
[0132] b. Fuzzy rule reasoning:
[0133] According to the preset fuzzy control rules, based on e and e c The value of , infers the adjustment amount Δk of the PID control parameter p , Δk i and Δk d Fuzzy rules are usually designed based on experience and system characteristics. Specifically, the present invention is based on the rule that if the error is large and the error change rate is also large, the proportional coefficient is increased to quickly reduce the error.
[0134] c. Defuzzification:
[0135] The inferred Δk p , Δk i and Δk d Defuzzify and convert into specific numerical values.
[0136] d.PID parameter adjustment:
[0137] Update the parameters of the PID controller:
[0138]
[0139] e.PID calculation:
[0140] The updated PID parameters are used to calculate the adjustment amount of the throttle opening or the brake pressure.
[0141] (4) Throttle / brake control:
[0142] a.Throttle control:
[0143] If acceleration is required (i.e. v r <v d ), controls the throttle actuator according to the throttle opening adjustment calculated by PID. The throttle opening is usually between 0% (closed) and 100% (fully open), and the specific value depends on the specific implementation and calibration of the vehicle.
[0144] b. Braking control:
[0145] If you need to slow down (i.e. v r >v d ), the brake system is controlled according to the brake pressure adjustment calculated by PID. Braking pressure is usually a relative value, 0 means no braking, and 100% means maximum braking pressure.
[0146] In order to verify the effect of this method, a simulation experiment was carried out taking horizontal parking path tracking as an example.
[0147] (1) The vehicle parameters and parallel parking space parameters used are:
[0148] (2) The design parameters of the controller based on the model predictive control algorithm are:
[0149] The number of state quantities Nx = 3;
[0150] The number of control quantities Nu=2, wherein the control quantity and control quantity increment for vehicle speed are set to: -3m / s≤υ≤3m / s, -0.04m / s≤Δυ≤0.04m / s; the control quantity and control quantity increment for the front wheel steering angle of the vehicle are set to: -33deg≤φ≤33deg, -0.5deg≤Δφ≤0.5deg;
[0151] Prediction step size Np = 26;
[0152] Control step size Nc=2;
[0153] Relaxation factor Row = 10;
[0154] The simulation step size dt=0.04.
[0155] (3) The relevant parameters of fuzzy PID control are:
[0156] Set the desired speed v d =3m / s.
[0157] Set the initial parameters of the PID controller, including the proportional coefficient k p =1.2, integral coefficient k i = 0.3 and differential coefficient k d =0.1.
[0158] (4) Simulation using Matlab, Figure 4 is the tracking error between the actual tracking trajectory and the expected trajectory, Figure 4 It can be seen that the error in the entire parking process does not exceed 0.015m, and the two peaks of the error correspond to the two turns of the vehicle.
[0159] The present invention adopts a model predictive control algorithm to control the lateral direction of the vehicle, and uses a PID algorithm to control the longitudinal speed of the parking vehicle, thereby reducing the fluctuation of the vehicle speed and keeping it near the expected speed, thereby improving the comfort and stability of automatic parking. The control method of the present invention can not only control the lateral direction of the parked vehicle, but also control the longitudinal speed.
[0160] The above description only describes the specific implementation mode of the present invention. Various examples do not limit the essential content of the present invention. After reading the description, ordinary technicians in the relevant technical field can modify or deform the specific implementation mode described above without departing from the essence and scope of the invention.
Claims
1. A parking trajectory tracking control method based on model predictive control and PID control, the control method comprising: a microcontroller calculates the required front wheel angle and vehicle speed, and then transmits them to a vehicle controller, the vehicle controller determines the steering wheel angle according to the front wheel angle, determines the throttle opening and brake pressure according to the vehicle speed, and controls the vehicle's actuator according to the steering wheel angle, the throttle opening and the brake pressure. Features: The microcontroller is a controller based on a model predictive control algorithm, which includes: firstly, establishing a prediction model to predict future states and outputs; then, solving the optimal solution of the system according to an objective function with constraints to obtain an optimal control sequence in the prediction time domain; then, applying the first element of the control sequence to the controlled object; (1) The method for establishing the prediction model is as follows: Establish the vehicle kinematic model and select the rear axle center O r As a reference point, the vehicle position coordinates are (x, y); The kinematic equation of the vehicle is: Where (x, y) is the vehicle position coordinate; υ is the vehicle speed; φ is the equivalent front wheel turning angle; θ is the heading angle; LL f -L r is the vehicle wheelbase; Consider the vehicle kinematic model as the control quantity and a control system with state variables χ = [xy θ]; The vehicle kinematic model is approximately linearized using the Taylor formula. The error model expression after linearization is: v r is the vehicle speed at reference point r; is the equivalent front wheel turning angle of the reference point r; θ r is the heading angle of the reference point r; Construct a new state vector: Get the new state space expression: The output equation is: E is a unit vector; Y = ψζ(k) + ΘΔU; N P It is the prediction time domain in the model prediction algorithm; (2) The objective function is: is the Kronecker product, Q is the output weight matrix, and R is the control increment weight matrix; (3) The constraints are: U min and U max They are respectively the minimum and maximum values of the control quantity in the control time domain.
2. The parking trajectory tracking control method based on model predictive control and PID control according to claim 1, Features: The method for determining the throttle opening and brake pressure according to the vehicle speed is: (1) System initialization; Set the desired speed v d ; Set the initial parameters of the PID controller, including the proportional coefficient k p , integration coefficient k i and the differential coefficient k d ; Initialize the input and output variable domain and fuzzy sets of the fuzzy controller; (2) Vehicle speed error calculation: Measure actual vehicle speed v r ; Calculate the vehicle speed error e=v d -v r ; Calculate the speed error change rate e c =(e cu -e pr ) / Δt, where e cu is the current error, e pr is the last error, Δt is the sampling time interval; (3) Fuzzy PID control: a. Fuzzification: The speed error e and error change rate e c Fuzzification, mapping to the domain of fuzzy sets; b. Fuzzy rule reasoning: According to the preset fuzzy control rules, based on e and e c The value of , infers the adjustment amount Δk of the PID control parameter p , Δk i and Δk d ; The fuzzy control rule is: if the error is large and the error change rate is also large, increase the proportional coefficient to quickly reduce the error; c. Defuzzification: The inferred Δk p , Δk i and Δk d Defuzzification, converting into specific numerical values; d.PID parameter adjustment: Update the parameters of the PID controller: e.PID calculation: The updated PID parameters are used to calculate the throttle opening adjustment amount or the brake pressure adjustment amount.
3. The parking trajectory tracking control method based on model predictive control and PID control according to claim 2, Features: Also includes a.Throttle control: If v r <v d , then the throttle actuator is controlled according to the throttle opening adjustment amount calculated by PID; b. Braking control: If v r >v d , the braking system is controlled according to the brake pressure adjustment calculated by PID.
