Interventional surgical robot simultaneous localization and mapping method and system

Abstract

The application discloses an interventional surgery robot synchronous positioning and three-dimensional map construction method and system. The system comprises a catheter assembly, an ultrasonic sensor and an electromagnetic positioning coil arranged at the front end inside the catheter assembly; an electromagnetic shielding layer is arranged between the ultrasonic sensor and the electromagnetic positioning coil; further comprising a proximal end driving module and an ultrasonic imaging system of the ultrasonic sensor, an electromagnetic positioning system matched with the electromagnetic positioning coil, and a synchronous positioning and mapping system (SLAM) for data processing; the SLAM acquires the 2D image of intravascular ultrasound and the spatial position information of the electromagnetic positioning coil, and performs the synchronous positioning and three-dimensional map construction of the interventional surgery robot. The application effectively combines intravascular ultrasound, an electromagnetic positioning system and a SLAM, can realize the intravascular autonomous positioning and navigation of the vascular interventional surgery robot, improves the navigation and position sensing capability of the robot in the interventional surgery, and effectively improves the accuracy and operation effect of the surgery.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent medical devices, and more particularly to an interventional surgery robot simultaneous positioning and three-dimensional map construction method and system. BACKGROUND

[0002] Vascular interventional surgery refers to a way of diagnosing and treating intravascular lesions by puncturing blood vessels and using instruments such as guide wires and catheters to enter the body. It has the advantages of precise operation, short operation time, small operation trauma, short postoperative recovery time, and less pain suffered by patients, and belongs to a kind of minimally invasive surgery. The vascular interventional surgery robot mainly performs catheter advancement and navigation in vascular interventional surgery. At present, the navigation technology of the interventional surgery robot mostly obtains the position state information of the catheter through extracorporeal imaging or spatial coordinate registration. These technologies usually have poor immediacy and low accuracy. The intravascular position state information has the characteristics of being more accurate, more immediate and more direct. However, the state information provided by the intravascular information of the catheter has not been used in the navigation of the interventional surgery robot.

[0003] Intravenous ultrasound (IVUS) refers to a medical imaging technology that combines non-invasive ultrasound technology with invasive catheter technology, using a special catheter with an ultrasonic probe at the tip. IVUS uses ultrasonic principles to detect the structure of the blood vessel wall and the surrounding tissue, and can be used to guide vascular interventional treatment, and is known as the "X-ray eyes" of the doctor in cardiovascular intervention.

[0004] The simultaneous localization and mapping (SLAM) system is mainly used to solve the positioning and mapping problems of robots moving in unknown environments. According to the sensors carried by the mobile robot, SLAM can be divided into laser SLAM and visual SLAM. Among them, visual SLAM uses a camera to obtain picture information of the scene, thereby completing the perception of the environment.

[0005] Beatriz Farola Barata et al. proposed an IVUS-based local vessel estimation method for robotic intravascular navigation (IEEE Robotics and Automation Letters, 2021), which approximates the blood vessel geometry near the catheter tip by a cylindrical model, and realizes the obstacle avoidance function of the vascular interventional robot through shape estimation. However, this research has not realized the positioning and mapping in the blood vessel using the imaging information of IVUS. SUMMARY

[0006] In view of the above defects in the prior art, the present application provides a method and system for SLAM of a vascular interventional surgery robot using IVUS, which can improve the autonomous positioning and navigation performance of the vascular interventional surgery robot by combining electromagnetic positioning, and is helpful for the development of intelligent medical robots.

[0007] To achieve the above object, in one aspect, the present application provides a method for simultaneous localization and three-dimensional map construction of an interventional surgery robot, characterized in that it comprises the following steps:

[0008] Step S1, the simultaneous localization and mapping system obtains intravascular ultrasound images as input frames by an ultrasound probe in the interventional surgery robot, and obtains map points;

[0009] Step S2, the input frames are preprocessed;

[0010] Step S3, an ORB feature point extraction algorithm is used to extract feature points;

[0011] Step S4, the pose of the ultrasound probe is estimated by a motion model;

[0012] Step S5, the optimal feature points in the region are retained by a non-maximum suppression algorithm, and multiple feature points at adjacent positions are eliminated;

[0013] Step S6, the simultaneous localization and mapping system obtains the depth of the image by an electromagnetic positioning coil in the interventional surgery robot, and generates three-dimensional map point information corresponding to the optimal feature points in combination with the feature points of the input frames; each map point information comprises 3D coordinates in a world coordinate system and a view direction; the view direction refers to the direction of the ray connecting the point cloud and the optical center of the corresponding observation key frame;

[0014] Step S7, the ORB feature points of each new image are extracted by an algorithm for real-time tracking, and compared with the nearest key frame to calculate the position of the feature points and roughly estimate the camera pose, which is used to determine whether the current frame needs to be added to the key frame;

[0015] Step S8, local map construction: according to the added key frame, a local map is constructed, and the local Bundle Adjustment is minimized to solve the re-projection error according to the feature points and camera pose in the local space, so as to obtain more precise camera pose and feature point spatial position, and meanwhile, the redundant key frames whose map points can be observed by other key frames are eliminated;

[0016] Step S8, the pose of the ultrasound probe is estimated by a motion model;

[0017] Step S9, using Kalman filtering method to observe and estimate the movement of the robot;

[0018] Step S10, constantly updating the map and position to realize the simultaneous localization and mapping of the robot.

[0019] The simultaneous localization and three-dimensional map construction method effectively combines IVUS, electromagnetic positioning system and SLAM, can realize the intravascular autonomous positioning and navigation of the vascular interventional surgery robot, improves the navigation and position sensing ability of the robot in the interventional surgery, and effectively improves the accuracy and surgery effect of the surgery.

[0020] Further, the simultaneous localization and mapping system does not introduce a loop detection module for detecting the repetition of the robot motion path.

[0021] Further, the preprocessing in step S2 includes converting the ultrasound image into a grayscale image.

[0022] Further, the ORB feature point extraction algorithm in step S3 includes the FAST algorithm. This feature makes the simultaneous localization and mapping system have the characteristics of fast calculation speed and high efficiency, which can effectively improve the running efficiency of the system.

[0023] Further, in step S5, the following information is saved for each key frame Ki: camera pose T(i,w), transformation matrix from the world coordinate system to the camera coordinate system, and camera parameters including principal point and focal length.

[0024] Further, the motion model in step S8 is the PnP algorithm.

[0025] On the other hand, the application also provides a simultaneous localization and three-dimensional map construction system of an interventional surgery robot, which implements the simultaneous localization and three-dimensional map construction method of the interventional surgery robot, and is characterized by comprising: a catheter assembly, an ultrasonic sensor and an electromagnetic positioning coil arranged at the front end inside the catheter assembly; an electromagnetic shielding layer is arranged between the ultrasonic sensor and the electromagnetic positioning coil.

[0026] It also includes an extracorporeal part of the operated body, which includes: a proximal end driving module and an ultrasonic imaging system of the ultrasonic sensor, an electromagnetic positioning system matched with the electromagnetic positioning coil, and a synchronous localization and mapping system for data processing; the synchronous localization and mapping system acquires the 2D image of the intravascular ultrasound and the spatial position information of the electromagnetic positioning coil, and performs the simultaneous localization and three-dimensional map construction of the interventional surgery robot.

[0027] Further, the electromagnetic positioning system selects NDI Aurora V3 series medical magnetic field generator.

[0028] Further, the electromagnetic shielding layer comprises two layers of separation layers between the fixing device of the electromagnetic positioning coil and the ultrasonic sensor and a shielding body between the separation layers.

[0029] Further, the separation layers are in the form of thin sheets made of electromagnetic wave-absorbing material; the shielding body is made of low-resistance metal material, and the shielding body is grounded.

[0030] Compared with the prior art, the above-mentioned application has the following advantages or beneficial effects:

[0031] 1. The IVUS, electromagnetic positioning system and SLAM are effectively combined to realize intravascular autonomous positioning and navigation of the vascular interventional surgery robot, improve the navigation and position sensing capability of the robot in the interventional surgery, and effectively improve the accuracy and surgery effect of the surgery.

[0032] 2. The electromagnetic positioning system adds spatial scale to the monocular IVUS image, making up for the lack of spatial scale in the traditional monocular SLAM.

[0033] 3. The position information image is obtained by intravascular ultrasound, reducing the dependence on X-ray imaging and contrast agent and reducing the radiation dose of doctors and patients.

[0034] 4. The synchronous positioning and three-dimensional map construction system is reasonably designed, and the electromagnetic shielding layer is used to reduce the interference of electromagnetic signals on the ultrasonic sensor as much as possible. BRIEF DESCRIPTION OF DRAWINGS

[0035] The present application and its features, shapes and advantages will become more apparent by reading the following detailed description of non-limiting embodiments, made with reference to the accompanying drawings. In all the drawings, the same references indicate the same parts. The drawings are not necessarily drawn to scale, the emphasis being on illustrating the principle of the application.

[0036] Figure 1 Flow chart of the synchronous positioning and three-dimensional map construction method in an embodiment of the present application;

[0037] Figure 2 Processing flow chart of the SLAM system in an embodiment of the present application;

[0038] Figure 3 End layout sectional view of the interventional surgery catheter in an embodiment of the present application;

[0039] Figure 4 Architecture diagram of the synchronous positioning and three-dimensional map construction system in an embodiment of the present application;

[0040] In which, 1, catheter; 2, ultrasonic sensor; 3, electromagnetic positioning coil fixing position; 4, separation layer; 5, shielding body. DETAILED DESCRIPTION

[0041] The structure and method in the present application will be further described below in conjunction with the accompanying drawings and specific embodiments. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. It is understood that detailed description of well-known devices, algorithms, circuits and methods is omitted so as not to obscure the description of the present application with unnecessary detail.

[0042] Embodiment 1

[0043] The embodiment provides an interventional robot synchronization positioning and map construction method based on intravascular ultrasound and electromagnetic positioning, and can realize autonomous positioning and navigation of a vascular interventional robot. For a specific process, refer to Figure 1 The image information obtained by the system is derived from an intravascular ultrasound sensor at the head of the interventional robot, and the sensor can obtain and transmit intravascular ultrasound images. The spatial position information is derived from an electromagnetic positioning system matched with the interventional robot, and the electromagnetic positioning system can obtain and transmit spatial coordinate information of a specific point on the interventional robot. The difference between coordinates at adjacent moments can be used as the depth of the image for the mapping part in the SLAM system. The information obtained by the two sensors is the information of the head of the catheter part of the interventional robot, which is helpful for guiding the movement of the robot in the human body blood vessel. After the image and position information are obtained, they are transmitted into the SLAM system. The specific execution process of the SLAM module is shown in Figure 2 First, the SLAM system pre-processes the input frame, and the pre-processing includes converting the ultrasound image into a gray image. At the same time, the spatial coordinate information is also converted into depth distance information through calculation.

[0044] Then, the FAST algorithm in the ORB feature point extraction algorithm is used for the ultrasound image after pre-processing. The FAST corner point is defined as: if a pixel point and enough pixel points in its surrounding field are in different areas, the pixel point may be a corner point. In the gray image used in the embodiment, if the gray value of the point is greater or less than the gray value of enough pixel points in its surrounding field, the point may be a corner point.

[0045] Specific steps are as follows:

[0046] 1. Select a pixel P from the picture. We will judge whether it is a feature point below. We first set its degree value as lp;

[0047] 2. Set a suitable threshold t;

[0048] 3. Consider a discrete Bresenham circle with a radius equal to 3 pixels and taking the pixel point as the center. There are 16 pixels on the boundary of the circle;

[0049] 4. If there are n consecutive pixels on this 16-pixel size circle, their pixel values are either all greater than lp+t or all less than lp-t, then it is a corner point. Usually n = 9 is the best.

[0050] After that, the non-maximum suppression algorithm (NMS) is used to retain the optimal feature points while eliminating the feature points at adjacent positions, thereby reducing the data amount of operation. The specific process is that a 3*3 window is taken on the image, if there are multiple feature points in the window, according to the size of the response value of the FAST feature point, the feature points with smaller response values are deleted, and only the feature points with the largest response value are retained, so as to achieve the purpose of screening feature points. At the same time, the pose of the IVUS sensor is estimated through the motion model, the motion model used is the PnP algorithm, according to the camera model, the relationship between the spatial point position and the pixel position can be established, considering n three-dimensional space points P and its projection p, the pose R, t of the camera is calculated. Its Lie group is represented as T. Use i to represent the i-th pixel point, and assume that the coordinates of a space point P i = [X i , Y i , Z i ] T , the projected pixel coordinates are u i = [u i , v i ] T , the following formula can be obtained:

[0051]

[0052] where S i is the camera intrinsic parameter matrix, which is related to the specific parameters of the camera.

[0053] After that, the spatial position information is combined with the ultrasound image, and the three-dimensional map information corresponding to the optimal feature points can be generated. That is, the simultaneous localization and mapping system obtains the depth of the image through the electromagnetic positioning coil in the surgical robot, and generates the three-dimensional map information corresponding to the optimal feature points in combination with the feature points of the input frame. Each map point has the following information: 3D coordinates in the world coordinate system, view direction, i.e. the average unit vector of all view directions (the direction refers to the direction of the ray connecting the point cloud and the optical center of the corresponding observation key frame); the following information is saved for each key frame Ki: camera pose T(i, w), transformation matrix from world coordinate system to camera coordinate system, camera parameters including principal point and focal length.

[0054] Specifically, the coordinates of the camera in the world coordinate system W (x w , y w , z w ) can be obtained by the electromagnetic positioning sensor; the position difference of the camera at adjacent time points is: The pixel point space coordinate compensated by the position difference is:

[0055] We can obtain the specific position of the feature point i in the world coordinate system and its change rule through the accumulation of the position difference compensation and the vector set composed of multiple compensated space coordinates.

[0056] Then, the ORB feature points of each new image are extracted for real-time tracking, and compared with the nearest key frame to calculate the position of the feature points and roughly estimate the camera pose. The current frame is used to determine whether it needs to be added to the key frame.

[0057] Then, combined with the motion model estimated ultrasonic sensor pose, according to the added key frame, a local map is further constructed, and according to the feature points and camera pose in the local space, the local Bundle Adjustment is solved to minimize the re-projection error, so as to obtain a more detailed camera pose and feature point space position, and to eliminate the redundant key frame that can be observed by other key frames.

[0058] Then, the Kalman filter method is used to observe and estimate the movement, thereby realizing the simultaneous localization and mapping of the robot.

[0059] It should be noted that the path through which the vascular interventional robot passes in the blood vessel usually does not repeat, so the loop detection module for detecting the repetition of the motion path of the robot does not have to be introduced in the method described in this embodiment.

[0060] Embodiment 2

[0061] The embodiment provides an interventional surgery robot simultaneous localization and three-dimensional map construction system, which is used to realize the interventional surgery robot simultaneous localization and three-dimensional map construction method in the embodiment 1, and refers to Figure 3 , which comprises a catheter assembly 1, an ultrasonic sensor 2 and an electromagnetic positioning coil 3 arranged at the front end inside the catheter assembly; an electromagnetic shielding layer is arranged between the ultrasonic sensor 2 and the electromagnetic positioning coil 3; further comprising an extracorporeal part of a surgical body, the extracorporeal part comprising a proximal end driving module and an ultrasonic imaging system of the ultrasonic sensor, an electromagnetic positioning system matched with the electromagnetic positioning coil, and a data processing simultaneous localization and mapping system; the simultaneous localization and mapping system obtains the 2D image of the intravascular ultrasound and the spatial position information of the electromagnetic positioning coil, and performs the simultaneous localization and three-dimensional map construction of the interventional surgery robot.

[0062] When the blood vessel is imaged by IVUS, the catheter containing the ultrasonic sensor is first sent into the target blood vessel through radial artery puncture, and after reaching the lesion site, the software end first sends a signal to the proximal drive module to start collecting, the proximal drive module drives the ultrasonic sensor at the tip of the catheter to rotate and scan, under the action of the ultrasonic excitation circuit, the ultrasonic transducer in the ultrasonic sensor emits ultrasonic waves, the ultrasonic waves enter the blood vessel tissue and are reflected by the various layers of tissue structures to form ultrasonic echoes, the ultrasonic imaging system processes the ultrasonic echo signals received by the transducer to obtain an ultrasonic signal image, and then uploads the ultrasonic signal image back to the ultrasonic imaging system host. The host end processes the collected signal image in the software and performs coordinate transformation (polar coordinates-->Cartesian coordinates), and finally displays the cross section and longitudinal section of the lumen and wall of the blood vessel in a specific range in the form of an image.

[0063] As a preferred embodiment, the electromagnetic positioning system selects NDIAuroraV3 series medical magnetic field generator. The electromagnetic shielding layer includes two layers of separation layers 4 between the fixing device of the electromagnetic positioning coil and the ultrasonic sensor and a shielding body 5 between the two separation layers.

[0064] As a preferred embodiment, the separation layer 4 is in the form of a sheet and is made of an electromagnetic wave-absorbing material; the electromagnetic wave-absorbing material includes, but is not limited to, ferrite and nano-powder wave-absorbing material. The material of the shielding body 5 is a low-resistance metal material, and the shielding body 5 is grounded through an external cable.

[0065] The architecture between the synchronous positioning and three-dimensional map construction system module of the interventional surgery robot is shown in Figure 4 The IVUS intravascular ultrasound module and the electromagnetic positioning system at the interventional robot end serve as information acquisition modules to acquire image and position information of the interventional robot end, and then transmit the image and position information to the upper computer for data and information processing related to SLAM. The controller for the data and information processing is a high-performance fast processor of TI company, DSP28335 type.

[0066] In summary, the application provides an interventional surgery robot synchronous positioning and three-dimensional map construction method and system. The system comprises a catheter assembly, an ultrasonic sensor and an electromagnetic positioning coil arranged at the front end inside the catheter assembly; an electromagnetic shielding layer is arranged between the ultrasonic sensor and the electromagnetic positioning coil; further comprising a proximal end driving module of the ultrasonic sensor and an ultrasonic imaging system, an electromagnetic positioning system matched with the electromagnetic positioning coil, and a synchronous positioning and mapping system (SLAM) for data processing; the SLAM acquires the 2D image of intravascular ultrasound and the spatial position information of the electromagnetic positioning coil, and performs synchronous positioning and three-dimensional map construction of the interventional surgery robot. The application effectively combines intravascular ultrasound, an electromagnetic positioning system and a SLAM, can realize intravascular autonomous positioning and navigation of a vascular interventional surgery robot, improves the navigation and position sensing capability of the robot in the interventional surgery, and effectively improves the accuracy and surgical effect of the surgery.

[0067] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the division of the above functional units and modules is exemplified, and in actual application, the above functions can be completed by different functional units and modules according to needs, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment can be integrated in one processing unit, or each unit can be physically present separately, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of software functional unit. In addition, the specific names of each functional unit and module are only for easy distinction, and do not limit the protection scope of the application. In the above embodiments, the description of each embodiment is focused on, and the parts not described or recorded in a certain embodiment can be referred to the relevant description of the other embodiments.

[0068] Those skilled in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be realized in electronic hardware or a combination of computer software and electronic hardware. Whether the functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the application.

[0069] The integrated modules / units, if implemented in the form of software functional units and sold or used as independent products, can be stored in a computer readable storage medium. Based on such understanding, all or part of the processes in the above-mentioned embodiments of the present application can also be completed by computer programs instructing related hardware. Among them, the computer program includes computer program code, which can be in the form of source code, object code, executable file or some intermediate form, etc. The computer readable medium can include any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier wave signal, telecommunication signal and software distribution medium, etc.

[0070] The preferred embodiments of the present application are described above. It should be understood that the present application is not limited to the specific embodiments described above, and that the devices and methods not described in detail should be understood as being implemented in the ordinary way in the art; any person skilled in the art can make many possible changes and modifications to the technical solutions of the present application, or modify them into equivalent embodiments with equivalent changes, without departing from the scope of the technical solutions of the present application, which does not affect the essential content of the present application. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application, without departing from the content of the technical solutions of the present application, still belongs to the scope of protection of the technical solutions of the present application.

Claims

1. An intervention robot simultaneous localization and mapping method, characterized by, The method comprises the following steps: Step S1, the simultaneous localization and mapping system obtains intravascular ultrasound images as input frames by an ultrasound probe in the interventional surgery robot, and obtains map points; Step S2, the input frames are preprocessed; Step S3, an ORB feature point extraction algorithm is used to extract feature points; Step S4, the pose of the ultrasound probe is estimated by a motion model; Step S5, the optimal feature points in the region are retained by a non-maximum suppression algorithm, and multiple feature points at adjacent positions are eliminated; Step S6, the simultaneous localization and mapping system obtains the depth of the image by an electromagnetic positioning coil in the interventional surgery robot, and generates three-dimensional map point information corresponding to the optimal feature points in combination with the feature points of the input frames; each map point information comprises 3D coordinates in a world coordinate system and a view direction; the view direction refers to the direction of the ray connecting the point cloud and the optical center of the corresponding observation key frame; Step S7, the ORB feature points of each new image are extracted by an algorithm for real-time tracking, and compared with the nearest key frame, the positions of the feature points are calculated, and the camera pose is roughly estimated, so as to determine whether the current frame needs to be added to the key frame; Step S8, local map construction: in combination with the calculation results of step S4 and the added key frame, a local map is constructed, and the camera pose and the spatial position of the feature points in the local space are solved to obtain a more refined camera pose and feature point spatial position, and meanwhile, the redundant key frames in which the map points can be observed by other key frames are eliminated; Step S9, a Kalman filtering method is used to observe and estimate the movement of the robot; Step S10, the map and the position are constantly updated, and the simultaneous localization and mapping of the robot is realized.

2. The method of claim 1, wherein, The simultaneous localization and mapping system does not introduce a loop detection module for detecting the repeated motion path of the robot.

3. The method of claim 1 or 2, wherein, The preprocessing in step S2 comprises converting the ultrasound images into grayscale images.

4. The method of claim 1 or 2, wherein, The ORB feature point extraction algorithm in step S3 comprises a FAST algorithm.

5. The method of claim 1 or 2, wherein, The motion model in step S4 is a PnP algorithm.

6. The method of Simultaneous Localization and Mapping for an interventional robot according to claim 1 or 2, characterized in that, In step S6, the following information is saved for each key frame Ki: a camera pose T(i, w), a transformation matrix from the world coordinate system to the camera coordinate system, and camera parameters including a principal point and a focal length.

7. An intervention robot simultaneous localization and mapping system for implementing the intervention robot simultaneous localization and mapping method of any one of claims 1 to 6, characterized in that, The method comprises the following steps: A catheter assembly, an ultrasound sensor and an electromagnetic positioning coil arranged at the front end inside the catheter assembly; an electromagnetic shielding layer is arranged between the ultrasound sensor and the electromagnetic positioning coil; Further comprising an extracorporeal part of a surgical body, the extracorporeal part comprising: a proximal end driving module and an ultrasound imaging system of the ultrasound sensor, an electromagnetic positioning system matched with the electromagnetic positioning coil, and a data processing simultaneous localization and mapping system; the simultaneous localization and mapping system obtains 2D images of intravascular ultrasound and spatial position information of the electromagnetic positioning coil, and performs the simultaneous localization and three-dimensional map construction of the interventional surgery robot.

8. The intervention robot simultaneous localization and mapping system of claim 7, wherein, The electromagnetic positioning system selects an NDIAuroraV3 series medical magnetic field generator.

9. The intervention robot simultaneous localization and mapping system of claim 7, wherein, The electromagnetic shielding layer comprises two layers of separation layers between the fixing device of the electromagnetic positioning coil and the ultrasonic sensor and a shielding body between the separation layers.

10. The intervention robot simultaneous localization and mapping system of claim 9, wherein, The separation layers are in the form of sheets and are made of a material capable of absorbing electromagnetic waves; the shielding body is made of a low-resistance metal material and is grounded.

Images