JP2019188520A

JP2019188520A - Reproduction device

Info

Publication number: JP2019188520A
Application number: JP2018083019A
Authority: JP
Inventors: 祐輝枝川; Yuki Edakawa
Original assignee: Azbil Corp
Current assignee: Azbil Corp
Priority date: 2018-04-24
Filing date: 2018-04-24
Publication date: 2019-10-31

Abstract

To make it possible to realize an operation according to preset external force and position without breaking an object and reducing a speed.SOLUTION: The reproduction device comprises: an actuator 1; a move part 3 which moves the actuator 1; a position detection part 4 which detects a position of a movable part 102 to a stationary part 101; an acceleration detection part 5 which detects an acceleration of the stationary part 101; an actuator control part 61 which adjusts a gain for a difference between the detected position and a reference position Pr, and outputs a driving current Ia on the basis of a current instruction value Irp as an adjustment result and the acceleration detected by the acceleration detection part 5; an external force detection part 62 which detects an external force F added to the movable part 102 on the basis of the current instruction value Irp, or the detected acceleration and a current value of the driving current Ia; and a work control part 11 which controls the actuator control part 61 and the move part 3 on the basis of the detected external force and position according to reference data which indicates a time sequence of the external force and position.SELECTED DRAWING: Figure 1

Description

この発明は、事前に設定された外力及び位置に従った動作を実現する再現装置に関する。 The present invention relates to a reproduction apparatus that realizes an operation according to a preset external force and position.

従来から、ロボットの作業教示方法に関し、ロボットの物体への接触圧を力データとして記録し、力センサの力の検出値がその力データと一致するように接触圧を制御する方法が提案されている（例えば特許文献１〜３参照）。これにより、熟練者の力加減を正確に伝え、再現することができる。 Conventionally, with respect to a robot work teaching method, a method has been proposed in which the contact pressure of a robot on an object is recorded as force data, and the contact pressure is controlled so that the detected force value of the force sensor matches the force data. (For example, refer to Patent Documents 1 to 3). Thereby, it is possible to accurately convey and reproduce the skill level of the skilled person.

特開平７−２２２６１９号公報JP-A-7-222619 特開２０１５−８５４９６号公報JP2015-85496A 特開２０１７−２１７７３８号公報JP 2017-217738 A

しかしながら、特許文献１〜３に開示された方法のように、力センサを用いた場合、物体を傷つけたり壊したりすることを防ぐためにはロボットの移動速度を十分落とす必要ある。そのため、再現できる作業の速度に限界が生じ、実際の作業とは異なる速度の作業しか再現できない。 However, when the force sensor is used as in the methods disclosed in Patent Documents 1 to 3, it is necessary to sufficiently reduce the moving speed of the robot in order to prevent the object from being damaged or broken. Therefore, there is a limit to the speed of work that can be reproduced, and only work at a speed different from the actual work can be reproduced.

この発明は、上記のような課題を解決するためになされたもので、物体を壊さず且つ速度を落とさずに、事前に設定された外力及び位置に従った動作を実現可能とする再現装置を提供することを目的としている。 The present invention has been made to solve the above-described problem, and provides a reproduction apparatus that can realize an operation according to a preset external force and position without breaking an object and reducing the speed. It is intended to provide.

この発明に係る再現装置は、固定部、及び当該固定部に対して変位可能な可動部を有するアクチュエータと、アクチュエータを移動する移動部と、固定部に対する可動部の位置を検出する位置検出部と、固定部の加速度を検出する加速度検出部と、位置検出部により検出された位置と基準位置との差分に対してゲインを調整し、当該調整結果である電流指令値及び加速度検出部により検出された加速度に基づいてアクチュエータに対する駆動電流を出力するアクチュエータ制御部と、アクチュエータ制御部において得られた電流指令値、又は、加速度検出部により検出された加速度及びアクチュエータ制御部により出力された駆動電流の電流値に基づいて、可動部に加わる外力を検出する外力検出部と、可動部に加わる外力及びアクチュエータの位置の時系列を示す基準データに従い、外力検出部により検出された外力及び位置検出部により検出された位置に基づいてアクチュエータ制御部及び移動部を制御する作業制御部とを備えたことを特徴とする。 The reproduction apparatus according to the present invention includes a fixed portion, an actuator having a movable portion that can be displaced with respect to the fixed portion, a moving portion that moves the actuator, and a position detection portion that detects the position of the movable portion with respect to the fixed portion. , An acceleration detection unit that detects the acceleration of the fixed unit, and a gain that is adjusted for the difference between the position detected by the position detection unit and the reference position, and is detected by the current command value and the acceleration detection unit that are the adjustment results Actuator control unit that outputs drive current to the actuator based on the measured acceleration, current command value obtained by the actuator control unit, acceleration detected by the acceleration detection unit, and current of the drive current output by the actuator control unit An external force detector that detects an external force applied to the movable part based on the value, an external force applied to the movable part, and an actuator And an operation control unit that controls the actuator control unit and the moving unit based on the external force detected by the external force detection unit and the position detected by the position detection unit according to the reference data indicating the time series of the positions of And

この発明によれば、上記のように構成したので、物体を壊さず且つ速度を落とさずに、事前に設定された外力及び位置に従った動作を実現可能となる。 According to this invention, since it comprised as mentioned above, it becomes possible to implement | achieve the operation | movement according to the preset external force and position, without destroying an object and reducing speed.

この発明の実施の形態１に係る再現装置の構成例を示す図である。It is a figure which shows the structural example of the reproduction apparatus which concerns on Embodiment 1 of this invention. この発明の実施の形態１における外力検出制御部の構成例を示す図である。It is a figure which shows the structural example of the external force detection control part in Embodiment 1 of this invention. この発明の実施の形態１におけるゲイン調整部の構成例を示す図である。It is a figure which shows the structural example of the gain adjustment part in Embodiment 1 of this invention. この発明の実施の形態１に係る再現装置による再現動作の一例を示すフローチャートである。It is a flowchart which shows an example of reproduction operation | movement by the reproduction apparatus which concerns on Embodiment 1 of this invention. 図５Ａ、図５Ｂは、この発明の実施の形態１に係る再現装置による再現動作の一例を示す図である。5A and 5B are diagrams showing an example of the reproduction operation by the reproduction apparatus according to Embodiment 1 of the present invention. この発明の実施の形態２に係る再現装置の構成例を示す図である。It is a figure which shows the structural example of the reproduction apparatus which concerns on Embodiment 2 of this invention. 図７Ａ、図７Ｂは、この発明の実施の形態２に係る再現装置による再現動作の一例を示す図である。7A and 7B are diagrams showing an example of a reproduction operation by the reproduction apparatus according to Embodiment 2 of the present invention.

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る再現装置の構成例を示す図である。
再現装置は、事前に設定された外力Ｆ及び位置に従った動作（再現動作）を行う装置である。ここでは、位置は、並進成分であるＸ，Ｙ，Ｚ及び回転成分であるθ_Ｘ，θ_Ｙ，θ_Ｚから成る６自由度のうちの１つ以上を含む。この再現装置は、図１に示すように、アクチュエータ１、エンドエフェクタ２、移動部３、位置検出部４、加速度検出部５、外力検出制御部６、モード設定部７、位置検出部（第２位置検出部）８、第１データ記録部９、第２データ記録部１０及び作業制御部１１を備えている。また、外力検出制御部６は、アクチュエータ制御部６１及び外力検出部６２から構成される。図１ではモード設定部７の図示を省略している。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1 FIG.
FIG. 1 is a diagram showing a configuration example of a reproduction apparatus according to Embodiment 1 of the present invention.
The reproduction device is a device that performs an operation (reproduction operation) according to an external force F and a position set in advance. Here, the position includes one or more of six degrees of freedom composed of X, Y, Z as translation components and θ _X , θ _Y , θ _{Z as} rotation components. As shown in FIG. 1, the reproduction apparatus includes an actuator 1, an end effector 2, a moving unit 3, a position detecting unit 4, an acceleration detecting unit 5, an external force detection control unit 6, a mode setting unit 7, a position detecting unit (second detecting unit). (Position detection unit) 8, first data recording unit 9, second data recording unit 10, and work control unit 11. The external force detection control unit 6 includes an actuator control unit 61 and an external force detection unit 62. In FIG. 1, the mode setting unit 7 is not shown.

アクチュエータ１は、固定部１０１、及び当該固定部１０１に対して変位可能な可動部１０２を有し、磁界に置かれたコイルに電流が供給されることで固定部１０１に対して可動部１０２を直動方向又は回転方向に変位可能とする。このアクチュエータ１は、移動部３に取付けられることで、全体が移送され、また、姿勢が変更される。なお、アクチュエータ１は移動部３に対して着脱可能である。 The actuator 1 includes a fixed portion 101 and a movable portion 102 that can be displaced with respect to the fixed portion 101, and the current is supplied to a coil placed in a magnetic field so that the movable portion 102 is moved relative to the fixed portion 101. Displaceable in the linear motion direction or rotational direction. The actuator 1 is attached to the moving unit 3 so that the entire actuator 1 is transferred and the posture is changed. The actuator 1 can be attached to and detached from the moving unit 3.

エンドエフェクタ２は、可動部１０２に取付けられる。図１では、エンドエフェクタ２として、接触した物体５０に対して研磨を行うエンドエフェクタが用いられている。 The end effector 2 is attached to the movable part 102. In FIG. 1, the end effector 2 is an end effector that polishes the contacted object 50.

移動部３は、アクチュエータ１を移動（移送及び姿勢変更）する。図１では、移動部３として、先端にアクチュエータ１（固定部１０１）が取付けられ、アクチュエータ１を移動可能なロボットを示している。 The moving unit 3 moves (transfers and changes the posture) the actuator 1. In FIG. 1, as the moving unit 3, a robot is shown in which the actuator 1 (fixed unit 101) is attached to the tip and the actuator 1 can be moved.

位置検出部４は、アクチュエータ１に設けられ、固定部１０１に対する可動部１０２の位置（相対位置）を検出する。この位置検出部４により検出された位置を示す信号（位置信号）は、アクチュエータ制御部６１及び作業制御部１１に出力される。 The position detection unit 4 is provided in the actuator 1 and detects the position (relative position) of the movable unit 102 with respect to the fixed unit 101. A signal (position signal) indicating the position detected by the position detector 4 is output to the actuator controller 61 and the work controller 11.

加速度検出部５は、固定部１０１に設けられ、固定部１０１の加速度を検出する。この際、加速度検出部５は、固定部１０１の重力加速度αｇ及び移動加速度α１のうちの一方、又は両方が加算された加速度（αｇ＋α１）を検出する。図２では、加速度検出部５が加速度（αｇ＋α１）を検出する場合を示している。この加速度検出部５により検出された加速度を示す信号（加速度信号）は、アクチュエータ制御部６１に出力される。 The acceleration detection unit 5 is provided in the fixed unit 101 and detects the acceleration of the fixed unit 101. At this time, the acceleration detector 5 detects an acceleration (αg + α1) obtained by adding one or both of the gravitational acceleration αg and the moving acceleration α1 of the fixed unit 101. FIG. 2 shows a case where the acceleration detection unit 5 detects acceleration (αg + α1). A signal (acceleration signal) indicating the acceleration detected by the acceleration detector 5 is output to the actuator controller 61.

アクチュエータ制御部６１は、位置検出部４により検出された位置と基準位置Ｐｒとの差分に対してゲイン（ループゲイン）を調整し、当該調整結果である電流指令値Ｉｒｐ及び加速度検出部５により検出された加速度に基づいてアクチュエータ１に対する駆動電流Ｉａを出力する。 The actuator controller 61 adjusts the gain (loop gain) with respect to the difference between the position detected by the position detector 4 and the reference position Pr, and is detected by the current command value Irp and the acceleration detector 5 as the adjustment result. A drive current Ia for the actuator 1 is output based on the acceleration thus generated.

外力検出部６２は、アクチュエータ制御部６１において得られた電流指令値Ｉｒｐ、又は、加速度検出部５により検出された加速度及びアクチュエータ制御部６１により出力された駆動電流Ｉａの電流値に基づいて、可動部１０２に加わる外力（反力）Ｆを検出する。この外力検出部６２により検出された外力Ｆを示す信号は、第１データ記録部９及び作業制御部１１に出力される。
アクチュエータ制御部６１及び外力検出部６２の構成例については後述する。 The external force detection unit 62 is movable based on the current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection unit 5 and the current value of the drive current Ia output by the actuator control unit 61. An external force (reaction force) F applied to the unit 102 is detected. A signal indicating the external force F detected by the external force detection unit 62 is output to the first data recording unit 9 and the work control unit 11.
Configuration examples of the actuator control unit 61 and the external force detection unit 62 will be described later.

モード設定部７は、再現装置の動作モードを教示モード又は再現モードに設定する。教示モードは、ユーザが再現装置にアクチュエータ１の動作を教示するための動作モードである。再現モードは、再現装置がユーザにより教示されたアクチュエータ１の動作を再現する動作モードである。また、モード設定部７は、ユーザ指示に応じて動作モードの設定を行う。 The mode setting unit 7 sets the operation mode of the reproduction device to the teaching mode or the reproduction mode. The teaching mode is an operation mode for the user to teach the operation of the actuator 1 to the reproduction apparatus. The reproduction mode is an operation mode in which the reproduction apparatus reproduces the operation of the actuator 1 taught by the user. The mode setting unit 7 sets an operation mode in response to a user instruction.

位置検出部８は、アクチュエータ１に設けられ、アクチュエータ１の位置（絶対位置）を検出する。この位置検出部８により検出された位置を示す信号（位置信号）は、第２データ記録部１０に出力される。 The position detector 8 is provided in the actuator 1 and detects the position (absolute position) of the actuator 1. A signal (position signal) indicating the position detected by the position detection unit 8 is output to the second data recording unit 10.

第１データ記録部９は、モード設定部７により動作モードが教示モードに設定されている場合に、外力検出部６２により検出された外力Ｆの時系列データを記録する。 The first data recording unit 9 records time-series data of the external force F detected by the external force detection unit 62 when the operation mode is set to the teaching mode by the mode setting unit 7.

第２データ記録部１０は、モード設定部７により動作モードが教示モードに設定されている場合に、位置検出部８により検出された位置の時系列データを記録する。 The second data recording unit 10 records time-series data of the position detected by the position detection unit 8 when the operation mode is set to the teaching mode by the mode setting unit 7.

作業制御部１１は、外力検出部６２により検出された外力Ｆに基づいて、アクチュエータ制御部６１及び移動部３を制御する。また、作業制御部１１は、モード設定部７により動作モードが再現モードに設定されている場合に、第１データ記録部９及び第２データ記録部１０により記録された時系列データを基準データとして取得し、当該基準データに従い、外力検出部６２により検出された外力Ｆ及び位置検出部４により検出された位置に基づいて、アクチュエータ制御部６１及び移動部３を制御することで、再現動作を実現する。なお、作業制御部１１は、基準位置Ｐｒ又はゲインの変更を行うことでアクチュエータ制御部６１を制御する。後述するゲイン調整部６５は位置偏差に基づいて電流指令値Ｉｒｐを出力しているが、上記ゲインの変更とは、上記位置偏差と電流指令値Ｉｒｐとの関係を示す関数の変更を意味している。また、上記関数の変更には、関数の傾きの変更も含まれる。また、作業制御部１１は、外力検出部６２により検出された外力Ｆ及び位置検出部４により検出された位置に加え、加速度検出部５により検出された加速度、及び作業制御部１１で管理している時間等も考慮して、上記再現動作を実現してもよい。 The work control unit 11 controls the actuator control unit 61 and the moving unit 3 based on the external force F detected by the external force detection unit 62. Further, when the operation mode is set to the reproduction mode by the mode setting unit 7, the work control unit 11 uses the time series data recorded by the first data recording unit 9 and the second data recording unit 10 as reference data. Based on the acquired external force F detected by the external force detection unit 62 and the position detected by the position detection unit 4 according to the reference data, the actuator control unit 61 and the moving unit 3 are controlled to realize the reproduction operation. To do. The work control unit 11 controls the actuator control unit 61 by changing the reference position Pr or the gain. A gain adjusting unit 65, which will be described later, outputs a current command value Irp based on the position deviation. The change in gain means a change in a function indicating the relationship between the position deviation and the current command value Irp. Yes. Moreover, the change of the function includes a change of the function inclination. In addition to the external force F detected by the external force detector 62 and the position detected by the position detector 4, the work controller 11 manages the acceleration detected by the acceleration detector 5 and the work controller 11. The above reproduction operation may be realized in consideration of the remaining time.

次に、外力検出制御部６の構成例について、図２を参照しながら説明する。なお図２では、アクチュエータ１、エンドエフェクタ２、位置検出部４及び加速度検出部５も図示している。
外力検出制御部６は、図２に示すように、位置速度変換部６３、減算器６４、ゲイン調整部６５、質量推定部６６、加速度補償部６７、加減算器６８、定電流制御部６９、及び外力検出部６２を有している。なお図２に示す外力検出制御部６において、外力検出部６２を除く機能部（位置速度変換部６３、減算器６４、ゲイン調整部６５、質量推定部６６、加速度補償部６７、加減算器６８及び定電流制御部６９）は、アクチュエータ制御部６１を構成する。 Next, a configuration example of the external force detection control unit 6 will be described with reference to FIG. In FIG. 2, the actuator 1, the end effector 2, the position detection unit 4, and the acceleration detection unit 5 are also illustrated.
As shown in FIG. 2, the external force detection control unit 6 includes a position / velocity conversion unit 63, a subtractor 64, a gain adjustment unit 65, a mass estimation unit 66, an acceleration compensation unit 67, an adder / subtractor 68, a constant current control unit 69, and An external force detector 62 is provided. In the external force detection control unit 6 shown in FIG. 2, the function units (position / speed conversion unit 63, subtractor 64, gain adjustment unit 65, mass estimation unit 66, acceleration compensation unit 67, adder / subtractor 68, and the external force detection unit 62 are excluded. The constant current control unit 69) constitutes an actuator control unit 61.

位置速度変換部６３は、位置検出部４により検出された位置を微分して速度に変換する。この速度は、固定部１０１に対する可動部１０２の速度（相対速度）を示す。この位置速度変換部６３により変換された速度を示す信号（速度信号）は、加減算器６８に出力される。 The position / velocity conversion unit 63 differentiates the position detected by the position detection unit 4 and converts it into a speed. This speed indicates the speed (relative speed) of the movable part 102 with respect to the fixed part 101. A signal (speed signal) indicating the speed converted by the position speed conversion unit 63 is output to the adder / subtractor 68.

減算器６４は、基準位置Ｐｒから位置検出部４により検出された位置を減算する。この減算器６４による減算結果を示す信号は、ゲイン調整部６５に出力される。 The subtracter 64 subtracts the position detected by the position detector 4 from the reference position Pr. A signal indicating the result of subtraction by the subtractor 64 is output to the gain adjustment unit 65.

ゲイン調整部６５は、減算器６４による減算結果（位置偏差）に対してゲインを調整し、電流指令値Ｉｒｐを出力する。ゲインは、アクチュエータ１におけるコンプライアンスの値であり、コンプライアンスは、ばね定数の逆数であり、固さ柔らかさを示す指標である。また、ゲイン調整部６５において、上記位置偏差と電流指令値Ｉｒｐとの関係を示す関数は線形でもよいし非線形でもよい。このゲイン調整部６５は、図２，３に示すように、ループゲイン測定部６５１、ゲイン交点制御部６５２及び可変ゲイン調整部６５３を有している。 The gain adjusting unit 65 adjusts the gain with respect to the subtraction result (positional deviation) by the subtractor 64 and outputs a current command value Irp. The gain is a compliance value in the actuator 1, and the compliance is the reciprocal of the spring constant and is an index indicating hardness and softness. In the gain adjusting unit 65, the function indicating the relationship between the position deviation and the current command value Irp may be linear or non-linear. As shown in FIGS. 2 and 3, the gain adjustment unit 65 includes a loop gain measurement unit 651, a gain intersection control unit 652, and a variable gain adjustment unit 653.

ループゲイン測定部６５１は、減算器６４から出力された信号のゲインを測定する。この際、ループゲイン測定部６５１は、図３に示すように、減算器６４から出力された信号に、発振器６５４によりゲインが１倍（０ｄＢ）となるべき基準となる周波数、すなわちゲイン交点に設定された基準となる周波数の正弦波を、加算器６５５を介して加算する。このループゲイン測定部６５１による正弦波の加算前後の信号は、ゲイン交点制御部６５２に出力される。 The loop gain measurement unit 651 measures the gain of the signal output from the subtracter 64. At this time, as shown in FIG. 3, the loop gain measuring unit 651 sets the signal output from the subtractor 64 to a reference frequency at which the gain should be 1 (0 dB) by the oscillator 654, that is, the gain intersection. The sine wave having the reference frequency is added via an adder 655. The signals before and after the addition of the sine wave by the loop gain measuring unit 651 are output to the gain intersection control unit 652.

ゲイン交点制御部６５２は、図３に示すように、比較器６５６によりループゲイン測定部６５１による正弦波の加算前後の信号での振幅比を比較する。このゲイン交点制御部６５２による比較結果を示す信号は、可変ゲイン調整部６５３に出力される。 As shown in FIG. 3, the gain intersection control unit 652 compares the amplitude ratios of the signals before and after the addition of the sine wave by the loop gain measurement unit 651 by the comparator 656. A signal indicating the comparison result by the gain intersection control unit 652 is output to the variable gain adjustment unit 653.

可変ゲイン調整部６５３は、ゲイン交点制御部６５２により比較された振幅比の倍率が１となるように、当該振幅比の倍率の逆数を調整値とし、減算器６４から出力された信号のゲインを調整する。すなわち、可変ゲイン調整部６５３は、ループゲイン測定部６５１による正弦波の加算前の信号の振幅レベルＥａに対して当該正弦波の加算後の信号の振幅レベルＥｂが高い場合（Ｅａ＜Ｅｂ）には調整値を大きくし、当該正弦波の加算前の信号の振幅レベルＥａに対して当該正弦波の加算後の信号の振幅レベルＥｂが低い場合（Ｅａ＞Ｅｂ）には調整値を小さくすることで、ゲインが１倍となるように調整する。この可変ゲイン調整部６５３によりゲインが調整された信号は、加減算器６８に電流指令値Ｉｒｐとして出力される。また、可変ゲイン調整部６５３によるゲインの調整値を示す信号は、質量推定部６６に出力される。 The variable gain adjustment unit 653 sets the gain of the signal output from the subtractor 64 as an adjustment value so that the magnification of the amplitude ratio compared by the gain intersection control unit 652 is 1, adjust. That is, the variable gain adjustment unit 653 has a higher amplitude level Eb of the signal after the addition of the sine wave than the amplitude level Ea of the signal before the addition of the sine wave by the loop gain measurement unit 651 (Ea <Eb). Increases the adjustment value and decreases the adjustment value when the amplitude level Eb of the signal after addition of the sine wave is lower than the amplitude level Ea of the signal before addition of the sine wave (Ea> Eb). Then, the gain is adjusted to be 1. The signal whose gain is adjusted by the variable gain adjusting unit 653 is output to the adder / subtractor 68 as a current command value Irp. In addition, a signal indicating the gain adjustment value by the variable gain adjustment unit 653 is output to the mass estimation unit 66.

なお、発振器６５４でゲインが１倍となるべき基準となる周波数の正弦波を加算するのは、ゲインが１倍となる周波数においてＥａ／Ｅｂ＝１となるため、Ｅａ／Ｅｂ＝１となるようにゲインを調整することで、ゲイン交点を常に１に維持できるためである。 The reason why the sine wave having the reference frequency that should be multiplied by 1 is added by the oscillator 654 is that Ea / Eb = 1 at the frequency at which the gain is multiplied by 1, so that Ea / Eb = 1. This is because the gain intersection can always be maintained at 1 by adjusting the gain.

また、減算器６４及びゲイン調整部６５は、位置検出部４により検出された位置と基準位置Ｐｒとの差分に基づく電流指令値Ｉｒｐを出力する位置制御手段（位相制御ループ）を構成する。 The subtractor 64 and the gain adjustment unit 65 constitute position control means (phase control loop) that outputs a current command value Irp based on the difference between the position detected by the position detection unit 4 and the reference position Pr.

質量推定部６６は、可変ゲイン調整部６５３によるゲインの調整値から、可動部１０２側の質量を推定する。すなわち、質量推定部６６は、ゲインの調整値の変化と質量の変化とが比例する原理を利用する。可動部１０２側の質量とは、可動部１０２を含む可動部１０２より先の構成の総質量である。ここでは、可動部１０２側の質量とは、可動部１０２の質量Ｍ１とエンドエフェクタ２の質量Ｍ２とが加算された質量（Ｍ１＋Ｍ２）である。
例えば、可動部１０２側の質量が規定値の２倍になったとすると、ゲインはその逆数倍の１／２となっており、Ｅａ／Ｅｂ＝１／２となる。これに対して、ゲインを１倍とするため、可変ゲイン調整部６５３は２倍の調整値でゲインを調整する。そして、質量推定部６６は、この可変ゲイン調整部６５３の調整値から、可動部１０２側の質量が規定値の２倍に変化したと推定できる。
この質量推定部６６により推定された質量を示す信号は、加速度補償部６７に出力される。 The mass estimation unit 66 estimates the mass on the movable unit 102 side from the gain adjustment value by the variable gain adjustment unit 653. That is, the mass estimation unit 66 uses the principle that the change in the gain adjustment value is proportional to the change in the mass. The mass on the movable part 102 side is the total mass of the configuration ahead of the movable part 102 including the movable part 102. Here, the mass on the movable unit 102 side is a mass (M1 + M2) obtained by adding the mass M1 of the movable unit 102 and the mass M2 of the end effector 2.
For example, if the mass on the movable part 102 side is twice the specified value, the gain is 1/2 of the reciprocal number, and Ea / Eb = 1/2. On the other hand, in order to make the gain 1 time, the variable gain adjustment unit 653 adjusts the gain with a double adjustment value. The mass estimation unit 66 can estimate from the adjustment value of the variable gain adjustment unit 653 that the mass on the movable unit 102 side has changed to twice the specified value.
A signal indicating the mass estimated by the mass estimation unit 66 is output to the acceleration compensation unit 67.

なお上記では、質量推定部６６により可動部１０２側の質量を推定する場合を示したが、これに限らず、他の方法を用いて可動部１０２側の質量を示す情報を取得してもよい。 In addition, although the case where the mass by the mass estimation part 66 estimates the mass by the side of the movable part 102 was shown above, not only this but the information which shows the mass by the side of the movable part 102 may be acquired using another method. .

加速度補償部６７は、外乱トルクを補正するための加速度補償値Ｉｒｃを出力する。この加速度補償部６７は、乗算器６７１及び係数乗算部６７２を有している。 The acceleration compensation unit 67 outputs an acceleration compensation value Irc for correcting the disturbance torque. The acceleration compensation unit 67 includes a multiplier 671 and a coefficient multiplication unit 672.

乗算器６７１は、加速度検出部５により検出された加速度と、質量推定部６６により推定された質量とを乗算する。この乗算器６７１による乗算結果を示す信号は、係数乗算部６７２及び外力検出部６２に出力される。 The multiplier 671 multiplies the acceleration detected by the acceleration detection unit 5 and the mass estimated by the mass estimation unit 66. A signal indicating a multiplication result by the multiplier 671 is output to the coefficient multiplier 672 and the external force detector 62.

係数乗算部６７２は、乗算器６７１による乗算結果に係数（１／Ｋｔ）を乗算する。なお、Ｋｔは、アクチュエータ１が発生する推力と駆動電流Ｉａとの比を表したトルク定数である。この係数乗算部６７２による乗算結果を示す信号は、加減算器６８に加速度補償値Ｉｒｃとして出力される。 The coefficient multiplication unit 672 multiplies the multiplication result by the multiplier 671 by a coefficient (1 / Kt). Kt is a torque constant that represents the ratio between the thrust generated by the actuator 1 and the drive current Ia. A signal indicating the multiplication result by the coefficient multiplier 672 is output to the adder / subtractor 68 as an acceleration compensation value Irc.

加減算器６８は、ゲイン調整部６５から出力された電流指令値Ｉｒｐに対し、加速度補償部６７から出力された加速度補償値Ｉｒｃを加算し、位置速度変換部６３から出力された速度信号を減算する。この加減算器６８による加減算結果を示す信号は、定電流制御部６９に電流指令値Ｉｒとして出力される。 The adder / subtracter 68 adds the acceleration compensation value Irc output from the acceleration compensation unit 67 to the current command value Irp output from the gain adjustment unit 65 and subtracts the velocity signal output from the position / velocity conversion unit 63. . A signal indicating the result of addition / subtraction by the adder / subtractor 68 is output to the constant current control unit 69 as a current command value Ir.

定電流制御部６９は、アクチュエータ１を駆動する駆動電流Ｉａを電流指令値Ｉｒに一致させるように制御する。この定電流制御部６９は、減算器６９１、駆動ドライバ６９２及び電流検出部６９３を有している。 The constant current control unit 69 controls the drive current Ia for driving the actuator 1 so as to coincide with the current command value Ir. The constant current control unit 69 includes a subtracter 691, a drive driver 692, and a current detection unit 693.

減算器６９１は、加減算器６８から出力された電流指令値Ｉｒから、電流検出部６９３により検出された駆動電流Ｉａの電流値を減算する。この減算器６９１による減算結果を示す信号は、駆動ドライバ６９２に出力される。 The subtractor 691 subtracts the current value of the drive current Ia detected by the current detector 693 from the current command value Ir output from the adder / subtractor 68. A signal indicating the result of subtraction by the subtractor 691 is output to the drive driver 692.

駆動ドライバ６９２は、減算器６９１による減算結果に応じた駆動電流Ｉａを発生する。この駆動ドライバ６９２により発生された駆動電流Ｉａは、電流検出部６９３を介してアクチュエータ１に出力される。 The drive driver 692 generates a drive current Ia corresponding to the result of subtraction by the subtractor 691. The drive current Ia generated by the drive driver 692 is output to the actuator 1 via the current detector 693.

電流検出部６９３は、駆動ドライバ６９２により発生された駆動電流Ｉａの電流値を検出する。この電流検出部６９３により検出された電流値を示す信号は、減算器６９１に出力される。 The current detection unit 693 detects the current value of the drive current Ia generated by the drive driver 692. A signal indicating the current value detected by the current detector 693 is output to the subtractor 691.

外力検出部６２は、アクチュエータ制御部６１において得られた電流指令値Ｉｒｐ、又は、加速度検出部５により検出された加速度及びアクチュエータ制御部６１により出力された駆動電流Ｉａの電流値に基づいて、可動部１０２に加わる外力Ｆを検出する。具体的には、外力検出部６２は、電流指令値Ｉｒｐ、又は、駆動電流Ｉａの電流値から加速度補償値Ｉｒｃを減算した結果に基づいて、可動部１０２に加わる外力Ｆを検出する。なお、可動部１０２に加わる外力Ｆとしては、エンドエフェクタ２が物体５０と接触した際に発生する力が挙げられる。また図２では、外力検出部６２が、加速度検出部５により検出された加速度及びアクチュエータ制御部６１により出力された駆動電流Ｉａの電流値に基づいて可動部１０２に加わる外力Ｆを検出する場合を示している。図２に示す外力検出部６２は、係数乗算部６２１、減算器６２２及び係数乗算部６２３を有している。 The external force detection unit 62 is movable based on the current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection unit 5 and the current value of the drive current Ia output by the actuator control unit 61. The external force F applied to the part 102 is detected. Specifically, the external force detection unit 62 detects the external force F applied to the movable unit 102 based on the current command value Irp or the result of subtracting the acceleration compensation value Irc from the current value of the drive current Ia. An example of the external force F applied to the movable unit 102 is a force generated when the end effector 2 comes into contact with the object 50. In FIG. 2, the external force detection unit 62 detects the external force F applied to the movable unit 102 based on the acceleration detected by the acceleration detection unit 5 and the current value of the drive current Ia output by the actuator control unit 61. Show. The external force detection unit 62 illustrated in FIG. 2 includes a coefficient multiplication unit 621, a subtracter 622, and a coefficient multiplication unit 623.

係数乗算部６２１は、加速度補償部６７の乗算器６７１による乗算結果に係数（１／Ｋｔ）を乗算する。この係数乗算部６２１による乗算結果を示す信号は、減算器６２２に出力される。 The coefficient multiplication unit 621 multiplies the multiplication result by the multiplier 671 of the acceleration compensation unit 67 by a coefficient (1 / Kt). A signal indicating the multiplication result by the coefficient multiplier 621 is output to the subtractor 622.

減算器６２２は、定電流制御部６９により発生された駆動電流Ｉａの電流値から、係数乗算部６２１による乗算結果を減算する。この減算器６２２による減算結果を示す信号は、係数乗算部６２３に出力される。 The subtractor 622 subtracts the multiplication result by the coefficient multiplication unit 621 from the current value of the drive current Ia generated by the constant current control unit 69. A signal indicating the result of subtraction by the subtractor 622 is output to the coefficient multiplier 623.

係数乗算部６２３は、減算器６２２による減算結果に係数（Ｋｔ）を乗算することで、外力Ｆを得る。この係数乗算部６２３により得られた外力Ｆを示す信号は、第１データ記録部９及び作業制御部１１に出力される。 The coefficient multiplying unit 623 obtains an external force F by multiplying the result of subtraction by the subtractor 622 by a coefficient (Kt). A signal indicating the external force F obtained by the coefficient multiplication unit 623 is output to the first data recording unit 9 and the work control unit 11.

なお、外力検出部６２が、アクチュエータ制御部６１において得られた電流指令値Ｉｒｐに基づいて可動部１０２に加わる外力Ｆを検出する場合には、係数乗算部を有する。この係数乗算部は、ゲイン調整部６５から出力された電流指令値Ｉｒｐに係数（Ｋｔ）を乗算することで、外力Ｆを得る。そして、この係数乗算部により得られた外力Ｆを示す信号は、第１データ記録部９及び作業制御部１１に出力される。 When the external force detection unit 62 detects the external force F applied to the movable unit 102 based on the current command value Irp obtained in the actuator control unit 61, the external force detection unit 62 includes a coefficient multiplication unit. The coefficient multiplication unit obtains an external force F by multiplying the current command value Irp output from the gain adjustment unit 65 by a coefficient (Kt). A signal indicating the external force F obtained by the coefficient multiplication unit is output to the first data recording unit 9 and the work control unit 11.

次に、外力検出制御部６の動作原理について説明する。なお以下では、アクチュエータ１として、発生した推力がエンドエフェクタ２に直接伝わるダイレクトドライブ形式のリニアアクチュエータを用い、固定部１０１に対して可動部１０２を直動させるものとする。このアクチュエータ１は、定電流制御部６９が電流指令値Ｉｒに応じて発生した駆動電流Ｉａにより駆動する。 Next, the operation principle of the external force detection control unit 6 will be described. In the following description, it is assumed that the actuator 1 is a direct drive type linear actuator in which the generated thrust is directly transmitted to the end effector 2, and the movable portion 102 is linearly moved with respect to the fixed portion 101. The actuator 1 is driven by the drive current Ia generated by the constant current control unit 69 according to the current command value Ir.

一方、位置検出部４は、固定部１０１に対する可動部１０２の直動方向における位置を検出する。
また、位置速度変換部６３は、位置検出部４により検出された位置を微分して速度に変換する。この速度は、固定部１０１に対する可動部１０２の速度を示す。 On the other hand, the position detection unit 4 detects the position of the movable unit 102 in the linear movement direction with respect to the fixed unit 101.
Further, the position / velocity conversion unit 63 differentiates the position detected by the position detection unit 4 and converts it into a speed. This speed indicates the speed of the movable part 102 with respect to the fixed part 101.

また、加速度検出部５は、固定部１０１の直動方向における加速度を検出する。以下では、加速度検出部５は、固定部１０１の直動方向成分における移動加速度α１と、固定部１０１の直動方向成分における重力加速度αｇとが加算された加速度（α１＋αｇ）を検出するものとする。 Further, the acceleration detection unit 5 detects the acceleration of the fixed unit 101 in the linear motion direction. Hereinafter, it is assumed that the acceleration detection unit 5 detects an acceleration (α1 + αg) obtained by adding the movement acceleration α1 in the linear motion direction component of the fixed unit 101 and the gravitational acceleration αg in the linear motion direction component of the fixed unit 101. .

また、位置検出部４により検出された位置は、減算器６４で基準位置Ｐｒと比較され、その差分がゲイン調整部６５を介して電流指令値Ｉｒを構成する要素の一つである電流指令値Ｉｒｐとして加減算器６８に与えられる。 The position detected by the position detector 4 is compared with the reference position Pr by the subtractor 64, and the difference is a current command value that is one of the elements constituting the current command value Ir via the gain adjuster 65. It is given to the adder / subtractor 68 as Irp.

電流指令値Ｉｒは、電流指令値Ｉｒｐの他、外乱トルクを補正するための加速度補償値Ｉｒｃで構成され、次式（１）で表される。
Ｉｒ＝Ｉｒｐ＋Ｉｒｃ（１） The current command value Ir is composed of an acceleration compensation value Irc for correcting disturbance torque in addition to the current command value Irp, and is represented by the following equation (1).
Ir = Irp + Irc (1)

なお、位置を単純にフィードバックすると制御系が不安定となる。そのため、実際には、位置速度変換部６３からの速度信号をマイナーループとして加減算器６８のマイナス出力に加えて安定化を行っているが、以下では省略する。 If the position is simply fed back, the control system becomes unstable. Therefore, in practice, the speed signal from the position / speed converter 63 is added as a minor loop to the minus output of the adder / subtractor 68 for stabilization.

また、ゲイン調整部６５では、位置制御ループのゲインを変えることで、アクチュエータ１におけるコンプライアンスの値を変化させることができる。 The gain adjustment unit 65 can change the compliance value in the actuator 1 by changing the gain of the position control loop.

ここで、駆動電流Ｉａに着目すると、外乱トルクがない場合には電流値は零になるが、外乱トルクがある場合にはそれに比例して電流値も変化する。
一般的な外乱トルクとしては、作業時にエンドエフェクタ２から受ける反力Ｆ、重力加速度αｇ及び移動加速度α１により発生する力、減速器のロストルク等が考えられる。ここで、アクチュエータ１はダイレクトドライブ形式のリニアアクチュエータであるため、減速器は持たず、ロストルクは考慮する必要は少ない。したがって、駆動電流Ｉａは、作業時にエンドエフェクタ２から受ける反力Ｆ、重力加速度αｇ及び移動加速度α１により発生する力に比例した値となる。なお以下では、反力Ｆは、エンドエフェクタ２が物体５０に接触した際に発生する力であるとする。 Here, focusing on the drive current Ia, the current value becomes zero when there is no disturbance torque, but the current value also changes in proportion to the disturbance torque when there is disturbance torque.
As a general disturbance torque, a reaction force F received from the end effector 2 during work, a force generated by the gravitational acceleration αg and the movement acceleration α1, a loss torque of the speed reducer, and the like can be considered. Here, since the actuator 1 is a linear actuator of a direct drive type, it does not have a speed reducer, and there is little need to consider loss torque. Therefore, the drive current Ia has a value proportional to the reaction force F received from the end effector 2 during work, the force generated by the gravitational acceleration αg, and the movement acceleration α1. Hereinafter, the reaction force F is assumed to be a force generated when the end effector 2 contacts the object 50.

ここで、アクチュエータ１の駆動電流Ｉａ、作業時にエンドエフェクタ２から受ける反力Ｆ、固定部１０１の直動方向成分における移動加速度α１、固定部１０１の直動方向成分における重力加速度αｇ、可動部１０２の質量Ｍ１、及び、エンドエフェクタ２の質量Ｍ２から、次式（２）の関係が成り立つ。
Ｆ＋（α１＋αｇ）・（Ｍ１＋Ｍ２）＝Ｋｔ・Ｉｒ＝Ｋｔ・（Ｉｒｐ＋Ｉｒｃ）
（２）
なお、Ｋｔはアクチュエータ１が発生する推力と駆動電流Ｉａとの比を表したトルク定数である。 Here, the driving current Ia of the actuator 1, the reaction force F received from the end effector 2 during work, the movement acceleration α1 in the linear motion direction component of the fixed portion 101, the gravitational acceleration αg in the linear motion direction component of the fixed portion 101, the movable portion 102 From the mass M1 and the mass M2 of the end effector 2, the relationship of the following formula (2) is established.
F + (α1 + αg) · (M1 + M2) = Kt · Ir = Kt · (Irp + Irc)
(2)
Kt is a torque constant representing the ratio between the thrust generated by the actuator 1 and the drive current Ia.

また、式（２）において外乱トルクを補正するための加速度補償値Ｉｒｃを次式（３）のように設定する。
（α１＋αｇ）・（Ｍ１＋Ｍ２）＝Ｋｔ・Ｉｒｃ（３） Further, the acceleration compensation value Irc for correcting the disturbance torque in the equation (2) is set as the following equation (3).
(Α1 + αg) · (M1 + M2) = Kt · Irc (3)

式（３）のように加速度補償値Ｉｒｃを設定した場合、式（２）からα１，αｇ，Ｍ１，Ｍ２の項が消え、次式（４）のように整理される。
Ｆ＝Ｋｔ・Ｉｒｐ（４） When the acceleration compensation value Irc is set as in Expression (3), the terms α1, αg, M1, and M2 disappear from Expression (2) and are rearranged as in Expression (4) below.
F = Kt · Irp (4)

このように、外乱トルクを補正するための加速度補償値Ｉｒｃを式（３）のように設定すると、作業時にエンドエフェクタ２から受ける反力Ｆと電流指令値Ｉｒｐは、比例関係になることがわかる。 As described above, when the acceleration compensation value Irc for correcting the disturbance torque is set as shown in the equation (3), the reaction force F received from the end effector 2 during the work and the current command value Irp have a proportional relationship. .

これは、作業時にエンドエフェクタ２から受ける反力Ｆが零、つまりエンドエフェクタ２が物体５０と接触していない場合、基準位置Ｐｒと実際の位置の差分に基づく電流指令値Ｉｒｐも零、つまり位置が変位しないことを意味している。
そして、エンドエフェクタ２が物体５０と接触した際に生じる反力Ｆは、電流指令値Ｉｒｐを監視することで知ることができる。 This is because when the reaction force F received from the end effector 2 during work is zero, that is, when the end effector 2 is not in contact with the object 50, the current command value Irp based on the difference between the reference position Pr and the actual position is also zero. Means no displacement.
The reaction force F generated when the end effector 2 comes into contact with the object 50 can be known by monitoring the current command value Irp.

そして、式（４）には、固定部１０１の直動方向成分における移動加速度α１、固定部１０１の直動方向成分における重力加速度αｇ、可動部１０２の質量Ｍ１、エンドエフェクタ２の質量Ｍ２の項目が含まれていない。
つまり、ロボットが急激に移動又は停止を行い移動加速度α１が発生した場合、及び、ロボットが連続して姿勢を変更し重力加速度αｇが変化した場合でも、アクチュエータ１の可動部１０２はゆれることなく反力Ｆを正しく検出できる。
そして、コンプライアンスの値も自由に設定できる。 The expression (4) includes items of a movement acceleration α1 in the linear motion direction component of the fixed portion 101, a gravitational acceleration αg in the linear motion direction component of the fixed portion 101, a mass M1 of the movable portion 102, and a mass M2 of the end effector 2. Is not included.
That is, even when the robot suddenly moves or stops and the movement acceleration α1 occurs, and even when the robot continuously changes its posture and the gravitational acceleration αg changes, the movable portion 102 of the actuator 1 does not shake. Force F can be detected correctly.
The compliance value can also be set freely.

なお、上述したように、エンドエフェクタ２が物体５０と急激に衝突する等して発生する反力Ｆは、電流指令値Ｉｒｐを監視することで知ることができる。また、アクチュエータ１には、反力Ｆと拮抗するように誘導電流が発生するため、駆動電流Ｉａから反力Ｆを検出することもできる。
しかしながら、位置制御ループにおいて、反力Ｆに対する電流指令値Ｉｒｐの応答は一般的に速くない。一方、反力Ｆに対する駆動電流Ｉａの応答は、可動部１０２が移動することにより発生する誘導電流によるものであるため、比較的速い。そこで、電流指令値Ｉｒｐを直接監視するのではなく、駆動電流Ｉａを監視することで反力Ｆの検出を行う。 As described above, the reaction force F generated when the end effector 2 suddenly collides with the object 50 can be known by monitoring the current command value Irp. Further, since an induced current is generated in the actuator 1 so as to antagonize the reaction force F, the reaction force F can be detected from the drive current Ia.
However, in the position control loop, the response of the current command value Irp to the reaction force F is generally not fast. On the other hand, the response of the drive current Ia to the reaction force F is relatively fast because it is due to the induced current generated by the movement of the movable portion 102. Therefore, the reaction force F is detected not by directly monitoring the current command value Irp but by monitoring the drive current Ia.

ここで、式（２）は以下の通りである。
Ｆ＋（α１＋αｇ）・（Ｍ１＋Ｍ２）＝Ｋｔ・Ｉｒ＝Ｋｔ・（Ｉｒｐ＋Ｉｒｃ）
（２） Here, Formula (2) is as follows.
F + (α1 + αg) · (M1 + M2) = Kt · Ir = Kt · (Irp + Irc)
(2)

一方、駆動電流Ｉａは次式（５）で表せる。
Ｉａ＝Ｉｒ＝Ｉｒｐ＋Ｉｒｃ（５） On the other hand, the drive current Ia can be expressed by the following equation (5).
Ia = Ir = Irp + Irc (5)

よって、式（２），（５）から次式（６）が得られる。
Ｆ＋（α１＋αｇ）・（Ｍ１＋Ｍ２）＝Ｋｔ・Ｉａ（６） Therefore, the following equation (6) is obtained from the equations (2) and (5).
F + (α1 + αg) · (M1 + M2) = Kt · Ia (6)

そして、式（６）の両辺から、式（３）の左辺である（（α１＋αｇ）・（Ｍ１＋Ｍ２））を減算して整理すると、次式（７）が得られる。
Ｆ＝Ｋｔ・（Ｉａ−（α１＋αｇ）・（Ｍ１＋Ｍ２）／Ｋｔ）（７） Then, by subtracting ((α1 + αg) · (M1 + M2)), which is the left side of equation (3), from both sides of equation (6), the following equation (7) is obtained.
F = Kt · (Ia− (α1 + αg) · (M1 + M2) / Kt) (7)

この式（７）に示されるように、駆動電流Ｉａから加速度補償値（α１＋αｇ）・（Ｍ１＋Ｍ２）／Ｋｔを差し引いてトルク定数Ｋｔをかけることで、反力Ｆを求めることができる。 As shown in the equation (7), the reaction force F can be obtained by subtracting the acceleration compensation value (α1 + αg) · (M1 + M2) / Kt from the drive current Ia and applying the torque constant Kt.

次に、外力検出制御部６による効果について説明する。
ロボットの動作は、一般的に、位置制御によりコントロールされる。そのため、物体５０の寸法誤差等により、予めプログラムされた目標位置と実際の位置が異なる場合、エンドエフェクタ２が物体５０と接触した際に大きな外力Ｆが発生し、エンドエフェクタ２又は物体５０に傷又は破損が発生する恐れがある。 Next, effects of the external force detection control unit 6 will be described.
The operation of the robot is generally controlled by position control. Therefore, when the target position programmed in advance differs from the actual position due to a dimensional error of the object 50, a large external force F is generated when the end effector 2 comes into contact with the object 50, and the end effector 2 or the object 50 is damaged. Or damage may occur.

その対策として、ロボットとエンドエフェクタ２との間に力センサを設置し、エンドエフェクタ２と物体５０との接触時に過大な外力Ｆが発生しそうになると力センサの検出結果をロボットにフィードバックし、過大な外力Ｆが発生しないようにする方法が考えられる。 As a countermeasure, a force sensor is installed between the robot and the end effector 2, and if an excessive external force F is likely to occur when the end effector 2 and the object 50 are in contact with each other, the detection result of the force sensor is fed back to the robot. A method for preventing the external force F from being generated can be considered.

しかしながら、過大な外力Ｆが発生したことを検出して停止指令を出してもロボットは急には止まれないため、停止指令が出た時点から急激に減速しても接触位置からずれた位置で停止してしまい、物体５０を押し潰してしまう。そして、位置の行き過ぎ量は移動速度に比例するため、エンドエフェクタ２を物体５０に近付ける速度を遅くせざるを得ない。 However, even if a stop command is issued after detecting the occurrence of excessive external force F, the robot does not stop suddenly, so even if it decelerates suddenly from the time when the stop command is issued, it stops at a position that deviates from the contact position. As a result, the object 50 is crushed. Since the amount of overshoot of the position is proportional to the moving speed, the speed at which the end effector 2 is brought closer to the object 50 must be slowed down.

上記の理由により、エンドエフェクタ２が物体５０と接触する可能性のある領域では、ロボットの移動速度を十分落とす必要がある。しかしながら、サイクルタイムを短くするため、エンドエフェクタ２を移動する速度は速くする必要がある。その結果、接触領域の近傍で速度を急激に落とすことになる。 For the above reason, it is necessary to sufficiently reduce the moving speed of the robot in a region where the end effector 2 may come into contact with the object 50. However, in order to shorten the cycle time, the moving speed of the end effector 2 needs to be increased. As a result, the speed is drastically reduced in the vicinity of the contact area.

一方、実施の形態１では、ロボット（移動部３）の先端にアクチュエータ１を取付け、また、外力検出制御部６は、アクチュエータ１が急激に移動又は停止されて移動加速度α１が発生した場合、及び、アクチュエータ１の姿勢が変更されて重力加速度αｇが変化した場合でも、可動部１０２に加わる反力Ｆを正しく検出でき、また、コンプライアンス値を任意に変えられる。そのため、ロボットが急に止まれない点は同じだが、位置の行き過ぎにより物体５０を押し潰してしまうことはない。よって、エンドエフェクタ２を物体５０に近づける速度を極端に遅くする必要がなく、また、安全に作業できる。 On the other hand, in the first embodiment, the actuator 1 is attached to the tip of the robot (the moving unit 3), and the external force detection control unit 6 is configured to generate the movement acceleration α1 when the actuator 1 is suddenly moved or stopped. Even when the posture of the actuator 1 is changed and the gravitational acceleration αg is changed, the reaction force F applied to the movable portion 102 can be correctly detected, and the compliance value can be arbitrarily changed. Therefore, the point that the robot cannot stop suddenly is the same, but the object 50 is not crushed due to excessive position. Therefore, it is not necessary to extremely slow the speed at which the end effector 2 is brought close to the object 50, and the work can be performed safely.

また、ロボットとエンドエフェクタ２との間に力センサを設置した場合、ロボットが急激に減速すると、エンドエフェクタ２の質量Ｍ２による影響で、力センサには負方向の加速度に比例した力が発生する。
ところが、上記加速度に比例した力とエンドエフェクタ２の物体５０との接触により発生する外力Ｆとを区別することは難しく、区別するためにはロボットの減速時間を大幅に長くせざるを得ない。 Further, when a force sensor is installed between the robot and the end effector 2, when the robot rapidly decelerates, a force proportional to the negative acceleration is generated in the force sensor due to the influence of the mass M2 of the end effector 2. .
However, it is difficult to distinguish between the force proportional to the acceleration and the external force F generated by the contact of the object 50 of the end effector 2, and in order to do so, the deceleration time of the robot must be significantly increased.

一方、外力検出制御部６では、アクチュエータ１が急激に加減速された場合でも正しく外力Ｆを検出でき、接触時にのみ外力Ｆを検出するため、アクチュエータ１の減速時間を長くする必要はない。 On the other hand, the external force detection control unit 6 can correctly detect the external force F even when the actuator 1 is suddenly accelerated and decelerated, and detects the external force F only at the time of contact. Therefore, it is not necessary to lengthen the deceleration time of the actuator 1.

また、力センサを用いた場合には、重力による影響をリアルタイムに補償し難いという課題もある。
すなわち、再現動作を行う場合にロボットが取りうる姿勢は常に一定ではなく、作業の状態に応じて変化させる場合が多い。
しかしながら、ロボットとエンドエフェクタ２との間に力センサを設置した場合には、ロボットの姿勢が水平ではないと、力センサには重力加速度αｇによる影響でロボットの姿勢とエンドエフェクタ２の質量Ｍ２に応じた力が発生する。 In addition, when a force sensor is used, there is a problem that it is difficult to compensate for the influence of gravity in real time.
That is, the posture that the robot can take when performing the reproduction operation is not always constant, and is often changed according to the state of the work.
However, when a force sensor is installed between the robot and the end effector 2, if the posture of the robot is not horizontal, the force sensor is affected by the gravitational acceleration αg to the posture of the robot and the mass M2 of the end effector 2. A corresponding force is generated.

一方、外力検出制御部６では、アクチュエータ１の姿勢が変更されて重力加速度αｇが変化した場合でも外力Ｆを正しく検出できるため、重力による影響をリアルタイムに補償できる。 On the other hand, since the external force detection control unit 6 can correctly detect the external force F even when the attitude of the actuator 1 is changed and the gravitational acceleration αg changes, the influence of gravity can be compensated in real time.

次に、実施の形態１に係る再現装置による再現動作の一例について、図４，５を参照しながら説明する。なお、図５Ａに示すように、実施の形態１に係る再現装置では、熟練者等のユーザが再現装置に対してアクチュエータ１の動作を教示する際には、アクチュエータ１が移動部３から切離された状態とされる。
再現装置による再現動作例では、まず、モード設定部７は、ユーザ指示に従い、再現装置の動作モードを教示モードに設定する（ステップＳＴ１）。その後、熟練者等のユーザは、アクチュエータ１を持って所望の力及び位置で動かす。これにより、外力検出部６２は外力Ｆを検出し、また、位置検出部８はアクチュエータ１の位置を検出する。 Next, an example of a reproduction operation by the reproduction apparatus according to the first embodiment will be described with reference to FIGS. As shown in FIG. 5A, in the reproduction apparatus according to the first embodiment, when a user such as an expert teaches the operation of the actuator 1 to the reproduction apparatus, the actuator 1 is separated from the moving unit 3. It is assumed that it was done.
In the reproduction operation example by the reproduction device, first, the mode setting unit 7 sets the operation mode of the reproduction device to the teaching mode in accordance with a user instruction (step ST1). Thereafter, a user such as an expert moves the actuator 1 with a desired force and position while holding the actuator 1. As a result, the external force detector 62 detects the external force F, and the position detector 8 detects the position of the actuator 1.

次いで、第１データ記録部９は、外力検出部６２により検出された外力Ｆの時系列データを記録する（ステップＳＴ２）。
また、第２データ記録部１０は、位置検出部８により検出された位置の時系列データを記録する（ステップＳＴ３）。
以上の動作により、再現装置は外力Ｆの時系列データ及び位置の時系列データを基準データとして記録できる。その後、図５Ｂに示すように、再現装置は、アクチュエータ１が移動部３に取付けられた状態とされる。 Next, the first data recording unit 9 records time series data of the external force F detected by the external force detection unit 62 (step ST2).
The second data recording unit 10 records time-series data of the position detected by the position detection unit 8 (step ST3).
With the above operation, the reproduction apparatus can record the time series data of the external force F and the time series data of the position as reference data. Thereafter, as shown in FIG. 5B, the reproduction apparatus is in a state where the actuator 1 is attached to the moving unit 3.

その後、モード設定部７は、ユーザ指示に従い、再現装置の動作モードを再現モードに設定する（ステップＳＴ４）。
次いで、作業制御部１１は、第１データ記録部９及び第２データ記録部１０により記録された時系列データを基準データとして取得し、当該基準データに従い、外力検出部６２により検出された外力Ｆ及び位置検出部４により検出された位置に基づいて、アクチュエータ制御部６１及び移動部３を制御することで、再現動作を実施する（ステップＳＴ５）。 Thereafter, the mode setting unit 7 sets the operation mode of the reproduction apparatus to the reproduction mode according to the user instruction (step ST4).
Next, the work control unit 11 acquires the time series data recorded by the first data recording unit 9 and the second data recording unit 10 as reference data, and the external force F detected by the external force detection unit 62 according to the reference data. Based on the position detected by the position detection unit 4, the actuator control unit 61 and the moving unit 3 are controlled to perform a reproduction operation (step ST5).

このように、実施の形態１に係る再現装置では、まず、ユーザがアクチュエータ１を持って作業を行い、その際に可動部１０２に加わる外力Ｆ及びアクチュエータ１の位置を検出して記録する。次に、再現装置では、記録した外力Ｆ及び位置に従ってアクチュエータ１が動作するようにアクチュエータ１の推力及び位置を制御する。また、実施の形態１に係る再現装置では、可動部１０２に加わる外力Ｆを正しく検出でき、また、コンプライアンス値を任意に変えられるため、エンドエフェクタ２が接触する物体５０が硬い場合でも移動部３の移動速度を落とすことなく教示及び再現を行うことができる。 As described above, in the reproduction apparatus according to the first embodiment, the user first performs work with the actuator 1 and detects and records the external force F applied to the movable portion 102 and the position of the actuator 1 at that time. Next, the reproduction device controls the thrust and position of the actuator 1 so that the actuator 1 operates according to the recorded external force F and position. Further, in the reproduction apparatus according to the first embodiment, the external force F applied to the movable unit 102 can be correctly detected, and the compliance value can be arbitrarily changed. Therefore, even when the object 50 that the end effector 2 contacts is hard, the moving unit 3 Teaching and reproduction can be performed without slowing down the movement speed.

なお上記では、可動部１０２を直動方向に変位可能とするアクチュエータ１を用いた場合を示した。しかしながら、これに限らず、加速度検出部５が角加速度を検出可能であれば、可動部１０２を回転方向に変位可能とするアクチュエータ１を用いることもできる。 In the above description, the case where the actuator 1 that allows the movable portion 102 to be displaced in the linear motion direction is used is shown. However, the present invention is not limited to this, and the actuator 1 that can displace the movable unit 102 in the rotational direction can be used as long as the acceleration detection unit 5 can detect angular acceleration.

また上記では、移動部３がロボットである場合を示した。しかしながら、これに限らず、移動部３として、直動機構又は回転機構を用いてもよい。 In the above description, the moving unit 3 is a robot. However, not limited to this, a linear motion mechanism or a rotation mechanism may be used as the moving unit 3.

以上のように、この実施の形態１によれば、再現装置は、固定部１０１及び可動部１０２を有するアクチュエータ１と、アクチュエータ１を移動する移動部３と、固定部１０１に対する可動部１０２の位置を検出する位置検出部４と、固定部１０１の加速度を検出する加速度検出部５と、位置検出部４により検出された位置と基準位置Ｐｒとの差分に対してゲインを調整し、当該調整結果である電流指令値Ｉｒｐ及び加速度検出部５により検出された加速度に基づいてアクチュエータ１に対する駆動電流Ｉａを出力するアクチュエータ制御部６１と、アクチュエータ制御部６１において得られた電流指令値Ｉｒｐ、又は、加速度検出部５により検出された加速度及びアクチュエータ制御部６１により出力された駆動電流Ｉａの電流値に基づいて、可動部１０２に加わる外力Ｆを検出する外力検出部６２と、可動部１０２に加わる外力Ｆ及びアクチュエータ１の位置の時系列を示す基準データに従い、外力検出部６２により検出された外力Ｆ及び位置検出部４により検出された位置に基づいてアクチュエータ制御部６１及び移動部３を制御する作業制御部１１とを備えた。これにより、実施の形態１に係る再現装置は、物体５０を壊さず且つ速度を落とさずに、事前に設定された外力Ｆ及び位置に従った動作を実現可能となる。 As described above, according to the first embodiment, the reproduction apparatus includes the actuator 1 having the fixed unit 101 and the movable unit 102, the moving unit 3 that moves the actuator 1, and the position of the movable unit 102 with respect to the fixed unit 101. The position detection unit 4 that detects the acceleration, the acceleration detection unit 5 that detects the acceleration of the fixed unit 101, and the difference between the position detected by the position detection unit 4 and the reference position Pr, and the adjustment result The actuator control unit 61 that outputs the drive current Ia to the actuator 1 based on the current command value Irp and the acceleration detected by the acceleration detection unit 5, and the current command value Irp obtained by the actuator control unit 61 or the acceleration Based on the acceleration detected by the detector 5 and the current value of the drive current Ia output by the actuator controller 61. The external force detection unit 62 for detecting the external force F applied to the movable unit 102, and the external force F and the position detected by the external force detection unit 62 according to the reference data indicating the time series of the external force F applied to the movable unit 102 and the position of the actuator 1 The actuator control unit 61 and the work control unit 11 that controls the moving unit 3 are provided based on the position detected by the detection unit 4. Thereby, the reproduction apparatus according to the first embodiment can realize an operation according to the preset external force F and position without damaging the object 50 and without reducing the speed.

実施の形態２．
実施の形態１では、ユーザがアクチュエータ１の動作を教示する際にアクチュエータ１が移動部３から切離された状態とされる場合を示した。これに対し、実施の形態２では、ユーザがアクチュエータ１の動作を教示する際にアクチュエータ１が移動部３に取付けられた状態とされる場合を示す。
図６はこの発明の実施の形態２に係る再現装置の構成例を示す図である。この図６に示す実施の形態２に係る再現装置では、図１に示す実施の形態１に係る再現装置に対し、位置検出部８を位置検出部（第２位置検出部）８ｂに変更している。その他の構成は同様であり、同一の符号を付して異なる部分についてのみ説明を行う。 Embodiment 2. FIG.
In the first embodiment, the case where the actuator 1 is separated from the moving unit 3 when the user teaches the operation of the actuator 1 is shown. In contrast, Embodiment 2 shows a case where the actuator 1 is attached to the moving unit 3 when the user teaches the operation of the actuator 1.
FIG. 6 is a diagram showing a configuration example of a reproduction apparatus according to Embodiment 2 of the present invention. In the reproduction device according to the second embodiment shown in FIG. 6, the position detection unit 8 is changed to a position detection unit (second position detection unit) 8b with respect to the reproduction device according to the first embodiment shown in FIG. Yes. Other configurations are the same, and only the different parts are described with the same reference numerals.

位置検出部８ｂは、移動部３に設けられ、移動部３の位置（絶対位置）を検出する。この位置検出部８ｂにより検出された位置を示す信号（位置信号）は、第２データ記録部１０に出力される。
また、第２データ記録部１０は、モード設定部７により動作モードが教示モードに設定されている場合に、位置検出部８ｂにより検出された位置の時系列データを記録する。 The position detection unit 8b is provided in the moving unit 3, and detects the position (absolute position) of the moving unit 3. A signal (position signal) indicating the position detected by the position detection unit 8 b is output to the second data recording unit 10.
The second data recording unit 10 records time-series data of the positions detected by the position detection unit 8b when the operation mode is set to the teaching mode by the mode setting unit 7.

実施の形態２に係る再現装置による再現動作例は、基本的には図４に示す実施の形態１に係る再現装置による再現動作例と同様であるが、図７Ａに示すように、ユーザがアクチュエータ１の動作を教示する際にもアクチュエータ１が移動部３に取付けられた状態とされている点が異なる。そして、実施の形態２に係る再現装置によっても、実施の形態１に係る再現装置と同様に、物体５０を壊さず且つ速度を落とさずに、事前に設定された外力Ｆ及び位置に従った動作を実現可能となる。また、実施の形態２に係る再現装置では、教示及び再現の際に共にアクチュエータ１が移動部３に取付けられた状態であるため、実施の形態１に係る再現装置に対して再現性がより高いと考えられる。 The reproduction operation example by the reproduction device according to the second embodiment is basically the same as the reproduction operation example by the reproduction device according to the first embodiment shown in FIG. 4, but as shown in FIG. 1 is also different in that the actuator 1 is attached to the moving unit 3. And also by the reproduction apparatus according to the second embodiment, similarly to the reproduction apparatus according to the first embodiment, the operation according to the preset external force F and position without damaging the object 50 and without reducing the speed. Can be realized. Further, in the reproduction apparatus according to the second embodiment, the actuator 1 is attached to the moving unit 3 during teaching and reproduction, and thus the reproducibility is higher than that of the reproduction apparatus according to the first embodiment. it is conceivable that.

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。 In the present invention, within the scope of the invention, any combination of the embodiments, any modification of any component in each embodiment, or omission of any component in each embodiment is possible. .

１アクチュエータ
２エンドエフェクタ
３移動部
４位置検出部
５加速度検出部
６外力検出制御部
７モード設定部
８，８ｂ位置検出部（第２位置検出部）
９第１データ記録部
１０第２データ記録部
１１作業制御部
１０１固定部
１０２可動部
５０物体
６１アクチュエータ制御部
６２外力検出部
６３位置速度変換部
６４減算器
６５ゲイン調整部
６６質量推定部
６７加速度補償部
６８加減算器
６９定電流制御部
６２１係数乗算部
６２２減算器
６２３係数乗算部
６５１ループゲイン測定部
６５２ゲイン交点制御部
６５３可変ゲイン調整部
６５４発振器
６５５加算器
６５６比較器
６７１乗算器
６７２係数乗算部
６９１減算器
６９２駆動ドライバ
６９３電流検出部 DESCRIPTION OF SYMBOLS 1 Actuator 2 End effector 3 Moving part 4 Position detection part 5 Acceleration detection part 6 External force detection control part 7 Mode setting part 8, 8b Position detection part (2nd position detection part)
9 First data recording unit 10 Second data recording unit 11 Work control unit 101 Fixed unit 102 Movable unit 50 Object 61 Actuator control unit 62 External force detection unit 63 Position speed conversion unit 64 Subtractor 65 Gain adjustment unit 66 Mass estimation unit 67 Acceleration Compensator 68 Adder / Subtractor 69 Constant current controller 621 Coefficient multiplier 622 Subtractor 623 Coefficient multiplier 651 Loop gain measuring unit 652 Gain intersection controller 653 Variable gain adjuster 654 Oscillator 655 Adder 656 Comparator 671 Multiplier 672 Coefficient multiplier 691 Subtractor 692 Drive driver 693 Current detection unit

Claims

An actuator having a fixed part and a movable part displaceable with respect to the fixed part;
A moving unit for moving the actuator;
A position detection unit for detecting the position of the movable unit with respect to the fixed unit;
An acceleration detector for detecting the acceleration of the fixed part;
The gain is adjusted with respect to the difference between the position detected by the position detection unit and the reference position, and the drive current for the actuator is calculated based on the current command value as the adjustment result and the acceleration detected by the acceleration detection unit. An actuator controller to output,
The external force applied to the movable part is detected based on the current command value obtained in the actuator control unit, or the acceleration detected by the acceleration detection unit and the current value of the drive current output by the actuator control unit. An external force detector;
The actuator control unit and the moving unit based on the external force detected by the external force detection unit and the position detected by the position detection unit according to reference data indicating a time series of the external force applied to the movable unit and the position of the actuator A reproduction device comprising a work control unit for controlling the machine.

A mode setting unit for setting the operation mode to the teaching mode or the reproduction mode;
A second position detector for detecting the position of the actuator;
A first data recording unit that records time-series data of the external force detected by the external force detection unit when the operation mode is set to the teaching mode by the mode setting unit;
A second data recording unit that records time-series data of the positions detected by the second position detection unit when the operation mode is set to the teaching mode by the mode setting unit;
The work control unit uses time series data recorded by the first data recording unit and the second data recording unit as the reference data when the operation mode is set to the reproduction mode by the mode setting unit. The reproduction apparatus according to claim 1, wherein the control is performed.

A mode setting unit for setting the operation mode to the teaching mode or the reproduction mode;
A second position detection unit for detecting a position of the moving unit;
A first data recording unit that records time-series data of the external force detected by the external force detection unit when the operation mode is set to the teaching mode by the mode setting unit;
A second data recording unit that records time-series data of the positions detected by the second position detection unit when the operation mode is set to the teaching mode by the mode setting unit;
The work control unit uses time series data recorded by the first data recording unit and the second data recording unit as the reference data when the operation mode is set to the reproduction mode by the mode setting unit. The reproduction apparatus according to claim 1, wherein the control is performed.