(57)【要約】【課題】  ロボット装置による作業においてワーク同士の接触により接触点が複数存在する場合でも容易に適用可能な、３軸力センサを６軸力センサの代替として作業を行うロボット制御装置を提供する。【解決手段】  力検出手段と位置検出手段とを有したロボットアームであって、前記ロボットアームの制御装置は、第１の対象物記第２の対象物の形状情報が記憶された形状記憶部と、力検出手段で検出された値と、位置検出手段で検出された値と、形状情報とから、第１の対象物と第２の対象物との複数の接触点の位置を推定する接触点推定部と、複数の接触点の位置、および力検出手段から検出された値とから第１の対象物と第２の対象物とが接触することで生じるトルクの値を算出するトルク値算出部と、を備えることを特徴とする制御装置を採用した。【選択図】  図４
2019155523.tif 000002
【特許請求の範囲】
【請求項１】
力検出手段と位置検出手段とを有したロボットアームにより、動作させられる第１の対象物を第２の対象物に接触させて作業を行うロボットを制御するロボット制御装置であって、
前記ロボット制御装置は、
前記第１の対象物と前記第２の対象物の形状情報が記憶された形状記憶部と、
前記力検出手段で検出された値と、前記位置検出手段で検出された値と、前記形状情報とから、前記第１の対象物と前記第２の対象物との複数の接触点の位置を推定する接触点推定部と、
前記複数の接触点の位置、および前記力検出手段から検出された値とから前記第１の対象物と前記第２の対象物とが接触することで生じるトルクの値を算出するトルク値算出部と、
を備えることを特徴とするロボット制御装置。
【請求項２】
請求項１に記載のロボット制御装置において、
前記トルク値算出部は、
前記複数の接触点の位置と、前記力検出手段から検出された値とを所定の座標系に変換し、線形計画問題を解くことで前記トルクの値を算出することを特徴とするロボット制御装置。
【請求項３】
請求項１または請求項２に記載のロボット制御装置において、
前記力検出手段は、３軸にかかるトルクの検出をしないことを特徴とするロボット制御装置。
【請求項４】
請求項１から請求項３のいずれか１項に記載のロボット制御装置において、
前記位置検出手段は、前記ロボットアームの各関節に備えられていることを特徴とするロボット制御装置。
【請求項５】
請求項１から請求項４のいずれか１項に記載のロボット制御装置において、
前記形状記憶部は、前記第１の対象物の形状情報としてロボットハンドの形状情報を記憶していることを特徴とするロボット制御装置。
【請求項６】
力検出手段と位置検出手段とを有したロボットアームにより、動作させられる第１の対象物を第２の対象物に接触させて作業を行うロボットの制御方法であって、
前記制御方法は、
前記力検出手段を用いて前記第１の対象物と前記第２の対象物が接触することで生じる反力の値を検出する力検出工程と、
前記位置検出手段を用いて前記ロボットアームの位置姿勢を検出する位置姿勢検出工程と、
前記第１の対象物と前記第２の対象物の形状情報を読み出す形状情報読出工程と、
前記力検出工程で検出された値と、前記位置検出工程で検出された値と、前記形状情報読出工程で読み出された形状情報とから、前記第１の対象物と前記第２の対象物との複数の接触点の位置を推定する接触点推定工程と、
前記複数の接触点の位置、および前記力検出工程で検出された値とから前記第１の対象物と前記第２の対象物とが接触することで生じるトルクの値を算出するトルク値算出工程と、
を有することを特徴とする制御方法。
【請求項７】
請求項６に記載の制御方法において、
前記トルク値算出工程は、
前記複数の接触点の位置と、前記力検出手段から検出された値とを所定の座標系に変換し、線形計画問題を解くことで前記トルクの値を算出することを特徴とする制御方法。
【請求項８】
請求項６または請求項７に記載の制御方法において、
前記第１の対象物と前記第２の対象物とを接触させている際、所定の時間が経過しても前記ロボットアームの位置姿勢に変化がない場合、
前記第１の対象物と前記第２の対象物とを離間させることを特徴とする制御方法。
【請求項９】
力検出手段と位置検出手段とを有したロボットアームにより、動作させられる第１の対象物を第２の対象物に接触させて組立を行うロボットを用いた物品の組立方法であって、
前記組立方法は、
前記力検出手段を用いて前記第１の対象物と前記第２の対象物が接触することで生じる反力の値を検出する力検出工程と、
前記位置検出手段を用いて前記ロボットアームの位置姿勢を検出する位置姿勢検出工程と、
前記第１の対象物と前記第２の対象物の形状情報を読み出す形状情報読出工程と、
前記力検出工程で検出された値と、前記位置検出工程で検出された値と、前記形状情報読出工程で読み出された形状情報とから、前記第１の対象物と前記第２の対象物との複数の接触点の位置を推定する接触点推定工程と、
前記複数の接触点の位置、および前記力検出工程で検出された値とから前記第１の対象物と前記第２の対象物とが接触することで生じるトルクの値を算出するトルク値算出工程と、
前記力検出工程で検出された値と、前記位置検出工程で検出された値と、前記トルク値算出工程で算出された値と、に基づいて前記第１の対象物を前記第２の対象物を組付ける組付工程と、を有することを特徴とする物品の組立方法。
【請求項１０】
請求項９に記載の物品の組立方法において、
前記トルク値算出工程は、
前記複数の接触点の位置と、前記力検出手段から検出された値とを所定の座標系に変換し、線形計画問題を解くことで前記トルクの値を算出することを特徴とする物品の組立方法。
【請求項１１】
請求項６から請求項１０のいずれか１項に記載の制御方法または物品の組立方法を実行させるための制御プログラム。
【請求項１２】
請求項１１に記載の制御プログラムを記録した、コンピュータで読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置およびロボット制御方法に関する。
【背景技術】
【０００２】
近年、工場の生産ラインにおいて、作業の自動化・省人化・効率化を図るために、ロボット装置による自動組付を行うケースが多くなっている。ロボット装置による自動組付を行う場合、ロボット装置で把持したワークを他のワークへ直接組付を行うが、精度の高い嵌合やワーク同士の接触の判定が必要な組付では、組付の際にワーク同士で生じる反力を検出し、動作制御に反映する必要がある。そこで、ロボット装置にＸＹＺ座標軸方向の力及び各軸回りのモーメント力を合わせた６軸方向の力検知センサを内蔵させ、ワーク同士を組付けた際に発生する力を検出し、精度の高いロボット装置の動作制御を実現する。しかしながら６軸力センサは、一般に構造が複雑でサイズが大きく、高価である。
【０００３】
特許文献１では、６軸力センサの代替として３軸力センサを用いてロボット装置の動作制御を行うことが記載されている。１軸方向の力と、その軸に直交な２軸方向の軸周りのモーメントの３つの成分を計測できる力センサを用い、力センサの検出結果とロボット装置のツールとワークの接触点を推定し、力の情報である残りの３つの成分を算出し６軸力センサの効果を得ている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１２−１３７４２１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら特許文献１に記載された制御装置はツールを直接ワークに接触させる動作を制御しており、基本的にロボット装置のツールとワークとの接触点は１つである。ロボット装置によるワークの自動組付において、例えば把持したワークを別のワークの穴に嵌合する場合、把持したワークの姿勢により、ワーク同士の接触点が複数存在する場合がある。ゆえに特許文献１に記載された制御装置を、上記のような嵌合作業に用いることは難しい。
【０００６】
そこで本発明は上述の課題に鑑み、ロボット装置による作業においてワーク同士の接触により接触点が複数存在する場合でも容易に適用可能な、３軸力センサを６軸力センサの代替として作業を行うロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記課題を解決するために本発明は、力検出手段と位置検出手段とを有したロボットアームにより、動作させられる第１の対象物を第２の対象物に接触させて作業を行うロボットを制御するロボット制御装置であって、前記ロボット制御装置は、前記第１の対象物と前記第２の対象物の形状情報が記憶された形状記憶部と、前記力検出手段で検出された値と、前記位置検出手段で検出された値と、前記形状情報とから、前記第１の対象物と前記第２の対象物との複数の接触点の位置を推定する接触点推定部と、前記複数の接触点の位置、および前記力検出手段から検出された値とから前記第１の対象物と前記第２の対象物とが接触することで生じるトルクの値を算出するトルク値算出部と、を備えることを特徴とするロボット制御装置を採用した。
【発明の効果】
【０００８】
本発明によれば、ロボット装置による作業においてワーク同士の接触による反力が生じる接触点が複数存在する場合でも、各接触点を推定でき、接触点が複数存在する嵌合のような作業に適用することができる。これにより、種々の作業を６軸力センサより安価な３軸力センサで実現することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１の実施形態および第２の実施形態におけるロボットシステムの概略構成を示した説明図である。
【図２】本発明の第１の実施形態および第２の実施形態における力センサ７００の概略を示した説明図である。
【図３】本発明の第１の実施形態および第２の実施形態におけるロボットアーム制御装置の概略を示したブロック図である。
【図４】本発明の第１の実施形態における嵌合処理の手順を示すフローチャートである。
【図５】本発明の第１の実施形態におけるロボットハンド本体３００とワークＷ１を示した模式図である。
【図６】図４のフローチャートにおける各ステップの状態図である。
【図７】本発明の第２の実施形態におけるロボットハンド本体３００とワークＷ１’を示した模式図である。
【図８】本発明の第２の実施形態における嵌合処理の手順を示すフローチャートである。
【図９】図８のフローチャートにおける各ステップの状態図である。
【発明を実施するための形態】
【００１０】
以下、添付図面を参照して本発明を実施するための形態につき説明する。なお、以下に示す実施形態はあくまでも一例であり、例えば細部の構成については本発明の趣旨を逸脱しない範囲において当業者が適宜変更することができる。また、本実施形態で取り上げる数値は、参考数値であって、本発明を限定するものではない。     
【００１１】
（第１の実施形態）
以下、添付図面を参照して、本実施形態１における構成および把持制御につき説明する。図１は、本発明のロボット装置を用いた本実施形態のロボットシステムの概略構成を示した図である。
【００１２】
図１において、本実施形態のロボットシステム１００は、ロボットアーム本体２００、ロボットハンド本体３００、ロボットシステム全体を制御するシステム制御装置４００、外部入力装置８００で構成される。組付対象物であるワークＷ１は、ワーク載置台Ｓ１上に載置され、被組付対象物であるワークＷ２は、ワーク固定台Ｓ２上に固定されている。
【００１３】
ロボットシステム１００によってワークＷ１、ワークＷ２を操作し、組付けることにより、工業製品、ないしはその部品を製造することができる。例えば、このワークＷ１、Ｗ２に対する組付操作は、ロボットアーム本体２００とロボットハンド本体３００を用いて組付対象物としてのワークＷ１を把持し、移動させ、さらにワークＷ１をワークＷ２に嵌合させるという操作によって行われる。
【００１４】
ロボットアーム本体２００は、本実施形態では多関節のロボットアームであり、ロボットアーム本体２００の根元は基台９００に固定されている。ロボットアーム本体２００の先端には、把持装置であるロボットハンド本体３００が力センサ７００を介して装着されている。このロボットハンド本体３００を用いてワークＷ１に対して動作を行う。また、ロボットアーム本体２００の各関節には、これらの関節を各々駆動する駆動源としてのモータ、およびモータの回転角度を検出する位置検出手段としてのエンコーダがそれぞれ設けられている。なお、エンコーダの設置位置および出力方式は問わない。
【００１５】
ロボットハンド本体３００は不図示のモータと開閉機構により２本の指を開閉し、ワークＷ１の把持ないし解放を行う１自由度以上のハンドであり、ワークＷ１をロボットアーム本体２００に対して相対変位させないように把持できれば良い。なお、本実施形態では指の本数を２本としたが、当業者が適宜変更することができる。
【００１６】
また本実施形態において、ロボットハンド３００は不図示のモータ駆動で指を動作させているが、空気圧駆動のようなエアグリッパであっても良い。さらに、ロボットハンド本体３００は、ワークＷ１をロボットアーム本体２００に対して相対変位させないように把持できれば良いため吸着機構であっても良い。
【００１７】
システム制御装置４００内には、ロボットアーム本体２００を制御するロボットアーム制御装置６００、ロボットハンド本体３００を制御するロボットハンド制御装置５００を備えている。なお、本実施形態ではロボットアーム本体２００とロボットハンド本体５００の制御を別々の制御装置を用いて実行しているが、１つの制御装置を用い統合して制御しても良い。
【００１８】
図１に示すようにロボットハンド制御装置５００とロボットアーム制御装置６００は、それぞれマイクロプロセッサなどから成るＣＰＵ５０１、６０１から構成される。さらに各ＣＰＵにバス接続されたＲＯＭ５０２、６０２、ＲＡＭ５０２、６０３、汎用信号インターフェイス５０４、６０４で構成されている。そしてロボットハンド本体３００およびロボットアーム本体２００のそれぞれに設けられた不図示のモータを、モータドライバ５０５、６０５を介して制御する。
【００１９】
ＲＯＭ５０２、６０２には、各種動作に応じてロボットハンド本体３００およびロボットアーム本体２００に対応する駆動部を制御するための制御プログラムや、それらの制御に必要なデータ等が格納されている。これらロボットシステムを制御する上で必要なデータ、設定値、制御プログラム等をＲＡＭ５０３、６０３で展開する。
【００２０】
また汎用入出力インターフェイスＩ／Ｏ５０４、６０４にはティーチングペンダント等の外部入力装置８００が接続されている。これによりユーザーが直接ロボットシステム１００を動作させることができる。汎用入出力インターフェイスＩ／Ｏ５０４、６０４はそれぞれロボットハンド本体３００、ロボットアーム本体２００と送受信可能に接続されており、さらに、ロボットハンド制御装置５００とロボットアーム制御装置６００とで通信可能にしている。これにより上述したロボットハンド本体３００およびロボットアーム本体２００の各エンコーダ及び力センサ７００が検出した値をもとにフィードバック制御を行うことが可能となっている。
【００２１】
ワークＷ１は、ワークＷ２が組付く組付部（凸部）を有する。本実施形態では、凸部は円筒状のピンを想定する。また、ワークＷ２は、ワークＷ１が組付くための組付部（凹部）を有する。本実施形態では凹部は筒形の穴を想定する。
【００２２】
図２は、力センサ７００の概略を示した模式図である。力センサ７００は、力センサ７００を構成する構造部材に加わる３軸方向の外力Ｆｘ、Ｆｙ、Ｆｚを計測することができる（図２（ａ））。これにより力センサ７００は力検出手段として作用する。力を検出する方法としては、力センサ７００を構成する構造部材のひずみ、変化量、ある素子の特性変化を検出して、加わる力を検出する方法があげられるが本実施形態では力を検出する方法は問わない。
【００２３】
力センサを用いて嵌合作業を行う場合、上述した３軸方向の力Ｆｘ、Ｆｙ、Ｆｚ以外に、３軸方向のトルクＮｘ、Ｎｙ、Ｎｚも計測できた方が良い。（図２（ｂ））。本実施形態の力センサ７００では、３軸方向の力Ｆｘ、Ｆｙ、Ｆｚしか検出できないため、トルクＮｘ、Ｎｙ、Ｎｚを算出する。なお本実施形態では、力センサ７００をロボットアーム本体２００の先端に装着するが、ロボットハンド本体３００の指先に力センサ７００を装着しても良い。
【００２４】
図３は、ロボットアーム２００の制御概略を示したブロック図である。ロボットアーム制御装置６００は、ＣＰＵ６０１の内部に、指令値生成部６１０、位置姿勢計測部６１１、接触点推定部６１２、トルク値算出部６１３、ロボットアーム制御部６１４を備えている。ＲＯＭ６０２内部にはワークＷ１、Ｗ２の形状を記憶しておく形状記憶部６２１、ロボットアーム本体２００の制御プログラムや各種の動作パラメータ等を記憶する指令値記憶部６２２を備えている。
【００２５】
ロボットアーム制御装置６００から出力されたロボットアーム本体２００の制御値は、ロボットアームモータドライバ６０５を介し、ロボットアーム本体２００の各関節に設けられた不図示のモータに供給される。そしてロボットアーム本体２００が動作した際に、力センサ７００と各関節に設けられた不図示のエンコーダから検出された、力センサ値とエンコーダ値をロボットアーム制御装置６００に返す。これによりロボットアーム本体２００のフィードバック制御が可能となる。
【００２６】
図３より指令値生成部６１０は、ロボットハンド本体３００の移動先となるロボットアーム本体２００の先端の目標位置及び姿勢に対して、ロボットアーム本体２００の各関節の取るべき角度の目標値を生成し位置姿勢計測部６１１に供給する。この目標値は指令値記憶部６２２から読み出す。ロボットアーム本体２００の各関節の取るべき角度の目標値が、指令値生成部６１０からの指令値となる。
【００２７】
位置姿勢計測部６１１は、指令値生成部６１０から供給されたロボットアーム本体２００の指令値と、エンコーダから取得した各関節のエンコーダ値に基づいてロボットハンド本体３００の位置姿勢となるロボットアーム本体２００の先端の位置姿勢を計測する。そして計測したロボットアーム本体２００の先端の位置姿勢の値を接触点推定部６１２に供給する。なおロボットアーム本体２００の先端の位置姿勢の値とは、各リンクの幾何学的情報と、エンコーダ値から変換した各関節の角度を用いて、一般的な順運動学で計算される並進３自由度と回転姿勢３自由度の合計６自由度の幾何情報である。
【００２８】
接触点推定部６１２は、後述する処理手順に基づいて、ワークＷ１とＷ２が互いに接触している際に反力が生じている接触点の数と接触点の位置を推定する。その際、形状記憶部６２１からワークＷ１、Ｗ２の形状情報を読み出す。
【００２９】
形状記憶部６２１には、ワークＷ１とＷ２のＣＡＤデータなどから形状情報をあらかじめ記憶されており、ワークＷ１とＷ２の形状情報を接触点推定部６１２から読み出される。
【００３０】
また接触点推定部６１２は、力センサ７００からの力センサ値も入力されている。これによりロボットアーム本体２００の先端の位置姿勢の値と、ワークＷ１、Ｗ２形状情報と、力センサからの力センサ値Ｆｘ、Ｆｙ、Ｆｚを用いて反力が生じている接触点の数と接触点の位置を推定する。
【００３１】
トルク値算出部６１３は、後述する処理手順に基づいて、接触点推定部６１２からの接触点の数と接触点の位置と、力センサ７００からの力センサ値Ｆｘ、Ｆｙ、Ｆｚを用いて、トルクＮｘ、Ｎｙ、Ｎｚを算出する。そして力センサ値Ｆｘ、Ｆｙ、Ｆｚと算出したトルクＮｘ、Ｎｙ、Ｎｚを合わせた６自由度情報をロボットアーム制御部６１４に供給する。
【００３２】
ロボットアーム制御部６１４は、後述する処理手順に基づいて、接触点推定部６１２からの接触点の位置と、トルク値算出部６１３からの力センサ値Ｆｘ、Ｆｙ、Ｆｚとトルク値Ｎｘ、Ｎｙ、Ｎｚを用いて、ロボットアーム本体２００を制御する。
【００３３】
以上により本実施形態では、３自由度の力センサで、あたかも６自由度の力センサが搭載されているように、インピーダンス制御をロボットアーム本体２００に実行することができる。
【００３４】
次に本実施形態における嵌合処理方法について説明する。図４は本実施形態における嵌合処理のフローチャートである。図６は図４のフローチャートの各ステップにおける状態図である。以下で説明するフローはロボットアーム制御装置６００により実行される。前提条件として、嵌合処理のフローはロボットハンド本体３００がワークＷ１を把持した状態から開始するものとする。また、ワークＷ１、Ｗ２の形状情報をすでに形状記憶部６２２に記憶されているものとする。
【００３５】
図４より、まずＳ１１でワークＷ１をワークＷ２に嵌合する制御プログラムを指令値記憶部６２１から読出し、指令値生成部６１０でロボットアーム本体２００の指令値を作成する。
【００３６】
次にＳ１２でワークＷ１、Ｗ２の形状情報を形状記憶部６２１より読み出す。ワークＷ１の凸部とワークＷ２の凹部はお互いが組付く部位である。
【００３７】
ここで図５の左図はロボットハンド本体３００によりワークＷ１を把持した際の模式図である。ワークＷ１はロボットハンド本体３００により把持されるので、本実施形態では図５の左図のように力センサ７００の中心Ｓを基準点としロボットハンド本体３００とワークＷ１を一体として考えた、エンドエフェクタ３０１の形状情報が記憶されている。
【００３８】
また、ワークＷ２はワーク固定台Ｓ２上に固定されて載置されているので、力センサ７００の中心Ｓを基準として設定したワークＷ２の形状情報も記憶されている。
【００３９】
次にＳ１３でロボットアーム本体２００を用いてワークＷ１をワークＷ２に接近させる（図６（ａ））。図６ではロボットハンド本体３００とワークＷ１を一体として考えた、エンドエフェクタ３０１で表している。
【００４０】
そしてＳ１４で力センサ７００による力センサ値Ｆｘ、Ｆｙ、Ｆｚの計測を開始し、矢印Ｃ方向へエンドエフェクタ３０１をロボットアーム２００により動作させる。次にエンドエフェクタ３０１とワークＷ２とを接触点Ｑで接触させ、接触力ＦＱを検出する（図６（ｂ））。ここで、力センサ７００で検出される力をＦＳとし、各成分をＦＳｘ、ＦＳｙ、ＦＳｚとする。
【００４１】
そしてＳ１５で接触力ＦＱの値を含んだ、力センサ７００で検出される力ＦＳが所定の値であるか判定する。この所定の値はユーザーによりあらかじめ設定され、指令値記憶部６２２に格納されているものとする。Ｓ１５：ＮＯであればＳ１４の直前まで戻り、再度エンドエフェクタ３０１とワークＷ２の接触動作を繰り返す。Ｓ１５：ＹＥＳであればＳ１６に進む。
【００４２】
Ｓ１６では、接触点推定部６１２において、力センサ７００で検出した力ＦＳとエンドエフェクタ３０１の位置姿勢から接触点Ｑの数と位置を推定する。本実施形態ではエンドエフェクタ３０１の位置姿勢を、エンドエフェクタ３０１の形状情報と、ロボットアーム本体２００に設けられたエンコーダの値から位置姿勢計測部６１１で求める。接触力ＦＱの値は、ステップＳ１５で得た力センサ値ＦＳｘ、ＦＳｙ、ＦＳｚを用いる。このＳ１６は位置姿勢検出工程と形状情報読出工程とを兼ねる。
【００４３】
ここで図６（ｂ）における接触点Ｑの数と位置は以下の方法で推定することができる。図６（ｂ）の状態ではワークＷ１の凸部がワークＷ２の凹部に嵌合していないため、エンドエフェクタ３００には、垂直なワークＷ２からの接触力ＦＱが働き、力センサ７００で検出される力ＦＳの値はそのまま接触力ＦＱの値となる。
【００４４】
よって図６（ｂ）の状態における接触点Ｑに関して、力センサ７００にモーメントが働いていないため、接触点数が１点であることが推定できる。さらに、ワークＷ２の形状情報より、ワークＷ２の高さは決まっているため、接触点ＱのＺ座標の値が決定する。
【００４５】
また、エンドエフェクタ３０１の形状情報とロボットアーム本体２００のエンコーダの値により、最もワークＷ２に近いエンドエフェクタ３０１の表面上の点が特定できるので、接触点ＱのＸＹ座標の値が推定できる。
【００４６】
次にＳ１７に進み、エンドエフェクタ３０１を矢印Ｄ方向へ移動させる（図６（ｃ））。
【００４７】
図６（ｃ）の状態ではワークＷ１の凸部がワークのＷ２の凹部に嵌合してすぐの状態であり、ワークＷ１の凸部の先端がワークＷ２の凹部に嵌合しているが接触点は１つの状態である。
【００４８】
Ｓ１８で、図６（ｃ）での接触力ＦＱが、ワークＷ２の上面に対して垂直方向以外に働いていることを力センサ７００により検出し、ワークＷ１がワークＷ２に嵌合してすぐの状態かどうかを判断する。Ｓ１８：ＮＯならばＳ１７の直前まで戻り、再度エンドエフェクタ３０１の移動を繰り返す。Ｓ１８：ＹＥＳならばＳ１９へ進む。
【００４９】
Ｓ１９ではエンドエフェクタ３０１を矢印Ｅ方向へ所定の距離移動させる（図６（ｄ））。そしてＳ２０へ進む。
【００５０】
Ｓ２０ではエンドエフェクタ３０１が矢印Ｅ方向へ移動可能か判断する（図６（ｅ））。この判断はエンドエフェクタ３０１を矢印Ｅ方向へ所定の距離、移動させようとしても、所定の時間が経過してもエンドエフェクタ３０１を移動できなかったどうかで判断する。図６（ｅ）の状態では、エンドエフェクタ３０１を矢印Ｅ方向へさらに移動させることができなくなるため、Ｓ２１へ進む。
【００５１】
Ｓ２０：ＮＯならば、Ｓ１９直前まで戻り再度エンドエフェクタ３０１を矢印Ｅ方向へ移動させる。
【００５２】
次にＳ２１で、接触点推定部６１２において、力センサ７００で検出した接触力とエンドエフェクタ３０１の位置姿勢から接触点の数と位置を推定する。図６（ｅ）の状態では嵌合を進める方向である矢印Ｅ方向に、エンドエフェクタ３０１を並進移動させようとするとそれ以上移動できないため、接触点が２つであることがわかる。
【００５３】
本実施形態では接触点の数を、エンドエフェクタ３０１が移動可能であるかどうかで判断したが、直前の状態（図６（ｄ））から力センサ７００が検出する値の方向が変化したことで判断しても良い。
【００５４】
そしてワークＷ２の形状情報、エンドエフェクタ３０１の形状情報、矢印Ｅ方向に進んだロボットアーム本体２００のエンコーダの値により、図６（ｅ）の状態における接触点ＰのＸＹＺ座標の値が推定できる。
【００５５】
そしてＳ２２で、トルク値算出部６１３において、接触点の数と位置と力センサ７００から検出される値ＦＳｘ、ＦＳｙ、ＦＳｚからトルク値ＮＳｘ、ＮＳｙ、ＮＳｚを以下の方法で算出する。
【００５６】
ここで図６（ｂ）および図６（ｃ）における接触状態では、接触点は１つであるので、力センサ７００の中心Ｓから接触点Ｑの位置をｒＳＱとすると、トルクＮＳｘ、ＮＳｙ、ＮＳｚは、点Ｓにおける力とモーメントのつり合い式より次式で計算される。なお、Ｔは転置行列を表し、チルダ（〜）は歪対称行列を表す。
【００５７】
【数１】
2019155523.tif 000003
【００５８】
図６（ｅ）の状態では、接触点は２つ（点Ｐ、Ｑ）であるので、力センサ７００の中心Ｓから接触点Ｐ、Ｑそれぞれの位置をｒＳＰ、ｒＳＱとする。
【００５９】
図６（ｅ）の紙面向かって右側にＷ２の凹部を円筒で簡略化した図を示す。同図において、接触点Ｐ、Ｑを通る面を破線で示した面Ｉに付した座標系Ａを定義する。座標系Ａの原点はＷ２の凹部の中心で凹部の底面にあり、座標系Ａのｙ座標の正方向は凹部の底面に沿って点Ｑのある方向に取る。また、Ｗ２の凹部全体を示す座標系を座標系Ｏ、力センサ７００の中心となる点Ｓにおける座標系を座標系Ｓとする。
【００６０】
ここで、座標系Ｏを基準に考えた場合の点Ｐ、Ｑに働く接触力をそれぞれＦＯＰ、ＦＯＱ、力センサ７００で検出される値をＦＳ（ＦＳｘ，ＦＳｙ，ＦＳｚ）、算出するトルク値をＮＳ（ＮＳｘ，ＮＳｙ，ＮＳｚ）とする。力センサ７００の中心である点Ｓを基準とした力・トルクのつり合い式は次式で示される。ＣＯＳは座標系Ｓで表されたＦＳ、ＮＳを座標系Ｏに変換するための回転行列である。行列Ｏはゼロ行列である。
【００６１】
【数２】
2019155523.tif 000004
は座標系Ｏを基準としたときのｒＳＰ、
【００６２】
【数３】
2019155523.tif 000005
は座標系Ｏを基準としたときのｒＳＱである。
【００６３】
【数４】
2019155523.tif 000006
【００６４】
このとき接触力ＦＯＰ、ＦＯＱは、Ｗ２の凹部の壁に対して垂直抗力として働くので、座標系Ａを基準とし、各点Ｐ、ＱのＦＡＰ、ＦＡＱのｙ成分を抜き出して考えた方が良い。よって座標系Ａを基準として制約条件は次式で示される。ＦＡＰｙはＦＡＰのｙ成分、ＦＡＱｙはＦＡＱのｙ成分である。ＣＯＡは座標系Ｏで表されたＦＯＰ、ＦＯＱを座標系Ａに変換するための回転行列である。
【００６５】
【数５】
2019155523.tif 000007
はｙ成分を抜き出すための単位行列である。
【００６６】
【数６】
2019155523.tif 000008
【００６７】
そして接触力ＦＯＰ、ＦＯＱ、算出するトルク値ＮＳを、以下の線形計画問題として解くことで求める。線形計画問題は評価関数を最適化して解くため、評価関数をｆ（ｘ）＝ｃＴｘ＝ＦＡＰｙ＋ＦＡＱｙとし、接触点Ｐ、Ｑで発生する接触力の座標系Ａでのｙ成分が最小になることを最適化として、数式３より次式で定義する。ｃＴは定数、ｘＴを求めたい接触力ＦＯＰ、ＦＯＱ、算出するトルク値ＮＳとする。
【００６８】
【数７】
2019155523.tif 000009
【００６９】
そして数式４の制約条件として、不等式制約条件と等式制約条件とで分けて示す。不等式制約条件としては、数式３より次式で示される。
【００７０】
【数８】
2019155523.tif 000010
【００７１】
一方、等式制約条件として、数式２より、次式で示される。
【００７２】
【数９】
2019155523.tif 000011
【００７３】
以上数式４〜６を用いて行列Ａと行列Ｂが求まるので、行列ｘが求まり一般的な内点法などの線形計画問題を解くことにより、トルク値ＮＳを求めることができる。
【００７４】
そしてＳ２３で、検出された力、算出されたトルク値と接触点位置を用いてロボットアーム本体２００を制御する。
【００７５】
図６（ｅ）の状態では、算出されたトルク値Ｎがゼロに近づくようにロボットアーム本体２００の姿勢を制御しつつ、ワークＷ１をワークＷ２の凹部の底面へ移動させるように力センサ値Ｆに基づき倣い制御する。
【００７６】
並進移動できない場合は、トルクＮがゼロに近づけるようにロボットアーム本体２００を矢印Ｆ方向に移動させ、エンドエフェクタ３０１の姿勢を制御する（図６（ｆ））。
【００７７】
そしてＳ２４でエンドエフェクタ３０１が一定時間矢印方向Ｆに移動できなくなったか判断する。ワークＷ１をワークＷ２に嵌合させると、ワークＷ２の凹部に倣ってワークＷ１の位置と姿勢が変化し、それに伴いエンドエフェクタ３０１の位置と姿勢も変化する。
【００７８】
しかし、ワークＷ１とワークＷ２との間で噛みつきが生じると、図６（ｆ）の状態から矢印方向Ｆに移動ができなくなる。
【００７９】
そこでＳ２４で噛みつきの有無を判断する。Ｓ２４で一定時間エンドエフェクタ３０１を矢印方向Ｆに移動できない場合、Ｓ２４：ＹＥＳとなりＳ２５へ進む。
【００８０】
Ｓ２５では、ワーク同士の噛みつきを解消するため、エンドエフェクタ３０１を矢印Ｇ方向へ移動させ、ワークＷ２からワークＷ１を離間させる（図６（ｈ））。図６（ｈ）のように、ワークＷ１とワークＷ２の接触状態が変わるまでエンドエフェクタ３０１を動作させた方が噛みつきを解消するのに好適である。そしてＳ１９の直前まで戻り、再度エンドエフェクタ３０１を矢印Ｅ方向へ移動させ、ワーク同士を接触させトルク値を算出し、倣い制御を行わせる。
【００８１】
Ｓ２４：ＮＯであれば、Ｓ２６へ進みワークＷ１がワークＷ２に嵌合したかどうか判断する（図６（ｇ））。嵌合動作が完了した条件としては、嵌合動作の完了位置までエンドエフェクタ３０１の位置と姿勢が移動したことを条件とする。例えば、ワークＷ１の底面がワークＷ２の凹部の底面まで移動することである。
【００８２】
また上記に加えて、ワークＷ１の押付方向における力センサ７００から検出された値がある閾値を超えたことを同時に満たすことを動作完了の条件としても良い。
【００８３】
上述の動作完了の条件を満たした場合、嵌合処理を完了としフローを終了する。逆に動作完了の条件を満たさない場合、ステップＳ２１の直前に戻り、再度倣い制御を行わせ嵌合を行い、物品であるワークＷ１をワークＷ２に組付、組立を行う。
【００８４】
なお、本実施形態では接触点が２つとなる図６（ｅ）の状態からトルク値を算出し、倣い制御を行わせたが、これに限らない。例えば接触点が１つである図６（ｂ）または図６（ｃ）からトルク値を算出し、倣い制御を行わせても良い。
【００８５】
図６（ｂ）、図６（ｃ）の状態では、接触点は１つであるので、力センサ７００の値に基づき、ワークＷ２の凹部中心へ向かってロボットアーム本体２００を倣い制御する。なお、移動方向は、接触力と垂直な方向でワークＷ２の凹部の中心、つまり嵌合完了位置へ向かう方向とする。
【００８６】
また、ワークＷ１とワークＷ２が非接触状態の場合、設計値上の嵌合完了位置へ並進移動させ、ワーク間で接触力が発生するまでロボットアーム本体２００の制御を継続する。
【００８７】
以上本実施形態のロボットアーム制御装によれば、高価な６軸の力センサを用いることなく、３軸の力センサでトルク値が必要な倣い制御を接触点が複数生じる嵌合作業で行うことができる。これにより、高価な６軸力センサを用いるより安価な３軸力覚センサで嵌合作業を実現することができる。
【００８８】
（第２の実施形態）
上述の第１の実施形態では、接触点が１つないし２つである場合について説明した。しかしながら、本発明は、接触点が３つ以上である場合においても実施できる。
【００８９】
以下では、第１の実施形態とは異なるハードウェアや制御系の構成の部分について図示し説明する。また、第１の実施形態と同様の部分については上記と同様の構成ならびに作用が可能であるものとし、その詳細な説明は省略するものとする。
【００９０】
本実施形態では、図１に示す第１の実施形態と同様に、ロボットシステム１００を用い、ロボットアーム制御装置６００の代わりに後述するロボットアーム制御装置６００’を用いる。つまりロボットシステム１００は、ロボットアーム本体２００、ロボットハンド本体３００、ロボットアーム制御装置６００’、ロボットハンド制御装置５００を備える。
【００９１】
第１の実施形態と異なる部分は、ロボットアーム制御装置６００’と把持するワークＷ１’である。以下、ロボットアーム制御装置６００’とワークＷ１’について説明する。
【００９２】
ロボットアーム制御装置６００’は、第１の実施形態と同様に、ＣＰＵ、ＲＯＭ、ＲＡＭ、汎用信号インターフェイスなどから構成されている。しかし、ＲＯＭに記憶されている制御プログラムの内容が異なる。制御プログラムの詳細は、後述する図７に示すフローチャート図で説明する。
【００９３】
ワークＷ１’は、第１の実施形態と同様に、ワークＷ２が組付く凸部を有し、凸部は円筒上のピンを想定する。しかし、ワークＷ１よりも凸部の組付け方向の長さが短く、凸部の直径が小さいものとする。これは、ワークＷ１’のツバの部分がワークＷ１と比較して、ワークＷ２に干渉しやすいことを意図している。
【００９４】
そのため、ワークＷ１と比較して、ワーク同士が３点で接触しやすい状態となっている。第１の実施形態と同様にエンドエフェクタ３０１の形式で示すと、図７のモデル３０１’のようになる。
【００９５】
次に本実施形態における嵌合処理方法について説明する。前提条件として、ロボットハンド本体３００はワークＷ１’を把持した状態から開始するものとする。図８は本実施形態における嵌合処理のフローチャート図、図９は図８の各ステップにおける状態図である。
【００９６】
図８より、ステップＳ１１〜Ｓ１８は、第１の実施形態と同様であるので説明を省略する。図９（ａ）は図７、図８におけるステップＳ１８までを終えた際の状態図である。
【００９７】
Ｓ１９’ではエンドエフェクタ３０１’を矢印Ｅ方向へ移動させる（図９（ａ））。そしてＳ２０’へ進む。
【００９８】
Ｓ２０’ではエンドエフェクタ３０１が矢印Ｅ方向へ移動可能か判断する（図９（ｂ））。図９（ｂ）の状態では、エンドエフェクタ３０１’を矢印Ｅ方向へさらに移動させることができなくなるため、Ｓ２１へ進む。
【００９９】
Ｓ２０’：ＮＯならば、Ｓ１９直前まで戻り再度エンドエフェクタ３０１を矢印Ｅ方向へ移動させる。
【０１００】
そしてＳ２１〜Ｓ２３より、第１の実施形態と同様に、ワークの形状とエンドエフェクタ３０１’の位置姿勢から接触点の位置と数をもとめ、接触点の情報と力センサ７００からの値からトルク値を算出する。
【０１０１】
そしてＳ２４’で、一定時間エンドエフェクタ３０１’を矢印方向Ｆに移動できないか判断する（図９（ｃ））。図９（ｃ）の状態ではワークＷ１とワークＷ２は接触点が３つであり、ワークＷ１のツバがワークＷ２に干渉しているため、エンドエフェクタ３０１’の姿勢を変えようと矢印Ｆ方向へ動かそうとしても動かすことができない。そのためＳ２５’へ進む。
【０１０２】
Ｓ２５’では矢印Ｇ方向にエンドエフェクタ３０１’を動かし、接触点がＱのみになる位置まで移動させる（図９（ｄ））。上記の移動において、ロボットアーム本体２００に対して、動作させた軌道を逆再生しても良いし、力制御を行いながら接触点がＱのみになった際に力センサ７００から検出される値を基に動作させても良い。
【０１０３】
そしてＳ２６’で、Ｓ２１〜Ｓ２３で算出したトルク値を弱める方向にエンドエフェクタ３０１’の姿勢を変更する（図９（ｅ））。エンドエフェクタ３０１’の姿勢の変化量は、ユーザが設定しても良いし、ワークまたはロボットアーム本体２００の剛性係数を元に自動で計算するようにしても良い。エンドエフェクタ３０１’の姿勢変更が完了したら、ステップＳ１９’の直前に戻り、再度エンドエフェクタ３０１’をＥ方向に移動させ嵌合処理を行う。
【０１０４】
Ｓ２４’：ＮＯであれば第１の実施形態と同様に、嵌合動作の完了条件を判断し、嵌合処理を終了する。
【０１０５】
以上本実施形態のロボットアーム制御装置６００’によれば、接触点が３点以上であっても、高価な６軸の力センサを用いることなく、３軸の力センサでトルク値が必要な倣い制御を接触点が複数生じる嵌合作業で行うことができる。
【０１０６】
特に嵌合部の径が狭いといった嵌合精度の高いワークである場合、ワーク同士が多数の点で接触してしまい、次の動作に進めなく場合があるので、本実施形態は非常に有用である。
【０１０７】
また第１および第２の実施形態では、エンドエフェクタ３０１の形状情報としてロボットハンド本体３００とワークＷ１を一体として考えたが、ロボットハンド本体３００のみとしてもよい。その際、ロボットハンド本体３００を構成する指や掌部の形状情報をエンドエフェクタ３０１として記憶し、把持するワークの形状情報を記憶することで第１および第２の実施形態の効果を得ることができる。
【０１０８】
以上述べた第１の実施形態、第２の実施形態の処理手順は具体的にはロボットアーム制御装置６００、６００’により実行されるものである。従って上述した機能を実現するソフトウェアのプログラムを記録した記録媒体をロボットアーム制御装置６００、６００’に供給し、ＣＰＵ（不図示）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるよう構成することができる。この場合、記録媒体から読み出されたプログラム自体が上述した各実施形態の機能を実現することになり、プログラム自体およびそのプログラムを記録した記録媒体は本発明を構成することになる。
【０１０９】
また各実施形態では、コンピュータで読み取り可能な記録媒体がＲＯＭ或いはＲＡＭであり、ＲＯＭ或いはＲＡＭにプログラムが格納される場合について説明したが、本発明はこのような形態に限定されるものではない。本発明を実施するためのプログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、ＨＤＤ、外部記憶装置、記録ディスク等を用いてもよい。
【符号の説明】
【０１１０】
１００  ロボットシステム
２００  ロボットアーム本体
３００  ロボットハンド本体
４００  ロボットシステム制御装置
５００  ロボットハンド制御装置
６００、６００’  ロボットアーム制御装置
６０１  ＣＰＵ
６０２  ＲＯＭ
６１０  指令値生成部
６１１  位置姿勢計測部
６１２  接触点推定部
６１３  トルク値算出部
６１４  ロボットアーム制御部
６２１  形状記憶部
７００  力センサ
８００  外部入力装置
Ｗ１、Ｗ１’、Ｗ２  ワーク
【図１】
2019155523.tif 000012
【図２】
2019155523.tif 000013
【図３】
2019155523.tif 000014
【図４】
2019155523.tif 000015
【図５】
2019155523.tif 000016
【図６】
2019155523.tif 000017
【図７】
2019155523.tif 000018
【図８】
2019155523.tif 000019
【図９】
2019155523.tif 000020