A spatial force, position, and attitude fusion calibration method and related equipment

By combining a gyroscope and an active light source board in a handheld mechanical surgical device, high-precision and real-time calibration of spatial pose was achieved, solving the problems of inconsistent accuracy and insufficient real-time performance of handheld mechanical surgical devices in surgical navigation, and significantly improving surgical accuracy and stability.

CN122312754BActive Publication Date: 2026-11-17CENT SOUTH UNIV
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202610789803.2
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2026-06-03
Publication Date
2026-11-17
Estimated Expiration
2046-06-03

AI Technical Summary

Technical Problem

In surgical navigation, existing technologies for spatial pose tracking of handheld mechanical surgical devices suffer from inconsistent accuracy, insufficient real-time performance, and continuity issues. In particular, optical obstruction or IMU drift leads to inaccurate positioning and hand-eye asynchrony.

Method used

Using a rigidly mounted gyroscope and an active light source board, the system decodes and corrects extrinsic parameters by acquiring the light spot emitted by the active light source board. Combined with IMU pre-integration and tightly coupled optical pose estimation, the system compensates for delay in real time and achieves spatial pose fusion calibration.

Benefits of technology

It improves the continuity and accuracy of spatial pose tracking of handheld mechanical surgical devices, reduces trajectory breaks, reduces positioning lag due to hand-eye asynchrony, and ensures high precision and real-time performance.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN122312754B_ABST
    Figure CN122312754B_ABST
Patent Text Reader

Abstract

This invention belongs to the field of surgical device pose calibration technology. It provides a spatial force, position, and attitude fusion calibration method and related equipment. The method includes: acquiring and decoding light points emitted by an active light source board according to AF-LED encoding to obtain two-dimensional image points; performing calculations and extrinsic parameter self-correction based on the two-dimensional image points to obtain the optical pose; acquiring the acceleration and angular velocity of a gyroscope, and using IMU pre-integration and optical pose tight coupling estimation based on the acceleration and angular velocity to obtain the estimation result; calculating the total delay based on the estimation result, and performing real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration result of the handheld mechanical surgical device. The beneficial effect of this invention is that it improves the accuracy of the spatial pose calibration of the handheld mechanical surgical device.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of surgical equipment posture calibration technology, and in particular to a spatial force, position and posture fusion calibration method and related equipment. Background Technology

[0002] In surgical navigation, the accuracy of spatial pose tracking of surgical equipment (such as dental handpieces) depends on the accuracy of the positioning device. At the same time, because different doctors use different hand force when operating the hand-held mechanical surgical device, the actual position of the surgical device is inconsistent with the position in the surgical navigation image. For example, when the hand-held mechanical surgical device enters the surgical object (such as teeth or body tissue), the acceleration caused by the resistance will make the image show that it has entered the surgical object more deeply, while in reality it only touches or enters shallowly.

[0003] Existing technologies for solving the above-mentioned technical problems (such as standalone active emitters or IMUs) have the following drawbacks: continuity, optics may lose points or be interrupted due to occlusion / glare / distortion; although IMUs are continuous, tilt angle jumps when approaching a vertical posture, accelerator noise is large, and geomagnetic interference causes drift; accuracy, optics have high single-shot accuracy but are affected by distortion, assembly micro-deformation and misnumbering, while IMU noise and zero bias superposition affect attitude / gravity estimation; real-time performance, image and fusion calculation introduce delays, which will cause hand-eye asynchrony during surgery if not compensated. Summary of the Invention

[0004] In order to at least solve one of the technical problems existing in the prior art, the present invention provides a spatial force, position and attitude fusion calibration method and related equipment.

[0005] One aspect of the present invention provides a spatial force, position, and attitude fusion calibration method for a handheld biomechanical surgical device, the handheld biomechanical surgical device being equipped with a rigidly mounted gyroscope and an active light source board, comprising: The light points emitted by the active light source board according to the AF-LED encoding are acquired and decoded to obtain two-dimensional image points; The optical pose is obtained by solving and self-correcting extrinsic parameters based on the two-dimensional image points. The acceleration and angular velocity of the gyroscope are obtained, and the IMU pre-integration and optical pose are used for tightly coupled estimation based on the acceleration and angular velocity to obtain the estimation result; The total delay is calculated based on the estimation results, and real-time forward extrapolation compensation is performed based on the total delay to obtain the spatial pose fusion calibration results of the handheld mechanical surgical device. The spatial pose fusion calibration results include position, attitude, position accuracy, and attitude accuracy.

[0006] According to the aforementioned spatial force, position, and attitude fusion calibration method, the light points emitted by the active light source board according to the AF-LED encoding are acquired and decoded to obtain two-dimensional image points, including: Each dot in the active light-emitting dot matrix emits light using a dual-slot frequency, where the dual-slot frequency is expressed as: ; in, Indicates two frequency bands and , Represents the number of frames in a period and ;frequency The number of frame periods is ,frequency The number of frame periods is ,and ,frequency and frequency The duration is , For superframe length; combined Use coprime arrays or arrays with low common factors, and combine them. The frequency signature is unique and the decoding short window is robust; Perform synchronization or DC protection on each point at the beginning of the superframe, and add 4 phases to each point during slot switching. The gray code mapping, and the staggered lighting of each point between neighboring points according to the scene brightness; And, it also includes AF-LED decoding: The sliding window and Barker-7 correlation are used to determine the different slots based on the maximum correlation. Short-window frequency estimation is used to estimate the autocorrelation peak distance of binary switch sequences in different slots, and the discrimination is made by cross-window phase consistency. The decoded frame number and phase are matched against nearest neighbors in the signature list; When frame drops / exposure flicker occur, a Viterbi search is performed based on the cost.

[0007] According to the aforementioned spatial force, position, and attitude fusion calibration method, the optical pose is obtained by solving and self-calibrating extrinsic parameters based on the two-dimensional image points, including: For the centroid of the two-dimensional image points in each frame Perform anti-distortion and with known configuration points Solving PnP yields the optimized optical pose. for: ; in Indicates in The transformation matrix from the camera coordinate system to the world coordinate system at any moment. for The transformation matrix from the world coordinate system to the rigid body coordinate system at any time. for Observation noise at any given moment This is the index of the centroid of a point in a two-dimensional image. Indicates the light source The centroid pixel coordinates on the image at any given time. This indicates a transformation from rigid body coordinates to world coordinates; If the light source lines of each frame of the two-dimensional image are insufficient, degradation constraints on lines or surfaces are applied to optimize the process. External parameters are normalized using zero mean and small covariance to perform online estimation to compensate for assembly dimension differences or thermal deformation.

[0008] According to the aforementioned spatial force, position, and attitude fusion calibration method, the acceleration and angular velocity of the gyroscope are obtained, the IMU pre-integration is determined based on the acceleration and angular velocity, and a tightly coupled estimation is performed based on the IMU pre-integration and optical pose to obtain the estimation result, including: The rotation increment is obtained by integrating the acceleration and angular velocity using IMU pre-integration within a time window. Speed ​​increment and position increment ; Based on rotation increment Speed ​​increment and position increment Perform rotation matrix and speed The update process is represented as follows: ; ; ; in, Angular velocity, For gyroscope bias It is the acceleration due to gravity. For acceleration, For acceleration bias, where the IMU pre-integrates Keep unchanged during updates; Intermittent correction of tight coupling: ; in, Indicates time The optimal pose estimate obtained by tight coupling optimization. This indicates a transformation from world coordinates to rigid body coordinates. This represents a Gaussian noise model with zero mean. For covariance, This describes the uncertainty of the optimized position factor. According to the spatial force, position, and attitude fusion calibration method, the total delay is calculated based on the estimation results, and real-time forward extrapolation compensation is performed based on the total delay to obtain the spatial pose fusion calibration result of the handheld mechanical surgical device, including: Get total latency ; Based on total delay Real-time extrapolation, attitude extrapolation, and uncertainty propagation are performed to obtain the spatial pose fusion calibration results of the handheld biomechanical surgical device, where the position extrapolation is: ; Position extrapolation is: ; Uncertainty propagation is as follows: ; ; in, For location, As a posture.

[0009] According to the aforementioned spatial force, position, and attitude fusion calibration method, the method further includes: The original magnetic vector of the gyroscope is obtained. After normalization, at least one of the following detection methods is performed: magnitude anomaly, direction abrupt change, magnetic-visual consistency, and chi-square threshold. The detection results are obtained. If the detection result is abnormal, the magnetic heading is shielded. If the magnetic heading is abnormal, the heading is maintained by IMU integration, and the noise of the gyroscope zero bias process and the heading process are increased. If the magnetic heading recovers from abnormal to normal, the original magnetic vector is recovered using a smooth reintroduction method, which includes zero linear recovery or exponential interpolation recovery. If there is a constant deviation in the magnetic heading, it is estimated by setting the heading bias parameter.

[0010] According to the aforementioned spatial force, position, and attitude fusion calibration method, the method further includes: Optical defects are detected by reprojection error based on the number of visible light sources; If optical defects exist, the noise in the process is adaptively amplified based on angular velocity and acceleration. If optical restoration is required, the following correction process should be performed: For each light source that passes the consistency test, a Huber / Geman-McClure robust kernel is used to suppress occasional bad pixels, and then a Mahalanobis distance threshold is used to remove outliers from occasional bad pixels. Weighted correction is performed on the optical pose observations within the filter, and then a smooth transition is achieved using a short window SLEEP and linear interpolation. External parameters were first verified and then fine-tuned. Recovery is performed using an RTS filter smoothing window.

[0011] Another aspect of the present invention provides a spatial force, position, and attitude fusion calibration device, comprising: The first module is used to acquire and decode the light points emitted by the active light source board according to the AF-LED encoding to obtain two-dimensional image points; The second module is used to perform calculations and extrinsic parameter self-correction based on the two-dimensional image points to obtain the optical pose. The third module is used to obtain the acceleration and angular velocity of the gyroscope. Based on the acceleration and angular velocity, IMU pre-integration and optical pose are used for tightly coupled estimation to obtain the estimation result. The fourth module is used to calculate the total delay based on the estimation results and perform real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration results of the handheld mechanical surgical device. The spatial pose fusion calibration results include position, attitude, position accuracy, and attitude accuracy.

[0012] Another aspect of the present invention provides an electronic device, including a processor and a memory; The memory is used to store programs; The processor executes the program to implement the method as described above.

[0013] This invention also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium and execute the computer instructions, causing the computer device to perform the methods described above.

[0014] The beneficial effects of this invention are as follows: Continuity: Trajectory is maintained by IMU pre-integration during optical occlusion; strong convergence with tight coupling after recovery significantly reduces trajectory breakage; Accuracy: Online fine-tuning of extrinsic parameters + 3σ position / attitude accuracy derived from covariance, and quality score Q make reliability visible and traceable; Real-time performance: Explicit measurement delay and forward extrapolation reduce positioning lag caused by hand-eye asynchrony; Robust coding: Barker-7 synchronization + coprime frequency set + HMM error suppression, can still correctly number frames even with 1-2 missing frames at 100Hz, significantly reducing the misclassification rate. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of a handheld mechanical surgical device according to an embodiment of the present invention; Figure 2 This is a diagram of the spatial force, position, and attitude fusion calibration structure according to an embodiment of the present invention; Figure 3 This is an installation structure diagram of the handheld mechanical surgical device and the spatial force, position and posture fusion calibration according to an embodiment of the present invention; Figure 4 This is a structural diagram of the active optical sensor mount according to an embodiment of the present invention; Figure 5 This is a schematic diagram of the spatial force, position, and attitude fusion calibration process according to an embodiment of the present invention; Figure 6 This is a schematic diagram of a spatial force, position, and attitude fusion calibration device according to an embodiment of the present invention; Reference numerals: 10-Mechanical surgical device; 20-Active light source board; 30-Gyroscope; 40-Active optical sensor mount. Detailed Implementation

[0016] The embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings. Throughout the description, the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions. In the following description, suffixes such as "module," "part," or "unit" used to denote elements are used only for the purpose of illustrative purposes and have no specific meaning in themselves. Therefore, "module," "part," or "unit" can be used interchangeably. Terms such as "first," "second," etc., are used only to distinguish technical features and should not be construed as indicating or implying relative importance, or implicitly indicating the number of indicated technical features, or implicitly indicating the sequential relationship of the indicated technical features. In the following description, the consecutive reference numerals for method steps are for ease of review and understanding. Adjusting the implementation order of steps, in conjunction with the overall technical solution of the present invention and the logical relationship between the various steps, will not affect the technical effect achieved by the technical solution of the present invention. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0017] refer to Figure 1 ,in Figure 1 The schematic diagram of the handheld biomechanical surgical device (dental handpiece) includes a biomechanical surgical device 10, a gyroscope (IMU, nine-axis biomechanical sensor) 30, and an active light source board 20, wherein the gyroscope 30 and the active light source board 20 are rigidly mounted on the top of the biomechanical surgical device 10.

[0018] like Figure 2 As shown, Figure 2This is a diagram of a spatial force, position, and attitude fusion calibration structure according to an embodiment of the present invention. The spatial force, position, and attitude fusion calibration structure includes an active light source board 20, a gyroscope 30, and an active optical sensor mount 40. The active light source board 20 is mounted on the active optical sensor mount 40. The gyroscope 30 is mounted on the other end of the active optical sensor mount 40 and connected to the mechanical surgical device 10. The mounting method is as follows: Figure 3 Its structural reference Figure 4 .

[0019] In some embodiments, reference Figure 2 The schematic diagram of the spatial force, position, and attitude fusion calibration process shown includes, but is not limited to, steps S100~S400: S100: Acquire and decode the light points emitted by the active light source board according to the AF-LED encoding to obtain two-dimensional image points; In some embodiments, each point in the active light-emitting dot matrix emits light using a dual-slot frequency, wherein the dual-slot frequency is expressed as: ; in, Indicates two frequency bands and , Represents the number of frames in a period and ;frequency The number of frame periods is ,frequency The number of frame periods is ,and ,frequency and frequency The duration is , For superframe length; combined Use coprime arrays or arrays with low common factors, and combine them. The frequency signature is unique and the decoding short window is robust; Perform synchronization or DC protection on each point at the beginning of the superframe, and add 4 phases to each point during slot switching. The gray code mapping, and the staggered lighting of each point between neighboring points according to the scene brightness; And, it also includes AF-LED decoding: The sliding window and Barker-7 correlation are used to determine the different slots based on the maximum correlation. Short-window frequency estimation is used to estimate the autocorrelation peak distance of binary switch sequences in different slots, and the discrimination is made by cross-window phase consistency. The decoded frame number and phase are matched against nearest neighbors in the signature list; When frame drops / exposure flicker occur, a Viterbi search is performed based on the cost.

[0020] S200 calculates and self-calibrates extrinsic parameters based on two-dimensional image points to obtain the optical pose.

[0021] In some embodiments, the centroid of each frame of two-dimensional image points Perform anti-distortion and with known configuration points Solving PnP yields the optimized optical pose. for: ; in Indicates in The transformation matrix from the camera coordinate system to the world coordinate system at any moment. for The transformation matrix from the world coordinate system to the rigid body coordinate system at any time. for Observation noise at any given moment The index of the centroid of a point in a two-dimensional image; Indicates the light source The centroid pixel coordinates on the image at any given time. This indicates a transformation from rigid body coordinates to world coordinates; If the light source lines in each frame of a 2D image are insufficient, degradation constraints on lines or surfaces are applied to optimize the process. External parameters are normalized using zero mean and small covariance to perform online estimation to compensate for assembly dimension differences or thermal deformation.

[0022] S300 acquires the acceleration and angular velocity of the gyroscope, and performs tight-coupled estimation based on the acceleration and angular velocity using IMU pre-integration and optical pose to obtain the estimation result.

[0023] In some embodiments, the rotation increment is obtained by integrating the acceleration and angular velocity using IMU pre-integration within a time window. Speed ​​increment and position increment ; Based on rotation increment Speed ​​increment and position increment Perform rotation matrix and speed The update process is represented as follows: ; ; ; in, Angular velocity, For gyroscope bias It is the acceleration due to gravity. For acceleration, For acceleration bias, where the IMU pre-integrates Keep unchanged during updates; Intermittent correction of tight coupling: ; in, Indicates time The optimal pose estimate obtained by tight coupling optimization. This indicates a transformation from world coordinates to rigid body coordinates. This represents a Gaussian noise model with zero mean. For covariance, Used to describe the uncertainty of the position factor obtained through optimization.

[0024] S400 calculates the total delay based on the estimation results and performs real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration results of the handheld mechanical surgical device. The spatial pose fusion calibration results include position, attitude, position accuracy, and attitude accuracy.

[0025] In some embodiments, the total latency is obtained. ; Based on total delay Real-time extrapolation, attitude extrapolation, and uncertainty propagation are performed to obtain the spatial pose fusion calibration results of the handheld biomechanical surgical device, where the position extrapolation is: ; Position extrapolation is: ; Uncertainty propagation is as follows: ; ; in, For location, As a posture.

[0026] In some embodiments, the method further includes obtaining the original magnetic vector of the gyroscope, normalizing the original magnetic vector, performing at least one of the following detections: magnitude anomaly, direction abrupt change, magneto-visual consistency, and chi-square threshold, and obtaining the detection result. If the detection result is abnormal, the magnetic heading is shielded.

[0027] In some embodiments, the following processing is performed based on the magnetic heading: if the magnetic heading is abnormal, the heading is maintained by IMU integration, and the gyroscope zero-bias process noise and heading process noise are increased; if the magnetic heading recovers from abnormal to normal, the original magnetic vector is recovered using a smooth reintroduction method, wherein the smooth reintroduction method includes zero-linear recovery or exponential interpolation recovery; if there is a constant deviation in the magnetic heading, it is jointly estimated by setting the heading bias parameter.

[0028] For example, an anomaly detection method is used to reject the test results, as follows: Obtain the original magnetic vector The normalized magnetic vector after calibration using soft and hard iron (e.g., ellipsoid / rotation matrix) is: ; in, and These are calibration parameters.

[0029] (1) Abnormal modulus length ; like Among them, the preset module length If so, it is considered abnormal; (2) Change in direction: Calculate the angle between the original magnetic vector and the magnetic vector at the previous moment. for: ; like If it is, then it is judged as abnormal, where The default value is , where And satisfy ms.

[0030] (3) Magnetism-vision consistency, including calculating visual heading when vision is available. and magnetic heading residuals: ; like And in continuous preset frame( If the condition is met, it is considered abnormal, where the preset residual is... .

[0031] (4) Innovation Chi-square threshold, where the innovation chi-square threshold is included in the EKF / factor plot to calculate magnetic observation innovation. Does it meet the following conditions: ; Among them, magnetic observation innovation , These are actual magnetic observation values. For prior estimation, for Observation model; The observation dimension (either 1 or 2). Indicates the preset value. Indicates the innovation chi-square distribution. Represents the square of the distance to Maharanobis. To innovate the covariance matrix; if magnetic observation innovation The value satisfies the above conditions (i.e., the threshold). If so, then the use of magnetic observation innovations to update the filter will be rejected.

[0032] In some embodiments, when any of (1) to (4) above is satisfied, it indicates that there is magnetic interference. In order to suppress jitter, hysteresis processing is performed, that is, magnetic interference is removed after a preset number of frames have been stabilized.

[0033] For example, when magnetic interference is present, only the heading dimension is masked; that is, the heading component is treated as an observable state in the filter, and partial observation masking is performed. (1) Magnetic interference Discontinue the use of magnetic observations: In EKF, reduce the measurement noise corresponding to the magnetic observations. (Or not update), and discard the magnetic factor in the factor graph; Heading source switching: If vision is available, provide heading observations using visual attitude or employ a weak constraint factor; If vision is unavailable, maintain heading by gyroscope integration, and appropriately increase the gyroscope zero-bias process noise and heading process noise (e.g., ×2~×5 times) to reduce overconfidence.

[0034] (2) During magnetic recovery (magnetic interference changes from true to false) A smooth reintroduction method is adopted, which restores the magnetic heading weight from 0 to the normal weight through linear or exponential interpolation within 0.5 to 2.0 s during the magnetic recovery period, thus avoiding the steep jump caused by hard cutting.

[0035] When there is a constant bias in the magnetic-visual heading, a heading bias parameter can be set at the back end for joint estimation to eliminate long-term system errors.

[0036] In some embodiments, the method further includes propagating the optical loss using an IMU and increasing the covariance to restore strong correction and avoid attitude jumps.

[0037] For example, when the optics are blocked, the system does not lose track and the covariance increases reasonably; once restored, strong constraint correction does not produce perceptible attitude jumps, avoiding visual discomfort to the operator and navigation interruptions.

[0038] The occlusion detection and status indicators include: the number of visible light sources. ,like (Four-point board setting) This indicates the presence of occlusion detection; reprojection error. If the root mean square error exceeds the threshold (e.g., 0.8~1.5 pixels) Continuous The frame indicates the presence of occlusion detection; and the PnP condition number. Excessive size will also trigger (geometric degradation).

[0039] The propagation of missing periods and covariance inflation include: When occlusion detection is indicated, the following processing is performed: (1) INS propagation (IMU pre-integration): ; (2) Adaptive expansion of process noise: The process noise is adaptively amplified based on the amplitude of angular velocity / acceleration. ; If the continuous blocking time exceeds (e.g., 1s), then multiply the whole by .

[0040] (3) Optionally, it also includes zero-velocity / constraint update: when a short pause in the instrument is detected (acceleration close to gravity, angular velocity below the threshold), zero-velocity update (ZUPT) or planar constraint is introduced to mitigate drift in order to achieve covariance. The value increases reasonably over time, providing a buffer for the absorption jump in subsequent strong corrections.

[0041] For example, a strong correction during the recovery period, but without jump output processing, means that when visibility / error returns to normal, the following processing is performed: (1) Observation selection and robust weights Only use light sources that have passed consistency (successful ID decoding + within the reprojection threshold); Huber / Geman-McClure robust kernels were used for each observation to suppress occasional bad spots; Outlier removal using Mahalanobis distance threshold: ; It is the square of the Mahalanobis distance.

[0042] (2) Two-segment alignment to avoid visual abruptness. Back-end correction (internal state): Perform a "large-weight" correction on the state using SE(3) observations inside the filter (or perform 3-5 iterations on the factor graph) to obtain At this point, the internal state is aligned with the visual state.

[0043] Smooth transition in external output (display layer): To avoid the surgeon seeing instantaneous jumps, short-window SLERP / linear interpolation is used at the output end. Smooth transition to the new state within milliseconds: attitude: , ; Location: ; Through the method of the embodiments of the present invention, the internal state is immediately aligned, the external display changes gradually, and the surgeon feels that it is naturally aligning.

[0044] (3) Optionally, external parameter / scale fine-tuning can be used. If systematic deviations persist after recovery, fine-tune the weak constraint extrinsic factors to gradually eliminate slow-varying errors such as installation thermal drift.

[0045] (4) Optionally, processing can be performed using RTS smoothing. If an additional delay of 50–100ms is allowed, an RTS filter can be introduced to smooth the most recent window (0.5–1.0s), making the recovered segment trajectory smoother without changing the real-time main output.

[0046] Figure 6 This is a schematic diagram of a spatial force, position, and attitude fusion calibration device according to an embodiment of the present invention. The device includes a first module 610, a second module 620, a third module 630, and a fourth module 640.

[0047] The system comprises four modules: the first module acquires and decodes the light points emitted by the active light source board according to the AF-LED encoding to obtain two-dimensional image points; the second module performs calculations and extrinsic parameter self-calibration based on the two-dimensional image points to obtain the optical pose; the third module acquires the acceleration and angular velocity of the gyroscope, and performs tight-coupled estimation based on the acceleration and angular velocity using IMU pre-integration and optical pose to obtain the estimation result; and the fourth module calculates the total delay based on the estimation result and performs real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration result of the handheld mechanical surgical device, which includes position, attitude, position accuracy, and attitude accuracy.

[0048] Exemplarily, with the cooperation of the first, second, third, and fourth modules in the device, the embodiment device can realize the aforementioned arbitrary spatial force, position, and attitude fusion calibration method, namely, acquiring and decoding the light points emitted by the active light source board according to the AF-LED encoding to obtain two-dimensional image points; performing calculation and extrinsic parameter self-correction based on the two-dimensional image points to obtain the optical pose; acquiring the acceleration and angular velocity of the gyroscope, and using IMU pre-integration and optical pose tight coupling estimation based on the acceleration and angular velocity to obtain the estimation result; calculating the total delay based on the estimation result, and performing real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration result of the handheld mechanical surgical device. The beneficial effects of the present invention are: continuity, the trajectory is maintained by IMU pre-integration when optical occlusion occurs; after recovery, tight coupling and strong convergence significantly reduce trajectory breakage; accuracy, online fine-tuning of extrinsic parameters + 3σ position / attitude accuracy derived from covariance, and quality score Q make the credibility visible and traceable; real-time performance, explicit measurement of delay and forward extrapolation reduce positioning lag caused by hand-eye asynchrony. Robust coding, Barker-7 synchronization + coprime frequency set + HMM error suppression, can still correctly number frames even with 1-2 missing frames at 100 Hz, significantly reducing the misclassification rate.

[0049] This invention also provides an electronic device, which includes a processor and a memory; The memory stores the program; The processor executes a program to perform the aforementioned spatial force, position, and attitude fusion calibration method; the electronic device has the function of carrying and running the software system for spatial force, position, and attitude fusion calibration provided in the embodiments of the present invention, such as a personal computer, minicomputer, mainframe, workstation, network or distributed computing environment, standalone or integrated computer platform, or communicating with charged particle tools or other imaging devices, etc.

[0050] This invention also provides a computer-readable storage medium storing a program that is executed by a processor to implement the spatial force, position, and attitude fusion calibration method described above.

[0051] In some alternative embodiments, the functions / operations mentioned in the block diagrams may not occur in the order shown in the operation diagrams. For example, depending on the functions / operations involved, two consecutively shown blocks may actually be executed substantially simultaneously, or the blocks may sometimes be executed in reverse order. Furthermore, the embodiments presented and described in the flowcharts of this invention are provided by way of example to provide a more comprehensive understanding of the technology. The disclosed methods are not limited to the operations and logic flows presented in the embodiments of this invention. Alternative embodiments are contemplated, in which the order of various operations is changed and sub-operations described as part of a larger operation are executed independently.

[0052] This invention also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium and execute the computer instructions, causing the computer device to perform the aforementioned spatial force, position, and attitude fusion calibration method.

[0053] Furthermore, although the invention has been described in the context of functional modules, it should be understood that, unless otherwise stated, one or more of the described functions and / or features may be integrated into a single physical device and / or software module, or one or more functions and / or features may be implemented in a separate physical device or software module. It is also understood that a detailed discussion of the actual implementation of each module is unnecessary for understanding the invention. Rather, considering the properties, functions, and internal relationships of the various functional modules in the apparatus disclosed in the embodiments of the invention, the actual implementation of the module will be understood within the scope of conventional skill of an engineer. Therefore, those skilled in the art can implement the invention as set forth in the claims using ordinary techniques without excessive experimentation. It is also understood that the specific concepts disclosed are merely illustrative and are not intended to limit the scope of the invention, which is determined by the full scope of the appended claims and their equivalents.

[0054] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, essentially, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0055] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can include, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device.

[0056] More specific examples of computer-readable media (a non-exhaustive list) include: electrical connections (electronic devices) having one or more wires, portable computer disk drives (magnetic devices), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Furthermore, computer-readable media can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in computer memory.

[0057] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0058] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0059] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

[0060] The above is a detailed description of the preferred embodiments of the present invention, but the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention, and these equivalent modifications or substitutions are all included within the scope defined by the claims of this application.

Claims

1. A spatial force, position, and attitude fusion calibration method for a handheld mechanical surgical device, wherein the handheld mechanical surgical device is equipped with a rigidly mounted gyroscope and an active light source board, characterized in that, include: The light points emitted by the active light source board according to the AF-LED encoding are acquired and decoded to obtain two-dimensional image points; The optical pose is obtained by solving and self-correcting extrinsic parameters based on the two-dimensional image points. The acceleration and angular velocity of the gyroscope are obtained, and the IMU pre-integration and optical pose are used for tightly coupled estimation based on the acceleration and angular velocity to obtain the estimation result; The total delay is obtained, and real-time forward push compensation is performed based on the total delay to obtain the spatial pose fusion calibration result of the handheld mechanical surgical device. The spatial pose fusion calibration result includes position, attitude, position accuracy and attitude accuracy. The acquisition of light points emitted by the active light source board according to the AF-LED encoding is performed by collecting and decoding to obtain two-dimensional image points, including: Each dot in the active light-emitting dot matrix emits light using a dual-slot frequency, where the dual-slot frequency is expressed as: in, Indicates two frequency bands and , Represents the number of frames in a period and ;frequency The number of frame periods is ,frequency The number of frame periods is ,and ,frequency and frequency The duration is , For superframe length; combined Use coprime arrays or arrays with low common factors, and combine them. The frequency signature is unique and the decoding short window is robust; Perform synchronization or DC protection on each point at the beginning of the superframe, and add 4 phases to each point during slot switching. The gray code mapping, and the staggered lighting of each point between neighboring points according to the scene brightness; And, it also includes AF-LED decoding: The sliding window and Barker-7 correlation are used to determine the different slots based on the maximum correlation. Short-window frequency estimation is used to estimate the autocorrelation peak distance of binary switch sequences in different slots, and the discrimination is made by cross-window phase consistency. The decoded frame number and phase are matched against nearest neighbors in the signature list; When frame drops / exposure flicker occur, a Viterbi search is performed based on the cost.

2. The spatial force, position, and attitude fusion calibration method according to claim 1, characterized in that, The step of calculating and self-calibrating extrinsic parameters based on the two-dimensional image points to obtain the optical pose includes: For the centroid of the two-dimensional image points in each frame Perform anti-distortion and with known configuration points Solving PnP yields the optimized optical pose. for: in Indicates in The transformation matrix from the world coordinate system to the camera coordinate system at any moment. for The transformation matrix from the rigid body coordinate system to the world coordinate system at any time. for Observation noise at any given moment This is the index of the centroid of a point in a two-dimensional image. Indicates the light source The centroid pixel coordinates on the image at any given time. This indicates a transformation from rigid body coordinates to world coordinates. Representation based on camera intrinsic parameter matrix Projecting 3D points onto the image plane and combining them with the pixel coordinates of the light source's centroid. The corresponding projection operator; If the light source lines of each frame of the two-dimensional image are insufficient, degradation constraints on lines or surfaces are applied to optimize the process. External parameters are normalized using zero mean and small covariance to perform online estimation to compensate for assembly dimension differences or thermal deformation.

3. The spatial force, position, and attitude fusion calibration method according to claim 2, characterized in that, The acceleration and angular velocity of the gyroscope are acquired, and based on the acceleration and angular velocity, a tightly coupled estimation is performed using IMU pre-integration and optical pose to obtain the estimation results, including: The rotation increment is obtained by integrating the acceleration and angular velocity using IMU pre-integration within a time window. Speed ​​increment and position increment ; Based on rotation increment Speed ​​increment and position increment Perform rotation matrix and speed The update process is represented as follows: in, Angular velocity, For gyroscope bias It is the acceleration due to gravity. For acceleration, For acceleration bias, where the IMU pre-integrates Keep it unchanged when updating. These are the displacement coordinates of the emitting point; Intermittent correction of tight coupling: in, Indicates time The optimal pose estimate obtained by tight coupling optimization. This represents the state prediction obtained through IMU pre-integration propagation. This indicates a transformation from world coordinates to rigid body coordinates. This represents a Gaussian noise model with zero mean. For covariance, Used to describe the uncertainty of the position factor obtained through optimization.

4. The spatial force, position, and attitude fusion calibration method according to claim 3, characterized in that, The process of obtaining the total delay and performing real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration result of the handheld biomechanical surgical device includes: Get total latency ; Based on total delay Real-time position extrapolation, attitude extrapolation, and uncertainty propagation are performed to obtain the spatial pose fusion calibration results of the handheld mechanical surgical device, where the position extrapolation is: in, Accelerate the world system; Attitude extrapolation is: Uncertainty propagation is as follows: in, For location, As a gesture, For positional subcovariance, Let be the attitude subcovariance.

5. The spatial force, position, and attitude fusion calibration method according to claim 4, characterized in that, The method further includes: Obtain the original magnetic vector of the gyroscope, normalize the original magnetic vector, and then perform at least one of the following detections: magnitude anomaly, direction abrupt change, magnetic-visual consistency, and chi-square threshold. Obtain the detection result. If the detection result is abnormal, then shield the magnetic heading. If the magnetic heading is abnormal, the heading is maintained by IMU integration, and the noise of the gyroscope zero bias process and the heading process are increased; if the magnetic heading recovers from abnormal to normal, the original magnetic vector is recovered by smooth reintroduction, which includes zero linear recovery or exponential interpolation recovery; if there is a constant deviation in the magnetic heading, it is estimated by setting the heading bias parameter.

6. The spatial force, position, and attitude fusion calibration method according to claim 4, characterized in that, The method further includes: Optical defects are detected by reprojection error based on the number of visible light sources; If optical defects exist, the noise in the process is adaptively amplified based on angular velocity and acceleration. If optical restoration is required, the following correction process should be performed: For each light source that passes the consistency test, a Huber / Geman-McClure robust kernel is used to suppress occasional bad pixels, and then a Mahalanobis distance threshold is used to remove outliers from occasional bad pixels. Weighted correction is performed on the optical pose observations within the filter, and then a smooth transition is achieved using short-window SLERP and linear interpolation. External parameters were first verified and then fine-tuned. Recovery is performed using an RTS filter smoothing window.

7. A spatial force, position, and attitude fusion calibration device, characterized in that, include: The first module is used to acquire and decode the light points emitted by the active light source board according to the AF-LED encoding to obtain two-dimensional image points; The second module is used to perform calculations and extrinsic parameter self-correction based on the two-dimensional image points to obtain the optical pose. The third module is used to obtain the acceleration and angular velocity of the gyroscope. Based on the acceleration and angular velocity, IMU pre-integration and optical pose are used for tightly coupled estimation to obtain the estimation result. The fourth module is used to calculate the total delay based on the estimation results and perform real-time forward extrapolation compensation based on the total delay to obtain the spatial pose fusion calibration results of the handheld mechanical surgical device. The spatial pose fusion calibration results include position, attitude, position accuracy and attitude accuracy. The acquisition of light points emitted by the active light source board according to the AF-LED encoding is performed by collecting and decoding to obtain two-dimensional image points, including: Each dot in the active light-emitting dot matrix emits light using a dual-slot frequency, where the dual-slot frequency is expressed as: in, Indicates two frequency bands and , Represents the number of frames in a period and ;frequency The number of frame periods is ,frequency The number of frame periods is ,and ,frequency and frequency The duration is , For superframe length; combined Use coprime arrays or arrays with low common factors, and combine them. The frequency signature is unique and the decoding short window is robust; Perform synchronization or DC protection on each point at the beginning of the superframe, and add 4 phases to each point during slot switching. The gray code mapping, and the staggered lighting of each point between neighboring points according to the scene brightness; And, it also includes AF-LED decoding: The sliding window and Barker-7 correlation are used to determine the different slots based on the maximum correlation. Short-window frequency estimation is used to estimate the autocorrelation peak distance of binary switch sequences in different slots, and the discrimination is made by cross-window phase consistency. The decoded frame number and phase are matched against nearest neighbors in the signature list; When frame drops / exposure flicker occur, a Viterbi search is performed based on the cost.

8. An electronic device, characterized in that, Including the processor and memory; The memory is used to store programs; The processor executes the program to implement the spatial force, position, and attitude fusion calibration method as described in any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The storage medium stores a program that is executed by a processor to implement the spatial force, position, and attitude fusion calibration method as described in any one of claims 1-6.

Citation Information

Patent Citations

  • Surgical navigation positioning system and method based on optical and inertial mixed pose tracking

    CN115530976A

  • Airborne high-precision optical fiber inertial measurement device and integrated navigation method

    CN116242343A