A dynamic spatio-temporal voxel decision planning method and device with traffic environment universality

Abstract

A dynamic space-time voxel decision planning method and device with traffic environment universality, the method comprising the following steps: based on the trajectory prediction result of each dynamic obstacle in the road, a plurality of dynamic voxels are generated; based on the dynamic voxels generated by each unit time of all candidate lanes, starting from the current position, for each possible target position, considering the transfer cost between each time or space connected voxel, searching for the voxel sequence with the lowest transfer cost; the voxel sequence searched is taken as a boundary constraint, and an optimized trajectory is further solved. The method reflects the position and speed of the dynamic obstacle on the road into the voxel, which can help the host vehicle to make more reasonable trajectory planning in various scenarios, ensuring safety while obtaining the optimal path.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology, and in particular to a dynamic spatiotemporal voxel decision-making and planning method. Background Technology

[0002] In the entire autonomous driving system, planning is typically a downstream module of prediction. After obtaining the future trajectory of a target obstacle, the planner needs to use the prediction results to plan a safe and comfortable trajectory. Existing planning methods utilize Markov Decision Processes (MDPs) and Partially Observed Markov Decision Processes (POMDPs) to implement lane-changing decisions in high-speed environments. Other methods sample strategies at intersections by building search tree structures, while still others introduce Stackelberg game models for decision-making and planning in converging traffic environments. The problem with these methods is that specific planning schemes are usually only applicable to a single traffic scenario, lacking discussion on the scalability and universality of the methods. Furthermore, switching planners back and forth between different road environments is clearly impractical for complex urban traffic environments. Therefore, designing and using a motion planner that is universally applicable to various road environments is essential.

[0003] In environments with frequent inter-individual interactions, the "spatiotemporal planning" method in autonomous driving technology has attracted considerable attention. Spatiotemporal planning refers to introducing an additional "time" dimension into the existing planning space to better represent the movement of dynamic obstacles and complete planning tasks that are safe, efficient, comfortable, and courteous. The operational process of spatiotemporal planning methods is generally as follows: given the prediction results, based on known waypoints or vehicle motion models, the space where obstacles might occupy is eliminated, resulting in other feasible three-dimensional driving spaces (including the road surface xy two-dimensional plane and the time axis t, referred to as "static spatiotemporal voxels" or "safe driving corridors"). Then, using the passable space as boundary constraints, the optimal planning trajectory is obtained by solving an optimization problem. The above planning methods have the following problems: 1. Voxels only contain static physical quantities such as boundary ranges, and the space contained in a voxel is considered equivalent, resulting in the safety cost of the vehicle landing at any position within the same voxel being equal; 2. Only static topological relationships between voxels are considered, without taking into account the impact of obstacle movement. This approach is unreasonable in certain complex interactive environments or when there is insufficient safety margin. For example, when a vehicle is following another vehicle, and the vehicle in front is traveling at a significantly lower speed than the lead vehicle, getting too close to the front of the voxel (reducing the safety distance) increases the risk of a rear-end collision. Furthermore, most current spatiotemporal planning schemes are only designed for high-speed environments with infrequent interactions and are not applicable to complex environments with various structures such as parallel lanes, curves, lane reductions (increases), ramps, intersections, and roundabouts. In conclusion, using "static spatiotemporal voxels" is unsuitable for dynamic traffic environments with frequent interactions and complex road structures. Summary of the Invention

[0004] This disclosure proposes a general representation method for traversable areas in spatiotemporal planning: dynamic spatiotemporal voxels. Dynamic voxels fully consider the projection and velocity of obstacles on the main road. They can not only represent the traversable area of ​​an autonomous vehicle (hereinafter referred to as "the vehicle") in each candidate lane and each unit of time when there are obstacles on the road, but also carry the dynamic attribute of the velocity projection of the obstacles. This can help the vehicle to make more reasonable trajectory planning and avoidance.

[0005] Based on this concept, the dynamic spatiotemporal voxel decision-making and planning method with universal applicability to traffic environments provided in this disclosure mainly includes the following steps: S1. Based on the trajectory prediction results of each dynamic obstacle in the road, generate multiple dynamic voxels. The attributes of the dynamic voxels include: the area that the main vehicle can pass through in each candidate lane and each unit of time, and the position and speed projection of the dynamic obstacle in the area. S2, based on the dynamic voxels generated for each unit of time for all candidate lanes, starting from the current position, for each possible target position, considers the transfer cost between each voxel connected in time or space, and searches for the voxel sequence with the lowest transfer cost; S3. Using the voxel sequence obtained in step S2 as boundary constraints, further solve for the optimized trajectory.

[0006] Furthermore, the specific method for dynamic voxel generation in step S1 includes: S11, based on kinematic constraints, calculates the area that the main vehicle can reach in each lane and in each unit of time; S12, based on the trajectory prediction results of each dynamic obstacle in the road, remove the space that other vehicles may occupy from the area accessible to the main vehicle, and add the position and velocity projection of the dynamic obstacle in the area to obtain dynamic voxels.

[0007] Furthermore, the specific method of step S11 includes: In the Frenet coordinate system (sd coordinate system with the main vehicle as the origin, where the s direction points to the tangent direction along the direction of lane travel, the d direction is perpendicular to the s direction, and the left-hand coordinate system points to the right and the right-hand coordinate system points to the left), the reachable area of ​​the main vehicle in each reachable lane (which includes the lane the main vehicle is currently in and the adjacent lanes that it may reach) is calculated per unit time, that is, the maximum possible range of each voxel in the length (s direction) and width (d direction).

[0008] The method for calculating the range of voxels in the s-direction is as follows: set up This represents the i-th time point in the planning period, with a unit time length of . Then the time corresponding to the (i+1)th time point is: ; The initial velocity at the start time of the planning period; Representative in planning time Minimum velocity and longitudinal displacement at the initial moment, Representative in planning time Maximum velocity and longitudinal displacement at the end time These are the minimum and maximum speeds, and the minimum and maximum accelerations (these physical quantities are predetermined parameters, usually set manually; the minimum speed is typically 0 m / s to prevent reversing; the maximum speed is related to the vehicle's own speed limit and the maximum speed allowed by the road; the minimum and maximum accelerations are determined by the maximum braking acceleration and maximum acceleration allowed by the vehicle's mechanical structure). Therefore:

[0009]

[0010] (1)

[0011]

[0012] (2) Based on the above formula, the unit time for each accessible lane is calculated. Within, the maximum and minimum range of voxels in the s-direction Furthermore, the specific method of step S12 includes: Determine whether the predicted position of the trajectory of a dynamic obstacle in the road overlaps with the sd space occupied by the voxel (this can be done using a vehicle circumcircle model or a circumrectangle model). If present, then the obstacle vehicle will be... n Location , forward direction and speed , vehicle front facing angle Vehicle body length ,width All are projected onto the SD coordinate system of the main vehicle to determine the intersection scenario between the obstacle vehicle and the main vehicle: Let the angle between the positive s-axis and the global coordinate system be θ in the sd coordinate system of the main vehicle. Then, it is determined by the angle between the frontal orientations of the two vehicles when they meet. The scenarios where the two intersect include the following three situations, let's assume... : (1) Convergence scenario: The two vehicles meet at an acute angle, i.e. Then, vehicles within a certain range of the intersection point, and those closer to the intersection point, are projected onto the SD coordinate system of the main vehicle to form virtual obstacles. The projected value of the vehicle's forward velocity is... The projected length of the vehicle body is ; (2) Meeting scenario: The two vehicles meet at an angle of right or obtuse angle, that is If it is a static obstacle, the projected value of the vehicle's forward velocity is considered to be... ; (3) If there are no obstacles within the space of the voxel, the velocity is recorded as -1.0; Based on the above projection calculations, the space occupied by obstacles is removed, and the velocity projection of the obstacles is obtained as the velocity value estimate of the front and rear boundaries of the dynamic voxel, thereby generating the dynamic voxel. If the length of the voxels after removing the space occupied is greater than a certain threshold Then the voxels will be preserved.

[0013] The obtained dynamic voxels can be represented as ,in: These are the coordinates of the voxel center point. Including two-dimensional coordinates in the Frenet system and the time layer where the voxel is located ; These are the length, width, and height of the voxel, respectively. The calculation method for the range of the length direction is shown in equations (1) and (2) below. The width is usually the lane width, and the height is the length per unit time. ; These are the velocity values ​​at the front and rear edges of the voxel, respectively, and the projected velocity value of the obstacle. These are, respectively, the index of the voxel itself, the index of the voxel's parent voxel (connected by an edge pointing to the voxel), and the index of the voxel's child voxels (connected by an edge pointed to by the voxel).

[0014] Furthermore, step S2 specifically includes: S21, treat each voxel in the dynamic voxels of all generated candidate lanes at each unit time as a node, start from the current position and construct a weighted directed graph to each possible target position, where the weight refers to the transfer cost between adjacent nodes. S22 uses a graph search algorithm to find the route with the lowest transfer cost.

[0015] Furthermore, step S21 specifically includes: According to equation (3), determine and Does a topological connection exist between the two nodes? The average vehicle length (this is mainly to account for errors caused by vehicle volume, and is approximated by the average vehicle length): (3) If a topological connection exists, the transfer cost is further calculated; otherwise, the cost is recorded as positive infinity. The methods for calculating the transfer cost include: For two voxels connected by a time edge, i.e., located in adjacent units of time on the same lane... and Transfer cost The calculation is shown in equation (4): (4)

[0016] ; Among them, The number of voxels generated in the lane where the main vehicle is located per unit time. ,exist per unit time, due to the presence of obstacles, the voxels will... Divided into and Two parts, For two voxels connected by a spatial edge, i.e., located in adjacent lanes, at the same unit of time... and Its transfer cost includes spatial transfer cost and collision time cost, as shown in equations (5) and (6) respectively:

[0017] (5)

[0018] ,

[0019] (6)

[0020]

[0021] in, The average velocity of the voxel can be determined by the minimum and maximum velocity ranges of equations (1) and (2).

[0022] Furthermore, the method in step S3 includes: The voxel sequences with the lowest cost obtained in step S2 are merged into the spatiotemporal tunnel of the main vehicle's travel, and used as boundary constraints. The optimal trajectory solution is obtained by solving a quadratic programming problem. That is, the position sequence in the Frenet coordinate system; wherein, the optimization objective of the quadratic programming problem includes one or more of the following: collision, safe distance, efficiency, distance to the target position, and policy consistency; By transforming the coordinate system, we can obtain its corresponding global coordinates.

[0023] The dynamic spatiotemporal voxel decision-making and planning device applying the above method includes: The dynamic voxel generation module is used to generate multiple dynamic voxels based on the trajectory prediction results of each dynamic obstacle in the road, which represent the area that the main vehicle can pass through in each candidate lane and in each unit of time. The voxel sequence search module is used to search for the voxel sequence with the lowest transfer cost from the dynamic voxels generated at each unit time of all candidate lanes, starting from the current position and considering the transfer cost between each temporally or spatially connected voxel for each possible target position. The trajectory optimization module is used to further solve for the optimized trajectory by using the voxel sequence obtained in the voxel sequence search module as boundary constraints.

[0024] Compared with the prior art, the beneficial effects of this disclosure are: (1) The proposed dynamic voxel design can be applied to different road environments (rather than being limited to parallel driving scenarios such as highways). For scenarios where obstacles such as intersections meet at a certain angle, the concept of "virtual obstacles" is proposed to transform all obstacles into the same coordinate system. (2) The proposed safe driving space is no longer a "static" spatiotemporal corridor, but has two front and rear boundaries with velocity projections. Therefore, the motion trend of voxels can be obtained, which helps the unmanned vehicle (i.e. the master vehicle) to better conduct safety assessments when choosing the best strategy. (3) In order to find a sequence of voxels from the current voxel to the target point among all voxels, a method for evaluating the transfer cost between two adjacent voxels is proposed, including spatial location cost and collision time cost. (4) The generation of dynamic voxels is based on the vehicle's motion model and the prediction results of the future trajectories of surrounding target obstacles, so it can be well combined with prediction models (such as neural network models); (5) The model was tested in a closed-loop environment in an autonomous driving simulation environment. Compared with other deep learning-based end-to-end planners, the proposed model showed better safety performance and efficiency indicators. Attached Figure Description

[0025] The above and other objects, features and advantages of this disclosure will become more apparent from the more detailed description of exemplary embodiments of this disclosure taken in conjunction with the accompanying drawings, in which the same reference numerals generally represent the same components.

[0026] Figure 1 This is a schematic diagram of an exemplary decision-making and planning method according to this disclosure; Figure 2 This is a schematic diagram of velocity projection in a dynamic voxel in a typical scene; Figure 3 An example diagram illustrating the connection relationships between dynamic voxels; Figure 4 This is a typical scenario for dynamic voxel space transfer; Figure 5Planning process for unmanned vehicles in intersection scenarios; Figure 6 Planning process for autonomous vehicles in convergence scenarios; Figure 7 Planning process for autonomous vehicles to overtake in straight-line scenarios. Detailed Implementation

[0027] Preferred embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure will be thorough and complete, and will fully convey the scope of the present disclosure to those skilled in the art.

[0028] This disclosure provides a dynamic spatiotemporal voxel-based decision-making and planning method applicable to traffic environments. A flowchart of a decision-making and planning process under an exemplary implementation is attached. Figure 1 As shown, the main steps include: First, based on the prediction results of other dynamic obstacles (a), multiple dynamic voxels are generated using the method proposed in this disclosure (b); considering the transfer cost between each temporally or spatially connected voxel (c), the optimal voxel sequence is searched; and it is used as a boundary constraint to solve for the optimized trajectory result (d).

[0029] The following is a further explanation of each step.

[0030] Step 1: Voxel generation voxels It is a three-dimensional cube built in the Frenet coordinate system (sd coordinate axis) and the time axis (t coordinate axis), and its physical properties include ,in This refers to the coordinates of the center point of the voxel. These represent the length, width (usually the width of the path), and height (usually the length per unit time) of the voxel, respectively, from which the coordinates of the six boundaries of the voxel can be derived. These represent the velocity values ​​of the front and rear boundaries of the voxel, respectively, and are the projected velocity values ​​of the obstacle. The designation representing a voxel. These represent the parent node label and child node label of the voxel node, respectively.

[0031] During the planning process, lanes and time intervals are typically used. voxels are generated as units. A voxel is actually the vehicle's position per unit time. The set of all possible positions on the corresponding lane is inherent, so determining the length range of the voxel is particularly important.

[0032] Assuming in voxels within time The length range has been determined, then in voxels within time The length range is determined by equations (1) and (2), where It is the initial velocity at the start time of the planning period. Representative in planning time Minimum velocity and longitudinal displacement at the initial moment, Representative in planning time Maximum velocity and longitudinal displacement at the end of the time. These are the minimum and maximum speeds, and the minimum and maximum accelerations, respectively.

[0033]

[0034]

[0035] (1)

[0036]

[0037] (2) Analysis shows that the initial position corresponds to the distance the vehicle travels with maximum deceleration (until it stops), and the final position corresponds to the distance traveled with maximum acceleration (until it reaches maximum speed and then travels at a constant speed). Therefore, theoretically, all positions within the length range are reachable for the vehicle under the constraints of speed and acceleration.

[0038] The aforementioned method is used to determine the reachable space per unit time for each feasible lane. Based on the corresponding prediction information, occupied positions within the space are removed and dynamic properties are added. According to different road structures in urban environments, the characteristics of encountered obstacles under different scenarios are discussed, and a general method for representing obstacle and non-obstacle spaces is proposed.

[0039] The specific method involves determining whether the predicted location overlaps with the SD space occupied by the voxel (this can be done using a vehicle circumcircle model or a circumrectangle model, etc.). If so, the obstacle vehicle is... n Location , forward direction and speed , vehicle front facing angle Vehicle body (length) ,width All are projected onto the SD coordinate system. Assume that in the SD coordinate system of the main vehicle, the angle between the positive s-axis and the global coordinate system is... Then, it is determined by the angle between the frontal orientations of the two vehicles when they meet. There are three possible scenarios (assuming) ): (1) The two vehicles meet at an acute angle ( ) This applies to parallel lane driving, intersection merging, ramp merging, etc., then the projected value of the vehicle's forward velocity is... The projected length of the vehicle body is , Figure 2 a) Show In special cases, particularly in merging scenarios, virtual obstacles are projected. Vehicles within a certain range (set to 10 meters) of the merging point and closer to the merging point are projected onto the main vehicle's SD coordinate system. This allows for early intervention for vehicles with absolute priority on the main road, ensuring safety. Figure 2 As shown in c); (2) The two vehicles meet at an angle of obtuse angle (including right angle). (This applies to encounter situations such as intersections, where the projected value of the vehicle's forward velocity is...) That is, it is considered a static obstacle, such as Figure 2 As shown in b); (3) There are no obstacles in the space of the voxel, and the speed is recorded as -1.0.

[0040] The obstacle velocity projection method described above forms the velocity value estimation of the front and rear boundaries of the dynamic voxel, such as... Figure 3 As shown, the gap between vehicles is a top view of a voxel. The green dashed line with arrows represents the case where there are no obstacles and the speed is -1.0. The blue dashed line represents the speed projection with an acute angle between the speeds, and the red dashed line represents the speed projection with an obtuse angle between the speeds, corresponding to a speed of 0.0.

[0041] Based on the above method, by removing obstacles from the space and calculating the projection velocity, dynamic voxels can be obtained, representing each passable lane and the passable area per unit time. If the length of the voxels after removing obstacles exceeds a certain threshold... The voxels are preserved.

[0042] Step 2: Searching for voxel sequences Based on the dynamic voxels generated for each unit of time across all candidate lanes, the sequence of voxels needs to be searched to reach each possible target location from the current position. To handle this search problem, each voxel is treated as a node, and the edges and weights (costs) between nodes are defined and calculated. A weighted directed graph is constructed, and graph search algorithms such as Dijkstra's algorithm are used to solve the problem.

[0043] There are two types of connections between nodes: one is a "time edge", which means that two voxels are in adjacent units of time in the same lane; the other is a "space edge", which means that two voxels are in the same unit of time in adjacent lanes.

[0044] According to equation (3) (where) (For the length of the vehicle body) Determine whether there is a topological connection between the two. If there is, calculate the transfer cost according to equation (4); if there is no topological connection, the cost is recorded as positive infinity.

[0045] (3) (4)

[0046] ; Vicliptes connected by time edges, such as Figure 3 As shown in a), this can occur in a merging scenario. Assuming in... The number of voxels generated in the lane where the main vehicle is located per unit time. ,exist Within a unit of time, due to the presence of obstacles, the voxel is divided into and Two parts. Then and or and It belongs to the time-related voxels. (By...) arrive Corresponding to "yield to merging vehicles", by arrive The corresponding "vehicles that rush to merge". The cost of the time edge is shown in equation (4). The calculation of the cost of the time edge not only considers the "transition smoothness" between voxels, that is, the proportion of the change in acceleration of the vehicle in a unit of time, but also takes into account the speed of the obstacle by utilizing the front and rear velocity boundary characteristics of the dynamic voxels. The lower the speed of the following vehicle, the lower the cost, and the higher the speed of the preceding vehicle, the lower the cost.

[0047] Spatially related voxels such as Figure 3 As shown in b), this can occur during lane changing in parallel lanes. Assume that voxels are generated in the current lane within the same unit of time. voxels are generated in adjacent lanes. The transfer cost mainly consists of two parts: spatial transfer cost (considering spatially overlapping regions) and collision time cost. Regarding the spatial transfer cost, as marked in the diagram... Angles can be divided into four main categories. Figure 4 Four typical scenarios were shown: 1. When the target lane is larger than the original lane in terms of its front and rear boundaries (or one side is larger and the other side is the same size), the resulting spatial shift is advantageous for lane changing. Figure 4 a), b); 2. Or the target lane is smaller in both the front and rear areas than the original lane (or one side is smaller and the other side is the same size), which is unfavorable for lane changing due to spatial transfer. Figure 4 d)); 3. This manifests as the target lane and the original lane overlapping each other. >0 and Warning: There is an obstacle behind the target lane. Figure 4 c)); 4. Both are negative indications that there is an obstacle ahead of the target lane.

[0048] Based on the above discussion, the included angle between the front ends of the two voxels is utilized. Angle with the back end The cosine value of the voxel is used to convert the impact of different situations on lane changing into spatial position costs, as shown in equation (5). However, the difference in voxel boundary positions still belongs to the static space category. In order to better consider the dynamic characteristics of the vehicle, it is also necessary to combine the motion state of the main vehicle and the motion state of the obstacle vehicle represented by the dynamic voxel. For example, under the same spatial cost, the speed of the vehicle behind the target lane is less than the speed of the main vehicle and the greater the speed difference, the more favorable it is for lane changing. Therefore, by drawing on the concept of "time-to-collision" (TTC) and combining the speed properties of the dynamic voxel, a collision cost is designed, as shown in equation (6), where The average velocity of the voxel can be calculated using the maximum and minimum velocity range of equation (1-2). Unlike the TTC method, which is only applicable to the collision time calculation of vehicles in parallel lanes, the collision cost in this invention relies on dynamic voxels and can be extended to any traffic scenario.

[0049]

[0050] (5)

[0051] ,

[0052] (6)

[0053] . Step 3: Solving for the optimal trajectory under voxel sequence constraints After obtaining the weighted directed graph using the above method, the Dijkstra search method is used to find the voxel sequence with the lowest cost corresponding to different strategies. The voxel sequences are merged into a spatiotemporal tunnel for autonomous vehicle driving and used as a boundary constraint. The strategy is obtained by solving a quadratic programming problem. The corresponding optimal trajectory solution This can be represented as... That is, the position sequence in the Frenet coordinate system, and its corresponding global coordinates can be obtained through coordinate system transformation.

[0054] Next, for each strategy's corresponding trajectory, a forward-looking cycle will be planned. Inside, from the collision ( ), safe distance ( ),efficiency( ), distance from the target location ( Consistency of strategy From the perspective of [missing information], an evaluation function is designed to score the strategy, as shown in equation (7). The strategy with the lowest penalty cost (or the highest reward score) is selected. The trajectory of the strategy is the final planning result. When no candidate strategy exists... When the emergency braking plan is activated, the braking acceleration is determined according to the position of the obstacles in front and behind, and the vehicle travels along the overall planned route.

[0055]

[0056] (7) Collision: This can be determined by methods such as the distance between coordinate center points, the intersection of the circumcircle, and the intersection of the circumcircle rectangle. This invention uses the center point distance method in the Frenet coordinate system. For time layer, time precision is .when Time Obstacles Location relative to the position of the main vehicle satisfy At that time, it was assumed that the cost of the collision between the two was... ,in This means that the function value is 1 when the condition is met, and 0 when the condition is not met.

[0057] Safety distance: Calculate the safety distance cost by referencing the obstacle position projection and velocity projection information of the dynamic voxel. Among them, safe distance The calculation is shown in equation (8). The front boundary of the voxel and its velocity. The back boundary of the voxel and its velocity.

[0058] (8)

[0059] Efficiency: Represented by the average longitudinal velocity of the plan. and ideal speed The gap The distance cost to the target location is the distance between the local target point and the global target point under the current strategy. Subtract the shortest destination distance among all strategies. ,Right now .

[0060] Strategy consistency: Used to constrain the stability of results between adjacent planning cycles and prevent abrupt changes in decision outcomes. For example, when two consecutive decision outcomes include "left lane change" and "right lane change" (regardless of order), .

[0061] Based on the objective function above, the strategy with the lowest cost is selected as the optimal strategy.

[0062] Application Examples Model applications in various traffic environments Figure 5-7 This paper demonstrates the driving process using the dynamic voxel decision-making and planning method proposed in this invention under different traffic scenarios. These include unprotected left turns at intersections, merging and lane changing in dense traffic, and overtaking behavior on straight roads. Specifically: Figure 5 The challenge of the intersection scenario shown is that vehicles from different entrances will converge at multiple angles, requiring the conversion of known obstacle prediction information into obstacle representations in the master vehicle coordinate system. The "dynamic spatiotemporal voxel" proposed in this invention, which is applicable to various scenarios, can solve this problem. Figure 5 a)-h) demonstrates how a vehicle, when making an unprotected left turn, can successfully avoid obstacles and autonomously plan its path to the destination when facing vehicles going straight, turning left, or turning right from the front, rear, and other directions. Figure 6 The challenges of the merging scenario also lie in the different orientations of vehicles merging, and the right-of-way priority between vehicles on the auxiliary road and those on the main road. Because the dynamic voxel generation process considers the projection and velocity of obstacles on the main road, it helps the main vehicle to avoid obstacles in advance; simultaneously, the three-dimensional voxels take into account the positional information of obstacles in the time dimension, thus making it easier for the main vehicle to find suitable opportunities to overtake and seize lanes, such as... Figure 6 c)-e).

[0063] Figure 7The demonstration illustrates the process of a vehicle overtaking, first changing lanes from its original lane to the left lane, and then opportunistically changing back to its original lane. The challenge lies in avoiding collisions with vehicles in front and behind in the target lane during the lane change. The cost transfer calculation method for inter-voxel temporal and spatial edges proposed in this invention effectively considers the lane change gap between the original and target lanes, as well as collision time estimation with vehicles in front and behind, significantly improving the success rate of lane changes and enhancing driving etiquette.

[0064] To test the superiority of this method, comparative tests were conducted in the SMARTS simulation environment for the three tasks mentioned above: unprotected left turns at intersections, merging, and overtaking. Each scenario underwent 50 experiments. Social vehicles were randomly generated using SUMO software (with set traffic density and speed ranges), including different vehicle distributions and initial speeds. Their behavior was controlled by the speed and lane-changing models embedded in the SUMO software. The planning methods used in the comparative experiments included reinforcement learning-based models and rule-based sampling methods, including Twin Delayed Deep Deterministic Policy Gradient (TD3), Soft Actor-Critic (SAC), Proximal Policy Optimization (PPO), and a planning method using polynomial curves to generate candidate sampling trajectories. The experiments were evaluated from two perspectives: task completion success rate (safety) and overall throughput time (efficiency). The test results are shown in Table 1. The experimental results demonstrate that, in terms of success rate, the method proposed in this invention can significantly improve the task success rate. Specifically, in intersection environments where vehicles meet at different angles, this method can promptly perceive the intentions and behaviors of other vehicles and accurately reflect obstacle information in the decision space, improving obstacle avoidance success rate. In merging tasks with dense obstacles, this method can search for the optimal solution in the feasible space (initiating emergency planning when no solution is found). Compared to the zero success rate of other learning-based methods, this invention achieves a 100% success rate. Furthermore, in overtaking tasks, this invention achieves the shortest passing time. The overall success rate reaches 99.3%, far exceeding that of learning-based methods.

[0065] Table 1 shows the comparison results with reinforcement learning models.

[0066] The above technical solutions are merely exemplary embodiments of the present invention. For those skilled in the art, based on the application methods and principles disclosed in the present invention, it is easy to make various types of improvements or modifications, and not limited to the methods described in the specific embodiments of the present invention. Therefore, the methods described above are merely preferred and not restrictive.

Claims

1. A dynamic spatiotemporal voxel-based decision-making and planning method with universal applicability to traffic environments, comprising the following steps: S1. Based on the trajectory prediction results of each dynamic obstacle in the road, generate multiple dynamic voxels. The attributes of the dynamic voxels include: the area that the main vehicle can pass through in each candidate lane and each unit of time, and the position and speed projection of the dynamic obstacle in the area. S2, based on the dynamic voxels generated for each unit of time for all candidate lanes, starting from the current position, for each possible target position, considers the transfer cost between each voxel connected in time or space, and searches for the voxel sequence with the lowest transfer cost; S3, using the voxel sequence obtained in step S2 as boundary constraints, further solve for the optimized trajectory; Step S2 specifically includes: S21, treat each voxel in the dynamic voxels of all generated candidate lanes at each unit time as a node, start from the current position and construct a weighted directed graph to each possible target position, where the weight refers to the transfer cost between adjacent nodes. S22, using a graph search algorithm to find the route with the lowest transfer cost; Step S21 specifically includes: According to formula (3), it is judged whether there is a topological connection between the two nodes, wherein and two nodes exist a topological connection relationship, wherein is the average vehicle body length: (3) If a topological connection exists, the transfer cost is further calculated; otherwise, the cost is recorded as positive infinity. The methods for calculating the transfer cost include: For two voxels connected by a time edge, i.e., located in adjacent units of time on the same lane... and Transfer cost The calculation is shown in equation (4): (4) ； Among them, The number of voxels generated in the lane where the main vehicle is located per unit time. ,exist per unit time, due to the presence of obstacles, the voxels will... Divided into and Two parts, For two voxels connected by a spatial edge, i.e., located in adjacent lanes, at the same unit of time... and Its transfer cost includes spatial transfer cost and collision time cost, as shown in equations (5) and (6) respectively: (5) , (6) in, The average velocity of the voxels.

2. The method according to claim 1, characterized in that, The method for dynamic voxel generation in step S1 includes the following steps: S11, based on kinematic constraints, calculates the area that the main vehicle can reach in each lane and in each unit of time; S12, based on the trajectory prediction results of each dynamic obstacle in the road, remove the space that other vehicles may occupy from the area accessible to the main vehicle, and add the position and velocity projection of the dynamic obstacle in the area to obtain dynamic voxels.

3. The method according to claim 2, characterized in that, The specific method of step S11 includes: In the Frenet coordinate system (sd coordinate system) with the vehicle as the origin, the s direction points tangentially to the direction of travel along the lane, the d direction is perpendicular to the s direction, points to the right in the left-hand coordinate system, and points to the left in the right-hand coordinate system. Calculate the reachable area of ​​the main vehicle in each reachable lane and in each unit of time, that is, the maximum possible range of each voxel in the longitudinal direction (s direction) and the width direction (d direction), wherein the reachable lane includes the lane where the main vehicle is currently located and the adjacent lanes that may be reached. Among them, the unit time for each accessible lane Within this context, the methods for calculating the maximum and minimum ranges of voxels in the s-direction include: set up This represents the i-th time point in the planning period, with a unit time length of . Then the time corresponding to the (i+1)th time point is: ; The initial velocity at the start time of the planning period; Representative in planning time Minimum velocity and longitudinal displacement at the initial moment, Representative in planning time Maximum velocity and longitudinal displacement at the end time These are the minimum and maximum velocities, and the minimum and maximum accelerations, respectively; then: （1） （2） The average velocity of the voxel is obtained from the minimum and maximum velocity ranges of equations (1) and (2).

4. The method according to claim 3, characterized in that, The specific method of step S12 includes: Determine whether the predicted position of the trajectory of a dynamic obstacle in the road overlaps with the sd space occupied by the voxel; If present, then the obstacle vehicle will be... n Location , forward direction and speed , vehicle front facing angle Vehicle length ,width All are projected onto the SD coordinate system of the main vehicle to determine the intersection scenario between the obstacle vehicle and the main vehicle: Let the angle between the positive s-axis and the global coordinate system be θ in the sd coordinate system of the main vehicle. Then, it is determined by the angle between the frontal orientations of the two vehicles when they meet. The scenarios where the two intersect include the following three situations, let's assume... : (1) Convergence scenario: The two vehicles meet at an acute angle, i.e. Then, vehicles within a certain range of the intersection point, and those closer to the intersection point, are projected onto the SD coordinate system of the main vehicle to form virtual obstacles. The projected value of the vehicle's forward velocity is... The projected length of the vehicle body is ; (2) Meeting scenario: The two vehicles meet at an angle of right or obtuse angle, that is If it is a static obstacle, the projected value of the vehicle's forward velocity is considered to be... ; (3) If there are no obstacles within the space of the voxel, the velocity is recorded as -1.0; Based on the above projection calculations, the space occupied by obstacles is removed, and the velocity projection of the obstacles is obtained. This velocity projection is used as an estimate of the front and rear boundaries of the dynamic voxel, and the dynamic voxel can then be generated. The dynamic voxel is represented as: ;in: Here are the coordinates of the voxel center point. Including two-dimensional coordinates in the Frenet system and the time layer where the voxel is located ; These are the length, width, and height of the voxel, respectively; These are the velocity values ​​at the front and rear edges of the voxel, respectively, and the projected velocity value of the obstacle. These are the voxel's own index, the list of parent voxel numbers, and the list of child voxel numbers, respectively. Parent voxels are connected by an edge pointing to the voxel, and child voxels are connected by an edge pointed to by the voxel.

5. The method according to claim 4, characterized in that, In step S12, a vehicle circumcircle model or circumrectangle model is used to determine whether the predicted position overlaps with the sd space occupied by the voxel.

6. The method according to claim 1, characterized in that, The method in step S3 includes: The voxel sequences with the lowest cost obtained in step S2 are merged into the spatiotemporal tunnel of the main vehicle's travel, and used as boundary constraints. The optimal trajectory solution is obtained by solving a quadratic programming problem. That is, the position sequence in the Frenet coordinate system; wherein, the optimization objective of the quadratic programming problem includes one or more of the following: collision, safe distance, efficiency, distance to the target position, and policy consistency; By transforming the coordinate system, we can obtain its corresponding global coordinates.

7. A dynamic spatiotemporal voxel decision-making and planning device with universal applicability to traffic environments, employing the method described in any one of claims 1-6, characterized in that, include: The dynamic voxel generation module is used to generate multiple dynamic voxels based on the trajectory prediction results of each dynamic obstacle in the road, and to determine the area that the main vehicle can pass through in each candidate lane and in each unit of time. The voxel sequence search module is used to search for the voxel sequence with the lowest transfer cost from the dynamic voxels generated at each unit time of all candidate lanes, starting from the current position and considering the transfer cost between each temporally or spatially connected voxel for each possible target position. The trajectory optimization module is used to further solve for the optimized trajectory by using the voxel sequence obtained in the voxel sequence search module as boundary constraints.

Images