Homotopy space-time trajectory planning method, device and storage medium

Abstract

The present invention discloses a homology-like space-time trajectory planning method, device and storage medium, including: dividing the trajectory planning environment into a static environment and a dynamic environment; geometrically modeling the static environment; geometrically modeling the dynamic environment by a Delauney triangulation method; making decisions on different homology-like space-time spaces based on a hierarchical cost mechanism to select the optimal homology-like space-time space; in the optimal homology-like space-time space, generating a coarse path based on search planning; in the optimal homology-like space-time space, optimizing according to the coarse path to generate a fine trajectory. The present invention geometrically models dynamic obstacles by a Delauney triangulation method, which is conducive to quickly finding different homology-like space-time spaces, and adopts a hierarchical cost evaluation mechanism to solve the problem that a single-dimensional cost cannot effectively evaluate and compare the advantages and disadvantages of different state points.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving decision-making and planning, and specifically relates to a homology-type spatiotemporal trajectory planning method, device and storage medium. Background Art

[0002] With the continuous development of autonomous driving technology, the mission requirements of autonomous driving vehicles have not only covered specific scenarios such as highways and elevated roads, but also involved general urban road application scenarios. In these scenarios, autonomous driving vehicles need to interact a lot with other traffic participants, such as vehicles and pedestrians.

[0003] In order to ensure safe and efficient driving of vehicles, spatiotemporal trajectory planning technology is increasingly being used. Currently, most of the spatiotemporal joint planning algorithm methods based on rules, such as spatiotemporal sampling, model predictive control, and Markov process, are often limited by the problem of explosion of search space dimensions; and methods based on deep learning are often difficult to train and their decision-making process is a "black box", which brings unknown safety risks and cannot be directly used in mass production.

[0004] The space of spacetime can be tailored by means of constraints called homotopy classes. Two trajectories are called of the same homotopy class if they can be transformed from one to the other continuously without intersecting any obstacles while keeping their starting and ending endpoints fixed.

[0005] In scenarios where the vehicle interacts with obstacles, spatiotemporal joint planning is often required to generate safe and efficient trajectories. However, spatiotemporal joint planning is subject to the dimensional explosion problem of the exploration space and cannot complete the exploration of effective trajectories in space and time and the decision-making of feasible spacetimes of different homology classes under real-time constraints. Summary of the invention

[0006] In order to solve the above technical problems, the present invention proposes a homotopy-type spatiotemporal trajectory planning method, device and storage medium.

[0007] In order to achieve the above object, the technical solution of the present invention is as follows:

[0008] In a first aspect, the present invention discloses a homotopy-like spatiotemporal trajectory planning method, comprising:

[0009] Step S1: dividing the trajectory planning environment into a static environment including only static obstacles and a dynamic environment including only dynamic obstacles;

[0010] Step S2: geometrically modeling the static environment to implement path planning;

[0011] Step S3: geometrically modeling the dynamic environment through the Delauney triangulation method to obtain different homology-like space-time spaces;

[0012] Step S4: Make decisions on different homology-like space-time spaces based on the hierarchical cost mechanism and select the optimal homology-like space-time space;

[0013] Step S5: generating a coarse path based on the search plan in the optimal homology-like space-time space;

[0014] Step S6: In the optimal homology-like space-time space, optimize according to the coarse path to generate a fine trajectory.

[0015] Based on the above technical solution, the following improvements can be made:

[0016] As a preferred solution, step S2 is specifically as follows:

[0017] Path planning is performed in a static environment to obtain a feasible path and a drivable corridor corresponding to the path.

[0018] As a preferred solution, step S3 includes:

[0019] Step S3.1: In the dynamic environment, each dynamic obstacle is regarded as a node, and all nodes, the left and right boundary points in front of the vehicle, and the starting point of the feasible path are formed into TriangleGraph 1 using the Delauney triangulation method;

[0020] Step S3.2: Starting from the starting point of the feasible path, the midpoint of each edge in TriangleGraph 1 is used as a new node to obtain TriangleGraph 2;

[0021] Step S3.3: According to the fact that each edge in TriangleGraph 2 is a path that does not collide with any dynamic obstacle in topology, different homology-like space-time spaces are obtained.

[0022] As a preferred solution, step S4 includes:

[0023] Step S4.1: Obtain static obstacle collision cost, dynamic obstacle collision cost, relative feasible path deviation cost, and relative target point distance field cost;

[0024] Step S4.2: Introduce a hierarchical cost mechanism and assign a corresponding priority to each cost;

[0025] Step S4.3: According to the hierarchical cost mechanism, the optimal homology-like space-time space is determined.

[0026] In a second aspect, the present invention further discloses a homotopy-type spatiotemporal trajectory planning device, comprising:

[0027] An environment division module, used for dividing the trajectory planning environment into a static environment including only static obstacles and a dynamic environment including only dynamic obstacles;

[0028] Static environment modeling module, used for geometric modeling of static environment to realize path planning;

[0029] The dynamic environment modeling module is used to geometrically model the dynamic environment through the Delauney triangulation method to obtain different homology-like space-time spaces;

[0030] The decision module is used to make decisions on different homology-like space-time spaces based on the hierarchical cost mechanism and select the optimal homology-like space-time space;

[0031] A coarse path generation module, used to generate a coarse path based on search planning in an optimal homology-like space-time space;

[0032] The fine path generation module is used to generate fine trajectories by optimizing the coarse path in the optimal homology-like space-time space.

[0033] As a preferred solution, the static environment modeling module is used to perform path planning in a static environment to obtain a feasible path and a drivable corridor corresponding to the path.

[0034] As a preferred solution, the dynamic environment modeling module includes:

[0035] The triangulation unit is used to treat each dynamic obstacle as a node in a dynamic environment, and use the Delauney triangulation method to form a TriangleGraph with all nodes, the left and right boundary points in front of the vehicle, and the starting point of the feasible path;

[0036] An updating unit is used to obtain TriangleGraph 2 starting from the starting point of the feasible path and taking the midpoint of each edge in TriangleGraph 1 as a new node;

[0037] The homology-like space-time space acquisition unit is used to acquire different homology-like space-time spaces according to the fact that each edge in TriangleGraph 2 is a path that has no topological collision with any dynamic obstacle.

[0038] As a preferred solution, the decision module includes:

[0039] The cost acquisition unit is used to obtain the static obstacle collision cost, dynamic obstacle collision cost, relative feasible path deviation cost, and distance field cost relative to the target point;

[0040] The priority assignment unit is used to introduce a hierarchical cost mechanism and assign a corresponding priority to each cost;

[0041] The decision unit is used to decide the optimal homology-like space-time space according to the hierarchical cost mechanism.

[0042] In a third aspect, the present invention further discloses a storage medium storing one or more computer-readable programs, wherein the one or more programs include instructions suitable for being loaded by a memory and executing the above-mentioned homology-type space-time trajectory planning method.

[0043] The present invention discloses a homotopy-type spatiotemporal trajectory planning method, device and storage medium, which have the following beneficial effects:

[0044] First, the present invention uses the Delauney triangulation method to perform geometric modeling on dynamic obstacles, which is conducive to quickly finding different homotopy-like space-time spaces.

[0045] Second, the present invention evaluates and searches for the edges of a limited number of triangles, which greatly reduces the solution space and improves the search efficiency.

[0046] Third, a hierarchical cost evaluation mechanism is adopted to solve the problem that a single-dimensional CST cannot effectively evaluate and compare the advantages and disadvantages of different state points. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0048] Figure 1 A flowchart of a homotopy-like spatiotemporal trajectory planning method provided by an embodiment of the present invention.

[0049] Figure 2 A schematic diagram of a trajectory planning environment provided by an embodiment of the present invention.

[0050] Figure 3 A schematic diagram of a dynamic environment provided by an embodiment of the present invention.

[0051] Figure 4 A schematic diagram of a TriangleGraph provided in an embodiment of the present invention.

[0052] Figure 5 A schematic diagram of the midpoints of each edge of a TriangleGraph provided in an embodiment of the present invention.

[0053] Figure 6 The second schematic diagram of TriangleGraph provided by an embodiment of the present invention.

[0054] Figure 7 Schematic diagram of different homology-like space-time spaces provided for embodiments of the present invention. DETAILED DESCRIPTION

[0055] The preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0056] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0057] Using ordinal numbers “first,” “second,” “third,” etc. to describe common objects merely indicates that different instances of similar objects are involved and is not intended to imply that the objects so described must have a given order in time, space, order, or in any other manner.

[0058] In addition, the expression of “comprising” an element is an “open” expression, which merely means that corresponding components or steps exist, and should not be interpreted as excluding additional components or steps.

[0059] In order to achieve the purpose of the present invention, some embodiments of a homotopy-type spatiotemporal trajectory planning method, device and storage medium are as follows: Figure 1 As shown, the homology-based spatiotemporal trajectory planning method includes:

[0060] Step S1: Figure 2 As shown, the trajectory planning environment is divided into a static environment including only static obstacles and a dynamic environment including only dynamic obstacles;

[0061] Figure 2 In , Ego is the ego car;

[0062] Static is a static obstacle;

[0063] O i (Specifically O1 and O2) are dynamic obstacles;

[0064] Step S2: geometrically modeling the static environment to implement path planning;

[0065] Step S3: geometrically modeling the dynamic environment through the Delauney triangulation method to obtain different homology-like space-time spaces;

[0066] Step S4: Make decisions on different homology-like space-time spaces based on the hierarchical cost mechanism and select the optimal homology-like space-time space;

[0067] Step S5: generating a coarse path based on the search plan in the optimal homology-like space-time space;

[0068] Step S6: In the optimal homology-like space-time space, optimize according to the coarse path to generate a fine trajectory.

[0069] Each step is described in detail below.

[0070] Step S2 is as follows: perform path planning in a static environment and obtain a collision-free path as a feasible path. ref and the drivable corridor PathCorridor corresponding to the path ref .

[0071] The above feasible path Path ref and PathCorridor ref As the initial solution and the initial drivable space, it is used as the decision basis for the optimal homotopy class selection in the subsequent dynamic environment.

[0072] Further, step S3 includes:

[0073] Step S3.1: In a dynamic environment, each dynamic obstacle O i Treat as a node like Figure 3 As shown;

[0074] Using the Delauney triangulation method, all nodes The two boundary points on the left and right in front of the vehicle and the feasible path Path ref The starting point forms a TriangleGraph. like Figure 4 As shown;

[0075] Step S3.2: From the feasible path Path ref Starting from the starting point, take TriangleGraph Each edge midpoint As a new node, get TriangleGraph II like Figure 5-6 As shown;

[0076] Step S3.3: According to TriangleGraph II Each edge E in e are all paths that do not collide with any dynamic obstacle in topology, expressing all possible topological connected spaces formed by dynamic obstacles, and obtaining different homology-like space-time spaces, such as Figure 7 shown.

[0077] The above-mentioned Delauney triangulation method connects a given set of points into non-overlapping triangles that do not contain any points in the inscribed circle. It ensures that the inscribed circle of the generated triangle does not contain any other points, thereby maximizing the "space utilization" of the generated triangle, making the triangle more uniform and closer to equilateral.

[0078] TriangleGraph Each node in represents a generalized obstacle or agent.

[0079] TriangleGraph Each edge between two nodes in Both represent the gap between obstacles, that is, the generalized passable space.

[0080] Further, step S4 includes:

[0081] Step S4.1: Obtain static obstacle collision cost, dynamic obstacle collision cost, relative feasible path deviation cost, and relative target point distance field cost;

[0082] Step S4.2: Introduce a hierarchical cost mechanism and assign a corresponding priority to each cost;

[0083] Step S4.3: According to the hierarchical cost mechanism, the optimal homology-like space-time space is determined.

[0084] By calculating TriangleGraph II The cost of each edge in the network is calculated to complete the total cost calculation from the starting node to the ending node or the termination condition, and the optimal topology and space decision are completed through the path search algorithm based on A* or Dijkstra's.

[0085] Because the shape and state (position, speed, orientation, predicted trajectory) of each dynamic obstacle are different, it is impossible to express the spatiotemporal constraints that an obstacle imposes on the vehicle in the form of points.

[0086] The present invention describes the spatiotemporal soft constraints generated by each obstacle through a variety of different types of costs, including but not limited to the collision cost of static obstacles, the collision cost of dynamic obstacles, the deviation cost relative to the feasible path, the distance field cost relative to the target point, etc.

[0087] \begin{aligned}Cost&

[0088] =Cost^{LevelHigh}+Cost^{LevelMid}

[0089] +Cost^{LevelLow}\\&

[0090] =(Cost^{LevelHigh}_{staticCollsion}

[0091] +Cost^{LevelHigh}_{dynamicCollsion})

[0092] +Cost^{LevelMid}_{Traverse}+(Cost^{LevelLow}_{ref}

[0093] +Cost^{LevelLow}_{target})\end{aligned}

[0094] Furthermore, the present invention introduces a hierarchical cost mechanism, assigning a priority level to each cost to complete the comparison of node and edge costs.

[0095] For example, the cost caused by a collision is more fatal than the cost caused by a feasible path deviation, so its comparison priority will be higher. Therefore, the hierarchical cost mechanism will first compare the cost of the higher priority type, and only compare the lower priority cost if they are the same.

[0096] \begin{aligned}Cost1&

[0097] =Cost1^{LevelHigh}+Cost1^{LevelMid}

[0098] +Cost1^{LevelLow}\\Cost2&

[0099] =Cost2^{LevelHigh}+Cost2^{LevelMid}

[0100] +Cost2^{LevelLow}\\if&Cost1^{LevelHigh}

[0101] >Cost2^{LevelHigh},\\&then Cost1

[0102] >Cost2\\else&ifCost1^{LevelHigh}=

[0103] =Cost2^{LevelHigh}and Cost1^{LevelMid}

[0104] >Cost2^{LevelMid},\\&then Cost1

[0105] >Cost2\\\end{aligned}

[0106] Furthermore, in steps S4 to S6, the optimal homology-like space-time space is determined among multiple feasible different homology-like space-time spaces (in the topologically connected space), and a coarse path is generated in the space. Based on the generation of feasible space-time corridors around the coarse path, the optimization solution is performed to complete the final trajectory generation.

[0107] It is worth noting that in the construction of the space-time corridor, the uncertainty of the geometric shape, speed, direction, etc. of the dynamic obstacles needs to be considered, and described as the corresponding uncertainty cost and safety cost, etc., to form an artificial potential gradient field, and finally generate a more refined and accurate trajectory through optimization methods. In some embodiments of a homotopy-type space-time trajectory planning device, the homotopy-type space-time trajectory planning device includes:

[0108] An environment division module, used for dividing the trajectory planning environment into a static environment including only static obstacles and a dynamic environment including only dynamic obstacles;

[0109] Static environment modeling module, used for geometric modeling of static environment to realize path planning;

[0110] The dynamic environment modeling module is used to geometrically model the dynamic environment through the Delauney triangulation method to obtain different homology-like space-time spaces;

[0111] The decision module is used to make decisions on different homology-like space-time spaces based on the hierarchical cost mechanism and select the optimal homology-like space-time space;

[0112] A coarse path generation module, used to generate a coarse path based on search planning in an optimal homology-like space-time space;

[0113] The fine path generation module is used to generate fine trajectories by optimizing the coarse path in the optimal homology-like space-time space.

[0114] Furthermore, the static environment modeling module is used to perform path planning in a static environment to obtain a feasible path and a drivable corridor corresponding to the path.

[0115] Furthermore, the dynamic environment modeling module includes:

[0116] The triangulation unit is used to treat each dynamic obstacle as a node in a dynamic environment, and use the Delauney triangulation method to form a TriangleGraph with all nodes, the left and right boundary points in front of the vehicle, and the starting point of the feasible path;

[0117] An updating unit is used to obtain TriangleGraph 2 starting from the starting point of the feasible path and taking the midpoint of each edge in TriangleGraph 1 as a new node;

[0118] The homology-like space-time space acquisition unit is used to acquire different homology-like space-time spaces according to the fact that each edge in TriangleGraph 2 is a path that has no topological collision with any dynamic obstacle.

[0119] Furthermore, the decision module includes:

[0120] The cost acquisition unit is used to obtain the static obstacle collision cost, dynamic obstacle collision cost, relative feasible path deviation cost, and distance field cost relative to the target point;

[0121] The priority assignment unit is used to introduce a hierarchical cost mechanism and assign a corresponding priority to each cost;

[0122] The decision unit is used to decide the optimal homology-like space-time space according to the hierarchical cost mechanism.

[0123] In this embodiment, the specific content of the homotopy-type space-time trajectory planning device is similar to the content of the homotopy-type space-time trajectory planning method disclosed in the above embodiment, and will not be repeated here.

[0124] An embodiment of the present invention further discloses a storage medium storing one or more computer-readable programs, wherein the one or more programs include instructions suitable for being loaded by a memory and executing the homology-type space-time trajectory planning method disclosed in the above embodiment.

[0125] When a vehicle interacts with other dynamic traffic participants and there are multiple traversable spatiotemporal solution spaces, it is very important to make decisions and plan to choose a path or trajectory that meets the requirements of passability, safety, and mission.

[0126] The present invention takes into account that autonomous driving vehicles on structured roads or quasi-structured roads can obtain different homotopy paths by modeling the traversable topological relationship between dynamically interacting obstacles to reduce useless space exploration, thereby accelerating exploration efficiency. Then, an environmental description of an artificial potential field is established through a hierarchical cost mechanism to complete the optimal homotopy spatiotemporal space decision, and the corresponding trajectory planning is realized in the optimal spatiotemporal trajectory solution convex space.

[0127] The present invention discloses a homotopy-type spatiotemporal trajectory planning method, device and storage medium, which have the following beneficial effects:

[0128] First, the present invention uses the Delauney triangulation method to perform geometric modeling on dynamic obstacles, which is conducive to quickly finding different homotopy-like space-time spaces.

[0129] Second, the present invention evaluates and searches for the edges of a limited number of triangles, which greatly reduces the solution space and improves the search efficiency.

[0130] Third, a hierarchical cost evaluation mechanism is adopted to solve the problem that a single-dimensional cost cannot effectively evaluate and compare the advantages and disadvantages of different state points.

[0131] It should be understood that the various techniques described herein can be implemented in combination with hardware or software, or a combination thereof. Thus, the method and apparatus of the present invention, or some aspects or portions of the method and apparatus of the present invention, can take the form of program codes (i.e., instructions) embedded in a tangible medium, such as a floppy disk, a CD-ROM, a hard disk drive, or any other machine-readable storage medium, wherein when the program is loaded into a machine such as a computer and executed by the machine, the machine becomes a device for practicing the present invention.

[0132] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for illustrating the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which shall fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A homotopy-type spatiotemporal trajectory planning method, characterized in that: include: Step S1: dividing the trajectory planning environment into a static environment including only static obstacles and a dynamic environment including only dynamic obstacles; Step S2: geometrically modeling the static environment to implement path planning; Step S3: geometrically modeling the dynamic environment through the Delauney triangulation method to obtain different homology-like space-time spaces; Step S4: Make decisions on different homology-like space-time spaces based on the hierarchical cost mechanism and select the optimal homology-like space-time space; Step S5: generating a coarse path based on the search plan in the optimal homology-like space-time space; Step S6: In the optimal homology-like space-time space, optimize according to the coarse path to generate a fine trajectory; The step S2 is specifically as follows: Perform path planning in a static environment to obtain a feasible path and a drivable corridor corresponding to the path; The step S3 comprises: Step S3.1: In the dynamic environment, each dynamic obstacle is regarded as a node, and all nodes, the left and right boundary points in front of the vehicle, and the starting point of the feasible path are formed into TriangleGraph 1 using the Delauney triangulation method; Step S3.2: Starting from the starting point of the feasible path, the midpoint of each edge in TriangleGraph 1 is used as a new node to obtain TriangleGraph 2; Step S3.3: Obtain different homology-like space-time spaces according to the fact that each edge in TriangleGraph 2 is a path that has no topological collision with any dynamic obstacle; The step S4 comprises: Step S4.1: Obtain static obstacle collision cost, dynamic obstacle collision cost, relative feasible path deviation cost, and relative target point distance field cost; Step S4.2: Introduce a hierarchical cost mechanism and assign a corresponding priority to each cost; Step S4.3: According to the hierarchical cost mechanism, the optimal homology-like space-time space is determined.

2. A homotopy-type space-time trajectory planning device, characterized in that: include: An environment division module, used for dividing the trajectory planning environment into a static environment including only static obstacles and a dynamic environment including only dynamic obstacles; Static environment modeling module, used for geometric modeling of static environment to realize path planning; The dynamic environment modeling module is used to geometrically model the dynamic environment through the Delauney triangulation method to obtain different homology-like space-time spaces; The decision module is used to make decisions on different homology-like space-time spaces based on the hierarchical cost mechanism and select the optimal homology-like space-time space; A coarse path generation module, used to generate a coarse path based on search planning in an optimal homology-like space-time space; A fine path generation module is used to generate fine trajectories based on the optimization of the coarse path in the optimal homology-like space-time space; The static environment modeling module is used to perform path planning in a static environment to obtain a feasible path and a drivable corridor corresponding to the path; The dynamic environment modeling module includes: The triangulation unit is used to treat each dynamic obstacle as a node in a dynamic environment, and use the Delauney triangulation method to form a TriangleGraph with all nodes, the left and right boundary points in front of the vehicle, and the starting point of the feasible path; An updating unit is used to obtain TriangleGraph 2 starting from the starting point of the feasible path and taking the midpoint of each edge in TriangleGraph 1 as a new node; A homotopy-like space-time space acquisition unit is used to acquire different homotopy-like space-time spaces according to the fact that each edge in TriangleGraph 2 is a path that has no topological collision with any dynamic obstacle; The decision module comprises: The cost acquisition unit is used to obtain the static obstacle collision cost, dynamic obstacle collision cost, relative feasible path deviation cost, and distance field cost relative to the target point; The priority assignment unit is used to introduce a hierarchical cost mechanism and assign a corresponding priority to each cost; The decision unit is used to decide the optimal homology-like space-time space according to the hierarchical cost mechanism.

3. A storage medium, characterized in that The storage medium stores one or more computer-readable programs, and the one or more programs include instructions, and the instructions are suitable for being loaded by the memory and executing the homotopy-type space-time trajectory planning method described in claim 1.

Images