Optimal velocity search method and system based on velocity-time graph

Abstract

This invention provides an optimal speed search method and system based on a speed-time graph, comprising the following steps: Sampling step: In autonomous driving, the time axis and speed axis are sampled using the vehicle's speed-time graph, dividing the two-dimensional space into multiple preset spaces, each preset space being considered a node; Evaluation step: Starting from the first node, the cost of each sampled node is calculated using a cost function, updating the minimum cost of each node as the calculation progresses until the cost calculation for all nodes in the last layer is completed; Optimal speed selection step: The node with the minimum cost is selected from the last layer of nodes, and the information of each node is traced back to obtain an optimal speed passing through n nodes as a speed reference for speed optimization. This invention incorporates jerk range during the search process, narrowing the search space and reducing search time.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and more specifically, to an optimal speed search method and system based on a speed-time graph. Background Technology

[0002] As a crucial function of the motion planning subsystem, the speed planning module in autonomous driving directly impacts the performance of the downstream longitudinal control layer. Inappropriate target speeds and accelerations can exceed the vehicle's operational limits, affecting the safety and comfort of passengers. Currently, the speed planning module is essentially divided into two parts: the first part uses a search method to generate the optimal speed, and the second part uses an optimization method to smooth the speed based on the optimal speed.

[0003] Existing technology 1: Optimal speed search method based on mileage-time graph (ST graph), which searches for and generates the optimal speed in a two-dimensional space with the horizontal axis being the time axis and the vertical axis being the mileage axis.

[0004] The existing technology specifically includes three parts: sampling, evaluation, and optimal speed selection.

[0005] 1. Sampling: The time axis and mileage axis are sampled uniformly or sparsely and densely through threshold sampling. The two-dimensional space is divided into many small spaces, and each small space is a node. The node includes information such as mileage, speed, time, acceleration, jerk, cost, and the previous node.

[0006] 2. Evaluation: Starting with the first node, the cost function is used to calculate the cost of each of the sampled nodes. The minimum cost of each node is updated as the calculation progresses until the costs of all nodes in the last layer are calculated. During this process, information such as the cost and the previous node are updated for subsequent selection. When updating the cost of each node, nodes are filtered based on their acceleration range; nodes that do not meet the requirements are not included in the cost calculation.

[0007] 2.1 Cost Calculation:

[0008] a. Obstacle cost: Calculated based on the distance from the obstacle; the farther away from the obstacle, the lower the cost.

[0009] b. Edge cost: including velocity cost, acceleration cost, and jerk cost.

[0010] 2.2 Node Update: Based on the current node, determine whether the next node is within the velocity and acceleration range. If not, skip the node and do not calculate the cost; if it is, update the cost.

[0011] Given the current node's mileage s_curr, velocity v_curr, next node's mileage s_next, and time sampling resolution t:

[0012] c. Calculate the acceleration a_next of the next node, and determine if it is within the acceleration range. If not, do not proceed with the following steps:

[0013] a_next=2×((s_next-s_curr) / t-v_curr) / t.

[0014] d. Calculate the velocity v_next based on a_next, and determine if it is within the velocity range. If not, do not proceed to the next step:

[0015] v_next = v_curr + a_next × t.

[0016] e. Calculation of the cost of the next node and node update.

[0017] 3. Optimal speed selection: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain the optimal speed through n nodes as the speed reference for speed optimization.

[0018] Prior Art 2: Chinese invention patent document CN112965501A discloses an autonomous driving speed planning method and device. Based on the current vehicle pose information, the behavioral commands output by the autonomous driving system, and the trajectory information output by the trajectory planning layer, the method updates the speed curve from the current moment to the target moment in real time. It configures the speed and acceleration at the current moment and the speed and acceleration at the target moment, and generates speed curves under different configurations based on a polynomial speed curve algorithm, using multiple generated speed curves as candidate speed curves. It solves for the parameters of each candidate speed curve through differentiation, including real-time speed, real-time acceleration, and real-time jerk. Based on vehicle kinematic constraints and the parameters of the candidate speed curves, it filters the candidate speed curves to obtain the optimal speed curve.

[0019] Prior Art 3: Chinese invention patent document CN115437377A discloses an autonomous driving speed planning method, electronic device, vehicle, and storage medium. The autonomous driving speed planning method includes acquiring historical operating data and actual road characteristics for each operating route; creating nodes based on actual road characteristics, constructing a directed graph with road segments between nodes as edges, and the attributes of the edges including historical operating data; statistically analyzing historical information for each road segment based on the directed graph, and using the historical information for each road segment to train a fuel consumption model; inputting real-time vehicle information into the trained fuel consumption model to output the target speed of the vehicle on each road segment based on the fuel consumption model; statistically analyzing historical information for each road segment from massive amounts of data through a directed graph, and inputting the historical information of the road segments into the fuel consumption model, thereby realizing the planning of vehicle speed based on fuel consumption.

[0020] Regarding the aforementioned technologies, the inventors believe that during the evaluation, only the mileage range of the next node that the current node can reach within the speed and acceleration range is considered, but jerk is not taken into account, resulting in a large number of search nodes and a long search time. Summary of the Invention

[0021] In view of the shortcomings of the prior art, the purpose of this invention is to provide an optimal velocity search method and system based on velocity-time diagrams.

[0022] An optimal velocity search method based on a velocity-time diagram according to the present invention includes the following steps:

[0023] Sampling steps: In autonomous driving, the time axis and velocity axis are sampled through the vehicle's speed-time graph, and the two-dimensional space is divided into multiple preset spaces, with each preset space serving as a node;

[0024] Evaluation steps: For all nodes obtained from sampling, starting from the first node, calculate the cost of each node using the cost function, and update the minimum cost of each node while calculating, until the cost of all nodes in the last layer has been calculated;

[0025] The optimal speed selection steps are as follows: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain an optimal speed that passes through n nodes as a speed reference for speed optimization.

[0026] Preferably, in the sampling step, the sampling method includes uniform sampling of the time axis and the velocity axis;

[0027] Sampling methods also include sparse sampling and dense sampling of both the time axis and the velocity axis;

[0028] Each node includes mileage, speed, time, acceleration, jerk, cost, and information about the previous node.

[0029] Preferably, in the evaluation step, when updating the cost of each node, nodes are filtered according to velocity range, acceleration range, and jerk range, and the cost is calculated for nodes that meet the requirements.

[0030] Preferably, the evaluation step includes the following steps:

[0031] Cost calculation steps: Calculate obstacle cost and edge cost;

[0032] Node update steps: Based on the current node, infer the next node through acceleration and jerk. Determine whether the next node is within the acceleration range and jerk range. If not, skip the node and do not perform cost calculation. If it is, perform cost calculation, obtain the next node and update the cost of the next node.

[0033] Preferably, in the cost calculation step, the obstacle cost is calculated based on the distance between the node and the obstacle; the farther the node is from the obstacle, the smaller the cost.

[0034] The edge cost includes velocity cost, acceleration cost, and jerk cost.

[0035] Preferably, the node update step includes the following steps:

[0036] Step S1: Given the current node's mileage s_curr, velocity v_curr, acceleration a_curr, and time sampling resolution t; given the candidate jerks for jerks, and initialize the upper and lower boundaries of the jerks to infinity; iterate through each jerk;

[0037] Step S2: Determine if the jerk is within the jerk acceleration range. If it is, proceed to step S3; otherwise, repeat step S1.

[0038] Step S3: Calculate the velocity v_next_init of the next node, and map v_next_init to the node v_next according to the velocity sampling resolution:

[0039] v_next_init=v_curr+a_curr×t+1 / 2×jerk×t×t;

[0040] Step S4: Calculate the acceleration a_next and jerk j_next of the next node:

[0041] j_next=2×(v_next-v_curr-a_curr×t) / (t×t);

[0042] a_next = a_curr - j_next × t;

[0043] Step S5: Determine whether a_next is within the acceleration range. If it is, proceed to step S6; otherwise, update the upper or lower bound of the acceleration range to jerk and repeat step S1.

[0044] Step S6: Calculate the mileage s_next of the next node:

[0045] s_next=s_curr+v_curr×t+1 / 2×a_curr×t×t+1 / 6×jerk×t×t×t;

[0046] Step S7: Next node cost update: If the cost calculated for the next node is smaller than the cost of the nodes that have already been added, then perform a cost update; otherwise, do not perform an update.

[0047] An optimal velocity search system based on a velocity-time diagram, provided by the present invention, includes the following modules:

[0048] Sampling module: In autonomous driving, the time axis and velocity axis are sampled by the vehicle's speed-time graph, and the two-dimensional space is divided into multiple preset spaces, with each preset space serving as a node;

[0049] Evaluation module: Take all the nodes obtained from sampling, starting from the first node, and use the cost function to calculate the cost of each node. Update the minimum cost of each node while calculating, until the cost of all nodes in the last layer has been calculated.

[0050] Optimal speed selection module: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain an optimal speed that passes through n nodes as a speed reference for speed optimization.

[0051] Preferably, in the sampling module, the sampling method includes uniform sampling of the time axis and the velocity axis;

[0052] Sampling methods also include sparse sampling and dense sampling of both the time axis and the velocity axis;

[0053] Each node includes mileage, speed, time, acceleration, jerk, cost, and information about the previous node.

[0054] Preferably, in the evaluation module, when updating the cost of each node, the nodes are filtered according to the velocity range, acceleration range, and jerk range, and the cost is calculated for the nodes that meet the requirements.

[0055] Preferably, the evaluation module includes the following modules:

[0056] Cost calculation module: Calculates obstacle cost and edge cost;

[0057] Node update module: Based on the current node, infer the next node through acceleration and jerk. Determine whether the next node is within the acceleration range and jerk range. If not, skip the node and do not perform cost calculation. If it is, perform cost calculation, obtain the next node and update the cost of the next node.

[0058] Compared with the prior art, the present invention has the following beneficial effects:

[0059] 1. This invention incorporates accelerometer range during the search process, thereby narrowing the search space and reducing search time;

[0060] 2. This invention reduces the number of search nodes and improves search efficiency while ensuring the optimality of the search solution. Attached Figure Description

[0061] 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:

[0062] Figure 1 This is a flowchart of the present invention. Detailed Implementation

[0063] 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.

[0064] This invention discloses an optimal velocity search method based on a velocity-time graph, such as... Figure 1 As shown, the present invention specifically includes three parts: sampling, evaluation, and optimal speed selection.

[0065] 1. Sampling: The time axis and velocity axis are sampled uniformly or sparsely and densely through threshold sampling. The two-dimensional space is divided into many small spaces, and each small space is a node. The node includes information such as mileage, velocity, time, acceleration, jerk, cost, and the previous node.

[0066] 2. Evaluation: Starting from the first node, calculate the cost of each of the sampled nodes using the cost function. Update the minimum cost of each node as you calculate, until the cost of all nodes in the last layer has been calculated. When updating the cost of each node, nodes are filtered based on velocity range, acceleration range, and jerk range; nodes that do not meet the requirements are not subject to cost calculation.

[0067] The speed range, acceleration range, and jerk range are determined based on vehicle parameters and set parameters. For example, the speed range is 0 to 12.5, where 12.5 is the currently set maximum speed for autonomous vehicles, but the value is not fixed and different values ​​can be set for different vehicles. The acceleration range is determined based on the actual parameters of the vehicle and can also be set appropriately. The same applies to jerk.

[0068] The evaluation includes the following:

[0069] 2.1 Cost Calculation: a. Obstacle Cost: Calculated based on the distance to the obstacle; the farther away from the obstacle, the lower the cost. b. Edge Cost: Includes velocity cost, acceleration cost, and jerk cost.

[0070] 2.2 Node Update: Based on the current node, infer the next node through acceleration and jerk, and determine whether the next node is within the acceleration range and jerk range. If not, skip the node and do not calculate the cost; if it is, calculate the cost, obtain the next node, and update the cost of the next node.

[0071] Given the current node's mileage s_curr, velocity v_curr, acceleration a_curr, jerks as candidate jerk values, and initialized jerks with infinite upper and lower boundaries, and a time sampling resolution t, iterate through each jerk.

[0072] c. Determine if the jerk is within the jerk's acceleration range. If it is not within the range, do not proceed to the next step.

[0073] d. Calculate the velocity v_next_init of the next node and map it to the node v_next according to the velocity sampling resolution. v_next_init is calculated according to the formula and is a specific value, but the velocity is divided into several ranges. This specific value is projected onto one of these ranges as v_next. For example, if the range is divided into 0, 0.1, 0.2, the calculated v_next_init is 0.07, which is mapped to the node 0.1 within the range. v_next_init is used for v_next. Many specific values ​​are calculated, and many values ​​may be mapped to a single node, but in the end, only the value with the lowest cost is placed in this node. After all calculations are completed, the node string with the lowest cost is found, which contains only nodes.

[0074] v_next_init=v_curr+a_curr×t+1 / 2×jerk×t×t.

[0075] e. Calculate the acceleration a_next and jerk j_next of the next node:

[0076] j_next=2×(v_next-v_curr-a_curr×t) / (t×t),

[0077] a_next = a_curr - j_next × t.

[0078] f. Determine if a_next is within the acceleration range. If it is not within the range, update the upper or lower bound of the acceleration range to jerk, but do not perform subsequent steps.

[0079] g. Calculate the mileage s_next of the next node:

[0080] s_next=s_curr+v_curr×t+1 / 2×a_curr×t×t+1 / 6×jerk×t×t×t.

[0081] h. Next node cost update: If the cost calculated for the next node is smaller than the costs of other nodes that have already been added, then a cost update is performed; otherwise, no update is performed.

[0082] 3. Optimal speed selection: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain the optimal speed through n nodes as the speed reference for speed optimization.

[0083] When updating the cost of each node, this invention filters nodes based on velocity range, acceleration range, and jerk range, omitting cost calculations for nodes that do not meet the requirements; and updates the nodes that do perform cost calculations. This invention, while ensuring the optimality of the search solution, considers the jerk range, reduces the number of search nodes, improves search efficiency, and significantly alleviates the problem of search time consumption.

[0084] The present invention also provides an optimal speed search system based on a speed-time graph. The optimal speed search system based on a speed-time graph can be implemented by executing the process steps of the optimal speed search method based on a speed-time graph. That is, those skilled in the art can understand the optimal speed search method based on a speed-time graph as a preferred embodiment of the optimal speed search system based on a speed-time graph.

[0085] The system includes the following modules:

[0086] Sampling Module: In autonomous driving, the time axis and velocity axis are sampled using the vehicle's speed-time graph. The two-dimensional space is divided into multiple preset spaces, each of which serves as a node. Sampling methods include uniform sampling of the time axis and velocity axis; sparse sampling and dense sampling of both the time axis and velocity axis are also included. Nodes include mileage, speed, time, acceleration, jerk, cost, and information about the previous node.

[0087] Evaluation Module: Starting with the first sampled node, the module calculates the cost of each node using a cost function, updating the minimum cost of each node as it is calculated, until the costs of all nodes in the last layer are calculated. When updating the cost of each node, nodes are filtered based on velocity range, acceleration range, and jerk range; only nodes meeting these criteria are included in the cost calculation.

[0088] The evaluation module includes the following modules: Cost calculation module: calculates obstacle cost and edge cost; Node update module: based on the current node, infers the next node through acceleration and jerk, and determines whether the next node is within the acceleration range and jerk range. If not, the node is skipped and no cost calculation is performed. If it is, the cost calculation is performed, the next node is obtained, and the cost of the next node is updated.

[0089] Optimal speed selection module: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain an optimal speed that passes through n nodes as a speed reference for speed optimization.

[0090] 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.

[0091] 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. An optimal velocity search method based on a velocity-time graph, characterized in that, Includes the following steps: Sampling steps: In autonomous driving, the time axis and velocity axis are sampled through the vehicle's speed-time graph, and the two-dimensional space is divided into multiple preset spaces, with each preset space serving as a node; Evaluation steps: For all nodes obtained from sampling, starting from the first node, calculate the cost of each node using the cost function, and update the minimum cost of each node while calculating, until the cost of all nodes in the last layer has been calculated; Optimal speed selection steps: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain an optimal speed that passes through n nodes as a speed reference for speed optimization. The evaluation process includes the following steps: Cost calculation steps: Calculate obstacle cost and edge cost; Node update steps: Based on the current node, infer the next node through acceleration and jerk. Determine whether the next node is within the acceleration range and jerk range. If not, skip the node and do not perform cost calculation. If it is, perform cost calculation, obtain the next node and update the cost of the next node. The node update steps include the following steps: Step S1: Given the current node's mileage s_curr, velocity v_curr, acceleration a_curr, and time sampling resolution t; given the candidate jerks for jerks, and initialize the upper and lower boundaries of the jerks to infinity; iterate through each jerk; Step S2: Determine if the jerk is within the jerk acceleration range. If it is, proceed to step S3; otherwise, repeat step S1. Step S3: Calculate the velocity v_next_init of the next node, and map v_next_init to the node v_next according to the velocity sampling resolution: v_next_init = v_curr + a_curr × t + 1 / 2 × jerk × t × t; Step S4: Calculate the acceleration a_next and jerk j_next of the next node: j_next = 2 × (v_next - v_curr - a_curr × t) / (t × t); a_next = a_curr - j_next × t; Step S5: Determine whether a_next is within the acceleration range. If it is, proceed to step S6; otherwise, update the upper or lower bound of the acceleration range to jerk and repeat step S1. Step S6: Calculate the mileage s_next of the next node: s_next = s_curr + v_curr × t + 1 / 2 × a_curr × t × t + 1 / 6 × jerk ×t × t × t; Step S7: Next node cost update: If the cost calculated for the next node is smaller than the cost of the nodes that have already been added, then perform a cost update; otherwise, do not perform an update.

2. The optimal velocity search method based on velocity-time diagram according to claim 1, characterized in that, In the sampling step, the sampling method includes uniform sampling of the time axis and the velocity axis; Sampling methods also include sparse sampling and dense sampling of both the time axis and the velocity axis; Each node includes mileage, speed, time, acceleration, jerk, cost, and information about the previous node.

3. The optimal velocity search method based on velocity-time diagram according to claim 1, characterized in that, In the evaluation step, when updating the cost of each node, nodes are filtered based on velocity range, acceleration range, and jerk range, and the cost is calculated for nodes that meet the requirements.

4. The optimal velocity search method based on velocity-time diagram according to claim 1, characterized in that, In the cost calculation step, the obstacle cost is calculated based on the distance between the node and the obstacle; the farther the node is from the obstacle, the smaller the cost. The edge cost includes velocity cost, acceleration cost, and jerk cost.

5. An optimal velocity search system based on a velocity-time diagram, characterized in that, The method for performing the optimal velocity search method based on velocity-time diagrams as described in any one of claims 1-4 includes the following modules: Sampling module: In autonomous driving, the time axis and velocity axis are sampled by the vehicle's speed-time graph, and the two-dimensional space is divided into multiple preset spaces, with each preset space serving as a node; Evaluation module: Take all the nodes obtained from sampling, starting from the first node, and use the cost function to calculate the cost of each node. Update the minimum cost of each node while calculating, until the cost of all nodes in the last layer has been calculated. Optimal speed selection module: Select the node with the lowest cost from the last layer of nodes, backtrack the information of each node, and obtain an optimal speed that passes through n nodes as a speed reference for speed optimization.

6. The optimal velocity search system based on velocity-time diagram according to claim 5, characterized in that, In the sampling module, the sampling method includes uniform sampling of the time axis and the velocity axis; Sampling methods also include sparse sampling and dense sampling of both the time axis and the velocity axis; Each node includes mileage, speed, time, acceleration, jerk, cost, and information about the previous node.

7. The optimal velocity search system based on velocity-time diagram according to claim 5, characterized in that, In the evaluation module, when updating the cost of each node, nodes are filtered based on velocity range, acceleration range, and jerk range, and the cost is calculated for nodes that meet the requirements.

8. The optimal velocity search system based on velocity-time diagram according to claim 7, characterized in that, The evaluation module includes the following modules: Cost calculation module: Calculates obstacle cost and edge cost; Node update module: Based on the current node, infer the next node through acceleration and jerk. Determine whether the next node is within the acceleration range and jerk range. If not, skip the node and do not perform cost calculation. If it is, perform cost calculation, obtain the next node and update the cost of the next node.

Images