Track optimization method, device and medium

Abstract

The invention relates to a trajectory optimization method and device and a medium, and the method comprises the steps: respectively determining a constraint direction vector of each obstacle according to a decision result of a decision module for each obstacle and the reference trajectory for a plurality of obstacles around a to-be-optimized trajectory point on the reference trajectory; processing the plurality of constraint direction vectors meeting a preset condition to reduce the total number of the constraint direction vectors; and constructing a constraint condition according to the processed constraint direction vector, and optimizing the track point to be optimized according to the constraint condition. The number of constraint conditions can be reduced, so that optimization time consumption is reduced, the stability and success rate of solution of an optimizer are improved, and the efficiency and success rate of the trajectory optimization method are improved.

Description

Technical Field

[0001] The present disclosure relates to the field of autonomous driving technology, and in particular, to a trajectory optimization method, apparatus, electronic device, and storage medium. Background Art

[0002] In autonomous driving tasks, common trajectory planning methods include: optimization-based methods, such as OBCA (Optimal Bounded-Curvature Algorithm), which usually use the sampled or searched trajectory as a reference trajectory, then define constraint conditions and objective functions based on the reference trajectory, and finally solve through an optimizer to obtain the optimized target trajectory. The problem is that it strongly depends on the solving performance of the optimizer. In the case of high problem complexity, the solving efficiency is low and the time consumption is large. In particular, in scenarios with many obstacles, due to the large number of constraint conditions generated by the obstacles, it is difficult for the optimizer to solve.

[0003] In view of this, the present application is specifically proposed. Summary of the Invention

[0004] To solve the above technical problems or at least partially solve the above technical problems, embodiments of the present disclosure provide a trajectory optimization method, device, and medium, which can reduce the number of constraint conditions, thereby reducing the time consumption of optimization solving, improving the stability and success rate of optimizer solving, and improving the trajectory optimization efficiency and success rate.

[0005] In a first aspect, an embodiment of the present disclosure provides a trajectory optimization method, which includes:

[0006] For multiple obstacles around the trajectory point to be optimized on the reference trajectory, respectively determine the constraint direction vectors of each obstacle according to the decision results of the decision module for each obstacle and the reference trajectory;

[0007] Process multiple constraint direction vectors that meet preset conditions to reduce the total number of constraint direction vectors;

[0008] Construct constraint conditions according to the processed constraint direction vectors, and optimize the trajectory point to be optimized according to the constraint conditions.

[0009] In a second aspect, an embodiment of the present disclosure further provides a trajectory optimization device, which includes:

[0010] A determination module, configured to respectively determine the constraint direction vectors of each obstacle according to the decision results of the decision module for each obstacle and the reference trajectory for multiple obstacles around the trajectory point to be optimized on the reference trajectory;

[0011] A processing module, configured to process a plurality of constraint direction vectors that meet preset conditions to reduce the total number of constraint direction vectors;

[0012] An optimization module, configured to construct a constraint condition according to the processed constraint direction vectors, and optimize the to-be-optimized trajectory points according to the constraint condition.

[0013] In a third aspect, an embodiment of the present disclosure further provides an electronic device, including: one or more processors; a storage device configured to store one or more programs; when the one or more programs are executed by the one or more processors, enabling the one or more processors to implement the trajectory optimization method as described above.

[0014] In a fourth aspect, an embodiment of the present disclosure further provides a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, the trajectory optimization method as described above is implemented.

[0015] A trajectory optimization method provided by an embodiment of the present disclosure, for a plurality of obstacles around a to-be-optimized trajectory point on a reference trajectory, respectively determines the constraint direction vectors of each obstacle according to the decision result of each obstacle by a decision module and the reference trajectory; processes a plurality of constraint direction vectors that meet preset conditions to reduce the total number of constraint direction vectors; constructs a constraint condition according to the processed constraint direction vectors, and optimizes the to-be-optimized trajectory points according to the constraint condition, thereby reducing the number of constraint conditions, reducing the time-consuming of optimization solving, improving the stability and success rate of optimizer solving, and achieving the purpose of improving the trajectory optimization efficiency and success rate. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In combination with the accompanying drawings and with reference to the following specific embodiments, the above and other features, advantages, and aspects of the embodiments of the present disclosure will become more obvious. Throughout the accompanying drawings, the same or similar reference numerals represent the same or similar elements. It should be understood that the drawings are schematic, and the original elements and elements are not necessarily drawn to scale.

[0017] Figure 1 Schematic diagram of determining the constraint direction vector of an obstacle in an embodiment Figure 1 ;

[0018] Figure 2 Schematic diagram of the process of trajectory optimization in an embodiment Figure 1 ;

[0019] Figure 3 Schematic diagram of the process of trajectory optimization in an embodiment Figure 2 ;

[0020] Figure 4 Schematic diagram of determining the constraint direction vector of an obstacle in an embodimentFigure 2 ;

[0021] Figure 5 Schematic of an obstacle constraint direction vector in an embodiment Figure 1 :

[0022] Figure 6 Schematic of a trajectory optimization process in an embodiment Figure 3 ;

[0023] Figure 7 Schematic of determining an obstacle constraint direction vector in an embodiment Figure 3 ;

[0024] Figure 8 Schematic of an obstacle constraint direction vector in an embodiment Figure 2 :

[0025] Figure 9 Schematic of a trajectory optimization process in an embodiment Figure 4 ;

[0026] Figure 10 Schematic of determining an obstacle constraint direction vector in an embodiment Figure 4 ;

[0027] Figure 11 Schematic of an obstacle constraint direction vector in an embodiment Figure 3 : [[ID='45']]

[0028] Figure 12 Schematic of a trajectory optimization process in an embodiment Figure 5 ;

[0029] Figure 13 Schematic of determining an obstacle constraint direction vector in an embodiment Figure 5 ;

[0030] Figure 14 Schematic of an obstacle constraint direction vector in an embodiment Figure 4 :

[0031] Figure 15 Schematic of determining an obstacle constraint direction vector in an embodiment Figure 6 ;

[0032] Figure 16 Schematic of a trajectory optimization process in an embodiment Figure 6 ;

[0033] Figure 17 Schematic of determining an obstacle constraint direction vector in an embodiment Figure 7 ;

[0034] Figure 18Schematic diagram of determining the obstacle constraint direction vector in the embodiment Figure 8 ;

[0035] Figure 19 Schematic diagram of determining the obstacle constraint direction vector in the embodiment Figure 9 ;

[0036] Figure 20 Schematic diagram of determining the obstacle constraint direction vector in the embodiment Figure 10 ;

[0037] <{ Figure 21 Schematic diagram of determining the target border of the obstacle in the embodiment;

[0038] Figure 22 Schematic diagram of the structure of an electronic device in the embodiment. Detailed implementation manners

[0039] Embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although some embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Instead, these embodiments are provided to more thoroughly and completely understand the present disclosure. It should be understood that the drawings and embodiments of the present disclosure are only for exemplary purposes and are not used to limit the protection scope of the present disclosure.

[0040] It should be noted that the concepts such as "first" and "second" mentioned in the present disclosure are only used to distinguish different devices, modules or units, and are not used to limit the order or interdependence relationship of the functions performed by these devices, modules or units.

[0041] The names of the messages or information exchanged between multiple devices in the embodiments of the present disclosure are only for illustrative purposes and are not used to limit the scope of these messages or information.

[0042] In the autonomous driving task, common trajectory planning methods include: optimization-based methods, such as OBCA (Optimal Bounded-Curvature Algorithm), which usually use the sampled or searched trajectory as the reference trajectory, then define the constraint conditions and the objective function based on the reference trajectory, and finally solve through an optimizer to obtain the optimized target trajectory. The problem with it is that it strongly depends on the solving performance of the optimizer. In the case of high problem complexity, the solving efficiency is low and the time consumption is large.

[0043] Specifically, in a scenario with many obstacles, due to the large number of constraint conditions generated by the obstacles, it is difficult for the optimizer to solve. Specifically, the constraint conditions generated by the obstacles can be expressed by the following expression: A·X >= b, where A represents the constraint direction vector generated by the obstacles, X represents the position of the vehicle, and b represents the expected distance to be maintained between the vehicle and the obstacles. As Figure 1 shown, ego represents the autonomous vehicle, and each obstacle has an active hyperplane (for example, the hyperplane of obstacle obj1 is the plane pointed to by label 1, and the hyperplane of obstacle obj2 is the plane pointed to by label 2). The vertical direction of the hyperplane is the direction of the corresponding constraint direction vector, as Figure 1 shown by vectors A1 and A2. Thus, there are two constraint conditions generated by the obstacles, namely A1·X >= b and A2·X >= b. The directions of vectors A1 and A2 are the gradients in the optimization solution process, indicating the directions that the optimized trajectory points should move away from to avoid collisions between the vehicle and the obstacles. Usually, the perception result is objective, that is, the direction of the hyperplane of the obstacle is objective. Therefore, the constraint directions (such as A1, A2) are also objective. In this way, the gradient of the optimized trajectory point is the superposition of many different directions A. If the superposed direction is unclear, it will cause difficulties for the optimizer to solve. Therefore, in order to reduce the difficulty of the optimizer to solve and improve the solving efficiency and stability of the optimizer, it is necessary to process the constraint direction vectors generated by the obstacles.

[0044] To address the above problems, the embodiments of the present application start from two aspects, namely, processing the direction and quantity of the constraint direction vectors generated by the obstacles. On the one hand, it can make the gradient of the optimized trajectory point clear, and on the other hand, it can reduce the number of constraint conditions, thereby accelerating the solving efficiency of the optimizer.

[0045] Embodiment 1

[0046] Figure 2 is a flowchart of a trajectory optimization method in an embodiment of the present disclosure. This method can be executed by a trajectory optimization device, which can be implemented in software and / or hardware, and the device can be configured in an electronic device. As Figure 2 shown, this method can specifically include the following steps:

[0047] S210. For multiple obstacles around the trajectory point to be optimized on the reference trajectory, respectively determine the constraint direction vectors of each obstacle according to the decision results of each obstacle by the decision module and the reference trajectory.

[0048] Among them, the reference trajectory is a global path planned on the map according to the starting position and target position of the vehicle. Usually, a search algorithm (such as the A* algorithm, Dijkstra algorithm, etc.) is used to search for an optimal path from the starting point to the ending point in the topological structure of the map. This path is a general driving route that takes into account the connectivity of the road network and traffic rules, but does not consider real-time traffic conditions and obstacles around the vehicle. Therefore, based on the reference trajectory, it is also necessary to optimize the trajectory points on the reference trajectory by combining the real-time obstacles around the vehicle to ensure that the vehicle can drive safely and smoothly.

[0049] The trajectory points to be optimized on the reference trajectory can be the trajectory points closest to the vehicle reference point (this reference point is usually the center point of the vehicle's rear axle) at the current moment among the unoptimized trajectory points on the reference trajectory, or the trajectory points that need to be optimized currently determined according to certain rules. Multiple obstacles around the trajectory points to be optimized can be detected by on-vehicle sensors. The decision-making module will give different decision results for each obstacle so that the planning module can plan a reasonable passing path. The decision results specifically include: forced overtaking, yielding, left avoidance, and right avoidance; among them, forced overtaking means that when the obstacle is in front of the vehicle, it indicates that the vehicle overtakes the obstacle (commonly understood as overtaking); yielding means that when the obstacle is in front of the vehicle, it indicates that the vehicle follows behind the obstacle; left avoidance means that when the obstacle is on the side of the vehicle, it indicates that the vehicle keeps driving on the left side of the obstacle; right avoidance means that when the obstacle is on the side of the vehicle, it indicates that the vehicle keeps driving on the right side of the obstacle.

[0050] In this embodiment, according to the decision results (forced overtaking, yielding, left avoidance or right avoidance) of the decision-making module for each obstacle, combined with the tangent or normal line of the reference trajectory at the trajectory point to be optimized, and the position of the obstacle, the constraint direction vectors of each obstacle are respectively determined, aiming to reduce the number of constraint conditions generated by the obstacle, thereby accelerating the solution efficiency of the optimizer.

[0051] S220. Process multiple constraint direction vectors that meet the preset conditions to reduce the total number of constraint direction vectors.

[0052] Among them, processing multiple constraint direction vectors that meet the preset conditions includes: for multiple constraint direction vectors with the same direction, using one of the constraint direction vectors to replace the multiple constraint direction vectors with the same direction; or, for multiple constraint direction vectors with the included angle between any two constraint direction vectors less than the threshold, using the average value of the multiple constraint direction vectors to replace the multiple constraint direction vectors, so as to achieve the purpose of reducing the number of constraint direction vectors.

[0053] S230 , constructing constraint conditions according to the processed constraint direction vector, and optimizing the trajectory points to be optimized according to the constraint conditions.

[0054] For example, according to the processed constraint direction vector, the constraint conditions are respectively constructed by the following expressions, and the objective function is solved by the optimizer according to all the constraint conditions to obtain the optimized trajectory points;

[0055] A·X>=b;

[0056] Where A represents the obstacle's constraint direction vector, X represents the vehicle's state information, and b represents the desired distance between the obstacle and the vehicle. As the number of obstacle constraint direction vectors decreases, the number of constraint inequalities mentioned above also decreases, thereby reducing the optimization difficulty.

[0057] Example 2

[0058] Based on the above embodiments, Figure 3 , when the decision module determines that an obstacle is to overtake the vehicle, this embodiment provides a specific implementation method for the above step S210, and on the basis of the specific implementation method, the implementation method of step S220 is concretized, as shown in FIG. Figure 3 As shown, the trajectory optimization method includes the following steps: S211, for multiple obstacles around the trajectory point to be optimized on the reference trajectory, when the decision result of the decision module for an obstacle is to overtake, the vehicle heading planned at the trajectory point to be optimized is determined as the direction of the constraint direction vector of the obstacle, and the constraint direction vector of the obstacle is generated with the second target point as the starting point of the constraint direction vector.

[0059] Among them, the second target point is any point on the third straight line, the second straight line is the normal of the reference trajectory at the trajectory point to be optimized, the third straight line is a straight line passing through the second reference point and parallel to the second straight line, and the second reference point is the point on the obstacle that is farthest from the second straight line.

[0060] like Figure 4As shown, when the decision-making module's decision result for an obstacle 310 is to cut in, the vehicle heading N planned at the to-be-optimized trajectory point Q is determined as the direction of the constraint direction vector of the obstacle 310, and the constraint direction vector F of the obstacle 310 is generated with the second target point P2 as the starting point of the constraint direction vector; the second target point P2 is any point on the third straight line 330, the second straight line 320 is the normal line of the reference trajectory 300 at the to-be-optimized trajectory point Q, the third straight line 330 is a straight line passing through the second reference point M2 and parallel to the second straight line 320, and the second reference point M2 is the point on the obstacle 310 that is farthest from the second straight line 320. From Figure 4 It can be seen that the third straight line 330 intercepts the obstacle 310, and all points of the obstacle 310 are located on the left side of the third straight line 330. Thus, as long as it is satisfied that the optimized trajectory point falls on the right side of the third straight line 330, it can be ensured that the vehicle will not collide with the obstacle 310. Therefore, the starting point of the constraint direction vector F is set as any point on the third straight line 330, and the direction of the constraint direction vector is the vehicle heading N planned at the to-be-optimized trajectory point Q.

[0061] S221. For the multiple constraint direction vectors of multiple obstacles with the decision result of cutting in, retain the constraint direction vectors whose starting points are on the third straight line as the second target straight line, and delete the remaining constraint direction vectors.

[0062] Wherein, the second target straight line is the straight line among the multiple third straight lines where the starting points of the multiple constraint direction vectors are located and is the farthest from the second straight line.

[0063] Exemplarily, as Figure 5 shown, for the multiple constraint direction vectors of multiple obstacles with the decision result of cutting in, retain the constraint direction vectors whose starting points are on the third straight line 330 as the second target straight line, and delete the remaining constraint direction vectors; wherein, the second target straight line is the straight line among the multiple third straight lines 330 where the starting points of the multiple constraint direction vectors are located and is the farthest from the second straight line 320. In Figure 5 it, the multiple constraint direction vectors of multiple obstacles with the decision result of cutting in are F1 and F2 respectively. Since the directions of F1 and F2 are the same, F1 and F2 are combined into one vector, and F1 is used to replace F2. This is because if the constraint conditions of F1 are satisfied, the constraint conditions of F2 must be satisfied. In other words, if the optimized trajectory point is located within the area constrained by F1, it must also be located within the area constrained by F2. Thus, the purpose of reducing the number of constraint direction vectors is achieved.

[0064] S231. Construct constraint conditions based on the processed constraint direction vectors, and optimize the trajectory points to be optimized according to the constraint conditions.

[0065] Embodiment III

[0066] Based on the above embodiments, referring to the flowchart of a trajectory optimization method as shown in Figure 6 When the decision result of the decision module for an obstacle is to yield, this embodiment gives a specific implementation manner for the above step S210, and on the basis of this specific implementation manner, the implementation manner of step S220 is specified, as shown in Figure 6 The trajectory optimization method includes the following steps:

[0067] S212. For multiple obstacles around the trajectory points to be optimized on the reference trajectory, when the decision result of the decision module for an obstacle is to yield, determine the direction opposite to the planned vehicle heading at the trajectory point to be optimized as the direction of the constraint direction vector of the obstacle, and generate the constraint direction vector of the obstacle with the first target point as the starting point of the constraint direction vector.

[0068] Wherein, the first target point is any point on the first straight line, the second straight line is the normal line of the reference trajectory at the trajectory point to be optimized, the first straight line is a straight line passing through the first reference point and parallel to the second straight line, and the first reference point is the point on the obstacle that is closest to the second straight line.

[0069] Exemplarily, as shown in Figure 7 When the decision result of the decision module for an obstacle 310 is to yield, determine the direction opposite to the planned vehicle heading N at the trajectory point Q to be optimized as the direction of the constraint direction vector F of the obstacle 310, and generate the constraint direction vector F of the obstacle 310 with the first target point P1 as the starting point of the constraint direction vector F; the first target point P1 is any point on the first straight line 340, the second straight line 320 is the normal line of the reference trajectory 300 at the trajectory point Q to be optimized, the first straight line 340 is a straight line passing through the first reference point M1 and parallel to the second straight line 320, and the first reference point M1 is the point on the obstacle 310 that is closest to the second straight line 320. It can be seen from Figure 7 that the first straight line 340 intercepts the obstacle 310, and all points of the obstacle 310 are located on the right side of the first straight line 340. Thus, as long as the optimized trajectory points fall on the left side of the first straight line 340, it can be ensured that the vehicle will not collide with the obstacle 310. Therefore, the starting point of the constraint direction vector F is set as any point on the first straight line 340, and the direction of the constraint direction vector is the direction opposite to the planned vehicle heading N at the trajectory point Q to be optimized.

[0070] S222. For multiple constraint direction vectors of multiple obstacles with a yielding decision result, retain the constraint direction vector of the first straight line where the starting point is located as the first target straight line, and delete the remaining constraint direction vectors.

[0071] Among them, the first target straight line is the straight line with the shortest distance between the multiple first straight lines where the starting points of the multiple constraint direction vectors are located and the second straight line.

[0072] Exemplarily, as Figure 8 shown, for multiple constraint direction vectors of multiple obstacles with a yielding decision result, retain the constraint direction vector of the first straight line 340 where the starting point is located as the first target straight line, and delete the remaining constraint direction vectors; among them, the first target straight line is the straight line with the shortest distance between the multiple first straight lines 340 where the starting points of the multiple constraint direction vectors are located and the second straight line 320. In Figure 8 it, for multiple constraint direction vectors of multiple obstacles with a yielding decision result which are F1 and F2 respectively, since the directions of F1 and F2 are the same, F1 and F2 are combined into one vector, and F2 is used to replace F1, because if the constraint conditions of F2 are satisfied, the constraint conditions of F1 must be satisfied. In other words, if the optimized trajectory point is located within the area constrained by F2, it must also be located within the area constrained by F1. In this way, the purpose of reducing the number of constraint direction vectors is achieved.

[0073] S232. Construct constraint conditions according to the processed constraint direction vectors, and optimize the trajectory points to be optimized according to the constraint conditions.

[0074] Embodiment 4

[0075] Based on the above embodiments, referring to the flowchart of a trajectory optimization method as Figure 9 shown, when the decision module's decision result for an obstacle is to bypass the obstacle from the left side of the obstacle, this embodiment gives a specific implementation manner for the above step S210, and on the basis of this specific implementation manner, the implementation manner of step S220 is specified, as Figure 9 shown, the trajectory optimization method includes the following steps:

[0076] S213. For multiple obstacles around the trajectory points to be optimized on the reference trajectory, when the decision module's decision result for an obstacle is to bypass the obstacle from the left side of the obstacle, determine the normal line of the reference trajectory at the trajectory points to be optimized, and determine the direction along the normal line away from the obstacle as the direction of the constraint direction vector of the obstacle; generate the constraint direction vector of the obstacle with the third target point on the obstacle as the starting point of the constraint direction vector.

[0077] Wherein, the third target point is any point on a fourth straight line, a fifth straight line is a tangent line of the reference trajectory at the trajectory point to be optimized, the fourth straight line is a straight line passing through a third reference point and parallel to the fifth straight line, and the third reference point is the point on the obstacle that is closest to the fifth straight line.

[0078] Exemplarily, as Figure 10 shown, when the decision result of the decision module for an obstacle 310 is to bypass the obstacle 310 from the left side thereof (i.e., keeping the vehicle driving on the left side of the obstacle), determine the normal line 350 of the reference trajectory 300 at the trajectory point Q to be optimized, and determine the direction of the constraint direction vector F of the obstacle 310 as the direction along the normal line 350 away from the obstacle 310; generate the constraint direction vector F of the obstacle 310 with the third target point P3 on the obstacle 310 as the starting point of the constraint direction vector F; wherein, the third target point P3 is any point on a fourth straight line 360, a fifth straight line 370 is a tangent line of the reference trajectory 300 at the trajectory point Q to be optimized, the fourth straight line 360 is a straight line passing through a third reference point M3 and parallel to the fifth straight line 370, and the third reference point M3 is the point on the obstacle 310 that is closest to the fifth straight line 370. From Figure 10 it can be seen that the fourth straight line 360 intercepts the obstacle 310, and all points of the obstacle 310 are located below the fourth straight line 360. Thus, as long as it is satisfied that the optimized trajectory point falls above the fourth straight line 360, it can be ensured that the vehicle will not collide with the obstacle 310. Therefore, set the starting point of the constraint direction vector F as any point on the fourth straight line 360, and the direction of the constraint direction vector is the direction along the normal line 350 away from the obstacle 310.

[0079] S223. For multiple constraint direction vectors with the same direction, retain the constraint direction vectors whose starting points are on the fourth straight line serving as the reference straight line, and delete the remaining constraint direction vectors.

[0080] Wherein, the reference straight line is the straight line with the shortest distance between the fifth straight line among the multiple fourth straight lines where the starting points of the multiple constraint direction vectors with the same direction are located.

[0081] Exemplarily, as Figure 11 shown, for multiple constraint direction vectors with the same direction, retain the constraint direction vectors whose starting points are on the fourth straight line 360 serving as the reference straight line, and delete the remaining constraint direction vectors; wherein, the reference straight line is the straight line with the shortest distance between the fifth straight line 370 among the multiple fourth straight lines 360 where the starting points of the multiple constraint direction vectors with the same direction are located. InFigure 11 In this case, F2 is used to replace F1. This is because if the constraint conditions of F2 are met, the constraint conditions of F1 must be met. In other words, if the optimized trajectory points are located within the area constrained by F2, they must also be located within the area constrained by F1. Or, if the optimized trajectory points do not collide with the obstacles corresponding to F2, then they definitely will not collide with the obstacles corresponding to F1 either. By processing multiple constraint direction vectors that meet the preset conditions, the purpose of further reducing the total number of constraint direction vectors is achieved, thereby reducing the constraint conditions, which can reduce the solving difficulty of the optimizer and improve the solving efficiency.

[0082] S233. Construct constraint conditions according to the processed constraint direction vectors, and optimize the trajectory points to be optimized according to the constraint conditions.

[0083] Embodiment Five

[0084] Based on the above embodiments, referring to the flowchart of a trajectory optimization method as shown in Figure 12 When the decision result of the decision module for an obstacle is to bypass the obstacle from the right side, this embodiment gives a specific implementation manner for the above step S210, and on the basis of this specific implementation manner, the implementation manner of step S220 is specified. As shown in Figure 12 The trajectory optimization method includes the following steps:

[0085] S214. For multiple obstacles around the trajectory points to be optimized on the reference trajectory, when the decision result of the decision module for an obstacle is to bypass the obstacle from the right side, determine the normal line of the reference trajectory at the trajectory points to be optimized, and determine the direction along the normal line away from the obstacle as the direction of the constraint direction vector of the obstacle; generate the constraint direction vector of the obstacle with the third target point on the obstacle as the starting point of the constraint direction vector.

[0086] Among them, the third target point is any point on the fourth straight line, the fifth straight line is the tangent line of the reference trajectory at the trajectory points to be optimized, the fourth straight line is a straight line passing through the third reference point and parallel to the fifth straight line, and the third reference point is the point on the obstacle that is closest to the fifth straight line. Exemplarily, as shown in Figure 13As shown in the figure, when the decision-making module decides to bypass an obstacle 310 from the right side of the obstacle 310, the normal line 350 of the reference trajectory 300 at the trajectory point Q to be optimized is determined, and the direction of the constraint direction vector F of the obstacle 310 is determined as the direction along the normal line 350 away from the obstacle 310; the constraint direction vector F of the obstacle 310 is generated with the third target point P3 on the obstacle 310 as the starting point of the constraint direction vector F; wherein, the third target point P3 is any point on the fourth straight line 360, the fifth straight line 370 is the tangent line of the reference trajectory 300 at the trajectory point Q to be optimized, the fourth straight line 360 is a straight line passing through the third reference point M3 and parallel to the fifth straight line 370, and the third reference point M3 is the point on the obstacle 310 with the shortest distance to the fifth straight line 370. From Figure 13 It can be seen that the fourth straight line 360 intercepts the obstacle 310, and all points of the obstacle 310 are located above the fourth straight line 360. In this way, as long as the optimized trajectory point falls below the fourth straight line 360, it can be ensured that the vehicle will not collide with the obstacle 310. Therefore, the starting point of the constraint direction vector F is set as any point on the fourth straight line 360, and the direction of the constraint direction vector is the direction along the normal line 350 away from the obstacle 310.

[0087] S224. For multiple constraint direction vectors with the same direction, retain the constraint direction vector with the fourth straight line where the starting point is located as the reference straight line, and delete the remaining constraint direction vectors.

[0088] Wherein, the reference straight line is the straight line with the shortest distance between the fifth straight line among the multiple fourth straight lines where the starting points of the multiple constraint direction vectors with the same direction are located.

[0089] Exemplarily, as Figure 14 shown, for multiple constraint direction vectors with the same direction, retain the constraint direction vector with the fourth straight line 360 where the starting point is located as the reference straight line, and delete the remaining constraint direction vectors; wherein, the reference straight line is the straight line with the shortest distance between the multiple fourth straight lines 360 where the starting points of the multiple constraint direction vectors with the same direction are located and the fifth straight line 370. In Figure 14 , F2 is used to replace F1, because if the constraint condition of F2 is satisfied, the constraint condition of F1 must be satisfied. In other words, if the optimized trajectory point is located within the region constrained by F2, it must also be located within the region constrained by F1, or, if the optimized trajectory point will not collide with the obstacle corresponding to F2, then it will definitely not collide with the obstacle corresponding to F1 either.

[0090] By processing multiple constraint direction vectors that meet the preset conditions, the purpose of further reducing the total number of constraint direction vectors is achieved, thereby reducing the constraint conditions, which can reduce the solution difficulty of the optimizer and improve the solution efficiency.

[0091] S234. Construct constraint conditions based on the processed constraint direction vectors, and optimize the trajectory points to be optimized according to the constraint conditions.

[0092] Generally speaking, through the obstacle constraint direction vectors determined according to the solutions in Embodiment 2 to Embodiment 5 above, the directions of the constraint direction vectors of some different obstacles are the same. For example, Figure 15 as shown, the directions of the constraint direction vectors of Obstacle 1 and Obstacle 2 are the same, and the directions of the constraint direction vectors of Obstacle 3 and Obstacle 4 are the same. In this way, for the obstacles in different azimuth regions around the vehicle (front, rear, left, and right), regardless of the number of obstacles, the directions of the obstacle constraint direction vectors are at most 4, which are the direction same as the vehicle's heading, the direction opposite to the vehicle's heading, the direction perpendicular to and upward from the vehicle's heading, and the direction perpendicular to and downward from the vehicle's heading. Compared with the existing solution where the directions of the obstacle constraint direction vectors are determined according to the objective number of obstacles, that is, there are as many directions of the obstacle constraint direction vectors as there are obstacles, the solution of this embodiment greatly reduces the directions of the obstacle constraint direction vectors, makes the gradient of the optimized trajectory points clear, reduces the solution difficulty of the optimizer, and is beneficial to improving the solution efficiency and stability of the optimizer.

[0093] Embodiment 6

[0094] Based on the above embodiments, this embodiment provides another solution for determining the constraint direction vectors of each obstacle according to the decision results of the decision module for each obstacle and the reference trajectory. As [[ID=ID=15]] Figure 16 shown, it includes the following steps:

[0095] S115a. For multiple obstacles around the trajectory point to be optimized on the reference trajectory, when the decision result of the decision module for an obstacle is to cut in, determine the direction perpendicular to the target border of the obstacle and away from the vehicle as the direction of the constraint direction vector of the obstacle, and generate the constraint direction vector of the obstacle with any point on the straight line where the target border is located as the starting point of the constraint direction vector.

[0096] Exemplarily, as Figure 17As shown, when the decision-making module's decision result for an obstacle is to cut in, the direction perpendicular to the target border E of the said obstacle 310 and away from the vehicle V is determined as the direction of the constraint direction vector A of the said obstacle 310. Taking any point on the straight line where the target border E is located as the starting point of the constraint direction vector, the constraint direction vector A of the said obstacle is generated.

[0097] S115b. When the decision result of the decision-making module for an obstacle is to yield, or to bypass from the left or right of the said obstacle, the direction perpendicular to the target border of the said obstacle and close to the vehicle is determined as the direction of the constraint direction vector of the said obstacle. Taking any point on the straight line where the target border is located as the starting point of the constraint direction vector, the constraint direction vector of the said obstacle is generated. Exemplarily, as Figure 18 shown, when the decision result of the decision-making module for an obstacle is to yield, the direction perpendicular to the target border E of the said obstacle 310 and close to the vehicle V is determined as the direction of the constraint direction vector A of the said obstacle 310. Taking any point on the straight line where the target border E is located as the starting point of the constraint direction vector, the constraint direction vector A of the said obstacle is generated.

[0098] As Figure 19 shown, when the decision result of the decision-making module for an obstacle 310 is to bypass from the left of the said obstacle 310, the direction perpendicular to the target border E of the said obstacle 310 and close to the vehicle V is determined as the direction of the constraint direction vector (such as A1 and A2) of the said obstacle. Taking any point on the straight line where the target border E is located as the starting point of the constraint direction vector (such as A1 and A2), the constraint direction vector of the said obstacle is generated.

[0099] As Figure 20 shown, when the decision result of the decision-making module for an obstacle 310 is to bypass from the right of the said obstacle 310, the direction perpendicular to the target border E of the said obstacle 310 and close to the vehicle V is determined as the direction of the constraint direction vector (such as A1 and A2) of the said obstacle. Taking any point on the straight line where the target border E is located as the starting point of the constraint direction vector (such as A1 and A2), the constraint direction vector (such as A1 and A2) of the said obstacle is generated.

[0100] S125. For multiple constraint direction vectors where the angle between any two of them is less than the threshold, use the average value of the said multiple constraint direction vectors to replace the said multiple constraint direction vectors.

[0101] According to the obstacle constraint direction vectors determined by the above solution, the directions of the constraint direction vectors of different obstacles are different. Therefore, when the number of obstacles is large, the number of obstacle constraint direction vectors is large. In response to this, multiple constraint direction vectors that meet preset conditions can be processed to reduce the total number of constraint direction vectors, thereby reducing the complexity of the optimizer's solution.

[0102] Exemplarily, for multiple constraint direction vectors where the angle between any two constraint direction vectors is less than a threshold, the average value of the multiple constraint direction vectors is used to replace the multiple constraint direction vectors. For example, the angle between constraint direction vector A1 and constraint direction vector A2 is less than the threshold, the angle between constraint direction vector A1 and constraint direction vector A3 is less than the threshold, and the angle between constraint direction vector A2 and constraint direction vector A3 is less than the threshold. Then, constraint direction vectors A1, A2, and A3 are processed, and the average value of constraint direction vectors A1, A2, and A3 is used to replace constraint direction vectors A1, A2, and A3. In this way, the original 3 constraint direction vectors are processed into 1 constraint direction vector, reducing the number of constraint direction vectors. Further, in this embodiment, the determination of the target border of the obstacle is involved. The target border E of the obstacle is determined in the following manner. For an obstacle, a first vector corresponding to each border of the obstacle and a second vector corresponding to each border are generated respectively; the product of the first vector and the second vector of each border is calculated respectively, and the border with the smallest product is determined as the target border of the obstacle. As Figure 21 shown, each border of the obstacle is regarded as a first vector respectively, and first vectors a, b, c, and d are obtained. Then, taking the four vertices of the obstacle as the starting points and the trajectory point Q to be optimized as the ending point, four second vectors are created respectively, and second vectors aa, bb, cc, and dd are obtained. The product of the first vector a and the second vector aa is calculated to obtain the first product, the product of the first vector b and the second vector bb is calculated to obtain the second product, the product of the first vector c and the second vector cc is calculated to obtain the third product, and the product of the first vector d and the second vector dd is calculated to obtain the fourth product. The border corresponding to the minimum value among the first product, the second product, the third product, and the fourth product is used as the target border. For example, if the first product is the smallest, the border corresponding to the first vector a is used as the target border E; if the second product is the smallest, the border corresponding to the second vector b is used as the target border E. The target border determined according to the above solution can assist in determining reasonable constraint direction vectors, so that the optimized trajectory point will not collide with the obstacle and is beneficial to the driving of the vehicle.

[0103] S135. Construct constraint conditions according to the processed constraint direction vectors, and optimize the trajectory point to be optimized according to the constraint conditions.

[0104] Figure 22 The following is a schematic structural diagram of an electronic device in an embodiment of the present disclosure. Specifically, refer to Figure 22 , which shows a schematic structural diagram of an electronic device 500 suitable for implementing the embodiments of the present disclosure. Figure 22 The shown electronic device is merely an example and should not impose any limitations on the functions and usage scope of the embodiments of the present disclosure.

[0105] As Figure 22 shown, the electronic device 500 may include a processing device 501, a ROM 502, a RAM 503, a bus 504, an input / output (I / O) interface 505, an input device 506, an output device 507, a storage device 508, and a communication device 509. The processing device (such as a central processing unit, a graphics processing unit, etc.) 501 can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 502 or the program loaded from the storage device 508 into the random access memory (RAM) 503 to implement the method of the embodiments as described in the present disclosure. In the RAM 503, various programs and data required for the operation of the electronic device 500 are also stored. The processing device 501, the ROM 502, and the RAM 503 are connected to each other through the bus 504. The input / output (I / O) interface 505 is also connected to the bus 504.

[0106] The above description is only a preferred embodiment of the present disclosure and an explanation of the applied technical principles. Those skilled in the art should understand that the scope of the disclosure involved in the present disclosure is not limited to the technical solutions formed by the specific combination of the above technical features, and should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the above disclosure concept. For example, the technical solutions formed by mutually replacing the above features with the (but not limited to) technical features with similar functions disclosed in the present disclosure.

Claims

1. A trajectory optimization method, characterized in that, The method includes: For a plurality of obstacles around an optimized trajectory point on a reference trajectory, respectively determine the constraint direction vectors of the obstacles according to the decision results of the decision module for each obstacle and the reference trajectory; Process the plurality of constraint direction vectors that meet the preset conditions to reduce the total number of constraint direction vectors; Construct constraint conditions according to the processed constraint direction vectors, and optimize the optimized trajectory point according to the constraint conditions.

2. The method according to claim 1, wherein The step of respectively determining the constraint direction vectors of the obstacles according to the decision results of the decision module for each obstacle and the reference trajectory includes: When the decision result of the decision module for an obstacle is to yield, determine the reverse direction of the vehicle heading planned at the optimized trajectory point as the direction of the constraint direction vector of the obstacle, and generate the constraint direction vector of the obstacle with the first target point as the starting point of the constraint direction vector; When the decision result of the decision module for an obstacle is to cut in, determine the vehicle heading planned at the optimized trajectory point as the direction of the constraint direction vector of the obstacle, and generate the constraint direction vector of the obstacle with the second target point as the starting point of the constraint direction vector; Wherein, the first target point is any point on a first straight line, the second straight line is the normal line of the reference trajectory at the optimized trajectory point, the first straight line is a straight line passing through a first reference point and parallel to the second straight line, and the first reference point is the point on the obstacle that is closest to the second straight line; The second target point is any point on a third straight line, the third straight line is a straight line passing through a second reference point and parallel to the second straight line, and the second reference point is the point on the obstacle that is farthest from the second straight line.

3. The method according to claim 2, wherein The step of processing the plurality of constraint direction vectors that meet the preset conditions to reduce the total number of constraint direction vectors includes: For the plurality of constraint direction vectors of the obstacles with the decision result of yielding, retain the constraint direction vectors whose starting point is on the first straight line that is the first target straight line, and delete the remaining constraint direction vectors; Wherein, the first target straight line is the straight line with the shortest distance between the second straight line among the plurality of first straight lines where the starting points of the plurality of constraint direction vectors are located; For the plurality of constraint direction vectors of the obstacles with the decision result of cutting in, retain the constraint direction vectors whose starting point is on the third straight line that is the second target straight line, and delete the remaining constraint direction vectors; Wherein, the second target straight line is the straight line with the farthest distance between the second straight line among the plurality of third straight lines where the starting points of the plurality of constraint direction vectors are located.

4. The method according to claim 1, characterized in that, The step of respectively determining the constraint direction vectors of the obstacles according to the decision results of the decision module for each obstacle and the reference trajectory includes: When the decision result of the decision module for an obstacle is to bypass the obstacle from the left or right side of the obstacle, determine the normal line of the reference trajectory at the optimized trajectory point, and determine the direction away from the obstacle along the normal line as the direction of the constraint direction vector of the obstacle. Generate a constraint direction vector of the obstacle with the third target point on the obstacle as the starting point of the constraint direction vector; Wherein, the third target point is any point on the fourth straight line, the fifth straight line is the tangent line of the reference trajectory at the point to be optimized of the trajectory, the fourth straight line is the straight line passing through the third reference point and parallel to the fifth straight line, and the third reference point is the point on the obstacle that is closest to the fifth straight line.

5. The method according to claim 4, wherein The processing of multiple constraint direction vectors that meet preset conditions to reduce the total number of constraint direction vectors includes: For multiple constraint direction vectors with the same direction, retain the constraint direction vector whose reference straight line where the starting point is located is the fourth straight line, and delete the remaining constraint direction vectors; Wherein, the reference straight line is the straight line with the shortest distance between the fifth straight line among the multiple fourth straight lines where the starting points of the multiple constraint direction vectors with the same direction are located.

6. The method according to claim 1, wherein The determination of the constraint direction vectors of each obstacle according to the decision results of each obstacle by the decision module and the reference trajectory includes: For each obstacle, generate a first vector corresponding to each frame and a second vector corresponding to each frame respectively according to each frame of the obstacle; Calculate the product of the first vector and the second vector of each frame respectively, and determine the frame with the smallest product as the target frame of the obstacle; When the decision result of the decision module for an obstacle is to cut in, determine the direction perpendicular to the target frame of the obstacle and away from the vehicle as the direction of the constraint direction vector of the obstacle, and generate the constraint direction vector of the obstacle with any point on the straight line where the target frame is located as the starting point of the constraint direction vector; When the decision result of the decision module for an obstacle is to give way or detour from the left or right side of the obstacle, determine the direction perpendicular to the target frame of the obstacle and close to the vehicle as the direction of the constraint direction vector of the obstacle, and generate the constraint direction vector of the obstacle with any point on the straight line where the target frame is located as the starting point of the constraint direction vector.

7. The method according to claim 6, wherein The processing of multiple constraint direction vectors that meet preset conditions to reduce the total number of constraint direction vectors includes: For multiple constraint direction vectors with an included angle less than a threshold between any two constraint direction vectors, use the average value of the multiple constraint direction vectors to replace the multiple constraint direction vectors.

8. The method according to claim 1, characterized in that, The construction of constraint conditions according to the processed constraint direction vectors and the optimization of the point to be optimized of the trajectory according to the constraint conditions include: According to the processed constraint direction vectors, construct constraint conditions respectively through the following expressions, and solve the objective function according to all constraint conditions by an optimizer to obtain the optimized trajectory point; A·X >= b; Wherein, A represents the constraint direction vector of the obstacle, X represents the state information of the vehicle, and b represents the expected distance between the obstacle and the vehicle.

9. An electronic device, characterized in that, The electronic device includes: One or more processors; A storage device for storing one or more programs; When the one or more programs are executed by the one or more processors such that the one or more processors implement 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 the program is executed by the processor, it implements the method according to any one of claims 1-8.

Images