JP2017100202A - ロボットシステム、制御装置、制御方法、及びプログラム - Google Patents

Abstract

【課題】スキャナ座標系からロボット座標系への座標変換の推定を高精度に実行できるロボットシステム、制御装置、制御方法、及びプログラムを提供する。【解決手段】ロボットシステム１は、スキャナ２、ロボット３、制御装置５を備え、エンドエフェクタ４０に固定した球体７０を用いることで、スキャナ座標系からロボット座標系へのキャリブレーションを高精度且つ自動で行う。制御装置５は、基準位置算出処理、回転成分算出処理、並進成分算出処理を実行する。各算出処理において、制御装置５は、ハンド部４２を様々な位置姿勢に移動させたときの球体７０の点群データをスキャナ２から取得し、球体７０の位置を推定する。制御装置５は、各算出処理で推定された球体７０の位置データに基づき、キャリブレーションの回転成分ＲＶと並進成分ＳＶを推定する。【選択図】図２

Description

本発明は、ロボットシステム、制御装置、制御方法、及びプログラムに関する。

従来、ターゲットをピッキング可能なハンド部を有する垂直多関節型のロボットと、ターゲットのスキャニングを実行可能なスキャナと、ロボット及びスキャナを駆動し制御する制御装置とを備えたロボットシステムが知られている。制御装置は、スキャナでスキャニングされたターゲットのスキャニングデータに基づき、ターゲットの位置姿勢を特定し、該位置姿勢を特定したターゲットの把持を指示する制御指令をロボットに出力する。

このようなロボットシステムでは、使用前に、スキャナの座標系であるスキャナ座標系からロボットの座標系であるロボット座標系への座標変換の推定（キャリブレーション）が必要である。例えば、マーカが印刷されたチェッカーボードをロボットに持たせ、三次元スキャナとして機能するステレオカメラから出力されるチェッカーボードの可視画像データに基づき、キャリブレーションを行うロボットビジョンシステム及び自動キャリブレーション方法が提案されている（例えば、特許文献１参照）。

特開２０１０−１７２９８６号公報

チェッカーボードを使ったキャリブレーション方法では、スキャナが可視画像データを出力できることが必須であるが、一般的なスキャナの中には、可視画像をそもそも撮影できないものや、撮影できても写りが悪くキャリブレーションには使えないスキャナ等も存在するため、これらのスキャナは使用できないという問題点があった。また、レーザ光の反射強度を可視画像として出力するスキャナを用いた場合、可視画像は、解像度が低いので、チェッカーボードのマーカ位置検出に十分な精度が得られにくいという問題点もあった。また、反射強度を使うので、チェッカーボードの背景とマーカの色が識別しにくいという問題点もあった。

本発明の目的は、スキャナ座標系からロボット座標系への座標変換の推定を高精度に実行できるロボットシステム、制御装置、制御方法、及びプログラムを提供することである。

本発明の第１態様に係るロボットシステムは、ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナと前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力する制御手段とを備えたロボットシステムであって、前記制御手段は、前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定手段を備え、前記推定手段は、前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定手段と、前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定手段と、前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定手段とを備えたことを特徴とする。

第１態様では、スキャナ座標系からロボット座標系へのキャリブレーションにおいて球体を用いる。そして、球体の表面をスキャナで三次元スキャニングさせ、その球体の点群データを用いることで、球体の位置を容易に推定できる。ロボットシステムは、そのような球体の点群データである第一、第二、第三スキャニングデータを用いることで、座標変換に用いる変換用回転成分と変換用並進成分を高精度に推定できる。

第１態様は、前記基準位置推定手段、前記回転成分推定手段、及び前記並進成分推定手段は、前記球体の前記点群データに基づき、前記球体の中心位置を推定する中心位置推定手段を備え、当該中心位置推定手段で推定された前記中心位置を、前記球体の位置として特定してもよい。それ故、球体の位置を容易且つ高精度に推定できる。

第１態様の前記球体は、少なくとも表面が拡散反射するものであってもよい。球体の表面は拡散反射するので、スキャナから照射される光（例えばレーザ光）を球体の表面に反射させ、その反射光を受光できる。このことから、球体の表面を正確にスキャニングできる。よって、ロボットシステムは、球体の位置を高精度に認識できる。

第１態様の前記ロボットは、回転と並進が可能な多関節を有するアーム部を備え、前記エンドエフェクタは、前記アーム部の先端部に設けられた取付部に着脱可能に取り付けられる被取付部と、前記ハンド部とは異なる位置に、前記球体を着脱可能に固定する球体固定部とを備えてもよい。球体は、ハンド部に保持させることもできるが、エンドエフェクタの球体固定部に固定することもできる。例えば、ハンド部が吸着方式でターゲットを保持するものがある。このタイプのハンド部では、球体に吸着させる部分が樹脂で形成されている場合があり、例えば球体を水平方向に保持した状態では、球体に吸着させる部分が球体の重さで下がる可能性がある。その場合、球体の正確な位置が推定できないので、球体固定部に固定することで、球体はエンドエフェクタと一体して移動するので、球体の位置がずれることがない。よって、球体の正確な位置が推定できるので、キャリブレーションを良好に行うことができる。

本発明の第２態様に係る制御装置は、ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナとを備えたロボットシステムの制御を行い、前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力することが可能な制御装置であって、前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定手段を備え、前記推定手段は、前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定手段と、前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定手段と、前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定手段とを備えたことを特徴とする。

第２態様では、スキャナ座標系からロボット座標系へのキャリブレーションにおいて球体を用いる。そして、球体の表面をスキャナで三次元スキャニングさせ、その球体の点群データを用いることで、球体の位置を容易に推定できる。ロボットシステムの制御装置は、そのような球体の点群データである第一、第二、第三スキャニングデータを用いることで、座標変換に用いる変換用回転成分と変換用並進成分を高精度に推定できる。

本発明の第３態様に係る制御方法は、ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナとを備えたロボットシステムの制御を行い、前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力可能な制御装置の制御方法であって、前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定工程を備え、前記推定工程は、前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定手段と、前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定工程と、前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定工程とを備えたことを特徴とする。第３態様では、制御装置が上記工程を行うことで、第２態様に記載の効果を得ることができる。

本発明の第４態様に係るプログラムは、ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナとを備えたロボットシステムの制御を行い、前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力可能な制御装置のコンピュータに実行させるプログラムであって、前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定ステップを備え、前記推定ステップは、前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定ステップと、前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定ステップと、前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定ステップとを備えたことを特徴とする。第４態様では、制御装置のコンピュータが上記プログラムを実行することで、第２態様に記載の効果を得ることができる。

ロボットシステム１の構成図（ピッキング動作時）である。 ロボットシステム１の構成図（キャリブレーション時）である。 キャリブレーション処理のフローチャートである。 基準位置算出処理のフローチャートである。 回転成分算出処理のフローチャートである。 並進成分算出処理のフローチャートである。 球体認識処理のフローチャートである。 球体の点群データの図である。 回転成分算出用のハンド部４２の位置姿勢のイメージ図である。 移動後の球体７０の位置の予測誤差と並進成分の誤差を示す図である。

以下、図面を参照し、本発明の実施形態を説明する。なお、参照する図面は本発明が採用し得る技術的特徴を説明する為に用いられるものである。図面に記載した各処理工程、及び装置構成等はそれのみに限定する趣旨ではなく単なる説明例である。

図１を参照し、ロボットシステム１の構成を説明する。ロボットシステム１は、スキャナ２、ロボット３、制御装置５を備える。ロボットシステム１は、ターゲット１０の位置と形状をスキャナ２で特定し、該ターゲット１０をロボット３によってピッキングして搬送するものである。

スキャナ２について説明する。スキャナ２は、ターゲット１０の三次元スキャニングが可能な周知の三次元スキャナ（３Ｄスキャナ）である。スキャナ２は、レーザ光をターゲット１０へ照射することで、ターゲット１０を撮影し、３次元のスキャニングデータに変換する。測定原理は例えば三角測量に基づいたものである。スリット状の光を複数パターン照射し、スリット光が物体に移った瞬間をカメラ（図示略）で撮影する。カメラとスリット光源部の距離、撮影した複数枚の画像を解析した結果から対象物との距離を算出し、点群データに変換する。これにより、物体の絶対的な大きさや形状を認識できる。なお、本実施形態では、一台のスキャナ２を用いているが、撮像する物体の形、数、大きさ等に合わせて、複数台のスキャナ２を組み合わせることもできる。

ロボット３について説明する。ロボット３は、周知である垂直多関節型の６軸ロボットである。ロボット３は、基部３１、アーム部３２、エンドエフェクタ４０等を備える。基部３１は、ロボット３の台座であり、内部にアーム部３２の駆動制御を行う為の駆動制御機構（図示略）を備える。基部３１の上面には、回転部Ｒ１が設けられている。回転部Ｒ１の軸心は、上下方向に延びる。アーム部３２は、基部３１の上面に支持されている。アーム部３２は、長尺状の第一アーム部３３と第二アーム部３４を備える。第一アーム部３３の一端部は、基部３１の上面に設けられた回転部Ｒ１上において、揺動部Ｑ１を介して揺動可能に支持されている。揺動部Ｑ１の軸心は、略水平方向に延びる。

第二アーム部３４の一端部は、上方に延びる第一アーム部３３の他端部に対して揺動部Ｑ２を介して揺動可能に連結されている。第二アーム部３４の長手方向中央部よりも先端側には、回転部Ｒ２が設けられている。回転部Ｒ２の軸心は、第二アーム部２４の軸線方向に沿って延びる。それ故、第二アーム部３４の回転部Ｒ２よりも先端側の部分は、回転部Ｒ２よりも後端側の部分に対し、回転部Ｒ２の軸心を中心に回転可能である。アーム部３２（第二アーム部３４）の先端部には、揺動部Ｑ３が設けられている。揺動部Ｑ３の軸心は、第二アーム部３４の軸線方向に直交する方向に延びる。揺動部Ｑ３には、回転部Ｒ３が設けられている。回転部Ｒ３の軸心は、揺動部Ｑ３の軸心に対して直交する。それ故、回転部Ｒ３は、第二アーム部３４に対し、揺動部Ｑ３の軸心を中心に揺動可能である。回転部Ｒ３には、フランジ部３４１が固定されている。それ故、フランジ部３４１は、回転部Ｒ３を中心に回転可能である。フランジ部３４１には、エンドエフェクタ４０が着脱可能に連結される。

エンドエフェクタ４０は、本体部４１とハンド部４２を備える。ハンド部４２は、本体部４１の先端側に固定されている。ハンド部４２は、例えば二つの爪部４２１を備え、それらを互いに接触又は離間する方向に移動させることで、ターゲット１０を把持したり、離したりすることができる。なお、ハンド部４２に設けられる爪部４２１の数、形状、動作等は、これに限らず、様々な変更が可能である。また、ハンド部４２によるターゲット１０の把持方式も、ターゲット１０を爪部４２１等で挟み込む方式に限らず、例えば、吸盤等を用いた吸着方式も適用可能である。

上記構成を備えるエンドエフェクタ４０の本体部４１の側面の先端側には、球体固定軸４５が設けられている。球体固定軸４５には、後述するキャリブレーション処理実行時に用いられる球体７０が着脱可能に取り付けられる。キャリブレーション処理では、ターゲット１０の代わりに、球体７０の位置を認識することで、スキャナ座標系からロボット座標系への変換の推定（キャリブレーション）が行われる。球体固定軸４５は、例えば、ネジ山が周面に形成された軸状に形成され、本体部４１の側面に対して垂直方向に外側に突出している。球体固定軸４５に対して、例えば、球体７０に形成された螺子孔（図示略）を螺合させることで、球体固定軸４５に対して球体７０が取り付けられる。球体固定軸４５の位置は、本体部４１であれば何処でもよいが、ハンド部４２に近い方がより好ましい。

制御装置５について説明する。制御装置５は、ＣＰＵ５１、ＨＤＤ５２、ＲＡＭ５３等を備える。ＨＤＤ５２には、後述するキャリブレーション処理（図３参照）を実行する為のキャリブレーションプログラム、後述する基準位置算出処理（図４参照）を実行する為の基準位置算出プログラム、後述する回転成分算出処理（図５参照）を実行する為の回転成分算出プログラム、後述する並進成分算出処理（図６参照）を実行する為の並進成分算出プログラム、後述する球体認識処理（図７参照）を実行する為の球体認識プログラム等、各種プログラムが記憶されている。制御装置５は、スキャナ２及びロボット３と有線又は無線により通信可能に接続されている。制御装置５は、例えば周知のＰＣ（パーソナルコンピュータ）を用いることができるが、専用機であってもよい。

図１を参照し、ロボットシステム１によるターゲット１０のピッキング動作の一例を簡単に説明する。スキャナ２は、机上に載置されたターゲット１０を撮像し、三次元スキャニングを実行する。スキャナ２は、三次元スキャニングによって得られた点群データをスキャニングデータとして、制御装置５に出力する。制御装置５のＣＰＵ５１は、スキャナ２から受信したスキャニングデータに基づき、ターゲット１０の位置姿勢認識を行う。基準位置は、例えばターゲット１０の重心付近とする。位置姿勢認識によって得られた位置姿勢情報は三次元ユークリッド変換行列で管理し、例えばＲＡＭ５３に記憶する。次いで、ターゲット１０を把持位置姿勢を決定する。ターゲット１０の把持する位置は、例えば基準位置よりＺ方向に少し上とする。把持する姿勢であるＺ軸回りの回転角ＲＺは、ターゲット１０の位置（ｘ，ｙ）を用いてＲＺ＝ｔａｎ^−１（ｙ／ｘ）とすればよい。ＣＰＵ５１は、ターゲット１０の位置姿勢と、決定した把持位置と姿勢に基づき、ロボット３に対し、ターゲット１０の位置までハンド部４２を移動させ、ターゲット１０を把持させる為の制御指令を出力する。ロボット３は、ＣＰＵ５１からの制御指令に基づき、ハンド部４２をターゲット１０の把持位置まで移動させた後、ターゲット１０を把持し、所定位置まで搬送して載置する。なお、ロボット３のハンド部４２は、Ｚ軸方向をアプローチ方向とし、リフトアップ方向もＺ軸方向とすればよい。

次に、ロボットシステム１におけるスキャナ２とロボット３の自動キャリブレーション方法について説明する。
−数学的な準備−
（三次元直交）座標系Ａから座標系Ｂへの座標変換をＴ_Ａ→Ｂと表す。この座標変換は、回転・スケール成分Ｒ∈ＧＬ_３および並進成分Ｓ∈Ｒ^３の組み合わせからなり、（Ｒ；Ｓ）のように表す。

ここで、ＧＬ_３は三次実正則行列の全体、Ｒ^３は三次元ユークリッド空間である。ロボット座標系をＷ、スキャナ座標系をＶとする。ハンド部４２の位置姿勢は、ロボット座標原点からの変換とみなすことで（Ｒ；Ｓ）と表せる。そこで、これらの数学的な準備を踏まえ、スキャナ座標系Ｖからロボット座標系Ｗへの座標変換Ｔ_Ｖ→Ｗ＝（Ｒ_Ｖ；Ｓ_Ｖ）を自動的に推定する為のキャリブレーション処理を説明する。

図２，図３を参照し、キャリブレーション処理を説明する。ユーザが制御装置５の入力部（図示略）において、キャリブレーション開始の指示を入力すると、制御装置５のＣＰＵ５１は、ＨＤＤ５２に記憶されたキャリブレーションプログラムを読み込み、図３に示すキャリブレーション処理を実行する。

−球体７０の取り付け−
先ず、キャリブレーション処理を開始する前に、作業者は、図２に示すように、エンドエフェクタ４０の球体固定軸４５に球体７０を手作業で取り付ける。球体７０の材料は、特に限定しないが、三次元スキャナに適した拡散反射する素材であるのが好ましい。なお、球体７０の少なくとも表面が拡散反射すればよいので、例えば、表面だけを拡散反射する素材で形成してもよく、表面を研磨等で加工することによって拡散反射させてもよい。本実施形態の球体７０の重量は、ピッキングの対象であるターゲット１０と略同一にするのが好ましく、例えば１００ｇである。球体７０のサイズは、ピッキングの対象であるターゲット１０と略同一サイズにするのが好ましい。また、本実施形態では、球体固定軸４５に球体７０を取り付けているが、ハンド部４２に球体７０を把持させてもよい。このとき、ハンド部４２の位置と、球体７０の中心位置とが一致していなくてもよい。但し、ロボット３が動作しても球体７０がずれ落ちてしまわないように、ハンド部４２に確実に固定されていることが重要である。それ故、本実施形態では、球体７０を球体固定軸４５に取り付けることで、ハンド部４２と一体して動作するエンドエフェクタ４０に確実に固定できる。球体７０の取付作業が完了した場合、ユーザは、制御装置５の入力部（図示略）において、球体７０の取付作業の完了操作を行う。

図３に示すように、ＣＰＵ５１は、球体７０の取付作業が完了したか否か判断する（Ｓ１）。完了操作が行われるまで（Ｓ１：ＮＯ）、ＣＰＵ５１は待機する。完了操作が行われた場合（Ｓ１：ＹＥＳ）、ＣＰＵ５１は、ＨＤＤ５２からサブルーチンである基準位置算出プログラムを読み出し、基準位置算出処理を実行する（Ｓ２）

−基準位置算出処理−
ＣＰＵ５１は、ループ変数ｋを０に初期化する（Ｓ１０）。ｋは、後述する球体認識処理を実行した回数に相当し、ＲＡＭ５３に記憶される。ＣＰＵ５１は、ｋに１加算し（Ｓ１１）、ＨＤＤ５２から球体位置認識プログラムを読み出し、球体認識処理を実行する（Ｓ１２）。

−球体認識処理−
球体認識処理は、基準位置算出処理、回転成分算出処理、及び並進成分算出処理の何れにおいても実行される処理であるので、最初に、球体認識処理の一般的な処理内容を説明する。球体認識処理はハンド部４２の位置姿勢（Ｒ；Ｓ）を引数とする。図７に示すように、ＣＰＵ５１はロボット３に対し、ハンド部４２の（Ｒ；Ｓ）への移動を開始する（Ｓ１０１）。ＣＰＵ５１は移動が完了したか否か判断する（Ｓ１０２）。移動が完了するまで（Ｓ１０２：ＮＯ）、ＣＰＵ５１は移動を継続する。移動が完了すると（Ｓ１０２：ＹＥＳ）、ＣＰＵ５１はスキャナ２に対してスキャン実行命令を送信し、スキャニングデータを取得する（Ｓ１０３）。スキャニングデータは、図８に示す点群データである。最後に、ＣＰＵ５１は取得した点群データから球体７０の中心位置ｑ∈Ｖを推定し（Ｓ１０４）、その推定した中心位置ｑを戻り値として、例えばＲＡＭ５３に記憶する。なお、球体７０の位置推定は、既存の手法を用いることができ、例えば、ＲＡＮＳＡＣ（RANdom SAmple Consensus）法を用いることができる。ＲＡＮＳＡＣ法は、ノイズが載ったデータに対しても高精度に中心位置ｑを推定できる。ＣＰＵ５１は球体認識処理を終了する。

図４に戻り、基準位置算出処理における球体認識処理（Ｓ１２）では、ＣＰＵ５１は、キャリブレーションで基準となる球体７０の位置を算出する。なお、球体認識処理を開始する前に、ユーザは、１個のハンド基準位置姿勢（Ｒ_Ａ；Ｓ_Ａ）を手作業で決め、ＨＤＤ５２に記憶する。この位置姿勢は、ピッキング対象エリア（スキャナ２のスキャン対象範囲と同義）の中央付近とする。ＣＰＵ５１は、球体７０の位置ｑ_ＡＫの推定が完了すると、ループ変数ｋはＬ以上か否か判断する（Ｓ１３）。Ｌは１以上とする。ｋがＬ未満の場合（Ｓ１３：ＮＯ）、ＣＰＵ５１はＳ１１に戻り、ｋに１加算して、球体認識処理を再度実行する（Ｓ１２）。それ故、ＣＰＵ５１は、球体認識処理をＬ回繰り返して実行する。球体位置を繰り返し認識することで、基準位置のバラつきを低減できる。

ＣＰＵ５１は、ループ変数ｋ∈｛１，・・・，Ｌ｝に対し、引数にＨＤＤ５２から読み出した（Ｒ_Ａ；Ｓ_Ａ）を与え、戻り値の球体位置をｑ_Ａｋ∈Ｖにセットする。つまり、ループ２回目以降は、ロボット３は動作しない。ｋがＬに達した場合（Ｓ１３：ＹＥＳ）、ＣＰＵ５１は、以下の［数２］に示すように、得られた球体位置のＬ個のデータを平均化することによって、基準位置ｑ_Ａを推定する（Ｓ１４）。

ＣＰＵ５１は、推定した基準位置ｑ_Ａを戻り値として、例えばＲＡＭ５３に記憶し、図３のキャリブレーション処理に戻ってＳ３に処理を進める。ＣＰＵ５１は、ＨＤＤ５２からサブルーチンである回転成分算出プログラムを読み出し、回転成分算出処理を実行する（Ｓ３）。

−回転成分算出処理−
ＣＰＵ５１は、ループ変数ｋを０に初期化する（Ｓ２０）。ｋは、球体認識処理を実行した回数に相当し、ＲＡＭ５３に記憶される。ＣＰＵ５１は、ｋに１加算し（Ｓ２１）、ＨＤＤ５２から球体位置認識プログラムを読み出し、球体認識処理を実行する（Ｓ２２）。

回転成分算出処理における球体認識処理（Ｓ２２）では、スキャナ座標系からロボット座標系への変換の回転成分Ｒ_Ｖを算出する。球体認識処理を開始する前に、ユーザは、Ｍ個の回転成分算出用のハンド部４２の位置姿勢（Ｒ_Ａ；Ｓ_Ｂ１），・・・，（Ｒ_Ａ；Ｓ_ＢＭ）を手作業で決め、ＨＤＤ５２に記憶する。なお、以下説明では、ハンド部４２の位置姿勢のことを「ハンド位置姿勢」と呼ぶ。Ｍは４以上とする。例えば、図９に示すように、これらの位置姿勢の回転成分は基準姿勢と同じ（Ｒ_Ａ）で、並進成分だけが変化している（Ｓ_Ｂ１，Ｓ_Ｂ２，・・・）点が重要である。これらのハンド位置姿勢は、球体７０がピッキング対象領域内に存在していること、尚且つ位置が同一直線あるいは同一平面上に分布しないようにすることを条件とする。

ＣＰＵ５１は、上記の球体認識処理を実行し（Ｓ２２）、球体７０の位置ｑ_ＢＫを推定する。ＣＰＵ５１は、ＲＡＭ５３に記憶するｋがＭ以上か否か判断する（Ｓ２３）。ｋがＭ未満の場合（Ｓ２３：ＮＯ）、ＣＰＵ５１はＳ２１に戻り、ｋに１加算して、球体認識処理を再度実行する（Ｓ２２）。それ故、ＣＰＵ５１は、球体認識処理をＭ回繰り返して実行する。

ＣＰＵ５１は、ループ変数ｋ∈｛１，・・・，Ｍ｝に対し、引数にＨＤＤ５２から読み出した（Ｒ_Ａ；Ｓ_Ｂｋ）を与え、戻り値の球体７０の位置（以下「球体位置」と呼ぶ）をｑ_Ｂｋ∈Ｖにセットする。ｋがＭに達した場合（Ｓ２３：ＹＥＳ）、ＣＰＵ５１は、回転成分Ｒ_Ｖを推定する（Ｓ２４）。これは、球体位置ｑ_Ｂ１，・・・，ｑ_ＢＭ∈Ｖからハンド部４２の位置（以下「ハンド位置」と呼ぶ）Ｓ_Ｂ１，・・・，Ｓ_ＢＭ∈Ｗへの対応付けを基に、以下の［数３］で表した仮の座標変換を求め、更に以下の［数４］のようにおけばよい。

なぜなら、回転成分算出用のハンド位置姿勢は基準位置姿勢と同じなので、ハンド部４２と球体７０の位置関係が常に保たれるからである。[数３]で示す仮の座標変換は、次の[数５]で示す誤差関数Ｅを最小化する変数により与えられる。

ＣＰＵ５１は、その推定した回転成分Ｒ_Ｖを戻り値として、例えばＲＡＭ５３に記憶し、図３のキャリブレーション処理に戻ってＳ４に処理を進める。ＣＰＵ５１は、ＨＤＤ５２からサブルーチンである並進成分算出プログラムを読み出し、並進成分算出処理を実行する（Ｓ４）。

−並進成分算出処理−
ＣＰＵ５１は、ループ変数ｋを０に初期化する（Ｓ３０）。ｋは、球体認識処理を実行した回数に相当し、ＲＡＭ５３に記憶される。ＣＰＵ５１は、ｋに１加算し（Ｓ３１）、ＨＤＤ５２から球体位置認識プログラムを読み出し、球体認識処理を実行する（Ｓ３２）。

並進成分算出処理における球体認識処理（Ｓ３２）では、スキャナ座標系からロボット座標系への変換の並進成分Ｓ_Ｖを算出する。基本的な処理内容を説明する。もし球体位置とハンド位置が一致していれば、座標変換の並進成分Ｓ_Ｖは、上述の回転成分算出処理で求めた仮の並進成分と同じ値になる。しかし、球体７０は手作業で取り付けるため、以下の［数６］に示す並進成分の誤差ΔＳは、ゼロとはならない。

そこで、スキャナ座標系からロボット座標系への変換の仮の回転成分と並進成分を設定し、ハンド部４２を動作させる。球体位置とハンド位置とが異なっていれば、ハンド部４２の移動によって、球体７０の予測される位置と現実の位置とに違いが出るはずである。この誤差からΔＳを推定する（図１０参照）。

先ず、ハンド部４２の移動に伴う球体７０の予測位置について説明する。基準位置姿勢（Ｒ_Ａ；Ｓ_Ａ）において、球体位置はｑ_Ａ∈Ｖであるとする。即ち、ロボット座標系では、以下の［数７］のようになる。

例えば、ハンド部４２を基準位置姿勢から（Ｒ'；Ｓ'）だけ相対移動したと仮定する。このとき、球体位置は、以下の［数８］に示す位置となる。

よって、スキャナ座標系における移動後の球体位置は、以下の［数９］に示す位置となる。

更に、［数３］の仮の座標変換を使えば、以下に示す［数１０］と書き換えられる。

以上の準備を元にして、並進成分算出処理を説明する。球体認識処理を開始する前に、ユーザは、Ｎ個の回転成分算出用のハンド位置姿勢（Ｒ_Ｃ１；Ｓ_Ｃ１），・・・，（Ｒ_ＣＮ；Ｓ_ＣＮ）を手作業で決め、ＨＤＤ５２に記憶する。Ｎは１以上とする。これらの位置姿勢は、球体７０がピッキング対象領域内に存在していること、異なる軸回転の姿勢が多いことを条件とする。ＣＰＵ５１は、上記の球体認識処理を実行し（Ｓ３２）、球体７０の位置ｑ_ＣＫを推定する。ＣＰＵ５１は、ＲＡＭ５３に記憶するｋがＮ以上か否か判断する（Ｓ３３）。ｋがＮ未満の場合（Ｓ３３：ＮＯ）、ＣＰＵ５１はＳ３１に戻り、ｋに１加算して、球体認識処理を再度実行する（Ｓ３２）。それ故、ＣＰＵ５１は、球体認識処理をＮ回繰り返して実行する。

ＣＰＵ５１は、ループ変数ｋ∈｛１，・・・，Ｎ｝に対し、引数にＨＤＤ５２から読み出した（Ｒ_Ｃｋ；Ｓ_Ｃｋ）を与え、戻り値の球体位置をｑ_Ｃｋ∈Ｖにセットする。ｋがＮに達した場合（Ｓ３３：ＹＥＳ）、ＣＰＵ５１は、並進成分Ｓ_Ｖ、つまりは、誤差ΔＳを推定する（Ｓ３４）。ΔＳは、以下の［数１１］に示す誤差関数Ｆを最小化する変数で与えられる。

但し、以下の［数１２］に示すように、

とし、（Ｒ'_Ｃｋ；Ｓ'_Ｃｋ）は、基準位置姿勢（Ｒ_Ａ；Ｓ_Ａ）から（Ｒ_Ｃｋ；Ｓ_Ｃｋ）への相対移動とする。最後に、以下の［数１３］に示すように、

とすることによって、ＣＰＵ５１は、スキャナ座標系からロボット座標系への変換（Ｒ_Ｖ；Ｓ_Ｖ）を得ることができる。ＣＰＵ５１は、その推定した回転成分Ｒ_Ｖを戻り値として、例えばＲＡＭ５３に記憶する。このようにして、スキャナ座標系からロボット座標系への変換（Ｒ_Ｖ；Ｓ_Ｖ）がＲＡＭ５３に記憶される。

ＣＰＵ５１は、図３のキャリブレーション処理のＳ５に処理を進め、球体７０の取外し作業が完了したか否か判断する。ユーザは、エンドエフェクタ４０の球体固定軸４５から、球体７０を手作業で取り外す。球体７０の取外し作業が完了した場合、ユーザは、制御装置５の入力部（図示略）において、球体７０の取外し作業の完了操作を行う。完了操作が行われるまでは（Ｓ５：ＮＯ）、ＣＰＵ５１は待機する。完了操作が行われた場合（Ｓ５：ＹＥＳ）、ＣＰＵ５１は、キャリブレーション処理を終了する。このようにして、ロボットシステム１は、球体７０を用いることで、スキャナ座標系からロボット座標系へのキャリブレーションを自動で且つ高精度に行うことができる。

以上説明において、図３に示すキャリブレーション処理を実行するＣＰＵ５１が、本発明の「推定手段」の一例である。図４に示す基準位置算出処理を実行するＣＰＵ５１が、本発明の「基準位置推定手段」の一例である。図５に示す回転成分算出処理を実行するＣＰＵ５１が、本発明の「回転成分推定手段」の一例である。図６に示す並進成分算出処理を実行するＣＰＵ５１が、本発明の「並進成分推定手段」の一例である。図７に示す球体認識処理を実行するＣＰＵ５１が、本発明の「中心位置推定手段」の一例である。図１に示すフランジ部３４１が本発明の「取付部」の一例である。本体部４１が本発明の「被取付部」の一例である。球体固定軸４５が本発明の「球体固定部」の一例である。

ＣＰＵ５１が実行するキャリブレーション処理が、本発明の「推定工程」及び「推定ステップ」の一例である。ＣＰＵ５１が実行する基準位置算出処理が、本発明の「基準位置推定工程」及び「基準位置推定ステップ」の一例である。ＣＰＵ５１が実行する回転成分算出処理が、本発明の「回転成分推定工程」及び「回転成分推定ステップ」の一例である。ＣＰＵ５１が実行する並進成分算出処理が、本発明の「並進成分推定工程」及び「並進成分推定ステップ」の一例である。

基準位置算出処理における球体認識処理（Ｓ１２）において、スキャナ２から取得する点群データは、本発明の「第一スキャニングデータ」の一例である。回転成分算出処理における球体認識処理（Ｓ２２）において、スキャナ２から取得する点群データは、本発明の「第二スキャニングデータ」の一例である。並進成分算出処理における球体認識処理（Ｓ３２）において、スキャナ２から取得する点群データは、本発明の「第三スキャニングデータ」の一例である。

以上説明したように、本実施形態のロボットシステム１は、スキャナ２、ロボット３、制御装置５を備える。ロボット３は、ターゲット１０を把持可能なハンド部４２を有するエンドエフェクタ４０を備え、回転と並進が可能である垂直多関節型ロボットである。スキャナ２は、ターゲット１０の三次元スキャニングを実行可能ある。制御装置５は、スキャナ２でスキャニングされたターゲット１０のスキャニングデータ（点群データ）に基づき、ターゲット１０の位置姿勢を特定し、該位置姿勢を特定したターゲット１０のピッキングを指示する制御指令をロボット３に出力する。制御装置５０のＣＰＵ５１は、スキャナ座標系からロボット座標系への座標変換に用いる回転成分Ｒ_Ｖと並進成分Ｓ_Ｖを夫々推定する。

具体的には、ＣＰＵ５１は、基準位置算出処理、回転成分算出処理、並進成分算出処理を実行する。基準位置推定処理では、ハンド部４２を基準位置姿勢に移動させたときの、エンドエフェクタ４０に取り付けた球体７０の表面形状の点群データをスキャナ２から取得し、球体７０の基準位置を推定する。回転成分算出処理では、ハンド部４２を、基準位置姿勢に対して、回転成分が同一で並進成分が異なる位置姿勢に移動させた後の球体７０の点群データをスキャナ２から取得し、取得した点群データに基づき推定される球体７０の位置データに基づき、回転成分Ｒ_Ｖを推定する。並進成分算出処理では、ハンド部４２を、基準位置姿勢に対して異なる回転成分である位置姿勢に移動させた後の球体７０の点群データをスキャナ２から取得し、取得した点群データに基づき推定される球体７０の位置データに基づき、並進成分Ｓ_Ｖを推定する。これにより、ロボットシステム１は、スキャナ座標系からロボット座標系へのキャリブレーションを高精度且つ自動で行うことができる。また、ロボットシステム１は、球体７０をキャリブレーションに用いることで、レーザ光の反射強度を可視画像として出力するスキャナを用いることができる。また、ロボットシステム１は、スキャナ２によるターゲット１０の測定場面に応じて、自動でキャリブレーションできるので、ターゲット１０の把持位置を自動で決定できる。

また、本実施形態のＣＰＵ５１は、基準位置算出処理、回転成分算出処理、及び並進成分算出処理において、球体認識処理を実行することで、スキャナ２から出力される球体７０の点群データに基づき、球体７０の中心位置を推定するので、球体７０の位置を容易且つ高精度に推定できる。よって、ロボットシステム１は、スキャナ座標系からロボット座標系へのキャリブレーションを高精度に行うことができる。

また、本実施形態において、キャリブレーション実行時に用いる球体７０は、少なくとも表面が拡散反射するものを採用するので、スキャナ２から照射されるレーザ光を球体７０の表面に反射させ、その反射光を受光できる。このことから、球体７０の表面を正確にスキャニングできる。よって、ロボットシステム１は、球体７０の位置を高精度に認識できるので、スキャナ座標系からロボット座標系へのキャリブレーションを高精度に行うことができる。

また、本実施形態のロボット３は、回転と並進が可能な多関節を有するアーム部３２を備える。エンドエフェクタ４０は、アーム部３２の先端部に設けられたフランジ部３４１に着脱可能に取り付けられる本体部４１と、ハンド部４２とは異なる位置に、球体７０を着脱可能に固定する球体固定軸４５を備える。球体７０は、ハンド部４２に把持させることもできるが、エンドエフェクタ４０の球体固定軸４５に固定することもできる。例えば、ハンド部４２のようにターゲット１０を挟み込む方式ではなく、吸着させてターゲット１０を保持する吸着方式のハンド部も存在する。このタイプでは、球体７０に吸着させる部分が樹脂で形成されている場合があり、球体７０を水平方向に保持した状態では、球体７０に吸着させる部分が球体７０の重さで下がる可能性がある。その場合、球体７０の正確な位置が推定できない。このような場合に、球体７０を球体固定軸４５に固定することで、球体７０はエンドエフェクタ４０と一体して移動するので、球体７０の位置がずれることがない。よって、球体７０の正確な位置が推定できるので、キャリブレーションを良好に行うことができる。

なお、本発明は上記実施の形態に限定されず、様々な変形が可能である。上記実施形態では、スキャナ２と制御装置５が別体であるが、例えば、スキャナ２と制御装置５が一体となったスマートカメラであってもよい。その場合、ＰＣのような制御装置が不要にできるので、ロボットシステムの構成を簡素化できる。その他、本発明のロボットは、例えば直交４軸ロボットの先端に、関節（揺動部）を備えたハンドを取り付けたロボットにも適用可能である。直交４軸とは、スライダを組み合わせたＸＹＺ軸と回転Ｒ軸からなるロボットである。回転Ｒ軸の先端に、１個の関節（揺動部）を備えたハンドを取り付けることで、並進と回転の運動を実現できる。

上記実施形態のロボット３は、３つの回転部Ｒ１〜Ｒ３と、３つの揺動部Ｑ１〜Ｑ３とを備えることで、６軸を備える垂直多関節型のロボットであるが、６軸に限らず、これよりも多くても少なくてもよい。また、ロボット３は、回転と並進が可能なロボットであればよく、垂直多関節型以外にも、例えば、水平多関節ロボットにも適用可能である。

上記実施形態では、説明の便宜上、机上において水平方向に並んで載置されたターゲット１０をピッキングする例を説明したが、ばら積みのターゲット１０をピッキングすることも可能である。

１ ロボットシステム
２ スキャナ
３ ロボット
５ 制御装置
１０ ターゲット
３２ アーム部
４０ エンドエフェクタ
４１ 本体部
４２ ハンド部
４５ 球体固定軸
５１ ＣＰＵ
７０ 球体
３４１ フランジ部

Claims (7)

1. ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、
   前記ターゲットのスキャニングを実行するスキャナと、
   前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力する制御手段と
   を備えたロボットシステムであって、
   前記制御手段は、
   前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定手段を備え、
   前記推定手段は、
   前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定手段と、
   前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定手段と、
   前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定手段と
   を備えたこと
   を特徴とするロボットシステム。
2. 前記基準位置推定手段、前記回転成分推定手段、及び前記並進成分推定手段は、前記球体の前記点群データに基づき、前記球体の中心位置を推定する中心位置推定手段を備え、当該中心位置推定手段で推定された前記中心位置を、前記球体の位置として特定すること
   を特徴とする請求項１に記載のロボットシステム。
3. 前記球体は、少なくとも表面が拡散反射するものであること
   を特徴とする請求項１又は２に記載のロボットシステム。
4. 前記ロボットは、
   回転と並進が可能な多関節を有するアーム部を備え、
   前記エンドエフェクタは、
   前記アーム部の先端部に設けられた取付部に着脱可能に取り付けられる被取付部と、
   前記ハンド部とは異なる位置に、前記球体を着脱可能に固定する球体固定部と
   を備えたこと
   を特徴とする請求項１から３の何れかに記載のロボットシステム。
5. ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナとを備えたロボットシステムの制御を行い、前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力することが可能な制御装置であって、
   前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定手段を備え、
   前記推定手段は、
   前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定手段と、
   前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定手段と、
   前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定手段と
   を備えたこと
   を特徴とする制御装置。
6. ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナとを備えたロボットシステムの制御を行い、前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力可能な制御装置の制御方法であって、
   前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定工程を備え、
   前記推定工程は、
   前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定手段と、
   前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定工程と、
   前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定工程と
   を備えたこと
   を特徴とする制御方法。
7. ターゲットを保持可能なハンド部を有するエンドエフェクタを備え、回転と並進が可能であるロボットと、前記ターゲットのスキャニングを実行するスキャナとを備えたロボットシステムの制御を行い、前記スキャナでスキャニングされた前記ターゲットのスキャニングデータに基づき、前記ターゲットの位置姿勢を特定し、該位置姿勢を特定した前記ターゲットの保持を指示する制御指令を前記ロボットに出力可能な制御装置のコンピュータに実行させるプログラムであって、
   前記スキャナの座標系であるスキャナ座標系から前記ロボットの座標系であるロボット座標系への座標変換に用いる回転成分と並進成分である変換用回転成分と変換用並進成分を夫々推定する推定ステップを備え、
   前記推定ステップは、
   前記ハンド部を基準位置姿勢に移動させたときの、前記エンドエフェクタに取り付けた球体の表面形状を三次元スキャニングした点群データである第一スキャニングデータを前記スキャナから取得し、取得した前記第一スキャニングデータに基づき、前記球体の基準位置を推定する基準位置推定ステップと、
   前記ハンド部を、前記基準位置姿勢に対して、回転成分が同一で並進成分が異なる第一位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第二スキャニングデータを前記スキャナから取得し、取得した前記第二スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用回転成分を推定する回転成分推定ステップと、
   前記ハンド部を、前記基準位置姿勢に対して異なる回転成分である第二位置姿勢に移動させた後の前記球体の表面形状を三次元スキャニングした点群データである第三スキャニングデータを前記スキャナから取得し、取得した前記第三スキャニングデータに基づき推定される前記球体の位置データに基づき、前記変換用並進成分を推定する並進成分推定ステップと
   を備えたこと
   を特徴とするプログラム。

Images