JP4618433B2 - Flexible control device and flexible control method for manipulator - Google Patents

Description

  The present invention relates to a flexible control device and a flexible control method for a manipulator.

As a conventional device that performs flexible control of a manipulator, there has been a device that flexibly controls the tip of a manipulator using a force sensor at the tip (for example, see Patent Document 1).
In addition, a method has been proposed in which the control gain of the flexible control is made variable by varying the degree of the position control mode and the force control mode in accordance with the contact state between the object and the manipulator (see Patent Document 2). .
FIG. 4 shows a block diagram of the control device of Patent Document 1. 101 performs position control calculation, and 102 performs compliance control calculation. Reference numeral 104 denotes force detection means, 105 denotes a servo amplifier, and 106 denotes a manipulator. The force detection means 104 detects the contact force at the tip of the manipulator 107 and sends it to the compliance control calculation 102. Based on the compliance model, 102 calculates a position deviation according to the contact force and adds it to the target position calculated by the position control calculation 101. The position control calculation 101 calculates the driving amount of each joint of the manipulator 106 from the changed target value, and sends this to the servo amplifier 105. The manipulator 106 drives the driving source of each joint by the servo amplifier 105, and as a result, performs a flexible operation according to the contact force detected by the force detection means 104.

FIG. 5 is a block diagram of the control device disclosed in Patent Document 2. In the figure, the manipulator 201 is provided with force detection means 202 and position detection means 206. The variable gain 203 can change the gain according to the degree of coordination between position and force. The torque converter 204 converts a signal obtained by multiplying the output of the force detector 202 by the variable gain 203 into joint torque. The position deviation detection unit 205 detects a difference between the position signal output from the position detection unit 206 and the command position output from the position command unit 207 as a position deviation.
The output of the position deviation detection means 205 is multiplied by the virtual spring element 208 and converted into torque, and the output of the torque conversion 204 is added and subtracted by the addition / subtraction calculation 209, and the output is processed by the proportional integration calculation means 210, If the drive command value for each joint of the manipulator is used, feedback compensation calculation can be performed.
Here, the variable gain 203 stabilizes the operation by decreasing the gain in the position control mode and increasing the gain in the force control mode to vary the degree of the position control mode and the force control mode.
As described above, in the flexible control of the conventional manipulator, the force sensor is installed for flexible control, and the gain of the flexible control means is adjusted to make the position control mode and the force control mode variable to stabilize the operation. A method was proposed.

Japanese Patent Laid-Open No. 5-189008 (page 4, FIG. 1) JP-A-2-310609 (page 10, FIG. 1)

By the way, in the actual work of the manipulator, the contact state between the manipulator and the work object is frequently switched from non-contact to contact and from contact to non-contact.
The switching of this state can be controlled smoothly by flexible control.However, if the contact force is larger than expected or changes rapidly, the accumulated amount of the integral element of the feedback compensation calculation becomes unstable when the contact state changes. There was a problem of causing a sudden movement.
Further, when there is an integral element in the feedback compensation calculation, if a force is applied due to contact between the manipulator and the work target object, a deviation occurs between the target position and the feedback position.
The flexible control works to change the target position to reduce the deviation amount, and the feedback compensation works to change the drive command in the direction to make the deviation smaller, so it usually operates without excessive accumulation of integral elements However, if the force is excessive and the state where the flexible control and the feedback compensation cannot be supported continues for a long time, the integral element is excessively accumulated. In this state, when the state is changed from the contact state to the non-contact state, the accumulated integral element is compensated in a direction in which it is released all at once. . Even if the flexible control gain is made variable by the control method of Patent Document 2, this problem cannot be avoided.
The present invention has been made in view of such problems, and an object of the present invention is to stabilize the operation of the manipulator by reducing the integral gain of the feedback compensation calculation according to the situation in the flexible control of the manipulator.

In order to solve the above problem, the present invention is as follows.
According to the first aspect of the present invention, in a flexible control device for a manipulator including force detection means and position detection means for each joint, a deviation between the target position of the joint and the position detected by the position detection means is input. A calculation unit that multiplies the deviation by a first gain value and an operation that integrates the deviation and multiplies the second gain value in parallel, and adds the calculation results, A value obtained by converting the output of the calculation unit into a torque command to the joint is compared with a preset threshold value, and the second gain value is decreased when the torque command is equal to or greater than the threshold value. And
According to a second aspect of the present invention, there is provided a flexible control method for a manipulator including a force detection unit and a position detection unit of each joint, and a deviation between a target position of the joint and a position detected by the position detection unit. The calculation of multiplying the first gain value and the calculation of integrating and multiplying the second gain value are performed in parallel, and the calculation results are added to convert the output of the calculation unit into a torque command to the joint Then, the torque command is compared with a preset threshold value, and the second gain value is decreased when the torque command becomes equal to or greater than the threshold value.

  ADVANTAGE OF THE INVENTION According to this invention, it can prevent that a rapid operation | movement arises by the bad influence of the integral element of feedback compensation at the time of the contact / non-contact switching of a manipulator and a work target, and can perform a stable operation | movement.

  Hereinafter, specific examples of the present invention will be described with reference to the drawings.

FIG. 1 is a diagram schematically illustrating an example of a control device and a manipulator to which the present invention is applied.
In FIG. 1, the manipulator 1 is drawn by extracting one of the multi-fingered hands that grip the work target object 20, and is provided with position detection means 3 at each joint, and a force is applied to the abdomen of each limb. Detection means 2 is provided. By detecting the force received from the work object 20 by the force detection means 2 and performing flexible control, the work object 20 can be gripped with an appropriate force.
FIG. 2 is a control block diagram illustrating a flexible control method for one joint of the manipulator 1. Based on the force detected by the force detection means 2, the displacement amount of one joint of the manipulator 1 is calculated by the flexible control model 4. The flexible control model is a physical model of rigidity, viscosity, and inertia, for example, and can be expressed as the following equation (1).

Here, K, D, and M are real numbers, and are constants of rigidity, viscosity, and inertia, respectively. Further, Δx represents a displacement amount, and F represents a force. By substituting the output value of the force detection means 2 into F of the equation (1) and solving the equation (1) for Δx, the model displacement amount Δx is obtained as the output of the flexible control model 4. The model displacement amount is added and subtracted with the output of the position command means 5 and then input as a target value for the feedback compensation calculation means 7.
Within the feedback compensation calculation means 7, the position deviation detection means 8 detects the difference between the input target value and the output of the joint position detection means 3 as a position deviation and inputs it to the proportional / integral calculation means 9.
In the proportional / integral calculation means 9, a value obtained by adding the input position deviation multiplied by the proportional gain 10 and the value obtained by integrating the integral calculation means 11 and then multiplying by the variable gain 12 is added. Output. This value becomes the output of the feedback calculation means 7, which is converted into torque by the torque converter 13 and becomes a torque command for driving the joint of the manipulator 1. Further, this torque command is compared with a preset threshold value in the threshold determination 14, and when the torque command becomes larger than the threshold value, the variable gain 12 is decreased. As the threshold value, a torque value corresponding to the maximum force assumed in the flexible control model 4 or a maximum torque value as a disturbance assumed in the feedback compensation calculation means 7 is set.

By reducing the integral gain (variable gain 12) based on the determination of the threshold value of the torque command, feedback compensation can be achieved even when the flexible control model and the feedback compensation calculation means require torque that cannot be handled or compensated for. It is possible to prevent an error from being accumulated in the integral element.
Note that the form and shaft configuration of the manipulator 1 shown in FIG. 1 are merely examples, and the present invention can be widely applied to manipulators in general. For example, as shown in FIG. 3, a manipulator having a jig 21 attached to the tip via the force detection means 2 performs a copying operation or a polishing operation on the work object 20 on the work table by the jig 21. .
In the case of FIG. 3, for example, in the flexible control model 4, the above-described control can be applied by converting the force detected by the force detection unit 2 into a value corresponding to each joint axis of the manipulator 1.
Further, the force detection means is not necessarily limited to a force sensor, and this control method can be applied even when an observer for estimating force is configured to perform force sensorless flexible control. Further, the threshold value is not limited to a fixed value, and can be changed according to the work situation and purpose.

  INDUSTRIAL APPLICABILITY The present invention can be widely applied to manipulators that perform flexible control, and is particularly useful when a force greater than expected can be applied to a manipulator during a contact operation depending on the operating environment or work object.

The figure which shows the example of the control apparatus and manipulator to which this invention is applied Control block diagram showing control method of manipulator of the present invention The figure which shows another example of the control apparatus and manipulator to which this invention is applied. Control block diagram showing control device of Patent Document 1 Control block diagram showing control device of Patent Document 2

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Manipulator 2 Force detection means 3 Position detection means 4 Flexible control model 5 Position command means 6 Addition / subtraction calculation 7 Feedback compensation calculation means 8 Position deviation detection means 9 Proportional / integral calculation means 10 Proportional gain 11 Integration calculation means 12 Variable gain 13 Torque conversion 14 Threshold judgment 20 Work object 21 Jig

Claims (2)

In a flexible control device for a manipulator provided with force detection means and position detection means for each joint,
A calculation that takes a deviation between a target position of the joint and a position detected by the position detection means as an input, multiplies the deviation by a first gain value, and integrates the deviation and multiplies a second gain value. Are provided in parallel, and an operation unit for adding the operation results is provided.
A value obtained by converting the output of the calculation unit into a torque command to the joint is compared with a preset threshold value, and when the torque command becomes equal to or greater than the threshold value, the second gain value is decreased. A flexible control device for a manipulator. In a flexible control method of a manipulator provided with force detection means and position detection means of each joint,
Performing in parallel the operation of multiplying the deviation between the target position of the joint and the position detected by the position detection means by the first gain value and the operation of integrating and multiplying the second gain value,
Add the calculation results,
Converting the output of the calculation unit into a torque command to the joint;
Comparing the torque command with a preset threshold;
A flexible control method for a manipulator, wherein the second gain value is decreased when the torque command becomes equal to or greater than the threshold value.