Method for calculating shortest distance between two robots based on discretization of circular slice contour

Abstract

Method for calculating the shortest distance between two robots based on the discretization of circular slice contours. The two robots are respectively decomposed into a number of interconnected geometric bodies; the geometric bodies of the two robots are enveloped by a solid of revolution; circular slices are made of the geometric bodies along the central axis direction of the solid of revolution, and a set of spatial circular slices of the two robots are obtained respectively; a parametric equation of the circular slice contour related to the robot posture in space is constructed; the contours of all circular slices on robot A are uniformly discretized; the shortest distances from all points in the discrete point set to the circular slice contours on robot B are calculated; all the shortest distances between the two robots are obtained, and the minimum value among them is selected as the shortest distance between the two robots. It can significantly improve the accuracy and efficiency of the distance algorithm, and provide a theoretical basis and technical guarantee for the calculation of the shortest distance between two robots in a complex or dynamic environment and the optimization of cooperative obstacle avoidance motion planning under spatial constraints.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-robot collaborative motion control, and particularly to a method for calculating the shortest distance between two robots based on the discretization of a circular slice contour. Background Art

[0002] With the rapid development of industrial robot technology, multi-robot collaborative operation is widely used in the modern industrial field. The shortest distance estimation between two robots is an important basis for performing collaborative motion control and ensuring the safe operation of robots. In existing research, teaching planning and offline planning simulation are mostly used to ensure the safety of multi-robot collaborative operation. Among them, offline planning simulation generally only performs collision or interference checking, that is, checking the planned motion path or trajectory. If interference occurs, the trajectory is adjusted until the interference is eliminated. This method cannot determine the collaborative motion path of two robots through one-time planning. Therefore, designing the spatial constraint conditions of the robots based on the shortest distance calculation result and then realizing the optimization of the two-robot motion planning is the key way to improve the efficiency and reliability of motion planning.

[0003] In the existing methods for optimizing the motion planning of two robots, most common distance calculation methods are based on the envelope description of simple geometric bodies, such as using line segments, spheres, capsules, convex polyhedra, standard cylinders for envelope, etc. However, due to the lightweight design of robots, complex outer shapes often appear, and it is difficult to use simple geometric bodies to achieve a tight envelope description of the robot structure, and it is not easy to directly model and calculate the shortest distance between two spatial convex polyhedra or two spatial cylinders. In addition, although the hierarchical bounding box method can achieve a tight envelope description of the robot body and realize reliable collision detection, its efficiency in calculating the shortest distance between two robots is low and it is difficult to meet the requirements of real-time motion planning of robots. Summary of the Invention

[0004] Aiming at the problems in the prior art, the present invention provides a method for calculating the shortest distance between two robots based on the discretization of a circular slice contour, aiming to provide an accurate spatial constraint basis for the collaborative obstacle avoidance motion control of two robots.

[0005] The method for calculating the shortest distance between two robots based on the discretization of a circular slice contour includes the following steps:

[0006] Step 1: Denote the two robots as robot A and robot B respectively;

[0007] Step 2: Decompose the two robots into a number of interconnected geometric bodies respectively;

[0008] Step 3: Envelope each geometric body of the two robots with a surface of revolution;

[0009] Step 4: Perform circular slicing on the geometric body along the central axis direction of the rotating body, and obtain the set of spatial circular slices of each of the two robots;

[0010] Step 5: Construct a parametric equation for the circular slice contour related to the robot posture in space;

[0011] Step 6: Uniformly discretize the contours of all circular slices on robot A, and obtain a discrete point set;

[0012] Step 7: Calculate the shortest distance from all points in the discrete point set to the circular slice contour on robot B ;

[0013]

[0014] Among them, denote the plane where the circular slice contour on robot B is located as plane P, is the center of the circular slice contour on robot B, is the radius of the circular slice contour on robot B, is the normal vector of the circular slice contour on robot B, is an arbitrary point in the discrete point set, is the intersection point of the center of the circular slice contour on robot B and the line segment ; is 's projection point on the plane ;

[0015] Step 8: Obtain all the shortest distances between the two robots according to Steps 6 and 7 , and select the minimum value among them as the shortest distance between the two robots.

[0016] Furthermore, the parametric equation of the circular slice contour on robot A is:

[0017]

[0018] Among them, is the center of the circular slice contour on robot A, is the radius of the circular slice contour on robot A, , are respectively a set of vector bases perpendicular to in space, is the normal vector of the circular slice contour on robot A, is the joint angle of robot A.

[0019] Furthermore, the discrete point set of robot A is , among which, , substitute the parameter equally divided into segments, and each segment corresponds to a discrete point, is the th segment in the segments.

[0020] Furthermore: the rotating body is a cylinder, a rotating body with a variable cross-sectional diameter, or a rotating body with a curved central axis.

[0021] Furthermore: the parametric equation of the circular slice contour on robot B :

[0022]

[0023] Among them, is the center of the circular slice contour on robot B, is the radius of the circular slice contour on robot B, , are respectively a set of vector bases perpendicular to in space, is the normal vector of the circular slice contour on robot B, is the joint angle of robot B.

[0024] Furthermore: the discrete point set of robot B is , among which, , divide the parameter equally into segments, and each segment corresponds to a discrete point, is the th segment in the segments.

[0025] Furthermore: the number range of discrete points in the discrete point set is 20 - 250.

[0026] Furthermore: the parameters of the circular slice are calculated through the screw transformation matrix, and the parameters of the circular slice include the three-dimensional space coordinates of the center of the circle and the normal vector of the circular plane.

[0027] Furthermore: the robot attitude parameters include the magnitudes of the joint angles of the robot and the lengths of each link.

[0028] Advantages of the present invention: By decomposing the structure of each robot and describing it with circular slice envelopes, each robot is described as a set of circular slices. Then, combined with the single-circle discretization representation strategy, the shortest distance between two robots is converted into the problem of the shortest distance from a spatial point to a spatial circular contour, which can significantly improve the accuracy and efficiency of the distance algorithm, and provide a theoretical basis and technical guarantee for the calculation of the shortest distance between two robots in a complex or dynamic environment and the optimization of cooperative obstacle avoidance motion planning under spatial constraints. Description of the Drawings

[0029] Figure 1 is the flowchart of the present invention;

[0030] Figure 2 is the schematic diagram of the robot structure based on the circular slice envelope description;

[0031] Figure 3 is the schematic diagram of the distance from a spatial point to the circular slice contour;

[0032] Figure 4 is the trend chart of the algorithm accuracy varying with the number of discrete points under four scenarios;

[0033] Figure 5 Trend chart of the algorithm time consumption varying with the number of discrete points under four scenarios. Detailed implementation manners

[0034] The following will describe the present invention in detail with reference to the accompanying drawings. The embodiments of the present invention will be described in detail below. The examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions from beginning to end. The embodiments described below by referring to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention. The orientation terms such as left, middle, right, up, and down in the embodiments of the present invention are only relative concepts to each other or are referenced based on the normal use state of the product, and should not be considered as restrictive.

[0035] The method for calculating the shortest distance between two robots based on the discretization of the circular slice contour, as Figure 1 shown, includes the following steps:

[0036] Step 1: Denote the two robots as robot A and robot B respectively;

[0037] Step 2: Decompose the connecting rods of the two robots into several interconnected geometric bodies;

[0038] Step 3: As shown in Figure 2 , envelope each geometric body of the two robots with a surface of revolution, and the surface of revolution is a cylinder, a surface of revolution with a variable cross-sectional diameter, or a surface of revolution with a curved central axis;

[0039] Step 4: Perform circular slicing on the geometric bodies along the central axis direction of the surface of revolution, and obtain the sets of spatial circular slices of the two robots respectively;

[0040] Step 5: Construct a parametric equation for the circular slice contour related to the robot's pose in space. The robot's pose parameters include the magnitudes of the angles of each joint of the robot and the lengths of each link. The parameters of the circular slice are calculated through a screw transformation matrix. During the movement of the robot, except for some structures, the parameters of the spatial circular slices that envelope the robot are generally related to the joint angles of the robot are related, that is, as shown by Figure 2 a joint angle is formed between the first link 1 and the second link 2 and the second link 2 is related to the joint angle a joint angle is formed between the second link 2 and the third link 3 and the third link 3 is related to the joint angle a joint angle is formed between the third link 3 and the fourth link 4 and the fourth link 4 is related to the joint angle a joint angle is formed between the fourth link 4 and the fifth link and the fifth link 5 is related to the joint angle a joint angle is formed between the fifth link 5 and the sixth link 6 and the sixth link 6 is related to the joint angle a joint angle is formed between the sixth link 6 and the seventh link 7 a joint angle is formed between the sixth link 6 and the seventh link 7 a joint angle is formed between the sixth link 6 and the seventh link 7 and the seventh link 7 is related to the joint angle a joint angle is formed between the sixth link 6 and the seventh link 7 a joint angle is formed between the sixth link 6 and the seventh link 7 a joint angle is formed between the sixth link 6 and the seventh link 7. The parameters of the circular slice include the three-dimensional coordinates of the center of the circle in space and the normal vector of the circular plane

[0041] Step 6: To reduce the computational difficulty and operation time, uniformly discretize the contours of all circular slices on robot A and obtain a discrete point set. Through the discretization and uniform sampling of a single circle, the problem of the shortest distance between two spatial circles is further simplified to the shortest distance from a discrete point to a circular contour

[0042] Step 7: As shown in Figure 3 calculate the shortest distance from all points in the discrete point set to the circular slice contour on robot B ;

[0043]

[0044] The plane where the circular slice contour on robot B is located is denoted as plane P. is the center of the circular slice contour on robot B, is the radius of the circular slice contour on robot B, is the normal vector of the circular slice contour on robot B, For any point in the discrete point set, The center and line segment of the circular slice contour on robot B The intersection of for In plane The projection point on

[0045] When calculating the shortest distance between each discrete point and the space circle, such as Figure 3 As shown, the second circle is located in the plane The center of the circle is , the radius is , the normal vector is .make For any point in space, for In plane Projection on, connection and , then the circle and the line segment Intersect at point .

[0046] Depend on Figure 3 It can be seen that any point in space To , The circle slice outlines with the center, radius and normal vector respectively The shortest distance is , from the geometric relationship and the Pythagorean theorem we can get:

[0047]

[0048] From equation (3), we can see that any point in space To the circular slice outline The shortest distance Can be converted to solve and ,in, For point The distance to plane P, For point Projection on plane P to the center of the circle Distance and radius The difference is calculated as follows:

[0049]

[0050]

[0051]

[0052] Among them, is the distance from point to the center of the circle . respectively represent and the cosine value and sine value of the included angle. Given and , the cosine value of the included angle between the two vectors can be calculated as:

[0053]

[0054] From the trigonometric identity it can be obtained that:

[0055]

[0056] Finally, substituting equations (4) to (8) into equation (3), equation (9) can be obtained;

[0057] Step 8: Obtain all the shortest distances between the two robots according to Steps 6 and 7 , and select the minimum value among them as the shortest distance between the two robots.

[0058] Among them, the parametric equation of the circular slice contour on robot A is:

[0059]

[0060] Among them, is the center of the circular slice contour on robot A, is the radius of the circular slice contour on robot A, , are respectively a set of vector bases perpendicular to in space, is the normal vector of the circular slice contour on robot A, is the joint angle of robot A; the discrete point set of robot A is , among which, , divide the parameter equally into segments, each segment corresponds to a discrete point, is the Segment

[0061] Parametric equation of the circular slice contour on robot B :

[0062]

[0063] Wherein, is the center of the circular slice contour on robot B, is the radius of the circular slice contour on robot B, and are respectively a set of vector bases perpendicular to in space, is the normal vector of the circular slice contour on robot B, is the joint angle of robot B; the discrete point set of robot B is , wherein, , the parameter is equally divided into segments, each segment corresponds to a discrete point, is the th segment in the th segment.

[0064] In addition, the number range of discrete points in the discrete point set is 20 - 250. As shown in combination with Figure 4 and Figure 5 , as the slice spacing continues to decrease, the algorithm accuracy in the four scenarios gradually improves; as the number of discrete points continues to increase, the algorithm accuracy in the four scenarios gradually improves; a representative relative pose relationship between two spatial circles is selected for example analysis; wherein, Figure 4 shows the relationship between the algorithm accuracy and the number of discrete points, where the abscissa represents the number of discrete points and the ordinate represents the algorithm accuracy. When the number of discrete points reaches 20, the algorithm accuracy in the four scenarios exceeds 90%; when the number of discrete points increases to 250, the algorithm accuracy in the four scenarios is close to the true value, showing high accuracy; Figure 5 shows the relationship between the algorithm time consumption and the number of discrete points. The analysis results show that as the number of discrete points increases, the running time of the algorithm also increases, and the two are in a proportional relationship. Therefore, in practical applications, first consider the algorithm accuracy requirements, and then calculate according to the algorithm accuracy under different discrete point values. On the premise of ensuring the accuracy requirements, try to reduce the discrete fineness as much as possible, and finally determine the value of the number of discrete points.

[0065] The foregoing has shown and described the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments, and what is described in the above embodiments and the specification is only to illustrate the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements fall within the scope of the present invention claimed. The scope of the present invention claimed is defined by the appended claims and their equivalents.

Claims

1. A method for calculating the shortest distance between two robots based on the discretization of a circular slice contour, characterized in that, Including the following steps: Step 1: Denote the dual robots as robot A and robot B respectively; Step 2: Decompose the dual robots into several interconnected geometric bodies respectively; Step 3: Envelop each geometric body of the dual robots with a surface of revolution; Step 4: Perform circular slicing on the geometric bodies along the central axis direction of the surface of revolution, and obtain the sets of spatial circular slices of the dual robots respectively; Step 5: Construct a parametric equation of the circular slice contour related to the robot posture in space; Step 6: Uniformly discretize the contours of all circular slices on robot A, and obtain a discrete point set; Step 7: Calculate the shortest distance from all points in the discrete point set to the circular slice contour on robot B ; ； Among them, the plane where the circular slice contour on robot B is located is denoted as plane P. is the center of the circular slice contour on robot B. is the radius of the circular slice contour on robot B. is the normal vector of the circular slice contour on robot B. is an arbitrary point in the discrete point set. is the intersection point of the center of the circular slice contour on robot B and the line segment . is 's projection point on the plane . Step 8: Obtain all the shortest distances between the two robots according to Step 6 and Step 7 , and select the minimum value among them as the shortest distance between the two robots.

2. The method for calculating the shortest distance between two robots based on the discretization of the circular slice profile according to claim 1, wherein: Parametric equation of the circular slice contour on robot A It is as follows: ； Among them, is the center of the circular slice contour on robot A, is the radius of the circular slice contour on robot A, , are respectively a set of vector bases perpendicular to in space, is the normal vector of the circular slice contour on robot A, is the joint angle of robot A.

3. The shortest distance calculation method between two robots based on the discretization of the circular slice contour according to claim 2, characterized in that: The discrete point set of robot A is , where . The parameter is equally divided into segments, and each segment corresponds to a discrete point. is the th segment among these segments.

4. The shortest distance calculation method between two robots based on the discretization of the circular slice profile according to claim 1, characterized in that: The surface of revolution is a cylinder, a surface of revolution with variable cross-sectional diameter, or a surface of revolution with a curved central axis.

5. The method for calculating the shortest distance between two robots based on the discretization of the circular slice profile according to claim 1, wherein: Parametric equation of the circular slice contour on robot B : ； Among them, is the center of the circular slice contour on robot B, is the radius of the circular slice contour on robot B, , are respectively a set of vector bases perpendicular to in space, is the normal vector of the circular slice contour on robot B, is the joint angle of robot B.

6. The method for calculating the shortest distance between two robots based on the discretization of the circular slice profile according to claim 5, wherein: The discrete point set of robot B is , where , the parameter is equally divided into segments, and each segment corresponds to a discrete point is the th segment among these segments 7. The method for calculating the shortest distance between two robots based on the discretization of the circular slice profile according to claim 3 or 6, characterized in that: The number range of discrete points in the discrete point set is 20 - 250.

8. The method for calculating the shortest distance between two robots based on the discretization of the circular slice contour according to claim 1, characterized in that: The parameters of the circular slice are calculated through a screw transformation matrix, and the parameters of the circular slice include the three-dimensional spatial coordinates of the center of the circle and the normal vector of the circular plane.

9. The method for calculating the shortest distance between two robots based on the discretization of the circular slice profile according to claim 1, characterized in that: The robot posture parameters include the magnitudes of the joint angles of the robot and the lengths of each connecting rod.

Images