A mobile robot path planning method and system considering narrow passages

Abstract

The application discloses a kind of mobile robot path planning method and system considering narrow channel, including map construction module, initial path solving module and path optimization module;Map construction module: the environment including narrow channel that robot is located is modeled and described;Initial path solving module: improved RRT algorithm fuses the detection mechanism to narrow channel, from the given starting point, by searching the configuration space, generate new node, constantly expand the tree structure constructed by random sampling configuration point, find a collision-free path from initial position to target position;Path optimization module: on the basis of RRT algorithm of fusing narrow channel detection mechanism, improve path node search mode, optimize the initial path processing, find parent node and find out a asymptotically optimal optimization path;The application is high to the response efficiency of narrow channel, fast execution speed, the asymptotically optimal path obtained can guarantee the safety of robot movement.

Description

Technical Field

[0001] This invention relates to the field of mobile robot technology, and more specifically to a method and system for mobile robot path planning that takes into account narrow passages. Background Technology

[0002] Path planning is one of the key technologies for the practical application of mobile robots and an important topic in the field of mobile robot research. Existing path planning algorithms are mainly used in mobile robots and autonomous driving of intelligent vehicles. By searching for obstacles in space and reasonably avoiding them, a continuous path connecting the starting point and the ending point is planned. In real life, different environmental maps are needed to deal with different work situations, especially in complex environments with narrow passages or multiple obstacles, such as libraries, hospitals, shopping malls, and express delivery stations.

[0003] However, conventional path planning and algorithms ignore the robot's own shape, size, and space occupation, resulting in low retrieval efficiency and poor robustness when facing different narrow passage environments.

[0004] Therefore, given that the algorithm can detect narrow passages or mixed obstacles, how to plan an asymptotically optimal path to allow the mobile robot to pass through efficiently and safely, thereby improving the robot's execution efficiency and the path's safety, is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] In view of this, the present invention provides a method and system for mobile robot path planning that takes into account narrow passages, which solves the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A path planning method for a mobile robot considering narrow passages includes the following steps:

[0008] S1. Construction of the environment map: Modeling and describing the environment in which the robot is located, including narrow passages;

[0009] S2. Initial path solving after improvement: The improved RRT algorithm integrates the detection mechanism for narrow channels. Starting from a given starting point, it generates new nodes by searching the configuration space and continuously expands the tree structure constructed by randomly sampled configuration points. Finally, it finds a collision-free path from the initial position to the target position in the expanded tree.

[0010] S3. Optimize the initial path: Based on the RRT algorithm that integrates the narrow channel detection mechanism, improve the path node search method, optimize the initial path, find parent nodes to reduce path inflection points, and find an asymptotically optimal optimized path.

[0011] Preferably, the specific content of constructing the environmental map in S1 includes:

[0012] The coordinates of the geometric center in S11 are x and y;

[0013] In S12, obstacle expansion is handled by expanding outward a certain distance from the obstacle's coordinates:

[0014] The distance of the expansion is:

[0015] L≥α*max(r)

[0016] Where α is the expansion coefficient, 0 < α < 1, and r is the slant radius of the robot;

[0017]

[0018] Where w is the robot width and h is the robot length.

[0019] Preferably, the specific content of S2 includes:

[0020] S21. In two-dimensional space, the robot is quantized as a node p. Bridges of length R extend to both sides of p along the X-axis, with the two endpoints designated as p1 and p2. Based on the bridges, a new set of vertical bridges of length R are orthogonally constructed, with the endpoints designated as p3 and p4.

[0021] Preferably, the specific content of S2 also includes:

[0022] S22. Randomly select the starting point Xinit and the ending point Xgoal in the established environment map, determine the initial conditions, initialize the random tree, and determine the algorithm parameters;

[0023] S23. Execute the sampling function, select a random point Xrand in the space, and find the nearest neighboring root node Xnear to the random point Xrand;

[0024] S24. Connect the random point Xrand and the nearest neighboring root node Xnear as the direction of tree growth, execute the tree growth function, set the step size as the length of the branch, grow the random tree, and determine the new node Xnew generated on the line connecting Xrand and Xnear according to the step size.

[0025] S25. Detect the narrow passage and execute the collision detection function. Traverse all obstacles, determine the space where the robot is located and whether the safety distance is satisfied. If an obstacle is encountered during the growth of the tree or it cannot pass through, stop the growth; otherwise, continue to grow.

[0026] S26. Repeat steps S22 - S25, call the connection function to connect the Xnew generated each time with Xgoal, and determine whether the distance between the two points is less than the step size. If it is less, directly connect the generated node Xnew with the end point Xgoal, and store all the node Xnew that meet the safety distance in the tree of the initial path in sequence, display the length of the planned initial path, and generate the initial path.

[0027] Preferably, the specific content for determining the space where the robot is located in S25 is:

[0028] When p3 and p4 are both inside the obstacle including the edge, and p1 and p2 are both in the free space or any one of the two points is inside the obstacle, the robot is in the narrow passage; when p1, p2, p3, and p4 are all in the free space, the robot is in the free space.

[0029] Preferably, the specific content for determining that the robot meets the safety distance in S25 is:

[0030] When the mobile robot is in the narrow passage, calculate the distance s on both sides of the narrow passage where the robot is located. Then the distance of the narrow passage should satisfy s > w, where w is the width of the robot.

[0031] When the mobile robot is in the free space, to ensure that the robot can turn freely or pass through the free space safely, it should satisfy r < R, where r is the oblique length radius of the robot.

[0032] Preferably, the specific content for optimizing the initial path includes:

[0033] S31. Execute the parent node search function for the new node Xnew stored in the initial path in sequence. Search for a new parent node Xparent on the existing path nodes within the area of range R1 around the node. Among them, the new path composed of the parent nodes has the minimum path length compared to the paths composed of other nodes already included in the tree. The range R1 gradually shrinks as the number of Xnew nodes for which the parent node search has been executed increases.

[0034] S32. Perform collision detection on the parent node Xparent. When there is an obstacle on the new path composed of the parent node Xparent, the parent node fails. If there is no obstacle, perform narrow passage detection on the parent node to determine whether the position where Xparent is located meets the safety distance requirement. If it meets, continue to grow; if it does not meet, stop growing.

[0035] S33. Connect the parent node Xparent with the node Xnew that performed the parent node search, and update the random tree path;

[0036] S34. Repeat S31-S33 to connect the parent node Xparent with the destination Xgoal each time. Determine if the distance between the two points is less than the step size. If it is less, connect them directly and display the optimized path.

[0037] Preferably, R1 in S31 is specifically:

[0038]

[0039] In the formula: The initial range is determined by the size of the environment map, and the number of nodes is the number of nodes for which a parent node search has been performed, with the value gradually increasing.

[0040] A mobile robot path planning system considering narrow passages includes: a map building module, an initial path solving module, and a path optimization module;

[0041] The map building module is used to model and describe the robot's environment, including narrow passages;

[0042] The initial path solving module is used to integrate the detection mechanism of narrow passages with the improved RRT algorithm. Starting from a given starting point, it generates new nodes by searching the configuration space, continuously expands the tree structure constructed by randomly sampled configuration points, and performs narrow passage detection and collision detection. Finally, it finds a collision-free path from the initial position to the target position in the expanded tree and stores all new nodes that meet the safety distance in the tree of the initial path in sequence.

[0043] The path optimization module is used to improve the path node search method based on the RRT algorithm that integrates narrow passage detection mechanism. It optimizes the initial path, finds parent nodes for new nodes stored in the initial path to reduce path inflection points, performs narrow passage detection and collision detection, and finds an asymptotically optimal optimized path.

[0044] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a mobile robot path planning method and system that considers narrow passages. It models the environmental map containing narrow passages, improves the shortcomings of path node redundancy by integrating the narrow passage detection mechanism and improving the node search method, thereby improving the RRT algorithm. Then, the improved RRT algorithm is used to obtain the asymptotic optimal path of the mobile robot. It has high response efficiency in narrow passages, fast execution speed, and the obtained asymptotic optimal path is conducive to ensuring the safety of robot movement. Attached Figure Description

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

[0046] Figure 1 The attached figure is a schematic diagram of a mobile robot path planning method considering narrow passages provided by the present invention;

[0047] Figure 2 The attached figure is a schematic diagram illustrating the environmental modeling of a long, narrow passage with few obstacles provided by the present invention;

[0048] Figure 3 The attached figure is a schematic diagram illustrating the environmental modeling of long, narrow passages with multiple obstacles provided by the present invention;

[0049] Figure 4 The attached figure is a schematic diagram of the robot node provided by the present invention located in a narrow passage;

[0050] Figure 5 The attached figure is a schematic diagram of the robot node provided by the present invention in free space;

[0051] Figure 6 The attached figure is a schematic diagram of the initial path solving method provided by the present invention;

[0052] Figure 7 The attached figure is a schematic diagram of the initial path result in a long, narrow passage environment with few obstacles provided by the present invention;

[0053] Figure 8 The attached figure is a schematic diagram of the initial path result in a long and narrow passage environment with multiple obstacles provided by the present invention;

[0054] Figure 9 The attached figure is a schematic diagram of the initial path optimization method provided by the present invention;

[0055] Figure 10 The attached figure is a schematic diagram of parent node search provided by the present invention;

[0056] Figure 11 The attached figure is a schematic diagram of the optimized initial path in a long, narrow passage environment with few obstacles, provided by the present invention.

[0057] Figure 12 The attached figure is a schematic diagram of the optimized initial path in a long, narrow passage environment with multiple obstacles, provided by the present invention.

[0058] Figure 13The attached figure is a schematic diagram of the initial path provided by the present invention when both the starting point and the ending point are within a long and narrow channel;

[0059] Figure 14 The attached figure is a schematic diagram of the optimized initial path provided by the present invention when both the starting and ending points are within a long and narrow channel;

[0060] Figure 15 The attached figure is a schematic diagram of the initial path provided by the present invention when both the starting point and the ending point are in free space;

[0061] Figure 16 The attached figure is a schematic diagram of the optimized initial path provided by the present invention when both the starting and ending points are in free space;

[0062] Figure 17 The attached figure is a schematic diagram of the initial path provided by the present invention when the starting point is in a relatively closed free space and the ending point is in free space;

[0063] Figure 18 The attached figure is a schematic diagram of the initial path optimization provided by the present invention when the starting point is in a relatively closed free space and the ending point is in free space;

[0064] Figure 19 The attached figure is a schematic diagram of the initial path of a special environment map containing a narrow passage provided by the present invention;

[0065] Figure 20 The attached figure is a schematic diagram of the optimized initial path of a special environment map containing narrow passages provided by the present invention. Detailed Implementation

[0066] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0067] This invention discloses a path planning method for a mobile robot considering narrow passages, comprising the following steps:

[0068] S1. Construction of the environment map: Modeling and describing the environment in which the robot is located, including narrow passages;

[0069] S2. Initial path solving after improvement: The improved RRT algorithm integrates the detection mechanism for narrow channels. Starting from a given starting point, it generates new nodes by searching the configuration space and continuously expands the tree structure constructed by randomly sampled configuration points. Finally, it finds a collision-free path from the initial position to the target position in the expanded tree.

[0070] S3. Optimize the initial path: Based on the RRT algorithm that integrates the narrow channel detection mechanism, improve the path node search method, optimize the initial path, find parent nodes to reduce path inflection points, and find an asymptotically optimal optimized path.

[0071] To further implement the above technical solution, the specific content of constructing the environmental map in S1 includes:

[0072] S11. Determine the geometric center coordinates of each obstacle;

[0073] S12. Based on the external geometric features of the mobile robot, to prevent collisions during the robot's retrieval path, obstacles are inflated to meet safety requirements, resulting in a safe environmental map, such as... Figure 2 and 3 .

[0074] To further implement the above technical solution, the geometric center coordinates in S11 are x and y;

[0075] In S12, obstacle expansion is handled by expanding outward a certain distance from the obstacle's coordinates:

[0076] The distance of the expansion is:

[0077] L≥α*max(r)

[0078] Where α is the expansion coefficient, 0 < α < 1, and r is the slant radius of the robot;

[0079]

[0080] Where w is the robot width and h is the robot length.

[0081] In this embodiment, the robot is simplified to a rectangle or square, with the slant radius being the distance from the robot's center to the vertex, which is half the diagonal of the two-dimensional rectangle.

[0082] To further implement the above technical solution, the specific content of S2 includes:

[0083] S21. In two-dimensional space, the robot is quantized as a node p. Bridges of length R extend from p along the X-axis to both sides, with endpoints designated p1 and p2. Based on these bridges, a new set of perpendicular bridges of length R are drawn, with endpoints designated p3 and p4. Figure 4 and 5 .

[0084] To further implement the above technical solutions, such as Figure 6 The specific content of S2 also includes:

[0085] S22. Randomly select the starting point Xinit and the ending point Xgoal in the established environment map, determine the initial conditions, determine the algorithm parameters, and initialize the random tree;

[0086] S23. Execute the sampling function, select a random point Xrand in the space, and find the nearest neighboring root node Xnear to the random point Xrand;

[0087] S24. Connect the random point Xrand to the nearest neighboring root node Xnear as the direction of tree growth. Execute the tree growth function, setting the step size as the length of the branches, and grow the random tree. On the line connecting Xrand and Xnear, determine the new node Xnew generated according to the step size.

[0088] The distance of the tree along the X-axis = the original node's x-distance + the distance grown * cosθ;

[0089] The distance of the tree along the Y-axis = the original node's y-distance + the distance grown * sinθ;

[0090] θ is the angle between the direction of tree growth and the X-axis, and the growth distance is the step size;

[0091] S25. Detect narrow passages and execute collision detection function, traverse all obstacles, determine the space where the robot is located and whether the safe distance is met. If the tree encounters an obstacle or cannot pass through during the growth process, stop growing; otherwise, continue growing.

[0092] Specifically: Collision detection is performed on the newly generated node Xnew in step S24. If it encounters an obstacle, Xnew becomes invalid, the node is abandoned, and the process returns to step S23. If it does not encounter an obstacle, it is determined whether the position of Xnew meets the safe distance requirement. If it does, growth continues; otherwise, growth stops, and the process returns to step S23.

[0093] S26. Repeat steps S23-S25, calling the connection function to connect each grown Xnew with Xgoal, checking if the distance between the two points is less than the step size. If it is, directly connect the generated node Xnew with the endpoint Xgoal, and store all nodes Xnew that meet the safe distance in the tree of the initial path. Display the planned initial path length, and generate the initial path, such as... Figure 7 and 8 .

[0094] To further implement the above technical solution, the specific content of determining the robot's location in S25 is as follows:

[0095] When p3 and p4 are both within an obstacle including the edge, and p1 and p2 are both in free space or any one of the two points is within the obstacle, the robot is in a narrow passage; when p1, p2, p3, and p4 are all in free space, the robot is in free space.

[0096] To further implement the above technical solution, the specific content for judging that the robot meets the safety distance in S25 is as follows:

[0097] When the mobile robot is in a narrow passage, calculate the distance s between the two sides of the narrow passage where the robot is located. Then the distance of the narrow passage should satisfy s > w, where w is the width of the robot;

[0098] When the mobile robot is in free space, to ensure that the robot can turn freely or pass through the free space safely, it should satisfy r < R, where r is the oblique length radius of the robot.

[0099] In this embodiment, a simulation analysis is carried out on the initial path result solved by the RRT algorithm that fuses and verifies the narrow passage detection mechanism: It is verified through simulation that the retrieval efficiency of the RRT algorithm that fuses the narrow passage detection mechanism in the narrow passage is much greater than that in free space; the RRT algorithm method that fuses the narrow passage detection mechanism has a faster execution efficiency and response speed compared with the traditional RRT algorithm, and can efficiently plan an asymptotically optimal path.

[0100] To further implement the above technical solution, as Figure 9 , the specific content for optimizing the initial path includes:

[0101] S31. Successively execute the parent node search function for the new node Xnew stored in the initial path. With the node Xnew as the center, construct a circular area with a radius of R1. Search for the parent node Xparent for the node Xnew within the circular area. Among them, the new path composed of the parent node Xparent has the minimum path length compared with the paths composed of other nodes already included in the tree. The range R1 gradually shrinks as the number of Xnew nodes that have executed the parent node search increases, that is, the closer to the end point, the smaller this range is, as Figure 10 ;

[0102] S32. Perform a collision detection on the parent node Xparent. When there is an obstacle in the new path composed of the parent node Xparent, this parent node fails. If there is no obstacle, perform a narrow passage detection on the parent node to judge whether the position where Xparent is located meets the safety distance requirement. If it meets, continue to grow; if it does not meet, stop growing;

[0103] [[ID=z8]]S33. Connect the parent node Xparent with the node Xnew that executes the parent node search, and update the random tree path;

[0104] S34. Repeat steps S31-S33, connecting each found parent node Xparent to the destination Xgoal. Check if the distance between the two points is less than the step size; if so, connect them directly and display the optimized path. Figure 11 and 12 .

[0105] To further implement the above technical solution, R1 in S31 is specifically as follows:

[0106]

[0107] In the formula: The initial range is determined by the size of the environment map, and the number of nodes is the number of nodes for which a parent node search has been performed, with the value gradually increasing.

[0108] Analysis of the simulation after initial path optimization:

[0109] After comparing the optimized path with the unoptimized path, it was found that the RRT algorithm with improved node search method is more efficient in terms of both the time and length of the asymptotically optimal path in narrow channels. It has high execution efficiency in path planning for narrow channels and fewer path inflection points.

[0110] In this embodiment, the optimized path formed by the node search method of the improved RRT algorithm includes a general map of the narrow passage: both the starting and ending points are within the long, narrow passage, such as... Figure 13 and 14 Both the starting and ending points are in free space, and a long, narrow passage connects the two points, such as... Figure 15 and 16 The starting point is in a relatively enclosed free space, and the ending point is in free space. A short, narrow passage connects the two points, such as... Figure 17 and 18 .

[0111] The simulation compared the paths planned before and after path optimization on the same map. In summary, different simulation environment maps verified that the RRT algorithm with the fusion of narrow passage detection mechanism and the RRT algorithm with improved node search method are generally applicable to narrow passage environment maps. Its execution efficiency and retrieval accuracy in narrow passages are significantly higher than those in free space. When facing narrow passages with multiple obstacles and maps with a mixture of free space, it can still maintain high retrieval accuracy.

[0112] The optimized path formed by improving the node search method of the RRT algorithm also includes special environment maps containing narrow passages, such as... Figure 19 and 20For special maps, such as maze maps with very large spaces containing many narrow passages and large areas of free space, the improved RRT algorithm with a modified node search method is significantly more efficient than the original RRT algorithm. This is because the original RRT algorithm, which incorporates a narrow passage detection mechanism, uses random sampling and does not need to find the shortest path. As long as safe passage is guaranteed, an asymptotically optimal path from the starting point to the ending point can be planned. However, its random sampling leads to some wasted search space in the free space. In contrast, the improved RRT algorithm with a modified node search method needs to find parent nodes within a certain range after each new node is generated to optimize the path length. The larger the map area, the more parent nodes can be connected within that range, and the greater the probability of finding the shortest path. Both the algorithm and the optimization scheme of this approach have high robustness.

[0113] A mobile robot path planning system considering narrow passages includes: a map building module, an initial path solving module, and a path optimization module;

[0114] The map building module is used to model and describe the robot's environment, including narrow passages;

[0115] The initial path solving module is used to integrate the detection mechanism of narrow passages with the improved RRT algorithm. Starting from a given starting point, it generates new nodes by searching the configuration space, continuously expands the tree structure constructed by randomly sampled configuration points, and performs narrow passage detection and collision detection. Finally, it finds a collision-free path from the initial position to the target position in the expanded tree and stores all new nodes that meet the safety distance in the tree of the initial path in sequence.

[0116] The path optimization module is used to improve the path node search method based on the RRT algorithm that integrates narrow passage detection mechanism. It optimizes the initial path, finds parent nodes for new nodes stored in the initial path to reduce path inflection points, performs narrow passage detection and collision detection, and finds an asymptotically optimal optimized path.

[0117] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to in the method section.

[0118] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A path planning method for a mobile robot considering narrow passages, characterized in that, Includes the following steps: S1. Construction of the environment map: Modeling and describing the environment in which the robot is located, including narrow passages; S2. Initial path solving after improvement: The improved RRT algorithm integrates the detection mechanism for narrow passages. Starting from a given starting point, it generates new nodes by searching the configuration space, continuously expands the tree structure constructed by randomly sampled configuration points, and performs narrow passage detection and collision detection. Finally, it finds a collision-free path from the initial position to the target position in the expanded tree, and stores all new nodes that meet the safety distance in the tree of the initial path in sequence. S3. Optimize the initial path: Based on the RRT algorithm that integrates the narrow passage detection mechanism, improve the path node search method, optimize the initial path, find parent nodes for new nodes stored in the initial path to reduce path inflection points, perform narrow passage detection and collision detection, and find an asymptotically optimal optimized path. The specific content of step S3 includes: S31. For each new node Xnew stored in the initial path, execute the parent node search function sequentially. Within the range R1 around the node, find a new parent node Xparent on the existing path node for Xnew. The new path formed by the parent node has the shortest length compared to the path formed by other nodes already included in the tree. The range R1 gradually shrinks as the number of Xnew nodes for which the parent node search has been performed increases. S32. Perform collision detection on the parent node Xparent. If there is an obstacle in the new path formed by the parent node Xparent, the parent node becomes invalid. If there is no obstacle, perform narrow passage detection on the parent node to determine whether the position of Xparent meets the safe distance requirement. If it does, continue to grow; otherwise, stop growing. S33. Connect the parent node Xparent with the node Xnew that performed the parent node search, and update the random tree path; S34. Repeat S31-S33 to connect the parent node Xparent with the destination Xgoal each time. Determine if the distance between the two points is less than the step size. If it is less, connect them directly and display the optimized path. R1 in S31 is specifically: ； In the formula: To define the initial range, its value is selected based on the size of the environment map. This represents the number of nodes whose parent node search has been performed, and its value gradually increases. The optimized paths formed by the improved node search method of the RRT algorithm include maze maps and general maps where the starting and ending points are both in long and narrow channels.

2. The mobile robot path planning method considering narrow passages according to claim 1, characterized in that, The specific content of constructing the environment map in S1 includes: S11. Determine the geometric center coordinates of each obstacle; S12. Expand the obstacles to meet safety requirements and obtain a safe environmental map.

3. The mobile robot path planning method considering narrow passages according to claim 2, characterized in that, The coordinates of the geometric center in S11 are x and y; In S12, obstacle expansion is handled by expanding outward a certain distance from the obstacle's coordinates: The distance of the expansion is: ； in, The coefficient of thermal expansion is 1 / 3. , where r is the slant radius of the robot; ； Where w is the robot width and h is the robot length.

4. The mobile robot path planning method considering narrow passages according to claim 1, characterized in that, The specific content of S2 includes: S21. In a two-dimensional space, the robot is quantified as a node p. Bridges with a length of R are extended to both sides in the X-axis direction of p, and the two endpoints are set as p1 and p2. Based on these bridges, a new set of perpendicular bridges with a length of R are constructed orthogonally, and the endpoints are set as p3 and p4 respectively.

5. A mobile robot path planning method considering narrow passages according to claim 4, characterized in that, The specific content of S2 further includes: S22. Randomly select the positions of the starting point Xinit and the ending point Xgoal in the already established environmental map, determine the initial conditions, initialize the random tree, and determine the algorithm parameters; S23. Execute the sampling function to select a random point Xrand in the space, and find the nearest neighboring root node Xnear to the random point Xrand; S24. Connect the random point Xrand and the nearest neighboring root node Xnear as the direction of tree growth. Execute the tree growth function, set the step size as the length of the tree branch, grow the random tree, and determine the newly generated node Xnew on the line connecting Xrand and Xnear according to the step size; S25. Detect narrow channels and execute the collision detection function. Traverse all obstacles, judge the space where the robot is located and whether the safety distance is satisfied. If an obstacle is encountered or passage is impossible during the tree growth process, stop growing; otherwise, continue growing; S26. Repeat steps S22 - S25, call the connection function to connect Xnew generated each time with Xgoal, judge whether the distance between the two points is less than the step size. If it is less, directly connect the generated node Xnew with the end point Xgoal, and sequentially store all the node Xnew that meet the safety distance in the tree of the initial path, display the length of the planned initial path, and generate the initial path.

6. A mobile robot path planning method considering narrow passages according to claim 5, characterized in that, The specific content of judging the space where the robot is located in S25 is: When both p3 and p4 are inside the obstacle including the edge, and both p1 and p2 are in the free space or any one of the two points is inside the obstacle, the robot is in a narrow channel; when p1, p2, p3, and p4 are all in the free space, the robot point is in the free space.

7. A mobile robot path planning method considering narrow passages according to claim 6, characterized in that, The specific content of judging that the robot meets the safety distance in S25 is: When the mobile robot is in a narrow channel, calculate the distance s on both sides of the narrow channel where the robot is located. Then the distance of the narrow channel should satisfy s > w, where w is the width of the robot; When the mobile robot is in the free space, to ensure that the robot can turn freely or pass through the free space safely, it should satisfy r < R, where r is the oblique radius of the robot.

8. A mobile robot path planning system considering narrow passages, based on the mobile robot path planning method considering narrow passages according to any one of claims 1-7, characterized in that, It includes: A map construction module, an initial path solving module, and a path optimization module; The map construction module is used to model and describe the environment where the robot is located, including narrow channels; The initial path solving module is used to utilize the improved RRT algorithm to integrate the detection mechanism for narrow channels. Starting from the given starting point, by retrieving the configuration space, generate new nodes, continuously expand the tree structure constructed by randomly sampled configuration points, and perform narrow channel detection and collision detection. Finally, find a collision-free path from the initial position to the target position in the expanded tree, and sequentially store all the new nodes that meet the safety distance in the tree of the initial path; The path optimization module is used to improve the path node search method based on the RRT algorithm that integrates narrow passage detection mechanism. It optimizes the initial path, finds parent nodes for new nodes stored in the initial path to reduce path inflection points, performs narrow passage detection and collision detection, and finds an asymptotically optimal optimized path.

Images