Autonomous driving corridor optimization construction method and system based on space-time joint planning

Abstract

The application provides an automatic driving corridor optimization construction method and system based on space-time joint planning, comprising: obtaining a current planning initial position, a guide point seed and environment representation information; projecting the guide point seed and the environment representation information to a reference line coordinate system to obtain corresponding Frenet coordinates, and initializing a semantic map according to the current planning initial position; converting the current environment representation information and the guide point seed from the Frenet coordinates to corresponding semantic map coordinates respectively; selecting the initial two seeds to form an initial corridor space and checking whether the semantic map covered by the corridor contains obstacles, if yes, returning an error; if not, inflating the initial corridor space to obtain a passable semantic corridor. The application constructs a drivable area that can pass within a certain time in the future through two aspects of time and space, and improves the rationality of the trajectory obtained by the original path.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving trajectory planning, in particular to an automatic driving corridor optimization construction method and system based on spatiotemporal joint planning. BACKGROUND

[0002] As an important part of future transportation, the related technology development of automatic driving is gradually valued. In automatic driving, trajectory planning plays a key role in connecting the upstream and downstream. A safe, reasonable, comfortable and smooth trajectory is the key to whether the downstream can realize safe tracking and whether the user experience is satisfactory.

[0003] Patent document CN115416693A discloses an automatic driving trajectory planning method and system based on a spatiotemporal corridor, which comprises the following steps: acquiring real-time pose information of a vehicle and obstacles around the vehicle; determining a reference line of the vehicle according to the pose information of the vehicle and constructing a three-dimensional configuration space; identifying the obstacles in the three-dimensional configuration space and intercepting an s-l plane at each target time point to obtain a two-dimensional convex subspace; constructing a three-dimensional convex subspace for all two-dimensional convex subspaces at each adjacent target time point; determining a spatiotemporal corridor for trajectory planning according to a rough trajectory of the vehicle in the planning time domain and all three-dimensional convex subspaces; performing optimization of a longitudinal trajectory and optimization of a lateral trajectory according to the spatiotemporal corridor and the rough trajectory, and determining an automatic driving trajectory according to the optimized longitudinal trajectory and the optimized lateral trajectory. In this method, each adjacent point is constructed, which reduces the drivable area and increases the number of spaces linearly, but causes unnecessary time consumption in the later optimization.

[0004] Therefore, in the automatic driving corridor optimization construction based on spatiotemporal joint planning, under the premise of ensuring that the space completely contains all seeds, it is urgent to reduce the number of generated spaces, obtain a larger optimization space, and reduce the time consumption in the later optimization. SUMMARY

[0005] In view of the defects in the prior art, the purpose of the present application is to provide an automatic driving corridor optimization construction method and system based on spatiotemporal joint planning.

[0006] According to the automatic driving corridor optimization construction method based on spatiotemporal joint planning provided by the present application, the following steps are included:

[0007] Step S1: acquiring a current planning initial position, a guide point seed and environment representation information;

[0008] Step S2: projecting the guide point seed and the environment representation information to a reference line coordinate system to obtain corresponding Frenet coordinates, and initializing a semantic map according to the current planning initial position;

[0009] Step S3: converting the current environment representation information and the guide point seed from Frenet coordinates into corresponding semantic map coordinates respectively;

[0010] Step S4: selecting the initial two seeds to form an initial corridor space and checking whether the corridor covers an obstacle in the semantic map, if yes, returning an error; if not, performing step S5;

[0011] Step S5: inflating the initial corridor space to obtain a passable semantic corridor.

[0012] Preferably, the initialized semantic map comprises a three-dimensional grid map constructed according to the resolution and range of the s-axis, the l-axis and the t-axis with the initial position of the current planning as the origin, and the value of each point in the three-dimensional grid map is initialized to 0;

[0013] Wherein, s represents the longitudinal distance relative to the reference line, l represents the transverse distance relative to the reference line, and t represents the relative time with the current time as the starting time.

[0014] Preferably, the environment representation information comprises dynamic semantic information of the obstacle pose changing over time and static semantic information of the obstacle pose being constant within a time period;

[0015] Converting the current environment representation information from Frenet coordinates into corresponding semantic map coordinates comprises:

[0016] Step S3.1: obtaining the corner position of different obstacles at different time points in sequence for dynamic semantic information, and extending the position of static obstacles to each time point for static semantic information to obtain the static obstacle position coordinates of the corresponding time point;

[0017] Step S3.2: converting the position information into corresponding semantic map coordinates according to the resolution set by the initialized semantic map according to the actual corner Frenet coordinates;

[0018] Step S3.3: judging whether the semantic map coordinates are within the set range, if yes, filling the position occupied by the corresponding obstacle according to the converted obstacle corner coordinates at different time; if not, excluding the position point.

[0019] Preferably, step S5 comprises:

[0020] Step S5.1: selecting the initial two seeds to form a first initial space;

[0021] Step S5.2: judging whether the current initial space collides with an obstacle, if not, inflating the positive and negative directions of the s-axis and the l-axis in sequence; if yes, returning an error and exiting;

[0022] Step S5.3: judging whether the current position point belongs to the last space, if yes, then pushing into the seed corresponding to the space, and taking the last point in the last space and the first point not belonging to the space to dilate; if no, then selecting the last point and the current point to generate a new initial space, and executing step S5.2;

[0023] Step S5.4: pushing the spaces obtained in sequence into the corresponding space list, and finally obtaining the semantic corridor that can be passed.

[0024] Preferably, the dilation is expanded according to a preset dilation step number and in a preset direction; the dilation is determined according to whether there is an obstacle in the current direction, if there is an obstacle, the dilation ends, and if there is no obstacle, the dilation continues.

[0025] When the dilation in all directions ends, the corresponding space dilation ends.

[0026] According to the automatic driving corridor optimization construction system based on space-time joint planning provided by the application, the following steps are included:

[0027] Module M1: obtaining a current planning initial position, a guide point seed and environment representation information;

[0028] Module M2: projecting the guide point seed and the environment representation information to a reference line coordinate system to obtain corresponding Frenet coordinates, and initializing a semantic map according to the current planning initial position;

[0029] Module M3: converting the current environment representation information and the guide point seed from Frenet coordinates to corresponding semantic map coordinates;

[0030] Module M4: selecting the initial two seeds to form an initial corridor space and checking whether the corridor covers an obstacle in the semantic map, if yes, returning an error; if no, triggering module M5;

[0031] Module M5: dilating the initial corridor space to obtain a passable semantic corridor.

[0032] Preferably, the initialized semantic map includes constructing a three-dimensional grid map according to the resolution and range of the s-axis, the l-axis and the t-axis three dimensions respectively with the current planning initial position as the origin, and initializing the value of each point in the three-dimensional grid map to 0.

[0033] Wherein, s represents the longitudinal distance relative to the reference line, l represents the transverse distance relative to the reference line, and t represents the relative time with the current time as the starting time.

[0034] Preferably, the environment representation information comprises dynamic semantic information of changes of the obstacle pose over time, static semantic information of invariability of the obstacle pose over a time period;

[0035] Converting the current environment representation information from the Frenet coordinates into corresponding semantic map coordinates comprises:

[0036] Module M3.1: sequentially obtaining the corner point positions of different obstacles at different time points for the dynamic semantic information, and extending the positions occupied by the static obstacles to each time point for the static semantic information, to obtain the static obstacle position coordinates at the corresponding time points;

[0037] Module M3.2: converting the position information into corresponding semantic map coordinates according to the resolution set in the initialized semantic map according to the actual corner point Frenet coordinates;

[0038] Module M3.3: judging whether the semantic map coordinates are within a set range, if yes, filling the positions occupied by the corresponding obstacles according to the converted obstacle corner point coordinates at different time points; if no, excluding the position point.

[0039] Preferably, the module M5 comprises:

[0040] Module M5.1: selecting the first two seeds to construct the first initial space;

[0041] Module M5.2: judging whether the current initial space collides with the obstacle, if no, expanding the positive and negative directions of the s-axis and the l-axis in turn; if yes, returning an error and exiting;

[0042] Module M5.3: judging whether the current position point belongs to the last space, if yes, pressing the corresponding seed contained in the space, and taking the last point in the last space and the first point not belonging to the space to expand; if no, selecting the last point and the current point to generate a new initial space, and triggering the module M5.2;

[0043] Module M5.4: pressing the spaces obtained in turn into the corresponding space list, and finally obtaining the semantic corridor that can be passed.

[0044] Preferably, the expansion is expanded according to a preset expansion step number and in a preset direction; the expansion is determined according to whether there is an obstacle in the current direction, if there is an obstacle, the expansion ends, and if there is no obstacle, the expansion continues;

[0045] When the expansion of all directions ends, the corresponding space expansion ends.

[0046] Compared with the prior art, the present application has the following beneficial effects:

[0047] 1、The application constructs the drivable area in a certain time in the future through time and space, and obtains a high-order trajectory in the drivable area, thereby improving the rationality of the original path trajectory;

[0048] 2、The application establishes the corridor according to the reference line direction, which can better enhance the correlation between the trajectory passable space and the reference line, and can more accurately obtain the passable area compared with the Cartesian coordinate system in the case of a large curvature of the road, thereby reducing the redundancy caused by completely covering the curve area;

[0049] 3、The application splits the corridor into two levels of horizontal and vertical directions based on time, mainly because under the speed allowed by the urban road condition, considering the horizontal and vertical directions based on time when generating the trajectory can obtain smoother and more reasonable horizontal speed and acceleration;

[0050] 4、The application expands the initial space in an inflation manner, which can obtain a more accurate space within a reasonable maximum limit, thereby improving the success rate of the later trajectory optimization. BRIEF DESCRIPTION OF DRAWINGS

[0051] Other features, objects and advantages of the present application will become more apparent from the following detailed description of non-limiting embodiments with reference to the attached drawings:

[0052] Figure 1 is a schematic diagram of the workflow of the application;

[0053] Figure 2 is a schematic diagram of the process of sequentially inflating the positive and negative directions of the s-axis and the l-axis in the application. DETAILED DESCRIPTION

[0054] The application will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the application, but do not limit the application in any form. It should be pointed out that, for those skilled in the art, without departing from the concept of the application, a number of changes and improvements can be made. These all belong to the protection scope of the application.

[0055] Example 1

[0056] According to the automatic driving corridor optimization construction method based on space-time joint planning provided by the application, as shown in Figure 1 , it comprises:

[0057] Step S1: Obtain the current planning initial position, guide point seed and environment representation information. The environment representation information includes static obstacle pose, dynamic obstacle pose, lane boundary line, stop line, etc. When obtaining, the environment representation is filtered according to the need.

[0058] Step S2: Project the guide point seed and the environment representation information into the reference line coordinate system to obtain corresponding Frenet coordinates (s, l, t), and initialize a semantic map according to the initial position of the current planning. The initialized semantic map includes constructing a three-dimensional grid map according to the resolution and range of the s-axis, l-axis and t-axis three dimensions with the initial position of the current planning as the origin, and initializing the value of each point in the three-dimensional grid map to 0. Wherein, s represents the longitudinal distance relative to the reference line, l represents the transverse distance relative to the reference line, and t represents the relative time with the current time as the starting time.

[0059] Step S3: Convert the current environment representation information and the guide point seed into corresponding semantic map coordinates from Frenet coordinates. The environment representation information obtained in step S1 includes two types, which are dynamic semantic information whose pose changes over time and static semantic information whose pose is constant within a time period. The dynamic semantic information includes the pose of dynamic obstacles, i.e. the obstacle centroid position at the corresponding time, the corner position of the obstacle, etc. The static voice information includes static obstacles, regularly defined drivable boundaries, stop lines, etc. Specifically, converting the current environment representation information from Frenet coordinates into corresponding semantic map coordinates includes:

[0060] Step S3.1: For dynamic semantic information, the corner positions of different obstacles at different times are obtained in sequence, i.e. (s, l, t), and for static semantic information, the positions occupied by static obstacles are extended to each time point to obtain the static obstacle position coordinates at the corresponding time point.

[0061] Step S3.2: According to the actual corner Frenet coordinates, the position information is converted into corresponding semantic map coordinates according to the resolution set in the initialized semantic map.

[0062] Step S3.3: Determine whether the semantic map coordinates are within the set range. If yes, fill in the position occupied by the corresponding obstacle according to the converted obstacle corner coordinates at different times; if not, exclude the position point.

[0063] Step S4: Select the initial two seeds to form an initial corridor space and check whether the semantic map covered by the corridor contains obstacles. If yes, return an error; if not, perform step S5.

[0064] Step S5: expanding the initial corridor space to obtain a passable semantic corridor. Step S5 includes: step S5.1: selecting two initial seeds to construct a first initial space. Step S5.2: judging whether the current initial space collides with an obstacle, if not, expanding the positive and negative directions of the s-axis and the l-axis in turn; if yes, returning an error and exiting. Step S5.3: judging whether the current position point belongs to the last space, if yes, pressing the corresponding seed contained in the space, and taking the last point in the last space and the first point not belonging to the space to expand. If not, selecting the last point and the current point to generate a new initial space, and executing step S5.2. Step S5.4: pressing the spaces obtained in turn into the corresponding space list, and finally obtaining a passable semantic corridor.

[0065] Specifically, the judgment of whether the current position point belongs to the last space is to judge whether the screened original seed belongs to the initial space, if not, the seed is added and the initial space is reconstructed, thereby reducing the number of generated spaces and obtaining a larger space for optimization under the premise of ensuring that the space completely contains all seeds.

[0066] Specifically, the expansion is to expand according to a preset expansion step number and in a preset direction. The expansion is determined according to whether there is an obstacle in the current direction, if there is an obstacle, the expansion ends, if there is no obstacle, the expansion continues. When the expansion of all directions ends, the expansion of the corresponding space ends. That is, as shown in the following figure, the s-axis and the l-axis are expanded in turn in the positive and negative directions. Figure 2

[0067] First, the s positive direction is expanded, a preset expansion step number n_step is set, each step expands a unit length (i.e. resolution length) in the expansion direction, it is judged whether the expansion is within the range of the semantic map, if yes, it is judged in turn whether all l and t corresponding to the current s have obstacles, if yes, the current s positive expansion stops; if no, the expansion direction is added by a unit length and then the obstacle judgment is performed.

[0068] Then, after the s positive direction judgment stops or reaches a specified step number, the s negative direction and the l positive direction and the l negative direction are judged in turn by the same method.

[0069] Next, after a preset period is completed, it is judged whether the corresponding direction is stopped due to the obstacle expansion, if not, the expansion continues.

[0070] Finally, it is judged whether all directions have ended, if yes, the corresponding space expansion ends; if no, the expansion cycle continues.

[0071] Example Two

[0072] ​The application further provides an automatic driving corridor optimization construction system based on spatio-temporal joint planning.

[0073] The automatic driving corridor optimization construction system based on spatio-temporal joint planning provided by the application comprises:

[0074] Module M1: obtaining a current planning initial position, a guide point seed and environment representation information.

[0075] Module M2: projecting the guide point seed and the environment representation information to a reference line coordinate system to obtain corresponding Frenet coordinates, and initializing a semantic map according to the current planning initial position. The initialized semantic map comprises a three-dimensional grid map constructed according to the resolution and range of the s-axis, the l-axis and the t-axis three dimensions with the current planning initial position as the origin, and the value of each point in the three-dimensional grid map is initialized to 0. Wherein, s represents the longitudinal distance relative to the reference line, l represents the transverse distance relative to the reference line, and t represents the relative time with the current time as the starting time.

[0076] Module M3: converting the current environment representation information and the guide point seed from Frenet coordinates to corresponding semantic map coordinates. The environment representation information comprises dynamic semantic information of obstacle poses changing over time and static semantic information of obstacle poses being unchanged in a time period. Converting the current environment representation information from Frenet coordinates to corresponding semantic map coordinates comprises: Module M3.1: sequentially obtaining the corner point positions of different obstacles and different time points for the dynamic semantic information, and extending the positions occupied by the static obstacles to each time point for the static semantic information to obtain the static obstacle position coordinates of the corresponding time points. Module M3.2: according to the actual corner point Frenet coordinates, converting the position information into corresponding semantic map coordinates according to the resolution set in the initialized semantic map. Module M3.3: judging whether the semantic map coordinates are within the set range, if yes, filling the positions occupied by the corresponding obstacles according to the converted obstacle corner point coordinates at different time; if not, excluding the position points.

[0077] Module M4: selecting the initial two seeds to form an initial corridor space and checking whether the semantic map covered by the corridor contains obstacles, if yes, returning an error; if not, triggering module M5.

[0078] Module M5: expanding the initial corridor space to obtain a passable semantic corridor. Specifically, module M5 includes: module M5.1: selecting two initial seeds to construct a first initial space. Module M5.2: determining whether the current initial space collides with an obstacle, if not, expanding in the positive and negative directions of the s-axis and the l-axis in turn; if yes, returning an error and exiting. Module M5.3: determining whether the current position point belongs to the last space, if yes, pressing into the seed contained in the corresponding space, and taking the last point in the last space and the first point not belonging to the space to expand; if not, selecting the last point and the current point to generate a new initial space, triggering module M5.2. Module M5.4: pressing the spaces obtained in turn into the corresponding space list, and finally obtaining a passable semantic corridor.

[0079] The expansion is performed according to a preset expansion step number and in a preset direction; the expansion is determined according to whether there is an obstacle in the current direction, and if there is an obstacle, the expansion ends, and if there is no obstacle, the expansion continues. When the expansion of all directions ends, the corresponding space expansion ends.

[0080] Those skilled in the art know that, in addition to implementing the system, device and each module thereof provided by the present application in a pure computer readable program code manner, the same program can also be implemented in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and each module thereof provided by the present application can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing methods and structures within hardware components.

[0081] The specific embodiments of the present application are described above. It needs to be understood that the present application is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essential content of the present application. The embodiments of the present application and the features in the embodiments can be combined with each other in any manner without conflict.

Claims

1. A method for constructing an autonomous driving corridor based on a spatio-temporal joint planning, characterized in that, The method comprises the following steps: Step S1: obtaining a current planning initial position, a guide point seed and environment representation information; Step S2: projecting the guide point seed and the environment representation information to a reference line coordinate system to obtain corresponding Frenet coordinates, and initializing a semantic map according to the current planning initial position; Step S3: converting the current environment representation information and the guide point seed from the Frenet coordinates to corresponding semantic map coordinates respectively; Step S4: selecting the initial two seeds to form an initial corridor space and checking whether the semantic map covered by the corridor space contains an obstacle, if yes, returning an error; if not, executing Step S5; Step S5: inflating the initial corridor space to obtain a passable semantic corridor; The initialized semantic map comprises constructing a three-dimensional grid map according to the resolution and range of the s-axis, the l-axis and the t-axis three dimensions respectively with the current planning initial position as the origin, and initializing the value of each point in the three-dimensional grid map as 0; Wherein, s represents the longitudinal distance relative to the reference line, l represents the transverse distance relative to the reference line, and t represents the relative time with the current time as the starting time; Step S5 comprises: Step S5.1: selecting the initial two seeds to construct a first initial space; Step S5.2: judging whether the current initial space collides with an obstacle, if not, inflating the positive and negative directions of the s-axis and the l-axis in turn; if yes, returning an error and exiting; Step S5.3: judging whether the current position point belongs to the last space, if yes, pressing the seed contained in the corresponding space, and taking the last point in the last space and the first point not belonging to the space to inflate; if not, selecting the last point and the current point to generate a new initial space, and executing Step S5.2; Step S5.4: pressing the obtained spaces in turn into the corresponding space list to obtain the passable semantic corridor finally.

2. The automatic driving corridor optimization construction method based on space-time joint planning according to claim 1, characterized in that, The environment representation information comprises dynamic semantic information of an obstacle pose changing over time and static semantic information of an obstacle pose being unchanged in a time period; Converting the current environment representation information from the Frenet coordinates to the corresponding semantic map coordinates comprises: Step S3.1: acquiring the corner point positions of different obstacles at different times in turn for the dynamic semantic information, and extending the positions of the static obstacles to each time point for the static semantic information to obtain the static obstacle position coordinates of the corresponding time points; Step S3.2: converting the position information into the corresponding semantic map coordinates according to the resolution set in the initialized semantic map according to the actual corner point Frenet coordinates; Step S3.3: judging whether the semantic map coordinates are within the set range, if yes, filling the positions occupied by the corresponding obstacles according to the converted obstacle corner point coordinates at different times; if not, excluding the position point.

3. The automatic driving corridor optimization construction method based on space-time joint planning according to claim 1, characterized in that, The inflation is expanded according to the preset inflation step number and in the preset direction; the inflation is determined according to whether there is an obstacle in the current direction, if there is an obstacle, the inflation ends, if there is no obstacle, the inflation continues; When the inflation of all directions ends, the corresponding space inflation ends.

4. An automatic driving corridor optimization construction system based on space-time joint planning, characterized in that, The method comprises the following steps: Module M1: obtain the current planning initial position, the guide point seed and the environment representation information; Module M2: project the guide point seed and the environment representation information to the reference line coordinate system to obtain the corresponding Frenet coordinates, and initialize the semantic map according to the current planning initial position; Module M3: convert the current environment representation information and the guide point seed from the Frenet coordinates to the corresponding semantic map coordinates respectively; Module M4: select the initial two seeds to form an initial corridor space and check whether the semantic map covered by the corridor contains obstacles, if yes, return an error; if not, trigger module M5; Module M5: inflate the initial corridor space to obtain a passable semantic corridor; The initialized semantic map includes constructing a three-dimensional grid map according to the resolution and range of the s-axis, l-axis and t-axis three dimensions respectively with the current planning initial position as the origin, and initializing the value of each point in the three-dimensional grid map to 0; Wherein, s represents the longitudinal distance relative to the reference line, l represents the transverse distance relative to the reference line, and t represents the relative time with the current time as the starting time; Module M5 includes: Module M5.1: select the initial two seeds to construct a first initial space; Module M5.2: determine whether the current initial space collides with obstacles, if not, inflate the positive and negative directions of the s-axis and l-axis in turn; if yes, return an error and exit; Module M5.3: determine whether the current position point belongs to the last space, if yes, press into the seed contained in the corresponding space, and take the last point in the last space and the first point not belonging to the space to inflate; if not, select the last point and the current point to generate a new initial space, and trigger module M5.2; Module M5.4: press the obtained spaces in turn into the corresponding space list to obtain the final passable semantic corridor. 5.The automatic driving corridor optimization construction system based on spatio-temporal joint planning according to claim 4, characterized in that, The environment representation information includes dynamic semantic information of the obstacle pose changing over time and static semantic information of the obstacle pose being unchanged within a time period; Converting the current environment representation information from the Frenet coordinates to the corresponding semantic map coordinates includes: Module M3.1: for dynamic semantic information, obtain the corner point positions of different obstacles at different times in turn, and for static semantic information, extend the position occupied by the static obstacle to each time point to obtain the static obstacle position coordinates at the corresponding time point; Module M3.2: according to the actual corner point Frenet coordinates, convert the position information into the corresponding semantic map coordinates according to the resolution set in the initialized semantic map; Module M3.3: determine whether the semantic map coordinates are within the set range, if yes, fill the position occupied by the corresponding obstacle according to the converted obstacle corner point coordinates at different times; if not, exclude the position point. 6.The automatic driving corridor optimization construction system based on spatio-temporal joint planning according to claim 4, characterized in that, The inflation is expanded according to the preset inflation step number and in the preset direction; the inflation is determined according to whether there is an obstacle in the current direction, if there is an obstacle, the inflation ends, if there is no obstacle, the inflation continues; When the inflation of all directions ends, the corresponding space inflation ends.

Images