Robot path planning method based on improved RRT-Connect

Abstract

The invention provides a robot path planning method based on improved RRT-Connect, and the method employs a bidirectional growth scheme of rapid exploration trees, enables the two rapid exploration trees to explore and expand outwards from a starting point and an end point at the same time, so as to accelerate the convergence speed of path finding. Meanwhile, the grey wolf algorithm is adopted to guide the generation positions of the random points, and the intersection of the two rapid exploration trees is accelerated, so that the efficiency and accuracy of the whole routing process are improved.

Description

Technical Field

[0001] The present invention relates to the field of robot path planning, and in particular to a robot path planning method based on improved RRT-Connect. Background Art

[0002] Robot path planning is one of the core issues in robotics, which is mainly about how to find the best route from the starting point to the end point in a complex environment. Rapidly-exploring Random Tree (RRT) can handle the dynamic changes of the environment and the problems of high-dimensional space, and is widely used in robot path planning. However, the rapid random search tree (RRT) has the disadvantages of long calculation time and poor path quality. Therefore, a solution that can optimize the application effect of the rapid random search tree in robot path planning is urgently needed. Summary of the invention

[0003] Purpose of the invention: The present invention aims to address the shortcomings of the current fast random search tree in robot path planning and propose a robot path planning method based on improved RRT-Connect.

[0004] Invention content: To achieve the above objectives, the present invention proposes the following technical solutions:

[0005] In a first aspect, a robot path planning method based on an improved RRT-Connect is provided, comprising:

[0006] Initialize the target space and determine the starting and ending points of the path;

[0007] Generate fast exploration trees at the starting point and the end point respectively, the two fast exploration trees simultaneously explore outward, and use the gray wolf algorithm to generate random points during the exploration process;

[0008] For each fast exploration tree, find a random point q in the node set of the fast exploration tree that is close to the fast exploration tree rand The nearest point near , from q near Start on q near to q rand The point q is intercepted in the direction of the straight line with a preset step size new , judge q near to q new Is there an obstacle between them? If so, discard point q new and regenerate the random point, otherwise point q new As the new node of the fast exploration tree; repeat this step until the two fast exploration trees intersect;

[0009] Based on the rapid exploration tree, a target path of the robot is determined.

[0010] As an optional implementation of the method of the first aspect, the gray wolf algorithm is used to generate random points, specifically including:

[0011] Initialize the position of the wolf pack in the target space, where each wolf represents a possible random point;

[0012] According to the position of the wolf pack, the cost function F is calculated:

[0013] F=a×dis(q,q goal )+b×dis(q,q obs )+c×dis(q,q before )

[0014] Where a, b, and c represent weight coefficients, dis(x, y) represents the distance between position point x and position point y in the target space, q represents the current position, and q goal represents the latest node of another fast exploration tree, q obs is the obstacle position, q before Represents the previous random point.

[0015] If the cost function meets the preset termination condition, the position of the optimal wolf is output as the random point; if the cost function does not meet the termination condition, the position of the wolf pack is updated and the cost function is recalculated; the update expression of the position of the wolf pack is:

[0016]

[0017] in, represents the current position of wolf α, represents the current position of β wolf, represents the current position of δ wolf, α wolf, β wolf, and δ wolf represent the three best solutions at present. Indicates the current position of the ω wolf following the α wolf, the β wolf, and the δ wolf; represents the relative distance between ω wolf and α wolf, represents the relative distance between ω wolf and β wolf, represents the relative distance between ω wolf and δ wolf; and represents a random vector of α wolves, and represents a random vector of β wolves, and A random vector representing δwolves; is the convergence factor, is a random number in the range [0, 1]; represents the next position of the ω wolf under the guidance of the α wolf, represents the next position of ω wolf under the guidance of β wolf, represents the next position of the ω wolf under the guidance of the ω wolf, Represents the updated position of ω wolf.

[0018] Specifically, the method further includes:

[0019] The rapid exploration tree uses a heuristic function to generate the random points, and the expression of the heuristic function is:

[0020]

[0021] Wherein, rand is a random number between 0 and 1, p is a set target bias threshold, and size(map) represents the target space.

[0022] As an optional implementation manner of the method of the first aspect, the step size is generated using a dynamic adjustment strategy, specifically including:

[0023] Set the initial step size to S;

[0024] Calculate q near The distance L to the nearest obstacle a , calculate q new The distance L between the obstacle d ;

[0025] When L a When it is greater than or equal to 2S, update the step length S d =S;

[0026] When L a When it is less than 2S, judge L d Is it greater than L a If so, update the step size S d =S; if not, then update the step size to:

[0027]

[0028] Wherein, D represents the preset minimum step size.

[0029] As an optional implementation manner of the method of the first aspect, the method further includes:

[0030] Perform path smoothing processing on the target path.

[0031] Specifically, performing path smoothing processing on the target path specifically includes:

[0032] Initialize the target point to be the end point;

[0033] Starting from the starting point, for each node on the target path, determining whether the straight line between the node and the target point crosses or touches an obstacle;

[0034] If yes, then on the target path, the position of the target point is shifted by one node toward the starting point;

[0035] If not, smoothing is performed on the nodes between the node and the target point;

[0036] For the target path after smoothing, the above steps are repeated until all nodes between the starting point and the end point are smoothed.

[0037] Furthermore, smoothing is performed on the nodes between the node and the target point, specifically including:

[0038] For nodes between the node and the target node, these nodes are deleted from the target path.

[0039] In a second aspect, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the processor executes the above-mentioned robot path planning method based on the improved RRT-Connect.

[0040] In a third aspect, an electronic device is provided, including:

[0041] One or more processors; and a memory associated with the one or more processors, the memory being used to store program instructions, which, when read and executed by the one or more processors, enable the electronic device to execute the above-mentioned robot path planning method based on the improved RRT-Connect.

[0042] Beneficial effects: Compared with the prior art, the robot path planning method based on improved RRT-Connect proposed in the present invention has the following advantages:

[0043] 1. The present invention adopts a bidirectional growth scheme of a fast exploration tree, so that two fast exploration trees simultaneously explore and expand outward from a starting point and an end point to accelerate the convergence speed of path finding.

[0044] 2. Considering that the two fast exploration trees are independent individuals, the search time may be too long. To solve this problem, the present invention uses the gray wolf algorithm to guide the generation position of random points and speed up the intersection of the two fast exploration trees.

[0045] 3. The present invention adopts an adaptive step-size adjustment scheme to replace the conventional fixed-step-size sampling strategy to speed up the path planning speed.

[0046] 4. The present invention also introduces a path smoothing strategy into the planned path to optimize the path and reduce unnecessary turning points. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of this specification or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this specification. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0048] Figure 1 A flowchart of a rapid exploration random tree algorithm (RRT) in the prior art.

[0049] Figure 2 A schematic flow chart of a robot path planning method based on improved RRT-Connect according to an embodiment.

[0050] Figure 3 The figure is a flow chart of a path smoothing strategy involved in the embodiment.

[0051] Figure 4 The figure is a schematic diagram of the structure of an electronic device involved in the embodiment. DETAILED DESCRIPTION

[0052] In order to enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in the embodiments of this specification will be clearly and completely described below in conjunction with the drawings in the embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this specification, not all of the embodiments. Based on the embodiments in this specification, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this specification.

[0053] It should be noted that: in other embodiments, the steps of the corresponding method are not necessarily performed in the order shown and described in this specification. In some other embodiments, the steps included in the method may be more or less than those described in this specification. In addition, a single step described in this specification may be decomposed into multiple steps for description in other embodiments; and multiple steps described in this specification may be combined into a single step for description in other embodiments.

[0054] Those skilled in the art will appreciate that the terms used in the embodiments of the present invention are only for the purpose of describing specific embodiments, and are not intended to limit the present invention. The singular forms of "a", "said" and "the" used in the embodiments of the present invention and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0055] The current path-finding process of the Rapid Exploration Random Tree (RRT) algorithm is as follows: Figure 1 As shown, the following steps are included:

[0056] S100: Initialize the entire space, define the initial point, end point, number of sampling points, step length between points and other information;

[0057] S102: randomly generate a point xrand in space;

[0058] S104: Find the point xnear closest to the random point in the point set of the known tree;

[0059] S106: intercepting a point xnew from xnear with a step length t in the straight line direction from xnear to xrand;

[0060] S108: Determine whether there is an obstacle between xnear and xnew, and if so, discard the point;

[0061] S110: add the xnew point to the tree set;

[0062] S112: Loop steps S102 to S110 until the loop end condition is met; the loop end condition is: there is an xnew point within the set neighborhood of the end point.

[0063] In the above steps S102 and S106, the process of searching for random points and finding sampling points xnew is time-consuming and has large uncertainty, which is the main reason why the existing RRT algorithm has a long calculation time and is full of uncertainty.

[0064] In view of this, this embodiment provides a robot path planning method based on improved RRT-Connect (hereinafter referred to as the method). Figure 2 As shown, the method includes steps S200 to S206:

[0065] S200: Initialize the target space and determine the starting point and end point of the path.

[0066] The target space mentioned above refers to the space that the robot needs to work in. To initialize the target space, specifically, it is necessary to determine the map of the target space, including the starting point, end point, number of sampling points, step length between points, obstacle positions, and other information that define the robot's path.

[0067] S202: Generate fast exploration trees at the starting point and the end point respectively, and the two fast exploration trees simultaneously explore outwards, and use the gray wolf algorithm to generate random points during the exploration process.

[0068] The growth process of the fast exploration tree is the path finding process, and the nodes obtained by path finding are stored in a tree structure, thereby obtaining a fast exploration tree. Compared with the existing RRT algorithm that uses a single fast exploration tree for path finding, this embodiment adopts a fast exploration tree bidirectional growth scheme, that is, a fast exploration tree is generated at the starting point and the end point respectively, one fast exploration tree uses the starting point as the root node, and the other fast exploration tree uses the end point as the root node. The two fast exploration trees simultaneously explore and expand outward from the starting point and the end point to speed up the convergence speed of the path finding process.

[0069] Although the two fast exploration trees uniformly sample the entire target space, the two fast exploration trees, as independent individuals, easily lead to over-dispersed sampling areas, which increases unnecessary search time.

[0070] In view of the above problems, in some implementations, a heuristic function may be used to generate random points, thereby optimizing the sampling process. The expression of the heuristic function is:

[0071]

[0072] Among them, rand is a random number between 0 and 1, p is the set target bias threshold, size(map) represents the target space, q goal Represents the latest node of another fast-explored tree.

[0073] The working principle of the heuristic function is: first set the target bias threshold p, then randomly obtain the probability value rand (value 0 to 1) according to the uniform probability distribution, and finally compare the probability value rand with the target bias threshold p. If the probability value rand is greater than the target bias threshold p, the random sampling point is used as the random point; if the probability value rand is less than the target bias threshold p, the latest node q of another fast exploration tree is used as the random point. goal as sampling point.

[0074] In some embodiments, in order to further speed up the search rate and reduce the path planning time, the Gray Wolf Algorithm (GWO) can also be combined with RRT-Connect. The Gray Wolf Algorithm has a powerful global search capability and can effectively avoid falling into a local optimal solution. Secondly, the Gray Wolf Algorithm converges quickly, which helps to find a better solution under limited computing resources. The basic idea of ​​the GWO algorithm is to transform the problem to be optimized into a multidimensional function in the search space, and then search for the optimal solution by simulating the social behavior of gray wolves. Among them, α wolf, β wolf, and δ wolf represent the best top three solutions, and ω wolf represents the remaining candidate solutions.

[0075] The specific process of determining random points using the gray wolf algorithm is as follows:

[0076] First, the positions of the wolves in the target space are initialized, with each wolf representing a possible random point.

[0077] Next, based on the position of the wolf pack, calculate the cost function F:

[0078] F=a×dis(q,q goal )+b×dis(q,q obs )+c×dis(q,q before )

[0079] Among them, a, b, c represent weight coefficients, dis(x, y) represents the distance between position point x and position point y in the target space, q represents the current position, and q goal represents the latest node of another fast exploration tree, q obs is the obstacle position, q before Represents the last random point. The goal of this cost function is to make the generated random point closer to the target, avoid obstacles, and try not to return to the vicinity of the last random point to avoid oscillation.

[0080] If the cost function meets the preset termination condition, the position of the optimal wolf is output as a random point; if the cost function does not meet the termination condition, the position of the wolf pack is updated and the cost function is recalculated; the expression for updating the position of the wolf pack is:

[0081]

[0082] in, represents the current position of wolf α, represents the current position of β wolf, represents the current position of δ wolf, α wolf, β wolf, and δ wolf represent the three best solutions at present. Indicates the current position of the ω wolf following the α wolf, the β wolf, and the δ wolf; represents the relative distance between ω wolf and α wolf, represents the relative distance between ω wolf and β wolf, represents the relative distance between ω wolf and δ wolf; and represents a random vector of α wolves, and represents a random vector of β wolves, and A random vector representing δwolves; is the convergence factor, is a random number in the range [0, 1]; represents the next position of the ω wolf under the guidance of the α wolf, represents the next position of ω wolf under the guidance of β wolf, represents the next position of ω wolf under the guidance of δ wolf, Represents the updated position of ω wolf.

[0083] By combining the Grey Wolf Algorithm (GWO) with RRT-Connect, the rapid exploration tree is guided to expand new search nodes, which can reduce the randomness and invalid space search to a certain extent, improve the efficiency of path search, and accelerate the convergence speed of the entire path-finding process.

[0084] S204: For each fast exploration tree, find a random point q in the node set of the fast exploration tree that is close to the fast exploration tree. rand The nearest point near , from q near Start on q near to q rand The point q is intercepted in the direction of the straight line with a preset step size new , judge q rand to q new Is there an obstacle between them? If so, discard point q new And regenerate a random point, otherwise point q new As the new node of the fast exploration tree; repeat this step until the two fast exploration trees intersect.

[0085] Since the RRT-Connect algorithm uses a certain step size strategy for expansion, the algorithm has low search efficiency and slow convergence speed. To address this defect, the present embodiment adds a function of adaptively adjusting the step size. The principle of adaptively adjusting the step size is described below in conjunction with a specific formula.

[0086] Set the initial step size of the fast exploration tree when exploring the unknown space to S, so as to obtain the virtual node q new , which contains the direction and approximate location information of the new node actually expanded. The coordinates of the virtual node can be obtained according to the following formula:

[0087]

[0088] Among them, P near Indicates point q near The position of P new Indicates point q new The position of P rand represents a random point q rand location.

[0089] Calculate q near The distance L to the nearest obstacle a :

[0090]

[0091] Among them, (x i ,y i ) is the coordinate of the nearest obstacle, (x g ,y g ) is point q near coordinate.

[0092] Calculate q new The distance L to the nearest obstacle d :

[0093]

[0094] Among them, (x d ,y d ) is the virtual node q new The coordinates of .

[0095] When L a When it is greater than or equal to 2S, it means that the new node actually expanded is far away from the obstacle. In this case, the dynamic step size S can be adjusted. d =S, speed up the search.

[0096] When L a When it is less than 2S, a second judgment is required. d Is it greater than L a If so, it means that the growing path has a tendency to move away from obstacles, then update the step size S d =S; if not, the following formula is used to calculate the difference, and the update step size is:

[0097]

[0098] Wherein, D represents the preset minimum step size.

[0099] The above dynamic adjustment strategy is mainly to keep the step size within a range that is neither too small nor too large, because a too small step size will increase the search time, and a too large step size will easily hit obstacles. By using a dynamic adjustment strategy to adjust the step size, the path-finding efficiency can be effectively improved.

[0100] S206: Determine the target path of the robot based on the rapid exploration tree.

[0101] After the two fast exploration trees intersect, the final target path can be determined based on the nodes stored in the two fast exploration trees. In other words, the union of the nodes stored in the two fast exploration trees constitutes the final target path.

[0102] In some implementations, after obtaining the target path, the target path may be smoothed to remove redundant points and unnecessary turning points in the path. Figure 3 As shown in Figure 1, the principle of the path smoothing strategy is that the line between two nodes cannot touch obstacles and cannot cross obstacles.

[0103] Specifically, assuming that the starting coordinates of the target path are P0(X0, Y0), and the end coordinates are P n (X n ,Y n ), the equation of the straight line between the current node and the end point can be obtained from the starting point coordinates and the end point coordinates:

[0104] Y=K(XX n )+Y n

[0105] Among them, K represents the slope of the straight line between the starting point and the end point, and (X, Y) represents the coordinates of the current node G.

[0106] Through the equation of the straight line, we can get the horizontal coordinate on the map (X0,X n ) all points on the straight line within the range, and determine whether these points belong to the obstacle area in turn. If there is a point that belongs to the obstacle area, the path smoothing strategy is violated, that is, the current node G and the end point P n The paths between them cannot be smoothed.

[0107] At this time, select the node P before the end point n-1 As the target node, the current node G and the target node P are judged again. n-1 Whether the path between them satisfies the path smoothing strategy, if so, smoothing is performed, that is, deleting the path between the current node G and the target node P. n-1 If not, the target node is moved forward by one node, and the path between the current node G and the target node is judged to satisfy the path smoothing strategy. This process is repeated until the path between the current node and the target node can be smoothed.

[0108] After each smoothing process, the current node is moved one position toward the end point, and the above process is repeated until all paths between the start point and the end point are smoothed.

[0109] Corresponding to the above-mentioned robot path planning method based on improved RRT-Connect, this embodiment also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the processor executes the above-mentioned robot path planning method based on improved RRT-Connect.

[0110] The computer-readable storage medium may be any available medium that can be stored in the electronic device or a data storage device such as a data center containing one or more available media. The available medium may be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state hard disk). The computer-readable storage medium includes instructions that instruct the electronic device to execute the above-mentioned robot path planning method based on the improved RRT-Connect.

[0111] This embodiment also provides an electronic device, such as Figure 4 As shown, the electronic device includes: a bus 401, a processor 402, a memory 403 and a communication interface 404. The processor 402, the memory 403 and the communication interface 404 communicate through the bus 401. The electronic device can be a server or a terminal device. It should be understood that the present application does not limit the number of processors and memories in the electronic device.

[0112] The bus 401 may be a peripheral component interconnect (PCI) bus, or an extended industry standard architecture (EISA) bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 4 The use of only one line does not mean that there is only one bus or one type of bus. Bus 401 may include a path for transmitting information between various components of the electronic device (eg, processor 402, memory 403, and communication interface 404).

[0113] The processor 402 may include any one or more processors such as a CPU, a graphics processing unit (GPU), a microprocessor (MP), or a digital signal processor (DSP).

[0114] The memory 403 may include a volatile memory, such as a random access memory (RAM). The memory 403 may also include a non-volatile memory, such as a read-only memory (ROM), a flash memory, a hard disk drive (HDD), or a solid state drive (SSD).

[0115] The memory 403 stores executable program codes, and the processor 402 executes the executable program codes to implement the aforementioned robot path planning method based on the improved RRT-Connect.

[0116] The communication interface 404 uses a transceiver module such as, but not limited to, a network interface card or a transceiver to implement communication between the electronic device and other devices or a communication network.

[0117] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0118] The above-mentioned embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A robot path planning method based on improved RRT-Connect, characterized in that: include: Initialize the target space and determine the starting and ending points of the path; Generate fast exploration trees at the starting point and the end point respectively, the two fast exploration trees simultaneously explore outward, and use the gray wolf algorithm to generate random points during the exploration process; For each fast exploration tree, find a random point q in the node set of the fast exploration tree that is close to the fast exploration tree rand The nearest point near , from q near Start on q near to q rand The point q is intercepted in the direction of the straight line with a preset step size new , judge q near to q new Is there an obstacle between them? If so, discard point q new and regenerate the random point, otherwise point q new As the new node of the fast exploration tree; repeat this step until the two fast exploration trees intersect; Based on the rapid exploration tree, a target path of the robot is determined.

2. The method according to claim 1, characterized in that The gray wolf algorithm is used to generate random points, including: Initialize the position of the wolf pack in the target space, where each wolf represents a possible random point; According to the position of the wolf pack, the cost function F is calculated: F=a×dis(q,q goal )+b×dis(q,q obs )+c×dis(q,q before ) Where a, b, and c represent weight coefficients, dis(x, y) represents the distance between position point x and position point y in the target space, q represents the current position, and q goal represents the latest node of another fast exploration tree, q obs is the obstacle position, q before Indicates the previous random point; If the cost function meets the preset termination condition, the position of the optimal wolf is output as the random point; if the cost function does not meet the termination condition, the position of the wolf pack is updated and the cost function is recalculated; the update expression of the position of the wolf pack is: in, represents the current position of wolf α, represents the current position of β wolf, represents the current position of δ wolf, α wolf, β wolf, and δ wolf represent the three best solutions at present. Indicates the current position of ω wolf following α wolf, β wolf, and δ wolf; represents the relative distance between ω wolf and α wolf, represents the relative distance between ω wolf and β wolf, represents the relative distance between ω wolf and δ wolf; and represents a random vector of α wolves, and represents a random vector of β wolves, and A random vector representing δwolves; is the convergence factor, is a random number in the range [0, 1]; represents the next position of the ω wolf under the guidance of the α wolf, represents the next position of ω wolf under the guidance of β wolf, represents the next position of ω wolf under the guidance of δ wolf, Represents the updated position of ω wolf.

3. The method according to claim 2, characterized in that The method further comprises: The rapid exploration tree uses a heuristic function to generate the random points, and the expression of the heuristic function is: Wherein, rand is a random number between 0 and 1, p is a set target bias threshold, and size(map) represents the target space.

4. The method according to claim 1, characterized in that: The step size is generated using a dynamic adjustment strategy, specifically including: Set the initial step size to S; Calculate q near The distance L to the nearest obstacle a , calculate q new The distance L between the obstacle d ; When L a When it is greater than or equal to 2S, update the step length S d =S; When L a When it is less than 2S, judge L d Is it greater than L a If so, update the step size S d =S; if not, then update the step size to: Wherein, D represents the preset minimum step size.

5. The method according to claim 1, characterized in that The method further comprises: Perform path smoothing processing on the target path.

6. The method according to claim 5, characterized in that Performing path smoothing processing on the target path specifically includes: Initialize the target point to be the end point; Starting from the starting point, for each node on the target path, determining whether the straight line between the node and the target point crosses or touches an obstacle; If yes, then on the target path, the position of the target point is shifted by one node toward the starting point; If not, smoothing is performed on the nodes between the node and the target point; For the target path after smoothing, the above steps are repeated until all nodes between the starting point and the end point are smoothed.

7. The method according to claim 6, characterized in that The nodes between the node and the target point are smoothed, specifically including: For nodes between the node and the target node, these nodes are deleted from the target path.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the processor is caused to perform the method according to any one of claims 1 to 7.

9. An electronic device, characterized in that: include: one or more processors; And a memory associated with the one or more processors, the memory is used to store program instructions, and when the program instructions are read and executed by the one or more processors, the electronic device executes the method as claimed in any one of claims 1 to 7.

Images