A lattice local path planning method based on artificial potential field optimization

Abstract

This invention discloses a lattice local path planning method based on artificial potential field optimization, comprising the following steps: acquiring global reference line information and setting the starting and ending points of the local path planning; constructing a vehicle artificial potential field based on the vehicle's driving state; converting the vehicle's Cartesian coordinate system to a Frenet coordinate system, decoupling the vehicle motion into two directions, lateral and longitudinal, and constructing a higher-order polynomial; optimizing the higher-order polynomial in the sampling space using the vehicle artificial potential field to obtain a candidate trajectory cluster; performing collision detection and trajectory filtering on the candidate trajectory cluster, outputting the optimal trajectory, and obtaining the current local path planning. This invention effectively improves sampling efficiency without sacrificing reasonable trajectories, optimizes the traditional sampling algorithm process, reduces unnecessary calculations, saves computational resources, and improves the system's response speed.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, and in particular relates to a lattice local path planning method based on artificial potential field optimization. Background Technology

[0002] Local path planning technology for autonomous vehicles is a crucial foundation for ensuring efficient and safe movement in complex environments. Currently, path planning methods for autonomous vehicles include graph search-based methods, optimization-based methods, sampling-based methods, and intelligent algorithms. Typical algorithms in graph search-based methods include Dijkstra's algorithm and A* algorithm. Dijkstra's algorithm outputs a shortest path through traversal, while A* algorithm introduces heuristics to improve search efficiency. However, the efficiency of graph search methods decreases as the dimensionality of the planning space increases, and its performance in dynamic scenarios is poor. Typical algorithms in optimization-based methods include model predictive control and artificial potential field methods, which can handle complex optimization problems, but have high computational complexity, long running time, heavy dependence on initial search conditions, and the problem of local optima. Sampling-based methods mainly include random sampling and deterministic sampling, which can flexibly adapt to changes in obstacles in dynamic scenarios and generate reasonable paths, but generate redundant paths, consume significant time and resources, and have unstable path quality. Intelligent algorithms can learn environmental features and automatically optimize the path planning process, but they rely on a large number of training samples and computational resources, and the quality of generated paths is affected by the quality of training data, resulting in poor interpretability. A single path planning algorithm can only be used for specific application environments and has poor universality.

[0003] Lattice planning, a commonly used path planning algorithm among autonomous driving manufacturers, incorporates the Frenet coordinate system into the planning process, decoupling vehicle motion into lateral and longitudinal motion. Each trajectory sample is derived from a combination of high-order polynomials in both directions. After generating and validating all trajectory samples, those that do not meet the constraints are removed. Finally, based on a cost allocated to each candidate path, the optimal trajectory is selected from the remaining branches. Lattice planning is characterized by its fast sampling speed and wide applicability, but it lacks directionality during the sampling process, leaving room for further optimization. Summary of the Invention

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

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

[0006] Obtain global reference line information and set the start and end points for local path planning;

[0007] Construct an artificial potential field for the vehicle based on its driving status;

[0008] The vehicle's Cartesian coordinate system is converted to the Frenet coordinate system, the vehicle motion is decoupled into two directions, lateral and longitudinal, and a higher-order polynomial is constructed. The higher-order polynomial is then optimized in the sampling space using the vehicle's artificial potential field to obtain a candidate trajectory cluster.

[0009] Collision detection and trajectory filtering are performed on the candidate trajectory clusters to output the optimal trajectory and obtain the current local path planning.

[0010] Preferably, the expression for the vehicle's artificial potential field is:

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

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

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

[0014]

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

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

[0017]

[0018] In the formula, q = (x, y) T Indicates the vehicle's current position, q n =(x n ,y n ) TLet ξn represent the position of the nth obstacle, a represent the lateral influence scale of the obstacle, b represent the longitudinal influence scale of the obstacle, and ξ2 represent the obstacle repulsion scale factor.

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

[0020]

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

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

[0023]

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

[0025] Preferably, the expression for the potential field function of the dynamic influencing factors is:

[0026]

[0027] In the formula, q represents the current position of the vehicle. on The location of the dynamic influencing factor is indicated by δ, which represents the relative velocity repulsive force constant, and υ is the value of υ. 0n Let ρ(q,q) represent the relative velocity with respect to the nth obstacle. on This represents the difference between the current location and the location of the dynamic influencing factors.

[0028] Preferably, the trajectory filtering process includes:

[0029] Set the maximum velocity, maximum acceleration, maximum deceleration, and maximum curvature of the trajectory as filtering conditions to obtain trajectories that meet the requirements;

[0030] Based on different vehicle driving scenarios, static and dynamic cost functions for the trajectory are designed. The total cost function is obtained based on the static and dynamic cost functions, and the optimal trajectory is selected based on the total cost function.

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

[0032] The present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method.

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

[0034] This invention first constructs an artificial potential field for the dynamic scene of the vehicle. This potential field includes the gravitational potential field of the local target point, the repulsive potential field of obstacles, the repulsive potential field of the road boundary, the potential field of the lane reference line, and the potential field of dynamic influencing factors, which can accurately describe the vehicle motion in a dynamic scene in real time. By utilizing the directionality of the artificial potential field in the Lattice sampling process, the sampling space is optimized, significantly reducing the number of Lattice sampling points and effectively improving sampling efficiency without sacrificing reasonable trajectories. Furthermore, a sampling cost function based on the constructed artificial potential field is designed, considering traditional static factors and integrating road geometric constraints and vehicle dynamics constraints to select the optimal trajectory. This invention optimizes the traditional sampling algorithm process by adding an optional collision detection process, reducing unnecessary calculations, saving computational resources, and improving the system's response speed. It also enables better local path planning for the vehicle. Attached Figure Description

[0035] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0036] Figure 1 This is a flowchart of a method according to an embodiment of the present invention;

[0037] Figure 2 This is a diagram of the artificial potential field structure according to an embodiment of the present invention. Detailed Implementation

[0038] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

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

[0040] Explanation of the nouns mentioned:

[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 axes: the horizontal x-axis and the vertical y-axis. These two number axes intersect at the origin (usually denoted as O), which is also the zero point of both number axes. Any point on the plane can be represented by an ordered pair of numbers (x, y), where x is the projection of the point onto the x-axis, and y is the projection of the point onto the y-axis.

[0042] In a three-dimensional Cartesian coordinate system, in addition to the x-axis and y-axis, a z-axis is added, perpendicular to both x-axis and y-axis. A point in three-dimensional space can be represented by a triple (x, y, z), where z is the projection of that point onto the z-axis. The Cartesian coordinate system provides a concise and intuitive method for representing and analyzing geometric figures, allowing complex geometric problems to be solved through algebraic operations. It has wide applications in mathematics, physics, engineering, computer science, and other fields, such as 3D modeling in computer graphics and describing the trajectories of moving objects in physics.

[0043] The core of the Frenet coordinate system lies in representing the position of a point 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 on the curve to the current point, and is usually used to represent the direction of travel along the curve. The lateral offset d represents the amount of displacement 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 the lane.

[0044] Example 1

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

[0046] Obtain global reference line information and set the start and end points for local path planning;

[0047] Construct an artificial potential field for the vehicle based on its driving status;

[0048] The vehicle's Cartesian coordinate system is converted to the Frenet coordinate system, the vehicle motion is decoupled into two directions, lateral and longitudinal, and a higher-order polynomial is constructed. The higher-order polynomial is then optimized in the sampling space using the vehicle's artificial potential field to obtain a candidate trajectory cluster.

[0049] Collision detection and trajectory filtering are performed on the candidate trajectory clusters to output the optimal trajectory and obtain the current local path planning.

[0050] Specifically, the following steps are included:

[0051] S1. Receive global reference line information, then perform local path planning for the current road segment, and set the start and end points;

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

[0053] S2.1. Design the gravitational potential field function for the local target point in the vehicle's artificial potential field. The local target point is not equal to the end point of the road segment. To prevent the gravitational force of a single endpoint from being too large at a distance and too small near the endpoint, the potential field function for the local target point is set during each local path planning step as follows:

[0054]

[0055] Where q = (x, y) T Indicates the vehicle's current position, q goal ρ(q,q) represents the local target point location, ξ1 represents the gravitational scale factor of the target point, and ρ(q,q) represents the target point location. goal () indicates the difference between the current position and the target position;

[0056] S2.2. Design the artificial potential field function for the vehicle and the repulsive force of the obstacle. The vehicle decouples into lateral and longitudinal motions. The influence of the obstacle on the vehicle's motion differs in the lateral and longitudinal directions. To represent the different influences on the vehicle in the lateral and longitudinal directions, the potential field function is as follows:

[0057]

[0058] Where, q n =(x n ,y n ) T Let ξn represent the position of the nth obstacle, a represent the lateral influence scale of the obstacle, b represent the longitudinal influence scale of the obstacle, and ξ2 represent the obstacle repulsion scale factor.

[0059] S2.3. Design the artificial potential field function for the road boundary repulsive potential field of the vehicle. During the vehicle's movement, it must not cross the lane boundary and should try to avoid driving on the lane lines. The potential field function is as follows:

[0060]

[0061] Where γ1 and γ2 represent road boundary scale factors, y1 and y2 represent road boundary locations, L represents road width, k represents gain parameter, and e represents the base of the natural logarithm;

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

[0063]

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

[0065] S2.5. Design the potential field function to influence the dynamic potential field of the vehicle. In dynamic scenarios, there are other moving vehicles, pedestrians, and other dynamic obstacles, and their motion needs to be reasonably predicted. The potential field function is as follows:

[0066]

[0067] Where δ represents the relative velocity factor, υ 0n This represents the relative velocity of the vehicle with respect 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] Based on the potential field function, the lowest gradient curve is output, and the lowest gradient interval is generated within the σ range near the curve.

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

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

[0073] S3.2. Decouple the vehicle motion laterally and longitudinally, expressing them using polynomials respectively. Simultaneously, optimize the sampling and generate trajectory clusters:

[0074] S3.2.1. Polynomial Representation of Vehicle Lateral Motion. Based on the simplified vehicle model and boundary conditions, consider using a fifth-degree polynomial to represent the vehicle's lateral motion:

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

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

[0077] S3.2.2. Polynomial Representation of Vehicle Longitudinal Motion. In parking and 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 +bt 5

[0079] Where b0-b5 represent unknown parameters, which can be solved by boundary conditions;

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

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

[0082] S3.3. When sampling the vehicles in the lateral and longitudinal directions, the sampling points are directly filtered according to the lowest gradient interval obtained in S2. The sampling points within the lowest gradient interval are retained, and the sampling points outside the interval are not considered. The above completes the optimization of the sampling points.

[0083] S3.4. After the horizontal and vertical sampling are completed separately, the horizontal and vertical samples are combined by aligning multiple sets of time T of the horizontal and vertical samples. m This yields a synthetic reference trajectory cluster that meets the requirements, containing complete path and velocity information.

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

[0085] S4.1. Optional Collision Detection for Candidate Trajectory Clusters. Since the influence of obstacles and dynamic factors on vehicle motion has been considered during the artificial potential field construction stage, selective collision detection can be performed in the trajectory cluster post-processing stage for verification purposes. Therefore, this stage is named Optional Collision Detection. OBB bounding boxes are used for both the vehicle and obstacles. At all points on the trajectory, obstacles are sequentially detected using the GJK method. If a collision occurs, the trajectory is discarded; otherwise, it is retained.

[0086] S4.2. For the candidate trajectory clusters that have undergone collision detection, the trajectory selection process is as follows:

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

[0088] S4.2.2. Design the static cost function Cost for the trajectory based on different vehicle driving scenarios. static With dynamic cost function Cost dynamic The total cost function is obtained as follows:

[0089] Cost = Cost static +Cost dynamic

[0090] The static cost function refers to the costs whose actual values ​​depend only on the initial and final states of the trajectory samples, such as lateral offset cost, velocity cost, acceleration cost, and time cost, as shown below:

[0091]

[0092] Where ω is the weight associated with each cost item, d ref v ref a ref These are the given reference lateral offset from the centerline, reference speed, and reference 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] Dynamic Cost dynamic These are costs that depend on other factors in the actual scenario, such as road geometry constraints and vehicle dynamics constraints. A heuristic cost term is 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 included as part of the cost function C. h Simultaneously, the influence of an artificial potential field is introduced, and the complex constraints calculated through the potential field function are used as part of the cost function C. APF The dynamic cost is represented as follows:

[0094] Cost dynamic =C h +C APF

[0095] The optimal trajectory is selected based on the total cost function.

[0096] S5. Output the optimal trajectory. Starting from the starting point, repeat S2-S4 until the end point of the current road segment is reached, completing the current local path planning, and waiting for the next instruction.

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

[0098] This embodiment provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method.

[0099] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A lattice local path planning method based on artificial potential field optimization, characterized in that, Includes the following steps: Obtain global reference line information and set the start and end points for local path planning; Construct an artificial potential field for the vehicle based on its driving status; The vehicle's Cartesian coordinate system is converted to the Frenet coordinate system, the vehicle motion is decoupled into two directions, lateral and longitudinal, and a higher-order polynomial is constructed. The higher-order polynomial is then optimized in the sampling space using the vehicle's artificial potential field to obtain a candidate trajectory cluster. Obtaining the candidate trajectory cluster specifically includes: outputting the lowest gradient curve based on the potential field function, and generating the lowest gradient interval within the σ range near the curve. During lateral and longitudinal sampling of the vehicle, the sampling points are directly filtered based on the obtained lowest gradient interval, retaining the sampling points within the lowest gradient interval and ignoring all sampling points outside the interval. This completes the optimization of the sampling points. After lateral and longitudinal sampling are completed, the lateral and longitudinal samples are synthesized by aligning multiple sets of time T of the lateral and longitudinal samples. m This yields a synthetic reference trajectory cluster that meets the requirements, containing complete path and velocity information; Collision detection and trajectory filtering are performed on the candidate trajectory clusters to output the optimal trajectory and obtain the current local path planning.

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

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

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

5. The method according to claim 2, characterized in that, The expression for the repulsive potential field function at the road boundary is: In the formula, γ1 and γ2 represent the scale factors of the road side boundaries, y1 and y2 represent the positions of the road side boundaries, 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 for the lane reference line potential field function is: In the formula, γ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 for the potential field function of the dynamic influencing factors is: In the formula, q represents the current position of the vehicle. on The location of the dynamic influencing factor is indicated by δ, which represents the relative velocity repulsive force constant, and υ is the value of υ. 0n Let ρ(q,q) represent the relative velocity with respect to the nth obstacle. on This represents the difference between the current location and the location of the dynamic influencing factors.

8. The method according to claim 1, characterized in that, The trajectory filtering process includes: Set the maximum velocity, maximum acceleration, maximum deceleration, and maximum curvature of the trajectory as filtering conditions to obtain trajectories that meet the requirements; Based on different vehicle driving scenarios, static and dynamic cost functions for the trajectory are designed. The total cost function is obtained based on the static and dynamic cost functions, and the optimal trajectory is selected 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-8.

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

Images