JP5523392B2 - キャリブレーション装置及びキャリブレーション方法 - Google Patents

Description

本発明は、キャリブレーション装置及びキャリブレーション方法に関する。

ビジョンセンサの情報からロボットが作業するためには、ビジョンセンサの座標系をロボット座標系に変換する必要があり、この変換行列を求めることをキャリブレーションと呼ぶ。

システムを立ち上げた時や、立ち上げ後に衝突などで位置ずれが発生した時には、キャリブレーションを実施する必要がある。ロボットによる作業に対して要求される精度が高くなるほど、キャリブレーションに対して要求される精度も高くなる。そのため、熟練作業者が時間をかけて作業を行っていたが、手間と時間とがかかり手戻りも多く、キャリブレーションが必要なビジョンセンサを用いたシステムの保全性が低下する傾向にある。

それに対して、特許文献１には、ロボットアーム装置のアーム先端部に指し棒を把持させ、ステレオ画像計測装置の画像内にアーム先端部の指し棒が映し出された状態で指し棒が長方形を描くようにロボットアームを動作させ、このときの位置データを基に、ステレオ画像計測装置の座標系からロボットアーム装置の座標系への座標変換データを計算することが記載されている。これにより、特許文献１によれば、キャリブレーション作業を自動化できるとされている。

また、特許文献２には、ロボットビジョンシステムにおいて、ロボットのアームの先端にチェッカーボードを固定し、機構的に既知な位置に固定された指示ピンがチェッカーボード上の交点と同一点となるようにアームを動かし、ロボット座標系におけるチェッカーボード上の各マーカーの３次元座標を求めることが記載されている。また、右カメラの撮影画像及び左カメラの撮影画像をそれぞれ画像解析して、ステレオカメラ座標系におけるチェッカーボード上の各マーカーの３次元座標を求め、ロボット座標系におけるマーカーの３次元座標とステレオカメラ座標系におけるマーカーの３次元座標とを対応付けることにより、ステレオカメラ座標系からロボット座標系への剛体変換行列を求めるとされている。これにより、特許文献２によれば、キャリブレーション作業を自動で行うことができるとされている。

特許第３４０２０２１号公報 特開２０１０−１７２９８６号公報

特許文献１、２に記載の技術は、いずれも、キャリブレーションを行う際にロボットのアーム先端にマーカーとなるものを把持又は固定させることが必要である。このため、システム立ち上げ後、ロボットへの所定の物体の衝突などにより既にキャリブレーション済みであるロボットの座標系とビジョンセンサの座標系との間に位置ずれが発生した際にも、システム立ち上げ時と同じキャリブレーション作業を行う必要がある。このため、システム立ち上げ後のキャリブレーションにおいて、システム立ち上げ時と同じ手間及び時間が必要になる。

例えば、実際の生産ラインでロボットの座標系とビジョンセンサの座標系との相対的な位置関係にずれが生じた際、生産ラインの中へ人が立ち入り、ロボットのアーム先端に指し棒を把持させる必要がある。あるいは、ロボットのアーム先端から作業用のエンドエフェクタを取り外し、ロボットのアーム先端にチェッカーボードを取り付ける必要がある。

本発明は、上記に鑑みてなされたものであって、システム立ち上げ後におけるキャリブレーションの手間及び時間をシステム立ち上げ時より低減できるキャリブレーション装置及びキャリブレーション方法を得ることを目的とする。

上述した課題を解決し、目的を達成するために、本発明の１つの側面にかかるキャリブレーション装置は、ロボットを有するシステムのキャリブレーションを行うキャリブレーション装置であって、前記ロボットの手先により作業が行われる作業エリア周辺の２次元画像を取得する２次元計測手段と、前記作業エリア周辺の３次元位置を取得する３次元計測手段と、前記３次元計測手段の座標系に対する前記ロボットの座標系の相対的な位置関係を記述する関連情報を記憶する第１の記憶手段と、前記２次元計測手段によりシステム立ち上げ時に取得された２次元画像と前記３次元計測手段によりシステム立ち上げ時に取得された３次元位置とを記憶する第２の記憶手段と、前記第２の記憶手段に記憶された３次元位置と前記３次元計測手段によりシステム立ち上げ後に取得された３次元位置とに基づいて、前記３次元計測手段の座標系に対する前記ロボットの座標系の相対的な位置関係のずれ量を推定する推定手段と、前記推定手段により推定されたずれ量を用いて、前記第２の記憶手段に記憶された２次元画像と前記２次元計測手段によりシステム立ち上げ後に取得された２次元画像との一致度を計算する計算手段と、前記計算手段により計算された一致度が閾値より小さい場合、前記推定手段により推定されたずれ量を用いて、前記第１の記憶手段に記憶された関連情報を補正する補正手段とを備えたことを特徴とする。

本発明によれば、システム運用時に３次元計測手段の座標系に対するロボットの座標系の相対的な位置関係のずれが発生した際、２次元計測手段及び３次元計測手段の計測データを用いてずれの補正を行うので、キャリブレーション用の専用の計測対象物の設置が必要なく、ロボットの作業エリアに作業者が入る必要がない。これにより、システム立ち上げ後におけるキャリブレーションの手間及び時間をシステム立ち上げ時より低減できる。

図１は、実施の形態に係るキャリブレーション装置の構成を示す図である。 図２は、実施の形態に係るキャリブレーション装置の構成を示す図である。 図３は、システム立ち上げ時におけるキャリブレーション装置の動作を示すフローチャートである。 図４は、５箇所の観測位置の位置と姿勢を表す概念図である。 図５は、各観測位置におけるマーカーの観測画像である。 図６は、システム立ち上げ後におけるキャリブレーション装置の動作を示すフローチャートである。 図７は、実施の形態の変形例に係るキャリブレーション装置の構成を示す図である。

以下に、本発明にかかるキャリブレーション装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。

実施の形態．
実施の形態にかかるキャリブレーション装置１００について説明する。

キャリブレーション装置１００は、図１に示すように、ロボットＲＢ及び３Ｄセンサ２１を有するロボットシステムＲＳのキャリブレーションを行う。キャリブレーションとは、例えば、３Ｄセンサ２１の座標系（ｘ，ｙ，ｚ）からロボットＲＢの座標系（Ｘ，Ｙ，Ｚ）への座標変換を行うための変換行列を求めることである。

まず、キャリブレーション装置１００がキャリブレーションを行う対象であるロボットシステムＲＳについて説明する。

ロボットＲＢは、例えば、６自由度の多関節ロボットであり、ロボットアームの先端には、フランジＲＢ２が設けられている。このフランジＲＢ２には作業用のツールとして例えばロボットハンドＲＢ１等が装着される。そして、このフランジＲＢ２には、ロボットハンドＲＢ１等のツールと並んで３Ｄセンサ２１及び２Ｄセンサ１１が取り付けられている。これにより、ロボットＲＢは、作業エリアＷＡの作業台ＷＴ上に配置された所定の対象物に対して組み付けや加工等の作業を行う。

ロボット座標系（Ｘ，Ｙ，Ｚ）の原点は、例えばロボットＲＢの基部の底面にあり、Ｚ軸を垂直方向にとって定義される。図１ではこのロボット座標系（Ｘ，Ｙ，Ｚ）を便宜上少しずらして描いている。フランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）は、ロボットＲＢの手先のフランジＲＢ２に定義された座標系で、同様に図１では便宜上少しずらして描いている。フランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）の位置と姿勢はロボットコントローラＲＣが制御している。

３Ｄセンサ２１を用いて対象物の位置を計測し、それに基づいてロボットＲＢを動作させるために、３Ｄセンサ２１によって得られる対象物の位置データをロボットＲＢが動作する座標系上のデータに変換する。つまり、３Ｄセンサ２１の座標系（ｘ，ｙ，ｚ）と、ロボット３０が動作する座標系（Ｘ，Ｙ，Ｚ）の間の関係を予め求める。ロボットアーム先端のフランジＲＢ２に定義されているフランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）をロボット座標系（Ｘ，Ｙ，Ｚ）に変換するパラメータが、ロボットコントローラＲＣの記憶部ＲＣ３にあらかじめ記憶されている。そして、このフランジＲＢ２に３Ｄセンサ２１が取付けられているので、センサ座標系（ｘ，ｙ，ｚ）をフランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）に変換するパラメータ（回転行列と並進ベクトル）をキャリブレーションすればよい。これにより、３Ｄセンサ２１で検出したセンサ座標系における対象物の座標をフランジ座標系における座標に変換し、さらにそれをロボット座標系における座標に変換することができる。

ロボットコントローラＲＣは、マイクロプロセッサからなる中央演算処理装置（ＣＰＵ）と、ＣＰＵにバス結合されたＲＯＭメモリ、ＲＡＭメモリ、不揮発性メモリ、ロボットの軸制御のためのディジタルサーボ回路と、サーボアンプ、外部信号の入出力装置、画像処理装置と結ばれた通信インターフェイス等を備えている。これら電子部品は、機能的に制御部ＲＣ１、演算部ＲＣ２、記憶部ＲＣ３を構成している。

記憶部ＲＣ３を構成するメモリのうち、ＲＯＭメモリにはシステムを制御するシステムプログラムが格納され、ＲＡＭメモリはＣＰＵが行なう処理のためのデータの一時記憶等に使用される。また、不揮発性メモリには、動作プログラムのデータや、システムの動作に必要な各種設定値が格納される。この各種設定値には、ロボットＲＢのフランジＲＢ２に定義されているフランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）をロボット座標系（Ｘ，Ｙ，Ｚ）に変換するパラメータ、すなわち、フランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）とセンサ座標系（ｘ，ｙ，ｚ）の相対的な関係を記述する関連情報（例えば、回転行列や並進ベクトルなどのパラメータ）も含まれている。ロボットコントローラＲＣは、フランジＲＢ２の位置姿勢を制御すると同時にそのデータをリアルタイムでアップデートしており、出力することができる。なお、本実施の形態の記憶部ＲＣ３は、ロボットコントローラＲＣの内部に設けられているが、外部に設けられた記憶装置であってもよい。

手動操作パネルＰＡは、ロボットＲＢの手動操作をするためのパネルであり、このパネル上に設けられたボタン、ジョイスティックなどを操作することにより、ロボットＲＢを手動操作できるようになっている。

次に、キャリブレーション装置１００の構成について図２を用いて説明する。図２は、キャリブレーション装置１００の構成を示すブロック図である。

キャリブレーション装置１００は、制御部９０、２次元計測部１０、３次元計測部２０、第１の記憶部３０、第２の記憶部４０、推定部５０、計算部６０、比較部６５、補正部７０、及び表示部８０を備える。

制御部９０は、キャリブレーション装置１００における各部を全体的に制御する。

２次元計測部１０は、作業エリアＷＡ周辺の２次元画像を取得する。作業エリアＷＡは、ロボットＲＢのロボットハンドＲＢ１等により作業が行われるためのエリアである。２次元計測部１０は、例えば、作業エリアＷＡ周辺における複数の計測位置のそれぞれで２次元画像を取得する。

具体的には、２次元計測部１０は、２Ｄセンサ１１及び信号処理部１２を有し、２Ｄセンサ１１により複数の計測位置のそれぞれの２次元画像を撮像し、２Ｄセンサ１１により撮像された複数の２次元画像の画像信号に対して信号処理部１２により所定の信号処理（アナログ信号処理等）を行う。

２次元計測部１０は、システム立ち上げ時に、取得した例えば複数の計測位置の２次元画像を制御部９０経由で第２の記憶部４０へ供給する。２次元計測部１０は、システム立ち上げ後に、取得した例えば複数の計測位置の２次元画像を制御部９０経由で計算部６０へ供給する。

３次元計測部２０は、作業エリアＷＡ周辺の３次元位置を取得する。３次元計測部２０は、例えば、作業エリアＷＡ周辺における複数の計測位置のそれぞれで３次元位置を取得する。３次元計測部２０は、取得した例えば複数の計測位置の３次元位置を制御部９０経由で第２の記憶部４０又は推定部５０へ供給する。

具体的には、３次元計測部２０は、３Ｄセンサ２１、信号処理部２２、及び演算部２３を有し、３Ｄセンサ２１は、右センサ２１１及び左センサ２１２を有する。右センサ２１１により複数の計測位置のそれぞれの２次元画像を撮像し、左センサ２１２により複数の計測位置のそれぞれの２次元画像を撮像し、右センサ２１１及び左センサ２１２により撮像された複数の２次元画像の画像信号に対して信号処理部２２により所定の信号処理（アナログ信号処理等）を行って３次元画像信号を生成し、生成された３次元画像信号から演算部２３は複数の計測位置の３次元位置を求める。

３次元計測部２０は、システム立ち上げ時に、取得した例えば複数の計測位置の３次元位置を制御部９０経由で第２の記憶部４０へ供給する。３次元計測部２０は、システム立ち上げ後に、取得した例えば複数の計測位置の３次元位置を制御部９０経由で推定部５０へ供給する。

第１の記憶部３０は、例えば、上述したロボットコントローラＲＣの記憶部ＲＣ３に含まれており、関連情報３１を記憶している。関連情報３１は、３次元計測部２０の座標系すなわちセンサ座標系（ｘ，ｙ，ｚ）に対するロボットの座標系（Ｘ，Ｙ，Ｚ）の相対的な位置関係を記述する情報である。関連情報３１は、例えば、センサ座標系（ｘ，ｙ，ｚ）をフランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）に変換するパラメータ（例えば、回転行列Ｒ_ｈや並進ベクトルＴ_ｈなどのパラメータ）や、フランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）をロボット座標系（Ｘ，Ｙ，Ｚ）に変換するパラメータ（例えば、回転行列Ｒ_ｆや並進ベクトルＴ_ｆなどのパラメータ）などを含む。

例えば、センサ座標系（ｘ，ｙ，ｚ）は、次の数式（１）でフランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）に変換される。

また、例えば、フランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）は、次の数式（２）でロボット座標系（Ｘ，Ｙ，Ｚ）に変換される。

数式（１）、数式（２）を結合すると、次の数式（３）になる。

すなわち、この式がセンサ座標系（ｘ，ｙ，ｚ）をロボット座標系（Ｘ，Ｙ，Ｚ）に変換する式である。

第２の記憶部４０は、システム立ち上げ時に取得された２次元画像を２次元計測部１０から制御部９０経由で受けて記憶する。例えば、第２の記憶部４０は、複数の計測位置の２次元画像を２次元画像群４１として記憶する。２次元画像群４１では、計測位置と２次元画像とが複数の計測位置のそれぞれについて対応付けられている。

また、第２の記憶部４０は、システム立ち上げ時に取得された３次元位置を３次元計測部２０から制御部９０経由で受けて記憶する。例えば、第２の記憶部４０は、複数の計測位置の３次元位置を３次元位置群４２として記憶する。３次元位置群４２では、計測位置と３次元位置とが複数の計測位置のそれぞれについて対応付けられている。

推定部５０は、システム立ち上げ後に取得された３次元位置を３次元計測部２０から制御部９０経由で受ける。これに応じて、推定部５０は、制御部９０経由で第２の記憶部４０にアクセスし、システム立ち上げ時に取得された３次元位置を第２の記憶部４０から制御部９０経由で読み出す。そして、推定部５０は、システム立ち上げ時に取得された３次元位置とシステム立ち上げ後に取得された３次元位置とに基づいて、３次元計測部２０の座標系（ｘ，ｙ，ｚ）に対するロボットＲＢの座標系（Ｘ，Ｙ，Ｚ）の相対的な位置関係のずれ量を推定する。推定部５０は、推定されたずれ量を制御部９０経由で計算部６０へ供給する。

計算部６０は、推定されたずれ量を推定部５０から制御部９０経由で受ける。また、計算部６０は、システム立ち上げ後に取得された２次元画像を２次元計測部１０から制御部９０経由で受ける。これに応じて、計算部６０は、制御部９０経由で第２の記憶部４０にアクセスし、システム立ち上げ時に取得された２次元画像を第２の記憶部４０から制御部９０経由で読み出す。そして、計算部６０は、推定部５０により推定されたずれ量を用いて、システム立ち上げ時に取得された２次元画像とシステム立ち上げ後に取得された２次元画像との一致度を計算する。例えば、計算部６０は、システム立ち上げ時に取得された２次元画像とシステム立ち上げ後に取得された２次元画像との個別の一致度を複数の計測位置のそれぞれについて計算し、複数の計測位置について計算された個別の一致度の総和を求め、その総和を上記の一致度として計算する。計算部６０は、計算された一致度を制御部９０経由で比較部６５へ供給する。

比較部６５は、計算された一致度を計算部６０から制御部９０経由で受ける。比較部６５は、計算された一致度を閾値と比較し、比較結果を制御部９０経由で補正部７０及び表示部８０へ供給する。

補正部７０は、比較結果を比較部６５から制御部９０経由で受ける。補正部７０は、計算部６０により計算された一致度が閾値より小さいことが比較結果により示されていた場合、推定部５０により推定されたずれ量を用いて、第１の記憶部３０に記憶された関連情報３１を補正する。

表示部８０は、比較結果を比較部６５から制御部９０経由で受ける。表示部８０は、計算部６０により計算された一致度が閾値以上であることが比較結果により示されていた場合、推定部５０により推定されたずれ量に応じた補正すべき方向を示すオブジェクトをロボットＲＢに関する画像に重ねて表示する。具体的には、表示部８０は、ディスプレイ８１及び表示制御部８２を有し、例えば２次元計測部１０によりシステム立ち上げ後に取得された２次元画像に例えば、図１に示す補正すべき方向を示す矢印のオブジェクトを重ね合わせた表示データを表示制御部８２により生成し、その表示データに応じた図１に示すような画像をディスプレイ８１により表示する。

次に、ロボットシステムＲＳのシステム立ち上げ時におけるキャリブレーション装置１００の動作について図３を用いて説明する。図３は、システム立ち上げ時におけるキャリブレーション装置１００の動作を示すフローチャートである。

ステップＳ１では、キャリブレーション装置１００が、図４に示すようなマーカーＭＫを用いたキャリブレーション（フルキャリブレーション）を実施する。

具体的には、マーカーＭＫが作業台ＷＴ（図１参照）上に配置される。作業者は、ロボットＲＢの手動操作パネルＰＡを操作して、順次ロボットＲＢの手先の位置と姿勢を制御しながら、３次元計測部２０によるマーカーＭＫの検出を例えば５回実行し、各回のロボットＲＢの姿勢データＲ_ｆ、Ｔ_ｆとマーカーＭＫのセンサ座標系上の座標とを対応付けて記録する。このときに姿勢を変えずに位置だけ手先を移動させた第１から第３の観測箇所（例えば、図４に示す観測１、観測２、観測３）にてマーカーＭＫの（例えば、白、黒交互に塗り分けたパターンにおける対角線の交点の）観測を行い、次いで手先の位置も姿勢も変化させた第４および第５の観測箇所（例えば、図４に示す観測４、観測５）でマーカーＭＫの観測を実行する（図５参照）。

各観測毎に、ロボットコントローラＲＣから出力されるフランジ座標系（Ｘ_Ｆ，Ｙ_Ｆ，Ｚ_Ｆ）の位置、姿勢を表す回転行列Ｒｆと並進ベクトルＴｆとが、画像処理装置から出力される観測されたマーカーＭＫのセンサ座標系（ｘ，ｙ，ｚ）と対応づけられて記録される。

第１から第３の観測箇所から並進ベクトルを含まない方程式が得られて、回転行列が容易に求まり、第１、第４及び第５の観測箇所からは回転行列、並進ベクトルを含む一般形の方程式が得られるが、回転行列がわかっていると並進ベクトルだけの方程式となり、容易に並進ベクトルが求まる。

例えば、フランジＲＢ２の姿勢をＲ_ｆｃに固定して、つまり３Ｄセンサ２１の姿勢を同じに保ったまま、２つの位置Ｔ_ｆ１およびＴ_ｆ２に移動してマーカーＭＫを観測したときのマーカーＭＫのセンサ座標系上の座標値をそれぞれ（ｘ_１，ｙ_１，ｚ_１）、（ｘ_２，ｙ_２，ｚ_２）とすると、ロボット座標系におけるマーカーＭＫの座標は同じであるから次の数式（４）が成り立つ。

数式（４）を変形すると、次の数式（５）が得られる。

同様にして、３Ｄセンサ２１の姿勢を同じに保ったまま第３の位置Ｔ_ｆ３に移動して、同じマーカーＭＫを観測すると、次の数式（６）が得られる。

数式（５）、数式（６）において未知数はＲ_ｈである。Ｒ_ｈは３×３の行列で９要素からなるが、回転行列であることから、各列（各行）の要素の２乗和が１になるという制約条件があり、（ｘ_１，ｙ_１，ｚ_１）、（ｘ_２，ｙ_２，ｚ_２）、（ｘ_３，ｙ_３，ｚ_３）が互いに異なっていれば、数式（５）、数式（６）からなる連立方程式を解いてＲ_ｈをユニークに決定できる。

このようにして、システム立ち上げ時のキャリブレーション（ステップＳ１）の実施後に、ステップＳ２〜ステップＳ７の処理を、システム立ち上げ後の再キャリブレーションの事前準備として行う。

ステップＳ２では、作業者は、ロボットＲＢの手動操作パネルＰＡを操作して、作業エリアＷＡ周辺で計測位置Ｐ（ｉ）（ｉ＝１，２，…，Ｎ）をＮ個登録し、ロボットコントローラＲＣ或いは計算機のメモリに保存する。そして、キャリブレーション装置１００は、計測位置を識別するためのパラメータｉに初期値０を設定する。

ステップＳ３では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉをインクリメントする。例えば、初回であれば、キャリブレーション装置１００は、ｉ＝１とする。

ステップＳ４では、キャリブレーション装置１００が、ロボットＲＢを計測位置Ｐ（ｉ）へ移動させる。例えば、初回であれば、キャリブレーション装置１００は、ロボットＲＢを計測位置Ｐ（１）へ移動させる。

ステップＳ５では、キャリブレーション装置１００が、計測位置Ｐ（ｉ）の３次元位置として３Ｄ点群データｂＰｎｔ（ｉ）を取得する。例えば、初回であれば、キャリブレーション装置１００は、計測位置Ｐ（１）の３Ｄ点群データｂＰｎｔ（１）を取得して第２の記憶部４０に保存する。

ステップＳ６では、キャリブレーション装置１００が、計測位置Ｐ（ｉ）の２次元画像として２Ｄ画像ｂＩｍｇ（ｉ）を取得する。例えば、初回であれば、キャリブレーション装置１００は、計測位置Ｐ（１）の２Ｄ画像ｂＩｍｇ（１）を取得して第２の記憶部４０に保存する。

ステップＳ７では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉが上限値Ｎより小さいか否かを判断する。キャリブレーション装置１００は、パラメータｉが上限値Ｎより小さい場合（ステップＳ７でＹｅｓ）、まだ未計測の計測位置があるものとして、処理をステップＳ３へ戻し、パラメータｉが上限値Ｎ以上である場合（ステップＳ７でＮｏ）、全ての計測位置の計測を完了したものとして、処理を終了する。

このようにして、各計測位置Ｐ（ｉ）（ｉ＝１，２，…，Ｎ）で、２Ｄ画像ｂＩｍｇ（ｉ）と３次元点群データｂＰｎｔ（ｉ）とをそれぞれ計測し、第２の記憶部４０に保存しておく。

次に、ロボットシステムＲＳのシステム立ち上げ後におけるキャリブレーション装置１００の動作について図６を用いて説明する。図６は、システム立ち上げ後におけるキャリブレーション装置１００の動作を示すフローチャートである。

キャリブレーション装置１００は、センサ座標系（ｘ，ｙ，ｚ）に対するロボット座標系（Ｘ，Ｙ，Ｚ）の相対的な位置関係にずれが発生したことを所定のセンサ（図示せず）により検知すると、ステップＳ１１以降の簡易キャリブレーション補正処理を開始する。

ステップＳ１１では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉに初期値０を設定する。

ステップＳ１２では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉをインクリメントする。例えば、初回であれば、キャリブレーション装置１００は、ｉ＝１とする。

ステップＳ１３では、キャリブレーション装置１００が、ロボットＲＢを計測位置Ｐ（ｉ）へ移動させる。例えば、初回であれば、キャリブレーション装置１００は、ロボットＲＢを計測位置Ｐ（１）へ移動させる。

ステップＳ１４では、キャリブレーション装置１００が、計測位置Ｐ（ｉ）の３次元位置として３Ｄ点群データａＰｎｔ（ｉ）を取得する。例えば、初回であれば、キャリブレーション装置１００は、計測位置Ｐ（１）の３Ｄ点群データａＰｎｔ（１）を取得する。

ステップＳ１５では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉが上限値Ｎより小さいか否かを判断する。キャリブレーション装置１００は、パラメータｉが上限値Ｎより小さい場合（ステップＳ１５でＹｅｓ）、まだ未計測の計測位置があるものとして、処理をステップＳ１２へ戻し、パラメータｉが上限値Ｎ以上である場合（ステップＳ１５でＮｏ）、全ての計測位置の計測を完了したものとして、処理をステップＳ１６へ進める。

このようにして、各計測位置Ｐ（ｉ）（ｉ＝１，２，…，Ｎ）で、３Ｄ点群データａＰｎｔ（ｉ）を計測する。

ステップＳ１６では、キャリブレーション装置１００が、センサ座標系（ｘ，ｙ，ｚ）に対するロボット座標系（Ｘ，Ｙ，Ｚ）の相対的な位置関係のずれ量を推定する。具体的には、キャリブレーション装置１００は、次の数式（７）の値を最小にする回転行列（回転成分）Ｒ、並進ベクトル（並進成分）Ｔを推定する。

_Ｎ
Σ｛ｂＰｎｔ（ｉ）−（Ｒ・ａＰｎｔ（ｉ）＋Ｔ）｝ ・・・（７）
^ｉ＝１
推定方法は特定の方法に限定されず、どんな手法を使っても良い。例えば、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法と呼ばれる手法で、回転行列Ｒ、並進ベクトルＴを求めることができる。

ステップＳ１７では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉに初期値０を設定する。

ステップＳ１８では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉをインクリメントする。例えば、初回であれば、キャリブレーション装置１００は、ｉ＝１とする。

ステップＳ１９では、キャリブレーション装置１００が、ステップＳ１６で推定されたずれ量（例えば、回転行列Ｒ、並進ベクトルＴ）を用いて計測位置を修正する。例えば、キャリブレーション装置１００は、次の数式（８）を用いて修正後のＰ’（ｉ）を計算する。

Ｐ’（ｉ）＝Ｒ・Ｐ（ｉ）＋Ｔ ・・・（８）

ステップＳ２０では、キャリブレーション装置１００が、ロボットＲＢを計測位置Ｐ’（ｉ）へ移動させる。例えば、初回であれば、キャリブレーション装置１００は、ロボットＲＢを計測位置Ｐ’（１）へ移動させる。

ステップＳ２１では、キャリブレーション装置１００が、計測位置Ｐ’（ｉ）の２次元画像として２Ｄ画像ａＩｍｇ（ｉ）を取得する。例えば、初回であれば、キャリブレーション装置１００は、計測位置Ｐ’（１）の２Ｄ画像ａＩｍｇ（１）を取得する。

ステップＳ２２では、キャリブレーション装置１００が、システム立ち上げ時に取得された２Ｄ画像ｂＩｍｇ（ｉ）とシステム立ち上げ後に取得された２Ｄ画像ａＩｍｇ（ｉ）との一致度である個別の一致度Ｅ（ｉ）を計算する。例えば、初回であれば、キャリブレーション装置１００は、システム立ち上げ時に取得された２Ｄ画像ｂＩｍｇ（１）とシステム立ち上げ後に取得された２Ｄ画像ａＩｍｇ（１）との一致度である個別の一致度Ｅ（１）を計算する。一致度の定義は例えば、画像の背景差分を用いて評価することが考えられる。

ステップＳ２３では、キャリブレーション装置１００が、計測位置を識別するためのパラメータｉが上限値Ｎより小さいか否かを判断する。キャリブレーション装置１００は、パラメータｉが上限値Ｎより小さい場合（ステップＳ２３でＹｅｓ）、まだ未計測の計測位置があるものとして、処理をステップＳ１８へ戻し、パラメータｉが上限値Ｎ以上である場合（ステップＳ２３でＮｏ）、全ての計測位置の計測を完了したものとして、処理をステップＳ２４へ進める。

このようにして、各計測位置Ｐ’（ｉ）（ｉ＝１，２，…，Ｎ）で２Ｄ画像ａＩｍｇ（ｉ）を計測するとともに、対応する各計測位置Ｐ（ｉ）及びＰ’（ｉ）（ｉ＝１，２，…，Ｎ）について個別の一致度Ｅ（ｉ）を計算する。

ステップＳ２４では、キャリブレーション装置１００が、個別の一致度Ｅ（ｉ）の総和が閾値Ｔ_Ｅより大きいか否かを判断する。すなわち、キャリブレーション装置１００は、次の数式（９）が成り立つか否かを判断する。

_Ｎ
ΣＥ（ｉ）＞Ｔ_Ｅ ・・・（９）
^ｉ＝１

キャリブレーション装置１００は、数式（９）が成り立たない場合（ステップＳ２４でＮｏ）、センサ座標系（ｘ，ｙ，ｚ）に対するロボット座標系（Ｘ，Ｙ，Ｚ）の相対的な位置関係のずれ量が補正可能な範囲内にあるものとして、処理をステップＳ２５へ進める。あるいは、キャリブレーション装置１００は、数式（９）が成り立つ場合（ステップＳ２４でＹｅｓ）、センサ座標系（ｘ，ｙ，ｚ）に対するロボット座標系（Ｘ，Ｙ，Ｚ）の相対的な位置関係のずれ量が補正可能な範囲を超えているものとして、処理をステップＳ２６へ進める。

ステップＳ２５では、キャリブレーション装置１００が、システム立ち上げ時に計算した変換行列に、ステップＳ１６で推定されたずれ量（例えば、回転行列Ｒ、並進ベクトルＴ）を反映させて補正する。例えば、キャリブレーション装置１００は、数式（１）における回転行列Ｒ_ｈ、並進ベクトルＴ_ｈに対して、それぞれ、回転行列Ｒ、並進ベクトルＴを加算したものを新たに回転行列Ｒ_ｈ、並進ベクトルＴ_ｈとする補正を行う。

ステップＳ２６では、キャリブレーション装置１００が、フルキャリブレーションを行う。

ここで、最初からフルキャリブレーションを行う場合、ステップＳ１と全く同じ処理を行うのであれば、キャリブレーション用のマーカーＭＫや冶具の計測位置姿勢を２Ｄセンサ、３Ｄセンサのそれぞれで試行錯誤しながら変更しなくてはならない。

それに対して、本実施の形態では、キャリブレーション装置１００が、推定されたずれ量に応じた補正すべき方向を示すオブジェクトをロボットＲＢに関する画像に重ねて表示する。例えば２次元計測部１０によりシステム立ち上げ後に取得された２次元画像に例えば、図１に示す補正すべき方向を示す矢印のオブジェクトを重ね合わせた表示データを表示制御部８２により生成し、その表示データに応じた図１に示すような画像をディスプレイ８１により表示する。これにより、どちらの方向に移動すればキャリブレーションの補正を行うために次に必要な情報が得られるかの方向性を示すことができる。すなわち、フルキャリブレーションをステップＳ１よりも簡易に行うことができる。

例えば、位置ずれ前及び位置ずれ後の３次元点群データｂＰｎｔ（ｉ）及びａＰｎｔ（ｉ）（ｉ＝１，２，…，Ｎ）を用いて、ｂＰｎｔ（ｉ）をどう動かせばａＰｎｔ（ｉ）との一致度が高くなるかについて、数式（７）を最小化するような回転行列Ｒ、並進ベクトルＴを計算する。そして、回転行列Ｒ、並進ベクトルＴでキャリブレーション用の各計測位置を変換後、作業者がライブ画像を見ながら矢印のオブジェクトに従って微修正する。微修正後にキャリブレーション用のプログラムを走らせて位置ずれ後の再立上げが完了する。

以上のように、実施の形態では、キャリブレーション装置１００が、システム立上げ完了時点で、事前に作業エリア周辺の２次元画像及び３次元位置を取得しておく。センサ座標系とロボット座標系との相対関係が衝突により変化したときや、３Ｄセンサをセンサ自体の調整や修理などの目的で取り外して再度取り付けたりして変化したときに、再度作業エリア周辺の２次元画像及び３次元位置を取得する。位置ずれ発生前後の３次元点群データ間の位置ずれ量を計算することでキャリブレーションデータの補正パラメータを計算するとともに、位置ずれ発生前後の２次元画像間の一致度を計算し閾値と比較することで位置ずれ量が補正可能な範囲内か否かを判断する。これにより、ロボットとビジョンセンサの相対関係が衝突や取り付け直しでずれが発生した際、ずれ量が補正可能な範囲内であれば自動で位置ずれ量を補正して復旧することができる。

したがって、システム運用時に位置ずれが発生した際、ゼロベースのフルキャリブレーションを行う必要がなく、作業エリア周辺の環境を撮影した２次元及び３次元の計測データを用いるので２Ｄセンサや３Ｄセンサの固定さえできれば、キャリブレーション用の専用の計測対象物の設置が必要なく、ロボットの作業エリアに作業者が入る必要がない。このため、再キャリブレーションの手間と時間を短縮できる。すなわち、システム立ち上げ後におけるキャリブレーションの手間及び時間をシステム立ち上げ時より低減できる。

また、実施の形態では、キャリブレーション装置１００が、推定されたずれ量に応じた補正すべき方向を示すオブジェクトをロボットＲＢに関する画像に重ねて表示する。これにより、補正すべき方向性が示されるので、ずれ量が補正可能な範囲を超えても、その後やり直すキャリブレーションの教示作業を支援してシステムの迅速な復旧を行うことができる。

なお、キャリブレーション装置１００ｉの表示部８０ｉは、ディスプレイ８１に代えて、又は、ディスプレイ８１に加えて、図７に示すようなＡＲ（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ）ツール８１ｉを有していても良い。すなわち、作業者ＷＰにＡＲツール８１ｉを持たせ、表示部８０ｉの表示制御部８２（図２参照）は、ＡＲツール８１ｉと例えば無線通信回線で通信を行い、表示すべきデータをＡＲツール８１ｉへ送信する。これにより、ＡＲツール８１ｉのディスプレイにロボットを動かすべき方向を示すことで、計測位置の修正を容易にするような支援を行ってもよい。

以上のように、本発明にかかるキャリブレーション装置は、ロボットを有するシステムのキャリブレーションに有用である。

１０ ２次元計測部
２０ ３次元計測部
３０ 第１の記憶部
４０ 第２の記憶部
５０ 推定部
６０ 計算部
８０、８０ｉ 表示部
１００、１００ｉ キャリブレーション装置

Claims (4)

1. ロボットを有するシステムのキャリブレーションを行うキャリブレーション装置であって、
   前記ロボットの手先により作業が行われる作業エリア周辺の２次元画像を取得する２次元計測手段と、
   前記作業エリア周辺の３次元位置を取得する３次元計測手段と、
   前記３次元計測手段の座標系に対する前記ロボットの座標系の相対的な位置関係を記述する関連情報を記憶する第１の記憶手段と、
   前記２次元計測手段によりシステム立ち上げ時に取得された２次元画像と前記３次元計測手段によりシステム立ち上げ時に取得された３次元位置とを記憶する第２の記憶手段と、
   前記第２の記憶手段に記憶された３次元位置と前記３次元計測手段によりシステム立ち上げ後に取得された３次元位置とに基づいて、前記３次元計測手段の座標系に対する前記ロボットの座標系の相対的な位置関係のずれ量を推定する推定手段と、
   前記推定手段により推定されたずれ量を用いて、前記第２の記憶手段に記憶された２次元画像と前記２次元計測手段によりシステム立ち上げ後に取得された２次元画像との一致度を計算する計算手段と、
   前記計算手段により計算された一致度が閾値より小さい場合、前記推定手段により推定されたずれ量を用いて、前記第１の記憶手段に記憶された関連情報を補正する補正手段と、
   を備えたことを特徴とするキャリブレーション装置。
2. 前記計算手段により計算された一致度が前記閾値以上である場合、前記推定手段により推定されたずれ量に応じた補正すべき方向を示すオブジェクトを前記ロボットに関する画像に重ねて表示する表示手段をさらに備えた
   ことを特徴とする請求項１に記載のキャリブレーション装置。
3. ロボットを有するシステムのキャリブレーションを行うキャリブレーション方法であって、
   システム立ち上げ時に、前記ロボットの手先により作業が行われる作業エリア周辺の２次元画像を取得する第１の２次元計測ステップと、
   システム立ち上げ時に、３次元計測手段により前記作業エリア周辺の３次元位置を取得する第１の３次元計測ステップと、
   前記３次元計測手段の座標系に対する前記ロボットの座標系の相対的な位置関係を記述する関連情報を記憶する第１の記憶ステップと、
   前記第１の２次元計測手段で取得された２次元画像と前記第１の３次元計測ステップで取得された３次元位置とを記憶する第２の記憶ステップと、
   システム立ち上げ後に、前記作業エリア周辺の２次元画像を取得する第２の２次元計測ステップと、
   システム立ち上げ後に、前記作業エリア周辺の３次元位置を取得する第２の３次元計測ステップと、
   前記第２の記憶ステップで記憶された３次元位置と前記第２の３次元計測ステップで取得された３次元位置とに基づいて、前記３次元計測手段の座標系に対する前記ロボットの座標系の相対的な位置関係のずれ量を推定する推定ステップと、
   前記推定ステップで推定されたずれ量を用いて、前記第２の記憶ステップで記憶された２次元画像と前記第２の２次元計測ステップで取得された２次元画像との一致度を計算する計算ステップと、
   前記計算ステップで計算された一致度が閾値より小さい場合、前記推定ステップで推定されたずれ量を用いて、前記第１の記憶ステップで記憶された関連情報を補正する補正ステップと、
   を備えたことを特徴とするキャリブレーション方法。
4. 前記計算ステップで計算された一致度が前記閾値以上である場合、前記推定ステップで推定されたずれ量に応じた補正すべき方向を示すオブジェクトを前記ロボットに関する画像に重ねて表示する表示ステップをさらに備えた
   ことを特徴とする請求項３に記載のキャリブレーション方法。

Images