Bone pin position tracking method based on optical and electromagnetic localization and kalman filtering

Abstract

A bone pin position tracking method based on optical and electromagnetic localization and Kalman filtering, relating to the technical field of surgical navigation. In order to solve the problems that a single electromagnetic localization method has low precision and is affected by magnetic field interference, and in a single optical localization method, a tracked target is lost caused by an optical signal being easily blocked, designed is a bone pin position tracking method based on optical and electromagnetic localization and Kalman filtering, comprising: using an optical sensor and an electromagnetic sensor to track the position of a bone pin tip, so as to obtain position point sets of the bone pin tip in optical tracking and electromagnetic tracking, respectively; unifying the position point sets of the bone pin tip in optical tracking and electromagnetic tracking to a same coordinate system, and constructing a position observation vector of the bone pin tip; and then, substituting the position observation vector into a position observation equation of the bone pin tip to calculate a motion state vector of the bone pin tip, achieving real-time tracking of the position of the bone pin tip. The method is applicable to complex-path and heavy-load surgery.

Description

Bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering Technical Field

[0001] The present invention belongs to the technical field of surgical navigation, and in particular relates to tracking of the position of a bone needle tip. Background Art

[0002] Pelvic fractures are severe, high-energy injuries with the highest disability and mortality rates of all fractures. Therefore, pelvic reduction robots require greater reduction forces and more complex reduction paths, which also impacts the accuracy of the navigation system. Furthermore, surgical instruments are prone to obstruction during surgery, making it difficult for traditional optical positioning methods for surgical navigation to meet this requirement.

[0003] Pelvic fracture reduction surgery is one of the most challenging orthopedic procedures, often referred to as "expert-level surgery" due to the complex anatomy of the pelvis and its deep location in the human body, making it difficult to visualize during surgery. Furthermore, fracture types are complex and diverse. In recent years, with the advancement of robotic-assisted minimally invasive surgery, the introduction of navigation systems has improved positioning accuracy, reduced instrument vibration, and enabled surgeons to perform the procedure with maximum precision. Surgical navigation allows surgeons to visualize the patient's internal structures in real time during surgery. During surgery, surgeons can use the maps created by this navigation system to more effectively plan the reduction path, ensuring greater accuracy and reducing the risk of damage to surrounding tissues. The navigation system also provides real-time feedback on instrument position and movement during surgery. Therefore, developing a precise navigation system for pelvic fracture reduction surgery is crucial for robotic pelvic reduction.

[0004] Navigation systems utilize different positioning methods to address varying surgical environments. Currently, surgical navigation systems utilize optical, electromagnetic, ultrasonic, and image-based positioning methods. Johns Hopkins University has developed a navigation robot system based on computed tomography (CT). The Robotics Institute at Harbin Institute of Technology has developed HIT-RAOS, which utilizes X-ray imaging for robot-assisted surgery. Germany has developed a navigation system based on optical tracking.

[0005] To date, optical and electromagnetic tracking systems represent the two primary technologies integrated into surgical navigation systems for computer-assisted image-guided surgery. Optical tracking systems offer high accuracy and environmental robustness, but require direct line of sight between the optical target and the camera. Electromagnetic tracking systems do not require line of sight between the sensor and the external reference, but the presence of ferromagnetic sources in space can significantly affect measurement results. Therefore, a single positioning method always has inevitable drawbacks, leading some researchers to adopt multi-sensor fusion methods to improve navigation accuracy. Xinyang Liu et al. developed a new method for tracking laparoscopic ultrasonic transducers during laparoscopic surgery by combining electromagnetic tracking technology and computer vision-based (e.g., ArUco) tracking methods through sensor fusion. The accuracy of the sensor tracking data obtained is better than that of using only an electromagnetic generator; Chao Shi et al. developed a new robotic system for pelvic fracture reduction, which achieved real-time 3D navigation technology for pelvic position by fusing optical tracking technology and non-rigid registration algorithm; Zhicheng Yang et al. proposed a method of using Kalman filtering to fuse optical tracking data and gyroscope sensor data. Therefore, using a gyroscope to estimate and supplement its direction, and performing data fusion through Kalman filtering can greatly improve tracking accuracy; Yongde Zhang et al. proposed fusing inertial navigation and electromagnetic navigation data using Kalman filtering to achieve a more accurate tracking technology for organ movement during surgery.

[0006] Compared to long bone reduction surgery, which only requires axial movement, pelvic bone pin reduction surgery involves a more complex path and greater reduction force. For example, if an optical navigation target is installed at the base of the traditional reduction pin for optical navigation, the complex path may cause the optical target to be obscured and lost during the reduction process, rendering the navigation system ineffective. Furthermore, the high reduction force causes significant deformation of the pin, resulting in the optical target at the end of the pin not being able to accurately reflect the position of the bone fragment.

[0007] Summary of the Invention

[0008] The present invention aims to solve the problems that a single electromagnetic positioning method has low accuracy and is interfered by magnetic fields, and a single optical positioning method has optical signals that are easily blocked, resulting in loss of tracking targets. A bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering is now provided.

[0009] The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering is as follows:

[0010] The position of the bone needle tip is tracked using optical sensors and electromagnetic sensors, and the position point sets of the bone needle tip during optical tracking and electromagnetic tracking are obtained respectively. The position point sets of the bone needle tip during optical tracking and electromagnetic tracking are unified into the same coordinate system, and the position observation vector of the bone needle tip is constructed. The position observation vector is then substituted into the position observation equation of the bone needle tip to calculate the motion state vector of the bone needle tip, thereby realizing real-time tracking of the bone needle tip position.

[0011] Furthermore, the optical sensor includes: a first group of optical target points 1, a second group of optical target points 5 and an optical tracking device 3; the electromagnetic sensor includes: an electromagnetic target point 2 and an electromagnetic tracking device 4; the first group of optical target points 1 and the electromagnetic target point 2 are relatively stationary arranged on the bone needle, and the second group of optical target points 5 is arranged on the electromagnetic tracking device 4; the tracking positions of the first group of optical target points 1 and the electromagnetic target point 2 are both registered to the position of the bone needle tip through rotational calibration, and the position of the bone needle tip is tracked in real time during the rotational calibration process, and the position of the bone needle tip is used to calculate the position point set of the bone needle tip during optical tracking and electromagnetic tracking, respectively.

[0012] Furthermore, the method for obtaining the position point set of the bone needle tip during optical tracking and electromagnetic tracking is:

[0013] The position point set of the bone needle tip in the OPM1 coordinate system when optically tracking the first set of optical targets 1 is calculated according to the following formula:

[0014] The position point set of the bone needle tip in the EM coordinate system during electromagnetic tracking of electromagnetic target 2 is calculated according to the following formula:

[0015] in, is the transformation matrix from OP coordinate system to OPM1 coordinate system, is the transformation matrix from OPM2 coordinate system to OP coordinate system, is the position point set of the spicule tip in the OPM2 coordinate system obtained during the rotation calibration process. is the transformation matrix from the EMS coordinate system to the EM coordinate system, is the position point set of the bone needle tip in the EMS coordinate system obtained during the rotation calibration process,

[0016] 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 group of optical target points 5, the OPM2 coordinate system represents the coordinate system of the first group of optical target points 1, the EM coordinate system represents the coordinate system of the electromagnetic tracking device 4, and EMS represents the coordinate system of the electromagnetic target point 2.

[0017] Furthermore, unifying the position points of the bone needle tip during optical tracking and electromagnetic tracking into the same coordinate system includes:

[0018] According to the following formula Unified to the OPM1 coordinate system:

[0019] in, is the transformation matrix from the EM coordinate system to the OPM1 coordinate system, It is the position point set of the bone needle tip in the OPM1 coordinate system when electromagnetic tracking is performed on electromagnetic target 2.

[0020] Furthermore, the least squares fitting registration method is used to calculate

[0021] Where n is the number of position points in the bone needle tip location point set, i = 1, 2, ..., n, C is the position error model coefficient, for The position information of the i-th position point in (x i ,y i ,z i )for The location information of the i-th location point in .

[0022] Furthermore, the position observation vector z(k) of the bone needle tip is expressed as: z(k)=[P OT(k) P ET(k) ] T ,

[0023] P OT(k) for The coordinates of the bone needle tip at time k, P ET(k) for The coordinates of the bone needle tip at time k.

[0024] Furthermore, the position observation equation of the bone needle tip is expressed as follows: z(k)=Hx(k)+v(k),

[0025] Where H is the observation matrix and I3 is the three-dimensional identity matrix, 03 is the three-dimensional zero matrix, v(k) is the measurement noise matrix, and x(k) is the motion state vector of the bone needle tip.

[0026] Furthermore, the measurement noise matrix v(k) obeys v(k)~N(0,R),

[0027] in, and are the root mean square errors of the first group of optical target 1 and electromagnetic target 2, respectively.

[0028] Furthermore, the motion state vector x(k) of the bone needle tip is expressed as:

[0029] Among them, P T(k) is the position coordinate of the bone needle tip tracked at time k, is the velocity of the spicule tip tracked at time k.

[0030] Furthermore, the optical sensor is a six-degree-of-freedom optical sensor, and the first group of optical targets 1 is located at the end of the bone needle; the electromagnetic sensor is a six-degree-of-freedom electromagnetic sensor, and the electromagnetic target 2 is located inside the bone needle and close to the needle tip.

[0031] The present invention first installs an optical sensor and an electromagnetic sensor on the bone needle, and then analyzes the error model of optical tracking and electromagnetic tracking. On this basis, the rotation calibration of the optical target and the electromagnetic target on the intelligent bone needle is completed, so that the optical six-dimensional sensor and the electromagnetic six-dimensional sensor can track the position of the bone needle tip at the same time. Then, the conversion relationship between the optical coordinate system and the electromagnetic coordinate system is obtained through point cloud registration, and the coordinate system of the surgical navigation system is unified. Finally, the electromagnetic and optical positioning information is fused using Kalman filtering to establish a positioning and navigation system based on electromagnetic and optical fusion of Kalman filtering. In addition, when the optical sensor is blocked and the optical target information is missing, the electromagnetic sensor is used to replace the optical sensor information for sensor fusion, so as to achieve the effect of robustly tracking the position of the bone needle tip in a complex surgical environment, so that this system can more accurately represent the posture of bone tissue in operations with complex paths and high loads. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] FIG1 is a schematic structural diagram of a bone needle according to a specific embodiment;

[0033] Figure 2 is a schematic diagram of error analysis, where (a) shows the tracking error of the optical tracking device as the angle changes, and (b) shows the tracking error of the electromagnetic tracking device as the distance changes;

[0034] Figure 3 is a schematic diagram of coordinate relations;

[0035] Figure 4 is a schematic diagram of coordinate system registration;

[0036] FIG5 is a schematic diagram of the experimental results, where (a) represents the Kalman filter experiment and (b) represents the optical occlusion experiment. DETAILED DESCRIPTION

[0037] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other in the absence of conflict.

[0038] This embodiment provides a 3D surgical navigation system that integrates optical positioning information and electromagnetic positioning information, specifically a bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering.

[0039] As shown in Figure 1, the material of the bone needle selected in this embodiment is 304 stainless steel, with a total length of 400mm and an outer diameter of 6mm, and is divided into two parts, the upper part is 365mm long, and a through hole with an inner diameter of 2.5mm is processed. The lower part is a bone needle drill bit, and the middle is connected with a threaded column with a diameter of 3mm. The electromagnetic target 2 of the 6-degree-of-freedom electromagnetic sensor is encapsulated in a heat shrink tube with an outer diameter equal to the inner diameter of the bone needle through hole. Then, the heat shrink tube is inserted from the end of the bone needle to the tip of the bone needle to reduce the position error of the needle tip position tracking caused by the deformation of the bone needle. The optical target of the 6-degree-of-freedom optical sensor is divided into two groups. The first group of optical target points 1 is fastened to the end of the bone needle by a connector, so that the bone needle integrates the electromagnetic target point 2 and the first group of optical target points 1, and can simultaneously collect the position tracking information of the two sensors. The second group of optical target points 5 of the 6-degree-of-freedom optical sensor is set on the electromagnetic tracking device 4.

[0040] The tracking error of the sensor is an important component of the error of the entire navigation system. The positioning data error of the optical target is related to the posture of the optical tracking device 3. In order to explore the posture of the first group of optical targets 1 when the positioning data error is minimized, the first group of optical targets 1 is fixed using an XYR three-axis displacement rotation platform. The first group of optical targets 1 is rotated at intervals of 5°, and the positioning error of the first group of optical targets 1 is recorded. The magnitude of the electromagnetic positioning error is affected by the distance between the electromagnetic target 2 and the electromagnetic tracking device 4. Since the precise positioning range of electromagnetic positioning technology is relatively small, it is easy to cause considerable positioning errors due to improper placement during actual use. Therefore, it is necessary to measure the relationship between the electromagnetic positioning error and the sensor placement position through experiments to provide a reference for subsequent experiments and the motion range of the electromagnetic sensor during actual use.

[0041] As shown in Figure 2, the optical navigation positioning system has a relatively low error when the first set of optical targets 1 and the optical tracking device 3 form an angle of approximately ±40°. This experimental result can provide a reference for the placement of optical sensors during surgery using optical positioning. However, for the electromagnetic target 2, the error increases as the electromagnetic target 2 moves farther from the center of the magnetic field generator in the YZ plane. Furthermore, as the distance in the Z direction increases, the positioning error of the electromagnetic target 2 increases significantly at all Y distances. Therefore, in subsequent experiments and actual use, the electromagnetic tracking device 4 should be used for tracking and positioning as close to the center of the magnetic field generator as possible.

[0042] The tracking positions of the first set of optical targets 1 and electromagnetic targets 2 are registered to the position of the bone needle tip by means of rotational calibration, so that the position of the bone needle tip is located at the same physical position during the optical tracking process and the electromagnetic tracking process. At the same time, the position of the bone needle tip is tracked in real time during the rotational calibration process. The principle of rotational calibration is: within the effective range of the optical positioning system, the bone needle tip is rotated 360° with the drill bit as the vertex in the conical hole of the calibration block. The angle between the rotation and the plumb line is between 30° and 60°. After the rotation is completed, the system automatically calculates the compensation value, root mean square error value, maximum error value and minimum deviation angle from the center of rotation to the optical passive rigid body coordinate system. After applying the compensation value, the position information output by the optical positioning system will not be the position information of the optical passive rigid body, but the position information of the bone needle tip. After 15 sets of calibration, the position point set of the bone needle tip in the OPM2 coordinate system (the coordinate system of the first set of optical targets 1) during the rotational calibration process is obtained. The accuracy (RMS error) is between 0.25-0.40mm. The calibration method of the electromagnetic positioning system is the same as that of the optical system. After 15 sets of calibration, the position point set of the bone needle tip in the EMS coordinate system (the coordinate system of the electromagnetic target point 2) during the rotation calibration process is obtained. The accuracy (RMS error) is around 0.50mm.

[0043] The position point set of the bone needle tip in the OPM1 coordinate system when optically tracking the first set of optical targets 1 is calculated according to the following formula:

[0044] The position point set of the bone needle tip in the EM coordinate system during electromagnetic tracking of electromagnetic target 2 is calculated according to the following formula:

[0045] in, is the transformation matrix from OP coordinate system to OPM1 coordinate system, is the transformation matrix from the OPM2 coordinate system to the OP coordinate system. and These are all conversion parameters calibrated in the optical sensor. It is the conversion matrix from the EMS coordinate system to the EM coordinate system, and is the conversion parameter calibrated in the electromagnetic sensor. 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 group of optical targets 5, and the EM coordinate system represents the coordinate system of the electromagnetic tracking device 4. In order to realize the hybrid tracking of the electromagnetic and optical positioning systems in the navigation system, after the tracking positions of the six-dimensional optical sensor and the six-dimensional electromagnetic sensor on the intelligent bone needle are registered to the position of the bone needle tip, it is necessary to unify the optical coordinate system and the electromagnetic coordinate system first, and obtain the conversion matrix from the optical coordinate system to the electromagnetic coordinate system before the next step of the fusion algorithm can be carried out. This embodiment calculates the conversion matrix from the EM coordinate system to the OPM1 coordinate system by the least squares fitting registration method. The core idea of ​​the least squares algorithm is to align two point sets through iterative optimization:

[0046] Where n is the number of position points in the bone needle tip location point set, i = 1, 2, ..., n, C is the position error model coefficient, for The position information of the i-th position point in (x i ,y i ,z i )for The position information of the i-th position point in [ 1 ]. Assuming a rigid body transformation (including translation and rotation) exists between the position point sets, the optimal rigid body transformation parameters are solved by minimizing the distance error between the two position point sets. Next, the position point sets after registration are transformed using the rotation matrix and the translation matrix. Finally, the root mean square error (RMS) of the transformation matrix is ​​calculated to evaluate the accuracy of the registration result, which is approximately 0.8 mm.

[0047] In this embodiment, the transformation matrix is ​​specifically:

[0048] Root mean square error RMSE = 1.371.

[0049] The position point set also contains some "bad points" with large measurement errors, which are affected by the accuracy of optical sensors and electromagnetic sensors. Therefore, the position point set registration effect is poor, resulting in a large root mean square error in the final transformation matrix. The "bad points" that do not overlap after registration can be clearly seen in the position point set registration diagram. In order to reduce the error of the transformation matrix obtained by registration, this embodiment screens out points with measurement errors greater than 0.2mm, and eliminates the "bad points" that obviously do not overlap after registration, and re-performs the registration experiment. As shown in Figure 4, the registration effect after removing the bad points is significantly better than the registration result when there are multiple data. Through the process of eliminating bad points, the root mean square error is reduced from 1.37 to 0.50. It meets the system usage requirements. According to the registration experiment of the electromagnetic coordinate system and the optical coordinate system, the transformation matrix from the optical reference coordinate system to the electromagnetic coordinate system is:

[0050] Finally, according to the following formula Unified to the OPM1 coordinate system:

[0051] in, is the transformation matrix from the EM coordinate system to the OPM1 coordinate system, It is the position point set of the bone needle tip in the OPM1 coordinate system when electromagnetic tracking is performed on electromagnetic target 2.

[0052] The Kalman filter algorithm for sensor fusion is a technique used in control theory and signal processing to combine measurements from multiple sensors to estimate the system's state. It is often used in navigation and robotics applications, where multiple data sources provide information about the system's position, velocity, and orientation. In this implementation, the Kalman filter algorithm fuses electromagnetic and optical sensor positioning data through prediction and update. The filter requires a process model for prediction based on process noise and a measurement equation for correction based on measurement noise.

[0053] Select the motion state vector of the bone needle tip at time k The motion state equation of the bone needle tip is constructed. The general form of the motion state equation of the bone needle tip is: x(k) = Ax(k-1) + w(k-1),

[0054] The specific form of the motion state equation of the bone needle tip is:

[0055] P T(k) 、P T(k-1) The position coordinates of the bone needle tip are tracked at time k and k-1 respectively. The movement speed of the bone needle tip is tracked at time k and k-1 respectively. is the acceleration of the bone needle tip tracked at time k, I3 is the three-dimensional unit matrix, 03 is the three-dimensional zero matrix, T s is the motion sampling period.

[0056] The uncertainty of the state equation of the bone needle tip is caused by the acceleration term. Assuming that the needle tip acceleration is is constant at each sampling time and is represented by an independent random variable that follows a zero-mean Gaussian distribution. in It is a diagonal matrix whose diagonal elements are the variances in the x, y, and z directions and the other elements are 0. Therefore, the acceleration term can be considered as process noise and obeys w(k)~N(0,Q),

[0057] In order to modify the state equation of the intelligent bone needle, the position observation vector z(k) = [P OT(k) P ET(k) ] T , P OT(k) for The coordinates of the bone needle tip at time k, P ET(k) for The coordinates of the bone needle tip at time k.

[0058] The observation equation for the position of the bone needle tip is as follows: z(k)=Hx(k)+v(k),

[0059] H is the observation matrix and v(k) is the measurement noise matrix and obeys v(k)~N(0,R), and are the root mean square errors of the first group of optical target 1 and electromagnetic target 2, respectively.

[0060] Unlike the process noise w(k), the measurement noise v(k) covariance matrix R changes at each time step.

[0061] This embodiment is based on a repositioning and positioning navigation method that integrates electromagnetic and optical fusion. It uses an optical tracking system (OTS) and an electromagnetic tracking system (EMTS) to locate the posture of surgical instruments in space. To address the issues of high optical accuracy but the need to ensure non-obstruction, and low electromagnetic accuracy and the influence of magnetic field distortion, a high-precision dynamic measurement method for in vivo and in vitro posture during fracture reduction that integrates electromagnetic and optical tracking technologies is proposed. First, the error models of the electromagnetic and optical tracking systems are analyzed, and the sensors are integrated into an intelligent bone needle and calibrated. Then, the optical and electromagnetic navigation systems are fused through Kalman filtering to achieve robust tracking of surgical instruments. The fused positioning of the optical tracking system (OTS) and the electromagnetic tracking system (EMTS) effectively compensates for sensor occlusion and provides continuous estimation of the instrument's posture.

[0062] Experiment A

[0063] The experiment used a calibrated bone spicule equipped with both an optical and electromagnetic positioning target. The optical target was affixed to the top of the spicule, while the electromagnetic target was affixed inside the spicule near the tip. The spicule was then affixed to a Siling Diana7 robotic arm. Based on previous error model analysis of optical and electromagnetic tracking devices, the angle between the optical target sphere and the binocular camera was set at approximately 45°. The range of motion of the robotic arm end and the electromagnetic target was set between 100 mm and 300 mm to ensure that both sensing systems were tested within their respective high-precision, effective tracking ranges.

[0064] The robotic arm pulls the bone needle in uniform linear motion in a plane perpendicular to the magnetic field generator (the YZ plane of the magnetic field generator's spatial coordinate system). To better simulate the vibrations during surgery, we added acceleration noise with a variance of 0.1 mm to the robotic arm's motion. The position of the bone needle tip during uniform motion was recorded by both the optical tracking system and the electromagnetic tracking system at a sampling rate of 20 Hz. Kalman filtering was used to process the data in real time and output the desired position coordinates. The result is shown in Figure 5(a).

[0065] Experiment B

[0066] Considering the high incidence of optical line of sight occlusion during actual surgery, a fusion of optical and electromagnetic sensors is proposed. Therefore, in the absence of one or more available optical marker data (e.g., in the case of line of sight occlusion), a sensor fusion algorithm is proposed that uses data from the EMTS to provide the missing optical data. This algorithm relies solely on the system model and available measurements. The latter is called a single-sensor fusion algorithm. Furthermore, an optical line of sight occlusion experiment, which is prone to occur during surgery, was designed. During the movement of the robotic arm pulling the bone needle, the optical target was artificially blocked twice, each occlusion lasting approximately 2 seconds, to test the reliability and stability of the algorithm. The experimental results are shown in Figure 5(b).

[0067] In Experiment A, the filtered curve shows good stability. When the spicule is close to the magnetic field generator (at a small z value), the electromagnetic sensor is more accurate, so the Kalman filter results tend to favor electromagnetic positioning. When the spicule is farther from the magnetic field generator (at a large z value), the optical sensor is more accurate, so the Kalman filter results gradually tend to favor optical positioning. Kalman filtering combines the more accurate sensor results with another positioning result to compensate for the desired needle tip position.

[0068] In Experiment B, by artificially blocking the optical target, a total of 31 sets of optical data were lost at a sampling frequency of 20 Hz, accounting for one-quarter of the experimental data. In the absence of optical sensor data, data from the EMTS was used to provide the missing optical data for the sensor fusion algorithm. The resulting Kalman filter curve remained relatively stable, with no loss of needle tip tracking or significant deviation from the true position, meeting experimental expectations.

[0069] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely illustrative of the principles and applications of the invention. It should be understood that many modifications may be made to the illustrative embodiments, and that other arrangements may be devised, without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in ways other than those described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be used in conjunction with other described embodiments.

Claims

1. A bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering, characterized in that: Specifically: The position of the bone needle tip is tracked by optical sensors and electromagnetic sensors, and the position point sets of the bone needle tip during optical tracking and electromagnetic tracking are obtained respectively. The position point sets of the bone needle tip during optical tracking and electromagnetic tracking are unified into the same coordinate system, and the position observation vector of the bone needle tip is constructed. Then, the position observation vector is substituted into the position observation equation of the bone needle tip to calculate the motion state vector of the bone needle tip, thereby realizing real-time tracking of the position of the bone needle tip.

2. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 1 is characterized in that: The optical sensor comprises: a first group of optical target points (1), a second group of optical target points (5) and an optical tracking device (3); the electromagnetic sensor comprises: an electromagnetic target point (2) and an electromagnetic tracking device (4); the first group of optical target points (1) and the electromagnetic target point (2) are relatively stationary arranged on the bone needle, and the second group of optical target points (5) is arranged on the electromagnetic tracking device (4); The tracking positions of the first group of optical target points (1) and electromagnetic target points (2) are registered to the position of the bone needle tip by means of rotational calibration, and the position of the bone needle tip is tracked in real time during the rotational calibration process. The position point set of the bone needle tip during optical tracking and electromagnetic tracking is calculated using the position of the bone needle tip.

3. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 2 is characterized in that: The method for obtaining the position point set of the bone needle tip during optical tracking and electromagnetic tracking is as follows: The position point set of the bone needle tip in the OPM1 coordinate system when optically tracking the first set of optical target points (1) is calculated according to the following formula: The position point set of the bone needle tip in the EM coordinate system during electromagnetic tracking of the electromagnetic target point (2) is calculated according to the following formula: in, is the transformation matrix from OP coordinate system to OPM1 coordinate system, is the transformation matrix from OPM2 coordinate system to OP coordinate system, is the position point set of the bone needle tip in the OPM2 coordinate system obtained during the rotation calibration process. is the transformation matrix from the EMS coordinate system to the EM coordinate system, is the position point set of the bone needle tip in the EMS coordinate system obtained during 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 group of optical target points (5), the OPM2 coordinate system represents the coordinate system of the first group of optical target points (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 target points (2).

4. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 3 is characterized in that: The method of unifying the position point set of the bone needle tip during optical tracking and electromagnetic tracking into the same coordinate system includes: According to the following formula Unified to the OPM1 coordinate system: in, is the transformation matrix from the EM coordinate system to the OPM1 coordinate system, It is the position point set of the bone needle tip in the OPM1 coordinate system when electromagnetically tracking the electromagnetic target point (2).

5. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 4 is characterized in that: The least squares fitting method was used to calculate Where n is the number of position points in the bone needle tip position point set, i = 1, 2, ..., n, C is the position error model coefficient, for The position information of the i-th position point in (x i ,y i ,z i )for The location information of the i-th location point in .

6. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 4 or 5, characterized in that: The position observation vector z(k) of the bone needle tip is expressed as: z(k)=[P OT(k) P ET(k) ] T , P OT(k) for The coordinates of the bone needle tip at time k, P ET(k) for The coordinates of the position of the bone needle tip at time k.

7. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 6 is characterized in that: The position observation equation of the bone needle tip is as follows: z(k)=Hx(k)+v(k), Where H is the observation matrix and I3 is the three-dimensional unit matrix, 03 is the three-dimensional zero matrix, v(k) is the measurement noise matrix, and x(k) is the motion state vector of the bone needle tip.

8. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 7 is characterized in that: The measurement noise matrix v(k) obeys v(k)~N(0,R), in, and are the root mean square errors of the first group of optical targets (1) and electromagnetic targets (2), respectively.

9. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 7, characterized in that: The expression of the motion state vector x(k) of the bone needle tip is: Among them, P T(k) is the position coordinate of the bone needle tip tracked at time k, is the movement speed of the bone needle tip tracked at time k.

10. The bone needle position tracking method based on optical and electromagnetic positioning and Kalman filtering according to claim 2, characterized in that: The optical sensor is a six-degree-of-freedom optical sensor, and the first group of optical targets (1) is located at the end of the bone needle; The electromagnetic sensor is a six-degree-of-freedom electromagnetic sensor, and the electromagnetic target point (2) is located inside the bone needle and close to the needle tip.

Images