A robotic arm control device and robot based on kinetic energy recovery

Abstract

The present invention relates to a manipulator control device and a robot based on kinetic energy recovery. The manipulator control device includes a motor, a motor drive module, a controller and an energy storage device; the motor drive module is connected to a power source, and controls the rotation of the motor through a control signal sent by the controller; the energy storage device is connected to the motor drive module, and receives the reverse current transmitted from the motor through the motor drive module and stores electric energy when the motor decelerates; and / or releases the stored electric energy to output a discharge current when the motor accelerates, and the discharge current is transmitted to the motor through the motor drive module. The manipulator control device based on kinetic energy recovery of the present invention, by providing an energy storage device, stores energy when the motor decelerates and releases energy when the motor accelerates, which can not only reduce the heating of the motor and improve the safety of equipment operation, but also save energy and reduce costs.

Description

Technical Field

[0001] The present invention relates to the field of robot motion control, and in particular to a robotic arm control device and a robot based on kinetic energy recovery. Background Art

[0002] A multi-axis robot has multiple arms, each with its own motor and motor driver module to control its motion. Figure 1 , Figure 1 This is a schematic diagram of a conventional robotic arm control device module. The conventional robotic arm control device includes a motor, a motor drive module, and a controller. The motor drive module is connected to a power source and controls the rotation of the motor, thereby controlling the movement of the robotic arm, through control signals generated by the controller.

[0003] The function of the motor drive module of the existing robotic arm control device is to rectify, filter, invert, and perform other processing on the current input by the power supply. For example, when the motor is a three-phase AC motor, but the power supply is a two-phase DC power supply, the motor drive module includes at least an inverter unit. The two-phase DC power output by the power supply is input to the motor drive module and then converted into the three-phase AC power required by the motor and output to the motor. In addition, the motor drive module in the prior art is generally provided with a number of capacitors. The function of these capacitors is to filter and stabilize the current to prevent excessive current fluctuations during the operation of the motor, which may affect the performance of circuit components or accidentally trigger the short-circuit protection unit of the motor drive module. Summary of the Invention

[0004] Based on this, the object of the present invention is to provide a robotic arm control device and a robot based on kinetic energy recovery.

[0005] A robotic arm control device based on kinetic energy recovery includes a motor, a motor drive module, a controller and an energy storage device; the motor drive module is connected to a power supply and controls the rotation of the motor through a control signal sent by the controller; the energy storage device is connected to the motor drive module and receives a reverse current transmitted from the motor through the motor drive module and stores electrical energy when the motor decelerates; and / or releases the stored electrical energy to output a discharge current when the motor accelerates, and the discharge current is transmitted to the motor through the motor drive module.

[0006] The robotic arm control device based on kinetic energy recovery of the present invention, by providing an energy storage device, stores energy when the motor decelerates and releases energy when the motor accelerates. This can not only reduce motor heating and improve equipment operation safety, but also save energy and reduce costs.

[0007] Furthermore, the energy storage device includes an auxiliary battery and a supercapacitor connected in parallel with the auxiliary battery; the supercapacitor is quickly charged in a short time and stores the electric energy in the auxiliary battery with a delayed delay.

[0008] Furthermore, it also includes a speed sensor and a charge and discharge management circuit; the speed sensor detects the speed of the motor and transmits the speed signal to the controller; the charge and discharge management circuit includes a charging switch and a discharging switch; the charging switch and the discharging switch are connected in parallel between the motor drive module and the energy storage device; when the controller detects that the motor is decelerating, the charging switch is controlled to be turned on and the discharging switch is controlled to be turned off, so that the energy storage device is charged; when the controller detects that the motor is accelerating, the charging switch is controlled to be turned off and the discharging switch is controlled to be turned on, so that the energy storage device is discharged.

[0009] Furthermore, the charge and discharge management circuit further includes a rectification and transformation module and an inversion and transformation module;

[0010] The rectifier and transformer module is connected to the motor drive module and the energy storage device, receives the multi-phase alternating current output by the motor drive module, and converts the multi-phase alternating current into two-phase direct current and outputs it to the energy storage device; the charging switch is provided on each phase line of the multi-phase alternating current output by the motor drive module to the rectifier and transformer module;

[0011] The inverter and transformer module is connected to the energy storage device and the motor drive module, receives the two-phase DC power output by the energy storage device, and converts the two-phase DC power into multi-phase AC power and outputs it to the motor drive module; the discharge switch is arranged on each phase line of the inverter and transformer module outputting multi-phase AC power to the motor drive module.

[0012] Furthermore, it also includes a voltage sensor; the voltage sensor detects the voltage of the energy storage device and transmits the voltage signal to the controller; when the controller detects that the voltage of the energy storage device is greater than the set maximum threshold, the controller controls the charging switch to be disconnected; when the controller detects that the voltage of the energy storage device is less than the set minimum threshold, the controller controls the discharging switch to be disconnected.

[0013] Furthermore, the charge and discharge management circuit also includes a discharge MOSFET tube, a charge MOSFET tube, a battery charge and discharge protection board, a first resistor, a second resistor and a filter capacitor;

[0014] The output terminals of the rectifier and transformer module for outputting two-phase direct current are referred to as the first output terminal and the second output terminal; the input terminals of the inverter and transformer module for inputting two-phase direct current are referred to as the first input terminal and the second input terminal;

[0015] The battery charge and discharge protection board is provided with a VCC terminal, a GND terminal, a DO terminal, a CO terminal, and a VM terminal; the VCC terminal is connected to the positive electrode of the auxiliary battery; the GND terminal is grounded; the DO terminal is connected to the gate of the discharge MOSFET tube and is normally high; the CO terminal is connected to the gate of the charge MOSFET tube and is normally high; the VM terminal is connected to the second output terminal of the rectifier and transformer module through a second resistor; the VM terminal detects the voltage of the second output terminal of the rectifier and transformer module; when the voltage of the second output terminal of the rectifier and transformer module exceeds a certain threshold and lasts for a period of time, the battery charge and discharge protection board outputs a low level to the DO terminal;

[0016] The positive electrode of the auxiliary battery is connected to the first output terminal of the rectifier and transformer module, and the negative electrode is grounded; the positive electrode of the auxiliary battery is connected to the VCC terminal of the battery charge and discharge protection board through a first resistor; one end of the filter capacitor is connected to the VCC terminal of the battery charge and discharge protection board, and the other end is grounded;

[0017] The source of the discharge MOSFET is connected to the negative electrode of the auxiliary battery, the gate is connected to the DO terminal of the battery charge and discharge protection board, and the drain is connected to the second input terminal of the inverter and transformer module; the discharge MOSFET is provided with a body diode D1, the cathode of the body diode D1 is connected to the drain, and the anode is connected to the source;

[0018] The source of the charging MOSFET is connected to the second output terminal of the rectifier and transformer module, the gate is connected to the CO terminal of the battery charge and discharge protection board, and the drain is connected to the drain of the discharge MOSFET. The charging MOSFET is provided with a body diode D2, the cathode of the body diode D2 is connected to the drain, and the anode is connected to the source.

[0019] The battery charge and discharge protection board detects the voltage of the energy storage device through the VCC terminal and the GND terminal; when the battery charge and discharge protection board detects that the voltage of the energy storage device is less than the minimum threshold, the DO terminal of the battery charge and discharge protection board becomes a low level; when the battery charge and discharge protection board detects that the voltage of the energy storage device exceeds the maximum threshold, the CO terminal of the battery charge and discharge protection board becomes a low level.

[0020] Furthermore, when the energy storage device releases energy, the controller controls the closing time of the discharge switch by solving the following control law equation:

[0021]

[0022] Wherein, T is the closing time of the discharge switch, is the first-order derivative of the energy actually released by the energy storage device, is the first-order derivative of the energy storage device’s expected release energy, is the second-order derivative of the energy actually released by the energy storage device, is the second-order derivative of the energy storage device expected to release energy, k is the parameter to be designed, and Λ is the parameter to be designed.

[0023] The present invention also provides a robot comprising at least one robotic arm and any one of the above-mentioned robotic arm control devices, wherein the robotic arm control device controls the movement of the robotic arm.

[0024] For better understanding and implementation, the present invention is described in detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 It is a schematic diagram of the existing robot arm control device module;

[0026] Figure 2 Schematic diagram of a module of a robotic arm control device based on kinetic energy recovery according to Example 1 of the present invention;

[0027] Figure 3 This is a schematic diagram of a module of a robotic arm control device based on kinetic energy recovery according to Example 2 of the present invention;

[0028] Figure 4 Schematic diagram of a module of a robotic arm control device based on kinetic energy recovery according to embodiment 3 of the present invention;

[0029] Figure 5 This is a schematic diagram of the energy storage device and charge and discharge management circuit structure of Example 3 of the present invention. DETAILED DESCRIPTION

[0030] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present application will be further described in detail below with reference to the accompanying drawings.

[0031] While using existing multi-axis robots, the inventors discovered that some sensitive components on the robots would occasionally break down, particularly when the robot's arm was frequently subjected to speed changes. Analysis and research revealed that the frequent speed changes in the multi-axis robot's arm were achieved by frequently changing the speed of the corresponding motor, which in turn led to severe overheating. Heat generated by the motors was then transferred to the robot, causing it to overheat, potentially damaging some temperature-sensitive components.

[0032] The present invention further investigated the root cause of heating caused by motor speed fluctuations and found that heating primarily occurs during the motor's deceleration phase. Currently, a three-phase AC motor is commonly used to control a robotic arm. It comprises a stator and a rotor mounted on the stator's central axis. Three-phase AC current flows through the stator windings. By controlling the magnitude and frequency of the three-phase AC current, a rotating magnetic field is generated. As the rotating magnetic field generated by the stator windings rotates, the rotor windings cut through the magnetic flux lines, generating an induced current. The magnetic field generated by the induced current interacts with the rotating magnetic field, causing the rotor to rotate relative to the stator. When the motor decelerates, the controller, through the motor drive module, adjusts the magnitude and frequency of the three-phase AC current flowing through the stator windings to reduce the speed of the stator's rotating magnetic field. At this point, due to inertia, the rotor maintains a relatively high speed. The rotor, still maintaining a relatively high speed, rotates in the opposite direction to the stator's rotating magnetic field, which is slowing down. This generates a back electromotive force (EMF), which reduces the rotor's speed. Simultaneously, this back electromotive force generates a partial reverse current in the stator windings, causing the motor to heat up.

[0033] While researching methods to reduce motor heat generation, the present inventors discovered that it was possible to recover and reuse some of the reverse current generated in the stator windings during the motor's deceleration phase. This not only addresses the internal heat generation issue, but also further improves energy efficiency. Based on this approach, the present inventors add an energy storage device to the existing multi-axis robot control device. This energy storage device converts mechanical energy into stored electrical energy during motor deceleration, and releases the stored electrical energy as motor assist during motor acceleration.

[0034] Example 1

[0035] See also Figure 2 , Figure 2 Schematic diagram of a module of a robotic arm control device based on kinetic energy recovery according to embodiment 1 of the present invention. The robotic arm control device based on kinetic energy recovery according to embodiment 1 of the present invention comprises a motor 10 , a motor driving module 20 , a controller 30 and an energy storage device 40 .

[0036] The motor drive module 20 is connected to a power source and controls the rotation of the motor 10 and thus the movement of the robot arm A through a control signal sent by the controller 30 .

[0037] The energy storage device 40 is connected to the motor drive module 20, and receives the reverse current transmitted from the motor 10 through the motor drive module 20 and stores electrical energy when the motor 10 decelerates; or releases the stored electrical energy and outputs a discharge current when the motor 10 accelerates. The output discharge current is transmitted to the motor 10 through the motor drive module 20, providing assistance for motor acceleration. In a specific embodiment, the motor is a three-phase AC motor, and the power supply is a two-phase DC power supply; the motor drive module is an inverter unit for converting two-phase DC power into three-phase AC power, its input end is connected to the two-phase DC power output of the power supply, and its output end is connected to the three-phase AC power input end of the motor; then the energy storage device is connected to the three-phase AC power output end of the motor drive module.

[0038] Specifically, the energy storage device 40 includes an auxiliary battery and a supercapacitor connected in parallel with the auxiliary battery. The supercapacitor is quickly charged in a short period of time when the motor decelerates, and the electric energy is delayed and stored in the auxiliary battery. The auxiliary battery itself can also complete charging and discharging, but the time for motor deceleration is generally very short, and the charging speed of the auxiliary battery is relatively slow, and it is impossible to complete charging in such a short time, so a supercapacitor connected in parallel with the auxiliary battery is added. The supercapacitor can be quickly charged in a short period of time when the motor decelerates, and then the electric energy is delayed and stored in the auxiliary battery. In this way, even if the time for motor deceleration is very short, the auxiliary battery can effectively complete charging. Since the auxiliary battery and the supercapacitor are connected in parallel to form an energy storage device, the voltage of the energy storage device is equal to the voltage of the auxiliary battery and also equal to the voltage of the supercapacitor.

[0039] The energy storage device 40 charges during motor 10 deceleration and discharges during motor 10 acceleration in the following manner: When the controller 30 decelerates the motor 10, it controls the magnitude and frequency of the current in the motor's stator windings to reduce the speed of the stator's rotating magnetic field. At this point, the motor's rotor maintains a relatively high speed due to inertia. Relative to the decreasing rotating magnetic field, the motor's rotor rotates in the opposite direction, cutting through the magnetic flux lines, thereby generating a reverse electromotive force (EMF). This causes the voltage at the motor 10 terminal to be higher than the voltage at the energy storage device 40. Because the voltage at the motor 10 terminal is higher and the voltage at the energy storage device 40 terminal is lower, the reverse current at the motor 10 terminal flows through the motor drive module 20 into the energy storage device 40, charging the energy storage device 40. When the controller 30 accelerates the motor, the motor 10 becomes a load that consumes electrical energy, the reverse electromotive force disappears, and its voltage is lower. However, the energy storage device 40 stores electrical energy and has a higher voltage. Therefore, the energy storage device 40 spontaneously releases electrical energy to form a discharge current, which is then transmitted to the motor 10 via the motor drive module 20, providing power to the motor 10.

[0040] Example 2

[0041] See also Figure 3 , Figure 3 Schematic diagram of a robotic arm control device module based on kinetic energy recovery according to Example 2 of the present invention. The robotic arm control device based on kinetic energy recovery according to Example 2 of the present invention includes a motor 10, a motor drive module 20, a controller 30, an energy storage device 40, a speed sensor 50, a charge and discharge management circuit 60, and a voltage sensor 70.

[0042] The motor drive module 20 is connected to a power source and controls the rotation of the motor 10 and thus the movement of the robot arm A through a control signal sent by the controller 30 .

[0043] The energy storage device 40 is connected to the motor drive module 20 via the charge and discharge management circuit 60. When the motor 10 decelerates, it receives the reverse current transmitted from the motor 10 through the motor drive module 20 and the charge and discharge management circuit 60 and stores the electrical energy. Alternatively, when the motor 10 accelerates, it releases the stored electrical energy and outputs a discharge current. The output discharge current is transmitted to the motor 10 via the charge and discharge management circuit 60 and the motor drive module 20, providing assistance for the acceleration of the motor 10. Specifically, the energy storage device 40 includes an auxiliary battery and a supercapacitor connected in parallel with the auxiliary battery. The supercapacitor is rapidly charged in a short period of time and stores the electrical energy in the auxiliary battery with a delayed delay.

[0044] The rotation speed sensor 50 detects the rotation speed of the motor 10 and transmits a rotation speed signal to the controller 30 .

[0045] The charge and discharge management circuit 60 includes a charging switch 62 and a discharging switch 64; the charging switch 62 and the discharging switch 64 are connected in parallel between the motor drive module 20 and the energy storage device 40; when the controller 30 detects that the motor 10 is decelerating, the controller 30 controls the charging switch 62 to be turned on and the discharging switch 64 to be turned off, so that the energy storage device 40 is charged; when the controller detects that the motor 10 is accelerating, the controller controls the charging switch 62 to be turned off and the discharging switch 64 to be turned on, so that the energy storage device 40 is discharged.

[0046] The voltage sensor 70 detects the voltage of the energy storage device 40 and transmits the voltage signal to the controller 30. When the controller 30 detects that the voltage of the energy storage device 40 is greater than the set maximum threshold, the controller controls the charging switch 62 to disconnect, so that the energy storage device 40 stops charging. Setting overcharge protection can ensure that the auxiliary battery will not be overcharged, extend the battery life, and ensure safety. When the controller 30 detects that the voltage of the energy storage device 40 is less than the set minimum threshold, the controller 30 controls the discharge switch 64 to disconnect, so that the energy storage device 40 stops discharging. Setting over-discharge protection is because, on the one hand, it can prevent the auxiliary battery from over-discharging and damaging the battery life, and on the other hand, when the auxiliary battery voltage is not high enough, the effect of releasing energy to assist the motor acceleration is limited.

[0047] Furthermore, to ensure smooth movement of robotic arm A and uniform speed changes of motor 10, the energy stored in energy storage device 40 should not be released all at once but rather should be released in multiple, steady bursts. Therefore, controller 30 adjusts the release rate of energy from the energy storage device by controlling the closing time of discharge switch 64. To ensure that the energy output of energy storage device 40 matches the expected output energy level and thus ensure the accuracy of robotic arm A's operation, a sliding mode algorithm is employed to control it.

[0048] Assume that the function of the discharge switch closing time T and the energy released by the energy storage device E is:

[0049] E=Z(T), where T is the closing time of the discharge switch.

[0050] The synovial function is defined as:

[0051] e=EE d , where E is the actual energy released by the energy storage device, E d is the energy that the energy storage device is expected to release, e is the difference between the actual and expected released energies, and Λ is the parameter to be designed.

[0052] Choose the Lyapunov function as:

[0053] Its derivative is:

[0054]

[0055] In order to make

[0056]

[0057] because for The inverse function of , so the control law can be written as:

[0058] Where T is the closing time of the discharge switch, is the first-order derivative of the actual energy released by the energy storage device, is the first-order derivative of the energy storage device’s expected release energy, is the second-order derivative of the energy actually released by the energy storage device, is the second-order derivative of the energy storage device's desired release energy, k is a parameter to be designed, and Λ is a parameter to be designed. By solving this control law equation, the controller can control the discharge switch closing time T, thereby controlling the energy release rate of the energy storage device.

[0059] Example 3

[0060] The structure of the kinetic energy recovery-based robotic arm control device of Example 3 of the present invention is basically the same as that of the kinetic energy recovery-based robotic arm control device of Example 2. The difference lies in the specific circuit structure design of the charge and discharge management circuit 60. Since the battery charge and discharge protection board of the charge and discharge management circuit 60 has a voltage detection function, the kinetic energy recovery-based robotic arm control device of Example 3 of the present invention does not include a voltage sensor. Figure 4 and Figure 5 , Figure 4 Schematic diagram of a module of a robotic arm control device based on kinetic energy recovery according to embodiment 3 of the present invention. Figure 5 Schematic diagram of the energy storage device and charge-discharge management circuit structure of Example 3 of the present invention. The robotic arm control device based on kinetic energy recovery of Example 3 of the present invention includes: a motor 10, a motor drive module 20, a controller 30, an energy storage device 40, a speed sensor 50, and a charge-discharge management circuit 60.

[0061] The charge and discharge management circuit 60 includes: a charging switch, a discharging switch, a rectifier and transformer module, an inverter and transformer module, a discharge MOSFET tube MOS1, a charging MOSFET tube MOS2, a battery charge and discharge protection board, a first resistor R1, a second resistor R2 and a filter capacitor C2.

[0062] The rectifier and transformer module is connected to the motor drive module and the energy storage device, receives the multi-phase AC power output by the motor drive module, and converts the multi-phase AC power into two-phase DC power for output to the energy storage device; the charging switch is provided on each phase line from the motor drive module to the rectifier and transformer module. The inverter and transformer module is connected to the energy storage device and the motor drive module, receives the two-phase DC power output by the energy storage device, and converts the two-phase DC power into multi-phase AC power for output to the motor drive module; the discharge switch is provided on each phase line from the inverter and transformer module to the motor drive module. The discharge MOSFET tube MOS1, the charging MOSFET tube MOS2, the battery charge and discharge protection board, the first resistor R1, the second resistor R2, and the filter capacitor C2 form a charge and discharge protection unit that prevents overcharging and over-discharging.

[0063] In this embodiment, the number of the motor drive modules is two. In other embodiments, the number of the motor drive modules may be other. In this embodiment, each motor drive module includes three phase lines, denoted as a first phase line, a second phase line, and a third phase line. In other embodiments, the number of phase lines of the motor drive module may be other.

[0064] Specifically, the three-phase AC input terminals of the rectifier and transformer module are denoted as U, V, and W; the two-phase DC output terminals of the rectifier and transformer module are denoted as the first output terminal and the second output terminal. The U terminal is connected to the first phase lines of multiple motor drive modules via a charging switch, with the first phase lines of different motor drive modules connected in parallel. Similarly, the V terminal is connected to the second phase lines of multiple motor drive modules via a charging switch, with the second phase lines of different motor drive modules connected in parallel. The W terminal is connected to the third phase lines of multiple motor drive modules via a charging switch, with the third phase lines of different motor drive modules connected in parallel. In this embodiment, the U terminal is connected to the first phase lines of the first and second motor drive modules via charging switches S1 and S7, respectively; the V terminal is connected to the second phase lines of the first and second motor drive modules via charging switches S2 and S8, respectively; and the W terminal is connected to the third phase lines of the first and second motor drive modules via charging switches S3 and S9, respectively. The first output end is connected to the positive electrode of the auxiliary battery, and the second output end is connected to the source (s electrode) of the charging MOSFET tube MOS2.

[0065] Specifically, the battery charge and discharge protection board is provided with a VCC terminal, a GND terminal, a VM terminal, a DO terminal, and a CO terminal. The VCC terminal is connected to the positive electrode of the auxiliary battery and is the working voltage input terminal; the auxiliary battery provides the working voltage for the battery charge and discharge protection board. The GND terminal is grounded. The DO terminal is connected to the gate (g pole) of the discharge MOSFET tube MOS1 and is normally high. The CO terminal is connected to the gate (g pole) of the charging MOSFET tube MOS2 and is normally high. The VM terminal is connected to the second output terminal of the rectifier and transformer module through the second resistor R2 to detect the voltage of the second output terminal of the rectifier and transformer module; when the voltage of the second output terminal of the rectifier and transformer module exceeds a certain threshold and lasts for a period of time, the battery charge and discharge protection board outputs a low level to the DO terminal. The second resistor R2 acts as a voltage pull-down resistor to keep the VM terminal at a low level under normal conditions.

[0066] Specifically, the positive electrode of the auxiliary battery is connected to the first output terminal of the rectifier and transformer module, and the negative electrode is grounded. The positive electrode of the auxiliary battery is connected to the VCC terminal of the battery charge and discharge protection board through the first resistor R1. One end of the supercapacitor C1 is connected to the positive electrode of the auxiliary battery, and the other end is connected to the negative electrode of the auxiliary battery. One end of the filter capacitor C2 is connected to the VCC terminal of the battery charge and discharge protection board, and the other end is grounded. The first resistor R1 is a reference power supply resistor, which forms a voltage divider circuit with the internal resistor of the battery charge and discharge protection board to control the reference voltage of the voltage comparator inside the battery charge and discharge protection board. The filter capacitor C2 plays a role in voltage stabilization and filtering.

[0067] Specifically, the source (s-pole) of the charging MOSFET tube MOS2 is connected to the second output terminal of the rectifier and transformer module, the gate (g-pole) is connected to the CO terminal of the battery charge and discharge protection board, and the drain (d-pole) is connected to the drain (d-pole) of the discharge MOSFET tube MOS1. The charging MOSFET tube MOS2 is provided with a body diode D2, the cathode of the body diode D2 is connected to the drain (d-pole), and the anode is connected to the source (s-pole).

[0068] Specifically, the source (s-pole) of the discharge MOSFET tube MOS1 is connected to the negative electrode of the auxiliary battery, the gate (g-pole) is connected to the DO terminal of the battery charge and discharge protection board, and the drain (d-pole) is connected to the second input terminal of the inverter and transformer module; the discharge MOSFET tube MOS1 is provided with a body diode D1, the cathode of the body diode D1 is connected to the drain (d-pole), and the anode is connected to the source (s-pole).

[0069] Specifically, the inverter and transformer module converts the two-phase direct current output by the energy storage device into three-phase alternating current and outputs it to the motor drive module, and the discharge current output by the inverter and transformer module supplies power to the motor through the motor drive module. The two-phase direct current input terminals of the inverter and transformer module are referred to as the first input terminal and the second input terminal, and the three-phase alternating current output terminals are referred to as the U output terminal, the V output terminal and the W output terminal. The first input terminal is connected to the positive electrode of the auxiliary battery, and the second input terminal is connected to the drain (d pole) of the discharge MOSFET tube MOS1. The U output terminal is connected to the first phase line of multiple motor drive modules through a discharge switch, the V output terminal is connected to the second phase line of multiple motor drive modules through a discharge switch, and the W output terminal is connected to the third phase line of multiple motor drive modules through a discharge switch. In this embodiment, the U output end is connected to the first phase line of the first motor drive module and the second motor drive module through the discharge switches S4 and S10 respectively; the V output end is connected to the second phase line of the first motor drive module and the second motor drive module through the discharge switches S5 and S11 respectively; the W output end is connected to the third phase line of the first motor drive module and the second motor drive module through the discharge switches S6 and S12 respectively; and the charging switch and the discharging switch on each phase line are connected in parallel with each other.

[0070] In this embodiment, the battery charge and discharge protection board detects the voltage of the energy storage device through the VCC and GND terminals. When the battery charge and discharge protection board detects that the voltage of the energy storage device is less than the minimum threshold, the DO terminal of the battery charge and discharge protection board changes to a low level; when the battery charge and discharge protection board detects that the voltage of the energy storage device exceeds the maximum threshold, the CO terminal of the battery charge and discharge protection board changes to a low level.

[0071] The following is based on Figure 5The circuit structure shown in the figure details the controller's process for controlling the charge and discharge management circuit. When the controller detects motor deceleration via the speed sensor, it controls the motor's corresponding charging switch to close and the discharge switch to open. The rectifier and transformer module converts the charging current from three-phase AC to two-phase DC, creating a voltage between the first and second output terminals. Because the DO and CO output terminals of the motor's charge and discharge protection board are normally high, the ds conductive channels of the discharge MOSFET MOS1 and the charge MOSFET MOS2 are normally conductive. At this point, the auxiliary battery's positive electrode is connected to the first output terminal, and its negative electrode is connected to the second output terminal via the conductive channels of the discharge MOSFET MOS1 and the charge MOSFET MOS2. The charging circuit (charging switch → rectifier and transformer module → charge MOSFET MOS2 → discharge MOSFET MOS1 → energy storage device) is complete, and the first and second output terminals charge the auxiliary battery. Because the nature of MOSFETs is that when the voltage at the gs terminal remains constant, the current at the ds terminal remains constant. Therefore, regardless of the voltage between the first and second output terminals, the charging current is always limited, effectively protecting the device.

[0072] When the controller detects motor acceleration, it controls the motor's corresponding discharge switch to close and the charge switch to open. Since the auxiliary battery's positive electrode is connected to the first input of the inverter and transformer module, and its negative electrode is connected to the second input of the inverter and transformer module via the discharge MOSFET MOS1, and the DO output of the motor's charge and discharge protection board is normally high, the conductive channel of the discharge MOSFET MOS1 is normally conductive. The discharge circuit ("energy storage device → discharge MOSFET MOS1 → inverter and transformer module → discharge switch") is conductive, and the auxiliary battery outputs two-phase DC power to the inverter and transformer module. The inverter and transformer module converts the auxiliary battery's two-phase DC power into three-phase AC power, which is then supplied to the motor via the motor drive module.

[0073] When the battery charge and discharge protection board detects, via its VCC and GND terminals, that the energy storage device's voltage exceeds the maximum threshold, its CO terminal goes low, disconnecting the charging MOSFET MOS2 and the charging circuit, halting charging of the energy storage device. Because the discharge circuit is independent of the charging MOSFET MOS2, disconnecting it will not affect discharge. This overcharge protection prevents overcharging of the auxiliary battery, extending battery life and ensuring safety.

[0074] When the battery charge and discharge protection board detects, via its VCC and GND terminals, that the voltage of the energy storage device is below the minimum threshold, its DO terminal goes low, disconnecting the discharge MOSFET MOS1 and the discharge circuit, halting discharge from the energy storage device. At this point, if a decelerating motor closes its corresponding charging switch, the charging current can still form a charging circuit through the body diode D1, even if the conductive channel of the discharge MOSFET MOS1 is disconnected. Therefore, disconnecting the discharge MOSFET MOS1 only halts discharge and does not affect charging. Over-discharge protection is required, firstly, because excessive discharge of the auxiliary battery can damage battery life, and secondly, because if the auxiliary battery voltage is not high enough, the energy released from it is limited in its ability to assist in motor acceleration.

[0075] The present invention's kinetic energy recovery-based robotic arm control device utilizes an energy storage device to store energy during motor deceleration and release it during acceleration. This not only reduces motor heating and improves operational safety, but also saves energy and reduces costs. The present invention also utilizes a charge-discharge management circuit, a speed sensor, a voltage sensor, and a control algorithm to control the charging and discharging of the energy storage device, ensuring safe charging and smooth discharge.

[0076] The above-described embodiments merely illustrate several implementations of the present invention. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art would be able to make numerous variations and improvements without departing from the spirit of the present invention, and all such variations and improvements fall within the scope of protection of the present invention.

Claims

1. A robotic arm control device based on kinetic energy recovery, characterized by: The invention comprises a motor, a motor drive module, a controller, an energy storage device, a speed sensor and a charge and discharge management circuit; the motor drive module is connected to a power source and controls the rotation of the motor through a control signal sent by the controller; the energy storage device is connected to the motor drive module and receives a reverse current transmitted from the motor through the motor drive module and stores electrical energy when the motor decelerates; and / or releases the stored electrical energy to output a discharge current when the motor accelerates, and the discharge current is transmitted to the motor through the motor drive module; The energy storage device includes an auxiliary battery and a supercapacitor connected in parallel with the auxiliary battery; The supercapacitor is quickly charged in a short time and stores the electric energy in the auxiliary battery with a delay; the speed sensor detects the speed of the motor and transmits the speed signal to the controller; the charge and discharge management circuit includes a charging switch and a discharging switch; the charging switch and the discharging switch are connected in parallel between the motor drive module and the energy storage device; when the controller detects that the motor is decelerating, the charging switch is turned on and the discharging switch is turned off to charge the energy storage device; when the controller detects that the motor is accelerating, the charging switch is turned off and the discharging switch is turned on to discharge the energy storage device; when the energy storage device releases energy, the controller controls the closing time of the discharge switch by solving the following control law equation: Where T is the closing time of the discharge switch, is the first-order derivative of the energy actually released by the energy storage device, is the first-order derivative of the energy storage device’s expected release energy, is the second-order derivative of the energy actually released by the energy storage device, is the second-order derivative of the energy storage device expected to release energy, k is the parameter to be designed, and Λ is the parameter to be designed.

2. The robotic arm control device based on kinetic energy recovery according to claim 1, characterized in that: The charge and discharge management circuit also includes a rectification and voltage conversion module and an inversion and voltage conversion module; The rectifier and transformer module is connected to the motor drive module and the energy storage device, receives the multi-phase alternating current output by the motor drive module, and converts the multi-phase alternating current into two-phase direct current and outputs it to the energy storage device; the charging switch is provided on each phase line of the multi-phase alternating current output by the motor drive module to the rectifier and transformer module; The inverter and transformer module is connected to the energy storage device and the motor drive module, receives the two-phase DC power output by the energy storage device, and converts the two-phase DC power into multi-phase AC power and outputs it to the motor drive module; the discharge switch is arranged on each phase line of the inverter and transformer module outputting multi-phase AC power to the motor drive module.

3. The robotic arm control device based on kinetic energy recovery according to claim 2, characterized in that: Also includes a voltage sensor; the voltage sensor detects the voltage of the energy storage device and transmits the voltage signal to the controller; when the controller detects that the voltage of the energy storage device is greater than a set maximum threshold, the controller controls the charging switch to be disconnected; When the controller detects that the voltage of the energy storage device is less than a set minimum threshold, the controller controls the discharge switch to be disconnected.

4. The robotic arm control device based on kinetic energy recovery according to claim 3, characterized in that: The charge and discharge management circuit also includes a discharge MOSFET tube, a charge MOSFET tube, a battery charge and discharge protection board, a first resistor, a second resistor and a filter capacitor; The output terminals of the rectifier and transformer module for outputting two-phase direct current are referred to as the first output terminal and the second output terminal; the input terminals of the inverter and transformer module for inputting two-phase direct current are referred to as the first input terminal and the second input terminal; The battery charge and discharge protection board is provided with a VCC terminal, a GND terminal, a DO terminal, a CO terminal, and a VM terminal; the VCC terminal is connected to the positive electrode of the auxiliary battery; the GND terminal is grounded; the DO terminal is connected to the gate of the discharge MOSFET tube and is normally high; the CO terminal is connected to the gate of the charge MOSFET tube and is normally high; the VM terminal is connected to the second output terminal of the rectifier and transformer module through a second resistor; the VM terminal detects the voltage of the second output terminal of the rectifier and transformer module; when the voltage of the second output terminal of the rectifier and transformer module exceeds a certain threshold and lasts for a period of time, the battery charge and discharge protection board outputs a low level to the DO terminal; The positive electrode of the auxiliary battery is connected to the first output terminal of the rectifier and transformer module, and the negative electrode is grounded; the positive electrode of the auxiliary battery is connected to the VCC terminal of the battery charge and discharge protection board through a first resistor; One end of the filter capacitor is connected to the VCC terminal of the battery charge and discharge protection board, and the other end is grounded; The source of the discharge MOSFET is connected to the negative electrode of the auxiliary battery, the gate is connected to the DO terminal of the battery charge and discharge protection board, and the drain is connected to the second input terminal of the inverter and transformer module; the discharge MOSFET is provided with a body diode D1, the cathode of the body diode D1 is connected to the drain, and the anode is connected to the source; The source of the charging MOSFET is connected to the second output terminal of the rectifier and transformer module, the gate is connected to the CO terminal of the battery charge and discharge protection board, and the drain is connected to the drain of the discharge MOSFET. The charging MOSFET is provided with a body diode D2, the cathode of the body diode D2 is connected to the drain, and the anode is connected to the source. The battery charge and discharge protection board detects the voltage of the energy storage device through the VCC terminal and the GND terminal; when the battery charge and discharge protection board detects that the voltage of the energy storage device is less than the minimum threshold, the DO terminal of the battery charge and discharge protection board becomes a low level; when the battery charge and discharge protection board detects that the voltage of the energy storage device exceeds the maximum threshold, the CO terminal of the battery charge and discharge protection board becomes a low level.

5. A robot comprising at least one robotic arm, characterized in that: It also includes a robot arm control device as described in any one of claims 1-4, wherein the robot arm control device controls the movement of the robot arm.

Images