CN116919595A - Bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering - Google Patents
Bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering Download PDFInfo
- Publication number
- CN116919595A CN116919595A CN202311038522.6A CN202311038522A CN116919595A CN 116919595 A CN116919595 A CN 116919595A CN 202311038522 A CN202311038522 A CN 202311038522A CN 116919595 A CN116919595 A CN 116919595A
- Authority
- CN
- China
- Prior art keywords
- optical
- needle
- electromagnetic
- tracking
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000003287 optical effect Effects 0.000 title claims abstract description 152
- 210000000988 bone and bone Anatomy 0.000 title claims abstract description 115
- 238000000034 method Methods 0.000 title claims abstract description 62
- 238000001914 filtration Methods 0.000 title claims abstract description 29
- 230000033001 locomotion Effects 0.000 claims abstract description 24
- 239000011159 matrix material Substances 0.000 claims description 43
- 230000009466 transformation Effects 0.000 claims description 26
- 238000005259 measurement Methods 0.000 claims description 13
- 230000004807 localization Effects 0.000 claims 1
- 239000007062 medium k Substances 0.000 claims 1
- 230000005291 magnetic effect Effects 0.000 abstract description 9
- 238000001356 surgical procedure Methods 0.000 abstract description 6
- 238000002474 experimental method Methods 0.000 description 12
- 230000004927 fusion Effects 0.000 description 10
- 238000005516 engineering process Methods 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 206010017076 Fracture Diseases 0.000 description 4
- 210000004197 pelvis Anatomy 0.000 description 4
- 238000005070 sampling Methods 0.000 description 4
- 206010034246 Pelvic fractures Diseases 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000006378 damage Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 239000010963 304 stainless steel Substances 0.000 description 1
- 229910000589 SAE 304 stainless steel Inorganic materials 0.000 description 1
- 208000027418 Wounds and injury Diseases 0.000 description 1
- 210000003484 anatomy Anatomy 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000005294 ferromagnetic effect Effects 0.000 description 1
- 238000002675 image-guided surgery Methods 0.000 description 1
- 238000000338 in vitro Methods 0.000 description 1
- 238000001727 in vivo Methods 0.000 description 1
- 208000014674 injury Diseases 0.000 description 1
- 238000002357 laparoscopic surgery Methods 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000002324 minimally invasive surgery Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 210000000056 organ Anatomy 0.000 description 1
- 210000003049 pelvic bone Anatomy 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011946 reduction process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 210000001519 tissue Anatomy 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2046—Tracking techniques
- A61B2034/2051—Electromagnetic tracking systems
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2046—Tracking techniques
- A61B2034/2055—Optical tracking systems
Landscapes
- Health & Medical Sciences (AREA)
- Surgery (AREA)
- Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Medical Informatics (AREA)
- Robotics (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Surgical Instruments (AREA)
Abstract
A bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering relates to the technical field of surgical navigation. The invention aims to solve the problems that a single electromagnetic positioning method is low in precision and can be interfered by a magnetic field, and an optical signal of the single optical positioning method is easy to be blocked, so that a tracking target is lost. The invention discloses a bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering, which is characterized in that the positions of bone needle points are tracked by utilizing an optical sensor and an electromagnetic sensor, the position point sets of the bone needle points during optical tracking and electromagnetic tracking are respectively obtained, the position point sets of the bone needle points during optical tracking and electromagnetic tracking are unified under the same coordinate system, the position observation vector of the bone needle points is constructed, then the position observation vector is substituted into a position observation equation of the bone needle points, the motion state vector of the bone needle points is calculated and obtained, and the real-time tracking of the position of the bone needle points is realized. The invention is suitable for complex path and heavy load surgery.
Description
Technical Field
The invention belongs to the technical field of surgical navigation, and particularly relates to tracking of needle tip positions of bone needles.
Background
Pelvic fractures are a serious high-energy injury, with the highest disability and mortality among all fractures. Therefore, the pelvis resetting robot has larger resetting force and more complex resetting path, which also has influence on the precision of the navigation system, and the probability of the surgical instrument being shielded in the surgical process is high, and the traditional surgical navigation optical positioning method is difficult to meet the requirement of being not shielded.
Since the pelvis has a complex anatomical structure and is located deep in the human body, the pelvis is difficult to reveal in the operation, and the fracture type is complex and various, so the pelvis fracture operation is one of the operations with the highest difficulty in the orthopaedics field, and is called as an expert operation in the orthopaedics field. In recent years, along with the development of robot-assisted minimally invasive surgery, the introduction of a navigation system can improve positioning accuracy, reduce shaking of surgical instruments, and help surgeons to finish surgery accurately to the greatest extent. Surgical navigation enables a surgeon to visualize internal structures of a patient's body in real-time during a surgical procedure. During surgery, the surgeon may use the map created by the navigation system to more reasonably plan the reduction path, thereby ensuring greater accuracy and reducing the risk of damage to surrounding tissue. The navigation system may also provide real-time feedback of the position and motion of the instrument during the surgical procedure. Therefore, constructing a precise pelvic fracture reduction surgical navigation system is important for the pelvic reduction robot.
In consideration of different surgical environment requirements, the navigation system can select different positioning methods, and the positioning methods of the current surgical navigation system mainly comprise optical positioning, electromagnetic positioning, ultrasonic positioning, image positioning and the like. The university of hopkins, johns developed a computer-tomograph (CT) based navigation robot system, the university of halbine industrial robot research developed a HIT-RAOS for robotic assisted surgery using X-ray images, and germany developed a set of optical tracking system based navigation systems.
To date, optical and electromagnetic tracking systems represent two major technologies integrated into surgical navigation systems for computer-aided image guided surgery. The optical tracking system has high precision and robustness to the environment, but a direct line-of-sight range between an optical target point and a camera is required; electromagnetic tracking systems do not need to take into account whether there is a line of sight occlusion between the sensor and an external reference, but the presence of ferromagnetic sources in space can have a significant impact on the measurement results. Therefore, a single positioning method always has unavoidable defects, so that some students adopt a method of fusing multiple sensors to improve navigation accuracy. Xinyang Liu et al developed a new method for tracking a laparoscopic ultrasonic transducer in laparoscopic surgery by combining an electromagnetic tracking technology and a computer vision-based (e.g. ArUco) tracking method in a sensor fusion mode, and the accuracy of the obtained sensing tracking data is better than that of the single electromagnetic generator; chao Shi et al developed a novel robot system for reduction of pelvic fracture, and realized real-time 3D navigation technique of pelvic position through fusion of optical tracking technique and non-rigid registration algorithm; the method for fusing optical tracking data and gyroscope sensing data by using Kalman filtering is proposed by Zhichong Yang et al, so that the direction of the gyroscope is estimated and supplemented by using the gyroscope, and the tracking precision can be well improved by carrying out data fusion by using Kalman filtering; yongde Zhang et al propose to fuse inertial navigation and electromagnetic navigation data by Kalman filtering, thereby realizing a more accurate tracking technology of organ motion in operation.
Compared with the long bone reduction operation, the axial movement is only needed, the path of the pelvic bone needle reduction operation is more complex, the reduction force is more increased, if the traditional optical navigation target is arranged at the root of the reduction bone needle for optical navigation, in the reduction process, the optical target is blocked and the tracking target is lost due to the complex path, so that the navigation system is invalid. And because the restoring force is large, the bone needle can be deformed greatly, so that the optical target at the tail end of the bone needle can not truly reflect the pose of the bone block.
Disclosure of Invention
The invention aims to solve the problems that a single electromagnetic positioning method is low in precision and can be interfered by a magnetic field, and an optical signal of the single optical positioning method is easy to be blocked to cause the loss of a tracking target, and provides a bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering.
The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering specifically comprises the following steps:
tracking the position of the needle point of the bone needle by utilizing an optical sensor and an electromagnetic sensor, respectively obtaining a set of position points of the needle point of the bone needle during optical tracking and electromagnetic tracking, unifying the set of position points of the needle point of the bone needle during optical tracking and electromagnetic tracking under the same coordinate system, constructing a position observation vector of the needle point of the bone needle, substituting the position observation vector into a position observation equation of the needle point of the bone needle, calculating to obtain a motion state vector of the needle point of the bone needle, and realizing real-time tracking of the position of the needle point of the bone needle.
Further, the optical sensor includes: a first set of optical targets 1, a second set of optical targets 5 and an optical tracking device 3, the electromagnetic sensor comprising: an electromagnetic target 2 and an electromagnetic tracking device 4, wherein the first group of optical targets 1 and the electromagnetic target 2 are arranged on the bone needle relatively static, and the second group of optical targets 5 are arranged on the electromagnetic tracking device 4; the tracking positions of the first group of optical targets 1 and the electromagnetic targets 2 are registered to the positions of the needle points of the bone needles in a rotating calibration mode, the positions of the needle points of the bone needles are tracked in real time in the rotating calibration process, and the positions of the needle points of the bone needles are used for calculating the position point sets of the needle points of the bone needles during optical tracking and electromagnetic tracking respectively.
Further, the method for obtaining the position point set of the needle point of the bone needle during the optical tracking and the electromagnetic tracking comprises the following steps:
calculating a position point set of the needle tip under the OPM1 coordinate system when optically tracking the first group of optical targets 1 according to the following
Calculating the position point set of the needle point of the bone needle under the EM coordinate system when the electromagnetic tracking electromagnetic target point 2 is calculated according to the following formula
wherein ,for the transformation matrix of the OP coordinate system to the OPM1 coordinate system,/for the transformation matrix of the OP coordinate system to the OPM1 coordinate system>For the transformation matrix of OPM2 coordinate system to OP coordinate system,/for the transformation matrix of OPM2 coordinate system to OP coordinate system>For the position point set of the needle point of the bone needle under the OPM2 coordinate system obtained in the rotation calibration process, < >>For the transformation matrix of EMS coordinate system to EM coordinate system, < > for>In order to obtain the position point set of the needle point of the bone needle under the EMS coordinate system in the rotation calibration process,
the OP coordinate system represents the coordinate system of the optical tracking device 3, the OPM1 coordinate system represents the coordinate system of the second set of optical targets 5, the OPM2 coordinate system represents the coordinate system of the first set of optical targets 1, the EM coordinate system represents the coordinate system of the electromagnetic tracking device 4, and the EMs represents the coordinate system of the electromagnetic targets 2.
Further, the unifying the position point set of the needle tip during the optical tracking and the electromagnetic tracking to the same coordinate system includes:
will be according toUnified into an OPM1 coordinate system:
wherein ,for the transformation matrix of the EM coordinate system to the OPM1 coordinate system,/for the transformation matrix of the EM coordinate system to the OPM1 coordinate system>Is a set of position points of the needle tip of the bone needle under an OPM1 coordinate system when the electromagnetic target spot 2 is electromagnetically tracked.
Further, a least squares fitting registration method is utilized to calculate
Wherein n is the number of position points in the set of position points of the needle tip, i=1, 2,..n, C is the position error model coefficient,is->Position information of the i-th position point of (x) i ,y i ,z i ) Is->Position information of the i-th position point in the list.
Further, the expression of the position observation vector z (k) of the spicule tip is:
z(k)＝[P OT(k) P ET(k) ] T ，
P OT(k) is thatCoordinates of position point of needle point of bone needle at time k, P ET(k) Is->And (3) the coordinates of the position point of the needle point of the bone needle at the moment k.
Further, the equation expression of the position observation of the needle tip is as follows:
z(k)＝Hx(k)+v(k)，
wherein H is an observation matrix and hasI 3 Is a three-dimensional identity matrix, 0 3 V (k) is a measurement noise matrix, and x (k) is a motion state vector of the needle tip.
Further, the measurement noise matrix v (k) is subjected to v (k) -N (0, R),
wherein , and />The root mean square of the errors of the first group of optical targets 1 and the electromagnetic targets 2 respectively.
Further, the expression of the motion state vector x (k) of the needle tip is:
wherein ,PT(k) Tracking the position coordinates of the needle tip of the bone needle for the moment k,the movement speed of the needle tip is tracked for time k.
Further, the optical sensor is a six-degree-of-freedom optical sensor, and the first group of optical targets 1 are positioned at the tail ends of bone spicules; the electromagnetic sensor is a six-degree-of-freedom electromagnetic sensor, and the electromagnetic target point 2 is positioned inside the bone needle and is close to the needle point.
The invention firstly installs an optical sensor and an electromagnetic sensor on the bone spicule, and then analyzes an error model of optical tracking and electromagnetic tracking. On the basis, the rotation calibration of the optical target spot and the electromagnetic target spot on the intelligent spicule is completed, so that the optical six-dimensional sensor and the electromagnetic six-dimensional sensor track the position of the spicule tip simultaneously, and then the conversion relation between the optical coordinate system and the electromagnetic coordinate system is obtained through point cloud registration, and the coordinate system of the operation navigation system is unified. And finally, utilizing Kalman filtering to fuse electromagnetic and optical positioning information, and establishing the electromagnetic and optical fused positioning navigation system based on the Kalman filtering. And the electromagnetic sensor is adopted to replace the optical sensor information to perform sensor fusion under the condition that the optical sensor is shielded and the optical target point information is missing, so that the effect of tracking the needle point position of the bone needle in a complex operation environment can be achieved, and the system can more accurately represent the pose of bone tissue in a complex path and a heavy-load operation.
Drawings
FIG. 1 is a schematic structural view of a bone needle according to an embodiment;
FIG. 2 is a schematic diagram of error analysis, wherein (a) represents tracking error of an optical tracking device with respect to angle and (b) represents tracking error of an electromagnetic tracking device with respect to distance;
FIG. 3 is a schematic diagram of a coordinate relationship;
FIG. 4 is a schematic diagram of coordinate system registration;
fig. 5 is a schematic diagram of experimental results, wherein (a) represents a kalman filter experiment and (b) represents an optical shielding experiment.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention. It should be noted that, without conflict, the embodiments of the present invention and features of the embodiments may be combined with each other.
The embodiment provides a 3D operation navigation system integrating optical positioning information and electromagnetic positioning information, in particular to a bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering.
As shown in FIG. 1, the bone needle is made of 304 stainless steel, has a total length of 400mm and an outer diameter of 6mm, and is divided into an upper section and a lower section. The upper section length is 365mm, a through hole with the inner diameter of 2.5mm is processed, the lower section is a bone needle drill, and the middle is connected by a threaded column with the diameter of 3 mm. The electromagnetic target point 2 of the 6-degree-of-freedom electromagnetic sensor is packaged in a heat-shrinkable tube with the outer diameter equal to the inner diameter of the bone needle through hole, and then the heat-shrinkable tube is inserted from the tail end of the bone needle to the tip of the bone needle, so that the position error of needle point position tracking caused by bone needle deformation is reduced. The optical targets of the 6-degree-of-freedom optical sensors are divided into two groups, the first group of optical targets 1 are fastened at the tail ends of the bone spicules through the connecting pieces, so that the bone spicules integrate the electromagnetic targets 2 and the first group of optical targets 1, and position tracking information of the two sensors can be acquired simultaneously. A second set of optical targets 5 of the 6-degree-of-freedom optical sensor is arranged on the electromagnetic tracking device 4.
The tracking error of the sensor is an important component of the error of the whole navigation system, and the positioning data error of the optical target point is related to the gesture of the optical tracking device 3. In order to explore the pose of the first set of optical targets 1 when the positioning data errors are minimal, the first set of optical targets 1 is fixed using an XYR triaxial displacement rotary platform. The first group of optical targets 1 are rotated at intervals of 5 degrees, and the positioning errors of the first group of optical targets 1 are recorded. The magnitude of the electromagnetic positioning error is influenced by the distance between the electromagnetic target 2 and the electromagnetic tracking device 4. Because the accurate positioning range of the electromagnetic positioning technology is smaller, the electromagnetic positioning technology is easy to cause quite large positioning errors due to improper placement in the actual use process. Therefore, it is necessary to experimentally measure the relation between the electromagnetic positioning error and the sensor placement position, and provide a reference for the motion range of the electromagnetic sensor in the subsequent experiments and actual use.
From the analysis of fig. 2, it can be seen that the optical navigation positioning system error is small when the first set of optical targets 1 makes an angle of about plus or minus 40 ° with the optical tracking device 3. The experimental results can provide a reference for the placement pose of the optical sensor in the optical positioning operation. Whereas for the electromagnetic target 2, the error is greater as the electromagnetic target 2 is farther from the center of the magnetic field generator in the YZ plane. And as the distance in the Z direction increases, the positioning error of the electromagnetic target 2 increases significantly over each Y distance. Therefore, the electromagnetic tracking device 4 should be used to track and position the magnetic field generator as close as possible to the center during the subsequent experiments and practical use.
The tracking positions of the first group of optical targets 1 and the electromagnetic targets 2 are registered to the positions of the needle points of the bone needles in a rotation calibration mode, so that the positions of the needle points of the bone needles are located at the same physical position in the optical tracking process and the electromagnetic tracking process. Meanwhile, the position of the needle point of the bone needle is tracked in real time in the rotation calibration process. The principle of rotation calibration is as follows: in the effective range of the optical positioning system, the needle point of the bone needle is positioned in the conical hole of the calibration block, and the drill bit is taken as the vertex to rotate for 360 degrees. The included angle between the rotation and the plumb line is between 30 degrees and 60 degrees, and after the rotation is finished, the system automatically calculates the compensation value, the error root mean square value, the maximum error value, the minimum deflection angle and the like from the rotation center to the optical passive rigid body coordinate system. After the compensation value is applied, the position information output by the optical positioning system is not the position information of the optical passive rigid body, but the position information of the needle point of the bone needle. Through 15 groups of calibration, the obtained position point set of the needle point of the bone needle under the OPM2 coordinate system (the coordinate system of the first group of optical targets 1) in the rotation calibration processThe accuracy (root mean square error value) is between 0.25 and 0.40 mm. The calibration method of the electromagnetic positioning system is the same as that of optics, and 15 groups of calibration are performed to obtain a position point set of the needle point of the bone needle under an EMS coordinate system (a coordinate system of an electromagnetic target 2) in the rotation calibration process>The accuracy (error root mean square value) is about 0.50 mm.
Calculating a position point set of the needle tip under the OPM1 coordinate system when optically tracking the first group of optical targets 1 according to the following
Calculating the position point set of the needle point of the bone needle under the EM coordinate system when the electromagnetic tracking electromagnetic target point 2 is calculated according to the following formula
wherein ,for the transformation matrix of the OP coordinate system to the OPM1 coordinate system,/for the transformation matrix of the OP coordinate system to the OPM1 coordinate system>Is a transformation matrix from an OPM2 coordinate system to an OP coordinate system. /> and />Are all calibrated conversion parameters in the optical sensor. />The conversion matrix from EMS coordinate system to EM coordinate system is the calibrated conversion parameter in electromagnetic sensor. The OP coordinate system represents the coordinate system of the optical tracking device 3 and the OPM1 coordinate system represents the second set of lightThe coordinate system of the target point 5 and the EM coordinate system represent the coordinate system of the electromagnetic tracking device 4. In order to realize the hybrid tracking of an electromagnetic and optical positioning system in a navigation system, after the tracking positions of a six-dimensional optical sensor and a six-dimensional electromagnetic sensor on an intelligent bone needle are registered to the position of the needle tip of the bone needle, the optical coordinate system and the electromagnetic coordinate system are unified to obtain a conversion matrix from the optical coordinate system to the electromagnetic coordinate system, so that the next fusion algorithm work can be performed. In the embodiment, a least square fitting registration method is used for calculating a conversion matrix of an EM coordinate system to an OPM1 coordinate system +.>The core idea of the least square algorithm is to align two position point sets in an iterative optimization mode:
wherein n is the number of position points in the set of position points of the needle tip, i=1, 2,..n, C is the position error model coefficient,is->Position information of the i-th position point of (x) i ,y i ,z i ) Is->Position information of the i-th position point in the list. Assuming a rigid body transformation (including translation and rotation) between the sets of position points, the optimal rigid body transformation parameters are solved by minimizing the distance error between the two sets of position points. Next, the registered position point set is obtained by transforming the rotation matrix and the translation matrix. Finally, a transformation matrix is obtained to calculate the root mean square error to evaluate the accuracy of the registration result, which is about 0.8mm.
The transformation matrix in this embodiment is specifically:
root mean square error rmse=1.371.
The set of location points also contains some "dead spots" with large measurement errors, which are affected by the accuracy of the optical and electromagnetic sensors. Therefore, the position point set registration effect is poor, the root mean square error of the final transformation matrix is larger, and the misaligned dead pixels after registration can be obviously seen in the position point set registration diagram. In order to reduce the error of the transformation matrix obtained by registration, the embodiment screens out the points with the measurement error larger than 0.2mm, eliminates the obviously misaligned bad points after registration, and carries out the registration experiment again. As shown in fig. 4, the registration effect after removing the dead pixel is significantly better than the registration result in the case of multiple data. By the process of eliminating dead pixels, the root mean square error is reduced from 1.37 to 0.50. Meets the use requirement of the system. The transformation matrix from the optical reference coordinate system to the electromagnetic coordinate system is obtained by an electromagnetic coordinate system and optical coordinate system registration experiment and is as follows:
finally, the following formula will be adoptedUnified into an OPM1 coordinate system:
wherein ,for the transformation matrix of the EM coordinate system to the OPM1 coordinate system,/for the transformation matrix of the EM coordinate system to the OPM1 coordinate system>Time bone for electromagnetic tracking electromagnetic target 2The set of needle points of the needle tip in the OPM1 coordinate system.
The kalman filter algorithm is a technique used in control theory and signal processing to combine measurements from multiple sensors to estimate the state of the system. It is often used for navigation and robotic applications where there are multiple data sources providing information about position, speed and direction. In this embodiment, the Kalman filtering algorithm fuses electromagnetic sensor and optical sensor positioning data by predicting and updating. The filter needs to be predicted in combination with the process noise to build the process model and corrected in combination with the measurement noise to build the measurement equation.
Motion state vector of selected spicule needle point at k momentAnd constructing a motion state equation of the needle tip of the bone needle. Wherein, the motion state equation of the needle point of the bone needle is generally as follows: x (k) =ax (k-1) +w (k-1),
the specific form of the motion state equation of the needle point of the bone needle is as follows:
P T(k) 、P T(k-1) tracking the position coordinates of the needle point of the bone needle at the moment k and the moment k-1 respectively,tracking the movement speed of the needle tip of the bone needle at the moment k and the moment k-1 respectively, and allowing the user to perform +.>Tracking acceleration to needle tip of bone needle at time k, I 3 Is a three-dimensional identity matrix, 0 3 Is a three-dimensional zero matrix, T s Is the motion sampling period.
The uncertainty of the state equation of the needle tip motion of the bone needle is caused by an acceleration term, and the needle tip acceleration is assumedIs constant during each sampling time and is represented by an independent random variable, subject to zero-mean gaussian distribution +.> wherein Is a diagonal matrix with diagonal elements in the x, y, z directions and other elements in 0. Therefore acceleration item->Can be considered as process noise and is subject to w (k) to N (0, q),
to correct the state equation of the intelligent spicule, the observation results of the optical sensor and the electromagnetic sensor are combined to establish a position observation vector z (k) = [ P ] OT(k) P ET(k) ] T ，P OT(k) Is thatCoordinates of position point of needle point of bone needle at time k, P ET(k) Is thatAnd (3) the coordinates of the position point of the needle point of the bone needle at the moment k.
The equation of observation of the needle tip position is expressed as follows:
z(k)＝Hx(k)+v(k)，
h is an observation matrix and hasv (k) is the measurement noise matrix and is subject to v (k) -N (0, R), and />The root mean square of the errors of the first group of optical targets 1 and the electromagnetic targets 2 respectively.
Unlike the process noise w (k), the measurement noise v (k) covariance matrix R varies at each time step.
The embodiment is based on an electromagnetic and optical fusion reset positioning navigation method, uses an Optical Tracking System (OTS) and an electromagnetic tracking system (EMTS) to position the gesture of a surgical instrument in space, and aims at the problems that the optical precision is high, but the electromagnetic precision is required to be ensured not to be blocked, the electromagnetic precision is low and the electromagnetic precision can be influenced by magnetic field distortion, and provides a fracture reset in-vivo and in-vitro gesture high-precision dynamic measurement method which fuses the electromagnetic tracking technology and the optical tracking technology. Firstly, an error model of an electromagnetic and optical tracking system is analyzed, a sensor is integrated on an intelligent bone needle and calibration is completed, and then optical navigation and electromagnetic navigation are fused through Kalman filtering, so that robust tracking of surgical instruments is realized. The fused positioning of the Optical Tracking System (OTS) and the electromagnetic tracking system (EMTS) effectively compensates for sensor occlusion, providing a continuous estimate of instrument pose.
Experiment A
Experiments are carried out by using the calibrated bone spicules with the optical positioning targets and the electromagnetic targets, the optical targets are fixed at the top ends of the bone spicules, and the electromagnetic targets are fixed at positions, close to the needle points, inside the bone spicules. And the bone needle is fixed on the Si Ling Diana7 mechanical arm. By analyzing the error models of the optical tracking equipment and the electromagnetic tracking equipment before, the angle between the optical target ball and the binocular camera is set to be about 45 degrees, and the distance between the electromagnetic targets in the movement range of the tail end of the mechanical arm is 100 mm-300 mm, so that the two sensing systems are tested in the high-precision effective tracking range.
The mechanical arm pulls the bone needle to move linearly at a uniform speed on a plane perpendicular to the magnetic field generator (YZ plane of the spatial coordinate system of the magnetic field generator). And in order to better simulate the shake during operation, the motion of the mechanical arm is added with acceleration noise with variance of 0.1 mm. And the position of the needle tip under the optical tracking system and the electromagnetic tracking system in the uniform motion process is recorded in a sampling period of 20hz, and the final position coordinates are processed and output in real time by Kalman filtering, and the result is shown in fig. 5 (a).
Experiment B
In consideration of the problem that the optical sight is blocked easily in the actual operation process, the fusion optical sensor and the electromagnetic sensor are proposed. Thus, in the absence of one or more available optical marker data (e.g., in the case of a line of sight occlusion), it is proposed to use data from the EMTS to provide missing optical data for a sensor fusion algorithm that relies solely on the system model and available measurements. The latter is called a single sensor fusion algorithm. And the optical sight shielding experiment which is easy to occur in the corresponding operation process is also designed, the optical target spot is artificially shielded twice in the process of dragging the bone needle by the mechanical arm, the shielding time length is about 2s each time, the reliability and the stability of the algorithm are tested, and the experimental result is shown in the figure 5 (b).
In experiment a, it can be seen that the stability of the filtered curve is better when the spicule is close to the magnetic field generator (z value is smaller), and the accuracy of the electromagnetic sensor is higher, so that the kalman filtering result is biased towards the electromagnetic positioning result. When the bone needle is far away from the magnetic field generator (when the z value is large), the accuracy of the optical sensor is high, so the kalman filter result starts to gradually deviate to the optical positioning result. Based on the sensor result with higher precision, the Kalman filtering is combined with another positioning result to compensate, so that the expected needle tip movement position is obtained.
In experiment B, 31 groups of optical data are lost in total under the condition of 20HZ sampling frequency by artificially shielding the optical target, and 1/4 of experimental data are occupied. In the absence of optical sensor data, the data from the EMTS is used to provide missing optical data to perform a sensor fusion algorithm. The final Kalman filtering curve is still stable, and the situation that the needle point position tracking is lost or the deviation from the true position is large does not occur, so that the Kalman filtering curve meets the experimental expectation.
Although the invention herein has been described with reference to particular embodiments, it is to be understood that these embodiments are merely illustrative of the principles and applications of the present invention. It is therefore to be understood that numerous modifications may be made to the illustrative embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the different dependent claims and the features described herein may be combined in ways other than as described in the original claims. It is also to be understood that features described in connection with separate embodiments may be used in other described embodiments.
Claims (10)
1. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering is characterized by comprising the following steps of:
tracking the position of the needle point of the bone needle by utilizing an optical sensor and an electromagnetic sensor, respectively obtaining a set of position points of the needle point of the bone needle during optical tracking and electromagnetic tracking, unifying the set of position points of the needle point of the bone needle during optical tracking and electromagnetic tracking under the same coordinate system, constructing a position observation vector of the needle point of the bone needle, substituting the position observation vector into a position observation equation of the needle point of the bone needle, calculating to obtain a motion state vector of the needle point of the bone needle, and realizing real-time tracking of the position of the needle point of the bone needle.
2. The method for tracking the position of a bone pin based on optical and electromagnetic positioning and kalman filtering according to claim 1, wherein said optical sensor comprises: -a first set of optical targets (1), -a second set of optical targets (5), and-an optical tracking device (3), the electromagnetic sensor comprising: the device comprises an electromagnetic target point (2) and an electromagnetic tracking device (4), wherein the first group of optical target points (1) and the electromagnetic target point (2) are relatively and statically arranged on a bone needle, and the second group of optical target points (5) are arranged on the electromagnetic tracking device (4);
the tracking positions of the first group of optical targets (1) and the electromagnetic targets (2) are registered to the positions of the needle points of the bone needles in a rotating calibration mode, the positions of the needle points of the bone needles are tracked in real time in the rotating calibration process, and the positions of the needle points of the bone needles are used for calculating the position point sets of the needle points of the bone needles during optical tracking and electromagnetic tracking respectively.
3. The method for tracking the position of the bone needle based on optical and electromagnetic positioning and Kalman filtering according to claim 2, wherein the method for obtaining the position point set of the bone needle tip during optical tracking and electromagnetic tracking is as follows:
calculating a set of position points of the needle tip under the OPM1 coordinate system when optically tracking the first group of optical targets (1) according to
Calculating the position point set of the needle point of the bone needle under the EM coordinate system when the electromagnetic tracking electromagnetic target point (2) is calculated according to the following formula
wherein ,for the transformation matrix of the OP coordinate system to the OPM1 coordinate system,/for the transformation matrix of the OP coordinate system to the OPM1 coordinate system>For the transformation matrix of OPM2 coordinate system to OP coordinate system,/for the transformation matrix of OPM2 coordinate system to OP coordinate system>For the position point set of the needle point of the bone needle under the OPM2 coordinate system obtained in the rotation calibration process, < >>For the transformation matrix of EMS coordinate system to EM coordinate system, < > for>In order to obtain the position point set of the needle point of the bone needle under the EMS coordinate system in the rotation calibration process,
the OP coordinate system represents the coordinate system of the optical tracking device (3), the OPM1 coordinate system represents the coordinate system of the second set of optical targets (5), the OPM2 coordinate system represents the coordinate system of the first set of optical targets (1), the EM coordinate system represents the coordinate system of the electromagnetic tracking device (4), and the EMs represents the coordinate system of the electromagnetic targets (2).
4. The method for tracking the position of the needle based on optical and electromagnetic positioning and kalman filtering according to claim 3, wherein the integrating the set of the position points of the needle tip in the optical tracking and the electromagnetic tracking into the same coordinate system comprises:
will be according toUnified into an OPM1 coordinate system:
wherein ,for the transformation matrix of the EM coordinate system to the OPM1 coordinate system,/for the transformation matrix of the EM coordinate system to the OPM1 coordinate system>Is a set of position points of the needle tip of the bone needle under an OPM1 coordinate system when the electromagnetic target point (2) is electromagnetically tracked.
5. The method for tracking the position of a bone needle based on optical and electromagnetic positioning and Kalman filtering according to claim 4The method is characterized in that the least square fitting registration method is utilized to calculate
Wherein n is the number of position points in the set of position points of the needle tip, i=1, 2,..n, C is the position error model coefficient,is->Position information of the i-th position point of (x) i ,y i ,z i ) Is->Position information of the i-th position point in the list.
6. The method for tracking the position of a bone needle based on optical and electromagnetic positioning and kalman filtering according to claim 4 or 5, wherein the expression of the position observation vector z (k) of the needle tip is:
P OT(k) is thatCoordinates of position point of needle point of bone needle at time k, P ET(k) Is->Bone needle tip at medium k momentIs defined by the location point coordinates of (a).
7. The method for tracking the position of the bone needle based on optical and electromagnetic positioning and Kalman filtering according to claim 6, wherein the expression of the equation of the position observation of the needle tip of the bone needle is as follows:
z(k)＝Hx(k)+v(k)，
wherein H is an observation matrix and hasI 3 Is a three-dimensional identity matrix, 0 3 V (k) is a measurement noise matrix, and x (k) is a motion state vector of the needle tip.
8. The method for tracking the position of a bone pin based on optical and electromagnetic localization and Kalman filtering according to claim 7, wherein the measurement noise matrix v (k) is subject to v (k) to N (0, R),
wherein , and />The error root mean square of the first group of optical targets (1) and the electromagnetic targets (2) respectively.
9. The method for tracking the position of the bone needle based on optical and electromagnetic positioning and Kalman filtering according to claim 7, wherein the expression of the motion state vector x (k) of the bone needle tip is:
wherein ,PT(k) When k isThe position coordinates of the needle point of the bone needle are tracked,the movement speed of the needle tip is tracked for time k.
10. The method for tracking the position of a bone pin based on optical and electromagnetic positioning and kalman filtering according to claim 2, characterized in that said optical sensor is a six-degree-of-freedom optical sensor and said first set of optical targets (1) is located at the end of the bone pin;
the electromagnetic sensor is a six-degree-of-freedom electromagnetic sensor, and the electromagnetic target point (2) is positioned inside the bone needle and is close to the needle point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311038522.6A CN116919595B (en) | 2023-08-17 | 2023-08-17 | Bone needle position tracking device based on optical and electromagnetic positioning and Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311038522.6A CN116919595B (en) | 2023-08-17 | 2023-08-17 | Bone needle position tracking device based on optical and electromagnetic positioning and Kalman filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116919595A true CN116919595A (en) | 2023-10-24 |
| CN116919595B CN116919595B (en) | 2024-06-07 |
Family
ID=88387922
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311038522.6A Active CN116919595B (en) | 2023-08-17 | 2023-08-17 | Bone needle position tracking device based on optical and electromagnetic positioning and Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116919595B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160258782A1 (en) * | 2015-02-04 | 2016-09-08 | Hossein Sadjadi | Methods and Apparatus for Improved Electromagnetic Tracking and Localization |
| CN106344153A (en) * | 2016-08-23 | 2017-01-25 | 深圳先进技术研究院 | Automatic tracking device and method for flexible puncture needle tip |
| CN114711968A (en) * | 2022-03-31 | 2022-07-08 | 广东工业大学 | Non-calibration target area positioning and tracking method based on surgical robot system |
| US20230114385A1 (en) * | 2021-10-08 | 2023-04-13 | City University Of Hong Kong | Mri-based augmented reality assisted real-time surgery simulation and navigation |
| CN116077181A (en) * | 2022-12-21 | 2023-05-09 | 杭州邦杰星医疗科技有限公司 | Medical bone drill bit feeding method and clamping equipment |
| CN117122414A (en) * | 2023-10-17 | 2023-11-28 | 上海市第六人民医院 | Active tracking type operation navigation system |
-
2023
- 2023-08-17 CN CN202311038522.6A patent/CN116919595B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160258782A1 (en) * | 2015-02-04 | 2016-09-08 | Hossein Sadjadi | Methods and Apparatus for Improved Electromagnetic Tracking and Localization |
| CN106344153A (en) * | 2016-08-23 | 2017-01-25 | 深圳先进技术研究院 | Automatic tracking device and method for flexible puncture needle tip |
| US20230114385A1 (en) * | 2021-10-08 | 2023-04-13 | City University Of Hong Kong | Mri-based augmented reality assisted real-time surgery simulation and navigation |
| CN114711968A (en) * | 2022-03-31 | 2022-07-08 | 广东工业大学 | Non-calibration target area positioning and tracking method based on surgical robot system |
| CN116077181A (en) * | 2022-12-21 | 2023-05-09 | 杭州邦杰星医疗科技有限公司 | Medical bone drill bit feeding method and clamping equipment |
| CN117122414A (en) * | 2023-10-17 | 2023-11-28 | 上海市第六人民医院 | Active tracking type operation navigation system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116919595B (en) | 2024-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| AU2020201097B2 (en) | Systems for assisting surgery | |
| CN111494009B (en) | Image registration method and device for surgical navigation and surgical navigation system | |
| Enayati et al. | A quaternion-based unscented Kalman filter for robust optical/inertial motion tracking in computer-assisted surgery | |
| US10342619B2 (en) | Method and device for determining the mechanical axis of a bone | |
| CN110264504B (en) | Three-dimensional registration method and system for augmented reality | |
| EP1769768A1 (en) | Surgical instrument calibration | |
| CN110101452A (en) | A kind of optomagnetic integrated positioning navigation method for surgical operation | |
| CN111134841B (en) | Method and tool for registering pelvis in hip replacement | |
| US20100049493A1 (en) | Planning assistance for correcting joint elements | |
| CN112190328A (en) | Holographic perspective positioning system and positioning method | |
| Gonçalves et al. | A vision system for robotic ultrasound guided orthopaedic surgery | |
| Šuligoj et al. | Medical applicability of a low-cost industrial robot arm guided with an optical tracking system | |
| Han et al. | Optimization scheme for online viewpoint planning of active optical navigation system in orthopedic surgeries | |
| CN113470184A (en) | Endoscope augmented reality error compensation method and device | |
| CN116919595B (en) | Bone needle position tracking device based on optical and electromagnetic positioning and Kalman filtering | |
| Ren et al. | Development of a wireless hybrid navigation system for laparoscopic surgery | |
| CN116363093A (en) | Method and device for searching rotation center of acetabulum, operation planning system and storage medium | |
| He et al. | Sensor-fusion based augmented-reality surgical navigation system | |
| Xia et al. | Kalman Filter-based EM-optical Sensor Fusion for Bone Needle Position Tracking | |
| Hardner et al. | Guided calibration of medical stereo endoscopes | |
| Tseng et al. | Image‐guided robotic navigation system for neurosurgery | |
| KR102708210B1 (en) | Method and apparatus for image guided surgery | |
| Chen et al. | Pose estimation of near-infrared surgical instruments based on binocular vision | |
| Enayati | Quaternion based unscented Kalman filter for robust motion tracking in neurosurgery | |
| Bergeles et al. | Model-based localization of intraocular microrobots for wireless electromagnetic control |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |