Robotic joint motion compensation method, system, and storage medium

Abstract

The application relates to a robot joint motion compensation method, system and storage medium. The method comprises the following steps: in the case that the amplitude of a disturbance signal is greater than a safety threshold, predicting a disturbance compensation amount of each joint, and performing negative feedback adjustment on the corresponding joint based on the corresponding disturbance compensation amount of each joint; in the case that the amplitude of a vibration signal is greater than a safety threshold, predicting a vibration compensation amount of each joint, and performing negative feedback adjustment on each joint of the robot based on the corresponding vibration compensation amount of each joint. The method can compensate the mechanical arm based on the disturbance compensation amount and the vibration compensation amount respectively, realize the effect of real-time compensation of coupling force disturbance and vibration in the scene without increasing damping elements or through signal filtering, has high compensation precision, and is suitable for coupling force disturbance compensation and vibration compensation in different scenes.

Description

Technical Field

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

[0002] Robot end effectors are prone to coupling force interference and vibration during operation, resulting in low positioning accuracy for motion control. Currently, there are two main methods for suppressing coupling force interference and vibration in robots: the first method involves installing external components such as damping elements on the robot body to absorb coupling force interference and vibration energy; the second method involves capturing coupling force interference signals and vibration signals, extracting the coupling force interference frequency and vibration frequency, and then filtering them to suppress vibration.

[0003] The first method suppresses vibration by installing additional damping elements, which increases the complexity and cost of the robot's structure. At the same time, the stiffness of the damping elements reduces the stiffness of the robot's structure, and the vibration suppression effect depends on the lifespan and characteristics of the damping elements. For example, temperature affects the damping elements' ability to absorb vibration energy.

[0004] In the second method, when the coupling force interference frequency or vibration frequency is different, the method of filtering by capturing the vibration frequency cannot suppress different vibration characteristics, and the extraction and analysis of vibration signals will produce a time delay, which cannot respond immediately to ensure the vibration suppression effect. Summary of the Invention

[0005] Therefore, it is necessary to provide a robot joint motion compensation method, system, and storage medium that can achieve real-time suppression of interference and vibration without relying on additional detection elements, and ensure that the suppression effect of interference and vibration is not affected by interference, in order to address the above-mentioned technical problems.

[0006] Firstly, this application provides a method for compensating for robot joint motion. The method includes:

[0007] Real-time detection of whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during movement;

[0008] When the amplitude of the detected interference signal exceeds the safety threshold, the interference compensation amount of each joint is predicted, and the corresponding joint is adjusted based on the interference compensation amount of each joint. The interference compensation amount is the amount of torque change required to compensate for the impact of coupling force interference on the movement of each joint of the robot.

[0009] When the amplitude of the detected vibration signal exceeds the safety threshold, the vibration compensation amount of each joint is predicted, and the robot's joints are adjusted by negative feedback based on the vibration compensation amount corresponding to each joint. The vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0010] After the negative feedback adjustment of each joint of the robot is completed, the negative feedback adjustment of each joint is stopped when the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold.

[0011] In one embodiment, predicting the disturbance compensation amount for each joint includes:

[0012] Obtain servo commands; servo commands correspond to the input torque of each joint torque motor, and are used to control each joint torque motor to operate to the corresponding input torque;

[0013] The input torque of each joint torque motor corresponding to the servo command is input into the joint dynamics model to obtain the ideal joint torque of each joint of the robot in an ideal environment without interference.

[0014] The servo commands control the rotation of the torque motors of each joint, and the actual joint torques of the joint torque motors are collected. Based on the ideal joint torques and the actual joint torques of each joint, the interference values ​​corresponding to each joint are determined.

[0015] Based on the interference value corresponding to each joint, the interference compensation amount corresponding to each joint is determined.

[0016] In one embodiment, predicting the vibration compensation amount for each joint includes:

[0017] The system acquires servo commands and parses the input torque of each joint torque motor corresponding to the servo commands through the motion controller. Each servo command corresponds to the input torque of each joint torque motor, and the servo commands are used to control each joint torque motor to operate to the corresponding input torque.

[0018] The input torque of each joint torque motor corresponding to the servo command is input into the equivalent rigid body dynamics model in the joint dynamics model to obtain the ideal rigid body motion signal of each joint of the robot in an ideal environment without interference; the ideal rigid body motion signal is the position information and / or velocity information of the joint under vibration-free conditions.

[0019] The servo commands control the rotation of the torque motors of each joint, and the actual motion signals of the joint torque motors are collected. Based on the ideal rigid body motion signals of each joint and the actual motion signals of each joint, the pure vibration value corresponding to each joint is determined. The actual motion signals are the position information and / or velocity information of the joints under vibration conditions.

[0020] Based on the pure vibration value corresponding to each joint, the vibration compensation amount corresponding to each joint is determined.

[0021] In one embodiment, based on the ideal rigid body motion signal and the actual motion signal of each joint, the pure vibration value corresponding to each joint is determined, including:

[0022] The ideal rigid body motion signal and the actual motion signal of each joint are input into the closed-loop controller. The closed-loop controller predicts the pure vibration value of each joint. The pure vibration value of each joint is fed back to the equivalent rigid body dynamics model to correct the ideal rigid body motion signal of each joint output by the equivalent rigid body dynamics model.

[0023] In one embodiment, the vibration compensation amount for each joint is determined based on the pure vibration value corresponding to each joint, including:

[0024] The pure vibration value corresponding to each joint is input into the frequency selector, and the vibration compensation amount corresponding to each joint within a specific frequency range is obtained through the frequency selector; the vibration compensation amount corresponding to each joint is fed back to the input of the motion controller to correct the input torque of each joint torque.

[0025] In one embodiment, negative feedback adjustment is performed on each joint of the robot based on the vibration compensation amount corresponding to each joint, including:

[0026] Based on the vibration compensation amount corresponding to each joint and the actual motion signal of each joint torque motor, the input torque of each joint torque motor is corrected, and the operation of each joint torque motor is controlled based on the modified input torque of each joint torque motor; wherein, the input torque of each joint torque motor is used as the input of the equivalent rigid body dynamics model.

[0027] Secondly, this application also provides a robot joint motion compensation system, characterized in that the system includes: a main control unit, a servo unit, a detection module, an interference suppression module, a vibration suppression module, and a judgment module;

[0028] The main control unit outputs servo commands to the servo unit;

[0029] The servo unit controls the movement of each joint of the robot according to servo commands;

[0030] The detection module monitors in real time whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during the movement process.

[0031] When the amplitude of the interference signal detected is greater than the safety threshold, the interference suppression module predicts the interference compensation amount for each joint and performs negative feedback adjustment on the corresponding joint based on the interference compensation amount for each joint; the interference compensation amount is the amount of torque change required to compensate for the impact of coupling force interference on the movement of each joint of the robot.

[0032] When the vibration suppression module detects that the amplitude of the vibration signal is greater than the safety threshold, it predicts the vibration compensation amount of each joint and performs negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint. The vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0033] After the robot's joints have completed negative feedback adjustment, the judgment module stops adjusting the robot's joints when the amplitude of the interference signal and the amplitude of the vibration signal at each joint are both less than the safety threshold.

[0034] Thirdly, this application also provides a computer device. The computer device includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to perform the following steps:

[0035] Real-time detection of whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during movement;

[0036] When the amplitude of the detected interference signal exceeds the safety threshold, the interference compensation amount of each joint is predicted, and the corresponding joint is adjusted based on the interference compensation amount of each joint. The interference compensation amount is the amount of torque change required to compensate for the impact of coupling force interference on the movement of each joint of the robot.

[0037] When the amplitude of the detected vibration signal exceeds the safety threshold, the vibration compensation amount of each joint is predicted, and the robot's joints are adjusted by negative feedback based on the vibration compensation amount corresponding to each joint. The vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0038] After the negative feedback adjustment of each joint of the robot is completed, the negative feedback adjustment of each joint is stopped when the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold.

[0039] Fourthly, this application also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program thereon, which, when executed by a processor, performs the following steps:

[0040] Real-time detection of whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during movement;

[0041] When the amplitude of the detected interference signal exceeds the safety threshold, the interference compensation amount of each joint is predicted, and the corresponding joint is adjusted based on the interference compensation amount of each joint. The interference compensation amount is the amount of torque change required to compensate for the impact of coupling force interference on the movement of each joint of the robot.

[0042] When the amplitude of the detected vibration signal exceeds the safety threshold, the vibration compensation amount of each joint is predicted, and the robot's joints are adjusted by negative feedback based on the vibration compensation amount corresponding to each joint. The vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0043] After the negative feedback adjustment of each joint of the robot is completed, the negative feedback adjustment of each joint is stopped when the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold.

[0044] Fifthly, this application also provides a computer program product. The computer program product includes a computer program that, when executed by a processor, performs the following steps:

[0045] Real-time detection of whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during movement;

[0046] When the amplitude of the detected interference signal exceeds the safety threshold, the interference compensation amount of each joint is predicted, and the corresponding joint is adjusted based on the interference compensation amount of each joint. The interference compensation amount is the amount of torque change required to compensate for the impact of coupling force interference on the movement of each joint of the robot.

[0047] When the amplitude of the detected vibration signal exceeds the safety threshold, the vibration compensation amount of each joint is predicted, and the robot's joints are adjusted by negative feedback based on the vibration compensation amount corresponding to each joint. The vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0048] After the negative feedback adjustment of each joint of the robot is completed, the negative feedback adjustment of each joint is stopped when the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold.

[0049] The aforementioned robot joint motion compensation method, system, and storage medium detect the real joint torque and real motion signals of each joint in real time using existing detection elements on the robotic arm. Based on simulation or modeling, the ideal joint torque and ideal rigid body motion signals of each joint are obtained. The interference compensation amount is determined by the real joint torque and the ideal joint torque, and the vibration compensation amount is determined by the real motion signal and the ideal rigid body motion signal. The robotic arm is then compensated for interference and vibration based on the interference compensation amount and the vibration compensation amount, respectively. This achieves the effect of real-time compensation for coupling force interference and vibration without adding damping elements or signal filtering. The compensation accuracy is high, and it is applicable to coupling force interference compensation and vibration compensation in different scenarios. Attached Figure Description

[0050] Figure 1 This is a diagram illustrating the application environment of a robot joint motion compensation method in one embodiment.

[0051] Figure 2 This is a structural distribution diagram of the robot system in a robot joint motion compensation method in one embodiment;

[0052] Figure 3 This is a schematic diagram of the main control vehicle in one embodiment;

[0053] Figure 4 This is a schematic diagram of a patient trolley in one embodiment;

[0054] Figure 5 This is a schematic diagram of the tool arm in one embodiment;

[0055] Figure 6 This is a flowchart illustrating a robot joint motion compensation method in one embodiment;

[0056] Figure 7 Here is a flowchart of interference suppression and vibration suppression in one embodiment;

[0057] Figure 8 This is a flowchart illustrating the process of predicting the interference compensation amount for each joint in one embodiment.

[0058] Figure 9 This is a schematic diagram of the interference suppression algorithm in one embodiment;

[0059] Figure 10 This is a flowchart illustrating the process of predicting the vibration compensation amount for each joint in one embodiment.

[0060] Figure 11 This is a schematic diagram of a vibration suppression algorithm in one embodiment;

[0061] Figure 12 This is a schematic diagram illustrating the implementation of the vibration suppression algorithm in one embodiment;

[0062] Figure 13 This is a schematic diagram of the structure of a robot joint motion compensation system in one embodiment;

[0063] Figure 14 This is a flowchart illustrating the suppression of interference and vibration during robot startup in one embodiment;

[0064] Figure 15 This is a flowchart of an algorithm for suppressing interference and vibration during robot startup in one embodiment;

[0065] Figure 16 This is a flowchart illustrating the process of suppressing interference and vibration during the normal operation of a robot in one embodiment, where the robot is suddenly subjected to interference or vibration and needs to be suppressed.

[0066] Figure 17 This is a flowchart of an algorithm for suppressing interference and vibration during normal robot operation in one embodiment, where interference / vibration is suddenly detected and needs to be suppressed.

[0067] Figure 18 This is an internal structural diagram of a computer device in one embodiment. Detailed Implementation

[0068] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0069] The robot joint motion compensation method provided in this application embodiment can be applied to, for example... Figure 1 The robot system shown is as follows: The main control carriage 1 is equipped with a main control arm 2, a controller 3, and a display 4; the patient lies on a patient carriage 5, which is equipped with a tool arm 6, an adjusting arm 8, and a suspension 9; the end of the tool arm 6 is equipped with an instrument 7, and the position of the tool arm 6 is controlled by the adjusting arm 8 and the suspension 9; the image carriage 10 is equipped with a display 11 and an image host 12. The structural distribution of the robot system is as follows. Figure 2 As shown; a schematic diagram of the main control vehicle 1 is shown below. Figure 3 As shown; a schematic diagram of patient trolley 5 is shown below. Figure 4 As shown; a schematic diagram of tool arm 6 is shown below. Figure 5 As shown.

[0070] There is a master-slave teleoperation mapping relationship between the master control arm 2 and the tool arm 6. By manipulating the master control arm 2, the tool arm 6 is controlled to move. The tool arm 6 is used to perform pre-planned surgical operations on the patient. During the surgical operation, the image host 12 processes the operating environment and displays the operation images, and displays the processed operation images on the monitor 11.

[0071] During robot operation, servo commands are received through controller 3, and based on these commands, the torque motors providing driving force to each joint of tool arm 6 are controlled to initiate servo motion. During the movement of tool arm 6, real-time detection is performed to check for interference and vibration at each joint. When the amplitude of the interference signal exceeds a safety threshold during the movement of any joint of tool arm 6, controller 3 predicts the interference compensation amount for each joint and performs negative feedback adjustment on each joint based on the corresponding interference compensation amount. The interference compensation amount is used to compensate for the impact of coupling force interference on the movement of each joint of the robot. The amount of torque change to be adjusted; when the amplitude of the vibration signal of each joint of the tool arm 6 is greater than the safety threshold during the movement, the controller 3 predicts the vibration compensation amount of each joint and performs negative feedback adjustment on each joint of the tool arm 6 based on the vibration compensation amount corresponding to each joint; the vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint; if after the negative feedback adjustment of each joint of the tool arm 6, the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold, then the controller 3 stops performing negative feedback adjustment on each joint of the tool arm 6.

[0072] In one embodiment, such as Figure 6 As shown, a robot joint motion compensation method is provided, which can be applied to... Figure 1 Taking the controller on the main control vehicle as an example, the following steps are included:

[0073] Step 602: Real-time detection of whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during the movement process.

[0074] Interference signals refer to the signals caused by coupling forces between series joints. If the entire drive path of the n-axis is not only between the n-1 and n-axis, but spans across n-1 or even nm, coupling will occur. For example, if the six-axis motor is not fixed to the five-axis structure, but is mounted on the three-axis structure and then transmitted to the six-axis via a transmission mechanism, then even when the six-axis motor is stationary, the movement of the four-axis and five-axis motors will cause movement along the six-axis. Vibration is interference caused by the vibration of the joint motors or the end effector of the robotic arm.

[0075] This embodiment can detect interference signals and vibration signals of each joint during the movement process using sensors. For example, the position or speed information of each joint torque motor can be collected by the encoder at the joint motor end or the encoder at the joint load end. The interference signal is reflected in the waveform corresponding to the position or speed information by the effect of a sudden increase in amplitude followed by a gradual decrease, while the vibration signal is reflected in the waveform corresponding to the position or speed information by the effect of maintaining a constant amplitude.

[0076] It should be noted that the encoder at the joint motor end or the encoder at the joint load end is an existing structure on the tool arm.

[0077] Optionally, the controller collects the position or speed information of each joint torque motor through the joint motor end encoder or the joint load end encoder, and determines whether the amplitude of the interference signal and vibration signal in the signal waveform diagram collected by the encoder exceeds the safety threshold.

[0078] Step 604: If the amplitude of the detected interference signal is greater than the safety threshold, predict the interference compensation amount of each joint, and perform negative feedback adjustment on each joint of the robot based on the interference compensation amount corresponding to each joint; the interference compensation amount is the amount of torque change required to compensate for the effect of coupling force interference on the movement of each joint of the robot.

[0079] The interference compensation amount can be determined based on the difference between the ideal torque and the actual torque of each joint, thus specifying the required torque change for the motor. Negative feedback adjustment refers to correcting the input torque of the motor for each joint based on the interference compensation amount until the amplitude of the interference signal at each joint is less than a safe threshold. The process of performing negative feedback adjustment on each joint of the robot based on the corresponding interference compensation amount is also a process of compensating for the joint torque of each joint.

[0080] Optionally, when interference occurs during the movement of the robot's joints, the controller determines the ideal torque of each joint through simulation or data deduction, and collects the real torque of each joint through an encoder. Based on the ideal joint torque and the real joint torque of each joint, the controller determines the interference compensation amount for each shutdown, and corrects the input torque of the torque motor of each joint based on the interference compensation amount, until the amplitude of the interference signal of each joint is less than the safety threshold.

[0081] Step 606: If the amplitude of the detected vibration signal is greater than the safety threshold, predict the vibration compensation amount of each joint, and perform negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint; the vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0082] The vibration compensation amount can be determined based on the difference between the ideal rigid body motion signal and the actual motion signal of each joint, thus determining the amount of motion change that the motor needs to adjust; the rigid body motion signal can be the position or velocity information of the joint. The process of performing negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint is also a process of compensating for the joint motion signal of each joint.

[0083] Optionally, when the amplitude of the detected vibration signal exceeds the safety threshold, the controller determines the ideal rigid body motion signal of each joint through simulation or data deduction, and collects the actual motion signal of each joint through an encoder. Based on the ideal rigid body motion signal and the actual motion signal of each joint, the controller determines the vibration compensation amount for each shutdown, and corrects the input torque of the torque motor of each joint based on the vibration compensation amount, until the actual motion signal of the torque motor is adjusted to be equal to the ideal rigid body motion signal.

[0084] Step 608: If, after negative feedback adjustment of each joint of the robot, the interference and vibration of each joint are less than the safety threshold, then stop the negative feedback adjustment of each joint of the robot.

[0085] Among them, such as Figure 7 As shown, when the robot starts, the controller sends servo commands to the robot. The robot's servo unit receives the servo commands and initiates servo motion. When both the interference signal and vibration signal exceed the safety threshold, the interference suppression module and vibration suppression module perform algorithmic compensation on the robot's current motion signal to suppress the interference and vibration amplitude. The compensated interference and vibration amplitude are then judged against the safety threshold. When the compensated interference and vibration exceed the set safety threshold, the joints continue to undergo closed-loop algorithm compensation. When the compensated interference and vibration are less than the set safety threshold, the compensated servo commands are output to the servo unit of the surgical robot to complete the compensated motion. The safety thresholds for interference and vibration are set according to the specific motion control accuracy target.

[0086] It should be noted that the ideal joint torque and ideal rigid body motion signal of each joint are generally obtained through the joint dynamics model. Due to the modeling error of the flexible model in the joint dynamics model, when the coupling force interference and vibration are fused together for interference prediction and compensation, it is impossible to accurately measure the ideal rigid body motion signal, which further leads to the problem of low vibration prediction and compensation accuracy. In order to solve this problem, this embodiment sets up a scheme for parallel prediction and compensation of interference and vibration, and only acquires the ideal rigid body motion signal of each shutdown, so as to avoid the problem of low vibration compensation accuracy caused by the error of the flexible model, and thus improve the compensation accuracy.

[0087] Optionally, after the robot's joints are adjusted using negative feedback, if the interference of each joint exceeds the safety threshold, the process returns to step 604 to compensate for the joint torque of each joint again until the interference of each joint is less than the safety threshold, at which point the negative feedback adjustment of the robot's joints is stopped. If the vibration of each joint exceeds the safety threshold, the process returns to step 606 to compensate for the joint motion signal of each joint again until the vibration of each joint is less than the safety threshold, at which point the negative feedback adjustment of the robot's joints is stopped.

[0088] In the above-mentioned robot joint motion compensation method, the actual joint torque and actual motion signal of each joint are detected in real time by existing detection elements on the robotic arm, and the ideal joint torque and ideal rigid body motion signal of each joint are obtained by simulation or modeling. The interference compensation amount is determined by the actual joint torque and the ideal joint torque, and the vibration compensation amount is determined by the actual motion signal and the ideal rigid body motion signal. The robotic arm is subjected to interference and vibration compensation based on the interference compensation amount and the vibration compensation amount, respectively. This achieves the effect of real-time compensation for coupling force interference and vibration without adding damping elements or signal filtering. The compensation accuracy is high and it is applicable to coupling force interference compensation and vibration compensation in different scenarios.

[0089] In one embodiment, such as Figure 8 As shown, the interference compensation amount for each joint is predicted, including:

[0090] Step 802: Obtain servo commands; the servo commands correspond to the input torque of each joint torque motor, and the servo commands are used to control each joint torque motor to operate to the corresponding input torque.

[0091] The servo commands are generated by the controller and transmitted to the torque motors of each joint in the tool arm, controlling each joint torque motor to operate to the specified input torque.

[0092] Step 804: Input the input torque of each joint torque motor corresponding to the servo command into the joint dynamics model to obtain the ideal joint torque of each joint of the robot in an ideal environment without interference.

[0093] Among them, the joint dynamics model is a model that simulates joint motion based on robot dynamics equations and studies the relationship between the force and motion of an object.

[0094] The joint dynamics model describes the relationship between the motion of the robotic arm and the input force or torque of each joint torque motor. Using the joint dynamics model, the ideal joint torque of each joint can be determined under undisturbed conditions without damaging the joint torque motors.

[0095] Step 806: Control the rotation of each joint torque motor according to the servo command, and collect the actual joint torque of the joint torque motor. Based on the ideal joint torque and the actual joint torque of each joint, determine the corresponding interference value of each joint.

[0096] The actual joint torque of the joint torque motor is obtained by detecting the joint torque sensor. The absolute value of the difference between the ideal joint torque and the actual joint torque of each joint is used as the interference value for each joint.

[0097] Step 808: Determine the interference compensation amount for each joint based on the interference value corresponding to each joint.

[0098] The interference compensation amount is the torque change required to adjust the corresponding joint. This compensation amount is data that can be recognized by the controller, while the interference value is used to evaluate interference components, including interference other than coupling force interference. To compensate for the impact of coupling force interference on the tool arm movement at each joint, the coupling interference force portion needs to be extracted from the interference value, and the interference compensation amount for each joint's coupling interference force needs to be determined based on this force. Specifically, the coupling interference force portion of the interference value can be extracted using a low-pass filter.

[0099] Optionally, since the calculation principle for the interference compensation of each joint is the same, this example uses one joint of the tool arm. Specifically, for example... Figure 9 As shown, the controller acquires the servo command Td, parses the servo command Td to obtain the input torque Tm of the joint torque motor, and inputs the input torque Tm of the joint torque motor into the joint dynamics model to obtain the ideal joint torque Tnj without interference calculated by the joint dynamics model, i.e., Tnj = Tm * Joint_model, where Joint_model represents the joint dynamics model; the joint torque motor of the tool arm operates to the input torque Tm. During the joint torque operation, the interference it experiences is Tdis. Under the influence of the interference Tdis, the torque of the joint torque motor is not equal to the input torque. The torque is determined by detecting the actual joint torque Tj using a joint torque sensor; based on the difference between the actual joint torque Tj and the ideal joint torque Tnj, the disturbance value Tndis experienced by the joint is predicted, i.e., Tndis = Tj – Tnj; the disturbance value Tndis is input into a low-pass filter to extract the coupling force interference component, and the corresponding disturbance compensation amount Tfb is determined based on the coupling force interference component, i.e., Tfb = Filter(Tndis), where Filter means filtering; the input torque of the joint torque motor is corrected based on the disturbance compensation amount Tfb corresponding to the joint and the servo command, thereby forming a closed-loop control.

[0100] In this embodiment, the input torque of the joint torque motor is corrected by servo commands and interference compensation, and the input torque is input into the joint dynamics model. The ideal joint torque without interference is calculated by the joint dynamics model. Based on the difference between the ideal joint torque and the real joint torque, the interference value of the joint is determined. The coupling force interference component in the interference value is extracted by a low-pass filter, and the interference compensation amount corresponding to the joint is determined based on the coupling force interference component. The process returns to the step of correcting the input torque of the joint torque motor by servo commands and interference compensation and continues to execute until the influence of the coupling force interference of the joint on the movement of the tool arm is compensated, that is, the end of the tool arm is executed to the desired position. This can achieve the effect of real-time compensation for coupling force interference without adding damping elements or signal filtering, and the compensation accuracy is high.

[0101] In one embodiment, such as Figure 10 As shown, the vibration compensation for each joint is predicted, including:

[0102] Step 1002: Obtain servo commands and parse the input torque of each joint torque motor corresponding to the servo commands through the motion controller; the servo commands correspond to the input torque of each joint torque motor, and the servo commands are used to control each joint torque motor to operate to the corresponding input torque.

[0103] The motion controller can be a PID controller. It adjusts the signal sent to the joint torque motor based on the joint motion signal, ensuring that the joint error remains within a specified accuracy range. In this embodiment, the motion controller calculates the input torque of the joint torque motor based on servo commands, joint motion signals, and vibration compensation.

[0104] Step 1004: Input the input torque of each joint torque motor corresponding to the servo command into the equivalent rigid body dynamics model in the joint dynamics model to obtain the ideal rigid body motion signal of each joint of the robot in an ideal environment without interference; the ideal rigid body motion signal is the position information and / or velocity information of the joint under vibration-free conditions.

[0105] The equivalent rigid body dynamics model describes the translation and rotation of each joint of the robotic arm under the action of force and torque. The ideal rigid body motion signal of each joint can be determined in an ideal environment without interference through the equivalent rigid body dynamics model.

[0106] Step 1006: Control the rotation of the torque motors of each joint according to the servo command, and collect the real motion signals of the joint torque motors. Based on the ideal rigid body motion signals of each joint and the real motion signals of each joint, determine the pure vibration value corresponding to each joint; the real motion signals are the position information and / or velocity information of the joint under vibration.

[0107] The actual motion signal is obtained by measuring the encoder at the joint torque motor end, or by measuring the encoder at the joint load end. The absolute value of the difference between the ideal rigid body motion signal and the actual motion signal of each joint is taken as the pure vibration value of each joint.

[0108] Step 1008: Determine the vibration compensation amount for each joint based on the pure vibration value corresponding to each joint.

[0109] The vibration compensation amount is the amount of motion change required to adjust each joint of the robot. The pure vibration value includes pure vibration interference components corresponding to different vibration frequencies, and the vibration compensation amount is the amount of motion change required to compensate for the influence of pure vibration interference components within a specific frequency range on the movement of the tool arm.

[0110] Optionally, since the calculation principle for vibration compensation is the same for all joints, this example uses one joint of the tool arm. Specifically, for example... Figure 11 As shown, the controller acquires the servo command Vd, parses the servo command Vd to obtain the input torque Tm of the joint torque motor, and inputs the input torque Tm of the joint torque motor into the equivalent rigid body dynamics model in the joint dynamics model for vibration-free calculation processing to obtain the ideal rigid body motion signal Rv without interference calculated by the equivalent rigid body dynamics model, i.e., Rv = A(Tm), where A represents the equivalent rigid body dynamics model, A = 1 / ((JM+JL)*s), where JM represents the motor end inertia of the joint, JL represents the load end inertia of the joint, and s is the Laplace domain operator; the joint torque motor of the tool arm operates to the input torque Tm. During the joint torque operation, under the influence of interference, the end of the tool arm does not reach the expected position. The system detects the actual motion signal V of the joint using a joint torque sensor. This signal can be either the measurement signal Mv from the encoder at the joint motor end or the measurement signal Lv from the encoder at the joint load end. The difference between the actual motion signal V and the ideal rigid body motion signal Rv is processed using pure vibration estimation to predict the pure vibration value Vib experienced by the joint, i.e., Vib = B(V – Rv), where B represents the pure vibration estimation. Vib is then subjected to vibration suppression processing, which involves extracting a pure vibration signal at a specific frequency and determining the corresponding vibration compensation amount Vfb for the joint, i.e., Vfb = C(Vib), where C represents the vibration suppression process. Based on the vibration compensation amount Vfb and servo commands, the input torque of the joint torque motor is corrected, thus forming a closed-loop control.

[0111] In this embodiment, the input torque of the joint torque motor is corrected by servo commands and vibration compensation, and the input torque is input into the equivalent rigid body dynamics model in the joint dynamics model. The equivalent rigid body dynamics model calculates an ideal rigid body motion signal without interference. Based on the difference between the ideal rigid body motion signal and the real motion signal, the pure vibration value of the joint is determined, and the vibration component in the pure vibration value is extracted. Based on the vibration component, the vibration compensation amount corresponding to the joint is determined, and the process returns to the step of correcting the input torque of the joint torque motor by servo commands and vibration compensation, and continues to execute until the impact of the joint vibration on the tool arm movement is compensated, that is, the end of the tool arm is executed to the desired position. This can achieve the effect of real-time compensation for vibration interference without adding damping elements or signal filtering, with high compensation accuracy.

[0112] In some embodiments, after obtaining the pure vibration values, the vibration compensation amount corresponding to the joint can be directly determined based on the pure vibration values. However, since the equivalent rigid body dynamics model is obtained through modeling, there are still errors compared to the environment without vibration interference. To make the ideal rigid body motion signals of each joint output by the equivalent rigid body dynamics model closer to the ideal data in an interference-free ideal environment, this embodiment feeds back the pure vibration values ​​corresponding to each joint to the equivalent rigid body dynamics model, corrects the ideal rigid body motion signals of each joint output by the equivalent rigid body dynamics model, obtains more accurate ideal rigid body motion signals, and further improves the accuracy of pure vibration value prediction. Specifically, as shown... Figure 12 As shown, based on the ideal rigid body motion signals and the actual motion signals of each joint, the pure vibration values ​​corresponding to each joint are determined, including the following steps:

[0113] The ideal rigid body motion signal and the actual motion signal of each joint are input into the closed-loop controller. The closed-loop controller predicts the pure vibration value of each joint. The pure vibration value of each joint is fed back to the equivalent rigid body dynamics model to correct the ideal rigid body motion signal of each joint output by the equivalent rigid body dynamics model.

[0114] One approach is to directly feed back the absolute value of the difference between the ideal rigid body motion signal and the actual motion signal of each joint into the equivalent rigid body dynamics model to correct the ideal rigid body motion signal. However, this method results in slow and inaccurate estimation of pure vibration signals. Therefore, to address these issues, this embodiment employs a closed-loop controller to process the absolute value of the difference between the ideal rigid body motion signal and the actual motion signal of each joint, enabling rapid and accurate prediction of pure vibration values. The closed-loop controller can be any combination of PID controllers, specifically a P-controller (proportional controller), PI-controller (integral controller), or PID-controller (derivative controller). Based on the processing strategy corresponding to the combination of PID controllers, the absolute value of the difference between the ideal rigid body motion signal and the actual motion signal of each joint is proportionally amplified, integrated, or differentiated.

[0115] In some embodiments, a frequency selector is used to perform vibration suppression processing on the pure vibration value Vib, that is, to extract the pure vibration signal at a specific frequency. Specifically, step 1008 includes the following steps:

[0116] The pure vibration value corresponding to each joint is input into the frequency selector, and the vibration compensation amount corresponding to each joint within a specific frequency range is obtained through the frequency selector; the vibration compensation amount corresponding to each joint is fed back to the input of the motion controller to correct the input torque of each joint torque.

[0117] The frequency selector is used to extract pure vibration signals of different frequencies from the pure vibration value and to adjust the amplification or reduction of the pure vibration signal to ensure the vibration compensation amount, so as to achieve the suppression of vibrations of multiple frequencies. For vibration interference of different frequencies, there is no need to re-estimate the vibration value; simply change the extraction range of the frequency selector. It is suitable for different application scenarios.

[0118] A frequency selector can be implemented using one of the following devices: a high-pass filter, a low-pass filter, or a band-pass filter. If a high-pass filter is selected, pure vibration signals within a frequency range above a preset value can be extracted; if a low-pass filter is selected, pure vibration signals within a frequency range below a preset value can be extracted; and if a band-pass filter is selected, pure vibration signals within a specific frequency range can be extracted.

[0119] In some embodiments, such as Figure 12 As shown, after obtaining the vibration compensation amount corresponding to each joint, the steps for negative feedback adjustment of each joint of the robot based on the vibration compensation amount corresponding to each joint include the following steps:

[0120] Based on the vibration compensation amount corresponding to each joint and the actual motion signal of each joint torque motor, the input torque of each joint torque motor is corrected, and the operation of each joint torque motor is controlled based on the modified input torque of each joint torque motor; wherein, the modified input torque of each joint torque motor is used as the input of the equivalent rigid body dynamics model.

[0121] Taking the actual motion signal as the position signal as an example, the vibration compensation amount is the position compensation amount, and the expected position corresponding to the corrected input torque is equal to the sum of the input position corresponding to the input torque before correction and the vibration compensation amount.

[0122] Optionally, such as Figure 12 As shown, the controller acquires the servo command Vd, parses the servo command Vd to obtain the input torque Tm of the joint torque motor, and inputs the input torque Tm of the joint torque motor into the equivalent rigid body dynamics model in the joint dynamics model for vibration-free calculation processing to obtain the ideal rigid body motion signal Rv without interference calculated by the equivalent rigid body dynamics model; the joint torque motor of the tool arm rotates to the input torque Tm. During the joint torque operation, under the influence of interference, the end of the tool arm does not reach the expected position. The actual motion signal V of the joint is detected by the joint torque sensor; the difference between the actual motion signal V and the ideal rigid body motion signal Rv is split into two paths, one of which is fed back to the closed-loop controller. The closed-loop controller is fast and precise. The pure vibration value is accurately predicted and fed back to the equivalent rigid body dynamics model. The ideal rigid body motion signal Rv of each joint output by the equivalent rigid body dynamics model is corrected. Based on the corrected ideal rigid body motion signal Rv and the real motion signal V, the pure vibration value Vib is re-determined and input to the frequency selector. In another input frequency selector, the frequency selector extracts the vibration compensation amount corresponding to each joint in a specific frequency range according to the time sequence. Based on the vibration compensation amount corresponding to each joint and the real motion signal of each joint torque motor, the input torque of each joint torque motor is corrected, and the operation of each joint torque motor is controlled based on the modified input torque of each joint torque motor, thereby forming a closed loop control.

[0123] In this embodiment, a closed-loop controller is used to process the absolute value of the difference between the ideal rigid body motion signal and the actual motion signal of each joint. This allows for rapid and accurate prediction of pure vibration values, which are then fed back to the equivalent rigid body dynamics model. This corrects the ideal rigid body motion signal output by the equivalent rigid body dynamics model, resulting in a more accurate ideal rigid body motion signal and further improving the accuracy of pure vibration value prediction. Furthermore, this embodiment uses a frequency selector to extract pure vibration signals of specific frequencies from the pure vibration values. These signals are then amplified or reduced in an adjustable manner to ensure vibration compensation, thereby suppressing vibrations at multiple frequencies. For vibration interference at different frequencies, there is no need to re-estimate the vibration value; simply changing the extraction range of the frequency selector is sufficient, making it suitable for various application scenarios.

[0124] In one embodiment, this embodiment provides detailed steps of a robot joint motion compensation method, specifically including the following steps:

[0125] Step 1: Real-time detection of whether the interference and vibration of each joint of the robot exceed the safety threshold during movement. If the interference of each joint of the robot exceeds the safety threshold during movement, proceed to Step 2; if the vibration of each joint of the robot exceeds the safety threshold during movement, proceed to Step 7.

[0126] Step 2, obtain servo commands; the servo commands correspond to the input torque of each joint torque motor, and the servo commands are used to control each joint torque motor to operate to the corresponding input torque;

[0127] Step 3: Input the input torque of each joint torque motor corresponding to the servo command into the joint dynamics model to obtain the ideal joint torque of each joint of the robot in an ideal environment without interference.

[0128] Step 4: Control the rotation of each joint torque motor according to the servo command, and collect the actual joint torque of the joint torque motor. Based on the ideal joint torque and the actual joint torque of each joint, determine the corresponding interference value of each joint.

[0129] Step 5: Determine the interference compensation amount for each joint based on the interference value corresponding to each joint.

[0130] Step 6: Adjust the robot joints using negative feedback based on the interference compensation amount corresponding to each joint.

[0131] Step 7: Obtain the servo command and parse the input torque of each joint torque motor corresponding to the servo command through the motion controller; the servo command corresponds to the input torque of each joint torque motor, and the servo command is used to control each joint torque motor to operate to the corresponding input torque;

[0132] Step 8: Input the input torque of each joint torque motor corresponding to the servo command into the equivalent rigid body dynamics model in the joint dynamics model to obtain the ideal rigid body motion signal of each joint of the robot in an ideal environment without interference; the ideal rigid body motion signal is the position information and / or velocity information of the joint under vibration-free conditions.

[0133] Step 9: Control the rotation of the torque motors of each joint according to the servo command, and collect the actual motion signals of the joint torque motors. Input the ideal rigid body motion signal and the actual motion signal of each joint into the closed-loop controller respectively. The closed-loop controller predicts the pure vibration value corresponding to each joint. The actual motion signal is the position information and / or velocity information of the joint under vibration. The pure vibration value corresponding to each joint is fed back to the equivalent rigid body dynamics model to correct the ideal rigid body motion signal of each joint output by the equivalent rigid body dynamics model.

[0134] Step 10: Input the pure vibration value corresponding to each joint into the frequency selector, and obtain the vibration compensation amount corresponding to each joint within a specific frequency range through the frequency selector; wherein, the vibration compensation amount corresponding to each joint is fed back to the input of the motion controller to correct the input torque of each joint torque.

[0135] Step 11: Based on the vibration compensation amount corresponding to each joint and the actual motion signal of each joint torque motor, correct the input torque of each joint torque motor, and control the operation of each joint torque motor based on the modified input torque of each joint torque motor; wherein, the input torque of each joint torque motor is used as the input of the equivalent rigid body dynamics model.

[0136] In this embodiment, interference and vibration compensation are performed on the robotic arm based on interference compensation and vibration compensation, respectively. This achieves the effect of real-time compensation for coupling force interference and vibration without adding damping elements or signal filtering. The compensation accuracy is high and it is applicable to coupling force interference compensation and vibration compensation in different scenarios.

[0137] It should be understood that although the steps in the flowcharts of the embodiments described above are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the embodiments described above may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the steps or stages of other steps.

[0138] Based on the same inventive concept, this application also provides a robot joint motion compensation system for implementing the robot joint motion compensation method described above. The solution provided by this device is similar to the implementation described in the above method; therefore, the specific limitations of the one or more robot joint motion compensation system embodiments provided below can be found in the limitations of the robot joint motion compensation method described above, and will not be repeated here.

[0139] In one embodiment, such as Figure 13 As shown, a robot joint motion compensation system is provided, including: a main control unit, a servo unit, a detection module, an interference suppression module, a vibration suppression module, and a judgment module, wherein:

[0140] The main control unit outputs servo commands to the servo unit;

[0141] The servo unit controls the movement of each joint of the robot according to servo commands;

[0142] The detection module monitors in real time whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during the movement process.

[0143] When the amplitude of the interference signal detected is greater than the safety threshold, the interference suppression module predicts the interference compensation amount for each joint and performs negative feedback adjustment on the corresponding joint based on the interference compensation amount for each joint; the interference compensation amount is the amount of torque change required to compensate for the impact of coupling force interference on the movement of each joint of the robot.

[0144] When the vibration suppression module detects that the amplitude of the vibration signal is greater than the safety threshold, it predicts the vibration compensation amount of each joint and performs negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint. The vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint.

[0145] After the robot's joints have completed negative feedback adjustment, the judgment module stops adjusting the robot's joints when the amplitude of the interference signal and the amplitude of the vibration signal at each joint are both less than the safety threshold.

[0146] In one embodiment, such as Figure 13 As shown, the interference suppression module includes: an interference-free calculation unit, an interference estimation unit, and an interference compensation unit;

[0147] The interference-free computing unit calculates the ideal joint torque of each joint of the robot in an ideal environment without interference through the joint dynamics model;

[0148] The interference estimation unit determines the true joint torque of each joint of the robot in a real environment, and determines the interference value corresponding to each joint based on the ideal joint torque and the true joint torque of each joint.

[0149] The interference compensation unit determines the interference compensation amount for each joint based on the interference value corresponding to each joint, and performs negative feedback adjustment on each joint of the robot based on the interference compensation amount corresponding to each joint.

[0150] In one embodiment, such as Figure 13 As shown, the vibration suppression module includes: a vibration-free calculation unit, a pure vibration estimation unit, and a vibration suppression unit;

[0151] The vibration-free computing unit calculates the ideal rigid body motion signals of each joint of the robot in an ideal environment without interference through the equivalent rigid body dynamics model in the joint dynamics model; the ideal rigid body motion signals are the position information and / or velocity information of the joints under vibration-free conditions.

[0152] The pure vibration estimation unit determines the real motion signals of each joint of the robot in a real environment. Based on the ideal rigid body motion signals of each joint and the real motion signals of each joint, it determines the pure vibration value corresponding to each joint. The real motion signals are the position information and / or velocity information of the joints under vibration.

[0153] The vibration suppression unit determines the vibration compensation amount for each joint based on the pure vibration value corresponding to each joint, and performs negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint.

[0154] In one embodiment, the pure vibration estimation unit inputs the ideal rigid body motion signal and the actual motion signal of each joint into the closed-loop controller, and predicts the pure vibration value corresponding to each joint through the closed-loop controller; wherein, the pure vibration value corresponding to each joint is fed back to the equivalent rigid body dynamics model to correct the ideal rigid body motion signal of each joint output by the equivalent rigid body dynamics model.

[0155] In one embodiment, the vibration suppression unit inputs the pure vibration value corresponding to each joint to the frequency selector, and obtains the vibration compensation amount corresponding to each joint within a specific frequency range through the frequency selector; wherein, the vibration compensation amount corresponding to each joint is fed back to the input of the motion controller to correct the input torque of each joint torque.

[0156] In one embodiment, the vibration suppression unit corrects the input torque of each joint torque motor based on the vibration compensation amount corresponding to each joint and the actual motion signal of each joint torque motor, and controls the operation of each joint torque motor based on the modified input torque of each joint torque motor; wherein, the input torque of each joint torque motor is used as the input of the equivalent rigid body dynamics model.

[0157] In one embodiment, such as Figure 13 As shown, the detection module includes a first detection module and a second detection module; the judgment module includes a first judgment module and a second judgment module.

[0158] The first detection module is used to detect whether the amplitude of the interference signal of each joint of the robot is greater than the safety threshold during the movement; the second detection module is used to detect whether the amplitude of the vibration signal of each joint of the robot is greater than the safety threshold during the movement; the first judgment module is used to judge whether the interference received by each joint of the robot is less than the set safety threshold; the second judgment module is used to judge whether the vibration received by each joint of the robot is less than the set safety threshold.

[0159] In some embodiments, the flowchart for suppressing interference and vibration during robot startup is as follows: Figure 14 As shown, when the robot starts, the controller sends servo commands to the robot; the robot's servo unit receives the servo commands and initiates servo motion; the first detection module and the second detection module respectively determine whether there is interference and vibration when the surgical robot starts; when the interference and vibration detected by the first detection module and the second detection module are both greater than the safety threshold, the interference suppression module and the vibration suppression module perform algorithm compensation on the robot's current motion signal to suppress the interference and vibration amplitude; the judgment module performs a safety threshold judgment on the system's compensated interference and vibration amplitude; when the compensated interference and vibration are less than the set safety threshold, the compensated servo command is output to the robot's servo unit to complete the compensated motion.

[0160] In some embodiments, the flowchart of the algorithm for suppressing interference and vibration during robot startup is as follows: Figure 15 As shown, the controller (CPU) is used to parse motion commands and process data measured by the robot's own components (e.g., encoder measurement signals). The servo unit is used to implement servo motion of the controlled system. The detection module is used to detect whether there is interference or vibration in the currently controlled system.

[0161] The interference suppression module includes an interference-free calculation unit, an interference estimation unit, and an interference compensation unit. The interference-free calculation unit is used to solve for the ideal system without interference; the interference estimation unit is used to solve for the interference components present in the actual system; and the interference compensation unit is used to extract the interference components in the actual system and use them as compensation quantities to negatively feed back to the control input of the system to eliminate the interference received by the system.

[0162] The vibration suppression module includes a vibration-free calculation unit, a pure vibration estimation unit, and a vibration suppression unit. The vibration-free calculation unit is used to solve for the motion information of an ideal rigid body system that does not contain vibration; the pure vibration estimation unit is used to solve for the vibration components present in the actual system; the vibration suppression unit is used to extract the pure vibration components in the actual system and use them as compensation negative feedback to the control input of the system to suppress the vibration phenomenon of the system.

[0163] The judgment module is used to determine whether the disturbance and vibration experienced by the system are less than the set safety threshold, so as to ensure that the disturbance and vibration components in the system control output are within the safety threshold.

[0164] In some embodiments, when a robot is suddenly disturbed / vibrated during normal operation and needs to suppress the disturbance / vibration, the flowchart for disturbance and vibration suppression is as follows: Figure 16 As shown, during normal operation, the robot suddenly experiences interference or vibration. The first and second detection modules determine whether the interference and vibration generated during the robot's movement are within safe thresholds. When both the interference and vibration detected by the first and second detection modules exceed the safe thresholds, the interference suppression module and vibration suppression module perform algorithmic compensation on the current motion signal of the surgical robot to suppress the interference and vibration amplitude. The judgment module then performs a safety threshold judgment on the compensated interference and vibration amplitude. When the compensated interference and vibration are less than the set safe threshold, the compensated servo command is output to the servo unit of the surgical robot to complete the compensated movement.

[0165] In some embodiments, when a robot is suddenly disturbed or vibrates during normal operation and needs to suppress this disturbance / vibration, the flowchart of the disturbance and vibration suppression algorithm is as follows: Figure 17 As shown, the detection module is used to detect whether there is interference and vibration in the currently controlled system. The interference suppression module includes an interference-free calculation unit, an interference estimation unit, and an interference compensation unit. The interference-free calculation unit is used to solve for the ideal system without interference; the interference estimation unit is used to solve for the interference components present in the actual system; the interference compensation unit is used to extract the interference components in the actual system and use them as compensation quantities to negatively feed back to the control input of the system to eliminate the interference received by the system.

[0166] The vibration suppression module includes a vibration-free calculation unit, a pure vibration estimation unit, and a vibration suppression unit. The vibration-free calculation unit is used to solve for the motion information of an ideal rigid body system that does not contain vibration; the pure vibration estimation unit is used to solve for the vibration components present in the actual system; the vibration suppression unit is used to extract the pure vibration components in the actual system and use them as compensation negative feedback to the control input of the system to suppress the vibration phenomenon of the system.

[0167] The judgment module is used to determine whether the disturbance and vibration experienced by the system are less than the set safety threshold, so as to ensure that the disturbance and vibration components in the system control output are within the safety threshold.

[0168] A servo unit is used to realize the servo motion of the controlled system.

[0169] The modules in the aforementioned robot joint motion compensation system can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in the processor of a computer device in hardware form or independent of it, or stored in the memory of a computer device in software form, so that the processor can call and execute the corresponding operations of each module.

[0170] In one embodiment, a computer device is provided, which may be a controller, and its internal structure diagram may be as follows: Figure 18 As shown, the computer device includes a processor, memory, input / output interface, communication interface, display unit, and input device. The processor, memory, and input / output interface are connected via a system bus, and the communication interface, display unit, and input device are also connected to the system bus via the input / output interface. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The input / output interface is used for exchanging information between the processor and external devices. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, mobile cellular networks, NFC (Near Field Communication), or other technologies. When executed by the processor, the computer program implements a robot joint motion compensation method. The display unit is used to form a visually visible image and can be a display screen, projection device, or virtual reality imaging device. The display screen can be an LCD screen or an e-ink screen. The input device of the computer device can be a touch layer covering the display screen, or buttons, trackballs, or touchpads set on the casing of the computer device, or external keyboards, touchpads, or mice, etc.

[0171] Those skilled in the art will understand that Figure 18 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0172] In one embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.

[0173] In one embodiment, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps in the above method embodiments.

[0174] In one embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above method embodiments.

[0175] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data shall comply with the relevant laws, regulations and standards of the relevant countries and regions.

[0176] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM). The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0177] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0178] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this patent application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. A method for compensating joint motion in a robot, characterized in that, The method includes: Real-time detection of whether the amplitude of interference signals and vibration signals of each joint of the robot exceed the safety threshold during movement; If the amplitude of the detected interference signal is greater than the safety threshold, the interference compensation amount of each joint is predicted, and the corresponding joint is negatively adjusted based on the interference compensation amount of each joint; the interference compensation amount is the amount of torque change required to compensate for the effect of coupling force interference on the movement of each joint of the robot. If the amplitude of the detected vibration signal is greater than the safety threshold, the vibration compensation amount of each joint is predicted, and the robot's joints are adjusted by negative feedback based on the vibration compensation amount corresponding to each joint; the vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint. After the negative feedback adjustment of each joint of the robot is completed, the negative feedback adjustment of each joint of the robot is stopped when the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold.

2. The method according to claim 1, characterized in that, The predicted interference compensation amount for each joint includes: Obtain servo commands; the servo commands correspond to the input torque of each joint torque motor, and the servo commands are used to control each joint torque motor to operate to the corresponding input torque; The input torque of each joint torque motor corresponding to the servo command is input into the joint dynamics model to obtain the ideal joint torque of each joint of the robot in an ideal environment without interference. The servo commands control the rotation of the torque motors of each joint, and the actual joint torques of the joint torque motors are collected. Based on the ideal joint torques and the actual joint torques of each joint, the interference values ​​corresponding to each joint are determined. Based on the interference value corresponding to each joint, the interference compensation amount corresponding to each joint is determined.

3. The method according to claim 1, characterized in that, The predicted vibration compensation for each joint includes: The system acquires servo commands and parses the input torque of each joint torque motor corresponding to the servo commands through the motion controller; the servo commands correspond to the input torque of each joint torque motor, and the servo commands are used to control each joint torque motor to operate to the corresponding input torque. The input torque of each joint torque motor corresponding to the servo command is input into the equivalent rigid body dynamics model in the joint dynamics model to obtain the ideal rigid body motion signal of each joint of the robot in an ideal environment without interference; the ideal rigid body motion signal is the position information and / or velocity information of the joint under vibration-free conditions. The servo commands control the rotation of the torque motors of each joint, and the actual motion signals of the joint torque motors are collected. Based on the ideal rigid body motion signals of each joint and the actual motion signals of each joint, the pure vibration value corresponding to each joint is determined. The actual motion signals are the position information and / or velocity information of the joint under vibration. Based on the pure vibration value corresponding to each joint, the vibration compensation amount corresponding to each joint is determined.

4. The method according to claim 3, characterized in that, The determination of the pure vibration value corresponding to each joint based on the ideal rigid body motion signal and the actual motion signal of each joint includes: The ideal rigid body motion signal and the actual motion signal of each joint are respectively input into the closed-loop controller, and the pure vibration value corresponding to each joint is predicted by the closed-loop controller; wherein, the pure vibration value corresponding to each joint is fed back to the equivalent rigid body dynamics model to correct the ideal rigid body motion signal of each joint output by the equivalent rigid body dynamics model.

5. The method according to claim 3, characterized in that, The determination of the vibration compensation amount for each joint based on the pure vibration value of each joint includes: The pure vibration value corresponding to each joint is input into the frequency selector, and the vibration compensation amount corresponding to each joint within a specific frequency range is obtained through the frequency selector; wherein, the vibration compensation amount corresponding to each joint is fed back to the input of the motion controller to correct the input torque of each joint torque.

6. The method according to claim 3, characterized in that, The negative feedback adjustment of each joint of the robot based on the vibration compensation amount corresponding to each joint includes: Based on the vibration compensation amount corresponding to each joint and the actual motion signal of each joint torque motor, the input torque of each joint torque motor is corrected, and the operation of each joint torque motor is controlled based on the modified input torque of each joint torque motor; wherein, the input torque of each joint torque motor is used as the input of the equivalent rigid body dynamics model.

7. A robot joint motion compensation system, characterized in that, The system includes: a main control unit, a servo unit, a detection module, an interference suppression module, a vibration suppression module, and a judgment module; The main control unit outputs servo commands to the servo unit; The servo unit controls the movement of each joint of the robot according to the servo command; The detection module detects in real time whether the amplitude of the interference signal and the amplitude of the vibration signal of each joint of the robot exceed the safety threshold during the movement process. When the amplitude of the interference signal detected is greater than the safety threshold, the interference suppression module predicts the interference compensation amount for each joint and performs negative feedback adjustment on the corresponding joint based on the interference compensation amount for each joint; the interference compensation amount is the amount of torque change required to compensate for the effect of coupling force interference on the movement of each joint of the robot. When the vibration suppression module detects that the amplitude of the vibration signal is greater than the safety threshold, it predicts the vibration compensation amount of each joint and performs negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint; the vibration compensation amount is the amount of motion change required to compensate for the effect of pure vibration interference in a specific frequency range on the movement of each joint. After the robot's joints have completed negative feedback adjustment, the judgment module stops performing negative feedback adjustment on the robot's joints when the amplitude of the interference signal and the amplitude of the vibration signal of each joint are both less than the safety threshold.

8. The system according to claim 7, characterized in that, The interference suppression module includes: an interference-free calculation unit, an interference estimation unit, and an interference compensation unit; The interference-free computing unit calculates the ideal joint torque of each joint of the robot in an ideal environment without interference through the joint dynamics model; The interference estimation unit determines the actual joint torque of each joint of the robot in a real environment, and determines the interference value corresponding to each joint based on the ideal joint torque and the actual joint torque of each joint. The interference compensation unit determines the interference compensation amount for each joint based on the interference value corresponding to each joint, and performs negative feedback adjustment on each joint of the robot based on the interference compensation amount corresponding to each joint.

9. The system according to claim 7, characterized in that, The vibration suppression module includes: a vibration-free calculation unit, a pure vibration estimation unit, and a vibration suppression unit; The vibration-free computing unit calculates the ideal rigid body motion signals of each joint of the robot in an ideal environment without interference through the equivalent rigid body dynamics model in the joint dynamics model; the ideal rigid body motion signals are the position information and / or velocity information of the joints under vibration-free conditions. The pure vibration estimation unit determines the real motion signals of each joint of the robot in a real environment, and determines the pure vibration value corresponding to each joint based on the ideal rigid body motion signals of each joint and the real motion signals of each joint; the real motion signals are the position information and / or velocity information of the joints under vibration conditions. The vibration suppression unit determines the vibration compensation amount for each joint based on the pure vibration value corresponding to each joint, and performs negative feedback adjustment on each joint of the robot based on the vibration compensation amount corresponding to each joint.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 6.

Images