Vehicle longitudinal and transverse control method and system, storage medium and vehicle

Abstract

The invention discloses a vehicle longitudinal and transverse control method, which comprises the following steps of: receiving a decision planning track of a controlled vehicle and an actual vehicle state of the controlled vehicle; according to the decision planning trajectory, the vehicle state and the current vehicle dynamic error, a plurality of reference trajectory points are selected on the trajectory fitting line, the actual reference trajectory in the prediction period is obtained, and the vehicle dynamic error is the deviation between the actual reference trajectory of the vehicle at the current moment and the prediction trajectory information and at least comprises the vehicle transverse error; performing rolling optimization on a nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an optimal control quantity sequence in a prediction time domain; and controlling the controlled vehicle according to the optimal control quantity sequence. The invention further discloses a corresponding system, a storage medium and a vehicle. By implementing the method, the smoothness and stability of longitudinal and transverse control of the vehicle can be improved.

Description

Technical Field

[0001] The present invention relates to a lateral and longitudinal control method in the field of autonomous driving, and more particularly to a vehicle longitudinal and lateral control method, system, storage medium, and vehicle based on nonlinear model predictive control (NMPC) for autonomous parking and driving. Background Art

[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 use the current vehicle state and reference trajectory to construct an error model. This error model is then linearized and discretized to form a quadratic cost function. After setting control variable constraints, the cost function is solved to obtain a series of control increments, and the first control increment in the sequence is applied to the controlled object. These steps are repeated in the next cycle, thus achieving sustainable lateral and longitudinal control of the vehicle.

[0003] Since MPC makes a linear prediction of subsequent states based on the first error state within the prediction domain, its 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 will have problems such as steering wheel jitter and understeer 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 the vehicle longitudinal and lateral control and avoid the occurrence of problems such as steering wheel shaking and understeering.

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

[0006] receiving the decision-making planning trajectory of the controlled vehicle and the actual vehicle state of the controlled vehicle;

[0007] Based on the decision-making planned trajectory, vehicle state, 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 of the vehicle at the current moment and the predicted trajectory information, which at least includes the vehicle lateral error;

[0008] Performing rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an optimal control quantity sequence in the prediction time domain;

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

[0010] The preset cost function at least includes a lateral offset error and a heading error with adjustable weights;

[0011] The method further comprises:

[0012] Obtaining a dynamic error in a previous prediction cycle, and when a lateral error of the vehicle in the dynamic error is greater than a third threshold, increasing a weight of the lateral error in the cost function and simultaneously decreasing a weight of the heading error in the current prediction cycle;

[0013] When the lateral error of the vehicle in the dynamic error is less than a fourth threshold, increasing the weight of the heading error in the cost function and reducing the weight of the lateral error during the current prediction period;

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

[0015] The step of selecting a plurality of reference trajectory points on the trajectory fitting line according to the decision-making planned trajectory, the vehicle state, and the current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period further includes:

[0016] Performing terminal extension processing on the decision planning trajectory;

[0017] Determine the distance between adjacent trajectory points in the reference trajectory according to the vehicle dynamic error;

[0018] According to the current speed and cycle time of the vehicle, the distance traveled by the vehicle at each moment in the prediction time domain is calculated respectively, and multiple reference trajectory points are selected to obtain the actual reference trajectory within this prediction cycle.

[0019] The distance between adjacent track points in the reference track points is determined based on the vehicle dynamic error, specifically:

[0020] obtaining a dynamic error of a previous prediction cycle, and when a lateral error of the vehicle in the dynamic error is greater than a first threshold, reducing a value of a dynamic adjustment factor to reduce a distance between adjacent reference points among the selected reference points in the current prediction cycle;

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

[0022] The second threshold is smaller than the first threshold.

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

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

[0025]

[0026]

[0027]

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

[0029]

[0030]

[0031] Among them, x k is the horizontal coordinate value of the vehicle at time k in this prediction period; k is the vertical coordinate value of the vehicle at time k; is the vehicle heading angle at time k; v k is the vehicle speed at time k; σ k is the front wheel angle of the vehicle at time k; a k is the vehicle angular velocity at time k; ref-x k ref-y is the horizontal coordinate of the reference trajectory at time k; k is the ordinate of the vehicle reference trajectory at time k; is the vehicle reference heading at time k; x k+1 is the vehicle's horizontal coordinate value at time k+1; y k+1 is the vertical coordinate value of the vehicle at time k+1; v is the vehicle heading angle at time k+1; k+1 is the vehicle speed at time k+1; cte k+1 is the lateral error of the vehicle at time k+1; epsi k+1 is the vehicle heading error at time k+1.

[0032] Wherein, the preset cost function is:

[0033]

[0034] in, Indicates the deviation of the front wheel angle corresponding to the curvature value at the reference trajectory point;

[0035] Indicates the calculation result of the control quantity introduced at the previous moment;

[0036] Represents the weight of the incremental rate of change of the constraint lateral error;

[0037] Represent the lateral offset error, heading error and speed error respectively, and λ is used to dynamically adjust the weights of the lateral error and heading error;

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

[0039] Indicates the increment of front wheel angle and acceleration;

[0040] The constraints are:

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

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

[0043] The step of performing rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an 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 to obtain the optimal control quantity sequence in the prediction time domain, which at least includes the vehicle front wheel steering angle at each time in the control time domain;

[0045] Applying the first control variable in the optimal control variable sequence to the controlled vehicle;

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

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

[0048] A real-time state acquisition unit, configured to receive the decision-making planning trajectory of the controlled vehicle and the actual vehicle state of the controlled vehicle;

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

[0050] A rolling optimization solving unit, configured to perform rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function, to obtain an optimal control quantity sequence within a prediction time domain;

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

[0052] Which further includes:

[0053] an adjacent distance coefficient adjustment unit, configured to determine the distance between adjacent reference 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 cycle is greater than a first threshold, the distance between adjacent reference points in the selected reference points is reduced in the current prediction cycle; and 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 cycle;

[0054] a weight coefficient adjustment unit, configured to obtain the dynamic error of the previous prediction cycle, and when the lateral error of the vehicle in the dynamic error is greater than a third threshold, increase the weight of the lateral error in the cost function and simultaneously reduce the weight of the heading error in the current prediction cycle; and when the lateral error of the vehicle in the dynamic error is less than a fourth threshold, increase the weight of the heading error in the cost function and simultaneously reduce the weight of the lateral error in the current prediction cycle;

[0055] The second threshold is smaller than the first threshold; and the fourth threshold is smaller than the third threshold.

[0056] Which 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] Among them, x k is the horizontal coordinate value of the vehicle at time k in this prediction period; k is the vertical coordinate value of the vehicle at time k; is the vehicle heading angle at time k; v k is the vehicle speed at time k; σ k is the front wheel angle of the vehicle at time k; a k is the vehicle angular velocity at time k; ref-x k ref-y is the horizontal coordinate of the reference trajectory at time k; k is the ordinate of the vehicle reference trajectory at time k; is the vehicle reference heading at time k; x k+1 is the vehicle's horizontal coordinate value at time k+1; y k+1 is the vertical coordinate value of the vehicle at time k+1; v is the vehicle heading angle at time k+1; k+1 is the vehicle speed at time k+1; cte k+1 is the lateral error of the vehicle at time k+1; epsi k+1 is the vehicle heading error at time k+1.

[0066] The method further includes a cost function and constraint setting unit for setting a cost function and constraint conditions, wherein the cost function at least includes a lateral offset error and a heading error with adjustable weights;

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

[0068]

[0069] in, Indicates the deviation of the front wheel angle corresponding to the curvature value at the reference trajectory point;

[0070] Indicates the calculation result of the control quantity introduced at the previous moment;

[0071] Represents the weight of the incremental rate of change of the constraint lateral error;

[0072] Represent the lateral offset error, heading error and speed error respectively, and λ is used to dynamically adjust the weights of the lateral error and heading error;

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

[0074] Indicates the increment of front wheel angle and acceleration;

[0075] The constraints are:

[0076] σ∈[σ min σ max ]

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

[0078] According to another aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above method are implemented.

[0079] In another aspect of the present invention, a computing device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the aforementioned method when executing the computer program.

[0080] Another aspect of the present invention provides a vehicle, which is provided with at least a lane change decision device, and the system as described above is deployed in the lane change decision device.

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

[0082] In the method provided by the present invention, after receiving the planned trajectory, the controlled vehicle extends the terminal trajectory and selects a suitable reference trajectory point on the trajectory fitting line based on the vehicle error as the actual reference trajectory for constructing a vehicle prediction model based on nonlinear model predictive control (NPMC). At the same time, after setting the control quantity constraint conditions, 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 in the lateral and longitudinal directions.

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

[0084] More specifically, in the present invention, by setting a dynamic adjustment factor for understeering and adjusting the spacing between reference points, 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 and solving the most appropriate control amount to reduce the error.

[0085] By setting a dynamic error control factor, the weights of different errors can be dynamically adjusted. When lateral error and heading error exist at the same time, priority is given to reducing the lateral error. When the lateral error is small, the heading error is corrected, thereby quickly completing the reference trajectory point following control.

[0086] At the same time, the curvature at the reference point is added to the cost function as feedforward, making full use of the information of the reference trajectory point and improving the response speed of the controller. In this way, the control amount calculated by NMPC in continuous large curvature turning scenarios can also allow the steering wheel to respond quickly, thereby maintaining a small vehicle error.

[0087] Moreover, the deviation between the control value at the previous moment and the current predicted value is introduced into the cost function, so that the NMPC calculated value maintains continuity, avoiding the situation in the existing technology where the steering wheel shakes due to the large change of the control value before and after the moment. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0089] Figure 1 A schematic diagram of the main process 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 A reference schematic diagram of a predicted trajectory and a reference trajectory within a prediction period of the present invention;

[0091] Figure 3 This is a schematic diagram of the application environment principle involved in the present invention;

[0092] Figure 4 The present invention provides a structural schematic diagram of an embodiment of a vehicle longitudinal and lateral control system based on nonlinear model predictive control;

[0093] Figure 5 for Figure 4 Schematic diagram of the structure of the reference trajectory acquisition unit. DETAILED DESCRIPTION

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

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

[0096] Step S10, receiving the decision-making planning trajectory of the controlled vehicle and the actual vehicle state of the controlled vehicle;

[0097] In specific examples, it is necessary to obtain map data of the vehicle's driving path or trajectory data output by the planning algorithm, that is, the planned trajectory of the controlled vehicle. Usually, it is necessary to obtain information such as the coordinates, heading angle, curvature, etc. of the trajectory. At the same time, it is necessary to obtain real-time status information of the controlled vehicle, such as current position and posture, speed, acceleration, heading angle, etc. This information can be obtained and processed by on-board sensors (such as GPS, lidar, camera, inertial measurement unit, etc.) and vehicle control unit. In some examples, it is also necessary to obtain external environment information, such as road signs, lane lines, obstacles, etc., and make planning and control decisions based on this information.

[0098] Step S11, selecting multiple reference trajectory points on the trajectory fitting line based on the decision-making planned trajectory, the vehicle state, and the current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period. The vehicle dynamic error is the deviation between the actual reference trajectory of the vehicle at the current moment and the predicted trajectory information, which at least includes the vehicle lateral error;

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

[0100] Step S110, performing terminal extension processing on the decision-making planning trajectory;

[0101] It can be understood that in the example of the present invention, in the subsequent nonlinear model predictive control (NMPC) process, it is necessary to predict a number of reference trajectory points in the time domain and compare them with the model state of NMPC. Therefore, it is necessary to extend the prediction at the end point of each trajectory so that the trajectory point can be correctly selected when the vehicle approaches the end point, thereby ensuring the accuracy of the NMPC calculation that depends on the trajectory points.

[0102] In one example, the information of the current reference trajectory point may include x0, y0, r, s0, using the vehicle kinematic model, the information of the next reference trajectory point x1, y1 can be derived from the current reference trajectory point. r, s1, and so on repeatedly to predict the length of the trajectory at the extended end point.

[0103] Among them, in the vehicle coordinate system:

[0104] d s =r·a

[0105]

[0106]

[0107] d φ =a

[0108] Then 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 from time k to time k+1 in this prediction period; d s is the arc length from time k to time k+1; r is the turning radius of the vehicle; d x is the longitudinal distance from time k to time k+1 in the vehicle body coordinate system; d y is the lateral distance from time k to time k+1 in the vehicle body coordinate system;

[0113] x0 is the horizontal coordinate value of the reference trajectory at time k in the global coordinate system; y0 is the vertical coordinate value of the reference trajectory at time k in the global coordinate system; 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 horizontal coordinate value of the reference trajectory at time k+1 in the global coordinate system; y1 is the vertical coordinate value of the reference trajectory at time k+1 in the global coordinate system; 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 k+1 reference trajectory point.

[0114] Step S111, based on the vehicle dynamic error, determines the distance between adjacent trajectory points in the reference trajectory point, or determines the number of trajectory points selected within the predicted period, etc.; for example, based on the current lateral deviation value, determines whether to shorten the distance between adjacent trajectory points, or adjust the number of reference points (for example, only preview about 3 to 5 points), etc.

[0115] In step S112, the distance traveled by the vehicle at each moment in the prediction time domain is calculated based on the current vehicle speed and cycle time, and multiple reference trajectory points are selected to obtain the actual reference trajectory in this prediction cycle.

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

[0117] The extended reference trajectory is converted from the global coordinate system to the vehicle body coordinate system. With s as the independent variable, x, y, and r are dependent variables, respectively, and four formulas are fitted using a fifth-order polynomial:

[0118] x=f1(s)

[0119] y=f2(s)

[0120]

[0121] r=f4(s)

[0122] According to the current speed and cycle time of the vehicle, the distance traveled by the vehicle at each moment in the prediction time domain is calculated:

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

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

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

[0126] Where: v is the current speed of the vehicle; i is the sequence of the prediction time domain, i = 1, ..., N, N is the prediction time domain; d t is the cycle time, in seconds;

[0127] Step S12, constructing a vehicle prediction model based on nonlinear model predictive control (NMPC) and obtaining a 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 following formula is used to sequentially calculate the predicted trajectory point information at each moment within the prediction period using the NMPC method;

[0130]

[0131]

[0132]

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

[0134]

[0135]

[0136] Among them, x k is the horizontal coordinate value of the vehicle at time k in this prediction period; k is the vertical coordinate value of the vehicle at time k; is the vehicle heading angle at time k; v k is the vehicle speed at time k; σ k is the front wheel angle of the vehicle at time k; a k is the vehicle angular velocity at time k; ref-x k ref-y is the horizontal coordinate of the reference trajectory at time k; k is the ordinate of the vehicle reference trajectory at time k; is the vehicle reference heading at time k; x k+1 is the vehicle's horizontal coordinate value at time k+1; y k+1 is the vertical coordinate value of the vehicle at time k+1; v is the vehicle heading angle at time k+1; k+1 is the vehicle speed at time k+1; cte k+1 is the lateral error of the vehicle at time k+1; epsi k+1 is the vehicle heading error at time k+1.

[0137] Step S13, performing rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an optimal control variable sequence in the prediction time domain;

[0138] First, in step S130, a cost function and constraints need to be set, wherein the cost function at least includes a lateral offset error and a heading error with adjustable weights;

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

[0140] Set the following cost function during lateral control:

[0141]

[0142] in, Indicates the deviation of the front wheel angle corresponding to the curvature value at the reference trajectory point, in order to speed up the controller response;

[0143] Indicates the introduction of the control quantity calculation result of the previous moment. 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 jitter problem caused by the large change of the control quantity value calculated by the existing MPC before and after.

[0144] Represents the weight of the incremental rate of change of the constraint lateral error;

[0145] They represent the lateral offset error, heading error, and speed error, i.e., the deviation between the actual value and the reference value; λ is used to dynamically adjust the weights of the lateral error and heading error;

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

[0147] Indicates the increment of front wheel angle and acceleration, the purpose is to ensure that the control quantity does not change suddenly;

[0148] The constraints are:

[0149] σ∈[σ min , O max ]

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

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

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

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

[0154] Applying the first control variable in the optimal control variable sequence to the controlled vehicle;

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

[0156] It can be understood that during the NMPC control process, within a prediction cycle, the NMPC-based vehicle prediction model calculates the optimal control input sequence for a period of time in the future based on the current state and reference trajectory. This sequence changes dynamically, with each time step corresponding to a control command. The first control variable of each prediction cycle should serve 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 gradually updated based on the prediction of the optimal control input, and the updated control commands are assigned to the controlled vehicle or system to perform control tasks, thereby achieving dynamic and smooth control of the vehicle.

[0157] In a specific example, in step S11, the distance between adjacent track points in the reference track point is determined according to the vehicle dynamic error, specifically:

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

[0159] When the vehicle lateral error cte in the dynamic error is less than a 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 cycle; this allows the MPC to better consider the correction of the heading error.

[0160] The second threshold is smaller than the first threshold.

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

[0162] Obtaining the dynamic error of the previous prediction cycle, and when the vehicle lateral error cte in the dynamic error is greater than a third threshold, increasing the lambda value in the cost function to increase the weight of the lateral error in the current prediction cycle, while reducing the weight of the heading error, thereby prompting the MPC to reduce the lateral error as quickly as possible and weaken the influence of the heading error;

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

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

[0165] It is understood that in the method provided by the present invention, after receiving the planned trajectory, the controlled vehicle extends the trajectory to its terminal point and selects appropriate reference trajectory points on the trajectory fitting line based on the vehicle error. These reference points serve as the actual reference trajectory for constructing the vehicle prediction model based on nonlinear model predictive control. Furthermore, after setting control variable constraints, a cost function is solved through rolling optimization to obtain a series of control variables, the first of which is applied to the controlled vehicle. These steps are repeated in the next cycle, thereby achieving sustainable lateral and longitudinal control of the vehicle.

[0166] In an embodiment of the present invention, in order to avoid the loss of accuracy caused by model linearization, a nonlinear model is used for model predictive control; at the same time, in order to solve problems such as steering wheel shaking and understeer in continuous large curvature turning scenarios, the curvature of the reference trajectory point and its dynamic understeer correction factor, the deviation between the control amount at the previous moment and the current predicted amount 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 and adjusting the spacing between reference points, reference trajectory points can be dynamically selected based on 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 and solving the most appropriate control quantity to reduce the error.

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

[0169] At the same time, the curvature at the reference point is added as feedforward to the cost function to make full use of the information of the reference trajectory point and improve the response speed of the controller. In this way, the control amount calculated by NMPC in continuous large curvature turning scenarios can also allow the steering wheel to respond quickly, thereby maintaining a small vehicle error.

[0170] The deviation between the control value at the previous moment and the current predicted value is introduced into the cost function to maintain the continuity of the NMPC calculated value, avoiding the steering wheel shaking caused by the large change of the control value before and after the existing technology.

[0171] like Figure 4 FIG. 1 is a schematic diagram showing a structure of an embodiment of a vehicle longitudinal and lateral control system based on nonlinear model predictive control provided by the present invention. Figure 5As shown, in this embodiment, the system 1 at least includes:

[0172] A real-time state acquisition unit 10 is used to receive the decision-making planning trajectory of the controlled vehicle and the actual vehicle state of the controlled vehicle;

[0173] The reference trajectory acquisition unit 11 is used to select multiple reference trajectory points on the trajectory fitting line according to the decision-making planned trajectory, vehicle state and current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period;

[0174] Wherein, the reference trajectory acquisition unit 11 further includes:

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

[0176] a trajectory point parameter determination unit 111 for determining the distance or number between adjacent trajectory points in the reference trajectory point according to 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 current vehicle speed and cycle time, select multiple reference trajectory points, and obtain the actual reference trajectory in this prediction period.

[0178] A predicted trajectory acquisition unit 12 is used to construct a vehicle prediction model based on nonlinear model predictive control and obtain a 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] Among them, x k is the horizontal coordinate value of the vehicle at time k in this prediction period; k is the vertical coordinate value of the vehicle at time k; is the vehicle heading angle at time k; v k is the vehicle speed at time k; σ k is the front wheel angle of the vehicle at time k; a k is the vehicle angular velocity at time k; ref-x k ref-y is the horizontal coordinate of the reference trajectory at time k; k is the ordinate of the vehicle reference trajectory at time k; is the vehicle reference heading at time k; x k+1 is the horizontal coordinate value of the vehicle at time k+1; k+1 is the vertical coordinate value of the vehicle at time k+1; is the vehicle heading angle at time k+1; v k+1 is the vehicle speed at time k+1; cte k+1 is the lateral error of the vehicle at time k+1; epsi k+1 is the vehicle heading error at time k+1.

[0187] A cost function and constraint setting unit 13 is used to set a cost function and constraint conditions, wherein the cost function at least includes a lateral offset error and a heading error with adjustable weights;

[0188] Among them, in the cost function and constraint setting unit 13, the cost function set in the lateral control process is as follows:

[0189]

[0190] in, Indicates the deviation of the front wheel angle corresponding to the curvature value at the reference trajectory point;

[0191] Indicates the calculation result of the control quantity introduced at the previous moment;

[0192] Represents the weight of the incremental rate of change of the constraint lateral error;

[0193] Represent the lateral offset error, heading error and speed error respectively, and λ is used to dynamically adjust the weights of the lateral error and heading error;

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

[0195] Indicates the increment of front wheel angle and acceleration;

[0196] The constraints are:

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

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

[0199] The rolling optimization solving unit 14 performs rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an optimal control quantity sequence in the prediction time domain;

[0200] a control processing unit 18, configured to control the controlled vehicle according to the optimal control quantity sequence;

[0201] A dynamic error evaluation unit 15 is configured to obtain a dynamic error based on a deviation between the actual reference trajectory and the predicted trajectory information at a current moment, wherein the dynamic error includes at least a vehicle lateral error cte value;

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

[0203] an adjacent distance coefficient adjustment unit 16 for determining the distance between adjacent track points in the reference track point according to the vehicle dynamic error;

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

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

[0206] The second threshold is smaller than the first threshold.

[0207] Which further includes:

[0208] a weight coefficient adjustment unit 17 for obtaining the dynamic error of the previous prediction cycle and, when the vehicle lateral error cte in the dynamic error is greater than a third threshold, increasing the lambda value in the cost function to increase the weight of the lateral error in the current prediction cycle and simultaneously reducing the weight of the heading error;

[0209] When the vehicle lateral error cte in the dynamic error is less than a fourth threshold, reducing the lambda value in the cost function to increase the weight of the heading error and reduce the weight of the lateral error in the current prediction period;

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

[0211] For more details, please refer to and combine the above Figures 1 to 3 The description is not repeated here.

[0212] In another aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the above-mentioned Figures 1 to 3 For more details, please refer to and combine the above Figures 1 to 3 The description is not repeated here.

[0213] In another aspect of the present invention, a computing device is provided, comprising 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 implement the aforementioned Figures 1 to 3 For more details, please refer to and combine the above Figures 1 to 3 The description is not repeated here.

[0214] Another aspect of the present invention further provides a vehicle, which is provided with at least a lane change decision device, wherein the lane change decision device is deployed with the aforementioned Figure 4 and Figure 5 For more details, please refer to and combine the above Figure 4 and Figure 5 The description is not repeated here.

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

[0216] In the method provided by this invention, after receiving a planned trajectory, the controlled vehicle extends the terminal trajectory and selects appropriate reference points on the trajectory fitting line based on vehicle errors. These points serve as the actual reference trajectory for constructing a vehicle prediction model based on nonlinear model predictive control (NPMC). Furthermore, after setting control variable constraints, a cost function is solved through rolling optimization to obtain a series of control variables, the first of which is applied to the controlled vehicle. These steps are repeated in the next cycle, thereby achieving sustainable lateral and longitudinal control of the vehicle.

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

[0218] More specifically, in the present invention, by setting a dynamic adjustment factor τ for understeering and adjusting the spacing between reference points, 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 and solving the most appropriate control amount 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 exist at the same time, priority is given to reducing the lateral error. When the lateral error is small, the heading error is corrected, thereby quickly completing the reference trajectory point following control.

[0220] At the same time, the curvature at the reference point is added to the cost function as feedforward, making full use of the information of the reference trajectory point and improving the response speed of the controller. In this way, the control amount calculated by NMPC in continuous large curvature turning scenarios can also allow the steering wheel to respond quickly, thereby maintaining a small vehicle error.

[0221] Moreover, the deviation between the control value at the previous moment and the current predicted value is introduced into the cost function, so that the NMPC calculated value maintains continuity, avoiding the situation in the existing technology where the steering wheel shakes due to the large change of the control value before and after the moment.

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

[0223] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts 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, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0224] The above disclosure is only a preferred embodiment of the present invention and certainly cannot be used to limit the scope of the present invention. Therefore, equivalent changes made according to the claims of the present invention are still within the scope of the present invention.

Claims

1. A vehicle longitudinal and lateral control method, characterized in that: At least the following steps are included: Receiving the decision planning trajectory of the controlled vehicle and the actual vehicle state of the controlled vehicle; According to the decision-making planning trajectory, the vehicle state and the 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 of the vehicle at the current moment and the predicted trajectory information, which at least includes the vehicle lateral error; According to the actual reference trajectory and the preset cost function, a nonlinear vehicle prediction model is subjected to rolling optimization to obtain an optimal control quantity sequence in the prediction time domain; The controlled vehicle is controlled according to the optimal control amount sequence.

2. The method according to claim 1, characterized in that in, The preset cost function at least includes a lateral offset error and a heading error with adjustable weights; The method further comprises: Obtaining a dynamic error of a previous prediction cycle, and when a lateral error of the vehicle in the dynamic error is greater than a third threshold, increasing a weight of the lateral error in the cost function in the current prediction cycle, and reducing a weight of the heading error at the same time; When the lateral error of the vehicle in the dynamic error is less than a fourth threshold, increasing the weight of the heading error in the cost function within the current prediction cycle, and reducing the weight of the lateral error at the same time; The fourth threshold is smaller than the third threshold.

3. The method according to claim 1 or 2, characterized in that The step of selecting a plurality of reference trajectory points on the trajectory fitting line according to the decision-making planning trajectory, the vehicle state and the current vehicle dynamic error to obtain the actual reference trajectory within the current prediction period further includes: Performing terminal extension processing on the decision planning trajectory; Determine the distance between adjacent trajectory points in the reference trajectory points according to the vehicle dynamic error; According to the current speed and cycle time of the vehicle, the distance traveled by the vehicle at each moment in the prediction time domain is calculated respectively, and multiple reference trajectory points are selected to obtain the actual reference trajectory in this prediction cycle.

4. The method according to claim 3, characterized in that: The distance between adjacent track points in the reference track point is determined according to the vehicle dynamic error, specifically: The dynamic error of the previous prediction cycle is obtained, and when the lateral error of the vehicle in the dynamic error is greater than a first threshold, the value of the dynamic adjustment factor is reduced to reduce the distance between adjacent reference points in the selected reference points in the current prediction cycle; When the vehicle lateral error in the dynamic error is less than a second threshold, increasing the value of the dynamic adjustment factor to expand the distance between adjacent reference points in the selected reference points within the current prediction cycle; The second threshold is smaller than the first threshold.

5. The method according to claim 4, characterized in that The method further comprises: performing rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and the preset cost function to obtain the optimal control quantity sequence in the prediction time domain, and further comprises: constructing a vehicle prediction model based on nonlinear model predictive control, and obtaining the predicted trajectory of the controlled vehicle; comprising: The following formula is used to calculate the predicted trajectory point information at each moment in the prediction period; in k+1 =in k +a k *d t Among them, x k is the horizontal coordinate value of the vehicle at time k in this prediction period; k is the vertical coordinate value of the vehicle at time k; v is the vehicle heading angle at time k; k is the vehicle speed at time k; σ k is the front wheel angle of the vehicle at time k; a k is the vehicle angular velocity at time k; ref_x k ref_y is the horizontal coordinate of the reference trajectory at time k; k is the ordinate of the vehicle reference trajectory at time k; is the reference heading of the vehicle at time k; x k+1 is the horizontal coordinate value of the vehicle at time k+1; y k+1 is the vertical coordinate value of the vehicle at time k+1; v is the vehicle heading angle at time k+1; k+1 is the vehicle speed at time k+1; cte k+1 is the lateral error of the vehicle at time k+1; epsi k+1 is the vehicle heading error at time k+1.

6. The method according to claim 5, characterized in that The preset cost function is: in, Indicates the deviation of the front wheel angle corresponding to the curvature value at the reference trajectory point; Indicates the calculation result of the control quantity introduced at the previous moment; Represents the weight of the incremental change rate of the lateral error added to the constraint; They represent the lateral offset error, heading error and speed error respectively, and λ is used to dynamically adjust the weights of the lateral error and heading error; Indicates control variables such as front wheel angle and acceleration; Indicates the increment of front wheel steering angle and acceleration; The constraints are: σ∈[σ min ,s max ] a∈[a min ,a max ]。 7. The method according to claim 6, characterized in that The step of performing rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an optimal control quantity sequence in the prediction time domain further includes: At time k, the cost function and constraint conditions are substituted into the nonlinear solver for solution calculation, and the optimal control quantity sequence in the prediction time domain is calculated, which at least includes the vehicle front wheel steering angle at each time in the control time domain; Applying the first control variable in the optimal control variable sequence to the controlled vehicle; Repeat the above steps at time k+1, thereby achieving sustainable control of the vehicle in the lateral and longitudinal directions.

8. A vehicle longitudinal and lateral control system, characterized in that: At least: A real-time state acquisition unit, used to receive the decision-making planning trajectory of the controlled vehicle and the actual vehicle state of the controlled vehicle; A reference trajectory acquisition unit is used to select multiple reference trajectory points on the trajectory fitting line according to the decision-making planning trajectory, the vehicle state and the 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 of the vehicle at the current moment and the predicted trajectory information, which at least includes the vehicle lateral error; A rolling optimization solving unit, used for performing rolling optimization on the nonlinear vehicle prediction model according to the actual reference trajectory and a preset cost function to obtain an optimal control quantity sequence in a prediction time domain; A control processing unit is used to control the controlled vehicle according to the optimal control quantity sequence.

9. The system according to claim 8, characterized in that Further including: The adjacent distance coefficient adjustment unit is used to determine the distance between adjacent track points in the reference track points according to the vehicle dynamic error; specifically, when the vehicle lateral error in the dynamic error of the previous prediction cycle is greater than a first threshold, the distance between adjacent reference points in the selected reference points is reduced in the current prediction cycle; 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 expanded in the current prediction cycle; A weight coefficient adjustment unit, used to obtain the dynamic error of the previous prediction cycle, when the lateral error of the vehicle in the dynamic error is greater than the third threshold, increase the weight of the lateral error in the cost function in the current prediction cycle, and reduce the weight of the heading error at the same time; when the lateral error of the vehicle in the dynamic error is less than the fourth threshold, increase the weight of the heading error in the cost function in the current prediction cycle, and reduce the weight of the lateral error at the same time; Among them, the second threshold is smaller than the first threshold; the fourth threshold is smaller than the third threshold.

10. The system according to claim 9, characterized in that Further including: A predicted trajectory acquisition unit, the predicted trajectory acquisition unit is used to construct a vehicle prediction model based on nonlinear model predictive control and obtain a predicted trajectory of the controlled vehicle; Specifically, the predicted trajectory acquisition unit is used to calculate and obtain the predicted trajectory point information at each moment in the prediction period using the following formula: in k+1 =in k +a k *d t Among them, x k is the horizontal coordinate value of the vehicle at time k in this prediction period; k is the vertical coordinate value of the vehicle at time k; v is the vehicle heading angle at time k; k is the vehicle speed at time k; σ k is the front wheel angle of the vehicle at time k; a k is the vehicle angular velocity at time k; ref_x k ref_y is the horizontal coordinate of the reference trajectory at time k; k is the ordinate of the vehicle reference trajectory at time k; is the reference heading of the vehicle at time k; x k+1 is the horizontal coordinate value of the vehicle at time k+1; y k+1 is the vertical coordinate value of the vehicle at time k+1; v is the vehicle heading angle at time k+1; k+1 is the vehicle speed at time k+1; cte k+1 is the lateral error of the vehicle at time k+1; epsi k+1 is the vehicle heading error at time k+1.

11. The system according to claim 10, characterized in that It further comprises a cost function and constraint setting unit, which is used to set a cost function and constraint conditions, wherein the cost function at least includes a lateral offset error and a heading error with adjustable weights; Specifically, in the cost function and constraint setting unit, the cost function in the lateral control process is set as follows: in, Indicates the deviation of the front wheel angle corresponding to the curvature value at the reference trajectory point; Indicates the calculation result of the control quantity introduced at the previous moment; Represents the weight of the incremental change rate of the lateral error added to the constraint; They represent the lateral offset error, heading error and speed error respectively, and λ is used to dynamically adjust the weights of the lateral error and heading error; Indicates control variables such as front wheel angle and acceleration; Indicates the increment of front wheel steering angle and acceleration; The constraints are: σ∈[σ min ,s max ] a∈[a min ,a max ]。 12. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

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, the steps of the method according to any one of claims 1 to 7 are implemented.

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

Images