RRT algorithm spraying mechanical arm path planning method and system based on dynamic step length

Abstract

The invention discloses a method and a system for path planning of a spraying mechanical arm based on an RRT algorithm of dynamic step size, the method for planning the path through the RRT algorithm is optimized, sampling points are discriminated in advance through the optimized RRT algorithm, and generation of redundant path points on a random tree and unnecessary collision detection are avoided; meanwhile, a target point gravitational component is added for guiding, the problem that a traditional method is poor in guiding performance is solved, the extended step length can be dynamically adjusted when approaching an obstacle, the step length can be recovered when being far away from the obstacle, the extended search capacity of the algorithm is improved, and meanwhile the environmental adaptability of the algorithm can be improved to a certain degree.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent equipment, and in particular to a method and system for path planning of a spraying robot arm based on an RRT algorithm with dynamic step size optimization. Background Art

[0002] With the continuous advancement of automation and robotics technology, spraying robots are widely used in industrial production, especially in the fields of automobile manufacturing and home appliance production. At present, in the field of construction, spraying robots have also begun to be used on a large scale in the spraying of fire retardant coatings on steel beams and the spraying of wall putty. Spraying robots need to complete spraying tasks efficiently and accurately in complex production environments, and path planning is one of the key technologies to ensure spraying quality and efficiency.

[0003] Traditional methods use artificial potential field method, RRT algorithm, etc. to plan the path of the spray robot. The artificial potential field method uses virtual force to realize path planning. The target point generates gravity and the obstacle generates repulsion. Under the combined force of the two, the robot is finally directed to the target point. This method has a fast execution speed and excellent performance in the robot workspace with no obstacles or few obstacles.

[0004] However, in the artificial potential field method, the gravitational and repulsive forces on the robot in a complex obstacle workspace may cancel each other out, resulting in a net force of zero. The robot falls into a local minimum and cannot determine which direction to move.

[0005] The RRT algorithm adopts a global random sampling strategy to randomly sample in space to generate a search tree. The search tree grows evenly in all directions and has probabilistic completeness, which can effectively solve planning problems in high-dimensional space.

[0006] However, the traditional RRT path planning algorithm adopts a global random sampling strategy with probabilistic completeness. However, because there is no clear target orientation, its guidance is poor. At the same time, the traditional RRT path planning algorithm usually relies on a fixed step size and cannot adapt to environmental changes, resulting in slow path planning speed and even collisions or errors.

[0007] Therefore, the existing path planning methods have the technical problem of low reliability due to the above-mentioned technical problems. It can be seen that how to improve the reliability of the path planning of the spray robot arm is a problem that needs to be solved in this field. Summary of the invention

[0008] In view of the technical problem that the existing path planning methods have low reliability due to the above-mentioned technical problems, the purpose of the present invention is to provide a method for spray robot arm path planning based on the RRT algorithm with dynamic step size, which effectively solves the technical problems existing in the above-mentioned path planning methods by optimizing the RRT algorithm. On this basis, a system for spray robot arm path planning capable of implementing the method is further provided.

[0009] In order to achieve the above object, the present invention provides a method for spraying robot arm path planning based on RRT algorithm with dynamic step size, and the spraying robot arm path planning method comprises the following steps:

[0010] Step 1: Establish the kinematic model of the spraying robot arm;

[0011] Step 2: Initialize the environment information and parameter setting information required to be input in the random number;

[0012] Step 3: Determine the node sampling method based on the size relationship between the probability threshold and the random probability, and update the node after each collision detection of the random probability;

[0013] Step 4: Based on the nodes updated in step 3, the new nodes are expanded along the direction of the resultant force under the combined action of the dynamic step size and the gravitational components of the target point and the sampling point;

[0014] Step 5: Perform collision detection between the end of the robot arm and each connecting rod and obstacles on the extended new node obtained in step 4.

[0015] Furthermore, the kinematic model of the robot arm is obtained by describing the posture of the robot arm and establishing the robot arm link coordinate system.

[0016] Furthermore, the environmental information in step 2 includes specific position information of the starting point and target point of the spraying robot arm, obstacle space information, and joint angles of the spraying robot arm corresponding to the starting point and target point.

[0017] Furthermore, the parameter setting information in step 2 includes a given maximum number of iterations, a target point error tolerance threshold, a first probability threshold, a second probability threshold, and a gravity coefficient.

[0018] Furthermore, in step 3, when the number of cycles is greater than the maximum number of iterations, and the random probability is ≥ the probability threshold, random sampling of sampling points is not performed; if the random probability is < the probability threshold, sampling points are randomly sampled within the motion range of each joint angle of the robotic arm.

[0019] Furthermore, when it is a sampling point randomly sampled in step 3, check whether the end of the robot arm and each connecting rod collide with an obstacle. If the end of the robot arm and each connecting rod collide with an obstacle, the point is discarded and a point closest to the sampling point is randomly searched in the workspace again as a parent node.

[0020] Furthermore, whether the end of the robot arm collides with the obstacle is determined based on whether the end of the robot arm falls into the obstacle space; and whether the connecting rod collides with the obstacle is determined based on the collision detection model.

[0021] Furthermore, the method for performing collision detection between the end of the robot arm and each connecting rod and the obstacle on the new node in step 5 is the same as that in step 3.

[0022] Furthermore, if no collision occurs, it is determined whether the end position at this time enters the target point error allowable threshold. If so, the path is backtracked, each node is connected, and the path planning is completed. If not, the joint angle of the new node is added to the random tree, and the cycle is started again in step 1.

[0023] If a collision occurs, adjust the gravity coefficient, reduce the step size in the gravity direction, and return to step 3 to start the loop.

[0024] In order to achieve the above object, the present invention provides a system for spraying robot arm path planning based on RRT algorithm with dynamic step size, and the spraying robot arm path planning system comprises:

[0025] A spraying robot arm dynamics model building module, wherein the spraying robot arm dynamics model building module obtains a kinematic model of the spraying robot arm by describing the robot arm posture and establishing a robot arm connecting rod coordinate system;

[0026] A random tree input parameter initialization module, which is used to reset the environment information and parameter settings for each path planning;

[0027] A node sampling and updating module, which determines the node sampling method according to the size relationship between the probability threshold and the random probability, and updates the node after each collision detection of the random probability;

[0028] A new node expansion module, which interacts with the node sampling and updating module data, uses the nodes in the node sampling and updating module as a basis, and generates new nodes by expanding along the direction of the resultant force under the joint action of the gravitational component of the target point and the gravitational component of the sampling point;

[0029] The collision detection module interacts with the new node expansion module data, performs collision detection again based on the new node generated by the new node expansion module through the method in the node sampling and updating module, and adjusts the gravity coefficient according to the collision detection result.

[0030] The method and system for spray robot path planning based on the dynamic step-size RRT algorithm provided by the present invention optimize the RRT algorithm. The optimized RRT algorithm distinguishes the sampling points in advance, avoiding the generation of redundant path points on the random tree and unnecessary collision detection; at the same time, the gravitational component of the target point is added for guidance, solving the problem of poor guidance of the traditional method, and the extended step size is dynamically adjusted (becomes smaller) when approaching an obstacle, and the step size is restored when away from the obstacle, which speeds up the extended search capability of the algorithm and improves the environmental adaptability of the algorithm to a certain extent. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments.

[0032] Figure 1 This is a flow chart of the method for path planning of the spray robot arm based on the dynamic step size RRT algorithm;

[0033] Figure 2 This is a schematic diagram of the robot arm link collision detection model in the method of spray robot arm path planning based on the dynamic step size RRT algorithm;

[0034] Figure 3 This is the new node expansion diagram under the action of dynamic step size and gravitational component in the method of spray robot path planning based on RRT algorithm with dynamic step size. DETAILED DESCRIPTION

[0035] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further explained below with reference to specific diagrams.

[0036] The existing RRT algorithm has poor guidance when performing path planning, usually relies on a fixed step size, cannot adapt to environmental changes, resulting in slow path planning speed, and even technical problems such as collisions or errors. The present invention optimizes the RRT algorithm. The optimized RRT algorithm distinguishes the sampling points in advance, avoiding the generation of redundant path points on the random tree and unnecessary collision detection; at the same time, the target point gravity component is added for guidance, solving the problem of poor guidance of the traditional method, and when approaching an obstacle, the extended step size will be dynamically adjusted (become smaller), and when away from the obstacle, the step size will be restored, which speeds up the algorithm's extended search capability and improves the algorithm's environmental adaptability to a certain extent.

[0037] Depend on Figure 1It can be seen that this solution can be based on the path planning of the RRT algorithm through the following steps:

[0038] S1: Establishment of the kinematic model of the spraying robot

[0039] S11: Establish kinematic model of spraying robot arm

[0040] This step describes the robot arm posture and establishes the robot arm link coordinate system to obtain the DH model of the robot arm to improve the accuracy of the spray robot arm.

[0041] S2: Initialization of random tree input parameters

[0042] Since the information of the input parameters is different each time the path planning is performed, it needs to be initialized and reset each time the path planning is performed.

[0043] S21: First, some information needs to be acquired, including the specific position coordinate information of the starting point Ps and the target point Pg of the spray robot arm, the obstacle space information, and the joint angles of the spray robot arm corresponding to the starting point and the target point.

[0044] The location information of the starting point, target point, and obstacle space information needs to be acquired through visual devices, such as depth cameras, and then passed to the algorithm as the initial conditions of the path planning algorithm.

[0045] The obstacle spatial information refers to the obstacle volume information and center point coordinate information obtained by the maximum envelope method.

[0046] Here, the joint angles of the spraying robot arm corresponding to the starting point and the target point are obtained by solving the spatial coordinate information of the above-mentioned starting point and the target point through the inverse kinematics of the robot arm to obtain the joint angles θs and θg of the spraying robot arm corresponding to the starting point and the target point. For the n-dimensional robot arm, θs = (θs1, θs1, …θsn), θg = (θg1, θg1, …θgn).

[0047] S22: This step sets some parameters, which specifically include: a given maximum number of iterations max, an allowable error threshold thr of the target point Pg, probability thresholds α (α=0 at the beginning, α=1 when a collision occurs) and β (β=1 at the beginning, β=0 when a collision occurs), and a gravitational coefficient g (g=1 at the beginning, g=0.1+0.5*rand when a collision occurs).

[0048] Here, the error tolerance threshold thr refers to the maximum error range allowed for the target point position.

[0049] The probability threshold is a pre-designed value used to determine whether the path planning is successful. That is, when the distance between the actual path and the target point is less than or equal to this threshold, the path planning is considered successful.

[0050] The probability threshold α is a threshold used to control whether a random sampling point is selected as a target point. For example, when the random probability p generated in the algorithm is less than α, it means that the threshold is not reached, that is, the random sampling point is not selected as the target point.

[0051] The probability threshold β is a threshold used to control whether the step length calculation method is an adaptive step length calculation method or an average step length calculation method. For example, when the random probability q generated in the algorithm is less than β, it means that this threshold is not reached, that is, the step length calculation method uses an adaptive step length calculation method.

[0052] The setting of probability thresholds α and β is based on environmental considerations. For example, when the number of obstacles in the environment is small, α and β can be set larger.

[0053] The gravity coefficient g is used to control the step length component affected by the target point. When the gravity coefficient g decreases, the step length component affected by the target point will become smaller. Using this gravity coefficient, the step length can be automatically reduced for path search when a collision occurs, reducing the possibility of a collision.

[0054] rand is a function in the algorithm, which can be used to generate a number between 0 and 1.

[0055] S3: Node sampling and node update

[0056] The sampling method is determined according to the size relationship between the probability threshold α and the random probability p, and is updated after each collision detection of the random probability p.

[0057] The random probability value p is a random number between 0 and 1 automatically generated by the rand() function in the algorithm.

[0058] S31: Determine whether the number of loops is greater than the maximum number of iterations max. If it is less than max, continue to determine the program, otherwise the program ends;

[0059] S32: If p ≥ α, no random sampling is performed, and the sampling point θrand is set equal to the target point θg. If p < α, random sampling is performed within the range of motion of each joint angle of the robot arm to obtain the sampling point θrand.

[0060] The robot arm joint angle motion range here is the inherent parameter at the factory.

[0061] S33: Use forward kinematics to determine whether the end of the robotic arm and each connecting rod collides with an obstacle when the sampling point is θrand.

[0062] Specifically, whether the end of the robot arm collides is determined based on whether the end of the robot arm falls into the obstacle space Cobs; and Figure 2 The collision detection model determines whether the connecting rods collide. If so, the point is discarded and random sampling is performed again in the workspace;

[0063] Figure 2 The robot arm link collision detection simplifies the links and obstacles by using the space envelope method, which simplifies the computational power of collision calculation to a certain extent.

[0064] Assume that the connecting rod cylinder surrounds the end face with centers O and O1 and radius r1, and the obstacle sphere surrounds the center O2 and radius r2.

[0065] When the sphere rolls purely along the side and end face of the connecting rod, the rolling trajectory of the sphere center O2 is shown in the line segment abcd, where the straight line segment ab represents the rolling trajectory of the sphere center O2 when the obstacle is between the two end faces of the connecting rod and is tangent to the connecting rod, as shown in position 1; in the arc bc, point b is the sphere center O2 rolling to be directly below the center O, and point c is the sphere center O2 rolling to be level with the center O. When the sphere rolls purely along the end face of the connecting rod, the rolling trajectory of the sphere center O2 is shown in position 2.

[0066] When the obstacle is between the two end faces of the connecting rod, ∠OO1O2 and ∠O1OO2 are both less than or equal to 90°. At this time, the distance d between the center of the circle O and the center of the sphere O2 can be calculated to determine whether the connecting rod collides with the obstacle. The calculation formula for the shortest non-collision distance l is as follows:

[0067]

[0068] The formula for calculating the shortest non-collision distance l here is to combine the given known structural parameters such as radius size and angle value through Figure 2 The formula is used to calculate the distance from the center of the circle O to the bcd line segment at different angles. This distance is the shortest non-collision distance l at different angles.

[0069] S34: If θrand is outside the obstacle space Cobs, find a point in the random tree T that is closest to the sampling point θrand as the parent node θnear;

[0070]

[0071] Where θi represents a set of joint angles of each node on the random tree T.

[0072] Step 4: Expand the new node by combining the dynamic step size and gravity component.

[0073] like Figure 3As shown in the figure, the joint angle θnew of the new node is based on the parent node θnear, and is generated by the joint action of the gravitational component of the target point θg and the gravitational component of the sampling point θrand along the direction of the resultant force. The resultant force is determined by the two gravitational components through the parallelogram law. The specific steps are as follows:

[0074] S41: Use the target point θg and the sampling point θrand to expand the parent node θnear. The expansion formula is as follows:

[0075]

[0076] Where s represents the fixed step size of the algorithm, g represents the gravitational coefficient, and ||θ rand -θ near || represents the joint distance between the sampling point θrand and the parent node θnear, ||θ g -θ near || represents the joint distance between the target point θg and the parent node θnear.

[0077] The above formula 3 can be used to control the size of the new node θnew. This formula fully considers the influence of the sampling point θrand, the parent node θnear, the target point θg, etc. on the result. At the same time, the gravity coefficient is introduced to control the size of the corresponding component, so that when encountering obstacles, the algorithm can better adaptively modify the step size.

[0078] S42: The weighted Manhattan distance is introduced to measure the distance between two groups of joints in the n-dimensional joint space. This formula can be used to better measure the distance between two groups of joints in the n-dimensional joint space, and the n degrees of freedom of the joint angle can be controlled. The expression is:

[0079] ||θ1-θ2||=λ1(θ 11 -θ 21 )+λ2(θ 12 -θ 22 )+···+λ n (θ 1n -θ 2n ) (4)

[0080] θ1-θ2=[λ1(θ 11 -θ 21 ),λ2(θ 12 -θ 22 ),···,λ n (θ 1n -θ 2n )] (5)

[0081] Among them, λ1, λ2…λn are calculation weights,

[0082] S43: λi is randomly selected from the average weight and the adaptive weight. The following expression can be used, which can better select the weight:

[0083]

[0084] Where q represents the random probability and β represents the adaptive growth threshold.

[0085] S44: Substitute formula (4)-(6) into formula (3) to calculate the joint angle θnew of the new node;

[0086] S5: Collision Detection

[0087] S51: When using forward kinematics to determine the joint angle θnew of the new node, whether the end of the robot arm and each connecting rod collide with the obstacle; whether the end of the robot arm collides can be determined by whether it falls into the obstacle space Cobs, and whether the connecting rod collides can be determined by Figure 2 The collision detection model is used for judgment.

[0088] S52: If no collision occurs, determine whether the end position at this time enters the target point error allowable threshold thr. If so, backtrack the path, connect each node, and complete the path planning. If not, add the joint angle θnew of the new node to the random tree, and return to S13 to start the loop;

[0089] Specifically, as an example, if the position of the target point obtained at the beginning is Pg(x,y,z) and the end position at this time is (x1,y1,z1), solve the distance If L≤the target point error tolerance threshold thr, it is determined that the end position enters the target point error tolerance threshold.

[0090] S53: If a collision occurs, the gravitational coefficient g is adjusted, the step length in the gravitational direction is reduced, and the process returns to S31 to start the cycle.

[0091] The method for spraying robot path planning based on the dynamic step length RRT algorithm composed of the above scheme first uses inverse kinematics to obtain the joint angles θs and θg of the spraying robot corresponding to the starting point and the target point; obtains the sampling point position θrand by random sampling or taking the target point as the sampling point, and judges whether the joints and connecting rods collide when the robot is at the sampling point according to the collision detection formula (1); in the case of no collision, the point closest to the sampling point is obtained according to formula (2) as the parent node θnear; the above information is brought into formula (3) to obtain a new node, wherein the three parameters involved in the control step length in formula (3) need to be solved using formulas (4)-(6). Through the above steps, the cycle is continuously repeated until a path that meets the error requirements is found.

[0092] The method for spraying robot arm path planning based on the RRT algorithm of dynamic step length given in this example scheme can constitute a corresponding software program and form a corresponding spraying robot arm path planning system when it is applied. When the software program is running, it will execute the above-mentioned method for spraying robot arm path planning and store it in the corresponding storage medium for the processor to call and execute.

[0093] The system of spray robot path planning based on dynamic step-size RRT algorithm mainly includes: spray robot dynamic model establishment module, random tree input parameter initialization module, node sampling and updating module, new node expansion module and collision detection module.

[0094] Among them, the spraying robot arm dynamics model establishment module in this system obtains the kinematic model of the spraying robot arm by describing the robot arm posture and establishing the robot arm link coordinate system.

[0095] The spraying robot arm dynamics model building module is configured to execute the above-mentioned spraying robot arm dynamics model building steps to realize corresponding functions.

[0096] The random tree input parameter initialization module in this system is used to reset the environmental information and parameter settings for each path planning.

[0097] The random tree input parameter initialization module is configured to execute the above steps of random tree input parameter initialization to realize corresponding functions.

[0098] The node sampling and updating module in this system determines the node sampling method according to the size relationship between the probability threshold and the random probability, and updates the node after each collision detection of the random probability.

[0099] The node sampling and updating module is configured to execute the above node sampling and updating steps to realize corresponding functions.

[0100] The new node expansion module in this system interacts with the node sampling and updating module data, uses the nodes in the node sampling and updating module as the basis, and expands along the direction of the resultant force to generate new nodes under the joint action of the gravitational component of the target point and the gravitational component of the sampling point.

[0101] The new node expansion module is configured to execute the above new node expansion steps to realize corresponding functions.

[0102] The collision detection module in this system interacts with the new node extension module data. Based on the new nodes generated by the new node extension module, the method in the module is determined by sampling to perform collision detection again and adjust the gravity coefficient according to the collision detection results.

[0103] The collision detection module is configured to execute the above-mentioned collision detection steps to realize corresponding functions.

[0104] The above-mentioned method of the present invention, or a specific system unit, or a part of the same, is a pure software architecture, which can be arranged on a physical medium, such as a hard disk, an optical disk, or any electronic device (such as a smart phone, a computer-readable storage medium) through program code. When the machine loads the program code and executes it (such as a smart phone loading and executing it), the machine becomes a device for implementing the present invention. The above-mentioned method and device of the present invention can also be transmitted in the form of program code through some transmission media, such as cables, optical fibers, or any transmission mode. When the program code is received, loaded and executed by a machine (such as a smart phone), the machine becomes a device for implementing the present invention.

[0105] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A method for spraying robot path planning based on the RRT algorithm with dynamic step length, characterized in that: The spraying robot arm path planning method comprises the following steps: Step 1: Establish the kinematic model of the spraying robot arm; Step 2: Initialize the environment information and parameter setting information required to be input in the random number; Step 3: Determine the node sampling method based on the size relationship between the probability threshold and the random probability, and update the node after each collision detection of the random probability; Step 4: Based on the nodes updated in step 3, the new nodes are expanded along the direction of the resultant force under the combined action of the dynamic step size and the gravitational components of the target point and the sampling point; Step 5: Perform collision detection between the end of the robot arm and each connecting rod and obstacles on the extended new node obtained in step 4.

2. The method for spraying robot path planning based on the RRT algorithm of dynamic step size according to claim 1 is characterized in that: By describing the position and posture of the robot and establishing the robot link coordinate system, the kinematic model of the robot is obtained.

3. The method for spraying robot path planning based on the RRT algorithm of dynamic step size according to claim 1 is characterized in that: The environmental information in step 2 includes the specific position information of the starting point and the target point of the spraying robot arm, the obstacle space information, and the joint angles of the spraying robot arm corresponding to the starting point and the target point.

4. The method for spraying robot path planning based on the RRT algorithm of dynamic step size according to claim 1 is characterized in that: The parameter setting information in step 2 includes a given maximum number of iterations, a target point error tolerance threshold, a first probability threshold, a second probability threshold, and a gravity coefficient.

5. The method for spraying robot path planning based on RRT algorithm with dynamic step size according to claim 1 is characterized in that: In step 3, when the number of cycles is greater than the maximum number of iterations, and the random probability is ≥ the probability threshold, random sampling of sampling points is not performed. If the random probability is < the probability threshold, random sampling of sampling points is performed within the range of motion of each joint angle of the robot arm.

6. The method for spraying robot path planning based on RRT algorithm with dynamic step size according to claim 1 is characterized in that: When it is the sampling point randomly sampled in step 3, check whether the end of the robot arm and each connecting rod collide with the obstacle. If the end of the robot arm and each connecting rod collide with the obstacle, the point is discarded and the point closest to the sampling point is randomly searched in the workspace as the parent node.

7. The method for spraying robot path planning based on the dynamic step length RRT algorithm according to claim 6 is characterized in that: Whether the end of the robotic arm collides with the obstacle is determined based on whether the end of the robotic arm falls into the obstacle space; whether the connecting rod collides with the obstacle is determined based on the collision detection model.

8. The method for spraying robot path planning based on RRT algorithm with dynamic step size according to claim 1, characterized in that: The method for performing collision detection between the end of the robot arm and each connecting rod and the obstacle on the new node in step 5 is the same as that in step 3.

9. The method for spraying robot path planning based on RRT algorithm with dynamic step size according to claim 8, characterized in that: If no collision occurs, determine whether the end position at this time enters the target point error allowable threshold. If so, backtrack the path, connect each node, and complete the path planning. If not, add the joint angle of the new node to the random tree and return to step 1 to start the loop. If a collision occurs, adjust the gravity coefficient, reduce the step size in the gravity direction, and return to step 3 to start the loop.

10. A system for spraying robot path planning based on the RRT algorithm with dynamic step length, characterized in that: The spraying robot arm path planning system comprises: A spraying robot arm dynamics model building module, wherein the spraying robot arm dynamics model building module obtains a kinematic model of the spraying robot arm by describing the robot arm posture and establishing a robot arm connecting rod coordinate system; A random tree input parameter initialization module, which is used to reset the environment information and parameter settings for each path planning; A node sampling and updating module, wherein the node sampling and updating module determines the node sampling method according to the size relationship between the probability threshold and the random probability, and updates the node after each collision detection of the random probability; A new node expansion module, which interacts with the node sampling and updating module data, uses the nodes in the node sampling and updating module as a basis, and generates new nodes by expanding along the direction of the resultant force under the joint action of the gravitational component of the target point and the gravitational component of the sampling point; The collision detection module interacts with the new node expansion module data, performs collision detection again based on the new node generated by the new node expansion module through the method in the node sampling and updating module, and adjusts the gravity coefficient according to the collision detection result.

Images