Robotic obstacle avoidance path planning method based on double-balanced fast exploration random fern

Abstract

The application discloses a mechanical arm obstacle avoidance path planning method based on a double-balanced fast exploration random vine, relates to the technical field of mechanical arm path planning, and comprises the following steps: combining a wavefront algorithm and a weighted undirected graph to determine a wrist point workspace connected path of a mechanical arm; two random tree groups are constructed; for two random trees in any random tree group, the random trees are respectively extended based on the wrist point workspace connected path and a principal component analysis algorithm, and when a new extended node is obtained, collision and connection tests are performed on the new extended node and a corresponding another random tree to obtain a test result; when the test result represents no collision and connection, the random tree group extension is marked as completed, and then node backtracking is performed according to the two random tree groups that have been extended to obtain an obstacle-free path of the mechanical arm. The application can improve the path planning efficiency and adaptability in a narrow environment.

Description

Technical Field

[0001] This application relates to the field of robotic arm path planning technology, and in particular to a robotic arm obstacle avoidance path planning method based on dual-balance rapid exploration of random vines. Background Technology

[0002] Obstacle avoidance path planning is a crucial problem faced by robotic systems during autonomous operation. While obstacles are relatively sparse in the typical industrial robotic arm operating environment, nuclear industry robotic arms have unique operational requirements. Robotic arms used for nuclear facility decommissioning need to enter the facility through pre-drilled openings in the exterior walls without damaging the facility to operate critical components. This means that obstacle avoidance path planning algorithms must calculate the path that allows the robotic arm to enter and exit the openings.

[0003] In early research in this field, researchers tended to use graph-based methods. These algorithms are computationally fast and resource-efficient, but they require complex mathematical preprocessing of the obstacle environment, such as transforming obstacle information from the task space to the configuration space. This preprocessing is often far more complex than the algorithm itself, time-consuming, and difficult to automate.

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

[0005] The purpose of this application is to provide a robotic arm obstacle avoidance path planning method based on dual-balance rapid exploration of random vines, which can improve the efficiency and adaptability of path planning in narrow environments.

[0006] To achieve the above objectives, this application provides the following solution:

[0007] This application provides a method for obstacle avoidance path planning for a robotic arm based on rapid exploration of random vines using a dual-balance system, including:

[0008] Obtain the starting configuration, ending configuration, and zero-position configuration of the robotic arm;

[0009] Combining wavefront algorithm and weighted undirected graph, the wrist workspace connectivity path of the robotic arm is determined based on the starting point configuration, the ending point configuration and the zero position configuration;

[0010] Construct four random trees: the first random tree starts with the starting configuration and targets the zero configuration; the second random tree starts with the ending configuration and targets the zero configuration; the third random tree starts with the zero configuration and targets the starting configuration; and the fourth random tree starts with the zero configuration and targets the ending configuration.

[0011] The first random tree and the third random tree are divided into a first random tree group, and the second random tree and the fourth random tree are divided into another random tree group;

[0012] For any two random trees in any random tree group, based on the wrist point workspace connectivity path, combined with the principal component analysis algorithm, they are extended respectively. When a new extended node is obtained, a collision and connectivity test is performed between the new extended node and the corresponding other random tree to obtain the test results.

[0013] When the test results indicate that there is no collision and the connection is complete, the expansion of the random tree group is marked as complete. Then, based on the two completed random tree groups, node backtracking is performed to obtain the obstacle-free path of the robotic arm.

[0014] According to the specific embodiments provided in this application, this application has the following technical effects: This application combines wavefront algorithm and weighted undirected graph to determine the wrist point workspace connectivity path of the robotic arm based on the starting point configuration, ending point configuration, and zero-position configuration. Combining wavefront algorithm with weighted undirected graph provides more accurate possible positions of wrist points in the obstacle avoidance path for subsequent steps, reducing the amount of data computation in subsequent steps to a certain extent. By using any random tree group of dual-tree combinations and combining it with principal component analysis algorithm for expansion, the efficiency of the newly extended nodes can be guaranteed, while ensuring the adaptability and feasibility of path point expansion in narrow environments. Finally, when the new extended node is connected to the corresponding other random tree without collision, the expansion is completed. Then, based on the two expanded random tree groups, node backtracking is performed to obtain the obstacle-free path of the robotic arm. Thus, this application obtains an accurate and feasible path, and the overall planning process is more efficient. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0016] Figure 1This is a flowchart illustrating a robotic arm obstacle avoidance path planning method based on rapid exploration of random vines using dual-balance, as provided in an embodiment of this application.

[0017] Figure 2 wrist point p near A schematic diagram illustrating the process of determining the new extension direction of the wrist point when near a convex obstacle.

[0018] Figure 3 wrist point p near A schematic diagram illustrating the process of determining the new extension direction of the wrist point when it is near a slit entrance or concave obstacle.

[0019] Figure 4 wrist point p near A schematic diagram illustrating the process of determining the new extension direction of the wrist point when inside the slit.

[0020] Figure 5 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0021] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0022] To make the objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0023] In one exemplary embodiment, such as Figure 1 As shown, a method for obstacle avoidance path planning of a robotic arm based on dual-balance rapid exploration of random vines is provided. This method is executed by a computer device, specifically by a computer device such as a terminal or server alone, or by a terminal and server together. In the embodiments of this application, it includes the following steps 101 to 106.

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

[0025] Step 102: Combining the wavefront algorithm and weighted undirected graph, determine the wrist point workspace connectivity path of the robotic arm based on the starting point configuration, the ending point configuration, and the zero position configuration. This path is the possible wrist point position in the obstacle avoidance path inferred through the wavefront algorithm and weighted undirected graph, which serves as the data basis for subsequent steps.

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

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

[0028] (2) For any path planning problem, convert it to WP x To WP y The subproblem of WP x →WP y Among them, WP x When WP is the starting point y The endpoint is a subproblem; WP x When the endpoint is WP y Let this be the starting point, and let it be another subproblem.

[0029] (3) Targeting WP x To WP y For any subproblem, a dual-tree pathfinding strategy and wavefront algorithm are used to search for a path to obtain an initial connected path. The initial connected path includes multiple spheres, where the center of each sphere represents the spatial location of each path point, and the radius of each sphere is the distance between the center of the sphere and the nearest obstacle.

[0030] The following uses WP x →WP y The specific implementation process of step (3) above will be illustrated using an example.

[0031] 1) Using WP x Generate a sphere for the center and calculate the corresponding fitness value; specifically, in WP x Place with WP y Generate a sphere s0 with center WP. x The radius r0 is WP x The distance to the nearest obstacle; then the fitness value f0 of the sphere s0 is calculated using the following formula:

[0032] 2) Add the sphere s0 and its corresponding fitness value f0 as an fitness 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; wherein, the preset root node itself has no practical meaning.

[0034] In one 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, then the following steps 4)-8) are executed repeatedly.

[0035] 4) Delete the spheres and their corresponding fitness values ​​in the fitness sequence Q whose centers are inside any sphere in the sTree structure, so as to update the fitness sequence once; in short, remove the members in the fitness sequence that are wrapped by any sphere. Through this process, the number of sampling points used to generate new spheres can be reduced, thereby reducing the amount of computation.

[0036] 5) Select the sphere s with the largest fitness value from the updated fitness sequence. i And add it to the ball tree structure, while adding the sphere s i The corresponding fitness value is removed from the fitness sequence to achieve a secondary update of the fitness sequence; wherein, the sphere s with the largest fitness value is removed. i As its corresponding parent node i The child nodes are inserted into the sTree structure. Additionally, the deletion process in this step is due to the sphere s... i The corresponding fitness value has been used as a member of the ball tree structure. To avoid affecting the application of members in the subsequent fitness sequence, it has been deleted.

[0037] 6) In the sphere s i The surface is sampled (e.g., n points are sampled uniformly) to obtain a set of surface sampling points.

[0038] 7) When any surface sampling point b in the set of surface sampling points is outside all spheres in the ball tree structure (i.e., the sampling point is not surrounded by any sphere in the ball tree), a new sphere s' is generated based on the surface sampling point, the corresponding new fitness value f' is calculated, and the new sphere s' and the corresponding new fitness value f' are added as a new fitness parameter to the fitness sequence after the second update, so as to realize the third update of the fitness sequence.

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

[0040] Without considering the two subproblems, the correct approach would be: if the distance between the new sphere and the endpoint is less than the radius r' of the new sphere or a preset threshold, then return the sphere tree structure sTree. However, this application adopts a dual-tree extension strategy, so "the distance between the new sphere s' and the endpoint" is modified to "the distance between the new sphere s1′ and the new sphere s′2 corresponding to the other subproblem".

[0041] Additionally, if the fitness sequence Q is deleted to an empty set, it indicates that path generation has failed.

[0042] 9) When the distance between the new sphere and the new sphere corresponding to another subproblem meets the preset condition, stop extending the sphere tree structure and mark the subproblem as complete; at this time, since there are two subproblems, two connected trees are obtained.

[0043] 10) Based on the ball tree structure corresponding to the two completed subproblems, determine the half-path between the start and end points by backtracking the nodes; specifically, starting from the nodes of the two connected trees that are connected to each other, backtrack from the parent node of each tree to the root node to obtain a connection WP. x With WP y A path consisting of spheres is a semi-path between the starting point and the ending point.

[0044] 11) Determine the initial connected path based on the half-paths between the start and end points of the two path planning problems. Since both path planning problems have a zero-position configuration, the initial connected path can be obtained by concatenating the two half-paths.

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

[0046] Specifically, within each sphere in the initial connected path, a certain number of sampling points are uniformly generated. Example: For a sphere s... i The number of sampling points N i Satisfying N i =(2r i d i ) 3 , where r i It is a sphere s i radius, d i It is a sphere s i The expected value of the minimum distance between the inner sampling points, i.e., d i =E(X) and X = min(|p j -p k | j≠k ), p j p kAll are sampling points. For spheres of different radii, d should also vary to ensure the number and density of sampling points. In practical application of the algorithm, it is necessary to specify d corresponding to the smallest sphere. min d corresponding to the largest sphere max Then, the d corresponding to each sphere is calculated using linear interpolation, thereby obtaining a set of sampling points.

[0047] (5) Based on preset cleaning rules, perform sampling point cleaning on all the sampling point sets, and then compare with WP. x WP y A set of sampling points to be used is formed; 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 an m / 2 probability of being 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 any unused sampling point p exists... j If the preset graph rules are met, then weighted edge e is used. ij Connect the sampling point p to be used i With the sampling point p to be used j This forms a weighted undirected graph. Where e ij =||p i -p j || / r i +r j The preset diagram rules include:

[0049] 1) Sample point p to be used i With the sampling point p to be used j The distance between them satisfies ||p i -p j ||<βd max Where β is the expansion coefficient, and d max This is the preset expectation of the minimum distance between sampling points within the largest sphere.

[0050] 2) Where, r i r j These are the sampling points to be used, p. i , sampling point p j The distance between the ball and the nearest obstacle, φ is the preset ball weight fit.

[0051] 3)r i ,r j >r min ; where r min Set the minimum obstacle distance.

[0052] (7) Use Dijkstra's algorithm to find the connection WP in the weighted undirected graph. x With WP y The shortest path is found to obtain the connected path of the wrist point workspace of the robotic arm. Specifically, the path is a series of spheres, where the center of each sphere is the spatial position of each point in the shortest path, and the radius of each sphere is the distance between the center of the sphere and the nearest obstacle.

[0053] Step 103: Construct four random trees, with the root nodes being the starting configuration, ending configuration, zero configuration, and zero position configuration, respectively. Specifically, the first random tree starts with the starting configuration and targets the zero configuration; the second random tree starts with the ending configuration and targets the zero configuration; the third random tree starts with the zero configuration and targets the starting configuration; and the fourth random tree starts with the zero configuration and targets the ending configuration. Each node in the random tree contains the joint angle information and wrist 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 located in a specific sphere in the connected path of the wrist point workspace. Subsequently, the four random trees attempt to extend towards their respective targets.

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

[0056] In a specific application, based on the wrist point workspace connectivity path and combined with the principal component analysis algorithm, the following steps are performed for extension and expansion:

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

[0058] The process of determining or updating random sampling points involves a sampling algorithm. The probability σ used in the sampling algorithm follows a Gaussian distribution, and the choice between "exploration" and "expansion" is based on the value of σ. Let p be the center of the current reference sphere (i.e., the reference point). i Radius r i .

[0059] The sampling algorithm "explores" the vicinity of the reference sphere with a probability of σ / 2: with the center p of the sphere as the reference point. i The position is the mean, r iA Gaussian distribution with standard deviation σ / 3 generates a random sampling point p in the task space. rand Alternatively, if σ > 1.8, then uniform sampling is performed throughout the entire task space.

[0060] The sampling algorithm can also attempt to "extend" with a probability of 1-σ / 2: using the center p of the next sphere from the reference sphere. i+1 Let r be the mean. i+1 • A Gaussian distribution with a standard deviation of σ / 3 generates sampling points p in the task space. rand Alternatively, if the reference sphere is the last sphere on the connected path in the wrist point's workspace, then directly set the center p of the reference sphere. i Set as random sampling point p rand .

[0061] At the beginning of the algorithm, σ is set to 1 / 3. As the algorithm runs, it performs "exploration" or "expansion" based on probability. When σ increases (more collisions, lower success rate of expansion), the probability of the algorithm taking the exploration step will increase. In addition, the value of σ is limited to the range of [0.1,2].

[0062] (2) Based on the random sampling point p rand Determine wrist point p near The corresponding joint angle is q. near And based on the wrist point p near Extend the line in the direction of the random sampling point with a fixed step size Δs to obtain a new wrist point p. new The wrist point p near On the random tree, the distance of the wrist point position from the random sampling point p rand The wrist point position of the nearest node.

[0063] (3) Inverse solution of the new wrist point p new To obtain the first three joint angles of the robotic arm (if there are multiple solutions, choose the one with q). near The most recent one).

[0064] (4) Based on the first three joint angles of the robotic arm, a collision test is performed to obtain the first collision result; specifically, the first three links of the robotic arm are collision detected. If a collision occurs at this time, it means that no matter how the angles of the last three joint angles change, the robotic arm will collide and it is necessary to change the first three joint angles (i.e., move the wrist point) to get out of the collision state.

[0065] (5) When the first collision result indicates that a collision has occurred, based on the random sampling point and the wrist point p nearUsing principal component analysis, the first three joint angles of the robotic arm are adjusted to avoid collisions, resulting in updated first three joint angles. The last three joint angles of the robotic arm corresponding to the initial position are then obtained. Based on the updated first and last three joint angles, a new node q is determined. new .

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

[0067] 1) Based on the random sampling points and the wrist point p near Calculate the original extension direction of the wrist point. The calculation formula is as follows:

[0068] 2) Using the wrist point p near A three-dimensional sphere is generated with the center as the sphere, and samples are taken inside the three-dimensional sphere to obtain an internal sampling point set; specifically, with wrist point p as the center... near Within a three-dimensional sphere with center R and radius R (a preset value), n random internal sampling points are generated in a uniform distribution, where n = (2R / d). 3 d is the expected distance of the sphere to its nearest point, i.e., 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 set of internal sampling points, execute the wrist point p. near Collision testing with the internal sampling points is performed, and the internal sampling points are assigned to set P based on the collision results. obs or set P free ; wherein, the set P obs The collision results of all internal sampling points in the set P are collisions. free The collision results of the internal sampling points were all negative.

[0070] 4) Based on the preset judgment rules, according to the set P obs and set P free Determine wrist point p near The characteristics of surrounding obstacles are considered, and the new extension direction of the wrist point is determined by combining the original extension direction of the wrist point; wherein, the preset judgment rule incorporates the principal component analysis algorithm, and the preset judgment rule includes:

[0071] When the set P obs It is an empty set, or, the set P obs When the number of elements is below a preset value, wrist point p near The surrounding obstacle features wrist point p nearThe sparse surrounding obstacles meant that the failure to extend successfully this time was accidental, and no new p was generated. new The preset probability is updated first, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point, directly generating a new p. rand .

[0072] When the set P free When the number of elements is below a preset value, wrist point p near The surrounding obstacle features wrist point p near When the system is surrounded by obstacles and unable to extend its reach, the preset probability is updated for the first time. Then, the system returns to the step of exploring based on the reference point and the preset probability to determine random sampling points, and p is regenerated. rand .

[0073] When the set P obs When the number of elements in set P is greater than a preset value, the set P is... obs Principal component analysis is performed to determine the first eigenvector and the first eigenvalue; specifically, for set P... obs Principal component analysis is performed, and the first eigenvalues ​​and first eigenvectors are arranged in descending order of eigenvalue. The first eigenvector α1 corresponding to the largest first eigenvalue λ1 is taken as the first principal component direction of the sample, and λ1 is the variance of the sample in that direction. Subsequently, the set P is used as the basis for further analysis. obs mean Using the first eigenvector and the first eigenvalue as the center, construct the confidence ellipsoid E. obs Specifically, the confidence ellipsoid E obs The direction vectors of each axis are the first eigenvectors α. i The corresponding semi-shaft length satisfies The size of K is according to χ 2 The choice of confidence level and degrees of freedom for the distribution.

[0074] When the set P free When the number of elements in set P is greater than a preset value, the set P is... free Principal component analysis was performed to determine the second eigenvector β. i With the second eigenvalue μ i ; with the set P free mean Using the second eigenvector and the second eigenvalue as the center, construct the confidence ellipsoid E. free .

[0075] When the set P free All elements are not located in the confidence ellipsoid E. obs Inner time, wrist point p near The surrounding obstacle features wrist point pnear It is located near a convex obstacle.

[0076] When the set P free There exist elements in the confidence ellipsoid E. obs Within, and, the set P free The mean of the set P obs When the mean of the wrist point p satisfies the first condition, near The surrounding obstacle features wrist point p near Located near a slit entrance or a concave barrier; wherein, the first condition is: the set P free The mean is not in the confidence ellipsoid E obs The set P obs The mean of the confidence ellipsoid E free .

[0077] When the set P free There exist elements in the confidence ellipsoid E. obs Within, and, the set P free The mean of the set P obs When the mean of the wrist point p satisfies the second condition, near The surrounding obstacle features wrist point p near Located inside the slit; the second condition is: the set P free The mean of the confidence ellipsoid E obs The set P obs The mean of the confidence ellipsoid E free .

[0078] When the set P free There exist elements in the confidence ellipsoid E. obs Within, and, the set P free The mean of the set P obs When the mean value does not meet the first condition and the second condition, the wrist point p near The surrounding obstacle features wrist point p near It is located near a convex obstacle.

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

[0080] like Figure 2 As shown, when wrist point p near The surrounding obstacle features wrist point p near When near a convex obstacle, the new extension direction of the wrist point is determined based on the original extension direction of the wrist point, the first feature vector, and the first feature value; specifically, let A be the first feature vector α corresponding to the minimum value of the first feature value λ. min The plane that is the normal vector extends the original direction of the wrist point. Projection onto plane A As a new direction of extension, p is generated. new This allows new nodes to "climb" along the surface of the convex barrier.

[0081] like Figure 3 As shown, when wrist point p near The surrounding obstacle features wrist point p near When near a slit entrance or concave obstacle, two new extension directions for the wrist point are generated; one of these new extension directions is determined based on the original extension direction of the wrist point, the first feature vector, and the first feature value, i.e., it is related to... The convex obstacle extends in the same direction, responsible for continuing exploration along the obstacle surface; the other wrist point extends in a new direction according to the set P. obs The mean and wrist point p near Confirmed, the calculation formula is as follows: They are responsible for "drilling" into the narrow passage.

[0082] like Figure 4 As shown, when wrist point p near The surrounding obstacle features wrist point p near When inside the slit, the new extension direction of the wrist point is determined based on the second eigenvector; specifically, with β... max The growth direction coincides significantly with the slit direction, effectively preventing frequent collisions within the slit. Based on the properties of eigenvalues ​​and eigenvectors, the principal component direction obtained through principal component analysis is actually a straight line, requiring further determination of its sign. To allow new nodes to extend deeper into the slit, the process backtracks p along the random tree. near Find several parent nodes and calculate the average position of these parent nodes. The new extension direction should be the direction away from the average position on the principal component line.

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

[0084] 6) Inversely 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, a collision test is performed to obtain the second collision result.

[0086] 8) If the second collision result indicates that no collision has occurred, then the first three optimized joint angles are marked as the updated first three joint angles.

[0087] 9) If the second collision result indicates that a collision has occurred, the preset probability is updated for the first time, and then the process returns 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 that no collision has occurred, obtain the last three joint angles of the robotic arm corresponding to the start, and determine the new node q based on the first three joint angles and the last three joint angles. new The first three joint angles may include the base, shoulder, and elbow joints to control the spatial position of the wrist point; the latter three joint angles control the attitude of the end effector, such as pitch, yaw, and roll.

[0089] (7) Based on the new node q new The overall collision detection of the robotic arm is performed to obtain the third collision result.

[0090] (8) When the third collision result indicates that a collision has occurred, obtain the spatial position of the end effector of the robotic arm before the collision and the spatial position at the time of the collision; specifically, when the first three links do not collide, but are combined with the last three joint angles to form a new node q new If the entire robotic arm then collides, it indicates that the point of collision is after the wrist point (near the end effector link). In this case, it is necessary to record the spatial position of the robotic arm's end effector before the collision. Spatial position during collision

[0091] (9) Based on the Jacobian matrix, determine the optimized nodes according to the spatial positions before and during the collision; specifically, the following steps are adopted:

[0092] 91) Based on the spatial position before and during the collision, the formula is used: Calculate the movement vector caused by the collision by the end effector.

[0093] 92) Calculate the robot arm's position at node q based on the movement vector. new The first three rows of the corresponding Jacobian matrix J p .

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

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

[0096] (11) When the fourth collision result indicates that a collision has occurred, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point; specifically, the preset probability is updated for the first time as follows: σ←1.1σ, at which time there are no new extension nodes.

[0097] (12) When the fourth collision result indicates that no collision has occurred, the optimized node is marked as a new extended node, with q near The parent node is attached to the random tree. Then, the preset probability is updated a second time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point. Specifically, the preset probability is updated a second time as follows: σ←0.9σ.

[0098] (13) When the third collision result indicates that no collision has occurred, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point.

[0099] Step 106: When the test result indicates collision-free connectivity, mark the random tree group expansion as complete. At this point, the first random tree and the third random tree are connected, and the expansion stops. Then, based on the two expanded random tree groups, node backtracking is performed to obtain the obstacle-free path of the robotic arm. Since the root nodes of the third and fourth random trees are the same, if the first and third random trees are connected and the second and fourth random trees are connected, the starting point and the ending point can be considered connected, i.e., the first random tree... Third random tree = Fourth random tree The second random tree. At this point, backtracking through all connected nodes yields the planned unobstructed path for the robotic arm.

[0100] If the connection fails, return to step 105 and perform the extension again.

[0101] In summary, this application proposes a Bidirectional Balanced Rapidly Exploring RandomVine (BBRRV) algorithm, which addresses the problems of low efficiency and poor adaptability in existing robotic arm path planning algorithms in confined environments. This application can be applied to sampling-based obstacle avoidance path planning methods for robotic arms in known confined environments, particularly for the obstacle avoidance requirements of 6-DOF industrial robotic arms in scenarios such as nuclear facility decommissioning.

[0102] In a specific nuclear facility decommissioning scenario, the robotic arm performs tasks such as moving radioactive materials and cutting radioactive conduits. During maintenance operations, holes need to be made in the external wall so the robotic arm can extend through them. To minimize radioactive contamination, the size of these openings must be minimized, and their location and size are typically fixed during the initial construction phase, becoming pre-designed holes for maintenance. In short, a nuclear facility involves the robotic arm extending into one opening, then withdrawing and extending into another.

[0103] When using the method of this application for path planning, the following parameters need to be adjusted: wrist extension step length Δd, which is generally slightly smaller than the size of the smallest known obstacle in the environment; joint angle extension step length Δq, which is non-linearly positively correlated with Δd and requires multiple trials for adjustment; the preset value R used in adjusting the first three joint angles of the robotic arm is generally set to 1.5 to 3 times Δd and requires multiple trials for adjustment; once the above parameters are adjusted to usable values, they do not need to be adjusted again as long as the working environment remains unchanged.

[0104] Based on the same inventive concept, this application also provides a system for implementing the methods described above. The solution provided by this system is similar to the solution described in the methods above; therefore, specific limitations in one or more system embodiments provided below can be found in the limitations of the methods described above, and will not be repeated here.

[0105] This application provides a robotic arm obstacle avoidance path planning system based on dual-balance rapid exploration of random vines, including:

[0106] The data acquisition module is used to acquire the starting configuration, ending configuration, and zero-position configuration of the robotic arm.

[0107] The initial path calculation module is used to combine the wavefront algorithm and the weighted undirected graph to determine the wrist point workspace connectivity path of the robotic arm based on the starting point configuration, the ending point configuration, and the zero position configuration.

[0108] The random tree construction module is used to construct four random trees. The first random tree starts with the starting configuration and targets the zero configuration; the second random tree starts with the ending configuration and targets the zero configuration; the third random tree starts with the zero configuration and targets the starting configuration; and the fourth random tree starts with the zero configuration and targets the ending configuration.

[0109] The dual-tree combination module is used 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] The tree expansion module is used to expand and extend two random trees in any random tree group based on the wrist point workspace connectivity path and in combination with the principal component analysis algorithm. When a new extended node is obtained, a collision and connectivity test is performed between the new extended node and the corresponding other random tree to obtain the test results.

[0111] The path generation module is used to mark the completion of the expansion of the random tree group when the test result indicates that there is no collision and the random tree group has been expanded. Then, based on the two completed random tree groups, the module performs node backtracking to obtain the obstacle-free path of the robotic arm.

[0112] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 5 As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and databases. The internal memory provides the environment for the operating system and computer programs stored in the non-volatile storage media. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communicating with external terminals via a network connection. When the computer program is executed by the processor, it implements a robotic arm obstacle avoidance path planning method based on dual-balance fast exploration of random vines.

[0113] Those skilled in the art will understand that Figure 5 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

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

[0115] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0116] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described 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 used for analysis, data stored, data displayed, 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 the relevant data must comply with relevant regulations.

[0118] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. 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), magnetic 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 take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

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

[0120] The technical features of the above embodiments can be combined in any way. For the sake of brevity, 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, they should be considered to be within the scope of this specification.

[0121] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for obstacle avoidance path planning for a robotic arm based on rapid exploration of random vines using a dual-balance system, characterized in that, The method includes: Obtain the starting configuration, ending configuration, and zero-position configuration of the robotic arm; Combining wavefront algorithm and weighted undirected graph, the wrist workspace connectivity path of the robotic arm is determined based on the starting point configuration, the ending point configuration, and the zero-position configuration; including: Determine a first path planning problem with the starting configuration as the starting point and the zero-position configuration as the ending point; determine a second path planning problem with the ending configuration as the starting point and the zero-position configuration as the ending point; For any path planning problem, it can be transformed into WP x to WP y Subproblems; among which, WP x When the starting point is WP y This is the endpoint, and it is a subproblem. WP x When it is the end point, WP y Let this be the starting point, and the next subproblem. against WP x to WP y For any subproblem, a dual-tree pathfinding strategy and wavefront algorithm are used to search for a path to obtain an initial connected path. The initial connected path includes multiple spheres, where the center of each sphere represents the spatial location of each path point, and the radius of each sphere is the distance between the center of the sphere and the nearest obstacle. For each sphere in the initial connected path, the interior is sampled to obtain multiple sets of sampling points; Based on preset cleaning rules, sampling point cleaning is performed on all the sampling point sets, and then... WP x , WP y This constitutes a set of sampling points to be used; Generate a weighted undirected graph based on the set of sampling points to be used; Use Dijkstra's algorithm to find connections in the weighted undirected graph. WP x and WP y Find the shortest path to obtain the wrist point workspace connectivity path of the robotic arm; Construct four random trees: the first random tree starts with the starting configuration and targets the zero configuration; the second random tree starts with the ending configuration and targets the zero configuration; the third random tree starts with the zero configuration and targets the starting configuration; and the fourth random tree starts with the zero configuration and targets the ending configuration. The first random tree and the third random tree are divided into a first random tree group, and the second random tree and the fourth random tree are divided into another random tree group; For any two random trees in any random tree group, based on the wrist point workspace connectivity path, combined with the principal component analysis algorithm, they are extended respectively. When a new extended node is obtained, a collision and connectivity test is performed between the new extended node and the corresponding other random tree to obtain the test results. When the test results indicate that there is no collision and the connection is complete, the expansion of the random tree group is marked as complete. Then, based on the two completed random tree groups, node backtracking is performed to obtain the obstacle-free path of the robotic arm.

2. The obstacle avoidance path planning method for a robotic arm based on rapid exploration of random vines using dual-balance as described in claim 1, characterized in that, against WP x to WP y For any subproblem, a dual-tree pathfinding strategy and wavefront algorithm are used to perform path search to obtain an initial connected path, including: by WP x Generate a sphere for the center and calculate the corresponding fitness value; The sphere and its corresponding fitness value are added as an fitness parameter to the fitness sequence; Establish a sphere tree structure including a preset root node, and add the sphere as a child node of the preset root node to the sphere tree structure; Delete the spheres and their corresponding fitness values ​​in the fitness sequence whose centers are located inside any sphere in the sphere tree structure, so as to update the fitness sequence once. Select the sphere with the highest fitness value from the updated fitness sequence. s i And add it to the ball tree structure, while also adding the sphere. s i The corresponding fitness value is deleted from the fitness sequence to achieve a secondary update of the fitness sequence; In the sphere s i The surface is sampled to obtain a set of surface sampling points; When any surface sampling point in the set of surface sampling points is outside all spheres in the sphere tree structure, a new sphere is generated based on the surface sampling point, the 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 realize the 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, return to the step of deleting the sphere and its corresponding fitness value that exist in the fitness sequence and whose center is inside any sphere in the sphere tree structure, so as to continue to extend the sphere tree structure; When the distance between the new sphere and the new sphere corresponding to another subproblem meets a preset condition, the extension of the sphere tree structure stops, and the subproblem is marked as complete. Based on the ball tree structure corresponding to the two marked subproblems, the half-path between the start point and the end point is determined by backtracking the nodes; Determine the initial connected path based on the half-paths between the start and end points corresponding to the two path planning problems.

3. The obstacle avoidance path planning method for a robotic arm based on rapid exploration of random vines using dual-balance as described in claim 1, characterized in that, Based on the connected paths in the wrist point workspace, and combined with the principal component analysis algorithm, the following steps are performed for expansion and extension: Using the first sphere in the connected path of the wrist point workspace as a reference point, and exploring based on the reference point and a preset probability, a random sampling point is determined. 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 point to obtain a new wrist point. p new The wrist point p near The wrist position is the position of the node on the random tree whose wrist position is closest to the random sampling point. Inverse solution of the new wrist point p new To obtain the first three joint angles of the robotic arm; Based on the first three joint angles of the robotic arm, a collision test is performed to obtain the first collision result; When the first collision result indicates that a collision has occurred, it is based on the random sampling point and the wrist point. p near Using principal component analysis, the first three joint angles of the robotic arm are adjusted to avoid collisions, resulting in updated first three joint angles. The last three joint angles of the robotic arm corresponding to the initial position are then obtained. Based on the updated first and last three joint angles, a new node is determined. q new ; When the first collision result indicates that no collision has occurred, the last three joint angles of the robotic arm corresponding to the start are obtained, and a new node is determined based on the first three joint angles and the last three joint angles. q new ; Based on the new node q new The overall collision detection of the robotic arm is performed to obtain the third collision result; When the third collision result indicates that a collision has occurred, the spatial position of the end effector of the robotic arm before the collision and the spatial position at the time of the collision are obtained. Based on the Jacobian matrix, the optimized node is determined according to the spatial position before the collision and the spatial position at the time of the collision; Based on the optimized nodes, the overall collision detection of the robotic arm is performed to obtain the fourth collision result; When the fourth collision result indicates that a collision has occurred, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point; When the fourth collision result indicates that no collision has occurred, the optimized node is marked as a new extended node, the preset probability is updated for the second time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point. When the third collision result indicates that no collision has occurred, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point.

4. The obstacle avoidance path planning method for a robotic arm based on rapid exploration of random vines under dual-balance as described in claim 3, characterized in that, Based on the random sampling points and the wrist points p near Using principal component analysis (PCA) algorithm, the first three joint angles of the robotic arm are adjusted to avoid collisions, resulting in updated first three joint angles, including: Based on random sampling points and the wrist points p near Calculate the original extension direction of the wrist point; With the wrist point p near A three-dimensional sphere is generated with the center of the sphere as the center, and samples are taken inside the three-dimensional sphere to obtain a set of internal sampling points; For any internal sampling point in the set of internal sampling points, execute the wrist point. p near Collision testing with the internal sampling points is performed, and the internal sampling points are assigned to set P based on the collision results. obs or set P free ; wherein, the set P obs The collision results of all internal sampling points in the set P are collisions. free The collision results of the internal sampling points in the data were all negative. Based on preset judgment rules, according to the set P obs and set P free Determine the wrist point p near The characteristics of surrounding obstacles are used to determine the new extension direction of the wrist point, which is determined in combination with the original extension direction of the wrist point; wherein, the preset judgment rule incorporates the principal component analysis algorithm. The wrist point is extended based on the new extension direction to obtain an optimized wrist point; The optimized wrist point is solved in reverse to obtain the first three optimized joint angles of the robotic arm; Based on the first three optimized joint angles of the robotic arm, a collision test is performed to obtain the second collision result; If the second collision result indicates that no collision occurred, then the first three optimized joint angles are marked as the updated first three joint angles; If the second collision result indicates that a collision has occurred, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point.

5. The obstacle avoidance path planning method for a robotic arm based on rapid exploration of random vines under dual-balance as described in claim 4, characterized in that, The preset judgment rules include: When the set P obs It is an empty set, or, the set P obs When the number of elements is below a preset value, wrist point p near The surrounding obstacles are characterized by wrist points. p near If the surrounding obstacles are sparse, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point; When the set P free When the number of elements is below a preset value, wrist point p near The surrounding obstacles are characterized by wrist points. p near When the area is surrounded by obstacles and difficult to extend, the preset probability is updated for the first time, and then the process returns to the step of exploring based on the reference point and the preset probability to determine the random sampling point. When the set P obs When the number of elements in set P is greater than a preset value, the set P is... obs Principal component analysis is performed to determine the first eigenvector and the first eigenvalue; using the set P obs The mean of the sphere is the center of the sphere. Based on the first eigenvector and the first eigenvalue, a confidence ellipsoid is constructed. E obs ; When the set P free When the number of elements in set P is greater than a preset value, the set P is... free Principal component analysis is performed to determine the second eigenvector and the second eigenvalue; using the set P free The mean of the sphere is the center of the sphere. Based on the second eigenvector and the second eigenvalue, a confidence ellipsoid is constructed. E free ; When the set P free None of the elements are located within the confidence ellipsoid. E obs Inner time, wrist point p near The surrounding obstacles are characterized by wrist points. p near It is located near a convex obstacle; When the set P free There exist elements in the confidence ellipsoid. E obs Within, and, the set P free The mean of the set P obs When the mean of the wrist point meets the first condition, p near The surrounding obstacles are characterized by wrist points. p near Located near a slit entrance or a concave barrier; wherein, the first condition is: the set P free The mean is not on the confidence ellipsoid. E obs The set P obs The mean of the confidence ellipsoid E free ; When the set P free There exist elements in the confidence ellipsoid. E obs Within, and, the set P free The mean of the set P obs When the mean of the wrist point satisfies the second condition, p near The surrounding obstacles are characterized by wrist points. p near Located inside the slit; the second condition is: the set P free The mean of the confidence ellipsoid E obs The set P obs The mean of the confidence ellipsoid E free ; When the set P free There exist elements in the confidence ellipsoid. E obs Within, and, the set P free The mean of the set P obs When the mean does not meet the first condition and the second condition, the wrist point p near The surrounding obstacles are characterized by wrist points. p near It is located near a convex obstacle.

6. The obstacle avoidance path planning method for a robotic arm based on rapid exploration of random vines using dual-balance as described in claim 5, characterized in that, The process of determining the new extension direction of the wrist point includes: When the wrist point p near The surrounding obstacles are characterized by wrist points. p near When near a convex obstacle, the new extension direction of the wrist point is determined based on the original extension direction of the wrist point, the first feature vector, and the first feature value. When the wrist point p near The surrounding obstacles are characterized by wrist points. p near When near a slit entrance or concave obstacle, two new wrist point extension directions are generated; one new wrist point extension direction is determined based on the original wrist point extension direction, the first feature vector, and the first feature value, while the other new wrist point extension direction is determined based on the set P. obs Mean and wrist point p near Sure; When the wrist point p near The surrounding obstacles are characterized by wrist points. p near When inside the slit, the new extension direction of the wrist point is determined based on the second feature vector.

7. The obstacle avoidance path planning method for a robotic arm based on rapid exploration of random vines under dual-balance as described in claim 1, characterized in that, Based on the set of sampling points to be used, a weighted undirected graph is generated, including: For any one of the unused sampling points in the set of unused sampling points p i After traversing the set of unused sampling points, if any unused sampling point exists... p j If the preset graph rules are met, then weighted edges are used. e ij Connect the sampling points to be used p i With the sampling points to be used p j This forms a weighted undirected graph; in, The preset diagram rules include: Sampling points to be used p i With the sampling points to be used p j The distance between them satisfies ;in, β The coefficient of thermal expansion is 1 / 3. d max The preset expectation is the minimum distance between sampling points within the largest sphere; ;in, r i , r j These are the sampling points to be used. p i Sampling points to be used p j Distance to the nearest obstacle It is a preset ball weight fit; ;in, r min Set the minimum obstacle distance.

Images