JPH07186076A - Manipulator controller - Google Patents

Abstract

PURPOSE:To prevent biting-in and overshooting at the time of a tool coming into contact with an object in the case of manipulator control. CONSTITUTION:A contact deciding means B rakes in the detection value Fs of force or a moment detected by means of a force detecting means A at every control cycle, and conducts comparison with a force threshold Fth previously designated, and decides that a tool has come into contact with an object in the case of the detection value Fs having become more than the force threshold value Fth continuously more than a control cycle number (m) previously designated. A change cycle number calculating means C divides a supposition spring constant change time Tw previously designated, by a predetermined control cycle time DELTAT, and calculates a change cycle number N at the time of changing a supposition spring constant, and a supposition spring constant changing means D changes the supposition spring constant of a manipulator E into a final spring constant KHend previously designated, by the control cycle of N times, when it is decided by means of the contact deciding means B that the tool has been brought into contact with the object.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator control device, and more particularly to a manipulator control device suitable for controlling a manipulator when a tool comes into contact with an object to perform work.

[0002]

2. Description of the Related Art When a manipulator is used to contact an object such as a polishing operation or a component fitting operation, generally, when the tool is moved to the object, position control is performed to bring the tool into contact with the object. When performing the work, force control is performed.

In order to perform the work smoothly in such a case, it is necessary to smoothly switch the control modes of the position control and the force control. The applicant of the present invention has disclosed, as a control device for realizing such smooth mode switching, in Japanese Patent Laid-Open No.
The coordinated control device for the position and force of the manipulator listed in No. 310609 was proposed.

FIG. 5 is a principle block diagram of this conventional apparatus.

In the figure, a manipulator section 1 is composed of manipulator driving means 2 for driving each joint of the manipulator and an arm section 3. A force detection unit 4 is provided at the tip of the arm unit 3 to search for a force or a moment that the manipulator receives from the external environment, or both of them (hereinafter, abbreviated as “force”) in the tool coordinate system. The output of the force detection means 4 passes through the force output filtering calculation means 41, whereby the high frequency component that causes vibration is cut. The output of the force output filtering calculation means 41 is input to the hand force variable gain calculation means 5, where the calculation processing for changing the gain in accordance with the degree of cooperation between position and force is performed. In the hand force variable gain calculation means 5, for example, calculation for lowering the gain is performed for the components of the position control mode, the degree of separation between the position control and the force control is increased, and more accurate position control and force control can be performed. To Further, the position control component is prevented from becoming unstable due to the force output feedback signal.

The degree of cooperation between position and force is determined by increasing or decreasing a virtual spring constant for each direction component in the tool coordinate system. The gain of the hand variable force gain calculation means 5 is increased or decreased according to the increase or decrease of the spring constant. In the case of the position control mode, the force feedback gain of the component having a large virtual spring constant, that is, the component for which more position control is desired, is reduced to be small. On the contrary, in the force control mode, the virtual spring constant of the component for which the force control is desired is made small, and the force feedback gain of the corresponding force control component is increased to be large. The increase / decrease of the force feedback gain is set so as to increase / decrease in inverse proportion to the virtual spring constant, for example.

The output torque converting means 6 multiplies the output from the hand force variable gain calculating means 5 by, for example, the transposed Jacobian matrix to convert the force in the tool coordinate system into the force in each joint coordinate system of the manipulator. Convert.

The force command means 7 sends out a force command signal representing a force or the like in the tool coordinate system. The force command signal is subjected to the same calculation by the force command filtering calculation means 71 as the force output filtering calculation means. In the command force variable gain calculation means 8 which applies the calculation processing for converting the gain according to the degree of the cooperation of the position and the force to the force command signal sent from the force command filtering calculation means 71, like the hand force variable gain calculation means 5. Is calculated. This is to make the command value and the level of the feedback signal the same. The command torque conversion means 9 converts the output from the command force variable gain calculation means 8 into, for example, a force or a moment in each joint coordinate system of the manipulator by multiplying the output by the transfer Jacobian matrix, or both.

The position of each joint of the manipulator is detected by the position detecting means 10. The command position of each joint of the manipulator is sent from the position command means 11.

The position deviation detecting means 12 detects a position deviation obtained as a difference between the position signal output from the position detecting means 10 and the command position output from the position commanding means 11.

The position deviation torque converting means 13 converts the virtual spring constant set in the tool coordinate system according to the degree of cooperation of the position and force into the virtual spring constant in each joint coordinate system of the manipulator. Based on the converted virtual spring constant and the position deviation output from the position deviation detecting means 12, the force is converted into a force or the like according to the position deviation.

The speed detecting means 14 detects the speed of each joint of the manipulator. The speed torque conversion means 15 converts the speed feedback gain set in the tool coordinate system into the speed feedback gain at each joint of the manipulator, and based on the converted speed feedback gain,
The speed detected by the speed detecting means 14 is converted into a force or the like according to the speed.

The feedback compensation calculation means 16 performs feedback compensation so that the sum of the outputs of the command torque conversion means 9 and the position deviation torque conversion means 13 matches the sum of the outputs of the output torque conversion means 6 and the speed torque conversion means 15. Calculate

In the feedback compensation calculation means 16, the addition / subtraction calculation means 17 gives a command for the force or the like output from the command torque conversion means 9 and a command for the force or the like corresponding to the position deviation output from the position deviation torque conversion means 13. Is added, and the signal of the force or the like output from the output torque converting means 6 and the signal of the force or the like corresponding to the speed output from the speed torque converting means 15 are subtracted, and the proportional-integral calculating means 18 adds / subtracts the calculating means. The deviation output from 17 is subjected to proportional, integral calculation processing and proportional, integral, and differential calculation processing to obtain a command value such as a force to drive means of each joint of the manipulator.

Finally, the arm portion 3 of each joint of the manipulator moves based on the command value from the feedback compensation calculation means 16, and coincides with at least one of the force command values in the component of the force control mode in which the virtual spring constant is small. As described above, a force is generated at the tool end, and in the component of the position control mode where the virtual spring constant is large, the force operates so as to match the position command value.

Further, when the virtual spring constant is an intermediate value, it attempts to operate up to a point where forces or the like corresponding to the position deviation balance, that is, the manipulator moves as if it is supported by a spring.

As described above, according to the conventional technique shown in FIG. 5, the position control mode and the force control mode can be smoothly switched by continuously changing the virtual spring constant. However, this prior art does not show a specific method for switching between the position control mode and the force control mode when the tool is in contact with the object or when the tool is working while moving on the object. It was

Therefore, as a concrete method of switching the control mode, for example, a method of instructing a change pattern of the virtual spring constant to the manipulator in advance by a program or teaching can be considered. When used, there was a problem that various defects would occur if the position of the object was displaced during the actual work. That is, when the object is displaced closer to the tool, the position control is weighted and the object comes into contact with the object, causing the tool to bite into the object, and conversely, the object moves farther from the tool. In the case of deviation, the object is brought into contact with weight on the force control, and there is a possibility that force overshoot may occur at the time of contact.

As a method of coping with this problem, it is possible to judge the contact between the tool and the object and switch the position control mode to the force control mode immediately after the contact. Such a mode switching technique is disclosed in Japanese Patent Laid-Open No. 3-1.
No. 84789.

This technique calculates the speed command value in each of the position control mode and the force control mode, converts each speed command value into a speed command value at each joint using an inverse Jacobian matrix, and servos the motor. Control.

Switching from the position control mode to the force control mode is performed when the manipulator contacts an object and the force detection value Fs of the force detector exceeds a force threshold Fth from a preset value. Start the transition from control mode to force control mode. After that, after the force detection value Fs reaches the target force Fo, the force control mode becomes complete.

As described above, according to the conventional technique of the mode switching, the mode switching can be performed in conjunction with the contact of the tool, so that the various problems due to the displacement of the object as described above do not occur.

[0023]

However, in the prior art relating to this mode switching, the force detection value Fs and the preset threshold value Fth are compared to switch between position control and force control, but in the vicinity of the threshold value Fth. Force detection value F
When noise is added to s, Fs may vibrate near Fth, and may or may not exceed Fth repeatedly. Therefore, position control and force control are frequently switched, and manipulator state There was a problem that was extremely unstable.

Since the speed command values in the position control mode and the force control mode do not always match before and after the control mode switching, a sudden change in speed may occur, causing a significant shock to the servo system. To prevent this, it is necessary to add acceleration / deceleration so that the speed command values match, but there is a problem that complicated calculation for acceleration / deceleration generation is required.

The present invention has been made to solve the above-mentioned conventional problems, and prevents overshoot of force and biting of the tool at the time of contact and separation of the tool with respect to an object, and stable operation and smoothness. An object of the present invention is to provide a manipulator control device capable of switching control modes.

[0026]

In order to solve the above problems, in the first configuration of the present invention, force detecting means for detecting at least one of a force and a moment that the tool receives from the external environment in each control cycle. And comparing the detection value of the force detection means with the force threshold value for each control cycle, and if the detection value is continuously equal to or more than the force threshold value for a predetermined number of control cycles or more, The number of change cycles when changing the virtual spring constant of the manipulator is calculated based on the contact determination means for determining that the tool has contacted the object and the virtual spring constant change time and the predetermined control cycle time that are designated in advance. Change cycle number calculation means,
From the time it is determined to be contact by the contact determination means,
And a virtual spring constant changing means for changing the virtual spring constant of the manipulator to a predetermined final virtual spring constant at the number of change cycles.

In the second configuration, in the first configuration, the distance detection means for detecting the distance between the tool and the object is compared with the detected distance and a predetermined distance threshold value. , A speed and an acceleration of the manipulator are changed based on the judgment result of at least one of the approach judging means and the contact judging means, which judges the approaching state when the distance becomes equal to or less than the distance threshold value. The speed / acceleration changing means, the speed / acceleration changing means changes the speed of the manipulator to a predetermined approach speed when the approach determining means determines that the manipulator is in an approaching state,
In the third structure, the speed of the manipulator is changed to a predetermined work speed when the contact judging unit judges that the contact is made, and in the second structure, the virtual spring constant changing unit makes the approaching speed change. It is characterized in that the virtual spring constant of the manipulator is changed to a pre-designated virtual spring constant at the time of contact between the time when the contact determining unit determines that the state is approaching and the time when the contact determining unit determines that the contact is in contact.

In the fourth structure, a series of position command values representing the trajectory of the manipulator and a speed set value corresponding to the position command values are output, and a predetermined position command value among the series of position command values is output. Position command means for outputting a predetermined approach speed as a speed set value corresponding to the position command value, and force detection means for detecting at least one of a force and a moment the tool receives from the external environment for each control cycle, When the detected value of the force detecting means is compared with a preset force threshold value for each control cycle, and the detected value continuously exceeds the force threshold value for a preset number of control cycles or more. The contact determination means for determining that the tool has contacted the object, and the speed / acceleration for changing the speed and acceleration of the manipulator based on the determination result of the contact determination means. The speed / acceleration changing means changes the speed of the manipulator to a predesignated work speed when the contact judging means judges that the tool has contacted the object. .

Further, in the fifth configuration, in the fourth configuration, the detected value of the force detecting means is compared with a second force threshold designated in advance for each control cycle, and the detected value is preset. The speed / acceleration changing means includes a departure determining means for determining that the tool has detached from the object when the second force threshold is continuously reached for a designated number of control cycles or more. Further, when it is determined that the tool has separated from the object by the separation determination means, the speed of the manipulator is increased to a predetermined separation speed.

[0030]

In the first configuration of the present invention, as shown in FIG.
As shown in, the contact determination means B detects the detected value F of the force or moment detected by the force detection means A for each control cycle.
s is fetched and compared with a preset force threshold Fth. Then, when the detected value Fs continuously becomes equal to or greater than the force threshold Fth for a preset number of control cycles m or more, it is determined that the tool has contacted the object. The change cycle number calculation means C uses a predesignated virtual spring constant change time Tw.
Is divided by a predetermined control cycle time ΔT, and the number N of changing cycles when changing the virtual spring constant is calculated. And
When the contact determination unit B determines that the tool has contacted the object, the virtual spring constant changing unit D determines a final spring constant KHend in which the virtual spring constant of the manipulator E is designated in advance.
Change in N control cycles.

As described above, according to the first configuration, the contact determining means B makes contact only when the detected value Fs continuously exceeds the force threshold Fth for a predetermined number of control cycles or more. Therefore, it is possible to prevent erroneous determination of contact due to noise in the detection signal of the force detection means, and it is possible to smoothly switch control modes. Also, since the control mode is switched from the position control mode to the force control mode by switching the virtual spring constant, the speed of the manipulator does not change suddenly and the control mode can be switched smoothly.

In the second configuration, as shown in FIG.
The approach judging means F compares the distance Sl detected by the distance detecting means with a distance threshold l designated in advance, and judges that the distance Sl is approaching when the distance Sl becomes equal to or less than the distance threshold l. When the approach determining means F determines that the manipulator is in the approaching state, the speed / acceleration changing means G changes the speed and acceleration of the manipulator from the values so far to a predetermined approaching speed, and the tool can gently contact the object. To do so. When the contact determining means B determines that the contact is made, the speed / acceleration changing means G increases the speed and acceleration of the manipulator to a predetermined work speed. With such a configuration, when it is determined that the manipulator is in the approaching state, the manipulator becomes slow, and the tool can be gently brought into contact with the target object, so that it is possible to prevent the tool from being caught or force overshooting. Conversely, even if the manipulator operation command according to the program or the teaching is first set to move at high speed when moving toward the object, according to this configuration, the tool causes the object to move. When a certain distance is approached, it is possible to automatically decelerate to a speed suitable for contact.

Further, in the third configuration, the virtual spring constant changing means D starts changing the virtual spring constant from the time when the approach determining means determines that the virtual spring constant is approaching until the tool comes into contact with the object. The virtual spring constant is changed to the virtual spring constant at the time of contact designated in advance. In this way, the virtual spring constant of the manipulator can be smoothly changed in a short time after the contact by bringing the virtual spring constant of the manipulator close to the final virtual spring constant to some extent before the tool comes into contact with the object. it can.

In the fourth configuration, first, the position command means outputs a series of position command values representing the trajectory of the manipulator and speed setting values corresponding to the position command values. At this time, after the predetermined position command value among the series of position command values, a predetermined approach speed is output as a speed set value corresponding to the position command value. This allows the manipulator to
The tool moves at a predetermined approach speed from a predetermined position on the way to the object.

When the contact determining means B determines that the contact is made, the speed / acceleration changing means changes the speed of the manipulator to a work speed designated in advance.

With this, the tool can be brought into contact with the object at a gentle approaching speed, so that it is possible to prevent the tool from being caught or force overshooting. Further, in the fifth configuration, the disengagement determination means compares the detection value of the force detection means with the second force threshold value, and the detection value is continuously applied to the second force for a predetermined number of control cycles or more. When it is below the threshold value, it is determined that the tool has detached from the object. Then, when the separation determination means determines that the tool has separated from the object, the speed / acceleration changing means increases the speed of the manipulator to a predetermined separation speed. As a result, the manipulator can be operated at high speed when the manipulator is separated from the target object, so that the work cycle can be shortened.

The configuration shown in the block diagram of FIG. 2 is obtained by adding the configuration of the present invention to the conventional configuration shown in FIG. Hereinafter, the operation of the manipulator control device according to the present invention will be described in more detail with reference to FIG. In addition, the same components as those in FIG. 5 are denoted by the same reference numerals, and the description thereof will be omitted.

(1) First, the case where the first configuration of the present invention is used will be described.

When the manipulator is used for the work, the operator first sets various control parameters, which will be described later, in the control parameter setting means 31. As such a parameter, for example, a value obtained by an experiment or a value obtained from the experience of the operator is used. Using this control parameter, the manipulator is specifically controlled as follows.

The detection value Fs of at least one of the force and the moment obtained from the force detecting means 4 attached to the tip of the arm portion 3 of the manipulator is the contact determining means 32.
Entered in. The contact determination means 32 compares the detected value Fs with the force threshold Fth preset by the control parameter setting means 31 for each control cycle. Then, the contact determination means 32 determines that the tool has contacted the object when the detected value Fs continuously exceeds the force threshold Fth for m cycles or more, and determines the contact detection signal as the virtual spring constant changing means 3
Send to 4. The number of cycles m for this determination is preset by the control parameter setting means 31.

The change cycle number calculation means 33 divides the virtual spring constant change time Tw preset by the control parameter setting means 31 by the control cycle time ΔT determined by the ability of the computer or the like used for controlling the manipulator to calculate the virtual spring. The number N of change cycles when changing the constant is determined.

Upon receipt of the contact detection signal from the contact determination means 32, the virtual spring constant changing means 34 starts changing the virtual spring constant. At this time, the virtual spring constant changing means 34
Is the final virtual spring constant KHend preset by the control parameter setting means 31 and the virtual spring constant current value buffer 3
The current value KH (cur) of the virtual spring constant stored in 9,
The change amount of the virtual spring constant per control cycle is calculated using the change cycle number N obtained by the change cycle number calculation means 33. Then, for each control cycle, the initial value KH of the virtual spring constant in that control cycle
The virtual spring constant KH (n + 1) in the next control cycle is obtained by adding the change amount to (n). The calculated virtual spring constant KH (n + 1) for the next cycle is output to the hand force variable gain calculating means 5, the command force variable gain calculating means 8 and the position deviation torque converting means 13. And
The hand force variable gain calculating means 5, the command force variable gain calculating means 8 and the position deviation torque converting means 13 are composed of the virtual spring constant KH from the virtual spring constant changing means 34 during the next cycle.
The virtual spring constant of the manipulator is changed by adjusting the feedback gain based on (n + 1).

In this way, the virtual spring constant of the manipulator is gradually changed to the final virtual spring constant KHend over N cycles from the time of contact determination.

According to these configurations, the contact can be determined with high accuracy, so that the control modes can be switched stably. Moreover, since the virtual spring constant is gradually changed, the control mode can be smoothly switched.

(2) Next, the case where the second and third configurations of the present invention are used will be described.

The second and third configurations are obtained by adding the approach determining means 38 and the speed / acceleration changing means 36 to the first configuration described above.

The distance Sl between the tool and the object is the arm 3
It is detected by a distance sensor attached to the. The detected distance Sl is input to the approach determination means 38. In the approach determining means 38, the distance threshold 1 previously set by the control parameter setting means 31 is compared with the distance Sl, and when the distance Sl becomes equal to or less than the distance threshold 1, the tool approaches the object. Then, the approach detection signal is output.

In the second configuration, this approach detection signal is input to the speed / acceleration changing means 36. Upon receiving the approach detection signal, the speed / acceleration changing means 36 lowers the speed / acceleration set value of the manipulator to a predetermined approach speed,
As a result, the manipulator is decelerated to a speed at which a shock does not occur at the time of contact. That is, when the speed / acceleration changing unit 36 receives the approach detection signal, the speed / acceleration changing unit 36 divides the speed / acceleration setting value given by the program or the teaching by the deceleration parameter x1 preset by the control parameter setting unit 31, It is passed to the speed setting unit 37 as a new speed / acceleration set value. In the position command means 11, the speed setting unit 3
The velocity / acceleration set values newly set in 7 are converted into position command values and used for controlling the manipulator. At this time, the deceleration parameter x1 is set in advance by the user in consideration of the speed designated by the program or the teaching and the approach speed determined by the characteristics of the manipulator.

In this way, when the approach determining means 38 determines that the manipulator is in the approaching state, the manipulator is decelerated and moves at a low speed until it comes into contact. After that, the contact determination means 3
When it is determined by 2 that the tool has contacted the object,
The contact detection signal is transmitted to the speed / acceleration changing means 36. Speed / acceleration changing means 3 that receives the contact detection signal
Reference numeral 6 denotes a speed setting unit 3 which multiplies a speed / acceleration set value when the manipulator approaches by a preset acceleration parameter x.
7, and the speed of the manipulator is increased to the working speed required for working. In this way, the manipulator can be gently contacted at a low speed at the time of contact, and can be moved at the speed required for the operation at the time of operation. Therefore,
It is possible to prevent biting of the tool and contact force overshoot.

Further, in the third configuration, the approach determining means 38
The approach detection signal emitted from the device is transmitted not only to the speed / acceleration changing means 36 but also to the virtual spring constant changing means 34. Upon receiving the approach detection signal, the virtual spring constant changing unit 34 changes the virtual spring constant to the contact-time virtual spring constant KHi set by the control parameter setting unit 31 in advance.
The virtual spring constant KHi at the time of contact is the final virtual spring constant K
The operator presets an intermediate value up to HEND as an appropriate value at the time of contact. The change of the virtual spring constant at the time of this approach is gradually performed as in the case of the above-described first configuration, but the operator changes the virtual spring constant change time at this time so as to match the distance threshold value l. Set. This virtual spring constant change time is divided by the control cycle time ΔT to obtain the number of change cycles, and the virtual spring constant KHi is gradually changed in the same manner as in the case of the above-described first configuration with the contact virtual spring constant KHi as the target value. To do.

Then, the change of the virtual spring constant after the tool comes into contact with the object is carried out in the same manner as in the case of the above-described first structure, starting from the virtual spring constant at contact KHi.

As described above, according to the third configuration, the virtual spring constant of the manipulator is brought close to the final virtual spring constant KHEND to some extent before the tool contacts the object. As a result, the change amount of the virtual spring constant after the contact determination can be reduced, so that the change of the virtual spring constant can be performed more smoothly.

(3) Next, the case where the fourth and fifth configurations of the present invention are used will be described.

The position command means 11 commands the manipulator based on a program, teaching, etc., to give a series of position command values representing the trajectory of the manipulator and speed set values corresponding to these position command values. At this time, the position command means 11 sets a speed set value from a certain predetermined position during the movement of the manipulator toward the target object to a predetermined approach speed among the series of position command values. This approach speed is
The speed is such that overshooting of force or biting of the tool does not occur when the tool contacts the object, and varies depending on the characteristics of each manipulator. Therefore, when the manipulator moves toward the target object, the manipulator is decelerated to the approach speed when it reaches a certain predetermined position on the way. Then, the manipulator moves at the approach speed until the contact determination unit 32 determines that the contact is made.

The detection value Fs obtained from the force detecting means 4 attached to the tip of the arm portion 3 of the manipulator is input to the contact determining means 32. The contact determination means 32 compares the detected value Fs with the force threshold Fth preset by the control parameter setting means 31 for each control cycle. Then, the contact determination means 32 determines that the tool has contacted the target object when the detected value Fs continuously exceeds the force threshold Fth for m cycles or more, and transmits a contact detection signal to the velocity / acceleration changing means 36. To do.

When the speed / acceleration changing means 36 receives the contact detection signal, the speed / acceleration at approaching is multiplied by the acceleration parameter x set in advance by the control parameter setting means 31 to obtain a new speed / acceleration set value. As speed setting unit 3
Pass to 7. In this way, the speed of the manipulator becomes
The speed is increased from the approach speed to a preset work speed.
As a result, the tool can be gently brought into contact with the object, so that it is possible to prevent the tool from being bitten in, overshoot of force, and the like.

In the fifth structure, a departure judging means is further provided. This separation determination means performs the opposite operation to the contact determination means described above. That is, the disengagement determination means compares the detected value Fs detected by the force detection means 4 with a second force threshold value for disengagement determination, and the detected value Fs continues for m ′ cycles or more to the second force. When it becomes less than or equal to the threshold value, it is determined that the tool has separated from the object, and a separation detection signal is transmitted to the speed / acceleration changing means 36. The number of cycles m ′ for this determination may be the same value as m for contact determination.

The speed / acceleration changing means 36 that has received the departure detection signal multiplies the speed / acceleration of the manipulator at the time of work by the acceleration parameter to obtain a new speed / acceleration set value, and the value is set to the speed setting unit 37. Pass to. The acceleration parameter at this time is preset by the control parameter setting means 31. According to this configuration, when the work at the time of contact is completed, the tool can be separated from the object at a high separation speed, so that the entire work cycle can be shortened.

[0059]

Embodiments of the present invention will be described with reference to the drawings.
FIG. 3 is a block diagram showing a manipulator control device according to the present invention. The apparatus shown in FIG. 3 embodies the configuration shown in FIG. Therefore, in FIG. 3, members corresponding to those in FIG. 2 are designated by the same reference numerals as those in FIG.

[First Embodiment] A first embodiment of the present invention will be described with reference to FIG. In the following description, first, the flow of feedback control of the entire manipulator control device will be described, and then a specific virtual spring constant changing mechanism will be described.

[0061] Feedback control of the wrist portion of the flow diagram 3 6-axis manipulator 1A tools receives from the external environment forces or moments or both (hereinafter, abbreviated as a force, etc.) is a six-axis force sensor 4A to measure It is installed. The detected value Fs of the 6-axis force sensor 4A is filtered by the force output filtering calculation means 41 for cutting signals exceeding the required frequency band in order to increase stability. For example, when the force output filtering calculation means 41 is a low-pass filter and the simplest temporary delay filter, the input h (s) and the output f
(S) performs an operation having a relationship of f (s) = 1 / (1 + Tc.s) .h (s). Here, s indicates the Laplace transform, and Tc indicates the time constant. The time constant Tc is arbitrarily set for each component according to the frequency to be cut.

The detected value Fs in the tool coordinate system that has been filtered by the force output filtering calculation means 41 is, in order to improve the degree of separation between the force loop and the position loop,
The manual force variable gain calculation means 5 multiplies the feedback gain Kffb. The feedback gain Kffb can be set as, for example, Kffb = FBFCT / (KH + FBFCT) using the virtual spring constant KH and the separation parameter FBFCT. Where the virtual spring constant K
Since H is small when the force control has a weight and is large when the position control has a weight, it becomes Kffb = 1 and 100% force feedback is applied in the complete force control mode of KH = 0. Then, since KH increases as the position control mode is entered, Kffb decreases ((<
1), force feedback will not be applied. The size of the separation parameter FBFCT can be arbitrarily set by the user for each component as necessary, in consideration of the virtual spring constant KH.

If the magnitude of the virtual spring constant KH is continuously changed, the control mode can be smoothly switched without causing a shock.

The output from the hand force variable gain calculating means 5 is then output by the output torque converting means 6 which multiplies the transposed matrix J ^T of the Jacobian matrix J determined by the current position and orientation of the manipulator, in the joint coordinate system of the manipulator. Is converted to a value such as.

The force command means 7 sends out a force command signal in which a force or the like is expressed in the tool coordinate system.

The force command signal from the force command means 7 is subjected to the same operation as the force output filtering operation means 41 by the force command filtering operation means 71 so that only the low frequency component is passed. This is to delay the command value as well as the force feedback delay. The command force variable gain calculation means 8 multiplies the force command signal sent from the force command filtering calculation means 71 by a force feedback gain according to the magnitude of the virtual spring constant, similarly to the hand force variable gain calculation means 5, to obtain a gain. Change. This is to make the magnitude of the feedback equal to the magnitude of the command value.

The command torque conversion means 9 performs the same calculation as the output torque conversion means 6 using the transposed Jacobian matrix J ^T , and outputs the output from the command force variable gain calculation means 8 to the command value in each joint coordinate system of the manipulator. Convert.

The position of each axis of the manipulator is detected by position detecting means 1 such as an encoder attached to the motor shaft.
It is performed by 0.

The command position of each axis of the manipulator is sent by the position command means 11.

The position deviation detecting means 12 calculates the position deviation obtained as the difference between the position signal output from the position detecting means 10 and the commanded position output from the position commanding means 11.

In this embodiment, the force control and the position control are adjusted by adjusting the virtual spring constant. That is, when the virtual spring constant KH is large (thus, the compliance is small), position control is performed, and when the virtual spring constant KH is small (the compliance is large), force control is performed. The virtual spring constant KH is set in the tool coordinate system. The position deviation torque converting means 13 converts the virtual spring constant KH set in the tool coordinate system into a virtual spring constant Kθ in each axis coordinate system of the manipulator, and outputs the position deviation signal from the position deviation detecting means 12. Is multiplied by a spring constant Kθ to be converted into a force or moment according to the position deviation.

The velocity detecting means 14 obtains the velocity of each axis of the manipulator by differentiating the position signal obtained from the position detecting means 10.

The speed torque converting means 15 converts the speed feedback gain KvH set in the tool coordinate system into the speed feedback gain at each joint of the manipulator,
This is multiplied by the speed signal output from the speed detecting means 14 to convert into a force or the like according to the speed of the manipulator.

The feedback compensation calculation means 16 adds the force command value output from the command torque conversion means 9 and the force command value corresponding to the position deviation output from the position deviation torque conversion means 13, and from the added value. The force detection value output from the output torque converting means 6 and the force detection value corresponding to the speed output from the speed torque converting means 15 are subtracted, and arithmetic processing such as proportionality and integration is performed on the deviation thereof, and each axis of the manipulator is processed. It is a command value such as force to the driving means.

The arithmetic expression of this control system is shown below. In the following, the case of using both force and moment will be described.

Now, the force and moment required to drive the manipulator in accordance with the command value are Fc (tool coordinate system).
Then, Fc is the force output from the force command variable gain calculation means 8 and the moment command Fi (tool coordinate system),
Target position Pi (world coordinate system) and current position P of the tool
Deviation from o (world coordinate system) Pi-Po (tool coordinate system) multiplied by virtual spring constant KH (tool coordinate system) and tool speed Vo (tool coordinate system) speed feedback gain KvH (tool coordinate) (Fc) = Fi + KH (Pi-Po) + KvH.Vo (1) The inertia term is ignored here. Convert Fc to torque τc of each axis of the manipulator. Here, when placing the Jacobian matrix of the manipulator and ^{J, τc = J T Fc =} J T Fi + J T KH (Pi -Po) + J T KvH · Vo ··· (2) where, ^T is denotes the transpose matrix. Further, by using the target value θi of each axis of the manipulator and the current value θo of each axis of the manipulator, Pi −Po = ΔP = JΔθ = J (θi −θo) (3) Vo = J · ωo (4) Can be approximated by Here, ωo is a time differential value of the present value θo of each axis of the manipulator. The formula (3) and (4) (2) to Substituting ^{τc = J T Fc = J T} Fi + J T KH J · (θi -θo) + J T KvHJ · ωo ·· (5). Here, we put the ^{τi = J T Fi Kθ = J} T KH J Kv θ = J T KvHJ, τc = τi + Kθ (θi -θo) + Kv θ · ωo ··· (6) , and the manipulator axes level Angles and speeds can be used.

Here, τi is a torque command value (level of each axis of the manipulator) Kθ: Virtual spring constant (level of each axis of the manipulator) Kv θ: Velocity feedback gain (level of each axis of the manipulator)

The position deviation torque converting means 13 calculates the Kθ (θi−θo) portion of the equation (6), and the speed torque converting means 15 calculates the Kvθ · ωo portion of the equation (6). There is.

In the command torque conversion means 9, the output of the command force variable gain calculation means 8 is Fi and the calculation of τi = J ^T Fi is performed.

The output torque converting means 6 calculates τo = J ^T Fo with the output of the hand force variable gain calculating means 5 as Fo.

The servo system of each axis of the manipulator is a torque loop, and the feedback compensation calculation means 16
Is a torque command value τc of each axis of the manipulator obtained by addition and subtraction calculation using the outputs of the command torque conversion, position deviation torque conversion, and speed torque conversion means, and the output torque conversion means 6 The feedback control calculation is performed so that the output torque τo of each joint of the manipulator obtained by the above becomes equal to τc. In the feedback control calculation, proportional / integral control can be performed by the proportional gain Kp and the integral gain KI. Furthermore, it is also possible to add phase compensation and the like if necessary. Also, gravity torque compensation Cg according to the position and orientation of the manipulator can be added.

The calculation result obtained by the feedback compensation calculation becomes the command value of at least one of the force and the moment to the driving means of the manipulator. This command value is converted into a motor current value and given as a motor current command value when the motor driver 171 and the motor are used as the driving means. The motor drives the arm portion of each joint of the manipulator through the reduction gear, and finally the manipulator can perform a desired movement. The signal resulting from the movement of the manipulator is detected by the position detecting means and the force detecting means, and is input as a feedback signal to the next calculating means, which is repeatedly calculated.

As described above, the virtual spring constant KH is calculated by the hand force variable gain calculating means 5 and the command force variable gain calculating means 8
Is used to control the feedback gain Kffb in (1) (see equation (2)). Also, the virtual spring constant KH is
It is used as a spring constant when the position deviation torque converting means 13 converts the position deviation into a torque command value according to the position deviation. By gradually changing this virtual spring constant KH, the rigidity of the manipulator is changed,
The weights of the position control mode and the force control mode can be arbitrarily changed.

Change of Virtual Spring Constant In the feedback control system as described above, in this embodiment, the virtual spring constant is changed as follows.
Switch the control mode. The process of changing the virtual spring constant is shown in the flowchart of FIG.

First, the 6-axis force sensor 4A detects the force and / or the moment received by the tool for each control cycle, and outputs the detected value Fs. Contact determination means 3
2 receives the detection value Fs from the 6-axis force sensor 4A (S
10) The detected value Fs and the force threshold Fth preset by the control parameter setting means 31 are compared for each control cycle using the following equation (a) (S12).

Fs ≧ Fth (7) Then, the contact determination means 32 increments the count value CNT by 1 when the expression (7) is satisfied (S14), and clears the count value CNT when not satisfied (S14). S2
6). Therefore, the count value CNT is cleared to 0 when the expression (7) is not continuously satisfied.

Further, in the contact determination means 32, the count value CNT is compared with the contact determination reference number m preset by the control parameter setting means 31 using the equation (8) (S).
16).

CNT ≧ m (8) Then, when the equation (8) is satisfied, the contact determination means 32 determines that the tool has contacted the object, and issues a contact detection signal. On the contrary, when the expression (8) is not satisfied, it is determined that the position control mode is continued, and the contact detection signal is not issued.

The parameters Fth and m used in the contact determination means 32 are set by the user by the control parameter setting means 31 before the work. At this time, the force threshold Fth is set. It is necessary to set it in consideration of the hardness of the target surface, the overshoot of the force at the time of contact, and the like. Regarding the touch determination reference number m, the degree of noise of the 6-axis force sensor 4A and the magnitude of the force threshold Fth. It is necessary to set it in consideration. For example, the contact determination reference count m
In the setting of, if m is too large, the contact time in the position control mode becomes long, so that the tool is likely to bite, and conversely, if m is too small, erroneous determination due to sensor noise is likely to occur. Therefore, at the time of setting, it is necessary to determine the value of m in consideration of these trade-offs. As such a set value, for example, a value obtained by experiments or the like, a value obtained from empirical knowledge of the user, or the like is used.

When the equation (8) is satisfied, the contact determining means 32
The contact detection signal output from is input to the virtual spring constant changing means 34. When the contact detection signal is received, the virtual spring constant changing means 34 starts changing the virtual spring constant.

Here, the change cycle number calculation means 33 is
From the virtual spring constant change time Tw preset by the control parameter setting means 31 and the predetermined control cycle time ΔT determined by the ability of the computer, the number N of change cycles when changing the virtual spring constant according to the equation (9). Is calculated (S18).

N = Tw / ΔT (9) It should be noted that this Tw is set to an appropriate length so that a shock or the like does not occur in the operation of the manipulator when the virtual spring constant is changed. The value of Tw is set based on, for example, an experiment or empirical knowledge of the user.

At the start of changing the virtual spring constant, the virtual spring constant changing means 34 reads the current value KH (cur) of the virtual spring constant from the virtual spring constant current value buffer 39, and at the same time outputs the current value KH (cur). , The final virtual spring constant KHEND set in advance by the control parameter setting means 31, and the change cycle number N obtained by the change cycle number calculation means 33.
By using the following equation (10), the virtual spring constant change amount ΔKH per control cycle is obtained (S20).

ΔKH = (KHEND−KH (cur)) / N (10) Here, the virtual spring constant current value buffer 39 detects the current value of the virtual spring constant from the position deviation torque converting means 13. .

The final virtual spring constant KHEND is an optimum value of the virtual spring constant when a tool, such as a grinder, comes into contact with an object to perform the work. Set based on empirical knowledge. For example, in the case of grinding work with a grinder, KHE
Set it to ND = 0 (N / cm).

Then, for each control cycle, the virtual spring constant in the next control cycle is calculated from the initial value K (n) of the virtual spring constant in the control cycle and the virtual spring constant change amount ΔKH using the following equation (11). Calculate KH (n + 1) (S2
2).

KH (n + 1) = KH (n) + ΔKH (11) This is repeated N times which is the number of change cycles. In this embodiment, in this way, the virtual spring constant of the manipulator is gradually changed to the final virtual spring constant KHEND over the virtual spring constant changing time Tw.

Hand force variable gain calculating means 5, command force variable gain calculating means 8, and position deviation torque converting means 13
Controls the manipulator by adjusting the gain for each control cycle based on the virtual spring constant KH (n + 1) of the next control cycle (S24).

As described above, according to this embodiment, the reliability of contact judgment is improved, and the control mode can be switched from position control to force control in a stable manner. Further, even when the force threshold Fth is set to be a little rough rather than the optimum value, highly reliable contact determination can be performed. Furthermore, since the control mode is switched by changing the virtual spring constant, the speed of the manipulator does not change suddenly, and the control mode can be switched smoothly.

Here, the virtual spring constant change amount ΔK
The fixed value obtained from Eq. (10) was used as H, but as another method, the current value of the virtual spring constant is detected for each control cycle, and the current value and the final virtual spring constant and its control cycle The virtual spring constant change amount may be obtained from the remaining number of change cycles.

In the present embodiment, the virtual spring constant changing means 36 is provided with a function of changing the virtual spring constant based on a preset position command value, and when the tool is separated from the object, the position command value is changed. It is also possible to change the virtual spring constant based on

In this case, for example, when the final point of the position command value during the contact work of the tool is P (n), the tool passes the previous passing point P (n-1) to the final point P (n). Change the virtual spring constant while moving to. The virtual spring constant changing time Tw at this time is defined by the following equation (12) using the moving speed v of the tool during the contact work.

Tw = (P (n) -P (n-1)) / v (12) Then, the virtual spring constant changing means 34 uses the Tw obtained by the equation (12) to change the cycle. Find the number and use equations (10) and (11)
Based on, the virtual spring constant is gradually changed as in the case of contact. However, in this case, since the mode control is switched from the force control to the position control, the virtual spring constant is changed from a small value to a large value. In this way, even when the tool is released, by changing the control mode by changing the virtual spring constant, the virtual spring is gradually contracted from the fully extended state, so that the tool can be smoothly released from the object. You can

At the time of disengagement, the tool disengagement is determined by counting the number of times the detected value Fs falls below the force threshold Fth, contrary to the contact determination means, and the virtual spring constant is determined from the time of disengagement. May be changed.

[Second Embodiment] Next, a second embodiment of the present invention will be described with reference to FIG.

In the second embodiment, the distance sensor 4B, the approach determining means 38, and the speed / acceleration changing means 36 are added to the apparatus of the first embodiment described above.

In the second embodiment, the distance sensor 4B
Is attached near the tool at the tip of the arm of the manipulator. And before starting work, the distance sensor 4B
Adjust the zero point between the measurement origin and the tool end.

The detected value Sl of the distance between the tool end and the object detected by the distance sensor 4B is input to the approach determining means 38.

The approach determination means 38 compares the distance detection value Sl with the distance threshold value l preset by the control parameter setting means 31 by the following equation (13).

Sl ≤ l (13) Then, when the above expression (13) is satisfied, the approach determining means 38 determines that the tool has approached the object and issues an approach detection signal. The approach detection signal is input to the speed / acceleration changing means 36 and the virtual spring constant changing means 34. Hereinafter, the operations of the speed / acceleration changing means 36 and the virtual spring constant changing means 34 will be described in order.

When the speed / acceleration changing means 36 receives the approach detection signal, the speed setting value of the manipulator which has been given in advance by a program, teaching or the like is set to the deceleration parameter x1 set in advance by the control parameter setting means 31.
By dividing by, the deceleration is changed to a predetermined approach speed range. This deceleration parameter x1 is for controlling the speed of the manipulator so that overshooting or biting may occur when the tool contacts the target object. Considering the speed of the manipulator before approaching, etc. The user sets the appropriate value.

The speed setting value changed by the speed / acceleration changing means 36 is set as a new speed setting value in the speed setting section 37 in the position command means 11. The position command means 11 generates a new position command value by performing an integral operation on the new speed set value set by the speed setting means 37, and the position command value is the position command in the position command means 11. Output from the department. The movement of the manipulator is controlled based on this position command value.

In this way, when the approach determining means 38 determines that the manipulator is in the approaching state, the manipulator is decelerated, and thereafter, the manipulator moves at a low speed until the tool contacts the object.

When the contact determining means 32 determines that the tool has contacted the object, the contact determining means 32 transmits a contact detection signal to the speed / acceleration changing means 36. Upon receiving the contact detection signal, the speed / acceleration changing means 36 multiplies the speed set value when the manipulator is in the approaching state by the speed increasing parameter x set in advance by the control parameter setting means 31 to increase the speed to a predetermined work speed. Change speed. The speed setting value changed by the speed / acceleration changing unit 36 is passed to the speed setting unit 37 in the position command unit 11 as a new speed setting value. Then, the position command means 11
A position command value is generated from this speed setting value. The movement of the manipulator is controlled based on this position command value. At this time, the acceleration a of the manipulator may be simultaneously increased by using the acceleration parameter x. This is done, for example, by multiplying the acceleration a by x ² .

In this way, after the contact judgment, the speed of the manipulator is accelerated to a speed suitable for the work at the time of contact.

On the other hand, when the virtual spring constant changing means 34 receives the approach detection signal, it starts changing the virtual spring constant with the contact virtual spring constant KHi preset by the control parameter setting means 31 as the target value. The contact virtual spring constant KHi is set in advance by the user to an appropriate value between the virtual spring constant in the position control mode and the final virtual spring constant KHEND. In the change of the virtual spring constant in the approaching state, when the tool contacts the object, the virtual spring constant of the manipulator is changed to the contact virtual spring constant KHi. That is, the virtual spring constant is changed according to the equations (9), (10) and (11) in the same manner as in the first embodiment between the time when the tool is in the approaching state and the time when the tool comes into contact with the object. . However, in this case, since the virtual spring constant of the manipulator at the time of contact is the virtual spring constant KHi at the time of contact, the virtual spring constant change time Tw is equal to the distance threshold l and the speed and acceleration of the manipulator at the time of approach. Needs to be decided based on This virtual spring constant change time Tw may be obtained by calculation from the distance threshold value 1 and the speed and acceleration of the manipulator in the approaching state in the control unit, or may be set in advance by the user using the control parameter setting means 31. It may be set to a value.

In this way, the virtual spring constant is changed to KHi during the approach. As a result, the tool comes into contact with the object while the virtual spring constant of the manipulator is KHi. The virtual spring constant after contact is changed in the same manner as in the first embodiment.

As described above, according to this embodiment, the speed of the manipulator is low when the tool contacts the object, and the virtual spring constant is small to some extent. It is possible to prevent impact and force overshoot on the tool and biting of the tool at the time of contact. In addition, since the virtual spring constant is close to the final virtual spring constant KHEND at the time of contact, the change amount of the virtual spring constant after contact can be reduced and the change time of the virtual spring constant after contact can be shortened.

Further, as another control method using the approach determining means in the present embodiment, the passing point taught near the landing point, the tool end position data, the approach speed preset by the parameter setting means, and the like are set. It may be used to recreate the position command value between the tool end position and the landing point, and bring the manipulator closer to the new position command value.

In the present embodiment, the speed / acceleration changing means 36 starts changing the speed and acceleration at the same time when it is determined that they are approaching or touching, but the invention is not limited to this. Among the passing points of the manipulator set in, the change of the velocity and the acceleration may be started from each passing point immediately after the approach determination and immediately after the contact determination. In this case, since the velocity and acceleration can be changed according to the passing point set first, it is necessary to recreate the trajectory of the manipulator as in the case of starting the change between passing points. Absent.

[Third Embodiment] A third embodiment of the present invention will be described with reference to FIG.

The position command means 11 commands the manipulator based on a program, teaching, etc., to output a series of position command values representing the trajectory of the manipulator and speed set values corresponding to these position command values. Further, the position command means 11
Among the series of position command values, the manipulator sets a speed set value from a predetermined position on the way to the object to a predetermined approach speed. This approach speed is
The speed is such that overshoot of force and biting of the tool do not occur when the tool contacts the object. The value of the approach speed depends on the characteristics of the manipulator and the type of work to be performed. Therefore, when the manipulator moves toward the target object, the manipulator is decelerated to the approach speed when it reaches a certain predetermined position on the way. Then, the manipulator moves at the approach speed until the contact determination unit 32 determines that the contact is made. At this time, the manipulator slowly approaches the object at the approaching speed by position control.

At this time, the detected value Fs of the force acting on the tool portion is always detected by the 6-axis force sensor 4A every cycle, and the detected value Fs is higher than the force threshold Fth in the contact determination means 32. It is compared in equation (7). Hereinafter, the contact determination means 32 determines the contact of the tool in the same manner as in the first embodiment. When it is determined that the tool has contacted the object, the contact determination means 32 transmits a contact detection signal to the speed / acceleration changing means 36.

Upon receiving the contact detection signal, the speed / acceleration changing means 36 increases the speed of the manipulator to the speed required for the work. That is, the speed / acceleration changing means 36 multiplies the approach speed of the manipulator by the speed increasing parameter x set in advance by the control parameter setting means 31 to speed up and change the speed set value to a predetermined work speed. The acceleration parameter x is set by the user in consideration of the approach speed of the manipulator and the working speed.

The speed setting value changed by the speed / acceleration changing means 36 is used as a new speed setting value in the position command means 11
It is passed to the speed setting unit 37 inside. Then, the position command means 11 generates a position command value from this speed set value. The movement of the manipulator is controlled based on this position command value. At this time, at the same time, the manipulator acceleration a
May be increased by using the acceleration parameter x. This is done, for example, by multiplying the acceleration a by x ² .

In this way, the manipulator can perform work at a desired work speed when it comes into contact with the object.

In this embodiment, the speed of the manipulator is controlled even when the tool is separated from the object after the work is completed. This speed control is performed as follows. In this case, the contact determination means 32 is used as the separation determination means by using the following expression (14) instead of the expression (7) as the determination expression in the contact determination means 32.

Fs ≦ Fth ′ (14) Here, Fs is a detection value obtained from the 6-axis force sensor 4A, and Fth ′ is a force threshold value for determination of separation. As Fth ', the same value as the force threshold Fth at the time of contact determination may be used.

Then, the above equation (14) is determined for each control cycle, and when the above equation (14) is continuously satisfied for m ′ control cycles or more, it is determined that the tool has separated from the object, A departure detection signal is transmitted to the acceleration changing means 36. Here, m ′ is set by the user in advance by the control parameter setting means 31, and may be the same value as m used in the contact determination in the first embodiment.

When the speed / acceleration changing means 36 receives the departure detection signal, the speed / acceleration changing means 36 multiplies the speed set value during the operation of the manipulator by the speed increasing parameter x'preset by the control parameter setting means 31 to change the speed. . The speed setting value changed by the speed / acceleration changing unit 36 is passed to the speed setting unit 37 in the position command unit 11 as a new speed setting value. Then, the position command means 11 generates a position command value from this speed set value. The movement of the manipulator is controlled based on this position command value. At this time, the acceleration a of the manipulator may be simultaneously increased by using the acceleration parameter x ′. This is carried out, for example, by multiplying the ^x'2 against acceleration a.

As described above, according to this embodiment, the speed of the manipulator at the time of contact can be reduced while satisfying the predetermined work speed, so that the tool can be gently contacted with the object, and the tool It is possible to prevent bite and overshoot of force. Further, by increasing the speed of the manipulator at the same time when the tool detachment is detected, the series of work cycles of the manipulator can be shortened.

In the present embodiment, the change of the speed and acceleration of the manipulator by the speed / acceleration changing means 36 is started at the same time when the contact judging means 32 judges that the contact is made, but not limited to this, for example, Similar to the case of the second embodiment, of the manipulator passing points set in advance by a program, teaching, etc., the change of the velocity and the acceleration is started from the respective passing points immediately after the approach determination and immediately after the contact determination. You may Preferably, by providing a passing point which passes first after landing on a slightly inner side of the target surface, the speed / acceleration can be quickly changed after switching the control mode, and the working speed can be satisfied in various works. In this case, since the velocity and acceleration can be changed according to the passing point set first, it is necessary to recreate the trajectory of the manipulator as in the case of starting the change between passing points. Absent.

[Brief description of drawings]

FIG. 1 is a principle configuration diagram of the present invention.

FIG. 2 is a block diagram showing a configuration of a manipulator control device according to the present invention.

FIG. 3 is a block diagram showing an embodiment of the present invention.

FIG. 4 is a flowchart showing a processing process in an embodiment of the present invention.

FIG. 5 is a block diagram showing a configuration of a conventional manipulator control device.

[Explanation of symbols]

 A force detection means B contact determination means C change cycle number calculation means D virtual spring constant change means E manipulator F approach determination means G speed / acceleration change means

Claims (5)

[Claims] 1. A manipulator control device for controlling a manipulator for performing a desired work by bringing a tool into contact with an object, wherein at least one of a force and a moment received by the tool from an external environment is detected in each control cycle. Force detecting means for comparing the detected value of the force detecting means with a force threshold value for each control cycle, and the detected value is continuously equal to or more than the force threshold value for a predetermined number of control cycles or more. Contact determination means for determining that the tool has contacted the object in the case of, and a change cycle when changing the virtual spring constant of the manipulator based on a preset virtual spring constant change time and a predetermined control cycle time. Change cycle number calculation means for calculating the number, from the point of time determined to be contact by the contact determination means,
A manipulator control device comprising: a virtual spring constant changing unit that changes the virtual spring constant of the manipulator to a predetermined final virtual spring constant at the number of change cycles. 2. The manipulator control device according to claim 1, further comprising: distance detecting means for detecting a distance between the tool and the object, the detected distance and a distance threshold specified in advance. Comparing, the approach determination means for determining the approach state when the distance is equal to or less than the distance threshold, the speed of the manipulator based on the determination result of at least one of the approach determination means and the contact determination means and Speed / acceleration changing means for changing the acceleration, wherein the speed / acceleration changing means changes the speed of the manipulator to a predetermined approach speed when the approach judging means judges the approaching state, and the contact judging means. A manipulator control device, characterized in that the speed of the manipulator is changed to a predetermined work speed when the contact is determined by the. 3. The manipulator control device according to claim 2, wherein the virtual spring constant changing unit is between a time point when the approach determining unit determines that the state is approaching and a time period when the contact determining unit determines that the state is a contact state. , A manipulator control device, characterized in that the virtual spring constant of the manipulator is changed to a virtual spring constant at the time of contact designated in advance. 4. A device for controlling a manipulator which brings a tool into contact with an object to perform a desired work, and outputs a series of position command values representing a trajectory of the manipulator and speed set values corresponding to the position command values. Along with
After a predetermined position command value among the series of position command values, position command means for outputting a predetermined approach speed as a speed set value corresponding to the position command value, and the tool receives from the external environment for each control cycle. A force detection means for detecting at least one of a force and a moment, and a control cycle in which the detection value of the force detection means is compared with a preset force threshold value for each control cycle, and the detection value is designated in advance. A number of consecutive contact determination means for determining that the tool has contacted the object when the force threshold value is exceeded or more, and a speed for changing the speed and acceleration of the manipulator based on the determination result of the contact determination means. And an acceleration changing unit, wherein the speed / acceleration changing unit determines the speed of the manipulator when the contact determining unit determines that the tool has contacted the object. Manipulator control apparatus characterized by changing the pre-specified work rate. 5. The manipulator control device according to claim 4, further comprising: comparing a detection value of the force detection means with a second force threshold designated in advance for each control cycle, and detecting the detection value. Includes a detachment determination unit that determines that the tool has detached from the object when is continuously less than or equal to the second force threshold value for a predetermined number of control cycles or more, and the speed / acceleration changing unit includes The manipulator control device further increases the speed of the manipulator to a predetermined separation speed when the separation determination means determines that the tool has separated from the object.