Integrated obstacle avoidance planning and control method for autonomous vehicle

Abstract

The invention relates to an integrated obstacle avoidance planning and control method for an autonomous vehicle. The method comprises the following steps: an NMPC integrated planning controller obtains a dynamic time step length based on the longitudinal speed of the center of mass of the vehicle; the NMPC integrated planning controller constructs a self-vehicle dynamics model as a prediction model, and discretizes the prediction model; the NMPC integrated planning controller establishes a total cost function including the surrounding vehicle rejection potential energy; the NMPC integrated planning controller constructs an obstacle avoidance boundary of the vehicle; constructing a time-varying manipulation stability domain of the vehicle; and solving through a nonlinear optimization problem solver to obtain an optimal control variable sequence of the vehicle, and acting a first value in the optimal control variable sequence on the vehicle. The method has the beneficial effects that the stability and the safety of the automatic driving vehicle can be improved, and the automatic driving vehicle can accurately, timely and safely avoid obstacles.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to the field of autonomous driving obstacle avoidance technology, and specifically refers to an integrated obstacle avoidance planning and control method for an autonomous driving vehicle. Background Art

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

[0003] In the existing obstacle avoidance planning control methods for autonomous vehicles, most of them use algorithms with fixed prediction steps. However, for planning control problems based on NMPC integrated planning controllers, if the prediction time domain of the fixed step length is short, the MPC algorithm will lose the ability to utilize part of the future information, making the solution result short-sighted, and thus making it impossible for the autonomous driving system to make reasonable decisions and control vehicle actions in time for emergencies. For example, when the vehicle is traveling at a slow speed, the distance that the NMPC integrated planning controller can predict is shorter than when the vehicle is traveling at medium and high speeds. When the vehicle needs to quickly adjust its state to deal with emergencies such as obstacle avoidance and lane change, the autonomous driving system cannot generate feasible trajectories and control instructions in time, thus affecting the safety and comfort of the autonomous driving vehicle. If the prediction time domain of the fixed step length is expanded, the scale of the solution will be larger, affecting the efficiency and real-time performance of the solution, and also causing the autonomous driving system to be unable to complete the prediction and decision of the future state of the vehicle in time, and thus unable to control the vehicle in time, reducing the safety of the autonomous driving vehicle.

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

[0005] In view of the deficiencies in the prior art, the present invention provides an integrated obstacle avoidance planning and control method for an autonomous driving vehicle, which can improve the stability and safety of an autonomous driving vehicle when avoiding obstacles, and enable the autonomous driving vehicle 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] 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 based on the longitudinal velocity of the vehicle's center of mass;

[0008] Step 2, the NMPC integrated planning controller constructs a continuous self-vehicle dynamics model including self-vehicle control variables as a prediction model based on the inherent parameters of the self-vehicle and the state variables of the self-vehicle that continuously change with time, and discretizes the continuous self-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 vehicle through the "axial bounding box" according to the motion state of the surrounding vehicle and its predicted trajectory information, and smoothes the obstacle avoidance boundary through the boundary optimization function;

[0011] Step 5: based on the dynamic time step described in step 1, the continuous longitudinal velocity of the ego vehicle is discretized, and based on the yaw rate stability boundary of the ego vehicle and the sideslip angle stability boundary of the ego vehicle, a time-varying control stability domain of the ego vehicle is constructed within the entire speed variation range of the ego vehicle;

[0012] Step 6: Through the nonlinear optimization problem solver, with the goal of minimizing the total cost function, and with the obstacle avoidance boundary and the time-varying control stability domain of the ego vehicle as constraints, the sequential quadratic programming (SQP) algorithm is used to solve the optimal control variable sequence of the ego vehicle dynamics model. Then the first value in the optimal control variable sequence is applied to the ego vehicle to achieve closed-loop control feedback.

[0013] Compared with the prior art, the beneficial effects of the present invention are as follows: by setting the number of time steps in the prediction time domain to a fixed value, the solution scale of the NMPC integrated planning controller can be stabilized, and the real-time performance of the solution of the NMPC integrated planning controller is ensured; by setting the total prediction distance in the prediction time domain to a fixed value, the dynamic time step can be changed 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 of the vehicle is x When the time step increases, the dynamic time step can be reduced accordingly, and vice versa, the dynamic time step can be increased accordingly, thereby ensuring the full utilization of future information by the NMPC integrated planning controller, and avoiding the problem in the prior art that the vehicle cannot generate obstacle avoidance trajectories and obstacle avoidance control instructions in time when driving at low speeds due to the fixed time step;

[0014] By establishing a total cost function including the surrounding vehicle repulsion potential energy, it is possible to define what solution is the optimal solution for the NMPC integrated planning controller, 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's obstacle avoidance trajectory;

[0015] The obstacle avoidance boundary is smoothed by the boundary optimization function, which can optimize the obstacle avoidance boundary so that there is enough space in front and behind the surrounding vehicle for a smooth transition, so that the ego vehicle can have more time and space to adjust, preventing the ego vehicle's obstacle avoidance trajectory from having sudden changes, which is conducive to the NMPC integrated planning controller to solve the obstacle avoidance trajectory;

[0016] By discretizing the continuous longitudinal velocity of the ego vehicle, a time-varying control stability domain of the ego vehicle is constructed within the entire speed variation range of the ego vehicle, ensuring that the stability of the vehicle can be maintained in a dynamically changing environment, significantly improving the safety and comfort of the ego vehicle during obstacle avoidance.

[0017] Preferably, the dynamic time step is obtained based on a first expression, wherein 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 velocity of the center of mass of the vehicle, 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 all set fixed values.

[0020] Preferably, the continuous self-vehicle dynamics model is obtained based on a second expression, and the second expression is:

[0021]

[0022] In the second expression, the inherent parameters, state variables, control variables and the moment of inertia of the vehicle around the z-axis I are included. z , the moment of inertia of the front and rear wheels of the vehicle I wf ,I wr , the longitudinal force F on the front and rear tires of the vehicle xf 、F xr , the lateral force F on the front and rear tires of the vehicle yf 、F yr ;

[0023] The inherent parameters of the vehicle include: gravitational acceleration g, vehicle curb mass m, wheel rolling radius R e , the distance a from the center of mass of the vehicle to the front axle of the vehicle, and the distance b from the center of mass of the vehicle to the rear axle of the vehicle;

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

[0025] The control variables of the vehicle include: the external torque T acting on the front and rear wheels of the vehicle df 、T dr and the vehicle's front wheel turning angle δ.

[0026] Preferably, the longitudinal force F exerted on the front and rear tires of the vehicle is xf 、F xr And the lateral force F on the front and rear tires of the vehicle yf 、F yr Based on the third expression, the third expression is:

[0027]

[0028] In the third expression, F x 、F t are the longitudinal force and lateral force actually exerted on a single tire of the vehicle, respectively. x0 is the longitudinal force on the tire under pure slip conditions obtained based on the fourth expression, F y0 is the lateral force on the tire under pure cornering conditions obtained based on the fifth expression;

[0029] The fourth expression is:

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

[0031] The fifth expression is:

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

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

[0034] In the fourth and fifth expressions, B, C, D, and E are parameters of the tire model, obtained by fitting the tire data;

[0035] The sixth expression is:

[0036]

[0037] 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 longitudinal velocity of the wheel center obtained by the sensor;

[0038] The seventh expression is:

[0039]

[0040] In the seventh expression, α f is the side slip angle of the front tire of the vehicle, α r is the side slip angle of the rear tire of the vehicle, δ f is the turning angle of the front wheel obtained by the sensor, is the yaw angle of the vehicle, a is the distance from the center of mass of the vehicle to the front axle of the vehicle, b is the distance from the center of mass of the vehicle to the rear axle of the vehicle, v x 、v yare the longitudinal velocity and lateral velocity of the vehicle's center of mass, respectively.

[0041] The beneficial effect of adopting the above-mentioned preferred technical solution is that: through the third expression, the mixed slip characteristics of the tire, that is, the "adhesion ellipse" relationship between the tire and the ground can be accurately obtained, so that the dynamic model of the self-vehicle can fully consider the nonlinear dynamic characteristics of the tire force. It will have a great impact on the stability of the dynamic system of the self-vehicle, thereby improving the prediction accuracy of the self-vehicle dynamic model.

[0042] Preferably, the artificial potential field model characterizing the pericyte repulsive potential energy is obtained based on the ninth expression, which is:

[0043]

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

[0045] Preferably, the total cost function is obtained based on the fifteenth expression, and the total cost function includes the total repulsion potential energy cost function represented by the eleventh expression, the initial trajectory following cost function represented by the twelfth expression, the manipulation cost function represented by the thirteenth expression, and the driving cost function represented by the fourteenth expression, wherein the total repulsion potential energy cost function is obtained based on the 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 repulsive potential energy cost function, N pis the number of time steps in the prediction time domain, and 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 reference path given by the NMPC integrated planning controller at time k+i, and the information contained in it is consistent with the output of the ego vehicle prediction model y c The information contained corresponds to, Q is used to characterize the weight of the initial trajectory followability cost function;

[0056] The thirteenth expression is:

[0057]

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

[0059] The fourteenth expression is:

[0060]

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

[0062] Preferably, 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 pericar, and fraction*pi maps the x coordinate to the range of [0,π] in proportion, as shown in the nineteenth expression:

[0063]

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

[0065] Preferably, the vehicle yaw rate stability boundary is obtained based on a twenty-third expression, and the twenty-third expression is:

[0066]

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

[0068] Preferably, the sideslip angle stability boundary of the vehicle's center of mass is obtained based on the twenty-seventh expression, and the twenty-seventh expression is:

[0069]

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

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

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

[0073] Figure 2 It is a schematic diagram of the dynamic time step and the predicted distance in the present invention;

[0074] Figure 3 It is a schematic diagram of the artificial potential field of the pericycle in the present invention;

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

[0076] Figure 5 Schematic diagram of obstacle avoidance boundary in the present invention;

[0077] Figure 6 It is a schematic diagram of the time-varying control stability domain of the vehicle in the present invention;

[0078] Figure 7Schematic diagram of obstacle avoidance trajectory in the present invention;

[0079] Figure 8 is a curve diagram of longitudinal speed variation of the vehicle during obstacle avoidance in the present invention;

[0080] Fig. 9 It is a curve diagram of the yaw angle change of the vehicle during the obstacle avoidance process in the present invention. DETAILED DESCRIPTION

[0081] The technical solutions in the embodiments of the present invention are described clearly and completely below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0082] Embodiment 1:

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

[0084] Step 1: The NMPC integrated planning controller obtains the longitudinal velocity v of the center of mass of the vehicle through sensors. x , and obtain the dynamic time step Ts(i) based on the 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 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;

[0088] The total prediction distance L in the first expression and the number of time steps N in the prediction time domain p are all set 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 predicted distance of each time step is the same, that is, when the longitudinal speed of the vehicle v x When the dynamic time step Ts(i) increases, the dynamic time step Ts(i) can be reduced accordingly. Conversely, the dynamic time step Ts(i) can be increased accordingly. In this embodiment, preferably, the total predicted distance L takes the maximum detection range of the environment perception sensors such as the laser radar and camera on the autonomous driving vehicle to maximize the use of future information. Since the number of time steps N in the prediction time domain pis a fixed value, thus stabilizing the solution scale of the NMPC integrated planning controller and ensuring the real-time performance of the solution of the NMPC integrated planning controller. By fixing the total prediction distance L, the ability of the NMPC integrated planning controller to fully utilize future information is guaranteed.

[0089] Step 2: construct a continuous ego-vehicle dynamics model as a prediction model in the NMPC integrated planning controller, and discretize the continuous ego-vehicle dynamics model according to the dynamic time step Ts(i);

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

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

[0092] The state variables of the ego vehicle include: the longitudinal velocity v of the vehicle's center of mass x , lateral velocity v of the vehicle's center of mass y , the vehicle's yaw rate ω, the vehicle's yaw angle The position X, Y of the vehicle's center of mass in the Cartesian coordinate system;

[0093] In the NMPC integrated planning controller, a continuous ego-vehicle dynamics model including ego-vehicle intrinsic parameters, ego-vehicle state variables, and control variables is constructed based on the second expression. 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 ,I wr are the rotational inertia of the front and rear wheels of the vehicle, T df 、T dr are the external moments acting on the front and rear wheels of the vehicle, F xf 、F xr are the longitudinal forces on the front and rear tires of the vehicle, respectively, and F yf 、F yr are the lateral forces on the front and rear tires of the vehicle, respectively, and δ is the front wheel turning angle of the vehicle;

[0096] Among them, the external torque T on the front and rear wheels of the vehicle df 、T dr , the front wheel steering angle δ of the vehicle is the control variable of the vehicle;

[0097] Since the nonlinear dynamic characteristics of tire force will have a great impact on the stability of the vehicle's dynamic system, when analyzing the stability of the vehicle's dynamic system, in order to fully describe the lateral and longitudinal coupling dynamic characteristics of the vehicle, it is necessary to fully consider the mixed slip characteristics of the tire, that is, the "adhesion ellipse" relationship between the tire and the ground. Therefore, the actual longitudinal force F exerted on a single tire of the vehicle is obtained through the third expression: x , lateral force F y , the third expression is:

[0098]

[0099] In the third expression, s is the tire slip rate, α is the tire side slip angle, F x0 is the longitudinal force on the tire under pure slip conditions, F y0 is the lateral force on the tire under pure cornering conditions;

[0100] In this embodiment, the influence of wheel camber angle and self-aligning torque is ignored, and the Pacejka model is used to calculate F x0 、F y0 Calculate the longitudinal force F on the tire under pure slip conditions in the third expression x0 , the lateral force F exerted on the tire under pure cornering conditions y0 are obtained through the fourth expression and the fifth expression respectively;

[0101] The fourth expression:

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

[0103] The fifth expression:

[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 obtained by fitting the tire data, s is the tire slip rate, and α is the tire side slip angle;

[0106] In the fourth and fifth expressions, B, C, D, and E are all parameters of the tire model, which are obtained by fitting the tire data. Specifically, B is a stiffness factor, which is related to the deformation stiffness of the tire, C is a shape factor of the tire, which affects the shape of the fourth and fifth expressions and the smoothness of the curve, D is a peak factor, which is related to the peak 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 the sixth expression, which is:

[0108]

[0109] The sixth expression is applicable to calculate the tire slip ratio s under all working conditions. 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 longitudinal velocity of the wheel center obtained by the sensor. In this embodiment, the longitudinal velocity of the wheel center v xw The longitudinal velocity v of the vehicle's center of mass x The values ​​are the same;

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

[0111]

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

[0113] When the dynamic model of the ego vehicle is constructed, based on the dynamic time step Ts(i), the continuous ego vehicle dynamic model shown in the second expression is discretized by the one-step Euler method shown in the eighth expression to obtain the iterative equation at time n+1 relative to time n. The eighth expression is:

[0114]

[0115] Step 3: Based on the predicted trajectory information of the pericar, an artificial potential field model representing the pericar repulsive potential energy is constructed through a probabilistic Gaussian function, and a total cost function including the pericar repulsive potential energy is established;

[0116] Specifically, the NMPC integrated planning controller obtains the trajectory information of the circumvehicle, that is, the position coordinates of the circumvehicle, according to the trajectory prediction algorithm of the circumvehicle. According to the trajectory information of the circumvehicle, the probability Gaussian function in the two-dimensional space is used to establish the artificial potential field model of the circumvehicle based on the ninth expression to characterize the repulsive potential energy around the circumvehicle. The ninth expression is:

[0117]

[0118] In the ninth expression, x and y are the coordinate positions of the vehicle, x m ,y m is the position coordinate of the mth vehicle, that is, the center position of the Gaussian function on the x-axis and y-axis, corresponding to the peak value of the Gaussian function, h m is the amplitude of the Gaussian function, by setting h m It can determine the peak value of the repulsive potential energy of the mth car, σ m is the parameter describing the influence range of the artificial potential field of the mth vehicle, that is, the standard deviation of the Gaussian function. By setting σ m Ability to adjust the influence range of the artificial potential field, σ m The value of depends on the length and width of the mth vehicle;

[0119] If there are multiple obstacles, the total repulsive potential energy is the accumulation of the repulsive potential energy 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. By adding the repulsive potential energy of the surrounding vehicle to 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 ego vehicle's obstacle avoidance trajectory. Adding the repulsive potential energy of the surrounding vehicle to the cost function, the eleventh expression is obtained:

[0122]

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

[0124] In order to make the vehicle return to the original driving trajectory as quickly as possible after avoiding obstacles and meet the control requirements of the controller to minimize the control amount and control increment, the following cost functions are also constructed:

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

[0126]

[0127] In the twelfth expression, represents the output of the ego vehicle prediction model at time k+i, which in this embodiment specifically is the ordinate of the ego vehicle's center of mass in the Cartesian coordinate system, the vehicle's yaw angle, and the longitudinal speed, i.e. r(k+i) represents the initial reference path given by the NMPC integrated planning controller at time k+i, and the information contained in it is consistent with the output of the ego vehicle prediction model. The information contained corresponds to It represents the error between the vehicle state obtained by the prediction model and the reference state initially given by the NMPC integrated planning controller at time k+i. Q is used to represent the weight of this cost function.

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

[0129]

[0130] In the thirteenth expression, represents the control quantity input to the prediction model of the ego vehicle at time k+i. In this embodiment, it is specifically the external torque of the front and rear wheels of the ego vehicle and the front wheel steering angle of the ego vehicle, that is, 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 quantity increment input to the ego vehicle prediction model at time k+i. In this embodiment, it specifically refers to the increment of the external torque of the front and rear wheels of the ego vehicle and the increment of the front wheel steering angle of the ego vehicle, that is, S is used to represent the weight of this cost function;

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

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

[0136] Step 4: Based on the motion state and 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 the trajectory information of the surrounding vehicle according to the trajectory prediction algorithm of the surrounding vehicle, and obtains the motion state including the state variables such as the speed, heading angle, acceleration, etc. of the surrounding vehicle through sensors;

[0138] On a structured road without a surrounding vehicle, the lane boundary is the drivable area of ​​the vehicle. When a surrounding vehicle appears on the road, the area within the road boundary except the expansion boundary of the surrounding vehicle is the drivable area of ​​the vehicle, that is, the obstacle avoidance boundary. In order to calculate the obstacle avoidance boundary of the vehicle, the present invention uses an "axial bounding box" to represent the boundary of the surrounding vehicle. According to the predicted trajectory of the surrounding vehicle and combined with the road boundary, the obstacle avoidance boundary where the vehicle can drive is constructed.

[0139] The axis-aligned bounding box (AABB) is a minimum rectangular box that is always aligned with the coordinate axis. It is used to represent the perimeter and to easily determine whether two axial bounding boxes intersect. When two axial bounding boxes are close or intersecting, the two axial bounding boxes can be merged through the sixteenth, seventeenth, and eighteenth expressions. In two-dimensional space, the calculation of the rectangular box of each axial bounding box can be defined by a minimum point (usually the point in the lower left corner) and a maximum point (usually the point in the upper right corner). If there are two axial bounding boxes, A and B, the sixteenth expression is:

[0140]

[0141] When the rectangular boxes of A and B intersect or are very close to each other, the boundaries of A and B need to be readjusted when constructing the obstacle avoidance boundary, and then A and B are merged into a new axial bounding box;

[0142] When merging A and B, firstly, the coordinates of the minimum point of the new axial bounding box, that is, the point at the lower left corner, need to be determined by the seventeenth expression, and then the coordinates of the maximum point of the new axial bounding box, that is, the point at the upper right corner, need to be determined by the eighteenth expression;

[0143] When determining the minimum point of the new axial bounding box, first take the minimum value of each coordinate component of the minimum point (lower left corner point) of the two axial bounding boxes A and B respectively. Assume that the minimum point of axial bounding box A is min A =(x 1A ,y 1A ), the minimum point of the axial bounding box B is min B =(x 1B ,y 1B ), then the seventeenth expression for representing the coordinates of the minimum point of the new axial bounding box is:

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

[0145] When determining the maximum point of the new axial bounding box, first take the maximum value of each coordinate component of the maximum point (upper right corner point) of the two axial bounding boxes A and B respectively. Assume that the maximum point of the axial bounding box A is max A =(x 2A ,y 2A ), the maximum point of the axial bounding box B is max B =(x 2B ,y 2B ), then 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] By using the minimum and maximum points of the new axial bounding box, a new two-dimensional rectangular box that completely contains the two original axial bounding boxes A and B can be defined. The new two-dimensional rectangular box can be further resized or positioned 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 the path and avoid collision with the dynamic surrounding vehicle.

[0148] In order to allow the surrounding vehicle to have enough space in front and behind for a smooth transition, so that the self-vehicle can have more time and space for adjustment, the present invention optimizes the obstacle avoidance boundary using the boundary optimization function based on the nineteenth expression;

[0149] Specifically, the variable fraction is defined as the length of the transition area before and after the pericycle, and fraction*pi maps the x coordinate to the range of [0,π] in proportion to ensure that the transition starts and ends at the correct position. The nineteenth expression is:

[0150]

[0151] In the nineteenth expression, L is the length of the road, W is the width of the road, and x start is the starting position of the pericycle, L obstacle , W obstacle are the length and width of the perimeter vehicle, L buffer is the length of the transition region, x trans_start 、x trans_end are the starting and ending positions of the transition area respectively;

[0152] By optimizing the nineteenth expression, the available space can be utilized more effectively, which provides a basis for further optimizing the solution of the feasible domain by the NMPC integrated planning controller. When the boundary of the axial bounding box is used as a constraint, the mutation of the constraint is reduced, which is beneficial to the solution of the optimization problem in MPC.

[0153] Step 5, obtaining the time-varying control stability domain of the ego vehicle, and constructing the time-varying control 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 control stability domain of the ego vehicle is based on the stability boundary of the ego vehicle's yaw rate and the stability boundary of the ego vehicle's center of mass sideslip angle;

[0155] When obtaining the stability boundary of the yaw rate of the ego vehicle, 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 20th expression, the limit of the yaw rate of the vehicle is as shown in the 21st expression:

[0158]

[0159] At the same time, based on the current acceleration of the vehicle and the longitudinal force F of the tires x , the maximum lateral force that the vehicle tire can provide is shown in the twenty-second expression:

[0160]

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

[0162]

[0163] In the 20th, 21st, 22nd, and 23rd expressions, m is the curb weight of the vehicle, ω ss is the yaw rate of the vehicle’s center of mass, F yf 、F yr are the lateral forces on the front and rear wheels of the vehicle, v x is the longitudinal velocity of the vehicle’s center of mass;

[0164] Since the main factor causing the unstable side slip phenomenon such as tail swinging of the vehicle's center of mass is usually the side slip of the vehicle's rear wheels, in this embodiment, the side slip angle stability boundary of the vehicle's center of mass is characterized by the side slip angle stability boundary of the vehicle's rear wheels;

[0165] When obtaining the side slip angle stability boundary of the rear tire of the ego vehicle, firstly, according to the tire brush model in the Pacejka model as shown in the twenty-fourth expression, the tire side slip angle of the rear tire of the ego vehicle when the lateral force limit is reached is obtained. The twenty-fourth expression is:

[0166]

[0167] In the twenty-fourth expression, α is the side slip angle of the rear tire of the vehicle, C α is the cornering stiffness of the rear tire of the vehicle, μ is the adhesion coefficient between the rear tire of the vehicle and the road, and according to the twenty-fourth expression, the twenty-sixth expression representing the side slip angle α of the rear tire of the vehicle and the twenty-fifth expression representing the side slip angle α of the rear tire of the vehicle when the lateral force limit is reached 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 sideslip angle stability boundary of the rear tire of the ego vehicle, that is, the sideslip angle stability boundary of the ego vehicle's center of mass, is shown in the twenty-seventh expression:

[0173]

[0174] For a linear dynamic model in which the longitudinal velocity of the ego vehicle is constant, the time-varying control stability domain of the ego vehicle can be characterized by the stability boundary of the yaw angular velocity of the ego vehicle as shown in the twenty-third expression and the stability boundary of the sideslip angle of the ego vehicle's center of mass as shown in the twenty-seventh expression. However, for a three-degree-of-freedom or five-degree-of-freedom dynamic model in which the longitudinal velocity of the ego vehicle varies, the time-varying control stability domain of the ego vehicle will vary with the state of the ego vehicle, and as the longitudinal velocity of the ego vehicle increases, the time-varying control stability domain of the ego vehicle will tend to shrink. In order to more accurately characterize the time-varying control stability domain of the ego vehicle at different speeds, in this embodiment, according to the characteristics of the predictive control of the NMPC integrated planning controller, the idea of ​​"zero-order hold" in the control field is adopted, and the longitudinal velocity of the ego vehicle is discretized in the prediction time domain, that is, it is assumed that the longitudinal velocity of the ego vehicle remains unchanged in each dynamic time step, and the local stability domain of the ego vehicle is calculated at discrete speed points, and then the stability domain of the ego vehicle is constructed within the entire speed variation range of the ego vehicle. When the MPC controller is actually running, the MPC controller can quickly obtain the time-varying control stability domain of the ego vehicle at the corresponding speed by means of a hash table.

[0175] Step 6: Through the nonlinear optimization problem solver, with the goal of minimizing the total cost function, and with the obstacle avoidance boundary and the time-varying control stability domain of the ego vehicle as constraints, the sequential quadratic programming (SQP) algorithm is used to solve the optimal control variable sequence of the ego vehicle dynamics model. Then the first value in the optimal control variable sequence is applied to the ego vehicle to achieve closed-loop control feedback;

[0176] In order to further optimize the solution process, especially when facing scenarios where similar problems need to be solved frequently, this 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 used as the initial values ​​of the optimization problem constructed in the next prediction time domain, and iterative optimization is performed, thereby improving the utilization rate of the solution obtained in each dynamic time step and improving the solution speed and quality of the NMPC integrated planning controller.

[0177] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in this field should understand that they can still modify the technical solutions recorded in the above embodiments, or replace some of the technical features therein by equivalents; 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 invention.

Claims

1. An integrated obstacle avoidance planning and control method for an autonomous driving vehicle, characterized in that: The following steps are involved: 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 based on the longitudinal velocity of the vehicle's center of mass; Step 2, the NMPC integrated planning controller constructs a continuous self-vehicle dynamics model including self-vehicle control variables as a prediction model based on the inherent parameters of the self-vehicle and the state variables of the self-vehicle that continuously change with time, and discretizes the continuous self-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 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; Step 4: The NMPC integrated planning controller constructs the obstacle avoidance boundary of the vehicle through the "axial bounding box" according to the motion state of the surrounding vehicle and its predicted trajectory information, and smoothes the obstacle avoidance boundary through the boundary optimization function; Step 5: based on the dynamic time step described in step 1, the continuous longitudinal velocity of the ego vehicle is discretized, and based on the yaw rate stability boundary of the ego vehicle and the sideslip angle stability boundary of the ego vehicle, a time-varying control stability domain of the ego vehicle is constructed within the entire speed variation range of the ego vehicle; Step 6: Through the nonlinear optimization problem solver, with the goal of minimizing the total cost function, and with the obstacle avoidance boundary and the time-varying control stability domain of the ego vehicle as constraints, the sequential quadratic programming (SQP) algorithm is used to solve the optimal control variable sequence of the ego vehicle dynamics model. Then the first value in the optimal control variable sequence is applied to the ego vehicle to achieve closed-loop control feedback.

2. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 1, characterized in that: The dynamic time step is obtained based on a first expression, which 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 center of mass of the vehicle, 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 all set fixed values.

3. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 1, characterized in that: The continuous ego-vehicle dynamics model is obtained based on a second expression, which is: In the second expression, the inherent parameters, state variables, control variables and the moment of inertia of the vehicle around the z-axis I are included. z , the moment of inertia of the front and rear wheels of the vehicle I wf ,I wr , the longitudinal force F on the front and rear tires of the vehicle xf 、F xr , the lateral force F on the front and rear tires of the vehicle yf 、F yr ; The inherent parameters of the vehicle include: gravitational acceleration g, vehicle curb mass m, wheel rolling radius R e , the distance a from the center of mass of the vehicle to the front axle of the vehicle, and the distance b from the center of mass of the vehicle to the rear axle of the vehicle; The state variables of the ego vehicle include: the longitudinal velocity v of the vehicle's center of mass x , lateral velocity v of the vehicle's center of mass y , the vehicle's yaw rate ω, the vehicle's yaw angle The position X, Y of the vehicle's center of mass in the Cartesian coordinate system; The control variables of the vehicle include: the external torque T acting on the front and rear wheels of the vehicle df , T dr and the vehicle's front wheel turning angle δ.

4. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 3, characterized in that: The longitudinal force F exerted on the front and rear tires of the vehicle xf 、F xr And the lateral force F on the front and rear tires of the vehicle yf 、F yr Based on the third expression, the third expression is: In the third expression, F x 、F y are the longitudinal force and lateral force actually exerted on a single tire of the vehicle, respectively. x0 is the longitudinal force on the tire under pure slip conditions obtained based on the fourth expression, F y0 is the lateral force on the tire under pure cornering conditions 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 silver(B y *α-E y (B y α-Arctan(B y *α)))) In the third, fourth and fifth expressions, s is the tire slip rate, α is the tire side slip angle, the tire slip rate s is obtained based on the sixth expression, and the tire side slip angle is obtained based on the seventh expression; In the fourth and fifth expressions, B, C, D, and E are parameters of the tire model, obtained by fitting the 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 longitudinal velocity of the wheel center obtained by the sensor; The seventh expression is: In the seventh expression, α f is the side slip angle of the front tire of the vehicle, α r is the side slip angle of the rear tire of the vehicle, δ f is the turning angle of the front wheel obtained by the sensor, is the yaw angle of the vehicle, a is the distance from the center of mass of the vehicle to the front axle of the vehicle, b is the distance from the center of mass of the vehicle to the rear axle of the vehicle, v x 、v y are the longitudinal velocity and lateral velocity of the vehicle's center of mass, respectively.

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

6. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 5, characterized in that: The total cost function is obtained based on the fifteenth expression, and the total cost function includes the total repulsion potential energy cost function represented by the eleventh expression, the initial trajectory following cost function represented by the twelfth expression, the manipulation cost function represented by the thirteenth expression, and the driving cost function represented by the fourteenth expression, wherein the total repulsion potential energy cost function is obtained based on the 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 the weight of the total repulsive potential energy cost function, N p is the number of time steps in the prediction time domain, 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, and the information contained in it is consistent with the output of the ego vehicle prediction model The information contained corresponds to, Q is used to characterize the weight of the initial trajectory followability cost function; The thirteenth expression is: In the thirteenth expression, represents the control amount increment input to the self-vehicle prediction model at time k+i, and R is used to represent the weight of the manipulation cost function; The fourteenth expression is: In the fourteenth expression, represents the control amount increment input to the vehicle prediction model at time k+i, and S is used to characterize the weight of the driving cost function.

7. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 1, characterized in that: 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 pericar, and fraction*pi maps the x coordinate to the range of [0,π] in proportion, as shown in the nineteenth expression: In the nineteenth expression, L is the road length, W is the road width, and x start is the starting position of the pericycle, L obstacle , W obstacle are the length and width of the perimeter vehicle, L buffer is the length of the transition region, x trans_start 、x trans_end are the starting and ending positions of the transition area respectively.

8. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 1, characterized in that: The yaw rate stability boundary of the vehicle is obtained based on the twenty-third expression, which is: In the twenty-third expression, m is the curb weight of the vehicle, ω ss is the yaw rate of the vehicle’s center of mass, F x is the longitudinal force of the vehicle tire, v x is the longitudinal velocity of the vehicle's center of mass.

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

10. The integrated obstacle avoidance planning and control method for an autonomous driving vehicle according to claim 1, characterized in that: 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 are used as the initial values ​​of the optimization problem constructed in the next prediction time domain for iterative optimization.

Images