Kinetic energy recovery based robotic arm control device and robot

Abstract

The application relates to a mechanical arm control device and a robot based on kinetic energy recovery. The mechanical arm control device comprises a motor, a motor driving module, a controller and an energy storage device; the motor driving module is connected with a power supply, and the rotation of the motor is controlled through a control signal sent by the controller; the energy storage device is connected with the motor driving module, and receives reverse current conducted by the motor driving module and stores energy when the motor is decelerated; when the motor is decelerated, the rotation of the motor is controlled by solving the following control law equation: the mechanical arm control device of the application stores energy when the motor is decelerated and releases energy when the motor is accelerated through the setting of the energy storage device, so that the heating of the motor can be reduced, the operation safety of the equipment can be improved, energy can be saved, and the cost can be reduced. The application also considers the change of the stress condition of the motor after the energy storage device is added, improves the motor control algorithm, and guarantees the accuracy of the operation of the mechanical arm.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot motion control, in particular to a mechanical arm control device based on kinetic energy recovery. BACKGROUND

[0002] Multi-axis robot has multiple mechanical arms, each of which is equipped with a separate motor and motor drive module to control its movement. Please refer to Figure 1 , Figure 1 is a schematic diagram of the existing mechanical arm control device module. The existing mechanical arm control device includes a motor, a motor drive module and a controller. The motor drive module is connected with the power supply, and the rotation of the motor is controlled by the control signal sent by the controller, thereby controlling the movement of the mechanical arm.

[0003] The function of the motor drive module of the existing mechanical arm control device is to rectify, filter, invert and other processes to the current input by the power supply. For example, when the motor is a three-phase alternating current motor, but the power supply is a two-phase direct current power supply, the motor drive module at least includes an inverter unit, and the two-phase direct current output by the power supply is converted into the three-phase alternating current required by the motor and output to the motor. In addition, the motor drive module in the prior art generally has a plurality of capacitors, and the function of these capacitors is to filter and stabilize the current to prevent the short circuit protection unit from being affected by the performance of the circuit components or the short circuit protection unit caused by the excessive fluctuation of the current during the operation of the motor. SUMMARY

[0004] Therefore, the purpose of the present application is to provide a mechanical arm control device and a robot based on kinetic energy recovery.

[0005] A mechanical arm control device based on kinetic energy recovery, comprising a motor, a motor drive module, a controller and an energy storage device; the motor drive module is connected with the power supply, and the rotation of the motor is controlled by the control signal sent by the controller; the energy storage device is connected with the motor drive module, and receives the reverse current conducted by the motor through the motor drive module and stores the electric energy when the motor is decelerated;

[0006] When the motor is decelerated, the controller controls the rotation of the motor by solving the following control law equation:

[0007] Wherein, M is a positive definite inertia matrix of n*n, C is a Coriolis / centrifugal matrix, G is a gravity vector, f is a friction disturbance term, is the desired motor speed, is the desired motor acceleration, Λ is a to-be-designed parameter, e is the difference between the actual and expected rotation angles of the motor, is the difference between the actual and expected rotation angles of the motor, k is a to-be-designed term, τ is the actual motor speed. 电源 Let α be the torque generated by the motor receiving power from the power source, and let α be the constant relating the torque generated by the motor during regenerative braking, which was determined experimentally to be the relationship between the speed and the torque.

[0008] The kinetic energy recovery-based robotic arm control device of the present invention stores energy when the motor decelerates and releases energy when the motor accelerates by setting up an energy storage device. This not only reduces motor heat generation and improves equipment operation safety, but also saves energy and reduces costs. The present invention also takes into account the changes in the force on the motor after adding the energy storage device and designs a new motor control algorithm to make the robotic arm movement more precise.

[0009] Furthermore, the energy storage device releases the stored electrical energy and outputs a discharge current when the motor accelerates, and the discharge current is conducted to the motor through the motor drive module.

[0010] Furthermore, during motor acceleration, the controller controls the motor's rotation by solving the following control law equation:

[0011] Where τ 电源 τ is the torque generated by the motor receiving power from the power source. 储能装置 Let M be the torque generated by the motor receiving energy from the energy storage device, C be the Coriolis / centripetal matrix, G be the gravity vector, and f be the friction disturbance term. For the desired motor speed, Let Λ be the desired motor acceleration, Λ be the parameter to be designed, and e be the difference between the actual and desired motor rotation angle. denoted as , where is the difference between the actual and desired motor speed, and k is a design term.

[0012] Furthermore, the energy storage device includes an auxiliary battery and a supercapacitor connected in parallel with the auxiliary battery; the supercapacitor charges rapidly in a short time and stores the electrical energy in the auxiliary battery with a delay.

[0013] Furthermore, it also includes a speed sensor and a charge / discharge management circuit; the speed sensor detects the motor speed and transmits the speed signal to the controller; the charge / 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, it controls the charging switch to be turned on and the discharging switch to be turned off, so that the energy storage device is charged; when the controller detects that the motor is accelerating, the controller controls the charging switch to be turned off and the discharging switch to be turned on, so that the energy storage device is discharged.

[0014] Furthermore, the charge and discharge management circuit also includes a rectifier and transformer module and an inverter and transformer module;

[0015] The rectification and transformation module is connected with the motor driving module and the energy storage device, receives the multi-phase alternating current output by the motor driving module, and converts the multi-phase alternating current into two-phase direct current and outputs the two-phase direct current to the energy storage device; the charging switch is arranged on each phase line of the motor driving module outputting the multi-phase alternating current to the rectification and transformation module;

[0016] The inversion and transformation module is connected with the energy storage device and the motor driving module, receives the two-phase direct current output by the energy storage device, and converts the two-phase direct current into multi-phase alternating current and outputs the multi-phase alternating current to the motor driving module; the discharging switch is arranged on each phase line of the inversion and transformation module outputting the multi-phase alternating current to the motor driving module.

[0017] Further, a voltage sensor is further included; the voltage sensor detects the voltage of the energy storage device and transmits a voltage signal to the controller; when the controller detects that the voltage of the energy storage device is greater than a set maximum threshold value, the controller controls the charging switch to be turned off; when the controller detects that the voltage of the energy storage device is less than a set minimum threshold value, the controller controls the discharging switch to be turned off.

[0018] Further, the charging and discharging management circuit further includes a discharging MOSFET tube, a charging MOSFET tube, a battery charging and discharging protection board, a first resistor, a second resistor and a filter capacitor;

[0019] The output end of the rectification and transformation module outputting two-phase direct current is recorded as a first output end and a second output end; and the input end of the inversion and transformation module inputting two-phase direct current is recorded as a first input end and a second input end;

[0020] The battery charging and discharging protection board is provided with a VCC end, a GND end, a DO end, a CO end and a VM end; the VCC end is connected with the positive electrode of the auxiliary battery; the GND end is grounded; the DO end is connected with the gate of the discharging MOSFET tube and is normally at a high level; the CO end is connected with the gate of the charging MOSFET tube and is normally at a high level; the VM end is connected with the second output end of the rectification and transformation module through the second resistor; the VM end detects the voltage of the second output end of the rectification and transformation module; when the voltage of the second output end of the rectification and transformation module exceeds a certain threshold value and lasts for a period of time, the battery charging and discharging protection board outputs a low level to the DO end;

[0021] The positive electrode of the auxiliary battery is connected with the first output end of the rectification and transformation module, and the negative electrode is grounded; the positive electrode of the auxiliary battery is connected with the VCC end of the battery charging and discharging protection board through the first resistor; one end of the filter capacitor is connected with the VCC end of the battery charging and discharging protection board, and the other end is grounded;

[0022] The source of the discharging MOSFET tube is connected with the negative pole of the auxiliary battery, the gate is connected with the DO end of the battery charging and discharging protection plate, and the drain is connected with the second input end of the inverter and voltage conversion module; the discharging MOSFET tube is provided with a body diode D1, the cathode of the body diode D1 is connected with the drain, and the anode is connected with the source;

[0023] The source of the charging MOSFET tube is connected with the second output end of the rectifier and voltage conversion module, the gate is connected with the CO end of the battery charging and discharging protection plate, and the drain is connected with the drain of the discharging MOSFET tube; the charging MOSFET tube is provided with a body diode D2, the cathode of the body diode D2 is connected with the drain, and the anode is connected with the source;

[0024] The battery charging and discharging protection plate detects the voltage of the energy storage device through the VCC end and the GND end; when the battery charging and discharging protection plate detects that the voltage of the energy storage device is less than the minimum threshold value, the DO end of the battery charging and discharging protection plate becomes low; when the battery charging and discharging protection plate detects that the voltage of the energy storage device exceeds the maximum threshold value, the CO end of the battery charging and discharging protection plate becomes low.

[0025] Further, when the energy storage device releases energy, the controller controls the closing time of the discharging switch by solving the following control law equation:

[0026]

[0027] Wherein, T is the closing time of the discharging switch, is the first derivative of the actual energy release of the energy storage device, is the first derivative of the expected energy release of the energy storage device, is the second derivative of the actual energy release of the energy storage device, is the second derivative of the expected energy release of the energy storage device, k is a to-be-designed parameter, and Λ is a to-be-designed parameter.

[0028] The application also provides a robot, which comprises at least one mechanical arm and further comprises any of the mechanical arm control devices.

[0029] In order to better understand and implement, the application is described in detail below with reference to the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0030] Figure 1 It is a schematic diagram of an existing mechanical arm control device module;

[0031] Figure 2 It is a schematic diagram of a mechanical arm control device module based on kinetic energy recovery in embodiment 1 of the application;

[0032] Figure 3 A mechanical arm control device module schematic diagram based on kinetic energy recovery for the embodiment 2 of the present application;

[0033] Figure 4 A mechanical arm control device module schematic diagram based on kinetic energy recovery for the embodiment 3 of the present application;

[0034] Figure 5 A schematic diagram of the structure of the energy storage device and the charge and discharge management circuit for the embodiment 3 of the present application. DETAILED DESCRIPTION

[0035] In order to make the purpose, technical solutions and advantages of the present application clearer, the following will further describe the embodiments of the present application in combination with the drawings.

[0036] The inventor found that some sensitive components on the robot are damaged irregularly when using the existing multi-axis robot, especially when the mechanical arm of the multi-axis robot is frequently controlled to change speed. After analysis and research, it is found that the frequent speed change of the corresponding motor of the multi-axis robot mechanical arm is actually realized by the frequent speed change of the motor, and the frequent speed change of the motor will cause a more serious heating problem. The heat conduction of the motor heating makes the temperature of the robot too high, and then damages some temperature-sensitive components.

[0037] The inventor further researched the root cause of the heating problem caused by the speed change of the motor, and found that the heating mainly occurs in the deceleration stage of the motor. At present, a three-phase alternating current motor is usually used to control the robot mechanical arm, which includes a stator and a rotor arranged on the shaft of the stator. The stator winding is supplied with three-phase alternating current; by controlling the size and frequency of the three-phase alternating current, a rotating magnetic field can be generated; when the rotating magnetic field generated by the stator winding rotates, the rotor winding cuts the magnetic induction lines to generate an induced current, and the magnetic field generated by the induced current interacts with the rotating magnetic field to make the rotor rotate relative to the stator. When the motor needs to decelerate, the controller adjusts the size and frequency of the three-phase alternating current of the stator winding through the motor drive module to reduce the speed of the stator rotating magnetic field. At this time, due to inertia, the rotor still maintains a high speed. The rotor still maintaining a high speed rotates in the opposite direction relative to the stator rotating magnetic field with a reduced speed, thereby generating a reverse electromotive force to reduce the speed of the rotor; at the same time, the reverse electromotive force will also generate a part of the reverse current on the stator winding to cause the motor to heat.

[0038] The present application is found that the part of the reverse current generated on the stator winding of the motor during the deceleration stage can be recycled and reused, which can solve the heat problem of internal friction and further improve the energy utilization efficiency. Based on the above problem solving concept, the present application adds an energy storage device to the existing multi-axis robot control device, which converts mechanical energy into electrical energy during motor deceleration and releases stored electrical energy as motor assistance during motor acceleration. Considering the new factors affecting the stability of motor rotation after adding the energy storage device, the present application further optimizes the control method of the mechanical arm motor after adding the energy storage device.

[0039] Example 1

[0040] Please refer to Figure 2 , Figure 2 The mechanical arm control device module based on kinetic energy recovery of embodiment 1 of the present application is shown. The mechanical arm control device based on kinetic energy recovery of embodiment 1 of the present application includes a motor 10, a motor drive module 20, a controller 30 and an energy storage device 40.

[0041] The motor drive module 20 is connected to the power supply, and the rotation of the motor 10 is controlled by the control signal sent by the controller 30, thereby controlling the movement of the robot mechanical arm A.

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

[0043] Specifically, the energy storage device 40 includes an auxiliary battery and a super capacitor connected in parallel with the auxiliary battery. The super capacitor is rapidly charged in a short time of motor deceleration and stores the electric energy into the auxiliary battery. The auxiliary battery can also complete charging and discharging, but the charging speed of the auxiliary battery is relatively slow, and it is impossible to complete charging in such a short time. Therefore, the super capacitor connected in parallel with the auxiliary battery is added. The super capacitor can be rapidly charged in a short time of motor deceleration, and then the electric energy is stored into the auxiliary battery. In this way, even if the time of motor deceleration is very short, the auxiliary battery can effectively complete charging. Since the auxiliary battery and the super capacitor are connected in parallel to form the 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 super capacitor.

[0044] The principle that the energy storage device 40 is charged when the motor 10 is decelerated and discharged when the motor 10 is accelerated is as follows: when the controller 30 controls the motor 10 to decelerate, the controller 30 reduces the rotating speed of the stator rotating magnetic field by controlling the size and frequency of the current in the motor stator winding. At this time, the motor rotor still maintains a high rotating speed due to inertia, reversely rotates and reversely cuts the magnetic induction lines relative to the rotating magnetic field with reduced rotating speed, thereby generating a reverse electromotive force, so that the voltage at the end of the motor 10 is higher than the voltage at the end of the energy storage device 40. Since the voltage at the end of the motor 10 is higher and the voltage at the end of the energy storage device 40 is lower, the reverse current at the end of the motor 10 flows into the energy storage device 40 through the motor drive module 20 to charge the energy storage device 40. When the controller 30 controls the motor to accelerate, the motor 10 becomes a load consuming electric energy, and the reverse electromotive force disappears, so that the voltage is relatively low. However, the energy storage device 40 stores electric energy, and the voltage is relatively high. Therefore, the energy storage device 40 spontaneously releases electric energy to form a discharge current, and the discharge current output by the energy storage device 40 is conducted to the motor 10 through the motor drive module 20 to assist the motor 10.

[0045] Further, considering that the motor rotor reversely cuts the magnetic induction lines during the motor deceleration stage, the magnetic field exerts a feedback force on the motor rotor to further decelerate the motor rotor; during the motor acceleration stage, the energy storage device 40 releases the stored electric energy to generate a discharge current, which increases the current size in the motor stator winding, and the motor rotor is subjected to assistance to further accelerate the motor rotor. If the feedback force and the assistance are not considered, and the old control algorithm is still used, the controller 30 will not be able to accurately control the motor 10, and the mechanical arm A will not be able to accurately move according to the preset trajectory, which affects the stability of the control. Therefore, the controller 30 also improves the control method of the motor 10 of the mechanical arm, and considers the feedback force during the deceleration stage and the assistance during the acceleration stage, which are divided into two aspects of the deceleration stage and the acceleration stage.

[0046] (I) Control algorithm during the deceleration stage

[0047] Let the voltage of the energy storage device after charging is U, and the capacity of the energy storage device is C, then the energy stored in the energy storage device during charging is E = 0.5CU 2 .

[0048] Let the dynamics model of the motor in the deceleration stage be: where M(q) is an n*n positive definite inertia matrix, is the Coriolis / centrifugal matrix, G(q) is the gravity vector, f is the friction disturbance term, n is the rotational speed, τ 电源 is the torque generated by the power supply to the motor, τ 回馈 is the braking torque when feedback braking, and where alpha is a constant related to the characteristics of the motor, used to describe the relationship between the torque generated by the motor during feedback braking and the rotational speed. This feedback coefficient can be determined through experiments or the characteristic curve of the motor; represents the rotational speed of the motor.

[0049] Based on the above dynamics model, a sliding mode algorithm is used to design the motor control algorithm:

[0050] Define the sliding film function s as:

[0051] e = q-q d , where q is the actual rotational angle of the motor, q d is the desired rotational angle of the motor, e is the difference between the actual and desired rotational angles of the motor, is the difference between the actual and desired rotational speeds of the motor, and Lambda is a parameter to be designed.

[0052] The Lyapunov function is selected as: where M is the n*n positive definite inertia matrix in the dynamics model.

[0053] Its derivative is:

[0054]

[0055] where is the derivative of M.

[0056] From to q, skew symmetry can be known:

[0057]

[0058] Solving (1.2) and (1.1) together, we have

[0059]

[0060] Solving (1.3) and the dynamics equation, we get:

[0061]

[0062] In order to make The control law is selected as:

[0063]

[0064] Wherein, M is the n*n positive definite inertia matrix in the dynamic model, C is the Coriolis / centrifugal matrix, G is the gravity vector, f is the friction disturbance term, is the desired motor speed, is the desired motor acceleration, Λ is the parameter to be designed, e is the difference between the actual and desired rotation angle of the motor, is the difference between the actual and desired rotation speed of the motor, k is the parameter to be designed, is the actual motor speed, τ 电源 is the torque generated by the motor receiving power.

[0065] The controller solves the control law equation and substitutes the actual motor related parameters to calculate the corresponding voltage control signal, and controls the motor through the motor drive module in the deceleration stage.

[0066] (II) Control algorithm in the acceleration stage

[0067] Let the dynamic model of the motor in the acceleration stage be:

[0068]

[0069]

[0070] Wherein, is the power generated by the motor receiving the energy storage device, τ 电源 is the torque generated by the motor receiving power, n is the motor speed.

[0071] The control law is designed as: Wherein τ 电源 is the torque generated by the motor receiving power, τ 储能装置 is the torque generated by the motor receiving the energy storage device.

[0072] The controller solves the control law equation and substitutes the actual motor related parameters to calculate the corresponding voltage control signal, and controls the motor through the motor drive module in the acceleration stage.

[0073] Example 2

[0074] Please refer toFigure 3 , Figure 3 Figure 1 is a schematic diagram of a mechanical arm control device module based on kinetic energy recovery according to an embodiment of the present application. The mechanical arm control device based on kinetic energy recovery according to the embodiment of the present application comprises a motor 10, a motor drive module 20, a controller 30, an energy storage device 40, a rotational speed sensor 50, a charge-discharge management circuit 60 and a voltage sensor 70.

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

[0076] The energy storage device 40 is connected to the motor drive module 20 through the charge-discharge management circuit 60. When the motor 10 is decelerating, the energy storage device 40 receives the reverse current conducted by the motor drive module 20 and the charge-discharge management circuit 60 from the motor 10 and stores the electrical energy; or when the motor 10 is accelerating, the energy storage device 40 releases the stored electrical energy to output a discharge current, and the output discharge current is conducted to the motor 10 through the charge-discharge management circuit 60 and the motor drive module 20 to provide power assistance for the acceleration of the motor 10. Specifically, the energy storage device 40 comprises an auxiliary battery and a super capacitor connected in parallel with the auxiliary battery; the super capacitor is rapidly charged in a short time and stores the electrical energy in the auxiliary battery.

[0077] The rotational speed sensor 50 detects the rotational speed of the motor 10 and transmits the rotational speed signal to the controller 30.

[0078] The charge-discharge management circuit 60 comprises 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.

[0079] 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 a set maximum threshold, the controller controls the charging switch 62 to open, so that the energy storage device 40 stops charging. The overcharge protection is set to ensure that the auxiliary battery will not be overcharged, prolong the battery life, and ensure safety. When the controller 30 detects that the voltage of the energy storage device 40 is less than a set minimum threshold, the controller 30 controls the discharging switch 64 to open, so that the energy storage device 40 stops discharging. The over-discharge protection is set to prevent the auxiliary battery from over-discharging, which can damage the battery life, and because when the auxiliary battery voltage is not high enough, its effect of releasing energy to assist the motor acceleration is limited.

[0080] Further, to ensure that the mechanical arm A moves smoothly and the speed of the motor 10 changes uniformly, the energy stored in the energy storage device 40 should not be released all at once, but should be released smoothly multiple times. Therefore, the controller 30 adjusts the release speed of the energy of the energy storage device 40 by controlling the closing time of the discharging switch 64. To ensure that the energy output by the energy storage device 40 meets the expected output energy size, so as to ensure the accuracy of the operation of the mechanical arm A, a sliding mode algorithm is used to control it.

[0081] Let the function of the discharging switch closing time T and the energy E released by the energy storage device be:

[0082] E = Z(T), where T is the discharging switch closing time.

[0083] Define the sliding mode function as:

[0084] e = E - E d , where E is the actual energy released by the energy storage device, E d is the expected energy released by the energy storage device, e is the difference between the actual and expected energy released, and Λ is a parameter to be designed.

[0085] Select the Lyapunov function as:

[0086] Its derivative is:

[0087]

[0088] So that

[0089]

[0090] Because is the inverse function of , the control law can be written as:

[0091]

[0092] wherein T is the discharging switch closing time, is the first derivative of the actual energy release of the energy storage device, is the first derivative of the expected energy release of the energy storage device, is the second derivative of the actual energy release of the energy storage device, is the second derivative of the expected energy release of the energy storage device, k is a parameter to be designed, and Lambda is a parameter to be designed. The controller solves the control law equation, so as to control the discharging switch closing time T, and further control the energy release speed of the energy storage device.

[0093] Example 3

[0094] The structure of the mechanical arm control device based on kinetic energy recovery in the embodiment 3 of the application is basically the same as that of the mechanical arm control device based on kinetic energy recovery in the embodiment 2, and the difference lies in the specific circuit structure design of the charge and discharge management circuit. Since the battery charge and discharge protection plate of the charge and discharge management circuit 60 has a voltage detection function, the mechanical arm control device based on kinetic energy recovery in the embodiment 3 of the application does not include a voltage sensor. Please refer to Figure 4 and Figure 5 , Figure 4 is a module schematic diagram of the mechanical arm control device based on kinetic energy recovery in the embodiment 3 of the application, Figure 5 is a structure schematic diagram of the energy storage device and the charge and discharge management circuit in the embodiment 3 of the application. The mechanical arm control device based on kinetic energy recovery in the embodiment 3 of the application comprises a motor 10, a motor drive module 20, a controller 30, an energy storage device 40, a rotating speed sensor 50 and a charge and discharge management circuit 60.

[0095] The charge and discharge management circuit comprises a charging switch, a discharging switch, a rectification and voltage conversion module, an inversion and voltage conversion module, a discharging MOSFET tube MOS1, a charging MOSFET tube MOS2, a battery charge and discharge protection plate, a first resistor R1, a second resistor R2 and a filter capacitor C2.

[0096] The rectification and transformation module is connected with the motor driving module and the energy storage device, receives the multi-phase alternating current output by the motor driving module, and converts the multi-phase alternating current into two-phase direct current to output to the energy storage device; the charging switch is arranged on each phase line of the motor driving module outputting the multi-phase alternating current to the rectification and transformation module. The inversion and transformation module is connected with the energy storage device and the motor driving module, receives the two-phase direct current output by the energy storage device, and converts the two-phase direct current into multi-phase alternating current to output to the motor driving module; the discharging switch is arranged on each phase line of the motor driving module outputting the multi-phase alternating current by the inversion and transformation module. The discharging MOSFET tube MOS1, the charging MOSFET tube MOS2, the battery charging and discharging protection plate, the first resistor R1, the second resistor R2 and the filter capacitor C2 form a charging and discharging protection unit, which prevents overcharging and overdischarging.

[0097] In the embodiment, the number of the motor driving modules is two, and in other embodiments, the number of the motor driving modules can be other. In the embodiment, each motor driving module includes three phase lines, which are referred to as a first phase line, a second phase line and a third phase line, and in other embodiments, the number of the phase lines of the motor driving module can be other.

[0098] Specifically, the three-phase alternating current input end of the rectification and transformation module is referred to as a U end, a V end and a W end; and the output end of the rectification and transformation module outputting two-phase direct current is referred to as a first output end and a second output end. The U end is connected with the first phase lines of a plurality of motor driving modules through charging switches, and the first phase lines of different motor driving modules are connected in parallel with each other. Similarly, the V end is connected with the second phase lines of a plurality of motor driving modules through charging switches, and the second phase lines of different motor driving modules are connected in parallel with each other; and the W end is connected with the third phase lines of a plurality of motor driving modules through charging switches, and the third phase lines of different motor driving modules are connected in parallel with each other. In the embodiment, the U end is connected with the first phase lines of the first motor driving module and the second motor driving module through charging switches S1 and S7 respectively; the V end is connected with the second phase lines of the first motor driving module and the second motor driving module through charging switches S2 and S8 respectively; and the W end is connected with the third phase lines of the first motor driving module and the second motor driving module through charging switches S3 and S9 respectively. The first output end is connected with the positive electrode of the auxiliary battery, and the second output end is connected with the source (s pole) of the charging MOSFET tube MOS2.

[0099] Specifically, the battery charge and discharge protection plate is provided with a VCC end, a GND end, a VM end, a DO end and a CO end. The VCC end is connected to the positive electrode of the auxiliary battery, serving as a working voltage input end; the auxiliary battery provides working voltage for the battery charge and discharge protection plate. The GND end is grounded. The DO end is connected to the gate (g end) of the discharge MOSFET tube MOS1, and is normally high level. The CO end is connected to the gate (g end) of the charge MOSFET tube MOS2, and is normally high level. The VM end is connected to the second output end of the rectification and voltage conversion module through a second resistor R2, and detects the voltage of the second output end of the rectification and voltage conversion module; when the voltage of the second output end of the rectification and voltage conversion module exceeds a certain threshold value and lasts for a period of time, the battery charge and discharge protection plate outputs low level to the DO end. The second resistor R2 functions as a voltage pull-down resistor, so that the VM end is normally low level.

[0100] Specifically, the positive electrode of the auxiliary battery is connected to the first output end of the rectification and voltage conversion module, and the negative electrode is grounded. The positive electrode of the auxiliary battery is connected to the VCC end of the battery charge and discharge protection plate through a first resistor R1. One end of the super capacitor 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 end of the battery charge and discharge protection plate, and the other end is grounded. The first resistor R1 is a reference power supply resistor, which, together with the internal resistor of the battery charge and discharge protection plate, constitutes a voltage dividing circuit, and controls the reference voltage of the voltage comparator inside the battery charge and discharge protection plate. The filter capacitor C2 functions as a voltage stabilizing and filtering device.

[0101] Specifically, the source (s end) of the charge MOSFET tube MOS2 is connected to the second output end of the rectification and voltage conversion module, the gate (g end) is connected to the CO end of the battery charge and discharge protection plate, and the drain (d end) is connected to the drain (d end) of the discharge MOSFET tube MOS1. The charge MOSFET tube MOS2 is provided with a body diode D2, the cathode of which is connected to the drain (d end), and the anode of which is connected to the source (s end).

[0102] Specifically, the source (s end) of the charge MOSFET tube MOS2 is connected to the second output end of the rectification and voltage conversion module, the gate (g end) is connected to the CO end of the battery charge and discharge protection plate, and the drain (d end) is connected to the drain (d end) of the discharge MOSFET tube MOS1. The charge MOSFET tube MOS2 is provided with a body diode D2, the cathode of which is connected to the drain (d end), and the anode of which is connected to the source (s end).

[0103] Specifically, the inverter and transformer module converts two-phase direct current output by the energy storage device into three-phase alternating current output to the motor drive module, and the discharge current output by the motor drive module supplies power to the motor. The two-phase direct current input end of the inverter and transformer module is the first input end and the second input end, and the three-phase alternating current output end is the U output end, the V output end and the W output end. The first input end is connected to the positive electrode of the auxiliary battery, and the second input end is connected to the drain (d terminal) of the discharge MOSFET tube MOS1. The U output end is connected to the first phase line of a plurality of motor drive modules through a discharge switch, the V output end is connected to the second phase line of a plurality of motor drive modules through a discharge switch, and the W output end is connected to the third phase line of a plurality of 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 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 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 discharge switches S6 and S12, respectively; and the charging switch and the discharge switch on each phase line are connected in parallel to each other.

[0104] In this embodiment, 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 value, the DO terminal of the battery charge and discharge protection board becomes low; when the battery charge and discharge protection board detects that the voltage of the energy storage device exceeds the maximum threshold value, the CO terminal of the battery charge and discharge protection board becomes low.

[0105] The following is based on Figure 5The circuit structure is shown, and the flow of the controller controlling the charge and discharge management circuit is described in detail. When the controller detects that the motor is decelerating through the speed sensor, the controller controls the motor to close the corresponding charging switch and open the discharge switch. The rectification and transformation module converts the charging current from three-phase alternating current to two-phase direct current, and a voltage exists between the first output end and the second output end. Since the DO output end and the CO output end of the motor charge and discharge protection board are high in the normal state, the d-s conduction channel of the discharge MOSFET tube MOS1 and the charging MOSFET tube MOS2 is turned on in the normal state. At this time, the positive electrode of the auxiliary battery is connected to the first output end, and the negative electrode is connected to the second output end through the conduction channel of the discharge MOSFET tube MOS1 and the charging MOSFET tube MOS2. The charging circuit "charging switch→rectification and transformation module→charging MOSFET tube MOS2→discharge MOSFET tube MOS1→energy storage device" is turned on, and the first output end and the second output end charge the auxiliary battery. Since the property of the MOSFET tube is that the current at the d-s end is constant when the voltage at the g-s end is constant, the size of the charging current is always limited regardless of the voltage between the first output end and the second output end, which can effectively protect the device.

[0106] When the controller detects that the motor is accelerating, the controller controls the motor to close the discharge switch and open the charging switch. Since the positive electrode of the auxiliary battery is connected to the first input end of the inverter and transformation module, and the negative electrode is connected to the second input end of the inverter and transformation module through the discharge MOSFET tube MOS1, and the DO output end of the motor charge and discharge protection board is high in the normal state, i.e., the conduction channel of the discharge MOSFET tube MOS1 is turned on in the normal state, the discharge circuit "energy storage device→discharge MOSFET tube MOS1→inverter and transformation module→discharge switch" is turned on, and the auxiliary battery outputs two-phase direct current to the inverter and transformation module. After the inverter and transformation module converts the two-phase direct current output by the auxiliary battery into three-phase alternating current, the motor is powered through the motor drive module.

[0107] When the battery charge and discharge protection board detects that the voltage of the energy storage device exceeds the maximum threshold value through the VCC end and the GND end, the CO end of the battery charge and discharge protection board becomes low, the charging MOSFET tube MOS2 is disconnected, the charging circuit is disconnected, and the energy storage device stops charging. Since the discharge circuit is not related to the charging MOSFET tube MOS2, even if the charging MOSFET tube MOS2 is disconnected, it will not affect the discharge. The overcharge protection is set to ensure that the auxiliary battery will not be overcharged, prolong the life of the battery, and ensure safety.

[0108] When the battery charging and discharging protection plate detects that the voltage of the energy storage device is lower than the minimum threshold value through the VCC terminal and the GND terminal, the DO terminal of the battery charging and discharging protection plate becomes low, the discharge MOSFET tube MOS1 is disconnected, the discharge circuit is disconnected, and the energy storage device stops discharging. At this time, if the motor exists deceleration, the charging switch corresponding to the motor is closed, and since the discharge MOSFET tube MOS1 has a body diode D1, even if the conduction channel of the discharge MOSFET tube MOS1 is disconnected, the charging current can form a charging circuit through the body diode D1. Therefore, the disconnection of the discharge MOSFET tube MOS1 only stops discharging, and does not affect charging. The reason for setting the over-discharge protection is that on the one hand, over-discharge of the auxiliary battery will damage 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 to accelerate is limited.

[0109] The mechanical arm control device based on kinetic energy recovery of the application can not only reduce the heating of the motor, improve the safety of the operation of the equipment, but also save energy and reduce costs by storing energy when the motor is decelerating and releasing energy when the motor is accelerating. The application also considers the changes in the force on the motor after the energy storage device is added, improves the motor control algorithm, and ensures the accuracy of the operation of the mechanical arm. The application further controls the charging and discharging of the energy storage device by setting the charging and discharging management circuit, the speed sensor, the voltage sensor and the control algorithm, and ensures the safety of charging and the stability of discharging.

[0110] The above-mentioned embodiments only express several embodiments of the application, and the description is more specific and detailed, but it cannot be understood as a limitation on the scope of the patent. It should be noted that for ordinary skilled persons in the art, without departing from the concept of the application, a number of modifications and improvements can be made, which are within the scope of protection of the application.

Claims

1. A robotic arm control device based on kinetic energy recovery, characterized in that: It includes a motor, a motor drive module, a controller, an energy storage device, a speed sensor, and a charge / discharge management circuit; the motor drive module is connected to a power supply and controls the rotation of the motor through control signals issued by the controller; the energy storage device is connected to the motor drive module and receives and stores electrical energy from the reverse current conducted by the motor through the motor drive module when the motor decelerates. When the motor decelerates, the controller controls the motor's rotation by solving the following control law equation: ,in, It is an n*n positive definite inertia matrix. It is a Coriolis / centripetal matrix. It is a gravity vector. For friction interference, For the desired motor speed, For the desired motor acceleration, For the parameters to be designed, This is the difference between the actual and desired rotation angle of the motor. This is the difference between the actual speed and the desired speed of the motor. Let s be the item to be designed, and s be the sliding membrane function. This is the actual motor speed. The torque generated by the motor receiving power from the electrical source. This is the constant relating torque and speed during regenerative braking of the motor, determined experimentally. The energy storage device includes an auxiliary battery and a supercapacitor connected in parallel with the auxiliary battery; the supercapacitor charges rapidly in a short time and stores the electrical energy in the auxiliary battery with a delay. The speed sensor detects the motor speed 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, it controls the charging switch to be turned on and the discharging switch to be turned off, so that the energy storage device is charged; when the controller detects that the motor is accelerating, the controller controls the charging switch to be turned off and the discharging switch to be turned on, so that the energy storage device is discharged. 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. This is the first derivative of the actual energy released by the energy storage device. This is the first derivative of the energy that the energy storage device expects to release. This is the second derivative of the energy actually released by the energy storage device. This is the second derivative of the energy that the energy storage device is expected to release.

2. The robotic arm control device based on kinetic energy recovery according to claim 1, characterized in that: The energy storage device releases the stored electrical energy and outputs a discharge current when the motor accelerates. The discharge current is conducted to the motor through the motor drive module.

3. The robotic arm control device based on kinetic energy recovery according to claim 2, characterized in that: When the motor accelerates, the controller controls the motor's rotation by solving the following control law equation: ,in The torque generated by the motor receiving power from the electrical source. The torque generated by the motor receiving energy from the energy storage device.

4. 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 rectifier and transformer module and an inverter and transformer module; The rectifier and transformer module is connected to the motor drive module and the energy storage device, receives the multiphase AC power output by the motor drive module, and converts the multiphase AC power into two-phase DC power to be output to the energy storage device; the charging switch is set on each phase line of the multiphase AC power output 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 from the energy storage device, and converts the two-phase DC power into multi-phase AC power output to the motor drive module; the discharge switch is set on each phase line of the multi-phase AC power output from the inverter and transformer module to the motor drive module.

5. The robotic arm control device based on kinetic energy recovery according to claim 4, characterized in that: 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 a set maximum threshold, the controller controls the charging switch to turn off; When the controller detects that the voltage of the energy storage device is less than the set minimum threshold, the controller controls the discharge switch to open.

6. The robotic arm control device based on kinetic energy recovery according to claim 4, characterized in that: The charge and discharge management circuit also includes a discharge MOSFET, a charge MOSFET, 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 that output two-phase DC power are designated as the first output terminal and the second output terminal; the input terminals of the inverter and transformer module that input two-phase DC power are designated as the first input terminal and the second input terminal. The battery charge / discharge protection board has a VCC terminal, a GND terminal, a DO terminal, a CO terminal, and a VM terminal. The VCC terminal is connected to the positive terminal of the auxiliary battery. The GND terminal is grounded. The DO terminal is connected to the gate of the discharge MOSFET and is normally at a high level. The CO terminal is connected to the gate of the charge MOSFET and is normally at a high level. 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 remains so for a period of time, the battery charge / discharge protection board outputs a low level to the DO terminal. The positive terminal of the auxiliary battery is connected to the first output terminal of the rectifier and transformer module, and the negative terminal is grounded; the positive terminal 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 terminal 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 / discharge protection board detects the voltage of the energy storage device through the VCC and GND terminals. When the battery charge / 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 / discharge protection board becomes low. When the battery charge / discharge protection board detects that the voltage of the energy storage device exceeds the maximum threshold, the CO terminal of the battery charge / discharge protection board becomes low.

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

Images