JPH04306712A - Controller for manipulator - Google Patents

Abstract

PURPOSE:To provide the controller of a manipulator, whose work handling part operates in accordance with a set dynamic characteristic and which can precisely follow a set target track. CONSTITUTION:The above controller 1 obtains set impedance (g) equivalent to the acceleration command value of the work handling part 25 of the manipulator 20, and the estimated value of inertia of the work handling part is obtained by the estimated value M*(theta) of an inertia from an inertia term operation part 31 and said set impedance (g). Furthermore, the torque tau of an articurate moter is calculated. The acceleration d<2>x/dt<2> of the work handling part, which is driven based on the torque is obtained based on a detected angle theta. As value obtained by multiplying difference of the acceleration d2x/dt<2> and set impedance(g) by a gain G is fed-back, the set change of the gain G can turn the difference of the estimated value reflected on the acceleration d2x/dt<2> smaller.

Description

[Detailed description of the invention]

0001

[Field of Industrial Application] The present invention relates to a control device that performs so-called impedance control that applies torque to the workpiece handling section of a manipulator to operate it at a target dynamic impedance, and in particular, The present invention relates to a control device for a manipulator that allows a manipulator to accurately follow its target trajectory.

0002

[Prior Art] As a control device for the above-mentioned manipulator, there is a method described in the paper "Impedance Control".
lAn Approach to Manipulation
ion”, ASME J. of Dynamic
Systems, Measurement, an
d Control, N. Hogan, Vol. 1
07-1, pp. There is a device disclosed in 1/22 (1985). A control block diagram corresponding to the above control device is shown in FIG. In the figure, the manipulator control device 1a is equipped with a dynamic impedance setting section 3 that expresses the target dynamic behavior of the workpiece handling section of the manipulator 20. The dynamic impedance setting unit 3 includes a target elastic coefficient matrix 4 (k/m) divided by the target elasticity m, and a target viscosity coefficient matrix 5.
(b/m) and a target inertia coefficient matrix 6 (1/m) are set in advance. Further, the workpiece handling section of the manipulator 20 is provided with a force sensor 14 made of, for example, a strain sensor, which detects the vector of the reaction force F from the external environment. The operation of the manipulator 20 is given by the following equation of motion. M(θ)・d2 θ/dt2 +C(
dθ/dt, θ) =
τ+J(θ)T ・F+D
…(1) Here,
θ: Joint angle (vector) M(θ)
: Inertia term (matrix) C (dθ/dt, θ): Nonlinear term (matrix) such as Coriolis force, centripetal force, viscosity, Coulomb friction, etc. τ: Torque given to joint motor D: Disturbance J (θ) such as torque ripple T: Transposed matrix of Jacobian F: Reaction force that the workpiece handling section receives from the external environment, and
The control device 1a determines the position x and speed dx/dx of the workpiece handling section based on the swing angle (angle θ) of the joint output from the angle sensor 21 provided on the joint motor of the manipulator 20. dt is calculated using the differentiator 34 and the converter 2. Furthermore, the control device 1a is
The deviation x−x0 between the above position x and the target position x0 set by the target position setting unit 7, and the above calculated speed dx
/dt, the reaction force F output from the force sensor 14,
Each coefficient matrix 4, 5 of the dynamic impedance setting section 3
, 6 to calculate the set impedance g. The set impedance g corresponds to the acceleration given as a command to the workpiece handling section, and is converted by the conversion section 8 into a function of the angle θ. That is, the configuration for determining the set impedance g is the acceleration calculation means according to the present invention. The conversion unit 8 calculates the acceleration d2 x/corresponding to the set impedance g using the following equation (2).
Convert dt2 to angular acceleration d2 θ/dt2. d2 θ/dt2 = J(θ)-1{
d2 x/dt2 - (dθ/dt)2
・dJ(θ)/
dθ} …(2)
Further, the inertia term calculation unit 31 includes a mathematical model for determining an inertia term related to the workpiece handling unit. Then, the estimated value M* (θ) of the inertia term of the manipulator 20 at that time is determined based on the angle θ input to the inertia term calculation unit 31 via the conversion unit 8 and the above mathematical model. Value M* (θ) and the above converted angular acceleration d2 θ/
dt2, the estimated value of inertia force M* (θ)・d2
θ/dt2 is determined. That is, the converter 8 and the inertia term calculator 31 convert the calculated acceleration command value d2 into
x/dt2, the angle θ (position) of the above joint, and the angular acceleration d
The estimated value M* (θ
)・d2 θ/dt2 is an estimated value calculation means. On the other hand, the nonlinear compensation calculator 30 uses terms such as Coriolis force, centripetal force, viscosity, and Coulomb friction (hereinafter referred to as nonlinear terms).
It is equipped with a mathematical model to find the . The nonlinear term is an estimated value C* (dθ/dt, θ) obtained by solving the mathematical model based on the angle and angular velocity of the joint. Then, the estimated value of the above inertial force M* (θ)・d2 θ/
dt2 and the estimated value of the nonlinear term C* (dθ/dt, θ
) and the reaction force F・J(θ)T detected by the force sensor 14 and converted into a function of the angle θ by the conversion unit 32, the dynamic characteristics of the workpiece handling unit are determined by the dynamic impedance setting unit. The torque τ that satisfies the target elasticity, viscosity, and inertia k, b, and m set as 3 is calculated. Incidentally, the calculation formula set in the control device 1a and corresponding to the torque τ is shown in the following equation (3). τ=C* (θ, dθ/dt)−J(
θ)T ・F+M* (θ)J(θ)−1
・{-(dθ/dt)2 ・dJ(θ)/
dθ+m−1 {F−b ・dx
/dt-k(x-x0)}}
...(3) However, x=L(θ), dx/d
Assume that t=J(θ)·dθ/dt holds, and L(θ) is a transformation matrix from the joint coordinate system to the orthogonal coordinate system. Further, the above-mentioned set impedance g is expressed by the following equation. g=g(F, dx/dt, x, d2 x0 /
dt2,dx0/dt,x0) = m
-1・{F-b・dx/dt-k(x-x0)}
(4) FIG. 5 shows an example in which the workpiece handling section follows a preset target trajectory using the manipulator 20 controlled by the control device 1a. In the figure, an obstacle 40 exists on the target trajectory P0 of the workpiece handling section. The workpiece handling section moves clockwise in the figure from the starting point S by the drive of the joint motor so as to follow a preset target trajectory P0. The actual movement locus of the workpiece handling section at this time is shown by a solid line P2. When the workpiece handling section comes into contact with the obstacle 40, the workpiece handling section moves along the surface of the obstacle 40 and receives a reaction force F from the obstacle 40 at the same time. The reaction force F is detected by the force sensor 14 of the workpiece handling section. However, in the figure, the plurality of straight lines indicating the reaction force F indicate the vector of the reaction force from above the obstacle 40 that each straight line intersects.

0003

By the way, in the conventional manipulator control device 1a, when calculating torque using the estimated values M* and C* of the inertia term and nonlinear term, the calculated torque ( Equation (3)) contains the estimation error ΔM(θ) (=M(θ)−M*
(θ)), ΔC(θ)(=C(dθ/dt, θ)−C*
(dθ/dt, θ)) (hereinafter referred to as modeling error. However, M(θ) and C(dθ/dt, θ) are unknown and represent the true values of each term above.) It is also reflected in the reaction force F mentioned above. Therefore, the acceleration calculated using the reaction force F is apparently calculated to be small due to the modeling error. Therefore, there is a problem in that the movement trajectory P2 of the workpiece handling section deviates from the target trajectory P0 as shown in the figure. Therefore, in the control device 1a, each coefficient matrix 4, 5, 6 (k/m, b/m, 1
By changing the value of /m) to a large value, it is possible to increase the accuracy of tracking the movement trajectory with respect to the target trajectory P0. For example, FIG. 6 shows the copying operation of the workpiece handling section when the value of the target elasticity k is set three times larger than that shown in FIG. 5. As shown, the above movement trajectory P3
is approaching the target trajectory P0, and its tracking accuracy is improving. However, by changing the setting of the value of k described above, the reaction force F applied to the workpiece handling section also becomes about three times as large. In this case, it is not possible to simultaneously satisfy the following accuracy of the workpiece handling section with respect to the target trajectory and the operation corresponding to the predetermined dynamic characteristics. Therefore, an object of the present invention is to provide a manipulator control device that allows a workpiece handling section to accurately follow a set target trajectory and operate with set dynamic characteristics.

0004

[Means for Solving the Problems] In order to achieve the above object, the main means adopted by the present invention is to connect a certain link of a manipulator formed by connecting a plurality of links via joints. an acceleration calculation means for calculating an acceleration command value for the workpiece handling section based on the position, speed, and reaction force of the workpiece handling section provided and set dynamic characteristics of the workpiece handling section; at least an estimated value calculation means for calculating an estimated value of the inertia force of the manipulator necessary for realizing the above based on the acceleration command value and the position and velocity of the joint; In the control device that calculates and outputs the torque of the drive source that drives the joint according to the force, the control device includes an acceleration detection means that detects the acceleration of the workpiece handling section, and an acceleration command value detected by the acceleration command value and the acceleration detection means that detects the acceleration of the workpiece handling section. The control device for the manipulator includes a control loop that adds a value obtained by multiplying the difference between the acceleration and the calculated acceleration by a gain to the calculated acceleration command value.

[0005]

[Operation] In the manipulator control device of the present invention, when calculating the torque output to the drive source that drives the joints of the manipulator, the estimated value of the inertia force of the workpiece handling section calculated by the estimated value calculation means is at least used. Estimated values generally include estimation errors, so when the workpiece handling section is driven based on the above torque, the acceleration of the workpiece handling section at that time and the reaction force received from the outside also include the above estimation error. reflected. That is, the difference between the acceleration detected by the acceleration detection means and the acceleration command value calculated by the acceleration calculation means represents the estimation error or, for example, disturbance. Therefore, by multiplying the difference between the detected acceleration and the calculated acceleration command value by a gain, and changing the setting of this gain, it is possible to adjust the estimation error included in the estimated value of the inertial force. Therefore, the above control device can accurately drive and control the workpiece handling section so that the workpiece handling section operates based on the set dynamic characteristics.

[0006]

DESCRIPTION OF THE PREFERRED EMBODIMENTS Examples embodying the present invention will be described below with reference to the accompanying drawings to provide an understanding of the present invention. It should be noted that the following examples are examples of embodying the present invention, and are not intended to limit the technical scope of the present invention. Here, FIG. 1 is a perspective view showing a manipulator according to an embodiment of the present invention, FIG. 2 is a block diagram showing a control device of the manipulator, and FIG. FIG. However, the same reference numerals are used for elements common to those of the conventional manipulator control device 1a shown in FIG. 4, and detailed explanation thereof will be omitted. As shown in FIG. 1, the manipulator 20 according to the present embodiment includes a first joint motor 9a fixed to a base (not shown) and a first joint motor 9a fixed to the rotational drive shaft of the joint motor 9a. A link 23, a second joint motor 9b fixed to the tip of the link 23, a second link 24 provided to the rotational drive shaft of the joint motor 9b, and a workpiece placed at the tip of the link 24. It consists of a handling section 25. The joint motors 9a and 9b are each integrally provided with an encoder (360000p/rotation) as an angle sensor 21, and the angle θa of the link 23 swinging with respect to the base and the axis of the link 23 can be measured. The angle θb of the link 24 swinging with respect to the angle θb is detected and output. Since the link lengths of the links 23 and 24 are determined in advance, the position of the workpiece handling section 25 in, for example, the x direction is determined by these angles θa and θb. Note that the joint motor 9 in FIG.
a and 9b, and similarly, the angle θ represents the above-mentioned angles θa and θb. The control device 1 that controls the manipulator 20 includes:
As shown in FIG. 2, the basic configuration is almost the same as that of the conventional control device 1a, and the difference in structure from the conventional control device 1a is that the angle θ detected by the angle sensor 21 is Differentiator 33 and converter 2 that calculate acceleration d2 θ/dt2 and acceleration d2 x/dt2
and the difference between the set impedance g (acceleration command value) from the target impedance setting section 3 and the acceleration d2 x/dt2 from the converting section 2 (g-d2 x/dt2
), and the subtracter 17 calculates the above difference (g-d2 x/dt
2) Gain setting unit 1 that sets the gain G to be multiplied by
5, and the value G (g-d2 x/
dt2) to the set impedance g, the subtracter 17, the gain setting section 15,
This is because a control loop configuration consisting of an adder 16 is adopted. Further, the control device 1 includes a target speed setting section 12 that sets the target speed dx0/dt of the workpiece handling section 25.
and a target acceleration setting section 13 for setting the target acceleration d2 Setting section 13
The target acceleration from Therefore, the set impedance g is expressed by the following equation (5). g=d2 x0 /dt2 +m-1
{F-b(dx/dt-dx0/dt)
−k(x−xo)}
…(
5) Then, the set impedance g is converted into angular acceleration in the conversion section 8 using equation (2).

On the other hand, the inertia term calculation unit 31 and the nonlinear compensation calculation unit 30 calculate the angle θ output from the angle sensor 21 of the joint motor 9, and the angular velocity and angular acceleration calculated from the angle θ by the differentiators 33 and 34. Based on , the estimated value M* (θ) of the inertial term and the estimated value C* (
θ, dθ/dt). That is, the angle sensor 21
, the differentiator 33 is an acceleration detection means for detecting the acceleration of the workpiece handling section 25. Then, the joint motor 9
The torque τ to be applied to is determined by the following equation (6). That is, equation (6) represents the control law for the manipulator 20. τ=C* (θ, dθ/dt)−J(
θ)T F+M* (θ)J(θ)-1
{-(dθ/dt)2 ・dJ(θ)/dθ
+g +G(g-d2 x
/dt2)}
...(6) Therefore, the acceleration of the workpiece handling section 25 d2 x/dt
2 are equations (1), (5), and the modeling error equation (ΔM=M(θ)−M*(θ), ΔC=C(dθ/d
t, θ)-C* (dθ/dt, θ)), the following (7
) is shown as the formula. d2 x/dt2 =g+{I+G}
-1{J(θ)}m-1{ΔM(θ)
・d2 θ/dt2
+ΔC(dθ/dt,θ)−D}


...(7) However, I is a unit matrix. the above(
In Equation 7), the second term on the right side represents the influence of modeling error and disturbance D on acceleration d2 x/dt2 . Therefore, by changing the setting of the gain G from the gain setting section 15, the influence of the modeling error and disturbance can be adjusted. For example, by making the gain G sufficiently large, the effects of the modeling error and disturbance can be made extremely small, thereby making it possible to bring the calculated acceleration d2 x/dt2 as close as possible to the set impedance g. can. Note that the converting sections 2, 8, and 32, the dynamic impedance setting section 3,
Nonlinear compensation calculator 30, inertia term calculator 31, differentiator 33
, 34, the gain setting unit 15, the adder 16, and the subtracter 17 are all configured by software running on a CPU (not shown), and are stored as a program in a memory (not shown). Further, the manipulator 20 is connected to the C of the control device 1.
It is controlled by the PU at a sampling period of 4 milliseconds. In addition, in the mathematical model of the nonlinear compensation calculator 30,
The viscosity and Coulomb friction of the drive shaft of the joint motor 9 are determined in advance by applying force (torque) to the stopped drive shaft in steps and measuring the torque when the drive shaft starts moving. Ta. Further, the coefficient related to the inertia term was obtained by applying a white noise-like torque to the joint motor 9 and simultaneously measuring the torque.

FIG. 3 shows an example in which the control device 1 of the manipulator 20 having the above-mentioned configuration is used to perform a copying operation for the target trajectory of the workpiece handling section 25. in this case,
In the control device 1, a target trajectory P0 similar to the conventional one is set in advance as the target position x0 to which the workpiece handling section 25 moves. Further, the target elasticity k, target viscosity b, and target inertia m of the dynamic impedance setting section 3 are, for example, k=1000N/m, b=200Ns/m, m
=10Kg is set. These are the same values as those set in the conventional case (FIG. 5). Further, the gain G is set to G=2I. In the conventional method, the above gain G corresponds to G=0, but by setting the above gain G(2I), the value of the second term on the right side of the above equation (7) becomes 1/3 compared to the conventional method, Control accuracy is tripled. Therefore, when the torque τ determined by the above equation (6) is applied to the joint motor 9 of the manipulator 20, the workpiece handling section 25 starts moving from the starting point S. At this time, the moving trajectory P1 of the workpiece handling section 25 and the reaction force F received by the workpiece handling section 25 are shown in the figure. According to this, the workpiece handling section 25 of the manipulator 20 is moving along a movement trajectory P1 closer to the target trajectory P0 while receiving the same reaction force F from the obstacle 40 as compared to the conventional method (FIG. 5). As described above, the manipulator control device 1 of this embodiment arbitrarily reduces the influence of the modeling error and disturbance by adjusting the gain G.
A control method is adopted in which the workpiece handling section 25 can be driven in an operation corresponding to the set dynamic characteristics, and the target trajectory tracking accuracy of the workpiece handling section 25 can be dramatically improved. Moreover, compared to conventional CPU operations, it is only necessary to increase one subtraction (subtracter 17), one addition (adder 16), and one multiplication (gain setting section 15).
PU load does not increase significantly. Further, the setting range of the set impedance g is generally limited by the upper limit of the feedback amount of the position, velocity, and acceleration of the workpiece handling section 25. However, according to the device of this embodiment, by changing the gain G, the relationship between the set impedance g and the feedback amount can be determined more freely than before. Therefore, by adjusting the gain G so as not to exceed the upper limit of the feedback amount, it is possible to widen the setting range of the set impedance g. on the other hand,
When the workpiece handling section 25 comes into contact with, for example, a hard external environment, it is necessary to set the value of the target elasticity k of the target impedance setting section 3 to a small value. In such a case, according to the conventional method, if the target elasticity k is set small, the target trajectory tracking accuracy becomes extremely poor. However, according to the method of the present invention, even if the value of the target elasticity k is small, the gain G
By setting a large value, impedance control can be performed with good tracking accuracy. Therefore, work that requires high tracking accuracy while coming into contact with a hard external environment, or work that assembles easily damaged parts, for example, can be performed with high precision.

[Effects of the Invention] The present invention is constructed as described above. Therefore, by adjusting the gain, control accuracy can be improved without depending on the set dynamic characteristics. As a result, the workpiece handling section is accurately driven and controlled using the set dynamic characteristics.

[Brief explanation of drawings]

FIG. 1 is a perspective view showing a manipulator according to an embodiment of the present invention.

FIG. 2 is a block configuration diagram showing a control device for the manipulator.

FIG. 3 is an explanatory diagram showing a copying operation of the workpiece handling section of the manipulator with respect to a target trajectory.

FIG. 4 is a block configuration diagram showing a conventional manipulator control device, which is an example of the background of the present invention.

FIG. 5 is an explanatory diagram showing the copying operation of the workpiece handling section of the conventional manipulator.

FIG. 6 is an explanatory diagram showing a copying operation performed under different setting conditions from the copying operation in FIG. 5;

[Explanation of symbols]

1, 1a...control device 3...Dynamic impedance setting section 4...Target elastic coefficient matrix 5...Target viscosity coefficient matrix 6...Target inertia coefficient matrix 8... Conversion section 9, 9a, 9b...Joint motor 14...force sensor 15...gain setting section 16... Adder 17...Subtractor 20...Manipulator 21...Angle sensor 23...Link 24...Link 25...Work handling section 31...Inertia term calculation unit

Claims (1)

[Claims] [Claim 1] The position, velocity, and reaction force of a workpiece handling section provided on a certain link of a manipulator formed by connecting a plurality of links via joints, and the set dynamic characteristics of the workpiece handling section. an acceleration calculation means that calculates an acceleration command value for the workpiece handling section based on the acceleration command value, and an estimated value of the inertia force of the manipulator necessary to realize the acceleration command value, based on the acceleration command value and the position and velocity of the joint. and an estimated value calculating means for calculating based on the estimated value of the inertial force and the reaction force, the control device calculates and outputs the torque of the drive source that drives the joint based on the estimated value of the inertial force and the reaction force. and a control loop that adds a value obtained by multiplying the difference between the calculated acceleration command value and the acceleration detected by the acceleration detection means by a gain to the calculated acceleration command value. A manipulator control device characterized by: