A method and system for medical device visualized positioning and trajectory tracking

Abstract

The application discloses a medical device visual positioning and trajectory tracking method and system, specifically comprising: collecting UWB anchor point and device tag ranging and CIR data, combining a motion model to complete position prior prediction; constructing a channel quality factor based on a first path peak energy ratio and kurtosis to obtain an initial ranging weight; generating a multi-anchor subset solution candidate position and calculating geometric residuals, fusing position deviations and residuals to obtain a space-time geometric alignment score, and removing abnormal subsets and abnormal ranging according to the score; updating the data-driven filter to output the accurate position of the device; rendering the position sequence into a trajectory on a three-dimensional interface, and superimposing and displaying a covariance into a three-dimensional confidence ellipsoid to intuitively represent the positioning uncertainty, and realizing medical device positioning and visual supervision.

Description

Technical Field

[0001] This application belongs to the medical field, and in particular relates to a method and system for visual positioning and trajectory tracking of medical devices. Background Technology

[0002] In modern smart healthcare systems, ultra-wideband (UWB) technology has become one of the preferred technologies for achieving high-precision indoor positioning due to its nanosecond-level time resolution, high ranging accuracy, and strong resistance to multipath interference. In a typical UWB positioning system, multiple fixed anchor points are deployed in space, and the target is equipped with a UWB tag. The distance between the tag and each anchor point can be measured in real time, and the target's three-dimensional coordinates are then calculated using a multi-point positioning algorithm. However, in complex electromagnetic environments such as hospitals and operating rooms, factors like walls, metal equipment, and human movement can easily cause non-line-of-sight propagation of UWB signals and severe multipath effects, leading to unpredictable outliers in the ranging data that far exceed the normal error range. If these abnormal ranging data are not processed before being used for position calculation, they will severely contaminate the positioning results, causing drastic trajectory jumps, deviations from the true path, and even rendering the entire positioning system inoperable.

[0003] Data fusion filtering can smooth trajectories and suppress Gaussian white noise to some extent. However, when faced with non-Gaussian, large-amplitude anomalous data caused by NLOS, the filter's state estimation is severely interfered with, performance degrades sharply, and may even lead to filter divergence. Common methods for anomaly detection and removal include residual-based checks, such as receiver autonomous integrity monitoring, which detects inconsistent ranging values ​​by constructing redundant observation equations. However, a single anomaly may contaminate all residuals, resulting in low identification accuracy. Another type of method utilizes UWB channel impulse response data to distinguish between line-of-sight and non-line-of-sight channels by analyzing statistical characteristics. While this type of method can identify potential sources of anomalies at the physical layer, it typically only performs a coarse binary classification, failing to represent the degree of channel quality. Furthermore, in dynamic scenarios, setting a decision threshold solely based on CIR features is difficult and lacks stability. Summary of the Invention

[0004] This disclosure provides a method and system for visual positioning and trajectory tracking of medical devices.

[0005] In a first aspect, embodiments of this disclosure provide a method for visual positioning and trajectory tracking of medical devices, including: Acquire the current time-based ranging raw data and channel impulse response (CIR) data between multiple UWB anchor points deployed in the treatment space and the UWB tag on the target treatment device; based on the state estimation of the previous time, generate the prior device position and covariance at the current time through the prediction step of unscented Kalman filtering; For each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data and the kurtosis of the first path component are calculated. A channel quality factor is jointly determined based on the ratio and kurtosis as the initial weight of the ranging data. Based on the multiple UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weight, and the geometric residual is calculated. The deviation between the candidate device location and the prior device location is calculated, and the deviation and the geometric residual are fused through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset; based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, anchor point subsets related to one or more potential abnormal ranging data are identified and eliminated, thereby obtaining a set of verified ranging data. The ranging data is used as the measurement value of the unscented Kalman filter. The filter update step is performed to calculate the position coordinates of the diagnostic and treatment equipment at the current time and the updated position covariance matrix. In the three-dimensional visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a three-dimensional confidence ellipsoid for superimposed display.

[0006] Optionally, for each set of ranging data, the calculation of the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component, and the determination of a channel quality factor based on the ratio and kurtosis as the initial weight of the ranging data, includes: Extract the channel impulse response (CIR) data h(t) for each group, calculate the peak energy of the first path and the total signal energy, and take the ratio of the two as the energy ratio R. Calculate the kurtosis value K of the first path component; The initial weight w is calculated using the following formula: w=R×[1-exp(-α×max(0,K-3))], where exp is the natural exponential function and α is the adjustment coefficient.

[0007] Optionally, based on the plurality of UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weights, and the geometric residual is calculated, including: If there are N anchor points in the treatment space, then M anchor points are randomly or all of them selected from the N anchor points to generate one or more anchor point subsets. For any subset j of anchor points, a weighted least squares algorithm is used to solve for the candidate device locations of the subset using the M ranging values ​​corresponding to the subset and the initial weights. ; The candidate device location Corresponding geometric residuals Calculated using the following formula: ,in Let be the coordinates of the i-th anchor point within the subset. For the corresponding distance measurement value, These are the corresponding initial weights.

[0008] Optionally, the step of generating the prior device position and covariance at the current time based on the state estimation of the previous time step through an unscented Kalman filter includes: The state vector of the unscented Kalman filter is ,in For location, For velocity, the preset device motion model is a constant velocity model, and the state transition function is f(); Based on the posterior state estimate of the previous time step k-1 and covariance A set of Sigma points is generated through a scaling-free transformation; The Sigma points are propagated through the state transition function f() of the device motion model to obtain the predicted Sigma point set; By performing a weighted summation on the predicted Sigma point set, the prior state estimate at the current time k can be reconstructed. and prior covariance matrix ; Estimation from prior state Extracting positional components , which serves as the prior equipment position at the current moment.

[0009] Optionally, the step of calculating the deviation between the candidate device position and the prior device position, and fusing the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset, includes: Calculate the location of each candidate device Prior device position from the unscented Kalman filter prediction step The Mahalanobis distance, as the bias ; Geometric residuals are processed using a Gaussian kernel function. and deviation Merging into spatiotemporal geometric alignment fractions The calculation formula is: ,in and These are the preset control parameters for geometric and motion deviations.

[0010] Optionally, the step of identifying and removing anchor point subsets associated with one or more potentially anomalous ranging data based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, thereby obtaining a set of validated ranging data, includes: Calculate the spatiotemporal geometric alignment score for all anchor subsets. ; Based on fractions The distribution of the data determines a consistency score threshold. The score is lower than The anchor subset is marked as an inconsistent subset; For the ranging data obtained from each UWB anchor point, the number of times the data appears in the inconsistent subset is counted to form an inconsistency count; Ranging data with an inconsistency count exceeding a preset threshold is identified as abnormal data and removed from the current ranging dataset. The remaining ranging data constitutes the verified ranging data.

[0011] Optionally, the step of using the ranging data as the measurement value for unscented Kalman filtering, performing the filtering update step, and calculating the current position coordinates of the diagnostic and treatment equipment and the updated position covariance matrix includes: Combine the remaining ranging data into a measurement vector; Based on prior state estimation Sigma points are generated through a scaling-free transformation, and measurement predictions are performed on the Sigma points to obtain the predicted measurement values. Calculate the Kalman gain and update the prior state estimate by combining it with actual ranging data. By combining the prior covariance, we obtain the posterior state estimate at the current time step. and posterior covariance matrix ; From posterior state estimation Extracting positions As position coordinates, and from the posterior covariance matrix The corresponding 3x3 submatrix is ​​extracted as the covariance matrix for the position solution.

[0012] Optionally, in the 3D visualization interface, rendering the position coordinate sequence as a trajectory line and converting the updated covariance matrix of each coordinate point into a 3D confidence ellipsoid for overlay display includes: For each location coordinate point, the 3x3 covariance matrix P is decomposed into eigenvalues ​​to obtain three eigenvalues. , , and the corresponding feature vector , , ; In the 3D visualization interface, the location coordinates are the center; Create a three-dimensional ellipsoid such that the directions of the three principal axes of the ellipsoid align with the three eigenvectors. , , Alignment; The lengths of the three semi-axis of an ellipsoid are proportional to the square root of the corresponding eigenvalue.

[0013] Secondly, this disclosure also provides a medical device visualization positioning and trajectory tracking system, including the following modules: The generation module is used to acquire the current time ranging raw data and channel impulse response (CIR) data between multiple UWB anchor points deployed in the treatment space and the UWB tag on the target treatment device; based on the state estimation of the previous time, the prior device position and covariance at the current time are generated through the prediction step of unscented Kalman filtering. The calculation module is used to calculate, for each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component, and to jointly determine a channel quality factor based on the ratio and kurtosis as the initial weight of the ranging data; based on the multiple UWB anchor points, multiple anchor point subsets are generated, and for any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weight, and the geometric residual is calculated; The identification module is used to calculate the deviation between the candidate device position and the prior device position, and fuse the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset; based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, the module identifies and removes anchor point subsets related to one or more potential abnormal ranging data, thereby obtaining a set of verified ranging data. The conversion module is used to use the ranging data as the measurement value of the unscented Kalman filter, perform the filter update step, calculate the position coordinates of the diagnostic and treatment equipment at the current time and the updated position covariance matrix; in the three-dimensional visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a three-dimensional confidence ellipsoid for superimposed display.

[0014] Further, for each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component are calculated. A channel quality factor is then jointly determined based on the ratio and kurtosis as the initial weight of the ranging data, including: Extract the channel impulse response (CIR) data h(t) for each group, calculate the peak energy of the first path and the total signal energy, and take the ratio of the two as the energy ratio R. Calculate the kurtosis value K of the first path component; The initial weight w is calculated using the following formula: w=R×[1-exp(-α×max(0,K-3))], where exp is the natural exponential function and 3 is the theoretical value of the kurtosis of the Gaussian distribution.

[0015] Further, based on the multiple UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weights, and the geometric residual is calculated, including: If there are N anchor points in the treatment space, then M anchor points are randomly or all of them selected from the N anchor points to generate one or more anchor point subsets. For any subset j of anchor points, a weighted least squares algorithm is used to solve for the candidate device locations of the subset using the M ranging values ​​corresponding to the subset and the initial weights. ; The candidate device location Corresponding geometric residuals Calculated using the following formula: ,in Let be the coordinates of the i-th anchor point within the subset. For the corresponding distance measurement value, These are the corresponding initial weights.

[0016] Furthermore, the prediction step based on the state estimation of the previous time step, using unscented Kalman filtering, generates the prior device position and covariance at the current time step, including: The state vector of the unscented Kalman filter is ,in For location, For velocity, the preset device motion model is a constant velocity model, and the state transition function is f(); Based on the posterior state estimate of the previous time step k-1 and covariance A set of Sigma points is generated through a scaling-free transformation; The Sigma points are propagated through the state transition function f() of the device motion model to obtain the predicted Sigma point set; By performing a weighted summation on the predicted Sigma point set, the prior state estimate at the current time k can be reconstructed. and prior covariance matrix ; Estimation from prior state Extracting positional components , which serves as the prior equipment position at the current moment.

[0017] Further, the step of calculating the deviation between the candidate device position and the prior device position, and fusing the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset, includes: Calculate the location of each candidate device Prior device position from the unscented Kalman filter prediction step The Mahalanobis distance, as the bias ; Geometric residuals are processed using a Gaussian kernel function. and deviation Merging into spatiotemporal geometric alignment fractions The calculation formula is: ,in and These are the preset control parameters for geometric and motion deviations.

[0018] Further, based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, the anchor point subsets associated with one or more potentially anomalous ranging data are identified and removed, thereby obtaining a set of validated ranging data, including: Calculate the spatiotemporal geometric alignment score for all anchor subsets. ; Based on fractions The distribution of the data determines a consistency score threshold. The score is lower than The anchor subset is marked as an inconsistent subset; For the ranging data obtained from each UWB anchor point, the number of times the data appears in the inconsistent subset is counted to form an inconsistency count; Ranging data with an inconsistency count exceeding a preset threshold is identified as abnormal data and removed from the current ranging dataset. The remaining ranging data constitutes the verified ranging data.

[0019] Further, the step of using the ranging data as the measurement value for unscented Kalman filtering, performing the filtering update step, and calculating the current position coordinates of the diagnostic and treatment equipment and the updated position covariance matrix includes: Combine the remaining ranging data into a measurement vector; Based on prior state estimation Sigma points are generated through a scaling-free transformation, and measurement predictions are performed on the Sigma points to obtain the predicted measurement values. Calculate the Kalman gain and update the prior state estimate by combining it with actual ranging data. By combining the prior covariance, we obtain the posterior state estimate at the current time step. and posterior covariance matrix ; From posterior state estimation Extracting positions As position coordinates, and from the posterior covariance matrix The corresponding 3x3 submatrix is ​​extracted as the covariance matrix for the position solution.

[0020] Furthermore, in the 3D visualization interface, rendering the position coordinate sequence as a trajectory line and converting the updated covariance matrix of each coordinate point into a 3D confidence ellipsoid for overlay display includes: For each location coordinate point, the 3x3 covariance matrix P is decomposed into eigenvalues ​​to obtain three eigenvalues. , , and the corresponding feature vector , , ; In the 3D visualization interface, the location coordinates are the center; Create a three-dimensional ellipsoid such that the directions of the three principal axes of the ellipsoid align with the three eigenvectors. , , Alignment; The lengths of the three semi-axis of an ellipsoid are proportional to the square root of the corresponding eigenvalue.

[0021] This invention improves the accuracy of trajectory tracking for diagnostic and therapeutic devices by embedding a multi-stage anomaly removal mechanism within the iteration cycle of an unscented Kalman filter. Initial weighting of ranging values ​​using channel impulse response (CIR) data provides preliminary suppression of non-line-of-sight interference at the physical level. A spatiotemporal geometric alignment score, fusing bias and geometric residuals, is constructed. Compared to methods relying solely on geometry or constraints, this more accurately identifies and locates anomalous measurement sources, avoiding misjudgments of normal data. Anomaly subsets are removed based on the statistical distribution of this score, eliminating reliance on fixed thresholds and enhancing the stability and universality of the method in complex diagnostic and therapeutic environments. Only rigorously validated data is used to update the filter state, thus avoiding filter divergence and error accumulation caused by anomalous data, resulting in high-precision, high-smoothness device trajectories. By visualizing the position covariance matrix as a three-dimensional confidence ellipsoid, the uncertainty of the positioning result at each moment is intuitively displayed to the user. Attached Figure Description

[0022] Figure 1A flowchart of a medical device visualization positioning and trajectory tracking method provided in this application; Figure 2 This is a schematic diagram of anomaly ranging data identification based on inconsistency counting provided in this application. Detailed Implementation

[0023] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0024] See in this application. Figure 1 As shown, a method for visual positioning and trajectory tracking of medical devices includes the following steps: S1, acquire the current time ranging raw data and channel impulse response (CIR) data between multiple UWB anchor points deployed in the treatment space and the UWB tag on the target treatment device; Communicates with UWB transceiver chips such as the DW3000 series via SPI or UART interface, and completes a ranging exchange using the bilateral bidirectional ranging protocol, namely the TWR protocol. The function dwt_readrxtimestamp in the device driver is called to read the received timestamp to calculate the signal flight time, and multiplies it by the speed of light to obtain the raw ranging data. The function dwt_readiccir is called to read the channel impulse response accumulator to obtain a complex vector form of CIR data.

[0025] S2, Based on the state estimate of the previous time step, the prior device position and covariance at the current time step are generated through the prediction step of unscented Kalman filtering; Define a state vector X containing three-dimensional position and three-dimensional velocity, and adopt a constant velocity CV motion model. The state transition function is a linear equation. Use the posterior state from the previous time step. Covariance The covariance matrix is ​​decomposed using the Cholesky decomposition algorithm (Cholesky function) to generate 2n+1 Sigma points, where n is the dimension of the state vector. Each Sigma point is then substituted into the state transition function for propagation. By weighted summation of the propagated Sigma points, the prior state estimate for the current time step is obtained. With the prior covariance matrix .

[0026] In an optional embodiment, the step of generating the prior device position and covariance at the current time based on the state estimate of the previous time step through an unscented Kalman filter prediction step includes: S201: The state vector of the unscented Kalman filter is ,in For location, For velocity, the preset device motion model is a constant velocity model, and the state transition function is f(); S202: Posterior state estimation based on the previous time k-1 and covariance A set of Sigma points is generated through a scaling-free transformation; S203: Propagate the Sigma points through the state transition function f() of the device motion model to obtain the predicted Sigma point set; S204: Perform a weighted summation on the predicted Sigma point set to reconstruct the prior state estimate at the current time k. and prior covariance matrix ; S205: Estimation from prior state Extracting positional components , which serves as the prior equipment position at the current moment.

[0027] The state vector X defined in S201 is 6-dimensional. The constant-velocity CV motion model assumes that the device moves at a constant velocity between two time steps, and the discrete-time state transition equation is: , where F is the state transition matrix. If the system update period is... If the time is 0.1 seconds, then the F matrix is I is a 3x3 identity matrix. For process noise, used to represent the random variation of velocity, i.e., acceleration, the covariance matrix Q is usually set to be the same as the standard deviation of acceleration. Related, for example, taking =0.5m / s 2 In S202, 13 Sigma points are generated using an unscaling transformation, where n=6 is the dimension of the state vector. When generating Sigma points, transformation parameters need to be set; a typical preferred value is... =0.001, =2, =0.

[0028] Substitute the 13 Sigma points into the state transition function f() for forward propagation, i.e., multiply each Sigma point by the state transition matrix F on the left. Then, sum the 13 predicted Sigma points after propagation according to preset weights to obtain the prior state estimate for the current time step. Simultaneously, the weighted covariance of the difference between the predicted Sigma point and the weighted mean is calculated, and the process noise covariance Q is added to obtain the prior covariance matrix. Estimation from 6-dimensional prior states Extract the first three elements from the given data. , , As the position of the priori equipment at the current moment This position represents the best prediction of the current position based on historical movement trajectories.

[0029] S3, For each set of ranging data, calculate the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, as well as the kurtosis of the first path component, and determine a channel quality factor based on the ratio and kurtosis as the initial weight of the ranging data. For each set of CIR complex vectors, iterate through the vectors to calculate the squared amplitude of each sampling point, and find the maximum value as the first path peak energy; sum the squared amplitudes of all sampling points in the vector to obtain the total signal energy, and calculate the ratio R between the two; take a fixed-length window around the first path peak, extract the amplitude of all sampling points within the window, and use the kurtosis calculation formula in statistics or call the kurtosis function in the standard statistics library to calculate the kurtosis K of the amplitude data; calculate the initial weight w using the preset fusion formula.

[0030] In some embodiments, for each set of ranging data, calculating the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component, and jointly determining a channel quality factor based on the ratio and kurtosis as the initial weight of the ranging data, includes: S301: Extract the channel impulse response (CIR) data h(t) for each group and calculate the peak energy of the first path. With total signal energy The ratio of the two is taken as the energy ratio R; S302: Calculate the kurtosis value K of the first path component; S303: The initial weight w is calculated by the following formula: w=R×[1-exp(-α×max(0,K-3))], where exp is the natural exponential function and α is the adjustment coefficient.

[0031] The Channel Impulse Response (CIR) data output from the UWB receiver is processed. This data is typically a complex vector containing 10¹⁶ samples. The location of the first path is determined using a search algorithm, usually by finding the first peak point in the entire CIR signal that exceeds a preset noise threshold. The peak energy of the first path is then calculated. This is the square of the amplitude at the peak point. Total signal energy. This is the sum of the squares of the amplitudes of all sampling points in the CIR vector. Energy ratio The value of R is between 0 and 1. The closer the R value is to 1, the higher the probability that the signal is propagating at line of sight (LOS) and the better the channel quality. For example, an R value greater than 0.8 is generally considered to be a high-quality line-of-sight signal.

[0032] To calculate the kurtosis K of the first path component, a window centered on the peak of the first path needs to be extracted from the CIR data, for example, a subsequence containing a total of 16 sampling points. Kurtosis K is a measure of the peakness of the amplitude distribution of this subsequence, calculated using the standard fourth-order moment formula. R and K are then substituted into the weighting formula. In this formula, the R term represents the global concentration of signal energy, while the term within square brackets uses a nonlinear function to penalize the case where the local amplitude distribution approximates the theoretical kurtosis value of 3 for pure noise.

[0033] For an ideal line-of-sight signal, its energy is highly concentrated in a single peak within the extraction window, exhibiting strong non-Gaussianity and heavy-tailed characteristics. In this case, the K value is much greater than 3. When K is much greater than 3, the exponential term in the formula approaches 0, causing the multiplier within the square brackets to approach 1. The weight w is mainly determined by the high component R, thus retaining the high weight. Conversely, for a non-line-of-sight signal severely affected by multipath or noise interference, its energy is diffused within the window, and the amplitude distribution is closer to a chaotic Gaussian distribution, causing the K value to drop significantly and approach 3. At this point, the exponential term approaches 1, and the multiplier within the square brackets decreases sharply and approaches 0, thus achieving strong attenuation of poor-quality ranging data. For example, assuming the adjustment coefficient... When encountering a high-quality LOS signal, such as R=0.9, K=10.0, the weight w is approximately 0.87; while when encountering a poor-quality signal with severe multipath interference, such as R=0.4, K=3.5, the weight w drops to approximately 0.09.

[0034] S4. Based on the multiple UWB anchor points, generate multiple anchor point subsets. For any anchor point subset, use the ranging data contained in the subset and the corresponding initial weights to calculate the candidate device position and calculate the geometric residual. If the total number of anchor points is N, a combinatorial generation algorithm is used to traverse all subsets containing at least 4 anchor points. For each anchor point subset, a weighted nonlinear least squares problem is constructed, with the objective function being to minimize the sum of squares of the weighted ranging errors. This problem is solved using iterative optimization algorithms such as the Gauss-Newton method or the Levenberg-Marquardt method to obtain candidate device location coordinates. The distances calculated between this candidate location and the anchor points in the subset are compared with the corresponding original ranging values, and the weighted root mean square error (RMSE) is calculated as the geometric residual for that subset.

[0035] In one embodiment, based on the plurality of UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weights, and the geometric residual is calculated, including: S401: If there are N anchor points in the treatment space, then randomly or all of the M anchor points are selected from the N anchor points to generate one or more anchor point subsets. S402: For any subset of anchor points j, a weighted least squares algorithm is used to solve for the candidate device locations of the subset using the M ranging values ​​corresponding to the subset and the initial weights. ; S403: Location of the candidate device Corresponding geometric residuals Calculated using the following formula: ,in Let be the coordinates of the i-th anchor point within the subset. For the corresponding distance measurement value, These are the corresponding initial weights.

[0036] Assuming N=8 UWB anchor points are deployed within the treatment space, each anchor point subset needs to contain at least M=4 anchor points for 3D localization. To obtain sufficient redundancy information for anomaly detection, all possible combinations of M=4 anchor points can be generated, resulting in 70 anchor point subsets. For cases with larger values ​​of N, such as N>12, a fixed number of subsets, such as 100, can be generated through random sampling to reduce computational complexity. In S402, for each anchor point subset j, the known 3D coordinates of the 4 included anchor points are used... , , , , corresponding distance measurement value , , , and the initial weights determined by the channel quality factor. , , , The location of a candidate device is determined using the weighted least squares (WLS) algorithm. The objective is to minimize the weighted sum of squared residuals. Iterative optimization methods such as the Gauss-Newton method are preferred for solving the problem, with the final position at the previous moment as the initial value for iteration.

[0037] After obtaining candidate positions Then, immediately calculate the geometric residuals. This value is the root mean square weighted error, representing the candidate position. The degree of fit with the geometric constraints of each distance measurement value within this subset. A smaller value For example, a value less than 0.2 meters indicates that the ranging data within this subset has good geometric consistency and is likely to not contain outliers. Conversely, if A large value, such as more than 0.5 meters, strongly suggests that there are at least one or more anomalous ranging data in the subset, making it impossible to find a location point that can simultaneously satisfy all geometric constraints.

[0038] S5, calculate the deviation between the candidate device position and the prior device position, and fuse the deviation and the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset; For each subset of candidate device locations, the Mahalanobis distance between these locations and the prior device locations generated in the UKF prediction step is calculated using the prior covariance matrix. The inverse matrix of the corresponding positional component is used as the bias; a nonlinear fusion function is planned, such as the product of two exponential decay functions. ,in It is a geometric residual. It is the deviation, i.e., the Mahalanobis distance. and These are hyperparameters that control the shape of the function, and the spatiotemporal geometric alignment score is calculated from them.

[0039] In some embodiments of this application, the step of calculating the deviation between the candidate device position and the prior device position, and fusing the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset, includes: S501: Calculate the location of each candidate device Prior device position from the unscented Kalman filter prediction step The Mahalanobis distance, as the bias ; S502: Using Gaussian kernel function to transform geometric residuals and deviation Merging into spatiotemporal geometric alignment fractions The calculation formula is: ,in and These are the preset control parameters for geometric and motion deviations.

[0040] Optionally, when calculating the Mahalanobis distance, the weight matrix used for Mahalanobis distance calculation is the position submatrix in the prior covariance matrix generated by the unscented Kalman filter prediction step. The calculated bias... This indicates that the subset of anchor points Calculated candidate positions Compared with prior position predicted by motion model Consistency between them. Smaller The value indicates that the candidate position conforms to the device's motion characteristics. The fusion formula uses a Gaussian kernel function to transform the deviations between the geometric and motion dimensions into a unified score ranging from 0 to 1. .parameter and These are key adjustment factors, representing the tolerable ranges of geometric residuals and biases, respectively.

[0041] The optimal range of parameters can be set according to the actual application scenario. For example, in an indoor medical environment, where UWB ranging accuracy is high, geometric deviation control parameters can be set. The distance is 0.25 meters; the movement speed of diagnostic and treatment equipment is generally not fast, and the UKF prediction is relatively smooth, allowing for the setting of motion deviation control parameters. The value is 1.0. Under the above parameter settings, if the geometric residual of the candidate positions of a subset of anchor points is calculated... =0.15m, deviation =0.5, then the spatiotemporal geometric alignment fraction is ≈0.737, this score is relatively high, see reference. Figure 2 As shown, this indicates that the subset has good consistency. Conversely, if another subset contains outlier ranging data, it indicates poor consistency. =1.0m, =3.0, then the fraction is The extremely low score indicates a serious conflict within the subset.

[0042] S6. Based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, identify and remove anchor point subsets associated with one or more potential outlier ranging data, thereby obtaining a set of validated ranging data. Collect the spatiotemporal geometric alignment scores of all anchor subsets into a set; calculate the median and median absolute deviation (MAD) of this score set; set a dynamic threshold, for example... c is a constant, usually 2 or 3; all anchor subsets with scores below this threshold are identified as abnormal subsets and discarded; the UWB ranging data contained in all retained normal subsets are collected and duplicates are removed to obtain the ranging dataset at the current time.

[0043] In an optional embodiment, the step of identifying and removing anchor subsets associated with one or more potentially anomalous ranging data based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor subsets, thereby obtaining a set of validated ranging data, includes: S601: Calculate the spatiotemporal geometric alignment score for all anchor subsets. ; S602: Based on fractions The distribution of the data determines a consistency score threshold. The score is lower than The anchor subset is marked as an inconsistent subset; S603: For the ranging data obtained from each UWB anchor point, count the number of times the data appears in the inconsistent subset to form an inconsistency count; S604: Distance measurement data with an inconsistency count exceeding a preset threshold is identified as abnormal data, and the data is removed from the current distance measurement dataset. The remaining distance measurement data constitutes the verified distance measurement data.

[0044] After calculating the alignment score of all anchor subsets Then, determine the threshold. Specifically, find the maximum value among all scores. Set threshold ,in This is a scaling factor, preferably in the range of [0.4, 0.6]. For example, it can be taken as... =0.5, which can automatically adjust the judgment standard based on the overall signal quality at the current moment. All The subset j is classified as an inconsistent subset. An inconsistency counter is set for each of the N UWB anchor points, with an initial value of 0. All subsets marked as inconsistent are iterated over; for each inconsistent subset, the inconsistency counters corresponding to the M anchor points contained in that subset are incremented by 1.

[0045] A threshold for inconsistency counts is set to determine abnormal ranging. This threshold is related to the total number of subsets each anchor point participates in. For example, with an N=8, M=4 configuration, each anchor point will appear in 35 subsets. If an anchor point's ranging data is abnormal, then most of the subsets it participates in will exhibit inconsistency. Therefore, the frequency threshold can be set as a percentage of the total number of participations, such as 40%, or 14 times. The inconsistency counts of all anchor points are checked; any anchor point with a count exceeding 14 is considered abnormal and its corresponding ranging data is removed. The remaining set of unremoved ranging data constitutes the validated ranging data, used for subsequent filtering updates.

[0046] S7, Using the ranging data as the measurement value of the unscented Kalman filter, execute the filter update step to calculate the current position coordinates of the diagnostic and treatment equipment and the updated position covariance matrix; The nonlinear measurement function H(x) calculates the Mahalanobis distance to each anchor point based on the position information in the state vector X. Substituting the Sigma points generated in the prediction step into the measurement function H(x) yields a set of predicted measurement values. The mean and information covariance matrix of the predicted measurement values ​​are then calculated through weighted summation. and the cross-covariance matrix of state and measurement ; Calculate Kalman gain The posterior state is obtained by using the difference between the actual ranging data and the predicted mean (i.e., the innovation) and combining it with the Kalman gain to update the prior state estimate. The prior covariance matrix is ​​updated using Kalman gain and the innovation covariance matrix to obtain the posterior covariance matrix. .

[0047] In some embodiments, the step of using the ranging data as the measurement value of an unscented Kalman filter, performing a filtering update step, and calculating the current position coordinates of the diagnostic and treatment device and the updated position covariance matrix includes: S701: Combine the remaining ranging data into a measurement vector; S702: Based on prior state estimation Sigma points are generated through a scaling-free transformation, and measurement predictions are performed on the Sigma points to obtain the predicted measurement values. S703: Calculate the Kalman gain and update the prior state estimate by combining it with the actual ranging data. By combining the prior covariance, we obtain the posterior state estimate at the current time step. and posterior covariance matrix ; S704: Estimation from posterior state Extracting positions As position coordinates, and from the posterior covariance matrix The corresponding 3x3 submatrix is ​​extracted as the covariance matrix for the position solution.

[0048] Assuming that after anomaly removal, there are L remaining ranging data points. These constitute an L-dimensional measurement vector. The UKF update step reuses the prior Sigma point set generated in the prediction step. Each prior Sigma point, representing a hypothetical state, undergoes a nonlinear transformation using the measurement function H(). The measurement function H() is a function that maps a 6-dimensional state vector to an L-dimensional measurement vector, where the i-th component of the function... It calculates the position in the state vector. to the i-th anchor point The Mahalanobis distance is calculated. The predicted value is obtained by weighted summation of the transformed L-dimensional Sigma point set. and covariance In calculation At the same time, the measurement noise covariance matrix also needs to be added. , It is typically an L×L diagonal matrix, with the diagonal elements representing the variance of the UWB distance measurement, for example (0.15m). 2 .

[0049] Calculate the cross-covariance matrix of state and measurement. Calculate Kalman gain ,in It is the new information covariance matrix, equal to Using actual effective measurement vectors Compared with predicted measurement values The difference is the innovation, which corrects the prior state: At the same time, the covariance matrix is ​​also updated: From the updated 6-dimensional posterior state vector Extract the first three dimensions This serves as the three-dimensional position coordinates of the diagnostic and treatment equipment at the current moment. Simultaneously, it is derived from the updated 6x6 posterior covariance matrix. Extract a 3x3 submatrix from the top left corner; this submatrix is ​​the uncertainty covariance matrix calculated for the current position.

[0050] S8. In the three-dimensional visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a three-dimensional confidence ellipsoid for superimposed display.

[0051] Using a 3D graphics engine such as OpenGL, VTK, or Unity; storing the time-updated sequence of position coordinates in a vertex buffer object (VBO), and drawing it using GL_LINE_STRIP primitives to form a trajectory line. In each update cycle, the updated 6x6 covariance matrix is ​​used... Extract the 3x3 position covariance submatrix from the top left corner; decompose this 3x3 submatrix using the eigenvalue decomposition algorithm eig from a linear algebra library such as Eigen or NumPy to obtain eigenvalues ​​and corresponding eigenvectors. The eigenvectors form a rotation matrix to determine the ellipsoid's pose; use the chi-square distribution percentage function to obtain a scaling factor corresponding to a specific confidence level, multiply this factor by the square root of the eigenvalues ​​to obtain the lengths of the three semi-axis of the ellipsoid; generate a unit sphere model in the 3D scene, position it to the location coordinates through translation, rotation, and scaling transformations, and deform it according to the calculated pose and semi-axis lengths to render a semi-transparent 3D confidence ellipsoid.

[0052] In some embodiments of this application, rendering the position coordinate sequence as a trajectory line in the 3D visualization interface, and converting the updated covariance matrix of each coordinate point into a 3D confidence ellipsoid for overlay display, includes: S801: Perform eigenvalue decomposition on the 3x3 covariance matrix P for each location coordinate point to obtain three eigenvalues. , , and the corresponding feature vector , , ; S802: In the 3D visualization interface, the location coordinates are the center; S803: Create a three-dimensional ellipsoid such that the directions of the three principal axes of the ellipsoid align with the three eigenvectors. , , Alignment; S804: The lengths of the three semi-axis of the ellipsoid are proportional to the square root of the corresponding eigenvalues. For example, let's set it as s× ,s× and s× , where s is the preset confidence interval scaling factor.

[0053] The visualization process transforms the abstract covariance matrix into an intuitive geometric figure. The position covariance matrix obtained after updating the UKF is shown below. Perform eigenvalue decomposition. The resulting three orthogonal eigenvectors are... , , These represent the three principal directions of the uncertainty distribution, and the corresponding eigenvalues , , This represents the magnitude of the variance along the principal directions. The geometric center of the ellipsoid is placed at the calculated position coordinates, and the ellipsoid is rotated so that the three mutually perpendicular principal axes are aligned with the eigenvectors. , , Their directions coincide.

[0054] The size of the ellipsoid is determined by the length of its semi-axis, which represents the degree of uncertainty in the positioning. The scale factor controls the confidence interval represented by the ellipsoid. To visualize the 95% confidence region, the scale factor value should be determined based on a chi-square distribution with 3 degrees of freedom, i.e., 2.795. Therefore, the lengths of the three semi-axis of the ellipsoid are set as follows: , , The rendered ellipsoid's volume indicates the overall positioning accuracy, while its shape and orientation represent the anisotropy of uncertainty. For example, when available anchor points are sparsely distributed in a certain direction, the ellipsoid will stretch along that direction.

[0055] In addition to the medical device visualization positioning and trajectory tracking methods described in any of the other embodiments above, this application also provides a medical device visualization positioning and trajectory tracking system, including the following modules: The generation module is used to acquire the current time ranging raw data and channel impulse response (CIR) data between multiple UWB anchor points deployed in the treatment space and the UWB tag on the target treatment device; based on the state estimation of the previous time, the prior device position and covariance at the current time are generated through the prediction step of unscented Kalman filtering. The calculation module is used to calculate, for each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component, and to jointly determine a channel quality factor based on the ratio and kurtosis as the initial weight of the ranging data; based on the multiple UWB anchor points, multiple anchor point subsets are generated, and for any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weight, and the geometric residual is calculated; The identification module is used to calculate the deviation between the candidate device position and the prior device position, and fuse the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset; based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, the module identifies and removes anchor point subsets related to one or more potential abnormal ranging data, thereby obtaining a set of verified ranging data. The conversion module is used to use the ranging data as the measurement value of the unscented Kalman filter, perform the filter update step, calculate the position coordinates of the diagnostic and treatment equipment at the current time and the updated position covariance matrix; in the three-dimensional visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a three-dimensional confidence ellipsoid for superimposed display.

[0056] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this application.

[0057] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this patent application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. A method for visual positioning and trajectory tracking of medical devices, characterized in that, Includes the following steps: Acquire the current time-based ranging raw data and channel impulse response (CIR) data between multiple UWB anchor points deployed in the treatment space and the UWB tag on the target treatment device; based on the state estimation of the previous time, generate the prior device position and covariance at the current time through the prediction step of unscented Kalman filtering; For each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data and the kurtosis of the first path component are calculated, and a channel quality factor is jointly determined based on the ratio and kurtosis as the initial weight of the ranging data. Based on the multiple UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weights, and the geometric residual is calculated. The deviation between the candidate device location and the prior device location is calculated, and the deviation and the geometric residual are fused through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset; based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, anchor point subsets related to one or more potential abnormal ranging data are identified and eliminated, thereby obtaining a set of verified ranging data. The ranging data is used as the measurement value of the unscented Kalman filter. The filter update step is performed to calculate the position coordinates of the diagnostic and treatment equipment at the current time and the updated position covariance matrix. In the three-dimensional visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a three-dimensional confidence ellipsoid for superimposed display.

2. The method according to claim 1, characterized in that, For each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component are calculated. A channel quality factor is then jointly determined based on the ratio and kurtosis as the initial weight of the ranging data, including: Extract the channel impulse response (CIR) data h(t) for each group, calculate the peak energy of the first path and the total signal energy, and take the ratio of the two as the energy ratio R. Calculate the kurtosis value K of the first path component; The initial weight w is calculated using the following formula: w=R×[1-exp(-α×max(0,K-3))], where exp is the natural exponential function and α is the adjustment coefficient.

3. The method according to claim 1, characterized in that, Based on the multiple UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weights, and the geometric residual is calculated, including: If there are N anchor points in the treatment space, then M anchor points are randomly or all of them selected from the N anchor points to generate one or more anchor point subsets. For any subset j of anchor points, a weighted least squares algorithm is used to solve for the candidate device locations of the subset using the M ranging values ​​corresponding to the subset and the initial weights. ; The candidate device location Corresponding geometric residuals Calculated using the following formula: ,in Let be the coordinates of the i-th anchor point within the subset. For the corresponding distance measurement value, These are the corresponding initial weights.

4. The method according to claim 1, characterized in that, The state estimation based on the previous time step, through the prediction step of unscented Kalman filtering, generates the prior device position and covariance at the current time step, including: The state vector of the unscented Kalman filter is ,in For location, For velocity, the preset device motion model is a constant velocity model, and the state transition function is f(); Based on the posterior state estimate of the previous time step k-1 and covariance A set of Sigma points is generated through a scaling-free transformation; The Sigma points are propagated through the state transition function f() of the device motion model to obtain the predicted Sigma point set; By performing a weighted summation on the predicted Sigma point set, the prior state estimate at the current time k can be reconstructed. and prior covariance matrix ; Estimation from prior state Extracting positional components , which serves as the prior equipment position at the current moment.

5. The method according to claim 4, characterized in that, The step of calculating the deviation between the candidate device position and the prior device position, and fusing the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score for each anchor point subset, includes: Calculate the location of each candidate device The Mahalanobis distance from the prior device position obtained from the unscented Kalman filter prediction step is used as the bias. ; Geometric residuals are processed using a Gaussian kernel function. and deviation Merging into spatiotemporal geometric alignment fractions The calculation formula is: ,in and These are the preset control parameters for geometric and motion deviations.

6. The method according to claim 1, characterized in that, The process involves identifying and removing anchor point subsets associated with one or more potentially anomalous ranging data based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, thereby obtaining a set of validated ranging data, including: Calculate the spatiotemporal geometric alignment score for all anchor subsets. ; Based on fractions The distribution of the data determines a consistency score threshold. The score is lower than The anchor subset is marked as an inconsistent subset; For the ranging data obtained from each UWB anchor point, the number of times the data appears in the inconsistent subset is counted to form an inconsistency count; Ranging data with an inconsistency count exceeding a preset threshold is identified as abnormal data and removed from the current ranging dataset. The remaining ranging data constitutes the verified ranging data.

7. The method according to claim 4, characterized in that, The step of using the ranging data as the measurement value for unscented Kalman filtering, performing the filtering update step, and calculating the current position coordinates of the diagnostic and treatment equipment and the updated position covariance matrix includes: Combine the remaining ranging data into a measurement vector; Based on prior state estimation Sigma points are generated through a scaling-free transformation, and measurement predictions are performed on the Sigma points to obtain the predicted measurement values. Calculate the Kalman gain and update the prior state estimate by combining it with actual ranging data. By combining the prior covariance, we obtain the posterior state estimate at the current time step. and posterior covariance matrix ; From posterior state estimation Extracting positions As position coordinates, and from the posterior covariance matrix The corresponding 3x3 submatrix is ​​extracted as the covariance matrix for the position solution.

8. The method according to claim 1, characterized in that, In the 3D visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a 3D confidence ellipsoid for overlay display, including: For each location coordinate point, the 3x3 covariance matrix P is decomposed into eigenvalues ​​to obtain three eigenvalues. , , and the corresponding feature vector , , ; In the 3D visualization interface, the location coordinates are the center; Create a three-dimensional ellipsoid such that the directions of the three principal axes of the ellipsoid align with the three eigenvectors. , , Alignment; The lengths of the three semi-axis of an ellipsoid are proportional to the square root of the corresponding eigenvalue.

9. A visual positioning and trajectory tracking system for medical devices, characterized in that, Includes the following modules: The generation module is used to acquire the current time ranging raw data and channel impulse response (CIR) data between multiple UWB anchor points deployed in the treatment space and the UWB tag on the target treatment device; based on the state estimation of the previous time, the prior device position and covariance at the current time are generated through the prediction step of unscented Kalman filtering. The calculation module is used to calculate the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to each set of ranging data, as well as the kurtosis of the first path component, and to jointly determine a channel quality factor based on the ratio and kurtosis as the initial weight of the ranging data. Based on the multiple UWB anchor points, multiple anchor point subsets are generated. For any anchor point subset, the candidate device position is calculated using the ranging data contained in the subset and the corresponding initial weights, and the geometric residual is calculated. The identification module is used to calculate the deviation between the candidate device position and the prior device position, and fuse the deviation with the geometric residual through a preset nonlinear function to obtain the spatiotemporal geometric alignment score of each anchor point subset; based on the statistical distribution of the spatiotemporal geometric alignment score set of all anchor point subsets, the module identifies and removes anchor point subsets related to one or more potential abnormal ranging data, thereby obtaining a set of verified ranging data. The conversion module is used to use the ranging data as the measurement value of the unscented Kalman filter, perform the filter update step, calculate the position coordinates of the diagnostic and treatment equipment at the current time and the updated position covariance matrix; in the three-dimensional visualization interface, the position coordinate sequence is rendered as a trajectory line, and the updated covariance matrix of each coordinate point is converted into a three-dimensional confidence ellipsoid for superimposed display.

10. The system according to claim 9, characterized in that, For each set of ranging data, the ratio of the peak energy of the first path to the total signal energy of the CIR data corresponding to the ranging data, and the kurtosis of the first path component are calculated. A channel quality factor is then jointly determined based on the ratio and kurtosis as the initial weight of the ranging data, including: Extract the channel impulse response (CIR) data h(t) for each group, calculate the peak energy of the first path and the total signal energy, and take the ratio of the two as the energy ratio R. Calculate the kurtosis value K of the first path component; The initial weight w is calculated using the following formula: w=R×[1-exp(-α×max(0,K-3))], where exp is the natural exponential function and α is the adjustment coefficient.

Images