An autonomous driving vehicle decision-making and motion planning method considering safety margin constraints

Abstract

The present invention is applicable to the field of autonomous vehicle control technology, and provides a decision-making and motion planning method for autonomous vehicles considering safety boundary constraints, including the following steps: First, a behavior decision-making module is built, and path and speed information is applied to a motion planner. Then, a nonlinear prediction model is obtained from a three-degree-of-freedom vehicle coupling model. Considering vehicle physical constraints and safety boundary constraints, a cost function is constructed using a model predictive control algorithm and solved to complete the trajectory planning task. Finally, the obtained control input is applied to the vehicle system to achieve the control effect of vehicle trajectory tracking. The present invention integrates the decision-making, planning, and control modules on the basis of considering the safety boundary of vehicle driving, thereby effectively improving driving safety, reducing the interference brought by information deviation in decision-making and motion planning to the system, and effectively coping with the characteristics of strong nonlinearity and strong coupling of the system, meeting the requirements of the system for control accuracy and safety.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving vehicle control, and in particular relates to an autonomous driving vehicle decision-making and regulation control method taking into account safety boundary constraints. Background Art

[0002] The autonomous driving system mainly includes the following four modules: environmental perception, behavior decision, trajectory planning and tracking control. The behavior decision module makes decisions on vehicle behavior based on the road and obstacle information provided by environmental perception; the trajectory planning module plans the path ahead of the vehicle and the speed of the vehicle after receiving the corresponding decision information, thereby generating a trajectory with path and speed information; the tracking control module accurately tracks the expected trajectory by controlling the corresponding chassis actuators according to the expected driving trajectory given by the trajectory planning module and the status information fed back by the vehicle in real time. For autonomous driving vehicles, safe driving is always the top priority, and autonomous driving vehicles that consider safety boundary constraints can greatly improve the safety of the system. Safety boundary constraints include vehicle driving position safety constraints and vehicle motion state safety constraints. Establishing safety boundaries and applying them to autonomous driving vehicles is an effective way to improve driving safety. Therefore, it is extremely important for autonomous driving vehicles to enable each module of the system to operate in real time and accurately while ensuring safety as much as possible.

[0003] At present, the main problems for the safety of autonomous driving vehicles are as follows: 1. Since the environment around the vehicle is a highly dynamic environment, when the behavior decision-making, trajectory planning and path tracking hierarchical control method is adopted, if the decision-making planning information deviates from the actual information, the controller may cause poor vehicle tracking effect or even collision if it tracks according to the established route. These factors are not conducive to the driving safety of the vehicle; 2. For the vehicle planning and control module, the real-time vehicle driving position and motion state information play a vital role in driving safety, but the key indicators such as vehicle driving position safety constraints and vehicle motion state safety constraints are not fully considered in the conventional safety constraints of the traditional vehicle system, resulting in that it cannot fully guarantee driving safety. At the same time, due to the complex structure of the vehicle system, it exhibits strong nonlinearity and high coupling of multi-dimensional motion. The controller established based on the vehicle linear model ignores many dynamic characteristics and is difficult to meet the control accuracy requirements of the system. Therefore, the vehicle safety is also difficult to be fully guaranteed. For this reason, this application proposes a decision-making and regulation method for autonomous driving vehicles considering safety boundary constraints. Summary of the invention

[0004] The purpose of the present invention is to provide a decision-making and control method for an autonomous driving vehicle taking into account safety boundary constraints, aiming to solve the problems raised in the above-mentioned background technology.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] A decision-making and motion planning method for autonomous vehicles considering safety boundary constraints, comprising the following steps:

[0007] Step S1: Build a behavior decision-making module. The behavior decision-making module based on the dynamic programming algorithm samples the road ahead of the vehicle, maps the sampled points onto the S-L and S-T diagrams, connects the sampled points with a quintic polynomial, constructs a corresponding cost function, solves the cost function with the dynamic programming algorithm, and applies the obtained path decision information and speed decision information to the motion planner;

[0008] Step S2: Obtain a lateral-longitudinal coupled non-linear prediction model from the vehicle three-degree-of-freedom coupled model. Select the front wheel steering angle and the front and rear wheel driving forces as control variables. Considering the vehicle physical constraints and safety boundary constraints, use the model predictive control algorithm to construct a cost function and solve it to obtain a control signal, thereby completing the trajectory planning task;

[0009] Step S3: Apply the obtained control signal to the vehicle system to achieve the control effects of vehicle trajectory planning and path tracking.

[0010] Further, in the above step S1, the specific method of path decision-making is as follows:

[0011] Sample equidistant path points in the lateral and longitudinal directions of the road ahead of the vehicle, and smoothly connect the sampled points in adjacent columns with a quintic polynomial curve; make a decision on the vehicle driving path based on the center line of the current lane in the Frenet coordinate system, so that the vehicle travels along a collision-free path;

[0012] The following equation holds for the quintic polynomial based on the longitudinal path sampling:

[0013]

[0014] After constructing the quintic polynomial curve, evaluate the quality of the path by summing the cost functions. The total cost function is a linear combination of the smoothness, obstacle avoidance, and reference line cost functions. The formula for the total cost function is as follows:

[0015] C t (f) = C s (f) + C o (f) + C g (f)

[0016] Among them, C t (f) is the total cost function, which generally measures the quality of the path; C s (f) is the smoothness cost function, which functions to measure the smoothness of the path; C o(f) is the obstacle avoidance cost function, and its role is to measure the quality of the path obstacle avoidance effect; C g (f) is the reference line cost function, and its role is to measure the vehicle's ability to follow the lane reference line.

[0017] Furthermore, the smoothness cost function of the path is measured by the following formula:

[0018] C s (f) = w1∫(f′(s)) 2 ds + w2∫(f″(s)) 2 ds

[0019] where w1 and w2 are the cost weight coefficients of the first derivative and the second derivative of the quintic polynomial of the path respectively;

[0020] The obstacle avoidance cost function of the path is set based on the distance between the obstacle and the vehicle. Let the distance be d, and the specific expression is as follows:

[0021]

[0022] where C n is a monotonically decreasing function, C c is the collision cost, d s is the safety distance, and d c is the dangerous distance;

[0023] The reference line cost function of the path is set as follows: When there are no obstacles around the path, the reference line is the center line of the path, and its function is defined as g(s). The reference line cost function is measured by the following formula:

[0024] C g (f) = ∫(f(s) - g(s)) 2 ds.

[0025] Furthermore, in the step S1, the dynamic programming algorithm is used to transform the multi-stage decision-making problem into a series of single-stage optimization problems, and solve them step by step to complete the decision-making process, solve the undetermined coefficients, and thus obtain the path decision-making information. The specific method is as follows:

[0026] Take the path sampling points in front of the vehicle as the research object;

[0027] Because the calculation result of the cost from the starting point to the sampling point in the i-th column is based on the sum of the total costs of all sampling points from the starting point to the (i - 1)-th column, the total cost from the starting point to each column of path sampling points is regarded as a stage;

[0028] Transform each stage decision-making problem into a single-stage optimization problem to obtain the path information with the minimum cost.

[0029] Further, in the step S1, the specific method of speed decision-making is as follows:

[0030] Discretize the obstacle information into rectangular borders on the S-T graph, and represent (t0, t1, …, t n ) as equally spaced points on the time axis with an interval of dt; the piecewise linear speed distribution function is represented as S = (s0, s1, …, s n ); make a decision on the vehicle speed based on the center line of the current lane in the Frenet coordinate system, and gradually solve it using the dynamic programming algorithm to complete the decision-making process, find the undetermined coefficients, and obtain the speed decision information;

[0031] The following equation holds for the fifth-order polynomial based on the lateral speed sampling:

[0032]

[0033] After constructing the fifth-order polynomial curve, evaluate the speed magnitude by summing the cost functions. The total cost function is a linear combination of the smoothness, obstacle avoidance, and reference speed cost functions. The formula for the total cost function is as follows:

[0034] C t (S) = C s (S) + C o (S) + C g (S)

[0035] Among them, C t (S) is the total cost function, which generally measures the rationality of the current speed; C s (S) is the speed smoothness cost function, whose role is to measure the smoothness of the speed change; C o (S) is the obstacle avoidance cost function, which is used to characterize the speed change process during obstacle avoidance; C g (S) is the reference speed cost function, which is used to measure the vehicle's ability to follow the reference speed.

[0036] Further, the smoothness cost function of the speed is measured by the following formula:

[0037]

[0038] The obstacle avoidance cost function of the speed is based on the distance between the obstacle and the vehicle in the S-T graph, and its expression is the same as that of the obstacle avoidance cost function of the path;

[0039] The expression of the reference speed cost function is as follows:

[0040]

[0041] The reference speed cost function indicates that when there are no obstacles or traffic light restrictions, the vehicle should follow the specified speed, Vref Describes the reference speed determined by road speed limits, curvature, and other traffic regulations.

[0042] Furthermore, in step S2, two front wheels and two rear wheels of the vehicle planar motion dynamics model are respectively replaced by an equivalent front wheel and rear wheel in the axial direction of the vehicle to obtain a vehicle three-degree-of-freedom coupling model; the longitudinal vehicle speed, lateral vehicle speed, and yaw motion equations are as follows:

[0043]

[0044] Among them, v x is the longitudinal speed, v y is the lateral speed, ω r is the yaw angular velocity, δ is the front wheel steering angle, F xr is the rear wheel longitudinal force, F xf is the front wheel longitudinal force, F yr is the rear wheel lateral force, F yf is the front wheel lateral force, m is the vehicle body mass, l f is the front axle distance, l r is the rear axle distance, I z is the moment of inertia;

[0045] The kinematic equation of the vehicle in the earth coordinate system is as follows:

[0046]

[0047] Among them, X and Y are the longitudinal and lateral positions respectively, and ψ is the yaw angle;

[0048] The tire brush model is used to calculate the tire lateral force, and its calculation formula is as follows:

[0049]

[0050] Among them, F y is the tire lateral force, C a is the cornering stiffness, α is the tire sideslip angle, μ is the road adhesion coefficient, F z is the vertical load of the vehicle tire;

[0051] The calculation formulas for the front wheel sideslip angle and the rear wheel sideslip angle are as follows:

[0052]

[0053] Among them, α f and α r are the front wheel sideslip angle and the rear wheel sideslip angle respectively.

[0054] Furthermore, in step S2, the design steps of the nonlinear model predictive programming controller considering safety boundary constraints are as follows:

[0055] Build a vehicle controller prediction model. The vehicle controller prediction model is obtained through a three-degree-of-freedom bicycle model of the vehicle as follows:

[0056]

[0057] Select the longitudinal velocity v x and the lateral velocity v y , the yaw angle ψ, the yaw angular velocity ω r , the sideslip angle β of the center of mass, the lateral displacement X in the earth coordinate system, and the longitudinal displacement Y in the earth coordinate system as state variables, that is:

[0058]

[0059] Assume that all state variables are measurable, the front wheel steering angle δ of the vehicle, and the longitudinal force F x of the vehicle are control variables, that is:

[0060] u = [δ, F x

[0061] Use the Euler method to discretize the controller prediction model. T s is the sampling time. At time k, the discretized prediction model is:

[0062]

[0063] Denote N p and N c as the prediction horizon and the control horizon respectively, and satisfy N c ≤ N p ; then at time k, there is the following sequence:

[0064]

[0065] Among them, U(k) is the system control sequence, is the state sequence; u(k|k), u(k + 1|k), …, u(k + N c - 1|k) are the predictions of the control variable u at time k for times k, k + 1, …, k + N c - 1 respectively; are the predictions of the state variables p at times k, k + 1, …, k + N - 1 at time k respectively;

[0066] The objective function of the controller is:

[0067]

[0068] ​Among them, Y(k+i|k) is the prediction of the longitudinal position Y at the k-th moment for the (k+i)-th moment; Y ref (k+i) is the reference longitudinal position at the (k+i)-th moment; v x (k+i|k) is the prediction of the longitudinal velocity v at the k-th moment for the (k+i)-th moment x ; v xref (k+i) is the reference longitudinal velocity at the (k+i)-th moment; ψ(k+i|k) is the prediction of the yaw angle ψ at the k-th moment for the (k+i)-th moment; ψ ref (k+i) is the reference yaw angle at the (k+i)-th moment; Δu(k+i|k) is the predicted increment of the control variable u at the k-th moment for the (k+i)-th moment; X(k+i|k) is the prediction of the lateral position X at the k-th moment for the (k+i)-th moment; X obs (k+i) is the lateral position of the obstacle at the (k+i)-th moment; Y obs (k+i) is the longitudinal position of the obstacle at the (k+i)-th moment; The first three terms of the objective function characterize the tracking ability of the system for the reference path and speed information given by the decision-making module, the fourth term characterizes the smoothness of the system, and the fifth term is the collision function of the obstacle

[0069] Furthermore, the safety boundary constraint of the vehicle driving position is as follows:

[0070]

[0071] Among them, d c is the vehicle body width, Y(k+i|k) is the prediction of the longitudinal position Y at the k-th moment for the (k+i)-th moment; Y ref (k+i) is the reference longitudinal position at the (k+i)-th moment; Y l (k+i|k) is the prediction of the left safety position boundary line at the k-th moment for the (k+i)-th moment; Y r (k+i|k) is the prediction of the right safety position boundary line at the k-th moment for the (k+i)-th moment; During the vehicle driving process, the position boundary of the relative change of the vehicle position is obtained through real-time calculation;

[0072] According to vehicle dynamics, the maximum yaw rate and the maximum centroid side slip angle are obtained by the following formula:

[0073]

[0074] Among them, μ is the road surface adhesion coefficient, α rs is the saturation value of the rear wheel side slip angle; ω rmax (k+i|k) is the predicted maximum value of the yaw rate at the k-th moment for the (k+i)-th moment; β max (k+i|k) is the predicted maximum value of the centroid side slip angle at the k-th moment for the (k+i)-th moment; v x(k+i|k) is the prediction of the longitudinal velocity at time k+i at time k; ω r (k+i|k) is the prediction of the yaw rate ω at time k+i at time k r .

[0075] The safety boundary constraints of the vehicle motion state are as follows:

[0076]

[0077] Assume that μ, ω r and v x can be estimated in real time, then the boundary line of the safety boundary of the vehicle motion state can be obtained by real-time calculation; where, ω r (k+i|k) is the prediction of the yaw rate ω at time k+i at time k r ; β(k+i|k) is the prediction of the sideslip angle β of the center of mass at time k+i at time k; the change values of the front wheel steering angle δ and the vehicle driving force F x are all between the allowed minimum and maximum values;

[0078] u min ≤u(k+i|k)≤u max , i = 0, 1,..., N c -1

[0079] where, u(k+i|k) is the prediction of the control quantity u at time k+i at time k; u min and u max are defined as follows:

[0080]

[0081] where, δ min and δ max are the minimum and maximum values of the front wheel steering angle respectively, and F xmin and F xmax are the minimum and maximum values of the vehicle driving force respectively.

[0082] Furthermore, the vehicle trajectory planning and path tracking control problem is described as an optimization problem in the following form:

[0083]

[0084]

[0085] Numerical optimization is solved online at each sampling moment to achieve the control objectives of the planning control system.

[0086] Compared with the prior art, the beneficial effects of the present invention are:

[0087] 1. The integrated method of decision-making, planning, and control adopted by the present invention can significantly reduce the interference caused by information deviation among the decision-making, planning, and control modules on the basis of effectively improving the driving safety of the vehicle, thereby further enhancing the system safety.

[0088] 2. On the premise of considering the safety constraints of the vehicle driving position and motion state, the present invention designs a non-linear model predictive planning controller that integrates the planning and control modules, so as to effectively cope with the characteristics of strong non-linearity and strong coupling of the system and meet the requirements of the system for control accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 is the overall vehicle control block diagram in the present invention.

[0090] Figure 2 is the three-degree-of-freedom coupling model of the vehicle, serving as the controller design model.

[0091] Figure 3 is the position relationship diagram of the vehicle with dynamic and static obstacles preset in Prescan.

[0092] Figure 4 is the path decision result diagram of the vehicle for static obstacles.

[0093] Figure 5 is the speed decision result diagram of the vehicle for static and dynamic obstacles.

[0094] Figure 6 is the vehicle trajectory tracking curve.

[0095] Figure 7 is the actual vehicle speed curve. DETAILED DESCRIPTION OF THE INVENTION

[0096] In order to make the objectives, technical solutions, and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0097] The following describes the specific implementation of the present invention in detail with reference to specific embodiments.

[0098] As Figure 1 and Figure 2 shown, a decision-making and planning and control method for an autonomous driving vehicle considering safety boundary constraints provided by an embodiment of the present invention includes the following steps:

[0099] Step S1: Build a behavior decision-making module. The behavior decision-making module based on the dynamic programming algorithm samples the road in front of the vehicle, maps the sampled points onto the S-L and S-T diagrams, connects the sampled points with a quintic polynomial, constructs a corresponding cost function, solves the cost function with the dynamic programming algorithm, and applies the obtained path decision information and speed decision information to the planning controller.

[0100] Step S2: Obtain a non-linear prediction model of lateral and longitudinal coupling from the vehicle's three-degree-of-freedom coupling model. Select the front-wheel steering angle and the front and rear wheel driving forces as control variables. Considering the vehicle's physical constraints and safety boundary constraints, use the model predictive control algorithm to construct a cost function and solve it to obtain a control signal, thus completing the trajectory planning task.

[0101] Step S3: Apply the obtained control signal to the vehicle system to achieve the control effects of vehicle trajectory planning and path tracking.

[0102] As a preferred embodiment of the present invention, in step S1, the specific method of path decision-making is as follows:

[0103] Sample equidistant path points horizontally and vertically on the road in front of the host vehicle, and smoothly connect the sampled points between adjacent columns with a quintic polynomial curve; make a decision on the vehicle driving path based on the center line of the current lane in the Frenet coordinate system to enable the vehicle to drive along a collision-free path.

[0104] The following equation holds for the quintic polynomial based on longitudinal path sampling:

[0105]

[0106] After constructing the quintic polynomial curve, evaluate the quality of the path by summing the cost functions. The total cost function is a linear combination of the smoothness, obstacle avoidance, and reference line cost functions. The formula for the total cost function is as follows:

[0107] C t (f) = C s (f) + C o (f) + C g (f) (2)

[0108] Among them, C t (f) is the total cost function, which generally measures the quality of the path; C s (f) is the smoothness cost function, which functions to measure the smoothness of the path; C o (f) is the obstacle avoidance cost function, which functions to measure the quality of the path obstacle avoidance effect; C g (f) is the reference line cost function, which functions to measure the vehicle's ability to follow the lane reference line.

[0109] As a preferred embodiment of the present invention, the smoothness cost function of the path is measured by the following formula:

[0110] C s (f) = w1∫(f′(s)) 2 ds + w2∫(f″(s)) 2 ds (3)

[0111] where w1 and w2 are the cost weight coefficients of the first derivative and the second derivative of the quintic polynomial of the path respectively;

[0112] The obstacle avoidance cost function of the path is set based on the distance between the obstacle and the vehicle. Express the distance as d, and the specific expression is as follows:

[0113]

[0114] where C n is defined as a monotonically decreasing function, C c is the collision cost, which has a very large value and helps to detect the infeasible path of the vehicle, d s is the safety distance, and d c is the danger distance;

[0115] The reference line cost function of the path is set as follows: When there is no obstacle around the path, the reference line is defined as the ideal driving path, and this line is usually extracted as the center line of the path. Define its function as g(s), and the reference line cost function is measured by the following formula:

[0116] C g (f) = ∫(f(s) - g(s)) 2 ds (5).

[0117] As a preferred embodiment of the present invention, in the step S1, the dynamic programming algorithm is used to transform the multi-stage decision-making problem into a series of single-stage optimization problems, and solve them step by step to complete the decision-making process, and solve the undetermined coefficients to obtain the path decision information. The specific method is as follows:

[0118] Take the path sampling points in front of the vehicle as the research object;

[0119] Because the calculation result of the cost from the starting point to the sampling points in the i-th column is based on the sum of the total costs of all sampling points from the starting point to the (i - 1)-th column, the total cost from the starting point to each column of path sampling points is regarded as a stage;

[0120] Transform each stage decision-making problem into a single-stage optimization problem to obtain the path information with the minimum cost.

[0121] As a preferred embodiment of the present invention, in the step S1, the specific method for speed decision-making is as follows:

[0122] Discretize the obstacle information into rectangular borders on the S-T graph, and represent (t0, t1, …, t n ) as equally spaced points on the time axis with an interval of dt; the piecewise linear speed distribution function is represented as S = (s0, s1, …, s n ); make a decision on the vehicle speed based on the center line of the current lane in the Frenet coordinate system, and use the dynamic programming algorithm to gradually solve to complete the decision-making process, find the undetermined coefficients, and obtain the speed decision information;

[0123] The following equation holds for the fifth-degree polynomial based on the lateral speed sampling:

[0124]

[0125] After constructing the fifth-degree polynomial curve, evaluate the speed magnitude by summing the cost functions. The total cost function is a linear combination of the smoothness, obstacle avoidance, and reference speed cost functions. The formula for the total cost function is as follows:

[0126] C t (S) = C s (S) + C o (S) + C g (S) (7)

[0127] Among them, C t (S) is the total cost function, which generally measures the rationality of the current speed; C s (S) is the speed smoothness cost function, whose role is to measure the smoothness of the speed change; C o (S) is the obstacle avoidance cost function, which is used to characterize the speed change process during obstacle avoidance; C g (S) is the reference speed cost function, which is used to measure the vehicle's ability to follow the reference speed.

[0128] As a preferred embodiment of the present invention, the smoothness cost function of the speed is measured by the following formula:

[0129]

[0130] The obstacle avoidance cost function of the speed is based on the distance between the obstacle and the vehicle in the S-T graph, and its expression is the same as formula (4);

[0131] The expression of the reference speed cost function is as follows:

[0132]

[0133] The reference speed cost function indicates that when there are no obstacles or traffic light restrictions, the vehicle should follow the specified speed, V ref Describes the reference speed determined by road speed limits, curvature, and other traffic regulations.

[0134] The dynamic programming algorithm is used to gradually solve to complete the decision-making process, find the undetermined coefficients, and obtain the speed decision information. Thus, the behavior decision-making module obtains the longitudinal and lateral path and speed information, and gives a reference quantity to the subsequent trajectory planning and tracking control module.

[0135] As a preferred embodiment of the present invention, in step S2, two front wheels and two rear wheels of the vehicle planar motion dynamics model are respectively replaced by an equivalent front wheel and rear wheel in the axial direction of the vehicle to obtain a vehicle three-degree-of-freedom coupling model; the longitudinal vehicle speed, lateral vehicle speed, and yaw motion equations are as follows:

[0136]

[0137] Among them, v x is the longitudinal speed, v y is the lateral speed, ω r is the yaw angular velocity, δ is the front wheel steering angle, F xr is the rear wheel longitudinal force, F xf is the front wheel longitudinal force, F yr is the rear wheel lateral force, F yf is the front wheel lateral force, m is the vehicle body mass, l f is the front wheelbase, l r is the rear wheelbase, I z is the moment of inertia;

[0138] The kinematic equation of the vehicle in the earth coordinate system is as follows:

[0139]

[0140] Among them, X and Y are the longitudinal and lateral positions respectively, and ψ is the yaw angle; the movement of the vehicle ultimately depends on the tire force, so how to effectively model the tire is of great significance. Typical tire models include the Uni-tire model, the Magic formula model, and the Tire Brush Models. Since high vehicle dynamics are required in motion planning and control, the Tire Brush Model is used to calculate the tire lateral force, and its calculation formula is as follows:

[0141]

[0142] Among them, F y is the tire lateral force, C ais the cornering stiffness, α is the tire cornering angle, μ is the road adhesion coefficient, and F z is the vertical load of the vehicle tire;

[0143] The calculation formulas for the front wheel cornering angle and the rear wheel cornering angle are as follows:

[0144]

[0145] where α f and α r are the front wheel cornering angle and the rear wheel cornering angle respectively.

[0146] In the embodiments of the present invention, considering that the present application needs to track the vehicle path and the coupling relationship between the longitudinal and lateral directions of the vehicle, when establishing the controller model, the longitudinal motion, the lateral motion and the yaw motion of the vehicle are mainly considered among the three degrees of freedom, and certain assumptions are made. In this model, it is considered that the vehicle body and the chassis are rigidly connected, and there is no roll and pitch motion of the vehicle body above the suspension. At the same time, the roll and pitch motions of the vehicle body caused by the longitudinal deformation displacement of the tire are also ignored. The influence of lateral and longitudinal aerodynamics on the yaw characteristics of the vehicle is ignored in the force analysis.

[0147] Since the vehicle has symmetry in the x-axis direction, the planar dynamic model of the vehicle can be simplified. The two front wheels and the two rear wheels of the planar motion dynamic model of the vehicle are respectively replaced by an equivalent front wheel and a rear wheel in the axis direction of the vehicle.

[0148] In the tire brush model, the vertical load F z of the vehicle tire can be calculated by the following formula:

[0149]

[0150] where F zf and F zr are the vertical loads of the front and rear wheels respectively, g is the acceleration due to gravity, and h cg is the height of the vehicle's center of mass. Thus, the design of the non-linear model predictive programming controller can be carried out according to this model.

[0151] As a preferred embodiment of the present invention, in the step S2, the design steps of the non-linear model predictive programming controller considering the safety boundary constraint are as follows:

[0152] First, establish a vehicle controller prediction model. The vehicle controller prediction model is obtained through the vehicle three-degree-of-freedom bicycle model as follows:

[0153]

[0154] Select the longitudinal speed v x and the lateral speed vy Yaw angle ψ, yaw rate ω r The sideslip angle β of the center of mass, the lateral displacement X in the geodetic coordinate system, and the longitudinal displacement Y in the geodetic coordinate system are used as state variables, that is:

[0155]

[0156] Assume that all state variables are measurable, the front wheel angle δ of the vehicle, and the longitudinal force F of the vehicle x are control variables, that is:

[0157] u = [δ, F x (17)

[0158] The Euler method is used to discretize the controller prediction model, and T s is the sampling time. At time k, the discretized prediction model is:

[0159]

[0160] Denote N p , N c as the prediction horizon and the control horizon respectively, and satisfy N c ≤ N p ; then at time k, there is the following sequence:

[0161]

[0162] where U(k) is the system control sequence, is the state sequence; u(k|k), u(k + 1|k), …, u(k + N c -1|k) are the predictions of the control variable u at time k for times k, k + 1, …, k + N c -1 respectively; are the predictions of the state variables p at times k, k + 1, …, k + N -1 at time k respectively;

[0163] Because the control objectives of the controller are to track the path and speed information of the vehicle's decision-making, maintain the vehicle's stability, and ensure that the vehicle can effectively avoid obstacles, the objective function of the controller is set as:

[0164]

[0165] where Y(k + i|k) is the prediction of the longitudinal position Y at time k + i at time k; Y ref (k + i) is the reference longitudinal position at time k + i; v x(k+i|k) is the prediction of the longitudinal velocity v at time k+i; x of; v xref (k+i) is the reference longitudinal velocity at time k+i; ψ(k+i|k) is the prediction of the yaw angle ψ at time k+i; ψ ref (k+i) is the reference yaw angle at time k+i; Δu(k+i|k) is the predicted increment of the control variable u at time k+i; X(k+i|k) is the prediction of the lateral position X at time k+i; X obs (k+i) is the lateral position of the obstacle at time k+i; Y obs (k+i) is the longitudinal position of the obstacle at time k+i; The first three terms of the objective function represent the tracking ability of the system for the reference path and speed information given by the decision-making module, the fourth term represents the smoothness of the system, and the fifth term is the collision function of the obstacle.

[0166] In the embodiment of the present invention, after obtaining the path and speed decision information given by the behavior decision-making module, the trajectory planning and tracking control module of the system needs to consider the vehicle position safety constraint and the vehicle motion state safety constraint, and handle the multi-variable problem of the system. Since model predictive control is a model-dependent design method, it can effectively handle systems with hard constraints and multiple variables. Considering the non-linear coupling characteristics between the longitudinal and lateral directions of the vehicle, this application applies a non-linear model predictive controller to handle the vehicle trajectory planning and path tracking control problems.

[0167] As a preferred embodiment of the present invention, the safety boundary constraint of the vehicle driving position is:

[0168]

[0169] where d c is the vehicle body width, Y(k+i|k) is the prediction of the longitudinal position Y at time k+i; Y ref (k+i) is the reference longitudinal position at time k+i; Y l (k+i|k) is the prediction of the left safety position boundary line at time k+i; Y r (k+i|k) is the prediction of the right safety position boundary line at time k+i; During the vehicle driving process, the position boundary of the relative change of the vehicle position is obtained by real-time calculation;

[0170] According to vehicle dynamics, the maximum yaw rate and the maximum centroidal side slip angle are obtained by the following formula:

[0171]

[0172] where μ is the road surface adhesion coefficient, α rsis the saturation value of the rear wheel sideslip angle; ω rmax (k+i|k) is the predicted maximum value of the yaw rate at time k+i at time k; β max (k+i|k) is the predicted maximum value of the center of mass sideslip angle at time k+i at time k; v x (k+i|k) is the prediction of the longitudinal velocity at time k+i at time k; ω r (k+i|k) is the prediction of the yaw rate ω at time k+i at time k r ;

[0173] According to Equation (22), the safety boundary constraints of the vehicle motion state are as follows:

[0174]

[0175] Assume that μ, ω r and v x can be estimated in real time, then the boundary line of the vehicle motion state safety boundary can be obtained by real-time calculation; among them, ω r (k+i|k) is the prediction of the yaw rate ω at time k+i at time k r ; β(k+i|k) is the prediction of the center of mass sideslip angle β at time k+i at time k; according to the actual system characteristics, the control quantity also needs to satisfy the conventional safety constraints shown in Equation (24), that is, the front wheel steering angle δ, the vehicle driving force F x The change values of are all between the allowed minimum and maximum values;

[0176] u min ≤u(k+i|k)≤u max , i = 0, 1,..., N c -1 (24)

[0177] Among them, u(k+i|k) is the prediction of the control quantity u at time k+i at time k; u min and u max The definitions of are as follows:

[0178]

[0179] Among them, δ min and δ max are the minimum and maximum values of the front wheel steering angle respectively, and F xmin and F xmax are the minimum and maximum values of the vehicle driving force respectively.

[0180] In the embodiments of the present invention, a vehicle has a relatively definite position safety boundary or safety area during driving. In different driving environments, the motion state parameters of vehicle driving stability also have a reasonable safety boundary. The position safety area formed by the vehicle safety boundary and the vehicle motion state safety area provide a large adjustment space for the change of the vehicle motion state. Defining the vehicle driving position safety boundary and the motion state safety boundary and designing a reasonable lane-changing control method can enable the vehicle to safely, stably and comfortably cope with different driving environments and further improve the robustness in dealing with time-varying environments.

[0181] As a preferred embodiment of the present invention, the vehicle trajectory planning and path tracking control problem is described as an optimization problem in the following form:

[0182]

[0183]

[0184] Numerical optimization is solved online at each sampling moment to achieve the control objectives of the planning control system.

[0185] Experimental verification

[0186] In order to verify the effectiveness of the present invention, the following simulation experiments are designed:

[0187] First, preset the driving scenario as shown in Figure 3 in Prescan, that is, the vehicle is driving on a straight road, there are two blue static obstacles and three red dynamic obstacles on the front road. These three dynamic obstacles move horizontally in a cyclic manner between the highway lanes at speeds of 3 m / s, 5 m / s and 7 m / s respectively. The two static obstacles are arranged in the bottom lane. Each sensor on the vehicle body detects the road conditions in real time and sends the corresponding environmental information to the behavior decision-making module to cope with emergencies;

[0188] Then, the behavior decision-making module makes corresponding path and speed decisions. Figure 4 It is the path decision result diagram of the vehicle for the static obstacle. Figure 5 It is the speed decision result diagram of the vehicle for the static and dynamic obstacles.

[0189] Finally, the vehicle plans and controls the front trajectory in real time, Figure 6 It is the vehicle trajectory tracking curve. Figure 7 It is the actual speed curve of the vehicle. In the obstacle avoidance experiments for static and dynamic obstacles, the vehicle maintains good decision-making and planning and control capabilities and ensures the driving safety of the vehicle in real time.

[0190] The above are only the preferred embodiments of the present invention. It should be noted that for those skilled in the art, without departing from the concept of the present invention, several modifications and improvements can be made, which should also be regarded as the protection scope of the present invention, and these will not affect the implementation effect of the present invention and the practicability of the patent.

Claims

1. A decision-making and motion planning method for autonomous vehicles considering safety margin constraints, characterized in that It includes the following steps: Step S1: Build a behavior decision-making module. The behavior decision-making module based on the dynamic programming algorithm samples the road ahead of the vehicle, maps the sampling points onto the S-L and S-T diagrams, connects the sampling points with a quintic polynomial, constructs a corresponding cost function, solves the cost function with the dynamic programming algorithm, and applies the obtained path decision information and speed decision information to the planning controller; Step S2: Obtain a lateral-longitudinal coupled non-linear prediction model from the vehicle three-degree-of-freedom coupled model. Select the front wheel steering angle and the front and rear wheel driving forces as control variables. Considering the vehicle physical constraints and safety boundary constraints, construct a cost function with the model predictive control algorithm and solve it to obtain a control signal, completing the trajectory planning task; Step S3: Apply the obtained control signal to the vehicle system to achieve the control effect of vehicle trajectory planning and path tracking; In the said step S1, the specific method of path decision-making is as follows: Sample equidistant path points transversely and longitudinally on the road ahead of the host vehicle, and smoothly connect the sampling points in adjacent columns with a quintic polynomial curve; Make a decision on the vehicle driving path based on the center line of the current lane in the Frenet coordinate system so that the vehicle travels along a collision-free path; The following equation holds for the quintic polynomial based on longitudinal path sampling: ； After constructing the quintic polynomial curve, evaluate the quality of the path by summing the cost functions. The total cost function is a linear combination of the smoothness, obstacle avoidance, and reference line cost functions. The formula for the total cost function is as follows: ； Among them, is the total cost function, which generally measures the quality of the path; is the smoothness cost function, whose function is to measure the smoothness of the path; is the obstacle avoidance cost function, whose function is to measure the quality of the obstacle avoidance effect of the path; is the reference line cost function, whose function is to measure the vehicle's ability to follow the lane reference line; In the said step S2, replace the two front wheels and two rear wheels of the vehicle planar motion dynamics model with an equivalent front wheel and rear wheel respectively in the axial direction of the vehicle to obtain the vehicle three-degree-of-freedom coupled model; The longitudinal vehicle speed, lateral vehicle speed, and yaw motion equations are as follows: ； Among them, is the longitudinal speed, is the lateral speed, is the yaw angular velocity, is the front wheel steering angle, is the longitudinal force of the rear wheel, is the longitudinal force of the front wheel, is the lateral force of the rear wheel, is the lateral force of the front wheel, m is the vehicle body mass, is the front wheelbase, is the rear wheelbase, is the moment of inertia; The kinematic equation of the vehicle in the earth coordinate system is as follows: ； Among them, and are the longitudinal and lateral positions respectively, is the yaw angle; Calculate the tire lateral force using the tire brush model, and its calculation formula is as follows: ； Among them, is the lateral force of the tire, is the cornering stiffness, is the tire slip angle, is the road surface adhesion coefficient, is the vertical load of the vehicle tire; The calculation formulas for the front wheel side slip angle and the rear wheel side slip angle are as follows: ； wherein, and are the front wheel slip angle and the rear wheel slip angle respectively; In the said step S2, the design steps of the non-linear model predictive planning controller considering the safety boundary constraints are as follows: Establish a vehicle controller prediction model. The vehicle controller prediction model is obtained from the vehicle three-degree-of-freedom bicycle model as follows: ； Select the longitudinal speed , lateral speed , yaw angle , yaw angular velocity , sideslip angle of the center of mass , lateral displacement in the geodetic coordinate system and longitudinal displacement in the geodetic coordinate system as state variables, namely: ； Assume that all state variables are measurable, and the front wheel angle of the vehicle , and the longitudinal force of the vehicle are control variables, that is: ； The Euler method is used to discretize the controller prediction model. is the sampling time, and at time , the discretized prediction model is: ； Denote \(N_p\) p and \(N_c\) c as the prediction horizon and the control horizon respectively, and they satisfy \(N_p\) c ≤ \(N_c\) p ; then at time instant \(k\), there is the following sequence: ； Among them, is the system control sequence, is the state sequence; , ,…, are respectively the predictions of the control quantity u at the k-th moment for the k-th moment, k + 1-th moment, …, k + N c - 1-th moment; , ,…, are respectively the predictions of the state quantity p at the k-th moment for the k-th moment, k + 1-th moment, …, k + N - 1-th moment; The objective function of the controller is: ； Among them, is the prediction of the longitudinal position at the (k + i)-th moment at the k-th moment ; is the reference longitudinal position at the (k + i)-th moment; is the prediction of the longitudinal velocity at the (k + i)-th moment at the k-th moment ; is the reference longitudinal velocity at the (k + i)-th moment; is the prediction of the yaw angle at the (k + i)-th moment at the k-th moment ; is the reference yaw angle at the (k + i)-th moment; is the predicted increment of the control quantity at the (k + i)-th moment at the k-th moment ; is the prediction of the lateral position at the (k + i)-th moment at the k-th moment ; is the lateral position of the obstacle at the (k + i)-th moment; is the longitudinal position of the obstacle at the (k + i)-th moment; The first three terms of the objective function characterize the tracking ability of the system for the reference path and speed information given by the decision-making module, the fourth term characterizes the stability of the system, and the fifth term is the collision function of the obstacle; The safety boundary constraint of the vehicle driving position is: ； where d c is the vehicle body width, is the prediction of the longitudinal position at the (k + i)-th moment at the k-th moment ; is the reference longitudinal position at the (k + i)-th moment; is the prediction of the left safety position boundary line at the (k + i)-th moment at the k-th moment; is the prediction of the right safety position boundary line at the (k + i)-th moment at the k-th moment; during the vehicle driving process, the position boundary of the relative change of the vehicle position is obtained through real-time calculation; From vehicle dynamics, the maximum yaw angular velocity and the maximum centroid side slip angle are obtained by the following formula: ； Among them, is the road surface adhesion coefficient, is the saturation value of the rear wheel sideslip angle; is the predicted maximum value of the yaw rate at the k + i moment at the k moment; is the predicted maximum value of the center-of-mass sideslip angle at the k + i moment at the k moment; is the prediction of the longitudinal speed at the k + i moment at the k moment; is the yaw rate at the k + i moment at the k moment prediction; The safety boundary constraints of the vehicle motion state are as follows: ； Assume , and can be estimated in real time, then the boundary line of the safety boundary of the vehicle motion state can be obtained through real-time calculation; where is the prediction of the yaw rate at the k + i moment at the k moment; is the prediction of the sideslip angle of the center of mass at the k + i moment at the k moment; the change values of the front wheel steering angle δ and the vehicle driving force are all between the allowed minimum and maximum values; ； Among them, is the prediction of the control quantity at the (k + i)-th moment at the k-th moment ; and are defined respectively as: ； wherein, and are the minimum and maximum values of the front wheel steering angle respectively, and are the minimum and maximum values of the vehicle driving force respectively.

2. The decision-making and motion planning method for an autonomous driving vehicle considering safety margin constraints according to claim 1, wherein The smoothness cost function of the path is measured by the following formula: ； Among them, , are the cost weight coefficients of the first derivative and the second derivative of the path quintic polynomial, respectively; The obstacle avoidance cost function of the path is set based on the distance between the obstacle and the vehicle. Represent the distance as d, and the specific expression is as follows: ； Among them, C n is a monotonically decreasing function, C c is the collision cost, d s is the safety distance, d c is the danger distance; The reference line cost function of the path is set as follows: When there is no obstacle around the path, the reference line is the center line of the path, and its function is defined as g(s). The reference line cost function is measured by the following formula: 。 3. The decision-making and motion planning method for autonomous vehicles considering safety margin constraints according to claim 1, characterized in that In the said step S1, use the dynamic programming algorithm to transform the multi-stage decision-making problem into a series of single-stage optimization problems, and gradually solve to complete the decision-making process, solve the undetermined coefficients, and thus obtain the path decision information. The specific method is as follows: Take the sampling points on the path in front of the vehicle as the research object; Since the calculation result of the cost from the starting point to the sampling points in the i-th column is based on the sum of the total costs of all sampling points from the starting point to the (i - 1)-th column, the total cost from the starting point to the sampling points in each column of the path is regarded as a stage; Convert the decision-making problem of each stage into a single-stage optimization problem to obtain the path information with the minimum cost.

4. The decision-making and motion planning method for an autonomous vehicle considering safety margin constraints according to claim 2, wherein In the step S1, the specific method of speed decision-making is as follows: Discretize the obstacle information into rectangular borders on the S-T diagram, and represent (t0, t1, …, t n ) as equally spaced points on the time axis with an interval of dt; represent the piecewise linear velocity distribution function as S = (s0, s1, …, s n ); make a decision on the vehicle's speed based on the center line of the current lane in the Frenet coordinate system, and use the dynamic programming algorithm to gradually solve to complete the decision-making process, find the undetermined coefficients, and obtain the speed decision information; The following equation holds for the fifth-order polynomial based on lateral speed sampling: ； After constructing the fifth-order polynomial curve, evaluate the speed magnitude by summing the cost functions. The total cost function is a linear combination of the smoothness, obstacle avoidance, and reference speed cost functions. The formula of the total cost function is as follows: ； Among them, is the total cost function, which generally measures the rationality of the current speed; is the speed smoothness cost function, which functions to measure the smoothness of speed changes; is the obstacle avoidance cost function, which is used to characterize the speed change process during obstacle avoidance; is the reference speed cost function, which is used to measure the vehicle's ability to follow the reference speed.

5. The decision-making and motion planning method for autonomous vehicles considering safety margin constraints according to claim 4, wherein The smoothness cost function of the speed is measured by the following formula: ； The obstacle avoidance cost function of the speed is based on the distance between the obstacle and the vehicle in the S-T diagram, and its expression is the same as that of the obstacle avoidance cost function of the path; The expression of the reference speed cost function is as follows: ； The reference speed cost function indicates that when there are no obstacles or traffic light restrictions, the vehicle should follow the specified speed. It describes the reference speed determined by road speed limits, curvature, and other traffic regulations.

6. The decision-making and motion planning method for an autonomous driving vehicle considering safety margin constraints according to claim 1, wherein The vehicle trajectory planning and path tracking control problem is described as an optimization problem in the following form: ； Perform numerical optimization and solution online at each sampling moment to achieve the control objective of the planning and control system.

Images