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

Abstract

The invention discloses a method for calculating the shortest distance between double robots based on circular slice contour discretization. The double robots are decomposed into a plurality of connected geometries respectively; enveloping each geometric body of the double robots by adopting a revolving body; performing circular slicing on the geometry in the direction of the central axis of the rotary body, and obtaining a set of respective spatial circular slices of the double robots; constructing a circular slice contour parameterization equation related to the posture of the robot in the space; uniformly discretizing the contours of all circular slices on the robot A; calculating the shortest distance from all points in the discrete point set to the contour of the circular slice on the robot B; all the shortest distances between the double robots are obtained, and the minimum value is selected as the shortest distance between the double robots; the precision and efficiency of a distance algorithm can be remarkably improved, and a theoretical basis and a technical guarantee are provided for calculation of the shortest distance between the two robots in a complex or dynamic environment and collaborative obstacle avoidance motion planning optimization under spatial constraints.

Description

Technical Field

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

[0002] With the rapid development of industrial robot technology, multi-robot cooperative operation has been widely applied in the modern industrial field. The shortest distance estimation between two robots is an important basis for performing cooperative 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 the cooperative operation of two robots. Among them, offline planning simulation generally only performs collision or interference inspection, that is, it checks the planned motion path or trajectory. If interference occurs, the trajectory is adjusted until the interference is eliminated. This method cannot determine the cooperative motion path of two robots through one planning. Therefore, designing the spatial constraint conditions of the robots based on the shortest distance calculation result and then realizing the optimization of the motion planning of two robots is the key way to improve the efficiency and reliability of the motion planning.

[0003] In the existing methods for optimizing the motion planning of two robots, most common distance calculation methods are based on the description of the envelope of simple geometric bodies, such as using line segments, spheres, capsules, convex polyhedrons, standard cylinder envelopes, etc. However, due to the lightweight design of robots, complex external structures often appear, and it is difficult to use simple geometric bodies to realize the tight envelope description of the robot structure, and the shortest distance between two spatial convex polyhedrons and between two spatial cylinders is not easy to directly model and calculate. In addition, although the hierarchical bounding box method can realize the tight envelope description of the robot body and achieve 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 profile, aiming to provide an accurate spatial constraint basis for the cooperative 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 profile 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 several 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 any 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, each segment corresponding to a discrete point, is the th segment in the segments.

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

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

[0022]

[0023] Wherein, 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 , wherein, , divide the parameter equally into segments, each segment corresponding 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 a spinor 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.

[0027] Furthermore: The robot pose parameters include the magnitudes of the joint angles of the robot and the lengths of the connecting rods.

[0028] Advantages of the present invention: By performing structural decomposition on 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 flow chart 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 with the same or similar functions throughout. The embodiments described 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 joint angles of each robot joint 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 robot's joint angles are related, that is, as shown in 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 and 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 , the joint angle and 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 , the joint angle , the joint angle and 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 , the joint angle , the joint angle , the joint angle and the joint angle ; 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 , the joint angle , the joint angle , the joint angle , the joint angle and the joint angle . 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] 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 .

[0045] When calculating the shortest distance between each discrete point and the space circle, as shown in Figure 3 , the second circle is located on the plane , its center is , the radius is , and the normal vector is . Let be an arbitrary point in space, be 's projection on the plane . Connect and , then the circle intersects the line segment at the point .

[0046] From Figure 3 , it can be known that the shortest distance from an arbitrary point in space to the circular slice contour 、 with the center, radius and normal vector respectively is . According to geometric relations and the Pythagorean theorem, we can get:

[0047]

[0048] From equation (3), it can be known that the shortest distance from an arbitrary point in space to the circular slice contour can be converted to solving and , where is the distance from the point to the plane P, is the distance from the projection of the point on the plane P to the center minus the 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] Where, 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; the discrete point set of robot B is , where, , divide the parameter equally 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 Figure 4 and Figure 5 , as the slice spacing continuously decreases, the algorithm accuracy in four scenarios gradually improves; as the number of discrete points continuously increases, the algorithm accuracy in four scenarios gradually improves; a representative relative pose relationship between two spatial circles is selected for example analysis; where, 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 four scenarios all exceeds 90%; when the number of discrete points increases to 250, the algorithm accuracy in four scenarios is already 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 requirement, and then calculate according to the algorithm accuracy under different discrete point values. On the premise of ensuring the accuracy requirement, try to reduce the discrete fineness as much as possible, and finally determine the value of the number of discrete points.

[0065] The basic principles, main features and advantages of the present invention have been shown and described above. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification only illustrates 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 all 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. The shortest distance calculation method between two robots based on discretization of circular slice contours is characterized by: The following steps are involved: Step 1: The two robots are respectively recorded as robot A and robot B; Step 2: Decompose the dual robots into several interconnected geometric bodies; Step 3: Envelope each geometric body of the dual robot with a body of revolution; Step 4: Circular slices are made on the geometric body along the central axis of the rotating body, and a set of spatial circular slices of each of the two robots is obtained; Step 5: Construct the parameterized 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 ; ； 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 ; Step 8: Obtain the shortest distances between the two robots according to steps 6 and 7 , and select the minimum value as the shortest distance between the two robots.

2. The method for calculating the shortest distance between two robots based on discretization of circular slice contours according to claim 1 is characterized in that: Parameterized equations of circular slice contour on robot A for: ； in, is the center of the circular slice contour on robot A, is the radius of the circular slice contour on robot A, , In space and A vertical set of vector bases, is the normal vector of the circular slice contour on robot A, is the joint angle of robot A.

3. The method for calculating the shortest distance between two robots based on discretization of circular slice contours according to claim 2 is characterized in that: The discrete point set of robot A is ,in, , the parameters Divide into segments, each segment corresponds to a discrete point, for The first part.

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

5. The method for calculating the shortest distance between two robots based on discretization of circular slice contours according to claim 1 is characterized in that: Parameterized equations of circular slice contour on robot B : ； in, is the center of the circular slice contour on robot B, is the radius of the circular slice contour on robot B, , In space and A vertical set of vector bases, is the normal vector of the circular slice contour on robot B, are the joint angles of robot B.

6. The method for calculating the shortest distance between two robots based on discretization of circular slice contours according to claim 5 is characterized in that: The discrete point set of robot B is ,in, , the parameters Divide into segments, each segment corresponds to a discrete point, for The first part.

7. The method for calculating the shortest distance between two robots based on discretization of circular slice contours according to claim 3 or 6, characterized in that: The number of discrete points in the discrete point set ranges from 20 to 250.

8. The method for calculating the shortest distance between two robots based on discretization of circular slice contours according to claim 1 is characterized in that: The parameters of the circular slice are obtained by calculating the spinor transformation matrix, and the parameters of the circular slice include the three-dimensional coordinates of the center space and the circular plane normal vector.

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

Images