Comprehensive maneuvering motion planning method and system for sudden lane changing of autonomous vehicle

Abstract

The invention discloses a comprehensive maneuvering motion planning method for sudden lane changing of an automatic driving vehicle, and relates to the technical field of automatic driving vehicles. The method comprises the following steps: receiving vehicle driving state information, wherein the driving state information comprises the positions, speeds, accelerations, course angles and yaw velocities of a vehicle and surrounding vehicles; constructing an MOBIL lane changing decision model, making a lane changing decision by using the model, judging whether a lane changing operation is carried out or not, and if so, carrying out calculation to obtain a lane changing movement track taking the minimum lane changing time and the minimum lane changing distance as optimization targets on the basis of a quintic polynomial form; and establishing a minimum safe collision-free circle based on a vehicle-vehicle geometrical relationship. According to the lane changing trajectory quintic polynomial optimization method based on the steady-state steering principle, the motion planning trajectory capable of achieving stable tracking of the vehicle under the emergency lane changing condition and avoiding the sideslip risk can be generated, and the method has good application prospects for autonomous collision avoidance and safe driving of the vehicle in the emergency scene of the dynamic environment.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving vehicles, and in particular to a method and system for comprehensive motion planning of sudden lane change maneuvers for autonomous driving vehicles. Background Art

[0002] The design of autonomous driving systems faces huge challenges, and safety becomes a key issue and the main focus of development. Although existing advanced driver assistance systems provide some support under regular driving conditions, fully autonomous driving systems must handle more complex and dangerous situations, especially those involving dynamic environmental uncertainties. In emergency situations such as sudden queue cutting, the system must make decisions quickly in a very short time, replan the trajectory, and precisely control steering, braking, and acceleration. Therefore, to achieve a safe and smooth transition in such high-risk situations, it is necessary to carefully integrate the control layer response during the planning process to ensure the safety of the vehicle throughout the collision avoidance maneuver.

[0003] For general scenarios, lane change collision avoidance can be regarded as an avoidance problem for autonomous systems. When the obstacle is stationary, the problem to be considered becomes a simple path planning problem. Although common sampling-based path planning algorithms such as graph search, rapid exploration random trees, and probabilistic road graphs have shown excellent performance in path planning in certain scenarios, the trajectory of point set planning often has deficiencies in curvature smoothness and is difficult to meet the requirements of precise vehicle control; in dynamic scenarios, the difficulty of local motion planning increases significantly, because the vehicle needs to consider not only static obstacles, but also the system dynamics characteristics of surrounding mobile traffic participants. In these cases, path planning is no longer just about finding a feasible path, but requires dynamically solving the avoidance-reaching task so that the controlled vehicle can avoid moving obstacles while maintaining safety. However, the controller designed in the prior art is difficult to effectively consider the uncertainty of surrounding unpredictable vehicles. To this end, we propose a comprehensive motion planning method and system for sudden lane change maneuvers for autonomous driving vehicles. Summary of the invention

[0004] The purpose of the present invention is to provide a method and system for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, so as to achieve autonomous collision avoidance in a complex dynamic environment.

[0005] According to a first aspect of the present invention, in order to achieve the above-mentioned purpose, the present invention provides the following technical solution: a method for comprehensive motion planning of sudden lane change maneuvers of an autonomous driving vehicle, comprising the following steps:

[0006] Receive vehicle driving status information, which includes the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles;

[0007] Construct a MOBIL lane-changing decision model and use it to make lane-changing decisions to determine whether to change lanes. If so, calculate the lane-changing trajectory with the minimum lane-changing time and distance as the optimization target based on a quintic polynomial form.

[0008] The minimum safe collision-free circle is established based on the geometric relationship between vehicles, and a high-order control barrier function is used to transform the collision-free constraint into the vehicle driving constraint in the model prediction algorithm. The vehicle longitudinal acceleration and wheel steering angle are solved in the rolling optimization process.

[0009] The obtained vehicle longitudinal acceleration and wheel steering angle are input into the actuator control module of the vehicle chassis, and the generated longitudinal acceleration command and wheel steering angle command are tracked respectively to complete the vehicle lane changing operation.

[0010] Furthermore, the driving state information of the vehicle is obtained through a vehicle speed sensor and CAN protocol communication, and the driving state information of surrounding vehicles is obtained through a laser radar and a millimeter wave radar.

[0011] Furthermore, the model is used to make a lane-changing decision to determine whether to perform a lane-changing operation. The specific judgment principles are as follows:

[0012] According to the status information of the received vehicle and surrounding vehicles, the acceleration gain of the vehicle after changing lanes under the current driving environment is calculated, plus the acceleration gain of the vehicle behind the target lane and the acceleration gain of the vehicle behind the lane multiplied by the politeness coefficient. If the value is greater than the set threshold, the lane change is allowed, otherwise it is not allowed.

[0013] Furthermore, based on the fifth-order polynomial form, the lane-changing trajectory with the minimum lane-changing time and lane-changing distance as the optimization target is calculated, as follows:

[0014] (1) If the current decision under the MOBIL model is to change lanes, then based on the longitudinal and lateral positions (X0, Y0) and speed (v x0 ,v y0 ), acceleration information (a x0 ,a y0 ), and assuming t f The end position (X f ,Y f )、speed(v xf ,v yf ) and acceleration (a xf ,a yf ) information, we can obtain a set of quintic polynomial equations, as follows:

[0015] X(t)=a0+a1t+a2t 2 +a3t 3+a4t 4 +a5t 5

[0016] Y(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0017]

[0018] In the formula, a i ,b i (i=0,1,…,5) represents the coefficients of the quintic polynomial to be solved. The coefficients are obtained by substituting the initial and end point position, velocity, and acceleration information to obtain the lane change trajectory (X(t), Y(t)) in the global coordinates.

[0019] (2) Assume that the end point information at the end of the lane change is the vehicle steady-state center of mass sideslip angle β under steady-state steering. ss The specific formula of the steady-state center of mass sideslip angle is determined by the road curvature formed by the fifth order:

[0020]

[0021] In the formula, ψ ss represents the steady-state yaw angle under steady-state steering, l f ,l r are the front and rear axle wheelbases, m represents the vehicle mass, C αr represents the cornering stiffness of the rear axle, K represents the road curvature, and v x Indicates the longitudinal speed of the vehicle;

[0022] (3) According to the geometric relationship between the heading angle θ, the sideslip angle β, and the yaw angle ψ, the steady-state yaw rate can be obtained as follows:

[0023]

[0024] In the formula, X, Y represent the coordinates of the vehicle in the global coordinate system, V represents the velocity direction at the center of mass of the vehicle body, and l f ,l r are the front and rear axle wheelbases, m represents the vehicle mass, C αr represents the cornering stiffness of the rear axle, and K represents the road curvature;

[0025] (4) The stability of the vehicle in emergency steering is improved by limiting the sideslip angle and yaw rate. The sideslip angle constraint and yaw rate constraint are expressed as:

[0026] |β des|≤arctan0.02μg

[0027]

[0028] Where μ is the road adhesion coefficient, g is the acceleration of gravity;

[0029] (5) The motion trajectory is optimized so that it meets the specified starting and ending conditions and the lateral stability constraints of the vehicle, while minimizing the lane change time and distance. The optimization objective can be described as:

[0030]

[0031] The constraint expression is:

[0032]

[0033] In the formula, J path represents the optimization target, X, Y represent the optimized lane change trajectory coordinates, t f represents the lane changing time, μ is the road adhesion coefficient, g is the acceleration of gravity, X0, Y0 represent the coordinates of the starting point of the vehicle lane changing, Indicates the lateral and longitudinal acceleration at the starting point of lane change, Indicates the lateral and longitudinal speeds at the starting point of lane change. Indicates the lateral and longitudinal acceleration at the end of lane change, represents the lateral and longitudinal speeds at the end of the lane change, V represents the speed at the center of mass of the vehicle, K represents the road curvature, and β represents the sideslip angle of the center of mass;

[0034] (6) The above constrained optimization objectives are solved by the QP solver to obtain a motion trajectory that satisfies the dynamic constraints of minimum lane changing time, minimum lane changing distance, and stable steering of the vehicle.

[0035] Furthermore, the model predictive control algorithm is used to track the quintic polynomial motion trajectory generated by the control, and the optimal tracking control result is obtained, as follows:

[0036] (1) Set the control input to the vehicle's longitudinal acceleration a x and the steering angle δ of the wheel, using the three-degree-of-freedom vehicle dynamics model, the expression can be written as:

[0037]

[0038] In the formula, I z represents the moment of inertia of the vehicle around the Z axis, C αf Indicates the front axle cornering stiffness;

[0039] (2) Based on the vehicle dynamics model of the above model predictive control, in the prediction time domain N pThe optimization goal of the design is optimized by rolling calculation of vehicle dynamics. The optimization goal is expressed as:

[0040]

[0041] Where x represents the selected tracking state, x ref represents a given reference state, P, Q, R are weight matrices, ζ, ω are relaxation factors, ρ1, ρ2 are their weight coefficients, J represents the performance index, u is the control quantity, and T refers to the matrix torque;

[0042] (3) According to the constraints expressed for the tracking task, the constraint function of the high-order system is designed, which can be expressed as:

[0043]

[0044]

[0045] Where V L represents the Lyapunov function, h(x) represents the constraint function, k1, k2 are weight coefficients, and ω is the relaxation factor;

[0046] Furthermore, the minimum safe collision-free circle constraint is established as follows:

[0047] (1) Assume that the vehicle consists of a vehicle with a length of l veh , width is w veh If it is contained in a rectangle, two circles are set according to geometric principles to represent the distance at which no collision occurs;

[0048] (2) Assume that between any two vehicles, a circle of any vehicle is tangent to at least the circle of the other vehicle and is tangent to the rectangle. Based on geometric principles, the radius r of the circle can be calculated and expressed as:

[0049]

[0050] Furthermore, the control output of the model predictive control is constrained based on the high-order control barrier function, so that the vehicle gradually converges to the reference trajectory during the rolling solution, as follows:

[0051] (1) The design is based on the control Lyapunov function constraint, which is expressed as:

[0052] V L (x)=||YY ref || 2

[0053] In the formula, Y represents the horizontal distance, Y ref Indicates the reference lateral distance;

[0054] (2) The corresponding Lie derivative form of the constraint is specifically expressed as:

[0055] L f V L (x) = 2(v y cosψ+v x sinψ)(YY ref )

[0056]

[0057] In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the lateral stiffness of the front and rear axles, and m represents the vehicle mass.

[0058] Furthermore, the control output of the model predictive control is constrained based on the high-order control barrier function, so that the vehicles can meet the non-collision constraint between vehicles during the rolling solution, as follows:

[0059] (1) Design a vehicle-vehicle constraint based on the control barrier function, which is expressed as:

[0060] h v1 (x) = (X Of -x of ) 2 +(Y Of -y of ) 2 -4r2≥0

[0061] h v2 (x) = (X Of -x or ) 2 +(Y Of -y or ) 2 -4r2≥0

[0062] h v3 (x) = (X Of -x of ) 2 +(Y Or -y of ) 2 -4r2≥0

[0063] h v4 (x) = (X Or -x or ) 2 +(Y Or -yor ) 2 -4r 2 ≥0

[0064] In the formula, They represent the center positions of the two safety circles in front and behind the vehicle, Respectively represent the center positions of the two safety circles in front and behind other vehicles;

[0065] (2) The corresponding Lie derivative form of the constraint is specifically expressed as:

[0066] L f h vi (x) = 2(v x cosψ-v y sinψ)(X O -x o )+2(v x sinψ+v y cosψ)(Y O -y o )

[0067]

[0068] L g1 L f h vi (x) = 2cosψ(X O -x O )+2sinψ(Y O -y O )

[0069]

[0070] In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the lateral stiffness of the front and rear axles, and m represents the vehicle mass.

[0071] Furthermore, the control output of the model predictive control is constrained based on the high-order control barrier function, so that the vehicle is guaranteed to meet the lane keeping constraint between the vehicle and the road during the rolling solution, as follows:

[0072] (1) Design a vehicle-road constraint based on the control barrier function, which is expressed as:

[0073] h r1 (x) = d lb -Y≥0

[0074] h r2 (x) = Yd ub ≥0

[0075] Where, d lb ,d ub represents the upper and lower boundaries of the road, and Y represents the lateral position of the vehicle;

[0076] (2) The corresponding Lie derivative form of the constraint is specifically expressed as:

[0077] L f h ri (x) = (-1) i (v x sinψ+v y cosψ)

[0078]

[0079] L g1 L f h ri (x) = (-1) i sinψ

[0080]

[0081] In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C ar represents the lateral stiffness of the front and rear axles, and m represents the vehicle mass.

[0082] According to a second aspect of the present invention, the present invention provides a system for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, which is used to implement the above-mentioned method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, comprising:

[0083] A receiving module is used to receive vehicle driving status information, which includes the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles;

[0084] The lane change decision and motion planning module is used to build a MOBIL lane change decision model, use the model to make lane change decisions, and determine whether to change lanes. If so, the lane change motion trajectory with the minimum lane change time and distance as the optimization target is calculated based on the fifth-order polynomial form;

[0085] The model predictive control module is used to establish the minimum safe collision-free circle based on the geometric relationship between vehicles, and use high-order control barrier functions to convert the collision-free constraints into vehicle driving constraints in the model predictive algorithm, and solve the vehicle longitudinal acceleration and wheel steering angle in the rolling optimization process;

[0086] The lane changing control module is used to input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, and respectively track the generated longitudinal acceleration command and wheel steering angle command to complete the vehicle lane changing operation.

[0087] The present invention has at least the following beneficial effects:

[0088] 1. The present invention uses a quintic polynomial optimization method for lane change trajectories based on the steady-state steering principle to generate a motion planning trajectory for vehicles to achieve stable tracking and no skidding risk in emergency lane change situations. It has good application prospects for autonomous collision avoidance and safe driving of vehicles in emergency scenarios in dynamic environments.

[0089] 2. The present invention designs a minimum safety circle for vehicle collision constraints based on geometric principles, which effectively characterizes the non-collision constraints of any vehicle.

[0090] 3. The present invention establishes a control barrier function based on the minimum safety circle constraint, and constructs a safety control constraint for lane change collision avoidance using the Li derivative of a high-order system, replacing the traditional MPC rolling optimization constraint to more accurately describe the vehicle-vehicle and vehicle-road constraint characteristics in a dynamic environment.

[0091] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] Figure 1 It is a schematic diagram of the process of the method of the present invention;

[0093] Figure 2 It is a calculation principle diagram of the high-order barrier function-model predictive control algorithm of the present invention;

[0094] Figure 3 A simulation effect diagram comparing the control effect of the present invention with that of the planning result based on a fixed lane changing time and a fixed lane changing distance;

[0095] Figure 4 This is a simulation effect diagram of the collision avoidance lane change planning and control algorithm of an embodiment of the present invention. DETAILED DESCRIPTION

[0096] The following will be combined with the drawings in the embodiments of the present disclosure to clearly and completely describe the technical solutions in the embodiments of the present disclosure. Obviously, the described embodiments are only part of the embodiments of the present disclosure, rather than all the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present disclosure.

[0097] See also Figure 1-Figure 4 , the present invention provides a technical solution: a comprehensive motion planning method for sudden lane change maneuvers of an autonomous driving vehicle, comprising the following steps:

[0098] S1. Receive vehicle driving status information, which includes the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles;

[0099] It should be noted that the vehicle information is obtained based on the vehicle speed sensor and CAN protocol communication, and the status information of other vehicles is obtained through laser radar, millimeter wave radar, etc., combined with positioning systems such as Beidou to accurately locate the vehicle in real time;

[0100] S2. Construct a MOBIL lane-changing decision model, use the model to make a lane-changing decision, and determine whether to perform a lane-changing operation. If so, calculate the lane-changing motion trajectory with the minimum lane-changing time and lane-changing distance as the optimization target based on the fifth-order polynomial form, as follows:

[0101] (S21) calculating the acceleration gain of the vehicle after changing lanes under the current driving environment state based on the received status information of the vehicle and surrounding vehicles, plus the sum of the acceleration gain of the vehicle behind the target lane and the acceleration gain of the vehicle behind the lane multiplied by the courtesy coefficient, and if the value is greater than a set threshold, the lane change is allowed, otherwise the lane change is not allowed;

[0102] (S22) If the current decision under the MOBIL model is that a lane change is required, based on the longitudinal and lateral positions (X0, Y0) and speed (v x0 ,v y0 ), acceleration information (a x0 ,a y0 ), and assuming t f The end position (X f ,Y f )、speed(v xf ,v yf ) and acceleration (a xf ,a yf ) information, we can get a quintic polynomial as follows:

[0103] X(t)=a0+a1t+a2t 2 +a3t3 +a4t 4 +a5t 5

[0104] Y(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0105]

[0106] In the formula, a i ,b i (i=0,1,…,5) represents the coefficients of the quintic polynomial to be solved. The coefficients are obtained by substituting the position, velocity, and acceleration information of the initial and final points, thereby solving the lane change trajectory (X(t), Y(t)) in the global coordinates.

[0107] (S23) The endpoint information at the end of the above-mentioned assumed lane change is the vehicle steady-state center of mass side slip angle β under steady-state steering. ss The specific formula of the steady-state center of mass sideslip angle is determined by the road curvature formed by the fifth order:

[0108]

[0109] In the formula, ψ ss represents the steady-state yaw angle under steady-state steering, l f ,l r are the front and rear axle wheelbases, m represents the vehicle mass, C αr represents the cornering stiffness of the rear axle, K represents the road curvature, and v x Indicates the longitudinal speed of the vehicle;

[0110] (S24) According to the geometric relationship between the heading angle θ, the sideslip angle β and the yaw angle ψ, the steady-state yaw rate can be obtained, which is as follows:

[0111]

[0112] In the formula, X, Y represent the coordinates of the vehicle in the global coordinate system, V represents the velocity direction at the center of mass of the vehicle body, and l f ,l r are the front and rear axle wheelbases, m represents the vehicle mass, C αr represents the cornering stiffness of the rear axle, and K represents the road curvature;

[0113] (S25) The stability of the vehicle under emergency steering is improved by limiting the sideslip angle and yaw rate, wherein the sideslip angle limit constraint and the yaw rate limit constraint are expressed as:

[0114] |βdes |≤arctan0.02μg

[0115]

[0116] Where μ is the road adhesion coefficient, g is the acceleration of gravity;

[0117] (S26) The motion trajectory is optimized so that it meets the specified starting and ending conditions and the lateral stability constraint of the vehicle, while minimizing the lane change time and distance. The optimization objective can be described as:

[0118]

[0119] The constraint expression is:

[0120]

[0121] In the formula, J path represents the optimization target, X, Y represent the optimized lane change trajectory coordinates, t f represents the lane changing time, μ is the road adhesion coefficient, g is the acceleration of gravity, X0, Y0 represent the coordinates of the starting point of the vehicle lane changing, Indicates the lateral and longitudinal acceleration at the starting point of lane change, Indicates the lateral and longitudinal speeds at the starting point of lane change. Indicates the lateral and longitudinal acceleration at the end of lane change. represents the lateral and longitudinal speeds at the end of the lane change, V represents the speed at the center of mass of the vehicle, K represents the road curvature, and β represents the sideslip angle of the center of mass;

[0122] (S27) solving the above constraint optimization objective by a QP solver to obtain a motion trajectory that satisfies the dynamic constraints of minimum lane changing time, minimum lane changing distance, and stable steering of the vehicle;

[0123] S3. Using the model predictive control algorithm, track the generated quintic polynomial motion trajectory, obtain the optimal tracking control result, and establish the minimum safe collision-free circle constraint, such as Figure 2 As shown, the details are as follows:

[0124] (S31) Using the model predictive control algorithm, the quintic polynomial motion trajectory generated by tracking control is obtained to obtain the optimal tracking control result, which is as follows:

[0125] (S31.1) Set the control input to the longitudinal acceleration a of the vehicle x and the steering angle δ of the wheel, using the three-degree-of-freedom vehicle dynamics model, the expression can be written as:

[0126]

[0127] In the formula, I z represents the moment of inertia of the vehicle around the Z axis, C αf Indicates the front axle cornering stiffness;

[0128] (S31.2) Based on the vehicle dynamics model described in (S31.1) of the model predictive control, in the prediction time domain N p The optimization goal of the design is optimized by rolling calculation of vehicle dynamics. The optimization goal is expressed as:

[0129]

[0130] In the formula, x represents the selected tracking state, x ref represents a given reference state, P, Q, R are weight matrices, ζ, ω are relaxation factors, ρ1, ρ2 are their weight coefficients, J represents the performance index, u is the control quantity, and T refers to the matrix torque;

[0131] (S31.3) According to the constraints expressed for the tracking task, the constraint function of the high-order system is designed, which can be expressed as:

[0132]

[0133] (S32) Establishing a minimum safe collision-free circle constraint, specifically as follows:

[0134] (S32.1) Assume that the vehicle consists of a vehicle with a length of l veh , width is w veh If it is contained in a rectangle, two circles are set according to geometric principles to represent the distance at which no collision occurs;

[0135] (S32.2) Assume that between any two vehicles, a circle of any vehicle is tangent to at least the circle of the other vehicle and is tangent to the rectangle. Based on the principle of geometry, the radius r of the circle can be obtained, which is expressed as:

[0136]

[0137] The safety constraint designed based on the minimum safety distance circle is that the predictive dynamics model used in the model predictive control is second-order differentiable, and its relative degree is 2;

[0138] S4. A high-order control barrier function is used to transform the collision-free constraint into the vehicle driving constraint in the model prediction algorithm, and the longitudinal acceleration of the vehicle and the wheel steering angle are obtained in the rolling optimization process, where the vehicle driving constraint includes the vehicle convergence constraint to the reference trajectory, the collision-free constraint between vehicles, and the lane keeping constraint between the vehicle and the road;

[0139] (S41) constraining the control output of the model predictive control based on the high-order control barrier function so that the vehicle gradually converges to the reference trajectory during the rolling solution, as follows:

[0140] (S41.1) The design is based on the control Lyapunov function constraint, which is expressed as:

[0141] V L (x)=||YY ref || 2

[0142] In the formula, Y represents the horizontal distance, Y ref Indicates the reference lateral distance;

[0143] (S41.2) The corresponding Lie derivative form constraint is specifically expressed as:

[0144] L f V L (x) = 2(v y cosψ+v x sinψ)(YY ref )

[0145]

[0146] L g1 L f V L (x) = 2sinψ(YY ref )

[0147]

[0148] In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the cornering stiffness of the front and rear axles, and m represents the vehicle mass;

[0149] (S42) constraining the control output of the model predictive control based on the high-order control barrier function so that the vehicle satisfies the non-collision constraint between vehicles during the rolling solution, as follows:

[0150] (S42.1) Design a vehicle-vehicle constraint based on a control barrier function, which is expressed as:

[0151] h v1 (x) = (X Of -x of ) 2 +(Y Of -yof ) 2 -4r2≥0

[0152] h v2 (x) = (X Of -x or ) 2 +(Y Of -y or ) 2 -4r2≥0

[0153] h v3 (x) = (X Of -x of ) 2 +(Y Or -y of ) 2 -4r2≥0

[0154] h v4 (x) = (X Or -x or ) 2 +(Y Or -y or ) 2 -4r 2 ≥0

[0155] In the formula, They represent the center positions of the two safety circles in front and behind the vehicle, Respectively represent the center positions of the two safety circles in front and behind other vehicles;

[0156] (S42.2) The corresponding Lie derivative form constraint is specifically expressed as:

[0157] L f h vi (x) = 2(v x cosψ-v y sinψ)(X O -x o )+2(v x sinψ+v y cosψ)(Y O -y o )

[0158]

[0159] L g1 L f h vi (x) = 2cosψ(X O -x O )+2sinψ(Y O -y O )

[0160]

[0161] In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the cornering stiffness of the front and rear axles, and m represents the vehicle mass;

[0162] (S43) constraining the control output of the model predictive control based on the high-order control barrier function so that the vehicle is guaranteed to satisfy the lane keeping constraint between the vehicle and the road during the rolling solution, as follows:

[0163] (S43.1) Design a vehicle-road constraint based on the control barrier function, which is expressed as:

[0164] h r1 (x) = d lb -Y≥0

[0165] h r2 (x) = Yd ub ≥0

[0166] Where, d lb ,d ub represents the upper and lower boundaries of the road, and Y represents the lateral position of the vehicle;

[0167] (S43.2) The corresponding Lie derivative form constraint is specifically expressed as:

[0168] L f h ri (x) = (-1) i (v x sinψ+v y cosψ)

[0169]

[0170] L g1 L f h ri (x) = (-1) i sinψ

[0171]

[0172] In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf,C αr represents the cornering stiffness of the front and rear axles, and m represents the vehicle mass;

[0173] S5. The obtained vehicle longitudinal acceleration and wheel steering angle are input into the actuator control module of the vehicle chassis, and the generated longitudinal acceleration command and wheel steering angle command are tracked respectively to complete the vehicle lane changing operation.

[0174] Next, the present invention is further described in conjunction with specific embodiments:

[0175] like Figure 3 As shown in the figure, the dotted line is the reference trajectory, the solid line is the actual trajectory, each dotted line is a group, and each three groups are a group. The first three groups of planning tracking lines are the results based on optimization, and the last six groups are the results not based on optimization. Fixed distance and fixed time strategies are used respectively. Each group of data represents the results at speeds of 10 m / s, 20 m / s, and 30 m / s. Through the optimized quintic polynomial, under the fixed distance strategy, the higher the speed, the less the planning results can meet the actual needs. At this time, due to the large lateral acceleration of the tracking, the lateral speed will also be large. Since the excessive yaw angular velocity may cause the vehicle to skid, the fixed distance strategy is the most unstable. Figure 3 It can also be seen that in the strategy based on fixed time, since the lane change distance is affected by the initial speed, the higher the initial speed means the longer the lane change distance. In this case, the tracking stability of the vehicle can be guaranteed. On the other hand, since the planning trajectory based on optimization can achieve a balance between the lane change time and the lane change distance at the same initial speed, the lane change trajectory based on optimization is shorter than the lane change distance based on the fixed time strategy.

[0176] like Figure 4 As shown, the strategy displays the position and historical trajectory at 2, 4, 6, and 9.5 seconds. The CBF-based constraints contain an accurate description of the obstacle motion information because the second-order derivatives of the system dynamics are performed in the high-order CBF. In terms of control input, the proposed strategy decelerates as much as possible to reduce the risk of collision. After 4 seconds, as the environment becomes more complex, the computational processing of the constraints becomes more diverse. Therefore, the proposed HOCBF-MPC can accurately describe the constraint relationship between the autonomous driving vehicle and the background vehicles in a complex dynamic environment, thereby finding a safe solution in multi-vehicle emergencies and maintaining the tracking stability of the vehicle.

[0177] In summary, this embodiment obtains vehicle and roadside environment status information; optimizes the lane change trajectory using the position information of the vehicle and surrounding vehicles; generates the lane change motion trajectory based on a quintic polynomial, including taking the minimum lane change time and the minimum lane change distance as optimization goals, and taking the vehicle steady-state steering as a reference; uses the model predictive control algorithm to establish a rolling time domain optimization problem, with the goal of accurately tracking the planned trajectory, and solving the longitudinal acceleration and tire steering angle of the vehicle in an emergency lane change scenario; including a collision-free constraint constructed based on a high-order control barrier function constraint, so as to avoid collisions between the vehicle and surrounding vehicles. This embodiment takes into account the vehicle motion planning and control in emergency lane change situations, and has good application prospects for autonomous collision avoidance and safe driving of vehicles in emergency scenarios in dynamic environments.

[0178] Embodiment 2:

[0179] This embodiment provides a system for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, which is used to implement the above-mentioned method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, including:

[0180] A receiving module is used to receive vehicle driving status information, which includes the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles;

[0181] The lane change decision and motion planning module is used to build a MOBIL lane change decision model, use the model to make lane change decisions, and determine whether to change lanes. If so, the lane change motion trajectory with the minimum lane change time and distance as the optimization target is calculated based on the fifth-order polynomial form;

[0182] The model predictive control module is used to establish the minimum safe collision-free circle based on the geometric relationship between vehicles, and use high-order control barrier functions to convert the collision-free constraints into vehicle driving constraints in the model predictive algorithm, and solve the vehicle longitudinal acceleration and wheel steering angle in the rolling optimization process;

[0183] The lane changing control module is used to input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, and respectively track the generated longitudinal acceleration command and wheel steering angle command to complete the vehicle lane changing operation.

[0184] Specifically, the above-mentioned receiving module, lane change decision and motion planning module, model prediction control module, and lane change control module can be embedded in a computer processing system. The computer calls the above-mentioned modules to complete the task of autonomous vehicle collision avoidance in a dynamic environment based on the above-mentioned comprehensive motion planning method for sudden lane change maneuvers of autonomous driving vehicles; the above-mentioned receiving module, lane change decision and motion planning module, model prediction control module, and lane change control module can perform operations according to the specific steps given in the above-mentioned comprehensive motion planning method for sudden lane change maneuvers of autonomous driving vehicles.

[0185] It should be noted that it should be understood that the division of the various modules of the above system is only the division of logical functions. In actual implementation, they can be fully or partially integrated into one physical entity, or they can be physically separated, and these modules can all be implemented in the form of software calling through processing elements; they can also be all implemented in the form of hardware; some modules can also be implemented in the form of software calling through processing elements, and some modules can be implemented in the form of hardware. For example, the receiving module can be a separately established processing element, or it can be integrated in a certain chip of the above device. In addition, it can also be stored in the memory of the above device in the form of program code, and a certain processing element of the above device calls and executes the functions of the above signal processing module. The implementation of other modules is similar. In addition, these modules can be fully or partially integrated together, or they can be implemented independently. The processing element described here can be an integrated circuit with signal processing capabilities. In the implementation process, each step of the above method or each module above can be completed by the hardware integrated logic circuit in the processor element or the instructions in the form of software.

[0186] For example, the above modules may be one or more integrated circuits configured to implement the above methods, such as one or more application specific integrated circuits (ASIC), or one or more digital singnal processors (DSP), or one or more field programmable gate arrays (FPGA). For another example, when a module is implemented in the form of a processing element scheduling program code, the processing element may be a general-purpose processor, such as a central processing unit (CPU) or other processors that can call program code. For another example, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0187] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0188] For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to the specific circumstances. When an element is referred to as being "assembled on", "installed on", "fixed on" or "set on" another element, it can be directly on the other element or there can also be a centered element. When an element is considered to be "connected" to another element, it can be directly connected to the other element or there may be a centered element at the same time. The terms "vertical", "horizontal", "up", "down", "left", "right" and similar expressions used herein are only for illustrative purposes and are not intended to be the only implementation method.

[0189] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

[0190] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

Claims

1. A comprehensive motion planning method for sudden lane change maneuvers of an autonomous driving vehicle, characterized in that: The specific steps include: Receive vehicle driving status information, which includes the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles; Construct a MOBIL lane-changing decision model and use it to make lane-changing decisions to determine whether to change lanes. If so, calculate the lane-changing trajectory with the minimum lane-changing time and distance as the optimization target based on a quintic polynomial form. The minimum safe collision-free circle is established based on the geometric relationship between vehicles, and a high-order control barrier function is used to transform the collision-free constraint into the vehicle driving constraint in the model prediction algorithm. The vehicle longitudinal acceleration and wheel steering angle are solved in the rolling optimization process. The obtained vehicle longitudinal acceleration and wheel steering angle are input into the actuator control module of the vehicle chassis, and the generated longitudinal acceleration command and wheel steering angle command are tracked respectively to complete the vehicle lane changing operation.

2. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 1, characterized in that: The driving state information of the vehicle is obtained through a vehicle speed sensor and CAN protocol communication, and the driving state information of surrounding vehicles is obtained through a laser radar and a millimeter wave radar.

3. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 2, characterized in that: The MOBIL model is used to make a lane-changing decision to determine whether to perform a lane-changing operation. The specific judgment principles are as follows: According to the status information of the received vehicle and surrounding vehicles, the acceleration gain of the vehicle after changing lanes under the current driving environment is calculated, plus the acceleration gain of the vehicle behind the target lane and the acceleration gain of the vehicle behind the lane multiplied by the politeness coefficient. If the value is greater than the set threshold, the lane change is allowed, otherwise it is not allowed.

4. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 3, characterized in that: Based on the fifth-order polynomial form, the lane-changing trajectory with the minimum lane-changing time and lane-changing distance as the optimization target is calculated, as follows: (1) If the current decision under the MOBIL model is to change lanes, then based on the longitudinal and lateral positions (X0, Y0) and speed (v x0 ,v y0 ), acceleration information (a x0 ,a y0 ), and assuming t f The end position (X f ,Y f )、speed(v xf ,v yf ) and acceleration (a xf ,a yf ) information, we can obtain a set of quintic polynomial equations, as follows: X(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 <h2 style=";text-align:left;direction:ltr">Y(t) = b0 + b1t + b2t<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +b3t<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> +b4t<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> +b5t<h2 style=";text-align:left;direction:ltr"> 5 In the formula, a i ,b i (i=0,1,…,5) represents the coefficients of the quintic polynomial to be solved. The coefficients are obtained by substituting the position, velocity, and acceleration information of the initial and final points, thereby solving the lane change trajectory (X(t), Y(t)) in the global coordinates. (2) Assume that the end point information at the end of the lane change is the vehicle steady-state center of mass sideslip angle β under steady-state steering. ss The specific formula of the steady-state center of mass sideslip angle is determined by the road curvature formed by the fifth order: In the formula, ψ ss represents the steady-state yaw angle under steady-state steering, l f ,l r are the front and rear axle wheelbases, m represents the vehicle mass, C αr represents the cornering stiffness of the rear axle, K represents the road curvature, and v x Indicates the longitudinal speed of the vehicle; (3) According to the geometric relationship between the heading angle θ, the sideslip angle β, and the yaw angle ψ, the steady-state yaw rate can be obtained as follows: In the formula, X, Y represent the coordinates of the vehicle in the global coordinate system, V represents the velocity direction at the center of mass of the vehicle body, and l f ,l r are the front and rear axle wheelbases, m represents the vehicle mass, C αr represents the cornering stiffness of the rear axle, and K represents the road curvature; (4) The stability of the vehicle in emergency steering is improved by limiting the sideslip angle and yaw rate. The sideslip angle constraint and yaw rate constraint are expressed as: |b des |≤arctan0.02μg Where μ is the road adhesion coefficient, g is the acceleration of gravity; (5) The motion trajectory is optimized so that it meets the specified starting and ending conditions and the lateral stability constraints of the vehicle, while minimizing the lane change time and distance. The optimization objective can be described as: The constraint expression is: In the formula, J path represents the optimization target, X, Y represent the optimized lane change trajectory coordinates, t f represents the lane changing time, μ is the road adhesion coefficient, g is the acceleration of gravity, X0, Y0 represent the coordinates of the starting point of the vehicle lane changing, Indicates the lateral and longitudinal acceleration at the starting point of lane change, Indicates the lateral and longitudinal speeds at the starting point of lane change. Indicates the lateral and longitudinal acceleration at the end of lane change. represents the lateral and longitudinal speeds at the end of the lane change, V represents the speed at the center of mass of the vehicle, K represents the road curvature, and β represents the sideslip angle of the center of mass; (6) The above constrained optimization objectives are solved by the QP solver to obtain a motion trajectory that satisfies the dynamic constraints of minimum lane changing time, minimum lane changing distance, and stable steering of the vehicle.

5. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 4, characterized in that: Using the model predictive control algorithm, the quintic polynomial motion trajectory generated by tracking control is obtained to obtain the optimal tracking control result, as follows: (1) Set the control input to the vehicle's longitudinal acceleration a x and the steering angle δ of the wheel, using the three-degree-of-freedom vehicle dynamics model, the expression can be written as: In the formula, I z represents the moment of inertia of the vehicle around the Z axis, C αf Indicates the front axle cornering stiffness; (2) Based on the vehicle dynamics model of the above model predictive control, in the prediction time domain N p The optimization goal of the design is optimized by rolling calculation of vehicle dynamics. The optimization goal is expressed as: In the formula, x represents the selected tracking state, x ref represents a given reference state, P, Q, R are weight matrices, ζ, ω are relaxation factors, ρ1, ρ2 are their weight coefficients, J represents the performance index, u is the control quantity, and T refers to the matrix torque; (3) According to the constraints expressed for the tracking task, the constraint function of the high-order system is designed, which can be expressed as: Where V L represents the Lyapunov function, h(x) represents the constraint function, k1, k2 are weight coefficients, and ω is the relaxation factor.

6. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 1, characterized in that: Establish the minimum safe collision-free circle constraint as follows: (1) Assume that the vehicle consists of a vehicle with a length of l evh , width is w veh If it is contained in a rectangle, two circles are set according to geometric principles to represent the distance at which no collision occurs; (2) Assume that between any two vehicles, a circle of any vehicle is tangent to at least the circle of the other vehicle and is tangent to the rectangle. Based on geometric principles, the radius r of the circle can be calculated and expressed as:

7. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 6, characterized in that: The control output of the model predictive control is constrained based on the high-order control barrier function, so that the vehicle gradually converges to the reference trajectory during the rolling solution, as follows: (1) The design is based on the control Lyapunov function constraint, which is expressed as: V L (x)=||YY ref || 2 In the formula, Y represents the horizontal distance, Y ref Indicates the reference lateral distance; (2) The corresponding Lie derivative form of the constraint is specifically expressed as: L f V L (x)=2(v y cosψ+v x sinψ)(Y-Y ref ) L g1 L f V L (x)=2sinψ(YY ref ) In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the lateral stiffness of the front and rear axles, and m represents the vehicle mass.

8. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 7, characterized in that: Based on the high-order control barrier function, the control output of the model predictive control is constrained so that the vehicles can meet the non-collision constraints between vehicles during the rolling solution, as follows: (1) Design a vehicle-vehicle constraint based on the control barrier function, which is expressed as: h v1 (x)=(x Of -x of ) 2 +(Y Of -y of ) 2 -4r 2 ≥0 h v2 (x)=(X Of -x or ) 2 +(Y Of -y or ) 2 -4r 2 ≥0 h v3 (x)=(X Of -x of ) 2 +(Y Or -y of ) 2 -4r 2 ≥0 h v4 (x)=(X Or -x or ) 2 +(Y Or -y or ) 2 -4r 2 ≥0 In the formula, They represent the center positions of the two safety circles in front and behind the vehicle, Respectively represent the center positions of the two safety circles in front and behind other vehicles; (2) The corresponding Lie derivative form of the constraint is specifically expressed as: L f h vi (x)=2(v x cosψ-v y sinψ)(X O -x o )+2(v x sinψ+v y cosψ)(Y O -y o ) L g1 L f h vi (x)=2cosψ(X O -x O )+2sinψ(Y O -y O ) In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the lateral stiffness of the front and rear axles, and m represents the vehicle mass.

9. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 8, characterized in that: The control output of the model predictive control is constrained based on the high-order control barrier function, so that the vehicle can meet the lane keeping constraints between the vehicle and the road during the rolling solution, as follows: (1) Design a vehicle-road constraint based on the control barrier function, which is expressed as: h r1 (x)=d lb -Y≥0 h r2 (x)=Y-d ub ≥0 Where, d lb ,d ub represents the upper and lower boundaries of the road, and Y represents the lateral position of the vehicle; (2) The corresponding Lie derivative form of the constraint is specifically expressed as: L f h ri (x)=(-1) i (v x sinψ+v y cosψ) L g1 L f h ri (x)=(-1) i sinψ In the formula, v x ,v y represents the longitudinal and lateral speed of the vehicle, ψ represents the vehicle heading angle, Y represents the lateral distance, and Y ref Indicates the reference lateral distance, C αf ,C αr represents the lateral stiffness of the front and rear axles, and m represents the vehicle mass.

10. A system for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, used to implement a method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to any one of claims 1 to 9, characterized in that: include: A receiving module is used to receive vehicle driving status information, which includes the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles; The lane change decision and motion planning module is used to build a MOBIL lane change decision model, use the model to make lane change decisions, and determine whether to change lanes. If so, the lane change motion trajectory with the minimum lane change time and distance as the optimization target is calculated based on the fifth-order polynomial form; The model predictive control module is used to establish the minimum safe collision-free circle based on the geometric relationship between vehicles, and use high-order control barrier functions to convert the collision-free constraints into vehicle driving constraints in the model predictive algorithm, and solve the vehicle longitudinal acceleration and wheel steering angle in the rolling optimization process; The lane changing control module is used to input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, and respectively track the generated longitudinal acceleration command and wheel steering angle command to complete the vehicle lane changing operation.

Images