Robot path planning method and system based on search direction cost and heuristic function optimization

Abstract

The invention relates to the technical field of automatic control, and provides a robot path planning method and system based on search direction cost and heuristic function optimization. In the method, a far-end neighborhood of a current position of a robot is abandoned, and a prediction path between the current position of the robot and a target terminal point is planned based on a near-end neighborhood of the current position of the robot; when an obstacle exists on a predicted path between the current position of the robot and a target end point, determining a path turning point of the robot based on an abandoned far-end neighborhood of the current position of the robot, screening a search neighborhood of the robot at the path turning point, and determining a target position of the robot based on the screened search neighborhood at the path turning point; and planning a path from the path turning point of the robot to the target end point, and generating a planned path from the current position of the robot to the target end point. Therefore, the rapid obstacle avoidance capability of the robot is effectively improved, the planning time in the path planning process of the robot is effectively shortened, and the path planning efficiency of the robot is improved.

Description

Technical Field

[0001] The present application relates to the field of automatic control technology, and in particular to a robot path planning method and system based on search direction cost and heuristic function optimization. Background Art

[0002] Path planning and obstacle avoidance are two key functions in the field of robot navigation. Together, they ensure that the robot can move effectively and avoid obstacles, ultimately reaching the navigation target point safely.

[0003] Global path planning is to find a feasible or optimal path from the starting point to the target point that meets certain performance requirements based on the prior environmental model, but it requires prior information about the environment and is computationally intensive. Local path planning, on the other hand, focuses on the robot's current local environmental information, and its information acquisition mainly relies on sensors, which changes in real time as the environment changes.

[0004] Most global path planning algorithms are designed for structured environments, such as visibility graphs and unit decomposition. With the advancement of technological research, continuously improved algorithms such as neural network algorithms, ant colony algorithms, and genetic algorithms have been proposed. Furthermore, heuristic function-based algorithms such as the A* algorithm, simulated annealing, and potential field methods have emerged. However, traditional A* global path planning algorithms and traditional dynamic window approaches (DWA) suffer from slow robot path planning speed, inflexible control, and poor robustness. Summary of the Invention

[0005] The purpose of this application is to provide a robot path planning method and system based on search direction cost and heuristic function optimization to solve or alleviate the problems existing in the above-mentioned prior art.

[0006] In order to achieve the above objectives, this application provides the following technical solutions:

[0007] This application provides a robot path planning method based on search direction cost and heuristic function optimization, including:

[0008] Discard the distal neighborhood of the robot's current position and plan the predicted path from the robot's current position to the target destination based on the proximal neighborhood of the robot's current position;

[0009] In response to an obstacle existing on a predicted path between the robot's current position and a target destination, determining a turning point in the path of the robot based on a distal neighborhood discarded by the robot's current position;

[0010] The robot's search neighborhood at the path turning point is screened out, and based on the screened search neighborhood at the path turning point, the robot's path from the path turning point to the target end point is planned to generate a planned path from the robot's current position to the target end point.

[0011] Preferably, the distal neighborhood of the robot's current position is discarded according to the heading angle of the robot's current position, and the proximal neighborhood of the robot's current position is used as the search neighborhood to plan a predicted path between the robot's current position and the target end point.

[0012] Preferably, the robot's rasterized environment map is marked for accessibility, and state information of each grid in the rasterized environment map is generated;

[0013] In response to a change in state information of a target grid on a predicted path between the current position of the robot and the target end point, it is determined that an obstacle exists on the predicted path between the current position of the robot and the target end point.

[0014] Preferably, in response to the presence of an obstacle on the predicted path between the robot's current position and the target end point, based on global path planning, a search is performed on the remote neighborhood abandoned by the robot's current position to determine a turning point in the robot's path.

[0015] Preferably, the estimated cost of moving the robot from the current position to the node to be explored in the remote neighborhood and the actual cost of moving the robot from the starting point to the current position are calculated by a heuristic function;

[0016] Add the expected cost value and the actual cost value to get the total cost from the robot's starting point to the node to be explored;

[0017] The node to be explored with the smallest total cost in the open list of nodes to be explored by the robot is selected as the turning point of the robot's path.

[0018] Preferably, starting from the turning point of the path, backtracking is performed from the parent node to the current position of the robot, generating a discrete coordinate chain connected in reverse order from the turning point of the path to the current position;

[0019] The storage order of the reverse-series discrete coordinate chain is reversed to obtain a sequence of path points arranged in sequence from the current position to the path turning point, so as to generate a path from the current position to the path turning point.

[0020] Preferably, according to the heuristic function:

[0021]

[0022] Evaluate the planned path from the robot's current position to the target end point; where G is the actual cost from the robot's current position to the next target node, H is the estimated cost from the robot's current position to the target end point, and m is the robot's mass; d is the Euclidean distance between the robot's current position and the target end point, D is the Euclidean distance between the robot's starting point and the target end point, and ω is the robot's search direction evaluation function dif α The weight of

[0023] When evaluating the path segment between the robot's current position and the path turning point in the planned path, dif α = 0; when evaluating the path segment from the turning point to the target end point in the planned path, dif α =1.

[0024] Preferably, the heading angle of the vector between the path turning point and the target node in the robot map coordinate system is calculated, and the search neighborhood of the robot at the path turning point is screened according to the heading angle.

[0025] Preferably, the search neighborhood is a neighborhood within a range of [-90°, 90°] on both sides of the bisector of the heading angle of the vector between the path turning point and the target node in the robot map coordinate system.

[0026] The present application also provides a robot path planning system based on search direction cost and heuristic function optimization, including:

[0027] a path prediction unit configured to discard a distal neighborhood of the robot's current position and plan a predicted path between the robot's current position and a target destination based on a proximal neighborhood of the robot's current position;

[0028] an obstacle avoidance unit, in response to an obstacle on the predicted path between the robot's current position and the target end point, determining a turning point in the robot's path based on a distal neighborhood discarded by the robot's current position;

[0029] The path adjustment unit is configured to screen out the search neighborhood of the robot at the path turning point, and plan the path from the path turning point to the target end point of the robot based on the screened search neighborhood at the path turning point, and generate a planned path from the current position of the robot to the target end point.

[0030] Beneficial effects:

[0031] The robot path planning method based on search direction cost and heuristic function optimization provided in the embodiment of the present application discards the distal neighborhood of the robot's current position and plans the predicted path between the robot's current position and the target end point based on the proximal neighborhood of the robot's current position; when there is an obstacle on the predicted path between the robot's current position and the target end point, the turning point of the robot's path is determined based on the distal neighborhood discarded by the robot's current position, and after screening the search neighborhood of the robot at the path turning point, the path from the robot's path turning point to the target end point is planned based on the search neighborhood screened out at the path turning point, thereby generating a planned path from the robot's current position to the target end point.

[0032] Therefore, by discarding the distal neighborhood of the robot's current position, the robot's predicted path is directionally improved, effectively reducing the global path planning time between the robot's current position and the target end point, greatly improving the robot's foresight in global path planning; only when there are obstacles on the predicted path between the robot's current position and the target end point, making it impossible for the robot to reach the target end point according to the predicted path, a directed search is performed based on the distal neighborhood discarded by the robot's current position to determine the turning point of the robot's path, effectively improving the robot's ability to quickly avoid obstacles in complex obstacle environments, and reducing the planning time in the robot's path planning process, thereby improving the robot's path planning efficiency; after determining the turning point of the path, the robot's path is directionally improved again based on the search neighborhood screened out at the path turning point, planning the path between the robot's path turning point and the target end point, realizing the robot's directional search in the path planning process, not only effectively improving the robot's ability to quickly avoid obstacles, but also effectively reducing the planning time in the robot's path planning process, improving the robot's path planning efficiency, improving the applicability and robustness of the navigation function in the ROS system in the robot, and realizing the robot's fast path planning and obstacle avoidance functions. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] The drawings and descriptions that constitute part of this application are intended to provide a further understanding of this application. The exemplary embodiments and descriptions of this application are intended to explain this application and do not constitute an improper limitation on this application. Among them:

[0034] Figure 1 A schematic flow chart of a robot path planning method based on search direction cost and heuristic function optimization according to some embodiments of the present application;

[0035] Figure 2 A schematic diagram of path planning for a robot using a traditional A* algorithm according to some embodiments of the present application;

[0036] Figure 3A schematic diagram of an improved path planning for a robot with search neighborhood orientation according to some embodiments of the present application;

[0037] Figure 4 A schematic diagram of a path planning of a robot after heuristic function optimization according to some embodiments of the present application;

[0038] Figure 5 A schematic structural diagram of a robot path planning system based on search direction cost and heuristic function optimization according to some embodiments of the present application. DETAILED DESCRIPTION

[0039] The present application will be described in detail below with reference to the accompanying drawings and in conjunction with embodiments. Each example is provided by way of explanation of the present application and does not limit the present application. In fact, it will be clear to those skilled in the art that modifications and variations can be made in the present application without departing from the scope or spirit of the present application. For example, a feature shown or described as part of one embodiment can be used in another embodiment to produce yet another embodiment. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present invention should fall within the scope of protection of the embodiments of the present invention.

[0040] In robot path planning, the A* path planning algorithm based on grid composition is currently the most widely used. This algorithm has been applied and optimized in various scenarios, resulting in numerous improved A* algorithms. For example, a bidirectional time-efficient improved A* algorithm is used to obtain the optimal path, and a variable-step-size bidirectional A* algorithm based on quadtree grid environment modeling with third-order Bezier curve trajectory optimization is used. Research has found that improving the A* algorithm by optimizing search directions and improving heuristic functions based on eight search directions and cost estimates based on Euclidean distance can reduce time complexity and improve efficiency.

[0041] To ensure that the firefighting robot can plan a route to the warning area more quickly after receiving a fire warning from a fixed camera, and can avoid previously unknown obstacles during navigation, the present embodiment proposes a robot path planning method based on search direction cost and heuristic function optimization. By screening out the 8-search direction A* algorithm and correcting it to a 5-search direction A* algorithm, the number of grids traversed by the robot path search is effectively reduced, thereby improving the path search efficiency.

[0042] At the same time, in special map scenarios (where there are obstacles between the robot's current node and the target node, such as a "C"-shaped map scenario), in order to avoid the 5-search direction A* algorithm falling into a directional dead zone after screening and being unable to plan a valid path, the robot's direction optimization strategy is improved by adding the search direction restriction as a cost to the heuristic of the A* algorithm, ensuring that the 5-search direction A* algorithm can reduce the search of redundant grids through directional guidance without falling into a directional dead zone.

[0043] like Figures 1 to 4 As shown in FIG, the robot path planning method based on search direction cost and heuristic function optimization includes:

[0044] Step S101: discard the distal neighborhood of the robot's current position, and plan a predicted path from the robot's current position to the target destination based on the proximal neighborhood of the robot's current position.

[0045] In the robot map coordinate system, the robot is always guided from its current position (i.e., the current node) toward the next node on the planned path, meaning the robot always moves toward the target node. In robot path planning, the neighborhood in the direction of the robot's motion is defined as the proximal neighborhood of the current position, while the neighborhood away from the direction of the robot's motion is defined as the distal neighborhood of the current position.

[0046] When there are no obstacles on the robot's global path planning path, the robot always advances toward the proximal neighborhood of its current position. At this point, the distal neighborhood of the robot's current position is discarded, and the robot's predicted path is directionally improved. In other words, based on the robot's current heading angle, the distal neighborhood of the robot's current position is discarded, and the proximal neighborhood of the robot's single-signed position is used as the search neighborhood. Only the proximal neighborhood of the robot's current position is explored and updated to plan the predicted path between the robot's current position and the target destination. This effectively reduces the global path planning time between the robot's current position and the target destination, greatly improving the robot's forward-looking performance during global path planning.

[0047] In this application, the A* algorithm can be combined to generate a predicted path between the robot's current position and the target destination. Specifically, based on the traditional eight-search direction A* algorithm, the exploration direction of the target node is divided into eight search areas around the robot's current position. The search area that deviates from the robot's movement direction is discarded based on the robot's heading angle. The neighboring nodes facing the robot's movement direction are added to the robot's open list, and the domain nodes in the open list are explored and updated to complete the path prediction between the robot's current position and the target destination.

[0048] Step S102: In response to an obstacle existing on the predicted path between the current position of the robot and the target end point, a turning point of the robot's path is determined based on a distal neighborhood discarded by the current position of the robot.

[0049] When there is an obstacle on the predicted path between the robot's current position and the target end point, the robot cannot reach the target end point according to the predicted path, and needs to re-plan the local path between the current position and the obstacle to avoid the obstacle. In the present application, when there is an obstacle on the predicted path between the robot's current position and the target end point, based on a global path planning algorithm (such as an A* algorithm), the far-end neighborhood abandoned by the robot's current position is searched to determine the robot's path turning point. In other words, when the robot cannot reach the target end point according to the predicted path, only the far-end neighborhood of the robot's current position is explored and updated, the local path between the robot's current position and the obstacle is directed and improved, the path planning time from the robot's current position to the obstacle is reduced, and the path of travel from the robot's current position to the obstacle is planned to avoid the obstacle.

[0050] To determine whether there are obstacles on the robot's predicted path, the robot's gridded environment map is first marked for traversability, generating state information for each grid in the gridded environment map. The robot's sensors scan the structured work scene and divide it into grids, marking grids with obstacles as "1" and those without obstacles as "0."

[0051] As the robot moves, the structured environment map changes in real time. The robot's sensors scan the map in real time, updating the changing grid states in real time. For example, when a moving obstacle appears in the robot's structured environment, the grid where the obstacle is located changes in real time, and the corresponding grid information also changes. When the obstacle moves onto the predicted path from the robot's current position to the target destination, the corresponding grid state information on the predicted path changes from "0" to "1," confirming that an obstacle exists on the robot's predicted path at that moment.

[0052] When an obstacle appears on the robot's predicted path, the robot must replan the local path between its current position and the obstacle to avoid it. Specifically, if the robot cannot reach the target destination according to the predicted path, it will make targeted improvements to the local path between the robot's current position and the obstacle. It will only explore and update the remote neighborhood of the current position, determine turning points in the robot's path, and then update the robot's local path to avoid the obstacle.

[0053] Among them, when determining the turning point of the robot's path, first, the estimated cost value from the robot's current position to the to-be-explored node in the remote neighborhood and the actual cost value of the robot moving from the starting point to the current position are calculated through the heuristic function; then, the calculated estimated cost value from the robot's current position to the to-be-explored node in the remote neighborhood and the actual cost value of the robot moving from the starting point to the current position are added together to obtain the total cost from the robot's starting point to the to-be-explored node; finally, the to-be-explored node with the smallest total cost in the open list constructed by the robot's to-be-explored nodes is selected as the robot's path turning point.

[0054] It should be noted that during the robot's node exploration and update process, multiple unexplored nodes constitute the robot's open list for exploration, and the explored nodes constitute the robot's closed list for exploration. In the open list, after the unexplored node is explored, it is removed from the open list and added to the closed list.

[0055] In this application, according to the formula:

[0056]

[0057] Determine the estimated cost H of the robot's current position (i.e., current node Y) to the node to be explored in the remote neighborhood, and the actual cost G of the robot moving from the starting point to the current position. The coordinates of the node to be explored X in N-dimensional space are X = (x1,…,x i ,…,x N ), the coordinate of the current node Y in the N-dimensional space is Y=(y1,…,y i ,…,y N ), N is a positive integer.

[0058] After calculating the total cost of each node to be explored in the open list, the distance between the node to be explored corresponding to the minimum total cost and the current position of the robot is the shortest. The exploration node is used as the turning point of the robot's path. Based on the turning point, the travel path between the current position of the robot and the travel path between the turning point and the target end point are planned and adjusted.

[0059] Step S103: filter out the search neighborhood of the robot at the path turning point, and plan the path from the path turning point to the target end point based on the filtered search neighborhood at the path turning point, and generate a planned path from the robot's current position to the target end point.

[0060] After determining the turning point of the robot's path, the path is traced back to the parent nodes one level at a time until the robot reaches its current position. This results in a discrete coordinate chain that is connected in reverse order from the turning point to the robot's current position. In other words, starting from the turning point, the parent node pointer is read and the parent node of the parent node is jumped to one level at a time until the robot's current position is finally reached. This forms a discrete coordinate chain that is connected in reverse order from the turning point to the robot's current position.

[0061] Next, the storage order of the reversed-series discrete coordinate chains is reversed to obtain a sequence of path points arranged in the order from the robot's current position to the path turning point. This sequence of path points, in other words, is arranged in the order of the robot's actual travel. This path point sequence is then used to generate the local path between the robot's current position and the path turning point. Thus, through pointer traversal, chain construction, and sequence reversal, the implicit path structure generated during the search phase is made explicit, effectively ensuring that the robot can directly obtain executable navigation commands, enabling it to travel along the adjusted path and effectively avoid obstacles along the predicted path.

[0062] When determining the path segment from the turning point of the robot's path to the target endpoint, the heading angle of the vector between the turning point of the path and the next target node in the robot's map coordinate system is calculated, and the robot's search neighborhood at the path turning point is screened based on the heading angle to achieve directional improvement of the robot's path planning. Specifically, the neighborhood contained in the range [-90°, 90°] on both sides of the angular bisector of the heading angle of the vector between the turning point of the path to the next target node in the robot's map coordinate system is used as the search neighborhood, and the path segment between the turning point of the robot's path and the target endpoint is gradually explored and updated. In this way, by adjusting the orientation of the path planning between the turning point of the robot's path and the target endpoint, the planning time of the robot's path planning process is effectively reduced, and the robot's path planning efficiency is improved.

[0063] In traditional robot path planning algorithms, such as the A* algorithm, in order to ensure that the optimal path can be searched, the search nodes will be increased because the estimated cost value of the heuristic function is less than the actual cost value, resulting in reduced search efficiency; when the estimated cost value of the heuristic function is greater than the actual cost value, the opposite situation will occur; only when the estimated cost value of the heuristic function is approximately equal to the actual cost value, the robot's search efficiency is the highest. In this application, when the current position of the robot (current node) is far from the target node, the weight of the estimated cost value of the evaluation function is increased to ensure that the algorithm focuses more on search efficiency; when the robot gradually approaches the target node, the weight of the estimated cost value is gradually reduced to ensure that the optimal path can be searched. Therefore, after determining the path segment between the current position of the robot and the path turning point and the path segment from the path turning point to the target end point, according to the heuristic function:

[0064]

[0065] Evaluate the planned path from the robot's current position to the target end point; where G is the actual cost from the robot's current position to the next target node, H is the estimated cost from the robot's current position to the target end point, and m is the robot's mass; d is the Euclidean distance between the robot's current position and the target end point, D is the Euclidean distance between the robot's starting point and the target end point, and ω is the robot's search direction evaluation function dif α When evaluating the path segment between the robot's current position and the path turning point in the planned path, dif α = 0; when evaluating the path segment from the turning point to the target end point in the planned path, dif α =1.

[0066] Therefore, by improving the search neighborhood of the robot's current position and the turning points of the path, the robot can realize directional search in the path planning process, which not only effectively improves the robot's ability to avoid obstacles quickly, but also effectively reduces the planning time in the robot's path planning process, improves the robot's path planning efficiency, and improves the applicability and robustness of the navigation function in the ROS system in the robot.

[0067] The present application also provides a robot path planning system based on search direction cost and heuristic function optimization, such as Figure 5 As shown, the system includes:

[0068] The path prediction unit 501 is configured to discard the distal neighborhood of the robot's current position and plan a predicted path between the robot's current position and a target destination based on the proximal neighborhood of the robot's current position;

[0069] The obstacle avoidance unit 502 determines a turning point in the robot's path based on a distal neighborhood discarded by the robot's current position in response to an obstacle on the predicted path between the robot's current position and the target destination;

[0070] The path adjustment unit 503 is configured to filter out the search neighborhood of the robot at the path turning point, and plan the path from the path turning point to the target end point of the robot based on the filtered search neighborhood at the path turning point, and generate a planned path from the current position of the robot to the target end point.

[0071] The robot path planning system based on search direction cost and heuristic function optimization provided in the embodiments of the present application can implement the steps and processes of the robot path planning method based on search direction cost and heuristic function optimization in any of the above embodiments, and achieve the same technical effects, which will not be repeated here.

[0072] In the description of the present invention, the terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples" mean that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any appropriate manner in any one or more embodiments or examples.

[0073] The foregoing description is merely a preferred embodiment of the present application and is not intended to limit the present application. Various modifications and variations are readily apparent to those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present application shall be included within the scope of protection of the present application.

Claims

1. A robot path planning method based on search direction cost and heuristic function optimization, characterized in that: include: Discard the distal neighborhood of the robot's current position and plan the predicted path from the robot's current position to the target destination based on the proximal neighborhood of the robot's current position; In response to an obstacle existing on a predicted path between the robot's current position and a target destination, determining a turning point in the path of the robot based on a distal neighborhood discarded by the robot's current position; The robot's search neighborhood at the path turning point is screened out, and based on the screened search neighborhood at the path turning point, the robot's path from the path turning point to the target end point is planned to generate a planned path from the robot's current position to the target end point.

2. The method according to claim 1, characterized in that According to the heading angle of the robot's current position, the distal neighborhood of the robot's current position is discarded, and the proximal neighborhood of the robot's current position is used as the search neighborhood to plan the predicted path between the robot's current position and the target end point.

3. The method according to claim 1, characterized in that Mark the robot's rasterized environment map for accessibility and generate status information for each grid in the rasterized environment map; In response to a change in state information of a target grid on a predicted path between the current position of the robot and the target end point, it is determined that an obstacle exists on the predicted path between the current position of the robot and the target end point.

4. The method according to claim 1, wherein In response to the presence of an obstacle on the predicted path between the robot's current position and the target end point, based on global path planning, a search is performed on the remote neighborhood abandoned by the robot's current position to determine the turning point of the robot's path.

5. The method according to claim 4, characterized in that The heuristic function is used to calculate the estimated cost of the robot's current position to the node to be explored in the remote neighborhood, as well as the actual cost of the robot moving from the starting point to the current position; Add the expected cost value and the actual cost value to get the total cost from the robot's starting point to the node to be explored; The node to be explored with the smallest total cost in the open list of nodes to be explored by the robot is selected as the turning point of the robot's path.

6. The method according to claim 1, characterized in that From the turning point of the path, trace back to the parent node level by level until the current position of the robot, and generate a discrete coordinate chain connected in reverse order from the turning point of the path to the current position; The storage order of the reverse-series discrete coordinate chain is reversed to obtain a sequence of path points arranged in sequence from the current position to the path turning point, so as to generate a path from the current position to the path turning point.

7. The method according to claim 1, characterized in that According to the heuristic function: Evaluate the planned path from the robot's current position to the target end point; where G is the actual cost from the robot's current position to the next target node, H is the estimated cost from the robot's current position to the target end point, and m is the robot's mass; d is the Euclidean distance between the robot's current position and the target end point, D is the Euclidean distance between the robot's starting point and the target end point, and ω is the robot's search direction evaluation function dif α The weight of When evaluating the path segment between the robot's current position and the path turning point in the planned path, dif α = 0; when evaluating the path segment from the turning point to the target end point in the planned path, dif α =1.

8. The method according to claim 1, characterized in that Calculate the heading angle of the vector between the turning point of the path and the target node in the robot map coordinate system, and filter the robot's search neighborhood at the turning point of the path based on the heading angle.

9. The method according to claim 8, characterized in that The search neighborhood is the area within the range [-90°, 90°] on both sides of the angular bisector of the heading angle of the vector between the path turning point and the target node in the robot map coordinate system.

10. A robot path planning system based on search direction cost and heuristic function optimization, characterized in that: include: a path prediction unit configured to discard a distal neighborhood of the robot's current position and plan a predicted path between the robot's current position and a target destination based on a proximal neighborhood of the robot's current position; an obstacle avoidance unit, in response to an obstacle on the predicted path between the robot's current position and the target end point, determining a turning point in the robot's path based on a distal neighborhood discarded by the robot's current position; The path adjustment unit is configured to screen out the search neighborhood of the robot at the path turning point, and plan the path from the path turning point to the target end point of the robot based on the screened search neighborhood at the path turning point, and generate a planned path from the current position of the robot to the target end point.

Images