A mechanical arm path planning method based on hybrid sampling and dynamic step length

Abstract

The application provides a mechanical arm path planning method based on hybrid sampling and dynamic step length, and relates to the technical field of mechanical arm motion planning. The method comprises the following steps: acquiring mechanical arm working environment information, establishing a tree structure according to initial joint angles and target joint angles, and initializing an expansion step length; selecting a sampling strategy according to a random probability; adjusting the expansion step length according to whether the distance between two trees is shortened by a sampling point, performing collision detection and adding a new node; greedily expanding a target tree; updating an operation tree and the target tree; if the two trees are connected, returning a planned path, entering a path post-processing stage, and performing path optimization by using redundant point removal and triangular interpolation. The mechanical arm path planning method based on hybrid sampling and dynamic step length can quickly plan a high-quality motion path for a mechanical arm in a high-dimensional complex space.

Description

Technical Field

[0001] This invention relates to the field of robotic arm motion planning technology, and in particular to a robotic arm path planning method based on hybrid sampling and dynamic step size. Background Technology

[0002] Path planning for robotic arms is a key problem in the field of robotic arms. It involves finding a collision-free path that starts at the initial configuration and ends at the target configuration, given the initial and target configurations of the robotic arm. Collision-free means that all configurations along the path are distributed within the unobstructed configuration space of the robotic arm.

[0003] Robotic arms often interact with the real physical world, and their operating environment is relatively complex; they generally have multiple degrees of freedom, and the spatial dimension of planning is relatively high; response time is also a requirement. Therefore, how to quickly plan high-quality motion paths for robotic arms in high-dimensional complex spaces is a huge challenge.

[0004] Graph search planning methods, exemplified by the A* algorithm, transform the path planning problem into a graph search problem by discretizing the workspace. While widely used in mobile robot path planning, they are limited to low-dimensional path planning problems due to limitations in algorithm complexity and computational cost. The artificial potential field method introduces the concept of a field: the target point generates an attractive field, and obstacles generate a repulsive field. The robot moves towards the target point under the combined force of these two potential fields. However, the artificial potential field method is prone to getting trapped in local minima, and constructing the potential field function is difficult for high-dimensional complex planning problems.

[0005] The Fast Expanding Random Tree (FRB) method grows a random tree by incrementally sampling randomly within the planning space until it reaches the target point, returning a feasible planned path. This method only performs collision detection on the sampled points in the planning space, without needing to establish explicit expressions for obstacles in the environment. It combines high efficiency and probabilistic completeness, and has been widely studied and applied. However, it is worth noting that the FRB employs a uniform random sampling strategy, which is inefficient and results in uncontrollable quality and significant randomness in the planned path. Summary of the Invention

[0006] The purpose of this invention is to provide a robotic arm path planning method based on hybrid sampling and dynamic step size, in order to solve the problems of low efficiency, long time consumption and poor path planning quality in the prior art.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] A robotic arm path planning method based on hybrid sampling and dynamic step size includes the following steps:

[0009] Step 1: Obtain the robotic arm's working environment information, specifically at the initial joint angle q. startWith the target joint angle q goal Establish trees T1 and T2, and use these two sets of joint angles as the root nodes of trees T1 and T2. Initialize the distance between trees T1 and T2, initialize the operation tree as T1, and initialize the target tree as T2.

[0010] Initialize the expansion step size. The initial value of the expansion step size is related to the distance between the initial and target configurations of the robotic arm, and the robotic arm configuration itself. The specific expression is as follows:

[0011]

[0012] Where D represents the expansion step size, ρ(q) start ,q goal ) represents the initial configuration q start With target configuration q goal The distance between them This represents the sum of the ranges of motion of all joints in the robotic arm. This represents the upper limit of joint j. This indicates the lower limit of joint j.

[0013] Step 2: Random probability P rand The random probability P is compared between [0,1). rand With threshold probability P threshold The magnitude relationship, when the random probability P rand The probability P is less than the threshold threshold At that time, the sampling strategy uses random sampling, that is, randomly selecting a set of joint angles as sampling points q within the range of motion of the robotic arm joints. sample Conversely, sampling points are generated based on a virtual potential field. Specifically, a gravitational potential field is virtually set up at the target node.

[0014]

[0015] Among them, U att (q) represents the gravitational potential field, and q represents the node newly added in the last expansion phase. g Let be the target node, ω be the gravitational potential coefficient, and ρ(q,q) be the target node. g ) represents the relationship between node q and target node q g The distance between them;

[0016] The attractive force corresponding to a gravitational potential field:

[0017]

[0018] Because mapping obstacle models in 3D space to the joint space of a robotic arm is extremely difficult, invalid sampling points are used to model obstacles. Invalid sampling points are those sampling points that are identified as collision nodes after collision detection. To improve efficiency, only one invalid sampling point is retained for obstacle modeling. When a new invalid sampling point is generated, the old invalid sampling point is replaced. A repulsive potential field is virtually set at the obstacle node.

[0019]

[0020] Among them, U req (q) represents the repulsive potential field, q represents the node newly added in the last expansion phase, q0 represents the obstacle node, q g For the target node, ρ(q,q) g ) represents the relationship between node q and target node q g The distance between them; ρ(q,q0) represents the distance between node q and node q0, ρ(q g ,q0) represents node q g The distance between node q0 and the obstacle node, where ρ0 is the maximum influence distance of the obstacle node and k is the repulsive potential field coefficient;

[0021] The repulsive force corresponding to a repulsive potential field:

[0022]

[0023] in:

[0024]

[0025] Sampling is performed along the direction of the resultant force of the attractive and repulsive forces to obtain the sampling point q. sample .

[0026] Step 3: Calculate the distance between the sampling point and the last node added in the target tree. Determine if this distance is less than the distance between trees T1 and T2. The distance between trees T1 and T2 is represented by the distance between their last added nodes. If it is less than the distance between trees T1 and T2, the sampling is considered valid, and the initial expansion step size is used. Otherwise, the sampling is considered uncertain, and one-third of the initial expansion step size is used as the expansion step size to obtain the new node q. new Perform collision detection; if no collision occurs, then add the new node q. new Add to the operation tree; if a collision occurs, skip to step five.

[0027] Step 4: The target tree is expanded into the operation tree using a greedy expansion strategy. The specific process is as follows:

[0028] a. The target tree is the last node q added in the operation tree. lastFor the sampling point, search the distance node q in the operation tree. last The nearest node q near At node q last With node q near The connection is expanded using one-third of the initial expansion step size to obtain node q1. Collision detection is performed. If a collision occurs, the greedy expansion process is exited; if no collision occurs, node q1 is added to the target tree.

[0029] b. The target tree continues with node q last As a sampling point, the search for distance node q in the operation tree is no longer performed. last The most recent node operation is performed directly on node q. last The connection to node q1 is expanded by one-third of the initial expansion step to obtain node q2. Node q1 is removed, and node q2 is directly connected to the target tree as the new node q1.

[0030] c. Repeat operation b until a collision occurs during collision detection or a connection between trees T1 and T2 is detected, then exit the greedy expansion process.

[0031] If the greedy expansion process exits due to the connection of trees T1 and T2, then proceed directly to step six.

[0032] Step 5: Switch the operation tree and the target tree. If the operation tree is T1, switch the operation tree to T2 and the corresponding target tree to T1. If the operation tree is T2, switch the operation tree to tree T1 and the corresponding target tree to T2, then return to step 2.

[0033] Step 6: Connect node q from tree T1 c1 Starting from the beginning, extract the parent nodes of each node in sequence, and arrange them in reverse order to obtain the first half of the desired path. Then, start from the connection node q of tree T2. c2 Starting from the beginning, extract the parent nodes of each node in sequence and arrange them in order to obtain the second half of the desired path. After merging the two paths, first remove redundant points from the merged path:

[0034] a. For a path, initialize q left The starting path point q start Initialize q right For the target path point q goal ;

[0035] b. Try connecting path point q left and path point q right If no collision occurs, remove path point q. left With path point q right Update q simultaneously for all path points between them. left For q rightUpdate q right For the target path point q goal If a collision occurs, calculate the path point q. left and path point q right Midpoint q in the path mid If a decimal is encountered, the rounding-up scheme is used to round q up. right Updated to path point q mid If q left +1=q right Then update q left For q right Update q right For the target path point q goal ;

[0036] c. Repeat operation b until q is reached. left +1=q goal ;

[0037] Redundant point removal only changes the connection state of path points, not their own state. Therefore, a triangle interpolation strategy is used to further process the path.

[0038] Initialize i = 2, minChange is 1% of the path length, select the i-th path point in the path, denoted as m2, take the path point m1 before m2 and the path point m3 after m2, and calculate path points m4 and m6:

[0039]

[0040] Wherein, ρ(m) i ,m j ) represents path point m i With m j The distance between them;

[0041] Calculate the midpoint between path point m4 and path point m6:

[0042]

[0043] Calculate the distance between path point m5 and path point m2. If the distance is greater than minChange, update path point m2 to m5, i = i + 2, and repeat the operation until i + 1 is greater than the total number of path points.

[0044] Compared with the prior art, the advantages of the present invention are:

[0045] 1. Based on random sampling, this invention introduces a sampling strategy based on a virtual potential field. Through the combined action of gravitational and repulsive potential fields, the sampling becomes more directional and efficient. At the same time, to overcome the problem that the virtual potential field is prone to getting trapped in local minima, the repulsive potential field function is improved to retain random sampling, thus forming a hybrid sampling strategy that shortens the sampling time while preserving the probability completeness.

[0046] 2. This invention uses a dynamic step size strategy, which can determine the appropriate initial expansion step size according to different path planning problems and different robot arm configurations. At the same time, it can adjust the size of the expansion step size according to whether the sampling point shortens the distance between two trees, so that the tree expansion is effective and reasonable, which is beneficial to improving the path quality.

[0047] 3. In the post-processing stage of the present invention, redundant point removal and triangle interpolation strategies are used to remove some meaningless redundant points on the planned path and improve the planned path by replacing them with interpolation points, thereby shortening the length of the planned path and improving the quality of the path. Attached Figure Description

[0048] Figure 1 This is a flowchart of a robotic arm path planning method based on hybrid sampling and dynamic step size in an embodiment of the present invention;

[0049] Figure 2 This is a schematic diagram of virtual potential field sampling in an embodiment of the present invention;

[0050] Figure 3 This is a schematic diagram illustrating the selection of the expansion step size based on whether the sampling point changes the distance between trees T1 and T2 in an embodiment of the present invention;

[0051] Figure 4 This is a schematic diagram illustrating the expansion of the target tree into the operation tree using a greedy expansion strategy in an embodiment of the present invention;

[0052] Figure 5 This is a schematic diagram of redundant point removal in the path post-processing stage of this invention.

[0053] Figure 6 This is a schematic diagram of the triangle interpolation strategy in the path post-processing stage of this invention. Detailed Implementation

[0054] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. These examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0055] like Figure 1 As shown, this embodiment discloses a robotic arm path planning method based on hybrid sampling and dynamic step size, including the following steps:

[0056] Step 1: Obtain a depth map using the robot's overhead depth camera. Convert all pixels in the depth map into a point cloud, resulting in a point cloud image. Perform filtering operations on the point cloud image: First, use pass-through filtering. Due to the limitations of the robotic arm's working range, set the X and Y axes to (-1.0, 1.0) and the Z axis to (0.5, 1.5) to filter out point clouds outside these ranges. Second, use voxel downsampling with a 3D voxel resolution of 0.01m, discretizing the entire space into several voxels with sides of 0.01m. For each voxel, retain only one point cloud data point. Finally, perform outlier filtering with a neighborhood of 20 and a standard deviation factor of 1.0. Convert the filtered point cloud map into an octree map to obtain the robotic arm's working environment information.

[0057] Determine the initial state and the target state, using the joint angles (q1, q2, ... q) in joint space. n The expression ) indicates that n=7 means the robotic arm is a 7-DOF robotic arm, with initial joint angle q start and target joint angle q goal Collision detection is performed. If a collision is found, the initial target state needs to be reset; this is done at the initial joint angle q. start With the target joint angle q goal Establish trees T1 and T2, and use these two sets of joint angles as the root nodes of their respective trees. Calculate the initial joint angle q. start With the target joint angle q goal The distance between trees T1 and T2 is used as the initial distance between them. The operation tree is initialized to T1, and the target tree is initialized to T2.

[0058] Initialize the expansion step size. The initial value of the expansion step size is related to the distance between the initial and target configurations of the robotic arm, and the robotic arm configuration itself. The specific expression is as follows:

[0059]

[0060] Where D represents the expansion step size, ρ(q) start ,q goal ) represents the initial configuration q start With target configuration q goal The distance between them This represents the sum of the ranges of motion of all joints in the robotic arm. This represents the upper limit of joint j. This indicates the lower limit of joint j.

[0061] The range of motion of each joint of the humanoid robotic arm is shown in Table 1:

[0062] Table 1 Joint Range of Humanoid Robotic Arm

[0063]

[0064] Step 2: Select a random probability P that is uniformly distributed between [0,1). rand Compare random probabilities P rand With threshold probability P threshold The magnitude relationship, the threshold probability P threshold =0.5, when the random probability P rand The probability P is less than the threshold threshold At that time, the sampling strategy uses random sampling, that is, randomly selecting a set of joint angles as sampling points q within the range of motion of the robotic arm joints. sample Conversely, sampling points are generated based on the virtual potential field. Specifically, sampling points are generated at the target node q. goal Virtually set up a gravitational potential field:

[0065]

[0066] Among them, U att (q) represents the gravitational potential field, and q represents the node newly added in the last expansion phase. g Let be the target node, ω be the gravitational potential coefficient, and ρ(q,q) be the target node. g ) represents the relationship between node q and target node q g The distance between them;

[0067] The attractive force corresponding to a gravitational potential field:

[0068]

[0069] Because it is extremely difficult to map obstacle models in 3D space to the joint space of a robotic arm, invalid sampling points are used to model obstacles. Invalid sampling points are sampling points that are identified as collision nodes after collision detection. To improve efficiency, only one invalid sampling point is retained for obstacle modeling. When a new invalid sampling point is generated, the old invalid sampling point is replaced.

[0070] Since virtual potential field sampling is prone to getting trapped in local minima, the distance between the target node and the obstacle node, and the distance between the newly added node and the target node are introduced into the repulsive potential field function, as follows:

[0071]

[0072] Among them, U req (q) represents the repulsive potential field, q represents the node newly added in the last expansion phase, q0 represents the obstacle node, q g For the target node, ρ(q,q) g ) represents the relationship between node q and target node q g The distance between them; ρ(q,q0) represents the distance between node q and node q0, ρ(q g,q0) represents node q g The distance between node q0 and the obstacle node, where ρ0 is the maximum influence distance of the obstacle node and k is the repulsive potential field coefficient;

[0073] The repulsive force corresponding to a repulsive potential field:

[0074]

[0075] in:

[0076]

[0077] like Figure 2 As shown, the target node q goal Generate a gravitational potential field, target node q goal For node q c Generates gravity F att The obstacle obs generates a repulsive potential field, which affects node q. c Generates repulsive force F req repulsive force F req It is F req1 With F req2 Calculate the resultant force, gravitational force F. att and repulsive force F req The resultant force F is sampled to obtain the sampling point q. sample .

[0078] Step 3: Calculate the distance between the sampling point and the last added node in the target tree, and determine whether this distance is less than the distance between trees T1 and T2 at this moment. The distance between trees T1 and T2 is represented by the distance between their respective last added nodes. When the distance between the sampling point and the last added node in the target tree is less than the distance between trees T1 and T2, the current sampling is considered valid, and the initial expansion step size is adopted. Otherwise, the sampling is considered to have uncertainty, and one-third of the initial expansion step size is used as the step size for expansion.

[0079] like Figure 3 As shown, at this moment, the operation tree is tree T1, the target tree is tree T2, and node s1 is a node in tree T1. For sampling point s2, adding it to tree T1 can shorten the distance between trees T1 and T2, so the initial expansion step size is used to expand to the target tree T2. For sampling point s3, adding it to tree T1 fails to shorten the distance between trees T1 and T2, and even increases the distance between trees T1 and T2. One-third of the initial expansion step size is used as the step size for expansion. Because the expansion step size is small, it fails to expand to node s3, and instead obtains node s4.

[0080] The new node q is obtained by expanding with the corresponding expansion step size. new Perform collision detection; if no collision occurs, then add the new node q. newAdd to the operation tree; if a collision occurs, skip to step five.

[0081] Step 4: The target tree is expanded into the operation tree using a greedy expansion strategy. The specific process is as follows:

[0082] a. The target tree is the last node q added in the operation tree. last For the sampling point, search the distance node q in the operation tree. last The nearest node q near At node q last With node q near The connection is expanded using one-third of the initial expansion step size to obtain node q1. Collision detection is performed. If a collision occurs, the greedy expansion process is exited; if no collision occurs, node q1 is added to the target tree.

[0083] b. The target tree continues with node q last As a sampling point, the search for distance node q in the operation tree is no longer performed. last The most recent node operation is performed directly on node q. last The connection to node q1 is expanded by one-third of the initial expansion step to obtain node q2. Node q1 is removed, and node q2 is directly connected to the target tree as the new node q1.

[0084] c. Repeat operation b until a collision occurs during collision detection or a connection between trees T1 and T2 is detected, then exit the greedy expansion process.

[0085] If the greedy expansion process exits due to the connection of trees T1 and T2, then proceed directly to step six.

[0086] like Figure 4 As shown, the target tree T2 directly uses node q1 in operation tree T1 as the sampling point and searches for the node in target tree T2 closest to node q1, obtaining node q2. Then, it expands towards node q1 with one-third of the initial expansion step size, obtaining node q3. At this point, the search for the node closest to node q1 in target tree T2 is no longer performed, because node q2 was the closest node to node q1 before node q3 was added to target tree T2, and node q3 is obtained by expanding node q2 towards node q1 with one-third of the initial expansion step size. Therefore, node q3 is now the closest node to node q1 in target tree T2, and no further search is needed. Expanding from node q3 towards node q1 yields node q4, and expanding from node q4 towards node q1 yields node q5. When expanding from node q5 towards node q6, a collision occurs, the greedy expansion process is stopped, and the intermediate nodes q3 and q4 are deleted, leaving only node q5, meaning nodes q2 and q5 are directly connected.

[0087] Step 5: Switch the operation tree and the target tree. If the operation tree is T1, switch the operation tree to T2 and the corresponding target tree to T1. If the operation tree is T2, switch the operation tree to tree T1 and the corresponding target tree to T2, then return to step 2.

[0088] Step 6: Connect node q from tree T1 c1 Starting from the beginning, extract the parent nodes of each node in sequence, and arrange them in reverse order to obtain the first half of the desired path. Then, start from the connection node q of tree T2. c2 Starting from the beginning, extract the parent nodes of each node in sequence and arrange them in order to obtain the second half of the desired path. After merging the two paths, first remove redundant points from the merged path:

[0089] a. For a path, initialize q left The starting path point q start Initialize q right For the target path point q goal ;

[0090] b. Try connecting path point q left and path point q right If no collision occurs, remove path point q. left With path point q right Update q simultaneously for all path points between them. left For q right Update q right For the target path point q goal If a collision occurs, calculate the path point q. left and path point q right Midpoint q in the path mid If a decimal is encountered, the rounding-up scheme is used to round q up. right Updated to path point q mid If q left +1=q right Then update q left For q right Update q right For the target path point q goal ;

[0091] c. Repeat operation b until q is reached. left +1=q goal ;

[0092] like Figure 5As shown, a path consisting of five path points n1, n2, n3, n4, and n5 is first checked to see if n1 and n5 can be directly connected. Since a collision occurs, the midpoint n3 is taken. If a decimal is encountered, the rounding up method is used to check if n1 and n3 can be directly connected. No collision occurs, and the redundant point n2 is removed. Then, on the path n3, n4, and n5, it is checked if n3 and n5 can be directly connected. No collision occurs, and the redundant point n4 is removed. Thus, the redundant point removal operation is completed, and the final path is n1, n3, and n5. Compared to the initial path, redundant points n2 and n4 have been removed.

[0093] Redundant point removal only changes the connection state of path points, not their own state. Therefore, a triangle interpolation strategy is used to further process the path.

[0094] Initialize i = 2, minChange is 1% of the path length, select the i-th path point in the path, denoted as m2, take the path point m1 before m2 and the path point m3 after m2, and calculate path points m4 and m6:

[0095]

[0096] Wherein, ρ(m) i ,m j ) represents path point m i With m j The distance between them;

[0097] Calculate the midpoint between path point m4 and path point m6:

[0098]

[0099] Calculate the distance between path point m5 and path point m2. If the distance is greater than minChange, update path point m2 to m5, i = i + 2, and repeat the operation until i + 1 is greater than the total number of path points.

[0100] like Figure 5 As shown, the initial path points are m1, m2 and m3. We interpolate between m1 and m2 to get m4, between m2 and m3 to get m6, and between m4 and m6 to get m5. We replace m2 with m5 to get the final path m1, m5 and m3.

[0101] To further verify the robotic arm path planning method based on hybrid sampling and dynamic step size described in this invention, the initial joint angles of the robotic arm were set to (0, -0.9, 0, -0.9, 0, 0, 0) in radians, the target joint angles were set to (-0.6, -0.03, 0.08, -0.86, -0.06, 0.07, 0) in radians, the obstacle size was set to (0.05, 0.5, 0.5) in meters, the position (x, y, z) was set to (0.4, -0.3, 0.9) in meters, and the posture (r, p, y) was set to (0.0, 0.0, 1.6) in radians. The comparison results with RRT, BiTRRT, PRM, and LazyPRM in this scenario are shown in Table 2.

[0102] Table 2 Test Results

[0103]

[0104] As can be seen, the robotic arm path planning method based on hybrid sampling and dynamic step size described in this invention is superior to other algorithms in both time and path length. Furthermore, after the path post-processing stage, the path length is further reduced to 5.39997, and the path quality is further improved.

[0105] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope defined by the claims of the present invention.

Claims

1. A robotic arm path planning method based on hybrid sampling and dynamic step size, characterized in that, Includes the following steps: Step 1: Obtain the robotic arm's working environment information, specifically at the initial joint angle q. start With the target joint angle q goal Establish trees T1 and T2, and use these two sets of joint angles as the root nodes of trees T1 and T2 respectively. Initialize the expansion step size and the distance between trees T1 and T2. Initialize the operation tree as T1 and the target tree as T2. Step 2: Based on the random probability P rand With threshold probability P threshold The magnitude relationship determines the sampling strategy, and the random probability P rand The sampling points are uniformly distributed between [0,1). When the random probability is less than the threshold probability, the sampling strategy uses random sampling, that is, randomly selecting a set of joint angles as sampling points q within the range of motion of the robotic arm joints. sample Conversely, sampling points q are generated based on the virtual potential field. sample The repulsive potential field function incorporates the distance between the target node and the obstacle node, as well as the distance between the newly added node and the target node. The specific expressions are as follows: Among them, U req (q) represents the repulsive potential field, q represents the node newly added in the last expansion phase, q0 represents the obstacle node, q g For the target node, ρ(q,q) g ) represents the relationship between node q and target node q g The distance between nodes q and q0, ρ(q,q0) represents the distance between node q and node q0. g ,q0) represents node q g The distance between the obstacle node and node q0, where ρ0 is the maximum influence distance of the obstacle node, and k is the repulsive potential field coefficient. Because it is extremely difficult to map obstacle models in 3D space to the joint space of a robotic arm, invalid sampling points are used to model obstacles. Invalid sampling points are sampling points that are identified as collision nodes after collision detection. To improve efficiency, only one invalid sampling point is retained for obstacle modeling. When a new invalid sampling point is generated, the old invalid sampling point is replaced. Step 3: Based on the current sampling point q sample Whether to change the distance between trees T1 and T2, and then expand the tree to obtain a new node q by adjusting the expansion step size accordingly. new Perform collision detection; if no collision occurs, then add the new node q. new Add to the operation tree; if a collision occurs, skip to step five. Step 4: The target tree is expanded to the operation tree using a greedy expansion strategy. It is determined whether tree T1 and tree T2 are connected to each other. If they are connected, proceed to step 6. Step 5: Switch the operation tree and the target tree. If the operation tree is T1, switch the operation tree to tree T2 and the target tree to tree T1. If the operation tree is T2, switch the operation tree to tree T1 and the target tree to tree T2, then return to step 2. Step 6: Connect node q from tree T1 c1 Starting from the beginning, extract the parent nodes of each node in sequence, and arrange them in reverse order to obtain the first half of the desired path. Then, start from the connection node q of tree T2. c2 Starting from the beginning, extract the parent nodes of each node in sequence and arrange them in order to obtain the second half of the desired path. After merging the two paths, perform path post-processing using redundant point removal and triangle interpolation strategies.

2. The robotic arm path planning method based on hybrid sampling and dynamic step size according to claim 1, characterized in that: The method for initializing the expansion step size in step one is as follows: the initial value of the expansion step size is related to the distance between the initial configuration and the target configuration of the robotic arm, and the configuration of the robotic arm itself. The specific expression is: Where D represents the expansion step size, ρ(q) start ,q goal ) represents the initial configuration q start With target configuration q goal The distance between them This represents the sum of the ranges of motion of all joints in the robotic arm. This represents the upper limit of joint j. This indicates the lower limit of joint j.

3. The robotic arm path planning method based on hybrid sampling and dynamic step size according to claim 1, characterized in that: In step three, based on the current sampling point q sample The process of choosing the expansion step size to determine whether to change the distance between trees T1 and T2 is as follows: Calculate the distance between the sampling point and the last node added in the target tree, and determine whether this distance is less than the distance between trees T1 and T2. The distance between trees T1 and T2 is represented by the distance between their last added nodes. If it is less than the distance between trees T1 and T2, the sampling is considered valid and the initial expansion step size is adopted. Otherwise, the sampling is considered to have uncertainty, and one-third of the initial expansion step size is adopted as the step size for expansion.

4. The robotic arm path planning method based on hybrid sampling and dynamic step size according to claim 1, characterized in that: The greedy expansion strategy process in step four is as follows: a. The target tree is the last node q added in the operation tree. last For the sampling point, search the distance node q in the operation tree. last The nearest node q near At node q last With node q near The connection is expanded using one-third of the initial expansion step size to obtain node q1. Collision detection is performed. If a collision occurs, the greedy expansion process is exited; if no collision occurs, node q1 is added to the target tree. b. The target tree continues with node q last As a sampling point, the search for distance node q in the operation tree is no longer performed. last The most recent node operation is performed directly on node q. last The connection to node q1 is expanded by one-third of the initial expansion step to obtain node q2. Node q1 is removed, and node q2 is directly connected to the target tree as the new node q1. c. Repeat operation b until a collision occurs during collision detection or tree T1 and T2 are detected to be connected, then exit the greedy expansion process.

5. The robotic arm path planning method based on hybrid sampling and dynamic step size according to claim 1, characterized in that: The method for removing redundant points in step six is ​​as follows: a. For a path, initialize q left The starting path point q start Initialize q right For the target path point q goal ; b. Try connecting path point q left and path point q right If no collision occurs, remove path point q. left With path point q right Update q simultaneously for all path points between them. left For q right Update q right For the target path point q goal If a collision occurs, calculate the path point q. left and path point q right Midpoint q in the path mid If a decimal is encountered, the rounding-up scheme is used to round q up. right Updated to path point q mid If q left +1=q right Then update q left For q right Update q right For the target path point q goal ; c. Repeat operation b until q is reached. left +1=q goal .

6. The robotic arm path planning method based on hybrid sampling and dynamic step size according to claim 1, characterized in that: The process of the triangle interpolation strategy in step six is ​​as follows: Initialize i = 2, minChange is one percent of the path length, select the i-th path point in the path, denoted as m2, take the path point m1 before path point m2 and the path point m3 after path point m2, and calculate path points m4 and m6: Wherein, ρ(m) i ,m j ) represents path point m i With m j The distance between them; Calculate the midpoint between path point m4 and path point m6: Calculate the distance between path point m5 and path point m2. If the distance is greater than minChange, update path point m2 to m5, i = i + 2, and repeat the operation until i + 1 is greater than the total number of path points.

Images