Mechanical arm path planning method of adaptive sampling domain under dense obstacles

Abstract

The invention relates to a mechanical arm path planning method of a self-adaptive sampling domain under dense obstacles. The method comprises the following steps: firstly, initializing map information and a random tree, and setting related parameters; secondly, an initial value of a sampling ellipsoid domain is defined, the long axis direction of the ellipsoid points to the target position in real time, the range of the ellipsoid sampling domain is expanded every time a given iteration number is increased, and when the number of obstacles in the sampling domain exceeds a given number or repulsive force borne by a current point exceeds a threshold value, the range of the ellipsoid sampling domain is narrowed, and flexible adjustment of the sampling domain is achieved. New nodes are expanded through an artificial potential field method, and collision detection and path optimization are carried out; and finally, repeating the steps until the maximum number of iterations is met, and outputting a drawn path. According to the method, the sampling domain range can be adaptively adjusted according to the change of the surrounding environment of the current node, the number of redundant nodes is reduced, the search efficiency is improved, the path length is reduced, and the search time of the mechanical arm under dense obstacles is greatly shortened.

Description

Technical Field

[0001] The present invention belongs to the field of robot control technology, and specifically relates to the field of robot arm path planning, in particular to a robot arm path planning method with an adaptive sampling domain under dense obstacles. Background Art

[0002] With the rapid development of my country's logistics and warehousing industries and the increasing consumption of Chinese residents, the demand for faster commodity logistics has steadily increased. Robotic arms, which combine fast handling speed and high strength, are widely used in the logistics and warehousing industry. How to use robotic arms to ensure accurate obstacle avoidance and sorting of items through intelligent technology while ensuring safety and stability during the movement of items has become a key issue that needs to be addressed in the logistics and warehousing industry.

[0003] The operating environment of logistics sorting robotic arms is characterized by significant spatial constraints and dense obstacles. This unique operating environment presents a dual challenge for robotic systems: on the one hand, they must ensure high-precision sorting and retrieval of items, while on the other hand, they must guarantee operational safety and efficiency within the confined space. Therefore, developing a path planning solution for robotic arms that adapts to dense obstacles is of great practical significance.

[0004] The rapidly-exploring random trees (RRT) algorithm currently used in six-axis robotic arms has unique advantages and limitations in the field of robotic arm path planning. This is primarily reflected in its strong adaptability to high-dimensional spaces. In complex obstacle environments, its planning success rate is 35-50% higher than that of the artificial potential field (APF) method. Numerous algorithms have also been derived. The latest research advance on the RRT algorithm is "Neural-Informed-RRT*: Learning-based Path Planning with Point Cloud State Representations under Admissible Ellipsoidal Constraints" (ICRA2024). This paper implements neural focusing to restrict the point cloud to a subset of acceptable ellipsoids in the Informed Rapidly-exploring Random Tree Star (Informed-RRT*) algorithm based on heuristic sampling. The algorithm is then fed into the point cloud deep learning model PointNet++ for inference to optimize the node expansion process. The robot arm sorts goods on shelves in a warehouse, which is a scenario with dense obstacles and high requirements for path quality. Therefore, the Informed-RRT* algorithm has become the mainstream path planning algorithm for robot arms in this scenario.

[0005] The current research on robot path planning using the Informed-RRT* algorithm under dense obstacles has the following problems:

[0006] (1) The sampling efficiency is low in an environment with dense obstacles. In the paper "Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible EllipsoidalHeuristic", an RRT* algorithm that is restricted to searching within a fixed-area elliptical sampling domain is proposed to address the problems of slow convergence speed and large randomness of the search range of the RRT* algorithm. In the paper, the search time is shortened by limiting the sampling domain range, and the sampling efficiency is greatly improved compared with the full-space sampling efficiency of the RRT* algorithm. However, for the path planning of a manipulator under dense obstacles, a fixed ellipsoid sampling domain may cause the sampling points to be too concentrated or irrationally distributed, which may easily fall into a local optimum or slow convergence speed. The path planning method proposed in this invention is based on the APF. The sampling domain range is adjusted according to changes in time and space through adaptive ellipsoid sampling domain sampling. The ellipsoid sampling domain can be expanded and shrunk, reducing unnecessary sampling consumption of the search tree, avoiding excessive concentration or irrational distribution of sampling points, greatly improving sampling efficiency, and significantly reducing path convergence time.

[0007] (2) The robot path is not safe and stable enough. The paper "Robot obstacle avoidance planning method based on improved RRT and improved APF" redefines the attraction and repulsion rules in the artificial potential field, so that it adopts a segmented potential field, reducing the excessive attraction when far away from the target, which causes the robot arm to produce a large joint angular velocity, and solves the problem of not having enough time to slow down and avoid obstacles. Redefining the potential field rules can effectively adjust the movement of the robot arm, but the potential field with obvious boundaries may cause the speed to jump during the robot arm control process, and the clamped object may suddenly slow down from fast, causing the object to fall, and the operation safety and stability cannot be guaranteed. The method proposed in this invention reduces the adaptive sampling domain range according to the repulsive force at the sampling point under APF and the number of obstacles in the sampling domain. The robot arm is expanded in the adaptive sampling domain to ensure that the robot arm has enough time to adjust its speed before the obstacle, avoiding the occurrence of collision. At the same time, the complete potential field is used to act on the robot arm during its operation, avoiding the speed jump phenomenon caused by reaching the boundary point of the potential field segmentation function during the robot arm control process, and ensuring the safety of the robot arm path planning and the stability of the clamped object. Summary of the Invention

[0008] The purpose of the invention is to provide an improved rapid expansion random tree algorithm, which realizes the cargo access function of the robot arm under dense obstacles by dynamically adjusting the size of the elliptical sampling domain. Compared with other rapid expansion random tree algorithms, the sampling efficiency is higher and the speed is faster. At the same time, the artificial potential field method is integrated to realize adaptive sampling domain range adjustment and expansion node path optimization, making the path smoother and more stable. The use of RRT* optimization enables the robot arm to select the optimal path during the path planning process to avoid falling into the local optimum and failing to reach the target point.

[0009] To achieve the above object, the present invention adopts the following scheme:

[0010] S1.1: Initialize the starting point start_point and the ending point goal_point, simulate and generate obstacles. Initialize the random tree, set the number of iterations and step size, define the artificial potential field parameters and the sampling ellipsoid domain adjustment parameters. The ellipsoid domain parameter setting formula is:

[0011] c=||goal-start|| (1)

[0012]

[0013] In the formula, start is the starting point, goal is the end point, c is the Euclidean distance from the starting point to the end point, a inital To initialize the length of the major axis of the ellipsoid, b inital To initialize the minor axis length of the ellipsoid, center is the center position of the ellipsoid. (c x ,c y ,c z ) is the center of the ellipsoid, and x′, y′, z′ are the coordinates in the coordinate system after S2.4 rotation.

[0014] S1.2: The robot grips the object and starts the path search from the starting position start_point. The ellipsoid sampling domain is dynamically adjusted according to the set adjustment frequency, the number of obstacles in the sampling domain, and the repulsion threshold of the sampling point. The range of the ellipsoid sampling domain is expanded with the increase of the number of iterations. When the number of obstacles increases by more than a given value or the sampling point exceeds the repulsion threshold, the range of the ellipsoid sampling domain is reduced, realizing dynamic and flexible adjustment of the ellipsoid sampling domain. The dynamic adjustment formula of the ellipsoid is:

[0015]

[0016] a i+1 =a i ·x (7)

[0017] b i+1 =b i ·x (8)

[0018] Where n is the number of times the ellipsoid has been expanded to a given value, the subscript k represents the kth expansion, a is the length of the major axis of the ellipsoid sampling domain, and b is the length of the minor axis of the ellipsoid sampling domain. The subscript i represents the i-th reduction of the ellipsoid domain when the set number of obstacles in the sampling domain or the repulsive force threshold of the sampling points is met, and x is the reduction ratio of the ellipsoid sampling domain.

[0019] S1.3: The generated random tree has a certain probability of directly generating the end point, jumping to S3 potential field control extension to perform node collision detection, otherwise entering S2 adaptive sampling.

[0020] S2: Adaptive sampling implements sampling operations by rotating and translating the dynamic ellipsoid domain, which includes the following sub-steps:

[0021] S2.1: Random sampling within the unit sphere. Randomly generate point coordinates (x, y, z) within the unit sphere. The point coordinates satisfy the following formula:

[0022] x 2 +y 2 +z 2 ≤1 (9)

[0023] S2.2: Scale to the ellipsoid axis length. The random points generated in S2.1 are scaled by matrix operations to achieve random sampling point scaling. At the same time, the obstacle center is converted to the ellipsoid sampling domain. If the number of obstacles detected in the ellipsoid sampling domain exceeds a given value, the ellipsoid sampling domain range is adjusted. The scaled sampling point coordinates P are obtained. scaled , the calculation formula is as follows:

[0024]

[0025] Where, P scaled is the coordinate of the sampling point after scaling, a is the length of the major axis of the current ellipsoid, b is the length of the minor axis of the current ellipsoid, and (x, y, z) is the coordinate of the sampling point.

[0026] S2.3: Rotate and align the main axis. Construct the rotation matrix R to align the main axis (long axis) of the ellipsoid from the start point to the end point. The formula for dynamically adjusting the direction of the ellipsoid and rotating the main axis is as follows:

[0027]

[0028] w=u×v (13)

[0029] R=[uvw] (14)

[0030] Where start is the starting position, goal is the ending position, c is the vector modulus from the starting point to the end point, u is the unit vector in the principal axis direction, v is the orthogonal vector in the plane perpendicular to u, w is the outer product of u and v, and R is the rotation matrix for adjusting the direction of the ellipsoid.

[0031] S2.4: Translate to the center of the new ellipsoid. Rotate and translate the sampling points in the ellipsoid domain obtained in S2.3 to the center of the new ellipsoid. The rotation and translation formula is as follows:

[0032] p final =R·p scaled +center (15)

[0033] Where p final is the final position of the sampling node in this round of sampling, R is the rotation matrix for adjusting the direction of the ellipsoid, and p scaled is the initial position of the sampling node after scaling, and center is the center of the ellipsoid.

[0034] S3: Potential field control tree expansion. Using the artificial potential field method, repulsion is applied to obstacles and attraction is applied to target nodes. The direction of the artificial potential field and the resultant force are calculated. The tree is expanded according to the potential field direction. If the new node does not collide with an obstacle, the new node is added to the tree. This includes the following sub-steps:

[0035] S3.1: Find the node closest to the random point. Find the node closest to the random point by looping through all nodes in the ellipsoid sampling domain and update the node parameters.

[0036] S3.2: Calculate the artificial potential field's force and expand the random tree based on the potential field's direction. The artificial potential field's force consists of attraction and repulsion. The attraction is generated by the attraction of the target point and the attraction of the random sampling points, while the repulsion is generated by the spherical and rectangular obstacles. The calculated repulsion at the current sampling point is compared with the repulsion threshold in each cycle. If the threshold is exceeded, the length of the major and minor axes of the sampling ellipsoid is reduced.

[0037] The formula for calculating attractiveness is as follows:

[0038]

[0039] F att =F att_goal +F att_rand (18)

[0040] Where, F att_goal is the attraction of the target point, F att_rand is the attraction of random sampling points, F att is the total attraction, α is the target attraction weight, β is the random point attraction weight, goal is the target point coordinate, q curr is the current node coordinate, q rand are the coordinates of random sampling points.

[0041] Repulsive force calculation formula (when 0 <d eff <ρ rep (Effective when) as follows:

[0042]

[0043]

[0044] Where, F rep1 is the basic repulsive force, F rep2 is the additional repulsive force, F rep is the total repulsive force of the obstacle, k rep is the repulsive strength coefficient, ρ rep is the range of repulsive force, n is the exponential parameter of additional repulsive force, ρ g is the distance from the current node to the target, d eff is the shortest distance from the current node to the obstacle surface, is the direction of repulsion, q curr is the current node coordinate.

[0045] Calculation of the closest distance to the obstacle and the direction of repulsion:

[0046] d eff =||q curr -c obs ||-r obs (twenty two)

[0047]

[0048] Where, d eff is the distance from the current node to the nearest point on the surface of the spherical obstacle, q clamp is the distance from the current node to the nearest point on the surface of the rectangular obstacle, is the direction of repulsion of the current node, q curr is the current node coordinate, c obs is the coordinate of the sphere center, r obs is the radius of the sphere, d obs =[d x ,d y ,d z ] is the size of the cuboid.

[0049] The net force is obtained by adding the attractive force and the repulsive force as follows:

[0050] F total =F att +F rep (26)

[0051]

[0052] Where, F total is the resultant force vector, F att is the total attraction vector, F rep is the total repulsive force vector, The direction of the resultant force.

[0053] S3.3: Collision detection. If the new node collides with an obstacle, return to S3.1 to find another node closest to the random point. Otherwise, execute S3.4 to add the new node to the tree.

[0054] S3.4: New nodes are added to the tree.

[0055] S4: Determine whether the child node and the parent node exist. If so, draw a line connecting the parent and child nodes, perform RRT* optimization, and return to S1.2 to continue iterating, and finally generate a drawing path. If the parent and child nodes do not all exist or after performing RRT* optimization, return to S1.2 to continue iterating, and finally generate a drawing path. It includes the following sub-steps:

[0056] S4.1: Determine whether the child node exists. If so, draw a new node. Otherwise, return and continue iteration.

[0057] S4.2: Determine whether the parent node exists. If so, draw a line connecting the parent and child nodes. Otherwise, return and continue iteration.

[0058] S4.3: Perform RRT* optimization and return to S1.2 to continue iteration.

[0059] S4.4: When the maximum number of iterations is reached, exit the loop and generate a drawing path.

[0060] The present invention has the following beneficial effects:

[0061] (1) Optimization of the number of sampling points. The present invention proposes a path planning method for a cargo access robot arm suitable for use in dense obstacles. In the narrow passages between warehouse shelves, traditional path planning algorithms such as the RRT algorithm and the Informed RRT* algorithm are difficult to operate efficiently due to the dense obstacles. These algorithms have low sampling efficiency in dense obstacle spaces and are difficult to adapt to. To this end, the present invention introduces an adaptive ellipsoid domain sampling mechanism. This mechanism can adjust the direction of the ellipsoid sampling domain in real time so that it always points to the end point. It can flexibly adjust the adaptive sampling domain according to the number of iterations and the number and position of obstacles, so that the sampling domain can be expanded or shrunk to adapt to dense obstacle environments. Experimental results show that compared with the RRT* and Informed RRT* algorithms, the improved Artificial Potential Field-Informed Rapidly-exploring Random Tree Star (APF-Informed-RRT*) algorithm of the present invention reduces the number of sampling points by 94.52% and 94.56% respectively, reduces the running time by 93.92% and 93.97% respectively, and reduces the path length by 65.51 cm and 78.30 cm respectively.

[0062] (2) Improved safety and stability of the robot path. When storing and retrieving goods in a space with dense obstacles, in addition to considering the impact of obstacles on the sampling points, the safe and stable operation of the robot is also crucial. In view of the high-dimensional characteristics of the robot path planning, the present invention adopts the progressively optimal rapidly expanding random tree RRT* as the basic sampling algorithm. However, the path planned by the traditional Informed RRT* algorithm usually has the problem of poor smoothness. To this end, the present invention introduces the artificial potential field method APF, which optimizes the expansion process of the RRT tree by calculating the potential field pressure of the current sampling point, significantly improving the smoothness and stability of the path. Experimental results show that compared with the APF-RRT* algorithm, the improved APF-Informed-RRT* algorithm of the present invention reduces the sampling nodes by 85.34%, the running time is reduced by 85.27%, and the path length is reduced by 1.26 cm. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 This is a flow chart of the path planning method for a robotic arm under dense obstacles;

[0064] Figure 2 It is a schematic diagram of the elliptical sampling domain in the initial stage under the two-dimensional plane;

[0065] Figure 3 It is a schematic diagram of the elliptical sampling domain in the intermediate stage in a two-dimensional plane;

[0066] Figure 4It is a schematic diagram of the elliptical sampling domain in the final stage in a two-dimensional plane;

[0067] Figure 5 This is a schematic diagram of the RRT* algorithm path planning under dense obstacles;

[0068] Figure 6 This is a schematic diagram of the Informed RRT* algorithm path planning under dense obstacles;

[0069] Figure 7 This is a schematic diagram of the APF-RRT* algorithm path planning under dense obstacles;

[0070] Figure 8 This is a schematic diagram of the improved APF-Informed-RRT* algorithm path planning under dense obstacles;

[0071] Figure 9 This is a physical picture of the starting point of the robot arm grasping the object;

[0072] Figure 10 This is a physical picture of the final position of the robotic arm grasping the object. DETAILED DESCRIPTION

[0073] The technical solutions in the embodiments of the present invention will be described clearly and completely below. The present invention relates to a path planning method for a cargo access robot suitable for use in a limited space.

[0074] Example: Figure 1 The figure shows a flow chart of the path planning method for a goods sorting robot arm in dense obstacle conditions. First, the starting and target positions are initialized, and obstacles are simulated. The number of iterations is set to enter a loop, and the expansion and reduction parameters of the ellipsoid adaptive sampling domain are defined. The ellipsoid adaptive sampling strategy in this algorithm is as follows: First, a loop is performed within a given total number of iterations to determine whether the specific number of iterations is a multiple of the number required for adaptive sampling domain expansion, whether the number of obstacles in the sampling domain exceeds a given value, or whether the repulsive force on the sampling point exceeds a given value. If it is a multiple of the number of iterations required for ellipsoid sampling domain expansion, the lengths of the major and minor axes of the ellipse are increased. If the number of obstacles in the ellipsoid sampling domain exceeds a given number or the repulsive force on the current sampling point exceeds a threshold, the lengths of the major and minor axes of the ellipsoid are reduced, so that the ellipsoid sampling domain range is adaptively adjusted as the above conditions change, and the next step is entered. If the number of iterations is not a multiple of the number of iterations required for ellipsoid expansion and both the obstacle and repulsive force conditions are not met, the size of the ellipsoid sampling domain is not changed, and the next step is entered. Figure 2 、 Figure 3 、 Figure 4The following diagrams show the real-time update of the adaptive sampling domain at the initial, intermediate, and final stages of the projected two-dimensional plane. Observation reveals that the sampling domain changes over time, with the overall sampling domain tending to expand. It shrinks when the number of obstacles in the sampling domain changes or the repulsive force on the sampling point changes, while the long axis always points to the target position in real time. The node then enters the artificial potential field-controlled extended random tree module, executing parent-child node connections and RRT* optimization. Finally, the above steps are repeated until the maximum number of iterations is met, generating the output drawing path. Specifically, the following steps are included:

[0075] S1.1: Initialize the map information 3D coordinate boundary range x max =150,y max =150,z max = 100, start_point = [10 10 10] and end_point = [140 140 90], set the coordinates and sizes of the generated sphere and rectangular obstacle. Initialize the random tree, set the maximum number of iterations max_iter = 20000, step_size = 5, goal tolerance goal_tolerance = 10, reconnection radius radius = 10, define the artificial potential field parameters target attraction weight α = 1.0, random point attraction weight β = 3.0, repulsion coefficient k rep = 0.1, repulsion effective range rep_range = 5, additional repulsion parameter napf = 2, given value ellipsoid expansion times n = 100, the number of iterations required for each round of ellipsoid expansion iter_per_expansion = 200, the ellipsoid domain parameter setting formula is:

[0076] c=||goal-start|| (1)

[0077]

[0078] iter_per_expansion=max_iter / n (5)

[0079] In the formula, start is the starting point, goal is the end point, c is the Euclidean distance from the starting point to the end point, a inital To initialize the length of the major axis of the ellipsoid, b inital To initialize the minor axis length of the ellipsoid, center is the center position of the ellipsoid. (c x ,c y ,c z ) is the center of the ellipsoid, x′, y′, z′ are the coordinates in the coordinate system after S2.4 rotation, and iter_per_expansion is the number of iterations required for each round of ellipsoid expansion.

[0080] S1.2: The robotic arm grasps the object and starts the path search from the starting position start_point. According to the set adjustment frequency, the ellipsoid sampling domain is dynamically adjusted every time the number of iterations increases by 200 times, or when the number of obstacles in the ellipsoid sampling domain is greater than or equal to 5, or when the repulsive force on the sampling point is greater than or equal to the threshold of 10. The range of the ellipsoid sampling domain gradually expands with the increase of the number of iterations. The length of the major and minor axes is expanded by a factor of 1.01 every time the number of iterations increases by 200 times. At the same time, the range of the ellipsoid sampling domain is reduced when the number of obstacles exceeds 5 or the sampling point exceeds the repulsive force threshold of 10. The length of the major and minor axes is reduced by a factor of 0.9 each time, realizing dynamic and flexible adjustment of the ellipsoid sampling domain. The dynamic adjustment formula of the ellipsoid is:

[0081]

[0082] a i+1 =a i ·x (8)

[0083] b i+1 =b i ·x (9)

[0084] Where n = 100 is the number of times the ellipsoid is expanded to a given value, the subscript k represents the kth expansion, a is the length of the major axis of the ellipsoid sampling domain, and b is the length of the minor axis of the ellipsoid sampling domain. The subscript i represents the number of times the ellipsoid is reduced when the set number of obstacles in the sampling domain or the repulsive force threshold of the sampling points is met, and x = 0.9 is the reduction ratio of the ellipsoid sampling domain.

[0085] S1.3: The generated random tree has a 30% probability of directly generating the end point, jumping to S3 potential field control extension to perform node collision detection, otherwise it enters S2 adaptive sampling.

[0086] S2: Adaptive sampling implements sampling operations by rotating and translating the dynamic ellipsoid domain, which includes the following sub-steps:

[0087] S2.1: Random sampling within the unit sphere. Randomly generate a point (x, y, z) within the unit sphere, and the point coordinates satisfy the following formula:

[0088] x 2 +y 2 +z 2 ≤1 (10)

[0089] S2.2: Scale to the ellipsoid axis length. The random points generated in S2.1 are scaled by matrix operations to achieve random sampling point scaling. At the same time, the obstacle center is converted to the ellipsoid sampling domain. If the number of obstacles detected in the ellipsoid sampling domain exceeds 5, the ellipsoid sampling domain range is adjusted. The scaled sampling point coordinates P are obtained. scaled , the calculation formula is as follows:

[0090]

[0091] Where, P scaled is the coordinate of the sampling point after scaling, a is the length of the major axis of the current ellipsoid, b is the length of the minor axis of the current ellipsoid, and (x, y, z) is the coordinate of the sampling point.

[0092] S2.3: Rotate and align the main axis. Construct the rotation matrix R to align the main axis (long axis) of the ellipsoid from the start point to the end point. The formula for dynamically adjusting the direction of the ellipsoid and rotating the main axis is as follows:

[0093]

[0094] w=u×v (14)

[0095] R=[uvw] (15)

[0096] Where start is the starting position, goal is the ending position, c is the vector modulus from the starting point to the end point, u is the unit vector in the principal axis direction, v is the orthogonal vector in the plane perpendicular to u, w is the outer product of u and v, and R is the rotation matrix for adjusting the direction of the ellipsoid.

[0097] S2.4: Translate to the new ellipsoid center. Rotate and translate the sampling points in the ellipsoid domain obtained in S2.3 to the new ellipsoid center. The rotation and translation formula is as follows:

[0098] p final =R·p scaled +center (16)

[0099] Where p final is the final position of the sampling node in this round of sampling, R is the rotation matrix for adjusting the direction of the ellipsoid, and p scaled is the initial position of the sampling node after scaling, and center is the center of the ellipsoid.

[0100] S3: Potential field control to expand the random tree. Use the artificial potential field method to apply repulsive force to obstacles and attractive force to target nodes. Calculate the direction of the artificial potential field and the resultant force. Expand the tree according to the potential field direction. If the new node does not collide with the obstacle, add the new node to the random tree. This includes the following sub-steps:

[0101] S3.1: Find the node closest to the random point. Find the node closest to the random point by looping through all nodes in the ellipsoid sampling domain and update the node parameters.

[0102] S3.2: Calculate the artificial potential field's force and expand the random tree based on the potential field's direction. The artificial potential field's force consists of attraction and repulsion. The attraction is generated by the attraction of the target point and the attraction of the random sampling points, while the repulsion is generated by the spherical and rectangular obstacles. The calculated repulsive force on the current sampling point is compared with a repulsion threshold of 10 in each cycle to dynamically adjust the sampling ellipsoid domain.

[0103] The formula for calculating attractiveness is as follows:

[0104]

[0105] F att =F att_goal +F att_rand (19)

[0106] Where, F att_goal is the attraction of the target point, F att_rand is the attraction of random sampling points, F att is the total attraction, α is the target attraction weight, β is the random point attraction weight, goal is the target point coordinate, q curr is the current node coordinate, q rand are the coordinates of random sampling points.

[0107] Repulsive force calculation formula (when 0 <d eff <ρ rep (Effective when) as follows:

[0108]

[0109] Where, F rep1 is the basic repulsive force, F rep2 is the additional repulsive force, F rep is the total repulsive force of the obstacle, k rep is the repulsive strength coefficient, ρ rep is the range of repulsive force, n is the exponential parameter of additional repulsive force, ρ g is the distance from the current node to the target, d eff is the shortest distance from the current node to the obstacle surface, is the direction of repulsion, q curr is the current node coordinate.

[0110] Calculation of the closest distance to the obstacle and the direction of repulsion:

[0111] d eff =||q curr -c obs ||-r obs (twenty three)

[0112]

[0113] Where, d eff is the distance from the current node to the nearest point on the surface of the spherical obstacle, q clamp is the distance from the current node to the nearest point on the surface of the rectangular obstacle, is the direction of repulsion of the current node, q curr is the current node coordinate, c obs is the coordinate of the sphere center, r obs is the radius of the sphere, d obs =[d x ,d y ,d z ] is the size of the cuboid.

[0114] The net force is obtained by adding the attractive force and the repulsive force as follows:

[0115] F total =F att +F rep (27)

[0116]

[0117] Where, F total is the resultant force vector, F att is the total attraction vector, F rep is the total repulsive force vector, The direction of the resultant force.

[0118] S3.3: Collision detection. If the new node collides with an obstacle, return to S3.1 to find another node closest to the random point. Otherwise, execute S3.4 to add the new node to the tree.

[0119] S3.4: New nodes are added to the tree.

[0120] S4: Determine whether the child node and the parent node exist. If so, draw a line connecting the parent and child nodes, perform RRT* optimization, and return to S1.2 to continue iterating, and finally generate a drawing path. If the parent and child nodes do not all exist or after performing RRT* optimization, return to S1.2 to continue iterating, and finally generate a drawing path. It includes the following sub-steps:

[0121] S4.1: Determine whether the child node exists. If so, draw a new node. Otherwise, return and continue iteration.

[0122] S4.2: Determine whether the parent node exists. If so, draw a line connecting the parent and child nodes. Otherwise, return and continue iteration.

[0123] S4.3: Execute RRT* optimization, return to S1.2 to continue iteration, and exit the loop when the maximum number of iterations, 20,000, is reached.

[0124] S4.4: Generate a drawing path.

[0125] To further verify the effectiveness of the aforementioned solution, Matlab simulations were performed to validate the adaptability of the improved APF-Informed-RRT* algorithm in dense obstacle environments. The algorithm was compared with the traditional RRT* algorithm, the Informed RRT* algorithm with a fixed-size elliptical sampling domain, and the APF-RRT* algorithm in an artificial potential field. Multiple repeated experiments were conducted with fixed obstacles on a given map, resulting in the simulation data shown in Table 1.

[0126] Table 1 Experimental data of four algorithms

[0127]

[0128] As can be seen from the table, the improved APF-Informed-RRT* algorithm of the present invention has significant improvements compared with the original RRT* algorithm, the Informed RRT* algorithm with a fixed-size elliptical sampling domain, and the APF-RRT* algorithm under the artificial potential field when the number of obstacles in a limited space gradually increases.

[0129] (1) In terms of path length, the improved APF-Informed-RRT* algorithm of the present invention is reduced by 65.51 cm compared with the original RRT* algorithm, by 78.30 cm compared with the Informed RRT* algorithm with a fixed-size elliptical sampling domain, and by 1.26 cm compared with the APF-RRT* algorithm under the artificial potential field. In terms of path length, the path length of the improved algorithm is effectively reduced, and the quality and efficiency of the path are improved.

[0130] (2) In terms of running time, the improved APF-Informed-RRT* algorithm of the present invention is reduced by 93.92% compared with the original RRT* algorithm, by 93.97% compared with the Informed RRT* algorithm with a fixed-size elliptical sampling domain, and by 85.27% compared with the APF-RRT* algorithm under an artificial potential field. In terms of running time, the running time of the improved algorithm is significantly reduced compared with the other three algorithms.

[0131] (3) In terms of search efficiency, the improved APF-Informed-RRT* algorithm of the present invention reduces the number of nodes by 94.52%, 94.55%, and 85.34% compared to the traditional RRT* algorithm, the Informed RRT* algorithm with a fixed-size elliptical sampling domain, and the APF-RRT* algorithm under an artificial potential field. The improved APF-Informed-RRT* algorithm significantly improves search efficiency.

[0132] When the robot arm is used to pick up goods and put them on the shelf under dense obstacles and execute the RRT* algorithm, the simulation results are as follows: Figure 5As shown; when the robot arm is used to pick up goods and put them on the shelf under dense obstacles and execute the Informed RRT* algorithm, the simulation results are as follows Figure 6 As shown; when the APF-RRT* algorithm is implemented to implement the robot arm to pick up goods and put them on the shelf under dense obstacles, the simulation results are as follows Figure 7 As shown; when the robot arm is used to pick up goods and put them on the shelf under dense obstacles and the improved algorithm APF-Informed-RRT* of the present invention is implemented, the simulation results are as follows Figure 8 As shown. The starting position of the robot arm to grasp the object is as follows Figure 9 As shown, the target position is Figure 10 As shown in the figure, the hardware consists of an FR5 robotic arm, a depth camera, and a main control unit. The Robot Operating System (ROS) is used as middleware to implement related operations. In the experimental environment, brackets and cartons are used to simulate the dense obstacle environment in a warehouse, and objects are placed from the starting position to the target position. When performing cargo grabbing and shelving operations under dense obstacles, the robotic arm needs to cope with the dense obstacles while also ensuring the smoothness of the path and the stability of the speed. Therefore, the improved APF-Informed-RRT* algorithm of this invention can better adapt to this situation.

[0133] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not limiting. Although the technical solutions of the present invention are described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the present invention, and all of them should be included in the scope of protection of the present invention.

Claims

1. A robotic arm path planning method with adaptive sampling domain under dense obstacles, characterized by: The steps include: S1.1: Initialize map information and random tree, set the total number of iterations and step size, define artificial potential field parameters and sampling ellipsoid domain adjustment parameters; S1.2: The robot grips the object and starts the path search from the starting position start_point. The ellipsoid sampling domain is dynamically adjusted according to the set adjustment frequency, the number of obstacles in the sampling domain, and the repulsion threshold of the sampling point. The range of the ellipsoid sampling domain is expanded with the increase of the number of iterations. When the number of obstacles increases by more than a given value or the sampling point exceeds the repulsion threshold, the range of the ellipsoid sampling domain is reduced, realizing dynamic and flexible adjustment of the ellipsoid sampling domain. The dynamic adjustment formula of the ellipsoid is: a i+1 =a i ·x (3) b i+1 =b i ·x (4) Where n is the number of times the ellipsoid is expanded for a given value, subscript k represents the kth expansion, a is the length of the major axis of the ellipsoid sampling domain, b is the length of the minor axis of the ellipsoid sampling domain, subscript i represents the number of times the ellipsoid domain is reduced when the set number of obstacles in the sampling domain or the repulsion threshold of the sampling point is met, and x is the ratio of the reduction of the ellipsoid sampling domain; S1.3: The generated random tree has a certain probability of directly generating the end point, jumping to S3 potential field control extension to perform node collision detection, otherwise it enters S2 adaptive sampling; S2: Adaptive sampling implements sampling operations by rotating and translating the dynamic ellipsoid domain, which includes the following sub-steps: S2.1: Random sampling within the unit sphere. Randomly generate coordinate points (x, y, z) within the unit sphere. The coordinates of the points satisfy the following formula: x 2 +y 2 +z 2 ≤1 (5) S2.2: Scale to the ellipsoid axis length. Use matrix operations to scale the random points generated in S2.1 within the sphere. At the same time, convert the obstacle center to the ellipsoid sampling domain. If the number of obstacles detected in the ellipsoid sampling domain exceeds a fixed value, adjust the ellipsoid sampling domain range to obtain the scaled sampling point coordinates P. scaled , the calculation formula is as follows: Where, P scaled is the coordinate of the sampling point after scaling, a is the length of the major axis of the current ellipsoid, b is the length of the minor axis of the current ellipsoid, and (x, y, z) is the coordinate of the sampling point; S2.3: Rotate and align the main axis. Construct the rotation matrix R to align the major axis of the ellipsoid from the start point to the end point. Dynamically adjust the direction of the ellipsoid to rotate and align the main axis. The relevant formula is as follows: w=u×v (9) R=[uvw] (10) Where start is the starting position, goal is the ending position, c is the vector modulus from the starting point to the ending point, u is the unit vector in the principal axis direction, v is the orthogonal vector in the plane perpendicular to u, w is the outer product of u and v, and R is the rotation matrix for adjusting the direction of the ellipsoid. S2.4: Translate to the center of the new ellipsoid. Rotate and translate the sampling points in the ellipsoid domain obtained in S2.3 to the center of the new ellipsoid. The rotation and translation formula is as follows: p final =R·p scaled +center (11) Where p final is the final position of the sampling node in this round of sampling, R is the rotation matrix for adjusting the direction of the ellipsoid, and p scaled is the initial position of the sampling node after scaling, and center is the center position of the new ellipsoid; S3: Potential field control expands the random tree. The artificial potential field method is used to apply repulsive force to obstacles and attractive force to target nodes. The direction of the artificial potential field and the resultant force are calculated. The tree is expanded according to the direction of the potential field. If the new node does not collide with the obstacle, the new node is added to the tree. The following sub-steps are included: S3.1: Find the node closest to the random point by traversing all nodes in the ellipsoid sampling domain and update the node parameters; S3.2: Calculate the artificial potential field force and expand the random tree according to the direction of the potential field. The artificial potential field force consists of two parts: attraction and repulsion. The attraction is composed of the attraction of the target point and the attraction of the random sampling point, and the repulsion is generated by the spherical obstacle and the rectangular obstacle. The calculated repulsion of the current sampling point is compared with the repulsion threshold in each cycle, and the sampling ellipsoid domain range is dynamically adjusted. The resultant force is obtained by adding the attraction and repulsion as follows: F total =F att +F rep (12) Where, F total is the resultant force vector, F att is the total attraction vector, F rep is the total repulsive force vector, is the direction of the resultant force; S3.3: Collision detection. If the new node collides with an obstacle, return to S3.1 to find another node closest to the random point. Otherwise, execute S3.4 to add the new node to the tree. S3.4: New nodes are added to the tree; S4: Determine whether the child node and the parent node exist. If so, draw a line connecting the parent and child nodes, perform an Asymptotically Optimal Rapidly-exploring Random Tree (RRT*) optimization, and return to S1.2 to continue iterating, ultimately generating a drawing path. If not all parent and child nodes exist or after performing RRT* optimization, return to S1.2 to continue iterating, ultimately generating a drawing path. This includes the following sub-steps: S4.1: Determine whether the child node exists. If so, draw a new node. Otherwise, return and continue iteration. S4.2: Determine whether the parent node exists. If so, draw a line connecting the parent and child nodes. Otherwise, return to continue iteration. S4.3: Execute RRT* optimization and return to S1.2 to continue iteration; S4.4: When the maximum number of iterations is reached, exit the loop and generate a drawing path.

Images