A motion path planning method, device, terminal and medium based on RRT algorithm

Abstract

The application discloses a motion path planning method and device based on an RRT algorithm, a terminal and a medium. The scheme provided by the application is based on the RRT algorithm, a three-dimensional model of an actual work scene is imported into a set program, position coordinates of a motion starting point and a motion ending point of actual work requirements are set, a motion step length required when a branch node of a random tree is expanded and a threshold size of an ending point convergence are set according to the size of the imported map model, simulation running is performed in the program with the set parameter scene model, and then an actual feasible and more accurate collision-free trajectory planning is calculated, so that the success rate of the insulated boom truck in executing the planned path is improved, and the technical problem of a low execution success rate of the path planned by using the RRT algorithm is solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of artificial intelligence, and in particular to a motion path planning method and device based on an RRT algorithm, a terminal and a medium. BACKGROUND

[0002] At present, with the development of economy and the rapid development of urbanization, the demand for electricity in social production and life is increasing, and the requirements for power quality and power supply stability are also increasing. In this case, the power department and staff must strengthen the construction of the power grid to ensure the normal operation of the power grid.

[0003] Currently, more than 90% of live working of the Southern Power Grid Company uses an insulating boom truck combined with an insulating glove method, and the operator needs to control the working vehicle while performing device disconnection and other work. For aerial working vehicles, the working environment is very complex and there are many obstacles (such as trees and streetlights) in the working environment. Currently, a mobile trajectory and trajectory points on the mobile trajectory can be determined by using an RRT (Rapidly-Exploring Random Trees) algorithm according to acquired planning working scene, target pose information and initial state information, but when facing a complex planning working scene, the path planned by the RRT algorithm has the technical problem of unstable execution success rate. SUMMARY

[0004] The present application provides a motion path planning method and device based on an RRT algorithm, a terminal and a medium, to solve the technical problem of unstable execution success rate of the path planned by the RRT algorithm.

[0005] To solve the above technical problem, the present application provides a motion path planning method based on an RRT algorithm in the first aspect, comprising:

[0006] According to the actual work requirements and working scene, a three-dimensional scene model of the working scene is constructed;

[0007] According to the work requirements, a path starting point and a path ending point are determined in the three-dimensional scene model;

[0008] According to the coordinates of the path starting point and the path ending point, a terminal threshold value parameter is calculated in combination with a preset terminal threshold value optimization formula;

[0009] According to the size data of the three-dimensional scene model, an expansion step length parameter is calculated in combination with a preset expansion step length optimization formula;

[0010] The RRT algorithm is executed according to the expansion step length parameter and the terminal threshold value parameter to obtain a motion path of the insulating boom truck.

[0011] Preferably, the end point threshold optimization formula is specifically:

[0012]

[0013] wherein Thr is the end point threshold parameter, respectively are three-dimensional coordinates of a path start point, respectively are three-dimensional coordinates of a path end point, and a is a first empirical coefficient.

[0014] Preferably, the expansion step optimization formula is specifically:

[0015]

[0016] wherein Delta is the expansion step parameter, , , respectively are lengths of x-axis, y-axis and z-axis of the three-dimensional scene model, and b is a second empirical coefficient.

[0017] Preferably, the execution of the RRT algorithm according to the expansion step parameter and the end point threshold parameter to obtain a motion path of an insulated aerial work platform truck specifically includes:

[0018] taking the path start point as a root node of a random tree, and obtaining a random node by random sampling in a non-obstacle region of the three-dimensional scene model;

[0019] searching for a target node closest to the random node in the generated random tree, and expanding a branch from the target node to the random node in a direction from the target node to the random node according to the expansion step parameter to grow a new node;

[0020] performing obstacle detection and judgment on the expanded path, if an obstacle is touched, re-performing random point sampling and a new round of random tree growth, if no obstacle is touched, adding the new node as a new leaf node to the random tree;

[0021] when a distance between a leaf node on the random tree and the path end point is less than the end point threshold parameter, determining that the random tree search is completed, and outputting a motion path generated by the random tree search.

[0022] The second aspect of the present application provides a motion path planning device based on an RRT algorithm, comprising:

[0023] a scene model construction unit configured to construct a three-dimensional scene model of a work scene according to actual work requirements and the work scene;

[0024] a path point determination unit configured to determine a path start point and a path end point in the three-dimensional scene model according to the job requirement;

[0025] an end point threshold parameter calculation unit configured to calculate an end point threshold parameter according to coordinates of the path start point and the path end point, in combination with a preset end point threshold optimization formula;

[0026] an extended step length parameter calculation unit configured to calculate an extended step length parameter according to size data of the three-dimensional scene model, in combination with a preset extended step length optimization formula;

[0027] a motion path generation unit configured to execute an RRT algorithm according to the extended step length parameter and the end point threshold parameter, to obtain a motion path of the insulated boom truck.

[0028] Preferably, the end point threshold optimization formula is specifically as follows:

[0029]

[0030] wherein Thr is the end point threshold parameter, respectively are three-dimensional coordinates of the path start point, respectively are three-dimensional coordinates of the path end point, and a is a first empirical coefficient.

[0031] Preferably, the extended step length optimization formula is specifically as follows:

[0032]

[0033] wherein Delta is the extended step length parameter, , , respectively are lengths of x-axis, y-axis and z-axis of the three-dimensional scene model, and b is a second empirical coefficient.

[0034] Preferably, the motion path generation unit is specifically configured to:

[0035] take the path start point as a root node of a random tree, and obtain a random node in a non-obstacle region of the three-dimensional scene model through random sampling;

[0036] search for a target node closest to the random node in the generated random tree, and grow a new node obtained through extending a branch from the target node to the random node in a direction of the random node according to the extended step length parameter;

[0037] perform obstacle detection and judgment on the extended path, if an obstacle is touched, re-perform random point sampling and a new round of random tree growth, if no obstacle is touched, add the new node as a new leaf node to the random tree;

[0038] When the distance between the leaf node on the random tree and the end point of the path is less than the end point threshold parameter, it is determined that the random tree search is completed, and a motion path generated by the random tree search is output.

[0039] The third aspect of the present application provides a motion path planning terminal based on an RRT algorithm, comprising a memory and a processor.

[0040] The memory is configured to store program code corresponding to the motion path planning method based on the RRT algorithm provided in the first aspect of the present application.

[0041] The processor is configured to execute the program code.

[0042] The fourth aspect of the present application provides a computer readable storage medium, wherein the computer readable storage medium stores program code corresponding to the motion path planning method based on the RRT algorithm provided in the first aspect of the present application.

[0043] From the above technical solutions, the present application has the following advantages:

[0044] The scheme provided by the present application is based on the RRT algorithm, a three-dimensional model of an actual work scene is imported into a set program, the position coordinates of the motion start point and the end point of the actual work requirement are set, the motion step length required when the random tree branch node expands and the threshold size of the end point convergence are automatically adapted according to the size of the imported map model, the simulation running is performed in the program with the set parameter scene model imported, and then the actual feasible and more accurate collision-free trajectory planning is calculated, so as to improve the success rate of the insulated boom truck in executing the planned path, thereby solving the technical problem that the success rate of the path planned by the RRT algorithm is unstable. BRIEF DESCRIPTION OF DRAWINGS

[0045] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiment or prior art description will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0046] Figure 1 The flowchart of the first embodiment of the motion path planning method based on the RRT algorithm provided by the present application.

[0047] Figure 2 The flowchart of the second embodiment of the motion path planning method based on the RRT algorithm provided by the present application.

[0048] Figure 3 A structural schematic diagram of a motion path planning device based on an RRT algorithm is provided in the present application. DETAILED DESCRIPTION

[0049] The applicant found through research that when the existing RRT (Rapidly-Exploring Random Trees) algorithm is used for path planning, the algorithm parameters such as the end point threshold and the expansion step length need to be manually set by the staff, but the accuracy of such parameter setting is easily affected by subjective factors such as the professional level and experience of the staff, and especially when facing complex planning work scenes, the accuracy of the manually set parameters is difficult to guarantee, thereby resulting in the technical problem that the execution success rate of the path planned by using the RRT algorithm is unstable.

[0050] Therefore, the embodiments of the present application provide a motion path planning method and device based on an RRT algorithm, a terminal and a medium, to solve the technical problem that the execution success rate of the path planned by using the RRT algorithm is low.

[0051] In order to make the purposes, characteristics and advantages of the present application more obvious and easy to understand, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the embodiments described below are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0052] First, an embodiment of a motion path planning method based on an RRT algorithm provided by the present application is described as follows:

[0053] Please refer to Figure 1 The first aspect of the present application provides a motion path planning method based on an RRT algorithm, comprising:

[0054] Step 101, constructing a three-dimensional scene model of a work scene according to actual work requirements and the work scene;

[0055] It can be understood that the example provided in the present embodiment is first converted into a 3d file format extension name readable by Matlab based on a Matlab running program method after the three-dimensional scene model of the work scene is constructed.

[0056] Step 102, determining a path starting point and a path ending point in the three-dimensional scene model according to the work requirements;

[0057] The selection of the starting point and the ending point can be made in a relatively open area to facilitate the expansion and convergence of the branches during the program running, under the premise of following the actual operation condition.

[0058] Step 103, calculating the ending threshold parameter according to the coordinates of the path starting point and the path ending point, and combining a preset ending threshold optimization formula.

[0059] Step 104, calculating the expansion step parameter according to the size data of the three-dimensional scene model, and combining a preset expansion step optimization formula.

[0060] Then, according to the size of the scene model after being imported into the program, the expansion step of the branch exploration and the ending threshold parameter when ending near the ending point are set through a preset parameter calculation formula.

[0061] The calculation method of the ending threshold parameter is as follows: the coordinates of the starting point x_init and the ending point x_goal are determined according to the following formula to select the ending threshold, and it is required that no obstacle detection occurs within the range with Thr as the radius:

[0062]

[0063] In the formula, Thr is the ending threshold parameter, are the three-dimensional coordinates of the path starting point, are the three-dimensional coordinates of the path ending point, and a is the first empirical coefficient, which is generally set as 20 by default.

[0064] The calculation method of the expansion step parameter is as follows: the parameters x L , y L , and z L are the lengths of the x-axis, y-axis, and z-axis of the imported scene model, and the selection of the expansion step Delta is determined according to the following formula:

[0065]

[0066] In the formula, Delta is the expansion step parameter, , , are the lengths of the x-axis, y-axis, and z-axis of the three-dimensional scene model, and b is the second empirical coefficient, which is generally set as 50 by default.

[0067] Step 105, executing the RRT algorithm according to the expansion step parameter and the ending threshold parameter to obtain the motion path of the insulated aerial work platform truck.

[0068] Finally, according to the set parameter value, the program is run, the RRT algorithm and the corresponding program running software are adopted, the three-dimensional modeling of the provided scene is based on, the tree branches are freely extended from the starting point in the simulated three-dimensional space, and the tree branches are continuously connected to form new nodes, until the tree branches at the end are connected to the set end point which needs to be planned to reach, the tree branches from the end point to the starting point are connected, and thus the feasible path of the insulating boom truck for automatically avoiding obstacles during movement is obtained.

[0069] Further, as shown in Figure 2 the step 105 can specifically include the following steps:

[0070] Step 1051, taking the path starting point as the root node of the random tree, and obtaining a random node in the non-obstacle region in the three-dimensional scene model through random sampling;

[0071] Step 1052, searching for a target node closest to the random node in the generated random tree, and extending the tree branches in the direction from the target node to the random node according to the expansion step parameter to obtain a new node;

[0072] Step 1053, performing obstacle detection and judgment on the extended path, if the obstacle is touched, re-performing random point sampling and a new round of random tree growth, if no obstacle is touched, adding the new node as a new leaf node to the random tree;

[0073] Step 1054, when the distance between the leaf node on the random tree and the path end point is less than the end point threshold parameter, it is determined that the random tree search is completed, and the motion path generated by the random tree search is output.

[0074] More specifically, regarding the process of extending the tree branches to explore the path, the content includes: (1) the algorithm takes the path starting point x_init as the root node of the random tree Three (T), and obtains a random point x_rand in the non-obstacle region in the environment model through random sampling.

[0075] (2) finding a node x_near closest to x_rand in the generated random tree; extending the tree branches in the direction from x_near to x_rand with Delta as the expansion step, and the new node obtained by growth is x_new.

[0076] (3) performing obstacle detection and judgment on the extended path, if the obstacle is touched, re-performing x_rand sampling and a new round of random tree growth; if no obstacle is touched, adding x_new as a new leaf node to the random tree Three.

[0077] (4) When the Euclidean distance between x on the random tree Three and x_goal is less than a set threshold Thr, the random tree search is completed, and a required feasible path from x_init to x_goal is obtained.

[0078] The above is a detailed description of an embodiment of the motion path planning method based on the RRT algorithm provided in the application. The following is a detailed description of an embodiment of a motion path planning device based on the RRT algorithm provided in the application.

[0079] Please refer to Figure 3 The embodiment provides a motion path planning device based on the RRT algorithm, which comprises:

[0080] The scene model construction unit 201 is configured to construct a three-dimensional scene model of the work scene according to actual work requirements and the work scene.

[0081] The path point determination unit 202 is configured to determine a path starting point and a path ending point in the three-dimensional scene model according to the work requirements.

[0082] The ending point threshold parameter calculation unit 203 is configured to calculate an ending point threshold parameter according to the coordinates of the path starting point and the path ending point, and in combination with a preset ending point threshold optimization formula.

[0083] The expansion step length parameter calculation unit 204 is configured to calculate an expansion step length parameter according to size data of the three-dimensional scene model, and in combination with a preset expansion step length optimization formula.

[0084] The motion path generation unit 205 is configured to execute the RRT algorithm according to the expansion step length parameter and the ending point threshold parameter, so as to obtain a motion path of the insulated boom truck.

[0085] Further, the ending point threshold optimization formula is specifically as follows:

[0086]

[0087] In the formula, Thr is the ending point threshold parameter, are three-dimensional coordinates of the path starting point, respectively, are three-dimensional coordinates of the path ending point, respectively, and a is a first empirical coefficient.

[0088] Further, the expansion step length optimization formula is specifically as follows:

[0089]

[0090] In the formula, Delta is the expansion step length parameter, , , are lengths of x-axis, y-axis and z-axis of the three-dimensional scene model, respectively, and b is a second empirical coefficient.

[0091] Further, the motion path generating unit 205 is specifically configured to:

[0092] take the path starting point as a root node of a random tree, and obtain a random node in a non-obstacle region in the three-dimensional scene model through random sampling;

[0093] search for a target node closest to the random node in the generated random tree, and expand a branch from the target node in a direction of the random node according to an expansion step parameter to grow a new node;

[0094] perform obstacle detection and judgment on the expanded path, if an obstacle is touched, re-perform random point sampling and a new round of random tree growth, if no obstacle is touched, add the new node as a new leaf node to the random tree;

[0095] when a distance between a leaf node on the random tree and the path ending point is less than an ending point threshold parameter, determine that the random tree search is completed, and output a motion path generated by the random tree search.

[0096] The above is a detailed description of an embodiment of the motion path planning device based on the RRT algorithm provided in the application, and the following is a detailed description of an embodiment of a motion path planning terminal based on the RRT algorithm and an embodiment of a computer readable storage medium provided in the application.

[0097] The third embodiment of the application provides a motion path planning terminal based on the RRT algorithm, and the types of the terminal include but are not limited to: a PC host, a server host, a smart phone, etc., and the terminal comprises a memory and a processor, wherein the memory and the processor can be connected through a data bus.

[0098] The memory is used to store program code, and the program code corresponds to the motion path planning method based on the RRT algorithm provided in the first aspect of the application.

[0099] The processor is used to execute the program code, so as to realize the motion path planning method based on the RRT algorithm mentioned in the above embodiments through execution of the program code.

[0100] The fourth aspect of the application provides a computer readable storage medium, and the computer readable storage medium has program code saved therein, and the program code corresponds to the motion path planning method based on the RRT algorithm provided in the first aspect of the application.

[0101] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the terminal, device and unit described above can refer to the corresponding processes in the foregoing method embodiments, and will not be described herein.

[0102] In several embodiments provided in the present application, it should be understood that the disclosed terminal, device and method can be implemented by other manners. For example, the above-described device embodiments are only illustrative, for example, the division of the units is only a logical function division, and actual implementation can have another division manner, for example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection can be indirect coupling or communication connection through some interfaces, devices or units, and can be electrical, mechanical or other forms.

[0103] The terms "first", "second", "third", "fourth" and the like in the description of the specification and the above drawings (if any) are used to distinguish similar objects, and do not necessarily indicate a specific order or sequence. It should be understood that the data thus used can be interchanged under appropriate circumstances, so that the embodiments of the present application described herein, for example, can be implemented in other than the order shown or described herein. In addition, the terms "include" and "have" and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device including a series of steps or units does not necessarily limit to those steps or units clearly listed, but can include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0104] The units described as separate components can or can not be physically separated, and the components shown as units can or can not be physical units, that is, they can be located in one place, or can be distributed on a plurality of network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the embodiment.

[0105] In addition, each functional unit in each embodiment of the present application can be integrated into a processing unit, or each unit can exist physically, or two or more units can be integrated into one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0106] The integrated unit, if implemented in the form of a software function unit and sold or used as an independent product, can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application essentially or say the part that contributes to the prior art or the whole or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a number of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk, and various media that can store program codes.

[0107] The above-described embodiments are only used to illustrate the technical solutions of the present application, rather than limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements for some technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A motion path planning method based on RRT algorithm, characterized in that, The method comprises the following steps: According to the actual operation requirements and operation scenarios, a three-dimensional scene model of the operation scenario is constructed; According to the operation requirements, a path starting point and a path ending point are determined in the three-dimensional scene model; According to the coordinates of the path starting point and the path ending point, a preset ending point threshold optimization formula is combined to calculate an ending point threshold parameter; According to the size data of the three-dimensional scene model, a preset expansion step optimization formula is combined to calculate an expansion step parameter; According to the expansion step parameter and the ending point threshold parameter, an RRT algorithm is executed to obtain a motion path of the insulated boom truck; The ending point threshold optimization formula is specifically: ； wherein Thr is the end-point threshold parameter, are three-dimensional coordinates of the path start point, respectively, are three-dimensional coordinates of the path end point, respectively, and a is a first empirical coefficient. The expansion step optimization formula is specifically: ； where Delta is the step size parameter, , , are the lengths of the x-axis, y-axis, z-axis of the three-dimensional scene model, respectively, and b is a second empirical coefficient.

2. The motion path planning method based on RRT algorithm according to claim 1, wherein, The motion path generation unit is specifically used for: The path starting point is taken as a root node of a random tree, and a random node is obtained in a non-obstacle region of the three-dimensional scene model through random sampling; A target node closest to the random node in the generated random tree is searched, and a new node is grown by expanding a branch from the target node to the direction of the random node according to the expansion step parameter; Obstacle detection is performed on the expanded path, if an obstacle is touched, random point sampling and a new round of random tree growth are performed again, if no obstacle is touched, the new node is added to the random tree as a new leaf node; When the distance between the leaf node on the random tree and the path ending point is less than the ending point threshold parameter, it is determined that the random tree search is completed, and a motion path generated by the random tree search is output.

3. A motion path planning device based on an RRT algorithm, characterized by, ​ ​ ​ ​ ​ ​ ​ ； wherein Thr is the end-point threshold parameter, are three-dimensional coordinates of the path start point, respectively, are three-dimensional coordinates of the path end point, respectively, and a is a first empirical coefficient. ​ ； In the formula, Delta is the step length parameter, , , are the lengths of the x-axis, y-axis, and z-axis of the three-dimensional scene model, respectively, and b is a second empirical coefficient.

4. The motion path planning device based on RRT algorithm according to claim 3, wherein, ​ ​ ​ ​ When the distance between the leaf node on the random tree and the end point of the path is less than the end point threshold parameter, it is determined that the random tree search is completed, and a motion path generated by the random tree search is output.

5. A motion path planning terminal based on an RRT algorithm, characterized by, The method comprises the following steps: a memory and a processor; the memory is used for storing program code corresponding to the motion path planning method based on the RRT algorithm in any one of claims 1 to 2; the processor is used for executing the program code.

6. A computer-readable storage medium, characterized in that, The computer readable storage medium has program code corresponding to the motion path planning method based on the RRT algorithm in any one of claims 1 to 2.

Images