A collision avoidance control method, system and storage medium for a planting surgery robot

Abstract

The application provides a kind of planting operation robot anti-collision control method, system and storage medium, after obtaining the data of base three-dimensional coordinate system, construct dynamic object three-dimensional model through the real-time coordinate value of B, C, D, E, F, G, H and E under base three-dimensional coordinate system, carry out collision detection by comparing dynamic object three-dimensional model and static object three-dimensional model, output collision warning when dynamic object three-dimensional model and static object three-dimensional model exist overlap.By obtaining the real-time coordinate value of multiple key positions on connecting shaft, the corresponding wrapped box three-dimensional model of connecting shaft is established, and collision detection is carried out by comparing dynamic object three-dimensional model and static object three-dimensional model, which can effectively avoid the collision of planting operation robot during operation.

Description

TECHNICAL FIELD

[0001] The application relates to the field of mechanical automatic control and the field of medical equipment, and relates to a dental implant surgery robot anti-collision control method, a system and a storage medium. BACKGROUND

[0002] The dental surgery robot is essentially a six-axis industrial robot, and comprises a control base, a plurality of joints and connecting shafts between the joints. Unlike the six-axis industrial robot in the prior art, the dental surgery robot is fixed with a dental implant handset on the terminal connecting shaft, and the dental implant handset is driven to move in the surgical space range by driving the plurality of joints and the plurality of connecting shafts, so as to implement the dental implant treatment surgery.

[0003] An important function of the dental surgery robot is that an operator can freely drag the dental implant handset to the surgical space range according to the relative position between the dental implant handset and the patient, and in this process, the plurality of joints and the plurality of connecting shafts cooperate with each other to enable the dental implant handset to be dragged to a specific position.

[0004] In this application scenario, the surgical tool is the dental implant handset mounted on the terminal connecting shaft of the dental surgery robot, and the driving logic of the existing robot focuses more on how to move the dental implant handset to the target position, and to a certain extent, the combined state of the plurality of joints and the plurality of connecting shafts between the dental implant handset and the control base is ignored. When the dental surgery robot is in the free dragging mode, each joint and each connecting shaft can be moved arbitrarily, and therefore, the joint and the connecting shaft can collide with the equipment shell, and after the collision, the robot can be stuck and must be recovered through complex processing. The industry urgently needs to propose a new solution to prevent the dental implant surgery robot from colliding in the application process. SUMMARY

[0005] The technical problem to be solved by the application is to provide a dental implant surgery robot anti-collision control method, a system and a storage medium, which can effectively prevent the dental implant surgery robot from colliding during operation, in view of the above defects of the prior art.

[0006] The technical solution adopted by the application to solve the technical problem is as follows:

[0007] A dental implant surgery robot anti-collision control method comprises the following steps:

[0008] S1. Obtain the origin, X-axis, Y-axis and Z-axis of the base three-dimensional coordinate system of the control base;

[0009] S2. Obtain the rotation angle of the first joint through the server control data, obtain the coordinate translation value between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system according to the distance L0 between the base and the first joint, and calculate the conversion relationship between the first joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the distance L0 between the base and the first joint and the rotation angle of the first joint and the real-time coordinate value in the base three-dimensional coordinate system; wherein B is the base three-dimensional coordinate system, and M1 is the first joint three-dimensional coordinate system;

[0010] S3. Obtain the rotation angle of the second joint through the server control data, obtain the coordinate translation value between the first joint three-dimensional coordinate system and the second joint three-dimensional coordinate system according to the length L1 of the first vertical axis and the length L2 of the first horizontal axis, and calculate the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the length L3 of the second vertical axis and the rotation angle of the second joint the real-time coordinate value in the base three-dimensional coordinate system; wherein M2 is the second joint three-dimensional coordinate system;

[0011] S4. Obtain the rotation angle of the third joint through the server control data, obtain the coordinate translation value between the second joint three-dimensional coordinate system and the third joint three-dimensional coordinate system according to the length L3 of the second vertical axis, and calculate the conversion relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to the length L4 of the second horizontal axis, the length L5 of the third horizontal axis, and the rotation angle of the third joint and the real-time coordinate value in the base three-dimensional coordinate system; wherein M3 is the third joint three-dimensional coordinate system;

[0012] S5. Obtain the rotation angle of the fourth joint through the server control data, obtain the coordinate translation value between the third joint three-dimensional coordinate system and the fourth joint three-dimensional coordinate system according to the length L4 of the second horizontal axis and the length L5 of the third horizontal axis, and calculate the conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the projection length L6 of the first bending axis, the length L7 of the fourth horizontal axis and the rotation angle of the fourth joint the real-time coordinate value in the base three-dimensional coordinate system; wherein M4 is the fourth joint three-dimensional coordinate system;

[0013] S6. Obtain the rotation angle of the fifth joint through the server control data, obtain the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system according to the projection length L6 of the first bending axis and the length L7 of the fourth transverse axis, and calculate the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the length L8 of the fifth transverse axis, the length L9 of the third vertical axis, and the rotation angle of the fifth joint and the real-time coordinate value in the base three-dimensional coordinate system; wherein M5 is the fifth joint three-dimensional coordinate system;

[0014] S7. Obtain the rotation angle of the sixth joint through the server control data, obtain the coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system according to the length L8 of the fifth transverse axis and the length L9 of the third vertical axis, and calculate the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the length L10 of the fourth vertical axis, and the rotation angle of the sixth joint the real-time coordinate value in the base three-dimensional coordinate system; wherein M6 is the sixth joint three-dimensional coordinate system;

[0015] S8. Obtain the real-time coordinate value of E in the base three-dimensional coordinate system, according to , , , , , , , , and the real-time coordinate value of E in the base three-dimensional coordinate system to construct a dynamic object three-dimensional model, the dynamic object three-dimensional model comprising a first vertical axis, a first transverse axis, a second vertical axis, a second transverse axis, a third transverse axis, a first bending axis, a fourth transverse axis, a fifth transverse axis, a third vertical axis, a fourth vertical axis, and a package box three-dimensional model of the planting mobile phone;

[0016] S9. Establish a static object three-dimensional model with the origin of the base three-dimensional coordinate system of the control base as the center;

[0017] S10. Perform collision detection by comparing the dynamic object three-dimensional model and the static object three-dimensional model, and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model overlap.

[0018] Compared with existing technologies, the beneficial effects of this technical solution are: by obtaining the real-time coordinate values ​​of multiple key positions on the connecting axis, a 3D model of the package corresponding to the connecting axis is established, and collision detection is performed by comparing the dynamic object 3D model and the static object 3D model, which can effectively avoid collisions of the implantation surgery robot during operation.

[0019] A collision avoidance control system for an implantation surgery robot, the collision avoidance control system comprising:

[0020] The base coordinate system acquisition module is used to acquire the origin, X-axis, Y-axis, and Z-axis of the base three-dimensional coordinate system for controlling the base;

[0021] The first coordinate value calculation module is used to obtain the rotation angle of the first joint through servo control data, and to obtain the coordinate translation value between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system based on the distance L0 between the control base and the first joint, thereby calculating the transformation relationship between the first joint three-dimensional coordinate system and the base three-dimensional coordinate system. ,according to The distance L0 between the control base and the first joint and the rotation angle of the first joint are calculated. and Real-time coordinate values ​​in the three-dimensional coordinate system of the base; where B is the three-dimensional coordinate system of the base and M1 is the three-dimensional coordinate system of the first joint;

[0022] The second coordinate value calculation module is used to obtain the rotation angle of the second joint through servo control data, and to obtain the coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system based on the length L1 of the first vertical axis and the length L2 connecting the first horizontal axis, thereby calculating the transformation relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system. ,according to The length L3 of the second vertical axis and the rotation angle of the second joint are calculated. Real-time coordinate values ​​in the three-dimensional coordinate system of the base; where M2 is the three-dimensional coordinate system of the second joint;

[0023] The third coordinate value calculation module is used to obtain the rotation angle of the third joint through server control data, and to obtain the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system based on the length L3 of the second vertical axis, thereby calculating the transformation relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system. Based on the length L4 of the second horizontal axis and the length L5 of the third horizontal axis, And the rotation angle of the third joint, calculated and Real-time coordinate values ​​in the three-dimensional coordinate system of the base; where M3 is the three-dimensional coordinate system of the third joint;

[0024] a fourth coordinate value calculation module, configured to obtain a rotation angle of the fourth joint through the servo control data, obtain a coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system according to the length L4 of the second transverse shaft and the length L5 of the third transverse shaft, and calculate a conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system , calculate the real-time coordinate value in the base three-dimensional coordinate system according to , the projection length L6 of the first bending shaft, the length L7 of the fourth transverse shaft, and the rotation angle of the fourth joint ; wherein, M4 is the fourth joint three-dimensional coordinate system

[0025] a fifth coordinate value calculation module, configured to obtain a rotation angle of the fifth joint through the servo control data, obtain a coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system according to the projection length L6 of the first bending shaft and the length L7 of the fourth transverse shaft, and calculate a conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system , calculate the real-time coordinate value in the base three-dimensional coordinate system according to , the length L8 of the fifth transverse shaft, the length L9 of the third vertical shaft, and the rotation angle of the fifth joint , and ; wherein, M5 is the fifth joint three-dimensional coordinate system

[0026] a sixth coordinate value calculation module, configured to obtain a rotation angle of the sixth joint through the servo control data, obtain a coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system according to the length L8 of the fifth transverse shaft and the length L9 of the third vertical shaft, and calculate a conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system , calculate the real-time coordinate value in the base three-dimensional coordinate system according to , the length L10 of the fourth vertical shaft, and the rotation angle of the sixth joint ; wherein, M6 is the sixth joint three-dimensional coordinate system

[0027] a dynamic model establishment module, configured to obtain the real-time coordinate value of E in the base three-dimensional coordinate system, and calculate a conversion relationship between the base three-dimensional coordinate system and the target three-dimensional coordinate system according to , , , , , , , , and E are real-time coordinate values of the base three-dimensional coordinate system to construct a dynamic object three-dimensional model, the dynamic object three-dimensional model comprising a first vertical axis, a first horizontal axis, a second vertical axis, a second horizontal axis, a third horizontal axis, a first bending axis, a fourth horizontal axis, a fifth horizontal axis, a third vertical axis, a fourth vertical axis and a package box three-dimensional model of the planting handset;

[0028] A static model establishing module is configured to establish a static object three-dimensional model with the origin of the base three-dimensional coordinate system of the control base as the center;

[0029] A collision analysis module is configured to perform collision detection by comparing the dynamic object three-dimensional model and the static object three-dimensional model, and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model overlap.

[0030] Correspondingly, a storage medium storing a computer program, the computer program comprising program instructions, when the program instructions are executed by a processor, the processor executes the planting operation robot anti-collision control method. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 is a flowchart of the planting operation robot anti-collision control method of the application.

[0032] Figure 2 is a schematic diagram of the planting operation robot anti-collision control system of the application.

[0033] Figure 3 is a whole schematic diagram of the planting operation robot of the application.

[0034] Figure 4 is a first size schematic diagram of the planting operation robot of the application.

[0035] Figure 5 is a second size schematic diagram of the planting operation robot of the application.

[0036] Figure 6 is a third size schematic diagram of the planting operation robot of the application.

[0037] In the figure, the components represented by each number are listed as follows:

[0038] The first joint 1, the second joint 2, the third joint 3, the fourth joint 4, the fifth joint 5, the sixth joint 6, the planting handset 7, the control base 8, the first vertical axis 9, the first horizontal axis 10, the second vertical axis 11, the second horizontal axis 12, the third horizontal axis 13, the first bending axis 14, the fourth horizontal axis 15, the fifth horizontal axis 16, the third vertical axis 17, and the fourth vertical axis 18.

[0039] The base coordinate system acquisition module 101, the first coordinate value calculation module 102, the second coordinate value calculation module 103, the third coordinate value calculation module 104, the fourth coordinate value calculation module 105, the fifth coordinate value calculation module 106, the sixth coordinate value calculation module 107, the dynamic model establishment module 108, the static model establishment module 109, and the collision analysis module 110. DETAILED DESCRIPTION

[0040] To make the objects, technical solutions and advantages of the present application clearer and more apparent, the present application will be further described in detail below with reference to the drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application and should not be used to limit the present application.

[0041] In the description of the present application, it should be understood that the terms "center", "upper", "lower", "front", "back", "left", "right" and the like indicate the orientation or positional relationship shown in the drawings, and are only used to facilitate the description of the present application and simplify the description, and therefore cannot be understood as limiting the present application. The terms "first", "second" are only for descriptive purposes and cannot be understood as indicating or implying relative importance.

[0042] In the description of the present application, it should be noted that unless otherwise explicitly specified and limited, the terms "mounting", "connection", "connection" should be understood broadly, for example, it can be fixedly connected, or detachably connected, or integrally connected; it can be mechanically connected, or electrically connected; it can be directly connected, or indirectly connected through an intermediate medium; it can be the communication between two components inside. When a component is referred to as "fixed to" or "provided on" another element, it can be directly on another component or there can be a middle component. When a component is considered to be "connected" to another element, it can be directly connected to another element or there can be a middle element. For those skilled in the art, the specific meanings of the above terms in the present application can be understood according to the specific circumstances.

[0043] The oral surgery robot is essentially a six-axis industrial robot, including a control base, a plurality of joints and connecting shafts between the joints. Unlike the six-axis industrial robot in the prior art, the oral surgery robot has an implant handset fixed on the end connecting shaft. By driving the plurality of joints and the plurality of connecting shafts, the implant handset is controlled to move within the surgical space range, so as to implement the oral implant treatment surgery.

[0044] An important function of the oral surgery robot is that the operator can freely drag the implant handset to the range of the surgery space according to the relative position between the implant handset and the patient, and in this process, multiple joints and multiple connecting shafts cooperate to make the implant handset be dragged to a specific position.

[0045] In this application scenario, the surgery tool is an implant handset installed on the end connecting shaft of the oral surgery robot, and the driving logic of the existing robot focuses more on how to move the implant handset to the target position, and to some extent, ignores the combined state of multiple joints and multiple connecting shafts between the implant handset and the control base. When the oral surgery robot is in the free dragging mode, each joint and each connecting shaft can be moved arbitrarily, so the joints and connecting shafts may collide with the equipment shell, and after the collision, the robot will be stuck and must be recovered through complex processing.

[0046] The industry urgently needs to propose a new scheme to prevent the implant surgery robot from colliding in the application process.

[0047] To solve the above problems, the present application provides an implant surgery robot anti-collision control method, system and storage medium, which are based on an implant surgery robot.

[0048] As shown in Figure 3 , Figure 4 , Figure 5 and Figure 6 , the implant surgery robot comprises a control base 8, a first joint 1, a second joint 2, a third joint 3, a fourth joint 4, a fifth joint 5 and a sixth joint 6, and an implant handset 7 is arranged on the implant surgery robot.

[0049] The connecting shafts between the joints comprise a first vertical shaft 9, a first horizontal shaft 10, a second vertical shaft 11, a second horizontal shaft 12, a third horizontal shaft 13, a first curved shaft 14, a fourth horizontal shaft 15, a fifth horizontal shaft 16, a third vertical shaft 17 and a fourth vertical shaft 18.

[0050] As shown in Figure 3 , Figure 4 , Figure 5 and Figure 6 , specifically, the distance between the control base 8 and the first joint 1 is L0. L0 is a known parameter.

[0051] The first joint 1 and the second joint 2 are connected through the first vertical shaft 9 and the first horizontal shaft 10, the length of the first vertical shaft 9 is L1, the length of the first horizontal shaft 10 is L2, and the shaft centers of the two ends of the first horizontal shaft 10 are and . L1 and L2 are known parameters, and The axis center of the two ends of the first transverse shaft 10. The so-called axis center can be understood as two points on the central axis of the first transverse shaft 10 similar to the cylindrical structure close to the two ends. In the subsequent data processing process, a wrapped box three-dimensional model needs to be established for the first transverse shaft 10 and other connecting shafts. In the field of collision detection, the wrapped box three-dimensional model is a simple geometric body (such as a cube, a sphere, and a polygon) that is approximately replaced by a complex object with a volume slightly larger than the target object. In the technical solution, the first transverse shaft and other connecting shafts are not simple cylindrical structures, but by selecting two points close to the end points on the "central axis", it is convenient to establish a wrapped box three-dimensional model later. Its core role is to wrap the complex object through the compact cylindrical structure geometry to achieve efficient collision detection. In the technical solution, the "axis center" of all connecting shafts is as such, and will not be repeated here.

[0052] The second joint 2 and the third joint 3 are connected by a second vertical shaft 11, the length of the second vertical shaft 11 is L3, and the axis centers of the two ends of the second vertical shaft 11 are and L3 is a known parameter, and are the axis centers of the two ends of the second vertical shaft 11, wherein is the axis center of the first transverse shaft 10, which is also the axis center of the second vertical shaft 11. It can be understood that the first transverse shaft 10 and the second vertical shaft 11 are connected in structure, and there is a common end point between them. This common end point model is .

[0053] The third joint 3 and the fourth joint 4 are connected by a second transverse shaft 12 and a third transverse shaft 13, the length of the second transverse shaft 12 is L4, the length of the third transverse shaft 13 is L5, the axis centers of the two ends of the second transverse shaft 12 are and , the axis centers of the two ends of the third transverse shaft 13 are and . Wherein, L4 and L5 are known parameters.

[0054] The fourth joint 4 and the fifth joint 5 are connected by a first curved shaft 14 and a fourth transverse shaft 15, the projection length of the first curved shaft 14 is L6, the length of the fourth transverse shaft 15 is L7, and the axis center of the end of the fourth transverse shaft 15 away from the first curved shaft 14 is L6 and L7 are known parameters. In the implant surgery robot in the technical solution, the first curved shaft 14 is a curved structure, in order to facilitate data processing, in the implementation of the technical solution, the first curved shaft 14 is approximated as a straight shaft structure, and a wrapping box three-dimensional model is established through the projection length.

[0055] The fifth joint 5 and the sixth joint 6 are connected through a fifth transverse shaft 16 and a third vertical shaft 17, the length of the fifth transverse shaft 16 is L8, the length of the third vertical shaft 17 is L9, the sixth joint 6 and the implant hand are connected through a fourth vertical shaft 18, and the length of the fourth vertical shaft 18 is L10; the shaft centers of the two ends of the fifth transverse shaft 16 are and The shaft centers of the two ends of the third vertical shaft 17 are and The shaft center of the end of the fourth vertical shaft 18 close to the implant hand is , and the drill bit of the implant hand is E. Wherein, L8, L9 and L10 are known parameters.

[0056] As shown in Figure 1 , in order to solve the above problems, the present application provides a kind of implant surgery robot anti-collision control method, the anti-collision control method includes the following steps:

[0057] S1. Obtain the origin, X-axis, Y-axis and Z-axis of the base three-dimensional coordinate system of control base 8. Control base 8 itself has a three-dimensional coordinate system, that is, the base three-dimensional coordinate system. By obtaining the origin, X-axis, Y-axis and Z-axis of the base three-dimensional coordinate system, a reference is established for subsequent coordinate translation and rotation.

[0058] In the technical solution, as shown in Figure 3 , the geometric center point of the base 8 can be controlled as the origin of the base three-dimensional coordinate system;With the central axis of the first vertical shaft 9 as the Z-axis, with the direction from the control base 8 to the first joint 1 as the Z-axis direction;With the central axis of the first transverse shaft 10 as the X-axis, with the direction from to on the first transverse shaft 10 as the X-axis direction;With the central axis of the third transverse shaft 13 as the Y-axis, with the direction from to on the third transverse shaft 13 as the Y-axis direction.

[0059] S2. Obtain the rotation angle of the first joint 1 through the server control data, obtain the coordinate translation value between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system according to the distance L0 between the control base 8 and the first joint 1, and calculate the conversion relationship between the first joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the distance L0 between the base 8 and the first joint 1 and the rotation angle of the first joint 1 are calculated and the real-time coordinate value in the base three-dimensional coordinate system; wherein, B is the base three-dimensional coordinate system, and M1 is the first joint three-dimensional coordinate system.

[0060] The base three-dimensional coordinate system can directly obtain the first joint three-dimensional coordinate system by translation on the Z axis, and the translation amount can be directly obtained according to L0, so as to obtain the conversion relationship between the first joint three-dimensional coordinate system and the base three-dimensional coordinate system . In addition, the first joint 1 directly controls the first vertical shaft 9 and the first horizontal shaft 10, and after the rotation angle of the first joint 1 is obtained through the servo control data, the distance L0 can be known according to L0 and the real-time coordinate value in the base three-dimensional coordinate system.

[0061] S3. The rotation angle of the second joint 2 is obtained through the servo control data, the coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system is obtained according to the length L1 of the first vertical shaft 9 and the length L2 connected by the first horizontal shaft 10, so as to calculate the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the length L3 of the second vertical shaft 11 and the rotation angle of the second joint 2 the real-time coordinate value in the base three-dimensional coordinate system; wherein, M2 is the second joint three-dimensional coordinate system.

[0062] The first joint three-dimensional coordinate system is translated upwards along the Z axis direction, and the second joint three-dimensional coordinate system can be obtained by translation along the X axis direction, and the translation amount can be directly obtained according to the length L1 of the first vertical shaft 9 and the first horizontal shaft 10, so as to obtain the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system . In addition, the second joint 2 controls the second vertical shaft 11, and according to , the length L3 of the second vertical shaft 11 and the rotation angle of the second joint 2 the real-time coordinate value in the base three-dimensional coordinate system. In steps S4 to S7, the conversion between different three-dimensional coordinate systems is realized by translation along the X axis, the Y axis and the Z axis, and the principle is consistent with that of steps S2 and S3, which will not be repeated here.

[0063] S4. The rotation angle of the third joint 3 is obtained through the servo control data, the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system is obtained according to the length L3 of the second vertical shaft 11, so as to calculate the conversion relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system , the length L5 of the third lateral axis 13, and the rotation angle of the third joint 3, the real-time coordinate value of E in the base three-dimensional coordinate system is calculated as and the real-time coordinate value of E in the base three-dimensional coordinate system; wherein M3 is the third joint three-dimensional coordinate system.

[0064] S5. The rotation angle of the fourth joint 4 is obtained through the server control data, the coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system is obtained according to the length L4 of the second lateral axis 12 and the length L5 of the third lateral axis 13, and the conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated , according to , the projection length L6 of the first bending axis 14, the length L7 of the fourth lateral axis 15, and the rotation angle of the fourth joint 4, the real-time coordinate value of E in the base three-dimensional coordinate system is calculated as the real-time coordinate value of E in the base three-dimensional coordinate system; wherein M4 is the fourth joint three-dimensional coordinate system.

[0065] S6. The rotation angle of the fifth joint 5 is obtained through the server control data, the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system is obtained according to the projection length L6 of the first bending axis 14 and the length L7 of the fourth lateral axis 15, and the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated , according to , the length L8 of the fifth lateral axis 16, the length L9 of the third vertical axis 17, and the rotation angle of the fifth joint 5, the real-time coordinate value of E in the base three-dimensional coordinate system is calculated as and the real-time coordinate value of E in the base three-dimensional coordinate system; wherein M5 is the fifth joint three-dimensional coordinate system.

[0066] S7. The rotation angle of the sixth joint 6 is obtained through the server control data, the coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system is obtained according to the length L8 of the fifth lateral axis 16 and the length L9 of the third vertical axis 17, and the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated , according to , the length L10 of the fourth vertical axis 18, and the rotation angle of the sixth joint 6, the real-time coordinate value of E in the base three-dimensional coordinate system is calculated as the real-time coordinate value of E in the base three-dimensional coordinate system; wherein M6 is the sixth joint three-dimensional coordinate system.

[0067] S8. The real-time coordinate value of E in the base three-dimensional coordinate system is obtained, according to , , , , , 、 、 、 and E are the real-time coordinate values of the base three-dimensional coordinate system, and a dynamic object three-dimensional model is constructed, which includes a first vertical axis 9, a first horizontal axis 10, a second vertical axis 11, a second horizontal axis 12, a third horizontal axis 13, a first curved axis 14, a fourth horizontal axis 15, a fifth horizontal axis 16, a third vertical axis 17, a fourth vertical axis 18, and a three-dimensional model of the package box of the implant handset.

[0068] 、 、 、 、 、 、 、 、 and E are the axis centers of the connecting axes or the tips of the implant handset drill needles. By determining the real-time coordinate values of the above-mentioned points, the three-dimensional model of the connecting axes and the three-dimensional model of the package box of the implant handset can be correspondingly established.

[0069] S9. A static object three-dimensional model is established with the origin of the base three-dimensional coordinate system of the control base 8 as the center. During the operation of the implant surgery robot, the control base remains stationary throughout the process, while the joints, connecting axes, and implant handset may be in a state of motion. The static object three-dimensional model corresponds to the collision detection three-dimensional model of the control base. The dynamic object three-dimensional model corresponds to the three-dimensional model of the package box of the implant handset.

[0070] S10. Collision detection is performed by comparing the dynamic object three-dimensional model and the static object three-dimensional model, and a collision warning is output when there is an overlap between the dynamic object three-dimensional model and the static object three-dimensional model. In step S10, the logic of collision detection is that when there is an overlap between the dynamic object three-dimensional model and the static object three-dimensional model, it indicates that there is a risk of collision or even a collision has occurred between the two, and a collision warning is output at this time.

[0071] Based on the above technical solution, by obtaining the real-time coordinate values of multiple key positions on the connecting axis, the three-dimensional model of the package box corresponding to the connecting axis is established, and collision detection is performed by comparing the dynamic object three-dimensional model and the static object three-dimensional model, which can effectively avoid the collision of the implant surgery robot during operation.

[0072] Preferably, step S10 specifically includes the following steps:

[0073] S1001. Extract the 3D data point set of the dynamic object 3D model and the 3D data point set of the static object 3D model. The dynamic object 3D model and the static object 3D model have already been established in the preceding steps, and the corresponding 3D data point sets can be directly extracted in step S1001.

[0074] S1002. Decompose the 3D data point set of the dynamic object 3D model and the 3D data point set of the static object 3D model to obtain a dynamic object 3D array and a static object 3D array. The dynamic object 3D array includes the X, Y, and Z components of the dynamic object 3D model, and the static object 3D array includes the X, Y, and Z components of the static object 3D model. In step S1002, the 3D data point set of the dynamic object 3D model and the 3D data point set of the static object 3D model are decomposed. For the 3D data point set of the dynamic object 3D model, the X, Y, and Z components of the dynamic object 3D model are obtained; for the 3D data point set of the static object 3D model, the X, Y, and Z components of the static object 3D model are obtained.

[0075] S1003. Based on the X components of the dynamic and static 3D object models, calculate the covariance matrix of the X components. ,in, , For the first The X component of a dynamic 3D model of an object for The mean, For the first The X component of a static 3D model of an object for The mean.

[0076] S1004. Based on the Y-components of the dynamic and static 3D object models, calculate the covariance matrix of the Y-components. ,in, , For the first The Y component of a dynamic 3D model of an object for The mean, For the first The Y component of a static 3D model of an object for The mean.

[0077] S1005. Calculate the covariance matrix of the Z-components based on the Z-components of the dynamic and static 3D object models. ,in, , For the first Z component of the dynamic object three-dimensional model, is the mean value of , is the mean value of Z component of the static object three-dimensional model , is the mean value of . The functions of steps S1003, S1004 and S1005 are to solve the X-axis, Y-axis and Z-axis covariance matrix, and to prepare for the subsequent calculation of the eigenvectors of the X component, Y component and Z component.

[0078] S1006. Eigenvalue decomposition is performed according to the covariance matrix of the X component , the covariance matrix of the Y component and the covariance matrix of the Z component to obtain the eigenvectors of the X component, Y component and Z component.

[0079] S1007. The maximum and minimum eigenvalues of the X component, the maximum and minimum eigenvalues of the Y component, the maximum and minimum eigenvalues of the Z component in the dynamic object three-dimensional model are obtained, and the maximum and minimum eigenvalues of the X component, the maximum and minimum eigenvalues of the Y component, the maximum and minimum eigenvalues of the Z component in the static object three-dimensional model are obtained.

[0080] S1008. The maximum and minimum eigenvalues of the X component in the dynamic object three-dimensional model are projected onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the X-axis; the maximum and minimum eigenvalues of the Y component in the dynamic object three-dimensional model are projected onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the Y-axis; the maximum and minimum eigenvalues of the Z component in the dynamic object three-dimensional model are projected onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the Z-axis.

[0081] S1009. The maximum and minimum eigenvalues of the X component in the static object three-dimensional model are projected onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the X-axis; the maximum and minimum eigenvalues of the Y component in the static object three-dimensional model are projected onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Y-axis; the maximum and minimum eigenvalues of the Z component in the static object three-dimensional model are projected onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Z-axis.

[0082] S1010. Perform spatial overlap judgment; judge whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the X axis and the bounding box three-dimensional model of the static object three-dimensional model on the X axis exist spatial overlap, judge whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Y axis and the bounding box three-dimensional model of the static object three-dimensional model on the Y axis exist spatial overlap, judge whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Z axis and the bounding box three-dimensional model of the static object three-dimensional model on the Z axis exist spatial overlap.

[0083] S1011. Output the spatial overlap judgment result, and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model exist overlap.

[0084] Based on the above technical scheme, after the characteristic vectors of the X component, the Y component and the Z component are analyzed, the maximum characteristic value and the minimum characteristic value corresponding to each component are correspondingly acquired, so as to establish the bounding box three-dimensional model on the X axis, the Y axis and the Z axis. By judging whether the bounding box three-dimensional models of the dynamic object three-dimensional model and the static object three-dimensional model exist overlap on the X axis, the Y axis and the Z axis in turn, whether there exists a collision risk is comprehensively judged.

[0085] Preferably, step S8 specifically comprises the following steps:

[0086] S801. Control the surgical robot to drive the implant handset so that the mounting sleeve on the implant handset is sleeved on the calibration column on the calibration plate. The three-dimensional data model of the calibration plate is known, and the relative positions of the identification points and the calibration columns on the calibration plate are also known. When the implant handset is controlled by the surgical robot to sleeve the mounting sleeve on the calibration column, the pose of the calibration column can be considered as being equivalent to the pose of the mounting sleeve on the implant handset for installing the drill needle.

[0087] S802. When the mounting sleeve on the implant handset is sleeved on the calibration column on the calibration plate, control the optical positioner to acquire the pose of the calibration plate in the optical three-dimensional coordinate system of the optical positioner and the pose of the implant handset in the optical three-dimensional coordinate system, and synchronously acquire the servo control data. At this time, a connection can be established between the servo three-dimensional coordinate system and the optical three-dimensional coordinate system.

[0088] S803. According to the pose of the calibration plate in the optical three-dimensional coordinate system of the optical positioner, the pose of the implant handset in the optical three-dimensional coordinate system and the servo control data, a coordinate conversion relationship is established between the base three-dimensional coordinate system, the optical three-dimensional coordinate system and the handset three-dimensional coordinate system of the implant handset.

[0089] S804. Read the real-time pose of the implant handset in the optical three-dimensional coordinate system.

[0090] S805. According to the real-time pose of the optical three-dimensional coordinate system and the coordinate conversion relationship between the base three-dimensional coordinate system and the mobile phone three-dimensional coordinate system, the real-time coordinate value of the drill bit E of the planting mobile phone in the base three-dimensional coordinate system is calculated. Since the coordinate conversion relationship has been established between the base three-dimensional coordinate system, the optical three-dimensional coordinate system and the mobile phone three-dimensional coordinate system of the planting mobile phone in step S803, after reading the real-time pose of the planting mobile phone in the optical three-dimensional coordinate system in step S804, the real-time coordinate value of the drill bit E in the base three-dimensional coordinate system can be obtained based on the real-time pose of the planting mobile phone in the optical three-dimensional coordinate system.

[0091] Based on the above calibration process, after completing the calibration program, the real-time state of the drill bit E can be unified to the base three-dimensional coordinate system to obtain the real-time coordinate value of the drill bit E in the base three-dimensional coordinate system, thereby obtaining the real-time state of the key node of the drill bit E.

[0092] Preferably, in step S3, the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated Specifically, the conversion relationship between the second joint three-dimensional coordinate system and the first joint three-dimensional coordinate system is obtained according to the coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system , which is calculated by and , wherein , is matrix multiplication.

[0093] In step S4, the conversion relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated Specifically, the conversion relationship between the third joint three-dimensional coordinate system and the second joint three-dimensional coordinate system is obtained according to the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system , which is calculated by and , wherein , is matrix multiplication.

[0094] In step S5, the conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated Specifically, the conversion relationship between the fourth joint three-dimensional coordinate system and the third joint three-dimensional coordinate system is obtained according to the coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system , which is calculated by and , wherein , ​​​For matrix multiplication.

[0095] In step S6, the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated Specifically, the conversion relationship between the fifth joint three-dimensional coordinate system and the fourth joint three-dimensional coordinate system is obtained according to the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system , and the conversion relationship is calculated by and , wherein , For matrix multiplication.

[0096] In step S7, the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated Specifically, the conversion relationship between the sixth joint three-dimensional coordinate system and the fifth joint three-dimensional coordinate system is obtained according to the coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system , and the conversion relationship is calculated by and , wherein , For matrix multiplication.

[0097] Preferably, step S9 specifically includes establishing a static object three-dimensional model with the origin of the base three-dimensional coordinate system of the control base as the center, and the static object model includes a first shell three-dimensional model, a second shell three-dimensional model, and a third shell three-dimensional model in turn. Correspondingly, step S10 specifically includes outputting a third-level collision warning when the dynamic object three-dimensional model and the third shell three-dimensional model overlap, controlling the planting surgery robot to stop automatic operation; outputting a second-level collision warning when the dynamic object three-dimensional model and the second shell three-dimensional model overlap, controlling the planting surgery robot to stop automatic operation, and controlling the planting surgery robot to move passively at a limited speed; and outputting a first-level collision warning when the dynamic object three-dimensional model and the first shell three-dimensional model overlap, and locking the planting surgery robot.

[0098] Unlike simply establishing a collision detection three-dimensional model, the static object model in the technical solution includes a first shell three-dimensional model, a second shell three-dimensional model, and a third shell three-dimensional model in turn, and during the collision detection analysis process, whether the first shell three-dimensional model, the second shell three-dimensional model, and the third shell three-dimensional model overlap with the dynamic object model is analyzed synchronously, and classified and processed according to the warning level.

[0099] ​​For example, when the dynamic object model in the planting operation robot intersects with the third outer shell three-dimensional model of the outermost layer, the movement of the planting operation robot is immediately stopped, and an alarm information is sent to remind the operator to apply an external force to move each connecting shaft to a safe area. If it has been moved to the safe area, the alarm is released. When the moving direction of the planting operation robot is still inward to the second outer shell three-dimensional model, the planting operation robot is controlled to immediately stop and send an alarm information, and the movement speed is reduced to 10% until the operator applies an external force to move each connecting shaft to a safe area, and then the original speed is restored. When the robot still continues to move inward to the first outer shell three-dimensional model, the planting operation robot is stopped and an alarm information is output. At this time, due to the high risk of collision, the manufacturer's technical personnel need to input a password for processing before moving the robot.

[0100] As shown in Figure 2 , in order to solve the above problems, the present application provides a kind of planting operation robot anti-collision control system, the anti-collision control system includes base coordinate system acquisition module, first coordinate value calculation module, second coordinate value calculation module, third coordinate value calculation module, fourth coordinate value calculation module, fifth coordinate value calculation module, sixth coordinate value calculation module, dynamic model establishment module, static model establishment module and collision analysis module, wherein:

[0101] The base coordinate system acquisition module is used to obtain the origin, X-axis, Y-axis and Z-axis of the base three-dimensional coordinate system of the control base.

[0102] The first coordinate value calculation module is used to obtain the rotation angle of the first joint through the servo control data, and the coordinate translation value between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system is obtained according to the distance L0 between the control base and the first joint, so as to calculate the conversion relationship between the first joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the distance L0 between the control base and the first joint and the rotation angle of the first joint, the real-time coordinate value of and in the base three-dimensional coordinate system is calculated; wherein B is the base three-dimensional coordinate system, and M1 is the first joint three-dimensional coordinate system.

[0103] The second coordinate value calculation module is used to obtain the rotation angle of the second joint through the servo control data, and the coordinate translation value between the first joint three-dimensional coordinate system and the second joint three-dimensional coordinate system is obtained according to the length L1 of the first vertical axis and the length L2 of the first horizontal axis, so as to calculate the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system , according to , the length L3 of the second vertical axis and the rotation angle of the second joint, the real-time coordinate value of Real-time coordinate value in the base three-dimensional coordinate system; wherein, M2 is the second joint three-dimensional coordinate system.

[0104] The third coordinate value calculation module is used for obtaining the rotation angle of the third joint through the server control data, obtaining the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system according to the length L3 of the second vertical shaft, and calculating the conversion relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system. According to the length L4 of the second horizontal shaft, the length L5 of the third horizontal shaft, And the rotation angle of the third joint, And Real-time coordinate value in the base three-dimensional coordinate system; wherein, M3 is the third joint three-dimensional coordinate system.

[0105] The fourth coordinate value calculation module is used for obtaining the rotation angle of the fourth joint through the server control data, obtaining the coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system according to the length L4 of the second horizontal shaft and the length L5 of the third horizontal shaft, and calculating the conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system. According to , the projection length L6 of the first bending shaft, the length L7 of the fourth horizontal shaft and the rotation angle of the fourth joint, Real-time coordinate value in the base three-dimensional coordinate system; wherein, M4 is the fourth joint three-dimensional coordinate system.

[0106] The fifth coordinate value calculation module is used for obtaining the rotation angle of the fifth joint through the server control data, obtaining the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system according to the projection length L6 of the first bending shaft and the length L7 of the fourth horizontal shaft, and calculating the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system. According to , the length L8 of the fifth horizontal shaft, the length L9 of the third vertical shaft and the rotation angle of the fifth joint, And Real-time coordinate value in the base three-dimensional coordinate system; wherein, M5 is the fifth joint three-dimensional coordinate system.

[0107] The sixth coordinate value calculation module is used for obtaining the rotation angle of the sixth joint through the server control data, obtaining the coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system according to the length L8 of the fifth horizontal shaft and the length L9 of the third vertical shaft, and calculating the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system. According to , the length L10 of the fourth vertical shaft and the rotation angle of the sixth joint, Real-time coordinate values ​​in the three-dimensional coordinate system of the base; where M6 is the three-dimensional coordinate system of the sixth joint.

[0108] The dynamic model building module is used to obtain the real-time coordinate values ​​of E in the three-dimensional coordinate system of the base, based on... , , , , , , , , The dynamic object 3D model is constructed using the real-time coordinate values ​​of E in the base 3D coordinate system. The dynamic object 3D model includes a first vertical axis, a first horizontal axis, a second vertical axis, a second horizontal axis, a third horizontal axis, a first curved axis, a fourth horizontal axis, a fifth horizontal axis, a third vertical axis, a fourth vertical axis, and a 3D model of the packaging box for planting the mobile phone.

[0109] The static model creation module is used to create a static 3D model of the object centered on the origin of the base's 3D coordinate system.

[0110] The collision analysis module is used to perform collision detection by comparing the 3D models of dynamic and static objects. When the 3D models of dynamic and static objects overlap, a collision warning is output.

[0111] Preferably, the collision analysis module includes:

[0112] The data point extraction unit is used to extract the 3D data point set of dynamic object 3D model and the 3D data point set of static object 3D model.

[0113] The data point set decomposition unit is used to decompose the three-dimensional data point set of the dynamic object 3D model and the three-dimensional data point set of the static object 3D model to obtain the dynamic object 3D array and the static object 3D array. The dynamic object 3D array includes the X component, Y component and Z component of the dynamic object 3D model, and the static object 3D array includes the X component, Y component and Z component of the static object 3D model.

[0114] The X-component covariance matrix calculation unit is used to calculate the covariance matrix of the X-component based on the X-components of the dynamic and static 3D object models. ,in, , For the first The X component of a dynamic 3D model of an object for The mean, For the first The X component of a static 3D model of an object For the mean value of the Y component of the first dynamic object three-dimensional model.

[0115] a Y component covariance matrix calculation unit configured to calculate a covariance matrix of the Y component according to the Y components of the dynamic object three-dimensional models and the static object three-dimensional models wherein, , is the mean value of the Y component of the first dynamic object three-dimensional model. is the mean value of the Y component of the first dynamic object three-dimensional model. is the mean value of the Y component of the first dynamic object three-dimensional model. is the mean value of the Y component of the first dynamic object three-dimensional model.

[0116] a Z component covariance matrix calculation unit configured to calculate a covariance matrix of the Z component according to the Z components of the dynamic object three-dimensional models and the static object three-dimensional models wherein, , is the Z component of the first dynamic object three-dimensional model. is the mean value of the Z component of the first dynamic object three-dimensional model. is the Z component of the first dynamic object three-dimensional model. , is the mean value of the Z component of the first dynamic object three-dimensional model.

[0117] an eigenvector calculation unit configured to perform eigenvalue decomposition according to the covariance matrix of the X component , the covariance matrix of the Y component and the covariance matrix of the Z component to obtain eigenvectors of the X component, the Y component and the Z component.

[0118] an eigenvalue calculation unit configured to obtain the maximum eigenvalue and the minimum eigenvalue of the X component, the maximum eigenvalue and the minimum eigenvalue of the Y component, the maximum eigenvalue and the minimum eigenvalue of the Z component in the dynamic object three-dimensional models, and the maximum eigenvalue and the minimum eigenvalue of the X component, the maximum eigenvalue and the minimum eigenvalue of the Y component, the maximum eigenvalue and the minimum eigenvalue of the Z component in the static object three-dimensional models.

[0119] ​​​​​​​​The dynamic model establishing unit is configured to project the maximum eigenvalue and the minimum eigenvalue of the X component in the dynamic object three-dimensional model onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the X axis; project the maximum eigenvalue and the minimum eigenvalue of the Y component in the dynamic object three-dimensional model onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the Y axis; and project the maximum eigenvalue and the minimum eigenvalue of the Z component in the dynamic object three-dimensional model onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the Z axis.

[0120] The static model establishing unit is configured to project the maximum eigenvalue and the minimum eigenvalue of the X component in the static object three-dimensional model onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the X axis; project the maximum eigenvalue and the minimum eigenvalue of the Y component in the static object three-dimensional model onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Y axis; and project the maximum eigenvalue and the minimum eigenvalue of the Z component in the static object three-dimensional model onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Z axis.

[0121] The space overlap judging unit is configured to perform space overlap judgment; judge whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the X axis and the bounding box three-dimensional model of the static object three-dimensional model on the X axis exist space overlap, whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Y axis and the bounding box three-dimensional model of the static object three-dimensional model on the Y axis exist space overlap, and whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Z axis and the bounding box three-dimensional model of the static object three-dimensional model on the Z axis exist space overlap.

[0122] The judgment result output unit is configured to output the space overlap judgment result and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model exist overlap.

[0123] Preferably, the dynamic model establishing module specifically comprises:

[0124] The robot control unit is configured to control the surgical robot to drive the implantation handset so that the mounting sleeve on the implantation handset is sleeved on the calibration column on the calibration plate.

[0125] The data acquisition unit is configured to control the optical positioner to acquire the pose of the calibration plate in an optical three-dimensional coordinate system of the optical positioner and the pose of the implantation handset in the optical three-dimensional coordinate system and synchronously acquire the servo control data when the mounting sleeve on the implantation handset is sleeved on the calibration column on the calibration plate.

[0126] The coordinate transformation relationship analysis unit is used to establish coordinate transformation relationships between the base three-dimensional coordinate system, the optical three-dimensional coordinate system, and the mobile phone three-dimensional coordinate system based on the pose of the calibration plate in the optical positioning instrument, the pose of the implantation mobile phone in the optical three-dimensional coordinate system, and the servo control data.

[0127] The real-time data reading unit is used to read the real-time pose of the planting mobile phone in the optical three-dimensional coordinate system.

[0128] The calibration execution unit is used to calculate the real-time coordinates of the drill bit E for planting the mobile phone in the base three-dimensional coordinate system based on the real-time pose of the optical three-dimensional coordinate system and the coordinate transformation relationship between the base three-dimensional coordinate system and the mobile phone three-dimensional coordinate system.

[0129] Preferably, the second coordinate value calculation module is further configured to: obtain the transformation relationship between the second joint three-dimensional coordinate system and the first joint three-dimensional coordinate system based on the coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system. ,pass and Calculated ,in, , This is matrix multiplication.

[0130] The third coordinate value calculation module is also used to: obtain the transformation relationship between the third joint three-dimensional coordinate system and the second joint three-dimensional coordinate system based on the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system. ,pass and Calculated ,in, , This is matrix multiplication.

[0131] The fourth coordinate value calculation module is also used to: obtain the transformation relationship between the third joint three-dimensional coordinate system and the third joint three-dimensional coordinate system based on the coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system. ,pass and Calculated ,in, , This is matrix multiplication.

[0132] The fifth coordinate value calculation module is also used to: obtain the transformation relationship between the fifth joint three-dimensional coordinate system and the fourth joint three-dimensional coordinate system based on the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system. ,pass and Calculated wherein, , is matrix multiplication.

[0133] The sixth coordinate value calculation module is further configured to obtain a conversion relationship between the sixth joint three-dimensional coordinate system and the fifth joint three-dimensional coordinate system according to a coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system , by and calculation wherein, , is matrix multiplication.

[0134] Preferably, the static model establishment module is configured to establish a static object three-dimensional model with a center of a base three-dimensional coordinate system of the control base, and the static object model comprises a first outer shell three-dimensional model, a second outer shell three-dimensional model and a third outer shell three-dimensional model in sequence from outside to inside.

[0135] The collision analysis module is configured to perform collision detection by comparing the dynamic object three-dimensional model and the static object three-dimensional model, and output a third-level collision warning when the dynamic object three-dimensional model and the third outer shell three-dimensional model overlap, control the planting surgery robot to stop automatic operation, output a second-level collision warning when the dynamic object three-dimensional model and the second outer shell three-dimensional model overlap, control the planting surgery robot to stop automatic operation and control the planting surgery robot to move passively at a limited speed, output a first-level collision warning when the dynamic object three-dimensional model and the first outer shell three-dimensional model overlap, lock the planting surgery robot, and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model overlap.

[0136] To solve the above problems, the application provides a storage medium, which stores a computer program, and the computer program comprises program instructions, and when the program instructions are executed by a processor, the processor executes the planting surgery robot anti-collision control method.

[0137] It should be understood that the application of the application is not limited to the above examples, and those skilled in the art can make improvements or changes according to the above description, and all these improvements and changes shall belong to the protection scope of the claims of the application.

Claims

1. A collision avoidance control method for a planting surgery robot, characterized by, The collision avoidance control method of the implant surgery robot is based on the implant surgery robot, and the implant surgery robot comprises a control base, a first joint, a second joint, a third joint, a fourth joint, a fifth joint and a sixth joint; The distance between the control base and the first joint is L0; The first joint and the second joint are connected through a first vertical shaft and a first horizontal shaft, the length of the first vertical shaft is L1, the length of the first horizontal shaft is L2, and the shaft centers of the two ends of the first horizontal shaft are P1 and P2 respectively; The second joint and the third joint are connected through a second vertical shaft, the length of the second vertical shaft is L3, and the shaft centers of the two ends of the second vertical shaft are P2 and P3 respectively; The third joint and the fourth joint are connected through a second horizontal shaft and a third horizontal shaft, the length of the second horizontal shaft is L4, the length of the third horizontal shaft is L5, the shaft centers of the two ends of the second horizontal shaft are P3 and P4 respectively, and the shaft centers of the two ends of the third horizontal shaft are P4 and P5 respectively; The fourth joint and the fifth joint are connected through a first curved shaft and a fourth horizontal shaft, the projection length of the first curved shaft is L6, the length of the fourth horizontal shaft is L7, and the shaft center of the end of the fourth horizontal shaft away from the first curved shaft is P6; The fifth joint and the sixth joint are connected through a fifth horizontal shaft and a third vertical shaft, the length of the fifth horizontal shaft is L8, the length of the third vertical shaft is L9, the sixth joint and the implant handset are connected through a fourth vertical shaft, the length of the fourth vertical shaft is L10, the shaft centers of the two ends of the fifth horizontal shaft are P6 and P7, the shaft centers of the two ends of the third vertical shaft are P7 and P8, the shaft center of the end of the fourth vertical shaft close to the implant handset is P9, and the drill bit of the implant handset is E; The collision avoidance control method comprises the following steps: S1. Obtain the origin, X-axis, Y-axis and Z-axis of the base three-dimensional coordinate system of the control base; S2. Obtain the rotation angle of the first joint through the server control data, and obtain the coordinate translation value between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system according to the distance L0 between the base and the first joint, so as to calculate the conversion relationship between the first joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M1 , according to B H M1 , the distance L0 between the base and the first joint and the rotation angle of the first joint, the real-time coordinate values of P1 and P2 in the base three-dimensional coordinate system are calculated; wherein B is the base three-dimensional coordinate system, M1 is the first joint three-dimensional coordinate system; S3. The rotation angle of the second joint is obtained through the server control data, the coordinate translation value of the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system is obtained according to the length L1 of the first vertical axis and the length L2 of the first horizontal axis, so as to calculate the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M2 , the real-time coordinate value of P3 in the base three-dimensional coordinate system is calculated according to B H M2 , the length L3 of the second vertical axis and the rotation angle of the second joint; wherein M2 is the second joint three-dimensional coordinate system; S4. Obtain the rotation angle of the third joint through servo control data, and obtain the coordinate translation value from the second joint's three-dimensional coordinate system to the third joint's three-dimensional coordinate system based on the length L3 of the second vertical axis. This allows for the calculation of the transformation relationship between the third joint's three-dimensional coordinate system and the base's three-dimensional coordinate system. B H M3 Based on the length L4 of the second transverse axis and the length L5 of the third transverse axis, B H M3 The rotation angle of the third joint is used to calculate the real-time coordinate values ​​of P4 and P5 in the three-dimensional coordinate system of the base; where M3 is the three-dimensional coordinate system of the third joint. S5. Obtain the rotation angle of the fourth joint through the server control data, and obtain the coordinate translation value of the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system according to the length L4 of the second transverse shaft and the length L5 of the third transverse shaft, so as to calculate the conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M4 , according to B H M4 , the projection length L6 of the first bending shaft, the length L7 of the fourth transverse shaft and the rotation angle of the fourth joint, the real-time coordinate value of P6 in the base three-dimensional coordinate system is calculated; wherein M4 is the fourth joint three-dimensional coordinate system; S6. Obtain the rotation angle of the fifth joint through the server control data, and obtain the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system according to the projection length L6 of the first bending shaft and the length L7 of the fourth transverse shaft, so as to calculate the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M5 , according to B H M5 , the length L8 of the fifth transverse shaft, the length L9 of the third vertical shaft and the rotation angle of the fifth joint, the real-time coordinate values of P7 and P8 in the base three-dimensional coordinate system are calculated; wherein M5 is the fifth joint three-dimensional coordinate system; S7. Obtain the rotation angle of the sixth joint through the server control data, obtain the coordinate translation value of the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system according to the length L8 of the fifth transverse axis and the length L9 of the third vertical axis, and calculate the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M6 , according to B H M6 , the length L10 of the fourth vertical axis and the rotation angle of the sixth joint, the real-time coordinate value of P9 in the base three-dimensional coordinate system is calculated; wherein M6 is the sixth joint three-dimensional coordinate system; S8. Obtain the real-time coordinate value of E in the base three-dimensional coordinate system, construct a dynamic object three-dimensional model according to the real-time coordinate values of P1, P2, P3, P4, P5, P6, P7, P8, P9 and E in the base three-dimensional coordinate system, and the dynamic object three-dimensional model comprises a first vertical shaft, a first horizontal shaft, a second vertical shaft, a second horizontal shaft, a third horizontal shaft, a first curved shaft, a fourth horizontal shaft, a fifth horizontal shaft, a third vertical shaft, a fourth vertical shaft and a wrapped box three-dimensional model of the implant handset; S9. Establish a static object three-dimensional model with the origin of the base three-dimensional coordinate system of the control base as the center, and the static object three-dimensional model comprises a first outer shell three-dimensional model, a second outer shell three-dimensional model and a third outer shell three-dimensional model in turn; S10. Collision detection is performed by comparing the dynamic object three-dimensional model and the static object three-dimensional model, and a collision warning is output when the dynamic object three-dimensional model and the static object three-dimensional model overlap.

2. The collision control method of a surgical robot for planting according to claim 1, wherein, Step S10 specifically comprises the following steps: S1001. Extract the three-dimensional data point set of the dynamic object three-dimensional model and the three-dimensional data point set of the static object three-dimensional model; S1002. Decompose the three-dimensional data point set of the dynamic object three-dimensional model and the three-dimensional data point set of the static object three-dimensional model to obtain a dynamic object three-dimensional array and a static object three-dimensional array, wherein the dynamic object three-dimensional array comprises X component, Y component and Z component of the dynamic object three-dimensional model, and the static object three-dimensional array comprises X component, Y component and Z component of the static object three-dimensional model; S1003. Calculate a covariance matrix COV of the X components according to the X components of the dynamic object three-dimensional models and the static object three-dimensional models X (i,j), wherein COV X (i,j) = E{[X i -E(X i )][X j -E(X j )]}, X i is the X component of the ith dynamic object three-dimensional model, E(X i ) is the mean of X i , X j is the X component of the jth static object three-dimensional model, and E(X j ) is the mean of X j ; S1004. Calculate a covariance matrix COV of Y components according to the Y components of the dynamic object three-dimensional models and the static object three-dimensional models Y (i,j), wherein COV Y (i,j) = E{[Y i -E(Y i )][Y j -E(Y j )]}, Y i is the Y component of the ith dynamic object three-dimensional model, E(Y i ) is the mean of Y i , Y j is the Y component of the jth static object three-dimensional model, and E(Y j ) is the mean of Y j ; S1005. Calculate a covariance matrix COV of Z components according to the Z components of the dynamic object three-dimensional models and the static object three-dimensional models Z (i,j), wherein COV Z (i,j) = E{[Z i -E(Z i )][Z j -E(Z j )]}, Z i is the Z component of the ith dynamic object three-dimensional model, E(Z i ) is the mean of Z i , Z j is the Z component of the jth static object three-dimensional model E(Z j ), and E(Z j ) is the mean of Z j ; S1006. Eigenvalues of the covariance matrix COV X (i,j) of the Y component Y (i,j) of the Z component Z (i,j) of the Z component are obtained by eigenvalue decomposition. S1007. Obtain the maximum eigenvalue and the minimum eigenvalue of the X component, the maximum eigenvalue and the minimum eigenvalue of the Y component, and the maximum eigenvalue and the minimum eigenvalue of the Z component of the dynamic object three-dimensional model, and obtain the maximum eigenvalue and the minimum eigenvalue of the X component, the maximum eigenvalue and the minimum eigenvalue of the Y component, and the maximum eigenvalue and the minimum eigenvalue of the Z component of the static object three-dimensional model; S1008. Project the maximum eigenvalue and the minimum eigenvalue of the X component of the dynamic object three-dimensional model onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the X axis; project the maximum eigenvalue and the minimum eigenvalue of the Y component of the dynamic object three-dimensional model onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the Y axis; and project the maximum eigenvalue and the minimum eigenvalue of the Z component of the dynamic object three-dimensional model onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the dynamic object three-dimensional model on the Z axis; S1009. Project the maximum eigenvalue and the minimum eigenvalue of the X component of the static object three-dimensional model onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the X axis; project the maximum eigenvalue and the minimum eigenvalue of the Y component of the static object three-dimensional model onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Y axis; and project the maximum eigenvalue and the minimum eigenvalue of the Z component of the static object three-dimensional model onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Z axis; S1010. Perform spatial overlap judgment; judge whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the X axis and the bounding box three-dimensional model of the static object three-dimensional model on the X axis exist spatial overlap, whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Y axis and the bounding box three-dimensional model of the static object three-dimensional model on the Y axis exist spatial overlap, and whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Z axis and the bounding box three-dimensional model of the static object three-dimensional model on the Z axis exist spatial overlap; S1011. Output the spatial overlap judgment result, and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model exist overlap.

3. The collision control method of a surgical robot for planting according to claim 1, wherein, Step S8 specifically comprises the following steps: S801. Control the planting robot to drive the planting hand to thereby sleeve the mounting sleeve on the planting hand to the calibration column on the calibration plate; S802. When the mounting sleeve on the planting hand is sleeved to the calibration column on the calibration plate, control the optical positioner to obtain the pose of the calibration plate in the optical three-dimensional coordinate system of the optical positioner and the pose of the planting hand in the optical three-dimensional coordinate system, and synchronously obtain the servo control data; S803. According to the pose of the calibration plate in the optical three-dimensional coordinate system of the optical positioner, the pose of the planting handset in the optical three-dimensional coordinate system and the servo control data, a coordinate conversion relationship is established between the base three-dimensional coordinate system, the optical three-dimensional coordinate system and the handset three-dimensional coordinate system of the planting handset; S804. The real-time pose of the planting handset in the optical three-dimensional coordinate system is read; S805. According to the real-time pose of the optical three-dimensional coordinate system and the coordinate conversion relationship between the base three-dimensional coordinate system and the handset three-dimensional coordinate system, the real-time coordinate value of the drill bit E of the planting handset in the base three-dimensional coordinate system is calculated.

4. The collision control method of a surgical robot for planting according to claim 1, wherein, In step S3, the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M2 Specifically: according to the coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system, the conversion relationship between the second joint three-dimensional coordinate system and the first joint three-dimensional coordinate system is obtained M1 H M2 , by B H M1 and M1 H M2 , the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M2 , wherein B H M2 = B H M1 * M1 H M2 , * is matrix multiplication; In step S4, the conversion relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M3 Specifically: according to the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system, the conversion relationship between the third joint three-dimensional coordinate system and the second joint three-dimensional coordinate system is obtained M2 H M3 , by B H M2 and M2 H M3 calculation B H M3 , B H M3 = B H M2 * M2 H M3 , * is matrix multiplication; In step S5, the conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M4 Specifically: according to the coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system, the conversion relationship between the fourth joint three-dimensional coordinate system and the third joint three-dimensional coordinate system is obtained M3 H M4 , by B H M3 and M3 H M4 calculation B H M4 , wherein B H M4 = B H M3 * M3 H M4 , * is matrix multiplication; In step S6, the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M5 Specifically: the conversion relationship between the fifth joint three-dimensional coordinate system and the fourth joint three-dimensional coordinate system is obtained according to the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system M4 H M5 , by B H M4 and M4 H M5 , the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M5 , wherein B H M5 = B H M4 * M4 H M5 , * is matrix multiplication; In step S7, the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M6 Specifically: according to the coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system, the conversion relationship between the sixth joint three-dimensional coordinate system and the fifth joint three-dimensional coordinate system is obtained M5 H M6 , by B H M5 and M5 H M6 , the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system is calculated B H M6 , wherein B H M6 = B H M5 * M5 H M6 , * is matrix multiplication.

5. The collision control method of a surgical robot for planting according to claim 1, wherein Step S10 specifically comprises: outputting a third-level collision warning when the dynamic object three-dimensional model overlaps with the third shell three-dimensional model, controlling the planting surgery robot to stop automatic operation; outputting a second-level collision warning when the dynamic object three-dimensional model overlaps with the second shell three-dimensional model, controlling the planting surgery robot to stop automatic operation and controlling the planting surgery robot to move passively at a limited speed; outputting a first-level collision warning when the dynamic object three-dimensional model overlaps with the first shell three-dimensional model, and locking the planting surgery robot.

6. A planting surgery robot anti-collision control system, the planting surgery robot anti-collision control system being based on the operation of a planting surgery robot, the planting surgery robot comprising a control base, a first joint, a second joint, a third joint, a fourth joint, a fifth joint and a sixth joint; The distance between the control base and the first joint is L0; The first joint and the second joint are connected by a first vertical shaft and a first horizontal shaft, the length of the first vertical shaft is L1, the length of the first horizontal shaft is L2, and the shaft centers of the two ends of the first horizontal shaft are P1 and P2, respectively; The second joint and the third joint are connected by a second vertical shaft, the length of the second vertical shaft is L3, and the shaft centers of the two ends of the second vertical shaft are P2 and P3, respectively; The third joint and the fourth joint are connected by a second horizontal shaft and a third horizontal shaft, the length of the second horizontal shaft is L4, the length of the third horizontal shaft is L5, the shaft centers of the two ends of the second horizontal shaft are P3 and P4, respectively, and the shaft centers of the two ends of the third horizontal shaft are P4 and P5, respectively; The fourth joint and the fifth joint are connected by a first curved shaft and a fourth horizontal shaft, the projection length of the first curved shaft is L6, the length of the fourth horizontal shaft is L7, and the shaft center of the end of the fourth horizontal shaft away from the first curved shaft is P6; The fifth joint and the sixth joint are connected by a fifth horizontal shaft and a third vertical shaft, the length of the fifth horizontal shaft is L8, the length of the third vertical shaft is L9, the sixth joint and the planting handset are connected by a fourth vertical shaft, the length of the fourth vertical shaft is L10; the shaft centers of the two ends of the fifth horizontal shaft are P6 and P7, the shaft centers of the two ends of the third vertical shaft are P7 and P8, the shaft center of the end of the fourth vertical shaft close to the planting handset is P9, and the drill bit of the planting handset is E; The anti-collision control system comprises: A base coordinate system acquisition module for acquiring the origin, X-axis, Y-axis and Z-axis of the base three-dimensional coordinate system of the control base; The first coordinate value calculation module is configured to obtain the rotation angle of the first joint through the servo control data, obtain the coordinate translation value between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system according to the distance L0 between the base and the first joint, and calculate the conversion relationship between the base three-dimensional coordinate system and the first joint three-dimensional coordinate system B H M1 , according to B H M1 , the distance L0 between the base and the first joint, and the rotation angle of the first joint, the real-time coordinate values of P1 and P2 in the base three-dimensional coordinate system are calculated; wherein B is the base three-dimensional coordinate system, and M1 is the first joint three-dimensional coordinate system. The second coordinate value calculation module is configured to obtain the rotation angle of the second joint through the servo control data, obtain the coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system according to the length L1 of the first vertical shaft and the length L2 of the first horizontal shaft, and calculate the conversion relationship between the second joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M2 , according to B H M2 , the length L3 of the second vertical shaft and the rotation angle of the second joint, the real-time coordinate value of P3 in the base three-dimensional coordinate system is calculated; wherein M2 is the second joint three-dimensional coordinate system; The third coordinate value calculation module is used to obtain the rotation angle of the third joint through server control data, and to obtain the coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system based on the length L3 of the second vertical axis, thereby calculating the transformation relationship between the third joint three-dimensional coordinate system and the base three-dimensional coordinate system. B H M3 Based on the length L4 of the second transverse axis and the length L5 of the third transverse axis, B H M3 The rotation angle of the third joint is used to calculate the real-time coordinate values ​​of P4 and P5 in the three-dimensional coordinate system of the base; where M3 is the three-dimensional coordinate system of the third joint. A fourth coordinate value calculation module is configured to obtain a rotation angle of the fourth joint through the server control data, obtain a coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system according to the length L4 of the second lateral shaft and the length L5 of the third lateral shaft, and calculate a conversion relationship between the fourth joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M4 , according to B H M4 , the projection length L6 of the first bending shaft, the length L7 of the fourth lateral shaft, and the rotation angle of the fourth joint, to obtain a real-time coordinate value of P6 in the base three-dimensional coordinate system; wherein M4 is the fourth joint three-dimensional coordinate system; The fifth coordinate value calculation module is configured to obtain the rotation angle of the fifth joint through the server control data, obtain the coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system according to the projection length L6 of the first bending shaft and the length L7 of the fourth transverse shaft, and calculate the conversion relationship between the fifth joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M5 , according to B H M5 , the length L8 of the fifth transverse shaft, the length L9 of the third vertical shaft and the rotation angle of the fifth joint, the real-time coordinate values of P7 and P8 in the base three-dimensional coordinate system are calculated; wherein M5 is the fifth joint three-dimensional coordinate system; A sixth coordinate value calculation module is configured to obtain the rotation angle of the sixth joint through the server control data, obtain the coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system according to the length L8 of the fifth horizontal shaft and the length L9 of the third vertical shaft, and calculate the conversion relationship between the sixth joint three-dimensional coordinate system and the base three-dimensional coordinate system B H M6 , calculate the real-time coordinate value of P9 in the base three-dimensional coordinate system according to B H M6 , the length L10 of the fourth vertical shaft and the rotation angle of the sixth joint; wherein M6 is the sixth joint three-dimensional coordinate system; The dynamic model establishing module is configured to acquire real-time coordinate values of E in the base three-dimensional coordinate system, and construct a dynamic object three-dimensional model according to the real-time coordinate values of P1, P2, P3, P4, P5, P6, P7, P8, P9 and E in the base three-dimensional coordinate system, wherein the dynamic object three-dimensional model comprises a first vertical axis, a first horizontal axis, a second vertical axis, a second horizontal axis, a third horizontal axis, a first bending axis, a fourth horizontal axis, a fifth horizontal axis, a third vertical axis, a fourth vertical axis and a package box three-dimensional model of the planting mobile phone; The static model establishing module is configured to establish a static object three-dimensional model with the origin of the base three-dimensional coordinate system of the control base as the center, wherein the static object three-dimensional model comprises a first shell three-dimensional model, a second shell three-dimensional model and a third shell three-dimensional model in sequence; The collision analysis module is configured to perform collision detection by comparing the dynamic object three-dimensional model and the static object three-dimensional model, and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model overlap.

7. The collision control system for a surgical robot of claim 6, wherein, The collision analysis module comprises: A data point set extraction unit configured to extract a three-dimensional data point set of the dynamic object three-dimensional model and a three-dimensional data point set of the static object three-dimensional model; A data point set decomposition unit configured to decompose the three-dimensional data point set of the dynamic object three-dimensional model and the three-dimensional data point set of the static object three-dimensional model to obtain a dynamic object three-dimensional array and a static object three-dimensional array, wherein the dynamic object three-dimensional array comprises an X component, a Y component and a Z component of the dynamic object three-dimensional model, and the static object three-dimensional array comprises an X component, a Y component and a Z component of the static object three-dimensional model; an X-component covariance matrix calculation unit configured to calculate a covariance matrix COV X (i,j) of X-components, based on X-components of the dynamic object three-dimensional models and the static object three-dimensional models X (i,j) = E{[X i -E(X i )][X j -E(X j )]}, X i is an X-component of the i-th dynamic object three-dimensional model, E(X i ) is a mean value of X i , X j is an X-component of the j-th static object three-dimensional model, and E(X j ) is a mean value of X j ​ a Y component covariance matrix calculation unit configured to calculate a covariance matrix COV of Y components based on Y components of the dynamic object three-dimensional models and the static object three-dimensional models Y (i,j), wherein COV Y (i,j) = E{[Y i -E(Y i )][Y j -E(Y j )]}, Y i is the Y component of the i-th dynamic object three-dimensional model, E(Y i ) is the mean of Y i , Y j is the Y component of the j-th static object three-dimensional model, and E(Y j ) is the mean of Y j ; a Z-component covariance matrix calculation unit configured to calculate a covariance matrix COV of the Z-component according to the Z-component of the dynamic object three-dimensional model and the Z-component of the static object three-dimensional model Z (i,j), wherein COV Z (i,j) = E{[Z i -E(Z i )][Z j -E(Z j )]}, Z i is the Z-component of the i-th dynamic object three-dimensional model, E(Z i ) is the mean of Z i , Z j is the Z-component of the j-th static object three-dimensional model E(Z j ), and E(Z j ) is the mean of Z j ; a feature vector calculation unit for performing eigenvalue decomposition on the covariance matrix COV X (i,j) of the X component, the covariance matrix COV Y (i,j) of the Y component, and the covariance matrix COV Z (i,j) of the Z component to obtain the feature vectors of the X component, the Y component, and the Z component; A characteristic value calculation unit configured to acquire maximum and minimum characteristic values of the X component, maximum and minimum characteristic values of the Y component, maximum and minimum characteristic values of the Z component in the dynamic object three-dimensional model, and acquire maximum and minimum characteristic values of the X component, maximum and minimum characteristic values of the Y component, maximum and minimum characteristic values of the Z component in the static object three-dimensional model; A dynamic model establishing unit configured to project the maximum and minimum characteristic values of the X component in the dynamic object three-dimensional model onto a characteristic vector of the X component to obtain a package box three-dimensional model of the dynamic object three-dimensional model on the X axis, project the maximum and minimum characteristic values of the Y component in the dynamic object three-dimensional model onto a characteristic vector of the Y component to obtain a package box three-dimensional model of the dynamic object three-dimensional model on the Y axis, and project the maximum and minimum characteristic values of the Z component in the dynamic object three-dimensional model onto a characteristic vector of the Z component to obtain a package box three-dimensional model of the dynamic object three-dimensional model on the Z axis. The static model establishing unit is configured to project the maximum eigenvalue and the minimum eigenvalue of the X component in the static object three-dimensional model onto the eigenvector of the X component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the X axis; project the maximum eigenvalue and the minimum eigenvalue of the Y component in the static object three-dimensional model onto the eigenvector of the Y component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Y axis; and project the maximum eigenvalue and the minimum eigenvalue of the Z component in the static object three-dimensional model onto the eigenvector of the Z component to obtain a bounding box three-dimensional model of the static object three-dimensional model on the Z axis. The spatial overlap judging unit is configured to perform spatial overlap judgment; judge whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the X axis and the bounding box three-dimensional model of the static object three-dimensional model on the X axis exist spatial overlap, whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Y axis and the bounding box three-dimensional model of the static object three-dimensional model on the Y axis exist spatial overlap, and whether the bounding box three-dimensional model of the dynamic object three-dimensional model on the Z axis and the bounding box three-dimensional model of the static object three-dimensional model on the Z axis exist spatial overlap. The judging result output unit is configured to output the spatial overlap judgment result and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model exist overlap.

8. The collision control system for a surgical robot of claim 6, wherein, The second coordinate value calculation module is further configured to obtain a conversion relationship between the second joint three-dimensional coordinate system and the first joint three-dimensional coordinate system according to a coordinate translation value from the first joint three-dimensional coordinate system to the second joint three-dimensional coordinate system M1 H M2 , by B H M1 and M1 H M2 computation B H M2 , wherein B H M2 = B H M1 * M1 H M2 , * is matrix multiplication; The third coordinate value calculation module is further configured to obtain a conversion relationship between the third joint three-dimensional coordinate system and the second joint three-dimensional coordinate system according to a coordinate translation value from the second joint three-dimensional coordinate system to the third joint three-dimensional coordinate system M2 H M3 , by B H M2 and M2 H M3 computation B H M3 , wherein B H M3 = B H M2 * M2 H M3 , * is matrix multiplication; The fourth coordinate value calculation module is further configured to obtain a conversion relationship between the fourth joint three-dimensional coordinate system and the third joint three-dimensional coordinate system according to a coordinate translation value from the third joint three-dimensional coordinate system to the fourth joint three-dimensional coordinate system M3 H M4 , by B H M3 and M3 H M4 computation B H M4 , wherein B H M4 = B H M3 * M3 H M4 , * is matrix multiplication; The fifth coordinate value calculation module is further configured to obtain a conversion relationship between the fifth joint three-dimensional coordinate system and the fourth joint three-dimensional coordinate system according to a coordinate translation value from the fourth joint three-dimensional coordinate system to the fifth joint three-dimensional coordinate system M4 H M5 , by B H M4 and M4 H M5 computation B H M5 , wherein B H M5 = B H M4 * M4 H M5 , * is matrix multiplication; The sixth coordinate value calculation module is further configured to obtain a conversion relationship between the sixth joint three-dimensional coordinate system and the fifth joint three-dimensional coordinate system according to a coordinate translation value from the fifth joint three-dimensional coordinate system to the sixth joint three-dimensional coordinate system M5 H M6 , by B H M5 and M5 H M6 computation B H M6 , wherein B H M6 = B H M5 * M5 H M6 , * is matrix multiplication.

9. The collision control system for a surgical robot of claim 6, wherein; The collision analysis module is configured to perform collision detection by comparing the dynamic object three-dimensional model and the static object three-dimensional model; output a third-level collision warning when the dynamic object three-dimensional model and the third-enclosure three-dimensional model exist overlap, control the planting surgery robot to stop automatic operation; output a second-level collision warning when the dynamic object three-dimensional model and the second-enclosure three-dimensional model exist overlap, control the planting surgery robot to stop automatic operation, and control the planting surgery robot to move passively at a limited speed; output a first-level collision warning when the dynamic object three-dimensional model and the first-enclosure three-dimensional model exist overlap, and lock the planting surgery robot; and output a collision warning when the dynamic object three-dimensional model and the static object three-dimensional model exist overlap.

10. A storage medium, characterized by The storage medium stores a computer program, and the computer program includes program instructions. When the program instructions are executed by the processor, the processor executes the planting surgery robot anti-collision control method in any one of claims 1-5.

Images