CN-120685110-B - Method, system and terminal equipment for quickly evaluating and searching optimal track

CN120685110BCN 120685110 BCN120685110 BCN 120685110BCN-120685110-B

Abstract

The invention relates to the technical field of track planning and provides a rapid evaluation search method, a rapid evaluation search system and terminal equipment of an optimal track, wherein the rapid evaluation search method comprises the steps of sampling a plurality of state points in a pre-planning space, and executing pre-collision detection on the state points to remove points in conflict with obstacles so as to obtain effective sampling points; and iteratively searching from an initial state point based on the result of the cost evaluation, namely calculating the track cost of a neighborhood state point and updating a search path along a gradient direction until the optimal feasible track is output. According to the method, invalid sampling points and a gradient iterative search mechanism are screened out through pre-collision detection, so that the track search efficiency is improved.

Inventors

ZHANG CHENGTAO
He Yaojiang
LUO WEIFEN
ZHENG WEIGUANG
CUI SHUWAN
ZHANG YANHUI
GAO FEI
PAN WENFANG
Chen Junao
WANG HAO

Assignees

广西科技大学

Dates

Publication Date: 20260508
Application Date: 20250617

Claims (3)

1. The rapid evaluation search method for the optimal track is characterized by comprising the following steps of: sampling a plurality of state points in a pre-planning space, and performing pre-collision detection on the state points to remove points conflicting with obstacles, so as to obtain effective sampling points; Performing cost evaluation on the effective sampling points, wherein the cost evaluation comprises heuristic cost based on historical planning track information and static cost based on motion characteristics; calculating track cost of the neighborhood state point and updating a search path along the gradient direction until the optimal feasible track is output; Performing pre-crash detection on the status points to remove points that collide with obstacles, obtaining valid sampling points includes: Establishing a grid map of a planning space, sampling state sampling points on the grid map, and projecting environmental barriers to the grid map; marking invalid sampling points by comparing the spatial relationship between the sampling point grids and the barrier grids, wherein when the sampling point grids and the barrier grids overlap, collision is judged, and the sampling point grids are regarded as invalid sampling points; Removing invalid sampling points to obtain valid sampling points; Heuristic cost evaluation includes: taking the tail end sampling point of the historical planning track as a reference standard, and preferentially selecting the tail end sampling point of the track in the neighborhood range of the reference standard in the current planning period; Determining the heuristic cost value of the current sampling point by calculating the state deviation of the current sampling point state and the state deviation of the tail end sampling point of the historical planning track; static cost assessment includes: Calculating the cost value of each effective sampling point in the transverse offset dimension, the speed dimension and the time dimension to form a static cost component of each effective sampling point; The cost evaluation of the effective sampling points further comprises: The heuristic cost value of each effective sampling point is weighted and combined with the static cost component to obtain the final cost value of the effective sampling point; The method further comprises the steps of sorting cost evaluation results of the effective sampling points, and selecting the sampling point with the minimum cost as an initial state point; based on the result of the cost evaluation, iteratively searching from the initial state point, calculating the track cost of the neighborhood state point and updating the search path along the gradient direction until the optimal feasible track is output, wherein the method comprises the following steps: S10, taking a current optimal track cost value as a search reference, and calculating a neighborhood state point set of the current optimal track tail end state point in a current state space; S20, generating a corresponding track of each neighborhood state point and calculating a real cost value to obtain a neighborhood track cost set; S30, determining a gradient search direction based on comparison between the neighborhood track cost set and the current optimal track cost, and updating the search track along the gradient direction; If the updated search track is not explored, repeating the steps S10-S30; if the updated search track has been explored, performing constraint satisfaction detection and collision detection on the updated search track: If the constraint satisfaction detection and the collision detection are passed, outputting an optimal feasible track; if the constraint satisfaction detection and the collision detection are not passed, steps S10 to S30 are performed.
2. A rapid evaluation search system of an optimal trajectory, wherein the rapid evaluation search system is applied to the rapid evaluation search method of an optimal trajectory of claim 1, the rapid evaluation search system comprising: the sampling module is used for sampling a plurality of state points in a pre-planning space, and performing pre-collision detection on the state points to remove points conflicting with obstacles so as to obtain effective sampling points; the evaluation module is used for evaluating the cost of the effective sampling points, and the cost evaluation comprises heuristic cost based on historical planning track information and static cost based on motion characteristics; And the searching module is used for iteratively searching from the initial state point based on the result of the cost evaluation, calculating the track cost of the neighborhood state point and updating the searching path along the gradient direction until the optimal feasible track is output.
3. A terminal device comprising a memory, a processor, and a program stored on the memory and executable on the processor, the program configured to implement the steps of the rapid assessment search method of claim 1.

Description

Method, system and terminal equipment for quickly evaluating and searching optimal track Technical Field The invention relates to the technical field of track planning, in particular to a method, a system and terminal equipment for quickly evaluating and searching an optimal track. Background With the continued development of low speed autopilot applications such as delivery, patrol and road sweeper, robotics are increasingly playing a role in social services. However, these vehicles have problems with limited computing resources due to size and power constraints of the computing platform they are equipped with. In particular, the vehicle-mounted sensing module is heavy, and the space left for the computing resources of the planning module is very limited, so that many trajectory planning algorithms are difficult to meet the real-time requirement and difficult to deploy in a practical scene. At present, track planning based on sampling is widely applied to mobile robot communities. However, for most of these algorithms, there is a trade-off between the number of track samples and the planning frequency, and in recent research work the problem of track planning under a resource constrained computer has not been solved well. The prior method has the following defects: When the scale of the candidate track set is large, the traversal type detection method brings significant calculation burden, so that the time consumption of collision detection is increased sharply, and the real-time requirement in practical application is difficult to meet. The traditional scheme generally needs to globally traverse the whole track set and execute complex computation of multi-dimensional evaluation indexes on each track, so that the computation complexity is increased sharply, the demand on computation resources is further improved greatly, and the real-time requirement is difficult to meet. Disclosure of Invention Aiming at the defects, the invention aims to provide a rapid evaluation search method, a rapid evaluation search system and terminal equipment for an optimal track, aiming at improving the search efficiency of the track by screening invalid sampling points and a gradient iterative search mechanism through pre-collision detection. To achieve the purpose, the invention adopts the following technical scheme: a rapid evaluation search method of an optimal trajectory, the rapid evaluation search method comprising: sampling a plurality of state points in a pre-planning space, and performing pre-collision detection on the state points to remove points conflicting with obstacles, so as to obtain effective sampling points; Performing cost evaluation on the effective sampling points, wherein the cost evaluation comprises heuristic cost based on historical planning track information and static cost based on motion characteristics; and based on the result of the cost evaluation, iteratively searching from the initial state point, namely calculating the track cost of the neighborhood state point and updating the searching path along the gradient direction until the optimal feasible track is output. Preferably, performing pre-collision detection on the status points to remove points that collide with obstacles, obtaining valid sampling points includes: Establishing a grid map of a planning space, sampling state sampling points on the grid map, and projecting environmental barriers to the grid map; marking invalid sampling points by comparing the spatial relationship between the sampling point grids and the barrier grids, wherein when the sampling point grids and the barrier grids overlap, collision is judged, and the sampling point grids are regarded as invalid sampling points; And removing the invalid sampling points to obtain valid sampling points. Preferably, the heuristic cost evaluation comprises: taking the tail end sampling point of the historical planning track as a reference standard, and preferentially selecting the tail end sampling point of the track in the neighborhood range of the reference standard in the current planning period; and determining the heuristic cost value of the current sampling point by calculating the state deviation of the current sampling point state and the end sampling point of the historical planning track. Further, static cost evaluation includes: And calculating the cost value of each effective sampling point in the transverse offset dimension, the speed dimension and the time dimension to form a static cost component of each effective sampling point. Further, performing cost evaluation on the valid sampling points further includes: and carrying out weighted combination on the heuristic cost value and the static cost component of each effective sampling point to obtain the final cost value of the effective sampling point. Preferably, the method further comprises the step of sorting cost evaluation results of the effective sampling points, and selecting the sampling point with the minimum co