Lattice local path planning method based on artificial potential field optimization

Abstract

The invention discloses an artificial potential field optimization-based lattice local path planning method, which comprises the following steps of: acquiring global reference line information, and setting a local path planning starting point and a local path planning ending point; constructing a vehicle artificial potential field according to the vehicle driving state; a Cartesian coordinate system of the vehicle is converted into a Frenet coordinate system, vehicle motion is decoupled into a transverse direction and a longitudinal direction, a high-order polynomial is constructed, the high-order polynomial is optimized in a sampling space through the vehicle artificial potential field, and a candidate track cluster is obtained; and performing collision detection and trajectory screening processing on the candidate trajectory cluster, outputting an optimal trajectory, and obtaining a current local path plan. On the premise that a reasonable track is not lost, the sampling efficiency is effectively improved, a traditional sampling algorithm process is optimized, unnecessary calculation is reduced, calculation resources are saved, and the response speed of a system is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and particularly relates to a lattice local path planning method based on artificial potential field optimization. Background Art

[0002] The local path planning technology of autonomous vehicles is an important basis to ensure the efficient and safe movement of vehicles in complex environments. At present, the path planning methods of autonomous vehicles include graph search-based methods, optimization-based methods, sampling-based methods, and intelligent algorithms, etc. In graph search-based methods, typical algorithms are Dijkstra algorithm and A* algorithm. Dijkstra outputs a shortest path through traversal search, and the A* algorithm introduces a heuristic term on this basis to improve the search efficiency. However, as the dimension of the planning space increases, the efficiency of graph search methods decreases, and their effects in dynamic scenarios are not good. In optimization-based methods, typical algorithms are model predictive control method and artificial potential field method, which can handle complex optimization problems, but have high computational complexity, long running time, strong dependence on initial search conditions, and there are local optimum problems. Sampling-based methods mainly include random sampling and deterministic sampling, which can flexibly adapt to the changes of obstacles in dynamic scenarios and generate reasonable paths, but will generate redundant paths, consume a large amount of time and resources, and at the same time the quality of the generated paths is unstable. Intelligent algorithms can learn environmental features and automatically optimize the path planning process, but they rely on a large number of training samples and computing resources, and the quality of the generated paths is affected by the quality of the training data, and the interpretability is poor. A single path planning algorithm can only be used for specific application environments, and the universality is poor.

[0003] As one of the commonly used path planning algorithms for current autonomous vehicle manufacturers, Lattice planning introduces the Frenet coordinate system into the planning, decouples the vehicle motion into lateral motion and longitudinal motion, and each trajectory sample comes from the combination of high-order polynomials in two directions. After generating and verifying all trajectory samples, the trajectory samples that do not meet the constraint conditions will be deleted. Finally, according to a certain cost assigned to each candidate path, the optimal trajectory is selected from the remaining branches. Lattice planning has the characteristics of fast sampling speed and strong applicability, but there is no directionality in the sampling process, and there is still room for further optimization. Summary of the Invention

[0004] The present invention proposes a lattice local path planning method based on artificial potential field optimization to solve the problems existing in the above-mentioned prior art.

[0005] To achieve the above object, the present invention provides a lattice local path planning method based on artificial potential field optimization, including the following steps:

[0006] Obtain global reference line information and set the starting and ending points of local path planning;

[0007] Construct the vehicle artificial potential field according to the vehicle driving state;

[0008] Convert the Cartesian coordinate system of the vehicle to the Frenet coordinate system, decouple the vehicle motion into two directions, namely transverse and longitudinal, construct a high-order polynomial, and optimize the high-order polynomial through the vehicle artificial potential field in the sampling space to obtain a candidate trajectory cluster;

[0009] Perform collision detection and trajectory screening on the candidate trajectory cluster, output the optimal trajectory, and obtain the current local path planning.

[0010] Preferably, the expression of the vehicle artificial potential field is:

[0011] U = U att +∑U rn +U rr +U ar +∑U ron ;

[0012] In the formula, U att represents the gravitational potential field function of the local target point, U rn represents the repulsive potential field function of the obstacle, U rr represents the repulsive potential field function of the road boundary, U ar represents the potential field function of the lane reference line, U ron represents the potential field function of dynamic influencing factors.

[0013] Preferably, the expression of the gravitational potential field function of the local target point is:

[0014]

[0015] In the formula, q represents the current position of the vehicle, q goal represents the position of the local target point, ξ1 represents the gravitational scale factor of the target point, and ρ(q, q goal ) represents the difference between the current position and the target position.

[0016] Preferably, the expression of the repulsive potential field function of the obstacle is:

[0017]

[0018] In the formula, q = (x, y) T represents the current position of the vehicle, q n =(x n , y n ) TIt represents the position of the nth obstacle, a represents the lateral influence scale of the obstacle, b represents the longitudinal influence scale of the obstacle, and ξ2 represents the repulsive force scale factor of the obstacle.

[0019] Preferably, the expression of the repulsive force potential field function of the road boundary is:

[0020]

[0021] In the formula, γ1 and γ2 respectively represent the boundary scale factors on both sides of the road, y1 and y2 represent the positions of the boundaries on both sides of the road, L represents the width of the road, e represents the base of the natural logarithm, and k represents the gain parameter.

[0022] Preferably, the expression of the potential field function of the lane reference line is:

[0023]

[0024] In the formula, γ3 represents the scale factor of the lane reference line, and y3, y4 represent the positions of the lane reference line.

[0025] Preferably, the expression of the potential field function of the dynamic influence factor is:

[0026]

[0027] In the formula, q represents the current position of the vehicle, q on represents the position of the dynamic influence factor, δ represents the relative velocity repulsive constant, υ 0n represents the relative velocity relative to the nth obstacle, and ρ(q, q on ) represents the difference between the current position and the position of the dynamic influence factor.

[0028] Preferably, the trajectory screening process includes:

[0029] Setting the maximum speed, maximum acceleration, maximum deceleration, and maximum path curvature of the trajectory as screening conditions to obtain a trajectory that meets the requirements;

[0030] According to different vehicle driving scenarios, designing a static cost function and a dynamic cost function for the trajectory, obtaining a total cost function based on the static cost function and the dynamic cost function, and screening out the optimal trajectory according to the total cost function.

[0031] The present invention also provides a computer device, including a memory, a processor, and a computer program stored on the memory, and the processor executes the computer program to implement the steps of the method.

[0032] The present invention provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the method are implemented.

[0033] Compared with the prior art, the present invention has the following advantages and technical effects:

[0034] The present invention first constructs an artificial potential field for the dynamic scenario of the vehicle. The potential field includes the gravitational potential field of local target points, the repulsive potential field of obstacles, the repulsive potential field of road boundaries, the potential field of lane reference lines, and the potential field of dynamic influencing factors, and can describe the vehicle movement in the dynamic scenario more real - time and accurately. Through the directivity of the artificial potential field, it is applied in the Lattice sampling process to optimize the sampling space, significantly reducing the number of Lattice sampling points. Without losing reasonable trajectories, the sampling efficiency is effectively improved. In addition, a sampling cost function based on the constructed artificial potential field is designed, considering traditional static factors and integrating road geometric constraints, vehicle dynamics constraints, etc., to select the optimal trajectory. The present invention optimizes the traditional sampling algorithm process, adds an optional collision detection process, reduces unnecessary calculations, saves computing resources, and improves the response speed of the system. It can better perform local path planning for the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] The drawings forming a part of this application are used to provide a further understanding of this application. The schematic embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation to this application. In the drawings:

[0036] Figure 1 is the flowchart of the method of the embodiment of the present invention;

[0037] Figure 2 is the structure diagram of the artificial potential field of the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0038] It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The following will refer to the drawings and combine the embodiments to detail this application.

[0039] It should be noted that the steps shown in the flowchart of the drawings can be executed in a computer system such as a set of computer - executable instructions, and although the logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order than here.

[0040] Explanation of the terms therein:

[0041] The Cartesian coordinate system is a two - dimensional or three - dimensional coordinate system proposed by the French mathematician and philosopher René Descartes in the 17th century. In a two - dimensional Cartesian coordinate system, the plane is divided by two mutually perpendicular number lines, namely the horizontal x - axis and the vertical y - axis. These two number lines intersect at the origin (usually denoted as O), and the origin is simultaneously the zero point of both number lines. Any point on the plane can be represented by an ordered pair of numbers (x, y), where x is the projection value of the point on the x - axis and y is the projection value of the point on the y - axis.

[0042] In a three - dimensional Cartesian coordinate system, in addition to the x - axis and y - axis, a z - axis perpendicular to the x - axis and y - axis is added. Points in three - dimensional space can be represented by a triple (x, y, z), where z is the projection value of the point on the z - axis. The Cartesian coordinate system provides a simple and intuitive method for the representation and analysis of geometric figures, enabling complex geometric problems to be solved through algebraic operations. It has a wide range of applications in fields such as mathematics, physics, engineering, and computer science. For example, it is used for three - dimensional modeling in computer graphics and for describing the motion trajectories of objects in physics.

[0043] The core of the Frenet coordinate system lies in representing the position of points on a curve using two coordinates: the arc length (s) along the curve and the lateral offset (d) perpendicular to the curve. The arc length s is the distance from a fixed starting point of the curve to the current point and is usually used to represent the forward direction along the curve. The lateral offset d represents the offset of the point in the direction perpendicular to the tangent of the curve and is usually used to represent the position of a vehicle in a lane.

[0044] Example 1

[0045] As Figure 1 shown, in this embodiment, a lattice local path planning method based on artificial potential field optimization is provided, including the following steps:

[0046] Obtain global reference line information and set the starting point and ending point of local path planning;

[0047] Construct an artificial potential field of the vehicle according to the vehicle's driving state;

[0048] Convert the Cartesian coordinate system of the vehicle to the Frenet coordinate system, decouple the vehicle motion into two directions, transverse and longitudinal, and construct a high - order polynomial. Optimize the high - order polynomial through the artificial potential field of the vehicle in the sampling space to obtain a candidate trajectory cluster;

[0049] Perform collision detection and trajectory screening on the candidate trajectory cluster, output the optimal trajectory, and obtain the current local path planning.

[0050] Specifically, it includes the following steps:

[0051] S1. Receive the global reference line information, then perform local path planning for the current road section, and set the starting point and the ending point.

[0052] S2. Consider the driving state of vehicles on a one-way two-lane road, and construct an artificial potential field for vehicles. The structure of the artificial potential field is as Figure 2 shown, and the steps are as follows:

[0053] S2.1. Design the gravitational potential field function of the local target point of the artificial potential field for vehicles. The local target point is not equal to the ending point of the road section. To prevent a large gravitational force from being generated when the distance to a single ending point is far and a small gravitational force from being generated when approaching the ending point, the local target point set during each local path planning has the following potential field function:

[0054]

[0055] where \(q=(x,y)\) T represents the current position of the vehicle, \(q\) goal represents the position of the local target point, \(\xi_1\) represents the gravitational scale factor of the target point, and \(\rho(q,q\) goal ) represents the difference between the current position and the target position;

[0056] S2.2. Design the repulsive potential field function of the obstacles in the artificial potential field for vehicles. The vehicle is decoupled into a process of lateral and longitudinal motion. The influence of obstacles on the vehicle's motion in lateral and longitudinal directions is not the same. To represent the different influences in the lateral and longitudinal directions of the vehicle, the potential field function is as follows:

[0057]

[0058] where \(q\) n =(x n ,y n ) T represents the position of the \(n\)th obstacle, \(a\) represents the lateral influence scale of the obstacle, \(b\) represents the longitudinal influence scale of the obstacle, and \(\xi_2\) represents the repulsive scale factor of the obstacle;

[0059] S2.3. Design the repulsive potential field function of the road boundary in the artificial potential field for vehicles. During the vehicle's motion, it cannot exceed the lane boundary and should try to avoid driving on the lane line. The potential field function is as follows:

[0060]

[0061] where \(\gamma_1,\gamma_2\) represent the road boundary scale factors, \(y_1,y_2\) represent the road boundary positions, \(L\) represents the road width, \(k\) represents the gain parameter, and \(e\) represents the base of the natural logarithm;

[0062] S2.4. Design the potential field function of the lane reference line for the vehicle's artificial potential field. During the vehicle's driving process, the vehicle should try to keep driving along the center of the lane (i.e., the lane reference line), and the potential field function is as follows:

[0063]

[0064] Among them, γ3 represents the lane reference line scale factor, and y3, y4 represent the lane reference line positions;

[0065] S2.5. Design the potential field function of the dynamic influencing factors for the vehicle's artificial potential field. In a dynamic scenario, there are other moving vehicles, pedestrians and other dynamic obstacles, and their movements need to be reasonably predicted. The potential field function is as follows:

[0066]

[0067] Among them, δ represents the relative speed factor, and υ 0n represents the relative speed of the vehicle relative to the nth obstacle;

[0068] S2.6. Considering the above conditions, construct the potential field function:

[0069] U = U att + ∑U rn + U rr + U ar + ∑U ron

[0070] According to the potential field function, output the lowest gradient curve, and generate the lowest gradient interval within the range of σ near the curve.

[0071] S3. Convert the vehicle's Cartesian coordinate system to the Frenet coordinate system, decouple the vehicle's motion into two directions, transverse and longitudinal, and express it through a high-order polynomial. Optimize using the constructed potential field in the sampling space. The steps are as follows:

[0072] S3.1. Convert the parameters in the Cartesian coordinate system to the parameters in the Frenet coordinate system;

[0073] S3.2. Decouple the vehicle's motion transversely and longitudinally, express them respectively with polynomials, and at the same time optimize the sampling and generate a trajectory cluster:

[0074] S3.2.1. Represent the vehicle's transverse motion with a polynomial. According to the vehicle's simple model and boundary conditions, consider using a fifth-order polynomial to represent the vehicle's transverse motion:

[0075] d(t) = k0 + k1t + k2t 2 + k3t 3 + k4t 4 + kt 5

[0076] Among them, d represents the lateral displacement, t represents the time, and k0 - k5 represent unknown parameters, which can be solved through boundary conditions;

[0077] S3.2.2. Represent the vehicle's longitudinal motion with a polynomial. In the parking and car - following scenarios, consider using a fifth - degree polynomial to represent the vehicle's longitudinal motion:

[0078] s(t) = b0 + b1t + b2t 2 + b3t 3 + b4t 4 + b5t 5

[0079] Among them, b0 - b5 represent unknown parameters, which can be solved through boundary conditions;

[0080] In the cruise scenario, consider using a fourth - degree polynomial to represent the vehicle's longitudinal motion:

[0081] s(t) = b0 + b1t + b2t 2 + b3t 3 + b4t 4

[0082] S3.3. When sampling the vehicle's lateral and longitudinal directions above, according to the lowest gradient interval obtained in S2, directly screen the sampling points, retain the sampling points within the lowest gradient interval, and do not consider all the sampling points outside the interval. The above completes the optimization of the sampling points.

[0083] S3.4. After completing the lateral sampling and longitudinal sampling respectively, perform the synthesis of the lateral sampling and longitudinal sampling. By aligning multiple groups of time T of the lateral and longitudinal sampling m , obtain a synthetic reference trajectory cluster that meets the requirements, which contains complete path and speed information.

[0084] S4. Perform optional collision detection and trajectory screening processing on the generated candidate trajectory cluster. The steps are as follows:

[0085] S4.1. Perform optional collision detection on the candidate trajectory cluster. In the construction link of the artificial potential field, the influence of obstacles and dynamic factors on the vehicle's movement has been considered. Therefore, in the post - processing link of the trajectory cluster, collision detection can be selectively performed for verification purposes. Therefore, this link is named optional collision detection. The ego - vehicle and obstacles use OBB bounding boxes. For all points on the trajectory, use the GJK method to detect the obstacles in turn. If a collision occurs, then eliminate this trajectory; if no collision occurs, then retain this trajectory

[0086] S4.2. Perform trajectory screening on the candidate trajectory cluster that has passed the collision detection. The steps are as follows:

[0087] S4.2.1. Set the maximum speed, maximum acceleration, maximum deceleration, and maximum path curvature of the trajectory, and leave the trajectory that meets the requirements;

[0088] S4.2.2. Design the static cost function Cost of the trajectory according to different vehicle driving scenarios static and the dynamic cost function Cost dynamic , and obtain the total cost function:

[0089] Cost = Cost static + Cost dynamic

[0090] Among them, the static cost function refers to those costs whose actual values only depend on the initial state and final state of the trajectory sample, such as lateral offset cost, speed cost, acceleration cost, time cost, etc., which are expressed as follows:

[0091]

[0092] Among them, ω is the weight associated with each cost item, d ref , v ref , a ref are the given reference lateral offset from the center line, reference driving speed, and reference driving acceleration. The maximum and minimum values of the lateral offset d and the time range T are the upper and lower limits determined by the sampling parameters;

[0093] The dynamic cost Cost dynamic are those costs that depend on other factors in the actual scenario, such as road geometry constraints, vehicle dynamics constraints, etc. Heuristic cost terms are used to approximate the true cost distribution in the sampling space. Here, the information of the previous optimal trajectory is introduced as a reference and used as a part C h of the cost function. At the same time, the influence of the artificial potential field is introduced, and the complex constraints calculated by the potential field function are used as a part C APF of the cost function. The dynamic cost is expressed as follows:

[0094] Cost dynamic = C h + C APF

[0095] According to the total cost function, select the optimal trajectory.

[0096] S5. Output the optimal trajectory. Starting from the starting point, repeat S2 - S4 until reaching the end of the current section, complete the current local path planning, and wait for the next instruction.

[0097] This embodiment also provides a computer device, including a memory, a processor, and a computer program stored on the memory. The processor executes the computer program to implement the steps of the method.

[0098] This embodiment provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the method are implemented.

[0099] The above is only a preferred specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed in the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A lattice local path planning method based on artificial potential field optimization, characterized in that: The following steps are involved: Get global reference line information and set the starting and ending points of local path planning; Constructing a vehicle artificial potential field according to the vehicle driving status; The Cartesian coordinate system of the vehicle is converted into the Frenet coordinate system, the vehicle motion is decoupled into the lateral and longitudinal directions, and a high-order polynomial is constructed. The high-order polynomial is optimized in the sampling space through the vehicle artificial potential field to obtain a candidate trajectory cluster; The candidate trajectory cluster is subjected to collision detection and trajectory screening processing, and the optimal trajectory is output to obtain the current local path planning.

2. The method according to claim 1, characterized in that The vehicle artificial potential field expression is: U=U att +∑U rn +U rr +U ar +∑U ron ; Where U att represents the gravitational potential field function of the local target point, U rn represents the obstacle repulsive force potential field function, U rr represents the road boundary repulsive potential field function, U ar represents the lane reference line potential field function, U ron Represents the potential field function of dynamic influencing factors.

3. The method according to claim 2, characterized in that The expression of the local target point gravitational potential field function is: In the formula, q represents the current position of the vehicle, q goal represents the local target point position, ξ1 represents the target point gravity scale factor, ρ(q,q goal ) represents the difference between the current position and the target position.

4. The method according to claim 2, characterized in that: The expression of the obstacle repulsive force potential field function is: Where q = (x, y) T represents the current position of the vehicle, q n =(x n ,y n ) T represents the position of the nth obstacle, a represents the lateral influence scale of the obstacle, b represents the longitudinal influence scale of the obstacle, and ξ2 represents the obstacle repulsion scale factor.

5. The method according to claim 2, characterized in that: The expression of the road boundary repulsive potential field function is: Where γ1 and γ2 represent the scale factors of the boundaries on both sides of the road, y1 and y2 represent the boundaries on both sides of the road, L represents the road width, e represents the base of the natural logarithm, and k represents the gain parameter.

6. The method according to claim 2, characterized in that The expression of the lane reference line potential field function is: Where γ3 represents the lane reference line scale factor, and y3 and y4 represent the lane reference line positions.

7. The method according to claim 2, characterized in that The expression of the potential field function of the dynamic influencing factor is: In the formula, q represents the current position of the vehicle, q on represents the position of the dynamic influencing factor, δ represents the relative velocity repulsion constant, υ 0n represents the relative speed to the nth obstacle, ρ(q,q on ) represents the difference between the current position and the position of the dynamic influencing factor.

8. The method according to claim 1, characterized in that The trajectory screening process includes: Set the maximum velocity, maximum acceleration, maximum deceleration and maximum curvature of the trajectory as screening conditions to obtain the trajectory that meets the requirements; According to different vehicle driving scenarios, the static cost function and dynamic cost function of the trajectory are designed, the total cost function is obtained based on the static cost function and the dynamic cost function, and the optimal trajectory is screened out based on the total cost function.

9. A computer device comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

Images