JP2553847B2 - Input value generator for parameter estimation device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial application] The present invention relates to a parameter estimation device for estimating a parameter included in a motion equation of a controlled object when controlling a controlled object such as an articulated robot manipulator. The present invention relates to an input value generation device for a parameter estimation device that generates an input value required for estimating a.

[Prior Art] Generally, the equation of motion of a controlled object such as a robot arm includes parameters that are determined by the inertia of each component constituting the controlled object, the acting gravity, the center of gravity, and the like. Therefore, if this parameter is appropriately determined to determine the motion equation of the controlled object, the controlled object can be appropriately controlled based on this equation of motion.

However, in the case of a control object in which each joint interferes with each other like a vertical articulated robot arm, and the load changes such as grasping or separating an object,
The parameters change due to interference and the size of the load. Therefore, with such a control target, the control characteristics change when the gain is fixed as in the conventional PID control method, and problems such as vibration and inability of precise movement occur.

As described above, for those having interference with each other, non-interference control is effective in which the interfering terms are canceled to make each axis independent.

However, in this non-interference control, correct values of inertia of each component, acting gravity, center of gravity, etc. are required.

Further, if one controller is applied to several types of vertical articulated robot arms, it is necessary to obtain and adjust parameters according to the controlled object each time.

Even in such a case, troublesome adjustment becomes unnecessary if the controller automatically follows the characteristic change of the controlled object and automatically changes the parameter to perform the control (adaptive control) that always maintains the optimum state. For that purpose, means for estimating the parameters is required. Even if adaptive control is not performed, if the parameters cannot be accurately determined because it is difficult to find the correct values for the inertia, acting gravity, center of gravity, etc. of each component, estimate the parameters as well. Is required.

As the parameter estimation means, for example, the least squares method is used, and the state quantity (position, velocity, etc.) of the controlled object is calculated.
There is a parameter estimation device that estimates a parameter based on.

By the way, in order to estimate such parameters,
The equation used to estimate the parameter (for example, the equation of motion of the controlled object itself) or the equation may be replaced in a form that is summarized for each parameter, but the replaced expression must be linear with respect to the parameter. is there.

On the other hand, in order to estimate the parameter, it is necessary to measure the state quantity of the controlled object, for example, in the above-mentioned robot arm, the rotation angle, angular velocity, angular acceleration, etc. of the arm.

However, although it is possible to measure up to angular velocity with high accuracy using a tachometer or other measurement means, angular acceleration tends to contain noise, and when an acceleration sensor is used as a measurement sensor, it can be attached to a vertical articulated joint. However, there are many problems such as the influence of gravity in the acceleration, which must be corrected depending on the posture.

Further, conventionally, there is a method of estimating parameters by using a filter for adaptive control. This method will be described by taking a two-joint arm of the multi-joint robot manipulator shown in FIG. 2 as an example.

The two-joint arm shown in FIG. 2 has a first joint 20 on the base side.
The first arm 20B rotates around A, and the second joint on the tip side
The second arm 20D rotates around 20C. The mass of the first arm 20B is the tip of the first arm 20B (that is, the second joint 20C), and the mass of the second arm 20D is the second arm.
The concentrated mass of the first arm 20B is m ₁ , the concentrated mass of the second arm 20D is m _2, and the rotation angle of the first arm 20B is θ ₁ Arm 2
The rotation angle of 0D is represented by θ ₂ .

Generally, the equation of motion as the characteristic of the two-joint arm which is the controlled object as shown in FIG. 2 is shown as follows. In the following equation, u ₁ and u ₂ are control inputs to the controlled object, and specifically, u ₁ is the first arm.
Input torque to 20B, u ₂ is an input torque to the second arm 20D.

Here, a ₁ , a ₂ , a ₃ , a ₄ , a ₅ are respectively the first arms 20B,
It is a parameter determined by the inertia of the second arm 20D, the acting gravity, the center of gravity, and the like. These parameters are associated with inertia, center of gravity position, etc. as follows.

a ₁ = (m ₁ + m ₂ ) d ₁ ² + m ₂ d ₂ ² a ₂ = m ₂ d ₂ ² d ₃ = m ₂ d ₁ d ₂ a ₄ = (m ₁ + m ₂ ) gd ₁ a ₅ = m ₂ gd ₂ where d ₁ is the length of the first arm 20B and d ₂ is the second arm 2
The length is 0D, and g is the gravitational acceleration. However, as described above, when the parameters a _{1 to} a ₅ change due to interference or the magnitude of the load, or because it is difficult to find the correct values for the inertia of each component, the acting gravity, the center of gravity, etc. If the parameters cannot be accurately determined, the parameters a _{1 to} a ₅ have unknown values and need to be estimated.

In the above equation (1), the matrix on the left side to which [ ₁ , ₂ ] is applied is the inertia matrix, the first term on the right side is the centrifugal force and Coriolis force, the second term is the gravitational term, and the third term is to each arm. It becomes the input torque.

Here, if ₁ and ₂ can be measured, and if the control inputs u ₁ and u ₂ are known, in the upper equation of the above equation (1), On the other hand, if the least squares method is applied, the parameters a _{1 to} a ₅ can be estimated.

_{a 1 1 + a 2 2 +} a 3 × [2cosθ _{2 1} + cosθ _{2 2} -s
in θ ₂ ( ₂ ² +2 _{1 2} )] + a ₄ sin θ ₁ + a ₅ sin (θ ₁ + θ
₂ ) -u ₁ = 0 However, as a practical matter, as described above, it is difficult to accurately measure the angular accelerations ₁ and ₂ that tend to contain noise.

Therefore, conventionally, the angular acceleration of ₁ ,
_The method of not requiring measurement of ₂ was adopted. in this way, As described above, it is necessary to apply an inverse matrix to the inertial matrix of θ on both sides from the left.

But with this method, Therefore, the linearity with respect to the parameter is lost as in the original equation. Therefore, the method using the conventional filter has a problem that the parameters cannot be estimated with high accuracy.

As described above, in the parameter estimation, conventionally, (i) the state quantity of the second derivative that is difficult to measure accurately, that is, the acceleration, is required as the state quantity of the controlled object required for the parameter estimation. , And the problem that the parameters cannot be estimated with high accuracy, and (ii) in order to eliminate the need to measure the state quantity of the second derivative, the inertia matrix of the second derivative in the equation of motion of the controlled object When the inverse matrix is applied to the equation of motion, the linearity with respect to the parameter is lost and it becomes difficult to estimate the parameter.

There was a problem.

The present invention has been made in view of the above problems, and an object thereof is to use a formula having linearity with respect to a parameter in a parameter estimation device and to perform a first-order derivative without using a state quantity of a second-order derivative of a control target. Input values for the parameter estimation device that generate the input values for the parameter estimation device so that the parameters can be estimated by using the state quantities up to and can easily and accurately estimate the parameters. Providing a generator.

[Means for Solving Problems] In the input value generation device for the parameter estimation device of the present invention, the parameter estimation device (22) estimates the parameters (a _{1 to} a ₅ ) included in the equation of motion of the controlled object (20). An input value generation device (10) for generating input values (ψ _{1 to} ψ ₆ ) necessary for: (a) an entire motion equation of the control target (20) Operator s +
In the equation (Equation 11) that does not include the state quantity of the second derivative of the controlled object (20) obtained by multiplying the operator 1 / (s + μ) that is the inverse operator of μ, the parameter estimation device (22) State quantities (θ ₁ , θ ₂ ,, of the controlled object (20) that appear in a form of being multiplied by the parameters (a _{1 to} a ₅ ) to be estimated.
A function device (12) that generates a function having ₁ and ₂ ) as variables, and outputs a function value obtained by giving the generated function a state quantity of a controlled object (20) that is a variable; At least a part ( ₁ , ₂ ) of the state quantity of the controlled object (20), an output value of the function device (12), and at least a part (u ₁ ) of the control input of the controlled object (20). On the other hand, the operator 1 / is applied to the entire equation of motion of the controlled object (20).
The equation obtained by multiplying (s + μ) (equation
The operator (1-μ / including 1 / (s + μ) in 11)
By operating (s + μ), −1 / (s + μ), the obtained values are input values (ψ _{1 to} ψ) to the parameter estimation device (22).
_And a filter device (13, 17) for outputting as ₆ ).

In the input value generation device for this parameter estimation device, the function device (12) is a control target (20) obtained by multiplying the entire equation of motion of the control target (20) by an operator 1 / (s + μ). In the equation (equation 11) that does not include the state quantity of the second derivative, the state quantity of the controlled object (20) that appears in the form of being multiplied by the parameters (a _{1 to} a ₅ ) to be estimated by the parameter estimation device (22) A function having (θ ₁ , θ ₂ , ₁ , ₂ ) as a variable is generated, and a function value obtained by giving the generated function a state quantity of the controlled object (20) that is a variable is output. In addition, the filter (13, 17) is controlled by (20)
Of at least some of the state quantities of ( ₁ , ₂ ), the output value of the function device (12), and at least some (u ₁ ) of the control inputs of the controlled object (20). (2
1 / (s in the equation (Equation 11) obtained by multiplying the entire equation of motion of 0) by the operator 1 / (s + μ)
Operators including (+ μ) (1-μ / (s + μ), -1 / (s +
μ)) is output as an input value (ψ _{1 to} ψ ₆ ) to the parameter estimation device (22).

Here, the operator 1 / is applied to the entire equation of motion of the controlled object (20).
By multiplying by (s + μ), an equation that has linearity with respect to the parameters (a _{1 to} a ₅ ) and does not include the state quantity of the bisection differential of the controlled object (20) is obtained. Therefore, in this equation, the function to be estimated is multiplied by the function and the operation including the operator including 1 / (s + μ) is performed by the function unit (12) and the filter (13, 17) to estimate the parameter. In the device (22), an equation having linearity with respect to the parameters (a _{1 to} a ₅ ) is used, and the state quantity up to the first derivative is used without using the state quantity of the second derivative of the controlled object (20). The input values (ψ _{1 to} ψ ₆ ) with which the parameters (a _{1 to} a ₅ ) can be estimated are obtained.

In the case of a two-joint arm included in the articulated robot manipulator of the controlled object (20), for example, the function device is a base side of the two-joint arm included in the articulated robot manipulator as the controlled object (20). each theta ₁ to the rotational angle of the first second arm, and distal of, -sin ₁ as a function upon a theta _2, generates a -sin (θ ₁ + θ ₂₎ and ₍₂ 1 _{+ 2)} cos [theta] ₂ and the state quantity theta ₁ of these functions to the controlled object (20), theta _{2, 1, 2} state quantity theta ₁ which is output from the measuring device for measuring (21) a, theta _{2, 1, 2}
And outputs the function value obtained by giving the filter to the state quantities ₁ and ₂ output from the measuring device (21) and the (2 ₁ + ₂ ) cos θ ₂ output from the function device (12), respectively. Values obtained by operating the operator μ / (s + μ) {μ / (s + μ)} ₁ , {μ / (s + μ)} ₂ and {μ / (s + μ)} (2 ₁ + ₂ ) cos θ ₂ and measurement apparatus state quantity output from (21) _{1, 2} and function unit device output value (12) ₍₂ 1 _{+ 2)} from the cos [theta] ₂ Metropolitan {s / (s + μ) } 1, {s / (s + μ)} 2 And {s /
_{(S + μ)} (2} 1 + 2) cosθ 2 calculates the input values [psi ₁ for the parameter estimator (22), respectively, [psi _2,
The first filter device (13) that outputs as ψ ₃ and -sinθ ₁ and -sin (θ that are output from the function device device (12).
₁ + θ ₂ ) and a value {1 / (s +) obtained by operating the operator −1 / (s + μ) on the control input u ₁ for the first joint on the base side of the two-joint arm as the controlled object (20)
μ)} sin θ ₁ , {1 / (s + μ)} sin (θ ₁ + θ ₂ ) and − {1 / (s + μ)} u ₁ are calculated, and input values ψ ₄ and ψ _{5 to the} parameter estimation device (22), respectively. , ψ ₆ , and a second filter device (17) for outputting as ψ ₆ .

[Example] Hereinafter, an input value generation device for the parameter estimation device according to the present invention (hereinafter, also simply referred to as an input value generation device).
With reference to the accompanying drawings, a detailed description will be given of the preferred embodiments.

(Explanation of Attached Drawings) FIG. 1 is a block diagram showing a circuit configuration of an input value generation device according to an embodiment of the present invention, and particularly, a two-joint arm of an articulated robot manipulator shown in FIG. It shows the case of applying to the control of.

FIG. 2 is an explanatory diagram for explaining the two-joint arm included in the multi-joint robot manipulator as described above. The first and second control inputs, that is, input torques u ₁ and u ₂ The case where the first arm 20B and the second arm 20D rotate about the first joint 20A and the second joint 20C, respectively, is shown.

(Structure of Embodiment) First, the structure of the input value generation device of this embodiment will be described in detail with reference to FIGS. 1 and 2.
Here, the case where the input value generation device of the present invention is applied to control of a two-joint arm included in a multi-joint robot manipulator (for example, a vertical multi-joint robot manipulator) is illustrated.

The input value generation device 10 of this embodiment includes a function device device 12. The function device 12 is connected to first to fourth output terminals 21 _{1 to} 21 ₄ of a measuring device 21 arranged on first and second output shafts 20 ₃ and 20 ₄ of a control target 20 described later, respectively. State quantities of the controlled object 20 given from the measuring device 21 to which the _first to _fourth input terminals 121 to 124 are connected (here, the first arm 20B and the second arm included in the articulated robot manipulator). 20D
Rotation angles) θ ₁ , θ ₂ and other state quantities (rotation speeds of the first arm 20B and the second arm 20D included in the articulated robot manipulator in this case) ₁ , ₂ There is. The function device 12 includes an operator 1 / which is an inverse operator of an operator s + μ that adds the derivative of the operand to the constant multiple of the operand to the entire equation of motion of the controlled object 20.
Controlled object 20 obtained by multiplying (s + μ)
In the equation that does not include the state quantity of the second derivative of, the parameter (a ₁ ~
state quantity of the control object 20 appearing in multiplied form in a ₅₎ (θ _1,
A function having θ ₂ , ₁ , ₂ ) as a variable is generated, and a function value obtained by giving the generated function a state quantity of the controlled object 20 which is a variable is output. Function device 12
, Specifically as a function, -sin ₁ and -sin (θ ₁ +
theta ₂₎ and (2 1 _{+ 2)} to generate the cos [theta] _2, these functions, the measuring device a function value obtained by applying the state quantities output from the 21 first to third output terminals 12 ₅ - It is designed to output from 12 ₇ respectively.

The function device 12 includes a first output terminal 21 ₁ of the measuring device 21.
A first function unit 12a for calculating and outputting a -sin ₁ from the state quantity theta ₁ given from the input end 12 ₁ a connected and the measuring device 21, the first measurement device 21, the second Output end 2
The first and second input terminals 12 ₁ and 12 ₂ are connected to 1 ₁ and 21 ₂ , respectively, and the state quantity θ of the controlled object 20 given from the measuring device 21 is given.
_{A first} adder 12b for adding ₁ and θ ₂ to each other and outputting the sum as a state quantity θ ₁ + θ ₂ and an input end connected to the output end of the first adder 12b The second function unit 12c for calculating and outputting −sin (θ ₁ + θ ₂ ) from the state quantity sum θ ₁ + θ ₂ given from 12b is included.

The function device 12 also includes a second output terminal 21 of the measuring device 21.
_A third function unit 12d for connecting the input terminal 12 ₂ to ₂ and calculating and outputting cos θ ₂ from the state quantity θ ₂ given from the measuring apparatus 21, and a third output terminal of the measuring apparatus 21. 21 ₃ and the fourth function unit 12e for the input terminal 12 ₃ and outputs the calculated state quantity _{1-2 1} provided from which the measuring device 21 is connected, an output terminal of the third function unit 12d Is connected to the first input end and the second input end is connected to the fourth output end 21 of the measuring device 21.
Connected to ₄ and given by the third function unit 12d
₂ c from θ ₂ and the state quantity ₂ given by the measuring device 21
a first multiplier 12f for calculating and outputting osθ ₂ ,
The first input end is connected to the output end of the third function unit 12d and the second input end is connected to the output end of the fourth function unit 12e, and cos θ given from the third function unit 12d _{A second} multiplier that calculates and outputs 2 ₁ cos θ ₂ from 2 and 2 ₁ given from the fourth function unit 12e, and the first and second multipliers 12
f, each of the first to the output terminal of 12g, the first and second non-inverting input terminal connected, the second multiplier 12f, and a ₂ cos [theta] ₂ and theta ₁ cos [theta] ₂ given from 12g to each other adding to a second adder 12h to output a _{(2 1 + 2) cosθ 2} , it is inclusions.

Input value generating device 10 is also part of the state quantities output from the measuring device 21, the output value of the function unit 12, and for some of the control inputs of the controlled object 20, the controlled object 20 An operator including 1 / (s + μ) in the equation obtained by multiplying the entire equation of motion by 1 / (s + μ) is made to act, and the obtained values are input values (ψ _{1 to} ψ ₆ ) to the parameter estimation device 22. ) Is output as a first filter device 13 and a second filter device 17.

The first filter device 13 is the third output end of the measuring device 21.
21 _{3 is connected} to the first and second input terminals 13 ₁ and 13 ₄ respectively, and the fourth output terminal 21 ₄ of the measuring device 21 is connected to the 3rd and 4th input terminals 13 ₂ and 1
3 ₅ are connected to each other and the 5th and 6th input terminals 13 ₃ and 13 ₆ are connected to the 3rd output terminal 12 ₅ of the function unit 12 (that is, the output terminal of the second adder 12h). , The operator for the state quantities ₁ and ₂ given from the measuring device 21 and the (2 ₁ + ₂ ) cos θ ₂ given from the function device 12 respectively. Let it work, (2 ₁ + ₂ ) cos θ ₂ is calculated and subtracted from the state quantities ₁ and ₂ provided by the measuring device 21 and the (2 ₁ + ₂ ) cos θ ₂ provided by the function device 12, respectively. ₍₂ 1 _{+ 2)} cosθ 2 input values [psi ₁ calculated respectively for the parameter estimator 22, [psi ₂ and [psi ₃ as the first to third input terminals 22 ₁ to 22 ₃ of the parameter estimation device 22
To give to.

In the second filter device 17, the first and second output ends 12 ₆ and 12 ₇ of the function device device 12 are connected to the first and second input ends 17 ₁ and 17 ₂ respectively, and the third input -sin ₁ end 17 ₃ is given from the first control input 20 function unit 12 is connected to _one of the controlled object _{20, -sin (θ 1 + θ} 2) and the first control of the controlled object 20 The operator is applied to the control input u ₁ given from the input terminal 20 _1. Of the parameter estimation device 22 as input values ψ ₄ , ψ ₅ and ψ ₆ for the parameter estimation device 22 respectively.
To the sixth input terminals 22 _{4 to} 22 ₆ .

In the first filter device 13 and the second filter device 17, μ represents a constant. Further, s is a variable s usually used in the Laplace transform, and since it is used in the time domain here, it corresponds to the differential operator d / dt. Therefore, s + μ is (d / dt + μ), that is, an operator that adds the constant (μ) times of the operand to the derivative of the operand, and 1 / (s + μ) is the inverse operator of s + μ, that is, (d / It is the inverse operator of dt + μ). More specifically, the operand is F, and the operand F is an operator.
Let G be the function obtained by acting 1 / (s + μ),
It is expressed as the following formula.

{1 / (s + μ)} * F = G Here, * means to apply an operator to the subsequent function. By transforming these by formally performing four arithmetic operations, the following equation is obtained.

F = s * G + μ * G This shows that the derivative of G and the sum of G and a constant multiple are F. Obtaining G by giving F is solving this differential equation. Therefore, 1 / (s + μ) is considered to be an operator that makes the solution G of this differential equation correspond to the given F.

Therefore, considering a filter that operates on the operator 1 / (s + μ), this filter is G (t) = {1 / (s + μ)} * F (t) when the input F (t) is given. The output G (t) is Operator μ /
Filters and operators that operate (s + μ)-/ (s +
The same applies to a filter that applies μ).

The first filter device 13 is the third output end of the measuring device 21.
An input terminal is connected to 21 ₃ and an operator is applied to the state quantity ₁ given by the measuring device 21. Let it work, The input end is connected to the first filter 13a for calculating and the fourth output end 21 ₄ of the measuring device 21.
For the state quantity ₂ given by A second filter 13b for calculating and outputting an input terminal to the first output terminal 12 ₅ of the function unit 12 is given from the function unit 12 is connected (2 ₁ + ₂ ) Co
operator for sθ ₂ And a third filter 13c for calculating

The first filter device 13 also includes a first filter 13a.
The inverting input terminal is connected to the output terminal and the non-inverting input terminal is connected to the third output terminal 21 ₃ of the measuring device 21 and is supplied from the first filter 13a. Input value by subtracting from the state quantity given by the measuring device 21 And a third adder 13d for calculating and applying it to the _first input end 22 ₁ of the parameter estimation device 22, and an inverting input end connected to the output end of the second filter 13b and a non-inverting input end connected to the measuring device. Connected to the fourth output 21 ₄ of 21 and fed from the second filter 13b Is subtracted from the state quantity ₂ given by the measuring device 21, and the input value Is added to the _second input end 22 ₂ of the parameter estimation device 22 and the inverting input end is connected to the output end of the third filter 13c and the non-inverting input end is a function unit. Is connected to the output of a second adder 12a included in the device 12, The were applied from the second adder 12h ₍₂ 1 ₊ 2) cosθ
Input value subtracted from ₂ And a fifth adder 13f for calculating and applying it to the _third input end 22 ₃ of the parameter estimation device 22.

The second filter device 17 has an input end 17 ₁ connected to the first output end 12 ₆ of the function device device 12, and an operator for −sin θ ₁ given from the function device device 12. A fourth filter 17a for calculating and applying to the _fourth input terminal 22 ₄ of the parameter estimation device 22, and a second filter of the function device device 12.
The input end 17 ₂ is connected to the output end 12 ₇ of the
For −sin (θ ₁ + θ ₂ ) given by Input value And a fifth filter 17b for calculating and applying the calculated value to the fifth input end 22 ₅ of the parameter estimation device 22 and the first control input end 20 ₁ of the control target 20 to which the input end 17 ₃ is connected. 20
For the control input u ₁ given from the first control input terminal 20 _{1 of} And a sixth filter 17c for calculating and applying it to the sixth input end 22 ₆ of the parameter estimation device 22.

As described above, the controlled object 20 is formed on the first arm 20B that rotates around the first joint 20A in response to the control input u ₁ while being influenced by the gravitational acceleration g, and on the tip end portion of the first arm 20B. And a second arm 20D which rotates about the second joint 20C in response to the control input u ₂ while being influenced by the gravitational acceleration g.

Here, the angle formed by the first arm 20B and the coordinate axis Y of the coordinate system XY on which the first joint 20A and the first arm 20B are placed (that is, the rotation angle of the first arm 20B) is the state quantity θ _1, and the second arm 2
The state quantity θ represents the angle (that is, the rotation angle of the second arm 20C) that 0D makes with the extension line of the first arm 20B in the plane on which the coordinate system XY lies.
₂ , the controlled object 20, that is, the operation of the two-joint arm, is controlled by the state quantities θ ₁ , θ ₂ , ₁ , ₁ , ₂ , ₁ , ₂ and the parameter a ₁ ~
It is expressed by the aforementioned equation of motion (Equation 1) using a ₅ and control inputs u ₁ and u ₂ . In the present embodiment, as will be described later, the entire equation of motion is multiplied by the operator 1 / (s + μ) and the parameters are estimated using an equation that does not include the state quantities ₁ and ₂ of the _second derivative. As the state quantities of the controlled object 20 , only θ ₁ , θ ₂ , ₁ , ₂ are measured.

Parameter estimator 22 has an input end 22 ₁ to 22 ₆ of the first through sixth are connected respectively to the first to the output end of the sixth input value generating device 10, provided from an input value generating device 10 By processing the input values ψ _{1 to} ψ ₆ by the least squares method, the parameter a ₁ included in the equation of motion of the controlled object 20
~ A ₅ is estimated, and the estimation result is output as estimation parameters ₁ to ₅ . Estimated parameter ₁ ~
₅ is used to create the control inputs u ₁ and u ₂ that are applied to the first and second control input terminals 20 ₁ and 20 ₂ of the controlled object 20 .

(Operation of Embodiment) Next, the operation of the input value generation device according to the present embodiment will be described in detail with reference to FIGS. 1 and 2.

The control inputs u ₁ and u ₂ are the first and second control input terminals 20 ₁ and 20 ₂ (that is, the motor arranged at the first joint 20A and the second joint 20C).
The motor to be placed on the control target 20 (that is, the two-joint arm included in the multi-joint robot manipulator) is Start to work according to.

Equation 2 is (a ₁ + 2a ₃ cos θ ₂ ) ₁ + (a ₂ + a ₃ cos θ ₂ ) ₂ = a ₃ ( ₂ ² +2 _{1 2} ) sin θ ₂ −a ₄ sin θ ₁ −a ₅ sin (θ ₁ + θ ₂ ). + u ₁ ... (equation 3) and a _{(a 2 + a 3 cosθ 2} ) 1 + a 2 2 = -a 3 1 2 sinθ 2 -a 5 sin (θ 1 + θ 2) + u 2 ... ( equation 4), the formula 3 because it contains all the unknown parameters a ₁ ~a _{5 to, a 1 1 + a 2 2} + a 3 obtained by modifying the equation _{3 [2 1 cosθ 2 + 2} cosθ 2 - (2 2 +2 1 2) sinθ 2] _{+ a 4 sinθ 1 + a 5} sin (θ 1 + θ 2) -u 1 = 0 ... ( equation 5) is used to estimate the parameters a ₁ ~a _5. It should be noted that Expression 5 is a modification of Expression 2 which is the equation of motion, and can be said to be an equation of motion itself.

Here, the operator on both sides of Expression 5 When multiplied, Becomes

By the way, the function f (x) of the variable x and the operator about, The relationship is established. It can be seen that this relationship is correct since it becomes a differential formula when rearranged by multiplying both sides by s + μ as shown below.

f (x) = (s + μ) fx− (x) −− μf
(X) = s (f (x)) − (x) ∴s (f (x)) = f (x) + (x) Therefore, in Expression 6, Can be rewritten as the following Expressions 7 to 10.

Substituting equations 7-10, equation 6 yields Becomes Thus, when the entire equation of motion of the controlled object 20 is multiplied by the operator 1 / (s + μ), the state quantity of the controlled object 20 does not include the _second derivative (acceleration ₁ and ₂ ),
And a linear model (equation) can be obtained for unknown parameters a _{1 to} a ₅ .

Here, the input values ψ _{1 to} ψ ₆ given from the input value generation device 10 of the present embodiment to the parameter estimation device 22 are Then, Equation 11 is a ₁ ψ ₁ + a ₂ ψ ₂ + a ₃ ψ ₃ + a ₄ ψ ₄ + a ₅ ψ ₅ + a ₆ ψ ₆ = 0
(Equation 12) and the parameter estimation device 22 receives the given input value ψ
_The unknown parameters a _{1 to} a ₅ can be estimated by the method of least squares based on _{1 to} ψ ₆ . Note that the parameter a ₆ is a ₆ = 1 and is known, as can be seen by comparing Expression 11 and Expression 12. Therefore, the input value generation device of this embodiment
In 10 , the input values ψ _{1 to} ψ ₆ are obtained and given to the parameter estimation device 22 as follows.

That is, first, second output shaft 20 ₃ of the controlled object 20, 20
₄ (here, the rotation axis of the first joint 20A and the rotation axis of the second joint 20C) is provided with the measuring device 21, so that the state quantities θ ₁ , θ ₂ , ₁ , ₂ are detected. An input value generator for outputting from each of the _{first to} fourth output terminals 21 _{1 to} 21 ₄
The ten _first to _fourth input terminals 12 _{1 to} 12 ₄ are provided.

In the input value generation device 10 , the first device included in the function device 12
The adder 12b of the state quantity θ ₁ ,
θ ₂ is added to each other and output as the state quantity sum θ ₁ + θ ₂ .

In the function unit 12, also, the state amount theta ₁ given from the measuring device 21 is provided to the fourth filter 1a of the second filter device 17 as -sin ₁ is processed in the first function unit 12a, also State quantity sum θ ₁ given from the first adder 12b
+ Θ ₂ is processed by the second function unit 12c and −sin (θ ₁ +
θ ₂ ) as the fifth filter 17 of the second filter device 17
given to b.

In the function device 12, the state quantity θ ₂ given from the measuring device 21 is further processed by the third function device 12d and given as cos θ ₂ to the first and second multipliers 12f and 12g. twenty one
The state quantity ₁ given by the above is processed by the fourth function unit 12e and given to the second multiplier 12g as 2 ₁ .

First multiplier 12f state quantity ₂ In given from the measuring device 21 and the cos [theta] ₂ given from the third function unit 12d is multiplied together included in the function unit 12, a ₂ cos [theta] ₂ second Given to the adder 12h.

In the second multiplier 12g included in the function unit device 12, cos θ ₂ given from the third function unit 12d and 2 ₁ given from the fourth function unit 12e are multiplied by each other, and 2 ₁ cos θ ₂
Is given to the second adder 12h as

In the second adder 12h included in the function unit 12, ₂ cos θ ₂ given from the first multiplier 12f and the second multiplier 12g
2 ₁ cos θ ₂ given by is added to each other, and (2
₁ + ₂ ) cos θ ₂ is given to the third filter 13c included in the first filter device 13.

In the first filter device 13, the state quantity ₁ given by the measuring device 21 is processed by the first filter 13a. Is given to the inverting input terminal of the third adder 13d as the above, and the state quantity ₂ given from the measuring device 21 is processed by the second filter 13b. Is given to the inverting input terminal of the fourth adder 13e.

In the first filter device 13, also provided from the second adder 12h function device apparatus 12 (2 1 _{+ 2)} cos [theta] ₂
Is processed by the third filter 13c Is given to the inverting input terminal of the fifth adder 13f.

The first filter device 13 further includes a third adder 13d.
The state quantity ₁ is applied from the measuring device 21 to the non-inverting input terminal of the Is given, Is calculated and given as an input value ψ ₁ to the first input end 22 ₁ of the parameter estimation device 22.

In the first filter device 13, in addition, a fourth adder 13
The state quantity ₂ is given from the measuring device 21 to the non-inverting input terminal of e and from the second filter 13b to the inverting input terminal. Is given, Is calculated and given as an input value ψ ₂ to the second input end 22 ₂ of the parameter estimation device 22.

In the first filter device 13, in addition, the fifth adder 13
from the function unit 12 to the non-inverting input terminal of f (2 1 _{+ 2)} c
osθ ₂ is applied and the third filter 13c is provided at the inverting input terminal.
From Is given, Is calculated and given as an input value ψ ₃ to the third input end 22 ₃ of the parameter estimation device 22.

In the second filter device 17, −sin θ ₁ given from the second function device 12c of the function device device 12 is used as the fifth filter 17a.
Input value processed by Is given to the _fourth input end 22 ₄ of the parameter estimation device 22 as

In the second filter device 17, −sin (θ ₁ + θ ₂ ) given from the second function device 12c of the function device device 12 is processed by the fifth filter 17b, and the converted value is obtained. Is given to the _fifth input end 22 ₅ of the parameter estimation device 22 as

In the second filter device 17, the first filter of the controlled object 20 is further added.
The control input u ₁ given from the control input end of is processed by the sixth filter 17c and the input value Is given to the _sixth input end 22 ₆ of the parameter estimation device 22.

In the parameter estimation device 22, the first value of the input value generation device 10
Through sixth input terminal 22 21 _to the output end of the first to sixth
Input value given to 2 ₆ Parameters included in the equation of motion of controlled object 20 using
a _{1 to} a ₅ are estimated by the method of least squares, and the estimation result is output as estimation parameters ₁ to ₅ .

That is, in the parameter estimation device 22, for the input Z The calculation process of the least squares method is executed, and the output P is sent out. Where U = ψ ₆ and the input Z is And the output P is Is.

Estimated parameter output from parameter estimation device 22
₁ to ₅ are used to create control inputs u ₁ and u ₂ for the controlled object 20 (that is, the 2-joint arm included in the multi-joint robot manipulator).

As described above, according to the input value generation device 10 of the present embodiment, the parameter estimation device 22 uses the equation (equation 11) having linearity with respect to the parameters a _{1 to} a ₅ and calculates the second derivative of the controlled object 20. The input values ψ _{1 to} ψ ₆ to the parameter estimation device 22 can be estimated so that the parameters a _{1 to} a ₅ can be estimated by using the state quantities up to the first derivative without using the state quantities (accelerations ₁ and ₂ ). Can be generated and given to the parameter estimation device 22. Therefore, the parameter estimation device 22 can easily and accurately estimate the parameters. As a result, appropriate control inputs u ₁ and u ₂ are input to the controlled object 20, and as a result, the controlled object 20 can be smoothly controlled. That is, in FIG. 2, the first arm 20B and the second arm 20D of the articulated robot smoothly rotate around the first joint 20A and the second joint 20C, respectively, and
It is possible to realize precise movement of the joint arm.

Although the present invention has been described with reference to the embodiments, the present invention is not limited to the above embodiments, and various modifications can be made. For example, in the above embodiment, the controlled object 20 of the present invention
Has been described as a two-joint arm included in the multi-joint robot manipulator, the present invention is not limited to this and can be applied to various objects such as a crane as a controlled object.

Further, as a parameter estimation method in the parameter estimation device 22, a method other than the least square method, for example, a method based on the stability method (gradient method) or the like may be used.

(3) Effects of the Invention As described above, the input value generation device for the parameter estimation device according to the present invention is configured as described in the [Problem Solving Means] column as a solving means. The parameter can be estimated by using the state quantity up to the first derivative without using the state quantity of the second derivative of the controlled object by using an equation having linearity with respect to the parameter in the estimation device. It is possible to generate an input value to the estimation device, and as a result, it is possible to easily and accurately estimate the parameter in the parameter estimation device.

[Brief description of drawings]

FIG. 1 is a circuit diagram showing an embodiment of an input value generation device according to the present invention, and FIG. 2 is an explanatory diagram for explaining a 2-joint arm included in a multi-joint robot manipulator. 10 …… Input value generator 12 …… Function device 12a, 12c to 12e …… Function device 12b, 12h …… Adder 12f, 12g …… Multiplier 13 …… Filter device 13a to 13c …… Filter 13d to 13f …… Adder 17 …… Filter device 17a to 17c …… Filter 20 …… Control target 20A …… First joint 20B …… First arm 20C …… Second joint 20D …… Second arm 21 …… Measuring device 22 ...... Parameter estimation device

Claims (2)

(57) [Claims] 1. A parameter estimation device (22) controls a controlled object (2
An input value generation device (10) for generating input values (ψ _{1 to} ψ ₆ ) necessary for estimating parameters (a _{1 to} a ₅ ) included in the equation of motion of (0), comprising: (a) control An operator s + that adds a constant multiple of the operand to the derivative of the operand to the entire equation of motion of the target (20)
In the equation (Equation 11) that does not include the state quantity of the second derivative of the controlled object (20) obtained by multiplying the operator 1 / (s + μ) that is the inverse operator of μ, the parameter estimation device (22) State quantities (θ ₁ , θ ₂ ,, of the controlled object (20) that appear in a form of being multiplied by the parameters (a _{1 to} a ₅ ) to be estimated.
A function device (12) that generates a function having ₁ and ₂ ) as variables, and outputs a function value obtained by giving the generated function a state quantity of a controlled object (20) that is a variable; At least a part ( ₁ , ₂ ) of the state quantities of the controlled object (20), the output value of the function device (12), and at least a part (u ₁ ) of the control input of the controlled object (20). Is obtained by multiplying the entire equation of motion of the controlled object (20) by the operator 1 / (s + μ), the operator (1-μ / (S + μ), −1 / (s + μ)), and the obtained value is output as an input value (ψ _{1 to} ψ ₆ ) to the parameter estimation device (22) (13,1)
7) An input value generation device for a parameter estimation device, characterized by comprising: 2. The function device, wherein the rotation angles of a first arm on the base side and a second arm on the front end side of a bi-joint arm included in a multi-joint robot manipulator as a controlled object (20) are respectively θ ₁ , Assuming θ ₂ is −si as a function
_{nθ 1, -sin (θ 1 +} θ 2) and ₍₂ 1 ₊ 2) cos
generates theta _2, the state quantity theta ₁ of these functions to the controlled object _{(20), θ 2, 1} , 2 state quantity theta ₁ which is output from the measuring device for measuring (21) a, θ _{2, 1, 2} outputs a function value obtained by applying the said filter is output from the state quantity ₁ that is output from the measuring device (21), ₂ and function unit device (12) (2 1 _{+ 2)} cos [theta]
_A value obtained by operating the operator μ / (s + μ) on 2 {μ / (s + μ)} ₁ , {μ / (s + μ)}
₂ and {μ / (s + μ) } (2 1 + 2) cosθ 2
When the state of ₁ output from the measuring device (21), ₂ and function unit device output value (12) ₍₂ 1 _{+ 2)} cos [theta] ₂
And {s / (s + μ)} ₁ , {s / (s + μ)} ₂ and {s / (s + μ)} (2 ₁ + ₂ ) cos θ ₂ are calculated, and input values ψ to the parameter estimation device (22) are calculated. _First filter device (13) for outputting ₁ , ₁ , ₂ and ₃
And −sin θ ₁ and −sin output from the function device (12)
A value obtained by operating the operator −1 / (s−μ) on (θ ₁ + θ ₂ ) and the control input u ₁ for the first joint on the base side of the two-joint arm as the controlled object (20) { 1 / (s
+ Μ)} sin θ ₁ , {1 / (s + μ)} sin (θ ₁ + θ ₂ ) and − {1 / (s + μ)} u ₁ are calculated, and input values ψ ₄ and ψ _{5 to the} parameter estimation device (22), respectively. An input value generation device for the parameter estimation device according to claim 1, further comprising a second filter device (17) for outputting as ψ ₆ .