A trajectory planning method based on potential field guidance

Abstract

The present invention relates to a trajectory planning method based on potential field guidance. The method sets sampling points along a road reference line in a Frenet coordinate system based on road information; adjusts the position and / or number of sampling points based on obstacle potential fields and road boundary potential fields, and sets sampling layers based on sampling points; connects sampling points of adjacent sampling layers to obtain a transverse segmented path that changes with a longitudinal path, and obtains N1 transverse paths related to the longitudinal path by splicing the transverse segmented paths. By performing path planning in this way, a smooth, safe, and high-quality transverse path can be planned. Then, speed planning is performed, and N1×N2 candidate trajectories are obtained by making the longitudinal path have N2 different speeds, and then an optimal trajectory is obtained, which can ensure that trajectories with different stability are provided, so that the optimal path meets the trajectory safety and stability requirements of an automatic vehicle.

Description

Technical Field

[0001] The present disclosure relates to the field of automatic vehicle trajectory planning, and in particular to a trajectory planning method based on potential field guidance. Background Art

[0002] Nowadays, the application scenarios of autonomous vehicles are becoming more and more extensive, especially in industrial parks and large factories. Because autonomous vehicles usually need to transport goods or perform various operations, there are higher requirements for their trajectory planning, which must not only be safe and feasible, but also ensure the smoothness of all trajectory points.

[0003] The current trajectory planning method for autonomous vehicles fails to fully utilize the reference line or road direction information, and the selection of sampling points also fails to fully utilize the obstacle information, so there are many unreasonable trajectories in the generated candidate trajectories. Summary of the invention

[0004] In view of the above-mentioned prior art, the technical problem solved by the present invention is at least to provide a trajectory planning method that can avoid obstacles and enable an automatic vehicle to drive smoothly.

[0005] In order to solve the above technical problems, the present invention proposes a trajectory planning method based on potential field guidance, the method comprising the following steps:

[0006] Based on the road information, sampling points are set along the road reference line in the Frenet coordinate system;

[0007] Based on the obstacle potential field and the road boundary potential field, adjusting the position and / or number of sampling points, and setting sampling layers based on the sampling points;

[0008] Connect the sampling points of adjacent sampling layers to obtain a transverse segmented path that changes with the longitudinal path. By splicing the transverse segmented paths, N1 transverse paths related to the longitudinal path are obtained. Path planning in this way can ensure that a smooth, safe and high-quality transverse path is planned.

[0009] Then, speed planning is performed to obtain N1×N2 candidate trajectories by making the longitudinal path have N2 different durations, and then the optimal trajectory is obtained; N1 and N2 are integers greater than 0. Through speed planning, it is guaranteed that trajectories with different smoothness are provided.

[0010] Through path planning and speed planning, the optimal path obtained meets the trajectory safety and stability requirements of the automatic vehicle.

[0011] In the above technical solution, by adopting the Frenet coordinate system, the trajectory of the automatic vehicle is based on the center line of the road or the approximate global path as a reference line, and the reference line itself provides the direction information of the movement of the automatic vehicle. The reference line is not necessarily the center line of the road. For example, for a two-lane road, it can be based on the center of the lane where the vehicle is located; and for unstructured roads such as warehouses, it can be a pre-planned approximate path as a reference line. By setting sampling points, a long path is divided into several short paths, which is conducive to reducing the path deviation and forming a trajectory that can avoid obstacles. The position and / or number of sampling points are adjusted by guiding the obstacle potential field and the road boundary potential field. It can be either adjusting the position or adding the number of sampling points, or adjusting the position and the number at the same time to increase the number of paths that can avoid obstacles in the candidate path; and using it as the optimal path to control the driving trajectory of the automatic vehicle, it can meet the safety requirements of the trajectory. When the vehicle is driving, when turning or avoiding obstacles, etc., it needs to drive horizontally, and the vehicle needs to drive in the longitudinal direction at the same time. Therefore, when planning the path, making the lateral path change dependent on the longitudinal path change is in line with the actual driving needs of the vehicle, so that the final planned trajectory can better meet the practicality. By splicing segmented paths, a smoother complete path can be obtained. Since vehicles have different smoothness at different driving speeds, making different longitudinal paths have different durations helps to plan the optimal path that meets vehicle driving safety and smoothness.

[0012] In the above technical solution, one implementation method of setting the sampling layer based on the sampling points includes the following steps:

[0013] Acquire a sampling point affected by the obstacle potential field and the road boundary potential field as the first sampling point;

[0014] The repulsive force of each first sampling point is calculated according to the obstacle potential field and the road boundary potential field, and the first sampling point is moved a set distance in the direction of the repulsive force.

[0015] In the above technical solution, the sampling layer is formed by multiple sampling points. By connecting the sampling points between adjacent sampling layers, lateral paths with different movement directions can be generated between adjacent sampling layers. The segmented paths formed between the sampling layers can be spliced ​​to form multiple complete candidate paths. A specific implementation method for setting the sampling layer based on the sampling points includes the following steps:

[0016] Get a sampling point as the current sampling point;

[0017] Step S: Add at least two sampling points with the same longitudinal position but different lateral positions near the current sampling point, and take the set of the current sampling point and the added sampling points as a sampling layer;

[0018] If there are still sampling points for which the sampling layer is not set, the next sampling point is obtained as the current sampling point, and the process returns to step S.

[0019] In the above technical solution, the optimal trajectory is a trajectory that meets the screening conditions; the screening conditions include collision detection conditions; the screening conditions include collision detection conditions; the collision detection conditions are used to determine whether the trajectory satisfies the requirement of preventing the vehicle from collision; the determination includes the following steps:

[0020] The center of the front axle, the center of the rear axle, and the midpoint of the front axle and the rear axle are taken as the center of the circle, and the distance between the front and rear axles is taken as the diameter of the circle;

[0021] Calculate the distance from the obstacle to the centers of the three circles;

[0022] Based on the comparison between the distance and the diameter, it is determined whether the vehicle collides with the obstacle.

[0023] In the above technical solution, the optimal trajectory is a trajectory that meets the screening conditions; the screening conditions include trajectory smoothness detection; the smoothness detection includes the following steps:

[0024] Based on the coordinates of the points on the trajectory in the Frenet coordinate system, obtain their coordinates in the Cartesian coordinate system, so as to obtain the maximum curvature and maximum acceleration of the trajectory; based on the maximum acceleration and maximum curvature, determine whether the trajectory is flat.

[0025] In the above technical solution, the optimal trajectory is a trajectory that meets the screening conditions; the screening conditions include using a cost function to calculate the cost of each candidate trajectory, the smaller the cost, the better the trajectory; the cost function is as follows:

[0026]

[0027] Where: cost is the required cost; w s 、w l 、w t 、w deviate is the weight coefficient, l end is the actual lateral position of the trajectory terminal, l target is the preset horizontal position of the trajectory terminal; T is the planning duration of the trajectory; J s is the rate of change of longitudinal acceleration, J l is the rate of change of lateral acceleration.

[0028] In the above technical solution, one way to obtain the obstacle potential field is to calculate through the following function:

[0029]

[0030] Where:

[0031] U obstacle is the obstacle potential field, K obs is the positive proportional coefficient of the obstacle potential field; ρ(p, q0) is the distance from the sampling point to the obstacle; ρ0 is the influence distance of the obstacle potential field;

[0032] The road boundary potential field is calculated by the following formula:

[0033]

[0034] Where: U road is the road boundary potential field, K road is the road boundary potential field proportionality coefficient, l car，road is the distance from the center of mass of the autonomous vehicle to the road boundary; W car For the vehicle width.

[0035] In the above technical solution, in the Frenet coordinate system, The trajectory points describing the trajectory; where s is the longitudinal position, is a function of time, is the longitudinal velocity, is the longitudinal acceleration, l is the lateral position, is a function of the longitudinal position, l′ is the derivative of the lateral position with respect to the longitudinal position, and l″ is the second-order derivative of the lateral position with respect to the longitudinal position.

[0036] In the above technical solution, the lateral position is a function of the longitudinal position and is described by the following quintic polynomial:

[0037] l(s)=c6·s 5 +c5·s 4 +c4·s 3 +c3·s 2 +c2·s+c1

[0038] Where: s is the longitudinal position; l(s) is the lateral position; c6, c5, c4, c3, c2, c1 are polynomial coefficients;

[0039] The longitudinal position is a function of time and is described by a fifth-order polynomial:

[0040] s(t)=a6·t 5 +a5·t 4 +a4·t 3 +a3·t 2 +a2·t+a1

[0041] In the formula: t is time, s is longitudinal position, a6, a5, a4, a3, a2, a1 are polynomial coefficients.

[0042] By using a quintic polynomial to describe the longitudinal trajectory, a trajectory with higher smoothness can be obtained with less calculation, thus meeting the real-time requirements for obtaining the optimal trajectory of the automatic vehicle.

[0043] Based on the above technical solutions regarding the methods, the present invention further proposes a computer-readable storage medium storing a computer program that can be loaded by a processor and execute any of the above methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0045] Figure 1 , one A schematic diagram of a method flow chart in an embodiment;

[0046] Figure 2 , one In the embodiment, the sampling layers are connected to form different paths;

[0047] Figure 3 , one A schematic diagram of the path planning effect of setting multiple groups of sampling layers in an embodiment;

[0048] Figure 4 , one Schematic diagram of the path planning effect of determining the sampling center under the guidance of the potential field in an embodiment;

[0049] Figure 5 , Schematic diagram of coordinate transformation of trajectory points in Frenet coordinate system and Cartesian coordinate system. DETAILED DESCRIPTION

[0050] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. The terms "first" and "second" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance or implicitly indicating the number of indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features.

[0051] The operations of the flow diagram in the implementation of the method of the present invention may not be implemented in order. On the contrary, the operations may be implemented in reverse order or simultaneously. In addition, one or more other operations may be added to the flow diagram. One or more operations may be removed from the flow diagram.

[0052] An implementation process of the method of the present invention is as follows Figure 1 As shown, the following steps are included:

[0053] Based on the road information, sampling points are set along the road reference line in the Frenet coordinate system and used as reference sampling points;

[0054] Based on the obstacle potential field and the road boundary potential field, adjust the position and / or number of reference sampling points and set the sampling layer;

[0055] The sampling points of adjacent sampling layers are connected to obtain a transverse segmented path that changes with the longitudinal path, and N1 transverse paths related to the longitudinal path are obtained by splicing the transverse segmented paths;

[0056] Then, by making the longitudinal path have N2 different durations, N1×N2 candidate trajectories are obtained, and then the optimal trajectory is obtained; N1 and N2 are integers greater than 0.

[0057] In the above method steps, a Frenet coordinate system is established based on road information. In the Frenet coordinate system, the center line of the road is used as the reference line. The reference line is not necessarily the center line of the road. For example, for a two-lane road, the center of the lane where the vehicle is located can be used as the reference line; for an unstructured road surface such as a warehouse, a rough path planned in advance can be used as the reference line. Sampling points are set along the reference line and used as reference sampling points. The reference sampling points can be randomly distributed or evenly distributed on the reference line.

[0058] When setting the sampling layer, if obstacles are not considered, reference sampling points are taken as current sampling points in sequence, and several sampling points are added at a certain distance near the current sampling point, and the set of these sampling points and the current sampling point is regarded as a sampling layer until the corresponding sampling layer is set based on all reference sampling points.

[0059] If obstacles are taken into consideration, based on the obstacle potential field and the road boundary potential field, the reference sampling point affected by the obstacle potential field and the road boundary potential field is used as the first sampling point, and the position of the first sampling point is moved to move away from the obstacle; or new sampling points are added to increase the segmented path, thereby increasing the segmented path that bypasses the obstacle. The aforementioned newly added sampling points are also reference sampling points. After adjusting the sampling points affected by the obstacles, the reference sampling points are obtained in turn as the current sampling points, and several sampling points are added at a certain distance near the current sampling point. The set of these sampling points and the current sampling point is used as a sampling layer. Until the corresponding sampling layers are set based on all reference sampling points.

[0060] One implementation method of adding a number of sampling points at regular intervals is to add at least two sampling points with the same longitudinal position but different lateral positions near the current sampling point, and to set these sampling points and the current sampling point as a sampling layer.

[0061] One implementation method of moving the position of the first sampling point is: according to the obstacle potential field and the road boundary potential field, the repulsive force of each first sampling point is calculated, and the first sampling point is moved a set distance in the direction of the repulsive force. The distance setting value can be adjusted to find a suitable value.

[0062] Select a sampling point from a sampling layer, then select a sampling point from an adjacent sampling layer, and connect the two sampling points to obtain a path. Through this path planning method, multiple segmented paths can be obtained between adjacent sampling layers. By splicing the segmented paths between different sampling layers, multiple complete paths can be obtained, such as Figure 2 shown. Figure 2 Three sampling layers are shown, each sampling layer has three sampling points, and there are nine segmented paths between two adjacent sampling layers. The segmented paths are spliced ​​together to obtain 27 candidate paths.

[0063] Figure 2 The method of the present invention is illustrated in a scenario where the obstacle potential field and the road boundary potential field are not considered. In this implementation scenario, the obstacle potential field and the road boundary potential field are both 0, and the initial sampling point position adjustment amount is 0, that is, the position can remain unchanged, the number can also remain unchanged, and the sampling layer is directly set based on the initial sampling point.

[0064] Figure 3 , Figure 4 Both illustrate the method of the present invention in a scenario where an obstacle potential field and a road boundary potential field are considered. Figure 3 By increasing the number of first sampling points, the number of sampling layers is increased, and then the number of segmented paths is increased, the mobility of the spliced ​​paths is improved, and the probability of the existence of paths that do not hit obstacles can be increased in the candidate paths. Figure 3 As shown in the figure, in this case, some segmented paths are very close to obstacles, or even the obstacles are on the segmented paths, while some segmented paths are far away from obstacles. By flexibly splicing the paths, there are finally candidate trajectories that can successfully bypass the obstacles, which can meet the safety requirements.

[0065] The optimal trajectory is obtained from the trajectory away from the obstacle, which not only meets the requirement of not colliding with the road boundary but also meets the requirement of trajectory smoothness. Figure 4By adjusting the position of the first sampling point, the generated segmented path is as far away from obstacles as possible while avoiding hitting the road boundary. At this time, the number of candidate trajectories that meet road safety and smoothness increases. In this way, the probability of obtaining the ideal optimal trajectory is significantly improved.

[0066] In the method of the present invention, the factor that determines the obstacle repulsion potential field is the distance between the sampling point and the obstacle. When the sampling point is not in the influence range of the obstacle, the potential energy value it receives is zero; after the sampling point enters the influence range of the obstacle, the greater the distance between the two, the smaller the potential energy value received by the sampling point. The smaller the distance, the greater the potential energy value received by the sampling point.

[0067] In the implementation of the method of the present invention, one way to obtain the obstacle potential field is to calculate through the following function:

[0068]

[0069] Where:

[0070] U obstacle is the obstacle potential field, K obs is the positive proportional coefficient of the obstacle potential field; ρ(p, q0) is the distance from the sampling point to the obstacle; ρ0 is the influence distance of the obstacle potential field. Beyond this distance, the judgment is no longer affected by the obstacle potential field.

[0071] The road boundary potential field is calculated by the following formula:

[0072]

[0073] Where: U road is the road boundary potential field, K road is the road boundary potential field proportionality coefficient, l car，road is the distance from the center of mass of the autonomous vehicle to the road boundary; W car is the car width. When the automatic car approaches the road boundary and the distance is within the range of one car width, the road boundary potential energy value will gradually increase, causing the automatic car to move away from the road boundary.

[0074] According to the potential field of the obstacle, the corresponding repulsive force F is obtained obs The formula is as follows:

[0075]

[0076] According to the road boundary potential field, the corresponding repulsive force F is obtained road The formula is as follows:

[0077]

[0078] Next, the resultant force on each first sampling point is calculated, and the first sampling point is moved along the direction of the resultant force by a set step length to reduce the influence of the obstacle potential field and the road boundary potential field on it.

[0079] After adding the obstacle potential field and the road boundary potential field to guide the adjustment of the sampling points, the generated candidate paths are as follows: Figure 3 , Figure 4 As shown in the two figures, it can be seen that the sampling point positions guided by the potential field are more reasonable, and all the generated paths have obvious obstacle avoidance trends and are smoother.

[0080] In the implementation of the method of the present invention, the direction of the reference line is longitudinal, and the lateral movement depends on the longitudinal movement, that is, if the vehicle needs to move laterally, such as turning, avoiding obstacles, etc., it needs to move laterally through longitudinal movement. Therefore, the method of the present invention is more in line with the actual movement of the vehicle. Set the variable of the lateral position to l and the variable of the longitudinal position to s, then the lateral path polynomial is:

[0081] l(s)=c6·s 5 +c5·s 4 +c4·s 3 +c3·s 2 +c2·s+c1

[0082] Where: s is the longitudinal position; l(s) is the lateral position; c6, c5, c4, c3, c2, c1 are polynomial coefficients. The calculation method of polynomial coefficients is as follows:

[0083] Using the initial position state [l0, l′0, l″0], the final position state [l s , l′ s , l″ s ] and the planned length S, written in the following matrix form, can determine all the coefficients of the quintic polynomial of a path l(s), and splice the segmented paths between different sampling layers together to form a complete long path.

[0084]

[0085] The method of the present invention not only considers whether the trajectory of the automatic vehicle can bypass obstacles, but also considers the smoothness of the driving of the automatic vehicle. As a further improvement to the above implementation method, different planning durations are configured for the candidate paths, so as to obtain candidate trajectories with different average speeds. Through the average speed and the curvature of the candidate trajectory, a smooth trajectory can be obtained to meet the smooth driving of the automatic vehicle.

[0086] In an implementation scheme with different planning durations, the longitudinal trajectory is a fifth-order polynomial with respect to time, as follows:

[0087] s(t)=a6·t 5 +a5·t 4 +a4·t 3 +a3·t 2 +a2·t+a1

[0088] In the formula: t is time, s is longitudinal position, a6, a5, a4, a3, a2, a1 are polynomial coefficients.

[0089] By using a quintic polynomial to describe the longitudinal trajectory, a trajectory with higher smoothness can be obtained with less calculation, thus meeting the real-time requirements for obtaining the optimal trajectory of the autonomous vehicle. The method for solving the polynomial coefficients of the longitudinal trajectory is the same as that of the path, that is, using the state at the initial moment And the state at the end time The following longitudinal trajectory matrix is ​​written for coefficient calculation. Where: T is the planning time, S0, are the longitudinal position, velocity and acceleration at the start time, S T , are the longitudinal position, velocity and acceleration at the end time respectively.

[0090] Longitudinal trajectory matrix:

[0091]

[0092] Calculate all longitudinal trajectories {S1(t), S2(t), ..., S N (t)}, where: N is the number of different durations configured for the candidate trajectory. Traverse each longitudinal trajectory and compare it with the calculated trajectory to find the horizontal and vertical states of the trajectory point.

[0093] For each longitudinal trajectory, a step length Δt is given, and the planning duration T is discretized into t∈{t0, t1, t2, …, T}. Substitute t into the polynomial of the longitudinal trajectory and calculate the longitudinal position at each discrete moment. Then change the vertical position Substituting into the path polynomial l(s), we can obtain the lateral state [ll′l″] corresponding to all discrete moments. So far, the lateral and longitudinal states of the trajectory points at all discrete moments are If there are N1 horizontal paths, and each vertical path has N2 different durations, then N1×N2 candidate trajectories can be generated.

[0094] In the screening of the optimal trajectory, if speed is not considered, only collision detection is required, that is, selecting a trajectory that does not collide with obstacles. Collision detection is implemented through the collision detection function: three circles are made with the center of the front axle, the center of the rear axle, and the midpoint of the two as the center of the circle, and the distance between the front and rear axles as the diameter. The entire body of the vehicle is enclosed in the circle. By calculating the distance from the obstacle to the three centers of the circle, it is determined whether the vehicle collides with the obstacle.

[0095] In the screening of the optimal trajectory, if the speed is taken into consideration, the trajectory with jumps can also be eliminated through the maximum acceleration to meet the requirements of trajectory safety and smoothness.

[0096] Furthermore, if the coordinates of the trajectory points of the trajectory are converted from the Frenet coordinate system to the Cartesian coordinate system, the maximum curvature of the trajectory can be used to eliminate the trajectory that is not smooth enough.

[0097] Assume that in the Frenet coordinate system, the trajectory points are expressed by the six- represents the Frenet state of the autonomous vehicle, where: s is the longitudinal position, is the longitudinal velocity, is the longitudinal acceleration, l is the lateral position, is a function of the longitudinal position, l′ is the derivative of the lateral position with respect to the longitudinal position, and l″ is the second-order derivative of the lateral position with respect to the longitudinal position. In the Cartesian coordinate system, the trajectory point represents the Cartesian state of the automatic vehicle through the sextet [xy θ kva], where: θ is the heading angle, and k is the trajectory curvature. The state conversion between the Frenet coordinate system and the Cartesian coordinate system is realized by the following formula. The schematic diagram of the trajectory conversion between the Frenet coordinate system and the Cartesian coordinate system is shown in Figure 5 shown.

[0098] x=x r -sin(θ r )

[0099] y=y r -cos(θ r )

[0100] θ x =Δθ+θ r

[0101]

[0102]

[0103] Where: subscript r represents the parameter of the reference point, which is the point where the actual position is projected onto the reference line; subscript x represents the parameter of the actual position, which is the position of the point on the trajectory, such as Figure 5 shown.

[0104] When the Cartesian coordinates of the trajectory are obtained, the acceleration a and curvature k of all points on the trajectory are traversed, and the maximum values ​​are taken as the maximum acceleration and maximum curvature of the entire trajectory. The maximum acceleration and maximum curvature can be used to determine whether the trajectory is smooth and eliminate trajectories that do not meet the requirements.

[0105] When the method of the present invention is applied, Cartesian coordinates may be established at the beginning, and data for calculation may be obtained in the Cartesian coordinate system, and then converted into Frenet coordinates. The conversion method is as follows:

[0106]

[0107] l'=(1-k r l)tan(Δθ)

[0108]

[0109]

[0110]

[0111] If there are multiple trajectories that do not collide and meet the requirements of smoothness, the cost of the entire process is further calculated, and the trajectory with the minimum cost is taken as the optimal trajectory. In the implementation of the present invention, a cost calculation method is as follows:

[0112]

[0113] Where cost is the required cost value; w s 、w l 、w t 、w deviate is the weight coefficient, T is the planning duration of the trajectory; J s is the rate of change of longitudinal acceleration, J l is the rate of change of lateral acceleration, It is used to reflect the acceleration change rate of each trajectory point. The purpose of these two items is to punish the trajectory with too fast acceleration change and not smooth enough. Reducing this cost can make the trajectory meet the comfort requirements. T is the planning time of the trajectory. As mentioned above, in one embodiment of the method of the present invention, different planning times are configured for the longitudinal trajectory, so that the trajectory has different speeds. This item punishes the trajectory with too long time. end is the actual lateral position of the trajectory terminal, l target is the preset lateral position of the track terminal, l end -l targetThe purpose is to punish trajectories that deviate from the target position. The cost of each trajectory is calculated through the cost function, and the trajectory with the minimum cost is selected as the optimal trajectory. The optimal trajectory is obtained according to the above (1), and the trajectory points are converted from Frenet coordinates to Cartesian coordinates. The automatic vehicle is controlled to drive according to the optimal trajectory.

[0114] In the process of optimal trajectory screening, collision detection, smoothness detection, and cost function, these three conditions can be used together as the conditions for screening the optimal trajectory, or one or two of them can be selected as the conditions for screening the optimal trajectory. A feasible implementation method is to formulate a screening strategy. In the screening strategy, the vehicle must be collision-free in the trajectory form, followed by being able to drive relatively smoothly, and finally the driving cost is minimized, so that the optimal trajectory must meet at least one condition. In summary, the method of the present invention includes two aspects: path planning and speed planning. In terms of path planning, the lateral position is set to a polynomial of the longitudinal position. First, the reasonable position of the sampling center is calculated through the obstacle potential field and the road boundary potential field. Sampling points are added near the sampling center to form a sampling layer. The sampling layers are connected by polynomials to form path fragments, and multiple fragments are spliced ​​into the desired path. In terms of speed planning, the longitudinal position of the planned path changes with time, the longitudinal trajectory is set as a polynomial about time, different planning durations are configured, and longitudinal trajectory clusters with different speeds are generated. Finally, the horizontal and longitudinal states of the trajectory points at different times are obtained through discretized time. In combination with collision detection, maximum acceleration, maximum curvature, cost function, etc., one or a combination of them is used to screen out the optimal trajectory and convert it into Cartesian coordinates as the trajectory of the automatic vehicle to help the automatic vehicle avoid obstacles and drive smoothly.

[0115] From the implementation of the method of the present invention, the method of trajectory planning of the present invention is not limited to automatic vehicles for various operations, but can also be other intelligent driving vehicles, automatic driving vehicles, unmanned vehicles, etc.

[0116] Through the description of the above implementation methods, those skilled in the art can clearly understand that the method of the present disclosure can be implemented by means of software plus necessary general hardware, and of course can also be implemented by special hardware including special integrated circuits, special CPUs, special memories, special components, etc. In general, all functions performed by computer programs can be easily implemented by corresponding hardware, and the specific hardware structures used to implement the same function can also be various, such as analog circuits, digital circuits or special circuits, etc. However, for the present disclosure, software program implementation is a better implementation method in most cases.

[0117] Although the embodiments of the present invention are described above in conjunction with the accompanying drawings, the present invention is not limited to the above specific embodiments and application fields, and the above specific embodiments are only illustrative and instructive, rather than restrictive. A person of ordinary skill in the art can also make many forms under the guidance of this specification and without departing from the scope of protection of the claims of the present invention, all of which belong to the protection of the present invention.

Claims

1. A trajectory planning method based on potential field guidance, characterized in that: The method comprises the following steps: Based on the road information, sampling points are set along the road reference line in the Frenet coordinate system; Based on the obstacle potential field and the road boundary potential field, the position of the sampling point is adjusted, and the sampling point affected by the obstacle potential field and the road boundary potential field is used as the first sampling point; According to the obstacle potential field and the road boundary potential field, the repulsive force of each first sampling point is calculated, and the first sampling point is moved a set distance in the direction of the repulsive force; A sampling layer is set based on each sampling point, and the sampling points between adjacent sampling layers are connected to obtain a transverse segmented path that changes with the longitudinal path. By splicing the transverse segmented paths, N1 transverse paths related to the longitudinal path are obtained; by making the longitudinal path have N2 different durations, N1×N2 candidate trajectories are obtained, and then the optimal trajectory is obtained, and the optimal trajectory is used as the driving trajectory of the automatic vehicle; N1 and N2 are integers greater than 0; The optimal trajectory is a trajectory that meets the screening conditions; the screening conditions include using a cost function to calculate the cost of each candidate trajectory, the smaller the cost, the better the trajectory; the cost function is as follows: Where: cost is the required cost; w s 、w l 、w t 、w deviate is the weight coefficient, l end is the actual lateral position of the trajectory terminal, l target is the preset horizontal position of the trajectory terminal; T is the planning duration of the trajectory; J s is the rate of change of longitudinal acceleration, J l is the rate of change of lateral acceleration.

2. The method according to claim 1, characterized in that The step of setting a sampling layer based on each sampling point comprises the following steps: Get a sampling point as the current sampling point; Step S: Add at least two sampling points with the same longitudinal position but different lateral positions near the current sampling point, and take the current sampling point and the added sampling point set as a sampling layer; If there are still sampling points for which the sampling layer is not set, the next sampling point is obtained as the current sampling point, and the process returns to step S.

3. The method according to claim 1, characterized in that The obstacle potential field is calculated by the following function: Where: U obstacle is the obstacle potential field, K obs is the positive proportionality coefficient of the obstacle potential field; ρ(p, q0) is the distance from the sampling point to the obstacle; ρ0 is the influence distance of the obstacle potential field; The road boundary potential field is calculated by the following formula: Where: U road is the road boundary potential field, K road is the road boundary potential field proportionality coefficient, l car，road is the distance from the center of mass of the autonomous vehicle to the road boundary; W car For the vehicle width.

4. The method according to claim 1, characterized in that: The candidate trajectory is in the Frenet coordinate system, using Describe the trajectory points; Where s is the longitudinal position, which is a function of time. is the longitudinal velocity, is the longitudinal acceleration; l is the lateral position, which is a function of the longitudinal position, l′ is the derivative of the lateral position with respect to the longitudinal position, and l″ is the second-order derivative of the lateral position with respect to the longitudinal position.

5. The method according to claim 4, characterized in that The lateral position is described by the following fifth-order polynomial: l(s)=c6·s 5 +c5·s 4 +c4·s 3 +c3·s 2 +c2·s+c1 Where: s is the longitudinal position; l(s) is the lateral position; c6, c5, c4, c3, c2, c1 are polynomial coefficients; The longitudinal position is described by a fifth-order polynomial: s(t)=a6·t 5 +a5·t 4 +a4·t 3 +a3·t 2 +a2·t+a1 In the formula: t is time, s is longitudinal position, a6, a5, a4, a3, a2, a1 are polynomial coefficients.

6. A computer-readable storage medium, characterized in that: A computer program is stored which can be loaded by a processor and execute the method according to any one of claims 1 to 5.

Images