Multi-driving-behavior parallel vehicle decision planning method based on environment topology

Abstract

The invention belongs to the technical field of intelligent automobiles, and particularly relates to a multi-driving-behavior parallel vehicle decision planning method based on environment topology, which comprises the steps of 1, candidate driving behavior generation based on an environment topological graph, 2, trajectory planning based on candidate driving behaviors, and 3, optimal trajectory screening. The step 1 comprises space-time voxel generation based on a safe reachable time interval, hierarchical environment topological graph construction based on the space-time voxels and driving behavior generation based on the environment topological graph; the step 2 comprises initial track search based on candidate driving strategy behaviors and track space-time joint optimization based on a space-time safety corridor; and finally, outputting an optimal'behavior-track 'combination. According to the method, the initial trajectory consistent with the driving strategy topology can be obtained, joint optimization of the trajectory spatial form and the time progress is realized, diversified candidate driving behaviors with high coverage and low redundancy are generated, and a safety guarantee of full-size collision avoidance is provided for a vehicle by establishing a space-time safety corridor.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of intelligent vehicles, and particularly relates to a multi-driving behavior parallel vehicle decision planning method based on environment topology for an autonomous vehicle. BACKGROUND

[0002] The decision planning of intelligent vehicles mainly includes three types: classic hierarchical type, multi-driving behavior parallel type and integrated type based on learning method. In the multi-driving behavior parallel type, the candidate driving behavior generation module replaces the behavior decision module in the hierarchical architecture which outputs a unique driving behavior in advance, and the trajectory planning module is still retained and a new optimal trajectory screening module is added downstream. The candidate driving behavior generation module first generates multiple results representing potential driving behaviors at one time, then calls the trajectory planning module in parallel with these results as input to generate specific driving trajectories corresponding to each potential driving behavior, and the optimal trajectory screening module evaluates and selects the optimal (lowest cost) trajectory according to a unified cost function. The key to making the parallel decision planning work well lies in providing a set of candidate driving behaviors and corresponding trajectories that sufficiently explore the feasible region of the vehicle, to support comprehensive evaluation of multiple possibilities in the future. However, some existing methods only sample target points of different densities on different target lanes to represent different candidate driving behaviors and generate trajectories, which may lead to redundant driving behavior representation; another part of the methods heuristically pre-selects multiple driving possibilities within each lane, which may also lead to insufficient driving behavior representation. In addition, how to correctly convert the information of a specific driving behavior into guidance and constraints for subsequent trajectory planning to generate an accurately matched driving trajectory still needs further exploration. SUMMARY

[0003] To solve the above technical problems, the present application proposes a multi-driving behavior parallel vehicle decision planning method based on environment topology, which includes: step 1, candidate driving behavior generation based on environment topology graph, step 2, trajectory planning based on candidate driving behavior, and step 3, optimal trajectory screening.

[0004] Step 1 of the candidate driving behavior generation based on environment topology graph includes time-space voxel generation based on safe reachable time interval, hierarchical environment topology graph construction based on time-space voxel, and driving behavior generation based on environment topology graph.

[0005] Step 1.1, the time-space voxel generation based on safe reachable time interval, receives the road environment information provided by the upstream perception and prediction link, models the time-space occupation within the prediction range of the ego vehicle, and updates the time interval of the initialized time-space voxel set using the modeling results, thereby realizing the sparse discrete representation of the time-space feasible region within the prediction range.

[0006] Further, the steps are as follows:

[0007] First, a three-dimensional spatiotemporal coordinate system is constructed by introducing a time dimension (T-axis), and spatiotemporal occupancy modeling of road environmental elements is performed within this coordinate system. A unified space occupancy model is then built for key environmental elements affecting the movement of intelligent vehicles, including road boundaries and obstacle vehicles.

[0008] Consider available road boundary lines The information is a set containing A set of discrete points with global coordinates Furthermore, Described as a union of a series of line segments connected end to end, forming a set of segments. The space occupied by the road boundary line composed of line segments ,in Indicates the spatial location of the endpoints of a line segment. This represents constructing a convex hull for the input point set. For the obstacle vehicle, the perceived rectangular bounding box is defined with length and width as follows: The predicted global continuous driving trajectory is For any time in the prediction time domain Its space occupation can be represented by a convex polygon constructed from a set of points as follows: ,in Indicates the time when the obstacle vehicle is Space occupancy points. Settings. These are the global coordinates of the vertices of the rectangular bounding box of the obstacle vehicle.

[0009] Extending the above two-dimensional space occupancy model to the three-dimensional spatiotemporal domain, line segments in two-dimensional space will form boundary surfaces perpendicular to the XOY plane in the three-dimensional spatiotemporal domain; the spatiotemporal occupancy of the boundary lines is represented as the union of these boundary surfaces; within the time interval... Within, the temporal and spatial occupancy of road boundary lines Formulated as: ,in The j-th line segment of the road boundary line represents the boundary surface formed in the spatiotemporal domain, consisting of the four spatiotemporal endpoints of the line segment, denoted by [symbol missing]. This indicates taking the union of sets.

[0010] To determine the spatiotemporal occupancy of the obstacle vehicle, the entire predicted trajectory is first uniformly sampled. This process yields the space occupancy of obstacle vehicles at several time points; the travel direction of obstacle vehicles between two adjacent sampling times is ignored. The difference between them, the obstacle vehicle in the time period Spacetime occupancy within for:

[0011] in denotes the time length of the predicted trajectory. The global spatio-temporal occupancy of the obstacle vehicle is denoted as .

[0012] Then, a spatio-temporal Frenet coordinate system is constructed and the initialization of the spatio-temporal voxel set is completed in this coordinate system. The Frenet coordinate system describes the spatial position by using the longitudinal distance s along the reference line and the lateral offset perpendicular to the reference line. The Frenet coordinate system is constructed with the right lane line of the current lane of the intelligent vehicle as the reference line; the global position discrete point set of the right lane line is denoted as by using the definition method consistent with the road boundary line; the second-order continuous reference line expression is obtained by using a cubic polynomial spline to interpolate .

[0013] Based on the definition function , the function is defined, which is used to output the corresponding global coordinate position given the coordinate position in the Frenet coordinate system . By using the above continuous reference line expression, the global coordinate position is given, and the uniquely determined is accurately found, and the process is defined as .

[0014] The time dimension (T axis) is further introduced to the above spatial coordinate system to extend it to a spatio-temporal Frenet coordinate system, and the spatial distribution of the spatio-temporal voxel is uniform. For a given S axis and L axis range, the continuous Frenet space is discretized into a uniform rectangular grid with a predefined S axis and L axis resolution .

[0015] The generation range of the voxel is limited in the current lane and the left and right adjacent lanes. Let the lane width be , when the vehicle is located in the middle lane, ; if it is located in the rightmost or leftmost lane, the boundary is changed to , and let .

[0016] A spatio-temporal voxel is initialized on each uniform grid created, and these spatio-temporal voxels have the same time interval , where is the duration of the predicted trajectory of the surrounding obstacle vehicle, so as to make full use of the prediction information.

[0017] Finally, the spatiotemporal voxel time intervals are updated. First, based on the reachability constraints determined by the vehicle's longitudinal travel capability, the time intervals for all voxels are updated for the first time. Given the target's longitudinal position... The car is based on its current longitudinal position and speed The shortest and longest times required to reach this location, and the shortest time. This describes the process of a vehicle starting from its initial position, continuously moving at maximum acceleration until it reaches its maximum speed and finally arrives at the designated target location; the longest time. This corresponds to the situation where the vehicle maintains maximum deceleration throughout the entire motion, until it reaches its minimum speed and finally arrives at the target location. By dynamically adjusting longitudinal acceleration, intelligent vehicles can achieve this within a time frame. Arrive at the target location at any time within the specified time. .

[0018] Based on the above analysis, for all S-axis positions equal to Given the current motion state (position and speed) of an intelligent vehicle, the spatiotemporal free voxels can be accurately calculated to determine the corresponding longitudinal reachable time interval, thereby updating the voxel time interval.

[0019] Further road environment safety constraints are introduced to update the voxel time interval a second time. This update uses the spatiotemporal occupancy model of obstacle vehicles to decompose those voxels that have collisions in the time span into a new set of sub-voxels that inherit the original spatial index according to their safe time sub-intervals, and finally obtains a set of spatiotemporal voxels with safe reachability attributes.

[0020] Obstacle vehicle spatiotemporal occupancy model Mapped to the spacetime Frenet coordinate system Define the spatiotemporal occupancy model by aligning the bounding boxes to the axes in the spatiotemporal Frenet coordinate system. The smallest corner point in the lower left corner is The largest corner point in the upper right corner is By calculating the indices of the two corner points on the S-axis and L-axis, the potential impact can be determined. Affected spacetime voxels. Iterate through all potentially affected spacetime voxels and calculate... Time period occupied Time interval with existing spacetime voxels intersection If the result is If the result is [missing information], it means that the current spacetime voxel is completely occupied by the obstacle vehicle and needs to be deleted; if the result is [missing information], it means that the voxel is completely occupied by the obstacle vehicle and needs to be deleted. If the result is zero, it means that the current spacetime voxel is partially occupied by the obstacle vehicle, and the time interval of the current voxel needs to be updated using the calculation result; if the result is zero... , it indicates that the current spatiotemporal voxel is occupied by the obstacle vehicle and needs to be divided into two parts, and the time interval of the current voxel needs to be updated as , and a new voxel is created at the same spatial position, and its time interval is set as .

[0021] By performing the above operation on all obstacle vehicle spatiotemporal occupancy mapped into the spatiotemporal Frenet coordinate system, the second update of the spatiotemporal voxel is finally completed.

[0022] Step 1.2, hierarchical environment topology construction based on spatiotemporal voxel, is to further abstract the discrete spatiotemporal voxel set into a structured environment topology graph to achieve a high-level understanding of the spatiotemporal feasible region. This process first constructs the local subgraph of each lane independently in units of lanes; then integrates it into a topology graph covering the global environment by connecting adjacent subgraphs. In this graph, nodes are formed by clustering adjacent spatiotemporal voxels in the same lane, and edges are represented by geometric polylines that always lie inside the spatiotemporal voxels.

[0023] Further, the steps are as follows:

[0024] First, local subgraph construction is performed. The spatiotemporal voxels in the same lane have the same lateral index ; the subsequent content uses to represent the spatiotemporal voxel in the current lane. The local subgraph of this lane is composed of nodes formed by clustering these spatiotemporal voxels and intra-lane connected edges connecting adjacent nodes. Each node needs to store the following information: unique number , lateral index of intra-lane spatiotemporal voxel , node type , set of spatiotemporal voxels contained by the node , and node center position . Three types of nodes are defined: entry node, split node, and merge node.

[0025] The entry node: if the time interval of the current spatiotemporal voxel is disjoint (i.e., the intersection is empty) with the time interval of its spatially adjacent voxel , a new entry node is initialized based on voxel .

[0026] The split node: if the current spatial position exists spatiotemporal voxels , and their time intervals are all disjoint with the same spatially adjacent voxel If the time intervals have a non-empty intersection, then an equal number of split nodes are created based on these spatiotemporal voxels.

[0027] The merging node: if the spatiotemporal voxel The time interval and Adjacent spacetime voxels If all time intervals have a non-empty intersection, then based on the current spatiotemporal voxel... Create a merge node.

[0028] The above method sequentially traverses all spatiotemporal voxels within the lane along the S-axis. If the current voxel does not meet the initialization conditions of the above three types of nodes, it is merged into the node where the previous spatially adjacent voxel is located by default.

[0029] After generating nodes, vertical connecting edges are constructed between these nodes. Each connecting edge is a unidirectional polyline that always lies within a spacetime voxel, connecting two interconnected nodes. The center. The basis for constructing a lane-connected edge between two nodes is that the spatiotemporal voxels they contain are spatially adjacent and have a non-empty intersection in terms of time intervals. Based on the above three types of node generation methods, it can be seen that: there are no connected edges between entrance nodes; while split nodes / merging nodes naturally have a connected relationship with their predecessor / successor nodes.

[0030] To form the connected edges within the aforementioned lanes, an iterative update strategy is adopted. First, the center positions of the two nodes are directly connected. (Hereinafter abbreviated as) ( ) is used as the initial connected edge, and samples are taken along the S-axis to obtain a series of sampling points. The L-axis coordinates of these sampling points And ensure sampling points Located at the interface of adjacent voxels in space. Next, we sequentially check whether all sampling points are located within a time interval shared by adjacent voxels. Assume... Located in voxels On the boundary surface, if If the current connected edge is deemed unsafe, subsequent checks will be stopped and the connection will be terminated. The axis coordinates are reset to the center of the shared time interval between adjacent voxels, and then the sampling point is inserted into the existing connected edge. At this point, the connected edge consists of two straight line segments, respectively... Repeat the above sampling and detection process for these line segments until all line segments within the connected edges are located in the spatiotemporal voxels.

[0031] By repeating this process on the set of voxels within each lane in the predicted spatiotemporal domain, a complete local sub-map is obtained, forming the basic components of the environmental topology map.

[0032] Then, the adjacent subgraphs are connected. The independent local subgraphs are associated by establishing cross-lane connecting edges between adjacent (lane) local subgraphs, and finally a global environment topology graph is formed. The cross-lane connecting edge is represented by a bidirectional polyline always located inside the spatiotemporal voxel, connecting an effective node pair located in adjacent subgraphs; the start point and the end point of the cross-lane connecting edge are located at the center positions of the two nodes respectively.

[0033] The process for determining whether two nodes constitute an effective node pair is as follows:

[0034] (I) Candidate voxel pair screening: first, for any two nodes from adjacent subgraphs , a candidate spatiotemporal voxel pair that is aligned on the S-axis (i.e., has the same index) is screened out; based on the designed node generation strategy, the spatiotemporal voxels in each node are adjacent to each other on the S-axis. Therefore, the alignment of the voxels in is determined by comparing the S-axis index ranges of the voxels in .

[0035] (II) Time dimension overlap verification: then, for each candidate voxel pair, the length of the common time interval is calculated. If the length exceeds the preset minimum time overlap threshold , the corresponding voxel pair is defined as an "effective connection unit".

[0036] (III) Spatial dimension overlap verification: finally, the algorithm searches for "effective connection segments" composed of consecutive "effective connection units" along the S-axis, and counts the number of units in each segment. Only when the number of units in the longest "effective connection segment" reaches or exceeds the minimum threshold , it is determined that there is an effective spatiotemporal overlap between , and a connection is established. For the effective node pair (assuming ), the following process is designed to generate a polyline cross-lane connecting edge:

[0037] First, an initial connecting edge is formed by defining a polyline composed of five key points: (1) ; (2) ; (3) ; (4) ; and (5) . The initial connecting edge is formed.

[0038] Subsequently, the algorithm refines the polyline to ensure that the connecting edge is finally located inside the spatiotemporal voxel.

[0039] ​Further, a strategy of preferential cross-lane connection is designed. This strategy confines the valid node pairs to pairs of "longitudinal driving routes". For each pair of longitudinal routes, the algorithm establishes a unique cross-lane connection through the first valid node pair searched in its interior according to the preset priority rules. The strategy of preferential cross-lane connection is as follows:

[0040] (I) Extraction of longitudinal driving routes: for any adjacent local subgraph (subscript represents the horizontal index of the subgraph ), the algorithm first extracts all paths from the entry node in its interior through depth-first search, and these paths are defined as "longitudinal driving routes". Each route represents a specific longitudinal driving behavior of the intelligent vehicle starting from the spatiotemporal feasible region represented by the specific entry node in the lane.

[0041] (II) Definition of node priority: the algorithm assigns priority to the node according to its importance in the topology: the merging node has the highest priority. As the convergence point of multiple routes, it establishes a connection between the two sets of longitudinal driving routes in the subgraphs. The priority of the entry node is next. The priority of the splitting node is the lowest. In addition, if there are multiple nodes of the same type in a route, the node with a smaller S-axis index, i.e., the node with a more forward spatial position, has a higher priority.

[0042] (III) Implementation process of preferential connection: the algorithm traverses each pair of longitudinal driving routes in , and performs the following process for the route pair: (a) respectively obtain the node sets in , and sort the priorities according to the above rules; (b) select nodes from the sorted node sets in turn to form candidate pairs, and apply the above effective node pair judgment process to them; (c) if it is determined to be effective, a cross-lane connection edge is generated, and the subsequent check of is immediately terminated, and then the next pair of longitudinal routes is processed.

[0043] Through this process, the algorithm ensures that the feasible transition between each pair of longitudinal driving routes is represented by only one representative high-priority connection. Repeat this process for all adjacent subgraphs to construct a compact set of cross-lane connections.

[0044] Finally, the nodes in all subgraphs, the intra-lane connection edges, and the preferentially generated cross-lane connection edges together constitute a compact global environment topology graph that can fully represent diversified driving behaviors.

[0045] ​​Step 1.3, the step of driving behavior generation based on the environment topology graph, is as follows: taking the current position of the ego vehicle as the starting point, searching for all the driving routes in the constructed environment topology graph , and generating driving channels consistent with the topology for each route . Each "route-channel" combination represents a specific driving behavior , and all combinations together constitute the final output candidate driving behavior set , providing clear and reliable guidance and constraints for subsequent trajectory planning.

[0046] Further, the steps are as follows:

[0047] First, driving routes are generated. Driving routes are generated based on graph search algorithms, and the primary task is to define the starting and ending nodes of the search. For the starting node , the current global spatial position of the intelligent vehicle is first mapped to the Frenet space position through the coordinate transformation function , and the initial time of the prediction time domain is combined to determine the unique spatiotemporal voxel where the vehicle is initially located . The node corresponding to this voxel in the topology graph is set as the starting node of the search .

[0048] For the ending node, a set is defined, which is composed of the ending nodes of all the aforementioned longitudinal driving routes. Each longitudinal driving route represents a specific longitudinal driving behavior, and its ending node marks the farthest region that the intelligent vehicle can safely reach within the prediction time domain under that behavior.

[0049] After determining the starting node and the set of ending nodes , a depth-first search is performed for each starting and ending node pair , where , to traverse the topology graph. Considering that there may be multiple search paths between a pair of starting and ending nodes, a path search threshold is set to balance path coverage and computational overhead. When the number of paths searched for a pair of starting and ending nodes reaches this threshold, the search will be terminated early.

[0050] For each searched node-to-node path , the following processing is required to finally generate the driving route : (1) Starting point connection: generating a geometric polyline connecting the initial spatiotemporal position of the intelligent vehicle and the center point of the starting node . . This process adopts the same strategy as the generation of the in-lane connectivity edges, ensuring that . always lies within the spatiotemporal voxel; (2) end extension: to enable the route to guide to the farthest end of the target region, the voxel with the largest longitudinal index in the is first found, and then, using the same method as the start connection process, a geometric polyline connecting the center point and the center point is generated. (3) path stitching: concatenating the in sequence forms a complete driving route in the Frenet coordinate system from . . .

[0051] Then, the driving lane is generated. After the driving route is generated , the driving lane is generated directly by the set of spatiotemporal voxels owned by all the node paths that constitute the driving route .

[0052] The trajectory planning based on candidate driving behaviors described in step 2 mainly includes two steps, namely, initial trajectory search based on candidate driving strategy behaviors and trajectory spatiotemporal joint optimization based on spatiotemporal safety corridors.

[0053] Step 2.1, initial trajectory search based on candidate driving strategy behaviors, uses the candidate driving behaviors provided by the driving behavior generation step based on the environment topology graph to efficiently search for an initial Frenet trajectory as the basis for subsequent trajectory optimization.

[0054] To ensure the effectiveness of the initial trajectory, it must simultaneously satisfy the three core conditions of kinematic feasibility, no collision, and topological consistency with the candidate behaviors.

[0055] The algorithm inputs include: the current Frenet coordinate position and speed of the vehicle, the candidate driving route , and the candidate driving lane . The algorithm output is an initial trajectory that satisfies various constraints.

[0056] First, two core data structures are created: an open set for storing search nodes to be evaluated; and a closed set ​This is used to record nodes that have already been evaluated to avoid duplicate searches. Then, based on the vehicle's initial state... Generate the starting search node Add it .

[0057] Next, the algorithm enters the main loop. In each iteration, from... Choose the one with the lowest total cost nodes and from Move to .examine Does the preset search termination condition meet? If it does, then call... Backtrack to the starting node to generate the optimal trajectory and terminate the search. If the goal has not yet been achieved, then... Expand the nodes.

[0058] The algorithm traverses the predefined control input set. Each control command in and through The function is for each Expand a successor child node For each generated child node Perform redundancy checks and expansion effectiveness checks.

[0059] For those that pass the validity check Perform node cost assessment and calculate the actual cumulative cost from the starting node to the current node. And combined with heuristic functions Calculate the total cost of this node. .

[0060] Finally, the opening episode has been updated. ,if There is no such thing as in Nodes in the same discrete state unit will be directly... join in ;if There is already a node in it. and If the values ​​are the same voxel, then compare their actual cumulative costs. ,like Then use Information Update .

[0061] The above process is repeated until an optimal path that meets the termination condition is found, or when... When the value is empty, the search termination condition is met, and the algorithm terminates.

[0062] The node expansion, for a global index of the node , uses to represent the node position, to represent the node speed, to represent the time consumed from the starting point to the current node, to represent the heading angle of the node in the global coordinate system ; let represent the node state, represent the node control input, and the child node state with a global index of j is obtained through the state transition equation.

[0063] The expansion validity check requires that each candidate child node expanded by the node pass the dynamic feasibility, topological consistency, and driving safety checks before being added to the priority queue. First, the trajectory segment is uniformly sampled times at time intervals , and then the following checks are performed at each sampling point : (1) Dynamic feasibility check: assess whether the S-axis and L-axis accelerations of the vehicle on the trajectory segment are always within the preset range

[0064] .

[0065] (2) Topological consistency check: ensure that the generated trajectory segment strictly follows the driving lane defined by the driving behavior .

[0066] (3) Driving safety check: ensure that the vehicle does not physically collide with any static or dynamic obstacles in the road environment during driving.

[0067] The node cost evaluation, the total cost of the node is composed of two parts: the actual cumulative cost from the starting node to the current node , and the heuristic cost from the current node to the search target. The searched trajectory should achieve comprehensive optimization in terms of driving smoothness and duration. The trajectory smoothness is quantified by the quadratic integral of the control input, and the driving time is directly accumulated.

[0068] To accelerate the search process and guide it to converge towards the target, further calculate the heuristic cost for each node to be evaluated ​​For trajectory planning in structured road environments, the objective is typically to follow the centerline of the target lane at a reference speed. Driving. This step designs the optimal control trajectory based on the Pontryagin minimum principle. The generation and cost calculation method of the trajectory realizes the weighted sum and minimization of control energy and travel time from the current node state to the target.

[0069] The search termination condition, as described above, ensures that the search process can end within a finite time and return a valid trajectory. The algorithm reuses the heuristic trajectory obtained when calculating the heuristic cost. When the optimal node From the beginning After popping up, follow the heuristic trajectory corresponding to that node. Sample and perform an extended validity check, where .

[0070] Maximum search time Within the range, if the validity check of any sampling point fails, the subsequent regular search process continues. If all sampling points pass the check, the position and heading of the last sampling point on the trajectory that passed the check are further evaluated. If the current sampling point's lateral position... and heading angle ,in If the preset lateral position and heading angle termination thresholds are set, the current position is considered sufficiently close to the target reference line and the movement trend is parallel to it, thus terminating the search. Subsequently, starting from the current optimal node... Start backtracking the optimal node sequence And generate the optimal trajectory .

[0071] Optimal search trajectory It consists of two parts: one part is starting from the initial node and proceeding along the optimal node sequence. Connect to the current node The piecewise quadratic polynomial locus;

[0072] The second part is the aforementioned heuristic trajectory. In The part within the range, at this time Optimal search trajectory This will be used to construct subsequent trajectory optimization problems, and to further improve trajectory quality through more detailed optimization.

[0073] Step 2.2: Spatiotemporal joint optimization of trajectory based on spatiotemporal safety corridor. The steps include constructing a spatiotemporal safety corridor based on the initial trajectory, high-order continuous sparse parameterization of the initial trajectory, and constructing the optimization problem.

[0074] The construction of the spatiotemporal safety corridor based on the initial trajectory is based on the initial trajectory. Segmented sampling, assuming the total trajectory duration is First, the trajectory is uniformly divided into M segments along the time axis. This segmentation structure corresponds to the subsequent trajectory parameterization scheme, providing a framework for organizing optimization variables; the duration of each trajectory segment is... To accurately capture the vehicle's attitude changes within each trajectory segment and ensure the geometric accuracy of the corridor, K uniform samplings are performed within each trajectory segment; therefore, the sampling time step is... MK discrete sampling times, including the endpoint, were generated. .

[0075] At each sampling time First, calculate the spatiotemporal convex hull occupied by the vehicle at that moment. Subsequently, the spatiotemporal occupancy of the vehicle at each sampling point was analyzed. As a seed, the convex polyhedron iterative dilation algorithm is invoked to iteratively expand the hyperplane of the polyhedron outwards, always including the seed area. Simultaneously, the safe spatiotemporal region enclosed by the polyhedron is maximized. Finally, storing the convex polyhedra generated at all sampling moments sequentially along the time axis constitutes a complete polyhedron consistent with the initial trajectory. A topologically consistent spacetime safety corridor.

[0076] The high-order continuous sparse parameterization of the initial trajectory adopts the minimum control trajectory class. For the initial trajectory The trajectory to be optimized is obtained by performing sparse parameterization with high-order continuity.

[0077] For a trajectory consisting of N segments of 2s⁻¹ polynomials Given an initial state Inter-segment connection point sequence Uniform segment duration vector and trajectory terminal state constraints , Through linear complexity mapping Establish the conversion relationship between parameters and polynomial coefficients: ,in These are the coefficients of the i-th polynomial. It is the connection point between the i-th and (i+1)-th polynomial segments. Let be the duration of the i-th polynomial. Based on the mapping. Established trajectory It is continuously differentiable on the 2s-2 order between adjacent segments. Segment Polynomial Trajectory It is represented as: where . Therefore, the trajectory to be optimized is represented as .

[0078] Based on the above parametric representation, the original trajectory optimization problem can be converted into a standard nonlinear programming problem with respect to the sparse variable . With the mapping provided by , the gradients of the objective function and the constraint functions with respect to c and T can be efficiently propagated to by the chain rule, thus realizing the simultaneous optimization of trajectory shape and time, i.e., the joint optimization in space and time:

[0079]

[0080] The optimization problem is constructed, including the objective function and the constraint conditions . The objective function includes: control smoothness cost , trajectory time cost , expected driving target offset cost . The constraint conditions used include time vector constraint , whole-body anti-collision constraint and dynamic feasibility constraint . The trajectory optimization problem is summarized above the objective function and the constraint condition, and the Sequential Quadratic Programming (SQP) method is used to solve the trajectory optimization problem.

[0081] The optimal trajectory screening of step 3 includes a step of screening the trajectory with the lowest cost in the candidate driving trajectory set . Each trajectory in is generated by step 2 based on the trajectory planning of the candidate driving behavior, and corresponds to the candidate driving behavior with the same subscript in the candidate driving behavior set output by step 1 based on the environment topology graph, which represents a candidate "behavior-trajectory" combination.

[0082]

[0083]

[0084] where is the index of the candidate behavior and trajectory in the corresponding set , and represents a candidate "behavior-trajectory" combination, indicates that all candidate trajectories are in a cost value is calculated within a time span, denotes a ride comfort cost, denotes a passing efficiency cost. Both costs are related to the specific design details of step 2 trajectory planning based on candidate driving behaviors.

[0085] In order to fairly select the lowest-cost trajectory from the candidate set of driving trajectories, the shortest duration of the candidate trajectory is selected as the selected trajectory is the one with the lowest cost. The control smoothness cost of the objective function in step 2 trajectory planning based on candidate driving behaviors is used to calculate The expected ride target deviation cost in step 2 trajectory planning based on candidate driving behaviors is used to calculate .

[0086] In summary, the final trajectory selection process is formalized as a simple minimization problem:

[0087]

[0088] where is the index of the lowest-cost trajectory (behavior), is defined as the optimal "behavior-trajectory" combination output by the decision planning module.

[0089] Advantages of the present application:

[0090] (1) The present application realizes sufficient exploration of the spatiotemporal coupling vehicle feasible region, and can generate diversified candidate driving behaviors with high coverage and low redundancy, laying the foundation for subsequent selection of the optimal "behavior-trajectory" combination. In addition, the structured candidate driving behavior representation method containing driving routes and driving channels realizes effective guidance and constraint of trajectory planning.

[0091] (2) The trajectory planning algorithm designed in the present application ensures that the initial trajectory obtained in the search stage is consistent with the driving strategy topology, and then realizes joint optimization of trajectory space form and time process in the trajectory optimization stage, provides full-size collision avoidance safety for the vehicle through the establishment of a spatiotemporal safety corridor, and finally realizes high-quality collaborative optimization of the trajectory in safety, efficiency and comfort. BRIEF DESCRIPTION OF DRAWINGS

[0092] Figure 1 is a schematic diagram of the overall architecture of the present application;

[0093] Figure 2 is the pseudo code of the initial trajectory search algorithm based on candidate driving behavior strategies of the present application. DETAILED DESCRIPTION

[0094] As Figure 1 shown, the application proposes a multi-driving behavior parallel vehicle decision planning method based on environmental topology, including steps 1, candidate driving behavior generation based on environmental topology map, step 2, trajectory planning based on candidate driving behavior, and step 3, optimal trajectory screening.

[0095] Step 1 of the candidate driving behavior generation based on environmental topology map includes space-time voxel generation based on safe reachable time interval, hierarchical environmental topology map construction based on space-time voxel, and driving behavior generation based on environmental topology map.

[0096] The step of generating space-time voxels based on safe reachable time interval receives road environment information provided by the upstream perception and prediction link, models the space-time occupation within the ego vehicle prediction range, and updates the time interval of the initialized space-time voxel set using the modeling results, thereby realizing sparse discrete representation of the space-time feasible region within the prediction range.

[0097] Further, the steps are as follows:

[0098] First, introduce the time dimension (T axis) to construct a three-dimensional space-time coordinate system, and model the space-time occupation of road environment elements in this coordinate system. For key environmental elements that affect intelligent vehicle motion, road boundaries and obstacles, a unified space occupation model is constructed.

[0099] The space-time occupation of the road boundary line considers the available road boundary line information as a set of discrete point sets containing global coordinate positions : (1)

[0100] where is the global coordinate of the jth scattered point constituting the road boundary line. The is described as a series of connected line segments and sets, forming a road boundary line space occupation composed of line segments:

[0101] (2)

[0102] where represents the spatial position of the line segment end point, represents the convex hull construction for the input point set.

[0103] For obstacle vehicles, define the perceived rectangular boundary box length and width as , and the predicted global continuous driving trajectory as . For any time in the prediction time domain,The spatial occupancy of the vehicle can be represented by a convex polygon constructed from a set of points as:

[0104] (3)

[0105] wherein represents the spatial occupancy points of the obstacle vehicle at time . This embodiment mainly considers a general obstacle vehicle which can be regarded as a rectangle, thus we set , as the global coordinates of the vertices of the obstacle vehicle's rectangular bounding box:

[0106] (4)

[0107] wherein is the coordinates of the vertices of the bounding box in the obstacle vehicle's local coordinate system, is the rotation matrix based on the heading of the obstacle vehicle. This embodiment assumes that the origin of the obstacle vehicle's local coordinate system is consistent with the point used to describe its predicted trajectory.

[0108] When extending the above two-dimensional spatial occupancy model to the three-dimensional space-time domain, since the position of the road boundary does not change over time, the space-time occupancy of the road boundary can be considered as the expansion of its spatial occupancy vertically along the time axis; the line segment in two-dimensional space will form a boundary surface perpendicular to the XOY plane in the three-dimensional space-time domain; the space-time occupancy of the boundary line is represented as the union of these boundary surfaces; thus, within the time interval , the space-time occupancy of the road boundary line described by equations (1) and (2) is formalized as:

[0109] (5)

[0110] (6)

[0111] wherein represents the boundary surface formed by the jth line segment of the road boundary line in the space-time domain, which is composed of the 4 space-time endpoints of the line segment, and the symbol represents the union.

[0112] For the space-time occupancy of the obstacle vehicle, the entire predicted trajectory is uniformly sampled times, obtaining the spatial occupancy of the obstacle vehicle at several time points; ignoring the difference in the heading of the obstacle vehicle between two adjacent sampling time points , the space-time occupancy of the obstacle vehicle within the time period is:

[0113] (7)

[0114] wherein , denotes the time length of the predicted trajectory. The spatiotemporal occupancy of the obstacle vehicle denoted by is a cuboid whose top and bottom faces are rectangles with vertices at . Therefore, the global spatiotemporal occupancy of the obstacle vehicle is denoted as over the entire prediction horizon.

[0115] Then, a spatiotemporal Frenet coordinate system is constructed and the initialization of the spatiotemporal voxel set is completed in this coordinate system. The Frenet coordinate system describes the spatial position by using the longitudinal distance s along the reference line and the lateral offset perpendicular to the reference line. In this embodiment, the Frenet coordinate system is constructed with the right lane line of the lane where the intelligent vehicle is currently located as the reference line; the global position discrete point set of the right lane line is denoted as using the definition method consistent with formula (1); in order to improve the continuity of the reference line, the cubic polynomial spline is used to interpolate , and the second-order continuous reference line expression is obtained as follows:

[0116] (8)

[0117] where is the cubic polynomial basis, are the coefficients of the cubic polynomial curve between the discrete points , and is the curve length between two points.

[0118] Based on , the function is further defined, which outputs the corresponding global coordinate position given the coordinate position in the Frenet coordinate system:

[0119] (9)

[0120] where is the projection point of the global coordinate point on the reference line, is the slope at the projection point. In addition, using the above continuous reference line expression (8), given the global coordinate position , the uniquely determined can also be accurately found, and this process is defined as .

[0121] ​The time dimension (T-axis) is further introduced to the above spatial coordinate system to extend it into a spacetime Frenet coordinate system, and the initialization of voxel set and the subsequent time interval update are performed in this coordinate system. As mentioned above, the spatial distribution of spacetime voxels is uniform, and for a given axis and axis range, with a predefined S-axis and L-axis resolution and The continuous Frenet space is discretized into a uniform rectangular grid. For a grid with index , the actual positions of its four vertices are respectively:

[0122] (10)

[0123] wherein are the indices on the S-axis and L-axis respectively, is the number of grid discretization along the S-axis, is the maximum and minimum longitudinal range for voxel generation, is the number of grid discretization along the L-axis, are the maximum and minimum lateral range for voxel generation, represents the upward rounding of the input value.

[0124] To standardize the driving behavior, the lane changing is limited to the adjacent lanes, and the continuous crossing of lanes is prohibited. Therefore, the generation range of voxels is limited within the current lane and the adjacent lanes on the left and right. Let the lane width be , when the vehicle is located in the middle lane, ; if it is located in the rightmost or leftmost lane, the boundary changes to respectively. To align the voxels with the lanes, the embodiment sets .

[0125] Initialize a spacetime voxel on each uniform grid created, and these spacetime voxels have the same time interval , wherein is the duration of the predicted trajectory of the surrounding obstacle vehicle to make full use of the prediction information.

[0126] Finally, update the time interval of the spacetime voxel. First, based on the reachable constraint determined by the longitudinal driving capability of the vehicle, the time interval of all voxels is updated for the first time. Given the target longitudinal position , the shortest and longest time required for the car to reach this position based on the current longitudinal position and speed are calculated by the following formulas:

[0127] (11)

[0128] (12)

[0129] The maximum acceleration and maximum deceleration that a car can achieve are respectively Its maximum and minimum speeds are respectively , shortest time This describes the process of a vehicle starting from its initial position, continuously moving at maximum acceleration until it reaches its maximum speed and finally arrives at the designated target location; and the longest time... This corresponds to the situation where the vehicle maintains maximum deceleration throughout the entire motion, until it reaches its minimum speed and finally arrives at the target location. By dynamically adjusting longitudinal acceleration, intelligent vehicles can achieve this within a time frame. Arrive at the target location at any time within the specified time. .

[0130] Based on the above analysis, for all S-axis positions equal to Given the current motion state (position and speed) of an intelligent vehicle, the spatiotemporal free voxels can be accurately calculated to determine the corresponding longitudinal reachable time interval, thereby updating the voxel time interval.

[0131] Further road environment safety constraints are introduced to update the voxel time interval a second time. This update uses the spatiotemporal occupancy model of obstacle vehicles to decompose those voxels that have collisions in the time span into a new set of sub-voxels that inherit the original spatial index according to their safe time sub-intervals, and finally obtains a set of spatiotemporal voxels with safe reachability attributes.

[0132] Obstacle vehicle spatiotemporal occupancy model Mapped to the spacetime Frenet coordinate system Define the spatiotemporal occupancy model by aligning the bounding boxes to the axes in the spatiotemporal Frenet coordinate system. The smallest corner point in the lower left corner is The largest corner point in the upper right corner is By calculating the indices of the two corner points on the S-axis and L-axis, the potential impact can be determined. Affected spacetime voxels. Iterate through all potentially affected spacetime voxels and calculate... Time period occupied Time interval with existing spacetime voxels intersection If the result is If the result is [missing information], it means that the current spacetime voxel is completely occupied by the obstacle vehicle and needs to be deleted; if the result is [missing information], it means that the voxel is completely occupied by the obstacle vehicle and needs to be deleted. or If the result is zero, it means that the current spacetime voxel is partially occupied by the obstacle vehicle, and the time interval of the current voxel needs to be updated using the calculation result; if the result is zero... , it indicates that the current spatiotemporal voxel is occupied by the obstacle vehicle and needs to be divided into two parts, and the time interval of the current voxel needs to be updated as , and a new voxel is created at the same spatial position, and its time interval is set as .

[0133] By performing the above operation on all obstacle vehicle spatiotemporal occupancy mapped into the spatiotemporal Frenet coordinate system, the second update of the spatiotemporal voxel is finally completed.

[0134] The hierarchical environment topology graph based on the spatiotemporal voxel is to further abstract the discrete spatiotemporal voxel set into a structured environment topology graph to realize the advanced understanding of the spatiotemporal feasible region. This process first constructs the local subgraph of each lane independently in units of lanes; then, by connecting adjacent subgraphs, it integrates them into a topology graph covering the global environment. In this graph, the nodes are clustered by adjacent spatiotemporal voxels in the same lane, and the edges are represented by geometric polylines always located inside the spatiotemporal voxels.

[0135] Further, the steps are as follows:

[0136] First, local subgraph construction is performed. The spatiotemporal voxels in the same lane have the same lateral index ; the subsequent content uses to represent the spatiotemporal voxel in the current lane . The local subgraph of this lane is composed of nodes formed by clustering these spatiotemporal voxels and intra-lane connected edges connecting adjacent nodes. Each node needs to store the following information: unique number , lateral index of intra-lane spatiotemporal voxel , node type , spatiotemporal voxel set contained by the node , and node center position . Three types of nodes are defined: entry node, split node, and merge node.

[0137] The entry node: if the time interval of the current spatiotemporal voxel is disjoint (i.e., the intersection is empty) with the time interval of its spatially adjacent voxel , a new entry node is initialized based on voxel .

[0138] The split node: if the current spatial position simultaneously exists spatiotemporal voxels , and their time intervals all have non-empty intersection with the time interval of the same spatially adjacent voxel , then an equal number of split nodes are created based on these spatiotemporal voxels, respectively.

[0139] The merging node: if the time interval of the spatio-temporal voxel has a non-empty intersection with the time interval of each of the spatially adjacent spatio-temporal voxels , a merging node is created based on the current spatio-temporal voxel .

[0140] The above method sequentially traverses all spatio-temporal voxels in the lane along the S-axis direction. If the current voxel does not meet the initialization conditions of the above three types of nodes, it is merged into the node in which the previous spatially adjacent voxel is located by default.

[0141] After the nodes are generated, further connected edges representing the longitudinal direction are constructed between these nodes. A connected edge is a one-way polyline always located inside the spatio-temporal voxels, connecting the centers of two mutually connected nodes and . Assuming that the S coordinate of the center position is larger, the connected edge is directed from to . The basis for constructing the in-lane connected edge between two nodes is that the spatio-temporal voxels contained by them are spatially adjacent and have a non-empty intersection in the time interval. Based on the generation method of the above three types of nodes, it is known that there is no connected edge between the entry nodes; and the split nodes / merging nodes naturally have a connected relationship with their previous / successive nodes.

[0142] To form the above in-lane connected edge, an iterative updating strategy is adopted. First, the center positions of two nodes and are directly connected as the initial connected edge (hereinafter referred to as ), and it is sampled along the S-axis to obtain a series of sampling points . The X-axis coordinate , S-axis coordinate , and T-axis coordinate of these sampling points are calculated as follows:

[0143]

[0144] (13)

[0145] wherein represents a set of positive integers, and is equal to 0.5 when , and is equal to 1 when .

[0146] It is ensured that the sampling points are located on the interface of the spatially adjacent voxels. Next, it is sequentially checked whether all the sampling points are located in the time interval shared by the adjacent voxels. Assuming that​ Located in voxels On the boundary surface, if If the current connected edge is deemed unsafe, subsequent checks will be stopped and the connection will be terminated. The T-axis coordinate is reset to the center of the shared time interval between adjacent voxels; that is:

[0147] (14)

[0148] Then, the sampling point is inserted into the existing connected edge. At this point, the connected edge consists of two line segments, namely... Repeat the above sampling and detection process for these line segments until all line segments within the connected edges are located in the spatiotemporal voxels.

[0149] The method described above is used to construct local subgraphs within a specific lane. By repeating this process on the set of voxels within each lane in the prediction spatiotemporal domain, a complete set of local subgraphs is obtained, forming the basic components of the environmental topology map.

[0150] Next, adjacent subgraphs are connected. By establishing cross-lane connecting edges between adjacent (lane) local subgraphs, mutually independent local subgraphs are linked together, ultimately forming a global environment topology graph. Cross-lane connecting edges are represented by bidirectional polylines that are always located inside the spatiotemporal voxels, connecting valid pairs of nodes located in adjacent subgraphs; the start and end points of the cross-lane connecting edges are located at the centers of these two nodes, respectively.

[0151] The process for determining whether two nodes constitute a valid node pair is as follows:

[0152] (I) Candidate voxel pair selection: First, for any two nodes from adjacent subgraphs... and Filter out those aligned on the S-axis ( Candidate spatiotemporal voxel pairs with the same index; based on the designed node generation strategy, the spatiotemporal voxels within each node are adjacent to each other on the S-axis. Therefore, by comparing... and medium-volume Axis Index Range and To determine its alignment.

[0153] (II) Time Dimension Overlap Validation: Subsequently, for each candidate voxel pair, the common time interval length is calculated. If this length exceeds the preset minimum time overlap threshold... The corresponding voxel pair is defined as an "effective connection unit".

[0154] (III) Spatial dimension overlap check: Finally, the algorithm searches for "valid connection segments" composed of consecutively arranged "valid connection units" along the S-axis, and counts the number of units in each segment. Only when the number of units in the longest "valid connection segment" reaches or exceeds the minimum threshold , it is determined that there is a valid spatio-temporal overlap between the two nodes, and thus a connection is established. The threshold is calculated from the preset minimum spatial overlap distance :

[0155] (15)

[0156] wherein is the spatial discretization step size along the S-axis. Otherwise, if the length of all "valid connection segments" does not reach the threshold, it is determined that no connection can be established between the two nodes.

[0157] For a valid node pair (assuming ), the following process is designed to generate a polyline cross-lane connecting edge:

[0158] First, an initial connecting edge is defined by a polyline composed of five key points. They are in order: (1) ; (2) , which is taken from the median valid connection unit in the longest valid connection segment between the two nodes, and is the voxel center belonging to in the unit. (3) : lane crossing point, which is located in the above-mentioned median valid connection unit, and is the center of the contact surface between the two voxels in the L-axis. (4) : transfer point located in node , corresponding to , is the voxel center belonging to in the median valid connection unit. (5) . The initial connecting edge is formed.

[0159] Subsequently, the algorithm refines the polyline to ensure that the connecting edge is ultimately located inside the spatio-temporal voxel. The middle two segments are already ensured to be inside the spatio-temporal voxel because their endpoints are precisely defined by voxels inside or on the boundary. The first and last segments connect the node center and a certain voxel center inside the node, and their straight line paths may cross non-spatio-temporal voxel regions. Therefore, the same iterative updating strategy as the intra-lane connecting edge is adopted.

[0160] Redundancy can exist in the valid node pairs. To this end, a strategy of preferential cross-lane connection is further designed. This strategy limits the valid node pairs to pairs of "longitudinal driving routes". For each pair of longitudinal routes, the algorithm establishes a unique cross-lane connection through the first valid node pair searched in the order of priority according to the preset priority rules. The strategy of preferential cross-lane connection is as follows:

[0161] (I) Extraction of longitudinal driving routes: for any adjacent local subgraph (subscript represents the horizontal index of the subgraph ), the algorithm first extracts all paths starting from the entry node in each internal part through depth-first search. These paths are defined as "longitudinal driving routes". Each route represents a specific longitudinal driving behavior of the intelligent vehicle starting from the spatiotemporal feasible region represented by a specific entry node in the lane. Therefore, connecting two longitudinal driving routes belonging to different lanes essentially represents the transition of the intelligent vehicle from one longitudinal driving behavior to another.

[0162] (II) Definition of node priority: the algorithm assigns priority to nodes according to their importance in the topology: the merging node has the highest priority. As the convergence point of multiple routes, it establishes connections between the set of longitudinal driving routes in the two subgraphs. The priority of the entry node is next. The priority of the splitting node is the lowest. In addition, if there are multiple nodes of the same type in a route, the node with a smaller S-axis index, i.e., the node with a more forward spatial position, has a higher priority.

[0163] (III) Implementation process of preferential connection: the algorithm traverses each pair of longitudinal driving routes in and executes the following process for the route pair: (a) obtain the node sets in and respectively, and sort them according to the above rules; (b) select nodes from the sorted node sets in turn to form candidate pairs, and apply the aforementioned valid node pair judgment process to them; (c) if it is determined to be valid, generate a cross-lane connection edge, and immediately terminate the subsequent check of , and then process the next pair of longitudinal routes.

[0164] Through this process, the algorithm ensures that only one representative high-priority connection represents the feasible transition between each pair of longitudinal driving routes. Repeat this process for all adjacent subgraphs to construct a compact set of cross-lane connections.

[0165] ​​Finally, all nodes in the subgraph, lane-connected edges, and optimally generated cross-lane connected edges together constitute a compact global environmental topology graph that can comprehensively represent diverse driving behaviors.

[0166] The steps for generating driving behavior based on the environmental topology map are as follows: starting from the vehicle's current position, search for... Topologically Different Driving Routes And generate driving lanes with topological consistency for each route. Each "route-channel" combination represents a specific driving behavior. All combinations together constitute the final output set of candidate driving behaviors. This provides clear and reliable guidance and constraints for subsequent trajectory planning.

[0167] Further, the steps are as follows:

[0168] First, a driving route is generated. The driving route is generated based on a graph search algorithm, the primary task of which is to define the start and end nodes of the search. For the start node... First, determine the current global spatial location of the intelligent vehicle. Through coordinate transformation function Mapped to Frenet spatial location Combined with the initial time in the prediction time domain Determine the initial spatiotemporal position of the vehicle. The unique spatiotemporal voxel in which it resides, and the node corresponding to this voxel in the topology graph, is set as the starting node for the search. .

[0169] For the terminating node, define a set. This set consists of the termination nodes of all the aforementioned longitudinal driving routes. Each longitudinal driving route represents a specific longitudinal driving behavior, and its termination node marks the farthest area that the intelligent vehicle can safely reach within the predicted time domain under that behavior.

[0170] Determine the starting node With the set of terminating nodes Then, for each start and end node pair ,in A depth-first search is performed to traverse the topology graph. Considering that there may be multiple search paths between a pair of start and end nodes, a path search threshold is set to balance path coverage and computational cost. When the number of paths found for a pair of start and end nodes reaches this threshold, the search will terminate prematurely.

[0171] For each searched inter-node path The following processing is required to finally generate the driving route. (1) Starting point connection: Generate the initial spatiotemporal position of the connected intelligent vehicle With the starting node Geometric broken line between center points This process employs the same strategy as generating connected edges within the lane, ensuring... (1) Always located within the spacetime voxel; (2) End point extension: In order to guide the route to the farthest point of the target area, first find the termination node. The voxel with the largest vertical index Then, using the same method as the connection process at the starting point, a connection is generated. center point and Geometric polyline at the center point (3) Path splicing: , and Sequentially linked together, forming a line from Departure, complete driving route in the Frenet coordinate system of spacetime .

[0172] Then, generate the driving lanes and driving routes. Afterwards, the driving passage The generation method is very straightforward; it consists of the driving route. Node path It is composed of the set of spatiotemporal voxels possessed by all nodes in the system.

[0173] Step 2, the trajectory planning based on candidate driving behavior, mainly includes two steps: initial trajectory search based on candidate driving strategy behavior and spatiotemporal joint optimization of trajectory based on spatiotemporal safety corridor.

[0174] The initial trajectory search based on candidate driving strategy behavior utilizes the candidate driving behaviors output in the driving behavior generation step based on the environmental topology graph. The provided prior information allows for the efficient search of an initial Frenet trajectory. This serves as the basis for subsequent trajectory optimization.

[0175] To ensure the validity of the initial trajectory, it must simultaneously satisfy kinematic feasibility, collision-free operation, and compatibility with candidate behaviors. The three core conditions are topological consistency. The algorithm in this step makes targeted improvements and extensions to the classic Hybrid A* framework, enabling incremental spatiotemporal search in the Frenet coordinate system. The pseudocode is as follows: Figure 2 As shown.

[0176] The algorithm input includes: the vehicle's current Frenet coordinates. With speed Candidate driving routes and candidate driving lane The algorithm outputs an initial trajectory that satisfies all constraints. .

[0177] First, create two core data structures: open sets. Used to store search nodes to be evaluated; closed set This is used to record nodes that have already been evaluated to avoid duplicate searches. Then, based on the vehicle's initial state... Generate the starting search node Add it .

[0178] Next, the algorithm enters the main loop. In each iteration, from... Choose the one with the lowest total cost nodes and from Move to .examine Does the preset search termination condition meet? If it does, then call... Backtrack to the starting node to generate the optimal trajectory and terminate the search. If the goal has not yet been achieved, then... Expand the nodes.

[0179] The algorithm traverses the predefined control input set. Each control command in and through The function is for each Expand a successor child node For each generated child node Perform redundancy checks and expansion validity checks. First, based on The corresponding state is determined by comparing its location with the discrete state space unit to determine whether it already exists. In the middle. Secondly, through Function test The validity check ensures that the trajectory segment corresponding to the node is not only kinematically feasible and collision-free with obstacles, but also that its spatiotemporal position is strictly within the current driving lane. Any invalid child nodes will be discarded.

[0180] For those that pass the validity check Perform node cost assessment and calculate the actual cumulative cost from the starting node to the current node. And combined with heuristic functions Calculate the total cost of this node. .

[0181] Finally, the opening episode has been updated. ,if There is no such thing as in Nodes in the same discrete state unit will be directly... join in ;if There is already a node in it. and If the values ​​are the same voxel, then compare their actual cumulative costs. ,like Then use Information Update .

[0182] The above process is repeated until an optimal path that meets the termination condition is found, or when... When the value is empty (indicating no feasible path was found under the current search constraints), the search termination condition is met, and the algorithm terminates. The node expansion mentioned above applies to global indexes of... nodes ,use Indicates the node position. Indicates node velocity. This represents the time elapsed from the starting point to the current node. Indicates the node in the global coordinate system The heading angle in the middle; let Indicates the node status. If the node controls the input, then the global index is... The state of the child nodes is obtained through the following state transition equation:

[0183] (16)

[0184] in To control the duration, It is a 2D identity matrix. It is a 2D zero matrix. The control inputs used to expand the nodes are from... The samples were obtained by uniform sampling, among which They are respectively The minimum and maximum values ​​that can be taken. They are respectively The minimum and maximum values ​​that can be taken.

[0185] The aforementioned extended validity check is performed by the node. Each candidate child node that is expanded The corresponding extended trajectory segment before being added to the priority queue It needs to pass dynamic feasibility, topology consistency, and driving safety checks. Specifically, firstly, in the trajectory segment... The above time interval Proceeding with non-uniform sampling, the following checks are performed at each sampling point :

[0186] (1) Dynamic feasibility check: This check aims to assess whether the S-axis and L-axis accelerations of the vehicle on the trajectory segment are always within the pre-set range .

[0187] (2) Topological consistency check: This check ensures that the generated trajectory segment strictly follows the driving lane defined by the driving behavior . The algorithm first computes the voxel space index corresponding to the vehicle position , then queries the spatio-temporal voxel corresponding to this index in the driving lane, and checks whether it is contained within the time interval of the corresponding voxel. If the sampling time is out of the time interval of this voxel, it is determined that the current trajectory segment is not consistent with the topology of the driving behavior, and the expansion is invalid.

[0188] (3) Travel safety check: This check aims to ensure that the vehicle will not collide with any static or dynamic obstacles in the road environment during travel. This check takes into account the geometric shapes of both the vehicle and the obstacles, enabling full-size collision avoidance for the initial trajectory. Two types of obstacles are mainly considered: road boundary lines modeled as a series of polylines and obstacle vehicles modeled as rectangles.

[0189] At each sampling point , the spatial occupancy of all obstacles is uniformly represented as the union of a series of convex hulls : :

[0190] (17)

[0191] where denotes the position of spatial occupancy point j of obstacle i at time , and is the number of elements in the point set, for lane boundary lines , and for obstacle vehicles .

[0192] To efficiently perform collision detection, the algorithm first converts the points in to the ego vehicle's local coordinate system to obtain :

[0193] (18)

[0194] where is used to calculate is the ego vehicle at the sampling point The heading angle at that point. The spatial occupancy of the vehicle in its local coordinate system is represented as:

[0195] (19)

[0196] It is important to note that the composition point It only depends on the prior shape of the vehicle and is independent of its motion state, therefore it is omitted. Based on the vehicle space occupancy model and the transformed obstacle space occupancy model The scaling factor is obtained by solving a linear programming problem based on a scaling point set. And through comparison Determine the magnitude relationship between the sampling points and 1. Does the vehicle collide with obstacles in the environment?

[0197] (20)

[0198] The above questions are intended to Each of them Looking for someone who can and by Composition Hyperplane separated by scaling point sets and maximize Furthermore, the second constraint on the above problem is that both sides are multiplied. ,make We can obtain the following linear programming problem:

[0199] (twenty one)

[0200] If the solution is obtained This indicates that at the sampling point If the vehicle does not collide with any obstacles, then a collision has occurred, and the current extension is invalid.

[0201] A candidate trajectory segment is considered a valid extension only if it passes all three tests mentioned above at all its sampling points. It can then be added to the priority queue for subsequent searches.

[0202] The aforementioned node cost assessment, total node cost It consists of two parts: from the starting node to the current node. Actual cumulative cost and from the current node Heuristic cost to search target The searched trajectory should achieve a comprehensive optimization in terms of smoothness and duration. The trajectory smoothness is quantified by the quadratic form of control inputs, while the travel time is directly accumulated. Suppose the optimal node index sequence from the initial node to the current node is , then the actual accumulated cost from to is:

[0203] (22)

[0204] where is a positive definite weight matrix to adjust the contribution of different control components to the smoothness cost, and are the trajectory smoothness weights of the corresponding axis, is the time weight factor.

[0205] To accelerate the search process and guide it to converge to the target, further calculate the heuristic cost for each to-be-evaluated node . For trajectory planning in structured road environments, the goal is usually to travel along the centerline of the target lane at a reference speed . The algorithm described in this step is based on the Pontryagin Minimum Principle to design an optimal control trajectory generation and its cost calculation method, which realizes the weighted sum minimization of control energy and travel time from the current node state to the target .

[0206] First, introduce the optimal control trajectory generation method of S-axis and L-axis respectively. For L-axis, consider the following form of control system:

[0207] (23)

[0208] where J is the control input cost under a known duration T, is the state of the control system, is the control system input, is the L-axis trajectory in the Frenet coordinate system. The optimal control input trajectory is expected to be obtained by minimizing J. Using the Pontryagin Minimum Principle, introduce the conjugate variable and define the Hamiltonian function H:

[0209] (24)

[0210] Based on H, the conjugate equation is calculated as:

[0211] (25)

[0212] The above conjugate equation is easy to solve to get Thus the optimal control input trajectory is solved as follows:

[0213] (26)

[0214] Further, the trajectory of is shown as follows:

[0215] (27)

[0216] where are the L-axis position and velocity of the current node. Continue to solve , define the trajectory terminal constraint as , where is the L-axis coordinate of the last route point in . This constraint requires the intelligent vehicle to travel along the centerline of the lane where the last point in is located. At this time, we have:

[0217] (28)

[0218] where are the L-axis velocity and position of the current node , respectively. Through simple matrix operations, we can solve .

[0219] Similarly, the S-axis optimal trajectory can be expressed as:

[0220] (29)

[0221] However, the key difference from the L-axis is that the S-axis trajectory only constrains , is free. Under this constraint, we require . At this time, we have:

[0222] (30)

[0223] where is the S-axis velocity of the current node . Through simple matrix operations, we can solve . Call the heuristic trajectory of the current node .

[0224] Substitute the above optimal control input trajectories of the S-axis and L-axis into to get:

[0225] (31)

[0226] Because All are known, and can be solved. Get smallest root ,Will Heuristic cost as the current node and the actual cumulative cost Add them together to get the total cost of the current node. .

[0227] The search termination condition, as described above, ensures that the search process can end within a finite time and return a valid trajectory. The algorithm reuses the heuristic trajectory obtained when calculating the heuristic cost. When the optimal node From the beginning After popping up, follow the heuristic trajectory corresponding to that node. Sample and perform an extended validity check, where .

[0228] At maximum search time Within the range, if the validity check of any sampling point fails, the subsequent regular search process continues. If all sampling points pass the check, the position and heading of the last sampling point on the trajectory that passed the check are further evaluated. If the current sampling point's lateral position... and heading angle ,in If the preset lateral position and heading angle termination thresholds are set, the current position is considered sufficiently close to the target reference line and the movement trend is parallel to it, thus terminating the search. Subsequently, starting from the current optimal node... Start backtracking the optimal node sequence And generate the optimal trajectory .

[0229] Optimal search trajectory It consists of two parts: one part is starting from the initial node and proceeding along the optimal node sequence. Connect to the current node Piecewise quadratic polynomial trajectory, adjacent nodes in the sequence and The trajectory between them can be represented as:

[0230] (32)

[0231] The second part is the aforementioned heuristic trajectory. In The part within the range, at this time Optimal search trajectory This will be used to construct subsequent trajectory optimization problems, and to further improve trajectory quality through more detailed optimization.

[0232] The spatiotemporal joint optimization of the trajectory based on the spatiotemporal safety corridor includes the following steps: constructing the spatiotemporal safety corridor based on the initial trajectory, parameterizing the initial trajectory with high-order continuous sparse parameters, and constructing the optimization problem.

[0233] The construction of the spatiotemporal safety corridor based on the initial trajectory is based on the initial trajectory. Segmented sampling, assuming the total trajectory duration is First, the trajectory is uniformly divided into M segments along the time axis. This segmentation structure corresponds to the subsequent trajectory parameterization scheme, providing a framework for organizing optimization variables; the duration of each trajectory segment is... To accurately capture the vehicle's attitude changes within each trajectory segment and ensure the geometric accuracy of the corridor, K uniform sampling operations are performed within each trajectory segment. Therefore, the sampling time step is... MK discrete sampling times, including the endpoint, were generated. ,in .

[0234] At each sampling time First, calculate the spatiotemporal convex hull occupied by the vehicle at that moment. Specifically, the geometry of a vehicle is determined by a set of vertices in its local coordinate system. (The formula for the vehicle's space occupancy model) is expressed. It is located in the global spatiotemporal coordinate system ( Spatiotemporal occupancy under ) Represented as:

[0235] (33)

[0236] The spatial position of each vertex in the global coordinate system Calculated using the following formula:

[0237] (34)

[0238] in Indicates the initial trajectory at At all times, at the Frenet coordinate position, The function maps this position to the global coordinate system. For the car in The heading angle at any given moment. It is a two-dimensional rotation matrix from the vehicle's local coordinate system to the global XY coordinate system, defined as:

[0239] (35)

[0240] Subsequently, the vehicle's spatiotemporal occupancy at each sampling point was used as the basis for the measurement. As a seed, the convex polytope iterative inflation algorithm is called to inflate the hyperplane of the polytope outward iteratively, while always containing the seed occupancy and maximizing the safe spatiotemporal region enclosed by the polytope. Finally, all the convex polytopes generated at each sampling time are stored sequentially along the time axis, thus constituting a complete spatiotemporal safe corridor that is topologically consistent with the initial trajectory .

[0241] The spatiotemporal safe corridor can adapt to the demand of joint spatiotemporal optimization of trajectories, and can further constrain the complete geometric shape of the vehicle by means of three-dimensional convex polytopes, thus directly ensuring the absolute safety of the driving trajectory. In addition, these three-dimensional convex polytopes also reserve sufficient continuous operation space for joint adjustment of the spatiotemporal profile of the trajectory.

[0242] The high-order continuous sparse parameterization of the initial trajectory adopts the minimum control amount trajectory class . The initial trajectory is sparsely parameterized with high-order continuity to obtain the trajectory to be optimized.

[0243] For a trajectory composed of N segments of s-order polynomials, given the initial state , the sequence of connection points between segments , the uniform segment duration vector , and the trajectory terminal state constraint , , a linear complexity mapping is established to establish the conversion relationship between parameters and polynomial coefficients:

[0244] (36)

[0245] where is the coefficient of the i-th polynomial, is the connection point between the i-th and i+1-th polynomials, is the duration of the i-th polynomial. Based on the mapping , the trajectory is continuously differentiable between adjacent segments with 2s-2 order. This embodiment sets s=3, which means that the position, velocity and acceleration of the trajectory endpoint are constrained and are 4-order continuously differentiable, which is higher than the initial trajectory and meets the demand of trajectory planning. Furthermore, the i-th polynomial trajectory is expressed as:

[0246] (37)

[0247] where . Therefore, the trajectory to be optimized is further expressed as .

[0248] Based on the above parametric representation, the original trajectory optimization problem can be converted into a standard nonlinear programming problem with respect to the sparse variable . With the mapping provided by , the gradients of the objective function and the constraint functions with respect to c and T can be efficiently propagated to q, T and by the chain rule, thus achieving the simultaneous optimization of trajectory shape and time, i.e., the joint optimization in space and time:

[0249] (38)

[0250] The optimization problem is constructed, and the details of the objective function and the constraint conditions are introduced in detail. The objective function includes: control smoothness cost , trajectory time cost , expected driving target offset cost .

[0251] (1) Control smoothness cost :

[0252] The control smoothness is represented using the integral of the control variable; based on , when , the control variable is . Therefore is formulated as:

[0253] (39)

[0254] where is the smoothness cost weight matrix.

[0255] (2) Trajectory time cost :

[0256] This cost represents the trajectory duration regularization:

[0257] (40)

[0258] where is the time weight.

[0259] (3) Expected driving target offset cost :

[0260] The expected driving target of trajectory planning is defined as that the ego vehicle can follow the target lane centerline at the reference speed . Therefore Mainly includes two parts: L-axis position of trajectory and target lane center line position , that is the L-axis coordinate of the last route point in the deviation square integral, requires the vehicle to approach the target lane center line; S-axis speed of trajectory and reference vehicle speed between the square of the integral, the vehicle speed close to the reference value. The specific formula is as follows:

[0261] (41)

[0262] where are the weights of the corresponding cost terms, respectively.

[0263] The constraints used include time vector constraints , whole-body collision avoidance constraints and dynamic feasibility constraints .

[0264] (1) Time vector constraints :

[0265] Synchronous adjustment of the space-time profile of the trajectory is achieved by optimizing the time vector T. To ensure that T has physical meaning and limit the trajectory within the predicted time domain, the following constraints are designed:

[0266] (42)

[0267] where is a vector with all elements being 1, is the minimum allowed trajectory duration.

[0268] (2) Whole-body collision avoidance constraints :

[0269] It is necessary to always constrain the ego vehicle's space occupancy within the space-time free corridor to avoid collision with obstacles. Similar to calculating the risk cost , along the optimized trajectory , a space-time safety polyhedron is constructed at each sampling point. For the sampling time on the trajectory segment , the space occupancy of the ego vehicle in the global XY coordinate system is . Then is formulated as:

[0270] (43)

[0271] where is the hyperplane set expression constituting the space-time safety polyhedron at the current sampling point.

[0272] (3) Dynamic feasibility constraints :

[0273] The dynamic feasibility constraints in the optimization phase are the same as the dynamic feasibility check in the search phase, requiring the vehicle's to always be within the preset range. Like the dynamic feasibility check, the dynamic feasibility constraints are also constructed at the sampling points along the optimized trajectory . For the sampling time on the trajectory segment , the ego vehicle's at the sampling time is calculated, and the values of them are then constrained:

[0274] (44)

[0275] where the calculation method of , , , is similar. The above objective function and constraint conditions are summarized as follows:

[0276] (45)

[0277] For the trajectory optimization problem constructed above, the Sequential Quadratic Programming (SQP) method is used for solving. The SQP algorithm is widely used due to its robustness and high efficiency in dealing with nonlinear constraint optimization problems.

[0278] The optimal trajectory screening in step 3 includes a step of screening the trajectory with the lowest cost from the candidate driving trajectory set . Each trajectory in is generated by step 2 based on the trajectory planning of the candidate driving behavior, and corresponds to the candidate driving behavior with the same subscript in the candidate driving behavior set output by step 1 based on the environment topology graph, indicating a candidate "behavior-trajectory" combination.

[0279] Let the cost function for screening the optimal trajectory be:

[0280]

[0281] where is the index of the candidate behavior and trajectory in the corresponding set , and represents a candidate "behavior-trajectory" combination, ​​denotes that all candidate trajectories are computed within a time horizon, denotes the ride comfort cost, denotes the traffic efficiency cost. Both costs are related to specific design details of the trajectory planning based on candidate driving behavior in Step 2.

[0282] To fairly select the cost-minimal trajectory from the candidate trajectory set , is selected as the shortest duration among the candidate trajectories. The control smoothness cost of the objective function in Step 2, trajectory planning based on candidate driving behavior, is used to compute the expected ride target deviation cost in Step 2, trajectory planning based on candidate driving behavior, .

[0283] In summary, the final trajectory selection process is formalized as a simple minimization problem:

[0284] (47)

[0285] where is the index of the cost-minimal trajectory (behavior), is defined as the optimal “behavior-trajectory” combination as the final output of the decision planning module.

Claims

1. An environmental topology-based multi-driving behavior parallel vehicle decision planning method, characterized in that: The steps include: Step 1, candidate driving behavior generation based on environmental topology map, including: Step 1.1, space-time voxel generation based on safe reachable time interval: receive the road environment information provided by the upstream perception and prediction link, introduce the time dimension to construct a three-dimensional space-time coordinate system, and model the space-time occupation of the road environment elements within the self-vehicle prediction range in the coordinate system, and construct a unified space occupation model for the road boundary and obstacle vehicles; construct a space-time Frenet coordinate system , and initialize the space-time voxel set in this coordinate system; using the modeling results, update the time interval of the initialized space-time voxel set, thereby realizing the sparse discrete representation of the space-time feasible region within the prediction range; Step 1.2, hierarchical environmental topology map construction based on spatiotemporal voxel: first, independently construct local subgraphs for each lane in units of lanes; then integrate them into a topology map covering the global environment by connecting adjacent subgraphs, and abstract the discrete spatiotemporal voxel set into a structured environmental topology map; in this graph, nodes are clustered from adjacent spatiotemporal voxels in the same lane, and edges are represented by geometric polylines that always lie inside the spatiotemporal voxels; Step 1.3, driving behavior generation based on the constructed environment topology map: starting from the current position of the ego vehicle, search out all the driving routes in the constructed environment topology map topologically different driving routes , and generate driving lanes consistent with the topology for each route ; each "route-lane" combination represents a specific driving behavior , and all combinations together constitute the final output candidate driving behavior set ; Step 2, trajectory planning based on candidate driving behavior, including: Step 2.1, Initial Trajectory Search Based on Candidate Driving Strategy Behavior: Utilize the candidate driving behavior output from the previous step to search for an initial Frenet trajectory based on the provided prior information as the basis for subsequent trajectory optimization; Step 2.2, spatiotemporal joint optimization of trajectory based on spatiotemporal safety corridor, including: Construction of spatiotemporal safety corridor based on initial trajectory: for initial trajectory Segmented sampling, K uniform sampling in each trajectory segment, at each sampling time Calculate the spatiotemporal occupancy convex hull of the ego vehicle at this time , the spatiotemporal occupancy of the ego vehicle at each sampling point As a seed, the hyperplane of the polyhedron is inflated outward by iteration, the safety spatiotemporal region wrapped by the polyhedron is maximized, the convex polyhedron generated at all sampling times is stored in sequence along the time axis, and the safety spatiotemporal corridor topologically consistent with the initial trajectory is constructed ​ High order continuous sparse parameterization of initial trajectories: using minimum control amount trajectory class performing sparse parameterization with high order continuity on the initial trajectories to obtain the trajectory to be optimized; Optimization problem construction: constructing an objective function and constraint conditions , the objective function includes: control smoothness cost , trajectory time cost , expected driving target offset cost , the constraint conditions include time vector constraints , whole body anti-collision constraints and dynamic feasibility constraints , the trajectory optimization problem is obtained by summarizing the objective function and the constraint conditions; Step 3, optimal trajectory screening: In the candidate driving trajectory set The lowest cost trajectory is selected from the middle. Each trajectory is generated by trajectory planning based on candidate driving behaviors in step 2, and combined with the candidate driving behavior set generated in step 1 based on the environment topology map. The corresponding driving behaviors under the same subscript. This represents a candidate "behavior-trajectory" combination, and the final output is the optimal "behavior-trajectory" combination.

2. The method of claim 1, wherein: Step 1.1, the steps of spatiotemporal voxel generation based on safe reachable time interval are as follows: First, introduce a three-dimensional spatiotemporal coordinate system and perform spatiotemporal occupancy modeling of road environment elements in this coordinate system; Consider available road boundary lines The information is a set containing A set of discrete points with global coordinates ;Will Described as a union of a series of line segments connected end to end, forming a set of segments. The space occupied by the road boundary line composed of line segments ,in Indicates the spatial location of the endpoints of a line segment. This represents constructing a convex hull for the input point set; for the obstacle vehicle, the length and width of the perceived rectangular bounding box are defined as follows: and The predicted global continuous driving trajectory is For any time in the prediction time domain Its space occupation can be represented by a convex polygon constructed from a set of points as follows: ,in Indicates the time when the obstacle vehicle is Space occupancy points; setting The global coordinates of the vertices of the rectangular bounding box of the obstacle vehicle; When the above two-dimensional space occupancy model is extended to a three-dimensional space-time domain, the line segments in the two-dimensional space form boundary surfaces perpendicular to the XOY plane in the three-dimensional space-time domain; the space-time occupancy of the boundary lines is represented as the union of these boundary surfaces; within the time interval , the space-time occupancy of the road boundary lines is formulated as: , where represents the boundary surface formed by the jth line segment of the road boundary lines in the space-time domain, which is composed of the 4 space-time endpoints of the line segment; the symbol represents the union. To determine the spatiotemporal occupancy of the obstacle vehicle, the entire predicted trajectory is first uniformly sampled. This process yields the space occupancy of obstacle vehicles at several time points; the travel direction of obstacle vehicles between two adjacent sampling times is ignored. The difference between them, the obstacle vehicle in the time period Spacetime occupancy within for: wherein denotes the duration of the predicted trajectory; the global spatio-temporal occupancy of the obstacle vehicle is denoted as over the entire prediction horizon. Then, the space-time Frenet coordinate system is constructed , and the initialization of the space-time voxel set is completed in this coordinate system; the Frenet coordinate system uses the longitudinal distance s along the reference line and the lateral offset perpendicular to the reference line to describe the spatial position; the Frenet coordinate system is constructed with the right lane line of the current lane of the intelligent vehicle as the reference line; the global position discrete point set of the right lane line is expressed as by using the definition method consistent with the road boundary line; the expression of the second-order continuous reference line is obtained by using the cubic polynomial spline line to interpolate ; Based on Define function , whose function is to give the coordinate position in the Frenet coordinate system Output the corresponding global coordinate position , given the global coordinate position , accurately find the unique , define the process as ; Augmenting the above spatial coordinate system with a time dimension to a spacetime Frenet coordinate system, the spatial distribution of spacetime voxels is uniform, for a given S-axis and L-axis range, with a pre-defined S-axis and L-axis resolution Discretizing the continuous Frenet space into a uniform rectangular grid; The generation range of the voxel is limited in the current lane and the left and right adjacent lanes; assuming the lane width is When the vehicle is in the middle lane, If it is in the rightmost or leftmost lane, the boundary changes to or , assuming ; Initialize a spatio-temporal voxel on each of the created uniform grids, the spatio-temporal voxels having the same time interval wherein Predict the duration of the trajectory for the surrounding obstacle vehicle to make full use of the prediction information; Finally, the spatiotemporal voxel time intervals are updated; firstly, based on the reachability constraints determined by the vehicle's longitudinal travel capability, the time intervals of all voxels are updated for the first time; given the target's longitudinal position... The car is based on its current longitudinal position and speed The shortest and longest times required to reach this location, and the shortest time. This describes the process of a vehicle starting from its initial position, continuously moving at maximum acceleration until it reaches its maximum speed and finally arrives at the designated target location; the longest time. This corresponds to the situation where the vehicle maintains maximum deceleration throughout the entire movement until it reaches its minimum speed and finally arrives at the target location; by dynamically adjusting longitudinal acceleration, intelligent vehicles can achieve this within a time range. Arrive at the target location at any time within the specified time. ; Based on the above analysis, for all space-time free voxels with S-axis position equal to the current motion state of the intelligent vehicle can be given, and the corresponding longitudinal reachable time interval can be accurately calculated to update the time interval of the voxel. Introduce road environment safety constraints to update the voxel time interval for the second time. This update uses the spatiotemporal occupancy model of the obstacle vehicle to decompose voxels with collision in the time span into a new set of sub-voxels that inherit the original spatial index, and finally obtain a set of spatiotemporal voxels with safe accessibility properties; Obstacle vehicle spatiotemporal occupancy model Mapping to the spatiotemporal Frenet coordinate system Axis-aligned bounding box in the spatiotemporal Frenet coordinate system, defining the spatiotemporal occupancy model The minimum corner point in the lower left corner is , and the maximum corner point in the upper right corner is ; by calculating the indices of the two corner points on the S and L axes, determine the spatiotemporal voxels that may be affected ; traverse all possible affected spatiotemporal voxels, calculate the occupied time period the intersection of the time interval with the existing spatiotemporal voxels : if the result is , it means that the current spatiotemporal voxel is completely occupied by the obstacle vehicle and needs to be deleted; if the result is or , it means that the current spatiotemporal voxel is partially occupied by the obstacle vehicle, and the time interval of the current voxel needs to be updated using the calculation result; if the result is , it means that the current spatiotemporal voxel is divided into two parts by the obstacle vehicle spatiotemporal occupancy, and the time interval of the current voxel needs to be updated to , and a new voxel is created at the same spatial position with its time interval set to ; By performing the above operation on all obstacle vehicle spatiotemporal occupancies mapped into the spatiotemporal Frenet coordinate system, the second update of the spatiotemporal voxel is finally completed.

3. The method of claim 1, wherein: Step 1.2, hierarchical environmental topology map construction based on spatiotemporal voxel, including: First, local subgraphs are constructed; spatiotemporal voxels within the same lane have the same lateral index. Subsequent content will be used Indicates the current lane The spatiotemporal voxels; the local subgraph of the lane consists of nodes formed by clustering these spatiotemporal voxels and lane-connected edges linking adjacent nodes; each node The following information needs to be stored: unique identifier Lateral index of spatiotemporal voxels within the lane Node type The set of spatiotemporal voxels contained in a node and the location of the node center Three node types are defined: entry node, split node, and merge node. The entry node: if the time interval of the current spatio-temporal voxel does not intersect with the time interval of its immediately preceding spatially adjacent voxel , initialize a new entry node based on voxel ; The split node: if the current spatial position At the same time, there exist spatiotemporal voxels, and their time intervals all have non-empty intersection with the time interval of the same spatially adjacent voxel , then create an equal number of split nodes based on these spatiotemporal voxels, respectively; The merging node: if the time interval of the spatiotemporal voxel and the time interval of the spatiotemporal voxel adjacent in space both have a non-empty intersection, a merging node is created based on the current spatiotemporal voxel ; The above method sequentially traverses all spatiotemporal voxels in the lane along the S-axis direction. If the current voxel does not meet the initialization conditions of the above three types of nodes, it is defaulted to be merged into the node of the spatially adjacent voxel; After the generation of nodes, the connected edges representing the longitudinal direction are constructed between these nodes, which is a one-way polyline always inside the spatio-temporal voxels, connecting two mutually connected nodes and The basis for constructing the connected edges between two nodes is that the spatio-temporal voxels contained by them are spatially adjacent and have non-empty intersection in the time interval; based on the generation method of the above three types of nodes, it can be known that there is no connected edge between the entry nodes, and the split nodes / merge nodes naturally have the connected relationship with the previous nodes / successor nodes. To form the above-mentioned intra-lane connecting edge, an iterative updating strategy is adopted; first, the center positions of two nodes are directly connected as the initial connecting edge, and the initial connecting edge is sampled along the S axis to obtain a series of sampling points and ; the L-axis coordinates of the sampling points are ; the L-axis coordinates of the sampling points are , and it is ensured that the sampling points are located on the interface of adjacent voxels; next, it is sequentially checked whether all the sampling points are located in the time interval shared by adjacent voxels; assuming that is located on the boundary surface of voxel , if , it is considered that the current connecting edge is unsafe, the subsequent checking is stopped, the T-axis coordinate of is reset as the center of the time interval shared by adjacent voxels, and then the sampling point is inserted into the existing connecting edge; at this time, the connecting edge includes two straight line segments, and and , the above-mentioned sampling and detection process is repeated for these straight line segments until all the straight line segments in the connecting edge are located in the space-time voxel. By repeating this process for the voxel set in each lane in the predicted spatiotemporal domain, a complete set of local subgraphs is obtained, which constitutes the basic component of the environmental topology map; Then, connect adjacent subgraphs; by establishing cross-lane connecting edges between adjacent local subgraphs, the independent local subgraphs are associated to form a global environmental topology map; The cross-lane connecting edge is represented by a bidirectional polyline that always lies inside the spatiotemporal voxel, connecting the effective node pairs in adjacent subgraphs; the start and end points of the cross-lane connecting edge are located at the center positions of the two nodes.

4. The method of claim 3, wherein: The process for determining whether two nodes constitute an effective node pair is as follows: (I) Candidate voxel pair selection: First, for any two nodes from adjacent subgraphs... and Filter out in On-axis aligned candidate spatiotemporal voxel pairs; based on the designed node generation strategy, the spatiotemporal voxels within each node are adjacent to each other on the S-axis; therefore, by comparison and S-axis index range of medium voxels and To determine its alignment; (II) Time dimension overlap verification: subsequently, for each candidate voxel pair, the length of the common time interval is calculated; if the length exceeds the preset minimum time overlap threshold The corresponding voxel pair is defined as an "effective connection unit". (III) Spatial dimension overlap check: Finally, the algorithm searches for "valid connection segments" composed of consecutively arranged "valid connection units" along the S-axis, and counts the number of units in each segment; only when the number of units in the longest "valid connection segment" reaches or exceeds the minimum threshold , it is determined that there is a valid spatio-temporal overlap between and , thereby establishing a connection; for valid node pairs and , the following process is designed to generate a cross-lane connecting edge in polyline form: The initial connected edge is formed by first initializing a connected edge by defining a polyline consisting of five key points: (1) : the center of node ; (2) : a transfer point located within node ; (3) : a lane crossing point; (4) : a transfer point located within node ; (5) : the center of node ; . Subsequently, the algorithm refines the polyline to ensure that the connecting edge is finally located inside the spatiotemporal voxel; The cross-lane optimal connection strategy limits the effective node pairs to pairs of "longitudinal driving routes"; for each pair of longitudinal routes, the algorithm establishes a unique cross-lane connection through the first effective node pair found in the pair according to the preset priority rules; The cross-lane optimal connection strategy is as follows: (I) Longitudinal driving route extraction: For any adjacent local subgraph and , the algorithm first extracts all paths from the entry node within each subgraph by depth-first search, which are defined as "longitudinal driving routes"; each route represents a specific longitudinal driving behavior of the intelligent vehicle from the spatiotemporal feasible region represented by the specific entry node in the lane; (II) Definition of node priority: the algorithm assigns priority to nodes based on their importance in the topology structure: the merging node has the highest priority; as a convergence point for multiple routes, it efficiently associates the longitudinal driving route sets in the two subgraphs through its connection; the priority of the entry node is second. The priority of the split node is the lowest; in addition, if there are multiple nodes of the same type in a route, the Axis index The priority of the node with a smaller space position is higher. (III) Implementation process of optimal connection: The algorithm traverses through two nested loops. and Each pair of longitudinal driving routes And perform the following process on the route: (a) obtain respectively and (a) Select nodes from the sorted node set and prioritize them according to the above rules; (b) Select nodes from the sorted node set in sequence. and (c) If the determination is valid, generate a cross-lane connected edge and immediately terminate the pairing process; The subsequent inspection will then proceed to process the next pair of longitudinal routes; Repeat this process for all adjacent subgraphs to build a compact set of cross-lane connections; Finally, all nodes within subgraphs, intra-lane connected edges, and the preferred cross-lane connected edges together form a global environment topology that is compact and can fully represent diverse driving behaviors.

5. The method of claim 1, wherein: Step 1.3, driving behavior generation based on environment topology, the steps are as follows: First, a driving route is generated; the driving route is generated based on a graph search algorithm, the primary task of which is to define the start and end nodes of the search; for the start node... First, determine the current global spatial location of the intelligent vehicle. Through coordinate transformation function Mapped to Frenet spatial location Combined with the initial time in the prediction time domain Determine the initial spatiotemporal position of the vehicle. The unique spatiotemporal voxel in which it resides, and the node corresponding to this voxel in the topology graph, is set as the starting node for the search. ; For the termination nodes, define a set consisting of all termination nodes of the aforementioned longitudinal driving routes; each longitudinal driving route represents a specific longitudinal driving behavior, and its termination node marks the farthest region that the intelligent vehicle can safely reach within the prediction horizon under this behavior; determining a start node and a set of end nodes then, for each pair of start and end nodes wherein a depth-first search is performed to traverse the topology graph; considering that there can be multiple search paths between a pair of start and end nodes, a path search threshold is set to balance path coverage and computation overhead the search is terminated early when the number of paths searched for a pair of start and end nodes reaches the threshold For each searched inter-node path The following processing is required to finally generate the driving route. (1) Starting point connection: Generate the initial spatiotemporal position of the connected intelligent vehicle With the starting node Geometric broken line between center points ; This process employs a strategy consistent with that used to generate connected edges within the lane, ensuring... (1) Always located within the spacetime voxel; (2) End point extension: In order to guide the route to the farthest point of the target area, first find the termination node. The voxel with the largest vertical index Then, using the same method as the connection process at the starting point, a connection is generated. center point and Geometric polyline at the center point (3) Path splicing: , and Sequentially linked together, forming a line from Departure, complete driving route in the Frenet coordinate system of spacetime ; Then, a driving channel is generated; a driving route is generated After, the driving channel The generation method is very direct, which is composed of the spatio-temporal voxel set owned by all nodes in the node path of the driving route ​ 6. The method of claim 1, wherein: Step 2.1, initial trajectory search based on candidate driving strategy behaviors, using the candidate driving behaviors output by the driving behavior generation step based on the environment topology graph With the provided prior information, an initial Frenet trajectory is efficiently searched out as the basis for subsequent trajectory optimization; The algorithm inputs include: the current Frenet coordinate position of the vehicle , the speed , the candidate driving route , and the candidate driving lane ; the algorithm output is an initial trajectory that satisfies various constraints ; First, two core data structures are created: an open set for storing search nodes to be evaluated; and a closed set for recording nodes that have already been evaluated to avoid duplicate searches; subsequently, based on the initial state and of the vehicle, a starting search node is generated and added to ; Next, the algorithm enters the main loop; in each iteration, from Choose the one with the lowest total cost nodes and from Move to ;examine Check if the preset search termination condition is met; if so, call... Backtrack to the starting node to generate the optimal trajectory and terminate the search; if the goal has not yet been achieved, then... Expand the nodes; The algorithm traverses the predefined control input set. Each control command in and through The function is for each Expand a successor child node For each generated child node Perform redundancy checks and expansion effectiveness checks; For nodes that pass the validity check Perform a node cost evaluation, calculating the actual cumulative cost from the start node to this node And combine with the heuristic function Calculate the total cost of this node ; Finally, the open set is updated If there is no node in the open set that is in the same discrete state unit as , then directly add to the open set ; if there is already a node in the open set that is in the same voxel as , compare the actual accumulated cost of the two , if , then use the information of to update ; The above process is repeated until an optimal path is found that satisfies the termination condition, or when When the search termination condition is reached, the algorithm terminates.

7. The method of claim 6, wherein: The node expansion, for the global index of the node , uses to represent the node position, to represent the node speed, to represent the time consumed from the starting point to the current node, to represent the heading angle of the node in the global coordinate system ; let represent the node state, represent the node control input, then the child node state with the global index is obtained through the state transition equation; The expanded effectiveness check is performed by the node Each candidate sub-node of the expansion Before joining the priority queue, the corresponding expansion track segment Must pass through dynamic feasibility, topological consistency and driving safety checks; first on the track segment With time interval Perform Secondly uniform sampling, and then at each sampling point Perform the following checks: (1) Dynamic feasibility check: assess that the S-axis and L-axis accelerations of the vehicle on the trajectory segment are always within the pre-set range within; (2) Topology consistency check: ensure that the generated trajectory segments strictly follow the driving behavior The defined driving lane ; (3) Travel safety check: Ensure that the vehicle does not collide with any static or dynamic obstacles in the road environment during travel; The aforementioned node cost assessment, total node cost It consists of two parts: from the starting node to the current node. Actual cumulative cost and from the current node Heuristic cost to search target The searched trajectory should achieve a comprehensive optimization in terms of driving smoothness and duration; trajectory smoothness is quantized by the quadratic integral of the control input, while driving time is directly accumulated; To accelerate the search process and direct it towards convergence to the goal, for each node to be evaluated a heuristic cost is computed ; for trajectory planning in structured road environments, the goal is usually to follow the centerline of the target lane at a reference vehicle speed ; this step designs an optimal control trajectory based on the Pontryagin's minimum principle whose cost is computed by a method that minimizes the weighted sum of control effort and travel time from the current node state to the goal; The search termination condition is to ensure that the search process can end in a limited time and return an effective trajectory, and the algorithm reuses the heuristic cost obtained by the above calculation When the optimal node After the open set Pops up, along the heuristic trajectory corresponding to the node Sampling and performing expansion effectiveness checking, wherein ; within a maximum search time , if the validity check of any sampling point fails, the subsequent regular search process is continued; if all sampling points pass the check, the position and heading of the last sampling point on the trajectory that passed the check is evaluated; if the current sampling point is and the heading angle , where are preset lateral position and heading angle termination thresholds, respectively, the current position is considered to be sufficiently close to the target reference line and the motion trend is considered to be parallel to it, and the search is terminated; subsequently, the optimal node sequence is backtracked starting from the current optimal node and the optimal trajectory is generated; Optimal search trajectory Comprises two parts: one part is from the initial node along the optimal node sequence Connected to the current node Segmented quadratic polynomial trajectory; The second part is the heuristic trajectory described above in the range of parts of the range, at which time the optimal search trajectory will be used to construct a subsequent trajectory optimization problem and improve the trajectory quality through more detailed optimization.

8. The method of claim 1, wherein: Step 2.2, spatiotemporal joint optimization of trajectory based on spatiotemporal safety corridor, the steps include spatiotemporal safety corridor construction based on initial trajectory, high-order continuous sparse parameterization of initial trajectory, and optimization problem construction; The initial trajectory-based spatiotemporal safety corridor construction is based on the segmentation sampling of the initial trajectory , the total time length of the trajectory is , the trajectory is first evenly divided into M trajectory segments along the time axis; this segmentation structure corresponds to the subsequent trajectory parameterization scheme, providing a framework for the organization of optimization variables; the time length of each trajectory segment is ; Precise capture of the vehicle's pose change in each trajectory segment and ensure the geometric accuracy of the corridor, K uniform sampling in each trajectory segment, so the sampling time step is , MK discrete sampling time points including the end point are generated , where ; At each sampling time , firstly calculate the spatiotemporal occupancy convex hull of the ego vehicle at the time ; Subsequently, the self-car space-time occupancy at each sampling point As a seed, the convex polyhedron iterative inflation algorithm is called to inflate the hyperplane of the polyhedron outward iteratively, while always containing the seed occupancy and maximizing the safe space-time region wrapped by the polyhedron; finally, the convex polyhedra generated at all sampling times are sequentially stored along the time axis, thus forming a complete space-time safe corridor consistent with the initial trajectory topology. a high-order continuous sparse parameterization of the initial trajectory, using a minimum control amount trajectory class performing a high-order continuous sparse parameterization on the initial trajectory to obtain an optimized trajectory For a trajectory composed of N segments of polynomials , given initial state , sequence of inter-segment connection points , uniform segment duration vector , and trajectory terminal state constraints , , A linear complexity mapping is established to convert parameters to polynomial coefficients: where is the coefficient of the th segment polynomial, is the connection point between the th and th segment polynomials, is the duration of the th segment polynomial. Based on mapping Established trajectory Between adjacent segments Step continuous differentiable; first Segment polynomial trajectory Is expressed as: Where ; Thus, the trajectory to be optimized is represented as ; Based on the above parameterized representation, the original trajectory optimization problem can be converted into a standard nonlinear programming problem with respect to the sparse variables ; using the mapping provided by , the gradients of the objective function and the constraint functions with respect to c and T can be efficiently propagated to and via the chain rule, thus achieving the simultaneous optimization of trajectory shape and time, i.e., the joint optimization of space and time: ; The optimization problem is constructed, including an objective function and constraint conditions ; The objective function includes: a control smoothness cost , a trajectory time cost , an expected travel target offset cost ; the constraints used include a time vector constraint , a whole-body anti-collision constraint , and a dynamic feasibility constraint ; the trajectory optimization problem is summarized according to the above objective function and constraints, and a sequential quadratic programming method is used to solve the trajectory optimization problem.

9. The method of claim 1, wherein: The optimal trajectory screening in step 3 includes: screening the trajectory with the lowest cost in the candidate trajectory set , Each trajectory is generated by step 2 based on the trajectory planning of the candidate driving behavior, and corresponds to the candidate driving behavior with the same subscript in the candidate driving behavior set output by step 1 based on the environment topology map , , which represents a candidate "behavior-trajectory" combination; Let the cost function for screening the optimal trajectory be: where are the indices of the candidate behaviors and trajectories in the corresponding sets and , denotes a candidate "behavior-trajectory" combination, denotes that the cost value is calculated within the time span of , denotes the ride comfort cost, denotes the traffic efficiency cost; Both of the two costs are related to the specific design details of the trajectory planning based on the candidate driving behaviors in step 2. To select the lowest cost trajectory from the set of candidate trajectories, selected as the shortest duration of the candidate trajectories; step 2 is based on the control smoothness cost of the objective function in the trajectory planning of the candidate driving behavior for calculating the expected driving target deviation cost in the trajectory planning of the candidate driving behavior for calculating ;​ In summary, the final trajectory screening process is formalized as a simple minimization problem: where is the index of the least-cost trajectory, is defined as the optimal "action-trajectory" combination that is the final output of the decision planning module.

Images