Surgical robot and visual module integrated navigation positioning system and method

Abstract

According to the navigation positioning system and method integrating the surgical robot and the visual module, the binocular camera and the base of the mechanical arm are connected through the connecting rod assembly, the position of the binocular camera can be flexibly changed, the risk that target tracking is lost due to shielding in the surgical process is effectively avoided, and the operation efficiency is improved. The encoder is arranged on the hinge point of the connecting rod assembly to assist in calculating the relative position of the free end of the mechanical arm and the binocular camera at the tail end of the connecting rod assembly, and an external parameter matrix of the current position of the camera can be accurately obtained without camera calibration after the position of the binocular camera is changed; the relative position posture relation of the binocular camera and the mechanical arm assembly can be rapidly updated, and the working efficiency of the surgical robot is greatly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of surgical robots, and in particular to a navigation and positioning system and method integrating a surgical robot with a visual module. Background Art

[0002] The surgical robotic navigation and positioning system is an advanced technology used to assist surgical procedures, aiming to improve the precision and accuracy of surgery. The system combines robotics, image processing, and positioning technologies to provide precise positioning and navigation guidance for doctors during surgery.

[0003] Surgical robot navigation and positioning systems also utilize positioning technology to achieve precise navigation. During surgery, the system uses tracking sensors or markers to determine the relative position of the robotic arm and the patient's body surface and internal anatomical structures. By collecting and processing position data in real time, the system calculates the posture and precise positioning of the robotic arm and the surgical target, enabling the surgeon to perform the surgical procedure with pinpoint accuracy.

[0004] However, in existing surgical navigation and positioning systems, the observation camera is generally fixed to facilitate position calibration. Once the camera is blocked and the position and posture information of the tracked target is lost, the camera position cannot be changed to update the information of the tracked target in time, which affects the work of the surgical robot. Summary of the Invention

[0005] In response to the above problems, the present invention proposes a new and more optimized navigation and positioning system and method integrating a surgical robot and a vision module.

[0006] The present invention is implemented by the following technical solutions: The present invention provides a navigation and positioning system integrating a surgical robot and a vision module, comprising: a binocular camera, a robotic arm assembly, a connecting rod assembly, a calibration plate, and a planar target. The robotic arm assembly includes a fixed base and a robotic arm capable of controlled movement. One end of the connecting rod assembly is rotatably connected to the base of the robotic arm assembly, and the other end of the connecting rod assembly is rotatably connected to the binocular camera. The robotic arm has at least a movable telescopic range that can move relatively close to or away from the base of the robotic arm assembly. The calibration plate is mounted at the free end of the robotic arm assembly, and the planar target is fixedly arranged in front of the binocular camera. The connecting rod assembly includes at least N connecting rods hinged in sequence, with a total of N+1 hinges formed at both ends of the connecting rod assembly and between every two adjacent connecting rods, where N is greater than or equal to 2. A rotation damping structure is provided at each hinge point, and an encoder is installed at each hinge point. The encoder's encoding information is used to calculate the relative position of the head and tail ends of the connecting rod assembly when the connecting rod assembly undergoes a rotational posture change, and the relative position of the free end and the binocular camera at the end of the connecting rod assembly is calculated with the help of the relative position of the head and tail ends of the connecting rod assembly.

[0007] The present invention also provides a navigation and positioning method for an integrated surgical robot and vision module, which is used to determine the relative position of a binocular camera at the free end of a robotic arm assembly and the end of a connecting rod assembly in the navigation and positioning system for an integrated surgical robot and vision module, comprising: Step S1, construct the rotation matrix T = T1×T2×···×T n To calculate the relative position of the connecting rod assembly at both ends, T n Represents the rotation matrix constructed by the position and posture of the next hinge point compared to the previous hinge point. The rotation matrix T n The calculation formula is:

[0008] Among them, parameter a is the distance between the previous and current hinge points along the X direction; parameter d is the distance between the previous and current hinge points along the Z direction; parameter α is the angle between the previous and current hinge points along the X direction; parameter θ is the rotation angle of the current hinge point around the Z axis. Parameter θ is calculated in real time by the encoder. The remaining three parameters are all determined during the design of the connecting rod assembly. The X direction and Z direction are the horizontal and vertical directions, respectively. Step S2, calculating the relative position and posture relationship between the free end of the robotic arm assembly and the binocular camera at the end of the connecting rod assembly, including: S21, the binocular camera takes pictures of the calibration plate at different positions, and obtains at least 15 sets of complete pictures of the calibration plate; S22, using Zhang's calibration algorithm ("A Flexible New Technique for Camera Calibration") to identify and extract corner points from an image of the calibration plate taken by the binocular camera. Then, the intrinsic and extrinsic parameters of each camera module are calculated. Using epipolar geometric constraints, the coordinate transformation relationship between the two camera modules of the binocular camera is solved to obtain the intrinsic parameters of the two camera modules, as well as the radial and tangential distortion coefficients. The intrinsic parameters include the camera focal length and the image center coordinates. The intrinsic parameters can be represented by a 3×4 matrix A. S23, using the camera distortion coefficient obtained after calibration to correct the distortion of the calibration plate image to obtain a new calibration plate image, and then using the corner detection algorithm to extract the position information of the corner points on the new calibration plate image, and calculate the three-dimensional spatial position of the corner points relative to the camera module coordinate system using the following formula, where and Indicates the horizontal and vertical coordinate values ​​of the corner pixel, A represents the camera module memory matrix, Represents the rotation and translation vectors of the camera module coordinate system and the calibration plate coordinate system, P is the three-dimensional space coordinate value of the corner point, Q represents the product of the internal parameter matrix and the external parameter matrix of the camera module, and the subscript and Represent the relevant parameters of the left camera module and the right camera module respectively. The subscript w indicates that the variable is based on the world coordinate system:

[0009] Then the above formula exists where the cross product of the left and right terms is equal to the zero vector:

[0010] Then the i-th row of the matrix Q of the above formula is recorded as , then the above formula can be obtained:

[0011] S24, by the matrix A T A is subjected to singular value decomposition, and the eigenvector of the minimum eigenvalue obtained is the three-dimensional space coordinate value of the corner point in the world coordinate system, including: S241, obtaining the three-dimensional spatial coordinate values ​​of all corner points on the calibration plate through the calculation method of S21-S23, and optimizing the position and posture of the calibration plate relative to the camera module coordinate system through the PnP algorithm; S242, in the hand-eye calibration unit, the six-axis encoding information of the robotic arm assembly corresponding to the calibration plate in the above-mentioned new calibration plate image is solved by calculating the position and posture transformation relationship of the calibration plate relative to the base of the robotic arm assembly through the rotation matrix T, wherein, through the position and posture relationship of the calibration plate relative to the camera module coordinate system and the position and posture transformation relationship of the calibration plate relative to the base of the robotic arm assembly, the calibration algorithm "Robot sensor calibration: solving AX=XB on the Euclidean group" is used to obtain the relative coordinate information of the end of the connecting rod assembly and the binocular camera, and the position and posture of the calibration plate relative to the base of the robotic arm assembly and the position and posture of the binocular camera relative to the base of the robotic arm assembly are obtained through the arithmetic operation of matrix inversion. As a result, all coordinate systems are converted into coordinate systems relative to the base of the robotic arm assembly.

[0012] The present invention has the following beneficial effects: the present invention can flexibly change the position of the binocular camera through the connecting rod assembly, effectively avoiding the risk of losing target tracking due to occlusion during surgery, and changing the position of the binocular camera does not require re-calibration of the camera to accurately obtain the external parameter matrix of the camera's current position, and can quickly update the relative position and posture relationship between the binocular camera and the robotic arm assembly, greatly improving the working efficiency of the surgical robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 Schematic diagram of the navigation and positioning system integrating the surgical robot and the vision module in the embodiment. DETAILED DESCRIPTION

[0014] To further illustrate various embodiments, the present invention is provided with accompanying drawings. These drawings form part of the present disclosure and are primarily used to illustrate the embodiments and, in conjunction with the relevant description in the specification, to explain the operating principles of the embodiments. By referring to these drawings, one of ordinary skill in the art will understand other possible embodiments and the advantages of the present invention. The components in the figures are not drawn to scale, and similar reference numerals are generally used to represent similar components.

[0015] The present invention will now be further described with reference to the accompanying drawings and specific embodiments.

[0016] See Figure 1As shown, as a preferred embodiment of the present invention, a navigation and positioning system integrating a surgical robot and a visual module is provided, comprising: a binocular camera 1, a robotic arm assembly 2, a connecting rod assembly 3, a calibration plate 4 and a plane target 5. The robotic arm assembly 2 comprises a fixed base 20 and a free end 21 capable of changing position through multi-axis movement of the robotic arm. One end of the connecting rod assembly 3 is rotatably connected to the base 20, and the other end of the connecting rod assembly 3 is rotatably connected to the binocular camera 1. The calibration plate 4 is mounted on the free end 21, so that when the free end 21 is controlled to move and change position, the distance between the binocular camera 1 and the calibration plate 4 changes, and the binocular camera 1 can take pictures of the calibration plate 4 at different distances. The plane target 5 is fixedly set in front of the binocular camera 1 as a simulated tracking target to estimate the relative position and posture relationship between the patient and the robotic arm assembly 2.

[0017] The connecting rod assembly 3 includes at least N connecting rods that are hingedly connected in sequence. Thus, a total of N+1 hinge points are formed at both ends of the connecting rod assembly 3 and between every two adjacent connecting rods, where N is greater than or equal to 2. For example, in this embodiment, the connecting rod assembly 3 includes two connecting rods. The ends of the two connecting rods that are separated from each other (i.e., the front and rear ends of the entire connecting rod assembly 3) and the ends at which the two connecting rods are hinged together form a total of three hinge points. Each hinge point is provided with a rotational damping structure, such as one implemented using oil damping, magnetic damping, or the like. This allows any connecting rod of the connecting rod assembly 3 or the binocular camera 1 to overcome the damping force of the rotational damping structure and rotate about the hinge point. After the external force is removed, each connecting rod or binocular camera 1 is locked in its adjusted position and remains substantially stationary due to the effect of the rotational damping structure.

[0018] Through the setting of the connecting rod assembly 3, the position of the binocular camera 1 can be adjusted by deflecting the binocular camera 1 and the angle of the connecting rod in the connecting rod assembly 3, thereby preventing the binocular camera 1 from being blocked.

[0019] Once the position of the binocular camera 1 serving as the visual module changes, it usually means that the spatial positioning of the surgical robot navigation system needs to be recalibrated, which has a great impact on the working efficiency of the surgical robot navigation system. In this embodiment, the following navigation and positioning method integrating the surgical robot and the visual module is further adopted to solve this problem.

[0020] [Step S1] Construct a rotation matrix to calculate the relative positions of the head and tail ends of the connecting rod assembly 3. An encoder 31 is installed at each hinge point of the connecting rod assembly 3. Therefore, when any link of the connecting rod assembly 3 undergoes a rotation posture change, the relative positions of the head and tail ends of the connecting rod assembly 3 can be calculated in real time by reading the encoding information of the encoder 31 and combining it with the length of each link. Furthermore, this embodiment calculates the relative positions of the head and tail ends of the connecting rod assembly 3 by constructing a rotation matrix. Taking the connecting rod assembly 3 with two links in this embodiment as an example, the hinge point where the connecting rod assembly 3 is connected to the base of the robotic arm assembly 2 is the first hinge point, the hinge point between the two links on the connecting rod assembly 3 is the second hinge point, and the hinge point where the connecting rod assembly 3 is connected to the binocular camera 1 is the third hinge point: The rotation matrix is ​​constructed by setting the four parameters to calculate the relative position and posture of the next hinge point relative to the previous hinge point (the direction closer to the base of the robot assembly 2 is the front). The rotation matrix T n The calculation formula is:

[0021] Among them, parameter a is the distance between the coordinate origin of the previous hinge point and the coordinate origin of the current hinge point along the X-axis direction of the previous hinge point triangular coordinate system; parameter d is the distance between the coordinate origin of the previous hinge point and the coordinate origin of the current hinge point along the Z-axis direction of the previous hinge point triangular coordinate system; parameter α is the angle between the previous hinge point and the Z-axis of the current hinge point along the X-axis direction of the previous hinge point triangular coordinate system; parameter θ is the angle between the previous hinge point and the X-axis of the current hinge point along the Z-axis direction of the previous hinge point triangular coordinate system. Parameter θ is obtained by real-time calculation by encoder 31, and the remaining three parameters are all confirmed when the connecting rod assembly 3 is designed. It should be noted that the X-axis direction and Z-axis direction of the triangular coordinate system at the hinge point are clearly understood by those skilled in the art.

[0022] In this embodiment, the position and posture of the second hinge relative to the first hinge and the position and posture of the third hinge relative to the second hinge can be respectively constructed into two rotation matrices through the above-mentioned rotation matrix, and the two rotation matrices are strung together through matrix multiplication operation, that is, T=T1×T2, where the right subscript numbers are used to distinguish different rotation matrices. It can be understood that if the number of hinges of the connecting rod assembly 3 is set to be more, more multiplied rotation matrices T=T1×T2×···×T can also be continuously constructed in the above-mentioned manner. n .

[0023] [Step S2] Calculate the relative position and attitude relationship between the free end 21 and the end of the connecting rod assembly 3. Since the connecting rod assembly 3 is mounted on the robotic arm assembly 2, the relative position and attitude relationship between the free end 21 (or the calibration plate 4 mounted on the free end 21) and the end of the connecting rod assembly 3 (or the binocular camera 1 mounted on the end of the connecting rod assembly 3) can be quickly obtained by combining the size of the connecting rod assembly 3 and its mounting position on the robotic arm assembly 2 with the above-mentioned rotation matrix method. The method is as follows: S21, the binocular camera 1 takes pictures of the calibration plate 4 at different positions to obtain at least 15 sets of complete pictures of the calibration plate.

[0024] S22 uses the Zhang calibration algorithm ("A Flexible New Technique for Camera Calibration") to identify and extract corners from the captured image. The intrinsic and extrinsic parameters of each camera are then calculated. Using epipolar geometric constraints, the coordinate transformation relationship between the two camera modules of binocular camera 1 (binocular camera 1 includes two camera modules, one on the left and one on the right) is calculated. The Zhang calibration algorithm and epipolar constraints allow for the rapid determination of the intrinsic parameters of the two camera modules, as well as the radial and tangential distortion coefficients. The intrinsic parameters include the camera focal length and the image center coordinates. These intrinsic parameters can be represented by a 3×4 matrix A.

[0025] S23, using the camera distortion coefficients obtained after calibration to correct the distortion of the 15 sets of images to obtain 15 new sets of images, and then using a corner detection algorithm to extract the position information of the corner points in the 15 new sets of images described in this article. The three-dimensional spatial position of the corner points relative to the camera module coordinate system is calculated using the following formula, where u and v represent the horizontal and vertical coordinate values ​​of the corner point pixels, and A represents the camera module memory matrix. represents the rotation and translation vectors of the camera module coordinate system and the calibration plate 4 coordinate system, P is the three-dimensional space coordinate value of the corner point, Q represents the product of the internal parameter matrix and the external parameter matrix of the camera module, and the subscript and Represent the relevant parameters of the left camera module and the right camera module respectively. The subscript w indicates that the variable is based on the world coordinate system:

[0026] Then the above formula exists where the cross product of the left and right terms is equal to the zero vector:

[0027] Then the i-th row of the matrix Q of the above formula is recorded as , then the above formula can be obtained:

[0028] S24, by the matrix A T A is subjected to singular value decomposition, and the eigenvector of the minimum eigenvalue obtained is the three-dimensional space coordinate value of the corner point in the world coordinate system, where: S241, obtain the three-dimensional spatial coordinate values ​​of all corner points on the calibration plate 4 through the above-mentioned calculation method, and optimize the position and posture of the calibration plate 4 relative to the camera module coordinate system through a PnP algorithm, such as the SRPnP algorithm "A simple, robust and fast method for the perspective-n-point problem".

[0029] S242: In the hand-eye calibration unit, the six-axis encoding information of the robotic arm assembly 2 corresponding to the calibration plate 4 in the above-mentioned 15 new sets of images is calculated using the rotation matrix T to solve the position and posture transformation relationship of the calibration plate 4 relative to the base of the robotic arm assembly 2. Specifically, the relative coordinate information of the end of the connecting rod assembly 3 and the binocular camera 1 is obtained using the calibration algorithm "Robot sensor calibration: solving AX=XB on the Euclidean group" through the position and posture relationship of the calibration plate 4 relative to the camera module coordinate system and the position and posture transformation relationship of the calibration plate 4 relative to the base of the robotic arm assembly 2. Specifically, the position and posture of the calibration plate 4 relative to the base of the robotic arm assembly 2 and the position and posture of the binocular camera 1 relative to the base of the robotic arm assembly 2 are obtained through the arithmetic operation of matrix inversion. As a result, all coordinate systems are converted into a coordinate system relative to the base of the robotic arm assembly 2.

[0030] During implementation, the link assembly 3 allows for flexible adjustment of the binocular camera 1's position, effectively avoiding the risk of losing tracking of the planar target 5 due to occlusion during surgery. Furthermore, changing the binocular camera 1's position eliminates the need for recalibration to accurately obtain the extrinsic parameter matrix of the camera's current position. This allows for rapid updating of the relative position and posture relationship between the binocular camera 1 and the robotic arm assembly 2, significantly improving the surgical robot's operating efficiency. The formula for calculating the coordinate transformation relationship between the planar target 5 and the binocular camera 1 can be obtained via step S23.

[0031] The navigation and positioning method for integrating the surgical robot and the visual module in this embodiment uses matrix operations, which has simpler formulas and faster calculation speeds while also ensuring accuracy.

[0032] The Zhang calibration algorithm "A Flexible New Technique for Camera Calibration", the SRPnP algorithm "A simple, robust and fast method for the perspective-n-point problem", and the calibration algorithm "Robot sensor calibration: solving AX=XB on the Euclidean group" mentioned in this embodiment are all mature existing algorithms in this field.

[0033] In addition, although the present invention has been specifically shown and described in conjunction with preferred embodiments, it should be understood by those skilled in the art that various changes in form and details made to the present invention without departing from the spirit and scope of the present invention as defined by the appended claims fall within the scope of protection of the present invention.

Claims

1. A navigation and positioning system integrating a surgical robot and a visual module, characterized in that: include: A binocular camera, a robotic arm assembly, a connecting rod assembly, a calibration plate, and a plane target. The robotic arm assembly includes a fixed base and a free end that can change position through multi-axis motion of the robotic arm. One end of the connecting rod assembly is rotatably connected to the base, and the other end of the connecting rod assembly is rotatably connected to the binocular camera. The calibration plate is installed at the free end, and the plane target is fixedly set in front of the binocular camera. The connecting rod assembly includes at least N connecting rods hinged in sequence, with a total of N+1 hinges formed at both ends of the connecting rod assembly and between every two adjacent connecting rods, where N is greater than or equal to 2. A rotation damping structure is provided at each hinge point, and an encoder is also installed at each hinge point. The encoder's encoding information is used to calculate the relative positions of the head and tail ends of the connecting rod assembly when the connecting rod assembly undergoes a rotational posture change, and the relative positions of the free end and the binocular camera at the end of the connecting rod assembly are calculated with the help of the relative positions of the head and tail ends of the connecting rod assembly.

2. A navigation and positioning method integrating a surgical robot and a visual module, characterized in that: The method for calculating the relative position of the binocular camera at the free end of the manipulator assembly and the end of the connecting rod assembly in the navigation and positioning system integrating the surgical robot and the visual module of claim 1 comprises: Step S1, construct the rotation matrix T = T1×T2×···×T n To calculate the relative position of the connecting rod assembly at both ends, T n Represents the rotation matrix constructed by the position and posture of the latter hinge point compared to the previous hinge point. The rotation matrix T n The calculation formula is: Among them, parameter a is the distance between the coordinate origin of the previous hinge point and the coordinate origin of the current hinge point along the X-axis direction of the previous hinge point triangular coordinate system; parameter d is the distance between the coordinate origin of the previous hinge point and the coordinate origin of the current hinge point along the Z-axis direction of the previous hinge point triangular coordinate system; parameter α is the angle between the previous hinge point and the Z-axis of the current hinge point along the X-axis direction of the previous hinge point triangular coordinate system; parameter θ is the angle between the previous hinge point and the X-axis of the current hinge point along the Z-axis direction of the previous hinge point triangular coordinate system; parameter θ is obtained by real-time calculation of the encoder, and the other three parameters are all confirmed when the connecting rod assembly is designed; Step S2, calculating the relative position and posture relationship between the free end of the robotic arm assembly and the binocular camera at the end of the connecting rod assembly, including: S21, the binocular camera takes pictures of the calibration plate at different positions, and obtains at least 15 sets of complete pictures of the calibration plate; S22, using Zhang's calibration algorithm ("A Flexible New Technique for Camera Calibration") to identify and extract corner points from an image of the calibration plate taken by the binocular camera. Then, the intrinsic and extrinsic parameters of each camera module are calculated. Using epipolar geometric constraints, the coordinate transformation relationship between the two camera modules of the binocular camera is solved to obtain the intrinsic parameters of the two camera modules, as well as the radial and tangential distortion coefficients. The intrinsic parameters include the camera focal length and the image center coordinates. The internal parameters can be represented by a 3×4 matrix A. S23, using the camera distortion coefficients obtained after calibration to correct the distortion of the calibration plate image to obtain a new calibration plate image, and then using a corner detection algorithm to extract the position information of the corner points on the new calibration plate image. The three-dimensional spatial position of the corner points relative to the camera module coordinate system is calculated using the following formula, where u and v represent the horizontal and vertical coordinate values ​​of the corner point pixels, and A represents the camera module memory matrix. Represents the rotation and translation vectors of the camera module coordinate system and the calibration plate coordinate system, P is the three-dimensional space coordinate value of the corner point, Q represents the product of the internal parameter matrix and the external parameter matrix of the camera module, and the subscript and Represent the relevant parameters of the left camera module and the right camera module respectively. The subscript w indicates that the variable is based on the world coordinate system: Then the above formula exists where the cross product of the left and right terms is equal to the zero vector: Then the i-th row of the matrix Q of the above formula is recorded as , then the above formula can be obtained: S24, by the matrix A T A is subjected to singular value decomposition, and the eigenvector of the minimum eigenvalue obtained is the three-dimensional space coordinate value of the corner point in the world coordinate system, including: S241, obtaining the three-dimensional spatial coordinate values ​​of all corner points on the calibration plate through the calculation method of S21-S23, and optimizing the position and posture of the calibration plate relative to the camera module coordinate system through the PnP algorithm; S242, in the hand-eye calibration unit, the six-axis encoding information of the robotic arm assembly corresponding to the calibration plate in the above-mentioned new calibration plate image is solved by calculating the position and posture transformation relationship of the calibration plate relative to the base of the robotic arm assembly through the rotation matrix T, wherein, through the position and posture relationship of the calibration plate relative to the camera module coordinate system and the position and posture transformation relationship of the calibration plate relative to the base of the robotic arm assembly, the calibration algorithm "Robot sensor calibration: solving AX=XB on the Euclidean group" is used to obtain the relative coordinate information of the end of the connecting rod assembly and the binocular camera, and the position and posture of the calibration plate relative to the base of the robotic arm assembly and the position and posture of the binocular camera relative to the base of the robotic arm assembly are obtained through the arithmetic operation of matrix inversion. As a result, all coordinate systems are converted into coordinate systems relative to the base of the robotic arm assembly.

Images