CN-116372926-B - Robot joint motion compensation method, system and storage medium

CN116372926BCN 116372926 BCN116372926 BCN 116372926BCN-116372926-B

Abstract

The application relates to a robot joint motion compensation method, a system and a storage medium. The method comprises the steps of predicting the interference compensation quantity of each joint when the amplitude of the interference signal is detected to be larger than a safety threshold value, carrying out negative feedback adjustment on the corresponding joint based on the interference compensation quantity corresponding to each joint, and predicting the vibration compensation quantity of each joint and carrying out negative feedback adjustment on each joint of the robot based on the vibration compensation quantity corresponding to each joint when the amplitude of the vibration signal is detected to be larger than the safety threshold value. The method can be used for carrying out interference and vibration compensation on the mechanical arm based on the interference compensation quantity and the vibration compensation quantity respectively, achieves the effect of compensating the coupling force interference and vibration in real time under the condition that a damping element is not added or signal filtering is not carried out, has high compensation precision, and is suitable for the coupling force interference compensation and the vibration compensation under different scenes.

Inventors

Request for anonymity
WANG JIAYIN

Assignees

上海微创医疗机器人（集团）股份有限公司

Dates

Publication Date: 20260508
Application Date: 20230410

Claims (10)

1. A method of robotic joint motion compensation, the method comprising: detecting whether the amplitude of the interference signal and the amplitude of the vibration signal of each joint of the robot are larger than a safety threshold in real time in the motion process; Under the condition that the amplitude of the interference signal is detected to be larger than a safety threshold, predicting the interference compensation quantity of each joint, and carrying out negative feedback adjustment on the corresponding joint based on the interference compensation quantity corresponding to each joint, wherein the interference compensation quantity is the moment variation quantity required to be adjusted for compensating the influence of coupling force interference on the motion of each joint of the robot; Under the condition that the amplitude of the vibration signal is detected to be larger than a safety threshold, predicting the vibration compensation quantity of each joint, and carrying out negative feedback adjustment on each joint of the robot based on the vibration compensation quantity corresponding to each joint, wherein the vibration compensation quantity is a motion change quantity required for compensating the influence of pure vibration interference in a specific frequency interval on the motion of each joint; After the joints of the robot complete the negative feedback adjustment, stopping the negative feedback adjustment on the joints of the robot under the condition that the amplitude of the interference signal and the amplitude of the vibration signal of the joints are smaller than the safety threshold value.
2. The method of claim 1, wherein predicting the amount of interference compensation for each joint comprises: the servo command is used for controlling the operation of each joint torque motor to the corresponding input torque; Inputting the input moment of each joint moment motor corresponding to the servo instruction into a joint dynamics model to obtain the ideal joint moment of each joint of the robot in an ideal environment without interference; controlling the moment motors of all joints to rotate according to the servo instruction, collecting the real joint moment of the moment motors of all joints, and determining the corresponding interference value of all joints based on the ideal joint moment of all joints and the real joint moment of all joints; And determining the interference compensation amount corresponding to each joint based on the interference value corresponding to each joint.
3. The method of claim 1, wherein predicting the vibration compensation amount for each joint comprises: The method comprises the steps of obtaining a servo command, analyzing input torque of each joint torque motor corresponding to the servo command through a motion controller, wherein the servo command corresponds to the input torque of each joint torque motor, and the servo command is used for controlling each joint torque motor to operate to the corresponding input torque; Inputting the input moment of each joint moment motor corresponding to the servo instruction into an equivalent rigid body dynamics model in a joint dynamics model to obtain ideal rigid body motion signals of each joint of the robot in an ideal environment without interference, wherein the ideal rigid body motion signals are position information and/or speed information corresponding to the joints under the condition without vibration; The moment motors of all joints are controlled to rotate according to the servo instructions, real motion signals of the moment motors of all joints are collected, and pure vibration values corresponding to all joints are determined based on ideal rigid motion signals of all joints and real motion signals of all joints, wherein the real motion signals are position information and/or speed information corresponding to the joints under the condition of vibration; and determining the vibration compensation amount corresponding to each joint based on the pure vibration value corresponding to each joint.
4. A method according to claim 3, wherein determining the net vibration value corresponding to each joint based on the ideal rigid body motion signal of each joint and the true motion signal of each joint comprises: And respectively inputting the ideal rigid body motion signals of the joints and the real motion signals of the joints into a closed-loop controller, and predicting the pure vibration values corresponding to the joints through the closed-loop controller, wherein the pure vibration values corresponding to the joints are fed back into the equivalent rigid body dynamics model, and the ideal rigid body motion signals of the joints output by the equivalent rigid body dynamics model are corrected.
5. A method according to claim 3, wherein determining the vibration compensation amount for each joint based on the pure vibration value for each joint comprises: And inputting the pure vibration value corresponding to each joint into a frequency selector, and obtaining the vibration compensation quantity corresponding to each joint in a specific frequency interval through the frequency selector, wherein the vibration compensation quantity corresponding to each joint is fed back to the input of the motion controller, and the input moment of each joint moment is corrected.
6. A method according to claim 3, wherein said negative feedback adjustment of each joint of the robot based on the vibration compensation amount corresponding to each joint comprises: And correcting the input torque of each joint torque motor based on the vibration compensation quantity corresponding to each joint and the real motion signal of each joint torque motor, and controlling each joint torque motor to operate based on the input torque after the modification of each joint torque motor, wherein the input torque of each joint torque motor is used as the input of an equivalent rigid body dynamic model.
7. The robot joint motion compensation system is characterized by comprising 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 a servo instruction to the servo unit; The servo unit controls each joint of the robot to move according to the servo instruction; The detection module detects whether the amplitude of the interference signal and the amplitude of the vibration signal of each joint of the robot are larger than a safety threshold in real time in the motion process; the interference suppression module predicts the interference compensation quantity of each joint under the condition that the amplitude of the interference signal is detected to be larger than a safety threshold value, and carries out negative feedback adjustment on the corresponding joint based on the interference compensation quantity corresponding to each joint, wherein the interference compensation quantity is the moment variation quantity required to be adjusted for compensating the influence of coupling force interference on the motion of each joint of the robot; The vibration suppression module predicts vibration compensation amounts of all joints under the condition that the amplitude of a vibration signal is detected to be larger than a safety threshold value, and carries out negative feedback adjustment on all joints of the robot based on the vibration compensation amounts corresponding to all joints, wherein the vibration compensation amounts are motion change amounts required for compensating the influence of pure vibration interference in a specific frequency interval on the motions of all joints; And after the joints of the robot are subjected to negative feedback adjustment, the judging module stops the negative feedback adjustment of the joints of the robot under the condition that the amplitude of the interference signal and the amplitude of the vibration signal of the joints are smaller than the safety threshold value.
8. The system of claim 7, wherein the interference suppression module comprises a no-interference operation unit, an interference estimation unit, and an interference compensation unit; the interference-free operation unit calculates and obtains the ideal joint moment of each joint of the robot in an ideal environment without interference through a joint dynamics model; The interference estimation unit determines the real joint moment of each joint of the robot in a real environment, and determines the corresponding interference value of each joint based on the ideal joint moment of each joint and the real joint moment of each joint; the interference compensation unit determines the interference compensation amount corresponding to 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 of claim 7, wherein the vibration suppression module comprises a no-vibration operation unit, a pure vibration estimation unit, and a vibration suppression unit; the vibration-free operation unit calculates an ideal rigid motion signal of each joint of the robot in an ideal environment without interference through an equivalent rigid dynamic model in the joint dynamic model, wherein the ideal rigid motion signal is position information and/or speed information corresponding to the joint under the condition of no vibration; the pure vibration estimation unit determines real motion signals of all joints of the robot in a real environment, and determines pure vibration values corresponding to all the joints based on ideal rigid motion signals of all the joints and the real motion signals of all the joints, wherein the real motion signals are position information and/or speed information corresponding to the joints under the condition of vibration; The vibration suppression unit determines vibration compensation amounts corresponding to the joints based on the pure vibration values corresponding to the joints, and performs negative feedback adjustment on the joints of the robot based on the vibration compensation amounts corresponding to the joints.
10. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the steps of the method of any of claims 1 to 6.

Description

Robot joint motion compensation method, system and storage medium Technical Field The present application relates to the field of robot control technologies, and in particular, to a method, a system, and a storage medium for robot joint motion compensation. Background The terminal of robot takes place coupling force interference and vibration phenomenon easily in the course of working, leads to its motion control's positioning accuracy very low. At present, the coupling force interference and vibration of a robot are mainly restrained in two modes, namely, a first mode, an external component such as a damping element is arranged on a robot body, the coupling force interference and vibration energy are absorbed through the external component to realize the coupling force interference and vibration restraint, and a second mode, a coupling force interference signal and a vibration signal are captured, coupling force interference frequency and vibration frequency are extracted, and vibration is restrained through filtering. The first mode is to additionally install the damping element to restrain vibration, so that the complexity and cost of the robot body structure are increased, meanwhile, the rigidity of the damping element reduces the rigidity of the robot body structure, and the vibration restraining effect depends on the service life and characteristic change of the damping element, such as the capability of the damping element for absorbing vibration energy can be influenced by temperature; In the second mode, when the coupling force interference frequency or the vibration frequency is different, the different vibration characteristics cannot be restrained by capturing the vibration frequency for filtering, and the time delay is generated when the vibration signal is extracted for analysis, so that the vibration restraining effect cannot be immediately responded and ensured. Disclosure of Invention In view of the foregoing, it is desirable to provide a robot joint motion compensation method, system, and storage medium that can realize real-time suppression of interference and vibration without relying on additional detection elements, and ensure that the interference and vibration suppression effects are not affected by the interference. In a first aspect, the present application provides a method of robot joint motion compensation. The method comprises the following steps: detecting whether the amplitude of the interference signal and the amplitude of the vibration signal of each joint of the robot are larger than a safety threshold in real time in the motion process; Under the condition that the amplitude of the interference signal is detected to be larger than a safety threshold, predicting the interference compensation quantity of each joint, and carrying out negative feedback adjustment on the corresponding joint based on the interference compensation quantity corresponding to each joint, wherein the interference compensation quantity is the moment variation quantity required to be adjusted for compensating the influence of the coupling force interference on the movement of each joint of the robot; Under the condition that the amplitude of the vibration signal is detected to be larger than a safety threshold, predicting the vibration compensation quantity of each joint, and carrying out negative feedback adjustment on each joint of the robot based on the vibration compensation quantity corresponding to each joint, wherein the vibration compensation quantity is a motion variation quantity required for compensating the influence of pure vibration interference in a specific frequency interval on the motion of each joint; After the joints of the robot complete the negative feedback adjustment, under the condition that the amplitude of the interference signal and the amplitude of the vibration signal of each joint are smaller than the safety threshold, the negative feedback adjustment of each joint of the robot is stopped. In one embodiment, predicting the amount of disturbance compensation for each joint includes: the servo command corresponds to the input torque of each joint torque motor, and the servo command is used for controlling each joint torque motor to operate to the corresponding input torque; inputting the input moment of each joint moment motor corresponding to the servo command into a joint dynamics model to obtain the ideal joint moment of each joint of the robot in an ideal environment without interference; Controlling the moment motors of all joints to rotate according to the servo instructions, collecting the real joint moment of the moment motors of all joints, and determining the corresponding interference value of all joints based on the ideal joint moment of all joints and the real joint moment of all joints; And determining the interference compensation amount corresponding to each joint based on the interference value corresponding to each joint. In one embodiment, predicting the