Comprehensive motion planning method and system for sudden lane change maneuvers of autonomous driving vehicles

Abstract

The present invention discloses a comprehensive motion planning method for sudden lane change maneuvers of an autonomous vehicle, and relates to the technical field of autonomous vehicles. The present invention comprises the following steps: receiving vehicle driving state information, the driving state information including the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles; constructing a MOBIL lane change decision model, utilizing the model to make lane change decisions, determining whether to perform a lane change operation, and if so, calculating a lane change motion trajectory with minimum lane change time and distance as the optimization target based on a quintic polynomial form; and establishing a minimum safe collision-free circle based on the geometric relationship between vehicles. The present invention utilizes 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. This method has good application prospects for autonomous collision avoidance and safe driving of vehicles in emergency scenarios in dynamic environments.

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 enormous challenges, with safety becoming a key issue and a major focus of development. While existing advanced driver assistance systems provide some support under routine 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 rapid decisions in a very short period of 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 responses during the planning process to ensure the safety of the vehicle throughout the collision avoidance maneuver.

[0003] Lane change collision avoidance in general scenarios can be viewed as an avoidance problem for autonomous systems. When obstacles are stationary, the problem becomes a simple path planning problem. Although common sampling-based path planning algorithms, such as graph search, rapidly exploring random trees, and probabilistic road graphs, have demonstrated excellent performance for path planning in deterministic scenarios, point set planning trajectories often suffer from insufficient curvature smoothness, making it difficult to meet the requirements for precise vehicle control. In dynamic scenarios, local motion planning becomes significantly more difficult because the vehicle must consider not only static obstacles but also the system dynamics of surrounding moving traffic participants. In these situations, path planning is no longer simply about finding a feasible path; it requires dynamically solving the avoidance-reach task, enabling the controlled vehicle to avoid moving obstacles while maintaining safety. However, controllers designed in existing technologies struggle to effectively account for the uncertainty of surrounding unpredictable vehicles. To address this, we propose a comprehensive motion planning method and system for sudden lane change maneuvers for autonomous 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 autonomous driving vehicles, so as to achieve autonomous collision avoidance in complex dynamic environments.

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

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

[0007] A MOBIL lane-changing decision model is constructed and used to make lane-changing decisions. The model determines whether to change lanes. If so, a lane-changing trajectory is calculated based on a quintic polynomial to optimize the minimum lane-changing time and distance.

[0008] A minimum safe collision-free circle is established based on the geometric relationship between vehicles. 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 obtained during 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, which tracks the generated longitudinal acceleration command and wheel steering angle command 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 lane-changing decisions and determine whether to perform a lane-changing operation. The specific judgment principles are as follows:

[0012] Based on the received status information of the vehicle and surrounding vehicles, the system calculates the acceleration gain of the vehicle after changing lanes in the current driving environment, the acceleration gain of the vehicle behind the target lane, the acceleration gain of the vehicle behind the vehicle in the current lane, and the sum of the politeness coefficient. If the sum is greater than the set threshold, the lane change is allowed; otherwise, the lane change is not allowed.

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

[0014] (1) If the current decision in 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 of the lane change at the time indicated (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] Where a i ,b i (i = 0, 1, ..., 5) represents the coefficients of the quintic polynomial to be solved. By substituting the initial and final position, velocity, and acceleration information to find the coefficients, the lane change trajectory (X(t), Y(t)) in global coordinates is obtained.

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

[0020]

[0021] Where, ψ 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] Where X, Y represent the coordinates of the vehicle in global coordinates, 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 due to 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] Where, 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 horizontal and vertical 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 at 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 vehicle steering.

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

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

[0037]

[0038] Where, I z represents the vehicle's moment of inertia around the Z axis, C αf Indicates the front axle lateral stiffness;

[0039] (2) According to 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, then 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 also tangent to the rectangle. Based on geometric principles, the radius r of this 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 can gradually converge 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] Where, Y represents the horizontal distance, Y ref Indicates the reference lateral distance;

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

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

[0056]

[0057] Where, 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] Where, Respectively 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 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] Where, 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 a 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 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] Where, 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, a system for integrated motion planning of sudden lane change maneuvers for an autonomous driving vehicle is provided, which is used to implement the aforementioned integrated motion planning method for sudden lane change maneuvers for an autonomous driving vehicle, comprising:

[0083] A receiving module is used to receive vehicle driving status information, including 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. The model is used to make lane change decisions and determine whether to change lanes. If so, the lane change trajectory is calculated based on a quintic polynomial to optimize the minimum lane change time and distance.

[0085] The model predictive control module is used to establish a minimum safe collision-free circle based on the geometric relationship between vehicles. It uses a high-order control barrier function to convert the collision-free constraint into the vehicle driving constraint in the model predictive algorithm. The vehicle longitudinal acceleration and wheel steering angle are solved during the rolling optimization process.

[0086] The lane change control module is used to input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, track the generated longitudinal acceleration command and wheel steering angle command respectively, and complete the vehicle lane change 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 motion planning trajectories that enable stable tracking of vehicles in emergency lane changes without the risk of skidding. This method has promising application prospects for autonomous collision avoidance and safe driving in emergency scenarios in dynamic environments.

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

[0090] 3. This paper establishes a control barrier function based on the minimum safety circle constraint and constructs safety control constraints for lane change collision avoidance using the Lipschitz derivatives of high-order systems, replacing the traditional MPC rolling optimization constraints to more accurately describe the vehicle-to-vehicle and vehicle-to-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 Schematic diagram of the process of the present invention;

[0093] Figure 2 Schematic diagram of the calculation principle 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 results 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 according to an embodiment of the present invention. DETAILED DESCRIPTION

[0096] The following will be combined with the accompanying 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 embodiments described are only part of the embodiments of the present disclosure, not all of the embodiments. Based on the embodiments of the present disclosure, all other embodiments obtained by ordinary technicians in this field without making any creative efforts 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, including the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles;

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

[0100] S2. Build a MOBIL lane-changing decision model and use it to make lane-changing decisions. Determine whether to change lanes. If so, calculate the lane-changing trajectory based on a quintic polynomial to minimize lane-changing time and distance. The details are as follows:

[0101] (S21) calculating, based on the received status information of the vehicle and surrounding vehicles, the acceleration benefit of the vehicle after changing lanes under the current driving environment, the sum of the acceleration benefit of the vehicle behind the target lane and the acceleration benefit of the vehicle behind the vehicle in the own lane multiplied by the courtesy coefficient. If the sum 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 to change lanes, then based on the current time t0, 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 of the lane change at the time indicated (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] Where a i ,b i (i = 0, 1, ..., 5) represents the coefficients of the quintic polynomial to be solved. By substituting the initial and final position, velocity, and acceleration information into the coefficients, the lane change trajectory (X(t), Y(t)) in the global coordinates is obtained.

[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 for the steady-state center of mass sideslip angle is determined by the road curvature formed by the fifth order:

[0108]

[0109] Where, ψ 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 as follows:

[0111]

[0112] Where X, Y represent the coordinates of the vehicle in global coordinates, 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) Improving the stability of the vehicle in emergency steering 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 due to 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] Where, 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 horizontal and vertical 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 at the center of mass;

[0122] (S27) solving the above-mentioned constrained 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 to 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 vehicle's longitudinal acceleration a x and the steering angle δ of the wheel, using a three-degree-of-freedom vehicle dynamics model, the expression can be written as:

[0126]

[0127] Where, I z represents the vehicle's moment of inertia around the Z axis, C αf Indicates the front axle lateral 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) Based on 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, then 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 on either vehicle is tangent to at least one of the other vehicles' circles and also tangent to the rectangle. Based on geometric principles, the radius r of this circle can be calculated using the formula:

[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. Use a high-order control barrier function to transform the collision-free constraint into vehicle motion constraints in the model prediction algorithm. The vehicle longitudinal acceleration and wheel steering angle are obtained during the rolling optimization process. The vehicle motion constraints include vehicle convergence constraints to the reference trajectory, vehicle-to-vehicle collision-free constraints, and vehicle-to-road lane-keeping constraints.

[0139] (S41) Constraining the control output of the model predictive control based on the high-order control barrier function ensures 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] Where, Y represents the horizontal distance, Y ref Indicates the reference lateral distance;

[0143] The corresponding Lie derivative constraint (S41.2) 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] Where, 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 vehicles are guaranteed to meet the non-collision constraint between vehicles during the rolling solution, specifically as follows:

[0150] (S42.1) Design a vehicle-vehicle constraint based on the 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] Where, Respectively 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] The corresponding Lie derivative constraint (S42.2) 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] Where, 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 ensures that the vehicle satisfies 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] Where, 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. Input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, and track the generated longitudinal acceleration command and wheel steering angle command respectively to complete the vehicle lane change operation.

[0174] Next, the present invention will be further described with reference to specific embodiments:

[0175] like Figure 3 As shown, 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 based on optimization results, and the last six groups are non-optimization results. 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 fifth-order 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 fixed-time strategy, since the lane change distance is affected by the initial speed, a higher initial speed means a longer lane change distance. In this case, the vehicle tracking stability can be guaranteed. On the other hand, since the optimization-based planning trajectory can simultaneously achieve a balance between lane change time and lane change distance, at the same initial speed, the lane change distance based on optimization is shorter than that 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 complex dynamic environments, thereby finding a safe solution in multi-vehicle emergencies and maintaining the vehicle's tracking stability.

[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 minimum lane change distance as optimization goals, and taking the vehicle's 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, thereby avoiding collisions between the vehicle and surrounding vehicles. This embodiment takes into account 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] Example 2:

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

[0180] A receiving module is used to receive vehicle driving status information, including 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. The model is used to make lane change decisions and determine whether to change lanes. If so, the lane change trajectory is calculated based on a quintic polynomial to optimize the minimum lane change time and distance.

[0182] The model predictive control module is used to establish a minimum safe collision-free circle based on the geometric relationship between vehicles. It uses a high-order control barrier function to convert the collision-free constraint into the vehicle driving constraint in the model predictive algorithm. The vehicle longitudinal acceleration and wheel steering angle are solved during the rolling optimization process.

[0183] The lane change control module is used to input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, track the generated longitudinal acceleration command and wheel steering angle command respectively, and complete the vehicle lane change 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 a division of logical functions. In actual implementation, they can be fully or partially integrated into a physical entity, or they can be physically separated. Moreover, these modules can all be implemented in the form of software called by processing elements; or they can all be implemented in the form of hardware; or some modules can be implemented in the form of software called by 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 into a chip of the above-mentioned device. In addition, it can also be stored in the memory of the above-mentioned device in the form of program code, and called by a processing element of the above-mentioned device to perform the functions of the above-mentioned signal processing module. The implementation of other modules is similar. In addition, these modules can all or partly be 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 of the above modules can be completed by the hardware integrated logic circuit in the processor element or by 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 (ASICs), one or more digital signal processors (DSPs), or one or more field programmable gate arrays (FPGAs). For another example, when a module is implemented by scheduling program code through a processing element, the processing element may be a general-purpose processor, such as a central processing unit (CPU) or other processor 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 document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus.

[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 be a central element. When an element is considered to be "connected" to another element, it can be directly connected to the other element or there can be a central element at the same time. The terms "vertical", "horizontal", "up", "down", "left", "right" and similar expressions used herein are for illustrative purposes only and are not intended to be the only embodiment.

[0189] While 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 these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

[0190] Throughout this specification, references to terms such as "one embodiment," "example," or "specific example" indicate that a specific feature, structure, material, or characteristic described in conjunction with that embodiment or example is included in at least one embodiment or example of the present disclosure. In this specification, schematic representations of these terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

Claims

1. A comprehensive motion planning method for sudden lane change maneuvers of an autonomous vehicle, characterized by: The specific steps include: Receive vehicle driving status information, including the position, speed, acceleration, heading angle, and yaw rate of the vehicle and surrounding vehicles; A MOBIL lane-changing decision model is constructed and used to make lane-changing decisions. The model determines whether to change lanes. If so, a lane-changing trajectory is calculated based on a quintic polynomial to optimize the minimum lane-changing time and distance. A minimum safe collision-free circle is established based on the geometric relationship between vehicles. 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 obtained during 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 change operation; Based on the fifth-order polynomial form, the lane-changing trajectory with the minimum lane-changing time and distance as the optimization goal is calculated as follows: (1) If the current decision under the MOBIL model is that a lane change is required, then based on the current The longitudinal and lateral positions of the vehicle at the moment ( ),speed( ), acceleration information( ), and the assumption The end position of the lane change at the time indicated ( ),speed( ) and acceleration ( ) information, we can obtain a set of quintic polynomial equations, as follows: Where, Represents the coefficients of the quintic polynomial to be solved. By substituting the initial and end point position, velocity, and acceleration information to find the coefficients, the lane change trajectory in the global coordinates can be obtained. ; (2) The final position at the end of the lane change is the vehicle's steady-state center of mass side slip angle under steady-state steering. The specific formula for the steady-state center of mass sideslip angle is determined by the road curvature formed by the fifth order: Where, represents the steady-state yaw angle under steady-state steering, Respectively, the front and rear axle wheelbase, Indicates the vehicle mass, represents the cornering stiffness of the rear axle, represents the road curvature, Indicates the longitudinal speed of the vehicle; (3) According to the heading angle , side slip angle and yaw angle The geometric relationship between and can be used to obtain the steady-state yaw rate, as follows: Where, represents the coordinates of the vehicle in global coordinates, Indicates the direction of velocity at the center of mass of the vehicle body, Respectively, the front and rear axle wheelbase, Indicates the vehicle mass, represents the cornering stiffness of the rear axle, Indicates the road curvature; (4) By limiting the sideslip angle and yaw rate, the stability of the vehicle in emergency steering is improved. The sideslip angle constraint and yaw rate constraint are expressed as: In the formula is the road adhesion coefficient, is the acceleration due to 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: Where, represents the optimization goal, represents the optimized lane-changing trajectory coordinates, Indicates lane change time, is the road adhesion coefficient, is the acceleration due to gravity, Indicates the coordinates of the vehicle's lane-changing starting point, Indicates the lateral and longitudinal acceleration at the starting point of lane change, Indicates the horizontal and vertical speeds at the starting point of lane change. Indicates the lateral and longitudinal acceleration at the end of lane change, Indicates the horizontal and vertical speed at the end of lane change. represents the velocity at the center of mass of the vehicle, represents the road curvature, represents the sideslip angle of the center of mass; (6) Solving the optimization objectives of the above constraints using a QP solver to obtain a motion trajectory that satisfies the dynamic constraints of minimum lane-changing time, minimum lane-changing distance, and vehicle stable steering; Establish the minimum safe collision-free circle constraint as follows: (7) Assume that the vehicle consists of a , width is If it is contained in a rectangle, then two circles are set according to geometric principles to represent the distance at which no collision occurs; (8) Assume that between any two vehicles, a circle of any vehicle is tangent to at least the circle of the other vehicle and is also tangent to the rectangle. Based on the principle of geometry, the radius of this circle can be calculated. , which can be expressed as: 。 2. The method for comprehensive motion planning of a sudden lane change maneuver for an autonomous vehicle according to claim 1, characterized in that: The driving status information of the vehicle itself is obtained through a vehicle speed sensor and CAN protocol communication, and the driving status 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 lane-changing decisions and determine whether to proceed. The specific judgment principles are as follows: Based on the received status information of the vehicle and surrounding vehicles, the system calculates the acceleration gain of the vehicle after changing lanes in the current driving environment, the acceleration gain of the vehicle behind the target lane, the acceleration gain of the vehicle behind the vehicle in the current lane, and the sum of the politeness coefficient. If the sum is greater than the set threshold, the lane change is allowed; otherwise, the lane change is not allowed.

4. The method for comprehensive motion planning of a sudden lane change maneuver for an autonomous driving vehicle according to claim 3, characterized in that: Using the model predictive control algorithm, the quintic polynomial motion trajectory generated by tracking control is obtained, and the optimal tracking control results are as follows: (1) Set the control input to the longitudinal acceleration of the vehicle and the steering angle of the wheels , using a three-degree-of-freedom vehicle dynamics model, its expression can be written as: Where, represents the vehicle's moment of inertia around the Z axis, Indicates the front axle lateral stiffness; (2) Based on the vehicle dynamics model of the above model predictive control, in the prediction time domain The optimization goal of the design is optimized by rolling calculation of vehicle dynamics. The optimization goal is expressed as: Where, Indicates the selected tracking state. represents a given reference state, are the weight matrices, , is the relaxation factor, is its weight coefficient, Represents performance indicators, is the control quantity, 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, represents the Lyapunov function, represents the constraint function, is the weight coefficient, is the relaxation factor.

5. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 4, characterized in that: The control output of the model predictive control is constrained based on a 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: Where, Indicates the horizontal distance, Indicates the reference lateral distance; (2) The corresponding Lie derivative form constraint is specifically expressed as: Where, represents the longitudinal and lateral speeds of the vehicle, represents the vehicle heading angle, Indicates the horizontal distance, represents the reference lateral distance, represents the cornering stiffness of the front and rear axles, Indicates the vehicle mass.

6. The method for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle according to claim 5, characterized in that: 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. The details are as follows: (1) Design a vehicle-vehicle constraint based on the control barrier function, which is expressed as: Where, Respectively 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 constraint is specifically expressed as: Where, represents the longitudinal and lateral speeds of the vehicle, represents the vehicle heading angle, Indicates the horizontal distance, represents the reference lateral distance, represents the cornering stiffness of the front and rear axles, Indicates the vehicle mass.

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 a 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. The details are as follows: (1) Design a vehicle-road constraint based on the control barrier function, which is expressed as: Where, Indicates the upper and lower boundaries of the road, Indicates the lateral position of the vehicle; (2) The corresponding Lie derivative form constraint is specifically expressed as: Where, represents the longitudinal and lateral speeds of the vehicle, represents the vehicle heading angle, Indicates the horizontal distance, represents the reference lateral distance, represents the cornering stiffness of the front and rear axles, Indicates the vehicle mass.

8. A system for comprehensive motion planning of sudden lane change maneuvers for an autonomous driving vehicle, for implementing a comprehensive motion planning method for sudden lane change maneuvers for an autonomous driving vehicle according to any one of claims 1 to 7, characterized in that: include: A receiving module is used to receive vehicle driving status information, including 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. The model is used to make lane change decisions and determine whether to change lanes. If so, the lane change trajectory is calculated based on a quintic polynomial to optimize the minimum lane change time and distance. The model predictive control module is used to establish a minimum safe collision-free circle based on the geometric relationship between vehicles. It uses a high-order control barrier function to convert the collision-free constraint into the vehicle driving constraint in the model predictive algorithm. The vehicle longitudinal acceleration and wheel steering angle are solved during the rolling optimization process. The lane change control module is used to input the obtained vehicle longitudinal acceleration and wheel steering angle into the actuator control module of the vehicle chassis, track the generated longitudinal acceleration command and wheel steering angle command respectively, and complete the vehicle lane change operation.

Images