JP2019093525A

JP2019093525A - Assortment device

Info

Publication number: JP2019093525A
Application number: JP2017227221A
Authority: JP
Inventors: 賢二栗原; Kenji Kurihara; 加納　史朗; Shiro Kano; 史朗加納
Original assignee: Azbil Corp
Current assignee: Azbil Corp
Priority date: 2017-11-27
Filing date: 2017-11-27
Publication date: 2019-06-20

Abstract

To provide an assortment device which can assort objects without through manual work regardless of weight and shapes of the objects.SOLUTION: A picking device 2 comprises: an actuator 21 having a fixed part 211 and a movable part 212; a holding part 22 that is attached to the movable part 212, and can hold an object 50 flowing on a belt conveyor 5; a position detecting part 24 that detects a position of the movable part 212 relative to the fixed part 211; an acceleration detecting part 25 that detects acceleration of the fixed part 211; an actuator control part 261 that adjusts gain with respect to a difference between the detected position and a reference position Pr, and outputs a driving current Ia to the actuator 21 on the basis of a current command value Irp resulting from the adjustment and the detected acceleration; an external force detecting part 262 that detects external force F applied to the movable part 212 on the basis of the current command value Irp or the detected acceleration and current value of the outputted driving current Ia; and a work control part 27 that controls the actuator control part 261 and the holding part 22 on the basis of the detected external force F.SELECTED DRAWING: Figure 2

Description

この発明は、仕分け作業を行う仕分け装置に関する。 The present invention relates to a sorting apparatus that performs sorting work.

従来、野菜又は果物等の農産物を仕分け箱に箱詰めする場合には、ベルトコンベアを流れる農産物の大きさを目視で判断し、人の手で仕分けをしている。 Conventionally, when packing agricultural products, such as vegetables or fruits, in a sorting box, the size of the agricultural products flowing on the belt conveyor is visually judged and sorting is performed by human hands.

これに対し、吸着カップホルダを搭載したロボットを用い、農産物の仕分けを自動で行う方法が提案されている（例えば特許文献１参照）。この方法では、吸着カップホルダを農産物に対して隙間を設けて近づけ、ブロアで吸引を行うことで農産物を吸着カップホルダに吸着させ、トレイパックへの収納を行う。 On the other hand, there has been proposed a method of automatically sorting produce by using a robot equipped with a suction cup holder (see, for example, Patent Document 1). In this method, the suction cup holder is brought close to the produce with a gap, and suction is performed by a blower to cause the produce to be absorbed by the suction cup holder and to be stored in the tray pack.

特開２００６−２０６１９３号公報JP, 2006-206193, A

このように、特許文献１に開示された方法では、吸着カップホルダを農産物に対して隙間を設けて近づけ、ブロアで吸引を行うことで農産物を吸着カップホルダに吸着させている。そのため、重量が大きい農産物及び形状が不均等な農産物では吸着が困難であるという課題がある。 As described above, in the method disclosed in Patent Document 1, the suction cup holder is brought close to the produce with a gap, and suction is performed with a blower to cause the produce to be adsorbed to the suction cup holder. Therefore, there is a problem that adsorption is difficult in agricultural products with large weights and agricultural products with uneven shapes.

この発明は、上記のような課題を解決するためになされたもので、物体の重量及び形状に寄らず、人の手を介さないで仕分けをすることができる仕分け装置を提供することを目的としている。 The present invention has been made to solve the problems as described above, and it is an object of the present invention to provide a sorting apparatus capable of sorting without depending on the weight and shape of an object and without human hands. There is.

この発明に係る仕分け装置は、ベルトコンベアを流れる物体を仕分けするピッキング装置を備え、ピッキング装置は、固定部、及び当該固定部に対して変位可能な可動部を有するアクチュエータと、可動部に取付けられ、ベルトコンベアを流れる物体を保持可能な保持部と、固定部に対する可動部の位置を検出する位置検出部と、固定部の加速度を検出する加速度検出部と、位置検出部により検出された位置と基準位置との差分に対してゲインを調整し、当該調整結果である電流指令値及び加速度検出部により検出された加速度に基づいてアクチュエータに対する駆動電流を出力するアクチュエータ制御部と、アクチュエータ制御部において得られた電流指令値、又は、加速度検出部により検出された加速度及びアクチュエータ制御部により出力された駆動電流の電流値に基づいて、可動部に加わる外力を検出する外力検出部と、外力検出部により検出された外力に基づいてアクチュエータ制御部及び保持部を制御する作業制御部とを備えたことを特徴とする。 A sorting apparatus according to the present invention includes a picking device for sorting objects flowing in a belt conveyor, and the picking device is attached to a fixed portion and an actuator having a movable portion displaceable with respect to the fixed portion, and a movable portion A holding unit capable of holding an object flowing through the belt conveyor, a position detecting unit detecting a position of the movable unit with respect to the fixed unit, an acceleration detecting unit detecting an acceleration of the fixed unit, and a position detected by the position detecting unit The actuator control unit adjusts the gain with respect to the difference with the reference position, and outputs the drive current to the actuator based on the current command value which is the adjustment result and the acceleration detected by the acceleration detection unit. Current command value or the acceleration detected by the acceleration detection unit and the actuator control unit An external force detection unit that detects an external force applied to the movable unit based on the current value of the driven drive current; and a work control unit that controls the actuator control unit and the holding unit based on the external force detected by the external force detection unit. It is characterized by having.

この発明によれば、上記のように構成したので、物体の重量及び形状に寄らず、人の手を介さないで仕分けをすることができる。 According to this invention, since it was comprised as mentioned above, according to the weight and shape of an object, it can sort without human's hand.

この発明の実施の形態１に係る仕分け装置の構成例を示す図である。It is a figure which shows the structural example of the classification apparatus which concerns on Embodiment 1 of this invention. この発明の実施の形態１におけるピッキング装置の構成例を示す図である。It is a figure which shows the structural example of the picking apparatus in 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 figure which shows the structural example of the operation | work control part in Embodiment 1 of this invention. この発明の実施の形態１に係る仕分け装置による仕分け作業の一例を示すフローチャートである。It is a flowchart which shows an example of the classification operation | work by the classification apparatus which concerns on Embodiment 1 of this invention. 図７Ａ、図７Ｂは、この発明の実施の形態１における推力制御部の動作例を説明する図であり、図７Ａはアクチュエータが発生する推力を示す図であり、図７Ｂは可動部の位置を示す図である。FIGS. 7A and 7B are diagrams for explaining an operation example of the thrust control unit according to the first embodiment of the present invention, FIG. 7A is a diagram showing the thrust generated by the actuator, and FIG. 7B shows the position of the movable portion. FIG. この発明の実施の形態１におけるピッキング装置を複数台設けた場合でのピッキング動作の一例を示す図である。It is a figure which shows an example of picking operation in the case where multiple picking apparatus in Embodiment 1 of this invention is provided.

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る仕分け装置の構成例を示す図である。
仕分け装置は、ベルトコンベア５を流れる農産物等の物体５０を仕分けする装置である。この仕分け装置は、図１に示すように、撮影装置１及びピッキング装置２を備えている。なお図１では、物体５０が農産物である場合を示し、符号６は物体５０が収納される出荷箱等の仕分け箱を示している。また図１における矢印は、物体５０が流れる方向を示している。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1
FIG. 1 is a view showing a configuration example of a sorting apparatus according to Embodiment 1 of the present invention.
The sorting device is a device that sorts objects 50 such as agricultural products flowing through the belt conveyor 5. As shown in FIG. 1, the sorting device includes an imaging device 1 and a picking device 2. In addition, in FIG. 1, the case where the object 50 is agricultural products is shown, and the code | symbol 6 has shown sorting boxes, such as a shipping box in which the object 50 is accommodated. The arrows in FIG. 1 indicate the direction in which the object 50 flows.

撮影装置１は、ベルトコンベア５を流れる物体５０を撮影する。この撮影装置１により撮影された画像を示す情報は、ピッキング装置２に出力される。なお図１では、撮影装置１とピッキング装置２との間の信号線の図示を省略している。また、撮影装置１は必須の構成ではなく、仕分け装置から取除いてもよい。以下では、撮影装置１を使用する場合を記述する。 The photographing device 1 photographs an object 50 flowing through the belt conveyor 5. Information indicating an image captured by the imaging device 1 is output to the picking device 2. In FIG. 1, illustration of signal lines between the photographing device 1 and the picking device 2 is omitted. Moreover, the imaging device 1 is not an essential component, and may be removed from the sorting device. Hereinafter, the case of using the photographing device 1 will be described.

ピッキング装置２は、１台以上設けられ、ベルトコンベア５を流れる物体５０を仕分けする。図１では、ピッキング装置２が１台設けられた場合を示している。このピッキング装置２は、図２に示すように、アクチュエータ２１、保持部２２、移動部２３、位置検出部２４、加速度検出部２５、外力検出制御部２６及び作業制御部２７を備えている。なお図２では、撮影装置１も図示している。また、外力検出制御部２６は、アクチュエータ制御部２６１及び外力検出部２６２から構成される。 One or more picking devices 2 are provided to sort the objects 50 flowing through the belt conveyor 5. FIG. 1 shows the case where one picking device 2 is provided. As shown in FIG. 2, the picking device 2 includes an actuator 21, a holding unit 22, a moving unit 23, a position detection unit 24, an acceleration detection unit 25, an external force detection control unit 26 and a work control unit 27. In FIG. 2, the photographing device 1 is also illustrated. Further, the external force detection control unit 26 includes an actuator control unit 261 and an external force detection unit 262.

アクチュエータ２１は、固定部２１１、及び当該固定部２１１に対して変位可能な可動部２１２を有し、磁界に置かれたコイルに電流が供給されることで固定部２１１に対して可動部２１２を直動方向又は回転方向に変位可能とする。このアクチュエータ２１は、移動部２３に取付けられており、全体が移送され、また、姿勢が変更される。なお、可動部２１２又は保持部２２が複数方向の自由度を持ち、アクチュエータ２１全体の移送及び姿勢の変更が不要である場合、移動部２３はなくてもよい。以下では、移動部２３を使用する場合を記述する。 The actuator 21 has a fixed portion 211 and a movable portion 212 which is displaceable with respect to the fixed portion 211, and a current is supplied to a coil placed in a magnetic field, whereby the movable portion 212 is fixed to the fixed portion 211. It can be displaced in the linear movement direction or the rotational direction. The actuator 21 is attached to the moving unit 23, the whole is transported, and the posture is changed. If the movable portion 212 or the holding portion 22 has a plurality of degrees of freedom, and there is no need to move the entire actuator 21 and change the posture, the moving portion 23 may be omitted. In the following, the case where the moving unit 23 is used will be described.

保持部２２は、可動部２１２に取付けられ、ベルトコンベア５を流れる物体５０を保持可能な機構である。図２では、保持部２２として、物体５０を把持可能なグリッパ（ハンド）が用いられている。なお、保持部２２としては、グリッパ以外にも、例えば、物体５０を吸着可能な吸着具（図８参照）を用いてもよい。また、ピッキング装置２が複数台設けられる場合には、各々のピッキング装置２が有する保持部２２が共同してベルトコンベア５を流れる物体５０のピッキングを行う。 The holding portion 22 is a mechanism attached to the movable portion 212 and capable of holding the object 50 flowing through the belt conveyor 5. In FIG. 2, a gripper (hand) capable of gripping the object 50 is used as the holding unit 22. In addition, as the holding part 22, you may use the suction tool (refer FIG. 8) which can adsorb | suck the object 50 other than a gripper, for example. When a plurality of picking devices 2 are provided, the holding portions 22 of the respective picking devices 2 jointly perform picking of the objects 50 flowing on the belt conveyor 5.

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

位置検出部２４は、アクチュエータ２１に設けられ、固定部２１１に対する可動部２１２の位置（相対位置）を検出する。この位置検出部２４により検出された位置を示す信号（位置信号）は、アクチュエータ制御部２６１に出力される。 The position detection unit 24 is provided in the actuator 21 and detects the position (relative position) of the movable unit 212 with respect to the fixed unit 211. A signal (position signal) indicating the position detected by the position detection unit 24 is output to the actuator control unit 261.

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

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

外力検出部２６２は、アクチュエータ制御部２６１において得られた電流指令値Ｉｒｐ、又は、加速度検出部２５により検出された加速度及びアクチュエータ制御部２６１により出力された駆動電流Ｉａの電流値に基づいて、可動部２１２に加わる外力（反力）Ｆを検出する。
アクチュエータ制御部２６１及び外力検出部２６２の構成例については後述する。 The external force detection unit 262 is movable based on the current command value Irp obtained by the actuator control unit 261, or the acceleration detected by the acceleration detection unit 25 and the current value of the drive current Ia output by the actuator control unit 261. An external force (reaction force) F applied to the portion 212 is detected.
Configuration examples of the actuator control unit 261 and the external force detection unit 262 will be described later.

作業制御部２７は、ピッキング装置２による仕分け作業を実現する。この際、作業制御部２７は、外力検出部２６２により検出された外力Ｆ及び撮影装置１により撮影された画像に基づいて、アクチュエータ制御部２６１、保持部２２及び移動部２３を制御することで、仕分け作業を実現する。なお、作業制御部２７は、基準位置Ｐｒ又はゲインの変更を行うことでアクチュエータ制御部２６１を制御する。また、作業制御部２７は、上記に加え、位置検出部２４により検出された位置、加速度検出部２５により検出された加速度、及び作業制御部２７で管理している時間等も考慮して、上記仕分け作業を実現してもよい。この作業制御部２７の構成例については後述する。 The work control unit 27 realizes sorting work by the picking device 2. At this time, the work control unit 27 controls the actuator control unit 261, the holding unit 22, and the moving unit 23 based on the external force F detected by the external force detection unit 262 and the image captured by the imaging device 1 Realize sorting work. The work control unit 27 controls the actuator control unit 261 by changing the reference position Pr or the gain. In addition to the above, the work control unit 27 takes into consideration the position detected by the position detection unit 24, the acceleration detected by the acceleration detection unit 25, the time managed by the work control unit 27, etc. A sorting operation may be realized. A configuration example of the work control unit 27 will be described later.

次に、外力検出制御部２６の構成例について、図３を参照しながら説明する。なお図３では、アクチュエータ２１、保持部２２、位置検出部２４及び加速度検出部２５も図示している。
外力検出制御部２６は、図３に示すように、位置速度変換部２６３、減算器２６４、ゲイン調整部２６５、質量推定部２６６、加速度補償部２６７、加減算器２６８、定電流制御部２６９、及び外力検出部２６２を有している。なお図３に示す外力検出制御部２６において、外力検出部２６２を除く機能部（位置速度変換部２６３、減算器２６４、ゲイン調整部２６５、質量推定部２６６、加速度補償部２６７、加減算器２６８及び定電流制御部２６９）は、アクチュエータ制御部２６１を構成する。 Next, a configuration example of the external force detection control unit 26 will be described with reference to FIG. In FIG. 3, the actuator 21, the holding unit 22, the position detection unit 24, and the acceleration detection unit 25 are also illustrated.
As shown in FIG. 3, the external force detection control unit 26 includes a position / speed conversion unit 263, a subtractor 264, a gain adjustment unit 265, a mass estimation unit 266, an acceleration compensation unit 267, an adder / subtractor 268, a constant current control unit 269, and An external force detection unit 262 is provided. In external force detection control unit 26 shown in FIG. 3, functional units (position / speed conversion unit 263, subtractor 264, gain adjustment unit 265, mass estimation unit 266, acceleration compensation unit 267, adder / subtractor 268, and the like) except external force detection unit 262. The constant current control unit 269) constitutes an actuator control unit 261.

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

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

ゲイン調整部２６５は、減算器２６４による減算結果（位置偏差）に対してゲインを調整し、電流指令値Ｉｒｐを出力する。ゲインは、アクチュエータ２１におけるコンプライアンスの値であり、コンプライアンスは、バネ定数の逆数であり、固さ柔らかさを示す指標である。また、ゲイン調整部２６５において、上記位置偏差と電流指令値Ｉｒｐとの関係を示す関数は線形でもよいし非線形でもよい。このゲイン調整部２６５は、図３，４に示すように、ループゲイン測定部２６５１、ゲイン交点制御部２６５２及び可変ゲイン調整部２６５３を有している。 The gain adjustment unit 265 adjusts the gain with respect to the subtraction result (positional deviation) by the subtractor 264, and outputs a current command value Irp. The gain is a value of compliance at the actuator 21, and the compliance is an inverse number of a spring constant, and is an index indicating hardness and hardness. Further, in the gain adjustment unit 265, 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. 3 and 4, the gain adjustment unit 265 includes a loop gain measurement unit 2651, a gain intersection control unit 2652, and a variable gain adjustment unit 2653.

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

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

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

なお、発振器２６５４でゲインが１倍となるべき基準となる周波数の正弦波を加算するのは、ゲインが１倍となる周波数においてＥａ／Ｅｂ＝１となるため、Ｅａ／Ｅｂ＝１となるようにゲインを調整することで、ゲイン交点を常に１に維持できるためである。 It should be noted that adding a sine wave of a reference frequency at which the gain should be 1 in the oscillator 2654 is such that Ea / Eb = 1 at the frequency at which the gain is 1 and therefore Ea / Eb = 1 The gain cross point can be always maintained at 1 by adjusting the gain to.

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

質量推定部２６６は、可変ゲイン調整部２６５３によるゲインの調整値から、可動部２１２側の質量を推定する。すなわち、質量推定部２６６は、ゲインの調整値の変化と質量の変化とが比例する原理を利用する。ここで、可動部２１２側の質量とは、保持部２２が物体５０を保持していない場合には、可動部２１２の質量Ｍ１と保持部２２の質量Ｍ２とが加算された質量（Ｍ１＋Ｍ２）であり、保持部２２が物体５０を保持している場合には、可動部２１２の質量Ｍ１と保持部２２の質量Ｍ２と物体５０の質量Ｍ３とが加算された質量（Ｍ１＋Ｍ２＋Ｍ３）である。なお図３では、質量推定部２６６が、可動部２１２の質量Ｍ１と保持部２２の質量Ｍ２とが加算された質量（Ｍ１＋Ｍ２）を推定する場合を示している。
例えば、可動部２１２側の質量が規定値の２倍になったとすると、ゲインはその逆数倍の１／２となっており、Ｅａ／Ｅｂ＝１／２となる。これに対して、ゲインを１倍とするため、可変ゲイン調整部２６５３は２倍の調整値でゲインを調整する。そして、質量推定部２６６は、この可変ゲイン調整部２６５３の調整値から、可動部２１２側の質量が規定値の２倍に変化したと推定できる。
この質量推定部２６６により推定された質量を示す信号は、加速度補償部２６７に出力される。 The mass estimation unit 266 estimates the mass on the movable unit 212 side from the adjustment value of the gain by the variable gain adjustment unit 2653. That is, the mass estimation unit 266 uses the principle that the change in gain adjustment value is proportional to the change in mass. Here, the mass on the movable part 212 side is a mass (M1 + M2) in which the mass M1 of the movable part 212 and the mass M2 of the holding part 22 are added when the holding part 22 does not hold the object 50. If the holding unit 22 holds the object 50, the mass M1 of the movable unit 212, the mass M2 of the holding unit 22 and the mass M3 of the object 50 are added (M1 + M2 + M3). FIG. 3 shows the case where the mass estimating unit 266 estimates a mass (M1 + M2) in which the mass M1 of the movable portion 212 and the mass M2 of the holding portion 22 are added.
For example, assuming that the mass on the movable portion 212 side is twice the specified value, the gain is 1⁄2 of the reciprocal thereof, and Ea / Eb = 1⁄2. On the other hand, in order to make the gain 1 time, the variable gain adjustment unit 2653 adjusts the gain with an adjustment value of 2 times. Then, from the adjustment value of the variable gain adjustment unit 2653, the mass estimation unit 266 can estimate that the mass on the movable unit 212 side has changed to twice the specified value.
The signal indicating the mass estimated by the mass estimation unit 266 is output to the acceleration compensation unit 267.

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

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

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

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

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

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

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

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

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

外力検出部２６２は、アクチュエータ制御部２６１において得られた電流指令値Ｉｒｐ、又は、加速度検出部２５により検出された加速度及びアクチュエータ制御部２６１により出力された駆動電流Ｉａの電流値に基づいて、可動部２１２に加わる外力Ｆを検出する。具体的には、外力検出部２６２は、電流指令値Ｉｒｐ、又は、駆動電流Ｉａの電流値から加速度補償値Ｉｒｃを減算した結果に基づいて、可動部２１２に加わる外力Ｆを検出する。なお、可動部２１２に加わる外力Ｆとしては、保持部２２が物体５０と接触した際に発生する力、及び、保持部２２により保持された物体５０が仕分け箱６と接触した際に発生する力が挙げられる。また図３では、外力検出部２６２が、加速度検出部２５により検出された加速度及びアクチュエータ制御部２６１により出力された駆動電流Ｉａの電流値に基づいて可動部２１２に加わる外力Ｆを検出する場合を示している。図３に示す外力検出部２６２は、係数乗算部２６２１、減算器２６２２及び係数乗算部２６２３を有している。 The external force detection unit 262 is movable based on the current command value Irp obtained by the actuator control unit 261, or the acceleration detected by the acceleration detection unit 25 and the current value of the drive current Ia output by the actuator control unit 261. The external force F applied to the portion 212 is detected. Specifically, the external force detection unit 262 detects the external force F applied to the movable portion 212 based on a result of subtracting the acceleration compensation value Irc from the current command value Irp or the current value of the drive current Ia. As the external force F applied to the movable portion 212, a force generated when the holding portion 22 contacts the object 50, and a force generated when the object 50 held by the holding portion 22 contacts the sorting box 6 Can be mentioned. Further, in FIG. 3, the external force detection unit 262 detects the external force F applied to the movable portion 212 based on the acceleration detected by the acceleration detection unit 25 and the current value of the drive current Ia output by the actuator control unit 261. It shows. The external force detection unit 262 illustrated in FIG. 3 includes a coefficient multiplication unit 2621, a subtractor 2622, and a coefficient multiplication unit 2623.

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

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

係数乗算部２６２３は、減算器２６２２による減算結果に係数（Ｋｔ）を乗算することで、外力Ｆを得る。この係数乗算部２６２３により得られた外力Ｆを示す信号は、作業制御部２７に出力される。 The coefficient multiplication unit 2623 obtains the external force F by multiplying the subtraction result of the subtractor 2622 by the coefficient (Kt). The signal indicating the external force F obtained by the coefficient multiplication unit 2623 is output to the work control unit 27.

なお、外力検出部２６２が、アクチュエータ制御部２６１において得られた電流指令値Ｉｒｐに基づいて可動部２１２に加わる外力Ｆを検出する場合には、係数乗算部を有する。この係数乗算部は、ゲイン調整部２６５から出力された電流指令値Ｉｒｐに係数（Ｋｔ）を乗算することで、外力Ｆを得る。そして、この係数乗算部により得られた外力Ｆを示す信号は、作業制御部２７に出力される。 When external force detection unit 262 detects external force F applied to movable unit 212 based on current command value Irp obtained by actuator control unit 261, it has a coefficient multiplication unit. The coefficient multiplication unit obtains the external force F by multiplying the current command value Irp output from the gain adjustment unit 265 by the coefficient (Kt). Then, the signal indicating the external force F obtained by the coefficient multiplication unit is output to the work control unit 27.

次に、作業制御部２７の構成例について、図５を参照しながら説明する。
作業制御部２７は、図５に示すように、接触制御部２７１、推力制御部２７２、保持制御部２７３、第２接触制御部２７４及び収納制御部２７５を有している。 Next, a configuration example of the work control unit 27 will be described with reference to FIG.
As shown in FIG. 5, the work control unit 27 includes a contact control unit 271, a thrust control unit 272, a holding control unit 273, a second contact control unit 274, and a storage control unit 275.

接触制御部２７１は、保持部２２がベルトコンベア５を流れる物体５０に力（第１の力）Ｆ１で接触するまで、当該保持部２２を移動させる。力Ｆ１は、保持部２２が物体５０に接触したことを認識可能な力であり、物体５０を破損しない程度に十分に弱い力である。 The contact control unit 271 moves the holding unit 22 until the holding unit 22 contacts the object 50 flowing through the belt conveyor 5 with the force (first force) F1. The force F1 is a force capable of recognizing that the holding portion 22 contacts the object 50, and a force weak enough not to damage the object 50.

推力制御部２７２は、接触制御部２７１による処理後、力リセットを実行する。力リセットとは、保持部２２による物体５０への接触を維持しつつ可動部２１２に加わる外力Ｆが力（第２の力）Ｆ２となるようにアクチュエータ２１が発生する推力を制御することである。力Ｆ２は、力Ｆ１より小さい力であり、限りなく零に近い力である。 The thrust control unit 272 executes force reset after processing by the contact control unit 271. The force reset is to control the thrust generated by the actuator 21 so that the external force F applied to the movable portion 212 becomes the force (second force) F2 while maintaining the contact to the object 50 by the holding portion 22. . The force F2 is a force smaller than the force F1 and is a force as close to zero as possible.

保持制御部２７３は、推力制御部２７２による処理後、保持部２２により当該保持部２２が接触している物体５０を保持させる。この際、保持制御部２７３は、撮影装置１により撮影された画像に基づいて、アクチュエータ制御部２６１、保持部２２及び移動部２３のうちの少なくとも一つを制御する。 The holding control unit 273 causes the holding unit 22 to hold the object 50 in contact with the holding unit 22 after the processing by the thrust control unit 272. At this time, the holding control unit 273 controls at least one of the actuator control unit 261, the holding unit 22, and the moving unit 23 based on the image captured by the imaging device 1.

第２接触制御部２７４は、保持制御部２７３による処理後、保持部２２により保持された物体５０が仕分け箱６に力（第３の力）Ｆ３で接触するまで、当該保持部２２を移動させる。力Ｆ３は、保持部２２により保持された物体５０が仕分け箱６に接触したことを認識可能な力であり、物体５０を破損しない程度に十分に弱い力である。 After processing by the holding control unit 273, the second contact control unit 274 moves the holding unit 22 until the object 50 held by the holding unit 22 contacts the sorting box 6 with a force (third force) F3. . The force F3 is a force capable of recognizing that the object 50 held by the holding unit 22 contacts the sorting box 6, and is sufficiently weak not to damage the object 50.

収納制御部２７５は、第２接触制御部２７４による処理後、保持部２２による物体５０の保持を解除する。 After being processed by the second contact control unit 274, the storage control unit 275 cancels the holding of the object 50 by the holding unit 22.

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

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

また、加速度検出部２５は、固定部２１１の直動方向における加速度を検出する。以下では、加速度検出部２５は、固定部２１１の直動方向成分における移動加速度α１と、固定部２１１の直動方向成分における重力加速度αｇとが加算された加速度（α１＋αｇ）を検出するものとする。 Further, the acceleration detection unit 25 detects the acceleration in the linear movement direction of the fixed unit 211. In the following, the acceleration detection unit 25 detects an acceleration (α1 + αg) in which the movement acceleration α1 in the linear movement direction component of the fixed unit 211 and the gravitational acceleration αg in the linear movement direction component of the fixed unit 211 are added. .

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

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

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

また、ゲイン調整部２６５では、位置制御ループのゲインを変えることで、アクチュエータ２１におけるコンプライアンスの値を変化させることができる。 Further, the gain adjusting unit 265 can change the value of the compliance of the actuator 21 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 that when there is a disturbance torque.
As a general disturbance torque, a reaction force F received from the holding unit 22 at the time of operation, a force generated by a gravitational acceleration αg and a moving acceleration α1, a loss torque of a reduction gear, and the like can be considered. Here, since the actuator 21 is a direct drive linear actuator, there is no decelerator, and it is not necessary to consider loss torque. Therefore, the drive current Ia has a value proportional to the force generated by the reaction force F, the gravitational acceleration αg and the movement acceleration α1 received from the holding portion 22 at the time of operation. In the following, it is assumed that the reaction force F is a force generated when the holding unit 22 contacts the object 50.

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

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

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

このように、外乱トルクを補正するための加速度補償値Ｉｒｃを式（３）のように設定すると、作業時に保持部２２から受ける反力Ｆと電流指令値Ｉｒｐは、比例関係になることがわかる。 As described above, when the acceleration compensation value Irc for correcting the disturbance torque is set as shown in equation (3), it is understood that the reaction force F received from the holding unit 22 at the time of operation and the current command value Irp have a proportional relationship. .

これは、作業時に保持部２２から受ける反力Ｆが零、つまり保持部２２が物体５０に接触していない場合、基準位置Ｐｒと実際の位置の差分に基づく電流指令値Ｉｒｐも零、つまり位置が変位しないことを意味している。
そして、保持部２２が物体５０に接触した際に生じる反力Ｆは、電流指令値Ｉｒｐを監視することで知ることができる。 This is because the reaction force F received from the holding unit 22 at the time of work is zero, that is, when the holding unit 22 does not contact the object 50, the current command value Irp based on the difference between the reference position Pr and the actual position is also zero, ie, the position Means not to be displaced.
The reaction force F generated when the holding unit 22 contacts the object 50 can be known by monitoring the current command value Irp.

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

なお、上述したように、保持部２２が物体５０と急激に衝突する等して発生する反力Ｆは、電流指令値Ｉｒｐを監視することで知ることができる。また、アクチュエータ２１には、反力Ｆと拮抗するように誘導電流が発生するため、駆動電流Ｉａから反力Ｆを検出することもできる。
しかしながら、位置制御ループにおいて、反力Ｆに対する電流指令値Ｉｒｐの応答は一般的に速くない。一方、反力Ｆに対する駆動電流Ｉａの応答は、可動部２１２が移動することにより発生する誘導電流によるものであるため、比較的速い。そこで、電流指令値Ｉｒｐを直接監視するのではなく、駆動電流Ｉａを監視することで反力Ｆの検出を行う。 Note that, as described above, the reaction force F generated when the holding portion 22 collides with the object 50 or the like rapidly can be known by monitoring the current command value Irp. Further, since an induced current is generated in the actuator 21 so as to antagonize the reaction force F, the reaction force F can also 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 212. Therefore, instead of directly monitoring the current command value Irp, the reaction force F is detected 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) and rearranging, 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 multiplying by the torque constant Kt.

次に、外力検出制御部２６による効果について説明する。
ロボットの動作は、一般的に、位置制御によりコントロールされる。そのため、物体５０の大きさの違い等により、予めプログラムされた物体５０の目標位置と実際の位置が異なる場合、保持部２２が物体５０に接触した際に大きな外力Ｆが発生し、物体５０に傷が付く恐れがある。 Next, the effect of the external force detection control unit 26 will be described.
The motion of the robot is generally controlled by position control. Therefore, when the target position and the actual position of the preprogrammed object 50 are different due to the size difference of the object 50, etc., a large external force F is generated when the holding unit 22 contacts the object 50, and There is a risk of damage.

その対策として、ロボットと保持部２２との間に力センサを設置し、保持部２２と物体５０との接触時に過大な外力Ｆが発生しそうになると力センサの検出結果をロボットにフィードバックし、過大な外力Ｆが発生しないようにする方法が考えられる。 As a countermeasure, a force sensor is installed between the robot and the holding unit 22. When an excessive external force F is likely to occur when the holding unit 22 contacts the object 50, the detection result of the force sensor is fed back to the robot. It is conceivable to prevent the generation of the external force F.

しかしながら、過大な外力Ｆが発生したことを検出して停止指令を出してもロボットは急には止まれないため、停止指令が出た時点から急激に減速しても接触位置からずれた位置で停止してしまい、物体５０を押し潰してしまう。そして、位置の行き過ぎ量は移動速度に比例するため、保持部２２を物体５０に近付ける速度を遅くせざるを得ない。 However, the robot does not stop suddenly even if it detects that an excessive external force F has been generated and issues a stop command, so even if it decelerates rapidly from the time the stop command is issued, it stops at a position deviated from the contact position. And the object 50 is crushed. Then, the amount of positional excess is proportional to the movement speed, so the speed at which the holding unit 22 approaches the object 50 must be reduced.

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

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

また、ロボットと保持部２２との間に力センサを設置した場合、ロボットが急激に減速すると、保持部２２の質量Ｍ２による影響で、力センサには負方向の加速度に比例した力が発生する。
ところが、上記加速度に比例した力と保持部２２の物体５０との接触により発生する外力Ｆとを区別することは難しく、区別するためにはロボットの減速時間を大幅に長くせざるを得ない。 When a force sensor is installed between the robot and the holding unit 22 and the robot decelerates rapidly, a force proportional to the negative acceleration is generated in the force sensor due to the mass M2 of the holding unit 22. .
However, it is difficult to distinguish between the force proportional to the acceleration and the external force F generated by the contact of the holding portion 22 with the object 50, and in order to distinguish, it is necessary to make the deceleration time of the robot significantly longer.

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

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

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

次に、作業制御部２７の動作例について、図６，７を参照しながら説明する。
作業制御部２７は、外力検出部２６２により検出された外力Ｆ及び撮影装置１により撮影された画像等に基づいて、アクチュエータ制御部２６１、保持部２２及び移動部２３を制御することで、ピッキング装置２による仕分け作業を実現する。なお、作業制御部２７は、基準位置Ｐｒ又はゲインの変更を行うことでアクチュエータ制御部２６１を制御する。ここで、ゲイン調整部２６５は位置偏差に基づいて電流指令値Ｉｒｐを出力しているが、上記ゲインの変更とは、上記位置偏差と電流指令値Ｉｒｐとの関係を示す関数の変更を意味している。また、上記関数の変更には、関数の傾きの変更も含まれる。 Next, an operation example of the work control unit 27 will be described with reference to FIGS.
The work control unit 27 controls the actuator control unit 261, the holding unit 22, and the moving unit 23 based on the external force F detected by the external force detection unit 262, the image captured by the imaging device 1, etc. Realize the sorting work according to 2. The work control unit 27 controls the actuator control unit 261 by changing the reference position Pr or the gain. Here, although the gain adjustment unit 265 outputs the current command value Irp based on the position deviation, the change of the gain means a change of a function indicating the relationship between the position deviation and the current command value Irp. ing. In addition, the change of the function includes the change of the slope of the function.

また図７Ａにおいて、横軸は時間を示し、縦軸はアクチュエータ２１が発生する推力を示している。また図７Ｂにおいて、横軸は時間を示し、縦軸は可動部２１２の位置を示している。また図７において、点ａは保持部２２がベルトコンベア５を流れる物体５０に接触したことを示し、点ｂは力リセットの開始を示し、点ｃは力リセットの完了を示している。 In FIG. 7A, the horizontal axis represents time, and the vertical axis represents the thrust generated by the actuator 21. In FIG. 7B, the horizontal axis represents time, and the vertical axis represents the position of the movable portion 212. Further, in FIG. 7, point a indicates that the holding unit 22 contacts the object 50 flowing on the belt conveyor 5, point b indicates the start of force reset, and point c indicates the completion of force reset.

ピッキング装置２による仕分け作業では、まず、図６に示すように、接触制御部２７１は、保持部２２がベルトコンベア５を流れる物体５０に力Ｆ１で接触するまで、当該保持部２２を移動させる（ステップＳＴ１）。図７Ａに示すように、力Ｆ１は、保持部２２が物体５０に接触したことを認識可能な力であり、物体５０を破損しない程度に十分に弱い力である。なお図７Ｂに示すように、保持部２２が物体５０に接触するまでは、可動部２１２は、位置が変化しない場合（実線）に限らず、自ら伸びる場合（破線）もあり得る。 In the sorting operation by the picking device 2, first, as shown in FIG. 6, the contact control unit 271 moves the holding unit 22 until the holding unit 22 contacts the object 50 flowing through the belt conveyor 5 with a force F1 ( Step ST1). As shown in FIG. 7A, the force F1 is a force capable of recognizing that the holding portion 22 has made contact with the object 50, and a force weak enough not to damage the object 50. As shown in FIG. 7B, until the holding portion 22 contacts the object 50, the movable portion 212 is not limited to the case where the position does not change (solid line), but may be the case where the movable portion 212 extends itself (broken line).

次いで、推力制御部２７２は、力リセットを実行し、保持部２２による物体５０への接触を維持しつつ可動部２１２に加わる外力Ｆが力Ｆ２となるようにアクチュエータ２１が発生する推力を制御する（ステップＳＴ２）。図７Ａに示すように、力Ｆ２は、力Ｆ１より小さい力であり、限りなく零に近い力である。このように、推力制御部２７２により可動部２１２の推力を制御することで、保持部２２による物体５０への接触を維持したまま保持部２２を物体５０上で移動させる（滑らせる）ことが可能となる。また、これにより、ピッキング装置２は、物体５０の高さを正確に検出可能となるため、その後の物体５０のピッキング制御が容易となる。 Next, the thrust control unit 272 executes force reset, and controls the thrust generated by the actuator 21 such that the external force F applied to the movable unit 212 becomes the force F2 while maintaining the contact of the holding unit 22 with the object 50. (Step ST2). As shown in FIG. 7A, the force F2 is a force smaller than the force F1, and is a force as close to zero as possible. As described above, by controlling the thrust of the movable portion 212 by the thrust control portion 272, the holding portion 22 can be moved (sliding) on the object 50 while maintaining the contact of the holding portion 22 with the object 50. It becomes. Moreover, since the picking apparatus 2 can detect the height of the object 50 correctly by this, picking control of the subsequent object 50 becomes easy.

次いで、保持制御部２７３は、保持部２２により当該保持部２２が接触している物体５０を保持させる（ステップＳＴ３）。この際、保持制御部２７３は、撮影装置１により撮影された画像に基づいて、アクチュエータ制御部２６１、保持部２２及び移動部２３のうちの少なくとも一つを制御する。
ここで、ベルトコンベア５を流れる物体５０は姿勢がそれぞれ異なる場合がある。そこで、保持制御部２７３は、撮影装置１により撮影された画像から物体５０の姿勢を検出し、その姿勢に基づいて、保持部２２の向きを、物体５０を保持し易い向きに変える。これにより、ピッキング装置２は、ベルトコンベア５を流れる物体５０を容易にピッキングできる。また、保持制御部２７３は、撮影装置１により撮影された画像から物体５０の大きさを検出し、その大きさに基づいて、保持部２２（グリッパ）の可動域を制御することも可能である。 Next, the holding control unit 273 causes the holding unit 22 to hold the object 50 in contact with the holding unit 22 (step ST3). At this time, the holding control unit 273 controls at least one of the actuator control unit 261, the holding unit 22, and the moving unit 23 based on the image captured by the imaging device 1.
Here, the objects 50 flowing through the belt conveyor 5 may have different attitudes. Therefore, the holding control unit 273 detects the posture of the object 50 from the image photographed by the photographing device 1 and changes the direction of the holding unit 22 to a direction in which the object 50 can be easily held based on the posture. Thus, the picking device 2 can easily pick the object 50 flowing on the belt conveyor 5. The holding control unit 273 can also detect the size of the object 50 from the image photographed by the photographing device 1 and control the movable range of the holding unit 22 (gripper) based on the size. .

なお上記では、保持制御部２７３が、撮影装置１により撮影された画像から物体５０の姿勢又は大きさを検出する場合を示した。しかしながら、これに限らず、撮影装置１が、画像に基づく物体５０の姿勢又は大きさの検出を行い、その検出結果を示す情報をピッキング装置２に出力してもよい。 In the above, the case where the holding control unit 273 detects the posture or the size of the object 50 from the image photographed by the photographing device 1 is shown. However, the present invention is not limited to this, and the imaging device 1 may detect the posture or size of the object 50 based on an image, and may output information indicating the detection result to the picking device 2.

次いで、第２接触制御部２７４は、保持部２２により保持された物体５０が仕分け箱６に力Ｆ３で接触するまで、当該保持部２２を移動させる（ステップＳＴ４）。力Ｆ３は、保持部２２により保持された物体５０が仕分け箱６に接触したことを認識可能な力であり、物体５０を破損しない程度に十分に弱い力である。 Next, the second contact control unit 274 moves the holding unit 22 until the object 50 held by the holding unit 22 contacts the sorting box 6 with a force F3 (step ST4). The force F3 is a force capable of recognizing that the object 50 held by the holding unit 22 contacts the sorting box 6, and is sufficiently weak not to damage the object 50.

収納制御部２７５は、保持部２２による物体５０の保持を解除する（ステップＳＴ５）。これにより、ピッキング装置２は、ピッキングしている物体５０を仕分け箱６に収納することができる。 The storage control unit 275 releases the holding of the object 50 by the holding unit 22 (step ST5). Thereby, the picking device 2 can store the picking object 50 in the sorting box 6.

以上の動作により、物体５０に傷を付けずに、且つ作業速度を落とさずに、ベルトコンベア５を流れる物体５０の仕分け作業が実施できる。 By the above-described operation, the sorting operation of the objects 50 flowing through the belt conveyor 5 can be performed without damaging the objects 50 and reducing the operation speed.

なお上記では、可動部２１２を直動方向に変位可能とするアクチュエータ２１を用いた場合を示した。しかしながら、これに限らず、加速度検出部２５が角加速度を検出可能であれば、可動部２１２を回転方向に変位可能とするアクチュエータ２１を用いることもできる。 In addition, in the above, the case where the actuator 21 which makes the movable part 212 displaceable in the linear motion direction was used was shown. However, the present invention is not limited to this, and it is also possible to use the actuator 21 capable of displacing the movable portion 212 in the rotational direction as long as the acceleration detection unit 25 can detect angular acceleration.

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

また上記では、ピッキング装置２が１台設けられた場合を示した。一方、ピッキング装置２を複数台設けた場合には、例えば図８に示すように、各々のピッキング装置２が互いに異なる方向から１つの物体５０を保持するよう制御する。このように、複数台のピッキング装置２を用いた場合には、１台のピッキング装置２を用いた場合に対し、物体５０をピッキングする際の自由度が増す。 Moreover, in the above, the case where one picking apparatus 2 was provided was shown. On the other hand, when a plurality of picking devices 2 are provided, for example, as shown in FIG. 8, each picking device 2 is controlled to hold one object 50 from different directions. As described above, when a plurality of picking devices 2 are used, the degree of freedom in picking the object 50 is increased as compared with the case where one picking device 2 is used.

以上のように、この実施の形態１によれば、ピッキング装置２は、固定部２１１及び可動部２１２を有するアクチュエータ２１と、可動部２１２に取付けられ、ベルトコンベア５を流れる物体５０を保持可能な保持部２２と、固定部２１１に対する可動部２１２の位置を検出する位置検出部２４と、固定部２１１の加速度を検出する加速度検出部２５と、位置検出部２４により検出された位置と基準位置Ｐｒとの差分に対してゲインを調整し、当該調整結果である電流指令値Ｉｒｐ及び加速度検出部２５により検出された加速度に基づいてアクチュエータ２１に対する駆動電流Ｉａを出力するアクチュエータ制御部２６１と、アクチュエータ制御部２６１において得られた電流指令値Ｉｒｐ、又は、加速度検出部２５により検出された加速度及びアクチュエータ制御部２６１により出力された駆動電流Ｉａの電流値に基づいて、可動部２１２に加わる外力Ｆを検出する外力検出部２６２と、外力検出部２６２により検出された外力Ｆに基づいてアクチュエータ制御部２６１及び保持部２２を制御する作業制御部２７とを備えたので、物体５０の重量及び形状に寄らず、人の手を介さないで仕分けをすることができる。 As described above, according to the first embodiment, the picking device 2 is attached to the actuator 21 having the fixed portion 211 and the movable portion 212 and the movable portion 212, and can hold the object 50 flowing through the belt conveyor 5. The position detection unit 24 that detects the position of the movable unit 212 with respect to the holding unit 22, the fixed unit 211, the acceleration detection unit 25 that detects the acceleration of the fixed unit 211, the position detected by the position detection unit 24 and the reference position Pr And an actuator control unit 261 that outputs a drive current Ia to the actuator 21 based on the current command value Irp that is the adjustment result and the acceleration detected by the acceleration detection unit 25; Current command value Irp obtained in unit 261 or acceleration detected by acceleration detection unit 25 The external force detection unit 262 detects an external force F applied to the movable unit 212 based on the current value of the drive current Ia output by the actuator control unit 261, and actuator control based on the external force F detected by the external force detection unit 262 Since the work control unit 27 that controls the unit 261 and the holding unit 22 is provided, it is possible to sort without using the hand of a person without depending on the weight and shape of the object 50.

なお、本願発明はその発明の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。 In the present invention, within the scope of the invention, modifications of optional components of the embodiment or omission of optional components of the embodiment is possible.

１体積測定装置
２ピッキング装置
５ベルトコンベア
６仕分け箱
２１アクチュエータ
２２保持部
２３移動部
２４位置検出部
２５加速度検出部
２６外力検出制御部
２７作業制御部
５０物体
２１１固定部
２１２可動部
２６１アクチュエータ制御部
２６２外力検出部
２６３位置速度変換部
２６４減算器
２６５ゲイン調整部
２６６質量推定部
２６７加速度補償部
２６８加減算器
２６９定電流制御部
２７１接触制御部
２７２推力制御部
２７３保持制御部
２７４第２接触制御部
２７５収納制御部
２６２１係数乗算部
２６２２減算器
２６２３係数乗算部
２６５１ループゲイン測定部
２６５２ゲイン交点制御部
２６５３可変ゲイン調整部
２６５４発振器
２６５５加算器
２６５６比較器
２６７１乗算器
２６７２係数乗算部
２６９１減算器
２６９２駆動ドライバ
２６９３電流検出部 DESCRIPTION OF SYMBOLS 1 Volumetric measuring apparatus 2 Picking apparatus 5 Belt conveyor 6 Sorting box 21 Actuator 22 Holding part 23 Moving part 24 Position detection part 25 Acceleration detection part 26 External force detection control part 27 Work control part 50 Object 211 Fixed part 212 Moveable part 261 Actuator control part 262 external force detection unit 263 position / speed conversion unit 264 subtractor 265 gain adjustment unit 266 mass estimation unit 267 acceleration compensation unit 268 adder / subtractor 269 constant current control unit 271 contact control unit 272 thrust control unit 273 holding control unit 274 second contact control unit 275 storage control unit 2621 coefficient multiplication unit 2622 subtraction unit 2623 coefficient multiplication unit 2651 loop gain measurement unit 2652 gain intersection control unit 2653 variable gain adjustment unit 2654 oscillator 2655 adder 2656 comparator 2671 multiplier 2672 coefficient multiplication unit 2691 subtraction 2692 Drive driver 2693 Current detection unit

Claims

It has a picking device to sort the objects flowing on the belt conveyor,
The picking device
An actuator having a fixed part and a movable part displaceable with respect to the fixed part;
A holder attached to the movable portion and capable of holding an object flowing through the belt conveyor;
A position detection unit that detects the position of the movable unit with respect to the fixed unit;
An acceleration detection unit that detects an acceleration of the fixed unit;
The gain is adjusted for the difference between the position detected by the position detection unit and the reference position, and the drive current to 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 control unit that outputs
The external force applied to the movable portion is detected based on the current command value obtained by the actuator control unit or the current value of the acceleration detected by the acceleration detection unit and the drive current output by the actuator control unit. An external force detection unit,
A sorting apparatus comprising: a work control unit configured to control the actuator control unit and the holding unit based on an external force detected by the external force detection unit.

The work control unit
The sorting apparatus according to claim 1, further comprising a contact control unit configured to move the holding unit until the holding unit contacts the object flowing on the belt conveyor with a first force.

The work control unit
After processing by the contact control unit, controlling the thrust generated by the actuator such that the external force applied to the movable portion becomes a second force smaller than the first force while maintaining the contact to the object by the holding unit The sorting apparatus according to claim 2, further comprising a thrust control unit.

The work control unit
The sorting apparatus according to claim 3, further comprising a holding control unit configured to hold an object with which the holding unit is in contact by the holding unit after processing by the thrust control unit.

A photographing device for photographing an object flowing on the belt conveyor;
5. The sorting apparatus according to claim 4, wherein the holding control unit controls at least one of the actuator and the holding unit based on an image captured by the imaging device.

The sorting apparatus according to any one of claims 1 to 5, wherein the holding unit is a hand capable of gripping an object or a suction tool capable of sucking and adsorbing an object.

A plurality of picking devices are provided. The sorting device according to any one of claims 1 to 6, wherein the plurality of picking devices are provided.