JPS6398709A - Method for compensating load weight of manipulator - Google Patents

Abstract

PURPOSE:To obtain the compensation command voltage with use of a correction function obtained from a yacobi matrix by applying control of integration (I) to a manipulator and therefore decreasing satisfactorily the deviation from the target position of the manipulator. CONSTITUTION:The increment of servo command voltage is measured when the load weight is applied in a state where the control function of integration (I) is used at a reference measuring point to set the deviation of the target value at about zero. Based on this measurement result, the feed-forward control for compensation of the load weight is carried out at a general position of a manipulator in place of the control function of integration (I). Thus, it is possible decrease the target value deviation approximately like in the case of the control of integration (I). Furthermore, the responsibility can be greatly improved compared with the control of integration (I).

Description

DETAILED DESCRIPTION OF THE INVENTION <Field of Industrial Application> The present invention relates to a compensation control method for load weight in a manipulator.

<Prior Art and Problems> When a manipulator grasps a workpiece and moves it along a target trajectory, it may deviate significantly from the target position due to the weight of the workpiece. In order to compensate for this shift or deviation, PID (proportional, integral, differential) control, especially I (integral) control, is effective and increases the feedback gain, but it also guarantees the stability of the servo system. Therefore, it is not possible to use a very large feedback gain. Therefore, it takes a long time to converge to the target position accuracy using an appropriate feedback gain, and this control method cannot be used when moving at high speed. .

In order to improve this drawback, there is a control method in which a force sensor is attached to the manipulator's wrist to actually measure and compensate for the load weight. This control method calculates the torque that acts on each axis of the manipulator when moving along the target trajectory using the load weight measured by a force sensor and the matrix and graph related to the manipulator mechanism, and then calculates the command voltage corresponding to this torque. This is to compensate for the load weight. However, in order to realize this method, a force sensor for measuring the load weight is required in addition to the manipulator, which has the drawback that it cannot be realized using only the manipulator.

There is also a method of identifying the load weight without using a force sensor. As a method for controlling the servo system, PD (
A method is also used that uses proportional/derivative (proportional/derivative) control or P (proportional) control to identify from the deviation from the target value and the servo rigidity of the servo system. In this method, it is necessary to measure in advance the relationship between the deviation from the target value and the torque generated by the servo system at that time, and to determine the servo rigidity.

Therefore, when using this method, the complicated work of calibrating the servo rigidity was required.

In view of the problems of the prior art described above, the purpose of the present invention is to shorten the convergence time to the target position, and also to eliminate the need for a sensor for measuring load weight and the need for calibration of the servo rigidity of the manipulator. The object of the present invention is to provide a manipulator control method that solves the problem of determining a command voltage for a servo system to compensate for the load weight.

<Means for solving the problem and its operation> The present invention provides l i4J control to the manipulator to sufficiently reduce the deviation of the manipulator from the target position, and based on the command voltage to each axis servo at that time, The basic idea is to identify the command voltage corresponding to the load weight and use a correction function determined from the Jacobian matrix to determine the compensation command voltage at a general position within the manipulator operating range.

The main principles of the present invention will be explained using a six-degree-of-freedom manipulator as an example. Arbitrary position θ” of manipulator = (θ
5. θ2. ...The torque acting on each axis at θ6) is τ
(6) 9 The torque that opposes the external force acting on the wrist is 1w (
θ) The torque that supports the manipulator's own weight is τ@(6)
Then, when the manipulator is statically balanced, the following relationship holds. (Here, T represents the transposed matrix.
) τ (θ) = τw (θ) + 1m (θ) - 111 If the force acting on hand a is F, the torque opposing this F is τw (θ
), the following relationship holds true.

However, J(θ) is a Jacobian matrix, and the acting external force F
The component of is given by the following equation, where M is the moment.

F T= (F,,F,,F,,M,,M,,M,)−
+3) When a load weight iW acts on the wrist, the force F acting on the wrist is given by the following equation, where the mass of the load is m, and l force acceleration is g.

FT-W"-(0,0,mg, 0.0.0)-(
4) On the other hand, assuming that a linear relationship exists between the command voltage V applied to the servo motor of each axis of the manipulator and the acting torque τ, it is defined by the following equation.

τ = K, V = -v, +, v'' - (5) However, K is a gain matrix related to the servo stiffness that is 0 except on the main diagonal as shown in the following equation (6), vw and v' are command voltages corresponding to load weight and dead weight compensation, respectively.

Taking equations (1) and (51) into consideration, the following relationship is obtained.

Here, the Jacobian transposed matrix JT(θ) and its inverse matrix (JT(θ))−1 are defined by the following equations.

When formula (2) is transformed using formulas (4) and (9), the following relationship is obtained.

The following relationship holds true from the relationship between the elements in equation (10).

1, T (θ)τw−111g, ! k(θ), -0 (k-1, 2, 4, 5, 6,) -
αυ reference measurement point θ = θ. When the load weight W is determined using the load command voltage VW (θ.) at , the following equation is obtained.

Using W in equation (12), the torque τW acting on the joint axis of a general manipulator at general position θ is calculated by equation (2).
can be found.

7w(θ) = ja(θ)・i-”(θ.),, VW
(θ.)·−αj The command voltage VW(θ) at the general position θ is given by the following equation using equation (7) using equation (13).

vw(δ)-, -1 so.

= am-' Js(θ)-i3(θo)4-Vw(θ
,>-(14) In equation (14), θ=θ. This means that the gravity compensation voltage VW(θ) at the manipulator general position θ can be given based on the measured command voltage at .

However, in equation (14), -1 is included in the gain matrix Km related to servo stiffness and its inverse matrix, and it is necessary to transform it into a form that removes the influence of servo stiffness. The operation of removing the influence of -1 from the gain matrix and its inverse matrix using the condition of Equation (11) will be described below.

Using the relationship in equation (7), the following relationship is obtained.

By using the condition of equation (11), the following relationship holds true for 7w(θ.).

...α1 As shown in equation (16), the elements τwi of 7w(θ0) are not linearly independent, and the above five relationships are given to the six parameters, so each parameter is It can be expressed using one parameter.

Here, it is assumed that A-(J"(θo))-'-(17).

The matrix on the left side of equation (16) is the matrix from which the third row of the A matrix is removed.

If the minor determinant formed by removing the i-th row and j-th column of the matrix A is defined as AIJ, then the following relationship holds between the elements of the vector 'Fw using Cramer's formula.

Using the relationship of formula (18), the command voltage VW of formula (14)
The influence of is removed from the gain matrix contained in (θ).

The elements of vector j3(θ) can be organized into the following relationship.

VW (θ) - to J3 (θ) 13th (θ.) - VW (θ.) Here, vector t 37 (θ.)
When the elements of are set as i, 1(θ.), (i
3(θ0) and 8<60)) can be rearranged into the following equation using equation (18).

With 13 teeth (θ.), w(θ)・ΣLi(θ0)τwll 1 Here, equation (20) can be transformed into the following form.

i, 1lT(θ0) 1w(θ.)・α5τw5−α2 m k VW k O・・−(22) As shown in equation (22), w(θ.) at (i3”(θ.).

)) can be expressed using 1 arbitrarily, so
In the gain matrix, the command voltage VW (θ) shown in equation (14) is
It is possible to remove it from within.

In this way, as shown in equation (23), the gain matrix was able to be removed from the equation for VW(θ).

Formula (51, f7) Command voltage vCe) C, Formula (2
:]) (7) Using VW(θ), it can be given by the following equation.

■ (θ)=VW(θ)+V'(θ) −(24
) Equation (24) can be rearranged into the following form using Equation (23).

V(θ)−F(θ, θO) vw(θo) + v”(
θ′)−(25) where the F(θ, θ.) matrix is expressed in the following form using equation (25).

-(26) From the relationships in equations (81, (91, and (14)), it can be easily seen that the following relationship holds true.

F・(θ., θ.)=I(I is the identity matrix.)−(
27) Among the parameters in equation (25), if the self-weight compensation voltage v0 (θ) of the manipulator is calibrated in advance, the command voltage corresponding to the load weight will be VW (θ).

) is the reference measurement point θ=θ. It is given by the following equation using the command voltage (θ0) when the load weight is applied at .

VW(θ.)・V(θ.)-V'(θ.')-(28
) As shown in equation (28), the reference measurement point θ=θ.

If VW (θ0) is measured at , the load weight compensation voltage at the manipulator general position can be given by the command voltage of equation (25).

<Embodiment> FIG. 1 shows an embodiment of the present invention, in which 2 is a load, 2 is a three-degree-of-freedom manipulator, and 3 is a controller. FIG. 2 shows a mechanical part of a three-degree-of-freedom manipulator related to gravity, and as shown in this figure, the three degrees of freedom are related to gravity compensation. At this time, the Jacobian matrix with two degrees of freedom is given by the following equation.

Applying the above-described method of the present invention to the Jacobian matrix of equation (30) to obtain the correction interval F(θ, θ0) yields the following equation.

−(:11) However, θ7−(θ1.θ2), θ. . −(θ1oθ2
0).

Furthermore, the self-weight compensation voltage v of the manipulator shown in this example is
Ce) is given by the following formula.

By using equations (31) and (32), the load weight compensation voltage v (e) at the general manipulator position is given by the following equation.

θ as the reference measurement point. ”-(20°, 40') is used,
The method for determining the compensation voltage VW (θ.) will be explained using an example in which an object of approximately 80 g is applied as the load weight. At the reference measurement point, the command voltage V(θ.) is measured when the voltage converges to the target position with sufficient accuracy due to the effects of the PID control, especially the l control. In addition, by determining the command voltage V'' (θ0) at the reference measurement point in the no-load state in advance,
Using equation (28), the compensation voltage VW corresponding to the load weight
(θ.) can be found.

V (u oJ)i,hi V-(tj oJ'k)
$3 し' I V w (tj o) It is given by the q-order equation.

By substituting equation (38) into equation (33), the load weight compensation voltage at the manipulator general position θ can be determined.

By organizing the above procedure, FIG. 3 shows a method for identifying the compensation voltage and compensating at the general position of the manipulator. Furthermore, prior to this step, a correction function matrix (θ, θ.) is calculated using the Jacobian matrix of the target manipulator. As shown in this flow, first, the manipulator dead weight compensation voltage V1 (θ.) at the reference measurement point θ=00 is determined.

Next, with the load weight acting on the manipulator,
By the effect of P [or l control of PID control, the target position deviation of the reference measurement point θ=00 is made sufficiently small. At this time, each axis command voltage V (θ0) is determined, and the command voltage VW (θ.) corresponding to the load weight is determined from equation (28). Using the command voltage VW (θ0) for this load weight, the formula (
33), calculate the load weight compensation voltage V@ at the manipulator general position θ, and perform feedforward control of the load weight compensation voltage V(6) by combining P control and PD sub-control to perform load weight compensation.

FIG. 4 shows a comparison of target position deviations using various control methods. In the figure, the Δ mark is the PD sub-control, the mark is the PID control, and the O mark is the control method according to the present invention, which is the result of performing the PD sub-control and the feedforward control of the present invention. As shown in the control results, it was found that using the control method of the present invention can significantly improve the error in the target position compared to the case where only PD sub-control is used. It can be seen that it is close to . Although PID control can achieve extremely low target value deviations due to the effect of l control, some problems remain regarding its responsiveness. FIG. 5 shows the S Lcp response of the PID control with the l control function added, and it can be seen that about 100 sampling steps are required before settling to the target value. However, the experimental results when using this control method are shown in FIG. 6, and it can be seen that the target value is settled in about 10 steps, and the response can be significantly improved compared to PID control.

<Effects of the Invention> As explained above, the servo command voltage increment when the load weight is applied is measured with the target value deviation made almost zero using the l control function at the reference measurement point; and based on the results. Feedforward control for load weight compensation at the manipulator general position is performed instead of the I control function, so it has the advantage of reducing the target deviation similar to when using the Habo I control, and has the same responsiveness as the I control. It has the advantage of being significantly improved compared to the case where it is applied. In addition, there is an advantage that the conventional load weight compensation method does not require calibration of the servo stiffness of the relationship between the servo command voltage and the load weight, and performs control using only the servo command voltage of the signal inside the controller.

[Brief explanation of the drawing]

Fig. 1 is a block diagram of an embodiment of the present invention, Fig. 2 is a link block diagram of an example of the manipulator shown in Fig. 1, Fig. 3 is a flowchart of compensation control, and Fig. 4 is a target value of each persimmon control method. A comparison explanatory diagram of deviation, FIG. 5, shows the conventional PID;11
An explanatory diagram of an example of a step response using control. FIG. 6 is an explanatory diagram of an example of a step response of the method of the present invention. In the figure, 1 is a load weight, 2 is a 3-degree-of-freedom manipulator, and 3 is a control controller.

Claims (1)

[Claims] When a load weight acts on the manipulator, first perform PI control or PID control using the deviation of the target position in an unloaded state at the load weight identification position of the workpiece to apply command voltage to the servo of each axis of the manipulator. is given, and the command voltage when it converges to the target position with sufficient accuracy is stored in the control device, and at the identified position, PI control or PID control is performed using the deviation of the target position with the load weight acting. By doing this, the servo command voltage is given to each axis of the manipulator, the command voltage when it converges on the target position with sufficient accuracy is memorized, and the identified position is determined from the difference between the command value in the no-load state and the command voltage in the loaded state. The command voltage of each axis servo corresponding to the load weight is identified, and the correction function using the Jacobian matrix regarding the manipulator mechanism and the command voltage corresponding to the load weight and the load at a general position within the operating range of the manipulator from the identified position are calculated. 1. A load weight compensation control method for a manipulator, comprising calculating a command voltage for compensating the weight, and performing feedforward control of the command voltage for compensating the load weight to a servo system of each axis of the manipulator.