Collision detection method for robot and its mechanical arm, control device

Abstract

The application relates to a robot and a collision detection method and a control device of a mechanical arm thereof. The robot comprises a first mechanical arm, a second mechanical arm and a control device configured to: based on structural features of the first and second mechanical arms, abstract and segment the first mechanical arm to form a first spatial line segment and abstract and segment the second mechanical arm to form a second spatial line segment; acquire a shortest distance between the first spatial line segment and the second spatial line segment; acquire a safety distance between the first spatial line segment and the second spatial line segment, the safety distance being configured based on a sum of a first inflation radius of the first spatial line segment and a second inflation radius of the second spatial line segment; compare the shortest distance with the safety distance, and determine whether a collision occurs between the first mechanical arm and the second mechanical arm based on a result obtained by comparison. Through the above implementation, the detection efficiency can be improved, and the collision between the mechanical arms can be avoided.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of medical devices, in particular to a robot and a collision detection method and control device for a mechanical arm thereof. BACKGROUND

[0002] With the development of society and the increasing cost of human labor, mechanical arms are increasingly widely used in industry and daily life.

[0003] With the increasing application requirements, a single mechanical arm cannot meet people's needs. In order to cope with this situation, multi-arm systems of mechanical arms have gradually entered the research field. Compared with single-arm systems, multi-arm systems need to consider the problem of mutual collision of mechanical arms during movement, because collision can cause damage to the mechanical arm including the body, motor or encoder, and reduce the service life of the mechanical arm. Especially in multi-arm systems of surgical robots, mutual collision between mechanical arms can easily reduce reliability and cause safety problems. SUMMARY

[0004] Therefore, it is necessary to provide a collision detection method and control device for a robot and a mechanical arm thereof, which has high collision detection efficiency and is beneficial to avoid safety problems caused by collision between mechanical arms.

[0005] The present application provides a collision detection method for a mechanical arm in a robot, the surgical robot comprising a first mechanical arm and a second mechanical arm, the method comprising:

[0006] Based on the obtained structural features of the first mechanical arm and the second mechanical arm, the first mechanical arm is abstracted and segmented to form a first spatial line segment, and the second mechanical arm is abstracted and segmented to form a second spatial line segment; the shortest distance between the first spatial line segment and the second spatial line segment is obtained, the two endpoints of the path corresponding to the shortest distance respectively terminating at the first spatial line segment and the second spatial line segment; the safety distance between the first spatial line segment and the second spatial line segment is obtained, the safety distance being configured based on the sum of the first inflation radius of the first spatial line segment and the second inflation radius of the second spatial line segment; the shortest distance and the safety distance are compared, and whether collision occurs between the first mechanical arm and the second mechanical arm is determined based on the result obtained by comparison.

[0007] The shortest distance between the first spatial line segment and the second spatial line segment is determined by: constructing a first equation based on the positions of two end points on the first spatial line segment, the first equation being used to represent the position of any point on the first spatial line segment; constructing a second equation based on the positions of two end points on the second spatial line segment, the second equation being used to represent the position of any point on the second spatial line segment; determining an over-determined equation based on the first equation and the second equation, the over-determined equation being associated with two end points of a path corresponding to the shortest distance; determining the positions of the two end points of the path corresponding to the shortest distance based on the over-determined equation; and determining the shortest distance based on the positions of the two end points of the path corresponding to the shortest distance.

[0008] The over-determined equation is determined based on the first equation and the second equation by: determining a third equation based on the first equation and the second equation, the third equation being associated with the distance between a point on the first spatial line segment and a point on the second spatial line segment; and converting the third equation into the over-determined equation based on a least square method or a gradient descent method.

[0009] The first spatial line segment includes one or more, and the second spatial line segment includes one or more. The comparison of the shortest distance and the safety distance is based on the results obtained by the comparison to determine whether a collision occurs between the first robot arm and the second robot arm. The comparison includes: comparing each of the first spatial line segments with each of the second spatial line segments, respectively, and determining whether a collision occurs between the first robot arm and the second robot arm based on a plurality of results obtained by the comparison.

[0010] The first spatial line segment includes one or more, and the second spatial line segment includes one or more. The comparison of the shortest distance and the safety distance is based on the results obtained by the comparison to determine whether a collision occurs between the first robot arm and the second robot arm. The comparison includes: obtaining a target task of the first robot arm and the second robot arm configured, determining one or more of the first spatial line segments and one or more of the second spatial line segments having a possibility of collision from the first spatial line segments and the second spatial line segments formed by segmentation based on the target task; comparing each of the determined first spatial line segments with each of the determined second spatial line segments, respectively, and determining whether a collision occurs between the first robot arm and the second robot arm based on a plurality of results obtained by the comparison.

[0011] The determining the one or more first spatial line segments and the one or more second spatial line segments with the collision possibility from the first spatial line segments and the second spatial line segments formed by the segmentation based on the target task comprises: when the target task obtained is a first task, determining the one or more first spatial line segments and the one or more second spatial line segments with the collision possibility from the first spatial line segments and the second spatial line segments formed by the segmentation based on the first task; or when the target task obtained is a second task, determining the one or more first spatial line segments and the one or more second spatial line segments with the collision possibility from the first spatial line segments and the second spatial line segments formed by the segmentation based on the second task.

[0012] The abstracting and segmenting the first robot arm and the second robot arm based on the structural features of the first robot arm and the second robot arm to form the first spatial line segments and the second spatial line segments comprises: obtaining a target task configured for the first robot arm and the second robot arm, and abstracting and segmenting the first robot arm and the second robot arm with the collision possibility to form the first spatial line segments and the second spatial line segments based on the target task and the structural features.

[0013] The target task comprises a joint degree of freedom constraint of the first robot arm and / or the second robot arm in a joint space.

[0014] The target task comprises a task degree of freedom constraint of the first robot arm and / or the second robot arm in a task space.

[0015] The method further comprises: stopping the motion control of the first robot arm and the second robot arm when it is determined that the collision between the first robot arm and the second robot arm occurs.

[0016] The structural features comprise a bending and flattening feature of the first robot arm and the second robot arm.

[0017] The comparing the shortest distance with the safety distance comprises: obtaining a collision probability between the structure of the first robot arm and the structure of the second robot arm; and comparing the first spatial line segments representing the structure of the first robot arm and the second spatial line segments representing the structure of the second robot arm in a descending order of the collision probability.

[0018] The application also provides a computer readable storage medium storing a computer program configured to be loaded and executed by a processor to implement the steps of the method according to any one of the above embodiments.

[0019] The application also provides a control device of a robot, comprising a memory for storing a computer program and a processor for loading and executing the computer program, wherein the computer program is configured to be loaded and executed by the processor to implement the steps of the method according to any one of the above embodiments.

[0020] The application also provides a robot, comprising a first mechanical arm, a second mechanical arm, and a control device configured to be coupled with the first mechanical arm and the second mechanical arm and configured to: abstract and segment the first mechanical arm to form a first spatial line segment and abstract and segment the second mechanical arm to form a second spatial line segment based on obtained structural features of the first mechanical arm and the second mechanical arm; obtain a shortest distance between the first spatial line segment and the second spatial line segment, the shortest distance corresponding to two endpoints of a path, the two endpoints respectively terminating at the first spatial line segment and the second spatial line segment; obtain a safety distance between the first spatial line segment and the second spatial line segment, the safety distance being configured based on a sum of a first inflation radius of the first spatial line segment and a second inflation radius of the second spatial line segment; and compare the shortest distance with the safety distance to determine whether a collision occurs between the first mechanical arm and the second mechanical arm based on a result obtained by the comparison.

[0021] The robot and the collision detection method and the control device of the mechanical arm of the robot have the following beneficial effects:

[0022] By abstracting and segmenting the first mechanical arm into a first spatial line segment, abstracting and segmenting the second mechanical arm into a second spatial line segment, and then comparing the obtained shortest distance between the first spatial line segment and the second spatial line segment with the configured safety distance, and further determining whether a collision occurs between the first mechanical arm and the second mechanical arm based on the comparison result, the detection method has low calculation amount and can improve detection efficiency, and the efficient detection method is conducive to avoiding safety problems caused by collisions between mechanical arms. BRIEF DESCRIPTION OF DRAWINGS

[0023] Figure 1 FIG. 1 is a structural schematic diagram of an embodiment of a surgical robot of the application;

[0024] Figure 2 FIG. 2 is a structural schematic diagram of an embodiment of a mechanical arm in the surgical robot of the application;

[0025] Figure 3 FIG. 3 is a flowchart of an embodiment of a collision detection method of the surgical robot of the application;

[0026] Figure 4 FIG. 4 is a flowchart of an embodiment of a collision detection method of the surgical robot of the application;

[0027] Figure 5 Fig. 1 is a state diagram of a first spatial line segment and a second spatial line segment of the present application;

[0028] Figure 6 Fig. 2 is a flow chart of an embodiment of a collision detection method of a surgical robot of the present application;

[0029] Figure 7 Fig. 3 is a structure diagram of a control device of a surgical robot of an embodiment of the present application. DETAILED DESCRIPTION

[0030] In order to facilitate the understanding of the present application, the present application will be described in more detail below with reference to the relevant drawings. The drawings show preferred embodiments of the present application. However, the present application can be implemented in many different forms and is not limited to the embodiments described herein. On the contrary, the purpose of providing these embodiments is to make the disclosure of the present application more thorough and comprehensive.

[0031] It should be noted that when an element is referred to as being "on" another element, it can be directly on the other element or there can be an intervening element. When an element is referred to as being "connected" to another element, it can be directly connected to the other element or there can be an intervening element. When an element is referred to as being "coupled" to another element, it can be directly coupled to the other element or there can be an intervening element. The terms "vertical", "horizontal", "left", "right" and similar expressions used herein are for illustrative purposes only and are not intended to be the only implementation. The terms "distal" and "proximal" used herein as directional terms are conventional terms used in the field of interventional medical devices, wherein "distal" means closer to the patient during a procedure and "proximal" means further away from the patient during a procedure. The terms "first", "second", and the like used herein denote one component and a class of two or more components having common characteristics.

[0032] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used in the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the present application. As used in the present application, the term "and / or" includes any and all combinations of one or more of the associated listed items. As used in the present application, the term "each" includes one or two or more. As used in the present application, the term "plurality" means two or more.

[0033] This application provides a robot. The robot includes multiple robotic arms that can be supported on a wall or hospital bed, or suspended from the ceiling. In some embodiments, the robot includes an industrial robot and a surgical robot. In some embodiments, the surgical robot includes a first robotic arm, a second robotic arm, and a movable base. The first and second robotic arms are supported on the base, and by moving the base, the first and second robotic arms can be moved as a whole. In some embodiments, the surgical robot includes a single-port surgical robot or a multi-port surgical robot. In some embodiments, such as... Figure 1 As shown, the surgical robot includes a catheter robot 1. A first manipulator (not shown) is located at the distal end of the first robotic arm 11 of the catheter robot 1, and a second manipulator (not shown) is located at the distal end of the second robotic arm 13. The first manipulator is equipped with and manipulates the inner catheter instrument 12 to perform bending movements, and the second manipulator is equipped with and manipulates the outer catheter instrument 14 to perform bending movements. The inner sheath of the inner catheter instrument 13 is inserted into the hollow outer sheath of the outer catheter instrument 14. Both instruments work together and are inserted into the body of the patient P via a guide 3 connected to the patient P lying on the operating table 2, to perform functions such as imaging, biopsy, or other surgical procedures or to assist in surgery. In some embodiments, the feeding movements of the inner catheter instrument 12 and the outer catheter instrument 14 can be achieved by means of the movement of the first robotic arm 11 and the second robotic arm 13.

[0034] In some embodiments, the first robotic arm and the second robotic arm have the same or different configurations; for example, both include the same configuration. In some embodiments, the first robotic arm and the second robotic arm include the same degrees of freedom. In some embodiments, such as Figure 2 As shown, the first robotic arm 11 and the second robotic arm 13 have redundant degrees of freedom, for example, both have more than 7 degrees of freedom to fully meet diverse motion control needs.

[0035] For example, such as Figure 2As shown, the first robot arm 11 and the second robot arm 13 have the same configuration, and the first robot arm 11 is taken as an example for illustration. The first robot arm 11 includes seven rotating joints J1-J7 and connecting rods lk1-lk7 connected between adjacent rotating joints. The seven rotating joints J1-J7 can be classified into two categories according to the rotating direction. The first category includes four rotating joints J1, J3, J5 and J7 rotating around a first axis, and the second category includes three rotating joints J2, J4 and J6 rotating around a second axis. The first axis and the second axis can intersect, such as orthogonal or oblique, so as to reduce the volume of the robot arm. Through cooperation of the seven rotating joints J1-J7, the end of the first robot arm 11, i.e., the first manipulator, can realize movement in any task degree of freedom in the Cartesian space. Of course, more or fewer rotating joints can be provided in other robot arms, and these rotating joints can be limited to two categories of rotating directions or more or fewer categories of rotating directions, which are not limited herein. The second robot arm 13 also includes seven rotating joints J1'-J7' and connecting rods connected between adjacent rotating joints. The classification and arrangement of the rotating joints J1'-J7' of the second robot arm 13 are basically the same as those of the rotating joints J1-J7 of the first robot arm 11, which will not be repeated here.

[0036] In some embodiments, the surgical robot includes a control device. The control device includes one or more processors. The one or more processors are coupled with the first robot arm 11 and the second robot arm 13. The one or more processors can be integrated in the surgical robot. The one or more processors can also be independent of the surgical robot. The one or more processors can also be deployed in the cloud.

[0037] In some embodiments, the present application provides a collision detection method for a robot arm in a robot, for example, in a surgical robot. The method is configured to be executed by a control device, and the method includes:

[0038] In step S11, based on the obtained configuration information, the first robot arm is abstracted and segmented to form a first spatial line segment, and the second robot arm is abstracted and segmented to form a second spatial line segment.

[0039] In some embodiments, the configuration information includes configuration information associated with the first spatial line segment of the first robot arm and configuration information associated with the second spatial line segment of the second robot arm. These configuration information, for example, includes the structure of the robot arm corresponding to the representation of the first spatial line segment and the second spatial line segment. For example, these configuration information can be manually input by a doctor or an assistant. For another example, these configuration information can be stored in a configuration file, and these configuration information can be obtained by reading the configuration file.

[0040] For example, the configuration information includes structural features of the robotic arms. For example, the first robotic arm can be abstracted and segmented into first spatial segments based on structural features of the first robotic arm, and the second robotic arm can be abstracted and segmented into second spatial segments based on structural features of the second robotic arm. The structural features of the robotic arms can be pre-stored in a configuration file, such as a model file of the catheter robot, and then automatically obtained from the configuration file.

[0041] In some embodiments, the structural features of the robotic arms include classification features. The classification features include features of links and joints. The respective robotic arms can be abstracted and segmented based on the classification features. For example, different links can be abstracted and segmented into different spatial segments, and different joints can be abstracted and segmented into different spatial segments based on a first configuration, i.e., different links and different joints can be respectively abstracted and segmented into different spatial segments. For another example, an immediately adjacent joint and a link driven by the joint can be abstracted and segmented into a same spatial segment based on a second configuration, and other joints and links of the robotic arm can be abstracted and segmented into other spatial segments based on the same principle.

[0042] In some embodiments, the structural features of the robotic arms include configuration features. For example, the configuration features include line features of the robotic arms, which are used to reflect the features of bending and straightening of the robotic arms. In some embodiments, the respective robotic arms can be abstracted and segmented based on the configuration features, for example, based on the line features. For example, one or more structures of the robotic arms that are substantially (i.e., approximately) on the same straight line can be abstracted and segmented into the same spatial segment, and one or more structures of the robotic arms that are substantially on different straight lines can be abstracted and segmented into different spatial segments.

[0043] Continuing to refer to Figure 2For example, the first mechanical arm 11 is taken as an example, in the first mechanical arm 11, since the connecting rod lk1, the rotating joint J1 and the connecting rod lk2 are basically in the same straight line, the three can be abstracted and divided into the first space segment 1a; since the connecting rod lk2, the rotating joint J2 and the connecting rod lk3 are basically in the same straight line, the three can be abstracted and divided into the first space segment 2a; since the connecting rod lk3, the rotating joint J3 and the connecting rod lk4 are basically in the same straight line, the three can be abstracted and divided into the first space segment 3a; since the connecting rod lk4, the rotating joint J4 and the connecting rod lk5 are basically in the same straight line, the three can be abstracted and divided into the first space segment 4a; since the connecting rod lk5, the rotating joint J5 and the connecting rod lk6 are basically in the same straight line, the three can be abstracted and divided into the first space segment 5a; since the connecting rod lk6, the rotating joint J6 and the connecting rod lk7 are basically in the same straight line, the three can be abstracted and divided into the first space segment 6a; since the connecting rod lk7 is a straight line, it can be abstracted and divided into the first space segment 7a. The abstraction and division of the second space segment of the second mechanical arm can refer to the abstraction and division of the first mechanical arm. Similarly, the second mechanical arm 13 can also be abstracted and divided into corresponding 1b-7b. For example, the first space segment 1a can include the entire connecting rod lk1, the entire rotating joint J1 and part of the connecting rod lk2 which are basically in the same straight line; for example, the first space segment 2a can include part of the connecting rod lk2, the entire rotating joint J4 and part of the connecting rod lk3 which are basically in the same straight line. That is, when the mechanical arm has a bending structure, the bending structure can be abstracted and divided into two different space segments.

[0044] In step S12, the shortest distance between the first space segment and the second space segment is obtained.

[0045] The two endpoints of the shortest distance corresponding path are located in the first space segment and the second space segment respectively, that is, one endpoint of the shortest distance corresponding path is located in the first space segment and the other endpoint is located in the second space segment. It can be understood that, that is, one of the two endpoints is a point on the first space segment and the other is a point on the second space segment. For simplicity of description, the endpoints of the shortest distance corresponding path can be simplified as the endpoints of the shortest distance. For example, the endpoint of the shortest distance is one of the two endpoints of the first space segment, or it can be any point in the region between the two endpoints of the first space segment.

[0046] In step S13, the safety distance between the first space segment and the second space segment is obtained.

[0047] In some embodiments, the expansion radius corresponding to each spatial line segment can be obtained by manual input from a doctor or an assistant, or can be automatically obtained from a configuration file. In some embodiments, the spatial line segments can represent the central axes of straight structures such as links or joints. In some embodiments, a first spatial line segment can be simulated to expand to form a first cylinder to equivalently represent the structure of a first robotic arm, and a second spatial line segment can be simulated to expand to form a second cylinder to equivalently represent the structure of a second robotic arm. The first cylinder has a first expansion radius, the second cylinder has a second expansion radius, and the safety distance is configured based on the sum of the first expansion radius and the second expansion radius. That is, the safety distance is related to the sum of the first expansion radius and the second expansion radius, for example, the safety distance is equal to the sum of the first expansion radius and the second expansion radius. For another example, the safety distance is equal to the product of the sum of the first expansion radius and the second expansion radius and a safety factor greater than 1. By setting the safety factor, a redundancy can be provided for collision detection.

[0048] The first expansion radius corresponds to the radial dimension of the structure represented by the first spatial line segment, and the second expansion radius corresponds to the radial dimension of the structure represented by the second spatial line segment. For example, the first expansion radius can be equal to or slightly greater than the radial dimension of the structure represented by the first spatial line segment, and the second expansion radius can be equal to or slightly greater than the radial dimension of the structure represented by the second spatial line segment. Further, the first expansion radius can be obtained based on the radial dimension of the structure represented by the first spatial line segment, and the second expansion radius can be obtained based on the radial dimension of the structure represented by the second spatial line segment. The radial dimensions of different structures of the robotic arm can be pre-stored in a configuration file for automatic acquisition, or can be input by a doctor or an assistant.

[0049] In some embodiments, the structure of the robotic arm along the axial direction of each spatial line segment can generally have different radial dimensions. Therefore, the first expansion radius can be the maximum radial dimension of the structure represented by the first spatial line segment, and the second expansion radius can be the maximum radial dimension of the structure represented by the second spatial line segment. In some embodiments, the first expansion radius can be the sum of the maximum radial dimension of the structure represented by the first spatial line segment and a bias dimension, and the second expansion radius can be the sum of the maximum radial dimension of the structure represented by the second spatial line segment and a bias dimension. The bias dimension can be understood as a fixed value to amplify the maximum radial dimension, and the bias dimension can be configured based on requirements.

[0050] In step S14, the shortest distance is compared with the safety distance, and based on the comparison result, it is determined whether a collision occurs between the first robotic arm and the second robotic arm.

[0051] Wherein, by considering the first spatial line segment along its axial direction according to the first expansion radius and considering the second spatial line segment along its axial direction according to the second expansion radius, it is equivalent to equate the first spatial line segment to a cylinder and equate the second spatial line segment to another cylinder, and whether the two cylinders are in contact is considered to determine whether the collision occurs.

[0052] In some embodiments, the method of comparing the shortest distance and the safety distance includes comparing the size of the two or comparing the ratio of the two. For example, when comparing the size, if the result obtained is that the shortest distance is less than or equal to the safety distance, it is determined that the first spatial line segment and the second spatial line segment collide, and it is further determined that the first robot arm and the second robot arm collide. If the result obtained is that the shortest distance is greater than the safety distance, it is determined that the first spatial line segment and the second spatial line segment do not collide, and it is further determined that the first robot arm and the second robot arm do not collide. For another example, when comparing the ratio, if the result obtained is that the ratio of the shortest distance to the safety distance is greater than or equal to 1, it is determined that the first spatial line segment and the second spatial line segment collide, and it can be further determined that the first robot arm and the second robot arm collide. If the result obtained is that the ratio of the shortest distance to the safety distance is less than 1, it is determined that the first spatial line segment and the second spatial line segment do not collide, and it can be further determined that the first robot arm and the second robot arm do not collide.

[0053] Through the above steps S11-S14, that is, by abstracting and dividing the first robot arm into a first spatial line segment, abstracting and dividing the second robot arm into a second spatial line segment, comparing the shortest distance between the obtained first spatial line segment and the second spatial line segment with the configured safety distance, and further determining whether the first robot arm and the second robot arm collide based on the comparison result, the detection method has low calculation amount and can improve detection efficiency. The efficient detection method is conducive to avoiding safety problems caused by collision.

[0054] In some embodiments, the two endpoints of the shortest distance can be defined as the first target point and the second target point, respectively. Since the two endpoints of the shortest distance terminate at the first spatial line segment and the second spatial line segment, respectively, and the straight line between the two points is the shortest, assuming that the first target point terminates at the first spatial line segment and the second target point terminates at the second spatial line segment, the position of the first target point on the first spatial line segment and the position of the second target point on the second spatial line segment can be determined, and the shortest distance can be obtained based on the positions of the first target point and the second target point.

[0055] In some embodiments, in combination with Figure 3 Referring to step S12, obtaining the shortest distance between the first spatial line segment and the second spatial line segment includes:

[0056] Step S121, constructing a first equation representing a position of any point on the first spatial line segment, and constructing a second equation representing a position of any point on the second spatial line segment.

[0057] The first equation can be constructed based on positions of two end points of the first spatial line segment, and the second equation can be constructed based on positions of two end points of the second spatial line segment. The first equation can represent any point on the first space, and thus can represent the first target point to be determined. The second equation can represent any point on the second space, and thus can represent the second target point to be determined.

[0058] Step S122, determining an over-determined equation based on the first equation and the second equation.

[0059] The over-determined equation is also referred to as a statically overdetermined equation, and is associated with two end points of the shortest distance, i.e., the first target point and the second target point to be determined. In some embodiments, an optimization method for solving the calculation problem, i.e., the shortest distance optimal solution, can be obtained based on the first equation and the second equation, and by using a method of deriving a loss function to find a minimum value. Examples of the method of deriving a loss function to find a minimum value include one of a least square method, a gradient descent method, etc.

[0060] For example, a third equation associated with a distance between a point on the first spatial line segment and a point on the second spatial line segment can be determined based on the first equation and the second equation. Then, the third equation can be converted into the required over-determined equation based on a method of deriving a loss function to find a minimum value, such as the least square method or the gradient descent method.

[0061] Step S123, determining positions of two end points of the shortest distance based on the over-determined equation.

[0062] The positions of the first target point and the second target point can be determined by solving the over-determined equation.

[0063] Step S124, determining the shortest distance between the first spatial line segment and the second spatial line segment based on the positions of the two end points of the shortest distance.

[0064] The shortest distance can be determined according to the positions of the first target point and the second target point.

[0065] For ease of understanding, the above steps S122-S124 can be exemplarily described in combination with embodiments. As shown in Figure 4 The first spatial line segment is line1, and the second spatial line segment is line2. The first spatial line segment line1 includes a first end point position P0 and a second end point position P1. The second spatial line segment line2 includes a first end point position Q0 and a second end point position Q1.

[0066] Since the first target point can be any point on the first spatial line segment, any point position P on the first spatial line segment line1 can be constructed based on the first end point position P0 and the second end point position P1 into the following equation:

[0067]

[0068] Since the second target point can be any point on the second spatial line segment, any point position Q on the second spatial line segment line2 can be constructed based on the first end point position Q0 and the second end point position Q1 into the following equation:

[0069]

[0070] The distance between any point position P on the first spatial line segment line1 and any point position Q on the second spatial line segment line2 can be expressed into the following equation:

[0071]

[0072] Taking square of both sides of the above equation (3) can obtain the following equation:

[0073] |PQ| 2 =(P-Q) 2 (4)

[0074] Converting the above equation (4) into a minimum value problem, for example, converting into a minimum value problem of least square method, can obtain the following equation:

[0075] min(P-Q) 2 (5)

[0076] Substituting the information of the above any point positions P and Q into the above equation (5), i.e. substituting the above equations (1) and (2) into the above equation (5), can obtain the following equation:

[0077] min(α(P0-P1)-β(Q0-Q1)+P1-Q1) 2 (6)

[0078] Based on the above equation (6), it can be converted into a solution of over-determined equation, specifically as follows:

[0079] α(P0-P1)-β(Q0-Q1)=Q1-P1 (7)

[0080] For convenience of solution, the above equation (7) can be converted into a matrix:

[0081]

[0082] Let A=(P0-P1,Q0-Q1), b = Q1 - P1, and the above equation (8) can be converted into:

[0083] Ax = b (9)

[0084] The above equation (9) can be multiplied by the transpose matrix A on both sides: T and the following equation is obtained:

[0085] A T Ax = A T b (10)

[0086] In the above equation (10), if A T A is invertible, i.e., det(A T A)≠0, the above equation (10) can be further converted into the following normal equation:

[0087] x = (A T A) -1 A T b (11)

[0088] In the above equation (10), if A T A is not invertible, i.e., det(A T A) = 0, the above equation (10) can be further converted into the following equation by using the generalized inverse of A T A:

[0089] x = pinv(A T A)A T b (12)

[0090] Solving the above equation (12) gives the value of x, i.e., the current values of α and β.

[0091] In view of the above equations (1) and (2), the values of α and β need to satisfy:

[0092]

[0093] The current values of α and β calculated are processed as follows:

[0094] If 0 < α < 1, α takes its calculated current value; if α > 1, α = 1; if α < 0, α = 0;

[0095] If 0 < β < 1, β takes its calculated current value; if β > 1, β = 1; if β < 0, β = 0.

[0096] Further, based on the obtained a, b, the first end position P0, the second end position P1, and the first end position Q0, the second end position Q1, the first target point position P and the second target point position Q corresponding to the shortest distance can be determined, and further, based on the first target point position P and the second target point position Q, the shortest distance of |PQ| in space can be calculated.

[0097] In some embodiments, assuming that the first expansion radius of the first spatial line segment line1 is r1, the second expansion radius of the second spatial line segment line2 is r2, and the safety factor is s, where s≥1, the comparison relationship between |PQ| and (r1+r2)*s can be determined, for example, in terms of comparison size:

[0098] If |PQ|>(r1+r2)*s, it can be determined that no collision occurs between the first spatial line segment line1 and the second spatial line segment line2, i.e., it can be determined that no collision occurs between the first robot arm and the second robot arm.

[0099] If |PQ|≤(r1+r2)*s, it can be determined that a collision occurs between the first spatial line segment line1 and the second spatial line segment line2, i.e., it can be determined that a collision occurs between the first robot arm and the second robot arm.

[0100] In some embodiments, according to the configuration, for the first robot arm, different structures of the first robot arm are usually represented by different first spatial line segments, i.e., the first spatial line segments can include one or more. For the second robot arm, different structures of the second robot arm are usually represented by different second spatial line segments, i.e., the second spatial line segments can also include one or more. In some embodiments, the first spatial line segments can have overlapping regions or no overlapping regions; the second spatial line segments can have overlapping regions or no overlapping regions; whether the first spatial line segments and / or the second spatial line segments have overlapping regions when being divided does not affect the implementation of the technical solutions described in the present application.

[0101] Whether a collision occurs between one or more structures of the first robot arm and one or more structures of the second robot arm can be configured to be determined by judging whether a collision occurs between the corresponding first spatial line segment and the corresponding second spatial line segment, for example, each first spatial line segment is compared with each second spatial line segment, and based on the multiple results obtained by comparison, it is determined whether a collision occurs between the first robot arm and the second robot arm. Wherein, when any first spatial line segment collides with any second spatial line segment, it can be determined that a collision occurs between the first robot arm and the second robot arm; when any first spatial line segment does not collide with any second spatial line segment, it can be determined that no collision occurs between the first robot arm and the second robot arm.

[0102] In some embodiments, the position of any point on the robot arm can be calculated based on the principle of kinematics in a reference coordinate system, such as the base coordinate system of the robot arm. Since the spatial line segment has correspondence or correlation with the structure of the robot arm it represents, the position of the corresponding end point of the spatial line segment can be determined by obtaining the position of the target point on the robot arm corresponding to or correlated with the target end point of the spatial line segment. For example, the process of obtaining the target end point of the target spatial line segment of the target robot arm briefly includes:

[0103] obtaining the kinematic model of the target robot arm and the current joint variable of the joint in the target robot arm, the current joint variable including the current joint angle and / or joint displacement; determining the position of the target point corresponding to or correlated with the target end point of the target spatial line segment in the target robot arm in the reference coordinate system based on the kinematic model of the target robot arm and the current joint variable thereof; and further determining the position of the target end point in the reference coordinate system based on the position of the target point, the correspondence or correlation between the target point and the target end point of the target spatial line segment.

[0104] Based on the above method, the positions of the two end points of any one or more spatial line segments in the target robot arm, such as the first robot arm and the second robot arm, in the reference coordinate system can be obtained.

[0105] In some embodiments, a continuous arm body of the target robot arm can be completely or incompletely abstracted and segmented according to the configuration. The continuous arm body includes a plurality of adjacent links and joints. Complete abstraction and segmentation can be understood as not ignoring any structure in the continuous arm body, i.e., completely characterizing the main structure of the continuous arm body. Incomplete abstraction and segmentation can be understood as ignoring at least part of the structure in the continuous arm body, which includes, for example, a complete link, a complete joint, a part of a link, and a part of a joint, i.e., incompletely characterizing the main structure of the continuous arm body.

[0106] In some embodiments, when the catheter robot is configured to perform different tasks (i.e., target tasks), more specifically, when the robot arm of the catheter robot, such as the first robot arm and / or the second robot arm, is configured to perform different tasks, the abstraction and segmentation of the first spatial line segment of the first robot arm and / or the second spatial line segment of the second robot arm can be fixed, i.e., the formed first spatial line segment and / or second spatial line segment can not change due to different configurations of the tasks. At this time, when performing collision detection, it is necessary to determine whether the corresponding first spatial line segment and second spatial line segment collide based on the comparison results of the shortest distance between the fixed first spatial line segment and second spatial line segment and their safety distances, respectively.

[0107] In some embodiments, the catheter robot is configured to perform different tasks, and the abstraction and segmentation of the first spatial line segment of the first robotic arm and / or the second spatial line segment of the second robotic arm can be flexible in view of different tasks corresponding to different collision positions between the first robotic arm and the second robotic arm where collisions can potentially occur, i.e., the abstraction and segmentation of the first robotic arm and / or the second robotic arm can be based on a target task.

[0108] For example, when the catheter robot is configured to perform a first task, in response to obtaining the first task, the abstraction and segmentation of the first spatial line segment of the first robotic arm and / or the second spatial line segment of the second robotic arm is performed according to a first configuration associated with the first task, i.e., based on the first task, one or more first spatial line segments and one or more second spatial line segments having collision possibilities are determined from the first spatial line segments and the second spatial line segments formed by the segmentation. When the catheter robot is configured to perform a second task, in response to obtaining the second task, the abstraction and segmentation of the first spatial line segment of the first robotic arm and / or the second spatial line segment of the second robotic arm is performed according to a second configuration associated with the second task, i.e., based on the second task, one or more first spatial line segments and one or more second spatial line segments having collision possibilities are determined from the first spatial line segments and the second spatial line segments formed by the segmentation.

[0109] Wherein, based on the abstraction and segmentation of the first robotic arm and / or the second robotic arm for the first task and the second task, the results of the first spatial line segments and / or the second spatial line segments formed by the two respectively are generally different. These different results include: (1) the structures of the first robotic arm represented by the formed first spatial line segments, and / or the structures of the second robotic arm represented by the formed second spatial line segments are different; and / or, (2) the number of the formed first spatial line segments, and / or the number of the formed second spatial line segments are different. Wherein, in order to reduce the computational amount of collision detection, the structures of the robotic arms where no collision can occur can be excluded as much as possible, and only the structures of the robotic arms where collision can occur are abstracted and segmented to form spatial line segments. Wherein, the structures of the first robotic arm and the second robotic arm where collision can occur under different tasks can be statistically or simulated in advance, and then based on the results of the statistics or simulation, the structures where no collision can occur under the corresponding task can be excluded, and finally the corresponding robotic arm is abstracted and segmented based on the remaining structures where collision can occur under the corresponding task to form the spatial line segments required under the task.

[0110] In this embodiment, as long as the first spatial line segment formed includes the structure where the collision position in the first robot arm being characterized is likely to collide, and the second spatial line segment includes the structure where the collision position in the second robot arm being characterized is likely to collide. In some embodiments, the first spatial line segment only includes the structure where the collision position in the first robot arm being characterized is likely to collide, and the second spatial line segment only includes the structure where the collision position in the second robot arm being characterized is likely to collide. According to different target tasks, the first spatial line segment and / or the second spatial line segment can be configured accordingly, which can greatly reduce the amount of calculation and improve the immediacy of collision detection.

[0111] In some embodiments, based on the different target tasks obtained, the first spatial line segment and / or the second spatial line segment suitable for different target tasks can be automatically configured. In some embodiments, the target task can be associated with the target joint degree of freedom of the target robot arm in the joint space. In some embodiments, the target task can be associated with the target task degree of freedom of the distal end of the target robot arm in the six-dimensional task space (or Cartesian space, workspace). If different target joint degrees of freedom or target task degrees of freedom are constrained to the target robot arm, the configuration and / or motion trajectory of the target robot arm will usually change, and thus the collision position between the robot arms can also change. Therefore, considering the possible changes in the collision position between the robot arms, the first spatial line segment and / or the second spatial line segment can be reconfigured. When reconfiguring the first spatial line segment and / or the second spatial line segment, the first robot arm can be abstracted and segmented to form a suitable first spatial line segment according to the structural characteristics of the first robot arm based on the configured target task, and / or the second robot arm can be abstracted and segmented to form a suitable second spatial line segment according to the structural characteristics of the second robot arm based on the configured target task. The suitable first spatial line segment and / or the suitable second spatial line segment can also be determined from the first spatial line segment formed by abstracting and segmenting the continuous arm body of the first robot arm and / or the second spatial line segment formed by abstracting and segmenting the continuous arm body of the second robot arm based on the configured target task. Here, "suitable" exemplarily refers to the spatial line segment that characterizes the structure of the robot arm that is likely to collide.

[0112] Continuing to refer to Figure 2The first robot arm is completely abstracted and segmented to form original space segments including the first space segments 1a-7a, and the second robot arm is completely abstracted and segmented to form original space segments including the second space segments 1b-7b. Based on this, in some embodiments, when the target task is to limit or allow the partial joint freedom of the revolute joints in at least one of the first robot arm and the second robot arm, for example, to limit the movement of the revolute joints J1, J2, J3, J4 in the first robot arm and to limit the movement of the revolute joints J1', J2', J3', J4' in the second robot arm, according to statistical data or simulation results, under the target task, the first space segments 1a, 2a, 3a, 4a will not collide with the second space segments 1b, 2b, 3b, 4b, the first space segments 5a, 6a, 7a will not collide with the second space segments 1b, 2b, 3b, 4b, and the second space segments 5b, 6b, 7b will not collide with the first space segments 1a, 2a, 3a, 4a, that is, only when there is a possibility of collision between the first space segments 5a, 6a, 7a and the second space segments 5b, 6b, 7b, the first space segments 5a, 6a, 7a can be determined from the original first space segments 1a-7a, and the second space segments 5b, 6b, 7b can be determined from the original second space segments 1b-7b, and whether a collision occurs between the first robot arm and the second robot arm is detected based on the first space segments 5a, 6a, 7a and the second space segments 5b, 6b, 7b. For example, the target task and the first space segment and the second space segment that need to participate in collision detection can be associated in a configuration file. Based on the target task, the space segments participating in collision detection can be determined, and the amount of calculation can be reduced.

[0113] In some embodiments, although some target tasks are different, if the collision positions between the robot arms do not change under these different target tasks, the configured first space segments and / or second space segments can be the same for these different target tasks, which includes: (1) the structures of the first robot arm represented by the formed first space segments and the second robot arm represented by the second space segments are the same; and (2) the number of the formed first space segments and the number of the formed second space segments are the same.

[0114] In some embodiments, the doctor or the assistant can also manually configure the first space segments and / or the second space segments according to actual situations and needs.

[0115] For example, at least one continuous arm body of the robot arm can be abstracted and segmented completely, the abstraction and segmentation can be based on the structural features of the continuous arm body, and a plurality of spatial segments formed are original spatial segments, which are spatial segments that do not participate in collision detection between robot arms without manual configuration. One or more target spatial segments are manually configured by the doctor or the assistant from the plurality of original spatial segments, the target spatial segments being spatial segments that participate in collision detection between robot arms after manual configuration. In some embodiments, to facilitate intuitive configuration by the human, an image model associated with the structure of the robot arm represented by the original spatial segments can be generated, which can be a two-dimensional or three-dimensional image model, and the image model can be displayed on a display of the catheter robot, which can be a touch display, for example. In some embodiments, controls associated with each original spatial segment or each structure of the robot arm represented by the original spatial segments can be generated in the above-mentioned image model, and triggering a corresponding control can be defined as selecting a corresponding original spatial segment as a target spatial segment, or, triggering a corresponding control can be defined as excluding a selected corresponding original spatial segment as a target spatial segment and selecting an unselected corresponding original spatial segment as a target spatial segment. For example, the control can be a checkbox, which is selected by checking; for another example, the control can be a text box, which is selected by inputting a corresponding serial number; for another example, the control can be the original spatial segment or the structure of the robot arm represented by the original spatial segment itself, and selecting the corresponding original spatial segment or the structure of the robot arm represented by the original spatial segment can achieve the configuration of the target spatial segment, and the selection can be a click on the corresponding original spatial segment or the structure represented by the original spatial segment, or a sliding touch along the basic direction of the original spatial segment or the structure. The operation of the above-mentioned control includes but is not limited to a touch screen, a mouse, and the like, and can also be voice input supported by a voice recognition module. In addition, a handle can be configured to control the movement of the catheter instrument, and the handle can be used to operate the above-mentioned control.

[0116] In some embodiments, referring to Figure 6 The motion states of the first robot arm and the second robot arm can be displayed in real time. The process of the real-time display includes:

[0117] In step S21, a first kinematic model of the first robot arm is obtained, and first joint variables of each joint in the first robot arm are obtained in real time.

[0118] In step S23, a second kinematic model of the second robot arm is obtained, and second joint variables of each joint in the second robot arm are obtained in real time.

[0119] The joint variable includes different contents according to different joint types. For example, for a rotary joint, the joint variable is a joint angle variable, and for a translational joint, the joint variable is a joint displacement variable.

[0120] The kinematic model of the robot arm can be established in advance based on D-H parameters.

[0121] In step S25, a first computer model of the first robot arm is generated based on the first kinematic model and the first joint variable, and a second computer model of the second robot arm is generated based on the second kinematic model and the second joint variable, in the same reference coordinate system, such as the base coordinate system of the catheter robot.

[0122] The computer model can be a two-dimensional or three-dimensional computer model.

[0123] In step S27, the first computer model and the second computer model are displayed on the display.

[0124] In some embodiments, when the first spatial line segment and the second spatial line segment are detected to collide, the first structure represented by the first spatial line segment can be highlighted in the first computer model, and the second structure represented by the second spatial line segment can be highlighted in the second computer model.

[0125] In some embodiments, when the first spatial line segment and the second spatial line segment are detected to collide, the first endpoint of the shortest distance between the first spatial line segment and the second spatial line segment can be highlighted in the first computer model, and the second endpoint of the shortest distance between the first spatial line segment and the second spatial line segment can be highlighted in the second computer model. The first endpoint and the second endpoint can represent the collision position.

[0126] The highlighting described above includes but is not limited to changing color, changing brightness, changing transparency, changing icon, flashing, etc.

[0127] In some embodiments, in order to prevent the surgical risks that may be caused by the collision, when the first robot arm and the second robot arm are detected to collide, the first robot arm and the second robot arm can be controlled to stop moving, i.e., all joints in the first robot arm and the second robot arm are controlled to stop moving.

[0128] In some embodiments, as soon as a collision is detected between the first robot arm and the second robot arm, the first robot arm and the second robot arm can be controlled to stop moving. In view of such a use scenario, the first spatial line segment and the second spatial line segment performing the above steps S12-S14 can be configured in order of high to low collision probability between the structure of the first robot arm represented by the first spatial line segment and the structure of the second robot arm represented by the second spatial line segment, so as to facilitate reducing the calculation amount of collision detection and improving the immediacy of collision detection. Of course, in any case, the first spatial line segment and the second spatial line segment performing the above steps S12-S14 can also be configured in order of high to low collision probability. The order of high to low collision probability can be determined based on historical statistical data generated by the doctor when operating the first robot arm or the second robot arm.

[0129] With continued reference to Figure 2 , the first robot arm is divided into first spatial line segments 1a-7a, and the second robot arm is divided into second spatial line segments 1b-7b. Taking the first spatial line segments 7a and 6a and the second spatial line segments 3b-7b as an example for description. It is assumed that from the statistical data, the collision probabilities between the first spatial line segment 7a and the second spatial line segments 7b-3b are 0.25, 0.20, 0.15, 0.10, 0.05, respectively, and the collision probabilities between the first spatial line segment 6a and the second spatial line segments 7b-3b are 0.20, 0.16, 0.12, 0.08, 0.04, respectively. The above collision probabilities are exemplary descriptions and do not represent actual statistical results.

[0130] In the above example, the collision probabilities are sorted in order from high to low as follows:

[0131] (A) between the first spatial line segment 7a and the second spatial line segment 7b, the collision probability is 0.25; (B) between the first spatial line segment 7a and the second spatial line segment 6b, and between the first spatial line segment 6a and the second spatial line segment 7b, the collision probability is 0.20; (C) between the first spatial line segment 6a and the second spatial line segment 6b, the collision probability is 0.16; (D) between the first spatial line segment 7a and the second spatial line segment 5b, the collision probability is 0.15; (E) between the first spatial line segment 6a and the second spatial line segment 5b, the collision probability is 0.12; (F) between the first spatial line segment 7a and the second spatial line segment 4b, the collision probability is 0.10; (G) between the first spatial line segment 6a and the second spatial line segment 4b, the collision probability is 0.08; (H) between the first spatial line segment 7a and the second spatial line segment 3b, the collision probability is 0.05; (I) between the first spatial line segment 6a and the second spatial line segment 3b, the collision probability is 0.04.

[0132] When comparing, the corresponding first spatial line segment and second spatial line segment can be compared in the order of (A) to (I).

[0133] The order of the collision probability can also be obtained based on the size of the range of motion of the structure of the first robotic arm represented by the first spatial line segment and the structure of the second robotic arm represented by the second spatial line segment, wherein the larger the range of motion, the higher the collision probability is generally.

[0134] With reference to Figure 2 , the first robotic arm is divided into first spatial line segments 1a-7a and the second robotic arm is divided into second spatial line segments 1b-7b. Only the first spatial line segments 7a-1a and the second spatial line segment 7b are taken as an example for illustration. Since the range of motion of the first spatial line segments 7a-1a gradually decreases, the collision probability between the first spatial line segments 7a-1a and the second spatial line segment 7b generally decreases in a decreasing manner, and thus the first spatial line segments 7a-1a can be compared with the second spatial line segment 7b in the order of the first spatial line segments 7a-1a.

[0135] In some embodiments, the order of the collision probability can be determined in combination with a target task of the distal end of the corresponding robotic arm under the target task, and the first spatial line segment and the second spatial line segment performing the above steps S12-S14 can be configured based on the order of the collision probability associated with the target task under the target task.

[0136] In some embodiments, in a catheter robot, the motion of the distal end of the catheter instrument includes a feeding motion and a bending motion, the feeding motion of the distal end of the catheter instrument is generally caused by the motion of the robotic arm, and the bending motion of the distal end of the catheter instrument is generally caused by the motion of the catheter instrument. Thus, the control of the catheter instrument can be decoupled into two parts, the first part including the control of the distal end of the robotic arm, i.e. by controlling the joint motion in the robotic arm to achieve the feeding motion of the distal end of the catheter instrument mounted on the distal end of the robotic arm; the second part including the control of the catheter instrument, i.e. by controlling the joint motion in the catheter instrument to achieve the bending motion of the distal end of the catheter instrument.

[0137] For example, in a catheter robot or other robot, there are various ways to control the motion of the distal end of the robot arm. For example, drag control can be implemented for the distal end of the robot arm to achieve the motion of the distal end of the robot arm, the drag control can resolve the force or torque sensed by the force or torque sensor into an incremental pose of the distal end of the robot arm, the target pose of the distal end of the robot arm can be determined in combination with the current pose of the distal end of the robot arm and the incremental pose, and the target joint variable of the joint in the robot arm can be determined in combination with the target pose and inverse kinematics, and the motion of the joint in the robot arm based on the target joint variable can make the distal end of the robot arm reach the target pose. For another example, remote control can be implemented for the distal end of the robot arm to achieve the motion of the distal end of the robot arm, the pose sent by the remote control can be an incremental pose, the target pose of the distal end of the robot arm can be determined in combination with the current pose of the distal end of the robot arm and the incremental pose, or the pose sent by the remote control can be a target pose, after the target pose is determined, the target joint variable of the joint in the robot arm can be determined in combination with the target pose and inverse kinematics, and the motion of the joint in the robot arm based on the target joint variable can make the distal end of the robot arm reach the target pose.

[0138] In some embodiments, when the effective degrees of freedom of the robot arm are redundant with respect to the target task of the distal end of the robot arm, i.e., the effective degrees of freedom of the robot arm are greater than the target task of the distal end of the robot arm, the distal end of the robot arm can have an infinite number of joint configurations when reaching the same target pose, and thus there can be multiple different trajectories that can reach the same target pose. If a collision between the first robot arm and the second robot arm is detected, it can not be necessary to immediately control the first robot arm and the second robot arm to stop moving at this time because there can be other trajectories that can avoid the collision. For example, when the second robot arm remains stationary and it is desired to control the first robot arm to reach a target pose, if a collision between the first robot arm and the second robot arm is detected, automatic obstacle avoidance processing can be performed on the first robot arm to control the first robot arm to reach the target pose while avoiding the first robot arm colliding with the second robot arm.

[0139] When a collision is detected or a risk of collision is detected, in one aspect, the collision can be avoided by selecting different joint configurations, and in one aspect, the collision can be avoided by re-planning the trajectory path points. The position of the collision point (i.e., the end points of the first spatial line segment and the second spatial line segment, respectively) determined in the above process can be used to provide a turning point of the optimal path, and the direction of motion of the robot arm can be changed at the turning point when avoiding the collision.

[0140] Exemplarily, when collision avoidance is performed by selecting different joint configurations, inverse kinematics of the robot arm can be solved based on the target pose and the arm shape angle, different joint configurations are obtained by selecting different arm shape angles, and then joint movements in the robot arm are controlled based on the obtained suitable joint configurations to achieve collision avoidance.

[0141] Exemplarily, when the trajectory path points are re-planned to avoid collision, the path point position or pose can be changed to prevent collision, for example, an optimal direction away from the collision point can be obtained according to the position of the collision point and the current link direction vector, such as the fastest direction away from the collision point can be selected according to the gradient descent method (for example, based on the same speed consideration), and then joint movements in the robot arm are controlled based on the target pose and the obtained direction away from the collision point to achieve collision avoidance.

[0142] The present application also provides a system of a robot. As shown in the accompanying drawings, the system can include a processor 501, a communications interface 502, a memory 503, and a communications bus 504. Figure 7

[0143] The processor 501, the communications interface 502, and the memory 503 complete mutual communication through the communications bus 504.

[0144] The communications interface 502 is configured to communicate with network elements such as various sensors or rotary motors or electromagnetic valves or other clients or servers.

[0145] The processor 501 is configured to execute the program 505, and specifically can execute related steps in the above method embodiments.

[0146] Specifically, the program 505 can include program code including computer operation instructions.

[0147] The processor 505 can be a central processing unit CPU, or a specific integrated circuit ASIC (Application Specific Integrated Circuit), or one or more integrated circuits configured to implement the embodiments of the present application, or a graphics processor GPU (Graphics Processing Unit). One or more processors included in the control device can be processors of the same type, such as one or more CPUs, or one or more GPUs; or can be processors of different types, such as one or more CPUs and one or more GPUs.

[0148] ​The memory 503 is configured to store a program 505. The memory 503 can include a high-speed RAM memory, and can also include a non-volatile memory, for example, at least one disk memory.

[0149] The program 505 can be specifically configured to enable the processor 501 to perform the following operations:

[0150] Based on the obtained structural features of the first robot arm and the second robot arm, the first robot arm is abstracted and segmented to form a first spatial line segment, and the second robot arm is abstracted and segmented to form a second spatial line segment;

[0151] The shortest distance between the first spatial line segment and the second spatial line segment is obtained, and the two endpoints of the shortest distance corresponding path respectively terminate at the first spatial line segment and the second spatial line segment;

[0152] The safety distance between the first spatial line segment and the second spatial line segment is obtained, and the safety distance is configured based on the sum of the first inflation radius of the first spatial line segment and the second inflation radius of the second spatial line segment;

[0153] The shortest distance and the safety distance are compared, and whether a collision occurs between the first robot arm and the second robot arm is determined based on the obtained comparison result.

[0154] The application also provides a computer readable storage medium, which stores a computer program configured to be loaded and executed by a processor to implement the steps of the method according to any one of the above embodiments.

[0155] The technical features of the above embodiments can be combined in any manner. To make the description concise, all possible combinations of the technical features in the above embodiments are not described, but as long as the combinations of the technical features do not exist contradictions, they should be considered as the scope of the present application.

[0156] The above embodiments only express several implementation manners of the present application, and the description is more specific and detailed, but it should not be understood as a limitation on the patent scope of the present application. It should be pointed out that for ordinary skilled in the art, without departing from the concept of the present application, several modifications and improvements can be made, which are all within the protection scope of the present application. Therefore, the patent protection scope of the present application should be subject to the appended claims.

Claims

1. A collision detection method of a robot arm in a robot, characterized by, The robot comprises a first mechanical arm and a second mechanical arm, and the method comprises: based on the obtained structural features of the first mechanical arm and the second mechanical arm, abstracting and segmenting the first mechanical arm to form a first spatial segment and the second mechanical arm to form a second spatial segment, obtaining a target task of the first mechanical arm and the second mechanical arm, and determining the first spatial segment and the second spatial segment with collision possibility based on the target task; obtaining the shortest distance between the first spatial segment and the second spatial segment with collision possibility comprises: constructing a first equation based on positions of two end points P0 and P1 on the first spatial line segment and constructing a second equation based on positions of two end points Q0 and Q1 on the second spatial line segment , the first equation being used to represent a position of any point on the first spatial line segment, the second equation being used to represent a position of any point on the second spatial line segment; based on the first equation and the second equation, determining a third equation related to the distance between the points on the first spatial segment and the points on the second spatial segment; transforming the third equation into an over-determined equation, and determining the positions of the two end points of the shortest distance corresponding path between the first spatial segment and the second spatial segment based on the over-determined equation; wherein if 0<α<1, α takes the current value calculated; if α>1, let α=1; if α<0, let α=0; if 0<β<1, β takes the current value calculated; if β>1, let β=1; if β<0, let β=0; determining the shortest distance based on the positions of the two end points of the shortest distance corresponding path; obtaining a safety distance between the first spatial segment and the second spatial segment, which is configured based on the sum of the first inflation radius of the first spatial segment and the second inflation radius of the second spatial segment; comparing the shortest distance with the safety distance, and determining whether a collision occurs between the first mechanical arm and the second mechanical arm based on the obtained comparison result.

2. The method of claim 1, wherein, The third equation is transformed into the over-determined equation based on the least square method or the gradient descent method.

3. The method of claim 1, wherein, The first spatial segment comprises one or more, and the second spatial segment comprises one or more, and the comparison of the shortest distance with the safety distance based on the obtained comparison result to determine whether a collision occurs between the first mechanical arm and the second mechanical arm comprises: based on the target task, determining one or more first spatial segments and one or more second spatial segments with collision possibility from the segmented first spatial segments and second spatial segments; comparing each determined first spatial segment with each determined second spatial segment respectively, and determining whether a collision occurs between the first mechanical arm and the second mechanical arm based on the multiple comparison results.

4. The method of claim 3, wherein, The determination of one or more first spatial segments and one or more second spatial segments with collision possibility from the segmented first spatial segments and second spatial segments based on the target task comprises: when the obtained target task is a first task, determining one or more first spatial segments and one or more second spatial segments with collision possibility from the segmented first spatial segments and second spatial segments based on the first task; or, When the target task is a second task, one or more first spatial line segments and one or more second spatial line segments having collision possibility are determined from the first spatial line segments and the second spatial line segments formed by the segmentation based on the second task.

5. The method of claim 1, wherein, The first spatial line segments and the second spatial line segments are formed by abstracting and segmenting the first robot arm and the second robot arm based on the structural features of the first robot arm and the second robot arm. The first spatial line segments and the second spatial line segments are formed by abstracting and segmenting the first robot arm and the second robot arm based on the structural features of the first robot arm and the second robot arm.

6. The method according to any one of claims 3 to 5, characterized in that, The target task includes joint degree of freedom constraints of the first robot arm and / or the second robot arm in a joint space.

7. The method according to any one of claims 3 to 5, characterized in that, The target task includes task degree of freedom constraints of the first robot arm and / or the second robot arm in a task space.

8. The method according to any one of claims 1 to 5, characterized in that, The method further includes: When it is determined that a collision occurs between the first robot arm and the second robot arm, stopping the motion control of the first robot arm and the second robot arm.

9. The method according to any one of claims 1 to 5, characterized in that, The comparison between the shortest distance and the safety distance includes: Obtaining a collision probability between the structure of the first robot arm and the structure of the second robot arm; According to the collision probability from high to low, comparing the first spatial line segments representing the structure of the first robot arm and the second spatial line segments representing the structure of the second robot arm.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program configured to be loaded and executed by a processor to implement the steps of the method according to any one of claims 1-9.

11. A control device of a robot characterized by comprising: It includes: a memory for storing a computer program; and a processor for loading and executing the computer program; The computer program is configured to be loaded and executed by the processor to implement the steps of the method according to any one of claims 1-9.

12. A robot, characterized in that It includes: a first robot arm; a second robot arm; and a control device configured to be coupled with the first robot arm and the second robot arm, and configured to: based on the structural features of the first robot arm and the second robot arm, abstract and segment the first robot arm to form first spatial line segments, and abstract and segment the second robot arm to form second spatial line segments, obtain a target task of the first robot arm and the second robot arm, and determine first spatial line segments and second spatial line segments having collision possibility based on the target task; obtaining the shortest distance between the first spatial line segments and the second spatial line segments having collision possibility includes: constructing a first equation based on positions of two end points P0 and P1 on the first spatial line segment and constructing a second equation based on positions of two end points Q0 and Q1 on the second spatial line segment , the first equation being used to represent a position of any point on the first spatial line segment, the second equation being used to represent a position of any point on the second spatial line segment; based on the first equation and the second equation, determining a third equation, the third equation being associated with the distance between a point on the first spatial line segment and a point on the second spatial line segment; transforming the third equation into an over-determined equation; determining positions of two end points of a shortest distance corresponding path between the first spatial line segment and the second spatial line segment based on the over-determined equation; wherein if 0 < a < 1, a takes its calculated current value; if a > 1, let a = 1; if a < 0, let a = 0; if 0 < β < 1, β takes its calculated current value; if β > 1, let β = 1; if β < 0, let β = 0; determining the shortest distance based on the positions of the two end points of the shortest distance corresponding path; obtaining a safety distance between the first spatial line segment and the second spatial line segment, the safety distance being configured based on a sum of a first inflation radius of the first spatial line segment and a second inflation radius of the second spatial line segment; comparing the shortest distance with the safety distance, and determining whether a collision occurs between the first robot arm and the second robot arm based on a result obtained by the comparison.

Images