An integrated obstacle avoidance planning and control method for autonomous vehicles

Abstract

The application relates to an integrated obstacle avoidance planning and control method for an automatic driving vehicle, and comprises the following steps: an NMPC integrated planning controller obtains a dynamic time step based on the longitudinal speed of the mass center of the vehicle; the NMPC integrated planning controller constructs a vehicle dynamics model as a prediction model and performs discretization processing on the prediction model; the NMPC integrated planning controller establishes a total cost function comprising a repulsive potential energy of a surrounding vehicle; the NMPC integrated planning controller constructs an obstacle avoidance boundary of the vehicle; a time-varying maneuver stability domain of the vehicle is constructed; an optimal control variable sequence of the vehicle is obtained by solving a nonlinear optimization problem solver, and the first value in the optimal control variable sequence is applied to the vehicle. The application has the beneficial effects of improving the stability and safety of the automatic driving vehicle, and enabling the automatic driving vehicle to accurately, timely and safely avoid obstacles.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, in particular to the technical field of automatic driving obstacle avoidance, and specifically to an integrated obstacle avoidance planning and control method for an automatic driving vehicle. BACKGROUND

[0002] With the development of automatic driving technology, vehicles with automatic driving function have gradually entered the actual road traffic environment. In the automatic driving system, the planning and control module has the function of planning a feasible trajectory for the ego vehicle and controlling the vehicle to track the feasible trajectory, which is a key link for making automatic driving behavior decisions and implementing actual actions of the vehicle. Whether the automatic driving planning and control module can re-plan the local path in the global path according to the surrounding environment information and obstacle avoidance requirements, quickly, accurately and stably complete obstacle avoidance trajectory prediction and control the vehicle to move along the obstacle avoidance trajectory has a decisive influence on the safety and comfort of the automatic driving vehicle.

[0003] In the existing obstacle avoidance planning and control method for automatic driving vehicles, most of them use algorithms with fixed prediction step. However, for the planning and control problem established based on the NMPC integrated planning controller, if the prediction time domain of the fixed step is short, the MPC algorithm will lose the ability to utilize a part of future information, making the result short-sighted, and thus the automatic driving system cannot make reasonable decisions and control the vehicle in time in response to sudden situations, for example, when the vehicle is driving at a slow speed, the NMPC integrated planning controller can only predict a shorter distance than when the vehicle is driving at a medium or high speed. When the vehicle needs to quickly adjust its state to respond to sudden situations such as obstacle avoidance and lane changing, the automatic driving system cannot generate a feasible trajectory and control instructions in time, thereby affecting the safety and comfort of the automatic driving vehicle. If the prediction time domain of the fixed step is expanded, the scale of the solution will be larger, affecting the efficiency and real-time performance of the solution, and also leading to the inability of the automatic driving system to timely predict the future state of the vehicle and make decisions, thereby failing to control the vehicle in time and reducing the safety of the automatic driving vehicle.

[0004] In addition, during the obstacle avoidance process of the automatic driving vehicle, the dynamics problems of the vehicle such as the nonlinear dynamics characteristics (such as slip and roll) between the vehicle tires and the ground will greatly increase the difficulty of controlling the automatic driving vehicle, especially when the vehicle is avoiding obstacles at medium or high speed, which will cause the center of gravity to deviate and roll. Whether the automatic driving system can accurately and quickly control the vehicle reasonably will directly affect the stability and safety of the vehicle. However, the existing obstacle avoidance planning and control integrated method in the automatic driving system does not fully consider these complex dynamic and safety constraints, resulting in the inability of the automatic driving vehicle to effectively protect the safety of passengers and the vehicle when avoiding obstacles at medium or high speed. Summary of the Invention

[0005] In response to the deficiencies in the prior art, the present invention provides an integrated obstacle avoidance planning and control method for autonomous vehicles that can improve the stability and safety of autonomous vehicles when avoiding obstacles, enabling autonomous vehicles to avoid obstacles accurately, timely and safely.

[0006] The present invention is implemented through the following technical solution, providing an integrated obstacle avoidance planning and control method for an autonomous driving vehicle, comprising the following steps:

[0007] In step 1, the NMPC integrated planning controller sets the total prediction distance and the number of time steps in the prediction time domain to fixed values, and obtains the dynamic time step size based on the longitudinal velocity of the ego vehicle's center of mass;

[0008] Step 2: The NMPC integrated planning controller constructs a continuous ego-vehicle dynamics model including ego-vehicle control variables as a prediction model based on the ego-vehicle's inherent parameters and the ego-vehicle's state variables that continuously change over time, and discretizes the continuous ego-vehicle dynamics model based on the dynamic time step.

[0009] Step 3: The NMPC integrated planning controller constructs an artificial potential field model representing the repulsive potential energy of the surrounding vehicle through a Gaussian function based on the predicted trajectory information of the surrounding vehicle, and establishes a total cost function including the repulsive potential energy of the surrounding vehicle;

[0010] Step 4: The NMPC integrated planning controller constructs the obstacle avoidance boundary of the ego vehicle through an "axial bounding box" based on the motion state of the surrounding vehicle and its predicted trajectory information, and smoothes the obstacle avoidance boundary through a boundary optimization function;

[0011] Step 5: Discretize the continuous ego vehicle longitudinal velocity based on the dynamic time step described in step 1, and construct a time-varying control stability domain for the ego vehicle over the entire ego vehicle velocity range based on the ego vehicle's yaw rate stability bounds and the ego vehicle's center of mass sideslip angle stability bounds.

[0012] In step six, a nonlinear optimization problem solver uses a sequential quadratic programming (SQP) algorithm to determine the optimal control variable sequence for the ego vehicle's dynamics model, with the goal of minimizing the total cost function and the obstacle avoidance boundary and the ego vehicle's time-varying maneuverability region as constraints. The first value in the optimal control variable sequence is then applied to the ego vehicle, achieving closed-loop control feedback.

[0013] Compared with the prior art, the application has the beneficial effects that: by setting the number of time steps in the prediction time domain as a fixed value, the solving scale of the NMPC integrated planning controller can be stabilized, the real-time performance of the NMPC integrated planning controller is ensured, by setting the total prediction distance in the prediction time domain as a fixed value, the dynamic time step can change with the change of the longitudinal speed of the vehicle, and the prediction distance of each time step is the same, that is, when the longitudinal speed v x of the vehicle increases, the dynamic time step can correspondingly decrease, and vice versa, the dynamic time step can correspondingly increase, thereby ensuring the full use of the NMPC integrated planning controller on future information, and avoiding the problem that the vehicle cannot generate an obstacle avoidance trajectory and obstacle avoidance control instruction in time when the vehicle is running at low speed caused by the fixed time step in the prior art;

[0014] By establishing the total cost function including the repulsive potential of the surrounding vehicle, it can be defined for the NMPC integrated planning controller what solution is the optimal solution, so that the NMPC integrated planning controller can make the ego vehicle avoid obstacles as much as possible in the process of optimizing the ego vehicle obstacle avoidance trajectory;

[0015] By smoothing the obstacle avoidance boundary through the boundary optimization function, the obstacle avoidance boundary can be optimized, so that there is enough space for the front and rear of the surrounding vehicle to make a smooth transition, thereby the ego vehicle can have more time and space for adjustment to prevent the obstacle avoidance trajectory of the ego vehicle from having a mutation, which is beneficial to the solution of the obstacle avoidance trajectory by the NMPC integrated planning controller;

[0016] By discretizing the continuous longitudinal speed of the ego vehicle, the time-varying handling stability domain of the ego vehicle is constructed in the entire speed change range of the ego vehicle, so that the stability of the vehicle can be maintained in the dynamically changing environment, and the safety and comfort of the ego vehicle in the obstacle avoidance process are significantly improved.

[0017] Preferably, the dynamic time step is obtained based on a first expression, and the first expression is:

[0018]

[0019] In the first expression, Ts(i) is the dynamic time step, L is the total prediction distance in the prediction time domain, N p is the number of time steps in the prediction time domain, v x is the longitudinal speed of the center of mass of the ego vehicle, i=1, 2, 3,..., N p represents the i-th prediction time step in the prediction time domain, and the total prediction distance L in the first expression, the number of time steps N p in the prediction time domain are fixed values.

[0020] As a preference, the continuous ego vehicle dynamics model is obtained based on a second expression, the second expression being:

[0021]

[0022] In the second expression, the intrinsic parameters of the ego vehicle, the state variables, the control variables and the moment of inertia I z of the ego vehicle around the z-axis are included wf wr , the longitudinal forces F xf experienced by the front and rear tires of the ego vehicle xr , the lateral forces F yf experienced by the front and rear tires of the ego vehicle yr .

[0023] The intrinsic parameters of the ego vehicle include the gravitational acceleration g, the curb weight m, the rolling radii R e of the wheels, the distance a from the center of mass of the ego vehicle to the front axle, the distance b from the center of mass of the ego vehicle to the rear axle.

[0024] The state variables of the ego vehicle include the longitudinal velocity v x of the center of mass of the ego vehicle, the lateral velocity v y of the center of mass of the ego vehicle, the yaw rate ω of the ego vehicle, the yaw angle φ of the ego vehicle , the position X, Y of the center of mass of the ego vehicle in the Cartesian coordinate system.

[0025] The control variables of the ego vehicle include the external torques T df exerted on the front and rear wheels of the ego vehicle dr , and the steering angle δ of the front wheels of the ego vehicle.

[0026] As a preference, the longitudinal forces F xf experienced by the front and rear tires of the ego vehicle xr and the lateral forces F yf experienced by the front and rear tires of the ego vehicle yr are obtained based on a third expression, the third expression being:

[0027]

[0028] In the third expression, F x , F t are the actual longitudinal forces and lateral forces experienced by the individual tires of the ego vehicle, F x0 is the longitudinal force experienced by the tire in the pure slip condition, obtained based on a fourth expression, F y0 is the lateral force experienced by the tire in the pure cornering condition, obtained based on a fifth expression. ​

[0029] The fourth expression is:

[0030] F X0 = D x sin(C x arctan(B x * E x (B x S-arctan(B x * S))

[0031] The fifth expression is:

[0032] F y0 = D y sin(C y arctan(B y * a-E y (B y a-arctan(B y * a))

[0033] In the third, fourth, and fifth expressions, s is all the tire slip ratio, and a is all the tire side slip angle, the tire slip ratio s is obtained based on a sixth expression, and the tire side slip angle is obtained based on a seventh expression;

[0034] In the fourth and fifth expressions, B, C, D, and E are all parameters of a tire model, which are fitted from tire data;

[0035] The sixth expression is:

[0036]

[0037] In the sixth expression, w w is the rotational angular velocity of the wheel obtained through a sensor, R e is the rolling radius of the wheel, v xw is the wheel center longitudinal velocity of the wheel obtained through a sensor;

[0038] The seventh expression is:

[0039]

[0040] In the seventh expression, a f is the side slip angle of the front wheel tire of the vehicle, a r is the side slip angle of the rear wheel tire of the vehicle, d f is the steering angle of the front wheel obtained through a sensor, is the yaw angle of the vehicle, a is the distance from the vehicle mass center to the front axle of the vehicle, b is the distance from the vehicle mass center to the rear axle of the vehicle, v x , and v ylongitudinal velocity and lateral velocity of a vehicle mass center, respectively.

[0041] The beneficial effects of the above preferred technical solutions are that: through the third expression, the hybrid slip characteristics of the tire, i.e., the "adhesion ellipse" relationship between the tire and the ground, can be accurately obtained, and thus the dynamics model of the ego vehicle can fully consider that the nonlinear dynamics characteristics of the tire force have a great influence on the stability of the dynamics system of the ego vehicle, thereby improving the prediction accuracy of the dynamics model of the ego vehicle.

[0042] As preferred, the artificial potential field model representing the repulsion potential energy of the surrounding vehicles is obtained based on a ninth expression, which is:

[0043]

[0044] In the ninth expression, x and y are the coordinate positions of the ego vehicle, x m and y n are the position coordinates of the mth surrounding vehicle, i.e., the center positions of the Gaussian function on the x-axis and the y-axis, corresponding to the peak value of the Gaussian function, h m is the amplitude of the Gaussian function, and σ m is a parameter describing the influence range of the artificial potential field of the mth surrounding vehicle, i.e., the standard deviation in the Gaussian function, the value of σ m depends on the length and width of the mth surrounding vehicle.

[0045] As preferred, the total cost function is obtained based on a fifteenth expression, the total cost function including a total repulsion potential energy cost function represented by an eleventh expression, an initial trajectory following cost function represented by a twelfth expression, a steering cost function represented by a thirteenth expression, and a driving cost function represented by a fourteenth expression, wherein the total repulsion potential energy cost function is obtained based on a tenth expression;

[0046] The fifteenth expression is:

[0047] J COST = J1+ J2+ J Δcontrol + J APF

[0048] The tenth expression is:

[0049]

[0050] The eleventh expression is:

[0051]

[0052] In the eleventh expression, P represents the weight of the total repulsion potential energy cost function, N pt is the number of time steps in the prediction horizon, t is the prediction time;

[0053] The twelfth expression is:

[0054]

[0055] In the twelfth expression, represents the output of the ego vehicle prediction model at time k+i, r(k+i) represents the initial initial reference path given by the NMPC integrated planning controller at time k+i, which contains information corresponding to the ego vehicle prediction model output y c The contained information corresponds to Q, which is used to represent the weight of the initial trajectory following cost function;

[0056] The thirteenth expression is:

[0057]

[0058] In the thirteenth expression, represents the control amount increment input to the ego vehicle prediction model at time k+i, and R is used to represent the weight of the steering cost function;

[0059] The fourteenth expression is:

[0060]

[0061] In the fourteenth expression, represents the control amount increment input to the ego vehicle prediction model at time k+i, and S is used to represent the weight of the driving cost function.

[0062] As a preferred, when the obstacle avoidance boundary is smoothed by the boundary optimization function, the variable fraction is defined as the length of the transition area before and after the vehicle, and fraction*pi proportionally maps the x coordinate to the range of [0,pi], as shown in the nineteenth expression:

[0063]

[0064] In the nineteenth expression, L is the road length, W is the road width, x start is the starting position of the vehicle, L obstacle , W ovstacle are the length and width of the vehicle, respectively, L buffer is the length of the transition area, x trans_start , x trans_end are the starting position and ending position of the transition area, respectively.

[0065] As preferred, the lateral velocity stability boundary of the ego vehicle is obtained based on a twenty-third expression, which is:

[0066]

[0067] In the twenty-third expression, m is the curb weight of the ego vehicle, ω ss is the yaw rate of the ego vehicle's mass center, F x is the longitudinal force of the ego vehicle's tire, v x is the longitudinal velocity of the ego vehicle's mass center.

[0068] As preferred, the side slip angle stability boundary of the ego vehicle's mass center is obtained based on a twenty-seventh expression, which is:

[0069]

[0070] In the twenty-seventh expression, α r，peak is the tire side slip angle of the ego vehicle's rear wheel tire when the lateral force reaches the limit, C α is the side slip stiffness of the ego vehicle's rear wheel tire, β ss is the side slip angle of the ego vehicle's mass center, b is the distance from the ego vehicle's mass center to the rear axle of the vehicle, v x is the longitudinal velocity of the ego vehicle's mass center, ω is the yaw rate of the ego vehicle.

[0071] As preferred, when the solver solves, a warm start strategy is used to optimize the solving process, that is, a series of solutions solved by the NMPC integrated planning controller in the previous prediction time domain is used as the initial value of the optimization problem constructed in the next prediction time domain for iterative optimization. BRIEF DESCRIPTION OF DRAWINGS

[0072] Figure 1 is the flow chart of the present application;

[0073] Figure 2 is the schematic diagram of the dynamic time step and the prediction distance in the present application;

[0074] Figure 3 is the schematic diagram of the artificial potential field of the ego vehicle in the present application;

[0075] Figure 4 is the schematic diagram of the axial bounding box in the present application;

[0076] Figure 5 is the schematic diagram of the obstacle avoidance boundary in the present application;

[0077] Figure 6 is the schematic diagram of the time-varying steering stability domain of the ego vehicle in the present application;

[0078] Figure 7A schematic diagram of an obstacle avoidance trajectory in the present application;

[0079] Figure 8 A longitudinal speed change curve of the ego vehicle in the obstacle avoidance process in the present application;

[0080] Figure 9 A yaw angle change curve of the ego vehicle in the obstacle avoidance process in the present application. DETAILED DESCRIPTION

[0081] The technical solutions in the embodiments of the present application will be described clearly and completely below. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

[0082] Embodiment 1

[0083] The embodiment provides an integrated obstacle avoidance planning and control method for an autonomous vehicle, and specifically includes the following steps:

[0084] Step 1: An NMPC integrated planning controller obtains the longitudinal speed v of the mass center of the ego vehicle through a sensor x , and obtains a dynamic time step Ts(i) based on a first expression;

[0085] Specifically, the first expression is:

[0086]

[0087] In the first expression, L is the total prediction distance, N p is the number of time steps in the prediction horizon, v x is the longitudinal speed of the mass center of the ego vehicle, i = 1, 2, 3,..., N p represents the ith prediction time step in the prediction horizon.

[0088] The total prediction distance L and the number of time steps N p in the prediction horizon in the first expression are both fixed values, the dynamic time step Ts(i) obtained based on the first expression can change with the change of the longitudinal speed of the vehicle, and the prediction distance of each time step is the same, that is, when the longitudinal speed v x of the vehicle increases, the dynamic time step Ts(i) can be reduced accordingly, and vice versa. In the present embodiment, preferably, the total prediction distance L takes the maximum detection range of the environmental perception sensors such as laser radar and camera on the autonomous vehicle, so as to make full use of future information. Since the number of time steps N pThe fixed value stabilizes the solution size of the NMPC integrated planning controller, ensures the real-time performance of the NMPC integrated planning controller, and ensures the full use of future information of the NMPC integrated planning controller by fixing the total prediction distance L.

[0089] In step two, a continuous self-vehicle dynamics model is constructed in the NMPC integrated planning controller as a prediction model, and the continuous self-vehicle dynamics model is discretized according to the dynamic time step Ts(i);

[0090] Specifically, the NMPC integrated planning controller obtains inherent parameters of the self-vehicle and state variables of the self-vehicle that continuously change over time through the vehicle model of the self-vehicle and sensors;

[0091] The inherent parameters of the self-vehicle include: gravitational acceleration g, vehicle kerb mass m, wheel rolling radius R e , the distance a from the vehicle center of mass to the front axle of the vehicle, and the distance b from the vehicle center of mass to the rear axle of the vehicle.

[0092] The state variables of the self-vehicle include: the longitudinal speed v x of the vehicle center of mass, the lateral speed v y of the vehicle center of mass, the yaw rate ω of the vehicle, the yaw angle φ of the vehicle , the position X and Y of the vehicle center of mass in the Cartesian coordinate system.

[0093] In the NMPC integrated planning controller, a continuous self-vehicle dynamics model including inherent parameters of the self-vehicle, state variables of the self-vehicle, and control variables is constructed based on a second expression, and the second expression is:

[0094]

[0095] In the second expression, I z is the moment of inertia of the vehicle around the z-axis, I wf and I wr are the moments of inertia of the front wheel and the rear wheel of the vehicle, T df and T dr are external torques acting on the front wheel and the rear wheel of the vehicle, F xf and F xr are longitudinal forces on the front wheel tire and the rear wheel tire of the vehicle, F yf and F yr are lateral forces on the front wheel tire and the rear wheel tire of the vehicle, and δ is the front wheel steering angle of the vehicle.

[0096] In the second expression, T df and T dr are external torques acting on the front wheel and the rear wheel of the vehicle, and the front wheel steering angle δ of the vehicle is a control variable of the self-vehicle.

[0097] Because the nonlinear dynamics characteristics of tire force have great influence on the stability of the vehicle dynamics system, in the analysis of the stability of the vehicle dynamics system, in order to comprehensively describe the lateral and longitudinal coupling dynamics characteristics of the vehicle, the mixed slip characteristics of the tire, i.e. the "adhesion ellipse" relationship between the tire and the ground, should be fully considered, so that the actual longitudinal force F x , lateral force F y of the single tire of the vehicle is obtained by the third expression.

[0098]

[0099] In the third expression, s is the tire slip ratio, α is the tire side slip angle, F x0 is the longitudinal force of the tire under the pure slip condition, F y0 is the lateral force of the tire under the pure side slip condition.

[0100] In this embodiment, the influences of the camber angle and the aligning torque of the wheel are ignored, the Pacejka model is selected to calculate F x0 , F y0 , and the longitudinal force F x0 of the tire under the pure slip condition and the lateral force F y0 of the tire under the pure side slip condition in the third expression are obtained by the fourth expression and the fifth expression respectively.

[0101] The fourth expression is:

[0102] F X0 = D x sin(C x arctan(B x *S-E x (B x S-arctan(B x *S))))

[0103] The fifth expression is:

[0104] F y0 = D y sin(C y arctan(B y *α-E y (B y α-arctan(B y *α))))

[0105] In the fourth and fifth expressions, B, C, D and E are parameters of the tire model, which can be fitted by tire data, s is the tire slip ratio, and α is the tire side slip angle.

[0106] In the fourth and fifth expressions, B, C, D, and E are parameters of the tire model, which are fitted from tire data. Specifically, B is a stiffness factor related to the deformation stiffness of the tire, C is a shape factor of the tire, which affects the shape and smoothness of the fourth and fifth expressions, D is a peak factor related to the peak value of the longitudinal force of the tire, and E is a curvature factor;

[0107] The tire slip ratio s in the third and fourth expressions is obtained based on a sixth expression, which is:

[0108]

[0109] The sixth expression is suitable for calculating the tire slip ratio s under all working conditions. In the sixth expression, ω w is the rotational angular velocity of the wheel obtained by a sensor, R e is the rolling radius of the wheel, v xw is the wheel center longitudinal velocity of the wheel obtained by a sensor. In this embodiment, the wheel center longitudinal velocity v xw of the wheel is the same as the longitudinal velocity v x of the vehicle mass center;

[0110] The tire side slip angle a in the third and fifth expressions is obtained based on a seventh expression, which is:

[0111]

[0112] In the seventh expression, a f is the side slip angle of the front wheel tire of the vehicle, a r is the side slip angle of the rear wheel tire of the vehicle, d f is the steering angle of the front wheel obtained by a sensor, is the yaw angle of the vehicle, a is the distance from the vehicle mass center to the front axle of the vehicle, b is the distance from the vehicle mass center to the rear axle of the vehicle, v x and v y are the longitudinal velocity and lateral velocity of the vehicle mass center, respectively;

[0113] When the self-vehicle dynamics model is completed, the continuous self-vehicle dynamics model shown by the second expression is discretized by a one-step Euler method based on a dynamic time step Ts(i) to obtain an iteration equation at time n+1 relative to time n, which is shown by an eighth expression:

[0114]

[0115] Step three, according to the predicted trajectory information of the surrounding vehicle, an artificial potential field model representing the repulsive potential energy of the surrounding vehicle is constructed by a probability Gaussian function, and a total cost function including the repulsive potential energy of the surrounding vehicle is established.

[0116] Specifically, the NMPC integrated planning controller obtains the trajectory information of the surrounding vehicle, i.e., the coordinate position of the surrounding vehicle, according to the trajectory prediction algorithm of the surrounding vehicle, and establishes an artificial potential field model of the surrounding vehicle based on the ninth expression by using a probability Gaussian function in a two-dimensional space to represent the repulsive potential around the surrounding vehicle, the ninth expression being:

[0117]

[0118] In the ninth expression, x and y are the coordinate positions of the ego vehicle, x m and y m are the coordinate positions of the mth surrounding vehicle, i.e., the center positions of the Gaussian function on the x-axis and the y-axis, corresponding to the peak value of the Gaussian function, h m is the amplitude of the Gaussian function, and by setting h m , the peak value of the repulsive potential of the mth surrounding vehicle can be determined, σ m is a parameter describing the influence range of the artificial potential field of the mth surrounding vehicle, i.e., the standard deviation in the Gaussian function, and by setting σ m , the influence range of the artificial potential field can be adjusted, and the value of σ m depends on the length and width of the mth surrounding vehicle.

[0119] If there are multiple obstacles, the total repulsive potential is the cumulative repulsive potential of each surrounding vehicle on the ego vehicle, as shown in the tenth expression:

[0120]

[0121] In the NMPC integrated planning controller, the cost function defines what solution is the optimal solution, and by adding the repulsive potential of the surrounding vehicle in the cost function, the NMPC integrated planning controller can make the ego vehicle avoid obstacles as much as possible in the process of optimizing the obstacle avoidance trajectory of the ego vehicle. The repulsive potential of the surrounding vehicle is added to the cost function to obtain the eleventh expression:

[0122]

[0123] In the eleventh expression, P represents the weight of the cost function, N is the number of time steps in the prediction horizon, and t is the prediction time.

[0124] In order to make the ego vehicle return to the initial driving trajectory as quickly as possible after avoiding obstacles, and to meet the control requirement that the control amount and the control increment generated by the controller are as small as possible, the following cost functions are also constructed:

[0125] The initial trajectory following cost function represented by the twelfth expression is as follows:

[0126]

[0127] In the twelfth expression, represents the output of the ego vehicle prediction model at the k+i moment, which in this embodiment is the longitudinal coordinate of the vehicle mass center of the ego vehicle, the vehicle yaw angle and the longitudinal vehicle speed in the Cartesian coordinate system, i.e. r(k+i) represents the initial reference path given by the NMPC integrated planning controller at the k+i moment, which contains information corresponding to the output of the ego vehicle prediction model. characterizes the error between the vehicle state obtained by the prediction model at the k+i moment and the reference state initially given by the NMPC integrated planning controller, and Q is used to represent the weight of this cost function;

[0128] The steering cost function represented by the thirteenth expression is:

[0129]

[0130] In the thirteenth expression, represents the control amount input to the ego vehicle prediction model at the k+i moment, which in this embodiment is the external torque of the front and rear wheels of the ego vehicle and the front wheel angle of the ego vehicle, i.e. and R is used to represent the weight of this cost function;

[0131] The driving cost function represented by the fourteenth expression is:

[0132]

[0133] In the fourteenth expression, represents the control amount increment input to the ego vehicle prediction model at the k+i moment, which in this embodiment is the increment of the external torque of the front and rear wheels of the ego vehicle and the increment of the front wheel angle of the ego vehicle, i.e. S is used to represent the weight of this cost function;

[0134] In summary, the total cost function is represented by the fifteenth expression:

[0135] J COST = J1+ J2+ J Δcontrol + J APF

[0136] Step four, according to the motion state and the predicted trajectory information of the obstacle vehicles around the ego vehicle, the obstacle avoidance boundary of the ego vehicle is constructed through the "axial bounding box", and the obstacle avoidance boundary is smoothed through the boundary optimization function;

[0137] Specifically, the NMPC integrated planning controller obtains trajectory information of the ego vehicle according to a trajectory prediction algorithm of the ego vehicle, and obtains motion states including a speed, a heading angle, an acceleration and the like of the ego vehicle through sensors;

[0138] On a structured road without the ego vehicle, a lane boundary is a drivable area of the ego vehicle, and when the ego vehicle appears on the road, an area in the road boundary except for an inflation boundary of the ego vehicle is a drivable area of the ego vehicle, that is, an obstacle avoidance boundary. To calculate the obstacle avoidance boundary of the ego vehicle, the application adopts an "axis-aligned bounding box" to represent the boundary of the ego vehicle, and completes construction of the obstacle avoidance boundary of the ego vehicle according to a predicted trajectory of the ego vehicle and the road boundary.

[0139] An axis-aligned bounding box (AABB) is a minimum rectangular frame always aligned with coordinate axes, and is used to represent the ego vehicle and is convenient for judging whether two axis-aligned bounding boxes intersect. When two axis-aligned bounding boxes are close or intersect, the two axis-aligned bounding boxes can be merged through the sixteenth, seventeenth and eighteenth expressions. In a two-dimensional space, a rectangular frame of each axis-aligned bounding box can be defined by a minimum point (usually a left lower corner point) and a maximum point (usually a right upper corner point). If there are two axis-aligned bounding boxes A and B, the sixteenth expression is as follows:

[0140]

[0141] When the rectangular frames of A and B intersect or are very close, the boundary of A and B needs to be adjusted and A and B are merged into a new axis-aligned bounding box when the obstacle avoidance boundary is constructed.

[0142] When A and B are merged, the minimum point of the new axis-aligned bounding box, that is, the coordinates of the left lower corner point, is determined through the seventeenth expression, and then the maximum point of the new axis-aligned bounding box, that is, the coordinates of the right upper corner point, is determined through the eighteenth expression.

[0143] When the minimum point of the new axis-aligned bounding box is determined, the minimum values of the coordinate components of the minimum points (left lower corner points) of the two axis-aligned bounding boxes A and B are taken respectively. Assuming that the minimum point of the axis-aligned bounding box A is min A =(x 1A ,y 1A ), the minimum point of the axis-aligned bounding box B is min B =(x 1B ,y 1B ), and the seventeenth expression for representing the coordinates of the minimum point of the new axis-aligned bounding box is as follows:

[0144] min=(min(x1 A ,x1B ), min(y1 A , y1 B ))

[0145] In determining the maximum point of the new axial bounding box, the maximum values of the respective coordinate components of the maximum points (the upper right corner points) of the two axial bounding boxes A and B are taken respectively, assuming that the maximum point of the axial bounding box A is max A = (x 2A , y 2A ), and the maximum point of the axial bounding box B is max B = (x 2B , y 2B ), the eighteenth expression for representing the coordinates of the maximum point of the new axial bounding box is:

[0146] max = (max(x2 A , x2 B ), max(y2 A , y2 B ))

[0147] The minimum point and the maximum point of the new axial bounding box are used to define a new two-dimensional rectangular frame that completely contains the two original axial bounding boxes A and B, and the new two-dimensional rectangular frame can be further adjusted in size or position according to the predicted moving trajectory of the surrounding vehicle, thereby providing a clear obstacle avoidance boundary for the autonomous driving system, so that the ego vehicle can effectively plan a path to avoid collision with the dynamic surrounding vehicle.

[0148] In order to provide sufficient space for the front and rear of the surrounding vehicle to transition smoothly, thereby allowing the ego vehicle to have more time and space to adjust, the present application optimizes the obstacle avoidance boundary based on the nineteenth expression using a boundary optimization function.

[0149] Specifically, the variable fraction is defined as the length of the transition region in front of and behind the surrounding vehicle, and fraction * pi maps the x coordinate to the range of [0, pi] in proportion, thereby ensuring that the transition starts and ends at the correct position. The nineteenth expression is:

[0150]

[0151] In the nineteenth expression, L is the road length, W is the road width, x start is the starting position of the surrounding vehicle, L obstacle , W obstacle are the length and width of the surrounding vehicle respectively, L buffer is the length of the transition region, and x trans_start , x trans_end are the starting position and ending position of the transition region respectively.

[0152] Through the optimization of the nineteenth expression, the available space can be more effectively utilized, which provides a basis for further optimizing the solution of the feasible region of the NMPC integrated planning controller, and when the boundary of the axis-aligned bounding box is used as a constraint, the mutation of the constraint is reduced, which is conducive to the solution of the optimization problem in the MPC.

[0153] Step five, obtaining the time-varying handling stability domain of the ego vehicle, and constructing the time-varying handling stability domain of the ego vehicle in the entire speed variation range of the ego vehicle through a discretization method;

[0154] Specifically, the construction of the time-varying handling stability domain of the ego vehicle is based on the yaw rate stability boundary of the ego vehicle and the sideslip angle stability boundary of the mass center of the ego vehicle;

[0155] When the yaw rate stability boundary of the ego vehicle is obtained, the centrifugal force caused by the yaw angle of the ego vehicle will not exceed the limit of the adhesion provided by the ground, as shown in the twentieth expression:

[0156]

[0157] Therefore, according to the twentieth expression, the limit of the yaw rate of the ego vehicle is as shown in the twenty-first expression:

[0158]

[0159] At the same time, based on the acceleration of the current ego vehicle, the longitudinal force F x provided by the tire of the ego vehicle, the maximum lateral force that can be provided by the tire of the ego vehicle is as shown in the twenty-second expression:

[0160]

[0161] In summary, the stability boundary of the yaw rate of the ego vehicle is as shown in the twenty-third expression:

[0162]

[0163] In the twentieth, twenty-first, twenty-second, and twenty-third expressions, m is the curb weight of the ego vehicle, ω ss is the yaw rate of the mass center of the ego vehicle, F yf and F yr are the lateral forces of the front wheels and the rear wheels of the ego vehicle, and v x is the longitudinal speed of the mass center of the ego vehicle.

[0164] Since the main factor of the spinout and other instability sideslip phenomena of the mass center of the ego vehicle is usually the sideslip of the rear wheels of the ego vehicle, in this embodiment, the sideslip angle stability boundary of the mass center of the ego vehicle is represented by the tire sideslip angle stability boundary of the rear wheels of the ego vehicle;

[0165] In obtaining the cornering angle stability boundary of the rear tire of the ego vehicle, first, according to the tire brush model in the Pacejka model as shown in the twenty-fourth expression, the tire cornering angle of the rear tire of the ego vehicle when reaching the lateral force limit is obtained, and the twenty-fourth expression is:

[0166]

[0167] In the twenty-fourth expression, a is the cornering angle of the rear tire of the ego vehicle, C α is the cornering stiffness of the rear tire of the ego vehicle, and μ is the adhesion coefficient between the rear tire of the ego vehicle and the road. According to the twenty-fourth expression, the twenty-sixth expression representing the cornering angle a of the rear tire of the ego vehicle and the twenty-fifth expression representing the tire cornering angle a of the rear tire of the ego vehicle when reaching the lateral force limit can be obtained;

[0168] The twenty-fifth expression is:

[0169]

[0170] The twenty-sixth expression is:

[0171]

[0172] According to the twenty-fifth and twenty-sixth expressions, the cornering angle stability boundary of the rear tire of the ego vehicle, i.e., the cornering angle stability boundary of the mass center of the ego vehicle is as shown in the twenty-seventh expression:

[0173]

[0174] For a linear dynamics model with constant longitudinal speed of the ego vehicle, the time-varying handling stability domain of the ego vehicle can be represented by the yaw rate stability boundary as shown in the twenty-third expression and the cornering angle stability boundary of the mass center of the ego vehicle as shown in the twenty-seventh expression. However, for a three-degree-of-freedom or five-degree-of-freedom dynamics model with varying longitudinal speed of the ego vehicle, the time-varying handling stability domain of the ego vehicle changes with the state of the ego vehicle, and as the longitudinal speed of the ego vehicle increases, the time-varying handling stability domain of the ego vehicle tends to shrink. In order to more accurately represent the time-varying handling stability domain of the ego vehicle at different speeds, in this embodiment, according to the characteristics of the NMPC integrated planning controller, the idea of "zero-order hold" in the control field is adopted, and the longitudinal speed of the ego vehicle is discretized in the prediction time domain, i.e., it is assumed that the longitudinal speed of the ego vehicle remains unchanged in each dynamic time step, and the local stability domain of the ego vehicle is calculated at the discrete speed points, and then the stability domain of the ego vehicle is constructed in the entire speed range of the ego vehicle. When the MPC controller is actually running, the MPC controller can quickly obtain the time-varying handling stability domain of the ego vehicle at the corresponding speed through the hash table.

[0175] Step six, through the nonlinear optimization problem solver, with the goal of minimizing the total cost function, with the time-varying maneuvering stability domain of the ego vehicle and the obstacle boundary as the constraint condition, the optimal control variable sequence of the ego vehicle dynamics model is solved by using the sequential quadratic programming (SQP) algorithm. Then the first value in the optimal control variable sequence is applied to the ego vehicle to realize closed-loop control feedback;

[0176] In order to further optimize the solving process, especially in the face of the scene that needs to frequently solve similar problems, the embodiment introduces a "warm start" strategy, that is, a series of solutions solved by the NMPC integrated planning controller in the previous prediction time domain are taken as the initial values of the optimization problem constructed in the next prediction time domain for iterative optimization, thereby improving the utilization rate of the solution obtained at each dynamic time step and improving the solving speed and quality of the NMPC integrated planning controller.

[0177] The above embodiments are only used to illustrate the technical solutions of the present application, but not limit it; based on the embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor are within the scope of protection of the present application. Although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements for part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. An integrated obstacle avoidance planning and control method for an autonomous vehicle, the method comprising: The method comprises the following steps: Step 1: the NMPC integrated planning controller sets the total predicted distance and the number of time steps in the prediction time domain as fixed values, and obtains a dynamic time step based on the longitudinal velocity of the vehicle center; Step 2: the NMPC integrated planning controller constructs a continuous vehicle dynamics model including the control variables of the vehicle as a prediction model based on the inherent parameters of the vehicle and the state variables of the vehicle continuously changing over time, and discretizes the continuous vehicle dynamics model based on the dynamic time step; Step 3: the NMPC integrated planning controller constructs an artificial potential field model representing the repulsive potential of the surrounding vehicle by a Gaussian function according to the prediction trajectory information of the surrounding vehicle, and establishes a total cost function including the repulsive potential of the surrounding vehicle; Step 4: the NMPC integrated planning controller constructs the obstacle avoidance boundary of the vehicle by an "axial bounding box" according to the motion state and the prediction trajectory information of the surrounding vehicle, and smoothes the obstacle avoidance boundary by a boundary optimization function; Step 5: the NMPC integrated planning controller discretizes the continuous longitudinal velocity of the vehicle based on the dynamic time step, and constructs the time-varying steering stability domain of the vehicle in the entire speed range of the vehicle based on the lateral velocity stability boundary and the side slip angle stability boundary of the vehicle; Step 6: the NMPC integrated planning controller solves the optimal control variable sequence of the vehicle dynamics model by a nonlinear optimization problem solver, taking minimizing the total cost function as the target, taking the obstacle avoidance boundary and the time-varying steering stability domain of the vehicle as the constraint conditions, and adopting a sequential quadratic programming algorithm; then the first value in the optimal control variable sequence is applied to the vehicle to realize closed-loop control feedback.

2. The integrated obstacle avoidance planning and control method for an autonomous vehicle of claim 1, wherein, The dynamic time step is obtained based on a first expression, the first expression is: In the first expression, Ts(i) is the dynamic time step, L is the total prediction distance in the prediction time domain, N p is the number of time steps in the prediction time domain, v x is the longitudinal velocity of the vehicle's center of mass, i=1,2,3,..,N p represents the i-th prediction time step in the prediction time domain, the total prediction distance L in the first expression, the number of time steps N in the prediction time domain p These are fixed values.

3. The integrated obstacle avoidance planning and control method for autonomous vehicles of claim 1, wherein, The continuous vehicle dynamics model is obtained based on a second expression, the second expression is: in the second expression, including the inherent parameters of the ego vehicle, state variables, control variables, and the moment of inertia I of the vehicle around the z axis z , the moment of inertia I of the front wheel, the rear wheel of the vehicle wf , I wr , the longitudinal force F received by the front wheel tire, the rear wheel tire of the vehicle xf , F xr , the lateral force F received by the front wheel tire, the rear wheel tire of the vehicle yf , F yr ; Intrinsic parameters of the ego vehicle include: gravitational acceleration g, vehicle curb mass m, wheel rolling radius R e , distance a of the vehicle center of mass to the vehicle front axle, distance b of the vehicle center of mass to the vehicle rear axle; State variables of the ego vehicle include: longitudinal speed v of the vehicle's center of mass x lateral speed v of the vehicle's center of mass y yaw angular speed ω of the vehicle, yaw angle φ of the vehicle position X, Y of the vehicle's center of mass in the Cartesian coordinate system The control variables of the ego vehicle include: the external torques T df , T dr applied to the front and rear wheels of the vehicle, and the steering angle δ of the front wheels of the vehicle.

4. The integrated obstacle avoidance planning and control method for an autonomous vehicle of claim 3, wherein, the longitudinal force F to which the front and rear tires of the vehicle are subjected xf , F xr and the lateral force F to which the front and rear tires of the vehicle are subjected yf , F yr is obtained on the basis of a third expression which is: In the third expression, F x , F y are the longitudinal force and lateral force actually borne by the single tire of the vehicle, respectively, x0 is the longitudinal force borne by the tire in the pure slip condition obtained based on the fourth expression, y0 is the lateral force borne by the tire in the pure cornering condition obtained based on the fifth expression; The fourth expression is: F X0 = D x sin(C x arctan(B x *S-E x (B x S-arctan(B x *S)))) The fifth expression is: F y0 = D y sin(C y arctan(B y *α-E y (B y α-arctan(B y *α)))) In the third, fourth and fifth expressions, s is the tire slip ratio, and a is the tire side slip angle, the tire slip ratio s is obtained based on a sixth expression, and the tire side slip angle is obtained based on a seventh expression; In the fourth and fifth expressions, B, C, D and E are parameters of the tire model, which are fitted from tire data; The sixth expression is: In the sixth expression, ω w is the rotational angular velocity of the wheel obtained by the sensor, R e is the rolling radius of the wheel, v xw is the wheel center longitudinal velocity of the wheel obtained by the sensor; The seventh expression is: In the seventh expression, a f is a side slip angle of a front wheel tire of the vehicle, a r is a side slip angle of a rear wheel tire of the vehicle, δ f is a rotation angle of the front wheel obtained by a sensor, is a yaw angle of the vehicle, a is a distance from a mass center of the vehicle to a front axle of the vehicle, b is a distance from the mass center of the vehicle to a rear axle of the vehicle, v x , v y are a longitudinal velocity and a lateral velocity of the mass center of the vehicle, respectively.

5. The integrated obstacle avoidance planning and control method for autonomous vehicles of claim 1, wherein, The artificial potential field model representing the repulsive potential of the surrounding vehicle is obtained based on a ninth expression, the ninth expression is: In the ninth expression, x, y are the coordinate positions of the ego vehicle, x m , y m are the position coordinates of the mth surrounding vehicle, that is, the center position of the Gaussian function on the x-axis and the y-axis, corresponding to the peak value of the Gaussian function, h m is the amplitude of the Gaussian function, σ m is a parameter describing the influence range of the artificial potential field of the mth surrounding vehicle, that is, the standard deviation in the Gaussian function, σ m The value of σ depends on the length and width of the mth surrounding vehicle.

6. The integrated obstacle avoidance planning and control method for an autonomous vehicle of claim 5, wherein, The total cost function is obtained based on a fifteenth expression, the total cost function includes a total repulsive potential cost function represented by an eleventh expression, an initial trajectory following cost function represented by a twelfth expression, a steering cost function represented by a thirteenth expression, and a driving cost function represented by a fourteenth expression, wherein the total repulsive potential cost function is obtained based on a tenth expression; The fifteenth expression is: J COST = J1+ J2+ J Δcontrol + J APF The tenth expression is: The eleventh expression is: In the eleventh expression, P represents a weight of the total repulsion potential cost function, N p is the number of time steps in the prediction horizon, and t is the prediction time. The twelfth expression is: In the twelfth expression, represents the output of the ego vehicle prediction model at time k+i, r(k+i) represents the initial reference path given by the NMPC integrated planning controller at time k+i, which contains the information corresponding to the output of the ego vehicle prediction model at time k+i contains the information corresponding to, Q is used to represent the weight of the initial trajectory following cost function; The thirteenth expression is: In the thirteenth expression, represents the control amount increment input to the ego vehicle prediction model at the k+i time point, and R is used to represent the weight of the maneuver cost function. The fourteenth expression is: In the fourteenth expression, denotes the control amount increment input to the ego vehicle prediction model at the k+i time point, and S is used to represent the weight of the travel cost function.

7. The integrated obstacle avoidance planning and control method for autonomous vehicles of claim 1, wherein, When the obstacle avoidance boundary is smoothed by the boundary optimization function, a variable fraction is defined as the length of the transition area before and after the vehicle, and fraction*pi proportionally maps the x coordinate to the range of [0, π], as shown in the nineteenth expression: In the nineteenth expression, L is a road length, W is a road width, x start is a start position of a roundabout, L obstacle , W obstacle are a length and a width of the roundabout, respectively, L buffer is a length of a transition area, x trans_start , x trans_end are a start position and an end position of the transition area, respectively.

8. The integrated obstacle avoidance planning and control method for an autonomous vehicle of claim 1, wherein, The lateral velocity stability boundary of the vehicle is obtained based on a twenty-third expression, which is: In the twenty-third expression, m is the curb mass of the ego vehicle, ω ss is the yaw angular velocity of the mass center of the ego vehicle, F x is the longitudinal force of the tire of the ego vehicle, v x is the longitudinal velocity of the mass center of the ego vehicle.

9. The integrated obstacle avoidance planning and control method for an autonomous vehicle of claim 1, wherein, The side slip angle stability boundary of the vehicle is obtained based on a twenty-seventh expression, which is: In the twenty-seventh expression, a r,peak is the tire side slip angle when the lateral force of the rear tire of the vehicle reaches the limit, C α is the tire cornering stiffness of the rear tire of the vehicle, β ss is the side slip angle of the center of mass of the vehicle, b is the distance from the center of mass of the vehicle to the rear axle of the vehicle, v x is the longitudinal velocity of the center of mass of the vehicle, and ω is the yaw rate of the vehicle.

10. The integrated obstacle avoidance planning and control method for an autonomous vehicle of claim 1, wherein, When the solver is solved, a warm start strategy is used to optimize the solving process, that is, a series of solutions solved by the NMPC integrated planning controller in the previous prediction time domain is used as the initial value of the optimization problem constructed in the next prediction time domain for iterative optimization.

Images