A fast evaluation search method, system and terminal device for an optimal trajectory

Abstract

The application relates to the technical field of trajectory planning, and proposes a fast evaluation search method, a system and a terminal device of an optimal trajectory. The fast evaluation search method comprises the following steps: sampling a plurality of state points in a pre-planning space, performing pre-collision detection on the state points to remove points in conflict with obstacles, and obtaining effective sampling points; performing cost evaluation on the effective sampling points, wherein the cost evaluation comprises heuristic cost based on historical planning trajectory information and static cost based on motion characteristics; based on the result of the cost evaluation, iteratively searching from an initial state point: calculating the trajectory cost of a neighborhood state point and updating a search path in a gradient direction until an optimal feasible trajectory is output. The application improves the search efficiency of the trajectory by pre-collision detection to screen out invalid sampling points and a gradient iterative search mechanism.

Description

Technical Field

[0001] This invention relates to the field of trajectory planning technology, and in particular to a rapid evaluation and search method, system and terminal device for optimal trajectory. Background Technology

[0002] With the continuous development of low-speed autonomous driving applications such as delivery, patrol, and road sweeping vehicles, the role of robotics in social services is becoming increasingly prominent. However, due to the size and power limitations of these vehicles, their computing platforms suffer from limited computing resources. In particular, the onboard perception module is heavily burdened, leaving very limited computing resources for the planning module. This makes it difficult for many trajectory planning algorithms to meet real-time requirements and deploy them in real-world scenarios.

[0003] Currently, sampling-based trajectory planning is widely used in the mobile robotics community. However, for most of these algorithms, there is a trade-off between the number of trajectory samples and the planning frequency, and in recent research, the trajectory planning problem on resource-constrained computers has not been well solved.

[0004] The shortcomings of existing methods are as follows:

[0005] When the candidate trajectory set is large, the traversal detection method will bring a significant computational burden, resulting in a sharp increase in the time consumption of collision detection, which is difficult to meet the real-time requirements of practical applications.

[0006] Traditional solutions typically require a global traversal of the entire trajectory set and complex calculations of multi-dimensional evaluation metrics for each trajectory, which leads to a sharp increase in computational complexity and a significant increase in the demand for computing resources, making it difficult to meet real-time requirements. Summary of the Invention

[0007] To address the aforementioned shortcomings, the present invention aims to propose a rapid evaluation and search method, system, and terminal device for optimal trajectories, which aims to improve the search efficiency of trajectories through pre-collision detection to filter out invalid sampling points and a gradient iterative search mechanism.

[0008] To achieve this objective, the present invention adopts the following technical solution:

[0009] A fast evaluation and search method for optimal trajectories, the fast evaluation and search method comprising:

[0010] Multiple state points are sampled in the pre-planned space, and pre-collision detection is performed on the state points to remove points that conflict with obstacles, thereby obtaining valid sampling points;

[0011] Cost evaluation is performed on the effective sampling points, including heuristic cost based on historical planning trajectory information and static cost based on motion characteristics;

[0012] Based on the cost evaluation results, an iterative search is performed starting from the initial state point: the trajectory cost of the neighboring state points is calculated and the search path is updated along the gradient direction until the optimal feasible trajectory is output.

[0013] Preferably, performing pre-collision detection on the state points to remove points that conflict with obstacles, and obtaining valid sampling points includes:

[0014] Establish a grid map of the planning space, sample state sampling points on the grid map, and project environmental obstacles onto the grid map;

[0015] Invalid sampling points are marked by comparing the spatial relationship between the sampling point grid and the obstacle grid: when the sampling point grid overlaps with the obstacle grid, it is determined to be a collision, and the sampling point grid is marked as an invalid sampling point;

[0016] Invalid sampling points are removed to obtain valid sampling points.

[0017] Preferably, the heuristic cost assessment includes:

[0018] Using the end sampling points of historical planning trajectories as reference benchmarks, the end sampling points of trajectories are preferentially selected within the neighborhood of the reference benchmarks in the current planning cycle.

[0019] The heuristic cost of the current sampling point is determined by calculating the deviation between the current sampling point state and the state of the final sampling point of the historical planned trajectory.

[0020] Furthermore, the static cost assessment includes:

[0021] Calculate the cost value of each valid sampling point in the horizontal offset dimension, velocity dimension, and time dimension to form the static cost component of each valid sampling point.

[0022] Furthermore, the cost evaluation of the effective sampling points also includes:

[0023] The heuristic cost value of each valid sampling point is weighted and combined with the static cost component to obtain the final cost value of the valid sampling point.

[0024] Preferably, the method further includes sorting the cost evaluation results of the effective sampling points and selecting the sampling point with the lowest cost as the initial state point.

[0025] Furthermore, based on the cost evaluation results, an iterative search is performed starting from the initial state point: calculating the trajectory cost of neighboring state points and updating the search path along the gradient direction until the optimal feasible trajectory is output, including the following steps:

[0026] S10: Using the current optimal trajectory cost as the search benchmark, calculate the set of neighborhood state points of the current optimal trajectory's end state point in the current state space;

[0027] S20: Generate the corresponding trajectory for each neighborhood state point and calculate the true cost value to obtain the neighborhood trajectory cost set;

[0028] S30: Determine the gradient search direction based on the comparison between the neighborhood trajectory cost set and the current optimal trajectory cost, and update the search trajectory along the gradient direction;

[0029] If the updated search path has not been explored, repeat steps S10-S30;

[0030] If the updated search trajectory has already been explored, then constraint satisfaction detection and collision detection are performed on the updated search trajectory:

[0031] If the constraint satisfaction test and collision detection pass, the output is the optimal feasible trajectory;

[0032] If the constraint satisfaction test and collision test fail, proceed to steps S10-S30.

[0033] A fast evaluation search system for optimal trajectories, wherein the fast evaluation search system is applied to the fast evaluation search method for optimal trajectories as described above, and the fast evaluation search system comprises:

[0034] The sampling module is used to sample multiple state points in a pre-planned space and perform pre-collision detection on the state points to remove points that conflict with obstacles, thereby obtaining valid sampling points;

[0035] An evaluation module is used to evaluate the cost of the effective sampling points. The cost evaluation includes heuristic cost based on historical planning trajectory information and static cost based on motion characteristics.

[0036] The search module is used to iteratively search from the initial state point based on the result of the cost evaluation: calculate the trajectory cost of the neighboring state points and update the search path along the gradient direction until the optimal feasible trajectory is output.

[0037] A terminal device includes: a memory, a processor, and a program stored in the memory and executable on the processor, the program being configured to implement the steps of the fast evaluation search method as described above.

[0038] A storage medium storing a fast evaluation search program, which, when executed by a processor, implements the steps of the fast evaluation search method as described above.

[0039] One of the above technical solutions has the following advantages or beneficial effects:

[0040] This invention obtains effective sampling points by sampling multiple state points in a pre-planned space and performing pre-collision detection on these state points to remove points that conflict with obstacles. This effectively eliminates a large number of unnecessary sampling points, greatly reducing subsequent unnecessary computation and thus improving the overall computational efficiency of the algorithm. Especially in complex environments, it achieves more efficient processing. By evaluating the cost of the effective sampling points, the cost distribution of state points can be estimated. The heuristic cost is calculated by focusing on historical planning results, which better guides the calculation of the optimal trajectory and achieves more efficient optimal trajectory calculation. The search for the optimal feasible trajectory can be directional, quickly finding the optimal trajectory by exploring only a small number of trajectories without traversing and generating all possible trajectories. This process greatly saves computation and further improves the overall computational efficiency of the algorithm. It effectively solves the problem of high computational complexity in traditional "sampling-evaluation-selection" methods, providing a stable, efficient, and real-time trajectory planning algorithm for mobile robots with low computing power. Attached Figure Description

[0041] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0042] Figure 1 This is a first flowchart of the fast evaluation and search method for the optimal trajectory provided in the embodiments of the present invention;

[0043] Figure 2 This is a schematic diagram of the structure of the fast evaluation and search system for the optimal trajectory provided in an embodiment of the present invention;

[0044] Figure 3 This is a flowchart of the optimal trajectory search process for the fast evaluation and search method for the optimal trajectory provided in this embodiment of the invention.

[0045] Figure 4 This is a flowchart of a traditional trajectory search method;

[0046] Figure 5 This is a first flowchart of the fast evaluation and search method for the optimal trajectory provided in the embodiments of the present invention;

[0047] Figure 6 This is a sampling point marking map implemented by the fast evaluation and search method for the optimal trajectory provided in this embodiment of the invention;

[0048] Figure 7 This is an invalid sampling point marking map implemented by the fast evaluation and search method for the optimal trajectory provided in this embodiment of the invention;

[0049] Figure 8 This is an effective sampling point marking map implemented by the fast evaluation and search method for the optimal trajectory provided in this embodiment of the invention;

[0050] Figure 9 This is a schematic diagram of the optimal trajectory generation in the fast evaluation and search method for the optimal trajectory provided in the embodiment of the present invention. Detailed Implementation

[0051] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0052] In this invention, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0053] With the continuous development of low-speed autonomous driving applications such as delivery, patrol, and road sweeping vehicles, the role of robotics in social services is becoming increasingly prominent. However, due to the size and power limitations of these vehicles, their computing platforms suffer from limited computing resources. In particular, the onboard perception module is heavily burdened, leaving very limited computing resources for the planning module. This makes it difficult for many trajectory planning algorithms to meet real-time requirements and deploy them in real-world scenarios.

[0054] Currently, sampling-based trajectory planning is widely used in the mobile robotics community. However, for most of these algorithms, there is a trade-off between the number of trajectory samples and the planning frequency, and in recent research, the trajectory planning problem on resource-constrained computers has not been well solved.

[0055] The existing method flowchart is as follows Figure 4 As shown, its shortcomings are as follows:

[0056] When the candidate trajectory set is large, the traversal detection method will bring a significant computational burden, resulting in a sharp increase in the time consumption of collision detection, which is difficult to meet the real-time requirements of practical applications.

[0057] Traditional solutions typically require a global traversal of the entire trajectory set and complex calculations of multi-dimensional evaluation metrics for each trajectory, which leads to a sharp increase in computational complexity and a significant increase in the demand for computing resources, making it difficult to meet real-time requirements.

[0058] Therefore, a fast evaluation and search method for the optimal trajectory is proposed, such as... Figure 1 and Figure 5 As shown, in a preferred embodiment of the present invention, the rapid evaluation search method includes the following steps:

[0059] S1: Sample multiple state points in the pre-planned space, and perform pre-collision detection on the state points to remove points that conflict with obstacles, thereby obtaining valid sampled points (e.g., Figure 8 (as shown);

[0060] Here, a state point refers to a sampling point generated in the state space according to certain rules, such as... Figure 6 As shown, each state point corresponds to a potential trajectory deformation parameter feature, which is used as a reference for subsequent trajectory planning. Pre-collision detection is a mechanism for early filtering of invalid sampling points. By comparing the spatial relationship between the sampling point and the obstacle on the grid map, it determines whether a collision has occurred. If overlap exists, it is marked as an invalid point (e.g., ...). Figure 7 As shown in the diagram, this ensures that all sampling points used in subsequent calculations are located in free space. Obstacles refer to fixed or dynamic obstacles pre-defined in the environment model; their position information is projected onto the grid map for collision detection and judgment with the sampling points.

[0061] Specifically, step S1 generates a sufficient number of sampling points in the state space to characterize potential motion states, while using collision detection to pre-filter out invalid sampling points that collide with obstacles, thereby narrowing the effective range of subsequent searches. The aim is to reduce the complexity of subsequent calculations and improve the overall algorithm efficiency while ensuring sufficient sampling. Its function is to provide high-quality basic input for subsequent cost evaluation, ensuring that the algorithm's computational resources are concentrated within the feasible solution space and avoiding unnecessary calculations on invalid sampling points.

[0062] S2: Perform a cost evaluation on the effective sampling points, the cost evaluation including heuristic cost based on historical planning trajectory information and static cost based on motion characteristics;

[0063] Historical planning trajectory information refers to trajectory data generated in the previous planning cycle, including the trajectory's terminal state point, velocity characteristics, and other information, used to construct a heuristic evaluation model. Heuristic cost is used to determine the priority of the sampling point, while static cost reflects the basic motion characteristic constraints of the sampling point within the current planning cycle.

[0064] Step S2 comprehensively considers historical planning information and current motion characteristics, assigning a comprehensive cost value to each sampling point through multi-dimensional cost calculation. This cost value reflects the priority of the sampling point in trajectory planning. Its purpose is to use historical information to guide current planning, while combining current motion characteristic constraints to quickly filter out potential high-quality trajectories, further optimizing search efficiency. Its function is to provide a priority ranking basis for subsequent trajectory searches, enabling the algorithm to focus on the region near the optimal solution and reduce unnecessary global traversal.

[0065] S3: Based on the cost evaluation results, iteratively search from the initial state point: calculate the trajectory cost of neighboring state points and update the search path along the gradient direction until the optimal feasible trajectory is output, such as... Figure 9 As shown.

[0066] Gradient information is constructed using cost evaluation results, and the algorithm iteratively optimizes towards the optimal solution, similar to gradient descent in optimization algorithms. Each iteration performs a local search based on the neighborhood information of the current optimal solution. The aim is to achieve efficient target trajectory search while ensuring solution quality and avoiding the high computational complexity of global traversal. Its function is to quickly converge to the optimal feasible trajectory through local neighborhood search and gradient update mechanisms, ensuring the algorithm's real-time performance and efficiency.

[0067] Trajectory cost refers to the cost of generating a complete trajectory for each neighboring state point, derived by comprehensively evaluating the static cost and heuristic cost of the trajectory. Gradient direction is the search direction determined by comparing the set of neighboring trajectory costs with the current optimal trajectory cost, representing the update direction of the current optimal solution. Iterative optimization search refers to the process of repeatedly executing neighboring state point generation, trajectory cost calculation, and gradient direction update to gradually approach the optimal solution.

[0068] In summary, the pre-collision detection sampling step effectively eliminates a large number of invalid sampling points located in obstacle areas, retaining only valid sampling points in free space. This step significantly reduces the computational load of subsequent cost evaluation and trajectory search, improving the overall efficiency of the algorithm. Then, in the cost evaluation step, a reasonable priority ranking is assigned to each valid sampling point by combining heuristic cost based on historical planned trajectory information and static cost based on motion features. This allows the algorithm to focus on potentially high-quality trajectory regions, avoiding unnecessary computation on low-priority sampling points and further optimizing search efficiency. Finally, in the iterative trajectory exploration and gradient search steps, gradient information constructed based on the cost evaluation results guides the algorithm to gradually converge along the optimal direction. Through local neighborhood search and dynamically adjusting the neighborhood strategy, the optimal feasible trajectory is found quickly and accurately, avoiding the high computational complexity of global traversal.

[0069] Preferably, performing pre-collision detection on the state points to remove points that conflict with obstacles, and obtaining valid sampling points includes:

[0070] Establish a grid map of the planning space, sample state sampling points on the grid map, and project environmental obstacles onto the grid map;

[0071] Invalid sampling points are marked by comparing the spatial relationship between the sampling point grid and the obstacle grid: when the sampling point grid overlaps with the obstacle grid, it is determined to be a collision, and the sampling point grid is marked as an invalid sampling point;

[0072] Invalid sampling points are removed to obtain valid sampling points.

[0073] The grid map of the planning space discretizes the continuous planning space into a grid-like map, facilitating collision detection and sampling point management. The sampling point grid refers to the grid cell containing the state sampling points generated on the grid map, used for spatial relationship comparison with the obstacle grid. The obstacle grid refers to the projected area of ​​environmental obstacles on the grid map, used for collision detection with the sampling point grid. Invalid sampling points are those that overlap with the obstacle grid, i.e., sampling points located within obstacle areas; these points are discarded to obtain valid sampling points. Figure 6 As shown.

[0074] Specifically, by establishing a grid map of the planning space, state sampling points are sampled on the grid map, environmental obstacles are projected onto the grid map, and invalid sampling points located in obstacle areas are identified and eliminated using spatial relationship comparison. The aim is to remove invalid data in the early stages through an efficient collision detection mechanism, thereby reducing the complexity of subsequent calculations and improving the overall efficiency and feasibility of the algorithm. This provides a clean and effective set of sampling points for subsequent cost evaluation and trajectory search, ensuring that computational resources are concentrated in reasonable trajectory candidate areas, avoiding unnecessary calculations for invalid sampling points, and improving the algorithm's real-time performance and accuracy. One feasible implementation is to generate uniformly distributed state points in the global planning space, establish a grid map and project obstacles, then mark invalid sampling points by comparing the grid relationship between sampling points and obstacles, and finally eliminate these points to obtain valid sampling points.

[0075] Preferably, the heuristic cost assessment includes:

[0076] Using the end sampling points of historical planning trajectories as reference benchmarks, the end sampling points of trajectories are preferentially selected within the neighborhood of the reference benchmarks in the current planning cycle.

[0077] The heuristic cost of the current sampling point is determined by calculating the deviation between the current sampling point state and the state of the final sampling point of the historical planned trajectory.

[0078] The historical planning trajectory refers to trajectory data generated in the previous planning cycle, including the trajectory's terminal state point and velocity characteristics, used to construct a heuristic evaluation model. The terminal sampling point is the last sampling point of the historical planning trajectory, serving as a reference benchmark for the current planning cycle and used to determine the priority search neighborhood. The neighborhood refers to the region defined in the state space centered on the terminal sampling point according to certain rules, used to filter candidate sampling points in the current planning cycle. The state deviation refers to the degree of difference between the current sampling point's state and the state of the terminal sampling point of the historical planning trajectory, typically calculated using Euclidean distance or other similarity metrics. The heuristic cost is the priority evaluation value of the current sampling point determined based on the state deviation, used to guide the trajectory search direction.

[0079] By employing heuristic cost evaluation, historical planning information and current motion characteristics are fully utilized to assign reasonable priority rankings to sampling points. This enables the algorithm to quickly focus on potentially high-quality trajectory regions, reducing the computational cost of global traversal and further improving the algorithm's search efficiency and trajectory quality. It also provides effective guidance for subsequent iterative searches. This heuristic guidance mechanism based on historical information ensures the algorithm's adaptability and robustness in complex environments, while simultaneously improving the real-time performance and accuracy of trajectory planning.

[0080] Preferably, the static cost assessment includes:

[0081] Calculate the cost value of each valid sampling point in the horizontal offset dimension, velocity dimension, and time dimension to form the static cost component of each valid sampling point.

[0082] By calculating costs from multiple dimensions, the basic motion characteristic constraints of each valid sampling point in trajectory planning are comprehensively evaluated. The aim is to quantify the feasibility and merits of sampling points from different perspectives, providing fundamental data support for subsequent comprehensive cost calculations. This ensures that the generated trajectory meets planning requirements in terms of lateral stability, velocity rationality, and time feasibility, thereby improving the quality and adaptability of the trajectory.

[0083] The lateral offset dimension cost refers to the degree to which a sampling point deviates from the reference path in the lateral direction, reflecting the stability requirements of the trajectory in the lateral direction, and is usually measured by the magnitude of the lateral offset. The velocity dimension cost refers to the degree of matching between the velocity value corresponding to the sampling point and the target velocity or safe velocity range, reflecting the rationality of the trajectory in terms of velocity, and is usually evaluated by velocity deviation or acceleration change. The time dimension cost refers to the degree of matching between the time parameter corresponding to the sampling point and the task time window, reflecting the temporal feasibility of the trajectory, and is usually measured by time urgency or time margin. The static cost component integrates the costs of the above three dimensions to form an overall evaluation result for each sampling point in terms of basic motion characteristics, which is used for subsequent comprehensive cost calculation.

[0084] Furthermore, the cost evaluation of the effective sampling points also includes:

[0085] The heuristic cost value of each valid sampling point is weighted and combined with the static cost component to obtain the final cost value of the valid sampling point.

[0086] Specifically, by comprehensively considering heuristic costs and static costs, a weighted combination is used to generate the final cost value for each sampling point, thus comprehensively reflecting the overall performance of the sampling point under the guidance of historical information and the constraints of current motion characteristics. The aim is to combine the guiding role of historical information with the constraints of the current state to form a comprehensive evaluation index, providing a more accurate priority ranking basis for subsequent trajectory search. Its function is to balance the influence of heuristic costs and static costs through weighted combination, ensuring that the algorithm fully considers current motion characteristics while utilizing historical information, thereby improving the accuracy and adaptability of trajectory planning.

[0087] Weighted combination is the process of linearly combining heuristic cost and static cost into a final value by assigning different weight coefficients to them. The weight coefficients reflect the relative importance of heuristic cost and static cost in the overall evaluation and are typically adjusted based on specific application scenarios and requirements. The final value is the result of the weighted combination and is used to rank and filter sampled points, determining their priority in trajectory search.

[0088] One implementation involves dynamically adjusting the weighting coefficients, changing the weights of heuristic and static costs in real time based on factors such as environmental complexity, obstacle density, or the reference value of historical trajectories. For example, in complex and changing environments, the weight of static costs is increased to emphasize the constraints of the current state; in relatively stable environments, the weight of heuristic costs is increased to fully utilize historical information. This enhances the algorithm's adaptability, ensuring optimal overall evaluation results are obtained in different scenarios.

[0089] Preferably, the method further includes sorting the cost evaluation results of the effective sampling points and selecting the sampling point with the lowest cost as the initial state point.

[0090] By comprehensively evaluating the heuristic cost and static cost of sampling points, a final generation value is generated. This generation value is then used to sort the sampling points, and the optimal sampling point is selected as the initial state point. This provides a high-quality starting point for subsequent iterative trajectory search, reduces invalid explorations during the search process, and improves the convergence speed and solution quality of the algorithm. Its role is to guide the algorithm to quickly approach the optimal solution through the reasonable selection of the initial state point, ensuring the efficiency and directionality of the search process.

[0091] Preferably, such as Figure 3 As shown, based on the cost evaluation results, the iterative search starts from the initial state point: calculating the trajectory cost of neighboring state points and updating the search path along the gradient direction until the optimal feasible trajectory is output, including the following steps:

[0092] S10: Using the current optimal trajectory cost as the search benchmark, calculate the set of neighborhood state points of the current optimal trajectory's end state point in the current state space;

[0093] Based on the current optimal solution, we explore potential better solutions in its neighborhood, similar to the neighborhood exploration strategy in local search algorithms. By searching for better solutions in the surrounding area of ​​the current optimal solution, we gradually approach the global optimal solution.

[0094] S20: Generate the corresponding trajectory for each neighborhood state point and calculate the true cost value to obtain the neighborhood trajectory cost set;

[0095] By generating actual trajectories and comprehensively evaluating static and heuristic costs, the true cost value of the trajectory corresponding to each neighborhood state point is obtained, providing data support for subsequent gradient direction determination.

[0096] S30: Determine the gradient search direction based on the comparison between the neighborhood trajectory cost set and the current optimal trajectory cost, and update the search trajectory along the gradient direction;

[0097] By comparing the cost of the neighborhood trajectory with the current optimal cost, the direction of the fastest cost decrease is found as the gradient direction, which guides the update of the search trajectory and moves the search process toward a better solution.

[0098] If the updated search path has not been explored, repeat steps S10-S30;

[0099] If the updated search trajectory has already been explored, then constraint satisfaction detection and collision detection are performed on the updated search trajectory:

[0100] If the constraint satisfaction test and collision detection pass, the output is the optimal feasible trajectory;

[0101] If the constraint satisfaction test and collision test fail, proceed to steps S10-S30.

[0102] The search baseline refers to the cost of the current optimal trajectory, used to compare with the cost set of neighboring trajectories to determine the gradient search direction. The neighboring state point set refers to the set of nearby state points generated centered on the end state point of the current optimal trajectory within the current state space, used to expand the search space. The true cost value refers to the actual cost of the complete trajectory generated for each neighboring state point, derived by comprehensively evaluating the static cost and heuristic cost of the trajectory. The gradient search direction refers to the search direction determined by comparing the cost set of neighboring trajectories with the cost of the current optimal trajectory, representing the update direction of the current optimal solution. Constraint satisfaction detection is the process of verifying whether the trajectory satisfies all kinematic and dynamic constraints. Collision detection is the process of verifying whether the trajectory collides with environmental obstacles. The optimal feasible trajectory is the trajectory that satisfies all constraints and has the minimum cost value, serving as the final output of the algorithm.

[0103] Step S10 uses the cost of the current optimal trajectory as the search benchmark, ensuring that each iteration optimizes based on the current optimal solution and avoids blind searching. Step S20 generates the corresponding trajectories of neighboring state points and calculates the true cost, providing detailed local information and data support for subsequent gradient direction determination. Step S30 compares the cost set of neighboring trajectories with the cost of the current optimal trajectory to determine the gradient search direction and update the search trajectory, ensuring that the search process moves towards a better solution. By repeatedly executing steps S10-S30, the algorithm continuously approaches the optimal solution until it finds the optimal feasible trajectory that satisfies the constraints. Combining constraint satisfaction detection and collision detection further ensures the feasibility and safety of the final trajectory. This iterative optimization and verification mechanism ensures the robustness and reliability of the algorithm in complex environments, avoids getting trapped in local optima, and ultimately outputs the optimal feasible trajectory that meets all requirements. Combining pre-collision detection sampling, cost evaluation, and initial state point selection optimizations, efficient trajectory planning is achieved on low-computing-power platforms, ensuring the feasibility and practicality of the algorithm in real-world applications.

[0104] A fast evaluation and search system for optimal trajectories, such as Figure 2 As shown, the fast evaluation search system is applied to the fast evaluation search method for the optimal trajectory as described above. The fast evaluation search system includes:

[0105] The sampling module is used to sample multiple state points in a pre-planned space and perform pre-collision detection on the state points to remove points that conflict with obstacles, thereby obtaining valid sampling points;

[0106] An evaluation module is used to evaluate the cost of the effective sampling points. The cost evaluation includes heuristic cost based on historical planning trajectory information and static cost based on motion characteristics.

[0107] The search module is used to iteratively search from the initial state point based on the result of the cost evaluation: calculate the trajectory cost of the neighboring state points and update the search path along the gradient direction until the optimal feasible trajectory is output.

[0108] This embodiment implements a rapid evaluation and search method and process for the optimal trajectory. Please refer to the above embodiments for details, which will not be repeated here.

[0109] Furthermore, embodiments of the present invention also propose a terminal device, the terminal device including a memory, a processor, and a program stored in the memory and executable on the processor, the program being configured to implement the steps of the fast evaluation search method as described above.

[0110] Since the program is configured to implement the steps of the rapid evaluation search method as described above, the program has at least all the beneficial effects brought about by all the technical solutions of all the foregoing embodiments, which will not be repeated here.

[0111] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0112] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A fast evaluation and search method for optimal trajectories, characterized in that, The rapid evaluation search method includes: Multiple state points are sampled in the pre-planned space, and pre-collision detection is performed on the state points to remove points that conflict with obstacles, thereby obtaining valid sampling points; Cost evaluation is performed on the effective sampling points, including heuristic cost based on historical planning trajectory information and static cost based on motion characteristics; Based on the cost evaluation results, an iterative search is performed starting from the initial state point: the trajectory cost of the neighboring state points is calculated and the search path is updated along the gradient direction until the optimal feasible trajectory is output. Perform pre-collision detection on the state points to remove points that conflict with obstacles, and obtain valid sampling points including: Establish a grid map of the planning space, sample state sampling points on the grid map, and project environmental obstacles onto the grid map; Invalid sampling points are marked by comparing the spatial relationship between the sampling point grid and the obstacle grid: when the sampling point grid overlaps with the obstacle grid, it is determined to be a collision, and the sampling point grid is marked as an invalid sampling point; Invalid sampling points are removed to obtain valid sampling points; Heuristic cost assessment includes: Using the end sampling points of historical planning trajectories as reference benchmarks, the end sampling points of trajectories are preferentially selected within the neighborhood of the reference benchmarks in the current planning cycle. The heuristic cost of the current sampling point is determined by calculating the deviation between the current sampling point state and the state of the end sampling point of the historical planned trajectory. Static cost assessment includes: Calculate the cost of each valid sampling point in the horizontal offset dimension, velocity dimension, and time dimension to form the static cost component of each valid sampling point; Cost evaluation of the effective sampling points also includes: The heuristic cost value of each valid sampling point is weighted and combined with the static cost component to obtain the final cost value of the valid sampling point. It also includes sorting the cost evaluation results of effective sampling points and selecting the sampling point with the lowest cost as the initial state point; Based on the cost evaluation results, an iterative search is performed starting from the initial state point: calculating the trajectory cost of neighboring state points and updating the search path along the gradient direction until the optimal feasible trajectory is output, including the following steps: S10: Using the current optimal trajectory cost as the search benchmark, calculate the set of neighborhood state points of the current optimal trajectory's end state point in the current state space; S20: Generate the corresponding trajectory for each neighborhood state point and calculate the true cost value to obtain the neighborhood trajectory cost set; S30: Determine the gradient search direction based on the comparison between the neighborhood trajectory cost set and the current optimal trajectory cost, and update the search trajectory along the gradient direction; If the updated search path has not been explored, repeat steps S10-S30; If the updated search trajectory has already been explored, then constraint satisfaction detection and collision detection are performed on the updated search trajectory: If the constraint satisfaction test and collision detection pass, the output is the optimal feasible trajectory; If the constraint satisfaction test and collision test fail, proceed to steps S10-S30.

2. A fast evaluation and search system for optimal trajectories, characterized in that, The fast evaluation search system is applied to the fast evaluation search method for the optimal trajectory of claim 1, and the fast evaluation search system includes: The sampling module is used to sample multiple state points in a pre-planned space and perform pre-collision detection on the state points to remove points that conflict with obstacles, thereby obtaining valid sampling points; An evaluation module is used to evaluate the cost of the effective sampling points. The cost evaluation includes heuristic cost based on historical planning trajectory information and static cost based on motion characteristics. The search module is used to iteratively search from the initial state point based on the result of the cost evaluation: calculate the trajectory cost of the neighboring state points and update the search path along the gradient direction until the optimal feasible trajectory is output.

3. A terminal device, characterized in that, The terminal device includes: a memory, a processor, and a program stored in the memory and executable on the processor, the program being configured to implement the steps of the fast evaluation search method as claimed in claim 1.

Images