Mechanical arm path planning method based on repulsive force range constraint

Abstract

The invention discloses a mechanical arm path planning method based on repulsive force range constraint, which belongs to the technical field of intelligent manufacturing, and comprises the following steps: introducing constraint sampling of probability target bias to improve planning efficiency and directivity; artificial potential field expansion based on repulsive force range constraint is adopted to guide the random tree to avoid obstacles and accelerate to approach a target point; and local optimum processing of the virtual target point is carried out to guide the random tree to effectively avoid local optimum. And redundancy detection processing: carrying out secondary optimization processing on the path by adopting a pruning method so as to eliminate redundant nodes. The method has good path planning efficiency, is suitable for actual operation and operation of the mechanical arm, and has remarkable advantages in the aspects of planning time, path length and stability.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent manufacturing technology, and in particular relates to a robot arm path planning method based on repulsive force range constraints. Background Art

[0002] With the rapid development of automation technology, collaborative robotic arms have gradually become a core component of intelligent manufacturing and multi-field automated operations. Their application scope has expanded to cutting-edge fields such as special operations, sand painting creation, and soft grasping control. Collaborative robotic arms need to operate in complex and diverse environments, which places higher demands on the efficiency and reliability of their path planning methods. Robotic arm path planning refers to planning a collision-free continuous path from the starting position to the target position for the end effector of the robotic arm within a given working space. In high-dimensional configuration space, traditional path planning methods such as the A* algorithm, ant colony algorithm, genetic algorithm, and artificial potential field method often face the challenge of a sharp increase in computational complexity.

[0003] The fast random search tree algorithm (RRT, ML S. Rapidly-Exploring Random Trees: A New Tool for Path Planning[J]. Research Report, 1999.) proposed by Steven M. LaValle effectively traverses non-convex high-dimensional environments by constructing random space-filling trees, demonstrating its planning capabilities in complex spaces. However, RRT is based on a blind search strategy, which leads to problems such as slow convergence and lengthy paths. To optimize path length, Karaman (KARAMAN S, FRAZZOLI E. Introduction and configuration of a collaborative robot in an assembly task as a means to decrease occupational risks and increase efficiency in a manufacturing company[J]. INTERNATIONALJOURNAL OF ROBOTICS RESEARCH, 2011, 30(7):846-894.) proposed the RRT* algorithm, which ensures that the planned path converges to the optimal path by reselecting parent nodes and backtracking to neighbor nodes. However, this method will sacrifice a certain degree of search efficiency. To accelerate exploration, Li Ji et al. (6-DOF robot path planning based on an improved RRT algorithm [J]. Computer Applications and Software, 2020, 37(09): 221-226.) introduced a weight factor to guide random sampling points to approach the target and adopted a dynamic incremental expansion strategy to accelerate the search process, but did not fully address the problem of falling into the local optimum. Liu Yaqiu et al. (An industrial robot path obstacle avoidance planning algorithm based on an improved rapidly expanding random tree [J]. Information and Control, 2021, 50(2): 235-246, 256.) set the distance between the newly expanded node and the parent node to exceed the distance to other nodes as a strategy to escape the local optimum. However, this method inevitably increases the time overhead of the algorithm. Han Kang et al. (Robot path planning based on improved RRT-Connect algorithm [J]. Computer Applications and Software, 2022, 39(03): 260-265.) adopted a hybrid goal-oriented and heuristic sampling strategy to accelerate the search process. Although performance improvement was shown in specific scenarios, its effectiveness has not been fully verified in three-dimensional space.After obtaining the initial path, Shang Deyong et al. (Robot obstacle avoidance path planning based on RRT*-DR algorithm [J]. Computer Integrated Manufacturing Systems, 2024, 30(03):1149-1160.) gradually obtained the asymptotically optimal path by sampling and expanding around the initial path node, but the path iteration efficiency was low. Zhou Yibang et al. (Robot path planning based on improved bidirectional RRT* algorithm [J]. Computer Applications, 2022, 42(S1):342-346.) incorporated the nearest node goal guidance and local path optimization mechanism on the basis of bidirectional RRT* to improve the convergence accuracy of the algorithm, but this method lacks stability. Jia Haoduo et al. (Robot adaptive path planning based on artificial potential field and Informed-RRT* algorithm [J]. Computer Integrated Manufacturing Systems, 2023.) dynamically adjusted the step size according to the relative position of the sampling point in the artificial potential field, which helps to reduce collisions in complex obstacle environments, but the algorithm iteration time is long.

[0004] Although the above methods have made some progress in improving RRT and its variants, they still face challenges in efficiency, stability, and adaptability to complex environments. Summary of the Invention

[0005] The purpose of the present invention is to overcome the above shortcomings and provide a robot arm path planning method based on repulsive force range constraints, which has good path planning efficiency and significant advantages in planning time, path length and stability.

[0006] The purpose of the present invention and the solution of its main technical problems are achieved by adopting the following technical solutions:

[0007] A robot arm path planning method based on repulsive force range constraint of the present invention comprises the following steps:

[0008] Step 1: Constrained sampling of probability target bias:

[0009] Set the target bias threshold p and generate random numbers during the sampling process and with the threshold For comparison, if , then directly select the target point as the sampling point to guide the random tree to expand to the target point. On the contrary, if , then continue to use the random sampling method and execute it in the full sampling domain; when performing random sampling, use the constrained sampling strategy to calculate the nearest node respectively. To a random point and to the destination The direction vector of the two direction vectors is defined as the expansion angle , expansion angle Calculate according to formula (3):

[0010]

[0011] By setting the expansion angle threshold α as a control mechanism, the range of random point generation is constrained. The threshold α is adjusted according to the density of obstacles in the current workspace. After each sampling iteration, the expansion angle is calculated. Is it within the preset threshold α? If the value is less than the threshold α, the sampling point is retained; otherwise, resampling is required until the condition is met;

[0012] In the sampling space, an improved artificial potential field model is introduced to limit the repulsive force range by dividing the spatial potential field. The improved artificial potential field model includes an attraction function and a repulsion function. The attraction function of the improved artificial potential field method is shown in formulas (4) and (5):

[0013]

[0014] ;

[0015] The repulsion function is shown in formula (6):

[0016]

[0017] Combined Force It is expressed as formula (7).

[0018]

[0019] in and Target points and random points right The attraction, is the gravitational field gain constant of the target point, is the random point gravitational field gain constant; It is an obstacle The repulsive force, is the gain constant of the obstacle repulsive field, and the resultant force is obtained according to the parallelogram law. direction and determine it as a new node The optimal expansion direction;

[0020] The random tree starts with an initial step size Explore in the target direction. When expanding a new node, if the new node does not collide with an obstacle and fails to connect directly to the target point, the step size will be adjusted to , is the step expansion factor. Once a collision is detected at the expansion node, the step will be initialized immediately to ,when When entering the target area, adjust the step size to a smaller value and expand the step size The selection strategy is expressed by formula (8):

[0021]

[0022] in is the radius of the variable step area of ​​the target point;

[0023] Combining the optimal expansion direction and dynamic step size, the artificial potential field with repulsive force range constraints expands new nodes Calculated according to formula (9);

[0024]

[0025] in for ;

[0026] Step 3: Local optimal processing of virtual target points:

[0027] When a node in the random tree is detected When trapped in a local optimum, construct a node With the initial target point Then select the distance Draw a straight line from the center of the nearest obstacle O The vertical line OA is determined, and the position 1.5 times the radius of the nearest obstacle from point O along the OA direction is set as the virtual target point V. When the random tree node reaches the virtual target point V, the virtual target point is removed and the search target is redirected to the initial target. , continue to explore the path;

[0028] Step 4: Redundancy detection and processing:

[0029] Based on the initial planned path, from the starting point Connect the third node and its subsequent nodes in sequence and perform collision detection. With the starting point When the line collides with an obstacle, the previous node of the node is selected and Direct connection, delete arrive All nodes between; similarly, from the end point At the beginning, the nodes are detected in turn and pruning is performed. At the same time, based on the first step of pruning, the intermediate nodes are pruned. From the second node to the second to last node, the three adjacent nodes are connected in sequence and collision detection is performed. If no collision occurs, the intermediate node is deleted. Finally, the optimized path after removing redundant points is obtained. → → → .

[0030] The above-mentioned robot arm path planning method based on repulsive force range constraint, wherein: the expansion angle threshold α in step 1 is set to .

[0031] Compared with the existing technology, the present invention has obvious advantages and beneficial effects. It can be seen from the above technical solutions that the present invention adopts constrained sampling with probability target bias, which constrains the generation range of sampling points to avoid invalid exploration and shorten the planning time; adopts artificial potential field expansion with repulsive range constraint, and guides the random tree to avoid obstacles and accelerate the approach to the target point by regulating the expansion direction and step size of the new node; adopts local optimal processing of virtual target points, dynamically adjusts the search target, and guides the algorithm to effectively avoid local optimality; adopts pruning to optimize the planning path, reduce the path length and reduce the degree of path tortuosity. In the simulation experiment and case test of the robot arm path planning, the present invention has significant advantages in planning time, path length and stability, and demonstrates good path planning efficiency, which is suitable for the actual operation and operation of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 Schematic diagram of the virtual target point.

[0033] Figure 2 Schematic diagram of pruning process.

[0034] Figure 3 Diagram of the robotic arm motion planning process.

[0035] Figure 4 This is the data diagram of the change of the joint angle of the robotic arm.

[0036] Figure 5 Schematic diagram of the test scenario.

[0037] FIG6 is a key frame diagram of the robot arm obstacle avoidance test during the motion process of the present invention:

[0038] (a) The end path of the robotic arm in the simulation environment;

[0039] (b) Initial pose;

[0040] (c) obstacle avoidance posture;

[0041] (d) Target pose.

[0042] Figure 7: Comparison of real robotic arm joint angle change data:

[0043] (a) the present invention;

[0044] (b) RRT algorithm. DETAILED DESCRIPTION

[0045] The following is a detailed description of the specific implementation, structure, features and efficacy of a robot arm path planning method based on repulsive force range constraint proposed in accordance with the present invention, in conjunction with the accompanying drawings and preferred embodiments.

[0046] Example:

[0047] A robot arm path planning method based on repulsive force range constraint includes the following steps:

[0048] Step 1: Constrained sampling of probability target bias shortens planning time by constraining the generation range of sampling points.

[0049] Set the target bias threshold p and generate random numbers during the sampling process and with the threshold For comparison, if , then directly select the target point as the sampling point to guide the random tree to expand to the target point. On the contrary, if , then continue to use the random sampling method and execute it in the full sampling domain; when performing random sampling, use the constrained sampling strategy to calculate the nearest node respectively. To a random point and to the destination The direction vector of the two direction vectors is defined as the expansion angle , expansion angle Calculate according to formula (3):

[0050]

[0051] By setting the expansion angle threshold α as a control mechanism, the range of random point generation is constrained. The threshold α is adjusted according to the density of obstacles in the current workspace. The expansion angle threshold α is set to After each sampling iteration, the expansion angle is calculated Is it within the preset threshold α? If the value is less than the threshold α, the sampling point is retained; otherwise, resampling is required until the condition is met.

[0052] The pseudo code of the constrained sampling strategy based on probability target bias is shown in Algorithm 1. The following is a detailed description of each function in the pseudo code:

[0053] (1) Random number generation function Rand: obtains a random number in the range [0,1);

[0054] (2) Random sampling function Sample: Random sampling in the entire sampling domain to obtain sampling points ;

[0055] (3) ExpansionAngle function: Calculate to as well as to The angle between two direction vectors.

[0056]

[0057] Step 2: Expand the artificial potential field constrained by the repulsive force range. By controlling the expansion direction and step size of the new nodes, the random tree is guided to avoid obstacles and accelerate towards the target point.

[0058] In the sampling space, an improved artificial potential field model is introduced to limit the repulsive force range by dividing the spatial potential field. The improved artificial potential field model includes two core components: the attraction function and the repulsion function. The attraction function is shown in formulas (4) and (5):

[0059]

[0060]

[0061] The repulsive force function is shown in formula (6):

[0062]

[0063] Therefore, the combined force It is expressed as formula (7).

[0064]

[0065] in and Target points and random points right attractiveness; is the target point gravitational field gain constant, is the random point gravitational field gain constant; It is an obstacle The repulsive force, is the gain constant of the obstacle repulsive field. According to the parallelogram law, the resultant force is obtained. direction and determine it as a new node The optimal expansion direction;

[0066] In order to further optimize the node expansion process, a dynamic step size adaptive adjustment mechanism is incorporated. In the initial stage, the random tree starts with an initial step size of Explore in the direction of the target; when expanding a new node, if the new node does not collide with an obstacle and cannot be directly connected to the target point, the step size will be adjusted to , is the step expansion factor. In complex environments, to prevent the risk of collision caused by large step lengths, once a collision is detected at the expanded node, the step length will be initialized immediately to , to ensure the stability of exploration; when the node approaches the target area, if a large step size is maintained, it may cause the node to oscillate in the target neighborhood, making it difficult to form an effective path. Therefore, when When entering the target area, adjust the step size to a smaller value. The selection strategy is expressed by formula (8):

[0067]

[0068] in is the radius of the variable step area of ​​the target point;

[0069] Combining the optimal expansion direction and dynamic step size, the artificial potential field with repulsive force range constraints expands new nodes Calculated according to formula (9);

[0070]

[0071] in for ;

[0072] The pseudo code of the artificial potential field expansion strategy based on repulsive force range constraints is shown in Algorithm 2. The following is a detailed description of each function in the pseudo code:

[0073] (1) Nearest node search function: Get the distance from the random tree Nearest Node .

[0074] (2) TotalForce: Combines the attraction of the target point and random point with the repulsion of the obstacle to calculate and return the resultant force acting on the current node. .

[0075] (3) Dynamic step size function AdaptiveStepSize: Adaptively adjust the expansion step size based on the collision detection results between the new node and the obstacle .

[0076] (4) Collision detection function CollisionFree: for new nodes expanded Performs a collision check and returns 1 if no collision occurs, otherwise returns 0.

[0077] (5) Distance function: Calculates and returns the Euclidean distance between two given points.

[0078]

[0079] Step 3: local optimal processing of virtual target points to guide the random tree to effectively avoid local optimality.

[0080] When a node in the random tree is detected When trapped in a local optimum, construct a node With the initial target point Then select the distance Draw a straight line from the center of the nearest obstacle O According to the spherical optimal path analysis, the node is most likely to escape the local optimal constraint along the OA direction. Therefore, the position along the OA direction that is 1.5 times the radius of the nearest obstacle from point O is set as the virtual target point V (e.g. Figure 1 As shown). At this point, the virtual target point V has a target attraction on the random tree node, guiding the random tree node out of the local optimum. Once the random tree node reaches the virtual target point V, it means that it has successfully escaped the local optimal constraint. Then the virtual target point is removed and the search target is redirected to the initial target. , continue exploring the path.

[0081] The pseudo code for local optimal processing based on the virtual target point is shown in Algorithm 3. The following is a detailed description of each function in the pseudo code:

[0082] (1) Local optimal detection function AtLocalMinimum: judge the current node Whether it falls into local optimum. If it falls into local optimum, it returns 1, otherwise it returns 0.

[0083] (2) NearestObstacle function: Find the node with the closest distance to the local optimal point The nearest obstacle and obtain the coordinates of the obstacle and radius .

[0084] (3) Virtual target point direction calculation function PerpVec: According to the coordinates of the nearest obstacle , falling into a local optimal node and target point Calculate the direction OA of the virtual target point.

[0085]

[0086] Step 4: Redundancy detection and processing: the path is optimized twice using the pruning method to remove redundant nodes.

[0087] Based on the initial planned path, from the starting point Connect the third node and its subsequent nodes in sequence and perform collision detection; when the detection node With the starting point When the line collides with an obstacle, the previous node of the node is selected and Direct connection, delete arrive All nodes between; similarly, from the end point At the beginning, the nodes are detected in turn and pruning is performed. At the same time, based on the first step of pruning, the intermediate nodes are pruned, from the second node to the second to last node, the three adjacent nodes are connected in sequence, and collision detection is performed. If no collision occurs, the intermediate node is deleted (such as Figure 2 As shown), the final optimized path after removing redundant points is → → → .

[0088] Experimental Example 1: Robotic Arm Path Planning Simulation Experiment

[0089] A six-degree-of-freedom robotic arm model was built in the MATLAB environment, and a robotic arm path planning simulation experiment was conducted. The four algorithms used for comparison had the same basic parameters, including a maximum number of iterations of 3000, an initial step size of 10, and a target bias probability of 0.4. The map size was set to 2000×2000×2000, with the starting point coordinates at (-400, 440, 480) and the target coordinates at (400, 400, 520). The coordinates of the starting and target points were mapped to the starting and target poses of the robotic arm's end effector, respectively. The resulting path was then smoothed and optimized using a cubic B-spline curve.

[0090] The robot arm motion planning process is as follows Figure 3 As shown in FIG, the black curve represents the end path of the manipulator planned by the present invention. The manipulator effectively avoids obstacles in an obstacle environment and reaches the target position with a better path. The joint angle change curve of the manipulator motion planning based on the present invention is shown in FIG. Figure 4 As shown in the figure, the curvature of the joint angle of the robotic arm changes smoothly and steadily.

[0091] In order to further quantitatively evaluate the comprehensive performance of each algorithm, Figure 3 The four algorithms were simulated and tested 20 times in the environment, and the simulation test data is shown in Figure 1. The simulation results show that the present invention significantly reduces the number of nodes in the robot arm path planning and achieves a faster planning speed, verifying the efficiency and practicality of the algorithm in the robot arm path planning.

[0092] Table 1 Performance data of the robot arm motion planning algorithm

[0093] Experimental Example 2: Robotic Arm Case Test

[0094] To verify the effectiveness of this invention in a real-world setting, physical testing was conducted on the Shadow Dexterous Hand platform. The experimental platform utilizes a UR10e six-degree-of-freedom collaborative robotic arm and a Shadow Dexterous Hand five-finger dexterous hand as its end effector. The testing environment was built on the ROS-Melodic system running Ubuntu 18.04. To implement the algorithm in practice, the invention was integrated into the Open Motion Planning Library (OMPL) and implemented and executed using MoveIt.

[0095] (1) Test scenario construction

[0096] In order to verify the path planning performance of the present invention, a complex path planning scenario was designed for the experiment. In this scenario, the task of the robot arm is to start from a specified starting point and move accurately to a predetermined position to grab a water cup. Figure 5 As shown in Figure 6(a), the test scene contains two obstacles of varying heights and sizes, simulating the complexity of a real-world working environment and examining the algorithm's adaptability to diverse obstacle conditions. During the test, the robot's starting coordinates were (-707.4, 531.4, -108.6) and its ending coordinates were (-713.1, -598.3, -152.0). Furthermore, the test scene was reconstructed in the RVIZ simulation environment, with gray spheres representing obstacles synchronized with the actual scene, as shown in Figure 6(a).

[0097] (2) Robotic arm path planning

[0098] During the experiment, the present invention generated an optimized path for the six-degree-of-freedom robotic arm. Figure 6(a) shows the planned path for the robotic arm, where the white line represents the trajectory of the robotic arm from its starting point to its end point. In actual operation, the robotic arm moved according to the planned path, successfully avoiding obstacles in the scene. Figures 6(b), (c), and (d) respectively show the key frames of the robotic arm during the movement, demonstrating that the present invention effectively avoids obstacles in complex environments and reaches the target location using a relatively optimal path, thus verifying the practicality of the present invention.

[0099] Using the angle change information of each joint during the robot arm's motion, we plotted the joint angle change diagrams for motion planning using the present invention and the RRT algorithm, as shown in Figure 7. This diagram demonstrates that the present invention achieves relatively smooth angle changes across each joint, resulting in a shorter planning time, further demonstrating the suitability of the present invention for the actual operation and manipulation of the robot arm.

[0100] The above description is merely a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Any simple modification, equivalent change and modification made to the above embodiment based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall still fall within the scope of the technical solution of the present invention.

Claims

1. A robot arm path planning method based on repulsive force range constraint, comprising the following steps: Step 1: Constrained sampling of probability target bias: Set the target bias threshold p and generate random numbers during the sampling process and with the threshold For comparison, if , then directly select the target point as the sampling point to guide the random tree to expand to the target point. On the contrary, if , then continue to use the random sampling method and execute it in the full sampling domain; when performing random sampling, use the constrained sampling strategy to calculate the nearest node respectively. To a random point and to the destination The direction vector of the two direction vectors is defined as the expansion angle , expansion angle Calculate according to formula (3): ； By setting the expansion angle threshold α as a control mechanism, the range of random point generation is constrained. The threshold α is adjusted according to the density of obstacles in the current workspace. After each sampling iteration, the expansion angle is calculated. Is it within the preset threshold α? If it is less than the threshold α, the sampling point is retained; Otherwise, sampling needs to be repeated until the conditions are met; In the sampling space, an improved artificial potential field model is introduced to limit the repulsive force range by dividing the spatial potential field. The improved artificial potential field model includes an attraction function and a repulsion function. The attraction function of the improved artificial potential field method is shown in formulas (4) and (5): ； ； The repulsion function is shown in formula (6): ； Combined Force Expressed by formula (7); ； in and Target points and random points right The attraction, is the target point gravitational field gain constant, is the random point gravitational field gain constant; It is an obstacle The repulsive force, is the gain constant of the obstacle repulsive field, and the resultant force is obtained according to the parallelogram law. direction and determine it as a new node The optimal expansion direction; The random tree starts with an initial step size Explore in the target direction. When expanding a new node, if the new node does not collide with an obstacle and fails to connect directly to the target point, the step size will be adjusted to , is the step expansion factor. Once a collision is detected at the expansion node, the step will be initialized immediately to ,when When entering the target area, adjust the step size to a smaller value and expand the step size The selection strategy is expressed by formula (8): ； in is the radius of the variable step area of ​​the target point; Combining the optimal expansion direction and dynamic step size, the artificial potential field with repulsive force range constraints expands new nodes Calculated according to formula (9); ； in for ; Step 3: Local optimal processing of virtual target points: When a node in the random tree is detected When trapped in a local optimum, construct a node With the initial target point Then select the distance Draw a straight line from the center of the nearest obstacle O The vertical line OA is determined, and the position 1.5 times the radius of the nearest obstacle from point O along the OA direction is set as the virtual target point V. When the random tree node reaches the virtual target point V, the virtual target point is removed and the search target is redirected to the initial target. , continue to explore the path; Step 4: Redundancy detection and processing: Based on the initial planned path, from the starting point Connect the third node and its subsequent nodes in sequence and perform collision detection. With the starting point When the line collides with an obstacle, the previous node of the node is selected and Direct connection, delete arrive All nodes between; similarly, from the end point At the beginning, the nodes are detected in turn and pruning is performed. At the same time, based on the first step of pruning, the intermediate nodes are pruned. From the second node to the second to last node, the three adjacent nodes are connected in sequence and collision detection is performed. If no collision occurs, the intermediate node is deleted. Finally, the optimized path after removing redundant points is obtained. → → → .

2. A method for robot arm path planning based on repulsive force range constraint according to claim 1, wherein: The expansion angle threshold α in step 1 is set to .

Images