Surgical robot positioning device, method and medium based on multi-sensor fusion

Abstract

The present invention provides an initial positioning device for an intelligent surgical robot based on multi-sensor fusion, comprising: an alignment handle, a first sensing module, a second sensing module, and a positioning module. The first sensing module is fixed to the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle to collect the quaternion output by the first sensing module. The second sensing module is installed on the body of the intelligent surgical robot to collect the cable length, and the second sensing module is rotatably connected to the alignment handle. When the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the cable length output by the second sensing module. The present invention also discloses an initial positioning method for an intelligent surgical robot based on multi-sensor fusion and a corresponding electronic device.

Description

Technical Field

[0001] The present invention relates to the field of medical technology, and in particular to an initial positioning device, method and computer-readable storage medium for an intelligent surgical robot based on multi-sensor fusion. Background Art

[0002] As an important part of modern medical technology, surgical robot systems are widely used in various complex surgeries, such as cardiac surgery, orthopedic surgery, urological surgery, etc. The core goal is to improve the accuracy of surgery, reduce trauma and improve surgical efficiency. A surgical robot system generally includes multiple subsystems such as a mechanical structure, sensors, actuators, a control system, etc. Among them, initial positioning, as the starting stage of a surgical robot system, its accuracy and efficiency directly affect the effect and safety of the entire surgical process.

[0003] Initial positioning refers to the process of accurately moving the robotic arm or manipulator to a predetermined initial position and posture when the surgical robot starts the surgery. This process requires accurately identifying the position relationship of the surgical robot relative to the patient, the operating table or other reference coordinate systems in the complex surgical environment and making adjustments. Due to the complexity of the surgical environment and the high risk of surgical operations, the accuracy and reliability of initial positioning are crucial. The traditional initial positioning methods have the following core defects: (1) Cooperative dragging method: The cooperative dragging method relies on manual adjustment by the operator. Specifically, manually adjust the position and posture of the robotic arm. However, in a complex environment, it is difficult to guarantee the accuracy and stability of manual operation. Especially in the case of high-precision positioning, errors are likely to occur. In addition, the cooperative dragging method takes a long time, reducing surgical efficiency, and there are subjective differences between different operators, resulting in poor repeatability. (2) Host computer control method: Although the host computer control method reduces manual intervention, it is still a human-in-the-loop control, facing the problem of limited environmental perception ability, resulting in insufficient positioning accuracy or a long time-consuming. At the same time, when the host computer controls the initial posture, it often lacks sufficient flexibility and is difficult to cope with unexpected situations in a complex surgical environment, affecting the effect of the entire initial positioning. Moreover, in the surgical preparation stage and the surgical process of a minimally invasive surgical robot, the positioning is mainly achieved by manual dragging. Only during the contraction and expansion process can automatic positioning be performed, and the final alignment and other processes still cannot be automatic, such as by dragging, commands, etc.; for example, vision-based methods use cameras to obtain images, but the effect is not good when the light changes or there is occlusion. Force feedback-based methods rely on force sensors, but are easily interfered by the outside world in a complex environment. Infrared positioning-based methods have high accuracy, but are limited by the position and quantity of markers and have limited applicability. In summary, how to quickly and efficiently achieve positioning through multi-sensors is of increasing importance for surgical treatment. Summary of the Invention

[0004] The present invention provides an initial positioning device, method and computer-readable storage medium for an intelligent surgical robot based on multi-sensor fusion. Based on the cooperative positioning technology of multi-sensor fusion, measurement and solution are carried out through attitude acquisition data more than twice to achieve accurate and efficient surgical positioning.

[0005] In the first aspect, an initial positioning device for an intelligent surgical robot based on multi-sensor fusion is provided, including: an alignment handle, a first sensing module, a second sensing module, and a positioning module. Among them, the first sensing module is fixed at the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle to collect the quaternion output by the first sensing module; the second sensing module is installed on the body of the intelligent surgical robot to collect the wire-pulling length, and the second sensing module is rotatably connected to the alignment handle; when the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the wire-pulling length output by the second sensing module, including the following calculation steps:

[0006] Step S1: Calculate the rotation matrix R from the quaternion:

[0007]

[0008] Among them, (x, y, z) represents the vector part of the quaternion. x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle. y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle. z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle. w represents the scalar part of the quaternion, that is, the cosine value of the half angle. R represents the rotation matrix;

[0009] Step S2: Extract the direction vector direction:

[0010]

[0011] Among them, direction represents the direction vector. (x, y, z) represents the vector part of the quaternion. x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle. y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle. z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle. w represents the scalar part of the quaternion, that is, the cosine value of the half angle;

[0012] Step S3: Calculate the three-dimensional position coordinates P of the alignment handle connection point

[0013] P 末端 = P 连接 - d × direction,

[0014] where direction represents the direction vector, P 末端 represents the three - dimensional position variable of the end of the alignment handle, d represents the length of the alignment handle, and P 连接 represents the three - dimensional position variable of the connection point of the alignment handle;

[0015] Step S4: Establish an equation:

[0016]

[0017] where P g represents the three - dimensional position coordinates of the second sensing module, P 连接 represents the three - dimensional position variable of the connection point of the alignment handle, L represents the length of the wire rope, x 连接 , y 连接 , z respectively represent the three - dimensional position coordinates of the connection point of the alignment handle; x g , y g , z g respectively represent the three - dimensional position coordinates of the second sensing module;

[0018] Step S5: The male docking head at the end of the alignment handle aligns with the female docking head and changes three postures, and solve the equation in Step S4, where the corresponding direction vector direction after the posture change complies with Step S2.

[0019] In some embodiments, it further includes: a docking head. When the alignment handle is in the measurement position, the female docking head is used to dock with the male docking head of the alignment handle.

[0020] In some embodiments, the positioning module is further configured to record the positioning of the robotic arm of the intelligent surgical robot that has been determined as the initial value for the next movement.

[0021] In some embodiments, the first sensing module performs an initialization operation before use, so that the zero - bias error is less than 0.05°; and the sampling frequency is set to 100 Hz.

[0022] In some embodiments, the range of the wire rope of the second sensing module is 0 - 1000 mm.

[0023] In some embodiments, the first sensing module is an inertial sensor; the second sensing module is a wire rope sensor.

[0024] Second aspect, a method for initial positioning of an intelligent surgical robot based on multi-sensor fusion is provided. Using the above-mentioned device for initial positioning of an intelligent surgical robot based on multi-sensor fusion, the following steps are executed, including: the first sensing module is fixed at the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle, so as to collect the quaternion output by the first sensing module; the second sensing module is installed on the intelligent surgical robot body to collect the wire length, and the second sensing module is rotatably connected to the alignment handle; in the case where the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the wire length output by the second sensing module, including the following calculation steps:

[0025] Step S1: Calculate the rotation matrix R from the quaternion:

[0026]

[0027] where (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, w represents the scalar part of the quaternion, that is, the cosine value of the half angle, and R represents the rotation matrix;

[0028] Step S2: Extract the direction vector direction:

[0029]

[0030] where direction represents the direction vector, (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, and w represents the scalar part of the quaternion, that is, the cosine value of the half angle;

[0031] Step S3: Calculate the three-dimensional position coordinates P of the alignment handle connection point:

[0032] P 末端 = P 连接 - d×dircetion,

[0033] where direction represents the direction vector, P 末端represents the three-dimensional position variable of the end of the alignment handle, d represents the length of the alignment handle, P 连接 represents the three-dimensional position variable of the connection point of the alignment handle;

[0034] Step S4: Establish an equation:

[0035]

[0036] where P g represents the three-dimensional position coordinates of the second sensing module, P 连接 represents the three-dimensional position variable of the connection point of the alignment handle, L represents the length of the wire, x 连接 , y 连接 , z connections respectively represent the three-dimensional position coordinates of the connection point of the alignment handle; x g , y g , z g respectively represent the three-dimensional position coordinates of the second sensing module;

[0037] Step S5: The male docking head at the end of the alignment handle aligns with the female docking head to change three postures, and solve the equation in Step S4, where the corresponding direction vector direction after the posture change follows Step S2.

[0038] In a third aspect, the present invention provides an electronic device, which includes: a processor; a memory, on which computer-readable instructions are stored, and when the computer-readable instructions are executed by the processor, the above-mentioned initial positioning method of the intelligent surgical robot based on multi-sensor fusion is realized.

[0039] In a fourth aspect, the present invention further provides a computer-readable storage medium, characterized in that program code is stored in the computer-readable storage medium, and the program code can be called by a processor to execute the above-mentioned initial positioning method of the intelligent surgical robot based on multi-sensor fusion.

[0040] Compared with the prior art, the present invention can at least achieve one of the following beneficial effects:

[0041] First, based on the cooperative positioning technology of multi-sensor fusion, through the data fusion of the inertial sensor and the wire sensor, combined with the attitude information (quaternion) and displacement information (change in wire length), high-precision pose recording is realized.

[0042] Second, the positioning module is used to realize the automatic adjustment of the robot (inverse solution control of position and attitude) to realize positioning, improve efficiency and reduce time consumption.

[0043] The Summary of the Invention section is provided to introduce, in a simplified form, the selection of concepts that will be further described in the Detailed Description below. The Summary of the Invention section is not intended to identify the key features or essential features of the present disclosure, nor is it intended to limit the scope of the present disclosure. Brief Description of the Drawings

[0044] By describing the exemplary embodiments of the present disclosure in more detail in conjunction with the accompanying drawings, the above and other objects, features, and advantages of the present disclosure will become more apparent, wherein, in the exemplary embodiments of the present disclosure, the same reference numerals generally represent the same components.

[0045] Figure 1 The hardware schematic diagram of the initial positioning device of the intelligent surgical robot based on multi-sensor fusion provided by the embodiment of the present application is shown;

[0046] Figure 2 The position parameter schematic diagram of the initial positioning device of the intelligent surgical robot based on multi-sensor fusion provided by the embodiment of the present application is shown. Detailed Description of the Embodiments

[0047] Embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided so that the present disclosure will be more thorough and complete, and will fully convey the scope of the present disclosure to those skilled in the art.

[0048] As used herein, the term "comprising" and its variations mean open-ended inclusion, i.e., "including but not limited to". Unless specifically stated otherwise, the term "or" means "and / or". The term "based on" means "at least partially based on". The terms "an exemplary embodiment" and "an embodiment" mean "at least one exemplary embodiment". The term "another embodiment" means "at least one additional embodiment". The terms "first", "second", etc. may refer to different or the same objects. Other explicit and implicit definitions may also be included hereinafter.

[0049] The present application provides an initial positioning device of an intelligent surgical robot based on multi-sensor fusion. Please refer to Figure 1 , which is a schematic diagram of the first embodiment of the present application. The following will be described in detail with reference to Figure 1 the first embodiment of the present application provides an initial positioning device of an intelligent surgical robot based on multi-sensor fusion.

[0050] The present application provides an initial positioning device of an intelligent surgical robot based on multi-sensor fusion. The processing flow of the device may include the following components: an alignment handle, a first sensing module, a second sensing module, and a positioning module, wherein,

[0051] The first sensing module is fixed to the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle, so as to collect the quaternion output by the first sensing module;

[0052] The second sensing module is installed on the intelligent surgical robot body to collect the wire-pulling length, and the second sensing module is rotatably connected to the alignment handle;

[0053] When the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the wire-pulling length output by the second sensing module.

[0054] This embodiment can, when the alignment handle moves, obtain the quaternion output by the first sensing module and the wire-pulling length output by the second sensing module after the movement, and determine the positioning of the robotic arm of the intelligent surgical robot. Based on the cooperative positioning technology of multi-sensor fusion, through the data fusion of an inertial sensor (Inertial Measurement Unit) and a wire-pulling sensor, combined with attitude information, for example, (x, y, z) represents the vector part, x is the product of the x component of the unit vector of the rotation axis of the vector part and the sine of half the angle, y is the product of the y component of the unit vector of the rotation axis of the vector part and the sine of half the angle, z is the product of the z component of the unit vector of the rotation axis of the vector part and the sine of half the angle, and w represents the four-scalar part, that is, the cosine value of half the angle; thus, (x, y, z, w) constitutes a quaternion; this embodiment can also take into account displacement information, such as: the change in the wire-pulling length, so as to achieve high-precision pose recording.

[0055] When the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion of the first sensing module and the wire-pulling length output by the second sensing module. Using the positioning module to achieve automatic adjustment of the robot (inverse kinematics control of position and attitude) to achieve positioning, improving efficiency and reducing time consumption. As Figure 1 FIG. shows a hardware schematic diagram of an intelligent surgical robot initial positioning device based on multi-sensor fusion provided by an embodiment of the present application.

[0056] In some exemplary embodiments disclosed in the present invention, it further includes: a poke card. When the alignment handle is in the measurement position, the female poke card head is used to dock with the male alignment handle head.

[0057] In some exemplary embodiments disclosed in the present invention, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the wire-pulling length of the second sensing module, including the following calculation steps:

[0058] Step S1: Calculate the rotation matrix R from the quaternion:

[0059]

[0060] where (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, w represents the scalar part of the quaternion, i.e., the cosine value of the half angle, and R represents the rotation matrix;

[0061] Step S2: Extract the direction vector direction:

[0062]

[0063] where direction represents the direction vector, (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, and w represents the scalar part of the quaternion, i.e., the cosine value of the half angle;

[0064] Step S3: Calculate the three-dimensional position coordinates P of the alignment handle connection point:

[0065] P 末端 = P 连接 - d×dircetion,

[0066] where direction represents the direction vector, P 末端 represents the three-dimensional position variable of the end of the alignment handle, d represents the alignment handle length, and P 连接 represents the three-dimensional position variable of the alignment handle connection point;

[0067] Step S4: Establish an equation:

[0068]

[0069] where P g represents the three-dimensional position coordinates of the second sensing module, P连接 represents the three-dimensional position variable of the alignment handle connection point, L represents the length of the pull line, x 连接 ,y 连接 , z 连接 They represent the three-dimensional position coordinates of the connection points of the alignment handles respectively; g ,y g , z g respectively represent the three-dimensional position coordinates of the second sensing module;

[0070] Step S5: The male poking head at the end of the alignment handle is aligned with the female poking head and changes three postures to solve the equation of step S4, wherein the corresponding direction vector direction of the posture after the change complies with the quantity of step S2.

[0071] like Figure 2 As shown, referring to the schematic diagram of the position parameters of the initial positioning device of the intelligent surgical robot based on multi-sensor fusion, only the quaternion (q1, q2) and the pull wire length (L1, L2) measurement data of the inertial sensor at two different postures are shown, combined with the known sensor position P and handle length d. In order to use the simultaneous equations of the rotation matrix and the direction vector, more measurements at different postures can be performed to calculate the target position coordinates P and the posture matrix.

[0072] In some exemplary embodiments disclosed in the present invention, the positioning module is also used to record the determined position of the robotic arm of the intelligent surgical robot as an initial value for the next movement.

[0073] In some exemplary embodiments disclosed in the present invention, the first sensing module is initialized before use so that the zero bias error is less than 0.05°; and the sampling frequency is set to 100 Hz.

[0074] In some exemplary embodiments disclosed in the present invention, the wire pulling range of the second sensing module is 0 to 1000 mm.

[0075] In some exemplary embodiments disclosed in the present invention, the first sensing module is an inertial sensor; and the second sensing module is a wire sensor.

[0076] Another exemplary embodiment of the present invention further provides an initial positioning method for an intelligent surgical robot based on multi-sensor fusion. Using the above-mentioned initial positioning device for an intelligent surgical robot based on multi-sensor fusion, the following steps are performed, including: The first sensing module is fixed to the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle, so as to collect the quaternion output by the first sensing module; The second sensing module is installed on the intelligent surgical robot body to collect the cable length, and the second sensing module is rotatably connected to the alignment handle; When the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the cable length output by the second sensing module.

[0077] An electronic device provided by an embodiment of the present invention may include a processor and a memory. Optionally, the electronic device may further include a transceiver. Among them, the processor is connected to the memory and the transceiver, such as through a communication bus. Computer-readable instructions are stored on the memory, and when the computer-readable instructions are executed by the processor, the steps of the above-mentioned initial positioning method for an intelligent surgical robot based on multi-sensor fusion are implemented.

[0078] In a specific implementation, as an embodiment, the processor may include one or more CPUs.

[0079] In a specific implementation, as an embodiment, the electronic device may also include multiple processors. For example, each of the processors may be a single-core processor (single-CPU) or a multi-core processor (multi-CPU). Here, the processor may refer to one or more devices, circuits, and / or processing cores for processing data (such as computer program instructions).

[0080] Among them, the memory is used to store the software program for implementing the solution of the present invention and is controlled by the processor to execute. The specific implementation manner may refer to the above method embodiment and will not be elaborated here.

[0081] The transceiver is used to communicate with a network device or with a terminal device.

[0082] Optionally, the transceiver may include a receiver and a transmitter. Among them, the receiver is used to implement the receiving function, and the transmitter is used to implement the transmitting function.

[0083] Optionally, the transceiver may be integrated with the processor or exist independently and be coupled to the processor through the interface circuit of the electronic device. The embodiment of the present invention does not make a specific limitation on this.

[0084] It should be noted that the structure of the above electronic device does not limit the electronic device. The actual electronic device may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements. In addition, the technical effects of the electronic device can refer to the technical effects of the above method embodiments and will not be elaborated here.

[0085] In an exemplary embodiment, the present invention further provides a computer-readable storage medium. At least one instruction is stored in the computer-readable storage medium, and the at least one instruction is loaded and executed by a processor to implement the steps of the above method for initial positioning of an intelligent surgical robot based on multi-sensor fusion. For example, the computer-readable storage medium may be a ROM, a random access memory (RAM), a CD-ROM, a magnetic tape, a floppy disk, and an optical data storage device, etc.

[0086] An embodiment of the present invention further provides an electronic device, which includes: a processor; a memory, and computer-readable instructions are stored on the memory. When the computer-readable instructions are executed by the processor, the above method for initial positioning of an intelligent surgical robot based on multi-sensor fusion is implemented.

[0087] An embodiment of the present invention provides a computer-readable storage medium, characterized in that program code is stored in the computer-readable storage medium, and the program code can be called by a processor to execute the above method for initial positioning of an intelligent surgical robot based on multi-sensor fusion.

[0088] It should also be understood that the memory in the embodiments of the present invention can be a volatile memory or a non-volatile memory, or can include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable ROM (PROM), an erasable PROM (EPROM), an electrically erasable PROM (EEPROM), or a flash memory. The volatile memory can be a random access memory (RAM), which is used as an external cache. By way of example but not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchlink DRAM (SLDRAM), and direct rambus RAM (DR RAM).

[0089] It should be understood that the term "and / or" in this article is merely a description of the association relationship of associated objects, indicating that three relationships can exist. For example, A and / or B can represent: A exists alone, A and B exist simultaneously, and B exists alone. Among them, A and B can be singular or plural. In addition, the character " / " in this article generally represents an "or" relationship between the associated objects before and after, but it may also represent an "and / or" relationship, which can be specifically understood with reference to the context before and after.

[0090] It should be understood that in various embodiments of the present invention, the magnitude of the sequence numbers of the above processes does not mean the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present invention.

[0091] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in connection with the embodiments disclosed herein can be implemented in electronic hardware, or in a combination of computer software and electronic hardware. Whether these functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functions in different ways for each specific application, but such implementation should not be considered to exceed the scope of the present invention.

[0092] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the devices, apparatuses, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be described herein again.

[0093] In several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division, and there may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection between each other can be through some interfaces, and the indirect coupling or communication connection of the devices or units can be in electrical, mechanical, or other forms.

[0094] The embodiments of the present disclosure have been described above. The above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations are obvious to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The choice of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements to the technology in the market, or to enable other ordinary skilled artisans in the art to understand the embodiments disclosed herein.

Claims

1. An initial positioning device for an intelligent surgical robot based on multi-sensor fusion, characterized in that, including: alignment handle, first sensing module, second sensing module, positioning module, stamping card, and wherein, the first sensing module is fixed at the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle, so as to collect the quaternion output by the first sensing module; the second sensing module is installed on the intelligent surgical robot body to collect the wire pulling length, and the second sensing module is rotatably connected with the alignment handle; when the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the wire pulling length output by the second sensing module, including the following calculation steps: Step S1: Calculate the rotation matrix R from the quaternion: where, (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, w represents the scalar part of the quaternion, that is, the cosine value of the half angle, and R represents the rotation matrix; Step S2: Extract the direction vector direction: where, direction represents the direction vector, (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the unit vector of the rotation axis of the vector part of the quaternion and the sine of the half angle, w represents the scalar part of the quaternion, that is, the cosine value of the half angle; Step S3: Calculate the three-dimensional position coordinates P of the connection point of the alignment handle; P 末端 = P 连接 -d × direction, wherein, direction represents the direction vector, P 末端 represents the three-dimensional position variable of the end of the alignment handle, d represents the length of the alignment handle, P 连接 represents the three-dimensional position variable of the connection point of the alignment handle; Step S4: Establish an equation; Among them, P g represents the three-dimensional position coordinates of the second sensing module, P 连接 represents the three-dimensional position variable of the alignment handle connection point, L represents the length of the wire, x 连接 , y 连接 , z 连接 respectively represent the three-dimensional position coordinates of the alignment handle connection point; x g , y g , z g respectively represent the three-dimensional position coordinates of the second sensing module; Step S5: The male stamping head at the end of the alignment handle aligns with the female stamping head to change three postures, and solve the equation in Step S4, wherein the corresponding direction vector direction after the posture change complies with Step S2; When the alignment handle is in the measurement position, the female stamping head is used to dock with the male stamping head of the alignment handle.

2. The initial positioning device of the intelligent surgical robot based on multi-sensor fusion according to claim 1, characterized in that, The positioning module is further configured to record the positioning of the robotic arm of the intelligent surgical robot that has been determined as the initial value for the next movement.

3. The initial positioning device of the intelligent surgical robot based on multi-sensor fusion according to claim 2, wherein, The first sensing module performs an initialization operation before use, so that the zero bias error is less than 0.05°; and the sampling frequency is set to 100 Hz.

4. The initial positioning device of an intelligent surgical robot based on multi-sensor fusion according to claim 3, wherein the wire pulling range of the second sensing module is 0 to 1000 mm.

5. The initial positioning device of an intelligent surgical robot based on multi-sensor fusion according to any one of claims 1 to 4, wherein the first sensing module is an inertial sensor; the second sensing module is a wire pulling sensor.

6. An initial positioning method for an intelligent surgical robot based on multi-sensor fusion, characterized in that, Using the initial positioning device of the intelligent surgical robot based on multi-sensor fusion according to any one of claims 1 to 5, perform the following steps, including: The first sensing module is fixed to the end of the alignment handle, and the Z-axis of the first sensing module coincides with the alignment handle, so as to collect the quaternion output by the first sensing module; The second sensing module is installed on the intelligent surgical robot body to collect the wire pulling length, and the second sensing module is rotatably connected to the alignment handle; When the first sensing module and / or the second sensing module moves, the positioning module determines the positioning of the robotic arm of the intelligent surgical robot according to the quaternion output by the first sensing module and the wire pulling length output by the second sensing module, including the following calculation steps: Step S1: Calculate the rotation matrix R from the quaternion: Where, (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the rotation axis unit vector of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the rotation axis unit vector of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the rotation axis unit vector of the vector part of the quaternion and the sine of the half angle, w represents the scalar part of the quaternion, that is, the cosine value of the half angle, and R represents the rotation matrix; Step S2: Extract the direction vector direction: Where, direction represents the direction vector, (x, y, z) represents the vector part of the quaternion, x is the product of the x component of the rotation axis unit vector of the vector part of the quaternion and the sine of the half angle, y is the product of the y component of the rotation axis unit vector of the vector part of the quaternion and the sine of the half angle, z is the product of the z component of the rotation axis unit vector of the vector part of the quaternion and the sine of the half angle, w represents the scalar part of the quaternion, that is, the cosine value of the half angle; Step S3: Calculate the three-dimensional position coordinates P of the connection point of the alignment handle; P 末端 = P 连接 - d × direction, where direction represents the direction vector, P 末端 represents the three-dimensional position variable of the end of the alignment handle, d represents the length of the alignment handle, and P 连接 represents the three-dimensional position variable of the connection point of the alignment handle; Step S4: Establish an equation: Among them, P g represents the three-dimensional position coordinates of the second sensing module, and P 连接 represents the three-dimensional position variable of the alignment handle connection point. L represents the length of the wire rope. x 连接 , y 连接 , and z respectively represent the three-dimensional position coordinates of the alignment handle connection point; x g , y g , and z g respectively represent the three-dimensional position coordinates of the second sensing module; Step S5: The poke card male head at the end of the alignment handle aligns with the poke card female head to change three postures, and solve the equation in Step S4, where the corresponding direction vector direction after the posture change complies with Step S2; When the alignment handle is in the measurement position, the poke card female head is used to dock with the male head of the alignment handle.

7. An electronic device, characterized in that, The electronic device includes: A processor; A memory, on which computer-readable instructions are stored. When the computer-readable instructions are executed by the processor, the initial positioning method of the intelligent surgical robot based on multi-sensor fusion as claimed in claim 6 is implemented.

8. A computer-readable storage medium, characterized in that, Program code is stored in the computer-readable storage medium, and the program code can be called by the processor to execute the initial positioning method of the intelligent surgical robot based on multi-sensor fusion as claimed in claim 6.

Images