A robotic arm instrument control method, apparatus, and storage medium

Abstract

The present application relates to the technical field of medical equipment, in particular to a mechanical arm instrument control method, device and storage medium.A kind of mechanical arm instrument control method, comprising: obtaining left instrument arm left instrument rod position information and right mechanical arm right instrument position information;According to left instrument rod position information, left instrument rod length range relationship is obtained, and according to right instrument rod position information, right instrument rod length range relationship is obtained;When left instrument rod length range relationship and right instrument rod length range relationship satisfy preset discrimination condition, then judge left instrument rod and right instrument rod collision, and control left instrument rod and right instrument rod stop movement.The technical scheme of the present application can effectively reduce the risk of instrument damage caused by instrument rod collision.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of medical equipment, in particular to a mechanical arm instrument control method and device and storage medium. BACKGROUND

[0002] In recent years, surgical robots have been widely applied, wherein a mechanical arm is a core component of the surgical robot, and a doctor realizes precise control of an instrument installed on the mechanical arm through a control system, so as to complete more accurate and meticulous operations.

[0003] The instrument rod of the surgical robot mechanical arm has high flexibility and accuracy, can simulate the movement of a human hand to help the doctor complete related operations, which enables the surgical robot to cooperate with the doctor to perform complex and delicate operations, and improves the success rate of the operation. However, in the operation process of the doctor, due to external objective factors, collision may occur in the movement of the instrument rod, and with the increase of the degree of collision, the instrument may be damaged, affecting the operation. SUMMARY

[0004] The problem solved by the present application is how to reduce the risk of instrument damage caused by collision of the instrument rod.

[0005] To solve the above problems, the present application provides a mechanical arm instrument control method, device and storage medium.

[0006] In a first aspect, the present application provides a mechanical arm instrument control method, comprising:

[0007] obtaining left instrument rod position information of a left instrument arm and right instrument position information of a right instrument arm;

[0008] obtaining a left instrument rod length range relationship according to the left instrument rod position information, and obtaining a right instrument rod length range relationship according to the right instrument rod position information;

[0009] When the left instrument rod length range relationship and the right instrument rod length range relationship satisfy a preset discrimination condition, it is judged that the left instrument rod and the right instrument rod collide, and the left instrument rod and the right instrument rod are controlled to stop moving.

[0010] Optionally, the left instrument rod position information includes left instrument rod front end position information and left instrument rod distal end position information; and the obtaining of the left instrument rod length range relationship according to the left instrument rod position information comprises:

[0011] obtaining a left instrument rod front end coordinate according to the left instrument rod front end position information, and obtaining a left instrument rod distal end coordinate according to the left instrument rod distal end position information;

[0012] According to the left instrument rod front end coordinate and the left instrument rod distal end coordinate, the left instrument rod length range relationship is obtained.

[0013] Optionally, the right instrument rod position information comprises right instrument rod front end position information and right instrument rod distal end position information; and the right instrument rod length range relationship is obtained according to the right instrument rod position information, comprising:

[0014] According to the right instrument rod front end position information, a right instrument rod front end coordinate is obtained, and according to the right instrument rod distal end position information, a right instrument rod distal end coordinate is obtained.

[0015] According to the right instrument rod front end coordinate and the right instrument rod distal end coordinate, the right instrument rod length range relationship is obtained.

[0016] Optionally, the preset discrimination condition comprises a first collision condition, a second collision condition, a third collision condition and an instrument length condition, and when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset discrimination condition, it is judged that the left instrument rod and the right instrument rod collide, comprising:

[0017] The left quadrilateral residual angle and the left rotation residual angle of the left instrument rod are obtained, and the right quadrilateral residual angle and the right rotation residual angle of the right instrument rod are obtained;

[0018] When the left instrument rod front end coordinate, the right instrument rod front end coordinate, the left instrument rod length range relationship, the right instrument rod length range relationship, the left quadrilateral residual angle and the right quadrilateral residual angle satisfy the first collision condition and the instrument length condition at the same time, it is judged that the left instrument rod and the right instrument rod collide;

[0019] Or, when the left instrument rod front end coordinate, the right instrument rod front end coordinate, the left instrument rod length range relationship, the right instrument rod length range relationship, the left quadrilateral residual angle and the right quadrilateral residual angle satisfy the second collision condition and the instrument length condition at the same time, it is judged that the left instrument rod and the right instrument rod collide;

[0020] Or, when the left instrument rod front end coordinate, the right instrument rod front end coordinate, the left instrument rod length range relationship, the right instrument rod length range relationship, the left rotation residual angle and the right rotation residual angle satisfy the third collision condition and the instrument length condition at the same time, it is judged that the left instrument rod and the right instrument rod collide.

[0021] Optionally, the left instrument rod length range relationship satisfies:

[0022] X L = X L2 + (X L2 -XL1 )*t L ;

[0023] Y L =Y L2 +(Y L2 -Y L1 )*t L ;

[0024] Z L =Z L2 +(Z L2 -Z L1 )*t L ;

[0025] wherein, (X L , Y L , Z L ) is the coordinate of the midpoint of the left instrument rod length range relation, (X L1 , Y L1 , Z L1 ) is the coordinate of the front end of the left instrument rod, (X L2 , Y L2 , Z L2 ) is the coordinate of the distal end of the left instrument rod, and t L is the left adjustment parameter of the left instrument rod length range relation.

[0026] Optionally, the right instrument rod length range relation satisfies:

[0027] X R =X R2 +(X R2 -X R1 )*t R ;

[0028] Y R =Y R2 +(Y R2 -Y R1 )*t R ;

[0029] Z R =Z R2 +(Z R2 -Z R1 )*t R ;

[0030] wherein, (X R , Y R , Z R ) is the coordinate of the midpoint of the right instrument rod length range relation, (X R1 , Y R1 , Z R1 ) is the coordinate of the front end of the right instrument rod, (X R2 , YR2 Z R2 ) represents the coordinates of the distal end of the right instrument rod, t R This refers to the right adjustment parameter for the range of lengths of the right instrument rod.

[0031] Optionally, the first collision condition satisfies:

[0032] X0 = X L2 +(X L2 -X L1 )*t L -r / sinθ kL =X R2 +(X R2 -X R1 )*t R +r / sinθ kR ;

[0033] Y0 = Y L2 +(Y L2 -Y L1 )*t L =Y R2 +(Y R2 -Y R1 )*t R ;

[0034] Z0 = Z L2 +(Z L2 -Z L1 )*t L +r / cosθ kL =Z R2 +(Z R2 -Z R1 )*t R +r / cosθ kR ;

[0035] The second collision condition is satisfied:

[0036] X0 = X L2 +(X L2 -X L1 )*t L +r / sinθ kL =X R2 +(X R2 -X R1 )*t R -r / sinθ kR ;

[0037] Y0 = Y L2 +(Y L2 -Y L1 )*t L =Y R2 +(YR2 - Y R1 )* t R ;

[0038] Z0= Z L2 +(Z L2 -Z L1 )* t L +r / cosθ kL = Z R2 +(Z R2 -Z R1 )* t R +r / cosθ kR ;

[0039] The third collision condition is satisfied:

[0040] X0= X L2 +(X L2 -X L1 )* t L = X R2 +(X R2 -X R1 )* t R ;

[0041] Y0== Y L2 +(Y L2 -Y L1 )* t L +r / sinθ jL = Y R2 +(Y R2 -Y R1 )* t R -r / sinθ jR ;

[0042] Z0= Z L2 +(Z L2 -Z L1 )* t L +r / cosθ jL = Z R2 +(Z R2 -Z R1 )* t R +r / 2cosθ jR ;

[0043] The instrument length condition is satisfied:

[0044]

[0045]

[0046] wherein (X O(X0, Y0, Z0) is the collision point coordinate of the left instrument rod and the right instrument rod, (X L1 , Y L1 , Z L1 ) is the left instrument rod front end coordinate, (X L2 , Y L2 , Z L2 ) is the left instrument rod distal end coordinate, t L is the left adjustment parameter, (X R1 , Y R1 , Z R1 ) is the right instrument rod front end coordinate, (X R2 , Y R2 , Z R2 ) is the right instrument rod distal end coordinate, t R is the right adjustment parameter, θ kL is the left quadrilateral residual angle, θ jL is the left rotation residual angle, θ kR is the right quadrilateral residual angle, θ jR is the right rotation residual angle D L is the preset length of the left instrument rod of the mechanical arm, D R is the preset length of the right instrument rod of the right instrument arm.

[0047] In a second aspect, a mechanical arm instrument control device comprises:

[0048] An acquisition module is configured to acquire left instrument rod position information of a left instrument arm and right instrument position information of a right instrument arm.

[0049] A processing module is configured to obtain a left instrument rod length range relationship according to the left instrument rod position information and obtain a right instrument rod length range relationship according to the right instrument rod position information.

[0050] A judgment module is configured to judge that the left instrument rod and the right instrument rod collide and control the left instrument rod and the right instrument rod to stop moving when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy a preset judgment condition.

[0051] In a third aspect, an electronic device comprises a memory and a processor.

[0052] The memory is configured to store a computer program.

[0053] The processor is configured to implement the mechanical arm instrument control method of the first aspect when executing the computer program.

[0054] In a fourth aspect, a computer readable storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the computer program implements the mechanical arm instrument control method according to the first aspect.

[0055] The mechanical arm instrument control method, device and storage medium have the following beneficial effects: according to the left instrument rod position information and the right instrument rod position information, the left instrument rod length range relationship and the right instrument rod length range relationship are obtained, that is, the position coordinate relationship of the straight line of the instrument rod center axis on the left and right instrument rods. Through the left instrument rod length range relationship and the right instrument rod length range relationship, the relative position relationship in the space of the left instrument rod and the right instrument rod can be accurately judged, and accurate collision judgment can be performed through the position relationship of the left instrument rod and the right instrument rod. Therefore, when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset judgment condition, it is judged that the left instrument rod and the right instrument rod collide. When the instrument rods contact, the left instrument rod and the right instrument rod are immediately controlled to stop moving. Since slight collision usually does not cause substantial damage to the instrument rods, the operation is stopped when slight collision occurs, which can prevent further serious collision and damage to the instrument rods, avoid the cost of maintenance and replacement due to equipment damage, effectively reduce the loss risk caused by collision of the instrument rods, and ensure stable operation of the doctor, improve the operation efficiency, and reduce the operation risk. BRIEF DESCRIPTION OF DRAWINGS

[0056] Figure 1 FIG. 1 is a flowchart of a mechanical arm instrument control method according to an embodiment of the present application;

[0057] Figure 2 FIG. 2 is a structural diagram of a left instrument arm instrument rod and a right instrument arm instrument rod according to an embodiment of the present application;

[0058] Figure 3 FIG. 3 is a structural diagram of a left quadrilateral residual angle of a left instrument arm instrument rod according to an embodiment of the present application;

[0059] Figure 4 FIG. 4 is a structural diagram of a rotation residual angle of a left instrument arm instrument rod and a right instrument arm instrument rod according to an embodiment of the present application;

[0060] Figure 5 FIG. 5 is a structural diagram of a left instrument rod and a right instrument rod satisfying a first collision condition according to an embodiment of the present application;

[0061] Figure 6 FIG. 6 is a structural diagram of a left instrument rod and a right instrument rod satisfying a second collision condition according to an embodiment of the present application;

[0062] Figure 7 FIG. 7 is a structural diagram of a mechanical arm instrument control device according to an embodiment of the present application. DETAILED DESCRIPTION

[0063] In order to make the above objectives, features and advantages of the present application more clear and comprehensible, specific embodiments of the present application are described in detail below with reference to the accompanying drawings. Although some embodiments of the present application are shown in the drawings, it should be understood that the present application can be implemented in various forms and should not be interpreted as being limited to the embodiments set forth herein, but rather the embodiments are provided to make the present application more thorough and complete. It should be understood that the drawings and embodiments of the present application are for exemplary purposes only and are not intended to limit the scope of the present application.

[0064] It should be understood that each of the steps recited in the method embodiments of the present application can be performed in different orders and / or in parallel. In addition, the method embodiments can include additional steps and / or omit the steps shown. The scope of the present application is not limited in this respect.

[0065] The term "comprising" and variations thereof as used herein are open-ended, and mean "including but not limited to". The term "based on" means "based, at least in part, on". The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments"; the term "optional" means "optional in at least some embodiments". Related definitions are given throughout the description. It should be noted that the concepts of "first", "second", etc. mentioned in the present application are only used to distinguish different devices, modules or units, and do not limit the order or interdependence of the functions performed by these devices, modules or units.

[0066] It should be noted that the modification of "one", "multiple" mentioned in the present application is illustrative and not limiting, and those skilled in the art should understand that unless otherwise explicitly indicated in the context, it should be understood as "one or more".

[0067] The mechanical arm instrument control method of the embodiment of the present application is mainly applied to control left and right instrument rods. Firstly, a three-dimensional space model is constructed, and the length range relationship of the left and right instrument rods in the three-dimensional space model is combined and solved, that is, the space straight line parameter equations of the central axes of the left and right instrument rods are combined and solved. Whether the intersection points of the two straight line parameter equations in the space are within the length range of the instruments is taken as a preset discrimination condition, so that the collision point space coordinates satisfying the preset discrimination condition of instrument collision, that is, the intersection point coordinates of the two straight line parameter equations, are obtained. The space motion coordinate system direction of the instrument rod is as follows: when the instrument rod is behind the laparoscopic surgery robot, the forward direction is the positive direction of the X axis, the left direction is the positive direction of the Y axis, and the upward direction is the positive direction of the Z axis. When facing the laparoscopic surgery robot, the left instrument rod is the left arm, the middle instrument rod is the mirror holding arm, and the right instrument rod is the right arm (note: the subscripts of the variables are L, C, and R, respectively). Since the traditional three-arm 6-degree-of-freedom surgery robot, the instrument is generally installed on the left arm or the right arm, and the distal end of the instrument pitch joint to the front end of the instrument roll is mostly visible in the console main screen during surgery, so the probability of collision of the instrument at the above-mentioned parts is extremely low, and in most cases, the left and right instrument rods collide.

[0068] As shown in Figure 1 The embodiment of the present application provides a mechanical arm instrument control method, which comprises the following steps:

[0069] Step S1, obtaining left instrument rod position information of a left mechanical arm and right instrument position information of a right mechanical arm;

[0070] Specifically, according to the constructed three-dimensional space model, the spatial position information of the left instrument rod and the right instrument rod in the three-dimensional space model is obtained, which includes the corresponding coordinates of the front end and the distal end of the instrument in the three-dimensional space model.

[0071] Step S2, obtaining the length range relationship of the left instrument rod according to the left instrument rod position information, and obtaining the length range relationship of the right instrument rod according to the right instrument rod position information;

[0072] Specifically, the front end position coordinates and the distal end position coordinates of the left instrument rod are obtained according to the left instrument rod position information, the front end position coordinates and the distal end position coordinates of the right instrument rod are obtained according to the right instrument rod position information, and the corresponding left instrument rod length range relationship, that is, the space straight line parameter equation of the central axis of the left instrument rod, is obtained according to the front end position coordinates and the distal end position coordinates. Similarly, the corresponding right instrument rod length range relationship, that is, the space straight line parameter equation of the central axis of the right instrument rod, is obtained according to the front end position coordinates and the distal end position coordinates of the right instrument rod.

[0073] Step S3, when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset discrimination condition, it is judged that the left instrument rod and the right instrument rod collide, and the left instrument rod and the right instrument rod are controlled to stop moving;

[0074] Specifically, when the left instrument rod length range relationship and the right instrument rod length range relationship will satisfy the preset discrimination condition, that is, the spatial position coordinates of the left instrument rod and the right instrument rod coincide, and the common solution of the spatial straight line equations of the left and right instrument rods is the coincident intersection coordinates, it is indicated that the left instrument rod and the right instrument rod collide. At this time, the control system gives the operator certain warning text or voice prompt to remind the collision risk, and transmits the instrument collision command to the master end of the console. In the case of obtaining the collision direction, the opposite direction resistance can be applied to the joint motor of the instrument rod according to the current moving direction. After the operator feels the resistance feedback prompt, the instrument rod can be separated at the collision position by following the resistance applied by the master. When the left and right instrument rods collide slightly, they slow down and stop moving, so as to finally realize the control of the instrument rod collision of the surgical robot, avoid further instrument rod collision, and further enhance the instrument damage, so as to achieve the purpose of protecting the instrument and effectively reduce the risk of instrument damage due to instrument rod collision.

[0075] In the embodiment, according to the left instrument rod position information and the right instrument rod position information, the left instrument rod length range relationship and the right instrument rod length range relationship are obtained, that is, the position coordinate relationship of the straight lines of the instrument rod center axes on the left and right instrument rods. Through the left instrument rod length range relationship and the right instrument rod length range relationship, the relative position relationship in the space of the left instrument rod and the right instrument rod can be accurately judged, and the collision can be accurately judged through the position relationship of the left instrument rod and the right instrument rod. Therefore, when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset discrimination condition, it is judged that the left instrument rod and the right instrument rod collide. When the instrument rods contact, the left instrument rod and the right instrument rod are immediately controlled to stop moving. Since slight collision usually does not cause substantial damage to the instrument rod, the operation is stopped when slight collision occurs, which can prevent further serious collision and damage to the instrument rod, avoid the cost of maintenance and replacement due to equipment damage, effectively reduce the loss risk caused by the collision of the instrument rod, ensure the stable operation of the doctor, improve the operation efficiency, and reduce the operation risk.

[0076] In an optional embodiment, the left instrument rod position information includes left instrument rod front end position information and left instrument rod distal end position information; and the left instrument rod length range relationship obtained according to the left instrument rod position information includes:

[0077] The coordinates of the front end of the left instrument rod are obtained based on the position information of the front end of the left instrument rod, and the coordinates of the far end of the left instrument rod are obtained based on the position information of the far end of the left instrument rod.

[0078] The length range of the left instrument rod is obtained based on the coordinates of the front end of the left instrument rod and the coordinates of the far end of the left instrument rod.

[0079] In an optional embodiment, the length range of the left instrument bar satisfies the following relationship:

[0080] X L =X L2 +(X L2 -X L1 )*t L ;

[0081] Y L =Y L2 +(Y L2 -Y L1 )*t L ;

[0082] Z L =Z L2 +(Z L2 -Z L1 )*t L ;

[0083] Among them, (X) L Y L Z L (X) represents the coordinates of the midpoint of the left instrument rod length range relationship. L1 Y L1 Z L1 (X) represents the coordinates of the front end of the left instrument lever. L2 Y L2 Z L2 ) represents the coordinates of the distal end of the left instrument lever, t L The left adjustment parameter is the relationship between the length range of the left instrument rod.

[0084] Specifically, such as Figure 2 As shown, Figure 2 As shown, Q1 is the left robotic arm, Q2 is the right robotic arm, and P... L1 P is the front end of the left robotic arm. L2 P is the end effector of the left robotic arm. R1 P is the front end of the right robotic arm. R2The right mechanical arm end is obtained. According to the left instrument rod position information, the position information of the front end and the far end of the instrument rod installed on the left mechanical arm is obtained, that is, the left instrument rod front end position information and the left instrument rod far end position information, and then the left instrument rod front end coordinate is obtained according to the left instrument rod front end position information, that is, the position coordinate of the front end of the instrument rod on the left mechanical arm in the three-dimensional space, and the left instrument rod far end coordinate is obtained according to the left instrument rod far end position information, that is, the position coordinate of the far end of the instrument rod on the left mechanical arm in the three-dimensional space, so as to construct the straight line parameter equation of the center axis of the instrument rod on the left instrument rod according to the left instrument rod front end coordinate and the left instrument rod far end coordinate, that is, the left instrument rod length range relationship, wherein the left instrument rod far end is the remote center of motion (RCM) of the robot, and the point of the left instrument rod far end is the fulcrum coinciding with the incision on the instrument axis. The instrument can only move along the instrument axis and rotate around the point as the fulcrum. No matter how the joints of the mechanical arm move, the spatial position of the point of the left instrument rod far end will not change. In actual engineering application, once the mechanical structure is determined, the coordinate of the left instrument rod far end can be determined and fixed.

[0085] Further, the length of the instrument rod on the center axis can be adjusted according to the range of the left adjustment parameter, that is, the range length of the left instrument rod length range relationship in the three-dimensional space. By selecting a matching adjustment parameter range according to different instruments, the accuracy and precision of collision judgment can be improved according to the accurate instrument rod length.

[0086] In the optional embodiment, according to the corresponding coordinates of the left instrument rod front end and the left instrument rod far end in the three-dimensional space, the left instrument rod length range relationship generated can obtain the three-dimensional space position coordinates of each point on the left instrument rod. According to the position coordinates, the specific position and movement trajectory of the left instrument rod in the three-dimensional space can be accurately judged, and whether the instrument rods on the left two mechanical arms collide can be accurately judged according to the position coordinates.

[0087] In an optional embodiment, the right instrument rod position information includes right instrument rod front end position information and right instrument rod far end position information; and the right instrument rod length range relationship is obtained according to the right instrument rod position information, including:

[0088] The right instrument rod front end coordinate is obtained according to the right instrument rod front end position information, and the right instrument rod far end coordinate is obtained according to the right instrument rod far end position information.

[0089] The right instrument rod length range relationship is obtained according to the right instrument rod front end coordinate and the right instrument rod far end coordinate.

[0090] In an optional embodiment, the right instrument rod length range relationship satisfies:

[0091] X R = X R2+ (X R2 - X R1 )* t R ;

[0092] Y R = Y R2 + (Y R2 - Y R1 )* t R ;

[0093] Z R = Z R2 + (Z R2 - Z R1 )* t R ;

[0094] wherein (X R , Y R , Z R ) is the coordinate of the midpoint of the length range relationship of the right instrument rod, (X R1 , Y R1 , Z R1 ) is the coordinate of the front end of the right instrument rod, (X R2 , Y R2 , Z R2 ) is the coordinate of the distal end of the right instrument rod, and t R is the right adjustment parameter of the length range relationship of the right instrument rod.

[0095] Specifically, the position information of the front end and the distal end of the instrument rod installed on the right mechanical arm is obtained according to the right instrument rod position information, i.e., the right instrument rod front end position information and the right instrument rod distal end position information, and then the right instrument rod front end coordinate, i.e., the position coordinate of the front end of the instrument rod on the right mechanical arm in the three-dimensional space, is obtained according to the right instrument rod front end position information, and the right instrument rod distal end coordinate, i.e., the position coordinate of the distal end of the instrument rod on the right mechanical arm in the three-dimensional space, is obtained according to the right instrument rod distal end position information, so as to construct the linear parameter equation of the center axis of the instrument rod on the right instrument rod, i.e., the length range relationship of the right instrument rod, wherein the distal end of the left instrument rod is the remote center of motion (RCM) of the robot, the point of the distal end of the left instrument rod is the fulcrum point coinciding with the incision on the instrument axis, the instrument can only move along the instrument axis and rotate around the point, and the spatial position of the point of the distal end of the left instrument rod will not change regardless of the movement of each joint of the mechanical arm, and in actual engineering application, once the mechanical structure is determined, the coordinate of the distal end of the left instrument rod can be determined and fixed.

[0096] Further, the length of the instrument rod on the central axis, i.e., the range length of the right instrument rod length range relationship in the three-dimensional space, can be adjusted according to the range of the right adjustment parameter. According to the adjustment parameter range matched with different instruments, the accuracy and precision of the collision judgment can be improved according to the accurate instrument rod length.

[0097] In the optional embodiment, according to the coordinates of the front end of the right instrument rod and the distal end of the right instrument rod in the three-dimensional space, the generated right instrument rod length range relationship can obtain the three-dimensional space position coordinates of each point on the right instrument rod. According to the position coordinates, the specific position and movement trajectory of the right instrument rod in the three-dimensional space can be accurately judged, and whether the instrument rods on the left and right mechanical arms collide can be accurately judged according to the position coordinates.

[0098] In an optional embodiment, the preset discrimination condition includes a first collision condition, a second collision condition, a third collision condition, and an instrument length condition. When the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset discrimination condition, it is determined that the left instrument rod and the right instrument rod collide, including:

[0099] Obtaining a left quadrilateral residual angle and a left rotation residual angle of the left instrument rod, and a right quadrilateral residual angle and a right rotation residual angle of the right instrument rod;

[0100] When the front end coordinates of the left instrument rod, the front end coordinates of the right instrument rod, the left instrument rod length range relationship, the right instrument rod length range relationship, the left quadrilateral residual angle, and the right quadrilateral residual angle satisfy the first collision condition and the instrument length condition at the same time, it is determined that the left instrument rod and the right instrument rod collide;

[0101] Or, when the front end coordinates of the left instrument rod, the front end coordinates of the right instrument rod, the left instrument rod length range relationship, the right instrument rod length range relationship, the left quadrilateral residual angle, and the right quadrilateral residual angle satisfy the second collision condition and the instrument length condition at the same time, it is determined that the left instrument rod and the right instrument rod collide;

[0102] Or, when the front end coordinates of the left instrument rod, the front end coordinates of the right instrument rod, the left instrument rod length range relationship, the right instrument rod length range relationship, the left rotation residual angle, and the right rotation residual angle satisfy the third collision condition and the instrument length condition at the same time, it is determined that the left instrument rod and the right instrument rod collide.

[0103] In an optional embodiment, the first collision condition satisfies:

[0104] X0=X L2 +(X L2 -X L1)*t L -r / sinθ kL =X R2 +(X R2 -X R1 )*t R +r / sinθ kR ;

[0105] Y0=Y L2 +(Y L2 -Y L1 )*t L =Y R2 +(Y R2 -Y R1 )*t R ;

[0106] Z0=Z L2 +(Z L2 -Z L1 )*t L +r / cosθ kL =Z R2 +(Z R2 -Z R1 )*t R +r / cosθ kR ;

[0107] The second collision condition is satisfied:

[0108] X0=X L2 +(X L2 -X L1 )*t L +r / sinθ kL =X R2 +(X R2 -X R1 )*t R -r / sinθ kR ;

[0109] Y0=Y L2 +(Y L2 -Y L1 )*t L =Y R2 +(Y R2 -Y R1 )*t R ;

[0110] Z0=Z L2 +(Z L2 -Z L1 )*t L +r / cosθ kL =Z R2 +(Z R2 -ZR1 )*t R +r / cosθ kR ;

[0111] The third collision condition is satisfied as follows:

[0112] X0 = X L2 +(X L2 -X L1 )*t L =X R2 +(X R2 -X R1 )*t R ;

[0113] Y0==Y L2 +(Y L2 -Y L1 )*t L +r / sinθ jL =Y R2 +(Y R2 -Y R1 )*t R -r / sinθ jR ;

[0114] Z0 = Z L2 +(Z L2 -Z L1 )*t L +r / cosθ jL =Z R2 +(Z R2 -Z R1 )*t R +r / 2cosθ jR ;

[0115] The length condition of the instrument is satisfied as follows:

[0116]

[0117] Among them, (X) O (X, Y0, Z0) are the coordinates of the collision point between the left and right instrument rods, respectively. L1 Y L1 Z L1 (X) represents the coordinates of the front end of the left instrument lever. L2 Y L2 Z L2 ) represents the coordinates of the distal end of the left instrument lever, t L For the left adjustment parameter, (X) R1 Y R1 Z R1 (X) represents the coordinates of the front end of the right instrument rod. R2 Y R2, Z R2 is the right instrument rod far end coordinate, t R is the right adjustment parameter, θ kL is the left quadrilateral residual angle, θ jL is the left rotation residual angle, θ kR is the right quadrilateral residual angle, θ jR is the right rotation residual angle D L is the left instrument rod preset length of the mechanical arm, D R is the right instrument rod preset length of the right mechanical arm.

[0118] Specifically, as shown in Figure 3 and Figure 4 , wherein θ kL is the left quadrilateral residual angle, since the mechanical arm parallelogram joint is a driven joint, the output signal of the encoder is a pulse signal, the frequency of which is proportional to the rotation angle, thus the parallelogram joint angle can be obtained through the number of pulses of the encoder output signal and the resolution of the encoder, wherein the angle = the number of pulses of the encoder output signal / the resolution of the encoder * 360°, the left quadrilateral residual angle is complementary to the mechanical arm parallelogram joint angle, the mechanical arm parallelogram joint angle can be obtained by the above angle formula, thus the left quadrilateral residual angle can be obtained, and the left rotation residual angle, the right quadrilateral residual angle and the right rotation residual angle can be obtained in the same way, thus no further description is given, through the above angle formula combined with the encoder and the decoder, the left quadrilateral residual angle and the left rotation residual angle of the left instrument rod, the right quadrilateral residual angle and the right rotation residual angle of the right instrument rod can be obtained, and a reasonable radius threshold value can be set according to the radius of the instrument rod, the radius value can be greater than or equal to the radius of the instrument rod, when the radius threshold value is slightly greater than the radius of the instrument rod, the collision of the left instrument rod and the right instrument rod can be judged before the collision occurs, the anti-collision control strategy of the mechanical arm can be implemented earlier, the left and right instrument rods are controlled to stop moving, thus the effect of preventing instrument collision is realized, when the radius threshold value is equal to the radius of the instrument rod, the instrument rod can be controlled to stop moving when a slight collision occurs, and the damage of the left and right instrument rods caused by the collision can be effectively avoided by setting a reasonable radius threshold value.

[0119] Specifically, as shown in Figure 4 and Figure 5 , the left quadrilateral residual angle θ kL , the left rotation residual angle θ jL , the right quadrilateral residual angle θ kR , and the right rotation residual angle θ jR, the radius threshold r, when the left instrument rod front end coordinate, the right instrument rod front end coordinate, the left instrument rod length range relationship, the right instrument rod length range relationship, the left quadrilateral residual angle and the right quadrilateral residual angle simultaneously satisfy the first collision condition and the preset instrument length condition, it is judged that the left instrument stem and the right instrument rod collide, that is, the left instrument rod along the negative side of the X axis and the right instrument rod along the positive side of the X axis collide, because the actual collision point is the left instrument rod center axis linear translation preset radius threshold r in the X axis negative direction component distance, that is, the X axis translation distance is r / sinθ kL , and the Z axis positive direction translation distance r / cosθ kL , the Y axis has no change, while the right instrument rod center axis linearly translates the preset radius threshold r in the X axis positive direction component, that is, the X axis translation distance is r / sinθ kR , and the Z axis positive direction translation distance r / cosθ kR , the Y axis has no change, therefore, the X axis component in the left instrument rod length range relationship minus the X axis translation distance is equal to the X axis component in the right instrument rod length range relationship plus the corresponding X axis translation distance, the Z axis component in the left instrument rod length range relationship plus the corresponding Z axis translation distance is equal to the Z axis component in the right instrument rod length range relationship plus the corresponding Z axis translation distance, and the Y axis component in the left instrument rod length range relationship is equal to the Y axis component in the right instrument rod length range relationship, that is, it is determined that there is an intersection point along the instrument rod axis on the left instrument rod and the right instrument rod.

[0120] Further, when the first collision condition is satisfied, then according to the obtained intersection point coordinates of the left instrument rod and the right instrument rod, and the left instrument rod front end coordinate and the right instrument rod front end coordinate satisfy the preset instrument length condition, that is, the distance between the intersection point coordinates and the left instrument rod front end coordinate is less than or equal to the preset length D L of the left instrument rod R , and the distance between the intersection point coordinates and the right instrument rod front end coordinate is less than or equal to the preset length D R of the right instrument rod, it is judged that the collision point of the left instrument stem and the right instrument stem is on the left instrument rod and the right instrument rod, and in summary, it is judged that the left instrument rod along the negative side of the X axis and the right instrument rod along the positive side of the X axis collide.

[0121] Specifically, the same as the above first collision condition determination principle, such as Figure 6As shown, when the coordinates of the left and right instrument poles, the length ranges of the left and right instrument poles, and the complementary angles of the left and right quadrilaterals simultaneously satisfy the second collision condition and the instrument length condition, it can be determined that the left and right instrument poles collide. Specifically, the left instrument pole collides with the right instrument pole along the negative X-axis. The actual collision point is the component distance of the left instrument pole's central axis linear translation along the preset radius threshold r in the positive X-axis direction, i.e., the X-axis translation distance is r / sinθ. kL And translated a distance r / cosθ in the positive Z-axis direction. kL The equation of the straight line of the central axis of the right instrument rod is translated in the positive X-axis direction by r / sinθ. kR And translated a distance r / cosθ in the positive Z-axis direction. kR Since the Y-axis remains unchanged, the sum of the X-axis component and the X-axis translation distance in the length range relationship of the left instrument rod is equal to the sum of the X-axis component and the corresponding X-axis translation distance in the length range relationship of the right instrument rod. The sum of the Z-axis component and the corresponding Z-axis translation distance in the length range relationship of the left instrument rod is equal to the sum of the Z-axis component and the corresponding Z-axis translation distance in the length range relationship of the right instrument rod. When the sum of the Y-axis component and the corresponding Y-axis component in the length range relationship of the left instrument rod are equal, it is determined that there is an intersection point between the left and right instrument rods along the axis of the instrument rod.

[0122] Furthermore, when the second collision condition is met, it is determined whether the coordinates of the intersection point of the left and right instrument rods, the coordinates of the front end of the left instrument rod, and the coordinates of the front end of the right instrument rod satisfy the preset instrument length condition, that is, the distance between the coordinates of the intersection point and the coordinates of the front end of the left instrument rod is less than or equal to the preset length D of the left instrument rod. L Meanwhile, the distance between the coordinates of this intersection point and the coordinates of the front end of the right instrument bar is less than or equal to the preset length D of the right instrument bar. R When the instrument length condition is met, the collision point of the left instrument rod and the right instrument rod is determined to be on the left instrument rod and the right instrument rod, respectively. In summary, it can be determined that the left instrument rod collides along the negative X-axis and the right instrument rod collides along the positive X-axis.

[0123] Specifically, such as Figure 3 As shown, similar to the above principle, when the coordinates of the left and right instrument poles, the length ranges of the left and right instrument poles, the left and right rotation angles, and the left and right rotation angles simultaneously satisfy the third collision condition and the instrument length condition, it can also be determined that the left and right instrument poles collide. That is, the left instrument pole collides with the right instrument pole along the negative Y-axis. The actual collision point is the component distance of the left instrument pole's central axis linear translation along the preset radius threshold r in the positive Y-axis direction, i.e., the Y-axis translation distance is r / sinθ. kL，And the Z-axis positive direction translation distance r / cosθ jL , the X-axis has no change, the right instrument rod central axis straight line equation in the Y-axis negative direction translation r / sinθ jL , and the Z-axis positive direction translation distance r / cosθ jR , the X-axis has no change, therefore, the Y-axis component in the left instrument rod length range relationship plus the Y-axis translation distance is equal to the Y-axis component in the right instrument rod length range relationship minus the corresponding Y-axis translation distance, the Z-axis component in the left instrument rod length range relationship plus the corresponding Z-axis translation distance is equal to the Z-axis component in the right instrument rod length range relationship plus the corresponding Z-axis translation distance, and the X-axis component in the left instrument rod length range relationship is equal to the X-axis component in the right instrument rod length range relationship, that is, the intersection point of the left instrument rod and the right instrument rod along the instrument rod central axis straight line exists.

[0124] Further, when the third collision condition is satisfied, whether the intersection point coordinates of the left instrument rod and the right instrument rod and the left instrument rod front end coordinates and the right instrument rod front end coordinates satisfy a preset instrument length condition, that is, the distance between the intersection point coordinates and the left instrument rod front end coordinates is less than or equal to the preset length D L of the left instrument rod, and the distance between the intersection point coordinates and the right instrument rod front end coordinates is less than or equal to the preset length D R of the right instrument rod is judged, when the instrument length condition is satisfied, the collision point of the left instrument rod and the right instrument rod on the left instrument rod and the right instrument rod is judged, and the above is summarized, that is, the collision of the left instrument rod along the X-axis negative direction side and the right instrument rod along the X-axis positive direction side is judged.

[0125] In the optional embodiment, by judging whether the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset discrimination condition, the three main collision conditions of the left instrument rod and the right instrument rod can be accurately judged, so that the left mechanical arm and the right mechanical arm are controlled to stop moving according to the collision condition, the collision is further avoided, and the mechanical arm instrument rod damage due to the collision is prevented.

[0126] As shown in Figure 7 , another embodiment of the application provides a mechanical arm instrument control device, which comprises:

[0127] An acquisition module is configured to acquire left instrument rod position information of a left instrument rod and right instrument position information of a right instrument rod.

[0128] A processing module is configured to obtain a left instrument rod length range relationship according to the left instrument rod position information and a right instrument rod length range relationship according to the right instrument rod position information.

[0129] A judging module is configured to judge that the left instrument rod and the right instrument rod collide and control the left instrument rod and the right instrument rod to stop moving when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy a preset judging condition.

[0130] The mechanical arm instrument control device has similar technical effects to the mechanical arm instrument control method, and thus repeated description is omitted.

[0131] The electronic device provided in the embodiment of the present application comprises a memory and a processor.

[0132] The memory is configured to store a computer program.

[0133] The processor is configured to implement the mechanical arm instrument control method when the computer program is executed.

[0134] The electronic device has similar technical effects to the mechanical arm instrument control method, and thus repeated description is omitted.

[0135] The computer readable storage medium provided in the embodiment of the present application stores a computer program, and the computer program is configured to implement the mechanical arm instrument control method when executed by a processor.

[0136] The computer readable storage medium has similar technical effects to the mechanical arm instrument control method, and thus repeated description is omitted.

[0137] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by a computer program instructing related hardware, and the program can be stored in a computer readable storage medium. When the program is executed, the program can include the processes of the above-mentioned embodiment methods. The storage medium can be a magnetic disc, an optical disc, a read-only memory (ROM) or a random access memory (RAM), etc. In the present application, the units described as separate components can be or can not be physically separated, and the components displayed as units can be or can not be physical units, i.e., they can be located in one place or distributed on multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment of the present application. In addition, the functional units in each embodiment of the present application can be integrated in one processing unit, or each unit can be physically present separately, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0138] Although the present application has been disclosed with reference to the above embodiments, the scope of the present application is not limited to the above. Various changes and modifications can be made to the present application without departing from the spirit and scope thereof, and such changes and modifications are intended to fall within the scope of the present application.

Claims

1. A robotic arm instrument control device, comprising: The method comprises the following steps: An acquisition module is configured to acquire left instrument rod position information of a left instrument arm and right instrument rod position information of a right instrument arm; A processing module is configured to obtain a left instrument rod length range relationship according to the left instrument rod position information and a right instrument rod length range relationship according to the right instrument rod position information; The left instrument rod position information comprises left instrument rod front end position information and left instrument rod distal end position information; the left instrument rod length range relationship is obtained according to the left instrument rod position information, which comprises obtaining a left instrument rod front end coordinate according to the left instrument rod front end position information, obtaining a left instrument rod distal end coordinate according to the left instrument rod distal end position information, and obtaining the left instrument rod length range relationship according to the left instrument rod front end coordinate and the left instrument rod distal end coordinate; The left instrument rod length range relationship satisfies: X L = X L2 + (X L2 -X L1 )·t L ; Y L = Y L2 + (Y L2 -Y L1 )·t L ; Z L = Z L2 + (Z L2 -Z L1 )·t L ; wherein (X L , Y L , Z L ) are coordinates of a midpoint of the length range relationship of the left instrument shaft, (X L1 , Y L1 , Z L1 ) are coordinates of the front end of the left instrument shaft, (X L2 , Y L2 , Z L2 ) are coordinates of the distal end of the left instrument shaft, and t L is a left adjustment parameter of the length range relationship of the left instrument shaft. A judging module is configured to judge that the left instrument rod and the right instrument rod collide and control the left instrument rod and the right instrument rod to stop moving when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy a preset discrimination condition.

2. The mechanical arm instrument control device of claim 1, wherein, The right instrument rod position information comprises right instrument rod front end position information and right instrument rod distal end position information; the right instrument rod length range relationship is obtained according to the right instrument rod position information, which comprises obtaining a right instrument rod front end coordinate according to the right instrument rod front end position information, obtaining a right instrument rod distal end coordinate according to the right instrument rod distal end position information, and obtaining the right instrument rod length range relationship according to the right instrument rod front end coordinate and the right instrument rod distal end coordinate. The preset discrimination condition comprises a first collision condition, a second collision condition, a third collision condition and an instrument length condition; the left instrument rod and the right instrument rod are judged to collide when the left instrument rod length range relationship and the right instrument rod length range relationship satisfy the preset discrimination condition, which comprises the following steps: The left instrument rod length range relationship satisfies:

3. The mechanical arm instrument control device of claim 2, wherein, The first collision condition satisfies: ​ ​ ​ ​ 4. The mechanical arm instrument control device of claim 3, wherein, ​ X R = X R2 + (X R2 - X R1 ) · t R ; Y R =Y R2 +(Y R2 -Y R1 )·t R ; Z R =Z R2 +(Z R2 -Z R1 )·t R ; wherein (X R , Y R , Z R ) are coordinates of the midpoint of the length range relationship of the right instrument rod, (X R1 , Y R1 , Z R1 ) are coordinates of the front end of the right instrument rod, (X R2 , Y R2 , Z R2 ) are coordinates of the distal end of the right instrument rod, and t R is the right adjustment parameter of the length range relationship of the right instrument rod.

5. The mechanical arm instrument control device of claim 4, wherein, ​ X0 = X L2 + (X L2 - X L1 ) · t L - r / sin θ kL = X R2 + (X R2 - X R1 ) · t R + r / sin θ kR ; Yo = Y L2 + (Y L2 - Y L1 ) · t L = Y R2 + (Y R2 - Y R1 ) · t R ; Z0=Z L2 +(Z L2 -Z L1 )·t L + r / cosθ kL =Z R2 +(Z R2 -Z R1 )·t R +r / cosθ kR ; The second collision condition is satisfied: X0 = X L2 + (X L2 - X L1 ) · t L + r / sin θ kL = X R2 + (X R2 - X R1 ) · t R - r / sin θ kR ; Yo = Y L2 + (Y L2 - Y L1 ) · t L = Y R2 + (Y R2 - Y R1 ) · t R ; Z0= Z L2 +(Z L2 -Z L1 ) · t L + r / cos θ kL = Z R2 +(Z R2 -Z R1 ) · t R + r / cos θ kR ; The third collision condition is satisfied: X0 = X L2 + (X L2 - X L1 ) · t L = X R2 + (X R2 - X R1 ) · t R ; Yo == Y L2 + (Y L2 - Y L1 ) · t L + r / sin θ jL = Y R2 + (Y R2 - Y R1 ) · t R - r / sin θ jR ; Z0=Z L2 +(Z L2 -Z L1 )·t L +r / cosθ jL =Z R2 +(Z R2 -Z R1 )·t R +r / 2cosθ jR ; The instrument length condition is satisfied: ； ； Among them, (X) 0， Y 0， Z0) represents the coordinates of the collision point between the left and right instrument rods, (X) L1 Y L1 Z L1 (X) represents the coordinates of the front end of the left instrument lever. L2 Y L2 Z L2 ) represents the coordinates of the distal end of the left instrument lever, t L For the left adjustment parameter, (X) R1 Y R1 Z R1 (X) represents the coordinates of the front end of the right instrument rod. R2 Y R2 Z R2 ) represents the coordinates of the distal end of the right instrument rod, t R Let θ be the right adjustment parameter. kL θ is the complementary angle of the left quadrilateral. jL Let θ be the left rotation complementary angle. kR θ is the complementary angle of the right quadrilateral. jR D is the right rotation complement. L The left lever of the robotic arm has a preset length, D. R The right instrument lever of the right instrument arm is preset to a certain length.

Images