A double-motor cooperative control robot joint based on torque closed loop

Abstract

This invention discloses a dual-motor cooperative control robot joint based on torque closed-loop control, comprising a joint housing, a first servo motor, a second servo motor, a first transmission gear, a second transmission gear, an intermediate transmission gear, a torque detection unit, a speed detection unit, and an embedded controller. This invention employs a master-slave axis cooperative control architecture with torque closed-loop feedback. Using the torque and speed of the intermediate transmission gear as control targets, it adjusts the outputs of the two motors in reverse. Simultaneously, an adaptive disturbance observer is introduced to achieve feedforward compensation for nonlinear disturbances, and an integrated fault-tolerant mechanism enhances system reliability. It achieves smooth switching between the intermediate transmission gear's loaded stationary mode and its specified speed rotation mode. This invention solves the problem that existing dual-motor cooperative control robot joints cannot simultaneously achieve stable stationary operation under load and high-precision rotation control, significantly improving the joint's anti-disturbance capability and reliability. It is applicable to various scenarios such as precision transmission, industrial robots, and collaborative robots.

Description

Technical Field

[0001] This invention relates to the field of robot joint control technology, specifically to a dual-motor cooperative control robot joint based on torque closed-loop. Background Technology

[0002] With the rapid development of robotics technology, robots are being used more and more widely in industrial precision assembly, heavy-duty operations, and human-robot collaboration. This places higher demands on the performance of robot joints, requiring them not only to have high load capacity and anti-disturbance capabilities, but also to be able to switch between flexible motion states while maintaining stability in a static state and precision in a rotating state.

[0003] Single-motor driven robot joints are limited by the motor's rated power and torque, often resulting in insufficient load capacity, slow dynamic response, and poor disturbance immunity, making it difficult to meet the demands of high-performance operations. Therefore, dual-motor cooperatively driven robot joints have gradually become a research hotspot in the industry, improving the joint's load capacity and control flexibility through the coordinated work of two motors.

[0004] Existing dual-motor cooperative control schemes mainly fall into the following categories: (1) Dual-motor synchronous drive scheme: By controlling the synchronous output torque of two motors, the output torque of the two motors is superimposed to improve the overall load capacity of the joint. For example, the dual RV reducer synchronous control technology used in heavy-duty industrial robots. This type of scheme is mainly for heavy-duty scenarios and can improve the load capacity of the joint. However, its control core is the synchronous speed of the motor. It cannot achieve independent torque control of the intermediate transmission components and cannot achieve stable stationary operation of the intermediate components under load. When the load fluctuates, the intermediate components are prone to drift.

[0005] (2) Dual-motor differential decoupling scheme: The multi-degree-of-freedom decoupling of the joint is achieved by the differential motion of the two motors. For example, the differential hip joint of the humanoid robot achieves the motion decoupling of the two degrees of freedom by rotating the two motors in the same direction / opposite direction. This type of scheme is mainly used for motion decoupling of multi-degree-of-freedom joints. However, its control core is the differential speed matching of the rotation speed. It lacks torque closed-loop control for intermediate transmission components, cannot cope with torque balance under load fluctuations, and is difficult to achieve stable stationary intermediate components.

[0006] (3) Dual-motor control scheme for variable stiffness joints: The joint stiffness is dynamically adjusted by adjusting the position and stiffness of the joint by two motors respectively. This type of scheme is mainly for stiffness adjustment of flexible collaborative robots, but it cannot take into account the dual-mode control of the stationary and rotating intermediate transmission components, and cannot meet the needs of scenarios where intermediate components need to switch between different motion states.

[0007] Furthermore, most existing dual-motor collaborative control schemes use the motor's own speed and position parameters as control targets, neglecting the actual load state of the intermediate transmission components. When there are load fluctuations in the intermediate transmission components, stable torque balance often cannot be achieved, causing the intermediate components to drift when stationary or experience speed fluctuations when rotating. Additionally, existing schemes often directly switch control parameters when switching operating modes, which can easily generate instantaneous torque shocks. This not only affects the control accuracy of the joint but also accelerates the wear of the transmission components, reducing the joint's service life. Summary of the Invention

[0008] The purpose of this invention is to provide a dual-motor cooperative control robot joint based on torque closed-loop, so as to solve the problems mentioned in the background art.

[0009] To achieve the above objectives, the present invention provides the following technical solution: This invention discloses a dual-motor cooperative control robot joint based on torque closed-loop, including a joint housing, a first servo motor, a second servo motor, a first transmission gear, a second transmission gear, an intermediate transmission gear, a torque detection unit, a speed detection unit, and an embedded controller.

[0010] The first servo motor and the second servo motor are symmetrically arranged in the joint housing. The output end of the first servo motor is connected to the first transmission gear, and the output end of the second servo motor is connected to the second transmission gear. The first transmission gear and the second transmission gear respectively mesh with the intermediate transmission gear. The torque detection unit is used to detect the load torque of the intermediate transmission gear in real time, and the speed detection unit is used to detect the actual speed of the intermediate transmission gear in real time. The embedded controller is electrically connected to the driver of the first servo motor, the driver of the second servo motor, the torque detection unit, and the speed detection unit, respectively. The embedded controller is configured to adjust the output torque of the first servo motor and the second servo motor according to the working mode, based on the load torque and the actual speed, so as to realize the torque balance or torque difference drive of the intermediate transmission gear.

[0011] Furthermore, the embedded controller supports switching between static and rotating modes. In static mode, the embedded controller allocates the output torque of the first servo motor and the second servo motor according to the load torque, so that the sum of the output torque of the first servo motor after transmission ratio conversion and the load torque is equal to the output torque of the second servo motor after transmission ratio conversion, thereby achieving torque balance of the intermediate transmission gear and keeping the intermediate transmission gear stationary.

[0012] Furthermore, in the static mode, the embedded controller also acquires the actual rotational speed of the intermediate transmission gear in real time. If the actual rotational speed deviates from the target rotational speed of 0, the output torque of the first servo motor and / or the second servo motor is finely adjusted until the actual rotational speed returns to 0, thereby achieving torque closed-loop correction.

[0013] Furthermore, in the rotation mode, the embedded controller calculates the target torque difference between the two motors based on the target rotational speed of the intermediate transmission gear, and the target torque difference satisfies:

[0014] in, This is the output torque of the first servo motor. This is the output torque of the second servo motor. Let be the pitch circle radius of the first transmission gear. Let be the pitch circle radius of the second transmission gear. Let be the pitch circle radius of the intermediate transmission gear. This represents the rotational inertia of the intermediate transmission gear and the load. The angular acceleration of the intermediate transmission gear. This is the load torque of the intermediate transmission gear.

[0015] Furthermore, in the rotation mode, the embedded controller also uses a PID algorithm to correct the torque difference based on the deviation between the actual rotation speed and the target rotation speed of the intermediate transmission gear, thereby achieving closed-loop speed control.

[0016] Furthermore, when switching between stationary and rotating modes, the embedded controller adjusts the torque difference between the two motors in a linear and gradual manner to achieve a smooth transition between modes and avoid torque shock.

[0017] Furthermore, the embedded controller also integrates an adaptive disturbance observer, which is used to equate the frictional nonlinearity, gear transmission error, and external load mutation in the joint system to a lumped disturbance, estimate the lumped disturbance in real time, and add the disturbance estimate as a feedforward compensation amount to the torque distribution calculation; the adaptive gain of the adaptive disturbance observer is adaptively adjusted according to the actual speed of the intermediate transmission gear. When the actual speed is lower than a preset threshold, the adaptive gain is increased to improve the response speed of the disturbance estimation; when the actual speed is higher than the preset threshold, the adaptive gain is decreased to suppress measurement noise.

[0018] Furthermore, the embedded controller also integrates a fault detection and fault-tolerant control module, which is used to monitor the operating status of the first servo motor and the second servo motor in real time, including the phase current of the motor, the winding temperature, and the encoder feedback signal. When any parameter exceeds the preset safety threshold, it is determined to be a motor fault, and the system automatically switches to single motor drive mode, where the normal motor drives the intermediate transmission gear alone to ensure the basic operating capability of the joint.

[0019] The beneficial effects of this invention are as follows: This invention adopts a control architecture that uses the torque and speed of the intermediate transmission gear as the control target, rather than the parameters of the motor itself. This allows for real-time response to load changes in intermediate components, achieving more precise control and solving the problem of the control target being disconnected from the actual load in the prior art.

[0020] This invention achieves coordinated control of dual working modes. In stationary mode, the load torque is actively offset by a torque closed loop, so that even if the load fluctuates, the intermediate component can be stably kept stationary, solving the problem of intermediate component drift in the stationary state in the prior art. In rotating mode, the speed control accuracy is ensured by a controllable torque difference drive combined with speed closed loop control. At the same time, the torque output can be adjusted in real time according to the load to avoid motor overload.

[0021] When switching working modes, this invention achieves a smooth transition through a linear gradual change in torque difference, avoiding instantaneous torque impact. This not only improves the smoothness of control but also reduces the wear of transmission components and extends the service life of the joints.

[0022] The solution of this invention balances load capacity and control flexibility, and can be applied to various scenarios such as precision transmission and robot joints that require load switching motion states, effectively improving the dynamic response performance and anti-disturbance capability of robot joints. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of the structure of the present invention.

[0024] In the attached image: 1. First servo motor; 2. Second servo motor; 3. First transmission gear; 4. Second transmission gear; 5. Intermediate transmission gear; 6. Torque detection unit; 7. Speed ​​detection unit; 8. Joint housing. Detailed Implementation

[0025] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] The present invention provides a dual-motor cooperative control robot joint based on torque closed-loop, such as... Figure 1 As shown, it includes a joint housing 8, a first servo motor 1, a second servo motor 2, a first transmission gear 3, a second transmission gear 4, an intermediate transmission gear 5, a torque detection unit 6, a speed detection unit 7, and an embedded controller.

[0027] The first servo motor 1 and the second servo motor 2 are symmetrically arranged on both sides inside the joint housing 8. The output shaft of the first servo motor 1 is connected to the first transmission gear 3, and the output shaft of the second servo motor 2 is connected to the second transmission gear 4. The first transmission gear 3 and the second transmission gear 4 are respectively meshed with the intermediate transmission gear 5 for transmission. The power of the two servo motors is transmitted to the intermediate transmission gear 5 through the gear transmission mechanism. The intermediate transmission gear 5 serves as the output component of the joint and is connected to the external load to provide power to the load.

[0028] The torque detection unit 6 uses a high-precision torque sensor, integrated at the output end of the intermediate transmission gear 5, which can detect the external load torque on the intermediate transmission gear 5 in real time. The speed detection unit 7 uses a high-precision absolute encoder, which is also located at the output end of the intermediate transmission gear 5, and can detect the actual speed of the intermediate transmission gear 5 in real time. The resolution can reach 16 bits, ensuring the accuracy of speed detection.

[0029] Both the first servo motor 1 and the second servo motor 2 employ high-performance servo drivers supporting FOC (Field-Oriented Control). FOC control allows for real-time adjustment of the motor current, thereby precisely controlling the motor's output torque. Simultaneously, the driver supports torque closed-loop control, position / speed closed-loop control, and dual-axis collaborative functionality, enabling synchronous control of the two motors. The driver communicates with the embedded controller via an EtherCAT bus, achieving high-speed command transmission and status feedback to ensure real-time control.

[0030] The embedded controller uses the high-performance embedded motion controller of the STM32H7 series. This controller has powerful floating-point arithmetic capabilities and can run complex multi-axis cooperative control algorithms. The controller receives feedback data from the torque detection unit and the speed detection unit in real time, and then calculates the output torque command of the two servo motors according to the current working mode, and sends the command to the motor driver to realize the cooperative control of the two motors.

[0031] To further enhance the system's disturbance rejection capability, this invention also introduces an Adaptive Disturbance Observer (ADOB), which uniformly equates unmodeled dynamics such as frictional nonlinearity, gear transmission error, and sudden changes in external load in the joint system to lumped disturbances. The disturbance is estimated in real time by an observer, and feedforward compensation is performed. Specifically, the state equation of the observer is: in, This is an estimate of the disturbance. Let g be the time constant of the observer, and g be the adaptive gain, which is adjusted adaptively according to changes in rotational speed. When the speed is low, the gain is increased to improve the response speed of disturbance estimation; when the speed is high, the gain is decreased to suppress noise. When calculating the target torque of the motor, the controller incorporates the disturbance estimate as a feedforward compensation into the torque distribution calculation, i.e.: In this way, even if there are unmodeled disturbances, they can be quickly compensated for, ensuring torque balance.

[0032] The embedded controller of this invention also integrates a fault detection and fault-tolerant control module, which monitors the operating status of the two servo motors in real time, including the motor's phase current, winding temperature, and encoder feedback signal. When a fault is detected in one of the motors, such as overcurrent, overload, or encoder signal loss, the controller automatically triggers the fault-tolerant mode, shuts down the driver of the faulty motor, and adjusts the control algorithm to switch to a single-motor drive mode, where the normal motor drives the intermediate transmission gear alone, ensuring that the joint can continue to complete the current task and avoiding safety accidents caused by sudden robot shutdown.

[0033] The control principle of this invention is: The motion state of the intermediate transmission gear 5 is determined by the torque difference between the first servo motor 1 and the second servo motor 2. When the torque of the two motors on the intermediate gear is balanced with the external load torque, the intermediate gear remains stationary. When there is a net torque difference between the two motors on the intermediate gear, the intermediate gear rotates under the action of this torque difference. The greater the torque difference, the greater the angular acceleration of the rotation. Based on this, the present invention adopts a control architecture of "master-slave axis coordination + torque closed-loop feedback", taking the torque and speed of the intermediate transmission gear as the control target, and adjusting the output of the two motors in reverse to achieve precise coordinated control.

[0034] The control method of the present invention includes two operating modes and smooth switching between modes, as detailed below: (1) Stationary mode: The intermediate transmission gear is stationary under load. The goal of this mode is to keep the intermediate transmission gear stable and stationary under the action of external load. The core is to achieve torque balance of the intermediate gear by using torque closed-loop control to completely offset the difference in output torque between the two motors against the external load torque.

[0035] The specific control steps are as follows: S101, Load Torque Acquisition: The torque sensor acquires the load torque of the intermediate transmission gear in real time. For example, when an external load pulls the intermediate gear downwards, For clockwise rotation, the controller uses the load torque as the reference parameter for torque balance.

[0036] S102, Motor Torque Distribution: Calculate the required output torque of the two motors based on the gear ratio; where, Let be the pitch circle radius of the first transmission gear. Let be the pitch circle radius of the second transmission gear. Let be the pitch circle radius of the intermediate transmission gear. This represents the rotational inertia of the intermediate transmission gear and the load. The angular acceleration of the intermediate transmission gear. Given the load torque of the intermediate transmission gear, then the output torque of the first motor is... The torque transmitted to the intermediate gear is The output torque of the second motor The torque transmitted to the intermediate gear is To achieve torque balance in the intermediate gear, the torque balance formula must be satisfied: ; In this embodiment, the parameters of the first transmission gear and the second transmission gear are exactly the same, and the number of teeth is the same. Therefore Therefore, the above formula can be simplified to: According to the formula, the controller converts the detected load torque into torque commands for the two motors and distributes them to the two servo motors so that the output torque of the two motors satisfies the relationship, thereby initially achieving torque balance of the intermediate gear.

[0037] S103, Torque Closed-Loop Correction: The controller obtains the actual rotational speed of the intermediate gear from the absolute encoder feedback in real time. The target speed in this mode is 0 rpm. If the actual speed is greater than 0, it indicates that the intermediate gear has a tendency to rotate clockwise. At this time, the controller will fine-tune the output torque of the second motor to increase the torque. Alternatively, fine-tune the output torque of the first motor to reduce... The controller will adjust the torque of the two motors in the opposite direction until the actual speed of the intermediate gear returns to 0. If the actual speed is less than 0, it means that the intermediate gear has a tendency to rotate counterclockwise. The controller will adjust the torque of the two motors in the opposite direction until the speed returns to 0.

[0038] At the same time, when external loads fluctuate, for example If the load suddenly increases, the torque sensor will provide real-time feedback on the change, and the controller will synchronously correct the torque distribution between the two motors to restore torque balance. This ensures that even if the load fluctuates, the intermediate gear can remain stable and stationary without drifting.

[0039] (2) Rotation mode: The intermediate transmission gear rotates at a specified speed. The goal of this mode is to make the intermediate gear rotate stably at a specified target speed. The core is to actively apply a controllable torque difference, while limiting the speed deviation through speed closed-loop control to ensure the speed control accuracy.

[0040] The specific control steps are as follows: S201, Target Parameter Setting: The controller receives the target speed of the intermediate gear input by the user. For example, 100 rpm clockwise, and then calculate the required angular acceleration of the intermediate gear based on the change in the target speed. Then, the effective driving torque required to drive the intermediate gear to rotate is calculated.

[0041] S202, Torque Difference Calculation: The driving torque M5 of the intermediate gear must satisfy the dynamic formula: , in It is the total rotational inertia of the intermediate gear and the external load, while It is the net torque difference transmitted from the two motors to the intermediate gear, that is Therefore, the target torque difference between the two motors The following conditions must be met: The controller calculates the target torque difference required by the two motors based on this formula, which serves as the control benchmark.

[0042] S203, Speed ​​Closed-Loop Control: The encoder provides real-time feedback on the actual rotational speed of the intermediate gear. The controller calculates the speed deviation. Then, the speed deviation is converted into a torque difference correction amount through the PID algorithm; like Increase (such as adding) Or reduce ), increase the speed of the intermediate gear; like Decrease (e.g., reduce) Or increase Reduce the speed; If the actual rotational speed is lower than the target rotational speed, it indicates insufficient driving force, and the controller will increase the torque difference. For example, increasing the output torque of the second motor Or reduce the output torque of the first motor. Increase the speed of the intermediate gear; if the actual speed is greater than the target speed, it indicates that the driving force is too large, and the controller will reduce the torque difference. For example, reducing the output torque of the second motor Or increase the output torque of the first motor This reduces the speed of the intermediate gear, enabling high-precision speed control.

[0043] S204, Torque Limiting Protection: To prevent motor overload damage, the controller sets an upper limit on the output torque of the two servo motors, which is 1.2 times the rated torque of the motors. When the external load torque... When the torque difference is too large, the controller will automatically reduce the torque difference to prioritize the safety of the motor, rather than forcibly maintaining the speed, thus avoiding motor overload.

[0044] (3) Smooth mode switching When switching between stationary and rotating modes, a smooth transition must be achieved through a "gradual change in torque difference": From "stationary to rotating": the controller gradually increases (Linearly increase from 0 to the target torque difference) to avoid instantaneous torque impact causing the intermediate gear to "jump"; From "rotation → rest": gradually decreases The torque is reduced to 0, and the torques of A and B are redistributed according to the load torque until the speed of the intermediate gear returns to 0. To avoid instantaneous torque shocks during mode switching, this invention employs a linear, gradual change in torque difference to achieve a smooth transition during mode switching. When switching from stationary mode to rotating mode, the controller linearly increases the torque difference between the two motors from 0 to the target torque difference, instead of switching to the target torque difference instantaneously. This allows the speed of the intermediate gear to gradually increase, avoiding instantaneous torque shocks and preventing speed jumps. When switching from rotating mode to stationary mode, the controller gradually reduces the torque difference from its current value to 0. At the same time, it redistributes the torque of the two motors according to the current load torque until the speed of the intermediate gear returns to 0, thus entering stationary mode. The entire switching process is smooth and shock-free.

[0045] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A robot joint based on torque closed-loop dual-motor cooperative control, characterized in that, It includes a joint housing, a first servo motor, a second servo motor, a first transmission gear, a second transmission gear, an intermediate transmission gear, a torque detection unit, a speed detection unit, and an embedded controller; The first servo motor and the second servo motor are symmetrically arranged in the joint housing. The output end of the first servo motor is connected to the first transmission gear, and the output end of the second servo motor is connected to the second transmission gear. The first transmission gear and the second transmission gear respectively mesh with the intermediate transmission gear for transmission. The torque detection unit is used to detect the load torque of the intermediate transmission gear in real time, and the speed detection unit is used to detect the actual speed of the intermediate transmission gear in real time. The embedded controller is electrically connected to the driver of the first servo motor, the driver of the second servo motor, the torque detection unit, and the speed detection unit, respectively. The embedded controller is configured to: Based on the working mode, the output torque of the first servo motor and the second servo motor is adjusted according to the load torque and the actual speed to achieve torque balance or torque difference drive of the intermediate transmission gear.

2. The dual-motor cooperative control robot joint based on torque closed-loop as described in claim 1, characterized in that, The embedded controller supports switching between static and rotating modes. In static mode, the embedded controller allocates the output torque of the first servo motor and the second servo motor according to the load torque, so that the sum of the output torque of the first servo motor after transmission ratio conversion and the load torque is equal to the output torque of the second servo motor after transmission ratio conversion, thereby achieving torque balance of the intermediate transmission gear and keeping the intermediate transmission gear stationary.

3. The dual-motor cooperative control robot joint based on torque closed-loop as described in claim 2, characterized in that, In the static mode, the embedded controller also acquires the actual rotational speed of the intermediate transmission gear in real time. If the actual rotational speed deviates from the target rotational speed of 0, the output torque of the first servo motor and / or the second servo motor is finely adjusted until the actual rotational speed returns to 0, thereby achieving torque closed-loop correction.

4. The dual-motor cooperative control robot joint based on torque closed-loop as described in claim 1, characterized in that, In the rotation mode, the embedded controller calculates the target torque difference between the two motors based on the target rotational speed of the intermediate transmission gear. The target torque difference satisfies the following: ； in, This is the output torque of the first servo motor. This is the output torque of the second servo motor. Let be the pitch circle radius of the first transmission gear. Let be the pitch circle radius of the second transmission gear. Let be the pitch circle radius of the intermediate transmission gear. This represents the rotational inertia of the intermediate transmission gear and the load. The angular acceleration of the intermediate transmission gear. This is the load torque of the intermediate transmission gear.

5. A dual-motor cooperative control robot joint based on torque closed-loop control according to claim 4, characterized in that, In the rotation mode, the embedded controller also uses a PID algorithm to correct the torque difference based on the deviation between the actual rotation speed and the target rotation speed of the intermediate transmission gear, thereby achieving closed-loop speed control.

6. The dual-motor cooperative control robot joint based on torque closed-loop as described in claim 1, characterized in that, When switching between stationary and rotating modes, the embedded controller adjusts the torque difference between the two motors in a linear and gradual manner to achieve a smooth transition between modes and avoid torque shock.

7. A dual-motor cooperative control robot joint based on torque closed-loop control according to claim 1, characterized in that, The embedded controller is an STM32 embedded motion controller, used to run a multi-axis collaborative control algorithm to achieve collaborative control of torque closed loop and speed closed loop.

8. A dual-motor cooperative control robot joint based on torque closed-loop control according to claim 1, characterized in that, The embedded controller also integrates an adaptive disturbance observer, which is used to equate the frictional nonlinearity, gear transmission error, and external load mutation in the joint system to a lumped disturbance, estimate the lumped disturbance in real time, and add the disturbance estimate as a feedforward compensation amount to the torque distribution calculation.

9. A dual-motor cooperative control robot joint based on torque closed-loop control according to claim 8, characterized in that, The adaptive gain of the adaptive disturbance observer is adaptively adjusted according to the actual rotational speed of the intermediate transmission gear. When the actual rotational speed is lower than a preset threshold, the adaptive gain is increased to improve the response speed of the disturbance estimation. When the actual rotational speed is higher than the preset threshold, the adaptive gain is reduced to suppress measurement noise.

10. A dual-motor cooperative control robot joint based on torque closed-loop control according to claim 1, characterized in that, The embedded controller also integrates a fault detection and fault-tolerant control module, which is used to monitor the operating status of the first servo motor and the second servo motor in real time. When a fault is detected in one of the motors, it automatically switches to a single motor drive mode, where the normal motor drives the intermediate transmission gear alone.

Images