JP6400321B2

JP6400321B2 - Work holding method

Info

Publication number: JP6400321B2
Application number: JP2014074214A
Authority: JP
Inventors: 孝樹水谷; 啓貴前本; 大輔中川
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2014-03-31
Filing date: 2014-03-31
Publication date: 2018-10-03
Anticipated expiration: 2034-03-31
Also published as: JP2015196208A

Description

本発明は、互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法に関する。 The present invention relates to a work holding method for holding a work with a first gripping finger and a second gripping finger that can face each other.

例えば、自動車あるいはその関連部品等の生産ラインでは、多数のロボットが用いられている。この種の生産ラインにおける生産効率を向上させるためには、ロボットで作業可能であると考えられる作業の可及的大部分をロボットに行わせることが望ましい。例えば、ハンド部に２個の把持指を有するロボットを用い、電気機器のコネクタ挿入孔にコネクタを挿入する工程を自動化することが考えられる。例えば、特許文献１では、ロボットのアーム部の先端に設けられたハンド部でコネクタを把持し、コンプライアンス制御を用いてコネクタを固定部に挿入することが提案されている。 For example, many robots are used in production lines for automobiles or related parts. In order to improve the production efficiency in this type of production line, it is desirable to cause the robot to perform as much of the work as possible that can be performed by the robot. For example, it is conceivable to automate the process of inserting a connector into a connector insertion hole of an electric device using a robot having two gripping fingers in the hand portion. For example, Patent Document 1 proposes that a connector is gripped by a hand portion provided at the tip of an arm portion of a robot, and the connector is inserted into a fixed portion using compliance control.

特開平４−１８９４９５号公報JP-A-4-189495

特許文献１の第５図等に示されるように、コネクタは、２個の長辺部と短辺部を有し、一方の短辺部に端子部が設けられ、且つ他方の短辺部にケーブルが接続された長尺体であることが一般的である。このような形状のコネクタを２個の把持指で把持するときには、把持指の長手方向と、コネクタの２個の長辺部とが直交するような姿勢が望ましい。コネクタを安定的に保持して、前記端子部をコネクタ挿入孔に確実に挿入し得るようにするためである。 As shown in FIG. 5 of Patent Document 1, the connector has two long side portions and short side portions, one short side portion is provided with a terminal portion, and the other short side portion is provided. Generally, it is a long body to which a cable is connected. When a connector having such a shape is gripped by two gripping fingers, a posture in which the longitudinal direction of the gripping finger and the two long sides of the connector are orthogonal to each other is desirable. This is to stably hold the connector so that the terminal portion can be reliably inserted into the connector insertion hole.

しかしながら、コネクタ（ワーク）の状態によっては、ケーブルの反力等の外乱によってコネクタに回転方向の力が作用することがある。このような事態が生じると、保持が不安定となる一因となる。 However, depending on the state of the connector (workpiece), a force in the rotational direction may act on the connector due to disturbance such as a reaction force of the cable. When such a situation occurs, it becomes a cause that the holding becomes unstable.

本発明は上記した問題を解決するためになされたもので、上記したようなワーク（例えば、コネクタ）の位置・姿勢のずれを吸収するコンプライアンス制御を実現しつつ、外力による望ましくない過剰な姿勢変化を抑制するワーク保持方法を提供することを目的とする。 The present invention has been made in order to solve the above-described problems, and realizes compliance control that absorbs the position / posture deviation of the workpiece (for example, the connector) as described above, and an undesirable excessive posture change due to an external force. An object of the present invention is to provide a work holding method for suppressing the above-described problem.

前記の目的を達成するために、本発明は、互いに対向可能な第１の把持指及び第２の把持指でワークを保持するワーク保持方法において、
前記第１の把持指及び前記第２の把持指の各々に第１のコンプライアンスモデル、第２のコンプライアンスモデルを設定し、且つ前記第１の把持指と前記第２の把持指の間に第３のコンプライアンスモデルを直接設定し、前記第１のコンプライアンスモデル、前記第２のコンプライアンスモデル及び前記第３のコンプライアンスモデルに基づいて、前記第１の把持指又は前記第２の把持指の移動先の目標座標を変数として含む連立運動方程式を構築する工程と、
前記第３のコンプライアンスモデルに基づく拘束条件下で前記第１の把持指と前記第２の把持指が相互に連動する前記目標座標を算出する工程と、
前記第１の把持指又は前記第２の把持指を前記目標座標に移動させる工程と、
を有し、
前記ワークに回転モーメントが生じて前記第１の把持指及び前記第２の把持指に反力が作用したとき、前記ワークの回転を防止するように前記目標座標を移動させることで前記第１の把持指及び前記第２の把持指の相対距離を一定に保つことを特徴とする。
To achieve the above object, the present invention provides a work holding method for holding a work with a first gripping finger and a second gripping finger that can face each other.
A first compliance model and a second compliance model are set for each of the first gripping finger and the second gripping finger, and a third is set between the first gripping finger and the second gripping finger. The compliance model of the first gripping finger or the second gripping finger is moved based on the first compliance model, the second compliance model, and the third compliance model. Constructing simultaneous equations of motion containing coordinates as variables;
Calculating the target coordinates at which the first gripping finger and the second gripping finger are interlocked with each other under a constraint condition based on the third compliance model;
Moving the first gripping finger or the second gripping finger to the target coordinates;
I have a,
When a rotational moment is generated in the workpiece and a reaction force acts on the first gripping finger and the second gripping finger, the first coordinate is moved by moving the target coordinates so as to prevent the workpiece from rotating. The relative distance between the gripping finger and the second gripping finger is kept constant .

すなわち、本発明においては、第１の把持指と第２の把持指とが仮想的な連結機構（例えば、仮想スタビライザ）によって連結されていると仮定し、両把持指の間に第３のコンプライアンスモデルを設定する。そして、第１〜第３のコンプライアンスモデルに基づいて構築される連立運動方程式を解くことにより、第１の把持指ないし第２の把持指の移動先の目標座標（目標位置）が求められる。 That is, in the present invention, it is assumed that the first gripping finger and the second gripping finger are coupled by a virtual coupling mechanism (for example, a virtual stabilizer), and the third compliance is between the gripping fingers. Set the model. Then, by solving the simultaneous equations of motion constructed based on the first to third compliance models, target coordinates (target positions) of the movement destinations of the first gripping finger or the second gripping finger are obtained.

第１の把持指ないし第２の把持指は、この目標座標（目標位置）となるように移動される。その結果、ワークの状態に関わらず、該ワークが安定して保持されるようになる。 The first gripping finger or the second gripping finger is moved so as to have this target coordinate (target position). As a result, the workpiece is stably held regardless of the workpiece state.

なお、ワークとしては、例えば、ケーブルが接続されたコネクタを挙げることができる。 In addition, as a workpiece | work, the connector to which the cable was connected can be mentioned, for example.

本発明によれば、第１の把持指と第２の把持指との間に仮想的な連結機構（例えば、仮想スタビライザ）が存在しているとの仮定に基づき、両把持指の間に第３のコンプライアンスモデルを設定するようにしている。この場合、第１〜第３のコンプライアンスモデルに基づいて連立運動方程式が構築される。該連立運動方程式を解くことにより、第１の把持指ないし第２の把持指の移動先の目標座標（目標位置）が求められる。 According to the present invention, based on the assumption that a virtual coupling mechanism (for example, a virtual stabilizer) exists between the first gripping finger and the second gripping finger, 3 compliance models are set. In this case, simultaneous equations of motion are constructed based on the first to third compliance models. By solving the simultaneous equation of motion, the target coordinates (target position) of the movement destination of the first gripping finger or the second gripping finger are obtained.

第１の把持指ないし第２の把持指は、このようにして求められた目標座標（目標位置）となるように移動される。その結果、ワークを安定して保持することができるようになる。 The first gripping finger or the second gripping finger is moved so as to be the target coordinates (target position) thus determined. As a result, the workpiece can be stably held.

本発明の実施の形態に係るコネクタ挿入装置の概略構成図である。It is a schematic block diagram of the connector insertion apparatus which concerns on embodiment of this invention. 図１に示したコネクタ挿入装置におけるハンド部の斜視図である。It is a perspective view of the hand part in the connector insertion apparatus shown in FIG. コネクタ挿入装置の制御系統を示すブロック図である。It is a block diagram which shows the control system of a connector insertion apparatus. 前記ハンド部のコンプライアンスモデルを示すモデル図である。It is a model figure which shows the compliance model of the said hand part. 図５Ａ〜図５Ｃは、第１の把持指と第２の把持指との間に仮想スタビライザに関するコンプライアンスモデルを構築していない従来技術において、コネクタにトルクが加わったときの第１の把持指及び第２の把持指の変位をフローとして示した要部概略斜視図である。FIG. 5A to FIG. 5C show the first gripping finger when torque is applied to the connector in the prior art in which a compliance model related to the virtual stabilizer is not constructed between the first gripping finger and the second gripping finger. It is the principal part schematic perspective view which showed the displacement of the 2nd holding finger as a flow. 図６Ａ及び図６Ｂは、第１の把持指と第２の把持指との間に仮想スタビライザに関するコンプライアンスモデルを構築した実施の形態において、コネクタが回転することを抑制しながらコネクタを移動させるフローを示した要部概略斜視図である。6A and 6B show a flow of moving the connector while suppressing the rotation of the connector in the embodiment in which the compliance model relating to the virtual stabilizer is constructed between the first gripping finger and the second gripping finger. It is the principal part schematic perspective view shown. 図７Ａ〜図７Ｄは、コネクタがコネクタ挿入孔に挿入されるまでの過程を示した要部斜視図である。FIG. 7A to FIG. 7D are perspective views of relevant parts showing a process until the connector is inserted into the connector insertion hole. 図８Ａ〜図８Ｃは、コネクタがコネクタ挿入孔に挿入されるまでの把持指の動作を示した要部平面図である。8A to 8C are main part plan views showing the operation of the gripping finger until the connector is inserted into the connector insertion hole.

以下、本発明に係るワーク保持方法につき、それを実施するための装置との関係で好適な実施の形態を挙げ、添付の図面を参照しながら詳細に説明する。なお、本実施の形態においては、コネクタをワークとして把持する場合を例示するものとする。 Hereinafter, a preferred embodiment of the work holding method according to the present invention will be described in detail in relation to an apparatus for carrying out the work, and will be described in detail with reference to the accompanying drawings. In the present embodiment, a case where a connector is gripped as a workpiece is illustrated.

図１は、本実施の形態に係るワーク保持方法を実施するためのコネクタ挿入装置１０の概略構成図である。コネクタ挿入装置１０は、図７Ａ等に示す電気機器のコネクタ挿入孔７５にコネクタ７０を挿入するための装置である。このコネクタ挿入装置１０は、ロボット１２と、このロボット１２を制御する制御部１４とを備える。 FIG. 1 is a schematic configuration diagram of a connector insertion device 10 for carrying out the work holding method according to the present embodiment. The connector insertion device 10 is a device for inserting the connector 70 into the connector insertion hole 75 of the electrical apparatus shown in FIG. 7A and the like. The connector insertion device 10 includes a robot 12 and a control unit 14 that controls the robot 12.

ロボット１２は、複数の駆動軸を有する多関節ロボットである。具体的には、ロボット１２は、ベース台１６と、鉛直な駆動軸を中心としてベース台１６に回動可能に支持された旋回部１８と、旋回部１８に回動可能に支持された多関節アーム２０と、多関節アーム２０の先端に設けられたハンド部２２とを有する。 The robot 12 is an articulated robot having a plurality of drive axes. Specifically, the robot 12 includes a base table 16, a turning unit 18 that is rotatably supported by the base table 16 around a vertical drive shaft, and a multi-joint that is rotatably supported by the turning unit 18. The arm 20 has a hand portion 22 provided at the tip of the multi-joint arm 20.

ロボット１２は、複数のアーム２０ａ〜２０ｃを有する多関節アーム２０の動作により、３次元空間上でハンド部２２の位置及び姿勢を変化させる。ロボット１２は、制御部１４の制御により、ロボット１２に設けられた各駆動軸が数値制御され、３次元空間上でのハンド部２２の位置及び姿勢が制御されるようになっている。 The robot 12 changes the position and posture of the hand unit 22 in the three-dimensional space by the operation of the articulated arm 20 having a plurality of arms 20a to 20c. In the robot 12, each drive axis provided in the robot 12 is numerically controlled by the control of the control unit 14, and the position and posture of the hand unit 22 in the three-dimensional space are controlled.

図２は、ハンド部２２の斜視図である。ハンド部２２は、コネクタ７０（図７Ａ参照）を把持可能な把持機構として構成される。ハンド部２２は、第１緩衝アーム２３ａ、第２緩衝アーム２３ｂの各先端に設けられた第１の把持指２４ａ、第２の把持指２４ｂと、第１の把持指２４ａ、第２の把持指２４ｂを移動させる２つの移動機構２６ａ、２６ｂと、２つの移動機構２６ａ、２６ｂが取り付けられたベース部２８とを有する。本実施の形態において、第１の把持指２４ａ、第２の把持指２４ｂは、平面状の把持面２５ａ、２５ｂを有する棒状の長尺体として構成され、互いに平行に配置される。 FIG. 2 is a perspective view of the hand unit 22. The hand unit 22 is configured as a gripping mechanism capable of gripping the connector 70 (see FIG. 7A). The hand unit 22 includes a first gripping finger 24a, a second gripping finger 24b, a first gripping finger 24a, and a second gripping finger provided at the tips of the first buffering arm 23a and the second buffering arm 23b. It has two moving mechanisms 26a and 26b for moving 24b, and a base portion 28 to which the two moving mechanisms 26a and 26b are attached. In the present embodiment, the first gripping finger 24a and the second gripping finger 24b are configured as rod-like long bodies having planar gripping surfaces 25a and 25b, and are arranged in parallel to each other.

第１の把持指２４ａ、第２の把持指２４ｂの先端には、それぞれ、図２及び図３に示すように、第１指用力覚センサ２７ａ、第２指用力覚センサ２７ｂが設けられる。 As shown in FIGS. 2 and 3, a first finger force sensor 27a and a second finger force sensor 27b are provided at the tips of the first grip finger 24a and the second grip finger 24b, respectively.

第１指用力覚センサ２７ａ、第２指用力覚センサ２７ｂによる検出情報は、制御部１４を構成する目標位置算出部１０８に信号として送信される。なお、制御部１４は、前記目標位置算出部１０８の他、サーボモータ制御部１１０を含んで構成される。 Information detected by the first finger force sensor 27 a and the second finger force sensor 27 b is transmitted as a signal to the target position calculation unit 108 constituting the control unit 14. The control unit 14 includes a servo motor control unit 110 in addition to the target position calculation unit 108.

第１の把持指２４ａ、第２の把持指２４ｂ（図２参照）は互いに対向可能である。第１の把持指２４ａ、第２の把持指２４ｂは、動作位置によって、それらの把持面２５ａ、２５ｂ同士が対向する位置となる場合（図２中のＸ方向、Ｙ方向及びＺ方向の位置が一致する場合）もあるが、把持面２５ａ、２５ｂ同士が対向する位置とならない場合もある。 The first gripping finger 24a and the second gripping finger 24b (see FIG. 2) can face each other. When the first gripping finger 24a and the second gripping finger 24b are in positions where the gripping surfaces 25a and 25b face each other depending on the operating position (the positions in the X, Y, and Z directions in FIG. 2 are In some cases, the gripping surfaces 25a and 25b may not be in a position facing each other.

第１の把持指２４ａを移動させるための移動機構２６ａは、ベース部２８に対してＹ方向（ベース部２８の延在方向）にスライド可能な第１移動部３０ａと、第１移動部３０ａに対してＺ方向にスライド可能な第２移動部３２ａと、第２移動部３２ａに対してＸ方向に移動可能な第３移動部３４ａとを有する。第１の把持指２４ａは、第１指用力覚センサ２７ａを介して、第３移動部３４ａの先端に取り付けられる。従って、第１の把持指２４ａは、Ｘ方向、Ｙ方向及びＺ方向の直交３軸の動作が可能である。 The moving mechanism 26a for moving the first gripping finger 24a includes a first moving unit 30a that can slide in the Y direction (extending direction of the base unit 28) with respect to the base unit 28, and a first moving unit 30a. On the other hand, it has the 2nd moving part 32a which can be slid in a Z direction, and the 3rd moving part 34a which can move to a X direction with respect to the 2nd moving part 32a. The first gripping finger 24a is attached to the tip of the third moving unit 34a via the first finger force sensor 27a. Therefore, the first gripping finger 24a can perform three-axis operation in the X direction, the Y direction, and the Z direction.

第１移動部３０ａは、第１駆動部３６の作用によりベース部２８に対してＹ方向にスライド可能である。第１駆動部３６は、サーボモータ４０ａと、ボールネジ及びナットからなるボールネジ機構３８ａとを有する。サーボモータ４０ａは、制御部１４を構成するサーボモータ制御部１１０（図３参照）により駆動制御され、これにより、第１の把持指２４ａのＹ方向の位置が制御される。 The first moving unit 30 a is slidable in the Y direction with respect to the base unit 28 by the action of the first driving unit 36. The first drive unit 36 includes a servo motor 40a and a ball screw mechanism 38a including a ball screw and a nut. The servo motor 40a is driven and controlled by a servo motor control unit 110 (see FIG. 3) constituting the control unit 14, and thereby the position of the first gripping finger 24a in the Y direction is controlled.

第２移動部３２ａは、第２駆動部４２の作用により第１移動部３０ａに対してＺ方向にスライド可能である。第２駆動部４２は、サーボモータ４４ａと、ボールネジ及びナットからなるボールネジ機構４６ａとを有する。サーボモータ４４ａもサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第１の把持指２４ａのＺ方向の位置が制御される。 The second moving part 32a is slidable in the Z direction with respect to the first moving part 30a by the action of the second driving part 42. The second drive unit 42 includes a servo motor 44a and a ball screw mechanism 46a including a ball screw and a nut. The servo motor 44a is also drive-controlled by the servo motor control unit 110 (see FIG. 3), and thereby the position of the first gripping finger 24a in the Z direction is controlled.

第３移動部３４ａは、第３駆動部４８の作用により第２移動部３２ａに対してＸ方向にスライド可能である。第３駆動部４８は、サーボモータ５０ａと、ボールネジ及びナットからなボールネジ機構５２ａとを有する。サーボモータ５０ａもサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第１の把持指２４ａのＸ方向の位置が制御される。 The third moving unit 34a is slidable in the X direction with respect to the second moving unit 32a by the action of the third driving unit 48. The third drive unit 48 includes a servo motor 50a and a ball screw mechanism 52a including a ball screw and a nut. The servo motor 50a is also driven and controlled by the servo motor control unit 110 (see FIG. 3), and thereby the position of the first gripping finger 24a in the X direction is controlled.

第２の把持指２４ｂを移動させる移動機構２６ｂは、第１駆動部５４の作用によりベース部２８に対してＹ方向にスライド可能な第１移動部３０ｂと、第２駆動部５６の作用により第１移動部３０ｂに対してＺ方向にスライド可能な第２移動部３２ｂと、第３駆動部５８の作用により第２移動部３２ｂに対してＸ方向に移動可能な第３移動部３４ｂとを有する。第２の把持指２４ｂは、第２指用力覚センサ２７ｂを介して、第３移動部３４ｂの先端に取り付けられる。従って、他方の第２の把持指２４ｂは、Ｘ方向、Ｙ方向及びＺ方向の直交３軸の動作が可能である。 The moving mechanism 26b that moves the second gripping finger 24b includes a first moving unit 30b that is slidable in the Y direction with respect to the base unit 28 by the operation of the first driving unit 54, and a second driving unit 56 that is operated by the second driving unit 56. The second moving unit 32b is slidable in the Z direction with respect to the first moving unit 30b, and the third moving unit 34b is movable in the X direction with respect to the second moving unit 32b by the action of the third driving unit 58. . The second gripping finger 24b is attached to the tip of the third moving unit 34b via the second finger force sensor 27b. Accordingly, the other second gripping finger 24b can perform three-axis operation in the X direction, the Y direction, and the Z direction.

他方の移動機構２６ｂにおける第１移動部３０ｂ、第２移動部３２ｂ及び第３移動部３４ｂは、それぞれ、上述した一方の移動機構２６ａにおける第１移動部３０ａ、第２移動部３２ａ及び第３移動部３４ａと同様に構成される。 The first moving unit 30b, the second moving unit 32b, and the third moving unit 34b in the other moving mechanism 26b are respectively the first moving unit 30a, the second moving unit 32a, and the third moving unit in the one moving mechanism 26a described above. It is comprised similarly to the part 34a.

また、他方の移動機構２６ｂにおける第１駆動部５４、第２駆動部５６及び第３駆動部５８は、それぞれ、上述した一方の移動機構２６ａにおける第１駆動部３６、第２駆動部４２及び第３駆動部４８と同様に構成される。すなわち、第１駆動部５４は、サーボモータ４０ｂとボールネジ機構３８ｂを有する。また、第２駆動部５６はサーボモータ４４ｂとボールネジ機構４６ｂを有し、第３駆動部５８はサーボモータ５０ｂとボールネジ機構５２ｂを有する。サーボモータ４０ｂ、４４ｂ、５０ｂは、制御部１４を構成するサーボモータ制御部１１０（図３参照）によって駆動制御され、これにより、第２の把持指２４ｂのＸ方向、Ｚ方向の位置が制御される。 In addition, the first driving unit 54, the second driving unit 56, and the third driving unit 58 in the other moving mechanism 26b are respectively the first driving unit 36, the second driving unit 42, and the second driving unit 26 in the one moving mechanism 26a. The third drive unit 48 is configured in the same manner. That is, the first drive unit 54 includes a servo motor 40b and a ball screw mechanism 38b. The second drive unit 56 has a servo motor 44b and a ball screw mechanism 46b, and the third drive unit 58 has a servo motor 50b and a ball screw mechanism 52b. The servo motors 40b, 44b, and 50b are driven and controlled by a servo motor control unit 110 (see FIG. 3) that constitutes the control unit 14, and thereby the positions of the second gripping fingers 24b in the X direction and the Z direction are controlled. The

上記のように構成される各移動機構２６ａ、２６ｂにより、第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させることができる。すなわち、制御部１４の制御作用下に、各移動機構２６ａ、２６ｂの作用下に第１の把持指２４ａ、第２の把持指２４ｂをＸ方向、Ｙ方向又はＺ方向に移動させることにより、ベース部２８に対する第１の把持指２４ａ、第２の把持指２４ｂの位置を個別に調整することが可能である。また、サーボモータ制御部１１０では、各エンコーダからの出力値を微分することで、第１の把持指２４ａ、第２の把持指２４ｂの速度を算出する。この値に基づき、速度制限を行うことができる。 The first holding fingers 24a and the second holding fingers 24b can be individually moved by the moving mechanisms 26a and 26b configured as described above. That is, the base 14 is moved by moving the first gripping finger 24a and the second gripping finger 24b in the X direction, Y direction, or Z direction under the control action of the control unit 14 under the action of the moving mechanisms 26a and 26b. It is possible to individually adjust the positions of the first holding finger 24a and the second holding finger 24b with respect to the unit 28. Further, the servo motor control unit 110 calculates the speeds of the first holding finger 24a and the second holding finger 24b by differentiating the output values from the respective encoders. Based on this value, the speed can be limited.

ベース部２８には、ブラケット６０を介してカメラ６２が取り付けられる。カメラ６２は、第１の把持指２４ａ、第２の把持指２４ｂ側を指向しており、第１の把持指２４ａ、第２の把持指２４ｂ及びその周辺を撮像可能な視野を有する。カメラ６２により撮像される画像情報は、制御部１４に送信され、制御部１４においてパターンマッチングを行い、把持部により把持されたコネクタ７０の位置及び姿勢を認識することが可能である。 A camera 62 is attached to the base portion 28 via a bracket 60. The camera 62 is directed toward the first gripping finger 24a and the second gripping finger 24b, and has a field of view capable of capturing images of the first gripping finger 24a, the second gripping finger 24b, and the periphery thereof. Image information captured by the camera 62 is transmitted to the control unit 14, and pattern matching is performed in the control unit 14 to recognize the position and orientation of the connector 70 held by the holding unit.

なお、本実施の形態におけるハンド部２２では、直交３軸の機構で構成された移動機構２６ａ、２６ｂにより第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させる構成となっているが、ハンド部２２の構成はこれに限られない。例えば、ハンド部２２において、直線動作と回転動作が可能な移動機構を第１の把持指２４ａ、第２の把持指２４ｂ毎に設け、それらの移動機構により第１の把持指２４ａ、第２の把持指２４ｂを個別に移動させる構成としてもよい。 In addition, in the hand part 22 in this Embodiment, it has the structure which moves the 1st holding finger 24a and the 2nd holding finger 24b separately by the moving mechanism 26a, 26b comprised by the mechanism of an orthogonal 3 axis | shaft. However, the configuration of the hand unit 22 is not limited to this. For example, in the hand unit 22, a moving mechanism capable of linear motion and rotational motion is provided for each of the first gripping finger 24 a and the second gripping finger 24 b, and the first gripping finger 24 a and the second gripping finger 24 b are provided by these moving mechanisms. It is good also as a structure which moves the holding finger 24b separately.

一方の移動機構２６ａにおける第１駆動部３６、第２駆動部４２及び第３駆動部４８は、それぞれボールネジ機構３８ａ、４６ａ、５２ａにより構成されるものに限られない。従って、第１駆動部３６、第２駆動部４２及び第３駆動部４８は、それぞれ、ボールネジ機構以外の形態のアクチュエータ、例えば、ラックアンドピニオン、シリンダ装置等により構成されてもよい。同様に、他方の移動機構２６ｂにおける第１駆動部５４、第２駆動部５６及び第３駆動部５８も、ボールネジ機構以外の形態のアクチュエータ、例えば、ラックアンドピニオン、シリンダ装置等により構成されてもよい。 The first drive unit 36, the second drive unit 42, and the third drive unit 48 in the one moving mechanism 26a are not limited to those configured by the ball screw mechanisms 38a, 46a, and 52a, respectively. Therefore, each of the first drive unit 36, the second drive unit 42, and the third drive unit 48 may be configured by an actuator other than the ball screw mechanism, for example, a rack and pinion, a cylinder device, or the like. Similarly, the first drive unit 54, the second drive unit 56, and the third drive unit 58 in the other moving mechanism 26b may be configured by actuators other than the ball screw mechanism, for example, a rack and pinion, a cylinder device, or the like. Good.

図１に示す制御部１４は、ロボット１２の多関節アーム２０の先端位置を数値制御するだけでなく、コネクタ７０をコネクタ挿入孔７５に挿入する際に、ハンド部２２の動作をコンプライアンス制御することが可能である。すなわち、コネクタ挿入装置１０は、コンプライアンス制御を用いて、ハンド部２２により把持したコネクタ７０を、コネクタ挿入孔７５に挿入することができる。 The control unit 14 shown in FIG. 1 not only numerically controls the tip position of the articulated arm 20 of the robot 12, but also compliance-controls the operation of the hand unit 22 when the connector 70 is inserted into the connector insertion hole 75. Is possible. That is, the connector insertion device 10 can insert the connector 70 gripped by the hand portion 22 into the connector insertion hole 75 using compliance control.

図４は、ハンド部２２のコンプライアンスモデルを示すモデル図である。周知の通り、コンプライアンス制御は、バネ、ダンパ、慣性パラメータを仮想的に設定したモデルの運動をロボットの動作として実現するものである。 FIG. 4 is a model diagram showing a compliance model of the hand unit 22. As is well known, the compliance control realizes a motion of a model in which springs, dampers, and inertia parameters are virtually set as a robot motion.

本実施の形態では、第１の把持指２４ａ、第２の把持指２４ｂの各々に、第１のコンプライアンスモデル、第２のコンプライアンスモデルが設定される。すなわち、第１の把持指２４ａに関する第１のコンプライアンスモデルは、第１仮想バネ１２０ａ及び第１仮想ダンパ１２２ａを含む。なお、図４には、当該コンプライアンスモデルにおいて、第１仮想バネ１２０ａの釣合位置及び第１仮想ダンパ１２２ａの基部に該当する移動物体を第１仮想台車１２４ａとして併せて示している。 In the present embodiment, a first compliance model and a second compliance model are set for each of the first gripping finger 24a and the second gripping finger 24b. That is, the first compliance model related to the first gripping finger 24a includes the first virtual spring 120a and the first virtual damper 122a. In FIG. 4, in the compliance model, the moving object corresponding to the balance position of the first virtual spring 120a and the base of the first virtual damper 122a is also shown as the first virtual carriage 124a.

一方、第２の把持指２４ｂに関する第２のコンプライアンスモデルは、第２仮想バネ１２０ｂ及び第２仮想ダンパ１２２ｂを含む。図４には、上記と同様に第２仮想バネ１２０ｂ及び第２仮想ダンパ１２２ｂの基部に該当する移動物体を第２仮想台車１２４ｂとして併せて示している。 On the other hand, the 2nd compliance model regarding the 2nd holding finger 24b contains the 2nd virtual spring 120b and the 2nd virtual damper 122b. In FIG. 4, the moving object corresponding to the bases of the second virtual spring 120b and the second virtual damper 122b is also shown as a second virtual carriage 124b in the same manner as described above.

第１仮想バネ１２０ａと第２仮想バネ１２０ｂ、第１仮想ダンパ１２２ａと第２仮想ダンパ１２２ｂは、それぞれ等価であるとする。すなわち、第１仮想バネ１２０ａ及び第２仮想バネ１２０ｂのバネ定数は等しく、第１仮想ダンパ１２２ａ及び第２仮想ダンパ１２２ｂの減衰係数は等しいとする。そこで、以下、第１仮想バネ１２０ａ及び第２仮想バネ１２０ｂのバネ定数をＫ、仮想質量をｍ、第１仮想ダンパ１２２ａ及び第２仮想ダンパ１２２ｂの減衰係数をＣとする。 It is assumed that the first virtual spring 120a and the second virtual spring 120b, and the first virtual damper 122a and the second virtual damper 122b are equivalent to each other. That is, the first virtual spring 120a and the second virtual spring 120b have the same spring constant, and the first virtual damper 122a and the second virtual damper 122b have the same damping coefficient. Therefore, hereinafter, the spring constant of the first virtual spring 120a and the second virtual spring 120b is K, the virtual mass is m, and the damping coefficient of the first virtual damper 122a and the second virtual damper 122b is C.

第１の把持指２４ａのコンプライアンス原点（釣合位置）をＰ_blc1、移動先の目標位置（目標座標）をＰ_d1、目標加速度をａ₁、目標速度をＶ₁、第１仮想台車１２４ａの速度をＶ_blc1、第１の把持指２４ａに作用する外力をＦ_e1とするとき、第１の把持指２４ａに関する運動方程式は、下記の式（１）として構築される。
ｍａ₁＝Ｆ_e1−Ｋ（Ｐ_d1−Ｐ_blc1）−Ｃ（Ｖ₁−Ｖ_blc1） …（１） The compliance origin (balance position) of the first gripping finger 24a is P _blc1 , the target position (target coordinates) of the movement destination is P _d1 , the target acceleration is a ₁ , the target speed is V ₁ , and the speed of the first virtual carriage 124a. the V _BLC1, when the external force acting on the first gripping fingers 24a and F _e1, equation of motion for the first gripping finger 24a is constructed as the following equation (1).
ma ₁ = F _e1 −K (P _d1 −P _blc1 ) −C (V ₁ −V _blc1 ) (1)

なお、外力Ｆ_e1は、第１指用力覚センサ２７ａによって検出される。また、コンプライアンス原点（釣合位置）Ｐ_blc1は、コンプライアンス制御がないとしたときの第１の把持指２４ａの目標位置であり、本実施の形態においては、第１の把持指２４ａの目標位置近傍に設定することができる。 The external force F _e1 is detected by the first finger force sensor 27a. The compliance origin (balance position) P _blc1 is the target position of the first gripping finger 24a when there is no compliance control, and in the present embodiment, the vicinity of the target position of the first gripping finger 24a. Can be set to

さらに、目標加速度ａ₁、目標速度Ｖ₁、及び第１仮想台車１２４ａの速度Ｖ_blc1は、後述する通り、離散化によりコンプライアンス原点（釣合位置）であるＰ_blc1、移動先の目標位置（目標座標）Ｐ_d1、サンプル時刻ｔ_N、制御周期Ｔにより表すことができる。 Further, the target acceleration a ₁ , the target speed V ₁ , and the speed V _blc1 of the first virtual carriage 124a are, as will be described later, P _blc1 , which is the compliance origin (balanced position), and the destination target position (target). Coordinates) P _d1 , sample time t _N , and control period T can be expressed.

一方、第２の把持指２４ｂのコンプライアンス原点（釣合位置）をＰ_blc2、移動先の目標位置（目標座標）をＰ_d2、目標加速度をａ₂、目標速度をＶ₂、第２仮想台車１２４ｂの速度をＶ_blc2、第２の把持指２４ｂに作用する外力をＦ_e2とするとき、第２の把持指２４ｂに関する運動方程式は、下記の式（２）として構築される。
ｍａ₂＝Ｆ_e2−Ｋ（Ｐ_d2−Ｐ_blc2）−Ｃ（Ｖ₂−Ｖ_blc2） …（２） On the other hand, the compliance origin (balance position) of the second gripping finger 24b is P _blc2 , the target position (target coordinates) of the movement destination is P _d2 , the target acceleration is a ₂ , the target speed is V ₂ , and the second virtual carriage 124b. the speed V _BLC2, when an external force acting on the second gripping fingers 24b and F _e2, equation of motion for the second gripping finger 24b is constructed as an expression (2) below.
ma ₂ = F _e2 −K (P _d2 −P _blc2 ) −C (V ₂ −V _blc2 ) (2)

上記と同様に、外力Ｆ_e2は第２指用力覚センサ２７ｂによって検出される。また、コンプライアンス原点（釣合位置）Ｐ_blc2は、は、コンプライアンス制御がないとしたときの第２の把持指２４ｂの目標位置であり、本実施の形態においては、第２の把持指２４ｂの目標位置近傍に設定することができる。 Similarly to the above, the external force F _e2 is detected by the second finger force sensor 27b. The compliance origin (balance position) P _blc2 is the target position of the second gripping finger 24b when there is no compliance control, and in this embodiment, the target of the second gripping finger 24b. It can be set near the position.

さらに、目標加速度ａ₂、目標速度Ｖ₂、及び第２仮想台車１２４ｂの速度Ｖ_blc2は、後述する通り、離散化によりコンプライアンス原点（釣合位置）であるＰ_blc2、移動先の目標位置（目標座標）Ｐ_d2、サンプル時刻ｔ_N、制御周期Ｔにより表すことができる。 Further, the target acceleration a _2, the target speed V _2, and the speed V _BLC2 second virtual carriage 124b is, as described later, by discretization P _BLC2 is compliance origin (balanced position), the destination of the target position (target Coordinate) P _d2 , sample time t _N , and control period T can be expressed.

ここで、本実施の形態においては、第１の把持指２４ａと第２の把持指２４ｂの間に第３のコンプライアンスモデルを設定する。第３のコンプライアンスモデルは、第３仮想バネ１２０ｃ及び第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６である。すなわち、この場合、第１の把持指２４ａと第２の把持指２４ｂとが、第３仮想バネ１２０ｃと第３仮想ダンパ１２２ｃを含む仮想スタビライザ１２６で連結されていると仮定する。第３仮想バネ１２０ｃのバネ定数はＫ_s、第３仮想ダンパ１２２ｃの減衰係数はＣ_sで表される。 Here, in the present embodiment, a third compliance model is set between the first gripping finger 24a and the second gripping finger 24b. The third compliance model is a virtual stabilizer 126 including a third virtual spring 120c and a third virtual damper 122c. That is, in this case, it is assumed that the first gripping finger 24a and the second gripping finger 24b are connected by the virtual stabilizer 126 including the third virtual spring 120c and the third virtual damper 122c. The spring constant of the third virtual spring 120c is represented by K _s , and the damping coefficient of the third virtual damper 122c is represented by C _s .

そして、仮想スタビライザ１２６の釣合距離をＤ_sとするとき、第１の把持指２４ａの運動方程式は、式（１）から、Ｋ_s（Ｐ_d1−Ｐ_d2＋Ｄ_s）を差し引き、さらに、Ｃ_s（Ｖ₁−Ｖ₂）を差し引くことで補正される。同様に、第２の把持指２４ｂの運動方程式は、式（２）から、Ｋ_s（Ｐ_d2−Ｐ_d1−Ｄ_s）を差し引き、さらに、Ｃ_s（Ｖ₂−Ｖ₁）を差し引くことで補正される。すなわち、下記の連立方程式が成立する。

When the balance distance of the virtual stabilizer 126 is D _s , the equation of motion of the first gripping finger 24 a is obtained by subtracting K _s (P _d1 −P _d2 + D _s ) from the equation (1), Correction is made by subtracting _s (V ₁ −V ₂ ). Similarly, the equation of motion of the second gripping finger 24b is obtained by subtracting K _s (P _d2 −P _d1 −D _s ) and further subtracting C _s (V ₂ −V ₁ ) from Equation (2). It is corrected. That is, the following simultaneous equations are established.

次に、目標加速度ａ₁、ａ₂、目標速度Ｖ₁、Ｖ₂、仮想台車の速度Ｖ_blcに対して、下記の式（３）〜（５）にて離散化を行う。

Next, the target accelerations a ₁ and a ₂ , the target speeds V ₁ and V ₂ , and the virtual carriage speed V _blc are discretized by the following equations (3) to (5).

式（３）〜（５）中のｔ_Nはサンプリング時刻、Ｔはサンプリング周期、Ｎはサンプリング回数である。サンプリング周期Ｔは、所望の制御性能に応じ、制御解析の技術知識に基づいて設定する。 In equations (3) to (5), t _N is the sampling time, T is the sampling period, and N is the number of times of sampling. The sampling period T is set based on the technical knowledge of control analysis according to the desired control performance.

このように、本実施の形態においては、第１の把持指２４ａ、第２の把持指２４ｂの各々にコンプライアンスモデル（第１のコンプライアンスモデル、第２のコンプライアンスモデル）を設定するのみならず、第１の把持指２４ａと第２の把持指２４ｂとの間にも、コンプライアンスモデル（第３のコンプライアンスモデル）を設定するようにしている。すなわち、第１の把持指２４ａと第２の把持指２４ｂの目標位置が相互に連動しあう拘束条件の下に算出される関係式を構築することで、仮想スタビライザ１２６が存在するとしている。これにより、第１の把持指２４ａ及び第２の把持指２４ｂによるコネクタ７０の把持が一層安定となる。 As described above, in the present embodiment, not only the compliance model (first compliance model, second compliance model) is set for each of the first gripping finger 24a and the second gripping finger 24b, A compliance model (third compliance model) is also set between the first gripping finger 24a and the second gripping finger 24b. That is, it is assumed that the virtual stabilizer 126 exists by constructing a relational expression calculated under a constraint condition in which the target positions of the first gripping finger 24a and the second gripping finger 24b are linked to each other. Thereby, the gripping of the connector 70 by the first gripping finger 24a and the second gripping finger 24b becomes more stable.

なお、図３に示すように、第１の把持指２４ａ、第２の把持指２４ｂのコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2、目標加速度ａ₁、ａ₂、目標速度Ｖ₁、Ｖ₂、第１仮想台車１２４ａ、第２仮想台車１２４ｂの速度Ｖ_blc1、Ｖ_blc2、第１の把持指２４ａ、第２の把持指２４ｂに作用する外力Ｆ_e1、Ｆ_e2の値は、目標位置算出部１０８に記憶される。 As shown in FIG. 3, the compliance origins (balance positions) P _blc1 and P _{blc2 of} the first gripping finger 24a and the second gripping finger 24b, target accelerations a ₁ and a ₂ , target speeds V ₁ and V ₂ , the speeds V _blc1 and V _{blc2 of} the first virtual carriage 124a and the second virtual carriage 124b, the values of the external forces F _e1 and F _e2 acting on the first gripping finger 24a and the second gripping finger 24b are calculated as target positions. Stored in the unit 108.

本実施の形態に係るコネクタ挿入装置１０は、基本的には以上のように構成されるものであり、以下、その作用及び効果について、本発明の実施の形態に係るコネクタ挿入方法との関係で説明する。 The connector insertion device 10 according to the present embodiment is basically configured as described above. Hereinafter, the operation and effect thereof are related to the connector insertion method according to the embodiment of the present invention. explain.

対象となるコネクタ７０は、図７Ａ等に示すように、コネクタ７０（オスコネクタ）の基端部にケーブル７２の一端が接続されたものであり、電子機器に設けられた接続部材７４（メスコネクタ）のコネクタ挿入孔７５（接続ポート）に当該コネクタ７０を挿入する。ケーブル７２は、図示しない他の電子機器から延出している。以下の説明において、コネクタ挿入装置１０の動作は、特に言及しない場合でも、制御部１４の制御作用下に行われるものとする。 As shown in FIG. 7A and the like, the target connector 70 is one in which one end of a cable 72 is connected to the proximal end portion of a connector 70 (male connector), and a connection member 74 (female connector) provided in an electronic device. The connector 70 is inserted into the connector insertion hole 75 (connection port). The cable 72 extends from another electronic device (not shown). In the following description, it is assumed that the operation of the connector insertion device 10 is performed under the control action of the control unit 14 even when not specifically mentioned.

コネクタ７０を接続部材７４のコネクタ挿入孔７５に挿入するために、先ず、制御部１４の制御作用下に、ロボット１２の多関節アーム２０を動作させて、カメラ６２がコネクタ７０を撮像できる位置（コネクタ認識可能位置）までハンド部２２を移動させる。そして、ハンド部２２がコネクタ認識可能位置まで移動したら、カメラ６２によりコネクタ７０を撮像するとともに、パターンマッチングを行うことによりコネクタ７０の位置及び姿勢を認識する。具体的には、制御部１４には、対象となるコネクタ７０の３次元形状モデル（３Ｄデータ）が記憶されており、撮像したコネクタ７０の画像情報と３Ｄデータとを照合することにより、コネクタ７０の位置及び姿勢を認識する。 In order to insert the connector 70 into the connector insertion hole 75 of the connection member 74, first, the articulated arm 20 of the robot 12 is operated under the control action of the control unit 14 so that the camera 62 can image the connector 70 ( The hand unit 22 is moved to the connector recognizable position. Then, when the hand unit 22 moves to a connector recognizable position, the camera 70 is imaged by the camera 62 and the position and orientation of the connector 70 are recognized by performing pattern matching. Specifically, the control unit 14 stores a three-dimensional shape model (3D data) of the target connector 70, and collates the image information of the imaged connector 70 with the 3D data to check the connector 70. Recognize the position and posture.

コネクタ７０の位置及び姿勢を認識したら、制御部１４の制御作用下に、ハンド部２２を把持位置まで移動するとともに、各移動機構２６ａ、２６ｂを動作させて、図７Ａに示すように、第１の把持指２４ａ、第２の把持指２４ｂによりコネクタ７０を把持する（把持工程）。この場合、第１の把持指２４ａ、第２の把持指２４ｂを互いに対向するように位置させ、コネクタ７０の互いに反対側の側面７０ａ、７０ｂ（一方の側面７０ａと他方の側面７０ｂ）にそれぞれ第１の把持指２４ａ、第２の把持指２４ｂを当接させて、コネクタ７０を把持する。 When the position and orientation of the connector 70 are recognized, the hand unit 22 is moved to the gripping position and the moving mechanisms 26a and 26b are operated under the control action of the control unit 14, and as shown in FIG. The connector 70 is gripped by the gripping fingers 24a and the second gripping fingers 24b (gripping step). In this case, the first gripping finger 24a and the second gripping finger 24b are positioned so as to face each other, and the first and second side faces 70a and 70b (one side face 70a and the other side face 70b) of the connector 70 respectively The first gripping finger 24 a and the second gripping finger 24 b are brought into contact with each other to grip the connector 70.

なお、第１の把持指２４ａ、第２の把持指２４ｂによりコネクタ７０を把持した後、再びコネクタ７０の位置及び姿勢をパターンマッチングにより認識し、把持した際のコネクタ７０の位置ずれが発生していないかどうかを確認してもよい。 Note that after the connector 70 is gripped by the first gripping finger 24a and the second gripping finger 24b, the position and orientation of the connector 70 are recognized again by pattern matching, and the connector 70 is displaced when gripped. You may check whether there is any.

このとき、制御部１４は、目標位置算出部１０８に送られた情報、すなわち、コネクタ７０を把持するために、コネクタ７０の位置や形状に基づき算出される第１の把持指２４ａ、第２の把持指２４ｂのコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2、第１仮想台車１２４ａ、第２仮想台車１２４ｂの速度Ｖ_blc1、Ｖ_blc2、第１の把持指２４ａ、第２の把持指２４ｂに作用する外力Ｆ_e1、Ｆ_e2に基づき、上記の連立運動方程式を解く。 At this time, the control unit 14 uses the information sent to the target position calculation unit 108, that is, the first gripping finger 24 a and the second gripping finger 24 a calculated based on the position and shape of the connector 70 to grip the connector 70. The compliance origin (balance position) P _blc1 and P _blc2 of the gripping finger 24b, the speeds V _blc1 and V _{blc2 of} the first virtual carriage 124a and the second virtual carriage 124b, the first gripping finger 24a and the second gripping finger 24b Based on the acting external forces F _e1 and F _e2 , the above simultaneous equations of motion are solved.

連立運動方程式において、未知数は、第１の把持指２４ａの、例えば、Ｘ軸に沿う方向の移動先の目標位置（目標座標）Ｐ_d1、第２の把持指２４ｂの移動先の目標位置（目標座標）Ｐ_d2の２個である。従って、連立運動方程式をＰ_d1、Ｐ_d2について離散化式に基づき解くことにより、各時刻でのＰ_d1、Ｐ_d2の値が順次求められる。 In the simultaneous equations of motion, the unknowns are the target position (target coordinates) P _{d1 of} the first gripping finger 24a in the direction along the X axis, for example, and the target position (target target) of the second gripping finger 24b. Coordinates) _Pd2 . Therefore, by solving based simultaneous equations of motion in the discretization equation for P _d1, P _d2, the value of P _d1, P _d2 at each time it is sequentially determined.

すなわち、目標位置算出部１０８により、離散化式による逐次演算の複数回サンプリングを行い、各サンプル時刻での第１の把持指２４ａ、第２の把持指２４ｂを如何なる程度移動させるべきかの情報が得られる。 That is, the target position calculation unit 108 performs sampling for a plurality of times of the sequential calculation by the discretization formula, and information on how much the first gripping finger 24a and the second gripping finger 24b should be moved at each sample time is obtained. can get.

目標位置算出部１０８は、この情報を、サーボモータ制御部１１０に送る。該サーボモータ制御部１１０は、第１の把持指２４ａ又は第２の把持指２４ｂの移動先が各サンプル時刻毎のＰ_d1、Ｐ_d2となるように、例えば、サーボモータ４４ａ又はサーボモータ４４ｂを駆動させる。その結果、第１の把持指２４ａ又は第２の把持指２４ｂがコネクタ７０を把持する位置まで移動する。 The target position calculation unit 108 sends this information to the servo motor control unit 110. For example, the servo motor control unit 110 controls the servo motor 44a or the servo motor 44b so that the movement destination of the first grip finger 24a or the second grip finger 24b becomes P _d1 and P _{d2 at} each sample time. Drive. As a result, the first gripping finger 24a or the second gripping finger 24b moves to a position where the connector 70 is gripped.

以上は、Ｚ軸に関してのコンプライアンス制御を例示しているが、Ｘ軸及びＹ軸に対しても同様に、コンプライアンス制御が営まれる。各軸のコンプライアンス制御は、同時に行われる。 The above illustrates the compliance control with respect to the Z axis, but the compliance control is similarly performed for the X axis and the Y axis. Compliance control of each axis is performed simultaneously.

このように、第１〜第３のコンプライアンスモデルを設定することにより、コネクタ７０を、第１の把持指２４ａと第２の把持指２４ｂが連動しあう条件下に把持することができる。 As described above, by setting the first to third compliance models, the connector 70 can be gripped under a condition in which the first gripping finger 24a and the second gripping finger 24b are interlocked with each other.

図５Ａ〜図５Ｃには、第１の把持指２４ａ、第２の把持指２４ｂの各々にのみコンプライアンスモデルを構築した従来技術において、把持しているコネクタ７０を、左方に平行移動させる際のフローを示している。すなわち、先ず、コネクタ７０を把持した第１の把持指２４ａ、第２の把持指２４ｂは、図５Ａ中の矢印Ｌに示すように、左方に平行移動しようとする。 FIGS. 5A to 5C show a case where the connector 70 being gripped is translated to the left in the conventional technique in which the compliance model is constructed only for each of the first gripping finger 24a and the second gripping finger 24b. The flow is shown. That is, first, the first gripping finger 24a and the second gripping finger 24b that grip the connector 70 try to translate leftward as indicated by the arrow L in FIG. 5A.

この場合、ケーブル７２による反力が生じる。その結果、図５Ｂに示すように、コネクタ７０に、矢印Ｍに示す方向の回転モーメントが生じる。このため、第１の把持指２４ａ、第２の把持指２４ｂに、矢印Ｎ１、Ｎ２方向の力が作用する。モーメントが過剰な場合、コネクタ７０が回転することに倣って第１の把持指２４ａ、第２の把持指２４ｂが大きく変位する。 In this case, a reaction force is generated by the cable 72. As a result, as shown in FIG. 5B, a rotational moment in the direction indicated by the arrow M is generated in the connector 70. For this reason, forces in the directions of arrows N1 and N2 act on the first gripping finger 24a and the second gripping finger 24b. When the moment is excessive, the first gripping finger 24a and the second gripping finger 24b are greatly displaced following the rotation of the connector 70.

その結果、図５Ｃに示すように、第１の把持指２４ａ、第２の把持指２４ｂの相対距離が大きくなる。このため、コネクタ７０の保持が不安定となる。 As a result, as shown in FIG. 5C, the relative distance between the first gripping finger 24a and the second gripping finger 24b increases. For this reason, the holding of the connector 70 becomes unstable.

図６Ａ及び図６Ｂには、本実施の形態に係るワーク保持方法を採用したコネクタ挿入装置１０のハンド部２２によって、把持しているコネクタ７０を、左方に平行移動させる際のフローを示している。 6A and 6B show a flow when the gripping connector 70 is translated to the left by the hand portion 22 of the connector insertion device 10 adopting the work holding method according to the present embodiment. Yes.

ここで、本実施の形態では、第１の把持指２４ａと第２の把持指２４ｂが仮想スタビライザ１２６で転結されたと仮定して、第１の把持指２４ａと第２の把持指２４ｂの間にも第３のコンプライアンスモデルを構築するようにしている。このため、図６Ａに示すようにコネクタ７０を把持した場合、第１の把持指２４ａ、第２の把持指２４ｂに対し、その変位を抑制しようとする力が作用する。 Here, in the present embodiment, it is assumed that the first gripping finger 24a and the second gripping finger 24b are rolled by the virtual stabilizer 126, and between the first gripping finger 24a and the second gripping finger 24b. In addition, a third compliance model is established. For this reason, when the connector 70 is gripped as shown in FIG. 6A, a force for suppressing the displacement acts on the first gripping finger 24a and the second gripping finger 24b.

従って、コネクタ７０が回転することが防止される。このため、図６Ｂに示すように、第１の把持指２４ａ、第２の把持指２４ｂの相対距離が小さくなり、この状態が保たれる。その結果、コネクタ７０が安定して第１の把持指２４ａ、第２の把持指２４ｂに保持される。すなわち、保持が安定化し、且つ第１の把持指２４ａに関するコンプライアンス制御、及び第２の把持指２４ｂに関するコンプライアンス制御とのバランスを採ることができる。 Therefore, the connector 70 is prevented from rotating. For this reason, as shown in FIG. 6B, the relative distance between the first gripping finger 24a and the second gripping finger 24b is reduced, and this state is maintained. As a result, the connector 70 is stably held by the first gripping finger 24a and the second gripping finger 24b. That is, the holding can be stabilized, and the balance between the compliance control related to the first gripping finger 24a and the compliance control related to the second gripping finger 24b can be taken.

第１の把持指２４ａ、第２の把持指２４ｂによりコネクタ７０を把持した後、次に、カメラ６２により撮像している画像情報から、制御部１４に記憶されている、接続部材７４の開口側部分７６の３次元形状モデル（３Ｄデータ）を用いてパターンマッチングを行い、接続部材７４の開口側部分７６の位置及び姿勢を認識する。そして、コネクタ７０の位置及び姿勢と、接続部材７４の開口側部分７６の位置及び姿勢とから、コネクタ７０と接続部材７４（コネクタ挿入孔７５）との相対位置を算出する。 After the connector 70 is gripped by the first gripping finger 24a and the second gripping finger 24b, the opening side of the connection member 74 stored in the control unit 14 is then stored from the image information captured by the camera 62. Pattern matching is performed using the three-dimensional shape model (3D data) of the portion 76 to recognize the position and orientation of the opening side portion 76 of the connecting member 74. Then, the relative position between the connector 70 and the connection member 74 (connector insertion hole 75) is calculated from the position and posture of the connector 70 and the position and posture of the opening side portion 76 of the connection member 74.

次に、算出された相対位置に基づき、制御部１４の制御作用下に、コネクタ７０の先端面７１が接続部材７４のコネクタ挿入孔７５に対向する位置に来るように、コネクタ７０を把持したハンド部２２の位置及び姿勢を変化させる。これにより、図７Ｂに示すように、接続部材７４のコネクタ挿入孔７５に対向する位置にコネクタ７０を配置する。すなわち、コネクタ７０の先端面７１を、接続部材７４のコネクタ挿入孔７５の真正面に位置させる。 Next, based on the calculated relative position, under the control action of the control unit 14, the hand that holds the connector 70 so that the distal end surface 71 of the connector 70 is located at a position facing the connector insertion hole 75 of the connection member 74. The position and posture of the unit 22 are changed. Thereby, as shown in FIG. 7B, the connector 70 is disposed at a position facing the connector insertion hole 75 of the connection member 74. That is, the front end surface 71 of the connector 70 is positioned directly in front of the connector insertion hole 75 of the connection member 74.

次に、制御部１４の制御作用下に、図７Ｃに示すように、コネクタ７０を把持した第１の把持指２４ａ、第２の把持指２４ｂを接続部材７４側に向かって移動させることにより、コネクタ７０をコネクタ挿入孔７５の途中位置まで挿入する（挿入工程）。すなわち、ここでは、先ず、コネクタ７０の先端部７０ｃだけをコネクタ挿入孔７５の入口部分に挿入する。 Next, under the control action of the control unit 14, as shown in FIG. 7C, the first gripping finger 24 a and the second gripping finger 24 b that grip the connector 70 are moved toward the connection member 74 side. The connector 70 is inserted to the middle position of the connector insertion hole 75 (insertion step). That is, here, first, only the distal end portion 70 c of the connector 70 is inserted into the inlet portion of the connector insertion hole 75.

このとき、制御部１４は、目標位置算出部１０８において、コネクタ７０の先端部７０ｃのみをコネクタ挿入孔７５の入口部分に挿入するべく、コネクタ挿入孔７５の位置から算出される第１の把持指２４ａ、第２の把持指２４ｂが向かうべき位置である新たなコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2を用いて、前述の把持工程と同様に第１〜第３のコンプライアンスモデルを設定し、コンプライアンス制御による目標位置算出を行う。 At this time, the control unit 14 causes the target position calculation unit 108 to calculate the first gripping finger calculated from the position of the connector insertion hole 75 so as to insert only the distal end portion 70c of the connector 70 into the entrance portion of the connector insertion hole 75. The first to third compliance models are set in the same manner as in the above-described gripping process using new compliance origins (balance positions) P _blc1 and P _blc2 that are positions to which the second gripping finger 24b should go. The target position is calculated by compliance control.

次に、先端部７０ｃがコネクタ挿入孔７５に挿入されたコネクタ７０を一方の第１の把持指２４ａにより押圧して、コネクタ７０をコネクタ挿入孔７５の内側面７８（一方の第１の把持指２４ａとは反対側の内側面７８）に押し付ける（押付工程）。これにより、コネクタ７０と当該内側面７８との間の摩擦力が大きくなり、先端部７０ｃだけがコネクタ挿入孔７５に挿入されたコネクタ７０が、一時的に接続部材７４に対して固定される。 Next, the connector 70 in which the distal end portion 70c is inserted into the connector insertion hole 75 is pressed by one first gripping finger 24a, and the connector 70 is pressed against the inner side surface 78 of the connector insertion hole 75 (one first gripping finger). It presses against the inner surface 78) opposite to 24a (pressing step). Thereby, the frictional force between the connector 70 and the inner side surface 78 is increased, and the connector 70 in which only the tip end portion 70 c is inserted into the connector insertion hole 75 is temporarily fixed to the connection member 74.

この工程においても、押し付けに適したコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2を用いることにより、同様のコンプライアンス制御を行うことができる。 Also in this step, similar compliance control can be performed by using compliance origins (balance positions) P _blc1 and P _blc2 suitable for pressing.

次に、図７Ｄに示すように、コネクタ７０をコネクタ挿入孔７５の内側面７８に押し当てた状態を維持しつつ、他方の第２の把持指２４ｂをコネクタ７０から離間させる。図８Ａは、図７Ｄにおいて、コネクタ７０及び接続部材７４を矢印Ａの方向から見た概略図である。このとき、一方の第１の把持指２４ａによりコネクタ７０が内側面７８に押し付けられてコネクタ７０が固定された状態となっていることから、他方の第２の把持指２４ｂをコネクタ７０から離間させても、ケーブル７２による拘束力に起因してコネクタ７０が接続部材７４から外れる（抜け出る）ことはない。すなわち、ケーブル７２がコネクタ７０を基端方向に引っ張るように作用しても、一方の第１の把持指２４ａでコネクタ７０を内側面７８に押し付けることによる固定力の方が、ケーブル７２による引っ張り力よりも大きいため、コネクタ７０が接続部材７４から外れることが防止される。 Next, as shown in FIG. 7D, the other second gripping finger 24 b is separated from the connector 70 while maintaining the state where the connector 70 is pressed against the inner side surface 78 of the connector insertion hole 75. FIG. 8A is a schematic view of the connector 70 and the connecting member 74 viewed from the direction of the arrow A in FIG. 7D. At this time, the connector 70 is pressed against the inner side surface 78 by one first gripping finger 24 a and the connector 70 is fixed, so that the other second gripping finger 24 b is separated from the connector 70. However, the connector 70 does not come off (disengage) from the connection member 74 due to the restraining force of the cable 72. In other words, even if the cable 72 acts to pull the connector 70 in the proximal direction, the fixing force by pressing the connector 70 against the inner surface 78 with one first gripping finger 24a is greater than the pulling force by the cable 72. Therefore, the connector 70 is prevented from being detached from the connection member 74.

次に、図８Ａに示すように、コネクタ７０をコネクタ挿入孔７５の内側面７８に押し当てた状態で、他方の第２の把持指２４ｂを、コネクタ７０の基端部側（コネクタ挿入孔７５に挿入される側に対して反対側）に移動させる（移動工程）。これにより、他方の第２の把持指２４ｂをコネクタ７０の基端部（基端面７０ｄ）に当接させる。なお、他方の第２の把持指２４ｂをコネクタ７０から離間させる動作と、他方の第２の把持指２４ｂをコネクタ７０の基端部側に移動させる動作とを、連続した一つの動作としてもよい。 Next, as shown in FIG. 8A, in a state where the connector 70 is pressed against the inner surface 78 of the connector insertion hole 75, the other second gripping finger 24 b is moved to the base end side (connector insertion hole 75 of the connector 70. To the side opposite to the side to be inserted into (moving step). As a result, the other second gripping finger 24 b is brought into contact with the base end portion (base end surface 70 d) of the connector 70. The operation of moving the other second gripping finger 24b away from the connector 70 and the operation of moving the other second gripping finger 24b to the proximal end side of the connector 70 may be a single continuous operation. .

この工程においても、移動に適したコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2を用いることにより、同様のコンプライアンス制御を行うことができる。なお、この工程以降は、第１の把持指２４ａ、第２の把持指２４ｂ毎に独立した動作となるため、第１の把持指２４ａ、第２の把持指２４ｂが連動しないように仮想スタビライザ１２６の機能を停止させる。具体的には、第３仮想バネ１２０ｃのバネ定数Ｋｓと、第３仮想ダンパ１２２ｃの減衰係数Ｃｓに０を代入する。 Also in this step, the same compliance control can be performed by using compliance origins (balance positions) P _blc1 and P _blc2 suitable for movement. After this step, since the operation is independent for each of the first gripping finger 24a and the second gripping finger 24b, the virtual stabilizer 126 is used so that the first gripping finger 24a and the second gripping finger 24b are not interlocked. Stop the function. Specifically, 0 is substituted into the spring constant Ks of the third virtual spring 120c and the damping coefficient Cs of the third virtual damper 122c.

次に、図８Ｃに示すように、他方の第２の把持指２４ｂによりコネクタ７０を押して、コネクタ７０をコネクタ挿入孔７５にさらに押し込む（押込工程）。これにより、コネクタ７０をコネクタ挿入孔７５の規定の挿入深さまで挿入する。なお、他方の第２の把持指２４ｂをコネクタ７０の基端部に当接させた後には、一方の第１の把持指２４ａによるコネクタ７０に対する押圧力を減少させてもよく、あるいは一方の第１の把持指２４ａをコネクタ７０から離間させてもよい。規定の挿入深さまで挿入されたコネクタ７０は、図示しないロック部（例えば、爪部等）の作用により接続部材７４に対して固定され、装着状態となる。 Next, as shown in FIG. 8C, the connector 70 is pushed by the other second gripping finger 24b, and the connector 70 is further pushed into the connector insertion hole 75 (pushing step). As a result, the connector 70 is inserted to a predetermined insertion depth of the connector insertion hole 75. In addition, after the other second gripping finger 24b is brought into contact with the proximal end portion of the connector 70, the pressing force of the first gripping finger 24a on the connector 70 may be reduced, or one of the first gripping fingers 24b may be decreased. One gripping finger 24 a may be separated from the connector 70. The connector 70 inserted to a specified insertion depth is fixed to the connection member 74 by the action of a lock part (not shown) (for example, a claw part or the like) and is in a mounted state.

この工程においても、押し込みに適したコンプライアンス原点（釣合位置）Ｐ_blc1、Ｐ_blc2を用いることにより、同様のコンプライアンス制御を行うことができる。 Also in this step, the same compliance control can be performed by using the compliance origin (balance position) P _blc1 and P _blc2 suitable for pushing.

以上説明したように、本実施の形態に係るコネクタ挿入方法及びコネクタ挿入装置１０によれば、コネクタ挿入孔７５にコネクタ７０を途中まで挿入した状態で、一方の第１の把持指２４ａによりコネクタ７０をコネクタ挿入孔７５の内側面７８に押し付けることにより、コネクタ７０が一時的に固定される。このため、コネクタ７０に接続されたケーブル７２の拘束力によって、コネクタ挿入孔７５とは反対方向にコネクタ７０を引っ張る力が作用した場合でも、コネクタ７０がコネクタ挿入孔７５から抜け出ることがない。そして、他方の第２の把持指２４ｂによりコネクタ挿入孔７５とは反対側からコネクタ７０を押して、コネクタ７０を押し込むため、コネクタ７０をコネクタ挿入孔７５の所定位置まで確実に挿入することができる。 As described above, according to the connector insertion method and the connector insertion device 10 according to the present embodiment, the connector 70 is inserted into the connector insertion hole 75 halfway through the connector 70 by the first gripping finger 24a. Is pressed against the inner surface 78 of the connector insertion hole 75 to temporarily fix the connector 70. For this reason, even when a force that pulls the connector 70 in the direction opposite to the connector insertion hole 75 is applied due to the restraining force of the cable 72 connected to the connector 70, the connector 70 does not come out of the connector insertion hole 75. Since the connector 70 is pushed in by pushing the connector 70 from the side opposite to the connector insertion hole 75 with the other second gripping finger 24b, the connector 70 can be reliably inserted to a predetermined position of the connector insertion hole 75.

その際に、第１〜第３のコンプライアンスモデルを設定したコンプライアンス制御を行うことにより、コネクタ７０の姿勢を、第１の把持指２４ａと第２の把持指２４ｂが連動しあう条件下に把持することができる。このため、外力による第１の把持指２４ａ、第２の把持指２４ｂの相対変位が抑制される。その結果、コネクタ７０を安定して把持することが可能となり、また、コネクタ挿入孔７５に確実に挿入することが可能となる。 At that time, by performing compliance control in which the first to third compliance models are set, the posture of the connector 70 is gripped under a condition in which the first gripping finger 24a and the second gripping finger 24b are interlocked with each other. be able to. For this reason, the relative displacement of the 1st holding finger 24a and the 2nd holding finger 24b by external force is suppressed. As a result, the connector 70 can be stably held and can be reliably inserted into the connector insertion hole 75.

上記において、本発明について好適な実施の形態を挙げて説明したが、本発明は前記実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲において、種々の改変が可能なことは言うまでもない。 In the above description, the present invention has been described with reference to preferred embodiments. However, the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the scope of the present invention. Needless to say.

例えば、この実施の形態では、コネクタ挿入に至るまでの各工程毎にコンプライアンス制御を行うようにしているが、コンプライアンス原点（釣合位置）が各工程における目標位置に連動して自動的に更新されるようにすることも可能である。この場合、各工程毎の制御ではなく、把持工程から押込工程に至る全工程において、連動したコンプライアンス制御によって目標位置を算出することが可能である。 For example, in this embodiment, compliance control is performed for each process until connector insertion, but the compliance origin (balance position) is automatically updated in conjunction with the target position in each process. It is also possible to do so. In this case, it is possible to calculate the target position by linked compliance control in all processes from the gripping process to the pushing process, not the control for each process.

１０…コネクタ挿入装置１２…ロボット
１４…制御部２２…ハンド部
２３ａ、２３ｂ…緩衝アーム２４ａ、２４ｂ…把持指
２７ａ、２７ｂ…指用力覚センサ
３８ａ、３８ｂ、４６ａ、４６ｂ、５２ａ、５２ｂ…ボールネジ機構
４０ａ、４０ｂ、４４ａ、４４ｂ、５０ａ、５０ｂ…サーボモータ
７０…コネクタ７５…コネクタ挿入孔
７８…内側面１０８…目標位置算出部
１１０…サーボモータ制御部
１２０ａ、１２０ｂ、１２０ｃ…仮想バネ
１２２ａ、１２２ｂ、１２２ｃ…仮想ダンパ
１２４ａ、１２４ｂ…仮想台車
１２６…仮想スタビライザ DESCRIPTION OF SYMBOLS 10 ... Connector insertion apparatus 12 ... Robot 14 ... Control part 22 ... Hand part 23a, 23b ... Buffer arm 24a, 24b ... Grasp finger 27a, 27b ... Finger force sensor 38a, 38b, 46a, 46b, 52a, 52b ... Ball screw mechanism 40a, 40b, 44a, 44b, 50a, 50b ... servo motor 70 ... connector 75 ... connector insertion hole 78 ... inside surface 108 ... target position calculation unit 110 ... servo motor control units 120a, 120b, 120c ... virtual springs 122a, 122b, 122c ... Virtual dampers 124a, 124b ... Virtual carriage 126 ... Virtual stabilizer

Claims

In a work holding method for holding a work with a first gripping finger and a second gripping finger that can face each other,
A first compliance model and a second compliance model are set for each of the first gripping finger and the second gripping finger, and a third is set between the first gripping finger and the second gripping finger. The compliance model of the first gripping finger or the second gripping finger is moved based on the first compliance model, the second compliance model, and the third compliance model. Constructing simultaneous equations of motion containing coordinates as variables;
Calculating the target coordinates at which the first gripping finger and the second gripping finger are interlocked with each other under a constraint condition based on the third compliance model;
Moving the first gripping finger or the second gripping finger to the target coordinates;
I have a,
When a rotational moment is generated in the workpiece and a reaction force acts on the first gripping finger and the second gripping finger, the first coordinate is moved by moving the target coordinates so as to prevent the workpiece from rotating. A work holding method, characterized in that a relative distance between a gripping finger and the second gripping finger is kept constant .

2. The work holding method according to claim 1, wherein the work is a connector to which a cable is connected.

3. The holding method according to claim 1, wherein the building step and the calculating step are performed for each coordinate axis in a three-dimensional orthogonal coordinate system in which the first compliance model to the third compliance model are set. A work holding method characterized by the above.