A vehicle longitudinal-lateral control method, system, storage medium and vehicle

Abstract

This invention discloses a vehicle longitudinal and lateral control method, comprising the steps of: receiving the decision-planning trajectory of the controlled vehicle and the actual vehicle state; selecting multiple reference trajectory points on the trajectory fitting line based on the decision-planning trajectory, vehicle state, and current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period, wherein the vehicle dynamic error is the deviation between the actual reference trajectory and the predicted trajectory information at the current moment, and includes at least the vehicle lateral error; performing rolling optimization on a nonlinear vehicle prediction model based on the actual reference trajectory and a preset cost function to obtain the optimal control quantity sequence within the prediction time domain; and controlling the controlled vehicle according to the optimal control quantity sequence. This invention also discloses a corresponding system, storage medium, and vehicle. Implementing this invention can improve the smoothness and stability of vehicle longitudinal and lateral control.

Description

Technical Field

[0001] This invention relates to lateral and longitudinal control methods in the field of autonomous driving, specifically to a vehicle longitudinal and lateral control method, system, storage medium, and vehicle based on Nonlinear Model Predictive Control (NMPC) applied to autonomous parking and driving processes. Background Technology

[0002] Model predictive control (MPC) is a commonly used control algorithm in the field of autonomous driving, often used for lateral and longitudinal vehicle control in scenarios such as parking and driving. Most MPC solutions involve constructing an error model using the current vehicle state and a reference trajectory, then linearizing and discretizing the error model to form a quadratic cost function. After setting control constraints, the cost function is solved to obtain a series of control increments, and the first control increment of the sequence is applied to the controlled object. The above steps are repeated in the next cycle, thus forming sustainable lateral and longitudinal control of the vehicle.

[0003] Because MPC performs linear predictions of subsequent states based on the first error state in the prediction time domain, the linearization of the error model will cause a certain degree of accuracy loss. In addition, in continuous large curvature scenarios such as parking, the kinematic model may cause problems such as steering wheel vibration and understeering when dealing with discontinuous large curvature changes. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a vehicle longitudinal and lateral control method, system, storage medium and vehicle, which can improve the smoothness and stability of vehicle longitudinal and lateral control and avoid problems such as steering wheel vibration and understeering.

[0005] To address the aforementioned technical problems, as one aspect of the present invention, a method for controlling the longitudinal and lateral directions of a vehicle is provided, comprising at least the following steps:

[0006] Receive the decision-making and planning trajectory of the controlled vehicle, as well as the actual vehicle status of the controlled vehicle;

[0007] Based on the decision-planning trajectory, vehicle status, and current vehicle dynamic error, multiple reference trajectory points are selected on the trajectory fitting line to obtain the actual reference trajectory within the current prediction period. The vehicle dynamic error is the deviation between the actual reference trajectory and the predicted trajectory information at the current moment, and it includes at least the vehicle lateral error.

[0008] Based on the actual reference trajectory and the preset cost function, the nonlinear vehicle prediction model is rolled to optimize and obtain the optimal control quantity sequence in the prediction time domain.

[0009] The controlled vehicle is controlled according to the optimal control sequence.

[0010] The preset cost function includes at least adjustable weights for lateral offset error and heading error.

[0011] The method further includes:

[0012] The dynamic error of the previous prediction period is obtained. When the vehicle lateral error in the dynamic error is greater than the third threshold, the weight of the lateral error in the cost function is increased in the current prediction period, while the weight of the heading error is decreased.

[0013] When the vehicle lateral error in the dynamic error is less than the fourth threshold, the weight of the heading error in the cost function is increased and the weight of the lateral error is decreased in the current prediction period.

[0014] The fourth threshold is less than the third threshold.

[0015] The step of selecting multiple reference trajectory points on the trajectory fitting line based on the decision planning trajectory, vehicle status, and current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period further includes:

[0016] The decision-making and planning trajectory is extended at its endpoint;

[0017] Based on the vehicle dynamic error, determine the distance between adjacent trajectory points in the reference trajectory points;

[0018] Based on the vehicle's current speed and cycle time, the distance traveled by the vehicle at each moment in the prediction time domain is calculated, and multiple reference trajectory points are selected to obtain the actual reference trajectory within the current prediction period.

[0019] Specifically, determining the distance between adjacent trajectory points in the reference trajectory based on vehicle dynamic error involves:

[0020] The dynamic error of the previous prediction period is obtained. When the vehicle lateral error in the dynamic error is greater than the first threshold, the value of the dynamic adjustment factor is reduced so as to reduce the distance between adjacent reference points in the selected reference points in the current prediction period.

[0021] When the vehicle lateral error in the dynamic error is less than the second threshold, the value of the dynamic adjustment factor is increased to expand the distance between adjacent reference points in the selected reference points within the current prediction period.

[0022] Wherein, the second threshold is less than the first threshold.

[0023] The step of performing rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and a preset cost function to obtain the optimal control quantity sequence in the prediction time domain further includes: constructing a vehicle prediction model based on nonlinear model predictive control and obtaining the predicted trajectory of the controlled vehicle; including:

[0024] The following formula is used to calculate the predicted trajectory point information for each moment within the prediction period;

[0025]

[0026]

[0027]

[0028] v k+1 =v k +a k *d t

[0029]

[0030]

[0031] Where, x k y represents the x-coordinate value of the vehicle at time k within this prediction period; k The vehicle's ordinate value at time k; Let v be the vehicle's heading angle at time k; k Let σ be the vehicle speed at time k; k Let a be the angle of the vehicle's front wheels at time k; k Let ref-x be the vehicle's angular velocity at time k; k ref-y is the x-coordinate of the reference trajectory at time k. k Let k be the ordinate of the vehicle's reference trajectory. Let x be the vehicle's reference heading at time k; k+1 y is the x-coordinate of the vehicle at time k+1; k+1 The vehicle's ordinate value at time k+1; v is the vehicle's heading angle at time k+1; k+1 Let be the vehicle speed at time k+1; cte k+1 epsi represents the lateral error of the vehicle at time k+1. k+1 Let $\frac{k+1}{k+1}$ be the vehicle heading error.

[0032] The preset cost function is as follows:

[0033]

[0034] in, This indicates the deviation between the predicted value and the front wheel angle corresponding to the curvature value at the reference trajectory point;

[0035] This indicates that the calculation results of the control quantity from the previous moment have been incorporated;

[0036] The weight representing the rate of change of the lateral error increment under constraint;

[0037] These represent the lateral offset error, heading error, and speed error, respectively, with λ used to dynamically adjust the weights of the lateral and heading errors.

[0038] Indicates control parameters such as front wheel angle and acceleration;

[0039] This represents the increment of the front wheel steering angle and acceleration;

[0040] The constraints are as follows:

[0041] σ∈[σ min , σ max ]

[0042] a∈[a min a max ].

[0043] The step of performing rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and a preset cost function to obtain the optimal control quantity sequence in the prediction time domain further includes:

[0044] At time k, the cost function and constraints are substituted into the nonlinear solver for calculation, and the optimal control quantity sequence in the prediction time domain is obtained, which includes at least the vehicle front wheel steering angle at each time in the control time domain.

[0045] Apply the first control quantity in the optimal control quantity sequence to the controlled vehicle;

[0046] Repeat the above steps at time k+1 to achieve sustainable control of the vehicle's lateral and longitudinal directions.

[0047] In another aspect, the present invention provides a vehicle longitudinal and lateral control system, which includes at least:

[0048] The real-time status acquisition unit is used to receive the decision-making and planning trajectory of the controlled vehicle, as well as the actual vehicle status of the controlled vehicle.

[0049] The reference trajectory acquisition unit is used to select multiple reference trajectory points on the trajectory fitting line based on the decision planning trajectory, vehicle status and current vehicle dynamic error, and obtain the actual reference trajectory within the current prediction period. The vehicle dynamic error is the deviation between the actual reference trajectory and the predicted trajectory information of the vehicle at the current moment, which includes at least the vehicle lateral error.

[0050] The rolling optimization solution unit is used to perform rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and the preset cost function to obtain the optimal control quantity sequence in the prediction time domain.

[0051] A control processing unit is used to control the controlled vehicle according to the optimal control quantity sequence.

[0052] This further includes:

[0053] The adjacent distance coefficient adjustment unit is used to determine the distance between adjacent trajectory points in the reference trajectory points based on the vehicle dynamic error. Specifically, when the vehicle lateral error in the dynamic error of the previous prediction period is greater than a first threshold, the distance between adjacent reference points in the selected reference points is reduced in the current prediction period; when the vehicle lateral error in the dynamic error is less than a second threshold, the distance between adjacent reference points in the selected reference points is increased in the current prediction period.

[0054] The weighting coefficient adjustment unit is used to obtain the dynamic error of the previous prediction period. When the vehicle lateral error in the dynamic error is greater than the third threshold, the weight of the lateral error in the cost function is increased in the current prediction period, while the weight of the heading error is decreased at the same time. When the vehicle lateral error in the dynamic error is less than the fourth threshold, the weight of the heading error in the cost function is increased in the current prediction period, while the weight of the lateral error is decreased at the same time.

[0055] Wherein, the second threshold is less than the first threshold; and the fourth threshold is less than the third threshold.

[0056] This further includes:

[0057] The predicted trajectory acquisition unit is used to construct a vehicle prediction model based on nonlinear model predictive control and obtain the predicted trajectory of the controlled vehicle.

[0058] Specifically, the predicted trajectory acquisition unit is used to calculate the predicted trajectory point information at each moment within the prediction period using the following formula:

[0059]

[0060]

[0061]

[0062] v k+1 =v k +a k *d t

[0063]

[0064]

[0065] Where, x k y represents the x-coordinate value of the vehicle at time k within this prediction period; k The vehicle's ordinate value at time k; Let v be the vehicle's heading angle at time k; k Let σ be the vehicle speed at time k; k Let a be the angle of the vehicle's front wheels at time k; k Let ref-x be the vehicle's angular velocity at time k; k ref-y is the x-coordinate of the reference trajectory at time k. k Let k be the ordinate of the vehicle's reference trajectory. Let x be the vehicle's reference heading at time k; k+1 y is the x-coordinate of the vehicle at time k+1; k+1 The vehicle's ordinate value at time k+1; v is the vehicle's heading angle at time k+1; k+1 Let be the vehicle speed at time k+1; cte k+1 epsi represents the lateral error of the vehicle at time k+1. k+1 Let $\frac{k+1}{k+1}$ be the vehicle heading error.

[0066] It further includes a cost function and constraint setting unit, used to set the cost function and constraint conditions, wherein the cost function includes at least adjustable weights for lateral offset error and heading error;

[0067] Specifically, in the cost function and constraint setting unit, the cost function set for the lateral control process is as follows:

[0068]

[0069] in, This indicates the deviation between the predicted value and the front wheel angle corresponding to the curvature value at the reference trajectory point;

[0070] This indicates that the calculation results of the control quantity from the previous moment have been incorporated;

[0071] The weight representing the rate of change of the lateral error increment under constraint;

[0072] These represent the lateral offset error, heading error, and speed error, respectively, with λ used to dynamically adjust the weights of the lateral and heading errors.

[0073] Indicates control parameters such as front wheel angle and acceleration;

[0074] This represents the increment of the front wheel steering angle and acceleration;

[0075] The constraints are as follows:

[0076] σ∈[σ min σ max ]

[0077] a∈[a min a max ].

[0078] In another aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method described above.

[0079] In another aspect, the present invention provides a computing device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to perform the steps of the method described above.

[0080] In another aspect, the present invention provides a vehicle having at least a lane change decision device thereon, wherein the lane change decision device is equipped with the system described above.

[0081] Implementing the embodiments of the present invention has the following beneficial effects:

[0082] In the method provided by this invention, after receiving the planned trajectory, the controlled vehicle extends the trajectory at the end of the trajectory and selects a suitable reference trajectory point on the trajectory fitting line according to the vehicle error as the actual reference trajectory to construct a vehicle prediction model based on nonlinear model predictive control (NPMC). At the same time, after setting the control quantity constraints, a series of control quantities are obtained by solving the cost function through rolling optimization, and the first control quantity is applied to the controlled vehicle. The above steps are repeated in the next cycle, thereby forming a sustainable control of the vehicle in both the lateral and longitudinal directions.

[0083] In the embodiments of the present invention, a nonlinear model is used for model predictive control (NPMC), which can avoid the accuracy loss caused by linearizing the model. At the same time, the curvature of the reference trajectory point and its dynamic understeering correction factor, the deviation between the control quantity at the previous moment and the current prediction quantity are introduced into the cost function and reasonable weights are set to solve problems such as steering wheel vibration and understeering in continuous large curvature turning scenarios.

[0084] More specifically, in this invention, by setting a dynamic adjustment factor for understeer, the spacing between reference points is adjusted, and reference trajectory points can be dynamically selected according to the vehicle state and error, so that the selected reference trajectory points are more consistent with the state predicted by the NMPC model, thereby optimizing the solution of the most suitable control quantity to reduce the error.

[0085] By setting a dynamic error control factor, the weights of different errors can be dynamically adjusted. When lateral and heading errors coexist, priority is given to reducing the lateral error; once the lateral error is small, the heading error is then corrected, thus quickly completing the reference trajectory point following control.

[0086] Meanwhile, the curvature at the reference point is added as a feedforward to the cost function, making full use of the information of the reference trajectory point to improve the controller's response speed. This allows the control quantity calculated by NMPC in continuous high curvature turning scenarios to enable the steering wheel to respond quickly, thereby maintaining a small vehicle error.

[0087] Moreover, the deviation between the control quantity at the previous moment and the current predicted value is introduced into the cost function, so that the NMPC calculation value remains continuous, avoiding the situation of steering wheel shaking caused by large changes in the control quantity between moments in the prior art. Attached Figure Description

[0088] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, obtaining other drawings based on these drawings without creative effort still falls within the scope of the present invention.

[0089] Figure 1 This is a schematic diagram of the main flow of an embodiment of a vehicle longitudinal and lateral control method based on nonlinear model predictive control provided by the present invention;

[0090] Figure 2 This is a reference schematic diagram of a predicted trajectory and a reference trajectory within a prediction period, as per the present invention.

[0091] Figure 3 This invention relates to a schematic diagram of the application environment principle;

[0092] Figure 4 This is a schematic diagram of the structure of an embodiment of a vehicle longitudinal and lateral control system based on nonlinear model predictive control provided by the present invention;

[0093] Figure 5 for Figure 4 A schematic diagram of the structure of the reference trajectory acquisition unit. Detailed Implementation

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

[0095] like Figure 1 The diagram shown illustrates the main flow of an embodiment of a vehicle longitudinal and lateral control method based on nonlinear model predictive control provided by the present invention; in conjunction with... Figure 2 , Figure 3 As shown, in this embodiment, the method includes at least the following steps:

[0096] Step S10: Receive the decision-making and planning trajectory of the controlled vehicle, as well as the actual vehicle status of the controlled vehicle.

[0097] In specific examples, it is necessary to acquire map data of the vehicle's driving path or trajectory data output by the planning algorithm, i.e., the planned trajectory of the controlled vehicle. Typically, information such as the trajectory's coordinates, heading angle, and curvature is required. Simultaneously, real-time status information of the controlled vehicle needs to be acquired, including current position and attitude, speed, acceleration, and heading angle. This information can be acquired and processed through onboard sensors (such as GPS, LiDAR, cameras, and inertial measurement units) and the vehicle control unit. In some cases, external environmental information, such as road signs, lane markings, and obstacles, also needs to be acquired, and planning and control decisions made based on this information.

[0098] Step S11: Based on the decision planning trajectory, vehicle status, and current vehicle dynamic error, select multiple reference trajectory points on the trajectory fitting line to obtain the actual reference trajectory within the current prediction period. The vehicle dynamic error is the deviation between the actual reference trajectory and the predicted trajectory information at the current moment, which includes at least the vehicle lateral error.

[0099] In a specific example, step S11 further includes:

[0100] Step S110: Extend the end of the decision planning trajectory;

[0101] It is understood that, in the example of this invention, the subsequent nonlinear model predictive control (NMPC) process requires the prediction of a number of reference trajectory points in the time domain and their comparison with the NMPC model state. Therefore, it is necessary to perform a prediction extension at the end of each trajectory segment so that the trajectory points can still be correctly selected when the vehicle approaches the end point, ensuring the accuracy of the trajectory points on which the NMPC calculation depends.

[0102] In one example, the information of the current reference trajectory point could include x0, y0, Using the vehicle kinematics model, information about the next reference trajectory point (x1, y1) can be derived from the current reference trajectory point (r, s0). By repeating r and s1, the length of the trajectory at the endpoint can be predicted.

[0103] In the vehicle coordinate system:

[0104] d s =r·a

[0105]

[0106]

[0107] d φ =a

[0108] In the global coordinate system:

[0109] x1=x0+d x *cosφ0-d y *sinφ0

[0110] y1=y0+d x *sinφ0-d y *cosφ0

[0111] φ1=φ0+d φ

[0112] Where: a is the angle rotated from time k to time k+1 within this prediction period; d s Let be the arc length traversed from time k to time k+1; r be the turning radius of the vehicle; d x d represents the longitudinal distance traversed from time k to time k+1 in the vehicle coordinate system; y Let k be the lateral distance traversed from time k to time k+1 in the vehicle coordinate system.

[0113] x0 is the x-coordinate value of the reference trajectory at time k in the global coordinate system; y0 is the y-coordinate value of the reference trajectory at time k in the global coordinate system. s0 is the heading angle of the reference trajectory at time k in the global coordinate system; s0 is the length from the first reference trajectory point to the current reference trajectory point at time k; x1 is the x-coordinate value of the reference trajectory at time k+1 in the global coordinate system; y1 is the y-coordinate value of the reference trajectory at time k+1 in the global coordinate system. s1 is the heading angle of the reference trajectory at time k+1 in the global coordinate system; s1 is the length from the first reference trajectory point to the current reference trajectory point k+1.

[0114] Step S111: Based on the vehicle dynamic error, determine the distance between adjacent trajectory points in the reference trajectory points, or determine the number of trajectory points selected within the prediction period; for example, based on the current lateral deviation value, determine whether to shorten the distance between adjacent trajectory points, or adjust the number of reference points (e.g., only pre-aiming at about 3 to 5 points), etc.

[0115] Step S112: Based on the vehicle's current speed and cycle time, calculate the distance the vehicle travels at each moment in the prediction time domain, select multiple reference trajectory points, and obtain the actual reference trajectory within the current prediction period.

[0116] More specifically, in one example, step S113 can be implemented in the following manner:

[0117] The extended reference trajectory is transformed from the global coordinate system to the vehicle body coordinate system. Let 's' be the independent variable, and x, y... and r are the dependent variables, respectively. Four formulas are obtained by fitting a fifth-order polynomial:

[0118] x = f1(s)

[0119] y = f2(s)

[0120]

[0121] r = f4(s)

[0122] Based on the vehicle's current speed and cycle time, calculate the distance traveled by the vehicle at each moment in the prediction time domain:

[0123] d(i)=v*i*d t

[0124] Substitute d(i) as the independent variable into each fitted function to calculate the corresponding... value.

[0125] but The processed reference trajectory points are passed to NMPC for constraint construction.

[0126] Where: v is the current speed of the vehicle; i is the sequence in the prediction time domain, i = 1, ..., N, where N is the prediction time domain; d t The loop time is in seconds (s).

[0127] Step S12: Construct a vehicle prediction model based on nonlinear model predictive control (NMPC) and obtain the predicted trajectory of the controlled vehicle.

[0128] In a specific example, step S12 is implemented using the following vehicle prediction model:

[0129] Specifically, in the vehicle prediction model, the predicted trajectory point information for each moment within the prediction period is calculated sequentially using the following formula and the NMPC method.

[0130]

[0131]

[0132]

[0133] v k+1 =v k +a k *d t

[0134]

[0135]

[0136] Where, x k y represents the x-coordinate value of the vehicle at time k within this prediction period; k The vehicle's ordinate value at time k; Let v be the vehicle's heading angle at time k; k Let σ be the vehicle speed at time k; k Let a be the angle of the vehicle's front wheels at time k; k Let ref-x be the vehicle's angular velocity at time k; k ref-y is the x-coordinate of the reference trajectory at time k. k Let k be the ordinate of the vehicle's reference trajectory. Let x be the vehicle's reference heading at time k; k+1 y is the x-coordinate of the vehicle at time k+1; k+1 The vehicle's ordinate value at time k+1; v is the vehicle's heading angle at time k+1; k+1 Let be the vehicle speed at time k+1; cte k+1 epsi represents the lateral error of the vehicle at time k+1. k+1 Let $\frac{k+1}{k+1}$ be the vehicle heading error.

[0137] Step S13: Perform rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and the preset cost function to obtain the optimal control quantity sequence in the prediction time domain.

[0138] First, in step S130, it is necessary to set the cost function and constraints. The cost function includes at least the lateral offset error and heading error with adjustable weights.

[0139] In one example, the steps of setting the cost function and constraints are as follows:

[0140] Set the following cost function in the lateral control process:

[0141]

[0142] in, This indicates the deviation between the predicted value and the front wheel angle corresponding to the curvature value at the reference trajectory point, with the aim of accelerating the controller's response speed;

[0143] This indicates that the control quantity calculation result from the previous moment is incorporated. In order to ensure the smoothness of the control quantity calculation, this can maintain the continuity of the NMPC solution value, thereby avoiding the steering wheel vibration problem caused by the large changes in the control quantity value calculated by the existing MPC between different moments.

[0144] The weight representing the rate of change of the lateral error increment under constraint;

[0145] These represent the lateral offset error, heading error, and speed error, respectively, which are the deviations between the actual values ​​and the reference values; λ is used to dynamically adjust the weights of the lateral and heading errors.

[0146] This represents control variables such as front wheel angle and acceleration; the purpose is to prevent the calculated control variables from being too large.

[0147] This represents the increment of the front wheel steering angle and acceleration, with the aim of ensuring that the control variables do not change abruptly;

[0148] The constraints are as follows:

[0149] σ∈[σ min O max ]

[0150] a∈[a min a max ].

[0151] Step S131: Obtain the optimal control quantity sequence in the prediction time domain by solving the cost function through rolling optimization, so as to apply the first control quantity in the optimal control quantity sequence to the controlled vehicle.

[0152] In a specific example, step S131 further includes:

[0153] At time k, the cost function and constraints are substituted into the nonlinear solver for calculation, and the optimal control quantity sequence in the prediction time domain is obtained. For example, it can be the front wheel steering angle of the vehicle at each time in the control time domain.

[0154] Apply the first control quantity in the optimal control quantity sequence to the controlled vehicle;

[0155] Repeat the above steps at time k+1 to achieve sustainable control of the vehicle's lateral and longitudinal directions.

[0156] Understandably, in the NMPC control process, within a prediction cycle, the vehicle prediction model based on NMPC calculates the optimal control input sequence for a future period based on the current state and reference trajectory. This sequence is dynamically changing, with each time step corresponding to a control command. The first control variable of each prediction cycle should be used as the initial input value for the current control cycle to achieve initial control of the controlled vehicle or system. In subsequent control cycles, the optimal control variable sequence is progressively updated based on the prediction of the optimal control input, and the updated control commands are assigned to the controlled vehicle or system to execute control tasks, thereby achieving dynamic and smooth control of the vehicle.

[0157] In a specific example, in step S11, the distance between adjacent trajectory points in the reference trajectory points is determined based on the vehicle dynamic error, specifically as follows:

[0158] The dynamic error of the previous prediction period is obtained. When the vehicle lateral error cte in the dynamic error is greater than the first threshold (indicating understeer), the value of the dynamic adjustment factor τ is reduced to reduce the distance between adjacent reference points in the selected reference points within the current prediction period. This can enable MPC to complete the correction of lateral error in the shortest possible distance.

[0159] When the vehicle lateral error cte in the dynamic error is less than the second threshold, the value of the dynamic adjustment factor τ is increased to expand the distance between adjacent reference points in the selected reference points within the current prediction period; this allows MPC to take the correction of heading error more seriously.

[0160] Wherein, the second threshold is less than the first threshold.

[0161] In a specific example, the method further includes:

[0162] The dynamic error of the previous prediction period is obtained. When the vehicle lateral error cte in the dynamic error is greater than the third threshold, the value of λ in the cost function is increased to increase the weight of the lateral error in the current prediction period and at the same time reduce the weight of the heading error, thereby prompting MPC to reduce the lateral error as soon as possible and weaken the impact of the heading error.

[0163] When the vehicle lateral error cte in the dynamic error is less than the fourth threshold, the value of λ in the cost function is reduced to increase the weight of the heading error in the current prediction period, while reducing the weight of the lateral error, so as to maintain a good heading error.

[0164] The fourth threshold is less than the third threshold.

[0165] Understandably, in the method provided by this invention, after receiving the planned trajectory, the controlled vehicle extends the trajectory at its end point and selects suitable reference trajectory points on the trajectory fitting line based on vehicle error, using these as actual reference trajectories to construct a vehicle prediction model based on nonlinear model predictive control. Simultaneously, after setting control quantity constraints, a series of control quantities are obtained by solving the cost function through rolling optimization, and the first control quantity is applied to the controlled vehicle. The above steps are repeated in the next cycle, thereby forming sustainable control of the vehicle's lateral and longitudinal directions.

[0166] In the embodiments of the present invention, in order to avoid the loss of accuracy caused by linearizing the model, a nonlinear model is used for model predictive control; at the same time, in order to solve the problems of steering wheel vibration and understeering in continuous high curvature turning scenarios, the curvature of the reference trajectory point and its dynamic understeering correction factor, the deviation between the control quantity at the previous moment and the current prediction quantity are introduced into the cost function and reasonable weights are set, so that the overall control effect of the present invention is greatly improved compared with the traditional MPC.

[0167] More specifically, by setting a dynamic adjustment factor τ for understeer, the spacing between reference points can be adjusted. Reference trajectory points can be dynamically selected based on vehicle status and error, making the selected reference trajectory points more consistent with the state predicted by the NMPC model, thereby optimizing the solution of the most suitable control quantity to reduce error.

[0168] By setting a dynamic error control factor λ, the weights of different errors can be dynamically adjusted. When lateral error and heading error coexist, priority is given to reducing lateral error; and when the lateral error is small, the heading error is then corrected, thereby quickly completing the reference trajectory point following control.

[0169] Meanwhile, the curvature at the reference point is added as a feedforward to the cost function, making full use of the information of the reference trajectory point to improve the controller's response speed. This allows the control quantity calculated by NMPC in continuous high curvature turning scenarios to enable the steering wheel to respond quickly, thereby maintaining a small vehicle error.

[0170] Furthermore, the deviation between the control quantity at the previous moment and the current predicted value is introduced into the cost function to keep the NMPC calculation value continuous and avoid steering wheel vibration caused by large changes in the control quantity between moments in the prior art.

[0171] like Figure 4 The diagram shown illustrates a structural schematic of an embodiment of a vehicle longitudinal and lateral control system based on nonlinear model predictive control provided by the present invention. (In conjunction with...) Figure 5As shown, in this embodiment, the system 1 includes at least:

[0172] The real-time status acquisition unit 10 is used to receive the decision-making and planning trajectory of the controlled vehicle, as well as the actual vehicle status of the controlled vehicle.

[0173] The reference trajectory acquisition unit 11 is used to select multiple reference trajectory points on the trajectory fitting line based on the decision planning trajectory, vehicle status and current vehicle dynamic error, and obtain the actual reference trajectory within the current prediction period.

[0174] The reference trajectory acquisition unit 11 further includes:

[0175] The trajectory extension processing unit 110 is used to extend the end of the decision planning trajectory;

[0176] The trajectory point parameter determination unit 111 is used to determine the distance or number between adjacent trajectory points in the reference trajectory points based on the vehicle dynamic error.

[0177] The reference trajectory point selection unit 112 is used to calculate the distance traveled by the vehicle at each moment in the prediction time domain based on the vehicle's current speed and cycle time, select multiple reference trajectory points, and obtain the actual reference trajectory within the current prediction period.

[0178] The predicted trajectory acquisition unit 12 is used to construct a vehicle prediction model based on nonlinear model predictive control and obtain the predicted trajectory of the controlled vehicle.

[0179] The predicted trajectory acquisition unit 12 is further configured to calculate the predicted trajectory point information at each moment within the prediction period using the following formula:

[0180]

[0181]

[0182]

[0183] v k+1 =v k +a k *d t

[0184]

[0185]

[0186] Where, x k y represents the x-coordinate value of the vehicle at time k within this prediction period; k The vehicle's ordinate value at time k; Let v be the vehicle's heading angle at time k; k Let σ be the vehicle speed at time k; k Let a be the angle of the vehicle's front wheels at time k; k Let ref-x be the vehicle's angular velocity at time k; k ref-y is the x-coordinate of the reference trajectory at time k. k Let k be the ordinate of the vehicle's reference trajectory. Let x be the vehicle's reference heading at time k; k+1 y is the x-coordinate of the vehicle at time k+1; k+1 The vehicle's ordinate value at time k+1; v is the vehicle's heading angle at time k+1; k+1 Let be the vehicle speed at time k+1; cte k+1 epsi represents the lateral error of the vehicle at time k+1. k+1 Let $\frac{k+1}{k+1}$ be the vehicle heading error.

[0187] The cost function and constraint setting unit 13 is used to set the cost function and constraint conditions. The cost function includes at least the lateral offset error and heading error with adjustable weights.

[0188] Specifically, in the cost function and constraint setting unit 13, the cost function set for the r-axis lateral control process is as follows:

[0189]

[0190] in, This indicates the deviation between the predicted value and the front wheel angle corresponding to the curvature value at the reference trajectory point;

[0191] This indicates that the calculation results of the control quantity from the previous moment have been incorporated;

[0192] The weight representing the rate of change of the lateral error increment under constraint;

[0193] These represent the lateral offset error, heading error, and speed error, respectively, with λ used to dynamically adjust the weights of the lateral and heading errors.

[0194] Indicates control parameters such as front wheel angle and acceleration;

[0195] This represents the increment of the front wheel steering angle and acceleration;

[0196] The constraints are as follows:

[0197] σ∈[σ min , σ max ]

[0198] a∈[a min a max ].

[0199] The rolling optimization solution unit 14 performs rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and the preset cost function to obtain the optimal control quantity sequence in the prediction time domain.

[0200] Control processing unit 18 is used to control the controlled vehicle according to the optimal control quantity sequence;

[0201] The dynamic error evaluation unit 15 is used to obtain the dynamic error based on the deviation between the actual reference trajectory and the predicted trajectory information at the current moment. The dynamic error includes at least the vehicle lateral error cte value.

[0202] It is understood that the system 1 further includes:

[0203] The adjacent distance coefficient adjustment unit 16 is used to determine the distance between adjacent trajectory points in the reference trajectory points based on the vehicle dynamic error.

[0204] Specifically, when the vehicle lateral error cte in the dynamic error of the previous prediction period is greater than the first threshold, the value of the dynamic adjustment factor τ is reduced in order to reduce the distance between adjacent reference points in the selected reference points within the current prediction period.

[0205] When the vehicle lateral error cte in the dynamic error is less than the second threshold, the value of the dynamic adjustment factor τ is increased to expand the distance between adjacent reference points in the selected reference points within the current prediction period.

[0206] Wherein, the second threshold is less than the first threshold.

[0207] This further includes:

[0208] The weighting coefficient adjustment unit 17 is used to obtain the dynamic error of the previous prediction period. When the vehicle lateral error cte in the dynamic error is greater than the third threshold, the λ value in the cost function is increased to increase the weight of the lateral error in the current prediction period and at the same time reduce the weight of the heading error.

[0209] When the vehicle lateral error cte in the dynamic error is less than the fourth threshold, the value of λ in the cost function is reduced to increase the weight of the heading error in the current prediction period and at the same time reduce the weight of the lateral error.

[0210] The fourth threshold is less than the third threshold.

[0211] For more details, please refer to and combine with the above. Figures 1 to 3 The description of that will not be repeated here.

[0212] In another aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned... Figures 1 to 3 The steps of the described method. For more details, please refer to and combine with the foregoing descriptions. Figures 1 to 3 The description of that will not be repeated here.

[0213] In another aspect, the present invention provides a computing device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the aforementioned... Figures 1 to 3 The steps of the described method. For more details, please refer to and combine with the foregoing descriptions. Figures 1 to 3 The description of that will not be repeated here.

[0214] In another aspect, the present invention provides a vehicle having at least a lane change decision device thereon, wherein the lane change decision device is configured with the aforementioned... Figure 4 and Figure 5 The system described. For more details, please refer to and combine with the aforementioned... Figure 4 and Figure 5 The description of that will not be repeated here.

[0215] Implementing the embodiments of the present invention has the following beneficial effects:

[0216] In the method provided by this invention, after receiving the planned trajectory, the controlled vehicle extends the trajectory at its end point and selects a suitable reference trajectory point on the trajectory fitting line based on the vehicle error. This reference point serves as the actual reference trajectory for constructing a vehicle prediction model based on nonlinear model predictive control (NPMC). Simultaneously, after setting control constraints, a series of control quantities are obtained by solving the cost function through rolling optimization. The first control quantity is then applied to the controlled vehicle. The above steps are repeated in the next cycle, thereby forming sustainable control of the vehicle's lateral and longitudinal directions.

[0217] In the embodiments of the present invention, a nonlinear model is used for model predictive control (NPMC), which can avoid the accuracy loss caused by linearizing the model. At the same time, the curvature of the reference trajectory point and its dynamic understeering correction factor, the deviation between the control quantity at the previous moment and the current prediction quantity are introduced into the cost function and reasonable weights are set to solve problems such as steering wheel vibration and understeering in continuous large curvature turning scenarios.

[0218] More specifically, in this invention, by setting a dynamic adjustment factor τ for understeer, the spacing between reference points is adjusted, and reference trajectory points can be dynamically selected according to the vehicle state and error, so that the selected reference trajectory points are more consistent with the state predicted by the NMPC model, thereby optimizing the solution of the most suitable control quantity to reduce the error.

[0219] By setting a dynamic error control factor λ, the weights of different errors can be dynamically adjusted. When lateral error and heading error coexist, priority is given to reducing lateral error; and when the lateral error is small, the heading error is then corrected, thereby quickly completing the reference trajectory point following control.

[0220] Meanwhile, the curvature at the reference point is added as a feedforward to the cost function, making full use of the information of the reference trajectory point to improve the controller's response speed. This allows the control quantity calculated by NMPC in continuous high curvature turning scenarios to enable the steering wheel to respond quickly, thereby maintaining a small vehicle error.

[0221] Moreover, the deviation between the control quantity at the previous moment and the current predicted value is introduced into the cost function, so that the NMPC calculation value remains continuous, avoiding the situation of steering wheel shaking caused by large changes in the control quantity between moments in the prior art.

[0222] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, apparatus, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0223] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0224] The above description is merely a preferred embodiment of the present invention and should not be construed as limiting the scope of the invention. Therefore, any equivalent variations made in accordance with the claims of the present invention are still within the scope of the present invention.

Claims

1. A method for controlling the longitudinal and lateral directions of a vehicle, characterized in that, It should include at least the following steps: Receive the decision-making and planning trajectory of the controlled vehicle, as well as the actual vehicle status of the controlled vehicle; Based on the decision-planning trajectory, vehicle status, and current vehicle dynamic error, multiple reference trajectory points are selected on the trajectory fitting line to obtain the actual reference trajectory within the current prediction period. The vehicle dynamic error is the deviation between the actual reference trajectory and the predicted trajectory information at the current moment, and it includes at least the vehicle lateral error. The nonlinear vehicle prediction model is rolled and optimized based on the actual reference trajectory and the preset cost function to obtain the optimal control quantity sequence in the prediction time domain; wherein the preset cost function includes at least lateral offset error and heading error with adjustable weights. The controlled vehicle is controlled according to the optimal control sequence.

2. The method as described in claim 1, characterized in that, The method further includes: The dynamic error of the previous prediction period is obtained. When the vehicle lateral error in the dynamic error is greater than the third threshold, the weight of the lateral error in the cost function is increased in the current prediction period, while the weight of the heading error is decreased. When the vehicle lateral error in the dynamic error is less than the fourth threshold, the weight of the heading error in the cost function is increased and the weight of the lateral error is decreased in the current prediction period. The fourth threshold is less than the third threshold.

3. The method as described in claim 1 or 2, characterized in that, The step of selecting multiple reference trajectory points on the trajectory fitting line based on the decision-planning trajectory, vehicle status, and current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period further includes: The decision planning trajectory is extended at its endpoint; Based on the vehicle dynamic error, determine the distance between adjacent trajectory points in the reference trajectory points; Based on the vehicle's current speed and cycle time, the distance traveled by the vehicle at each moment in the prediction time domain is calculated, and multiple reference trajectory points are selected to obtain the actual reference trajectory within the current prediction period.

4. The method as described in claim 3, characterized in that, The step of determining the distance between adjacent trajectory points in the reference trajectory based on vehicle dynamic error is as follows: The dynamic error of the previous prediction period is obtained. When the vehicle lateral error in the dynamic error is greater than the first threshold, the value of the dynamic adjustment factor is reduced so as to reduce the distance between adjacent reference points in the selected reference points in the current prediction period. When the vehicle lateral error in the dynamic error is less than the second threshold, the value of the dynamic adjustment factor is increased to expand the distance between adjacent reference points in the selected reference points within the current prediction period. Wherein, the second threshold is less than the first threshold.

5. The method as described in claim 4, characterized in that, The step of performing rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and a preset cost function to obtain the optimal control quantity sequence in the prediction time domain further includes: constructing a vehicle prediction model based on nonlinear model predictive control and obtaining the predicted trajectory of the controlled vehicle; including: The following formula is used to calculate the predicted trajectory point information for each moment within the prediction period; in, This represents the x-coordinate value of the vehicle at time k within this prediction period; The vehicle's ordinate value at time k; Let k be the vehicle's heading angle at time k; Let k be the vehicle speed at time k; Let k be the angle of the vehicle's front wheels. Let k be the vehicle's angular velocity at time k; Let k be the x-coordinate of the reference trajectory. Let k be the ordinate of the vehicle's reference trajectory. The reference heading for the vehicle at time k; The x-coordinate value of the vehicle at time k+1; The vehicle's ordinate value at time k+1; Let k+1 be the vehicle's heading angle. Let k+1 be the vehicle speed. The vehicle's lateral error at time k+1; Let $\frac{k+1}{k+1}$ be the vehicle heading error.

6. The method as described in claim 5, characterized in that, The preset cost function is: in, This indicates the deviation between the predicted value and the front wheel angle corresponding to the curvature value at the reference trajectory point; This indicates that the calculation results of the control quantity from the previous moment have been incorporated; The weight representing the rate of change of the lateral error increment under constraint; These represent lateral offset error, heading error, and velocity error, respectively. Weights used to dynamically adjust lateral and heading errors; Indicates the front wheel angle and acceleration control values; This represents the increment of the front wheel steering angle and acceleration; The constraints are as follows: 。 7. The method as described in claim 6, characterized in that, The step of performing rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and a preset cost function to obtain the optimal control quantity sequence in the prediction time domain further includes: At time k, the cost function and constraints are substituted into the nonlinear solver for calculation, and the optimal control quantity sequence in the prediction time domain is obtained, which includes at least the vehicle front wheel steering angle at each time in the control time domain. Apply the first control quantity in the optimal control quantity sequence to the controlled vehicle; Repeat the above steps at time k+1 to achieve sustainable control of the vehicle's lateral and longitudinal directions.

8. A vehicle longitudinal and lateral control system, characterized in that, At least including: The real-time status acquisition unit is used to receive the decision-making and planning trajectory of the controlled vehicle, as well as the actual vehicle status of the controlled vehicle. The reference trajectory acquisition unit is used to select multiple reference trajectory points on the trajectory fitting line based on the decision planning trajectory, vehicle status and current vehicle dynamic error, and obtain the actual reference trajectory within the current prediction period. The vehicle dynamic error is the deviation between the actual reference trajectory and the predicted trajectory information of the vehicle at the current moment, which includes at least the vehicle lateral error. The rolling optimization solution unit is used to perform rolling optimization on the nonlinear vehicle prediction model based on the actual reference trajectory and the preset cost function to obtain the optimal control quantity sequence in the prediction time domain; wherein the preset cost function includes at least lateral offset error and heading error with adjustable weights. A control processing unit is used to control the controlled vehicle according to the optimal control quantity sequence.

9. The system as described in claim 8, characterized in that, Further includes: The adjacent distance coefficient adjustment unit is used to determine the distance between adjacent trajectory points in the reference trajectory points based on the vehicle dynamic error. Specifically, when the vehicle lateral error in the dynamic error of the previous prediction period is greater than a first threshold, the distance between adjacent reference points in the selected reference points is reduced in the current prediction period; when the vehicle lateral error in the dynamic error is less than a second threshold, the distance between adjacent reference points in the selected reference points is increased in the current prediction period. The weighting coefficient adjustment unit is used to obtain the dynamic error of the previous prediction period. When the vehicle lateral error in the dynamic error is greater than the third threshold, the weight of the lateral error in the cost function is increased in the current prediction period, while the weight of the heading error is decreased at the same time. When the vehicle lateral error in the dynamic error is less than the fourth threshold, the weight of the heading error in the cost function is increased in the current prediction period, while the weight of the lateral error is decreased at the same time. Wherein, the second threshold is less than the first threshold; and the fourth threshold is less than the third threshold.

10. The system as described in claim 9, characterized in that, Further includes: The predicted trajectory acquisition unit is used to construct a vehicle prediction model based on nonlinear model predictive control and obtain the predicted trajectory of the controlled vehicle. Specifically, the predicted trajectory acquisition unit is used to calculate the predicted trajectory point information at each moment within the prediction period using the following formula: in, This represents the x-coordinate value of the vehicle at time k within this prediction period; The vehicle's ordinate value at time k; Let k be the vehicle's heading angle at time k; Let k be the vehicle speed at time k; Let k be the angle of the vehicle's front wheels. Let k be the vehicle's angular velocity at time k; Let k be the x-coordinate of the reference trajectory. Let k be the ordinate of the vehicle's reference trajectory. The reference heading for the vehicle at time k; The x-coordinate value of the vehicle at time k+1; The vehicle's ordinate value at time k+1; Let k+1 be the vehicle's heading angle. Let k+1 be the vehicle speed. The vehicle's lateral error at time k+1; Let $\frac{k+1}{k+1}$ be the vehicle heading error.

11. The system as claimed in claim 10, characterized in that, It further includes a cost function and constraint setting unit for setting the cost function and constraints; Specifically, in the cost function and constraint setting unit, the cost function set for the lateral control process is as follows: in, This indicates the deviation between the predicted value and the front wheel angle corresponding to the curvature value at the reference trajectory point; This indicates that the calculation results of the control quantity from the previous moment have been incorporated; The weight representing the rate of change of the lateral error increment under constraint; These represent lateral offset error, heading error, and velocity error, respectively. Weights used to dynamically adjust lateral and heading errors; Indicates the front wheel angle and acceleration control values; This represents the increment of the front wheel steering angle and acceleration; The constraints are as follows: 。 12. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 7.

13. A computing device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1 to 7.

14. A vehicle having at least one lane change decision device, wherein the lane change decision device is equipped with a system as described in any one of claims 8 to 11.

Images