Mobile robot path planning method, device, electronic device and storage medium

Abstract

The present invention provides a mobile robot path planning method, device, electronic device, and storage medium, belonging to the technical field of robot motion control path planning. The method includes: constructing a first super ellipsoid based on the real-time progress of a bidirectional rapid exploration random tree exploration, adaptively adjusting the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sampling to obtain a first sampling point; expanding the bidirectional rapid exploration random tree; after expansion, if a direct connection is not possible and the number of tree nodes in the reverse tree is greater than the number of tree nodes in the forward tree, exchanging the forward tree and the reverse tree and returning to step 1 until the reverse tree and the forward tree can be directly connected, merging the forward tree and the reverse tree to obtain an initial path solution; calculating the time consumed by the loop to the end, and if it is not less than a time threshold, outputting the initial path solution as the optimal solution. The present invention effectively solves the technical problem of low path planning efficiency in the prior art when processing complex environments.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion control path planning, and in particular to a mobile robot path planning method, device, electronic equipment and storage medium. Background Art

[0002] With the rapid development of robotics, how to achieve efficient, safe and optimized motion planning in space has become one of the research hotspots. Currently, motion planning algorithms have been widely used in various fields, including but not limited to robot navigation, industrial automation, space satellite orbit planning, and logistics and warehousing automation. In order to complete the planning task efficiently, a variety of path planning algorithms have been proposed. Among them, fast search random tree ( ) show excellent robustness and adaptability in high-dimensional space.

[0003] The algorithm is based on random sampling of unknown space, extracting spatial information, and exploring the spatial state through incremental growth of the tree. After continuous iteration of random samples, the planner gradually establishes a set of trajectories. As the number of sampling points increases, the random tree can describe the structure of the unknown space with higher resolution. Therefore, It has high flexibility in dealing with high-dimensional and nonlinear problems, and has a wide range of applications in robotic systems with complex degrees of freedom and multiple constraints, including unmanned vehicles ( )、Robotic Arm( ), drones( ), surface aircraft ( ). Karaman and Frazzoli proposed in "Sampling-based algorithms for optimal motion planning" Algorithm, as One of the best variants of the algorithm, After the trajectory is established, it will search for a better connection method in the neighborhood around the sampling point, which has asymptotic optimality. When the number of iterations approaches infinity, The probability of finding the optimal solution is close to 100%.

[0004] However, The convergence effect depends largely on the quality of random sampling. If there is a lack of sufficient sampling in a specific area, or the sampling in a local area is too redundant, the planning efficiency will be significantly reduced. The existing technology effectively constrains the sampling space by constructing a hyperellipsoid, avoiding the problem of over-exploration of the random tree, so that it can better approach the optimal solution within a limited time. Another existing technology uses the bisection method to create the growth parent node, and prunes the tree nodes in the local area, significantly improving the quality of the initial solution. However, the planning efficiency of the above algorithms is still not enough to cope with more complex high-dimensional environments or path planning problems with complex constraints. Summary of the Invention

[0005] In view of this, it is necessary to provide a mobile robot path planning method, device, electronic device and storage medium to solve the technical problem of low path planning efficiency in the existing technology when dealing with complex environments.

[0006] In order to solve the above technical problems, in a first aspect, the present invention provides a mobile robot path planning method, comprising:

[0007] Step 1: Explore the motion path of the mobile robot in the target map space based on a preset bidirectional rapid exploration random tree, construct a first super ellipsoid based on the real-time progress of the bidirectional rapid exploration random tree exploration, adaptively adjust the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability;

[0008] Step 2: Expand the bidirectional rapid exploration random tree based on the first sampling point;

[0009] Step 3: After expansion, if the reverse tree and the forward tree cannot be directly connected, and the number of tree nodes in the reverse tree is greater than the number of tree nodes in the forward tree, exchange the forward tree and the reverse tree and return to step 1 until the reverse tree and the forward tree can be directly connected. Merge the forward tree and the reverse tree and backtrack to obtain the initial path solution and enter step 4;

[0010] Step 4: Calculate the time consumed from the start of step 1 to the end of the loop of step 3. If the time is not less than a preset time threshold, output the initial path solution as the optimal solution.

[0011] In some embodiments of the present invention, the bidirectional rapid exploration random tree uses the robot's starting position and target position as root nodes, uses a preset step size as the exploration step size, and outputs the real-time progress of the exploration in the form of T=(V,E), where T is the feasible path of the mobile robot explored by the bidirectional rapid exploration random tree, V is the set of tree nodes, and E is the set of link relationships between tree nodes;

[0012] The method explores the motion path of the mobile robot in the target map space based on a preset bidirectional rapid exploration random tree, constructs a first super ellipsoid based on the real-time progress of the bidirectional rapid exploration random tree exploration, adaptively adjusts the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and obtains a first sampling point by random sampling based on the sampling probability, including:

[0013] The two latest tree nodes of the bidirectional rapid exploration random tree in the real-time progress are used as two foci of the first super ellipsoid, and the major axis length and the minor axis length of the first super ellipsoid are determined based on the spatial distance between the two foci;

[0014] Adjusting the sampling probabilities of random sampling within the first hyperellipsoid and random sampling in the target map space based on a preset sampling probability based on the ratio of the spatial distance from the robot's starting position to the target position and the major axis length;

[0015] Generate a random number in the range of [0, 1]. If the random number is greater than the adjusted sampling probability, random sampling is performed within the first super ellipsoid; otherwise, random sampling is performed in the target map space.

[0016] In some embodiments of the present invention, the expanding the bidirectional rapid exploration random tree based on the first sampling point includes:

[0017] A new node is generated based on the first sampling point and added to the bidirectional rapid exploration random tree, and a bisection method is used to smoothly prune the bidirectional rapid exploration random tree after the new node is added.

[0018] In some embodiments of the present invention, generating a new node based on the first sampling point and adding it to the bidirectional rapid exploration random tree, and using a bisection method to smoothly prune the bidirectional rapid exploration random tree after adding the new node, includes:

[0019] Traversing and calculating the Euclidean distance between each tree node in the node set output by the forward tree and the first sampling point;

[0020] Select the node with the smallest distance as the first starting state of the forward growth of the forward tree, and perform a single-step exploration towards the first sampling point based on the preset step size to obtain the first new node;

[0021] If no obstacle is detected during the expansion of the current forward tree, then the first furthest ancestor state that can be traced back to the first starting state during the growth of this forward tree is returned;

[0022] Creating a first parent node between the first furthest ancestor state and the first starting state based on bisection to replace the first starting state;

[0023] If the first angle between the replaced first starting state, the first furthest ancestor state, and the first new node with the first starting state as the vertex does not reach a preset dichotomy threshold, then a new first parent node is created between the first furthest ancestor state and the replaced first starting state based on dichotomy to replace the replaced first starting state, and the cycle is repeated until the first angle reaches the dichotomy threshold or an obstacle is detected on the planned path;

[0024] updating the forward tree based on the first new node, the first furthest ancestor state, and the first parent node;

[0025] Traversing and calculating the Euclidean distance between each node in the node set output by the reverse tree and the first new node;

[0026] Selecting the node with the smallest distance as the second starting state of reverse growth, exploring toward the first new node without adding a step size limit until an obstacle or a dual-tree connection is detected, thereby obtaining a second new node;

[0027] Return to the second furthest ancestor state that the second starting state can trace back to during this growth process;

[0028] Creating a second parent node between the second furthest ancestor state and the second starting state based on bisection to replace the second starting state;

[0029] If the second angle between the replaced second starting state, the second furthest ancestor state, and the second new node with the second starting state as the vertex does not reach a preset dichotomy threshold, then a new second parent node is created between the second furthest ancestor state and the replaced second starting state based on dichotomy to replace the replaced second starting state, and the cycle is repeated until the second angle reaches the dichotomy threshold or an obstacle is detected on the planned path;

[0030] The inverted tree is updated based on the second new node, the second furthest ancestor state, and the second parent node.

[0031] In some embodiments of the present invention, the mobile robot path planning method further includes:

[0032] If the time is less than a preset time threshold, the initial path solution is optimized based on the first set of tree nodes of the initial path solution.

[0033] In some embodiments of the present invention, optimizing the initial path solution based on the first set of tree nodes of the initial path solution includes:

[0034] A second hyperellipsoid is constructed with the robot's target position as the first focus and all other nodes in the first set except the target position as the second focus, wherein the sum of the spatial distances from any point in the second hyperellipsoid to the first focus and the second focus is no greater than a preset current optimal path cost value;

[0035] Randomly sampling within the second super ellipsoid to obtain a second sampling point;

[0036] The initial path solution is optimized using a bisection method based on the second sampling point.

[0037] In some embodiments of the present invention, optimizing the initial path solution using a bisection method based on the second sampling point includes:

[0038] Traverse and calculate the Euclidean distance from each tree node in the first set to the second sampling point, and obtain the tree node with the minimum distance;

[0039] Connect all tree nodes in the first set one by one in the direction from the robot's starting position to the target position, and measure the size of the third angle in the connection line with the tree node with the smallest distance as the vertex;

[0040] Replace the tree node with the smallest distance with the second sampling point and update the link relationship between tree nodes to obtain the latest path solution;

[0041] Connect all tree nodes in the latest path solution one by one in the direction from the robot's starting position to the target position, and measure the size of the fourth angle in the connecting line with the second sampling point as the vertex;

[0042] If the fourth angle is not greater than the third angle, returning to the step of randomly sampling within the second super ellipsoid to obtain a second sampling point, until the fourth angle is greater than the third angle;

[0043] Replace the initial path solution with the latest path solution.

[0044] In a second aspect, the present invention further provides a mobile robot path planning device, comprising:

[0045] A sampling module is used to construct a first super ellipsoid based on the real-time progress of exploration of the preset bidirectional rapid exploration random tree in the target map space, adaptively adjust the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability;

[0046] An expansion module, for expanding the bidirectional fast exploration random tree based on the first sampling point;

[0047] A backtracking module is used for, after expansion, if the number of tree nodes of the reverse tree is greater than the number of tree nodes of the forward tree, exchanging the forward tree and the reverse tree with each other and returning to the step of constructing a first super ellipsoid based on the real-time progress of the exploration of the preset bidirectional rapid exploration random tree in the target map space, adaptively adjusting the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sampling to obtain the first sampling point based on the sampling probability, until the reverse tree and the forward tree can be directly connected, merging the forward tree and the reverse tree, and backtracking to obtain the initial path solution and enter step 4;

[0048] The output module is used to calculate the time consumed from the start of the bidirectional rapid exploration random tree exploration to finding the initial path solution, and if the time is not less than a preset time threshold, the initial path solution is output as the optimal solution.

[0049] In a third aspect, the present invention further provides an electronic device, comprising:

[0050] Memory, used to store programs;

[0051] A processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the mobile robot path planning method described in any one of the above method items.

[0052] In a fourth aspect, the present invention also provides a storage medium for storing computer-readable programs or instructions, which, when executed by a processor, can implement the steps in the mobile robot path planning method described in any one of the above-mentioned method items.

[0053] The beneficial effects of the present invention are as follows: the present invention provides a mobile robot path planning method that establishes a hyperellipsoid for sampling based on bidirectional rapid exploration of random trees. The method then expands the dual trees based on the sampling points to merge the dual trees, thereby finding a path solution. Finally, based on the time required to solve the path solution, the method determines whether to output the path solution as the optimal solution, thereby completing the path planning of the mobile robot. The present invention associates the hyperellipsoid with the exploration progress of the random tree, thereby obtaining a new adaptive sampler that can dynamically adjust the sampling strategy based on the exploration progress of the random tree, adaptively balancing the global expansion and local exploration of the random tree, enabling rapid convergence in various environments, further shortening the planning time, and thus resolving the technical problem of low path planning efficiency in complex environments in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative work.

[0055] Figure 1 A schematic diagram of the planning experimental environment provided by the present invention;

[0056] Figure 2 A schematic flow chart of an embodiment of a mobile robot path planning method provided by the present invention;

[0057] Figure 3 for Figure 1 Flow chart of step S201 in an embodiment;

[0058] Figure 4 for Figure 1 A flow chart of an embodiment of step S202;

[0059] Figure 5 A schematic diagram of the random tree bidirectional expansion strategy provided by the present invention;

[0060] Figure 6 A schematic diagram of a flow chart of an embodiment of a path solution optimization method provided by the present invention;

[0061] Figure 7 for Figure 6 A flow chart of an embodiment of step S603;

[0062] Figure 8 A schematic structural diagram of an embodiment of a mobile robot path planning device provided by the present invention;

[0063] Figure 9 This is a structural diagram of an embodiment of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0064] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making any creative efforts shall fall within the scope of protection of the present invention.

[0065] In the description of the embodiments of the present invention, unless otherwise specified, "plurality" means two or more. "And / or" describes the association relationship between associated objects, indicating that three relationships can exist. For example, "A and / or B" can mean: A exists alone, A and B exist simultaneously, or B exists alone.

[0066] The terms "first," "second," and so on, used in the embodiments of the present invention are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, technical features designated as "first" or "second" may explicitly or implicitly include at least one such feature.

[0067] References herein to "embodiments" mean that a particular feature, structure, or characteristic described in connection with the embodiments may be included in at least one embodiment of the present invention. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor does it constitute a separate or alternative embodiment that is mutually exclusive of other embodiments. It is understood, both explicitly and implicitly, by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0068] Before presenting the embodiments, the following terms are explained.

[0069] Bidirectional Rapidly-exploring Random Tree (BRRT): is an improved path planning algorithm designed to solve the traditional rapid exploration random tree ( ) The algorithm has problems such as low efficiency, strong randomness, and slow convergence. By growing two trees from the starting point and the end point to the middle at the same time during the search process, the convergence speed of the algorithm and the efficiency of path search are accelerated.

[0070] The hyperellipsoid is a three-dimensional geometric object that holds a significant place in mathematics and geometry. It is a generalization of the ellipsoid, defined by allowing different exponents of variables in algebraic representations. In data visualization, the hyperellipsoid can be used to represent the distribution of high-dimensional data. By mapping data points onto the hyperellipsoid, clustering and distribution characteristics of the data can be intuitively demonstrated.

[0071] Flag bit: In a forward-threaded binary tree, the flag bit is used to indicate whether the right pointer of the current node points to the right child node or the successor node. In a reverse-threaded binary tree, the flag bit is used to indicate whether the left pointer of the current node points to the left child node or the predecessor node. Flag bit = Trapped , it means that the current tree has explored an obstacle. If the flag bit = Reached, it means that it has successfully expanded from a certain node to a new node or has reached the target node or target area.

[0072] The furthest ancestor state refers to an ancestor node found in the random tree when tracing back from a node (usually the nearest node) to the root node. The connection between this node and the newly generated node does not pass through obstacles and is the farthest node from the new node that meets the conditions.

[0073] The present invention provides a mobile robot path planning method, device, electronic device and storage medium, which are described below respectively.

[0074] The present invention is applied to the path exploration of robots in complex space environments. First, the map space needs to be initialized, that is, the location information, size parameters and other relevant data of the obstacles are input into the map matrix to establish an initial map model; then, based on the predetermined safety distance parameters, the obstacle area is expanded to expand the obstacle boundary range and form an expanded obstacle area with a safety margin. This embodiment adopts The simulated multi-room map environment, the map environment is as follows Figure 1 As shown, the right direction is the positive direction of the X axis, and the upward direction is the positive direction of the Y axis.

[0075] Secondly, set the planning parameters, such as entering the robot's starting position and target location ; Exploration step of random tree Used to control the accuracy of the expansion process; dichotomy threshold , used to determine whether the processing path needs to be further refined; hyperellipsoid parameters , used to define the shape of the hyperellipsoid sampling area; the initial sampling probability within the hyperellipsoid ; Planning time threshold , which is used to control the time cost of the planning process. In this embodiment, the initial position and target position of the robot are (100,720,20) and (30,10,10) respectively, the sampling number is set to 5000 times, and the basic parameters of the experiment are: the exploration step size of the random tree 5 meters; dichotomy threshold is 2 meters, the hyperellipsoid parameters , initial sampling probability within the hyperellipsoid , planning time threshold t 3 seconds.

[0076] Figure 2 A flow chart of an embodiment of the mobile robot path planning method provided by the present invention is shown as follows: Figure 2 As shown, also combined Figure 1 、 Figure 3 、 Figure 4 and Figure 5 ,Mobile robot path planning methods include:

[0077] S201, based on the preset bidirectional rapid exploration random tree, explore the motion path of the mobile robot in the target map space, and construct a first super ellipsoid based on the real-time progress of the bidirectional rapid exploration random tree exploration, adaptively adjust the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sample based on the sampling probability to obtain a first sampling point .

[0078] In some embodiments of the present invention, a bidirectional fast exploration random tree is used to determine the robot's starting position. and target location As the root node, with a preset step size is the exploration step size, and the bidirectional fast exploration random tree is T= ( V , E ) form, where T is the feasible path of the mobile robot explored by the exploration tree, V is a collection of tree nodes, E It is a collection of link relationships between tree nodes.

[0079] It should be noted that in the bidirectional rapid exploration random tree, the starting position The root node is defined as a forward tree with the target position The root node is defined as a reverse tree. The tree nodes explored by the two trees in the map space are the position coordinates in the map space. The link relationship between the tree nodes is to connect these position coordinates in series, thereby representing the direction and position that the mobile robot should reach during the movement process.

[0080] like Figure 3 , step S201 specifically includes:

[0081] S301: Using the two latest tree nodes of the bidirectional rapid exploration random tree in the real-time progress as the two foci of the first super ellipsoid, and determining the major axis length and the minor axis length of the first super ellipsoid based on the spatial distance between the two foci.

[0082] In a specific embodiment, first, the node most recently explored by the dual tree X explore_a and X explore_b As the focus of the hyperellipsoid, where the major axis of the hyperellipsoid is:

[0083] (1);

[0084] Other minor axes:

[0085] (2).

[0086] S302 : Based on the ratio of the spatial distance from the robot's starting position to the target position and the major axis length, the sampling probabilities of random sampling within the first hyperellipsoid and random sampling in the target map space are adjusted on the basis of a preset sampling probability.

[0087] In a specific embodiment, the sampling probability is dynamically adjusted according to the size of the hyperellipsoid, using the parameter P in Divide [0,1] into two intervals, representing the probability of random sampling within the hyperellipsoid and random sampling in the entire map space, where:

[0088] (3)

[0089] S303: Generate a random number in the range of [0, 1]. If the random number is greater than the adjusted sampling probability, random sampling is performed within the first super ellipsoid; otherwise, random sampling is performed in the target map space.

[0090] In a specific embodiment, a random number function is used to generate a random number in the range [0,1]. ,if If , then random sampling is performed within the hyperellipsoid, otherwise random sampling is performed in the entire map space.

[0091] Furthermore, the random sampling method used within the hyperellipsoid is:

[0092] First, randomly generate the azimuth angle in the spherical coordinate system and elevation , respectively used to describe the projection direction of the XY plane and the direction of the Z axis, generating a random radius .

[0093] Next, convert the spherical coordinates to Cartesian coordinates:

[0094] (4)

[0095] Then, calculate the rotation axis and rotation angle:

[0096] Use unit vector Indicates the target direction. Assuming the initial direction is the X axis, the rotation coordinate axis is as follows:

[0097] (5)

[0098] in, .

[0099] Then, the calculated rotation axis is normalized.

[0100] The rotation angle is determined by the dot product of the two vectors. The specific formula is as follows:

[0101] (6)

[0102] Preferably, the Rodrigues formula is used to return the rotation matrix of the spherical coordinate system to the hyperellipsoidal coordinate system , the specific formula is as follows:

[0103] (7)

[0104] in, I is the identity matrix, K is the rotation axis u The antisymmetric matrix of .

[0105] Finally, the rotation matrix Act on the vector coordinates of the point to get the sampling point :

[0106] (8)

[0107] S202: Expand the bidirectional rapid exploration random tree based on the dichotomy method and the first sampling point.

[0108] In order to make the path after the bidirectional rapid exploration random tree is expanded smoother and reduce unnecessary branches or nodes, in some embodiments of the present invention, step S202 includes: generating a new node based on the first sampling point and adding it to the bidirectional rapid exploration random tree, and using a bisection method to smoothly prune the bidirectional rapid exploration random tree after the new node is added.

[0109] Furthermore, if Figure 4 In some embodiments of the present invention, step S202 specifically includes:

[0110] S401, traverse and calculate the node set output by the forward tree From each node to the first sampling point The Euclidean distance of

[0111] S402: Select the node with the smallest distance as the first starting state for the forward growth of the forward tree , based on the preset step size towards the first sampling point Perform single-step exploration to obtain the first new node ;

[0112] S403: If no obstacles are detected during the expansion of the current forward tree, return to the first starting state in the growth process of the current forward tree. The first most distant ancestor state that can be traced back ;

[0113] Preferably, return the forward tree flag ,like , it means that the forward tree did not detect any obstacles during this expansion process and the current path is feasible.

[0114] S404, in the first most distant ancestor state and the first starting state Create the first parent node based on bisection Replace the first starting state .

[0115] S405: If the first starting state after replacement , the first most distant ancestor state and the first new node The first starting state between the three points The first angle of the vertex does not reach the preset dichotomy threshold , then in the first most distant ancestor state and the first starting state after replacement Create a new first parent node based on bisection Replace the first starting state after the above replacement , repeat until the first angle reaches the dichotomy threshold Or an obstacle is detected on the planned path.

[0116] It should be noted that if Figure 5 ,Will 、 、 Connected in sequence, the line is connected The angle of the vertex is the reference angle, and then Replace with After that, the reference angle becomes significantly larger. According to the triangle inequality, the larger the angle between the three points, the smaller the distance between the three points, and thus the total distance of the path on the random tree is also smaller.

[0117] S406: Based on the first new node , the first most distant ancestor state and the first parent node Update the forward tree.

[0118] In a specific embodiment, the forward tree is updated as follows:

[0119] Update the forward tree if , then:

[0120] (9)

[0121] otherwise:

[0122] (10)

[0123] S407, traverse and calculate the node set output by the reverse tree From each node to the first new node The Euclidean distance of

[0124] S408: Select the node with the smallest distance as the second starting state of reverse growth , towards the first new node Explore without step size limit until an obstacle or a double tree connection is detected and the second new node is obtained ;

[0125] S409: Return to the second starting state during this growth process The second most distant ancestor state that can be traced back ;

[0126] S410, in the second most distant ancestor state and the second starting state Create the second parent node based on bisection Replace the second starting state ;

[0127] S411, if the second starting state after replacement , the second most distant ancestor state and the second new node The second starting state between the three points The second angle of the vertex does not reach the preset dichotomy threshold , then in the second most distant ancestor state and the second starting state after replacement Create a new second parent node based on bisection Replace the second starting state after the above replacement , repeat until the second angle reaches the dichotomy threshold D dichotomy Or an obstacle is detected on the planned path.

[0128] S412, based on the second new node , the second most distant ancestor state and the second parent node Update the reverse tree.

[0129] Specifically, the updating process of the reverse tree is the same as above.

[0130] It should be noted that as an efficient search algorithm, the bisection method can quickly locate the target value by gradually reducing the search range to half. In the process of pruning random trees, the bisection method can be used to optimize the tree structure, which can effectively reduce redundant nodes or paths and optimize the topological structure of the random tree, thereby accelerating the convergence of the algorithm.

[0131] S203. After expansion, if the reverse tree and the forward tree cannot be directly connected, and the number of tree nodes in the reverse tree is greater than the number of tree nodes in the forward tree, the forward tree and the reverse tree are exchanged with each other and the process returns to step S201 until the reverse tree and the forward tree can be directly connected. The forward tree and the reverse tree are merged and backtracked to obtain the initial path solution and the process proceeds to step S204.

[0132] In a specific embodiment, if the number of nodes in the reverse tree is greater than the number of nodes in the forward tree, that is, , then swap the two trees, that is , in order to overcome the performance instability caused by the rapid expansion of the reverse tree without step size limit. Loop calculation until the reverse tree flag is , merge the double trees, according to the set of link relationships between tree nodes E Backtrack to calculate the initial path solution , and the initial path solution The output form is still ， is a collection of tree nodes, It is a collection of link relationships between tree nodes.

[0133] S204: Calculate the time consumed from step S201 to the end of the loop in step S203 t , if time t Not less than the preset time threshold , then the initial path solution As the optimal solution Output.

[0134] Compared to existing technologies, the present invention provides a mobile robot path planning method that establishes a hyperellipsoid for sampling based on bidirectional rapid exploration of random trees. The method then expands the dual trees based on the sampling points to merge them, thereby finding a path solution. Finally, based on the time required to find the path solution, the method determines whether to output the path solution as the optimal solution, thereby completing the mobile robot's path planning. The present invention associates the hyperellipsoid with the output path of the random tree, resulting in a new adaptive sampler that dynamically adjusts the sampling strategy based on the exploration progress of the random tree, adaptively balancing the random tree's global expansion and local exploration. It also converges rapidly even in complex environments, further shortening planning time and addressing the technical issue of low path planning efficiency in complex environments encountered by existing technologies.

[0135] To prevent the algorithm from converging prematurely and failing to obtain a globally optimal path solution, in some embodiments of the present invention, the mobile robot path planning method further includes:

[0136] If the initial path solution is found Time consumed t Less than the preset time threshold , then based on the initial path solution The first node set Solution for the initial path Optimize.

[0137] like Figure 6 In some embodiments of the present invention, the optimization process specifically includes:

[0138] S601, starting position of the robot is the first focus, target position As the second focus, a second hyperellipsoid is constructed, where the sum of the Euclidean distances from any point in the second hyperellipsoid to the first focus and the second focus is not greater than the preset current optimal path cost value .

[0139] In a specific embodiment, the general equation of the second hyperellipsoid is:

[0140] (11)

[0141] It is understandable that the major axis and minor axis of the second hyperellipsoid will be updated in real time as the better path is solved, but the focus is a fixed point to ensure its convergence.

[0142] S602: Randomly sample within the second super ellipsoid to obtain a second sampling point .

[0143] It should be noted that the random sampling method in the second super ellipsoid is consistent with that in the first super ellipsoid.

[0144] S603, based on the second sampling point Use bisection method to solve the initial path Optimize.

[0145] Furthermore, if Figure 7 In some embodiments of the present invention, step S603 includes:

[0146] S701, traverse and calculate the first set From each tree node to the second sampling point Euclidean distance, get the tree node with the minimum distance .

[0147] It should be noted that, since the present invention adopts the dichotomy method, the number of nodes in the planned path set is very small, so it is necessary to In other special embodiments, if there are a large number of nodes in the planned path set, considering the uniformity and representativeness of the sampling points, the optimization radius can be set with the second sampling point as the center, and the first set can be optimized. V P The nodes within this radius are traversed and optimized.

[0148] S702, the first set All tree nodes in the robot are connected one by one in the direction from the starting position to the target position, and the tree node with the smallest distance in the connection is measured. is the size of the third angle of the vertex;

[0149] S703, the second sampling point Replace the node with the smallest distance Later update the link relationship between tree nodes , get the latest path solution ;

[0150] S704, solve the latest path All tree nodes in the direction from the robot's starting position to the target position are connected one by one, and the second sampling point in the connection is measured. is the size of the fourth angle of the vertex;

[0151] S705: If the fourth angle is not greater than the third angle, return to step S702 until the fourth angle is greater than the third angle;

[0152] S706, use the latest path solution Replace the initial path solution .

[0153] In summary, the present invention provides a path planning method for a mobile robot. The main contents of the method include: (1) designing a new adaptive sampler, which can dynamically adjust the sampling strategy according to the exploration progress of the random tree, adaptively balance the global expansion and local exploration of the random tree, and further shorten the planning time. (2) simplifying This approach combines the asymptotically optimal process with a bisection method to expand it into a faster-converging bidirectional programming approach, effectively optimizing the topological structure of random trees. Furthermore, because the proposed tree expansion algorithm is a tree expansion algorithm, it can be combined with other samplers and graph pruning methods. This method can quickly obtain a good initial solution in various environments and ensures rapid convergence to the optimal solution, thus possessing significant engineering value.

[0154] The technical effects of the present invention are further demonstrated below with reference to a specific embodiment. Figure 1 The sampling test map and basic parameters of the sampling experiment are used + As a comparison algorithm, As a sampling strategy, as a tree pruning strategy, where It represents the path planning method provided by the present invention.

[0155] The quality of the initial paths generated by the algorithm is compared using seven evaluation metrics. The first four metrics are used to evaluate the algorithm's ability to plan an initial solution: is the time to obtain the initial solution, is the Euclidean distance cost of the initial solution, represents the number of nodes used in the initial solution of the plan, The last three indicators are used to evaluate the convergence speed of the algorithm: preferably, T 5% The cost is time, of which is the cost of the optimal solution, and it is approximately assumed that the path cost converges to That is, the optimal solution is obtained; Indicates the number of nodes used in planning the optimal solution, The success rate of finding the optimal solution within the specified time is shown in Table 1.

[0156] Table 1 Experimental data

[0157]

[0158] like Figure 8 In a second aspect, the present invention further provides a mobile robot path planning device 80, comprising:

[0159] Sampling module 810, configured to construct a first super ellipsoid based on the real-time progress of exploration of the target map space by a preset bidirectional rapid exploration random tree, adaptively adjust the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability;

[0160] An expansion module 820, configured to expand the bidirectional rapid exploration random tree based on the first sampling point;

[0161] Backtracking module 830 is used to, after expansion, if the number of tree nodes in the reverse tree is greater than the number of tree nodes in the forward tree, swap the forward tree and the reverse tree and return to step S201 until the reverse tree and the forward tree can be directly connected, merge the forward tree and the reverse tree, backtrack to obtain the initial path solution, and then enter step S204;

[0162] The output module 840 is used to calculate the time consumed from the start of the bidirectional rapid exploration random tree exploration to the finding of the initial path solution. If the time is not less than a preset time threshold, the initial path solution is output as the optimal solution.

[0163] like Figure 9 In a third aspect, the present invention further provides an electronic device 90, comprising:

[0164] Memory 910, used for storing programs;

[0165] The processor 920 is coupled to the memory 910 and is used to execute the program stored in the memory 910 to implement the steps in the mobile robot path planning method described in any one of the above method items.

[0166] In a fourth aspect, the present invention also provides a storage medium for storing computer-readable programs or instructions, which, when executed by a processor, can implement the steps in the mobile robot path planning method described in any one of the above-mentioned method items.

[0167] The above is a detailed introduction to the mobile robot path planning method, device, electronic device and storage medium provided by the present invention. Specific examples are used in this article to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core ideas. At the same time, for technical personnel in this field, according to the ideas of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

Claims

1. A mobile robot path planning method, characterized in that: include: Step 1: Explore the motion path of the mobile robot in the target map space based on a preset bidirectional rapid exploration random tree, construct a first super ellipsoid based on the real-time progress of the bidirectional rapid exploration random tree exploration, adaptively adjust the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability; Step 2: Expand the bidirectional rapid exploration random tree based on the bisection method and the first sampling point; Step 3: After expansion, if the reverse tree and the forward tree cannot be directly connected, and the number of tree nodes in the reverse tree is greater than the number of tree nodes in the forward tree, exchange the forward tree and the reverse tree and return to step 1 until the reverse tree and the forward tree can be directly connected. Merge the forward tree and the reverse tree and backtrack to obtain the initial path solution and enter step 4; Step 4: Calculate the time consumed from step 1 to the end of the loop of step 3. If the time is not less than a preset time threshold, output the initial path solution as the optimal solution. The bidirectional rapid exploration random tree takes the robot's starting position and target position as root nodes, takes a preset step length as the exploration step length, and the bidirectional rapid exploration random tree takes T =( V , E ) outputs the real-time progress of the exploration in the form of T It is a fast bidirectional exploration of feasible paths for mobile robots explored by random trees. V is a collection of tree nodes, E It is a collection of link relationships between tree nodes; The method explores the motion path of the mobile robot in the target map space based on a preset bidirectional rapid exploration random tree, constructs a first super ellipsoid based on the real-time progress of the bidirectional rapid exploration random tree exploration, adaptively adjusts the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and obtains a first sampling point by random sampling based on the sampling probability, including: The two latest tree nodes of the bidirectional rapid exploration random tree in the real-time progress are used as two foci of the first super ellipsoid, and the major axis length and the minor axis length of the first super ellipsoid are determined based on the spatial distance between the two foci; Adjusting the sampling probabilities of random sampling within the first hyperellipsoid and random sampling in the target map space based on a preset sampling probability based on the ratio of the spatial distance from the robot's starting position to the target position and the major axis length; Generate a random number in the range of [0, 1]. If the random number is greater than the adjusted sampling probability, random sampling is performed within the first super ellipsoid; otherwise, random sampling is performed in the target map space.

2. The mobile robot path planning method according to claim 1, characterized in that: The bidirectional fast exploration random tree based on the dichotomy method and the first sampling point is extended, including: A first new node is generated based on the first sampling point and added to the bidirectional rapid exploration random tree, and a bisection method is used to smoothly prune the bidirectional rapid exploration random tree after the first new node is added.

3. The mobile robot path planning method according to claim 2, characterized in that: The generating of a first new node based on the first sampling point and adding the node to the bidirectional rapid exploration random tree, and performing smooth pruning on the bidirectional rapid exploration random tree after adding the first new node by using a bisection method, includes: Traversing and calculating the Euclidean distance between each tree node in the node set output by the forward tree and the first sampling point; Select the node with the smallest distance as the first starting state of the forward growth of the forward tree, and perform a single-step exploration towards the first sampling point based on the preset step size to obtain the first new node; If no obstacle is detected during the expansion of the current forward tree, then the first furthest ancestor state that can be traced back to the first starting state during the growth of this forward tree is returned; Creating a first parent node between the first furthest ancestor state and the first starting state based on bisection to replace the first starting state; If the size of the first angle between the replaced first starting state, the first furthest ancestor state, and the first new node with the first starting state as the vertex does not reach a preset dichotomy threshold, then a new first parent node is created between the first furthest ancestor state and the replaced first starting state based on dichotomy to replace the replaced first starting state, and the cycle is repeated until the size of the first angle reaches the dichotomy threshold or an obstacle is detected on the planned path; updating the forward tree based on the first new node, the first furthest ancestor state, and the first parent node; Traversing and calculating the Euclidean distance between each node in the node set output by the reverse tree and the first new node; Selecting the node with the smallest distance as the second starting state of reverse growth, exploring toward the first new node without adding a step size limit until an obstacle or a dual-tree connection is detected, thereby obtaining a second new node; Return to the second furthest ancestor state that the second starting state can trace back to during this growth process; Creating a second parent node between the second furthest ancestor state and the second starting state based on bisection to replace the second starting state; If the size of the second angle between the replaced second starting state, the second furthest ancestor state, and the second new node with the second starting state as the vertex does not reach a preset dichotomy threshold, then a new second parent node is created between the second furthest ancestor state and the replaced second starting state based on dichotomy to replace the replaced second starting state, and the cycle is repeated until the size of the second angle reaches the dichotomy threshold or an obstacle is detected on the planned path; The inverted tree is updated based on the second new node, the second furthest ancestor state, and the second parent node.

4. The mobile robot path planning method according to claim 1, characterized in that: The mobile robot path planning method further includes: If the time is less than a preset time threshold, the initial path solution is optimized based on the first set of tree nodes of the initial path solution.

5. The mobile robot path planning method according to claim 4, characterized in that: The step of optimizing the initial path solution based on the first set of tree nodes of the initial path solution includes: A second hyperellipsoid is constructed with the robot's starting position as the first focus and the robot's target position as the second focus, wherein the sum of the spatial distances from any point within the second hyperellipsoid to the first focus and the second focus is no greater than a preset current optimal path cost value; Randomly sampling within the second super ellipsoid to obtain a second sampling point; The initial path solution is optimized using a bisection method based on the second sampling point.

6. The mobile robot path planning method according to claim 5, characterized in that: The optimizing the initial path solution by using a bisection method based on the second sampling point includes: Traverse and calculate the Euclidean distance from each tree node in the first set to the second sampling point, and obtain the tree node with the minimum distance; Connect all tree nodes in the first set one by one in the direction from the robot's starting position to the target position, and measure the size of the third angle in the connection line with the tree node with the smallest distance as the vertex; Replace the tree node with the smallest distance with the second sampling point and update the link relationship between tree nodes to obtain the latest path solution; Connect all tree nodes in the latest path solution one by one in the direction from the robot's starting position to the target position, and measure the size of the fourth angle in the connecting line with the second sampling point as the vertex; If the fourth angle is not greater than the third angle, returning to the step of randomly sampling within the second super ellipsoid to obtain a second sampling point, until the fourth angle is greater than the third angle; Replace the initial path solution with the latest path solution.

7. A mobile robot path planning device, characterized in that: include: A sampling module is used to construct a first super ellipsoid based on the real-time progress of exploration of the preset bidirectional rapid exploration random tree in the target map space, adaptively adjust the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability; An expansion module, for expanding the bidirectional fast exploration random tree based on the first sampling point; A backtracking module is used for, after expansion, if the number of tree nodes of the reverse tree is greater than the number of tree nodes of the forward tree, exchanging the forward tree and the reverse tree with each other and returning to the step of constructing a first super ellipsoid based on the real-time progress of the exploration of the preset bidirectional rapid exploration random tree in the target map space, adaptively adjusting the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and randomly sampling to obtain the first sampling point based on the sampling probability, until the reverse tree and the forward tree can be directly connected, merging the forward tree and the reverse tree, and backtracking to obtain the initial path solution and enter step 4; An output module is used to calculate the time consumed from the start of the bidirectional rapid exploration random tree exploration to finding the initial path solution, and if the time is not less than a preset time threshold, output the initial path solution as the optimal solution; The bidirectional rapid exploration random tree takes the robot's starting position and target position as root nodes, takes a preset step length as the exploration step length, and the bidirectional rapid exploration random tree takes T =( V , E ) outputs the real-time progress of the exploration in the form of T It is a fast bidirectional exploration of feasible paths for mobile robots explored by random trees. V is a collection of tree nodes, E It is a collection of link relationships between tree nodes; The method explores the motion path of the mobile robot in the target map space based on a preset bidirectional rapid exploration random tree, constructs a first super ellipsoid based on the real-time progress of the bidirectional rapid exploration random tree exploration, adaptively adjusts the sampling probability of the first super ellipsoid according to the size of the first super ellipsoid, and obtains a first sampling point by random sampling based on the sampling probability, including: The two latest tree nodes of the bidirectional rapid exploration random tree in the real-time progress are used as two foci of the first super ellipsoid, and the major axis length and the minor axis length of the first super ellipsoid are determined based on the spatial distance between the two foci; Adjusting the sampling probabilities of random sampling within the first hyperellipsoid and random sampling in the target map space based on a preset sampling probability based on the ratio of the spatial distance from the robot's starting position to the target position and the major axis length; Generate a random number in the range of [0, 1]. If the random number is greater than the adjusted sampling probability, random sampling is performed within the first super ellipsoid; otherwise, random sampling is performed in the target map space.

8. An electronic device, characterized in that: include: Memory, used to store programs; A processor, coupled to the memory, is configured to execute the program stored in the memory to implement the steps of the mobile robot path planning method according to any one of claims 1 to 6.

9. A storage medium, characterized in that: Used to store computer-readable programs or instructions, which, when executed by a processor, can implement the steps in the mobile robot path planning method described in any one of claims 1 to 6.

Images