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

Abstract

The present invention discloses a robot path planning method, device, electronic device and storage medium, the method comprises: obtaining a robot starting point, a target point and an initial random tree; judging whether there is an obstacle between the starting point and the target point; if there is no obstacle between the starting point and the target point, obtaining the middle position between the starting point and the target point and marking it as an intermediate node, using a preset first random tree search method, starting from the starting point and the target point towards the intermediate node and starting from the intermediate node towards the starting point and the target point to perform a first path search; if there is an obstacle between the starting point and the target point, marking the shape of the obstacle, obtaining the vertex position of the obstacle and marking it as a vertex, using a preset second random tree search method, starting from the starting point and the target point at the same time towards each other and avoiding the vertex to perform a second path search. The present invention solves the technical problems of low efficiency and tortuous paths obtained by the path planning algorithm in the prior art.

Description

Technical Field

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

[0002] Oil and gas pipeline transportation is one of the three major transportation modes in the world. Due to the age of the pipeline and the erosion of the transported materials, impurities will be generated in the pipeline, causing damage. Therefore, it is necessary to use pipeline robots to realize automatic navigation and patrol in the pipeline and automatic driving. Therefore, path planning has become the research focus of robot autonomous navigation. Among them, the path planning algorithm, as the main content of the mobile robot, ensures the safe and stable driving of the robot in the specified space. The path planning algorithm must be accurate, effective, convenient and easy to implement. At present, the feasible path planning algorithms and theories available at home and abroad mainly include ant colony algorithm, particle swarm optimization algorithm, genetic algorithm, artificial potential field method and A* algorithm. These algorithms can quickly converge to the optimal path in a simple environment, but when dealing with complex environments and high-dimensional spaces, on the one hand, the convergence speed will drop sharply, and on the other hand, the path generated does not consider the kinematic constraints of the mobile robot, resulting in the planned path cannot be executed by the robot.

[0003] With the widespread application of various types of robots, a path planning algorithm that is better than most algorithms has been developed - the RRT algorithm. The current RRT algorithm has a fast convergence speed and can efficiently solve path planning problems in unknown environments or high-latitude environments. However, the RRT algorithm also has disadvantages such as algorithm randomness, reduced efficiency when the obstacles are narrow, and a relatively tortuous generated path. It is not conducive to the robot to travel according to the path planned by this algorithm, and it is necessary to update this algorithm and then carry out the robot's path planning. Summary of the invention

[0004] The purpose of the present invention is to overcome the above-mentioned technical deficiencies and provide a robot path planning method, device, electronic device and storage medium to solve the technical problem that the robot path obtained by the path planning algorithm in the prior art is inefficient and tortuous.

[0005] In order to achieve the above technical objectives, the present invention adopts the following technical solutions:

[0006] In a first aspect, the present invention provides a robot path planning method, comprising:

[0007] Get the robot's starting point, target point, and initial random tree;

[0008] Determine whether there is an obstacle between the starting point and the target point;

[0009] If there is no obstacle between the starting point and the target point, the middle position between the starting point and the target point is obtained and marked as the middle node, and a first path search is performed from the starting point and the target point toward the middle node and from the middle node toward the starting point and the target point using a preset first random tree search method;

[0010] If there is an obstacle between the starting point and the target point, the shape of the obstacle is marked, and the vertex position of the obstacle is obtained and marked as the vertex. The preset second random tree search method is used to start from the starting point and the target point at the same time, toward each other and avoiding the vertex to perform a second path search.

[0011] In some embodiments, the obtaining of the robot starting point, target point, and initial random tree includes:

[0012] Establish the robot's initial working state grid map;

[0013] The initial working environment is partitioned according to the grid map to determine the working area that requires path planning.

[0014] In some embodiments, if there is an obstacle between the starting point and the target point, marking the shape of the obstacle, and obtaining the vertex position of the obstacle and marking it as a vertex includes:

[0015] Obtain the shape of the obstacle, where the shape includes a polygon, a circle, and a combination of a circle and a polygon;

[0016] According to the shape, the vertex position of the obstacle is determined based on a preset collision method and marked as a vertex.

[0017] In some embodiments, the step of performing a second path search from the starting point and the target point simultaneously toward each other and avoiding the vertex includes:

[0018] Generating a first random point and a second random point in the working area, wherein the first random point corresponds to the starting point and the second random point corresponds to the target point;

[0019] Determine a first vertex closest to the first random point according to the first random point, and determine a second vertex closest to the second random point according to the second random point;

[0020] According to the first vertex and the second vertex, a fast search strategy is used to determine the first child node and the second child node closest to the first vertex and the second vertex;

[0021] Based on the first child node and the second child node, the starting point and the target point are quickly expanded to form a starting point random tree and a target point random tree;

[0022] According to the starting point random tree and the target point random tree, the preset connect function method is used to connect the starting point random tree and the target point random tree.

[0023] In some embodiments, the method of rapidly expanding the starting point and the target point based on the first sub-node and the second sub-node includes:

[0024] Get the initial step size of the random tree, and search the first child node and the second child node;

[0025] Determine whether the first child node and the second child node collide with the obstacle under the initial step state;

[0026] If the first child node and the second child node collide with an obstacle, the expansion step size is adjusted to the initial step size minus a set value;

[0027] If the first child node and the second child node do not collide with the obstacle, the expansion step length is adjusted to the initial step length plus a set value.

[0028] In some embodiments, after the starting point and the target point are rapidly expanded based on the first sub-node and the second sub-node, the method further includes:

[0029] According to the generated random tree path, the preset non-uniform rational B-spline method is used to smooth the random tree path to obtain a more optimized path.

[0030] In some embodiments, the expression of the extension step is:

[0031] Among them, ε represents the expansion step, a represents the set value of the step change, lrr' represents the connection between the current sampling point and the previous sampling point, and x obs Indicates the obstacle area.

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

[0033] The acquisition module is used to obtain the robot's starting point, target point, and initial random tree;

[0034] A judgment module is used to judge whether there is an obstacle between the starting point and the target point;

[0035] A first path search module, if there is no obstacle between the starting point and the target point, obtains the middle position between the starting point and the target point as the middle node, and uses a preset first random tree search method to perform a first path search from the starting point and the target point toward the middle node and from the middle node toward the starting point and the target point;

[0036] The second path search module, if there is an obstacle between the starting point and the target point, marks the shape of the obstacle, obtains the vertex position of the obstacle and marks it as a vertex, uses the preset second random tree search method, starts from the starting point and the target point at the same time, heads towards each other and avoids the vertex to perform a second path search.

[0037] In a third aspect, the present invention further provides an electronic device, comprising: a processor and a memory;

[0038] The memory stores a computer-readable program executable by the processor;

[0039] When the processor executes the computer-readable program, the steps in the robot path planning method described above are implemented.

[0040] In a fourth aspect, the present invention further provides a computer-readable storage medium, which stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps in the robot path planning method as described above.

[0041] Compared with the prior art, the robot path planning method, device, electronic device and storage medium provided by the present invention first obtain the robot starting point, target point and initial random tree; then determine whether there is an obstacle between the starting point and the target point; if there is no obstacle between the starting point and the target point, obtain the middle position between the starting point and the target point and mark it as the middle node, adopt a preset first random tree search method, start from the starting point and the target point towards the middle node and from the middle node towards the starting point and the target point to perform a first path search; if there is an obstacle between the starting point and the target point, mark the shape of the obstacle, obtain the vertex position of the obstacle and mark it as the vertex, adopt a preset second random tree search method, start from the starting point and the target point at the same time towards each other and avoid the vertex to perform a second path search; the robot path planning method provided by the present invention improves the efficiency of path planning by adopting a two-way search strategy, and avoids the problem of collision and tortuous road when the robot is driving by avoiding obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is a flow chart of an embodiment of a robot path planning method provided by the present invention;

[0043] Figure 2 is a flow chart of another embodiment of the robot path planning method provided by the present invention;

[0044] Figure 3 It is a flowchart of an embodiment of step S104 in the robot path planning method provided by the present invention;

[0045] Figure 4 is a flowchart of another embodiment of step S104 in the robot path planning method provided by the present invention;

[0046] Figure 5 It is a flowchart of an embodiment of step S404 in the robot path planning method provided by the present invention;

[0047] Figure 6 is a schematic diagram of an embodiment of a robot path planning device provided by the present invention;

[0048] Figure 7 It is a schematic diagram of the operating environment of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0050] The robot path planning method, device, equipment or computer-readable storage medium involved in the present invention can be used in narrow areas such as pipelines, tunnels and mines that need to be monitored by robots. The method, device, equipment or computer-readable storage medium involved in the present invention can be integrated with the above-mentioned system or relatively independent.

[0051] This embodiment provides a robot path planning method. Figure 1 is a flow chart of the robot path planning method provided by an embodiment of the present invention, please refer to Figure 1 ,Robot path planning methods include:

[0052] S101, obtaining the robot's starting point, target point and initial random tree;

[0053] S102, determining whether there is an obstacle between the starting point and the target point;

[0054] S103, if there is no obstacle between the starting point and the target point, obtain the middle position between the starting point and the target point and mark it as the middle node, and use the preset first random tree search method to perform a first path search from the starting point and the target point toward the middle node and from the middle node toward the starting point and the target point;

[0055] S104: If there is an obstacle between the starting point and the target point, mark the shape of the obstacle, obtain the vertex position of the obstacle and mark it as a vertex, and use the preset second random tree search method to start from the starting point and the target point at the same time, toward each other and avoiding the vertex to perform a second path search.

[0056] In step S101, after obtaining the starting point, target point and initial random tree of the robot, the starting point and target point are substituted into the initial random tree so that the random tree is expanded to perform path search.

[0057] In step S103, when there is no obstacle between the initial point and the target point, the intermediate node x center As the third node, RRT* can further accelerate path expansion by generating four trees simultaneously, where the expression of the intermediate node is:

[0058]

[0059] In step S104, when there is an obstacle between the target point and the starting point, in order to avoid repeated path planning, each point of the obstacle in space is marked, and the random tree is allowed to avoid the marked obstacle vertices during the expansion process, so that the algorithm has a clear target point in the search, thereby quickly determining the search direction of the random tree.

[0060] In this embodiment, the expansion direction of the random tree is first planned by judging whether there is an obstacle between the starting point and the target point. If there is no obstacle, the middle position between the starting point and the target point is marked as an intermediate node, so that the random tree is expanded from the intermediate node, the starting point and the target point at the same time, generating four trees, which greatly improves the path planning efficiency; if there is an obstacle, the obstacle is marked so that the random tree avoids the obstacle when expanding, avoiding repeated planning of the path.

[0061] In some embodiments, see Figure 2 , Figure 2 This is a flow chart of another embodiment of the present invention, wherein the process of obtaining the robot starting point, target point, and initial random tree includes:

[0062] S201, establishing a grid map of the robot's initial working state;

[0063] S202: partition the initial working environment according to the grid map to determine the working area that needs path planning.

[0064] In this embodiment, in order to facilitate the accurate positioning of the expansion nodes of the random tree and the spatial positions of obstacles, before the random tree expansion, it is necessary to establish a grid map in the working area of ​​the robot, and based on the established grid map, the area where path planning is required is divided to determine the working area of ​​the algorithm.

[0065] In some embodiments, see Figure 3 , Figure 3This is a flow chart of an embodiment of step S104, wherein if there is an obstacle between the starting point and the target point, the shape of the obstacle is marked, and the vertex position of the obstacle is obtained and marked as a vertex, including:

[0066] S301, obtaining the shape of the obstacle, wherein the shape includes a polygon, a circle, and a combination of a circle and a polygon;

[0067] S302: According to the shape, determine the vertex position of the obstacle based on a preset collision method and mark it as a vertex.

[0068] In step S301, in order to accurately mark the vertex positions of the obstacle, it is necessary to obtain the shape of the obstacle. The shape of the obstacle determines the way of selecting its vertices, wherein an irregular obstacle can be equivalent to a polygonal obstacle.

[0069] In step S302, when the shape of the obstacle is a polygon, the convex points with internal angles less than or equal to 180 degrees are taken as vertices; when the shape of the obstacle is a circle, the circle is divided into 8 equal parts with the center of the circle as the center, and 8 nodes are sampled on the edge of the circle as vertices; when the shape of the obstacle includes polygons and circles, the vertex setting method of the polygons and circles remains unchanged, and the intersection of the polygon and the circle is also set as a vertex.

[0070] In this embodiment, the vertices of the obstacle are marked according to the shape of the obstacle and the position of the obstacle in space to guide the direction of random tree expansion.

[0071] In some embodiments, see Figure 4 , Figure 4 This is a flow chart of another embodiment of step S104, wherein the second path search is performed from the starting point and the target point simultaneously toward each other and avoiding the vertex, including:

[0072] S401, generating a first random point and a second random point in a working area, wherein the first random point corresponds to a starting point, and the second random point corresponds to a target point;

[0073] S402: determining a first vertex closest to the first random point based on the first random point, and determining a second vertex closest to the second random point based on the second random point;

[0074] S403: According to the first vertex and the second vertex, a fast search strategy is used to determine the first child node and the second child node closest to the first vertex and the second vertex;

[0075] S404: Based on the first child node and the second child node, the starting point and the target point are rapidly expanded to form a starting point random tree and a target point random tree;

[0076] S405 , according to the starting point random tree and the target point random tree, a preset connect function method is used to connect the starting point random tree and the target point random tree.

[0077] In this embodiment, after determining the vertex position of the obstacle, in order to enable the random tree starting from the target point and the starting point to avoid the obstacle and expand, it is necessary to find a random point closest to the vertex of the obstacle to form the expansion destination point of the random tree, guide the random tree to expand, and use the connection function to connect the random trees that are expanded in both directions to form a complete robot path.

[0078] In some embodiments, see Figure 5 , Figure 5 This is a flow chart of an embodiment of step S404, wherein the process of rapidly expanding the starting point and the target point based on the first sub-node and the second sub-node includes:

[0079] S501, obtaining an initial step length of the random tree, and searching for the first child node and the second child node;

[0080] S502, respectively determining whether the first child node and the second child node collide with an obstacle in the initial step state;

[0081] S503: If the first child node and the second child node collide with an obstacle, the expansion step length is adjusted to the initial step length minus a set value;

[0082] S504: If the first subnode and the second subnode do not collide with the obstacle, the expansion step length is adjusted to the initial step length plus a set value.

[0083] In this embodiment, since KB-RRT* uses a fixed step size for search, and the fixed step size will limit the expansion speed of the random tree when exploring the obstacle-free space, an adaptive step size adjustment method is added to increase the global step size (for the entire tree); the adaptive step size adjustment algorithm still uses the initial step size for search at the beginning. When searching for a new node for the first time, if the tree does not encounter an obstacle, the step size of the next step will increase by a set step size on the original basis, and accumulate in subsequent searches; when an obstacle is encountered during the expansion process, the expansion step size will be reduced by a set step size on the original basis to reduce the collision probability, and it will also be accumulated in subsequent searches; specifically, the expression of the expansion step size is:

[0084] Among them, ε represents the expansion step, a represents the set value of the step change, lrr' represents the connection between the current sampling point and the previous sampling point, and x obs Indicates the obstacle area.

[0085] It should be noted that, in this embodiment, the value of the adaptive adjustment step is set to 0.5, that is, a=0.5.

[0086] In some embodiments, after the starting point and the target point are rapidly expanded based on the first sub-node and the second sub-node, the method further includes:

[0087] According to the generated random tree path, the preset non-uniform rational B-spline method is used to smooth the random tree path to obtain a more optimized path.

[0088] In this embodiment, in order to make the robot move more smoothly, the generated path needs to be smoothed. In this embodiment, the non-uniform rational B-spline method (NURBS) is used to smooth the path to optimize the path. The NURBS curve retains all the advantages of the B-spline curve, and at the same time, NURBS also has advantages that the B-spline curve method does not have, including perspective invariance, accurate representation of quadratic surfaces such as spheres, and freer shape control.

[0089] Based on the above robot path planning method, the embodiment of the present invention also provides a robot path planning device 600, see Figure 6 The robot path planning device 600 includes an acquisition module 610, a judgment module 620, a first path search module 630 and a second path search module 640.

[0090] An acquisition module 610 is used to acquire a robot starting point, a target point, and an initial random tree;

[0091] A determination module 620 is used to determine whether there is an obstacle between the starting point and the target point;

[0092] A first path search module 630, if there is no obstacle between the starting point and the target point, obtains the middle position between the starting point and the target point as the middle node, and uses a preset first random tree search method to perform a first path search from the starting point and the target point toward the middle node and from the middle node toward the starting point and the target point;

[0093] The second path search module 640, if there is an obstacle between the starting point and the target point, marks the shape of the obstacle, obtains the vertex position of the obstacle and marks it as a vertex, uses the preset second random tree search method, starts from the starting point and the target point at the same time towards each other and avoids the vertex to perform a second path search.

[0094] like Figure 7As shown, based on the above robot path planning method, the present invention also provides an electronic device, which can be a computing device such as a mobile terminal, a desktop computer, a notebook, a palm computer, and a server. The electronic device includes a processor 710, a memory 720, and a display 730. Figure 7 Only some components of the electronic device are shown, but it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.

[0095] In some embodiments, the memory 720 may be an internal storage unit of the electronic device, such as a hard disk or memory of the electronic device. In other embodiments, the memory 720 may also be an external storage device of the electronic device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. equipped on the electronic device. Further, the memory 720 may also include both an internal storage unit of the electronic device and an external storage device. The memory 720 is used to store application software and various types of data installed in the electronic device, such as program codes for installing the electronic device. The memory 720 may also be used to temporarily store data that has been output or is to be output. In one embodiment, a robot path planning program 740 is stored on the memory 720, and the robot path planning program 740 can be executed by the processor 710, thereby realizing the robot path planning method of each embodiment of the present application.

[0096] In some embodiments, the processor 710 can be a central processing unit (CPU), a microprocessor or other data processing chip, which is used to run the program code stored in the memory 720 or process data, such as executing a robot path planning method.

[0097] In some embodiments, the display 730 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, an OLED (Organic Light-Emitting Diode) touch device, etc. The display 730 is used to display information on the robot path planning device and to display a visual user interface. The components 710-730 of the electronic device communicate with each other via a system bus.

[0098] Of course, those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing related hardware (such as a processor, a controller, etc.) through a computer program, and the program can be stored in a computer-readable storage medium, and the program can include the processes of the above-mentioned method embodiments when executed. The storage medium can be a memory, a disk, an optical disk, etc.

[0099] The specific implementation of the present invention described above does not constitute a limitation on the protection scope of the present invention. Any other corresponding changes and modifications made based on the technical concept of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A robot path planning method, characterized in that: include: Get the robot's starting point, target point, and initial random tree; Determine whether there is an obstacle between the starting point and the target point; If there is no obstacle between the starting point and the target point, the middle position between the starting point and the target point is obtained and marked as the middle node, and a first path search is performed from the starting point and the target point toward the middle node and from the middle node toward the starting point and the target point using a preset first random tree search method; If there is an obstacle between the starting point and the target point, the shape of the obstacle is marked, and the vertex position of the obstacle is obtained and marked as a vertex, and a preset second random tree search method is used to generate a first random point and a second random point in the working area, wherein the first random point corresponds to the starting point, and the second random point corresponds to the target point; according to the first random point, the first vertex closest to the first random point is determined, and according to the second random point, the second vertex closest to the second random point is determined; according to the first vertex and the second vertex, a fast search strategy is used to determine the first subnode and the second subnode closest to the first vertex and the second vertex; the initial step length of the random tree is obtained, and the first subnode and the second subnode are searched; It is determined whether the first child node and the second child node collide with the obstacle under the initial step state; if the first child node and the second child node collide with the obstacle, the expansion step is adjusted to the initial step minus a set value; if the first child node and the second child node do not collide with the obstacle, the expansion step is adjusted to the initial step plus a set value to form a starting point random tree and a target point random tree; according to the generated random tree path, the preset non-uniform rational B-spline method is used to smooth the random tree path to obtain a more optimized path, and the preset connect function method is used to connect the starting point random tree and the target point random tree.

2. The robot path planning method according to claim 1, characterized in that: The step of obtaining the robot's starting point, target point, and initial random tree includes: Establish the robot's initial working state grid map; The initial working environment is partitioned according to the grid map to determine the working area that requires path planning.

3. The robot path planning method according to claim 2, characterized in that: If there is an obstacle between the starting point and the target point, the shape of the obstacle is marked, and the vertex position of the obstacle is obtained and marked as a vertex, including: Obtain the shape of the obstacle, where the shape includes a polygon, a circle, and a combination of a circle and a polygon; According to the shape, the vertex position of the obstacle is determined based on a preset collision method and marked as a vertex.

4. The robot path planning method according to claim 1, characterized in that: The expression of the expansion step length is: , in, represents the expansion step, a represents the set value of the step change, lrr' represents the connection between the current sampling point and the previous sampling point, Indicates the obstacle area.

5. A robot path planning device, characterized in that: include: The acquisition module is used to obtain the robot's starting point, target point, and initial random tree; A judgment module is used to judge whether there is an obstacle between the starting point and the target point; A first path search module, if there is no obstacle between the starting point and the target point, obtains the middle position between the starting point and the target point as the middle node, and uses a preset first random tree search method to perform a first path search from the starting point and the target point toward the middle node and from the middle node toward the starting point and the target point; The second path search module, if there is an obstacle between the starting point and the target point, marks the shape of the obstacle, obtains the vertex position of the obstacle and marks it as a vertex, and uses a preset second random tree search method to generate a first random point and a second random point in the working area, wherein the first random point corresponds to the starting point, and the second random point corresponds to the target point; according to the first random point, determines the first vertex closest to the first random point, and according to the second random point, determines the second vertex closest to the second random point; according to the first vertex and the second vertex, uses a fast search strategy to determine the first subnode and the second subnode closest to the first vertex and the second vertex; obtains the initial step size of the random tree, and searches the first subnode and the second subnode; It is determined whether the first child node and the second child node collide with the obstacle under the initial step state; if the first child node and the second child node collide with the obstacle, the expansion step is adjusted to the initial step minus a set value; if the first child node and the second child node do not collide with the obstacle, the expansion step is adjusted to the initial step plus a set value to form a starting point random tree and a target point random tree; according to the generated random tree path, the preset non-uniform rational B-spline method is used to smooth the random tree path to obtain a more optimized path, and the preset connect function method is used to connect the starting point random tree and the target point random tree.

6. An electronic device, characterized in that: include: Processor and memory; The memory stores a computer-readable program executable by the processor; When the processor executes the computer-readable program, the steps in the robot path planning method according to any one of claims 1 to 4 are implemented.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps in the robot path planning method as described in any one of claims 1-4.

Images