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

Abstract

The invention provides a mobile robot path planning method and device, electronic equipment and a storage medium, and belongs to the technical field of robot motion control path planning, and the method comprises the steps: constructing a first hyper-ellipsoid based on the real-time progress of bidirectional rapid exploration random tree exploration, adaptively adjusting the sampling probability of the first hyper-ellipsoid according to the size of the first hyper-ellipsoid, and randomly sampling to obtain a first sampling point; expanding the bidirectional rapid exploration random tree; after expansion, if direct connection cannot be realized and the number of tree nodes of the reverse tree is greater than the number of tree nodes of the forward tree, the forward tree and the reverse tree are mutually exchanged and then the step 1 is executed until the forward tree and the reverse tree can be directly connected, and the forward tree and the reverse tree are combined to obtain an initial path solution; and calculating the time consumed by the end of the loop, and if the time is not less than a time threshold, outputting the initial path solution as an optimal solution. The technical problem that in the prior art, the path planning efficiency is low when a complex environment is processed is effectively solved.

Description

Technical Field

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

[0002] With the rapid development of robot technology, how to achieve efficient, safe and optimized motion planning in space has become one of the research hotspots. At present, motion planning algorithms have been widely applied in various fields, including but not limited to robot navigation, industrial automation, aerospace satellite orbit planning, and logistics and warehousing automation. To efficiently complete the planning task, a variety of path planning algorithms have been proposed. Among them, sampling-based planning algorithms represented by the Rapidly-exploring Random Tree ( ) show excellent robustness and adaptability in high-dimensional spaces.

[0003] The algorithm is based on randomly sampling the unknown space, extracting spatial information, and exploring the space state through the incremental growth of a tree. After continuously iterating 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 extremely high flexibility in dealing with high-dimensional and non-linear problems and has a quite wide application in robot systems with complex degrees of freedom and multiple constraints, covering driverless vehicles ( ), robotic arms ( ), unmanned aerial vehicles ( ), and surface vehicles ( ). Karaman and Frazzoli proposed the algorithm in "Sampling-based algorithms for optimal motion planning". As one of the optimal variants of the algorithm, after establishing the trajectory, it will search for a better connection method in the neighborhood around the sampling point and has asymptotic optimality. When the number of iterations approaches infinity, the probability of finding the optimal solution approaches 100%. However,

[0004] However, The convergence effect depends to a large extent on the quality of random sampling. If there is insufficient sampling in a specific area or the sampling in a local area is too redundant, the planning efficiency will be significantly reduced. Existing technologies effectively constrain the sampling space by constructing a hyper-ellipsoid, avoiding the problem of over-exploration of the random tree, enabling it to better approximate the optimal solution within a limited time. Another existing technology uses the bisection method to create growing parent nodes, optimizing the pruning of tree nodes in the local area and significantly improving the quality of the initial solution. However, the planning efficiency of the above algorithms is still insufficient to handle 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 path planning method, device, electronic device and storage medium for a mobile robot to solve the technical problem of low path planning efficiency of existing technologies when dealing with complex environments.

[0006] To solve the above technical problem, in the first aspect, the present invention provides a path planning method for a mobile robot, including: Step 1: Explore the movement path of the mobile robot in the target map space based on a preset bidirectional rapidly-exploring random tree, construct a first hyper-ellipsoid based on the real-time progress of the exploration of the bidirectional rapidly-exploring random tree, adaptively adjust the sampling probability of the first hyper-ellipsoid according to the size of the first hyper-ellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability; Step 2: Expand the bidirectional rapidly-exploring random tree based on 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 of the reverse tree is greater than the number of tree nodes of the forward tree, exchange the forward tree and the reverse tree with each other 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 an initial path solution and enter Step 4; Step 4: Calculate the time consumed from the start of Step 1 until the end of the loop in Step 3. If the time is not less than a preset time threshold, output the initial path solution as the optimal solution.

[0007] In some embodiments of the present invention, the bidirectional rapidly-exploring random tree uses the starting position and the target position of the robot as the root nodes, uses a preset step size as the exploration step size, and the bidirectional rapidly-exploring random tree 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 rapidly-exploring random tree, V is the set of tree nodes, and E is the set of link relationships between tree nodes; Explore the motion path of a mobile robot in the target map space based on a preset bidirectional rapidly-exploring random tree (RRT), construct a first super-ellipsoid based on the real-time progress of the exploration by the bidirectional RRT, 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, including: Use the two latest tree nodes of the bidirectional RRT in the real-time progress as the two foci of the first super-ellipsoid, and determine the major axis length and minor axis length of the first super-ellipsoid based on the spatial distance between the two foci; Adjust the sampling probabilities of random sampling within the first super-ellipsoid and random sampling in the target map space based on the ratio of the spatial distance from the starting position of the robot to the target position to the major axis length on the basis of a preset sampling probability; Generate a random number with a value range of [0, 1]. If the random number is greater than the adjusted sampling probability, randomly sample within the first super-ellipsoid; otherwise, randomly sample in the target map space.

[0008] In some embodiments of the present invention, expanding the bidirectional RRT based on the first sampling point includes: Generate a new node based on the first sampling point and add it to the bidirectional RRT, and perform smooth pruning on the bidirectional RRT after adding the new node using the bisection method.

[0009] In some embodiments of the present invention, generating a new node based on the first sampling point and adding it to the bidirectional RRT, and performing smooth pruning on the bidirectional RRT after adding the new node using the bisection method includes: Traverse and calculate the Euclidean distance from each tree node in the node set output by the forward tree to the first sampling point; Select the node with the minimum distance as the first starting state for the forward growth of the forward tree, and perform a single-step exploration towards the first sampling point based on a preset step size to obtain a first new node; If no obstacle is detected during the expansion of the current forward tree, return the first farthest ancestor state that the first starting state can trace back to during this forward tree growth process; Create a first parent node between the first farthest ancestor state and the first starting state based on the bisection method to replace the first starting state; If the first included angle with the first starting state as the vertex among the three points of the replaced first starting state, the first farthest ancestor state, and the first new node does not reach the preset bisection threshold, create a new first parent node between the first farthest ancestor state and the replaced first starting state to replace the replaced first starting state, and repeat this process until the first included angle reaches the bisection threshold or an obstacle is detected on the planned path; Update the forward tree based on the first new node, the first farthest ancestor state, and the first parent node; Traverse and calculate the Euclidean distance from each node in the node set output by the backward tree to the first new node; Select the node with the minimum distance as the second starting state for backward growth, and explore towards the first new node without step size limitation until an obstacle or a double-tree connection is detected to obtain a second new node; Return the second farthest ancestor state that the second starting state can trace back to during this growth process; Create a second parent node between the second farthest ancestor state and the second starting state based on the bisection method to replace the second starting state; If the second angle with the second starting state as the vertex among the three points of the replaced second starting state, the second farthest ancestor state, and the second new node does not reach the preset bisection threshold, then create a new second parent node between the second farthest ancestor state and the replaced second starting state to replace the replaced second starting state, and repeat the process until the second angle reaches the bisection threshold or an obstacle is detected on the planned path; Update the backward tree based on the second new node, the second farthest ancestor state, and the second parent node.

[0010] In some embodiments of the present invention, the mobile robot path planning method further includes: If the time is less than the preset time threshold, optimize the initial path solution based on the first set of tree nodes of the initial path solution.

[0011] 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: Construct a second super-ellipsoid with the target position of the robot as the first focus and other nodes in the first set except the target position as the second focus, where the sum of the spatial distances from any point inside the second super-ellipsoid to the first focus and the second focus is not greater than the preset current optimal path cost value; Randomly sample to obtain a second sampling point inside the second super-ellipsoid; Optimize the initial path solution based on the second sampling point using the bisection method.

[0012] In some embodiments of the present invention, optimizing the initial path solution based on the second sampling point using the bisection method includes: Traverse and calculate the Euclidean distance from each tree node in the first set to the second sampling point to obtain the tree node with the minimum distance; Connect all the tree nodes in the first set one by one along the direction from the starting position of the robot to the target position, and measure the size of the third included angle with the tree node having the minimum distance as the vertex in the connection line; Replace the tree node with the minimum distance with the second sampling point, and update the link relationship between tree nodes to obtain the latest path solution; Connect all the tree nodes in the latest path solution one by one along the direction from the starting position of the robot to the target position, and measure the size of the fourth included angle with the second sampling point as the vertex in the connection line; If the fourth included angle is not greater than the third included angle, return to the step of randomly sampling the second sampling point within the second super-ellipsoid until the fourth included angle is greater than the third included angle; Replace the initial path solution with the latest path solution.

[0013] In a second aspect, the present invention further provides a mobile robot path planning device, including: A sampling module, configured to construct a first super-ellipsoid based on the real-time progress of exploring in the target map space by a preset bidirectional rapidly-exploring random tree, adaptively adjust the sampling probability of the first super-ellipsoid according to the size of the first super-ellipsoid, and randomly sample a first sampling point based on the sampling probability; An expanding module, configured to expand the bidirectional rapidly-exploring random tree based on the first sampling point; A backtracking module, after expansion, if the number of tree nodes of the reverse tree is greater than the number of tree nodes of the forward tree, exchange the forward tree and the reverse tree with each other and return to the step of constructing a first super-ellipsoid based on the real-time progress of exploring in the target map space by a preset bidirectional rapidly-exploring random tree, adaptively adjust the sampling probability of the first super-ellipsoid according to the size of the first super-ellipsoid, and randomly sample a first sampling point based on the sampling probability, 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 four; An output module, configured to calculate the time consumed from the start of exploration of the bidirectional rapidly-exploring random tree until the initial path solution is found, and if the time is not less than a preset time threshold, output the initial path solution as the optimal solution.

[0014] In a third aspect, the present invention further provides an electronic device, including: A memory, configured to store a program; A processor, coupled to the memory, configured 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.

[0015] Fourthly, the present invention further provides a storage medium for storing computer-readable programs or instructions, and when the programs or instructions are executed by a processor, the steps in the mobile robot path planning method described in any one of the above method items can be implemented.

[0016] The beneficial effects of the present invention are as follows: The present invention provides a mobile robot path planning method, which establishes a superellipsoid for sampling on the basis of the bidirectional rapidly-exploring random tree exploration, then expands the two trees according to the sampling points to merge the two trees, so as to find a path solution, and finally determines whether to output the path solution as the optimal solution according to the time required to solve the path solution, thereby completing the path planning of the mobile robot. The present invention correlates the exploration progress of the superellipsoid with that of the random tree, thereby obtaining 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, can quickly converge in various environments, further shortens the planning time, and thus solves the technical problem of low path planning efficiency in the prior art when dealing with complex environments. BRIEF DESCRIPTION OF THE DRAWINGS In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present invention, and those skilled in the art can obtain other drawings without creative efforts based on these drawings.

[0017] Figure 1 It is a schematic diagram of the planning experimental environment provided by the present invention; Figure 2 It is a schematic flowchart of an embodiment of the mobile robot path planning method provided by the present invention; Figure 3 For Figure 1 a schematic flowchart of an embodiment of step S201 in Figure 4 For Figure 1 a schematic flowchart of an embodiment of step S202 in Figure 5 It is a schematic diagram of the random tree bidirectional expansion strategy provided by the present invention; Figure 6 It is a schematic flowchart of an embodiment of the path solution optimization method provided by the present invention; Figure 7 For Figure 6 a schematic flowchart of an embodiment of step S603 in Figure 8 It is a schematic structural diagram of an embodiment of the mobile robot path planning device provided by the present invention; Figure 9Schematic structural diagram of an embodiment of the electronic device provided by the present invention. Detailed implementation manners

[0018] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative efforts fall within the protection scope of the present invention.

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

[0020] The descriptions such as "first" and "second" involved in the embodiments of the present invention are only for the purpose of description, and cannot be understood as indicating or implying their relative importance or implicitly indicating the quantity of the indicated technical features. Therefore, the technical features defined with "first" and "second" may explicitly or implicitly include at least one such feature.

[0021] Referring to "embodiment" in this article means that the specific features, structures or characteristics described in connection with the embodiment may be included in at least one embodiment of the present invention. The phrase appears in various positions in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

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

[0023] Bidirectional Rapidly-exploring Random Tree (BRRT): An improved path planning algorithm designed to solve problems such as low efficiency, strong randomness, and slow convergence speed in the traditional Rapidly-exploring Random Tree ( algorithm. By growing two trees simultaneously from the starting point and the ending point towards the middle during the search process, the convergence speed of the algorithm and the path search efficiency are thus accelerated.

[0024] Hyperellipsoid: A three-dimensional geometric body that holds an important position in mathematics and geometry. It is a generalization of the ellipsoid and is defined by allowing different exponents for variables in the algebraic representation. In the field of data visualization, hyperellipsoids can be used to represent the distribution of high-dimensional data. By mapping data points onto hyperellipsoids, the clustering and distribution characteristics of the data can be intuitively displayed.

[0025] 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 backward 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 indicates that an obstacle has been encountered in the current tree exploration. If the flag bit = Reached it indicates that a new node has been successfully expanded from a certain node or the target node or target area has been reached.

[0026] Farthest ancestor state: In a random tree, it refers to an ancestor node found during the process of tracing back from a certain node (usually the nearest node) to the root node. The connection line between this node and the newly generated node does not pass through an obstacle, and it is the farthest node that meets the condition from the new node.

[0027] The present invention provides a path planning method, device, electronic device, and storage medium for a mobile robot, which will be described separately below.

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

[0029] Secondly, set the planning parameters, such as inputting the starting position and the target position of the robot; the exploration step size of the random tree is used to control the accuracy of the expansion process; the bisection threshold , which is used to determine whether further path refinement is required; the hyperellipsoid parameter , which is used to define the shape of the hyperellipsoid sampling area; the initial sampling probability inside the hyperellipsoid; the planning time threshold , used to control the time cost of the planning process. In this embodiment, the initial position and the target position of the robot are (100, 720, 20) and (30, 10, 10) respectively, the number of sampling times is set to 5000 times, and the basic parameters of the experiment are: the exploration step size of the random tree is 5 meters; the bisection method threshold is 2 meters, the super-ellipsoid parameter , the initial sampling probability inside the super-ellipsoid , the planning time threshold t is 3 seconds.

[0030] Figure 2 is a schematic flowchart of an embodiment of the mobile robot path planning method provided by the present invention. As Figure 2 shown, also combined with Figure 1 , Figure 3 , Figure 4 and Figure 5 , the mobile robot path planning method includes: S201. Explore the motion path of the mobile robot in the target map space based on a preset bidirectional rapidly-exploring random tree, construct a first super-ellipsoid based on the real-time progress of the exploration of the bidirectional rapidly-exploring random tree, 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 .

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

[0032] It should be noted that in the bidirectional rapidly-exploring random tree, the one with the starting position as the root node is defined as the forward tree, and the one with the target position as the root node is defined as the reverse tree. The tree nodes explored by the two trees in the map space are the position coordinates in the map space, and the link relationships between the tree nodes are to concatenate these position coordinates, so as to represent in what direction the mobile robot should move to what position during the movement.

[0033] As Figure 3, step S201 specifically includes: S301. Take the two latest tree nodes of the bidirectional rapidly-exploring random tree in the real-time progress as the two foci of the first super-ellipsoid, and determine the major axis length and minor axis length of the first super-ellipsoid based on the spatial distance between the two foci.

[0034] In a specific embodiment, first, take the nodes X explore_a and X explore_b explored by the double tree most recently as the foci of the super-ellipsoid, where the major axis of the super-ellipsoid: (1); The remaining minor axis: (2).

[0035] S302. Adjust the sampling probabilities of random sampling within the first super-ellipsoid and random sampling in the target map space based on the ratio of the spatial distance from the starting position to the target position of the robot to the major axis length on the basis of a preset sampling probability.

[0036] In a specific embodiment, dynamically adjust the sampling probability according to the size of the super-ellipsoid, and use the parameter P in to divide [0, 1] into two intervals, respectively representing the probabilities of random sampling within the super-ellipsoid and random sampling in the entire map space, where: (3) S303. Generate a random number with a value range of [0, 1]. If the random number is greater than the adjusted sampling probability, randomly sample within the first super-ellipsoid; otherwise, randomly sample in the target map space.

[0037] In a specific embodiment, use a random number function to generate a random number within [0, 1] , if then randomly sample within the super-ellipsoid; otherwise, randomly sample in the entire map space.

[0038] Furthermore, the method of random sampling within the super-ellipsoid is: First, randomly generate the azimuth angle and elevation angle in the spherical coordinate system, which are respectively used to describe the projection direction in the X-Y plane and the direction of the Z axis, and generate a random radius .

[0039] Next, convert the spherical coordinates to Cartesian coordinates: (4) Subsequently, calculate the rotation axis and rotation angle: Use a unit vector to represent the target direction. Assuming the initial direction is the X-axis, the coordinate axes are rotated as follows: (5) Among them, .

[0040] Subsequently, normalize the calculated rotation axis.

[0041] The rotation angle is determined by the dot product of the two vectors. The specific formula is as follows: (6) Preferably, use the Rodrigues formula to return the rotation matrix for focusing the spherical coordinate system to the super-ellipsoid coordinate system , and the specific formula is as follows: (7) Among them, I is the identity matrix, K is the rotation coordinate axis u of the skew-symmetric matrix.

[0042] Finally, apply the rotation matrix to the vector coordinates of the point to obtain the sampling point : (8) S202. Expand the bidirectional rapidly-exploring random tree based on the bisection method and the first sampling point.

[0043] In order to make the path after expanding the bidirectional rapidly-exploring random tree 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 rapidly-exploring random tree, and using the bisection method to smoothly prune the bidirectional rapidly-exploring random tree after adding the new node.

[0044] Furthermore, as Figure 4 , in some embodiments of the present invention, step S202 specifically includes: S401. Traverse and calculate the Euclidean distance from each node in the node set output by the forward tree to the first sampling point ; S402. Select the node with the smallest distance as the first starting state for the forward growth of the forward tree, and perform a single-step exploration towards the first sampling point to obtain the first new node ; S403. If no obstacle is detected during the expansion of the current forward tree, return the first starting state during the growth process of the current forward tree. The first farthest ancestor state that can be traced back to ; Preferably, return the forward tree flag bit , if , it indicates that no obstacle is detected during the expansion of the forward tree, and the current path is feasible.

[0045] S404. Create the first parent node based on the bisection method between the first farthest ancestor state and the first starting state to replace the first starting state . .

[0046] S405. If the first included angle with the first starting state , the first farthest ancestor state and the first new node as the vertex does not reach the preset bisection threshold at the three points, then create a new first parent node based on the bisection method between the first farthest ancestor state and the replaced first starting state to replace the above-mentioned replaced first starting state , and repeat the process until the first included angle reaches the bisection threshold or an obstacle is detected on the planned path. It should be noted that as

[0047] Figure 5 , connect , , , in sequence. The angle with as the vertex on the connecting line is the reference angle. After replacing with , the angle of the reference angle becomes significantly larger. According to the triangle inequality, the larger the included angle between three points, the smaller the distance between the three points connected by a line. Therefore, the total distance of this path on the random tree is also smaller.

[0048] S406. Update the forward tree based on the first new node , the first farthest ancestor state and the first parent node .

[0049] In a specific embodiment, the forward tree is updated as follows: Update the forward tree. If , then there is: (9) Otherwise: (10) S407. Traverse and calculate the Euclidean distance from each node in the node set output by the reverse tree to the first new node ; S408. Select the node with the minimum distance as the second starting state for reverse growth and explore towards the first new node without step size limit until an obstacle or a double-tree connection is detected, and obtain the second new node ; S409. Return the second farthest ancestor state that the second starting state can be traced back to during this growth process ; S410. Create a second parent node between the second farthest ancestor state and the second starting state based on the bisection method to replace the second starting state ; S411. If the second included angle with the second starting state as the vertex among the replaced second starting state , the second farthest ancestor state and the second new node does not reach the preset bisection threshold , then create a new second parent node between the second farthest ancestor state and the replaced second starting state to replace the above replaced second starting state , and repeat the process until the second included angle reaches the bisection threshold D or an obstacle is detected on the planned path dichotomy .

[0050] S412. Update the reverse tree based on the second new node , the second farthest ancestor state and the second parent node .

[0051] Specifically, the update process of the reverse tree is the same as above

[0052] It should be noted that the dichotomy, as an efficient search algorithm, quickly locates the target value by gradually reducing the search range by half. During the pruning process of the random tree, the dichotomy is used to optimize the tree structure, which can effectively reduce redundant nodes or paths, optimize the topological structure of the random tree, and thus accelerate the convergence speed of the algorithm.

[0053] 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 that in the forward tree, exchange the forward tree and the reverse tree with each other 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 enter step S204.

[0054] In a specific embodiment, if the number of nodes in the reverse tree is greater than that in the forward tree, that is , then exchange the two trees, that is , so as to overcome the performance instability caused by the lack of step length limit and fast expansion of the reverse tree. Perform loop calculation until the reverse tree flag bit , merge the dual trees, and backtrack to calculate the initial path solution according to the set of link relationships between tree nodes E , and the output form of the initial path solution is still , where ， is the set of tree nodes, and is the set of link relationships between tree nodes.

[0055] S204. Calculate the time consumed from the start of step S201 until the end of the loop in step S203 t , if the time t is not less than the preset time threshold , then use the initial path solution as the optimal solution and output it.

[0056] Compared with the prior art, a path planning method for a mobile robot provided by the present invention samples by establishing a hyperellipsoid on the basis of the exploration of a bidirectional rapidly-exploring random tree, then expands the dual trees according to the sampled points to merge the dual trees, thereby finding a path solution, and finally determines whether to output the path solution as the optimal solution according to the time required to solve the path solution, so as to complete the path planning of the mobile robot. The present invention associates the hyperellipsoid with the output path of the random tree, thereby obtaining 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, quickly converge even in a complex environment, further shorten the planning time, and thus solve the technical problem of low path planning efficiency of the prior art when dealing with a complex environment.

[0057] In some embodiments of the present invention, to avoid the algorithm from converging prematurely and failing to obtain the globally optimal path solution, the mobile robot path planning method further includes: If an initial path solution is found The time consumed t is less than a preset time threshold , then based on the first node set of the initial path solution optimize the initial path solution .

[0058] For example Figure 6 , in some embodiments of the present invention, the optimization process specifically includes: S601. Taking the starting position of the robot as the first focus and the target position as the second focus, construct a second super-ellipsoid, where the sum of the Euclidean distances from any point inside the second super-ellipsoid to the first focus and the second focus is not greater than the preset current optimal path cost value .

[0059] In a specific embodiment, the general equation of the second super-ellipsoid is: (11) It can be understood that the major axis and minor axis of the second super-ellipsoid will be updated in real time as a better path is solved, but the foci are fixed points to ensure its convergence.

[0060] S602. Randomly sample inside the second super-ellipsoid to obtain a second sampling point .

[0061] It should be noted that the sampling method for random sampling in the second super-ellipsoid is the same as that in the first super-ellipsoid.

[0062] S603. Optimize the initial path solution using the bisection method based on the second sampling point .

[0063] Furthermore, for example Figure 7 , in some embodiments of the present invention, step S603 includes: S701. 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.

[0064] It should be noted that since the bisection method is adopted in the present invention, the number of nodes in the planned path set is extremely small, so the first set Traverse to avoid result distortion caused by insufficient sampling. In other special embodiments, if the number of nodes in the planned path set is large, considering the uniformity and representativeness of sampling points, an optimization radius can be set centered on the second sampling point to traverse and optimize the nodes within this radius in the first set. V P Traverse and optimize the nodes within this radius in the first set.

[0065] S702. Connect all the tree nodes in the first set one by one in the direction from the starting position of the robot to the target position, and measure the size of the third included angle with the tree node with the smallest distance in the connection line as the vertex. S703. Replace the node with the smallest distance with the second sampling point and then update the link relationship between the tree nodes to obtain the latest path solution ; S704. Connect all the tree nodes in the latest path solution one by one in the direction from the starting position of the robot to the target position, and measure the size of the fourth included angle with the second sampling point as the vertex. S705. If the fourth included angle is not greater than the third included angle, return to step S702 until the fourth included angle is greater than the third included angle; S706. Replace the initial path solution with the latest path solution .

[0066] In summary, a path planning method for a mobile robot provided by the present invention mainly includes: (1) Design a new adaptive sampler that 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) Simplify the asymptotically optimal process and combine it with the bisection method to expand it into a bidirectional planning method with a faster convergence speed, effectively optimizing the topological structure of the random tree. In addition, since the present invention proposes a tree expansion algorithm, it can be combined with other samplers and graph pruning methods. The present invention can quickly obtain a better initial solution in various environments and ensure the convergence speed of the optimal solution, and has good engineering practical value.

[0067] Next, a specific embodiment is used to further demonstrate the technical effect of the present invention. Similarly, combined with Figure 1 the sampling test map and the basic parameters of the sampling experiment, use + as a comparison algorithm, that is As a sampling strategy, As a tree pruning strategy, where represents the path planning method provided by the present invention.

[0068] The quality of the initial path generated by the algorithm is compared through seven evaluation indicators. The first four indicators are used to evaluate the ability of the algorithm to plan the 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 to plan the initial solution, represents the success rate of planning the initial solution. The last three indicators are used to evaluate the convergence speed of the algorithm: Preferably, use T 5% represents the time to obtain a cost of where is the cost of the optimal solution, and it is approximately considered that the path cost converges to i.e., the optimal solution is obtained; represents the number of nodes used to plan the optimal solution, represents the success rate of planning the optimal solution within the specified time. The statistical data comes from 50 independent runs of each algorithm, and the specific experimental data is given in Table 1.

[0069] Table 1 Experimental Data

[0070] Such as Figure 8 , in a second aspect, the present invention further provides a mobile robot path planning device 80, including: A sampling module 810, configured to construct a first hyperellipsoid based on the real-time progress of exploring in the target map space by a preset bidirectional rapidly-exploring random tree, adaptively adjust the sampling probability of the first hyperellipsoid according to the size of the first hyperellipsoid, and randomly sample to obtain a first sampling point based on the sampling probability; An expansion module 820, configured to expand the bidirectional rapidly-exploring random tree based on the first sampling point; A backtracking module 830, after expansion, if the number of tree nodes of the reverse tree is greater than the number of tree nodes of the forward tree, exchange the forward tree and the reverse tree with each other 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 and backtrack to obtain an initial path solution and enter step S204; An output module 840, configured to calculate the time consumed from the start of exploring the bidirectional rapidly-exploring random tree until the initial path solution is found, and if the time is not less than a preset time threshold, output the initial path solution as the optimal solution.

[0071] Such as Figure 9 , in a third aspect, the present invention further provides an electronic device 90, including: A memory 910 for storing programs; A processor 920, coupled to the memory 910, for executing the programs 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.

[0072] In a fourth aspect, the present invention further provides a storage medium for storing computer-readable programs or instructions, and when the programs or instructions are executed by a processor, the steps in the mobile robot path planning method described in any one of the above method items can be implemented.

[0073] The above has introduced in detail a mobile robot path planning method, device, electronic device and storage medium provided by the present invention. Specific examples are used herein to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation manner and application scope. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A path planning method for a mobile robot, characterized in that, Including: Step 1: Explore the motion path of the mobile robot in the target map space based on the preset bidirectional rapidly-exploring random tree (RRT), construct a first super-ellipsoid based on the real-time progress of the exploration by the bidirectional RRT, 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 RRT based on the bisection method and the first sampling point; 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 that in the forward tree, exchange the forward tree and the reverse tree with each other 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 an initial path solution and enter Step 4; Step 4: Calculate the time consumed from the start of Step 1 until the end of the loop in Step 3. If the time is not less than the preset time threshold, output the initial path solution as the optimal solution.

2. The mobile robot path planning method according to claim 1, wherein The bidirectional rapidly-exploring random tree takes the starting position and the target position of the robot as the root nodes, and uses a preset step size as the exploration step size. Moreover, the bidirectional rapidly-exploring random tree T =( V , E ) outputs the real-time progress of the exploration in the form of, where T is the feasible path of the mobile robot explored by the bidirectional rapidly-exploring random tree, V is the set of tree nodes, E is the set of the link relationships between the tree nodes; The exploration of the motion path of the mobile robot in the target map space based on the preset bidirectional RRT, the construction of the first super-ellipsoid based on the real-time progress of the exploration by the bidirectional RRT, the adaptive adjustment of the sampling probability of the first super-ellipsoid according to the size of the first super-ellipsoid, and the random sampling to obtain the first sampling point based on the sampling probability include: Take the two latest tree nodes of the bidirectional RRT in the real-time progress as the two foci of the first super-ellipsoid, and determine the major axis length and minor axis length of the first super-ellipsoid based on the spatial distance between the two foci; Adjust the sampling probabilities of random sampling within the first super-ellipsoid and random sampling in the target map space based on the ratio of the spatial distance from the starting position of the robot to the target position to the major axis length on the basis of the preset sampling probability; Generate a random number with a value range of [0, 1]. If the random number is greater than the adjusted sampling probability, randomly sample within the first super-ellipsoid; otherwise, randomly sample in the target map space.

3. The mobile robot path planning method according to claim 1, wherein The expansion of the bidirectional RRT based on the bisection method and the first sampling point includes: Generate a first new node based on the first sampling point and add it to the bidirectional RRT, and perform smooth pruning on the bidirectional RRT after adding the first new node using the bisection method.

4. The mobile robot path planning method according to claim 3, wherein The generation of the first new node based on the first sampling point and adding it to the bidirectional RRT, and the performance of smooth pruning on the bidirectional RRT after adding the first new node using the bisection method include: Traverse and calculate the Euclidean distance from each tree node in the node set output by the forward tree to the first sampling point; Select the node with the minimum distance as the first starting state for the forward growth of the forward tree, and perform a single-step exploration towards the first sampling point based on a preset step length to obtain a first new node; If no obstacle is detected during the expansion of the current forward tree, return the first farthest ancestor state that the first starting state can backtrack to during the growth process of the current forward tree; Create a first parent node between the first farthest ancestor state and the first starting state based on the bisection method to replace the first starting state; If the magnitude of the first included angle with the first starting state as the vertex among the three points of the first starting state after replacement, the first farthest ancestor state, and the first new node does not reach the preset bisection threshold, then a new first parent node is created based on bisection between the first farthest ancestor state and the first starting state after replacement to replace the first starting state after replacement, and this process is repeated until the magnitude of the first included angle reaches the bisection threshold or an obstacle is detected on the planned path; Update the forward tree based on the first new node, the first farthest ancestor state, and the first parent node; Traverse and calculate the Euclidean distance from each node in the node set output by the backward tree to the first new node; Select the node with the minimum distance as the second starting state for backward growth, and explore towards the first new node without step size limitation until an obstacle or a connection between the two trees is detected to obtain a second new node; Return the second farthest ancestor state that the second starting state can trace back to during this growth process; Create a second parent node based on bisection between the second farthest ancestor state and the second starting state to replace the second starting state; If the magnitude of the second included angle with the second starting state as the vertex among the three points of the second starting state after replacement, the second farthest ancestor state, and the second new node does not reach the preset bisection threshold, then a new second parent node is created based on bisection between the second farthest ancestor state and the second starting state after replacement to replace the second starting state after replacement, and this process is repeated until the magnitude of the second included angle reaches the bisection threshold or an obstacle is detected on the planned path; Update the backward tree based on the second new node, the second farthest ancestor state, and the second parent node; 5. The mobile robot path planning method according to claim 1, wherein The mobile robot path planning method further includes: If the time is less than the preset time threshold, optimize the initial path solution based on the first set of tree nodes of the initial path solution; 6. The mobile robot path planning method according to claim 5, wherein The optimizing the initial path solution based on the first set of tree nodes of the initial path solution includes: Construct a second superellipsoid with the starting position of the robot as the first focus and the target position of the robot as the second focus, where the sum of the spatial distances from any point inside the second superellipsoid to the first focus and the second focus is not greater than the preset current optimal path cost value; Randomly sample second sampling points inside the second superellipsoid; Optimize the initial path solution based on the second sampling points using bisection; 7. The mobile robot path planning method according to claim 6, wherein The optimizing the initial path solution based on the second sampling points using bisection includes: Traverse and calculate the Euclidean distance from each tree node in the first set to the second sampling point to obtain the tree node with the minimum distance; Connect all the tree nodes in the first set in sequence from the starting position of the robot to the target position, and measure the magnitude of the third included angle with the tree node with the minimum distance as the vertex in the connection; Replace the tree node with the minimum distance with the second sampling point and update the link relationship between the tree nodes to obtain the latest path solution; Connect all the tree nodes in the latest path solution in sequence from the starting position of the robot to the target position, and measure the magnitude of the fourth included angle with the second sampling point as the vertex in the connection; If the fourth included angle is not greater than the third included angle, return to the step of randomly sampling to obtain a second sampling point within the second super-ellipsoid until the fourth included angle is greater than the third included angle; Replace the initial path solution with the latest path solution.

8. A mobile robot path planning device, characterized in that, Comprising: A sampling module, configured to construct a first super-ellipsoid based on the real-time progress of exploration of a preset bidirectional rapidly-exploring random tree in a 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, configured to expand the bidirectional rapidly-exploring random tree based on the first sampling point; A backtracking module, after expansion, if the number of tree nodes of the reverse tree is greater than the number of tree nodes of the forward tree, exchange the forward tree and the reverse tree with each other and then return to the step of constructing a first super-ellipsoid based on the real-time progress of exploration of a preset bidirectional rapidly-exploring random tree in a 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, until the reverse tree and the forward tree can be directly connected, merge the forward tree and the reverse tree and backtrack to obtain an initial path solution and enter step four; An output module, configured to calculate the time consumed from the start of exploration of the bidirectional rapidly-exploring random tree until the initial path solution is found, and if the time is not less than a preset time threshold, output the initial path solution as the optimal solution.

9. An electronic device, characterized in that, Comprising: A memory, configured to store a program; A processor, coupled to the memory, configured to execute the program stored in the memory to implement the steps in the mobile robot path planning method described in any one of claims 1 to 7 above.

10. A storage medium, characterized in that, For storing a computer-readable program or instruction, when the program or instruction is executed by a processor, it can implement the steps in the mobile robot path planning method described in any one of claims 1 to 7 above.

Images