A Double-Arm Collision Detection and Obstacle Avoidance Path Planning Method Based on an Improved Artificial Potential Field Method

Abstract

The present invention belongs to the field of robot obstacle avoidance technology, and specifically relates to a dual-arm collision detection and obstacle avoidance path planning method based on an improved artificial potential field method, including: obtaining a three-dimensional model of a dual robotic arm; initializing the dual robotic arm workspace and environmental obstacle information; respectively calculating the repulsive force distance between each joint sphere of the dual robotic arm, each connecting rod cylinder and the obstacle; defining the robotic arm undergoing path planning as the main arm, and the other as the slave arm, calculating the position of its joint and connecting rod center according to the slave arm joint angle and setting a repulsive force field, and at the same time setting a repulsive force field at the obstacle center point, setting a gravitational field at the target point of the main arm, using the improved artificial potential field method to generate a trajectory and update the current end position of the main arm; judging whether the end of the robotic arm reaches the target point, if not, repeating the execution until the robotic arm reaches the target point. The present invention reduces the amount of calculation in a complex environment, improves planning efficiency and the safety of robot operations.

Description

Technical Field

[0001] The invention belongs to the technical field of robot obstacle avoidance, and in particular relates to a double-arm collision detection and obstacle avoidance path planning method based on an improved artificial potential field method. Background Art

[0002] With the advent of the Industrial 4.0 era, it is a current development trend that robots enter factories to replace human operations. Humanoid robots can perform some repetitive and dangerous work. When humanoid robots use two arms to work, their collision-free path planning is the first problem that needs to be solved.

[0003] The traditional artificial potential field method is to set up a virtual artificial potential field at the end of the robot arm. The target point generates a gravitational field, and the obstacle generates a repulsive field acting on the end of the robot arm. The robot arm avoids obstacles and reaches the target point under the action of the gravitational field and the repulsive field. There are the following problems: when the end of the robot arm is far away from the target point, the gravitational force is too large, local oscillations, and the target point near the obstacle is unreachable. At the same time, the existing collision detection methods generally use AABB bounding boxes, OBB bounding boxes, and spherical bounding boxes. The current AABB bounding boxes and OBB bounding boxes cannot directly calculate the shortest distance from the object to the bounding box, and are only used to determine whether the objects intersect;

[0004] The most common method used in dual-arm collaboration is master-slave planning, that is, the master arm considers environmental obstacles for obstacle avoidance planning. After obtaining the collision-free path of the master arm, the slave arm takes the movement path of the master arm into consideration for obstacle avoidance planning. However, there is the following problem: when the master arm falls into local oscillation, the slave arm reaches the target point faster. At this time, the master arm's planning cannot avoid obstacles for the slave arm, and a collision occurs. Summary of the invention

[0005] Purpose of the invention: The purpose of the present invention is to address the deficiencies in the prior art and provide a dual-arm collision detection and obstacle avoidance path planning method based on an improved artificial potential field method, which is easy to deploy and has high solving efficiency. The motion trajectory of a humanoid robot dual-arm system is generated by a path generation method based on the artificial potential field method, which reduces the amount of calculation in a complex environment and improves the planning efficiency and the safety of the robot's operation.

[0006] Technical solution: The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method of the present invention comprises the following steps:

[0007] S1: Obtain the three-dimensional model of the dual robotic arms and establish the forward and inverse kinematics model according to their joint types;

[0008] S2: Initialize the dual-manipulator workspace, and give the dual-manipulator base pose transformation matrix, initial point, and target point;

[0009] S3: Initialize environmental obstacle information, use cuboids and spheres to envelop obstacles, use spheres to envelop the robot joints, and use cylinders to envelop the robot links according to the three-dimensional model and kinematic model of the dual robot arms;

[0010] S4: Calculate the repulsive force distance between each joint sphere and each connecting rod cylinder of the dual robotic arms and the obstacle respectively. When one robotic arm is calculating, the joint and connecting rod envelope of the other robotic arm at this time is regarded as an obstacle;

[0011] S5: Define the robot arm that is currently planning the path as the master arm, and the other as the slave arm. Calculate the position of the joint and the center of the connecting rod according to the joint angle of the slave arm and set a repulsive field. At the same time, set up a repulsive field at the center of the obstacle, and set up a gravitational field at the target point of the master arm. Use the improved artificial potential field method to generate the trajectory and update the current end position of the master arm, including:

[0012] S51, calculating the gravity at the end of the main arm;

[0013] S52, calculating the repulsion force from the end of the arm and the repulsion force of the obstacle;

[0014] S53, calculating the resultant force at the end of the main arm;

[0015] S54, updating the end position of the main arm;

[0016] S55, determine whether it is trapped in a local minimum, if so, adopt an escape strategy; if not, execute step S6;

[0017] S6: Determine whether the end of the robot arm has reached the target point. If not, repeat steps S4 and S5 until the robot arm reaches the target point.

[0018] To further improve the above technical solution, in S1, dimensional data is obtained based on the three-dimensional model of the robotic arm, the DH model of the robotic arm is established and the forward and inverse kinematics are solved, the base coordinate system of the left arm is set as the world coordinate system, and the representation of the object in the world coordinate system in the base coordinate system of the right arm is obtained through the posture transformation matrix.

[0019] Furthermore, the calculation of the repulsive force distance between each joint sphere of the dual robotic arm, each connecting rod cylinder and the obstacle in S4 includes:

[0020] The environmental obstacles and the joint sphere and connecting rod cylinder of another robot arm are used as obstacles of the current robot arm;

[0021] For each joint sphere of the current robot arm, calculate the repulsive distance from it to the environmental obstacles, the joint sphere of the other robot arm, and the connecting rod cylinder; for each connecting rod cylinder of the current robot arm, calculate the repulsive distance from it to the environmental obstacles, the joint sphere of the other robot arm, and the connecting rod cylinder;

[0022] The repulsion distance calculation for the joint sphere includes: using the shortest distance from a point to a cuboid to calculate the repulsion distance between the joint sphere and the obstacle cuboid, using the shortest distance from point to point to calculate the repulsion distance between the joint sphere and the obstacle sphere, using the shortest distance from point to point to calculate the repulsion distance between the joint sphere and another robotic arm joint sphere, and using the shortest distance from a point to a cylinder to calculate the repulsion distance between the joint sphere and another robotic arm connecting rod cylinder;

[0023] The repulsion distance calculation for the connecting rod cylinder includes: using the shortest distance from the cylinder to the cuboid to calculate the repulsion distance between the connecting rod cylinder and the obstacle cuboid, using the shortest distance from the point to the cylinder to calculate the repulsion distance between the connecting rod cylinder and the obstacle sphere, using the shortest distance between the point and the cylinder to calculate the repulsion distance between the connecting rod cylinder and the joint sphere of another robotic arm, and using the shortest distance between cylinders to calculate the repulsion distance between the connecting rod cylinder and the connecting rod cylinder of another robotic arm.

[0024] Furthermore, the shortest distance between the point and the cuboid is obtained by converting the point into a cuboid coordinate system and calculating the distance components in three directions; the shortest distance between the point and the point is calculated using the minimum distance in the normal direction between two spheres; the shortest distance between the point and the cylinder is obtained by comprehensively calculating the horizontal plane distance and the vertical direction distance; the distance between the cylinders is obtained by calculating the straight-line distance between the center points of each cylinder;

[0025] The distance between the point and the cuboid is calculated as follows: the center of the cuboid is taken as the center of the repulsive field, and the pose matrix of the center point of the cuboid in the world coordinate system is assumed to be , length, width and height are , a point in space is , then the distance calculation process from the point to the center of the cuboid is as follows:

[0026] The point is expressed in the cuboid coordinate system as , the length from the point to the center of the cuboid is , the repulsive distance from the point to the cuboid is:

[0027] ;

[0028] The center of the cylinder is taken as the center of the repulsive field, and the distance between the point pointing to the center of the repulsive field and the obstacle is taken as the repulsive distance from the point to the cylinder. Assume that the pose matrix of the center point of the cylinder in the world coordinate system is , the radius is , Gao Wei , a point in space , then the calculation method of the repulsive distance from the point to the cylinder is:

[0029] The representation of this point in the cylindrical coordinate system is: , the length from the point to the center of the cylinder is , the repulsive distance from this point to the cylinder is:

[0030] .

[0031] Furthermore, in step S51:

[0032] Calculate the gravitational distance based on the initial point and the target point , ,

[0033] In the formula, is the initial point, is the target point, l represents the left arm, and r represents the right arm;

[0034] Establish a gravitational field, assuming is the gravitational coefficient, In order to set the variable force range of the gravitational field, the improved gravitational potential energy function is expressed as:

[0035] ;

[0036] According to the potential energy derivative, the gravitational function is obtained and the gravitational force is calculated as:

[0037] .

[0038] Furthermore, in step S52, the repulsive force is calculated according to the repulsive distance calculated in step S4, and a repulsive field is established at the center point of the obstacle. The improved repulsive potential energy function is:

[0039]

[0040] Where: is the repulsion coefficient, Represents the distance from the center repulsion point of each obstacle, Represent the distances between the repulsive points on the surfaces of each obstacle, Refers to the maximum distance on the surface of each obstacle; Point out differences;

[0041] According to the potential energy derivative, the repulsive function is obtained and the gravitational force is calculated as:

[0042] .

[0043] The repulsive force at each joint is: ,in, is the number of obstacles, including environmental obstacles and another robotic arm.

[0044] Furthermore, the method for calculating the resultant force at the end of the main arm in step S53 is:

[0045] ,in, The number of main arm joints.

[0046] Furthermore, in step S54, the method for updating the position of the main arm end is:

[0047]

[0048] in, is the updated position, is the position at the previous moment, is the step length.

[0049] Furthermore, in step S55, the disengagement strategy includes:

[0050] Move one step toward the target point and perform collision detection;

[0051] If no collision occurs, the point is used to update the end position of the robot arm;

[0052] If a collision occurs, a random direction is sampled in space.

[0053] Beneficial effect: Compared with the prior art, the advantages of the present invention are: the present invention adopts several commonly used geometric bodies to envelop obstacles and robotic arms, refines the modeling of joint spheres and connecting rod cylinders and the calculation of obstacle envelopes, is easy to deploy, and improves computing efficiency; takes into account the influence of obstacles on the joints and connecting rods of the robotic arm, and improves the quality of the collision-free path of the robotic arm; in the path planning process, regards the other robotic arm as a dynamic obstacle, and dynamically adjusts the path of the main arm by updating the position of the other robotic arm in real time to avoid collision, taking into account the mutual collision between the two arms instead of adopting the master-slave planning method, effectively avoiding the situation where the master robotic arm planning lags behind the slave robotic arm and cannot consider the slave robotic arm; the improved gravitational and repulsive potential energy functions, as well as the local optimal escape strategy, effectively overcome the limitations of the traditional artificial potential field method in complex scenes and improve the adaptability of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a complete algorithm flow chart of the present invention;

[0055] Figure 2 1 is a schematic diagram of a dual-arm planning scenario in Example 2 of the present invention, wherein: 1 is the left arm, 2 is the right arm, 3 is a rectangular obstacle, and 4 is a spherical obstacle;

[0056] Figure 3 is a configuration diagram of the mechanical arm in Example 2 of the present invention;

[0057] Figure 4 5 is a schematic diagram of the envelope of obstacles and the dual-arm joint links in Example 2 of the present invention, 5 is the envelope of the left arm collision geometry, and 6 is the envelope of the right arm collision geometry;

[0058] Figure 5 2 is a schematic diagram of calculating the distance between a point and a rectangular parallelepiped repulsive force in Embodiment 2 of the present invention;

[0059] Figure 6 1 is a schematic diagram of calculating the distance from a point to a cylinder for repulsive force in Embodiment 2 of the present invention;

[0060] Figure 7 is a schematic diagram of the repulsive distance between the cylinder and the cuboid in Example 2 of the present invention;

[0061] Figure 8 Schematic diagram of the repulsive distance between cylinders in Example 2 of the present invention;

[0062] Fig. 9 This is a schematic diagram of the forces acting on the dual mechanical arms in Embodiment 2 of the present invention;

[0063] Fig.10 This is a comparative analysis of the distance between the end of the dual robotic arm and the target point in Example 2 of the present invention. The left sub-figure is the traditional artificial potential field method, and the right sub-figure is the improved method of the present invention. DETAILED DESCRIPTION

[0064] The technical solution of the present invention is described in detail below with reference to the accompanying drawings, but the protection scope of the present invention is not limited to the embodiments.

[0065] Example 1: Figure 1 The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method includes the following steps:

[0066] S1: Obtain the three-dimensional model of the dual robotic arms and establish the forward and inverse kinematics model according to their joint types;

[0067] S2: Initialize the dual-manipulator workspace, and give the dual-manipulator base pose transformation matrix, initial point, and target point;

[0068] S3: Initialize environmental obstacle information, use cuboids and spheres to envelop obstacles, and use spheres to envelop the robot arm joints and cylinders to envelop the robot arm links based on the three-dimensional model and kinematic model for obstacle distance calculation and collision detection;

[0069] S4: Calculate the repulsive distance between each joint sphere, each connecting rod cylinder and the obstacle. When each robot arm is calculating, the joint and connecting rod envelope of the other robot arm at this time is regarded as an obstacle and calculated together;

[0070] S5: Initialize the trajectory using the improved artificial potential field method. Each robot arm is called the master arm during planning, and the other robot arm is called the slave arm. The positions of the joints and connecting rod centers of the slave arms are calculated according to the joint angles of the slave arms, and a repulsive field is set. At the same time, a repulsive field is set at the center of the obstacle, and a gravitational field is set at the target point of the master arm. When the improved artificial potential field method starts to work, the trajectory is generated and the end position of the current master arm is updated;

[0071] S6: Determine whether the end of the robot arm has reached the target point. If not, repeat steps S4 and S5 until the robot arm reaches the target point.

[0072] Example 2: Based on the solution provided in the example, in S1, the dimension data is obtained according to the three-dimensional model of the robot arm, as shown in Table 1, the DH model of the robot arm is established and the forward and inverse kinematics are solved, and the base coordinate system of the left arm is set as the world coordinate system. The representation of the object in the world coordinate system in the base coordinate system of the right arm can be obtained through the posture transformation matrix of the two arms. Taking the left arm as an example, the base of the robot arm is fixed, so the repulsive field is established from the second joint and the first connecting rod of the left arm. The position of the joints , left arm The connecting rod center is .

[0073] surface The DH parameter table used in this example

[0074]

[0075] Figure 2 It is a schematic diagram of the application scenario of the dual-arm robot of the present invention, wherein: 1 is the left arm, 2 is the right arm, 3 is a rectangular obstacle, and 4 is a spherical obstacle. Figure 3 This is the configuration diagram of the robotic arm, from the base to the joint or end effector (x6, y6, z6).

[0076] Use a spherical model to envelop the joints of the robot arm, define the coordinates of the sphere center and the radius of the sphere to represent the size and position of the joints; use a cylindrical model to envelop the connecting rods of the robot arm, including the starting point, end point, radius, center point and height, such as Figure 4 As shown, 5 is the geometric envelope of the left arm, and 6 is the geometric envelope of the right arm.

[0077] In S4, the repulsion distance calculation process for a single robotic arm: all parts of the other robotic arm (joint sphere and connecting rod cylinder) are regarded as obstacles, and the calculation is divided into two categories: the repulsion distance of the joint and the repulsion distance of the connecting rod.

[0078] The repulsive distance calculation for the joint sphere includes: calculating the distance between it and the environmental obstacles: if the obstacle is a cuboid, use the point-to-cuboid distance calculation; if the obstacle is a sphere, use the point-to-point distance calculation; calculating the distance between it and the joint of another robotic arm: using the point-to-point distance calculation; calculating the distance between it and the connecting rod of another robotic arm: using the point-to-cylinder distance calculation.

[0079] The repulsive distance calculation of the connecting rod cylinder includes: calculating its distance from environmental obstacles: for rectangular obstacles, use the cylinder-to-rectangular distance calculation; for spherical obstacles, use the cylinder-to-point distance calculation; calculating its distance from another robotic arm joint: use the cylinder-to-point distance calculation; calculating its distance from another robotic arm connecting rod: use the cylinder-to-cylinder distance calculation.

[0080] Calculate the repulsive distance between the robot arm and the obstacle, involving the shortest distance from the point to the cuboid, such as Figure 5 As shown, the shortest distance from a point to a cylinder is Figure 6 As shown, the repulsive distance from the cylinder to the cuboid is Figure 7 As shown, the cylinder-to-cylinder repulsion distance is calculated as follows: Figure 8 As shown. The present invention uses the center of the cuboid and the center of the cylinder as the center of the repulsive field, and uses the distance between the point pointing to the center of the repulsive field and the obstacle as the repulsive distance. The repulsive distance calculation method of the cylinder is the same. Specific distance calculation method: the distance from a point to a cuboid is calculated by converting the point to the cuboid coordinate system and calculating the distance components in three directions; the distance from a point to a cylinder is a comprehensive calculation of the horizontal distance and the vertical distance; the distance from a cylinder to a cylinder is the calculation of the straight-line distance between the center points of the two cylinders.

[0081] Point to cuboid repulsion distance: Assume that the pose matrix of the cuboid center point in the world coordinate system is , length, width and height are , a point in space is , calculate the representation of the point in the cuboid coordinate system: , the length from the point to the center of the cuboid is , then the repulsive distance from the point to the cuboid is the distance from the point to the center of the cuboid. The calculation process of the distance from the point to the center of the cuboid is as follows:

[0082]

[0083] The repulsive distance from the point to the cylinder, assuming that the pose matrix of the center point of the cylinder in the world coordinate system is , the radius is , Gao Wei , a point in space , calculate the representation of the point in the cylindrical coordinate system: , the length from the point to the center of the cylinder is , then the calculation process of the repulsive distance from the point to the cylinder is as follows:

[0084] .

[0085] S51: Calculate the gravity at the end of the robot arm. First, calculate the gravity distance based on the initial point and the target point. , ; In the formula, is the initial point, is the target point, l represents the left arm, and r represents the right arm.

[0086] Use the following formula to calculate gravity and establish the gravitational field, assuming is the gravitational coefficient, To set the variable force range of the gravitational field, the improved gravitational potential energy function is expressed as

[0087]

[0088] According to the derivative of potential energy, the gravitational function can be expressed as:

[0089]

[0090] The gravitational field is divided into constant force field and variable force field, as shown in the formula, The constant force field can solve the obstacle avoidance problem and repulsion calculation between fixed objects; The variable force field can solve the distance conversion between dynamic objects. The two are collectively called , by judging The size of the gravitational field is used to calculate the true value of the gravitational field in real time, avoiding the problem in traditional algorithms where the distance between the initial point and the target point is too large, resulting in excessive gravitational force.

[0091] S52: Calculate the repulsive force according to the repulsive distance calculated in step S4, establish a repulsive field at the center of the obstacle, and the repulsive potential energy is affected by the distance between the end of the robot arm and the target point, which can effectively solve the problem that the target point is unreachable near the obstacle. The improved repulsive potential energy function is:

[0092]

[0093] Where: is the repulsion coefficient, Represents the distance from the center repulsion point of each obstacle, Represent the distances between the repulsive points on the surfaces of each obstacle, Refers to the maximum distance on the surface of each obstacle; Point out the difference.

[0094] Taking the derivative, we can get the repulsion function:

[0095]

[0096] S53, the repulsive force of each joint is ,in, is the number of all obstacles (including environmental obstacles and the joints and links of the other robot arm).

[0097] S54. Calculate the force at the end of the robot arm ,in, is the number of joints of the robot arm, as shown in the attached Fig. 9 As shown, the various force vectors are superimposed.

[0098] S55, update the end position of the robot arm ,in, is the movement step length, is the end position of the robot arm at the current moment.

[0099] S56: Determine whether the robot falls into a local minimum based on the change in potential energy and the distance between the end of the robot arm and the target point. If it falls into a local minimum, the robot arm moves one step toward the target point and performs collision detection. If the robot arm does not collide, the point is used to update the end position of the robot arm, otherwise it samples in a random direction in space.

[0100] It is determined whether the end of the robot arm has reached the target point. If not, S4 and steps S51 to S56 are repeatedly executed until the robot arm reaches the target point.

[0101] like Fig.10 As shown, the left sub-figure is the traditional artificial potential field method, and the right sub-figure is the improved method of the present invention. The distance between the dual robotic arms and the target point is compared as a parameter. It can be seen that the present invention significantly improves the quality of the collision-free path of the robotic arms.

[0102] As described above, although the present invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the present invention itself. Various changes in form and details may be made without departing from the spirit and scope of the present invention as defined in the appended claims.

Claims

1. A dual-arm collision detection and obstacle avoidance path planning method based on an improved artificial potential field method, characterized in that: The following steps are involved: S1: Obtain the three-dimensional model of the dual robotic arms and establish the forward and inverse kinematics model according to their joint types; S2: Initialize the dual-manipulator workspace, and give the dual-manipulator base pose transformation matrix, initial point, and target point; S3: Initialize environmental obstacle information, use cuboids and spheres to envelop obstacles, use spheres to envelop the robot joints, and use cylinders to envelop the robot links according to the three-dimensional model and kinematic model of the dual robot arms; S4: Calculate the repulsive force distance between each joint sphere and each connecting rod cylinder of the dual robotic arms and the obstacle respectively. When one robotic arm is calculating, the joint and connecting rod envelope of the other robotic arm at this time is regarded as an obstacle; S5: Define the robot arm that is currently planning the path as the master arm, and the other as the slave arm. Calculate the position of the joint and the center of the connecting rod according to the joint angle of the slave arm and set a repulsive field. At the same time, set up a repulsive field at the center of the obstacle, and set up a gravitational field at the target point of the master arm. Use the improved artificial potential field method to generate the trajectory and update the current end position of the master arm, including: S51, calculating the gravity at the end of the main arm; S52, calculating the repulsion force from the end of the arm and the repulsion force of the obstacle; S53, calculating the resultant force at the end of the main arm; S54, updating the end position of the main arm; S55, determine whether it is trapped in a local minimum, if so, adopt an escape strategy; if not, execute step S6; S6: Determine whether the end of the robot arm has reached the target point. If not, repeat steps S4 and S5 until the robot arm reaches the target point.

2. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 1, characterized in that: In S1, dimensional data is obtained based on the three-dimensional model of the robotic arm, the DH model of the robotic arm is established and the forward and inverse kinematics are solved, the base coordinate system of the left arm is set as the world coordinate system, and the representation of the object in the world coordinate system in the base coordinate system of the right arm is obtained through the posture transformation matrix.

3. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 1, characterized in that: The calculation of the repulsive force distance between each joint sphere of the dual robot arm, each connecting rod cylinder and the environmental obstacle in S4 includes: The environmental obstacles and the joint sphere and connecting rod cylinder of another robot arm are used as obstacles of the current robot arm; For each joint sphere of the current robot arm, calculate the repulsive distance from it to the environmental obstacles, the joint sphere of the other robot arm, and the connecting rod cylinder; for each connecting rod cylinder of the current robot arm, calculate the repulsive distance from it to the environmental obstacles, the joint sphere of the other robot arm, and the connecting rod cylinder; The repulsion distance calculation for the joint sphere includes: using the shortest distance from a point to a cuboid to calculate the repulsion distance between the joint sphere and the obstacle cuboid, using the shortest distance from point to point to calculate the repulsion distance between the joint sphere and the obstacle sphere, using the shortest distance from point to point to calculate the repulsion distance between the joint sphere and another robotic arm joint sphere, and using the shortest distance from a point to a cylinder to calculate the repulsion distance between the joint sphere and another robotic arm connecting rod cylinder; The repulsion distance calculation for the connecting rod cylinder includes: using the shortest distance from the cylinder to the cuboid to calculate the repulsion distance between the connecting rod cylinder and the obstacle cuboid, using the shortest distance from the point to the cylinder to calculate the repulsion distance between the connecting rod cylinder and the obstacle sphere, using the shortest distance between the point and the cylinder to calculate the repulsion distance between the connecting rod cylinder and the joint sphere of another robotic arm, and using the shortest distance between cylinders to calculate the repulsion distance between the connecting rod cylinder and the connecting rod cylinder of another robotic arm.

4. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 3 is characterized in that: The shortest distance between a point and a cuboid is obtained by converting the point to a cuboid coordinate system and calculating the distance components in three directions; the shortest distance between points is calculated using the minimum distance in the normal direction between two spheres; the shortest distance between a point and a cylinder is obtained by comprehensively calculating the horizontal distance and the vertical distance; the distance between cylinders is obtained by calculating the straight-line distance between the center points of each cylinder; The distance between the point and the cuboid is calculated as follows: the center of the cuboid is taken as the center of the repulsive field, and the pose matrix of the center point of the cuboid in the world coordinate system is assumed to be , length, width and height are , a point in space is , then the distance calculation process from the point to the center of the cuboid is as follows: The point is expressed in the cuboid coordinate system as , the length from the point to the center of the cuboid is , the calculation formula of the repulsive distance from a point to a cuboid is: ； In the formula: abs is the absolute value of the integer; The center of the cylinder is taken as the center of the repulsive field, and the distance between the point pointing to the center of the repulsive field and the obstacle is taken as the repulsive distance from the point to the cylinder. Assume that the pose matrix of the center point of the cylinder in the world coordinate system is , the radius is , Gao Wei , a point in space , then the calculation method of the repulsive distance from the point to the cylinder is: The representation of this point in the cylindrical coordinate system is: , the length from the point to the center of the cylinder is , the repulsive distance from this point to the cylinder is: 。 5. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 4, characterized in that: In step S51: Calculate the gravitational distance based on the initial point and the target point , , In the formula, is the initial point, is the target point, l represents the left arm, and r represents the right arm; Establish a gravitational field, assuming is the gravitational coefficient, In order to set the variable force range of the gravitational field, the improved gravitational potential energy function is expressed as: ； According to the potential energy derivative, the gravitational function is obtained and the gravitational force is calculated as: 。 6. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 5, characterized in that: In step S52, the repulsive force is calculated according to the repulsive distance calculated in step S4, and a repulsive field is established at the center point of the obstacle. The improved repulsive potential energy function is: ； Where: is the repulsion coefficient, Represents the distance from the center repulsion point of each obstacle, Represent the distances between the repulsive points on the surfaces of each obstacle, Refers to the maximum distance on the surface of each obstacle; Point out differences; According to the potential energy derivative, the repulsive function is obtained and the gravitational force is calculated as: ； The repulsive force at each joint is: ,in, is the number of obstacles, including environmental obstacles and the other robotic arm.

7. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 6, characterized in that: The method for calculating the combined force at the end of the main arm in step S53 is: ,in, The number of main arm joints.

8. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 7, characterized in that: In step S54, the method for updating the position of the main arm end is: ； in, is the updated position, is the position at the previous moment, is the step length.

9. The double-arm collision detection and obstacle avoidance path planning method based on the improved artificial potential field method according to claim 8, characterized in that: In step S55, the disengagement strategy includes: Move one step toward the target point and perform collision detection; If no collision occurs, the point is used to update the end position of the robot arm; If a collision occurs, a random direction is sampled in space.

Images