Surgical robot precise pose real-time registration method and system based on multi-sensor fusion

Abstract

This invention belongs to the field of medical robot technology and discloses a method and system for real-time precise pose registration of surgical robots based on multi-sensor fusion. The system includes: a multi-source sensor acquisition component; a time-space calibration mechanism; a fusion calculation and anomaly removal platform; a deformation sensing and dynamic compensation device; and a registration result closed-loop feedback unit. This invention integrates a multi-source sensor acquisition component and a synchronous control board at the hardware level. It utilizes a high-precision clock driven by an onboard crystal oscillator to generate nanosecond-level timestamps and combines a cross-correlation delay calculation chip to execute the cross-correlation function method for delay estimation, achieving microsecond-level precise correction of the delay between various sensor sources. The coordinate transformation unit automatically selects a matching calibration matrix based on the correction results, unifying multi-source data such as optical positioning, inertial measurement, torque sensor, ultrasound, or electromagnetic positioning into a global Cartesian coordinate system. This structure effectively ensures the consistency of the temporal and spatial references of the output data from different sensors.

Description

Technical Field

[0001] This invention belongs to the field of medical robot technology, specifically a method and system for real-time registration of precise pose of surgical robots based on multi-sensor fusion. Background Technology

[0002] In complex surgical procedures, surgical robots rely on various types of sensors to acquire pose, force, and imaging information in real time to assist in precise operation. In current clinical applications, optical locators, inertial measurement units, torque sensors, and ultrasonic probes or electromagnetic positioning devices are often used in combination to cover different detection ranges and measurement dimensions. However, due to the differences in sampling frequency, data transmission delay, and coordinate reference system among various sensors, directly using their data for control calculations can easily lead to time alignment errors and spatial registration deviations, affecting the accuracy of fused pose.

[0003] Furthermore, the surgical environment is complex and variable, and sensor outputs are easily affected by factors such as obstruction, mechanical vibration, and electromagnetic interference, resulting in abnormal values. Existing systems have limited mechanisms for anomaly detection and dynamic weight adjustment, and cannot effectively eliminate or suppress anomaly sources before data fusion, leading to decreased result stability. On the other hand, surgical instruments may undergo slight deformation due to force during operation. If such deformations cannot be compensated in the real-time control link, they will cause deviations between the end effector position and the desired position, thereby affecting the safety and accuracy of the surgery.

[0004] In existing technologies, deformation compensation often relies on offline calibration or low-frequency correction, which is difficult to meet the real-time requirements during surgery. At the same time, functions such as multi-source data acquisition, time-space synchronous calibration, fusion calculation, anomaly removal, deformation perception and compensation, and feedback updates are often scattered in different modules, lacking an efficient and collaborative overall architecture. Summary of the Invention

[0005] The purpose of this invention is to provide a method and system for real-time registration of precise pose of surgical robots based on multi-sensor fusion, so as to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a real-time registration system for precise pose of a surgical robot based on multi-sensor fusion, the system comprising:

[0007] The multi-source sensing acquisition component includes an optical locator, an inertial measurement unit, a torque sensor, and an ultrasonic probe or electromagnetic locator that are fixedly installed at the end of the surgical robot. Each sensor is connected to a synchronization control board through an independent hardware interface. The synchronization control board is configured to generate a nanosecond-level precision timestamp by driving a hardware clock with an onboard crystal oscillator when acquiring data, and to package the timestamp and the corresponding sensing data into a cache queue.

[0008] The time-space calibration mechanism includes a signal acquisition buffer, a cross-correlation delay calculation chip, and a coordinate transformation unit. The signal acquisition buffer is used to receive buffered data from the multi-source sensor acquisition components. The cross-correlation delay calculation chip determines the delay correction amount Δt between each sensor source by running the cross-correlation function method delay estimation formula and outputs it to the synchronization control board for microsecond-level time adjustment. The coordinate transformation unit has multiple sets of calibration matrices pre-stored and automatically selects the corresponding matrix based on the latest delay correction amount each time the data is updated to convert the coordinate system data of each sensor into a unified global rectangular coordinate system.

[0009] The fusion solution and anomaly removal platform includes a weight calculation processor and an anomaly detection module. The weight calculation processor calculates the fusion weight of each sensor source based on the confidence level and historical error weighting formula. The anomaly detection module uses the residual multiple threshold criterion formula to detect anomalies in the input data. When anomaly data is detected, the weight of the corresponding sensor source is reduced to below 0.05, and the fusion result is recalculated by the weight calculation processor.

[0010] The deformation sensing and dynamic compensation device includes a deformation extraction unit and a compensation calculation unit. The deformation extraction unit uses the ultrasonic speckle tracking method to obtain the current deformation and convert it into a numerical vector. The compensation calculation unit generates a compensation vector based on the deformation compensation vector calculation formula, and performs vector superposition of the compensation vector with the preliminary pose after fusion calculation to generate a corrected pose. The corrected residual value is output to the weight calculation processor of the fusion calculation and anomaly removal platform in real time.

[0011] The registration result closed-loop feedback unit includes a high-speed communication interface and a parameter update buffer. The high-speed communication interface is used to send the corrected pose to the surgical robot control system with a delay of no more than 50 milliseconds. The parameter update buffer receives the motion execution data returned by the surgical robot control system and writes it directly into the parameter update register of the fusion calculation and anomaly removal platform and the deformation perception and dynamic compensation device, so as to adjust the weight calculation parameters and deformation compensation model parameters in real time in the next fusion calculation cycle, forming a closed-loop processing structure of acquisition-calibration-fusion-compensation-feedback.

[0012] Preferably, the multi-source sensing acquisition component is used for:

[0013] (1) Includes an optical positioner, an inertial measurement unit and a torque sensor fixedly installed on the end flange of the surgical robot, and an ultrasonic probe or electromagnetic positioner installed through a quick-release interface of ISO9409-1-50-4-M6 standard, with a repeat installation error of no more than 0.05 mm.

[0014] Precision positioning pins and stepped limiting surfaces are set between the flange and each sensor as mechanical references. After installation, the position is verified by using a 0.02mm precision gauge block.

[0015] Each sensor signal output terminal is electrically connected to the high-speed data port of the synchronous control board through independent hardware interfaces such as Gigabit Ethernet (GigE Vision), SPI bus, RS-422 serial communication, and USB 3.0 or analog signal interface. The communication protocol and rate are fixed in the design stage and do not depend on external dynamic modification to ensure the stability and consistency of multimodal data acquisition.

[0016] (2) The built-in temperature-compensated crystal oscillator (frequency stability better than ±0.1 ppm) of the synchronous control board drives the hardware clock to generate a timestamp with a resolution of 1ns, and stores it in the onboard FIFO buffer (buffer depth 256 frames, using a cyclic overwrite strategy) when acquiring data.

[0017] Time synchronization correction uses a cross-correlation function method to perform a delay estimation algorithm through a cross-correlation delay calculation chip. It calculates the delay Δt between each sensor source within a 512 sampling point sliding window. When Δt exceeds the 0.5μs threshold, it triggers the time-space calibration processing unit to update the timestamp of each data frame at a resolution of 1μs. The corrected data is then transmitted to the subsequent time-space calibration processing unit through the onboard PCIe interface, achieving seamless connection between time synchronization and subsequent spatial registration.

[0018] The delay estimation algorithm using the cross-correlation function method is as follows:

[0019]

[0020] In the formula:

[0021] : Normalized cross-correlation coefficient (dimensionless), with a value range of [-1, 1];

[0022] The first derivative of the normal force signal output by the torque sensor (unit: N / s);

[0023] : Optical flow modulus of region of interest in ultrasound image (unit: pixels / frame);

[0024] , : respectively and The mean;

[0025] Number of sampling points (unit: frames);

[0026] Delay time (unit: seconds or milliseconds), positive values ​​indicate... Compared to Delay.

[0027] Preferably, the time-space calibration mechanism is used for:

[0028] (1) This system includes a multi-source sensor acquisition component, a synchronous control board and a data processing unit. The multi-source sensor acquisition component consists of an optical positioner, an inertial measurement unit, a torque sensor and a replaceable ultrasonic probe or electromagnetic positioner that are fixedly installed at the end of the surgical robot. The positions of each sensor are precisely registered through the end mechanical interface and electrically connected to the high-speed data port of the synchronous control board through an independent hardware interface. The interface type and communication protocol are fixed in the design stage.

[0029] The synchronization control board has a built-in high-stability onboard crystal oscillator and hardware clock unit to generate nanosecond-level precision timestamps and call the cross-correlation function method delay estimation formula to calculate the delay correction amount Δt between each sensor source and perform microsecond-level time synchronization.

[0030] The data processing unit includes a signal acquisition buffer, a cross-correlation delay calculation chip, and a coordinate transformation unit. The buffer receives multi-source data frames and timestamps, and the Δt output by the calculation chip is used to select the corresponding calibration matrix to unify the coordinate system data of each sensor to the global rectangular coordinate system, with a transformation delay of no more than 5 microseconds.

[0031] The innovation of this solution lies in directly driving the calibration matrix to switch dynamically by adjusting the delay correction amount, forming a hardware closed-loop processing flow that integrates time synchronization and spatial alignment. This is different from the existing method of post-processing only on the software side, and achieves low-latency and stable pose reference unification.

[0032] (2) Multi-source data under a unified coordinate system is input to the fusion solution and anomaly removal platform. The weight calculation processor calls the confidence and historical error weighting formula to generate the fusion weight of each sensor source. The anomaly judgment module executes the residual multiple threshold criterion formula to detect abnormal data and dynamically adjusts the weight to below 0.05 before recalculating the fusion pose.

[0033] The deformation sensing and dynamic compensation device extracts the deformation and measures the change in normal force at the end through the ultrasonic speckle tracking method. The compensation calculation unit generates a compensation vector based on the deformation compensation vector calculation formula, which is superimposed with the preliminary pose vector to form a corrected pose.

[0034] The registration result closed-loop feedback unit transmits the corrected pose to the surgical robot control system through a high-speed communication interface with a delay of no more than 50 milliseconds, and writes the motion execution data returned by the system into the parameter update register of the fusion calculation and dynamic compensation module to update the weight calculation parameters and compensation model parameters of the next fusion cycle, forming a closed-loop structure of acquisition-calibration-fusion-compensation-feedback.

[0035] The innovation of this solution is also reflected in the dynamic reduction of the weight of abnormal data to an extremely low value and the immediate triggering of secondary fusion, as well as the linkage between deformation compensation and registration feedback to update the fusion weight, which realizes real-time collaborative optimization of spatial accuracy and mechanical compensation, unlike the fragmented mode of existing serial processing.

[0036] The formula for weighting confidence level and historical error is:

[0037]

[0038] In the formula:

[0039] The weight of the i-th sensor source (dimensionless, range 0-1);

[0040] : Current cycle number Confidence level (dimensionless) of each sensor source;

[0041] :past Average residual for each period (unit: mm);

[0042] Real-time working status (1 for normal, 0 for abnormal);

[0043] Error attenuation coefficient (unit) ;

[0044] Number of fused sensor sources (dimensionless);

[0045] The formula for the residual multiple threshold criterion is:

[0046]

[0047] : Residual multiple (dimensionless);

[0048] Current period residual (unit: millimeters);

[0049] Historical residuals mean square (unit: millimeters^2);

[0050] Judgment condition: when When T is a preset multiple threshold, ranging from 2.0 to 4.0, the data from the i-th sensor source is determined to be abnormal and removed.

[0051] Preferably, the fusion solution and anomaly removal platform is used for:

[0052] (1) The fusion computing unit includes a weight calculation processor and an anomaly detection module. The weight calculation processor has a built-in fixed-precision arithmetic unit. Based on the confidence and historical error weighting formula, it performs fusion weight calculation on the data from each sensor source from the multi-source sensor acquisition component to generate a weight coefficient matrix.

[0053] The matrix is ​​directly transmitted to the pose fusion and calculation unit via the DMA data bus, avoiding CPU blocking latency.

[0054] The calculation period N for historical error is preset to 50 (corresponding to a 0.25-second window when the sampling frequency is 200Hz). This value can be adjusted within the range of 20 to 100 to adapt to different surgical operation rhythms.

[0055] To prevent data mismatch, both the weight calculation processor and the anomaly detection module use dual-buffered SRAM for alternating storage. Each buffer is configured to hold one frame of multi-source sensor fusion data (e.g., 256B to 1KB), and the data bit width is fixed at 32 bits and is consistent with the bus width.

[0056] The buffer switching is triggered by a timer interrupt driven by an onboard 10MHz crystal oscillator. The interrupt period matches the sampling period (e.g., 200Hz sampling corresponds to a 5ms interrupt) and has a higher priority than non-real-time tasks. The fusion calculation completion signal is generated by a DMA transfer completion flag and synchronized with the interrupt signal through hardware handshake logic to ensure that the data has been fully written and verified before the switching.

[0057] (2) The anomaly detection module calls the residual multiple threshold criterion formula to detect anomalies in the input data. When the residual multiple exceeds the threshold 3.0 (set according to the 3σ principle), the corresponding sensor source weight is automatically adjusted to the range of 0.02 to 0.05, and the anomaly mark is passed back to the weight calculation processor to recalculate the fusion result in real time.

[0058] If the same data source remains normal for 5 consecutive sampling periods, its weight will be dynamically restored to the calculated value according to the historical error. When all sensor sources are judged to be abnormal at the same time, the system enters the safety maintenance mode, outputs the most recent normal fused pose, and marks it as a state with no new data, ensuring that the surgical robot control system has stable input when data is missing. The above weight update and abnormal judgment are completed in a closed loop within a single fusion period and distributed to downstream modules through the system data bus, forming a real-time collaborative mechanism of acquisition-computation-fault tolerance-output.

[0059] Preferably, the deformation sensing and dynamic compensation device is used for:

[0060] (1) The deformation compensation unit consists of a deformation extraction unit and a compensation calculation unit. The deformation extraction unit adopts the ultrasonic speckle tracking method, obtains continuous echo frames of the target tissue during the operation at a sampling frequency of 10MHz, and extracts the displacement of the speckle in the continuous frames through the window correlation algorithm.

[0061] The extracted two-dimensional or three-dimensional displacement data is normalized and converted into a fixed-length numerical vector (e.g., [Δx, Δy, Δz]). The data bit width of this vector is consistent with the downstream bus width (32 bits) and stored in a double-buffered SRAM to ensure that no data loss or mismatch occurs under high-frequency sampling conditions. The buffer switching is triggered by the onboard clock interrupt, the interrupt period is strictly matched with the sampling period, and data integrity verification is completed before switching.

[0062] (2) The compensation calculation unit calls the deformation compensation vector calculation formula and generates the compensation vector based on the deformation vector and the change in the normal force at the end. The calculation process is completed within a fixed-precision arithmetic unit to reduce numerical accumulation error.

[0063] The generated compensation vector and the preliminary pose obtained by fusion calculation are added component by component in a hardware vector superimposed unit to obtain the corrected pose;

[0064] The pose correction is not only used for surgical robot execution in this cycle, but also calculates residual values ​​and outputs them in real time to the weight calculation processor of the fusion solution and anomaly removal platform. This is used to dynamically adjust the weight allocation and compensation model parameters for the next fusion cycle, realizing a closed-loop optimization process of deformation perception, pose compensation and weight correction.

[0065] Compared with existing technologies that rely solely on a single sensor for compensation, this scheme fuses the ultrasonic speckle displacement field with mechanical sensing information to form a compensation vector, which significantly improves the accuracy and stability of pose correction.

[0066] The formula for calculating the deformation compensation vector is:

[0067]

[0068] In the formula:

[0069] Deformation compensation vector (unit: millimeters);

[0070] Tissue deformation calculated by ultrasound speckle tracking (unit: mm);

[0071] : Change in normal force at the distal end (unit: Newtons);

[0072] Force-deformation conversion factor (unit: mm / Newton);

[0073] Weighting coefficients (dimensionless) satisfy the following conditions: .

[0074] Preferably, the registration result closed-loop feedback unit is used for:

[0075] (1) The data interaction unit consists of a high-speed communication interface and a parameter update buffer. The high-speed communication interface is a hardware interface that conforms to the gigabit Ethernet protocol, supports full-duplex transmission mode, and realizes data pass-through through a ring DMA cache at the hardware link layer to ensure that the total link delay from the generation of the corrected pose to the transmission to the surgical robot control system does not exceed 50 milliseconds.

[0076] The transmission protocol parameters of the interface (such as data frame length, verification method, and handshake sequence) are fixed during the design phase and do not depend on external software modifications.

[0077] The parameter update buffer uses a double buffer structure to store the motion execution data returned by the control system. Each buffer has a CRC check field to perform integrity verification after the data is written, preventing abnormal parameter updates due to transmission errors.

[0078] The buffer switching is triggered by the onboard timer interrupt and synchronized with the communication interface receive completion signal to ensure that the returned data corresponds one-to-one with the control command sent.

[0079] (2) The parameter update register is directly connected to the control bus of the fusion calculation and anomaly removal platform and the deformation sensing and dynamic compensation device. The register adopts a 32-bit fixed-length structure and is equipped with a write protection flag to prevent asynchronous modification during the fusion calculation process.

[0080] Before the start of the next fusion calculation cycle, the register contents are called by the fusion platform to update the weight calculation parameters in the weight calculation processor and synchronously update the compensation model parameters in the deformation compensation unit, thereby ensuring the dynamic consistency of multi-source fusion and compensation calculation. This design combines low-latency transmission of the communication interface, integrity verification of the buffer, and dynamic updating of register-level parameters to form a closed-loop data flow of acquisition-calibration-fusion-compensation-feedback. Compared with the existing one-way transmission or static parameter update schemes, it can maintain continuous optimization of calculation parameters while ensuring real-time performance, and improve the robustness and controllability of the system in complex surgical environments.

[0081] This invention also provides a method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion. Based on the above system, the specific steps of this method are as follows:

[0082] S1 acquires optical, inertial, torque, and ultrasonic or electromagnetic positioning data through multi-source sensing acquisition components. The synchronization control board generates nanosecond-level timestamps for each set of data and transmits them to the time-space calibration mechanism. In this mechanism, the cross-correlation delay calculation chip executes the cross-correlation function method delay estimation formula to calculate the delay Δt, and the coordinate transformation unit unifies the multi-source data into the global rectangular coordinate system.

[0083] S2 inputs the unified multi-source data into the fusion solution and anomaly removal platform. The confidence and historical error weighting formula is executed in the weight calculation processor to obtain the fusion weight. The residual multiple threshold criterion formula is executed in the anomaly judgment module to determine whether to remove the abnormal data source. After removal, the fusion pose is recalculated.

[0084] S3 inputs the initial fusion pose into the deformation sensing and dynamic compensation device. The deformation extraction unit extracts the ultrasonic deformation and measures the change in end-effector normal force. The deformation compensation vector calculation formula is executed in the compensation calculation unit to generate a compensation vector. The compensation vector is applied to the initial fusion pose to obtain the corrected pose. The corrected pose is sent to the surgical robot control system through the registration result closed-loop feedback unit. The motion execution data returned by the system is written into the parameter update register of the fusion solution and anomaly removal platform and the deformation sensing and dynamic compensation device to update the weight parameters and compensation model parameters for the next fusion cycle.

[0085] Preferably, the specific steps in S1 are as follows:

[0086] The S11 multi-source sensor acquisition component consists of an optical positioner, an inertial measurement unit, a torque sensor, and a replaceable ultrasonic probe or electromagnetic positioner. Each sensor is installed at the end of the surgical robot through a fixed mechanical interface, and its position and attitude are mechanically registered using a special calibration tool during the assembly stage.

[0087] Data output from each sensor is transmitted to the synchronization control board via an independent high-speed hardware interface (including fiber optic, LVDS, or gigabit Ethernet interface). The physical layer protocol of the high-speed interface is fixed in the hardware to ensure stable transmission rate and latency characteristics.

[0088] The synchronization control board is equipped with a hardware clock unit driven by a high-stability temperature-compensated crystal oscillator (frequency stability better than ±0.1ppm). When the first byte of each data packet is received, the timestamp generation logic is triggered, and a nanosecond-level precision timestamp field is added to the data in the board-level SRAM cache.

[0089] The data is then encapsulated in a preset fixed-length frame format, which includes a data source identifier, timestamp, sampling sequence number, and CRC checksum, providing a traceable and uniquely identified data unit for subsequent calibration and fusion processing;

[0090] The data output by the S12 synchronization control board is transmitted to the time-space calibration mechanism through the bus interface. The mechanism includes a cross-correlation delay calculation chip and a coordinate transformation unit. When receiving data, the cross-correlation delay calculation chip calls the cross-correlation function method delay estimation formula to calculate the delay Δt between each sensor source, and feeds back the delay correction amount to the synchronization control board through a dedicated SPI interface to realize the hardware adjustment of the microsecond-level time base.

[0091] The coordinate transformation unit pre-stores multiple sets of calibration matrices obtained through calibration experiments, each matrix corresponding to different sensor combinations and installation states;

[0092] After receiving the latest delay correction, the coordinate transformation unit selects the calibration matrix that matches the current state from the internal lookup table, and performs matrix operations on the coordinate data of each sensor in the FPGA hardware pipeline to convert them into a unified global rectangular coordinate system.

[0093] The conversion process is completed in one step at the hardware level without software interruption, avoiding latency fluctuations caused by multi-task scheduling, thus ensuring consistency in both time and space.

[0094] Preferably, the specific steps in S2 are as follows:

[0095] S21 will input multi-source sensor data, which is unified into a global Cartesian coordinate system, into the fusion solution and anomaly removal platform;

[0096] In the weight calculation processor within the platform, the confidence and historical error weighting formula is called, and the corresponding fusion weights are generated by combining the historical performance data of each sensor with the current confidence index, and a weight coefficient matrix is ​​constructed.

[0097] The weight calculation processor consists of a fixed-precision arithmetic unit, a weight coefficient register, and a high-speed data interface. The calculation results are written to the dual-buffered SRAM through the internal DMA (direct memory access) controller for parallel reading by the anomaly detection unit.

[0098] The calculation cycle of the weight calculator is triggered by the onboard clock, ensuring that calculation, storage and subsequent detection processes are performed on the same time base;

[0099] S22 simultaneously inputs the fused solution result and the original data from each sensor into the abnormal data detection and rejection processing unit. This unit includes a residual operation chip, a comparison register array, and a control logic circuit.

[0100] The residual operation chip calculates the residual value of each data source based on the residual multiple threshold criterion formula, and compares it with the threshold stored in the comparison register array one by one;

[0101] When the residual value exceeds the threshold, the control logic circuit generates a weight adjustment signal, rewrites the weight register value of the corresponding data source to below 0.05, and writes it back to the weight calculation processor via the DMA controller.

[0102] The weight calculation processor re-executes weighted fusion within the same fusion cycle, and the updated fused pose is output to the system cache via a high-speed bus to provide synchronous and consistent input data for the deformation compensation process in the next stage.

[0103] The specific steps in S3 are as follows:

[0104] S31 inputs the unified spatial position and attitude information (i.e., preliminary fused pose) obtained after time synchronization, delay compensation, abnormal data removal and weighted fusion processing of multi-source sensor data in the fusion computing unit to the deformation sensing and dynamic compensation device; the deformation extraction unit consists of an ultrasonic probe array interface, an electrical signal acquisition module and a speckle mode analysis and processing chip, which extracts the deformation vector of the end structure based on the ultrasonic speckle tracking method, and measures the change of normal force at the end through an integrated multi-axis force sensing amplifier;

[0105] Deformation and normal force data are cached locally on the FPGA and then sent to the compensation calculation unit. The compensation calculation unit includes a vector operation core, a register file and an onboard high-speed multiplier-adder. It executes the deformation compensation vector calculation formula to generate a three-dimensional compensation vector. The compensation vector and the preliminary fused pose vector are superimposed component by component in the hardware adder to obtain the corrected pose. The residual value of the corrected pose is stored in the residual register in real time for the weight calculation processor of the fusion solution and anomaly removal platform to read.

[0106] S32 sends the corrected pose data, which includes the three-dimensional position parameters and three-dimensional attitude angle parameters of the end effector in the global Cartesian coordinate system, generated by the deformation sensing and dynamic compensation device, to the surgical robot control system in the form of data frames through the gigabit Ethernet high-speed communication interface built into the registration result closed-loop feedback unit. This data is used to drive its motion control module to execute the corresponding trajectory adjustment and action commands.

[0107] The closed-loop feedback unit has a built-in transmit buffer and clock synchronization module to ensure that the transmission delay does not exceed 50 milliseconds.

[0108] After receiving the corrected pose, the surgical robot control system executes the corresponding motion commands and packages and returns the actual motion execution data, such as joint position, speed, and torque.

[0109] After the data is received and buffered, it is written into the parameter update register of the fusion calculation and anomaly removal platform and the deformation sensing and dynamic compensation device. In the next fusion calculation cycle, the system synchronously updates the weight calculation parameters and deformation compensation model parameters based on the latest data in the register, realizing a closed-loop hardware link of acquisition-calibration-fusion-compensation-feedback.

[0110] The beneficial effects of this invention are as follows:

[0111] 1. This invention integrates multi-source sensing acquisition components and a synchronous control board at the hardware level. It utilizes a high-precision clock driven by an onboard crystal oscillator to generate nanosecond-level timestamps and combines a cross-correlation delay calculation chip to execute the cross-correlation function method delay estimation formula, thereby achieving microsecond-level precise correction of the delay between various sensing sources. The coordinate transformation unit automatically selects a matching calibration matrix based on the correction results, unifying multi-source data such as optical positioning, inertial measurement, torque sensor, ultrasonic or electromagnetic positioning into a global rectangular coordinate system. This structure can effectively ensure the consistency of the time and spatial reference of the output data of different sensors, reduce fusion errors caused by time drift or coordinate inconsistency, and provide reliable input conditions for subsequent high-precision fusion calculation.

[0112] 2. This invention introduces a weight calculation processor and an anomaly detection module into the fusion solution and anomaly removal platform. The former generates a fusion weight coefficient matrix based on the confidence level and historical error weighting formula, while the latter identifies and removes abnormal data sources based on the residual multiple threshold criterion formula. After removal, the corresponding weights are reduced to a preset low value and the fusion pose is recalculated. This process can dynamically filter out highly reliable and effective data from multi-source information, while suppressing the interference of sudden noise, drift, or failed sensors on the overall fusion result in real time. This improves the stability and accuracy of pose calculation and ensures the continuous high-precision operation of the robot in complex interference environments.

[0113] 3. This invention sets up a deformation extraction unit and a compensation calculation unit in the mechanical compensation stage. It uses the ultrasonic speckle tracking method to extract real-time deformation and measure the change of normal force at the end. Based on the deformation compensation vector calculation formula, a compensation vector is generated and superimposed with the preliminary fused pose to obtain the corrected pose. The correction result is fed back to the surgical robot control system via a high-speed communication interface with a delay of no more than 50 milliseconds, and the weight calculation parameters and compensation model parameters are updated synchronously to form a closed-loop processing structure of acquisition-calibration-fusion-compensation-feedback. This design can effectively correct the pose deviation caused by instrument deformation and ensure that millimeter-level registration accuracy is maintained during dynamic load and soft tissue contact. Attached Figure Description

[0114] Figure 1 This is a flowchart of the real-time registration system for precise pose of a surgical robot based on multi-sensor fusion, as described in this invention.

[0115] Figure 2 This is a flowchart of the real-time registration method for precise pose of a surgical robot based on multi-sensor fusion, as described in this invention. Detailed Implementation

[0116] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0117] like Figures 1 to 2 As shown, this embodiment of the invention provides a real-time registration system for precise pose of a surgical robot based on multi-sensor fusion. The system includes:

[0118] The multi-source sensing acquisition component includes an optical locator, an inertial measurement unit, a torque sensor, and a detachable ultrasonic probe or electromagnetic locator, all fixedly installed at the end of the surgical robot. Each sensor is connected to the synchronization control board via an independent hardware interface. The synchronization control board is configured to generate a nanosecond-level precision timestamp by driving a hardware clock with an onboard crystal oscillator when acquiring data, and to package the timestamp and the corresponding sensing data into a cache queue.

[0119] The time-space calibration mechanism includes a signal acquisition buffer, a cross-correlation delay calculation chip, and a coordinate transformation unit. The signal acquisition buffer is used to receive buffered data from the multi-source sensor acquisition components. The cross-correlation delay calculation chip determines the delay correction amount Δt between each sensor source by running the cross-correlation function method delay estimation formula and outputs it to the synchronization control board for microsecond-level time adjustment. The coordinate transformation unit has multiple sets of calibration matrices pre-stored and automatically selects the corresponding matrix based on the latest delay correction amount each time the data is updated to convert the coordinate system data of each sensor into a unified global rectangular coordinate system.

[0120] The fusion solution and anomaly removal platform includes a weight calculation processor and an anomaly detection module. The weight calculation processor calculates the fusion weight of each sensor source based on the confidence level and historical error weighting formula. The anomaly detection module uses the residual multiple threshold criterion formula to detect anomalies in the input data. When anomaly data is detected, the weight of the corresponding sensor source is reduced to below 0.05, and the fusion result is recalculated by the weight calculation processor.

[0121] The deformation sensing and dynamic compensation device includes a deformation extraction unit and a compensation calculation unit. The deformation extraction unit uses the ultrasonic speckle tracking method to obtain the current deformation and convert it into a numerical vector. The compensation calculation unit generates a compensation vector based on the deformation compensation vector calculation formula, and performs vector superposition of the compensation vector with the preliminary pose after fusion calculation to generate a corrected pose. The corrected residual value is output to the weight calculation processor of the fusion calculation and anomaly removal platform in real time.

[0122] The registration result closed-loop feedback unit includes a high-speed communication interface and a parameter update buffer. The high-speed communication interface is used to send the corrected pose to the surgical robot control system with a delay of no more than 50 milliseconds. The parameter update buffer receives the motion execution data returned by the surgical robot control system and writes it directly into the parameter update register of the fusion calculation and anomaly removal platform and the deformation perception and dynamic compensation device, so as to adjust the weight calculation parameters and deformation compensation model parameters in real time in the next fusion calculation cycle, forming a closed-loop processing structure of acquisition-calibration-fusion-compensation-feedback.

[0123] Example

[0124] In this embodiment, the real-time registration system for precise pose of a surgical robot based on multi-sensor fusion mainly includes a multi-source sensor acquisition component, a synchronous control board, a time-space calibration mechanism, a fusion calculation and anomaly removal platform, a deformation sensing and dynamic compensation device, and a registration result closed-loop feedback unit. The structure and connection of each part are as follows:

[0125] The multi-source sensor acquisition component is fixedly installed at the end of the surgical robot, including an optical locator, an inertial measurement unit, a torque sensor, and a detachable ultrasonic probe or electromagnetic locator. Each sensor is electrically connected to the synchronization control board through an independent hardware interface. The synchronization control board is equipped with a high-precision clock driven by an onboard crystal oscillator. When acquiring data, it generates a nanosecond-level precision timestamp and encapsulates the timestamp and the corresponding sensor data in a preset data format and stores them in a cache queue.

[0126] The time-space calibration mechanism includes a signal acquisition buffer, a cross-correlation delay calculation chip, and a coordinate transformation unit. The signal acquisition buffer receives buffered data output from the multi-source sensor acquisition components. The cross-correlation delay calculation chip executes the cross-correlation function method delay estimation formula to calculate the delay correction amount Δt for each sensor source and outputs Δt to the synchronization control board for microsecond-level time adjustment.

[0127] The coordinate transformation unit pre-stores multiple sets of calibration matrices. When the data is updated, it automatically selects the corresponding matrix according to the latest delay correction amount to unify the coordinate system data of each sensor to the global rectangular coordinate system.

[0128] The fusion solution and anomaly removal platform includes a weight calculation processor and an anomaly detection module. The weight calculation processor executes the confidence and historical error weighting formula within a fixed-precision arithmetic unit to calculate the fusion weights corresponding to each sensor source and generate a weight coefficient matrix for pose fusion. The anomaly detection module executes the residual multiple threshold criterion formula to detect anomalies in the input data. When an anomaly is detected, the weight of the corresponding sensor source is reduced to below 0.05, and then the weight calculation processor recalculates the fusion pose.

[0129] The deformation sensing and dynamic compensation device includes a deformation extraction unit and a compensation calculation unit. The deformation extraction unit acquires the current deformation based on the ultrasonic speckle tracking method and measures the change in normal force at the end, converting it into a numerical vector. The compensation calculation unit executes the deformation compensation vector calculation formula to generate a compensation vector, and then superimposes it with the preliminary pose obtained from the fusion solution to form a corrected pose. Simultaneously, the corrected residual value is output in real time to the weight calculation processor of the fusion solution and anomaly removal platform.

[0130] The registration result closed-loop feedback unit includes a high-speed communication interface and a parameter update buffer. The high-speed communication interface transmits the corrected pose to the surgical robot control system via gigabit Ethernet under a delay of no more than 50 milliseconds. The parameter update buffer receives the motion execution data returned by the control system and writes it into the parameter update register of the fusion calculation and anomaly removal platform and the deformation sensing and dynamic compensation device, so as to update the weight calculation parameters and deformation compensation model parameters in the next fusion calculation cycle, thereby forming a closed-loop processing structure of acquisition-calibration-fusion-compensation-feedback.

[0131] In this embodiment, through the above structure and data processing flow, high-precision time and space synchronization, fusion calculation, anomaly removal and dynamic deformation compensation of multi-source sensor data are achieved, and the operating parameters can be updated in real time in the surgical robot control system to ensure high precision and stability even in complex surgical scenarios.

[0132] The multi-source sensing acquisition component is used for:

[0133] Includes an optical positioner, inertial measurement unit, and torque sensor fixedly mounted on the end flange of the surgical robot, as well as an ultrasonic probe or electromagnetic positioner installed via a quick-release interface according to the ISO9409-1-50-4-M6 standard, with a repeatability error of no more than 0.05 mm.

[0134] Precision positioning pins and stepped limiting surfaces are set between the flange and each sensor as mechanical references. After installation, the position is verified by using a 0.02mm precision gauge block.

[0135] Each sensor signal output terminal is electrically connected to the high-speed data port of the synchronous control board through independent hardware interfaces such as Gigabit Ethernet (GigE Vision), SPI bus, RS-422 serial communication, and USB 3.0 or analog signal interface. The communication protocol and rate are fixed in the design stage and do not depend on external dynamic modification to ensure the stability and consistency of multimodal data acquisition.

[0136] The synchronous control board has a built-in temperature-compensated crystal oscillator (frequency stability better than ±0.1ppm) to drive the hardware clock to generate a timestamp with a resolution of 1ns. When acquiring data, the timestamp is associated with the corresponding sensor data frame and stored in the onboard FIFO buffer (buffer depth 256 frames, using a cyclic overwrite strategy).

[0137] Time synchronization correction uses a cross-correlation function method to estimate the delay by executing the cross-correlation delay calculation chip. Within a 512 sampling point sliding window, the delay Δt between each sensor source is calculated. When Δt exceeds the 0.5μs threshold, the time-space calibration processing unit is triggered to update the timestamp of each data frame at a resolution of 1μs. The corrected data is then transmitted to the subsequent time-space calibration processing unit through the onboard PCIe interface, achieving seamless connection between time synchronization and subsequent spatial registration.

[0138] The time-space calibration mechanism is used for:

[0139] This system includes a multi-source sensor acquisition component, a synchronous control board, and a data processing unit. The multi-source sensor acquisition component consists of an optical positioner, an inertial measurement unit, a torque sensor, and a replaceable ultrasonic probe or electromagnetic positioner, all fixedly installed at the end of the surgical robot. The positions of each sensor are precisely registered through the end mechanical interface and electrically connected to the high-speed data port of the synchronous control board through an independent hardware interface. The interface type and communication protocol are fixed during the design phase.

[0140] The synchronization control board has a built-in high-stability onboard crystal oscillator and hardware clock unit to generate nanosecond-level precision timestamps and call the cross-correlation function method delay estimation formula to calculate the delay correction amount Δt between each sensor source and perform microsecond-level time synchronization.

[0141] The data processing unit includes a signal acquisition buffer, a cross-correlation delay calculation chip, and a coordinate transformation unit. The buffer receives multi-source data frames and timestamps, and the Δt output by the calculation chip is used to select the corresponding calibration matrix to unify the coordinate system data of each sensor to the global rectangular coordinate system, with a transformation delay of no more than 5 microseconds.

[0142] The innovation of this solution lies in directly driving the calibration matrix to switch dynamically by adjusting the delay correction amount, forming a hardware closed-loop processing flow that integrates time synchronization and spatial alignment. This is different from the existing method of post-processing only on the software side, and achieves low-latency and stable pose reference unification.

[0143] Multi-source data under a unified coordinate system is input to the fusion solution and anomaly removal platform. The weight calculation processor calls the confidence and historical error weighting formula to generate the fusion weight of each sensor source. The anomaly judgment module executes the residual multiple threshold criterion formula to detect abnormal data and dynamically adjusts the weight to below 0.05 before recalculating the fusion pose.

[0144] The deformation sensing and dynamic compensation device extracts the deformation and measures the change in normal force at the end through the ultrasonic speckle tracking method. The compensation calculation unit generates a compensation vector based on the deformation compensation vector calculation formula, which is superimposed with the preliminary pose vector to form a corrected pose.

[0145] The registration result closed-loop feedback unit transmits the corrected pose to the surgical robot control system through a high-speed communication interface with a delay of no more than 50 milliseconds, and writes the motion execution data returned by the system into the parameter update register of the fusion calculation and dynamic compensation module to update the weight calculation parameters and compensation model parameters of the next fusion cycle, forming a closed-loop structure of acquisition-calibration-fusion-compensation-feedback.

[0146] The innovation of this solution is also reflected in the dynamic reduction of the weight of abnormal data to an extremely low value and the immediate triggering of secondary fusion, as well as the linkage between deformation compensation and registration feedback to update the fusion weight, which realizes real-time collaborative optimization of spatial accuracy and mechanical compensation, unlike the fragmented mode of existing serial processing.

[0147] The integrated solution and anomaly removal platform is used for:

[0148] The fusion computing unit includes a weight calculation processor and an anomaly detection module. The weight calculation processor has a built-in fixed-precision arithmetic unit. Based on the confidence level and historical error weighting formula, it performs fusion weight calculation on the data from each sensor source from the multi-source sensor acquisition component to generate a weight coefficient matrix.

[0149] The matrix is ​​directly transmitted to the pose fusion and calculation unit via the DMA data bus, avoiding CPU blocking latency.

[0150] The calculation period N for historical error is preset to 50 (corresponding to a 0.25-second window when the sampling frequency is 200Hz). This value can be adjusted within the range of 20 to 100 to adapt to different surgical operation rhythms.

[0151] To prevent data mismatch, both the weight calculation processor and the anomaly detection module use dual-buffered SRAM for alternating storage. Each buffer is configured to hold one frame of multi-source sensor fusion data (e.g., 256B to 1KB), and the data bit width is fixed at 32 bits and is consistent with the bus width.

[0152] The buffer switching is triggered by a timer interrupt driven by an onboard 10MHz crystal oscillator. The interrupt period matches the sampling period (e.g., 200Hz sampling corresponds to a 5ms interrupt) and has a higher priority than non-real-time tasks. The fusion calculation completion signal is generated by a DMA transfer completion flag and synchronized with the interrupt signal through hardware handshake logic to ensure that the data has been fully written and verified before the switching.

[0153] The anomaly detection module calls the residual multiple threshold criterion formula to detect anomalies in the input data. When the residual multiple exceeds the threshold of 3.0 (set according to the 3σ principle), the corresponding sensor source weight is automatically adjusted to the range of 0.02 to 0.05, and the anomaly mark is passed back to the weight calculation processor to recalculate the fusion result in real time.

[0154] If the same data source remains normal for 5 consecutive sampling periods, its weight will be dynamically restored to the calculated value according to the historical error. When all sensor sources are judged to be abnormal at the same time, the system enters the safety maintenance mode, outputs the most recent normal fused pose, and marks it as a state with no new data, ensuring that the surgical robot control system has stable input when data is missing. The above weight update and abnormal judgment are completed in a closed loop within a single fusion period and distributed to downstream modules through the system data bus, forming a real-time collaborative mechanism of acquisition-computation-fault tolerance-output.

[0155] The deformation sensing and dynamic compensation device is used for:

[0156] The deformation compensation unit consists of a deformation extraction unit and a compensation calculation unit. The deformation extraction unit uses the ultrasonic speckle tracking method to acquire continuous echo frames of the target tissue during the operation at a sampling frequency of 10MHz, and extracts the displacement of the speckle in the continuous frames through a window correlation algorithm.

[0157] The extracted two-dimensional or three-dimensional displacement data is normalized and converted into a fixed-length numerical vector (e.g., [Δx, Δy, Δz]). The data bit width of this vector is consistent with the downstream bus width (32 bits) and stored in a double-buffered SRAM to ensure that no data loss or mismatch occurs under high-frequency sampling conditions. The buffer switching is triggered by the onboard clock interrupt, the interrupt period is strictly matched with the sampling period, and data integrity verification is completed before switching.

[0158] The compensation calculation unit calls the deformation compensation vector calculation formula and generates a compensation vector based on the deformation vector and the change in the normal force at the end. The calculation process is completed within a fixed-precision arithmetic unit to reduce numerical accumulation error.

[0159] The generated compensation vector and the preliminary pose obtained by fusion calculation are added component by component in a hardware vector superimposed unit to obtain the corrected pose;

[0160] The pose correction is not only used for surgical robot execution in this cycle, but also calculates residual values ​​and outputs them in real time to the weight calculation processor of the fusion solution and anomaly removal platform. This is used to dynamically adjust the weight allocation and compensation model parameters for the next fusion cycle, realizing a closed-loop optimization process of deformation perception, pose compensation and weight correction.

[0161] Compared with existing technologies that rely solely on a single sensor for compensation, this approach fuses the ultrasonic speckle displacement field with mechanical sensing information to form a compensation vector, significantly improving the accuracy and stability of pose correction.

[0162] The registration result closed-loop feedback unit is used for:

[0163] The data interaction unit consists of a high-speed communication interface and a parameter update buffer. The high-speed communication interface is a hardware interface that conforms to the gigabit Ethernet protocol, supports full-duplex transmission mode, and achieves data pass-through through a ring DMA cache at the hardware link layer, ensuring that the total link delay from the generation of the corrected pose to the transmission to the surgical robot control system does not exceed 50 milliseconds.

[0164] The transmission protocol parameters of the interface (such as data frame length, verification method, and handshake sequence) are fixed during the design phase and do not depend on external software modifications.

[0165] The parameter update buffer uses a double buffer structure to store the motion execution data returned by the control system. Each buffer has a CRC check field to perform integrity verification after the data is written, preventing abnormal parameter updates due to transmission errors.

[0166] The buffer switching is triggered by the onboard timer interrupt and synchronized with the communication interface receive completion signal to ensure that the returned data corresponds one-to-one with the control command sent.

[0167] The parameter update register is directly connected to the control bus of the fusion calculation and anomaly removal platform and the deformation sensing and dynamic compensation device. The register adopts a 32-bit fixed-length structure and is equipped with a write protection flag to prevent asynchronous modification during the fusion calculation process.

[0168] Before the start of the next fusion calculation cycle, the register contents are called by the fusion platform to update the weight calculation parameters in the weight calculation processor and synchronously update the compensation model parameters in the deformation compensation unit, thereby ensuring the dynamic consistency of multi-source fusion and compensation calculation. This design combines low-latency transmission of the communication interface, integrity verification of the buffer, and dynamic updating of register-level parameters to form a closed-loop data flow of acquisition-calibration-fusion-compensation-feedback. Compared with the existing one-way transmission or static parameter update schemes, it can maintain continuous optimization of calculation parameters while ensuring real-time performance, and improve the robustness and controllability of the system in complex surgical environments.

[0169] This invention also provides a method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion. Based on the above system, the specific steps of this method are as follows:

[0170] S1 acquires optical, inertial, torque, and ultrasonic or electromagnetic positioning data through multi-source sensing acquisition components. The synchronization control board generates nanosecond-level timestamps for each set of data and transmits them to the time-space calibration mechanism. In this mechanism, the cross-correlation delay calculation chip executes the cross-correlation function method delay estimation formula to calculate the delay Δt, and the coordinate transformation unit unifies the multi-source data into the global rectangular coordinate system.

[0171] S2 inputs the unified multi-source data into the fusion solution and anomaly removal platform. The confidence and historical error weighting formula is executed in the weight calculation processor to obtain the fusion weight. The residual multiple threshold criterion formula is executed in the anomaly judgment module to determine whether to remove the abnormal data source. After removal, the fusion pose is recalculated.

[0172] S3 inputs the initial fusion pose into the deformation sensing and dynamic compensation device. The deformation extraction unit extracts the ultrasonic deformation and measures the change in end-effector normal force. The deformation compensation vector calculation formula is executed in the compensation calculation unit to generate a compensation vector. The compensation vector is applied to the initial fusion pose to obtain the corrected pose. The corrected pose is sent to the surgical robot control system through the registration result closed-loop feedback unit. The motion execution data returned by the system is written into the parameter update register of the fusion solution and anomaly removal platform and the deformation sensing and dynamic compensation device to update the weight parameters and compensation model parameters for the next fusion cycle.

[0173] The specific steps in S1 are as follows:

[0174] The multi-source sensor acquisition component consists of an optical positioner, an inertial measurement unit, a torque sensor, and a replaceable ultrasonic probe or electromagnetic positioner. Each sensor is installed at the end of the surgical robot through a fixed mechanical interface, and its position and attitude are mechanically registered using a special calibration tool during the assembly stage.

[0175] Data output from each sensor is transmitted to the synchronization control board via an independent high-speed hardware interface (including fiber optic, LVDS, or gigabit Ethernet interface). The physical layer protocol of the high-speed interface is fixed in the hardware to ensure stable transmission rate and latency characteristics.

[0176] The synchronization control board is equipped with a hardware clock unit driven by a high-stability temperature-compensated crystal oscillator (frequency stability better than ±0.1ppm). When the first byte of each data packet is received, the timestamp generation logic is triggered, and a nanosecond-level precision timestamp field is added to the data in the board-level SRAM cache.

[0177] The data is then encapsulated in a preset fixed-length frame format, which includes a data source identifier, timestamp, sampling sequence number, and CRC checksum, providing a traceable and uniquely identified data unit for subsequent calibration and fusion processing;

[0178] The data output by the synchronization control board is transmitted to the time-space calibration mechanism through the bus interface. The mechanism includes a cross-correlation delay calculation chip and a coordinate transformation unit. When receiving data, the cross-correlation delay calculation chip calls the cross-correlation function method delay estimation formula to calculate the delay Δt between each sensor source, and feeds back the delay correction amount to the synchronization control board through a dedicated SPI interface to realize the hardware adjustment of the microsecond-level time base.

[0179] The coordinate transformation unit pre-stores multiple sets of calibration matrices obtained through calibration experiments, each matrix corresponding to different sensor combinations and installation states;

[0180] After receiving the latest delay correction, the coordinate transformation unit selects the calibration matrix that matches the current state from the internal lookup table, and performs matrix operations on the coordinate data of each sensor in the FPGA hardware pipeline to convert them into a unified global rectangular coordinate system.

[0181] The conversion process is completed in one step at the hardware level without software interruption, avoiding latency fluctuations caused by multi-task scheduling, thus ensuring consistency in both time and space.

[0182] The specific steps in S2 are as follows:

[0183] Multi-source sensor data, unified into a global Cartesian coordinate system, is input into a fusion solution and anomaly removal platform.

[0184] In the weight calculation processor within the platform, the confidence and historical error weighting formula is called, and the corresponding fusion weights are generated by combining the historical performance data of each sensor with the current confidence index, and a weight coefficient matrix is ​​constructed.

[0185] The weight calculation processor consists of a fixed-precision arithmetic unit, a weight coefficient register, and a high-speed data interface. The calculation results are written to the dual-buffered SRAM through the internal DMA (direct memory access) controller for parallel reading by the anomaly detection unit.

[0186] The calculation cycle of the weight calculator is triggered by the onboard clock, ensuring that calculation, storage and subsequent detection processes are performed on the same time base;

[0187] The fused solution result and the original data from each sensor are simultaneously input into the abnormal data detection and rejection processing unit, which includes a residual calculation chip, a comparison register array and a control logic circuit.

[0188] The residual operation chip calculates the residual value of each data source based on the residual multiple threshold criterion formula, and compares it with the threshold stored in the comparison register array one by one;

[0189] When the residual value exceeds the threshold, the control logic circuit generates a weight adjustment signal, rewrites the weight register value of the corresponding data source to below 0.05, and writes it back to the weight calculation processor via the DMA controller.

[0190] The weight calculation processor re-executes weighted fusion within the same fusion cycle, and the updated fused pose is output to the system cache via a high-speed bus to provide synchronous and consistent input data for the deformation compensation process in the next stage.

[0191] The specific steps in S3 are as follows:

[0192] The unified spatial position and attitude information (i.e., preliminary fused pose) obtained after time synchronization, delay compensation, abnormal data removal and weighted fusion processing of multi-source sensor data in the fusion computing unit is input to the deformation sensing and dynamic compensation device; the deformation extraction unit consists of an ultrasonic probe array interface, an electrical signal acquisition module and a speckle mode analysis and processing chip, which extracts the deformation vector of the end structure based on the ultrasonic speckle tracking method, and measures the change of normal force at the end through an integrated multi-axis force sensing amplifier;

[0193] Deformation and normal force data are cached locally on the FPGA and then sent to the compensation calculation unit. The compensation calculation unit includes a vector operation core, a register file and an onboard high-speed multiplier-adder. It executes the deformation compensation vector calculation formula to generate a three-dimensional compensation vector. The compensation vector and the preliminary fused pose vector are superimposed component by component in the hardware adder to obtain the corrected pose. The residual value of the corrected pose is stored in the residual register in real time for the weight calculation processor of the fusion solution and anomaly removal platform to read.

[0194] The corrected pose data, which includes the three-dimensional position parameters and three-dimensional attitude angle parameters of the end effector in the global Cartesian coordinate system, generated by the deformation sensing and dynamic compensation device, is sent to the surgical robot control system in the form of data frames through the gigabit Ethernet high-speed communication interface built into the registration result closed-loop feedback unit. This data is used to drive its motion control module to execute the corresponding trajectory adjustment and action commands.

[0195] The closed-loop feedback unit has a built-in transmit buffer and clock synchronization module to ensure that the transmission delay does not exceed 50 milliseconds.

[0196] After receiving the corrected pose, the surgical robot control system executes the corresponding motion commands and packages and returns the actual motion execution data, such as joint position, speed, and torque.

[0197] After the data is received and buffered, it is written into the parameter update register of the fusion calculation and anomaly removal platform and the deformation sensing and dynamic compensation device. In the next fusion calculation cycle, the system synchronously updates the weight calculation parameters and deformation compensation model parameters based on the latest data in the register, realizing a closed-loop hardware link of acquisition-calibration-fusion-compensation-feedback.

[0198] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0199] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art 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 appended claims and their equivalents.

Claims

1. A real-time precise pose registration system for surgical robots based on multi-sensor fusion, characterized in that: The system includes: The multi-source sensing acquisition component includes an optical positioner, an inertial measurement unit, a torque sensor, and an ultrasonic probe or electromagnetic positioner. Each sensor is connected to a synchronization control board through an independent hardware interface. The synchronization control board generates a timestamp and stores it in association with the sensing data. The time-space calibration mechanism includes a signal acquisition buffer, a delay calculation chip, and a coordinate transformation unit. The delay calculation chip calculates the delay correction amount, and the coordinate transformation unit converts the data from each sensor into a unified coordinate system. The fusion solution and anomaly removal platform includes a weight calculation processor and an anomaly detection module. The weight calculation processor calculates the fusion weights, and the anomaly detection module detects anomalies, adjusts the corresponding weights, and updates the fusion result. A deformation sensing and dynamic compensation device includes a deformation extraction unit and a compensation calculation unit. The deformation extraction unit extracts deformation variables, and the compensation calculation unit generates a compensation vector and superimposes it with a preliminary pose to form a corrected pose. The registration result closed-loop feedback unit includes a communication interface and a parameter update buffer. The communication interface sends the corrected pose to the surgical robot control system, and the parameter update buffer receives the feedback data and updates the relevant parameters.

2. The real-time registration system for precise pose of a surgical robot based on multi-sensor fusion as described in claim 1, characterized in that: The multi-source sensing acquisition component is used for: (1) Includes an optical positioner, an inertial measurement unit, a torque sensor, and an ultrasonic probe or electromagnetic positioner that are fixedly installed at the end of the surgical robot; each sensor is electrically connected to the synchronous control board via an independent hardware interface. (2) The synchronization control board generates a nanosecond-level precision timestamp and stores the timestamp in association with the collected sensor data. Time synchronization is corrected according to the preset delay estimation algorithm.

3. The real-time registration system for precise pose of a surgical robot based on multi-sensor fusion according to claim 2, characterized in that: The time-space calibration mechanism is used for: (1) The data processing unit includes a signal acquisition buffer, a cross-correlation delay calculation chip and a coordinate transformation unit; the signal acquisition buffer receives the buffered data output by the multi-source sensor acquisition component; the cross-correlation delay calculation chip runs the cross-correlation function method delay estimation formula to determine the delay correction amount Δt between each sensor source, and outputs the correction amount to the synchronization control board to perform microsecond-level time adjustment; (2) Multiple calibration matrices are pre-stored in the coordinate transformation unit. When the data is updated, the corresponding calibration matrix is ​​selected according to the latest delay correction amount to convert the coordinate system data collected by each sensor into a global rectangular coordinate system. The conversion process is completed at the hardware level to ensure the real-time performance of coordinate transformation and data consistency.

4. The real-time registration system for precise pose of a surgical robot based on multi-sensor fusion as described in claim 3, characterized in that: The fusion computation and anomaly removal platform is used for: (1) The fusion computing unit includes a weight calculation processor and an anomaly detection module; the weight calculation processor calculates the fusion weight of each sensor source based on the confidence level and the historical error weighting formula. The calculation is performed in a fixed-precision arithmetic unit and generates a weight coefficient matrix for subsequent fusion. (2) The anomaly detection module uses the residual multiple threshold criterion formula to detect anomalies in the input data, and adjusts the weight of the corresponding sensor source to below 0.05 when anomaly data is detected; The adjusted weighting coefficients are recalculated by the weighting calculation processor to achieve the fusion result, which is then output to the system data bus.

5. The real-time registration system for precise pose of a surgical robot based on multi-sensor fusion according to claim 4, characterized in that: The deformation sensing and dynamic compensation device is used for: (1) The deformation compensation unit includes a deformation extraction unit and a compensation calculation unit; the deformation extraction unit obtains the current deformation based on the ultrasonic speckle tracking method and converts the deformation into a numerical vector for subsequent use by the compensation calculation unit; (2) The compensation calculation unit generates a compensation vector according to the deformation compensation vector calculation formula, and superimposes it with the preliminary pose obtained by fusion calculation to form a corrected pose; the residual value of the corrected pose is output to the weight calculation processor of the fusion calculation and anomaly removal platform in real time.

6. The real-time registration system for precise pose of a surgical robot based on multi-sensor fusion according to claim 5, characterized in that: The registration result closed-loop feedback unit is used for: (1) The data interaction unit includes a high-speed communication interface and a parameter update buffer; the high-speed communication interface sends the corrected pose to the surgical robot control system under a delay of no more than 50 milliseconds; the parameter update buffer receives the motion execution data returned by the control system and writes it into the specified parameter update register; (2) The parameter update register is connected to the fusion calculation and anomaly removal platform and the deformation sensing and dynamic compensation device. In the next fusion calculation cycle, the weight calculation parameters and deformation compensation model parameters are updated based on the register content, thus forming a closed-loop processing structure of acquisition, calibration, fusion, compensation and feedback.

7. A method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion, characterized in that: The method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion is based on the system described in claim 6, and the specific steps of the method are as follows: S1 acquires optical, inertial, torque sensor and ultrasonic or electromagnetic positioning data through multi-source sensing acquisition components. The synchronization control board generates nanosecond-level timestamps for each set of data and transmits them to the time-space calibration mechanism. In this mechanism, the cross-correlation delay calculation chip executes the cross-correlation function method delay estimation formula to calculate the delay Δt, and the coordinate transformation unit unifies the multi-source data into the global rectangular coordinate system. S2 inputs the unified multi-source data into the fusion solution and anomaly removal platform. The confidence and historical error weighting formula is executed in the weight calculation processor to obtain the fusion weight. The residual multiple threshold criterion formula is executed in the anomaly judgment module to determine whether to remove the abnormal data source. After removal, the fusion pose is recalculated. S3 inputs the initial fused pose into the deformation sensing and dynamic compensation device. The deformation extraction unit extracts the ultrasonic deformation and measures the change in normal force at the end. The deformation compensation vector calculation formula is executed in the compensation calculation unit to generate a compensation vector. The compensation vector is then applied to the initial fused pose to obtain the corrected pose. The corrected pose is sent to the surgical robot control system through the registration result closed-loop feedback unit, and the motion execution data returned by the system is written into the parameter update register of the fusion calculation and anomaly removal platform and the deformation perception and dynamic compensation device to update the weight parameters and compensation model parameters of the next fusion cycle.

8. The method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion according to claim 7, characterized in that: The specific steps in S1 are as follows: S11 acquires optical positioning, inertial measurement, torque sensor and ultrasonic probe or electromagnetic positioning data through multi-source sensing acquisition components; the data output by each sensor is transmitted to the synchronization control board through an independent hardware interface, and the synchronization control board generates nanosecond-level timestamps for each group of received data and encapsulates it according to a preset data format for subsequent processing; S12 sends the data output by the synchronization control board to the time-space calibration mechanism; the cross-correlation delay calculation chip in the mechanism runs the cross-correlation function method delay estimation formula to calculate the delay Δt, and the coordinate transformation unit converts and unifies the multi-source data to the global rectangular coordinate system according to the pre-stored calibration matrix.

9. The method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion according to claim 8, characterized in that: The specific steps in S2 are as follows: S21 inputs multi-source data in a unified coordinate system to the fusion solution and anomaly removal platform; the platform's weight calculation processor executes the confidence and historical error weighting formula to calculate the fusion weights corresponding to each sensor data source and generates a weight coefficient matrix for pose fusion. S22 executes the residual multiple threshold criterion formula in the anomaly determination module, and determines whether to remove a specific sensor data source based on the formula calculation result. When an anomaly is detected, the weight of the data source is set to a predetermined low value, and the fused pose is recalculated in the weight calculation processor.

10. The method for real-time registration of precise pose of a surgical robot based on multi-sensor fusion according to claim 9, characterized in that: The specific steps in S3 are as follows: S31 will initially integrate pose input deformation sensing and dynamic compensation devices; the deformation extraction unit extracts deformation based on the ultrasonic speckle tracking method and measures the change in end-effector normal force. The compensation calculation unit executes the deformation compensation vector calculation formula to generate a compensation vector, and then superimposes it with the preliminary fused pose to form a corrected pose. S32 sends the corrected pose to the surgical robot control system through the registration result closed-loop feedback unit; it receives the motion execution data returned by the control system and writes the data into the parameter update register of the fusion calculation and anomaly removal platform and the deformation perception and dynamic compensation device to update the weight parameters and compensation model parameters of the next fusion cycle.

Images