Joint control method, system and storage medium of a surgical robot

Abstract

The application provides a joint control method, system and storage medium of a surgical robot, the method comprising: in an offline initialization stage, controlling the joint to run along a preset trajectory, synchronously collecting motor electric angle and joint output torque, performing equal-angle resampling and fast Fourier transform, obtaining each harmonic sine and cosine coefficient of an initialization torque fluctuation model, in an online compensation stage, monitoring motor phase voltage and current in real time, constructing a voltage balance equation and online estimating winding resistance by using a recursive least square method; constructing a feedforward compensation model by using the initialization torque fluctuation model, taking the harmonic coefficient as a state vector to build a Kalman filter, taking residual torque error as an observation value, combining winding resistance variation to adjust process noise, realizing recursive updating of the state vector, and outputting a feedforward compensation torque in real time; and simultaneously using a digital band-stop filter to suppress inherent resonance of a joint connecting rod and improve joint control precision and stability.

Description

Technical Field

[0001] This application belongs to the field of control, and in particular relates to a joint control method, system and storage medium for a surgical robot. Background Technology

[0002] Surgical robots, as high-end medical devices, typically utilize permanent magnet synchronous motors (PMSMs) for direct or indirect drive of their core joints, enabling high-precision and highly stable surgical operations. However, the inherent non-ideal characteristics of PMSMs, such as cogging torque, flux harmonics, current measurement errors, and inverter nonlinearity, cause periodic fluctuations in the motor's output torque, known as torque pulsation. This torque pulsation is transmitted to the robot joints, reducing motion smoothness and end-effector positioning accuracy. In demanding and delicate surgeries, this can affect surgical outcomes and even threaten patient safety. To suppress torque pulsation, existing technologies often employ feedforward compensation methods based on offline identification. This method pre-measures and establishes a functional relationship model between torque fluctuations and the motor's electrical angle before operation. During actual operation, the model is consulted based on the real-time electrical angle, and the calculated compensation torque is superimposed onto the control commands.

[0003] The torque fluctuation model established by the traditional offline feedforward compensation method described above cannot adapt to the changing operating conditions of the motor during operation. The motor winding resistance experiences a temperature rise effect with increasing load and operating time, causing changes in resistance value. These changes affect the motor's electromagnetic torque constant, gradually rendering the pre-calibrated static compensation model ineffective and reducing compensation accuracy. Surgical robots, in pursuit of lightweight response, often have flexible linkage structures, which are prone to triggering inherent resonances in the mechanical structure during rapid movements, generating vibrations and affecting stability. Traditional controller planning requires a trade-off between suppressing vibration and ensuring response speed. Furthermore, while some compensation methods can update the model online, the update rate is usually set to a fixed value, failing to account for the parameter changes under different heating conditions. When temperature changes rapidly, the update rate cannot keep up; when changes are slow, it may utilize noise to cause instability. Adjusting the model update rate according to the actual thermal state of the motor is a key technical challenge for improving compensation effectiveness. Summary of the Invention

[0004] To address the problem that existing technologies are unable to adapt to changing operating conditions during motor operation and cannot adjust the model update rate according to the actual thermal state of the motor.

[0005] In a first aspect, this disclosure provides a method for joint control of a surgical robot, comprising: During the offline initialization phase, the joints of the surgical robot are controlled to run along a preset trajectory. Simultaneously, the electrical angle of the motor and the output torque data of the joint are collected. The data are resampled at equal angular intervals, and the initialization parameters are extracted through fast Fourier transform. During the online compensation phase, the phase current and phase voltage of the motor are monitored in real time. The motor voltage balance equation is constructed and the recursive least squares method is used to estimate the motor winding resistance online. A feedforward compensation model based on the initial torque fluctuation model is constructed, and the sine and cosine coefficients of the harmonic components in the model are used as state vectors to establish a Kalman filter. The current command value output by the joint servo driver is filtered out for low-frequency drive components by a high-pass filter. Then, the current value corresponding to the compensation torque output by the feedforward compensation model in the previous cycle is subtracted to obtain the residual current command. The residual current command is multiplied by the motor torque constant to convert it into residual torque error, which is used as the observation value of the Kalman filter. Based on the change in the winding resistance estimate, the process noise covariance matrix is ​​updated, and the update rate of the state vector is adjusted. According to the real-time acquired motor electrical angle and the updated state vector, the feedforward compensation torque is calculated in real time through the feedforward compensation model and converted into a current superimposed on the driver command. The current command value output by the joint servo driver is filtered in real time through one or more digital band-stop filters; the filtered current command is then sent to the motor driver.

[0006] Optionally, the control system operates the surgical robot joints along a preset trajectory, synchronously collecting electrical angle data of the motors and output torque data of the joints. The data is then resampled at equal angular intervals, and initialization parameters are extracted using a fast Fourier transform, including: The surgical robot joints are controlled to run at a constant speed for at least one complete mechanical rotation. The electrical angles of the motors and the output torque of the joints are collected synchronously. The original data is resampled at fixed electrical angle intervals. The resampled joint output torque data is subjected to a fast Fourier transform to extract the sine and cosine coefficients of the predetermined harmonic orders, namely the first, second, fourth, and sixth harmonics, as initialization parameters.

[0007] Optionally, the real-time monitoring of motor phase current and phase voltage, and the online estimation of motor winding resistance by constructing a motor voltage balance equation and using the recursive least squares method, includes: A motor voltage balance equation is constructed based on phase voltage, phase current, phase current change rate, motor electric angular velocity, winding resistance, winding inductance, and back electromotive force coefficient. The real-time acquired phase voltage, phase current, and motor electric angular velocity are substituted into the equation, the winding resistance is used as the parameter to be estimated, and the phase current is used as the regression vector. The equation is then iteratively calculated using a recursive least squares method with a forgetting factor, where the forgetting factor is set to a value between 0.99 and 0.999.

[0008] Optionally, the step of filtering out low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, subtracting the current value corresponding to the compensation torque output by the feedforward compensation model of the previous cycle to obtain the residual current command, and multiplying the residual current command by the motor torque constant to convert it into residual torque error, which is then used as the observation value of the Kalman filter, includes: The sine and cosine coefficients of the first, second, fourth, and sixth harmonics of the initial torque wave model are constructed into an eight-dimensional state vector. After filtering out low-frequency drive components by a high-pass filter, the current command value output by the servo driver is subtracted from the compensation current value obtained by dividing the feedforward compensation torque calculated in the previous cycle by the motor torque constant to obtain the residual current command. The residual current command is then multiplied by the motor torque constant to obtain the residual torque error, which is used as the single-dimensional observation value of the Kalman filter.

[0009] Optionally, the step of updating the process noise covariance matrix and adjusting the update rate of the state vector based on the change in the estimated winding resistance includes: In each control cycle, the absolute value of the difference between the estimated winding resistance at the current moment and the estimated value at the previous moment is calculated to obtain the resistance change. The resistance change is multiplied by a preset proportional coefficient, and the resulting product is added to a preset base noise value. The diagonal element value corresponding to the state vector in the process noise covariance matrix is ​​updated with the sum of the sums.

[0010] Optionally, the step of calculating the feedforward compensation torque in real time based on the real-time acquired motor electrical angle and the updated state vector using a feedforward compensation model includes: Based on the real-time acquired electric angle of the motor and the updated harmonic sine and cosine coefficients of the Kalman filter, the feedforward compensation torque is calculated by summing the Fourier series of selected harmonic orders, including the first, second, fourth, and sixth harmonic orders.

[0011] Optionally, the step of real-time filtering the current command value output by the joint servo driver through one or more digital band-stop filters includes: Plan one or more second-order infinite impulse response bandstop filters, such that the center frequency of the stopband of the filter corresponds to the first and second order natural resonant frequencies of the linkage structure of the surgical robot, respectively. In each control cycle, the current command value output by the joint servo driver is input into the filter for forward calculation.

[0012] In a second aspect, this disclosure provides a joint control system for a surgical robot, comprising the following modules: The offline initialization module is used to control the joints of the surgical robot to run on a preset trajectory, synchronously collect the electrical angle of the motor and the output torque data of the joint, resample the data at equal angle intervals, and extract the initialization parameters through fast Fourier transform; The online compensation module is used to monitor the phase current and phase voltage of the motor in real time. By constructing the motor voltage balance equation and using the recursive least squares method, the motor winding resistance value is estimated online. A feedforward compensation model based on the initial torque fluctuation model is constructed, and the sine and cosine coefficients of the harmonic components in the model are used as state vectors to establish a Kalman filter. The generation module is used to filter out low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, subtract the current value corresponding to the compensation torque output by the feedforward compensation model in the previous cycle to obtain the residual current command, and multiply the residual current command by the motor torque constant to convert it into residual torque error, which is used as the observation value of the Kalman filter; based on the change in the winding resistance estimate, the process noise covariance matrix is ​​updated and the update rate of the state vector is adjusted; according to the real-time acquired motor electrical angle and the updated state vector, the feedforward compensation torque is calculated in real time through the feedforward compensation model and converted into a current superimposed on the driver command; The output module is used to filter the current command value output by the joint servo driver in real time through one or more digital band-stop filters; and send the filtered current command to the motor driver.

[0013] Furthermore, the control system operates the surgical robot joints along a preset trajectory, synchronously collecting electrical angle data of the motors and output torque data of the joints. The data is resampled at equal angular intervals, and initialization parameters are extracted using a fast Fourier transform, including: The surgical robot joints are controlled to run at a constant speed for at least one complete mechanical rotation. The electrical angles of the motors and the output torque of the joints are collected synchronously. The original data is resampled at fixed electrical angle intervals. The resampled joint output torque data is subjected to a fast Fourier transform to extract the sine and cosine coefficients of the predetermined harmonic orders, namely the first, second, fourth, and sixth harmonics, as initialization parameters.

[0014] Furthermore, the real-time monitoring of motor phase current and phase voltage, by constructing a motor voltage balance equation and using the recursive least squares method, estimates the motor winding resistance online, including: A motor voltage balance equation is constructed based on phase voltage, phase current, phase current change rate, motor electric angular velocity, winding resistance, winding inductance, and back electromotive force coefficient. The real-time acquired phase voltage, phase current, and motor electric angular velocity are substituted into the equation, the winding resistance is used as the parameter to be estimated, and the phase current is used as the regression vector. The equation is then iteratively calculated using a recursive least squares method with a forgetting factor, where the forgetting factor is set to a value between 0.99 and 0.999.

[0015] Further, the step of filtering out low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, subtracting the current value corresponding to the compensation torque output by the feedforward compensation model of the previous cycle, to obtain the residual current command, and multiplying the residual current command by the motor torque constant to convert it into residual torque error, which is used as the observation value of the Kalman filter, includes: The sine and cosine coefficients of the first, second, fourth, and sixth harmonics of the initial torque wave model are constructed into an eight-dimensional state vector. After filtering out low-frequency drive components by a high-pass filter, the current command value output by the servo driver is subtracted from the compensation current value obtained by dividing the feedforward compensation torque calculated in the previous cycle by the motor torque constant to obtain the residual current command. The residual current command is then multiplied by the motor torque constant to obtain the residual torque error, which is used as the single-dimensional observation value of the Kalman filter.

[0016] Furthermore, the step of updating the process noise covariance matrix and adjusting the update rate of the state vector based on the change in the estimated winding resistance includes: In each control cycle, the absolute value of the difference between the estimated winding resistance at the current moment and the estimated value at the previous moment is calculated to obtain the resistance change. The resistance change is multiplied by a preset proportional coefficient, and the resulting product is added to a preset base noise value. The diagonal element value corresponding to the state vector in the process noise covariance matrix is ​​updated with the sum of the sums.

[0017] Further, the step of calculating the feedforward compensation torque in real time using a feedforward compensation model based on the real-time acquired motor electrical angle and the updated state vector includes: Based on the real-time acquired electric angle of the motor and the updated harmonic sine and cosine coefficients of the Kalman filter, the feedforward compensation torque is calculated by summing the Fourier series of selected harmonic orders, including the first, second, fourth, and sixth harmonic orders.

[0018] Furthermore, the real-time filtering of the current command value output by the joint servo driver through one or more digital band-stop filters includes: Plan one or more second-order infinite impulse response bandstop filters, such that the center frequency of the stopband of the filter corresponds to the first and second order natural resonant frequencies of the linkage structure of the surgical robot, respectively. In each control cycle, the current command value output by the joint servo driver is input into the filter for forward calculation.

[0019] This invention establishes a torque fluctuation feedforward compensation model and considers in real-time the impact of motor winding resistance changes with temperature on torque output accuracy. This allows for continuous correction of the compensation model, ensuring high-precision compensation for joint output torque fluctuations under various operating conditions, thereby improving the smoothness of surgical robot joint movements and trajectory tracking accuracy. Correlating the correction rate of the compensation model with motor temperature changes balances the convergence speed and stability of the compensation algorithm. By suppressing mechanical resonance, the risk of control system instability due to structural vibration is avoided, further enhancing the system's control performance and reliability in high-precision, high-stability operating scenarios. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the hardware device architecture provided by the present invention; Figure 2 A flowchart of a joint control method for a surgical robot provided by the present invention; Figure 3 This is a schematic diagram of the full-cycle control architecture provided by the present invention. Detailed Implementation

[0021] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this specification. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this specification as detailed in the appended claims.

[0022] It should be understood that the terms “comprising” and “having”, and any variations thereof, in the embodiments of this specification are intended to cover but not exclude inclusion. For example, a product or device that includes a series of components is not necessarily limited to those components that are explicitly listed, but may include other components that are not explicitly listed or that are inherent to such product or device.

[0023] Hardware device architecture such as Figure 1 As shown, it mainly consists of a central controller, servo driver, joint motors, position encoder, torque sensor, and robotic arm. In the first embodiment, this invention proposes a joint control method for a surgical robot, such as... Figure 2 As shown, it includes the following steps: S1, In the offline initialization phase, the joints of the surgical robot are controlled to run on a preset trajectory. The electrical angle of the motor and the output torque of the joint are collected synchronously. The data are resampled at equal angle intervals and the initialization parameters are extracted by fast Fourier transform. By programming the motion controller, the surgical robot can perform one or more complete mechanical cycles at a low, constant angular velocity under no-load conditions, reciprocating within the maximum permissible mechanical travel range, with angular velocity commands. The angular velocity is kept constant, such as 5° / s, to ensure smooth operation and reduce interference from factors such as nonlinear friction. A high-precision data acquisition card or the controller's built-in ADC module is used to synchronously acquire the electrical angle output from the motor encoder at a fixed time frequency, such as 5kHz. The output torque measured by the torque sensor installed at the joint output end Two sets of time series data were obtained. Perform equal-angle interval resampling to create a [model / structure]. A standard electrical angle sequence with a period of radians and containing N equally spaced points. For example, using Python's NumPy library function numpy.linspace from 0 to... Generate N points; use a linear interpolation algorithm, such as the scipy.interpolate.interp1d function in the SciPy library, to interpolate the time series data. Mapping onto the standard electrical angle sequence yields a new torque sequence. Each torque value in this sequence corresponds to a unique, equally spaced electrical angle. The torque sequence is then resampled at equal angular intervals. To perform a Fast Fourier Transform (FFT), you can use the `fft` function in MATLAB or the `scipy.fft.fft` function in Python's SciPy library. The result is a set of complex numbers, where the real and imaginary parts correspond to predetermined harmonic orders `n`, for example, `n=1, 2, 4, 6`, etc., and the cosine coefficients are... and sine coefficient By scaling the transformation result, the initial torque fluctuation model can be obtained. Sine coefficients of each harmonic in With cosine coefficient .

[0024] In one possible embodiment, the control of the surgical robot joints operates along a preset trajectory, synchronously acquiring electrical angle data of the motors and output torque data of the joints, resampling the data at equal angular intervals, and extracting initialization parameters through a fast Fourier transform, including: The surgical robot joints are controlled to run at a constant speed for at least one complete mechanical rotation. The electrical angles of the motors and the output torque of the joints are collected synchronously. The original data is resampled at fixed electrical angle intervals. The resampled joint output torque data is subjected to a fast Fourier transform to extract the sine and cosine coefficients of the predetermined harmonic orders, namely the first, second, fourth, and sixth harmonics, as initialization parameters.

[0025] The joint is set to operate at a relatively low, constant angular velocity, such as 5 degrees / second, to reduce the influence of viscous friction and inertial torque on the measurement. The range of motion must cover at least one 360-degree mechanical rotation; however, those skilled in the art will understand that the mechanical rotation does not necessarily have to be 360 ​​degrees, but rather based on the maximum permissible range. During this process, electrical angles provided by the motor's built-in encoder are synchronously acquired at a high sampling rate, such as 5 kHz. The torque value measured by the torque sensor installed at the joint output end To perform Fourier analysis, the time series data needs to be converted into angle series data. A fixed electrical angle interval is set. For example, one electrical cycle The radian is divided into 1024 points, that is... By analyzing the raw torque data... Perform linear interpolation to obtain the torque value at each equiangular interval. Fast Fourier Transform (FFT) is applied to the torque sequence covering the complete mechanical cycle after resampling. From the FFT results, based on the conversion relationship of the number of motor pole pairs, the amplitudes and phases corresponding to the first, second, fourth, and sixth harmonic components of the electrical angle are extracted, and the corresponding sine coefficients are calculated. With cosine coefficient The coefficient , , , , , , , This will be used as the initial value for the feedforward compensation model.

[0026] S2, In the online compensation stage, the phase current and phase voltage of the motor are monitored in real time. The motor voltage balance equation is constructed and the recursive least squares method is used to estimate the motor winding resistance value online. A feedforward compensation model based on the initial torque fluctuation model is constructed, and the sine coefficient and cosine coefficient of the harmonic components in the model are used as state vectors to establish a Kalman filter. The controller's built-in ADC module acquires the two-phase or three-phase current of the permanent magnet synchronous motor in real time at a frequency of 100μs per control cycle. , , and phase voltage , , The acquired current and phase voltage are then converted to current in the dq rotating coordinate system using Clarke transform and Park transform. , and voltage , Construct motor voltage balance equations based on the dq axis, taking the q axis as an example. Inductors , and magnetic chain Given the motor parameters and electrical angular velocity The rate of change of current is obtained in real time by the motor encoder. By measuring the current Perform backward difference approximation calculation, i.e. , To control the period, the equation is rearranged as follows: The linear form of the expression, where the parameter to be estimated is the resistance. Observed values Observation matrix The recursive least squares (RLS) algorithm with a forgetting factor is applied online to solve the problem and calculate the gain vector. Calculate the prediction error Update the resistance estimate Update the covariance matrix ,in The forgetting factor is set to a value slightly less than 1, for example, 0.999. Simultaneously, a feedforward compensation model is established. Select the main harmonic components that have the greatest impact on torque fluctuation, for example, n=1,2,4,6, and construct the state vector of the Kalman filter using the sine and cosine coefficients. .

[0027] In one possible embodiment, the real-time monitoring of motor phase current and phase voltage, and the online estimation of motor winding resistance by constructing a motor voltage balance equation and using recursive least squares method, includes: A motor voltage balance equation is constructed based on phase voltage, phase current, phase current change rate, motor electric angular velocity, winding resistance, winding inductance, and back electromotive force coefficient. The real-time acquired phase voltage, phase current, and motor electric angular velocity are substituted into the equation, the winding resistance is used as the parameter to be estimated, and the phase current is used as the regression vector. The equation is then iteratively calculated using a recursive least squares method with a forgetting factor, where the forgetting factor is set to a value between 0.99 and 0.999.

[0028] Voltage equations for permanent magnet synchronous motors in the dq rotating coordinate system: This equation can be reconstructed into a linear regression form. The parameters to be estimated Let R be the resistance, and the regression vector be... q-axis current Observation for In each servo control cycle, for example 100μs, phase voltage commands are obtained from the driver in real time. , Phase current feedback , and the motor electrical angular velocity obtained by the encoder derivative Phase current change rate Calculated by backward difference approximation, i.e. ,in To control the cycle. Motor inductance. , and back electromotive force coefficient These are pre-calibrated, known parameters. An application with a forgetting factor is used. The recursive least squares (RLS) method is used to iteratively solve the problem: calculating the prediction error. Update gain Update the resistance estimate Update the covariance matrix Forgetting factor The preferred value is 0.998, which strikes a balance between tracking the slow change in resistance and suppressing measurement noise.

[0029] S3. After filtering out low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, the current value corresponding to the compensation torque output by the feedforward compensation model in the previous cycle is subtracted to obtain the residual current command. The residual current command is then multiplied by the motor torque constant to convert it into a residual torque error, which is used as the observation value of the Kalman filter. Based on the change in the winding resistance estimate, the process noise covariance matrix is ​​updated, and the update rate of the state vector is adjusted. According to the real-time acquired motor electrical angle and the updated state vector, the feedforward compensation torque is calculated in real time through the feedforward compensation model and converted into a current superimposed on the driver command. In each control cycle k, the q-axis current command output by the servo driver PID controller is obtained. Pass this instruction through a first-order digital high-pass filter; the difference equation is: ,in The filter coefficients, related to the cutoff frequency, are used to filter out low-frequency command components corresponding to the macroscopic motion of the joint. The compensation torque calculated by the feedforward compensation model is obtained by subtracting the previous cycle (k-1 cycles) from the filtered current command. The converted current value, i.e. ,in Given the motor torque constant, the residual current command is obtained. Multiply the residual current command by the motor torque constant. The residual torque error is obtained. This value is used as the observation value of the Kalman filter. The state equation of the Kalman filter is then established. With observation equation The state transition matrix F is set as the identity matrix I, indicating that the harmonic coefficients themselves are slowly varying. Observation matrix. Based on the current electrical angle of the motor Build, Calculate the change in the current resistance estimate. And adjust the process noise covariance matrix. ,in The basic noise diagonal matrix, The proportionality coefficient is positive, and I is the identity matrix. When the resistance changes rapidly (i.e., temperature changes are significant), the Q matrix is ​​increased to improve the Kalman filter's gain and state update rate. The standard five-step prediction and update algorithm of the Kalman filter is executed: time prediction. Covariance prediction Calculate Kalman gain ,in To observe the noise variance, state update Covariance update Use the updated state vector Harmonic coefficients in and and the current real-time electrical angle Through the feedforward compensation model The feedforward compensation torque for the current cycle is calculated in real time, and this torque is converted into current and superimposed on the driver's instructions.

[0030] In one possible embodiment, after filtering out low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, subtracting the current value corresponding to the compensation torque output by the feedforward compensation model of the previous cycle to obtain the residual current command, and multiplying the residual current command by the motor torque constant to convert it into a residual torque error, which is then used as the observation value of the Kalman filter, includes: The sine and cosine coefficients of the first, second, fourth, and sixth harmonics of the initial torque wave model are constructed into an eight-dimensional state vector. After filtering out low-frequency drive components by a high-pass filter, the current command value output by the servo driver is subtracted from the compensation current value obtained by dividing the feedforward compensation torque calculated in the previous cycle by the motor torque constant to obtain the residual current command. The residual current command is then multiplied by the motor torque constant to obtain the residual torque error, which is used as the single-dimensional observation value of the Kalman filter.

[0031] Define the state vector of the Kalman filter. for There are a total of 8 elements. In each control cycle, the total q-axis current command output by the servo driver is... By using a first-order high-pass filter with a low cutoff frequency, such as 2Hz, the low-frequency command components used to generate macroscopic motion are filtered out, resulting in the high-frequency current components. Calculate the compensation torque output by the feedforward compensation model in the previous period k-1. The corresponding compensation current ,in The motor torque constant is used. The residual current command is calculated as follows: The residual current command represents the fluctuation portion that the current model fails to fully compensate for, which is made up by the controller's closed-loop feedback. The residual current command is multiplied by the torque constant. The residual torque error is obtained. The scalar value As observations of the Kalman filter. The filter's observation matrix. Then based on the current electric angle of the motor for .

[0032] In one possible embodiment, updating the process noise covariance matrix based on the change in the estimated winding resistance and adjusting the update rate of the state vector includes: In each control cycle, the absolute value of the difference between the estimated winding resistance at the current moment and the estimated value at the previous moment is calculated to obtain the resistance change. The resistance change is multiplied by a preset proportional coefficient, and the resulting product is added to a preset base noise value. The diagonal element value corresponding to the state vector in the process noise covariance matrix is ​​updated with the sum of the sums.

[0033] The change in torque ripple harmonic coefficient is related to the changes in motor temperature and winding resistance R. In each control cycle k, the current resistance value is obtained from the RLS estimator. Resistance value of the previous cycle Calculate the change in resistance. The process noise covariance matrix of a Kalman filter Typically a diagonal matrix, with diagonal elements This represents the uncertainty of each component of the state vector. The method adjusts... Matrix, diagonal elements Update according to the following rules: .in, It is a preset low base noise value, such as 1e-7, to ensure that the filter maintains a certain degree of self-adjustment even when the temperature is stable. It is a proportionality coefficient, for example, 0.05, used for adjustment. Sensitivity to changes in resistance. When the motor temperature changes rapidly, Increase, make Increasing the diagonal elements of the matrix increases the Kalman gain, making the filter more confident in new observations, thus accelerating the tracking and adjustment of harmonic coefficients; conversely, when the temperature is stable, As the value decreases, the filter places more faith in the model's predictions, resulting in a smoother estimation. Optionally, the proportionality coefficient can be measured in torque. 2 / ohm.

[0034] In one possible embodiment, the step of calculating the feedforward compensation torque in real time using a feedforward compensation model based on the real-time acquired motor electrical angle and the updated state vector includes: Based on the real-time acquired electric angle of the motor and the updated harmonic sine and cosine coefficients of the Kalman filter, the feedforward compensation torque is calculated by summing the Fourier series of selected harmonic orders, including the first, second, fourth, and sixth harmonic orders.

[0035] Within each servo control cycle, the Kalman filter, after prediction and update steps, outputs a state vector that is the best estimate at the current moment. That is, the updated harmonic coefficient Simultaneously, the current electrical angle is obtained in real time from the motor encoder. The feedforward compensation torque is synthesized using Fourier series expansion. Calculated This will be converted into a feedforward current command. This current is then added to the total current command of the servo controller, thereby proactively canceling out the fluctuations before they occur.

[0036] S4 filters the current command value output by the joint servo driver in real time through one or more digital band-stop filters; and sends the filtered current command to the motor driver.

[0037] Offline identification methods, such as applying swept-frequency signals or white noise excitation to the robot joints using a signal analyzer, are employed to measure the vibration response of the robot joints. By calculating the frequency response function (FRF), the first-order and higher-order natural resonant frequencies of the robot joint links are determined. , , For each resonant frequency that needs to be suppressed... We can design a second-order IIR digital bandstop filter, also known as a notch filter. The filter's transfer function can be expressed as: The normalized center frequency , The controller sampling frequency is given by parameter r, which controls the bandwidth of the notch filter. The closer r is to 1, the narrower the bandwidth; typically, a value between 0.95 and 0.99 is chosen. G is the gain to ensure the signal amplitude remains constant at non-stopband frequencies. This transfer function is then converted into a differential equation form for implementation in the digital controller: Where x(k) is the input current command value, y(k) is the filtered output value, and the filter coefficients are... , , , , Determined by the transfer function parameters. In the real-time control loop, the current command generated by the servo drive PID controller... Before being sent to the current loop, the current command is first calculated using one or more such difference equations. If multiple resonance peaks exist, multiple band-stop filters are connected in series to filter the current command one by one, thereby attenuating the energy at the resonance frequency and suppressing mechanical vibration.

[0038] In one possible embodiment, the real-time filtering of the current command value output by the joint servo driver through one or more digital band-stop filters includes: Plan one or more second-order infinite impulse response bandstop filters, such that the center frequency of the stopband of the filter corresponds to the first and second order natural resonant frequencies of the linkage structure of the surgical robot, respectively. In each control cycle, the current command value output by the joint servo driver is input into the filter for forward calculation.

[0039] The first two natural resonant frequencies of a specific joint linkage structure of a surgical robot in a typical pose are determined through experimental modal analysis or finite element simulation. For example, the first resonant frequency is measured. =95Hz, second-order resonant frequency =210Hz. For the above two frequencies, plan two second-order IIR digital notch filters respectively. The filter transfer function is: Among them, the normalized center frequency , The sampling frequency of the servo control system is used; the parameter r controls the bandwidth of the notch filter, taking a value close to 1, for example, r=0.985, to achieve narrow bandwidth suppression and reduce the impact on the phase of other frequency bands. Two sets of resonant frequencies are known. , and controller sampling frequency By substituting parameters such as bandwidth coefficient r and gain G into the transfer function and then discretizing it, the filter coefficients corresponding to the difference equation can be derived. , , , , In the real-time control loop, the total q-axis current command, which includes motion commands, friction compensation, and torque fluctuation feedforward compensation, will be used. As input, it passes sequentially through the two cascaded IIR filters mentioned above. The difference equation is: Where x(k) is the input and y(k) is the output. The output of the first filter serves as the input of the second filter, sending the filtered current command to the motor driver, thereby suppressing energy that might induce mechanical resonance at the command source.

[0040] During the offline phase, each joint of the surgical robot was initialized and calibrated. The joint was controlled to rotate at a low constant speed, for example, 5 degrees / second, for at least one revolution, while the motor electrical angle and joint output torque were synchronously acquired at a frequency of 5 kHz. The acquired data was resampled at equal angular intervals, for example, dividing a 360-degree electrical cycle into 1024 points. The resampled torque sequence was analyzed using Fast Fourier Transform (FFT) to extract the sine and cosine coefficients of the main 1st, 2nd, 4th, and 6th harmonic components, which were then used as the initial parameters for the torque fluctuation model.

[0041] After entering the online compensation stage, a motor voltage balance equation based on the dq axis is constructed, and a forgetting factor is used, for example... A recursive least squares (RLS) method with a value of 0.995 is used to estimate the motor winding resistance in real time online. Information such as phase voltage, phase current, and motor speed available from the driver is used to track resistance changes caused by motor temperature rise. A Kalman filter with eight harmonic coefficients as the state vector is constructed for online optimization of the torque compensation model. The observations of this filter are driven by a residual torque error signal, which is obtained by high-pass filtering the total current command (e.g., at a cutoff frequency of 2Hz to remove macroscopic motion commands) and subtracting the compensation current component predicted by the model in the previous cycle. This design separates the torque fluctuation component not compensated by the model as the error signal driving the model.

[0042] The process noise covariance Q of the Kalman filter is correlated with the resistance change estimated by RLS. By setting a base noise value, such as 1e-7, and a scaling factor, such as 0.05, the Q value increases accordingly when the motor temperature changes rapidly, thereby improving the convergence speed of the filter and its ability to track changes in model parameters; when the temperature is stable, the Q value decreases, making the parameter estimation smoother. Based on the eight harmonic coefficients updated in real time by the Kalman filter and the current motor electrical angle, the feedforward compensation torque is calculated by summing Fourier series. In addition, before sending the current command to the driver, the command passes through one or more series-connected second-order IIR digital notch filters. The center frequency of the filter is set at the pre-calibrated first-order (e.g., 95Hz) and second-order (e.g., 210Hz) mechanical resonant frequencies of the robot link. Through narrowband suppression, the vibration of the robotic arm caused by the control signal is prevented, ensuring the smoothness and accuracy of the surgical operation. The full-cycle control architecture of this invention is as follows: Figure 3 As shown.

[0043] To verify the reliability of the various technologies in the scheme, an experiment was conducted on the third joint of a seven-degree-of-freedom surgical robot.

[0044] The experimental conditions involved controlling the joint to operate continuously at a constant speed of 10 degrees Celsius per second for 30 minutes under load to simulate the motor temperature rise during prolonged surgery. Four schemes were compared: Scheme A, baseline control without any compensation; Scheme B, using only offline calibrated static feedforward compensation; Scheme C, using feedforward compensation but with a fixed process noise covariance Q and no notch filter; and Scheme D, the complete compensation scheme proposed in this paper. Experimental data were acquired using a high-precision torque sensor and encoder. Key performance indicators included the residual peak-to-peak value and root mean square value of the joint output torque, and the end-effector vibration acceleration at the 95Hz resonant frequency.

[0045] When the motor is cold for 1 minute, the peak-to-peak torque fluctuation of Scheme A (based on the baseline control) is 0.25 N·m. After static compensation, Scheme B reduces it to 0.08 N·m, Scheme C reduces it to 0.07 N·m after compensation, and Scheme D, with full compensation, reaches 0.06 N·m. When the motor is hot for 30 minutes, the torque fluctuation of Scheme A deteriorates to 0.28 N·m, Scheme B increases to 0.15 N·m due to model mismatch, Scheme C partially tracks the changes but the fluctuation still increases to 0.09 N·m, while Scheme D, benefiting from the noise covariance related to the resistance change rate, has a peak-to-peak torque fluctuation of only 0.065 N·m, almost unaffected by temperature rise. The root mean square value of torque fluctuation also shows the same trend, with values ​​of 90, 50, 30, and 21 mN·m for Schemes A, B, C, and D respectively under hot conditions.

[0046] Experimental results show that static feedforward compensation can suppress approximately 68% of torque fluctuations in the initial state, but its performance degrades by more than 80% as the motor temperature increases. Compared to static compensation scheme B, the conventional compensation scheme C, which does not consider temperature effects, can further reduce fluctuations by 40% under hot conditions. The proposed scheme D, by establishing a correlation between process noise and the estimated rate of change of resistance, further reduces residual fluctuations by approximately 30% under hot conditions compared to scheme C, achieving compensation for temperature changes. Furthermore, by comparing the end-vibration of schemes C and D, the notch filter in scheme D reduces the vibration acceleration at 95Hz from 1.2 m / s². 2 It was suppressed to 0.1 m / s 2 The following improvements enhance the system's mechanical stability. In summary, the complete solution proposed in this paper achieves comprehensive suppression of torque fluctuations and mechanical resonance through multi-component collaboration, and reduces the sensitivity of compensation performance to operating temperature.

[0047] In a second embodiment, the present invention also provides a joint control system for a surgical robot, comprising the following modules: The offline initialization module is used to control the joints of the surgical robot to run on a preset trajectory, synchronously collect the electrical angle of the motor and the output torque data of the joint, resample the data at equal angle intervals, and extract the initialization parameters through fast Fourier transform; The online compensation module is used to monitor the phase current and phase voltage of the motor in real time. By constructing the motor voltage balance equation and using the recursive least squares method, the motor winding resistance value is estimated online. A feedforward compensation model based on the initial torque fluctuation model is constructed, and the sine and cosine coefficients of the harmonic components in the model are used as state vectors to establish a Kalman filter. The generation module is used to filter out low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, subtract the current value corresponding to the compensation torque output by the feedforward compensation model in the previous cycle to obtain the residual current command, and multiply the residual current command by the motor torque constant to convert it into residual torque error, which is used as the observation value of the Kalman filter; based on the change in the winding resistance estimate, the process noise covariance matrix is ​​updated and the update rate of the state vector is adjusted; according to the real-time acquired motor electrical angle and the updated state vector, the feedforward compensation torque is calculated in real time through the feedforward compensation model and converted into a current superimposed on the driver command; The output module is used to filter the current command value output by the joint servo driver in real time through one or more digital band-stop filters; and send the filtered current command to the motor driver.

[0048] It should be understood that in the foregoing description of the embodiments in this specification, various features are combined in a single embodiment, drawing, or description for the purpose of simplifying the description and to aid in understanding a feature. However, this does not mean that the combination of these features is necessary, and those skilled in the art, upon reading this specification, may readily identify some of the devices as separate embodiments. That is, the embodiments in this specification can also be understood as an integration of multiple secondary embodiments. And the content of each secondary embodiment is valid even if it contains fewer than all the features of a single foregoing disclosed embodiment.

Claims

1. A joint control method for a surgical robot, characterized in that, Includes the following steps: During the offline initialization phase, the joints of the surgical robot are controlled to run along a preset trajectory. Simultaneously, the electrical angle of the motor and the output torque data of the joint are collected. The data are resampled at equal angular intervals, and the initialization parameters are extracted through fast Fourier transform. During the online compensation phase, the phase current and phase voltage of the motor are monitored in real time. The motor voltage balance equation is constructed and the recursive least squares method is used to estimate the motor winding resistance online. A feedforward compensation model based on the initial torque fluctuation model is constructed, and the sine and cosine coefficients of the harmonic components in the model are used as state vectors to establish a Kalman filter. After filtering out low-frequency drive components by a high-pass filter, the current command value output by the joint servo driver is subtracted from the current value corresponding to the compensation torque output by the feedforward compensation model of the previous cycle to obtain the residual current command. The residual current command is then multiplied by the motor torque constant to convert it into residual torque error, which is used as the observation value of the Kalman filter. Based on the change in the estimated winding resistance, the process noise covariance matrix is ​​updated, and the update rate of the state vector is adjusted. According to the real-time acquired motor electrical angle and the updated state vector, the feedforward compensation torque is calculated in real time through the feedforward compensation model and converted into a current superimposed on the driver command. The current command value output by the joint servo driver is filtered in real time through one or more digital band-stop filters. The filtered current command is sent to the motor driver.

2. The method according to claim 1, characterized in that, The surgical robot joints are controlled to run along a preset trajectory. Simultaneously, electrical angle data of the motors and output torque data of the joints are collected. This data is resampled at equal angular intervals, and initialization parameters are extracted using a fast Fourier transform, including: The surgical robot joints are controlled to run at a constant speed for at least one complete mechanical rotation. The electrical angles of the motors and the output torque of the joints are collected synchronously. The original data is resampled at fixed electrical angle intervals. The resampled joint output torque data is subjected to a fast Fourier transform to extract the sine and cosine coefficients of the predetermined harmonic orders, namely the first, second, fourth, and sixth harmonics, as initialization parameters.

3. The method according to claim 1, characterized in that, The real-time monitoring of motor phase current and phase voltage, by constructing a motor voltage balance equation and using the recursive least squares method, estimates the motor winding resistance online, including: A motor voltage balance equation is constructed based on phase voltage, phase current, phase current change rate, motor electric angular velocity, winding resistance, winding inductance, and back electromotive force coefficient. The real-time acquired phase voltage, phase current, and motor electric angular velocity are substituted into the equation, the winding resistance is used as the parameter to be estimated, and the phase current is used as the regression vector. The equation is then iteratively calculated using a recursive least squares method with a forgetting factor, where the forgetting factor is set to a value between 0.99 and 0.

999.

4. The method according to claim 1, characterized in that, The step of updating the process noise covariance matrix and adjusting the update rate of the state vector based on the change in the estimated winding resistance includes: In each control cycle, the absolute value of the difference between the estimated winding resistance at the current moment and the estimated value at the previous moment is calculated to obtain the resistance change. The resistance change is multiplied by a preset proportional coefficient, and the resulting product is added to a preset base noise value. The diagonal element value corresponding to the state vector in the process noise covariance matrix is ​​updated with the sum of the sums.

5. The method according to claim 1 or 3, characterized in that, The step of calculating the feedforward compensation torque in real time based on the real-time acquired motor electrical angle and the updated state vector using a feedforward compensation model includes: Based on the real-time acquired electric angle of the motor and the updated harmonic sine and cosine coefficients of the Kalman filter, the feedforward compensation torque is calculated by summing the Fourier series of selected harmonic orders, including the first, second, fourth, and sixth harmonic orders.

6. The method according to claim 1, characterized in that, The real-time filtering of the current command value output by the joint servo driver through one or more digital band-stop filters includes: Plan one or more second-order infinite impulse response bandstop filters, such that the center frequency of the stopband of the filter corresponds to the first and second order natural resonant frequencies of the linkage structure of the surgical robot, respectively. In each control cycle, the current command value output by the joint servo driver is input into the filter for forward calculation.

7. A joint control system for a surgical robot, characterized in that, Includes the following modules: The offline initialization module is used to control the joints of the surgical robot to run on a preset trajectory, synchronously collect the electrical angle of the motor and the output torque data of the joint, resample the data at equal angle intervals, and extract the initialization parameters through fast Fourier transform; The online compensation module is used to monitor the phase current and phase voltage of the motor in real time. By constructing the motor voltage balance equation and using the recursive least squares method, the motor winding resistance value is estimated online. A feedforward compensation model based on the initial torque fluctuation model is constructed, and the sine and cosine coefficients of the harmonic components in the model are used as state vectors to establish a Kalman filter. The generation module is used to filter out the low-frequency drive components from the current command value output by the joint servo driver through a high-pass filter, subtract the current value corresponding to the compensation torque output by the feedforward compensation model of the previous cycle, obtain the residual current command, and multiply the residual current command by the motor torque constant to convert it into residual torque error, which is used as the observation value of the Kalman filter. Based on the change in the estimated winding resistance, the process noise covariance matrix is ​​updated, and the update rate of the state vector is adjusted. According to the real-time acquired motor electrical angle and the updated state vector, the feedforward compensation torque is calculated in real time through the feedforward compensation model and converted into a current superimposed on the driver command. The output module is used to filter the current command value output by the joint servo driver in real time through one or more digital band-stop filters. The filtered current command is sent to the motor driver.

8. The system according to claim 7, characterized in that, The surgical robot joints are controlled to run along a preset trajectory. Simultaneously, electrical angle data of the motors and output torque data of the joints are collected. This data is resampled at equal angular intervals, and initialization parameters are extracted using a fast Fourier transform, including: The surgical robot joints are controlled to run at a constant speed for at least one complete mechanical rotation. The electrical angles of the motors and the output torque of the joints are collected synchronously. The original data is resampled at fixed electrical angle intervals. The resampled joint output torque data is subjected to a fast Fourier transform to extract the sine and cosine coefficients of the predetermined harmonic orders, namely the first, second, fourth, and sixth harmonics, as initialization parameters.

9. The system according to claim 7, characterized in that, The step of calculating the feedforward compensation torque in real time based on the real-time acquired motor electrical angle and the updated state vector using a feedforward compensation model includes: Based on the real-time acquired electric angle of the motor and the updated harmonic sine and cosine coefficients of the Kalman filter, the feedforward compensation torque is calculated by summing the Fourier series of selected harmonic orders, including the first, second, fourth, and sixth harmonic orders.

10. A computer-readable storage medium storing a computer program thereon, characterized in that, The computer program, when executed by a processor, implements the method as described in any one of claims 1-6.

Images