Mechanical arm path planning method based on mixed sampling and dynamic step length

Abstract

The invention provides a mechanical arm path planning method based on mixed sampling and dynamic step length, and relates to the technical field of mechanical arm motion planning. The method comprises the steps that working environment information of the mechanical arm is obtained, a tree structure is established according to an initial joint angle and a target joint angle, and the extension step length is initialized; selecting a sampling strategy according to the random probability; adjusting the extended step length according to whether the sampling point shortens the distance between the two trees, carrying out collision detection and adding a new node; performing greedy expansion on the target tree; updating the operation tree and the target tree; and if the two trees are connected, returning to plan the path, entering a path post-processing stage, and performing path optimization by using redundant point removal and triangular interpolation. According to the mechanical arm path planning method based on mixed sampling and dynamic step length provided by the invention, a high-quality motion path can be quickly planned for the mechanical arm in a high-dimensional complex space.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm motion planning, and in particular to a robot arm path planning method based on hybrid sampling and dynamic step length. Background Art

[0002] Robot path planning is a key issue in the field of robotic arms. That is, given the starting configuration and target configuration of the robotic arm, a collision-free path starting from the starting configuration and ending at the target configuration is output. Collision-free means that all configurations on the path are distributed in the obstacle-free configuration space of the robotic arm.

[0003] Robotic arms often interact with the real physical world, and their operating environment is relatively complex. They usually have multiple degrees of freedom, and the planning space dimension is relatively high. There are certain requirements for response time. Therefore, how to quickly plan a high-quality motion path for the robotic arm in a high-dimensional complex space is a huge challenge.

[0004] Graph search planning methods represented by the A* algorithm convert path planning problems into graph search problems by discretizing the workspace. Although they are widely used in mobile robot path planning, they are limited by algorithm complexity and computational cost and are only applicable to low-dimensional path planning problems. The artificial potential field method introduces the concept of field. The target point generates a gravitational field, and the obstacle generates a repulsive field. The robot moves toward the target point under the combined force of the two potential fields. However, the artificial potential field method is prone to local minima problems, and it is difficult to construct potential field functions for high-dimensional complex planning problems.

[0005] The rapidly expanding random tree method grows the random tree by incrementally sampling in the planning space until the random tree reaches the target point and returns a feasible planning path. This method only performs collision detection on the sampling points in the planning space and does not require the establishment of an explicit expression of obstacles in the environment. It has both high efficiency and probabilistic completeness and has been widely studied and applied. However, it is worth noting that the rapidly expanding random tree method uses a uniform random sampling strategy, which is inefficient and the quality of the planned path is uncontrollable, with high randomness. Summary of the invention

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

[0007] To achieve the above object, the present invention adopts the following technical solutions:

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

[0009] Step 1: Obtain the working environment information of the robot arm, respectively at the initial joint angle q startand 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 to T1, and initialize the target tree to T2;

[0010] Initialize the expansion step length. The initial value of the expansion step length is related to the distance between the starting configuration and the target configuration of the robot arm and the configuration of the robot arm. The specific expression is:

[0011]

[0012] Where D represents the expansion step size, ρ(q start ,q goal ) represents the starting configuration q start With the target configuration q goal The distance between It represents the sum of the movable ranges of each joint of the robot arm. represents the joint upper limit of joint j, Represents the joint lower limit of joint j.

[0013] Step 2: Random Probability P rand Uniformly distributed between [0,1), compare the random probability P rand With the threshold probability P threshold When the random probability P rand Less than the threshold probability P threshold When , the sampling strategy uses random sampling, that is, a set of joint angles are randomly selected as sampling points q within the range of motion of the robot joint. sample Otherwise, the sampling points are generated based on the virtual potential field. The specific method is: a virtual gravitational potential field is set at the target node:

[0014]

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

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

[0017]

[0018] Since it is extremely difficult to map the obstacle model in the three-dimensional space to the joint space of the robot arm, invalid sampling points are used to model the obstacles. Invalid sampling points refer to 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, and 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 newly added node in the last expansion phase, q0 represents the obstacle node, and q g is the target node, ρ(q,q g ) represents the node q and the target node q g The distance between nodes q and q0; ρ(q,q0) represents the distance between node q and node q0, ρ(q g ,q0) represents node q g The distance from the node q0, ρ0 is the maximum influence distance of the obstacle node, and k is the repulsive potential field coefficient;

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

[0022]

[0023] in:

[0024]

[0025] Sampling is performed in the direction of the combined force of attraction and repulsion 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, and determine whether this distance is less than the distance between trees T1T2. The distance between trees T1T2 is represented by the distance between the last nodes added. If it is less than the distance between trees T1T2, the sampling is considered valid and the initial expansion step is used. Otherwise, the sampling is considered uncertain and one-third of the initial expansion step is used as the step to expand. The new node q is obtained by expanding with the corresponding expansion step. new , perform collision detection, if no collision occurs, the new node q new Add to the operation tree; if a collision occurs, jump to step 5.

[0027] Step 4: The target tree is expanded to 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 sampling points, search for distance node q in the operation tree last The nearest node q near , at node q last With node q near The connection of is expanded using one third of the initial expansion step length as the step length to obtain node q1, and 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, no longer search for the distance node q in the operation tree last The operation of the nearest node is directly on node q last The line connected 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 the collision detection process or it is detected that the trees T1T2 are connected, and then exit the greedy expansion process.

[0031] If the greedy expansion process exits because the trees T1T2 are connected, then directly execute step 6.

[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 T1 and the corresponding target tree to T2, and 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 turn, and arrange them in reverse order to obtain the first half of the expected path, and then from the connection node q of tree T2 c2 Start by extracting the parent nodes of each node in turn and arranging them in order to obtain the second half of the expected path. After merging the above two paths, first remove redundant points from the merged path:

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

[0035] b. Try to connect the path point q left and path point q right , if no collision occurs, remove the path point q left With the waypoint q right All path points between them are updated simultaneously left Q right, update q right is the target path point q goal , if a collision occurs, calculate the path point q left and path point q right At the midpoint q in the path mid If a decimal is encountered, round up to the integer. right Update to path point q mid , if q left +1=q right , then update q left for q right , update q right is the target path point q goal ;

[0036] c. Loop through operation b until q left +1=q goal ;

[0037] The redundant point removal operation only changes the connection status of the path points, but cannot change the status of the path points themselves. The 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, record it as m2, take the previous path point m1 and the next path point m3 of path point m2, and calculate path points m4 and m6:

[0039]

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

[0041] Calculate the midpoint between waypoint m4 and waypoint 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 present invention is beneficial in that:

[0045] 1. On the basis of random sampling, the present invention introduces a sampling strategy based on a virtual potential field. Through the joint action of the gravitational potential field and the repulsive potential field, the sampling is more directional and more efficient. At the same time, in order to overcome the problem that the virtual potential field is prone to fall into a local minimum, the repulsive potential field function is improved, random sampling is retained, and a hybrid sampling strategy is formed, which not only shortens the sampling time but also retains the probability completeness;

[0046] 2. The present 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, and can adjust the size of the expansion step size according to whether the sampling point shortens the distance between the two trees, so that the tree expansion is effective and reasonable, which is beneficial to the improvement of path quality;

[0047] 3. The present invention uses redundant point removal and triangle interpolation strategies in the path post-processing stage, removes some meaningless redundant points on the planned path and improves the planned path by replacing interpolation points, shortens the length of the planned path, and improves the quality of the path. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 is a flow chart of a robot arm path planning method based on hybrid sampling and dynamic step length in an embodiment of the present invention;

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

[0050] Figure 3 A schematic diagram of selecting an expansion step size according to whether a sampling point changes the distance between trees T1T2 in an embodiment of the present invention;

[0051] Figure 4 A schematic diagram of a target tree in an embodiment of the present invention being expanded to an operation tree using a greedy expansion strategy;

[0052] Figure 5 A schematic diagram of removing redundant points in the path post-processing stage in an embodiment of the present invention;

[0053] Figure 6 Schematic diagram of a triangle interpolation strategy in the path post-processing stage in an embodiment of the present invention. DETAILED DESCRIPTION

[0054] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and examples. The following examples are used to explain the present invention, but are not used to limit the scope of the present invention.

[0055] like Figure 1 As shown, in this embodiment, a robot arm path planning method based on hybrid sampling and dynamic step length is disclosed, including the following steps:

[0056] Step 1: Use the depth camera on the robot's head to obtain a depth map, convert all pixels in the depth map into point clouds, and obtain a point cloud map. Perform filtering operations on the point cloud map: First, perform straight-through filtering. Due to the limitation of the working range of the robotic arm, set the range of the X-axis and Y-axis to (-1.0, 1.0), and the range of the Z-axis to (0.5, 1.5), and filter out the point clouds that exceed the range; secondly, use voxel downsampling, the three-dimensional voxel resolution is 0.01m, and the entire space is discretized into several voxels with a side length of 0.01m. For the point cloud in each voxel, only one point cloud data is retained; finally, perform outlier filtering, the number of neighborhood points is 20, and the standard deviation multiple is 1.0. Convert the filtered point cloud map into an octree map to obtain the working environment information of the robotic arm.

[0057] Determine the initial state and target state, with the joint angles (q1, q2, ...q n ), n = 7 means that the robot arm is a 7-DOF robot arm, and the initial joint angle q start and the target joint angle q goal Perform collision detection. If there is a collision, reset the target state of the initial state; respectively, at the initial joint angle q start and 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 to calculate the initial joint angle q start and the target joint angle q goal The distance between trees T1 and T2 is used as the initial distance between the operation tree and the target tree is initialized to T1 and T2.

[0058] Initialize the expansion step length. The initial value of the expansion step length is related to the distance between the starting configuration and the target configuration of the robot arm and the configuration of the robot arm. The specific expression is:

[0059]

[0060] Where D represents the expansion step size, ρ(q start ,q goal ) represents the starting configuration q start With the target configuration q goal The distance between It represents the sum of the movable ranges of each joint of the robot arm. represents the joint upper limit of joint j, Represents the joint lower limit of joint j.

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

[0062] Table 1 Joint range of humanoid robot arm

[0063]

[0064] Step 2: Take a random probability P uniformly distributed between [0,1) rand , compare the random probability P rand With the threshold probability P threshold The size relationship between the threshold probability P threshold =0.5, when the random probability P rand Less than the threshold probability P threshold When , the sampling strategy uses random sampling, that is, a set of joint angles are randomly selected as sampling points q within the range of motion of the robot joint. sample Otherwise, the sampling points are generated based on the virtual potential field. The specific method is: 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, q represents the newly added node in the last expansion phase, and q g is the target node, ω is the gravitational potential field coefficient, ρ(q,q g ) represents the node q and the target node q g The distance between

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

[0068]

[0069] Since it is extremely difficult to map the obstacle model in the three-dimensional space to the joint space of the robot arm, invalid sampling points are used to model the obstacles. Invalid sampling points refer to 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] Based on the problem that virtual potential field sampling is prone to fall into 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 newly added node in the last expansion phase, q0 represents the obstacle node, and q g is the target node, ρ(q,q g ) represents the node q and the target node q g The distance between nodes q and q0; ρ(q,q0) represents the distance between node q and node q0, ρ(q g,q0) represents node q g The distance from the node q0, ρ0 is the maximum influence distance of the obstacle node, and k is the repulsive potential field coefficient;

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

[0074]

[0075] in:

[0076]

[0077] like Figure 2 As shown, the target node q goal Generate gravitational potential field, target node q goal For node q c Generates gravitational force F att , the obstacle obs generates a repulsive potential field on the node q c Generate repulsive force F req , repulsive force F req Yes F req1 With F req2 The net force of the gravitational force F att and repulsive force F req The resultant force F is sampled and the sampling point q is obtained sample .

[0078] Step 3: Calculate the distance between the sampling point and the last node added to the target tree, and determine whether this distance is less than the distance between the trees T1T2 at this moment. The distance between the trees T1T2 is expressed as the distance between the last nodes added to each tree. When the distance between the sampling point and the last node added to the target tree is less than the distance between the trees T1T2, the current sampling is considered valid and the initial expansion step is used. Otherwise, the sampling is considered uncertain and one-third of the initial expansion step is used as the step 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 T1T2, so the initial expansion step is used to expand to target tree T2. For sampling point s3, adding it to tree T1 fails to shorten the distance between trees T1T2, and even increases the distance between trees T1T2. One third of the initial expansion step is used as the step length for expansion. Due to the small expansion step length, it fails to expand to node s3, but obtains node s4 instead.

[0080] Expand with the corresponding expansion step to get the new node q new , perform collision detection, if no collision occurs, the new node q newAdd to the operation tree; if a collision occurs, jump to step 5.

[0081] Step 4: The target tree is expanded to 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 sampling points, search for distance node q in the operation tree last The nearest node q near , at node q last With node q near The connection of is expanded using one third of the initial expansion step length as the step length to obtain node q1, and 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, no longer search for the distance node q in the operation tree last The operation of the nearest node is directly on node q last The line connected 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 the collision detection process or it is detected that the trees T1T2 are connected, and then exit the greedy expansion process.

[0085] If the greedy expansion process exits because the trees T1T2 are connected, then directly execute step 6.

[0086] like Figure 4 As shown, the target tree T2 directly uses the node q1 in the operation tree T1 as the sampling point, searches for the node closest to the node q1 in the target tree T2, obtains the node q2, and expands to the node q1 with one-third of the initial expansion step length to obtain the node q3; at this time, the node closest to the node q1 in the target tree T2 is no longer searched, because the node q2 is the node closest to the node q1 when the node q3 is not added to the target tree T2, and the node q3 is the node q2 is expanded to the node q1 with one-third of the initial expansion step length. Therefore, the node q3 is the node closest to the node q1 in the target tree T2, so there is no need to search. Based on the node q3, the node q4 is expanded to the node q1, and the node q5 is expanded to the node q1 based on the node q4. When expanding to the node q6 based on the node q5, a collision occurs, and the greedy expansion process is terminated, and the intermediate node q3 and the node q4 are deleted, and only the node q5 is retained, that is, the node q2 is directly connected to the node q5.

[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 T1 and the corresponding target tree to T2, and 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 turn, and arrange them in reverse order to obtain the first half of the expected path, and then from the connection node q of tree T2 c2 Start by extracting the parent nodes of each node in turn and arranging them in order to obtain the second half of the expected path. After merging the above two paths, first remove redundant points from the merged path:

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

[0090] b. Try to connect the path point q left and path point q right , if no collision occurs, remove the path point q left With the waypoint q right All path points between them are updated simultaneously left for q right , update q right is the target path point q goal , if a collision occurs, calculate the path point q left and path point q right At the midpoint q in the path mid If a decimal is encountered, round up to the integer. right Update to path point q mid , if q left +1=q right , then update q left for q right , update q right is the target path point q goal ;

[0091] c. Loop through operation b until q left +1=q goal ;

[0092] like Figure 5As shown in the figure, a path consisting of five path points n1, n2, n3, n4, and n5 is first judged whether n1 and n5 can be directly connected. Due to a collision, the midpoint n3 is taken. If a decimal is encountered, the upward rounding scheme is adopted to judge whether 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 judged whether n3 and n5 can be directly connected. No collision occurs, and the redundant point n4 is removed. At this point, the redundant point removal operation is completed, and the final path is n1, n3, and n5. Compared with the initial path, the redundant points n2 and n4 are removed.

[0093] The redundant point removal operation only changes the connection status of the path points, but cannot change the status of the path points themselves. The 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, record it as m2, take the previous path point m1 and the next path point m3 of path point m2, and calculate path points m4 and m6:

[0095]

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

[0097] Calculate the midpoint between waypoint m4 and waypoint 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. m4 is obtained by interpolating between m1 and m2, m6 is obtained by interpolating between m2 and m3, and m5 is obtained by interpolating between m4 and m6. m5 is used to replace m2 to obtain the final paths m1, m5 and m3.

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

[0102] Table 2 Test results

[0103]

[0104] It can be seen that the robot arm path planning method based on hybrid sampling and dynamic step size described in the present invention is superior to other algorithms in terms of time and path length, and 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, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements 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 robot arm path planning method based on hybrid sampling and dynamic step length, characterized in that: The following steps are involved: Step 1: Obtain the working environment information of the robot arm, respectively at the initial joint angle q start and 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 to T1, and initialize the target tree to T2; Step 2: According to the random probability P rand With the threshold probability P threshold The size relationship determines the sampling strategy, and the sampling operation is performed to obtain the corresponding sampling point q sample ; Step 3: According to the current sampling point q sample Whether to change the distance between trees T1 and T2. Select the expansion step size and expand the new node q with the corresponding expansion step size. new , perform collision detection, if no collision occurs, the new node q new Add to the operation tree; if a collision occurs, jump to step 5; Step 4: The target tree is expanded to the operation tree using a greedy expansion strategy to determine whether tree T1 and tree T2 are connected to each other. If so, execute 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 corresponding target tree to tree T1; if the operation tree is T2, switch the operation tree to tree T1, and the corresponding target tree to tree T2, and 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 turn, and arrange them in reverse order to obtain the first half of the expected path, and then from the connection node q of tree T2 c2 Starting from this, the parent nodes of each node are extracted in turn, and they are arranged in order to obtain the second half of the expected path. After merging the above two paths, the redundant point removal and triangle interpolation strategies are used for path post-processing.

2. The robot arm path planning method based on hybrid sampling and dynamic step length according to claim 1, characterized in that: The method for initializing the extension step length in step 1 is: the initial value of the extension step length is related to the distance between the starting configuration and the target configuration of the robot arm and the configuration of the robot arm. The specific expression is: Where D represents the expansion step size, ρ(q start ,q goal ) represents the starting configuration q start With the target configuration q goal The distance between It represents the sum of the movable ranges of each joint of the robot arm. represents the joint upper limit of joint j, Represents the joint lower limit of joint j.

3. The robot arm path planning method based on hybrid sampling and dynamic step length according to claim 1, characterized in that: In step 2, according to the random probability P rand With the threshold probability P threshold The process of determining the sampling strategy is: random probability P rand Uniformly distributed between [0,1). When the random probability is less than the threshold probability, the sampling strategy uses random sampling, that is, a set of joint angles are randomly selected as sampling points within the range of motion of the robot joint. On the contrary, the sampling points are generated based on the virtual potential field, and 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. The specific expression is as follows: Among them, U req (q) represents the repulsive potential field, q represents the newly added node in the last expansion phase, q0 represents the obstacle node, and q g is the target node, ρ(q,q g ) represents the node q and the target node q g The distance between nodes q and q0; ρ(q,q0) represents the distance between node q and node q0, ρ(q g ,q0) represents node q g The distance from the node q0, ρ0 is the maximum influence distance of the obstacle node, and k is the repulsive potential field coefficient; Since it is extremely difficult to map the obstacle model in three-dimensional space to the joint space of the robotic arm, invalid sampling points are used to model the obstacles. Invalid sampling points refer to sampling points that are identified as collision nodes after collision detection. In order 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.

4. The robot arm path planning method based on hybrid sampling and dynamic step length according to claim 1, characterized in that: In step 3, according to the current sampling point q sample Whether to change the distance between trees T1 and T2 The process of selecting the expansion step size is: 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. The distance between trees T1 and T2 is expressed as the distance between their last added nodes. When 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 step size for expansion.

5. The robot arm path planning method based on hybrid sampling and dynamic step length according to claim 1, characterized in that: The greedy expansion strategy process in step 4 is: a. The target tree is the last node q added in the operation tree last For sampling points, search for distance node q in the operation tree last The nearest node q near , at node q last With node q near The connection of is expanded using one third of the initial expansion step length as the step length to obtain node q1, and 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, no longer search for the distance node q in the operation tree last The operation of the nearest node is directly on node q last The line connected 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 the collision detection process or it is detected that trees T1 and T2 are connected, and then exit the greedy expansion process.

6. The robot arm path planning method based on hybrid sampling and dynamic step length according to claim 1, characterized in that: The method for removing redundant points in step 6 is: a. For a path, initialize q left is the starting path point q start , initialize q right is the target path point q goal ; b. Try to connect the path point q left and path point q right , if no collision occurs, remove the path point q left With the waypoint q right All path points between them are updated simultaneously left for q right , update q right is the target path point q goal , if a collision occurs, calculate the path point q left and path point q right At the midpoint q in the path mid If a decimal is encountered, round up to the integer. right Update to path point q mid , if q left +1=q right , then update q left for q right , update q right is the target path point q goal ; c. Loop through operation b until q left +1=q goal .

7. The robot arm path planning method based on hybrid sampling and dynamic step length according to claim 1, characterized in that: The process of the triangle interpolation strategy in step 6 is: initialize i=2, minChange is 1% of the path length, select the i-th path point in the path, record it as m2, take the previous path point m1 and the next path point m3 of the path point m2, and calculate the path points m4 and m6: Among them, ρ(m i ,m j ) represents the path point m i With m j The distance between Calculate the midpoint between waypoint m4 and waypoint 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