JP6711536B2

JP6711536B2 - External force detection method

Info

Publication number: JP6711536B2
Application number: JP2016211596A
Authority: JP
Inventors: 茂川瀬
Original assignee: Azbil Corp
Current assignee: Azbil Corp
Priority date: 2016-10-28
Filing date: 2016-10-28
Publication date: 2020-06-17
Anticipated expiration: 2036-10-28
Also published as: CN109844477A; CN109844477B; JP2018072135A; WO2018079107A1

Description

この発明は、アクチュエータ可動部に加わる外力を検出する外力検出方法に関する。 The present invention relates to an external force detecting method for detecting an external force applied to an actuator movable section.

従来から、組み立て、押し付けや研磨等の作業を行う作業装置では、産業用ロボット（以下、ロボットと称す）等が多く用いられている。このロボットには、アームの先端にハンド等のエンドエフェクタが取付けられており、作業対象物（部品又はワーク）を把持することで作業を行う。 2. Description of the Related Art Conventionally, industrial robots (hereinafter referred to as robots) and the like have been widely used in work devices that perform work such as assembly, pressing, and polishing. An end effector such as a hand is attached to the tip of an arm of this robot, and the work is performed by gripping a work target (part or work).

一方、ロボットの動作は、一般的に、位置制御によりコントロールされる。そのため、作業対象物の寸法誤差や把持位置誤差等により、予めプログラムされた作業対象物の目標位置と実際の位置とが異なる場合、作業対象物が他の物体と接触した際に大きな力（外力）が発生し、作業対象物に傷や破損が発生する恐れがある。 On the other hand, the operation of the robot is generally controlled by position control. Therefore, when the target position and the actual position of the pre-programmed work object differ due to dimensional error and grip position error of the work object, a large force (external force) occurs when the work object comes into contact with another object. ) Occurs, and the work object may be damaged or damaged.

その対策として、作業対象物の位置誤差により発生する力を吸収する冶具（いわゆる「バッファ」）を別途設置する場合がある。しかしながら、このバッファは、作業対象物の形状や材料毎に要求される特性が異なるため、作業対象物の種類の数だけ異なるバッファを用意する必要があり、都度設計となる。そのため、コストが増大し、且つ装置が大型化するという課題がある。 As a countermeasure, a jig (so-called “buffer”) that absorbs the force generated by the position error of the work object may be separately installed. However, since this buffer has different characteristics required for each shape and material of the work object, it is necessary to prepare as many different buffers as the number of kinds of work objects, and each buffer is designed. Therefore, there are problems that the cost is increased and the device is enlarged.

それに対し、ロボットとエンドエフェクタとの間に力覚センサを設置し、作業対象物の接触時に過大な力が発生しそうになると力覚センサの検出結果をロボットにフィードバックし、過大な力が発生しないようにする方法もある。この場合には、バッファが不要となる。しかしながら、力覚センサは高価である。 On the other hand, a force sensor is installed between the robot and the end effector, and if an excessive force is likely to occur when the work object comes into contact, the detection result of the force sensor is fed back to the robot, and the excessive force does not occur. There is also a way to do so. In this case, the buffer is unnecessary. However, force sensors are expensive.

また、力覚センサを用いた場合には、以下に述べる理由により、作業時間の短縮が難しいという課題がある。 Further, when the force sensor is used, there is a problem that it is difficult to shorten the working time for the reason described below.

すなわち、作業対象物が他の物体と接触する位置に誤差がある場合、接触時に過大な力が発生したことを検出して停止指令を出すが、可動部が大きくて重く且つ減速機構を有するロボットは急には止まれない。
また、接触時に発生する力は、慣性による衝撃力と接触時にロボットが発生している力との和となる。ここで、慣性による衝撃力は、作業対象物及びロボット可動部の質量と移動速度との積に比例する。しかしながら、ロボットは大きくて重い機構を有しているため、慣性による衝撃力を小さくするためには、接触直前の移動速度を遅くする必要がある。 That is, when there is an error in the position where the work object contacts another object, a stop command is issued by detecting that an excessive force is generated at the time of contact, but a robot with a large and heavy moving part and a deceleration mechanism. Can't stop suddenly.
Further, the force generated at the time of contact is the sum of the impact force due to inertia and the force generated by the robot at the time of contact. Here, the impact force due to inertia is proportional to the product of the mass of the work object and the movable part of the robot and the moving speed. However, since the robot has a large and heavy mechanism, it is necessary to reduce the moving speed immediately before contact in order to reduce the impact force due to inertia.

また、過大な力が発生したことを検出して停止指令を出してもロボットは急には止まれないため、停止指令が出た時点から急激に減速しても接触位置からずれた位置で停止し、作業対象物を押し潰してしまう。そして、位置の行き過ぎ量は移動速度に比例するため、作業対象物を他の物体に近づける速度を遅くせざるを得ない。 Also, the robot does not stop suddenly even if it detects that an excessive force is generated and issues a stop command.Therefore, even if the robot decelerates suddenly from the time when the stop command is issued, it stops at a position displaced from the contact position. , The work object is crushed. Since the amount of position overshoot is proportional to the moving speed, the speed at which the work target is brought closer to other objects must be slowed down.

上記の理由により、作業対象物が他の物体と接触する可能性のある領域では、ロボットの移動速度を十分落とす必要がある。しかしながら、サイクルタイムを短くするため、作業対象物を移送する速度は速くする必要がある。その結果、接触領域の近傍で速度を急激に落とすことになる。 For the above reason, it is necessary to sufficiently reduce the moving speed of the robot in the area where the work target object may come into contact with other objects. However, in order to shorten the cycle time, it is necessary to increase the transfer speed of the work object. As a result, the velocity is drastically reduced in the vicinity of the contact area.

しかしながら、エンドエフェクタは力覚センサの先に取付けられている。そのため、ロボットが急激に減速した場合には、エンドエフェクタの質量による影響で、力覚センサには負方向の加速度に比例した力が発生する。
ところが、上記加速度に比例した力と作業対象物の接触により発生する力とを区別することは難しく、区別するためにはロボットの減速時間を大幅に長くせざるを得ない。 However, the end effector is attached ahead of the force sensor. Therefore, when the robot rapidly decelerates, a force proportional to the acceleration in the negative direction is generated in the force sensor due to the influence of the mass of the end effector.
However, it is difficult to distinguish between the force proportional to the acceleration and the force generated by the contact of the work target, and the deceleration time of the robot must be significantly lengthened in order to distinguish.

また、力覚センサを用いた場合には、以下に述べる理由により、重力による影響をリアルタイムに補償し難いという課題がある。 Further, when the force sensor is used, it is difficult to compensate the influence of gravity in real time for the reason described below.

すなわち、組み立て、押し付けや研磨等の作業を行う場合にロボットが取りうる姿勢は常に一定ではなく、作業の状態に応じて変化させる場合が多い。例えば、曲面をトレースしながら研磨を行う作業では、姿勢を連続して変化させる必要がある。
しかしながら、上記の通り、エンドエフェクタは力覚センサの先に取付けられているため、ロボットの姿勢が水平ではない場合、力覚センサには重力加速度による影響でロボットの姿勢とエンドエフェクタの質量に応じた力が発生する。 That is, the posture that the robot can take when performing work such as assembling, pressing, and polishing is not always constant, but is often changed according to the state of the work. For example, in the work of polishing while tracing a curved surface, it is necessary to continuously change the posture.
However, as described above, since the end effector is attached to the end of the force sensor, if the robot's posture is not horizontal, the force sensor will be affected by the gravitational acceleration, depending on the robot's posture and the mass of the end effector. Generated force.

一方、重力加速度の影響を補償する重力補償手段として、例えば特許文献１に開示された方法が挙げられる。この特許文献１では、予めオフラインで姿勢に応じた重力の影響により力覚センサに発生する力を学習しておく。そして、実際の作業時に発生する力から学習した力を差し引くことで、作業力を算出している。しかしながら、この方法では、作業対象物が変わる度に学習を行う必要がある。また、学習は物体との接触前に行う必要があり、ロボットが連続して姿勢を変えるような場合には重力補償はできない。 On the other hand, as a gravity compensating means for compensating for the influence of gravity acceleration, for example, the method disclosed in Patent Document 1 can be cited. In Patent Document 1, the force generated in the force sensor due to the influence of gravity according to the posture is learned off-line in advance. Then, the work force is calculated by subtracting the learned force from the force generated during the actual work. However, with this method, it is necessary to perform learning each time the work target changes. Further, learning must be performed before contact with an object, and gravity compensation cannot be performed when the robot continuously changes its posture.

なお上記では、可動部に加わる外力として、作業対象物と他の物体とが接触した際に発生する力を示したが、これに限らず、エンドエフェクタと作業対象物とが接触した際に発生する力についても同様である。 In the above, as the external force applied to the movable part, the force generated when the work target comes into contact with another object is shown, but the external force is not limited to this, and occurs when the end effector comes into contact with the work target. The same applies to the power to do.

特開２０１２−１１５９１２号公報JP 2012-115912 A

上記の通り、ロボットと力覚センサを用いて組み立て等の作業を行う場合、作業時間が長くなる。一方、作業時間を短くしようとすると作業対象物を傷付け、押し潰し、接触を正しく検出できなくなる。また、重力補償をリアルタイムで行うことも難しい。このように、力覚センサを用いた場合には、ロボットが急激に加減速した場合又は姿勢が変更した場合に、外力を正しく検出できないという課題がある。 As described above, when performing work such as assembly using the robot and the force sensor, the work time becomes long. On the other hand, if the work time is shortened, the work object is damaged, crushed, and contact cannot be detected correctly. It is also difficult to perform gravity compensation in real time. As described above, when the force sensor is used, there is a problem that the external force cannot be correctly detected when the robot is rapidly accelerated or decelerated or when the posture is changed.

この発明は、上記のような課題を解決するためになされたもので、可動部が急激に加減速された場合又は姿勢が変更された場合でも、可動部に加わる外力を正しく検出できる外力検出方法を提供することを目的としている。 The present invention has been made to solve the above problems, and an external force detection method capable of correctly detecting an external force applied to a movable part even when the movable part is rapidly accelerated or decelerated or the posture is changed. Is intended to provide.

この発明に係る外力検出方法は、加速度検出手段は、固定部に対して可動部を変位可能なアクチュエータにおける当該固定部の加速度を検出し、位置検出手段は、固定部に対する可動部の位置を検出し、位置制御手段は、位置検出手段により検出された位置と基準位置との差分に基づく電流指令値を出力し、加速度補償手段は、加速度検出手段により検出された加速度と、可動部側の質量との乗算結果に基づく加速度補償値を出力し、加算手段は、位置制御手段から出力された電流指令値に、加速度補償手段から出力された加速度補償値を加算し、定電流制御手段は、アクチュエータを駆動する駆動電流の電流値を、加算手段により加速度補償値が加算された電流指令値に一致させ、外力検出手段は、駆動電流の電流値から加速度補償値を減算した結果に基づいて、可動部に加わる外力を検出することを特徴とする。 In the external force detecting method according to the present invention, the acceleration detecting means detects the acceleration of the fixed portion in the actuator capable of displacing the movable portion with respect to the fixed portion, and the position detecting means detects the position of the movable portion with respect to the fixed portion. The position control means outputs a current command value based on the difference between the position detected by the position detecting means and the reference position, and the acceleration compensating means calculates the acceleration detected by the acceleration detecting means and the mass on the movable part side. And outputs the acceleration compensation value based on the result of multiplication with, and the addition means adds the acceleration compensation value output from the acceleration compensation means to the current command value output from the position control means, and the constant current control means is the actuator. The current value of the drive current for driving the drive current is made to match the current command value to which the acceleration compensation value has been added by the addition means, and the external force detection means moves based on the result of subtracting the acceleration compensation value from the current value of the drive current. It is characterized in that the external force applied to the section is detected.

この発明によれば、上記のように構成したので、アクチュエータが急激に加減速された場合又は姿勢が変更された場合でも、可動部に加わる外力を正しく検出できる。 According to the present invention, since it is configured as described above, the external force applied to the movable portion can be correctly detected even when the actuator is rapidly accelerated/decelerated or the posture is changed.

この発明の実施の形態１に係る外力検出装置を備えた作業装置の構成例を示す図である。It is a figure which shows the structural example of the working apparatus provided with the external force detection apparatus which concerns on 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. 図３Ａ、図３Ｂは、この発明の実施の形態１に係る外力検出装置において、アクチュエータが急激に加減速された状態で可動部がワークに接触したときの外力の検出を説明する図であって、図３Ａは外力検出部における減算器に入力された駆動電流及び加速度補償値を示す図であり、図３Ｂは外力検出部により検出された外力を示す図である。FIG. 3A and FIG. 3B are diagrams for explaining detection of an external force when the movable part comes into contact with a work in a state where the actuator is rapidly accelerated and decelerated in the external force detection device according to the first embodiment of the present invention. 3A is a diagram showing the drive current and the acceleration compensation value input to the subtractor in the external force detection unit, and FIG. 3B is a diagram showing the external force detected by the external force detection unit.

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る外力検出装置（接触制御装置）を備えた作業装置の構成例を示す図である。
作業装置は、組み立て、押し付けや研磨等の作業を行う装置である。この作業装置は、図１に示すように、アクチュエータ１、位置検出器（位置検出手段）２、位置速度変換部３、加速度検出器（加速度検出手段）４、減算器５、ゲイン調整部６、質量推定部（質量推定手段）７、加速度補償部（加速度補償手段）８、加減算器（加算手段）９、定電流制御部（定電流制御手段）１０及び外力検出部（外力検出手段）１１を備えている。
なお、位置検出器２、位置速度変換部３、加速度検出器４、減算器５、ゲイン調整部６、質量推定部７、加速度補償部８、加減算器９、定電流制御部１０及び外力検出部１１は、外力検出装置を構成する。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1.
FIG. 1 is a diagram showing a configuration example of a working device including an external force detection device (contact control device) according to Embodiment 1 of the present invention.
The working device is a device that performs work such as assembling, pressing and polishing. As shown in FIG. 1, this working device includes an actuator 1, a position detector (position detecting means) 2, a position/velocity converting section 3, an acceleration detector (acceleration detecting means) 4, a subtractor 5, a gain adjusting section 6, A mass estimation unit (mass estimation unit) 7, an acceleration compensation unit (acceleration compensation unit) 8, an adder/subtractor (addition unit) 9, a constant current control unit (constant current control unit) 10, and an external force detection unit (external force detection unit) 11 are provided. I have it.
The position detector 2, the position/speed converter 3, the acceleration detector 4, the subtractor 5, the gain adjuster 6, the mass estimator 7, the acceleration compensator 8, the adder/subtractor 9, the constant current controller 10, and the external force detector. Reference numeral 11 constitutes an external force detection device.

アクチュエータ１は、磁界に置かれたコイルに電流が供給されることで、固定部１０１に対して可動部１０２を直動方向又は回転方向に変位可能とする。このアクチュエータ１は、ロボット（不図示）等の先端に取付けられており、全体が移送され、また、姿勢が変更される。
また、可動部１０２には、エンドエフェクタ１２が取付けられている。図１では、エンドエフェクタ１２としてグリッパ（ハンド）が取付けられている。グリッパは、作業対象物を把持自在に構成されている。なお以下では、作業対象物として、ワーク５０を用いた場合を示すが、部品を用いてもよい。 The actuator 1 is capable of displacing the movable portion 102 in the linear movement direction or the rotation direction with respect to the fixed portion 101 by supplying a current to a coil placed in a magnetic field. The actuator 1 is attached to the tip of a robot (not shown) or the like, and the whole is transferred and the posture is changed.
Further, the end effector 12 is attached to the movable portion 102. In FIG. 1, a gripper (hand) is attached as the end effector 12. The gripper is configured to be capable of gripping a work target. In the following, the case where the work 50 is used as the work target is shown, but parts may be used.

位置検出器２は、アクチュエータ１に設けられ、固定部１０１に対する可動部１０２の位置（相対位置）を検出する。この位置検出器２により検出された位置を示す信号（位置信号）は、位置速度変換部３及び減算器５に出力される。 The position detector 2 is provided in the actuator 1 and detects the position (relative position) of the movable portion 102 with respect to the fixed portion 101. A signal (position signal) indicating the position detected by the position detector 2 is output to the position/speed converter 3 and the subtractor 5.

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

加速度検出器４は、固定部１０１に設けられ、固定部１０１の加速度を検出する。この際、加速度検出器４は、固定部１０１の重力加速度αｇ及び移動加速度α１のうちの一方、又は両方が加算された加速度（αｇ＋α１）を検出する。図１では、加速度検出器４が加速度（αｇ＋α１）を検出する場合を示している。この加速度検出器４により検出された加速度を示す信号（加速度信号）は、加速度補償部８に出力される。 The acceleration detector 4 is provided on the fixed portion 101 and detects the acceleration of the fixed portion 101. At this time, the acceleration detector 4 detects the 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. 1 shows a case where the acceleration detector 4 detects acceleration (αg+α1). A signal (acceleration signal) indicating the acceleration detected by the acceleration detector 4 is output to the acceleration compensator 8.

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

ゲイン調整部６は、アクチュエータ１におけるコンプライアンス（バネ定数の逆数：固さ柔らかさの指標）の値を調整する。このゲイン調整部６は、図１，２に示すように、ループゲイン測定部６０１、ゲイン交点制御部６０２及び可変ゲイン調整部６０３を有している。 The gain adjusting unit 6 adjusts the value of the compliance (the reciprocal of the spring constant: an index of hardness and softness) in the actuator 1. As shown in FIGS. 1 and 2, the gain adjusting unit 6 includes a loop gain measuring unit 601, a gain intersection control unit 602, and a variable gain adjusting unit 603.

ループゲイン測定部６０１は、減算器５から出力された信号のループゲインを測定する。この際、ループゲイン測定部６０１は、図２に示すように、減算器５から出力された信号に、発振器６０１１によりループゲインが１倍（０ｄＢ）となる周波数、すなわちゲイン交点に設定された周波数の正弦波を、加算器６０１２を介して加算する。このループゲイン測定部６０１による正弦波の加算前後の信号は、ゲイン交点制御部６０２に出力される。 The loop gain measuring unit 601 measures the loop gain of the signal output from the subtractor 5. At this time, as shown in FIG. 2, the loop gain measurement unit 601 determines that the signal output from the subtractor 5 has a frequency at which the loop gain is 1 times (0 dB) by the oscillator 6011, that is, the frequency set at the gain intersection point. The sine waves of are added via the adder 6012. The signals before and after the addition of the sine wave by the loop gain measurement unit 601 are output to the gain intersection control unit 602.

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

可変ゲイン調整部６０３は、ゲイン交点制御部６０２により比較された振幅比の倍率が１となるように、減算器５から出力された信号のゲインを調整する。この可変ゲイン調整部６０３によりループゲインが調整された信号は、加減算器９に電流指令値Ｉｒｐとして出力される。また、可変ゲイン調整部６０３によるループゲインの調整値を示す信号は、質量推定部７に出力される。 The variable gain adjustment unit 603 adjusts the gain of the signal output from the subtractor 5 so that the multiplication factor of the amplitude ratio compared by the gain intersection control unit 602 becomes 1. The signal whose loop gain has been adjusted by the variable gain adjustment unit 603 is output to the adder/subtractor 9 as the current command value Irp. A signal indicating the loop gain adjustment value by the variable gain adjustment unit 603 is output to the mass estimation unit 7.

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

質量推定部７は、可変ゲイン調整部６０３によるループゲインの調整値から、可動部１０２側の質量を推定する。すなわち、質量推定部７は、ループゲインの変化と質量の変化とが比例する原理を利用する。ここで、可動部１０２側の質量とは、エンドエフェクタ１２がワーク５０を把持していない場合には、可動部１０２の質量Ｍ１とエンドエフェクタ１２の質量Ｍ２とが加算された質量（Ｍ１＋Ｍ２）であり、エンドエフェクタ１２がワーク５０を把持している場合には、可動部１０２の質量Ｍ１とエンドエフェクタ１２の質量Ｍ２とワーク５０の質量Ｍ３とが加算された質量（Ｍ１＋Ｍ２＋Ｍ３）である。なお図１では、質量推定部７が、可動部１０２の質量Ｍ１とエンドエフェクタ１２の質量Ｍ２とワーク５０の質量Ｍ３とが加算された質量（Ｍ１＋Ｍ２＋Ｍ３）を推定する場合を示している。この質量推定部７により推定された質量を示す信号は、加速度補償部８に出力される。 The mass estimating unit 7 estimates the mass on the movable unit 102 side from the adjustment value of the loop gain by the variable gain adjusting unit 603. That is, the mass estimation unit 7 uses the principle that the change in loop gain and the change in mass are proportional. Here, the mass on the movable portion 102 side is the mass (M1+M2) obtained by adding the mass M1 of the movable portion 102 and the mass M2 of the end effector 12 when the end effector 12 does not grip the work 50. If the end effector 12 is gripping the work 50, the mass (M1+M2+M3) is the sum of the mass M1 of the movable part 102, the mass M2 of the end effector 12, and the mass M3 of the work 50. Note that FIG. 1 shows a case where the mass estimation unit 7 estimates the mass (M1+M2+M3) obtained by adding the mass M1 of the movable unit 102, the mass M2 of the end effector 12, and the mass M3 of the work 50. The signal indicating the mass estimated by the mass estimating unit 7 is output to the acceleration compensating unit 8.

なお、ゲイン調整部６及び質量推定部７の動作原理は、下記の特許文献２と同様であり、その詳細については説明を省略する。
また上記では、質量推定部７により可動部１０２側の質量を推定する場合を示したが、これに限らず、他の方法を用いて可動部１０２側の質量を取得してもよい。
特開２０１０−１８２０８４号公報 The operating principles of the gain adjusting unit 6 and the mass estimating unit 7 are the same as in Patent Document 2 below, and the detailed description thereof is omitted.
Further, although the case where the mass estimation unit 7 estimates the mass on the movable unit 102 side has been described above, the present invention is not limited to this, and another method may be used to acquire the mass on the movable unit 102 side.
JP, 2010-182084, A

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

乗算器８０１は、加速度検出器４により検出された加速度と、質量推定部７により推定された質量とを乗算する。この乗算器８０１による乗算結果を示す信号は、係数乗算部８０２及び外力検出部１１に出力される。
係数乗算部８０２は、乗算器８０１による乗算結果に係数（１／Ｋｔ）を乗算する。なお、Ｋｔは、アクチュエータ１が発生する推力と駆動電流Ｉａとの比を表したトルク定数である。この係数乗算部８０２による乗算結果を示す信号は、加減算器９に加速度補償値Ｉｒｃとして出力される。 The multiplier 801 multiplies the acceleration detected by the acceleration detector 4 and the mass estimated by the mass estimation unit 7. A signal indicating the multiplication result of the multiplier 801 is output to the coefficient multiplication unit 802 and the external force detection unit 11.
The coefficient multiplication unit 802 multiplies the multiplication result of the multiplier 801 by a coefficient (1/Kt). Note that Kt is a torque constant that represents the ratio of the thrust generated by the actuator 1 and the drive current Ia. The signal indicating the multiplication result by the coefficient multiplication unit 802 is output to the adder/subtractor 9 as the acceleration compensation value Irc.

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

定電流制御部１０は、アクチュエータ１を駆動する駆動電流Ｉａを電流指令値Ｉｒに一致させるように制御する。この定電流制御部１０は、減算器１００１、駆動ドライバ１００２及び電流検出器１００３を有している。 The constant current control unit 10 controls the drive current Ia that drives the actuator 1 to match the current command value Ir. The constant current control unit 10 has a subtractor 1001, a drive driver 1002, and a current detector 1003.

減算器１００１は、加減算器９から出力された電流指令値Ｉｒから、電流検出器１００３により検出された駆動電流Ｉａの電流値を減算する。この減算器１００１による減算結果を示す信号は、駆動ドライバ１００２に出力される。
駆動ドライバ１００２は、減算器１００１による減算結果に応じた駆動電流Ｉａを発生する。この駆動ドライバ１００２により発生された駆動電流Ｉａは、電流検出器１００３を介してアクチュエータ１に出力される。
電流検出器１００３は、駆動ドライバ１００２により発生された駆動電流Ｉａの電流値を検出する。この電流検出器１００３により検出された電流値を示す信号は、減算器１００１に出力される。 The subtractor 1001 subtracts the current value of the drive current Ia detected by the current detector 1003 from the current command value Ir output from the adder/subtractor 9. A signal indicating the result of the subtraction by the subtractor 1001 is output to the drive driver 1002.
The drive driver 1002 generates a drive current Ia according to the subtraction result of the subtractor 1001. The drive current Ia generated by the drive driver 1002 is output to the actuator 1 via the current detector 1003.
The current detector 1003 detects the current value of the drive current Ia generated by the drive driver 1002. A signal indicating the current value detected by the current detector 1003 is output to the subtractor 1001.

外力検出部１１は、駆動電流Ｉａの電流値から加速度補償値Ｉｒｃを減算した結果に基づいて、可動部１０２に加わる外力（反力）Ｆを検出する。なお、可動部１０２に加わる外力Ｆとしては、エンドエフェクタ１２が把持したワーク５０と他の物体とが接触した際に発生する力や、エンドエフェクタ１２とワーク５０とが接触した際に発生する力が挙げられる。この外力検出部１１は、係数乗算部１１０１、減算器１１０２及び係数乗算部１１０３を有している。 The external force detection unit 11 detects an external force (reaction force) F applied to the movable unit 102 based on the result of subtracting the acceleration compensation value Irc from the current value of the drive current Ia. The external force F applied to the movable portion 102 is a force generated when the work 50 gripped by the end effector 12 comes into contact with another object, or a force generated when the end effector 12 comes into contact with the work 50. Is mentioned. The external force detection unit 11 has a coefficient multiplication unit 1101, a subtractor 1102, and a coefficient multiplication unit 1103.

係数乗算部１１０１は、加速度補償部８の乗算器８０１による乗算結果に係数（１／Ｋｔ）を乗算する。この係数乗算部１１０１による乗算結果を示す信号は、減算器１１０２に出力される。
減算器１１０２は、定電流制御部１０により発生された駆動電流Ｉａの電流値から、係数乗算部１１０１による乗算結果を減算する。この減算器１１０２による減算結果を示す信号は、係数乗算部１１０３に出力される。
係数乗算部１１０３は、減算器１１０２による減算結果に係数（Ｋｔ）を乗算することで、外力Ｆを得る。 The coefficient multiplication unit 1101 multiplies the multiplication result by the multiplier 801 of the acceleration compensation unit 8 by the coefficient (1/Kt). The signal indicating the multiplication result by the coefficient multiplication unit 1101 is output to the subtractor 1102.
The subtractor 1102 subtracts the multiplication result by the coefficient multiplication unit 1101 from the current value of the drive current Ia generated by the constant current control unit 10. A signal indicating the result of the subtraction by the subtractor 1102 is output to the coefficient multiplication unit 1103.
The coefficient multiplication unit 1103 obtains the external force F by multiplying the subtraction result of the subtractor 1102 by the coefficient (Kt).

次に、実施の形態１に係る外力検出装置の動作原理について説明する。なお以下では、アクチュエータ１として、発生した推力がワーク５０に直接伝わるダイレクトドライブ形式のリニアアクチュエータを用い、固定部１０１に対して可動部１０２を直動させるものとする。このアクチュエータ１は、定電流制御部１０が電流指令値Ｉｒに応じて発生した駆動電流Ｉａにより駆動する。 Next, the operating principle of the external force detection device according to the first embodiment 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 workpiece 50, and the movable portion 102 is directly 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 10 according to the current command value Ir.

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

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

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

電流指令値Ｉｒは、電流指令値Ｉｒｐの他、外乱トルクを補正するための加速度補償値Ｉｒｃで構成され、次式（１）で表される。
Ｉｒ＝Ｉｒｐ＋Ｉｒｃ（１） The current command value Ir is composed of the current command value Irp and the acceleration compensation value Irc for correcting the disturbance torque, 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 actuality, the velocity signal from the position/velocity converter 3 is added as a minor loop to the minus output of the adder/subtractor 9 for stabilization, but this is omitted below.

また、ゲイン調整部６では、位置制御ループのループゲインを変えることで、アクチュエータ１におけるコンプライアンスの値を変化させることができる。 Further, the gain adjusting unit 6 can change the compliance value in the actuator 1 by changing the loop 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 that when there is disturbance torque.
As the general disturbance torque, a reaction force received from the work 50 at the time of work, a force generated by gravity and moving acceleration, a loss torque of the speed reducer, and the like can be considered. Here, since the actuator 1 is a direct drive type linear actuator, it does not have a decelerator and it is not necessary to consider the loss torque. Therefore, the drive current Ia has a value proportional to the force generated by the reaction force, gravity, and moving acceleration received from the work 50 during work. In the following, the reaction force is assumed to be a force generated when the work 50 comes into contact with another object.

ここで、アクチュエータ１の駆動電流をＩａ、作業時にワーク５０から受ける反力をＦ、固定部１０１の直動方向成分における移動加速度をα１、固定部１０１の直動方向成分における重力加速度をαg、可動部１０２の質量をＭ１，エンドエフェクタ１２の質量をＭ２、ワーク５０の質量をＭ３とする。この場合、次式（２）の関係が成り立つ。
Ｆ＋（α１＋αｇ）・（Ｍ１＋Ｍ２＋Ｍ３）＝Ｋｔ・Ｉｒ＝Ｋｔ・（Ｉｒｐ＋Ｉｒｃ）
（２）
なお、Ｋｔはアクチュエータ１が発生する推力と駆動電流Ｉａとの比を表したトルク定数である。 Here, the drive current of the actuator 1 is Ia, the reaction force received from the work 50 at the time of work is F, the moving acceleration of the fixed part 101 in the linear motion component is α1, the gravitational acceleration of the fixed part 101 in the linear motion component is αg, The mass of the movable part 102 is M1, the mass of the end effector 12 is M2, and the mass of the workpiece 50 is M3. In this case, the relationship of the following expression (2) is established.
F+(α1+αg)·(M1+M2+M3)=Kt·Ir=Kt·(Irp+Irc)
(2)
Note that Kt is a torque constant that represents the ratio of 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 in the following equation (3).
(Α1+αg)·(M1+M2+M3)=Kt·Irc (3)

式（３）のように加速度補償値Ｉｒｃを設定した場合、式（２）からα１、αｇ、Ｍ１、Ｍ２、Ｍ３の項が消え、次式（４）のように整理される。
Ｆ＝Ｋｔ・Ｉｒｐ（４） When the acceleration compensation value Irc is set as in Expression (3), the terms α1, αg, M1, M2, and M3 disappear from Expression (2), and the expression is 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), it is understood that the reaction force F received from the work 50 during the work and the current command value Irp have a proportional relationship.

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

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

なお、上述したように、ワーク５０が他の物体と接触して発生する反力Ｆは、電流指令値Ｉｒｐを監視することで知ることができる。
しかしながら、位置制御ループにおいて、反力Ｆに対する電流指令値Ｉｒｐの応答は一般的に速くない。一方、反力Ｆに対する駆動電流Ｉａの応答は比較的早い。そこで、電流指令値Ｉｒｐを直接監視するのではなく、駆動電流Ｉａを監視することで反力Ｆの検出を行う。 In addition, as described above, the reaction force F generated when the workpiece 50 comes into contact with another object can be known by monitoring the current command value Irp.
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. Therefore, instead of directly monitoring the current command value Irp, the reaction force F is detected by monitoring the drive current Ia.

ここで、式（２）は以下の通りである。
Ｆ＋（α１＋αｇ）・（Ｍ１＋Ｍ２＋Ｍ３）＝Ｋｔ・Ｉｒ＝Ｋｔ・（Ｉｒｐ＋Ｉｒｃ）
（２） Here, the equation (2) is as follows.
F+(α1+αg)·(M1+M2+M3)=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)

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

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

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

図３では、可動部１０２が図１に示すように下方向に直動してワーク５０が他の物体（不図示）に接触した場合での信号波形を示している。なお、図３Ａでは減算器１１０２に入力された駆動電流Ｉａ及び加速度補償値Ｉｒｃ（＝（（α１＋αｇ）・（Ｍ１＋Ｍ２＋Ｍ３））／Ｋｔ）を示し、図３Ｂでは外力検出部１１により検出された反力Ｆを示している。この図３に示すように、駆動電流Ｉａから加速度補償値Ｉｒｃ（＝（（α１＋αｇ）・（Ｍ１＋Ｍ２＋Ｍ３））／Ｋｔ）を減算して係数（Ｋｔ）を乗算することで、反力Ｆを正確に検出できる。 FIG. 3 shows a signal waveform in the case where the movable part 102 moves downward as shown in FIG. 1 and the workpiece 50 comes into contact with another object (not shown). 3A shows the drive current Ia and the acceleration compensation value Irc (=((α1+αg)·(M1+M2+M3))/Kt) input to the subtractor 1102, and the reaction force detected by the external force detector 11 in FIG. 3B. F is shown. As shown in FIG. 3, by subtracting the acceleration compensation value Irc (=((α1+αg)·(M1+M2+M3))/Kt) from the drive current Ia and multiplying it by the coefficient (Kt), the reaction force F is accurately calculated. Can be detected.

次に、実施の形態１に係る外力検出装置による効果について説明する。
上記の通り、ロボットの動作は、一般的に、位置制御によりコントロールされる。そのため、作業対象物の寸法誤差や把持位置誤差等により、予めプログラムされた作業対象物の目標位置と実際の位置が異なる場合、作業対象物が他の物体と接触した際に大きな力が発生し、作業対象物に傷や破損が発生する恐れがある。 Next, effects of the external force detection device according to the first embodiment will be described.
As described above, the operation of the robot is generally controlled by position control. Therefore, if the target position and the actual position of the pre-programmed work object differ due to dimensional error and grip position error of the work object, a large force is generated when the work object comes into contact with another object. , The work object may be damaged or damaged.

その対策として、ロボットとエンドエフェクタとの間に力覚センサを設置し、作業対象物の接触時に過大な力が発生しそうになると力覚センサの検出結果をロボットにフィードバックし、過大な力が発生しないようにする方法もある。 As a countermeasure, a force sensor is installed between the robot and the end effector, and if an excessive force is likely to be generated when the work object comes into contact, the detection result of the force sensor is fed back to the robot to generate an excessive force. There is also a way to prevent it.

しかしながら、過大な力が発生したことを検出して停止指令を出してもロボットは急には止まれないため、停止指令が出た時点から急激に減速しても接触位置からずれた位置で停止してしまい、作業対象物を押し潰してしまう。そして、位置の行き過ぎ量は移動速度に比例するため、作業対象物を他の物体に近付ける速度を遅くせざるを得ない。 However, the robot does not stop suddenly even if it detects that an excessive force has been generated and issues a stop command.Therefore, even if the robot decelerates rapidly from the time the stop command is issued, it stops at a position displaced from the contact position. Ends up crushing the work object. Since the amount of position overshoot is proportional to the moving speed, the speed at which the work target is brought closer to other objects must be slowed down.

一方、実施の形態１では、ロボット等の先端にアクチュエータ１を取付け、また、外力検出装置は、アクチュエータ１が急激に移動又は停止されて移動加速度が発生した場合や、アクチュエータ１の姿勢が変更されて重力加速度が変化した場合でも、可動部１０２に加わる反力Ｆを正しく検出でき、また、コンプライアンス値を任意に変えられる。そのため、ロボットが急に止まれない点が同じだが、位置の行き過ぎにより作業対象物を押し潰してしまうことはない。よって、作業対象物を他の物体に近づける速度を極端に遅くする必要がなく、また、安全に作業できる。 On the other hand, in the first embodiment, the actuator 1 is attached to the tip of a robot or the like, and the external force detection device changes the posture of the actuator 1 when the actuator 1 is suddenly moved or stopped to generate a moving acceleration. Even when the gravity acceleration changes, the reaction force F applied to the movable portion 102 can be correctly detected, and the compliance value can be arbitrarily changed. Therefore, the robot does not stop suddenly, but it does not crush the work target due to excessive position. Therefore, it is not necessary to extremely slow down the speed at which the work target is brought closer to other objects, and the work can be performed safely.

また、従来では、エンドエフェクタは力覚センサの先に取付けられており、ロボットが急激に減速した場合、エンドエフェクタの質量による影響で、力覚センサには負方向の加速度に比例した力が発生する。
ところが、上記加速度に比例した力と作業対象物の接触により発生する力とを区別することは難しく、区別するためにはロボットの減速時間を大幅に長くせざるを得ない。 Also, in the past, the end effector was attached in front of the force sensor, and when the robot rapidly decelerated, the force sensor generated a force proportional to the acceleration in the negative direction due to the influence of the mass of the end effector. To do.
However, it is difficult to distinguish between the force proportional to the acceleration and the force generated by the contact of the work target, and the deceleration time of the robot must be significantly lengthened in order to distinguish.

一方、実施の形態１に係る外力検出装置では、アクチュエータ１が急激に加減速された場合でも正しく外力Ｆを検出でき、接触時にのみ力を検出するため、アクチュエータ１の減速時間を長くする必要はない。 On the other hand, in the external force detection device according to the first embodiment, the external force F can be correctly detected even when the actuator 1 is rapidly accelerated/decelerated, and the force is detected only when the actuator 1 comes into contact, so that it is not necessary to extend the deceleration time of the actuator 1. Absent.

また、力覚センサを用いた場合には、重力による影響をリアルタイムに補償し難いという課題もある。
すなわち、組み立て、押し付けや研磨等の作業を行う場合にロボットが取りうる姿勢は常に一定ではなく、作業の状態に応じて変化させる場合が多い。例えば、曲面をトレースしながら研磨を行う作業では、姿勢を連続して変化させる必要がある。
しかしながら、上記の通り、エンドエフェクタは力覚センサの先に取付けられているため、ロボットの姿勢が水平ではない場合、力覚センサには重力加速度による影響でロボットの姿勢とエンドエフェクタの質量に応じた力が発生する。 Further, when the force sensor is used, it is difficult to compensate for the influence of gravity in real time.
That is, the posture that the robot can take when performing work such as assembling, pressing, and polishing is not always constant, but is often changed according to the state of the work. For example, in the work of polishing while tracing a curved surface, it is necessary to continuously change the posture.
However, as described above, since the end effector is attached to the end of the force sensor, if the robot's posture is not horizontal, the force sensor will be affected by the gravitational acceleration, depending on the robot's posture and the mass of the end effector. Generated force.

一方、実施の形態１に係る外力検出装置では、アクチュエータ１の姿勢が変更されて重力加速度が変化した場合でも外力Ｆを正しく検出できるため、重力による影響をリアルタイムに補償できる。 On the other hand, in the external force detection device according to the first embodiment, the external force F can be correctly detected even when the posture of the actuator 1 is changed and the gravitational acceleration is changed, so that the influence of gravity can be compensated in real time.

なお上記では、可動部１０２を直動方向に変位可能とするアクチュエータ１を用いた場合を示した。しかしながら、これに限らず、加速度検出器４が角加速度を検出可能であれば、可動部１０２を回転方向に変位可能とするアクチュエータ１を用いることもできる。 Note that, in the above, the case where the actuator 1 capable of displacing the movable portion 102 in the linear movement direction is used is shown. However, the present invention is not limited to this, and if the acceleration detector 4 can detect angular acceleration, the actuator 1 that can displace the movable portion 102 in the rotation direction can be used.

以上のように、この実施の形態１によれば、加速度検出器４は固定部１０１の加速度を検出し、位置検出器２は固定部１０１に対する可動部１０２の位置を検出し、位置制御手段（減算器５及びゲイン調整部６）は、位置検出器２により検出された位置と基準位置Ｐｒとの差分に基づく電流指令値Ｉｒｐを出力し、加速度補償部８は、加速度検出器４により検出された加速度と、可動部１０２側の質量との乗算結果に基づく加速度補償値Ｉｒｃを出力し、加減算器９は電流指令値Ｉｒｐに加速度補償値Ｉｒｃを加算し、定電流制御部１０は駆動電流Ｉａの電流値を電流指令値Ｉｒに一致させ、外力検出部１１は、駆動電流Ｉａの電流値から加速度補償値Ｉｒｃを減算した結果に基づいて外力Ｆを検出するように構成したので、可動部１０２が急激に加減速された場合又は姿勢が変更された場合でも、可動部１０２に加わる外力Ｆを正しく検出できる。 As described above, according to the first embodiment, the acceleration detector 4 detects the acceleration of the fixed portion 101, the position detector 2 detects the position of the movable portion 102 with respect to the fixed portion 101, and the position control means ( The subtractor 5 and the gain adjuster 6) output a current command value Irp based on the difference between the position detected by the position detector 2 and the reference position Pr, and the acceleration compensator 8 is detected by the acceleration detector 4. Output the acceleration compensation value Irc based on the multiplication result of the acceleration and the mass of the movable portion 102 side, the adder/subtractor 9 adds the acceleration compensation value Irc to the current command value Irp, and the constant current controller 10 drives the driving current Ia. Since the external force detection unit 11 is configured to detect the external force F based on the result of subtracting the acceleration compensation value Irc from the current value of the drive current Ia, the movable unit 102 The external force F applied to the movable portion 102 can be correctly detected even when the acceleration/deceleration is rapidly performed or the posture is changed.

なお、本願発明はその発明の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。 It should be noted that, in the present invention, it is possible to modify any constituent element of the embodiment or omit any constituent element of the embodiment within the scope of the invention.

１アクチュエータ
２位置検出器（位置検出手段）
３位置速度変換部
４加速度検出器（加速度検出手段）
５減算器
６ゲイン調整部
７質量推定部
８加速度補償部（加速度補償手段）
９加減算器（加算手段）
１０定電流制御部（定電流制御手段）
１１外力検出部（外力検出手段）
１２エンドエフェクタ
５０ワーク
１０１固定部
１０２可動部
６０１ループゲイン測定部
６０２ゲイン交点制御部
６０３可変ゲイン調整部
８０１乗算器
８０２係数乗算部
１００１減算器
１００２駆動ドライバ
１００３電流検出器
１１０１係数乗算部
１１０２減算器
１１０３係数乗算部
６０１１発振器
６０１２加算器
６０２１比較器 1 Actuator 2 Position detector (position detection means)
3 Position/velocity converter 4 Accelerometer (accelerometer)
5 Subtractor 6 Gain Adjuster 7 Mass Estimator 8 Acceleration Compensator (Acceleration Compensator)
9 Adder/subtractor (adding means)
10 constant current control unit (constant current control means)
11 External force detection unit (external force detection means)
12 end effector 50 work 101 fixed part 102 movable part 601 loop gain measurement part 602 gain intersection control part 603 variable gain adjustment part 801 multiplier 802 coefficient multiplication part 1001 subtractor 1002 drive driver 1003 current detector 1101 coefficient multiplication part 1102 subtractor 1103 Coefficient multiplying unit 6011 Oscillator 6012 Adder 6021 Comparator

Claims

The acceleration detecting means detects the acceleration of the fixed part in the actuator capable of displacing the movable part with respect to the fixed part,
The position detecting means detects the position of the movable part with respect to the fixed part,
The position control means outputs a current command value based on the difference between the position detected by the position detection means and the reference position,
The acceleration compensating means outputs an acceleration compensation value based on a multiplication result of the acceleration detected by the acceleration detecting means and the mass on the movable portion side,
The adding means adds the acceleration compensation value output from the acceleration compensating means to the current command value output from the position control means,
The constant current control means matches the current value of the drive current for driving the actuator with the current command value to which the acceleration compensation value is added by the adding means,
The external force detecting means detects an external force applied to the movable portion based on a result obtained by subtracting the acceleration compensation value from the current value of the drive current.

The external force detecting method according to claim 1, wherein the acceleration detecting means detects an acceleration obtained by adding one or both of a gravitational acceleration and a moving acceleration of the fixed portion.

The said actuator is a direct drive type linear actuator. The external force detection method of Claim 1 or Claim 2 characterized by the above-mentioned.

The mass estimating means estimates the mass of the movable part side,
The acceleration compensating means outputs an acceleration compensation value based on a multiplication result of the acceleration detected by the acceleration detecting means and the mass estimated by the mass estimating means. The external force detecting method according to any one of the above.