Robot collision avoidance method, apparatus and robot control system

Abstract

The application provides a robot anti-collision method, device and robot control system, and the method comprises the steps of establishing a workpiece coordinate system, constructing a robot workspace under the workpiece coordinate system, obtaining a first pose of a tool end of the robot moving to a target point of the robot workspace, calculating a second pose according to the first pose and a first coordinate system transformation matrix, the second pose being a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix being a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system, calculating an enclosing box of each joint connecting rod and the tool of the robot in the robot base coordinate system according to the second pose, obtaining a plurality of base coordinate enclosing boxes, the enclosing box being a smallest cuboid wrapping the joint part, and the joint part comprising the joint connecting rod and the tool, and determining whether collision occurs between the joint parts corresponding to the plurality of base coordinate enclosing boxes by using the separate axis theorem, so that the problem of high cost of anti-collision through three-dimensional vision technology is solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot path planning, in particular to a robot anti-collision method, device, computer program product and robot control system. BACKGROUND

[0002] With the reduction of labor costs and the development of robot technology and computer vision technology, the proportion of robots used in the production process will be higher and higher, and the daily demand is no longer simply grabbing workpieces on a conveyor belt. Deep frame grabbing projects are becoming more and more common, and even more, there will be some narrow space for workpiece grabbing requirements. The scene does not have much freedom for the robot, which requires the robot to have obstacle avoidance function and trajectory planning function. The existing collision detection algorithm is developed using three-dimensional vision, and the system cost is high. The collision detection range is limited by the field of view of the sensor, and the algorithm data processing amount is large, which requires high hardware system, and is not conducive to rapid deployment. SUMMARY

[0003] The main purpose of the present application is to provide a robot anti-collision method, device, computer program product and robot control system to at least solve the problem of high cost of anti-collision by three-dimensional vision technology in the prior art.

[0004] In order to achieve the above-mentioned purpose, according to one aspect of the present application, a robot anti-collision method is provided, comprising: establishing a workpiece coordinate system and constructing a robot workspace under the workpiece coordinate system; obtaining a first pose of a tool end of the robot moving to a target point of the robot workspace; calculating a second pose according to the first pose and a first coordinate system transformation matrix, the second pose being a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix being a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system; calculating the bounding box of each joint link and the tool of the robot in the robot base coordinate system according to the second pose to obtain a plurality of base coordinate bounding boxes, the bounding box being the smallest cuboid wrapping the joint part, and the joint part including the joint link and the tool; and determining whether collision occurs between the joint parts corresponding to the plurality of base coordinate bounding boxes by using the separate axis theorem.

[0005] Optionally, the bounding box of each joint link and the tool of the robot in the robot base coordinate system is calculated according to the second pose, to obtain a plurality of base coordinate bounding boxes, including: the joint angle of each joint is calculated according to the second pose by using inverse kinematics; the first joint link bounding box of each joint link is established according to the size of each joint link of the robot, and the first joint link bounding box is the bounding box of the joint link in the joint coordinate system corresponding to the joint link; and the base coordinate bounding box of each joint link is calculated according to the joint angle of each joint and the first joint link bounding box of each joint link.

[0006] Optionally, the bounding box of each joint link and the tool of the robot in the robot base coordinate system is calculated according to the second pose, to obtain a plurality of base coordinate bounding boxes, including: a pose change matrix is obtained, the pose change matrix is a transformation matrix of the pose of the tool end to the pose of the tool coordinate system, and the tool coordinate system is a coordinate system with the tool center point as the origin; the second coordinate system transformation matrix is calculated according to the pose change matrix and the first coordinate system transformation matrix, the second coordinate system transformation matrix is a matrix for coordinate conversion between the tool coordinate system and the robot base coordinate system; the second joint link bounding box of each tool is established according to the size of the tool, and the second joint link bounding box is the bounding box of the tool in the tool coordinate system; and the base coordinate bounding box of each tool is calculated according to the second coordinate system transformation matrix and the second joint link bounding box.

[0007] Optionally, the method further comprises: obtaining a first base coordinate bounding box and a second base coordinate bounding box, the first base coordinate bounding box and the second base coordinate bounding box being any two of the base coordinate bounding boxes; calculating normal vectors of each face of the first base coordinate bounding box and the second base coordinate bounding box to obtain a plurality of first normal vectors; calculating normal vectors of all edge combinations to obtain a plurality of second normal vectors, the edge combinations including an edge of the first base coordinate bounding box and an edge of the second base coordinate bounding box; calculating projection points of all vertices of the first base coordinate bounding box on the first normal vectors to obtain projections of each edge of the first base coordinate bounding box on the first normal vectors, calculating projection points of all vertices of the second base coordinate bounding box on the first normal vectors to obtain projections of each edge of the second base coordinate bounding box on the first normal vectors, calculating projection points of all vertices of the first base coordinate bounding box on the second normal vectors to obtain projections of each edge of the first base coordinate bounding box on the second normal vectors, and calculating projection points of all vertices of the second base coordinate bounding box on the second normal vectors to obtain projections of each edge of the second base coordinate bounding box on the second normal vectors; determining that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the first normal vectors in a case where projections of any edge of the first base coordinate bounding box on the first normal vectors and projections of any edge of the second base coordinate bounding box on the first normal vectors do not overlap, and determining that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the second normal vectors in a case where projections of any edge of the first base coordinate bounding box on the second normal vectors and projections of any edge of the second base coordinate bounding box on the second normal vectors do not overlap; and determining that the joint member corresponding to the first base coordinate bounding box and the joint member corresponding to the second base coordinate bounding box do not collide in a case where the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the first normal vectors and the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the second normal vectors.

[0008] Optionally, after determining whether the joint members corresponding to the plurality of base coordinate bounding boxes collide by using the separate axis theorem, the method further comprises: in a case where it is determined that any two of the base coordinate bounding boxes collide, marking the target point as an unreachable point.

[0009] Optionally, the workpiece coordinate system is established, and the robot workspace is constructed under the workpiece coordinate system, including: controlling the tool center of the robot to move to a first point, a second point and a third point of the work plane in turn to obtain a first coordinate, a second coordinate and a third coordinate, the first coordinate being a coordinate of the tool center in the robot base coordinate system when the tool center is located at the first point, the second coordinate being a coordinate of the tool center in the robot base coordinate system when the tool center is located at the second point, and the third coordinate being a coordinate of the tool center in the robot base coordinate system when the tool center is located at the third point, the first point, the second point and the third point being not collinear; calculating a first plane vector according to the first coordinate and the second coordinate, and calculating a second plane vector according to the first coordinate and the third coordinate; calculating three unit orthogonal bases according to the first plane vector and the second plane vector; establishing the workpiece coordinate system according to the three unit orthogonal bases; and constructing the robot workspace in the workpiece coordinate system according to the length, the width and the height of the workspace.

[0010] Optionally, before the second pose is calculated according to the first pose and a first coordinate system transformation matrix, the method further includes: calculating the first coordinate system transformation matrix according to the three unit orthogonal bases and unit orthogonal bases of the robot base coordinate system.

[0011] According to another aspect of the present application, a robot anti-collision device is provided, including: an establishing unit configured to establish a workpiece coordinate system and construct a robot workspace under the workpiece coordinate system; an obtaining unit configured to obtain a first pose of a tool end of the robot moving to a target point of the robot workspace; a first calculating unit configured to calculate a second pose according to the first pose and a first coordinate system transformation matrix, the second pose being a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix being a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system; a second calculating unit configured to calculate a bounding box of each joint connecting rod and the tool of the robot in the robot base coordinate system according to the second pose to obtain a plurality of base coordinate bounding boxes; and a determining unit configured to determine whether the plurality of base coordinate bounding boxes collide with each other by using a separate axis theorem.

[0012] According to still another aspect of the present application, a computer program product is provided, including a computer program, which, when executed by a processor, implements any of the methods described above.

[0013] According to yet another aspect of the present application, a robot control system is provided, comprising a robot, one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs comprising instructions for performing any of the methods described.

[0014] By applying the technical solution of the present application, in the robot anti-collision method described above, when the tool end of the robot moves to the target point in the robot workspace, the bounding box of the joint connecting rod and the tool of the robot is constructed, and all the bounding boxes are converted to the robot base coordinate system through coordinate conversion to obtain a plurality of base coordinate bounding boxes. Whether the plurality of base coordinate bounding boxes collide can be determined by using the separation axis theorem. If a collision occurs, it indicates that the movement of the tool end of the robot to the target point in the robot workspace will cause a collision between the joint connecting rod and the tool of the robot. Therefore, collision can be avoided during path planning, without using three-dimensional vision technology for anti-collision, greatly reducing the cost and solving the problem of high cost of anti-collision through three-dimensional vision technology in the prior art. The problem of low reliability of anti-collision caused by the blind spot of the three-dimensional vision sensor is also avoided. BRIEF DESCRIPTION OF DRAWINGS

[0015] Figure 1 A hardware structure block diagram of a mobile terminal for performing a robot anti-collision method according to an embodiment of the present application is shown;

[0016] Figure 2 A flowchart of a robot anti-collision method according to an embodiment of the present application is shown;

[0017] Figure 3 A schematic diagram of a tool coordinate system, a joint coordinate system, and a robot base coordinate system according to an embodiment of the present application is shown;

[0018] Figure 4 A schematic diagram of a workpiece coordinate system according to an embodiment of the present application is shown;

[0019] Figure 5 A structure block diagram of a robot anti-collision device according to an embodiment of the present application is shown.

[0020] Among the above drawings, the following reference signs are included:

[0021] 102, processor; 104, memory; 106, transmission device; 108, input / output device. DETAILED DESCRIPTION

[0022] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0023] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort should fall within the scope of protection of the present application.

[0024] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0025] As described in the background section, collision avoidance using 3D vision technology in the prior art is costly. To solve this technical problem, embodiments of this application provide a robot collision avoidance method, apparatus, computer program product, and robot control system.

[0026] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

[0027] The methods and embodiments provided in this application can be executed on a mobile terminal, computer terminal, or similar computing device. Taking running on a mobile terminal as an example, Figure 1 This is a hardware structure block diagram of a mobile terminal for a robot collision avoidance method according to an embodiment of the present invention. Figure 1 As shown, a mobile terminal may include one or more ( Figure 1 Only one is shown in the diagram. A processor 102 (which may include, but is not limited to, a microprocessor MCU or a programmable logic device FPGA, etc.) and a memory 104 for storing data are also shown. The mobile terminal may further include a transmission device 106 for communication functions and an input / output device 108. Those skilled in the art will understand that... Figure 1The illustrated structure is merely schematic and does not limit the structure of the mobile terminal described above. For example, the mobile terminal can further include more or less components than those shown, or have different configurations of the components shown. Figure 1 The mobile terminal can also include more or less components than those shown, or have a different configuration or arrangement of the components. The various components shown in the Figure can be variously combined and / or divided. Figure 1

[0028] The memory 104 is operable to store a computer program, such as a software program of an application and modules, e.g., a computer program corresponding to the robot collision avoidance method in the embodiments of the present application. The processor 102 performs various functional applications and data processing by running the computer program stored in the memory 104, i.e., implements the method described above. The memory 104 can include a high-speed random access memory, and also can include a nonvolatile memory, such as one or more magnetic storage devices, a flash memory, or other nonvolatile solid-state memory. In some examples, the memory 104 can further include a memory remotely disposed relative to the processor 102, which can be connected to the mobile terminal through a network. Examples of the network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof. The transmission device 106 is used to receive or send data via a network. The specific examples of the network can include a wireless network provided by a communication provider of the mobile terminal. In one example, the transmission device 106 includes a network adapter (NIC), which can be connected to other network devices through a base station so as to communicate with the Internet. In one example, the transmission device 106 can be a radio frequency (RF) module, which is used to communicate with the Internet in a wireless manner.

[0029] In the embodiments, a robot collision avoidance method running on a mobile terminal, a computer terminal or a similar computing device is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0030] Figure 2 is a flowchart of the robot collision avoidance method according to the embodiments of the present application. As shown in Figure 2 , the method includes the following steps:

[0031] Step S201, establishing a workpiece coordinate system and constructing a robot working space under the workpiece coordinate system;

[0032] Step S202, acquiring a first pose of a tool end of the robot moving to a target point of the robot working space;

[0033] ​Step S203, a second pose is calculated according to the first pose and the first coordinate system transformation matrix, the second pose is the pose of the tool end in the robot base coordinate system, and the first coordinate system transformation matrix is the matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system;

[0034] Step S204, a bounding box of each joint connecting rod and the tool of the robot in the robot base coordinate system is calculated according to the second pose, to obtain a plurality of base coordinate bounding boxes, the bounding box is the smallest cuboid wrapping the joint member, and the joint member includes the joint connecting rod and the tool;

[0035] Step S205, whether a collision occurs between the joint members corresponding to the plurality of base coordinate bounding boxes is determined by using the separation axis theorem.

[0036] In the robot anti-collision method, when the tool end of the robot moves to the target point in the robot workspace, the bounding boxes of the joint connecting rods and the tool of the robot are constructed, all the bounding boxes are converted to the robot base coordinate system through coordinate conversion, a plurality of base coordinate bounding boxes are obtained, and whether a collision occurs between the plurality of base coordinate bounding boxes is determined by using the separation axis theorem. If a collision occurs, it indicates that the tool end of the robot moving to the target point in the robot workspace will cause a collision between the joint connecting rods and the tool of the robot, so that the collision can be avoided during path planning, without using three-dimensional vision technology for anti-collision, greatly reducing the cost, and solving the problem of high cost of anti-collision by three-dimensional vision technology in the prior art. The problem of low reliability of anti-collision caused by the blind spot of the three-dimensional vision sensor is also avoided.

[0037] In order to facilitate the simulation of the joint connecting rod, in an optional implementation method, the step S204 includes:

[0038] Step S2041, the joint angles of each joint are calculated according to the second pose by using inverse kinematics;

[0039] Step S2042, a first joint connecting rod bounding box of each joint connecting rod is established according to the size of each joint connecting rod of the robot, the first joint connecting rod bounding box is the bounding box of the joint connecting rod in the joint coordinate system corresponding to the joint connecting rod;

[0040] Step S2043, the base coordinate bounding box of each joint connecting rod is calculated according to the joint angles of each joint and the first joint connecting rod bounding box of each joint connecting rod.

[0041] In the implementation method, as Figure 3As shown, the rotation direction and size of the robot joints are obtained by the inverse kinematics solution of the pose matrix and the MDH parameters, and the values of the joint angles θ1-θ6 are obtained for a six-degree-of-freedom general robot. The coordinate system of each joint is established by forward kinematics. In the MDH parameters, a represents the length of the connecting rod, and d represents the connecting rod offset. Thus, the circumscribed bounding box of the connecting rod under the coordinate system of each joint, i.e., the first joint connecting rod bounding box, can be established. The above base coordinate bounding box of each joint connecting rod is calculated according to the joint angle of each joint and the above first joint connecting rod bounding box.

[0042] In order to facilitate the simulation of the robot tool, in an alternative implementation method, the step S204 further includes:

[0043] In step S2044, a pose change matrix is obtained, which is a transformation matrix of the pose of the tool end to the pose of the tool coordinate system, and the tool coordinate system is a coordinate system with the tool center point as the origin;

[0044] In step S2045, a second coordinate system transformation matrix is calculated according to the pose change matrix and the first coordinate system transformation matrix, which is a matrix for coordinate conversion between the tool coordinate system and the robot base coordinate system;

[0045] In step S2046, a second joint connecting rod bounding box of each tool is established according to the size of the tool, and the second joint connecting rod bounding box is a bounding box of the tool in the tool coordinate system;

[0046] In step S2047, the base coordinate bounding box of each tool is calculated according to the second coordinate system transformation matrix and the second joint connecting rod bounding box.

[0047] In the above implementation method, as shown, Figure 3 the tool size length, width and height are measured as [T a , T b , T c ], respectively, and the eight corner points of the tool circumscribed bounding box (second joint connecting rod bounding box) in the tool coordinate system are represented as: Pc1=[Ta / 2, T b / 2, 0], Pc2=[-T a / 2, T b / 2, 0], Pc3=[T a / 2, -T b / 2, 0], Pc4=[-T a / 2, -T b / 2, 0], Pc5=[Ta / 2, T b / 2, T c ], Pc6=[-T a / 2, Tb / 2, T c ], Pc7 = [T a / 2, -T b / 2, T c ], Pc8 = [-T a / 2, -T b / 2, T c ], the rotation relationship R1 and the translation relationship O1 are obtained through the coordinate system conversion formula, the coordinate system transformation matrix is: [R, t] = [R1, O1], and it is known that the position of the robot tool center in the workpiece coordinate system is: [x n , y n , z n ] T = [R, t] -1 [x b , y b , z b ] T , wherein [x b , y b , z b ] represents the position of the robot tool center in the base coordinate system, and [x n , y n , z n ] represents the position in the workpiece coordinate system, and the robot tool target pose is known: , wherein n x , n y , n z , o x , o y , o z , a x , a y , a z are rotation matrix parameters, p x , p y , p z are translation parameters, and are used to describe the rotation and translation of the tool target pose represented in the base coordinate system; and Ttar = Ttcp·Ttool, wherein Ttcp is the pose of the tool center when the tool reaches the target position, Ttool is the pose transformation matrix of the tool end to the tool center, and Ttcp = Ttar·Ttool -1 , Pn = Ttcp·Pc, wherein Pn is an external bounding box corner point represented in the base coordinate system, and Pc is a set of eight corner points (Pc1-Pc8) represented in the TCP coordinate system.

[0048] In order to determine whether there is a collision, in an alternative implementation method, the above step S205 includes:

[0049] Step S2051, obtaining a first base coordinate bounding box and a second base coordinate bounding box, the first base coordinate bounding box and the second base coordinate bounding box being any two base coordinate bounding boxes;

[0050] Step S2052, calculating normal vectors of each face of the first base coordinate bounding box and normal vectors of each face of the second base coordinate bounding box to obtain a plurality of first normal vectors;

[0051] Step S2053, calculating normal vectors of all edge combinations to obtain a plurality of second normal vectors, the edge combinations including an edge of the first base coordinate bounding box and an edge of the second base coordinate bounding box;

[0052] Step S2054, calculating projection points of all vertices of the first base coordinate bounding box on the first normal vectors to obtain projections of each edge of the first base coordinate bounding box on the first normal vectors, calculating projection points of all vertices of the second base coordinate bounding box on the first normal vectors to obtain projections of each edge of the second base coordinate bounding box on the first normal vectors, calculating projection points of all vertices of the first base coordinate bounding box on the second normal vectors to obtain projections of each edge of the first base coordinate bounding box on the second normal vectors, and calculating projection points of all vertices of the second base coordinate bounding box on the second normal vectors to obtain projections of each edge of the second base coordinate bounding box on the second normal vectors;

[0053] Step S2055, determining that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the first normal vectors in a case where projections of any edge of the first base coordinate bounding box on the first normal vectors and projections of any edge of the second base coordinate bounding box on the first normal vectors do not overlap, and determining that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the second normal vectors in a case where projections of any edge of the first base coordinate bounding box on the second normal vectors and projections of any edge of the second base coordinate bounding box on the second normal vectors do not overlap;

[0054] Step S2056, determining that the joint corresponding to the first base coordinate bounding box and the joint corresponding to the second base coordinate bounding box do not collide in a case where the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the first normal vectors and the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the second normal vectors.

[0055] In the implementation method, the normal vector of each face of the two cuboids is calculated, and the normal vector of each edge of A and B is calculated; for each face of cuboid A, the normal vector of the face can be obtained through the cross product of two edges of the face:A = adge1xedge2, for each edge e of cuboid A and cuboid B a , e b , e AB = e a x e b , for each edge e of cuboid A and B, respectively, calculate the projection of all vertices of A and B on n A and n AB , respectively, if the projections of all edges do not overlap, then A and B do not overlap, that is, the relationship between the bounding box of each connecting rod of the body and the scene bounding box can be determined by using the separating axis theorem, if it is determined that none of them intersects, it is considered that there is no collision, otherwise it is considered that a collision occurs.

[0056] In order to prevent collision, in an optional implementation method, after determining whether collision occurs between the plurality of above-mentioned base coordinate bounding boxes and the above-mentioned joint members corresponding thereto by using the separating axis theorem, the above-mentioned method further comprises:

[0057] Step S301, in the case where it is determined that any two of the above-mentioned base coordinate bounding boxes collide, marking the above-mentioned target point as an unreachable point.

[0058] In the above-mentioned implementation method, the collision of any two of the above-mentioned base coordinate bounding boxes indicates that a collision will occur at the target point, and the above-mentioned target point is marked as an unreachable point. All unreachable points of the working space are found out, so as to avoid collision, or the target point is judged to be an unreachable point each time to avoid collision.

[0059] In order to establish the workpiece coordinate system, in an optional implementation method, the above-mentioned step S201 comprises:

[0060] Step S2011, controlling the tool center of the robot to move to a first point, a second point and a third point of the working plane in sequence to obtain a first coordinate, a second coordinate and a third coordinate, the first coordinate being the coordinate of the tool center in the robot base coordinate system when the tool center is located at the first point, the second coordinate being the coordinate of the tool center in the robot base coordinate system when the tool center is located at the second point, and the third coordinate being the coordinate of the tool center in the robot base coordinate system when the tool center is located at the third point, the first point, the second point and the third point being non-collinear;

[0061] Step S2012, calculating a first plane vector according to the first coordinate and the second coordinate, and calculating a second plane vector according to the first coordinate and the third coordinate;

[0062] Step S2013, calculating three unit orthogonal bases according to the first plane vector and the second plane vector;

[0063] Step S2014, the workpiece coordinate system is established according to the three unit orthogonal bases;

[0064] Step S2015, the robot workspace is constructed in the workpiece coordinate system according to the length, width and height of the workspace.

[0065] In the implementation method, the robot workspace is constructed by using a robot teaching method, assuming that a work plane required by the robot is xoy as shown in the shadow plane, the robot TCP is moved to points Ot, p1 and p2 respectively, and the positions are recorded, wherein Ot is any corner point on the work plane, p1 and p2 are any points on the edges adjacent to the corner point, as shown in the figure, further, two direction vectors A1 and A2 with Ot as the common point are obtained by the calculation formula: A1 = p1-Ot, A2 = p2-Ot; further, a set of unit orthogonal bases with Ot as the origin is established, as shown in the figure, and the calculation formula is: Figure 4 Figure 4 Figure 4 wherein, is a set of unit orthogonal bases based on the robot base coordinate system, as shown in the figure, and the reference coordinate system of the robot workspace has been established, the length, width and height of the workspace of the robot are confirmed as (a, b, d), Ot is taken as the origin of the coordinate system, and Figure 3 is taken as the reference coordinate system to establish a cubic workspace with the length, width and height of (a, b, d).

[0066] In order to calculate the conversion matrix, in an alternative implementation method, before the second pose is calculated according to the first pose and the first coordinate system conversion matrix, the method further comprises:

[0067] Step S401, the first coordinate system conversion matrix is calculated according to the three unit orthogonal bases and the unit orthogonal bases of the robot base coordinate system.

[0068] In the implementation method, a set of unit orthogonal bases of the robot base coordinate system is known: e1 = (0, 0, 1), e2 = (0, 1, 0), e3 = (1, 0, 0), the rotation relationship R1 and the translation relationship O1 are obtained by the coordinate system conversion formula, Ot is the vector from the origin to the point, the first coordinate system conversion matrix is: [R, t] = [R1, O1], and it is known that the position of the robot tool center in the workpiece coordinate system is: [x n , y n , z n ] T = [R, t] -1 [x b , y b , z b ]​​​​T wherein, [x b , y b , z b ] is the position of the robot tool center expressed in the base coordinate system, [x n , y n , z n ] is the representation of the position in the workpiece coordinate system.

[0069] It should be noted that the steps illustrated in the flowcharts of the drawings can be performed in a computer system such as a set of computer-executable instructions, and while the logical flow of the steps is shown in the order executed, in some instances, the steps performed can be performed in a different order than those described or shown.

[0070] The embodiments of the present application also provide a robot anti-collision device. It should be noted that the robot anti-collision device of the embodiments of the present application can be used to execute the robot anti-collision method provided by the embodiments of the present application. The device is used to implement the above embodiments and preferred embodiments, and those which have been described will not be repeated. As used below, the term "module" can be a combination of software and / or hardware that implements a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware, or a combination of software and hardware can also be implemented and conceived.

[0071] The robot anti-collision device provided by the embodiments of the present application is described below.

[0072] Figure 5 is a structural block diagram of a robot anti-collision device according to the embodiments of the present application. As Figure 5 shown, the device includes:

[0073] The establishment unit 10 is configured to establish a workpiece coordinate system and construct a robot workspace in the workpiece coordinate system.

[0074] The acquisition unit 20 is configured to acquire a first pose of a tool end of the robot moving to a target point of the robot workspace.

[0075] The first calculation unit 30 is configured to calculate a second pose according to the first pose and a first coordinate system transformation matrix, the second pose being a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix being a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system.

[0076] The second calculation unit 40 is configured to calculate a bounding box of each joint connecting rod and the tool of the robot in the robot base coordinate system according to the second pose, to obtain a plurality of base coordinate bounding boxes.

[0077] The determination unit 50 is used to determine whether the multiple base coordinate bounding boxes collide using the separation axis theorem.

[0078] In the aforementioned robot collision avoidance device, when the robot's tool end effector moves to the target point in the robot's workspace, a bounding box is constructed for the joint links and the robot's tool. Through coordinate transformation, all bounding boxes are converted to the robot's base coordinate system, resulting in multiple base coordinate bounding boxes. The separating axis theorem can then be used to determine whether a collision occurs between these multiple base coordinate bounding boxes. If a collision occurs, it indicates that the robot's tool end effector moving to the target point in the robot's workspace will cause a collision between the joint links and the robot's tool. This allows for collision avoidance during path planning, eliminating the need for 3D vision technology for collision avoidance, significantly reducing costs and solving the problem of high costs associated with existing 3D vision-based collision avoidance technologies. Furthermore, it avoids the low reliability of collision avoidance due to blind spots in 3D vision sensors.

[0079] To facilitate the simulation of joint links, in one optional implementation method, the aforementioned first calculation unit includes:

[0080] The first calculation module is used to calculate the joint angles of each joint based on the second pose using inverse kinematics.

[0081] The first establishment module is used to establish a first joint link bounding box for each of the aforementioned joint links based on the dimensions of each of the aforementioned joint links of the robot. The first joint link bounding box is the bounding box of the aforementioned joint link in the joint coordinate system corresponding to the aforementioned joint link.

[0082] The second calculation module is used to calculate the base coordinate bounding box of each joint link based on the joint angle of each joint and the bounding box of each of the first joint links.

[0083] In the above implementation methods, such as Figure 3 As shown, the rotation direction and magnitude of the robot joints are obtained through the pose matrix and the inverse kinematics solution of the MDH parameters. For a six-DOF general robot, the values ​​of joint angles θ1 to θ6 are obtained. The coordinate system of each joint is established through forward kinematics. It is known that in the MDH parameters, a represents the link length and d represents the link offset. Thus, the bounding box of the link in each joint coordinate system can be established, that is, the first joint link bounding box. The base coordinate bounding box of each joint link is calculated based on the joint angles of each joint and the first joint link bounding box.

[0084] To facilitate the use of tools for simulating robots, in one optional implementation, the first computing unit further includes:

[0085] The first acquisition module is used to acquire the pose transformation matrix, which is the transformation matrix from the pose of the tool end to the pose of the tool coordinate system, and the tool coordinate system is a coordinate system with the tool center point as the origin.

[0086] The third calculation module is used to calculate the second coordinate system transformation matrix based on the pose change matrix and the first coordinate system transformation matrix. The second coordinate system transformation matrix is ​​the matrix for coordinate transformation between the tool coordinate system and the robot base coordinate system.

[0087] The second creation module is used to create the second joint link bounding box of each of the tools according to the size of the tools. The second joint link bounding box is the bounding box of the tool in the tool coordinate system.

[0088] The fourth calculation module is used to calculate the base coordinate bounding box of each of the above tools based on the above-mentioned second coordinate system transformation matrix and the above-mentioned second joint link bounding box.

[0089] In the above implementation methods, such as Figure 3 As shown, the dimensions of the measuring tool are [T] (length, width, and height). a T b T c Then, the eight corner points of the tool's outer bounding box (second joint link bounding box) in the tool coordinate system are represented as: Pc1 = [Ta / 2, T b / 2,0],Pc2=[-T a / 2,T b / 2,0],Pc3=[T a / 2, -T b / 2,0],Pc4=[-T a / 2, -T b / 2, 0], Pc5 = [Ta / 2, T] b / 2,T c ], Pc6 = [-T a / 2,T b / 2,T c ], Pc7 = [T a / 2, -T b / 2,T c ], Pc8 = [-T a / 2, -T b / 2,T c The rotation relationship R1 and the translation relationship O1 are obtained through the coordinate system transformation formula. The transformation matrix is: [R, t] = [R1, O1]. Therefore, the position of the robot tool center in the workpiece coordinate system is: [x...]. n y n , z n ]T = [R, t] -1 [x b , y b , z b ] T where [x b , y b , z b ] is the position of the robot tool center expressed in the base frame, [x n , y n , z n ] is the position expressed in the workpiece frame, and the robot tool target pose is known: where n x , n y , n z , o x , o y , o z , a x , a y , a z are rotation matrix parameters, p x , p y , p z are translation parameters, for describing the rotation and translation of the tool target pose expressed in the base frame; and Ttar = Ttcp·Ttool, where Ttcp is the pose of the tool center when the tool reaches the target position, Ttool is the pose transformation matrix of the tool end to the tool center, and Ttcp = Ttar·Ttool -1 , Pn = Ttcp·Pc, where Pn is the corner point of the outer bounding box expressed in the base frame, and Pc is a set of eight corner points (Pc1-Pc8) expressed in the TCP frame.

[0090] To determine whether there is a collision, in an alternative implementation, the determination unit includes:

[0091] A second acquisition module is configured to acquire a first base coordinate bounding box and a second base coordinate bounding box, the first base coordinate bounding box and the second base coordinate bounding box being any two base coordinate bounding boxes.

[0092] A fifth calculation module is configured to calculate the normal vectors of the faces of the first base coordinate bounding box and the normal vectors of the faces of the second base coordinate bounding box to obtain a plurality of first normal vectors.

[0093] A sixth calculation module is configured to calculate the normal vectors of all edge combinations to obtain a plurality of second normal vectors, the edge combinations including an edge of the first base coordinate bounding box and an edge of the second base coordinate bounding box.

[0094] The seventh calculation module is used to calculate the projection points of all vertices of the first base coordinate bounding box onto the first normal vector, to obtain the projection of each side of the first base coordinate bounding box onto the first normal vector; calculate the projection points of all vertices of the second base coordinate bounding box onto the first normal vector, to obtain the projection of each side of the second base coordinate bounding box onto the first normal vector; calculate the projection points of all vertices of the first base coordinate bounding box onto the second normal vector, to obtain the projection of each side of the first base coordinate bounding box onto the second normal vector; calculate the projection points of all vertices of the second base coordinate bounding box onto the second normal vector, to obtain the projection of each side of the second base coordinate bounding box onto the second normal vector.

[0095] The first determining module is configured to determine that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the first normal vector when the projection of any side of the first base coordinate bounding box onto the first normal vector does not overlap with the projection of any side of the second base coordinate bounding box onto the first normal vector; and to determine that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the second normal vector when the projection of any side of the first base coordinate bounding box onto the second normal vector does not overlap with the projection of any side of the second base coordinate bounding box onto the second normal vector.

[0096] The second determining module is used to determine that the joint corresponding to the first base coordinate bounding box and the joint corresponding to the second base coordinate bounding box do not collide when the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the first normal vectors and the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the second normal vectors.

[0097] In the above implementation method, the normal vector of each face of the two cuboids is calculated, and the normal vector of each edge of A and B is calculated; for each face of cuboid A, its normal vector can be obtained by the cross product of the two edges on the face: n A = edge1 × edge2, for each edge e of cuboid A and cube B a e b The normal vector can be calculated as: n AB =e a ×e b For cuboids A and B, calculate the value of each cuboid at n vertices. A and n AB If the projections of all sides do not overlap, then A and B do not overlap. The relationship between the bounding boxes of each link of the body and the bounding box of the scene can be determined by using the separating axis theorem. If it is determined that no two links intersect, it is considered that there will be no collision; otherwise, it is considered that a collision will occur.

[0098] To prevent collision, in an alternative implementation, the device further comprises:

[0099] a marking unit configured to mark the target point as an unreachable point if it is determined that any two of the base coordinate bounding boxes collide with each other.

[0100] In the implementation, if any two of the base coordinate bounding boxes collide with each other, it indicates that the target point will collide with another object, and the target point is marked as an unreachable point. All the unreachable points in the work space are found out, and thus the collision is avoided, or the target point is determined to be an unreachable point each time the robot moves to avoid the collision.

[0101] To establish the work piece coordinate system, in an alternative implementation, the establishing unit comprises:

[0102] a control module configured to control the tool center of the robot to move to a first point, a second point and a third point on the work plane in sequence to obtain a first coordinate, a second coordinate and a third coordinate, the first coordinate being the coordinate of the tool center in the robot base coordinate system when the tool center is at the first point, the second coordinate being the coordinate of the tool center in the robot base coordinate system when the tool center is at the second point, and the third coordinate being the coordinate of the tool center in the robot base coordinate system when the tool center is at the third point, the first point, the second point and the third point not being collinear;

[0103] an eighth calculation module configured to calculate a first plane vector based on the first coordinate and the second coordinate, and calculate a second plane vector based on the first coordinate and the third coordinate;

[0104] a ninth calculation module configured to calculate three unit orthogonal bases based on the first plane vector and the second plane vector;

[0105] a third establishing module configured to establish the work piece coordinate system based on the three unit orthogonal bases;

[0106] a fourth establishing module configured to construct the robot work space in the work piece coordinate system based on the length, the width and the height of the work space.

[0107] In the implementation, the robot work space is constructed by using a robot teaching method. It is assumed that the work plane required by the robot is xoy as shown in the shadow plane, the TCP of the robot is moved to the points Ot, p1 and p2 respectively, and the positions are recorded, where Ot is any corner point on the work plane, and p1 and p2 are any points on the edges adjacent to the corner point. Figure 4 ​Figure 4 Further, two direction vectors A1 and A2 with Ot as the common point are obtained by the calculation formula: A1 = p1 - Ot, A2 = p2 - Ot; further, a set of unit orthogonal bases with Ot as the origin is established, as shown in Figure 4 The calculation formula is as follows: Wherein, is a set of unit orthogonal bases based on the robot base coordinate system, as shown in Figure 3 As shown, the reference coordinate system of the robot workspace has been established, and it is confirmed that the length, width, and height of the workspace of the robot are (a, b, d), with Ot as the origin of the coordinate system, and A cubic workspace with the length, width, and height of (a, b, d) is established based on the reference coordinate system.

[0108] In order to calculate the conversion matrix, an optional implementation method of the above device further comprises:

[0109] The third calculation unit is configured to calculate the first coordinate system conversion matrix based on the three unit orthogonal bases and the unit orthogonal bases of the robot base coordinate system before calculating the second pose based on the first pose and the first coordinate system conversion matrix.

[0110] In the above implementation method, a set of unit orthogonal bases of the robot base coordinate system is known: e1 = (0, 0, 1), e2 = (0, 1, 0), e3 = (1, 0, 0), the rotation relationship R1 is obtained by the coordinate system conversion formula, the translation relationship O1 is the vector from the Ot point to the origin, the first coordinate system conversion matrix is: [R, t] = [R1, O1], and thus it is known that the position of the robot tool center in the workpiece coordinate system is: [x n , y n , z n ] T = [R, t] -1 [x b , y b , z b ] T Wherein, [x b , y b , z b ] represents the position of the robot tool center in the base coordinate system, and [x n , y n , z n ] represents the position of the robot tool center in the workpiece coordinate system.

[0111] The robot anti-collision device includes a processor and a memory, and the establishment unit, the acquisition unit, the first calculation unit, the second calculation unit, and the determination unit are all stored in the memory as program units, and the corresponding functions are realized by the processor executing the program units stored in the memory. The modules are all located in the same processor, or the modules are located in different processors in any combination.

[0112] The processor includes a core, and the core calls the corresponding program units in the memory. The core can be one or more, and the problem of high cost of anti-collision by three-dimensional vision technology in the prior art can be solved by adjusting the core parameters.

[0113] The memory can include a non-permanent memory in a computer readable medium, a random access memory (RAM), and / or a non-volatile memory such as a read-only memory (ROM) or a flash memory (flash RAM), and the memory includes at least one memory chip.

[0114] The embodiment of the application provides a computer readable storage medium, and the computer readable storage medium includes a stored program, wherein the computer readable storage medium controls a device where the computer readable storage medium is located to execute the robot anti-collision method when the program runs.

[0115] Specifically, the robot anti-collision method includes:

[0116] In step S201, a workpiece coordinate system is established, and a robot workspace is constructed under the workpiece coordinate system.

[0117] In step S202, a first pose of a tool end of the robot moving to a target point of the robot workspace is acquired.

[0118] In step S203, a second pose is calculated according to the first pose and a first coordinate system transformation matrix, the second pose is a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix is a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system.

[0119] In step S204, a bounding box of each joint link and the tool of the robot in the robot base coordinate system is calculated according to the second pose, a plurality of base coordinate bounding boxes are obtained, the bounding box is a smallest cuboid wrapping the joint part, and the joint part includes the joint link and the tool.

[0120] In step S205, it is determined whether collision occurs between the joint parts corresponding to the plurality of base coordinate bounding boxes by using the separate axis theorem.

[0121] The embodiment of the present application provides a processor used for running a program, wherein the processor implements the robot anti-collision method when running the program.

[0122] Specifically, the robot anti-collision method comprises the following steps.

[0123] In step S201, a workpiece coordinate system is established, and a robot workspace is constructed under the workpiece coordinate system.

[0124] In step S202, a first pose of a tool end of the robot moving to a target point of the robot workspace is acquired.

[0125] In step S203, a second pose is calculated according to the first pose and a first coordinate system transformation matrix, the second pose is a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix is a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system.

[0126] In step S204, a bounding box of each joint connecting rod and the tool of the robot in the robot base coordinate system is calculated according to the second pose, a plurality of base coordinate bounding boxes are obtained, the bounding box is a smallest cuboid wrapping the joint member, and the joint member comprises the joint connecting rod and the tool.

[0127] In step S205, whether collision occurs between the joint members corresponding to the plurality of base coordinate bounding boxes is determined by using the separate axis theorem.

[0128] The embodiment of the present application provides a robot control system, which comprises a robot, a processor, a memory, and a program stored in the memory and capable of running on the processor, and at least the following steps are implemented when the processor executes the program:

[0129] In step S201, a workpiece coordinate system is established, and a robot workspace is constructed under the workpiece coordinate system.

[0130] In step S202, a first pose of a tool end of the robot moving to a target point of the robot workspace is acquired.

[0131] In step S203, a second pose is calculated according to the first pose and a first coordinate system transformation matrix, the second pose is a pose of the tool end in a robot base coordinate system, and the first coordinate system transformation matrix is a matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system.

[0132] Step S204, calculating the bounding box of each joint link and the tool of the robot in the robot base coordinate system according to the second pose, obtaining a plurality of base coordinate bounding boxes, the bounding box being the smallest cuboid wrapping the joint member, the joint member including the joint link and the tool;

[0133] Step S205, determining whether collision occurs between the joint members corresponding to the plurality of base coordinate bounding boxes by using the separate axis theorem.

[0134] The application also provides a computer program product adapted to execute the program of initializing at least the following method steps when executed on a data processing device:

[0135] Step S201, establishing a workpiece coordinate system and constructing a robot workspace in the workpiece coordinate system;

[0136] Step S202, obtaining a first pose of the tool end of the robot moving to a target point in the robot workspace;

[0137] Step S203, calculating a second pose according to the first pose and a first coordinate system transformation matrix, the second pose being the pose of the tool end in the robot base coordinate system, the first coordinate system transformation matrix being the matrix for coordinate conversion between the workpiece coordinate system and the robot base coordinate system;

[0138] Step S204, calculating the bounding box of each joint link and the tool of the robot in the robot base coordinate system according to the second pose, obtaining a plurality of base coordinate bounding boxes, the bounding box being the smallest cuboid wrapping the joint member, the joint member including the joint link and the tool;

[0139] Step S205, determining whether collision occurs between the joint members corresponding to the plurality of base coordinate bounding boxes by using the separate axis theorem.

[0140] Obviously, those skilled in the art should understand that the modules or steps of the application described above can be realized by general computing devices, which can be concentrated on a single computing device or distributed on a network composed of multiple computing devices, and they can be realized by program codes executable by computing devices, so that they can be stored in storage devices and executed by computing devices, and in some cases, the steps shown or described can be executed in different order, or they can be manufactured into individual integrated circuit modules, or multiple modules or steps can be manufactured into a single integrated circuit module. Thus, the application is not limited to any specific combination of hardware and software.

[0141] Those skilled in the art will appreciate that embodiments of the application can be readily used as software, hardware, or a combination of software and hardware. In a typical configuration, a computing device includes one or more processors (CPUs), input / output interfaces, network interfaces, and memory.

[0142] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 means for functionally implementing the steps listed in the flowchart block or blocks.

[0143] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 means for functionally implementing the steps listed in the flowchart block or blocks.

[0144] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks Figure 1 means for functionally implementing the steps listed in the flowchart block or blocks.

[0145] In a typical configuration, a computing device includes one or more processors (CPUs), input / output interfaces, network interfaces, and memory.

[0146] The memory can include non-persistent memory and / or persistent memory, such as flash memory, read-only memory (ROM), and / or volatile or non-volatile random access memory (RAM), among others. The memory is an example of computer readable media.

[0147] Computer-readable media includes permanent and non-permanent, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disc read-only memory (CD-ROM), digital versatile disc (DVD), or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information accessible to a computing device. According to the definition herein, computer readable media does not include transitory media such as modulated data signals and carriers.

[0148] It should also be noted that the terms "comprising", "including", or any other variant thereof are intended to cover non-exclusive inclusion, so that processes, methods, articles or devices that include a series of elements not only include those elements, but also include other elements not explicitly listed or inherent to such processes, methods, articles or devices. Without more limitations, the element defined by the statement "comprising a" does not exclude the presence of additional identical elements in the process, method, article or device that includes the element.

[0149] From the above description, it can be seen that the above-mentioned embodiments of the present application achieve the following technical effects:

[0150] 1) In the robot anti-collision method of the present application, when the tool end of the robot moves to the target point of the robot workspace, the bounding box of the joint connecting rod and the tool of the robot is constructed, and all the bounding boxes are converted to the robot base coordinate system through coordinate conversion, to obtain a plurality of base coordinate bounding boxes. Whether the plurality of base coordinate bounding boxes collide can be determined by using the separation axis theorem. If a collision occurs, it indicates that the movement of the tool end of the robot to the target point of the robot workspace will cause a collision between the joint connecting rod and the tool of the robot. Therefore, collision can be avoided during path planning, without using three-dimensional vision technology for anti-collision, greatly reducing the cost, and solving the problem of high cost of anti-collision by three-dimensional vision technology in the prior art. The problem of low reliability of anti-collision caused by the blind spot of three-dimensional vision sensor is also avoided.

[0151] 2) In the robot anti-collision device of the application, when the tool end of the robot moves to the target point of the robot workspace, the bounding box of the joint connecting rod and the tool of the robot is constructed, and all the bounding boxes are converted to the robot base coordinate system through coordinate conversion, to obtain multiple base coordinate bounding boxes. Whether the multiple base coordinate bounding boxes collide can be determined by using the separation axis theorem. If collision occurs, it indicates that the movement of the tool end of the robot to the target point of the robot workspace will cause collision between the joint connecting rod and the tool of the robot. Collision can be avoided during path planning, without using three-dimensional vision technology for anti-collision, greatly reducing the cost, and solving the problem of high cost of anti-collision through three-dimensional vision technology in the prior art. The problem of low reliability of anti-collision caused by the blind spot of the three-dimensional vision sensor is also avoided.

[0152] The above only describes the preferred embodiments of the application and is not intended to limit the application. Those skilled in the art can make various modifications and changes to the application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the application shall be included in the protection scope of the application.

Claims

1. A robot collision avoidance method, characterized in that, include: Establish a workpiece coordinate system, and construct the robot workspace under the workpiece coordinate system; Obtain the first pose of the robot's tool end effector as it moves to the target point in the robot's workspace; The second pose is calculated based on the first pose and the first coordinate system transformation matrix. The second pose is the pose of the tool end in the robot base coordinate system. The first coordinate system transformation matrix is ​​the matrix for coordinate transformation between the workpiece coordinate system and the robot base coordinate system. Based on the second pose, the bounding boxes of each joint link and the robot's tool in the robot's base coordinate system are calculated to obtain multiple base coordinate bounding boxes. The bounding box is the smallest cuboid that encloses the joint component, and the joint component includes the joint link and the tool. The separation axis theorem is used to determine whether a collision occurs between the joints corresponding to the multiple base coordinate bounding boxes. Calculating the bounding boxes of each joint link and the robot's tool in the robot's base coordinate system based on the second pose, resulting in multiple base coordinate bounding boxes, includes: obtaining a pose transformation matrix, which is a transformation matrix from the pose of the tool's end effector to the pose of the tool coordinate system, where the tool coordinate system is a coordinate system with the tool's center point as the origin; calculating a second coordinate system transformation matrix based on the pose transformation matrix and the first coordinate system transformation matrix, where the second coordinate system transformation matrix is ​​a coordinate transformation matrix between the tool coordinate system and the robot's base coordinate system; and establishing a second joint link bounding box for each tool based on the tool's dimensions, where the second joint link bounding box is the bounding box of the tool in the tool coordinate system. The base coordinate bounding box of each tool is calculated based on the second coordinate system transformation matrix and the second joint link bounding box.

2. The method according to claim 1, characterized in that, Based on the second pose, the bounding boxes of each joint link and the robot's tool in the robot's base coordinate system are calculated, resulting in multiple base coordinate bounding boxes, including: The joint angles of each joint are calculated using inverse kinematics based on the second pose. Based on the dimensions of each joint link of the robot, a first joint link bounding box is established for each joint link. The first joint link bounding box is the bounding box of the joint link in the joint coordinate system corresponding to the joint link. The base coordinate bounding box of each joint link is calculated based on the joint angle of each joint and the bounding box of each first joint link.

3. The method according to claim 1, characterized in that, Determining whether a collision occurs between the joints corresponding to the multiple base coordinate bounding boxes using the separation axis theorem includes: Obtain the first base coordinate bounding box and the second base coordinate bounding box, wherein the first base coordinate bounding box and the second base coordinate bounding box are any two of the base coordinate bounding boxes; Calculate the normal vectors of each face of the first base coordinate bounding box and the normal vectors of each face of the second base coordinate bounding box to obtain multiple first normal vectors; Calculate the normal vector of all edge combinations to obtain multiple second normal vectors. The edge combination includes one edge of the first base coordinate bounding box and one edge of the second base coordinate bounding box. Calculate the projection points of all vertices of the first base coordinate bounding box onto the first normal vector to obtain the projection of each edge of the first base coordinate bounding box onto the first normal vector. Calculate the projection points of all vertices of the second base coordinate bounding box onto the first normal vector to obtain the projection of each edge of the second base coordinate bounding box onto the first normal vector. Calculate the projection points of all vertices of the first base coordinate bounding box onto the second normal vector to obtain the projection of each edge of the first base coordinate bounding box onto the second normal vector. Calculate the projection points of all vertices of the second base coordinate bounding box onto the second normal vector to obtain the projection of each edge of the second base coordinate bounding box onto the second normal vector. If the projection of any side of the first base coordinate bounding box onto the first normal vector does not overlap with the projection of any side of the second base coordinate bounding box onto the first normal vector, then it is determined that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the first normal vector. If the projection of any side of the first base coordinate bounding box onto the second normal vector does not overlap with the projection of any side of the second base coordinate bounding box onto the second normal vector, then it is determined that the first base coordinate bounding box and the second base coordinate bounding box do not overlap on the second normal vector. If the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the first normal vectors and the first base coordinate bounding box and the second base coordinate bounding box do not overlap on all the second normal vectors, it is determined that the joint corresponding to the first base coordinate bounding box and the joint corresponding to the second base coordinate bounding box do not collide.

4. The method according to claim 1, characterized in that, After determining whether a collision occurs between the joints corresponding to the multiple base coordinate bounding boxes using the separation axis theorem, the method further includes: If any two of the base coordinate bounding boxes are determined to collide, the target point is marked as an unreachable point.

5. The method according to any one of claims 1 to 4, characterized in that, Establish a workpiece coordinate system, and construct a robot workspace within the workpiece coordinate system, including: The tool center of the robot is controlled to move sequentially to the first point, the second point, and the third point on the working plane to obtain the first coordinate, the second coordinate, and the third coordinate. The first coordinate is the coordinate of the tool center in the robot's base coordinate system when it is located at the first point, the second coordinate is the coordinate of the tool center in the robot's base coordinate system when it is located at the second point, and the third coordinate is the coordinate of the tool center in the robot's base coordinate system when it is located at the third point. The first point, the second point, and the third point are not collinear. A first plane vector is calculated based on the first coordinate and the second coordinate, and a second plane vector is calculated based on the first coordinate and the third coordinate. Three orthonormal bases are calculated based on the first plane vector and the second plane vector; Establish the workpiece coordinate system based on the three aforementioned orthogonal bases; The robot workspace is constructed in the workpiece coordinate system based on the length, width, and height of the workspace.

6. The method according to claim 5, characterized in that, Before calculating the second pose based on the first pose and the first coordinate system transformation matrix, the method further includes: The transformation matrix of the first coordinate system is calculated based on the three orthogonal bases and the orthogonal base of the robot base coordinate system.

7. A robot collision avoidance device, characterized in that, include: A unit is established to establish a workpiece coordinate system and construct a robot workspace under the workpiece coordinate system. The acquisition unit is used to acquire the first pose of the robot's tool end effector moving to the target point in the robot's workspace; The first calculation unit is used to calculate the second pose based on the first pose and the first coordinate system transformation matrix. The second pose is the pose of the tool end in the robot base coordinate system. The first coordinate system transformation matrix is ​​the matrix for coordinate transformation between the workpiece coordinate system and the robot base coordinate system. The second calculation unit is used to calculate the bounding boxes of each joint link and the robot's tool in the robot's base coordinate system according to the second pose, and to obtain multiple base coordinate bounding boxes. A determining unit is used to determine whether the multiple base coordinate bounding boxes collide using the separating axis theorem. The first calculation unit further includes: a first acquisition module, used to acquire a pose transformation matrix, wherein the pose transformation matrix is ​​a transformation matrix from the pose of the tool end to the pose of the tool coordinate system, and the tool coordinate system is a coordinate system with the tool center point as the origin; a third calculation module, used to calculate a second coordinate system transformation matrix based on the pose transformation matrix and the first coordinate system transformation matrix, wherein the second coordinate system transformation matrix is ​​a coordinate transformation matrix between the tool coordinate system and the robot base coordinate system; a second establishment module, used to establish a second joint link bounding box for each tool based on the size of the tool, wherein the second joint link bounding box is the bounding box of the tool in the tool coordinate system; and a fourth calculation module, used to calculate the base coordinate bounding box of each tool based on the second coordinate system transformation matrix and the second joint link bounding box.

8. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method described in any one of claims 1 to 6.

9. A robot control system, characterized in that, include: A robot, one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs comprising methods for performing any one of claims 1 to 6.

Images