JP2018167395A - ロボット装置、制御装置、及びロボットの制御方法 - Google Patents

Abstract

【課題】内側部材及び内側部材に嵌挿可能な内周面を持つ外側部材の一方を他方に容易に嵌挿可能なロボット装置及び該ロボット装置の制御方法を提供すること。【解決手段】多関節ロボットと、多関節ロボットが受ける反力を検出可能な力覚センサ２２と、多関節ロボットが把持した円環部品Ｗ２を、円錐形状のワーク本体Ｗ１に嵌挿させる嵌挿動作を行う際に、多関節ロボットが把持する円環部品Ｗ２に設定した第１座標系１１８に基づいて作成された指定軌道で、円環部品Ｗ２を移動させ、円環部品Ｗ２がワーク本体Ｗ１に接触して力覚センサ２２が反力を検出すると、反力から、円環部品Ｗ２をワーク本体Ｗ１の外周面の傾斜方向に沿って移動させるための傾斜方向に沿った第２座標系Ｑを円環部品Ｗ２の接触点に設定し、第２座標系Ｑで円環部品Ｗ２を把持した多関節ロボットを移動制御する制御装置と、を備えた。【選択図】図５

Description

本発明は、力覚センサを有する多関節ロボットを備えたロボット装置及び多関節ロボットの制御方法に関する。

近年、産業用ロボットは、様々な用途に使用されており、昨今においては、部品を自動で組み立てる組み立てラインに使用されることが多くなってきている。その中でも、多関節ロボットは、人間と同様の動きを行えるだけの自由度を持っているため、多くの組み立てラインに導入されている。

しかし、多関節ロボットは、スカラロボットや直行ロボットのような平面位置の再現性に優れたロボットと比べて様々な姿勢を取ることができる反面、自由度が上がった分、姿勢の再現性にばらつきが生じる。そのため、多関節ロボットは、組み立てラインでの部品の嵌合や挿入などの組み付けに失敗する場合があり、多くの多関節ロボットは、力覚センサを用いたフィードバック制御で姿勢の再現性を補っている（非特許文献１参照）。

ここで、力覚センサを用いたフィードバック制御は、力覚センサをロボットアームの先端に取り付け、接触等で生じる外力を力覚センサの各軸ベクトル方向の力として検出し、その方向に対して位置補正を行う。位置補正は、サンプリング周期毎に行われ、判断された力を設定されたパラメータであるバネ定数や粘性係数を基に位置の補正量として計算を行い、力を緩和する方向に動作させる補正手段である。力覚センサを用いたフィードバック制御を取り入れることにより、組み立て部品の公差や動作中のロボットの姿勢の再現性によるズレで部品の組み立てが失敗することを防止することができる。

しかし、例えば、略円錐形のワーク本体に円環部品を組み付ける際に、円環部品を一定の姿勢に保ちながらワーク本体の斜面と一定接触状態を続ける必要がある場合、ワーク本体の斜面方向を認識し、認識した斜面方向へ円環部品を移動させなければならない。これに対しては、斜面に対して粗い間隔で教示を行い、この確認作業を実行する際に斜面に接触したときに力覚センサが検出する検出値から、斜面に対して一定接触状態で動作する教示点を作成するロボット装置が提案されている（特許文献１参照）。

新版ロボット工学ハンドブック ２．４．２章 力制御 Ｐ２８７−２９５

特開平５−２６５５３７号公報

しかしながら、特許文献１に記載のロボット装置は、教示した際の軌道補間点を守るものであり、実動作上で斜面方向が個々に変わる様な経路が不規則な物体に対しては、事前に教示できないため対応することができない。例えば、上述のような円錐形状のワーク本体の傾斜面に円環部品の組み付けを行う場合、ワーク本体の傾斜面との接触が円環部品の内周上のどの位置でも起こり得るため、対応させることができないという問題があった。

そこで、本発明は、内側部材及び前記内側部材に嵌挿可能な内周面を持つ外側部材の一方の部材を他方の部材に容易に嵌挿可能なロボット装置及び多関節ロボットの制御方法を提供することを目的とする。

本発明は、ロボット装置において、多関節ロボットと、前記多関節ロボットが受ける反力を検出可能な力覚センサと、前記多関節ロボットが把持した、外周面を持つ内側部材及び前記内側部材に嵌挿可能な内周面を持つ外側部材のいずれか一方の部材を、他方の部材に嵌挿させる嵌挿動作を行う際に、前記多関節ロボットが把持する前記一方の部材に設定した第１座標系に基づいて作成された指定軌道で、前記内側部材の小径部分から大径部分に向かって前記外側部材を相対移動させ、前記多関節ロボットが把持した前記一方の部材が前記他方の部材に接触して前記力覚センサが前記反力を検出すると、前記反力から、前記多関節ロボットが把持した前記一方の部材を前記内側部材の外周面の傾斜方向に沿って移動させるための前記傾斜方向に沿った第２座標系を前記多関節ロボットが把持した前記一方の部材に設定し、前記第２座標系で前記一方の部材を把持した前記多関節ロボットを移動制御する制御装置と、を備えたことを特徴とする。

本発明によれば、内側部材及び前記内側部材に嵌挿可能な内周面を持つ外側部材の一方の部材を他方の部材に容易に嵌挿可能なロボット装置及び多関節ロボットの制御方法を提供することができる。

本発明の第１実施形態に係るロボット装置の全体構造を模式的に示す斜視図である。 第１実施形態に係る多関節ロボットを制御する制御装置のブロック図である。 第１実施形態に係る多関節ロボットによる円環部品のワーク本体への嵌挿を示す図である。 第１実施形態に係る多関節ロボットによる円環部品のワーク本体への嵌挿動作を示すフローチャートである。 円環部品の嵌挿途中で円環部品がワーク本体に接触した際の多関節ロボットの軌道計算を説明するための図である。 第２実施形態に係るロボット装置がワーク本体に円環部品を嵌合させる状態を示す図である。 第３実施形態に関る多関節ロボットによる角環部品のワーク本体への嵌挿を示す斜視図である。 第３実施形態に係る多関節ロボットが把持している異なる形状を持つ部品の上面図である。 第３実施形態に係る多関節ロボットが把持している角環部品の嵌挿順序を示す上面図である。 第４実施形態に関る多関節ロボットによる角環部品のワーク本体への嵌挿を示す斜視図である。 第４実施形態に係る多関節ロボットによる角環部品のワーク本体への嵌挿動作を示すフローチャートである。 第４実施形態に係る多関節ロボットが把持している角環部品の嵌挿順序を示す上面図である。

＜第１実施形態＞
以下、本発明の第１実施形態に係るロボット装置１について、図１から図５を参照しながら説明する。まず、第１実施形態に係るロボット装置全体の概略構成について、図１及び図２を参照しながら説明する。図１は、本発明の第１実施形態に係るロボット装置１の全体構造を模式的に示す斜視図である。図２は、第１実施形態に係る多関節ロボット２を制御する制御装置３のブロック図である。第１実施形態に係るワーク本体Ｗ１は外周面が円錐状である内側部材であり、円環部品Ｗ２はワーク本体Ｗ１に嵌挿可能な円筒状の内周面を持つ外側部材である。

図１に示すように、ロボット装置１は、内側部材としてのワーク本体Ｗ１に外側部材としての円環部品Ｗ２を組み付ける多関節ロボット２と、多関節ロボット２を制御する制御部としての制御装置３と、を備えている。

多関節ロボット２は、６軸多関節のロボットアーム２０と、ロボットアーム２０の先端に接続されたエンドエフェクタ２１と、ロボットアーム２０とエンドエフェクタ２１との間に配設された力覚センサ２２と、を備えている。なお、本実施形態においては、力覚センサ２２を多関節ロボット２の構成要素としたが、ロボット装置１の構成要素としてもよい。

ロボットアーム２０は、各関節を各関節軸周りにそれぞれ回転駆動する６つのアクチュエータ（図示せず）を備えており、６つのアクチュエータのそれぞれを選択的に駆動することでエンドエフェクタ２１を任意の３次元位置に移動させる。エンドエフェクタ２１は、複数のフィンガ２１ａを備えており、複数のフィンガ２１ａでワーク（本実施形態においては円環部品Ｗ２）を把持する。力覚センサ２２は、組み付け途中でフィンガ２１ａが把持する円環部品Ｗ２がワーク本体Ｗ１に接触した際に生じる３軸方向の力及びモーメントを検出可能に構成されている。

図２に示すように、制御装置３は、軌道生成部３０と、軌道保管部３１と、制御指令部３２と、補正計算部３３と、接触判断部３４と、座標演算部３５と、不図示の姿勢制御部と、を備えている。軌道生成部３０は、予め円環部品Ｗ２の任意の位置（多関節ロボット２の手先）に設定された第１座標系に基づいて、ロボットアーム２０の指定軌道としての目標軌道を生成する。軌道保管部３１は、軌道生成部３０で生成された目標軌道を保管する。制御指令部３２は、軌道保管部３１に保存されている位置指令値と、フィードバックされるロボットアーム２０の現在位置と、に基づいてロボットアーム２０の各種リミット内の各軸指令値として計算を行い、ロボットアーム２０をフィードバック制御する。補正計算部３３は、制御指令部３２のフィードバック制御の実施により、力覚センサ２２から反力（以下、「力覚値」という）が入力された際に、力覚値に基づいて位置補正量を計算する。

接触判断部３４は、力覚センサ２２から入力される力覚値から所定の閾値に基づいて接触判断を行う。つまり、本実施形態においては、制御装置３は、制御指令部３２によるフィードバック制御に加え、接触判断部３４により接触判断された場合の接触条件を加えている。接触判断部３４により接触判断されると、ワーク本体Ｗ１の斜面方向に沿った進行方向を定義した第２座標系を計算するため、座標演算部３５に接触判断時の力覚値を入力する。座標演算部３５は、接触判断時の力覚値から、斜面方向に進行方向を、垂直抗力方向に規定した軸方向を向けたロボットアーム２０の第２座標系を作成する。姿勢制御部は、多関節ロボット２に嵌挿動作を実行させる際に、円環部品Ｗ２が一定姿勢を保持するように多関節ロボット２の姿勢制御を行う。

また、制御装置３は、コンピュータ読み取り可能な記録媒体に記録された制御プログラムを読み取り可能な記録メディア読取装置を備えており、記録媒体に記録された制御プログラムを読み取って実行することも可能となっている。

次に、第１実施形態に係るロボット装置１の多関節ロボット２によるワーク本体Ｗ１への円環部品Ｗ２の組み付け動作（制御装置３による多関節ロボット２の制御プログラム）について、図３及び図４を参照しながら説明する。図３は、第１実施形態に係る多関節ロボット２による円環部品Ｗ２のワーク本体Ｗ１への嵌挿を示す図である。図４は、第１実施形態に係る多関節ロボット２による円環部品Ｗ２のワーク本体Ｗ１への嵌挿動作を示すフローチャートである。

図３（ａ）に示すように、円環部品Ｗ２を略円錐形状のワーク本体Ｗ１の小径部分から大径部分に向かって相対移動させて嵌挿する際に、円環部品Ｗ２の軸中心とワーク本体Ｗ１の軸中心とが一致していると、円環部品Ｗ２を直動させることで組み付けできる。

一方、図３（ｂ）及び図３（ｃ）に示すように、多関節ロボット２の位置再現性やワーク本体Ｗ１の公差などの影響で、それぞれの軸中心にズレ１１３が生じると、嵌挿途中でワーク本体Ｗ１の傾斜面に円環部品Ｗ２が接触する。そのため、接触後は、円環部品Ｗ２をワーク本体Ｗ１の傾斜面に沿って動作させる必要がある。しかし、内周面が円筒状の円環部品Ｗ２を、外周面が円錐状のワーク本体Ｗ１へ組み付ける際には、軸中心のズレ１１３が円環部品Ｗ２の内周面のいずれでも起こり得るため、接触点における傾斜方向１１５が不定になる。

そこで、本発明は、接触判断時の力覚値から、接触点におけるワーク本体Ｗ１の外周面に沿った傾斜方向１１５を演算し、傾斜方向１１５に沿って円環部品Ｗ２が移動できるように、傾斜方向１１５を多関節ロボット２の第２座標系として設定する。以下、ワーク本体Ｗ１の円環部品Ｗ２の嵌挿動作に従って説明する。

図４に示すように、まず、円環部品Ｗ２を移動させる際の任意の第１座標系を多関節ロボット２の手先（円環部品Ｗ２上）に設定すると、軌道生成部３０が第１座標系に基づいて、円環部品Ｗ２を嵌挿位置まで移動させる目標軌道を作成する（ステップＳ１）。作成された目標軌道は、軌道保管部３１に保管される。次に、制御指令部３２が目標軌道に従って円環部品Ｗ２が移動するように多関節ロボット２の移動制御を開始すると共に（ステップＳ２）、検出される力覚値に基づいたフィードバック制御を開始する（ステップＳ３）。これにより、嵌挿動作が開始される（嵌挿開始工程）。

嵌挿動作が開始されると、補正計算部３３により、力覚値が入力された際の位置補正量の計算が行われる。計算された位置補正量は、制御指令部３２に送られ、制御指令部３２で位置指令値、現在位置及び位置補正量に基づいたロボットアーム２０の各種リミット内の各軸指令値が計算され、この計算に基づいてロボットアーム２０が順次制御される。

ここで、力覚センサ２２が力覚値を検出すると、接触判断部３４が、力覚センサ２２から入力された力覚値から所定の閾値に基づいた接触判断を行う。具体的には、接触判断部３４は、まず、検出した力覚値が閾値未満であるかを判断する（ステップＳ４、Ｓ５）。検出した力覚値が閾値未満の場合は、円環部品Ｗ２がワーク本体Ｗ１に接触していないと判断し、最終到達位置である嵌挿位置１０６に到達していれば、制御を終了し、到達していなければ、ステップＳ３に戻る（ステップＳ９）。

一方、検出した力覚値が閾値以上である場合（ステップＳ５のＮ）、接触判断部３４は、円環部品Ｗ２がワーク本体Ｗ１に接触したと判断し、座標演算部３５に力覚値が入力される（反力取得工程、ステップＳ６）。座標演算部３５に力覚値が入力されると、座標演算部３５は、入力された力覚値からワーク本体Ｗ１の外周面に沿った傾斜方向を演算し、第２座標系を計算する（座標系作成工程、ステップＳ７）。なお、この計算方法については、後に詳しく説明する。

座標演算部３５により第２座標系が作成されると、作成された第２座標系を軌道生成部３０と補正計算部３３とに送り、軌道生成部３０が新たな進行方向に動作する位置指令値を再演算する（軌道再設定工程、ステップＳ８）。

再計算された位置指令値は、軌道保管部３１に保存されている従来の位置指令値に上書きされ、上書きされた位置指令値は、制御指令部３２に送られる。補正計算部３３では、作成された第２座標系に対する位置補正量が計算され、位置補正量が制御指令部３２に送られる。位置補正量が送られると、制御指令部３２は、上書きされた位置指令値と、現在値、作成された第２座標系に対する補正量を反映し、多関節ロボット２の各種リミット内の各軸指令値として計算を行い、多関節ロボット２を制御する。座標演算部３５で計算された第２座標系に沿った動作は、次の接触判断がされるまで実行され、次の接触判断がされたら、再度座標系を更新し、新しい位置指令値を作成する。そして、円環部品Ｗ２が嵌挿位置１０６に到達していれば、制御を終了し、到達していなければ、ステップＳ３に戻る（嵌挿終了工程、ステップＳ９）。

なお、このとき、制御装置３により、円環部品Ｗ２の一定姿勢を保つように多関節ロボット２の姿勢制御が行われるが、その説明は省略する。

次に、多関節ロボット２の軌道を再設定する際の軌道の計算方法について、図５を参照しながら説明する。図５は、円環部品Ｗ２の嵌挿途中で円環部品Ｗ２がワーク本体Ｗ１に接触した際の多関節ロボット２の軌道計算を説明するための図である。

本実施形態においては、進行方向、力覚ベクトル共に同方向であり、これを従来の第１座標系１１８として定義し、進行方向をＺ軸として直進しているものとして計算する。まず、第１座標系１１８における接触力を各ベクトル並行方向（Ｆｘ、Ｆｙ、Ｆｚ）と定義する。本実施形態においては、並進Ｆ＝（Ｆｘ、Ｆｙ、Ｆｚ）、モーメントＭＦ＝（ＭＦｘ、ＭＦｙ、ＭＦｚ）の６軸を検出する力覚センサを使用し、そのうちの並進を使用する。

３軸力覚センサの場合、力の成分が並進方向Ｆｚ、モーメント方向ＭＦｘ、ＭＦｙとして出力されることが多いが、この場合、ＭＦｘ、ＭＦｙを並進方向Ｆｘ、Ｆｙに変換を行った後、次の計算を行う。まず、軸中心を視点とした接触点の方向を計算するため、Ｆｘ、Ｆｙの合力Ｆｘｙを計算する。

次に、上述で計算した合力Ｆｘｙの角度を計算する。この角度は、進行方向を法線とした平面に対するエンドエフェクタ２１の軸中心を視点とした円環部品Ｗ２の接触点の方向であり、Ｚ軸の回転角度として定義される。

次に、Ｆｚと合力Ｆｘｙのベクトル角度を計算する。この角度は、進行方向を法線とした平面に対する垂直抗力ベクトル１１７の方向であり、傾斜方向１１５及び斜面方向ベクトル１１６の計算をするためのＹ軸の回転角度として定義される。

次に、把持中心座標（従来の第１座標系１１８）をＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）とし、接触する円環部品Ｗ２の半径をｒ、ロボットアーム２０の先端取付け面の座標０から座標Ｐまでの距離をＬとする。すると、上述の数２の式より、接触した座標Ｑの並進成分（Ｑｘ、Ｑｙ、Ｑｚ）は、以下の式で表すことができる。

円環部品Ｗ２の半径が不明の場合は０として定義することができ、その場合、従来の第１座標系上の位置を中心とした新規座標系を作成することになる。まず、上述の数２から数４の式より、座標Ｑの並進及び回転パラメータを求める。

次に、数５の式のパラメータより、座標Ｑを求める。本実施形態における座標の計算は、オイラー角のＺＹＸ行列計算とし、以下の式により求めることができる。

数５の式のパラメータより、数６から数８の式の座標計算を行った結果を以下に示す。

数９の式より求められた座標を、第２座標系Ｑとして定義する。この第２座標系Ｑは、斜面方向ベクトル１１６にＺ軸、垂直抗力ベクトル１１７にＸ軸方向を向けた座標となる。

次に、登録した第２座標系Ｑに対する現在位置教示点Ａの作成を行う。まず、ベース座標より第２座標系Ｑまでの位置を順運動学計算で計算し、計算した座標を新規に現在位置教示点Ａとして登録する。

次に、嵌挿位置１０６までの軌道を計算する。まず、従来の軌道より、従来の最終到達位置を順運動学計算で計算し、目標教示点Ｂとして登録する。次に、現在位置教示点のＰと目標教示点Ｂ間の進行方向の移動距離Ｄを計算する。このＰＢ間の移動距離Ｄは、進行方向として定義していたＺ軸方向の距離になる。

次に、現在位置教示点Ａから傾斜方向１１５へ、従来の第１座標系１１８のＺ軸方向に移動距離Ｄだけ移動させた新規教示点Ｂ´を作成する。まず、ＢＢ´間の距離Ｒは、次の式で計算される。

数８及び数１０の式より、新規教示点Ｂ´のパラメータは以下のようになる。

数１１の式のパラメータより、数６から数８の式の座標計算を行った結果を以下に記す。

数１２の式より求められた座標を、新規教示点Ｂ´として定義する。計算した現在位置教示点Ａと、新規教示点Ｂ´を用い、第２座標系Ｑに対する直進補間を実施すると、ワーク本体Ｗ１の傾斜面に沿った軌道を作成することができる。そして、中継位置１０４に到達すると、斜面方向が変化するが、サンプリング周期毎に接触条件の変化を確認することで、再度軌道の修正を行い、円環部品Ｗ２をワーク本体Ｗ１に嵌挿することが可能になる。

以上説明したように、第１実施形態に係るロボット装置１は、円環部品Ｗ２をワーク本体Ｗ１に嵌挿させる際に、円環部品Ｗ２がワーク本体Ｗ１に接触すると、接触点を原点としたワーク本体Ｗ１の傾斜方向に沿った第２座標系Ｑを設定する。そのため、円環部品Ｗ２をワーク本体Ｗ１の傾斜方向に沿って移動させることができる。これにより、例えば、円環部品Ｗ２やワーク本体Ｗ１の公差や動作中のロボットアーム２０の姿勢の再現性によるズレが生じた場合においても、円環部品Ｗ２のワーク本体Ｗ１への組み付けの失敗を低減させることができる。その結果、生産性を向上させることができる。

また、接触点の力覚値に応じて傾斜方向を演算するため、円錐形状のワーク本体Ｗ１に円環部品Ｗ２の組み付けを行う場合のように、ワーク本体Ｗ１の傾斜面との接触が円環部品Ｗ２の内周上のどの位置でも起こり得る場合でも、対応させることが可能になる。

＜第２実施形態＞
次に、本発明の第２実施形態に係るロボット装置１Ａについて、図１及び図２を援用すると共に、図６を参照しながら説明する。第２実施形態に係るロボット装置１Ａは、中継位置１０４以降の円環部品Ｗ２の動作が第１実施形態と相違する。そのため、第２実施形態においては、第１実施形態と相違する点、即ち、中継位置１０４以降の円環部品Ｗ２の動作を中心に説明し、第１実施形態と同様の構成については、同じ符号を付してその説明を省略する。

図６は、第２実施形態に係るロボット装置１Ａがワーク本体Ｗ１に円環部品Ｗ２を組み付ける状態を示す図である。

第１実施形態においては、嵌挿位置１０６まで接触条件の変化を確認し、円環部品Ｗ２の組み付け中継位置１０４で再度軌道の修正を行っている。しかし、事前に円環部品Ｗ２の嵌挿位置１０６が分かっている場合、中継位置１０４までの補正のみを指示することで、補正による斜面間の軌道を作成し、動作させることができる。その後、補正を終了して従来の第１座標系１１８に対する中継位置１０４から嵌挿位置１０６までの軌道を生成することで、円環部品Ｗ２のワーク本体Ｗ１への組み付けが可能になる。よって、第１実施形態より計算時間を短縮することができる。

＜第３実施形態＞
次に、本発明の第３実施形態に係るロボット装置について、図１及び図４を援用すると共に、図７から図９を参照しながら説明する。第３実施形態に係るロボット装置は、内側部材及び外側部材の形状が第１実施形態と相違する。そのため、第３実施形態においては、第１実施形態と相違する点、即ち、内側部材及び外側部材の形状の差による相違による動作の差を中心について説明し、第１実施形態と同様の構成については同じ符号を付してその説明を省略する。

図７は、第３実施形態に関る多関節ロボット２による角環部品Ｗ４のワーク本体Ｗ３への嵌挿を示す斜視図である。図８は、第３実施形態に係る多関節ロボット２が把持している異なる形状を持つ部品の上面図である。図９は、第３実施形態に係る多関節ロボット２が把持している角環部品Ｗ４の嵌挿順序を示す上面図である。

第３実施形態に係るワーク本体Ｗ３は外周面が角錐状である内側部材であり、角環部品Ｗ４はワーク本体Ｗ３に嵌挿可能な角環状の内周面を持つ外側部材である。以下、第１実施形態と同様に、角環部品Ｗ４のワーク本体Ｗ３への嵌挿動作を説明する。

なお、角環部品Ｗ４の形状は図７に示す四角形に限定しない。例えば、図８（ａ）及び（ｂ）に示すような正多角環部品Ｗ５及びＷ６や、図８（ｃ）に示すような長方角環部品Ｗ７、図８（ｄ）に示すような多角環部品Ｗ８など、多種多様な形状（三角形以上の多角形）に対応する。

図７に示すように、ワーク本体Ｗ３と角環部品Ｗ４との間には中心軸ズレ１１３が生じており、角環部品Ｗ４をワーク本体Ｗ３に下降させるだけでは組付けることができない。そこで、第１実施形態と同様に、接触判断時の力覚値から、接触点におけるワーク本体Ｗ３の外周面に沿った傾斜方向１１５を計算し、傾斜方向１１５に沿って角環部品Ｗ４が移動できるように、傾斜方向１１５を多関節ロボット２の第２座標系として設定する。以下、ワーク本体Ｗ３の角環部品Ｗ４の嵌挿動作に従って説明する。なお、本実施形態では、嵌挿方向に対する回転位相ズレは存在しないものとする。

図９（ａ）に示すように、中心軸ズレ１１３により、角環部品Ｗ４は嵌挿途中でワーク本体Ｗ３の斜面１２２に接触し、斜面後方１２３へ力がかかる。このとき、第１実施形態で説明した反力取得工程（ステップＳ４）から軌道再設定工程（ステップＳ８）までの計算により、斜面方向１２３へ移動する。

なお、第１実施形態では、第２座標系の位置を円環部品Ｗ１の円周上に作成するが、第３実施形態の角環部品Ｗ４の場合、円周は存在しない。そのため、計算時に使用する数４、数５及び数１０の半径の値は０として定義し、従来の第１座標系上の位置を中心とした第２座標系を作成する。

斜面方向１２３に移動すると、図９（ｂ）に示すように、ワーク本体Ｗ３の稜線１２４と角環部品Ｗ４の内周角１２５が接触し、稜線方向１２６へ閾値以上の力が生じる。嵌挿位置１０６へ到達するまで、サンプリング周期毎に接触条件の変化を確認しているため、検出した力覚値よりステップＳ４からステップＳ８までの再計算を行い、再度軌道の修正を行う。再計算により定義された新規軌道により、角環部品Ｗ４は稜線方向１２６に移動が開始され、図５で示した中継位置１０４まで移動する。

中継位置１０４まで移動すると、図９（ｃ）に示すように、ワーク本体Ｗ３と角環部品Ｗ４の中心軸ズレ１１３が解消して嵌め合い、接触条件が変化し、閾値以上の力が生じる。嵌挿位置１０６へ到達するまで、サンプリング周期毎に接触条件の変化を確認しているため、検出した力覚値よりステップＳ４からステップＳ８までの再計算を行い、再度軌道の修正を行う。再計算により定義された新規軌道により、角環部品Ｗ４は嵌挿位置１０６まで移動し、ワーク本体Ｗ３に嵌挿することが可能になる。

以上、本実施形態に示すように、本発明では円環形状だけではなく、角環形状の部材の嵌挿も可能である。

＜第４実施形態＞
次に、本発明の第４実施形態に係るロボット装置について、図１、図４及び図７を援用すると共に、図１０から図１２を参照しながら説明する。第４実施形態に係るロボット装置は、ワーク本体Ｗ３と角環部品Ｗ４との間に回転位相ズレが存在する点が第３実施形態と相違する。そのため、第４実施形態においては、第３実施形態と相違する点、即ち、ワーク本体Ｗ３と角環部品Ｗ４の回転位相ズレを解消するために実行する回転動作について説明し、第１及び第３実施形態と同様の構成については、同じ符号を付して説明を省略する。

図１０は、第４実施形態に関る多関節ロボット２による角環部品Ｗ４のワーク本体Ｗ３への嵌挿を示す斜視図である。図１１は、第４実施形態に係る多関節ロボット２による角環部品Ｗ４のワーク本体Ｗ３への嵌挿動作を示すフローチャートである。

第３実施形態では、図７に示すようにワーク本体Ｗ３と角環部品Ｗ４の回転位相ズレはなく、位相が一致している。そのため、図４に示すように、新規軌道作成後、閾値未満（ステップＳ５のＹ）の場合は、第２座標系による新規軌道へ移動し、中継位置１０４まで移動した後、再計算が行われ、嵌挿位置１０６に到達する（ステップＳ９のＹ）と嵌挿が可能であった。しかし、本実施形態では、図１０に示すように、ワーク本体Ｗ３と角環部品Ｗ４の間に回転位相ズレが生じ、位相が一致しない場合は、嵌挿位置１０６まで到達せずに途中で止まり嵌挿が不可能である。そのため、角環部品Ｗ４をワーク本体Ｗ３へ組み付ける為には、嵌挿方向を軸とした回転動作１２７をさせることで回転位相ズレを合わせる必要がある。

そこで、回転位相ズレを合わせるため、図１１に示すようなフローを追加する。まず、閾値未満（ステップＳ５のＹ）の場合に、第２座標系による新規軌道へ移動しているかを判断する（新規軌道判断工程、ステップＳ１０）。第２座標系による新規軌道へ移動中の場合は、新規軌道への移動に従来の嵌挿方向を軸とした回転動作１２７、つまり、第１座標系の嵌挿方向に対する回転動作１２７を付加する（回転動作付加工程、ステップＳ１１）。新規軌道へ移動中に回転動作１２７による回転を行うことで回転位相ズレを合わせ、嵌挿を行えるようにする。以降に、上記ステップＳ１０及びステップＳ１１を用いた、第４実施形態に関する嵌挿順序について説明する。

図１２は、第４実施形態に係る多関節ロボット２が把持している角環部品Ｗ４の嵌挿順序を示す上面図である。

図１２（ａ）に示すように、中心軸ズレ１１３及び回転位相ズレにより、角環部品Ｗ４は嵌挿途中でワーク本体Ｗ３の稜線１２８に接触する。このとき、第３実施形態で説明した反力取得工程（ステップＳ４）から軌道再設定工程（ステップＳ８）までの計算により、第２座標系による新規軌道が作成され、稜線方向１２９へ移動する。

稜線方向１２９へ移動すると、途中でワーク本体Ｗ３の稜線１３０に角環部品Ｗ４が接触するが、軽度の接触により閾値未満（ステップＳ５のＹ）となる。すると、稜線方向１２９へ移動しているため第２座標系による新規軌道と判断される（ステップＳ１０のＹ）。新規軌道と判断されると、回転動作１２７を行うため第１座標系の嵌挿方向に対するモーメントのインピーダンス制御が定義され、実行される（ステップＳ１１）。これにより、嵌挿方向に対するモーメント方向に力が生じると、新規軌道の移動に生じた力が緩和する方向、つまり、嵌挿方向に対する回転方向にインピーダンス制御による補正量が付加され、回転動作を行うことができる。

本実施形態では、角環部品Ｗ４はワーク本体Ｗ３の稜線１３０に接触しているため、稜線方向１２９へ移動中に摩擦力ｆによるねじれが発生し、第１座標系の嵌挿方向に対するモーメントの力Ｍｆが生じる。モーメント方向に生じた力Ｍｆはインピーダンス制御により補正量として新規軌道に付加されるため、稜線方向１２９へ移動しながら力Ｍｆが緩和する方向へ回転する回転動作１２７となる。

回転動作１２７により角環部品Ｗ４は回転をしながら稜線方向１２９へ移動する。移動すると、図１２（ｂ）に示すように、ワーク本体Ｗ３の稜線方向１３０と角環部品Ｗ４の内周角１３１が合わる。さらに稜線方向１２９に移動すると再度接触条件が変化し、ワーク本体Ｗ３の稜線と角環部品Ｗ４の内周角１３１が合わさった稜線方向１３２へ閾値以上の力が生じる。嵌挿位置１０６へ到達するまで、サンプリング周期毎に接触条件の変化を確認しているため、検出した力覚値よりステップＳ４からステップＳ８までの再計算を行い、再度軌道の修正を行う。再計算により定義された新規軌道により、角環部品Ｗ４は稜線方向１３２へ移動し、中継位置１０４まで移動する。

中継位置１０４まで移動すると、図１２（ｃ）に示すように、ワーク本体Ｗ３と角環部品Ｗ４の中心軸ズレ１１３が解消して嵌め合い、接触条件が変化し、閾値以上の力が生じる。嵌挿位置１０６へ到達するまで、サンプリング周期毎に接触条件の変化を確認しているため、検出した力覚値よりステップＳ４からステップＳ８までの再計算を行い、再度軌道の修正を行う。再計算により定義された新規軌道により、角環部品Ｗ４は嵌挿位置１０６まで移動し、ワーク本体Ｗ３に嵌挿することが可能になる。以上のように、回転位相ズレが発生した場合もワーク本体Ｗ３に角環部品Ｗ４は嵌挿が可能となる。

以上、本実施形態に示すように、本発明では回転位相ズレがある角環形状の部材の嵌挿も可能である。

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。

例えば、第１及び第２実施形態においては、一方の部材としての円環部品Ｗ２をエンドエフェクタ２１に把持させて、他方の部材としてのワーク本体Ｗ１に嵌挿させる嵌挿動作を用いて説明したが、本発明においてはこれに限定されない。例えば、エンドエフェクタ２１にワーク本体Ｗ１を把持させて円環部品Ｗ２に嵌挿させる場合にも用いることができる。第３及び第４実施形態においても同様に、ワーク本体Ｗ３を把持させて角環部品Ｗ４に嵌挿させる場合にも用いることができる。

また、第１及び第２実施形態においては、円環部品Ｗ２がワーク本体Ｗ１に接触すると、接触点を原点としたワーク本体Ｗ１の傾斜方向に沿った第２座標系Ｑを設定したが、本発明においてはこれに限定されない。例えば、傾斜方向が分かれば、接触点とは異なる任意の位置に第２座標系を設定してもよい。第３及び第４実施形態においても同様に、傾斜方向が判れば、接触点とは異なる任意の第２座標系を設定してもよい。

１、１Ａ ロボット装置
２ 多関節ロボット
３、３Ａ 制御装置
２２ 力覚センサ
１１５ 傾斜方向
１１８ 第１座標系
Ｑ 第２座標系
Ｗ１ ワーク本体（内側部材）
Ｗ２ 円環部品（外側部材）

本発明は、ロボットと、前記ロボットに接続されたエンドエフェクタと、力覚センサと、前記エンドエフェクタを、ワークに対して相対的に移動させて所定の作業を行う際に、予め設定された第１の軌道で、前記エンドエフェクタを移動させ、前記エンドエフェクタと前記ワークとの間で発生する力を、前記力覚センサによって検出し、前記力覚センサで検出された値から、前記第１の軌道で移動させられている前記エンドエフェクタの移動方向に対して傾斜している、前記ワークに設けられた傾斜面の傾斜方向に前記エンドエフェクタを移動させる第２の軌道を求め、前記第１の軌道から前記第２の軌道に切り換えて、前記エンドエフェクタを移動させる制御装置と、を備えた、ことを特徴とするロボット装置である。
また、本発明は、力覚センサを有するロボットに接続されたエンドエフェクタを、ワークに対して相対的に移動させて所定の作業を実行させるロボットの制御方法において、制御部が、予め設定された第１の軌道で、前記エンドエフェクタを移動させて、前記所定の作業を開始する作業開始工程と、前記制御部が、前記エンドエフェクタと前記ワークとの間で発生する力を、前記力覚センサによって取得する取得工程と、前記制御部が、前記力覚センサで取得された値から、前記第１の軌道で移動させられている前記エンドエフェクタの移動方向に対して傾斜している、前記ワークに設けられた傾斜面の傾斜方向に前記エンドエフェクタを移動させる第２の軌道を演算する演算工程と、前記制御部が、前記第２の軌道を設定する軌道設定工程と、前記制御部が、設定された前記第２の軌道に従って前記ロボットを制御して、前記所定の作業を実行させる作業実行工程と、を備えた、ことを特徴とするロボットの制御方法である。
また、本発明は、ロボットに接続されたエンドエフェクタと、力覚センサと、を有するロボット装置を制御する制御装置であって、前記エンドエフェクタを、ワークに対して相対的に移動させて所定の作業を行う際に、予め設定された第１の軌道で、前記エンドエフェクタを移動させ、前記エンドエフェクタと前記ワークとの間で発生する力を、前記力覚センサによって検出し、前記力覚センサで検出された値から、前記第１の軌道で移動させられている前記エンドエフェクタの移動方向に対して傾斜している、前記ワークに設けられた傾斜面の傾斜方向に前記エンドエフェクタを移動させる第２の軌道を求め、前記第１の軌道から前記第２の軌道に切り換えて、前記エンドエフェクタを移動させることを特徴とする制御装置である。

Claims (9)

1. 多関節ロボットと、
   前記多関節ロボットが受ける反力を検出可能な力覚センサと、
   前記多関節ロボットが把持した、外周面を持つ内側部材及び前記内側部材を嵌挿可能な内周面を持つ外側部材のいずれか一方の部材を、他方の部材に嵌挿させる嵌挿動作を行う際に、前記多関節ロボットが把持する前記一方の部材に設定した第１座標系に基づいて作成された指定軌道で、前記内側部材の小径部分から大径部分に向かって前記外側部材を相対移動させ、前記多関節ロボットが把持した前記一方の部材が前記他方の部材に接触して前記力覚センサが前記反力を検出すると、前記反力から、前記多関節ロボットが把持した前記一方の部材を前記内側部材の外周面の傾斜方向に沿って移動させるための前記傾斜方向に沿った第２座標系を前記多関節ロボットが把持した前記一方の部材に設定し、前記第２座標系で前記一方の部材を把持した前記多関節ロボットを移動制御する制御装置と、を備えた、
   ことを特徴とするロボット装置。
2. 前記制御装置は、前記嵌挿動作を行う際に、前記多関節ロボットが把持した前記一方の部材が一定姿勢を保つように前記多関節ロボットの姿勢制御を行う、
   ことを特徴とする請求項１に記載のロボット装置。
3. 前記制御装置は、前記多関節ロボットが把持した前記一方の部材が前記他方の部材に接触した際の接触点を原点とする前記第２座標系を、前記一方の部材に設定する、
   ことを特徴とする請求項１又は２に記載のロボット装置。
4. 多関節ロボットが把持した、外周面を持つ内側部材及び前記内側部材を嵌挿可能な内周面を持つ外側部材のいずれか一方の部材を、他方の部材に嵌挿させる嵌挿動作を実行させる多関節ロボットの制御方法において、
   制御部が、前記多関節ロボットが把持する前記一方の部材に設定した第１座標系に基づいて作成された指定軌道で、前記内側部材の小径部分から大径部分に向かって前記外側部材を相対移動させて、前記内側部材と前記外側部材との嵌挿動作を開始する嵌挿開始工程と、
   前記制御部が、前記多関節ロボットが把持した前記一方の部材が前記他方の部材に接触することで、前記多関節ロボットが受ける反力を検出可能な力覚センサが検出した反力を取得する反力取得工程と、
   前記制御部が、取得した前記反力から、前記内側部材と前記外側部材との接触点における前記内側部材の外周面に沿った傾斜方向を進行方向とする第２座標系を前記多関節ロボットが把持した前記一方の部材に作成する座標系作成工程と、
   前記制御部が、前記第２座標系で前記多関節ロボットが把持した前記一方の部材が移動するように前記多関節ロボットの軌道を再設定する軌道再設定工程と、
   前記制御部が、再設定された軌道に従って前記多関節ロボットを制御して、前記内側部材と前記外側部材とを嵌挿させる嵌挿終了工程と、を備えた、
   ことを特徴とする多関節ロボットの制御方法。
5. 前記制御部は、前記多関節ロボットに前記嵌挿動作を実行させる際に、前記多関節ロボットが把持した前記一方の部材が一定姿勢を保つように前記多関節ロボットの姿勢制御を行う、
   ことを特徴とする請求項４に記載の多関節ロボットの制御方法。
6. 前記制御部は、前記座標系作成工程において、前記多関節ロボットが把持した前記一方の部材が前記他方の部材に接触した際の接触点を原点とする前記第２座標系を、前記一方の部材に設定する、
   ことを特徴とする請求項４又は５に記載の多関節ロボットの制御方法。
7. 前記制御部が、前記再設定された軌道を移動中かを判断する新規軌道判断工程と、
   前記設定された軌道を移動中に、前記第１座標系に基づいて作成された指定軌道の嵌挿方向を軸とした回転を付加させながら嵌挿させる回転動作付加工程と、を備えた、
   ことを特徴とする請求項４から６のいずれか１項に記載の多関節ロボットの制御方法。
8. 請求項４から７のいずれか１項に記載の各工程をコンピュータに実行させるための多関節ロボットの制御プログラム。
9. 請求項８に記載の多関節ロボットの制御プログラムを記録したコンピュータ読み取り可能な記録媒体。

Images