ROBOT JOINT TORQUE CONTROL SYSTEM AND LOAD COMPENSATION METHOD THEREFOR

Abstract

A robot joint torque control system and a load compensation method therefor are provided, which relate to the technical field of robot joint motion control. A mathematical model of the robot joint torque control system is established first. Equivalent transformation is performed on a system functional block diagram thereof, and then it can be seen that load parameters have a great influence on joint torque output. A load compensation controller is designed to effectively eliminate the influence of the load parameters on an output torque of the joint. The system is equivalent to an inertial element on the basis of the compensation, and then a PD controller parameter is adjusted to increase an open-loop gain of the system, so as to increase a system bandwidth and increase a response speed of the joint torque control system, thereby improving performance of the joint torque control system.

Claims

1. A load compensation method for a robot joint torque control system, comprising accumulating a current i.sub.f whose joint-end velocity is compensated for by a load compensation controller and an original current loop input instruction i.sub.r, and then using the accumulated current as a current loop input instruction of a joint torque control system to effectively compensate for an influence of load parameters on an output torque of the joint torque control system; wherein the load compensation controller is $G_f\left(s\right)=\frac{{\mathrm{nK}}_v}{K_a},$ wherein n is a reduction ratio of a gear reducer, K.sub.v is a velocity feedback coefficient of a servo motor, and K.sub.a is a current loop control parameter; an open-loop transfer function of the joint torque control system is: $G\left(s\right)=\frac{T_p}{E_T}=\frac{{\mathrm{nK}}_t{K}_a{K}_{\mathrm{PD}}}{\mathrm{Ls}+R+K_a{K}_t}=\frac{\frac{{\mathrm{nK}}_t{K}_a{K}_{\mathrm{PD}}}{R+K_a{K}_t}}{\frac{s}{\omega }+1}=\frac{K_a}{\frac{s}{\omega }+1}$ wherein a corner frequency of an inertial element: $\omega =\frac{R+K_a{K}_t}{L},$ an open-loop gain $K_o=\frac{K_{\mathrm{PD}}{\mathrm{nK}}_t{K}_a}{R+K_a{K}_t},$ K.sub.PD is a PD controller parameter, K.sub.t is a torque coefficient of the servo motor, L is an inductance of the servo motor, and R is a resistance of the servo motor.

2. (canceled)

3. (canceled)

4. The load compensation method for the robot joint torque control system according to claim 1, wherein when the PD controller parameter K.sub.PD is increased, the open-loop gain K.sub.o of the system increases accordingly, and a cutoff frequency when the joint torque control system passes through a 0 db line increases; that is, a system bandwidth increases, thereby improving rapidity of the system.

5. A joint torque control system according to claim 1, comprising a joint body and a control system, wherein the joint body comprises a servo motor, a gear reducer, a torque sensor, and an absolute encoder, wherein an output shaft of the servo motor is rigidly and fixedly connected to the gear reducer, the gear reducer is rigidly connected to a joint-end load by means of the torque sensor, and the absolute encoder is mounted at the joint-end load; the control system comprises a first comparator, a torque loop controller, a second comparator, and a current loop controller, wherein the first comparator, the torque loop controller, the second comparator, and the current loop controller are sequentially in signal connection.

6. The joint torque control system according to claim 5, wherein the torque sensor is used for detecting an actual torque of a joint end; a current sensor is mounted inside the servo motor and used for detecting an actual output current of the servo motor; and the absolute encoder is used for detecting an actual position of the joint-end load.

7. The joint torque control system according to claim 6, wherein in the control system, a given current and the actual output current of the servo motor are compared and subtracted, and pass through the second comparator to control current output of the servo motor and form an internal current loop; a given torque and the actual torque of the joint end are compared and subtracted, and then pass through the first comparator and the internal current loop to control joint torque output and form an external torque loop.

8. (canceled)

9. (canceled)

10. The joint torque control system according to claim 5, wherein when the PD controller parameter K.sub.PD is increased, the open-loop gain K.sub.o of the system increases accordingly, and a cutoff frequency when the joint torque control system passes through a 0 db line increases; that is, a system bandwidth increases, thereby improving rapidity of the system.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0017] FIG. 1 is a diagram of a robot joint torque control system in the present disclosure.

[0018] FIG. 2 is a functional block diagram of the robot joint torque control system in the present disclosure.

[0019] FIG. 3 is a block diagram of the joint torque control system in the present disclosure after primary transformation.

[0020] FIG. 4 is a block diagram of the joint torque control system in the present disclosure after secondary transformation.

[0021] FIG. 5 is a block diagram of the joint torque control system in the present disclosure with a load compensation controller added thereto.

[0022] FIG. 6 is a functional block diagram of the joint torque control system in the present disclosure after equivalent transformation of FIG. 5.

[0023] FIG. 7 is an open-loop Bode diagram of the joint torque control system in the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0024] The present disclosure is further illustrated below with reference to the accompanying drawings and specific embodiments, but the protection scope of the present disclosure is not limited thereto.

[0025] As shown in FIG. 1, a robot joint torque control system is formed by a joint body and a control system. The joint body is formed by a servo motor, a gear reducer, a torque sensor, and an absolute encoder, an output shaft of the servo motor is rigidly and fixedly connected to the gear reducer, the gear reducer is rigidly connected to a joint-end load by means of the torque sensor, and the torque sensor is used for detecting an actual torque of a joint end; a current sensor is mounted inside the servo motor and used for detecting an actual output current of the servo motor; and the absolute encoder is mounted at the joint-end load and used for detecting an actual position of the joint-end load. The control system is formed by a first comparator, a torque loop controller, a second comparator, and a loop current controller. A given current and the actual output current of the servo motor are compared and subtracted, and then pass through the second comparator to control current output of the servo motor and form an internal current loop; a given torque and the actual torque of the joint end are compared and subtracted, and then pass through the first comparator and the internal current loop to control joint torque output and form an external torque loop.

[0026] According to a functional diagram of the robot joint torque control system, mathematical modeling on the robot torque control system is as follows:

[0027] The servo motor uses a current closed loop, and has an output voltage U.sub.c being:

U.sub.c=K.sub.a(i.sub.r−K.sub.Ii.sub.a)  (1)

[0028] where K.sub.a is a current loop control parameter, i.sub.r is a current loop input instruction, K.sub.I is a circuit loop feedback coefficient of the servo motor, and i.sub.a is an actual current value; the current loop input instruction i.sub.r is obtained by multiplying a joint torque error E.sub.T and a PD controller parameter K.sub.PD, and the joint torque error E.sub.T is obtained by subtracting a given torque T.sub.r of the joint and an actual torque T.sub.p of the joint end.

[0029] A voltage balance equation of the servo motor is:

[00005]$\begin{array}{cc}{U}_c=E+L\frac{{\mathrm{di}}_a}{\mathrm{dt}}+{\mathrm{Ri}}_a& \left(2\right)\end{array}$

[0030] where E is a counter electromotive force of the servo motor, and

[00006]$E={\mathrm{nK}}_v\frac{d\theta }{\mathrm{dt}},$

n is a reduction ratio of the gear reducer, K.sub.v is a velocity feedback coefficient of the servo motor, θ is an actual position of the joint end,

[00007]$\frac{d\theta }{\mathrm{dt}}$

is an actual velocity of the joint end, L is an inductance of the servo motor, and R is a resistance of the servo motor.

[0031] An output torque of the servo motor is:

T.sub.m=i.sub.aK.sub.t  (3)

[0032] where K.sub.t is a torque coefficient of the servo motor.

[0033] Since the torque sensor has large rigidity, the elastic deformation thereof is neglected, and then a relationship between outputs of the motor output end and the actual torque of the joint end is:

T.sub.p=nT.sub.m  (4)

[0034] where T.sub.m is the output torque of the servo motor, and T.sub.p is the actual torque of the joint end.

[0035] A torque balance equation of the joint end is:

[00008]$\begin{array}{cc}{T}_p=J_m\frac{d{\theta }^2}{\mathrm{dt}}+B_m\frac{d\theta }{\mathrm{dt}}+G& \left(5\right)\end{array}$

[0036] where J.sub.m is a total moment of inertia converted to the joint-end load, and is a sum of converted moments of inertia of the load, the torque sensor, the gear reducer, the servo motor, and other components, B.sub.m is a total viscous damping coefficient converted to the joint-end load, and is a sum of converted viscous damping coefficients of the load, the gear reducer, and the servo motor, and G is a gravity term of the joint end; regarding the gravity term G of the joint end, the influence of the gravity term on the system output is eliminated by a method such as gravity compensation (in the prior art), and the influence of the gravity term is not considered in the present disclosure.

[0037] Equations (1) to (5) are combined and subjected to Laplace transformation to establish a functional block diagram of the robot joint torque control system, as shown in FIG. 2.

[0038] Equivalent transformation is performed on the block diagram of FIG. 2, a node A is moved backward (across K.sub.a) to a node B to obtain a block diagram of the joint torque control system after primary transformation, as shown in FIG. 3.

[0039] Since the closed loop 1 in FIG. 3 is a typical closed-loop feedback loop, arrangement is made according to simplified rules for closed-loop feedback to obtain a block diagram of the joint torque control system after secondary transformation, as shown in FIG. 4.

[0040] It can be seen from FIG. 4 that load parameters J.sub.m and B.sub.m have a certain influence on actual torque output of the system through a loop A (the load parameters reduce the actual torque output of the system), and the larger the load parameters, the larger the disturbing influence on the actual output torque of the system.

[0041] In order to eliminate the influence of the load parameters on the joint torque control system, a load compensation controller is designed, a current i.sub.f whose signal value of a joint-end velocity (which may be obtained after differential processing is performed on an actual position signal θ of the joint end) is compensated for by the load compensation controller is selected, and i.sub.f and i.sub.r are added and then used as a current loop input instruction. The principle is as follows: at a node C, a value of {dot over (θ)} after passing through a loop C and a value of {dot over (θ)} after passing through a loop B are equal in size and opposite in sign, so that they cancel each other out, thereby eliminating the influence of the load parameters on the force control system, as shown in FIG. 5.

[0042] At the node C:

{dot over (θ)}nK.sub.v={dot over (θ)}.sub.fG.sub.f(s)K.sub.a  (6)

[0043] Thus, the load compensation controller is:

[00009]$\begin{array}{cc}{G}_f\left(s\right)=\frac{{\mathrm{nK}}_v}{K_a}& \left(7\right)\end{array}$

[0044] The influence of the load parameters on the output torque is effectively eliminated by the load compensation controller, and equivalent transformation is performed on the whole joint torque control system to obtain a transformed joint torque control functional block diagram, as shown in FIG. 6.

[0045] An open-loop transfer function of the robot joint torque control system is:

[00010]$\begin{array}{cc}G\left(s\right)=\frac{T_p}{E_T}=\frac{{\mathrm{nK}}_t{K}_a{K}_{\mathrm{PD}}}{\mathrm{Ls}+R+K_a{K}_t}=\frac{\frac{{\mathrm{nK}}_t{K}_a{K}_{\mathrm{PD}}}{R+K_a{K}_t}}{\frac{s}{\omega }+1}=\frac{K_a}{\frac{s}{\omega }+1}& \left(8\right)\end{array}$

[0046] in the equation, a corner frequency of an inertial element:

[00011]$\omega =\frac{R+K_a{K}_t}{L},$

and an open-loop gain

[00012]$K_o=\frac{K_{\mathrm{PD}}{\mathrm{nK}}_t{K}_a}{R+K_a{K}_t}.$

[0047] The system is a typical inertial element, and when the PD controller parameter K.sub.PD is adjusted, the open-loop gain of the system changes accordingly, and a Bode diagram thereof also changes. As shown in FIG. 7, when the open-loop gain K.sub.o increased to K.sub.o′; that is, K.sub.o<K.sub.o′, an open-loop Bode diagram of the joint torque control system moves up as a whole, so that a cutoff frequency when the system passes through a 0 db line increases; that is, ω.sub.1<ω.sub.2, where ω.sub.1 is the cutoff frequency of the system when the open-loop gain is Ko, and ω.sub.2 is the cutoff frequency of the system when the open-loop gain is Ko′; since an open-loop cutoff frequency of a Bode diagram of the open-loop transfer function is a system bandwidth, the system bandwidth increases, thereby enhancing rapidity of the system and improving control performance of the system.

[0048] The described embodiments are preferred embodiments of the present disclosure, but the present disclosure is not limited to the aforementioned embodiments. Any obvious improvements, substitutions or modifications that can be made by those skilled in the art without departing from the essential content of the present disclosure shall fall within the protection scope of the present disclosure.