An integrated method for lane-changing planning and control in unmanned driving

Abstract

The present invention relates to an unmanned lane change planning and control integrated method, including: 1) establishing a probability collision area around a dynamic obstacle vehicle; 2) constructing a self-vehicle prediction model; 3) initially constructing an optimization target and a constraint condition according to upstream information and the self-vehicle prediction model; 4) judging the initially constructed constraint condition, if the constraint condition is non-convex or the upper bound is less than the lower bound, proceeding to step 5), otherwise proceeding to step 6); 5) modifying the expected value in the constraint condition and the optimization target, and reconstructing the constraint condition and the optimization target; 6) constructing a nonlinear optimal control problem; 7) solving the nonlinear optimal control problem, and obtaining the initial state of planning control; 8) if the solution fails, using the reconstructed target reference as the initial solution of planning control. Compared with the prior art, the present invention fully considers the uncertainty of environmental prediction, and combines the changing prediction step length, so as to ensure the accuracy of the solution result and improve the solution efficiency.

Description

Technical Field

[0001] The present invention relates to the field of intelligent automobile control technology, and in particular to an integrated method for unmanned driving lane change planning and control. Background Art

[0002] In the traditional unmanned vehicle architecture, trajectory planning and trajectory tracking control belong to the decision-making planning system and motion control system respectively. In recent years, with the in-depth research on unmanned driving technology, researchers have discovered the advantages and necessity of combining trajectory planning with tracking control, so many unified control methods for trajectory planning and tracking have emerged.

[0003] However, the existing integrated methods of local trajectory planning and tracking control only consider deterministic information, without systematically modeling and processing uncertain information, and most of them use fixed prediction steps, or do not use appropriate models to adapt the algorithm to the changing prediction step. For planning and control problems based on MPC (Model Predictive Control), if the prediction time domain is short, the algorithm will lose some of its ability to utilize future information, making the solution short-sighted; and when a fixed prediction step is selected, if the prediction time domain is expanded, the scale of the optimal control problem will increase, affecting the real-time performance of the solution. Summary of the invention

[0004] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide an integrated method for unmanned lane change planning and control, which can fully consider the uncertainty of environmental prediction and achieve the purpose of efficient and accurate solution.

[0005] The purpose of the present invention can be achieved by the following technical solution: An unmanned lane change planning and control integrated method comprises the following steps:

[0006] S1. Obtain upstream information, define the predicted lateral error and longitudinal error as two random variables according to the motion state of the surrounding dynamic obstacle vehicles and their predicted trajectory information, obtain the probability expression of the predicted error, and convert it into a deterministic constraint, and establish the probabilistic collision area in combination with the vehicle length, width and safety margin;

[0007] S2, constructing a vehicle dynamics model in the Frenet coordinate system as a self-vehicle prediction model;

[0008] S3, construct the initial optimization objectives and constraints based on the upstream information and the self-vehicle prediction model;

[0009] S4, judging the current constraint condition, if it is non-convex or the upper bound is smaller than the lower bound, executing step S5; otherwise, executing step S6;

[0010] S5. According to the left overtaking principle, the lane line constraint is modified, the spatial position constraint is changed to a constraint that the longitudinal position of the vehicle is located in front of or behind the obstacle vehicle, and the expected lateral distance is changed to reconstruct the constraint conditions and optimization objectives, and then step S6 is executed;

[0011] S6. Obtain constraint equations based on the relationship between the state vector, the control input vector, and the prediction step length, input all optimization target items at the time in the prediction time domain, obtain the final optimization target function, combine the ego-vehicle prediction model with the chance constraint, and construct a nonlinear optimal control problem;

[0012] S7, solving the nonlinear optimal control problem and obtaining the initial state of planning control. If the solution fails, the reconstructed optimization target is used as the initial state of planning control;

[0013] S8. Control the initial state according to the plan to achieve vehicle tracking control.

[0014] Furthermore, the step S1 specifically includes the following steps:

[0015] S11, obtaining upstream information, wherein the upstream information includes reference lines, positioning, perception, and trajectory prediction information;

[0016] S12. Use point mass motion model to describe the motion state of the vehicle around dynamic obstacles;

[0017] S13, designing a feedback controller according to the random variables of the lateral speed and longitudinal speed of the dynamic obstacle vehicle;

[0018] S14. Define chance constraints based on the uncertainty caused by the random speed variation of dynamic obstacle vehicles;

[0019] S15, constructing a probability distribution expression based on the lateral position and longitudinal position errors of the dynamic obstacle vehicle;

[0020] S16, converting the probability distribution expression into a certain confidence region;

[0021] S17. Establish a probabilistic collision area around the vehicle based on the length and width of the dynamic obstacle vehicle and an additional safety margin.

[0022] Furthermore, the feedback controller in step S13 is specifically:

[0023] u obj,k =K(ξ obj,k -ξ obj,ref,k )

[0024] Among them, the control input u obj,k represents the random variation of the lateral and longitudinal vehicle speeds, ξ obj,ref,krepresents the reference state of the dynamic obstacle vehicle provided by the prediction module, ξ obj,k Indicates the current state of the dynamic obstacle vehicle;

[0025] The opportunity constraints in step S14 are specifically:

[0026] Pr(ξ k ∈Ξ′ safe,k (ω(u obj,k )))≥p

[0027] That is, the system state vector ξ k In the collision-free safety zone Ξ′ safe,k The probability of being greater than p;

[0028] The probability distribution of the lateral position error and the longitudinal position error of the dynamic obstacle vehicle in step S15 obeys the normal distribution:

[0029]

[0030]

[0031] Among them, e x,k and e y,k are the variances of the lateral and longitudinal position errors, respectively. and are the variances of the lateral and longitudinal position errors, μ x and μ y are the means of the horizontal and vertical positions, respectively.

[0032] Furthermore, the specific process of step S16 is as follows:

[0033] According to the random variable sum of squares subject to χ with 2 degrees of freedom 2 Distribution, the confidence region satisfies the interior area of ​​the elliptical contour:

[0034]

[0035] in, is its p quantile;

[0036] The lengths of the semi-major axis and semi-minor axis of the ellipse boundary corresponding to the confidence region are:

[0037]

[0038]

[0039] in, and are the lengths of the semi-major and semi-minor axes of the ellipse respectively.

[0040] Furthermore, the specific process of step S17 is as follows:

[0041] According to the length and width of the vehicle and the confidence area, the probability collision rectangle around the vehicle is established as:

[0042]

[0043]

[0044] Among them, L rec,k and W rec,k are the length and width of the collision rectangle, l obj and w obj are the length and width of the vehicle, ε safe is an additional safety margin.

[0045] Furthermore, the vehicle prediction model in step S2 is specifically:

[0046]

[0047] in, is the state vector of the system, s k ,l k , v k are the longitudinal position, lateral position, heading angle and speed of the vehicle, u k =[δ f,k, a k ] T is the control input vector of the system, δ f,k ,a k are the steering wheel angles respectively.

[0048] Furthermore, the step S3 specifically includes the following steps:

[0049] S31. Taking the lateral distance of the vehicle's current position from the reference line at the kth moment as the optimization target:

[0050] J l,k =q l (l k -l ref ) 2

[0051] Among them, q l is the weight of the lateral distance objective term in the initial solution generation algorithm, l ref =0 indicates the expected value of the horizontal distance from the reference line;

[0052] S32, taking the heading angle deviation of the vehicle relative to the reference line at the kth moment as the optimization target:

[0053]

[0054] in, The weight of the heading angle target term in the initial solution generation algorithm;

[0055] S33, taking the error between the vehicle speed and the desired speed at the kth moment as the optimization target:

[0056] J v,k =q v (v k -v ref,k ) 2

[0057] Among them, q v is the weight of the vehicle speed target item in the initial solution generation algorithm, v ref,k is the expected value of the vehicle speed at the kth moment;

[0058] S34, taking the steering wheel angle and acceleration of the vehicle at the kth moment as the optimization target:

[0059]

[0060]

[0061] in, and r a are the weights of the steering wheel angle and acceleration objective items respectively;

[0062] S35. Establishing vehicle collision safety constraints:

[0063] (s ego,max <s obj,min )∪(s ego,min >s obj,max )∪(l ego,max <l obj,min )∪(l ego,min >l obj,max )

[0064] Among them, s ego,max and ego,min are the maximum and minimum values ​​of the horizontal coordinate of the vehicle, respectively, ego,max and l ego,min The maximum and minimum values ​​of the ordinate of the vehicle are, respectively, s obj,max and obj,min are the maximum and minimum values ​​of the horizontal coordinate of the obstacle vehicle, respectively, obj,max and l obj,min are the maximum and minimum values ​​of the ordinate of the obstacle vehicle respectively;

[0065] S36. Establish the vehicle heading angle deviation constraint at the kth moment:

[0066]

[0067] in, is the corresponding heading angle deviation limit;

[0068] S37. Establishing vehicle speed constraints:

[0069] 0≤v k ≤v limit

[0070] Among them, v limit is the maximum speed limit of the vehicle;

[0071] S38. Establishing vehicle steering wheel angle constraints:

[0072] |δ f,k |≤δ f,limit

[0073] Among them, δ f,limit is the maximum steering wheel angle;

[0074] S39. Establish acceleration constraints:

[0075] a dec,limit ≤a k ≤a acc,limit

[0076] Among them, a acc,limit and a dec,limit are the maximum acceleration and maximum deceleration of the vehicle respectively.

[0077] Furthermore, the step S4 specifically includes the following steps:

[0078] S41. First, determine whether the lane boundary and collision safety constraints are non-convex:

[0079] (s obj,left ≤s ego ≤s lane,left )∪(s obj,right ≤s ego ≤s lane,right )

[0080] If the judgment is yes, execute step S5, otherwise execute step S42;

[0081] S42. Determine whether the upper bound of the security constraint is less than the lower bound:

[0082] s lane,left <s obj,left

[0083] s obj,right <s lane,right

[0084] If the judgment is yes, execute step S5, otherwise execute step S6;

[0085] Among them, s lane,left and lane,right are the left and right boundaries of the lane, s obj,left and obj,right They are respectively the left and right boundaries of the collision safety zone determined by the obstacle vehicle.

[0086] Furthermore, the specific process of reconstructing the constraint conditions and the optimization target in step S5 is as follows:

[0087] S51. If the constraint condition is non-convex, follow the principle of overtaking on the left and reconstruct the collision safety constraint as follows:

[0088] s obj,left ≤s ego ≤s lane,left

[0089] Among them, s lane,left and obj,left They are respectively the left boundary of the lane and the left boundary of the collision safety zone determined by the obstacle vehicle;

[0090] S52. If the constraint condition is that the upper bound is smaller than the lower bound, then on the one hand, the spatial position constraint is reconstructed to be located behind the dynamic obstacle vehicle:

[0091] s ego ≤s obj,rear

[0092] Modify the desired lateral distance in the optimization objective:

[0093]

[0094] Modify the expected speed in the optimization objective to:

[0095]

[0096] Among them, s obj,rear and obj,front are the rear and front boundaries of the collision safety zone determined by the obstacle vehicle, respectively, v ref is the desired reference speed, v left and v right are the speeds of the dynamic obstacle vehicles in front of the left and right lanes, respectively, and w lane is the lane width, l lb Constrain lower bounds for the reconstructed lateral position;

[0097] On the other hand, the spatial position constraint is reconstructed as a dynamic obstacle in front of the vehicle:

[0098] s ego ≥sobj,front

[0099] Among them, s obj,rear and obj,front They are the rear and front boundaries of the collision safety zone determined by the obstacle vehicle, respectively. At this time, the expected state value in the optimization objective does not need to be reconstructed.

[0100] Furthermore, the step S6 specifically includes the following steps:

[0101] S61, simplify the relationship between the state vector, the control input vector and the prediction step length into a function f d , which constitutes the equation part of the constraint:

[0102]

[0103]

[0104] S62, adding up the optimization objective items at all times in the prediction time domain to obtain the final optimization objective function:

[0105]

[0106] The prediction time domain uses a variable prediction step method, the total control time domain is 10s, the precise control time domain is 5s, and the prediction step length of the first 5.0s is T S = 0.2s, the number of prediction steps is N S =25; the prediction step length for the next 5.0s is T L = 1.0s, the number of prediction steps is N S =25; total number of prediction steps N total =N S +N L =30;

[0107] S63. Combining the optimization objective with the constraints, including the chance constraints, the objective function and the constraints are expressed in the form of vectors and matrices, and the optimal control problem based on SMPC (Stochastic Model Predictive Control) is obtained:

[0108]

[0109]

[0110]

[0111]

[0112]

[0113]

[0114]

[0115] Among them, Ξ and U are the feasible sets of system state and control input, respectively, Q and R are the semi-positive definite weight matrices of system state vector and control input vector, respectively.

[0116] Compared with the prior art, the present invention fully considers the uncertainty of environmental prediction by establishing the probabilistic collision area of ​​surrounding dynamic obstacle vehicles, and combines the predicted uncertainty with the subsequently established constraint conditions, thereby being able to systematically model and process uncertainty information, so that the unmanned vehicle can effectively improve the accuracy of the solution results when executing the planning and control integrated algorithm solution.

[0117] The present invention reconstructs the constraints and optimization objectives by judging whether the constraints are non-convex or the upper bound is smaller than the lower bound, and then constructs a nonlinear optimization control problem. When constructing the nonlinear optimization control problem, a variable prediction step size is used, and the calculation in the long time domain is simplified to effectively improve the solution efficiency, ensure the real-time solution, and make full use of future information to obtain future guiding results. BRIEF DESCRIPTION OF THE DRAWINGS

[0118] Figure 1 It is a schematic diagram of the method flow of the present invention;

[0119] Figure 2 It is a schematic diagram of the application process of the embodiment;

[0120] Figure 3 A schematic diagram of a probability collision rectangular area of ​​a vehicle with surrounding dynamic obstacles in the present invention;

[0121] Figure 4 Schematic diagram of AABB collision detection in the Frenet coordinate system of the present invention;

[0122] Figure 5a and Figure 5b It is a schematic diagram of reconstructing the optimization target based on the speed of the dynamic obstacle vehicle in the present invention;

[0123] Figure 6 This is a schematic diagram of the optimization target reconstructed based on the lateral distance constraint of the present invention. DETAILED DESCRIPTION

[0124] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0125] Example

[0126] like Figure 1As shown, an unmanned driving lane change planning and control integrated method includes the following steps:

[0127] S1. Obtain upstream information, define the predicted lateral error and longitudinal error as two random variables according to the motion state of the surrounding dynamic obstacle vehicles and their predicted trajectory information, obtain the probability expression of the predicted error, and convert it into a deterministic constraint, and establish the probabilistic collision area in combination with the vehicle length, width and safety margin;

[0128] The upstream information includes reference line, positioning, perception and trajectory prediction information. The reference line information can be collected by the navigation module, the positioning information can be collected by the IMU module, and the perception and trajectory prediction information can be collected by the perception prediction module.

[0129] S2, constructing a vehicle dynamics model in the Frenet coordinate system as a self-vehicle prediction model;

[0130] S3, construct the initial optimization objectives and constraints based on the upstream information and the self-vehicle prediction model;

[0131] S4, judging the current constraint condition, if it is non-convex or the upper bound is smaller than the lower bound, executing step S5; otherwise, executing step S6;

[0132] S5. According to the left overtaking principle, the lane line constraint is modified, the spatial position constraint is changed to a constraint that the longitudinal position of the vehicle is located in front of or behind the obstacle vehicle, and the expected lateral distance is changed to reconstruct the constraint conditions and optimization objectives, and then step S6 is executed;

[0133] S6. Obtain constraint equations based on the relationship between the state vector, the control input vector, and the prediction step length, input all optimization target items at the time in the prediction time domain, obtain the final optimization target function, combine the ego-vehicle prediction model with the chance constraint, and construct a nonlinear optimal control problem;

[0134] S7, solving the nonlinear optimal control problem and obtaining the initial state of planning control. If the solution fails, the reconstructed optimization target is used as the initial state of planning control;

[0135] S8. Control the initial state according to the plan to achieve vehicle tracking control.

[0136] In step S1, a point mass motion model is specifically used to describe the motion state of the dynamic obstacle vehicle;

[0137] Then, a feedback controller is designed based on the random variables of the lateral and longitudinal speeds of the dynamic obstacle vehicle;

[0138] Define chance constraints based on the uncertainty caused by the random speed variation of dynamic obstacle vehicles;

[0139] Based on the lateral position and longitudinal position errors of the dynamic obstacle vehicle, a probability distribution expression is constructed;

[0140] Then convert the probability expression into a certain confidence region;

[0141] Finally, the probabilistic collision area around the vehicle is given by combining the length and width of the dynamic obstacle vehicle and the additional safety margin;

[0142] The feedback controller is:

[0143] u obj,k =K(ξ obj,k -ξ obj,ref,k )

[0144] Control input u obj,k represents the random variation of the lateral and longitudinal vehicle speeds, ξ obj,ref,k represents the reference state of the dynamic obstacle vehicle provided by the prediction module, ξ obj,k Indicates the current state of the dynamic obstacle vehicle;

[0145] Chance constraints are defined as follows:

[0146] Pr(ξ k ∈Ξ' safe,k (ω(u obj,k )))≥p

[0147] System state vector ξ k In the collision-free safety zone Ξ′ safe,k The probability of being greater than p;

[0148] In addition, the probability distribution of the lateral position error and longitudinal position error of the surrounding dynamic obstacle vehicle follows the normal distribution:

[0149]

[0150]

[0151] e x,k and e y,k Respectively represent the variance of the lateral and longitudinal position errors, and Respectively represent the variance of the lateral position and longitudinal position errors, μ x and μ y represents the mean of the horizontal and vertical positions;

[0152] According to the random variable sum of squares subject to Φ with 2 degrees of freedom 2 Distribution, the confidence region satisfies the interior area of ​​the elliptical contour:

[0153]

[0154] is its p quantile;

[0155] The lengths of the semi-major and semi-minor axes of the ellipse boundary corresponding to the confidence region are:

[0156]

[0157]

[0158] and Respectively represent the lengths of the semi-major axis and semi-minor axis of the ellipse;

[0159] Finally, based on the length and width of the vehicle and the confidence area, the probability collision rectangle around the vehicle is:

[0160]

[0161]

[0162] L rec,k and W rec,k They represent the length and width of the collision rectangle respectively, and the length of the vehicle is l obj and width w obj , ε safe is an additional safety margin.

[0163] In step S2, the ego vehicle prediction model is:

[0164]

[0165] in, Defined as the state vector of the system, s k ,l k , v k are the longitudinal position, lateral position, heading angle and speed of the vehicle, u k =[δ f,k ,a k ] T is the control input vector of the system, δ f,k ,a k are the steering wheel angles respectively.

[0166] In step S3, the specific contents are:

[0167] 31) The lateral distance of the vehicle's current position from the reference line at the kth moment is taken as the optimization target:

[0168] J l,k =q l (l k -lref ) 2

[0169] Among them, q l represents the weight of the lateral distance target term in the initial solution generation algorithm, l ref =0 indicates the expected value of the horizontal distance from the reference line;

[0170] 32) The vehicle’s heading angle deviation from the reference line at the kth moment is taken as the optimization target:

[0171]

[0172] in, Represents the weight of the heading angle target term in the initial solution generation algorithm;

[0173] 33) The error between the vehicle and the desired speed at the kth moment is taken as the optimization target:

[0174] J v,k =q v (v k -v ref,k ) 2

[0175] Among them, q v represents the weight of the vehicle speed target item in the initial solution generation algorithm, v ref,k represents the expected value of the vehicle speed at the kth moment;

[0176] 34) Take the steering wheel angle and acceleration of the vehicle at the kth moment as the optimization target:

[0177]

[0178]

[0179] in, and r a Respectively represent the weights of the steering wheel angle and acceleration objective items;

[0180] 35) Establish vehicle collision safety constraints:

[0181] (s ego,max <s obj,min )∪(s ego,min >s obj,max )∪(l ego,max <l obj,min )∪(l ego,min >l obj,max )

[0182] Among them, the maximum value of the horizontal coordinate of the vehicle is s ego,max , the minimum value is s ego,min; The maximum value of the ordinate of the vehicle is l ego,min , the minimum value is l ego,min ; The maximum value of the horizontal coordinate of the obstacle vehicle is s obj,max , the minimum value is s obj,min ; The maximum value of the vertical coordinate of the obstacle vehicle is l obj,max , the minimum value is l obj,min ;

[0183] 36) Establish the vehicle heading angle deviation constraint at the kth moment:

[0184]

[0185] in, is the corresponding heading angle deviation limit;

[0186] 37) Establish vehicle speed constraints:

[0187] 0≤v k ≤v limit

[0188] Among them, v limit is the maximum speed limit of the vehicle;

[0189] 38) Establish vehicle steering wheel angle constraints:

[0190] |δ f,k |≤δ f,limit

[0191] Among them, δ f,limit is the maximum steering wheel angle;

[0192] 39) Establish acceleration constraints:

[0193] a dec,limit ≤a k ≤a acc,limit

[0194] Among them, a acc,limit is the maximum acceleration of the vehicle, a dec,limit is the maximum deceleration.

[0195] In step S4, the specific contents are:

[0196] 41) Determine whether the lane boundary and collision safety constraints are non-convex:

[0197] (s obj,left ≤s ego ≤s lane,left )∪(s obj,right ≤s ego ≤s lane,right )

[0198] 42) Determine whether the upper bound of the safety constraint is less than the lower bound:

[0199] s lane,left <s obj,left

[0200] s obj,right <s lane,right

[0201] Among them, s lane,left and lane,right Represent the left and right boundaries of the lane, s obj,left and obj,right They represent the left and right boundaries of the collision safety area determined by the obstacle vehicle.

[0202] In step S5, the specific contents are:

[0203] 51) If the constraint condition is non-convex, follow the principle of overtaking on the left and modify the heavy collision safety constraint to:

[0204] s obj,left ≤s ego ≤s lane,left

[0205] Among them, s lane,left and obj,left They represent the left boundary of the lane and the left boundary of the collision safety zone determined by the obstacle vehicle respectively;

[0206] 52) If the upper bound of the constraint is smaller than the lower bound, the spatial position constraint is reconstructed to be located behind the dynamic obstacle vehicle:

[0207] s ego ≤s obj,rear

[0208] Modify the desired lateral distance in the optimization objective:

[0209]

[0210] Modify the expected speed in the optimization objective to:

[0211]

[0212] Among them, s obj,rear and obj,frobt They represent the rear and front boundaries of the collision safety zone determined by the obstacle vehicle, respectively, and v ref represents the desired reference speed, v left and v right Respectively represent the speed of the dynamic obstacle vehicle in front of the left and right lanes, w lane Indicates lane width, l lb represents the lower bound of the reconstructed lateral position constraint;

[0213] And reconstruct the spatial position constraint as the front of the dynamic obstacle vehicle. At this time, the expected state value in the optimization objective does not need to be reconstructed:

[0214] s ego ≥s obj,front

[0215] Among them, s obj,rear and obj,frobt They represent the rear and front boundaries of the collision safety zone determined by the obstacle vehicle, respectively;

[0216] 53) When the upper bound of the safety constraint is smaller than the lower bound, the two reconstructed constraint conditions and optimization target solutions located in front of and behind the dynamic obstacle vehicle are substituted into the nonlinear optimal control problem to solve the initial solution of the planning control and select the solution that minimizes the optimization target value.

[0217] In step S6, the specific contents are:

[0218] 61) The relationship between the state vector, control input vector and prediction step length is simplified to a function f d , which constitutes the equation part of the constraint:

[0219]

[0220]

[0221] 62) Add up the optimization objective items at all times in the prediction time domain to obtain the final optimization objective function:

[0222]

[0223] The prediction time domain uses a variable prediction step method, the total control time domain is 10s, the precise control time domain is 5s, and the prediction step length of the first 5.0s is T S = 0.2s, the number of prediction steps is N S =25; the prediction step length for the next 5.0s is T L = 1.0s, the number of prediction steps is N S =25; total number of prediction steps N total =N S +N L =30;

[0224] 63) Combining the optimization objective with the constraints, including the chance constraint, the objective function and the constraints are expressed in the form of vectors and matrices, and the optimal control problem based on SMPC is obtained:

[0225]

[0226]

[0227]

[0228]

[0229]

[0230]

[0231] Among them, Ξ and U represent the feasible sets of system state and control input respectively, Q and R are the semi-positive definite weight matrices of system state vector and control input vector respectively, which are defined as follows:

[0232]

[0233] Optionally, the specific content of step 8) is:

[0234] In step S7, if the nonlinear optimal control problem is abandoned, the reference line provided by the upstream navigation module is directly output.

[0235] This embodiment applies the above technical solution, such as Figure 2 As shown:

[0236] Step 1: Based on the information obtained from multiple upstream modules, the point mass motion model is used to describe the motion state of the surrounding dynamic obstacle vehicles. Then, the random changes of the lateral and longitudinal speeds are defined as feedback controllers to obtain the predicted error of the dynamic obstacle vehicles. The two random variables of lateral error and longitudinal error obey the standard normal distribution. The sum of the squares of the two random variables is obtained to obey the χ2 distribution with 2 degrees of freedom. 2 distribution, the prediction error remains below the probability p, which can be expressed as the inner area of ​​the centroid area in the ellipse contour line, and the semi-major axis and semi-minor axis of the ellipse boundary are obtained. Combined with the length and width of the dynamic obstacle vehicle and the defined safety margin, the probability collision rectangular area around the dynamic obstacle vehicle can be obtained, such as Figure 3 Defining this region takes into account the uncertainty of the prediction, and when the constraints are established later, the uncertainty of the prediction can be incorporated into the establishment of the constraints.

[0237] Step 2: Combine the vehicle kinematic model in the Cartesian coordinate system to give the vehicle dynamics model in the Frenet coordinate system:

[0238]

[0239] Step 3: Initial establishment of optimization objectives and constraints, including:

[0240] a) The lateral distance that the vehicle's current position deviates from the reference line at the kth moment;

[0241] b) The heading angle deviation of the vehicle relative to the reference line at the kth moment;

[0242] c) the error between the vehicle speed and the expected speed at the kth moment;

[0243] d) The steering wheel angle and acceleration of the vehicle at the kth moment are taken as optimization targets;

[0244] e) Use fast collision detection based on coordinate axis bounding boxes to establish vehicle collision safety constraints, such as Figure 4 As shown;

[0245] f) define the corresponding heading angle deviation limit and establish the vehicle heading angle deviation constraint at the kth moment;

[0246] g) Define the maximum speed limit for vehicles and establish vehicle speed constraints;

[0247] h) Define the maximum steering wheel angle limit for the driving direction and establish the vehicle steering wheel angle constraint;

[0248] i) Define the maximum acceleration and deceleration of the vehicle and establish acceleration constraints.

[0249] Step 4: Then make the following judgments on the initially constructed constraints:

[0250] a) Judgment 1: Whether the constraint set of lane boundary and collision safety is a non-convex set, if so, proceed to step 5, otherwise skip step 5 and go directly to step 6;

[0251] b) Judgment 2: Whether the upper bound of the constraint set of lane boundary and collision safety is smaller than the lower bound, if so, proceed to step 5, otherwise skip step 5 and go directly to step 6.

[0252] Step 5: Restructure the constraints and optimization objectives

[0253] Step 51: If the constraint condition is non-convex, follow the principle of overtaking on the left, reconstruct the lane line constraint to only retain the constraint of the left lane, replace the initial constraint condition with the reconstructed constraint condition, and go to step 6;

[0254] Step 52: If the upper bound of the constraint condition is smaller than the lower bound, indicating that there is no collision-free zone, the spatial position constraint is changed to two types of constraints related to the longitudinal position of the ego vehicle being in front of or behind the obstacle vehicle. Among them, the constraint behind the obstacle vehicle requires the optimization target to be reconstructed, specifically, the dynamic obstacle vehicle with the highest speed within the safe distance in front of the vehicle is selected as the following vehicle target and constructed as the optimization target, such as Figure 5a and Figure 5b As shown in the figure, the corresponding expected lateral distance is also reconstructed as the center line of the lane where the following target is located. The expected lateral distance is reconstructed as follows: Figure 6 As shown. Reconstruct the constraints and the desired state in the optimization objective to replace the initial constraints and the initial desired state, and go to step 6;

[0255] If the vehicle is in front of the obstacle, there is no need to reconstruct the optimization target. Instead, the reconstructed constraints will replace the initially constructed constraints and go to step 6.

[0256] Step 53: Substitute the two reconstructed constraint conditions and optimization target solutions located in front of and behind the dynamic obstacle vehicle into the nonlinear optimal control problem to solve the initial solution of the planning control and select a solution with a smaller objective function value.

[0257] Step 6: Construct a nonlinear optimal control problem by combining the optimization objective, constraints, and prediction model. The specific steps include:

[0258] Step 61: Perform discrete time sampling of the system with a sampling time of T, and transform the vehicle kinematic model into a state equation in discrete space:

[0259]

[0260] Among them, the definition is the state vector of the system, u k =[δ f,k ,a k ] T is the control input vector of the system;

[0261] Step 62: Simplify the relationship between the state vector, the control input vector and the prediction step length into a function f d , giving the equality part of the constraint condition,:

[0262]

[0263]

[0264] Step 63: Add up the optimization objective items at all times in the prediction time domain to obtain the final optimization objective function:

[0265]

[0266] The prediction time domain uses a variable prediction step method, the total control time domain is 10s, the precise control time domain is 5s, and the prediction step length of the first 5.0s is T S = 0.2s, the number of prediction steps is N S =25; the prediction step length for the next 5.0s is T L = 1.0s, the number of prediction steps is N S =25; total number of prediction steps N total =NS +N L =30;

[0267] Step 64: Combining the optimization objective with the constraints, including the chance constraint, the objective function and the constraints are expressed in the form of vectors and matrices to obtain the optimal control problem based on SMPC:

[0268]

[0269]

[0270]

[0271]

[0272]

[0273]

[0274] Among them, Ξ and U represent the feasible sets of system state and control input respectively, Q and R are the semi-positive definite weight matrices of system state vector and control input vector respectively, which are defined as follows:

[0275]

[0276] Step 7: Solve the nonlinear optimal control problem and obtain the initial state of planning control;

[0277] Step 8: If the solution fails, give up solving the nonlinear optimal control problem and directly output the reference line provided by the upstream navigation module.

[0278] To sum up, this technical solution solves the problem that the existing integrated algorithm of local trajectory planning and tracking control only considers deterministic information but does not systematically model and process uncertain information. This technical solution uses a variable prediction step size to balance the accuracy and computational efficiency of the solution, and can provide a guiding initial value trajectory, so that the unmanned vehicle can improve the solution success rate and solution efficiency when executing the planning and control integrated algorithm solution, thereby obtaining better control quantity results, which are then sent to the unmanned vehicle for execution.

Claims

1. An integrated method for unmanned lane change planning and control, characterized in that: The following steps are involved: S1. Obtain upstream information, define the predicted lateral error and longitudinal error as two random variables according to the motion state of the surrounding dynamic obstacle vehicles and their predicted trajectory information, obtain the probability expression of the predicted error, and convert it into a deterministic constraint, and establish the probabilistic collision area in combination with the vehicle length, width and safety margin; S2, constructing a vehicle dynamics model in the Frenet coordinate system as a self-vehicle prediction model; S3, construct the initial optimization objectives and constraints based on the upstream information and the self-vehicle prediction model; S4, judging the current constraint condition, if it is non-convex or the upper bound is smaller than the lower bound, executing step S5; Otherwise, execute step S6; S5. According to the left overtaking principle, the lane line constraint is modified, the spatial position constraint is changed to a constraint that the longitudinal position of the vehicle is located in front of or behind the obstacle vehicle, and the expected lateral distance is changed to reconstruct the constraint conditions and optimization objectives, and then step S6 is executed; S6. Obtain constraint equations based on the relationship between the state vector, the control input vector, and the prediction step length, input all optimization target items at the time in the prediction time domain, obtain the final optimization target function, combine the ego-vehicle prediction model with the chance constraint, and construct a nonlinear optimal control problem; S7, solving the nonlinear optimal control problem and obtaining the initial state of planning control. If the solution fails, the reconstructed optimization target is used as the initial state of planning control; S8, realize vehicle tracking control according to the planned control initial state; The step S1 specifically includes the following steps: S11, obtaining upstream information, wherein the upstream information includes reference lines, positioning, perception, and trajectory prediction information; S12. Use point mass motion model to describe the motion state of the vehicle around dynamic obstacles; S13, designing a feedback controller according to the random variables of the lateral speed and longitudinal speed of the dynamic obstacle vehicle; S14. Define chance constraints based on the uncertainty caused by the random speed variation of dynamic obstacle vehicles; S15, constructing a probability distribution expression based on the lateral position and longitudinal position errors of the dynamic obstacle vehicle; S16, converting the probability distribution expression into a certain confidence region; S17, establishing a probabilistic collision area around the vehicle based on the length and width of the dynamic obstacle vehicle and an additional safety margin; The feedback controller in step S13 is specifically: you obj，k =K(ξ obj，k -x obj，ref，k ) Among them, the control input u obj，k represents the random variation of the lateral and longitudinal vehicle speeds, ξ obj，ref，k represents the reference state of the dynamic obstacle vehicle provided by the prediction module, ξ obj，k Indicates the current state of the dynamic obstacle vehicle; The opportunity constraints in step S14 are specifically: Pr(ξ k ∈Ξ′ safe，k (ω(u obj，k )))≥p That is, the system state vector ξ k In the collision-free safety zone Ξ′ safe，k The probability of being greater than p; The probability distribution of the lateral position error and the longitudinal position error of the dynamic obstacle vehicle in step S15 obeys the normal distribution: Among them, e x，k and e y，k are the variances of the lateral and longitudinal position errors, respectively. and are the variances of the lateral and longitudinal position errors, μ x and μ y are the means of the horizontal and vertical positions, respectively; The specific process of step S16 is as follows: According to the random variable sum of squares subject to χ with 2 degrees of freedom 2 Distribution, the confidence region satisfies the interior area of ​​the elliptical contour: in, is its p quantile; The lengths of the semi-major axis and semi-minor axis of the ellipse boundary corresponding to the confidence region are: in, and are the lengths of the semi-major and semi-minor axes of the ellipse respectively; The specific process of step S17 is as follows: According to the length and width of the vehicle and the confidence area, the probability collision rectangle around the vehicle is established as: Among them, L rec，k and W rec，k are the length and width of the collision rectangle, l obj and w obj are the length and width of the vehicle, ε safe is an additional safety margin; The vehicle prediction model in step S2 is specifically: in, is the state vector of the system, s k , l k , v k are the longitudinal position, lateral position, heading angle and speed of the vehicle, u k =[δ f，k , a k ] T is the control input vector of the system, δ f，k , a k are the steering wheel angles respectively; The step S3 specifically comprises the following steps: S31. Taking the lateral distance of the vehicle's current position from the reference line at the kth moment as the optimization target: J l，k =q l (l k -l ref ) 2 Among them, q l is the weight of the lateral distance objective term in the initial solution generation algorithm, l ref =0 indicates the expected value of the horizontal distance from the reference line; S32, taking the heading angle deviation of the vehicle relative to the reference line at the kth moment as the optimization target: in, The weight of the heading angle target term in the initial solution generation algorithm; S33, taking the error between the vehicle speed and the desired speed at the kth moment as the optimization target: J v，k =q v (v k -v ref，k ) 2 Among them, q v is the weight of the vehicle speed target item in the initial solution generation algorithm, v ref，k is the expected value of the vehicle speed at the kth moment; S34, taking the steering wheel angle and acceleration of the vehicle at the kth moment as the optimization target: in, and r a are the weights of the steering wheel angle and acceleration objective items respectively; S35. Establishing vehicle collision safety constraints: (s ego，max <s obj，min )∪(s ego，min >s obj，max )∪(l ego，max <l obj，min )∪(l ego，min >l obj，max ) Among them, s ego，max and ego，min are the maximum and minimum values ​​of the horizontal coordinate of the vehicle, respectively, ego，max and l ego，min The maximum and minimum values ​​of the ordinate of the vehicle are, respectively, s obj,max and obj,min are the maximum and minimum values ​​of the horizontal coordinate of the obstacle vehicle, respectively, obj,max and l obj,min are the maximum and minimum values ​​of the ordinate of the obstacle vehicle respectively; S36. Establish the vehicle heading angle deviation constraint at the kth moment: in, is the corresponding heading angle deviation limit; S37. Establishing vehicle speed constraints: 0≤v k ≤v limit Among them, v limit is the maximum speed limit of the vehicle; S38. Establishing vehicle steering wheel angle constraints: |d f，k |≤δ f，limit Among them, δ f，limit is the maximum steering wheel angle; S39. Establish acceleration constraints: a dec,limit ≤a k ≤a acc,limit Among them, a acc，limit and a dec，limit are the maximum acceleration and maximum deceleration of the vehicle respectively; The step S4 specifically comprises the following steps: S41. First, determine whether the lane boundary and collision safety constraints are non-convex: (s obj，left ≤s ego ≤s lane，left )∪(s obj,right ≤s ego ≤s lane,right ) If the judgment is yes, execute step S5, otherwise execute step S42; S42. Determine whether the upper bound of the security constraint is less than the lower bound: s lane,left <s obj，left s obj，right <s lane，right If the judgment is yes, execute step S5, otherwise execute step S6; Among them, s lane，left and lane，right are the left and right boundaries of the lane, s obj,left and obj，right are the left and right boundaries of the collision safety zone determined by the obstacle vehicle, respectively; The specific process of reconstructing the constraint conditions and optimization objectives in step S5 is as follows: S51. If the constraint condition is non-convex, follow the principle of overtaking on the left and reconstruct the collision safety constraint as follows: s obj，left ≤s ego ≤s lane，left Among them, s lane，left and obj,left They are respectively the left boundary of the lane and the left boundary of the collision safety zone determined by the obstacle vehicle; 52) If the constraint condition is that the upper bound is smaller than the lower bound, then on the one hand, the spatial position constraint is reconstructed to be located behind the dynamic obstacle vehicle: s ego ≤s obj,rear Modify the desired lateral distance in the optimization objective: Modify the expected speed in the optimization objective to: Among them, s obj,rear and obj，front are the rear and front boundaries of the collision safety zone determined by the obstacle vehicle, respectively, v ref is the desired reference speed, v left and v right are the speeds of the dynamic obstacle vehicles in front of the left and right lanes, respectively, and w lane is the lane width, l lb Constrain lower bounds for the reconstructed lateral position; On the other hand, the spatial position constraint is reconstructed as a dynamic obstacle in front of the vehicle: s ego ≥s obj，front Among them, s obj,rear and obj，front are the rear boundary and the front boundary of the collision safety area determined by the obstacle vehicle, respectively. At this time, the expected state value in the optimization objective does not need to be reconstructed; The step S6 specifically comprises the following steps: S61, simplify the relationship between the state vector, the control input vector and the prediction step length into a function f d , which constitutes the equation part of the constraint: S62, adding up the optimization objective items at all times in the prediction time domain to obtain the final optimization objective function: The prediction time domain uses a variable prediction step method, the total control time domain is 10s, the precise control time domain is 5s, and the prediction step length of the first 5.0s is T S = 0.2s, the number of prediction steps is N S =25; the prediction step length for the next 5.0s is T L = 1.0s, the number of prediction steps is N S =25; total number of prediction steps N total =N S +N L =30; S63. Combining the optimization goal with the constraints, including the chance constraint, the objective function and the constraints are expressed in the form of vectors and matrices to obtain the optimal control problem based on SMPC: Among them, Ξ and U are the feasible sets of system state and control input, respectively, Q and R are the semi-positive definite weight matrices of system state vector and control input vector, respectively.

Images