A robot arm collision determination method and device

Abstract

The application relates to the technical field of medical equipment, in particular to a mechanical arm collision judgment method and device. A mechanical arm collision judgment method comprises the following steps: acquiring left instrument rod state information, right instrument rod state information and mirror holding arm state information; obtaining left instrument rod shell straight line relationship according to the left instrument rod state information, obtaining right instrument rod shell straight line relationship according to the right instrument rod state information, and obtaining mirror holding arm left shell straight line relationship and mirror holding arm right shell straight line relationship according to the mirror holding arm state information of the mirror holding arm; when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy a first preset discrimination condition, judging that the left mechanical arm and the mirror holding arm collide; and when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy a second preset discrimination condition, judging that the right mechanical arm and the mirror holding arm collide. The technical scheme of the application can accurately judge whether the mechanical arm collides.

Description

TECHNICAL FIELD

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

[0002] With the continuous development of science and technology, the application prospect of surgical robots becomes more and more broad, among which, the mechanical arm has precise motion control and highly flexible operation ability, and through installing different instruments on the mechanical arm, the high-precision surgical operation can be assisted by the doctor.

[0003] The laparoscopic surgical robot is gradually known and trusted by doctors and patients due to its stable performance in clinic, improved surgical efficiency and many other advantages. However, since the main screen of most laparoscopic surgical robot consoles has a field of view for the lesion position, it is not easy to detect the collision between the slide rods of the mechanical arm during movement, which may affect the operation. SUMMARY

[0004] The problem solved by the present application is how to accurately judge whether the mechanical arm collides.

[0005] To solve the above problems, the present application provides a mechanical arm collision judgment method and device.

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

[0007] Obtaining left instrument rod state information of a left mechanical arm, right instrument rod state information of a right mechanical arm and mirror holding arm state information of a mirror holding arm;

[0008] According to the left instrument rod state information, a left instrument rod shell straight line relationship is obtained, according to the right instrument rod state information, a right instrument rod shell straight line relationship is obtained, and according to the mirror holding arm state information of the mirror holding arm, a mirror holding arm left shell straight line relationship and a mirror holding arm right shell straight line relationship are obtained;

[0009] When the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy a first preset discrimination condition, it is judged that the left mechanical arm and the mirror holding arm collide, and when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy a second preset discrimination condition, it is judged that the right mechanical arm and the mirror holding arm collide.

[0010] Optionally, the left instrument rod state information includes left instrument rod end coordinates, left instrument rod distal end coordinates, left instrument rod joint included angle, left instrument rod quadrilateral residual angle, left mechanical arm shell distance and left mechanical arm center axis distance; according to the left instrument rod state information, the left instrument rod shell straight line relationship is obtained, which comprises:

[0011] According to the left instrument rod joint included angle, X-axis left instrument rod joint included angle, Y-axis left instrument rod joint included angle and Z-axis left instrument rod joint included angle are obtained;

[0012] According to the left instrument rod end coordinates, the left instrument rod distal end coordinates, the left instrument rod joint included angle, the left instrument rod quadrilateral residual angle, the left mechanical arm center axis distance and the left mechanical arm shell distance, the left instrument rod shell straight line relationship is generated.

[0013] Optionally, the left instrument rod shell straight line relationship satisfies:

[0014] X L =X L3 +(X L1 cosθ PLX -X L3 )*t L +d L sinθ KL ;

[0015] Y L =Y L3 +(Y L1 cosθ PLY -Y L3 )*t L -d Lr ;

[0016] Z L =Z L3 +(Z L1 cosθ PLZ -Z L3 )*t L +d L cosθ KL ;

[0017] Wherein, (X L , Y L , Z L ) is the left instrument rod shell straight line relationship coordinates, (X L1 , Y L1 , Z L1 ) is the left instrument rod end coordinates, (X L3 , Y L3 , Z L3 ) is the left instrument rod distal end coordinates, cosθ PLX is the X-axis component of the left instrument rod joint included angle, cosθ PLY is the Y-axis component of the left instrument rod joint included angle, cosθ PLZ is the Z-axis of the left instrument rod joint included angle, θ KL is the left instrument rod quadrilateral residual angle, t Lis a coordinate of the right instrument rod outer shell linear relationship, d L is a right mechanical arm center axis distance, d Lr is a right mechanical arm outer shell distance.

[0018] Optionally, the right instrument rod state information comprises a right instrument rod end coordinate, a right instrument rod distal end coordinate, a right instrument rod joint included angle, a right instrument rod quadrilateral residual angle, the right mechanical arm outer shell distance, and the right mechanical arm center axis distance; the right instrument rod outer shell linear relationship according to the right instrument rod state information comprises:

[0019] The right instrument rod outer shell linear relationship is generated according to the right instrument rod end coordinate, the right instrument rod distal end coordinate, the right instrument rod joint included angle, the right instrument rod quadrilateral residual angle, the right mechanical arm center axis distance, and the right mechanical arm outer shell distance.

[0020] Optionally, the right instrument rod outer shell linear relationship satisfies:

[0021] X R = X R3 + (X R1 cos θ PRX - X R3 ) * t R + d R sin θ KR ;

[0022] Y R = Y R3 + (Y R1 cos θ PRY - Y R3 ) * t R + d Rl ;

[0023] Z R = Z R3 + (Z R1 cos θ PRZ - Z R3 ) * t R + d R cos θ KR ;

[0024] wherein (X R , Y R , Z R ) is a coordinate of the right instrument rod outer shell linear relationship, (X R1 , Y R1 , Z R1 ) is a right instrument rod end coordinate, (X R3 , Y R3 , Z R3) is the right instrument rod distal end coordinate, cosθ PRX is the X-axis component of the right instrument rod joint angle, cosθ PRY is the Y-axis component of the right instrument rod joint angle, cosθ PRZ is the Z-axis component of the right instrument rod joint angle, θ KR is the right instrument rod quadrilateral residual angle, t R is the right instrument rod housing linear relationship adjustment parameter, d R is the right mechanical arm center axis distance, d Rl is the right mechanical arm housing distance.

[0025] Optionally, the holding mirror arm state information includes a lens end coordinate, a lens distal end coordinate, a holding mirror arm quadrilateral residual angle, a holding mirror arm left housing distance, a holding mirror arm right housing distance, and a lens center axis distance; and the holding mirror arm left housing linear relationship and the holding mirror arm right housing linear relationship are obtained according to the holding mirror arm state information of the holding mirror arm, including:

[0026] generating the holding mirror arm left housing linear relationship according to the lens end coordinate, the lens distal end coordinate, the holding mirror arm quadrilateral residual angle, and the holding mirror arm left housing distance;

[0027] generating the holding mirror arm right housing linear relationship according to the lens end coordinate, the lens distal end coordinate, the holding mirror arm quadrilateral residual angle, and the holding mirror arm right housing distance.

[0028] Optionally, the holding mirror arm left housing linear relationship satisfies:

[0029] X CL =X C2 +(X C1 -X C2 )*t C +d C sinθ kC ;

[0030] Y CL =Y C2 +(Y C1 -Y C2 )*t C +d Cl ;

[0031] Z CL =Z C2 +(Z C1 -Z C2 )*t C +d C cosθ kC ;

[0032] The right holding arm shell linear relationship satisfies:

[0033] X CR =X C2 +(X C1 -X C2 )*t C +d C sinθ kC ;

[0034] Y CR =Y C2 +(Y C1 -Y C2 )*t C -d Cr ;

[0035] Z CR =Z C2 +(Z C1 -Z C2 )*t C +d C cosθ kC ;

[0036] Wherein, (X CL , Y CL , Z CL ) is the coordinate of the left holding arm shell linear relationship, (X CR , Y CR , Z CR ) is the coordinate of the right holding arm shell linear relationship, (X C1 , Y C1 , Z C1 ) is the coordinate of the lens end, (X C2 , Y C2 , Z C2 ) is the coordinate of the lens far end, t C is the adjustment parameter of the left holding arm shell linear relationship, θ kC is the quadrilateral residual angle of the holding arm, d Cl is the left holding arm shell distance, d Cr is the right holding arm shell distance, and d C is the lens rod distance.

[0037] Optionally, the first preset judgment condition includes a first intersection condition and a first range condition; the second preset judgment condition includes a second intersection condition and a second range condition; when the left instrument rod shell linear relationship and the left holding arm shell linear relationship satisfy the first preset judgment condition, it is judged that the left mechanical arm and the holding arm collide, and when the right instrument rod shell linear relationship and the right holding arm shell linear relationship satisfy the second preset judgment condition, it is judged that the right mechanical arm and the holding arm collide, including:

[0038] obtaining a left slide table length, a left joint angle, a left quadrilateral angle, a right slide table length, a right joint angle and a right quadrilateral angle;

[0039] obtaining the first range condition according to the left slide table length, the left joint angle, the left quadrilateral angle and a left instrument rod distal end coordinate, and obtaining the second range condition according to the right slide table length, the right joint angle, the right quadrilateral angle and a right instrument rod distal end coordinate;

[0040] obtaining a left intersection point coordinate when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy the first intersection condition;

[0041] judging that the left mechanical arm and the mirror holding arm collide when the left intersection point coordinate satisfies the first range condition;

[0042] obtaining a right intersection point coordinate when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy the second intersection condition;

[0043] judging that the right mechanical arm and the mirror holding arm collide when the right intersection point coordinate satisfies the second range condition.

[0044] Optionally, the first intersection condition satisfies:

[0045] X OL = X L3 + (X L1 cos θ PLX - X L3 ) * t L + d L sin θ KL = X C2 + (X C1 - X C2 ) * t C + d C sin θ kC ;

[0046] Y OL = Y L3 + (Y L1 cos θ PLY - Y L3 ) * t L - d Lr = Y C2 + (Y C1 - Y C2 ) * t C + d Cl ;

[0047] Z OL = Z L3+ (Z L1 cos θ PLZ - Z L3 )* t L + d L cos θ KL = Z C2 + (Z C1 - Z C2 )* t C + d C cos θ kC ;

[0048] The first range condition is satisfied:

[0049] X L3 < X OL < X L3 + H L * cos θ rLX + H L * cos θ wLX ;

[0050] Y L3 < Y OL < Y L3 + H L * cos θ rLY + H L * cos θ wLY ;

[0051] Z L3 < Z OL < Z L3 + H L * cos θ rLZ + H L * cos θ wLZ ;

[0052] The second intersection condition is satisfied:

[0053] X OR = X R3 + (X R1 cos θ PRX - X R3 )* t R + d R sin θ KR = X C2 + (X C1 - X C2 )* t C + d C sin θ kC ;

[0054] Y OR = Y R3 + (Y R1 cos θPRY -Y R3 )*t R +d Rl =Y C2 +(Y C1 -Y C2 )*t C -d Cr ;

[0055] Z OR =Z R3 +(Z R1 cosθ PRZ -Z R3 )*t R +d R cosθ KR =Z C2 +(Z C1 -Z C2 )*t C +d C cosθ kC ;

[0056] The second range condition is satisfied:

[0057] X R3 <X OR <X R3 +H R *cosθ rRX +H R *cosθ wRX ;

[0058] Y R3 <Y OR <Y R3 +H R *cosθ rRY +H R *cosθ wRY ;

[0059] Z R3 <Z OR <Z R3 +H R *cosθ rRZ +H R *cosθ wRZ ;

[0060] Among them, (X) OL Y OL Z OL (X) is the left intersection point, (X) OR Y OR Z OR ) represents the coordinates of the right intersection point, θ rLX Let θ be the X-axis component of the left joint angle.rLY is a Y-axis component of the left joint angle, θ rLZ is a Z-axis component of the left joint angle, θ wLX is an X-axis component of the left quadrilateral angle, θ wLY is a Y-axis component of the left quadrilateral angle, θ wLZ is a Z-axis component of the left quadrilateral angle, θ rLX is an X-axis component of the left joint angle, θ rRY is a Y-axis component of the left joint angle, θ rRZ is a Z-axis component of the left joint angle, θ wRX is an X-axis component of the left quadrilateral angle, θ wRY is a Y-axis component of the left quadrilateral angle, θ wRZ is a Z-axis component of the left quadrilateral angle, H L is the left slide length, H R is the right slide length.

[0061] In a second aspect, a mechanical arm collision judgment device includes:

[0062] An acquisition module is configured to acquire left instrument rod state information of a left mechanical arm, right instrument rod state information of a right mechanical arm, and mirror holding arm state information of a mirror holding arm.

[0063] A processing module is configured to obtain a left instrument rod shell straight line relationship according to the left instrument rod state information, obtain a right instrument rod shell straight line relationship according to the right instrument rod state information, and obtain a mirror holding arm left shell straight line relationship and a mirror holding arm right shell straight line relationship according to the mirror holding arm state information.

[0064] A judgment module is configured to judge that the left mechanical arm and the mirror holding arm collide when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy a first preset discrimination condition, and judge that the right mechanical arm and the mirror holding arm collide when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy a second preset discrimination condition.

[0065] The mechanical arm collision judgment method, device and storage medium have the following beneficial effects: in a three-dimensional space, the motion state and motion track of the mechanical arm can be accurately judged according to the acquired left instrument rod state information, right instrument rod state information and mirror holding arm state information, so that whether the collision between the mechanical arms occurs can be accurately judged according to the state information; the linear relationship of the left instrument rod shell is constructed according to the left instrument rod state information, so that the linear track of the shell on the side close to the mirror holding arm of the left mechanical arm in the three-dimensional space can be obtained, the linear relationship of the left shell of the mirror holding arm is constructed according to the mirror holding arm state information, so that the linear track of the shell on the side close to the left mechanical arm of the mirror holding arm can also be obtained, and whether the collision between the right shell of the left mechanical arm and the left shell of the mirror holding arm occurs can be accurately judged through the two shell linear tracks, and in the same way, whether the collision between the right mechanical arm and the mirror holding arm occurs can also be judged according to the constructed linear relationship of the right mechanical arm shell and the linear relationship of the right shell of the mirror holding arm; whether the linear relationship of the left instrument rod shell and the linear relationship of the left shell of the mirror holding arm satisfy the first preset judgment condition is further judged, if yes, it is judged that the collision between the left mechanical arm and the mirror holding arm occurs, and if not, it is indicated that the collision between the left mechanical arm and the mirror holding arm does not occur, and in the same way, whether the collision between the right mechanical arm and the mirror holding arm occurs is judged through judging whether the linear relationship of the right instrument shell and the linear relationship of the right shell of the mirror holding arm satisfy the second preset condition, and through the above method, the collision that is not easy to be perceived by the doctor can be accurately judged, the collision is accurately judged at the first time when the shells of the mechanical arms contact, and the occurrence of the accident can be effectively avoided, the risk in the operation process is reduced, and the work efficiency of the doctor is improved. BRIEF DESCRIPTION OF DRAWINGS

[0066] Figure 1 A flowchart of a mechanical arm collision judgment method of an embodiment of the present application is shown in the figure.

[0067] Figure 2 A structural diagram of a left instrument arm instrument rod and a left sliding table rod of an embodiment of the present application is shown in the figure.

[0068] Figure 3 A structural diagram of a left mechanical arm and a left quadrilateral residual angle of a left instrument arm instrument rod mounted on the left mechanical arm of an embodiment of the present application is shown in the figure.

[0069] Figure 4 A structural diagram of a mechanical arm collision judgment device of an embodiment of the present application is shown in the figure. DETAILED DESCRIPTION

[0070] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the accompanying drawings and embodiments of the present invention are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0071] It should be understood that the various steps described in the method embodiments of the present invention may be performed in different orders and / or in parallel. Furthermore, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this respect.

[0072] The term "comprising" and its variations as used herein are open-ended, meaning "including but not limited to". The term "based on" means "at least partially based 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 "optionally" means "optional embodiments". Definitions of other terms will be given in the following description. It should be noted that the concepts of "first", "second", etc., mentioned in this invention are used only to distinguish different devices, modules, or units, and are not intended to limit the order of functions performed by these devices, modules, or units or their interdependencies.

[0073] It should be noted that the terms "a" and "a plurality of" used in this invention are illustrative rather than restrictive. Those skilled in the art should understand that, unless otherwise expressly indicated in the context, they should be understood as "one or more".

[0074] In the robotic arm collision detection method of this embodiment, a three-dimensional spatial model is first constructed, with forward as the positive X-axis, left as the positive Y-axis, and upward as the positive Z-axis. The three-arm, six-DOF laparoscopic surgical robot is located within this three-dimensional space, as follows: Figure 2 As shown, the quadrilateral angles are the angles formed by the rotation of the instrument rod in the X-axis and Z-axis planes. The three robotic arms are arranged from left to right as the left robotic arm, the lens-holding arm, and the right robotic arm. Therefore, the common collision modes are collisions between the left robotic arm and the lens-holding arm, and collisions between the right robotic arm and the lens-holding arm. The left and right robotic arms are used to mount the instruments, while the middle lens-holding arm is used to mount the lens.

[0075] like Figure 1 As shown, this embodiment of the invention provides a method for determining collisions in a robotic arm, including:

[0076] Step S1, obtaining left instrument rod state information of the left mechanical arm, right instrument rod state information of the right mechanical arm and mirror holding arm state information of the mirror holding arm;

[0077] Specifically, the state information of the left instrument rod, the right instrument rod and the mirror holding arm in the three-dimensional space is obtained, which includes position information, angle information and distance information. The motion state and trajectory of the left mechanical arm, the right mechanical arm and the mirror holding arm can be accurately judged through the state information, and the collision of the mechanical arms can be accurately judged according to the motion state.

[0078] Step S2, obtaining left instrument rod shell straight line relationship according to the left instrument rod state information, obtaining right instrument rod shell straight line relationship according to the right instrument rod state information, and obtaining mirror holding arm left shell straight line relationship and mirror holding arm right shell straight line relationship according to the mirror holding arm state information;

[0079] Specifically, as shown in Figure 2 the left instrument rod shell straight line relationship, i.e. the straight line relationship of the left mechanical arm shell on the right side close to the side of the mirror holding arm. Similarly, the right instrument rod shell straight line relationship is constructed according to the right instrument rod state information, and the mirror holding arm left shell straight line relationship and the mirror holding arm right shell straight line relationship are constructed according to the mirror holding arm state information. Through the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship, it can be used to judge whether the linear trajectory of the left shell of the left mechanical arm and the left shell of the mirror holding arm exists in the three-dimensional space. If the two mechanical arms collide, the above two shells first come into contact, so the straight line relationship of the left mechanical arm and the mirror holding arm must exist in the three-dimensional space. Similarly, whether the right instrument shell straight line relationship and the mirror holding arm right shell straight line relationship exist in the three-dimensional space can also be used to judge whether the two mechanical arms collide.

[0080] Step S3, when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy the first preset discrimination condition, it is judged that the left mechanical arm and the mirror holding arm collide, and when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy the second preset discrimination condition, it is judged that the right mechanical arm and the mirror holding arm collide.

[0081] Specifically, when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship exist in the three-dimensional space, and the intersection position is on the linear trajectory of the left mechanical arm, i.e. the intersection point coincides with the coordinate point of a certain point of the left mechanical arm in the three-dimensional space, it is indicated that the left instrument rod satisfies the first preset discrimination condition, and it is judged that the left mechanical arm and the mirror holding arm collide. Similarly, when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy the second preset discrimination condition, it is indicated that the right mechanical arm and the mirror holding arm collide.

[0082] In this embodiment, in the three-dimensional space, according to the obtained left instrument rod state information, right instrument rod state information and mirror holding arm state information, the motion state and motion trajectory of the mechanical arm can be accurately judged, so that whether the collision between the mechanical arms occurs can be accurately judged according to the state information; the left instrument rod shell straight line relationship is constructed according to the left instrument rod state information, so that the straight line trajectory of the shell on the side close to the mirror holding arm of the left mechanical arm in the three-dimensional space can be obtained, the mirror holding arm left shell straight line relationship is constructed according to the mirror holding arm state information, so that the straight line trajectory of the shell on the side close to the left mechanical arm of the mirror holding arm can also be obtained, and whether the collision between the right shell of the left mechanical arm and the left shell of the mirror holding arm occurs can be accurately judged through the two shell straight line trajectories, and the same is true for judging whether the right mechanical arm and the mirror holding arm collide according to the constructed right mechanical arm shell straight line relationship and mirror holding arm right shell straight line relationship; specifically, whether the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy a first preset judgment condition is judged, if yes, it is judged that the left mechanical arm and the mirror holding arm collide, and if not, it is indicated that the left mechanical arm and the mirror holding arm do not collide, and in the same way, whether the right mechanical arm and the mirror holding arm collide is judged by judging whether the right instrument shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy a second preset condition, and through the above method, the collision that is not easy for the doctor to perceive can be accurately judged, the collision is accurately judged at the first time when the shells of the mechanical arms contact, and the occurrence of accidents can be effectively avoided, the risk in the surgical process is reduced, and the work efficiency of the doctor is improved.

[0083] In an optional embodiment, the left instrument rod state information includes a left instrument rod end coordinate, a left instrument rod distal end coordinate, a left instrument rod joint included angle, a left instrument rod quadrilateral residual angle, a left mechanical arm shell distance and a left mechanical arm center axis distance, and the left instrument rod shell straight line relationship is obtained according to the left instrument rod state information, including:

[0084] The left instrument rod shell straight line relationship is generated according to the left instrument rod end coordinate, the left instrument rod distal end coordinate, the left instrument rod joint included angle, the left instrument rod quadrilateral residual angle, the left mechanical arm center axis distance and the left mechanical arm shell distance.

[0085] In an optional embodiment, the left instrument rod shell straight line relationship satisfies:

[0086] X L =X L3 +(X L1 cosθ PLX -X L3 )*t L +d L sinθ KL ;

[0087] Y L = Y L3 + (Y L1 cos θ PLY - Y L3 )*t L - d Lr ;

[0088] Z L = Z L3 + (Z L1 cos θ PLZ - Z L3 )*t L + d L cos θ KL ;

[0089] wherein (X L , Y L , Z L ) is the coordinate of the straight line relationship of the left instrument rod shell, (X L1 , Y L1 , Z L1 ) is the coordinate of the left instrument rod end, (X L3 , Y L3 , Z L3 ) is the coordinate of the left instrument rod distal end, cos θ PLX is the left instrument rod joint included angle of the X axis, cos θ PLY is the left instrument rod joint included angle of the Y axis, cos θ PLZ is the left instrument rod joint included angle of the Z axis, θ KL is the left instrument rod quadrilateral residual angle, t L is the adjustment parameter of the straight line relationship of the left instrument rod shell, d L is the distance of the left mechanical arm center axis, d Lr is the distance of the left mechanical arm shell.

[0090] In an optional embodiment, the right instrument rod state information comprises right instrument rod end coordinate, right instrument rod distal end coordinate, right instrument rod joint included angle, right instrument rod quadrilateral residual angle, right mechanical arm shell distance and right mechanical arm center axis distance, and the right instrument rod shell straight line relationship is obtained according to the right instrument rod state information, comprising:

[0091] generating the right instrument rod shell straight line relationship according to the right instrument rod end coordinate, right instrument rod distal end coordinate, right instrument rod joint included angle, right instrument rod quadrilateral residual angle, right mechanical arm center axis distance and right mechanical arm shell distance.

[0092] In an alternative embodiment, the right instrument bar housing linear relationship satisfies:

[0093] X R = X R3 + (X R1 cos θ PRX - X R3 )*t R + d R sin θ KR ;

[0094] Y R = Y R3 + (Y R1 cos θ PRY - Y R3 )*t R + d Rl ;

[0095] Z R = Z R3 + (Z R1 cos θ PRZ - Z R3 )*t R + d R cos θ KR ;

[0096] wherein (X R , Y R , Z R ) is the coordinate of the right instrument bar housing linear relationship, (X R1 , Y R1 , Z R1 ) is the coordinate of the right instrument bar end, (X R3 , Y R3 , Z R3 ) is the coordinate of the right instrument bar distal end, cos θ PRX is the X-axis right instrument bar joint angle, cos θ PRY is the Y-axis right instrument bar joint angle, cos θ PRZ is the Z-axis right instrument bar joint angle, θ KR is the right instrument bar quadrilateral residual angle, t R is the adjustment parameter of the right instrument bar housing linear relationship, d R is the right mechanical arm center axis distance, d Rl is the right mechanical arm housing distance.

[0097] Specifically, as shown in Figure 3 wherein θ KLP1 is the end of the left instrument rod, P2 is the front end of the left instrument rod, P3 is the distal end of the left instrument rod, W is the shell of the left mechanical arm sliding table, and the straight line equation of the shell of the sliding table on the positive side of the Y axis is the left instrument rod shell straight line relationship in the scheme, the sliding table shell axis of the left mechanical arm represented by the left instrument rod shell straight line relationship is closer to the mirror holding arm, and the first collision occurs when a collision occurs. Obtain the spatial position information of the left instrument rod in the three-dimensional space model, which includes the corresponding coordinates of the left instrument rod end and the distal end in the three-dimensional space model, and further obtain the angle information of the left instrument rod, which includes the joint included angle of the left instrument rod and the quadrilateral residual angle, wherein θ PR The joint included angle of the left instrument rod is the included angle of the line connecting the front end of the left instrument rod and the end of the left instrument rod and the extension line of the left instrument rod, and the quadrilateral residual angle is the included angle of the perpendicular line of the left instrument rod and the Z axis. Since the instrument pitch joint is a driven joint, the output signal of the encoder is a pulse signal, and its frequency is proportional to the rotation angle, so the pitch joint angle can be obtained by the number of pulses of the encoder output signal and the resolution of the encoder, wherein the angle = number of pulses of the encoder output signal / resolution of the encoder * 360°, and the quadrilateral angle can also be obtained, and then the residual angle of the quadrilateral angle is obtained. According to the joint included angle of the left instrument rod, the left instrument rod end coordinate and the left instrument rod distal end coordinate, the straight line parameter equation of the left instrument rod is generated, that is, the straight line parameter equation of the line connecting the two points is constructed according to the coordinates of the two points in space, and the center axis of the sliding table rod and the center axis of the instrument rod have a parallel relationship in space, therefore, according to the left instrument rod quadrilateral residual angle and the distance between the center axes of the left mechanical arm, the X axis positive direction translation distance d R sinθ KR of the left instrument rod straight line parameter equation is obtained, and the Z axis positive direction translation distance d R cosθ KR of the left instrument rod straight line parameter equation is obtained, and the Y axis negative direction translation distance d Rl of the left instrument rod straight line parameter equation is obtained according to the left mechanical arm shell distance. The straight line parameter equation of the left instrument rod is translated according to the translation, so that the left instrument rod shell straight line relationship is finally obtained, that is, the straight line equation of the shell on the side of the left mechanical arm close to the mirror holding arm. According to the right instrument rod end coordinate, the right instrument rod distal end coordinate, the right instrument rod joint included angle, the right instrument rod quadrilateral residual angle, the distance between the center axes of the right mechanical arm and the right mechanical arm shell distance, the right instrument rod shell straight line relationship can also be obtained, that is, the straight line relationship of the shell on the side of the right mechanical arm close to the mirror holding arm.

[0098] In the optional embodiment, the linear relationship of the left instrument rod shell of the mechanical arm close to the shell of the mirror holding arm is obtained according to the state information of the left instrument rod, that is, the linear relationship of the right shell of the left mechanical arm. The linear relationship of the right shell of the left mechanical arm can be obtained through the linear relationship, and the intersection coordinates of the two linear relationships in the three-dimensional space can be obtained by solving the two linear relationships, so that the collision of the mechanical arm can be accurately judged according to the intersection coordinates.

[0099] In an optional embodiment, the state information of the mirror holding arm includes the lens end coordinate, the lens far end coordinate, the mirror holding arm quadrilateral residual angle, the mirror holding arm left shell distance, the mirror holding arm right shell distance and the lens center axis distance. The mirror holding arm left shell linear relationship and the mirror holding arm right shell linear relationship are obtained according to the state information of the mirror holding arm, and the method comprises:

[0100] According to the lens end coordinate, the lens far end coordinate, the mirror holding arm quadrilateral residual angle and the mirror holding arm left shell distance, the mirror holding arm left shell linear relationship is generated.

[0101] According to the lens end coordinate, the lens far end coordinate, the mirror holding arm quadrilateral residual angle and the mirror holding arm right shell distance, the mirror holding arm right shell linear relationship is generated.

[0102] In an optional embodiment, the mirror holding arm left shell linear relationship satisfies:

[0103] X CL =X C2 +(X C1 -X C2 )*t C +d C sinθ kC ;

[0104] Y CL =Y C2 +(Y C1 -Y C2 )*t C +d Cl ;

[0105] Z CL =Z C2 +(Z C1 -Z C2 )*t C +d C cosθ kC ;

[0106] The mirror holding arm right shell linear relationship satisfies:

[0107] XCR = X C2 + (X C1 - X C2 )*t C + d C sin θ kC ;

[0108] Y CR = Y C2 + (Y C1 - Y C2 )*t C - d Cr ;

[0109] Z CR = Z C2 + (Z C1 - Z C2 )*t C + d C cos θ kC ;

[0110] wherein (X CL , Y CL , Z CL ) is the coordinate of the left housing linear relationship of the mirror holding arm, (X CR , Y CR , Z CR ) is the coordinate of the right housing linear relationship of the mirror holding arm, the left housing linear relationship of the mirror holding arm is the (X C1 , Y C1 , Z C1 ), the end of the lens coordinate is (X C2 , Y C2 , Z C2 ), θ kC is the quadrilateral angle of the mirror holding arm, d Cl is the left housing distance of the mirror holding arm, d Cr is the right housing distance of the mirror holding arm, and d C is the lens rod distance.

[0111] Specifically, the lens straight line parameter equation is generated according to the lens end coordinate and the lens far end coordinate, the lens straight line parameter equation is translated according to the lens rod distance and the holding lens arm quadrilateral residual angle, i.e. the residual angle of the quadrilateral angle, to obtain the translation distance of the X-axis square and the Z-axis positive direction of the lens straight line parameter equation, and the translation distance of the Y-axis positive direction of the lens straight line equation is obtained according to the holding lens arm left shell distance, wherein the holding lens arm left shell distance is the distance from the center axis straight line of the slide table of the holding lens arm to the left shell of the holding lens arm, and the left shell is the shell close to the left mechanical arm, and similarly, the holding lens arm right shell distance is the shell close to the right mechanical arm of the holding lens arm, so that after the lens straight line parameter equation is translated according to the above translation distances, the holding lens arm left shell straight line relationship, i.e. the straight line relationship of the shell close to the left mechanical arm of the holding lens arm, is obtained, and similarly, the translation distance of the X-axis square and the Z-axis positive direction of the lens straight line parameter equation is obtained according to the lens rod distance and the holding lens arm quadrilateral residual angle, the Y-axis negative direction translation distance is obtained according to the holding lens arm right shell distance, and then the holding lens arm right shell straight line relationship, i.e. the straight line relationship of the shell close to the right mechanical arm of the holding lens arm, is obtained.

[0112] In the optional embodiment, the relative position relationship of the left and right mechanical arms and the holding lens arm shell can be accurately judged through the holding lens arm left shell straight line relationship and the holding lens arm right shell straight line relationship, and whether the two mechanical arms collide can be further judged by whether there is a common coordinate point in the current relationship of the mechanical arm shell, so that the collision can be accurately judged.

[0113] In an optional embodiment, the first preset judgment condition includes a first intersection condition and a first range condition, the second preset judgment condition includes a second intersection condition and a second range condition, and when the left instrument rod shell straight line relationship and the holding lens arm left shell straight line relationship satisfy the first preset judgment condition, it is judged that the left mechanical arm and the holding lens arm collide, and when the right instrument rod shell straight line relationship and the holding lens arm right shell straight line relationship satisfy the second preset judgment condition, it is judged that the right mechanical arm and the holding lens arm collide, which includes:

[0114] The left slide table length, the left joint angle, the left quadrilateral angle, the right slide table length, the right joint angle and the right quadrilateral angle are obtained.

[0115] The first range condition is obtained according to the left slide table length, the left joint angle, the left quadrilateral angle and the left instrument rod far end coordinate, and the second range condition is obtained according to the right slide table length, the right joint angle, the right quadrilateral angle and the right instrument rod far end coordinate.

[0116] The left intersection point coordinate is obtained according to the left instrument rod shell straight line relationship, the holding lens arm left shell straight line relationship and the first intersection condition.

[0117] When the left intersection point coordinate satisfies a first range condition, it is determined that the left mechanical arm and the mirror holding arm collide.

[0118] According to the right instrument rod shell straight line relationship, the mirror holding arm right shell straight line relationship, and the second intersection condition, a right intersection point coordinate is obtained.

[0119] When the right intersection point coordinate satisfies a second range condition, it is determined that the right mechanical arm and the mirror holding arm collide.

[0120] In an optional embodiment, the first intersection condition satisfies:

[0121] X OL =X L3 +(X L1 cosθ PLX -X L3 )*t L +d L sinθ KL =X C2 +(X C1 -X C2 )*t C +d C sinθ kC ;

[0122] Y OL =Y L3 +(Y L1 cosθ PLY -Y L3 )*t L -d Lr =Y C2 +(Y C1 -Y C2 )*t C +d Cl ;

[0123] Z OL =Z L3 +(Z L1 cosθ PLZ -Z L3 )*t L +d L cosθ KL =Z C2 +(Z C1 -Z C2 )*t C +d C cosθ kC ;

[0124] The first range condition satisfies:

[0125] X L3 <XOL < X L3 + H L *cos theta rLX + H L *cos theta wLX ;

[0126] Y L3 < Y OL < Y L3 + H L *cos theta rLY + H L *cos theta wLY ;

[0127] Z L3 < Z OL < Z L3 + H L *cos theta rLZ + H L *cos theta wLZ ;

[0128] The second intersection condition is satisfied:

[0129] X OR = X R3 + (X R1 cos theta PRX - X R3 ) * t R + d R sin theta KR = X C2 + (X C1 - X C2 ) * t C + d C sin theta kC ;

[0130] Y OR = Y R3 + (Y R1 cos theta PRY - Y R3 ) * t R + d Rl = Y C2 + (Y C1 - Y C2 ) * t C - d Cr ;

[0131] Z OR = Z R3 + (Z R1 cos theta PRZ - Z R3 ) * t R + d R cos thetaKR = Z C2 + (Z C1 - Z C2 )*t C + d C cos θ kC ;

[0132] The second range condition is satisfied:

[0133] X R3 < X OR < X R3 + H R *cos θ rRX + H R *cos θ wRX ;

[0134] Y R3 < Y OR < Y R3 + H R *cos θ rRY + H R *cos θ wRY ;

[0135] Z R3 < Z OR < Z R3 + H R *cos θ rRZ + H R *cos θ wRZ ;

[0136] wherein (X OL , Y OL , Z OL ) is the left intersection point, (X OR , Y OR , Z OR ) is the right intersection point coordinate, θ rLX is the X-axis component of the left joint angle, θ rLY is the Y-axis component of the left joint angle, θ rLZ is the Z-axis component of the left joint angle, θ wLX is the X-axis component of the left quadrilateral angle, θ wLY is the Y-axis component of the left quadrilateral angle, θ wLZ is the Z-axis component of the left quadrilateral angle, θ rLX is the X-axis component of the left joint angle, θ rRY is the Y-axis component of the left joint angle, θ rRZ is the Z-axis component of the left joint angle, θ wRX is the X-axis component of the left quadrilateral angle, θ wRYY axis component of the left quadrilateral angle, θ wRZ Z axis component of the left quadrilateral angle, H L The left slide table length, H R The right slide table length.

[0137] Specifically, the left slide table length, the left joint angle and the left quadrilateral angle are obtained, wherein the left slide table length is the length of the left mechanical arm slide table rod, and the right slide table rod is the length of the right mechanical arm slide table rod, the left joint angle and the first range condition is obtained according to the left slide table length, the left joint angle and the left quadrilateral angle, the range condition is the positional relationship of the left mechanical arm slide table rod in the three-dimensional space, when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy the first intersection condition, the left intersection point coordinates are obtained, and the left intersection point left side satisfies the first range condition, that is, the left mechanical arm and the mirror holding arm exist intersection points, and the intersection points are on the left mechanical arm slide table, then it is judged that the left mechanical arm and the mirror holding arm are inflated, and when the right mechanical arm and the mirror holding arm satisfy the second intersection condition and the second range condition at the same time, it is judged that the right mechanical arm and the mirror holding arm collide.

[0138] In the optional embodiment, by judging whether the mirror holding arm satisfies the preset condition with the left mechanical arm and the right mechanical arm respectively, it is accurately judged whether the mechanical arm collides, the judgment structure can be accurately obtained at the first time when the collision occurs, so as to avoid the influence of inflation on the operation progress, improve the work efficiency and operation of the doctor, and further reduce the operation risk.

[0139] As shown in Figure 4 Another embodiment of the application provides a mechanical arm collision judgment device, which comprises:

[0140] An acquisition module is configured to acquire left instrument rod state information of a left mechanical arm, right instrument rod state information of a right mechanical arm and mirror holding arm state information of a mirror holding arm.

[0141] A processing module is configured to obtain a left instrument rod shell straight line relationship according to the left instrument rod state information, obtain a right instrument rod shell straight line relationship according to the right instrument rod state information, and obtain a mirror holding arm left shell straight line relationship and a mirror holding arm right shell straight line relationship according to the mirror holding arm state information.

[0142] A judgment module is configured to judge that the left mechanical arm and the mirror holding arm collide when the left instrument rod shell straight line relationship and the mirror holding arm left shell straight line relationship satisfy a first preset discrimination condition, and judge that the right mechanical arm and the mirror holding arm collide when the right instrument rod shell straight line relationship and the mirror holding arm right shell straight line relationship satisfy a second preset discrimination condition.

[0143] The mechanical arm collision judgment device in the embodiment of the present application has similar technical effects to the mechanical arm collision judgment method, and thus will not be described again.

[0144] 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), a random access memory (RAM), or the like. In the present application, the units described as separate components can or can not be physically separated, and the components shown as units can or can not be physical units, that is, they can be located in one place, or they can be distributed on multiple network units. Part 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 exist physically independently, 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.

[0145] Although the present application is disclosed as above, the protection scope of the present application is not limited to this. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present application, and these changes and modifications will fall within the protection scope of the present application.

Claims

1. A robot arm collision determination method characterized by comprising: The method comprises the following steps: obtaining left instrument rod state information of a left mechanical arm, right instrument rod state information of a right mechanical arm, and mirror holding arm state information of a mirror holding arm; obtaining left instrument rod shell straight line relationship according to the left instrument rod state information, obtaining right instrument rod shell straight line relationship according to the right instrument rod state information, and obtaining left mirror holding arm shell straight line relationship and right mirror holding arm shell straight line relationship according to the mirror holding arm state information; when the left instrument rod shell straight line relationship and the left mirror holding arm shell straight line relationship satisfy a first preset discrimination condition, judging that the left mechanical arm and the mirror holding arm collide, and when the right instrument rod shell straight line relationship and the right mirror holding arm shell straight line relationship satisfy a second preset discrimination condition, judging that the right mechanical arm and the mirror holding arm collide; the left instrument rod state information comprises left instrument rod end coordinate, left instrument rod distal end coordinate, left instrument rod joint included angle, left instrument rod quadrilateral residual angle, left mechanical arm shell distance, and left mechanical arm center axis distance; the left instrument rod shell straight line relationship is obtained according to the left instrument rod state information, which comprises generating the left instrument rod shell straight line relationship according to the left instrument rod end coordinate, the left instrument rod distal end coordinate, the left instrument rod joint included angle, the left instrument rod quadrilateral residual angle, the left mechanical arm center axis distance, and the left mechanical arm shell distance; the left instrument rod shell straight line relationship satisfies: X L = X L3 + (X L1 cosθ PLX - X L3 )·t L +d L sinθ KL ; Y L =Y L3 +(Y L1 cosθ PLY -Y L3 )·t L -d Lr ; Z L =Z L3 +(Z L1 cosθ PLZ -Z L3 )·t L +d L cosθ KL ; wherein (X L , Y L , Z L ) are coordinates of the straight line relationship of the left instrument rod shell, (X L1 , Y L1 , Z L1 ) are coordinates of the left instrument rod end, (X L3 , Y L3 , Z L3 ) are coordinates of the left instrument rod distal end, cosθ PLX is an X-axis component of the left instrument rod joint included angle, cosθ PLY is a Y-axis component of the left instrument rod joint included angle, cosθ PLZ is a Z-axis component of the left instrument rod joint included angle, θ KL is a left instrument rod quadrilateral residual angle, t L is an adjustment parameter of the straight line relationship of the left instrument rod shell, d L is a left mechanical arm center axis distance, d Lr is a left mechanical arm shell distance.

2. The robot collision determination method according to claim 1, characterized by, the right instrument rod state information comprises right instrument rod end coordinate, right instrument rod distal end coordinate, right instrument rod joint included angle, right instrument rod quadrilateral residual angle, right mechanical arm shell distance, and right mechanical arm center axis distance; the right instrument rod shell straight line relationship is obtained according to the right instrument rod state information, which comprises: generating the right instrument rod shell straight line relationship according to the right instrument rod end coordinate, the right instrument rod distal end coordinate, the right instrument rod joint included angle, the right instrument rod quadrilateral residual angle, the right mechanical arm center axis distance, and the right mechanical arm shell distance.

3. The robot collision determination method according to claim 2, characterized by, the right instrument rod shell straight line relationship satisfies: X R = X R3 + (X R1 cosθ PRX - X R3 )·t R +d R sinθ KR ; Y R =Y R3 +(Y R1 cosθ PRY -Y R3 )·t R +d Rl ; Z R =Z R3 +(Z R1 cosθ PRZ -Z R3 )·t R +d R cosθ KR ; wherein (X R , Y R , Z R ) is the coordinate of the linear relationship of the right instrument rod shell, (X R1 , Y R1 , Z R1 ) is the coordinate of the right instrument rod end, (X R3 , Y R3 , Z R3 ) is the coordinate of the right instrument rod distal end, cosθ PRX is the X-axis component of the right instrument rod joint angle, cosθ PRY is the Y-axis component of the right instrument rod joint angle, cosθ PRZ is the Z-axis component of the right instrument rod joint angle, θ KR is the right instrument rod quadrilateral residual angle, t R is the adjustment parameter of the linear relationship of the right instrument rod shell, d R is the center axis distance of the right mechanical arm, d Rl is the shell distance of the right mechanical arm.

4. The robot collision determination method according to claim 2, characterized by, the mirror holding arm state information comprises lens end coordinate, lens distal end coordinate, mirror holding arm quadrilateral residual angle, mirror holding arm left shell distance, mirror holding arm right shell distance, and lens center axis distance; the left mirror holding arm shell straight line relationship and the right mirror holding arm shell straight line relationship are obtained according to the mirror holding arm state information, which comprises: generating the left mirror holding arm shell straight line relationship according to the lens end coordinate, the lens distal end coordinate, the mirror holding arm quadrilateral residual angle, and the mirror holding arm left shell distance; generating the right mirror holding arm shell straight line relationship according to the lens end coordinate, the lens distal end coordinate, the mirror holding arm quadrilateral residual angle, and the mirror holding arm right shell distance.

5. The robot collision determination method according to claim 4, characterized by, the left mirror holding arm shell straight line relationship satisfies: X CL = X C2 + X C1 - X C2 · t C + d C sin θ kC ; Y CL =Y C2 +(Y C1 -Y C2 )·t C +d Cl ; Z CL =Z C2 +(Z C1 -Z C2 )·t C +d C cosθ kC ; the right mirror holding arm shell straight line relationship satisfies: X CR = X C2 + X C1 - X C2 · t C + d C sin θ kC ; Y CR =Y C2 +(Y C1 -Y C2 )·t C -d Cr ; Z CR =Z C2 +(Z C1 -Z C2 )·t C +d C cosθ kC ; wherein (X CL , Y CL , Z CL ) is the coordinate of the left outer shell linear relationship of the mirror holding arm, (X CR , Y CR , Z CR ) is the coordinate of the right outer shell linear relationship of the mirror holding arm, (X C1 , Y C1 , Z C1 ) is the coordinate of the lens end, (X C2 , Y C2 , Z C2 ) is the coordinate of the lens far end, t C is the adjustment parameter of the left outer shell linear relationship of the mirror holding arm, θ kC is the four-edge polygon residual angle of the mirror holding arm, d Cl is the left outer shell distance of the mirror holding arm, d Cr is the right outer shell distance of the mirror holding arm, and d C is the lens rod distance.

6. The robot collision determination method according to claim 4, characterized by, The first preset discrimination condition comprises a first intersection condition and a first range condition; the second preset discrimination condition comprises a second intersection condition and a second range condition; the left mechanical arm and the mirror-holding arm are determined to collide when the straight line relationship of the left instrument rod shell and the straight line relationship of the left shell of the mirror-holding arm satisfy the first preset discrimination condition, and the right mechanical arm and the mirror-holding arm are determined to collide when the straight line relationship of the right instrument rod shell and the straight line relationship of the right shell of the mirror-holding arm satisfy the second preset discrimination condition, comprising: obtaining a left slide table length, a left joint angle, a left quadrilateral angle, a right slide table length, a right joint angle and a right quadrilateral angle; obtaining the first range condition according to the left slide table length, the left joint angle, the left quadrilateral angle and a left instrument rod distal end coordinate, and obtaining the second range condition according to the right slide table length, the right joint angle, the right quadrilateral angle and a right instrument rod distal end coordinate; obtaining a left intersection point coordinate when the straight line relationship of the left instrument rod shell and the straight line relationship of the left shell of the mirror-holding arm satisfy the first intersection condition; determining the left mechanical arm and the mirror-holding arm to collide when the left intersection point coordinate satisfies the first range condition; obtaining a right intersection point coordinate when the straight line relationship of the right instrument rod shell and the straight line relationship of the right shell of the mirror-holding arm satisfy the second intersection condition; determining the right mechanical arm and the mirror-holding arm to collide when the right intersection point coordinate satisfies the second range condition.

7. The robot collision determination method according to claim 6, wherein The first intersection condition satisfies: X OL =X L3 +(X L1 cosθ PLX - X L3 )·t L +d L sinθ KL =X C2 +(X C1 -X C2 )·t C +d C sinθ kC ; Y OL =Y L3 +(Y L1 cosθ PLY -Y L3 )·t L -d Lr =Y C2 +(Y C1 -Y C2 )·t C +d Cl ; Z OL = Z L3 + (Z L1 cos θ PLZ - Z L3 ) · t L + d L cos θ KL = Z C2 + (Z C1 - Z C2 ) · t C + d C cos θ kC ; The first range condition satisfies: X L3 <X OL < X L3 +H L ·cosθ rLX +H L ·cosθ wLX ; Y L3 <Y OL < Y L3 +H L ·cosθ rLY +H L ·cosθ wLY ; Z L3 <Z OL < Z L3 +H L ·cosθ rLZ +H L ·cosθ wLZ ; The second intersection condition satisfies: X OR = X R3 + (X R1 cosθ PRX )·t R3 +d R sinθ R ; X KR = X C2 + (X C1 -X C2 )·t C +d C sinθ kC ; Y OR =Y R3 +(Y R1 cosθ PRY -Y R3 )·t R +d Rl =Y C2 +(Y C1 -Y C2 )·t C -d Cr ; Z OR = Z R3 + (Z R1 cos θ PRZ - Z R3 ) · t R + d R cos θ KR = Z C2 + (Z C1 - Z C2 ) · t C + d C cos θ kC ; The second range condition satisfies: X R3 <X OR < X R3 +H R ·cosθ rRX +H R ·cosθ wRX ; Y R3 <Y OR < Y R3 +H R ·cosθ rRY +H R ·cosθ wRY ; Z R3 <Z OR < Z R3 +H R ·cosθ rRZ +H R ·cosθ wRZ ; wherein (X OL , Y OL , Z OL ) is the left intersection point, (X OR , Y OR , Z OR ) is the right intersection point coordinate, θ rLX is the X-axis component of the left joint angle, θ rLY is the Y-axis component of the left joint angle, θ rLZ is the Z-axis component of the left joint angle, θ wLX is the X-axis component of the left quadrilateral angle, θ wLY is the Y-axis component of the left quadrilateral angle, θ wLZ is the Z-axis component of the left quadrilateral angle, θ rLX is the X-axis component of the right joint angle, θ rRY is the Y-axis component of the right joint angle, θ rRZ is the Z-axis component of the right joint angle, θ wRX is the X-axis component of the right quadrilateral angle, θ wRY is the Y-axis component of the right quadrilateral angle, θ wRZ is the Z-axis component of the right quadrilateral angle, H L is the left slide length, H R is the right slide length.

8. A robot arm collision determination device characterized by comprising: comprising: an obtaining module, configured to obtain left instrument rod state information of a left mechanical arm, right instrument rod state information of a right mechanical arm and mirror-holding arm state information of a mirror-holding arm; a processing module, configured to obtain a left instrument rod shell straight line relationship according to the left instrument rod state information, obtain a right instrument rod shell straight line relationship according to the right instrument rod state information, and obtain a mirror-holding arm left shell straight line relationship and a mirror-holding arm right shell straight line relationship according to the mirror-holding arm state information of the mirror-holding arm; wherein the left instrument rod state information comprises a left instrument rod end coordinate, a left instrument rod distal end coordinate, a left instrument rod joint included angle, a left instrument rod quadrilateral residual angle, a left mechanical arm shell distance and a left mechanical arm center axis distance; the left instrument rod shell straight line relationship is obtained according to the left instrument rod state information, comprising: generating the left instrument rod shell straight line relationship according to the left instrument rod end coordinate, the left instrument rod distal end coordinate, the left instrument rod joint included angle, the left instrument rod quadrilateral residual angle, the left mechanical arm center axis distance and the left mechanical arm shell distance; the left instrument rod shell straight line relationship satisfies: X L = X L3 + L1 cosθ PLX ·t L3 - L d L sinθ KL ; Y L =Y L3 +(Y L1 cosθ PLY -Y L3 )·t L -d Lr ; Z L =Z L3 +(Z L1 cosθ PLZ -Z L3 )·t L +d L cosθ KL ; wherein (X L , Y L , Z L ) are coordinates of the linear relationship of the left instrument rod shell, (X L1 , Y L1 , Z L1 ) are coordinates of the left instrument rod end, (X L3 , Y L3 , Z L3 ) are coordinates of the left instrument rod distal end, cosθ PLX is an X-axis component of the left instrument rod joint angle, cosθ PLY is a Y-axis component of the left instrument rod joint angle, cosθ PLZ is a Z-axis component of the left instrument rod joint angle, θ KL is a left instrument rod quadrilateral residual angle, t L is an adjustment parameter of the linear relationship of the left instrument rod shell, d L is a left mechanical arm center axis distance, d Lr is a left mechanical arm shell distance. a determining module, configured to determine the left mechanical arm and the mirror-holding arm to collide when the left instrument rod shell straight line relationship and the mirror-holding arm left shell straight line relationship satisfy a first preset discrimination condition, and determine the right mechanical arm and the mirror-holding arm to collide when the right instrument rod shell straight line relationship and the mirror-holding arm right shell straight line relationship satisfy a second preset discrimination condition.

Images