JPWO2018142582A1 - 変換係数算出装置、変換係数算出方法及び変換係数算出プログラム - Google Patents

Abstract

変換係数算出部（１３）が、ロボットハンド（１）の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部（１１）により特定されたロボット座標系での位置とセンサ情報取得部（１２）により取得されたセンサ情報が示すセンサ座標系での位置との組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出する。

Description

この発明は、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出装置、変換係数算出方法及び変換係数算出プログラムに関するものである。

ロボットシステムは、例えば、加工対象等の物体を把持するロボットハンドが取り付けられているロボットアームなどを備えているシステムである。
また、ロボットシステムは、物体及びロボットハンドの周辺を計測するセンサを備えており、センサにより計測されたセンサ情報を用いて、ロボットアーム及びロボットハンドの位置及び姿勢を制御するようにしている。

ただし、センサにより計測されたセンサ情報は、物体等のセンサ座標系での３次元位置を示す情報である。このため、ロボットシステムが、ロボットアーム等の位置及び姿勢の制御を行う際、センサ情報を用いることができるようにするには、センサ情報が示す３次元位置をセンサ座標系からロボット座標系に変換する必要がある。
センサ座標系は、センサを中心とする座標系であり、ロボット座標系は、ロボットを中心とするロボット座標系である。

以下の特許文献１には、ロボットハンドが画像認識用のマーカを把持し、センサであるカメラがマーカを撮影することで、座標の変換係数を算出する変換係数算出装置が開示されている。
この変換係数算出装置は、カメラにより撮影された映像内のマーカのセンサ座標系（カメラ座標系）での位置と、ロボットハンドにより把持されているマーカのロボット座標系での位置とから、センサであるカメラとロボットとの相対的な位置関係を示すパラメータを算出する。
センサであるカメラとロボットとの相対的な位置関係を示すパラメータは、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる座標の変換係数に相当する。
この変換係数算出装置は、カメラとロボットとの相対的な位置関係を示すパラメータを算出すると、以降、相対的な位置関係を示すパラメータを用いて、センサにより計測されたセンサ情報が示す物体等の３次元位置をロボット座標系の３次元位置に変換する。

特開２０１４−１８０７２０号公報

従来の変換係数算出装置は以上のように構成されているので、センサがカメラであり、ロボットハンドがマーカを把持するようにすれば、センサであるカメラとロボットとの相対的な位置関係を示すパラメータを算出することができる。しかし、センサが、例えば、レーザスキャナなどの３次元センサである場合、マーカを撮影することができないため、センサとロボットとの相対的な位置関係を示すパラメータを算出することができないという課題があった。

この発明は上記のような課題を解決するためになされたもので、センサがレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる変換係数算出装置、変換係数算出方法及び変換係数算出プログラムを得ることを目的とする。

この発明に係る変換係数算出装置は、物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体のロボット座標系での位置又は物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得部と、ロボットハンドに固定されている物体を計測するセンサから、物体のセンサ座標系での位置又は物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得部とを設け、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するようにしたものである。

この発明によれば、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、センサがレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果がある。

この発明の実施の形態１による変換係数算出装置４を実装しているロボットシステムを示す構成図である。 この発明の実施の形態１による変換係数算出装置４を示すハードウェア構成図である。 変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合のコンピュータのハードウェア構成図である。 変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。 この発明の実施の形態２による変換係数算出装置６を実装しているロボットシステムを示す構成図である。 この発明の実施の形態２による変換係数算出装置６を示すハードウェア構成図である。 変換係数算出装置６がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。 この発明の実施の形態４による変換係数算出装置６を実装しているロボットシステムを示す構成図である。 この発明の実施の形態４による変換係数算出装置６を示すハードウェア構成図である。

以下、この発明をより詳細に説明するために、この発明を実施するための形態について、添付の図面に従って説明する。

実施の形態１．
図１はこの発明の実施の形態１による変換係数算出装置４を実装しているロボットシステムを示す構成図である。
図１において、ロボットハンド１はロボットアームの先端等に取り付けられており、例えば、加工対象等の物体１０が固定されている。
ロボットアームは、少なくとも１つ以上の関節を備えており、１つ以上の関節が制御されることで、ロボットハンド１の位置又は姿勢が切り替えられる。
ロボットハンド１は、物体１０を把持する機能を有している。ただし、ロボットハンド１は、物体１０を固定することができればよく、物体１０を把持する方式は問わない。例えば、多数の指を開閉することで物体１０を挟み込む方式、磁力又は吸引によって物体１０を把持する方式などがある。

ロボット制御部２はロボットアーム又はロボットハンド１の関節を制御することで、ロボットハンド１の位置又は姿勢を切り替える制御装置である。ロボットハンド１の姿勢は、例えば、ロボットが置かれている面に対する角度などが該当する。
センサ３はロボットハンド１により把持されている物体１０を計測するレーザスキャナなどの３次元センサであり、物体１０の計測結果を示すセンサ情報を出力する。
センサ３から出力されるセンサ情報は、空間中の物体１０の表面の３次元座標（Ｘ, Ｙ, Ｚ）の集合である。センサ３による物体１０の計測方法として、ステレオビジョンに基づく方法、Ｔｉｍｅ−ｏｆ−Ｆｌｉｇｈｔ法などが知られている。

変換係数算出装置４は、位置姿勢情報取得部１１、センサ情報取得部１２、変換係数算出部１３及び座標変換部１４を備えている。
図２はこの発明の実施の形態１による変換係数算出装置４を示すハードウェア構成図である。
位置姿勢情報取得部１１は例えば図２に示す位置姿勢情報取得回路２１で実現されるものであり、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での３次元位置を特定する処理を実施する。
センサ情報取得部１２は例えば図２に示すセンサ情報取得回路２２で実現されるものであり、センサ３から物体１０のセンサ座標系での３次元位置を示すセンサ情報を取得する処理を実施する。
なお、センサ座標系は、センサ３を中心とする座標系であり、ロボット座標系は、ロボットハンド１を中心とするロボット座標系である。

変換係数算出部１３は例えば図２に示す変換係数算出回路２３で実現される。
変換係数算出部１３はロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部１１により特定されたロボット座標系での３次元位置とセンサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での３次元位置との組を保持する記憶部１３ａを備えている。
変換係数算出部１３は、記憶部１３ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する処理を実施する。
また、変換係数算出部１３は、ロボット座標系での３次元位置とセンサ座標系での３次元位置との組を保持する毎に、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する処理を実施する。

座標変換部１４は例えば図２に示す座標変換回路２４で実現される。
座標変換部１４は変換係数算出部１３により算出された変換係数である回転行列Ｒ及び並進ベクトルｔを用いて、センサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での３次元位置をロボット座標系での３次元位置に変換する処理を実施する。

図１では、変換係数算出装置４の構成要素である位置姿勢情報取得部１１、センサ情報取得部１２、変換係数算出部１３及び座標変換部１４のそれぞれが、図２に示すような専用のハードウェア、即ち、位置姿勢情報取得回路２１、センサ情報取得回路２２、変換係数算出回路２３及び座標変換回路２４で実現されるものを想定している。
ここで、変換係数算出部１３が備えている記憶部１３ａは、例えば記憶処理回路で実現される。
この記憶処理回路は、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒy）などの不揮発性又は揮発性の半導体メモリや、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）などが該当する。
また、位置姿勢情報取得回路２１、センサ情報取得回路２２、変換係数算出回路２３及び座標変換回路２４は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、または、これらを組み合わせたものが該当する。

変換係数算出装置４の構成要素は、専用のハードウェアで実現されるものに限るものではなく、変換係数算出装置４がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
ソフトウェア又はファームウェアはプログラムとして、コンピュータのメモリに格納される。コンピュータは、プログラムを実行するハードウェアを意味し、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）などが該当する。
図３は変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合のコンピュータのハードウェア構成図である。
変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合、変換係数算出部１３が備えている記憶部１３ａをコンピュータのメモリ３１上に構成するとともに、位置姿勢情報取得部１１の位置姿勢情報取得処理手順、センサ情報取得部１２のセンサ情報取得処理手順、変換係数算出部１３の変換係数算出処理手順及び座標変換部１４の座標変換処理手順をコンピュータに実行させるための変換係数算出プログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行するようにすればよい。

図３において、通信回路３３はロボットハンド１から位置姿勢情報を受信するとともに、センサ３からセンサ情報を受信し、また、制御信号をロボット制御部２に出力する回路である。
表示装置３４は例えば液晶ディスプレイで実現されるものであり、プロセッサ３２の処理の様子などを表示する。
図４は変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。

次に動作について説明する。
ロボットハンド１は、物体１０を把持している。この実施の形態１では、一例として、球体の物体１０を把持しているものとする。
ロボット制御部２は、ロボットアーム又はロボットハンド１の関節を制御することで、ロボットハンド１の位置又は姿勢を切り替える。
この実施の形態１では、ロボット制御部２が、変換係数算出部１３から出力される制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置又は姿勢を切り替えるものとする。
また、この実施の形態１では、説明の便宜上、Ｎ（Ｎは３以上の整数）回、ロボットアーム又はロボットハンド１の関節を制御するものとする。

ロボットハンド１により把持されている物体１０は、ロボット制御部２によってロボットアーム又はロボットハンド１の関節が制御されることで、ｎ（ｎ＝１，２，・・・，Ｎ）回目の制御では、ロボット座標系でＰ_ｒ ^ｎの位置に移動される（図４のステップＳＴ１）。この段階では、ｎ＝１であるため、ロボット座標系でＰ_ｒ ^１の位置に移動される。
位置Ｐ_ｒ ^ｎは、以下の式（１）に示すように、ロボット座標系のＸ軸における座標Ｘ_ｒ、Ｙ軸における座標Ｙ_ｒ、Ｚ軸における座標Ｚ_ｒで表される。

式（１）において、Ｔは転置を表す記号であり、例えば、ｘ^Ｔはベクトルｘの転置である。

位置姿勢情報取得部１１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎを特定する（図４のステップＳＴ２）。
位置姿勢情報からロボット座標系での３次元位置Ｐ_ｒ ^ｎを特定する処理は公知の技術であるため詳細な説明を省略するが、例えば、以下に示すような処理で３次元位置Ｐ_ｒ ^ｎを特定することができる。
例えば、ロボットハンド１の位置及び姿勢と、ロボットハンド１により把持されている物体１０の３次元位置Ｐ_ｒ ^ｎとの対応関係を示すテーブルを事前に用意し、位置姿勢情報取得部１１が、テーブルを参照することで、位置姿勢情報が示す位置及び姿勢に対応する３次元位置Ｐ_ｒ ^ｎを特定する。

センサ３は、ロボット制御部２によって、ロボット座標系でＰ_ｒ ^ｎ（ｎ＝１，２，・・・，Ｎ）の位置に移動された物体１０を計測し、物体１０の計測結果として、物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を示すセンサ情報を出力する。

この実施の形態１では、センサ３から出力されるセンサ情報は、物体１０の表面におけるＭ点の位置を示す３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎである。

センサ情報取得部１２は、センサ３から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を特定する（図４のステップＳＴ３）。
Ｍは１以上の整数であり、Ｍ＝１の場合、Ｐ_ｓ ^ｎ＝ｐ_１ ^ｎである。
Ｍ≧２の場合、センサ情報取得部１２は、物体１０の形状を示す形状データを用いて、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎから物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを算出する。
この実施の形態１では、ロボットハンド１が球体の物体１０を把持しているため、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎは、球体の表面上のいずれかの点である。
このため、センサ情報取得部１２は、物体１０の形状を示す形状データとして、球体の形状を表す球の方程式を使用し、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに球の方程式をフィッティングすることで、物体１０の中心の位置（Ｘ_０，Ｙ_０，Ｚ_０）として、３次元位置Ｐ_ｓ ^ｎを得ることができる。

３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに球の方程式をフィッティングする方法としては、複数の方法が考えられるが、ここでは、一例として、最小二乗法によるフィッティング方法を説明する。
まず、球の方程式は、以下の式（３）で表されるものとする。

式（３）において、ｒは球体の半径である。
このとき、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの二乗誤差は、以下の式（４）で表される。

式（４）において、||ｘ||はベクトルｘのノルムを表す記号である。
センサ情報取得部１２は、式（４）で表される二乗誤差が最小となるＰ_ｓ ^ｎをセンサ座標系での３次元位置Ｐ_ｓ ^ｎとして算出する。

式（４）で表される二乗誤差が最小となるＰ_ｓ ^ｎを算出する方法も複数存在するが、例えば、非特許文献１に開示されている「Direct Least Squares Algorithm」などの方法を用いることができる。
この実施の形態１では、球体が真球である例を説明しているが、物体１０が真球ではない楕円体であってもよく、この場合は、楕円体の形状を表す方程式を使用するようにすればよい。
［非特許文献１］
David Eberly著、“Least Squares Fitting of Data,”1999年7月15日発行、https://www.geometrictools.com/Documentation/LeastSquaresFitting.pdf

なお、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの中には、ロボットハンド１により把持されている物体１０の３次元位置以外の不要な位置の計測結果も含まれる場合がある。
この場合には、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの中から、不要な位置の計測結果を除去する必要がある。不要な位置の計測結果の除去は、人手によって除去するようにしてもよいが、例えば、以下の非特許文献２に記載されている方法を用いて、除去するようにしてもよい。
非特許文献２には、Ｍ点の３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎの中から、ロボットハンド１により把持されている物体１０の領域の計測結果を自動的に抽出する方法が記載されている。
［非特許文献２］
Anas Abuzainaら著、“Sphere Detection in Kinect Point Clouds via the 3D Hough Transform,” Computer Analysis of Images and Patterns, vol. 8048, pp. 290〜297, Springer, 2013年発行

変換係数算出部１３は、位置姿勢情報取得部１１により特定された物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎと、センサ情報取得部１２により特定された物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎとを取得し、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存する（図４のステップＳＴ４）。
変換係数算出部１３は、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図４のステップＳＴ５）。
この実施の形態１では、Ｎは３以上の整数であり、この段階では、ｎ＝１であるため、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定される。

変換係数算出部１３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定すると（図４のステップＳＴ５：ＮＯの場合）、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する（図４のステップＳＴ６）。
変換係数算出部１３からロボットハンド１の位置又は姿勢の切り替えを指示する制御信号が出力されると、ステップＳＴ１〜ＳＴ５の処理が繰り返される。

即ち、ロボット制御部２は、変換係数算出部１３から制御信号を受けると、その制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置及び姿勢を切り替える制御を行う。
これにより、ロボットハンド１により把持されている物体１０は、ロボット座標系でＰ_ｒ ^ｎの位置に移動される（図４のステップＳＴ１）。例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが２であれば、ロボット座標系でＰ_ｒ ^２の位置に移動される。例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが３であれば、ロボット座標系でＰ_ｒ ^３の位置に移動される。
位置姿勢情報取得部１１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎを特定する（図４のステップＳＴ２）。

センサ３は、ロボット制御部２によって、ロボット座標系でＰ_ｒ ^ｎの位置に移動された物体１０を計測し、物体１０の計測結果として、物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを示すセンサ情報を出力する。
センサ情報取得部１２は、センサ３から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを特定する（図４のステップＳＴ３）。
変換係数算出部１３は、位置姿勢情報取得部１１により特定された物体１０のロボット座標系での３次元位置Ｐ_ｒ ^ｎと、センサ情報取得部１２により特定された物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎとを取得し、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存する（図４のステップＳＴ４）。
変換係数算出部１３は、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組を記憶部１３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図４のステップＳＴ５）。

変換係数算出部１３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していると判定すると（図４のステップＳＴ５：ＹＥＳの場合）、変換係数として回転行列Ｒ及び並進ベクトルｔを算出する（図４のステップＳＴ７）。
センサ座標系での３次元位置Ｐ_ｓ ^ｎをロボット座標系での３次元位置Ｐ_ｒ ^ｎに変換する際の剛体変換は、回転行列Ｒ及び並進ベクトルｔを用いると、以下の式（５）で表される。

以下、変換係数算出部１３による回転行列Ｒ及び並進ベクトルｔの算出処理を具体的に説明する。

変換係数算出部１３は、記憶部１３ａに保持されているＮ個の組を取得する。
即ち、変換係数算出部１３は、以下に示すように、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組をＮ個取得する。ｎ＝１，２，・・・，Ｎである。
Ｐ_ｒ ^１−Ｐ_ｓ ^１の組
Ｐ_ｒ ^２−Ｐ_ｓ ^２の組
：
Ｐ_ｒ ^Ｎ−Ｐ_ｓ ^Ｎの組

変換係数算出部１３は、ロボット座標系での３次元位置Ｐ_ｒ ^ｎとセンサ座標系での３次元位置Ｐ_ｓ ^ｎとの組をＮ個取得すると、以下に示すように、Ｎ個の組から回転行列Ｒ及び並進ベクトルｔを算出する。
Ｎ個の組から回転行列Ｒを算出する方法として、例えば、以下の非特許文献３に記載されている方法を用いることができる。
非特許文献３に記載されている方法は、Ｎ個の組における３次元位置Ｐ_ｒ ^ｎと３次元位置Ｐ_ｓ ^ｎを出来る限り近づける方法である。
即ち、非特許文献３に記載されている方法は、以下の式（６）を最小にする回転行列Ｒを算出する方法である。

［非特許文献３］
八木康史ら編、「コンピュータビジョン最先端ガイド３」、アドコム・メディア株式会社、2010年12月8日発行、ｐ．36〜37

変換係数算出部１３は、式（６）を最小にする回転行列Ｒを算出するため、まず、以下の式（７）で表される共分散行列Ａを算出する。

式（７）において、μ_ｒはロボット座標系での物体１０の観測点の重心座標であり、以下の式（８）で表される。
μ_ｓはセンサ座標系での物体１０の観測点の重心座標であり、以下の式（９）で表される。

変換係数算出部１３は、共分散行列Ａを算出すると、以下の式（１０）に示すように、共分散行列Ａを特異値分解する。

式（１０）において、Ｕ，Ｖはユニタリ行列であり、ユニタリ行列Ｕ，Ｖは共分散行列Ａを特異値分解することで得られる。
変換係数算出部１３は、以下の式（１１）に示すように、ユニタリ行列Ｕ，Ｖを用いて、回転行列Ｒを算出する。

式（１１）において、ｄｅｔ（Ｘ）は行列Ｘの行列式を表している。
変換係数算出部１３は、回転行列Ｒを算出すると、以下の式（１２）に示すように、回転行列Ｒと、物体１０の観測点の重心座標μ_ｒ，μ_ｓとを用いて、並進ベクトルｔを算出する。

座標変換部１４は、変換係数算出部１３により算出された回転行列Ｒ及び並進ベクトルｔを取得し、回転行列Ｒ及び並進ベクトルｔを記憶する。
その後、座標変換部１４は、センサ情報取得部１２からセンサ情報を受けると、記憶している回転行列Ｒ及び並進ベクトルｔと、そのセンサ情報が示す物体１０のセンサ座標系での３次元位置Ｐ_ｓを以下の式（１３）に代入することで、物体１０のセンサ座標系での３次元位置Ｐ_ｓをロボット座標系での３次元位置Ｐ_ｒに変換する（図４のステップＳＴ８）。

以上で明らかなように、この実施の形態１によれば、変換係数算出部１３が、ロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部１１により特定されたロボット座標系での位置とセンサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での位置との組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、センサ３がレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果を奏する。

実施の形態２．
上記実施の形態１では、ロボットハンド１により把持されている物体１０が球体である例を示したが、この実施の形態２では、ロボットハンド１により把持されている物体１０が平板である例を説明する。

図５はこの発明の実施の形態２による変換係数算出装置６を実装しているロボットシステムを示す構成図である。図５において、図１と同一符号は同一または相当部分を示すので説明を省略する。
センサ５はロボットハンド１により把持されている物体１０を計測するレーザスキャナなどの３次元センサであり、物体１０の計測結果を示すセンサ情報を出力する。
センサ５から出力されるセンサ情報は、空間中の物体１０の表面である平面のパラメータ（以下、「平面パラメータ」と称する）の集合である。センサ５による物体１０の計測方法として、ステレオビジョンに基づく方法、Ｔｉｍｅ−ｏｆ−Ｆｌｉｇｈｔ法などが知られている。
なお、センサ５は、平面パラメータの集合であるセンサ情報を出力するほか、図１のセンサ３と同様に、物体１０の表面の３次元座標（Ｘ, Ｙ, Ｚ）の集合であるセンサ情報を出力する。

変換係数算出装置６は、位置姿勢情報取得部４１、センサ情報取得部４２、変換係数算出部４３及び座標変換部４４を備えている。
図６はこの発明の実施の形態２による変換係数算出装置６を示すハードウェア構成図である。
位置姿勢情報取得部４１は例えば図６に示す位置姿勢情報取得回路５１で実現されるものであり、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での平面パラメータを特定する処理を実施する。
センサ情報取得部４２は例えば図６に示すセンサ情報取得回路５２で実現されるものであり、センサ５から物体１０のセンサ座標系での平面パラメータを示すセンサ情報を取得する処理を実施する。

変換係数算出部４３は例えば図６に示す変換係数算出回路５３で実現される。
変換係数算出部４３はロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持する記憶部４３ａを備えている。
変換係数算出部４３は、記憶部４３ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する処理を実施する。
また、変換係数算出部４３は、ロボット座標系での平面パラメータとセンサ座標系での平面パラメータとの組を保持する毎に、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する処理を実施する。

座標変換部４４は例えば図６に示す座標変換回路５４で実現される。
座標変換部４４は変換係数算出部４３により算出された変換係数である回転行列Ｒ及び並進ベクトルｔを用いて、センサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での３次元位置をロボット座標系での３次元位置に変換する処理を実施する。

図５では、変換係数算出装置６の構成要素が専用のハードウェアで実現されるものを想定している。
しかし、変換係数算出装置６の構成要素は、専用のハードウェアで実現されるものに限るものではなく、変換係数算出装置６がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
変換係数算出装置６がソフトウェア又はファームウェアなどで実現される場合、変換係数算出部４５が備えている記憶部４５ａを図３に示すコンピュータのメモリ３１上に構成するとともに、位置姿勢情報取得部４１の位置姿勢情報取得処理手順、センサ情報取得部４２のセンサ情報取得処理手順、変換係数算出部４３の変換係数算出処理手順及び座標変換部４４の座標変換処理手順をコンピュータに実行させるための変換係数算出プログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行するようにすればよい。
図７は変換係数算出装置４がソフトウェア又はファームウェアなどで実現される場合の処理手順に相当する変換係数算出方法を示すフローチャートである。

次に動作について説明する。
この実施の形態２では、物体１０である平板は、ロボット座標系で無限に広がる平面であるものと仮定し、平板の平面は、以下の式（１４）が示す平面パラメータπ_ｒで表されるものとする。

式（１４）において、ｎ_ｒは平面の法線ベクトル、ｄ_ｒはロボット座標系の原点から平面までの距離である。ロボット座標系の原点は、例えばロボットハンド１の中心位置である。

ロボットハンド１は、平板の物体１０を把持している。
ロボット制御部２は、ロボットアーム又はロボットハンド１の関節を制御することで、ロボットハンド１の位置又は姿勢を切り替える。
この実施の形態２でも、上記実施の形態１と同様に、ロボット制御部２が、制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置又は姿勢を切り替えるものとする。
また、この実施の形態２では、説明の便宜上、Ｎ（Ｎは３以上の整数）回、ロボットアーム又はロボットハンド１の関節を制御するものとする。

ロボットハンド１により把持されている物体１０の平面は、ロボット制御部２によってロボットアーム又はロボットハンド１の関節が制御されることで、ｎ（ｎ＝１，２，・・・，Ｎ）回目の制御では、以下の式（１５）に示す平面パラメータπ_ｒ ^ｎで表される平面となる（図７のステップＳＴ１１）。この段階では、ｎ＝１であるため、物体１０の平面が平面パラメータπ_ｒ ^１で表される平面となる。

位置姿勢情報取得部４１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎを特定する（図７のステップＳＴ１２）。
位置姿勢情報からロボット座標系での平面パラメータπ_ｒ ^ｎを特定する処理は公知の技術であるため詳細な説明を省略するが、例えば、以下に示すような処理で平面パラメータπ_ｒ ^ｎを特定することができる。
例えば、ロボットハンド１の位置及び姿勢と、ロボットハンド１により把持されている物体１０の平面パラメータπ_ｒ ^ｎとの対応関係を示すテーブルを事前に用意し、位置姿勢情報取得部４１が、テーブルを参照することで、位置姿勢情報が示す位置及び姿勢に対応する平面パラメータπ_ｒ ^ｎを特定する。

センサ５は、ロボット制御部２によって、位置又は姿勢が切り替えられたロボットハンド１に把持されている物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を示すセンサ情報を出力する。

この実施の形態２では、センサ５から出力されるセンサ情報は、物体１０の表面におけるＭ点の平面パラメータを示す３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎである。

センサ情報取得部４２は、センサ５から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を特定する（図７のステップＳＴ１３）。
Ｍは１以上の整数であり、Ｍ＝１の場合、π_ｓ ^ｎ＝ｐ_１ ^ｎである。
Ｍ≧２の場合、センサ情報取得部４２は、物体１０の形状を示す形状データを用いて、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎから物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎを算出する。
この実施の形態２では、ロボットハンド１が平板の物体１０を把持しているため、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎは、平板の表面上のいずれかの点である。
このため、センサ情報取得部４２は、物体１０の形状を示す形状データとして、平板の形状を表す平面の方程式を使用し、３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに平面の方程式をフィッティングすることで、物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎを得ることができる。

３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに平面の方程式をフィッティングする方法としては、複数の方法が考えられるが、上記実施の形態１と同様に、最小二乗法によるフィッティング方法を用いることができる。平面の方程式は、例えば、以下の式（１７）で表される。

式（１７）において、（ａ，ｂ，ｃ）は平面の法線ベクトルである。

変換係数算出部４３は、位置姿勢情報取得部４１により特定された物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎと、センサ情報取得部４２により特定された物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎとを取得し、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存する（図７のステップＳＴ１４）。
変換係数算出部４３は、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図７のステップＳＴ１５）。
この実施の形態２では、Ｎは３以上の整数であり、この段階では、ｎ＝１であるため、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定される。

変換係数算出部４３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していないと判定すると（図７のステップＳＴ１５：ＮＯの場合）、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する（図７のステップＳＴ１６）。
変換係数算出部４３からロボットハンド１の位置又は姿勢の切り替えを指示する制御信号が出力されると、ステップＳＴ１１〜ＳＴ１５の処理が繰り返される。

即ち、ロボット制御部２は、変換係数算出部４３から制御信号を受けると、その制御信号に従ってロボットアーム又はロボットハンド１の関節の角度を変えることで、ロボットハンド１の位置及び姿勢を切り替える制御を行う。
これにより、ロボットハンド１により把持されている物体１０の平面は、平面パラメータπ_ｒ ^ｎで表される平面となる（図７のステップＳＴ１１）。例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが２であれば、物体１０の平面が平面パラメータπ_ｒ ^２で表される平面となる、例えば、ロボットアーム又はロボットハンド１の関節を制御した回数ｎが３であれば、物体１０の平面が平面パラメータπ_ｒ ^３で表される平面となる。
位置姿勢情報取得部４１は、ロボットハンド１の位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎを特定する（図７のステップＳＴ１２）。

センサ５は、ロボット制御部２によって、位置又は姿勢が切り替えられたロボットハンド１に把持されている物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎ（ｎ＝１，２，・・・，Ｎ）を示すセンサ情報を出力する。
センサ情報取得部４２は、センサ５から出力されたセンサ情報を取得し、センサ情報から物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎを特定する（図７のステップＳＴ１３）。
変換係数算出部４３は、位置姿勢情報取得部４１により特定された物体１０のロボット座標系での平面パラメータπ_ｒ ^ｎと、センサ情報取得部４２により特定された物体１０のセンサ座標系での平面パラメータπ_ｓ ^ｎとを取得し、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存する（図７のステップＳＴ１４）。
変換係数算出部４３は、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組を記憶部４３ａに保存すると、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達したか否かを判定する（図７のステップＳＴ１５）。

変換係数算出部４３は、ロボットアーム又はロボットハンド１の関節を制御した回数ｎがＮ回に到達していると判定すると（図７のステップＳＴ１５：ＹＥＳの場合）、変換係数として回転行列Ｒ及び並進ベクトルｔを算出する（図７のステップＳＴ１７）。
ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの関係は、回転行列Ｒ及び並進ベクトルｔを用いると、以下の式（１８）で表される。

以下、変換係数算出部１３による回転行列Ｒ及び並進ベクトルｔの算出処理を具体的に説明する。

変換係数算出部４３は、記憶部４３ａに保持されているＮ個の組を取得する。
即ち、変換係数算出部４３は、以下に示すように、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をＮ個取得する。ｎ＝１，２，・・・，Ｎである。
π_ｒ ^１−π_ｓ ^１の組
π_ｒ ^２−π_ｓ ^２の組
：
π_ｒ ^Ｎ−π_ｓ ^Ｎの組

変換係数算出部４３は、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をＮ個取得すると、以下に示すように、Ｎ個の組から回転行列Ｒ及び並進ベクトルｔを算出する。
Ｎ個の組から回転行列Ｒを算出する方法として、例えば、以下の非特許文献４に記載されている方法を用いることができる。
非特許文献４に記載されている方法では、以下の式（１９）を最小にする回転行列Ｒを算出する方法である。

［非特許文献４］
Yuichi Taguchiら著、“Point-Plane SLAM for Hand-Held 3D Sensors,” Proceedings of IEEE International Conference on Robotics and Automation, pp. 5182-5189, 2013年発行

変換係数算出部４３は、回転行列Ｒを算出すると、以下の式（２０）に示す連立１次方程式を解くことで、並進ベクトルｔを求める。

座標変換部４４は、変換係数算出部４３により算出された回転行列Ｒ及び並進ベクトルｔを取得し、回転行列Ｒ及び並進ベクトルｔを記憶する。
その後、座標変換部４４は、センサ情報取得部１２から物体１０のセンサ座標系での３次元位置Ｐ_ｓを示すセンサ情報を受けると、記憶している回転行列Ｒ及び並進ベクトルｔと、センサ情報が示す３次元位置Ｐ_ｓとを以下の式（２１）に代入することで、物体１０のセンサ座標系での３次元位置Ｐ_ｓをロボット座標系での３次元位置Ｐ_ｒに変換する（図７のステップＳＴ１８）。

以上で明らかなように、この実施の形態２によれば、変換係数算出部４３が、ロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、センサ５がレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果を奏する。

実施の形態３．
上記実施の形態１では、ロボットハンド１により把持されている物体１０が球体である例を示し、上記実施の形態２では、ロボットハンド１により把持されている物体１０が平板である例をしている。ロボットハンド１により把持されている物体１０は、球体や平板に限るものではない。

この実施の形態３では、物体１０の形状が３次元モデルで表される形状であれば、物体１０の形状を示す形状データとして、３次元モデルを用いる。
例えば、角などを有する立方体のように、形状に特徴がある物体１０は、形状を３次元モデルで表すことができる。また、センサ情報が示す３次元点群データから、角などの特徴がある点を抽出することができる。
３次元モデルとしては、ロボットハンド１により把持される物体１０のＣＡＤモデル又はポリゴンデータなどを使用することができる。

上記実施の形態１では、センサ情報取得部１２が、物体１０のセンサ座標系での３次元位置Ｐ_ｓ ^ｎを算出する際、物体１０の形状を示す形状データとして、球体の形状を表す球の方程式を使用するのに対して、この実施の形態３では、物体１０の３次元モデルを使用する点で相違している。
したがって、この実施の形態３では、センサ情報取得部１２は、物体１０の形状を示す形状データとして、物体１０の３次元モデルを使用し、物体１０の角などの特徴がある点の位置を示す３次元点群データｐ_１ ^ｎ，ｐ_２ ^ｎ，・・・，ｐ_Ｍ ^ｎに物体１０の３次元モデルをフィッティングすることで、物体１０の中心の位置として、３次元位置Ｐ_ｓ ^ｎを得ることができる。
その他は、上記実施の形態１と同様であるため詳細な説明を省略する。

実施の形態４．
上記実施の形態１，２では、変換係数算出部１３又は変換係数算出部４３が、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数を算出する例を示している。
しかし、ロボットハンド１により把持される物体１０の姿勢によっては、センサ３又はセンサ５の計測結果に大きな誤差が含まれてしまうことがある。センサ３又はセンサ５の計測結果に大きな誤差が含まれていれば、センサ３又はセンサ５から大きな誤差を含むセンサ情報が変換係数算出部１３又は変換係数算出部４３に出力されるため、変換係数算出部１３又は変換係数算出部４３により算出される変換係数の精度が劣化する。
例えば、物体１０が平板である場合、平板がセンサ５と正対している状況と、平板がセンサ５と正対していない状況とを比べてみると、正対していない状況で計測されたセンサ情報の方が大きな誤差を含んでいる。
ただし、形状や材質などの物体１０の特性によっては、物体１０の姿勢が変わっても、センサ情報に含まれる誤差に変化がない場合もある。
この実施の形態４では、物体１０の姿勢によって、変換係数算出部１３又は変換係数算出部４３により算出される変換係数の精度が劣化する状況を回避することができる例を説明する。

図８はこの発明の実施の形態４による変換係数算出装置６を実装しているロボットシステムを示す構成図である。図８において、図５と同一符号は同一または相当部分を示している。
図９はこの発明の実施の形態４による変換係数算出装置６を示すハードウェア構成図である。図９において、図６と同一符号は同一または相当部分を示している。
変換係数算出装置６は、位置姿勢情報取得部４１、センサ情報取得部４２、変換係数算出部４５、座標変換部４４及び位置姿勢調整部４６を備えている。

変換係数算出部４５は例えば図９に示す変換係数算出回路５５で実現される。
変換係数算出部４５は図５の変換係数算出部４３と同様に、ロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持する記憶部４５ａを備えている。
変換係数算出部４５は図５の変換係数算出部４３と同様に、記憶部４５ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する処理を実施する。
また、変換係数算出部４５は図５の変換係数算出部４３と同様に、ロボット座標系での平面パラメータとセンサ座標系での平面パラメータとの組を保持する毎に、ロボットハンド１の位置又は姿勢の切り替えを指示する制御信号をロボット制御部２に出力する処理を実施する。

変換係数算出部４５は図５の変換係数算出部４３と異なり、位置姿勢調整部４６によりロボットハンド１の位置又は姿勢が調整されると、その後、ロボット制御部２によりロボットハンド１の位置又は姿勢が切り替えられる毎に、ロボット座標系での平面パラメータとセンサ座標系での平面パラメータとの組を記憶部４５ａに保持する。
また、変換係数算出部４５は、記憶部４５ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを再度算出する処理を実施する。
位置姿勢調整部４６は例えば図９に示す位置姿勢調整回路５６で実現される。
位置姿勢調整部４６は変換係数算出部４５により最初に変換係数が算出された後、ロボット制御部２を制御することで、ロボットハンド１の位置又は姿勢を調整する。

この実施の形態４では、図５の変換係数算出装置６に変換係数算出部４５及び位置姿勢調整部４６が適用されている例を説明するが、図１の変換係数算出装置４に適用されているものであってもよい。
図１の変換係数算出装置４に適用される場合、変換係数算出部４５は、図１の変換係数算出部１３と同様に、位置姿勢情報取得部１１により特定されたロボット座標系での３次元位置とセンサ情報取得部１２により取得されたセンサ情報が示すセンサ座標系での３次元位置との組を保持する。
また、変換係数算出部４５は、図１の変換係数算出部１３と同様に、記憶部４５ａに保持されている複数の組から、センサ座標系での３次元位置をロボット座標系での３次元位置に変換する際に用いる変換係数として、剛体変換における回転行列Ｒ及び並進ベクトルｔを算出する。

図８では、変換係数算出装置６の構成要素が専用のハードウェアで実現されるものを想定している。
しかし、変換係数算出装置６の構成要素は、専用のハードウェアで実現されるものに限るものではなく、変換係数算出装置６がソフトウェア、ファームウェア、または、ソフトウェアとファームウェアとの組み合わせで実現されるものであってもよい。
変換係数算出装置６がソフトウェア又はファームウェアなどで実現される場合、変換係数算出部４５が備えている記憶部４５ａを図３に示すコンピュータのメモリ３１上に構成するとともに、位置姿勢情報取得部４１の位置姿勢情報取得処理手順、センサ情報取得部４２のセンサ情報取得処理手順、変換係数算出部４５の変換係数算出処理手順、座標変換部４４の座標変換処理手順及び位置姿勢調整部４６の処理手順をコンピュータに実行させるための変換係数算出プログラムをメモリ３１に格納し、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行するようにすればよい。

次に動作について説明する。
この実施の形態４では、物体１０が平板である例を説明する。
変換係数算出部４５は、上記実施の形態２における図５の変換係数算出部４３と同様に、図７のステップＳＴ１１〜ＳＴ１７を実行することにより、変換係数として、回転行列Ｒ及び並進ベクトルｔを算出する。

変換係数算出部４５は、回転行列Ｒ及び並進ベクトルｔを算出した後、平板が概ねセンサ５と正対している状況となる平面パラメータπ_ｓ＝（ｎ_ｓ，ｄ_ｓ）を設定する。
即ち、変換係数算出部４５は、センサ５の光軸を表すベクトルｎ_ａと、平板の物体１０における平面の法線ベクトルｎ_ｒとのなす角が例えば５度以内になり、かつ、ロボット座標系の原点から平面までの距離ｄ_ｒがセンサ５の測距可能範囲内になるようなセンサ座標系での平面パラメータπ_ｓ＝（ｎ_ｓ，ｄ_ｓ）を設定する。

変換係数算出部４５は、既に算出している回転行列Ｒ及び並進ベクトルｔを用いて、設定したセンサ座標系での平面パラメータπ_ｓ＝（ｎ_ｓ，ｄ_ｓ）をロボット座標系での平面パラメータπ_ｒ＝（ｎ_ｒ，ｄ_ｒ）に変換する。
位置姿勢調整部４６は、物体１０の平面が、変換係数算出部４５により変換された平面パラメータπ_ｒ＝（ｎ_ｒ，ｄ_ｒ）で表される平面となるように、ロボット制御部２を制御することで、ロボットハンド１の位置又は姿勢を調整する。

変換係数算出部４５は、位置姿勢調整部４６によりロボットハンド１の位置又は姿勢が調整されると、その後、図７のステップＳＴ１１〜ＳＴ１５をＮ回実行することにより、ロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をそれぞれ記憶部４５ａに保持する。
変換係数算出部４５は、記憶部４５ａからロボット座標系での平面パラメータπ_ｒ ^ｎとセンサ座標系での平面パラメータπ_ｓ ^ｎとの組をＮ個取得し、Ｎ個の組から回転行列Ｒを再度算出する。
変換係数算出部４５は、回転行列Ｒを再度算出すると、その回転行列Ｒを用いて、並進ベクトルｔを再度算出する。

以上で明らかなように、この実施の形態４によれば、変換係数算出部４５により変換係数が算出された後、ロボットハンド１の位置又は姿勢を調整する位置姿勢調整部４６を備え、変換係数算出部４５は、位置姿勢調整部４６によりロボットハンド１の位置又は姿勢が調整された後、再度、ロボットハンド１の位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部４１により特定されたロボット座標系での平面パラメータとセンサ情報取得部４２により取得されたセンサ情報が示すセンサ座標系での平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出するように構成したので、物体１０の姿勢によって、変換係数算出部４５により算出される変換係数の精度が劣化する状況を回避することができる効果を奏する。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

この発明は、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出装置、変換係数算出方法及び変換係数算出プログラムに適している。

１ ロボットハンド、２ ロボット制御部、３，５ センサ、４，６ 変換係数算出装置、１０ 物体、１１ 位置姿勢情報取得部、１２ センサ情報取得部、１３ 変換係数算出部、１３ａ 記憶部、１４ 座標変換部、２１ 位置姿勢情報取得回路、２２ センサ情報取得回路、２３ 変換係数算出回路、２４ 座標変換回路、３１ メモリ、３２ プロセッサ、３３ 通信回路、３４ 表示装置、４１ 位置姿勢情報取得部、４２ センサ情報取得部、４３ 変換係数算出部、４３ａ 記憶部、４４ 座標変換部、４５ 変換係数算出部、４５ａ 記憶部、４６ 位置姿勢調整部、５１ 位置姿勢情報取得回路、５２ センサ情報取得回路、５３ 変換係数算出回路、５４ 座標変換回路、５５ 変換係数算出回路、５６ 位置姿勢調整回路。

この発明に係る変換係数算出装置は、物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体のロボット座標系での位置又は物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得部と、ロボットハンドに固定されている物体を計測するセンサから、物体のセンサ座標系での位置又は物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得部とを設け、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出し、センサ情報取得部により取得されたセンサ情報が、物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、物体の形状を示す形状データを用いて、３次元点群データから物体のセンサ座標系での位置又は平面パラメータを算出するようにしたものである。

この発明によれば、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出し、センサ情報取得部により取得されたセンサ情報が、物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、物体の形状を示す形状データを用いて、３次元点群データから物体のセンサ座標系での位置又は平面パラメータを算出するように構成したので、センサがレーザスキャナなどの３次元センサであっても、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出することができる効果がある。

この発明に係る変換係数算出装置は、物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、位置姿勢情報から物体のロボット座標系での位置又は物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得部と、ロボットハンドに固定されている物体を計測するセンサから、物体のセンサ座標系での位置を示すセンサ情報又は物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得部とを設け、変換係数算出部が、ロボットハンドの位置又は姿勢が切り替えられる毎に、位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータとセンサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、保持している複数の組から、センサ座標系での位置をロボット座標系での位置に変換する際に用いる変換係数を算出し、センサ情報取得部により取得されたセンサ情報が、物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、物体の形状を示す形状データを用いて、３次元点群データから物体のセンサ座標系での位置又は平面パラメータを算出するようにしたものである。

Claims (9)

1. 物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、前記位置姿勢情報から前記物体のロボット座標系での位置又は前記物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得部と、
   前記ロボットハンドに固定されている物体を計測するセンサから、前記物体のセンサ座標系での位置又は前記物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得部と、
   前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出部と
   を備えた変換係数算出装置。
2. 前記変換係数算出部により算出された変換係数を用いて、前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置を前記ロボット座標系での位置に変換する座標変換部を備えたことを特徴とする請求項１記載の変換係数算出装置。
3. 前記変換係数算出部は、前記センサ情報取得部により取得されたセンサ情報が、前記物体の表面における複数の位置又は複数の平面パラメータを示す３次元点群データであれば、前記物体の形状を示す形状データを用いて、前記３次元点群データから前記物体のセンサ座標系での位置又は平面パラメータを算出することを特徴とする請求項１記載の変換係数算出装置。
4. 前記変換係数算出部は、前記物体が球体であれば、前記物体の形状を示す形状データとして、前記球体の形状を表す球の方程式を用いることを特徴とする請求項３記載の変換係数算出装置。
5. 前記変換係数算出部は、前記物体が平板であれば、前記物体の形状を示す形状データとして、前記平板の形状を表す平面の方程式を用いることを特徴とする請求項３記載の変換係数算出装置。
6. 前記変換係数算出部は、前記物体の形状が３次元モデルで表される形状であれば、前記物体の形状を示す形状データとして、前記３次元モデルを用いることを特徴とする請求項３記載の変換係数算出装置。
7. 前記変換係数算出部により変換係数が算出された後、前記ロボットハンドの位置又は姿勢を調整する位置姿勢調整部を備え、
   前記変換係数算出部は、前記位置姿勢調整部によりロボットハンドの位置又は姿勢が調整された後、再度、前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出することを特徴とする請求項１記載の変換係数算出装置。
8. 位置姿勢情報取得部が、物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、前記位置姿勢情報から前記物体のロボット座標系での位置又は前記物体のロボット座標系での平面のパラメータである平面パラメータを特定し、
   センサ情報取得部が、前記ロボットハンドに固定されている物体を計測するセンサから、前記物体のセンサ座標系での位置又は前記物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得し、
   変換係数算出部が、前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得部により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得部により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出する
   変換係数算出方法。
9. 物体が固定されているロボットハンドの位置及び姿勢を示す位置姿勢情報を取得し、前記位置姿勢情報から前記物体のロボット座標系での位置又は前記物体のロボット座標系での平面のパラメータである平面パラメータを特定する位置姿勢情報取得処理手順と、
   前記ロボットハンドに固定されている物体を計測するセンサから、前記物体のセンサ座標系での位置又は前記物体のセンサ座標系での平面のパラメータである平面パラメータを示すセンサ情報を取得するセンサ情報取得処理手順と、
   前記ロボットハンドの位置又は姿勢が切り替えられる毎に、前記位置姿勢情報取得処理手順により特定されたロボット座標系での位置又は平面パラメータと前記センサ情報取得処理手順により取得されたセンサ情報が示すセンサ座標系での位置又は平面パラメータとの組を保持し、前記保持している複数の組から、前記センサ座標系での位置を前記ロボット座標系での位置に変換する際に用いる変換係数を算出する変換係数算出処理手順と
   をコンピュータに実行させるための変換係数算出プログラム。

Images