An automatic driving vehicle control method and device based on MPC control

Abstract

The present application belongs to the field of automatic driving automatic control technology, and particularly relates to an automatic driving vehicle control method and device based on MPC control. According to different working conditions, the present application calculates the steering wheel angle change rate constraint in real time and self-adaptively, and then according to a dynamics error model, a control sequence in a control time domain is obtained by solving a target function under the condition of satisfying state, control quantity and control quantity change rate constraints, so that the demand of different working conditions on the control quantity change rate is met while the lateral comfort is improved. In addition, for the smooth transition from the actual steering wheel angle to the target steering wheel angle when the vehicle just enters the automatic driving, and for the slow driving of the vehicle in the smooth process, the tire wear during the steering wheel in-place return is avoided, and the steering wheel control comfort is improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving and automatic control technology, specifically relating to an autonomous vehicle control method and device based on MPC control. Background Technology

[0002] Trajectory tracking control of autonomous vehicles is one of the key technologies for realizing autonomous driving. The lateral control function in trajectory tracking mainly reduces the lateral deviation between the vehicle's current trajectory and the target trajectory by controlling the steering angle of the vehicle's front wheels, so that the vehicle can travel along the planned target trajectory.

[0003] In existing technologies, the most commonly used method for implementing trajectory tracking control is proportional-integral-derivative control (PID control). PID control is relatively simple, but its asymptotic stability is poor, resulting in low stability of vehicle lateral control and low tracking accuracy.

[0004] Model Predictive Control (MPC) offers good control tracking accuracy, thus effectively addressing the aforementioned issues. The principle of existing lateral driving MPC control is as follows: Figure 1 As shown, the feedforward steering wheel angle calculated by trajectory curvature and the feedback steering wheel angle calculated by MPC are first added together. Then, the target steering wheel angle obtained after addition is subjected to amplitude limiting to obtain the target steering wheel angle applied to the controlled vehicle. The feedback steering wheel angle calculated by MPC is mainly based on the vehicle's dynamic error model and the current state variables, which can predict the system output over a period of time (i.e., the prediction time domain and the control time domain). Under the conditions of satisfying the objective function and constraints on the state, control variables, and the rate of change of control variables, the objective function is solved to obtain the control sequence in the control time domain. Then, the first value in the control time domain is used as the current output value and applied to the controlled object. The above process is repeated in the next cycle, continuously completing constrained optimization problems, thereby achieving continuous lateral control of the vehicle.

[0005] Although MPC considers the control variable rate of change constraint when solving the objective function, the control variable rate of change constraint is generally the maximum speed at which the steer-by-wire system can respond. It does not constrain according to the specific operating conditions and cannot adapt to different vehicle speeds and curves with different curvatures. Summary of the Invention

[0006] The purpose of this invention is to provide an autonomous vehicle control method and device based on MPC control, in order to solve the problem that the existing autonomous driving lateral MPC control method cannot adapt to all operating conditions.

[0007] To address the aforementioned technical problems, this invention provides an autonomous vehicle control method based on MPC control, comprising the following steps:

[0008] 1) Obtain the maximum steering wheel angle change limit Δu based on the curvature of the target trajectory. k_lmt (k), and obtain the maximum steering wheel angle change limit Δu at the current vehicle speed based on the actual vehicle speed. v_lmt (k), taking the minimum of the two limit values ​​yields the maximum constraint Δu on the rate of change of the steering wheel angle. max (k);

[0009] 2) Based on the difference between the target position and the actual position, and combined with the dynamic error model, the maximum constraint Δu of the steering wheel angle change rate must be satisfied. max The constraint condition (k) is used to obtain the feedback steering wheel angle at the current moment based on MPC control; and the target steering wheel angle at the current moment is obtained based on the feedback steering wheel angle at the current moment, so as to perform lateral control of the vehicle.

[0010] Its beneficial effects are as follows: This invention adaptively calculates the steering wheel angle change rate limit according to different working conditions. Specifically, it calculates the maximum steering wheel angle change rate limit for the corresponding front curvature and the maximum steering wheel angle change rate limit for the current speed based on different curvatures and vehicle speeds. The minimum of the two is used to constrain the steering wheel angle change rate. Then, based on the dynamic error model, the objective function is solved under the conditions of satisfying the constraints of state, control quantity, and control quantity change rate to obtain the control sequence in the control time domain. This can meet the requirements of steering wheel angle control quantity change rate for different curvatures and vehicle speeds while improving lateral comfort.

[0011] Furthermore, if the vehicle is transitioning from non-autonomous driving to autonomous driving, then when entering autonomous driving, the initial steering wheel angle during MPC control is set to the actual steering wheel angle currently fed back by the vehicle; otherwise, the initial steering wheel angle during MPC control is set to the target steering wheel angle output at the current moment.

[0012] Its beneficial effect is that when the vehicle first enters autonomous driving mode, the initial steering wheel angle during MPC control is set to the actual steering wheel angle currently fed back by the vehicle, so as to avoid poor comfort caused by sudden changes in steering wheel angle.

[0013] Furthermore, the target steering wheel angle at the current moment is the sum of the feedback steering wheel angle at the current moment and the feedforward steering wheel angle at the current moment, and the feedforward steering wheel angle at the current moment is obtained by curvature compensation calculation of the curvature of the target trajectory.

[0014] Its beneficial effect is that adding the result of feedforward control can improve the lag caused by feedback control.

[0015] Furthermore, the dynamic error model is as follows:

[0016]

[0017] In the formula, e cg , These are the lateral deviation and the rate of change of the lateral deviation, respectively; θ e , These are the orientation angle deviation and the rate of change of orientation angle deviation, respectively; I z C is the vehicle's moment of inertia. f For the front wheel lateral stiffness, C r For the rear wheel lateral stiffness, l r l is the distance from the center of gravity to the rear wheel. f V is the distance from the center of gravity to the front wheel, m is the total mass of the vehicle, and V is the distance from the center of gravity to the front wheel. x Let δ be the component of the vehicle's speed along the direction of its front. f For the front wheel deflection angle, is the derivative of the heading angle.

[0018] Furthermore, the optimization objective function used in MPC control is:

[0019]

[0020] In the formula, J is the set objective function; x(k) is the state matrix; u(k) is the control variable, which is the steering wheel angle; N is the prediction and control time domain; x k For the predicted state matrix; x r U is the reference state matrix; Q is the state weighting matrix; u k R is the steering wheel angle matrix; R is the control weighting matrix.

[0021] Furthermore, the constraint function used in MPC control is:

[0022] x(k+1)=A d x(k)+B d u(k)

[0023] x min ≤x(k)≤x max

[0024] u_min≤u(k)≤u_max

[0025] |Δu(k)|≤Δu max

[0026] x(0)=x0

[0027] u(0)=u0

[0028] A d=(I-0.5A*T) s ) -1 (I+0.5A*T s )

[0029] B d =B*T s

[0030]

[0031]

[0032]

[0033] In the formula, x min and x max These represent the minimum and maximum constraints of the state matrix, respectively; u_min and u_max represent the minimum and maximum constraints of the control quantity, respectively; Δu(k) is the rate of change of the control quantity; Δu max The maximum constraint for the rate of change of the control quantity is defined by x0, where x0 is the initial state, x(0) is the matrix of lateral distance deviation, lateral distance deviation rate of change, heading angle deviation, and heading angle deviation rate of change at the current time, u(0) is the control quantity at the initial time, u0 is the actual steering wheel angle at the current time, and T s To control the cycle.

[0034] Furthermore, the target steering wheel angle at the current moment also needs to be limited.

[0035] Its beneficial effect is to limit the target steering wheel angle to ensure the stable and reliable operation of the system.

[0036] Furthermore, when the rate of change of steering wheel angle exceeds the normal gradient limit, the vehicle speed is controlled to be the reference speed v of the target trajectory. ref (k) and the set vehicle speed v set The smaller of the values ​​is used; otherwise, the vehicle speed is controlled to respond to the target trajectory.

[0037] Its beneficial effects are as follows: Considering that when a vehicle first enters autonomous driving mode, there is usually a large step difference between the actual steering wheel angle and the target steering wheel angle, if the target steering wheel angle is directly responded to, the steering wheel will suddenly change to the target angle, resulting in poor comfort. Therefore, the actual steering wheel angle is smoothly transitioned to the target steering wheel angle. When the rate of change of the steering wheel angle is large, the longitudinal response is controlled at a low speed (the smaller of the reference speed of the target trajectory and the set speed) during the steering wheel return to center. This avoids tire wear during the steering wheel return to center and improves steering wheel control comfort.

[0038] To address the aforementioned technical problems, the present invention also provides an autonomous vehicle control device based on MPC control, comprising a memory and a processor. The processor is used to execute computer program instructions stored in the memory to implement the autonomous vehicle control method based on MPC control described above, and can achieve the same beneficial effects as the method. Attached Figure Description

[0039] Figure 1 This is a schematic diagram of the lateral MPC control principle in existing autonomous driving technology;

[0040] Figure 2 This is a flowchart of the present invention for solving the MPC feedback steering wheel angle considering the constraint of the rate of change of steering wheel angle;

[0041] Figure 3 This is a flowchart of the steering wheel angle smoothing and speed limiting process when entering autonomous driving mode according to the present invention. Detailed Implementation

[0042] The main concept of this invention is to adaptively calculate the steering wheel angle change rate constraint in real time according to different working conditions, and then solve the objective function according to the dynamic error model under the conditions of satisfying the state, control quantity and control quantity change rate constraints to obtain the control sequence in the control time domain. This improves lateral comfort while meeting the control quantity change rate requirements of different working conditions. Moreover, when entering autonomous driving, the steering wheel angle smoothly transitions from the actual steering wheel angle to the target steering wheel angle, and the vehicle is controlled to drive slowly during the smoothing process. This avoids tire wear during the steering wheel return to center and improves steering wheel control comfort.

[0043] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. The following examples will help those skilled in the art to further understand this invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of this invention, and these all fall within the protection scope of this invention.

[0044] Method Implementation Examples:

[0045] This invention first adaptively calculates the steering wheel angle change rate constraint in real time according to different operating conditions. Then, based on the dynamic error model, it solves the objective function under the constraints of state, control quantity, and control quantity change rate to obtain the feedback steering wheel angle in the control time domain. The flowchart for solving the MPC feedback steering wheel angle considering the steering wheel angle change rate constraint is shown below. Figure 2 As shown.

[0046] 1) Based on the curvature k of the target trajectory, look up the table (obtained from the curvature calibration based on the minimum turning radius of the actual vehicle and the driving conditions) to obtain the maximum steering wheel angle change limit Δu for the current curvature. k_lmt (k).

[0047] 2) Based on v(k), look up the table (obtained from the vehicle speed calibration under driving conditions) to obtain the maximum steering wheel angle change limit Δu at the current vehicle speed. v_lmt (k).

[0048] 3) Taking the smaller of the two results obtained in steps 1) and 2) yields the maximum constraint on the rate of change of the steering wheel angle, i.e., Δu. max (k)=min(Δu k_lmt (k), Δu v_lmt (k)).

[0049] 4) That is, according to the dynamic error model, in state (x) min x max ), control quantity u min u max and the rate of change of the control quantity Δu max (k) Solving the objective function under constraints yields the control sequence in the control time domain, i.e., the feedback steering wheel angle u at the current moment. cmd (k). The vehicle's dynamic error model is as follows:

[0050]

[0051] In the formula, e cg , These are the lateral deviation and the rate of change of the lateral deviation, respectively. The unit of lateral deviation is meters (m). θ e , These are the orientation angle deviation and the rate of change of orientation angle deviation, respectively, in radians; I Z C represents the vehicle's moment of inertia, expressed in kg·m². f For the front wheel lateral stiffness, C r For the rear wheel lateral stiffness, l r l is the distance from the center of gravity to the rear wheel. f V is the distance from the center of gravity to the front wheel, m is the total mass of the vehicle, and V is the distance from the center of gravity to the front wheel. x Let δ be the component of the vehicle's speed along the direction of its front. f For the front wheel deflection angle, Let be the derivative of the heading angle. The dynamic error model can be expressed as... The objective function and constraint functions are set as follows:

[0052]

[0053] x(k+1)=A dx(k)+B d u(k)

[0054] x min ≤x(k)≤x max

[0055] u_min≤u(k)≤u_max

[0056] |Δu(k)|≤Δu max

[0057] x(0)=x0

[0058] u(0)=u0

[0059] A d =(I-0.5A*T) s ) -1 (I+0.5A*T s )

[0060] B d =B*T s

[0061] In the formula, J is the set objective function; x(k) is the state matrix; u(k) is the control variable, which is the steering wheel angle; N is the prediction and control time domain, in seconds; x k For the prediction state matrix ;x r The reference state matrix is ​​Q, which is 0 by default; Q is the state weighting matrix; u k R is the steering wheel angle matrix; R is the control weighting matrix; x min and x max These represent the minimum and maximum constraints of the state matrix, respectively; u_min and u_max represent the minimum and maximum constraints of the control variable, respectively; Δu max The maximum constraint for the rate of change of the control variable is calculated adaptively in real time according to different operating conditions; x0 is the state at the initial moment; u0 is the control variable at the initial moment; x(0) is a 4x1 matrix of the lateral distance deviation, the rate of change of the lateral distance deviation, the heading angle deviation, and the rate of change of the heading angle deviation at the current moment; u(0) is the actual steering wheel angle at the current moment, T s To control the cycle.

[0062] 5) When the feedback steering wheel angle of the previous moment and the feedforward steering wheel angle of the current moment (obtained by curvature compensation calculation of the curvature of the target trajectory) are added together, and then amplitude limiting is performed, the target steering wheel angle of the current moment is obtained to perform lateral control of the vehicle.

[0063] Furthermore, when first entering autonomous driving mode, there is often a significant abrupt difference between the actual steering wheel angle and the target steering wheel angle. Directly responding to the target steering wheel angle is not feasible, as it would abruptly change the steering wheel position, leading to poor comfort. Therefore, a smooth transition from the current actual steering wheel angle to the target angle is necessary. During this period, the lack of a steering wheel response to the target angle inevitably results in lateral control errors, so the longitudinal response cannot directly address the desired speed of the target trajectory. However, stationary steering to return to center is also not advisable, as this would cause tire wear. Therefore, this invention controls the longitudinal response at a lower vehicle speed (calibrated value) during the steering wheel return-to-center period. The specific flowchart is as follows. Figure 3 As shown.

[0064] 1) Determine whether the vehicle has entered the autonomous driving mode. Based on the fact that the previous time was in the non-autonomous driving mode and the current time is in the autonomous driving mode, the vehicle enters the autonomous driving mode: When entering the autonomous driving mode, the initial time control quantity u(0) in the MPC constraint is the actual steering wheel angle currently fed back by the vehicle. Otherwise, the initial time control quantity u(0) is equal to the initial target steering wheel angle output by the MPC at the current time.

[0065] 2) Real-time adaptive calculation of steering wheel angle change rate constraints based on different operating conditions, i.e., obtaining the steering wheel angle change rate constraint limit value by looking up tables based on the current vehicle speed and curvature, as detailed below. Figure 2 .

[0066] 3) Determine if speed limit is applied: When the rate of change of steering wheel angle is less than or equal to the normal gradient limit (calibrated value, such as 3 rad / 10 ms, which is smaller than the maximum constraint of the rate of change of steering wheel angle), the speed limit flag is reset, and the vehicle speed responds to the target trajectory. Otherwise (rate of change of steering wheel angle > normal gradient limit), the speed limit flag is activated, and the vehicle speed v is controlled. cmd (k) represents the reference vehicle speed v of the target trajectory. ref (k) and the set vehicle speed v set Take the smaller value of (the calibration value, such as 3 km / h), i.e., v cmd (k)=min(v ref (k),v set It should be noted that this section concerns the vehicle's own speed control, and speed control itself is not related to the above. Figure 1 MPC control in the middle requires separate control.

[0067] In summary, this invention has the following characteristics: 1) It adaptively calculates the steering wheel angle change rate limit according to different working conditions, which can meet the requirements of different curvatures and vehicle speeds for the steering wheel angle control change rate while improving lateral comfort. 2) When entering autonomous driving, it smoothly transitions from the actual steering wheel angle to the target steering wheel angle and controls the vehicle to drive slowly during the smoothing process, which avoids tire wear during the steering wheel return to center and improves steering wheel control comfort.

[0068] Device Example:

[0069] An embodiment of an autonomous vehicle control method based on MPC control according to the present invention includes a memory, a processor, and an internal bus. The processor and the memory communicate and interact with each other via the internal bus. The memory includes at least one software functional module stored in the memory. The processor executes various functional applications and data processing by running the software program and module stored in the memory, thereby implementing the autonomous vehicle control method based on MPC control described in this embodiment of the invention. The processor can be a microprocessor (MCU), a programmable logic device (FPGA), or other processing devices. The memory can be any type of memory that stores information using electrical energy, such as RAM or ROM, or other types of memory.

Claims

1. A method for controlling an autonomous vehicle based on MPC control, characterized in that, Includes the following steps: 1) Obtain the maximum steering wheel angle change limit based on the curvature of the target trajectory. And based on the actual vehicle speed, obtain the maximum steering wheel angle change limit for the current vehicle speed. The minimum of the two limits is taken as the maximum constraint on the rate of change of steering wheel angle. The absolute value of the steering wheel angle change rate is less than or equal to the maximum constraint of the steering wheel angle change rate. The control variable change rate constraint condition that the dynamic error model in MPC control must satisfy; 2) Based on the difference between the target position and the actual position, and combined with the control quantity change rate constraint, MPC control is performed to obtain the feedback steering wheel angle at the current moment calculated by MPC; the feedforward steering wheel angle at the current moment calculated based on the curvature of the target trajectory and the feedback steering wheel angle at the current moment are added together, and the added steering wheel angle is limited to obtain the target steering wheel angle at the current moment, so as to perform lateral control of the vehicle.

2. The autonomous vehicle control method based on MPC control according to claim 1, characterized in that, If the vehicle is transitioning from non-autonomous driving to autonomous driving, then when entering autonomous driving, the initial steering wheel angle during MPC control is set to the actual steering wheel angle currently fed back by the vehicle; otherwise, the initial steering wheel angle during MPC control is set to the target steering wheel angle output at the current moment.

3. The autonomous vehicle control method based on MPC control according to claim 1, characterized in that, The current feedforward steering wheel angle is obtained by calculating the curvature compensation of the target trajectory.

4. The autonomous vehicle control method based on MPC control according to claim 1, characterized in that, The dynamic error model is as follows: In the formula, e cg , These are the lateral deviation and the rate of change of the lateral deviation, respectively; θ e , These are the orientation angle deviation and the rate of change of orientation angle deviation, respectively; I z C is the vehicle's moment of inertia. f For the front wheel lateral stiffness, C r For the rear wheel lateral stiffness, l r l is the distance from the center of gravity to the rear wheel. f V is the distance from the center of gravity to the front wheel, m is the total mass of the vehicle, and V is the distance from the center of gravity to the front wheel. x Let δ be the component of the vehicle's speed along the direction of its front. f For the front wheel deflection angle, is the derivative of the heading angle.

5. The autonomous vehicle control method based on MPC control according to claim 1, characterized in that, The optimization objective function used in MPC control is: In the formula, J is the set objective function; x(k) is the state matrix; u(k) is the control variable, which is the steering wheel angle; N is the prediction and control time domain; x k For the predicted state matrix; x r U is the reference state matrix; Q is the state weighting matrix; u k R is the steering wheel angle matrix; R is the control weighting matrix.

6. The autonomous vehicle control method based on MPC control according to claim 5, characterized in that, The constraint function used in MPC control is: In the formula, x min and x max These are the minimum and maximum constraints of the state matrix, respectively; u_min and u_max are the minimum and maximum constraints of the control variable, respectively. To control the rate of change of quantity; The maximum constraint is the rate of change of the control variable; x0 is the initial state, x(0) is the matrix of lateral distance deviation, rate of change of lateral distance deviation, heading angle deviation, and rate of change of heading angle deviation at the current time; u(0) is the control variable at the initial time, u0 is the actual steering wheel angle at the current time, and T s To control the cycle.

7. The autonomous vehicle control method based on MPC control according to claim 1, characterized in that, When the rate of change of steering wheel angle exceeds the normal gradient limit, control the vehicle speed to be the reference speed v of the target trajectory. ref (k) and the set vehicle speed v set The smaller of the values ​​is used; otherwise, the vehicle speed is controlled to respond to the target trajectory.

8. An autonomous vehicle control device based on MPC control, characterized in that, It includes a memory and a processor, the processor being used to execute computer program instructions stored in the memory to implement an MPC-based autonomous vehicle control method, the control method being: 1) Obtain the maximum steering wheel angle change limit based on the curvature of the target trajectory. And based on the actual vehicle speed, obtain the maximum steering wheel angle change limit for the current vehicle speed. The minimum of the two limits is taken as the maximum constraint on the rate of change of steering wheel angle. The absolute value of the steering wheel angle change rate is less than or equal to the maximum constraint of the steering wheel angle change rate. The control variable change rate constraint condition that the dynamic error model in MPC control must satisfy; 2) Based on the difference between the target position and the actual position, and combined with the control quantity change rate constraint, MPC control is performed to obtain the feedback steering wheel angle at the current moment calculated by MPC; the feedforward steering wheel angle at the current moment calculated based on the curvature of the target trajectory and the feedback steering wheel angle at the current moment are added together, and the added steering wheel angle is limited to obtain the target steering wheel angle at the current moment, so as to perform lateral control of the vehicle.

9. The autonomous vehicle control device based on MPC control according to claim 8, characterized in that, If the vehicle is transitioning from non-autonomous driving to autonomous driving, then when entering autonomous driving, the initial steering wheel angle during MPC control is set to the actual steering wheel angle currently fed back by the vehicle; otherwise, the initial steering wheel angle during MPC control is set to the target steering wheel angle output at the current moment.

10. The autonomous vehicle control device based on MPC control according to claim 8, characterized in that, The current feedforward steering wheel angle is obtained by calculating the curvature compensation of the target trajectory.

11. The autonomous vehicle control device based on MPC control according to claim 8, characterized in that, The dynamic error model is as follows: In the formula, e cg , These are the lateral deviation and the rate of change of the lateral deviation, respectively; θ e , These are the orientation angle deviation and the rate of change of orientation angle deviation, respectively; I z C is the vehicle's moment of inertia. f For the front wheel lateral stiffness, C r For the rear wheel lateral stiffness, l r l is the distance from the center of gravity to the rear wheel. f V is the distance from the center of gravity to the front wheel, m is the total mass of the vehicle, and V is the distance from the center of gravity to the front wheel. x Let δ be the component of the vehicle's speed along the direction of its front. f For the front wheel deflection angle, is the derivative of the heading angle.

12. The autonomous vehicle control device based on MPC control according to claim 8, characterized in that, The optimization objective function used in MPC control is: In the formula, J is the set objective function; x(k) is the state matrix; u(k) is the control variable, which is the steering wheel angle; N is the prediction and control time domain; x k For the predicted state matrix; x r U is the reference state matrix; Q is the state weighting matrix; u k R is the steering wheel angle matrix; R is the control weighting matrix.

13. The autonomous vehicle control device based on MPC control according to claim 12, characterized in that, The constraint function used in MPC control is: In the formula, x min and x max These are the minimum and maximum constraints of the state matrix, respectively; u_min and u_max are the minimum and maximum constraints of the control variable, respectively. To control the rate of change of quantity; The maximum constraint is the rate of change of the control variable; x0 is the initial state, x(0) is the matrix of lateral distance deviation, rate of change of lateral distance deviation, heading angle deviation, and rate of change of heading angle deviation at the current time; u(0) is the control variable at the initial time, u0 is the actual steering wheel angle at the current time, and T s To control the cycle.

14. The autonomous vehicle control device based on MPC control according to claim 8, characterized in that, When the rate of change of steering wheel angle exceeds the normal gradient limit, control the vehicle speed to be the reference speed v of the target trajectory. ref (k) and the set vehicle speed v set The smaller of the values ​​is used; otherwise, the vehicle speed is controlled to respond to the target trajectory.

Images