Hybrid force and position control method and system for surgical robot arm

Abstract

The application relates to the technical field of robot control, and particularly discloses a force-position hybrid control method and system for a surgical robot mechanical arm, the method comprising the following steps: acquiring joint torque data and end force sensor data, and extracting periodic characteristic parameters of physiological motion through frequency domain analysis; establishing a human physiological motion prediction model based on the periodic characteristic parameters, and generating tissue displacement prediction values and contact force fluctuation prediction values in real time; feeding forward compensating the position control channel with the tissue displacement prediction values, and feeding forward compensating the force control channel with the contact force fluctuation prediction values; generating joint driving instructions based on a selection matrix decoupling distribution, driving joint motion, and online correcting the periodic characteristic parameters, and cyclically executing; the application realizes real-time physiological motion prediction and double-channel cooperative feedforward compensation, overcomes the problem that the force control and position control channels are not consistent in response under physiological motion coupling disturbance, and improves the force-position hybrid control precision and stability of the surgical robot.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a force-position hybrid control method and system for surgical robot arms. Background Technology

[0002] In surgical robotics, robotic arms need to perform high-precision contact operations within a limited operating space. Force-position hybrid control, which can simultaneously regulate the end-effector position and contact force, is a core control strategy for achieving compliant contact operations.

[0003] Existing force-position hybrid control methods for surgical robot arms typically decouple the force control subspace and position control subspace using a selection matrix, allowing force control and position control to be implemented independently in their respective degrees of freedom. The force control channel adjusts the contact force based on the end-effector contact force signal, while the position control channel tracks the desired trajectory based on the end-effector pose calculated kinematically by the joint encoder. This framework achieves effective decoupling when the contact target is stationary. However, during surgical procedures, physiological activities such as respiration and heartbeat cause periodic tissue displacement, resulting in fluctuations in the end-effector contact force and relative position deviations. Because the force control and position control targets differ, the adjustment requirements of the two channels for the robotic arm inherently contradict each other; this phenomenon is called physiological motion coupling perturbation.

[0004] The presence of physiological motion coupling disturbances has a fundamental negative impact on the stability and accuracy of the force-position hybrid control of surgical robot arms. Current technologies typically treat this disturbance as random noise and suppress it using a fixed-bandwidth low-pass filter, thus filtering out the physiological motion component in the force control channel feedback signal. However, filtering does not eliminate the physical influence of physiological motion on the system: when tissue moves towards the end effector of the robotic arm due to physiological motion, the relative position between the end effector and the tissue passively changes, causing physical fluctuations in the contact force. The force control channel, due to filtering, loses its ability to perceive the actual contact force changes within this frequency band and cannot actively adjust to maintain a constant force; the position control channel, based on joint encoder feedback, also cannot perceive the periodic displacement of the tissue and cannot actively adjust the end effector position to follow the tissue movement. Both control channels lack the ability to actively perceive physiological motion, and existing methods do not establish a predictive mechanism for the periodic patterns of physiological motion. This results in the robotic arm end effector being unable to actively follow tissue displacement or actively adjust contact force fluctuations; the end effector position passively deviates from the desired trajectory with tissue movement, causing continuous fluctuations in the contact force. Furthermore, the phase lag introduced by filtering further exacerbates the response delay of the force control channel to changes in actual contact force, causing the overall control performance of the system to continuously decline under physiological motion disturbances, making it difficult to guarantee the dual precision requirements of the robotic arm end contact force and position during surgical operations. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, embodiments of the present invention provide a force-position hybrid control method and system for a surgical robot arm to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a force-position hybrid control method for a surgical robot arm, comprising the following steps: S1. Obtain the time series of torque data of each joint of the robotic arm and the end force sensor data, perform frequency domain analysis on the time series, and extract the periodic characteristic parameters of human physiological motion; S2. Based on the periodic characteristic parameters, establish a human physiological motion prediction model, and use the physiological motion prediction model to generate the predicted value of tissue displacement and the predicted value of contact force fluctuation at the current moment in real time. S3. Based on the predicted tissue displacement value, feedforward compensation is performed on the expected end pose of the position control channel, and based on the predicted contact force fluctuation value, feedforward compensation is performed on the expected contact force of the force control channel, and the compensated position control command and force control command are generated respectively. S4. Based on the selection matrix, decouple and allocate the compensated position control command and force control command, map them to the corresponding control degree of freedom direction, and generate joint drive commands. S5. Drive the robotic arm joints to move based on the joint drive command, update the torque data of each joint and the end force sensor data in real time, use the updated data to correct the periodic characteristic parameters online, and return to S2 to execute the cycle.

[0007] Preferably, the steps for obtaining the periodic characteristic parameters are as follows: During the preset data acquisition period after the surgical robot's end arm establishes contact with human tissue, torque data of each joint of the robot arm and end force sensor data are collected simultaneously. The joint torque data and end-effector force data are arranged according to the acquisition time to form joint torque time series and end-effector force time series, respectively. Fast Fourier transforms were performed on the joint torque time series and the end force time series to obtain the joint torque spectrum and the end force spectrum, respectively. In the joint torque spectrum and the end force spectrum, frequency components with amplitudes exceeding a preset amplitude threshold are extracted respectively. The frequency value, amplitude, and phase of the extracted frequency components are recorded as the joint torque frequency domain parameter and the end force frequency domain parameter, respectively. The frequency domain parameters of joint torque and end force are weighted and fused, and the fusion result is used as the periodic characteristic parameter of human physiological motion.

[0008] Preferably, the steps for establishing the human physiological movement prediction model are as follows: The frequency, amplitude, and phase of each frequency component in the periodic feature parameters are extracted to establish a human physiological motion prediction model. The prediction model takes the current time as input, outputs the physiological motion prediction value at the current time, and uses the physiological motion prediction value as the tissue displacement prediction value at the current time. Within a preset acquisition period, the change in data from the end force sensor is extracted, and kinematic calculations are performed on the joint encoder data to obtain the change in tissue displacement. Calculate the ratio of the change in end contact force to the change in tissue displacement, and use this ratio as the end contact stiffness; Multiplying the end contact stiffness by the predicted tissue displacement yields the predicted contact force fluctuation at the current moment.

[0009] Preferably, the steps for generating the position control command are as follows: Differentiate the predicted tissue displacement value to obtain the predicted tissue velocity value at the current moment; Determine the direction of the predicted tissue motion speed. If the predicted tissue motion speed direction points to the end of the robotic arm, then move the desired end pose of the position control channel backward along the contact normal to the displacement corresponding to the predicted tissue displacement value, and generate a compensated position control command. If the predicted direction of the tissue motion speed deviates from the end of the robotic arm, the displacement corresponding to the predicted tissue displacement value of the desired end pose of the position control channel is advanced along the contact normal, and a compensated position control command is generated.

[0010] Preferably, the steps for generating the force control command are as follows: Determine the direction of the predicted tissue movement speed. If the predicted tissue movement speed is pointing towards the end of the robotic arm, then the difference between the preset expected contact force and the predicted contact force fluctuation is used as the compensated expected contact force. If the predicted direction of the tissue movement speed is away from the end of the robotic arm, the sum of the preset expected contact force and the predicted value of the contact force fluctuation will be used as the compensated expected contact force. Force control commands are generated based on the deviation between the compensated expected contact force and the end force sensor data.

[0011] Preferably, the step of generating joint drive commands is as follows: Based on the selection matrix, the compensated position control command is assigned to the position control degree of freedom direction, and the force control command is assigned to the force control degree of freedom direction. The two are superimposed in Cartesian space to generate a Cartesian space composite command. Based on the Jacobian matrix of the robotic arm, the Cartesian space synthesis instructions are mapped to the joint space to generate joint drive instructions.

[0012] Preferably, the steps for online correction of the periodic characteristic parameters are as follows: The movement of each joint of the robotic arm is driven by joint drive commands, and the torque data of each joint and the end force sensor data are updated in real time. The difference between the current physiological motion prediction value output by the human physiological motion prediction model and the measured value of the end force sensor is calculated, and the difference is used as the prediction error at the current moment. Based on the prediction error, the frequency value, amplitude and phase of each frequency component in the periodic characteristic parameter are corrected online to obtain the corrected periodic characteristic parameter. Return the corrected periodic feature parameters to S2, update the human physiological motion prediction model, and repeat S2 to S5.

[0013] To achieve the above objectives, the present invention provides the following technical solution: a force-position hybrid control system for a surgical robot arm, comprising implementing the above-mentioned force-position hybrid control method for the surgical robot arm, including: Parameter extraction module: used to acquire time series of torque data of each joint of the robotic arm and end force sensor data, perform frequency domain analysis on the time series, and extract periodic characteristic parameters of human physiological motion; Predictive modeling module: Based on the periodic feature parameters, establish a human physiological motion prediction model, and use the physiological motion prediction model to generate the predicted value of tissue displacement and the predicted value of contact force fluctuation at the current moment in real time; Feedforward compensation module: Based on the predicted tissue displacement value, feedforward compensation is performed on the expected end pose of the position control channel, and based on the predicted contact force fluctuation value, feedforward compensation is performed on the expected contact force of the force control channel, generating compensated position control commands and force control commands respectively. Decoupling and Allocation Module: Based on the selection matrix, the compensated position control command and force control command are decoupled and allocated, and the two are mapped to the corresponding control degree of freedom directions to generate joint drive commands; Online correction module: Based on the joint drive command, the robot arm joints are driven to move, and the torque data of each joint and the end force sensor data are updated in real time. The updated data is used to correct the periodic characteristic parameters online, and the module is returned to the prediction modeling module for repeated execution.

[0014] As described above, the force-position hybrid control method and system for the surgical robot arm provided by the present invention has at least the following beneficial effects: The force-position hybrid control method and system for surgical robot arms provided by this invention, by simultaneously acquiring torque data of each joint and end-effector force sensor data during contact operation and performing frequency domain analysis, transforms human physiological motion from imperceptible random disturbances in the traditional force-position hybrid control framework into quantifiable periodic characteristic parameters. This allows for the establishment of a physiological motion prediction model that outputs predicted tissue displacement and contact force fluctuations in real time, providing a unified data foundation for the coordinated compensation of force control and position control channels. This fundamentally changes the existing method of passively responding to physiological motion disturbances.

[0015] In the feedforward compensation stage, this invention introduces a direction determination mechanism for the predicted tissue motion velocity. The compensation logic of the two channels is dynamically switched based on the tissue motion direction, ensuring that the position control channel and force control channel coordinate their actions based on the same physical judgment at any given time. This eliminates command conflicts caused by the two channels independently sensing physiological motion disturbances, ensuring the stability of the force-position hybrid control of the surgical robot arm under physiological motion conditions. Furthermore, this invention introduces an online correction mechanism for periodic characteristic parameters in the closed-loop control loop. It continuously corrects the frequency, amplitude, and phase of the physiological motion prediction model using prediction errors, enabling the prediction model to track the slow drift of physiological motion characteristics. This ensures the continuous effectiveness of prediction accuracy during long-duration surgical operations, further improving the dual-channel control accuracy of the force-position hybrid control. Attached Figure Description

[0016] The present invention will be further described with reference to the accompanying drawings, but the embodiments in the drawings do not constitute any limitation on the present invention. For those skilled in the art, other drawings can be obtained based on the following drawings without creative effort.

[0017] Figure 1 This is a flowchart illustrating the force-position hybrid control method for the surgical robot arm of the present invention.

[0018] Figure 2 This is a schematic diagram of the force-position hybrid control system of the surgical robot arm of the present invention. Detailed Implementation

[0019] 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.

[0020] Example 1 Please see Figure 1As shown, the present invention provides a force-position hybrid control method for a surgical robot arm, comprising the following steps: S1. Obtain the time series of torque data of each joint of the robotic arm and the end force sensor data, perform frequency domain analysis on the time series, and extract the periodic characteristic parameters of human physiological motion; The steps for obtaining the periodic feature parameters are as follows: During the preset data acquisition period after the surgical robot's end arm establishes contact with human tissue, torque data of each joint of the robot arm and end force sensor data are collected simultaneously. The joint torque data and end-effector force data are arranged according to the acquisition time to form joint torque time series and end-effector force time series, respectively. Fast Fourier transforms were performed on the joint torque time series and the end force time series to obtain the joint torque spectrum and the end force spectrum, respectively. In the joint torque spectrum and the end force spectrum, frequency components with amplitudes exceeding a preset amplitude threshold are extracted respectively. The frequency value, amplitude, and phase of the extracted frequency components are recorded as the joint torque frequency domain parameter and the end force frequency domain parameter, respectively. The frequency domain parameters of joint torque and end force are weighted and fused, and the fusion result is used as the periodic characteristic parameter of human physiological motion.

[0021] In this embodiment, the torque data for each joint is output by torque sensors installed at each joint of the robotic arm, including the torque value borne by each joint at the current moment and the corresponding acquisition timestamp; the end effector force sensor data is output by a force sensor installed at the end flange of the robotic arm, including the triaxial component values ​​of the end contact force and the corresponding acquisition timestamp. After the end effector of the surgical robot establishes contact with human tissue, the periodic displacement of the tissue caused by physiological activities such as breathing and heartbeat is transmitted to the end effector through the contact interface. The end effector force sensor senses the periodic fluctuations of the contact force, and the torque sensors of each joint synchronously sense the periodic changes in the joint torque caused by this, so that both the joint torque time series and the end effector force time series contain the periodic components of physiological motion.

[0022] In this embodiment, the preset acquisition period can be set to cover the duration of several complete physiological movement cycles. For example, considering that the human respiratory frequency is approximately 0.2Hz to 0.3Hz, corresponding to a cycle of approximately 3 to 5 seconds; and the heart rate is approximately 1Hz to 1.5Hz, corresponding to a cycle of approximately 0.7 to 1 second, to ensure sufficient frequency resolution for the subsequent Fast Fourier Transform (FFT), the preset acquisition period can be set to 10 seconds. This covers several complete respiratory cycles and several complete heart rate cycles, ensuring that the FFT has sufficient frequency resolution and can effectively distinguish between respiratory and heart rate components. It should be noted that the specific value of the preset acquisition period can be adjusted according to the patient's physiological state in the actual surgical scenario; the above values ​​are merely illustrative examples.

[0023] In this embodiment, the joint torque time series and the end-effector force time series are subjected to Fast Fourier Transform (FFT) to obtain the joint torque spectrum and the end-effector force spectrum, respectively. Both spectra are plotted with frequency on the horizontal axis and amplitude on the vertical axis, reflecting the energy distribution of each frequency component in the signal. Human physiological motion is a continuous and stable periodic motion, which manifests as significant amplitude peaks at several discrete frequencies in the spectrum. Components from non-physiological sources, such as joint friction of the robotic arm and jitter in the controller output, are distributed over a wider frequency range, with relatively low amplitudes and no obvious peak characteristics. The two can be distinguished by their spectral morphology.

[0024] In this embodiment, the preset amplitude threshold is determined as follows: Before the start of the preset acquisition period, the robotic arm's end effector contacts human tissue but remains stationary. Joint torque data and end effector force sensor data for the same duration as the preset acquisition period are acquired. A Fast Fourier Transform is performed on the data to obtain the background spectrum under static contact conditions. The mean amplitude of all frequency points in the background spectrum is used as the preset amplitude threshold. Frequency components with amplitudes exceeding this threshold in the joint torque spectrum and end effector force spectrum are extracted as physiological motion-related frequency components. It should be noted that the mean amplitude of the background spectrum under static contact conditions reflects the combined level of sensor background noise and robotic arm structural vibration. Using this as a threshold can effectively distinguish between periodic amplitude peaks caused by physiological motion and background noise.

[0025] In this embodiment, the specific method of weighted fusion is as follows: taking the frequency component corresponding to respiratory motion as an example, let the amplitude of the frequency component extracted from the joint torque spectrum be _____. The amplitude of the corresponding frequency component extracted from the end force spectrum is The corresponding preset weighting coefficients are as follows: and ,and The amplitude after fusion The calculation formula is: ; The phase after fusion is based on the phase in the end force spectrum parameters because the end force sensor directly senses the force change at the contact interface, and its phase has a more direct temporal relationship with tissue movement; the frequency value after fusion is the arithmetic mean of the two extracted results. The fused frequency values Amplitude and phase Together they constitute the periodic characteristic parameters, which are output to step S2 to establish a human physiological motion prediction model.

[0026] It should be noted that after the preset data collection period ends, the signal-to-noise ratio (SNR) of the joint torque time series and the end force time series at the physiological motion frequency components is calculated separately. Data sources with higher SNR correspond to larger weighting coefficients.

[0027] In one specific embodiment, if the signal-to-noise ratio (SNR) of the joint torque time series at that frequency component is 15 dB and the SNR of the end force time series at that frequency component is 20 dB, then weights can be allocated according to the SNR ratio. Take 0.43, Take 0.57.

[0028] S2. Based on the periodic characteristic parameters, establish a human physiological motion prediction model, and use the physiological motion prediction model to generate the predicted value of tissue displacement and the predicted value of contact force fluctuation at the current moment in real time. The steps for establishing a human physiological movement prediction model are as follows: The frequency, amplitude, and phase of each frequency component in the periodic feature parameters are extracted to establish a human physiological motion prediction model. The prediction model takes the current time as input, outputs the physiological motion prediction value at the current time, and uses the physiological motion prediction value as the tissue displacement prediction value at the current time. Within a preset acquisition period, the change in data from the end force sensor is extracted, and kinematic calculations are performed on the joint encoder data to obtain the change in tissue displacement. Calculate the ratio of the change in end contact force to the change in tissue displacement, and use this ratio as the end contact stiffness; Multiplying the end contact stiffness by the predicted tissue displacement yields the predicted contact force fluctuation at the current moment.

[0029] In this embodiment, human physiological movements are caused by continuous physiological activities such as breathing and heartbeat. Essentially, they are periodic movements resulting from the superposition of multiple frequency components, which can be described using the superposition of cosine functions of each frequency component. The mathematical expression for the human physiological movement prediction model is as follows: ,in, This represents the predicted physiological movement value at time t, i.e., the predicted tissue displacement value at time t.N The total number of frequency components contained in the periodic characteristic parameter is determined by the number of frequency components extracted from S1. This represents the amplitude of the k-th frequency component; This represents the frequency value of the k-th frequency component. Let t represent the phase of the k-th frequency component, t represent the current time, and k be the number of each frequency component.

[0030] It should be noted that all parameters of the human physiological motion prediction model are directly assigned values ​​from the periodic characteristic parameters output by S1. Substituting the current time t into the above formula, the output is... As a physiological motion prediction value at the current moment, this prediction value reflects the periodic displacement of human tissue relative to the initial contact position at the current moment.

[0031] In this embodiment, the process of acquiring the end-effector contact stiffness is as follows: During a preset acquisition period, the end-effector force sensor continuously records the contact force value, and the joint encoder continuously records the rotation angle values ​​of each joint. The difference between the maximum and minimum values ​​of the end-effector force sensor data during the preset acquisition period is extracted as the change in end-effector contact force. Using the joint encoder data, the end-effector position time series is obtained through forward kinematics calculation of the robotic arm, and the difference between the maximum and minimum values ​​of the end-effector position time series during the preset acquisition period is extracted as the change in tissue displacement.

[0032] It should be noted that, within the preset data acquisition period, the periodic fluctuations in both the end-effector contact force and the end-effector position are caused by human physiological movements. The ratio of their changes reflects the linear relationship between force and displacement at the contact interface, physically representing the change in contact force per unit displacement, i.e., the end-effector contact stiffness. Dividing the change in end-effector contact force by the change in tissue displacement yields the end-effector contact stiffness. The method for kinematic calculation of the joint encoder data is common knowledge in this field and will not be elaborated upon here.

[0033] S3. Based on the predicted tissue displacement value, feedforward compensation is performed on the expected end pose of the position control channel, and based on the predicted contact force fluctuation value, feedforward compensation is performed on the expected contact force of the force control channel, and the compensated position control command and force control command are generated respectively. The steps for generating the position control command are as follows: Differentiate the predicted tissue displacement value to obtain the predicted tissue velocity value at the current moment; Determine the direction of the predicted tissue motion speed. If the predicted tissue motion speed direction points to the end of the robotic arm, then move the desired end pose of the position control channel backward along the contact normal to the displacement corresponding to the predicted tissue displacement value, and generate a compensated position control command. If the predicted direction of the tissue motion speed deviates from the end of the robotic arm, the displacement corresponding to the predicted tissue displacement value of the desired end pose of the position control channel is advanced along the contact normal, and a compensated position control command is generated.

[0034] In this embodiment, the predicted tissue displacement value output by S2 is... Differentiating with respect to time t yields the predicted value of the tissue's velocity at the current moment. .

[0035] The positive and negative values ​​reflect the direction of tissue movement: the direction along the contact normal pointing towards the end of the robotic arm is defined as the positive direction. This indicates that the tissue is moving towards the end of the robotic arm. This indicates that the tissue is moving away from the direction of the robotic arm's end effector.

[0036] when When tissue is pressed into the end effector of the robotic arm, if the position control channel maintains the original desired pose, the end effector will passively bear the tissue compression, causing the contact force to exceed the expected value. In this case, the desired end effector pose should be moved backward along the contact method. The corresponding displacement causes the end effector of the robotic arm to actively move aside, generating a compensated position control command.

[0037] when At time 0, the tissue moves away from the robotic arm's end effector. If the position control channel maintains the original desired pose, the contact force between the robotic arm's end effector and the tissue will decrease or even disappear. At this point, the desired robotic arm end effector pose is advanced along the contact method. The corresponding displacement causes the robotic arm end effector to actively follow, generating a compensated position control command.

[0038] In this embodiment, the contact normal direction is determined as follows: within a preset acquisition period, the time series of the robotic arm's end-effector position is obtained through forward kinematics calculation using joint encoder data. Principal component analysis is then performed on the time series of the robotic arm's end-effector position, and the direction of the principal component with the largest variance contribution rate is taken as the contact normal direction. This is because tissue displacement caused by human physiological movement is mainly transmitted to the robotic arm's end-effector along the normal direction of the contact interface, and the principal motion direction in the time series of the robotic arm's end-effector position has a strong correspondence with the contact normal direction.

[0039] Within the preset data acquisition period, physiological movements are not yet significantly affected by the active adjustment of the robotic arm. Therefore, the time series of the robotic arm's end-effector position acquired during this period can accurately reflect the passive displacement caused by tissue physiological movements. Principal component analysis based on the data from this period can extract the normal direction, avoiding interference from active movements after intervention on the normal direction estimation.

[0040] The steps for generating the force control command are as follows: Determine the direction of the predicted tissue movement speed. If the predicted tissue movement speed is pointing towards the end of the robotic arm, then the difference between the preset expected contact force and the predicted contact force fluctuation is used as the compensated expected contact force. If the predicted direction of the tissue movement speed is away from the end of the robotic arm, the sum of the preset expected contact force and the predicted value of the contact force fluctuation will be used as the compensated expected contact force. Force control commands are generated based on the deviation between the compensated expected contact force and the end force sensor data.

[0041] In this embodiment, the compensation logic of the force control channel and the position control channel share the same predicted tissue motion velocity value. The direction judgment result indicates that the two channels coordinate their actions based on the same physical judgment.

[0042] when When tissue is pressed into the end effector of the robotic arm, the contact force at the end effector increases due to tissue compression. This increase is the predicted contact force fluctuation value output by S2. If the force control channel still targets the preset desired contact force, it will misjudge the increase in contact force caused by physiological movement as an error that needs to be eliminated and output a retreat command, causing a conflict with the position control channel. In this case, the predicted contact force fluctuation value is subtracted from the preset desired contact force to obtain the compensated desired contact force. The force control channel then targets the compensated desired contact force and no longer responds to the increase in contact force caused by physiological movement. It generates a force control command based on the deviation between the compensated desired contact force and the end effector force sensor data.

[0043] when When the tissue moves away from the end effector of the robotic arm, the contact force at the end effector will decrease due to the tissue moving away. At this time, the preset expected contact force is added to the predicted value of the contact force fluctuation to obtain the compensated expected contact force. The force control channel takes the compensated expected contact force as the target and actively increases the force control target to maintain contact. The force control command is generated based on the deviation between the compensated expected contact force and the end effector force sensor data.

[0044] It is important to note that in existing force-position hybrid control feedforward compensation methods, the compensation direction is usually fixed, meaning that compensation is performed in the same direction regardless of the direction of the external disturbance. In this embodiment, however, the compensation direction is dynamically determined by judging the direction of tissue movement velocity. Specifically, when the tissue moves towards the end effector of the robotic arm, the robotic arm actively yields and reduces the contact force; when the tissue moves away from the end effector, the robotic arm actively follows and increases the force. This direction-aware, differentiated response control strategy allows the robotic arm to proactively adjust its control behavior according to changes in the external environment, rather than passively compensating for displacements that have already occurred.

[0045] S4. Based on the selection matrix, decouple and allocate the compensated position control command and force control command, map them to the corresponding control degree of freedom direction, and generate joint drive commands. The steps for generating joint drive commands are as follows: Based on the selection matrix, the compensated position control command is assigned to the position control degree of freedom direction, and the force control command is assigned to the force control degree of freedom direction. The two are superimposed in Cartesian space to generate a Cartesian space composite command. Based on the Jacobian matrix of the robotic arm, the Cartesian space synthesis instructions are mapped to the joint space to generate joint drive instructions.

[0046] In this embodiment, a diagonal matrix is ​​selected, with its diagonal elements corresponding to the motion degrees of freedom directions of the robotic arm's end effector. Elements corresponding to the force control degrees of freedom are set to 1, and elements corresponding to the position control degrees of freedom are set to 0. Based on the selection matrix, the compensated position control commands are assigned to the position control degrees of freedom directions, and the force control commands are assigned to the force control degrees of freedom directions. The two commands act independently and do not interfere with each other in their respective degree of freedom directions. The two commands are superimposed in Cartesian space to obtain a Cartesian space composite command.

[0047] It should be noted that the Jacobian matrix is ​​calculated in real time from the encoder data of each joint at the current moment to ensure that the mapping relationship is consistent with the current configuration of the robotic arm.

[0048] S5. Drive the robotic arm joints to move based on the joint drive command, update the torque data of each joint and the end force sensor data in real time, use the updated data to correct the periodic characteristic parameters online, and return to S2 to execute the cycle.

[0049] The steps for online correction of periodic characteristic parameters are as follows: The movement of each joint of the robotic arm is driven by joint drive commands, and the torque data of each joint and the end force sensor data are updated in real time. The difference between the current physiological motion prediction value output by the human physiological motion prediction model and the measured value of the end force sensor is calculated, and the difference is used as the prediction error at the current moment. Based on the prediction error, the frequency value, amplitude and phase of each frequency component in the periodic characteristic parameter are corrected online to obtain the corrected periodic characteristic parameter. Return the corrected periodic feature parameters to S2, update the human physiological motion prediction model, and repeat S2 to S5.

[0050] In this embodiment, the prediction error is calculated as follows: the measured value of the end force sensor at the current moment is subtracted from the physiological motion prediction value output by the human physiological motion prediction model at the same moment, and the difference is the prediction error at the current moment. A positive prediction error indicates that the actual physiological motion amplitude is greater than the predicted value, and a negative prediction error indicates that the actual physiological motion amplitude is less than the predicted value.

[0051] In this embodiment, the gradient descent method is employed, with the optimization objective of minimizing the prediction error, to iteratively correct the amplitude and phase of the periodic characteristic parameters. Specifically, the partial derivatives of the prediction error with respect to the amplitude and phase are calculated respectively, and the amplitude and phase are adjusted along the negative gradient direction so that the model output gradually approximates the sensor's measured values.

[0052] The frequency value is corrected by zero-crossing detection of the prediction error. Over several consecutive control cycles, the time interval between the prediction error sign flips is statistically analyzed. The time interval between two adjacent sign flips is multiplied by two to obtain an estimate of the current actual physiological cycle. The reciprocal of this estimate is used to obtain the frequency estimate, which replaces the current frequency value.

[0053] The corrections for amplitude, phase, and frequency values ​​are performed sequentially: frequency correction precedes amplitude and phase correction, as frequency accuracy is a prerequisite for amplitude and phase correction. The correction process is performed once per control cycle, and the corrected cycle characteristic parameters are immediately used to update the prediction model, enabling the model to continuously track real-time changes in physiological motion.

[0054] Example 2 Please see Figure 2 As shown, the present invention provides a force-position hybrid control system for a surgical robot arm, including a parameter extraction module, a prediction modeling module, a feedforward compensation module, a decoupling allocation module, and an online correction module.

[0055] Parameter extraction module: used to acquire time series of torque data of each joint of the robotic arm and end force sensor data, perform frequency domain analysis on the time series, and extract periodic characteristic parameters of human physiological motion; Predictive modeling module: Based on the periodic feature parameters, establish a human physiological motion prediction model, and use the physiological motion prediction model to generate the predicted value of tissue displacement and the predicted value of contact force fluctuation at the current moment in real time; Feedforward compensation module: Based on the predicted tissue displacement value, feedforward compensation is performed on the expected end pose of the position control channel, and based on the predicted contact force fluctuation value, feedforward compensation is performed on the expected contact force of the force control channel, generating compensated position control commands and force control commands respectively. Decoupling and Allocation Module: Based on the selection matrix, the compensated position control command and force control command are decoupled and allocated, and the two are mapped to the corresponding control degree of freedom directions to generate joint drive commands; Online correction module: Based on the joint drive command, the robot arm joints are driven to move, and the torque data of each joint and the end force sensor data are updated in real time. The updated data is used to correct the periodic characteristic parameters online, and the module is returned to the prediction modeling module for repeated execution.

[0056] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

[0057] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A force-position hybrid control method for a surgical robot arm, characterized in that, include: S1. Obtain the time series of torque data of each joint of the robotic arm and the end force sensor data, perform frequency domain analysis on the time series, and extract the periodic characteristic parameters of human physiological motion; S2. Based on the periodic characteristic parameters, establish a human physiological motion prediction model, and use the physiological motion prediction model to generate the predicted value of tissue displacement and the predicted value of contact force fluctuation at the current moment in real time. S3. Based on the predicted tissue displacement value, feedforward compensation is performed on the expected end pose of the position control channel, and based on the predicted contact force fluctuation value, feedforward compensation is performed on the expected contact force of the force control channel, and the compensated position control command and force control command are generated respectively. S4. Based on the selection matrix, decouple and allocate the compensated position control command and force control command, map them to the corresponding control degree of freedom direction, and generate joint drive commands. S5. Drive the robotic arm joints to move based on the joint drive command, update the torque data of each joint and the end force sensor data in real time, use the updated data to correct the periodic characteristic parameters online, and return to S2 to execute the cycle.

2. The force-position hybrid control method for the surgical robot arm according to claim 1, characterized in that, The steps for obtaining the periodic feature parameters are as follows: During the preset data acquisition period after the surgical robot's end arm establishes contact with human tissue, torque data of each joint of the robot arm and end force sensor data are collected simultaneously. The joint torque data and end-effector force data are arranged according to the acquisition time to form joint torque time series and end-effector force time series, respectively. Fast Fourier transforms were performed on the joint torque time series and the end force time series to obtain the joint torque spectrum and the end force spectrum, respectively. In the joint torque spectrum and the end force spectrum, frequency components with amplitudes exceeding a preset amplitude threshold are extracted respectively. The frequency value, amplitude, and phase of the extracted frequency components are recorded as the joint torque frequency domain parameter and the end force frequency domain parameter, respectively. The frequency domain parameters of joint torque and end force are weighted and fused, and the fusion result is used as the periodic characteristic parameter of human physiological motion.

3. The force-position hybrid control method for the surgical robot arm according to claim 1, characterized in that, The steps for establishing a human physiological movement prediction model are as follows: The frequency, amplitude, and phase of each frequency component in the periodic feature parameters are extracted to establish a human physiological motion prediction model. The prediction model takes the current time as input, outputs the physiological motion prediction value at the current time, and uses the physiological motion prediction value as the tissue displacement prediction value at the current time. Within a preset acquisition period, the change in data from the end force sensor is extracted, and kinematic calculations are performed on the joint encoder data to obtain the change in tissue displacement. Calculate the ratio of the change in end contact force to the change in tissue displacement, and use this ratio as the end contact stiffness; Multiplying the end contact stiffness by the predicted tissue displacement yields the predicted contact force fluctuation at the current moment.

4. The force-position hybrid control method for the surgical robot arm according to claim 1, characterized in that, The steps for generating the position control command are as follows: Differentiate the predicted tissue displacement value to obtain the predicted tissue velocity value at the current moment; Determine the direction of the predicted tissue motion speed. If the predicted tissue motion speed direction points to the end of the robotic arm, then move the desired end pose of the position control channel backward along the contact normal to the displacement corresponding to the predicted tissue displacement value, and generate a compensated position control command. If the predicted direction of the tissue motion speed deviates from the end of the robotic arm, the displacement corresponding to the predicted tissue displacement value of the desired end pose of the position control channel is advanced along the contact method to generate a compensated position control command.

5. The force-position hybrid control method for the surgical robot arm according to claim 1, characterized in that, The steps for generating the force control command are as follows: Determine the direction of the predicted tissue movement speed. If the predicted tissue movement speed is pointing towards the end of the robotic arm, then the difference between the preset expected contact force and the predicted contact force fluctuation is used as the compensated expected contact force. If the predicted direction of the tissue movement speed is away from the end of the robotic arm, the sum of the preset expected contact force and the predicted value of the contact force fluctuation will be used as the compensated expected contact force. Force control commands are generated based on the deviation between the compensated expected contact force and the end force sensor data.

6. The force-position hybrid control method for the surgical robot arm according to claim 1, characterized in that, The steps for generating joint drive commands are as follows: Based on the selection matrix, the compensated position control command is assigned to the position control degree of freedom direction, and the force control command is assigned to the force control degree of freedom direction. The two are superimposed in Cartesian space to generate a Cartesian space composite command. Based on the Jacobian matrix of the robotic arm, the Cartesian space synthesis instructions are mapped to the joint space to generate joint drive instructions.

7. The force-position hybrid control method for the surgical robot arm according to claim 1, characterized in that, The steps for online correction of periodic characteristic parameters are as follows: The movement of each joint of the robotic arm is driven by joint drive commands, and the torque data of each joint and the end force sensor data are updated in real time. The difference between the current physiological motion prediction value output by the human physiological motion prediction model and the measured value of the end force sensor is calculated, and the difference is used as the prediction error at the current moment. Based on the prediction error, the frequency value, amplitude and phase of each frequency component in the periodic characteristic parameter are corrected online to obtain the corrected periodic characteristic parameter. Return the corrected periodic feature parameters to S2, update the human physiological motion prediction model, and repeat S2 to S5.

8. A force-position hybrid control system for a surgical robot arm, used to implement the force-position hybrid control method for the surgical robot arm according to any one of claims 1-7, characterized in that, include: Parameter extraction module: used to acquire time series of torque data of each joint of the robotic arm and end force sensor data, perform frequency domain analysis on the time series, and extract periodic characteristic parameters of human physiological motion; Predictive modeling module: Based on the periodic feature parameters, establish a human physiological motion prediction model, and use the physiological motion prediction model to generate the predicted value of tissue displacement and the predicted value of contact force fluctuation at the current moment in real time; Feedforward compensation module: Based on the predicted tissue displacement value, feedforward compensation is performed on the expected end pose of the position control channel, and based on the predicted contact force fluctuation value, feedforward compensation is performed on the expected contact force of the force control channel, generating compensated position control commands and force control commands respectively. Decoupling and Allocation Module: Based on the selection matrix, the compensated position control command and force control command are decoupled and allocated, and the two are mapped to the corresponding control degree of freedom directions to generate joint drive commands; Online correction module: Based on the joint drive command, the robot arm joints are driven to move, and the torque data of each joint and the end force sensor data are updated in real time. The updated data is used to correct the periodic characteristic parameters online, and the module is returned to the prediction modeling module for repeated execution.

Images