Robot-based safe collision interference detection method, device and equipment and storage medium

Abstract

The invention discloses a robot safety collision interference detection method, device and equipment and a storage medium, and relates to the field of robots. M groups of candidate cylinder collision pairs are determined and screened from a mechanical arm and a tool end of the robot according to a kinematics equation; selecting a group of candidate cylinder collision pairs, calculating a first minimum distance from two circular surfaces formed by the to-be-measured cylinder collision boxes to the reference cylinder collision box, and measuring a second minimum distance from the side surfaces of the to-be-measured cylinder collision boxes to the reference cylinder collision box; determining the interference state of the candidate cylinder collision pair according to the round surface radius of the reference cylinder collision box, the first minimum distance and the second minimum distance; polling M groups of candidate cylinder collision pairs to complete mechanical arm interference detection. According to the scheme, the interference calculation of the mechanical arm is carried out by adopting the cylindrical collision box, the collision pair is selected in advance, the interference calculation method of the cylindrical collision box is optimized, the accuracy of interference detection is ensured, and the requirement of calculation real-time performance is met.

Description

Technical Field

[0001] Embodiments of the present application relate to the field of robots, and in particular to a robot safety collision interference detection method, device, equipment and storage medium. Background Art

[0002] Robot interference detection is to calculate the spatial position between the joint links of the robot or between the robot and external obstacles in real time during the operation of the robot to determine whether a collision will occur. Interference detection is a test operation that must be performed during the robot design verification stage.

[0003] In the related art, for example, CN111546376A adopts a method of combining a spherical collision box and a capsule collision box, adopts a capsule envelope arm, and uses the spherical center and the capsule axis to calculate the distance, thereby making interference judgments. Another example is CN112171671A adopts a cube collision box model, obtains the spatial position of the collision box model according to the forward kinematics of the robot arm, and uses the collision box model and the external discrete catenary model to make interference judgments.

[0004] Both of the above methods use the method of constructing a collision box of the robot model to judge the interference between the robot body or the outside world, but both methods have certain defects. Although the first method uses a combination of a spherical collision box and a capsule collision box to replace the robot model for interference calculation, it does not pre-process the collision between the collision boxes. When calculating the interference, the interference between all collision boxes is calculated, which may increase the amount of calculation and affect the real-time performance. The second method uses a cube collision box to calculate the interference between the external discrete catenary model, but does not perform interference calculations between the robot body collision boxes. Summary of the invention

[0005] The embodiments of the present application provide a robot safety collision interference detection method, device, equipment and storage medium to solve the problems of inaccurate interference processing measurement and complex calculation in related technologies.

[0006] In one aspect, the present application provides a robot safety collision interference detection method, the method comprising:

[0007] The robot's mechanical arm and tool end are divided into several cylindrical collision boxes according to their sizes, the spatial positions of the collision boxes under different motion states are determined according to the kinematic equations, and M groups of candidate cylindrical collision pairs are screened;

[0008] Select a group of candidate cylinder collision pairs according to the combination, calculate the first minimum distance between the two circular surfaces of the cylinder collision box to be tested and the reference cylinder collision box, and measure the second minimum distance between the side surface of the cylinder collision box to be tested and the reference cylinder collision box;

[0009] The interference state of the candidate cylinder collision pair is determined according to the circular surface radius value of the reference cylinder collision box, the first minimum distance and the second minimum distance; M groups of candidate cylinder collision pairs are polled, and when at least one group of candidate cylinder collision pairs has interference, it is determined that the robot arm has collision interference.

[0010] Specifically, after selecting the cylindrical collision pair, the method further includes:

[0011] A coordinate system is established, and the axis of the reference cylindrical collision box is parallel to the Z-axis direction of the coordinate system by using rotation transformation, and the axis of the cylindrical collision box to be measured is perpendicular to the Y-axis direction of the coordinate system.

[0012] Specifically, the rotation transformation process includes:

[0013] Based on the established coordinate system, the axis vectors L1 and L2 of the two cylinder collision boxes are calculated as follows:

[0014] L1=P12-P11

[0015] L2=P22-P21

[0016] Wherein P11 and P12 respectively represent the center coordinates of the two circular surfaces of the reference cylindrical collision box, and P21 and P22 respectively represent the center coordinates of the two circular surfaces of the cylindrical collision box to be tested;

[0017] Calculate the transformation matrix Rx for rotation around the X-axis, so that L1′=Rx*L1, L2′=Rx*L2, and L1′ is perpendicular to the Y-axis; calculate the transformation matrix Ry for rotation around the Y-axis, so that L1″=Ry*L1′, L2″=Ry*L2′, and L1″ is in the same direction as the Z-axis; calculate the transformation matrix Rz for rotation around the Z-axis, so that L2″′=Rz*L2″, and L2″′ is perpendicular to the Y-axis;

[0018] When a center coordinate point of the reference cylindrical collision box is used as the origin of the coordinate system, the center coordinates of each circular surface of the cylindrical collision box after coordinate transformation are calculated; the expression is as follows:

[0019] P12=Ry*Rx*(P12-P11)

[0020] P21=Rz*Ry*Rx*(P21-P11)

[0021] P22=Rz*Ry*Rx*(P22-P11)

[0022] Among them, P11 = {0, 0, 0} is the coordinate origin.

[0023] Specifically, the calculation of the first minimum distance between the two circular surfaces formed by the to-be-tested cylindrical collision box and the reference cylindrical collision box includes:

[0024] Project the two circular surfaces of the cylindrical collision box to be tested onto a horizontal plane to determine the positional relationship of the coordinate origin relative to the projected image; wherein, when the cylindrical collision box to be tested is parallel to the reference cylindrical collision box, the projected image is a standard circle; when the cylindrical collision box to be tested is perpendicular to the reference cylindrical collision box, the projected image is a line segment; when the cylindrical collision box to be tested is in an intersecting state with the reference cylindrical collision box, the projected image is an ellipse;

[0025] When the coordinate origin is located on any projection image contour line or inside the contour line, determine the shortest distance dmin′ from the coordinate origin to the projection image = 0; when the coordinate origin is not located on any projection image contour line or inside the contour line, calculate the shortest distance dmin′ from the coordinate origin to the projection image;

[0026] Determine the relative position relationship between the two circular surfaces of the cylindrical collision box to be tested and the reference plane where the two reference circular surfaces of the reference cylindrical collision box are located;

[0027] When there is a circular surface to be measured that cuts at least one reference plane, it indicates that part of the circular surface to be measured is located above or below the reference cylindrical collision box, remove the projection contour line corresponding to the part located above or below the reference cylindrical collision box in the projection image, calculate the shortest distance dmin″ from the coordinate origin to the remaining projection contour line, and determine it as the first minimum distance dmin1;

[0028] When the two circular surfaces to be measured do not intersect the two reference planes, the shortest distance dmin′ from the coordinate origin to the projection image is determined as the first minimum distance dmin1.

[0029] Specifically, before calculating the first minimum distance, the method further includes:

[0030] The Z-axis values ​​of the coordinates of the highest and lowest points of the two circular surfaces corresponding to the collision box of the cylinder to be tested are calculated respectively, as shown below:

[0031] Zmax=Pz+r2*sinβ

[0032] Zmin=Pz-r2*sinβ

[0033] Among them, Zmax is the Z-axis value of the highest point of the circular surface, Zmin is the Z-axis value of the lowest point of the circular surface; Pz is the Z-axis value of the center of the circle on the circular surface, r2 is the circular radius of the cylindrical collision box to be tested, and β is the angle v between the axis of the cylindrical collision box to be tested and the Z axis

[0034] When the Z-axis values ​​of the highest points of the two circular surfaces, or the Z-axis values ​​of the lowest points of the circular surfaces, both satisfy the relationship Zmin>d, or Zmax<0, it indicates that the cylindrical collision box to be tested is located above or below the reference cylindrical collision box as a whole, and the first minimum distance is set to be greater than the maximum value of the length and diameter of all candidate cylindrical collision pairs; d represents the cylindrical height of the reference cylindrical collision box.

[0035] Specifically, the step of measuring the second minimum distance between the side of the cylindrical collision box to be measured and the reference cylindrical collision box includes:

[0036] The axes of the two cylindrical collision boxes are used as the line segments to be measured, and the shortest distance dis between the two line segments to be measured is calculated;

[0037] The second minimum distance dmin2 is determined according to the circular surface radius of the collision box of the cylinder to be tested, which is expressed as follows:

[0038] dmin2=dis-r2.

[0039] Specifically, determining the interference state of the cylindrical collision pair according to the circular surface radius value of the reference cylindrical collision box, the first minimum distance, and the second minimum distance includes:

[0040] When the first minimum distance or the second minimum distance is smaller than the circular surface radius of the reference cylinder collision box, it is determined that the selected candidate cylinder collision pair has an interference collision, otherwise no interference collision occurs.

[0041] On the other hand, the present application provides a robot safety collision interference detection device, the device comprising:

[0042] The first screening module is used to split the robot's mechanical arm and tool end into several cylindrical collision boxes according to their sizes, determine the spatial positions of the collision boxes under different motion states according to the kinematic equation, and screen M groups of candidate cylindrical collision pairs;

[0043] A distance calculation module is used to select a group of candidate cylinder collision pairs according to the combination, calculate the first minimum distance between the two circular surfaces of the cylinder collision box to be tested and the reference cylinder collision box, and measure the second minimum distance between the side surface of the cylinder collision box to be tested and the reference cylinder collision box;

[0044] A state judgment module is used to determine the interference state of the candidate cylinder collision pair according to the circular surface radius value of the reference cylinder collision box, the first minimum distance and the second minimum distance; poll M groups of candidate cylinder collision pairs, and when there is at least one group of candidate cylinder collision pairs with interference, determine that the robot arm has collision interference.

[0045] On the other hand, the present application provides a computer device, comprising a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the robot safety collision interference detection method described in the above aspects.

[0046] On the other hand, the present application provides a computer-readable storage medium, wherein the readable storage medium stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by a processor to implement the robot safety collision interference detection method described in the above aspects.

[0047] The beneficial effects brought about by the technical solution provided in the embodiment of the present application include at least: forming a collision pair with a collision box composed of robot joints, and evaluating all possible minimum distance values ​​of two cylinders and the cylinder radius. Compared with the conventional cylinder interference calculation method, this evaluation method divides the relative positions of the cylinders into multiple different types, which are discussed in different categories. Moreover, it is only necessary to perform coordinate transformation and other operations on the cylinder collision box to uniformly calculate the minimum distance and perform interference judgment, which simplifies the calculation type and reduces the amount of calculation. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 is a flow chart of a robot safety collision interference detection method provided by an embodiment of the present application;

[0049] Figure 2 It is a flow chart of the algorithm for interference detection of M groups of candidate cylinder collision pairs;

[0050] Figure 3 It is a schematic diagram of an algorithm for selecting candidate cylinder collisions for interference judgment;

[0051] Figure 4 A schematic diagram showing the positions of two candidate cylinder collision pairs after coordinate transformation in one possible case;

[0052] Figure 5 The algorithm flow chart for calculating the first minimum distance is listed;

[0053] Figure 6 A schematic diagram of calculating the shortest distance from the coordinate origin to the remaining projection contour line in the projection coordinate system is shown;

[0054] Figure 7 The structural block diagram of the robot safety collision interference detection device is shown;

[0055] Figure 8A structural block diagram of a computer device provided by an exemplary embodiment of the present application is shown. DETAILED DESCRIPTION

[0056] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.

[0057] The term "multiple" as used herein refers to two or more than two. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the related objects are in an "or" relationship.

[0058] Glossary:

[0059] Collision box: The collision box (circle, sphere, body) of a point set, object or a group of objects completely encloses the combination of objects in a relatively simple closed space. The main purpose of obtaining the bounding volume (circle, box) is for collision detection. Simple geometric bodies such as balls and boxes are used to surround objects with complex shapes, so as to quickly eliminate those objects that are unlikely to collide through simple calculations.

[0060] Figure 1 : is a flowchart of a robot safety collision interference detection method provided in an embodiment of the present application, comprising the following steps:

[0061] S1. Split the robot's mechanical arm and tool end into several cylindrical collision boxes according to their sizes, determine the spatial positions of the collision boxes under different motion states according to the kinematic equation, and screen M groups of candidate cylindrical collision pairs;

[0062] For a complex robot, any of its joints can move in a certain direction, so in theory, collision detection requires interference detection of all joints in pairs. However, this method will obviously increase the workload of testing and verification. This application chooses to first obtain the physical model of the robot's mechanical arm during the design phase, and construct a cylindrical collision box for the mechanical arm and the tool end (such as the grasping part, the clamping part, the suction pen end, etc.) according to parameters such as length and arm diameter. In subsequent tests, the cylindrical collision box is directly used to replace the motion path and range of the mechanical arm. In order to avoid one-by-two team detection, the spatial position of the collision box under different motion states can be determined according to the kinematic equation, and M groups of candidate cylinder collision pairs can be preliminarily screened out according to the possible spatial positions. In the subsequent interference calculation, it is only necessary to perform interference judgment on the selected cylindrical collision pairs in turn, and no interference judgment is performed between other cylindrical collision boxes, which reduces the amount of calculation.

[0063] S2, selecting a group of candidate cylinder collision pairs according to the combination, calculating the first minimum distance between the two circular surfaces of the cylinder collision box to be tested and the reference cylinder collision box, and measuring the second minimum distance between the side surface of the cylinder collision box to be tested and the reference cylinder collision box;

[0064] Interference calculation is performed on all cylinders (cylinder collision boxes) that may interfere. Considering that the interference calculation between cylinders is relatively complicated, the conventional calculation method needs to consider more situations. In order to optimize the interference calculation between cylinders, after selecting a group of candidate cylinder collision pairs, this application defines one of them as the reference cylinder collision box (referred to as the first cylinder) and the other as the cylinder collision box to be tested (referred to as the second cylinder). In order to facilitate calculation, a coordinate system can be established with one of them (this application uses the reference cylinder collision box) as the reference, and then the relative position and distance relationship between the second cylinder and the first cylinder are calculated to perform interference collision detection.

[0065] The minimum distance from the second cylinder to the first cylinder is usually calculated in two parts, including the minimum distance from the circular (top) surface of the second cylinder to the first cylinder (the side or circular surface formed by the periphery), and the minimum distance from the side of the second cylinder to the first cylinder (the side or circular surface). This application defines the minimum distance from the circular surface of the second cylinder to the first cylinder as the first minimum distance dmin1, and the minimum distance from the side of the second cylinder to the first cylinder as the second minimum distance dmin2.

[0066] S3. Determine the interference state of the candidate cylinder collision pair according to the circular surface radius value of the reference cylinder collision box, the first minimum distance, and the second minimum distance.

[0067] On the premise of verifying with the first cylinder as the benchmark, the circular surface radius of the cylinder is used as a reference for comparison with the first minimum distance and the second minimum distance calculated above. When the first minimum distance and the second minimum distance are greater than the circular surface radius of the first cylinder, it can be determined that there is no interference collision between the selected candidate cylinder collision pairs. At this time, the next collision pair can be selected and the above operations can be repeated.

[0068] Figure 2 This is an algorithm flow chart for interference detection of M groups of candidate cylinder collision pairs. After disassembling M pairs of cylinders, they are selected and interference judged in turn according to possible combinations until all M groups are detected and the state of the robot arm is determined. When the above judgment conditions are not met, it means that there may be interference collision between the two cylinders, that is, it is determined that the robot arm has collision interference, and an alarm can be issued or manual intervention can be used to modify the model structure.

[0069] Figure 3It is a schematic diagram of an algorithm for interference judgment for selecting candidate cylinder collisions. Specifically, interference or non-interference is determined based on comparison between the first minimum distance and the second minimum distance and the radius value of the circular surface of the first cylinder, and the state is finally determined.

[0070] The above judgment conditions are evaluated based on all possible minimum distance values ​​and cylinder radii of the two cylinders. Compared with the conventional cylinder interference calculation method, this evaluation method divides the relative positions of the cylinders into multiple different types for classification discussion. It only requires coordinate transformation of the cylinder collision box to uniformly calculate the minimum distance and perform interference judgment, which simplifies the calculation type and reduces the amount of calculation.

[0071] The following focuses on how to calculate the "minimum distance" between cylinders.

[0072] For a group of cylindrical collision pairs that are determined and selected, the first thing to do is to establish a coordinate system, fix one of the cylinders and optimize its position to reduce the amount of calculation. A feasible solution is to establish a three-dimensional coordinate system based on the length direction and circular surface of one of the cylinders (the first cylinder in this application), especially to coincide the axial direction of the cylinder with the coordinate axis. On this basis, this application proposes a method for coordinate system transformation, which uses a rotation transformation to make the axis of the first cylinder parallel to the Z-axis direction of the coordinate system, and the axis of the second coordinate system perpendicular to the Y-axis direction of the coordinate system, so that data simplification can be achieved in at least two directions. As for the structure of the transformation matrix, it is specifically determined according to the different extracted candidate collision pairs.

[0073] It should be noted here that the three-dimensional coordinate system is not a Cartesian coordinate system in a strict sense, because the position relationship of the coordinate system after the rotation matrix transformation will change the absolute position relationship of the original cylinder.

[0074] The rotation transformation process may include the following steps:

[0075] Assume that the center coordinates of the two circular surfaces of cylinder 1 are known to be P11 (x11, y11, z11) and P12 (x12, y12, z12), and the center coordinates of the two circular surfaces of cylinder 2 are P21 (x21, y21, z21) and P22 (x22, y22, z22).

[0076] 1) Based on the established coordinate system, calculate the axis vectors L1 and L2 of the two cylinder collision boxes as follows:

[0077] L1=P12-P11

[0078] L2=P22-P21

[0079] In the above formula, P11 and P12 respectively represent the center coordinates of the two circular surfaces of the reference cylinder collision box, and P21 and P22 respectively represent the center coordinates of the two circular surfaces of the cylinder to be measured collision box;

[0080] 2), Calculate the transformation matrix Rx for rotation around the X-axis, so that L1′ = Rx * L1, L2′ = Rx * L2, and L1′ is perpendicular to the Y-axis; calculate the transformation matrix Ry for rotation around the Y-axis, so that L1″ = Ry * L1′, L2″ = Ry * L2′, and L1″ has the same direction as the Z-axis; calculate the transformation matrix Rz for rotation around the Z-axis, so that L2″′ = Rz * L2″, and L2″′ is perpendicular to the Y-axis;

[0081] 3), Keep the coordinate position of P11 unchanged, and calculate the center coordinates v of each circular surface of the cylinder collision box after coordinate transformation as follows:

[0082] P12 = P11 + Ry * Rx * (P12 - P11)

[0083] P21 = P11 + Rz * Ry * Rx * (P21 - P11)

[0084] P22 = P11 + Rz * Ry * Rx * (P22 - P11)

[0085] When taking the center coordinate point of one of the circular surfaces of the reference cylinder collision box as the coordinate system origin, it is further simplified as follows:

[0086] P12 = Ry * Rx * (P12 - P11)

[0087] P21 = Rz * Ry * Rx * (P21 - P11)

[0088] P22 = Rz * Ry * Rx * (P22 - P11)

[0089] Where P11 = {0, 0, 0}, which is the coordinate origin.

[0090] After the coordinate transformation of this step, the cylinder can be directly projected onto the corresponding coordinate system, and the positional relationship between the two cylinders can be calculated according to the distance between the projections, and collision detection can be performed.

[0091] Figure 4 Figure 37 shows a schematic diagram of the positions of two candidate cylinder collision pairs after coordinate transformation in a possible case, where the center (P11) of the lower bottom circular surface of cylinder 1 coincides with the coordinate system origin, and the axis vector coincides with the Z-axis. The following calculations are deduced based on this.

[0092] Calculating the first minimum distance from the two circular surfaces of the second cylinder to the first cylinder includes:

[0093] 1) Project the two circular surfaces of the second cylinder onto a horizontal plane (corresponding to the XOY plane of the coordinate system) to determine the positional relationship of the coordinate origin relative to the projected image.

[0094] Here we need to consider the special situation of different cylinder position relationships. Figure 4 The second cylinder is tilted with respect to the first cylinder, and the height of the two cylinders overlaps. The specific shape of the projected image is determined by the angle between the axis of the second cylinder and the Z axis. It is clear that when the second cylinder is parallel to the first cylinder, the projected image is a standard circle. When the second cylinder is perpendicular to the first cylinder, the projected image is a line segment. When the second cylinder intersects with the first cylinder, the projected image is an ellipse, that is, Figure 4 The situation shown.

[0095] 2) When the coordinate origin is on or inside any projection image contour line, determine the shortest distance dmin′ from the coordinate origin to the projection image = 0; when the coordinate origin is not on or inside any projection image contour line, calculate the shortest distance dmin′ from the coordinate origin to the projection image;

[0096] 3) Determine the relative position relationship between the two circular surfaces of the cylindrical collision box to be tested and the reference plane where the two reference circular surfaces of the reference cylindrical collision box are located;

[0097] This step is essentially to determine the relative position of the two cylinders, so as to make targeted interference judgments. Because if the second cylinder is completely above or below the first cylinder, theoretically no interference will occur, and this situation does not require too much resources to prove. For this reason, before the projection calculation, the position can also be judged by the Z-axis height of the two cylinders, which is summarized as follows:

[0098] Calculate the Z-axis values ​​of the highest and lowest points of the two circular surfaces of the collision box of the cylinder to be tested, respectively, as shown below:

[0099] Zmax=Pz+r2*sinβ

[0100] Zmin=Pz-r2*sinβ

[0101] Wherein, Zmax is the Z-axis value of the highest point of the circular surface, Zmin is the Z-axis value of the lowest point of the circular surface; Pz is the Z-axis value of the center of the circular surface, r2 is the circular surface radius of the second cylinder, and β is the angle between the axis of the second cylinder and the Z axis;

[0102] When the Z-axis values ​​of the highest points of the two circular surfaces, or the Z-axis values ​​of the lowest points of the circular surfaces, both satisfy the relationship Zmin>d, or Zmax<0, it indicates that the cylindrical collision box to be tested is located above or below the reference cylindrical collision box as a whole, and the first minimum distance is set to be greater than the maximum value dminb of the length and diameter of all candidate cylindrical collision pairs; d represents the cylindrical height of the reference cylindrical collision box.

[0103] Figure 5 The algorithm flow chart for calculating the first minimum distance is listed. First, it is determined whether the two circular surfaces of the second cylinder are located above or below the first cylinder as a whole (satisfying Zmin>d, or Zmax<0). This is to filter out the situation that will definitely not cause interference and reduce the amount of subsequent calculations. Figure 4 ), that is, the second cylinder is partially above the first cylinder and partially below it in terms of Z-axis height (overlapping with the first cylinder). The following will focus on interference judgment on this situation.

[0104] 4) When there is a circular surface to be measured that intersects at least one reference plane (the reference plane is the plane where the two circular surfaces of the first cylinder are located), it is indicated that the part of the circular surface to be measured is located above or below the collision box of the reference cylinder, and the projection contour line corresponding to the part located above or below the collision box of the reference cylinder in the projection image is removed, and the shortest distance dmin″ from the coordinate origin to the remaining projection contour line is calculated, and it is determined as the first minimum distance;

[0105] As for how to determine whether the circular surface to be measured intersects at least one reference plane, it is still to determine the values ​​of Zmax and Zmin. When the Z-axis values ​​of the two center coordinates of the circular surface to be measured both satisfy Zmin<0, or Zmax>d, it means that part of the circular surface to be measured is located above or below the collision box of the reference cylinder.

[0106] Figure 6 The diagram shows the shortest distance between the origin and the remaining projection contour in the projection coordinate system. The core idea of ​​this scheme is to remove the part of the second cylinder located above the first cylinder (because the circular surface of the first cylinder is parallel to the XOY plane), and the remaining circular surface projection contour is used to calculate the shortest distance. Figure 5 The other circular projection of the second cylinder is farther away from the origin of the coordinate system, so it is not drawn in the figure.

[0107] by Figure 6 Taking the contour line shown as an example, the shortest distance dmin″ (that is, Figure 6 The straight-line distance between point A and the origin is determined as the first minimum distance dmin1.

[0108] When the two circular surfaces to be measured do not intersect the two reference planes, the shortest distance dmin′ from the coordinate origin to the projection image is determined as the first minimum distance dmin1.

[0109] When measuring and calculating the second minimum distance, similar to the above, the situation that will definitely not cause interference can be filtered out first to reduce the amount of subsequent calculations. Specifically, it is determined whether the X-axis values ​​of the centers of the two circular surfaces corresponding to the second cylinder have the same sign or the Z-axis values ​​are both greater than d or less than 0. If so, it means that the side of the second cylinder will not interfere with the first cylinder, or the interference will occur after the first cylinder and the second cylinder are switched in sequence (each two groups of M groups of candidate cylinder collision pairs contain the same cylinder, but the order of the cylinders is different), which will be specifically calculated in the method for calculating the first minimum distance. In this case, the second minimum distance dmin2 is set to be greater than the maximum value of the length and diameter of all candidate cylinder collision pairs. If it is not satisfied, the axes of the two cylinder collision boxes are used as the line segments to be measured, and the shortest distance dis between the two line segments to be measured is calculated; the second minimum distance dmin2 is determined according to the radius of the circular surface of the cylinder collision box to be measured, expressed as dmin2=dis-r2.

[0110] Subsequently, the interference state of the cylinder collision pair is determined according to the first and second minimum distances. When the first minimum distance or the second minimum distance is smaller than the circular surface radius of the reference cylinder collision box, it is determined that the selected candidate cylinder collision pair has an interference collision, otherwise no interference collision occurs.

[0111] In summary, in the present application, the robot arm and the tool end are split into several cylindrical bounding boxes, and then preprocessing is performed to find the cylindrical collision pairs that may interfere with each other during the movement. When the interference calculation is actually performed, the interference calculation is only performed on the selected cylindrical collision pairs to avoid invalid interference calculations. Considering that the interference calculation between cylinders is relatively complicated, the conventional calculation method needs to consider more situations. In order to optimize the interference calculation between cylinders, the relative positions of the cylindrical collision boxes are divided into a variety of different types for discussion. It is only necessary to perform coordinate transformation and plane projection on the cylindrical bounding boxes, and then the minimum distance can be uniformly calculated to perform interference judgment, which simplifies the calculation type and reduces the amount of calculation.

[0112] In actual application, during the robot's pre-planning or real-time movement stage, pre-judgment of the robot's interference situation can avoid interference between the robot bodies or between the robot body and the tool, avoid damage to the robot, reduce losses, and the optimized interference detection has a small amount of calculation and does not affect the robot's real-time performance.

[0113] Figure 7The structure block diagram of the robot safety collision interference detection device provided in an embodiment of the present application is shown, and the device includes:

[0114] The first screening module 710 is used to split the robot's mechanical arm and tool end into a number of cylindrical collision boxes according to their sizes, determine the spatial positions of the collision boxes under different motion states according to the kinematic equation, and screen M groups of candidate cylindrical collision pairs;

[0115] A distance calculation module 720 is used to select a group of candidate cylinder collision pairs according to the combination, calculate the first minimum distance between the two circular surfaces of the cylinder collision box to be tested and the reference cylinder collision box, and measure the second minimum distance between the side surface of the cylinder collision box to be tested and the reference cylinder collision box;

[0116] The state judgment module 730 is used to determine the interference state of the candidate cylinder collision pair according to the circular surface radius value of the reference cylinder collision box, the first minimum distance and the second minimum distance; poll M groups of candidate cylinder collision pairs, and when there is at least one group of candidate cylinder collision pairs with interference, determine that the robot arm has collision interference.

[0117] The robot safety collision interference detection device provided in the embodiment of the present application can be applied to the robot safety collision interference detection method provided in the above embodiment. For relevant details, refer to the above method embodiment. The implementation principle and technical effect are similar and will not be repeated here.

[0118] It should be noted that the robot safety collision interference detection device provided in the embodiment of the present application is only illustrated by the division of the above-mentioned functional modules / functional units. In actual applications, the above-mentioned functions can be assigned to different functional modules / functional units as needed, that is, the internal structure of the robot safety collision interference detection device is divided into different functional modules / functional units to complete all or part of the functions described above. In addition, the implementation method of the robot safety collision interference detection method provided in the above method embodiment and the implementation method of the robot safety collision interference detection device provided in this embodiment belong to the same concept. The specific implementation process of the robot safety collision interference detection device provided in this embodiment is detailed in the above method embodiment, which will not be repeated here.

[0119] Figure 8The block diagram of a computer device provided by an exemplary embodiment of the present application is shown. It is a computer device such as a desktop computer, a laptop computer, a PDA, and a cloud server. The computer device may include, but is not limited to, a processor and a memory. Among them, the processor and the memory may be connected by a bus or in other ways. Among them, the processor may be a central processing unit (CPU). The processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, graphics processors (GPU), embedded neural network processors (NPU) or other dedicated deep learning coprocessors, discrete gates or transistor logic devices, discrete hardware components and other chips, or a combination of the above-mentioned various chips.

[0120] The processor may include one or more processing cores, such as a 4-core processor, an 8-core processor, etc. The processor may be implemented in at least one hardware form of DSP (Digital Signal Processing), FPGA (Field-Programmable Gate Array), and PLA (Programmable Logic Array). The processor may also include a main processor and a coprocessor. The main processor is a processor for processing data in the awake state, also known as a CPU (Central Processing Unit); the coprocessor is a low-power processor for processing data in the standby state. In some embodiments, the processor may be integrated with a GPU (Graphics Processing Unit), which is responsible for rendering and drawing the content that needs to be displayed on the display screen. In some embodiments, the processor may also include an AI (Artificial Intelligence) processor, which is used to process computing operations related to machine learning.

[0121] As a non-transient computer-readable storage medium, the memory can be used to store non-transient software programs, non-transient computer executable programs and modules, such as program instructions / modules corresponding to the methods in the above-mentioned embodiments of the present application. The processor executes various functional applications and data processing of the processor by running the non-transient software programs, instructions and modules stored in the memory, that is, the method in the above-mentioned method implementation is realized. The memory may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created by the processor, etc. In addition, the memory may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely arranged relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network and a combination thereof.

[0122] In some embodiments, the computer device may further optionally include: a peripheral device interface and at least one peripheral device. The processor, the memory and the peripheral device interface may be connected via a bus or a signal line. Each peripheral device may be connected to the peripheral device interface via a bus, a signal line or a circuit board. Specifically, the peripheral device includes: at least one of a radio frequency circuit, a display screen and a keyboard.

[0123] The peripheral device interface can be used to connect at least one peripheral device related to I / O (Input / Output) to the processor and the memory. In some embodiments, the processor, the memory, and the peripheral device interface are integrated on the same chip or circuit board; in some other embodiments, any one or two of the processor, the memory, and the peripheral device interface can be implemented on a separate chip or circuit board, which is not limited in this embodiment.

[0124] The display screen is used to display the UI (User Interface). The UI may include graphics, text, icons, videos and any combination thereof. When the display screen is a touch screen, the display screen also has the ability to collect touch signals on the surface or above the surface of the display screen. The touch signal can be input to the processor as a control signal for processing. At this time, the display screen can also be used to provide virtual buttons and / or virtual keyboards, also known as soft buttons and / or soft keyboards. In some embodiments, the display screen can be one, arranged on the front panel of the computer device; in other embodiments, the display screen can be at least two, respectively arranged on different surfaces of the computer device or in a folding design; in other embodiments, the display screen can be a flexible display screen, arranged on a curved surface or a folding surface of the computer device. Even, the display screen can be set to a non-rectangular irregular shape, that is, a special-shaped screen. The display screen can be made of materials such as LCD (Liquid Crystal Display), OLED (Organic Light-Emitting Diode) and the like.

[0125] The power supply is used to power various components in the computer device. The power supply can be AC, DC, a disposable battery, or a rechargeable battery. When the power supply includes a rechargeable battery, the rechargeable battery can be a wired rechargeable battery or a wireless rechargeable battery. A wired rechargeable battery is a battery that is charged through a wired line, and a wireless rechargeable battery is a battery that is charged through a wireless coil. The rechargeable battery can also be used to support fast charging technology.

[0126] Those skilled in the art will appreciate that the structure shown in this embodiment does not constitute a limitation on the computer device, and may include more or fewer components than shown in the figure, or combine certain components, or adopt a different component arrangement.

[0127] The embodiment of the present application also discloses a computer-readable storage medium. Specifically, the computer-readable storage medium is used to store a computer program, and when the computer program is executed by the processor, the method in the above-mentioned method implementation is implemented. Those skilled in the art can understand that the implementation of all or part of the process in the above-mentioned implementation method of the present application can be completed by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the process of the implementation of the above-mentioned methods. Among them, the storage medium can be a disk, an optical disk, a read-only memory (ROM), a random access memory (RAM), a flash memory (Flash Memory), a hard disk (Hard Disk Drive, abbreviated as: HDD) or a solid-state drive (SSD), etc.; the storage medium can also include a combination of the above-mentioned types of memory.

[0128] This specific embodiment is merely an explanation of the present invention and is not a limitation of the present invention. After reading this specification, those skilled in the art may make non-creative modifications to the present embodiment as needed. However, as long as they are within the scope of the claims of the present invention, they are protected by the patent law.

Claims

1. A robot safety collision interference detection method, characterized in that: The method comprises: The robot's mechanical arm and tool end are divided into several cylindrical collision boxes according to their sizes, the spatial positions of the collision boxes under different motion states are determined according to the kinematic equations, and M groups of candidate cylindrical collision pairs are screened; Select a group of candidate cylinder collision pairs according to the combination, calculate the first minimum distance between the two circular surfaces of the cylinder collision box to be tested and the reference cylinder collision box, and measure the second minimum distance between the side surface of the cylinder collision box to be tested and the reference cylinder collision box; The interference state of the candidate cylinder collision pair is determined according to the circular surface radius value of the reference cylinder collision box, the first minimum distance and the second minimum distance; M groups of candidate cylinder collision pairs are polled, and when at least one group of candidate cylinder collision pairs has interference, it is determined that the robot arm has collision interference.

2. The method according to claim 1, characterized in that: After selecting the cylindrical collision pair, the method further includes: A coordinate system is established, and the axis of the reference cylindrical collision box is parallel to the Z-axis direction of the coordinate system by using rotation transformation, and the axis of the cylindrical collision box to be measured is perpendicular to the Y-axis direction of the coordinate system.

3. The method according to claim 2, characterized in that The rotation transformation process includes: Based on the established coordinate system, the axis vectors L1 and L2 of the two cylinder collision boxes are calculated as follows: L1=P12-P11 L2=P22-P21 Wherein P11 and P12 respectively represent the center coordinates of the two circular surfaces of the reference cylindrical collision box, and P21 and P22 respectively represent the center coordinates of the two circular surfaces of the cylindrical collision box to be tested; Calculate the transformation matrix Rx for rotation around the X-axis, so that L1′=Rx*L1, L2′=Rx*L2, and L1′ is perpendicular to the Y-axis; calculate the transformation matrix Ry for rotation around the Y-axis, so that L1″=Ry*L1′, L2″=Ry*L2′, and L1″ is in the same direction as the Z-axis; calculate the transformation matrix Rz for rotation around the Z-axis, so that L2″′=Rz*L2″, and L2″′ is perpendicular to the Y-axis; When a center coordinate point of the reference cylindrical collision box is used as the origin of the coordinate system, the center coordinates of each circular surface of the cylindrical collision box after coordinate transformation are calculated; the expression is as follows: P12=Ry*Rx*(P12-P11) P21=Rz*Ry*Rx*(P21-P11) P22=Rz*Ry*Rx*(P22-P11) Among them, P11={0,0,0} is the coordinate origin.

4. The method according to any one of claims 1 to 3, characterized in that: The calculating of the first minimum distance between the two circular surfaces formed by the to-be-tested cylindrical collision box and the reference cylindrical collision box comprises: Project the two circular surfaces of the cylindrical collision box to be tested onto a horizontal plane to determine the positional relationship of the coordinate origin relative to the projected image; wherein, when the cylindrical collision box to be tested is parallel to the reference cylindrical collision box, the projected image is a standard circle; when the cylindrical collision box to be tested is perpendicular to the reference cylindrical collision box, the projected image is a line segment; when the cylindrical collision box to be tested is in an intersecting state with the reference cylindrical collision box, the projected image is an ellipse; When the coordinate origin is located on any projection image contour line or inside the contour line, determine the shortest distance dmin′ from the coordinate origin to the projection image = 0; when the coordinate origin is not located on any projection image contour line or inside the contour line, calculate the shortest distance dmin′ from the coordinate origin to the projection image; Determine the relative position relationship between the two circular surfaces to be tested of the cylindrical collision box to be tested and the reference plane where the two reference circular surfaces of the reference cylindrical collision box are located; When there is a circular surface to be measured that cuts at least one reference plane, it indicates that part of the circular surface to be measured is located above or below the reference cylindrical collision box, remove the projection contour line corresponding to the part located above or below the reference cylindrical collision box in the projection image, calculate the shortest distance dmin″ from the coordinate origin to the remaining projection contour line, and determine it as the first minimum distance dmin1; When the two circular surfaces to be measured do not intersect the two reference planes, the shortest distance dmin′ from the coordinate origin to the projection image is determined as the first minimum distance dmin1.

5. The method according to claim 4, characterized in that Before calculating the first minimum distance, the method further includes: The Z-axis values ​​of the coordinates of the highest and lowest points of the two circular surfaces corresponding to the collision box of the cylinder to be tested are calculated respectively, as shown below: Zmax=Pz+r2*sinβ Zmin=Pz-r2*sinβ Among them, Zmax is the Z-axis value of the highest point of the circular surface, Zmin is the Z-axis value of the lowest point of the circular surface; Pz is the Z-axis value of the center of the circle on the circular surface, r2 is the circular radius of the collision box of the cylindrical object to be tested, and β is the angle between the axis of the collision box of the cylindrical object to be tested and the Z axis; When the Z-axis values ​​of the highest points of the two circular surfaces, or the Z-axis values ​​of the lowest points of the circular surfaces, both satisfy the relationship Zmin>d, or Zmax<0, it indicates that the cylindrical collision box to be tested is located above or below the reference cylindrical collision box as a whole, and the first minimum distance is set to be greater than the maximum value of the length and diameter of all candidate cylindrical collision pairs; d represents the cylindrical height of the reference cylindrical collision box.

6. The method according to claim 1, characterized in that The step of measuring the second minimum distance between the side of the cylindrical collision box to be measured and the reference cylindrical collision box includes: The axes of the two cylindrical collision boxes are used as the line segments to be measured, and the shortest distance dis between the two line segments to be measured is calculated; The second minimum distance dmin2 is determined according to the circular radius of the collision box of the cylinder to be tested, which is expressed as follows: dmin2=dis-r2.

7. The method according to claim 6, characterized in that The step of determining the interference state of the cylindrical collision pair according to the circular surface radius value of the reference cylindrical collision box, the first minimum distance and the second minimum distance includes: When the first minimum distance or the second minimum distance is smaller than the circular surface radius of the reference cylinder collision box, it is determined that the selected candidate cylinder collision pair has an interference collision, otherwise no interference collision occurs.

8. A robot safety collision interference detection device, characterized in that: The device comprises: The first screening module is used to split the robot's mechanical arm and tool end into several cylindrical collision boxes according to their sizes, determine the spatial positions of the collision boxes under different motion states according to the kinematic equation, and screen M groups of candidate cylindrical collision pairs; A distance calculation module is used to select a group of candidate cylinder collision pairs according to the combination, calculate the first minimum distance between the two circular surfaces of the cylinder collision box to be tested and the reference cylinder collision box, and measure the second minimum distance between the side surface of the cylinder collision box to be tested and the reference cylinder collision box; A state judgment module is used to determine the interference state of the candidate cylinder collision pair according to the circular surface radius value of the reference cylinder collision box, the first minimum distance and the second minimum distance; poll M groups of candidate cylinder collision pairs, and when there is at least one group of candidate cylinder collision pairs with interference, determine that the robot arm has collision interference.

9. A computer device, characterized in that: The computer device includes a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set is loaded and executed by the processor to implement the robot safety collision interference detection method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The readable storage medium stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the robot safety collision interference detection method as described in any one of claims 1 to 7.

Images