Master-slave isomorphic surgical robot anti-shake filtering and safety constraint control method and device

Abstract

This invention relates to the field of robot motion control technology, and particularly to a method and apparatus for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot. The method includes: acquiring multi-degree-of-freedom (DOF) operation signals of the master hand; preprocessing the DDF operation signals; performing state estimation on the preprocessed DDF operation signals to obtain velocity estimates for each DDF of the master hand; generating gating coefficients based on the velocity estimates; performing gated notch filtering on the velocity estimates of at least one DDF; performing low-pass filtering on the gated notch filtering velocity estimates to obtain filtered DDF velocity signals of the master hand; mapping the filtered DDF velocity signals of the master hand to slave hand motion control commands based on the DDF correspondence between the master and slave hands; and performing motion safety constraint processing on the slave hand motion control commands to generate the final slave hand control commands. This invention can significantly improve the smoothness, stability, safety, and responsiveness of the master-slave isomorphic surgical robot during teleoperation.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, and in particular to a method and device for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot. Background Technology

[0002] Transoral surgical robots typically enter the target area through the body's natural cavities, characterized by minimal trauma but limited pathways and narrow operating space. These robots require a high degree of precision, smoothness, and safety in their master-slave operation.

[0003] In a master-slave teleoperation system, the doctor inputs the operation intention through the master hand, and the slave hand performs actions such as bending, rotating, inserting, or withdrawing according to the master hand's instructions. For master-slave isomorphic robots, the master hand and slave hand have a corresponding relationship in terms of degree of freedom configuration, such as both including bending degree of freedom, rotational degree of freedom around itself, and axial translation degree of freedom.

[0004] However, the master hand operation signal usually contains the following interferences: physiological tremors of the doctor's hand, typically concentrated in the 8-12Hz range; measurement noise introduced by encoders, magnetic sensors or displacement sensors; occasional glitch during communication or sampling; minor vibrations caused by gaps, friction or elastic deformation of the master hand mechanism; and mechanical shocks from the hand caused by sudden changes in speed or acceleration.

[0005] Existing methods often employ simple low-pass filtering or moving average filtering to smooth the master signal. However, these methods have significant shortcomings: if the filtering strength is weak, it is difficult to effectively suppress jitter; if the filtering strength is strong, it will introduce a large phase lag, making master-slave operation sticky; the delay becomes significant when the moving average filter window is increased; a single filter cannot simultaneously achieve anti-jump, low delay, and smooth control; and there is a lack of safety constraints on slave speed, acceleration, and jerk, which can easily cause abrupt changes in slave actions.

[0006] Therefore, there is a need for a method for anti-shaking filtering and safety constraint control suitable for master-slave isomorphic surgical robots, which can suppress master hand tremor and improve slave hand movement safety while ensuring master-slave following performance. Summary of the Invention

[0007] This invention provides a method and apparatus for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot. It aims to solve the problems of existing master-slave isomorphic surgical robot control methods, which are difficult to simultaneously achieve the anti-shake filtering effect of the master hand operation signal and the low-latency tracking of the master-slave operation, and lack safety constraints on the slave hand movement, which can easily cause sudden changes in the slave hand movement.

[0008] In a first aspect, the present invention provides a method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot. The master-slave isomorphic surgical robot includes a master hand, a controller, and a slave hand. The master hand and the slave hand have corresponding multi-degree-of-freedom motion relationships. The method includes: acquiring multi-degree-of-freedom operation signals of the master hand and preprocessing the multi-degree-of-freedom operation signals; performing state estimation on the preprocessed multi-degree-of-freedom operation signals to obtain velocity estimates for each degree of freedom of the master hand; generating gating coefficients based on the velocity estimates for each degree of freedom of the master hand, and performing gated notch filtering on the velocity estimate for at least one degree of freedom based on the gating coefficients; performing low-pass filtering on the velocity estimates after gated notch filtering to obtain a multi-degree-of-freedom filtered velocity signal of the master hand; mapping the multi-degree-of-freedom filtered velocity signal of the master hand to a motion control command of the slave hand according to the degree-of-freedom correspondence between the master hand and the slave hand; performing motion safety constraint processing on the motion control command of the slave hand to limit the velocity amplitude, acceleration amplitude, and jerk amplitude during the movement of the slave hand, and generating a final control command for the slave hand.

[0009] Optionally, the multi-degree-of-freedom operation signals of the master hand include rotational degree-of-freedom signals, bending degree-of-freedom signals, and axial translational degree-of-freedom signals; the slave hand has rotational motion degrees of freedom, bending motion degrees of freedom, and axial translational degrees of freedom corresponding to the master hand.

[0010] Optionally, the preprocessing of the multi-degree-of-freedom operation signal includes: performing continuous processing on the angle signal with periodic variation characteristics to eliminate data discontinuity caused by periodic angle jumps;

[0011] The operation signals corresponding to different degrees of freedom are subjected to unit conversion and normalization.

[0012] Optionally, the preprocessing of the multi-degree-of-freedom operation signal further includes: suppressing abnormal transitions in the multi-degree-of-freedom operation signal; the abnormal transition suppression includes: determining whether the difference between the operation signal value at the current sampling time and the operation signal value at the previous sampling time for each degree of freedom exceeds the preset transition threshold of the corresponding degree of freedom; if so, the operation signal value at the current sampling time is determined as an abnormal value, and the abnormal value is replaced by the operation signal value at the historical sampling time of the corresponding degree of freedom.

[0013] Optionally, the step of performing state estimation on the preprocessed multi-degree-of-freedom operation signals includes: using an α-β state estimator to predict and correct the position and velocity of each degree of freedom based on a constant velocity model, so as to obtain the velocity estimation results of the corresponding degree of freedom.

[0014] Optionally, generating gating coefficients based on the velocity estimates of each degree of freedom of the master hand includes: generating the gating coefficients based on the comparison result of the absolute value of the velocity estimates of each degree of freedom with a preset velocity threshold, wherein the magnitude of the gating coefficients is negatively correlated with the absolute value of the velocity estimates; the gating notch filtering process includes: weighting and fusing the original velocity estimates of each degree of freedom with the velocity estimates after gating notch filtering based on the gating coefficients, so as to enhance the notch filtering effect when the absolute value of the velocity estimates is less than the preset velocity threshold, and weaken the notch filtering effect when the absolute value of the velocity estimates is greater than the preset velocity threshold.

[0015] Optionally, the degree-of-freedom correspondence between the master hand and the slave hand includes: the rotational degree of freedom of the master hand corresponds to the rotational motion of the slave hand; the bending degree of freedom of the master hand corresponds to the bending motion of the slave hand; and the axial movement degree of freedom of the master hand corresponds to the insertion or withdrawal motion of the slave hand along the natural cavity direction.

[0016] Optionally, the motion safety constraint processing includes: constraining the velocity amplitude of the slave motion control command according to the slave motion state; constraining the acceleration amplitude of the slave motion control command according to the velocity change process; and constraining the jerk amplitude of the slave motion control command according to the acceleration change process; wherein the safety constraint parameter corresponding to the axial translation degree of freedom is less than or equal to the safety constraint parameter corresponding to other degrees of freedom.

[0017] Secondly, the present invention provides a master-slave isomorphic surgical robot anti-shake filtering and safety constraint control device, comprising: a signal acquisition module for acquiring multi-degree-of-freedom operation signals of the master hand; a preprocessing module for preprocessing the multi-degree-of-freedom operation signals; a state estimation module for performing state estimation on the preprocessed multi-degree-of-freedom operation signals to obtain velocity estimates for each degree of freedom of the master hand; a gated notch filter module for generating gate coefficients based on the velocity estimates for each degree of freedom of the master hand, and performing gated notch filtering on the velocity estimates for at least one degree of freedom based on the gate coefficients; a low-pass filter module for performing low-pass filtering on the velocity estimates after gated notch filtering to obtain a multi-degree-of-freedom filtered velocity signal of the master hand; a master-slave mapping module for mapping the multi-degree-of-freedom filtered velocity signal of the master hand to a motion control command of the slave hand according to the degree-of-freedom correspondence between the master hand and the slave hand; and a safety constraint module for performing motion safety constraint processing on the motion control command of the slave hand to limit the velocity amplitude, acceleration amplitude, and jerk amplitude during the movement of the slave hand, and generating a final control command for the slave hand.

[0018] Thirdly, the present invention provides an electronic device including a processor and a memory, the memory storing computer-readable instructions that, when executed by the processor, perform the steps of the method provided in the first aspect above.

[0019] Fourthly, the present invention provides a storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the method provided in the first aspect above.

[0020] Fifthly, the present invention provides a computer program product comprising a computer program that, when executed by a processor, performs the steps of the method provided in the first aspect above.

[0021] As can be seen from the above technical solutions, the present invention has the following advantages:

[0022] This invention provides a method and apparatus for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot. The method includes: acquiring multi-degree-of-freedom operation signals of the master hand and preprocessing the multi-degree-of-freedom operation signals; performing state estimation on the preprocessed multi-degree-of-freedom operation signals to obtain velocity estimates for each degree of freedom of the master hand; generating gating coefficients based on the velocity estimates of each degree of freedom of the master hand, and performing gated notch filtering on the velocity estimate of at least one degree of freedom based on the gating coefficients; performing low-pass filtering on the velocity estimates after gated notch filtering to obtain a multi-degree-of-freedom filtered velocity signal of the master hand; mapping the multi-degree-of-freedom filtered velocity signal of the master hand to a motion control command of the slave hand according to the degree-of-freedom correspondence between the master hand and the slave hand; performing motion safety constraint processing on the motion control command of the slave hand to limit the velocity amplitude, acceleration amplitude, and jerk amplitude during the movement of the slave hand, and generating a final control command for the slave hand. This achieves adaptive and precise suppression of physiological tremors in the multi-degree-of-freedom operation signals of the master hand, effectively filtering out hand tremor interference during low-speed fine operation, while maintaining signal pass-through to reduce phase lag during high-speed active operation, thus achieving synergistic optimization of anti-shake and low latency. Furthermore, by applying multiple safety constraints of speed, acceleration, and jerk to the slave hand control commands, sudden changes in slave hand movements and mechanical shocks are avoided, ultimately significantly improving the smoothness, stability, safety, and responsiveness of the master-slave isomorphic surgical robot during teleoperation. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of the master-slave isomorphic surgical robot system structure in this invention;

[0024] Figure 2 This is a flowchart illustrating the steps of Embodiment 1 of the master-slave isomorphic surgical robot anti-shake filtering and safety constraint control method of the present invention;

[0025] Figure 3 This is a flowchart illustrating the steps of Embodiment 2 of the master-slave isomorphic surgical robot anti-shake filtering and safety constraint control method of the present invention;

[0026] Figure 4 This is a schematic diagram of the multi-degree-of-freedom position / angle filtering results in this invention;

[0027] Figure 5 This is a schematic diagram of the multi-degree-of-freedom velocity filtering results in this invention;

[0028] Figure 6 This is a schematic diagram showing the amplitude changes after filtering the three degrees of freedom (rotation, bending, and translation) in this invention using differentiated filtering parameters.

[0029] Figure 7 This is a schematic diagram of notch center frequency update based on spectrum detection in this invention;

[0030] Figure 8 This is a schematic diagram of the structure of Embodiment 3 of the master-slave isomorphic surgical robot anti-shake filtering and safety constraint control device in this invention. Detailed Implementation

[0031] The embodiments of the present invention will be described in detail below.

[0032] In this embodiment, unless otherwise explicitly specified and limited, terms such as "set in," "connected," and "linked" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a direct connection or a connection through one or more intermediate media. Those skilled in the art can understand the specific meaning of these terms in this embodiment based on the specific circumstances. The directional terms appearing in this embodiment are for the purpose of better describing the characteristics of the features and the relationships between them. It should be understood that when the placement direction of this embodiment changes, the direction of the characteristics of the features and the relationships between them also changes accordingly. Therefore, directional terms do not constitute an absolute limitation on the characteristics of the features and the relationships between them in space, but only a relative limitation.

[0033] Example 1

[0034] Please see Figure 1 , Figure 1 This is a schematic diagram of the master-slave isomorphic surgical robot system of the present invention; the master-slave isomorphic surgical robot includes a master hand, a controller, and a slave hand, wherein the master hand and the slave hand have corresponding multi-degree-of-freedom motion relationships. Optionally, the master-slave isomorphic surgical robot is a master-slave isomorphic transoral surgical robot.

[0035] Please see Figure 2 , Figure 2This is a flowchart illustrating the steps of an embodiment of the master-slave isomorphic surgical robot anti-shake filtering and safety constraint control method of the present invention; the method includes:

[0036] Step S101: Obtain the multi-degree-of-freedom operation signals of the master hand and preprocess the multi-degree-of-freedom operation signals.

[0037] In a specific implementation, the multi-degree-of-freedom operation signals of the master hand include rotational degree-of-freedom signals, bending degree-of-freedom signals, and axial translational degree-of-freedom signals; the slave hand has rotational motion degrees of freedom, bending motion degrees of freedom, and axial motion degrees of freedom corresponding to the master hand.

[0038] In a specific implementation, the preprocessing of the multi-degree-of-freedom operation signal includes:

[0039] 1. For angle signals exhibiting periodic variations, continuous processing is performed to eliminate data discontinuities caused by periodic angle jumps. This can be understood as follows: for a degree of freedom of rotation around itself, if the rotation angle jumps between 0° and 360°, angle expansion is first performed to convert the periodic angle measurements into continuous angle values.

[0040] 2. Perform unit conversion and normalization on the operation signals corresponding to different degrees of freedom.

[0041] 3. Perform abnormal transition suppression on the multi-degree-of-freedom operation signal. The abnormal transition suppression includes: determining whether the difference between the operation signal value at the current sampling time and the operation signal value at the previous sampling time for each degree of freedom exceeds the preset transition threshold of the corresponding degree of freedom; if so, the operation signal value at the current sampling time is determined as an abnormal value, and the abnormal value is replaced by the operation signal value at the historical sampling time of the corresponding degree of freedom.

[0042] Step S102: Perform state estimation on the preprocessed multi-degree-of-freedom operation signals to obtain velocity estimates for each degree of freedom of the master hand.

[0043] In a specific implementation, an α-β state estimator is used to predict and correct the position and velocity of each degree of freedom based on a constant velocity model, so as to obtain the velocity estimation results of the corresponding degree of freedom.

[0044] For each degree of freedom, a position-velocity state is established, a constant velocity model is used for prediction, and the position and velocity are corrected after calculating the residuals. The position correction gain and velocity correction gain are set separately for different degrees of freedom.

[0045] Step S103: Generate gating coefficients based on the velocity estimates of each degree of freedom of the master hand, and perform gated notch filtering on the velocity estimates of at least one degree of freedom based on the gating coefficients.

[0046] In a specific implementation, the gating coefficients are generated based on a comparison between the absolute values ​​of the velocity estimates for each degree of freedom and a preset velocity threshold. The magnitude of the gating coefficients is negatively correlated with the absolute value of the velocity estimates. The gated notch filtering process includes: weighted fusion of the original velocity estimates for each degree of freedom and the velocity estimates filtered by the notch filter based on the gating coefficients, so as to enhance the notch filtering effect when the absolute value of the velocity estimate is less than the preset velocity threshold, and weaken the notch filtering effect when the absolute value of the velocity estimate is greater than the preset velocity threshold.

[0047] In an optional embodiment, the notch filter is a digital notch filter with its center frequency set in the range of 8Hz to 12Hz to suppress physiological tremors of the human hand. The center frequency of the notch filter is dynamically updated based on the spectral peak value of the primary hand speed signal in the 8Hz to 12Hz frequency band.

[0048] Step S104: Perform low-pass filtering on the velocity estimate after gated notch filtering to obtain the master multi-degree-of-freedom filtered velocity signal.

[0049] In an optional implementation, the low-pass filter is a second-order Bessel low-pass filter, and different cutoff frequencies are used for different degrees of freedom.

[0050] Step S105: Based on the degree-of-freedom correspondence between the master hand and the slave hand, map the master hand's multi-degree-of-freedom filtered velocity signal into a slave hand motion control command.

[0051] In a specific implementation, the degree-of-freedom correspondence between the master hand and the slave hand includes: the rotational degree of freedom of the master hand corresponds to the rotational movement of the slave hand; the bending degree of freedom of the master hand corresponds to the bending movement of the slave hand; and the axial movement degree of freedom of the master hand corresponds to the insertion or withdrawal movement of the slave hand along the natural cavity direction.

[0052] In this embodiment, the mapping is a proportional mapping, which involves multiplying the master hand's multi-degree-of-freedom filtered velocity signal with the mapping coefficients corresponding to each degree of freedom to obtain the slave hand's motion control command. The mapping is as follows: based on the current configuration of the master hand, the master hand's multi-degree-of-freedom filtered velocity signal is mapped to the slave hand's motion control command using a configuration mapping Jacobian matrix.

[0053] Step S106: Perform motion safety constraint processing on the slave hand motion control command to limit the speed amplitude, acceleration amplitude and jerk amplitude during the slave hand motion process, and generate the slave hand final control command.

[0054] In a specific implementation, the motion safety constraint processing includes: constraining the velocity amplitude of the slave hand motion control command based on the slave hand motion state; constraining the acceleration amplitude of the slave hand motion control command based on the velocity change process; and constraining the jerk amplitude of the slave hand motion control command based on the acceleration change process. The safety constraint parameter corresponding to the axial movement degree of freedom is less than or equal to the safety constraint parameters corresponding to other degrees of freedom, thereby improving safety during insertion and withdrawal through the natural cavity.

[0055] Compared with the prior art, the method provided in this embodiment has at least the following beneficial effects:

[0056] (1) Good anti-shake effect: The gating notch filter is used to specifically suppress the physiological tremor of the human body at 8-12Hz, avoiding the reliance on strong low-pass filtering alone.

[0057] (2) Small master-slave delay: α-β state estimation and second-order Bessel lightweight low-pass are used to reduce phase lag and maintain master-slave responsiveness.

[0058] (3) Strong universality: It is applicable to master-slave isomorphic surgical robots with multiple degrees of freedom, and is not limited to a specific number of degrees of freedom or a specific configuration.

[0059] (4) Reduce the risk of false filtering: Enhance notch filtering during low-speed fine operation and weaken notch filtering during fast active operation by using the gating coefficient to achieve adaptive filtering.

[0060] (5) Improve slave safety: Through triple constraints of speed, acceleration and jerk, avoid sudden changes in slave commands and mechanical shocks.

[0061] (6) Suitable for force feedback systems: It does not rely on force feedback signals and is suitable for master-slave control systems based solely on the position, angle, or displacement signals of the master hand.

[0062] (7) Simple engineering implementation: The filters and constraint algorithms used have low computational load, making them suitable for online operation of real-time controllers.

[0063] Example 2

[0064] Please see Figure 3 , Figure 3 This is a flowchart illustrating the steps of a second embodiment of the anti-shake filtering and safety constraint control method for a master-slave isomorphic surgical robot according to the present invention. This embodiment provides an anti-shake filtering and safety constraint control method for a master-slave isomorphic surgical robot. The master-slave isomorphic surgical robot includes a master hand, a controller, and a slave hand. The master hand and the slave hand have corresponding three degrees of freedom motion relationships, specifically including rotational degrees of freedom around themselves. Bending degree of freedom and axial translational degrees of freedom .

[0065] Define the master hand's three-degree-of-freedom generalized coordinates for:

[0066]

[0067] in, Indicates the angle of rotation about itself; Indicates the bending angle; Indicates the amount of axial movement; This represents the discrete sampling time.

[0068] Main hand sensor measurement value for:

[0069]

[0070] in, The value measured by the sensor of the main hand's rotation angle around itself; The value is measured by the bending angle sensor; This is the measurement value from the axial movement sensor.

[0071] Its signal model Simplified to:

[0072]

[0073] in, To express the doctor's true intentions; This indicates a combination of interferences, including human hand tremors, sensor noise, communication glitches, and minor perturbations in the mechanism.

[0074] The key steps in this embodiment are as follows:

[0075] 1. Angular Expansion

[0076] The periodic angle measurements are preprocessed to convert them into continuous angle values ​​for the degree of freedom of rotation around itself. If the rotation angle exists at 0° With 360 The transition begins with an angular expansion:

[0077]

[0078] Wrap(.) indicates that the angle difference is limited to... Within the range.

[0079] The preprocessed three-degree-of-freedom signal is as follows:

[0080]

[0081] 2. Burr removal

[0082] For each degree of freedom Determine if an abnormal transition has occurred:

[0083]

[0084] If this condition is met, the current sampling point is considered an outlier, and the neighborhood median is used as the replacement.

[0085] =median[ , ]

[0086] in, This is the maximum speed allowed for this degree of freedom; This is the coefficient for judging abnormal jumps; Indicates the sampling period.

[0087] 3. α-β state estimation

[0088] Establish position-velocity states for each degree of freedom:

[0089]

[0090] Using a constant velocity model:

[0091]

[0092]

[0093] Calculate the residuals:

[0094]

[0095] Correct position and speed:

[0096]

[0097] +

[0098] in, Position correction gain; This is for speed correction gain.

[0099] The three-degree-of-freedom velocity estimation results are as follows:

[0100]

[0101] 4. Gated notch filter

[0102] Physiological tremors in the human hand are typically concentrated between 8 and 12 Hz. This invention employs a gated notch filter to enhance tremor suppression during low-speed, fine-tuning operations and reduce the impact of notch filters during fast, active operations.

[0103] For the first With one degree of freedom, the second-order notch filter is:

[0104]

[0105] in:

[0106]

[0107] in, The center frequency of the notch filter; The sampling frequency; Let be the radius of the notch filter pole.

[0108] The notch filter center frequency can be fixed at a frequency within the range of 8–12 Hz, or it can be updated based on real-time spectrum detection results.

[0109] Define the gating coefficient :

[0110]

[0111] in, The threshold for gating speed; Indicates amplitude limiting operation; ( The closer the signal is to 1, the stronger the notch effect. ( The closer the value is to 0, the weaker the notch effect.

[0112] The notch output is:

[0113]

[0114] The gated fusion output is:

[0115]

[0116] This achieves the control effect of anti-shake for low-speed fine operation and low-latency control for fast active operation.

[0117] 5. Second-order Bessel low-pass filter

[0118] Second-order Bessel low-pass filtering is applied to the velocity signal after gated notch filtering:

[0119]

[0120] Its difference equation is:

[0121]

[0122] The filter coefficients are determined based on the sampling frequency and the cutoff frequency.

[0123] The velocity after three-degree-of-freedom filtering is:

[0124]

[0125] 6. Master-slave isomorphic mapping

[0126] Since the master-slave isomorphic surgical robot has an isomorphic configuration, the three degrees of freedom of the master hand correspond to the three degrees of freedom of the slave hand, and the mapping can be a proportional mapping:

[0127]

[0128] in:

[0129]

[0130] These represent the master-slave mapping ratios for rotation, bending, and axial movement around themselves, respectively.

[0131] If there is a non-linear relationship between the master and slave components, configuration mapping Jacobi can also be used:

[0132]

[0133] in, The Jacobian matrix is ​​the configurational mapping from master hand to slave hand.

[0134] 7. Safety constraints for velocity, acceleration, and jerk.

[0135] After filtering and mapping, the desired velocity from the hand is obtained. To avoid abrupt changes in hand movements, speed, acceleration, and jerk constraints are applied to them.

[0136] The expected acceleration is:

[0137]

[0138] First, apply jerk restrictions:

[0139]

[0140] in, It is a saturation function;

[0141] Then apply acceleration limits:

[0142]

[0143] Update speed:

[0144]

[0145] And impose a speed limit:

[0146]

[0147] Finally, the desired position from the hand is obtained:

[0148]

[0149] in:

[0150]

[0151]

[0152]

[0153] Among them, the axial translation degree of freedom The speed, acceleration, and jerk limits are preferably set to be relatively conservative to improve the safety of the insertion and withdrawal process.

[0154] In summary, the method of this embodiment is summarized as follows:

[0155] The controller uses a fixed sampling period The signals from three degree-of-freedom sensors are collected to form:

[0156]

[0157] in, The measured value is from the main hand sensor. The value measured by the sensor of the main hand's rotation angle around itself; The value is measured by the bending angle sensor; This is the measurement value from the axial movement sensor.

[0158] The controller first By performing angular expansion, continuous rotation angles are obtained. Subsequently, abnormal transition detection was performed on the three-degree-of-freedom signal, and sampling points exceeding the physical velocity threshold were removed.

[0159] For the preprocessed signal, run the α-β state estimator separately to obtain a three-degree-of-freedom smoothed velocity estimate: Then, the gating coefficient is generated based on the speed magnitude:

[0160]

[0161] in, This is the gating speed threshold.

[0162] When the doctor is performing a slow and delicate operation, the gating coefficient is relatively large, and the notch filter suppresses 8–12 Hz tremor; when the doctor moves the main hand quickly, the gating coefficient is relatively small, and the system mainly retains the doctor's active operation intention.

[0163] Subsequently, the controller uses a second-order Bessel low-pass filter to further reduce high-frequency noise and converts the filtered speed signal into the slave's desired speed through isomorphic mapping.

[0164] Finally, the controller applies jerk, acceleration, and speed limit to the slave's desired speed, generating smooth and safe slave control commands.

[0165] Figure 4 This is a schematic diagram of the multi-degree-of-freedom position / angle filtering results; Figure 5 This is a schematic diagram of the multi-degree-of-freedom velocity filtering results. Please refer to [link / reference]. Figure 4 and Figure 5 The results are analyzed as follows:

[0166] Position / Angle Results: The filtered outputs for a1 rotation and a3 axial movement follow the intended target well, while significantly reducing high-frequency fluctuations in noisy measurements; the fixed notch and adaptive notch show little difference in the position domain, indicating that both maintain a low-frequency operating trend. Curved Channel a2: The filtered output is much smoother than the noisy measurement, but there are significant deviations from the intended target in some areas, indicating that the curved channel parameters are more conservative, with stronger safety constraints or low-pass filtering, resulting in enhanced anti-shake performance at the cost of some tracking accuracy.

[0167] Velocity results: The gray difference velocity jitter is the most obvious, and the green curve is significantly smoothed after alpha-beta; blue and red further suppress high-frequency jitter, and the black safety constraint results in the smoothest velocity, indicating that velocity, acceleration and jerk constraints are effective.

[0168] Furthermore, in this embodiment, different filtering parameters are used for the three degrees of freedom to adapt to the different motion characteristics and safety requirements of each degree of freedom.

[0169] For the degree of freedom of rotation about itself To ensure the sensitivity of attitude adjustment, the α-β gain can be set relatively large, and the low-pass cutoff frequency can be relatively high to ensure fast response.

[0170] For bending degrees of freedom Since it directly affects the end-effector posture and surgical precision, the gating notch weight can be relatively strong, and the low-pass cutoff frequency can be appropriately reduced to enhance the anti-shake effect.

[0171] For axial translational degrees of freedom Because of its corresponding insertion and withdrawal actions, it focuses on applying speed, acceleration and jerk limits, and the constraint parameters are set conservatively to prevent sudden advance or retraction along the cavity direction, thereby improving the safety of insertion and withdrawal through the natural cavity.

[0172] Figure 6 A schematic diagram illustrating the amplitude changes after filtering the three degrees of freedom (rotation, bending, and translation) using differentiated filtering parameters; please refer to [link / reference]. Figure 6 It can be seen that the original velocity has a significant peak in the 8–12 Hz flutter frequency band, especially Approximately 10 Hz Approximately 9 Hz Approximately 11 Hz; the blue and red curves show a significant decrease in amplitude in this frequency band after filtering, proving that gated notch filtering and Bessel low-pass filtering effectively suppress hand tremor.

[0173] Furthermore, in this embodiment, the notch filter center frequency is dynamically updated based on the peak frequency of the master hand speed signal within a preset frequency band.

[0174] The controller calculates the spectral energy of the master hand speed signal within a certain time window and searches for energy peaks in the range of 8–12 Hz. If the energy in this frequency band exceeds a preset threshold, it indicates the presence of significant physiological hand tremors, and the notch filter center frequency is updated to this peak frequency; if no obvious tremor peak is detected, the notch filter weight is reduced or the default center frequency is maintained.

[0175] This method can adapt to changes in tremor frequency under different operators and different fatigue states, achieving a personalized tremor suppression effect.

[0176] Figure 7 This is a schematic diagram illustrating notch filter center frequency updates based on spectrum detection. Please refer to [link / reference]. Figure 7 It can be seen that, The center of the notch converges from 10 Hz to approximately 10.2 Hz. The frequency converged from 11 Hz to approximately 11.2 Hz, consistent with the set vibration frequency; The degrees of freedom fluctuate around 8.5–9.3 Hz, indicating that the curved channel is more affected by active motion and noise, but can still be locked in the flutter frequency band.

[0177] Example 3

[0178] Please see Figure 8 , Figure 8 This is a schematic diagram of an embodiment of the master-slave isomorphic surgical robot anti-shake filtering and safety constraint control device of the present invention; this embodiment provides an anti-shake filtering and safety constraint control device for a master-slave isomorphic surgical robot, including:

[0179] Signal acquisition module 201 is used to acquire multi-degree-of-freedom operation signals of the master hand;

[0180] The preprocessing module 202 is used to preprocess the multi-degree-of-freedom operation signal. The preprocessing includes continuous processing of angle signals with periodic variation characteristics, unit conversion and normalization processing of operation signals corresponding to different degrees of freedom, and abnormal jump suppression of the multi-degree-of-freedom operation signal.

[0181] State estimation module 203 is used to perform state estimation on the preprocessed multi-degree-of-freedom operation signal to obtain the velocity estimate of each degree of freedom of the master hand;

[0182] The gated notch filter module 204 is used to generate gated coefficients based on the velocity estimates of each degree of freedom of the master hand, and to perform gated notch filtering on the velocity estimates of at least one degree of freedom based on the gated coefficients.

[0183] The low-pass filter module 205 is used to perform low-pass filtering on the velocity estimate after gated notch filtering in order to obtain the master hand multi-degree-of-freedom filtered velocity signal.

[0184] The master-slave mapping module 206 is used to map the master hand's multi-degree-of-freedom filtered velocity signal into the slave hand's motion control command according to the degree-of-freedom correspondence between the master hand and the slave hand;

[0185] The safety constraint module 207 is used to perform motion safety constraint processing on the slave hand motion control command to limit the speed amplitude, acceleration amplitude and jerk amplitude during the slave hand motion process, and generate the slave hand final control command.

[0186] The specific working principles of each module are described in the relevant descriptions of Embodiment 1 and Embodiment 2, and will not be repeated here.

[0187] Example 4

[0188] This invention also provides an electronic device, including a memory and a processor. The memory stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of a master-slave isomorphic surgical robot anti-shake filtering and safety constraint control method according to any embodiment.

[0189] Where there is no conflict, the above embodiments and features can be combined with each other.

[0190] Finally, it should be noted that the above embodiments are only used to illustrate the preferred technical solutions of the present invention, and are not intended to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the essence and scope of the present invention.

Claims

1. A method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot, characterized in that, The master-slave isomorphic surgical robot includes a master hand, a controller, and a slave hand, wherein the master hand and the slave hand have corresponding multi-degree-of-freedom motion relationships, and the method includes: Acquire the multi-degree-of-freedom operation signals of the main hand and preprocess the multi-degree-of-freedom operation signals; State estimation is performed on the preprocessed multi-degree-of-freedom operation signals to obtain velocity estimates for each degree of freedom of the master hand. Gating coefficients are generated based on the velocity estimates of each degree of freedom of the master hand, and gating notch filtering is performed on the velocity estimates of at least one degree of freedom based on the gating coefficients. The velocity estimate after gated notch filtering is low-pass filtered to obtain the master hand multi-degree-of-freedom filtered velocity signal; Based on the degree-of-freedom correspondence between the master hand and the slave hand, the multi-degree-of-freedom filtered velocity signal of the master hand is mapped into motion control commands for the slave hand; The motion control command of the slave hand is subjected to motion safety constraint processing to limit the velocity amplitude, acceleration amplitude and jerk amplitude during the movement of the slave hand, and the final control command of the slave hand is generated.

2. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The master hand's multi-degree-of-freedom operation signals include rotational degree-of-freedom signals, bending degree-of-freedom signals, and axial translational degree-of-freedom signals; the slave hand has rotational motion degrees of freedom, bending motion degrees of freedom, and axial motion degrees of freedom corresponding to the master hand.

3. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The preprocessing of the multi-degree-of-freedom operation signal includes: Continuous processing is performed on angle signals with periodic variation characteristics to eliminate data discontinuity caused by periodic jumps in angle; The operation signals corresponding to different degrees of freedom are subjected to unit conversion and normalization.

4. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The preprocessing of the multi-degree-of-freedom operation signal further includes: suppressing abnormal transitions in the multi-degree-of-freedom operation signal; the abnormal transition suppression includes: Determine whether the difference between the operation signal value at the current sampling time and the operation signal value at the previous sampling time for each degree of freedom exceeds the preset transition threshold of the corresponding degree of freedom; if so, determine the operation signal value at the current sampling time as an outlier and replace the outlier with the operation signal value at the historical sampling time of the corresponding degree of freedom.

5. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The process of performing state estimation on the preprocessed multi-degree-of-freedom operation signals includes: An α-β state estimator is used to predict and correct the position and velocity of each degree of freedom based on a constant velocity model, so as to obtain the velocity estimation results of the corresponding degree of freedom.

6. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The step of generating gating coefficients based on the velocity estimates of each degree of freedom of the master hand includes: generating the gating coefficients based on the comparison result of the absolute value of the velocity estimates of each degree of freedom with a preset velocity threshold, wherein the magnitude of the gating coefficients is negatively correlated with the absolute value of the velocity estimates; the gating notch filtering process includes: weighting and fusing the original velocity estimates of each degree of freedom with the velocity estimates after gating notch filtering based on the gating coefficients, so as to enhance the notch filtering effect when the absolute value of the velocity estimates is less than the preset velocity threshold, and weaken the notch filtering effect when the absolute value of the velocity estimates is greater than the preset velocity threshold.

7. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The degree-of-freedom correspondence between the master hand and the slave hand includes: The rotational degree of freedom of the master hand corresponds to the rotational motion of the slave hand; The bending degree of freedom of the master hand corresponds to the bending motion of the slave hand; The axial movement degree of freedom of the master hand corresponds to the insertion or withdrawal movement of the slave hand along the natural cavity direction.

8. The method for anti-shake filtering and safety constraint control of a master-slave isomorphic surgical robot according to claim 1, characterized in that, The motion safety constraint processing includes: The speed amplitude of the slave hand motion control command is constrained based on the slave hand motion state. The acceleration amplitude of the hand motion control command is constrained based on the speed change process. The acceleration amplitude is constrained for the hand motion control command based on the acceleration change process. Among them, the safety constraint parameter corresponding to the axial translation degree of freedom is less than or equal to the safety constraint parameter corresponding to the other degrees of freedom.

9. A master-slave isomorphic surgical robot anti-shake filtering and safety constraint control device, characterized in that, include: The signal acquisition module is used to acquire multi-degree-of-freedom operation signals of the main hand. The preprocessing module is used to preprocess the multi-degree-of-freedom operation signals; The state estimation module is used to perform state estimation on the preprocessed multi-degree-of-freedom operation signals to obtain the velocity estimates of each degree of freedom of the master hand. A gated notch filter module is used to generate gate coefficients based on the velocity estimates of each degree of freedom of the master hand, and to perform gated notch filtering on the velocity estimates of at least one degree of freedom based on the gate coefficients. The low-pass filter module is used to perform low-pass filtering on the velocity estimate after gated notch filtering in order to obtain the master multi-degree-of-freedom filtered velocity signal. The master-slave mapping module is used to map the multi-degree-of-freedom filtered velocity signal of the master hand into motion control commands of the slave hand according to the degree-of-freedom correspondence between the master hand and the slave hand; The safety constraint module is used to perform motion safety constraint processing on the slave hand motion control command to limit the velocity amplitude, acceleration amplitude and jerk amplitude during the slave hand motion process, and generate the slave hand final control command.

10. An electronic device, characterized in that, It includes a processor and a memory, the memory storing computer-readable instructions that, when executed by the processor, perform the method as described in any one of claims 1-8.

Images