Intelligent vehicle trajectory planning method and system based on spatiotemporal sampling evaluation
By using a spatiotemporal sampling evaluation method, and optimizing trajectory planning with an initial spatiotemporal state grid and lookup table, the problem of optimal solution loss caused by path and speed decoupling is solved, and real-time and safe optimal trajectory planning for intelligent vehicles is achieved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- COWA TECHNOLOGY CO LTD
- Filing Date
- 2023-02-24
- Publication Date
- 2026-06-09
AI Technical Summary
Existing trajectory planning methods that decouple path and speed may lead to the loss of optimal solutions or no solutions in autonomous driving, and the computational cost of high-dimensional state spaces is large, making it difficult to run in real time.
A spatiotemporal sampling-based evaluation method is adopted. By initializing the spatiotemporal state grid and lookup table, a spatiotemporal state grid is generated. Combined with static and dynamic obstacle cost maps, trajectory planning is optimized. Offline initial estimation table and lookup table are used to accelerate the calculation and generate the optimal trajectory in real time.
It achieves optimal trajectory planning in time and space, prevents vehicle collisions, improves the safety and real-time planning capabilities of intelligent driving, and reduces computational complexity.
Smart Images
Figure CN116149336B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the technical field of intelligent vehicle trajectory planning, specifically to an intelligent vehicle trajectory planning method and system based on spatiotemporal sampling evaluation. Background Technology
[0002] Autonomous driving currently relies primarily on autonomous driving (AD) for individual vehicles. AD is mainly divided into several modules: localization, perception, prediction, decision planning, and control execution.
[0003] Currently, the trajectory planning solutions for autonomous driving mainly focus on decoupling path and speed, such as the EM planner planning solution open-sourced by Baidu Apollo.
[0004] The Em planner approach decouples path and velocity. It constructs a drivable area using static information (high-precision map and static obstacles), searches for discrete guide path points using dynamic programming, and then uses quadratic programming to transform the drivable area into constraints in the Frescher coordinate system. Path smoothing is achieved by minimizing path smoothness, obstacle avoidance magnitude, and lane cost. The generated smoothed path is used as a projection reference path for the predicted trajectory of dynamic obstacles. Similarly, discrete velocity sequence points are searched using dynamic programming, and quadratic programming constraints are constructed using their upper and lower bounds. Velocity smoothing is achieved by minimizing the difference from the target velocity, the smoothness of acceleration and jerk. Finally, a single-frame trajectory is generated by combining the smoothed path and smoothed velocity.
[0005] This trajectory planning method, which decouples path planning and velocity planning, only performs velocity planning on the optimal path. Decoupling path and velocity planning reduces the solution space, which may lead to the loss of the optimal solution or even no solution in some cases.
[0006] Finding the optimal spatiotemporal solution in the planning space is a challenge for trajectory planning. By using a Frenet frame based on reference lines, a spatiotemporal state grid is obtained by sampling along the horizontal and vertical axes. This directly generates candidate trajectories by combining time and space. Since the state grid has a high dimension and requires a large amount of computation, it is necessary to control the dispersion of each dimension to ensure that the algorithm can run in real time.
[0007] Therefore, a new technical solution is needed to improve the above-mentioned technical problems. Summary of the Invention
[0008] In view of the shortcomings of the prior art, the purpose of this invention is to provide an intelligent vehicle trajectory planning method and system based on spatiotemporal sampling evaluation.
[0009] According to the present invention, an intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation is provided, the method comprising the following steps:
[0010] Step S1: Initialize the planner;
[0011] Step S2: Update the road search space representation;
[0012] Step S3: Generate an optional trajectory starting from the vehicle's position and ending at a node in the grid;
[0013] Step S4: Generate an optional trajectory using two nodes in the grid as the starting and ending points;
[0014] Step S5: Select the optimal endpoint among the grid nodes;
[0015] Step S6: Backtrack to the vehicle's position from the optimal endpoint and select the optimal trajectory.
[0016] Preferably, in step S1, the dispersion of the spatiotemporal state grid in the horizontal and vertical directions, acceleration, velocity range, and time range is defined during initialization, and an initial guess table (IGT) is established for numerically solving the parameters of the third-order helical curve between two points. The initialization operation is only performed once during planner initialization.
[0017] Preferably, step S2 includes the following steps:
[0018] Step S2.1: Update mesh nodes and internal connection paths. Based on the predefined Connectivity Pattern between mesh nodes, establish a mesh representation along the reference line.
[0019] Step S2.2: Offline lookup table, LUT update. As the reference line changes, update a correspondence description that maps each grid value in the Cartesian coordinate system (X, Y) to the Fraser coordinate system (S, L) and store it in a lookup table (LUT).
[0020] Step S2.3: Update the static cost map and the dynamic cost map. By looking up the coordinate information of static obstacles and dynamic obstacles and their predicted trajectories through the LUT, update the cost described in the SL coordinate system on each grid cell.
[0021] Preferably, step S3 includes the following steps:
[0022] Step S3.1: Calculate the planning starting point, compensate for the vehicle's movement during the planning control delay, and estimate the vehicle's state s_future in the short future;
[0023] Step S3.2: Path generation, calculate the path connecting s_future and mesh nodes;
[0024] Step S3.3: Trajectory and cost calculation. Sampling path points on the path calculated in step S3.2 and calculating the cost_static with the static obstacle cost map; adding acceleration parameters (acc profile) to each path to obtain the trajectory; calculating the cost_dynamic of dynamic obstacles on the dynamic obstacle cost map based on the trajectory; at each node, selecting the lowest cost from all trajectories reaching that node and updating it as the cost of that node.
[0025] Preferably, step S4 includes the following steps:
[0026] Step S4.1: Path generation. For each vertical discrete value station, calculate the path from the station to nodes on other stations based on the predefined extension method Connectivity Pattern and the spiral curve parameters updated in step S3.
[0027] Step S4.2: Trajectory and cost calculation. The subsequent steps are the same as in step S3.3. The connection trajectory between a pair of nodes and the cost after considering dynamic obstacles are obtained. The above process is repeated until all grid nodes have been updated.
[0028] Step S5 includes the following steps:
[0029] Step S5.1: Select the optimal trajectory endpoint. After step S5, the cost of all nodes on the grid has been updated. Select the optimal node as the trajectory endpoint according to a certain endpoint selection strategy.
[0030] Step S5.2: Backtrack from the endpoint of the optimal trajectory to find the final planned optimal trajectory.
[0031] The present invention also provides an intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation, the system comprising the following modules:
[0032] Module M1: Initialize the planner;
[0033] Module M2: Updates the road search space representation;
[0034] Module M3: Generates an optional trajectory starting from the vehicle's position and ending at nodes in the grid;
[0035] Module M4: Generates an optional trajectory using two nodes in the grid as the starting and ending points;
[0036] Module M5: Select the optimal endpoint among the grid nodes;
[0037] Module M6: Backtracks to the vehicle's position from the optimal endpoint to select the optimal trajectory.
[0038] Preferably, during initialization, the spatiotemporal state grid is defined in terms of the horizontal and vertical axes, acceleration, velocity range, and time range, and an initial guess table (IGT) is established to numerically solve the parameters of the third-order spiral curve between two points. The initialization operation is only performed once during planner initialization.
[0039] Preferably, module M2 includes the following modules:
[0040] Module M2.1: Mesh node and internal connection path update, establishes mesh representation along reference lines based on the predefined Connectivity Pattern between mesh nodes;
[0041] Module M2.2: Offline lookup table, LUT update. As the reference line changes, it updates a description of the correspondence between each grid value in the Cartesian coordinate system (X, Y) and the Fraser coordinate system (S, L), and stores it in a lookup table (LUT).
[0042] Module M2.3: Updates the static cost map and dynamic cost map. By looking up the coordinate information of static and dynamic obstacles and their predicted trajectories through a LUT, the cost described in the SL coordinate system on each grid cell is updated.
[0043] Preferably, module M3 includes the following modules:
[0044] Module M3.1: Calculates the starting point of the planning, compensates for the movement of the vehicle during the planning and control delay, and estimates the vehicle's state s_future in the short future;
[0045] Module M3.2: Path generation, calculates the path connecting s_future and mesh nodes;
[0046] Module M3.3: Trajectory and cost calculation. Sampling path points on the path calculated in Module M3.2 and calculating the cost_static with the static obstacle cost map; adding acceleration parameters (acc profile) to each path to obtain the trajectory; calculating the cost_dynamic of dynamic obstacles on the dynamic obstacle cost map based on the trajectory; at each node, selecting the lowest cost from all trajectories reaching that node to update the cost of that node.
[0047] Preferably, module M4 includes the following modules:
[0048] Module M4.1: Path generation. For each vertical discrete value station, the path from the station to nodes on other stations is calculated based on the predefined extension method Connectivity Pattern and the updated spiral curve parameters in Module M3.
[0049] Module M4.2: Trajectory and cost calculation. Subsequent modules are the same as module M3.3. They obtain the connection trajectory between a pair of nodes and the cost after considering dynamic obstacles. The above process is repeated until all grid nodes have been updated.
[0050] Module M5 includes the following modules:
[0051] Module M5.1: Select the optimal trajectory endpoint. After module M5, the cost of all nodes on the grid has been updated. The optimal node is selected as the trajectory endpoint according to a certain endpoint selection strategy.
[0052] Module M5.2: Backtracks from the endpoint of the optimal trajectory to find the final planned optimal trajectory.
[0053] Compared with the prior art, the present invention has the following beneficial effects:
[0054] 1. The intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation provided by this invention can cover the optimal trajectory in time and space, thereby preventing collisions between the vehicle and other vehicles and improving the level of intelligent driving.
[0055] 2. The intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation provided by this invention makes extensive use of offline initial guess table (IGT) as the initial solution for path generation parameters, which accelerates the generation process and enables real-time planning while ensuring path coverage.
[0056] 3. The intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation provided by this invention utilizes lookup tables (LUTs) to accelerate coordinate system transformation efficiency on regular roads, enabling real-time planning and ensuring safety;
[0057] 4. The intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation provided by this invention establishes a static cost map and a dynamic cost map based on the Frescher coordinate system, providing an efficient cost representation form for evaluating the optimal trajectory. Attached Figure Description
[0058] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:
[0059] Figure 1 This is a basic flowchart of the trajectory planning based on spatiotemporal sampling evaluation of the present invention;
[0060] Figure 2 Update the state grid flowchart for this invention;
[0061] Figure 3 This is a flowchart illustrating the trajectory of a vehicle directly reaching a grid node, as planned in this invention.
[0062] Figure 4 This is a flowchart illustrating the trajectory within the grid nodes of the present invention.
[0063] Figure 5 The optimal trajectory flowchart is selected for this invention. Detailed Implementation
[0064] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.
[0065] Example 1:
[0066] According to the present invention, an intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation is provided, the method comprising the following steps:
[0067] Step S1: Initialize the planner; during initialization, define the dispersion of the spatiotemporal state grid in the horizontal and vertical directions, acceleration, velocity range, and time range, and establish the initial guess table (IGT) for numerically solving the parameters of the third-order spiral curve between two points. The initialization operation is only performed once during planner initialization.
[0068] Step S2: Update the road search space representation;
[0069] Step S2.1: Update mesh nodes and internal connection paths. Based on the predefined Connectivity Pattern between mesh nodes, establish a mesh representation along the reference line.
[0070] Step S2.2: Offline lookup table, LUT update. As the reference line changes, update a correspondence description that maps each grid value in the Cartesian coordinate system (X, Y) to the Fraser coordinate system (S, L) and store it in a lookup table (LUT).
[0071] Step S2.3: Update the static cost map and the dynamic cost map. By looking up the coordinate information of static obstacles and dynamic obstacles and their predicted trajectories through the LUT, update the cost described in the SL coordinate system on each grid cell.
[0072] Step S3: Generate an optional trajectory starting from the vehicle's position and ending at a node in the grid;
[0073] Step S3.1: Calculate the planning starting point, compensate for the vehicle's movement during the planning control delay, and estimate the vehicle's state s_future in the short future;
[0074] Step S3.2: Path generation, calculate the path connecting s_future and mesh nodes;
[0075] Step S3.3: Trajectory and cost calculation. Sampling path points on the path calculated in step S3.2 and calculating the cost_static with the static obstacle cost map; adding acceleration parameters (acc profile) to each path to obtain the trajectory; calculating the cost_dynamic of dynamic obstacles on the dynamic obstacle cost map based on the trajectory; at each node, selecting the lowest cost from all trajectories reaching that node and updating it as the cost of that node.
[0076] Step S4: Generate an optional trajectory using two nodes in the grid as the starting and ending points;
[0077] Step S4.1: Path generation. For each vertical discrete value station, calculate the path from the station to nodes on other stations based on the predefined extension method Connectivity Pattern and the spiral curve parameters updated in step S3.
[0078] Step S4.2: Trajectory and cost calculation. The subsequent steps are the same as in step S3.3. The connection trajectory between a pair of nodes and the cost after considering dynamic obstacles are obtained. The above process is repeated until all grid nodes have been updated.
[0079] Step S5: Select the optimal endpoint among the grid nodes;
[0080] Step S5.1: Select the optimal trajectory endpoint. After step S5, the cost of all nodes on the grid has been updated. Select the optimal node as the trajectory endpoint according to a certain endpoint selection strategy.
[0081] Step S5.2: Backtrack from the endpoint of the optimal trajectory to find the final planned optimal trajectory.
[0082] Step S6: Backtrack to the vehicle's position from the optimal endpoint and select the optimal trajectory.
[0083] The present invention also provides an intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation. The intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation can be implemented by executing the process steps of the intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation. That is, those skilled in the art can understand the intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation as a preferred embodiment of the intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation.
[0084] Example 2:
[0085] The present invention also provides an intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation, the system comprising the following modules:
[0086] Module M1: Initializes the planner; during initialization, it defines the dispersion of the spatiotemporal state grid in the horizontal and vertical directions, acceleration, velocity range, and time range, and establishes the initial guess table (IGT) for numerically solving the parameters of the third-order helical curve between two points. The initialization operation is only performed once during planner initialization.
[0087] Module M2: Updates the road search space representation;
[0088] Module M2.1: Mesh node and internal connection path update, establishes mesh representation along reference lines based on the predefined Connectivity Pattern between mesh nodes;
[0089] Module M2.2: Offline lookup table, LUT update. As the reference line changes, it updates a description of the correspondence between each grid value in the Cartesian coordinate system (X, Y) and the Fraser coordinate system (S, L), and stores it in a lookup table (LUT).
[0090] Module M2.3: Updates the static cost map and dynamic cost map. By looking up the coordinate information of static and dynamic obstacles and their predicted trajectories through a LUT, the cost described in the SL coordinate system on each grid cell is updated.
[0091] Module M3: Generates an optional trajectory starting from the vehicle's position and ending at nodes in the grid;
[0092] Module M3.1: Calculates the starting point of the planning, compensates for the movement of the vehicle during the planning and control delay, and estimates the vehicle's state s_future in the short future;
[0093] Module M3.2: Path generation, calculates the path connecting s_future and mesh nodes;
[0094] Module M3.3: Trajectory and cost calculation. Sampling path points on the path calculated in Module M3.2 and calculating the cost_static with the static obstacle cost map; adding acceleration parameters (acc profile) to each path to obtain the trajectory; calculating the cost_dynamic of dynamic obstacles on the dynamic obstacle cost map based on the trajectory; at each node, selecting the lowest cost from all trajectories reaching that node to update the cost of that node.
[0095] Module M4: Generates an optional trajectory using two nodes in the grid as the starting and ending points;
[0096] Module M4.1: Path generation. For each vertical discrete value station, the path from the station to nodes on other stations is calculated based on the predefined extension method Connectivity Pattern and the updated spiral curve parameters in Module M3.
[0097] Module M4.2: Trajectory and cost calculation. Subsequent modules are the same as module M3.3. They obtain the connection trajectory between a pair of nodes and the cost after considering dynamic obstacles. The above process is repeated until all grid nodes have been updated.
[0098] Module M5: Select the optimal endpoint among the grid nodes;
[0099] Module M5.1: Select the optimal trajectory endpoint. After module M5, the cost of all nodes on the grid has been updated. The optimal node is selected as the trajectory endpoint according to a certain endpoint selection strategy.
[0100] Module M5.2: Backtracks from the endpoint of the optimal trajectory to find the final planned optimal trajectory.
[0101] Module M6: Backtracks to the vehicle's position from the optimal endpoint to select the optimal trajectory.
[0102] Example 3:
[0103] The planning method proposed in this invention makes extensive use of look-up tables (LUTs) to transform some online computations into online table lookups, thereby reducing computational complexity; at the same time, it covers feasible planning spaces to include spatiotemporally optimal solutions, and finds the optimal trajectory through trajectory evaluation.
[0104] Step 0. Initialization process:
[0105] During initialization, the dispersion of the spatiotemporal state grid in the horizontal and vertical directions (each horizontal sample is denoted as 1 station), acceleration, velocity range, and time interval are defined. An initial guess table (IGT) is also established for numerically solving the parameters of the third-order helical curve between two points. The initialization operation is performed only once during planner initialization.
[0106] Step 1. Update the state grid: see Figure 2 As the vehicle's relative position to the road reference line changes during its motion, the shape description parameters (spiral curve parameters) of the nodes (vertex) and the paths connecting the nodes in the spatiotemporal mesh, which deform with the reference line, are updated:
[0107] (1) Mesh Node and Internal Connection Path Update: Based on the predefined connectivity pattern between mesh nodes, a mesh representation is built along the reference line. Due to the change in the shape of the reference line, nodes in states (stations) that the vehicle has already passed through are deleted, and new states are added after the state farthest from the vehicle. That is, mesh nodes are only added or deleted, but do not move with the vehicle's position. The spiral curve connections between unchanged nodes reuse previous parameters, and the connections associated with newly added nodes are iteratively updated until convergence using the corresponding parameters in the Initial Estimation Table (IGT) as initial values.
[0108] (2) Offline Look-Up Table (LUT) Update: As the reference line changes, an updated description of the correspondence between each grid value in the Cartesian coordinate system (X, Y) and the Fraser coordinate system (S, L) is stored in a look-up table (LUT). When evaluating the cost of grids in the cost map describing dynamic / static obstacles (e.g., the lateral and longitudinal distances of obstacles to the vehicle in the SL coordinate system), the cost can be obtained through lookup and interpolation, without having to find the corresponding points on the reference line and then calculate the SL distance.
[0109] (3) Update of static cost map and dynamic cost map: The cost described in the SL coordinate system of each grid cell is updated by looking up the coordinate information of static obstacles and dynamic obstacles and their predicted trajectories through LUT.
[0110] Step 2. Plan the trajectory from the vehicle directly to the grid nodes (lattice vertices): see Figure 3 On the updated mesh in step 1, a path is generated directly from the vehicle's position to the mesh node using a 5th-order spiral curve:
[0111] (1) Calculate the starting point of the planning: compensate for the movement of the vehicle during the control delay and estimate the state of the vehicle in the short future (s_future).
[0112] (2) Path generation: Calculate the path connecting s_future and grid nodes.
[0113] (3) Trajectory and cost calculation: Sample path points on the path calculated in (2) and calculate the cost (cost_static) with the static cost map; add acceleration parameters (accprofile) to each path to obtain the trajectory; calculate the cost of dynamic obstacles (cost_dynamic) on the dynamic cost map based on the trajectory; at each node, select the lowest cost from all trajectories that reach the node and update it as the cost of the node.
[0114] Step 3. Plan the trajectory within the grid nodes: See Figure 4 Step 2 updates the cost of nodes on the grid based on the trajectories from the compensated planning starting point (s_future) to each node. This step updates the cost of nodes within the grid by updating the trajectories between nodes:
[0115] (1) Path generation: For each longitudinal discrete value (station), the path from the station to other stations is calculated based on the predefined extension method (Connectivity Pattern) and the updated spiral curve parameters in step 2 (the path between two nodes uses a third-order spiral curve).
[0116] (2) Trajectory and Cost Calculation: The subsequent steps are similar to step 2.(3). Obtain the connection trajectory between a pair of nodes and the cost after considering dynamic obstacles. Repeat the above process until all mesh nodes have been updated.
[0117] Step 4. Select the optimal trajectory, see Figure 5 :
[0118] (1) Select the optimal trajectory endpoint: After step 4, the cost of all nodes on the grid has been updated. Based on a certain endpoint selection strategy, the optimal node is selected as the trajectory endpoint.
[0119] (2) Backtrack from the endpoint of the optimal trajectory to find the final planned optimal trajectory.
[0120] The trajectory planning method based on spatiotemporal sampling provided by this invention can cover the optimal trajectory in time and space, thereby preventing collisions with other vehicles and improving the level of intelligent driving.
[0121] The trajectory planning method based on spatiotemporal sampling provided by this invention extensively utilizes offline initial guess tables (IGTs) as initial solutions for path generation parameters, accelerating the generation process and enabling real-time planning while ensuring path coverage. Furthermore, the method employs lookup tables (LUTs) to accelerate coordinate system transformation efficiency on regular roads, enabling real-time planning and ensuring safety. Finally, the method establishes static and dynamic cost maps based on the Fréchet coordinate system, providing an efficient cost representation for evaluating the optimal trajectory.
[0122] Those skilled in the art can understand this embodiment as a more specific description of Embodiment 1 and Embodiment 2.
[0123] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.
[0124] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.
Claims
1. A method for intelligent vehicle trajectory planning based on spatiotemporal sampling evaluation, characterized in that, The method includes the following steps: Step S1: Initialize the planner; Step S2: Update the road search space representation; Step S3: Generate an optional trajectory starting from the vehicle's position and ending at a node in the grid; Step S4: Generate an optional trajectory using two nodes in the grid as the starting and ending points; Step S5: Select the optimal endpoint among the grid nodes; Step S6: Backtrack to the vehicle's position from the optimal endpoint and select the optimal trajectory; In step S1, the dispersion of the spatiotemporal state grid in the horizontal and vertical directions, acceleration, velocity range, and time range is defined during initialization, and an initial guess table (IGT) is established for numerically solving the parameters of the third-order spiral curve between two points. The initialization operation is only performed once during planner initialization. Step S2 includes the following steps: Step S2.1: Update mesh nodes and internal connection paths. Based on the predefined Connectivity Pattern between mesh nodes, establish a mesh representation along the reference line. Step S2.2: Offline lookup table, LUT update. As the reference line changes, update a correspondence description that maps each grid value in the Cartesian coordinate system (X, Y) to the Fraser coordinate system (S, L) and store it in a lookup table (LUT). Step S2.3: Update the static cost map and the dynamic cost map. By looking up the coordinate information of static obstacles and dynamic obstacles and their predicted trajectories through the LUT, update the cost described in the SL coordinate system on each grid cell.
2. The intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation according to claim 1, wherein step S3 includes the following steps: Step S3.1: Calculate the planning starting point, compensate for the vehicle's movement during the planning control delay, and estimate the vehicle's state s_future in the short future; Step S3.2: Path generation, calculate the path connecting s_future and mesh nodes; Step S3.3: Trajectory and cost calculation. Sampling path points on the path calculated in step S3.2 and calculating the cost_static with the static obstacle cost map; adding the acceleration parameter accprofile to each path to obtain the trajectory; calculating the cost_dynamic of dynamic obstacles on the dynamic obstacle cost map based on the trajectory; at each node, selecting the lowest cost from all trajectories reaching that node and updating it as the cost of that node.
3. The intelligent vehicle trajectory planning method based on spatiotemporal sampling evaluation according to claim 1, wherein step S4 includes the following steps: Step S4.1: Path generation. For each vertical discrete value station, calculate the path from the station to nodes on other stations based on the predefined extension method Connectivity Pattern and the spiral curve parameters updated in step S3. Step S4.2: Trajectory and cost calculation. The subsequent steps are the same as in step S3.
3. The connection trajectory between a pair of nodes and the cost after considering dynamic obstacles are obtained. The above process is repeated until all grid nodes have been updated. Step S5 includes the following steps: Step S5.1: Select the optimal trajectory endpoint. After step S5, the cost of all nodes on the grid has been updated. Select the optimal node as the trajectory endpoint according to the preset endpoint selection strategy. Step S5.2: Backtrack from the endpoint of the optimal trajectory to find the final planned optimal trajectory.
4. An intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation, characterized in that, The system includes the following modules: Module M1: Initialize the planner; Module M2: Updates the road search space representation; Module M3: Generates an optional trajectory starting from the vehicle's position and ending at a node in the grid; Module M4: Generates an optional trajectory using two nodes in the grid as the starting and ending points; Module M5: Select the optimal endpoint among the grid nodes; Module M6: Backtracks to the vehicle's position from the optimal endpoint to select the optimal trajectory; During initialization, the spatiotemporal state grid is defined in terms of the horizontal and vertical axes, acceleration, velocity range, and time range. An initial guess table (IGT) is also established to numerically solve the parameters of the third-order spiral curve between two points. The initialization operation is only performed once during planner initialization. Module M2 includes the following modules: Module M2.1: Mesh node and internal connection path update, establishes mesh representation along reference lines based on the predefined Connectivity Pattern between mesh nodes; Module M2.2: Offline lookup table, LUT update. As the reference line changes, it updates a description of the correspondence between each grid value in the Cartesian coordinate system (X, Y) and the Fraser coordinate system (S, L), and stores it in a lookup table (LUT). Module M2.3: Updates the static cost map and dynamic cost map. By looking up the coordinate information of static and dynamic obstacles and their predicted trajectories through a LUT, the cost described in the SL coordinate system on each grid cell is updated.
5. The intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation according to claim 4, wherein module M3 comprises the following modules: Module M3.1: Calculates the starting point of the planning, compensates for the movement of the vehicle during the planning and control delay, and estimates the vehicle's state s_future in the short future; Module M3.2: Path generation, calculates the path connecting s_future and mesh nodes; Module M3.3: Trajectory and cost calculation. Sampling path points on the path calculated in Module M3.2 and calculating the cost_static with the static obstacle cost map; adding the acceleration parameter accprofile to each path to obtain the trajectory; calculating the cost_dynamic of dynamic obstacles on the dynamic obstacle cost map based on the trajectory; at each node, selecting the lowest cost from all trajectories reaching that node to update the cost of that node.
6. The intelligent vehicle trajectory planning system based on spatiotemporal sampling evaluation according to claim 4, wherein module M4 comprises the following modules: Module M4.1: Path generation. For each vertical discrete value station, the path from the station to nodes on other stations is calculated based on the predefined extension method Connectivity Pattern and the updated spiral curve parameters in Module M3. Module M4.2: Trajectory and cost calculation. Subsequent modules are the same as module M3.
3. They obtain the connection trajectory between a pair of nodes and the cost after considering dynamic obstacles. The above process is repeated until all grid nodes have been updated. Module M5 includes the following modules: Module M5.1: Select the optimal trajectory endpoint. After module M5, the cost of all nodes on the grid has been updated. The optimal node is selected as the trajectory endpoint according to the preset endpoint selection strategy. Module M5.2: Backtracks from the endpoint of the optimal trajectory to find the final planned optimal trajectory.