Mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration random rattan

Abstract

The invention discloses a mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration random rattan, and relates to the technical field of mechanical arm path planning, the method comprises the following steps: combining a wavefront algorithm and a weighted undirected graph to determine a wrist point working space communication path of a mechanical arm; constructing two random tree groups; for two random trees in any random tree group, based on a wrist point working space communication path, respectively performing expansion and extension by combining a principal component analysis algorithm, and when a new extension node is obtained, performing a collision and communication test with the other corresponding random tree based on the new extension node to obtain a test result; and when the test result shows that no collision connection exists, marking that the expansion of the random tree groups is completed, and then carrying out node backtracking according to the two expanded random tree groups so as to obtain a barrier-free path of the mechanical arm. According to the invention, the path planning efficiency and adaptability in a narrow environment can be improved.

Description

Technical Field

[0001] The present application relates to the technical field of robotic arm path planning, and particularly to a method for obstacle avoidance path planning of a robotic arm based on a double-balanced rapidly-exploring random vine. Background Art

[0002] Obstacle avoidance path planning is an important issue faced by robotic systems during autonomous operation. In the operating environment of conventional industrial robotic arms, obstacles are relatively sparse, while robotic arms in the nuclear industry have relatively special operating requirements. The robotic arm used for nuclear facility decommissioning needs to enter the interior of the facility through a limited-sized hole reserved on the outer wall without damaging the facility and operate on key components, which means that the obstacle avoidance path planning algorithm needs to calculate the path for the robotic arm to enter and exit the hole.

[0003] In the early research in this field, researchers tended to use graph theory-based methods. Such algorithms have fast calculation speed and low resource consumption, but they require complex mathematical preprocessing of the obstacle environment, such as converting the obstacle information in the task space into the configuration space. These preprocessings are usually much more complex than the algorithms themselves, consuming a large amount of time and being difficult to operate automatically.

[0004] With the improvement of computer computing power, researchers have turned to propose various sampling-based algorithms. Sampling-based algorithms generally do not require complex mathematical processing of the task space or obstacles, and can directly perform path planning after obtaining all or part of the obstacle information. RRT (Rapidly Exploring Random Tree algorithm) is a classic sampling-based algorithm with various variants. However, existing RRT and variant algorithms are inefficient in narrow environments and have weak adaptability to robotic arms. Therefore, there is an urgent need for an efficient and adaptable obstacle avoidance path planning method designed for robotic arms. Summary of the Invention

[0005] The purpose of the present application is to provide a method for obstacle avoidance path planning of a robotic arm based on a double-balanced rapidly-exploring random vine, which can improve the path planning efficiency and adaptability in narrow environments.

[0006] To achieve the above purpose, the present application provides the following solutions:

[0007] The present application provides a method for obstacle avoidance path planning of a robotic arm based on a double-balanced rapidly-exploring random vine, including:

[0008] Obtaining the starting configuration, ending configuration, and zero configuration of the robotic arm;

[0009] Combining the wavefront algorithm and the weighted undirected graph, and determining the connected path of the wrist point workspace of the robotic arm according to the starting configuration, the ending configuration, and the zero configuration;

[0010] Construct four random trees; among them, the first random tree starts from the starting configuration and aims at the zero configuration, and the second random tree starts from the ending configuration and aims at the zero configuration; the third random tree starts from the zero configuration and aims at the starting configuration; the fourth random tree starts from the zero configuration and aims at the ending configuration;

[0011] Divide the first random tree and the third random tree into a first random tree group, and divide the second random tree and the fourth random tree into another random tree group;

[0012] For two random trees in any random tree group, based on the connected path of the wrist point workspace, combined with the principal component analysis algorithm, perform extension and extension respectively, and when a new extended node is obtained, perform collision and connectivity tests based on the new extended node and the corresponding other random tree to obtain test results;

[0013] When the test result indicates collision-free connectivity, mark the extension of the random tree group as completed, and then perform node backtracking according to the two extended random tree groups to obtain the obstacle-free path of the robotic arm.

[0014] According to the specific embodiments provided by the present application, the present application has the following technical effects: The present application combines the wavefront algorithm and the weighted undirected graph to determine the connected path of the wrist point workspace of the robotic arm according to the starting configuration, the ending configuration and the zero configuration. Since the wavefront algorithm is combined with the weighted undirected graph, it can provide more accurate possible positions of the wrist points in the obstacle avoidance path for the subsequent steps, and reduce the data calculation amount in the subsequent steps to a certain extent. Through any random tree group of the double-tree combination, combined with the principal component analysis algorithm, extension and extension are performed respectively, which can ensure the efficiency of the newly extended nodes obtained by the extension and extension, and at the same time ensure the adaptability and feasibility of the path point extension in a narrow environment. Finally, when the new extended node is collision-free and connected to the corresponding other random tree, the extension is completed, and then according to the two extended random tree groups, node backtracking is performed to obtain the obstacle-free path of the robotic arm. At this point, the present application obtains an accurate and feasible path, and the overall planning process is more efficient. Description of the Drawings

[0015] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0016] Figure 1Schematic flowchart of the robotic arm obstacle avoidance path planning method based on the double-balanced rapidly-exploring random tree for an embodiment of the present application.

[0017] Figure 2 For the wrist point p near Schematic diagram of the determination process of the new extension direction of the wrist point when it is near a convex obstacle.

[0018] Figure 3 For the wrist point p near Schematic diagram of the determination process of the new extension direction of the wrist point when it is near the entrance of a slit or a concave obstacle.

[0019] Figure 4 For the wrist point p near Schematic diagram of the determination process of the new extension direction of the wrist point when it is inside a slit.

[0020] Figure 5 Schematic diagram of the structure of a computer device provided for an embodiment of the present application. Detailed implementation manners

[0021] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than 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 efforts shall fall within the scope of protection of the present application.

[0022] To make the objectives, features, and advantages of the present application more obvious and understandable, the present application will be further described in detail below in conjunction with the accompanying drawings and specific implementation manners.

[0023] In an exemplary embodiment, as Figure 1 shown, a robotic arm obstacle avoidance path planning method based on the double-balanced rapidly-exploring random tree is provided. This method is executed by a computer device, and specifically, it can be executed independently by a computer device such as a terminal or a server, or jointly executed by a terminal and a server. In the embodiments of the present application, the following steps 101 to 106 are included.

[0024] Step 101, obtain the starting configuration, the ending configuration, and the zero configuration of the robotic arm. These three configurations correspond to the data information of the starting point, the ending point, and the zero position in the path planning, including joint angle information and wrist point position information.

[0025] Step 102, combine the wavefront algorithm and the weighted undirected graph, and determine the connected path of the wrist point working space of the robotic arm according to the starting configuration, the ending configuration, and the zero configuration; this path is the possible positions of the wrist point in the obstacle avoidance path inferred by the above wavefront algorithm and weighted undirected graph, so as to serve as the data basis for the subsequent steps.

[0026] In an application example, step 102 includes the following steps (1)-(7).

[0027] (1) Determine the first path planning problem starting from the starting configuration WP1 and ending at the zero configuration WP0, i.e., WP1→WP0; determine the second path planning problem starting from the ending configuration WP2 and ending at the zero configuration WP0, i.e., WP2→WP0.

[0028] (2) For any path planning problem, convert it into a sub-problem from WP x to WP y , i.e., WP x →WP y ; where, when WP x is the starting point, WP y is the ending point, which is one sub-problem; when WP x is the ending point, WP y is the starting point, which is another sub-problem.

[0029] (3) For any sub-problem from WP x to WP y , adopt the double-tree pathfinding strategy and the wavefront algorithm to perform path search to obtain an initial connected path; the initial connected path includes multiple spheres, the center of each sphere represents the spatial position of each path point, and the radius of the sphere is the distance between the center of the sphere and the nearest obstacle.

[0030] The following takes WP x →WP y as an example to elaborate on the specific implementation process of the above step (3).

[0031] 1) Generate a sphere with the center of WP x and calculate the corresponding fitness value; specifically, generate a sphere s0 at WP x and WP y , the center of the sphere is WP x , and the radius r0 is the distance between WP x and the nearest obstacle; then calculate the fitness value f0 of the sphere s0 using the following formula:

[0032] 2) Add the sphere s0 and the corresponding fitness value f0 as an adaptation parameter to the fitness sequence Q.

[0033] 3) Establish a sphere tree structure sTree including a preset root node parent0, and add the sphere s0 as a child node of the preset root node to the sphere tree structure; where the preset root node itself has no actual meaning.

[0034] In an application, between step 3) and step 4), it is necessary to determine whether the fitness sequence is empty. If the fitness sequence is not empty, the following steps 4)-8) are looped and executed.

[0035] 4) Delete the spheres whose centers are inside any sphere in the ball tree structure sTree and the corresponding fitness values existing in the fitness sequence Q, so as to achieve an update of the fitness sequence; in short, clear the members in the fitness sequence that are wrapped by any sphere. Through this processing, the number of sampling points based on which new spheres are generated subsequently can be reduced, thereby reducing the computational amount.

[0036] 5) Select the sphere s with the largest fitness value from the fitness sequence after the first update i , and add it to the ball tree structure. At the same time, delete the sphere s i and the corresponding fitness value from the fitness sequence, so as to achieve a second update of the fitness sequence; among them, take the sphere s i with the largest fitness value as the child node of its corresponding parent node parent i , and insert it into the ball tree structure sTree. In addition, the deletion process in this step is because the sphere s i and the corresponding fitness value have become members of the ball tree structure. To avoid their influence on the application of the members in the subsequent fitness sequence, they are deleted.

[0037] 6) Sample (such as uniformly sampling n points) on the surface of the sphere s i to obtain a set of surface sampling points.

[0038] 7) When there is any surface sampling point b in the set of surface sampling points that is outside all the spheres in the ball tree structure (that is, this sampling point is not surrounded by any ball in the ball tree), generate a new sphere s' according to the surface sampling point, calculate the corresponding new fitness value f', and add the new sphere s' and the corresponding new fitness value f' as a new adaptation parameter to the fitness sequence after the second update, so as to achieve a third update of the fitness sequence.

[0039] 8) When the distance between the new sphere s' and the new sphere of another sub-problem does not meet the preset condition, return to the step of deleting the spheres whose centers are inside any sphere in the ball tree structure and the corresponding fitness values existing in the fitness sequence, so as to continue to extend the ball tree structure.

[0040] When the two sub - problems are not considered, it should be: if the distance between the new sphere and the end point is less than the radius r' of the new sphere or a preset threshold, then return the ball - tree structure sTree; however, this application adopts a double - tree extension strategy. Therefore, "the distance between the new sphere s' and the end point" is modified to "the distance between the new sphere s1′ and the new sphere s′2 corresponding to the other sub - problem".

[0041] In addition, if the fitness sequence Q is deleted to an empty set, it indicates that the path generation fails.

[0042] 9) When the distance between the new sphere and the new sphere corresponding to the other sub - problem meets the preset condition, stop extending the ball - tree structure and mark the sub - problem as completed; at this time, since there are two sub - problems, what is obtained are two connected trees.

[0043] 10) According to the ball - tree structures corresponding to the two sub - problems marked as completed, through node backtracking, determine the semi - path between the start point and the end point; specifically, starting from the nodes where the two connected trees are connected to each other, backtrack the parent nodes until the root nodes respectively, and a path composed of spheres connecting WP x and WP y can be obtained, that is, the semi - path between the start point and the end point.

[0044] When the distance between the new sphere and the new sphere corresponding to the other sub - problem meets the preset condition, stop extending the ball - tree structure and mark the sub - problem as completed; at this time, since there are two sub - problems, what is obtained are two connected trees.

[0045] (4) Sample the interior of each sphere of the initial connected path to obtain a plurality of sets of sampling points.

[0046] Specifically, inside each sphere in the initial connected path, a certain number of sampling points are uniformly generated. Example: for a certain sphere s i , the number of sampling points N i satisfies N i =(2r i d i ) 3 , where r i is the radius of the sphere s i , d i is the expected value of the minimum distance between sampling points inside the sphere s i , that is, d i =E(X) and X = min(|p j -p k | j≠k ), p j , p kare all sampling points. For spheres of different radius, d should also be changed to ensure the number and density of sampling points. When the algorithm is actually used, it is necessary to specify the d corresponding to the smallest sphere. min d corresponding to the largest sphere max , then the d corresponding to each sphere is calculated by linear interpolation to obtain a set of sampling points.

[0047] (5) Based on the preset cleaning rules, all the sampling point sets are cleaned up, and then compared with WP x 、WP y A set of unused sampling points is formed; wherein, the preset cleaning rule is: traverse all sampling points in the sampling point set, and if a sampling point is surrounded by two or more (assuming m) spheres, then the sampling point has a probability of m / 2 to be deleted.

[0048] (6) Generate a weighted undirected graph based on the set of unused sampling points; specifically, for any unused sampling point p in the set of unused sampling points i After traversing the set of unused sampling points, if there is any unused sampling point p j If the preset graph rules are met, weighted edges e are used. ij Connect the unused sampling point p i and the sampling point p to be used j , forming a weighted undirected graph. Among them, e ij =||p i -p j || / r i +r j , the preset graph rules include:

[0049] 1) Standby sampling point p i and the sampling point p to be used j The distance between them satisfies ||p i -p j ||<βd max ; where β is the expansion coefficient, d max The preset desired minimum distance between sample points within the maximum sphere.

[0050] 2) Among them, r i 、r j They are the sampling points p to be used i , standby sampling point p j The distance to the nearest obstacle, φ is the preset sphere coincidence.

[0051] 3)r i ,r j >r min ; Among them, r min The preset minimum obstacle distance.

[0052] (7) Use the Dijkstra algorithm to find the shortest path connecting WP x and WP y in the weighted undirected graph to obtain the connected path of the wrist point workspace of the robotic arm. Specifically, the path is a string of spheres, the centers of the spheres are the spatial positions of the points in the found shortest path, and the radius of the sphere is the distance between the center of the sphere and the nearest obstacle.

[0053] Step 103, construct four random trees, and the root nodes are the starting configuration, the ending configuration, the zero configuration, and the zero configuration in sequence; specifically, the first random tree starts from the starting configuration and aims at the zero configuration, the second random tree starts from the ending configuration and aims at the zero configuration; the third random tree starts from the zero configuration and aims at the starting configuration; the fourth random tree starts from the zero configuration and aims at the ending configuration; each node on the random tree contains the joint angle information and the wrist point position information of the robotic arm.

[0054] Step 104, divide the first random tree and the third random tree into a first random tree group, and divide the second random tree and the fourth random tree into another random tree group; the wrist point positions corresponding to the root nodes of the four random trees are all in a specific sphere in the connected path of the wrist point workspace. Subsequently, the 4 random trees attempt to extend towards their respective targets.

[0055] Step 105, for any two random trees in a random tree group, based on the connected path of the wrist point workspace, combined with the principal component analysis algorithm, perform extended extensions respectively, and when a new extended node is obtained, perform collision and connectivity tests based on the new extended node and the corresponding other random tree to obtain test results.

[0056] In a specific application, the steps of performing extended extensions respectively based on the connected path of the wrist point workspace and combined with the principal component analysis algorithm include:

[0057] (1) Take the first sphere in the connected path of the wrist point workspace as a reference point, and explore based on the reference point and a preset probability σ to determine a random sampling point.

[0058] Among them, the process of determining or updating the random sampling point involves a sampling algorithm, and the probability σ used in the sampling algorithm conforms to a Gaussian distribution, and "exploration" or "expansion" is selected according to the value of σ. Let the center of the current reference sphere (i.e., the reference point) be p i and the radius be r i .

[0059] The sampling algorithm performs "exploration" near the reference sphere with a probability of σ / 2: with the position of the center of the sphere p i as the mean and r i· Generate a random sampling point p in the task space according to a Gaussian distribution with σ / 3 as the standard deviation rand ; or, if σ > 1.8, perform uniform sampling within the entire task space.

[0060] The sampling algorithm can also attempt to "expand" with a probability of 1 - σ / 2: use the center p of the next sphere of the reference sphere i+1 as the mean, and r i+1 · Generate a sampling point p in the task space according to a Gaussian distribution with σ / 3 as the standard deviation rand ; or, if the reference sphere is the last sphere on the connected path of the wrist point workspace, directly set the center p of the reference sphere i as the random sampling point p rand .

[0061] At the beginning of the algorithm, σ is set to 1 / 3, and during the running of the algorithm, "exploration" or "expansion" is performed based on probability. When σ increases (more collisions, lower extension success rate), the probability of the algorithm taking exploration will increase. In addition, the value of σ is restricted within the range of [0.1, 2].

[0062] (2) Based on the random sampling point p rand , determine the wrist point p near , and the corresponding joint angle is q near , and extend in the direction of the random sampling point from the wrist point p near with a fixed step size Δs to obtain a new wrist point p new ; the wrist point p near is the wrist point position of the node on the random tree that is closest to the random sampling point p rand .

[0063] (3) Inverse solve the new wrist point p new to obtain the first three joint angles of the robotic arm (if there are multiple solutions, select the one closest to q near ).

[0064] (4) Based on the first three joint angles of the robotic arm, perform a collision test to obtain a first collision result; specifically, perform collision detection on the first three links of the robotic arm. If a collision occurs at this time, it means that regardless of how the angles of the last three joint angles change, the robotic arm will collide, and the first three joint angles (i.e., move the wrist point) need to be changed to get out of the collision state.

[0065] (5) When the first collision result indicates a collision, based on the random sampling point and the wrist point p near, combined with the principal component analysis algorithm, adjust the first three joint angles of the robotic arm to avoid collision, and obtain the updated first three joint angles; obtain the last three joint angles of the robotic arm corresponding to the starting point, and determine the new node q according to the updated first three joint angles and the last three joint angles new .

[0066] In one application, step (5) above specifically includes the following processing:

[0067] 1) According to the randomly sampled points and the wrist point p near , calculate the original extension direction of the wrist point The calculation formula is as follows:

[0068] 2) Generate a three-dimensional sphere with the wrist point p near as the center of the sphere, and sample inside the three-dimensional sphere to obtain an internal sampling point set; specifically, generate n random internal sampling points uniformly distributed within a three-dimensional sphere with the wrist point p near as the center of the sphere and R (preset value) as the radius, where n = (2R / d) 3 , d is the expected distance to its nearest point inside the sphere, that is, d = E(X) and X = min(|p j - p k | j≠k ), and it must satisfy d < Δs.

[0069] 3) For any internal sampling point in the internal sampling point set, perform a collision test between the wrist point p near and the internal sampling point, and divide the internal sampling point into set P obs or set P free according to the collision result; among them, the collision results of the internal sampling points in the set P obs are all collisions, and the collision results of the internal sampling points in the set P free are all non-collisions.

[0070] 4) Based on a preset judgment rule, according to the set P obs and set P free , determine the surrounding obstacle characteristics of the wrist point p near , and combine the original extension direction of the wrist point to determine the new extension direction of the wrist point; among them, the preset judgment rule incorporates the principal component analysis algorithm, and the preset judgment rule includes:

[0071] When the set P obs is an empty set, or the number of elements in the set P obs is below a preset numerical value, the surrounding obstacle characteristics of the wrist point p near are the wrist point p nearThe surrounding obstacles are sparse. The failure to successfully extend this time is accidental, and no new p is generated new , perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point, and directly generate a new p rand .

[0072] When the number of elements in the set P free is below the preset number value, the feature of the surrounding obstacles of the wrist point p near is that the wrist point p near is in a state where it is surrounded by obstacles and difficult to extend. Perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point, and regenerate p rand .

[0073] When the number of elements in the set P obs is greater than the preset number value, perform a principal component analysis on the set P obs to determine the first eigenvector and the first eigenvalue; specifically, perform a principal component analysis on the set P obs , arrange the first eigenvalue and the first eigenvector in descending order of the eigenvalue, and take the first eigenvector α1 corresponding to the largest first eigenvalue λ1 as the first principal component direction of the sample, and λ1 is the variance of the sample in this direction. Subsequently, use the mean obs of the set P as the center of the sphere, and construct a confidence ellipsoid E obs according to the first eigenvector and the first eigenvalue; specifically, the axis direction vectors of the confidence ellipsoid E obs are the first eigenvector α i , and the corresponding semi-axis lengths satisfy The size of K is selected according to the confidence level and degrees of freedom of the χ 2 distribution.

[0074] When the number of elements in the set P free is greater than the preset number value, perform a principal component analysis on the set P free to determine the second eigenvector β i and the second eigenvalue μ i ; use the mean free of the set P as the center of the sphere, and construct a confidence ellipsoid E free .

[0075] When all elements in the set P free are not within the confidence ellipsoid E obs , the feature of the surrounding obstacles of the wrist point p near is that the wrist point pnear Near a convex obstacle.

[0076] When there is an element in the set P free inside the confidence ellipsoid E obs , and the mean value of the set P free and the mean value of the set P obs satisfy the first condition, the obstacle feature around the wrist point p near is that the wrist point p near is near the slit entrance or a concave obstacle; where the first condition is: the mean value of the set P free is not inside the confidence ellipsoid E obs , the mean value of the set P obs is inside the confidence ellipsoid E free .

[0077] When there is an element in the set P free inside the confidence ellipsoid E obs , and the mean value of the set P free and the mean value of the set P obs satisfy the second condition, the obstacle feature around the wrist point p near is that the wrist point p near is inside the slit; the second condition is: the mean value of the set P free is inside the confidence ellipsoid E obs , the mean value of the set P obs is inside the confidence ellipsoid E free .

[0078] When there is an element in the set P free inside the confidence ellipsoid E obs , and the mean value of the set P free and the mean value of the set P obs do not satisfy the first condition and the second condition, the obstacle feature around the wrist point p near is that the wrist point p near is near a convex obstacle.

[0079] Correspondingly, the process of determining the new extension direction of the wrist point includes:

[0080] As Figure 2 shown, when the obstacle feature around the wrist point p near is that the wrist point p near is near a convex obstacle, determine the new extension direction of the wrist point according to the original extension direction of the wrist point, the first eigenvector and the first eigenvalue; specifically, let A be the plane with the first eigenvector α min corresponding to the minimum value of the first eigenvalue λ as the normal vector, and project the original extension direction of the wrist point Projection on plane A As the new extension direction, and then generate p new . This allows the new node to "cling" along the surface of the convex obstacle.

[0081] Such as Figure 3 shown, when the wrist point p near The surrounding obstacle feature is that when the wrist point p near is near the slit entrance or concave obstacle, two new extension directions of the wrist point are generated; among them, one new extension direction of the wrist point is determined according to the original extension direction of the wrist point, the first eigenvector and the first eigenvalue, that is, the same as the convex obstacle extension direction, responsible for continuing to explore along the obstacle surface; the other new extension direction of the wrist point is determined according to the mean value of the set P obs and the wrist point p near , and the calculation formula is responsible for "drilling" into the narrow channel.

[0082] Such as Figure 4 shown, when the wrist point p near The surrounding obstacle feature is that when the wrist point p near is inside the slit, the new extension direction of the wrist point is determined according to the second eigenvector; specifically, taking β max as the growth direction, this direction has a high coincidence degree with the slit direction and can effectively prevent frequent collisions inside the slit. According to the properties of eigenvalues and eigenvectors, the principal component direction solved by principal component analysis is actually a straight line, and the positive and negative signs of the direction need to be further determined. In order to let the new node continue to extend deeper into the slit, trace back several parent nodes of p near along the random tree and find the average position of these parent nodes. The new extension direction should be the direction away from the average position on the principal component straight line.

[0083] 5) Extend based on the new extension direction of the wrist point to obtain an optimized wrist point. Specifically, according to the new extension direction of the wrist point (coinciding with ), extend with a fixed step size Δs to generate one or two corresponding optimized wrist points.

[0084] 6) Inverse solve the optimized wrist point to obtain the first three optimized joint angles of the robotic arm.

[0085] 7) Based on the first three optimized joint angles of the robotic arm, perform a collision test to obtain a second collision result.

[0086] 8) If the second collision result indicates no collision, mark the first three optimized joint angles as the updated first three joint angles.

[0087] 9) If the second collision result indicates a collision, perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine the random sampling point.

[0088] (6) When the first collision result indicates no collision, obtain the last three joint angles of the manipulator corresponding to the start, and determine a new node q according to the first three joint angles and the last three joint angles. new . Among them, the first three joint angles can include the base, shoulder, and elbow joints to control the spatial position of the wrist point; the last three joint angles control the posture of the end effector, such as pitch, yaw, and roll.

[0089] (7) Based on the new node q new , perform an overall collision detection of the manipulator to obtain a third collision result.

[0090] (8) When the third collision result indicates a collision, obtain the pre-collision spatial position and the spatial position at the time of collision of the end effector of the manipulator; specifically, when the corresponding first three linkages do not collide, but the whole manipulator collides after being combined with the last three joint angles into the new node q new , it means that the collision point is after the wrist point (the linkage close to the end). At this time, it is necessary to record the pre-collision spatial position and the spatial position at the time of collision

[0091] (9) Based on the Jacobian matrix, determine the optimized node according to the pre-collision spatial position and the spatial position at the time of collision; specifically, the following steps are adopted:

[0092] 91) According to the pre-collision spatial position and the spatial position at the time of collision, use the formula: Calculate the movement vector caused by the end effector's collision. [[ID=2?]]

[0093] 92) Calculate the first three rows J new of the Jacobian matrix corresponding to the manipulator at the new node q p .

[0094] 93) Calculate the new joint angles based on the first three rows of the Jacobian matrix and update q new : Δq is a fixed and preset joint angle step, is the pseudo-inverse of J p .

[0095] (10) Based on the optimized node, perform an overall collision detection of the manipulator to obtain a fourth collision result.

[0096] (11) When the fourth collision result indicates a collision, perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point; specifically, the first update of the preset probability is: σ ← 1.1σ, and there are no new extended nodes at this time.

[0097] (12) When the fourth collision result indicates no collision, mark the optimized node as a new extended node, with q near as the parent node, attach it to the random tree, then perform a second update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point; specifically, the second update of the preset probability is: σ ← 0.9σ.

[0098] (13) When the third collision result indicates no collision, perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point.

[0099] Step 106, when the test result indicates collision-free connectivity, mark the expansion of the random tree group as completed. At this time, the first random tree and the third random tree are connected, and the extension stops. Then, according to the two expanded random tree groups, perform node backtracking to obtain the obstacle-free path of the robotic arm; since the root nodes of the third random tree and the fourth random tree are the same, if the first random tree is connected to the third random tree and the second random tree is connected to the fourth random tree, it can be considered that the start point and the end point are connected, that is, the first random tree The third random tree = the fourth random tree The second random tree. At this time, backtrack all the connected nodes to obtain the obstacle-free path of the planned robotic arm.

[0100] If connectivity cannot be achieved, return to step 105 and re-expand and extend.

[0101] In summary, the present application proposes a Bidirectional Balanced Rapidly Exploring Random Vine (BBRRV) algorithm, which can solve the problems of low path planning efficiency and poor adaptability of existing robotic arm path planning algorithms in narrow environments. The present application can be applied to a sampling-based obstacle avoidance path planning method for robotic arms in known narrow environments, especially for the obstacle avoidance requirements of 6-degree-of-freedom industrial robotic arms in scenarios such as nuclear facility decommissioning.

[0102] In a specific nuclear facility decommissioning scenario, the tasks performed by the robotic arm are to move some radioactive materials, cut radioactive pipes, etc. When the nuclear facility performs maintenance operations, it is necessary to open holes in the outer wall and extend the robotic arm from the outside through these holes for operation. To reduce radioactive contamination, it is necessary to minimize the size of the holes in the outer wall as much as possible, and the hole positions and sizes are generally fixed during the initial construction stage and become preset holes for maintenance. In short, in a nuclear facility, it is necessary for the robotic arm to extend into one hole for operation and then withdraw from the hole and extend into another hole.

[0103] During the path planning process using the method of the present application, the following parameters need to be adjusted: the wrist point extension step size Δd, which is generally slightly smaller than the size of the smallest known obstacle in the environment; the joint angle extension step size Δq, which is non-linearly positively correlated with Δd and needs to be adjusted through multiple experiments; the preset value R used during the adjustment of the first three joint angles of the robotic arm, which is generally set to 1.5 to 3 times of Δd and needs to be adjusted through multiple experiments; after the above parameters are adjusted to available values, as long as the operating environment remains unchanged subsequently, there is no need to adjust them anymore.

[0104] Based on the same inventive concept, the embodiment of the present application also provides a system for implementing the method involved above. The implementation solution provided by this system to solve problems is similar to the implementation solution described in the above method. Therefore, the specific limitations in one or more system embodiments provided below can refer to the limitations on the method in the above text and will not be elaborated here.

[0105] A robotic arm obstacle avoidance path planning system based on a double-balanced rapidly-exploring random vine provided by the present application includes:

[0106] A data acquisition module, configured to acquire the starting configuration, ending configuration, and zero position configuration of the robotic arm.

[0107] An initial path calculation module, configured to determine the connected path of the wrist point workspace of the robotic arm according to the starting configuration, the ending configuration, and the zero position configuration by combining the wavefront algorithm and the weighted undirected graph.

[0108] A random tree construction module, configured to construct four random trees; among them, the first random tree starts from the starting configuration and targets the zero position configuration, the second random tree starts from the ending configuration and targets the zero position configuration; the third random tree starts from the zero position configuration and targets the starting configuration; the fourth random tree starts from the zero position configuration and targets the ending configuration.

[0109] A double-tree combination module, configured to divide the first random tree and the third random tree into a first random tree group, and divide the second random tree and the fourth random tree into another random tree group.

[0110] A tree expansion and extension module is used to perform expansion and extension on two random trees in any random tree group respectively based on the connected path of the wrist point workspace, in combination with the principal component analysis algorithm. When a new extended node is obtained, a collision and connection test is performed between the new extended node and the corresponding other random tree to obtain a test result.

[0111] A path generation module is used to mark the completion of the expansion of the random tree group when the test result indicates collision-free connection. Then, based on the two expanded random tree groups, node backtracking is performed to obtain the obstacle-free path of the robotic arm.

[0112] In an exemplary embodiment, a computer device is provided. The computer device can be a server or a terminal, and its internal structure diagram can be as Figure 5 shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O), and a communication interface. Among them, the processor, the memory, and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The input / output interface of the computer device is used to exchange information between the processor and external devices. The communication interface of the computer device is used to communicate with external terminals through a network connection. When the computer program is executed by the processor, it realizes the robotic arm obstacle avoidance path planning method based on the double-balanced rapidly exploring random vine.

[0113] Those skilled in the art can understand that Figure 5 the structure shown in

[0114] is only a block diagram of some structures related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine some components, or have different component arrangements.

[0115] In an exemplary embodiment, a computer device is provided, including: a memory, a processor, and a computer program stored on the memory and executable on the processor. The processor executes the computer program to implement the steps in the above method embodiments.

[0116] In an exemplary embodiment, a computer program product is provided, including a computer program which, when executed by a processor, implements the steps in the above method embodiments.

[0117] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use, and processing of relevant data need to comply with relevant regulations.

[0118] Those of ordinary skill in the art can understand that all or part of the processes of implementing the above method embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above method embodiments. Among them, any reference to a memory, database, or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memories. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0119] The databases involved in the embodiments provided in this application can include at least one of relational databases and non-relational databases. Non-relational databases can include distributed databases based on blockchain, etc., and are not limited thereto. The processors involved in the embodiments provided in this application can be general-purpose processors, central processors, graphics processors, digital signal processors, programmable logic devices, data processing logics based on quantum computing, etc., and are not limited thereto.

[0120] The technical features of the above embodiments can be combined arbitrarily. For the sake of concise description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope recorded in this specification.

[0121] Specific examples are used in this article to elaborate on the principles and implementation manners of this application. The description of the above embodiments is only used to help understand the method and its core idea of this application; at the same time, for those of ordinary skill in the art, according to the idea of this application, there will be changes in the specific implementation manners and application scopes. To sum up, the content of this specification should not be construed as a limitation to this application.

Claims

1. A manipulator obstacle avoidance path planning method based on a double-balanced fast-exploring random vine, characterized in that, The method comprises: Obtain the starting configuration, end configuration and zero position configuration of the robotic arm; In combination with a wavefront algorithm and a weighted undirected graph, a connection path of the wrist workspace of the robotic arm is determined according to the starting configuration, the end configuration, and the zero position configuration; Construct four random trees; the first random tree starts with the starting configuration and targets the zero-position configuration; the second random tree starts with the ending configuration and targets the zero-position configuration; the third random tree starts with the zero-position configuration and targets the starting configuration; the fourth random tree starts with the zero-position configuration and targets the ending configuration; Dividing the first random tree and the third random tree into a first random tree group, and dividing the second random tree and the fourth random tree into another random tree group; For any two random trees in any random tree group, based on the wrist workspace connectivity path, combined with the principal component analysis algorithm, they are extended and extended respectively. When a new extended node is obtained, a collision and connectivity test is performed based on the new extended node and the corresponding other random tree to obtain a test result. When the test result indicates collision-free connectivity, the random tree group expansion is marked complete, and then node backtracking is performed based on the two random tree groups that have completed expansion to obtain an obstacle-free path for the robotic arm.

2. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 1, wherein Combining a wavefront algorithm and a weighted undirected graph, determining a connection path of the wrist workspace of the manipulator according to the starting configuration, the end configuration, and the zero position configuration, including: Determine a first path planning problem with the starting point configuration as the starting point and the zero-position configuration as the ending point; determine a second path planning problem with the ending point configuration as the starting point and the zero-position configuration as the ending point; For any path planning problem, it is converted into a sub-problem from WP x to WP y ; where, when WP x is the starting point, WP y is the ending point, which is a sub-problem; when WP x is the ending point, WP y is the starting point, which is another sub-problem; For any sub-problem from WP x to WP y a double-tree pathfinding strategy and a wavefront algorithm are adopted to perform path search to obtain an initial connected path; the initial connected path includes a plurality of spheres, the center of each sphere represents the spatial position of each path point, and the radius of the sphere is the distance between the center of the sphere and the nearest obstacle; Sampling the interior of each sphere of the initial connected path to obtain a plurality of sampling point sets; Based on the preset cleaning rules, clean the sampling points in all the sets of sampling points, and then form a set of sampling points to be used together with WP x and WP y ; generating a weighted undirected graph according to the set of unused sampling points; Use the Dijkstra algorithm to find the shortest path connecting WP in the weighted undirected graph x and WP y to obtain the connected path of the wrist point workspace of the robotic arm.

3. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 2, wherein, For WP x to WP y For any sub-problem, a dual-tree pathfinding strategy and a wavefront algorithm are used to perform path search to obtain an initial connected path, including: With WP x Generate a sphere with the center of the sphere, and calculate the corresponding fitness value; Adding the sphere and the corresponding fitness value as an adaptation parameter to the fitness sequence; Establishing a ball tree structure including a preset root node, and adding the ball as a child node of the preset root node to the ball tree structure; Deleting the spheres in the fitness sequence whose centers are inside any sphere in the sphere tree structure and the corresponding fitness values, so as to achieve an update of the fitness sequence; Select the sphere s with the largest fitness value from the fitness sequence after one update i , and add it to the sphere tree structure. At the same time, delete the sphere s i and the corresponding fitness value from the fitness sequence to achieve the secondary update of the fitness sequence; Sampling is performed on the surface of the sphere s i to obtain a set of surface sampling points; When any surface sampling point in the surface sampling point set is outside all spheres in the sphere tree structure, a new sphere is generated according to the surface sampling point, a corresponding new fitness value is calculated, and the new sphere and the corresponding new fitness value are added as a new fitness parameter to the fitness sequence after the second update, so as to achieve a third update of the fitness sequence; When the distance between the new sphere and the new sphere corresponding to another subproblem does not meet the preset condition, returning to the step of deleting the spheres in the fitness sequence whose centers are inside any sphere in the sphere tree structure and the corresponding fitness values, so as to continue extending the sphere tree structure; When the distance between the new sphere and the new sphere corresponding to another subproblem satisfies a preset condition, stop extending the sphere tree structure and mark the subproblem as completed; According to the ball tree structure corresponding to the two marked completed sub-problems, determine the half path between the starting point and the end point by backtracking the nodes; The initial connected path is determined based on the half paths between the starting point and the end point corresponding to the two path planning problems.

4. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 1, wherein, Based on the wrist workspace connectivity path, combined with the principal component analysis algorithm, the following steps are performed to expand and extend the wrist workspace: Taking the first sphere in the connected path of the wrist workspace as a reference point, and performing exploration based on the reference point and a preset probability to determine a random sampling point; Based on the random sampling points, determine the wrist point p near , and based on the wrist point p near extend in the direction of the random sampling points to obtain a new wrist point p new ; the wrist point p near is the wrist point position of the node on the random tree with the closest wrist point position to the random sampling points; Inverse-solve the new wrist point p new to obtain the first three joint angles of the robotic arm; Performing a collision test based on the first three joint angles of the robotic arm to obtain a first collision result; When the first collision result indicates a collision, based on the randomly sampled points and the wrist point p near , combined with the principal component analysis algorithm, adjust the first three joint angles of the robotic arm to get out of the collision, and obtain the updated first three joint angles; obtain the last three joint angles of the robotic arm corresponding to the start, and determine the new node q according to the updated first three joint angles and the last three joint angles new ; When the first collision result indicates that no collision has occurred, obtain the last three joint angles of the manipulator corresponding to the start, and determine the new node q based on the first three joint angles and the last three joint angles new ; Based on the new node q new , perform an overall collision detection of the robotic arm to obtain a third collision result; When the third collision result indicates that a collision occurs, obtaining a spatial position of the end effector of the robotic arm before the collision and a spatial position of the end effector during the collision; Determining an optimization node based on the Jacobian matrix according to the spatial position before the collision and the spatial position at the time of the collision; Based on the optimized node, performing overall collision detection of the robotic arm to obtain a fourth collision result; When the fourth collision result indicates that a collision occurs, performing a first update on the preset probability, and then returning to the step of performing exploration based on the reference point and the preset probability to determine a random sampling point; When the fourth collision result indicates that no collision occurs, marking the optimized node as a new extended node, then performing a second update on the preset probability, and then returning to the step of exploring based on the reference point and the preset probability to determine a random sampling point; When the third collision result indicates that no collision occurs, the preset probability is first updated, and then the process returns to the step of performing exploration based on the reference point and the preset probability to determine a random sampling point.

5. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 4, wherein Based on the random sampling points and the wrist point p near , combined with the principal component analysis algorithm, adjust the first three joint angles of the robotic arm to avoid collision, and obtain the updated first three joint angles, including: According to the randomly sampled points and the wrist point p near , calculate the original extension direction of the wrist point; Taking the wrist point p near as the center to generate a three-dimensional sphere, and sampling inside the three-dimensional sphere to obtain an internal sampling point set; For any internal sampling point in the set of internal sampling points, perform the wrist point p near Collision test with the internal sampling point, and divide the internal sampling point into set P obs Or set P free ; Among them, the collision results of the internal sampling points in the set P obs All show collisions, and the collision results of the internal sampling points in the set P free All show no collisions; Based on a preset judgment rule, according to the set P obs and the set P free , determine the obstacle characteristics around the wrist point p near , and determine the new extension direction of the wrist point in combination with the original extension direction of the wrist point; wherein, the principal component analysis algorithm is incorporated in the preset judgment rule; Extending the wrist point based on the new extension direction to obtain an optimized wrist point; Inversely solving the optimized wrist point to obtain the first three optimized joint angles of the robotic arm; Performing a collision test based on the first three optimized joint angles of the robotic arm to obtain a second collision result; If the second collision result indicates that no collision occurs, marking the first three optimized joint angles as the updated first three joint angles; If the second collision result indicates that a collision has occurred, the preset probability is first updated, and then the process returns to the step of performing exploration based on the reference point and the preset probability to determine a random sampling point.

6. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 5, wherein The preset judgment rules include: When the set P obs is an empty set, or when the number of elements in the set P obs is below a preset quantity value, the obstacle characteristics around the wrist point p near are that the obstacles around the wrist point p near are sparse. Perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point; When the number of elements in the set P free is below a preset quantity value, the feature of the surrounding obstacles of the wrist point p near is that the wrist point p near is in a state where it is surrounded by obstacles and difficult to extend. Perform a first update on the preset probability, and then return to the step of exploring based on the reference point and the preset probability to determine a random sampling point; When the number of elements in the set P obs is greater than a preset numerical value, perform principal component analysis on the set P obs to determine the first eigenvector and the first eigenvalue; with the mean value of the set P obs as the center of the sphere, construct a confidence ellipsoid E obs ; When the number of elements in the set P free is greater than a preset numerical value, perform principal component analysis on the set P free to determine the second eigenvector and the second eigenvalue; with the mean of the set P free as the center of the sphere, construct a confidence ellipsoid E free ; When all elements in the set P free are not within the confidence ellipsoid E obs , the surrounding obstacle feature of the wrist point p near is that the wrist point p near is near a convex obstacle; When there are elements in the set P free inside the confidence ellipsoid E obs and, when the mean value of the set P free satisfies the first condition, the characteristics of the surrounding obstacles of the wrist point p obs are that the wrist point p near is near the slit entrance or concave obstacle; where, the first condition is: the mean value of the set P near is not inside the confidence ellipsoid E free , the mean value of the set P obs is inside the confidence ellipsoid E obs ; free ; When there are elements in the set P free inside the confidence ellipsoid E obs , and the mean of the set P free and the mean of the set P obs satisfy the second condition, the surrounding obstacle feature of the wrist point p near is that the wrist point p near is inside the slit; the second condition is: the mean of the set P free is inside the confidence ellipsoid E obs , the mean of the set P obs is inside the confidence ellipsoid E free ; When the set P free has elements within the confidence ellipsoid E obs , and the mean of the set P free does not satisfy the first condition and the second condition with the mean of the set P obs , the surrounding obstacle feature of the wrist point p near is that the wrist point p near is near a convex obstacle.

7. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 6, wherein The process of determining the new extension direction of the wrist point includes: When the wrist point p near The surrounding obstacle feature is the wrist point p near When it is near a convex obstacle, determine the new extension direction of the wrist point according to the original extension direction of the wrist point, the first eigenvector and the first eigenvalue; When the wrist point p near The surrounding obstacle feature is the wrist point p near When it is near the slit entrance or the concave obstacle, two new extension directions of the wrist point are generated; among them, one new extension direction of the wrist point is determined according to the original extension direction of the wrist point, the first eigenvector and the first eigenvalue, and the other new extension direction of the wrist point is determined according to the mean value of the set P obs and the wrist point p near is determined; When the wrist point p near The surrounding obstacle feature is the wrist point p near When it is inside the slit, determine the new extension direction of the wrist point according to the second eigenvector.

8. The method for obstacle avoidance path planning of a robotic arm based on a double-balanced fast-exploring random vine according to claim 2, wherein Generating a weighted undirected graph according to the set of unused sampling points includes: For any standby sampling point p in the set of standby sampling points i After traversing the set of unused sampling points, if there is any unused sampling point p j If the preset graph rules are met, weighted edges e are used. ij Connect the unused sampling point p i and the sampling point p to be used j , forming a weighted undirected graph; where e ij = ||p i - p j || / r i + r j , and the preset graph rules include: Standby sampling point p i The distance between the standby sampling point p j and satisfies ||p i - p j || < βd max ; where β is the expansion coefficient, and d max is the preset expectation of the minimum distance of the sampling points within the maximum sphere; where r i and r j are the distances between the sampling points p i , p j to be used and the nearest obstacle respectively, and φ is the preset sphere coincidence degree; r i , r j > r min ; wherein, r min is the preset minimum obstacle distance.

Images