Driving path planning method for autonomous vehicle

Abstract

The invention discloses a driving path planning method for an automatic driving vehicle. The method comprises the following steps: step 1, acquiring a vehicle position at the current moment, obstacle positions around the vehicle and a planned path end point position; 2, constructing an artificial potential field function, wherein the artificial potential field function comprises a gravitational field function and a repulsive field function; 3, according to the gravitational field function and the repulsive force field function, the gravitational force and the repulsive force borne by the vehicle are obtained, and the resultant force borne by the vehicle is obtained according to the gravitational force and the repulsive force borne by the vehicle; 4, determining a target course angle of the vehicle according to the resultant force borne by the vehicle, and updating the position of the vehicle at the next moment according to the target course angle; repeating the step 2 to the step 4 until the vehicle arrives at the end point of the planned path; obtaining a vehicle position set; and 5, taking discrete points in the vehicle position set as an initial solution, and obtaining a smooth planned path based on a quadratic programming method.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle path planning, and in particular relates to a method for planning a driving path for an autonomous driving vehicle. Background Art

[0002] The artificial potential field method is easy to calculate, has strong real-time performance, and is easy to control at the bottom level. It is often used in local path planning, but there are still some problems with the artificial potential field method. For example, the artificial potential field method ignores the effect of repulsion due to excessive gravity near the starting point, causing collisions between smart cars and obstacles, and the appearance of local minimum points. These problems directly affect the safety of vehicle driving and the feasibility of path planning. Summary of the invention

[0003] The purpose of the present invention is to provide a method for planning a driving path for an autonomous vehicle, which can improve the safety and feasibility of path planning.

[0004] The technical solution provided by the present invention is:

[0005] A method for planning a driving path for an autonomous vehicle comprises the following steps:

[0006] Step 1: Obtain the current vehicle position, the obstacle positions around the vehicle, and the end point position of the planned path;

[0007] Step 2: construct an artificial potential field function, which includes: a gravitational field function and a repulsive field function;

[0008] Among them, the gravitational field function is:

[0009] The repulsive field function is:

[0010] In the formula, is the correction gain coefficient of the gravitational field, ρ(q,q goal ) is the distance from the current vehicle position to the end point of the planned path, m is the repulsive field coefficient factor, ρ0 is the radius of the safety association range, ρ(q, q obs ) is the distance from the current vehicle position to the obstacle;

[0011] Step 3: Obtain the gravitational force and repulsive force on the vehicle according to the gravitational field function and the repulsive field function, respectively, and obtain the resultant force on the vehicle according to the gravitational force and repulsive force on the vehicle;

[0012] Step 4: determining the target heading angle of the vehicle according to the resultant force applied to the vehicle, and updating the position of the vehicle at the next moment according to the target heading angle;

[0013] Repeat steps 2 to 4 until the vehicle position reaches the end point of the planned path; obtain a vehicle position set;

[0014] Step 5: Use the discrete points in the vehicle position set as the initial solution and plan a smooth path based on the quadratic programming method.

[0015] Preferably, the calculation formula of the correction gain coefficient of the gravitational field is:

[0016]

[0017] in, is the baseline gain coefficient of the gravitational field, and its value range is (0, 1).

[0018] Preferably, the gravitational force on the vehicle is calculated by the following formula:

[0019]

[0020] Preferably, the repulsive force applied to the vehicle is calculated by the following formula:

[0021]

[0022] Preferably, the calculation method of the resultant force applied to the vehicle is:

[0023]

[0024] In the formula, F H The total force on the vehicle, F y represents the gravitational force on the vehicle, s represents the number of obstacles around the vehicle; F c-i The repulsive force exerted on the vehicle by obstacle i.

[0025] Preferably, when the vehicle falls into a local minimum point, the resultant force on the vehicle is calculated by the following formula:

[0026]

[0027] Among them, F H The total force on the vehicle, F y represents the gravitational force on the vehicle, s represents the number of obstacles around the vehicle; F c-i The repulsive force of obstacle i on the vehicle, F z is the resultant force correction value.

[0028] Preferably, the resultant force correction value is calculated by the following formula:

[0029]

[0030] Where a is the resultant force correction coefficient, Fz0 is the reference force correction value, F z0 The value range is [0.8,1].

[0031] Preferably, the resultant force correction coefficient is calculated by the following formula:

[0032]

[0033] Among them, ρ min (q,q obs )The minimum distance between the vehicle and the obstacle.

[0034] The beneficial effects of the present invention are:

[0035] The method for planning a driving path for an autonomous driving vehicle provided by the present invention can improve the safety and feasibility of path planning. DETAILED DESCRIPTION

[0036] The present invention is described in further detail below so that those skilled in the art can implement it according to the description.

[0037] The present invention provides a method for planning a driving path for an autonomous driving vehicle, and the specific implementation process thereof is as follows.

[0038] 1. Collect the current vehicle position, the obstacle positions around the vehicle, and the planned path end position in real time according to the set collection frequency. The planned path end position is the destination position that the vehicle is going to reach; the obstacles around the vehicle refer to other vehicles, pedestrians, and fixed obstacles within the safety association range.

[0039] In one embodiment, a circular area with a radius of 50 meters and a center of mass of the vehicle is set as the safety association range.

[0040] 2. Construct artificial potential field functions, including gravitational field function and repulsive field function.

[0041] The gravitational field function is:

[0042] The repulsive field function is:

[0043] In the formula, is the correction gain coefficient of the gravitational field, ρ(q,q goal ) is the distance from the current vehicle position to the end point of the planned path, m is the repulsive field coefficient factor, ρ0 is the radius of the safety association range; ρ(q, q obs ) is the distance from the current vehicle position to the obstacle.

[0044] Among them, the calculation formula of the correction gain coefficient of the gravitational field is:

[0045]

[0046] in, is the baseline gain coefficient of the gravitational field, and its value range is (0, 1).

[0047] Due to the traditional gravitational field function, the magnitude of gravity is proportional to the distance. The gravity near the starting point is relatively large, but if the gravity is too large, the effect of repulsion will be ignored, which can easily cause a collision between the smart car and the obstacle. Therefore, the gravitational potential field gain coefficient in the gravitational potential field function is corrected in the present invention to avoid the problem of collision caused by excessive gravity near the starting point, thereby improving the safety of path planning.

[0048] 3. Calculate the net force acting on the autonomous vehicle.

[0049] The gravitational force on the vehicle is the negative gradient of the gravitational field function, which is calculated by the following formula:

[0050]

[0051] In the formula, is the correction gain coefficient of the gravitational field, ρ(q,q goal ) is the distance from the current vehicle position to the end point of the planned path.

[0052] Since the correction gain coefficient of the gravitational field is inversely proportional to the distance between the vehicle and the end point of the planned path, the correction gain coefficient of the gravitational field is smallest at the starting point, thereby adjusting the gravitational force acting on the vehicle and preventing the gravitational force acting on the vehicle from being excessive, causing a collision between the autonomous driving vehicle and an obstacle.

[0053] The repulsive force on the vehicle is the negative gradient of the repulsive field function, which is calculated by the following formula:

[0054]

[0055] Where m is the repulsive field coefficient factor, ρ0 is the radius of the safety association range; ρ(q,q obs ) is the distance from the current vehicle position to the obstacle.

[0056] The resultant force on the vehicle is the sum of gravity and all repulsive forces. Assuming that there are s obstacles around the autonomous vehicle, the resultant force on the smart vehicle is calculated by the following formula:

[0057]

[0058] In the formula, F H The total force on the vehicle, F y represents the gravitational force on the vehicle, s represents the number of obstacles around the vehicle; Fc-i The repulsive force exerted on the vehicle by obstacle i.

[0059] Fourth, determine the target heading angle of the vehicle according to the resultant force acting on the vehicle, and update the position of the vehicle at the next moment according to the target heading angle.

[0060] The present invention uses the angle between the direction of the resultant force on the autonomous driving vehicle and the center line of the road as the vehicle heading angle. According to the vehicle kinematic model, if the influence of the center of mass sideslip angle is not considered, the velocity direction and heading angle of the vehicle's center of mass position remain consistent. The resultant force on the autonomous driving vehicle (self-vehicle) in the artificial potential field changes the size of the vehicle's center of mass velocity and determines the vehicle's heading angle in the next stage, that is, the center of mass velocity direction. Assume that during the driving process, the center of mass velocity component V of the autonomous driving vehicle (self-vehicle) along the longitudinal direction of the road y Keep constant, under the action of space-time risk field force, the lateral component V of the vehicle center of mass velocity x It can be calculated according to the heading angle using the following formula:

[0061] v x (t) = v y (t)tanψ(t);

[0062] Where t is the time t of the path planning process, and ψ(t) is the vehicle heading angle at time t.

[0063] In the process of path planning, the next intelligent vehicle (ego vehicle) position is updated at equal time intervals ΔT. The position of the vehicle at time t+1 is estimated based on the center of mass position and speed of the moving vehicle (ego vehicle) at time t. The calculation formula is as follows:

[0064]

[0065] Through the above process, the vehicle position at each moment in the process of reaching the end point of the planned path can be obtained, and these positions constitute the vehicle position set.

[0066] 5. Using the discrete points in the vehicle position set as the initial solution, a smooth planning path is obtained based on the quadratic programming method.

[0067] Assume that the vehicle position set obtained is A T ={a0, a1, a2, ..., a N-1 , a N In order to generate a smooth driving path and reduce the path tracking control requirements, it is also necessary to T Smoothing is performed. Smoothing mainly includes two steps, first, the smoothing of discrete path points, and then the generation of smoothed paths.

[0068] (1) Smoothing of discrete path points

[0069] The objective function established for the smoothing problem of discrete position points in the present invention takes into account two aspects: the smoothness of the planned path and the similarity with the original path. T ={p0, p1, p2, ... p N-1 , p N} is the location set corresponding to the smoothed planned path, a i (x o_i ,y o_i ) and p i (x i ,y i ) are the coordinates of the corresponding positions before and after smoothing.

[0070] The path smoothness in the objective function of the smoothing problem of discrete position points is defined as follows:

[0071]

[0072] The geometric similarity in the objective function of the smoothing problem of discrete position points is defined as follows:

[0073]

[0074] Where N is the number of waypoints.

[0075] The comprehensive objective function of the discrete position point smoothing problem is:

[0076] Cost = w1f1 + w2f2;

[0077] Among them, w1 and w2 are the weight coefficients of each sub-objective function.

[0078] Discrete position point smoothing takes into account the physical constraints of the vehicle steering system and the position constraints of the original path points.

[0079] According to the geometric relationship, the physical constraints of the vehicle steering system are expressed as follows:

[0080]

[0081] Among them, R min is the minimum turning radius of the vehicle.

[0082] The smoothed waypoints should be located near the original waypoints. The position constraints of the original waypoints are expressed as follows:

[0083] x oi -X Buff ≤x i ≤x oi +X Buff ;

[0084] y oi -Y Buff ≤y i≤y oi +Y Buff ;

[0085] Among them, X Buff , Y Buff is a threshold greater than zero, and their values ​​increase with the increase of road curvature.

[0086] In summary, the objective function Cost and the two constraints constitute the mathematical model of the discrete position point smoothing problem. The smoothing problem can be transformed into a standard quadratic programming form for optimization and solution to obtain a smooth discrete path point set.

[0087] (2) Smooth path generation

[0088] As a preferred embodiment, the present invention uses a quintic polynomial to perform piecewise fitting on the smooth position points, thereby generating a smooth path. If a smooth curve is used to connect all the position points, complex scenes will require complex high-order polynomial curves to connect smoothly, with many parameters to be determined, and problems such as large fluctuations in the curvature of the planned trajectory are likely to occur.

[0089] The discrete position point set P after smoothing is known T Contains N+1 path points, then the obstacle avoidance path can be divided into N

[0090] There are intervals, and the path of each interval is represented by two quintic polynomials. For example, the polynomial of the i-th subinterval is represented as follows:

[0091]

[0092] The variable to be optimized in the i-th subinterval is expressed as follows:

[0093] A i =[a 0,i ,a 1,i ,a 2,i a 3,i ,a 4,i ,a 5,i ] T

[0094] B i =[b 0,i ,b 1,i ,b 2,i ,b 3,i ,b 4,i ,b 5,i ] T ;

[0095] To solve the polynomial curve parameters P in each interval i =[A i ; B i], the present invention adopts the quadratic programming method to solve, and the optimization objective function is defined as follows:

[0096]

[0097] Where t represents a continuous variable defined on the interval [0, 1].

[0098] In the process of solving the polynomial curve parameters, in order to ensure that the local reference path formed by connecting each segment of the polynomial curve is continuous and smooth, the position, velocity, and acceleration constraints of the starting point and the end point, the position constraints of the intermediate points (excluding the starting point and the end point), and the continuity constraints at the connection points of adjacent polynomial curves must be met, and all constraints are linear constraints.

[0099] The present invention adopts a quintic polynomial to perform piecewise fitting on the smooth path points, which avoids the problems of many parameters to be determined and large fluctuations in the curvature of the planned trajectory compared to the method of connecting all position points with a smooth curve.

[0100] In the process of planning the path of the smart car by the traditional artificial potential field method, the smart car often falls into the problem of local minimum points. The local minimum point problem is divided into two cases. One is that there is an obstacle acting on the smart car, and the target point, the smart car and the obstacle are in a straight line. The other is that two or more obstacles act on the smart car together, and the total repulsive force of the obstacles and the gravitational force generated by the target point are in the same straight line and act on the autonomous driving vehicle at the same time. In both cases, the resultant force will be 0, causing the smart car to judge that the point is the target point and stop moving forward. The key to solving this problem is to ensure that when the smart car is driving, the resultant force values ​​at other positions except the target point are not 0.

[0101] As a preferred embodiment, when the vehicle falls into a local minimum point (the resultant force of gravity and repulsion is 0), the present invention calculates the resultant force on the vehicle by the following formula:

[0102]

[0103] Among them, F H The total force on the vehicle, F y represents the gravitational force on the vehicle, s represents the number of obstacles around the vehicle; F c-i The repulsive force of obstacle i on the vehicle, F z is the resultant force correction value.

[0104] The resultant force correction value is calculated by the following formula:

[0105]

[0106] Where a is the resultant force correction coefficient, F z0is the reference force correction value, F z0 The value range is [0.8,1]. When there is only one obstacle, if the distance between the autonomous vehicle and the obstacle exceeds half of the safe distance, the resultant force correction value is assigned to 1, otherwise it is assigned to 0.5. If there are multiple obstacles, the situation is more complicated, and a function is needed to assign the resultant force correction value until the autonomous vehicle jumps out of the local minimum point and enters the next state, thereby improving the feasibility of path planning. If the resultant force is not 0 in the next state, the autonomous vehicle continues to move forward under the action of gravity and repulsion.

[0107] As a further preference, the resultant force correction coefficient is calculated by the following formula:

[0108]

[0109] Among them, ρ min (q,q obs )The minimum distance between the vehicle and an obstacle, that is, the distance between the vehicle and the obstacle closest to the vehicle.

[0110] Then, the target heading angle of the vehicle is determined according to the corrected resultant force to obtain the position of the vehicle at the next moment.

[0111] The resultant force correction coefficient is calculated by the above formula, and the resultant force correction value is adjusted according to the minimum distance between the autonomous driving vehicle and the obstacle (the distance between the vehicle and the obstacle closest to the vehicle) and the number of obstacles. When the minimum distance between the vehicle and the obstacle is large, the resultant force correction value is appropriately increased to enable the vehicle to jump out of the local minimum point as quickly as possible while avoiding vehicle collision, thereby improving the efficiency of path planning. When the number of obstacles is large, the resultant force correction value is appropriately reduced to avoid collision between the vehicle and the obstacle, thereby ensuring the safety of path planning.

[0112] Although the embodiments of the present invention have been disclosed as above, they are not limited to the applications listed in the specification and the implementation modes. They can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and the scope of equivalents, the present invention is not limited to the specific details and the embodiments shown and described herein.

Claims

1. A method for planning a driving path for an autonomous driving vehicle, characterized in that: The steps include: Step 1: Obtain the current vehicle position, the obstacle positions around the vehicle, and the end point position of the planned path; Step 2: construct an artificial potential field function, which includes: a gravitational field function and a repulsive field function; Among them, the gravitational field function is: The repulsive field function is: In the formula, is the correction gain coefficient of the gravitational field, ρ(q,q goal ) is the distance from the current vehicle position to the end point of the planned path, m is the repulsive field coefficient factor, ρ0 is the radius of the safety association range, ρ(q, q obs ) is the distance from the current vehicle position to the obstacle; Step 3: Obtain the gravitational force and repulsive force on the vehicle according to the gravitational field function and the repulsive field function respectively, and obtain the resultant force on the vehicle according to the gravitational force and repulsive force on the vehicle; Step 4: determining the target heading angle of the vehicle according to the resultant force applied to the vehicle, and updating the position of the vehicle at the next moment according to the target heading angle; Repeat steps 2 to 4 until the vehicle position reaches the end point of the planned path; obtain a vehicle position set; Step 5: Use the discrete points in the vehicle position set as the initial solution and plan a smooth path based on the quadratic programming method.

2. The method for planning a driving path for an autonomous driving vehicle according to claim 1, characterized in that: The calculation formula of the correction gain coefficient of the gravitational field is: in, is the baseline gain coefficient of the gravitational field, and its value range is (0, 1).

3. The method for planning a driving path for an autonomous driving vehicle according to claim 2, characterized in that: The gravitational force on the vehicle is calculated by the following formula:

4. The method for planning a driving path for an autonomous driving vehicle according to claim 3, characterized in that: The repulsive force on the vehicle is calculated by the following formula:

5. The method for planning a driving path for an autonomous driving vehicle according to claim 4, characterized in that: The calculation method of the resultant force on the vehicle is: In the formula, F H The total force on the vehicle, F y represents the gravitational force on the vehicle, s represents the number of obstacles around the vehicle; F c-i The repulsive force exerted on the vehicle by obstacle i.

6. The method for planning a driving path for an autonomous driving vehicle according to claim 4 or 5, characterized in that: When the vehicle falls into a local minimum point, the resultant force on the vehicle is calculated by the following formula: Among them, F H The total force on the vehicle, F y represents the gravitational force on the vehicle, s represents the number of obstacles around the vehicle; F c-i The repulsive force of obstacle i on the vehicle, F z is the resultant force correction value.

7. The method for planning a driving path for an autonomous driving vehicle according to claim 6, characterized in that: The resultant force correction value is calculated by the following formula: Where a is the resultant force correction coefficient, F z0 is the reference force correction value, F z0 The value range is [0.8,1].

8. The method for planning a driving path for an autonomous driving vehicle according to claim 7, characterized in that: The resultant force correction factor is calculated by the following formula: Among them, ρ min (q,q obs )The minimum distance between the vehicle and the obstacle.

Images