Path planning method of rectangular robot based on low-computing-power platform

Abstract

The invention relates to the technical field of industrial robots, in particular to a path planning method for a rectangular robot based on a low-computing-power platform, and the method comprises the steps: S1, building a grid map of a working environment of the rectangular robot; s2, planning an initial path from the position of the robot to an end point based on an A * algorithm in the grid map, wherein the initial path comprises a starting point, the end point, a plurality of path points and sub-paths for connecting two adjacent path points; and S3, correcting the initial path based on a hybrid A * algorithm. According to the method, the problem that potential collision exists in the path planned by the A * algorithm for the rectangular contour robot can be solved, meanwhile, it is ensured that the calculated amount is matched with resources of a low-calculation-power platform, and long calculation time is not generated.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial robots, and in particular to a path planning method for a rectangular robot based on a low computing power platform. Background Art

[0002] Path planning for rectangular robots is one of the current research hotspots, which involves the robot's ability to navigate autonomously in complex environments. The goal of path planning is to find an optimal path between a given starting point and end point while avoiding obstacles and other potential risks. Commonly used path planning algorithms include RRT algorithm, ant colony algorithm, artificial potential field algorithm, SFLA, A* algorithm, etc. The RRT algorithm has high search efficiency, but the path obtained is often far from the optimal path. The convergence speed of the ant colony algorithm is relatively slow and it is easy to fall into a local optimal state. Although the artificial potential field method has a simple structure and is easy to implement, it still has problems such as unreachable targets, oscillations in front of obstacles, and unreachable planned paths. Compared with other path planning algorithms, the A* algorithm has a simple model construction and high search efficiency. The solution obtained for static scenes is close to the optimal solution, and it has strong scalability and adaptability to different scenes.

[0003] The A* algorithm also has many shortcomings, such as generating too many turning points in the path, the path is not smooth enough, etc., and it cannot be used for path planning of rectangular robots.

[0004] As an improvement of the A* algorithm, the hybrid A* algorithm calculates whether the robot will collide at multiple angles at each point when planning the path, and then takes the most reasonable posture as the path point. Since the robot is at different angles when searching for path points, the amount of calculation is greatly increased, which is not suitable for running on some low-computing controllers. Summary of the invention

[0005] The present invention provides a path planning method for a rectangular robot based on a low-computing power platform, which can solve the problem of potential collision of the path planned by the A* algorithm for the rectangular contour robot, while ensuring that the calculation amount is compatible with the resources of the low-computing power platform and does not incur long calculation time.

[0006] This application provides the following technical solutions:

[0007] A path planning method for a rectangular robot based on a low computing power platform comprises the following steps:

[0008] S1, establish a grid map of the rectangular robot working environment;

[0009] S2. Planning an initial path from the robot position to the end point in the grid map based on the A* algorithm, wherein the initial path includes a starting point, an end point, a number of path points and sub-paths;

[0010] S3. Modify some sub-paths based on the hybrid A* algorithm.

[0011] Technical principle: In the grid map, the A* algorithm is first used to plan the initial planning path from the starting point to the end point, and then part of the path is detected and corrected using the hybrid A* algorithm to obtain the final planning path.

[0012] Beneficial effects: The A* algorithm has a simple model construction and high search efficiency. The solution obtained for static scenes is close to the optimal solution and can be well applied to low-computing power platforms. On this basis, the hybrid A* algorithm is used to further detect and correct some path segments, which can solve the problem of potential collisions in the paths planned by the A* algorithm for rectangular contour robots. At the same time, the length of the corrected path is controlled to ensure that the amount of calculation is compatible with the resources of the low-computing power platform and does not incur long calculation time.

[0013] Further, the S3 step includes:

[0014] S31, setting angle threshold and grid cost;

[0015] S32, checking the initial path length, and when the initial path length is less than the corrected length, replanning the path based on the hybrid A* algorithm; when the initial path length is greater than or equal to the corrected length, executing S33;

[0016] S33, modifying the starting segment based on the hybrid A* algorithm, where the starting segment is a subpath between the starting point and the first path point;

[0017] S34, modifying the end segment based on the hybrid A* algorithm, where the end segment is the subpath between the last path point and the end point;

[0018] S35, traverse all path points, perform collision detection on each path point, and if a collision is detected at a certain path point, use the hybrid A* algorithm to correct the subpath containing the path point.

[0019] Beneficial effects: When the planned path is short, computing resources are sufficient. At this time, using the Hybrid A* algorithm to plan the path can completely avoid collisions of rectangular robots. When the planned path is long, the fixed-length segments of the starting and ending segments are optimized respectively, and all remaining path points are detected and corrected. The corrected planned path is truly usable and close to the optimal solution.

[0020] Further, the S33 includes:

[0021] S331, calculating the starting grid cost and comparing it with the cost threshold, when the starting grid cost is greater than or equal to the cost threshold, executing S332;

[0022] S332, calculating a first angle, where the first angle is the angle between the current direction of the robot and the direction of the starting segment, and executing S333 when the first angle is greater than or equal to the angle threshold;

[0023] S333, calculate the second angle, which is the angle between the opposite direction of the current direction of the robot and the direction of the starting segment. When the second angle is less than the angle threshold, traverse all path points from the starting point to the end point and calculate the grid cost value, search for the first path point whose grid cost value is less than the cost threshold and count it as start_P, rotate the angle of the path point between the starting point and start_P by 180 degrees to generate a corrected sub-path, and use the corrected path to replace the corresponding sub-path in the initial path to obtain the output path; when the second angle is greater than or equal to the angle threshold, execute S334;

[0024] S334, using the hybrid A* algorithm to plan a 10-meter-long correction sub-path starting from the starting point, and using the correction sub-path to replace the corresponding sub-path in the initial path to obtain an output path.

[0025] Beneficial effect: For the sub-path starting from the starting point, the robot's various motion modes are tested in turn to see if they can adapt to the planned path, including rotating in place to align the path direction and then moving along the path, moving along the path in the original direction, moving along the path in reverse, and moving along the path in reverse for a distance and then rotating 180 degrees to move along the path. If none of the above motion modes can adapt to the path, indicating that there is still a collision, the hybrid A* algorithm is used to re-plan the path starting from the starting point. In order to take into account the computing resources and computing efficiency of the low-computing power platform, the length of the re-planned path is limited by a preset correction length.

[0026] Further, the S34 includes:

[0027] S341, calculating the end point grid cost of the output path obtained in S33 and comparing it with the cost threshold, and when the end point grid cost is greater than or equal to the cost threshold, executing S342;

[0028] S342, calculating a third angle, where the third angle is the angle between the end point direction and the end segment direction, and executing S343 when the third angle is greater than or equal to the angle threshold;

[0029] S343, calculate the fourth angle, which is the angle between the opposite direction of the current direction of the robot and the direction of the end segment. When the fourth angle is less than the angle threshold, traverse all path points from the end point to the starting point and calculate the grid cost value, search for the first path point whose grid cost value is less than the cost threshold and count it as goal_P, rotate the angle of the path point between the end point and goal_P by 180 degrees to generate a corrected subpath, and use the corrected path to replace the corresponding subpath in the initial path to obtain the output path; when the fourth angle is greater than or equal to the angle threshold, execute S344;

[0030] S344, using the hybrid A* algorithm to plan a 10-meter-long correction sub-path before the end point, and using the correction sub-path to replace the corresponding sub-path in the initial path to obtain an output path.

[0031] Beneficial effects: For the sub-path before the end point, the robot's various motion modes are tested in turn to see if they can adapt to the planned path, including rotating in situ to align the path direction and then moving along the path, moving along the path in the original direction, moving along the path in reverse, and moving along the path in reverse for a distance and then rotating 180 degrees at the path point that satisfies the in-situ rotation and then moving along the path. If the above motion modes cannot avoid collisions, the hybrid A* algorithm is used to re-plan the path before the end point. In order to take into account the computing resources and computing efficiency of low-computing power platforms, the length of the re-planned path is limited by a preset correction length.

[0032] Further, the S35 includes:

[0033] S351, traverse all path points from the starting point to the end point, and perform collision detection at each path point;

[0034] S352. If a collision is detected at a certain path point, the hybrid A* algorithm is used to plan sub-paths with half the correction length before and after the path point to obtain a correction sub-path, and the correction sub-path is used to replace the corresponding sub-path in the output path; if the length between the path point and the adjacent path point is less than half of the correction length, the hybrid A* algorithm is used to plan the sub-path between the path point and the adjacent path point to obtain a correction sub-path, and the correction sub-path is used to replace the corresponding sub-path in the output path.

[0035] Beneficial effect: After completing the path correction of the starting segment and the end segment, collision detection is performed on all remaining path points and correction is performed using the hybrid A* algorithm, which ensures that the planned full path is truly available and will not collide, while maximizing the use of computing resources on low-computing-power platforms.

[0036] Furthermore, the angle threshold is 45 degrees.

[0037] Furthermore, the corrected length is 10 meters.

[0038] Furthermore, the grid map is a multi-layer grid map.

[0039] Beneficial effects: Multi-layer grid maps are an effective tool for dealing with complex environments. They divide the environment into multiple levels, provide rich environmental information, and have the advantages of high flexibility, real-time performance, and computational efficiency.

[0040] Furthermore, the rectangular robot is a differential model.

[0041] Beneficial effects: The robot in the differential model has two independently controlled driving wheels, which can rotate on the spot and increase the flexibility of path adjustment. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 A step diagram of a path planning method for a rectangular robot based on a low computing power platform;

[0043] Figure 2 The figure shows a detailed flow chart of a path planning method for a rectangular robot based on a low-computing-power platform. DETAILED DESCRIPTION

[0044] The following is further described in detail through specific implementation methods:

[0045] For ease of description, the present invention makes the following definitions:

[0046] robot_cost: the cost value of the grid where the robot is currently located, that is, the cost value of the grid at the starting point of the path;

[0047] threshold_diatance_obstacle: The minimum allowed distance between the robot and obstacles when rotating in place;

[0048] threshold_cost: cost threshold, the minimum cost of a robot colliding with an obstacle when rotating in place;

[0049] cost_scaling_factor: the expansion coefficient of the expansion layer in the multi-layer raster map;

[0050] threshold_yaw_error: Angle threshold, the maximum angle difference allowed between the robot's current direction and the path direction;

[0051] diff_yaw_front: the first angle, the angle between the robot's forward direction at the starting point and the direction of the path's starting segment;

[0052] diff_yaw_back: The second angle, the angle between the robot's backward direction at the starting point and the direction of the path's starting segment; diff_goal_yaw_front: The third angle, the angle between the robot's forward direction near the end point and the direction of the path's ending segment;

[0053] diff_goal_yaw_back: The fourth angle, which is the angle between the direction opposite to the robot's forward direction when approaching the end position and the direction of the end segment of the path;

[0054] goal_cost: The cost value of the grid where the destination is located.

[0055] Embodiment 1

[0056] This embodiment provides a path planning method for a rectangular robot based on a low computing power platform, comprising the following steps:

[0057] S1, establish a grid map of the rectangular robot working environment;

[0058] Before path planning, the environment needs to be modeled and a global occupancy grid map needs to be established. The grid map divides the environment into discrete grid cells, establishing a simple and effective method for representing the environment.

[0059] The grid map uses the lower left corner as the coordinate origin, with the x-axis to the right and the y-axis to the top, to construct an X*Y global occupancy grid map, where X is the length of the map and Y is the width of the map. Choose a suitable grid resolution, that is, the width and height of each grid. The choice of grid resolution depends on the size of the robot and the complexity of the environment. According to the size of the environment and the grid resolution, calculate the number of grids to generate a grid map.

[0060] Environmental information can be obtained through sensor data, including lidar scanning, camera capturing environmental images, ultrasonic sensor detection, etc. The data obtained by the sensor will be converted into coordinates in the raster map. The accuracy of environmental perception can be improved by fusing data from different sensors.

[0061] In this embodiment, a multi-layer grid map is established to divide the environment into multiple levels. It has the advantages of multi-dimensional information representation, high flexibility and efficient path planning, can provide rich semantic understanding, and has good scalability and customizability.

[0062] S2. Plan an initial path from the robot position to the end point in the grid map based on the A* algorithm. The planned initial path includes a starting point, an end point, a number of path points and sub-paths.

[0063] Among them, the path between the starting point and the adjacent path points, the end point and the adjacent path points, and the path between adjacent path points are all called sub-paths.

[0064] The A* algorithm calculates the priority of each path point through a heuristic function. The expression of the heuristic function is as follows:

[0065] f(n)=g(n)+h(n)

[0066] Among them, f(n) is the comprehensive priority of node n; g(n) is the cost of node n from the starting point; h(n) is the estimated cost of node n from the end point.

[0067] During the operation of the A* algorithm, two sets are used to represent the nodes to be traversed and the nodes that have been traversed, usually called the open list and the close list. Each time, the node with the smallest f(n) value (highest priority) is selected from the priority queue as the next node to be traversed.

[0068] S3, based on the Hybrid A* algorithm, the initial path is corrected, including the following sub-steps:

[0069] S31, setting angle threshold and grid cost;

[0070] The rectangular robot used in this implementation is a differential model that can rotate in place. The robot can be adjusted to adapt to the path through a variety of movement methods, including rotating in place to align the path direction and then moving along the path, maintaining the original direction and rotating in the path direction, rotating and moving in the path direction in reverse, moving a certain distance along the path and then finding a path point that satisfies the in-place rotation and then rotating 180 degrees to move along the path. The corresponding constraint parameters are established according to the actual performance of the rectangular robot.

[0071] If the actual distance between the robot and the obstacle is greater than threshold_diatance_obstacle, the robot can rotate freely in place. In this embodiment, threshold_diatance_obstacle is converted into a cost value threshold_cost according to the expansion radius and expansion coefficient of the expansion layer of the multi-layer grid map. The conversion relationship is shown in the following formula:

[0072] threshold_cost=253*std::exp(-1.0*cost_scaling_factor*threshold_diatance_obstacle)

[0073] Where cost_scaling_factor is the expansion coefficient of the expansion layer.

[0074] In addition, the aspect ratio of the rectangular robot will affect its obstacle avoidance ability when rotating in place. When the robot rotates to a certain angle, it collides with obstacles. Therefore, the angle threshold threshold_yaw_error is set based on the aspect ratio of the rectangular robot. In this embodiment, the length / width of the rectangular robot is 1.01 / 0.7, and the corresponding angle threshold is 45 degrees. In the path planning method of this embodiment, the angle threshold of 45 degrees is only an example and can be limited according to the actual size and performance data of the robot.

[0075] S32, checking the initial path length, and when the initial path length is less than the corrected length, replanning the path based on the hybrid A* algorithm; when the initial path length is greater than or equal to the corrected length, executing S33;

[0076] When the planned path is short, computing resources are sufficient. In this case, using the Hybrid A* algorithm to plan the path can give full play to the advantages of Hybrid A*, completely avoid collisions of rectangular robots, and not produce long computing delays. When the planned path is long, the initial path is optimized in segments, and all remaining path points are detected and corrected to gradually approach the optimal solution.

[0077] The general process of the Hybrid A* algorithm is as follows:

[0078] Mark the starting point as open and add it to the open set;

[0079] When the open set is not empty, continue to the next step, otherwise return to the starting point;

[0080] Take the point nPred with the smallest F value from the open set and start processing;

[0081] If nPred is in the open state, it means that it is a point that can be expanded, then continue, otherwise return to the point;

[0082] Mark the nPred state as close and remove it from the open set;

[0083] Check whether the point is the target point. If so, return to the node nPred directly, indicating the end of the search; otherwise, continue searching.

[0084] S33, modifying the starting segment, wherein the starting segment is the sub-path between the starting point and the first path point;

[0085] Specifically, S33 includes the following sub-steps:

[0086] S331. Calculate the grid cost robot_cost at the starting position. When robot_cost < threshold_cost, it means the robot can complete a原地 rotation at the path starting point and there is no need to correct the path. When robot_cost ≥ threshold_cost, it means the condition for原地 rotation cannot be met, and at this time, execute S332;

[0087] S332. Calculate diff_yaw_front. When diff_yaw_front < threshold_yaw_error, it means the robot can follow the path and move forward without adjusting the path of the starting segment. When diff_yaw_front ≥ threshold_yaw_error, it means the robot will collide when following the route forward, and at this time, execute S333;

[0088] S333. Calculate diff_yaw_back. When diff_yaw_back < threshold_yaw_error, it means the robot can follow the initial path backward. Traverse all path points from the starting point to the ending point and calculate the grid cost value. Search for the first path point with a grid cost value less than threshold_yaw_error and record it as start_P. Rotate the angles of the path points between the starting point and start_P by 180 degrees to generate a corrected sub-path, and use this corrected path to replace the corresponding sub-path in the initial path to obtain the output path. When diff_yaw_back ≥ threshold_yaw_error, it means the robot cannot meet the condition of following the initial path backward, and at this time, execute S334;

[0089] S334. Use the hybrid A* algorithm to plan a corrected sub-path with a length of 10 meters starting from the starting point, and use this corrected sub-path to replace the corresponding sub-path in the initial path to obtain the output path;

[0090] S34. Correct the ending segment. The ending segment is the sub-path between the last path point and the ending point, including the following sub-steps:

[0091] S341. Calculate the grid cost goal_cost at the ending point for the output path obtained in S33. When goal_cost < threshold_cost, it means the robot can complete a原地 rotation at the path ending point and there is no need to correct the path. When goal_cost ≥ threshold_cost, it means the condition for原地 rotation cannot be met, and at this time, execute S342;

[0092] It should be noted that the term "原地 rotation" in the translation is a placeholder as the specific Chinese term seems not to be accurately translated into a common English expression. You may need to adjust it according to the actual meaning.S342. Calculate diff_goal_yaw_front. When diff_goal_yaw_front < threshold_yaw_error, it means the robot can reach the end point while maintaining the forward direction without adjustment. When diff_goal_yaw_front ≥ threshold_yaw_error, it means the robot will collide when following the path forward. In this case, execute S343;

[0093] S343. Calculate diff_goal_yaw_back. When diff_goal_yaw_back < threshold_yaw_error, it means the robot can reach the end point by backing up. Traverse all the path points from the end point to the start point and calculate the grid cost value. Search for the first path point with a grid cost value less than threshold_yaw_error and record it as goal_P. Rotate the angles of the path points between the end point and goal_P by 180 degrees to generate a corrected sub-path, and use this corrected path to replace the corresponding sub-path in the initial path to obtain the output path. When diff_goal_yaw_back ≥ threshold_yaw_error, execute S344;

[0094] S344. Use the hybrid A* algorithm to plan a corrected sub-path with a length of 10 meters starting from the start point, and use this corrected sub-path to replace the corresponding sub-path in the initial path to obtain the output path.

[0095] S35. Traverse all the path points to detect collision situations. For the path points where collisions occur, use the hybrid A* algorithm to plan a corrected sub-path that includes this path point, specifically including the following sub-steps:

[0096] S351. Traverse all the path points from the start point to the end point and perform collision detection at each path point;

[0097] S352. If a collision is detected at a certain path point, use the hybrid A* algorithm to plan a corrected sub-path that is 5 meters before and after this path point to obtain the corrected sub-path, and use the corrected sub-path to replace the corresponding sub-path in the output path. If the length between this path point and the adjacent path point is less than 5 meters, use the hybrid A* algorithm to plan the sub-path between this path point and the adjacent path point to obtain the corrected sub-path, and use the corrected sub-path to replace the corresponding sub-path in the output path.

[0098] The correction sub-path of the path planning method of the present invention is 10 meters in length and is only used for example. It can be expanded to a corresponding length according to the actual computing power of the rectangular robot. For example, when the computing power resources of the rectangular robot are sufficient, the length of each planned correction sub-path can be set to 12 meters or longer. When the computing power resources of the rectangular robot are relatively tight, the length of each planned correction sub-path can be set to 8 meters or shorter.

[0099] The above are only embodiments of the present invention. The invention is not limited to the field involved in this implementation case. The common knowledge such as the known specific structure and characteristics in the scheme is not described in detail here. It should be pointed out that for those skilled in the art, several deformations and improvements can be made without departing from the structure of the present invention, which should also be regarded as the protection scope of the present invention, and these will not affect the effect of the implementation of the present invention and the practicality of the patent. The scope of protection required by this application shall be based on the content of its claims, and the specific implementation methods and other records in the specification can be used to interpret the content of the claims.

Claims

1. A path planning method for a rectangular robot based on a low computing power platform, characterized in that: The following steps are involved: S1, establish a grid map of the rectangular robot working environment; S2. Planning an initial path from the robot position to the end point in the grid map based on the A* algorithm, wherein the initial path includes a starting point, an end point, a number of path points and sub-paths; S3. Modify the initial path based on the hybrid A* algorithm.

2. The path planning method for a rectangular robot based on a low computing power platform according to claim 1, characterized in that: The S3 steps include: S31, setting angle threshold and grid cost; S32, checking the initial path length, and when the initial path length is less than the corrected length, replanning the path based on the hybrid A* algorithm; when the initial path length is greater than or equal to the corrected length, executing S33; S33, modifying the starting segment based on the hybrid A* algorithm, where the starting segment is a subpath between the starting point and the first path point; S34, modifying the end segment based on the hybrid A* algorithm, where the end segment is the subpath between the last path point and the end point; S35, traverse all path points, perform collision detection on each path point, and if a collision is detected at a certain path point, use the hybrid A* algorithm to correct the subpath containing the path point.

3. The path planning method for a rectangular robot based on a low computing power platform according to claim 2, characterized in that: The S33 includes: S331, calculating the starting grid cost and comparing it with the cost threshold, when the starting grid cost is greater than or equal to the cost threshold, executing S332; S332, calculating a first angle, where the first angle is the angle between the current direction of the robot and the direction of the starting segment, and executing S333 when the first angle is greater than or equal to the angle threshold; S333, calculate the second angle, which is the angle between the opposite direction of the current direction of the robot and the direction of the starting segment. When the second angle is less than the angle threshold, traverse all path points from the starting point to the end point and calculate the grid cost value, search for the first path point whose grid cost value is less than the cost threshold and count it as start_P, rotate the angles of all path points between the starting point and start_P by 180 degrees to generate a corrected sub-path, and use the corrected path to replace the corresponding sub-path in the initial path to obtain the output path; when the second angle is greater than or equal to the angle threshold, execute S334; S334, using the hybrid A* algorithm to plan a 10-meter-long correction sub-path starting from the starting point, and using the correction sub-path to replace the corresponding sub-path in the initial path to obtain an output path.

4. The path planning method for a rectangular robot based on a low computing power platform according to claim 3, characterized in that: The S34 includes: S341, calculating the end point grid cost of the output path obtained in S33 and comparing it with the cost threshold, and when the end point grid cost is greater than or equal to the cost threshold, executing S342; S342, calculating a third angle, where the third angle is the angle between the end point direction and the end segment direction, and executing S343 when the third angle is greater than or equal to the angle threshold; S343, calculate the fourth angle, which is the angle between the opposite direction of the current direction of the robot and the direction of the end segment. When the fourth angle is less than the angle threshold, traverse all path points from the end point to the starting point and calculate the grid cost value, search for the first path point whose grid cost value is less than the cost threshold and count it as goal_P, rotate the angle of the path point between the end point and goal_P by 180 degrees to generate a corrected subpath, and use the corrected path to replace the corresponding subpath in the initial path to obtain the output path; when the fourth angle is greater than or equal to the angle threshold, execute S344; S344, using the hybrid A* algorithm to plan a 10-meter-long correction sub-path before the end point, and using the correction sub-path to replace the corresponding sub-path in the initial path to obtain an output path.

5. The path planning method for a rectangular robot based on a low computing power platform according to claim 4, characterized in that: The S35 includes: S351, traverse all path points from the starting point to the end point, and perform collision detection at each path point; S352. If a collision is detected at a certain path point, the hybrid A* algorithm is used to plan sub-paths with half the correction length before and after the path point to obtain a correction sub-path, and the correction sub-path is used to replace the corresponding sub-path in the output path; if the length between the path point and the adjacent path point is less than half of the correction length, the hybrid A* algorithm is used to plan the sub-path between the path point and the adjacent path point to obtain a correction sub-path, and the correction sub-path is used to replace the corresponding sub-path in the output path.

6. The path planning method for a rectangular robot based on a low computing power platform according to claim 3, characterized in that: The angle threshold is 45 degrees.

7. The path planning method for a rectangular robot based on a low computing power platform according to claim 2, characterized in that: The corrected length is 10 meters.

8. The path planning method for a rectangular robot based on a low computing power platform according to claim 1, characterized in that: The grid map is a multi-layer grid map.

9. The path planning method for a rectangular robot based on a low computing power platform according to claim 1, characterized in that: The rectangular robot is a differential model.

Images