(57)【要約】【課題】ロボットのアームに設置されたカメラの校正を容易に行うことのできる技術を提供する。【解決手段】アーム制御部は、アームを動作させ、ロボットのロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心としてカメラを回転させて複数の回転位置で停止させる。カメラ制御部は、複数の回転位置において校正ジグの校正パターンをカメラに撮像させる。カメラ校正実行部は、複数の回転位置で撮像された校正パターン画像を用いて、校正ジグの校正ジグ座標系における座標と基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて外部パラメーターを決定する。【選択図】図５
2019155556.tif 000002
【特許請求の範囲】
【請求項１】
カメラが設置されたアームを有するロボットを制御する制御装置であって、
前記アームを制御するアーム制御部と、
前記カメラを制御するカメラ制御部と、
前記ロボットのロボット座標系における座標と前記カメラのカメラ座標系における座標を相互に変換する座標変換を計算するための前記カメラの外部パラメーターを決定するカメラ校正実行部と、
を備え、
前記アーム制御部は、前記アームを動作させ、前記ロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させ、
前記カメラ制御部は、前記複数の回転位置における校正ジグの校正パターンを前記カメラに撮像させ、
前記カメラ校正実行部は、前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記外部パラメーターを決定する、制御装置。
【請求項２】
請求項１に記載の制御装置であって、
前記カメラ校正実行部は、
（ｉ）前記複数の回転位置で撮像された前記校正パターン画像を用いて、前記校正ジグ座標系と前記基準座標系の回転を示す回転行列を推定する回転行列推定処理と、
（ｉｉ）前記複数の回転位置で撮像された前記校正パターン画像を用いて、前記校正ジグ座標系と前記基準座標系の並進移動を示す並進ベクトルを推定する並進ベクトル推定処理と、
を実行し、
（ｉｉｉ）推定された前記回転行列と前記並進ベクトルとを組み合わせることによって、前記校正ジグ座標系の座標と前記基準座標系の座標を相互に変換する座標変換行列を求める、
制御装置。
【請求項３】
請求項２に記載の制御装置であって、
前記カメラ校正実行部は、前記回転行列推定処理において、
前記複数の回転位置で撮像された前記校正パターン画像から、前記基準座標系の各座標軸の方向をベクトル方向とし前記回転の角度をベクトル長さとする３つの回転ベクトルを推定し、
前記３つの回転ベクトルを行成分又は列成分として配列することによって前記回転行列を決定する、
制御装置。
【請求項４】
請求項２又は３に記載の制御装置であって、
前記カメラ校正実行部は、前記基準座標系の各座標軸に関して、
（ａ）前記複数の回転位置のうちの第１回転位置と第２回転位置において撮像された前記校正パターン画像から前記カメラ座標系の座標と前記校正ジグ座標系の座標を相互に変換する変換行列をそれぞれ推定し、
（ｂ）前記第１回転位置と前記第２回転位置に関して推定された前記変換行列の並進ベクトル成分の差から、前記カメラ座標系から見た前記第１回転位置と前記第２回転位置における前記校正ジグ座標系の座標原点の差を表す差ベクトルを求め、
（ｃ）前記差ベクトルから、回転中心として用いた座標軸に直交する平面に前記基準座標系と前記校正ジグ座標系を投影したときの前記基準座標系と前記校正ジグ座標系の座標原点の間の距離を求め、
（ｄ）前記基準座標系の３つの座標軸について得られた前記基準座標系と前記校正ジグ座標系の座標原点の間の前記距離から、前記校正ジグ座標系と前記基準座標系の並進移動を示す並進ベクトルを算出する、制御装置。
【請求項５】
カメラが設置されたアームを有するロボットを制御する制御装置であって、
プロセッサーを備え、
前記プロセッサーは、
前記ロボットの前記アームを動作させ、ロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させ、
前記複数の回転位置における校正ジグの校正パターンを前記カメラに撮像させ、
前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記カメラの外部パラメーターを決定する、制御装置。
【請求項６】
請求項１〜５のいずれか一項に記載の制御装置に接続されたロボット。
【請求項７】
ロボットと、
前記ロボットに接続された請求項１〜５のいずれか一項に記載の制御装置と、
を備えるロボットシステム。
【請求項８】
カメラが設置されたアームを有するロボットを備えるロボットシステムにおいて前記カメラの外部パラメーターを決定するための校正を行う方法であって、
前記ロボットの前記アームを動作させ、ロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させ、
前記複数の回転位置において校正ジグの校正パターンを前記カメラに撮像させ、
前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記外部パラメーターを決定する、方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットのカメラの校正に関するものである。
【背景技術】
【０００２】
ロボットに高度な処理を行わせるために、ロボットにカメラを設置して眼の機能を持たせる場合がある。カメラの設置方法として、カメラをロボットアームとは独立して設置する方法と、カメラをロボットアームに設置する方法とがある。ロボットアームに設定されたカメラは、「ハンドアイ」と呼ばれている。ハンドアイは、より広い視野を得ることができ、また作業する手先部の視野が確保できるなどの利点がある。
【０００３】
特許文献１には、アームに設置されたカメラを利用したロボットシステムにおける座標系の校正方法が記載されている。特許文献１にも記載されているように、アームに設置されたカメラを利用する場合には、カメラ座標系とロボット座標系の間の未知の変換行列Ｘに関していわゆるＡＸ＝ＸＢ問題を解く必要があり、カメラの校正が困難であるという課題がある。ＡＸ＝ＸＢ問題の解法には、非線形最適化処理が必要であり、また、最適解が与えられる保証がない。ＡＸ＝ＸＢ問題を回避するため、特許文献１では、ロボットの動きに制限を加えることにより、座標系の変換行列を求める問題を線形化する技術が記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１２−９１２８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１に記載された技術では、処理結果として得られる変換行列が、画像を用いた校正ジグの位置推定の精度に依存するという課題がある。すなわち、校正ジグの位置推定の精度を上げるためには、ロボットの動きが大きい方が有利であるが、ロボットの大きな動きは、動きの精度が低くなるという問題がある。一方、ロボットの動きの精度を上げようとすれば、動きを小さくするのが好ましいが、この場合は画像を用いた校正パターンの位置推定の精度が低くなるという問題がある。そこで、特許文献１とは異なる方法で、アームに設置されたカメラの校正を容易に行うことのできる技術が望まれている。
【課題を解決するための手段】
【０００６】
本発明の一形態によれば、カメラが設置されたアームを有するロボットを制御する制御装置が提供される。この制御装置は、前記アームを制御するアーム制御部と；前記カメラを制御するカメラ制御部と；前記ロボットのロボット座標系における座標と前記カメラのカメラ座標系における座標を相互に変換する座標変換を計算するための前記カメラの外部パラメーターを決定するカメラ校正実行部と；を備える。前記アーム制御部は、前記アームを動作させ、前記ロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させる。前記カメラ制御部は、前記複数の回転位置における校正ジグの校正パターンを前記カメラに撮像させる。前記カメラ校正実行部は、前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記外部パラメーターを決定する。
【図面の簡単な説明】
【０００７】
【図１】ロボットシステムの概念図。
【図２】複数のプロセッサーを有する制御装置の一例を示す概念図。
【図３】複数のプロセッサーを有する制御装置の他の例を示す概念図。
【図４】ロボットと制御装置の機能を示すブロック図。
【図５】ロボットの座標系を示す説明図。
【図６】ハンドアイの校正処理で使用するロボットの座標系を示す説明図。
【図７】ハンドアイの校正処理の手順を示すフローチャート。
【図８】ハンドアイを複数の回転位置に回転させた状態を示す説明図。
【図９】複数の回転位置で撮影された校正パターン画像の例を示す説明図。
【図１０】回転行列推定処理の詳細手順を示すフローチャート。
【図１１】並進ベクトルの推定に使用するベクトルｄを示す説明図。
【図１２】並進ベクトル推定処理の詳細手順を示すフローチャート。
【発明を実施するための形態】
【０００８】
A. ロボットシステムの構成
図１は、一実施形態におけるロボットシステムの概念図である。このロボットシステムは、ロボット１００と、ロボット１００に接続された制御装置２００とを備えている。このロボット１００は、カメラで作業対象を認識し、自在に力を加減して、自律的に判断しながら作業を行える自律型ロボットである。また、ロボット１００は、予め作成された教示データに従って作業を実行するティーチングプレイバックロボットとして動作することも可能である。
【０００９】
ロボット１００は、基台１１０と、胴部１２０と、肩部１３０と、首部１４０と、頭部１５０と、２つのアーム１６０Ｌ，１６０Ｒとを備えている。アーム１６０Ｌ，１６０Ｒには、ハンド１８０Ｌ，１８０Ｒが着脱可能に取り付けられている。これらのハンド１８０Ｌ，１８０Ｒは、ワークや道具を把持するエンドエフェクターである。頭部１５０には、カメラ１７０Ｌ，１７０Ｒが設置されている。これらのカメラ１７０Ｌ，１７０Ｒは、アーム１６０Ｌ，１６０Ｒとは独立して設けられており、その位置姿勢が変化しない固定カメラである。アーム１６０Ｌ，１６０Ｒの手首部には、カメラとしてのハンドアイ１７５Ｌ，１７５Ｒが設けられている。アーム１６０Ｌ，１６０Ｒには、カメラ１７０Ｌ，１７０Ｒやハンドアイ１７５Ｌ，１７５Ｒの校正ジグ４００を設置可能である。以下では、ハンドアイ１７５Ｌ，１７５Ｒと区別するために、頭部１５０に設けられたカメラ１７０Ｌ，１７０Ｒを「固定カメラ１７０Ｌ，１７０Ｒ」とも呼ぶ。
【００１０】
アーム１６０Ｌ，１６０Ｒの手首部には、更に、力覚センサー１９０Ｌ，１９０Ｒが設けられている。力覚センサー１９０Ｌ，１９０Ｒは、ハンド１８０Ｌ，１８０Ｒがワークに及ぼしている力に対する反力やモーメントを検出するセンサーである。力覚センサー１９０Ｌ，１９０Ｒとしては、例えば、並進３軸方向の力成分と、回転３軸回りのモーメント成分の６成分を同時に検出することができる６軸力覚センサーを用いることができる。なお、力覚センサー１９０Ｌ，１９０Ｒは省略可能である。
【００１１】
アーム１６０Ｌ，１６０Ｒと、カメラ１７０Ｌ，１７０Ｒと、ハンドアイ１７５Ｌ、１７５Ｒと、ハンド１８０Ｌ，１８０Ｒと、力覚センサー１９０Ｌ，１９０Ｒの符号の末尾に付された「Ｌ」「Ｒ」の文字は、それぞれ「左」「右」を意味する。これらの区別が不要の場合には、「Ｌ」「Ｒ」の文字を省略した符号を使用して説明する。
【００１２】
制御装置２００は、プロセッサー２１０と、メインメモリー２２０と、不揮発性メモリー２３０と、表示制御部２４０と、表示部２５０と、Ｉ／Ｏインターフェース２６０とを有している。これらの各部は、バスを介して接続されている。プロセッサー２１０は、例えばマイクロプロセッサー又はプロセッサー回路である。制御装置２００は、Ｉ／Ｏインターフェース２６０を介してロボット１００に接続される。なお、制御装置２００をロボット１００の内部に収納してもよい。
【００１３】
制御装置２００の構成としては、図１に示した構成以外の種々の構成を採用することが可能である。例えば、プロセッサー２１０とメインメモリー２２０を図１の制御装置２００から削除し、この制御装置２００と通信可能に接続された他の装置にプロセッサー２１０とメインメモリー２２０を設けるようにしてもよい。この場合には、当該他の装置と制御装置２００とを合わせた装置全体が、ロボット１００の制御装置として機能する。他の実施形態では、制御装置２００は、２つ以上のプロセッサー２１０を有していてもよい。更に他の実施形態では、制御装置２００は、互いに通信可能に接続された複数の装置によって実現されていてもよい。これらの各種の実施形態において、制御装置２００は、１つ以上のプロセッサー２１０を備える装置又は装置群として構成される。
【００１４】
図２は、複数のプロセッサーによってロボットの制御装置が構成される一例を示す概念図である。この例では、ロボット１００及びその制御装置２００の他に、パーソナルコンピューター５１０，５２０と、ＬＡＮなどのネットワーク環境を介して提供されるクラウドサービス５００とが描かれている。パーソナルコンピューター５１０，５２０は、それぞれプロセッサーとメモリーとを含んでいる。また、クラウドサービス５００においてもプロセッサーとメモリーを利用可能である。これらの複数のプロセッサーの一部又は全部を利用して、ロボット１００の制御装置を実現することが可能である。
【００１５】
図３は、複数のプロセッサーによってロボットの制御装置が構成される他の例を示す概念図である。この例では、ロボット１００の制御装置２００が、ロボット１００の中に格納されている点が図２と異なる。この例においても、複数のプロセッサーの一部又は全部を利用して、ロボット１００の制御装置を実現することが可能である。
【００１６】
図４は、ロボット１００と制御装置２００の機能を示すブロック図である。制御装置２００のプロセッサー２１０は、不揮発性メモリー２３０に予め格納された各種のプログラム命令２３１を実行することにより、アーム制御部２１１と、カメラ制御部２１２と、カメラ校正実行部２１３との機能をそれぞれ実現する。カメラ校正実行部２１３は、変換行列推定部２１４を有している。但し、これらの各部２１１〜２１４の機能の一部又は全部をハ―ドウェア回路で実現しても良い。これらの各部２１１〜２１４の機能については後述する。
【００１７】
不揮発性メモリー２３０には、プログラム命令２３１の他に、カメラ内部パラメーター２３２とカメラ外部パラメーター２３３とが格納される。これらのパラメーター２３２，２３３は、固定カメラ１７０のパラメーターと、ハンドアイ１７５のパラメーターをそれぞれ含んでいる。なお、本実施形態では、固定カメラ１７０の内部パラメーターと外部パラメーターは制御装置２００にとって既知であるものと仮定する。また、ハンドアイ１７５の内部パラメーターは既知であり、外部パラメーターは未知であるものと仮定する。後述する校正処理では、ハンドアイ１７５の外部パラメーターが生成される。内部パラメーター２３２と外部パラメーター２３３の内容については更に後述する。
【００１８】
B. ロボットの座標系と座標変換
図５は、ロボット１００のアーム１６０の構成と各種の座標系を示す説明図である。２つのアーム１６０Ｌ，１６０Ｒのそれぞれには、７つの関節Ｊ１〜Ｊ７が設けられている。関節Ｊ１，Ｊ３，Ｊ５，Ｊ７はねじり関節であり、関節Ｊ２，Ｊ４，Ｊ６は曲げ関節である。なお、図１の胴部１２０と肩部１３０の間にはねじり関節が設けられているが、図５では図示が省略されている。個々の関節には、それらを動作させるためのアクチュエーターと、回転角度を検出するための位置検出器とが設けられている。
【００１９】
アーム１６０の手先には、ツール中心点ＴＣＰ（Tool Center Point）が設定されている。典型的には、ロボット１００の制御は、ツール中心点ＴＣＰの位置姿勢を制御するために実行される。なお、位置姿勢（position and orientation）とは、３次元の座標系における３つ座標値と、各座標軸回りの回転とで規定される状態を意味する。
【００２０】
アーム１６０Ｌ，１６０Ｒには、校正ジグ４００を予め定めた設置状態で設置可能である。図５の例では、左アーム１６０Ｌのハンドアイ１７５の校正に使用される校正ジグ４００が、右アーム１６０Ｒの手先部に固定されている。校正ジグ４００を右アーム１６０Ｒに取り付ける際には、右アーム１６０Ｒのハンド１８０Ｒを取り外すようにしてもよい。左アーム１６０Ｌのハンド１８０Ｌも同様である。後述するように、校正ジグ４００をアーム１６０に取り付ける必要は無く、床面等に設置してもよい。但し、校正ジグ４００をアーム１６０のハンド１８０に取り付られるようにすれば、ハンドアイ１７５やカメラ１７０の校正が容易になるという利点がある。
【００２１】
実施形態におけるハンドアイ１７５の校正は、ハンドアイ１７５の内部パラメーターと外部パラメーターのうち、外部パラメーターを推定する処理である。内部パラメーターは、ハンドアイ１７５及びそのレンズ系の固有のパラメーターであり、例えば射影変換パラメーターや歪パラメーターなどを含んでいる。ハンドアイ１７５の内部パラメーターは、校正ジグ４００を複数の位置姿勢で撮影して得られる複数の校正パターン画像を用いて、カメラの校正を行う標準的なソフトウェアであるOpenCVやMATLABのカメラキャリブレーション関数を用いて実行可能である。以下では、ハンドアイ１７５の内部パラメーターは予め決定されているものと仮定する。ハンドアイ１７５の外部パラメーターは、ハンドアイ１７５のカメラ座標系ΣCとロボット１００のロボット座標系ΣRの間の相対的な位置姿勢を計算する際に使用されるパラメーターである。
【００２２】
図５には、ロボット１００に関する座標系として、以下の座標系が描かれている。
（１）ロボット座標系ΣR：ロボット１００の基準点を座標原点とする座標系
（２）手首座標系ΣW：アーム１６０の手首を座標原点とする座標系
（３）校正ジグ座標系ΣJ：校正ジグ４００上の所定の位置を座標原点とする座標系
（４）カメラ座標系ΣC：ハンドアイ１７５に設定された座標系
なお、本明細書で使用する各座標系は、直交する３つの座標軸を有する直交座標系である。
【００２３】
手首座標系ΣWは、アーム１６０のうちでハンドアイ１７５が設置されたアーム部分の座標系である。但し、ハンドアイ１７５は、アーム１６０の手首部以外の部分に設置してもよい。また、ハンドアイ１７５は、アーム１６０に着脱可能な状態で設置されていてもよい。ハンドアイ１７５が設定されているアーム部分を「カメラ設置部位」とも呼ぶ。手首座標系ΣWは、アーム１６０のカメラ設置部位に設定されたカメラ設置部位座標系の一種である。手首座標系ΣWとカメラ座標系ΣCは、２つのハンドアイ１７５Ｌ，１７５Ｒにそれぞれ別個に設定される。以下の説明では、左アーム１６０Ｌのハンドアイ１７５Ｌを校正対象とするので、手首座標系ΣW及びカメラ座標系ΣCとして左アーム１６０Ｌ用の座標系を使用する。なお、図５では、図示の便宜上、校正ジグ座標系ΣJの原点が実際の位置からずれた位置に描かれている。
【００２４】
一般に、或る座標系ΣAから他の座標系ΣBへの変換、又は、これらの座標系での位置姿勢の変換は、以下に示す同次変換行列AΣB（Homogeneous transformation matrix）で表現される。
【数１】
2019155556.tif 000003
ここで、Rは回転行列（Rotation matrix）、Tは並進ベクトル（Translation vector）、Rx，Ry，Rzは回転行列Rの列成分である。以下では、同次変換行列AΣBを「座標変換行列AΣB」、「変換行列AΣB」又は、単に「変換AΣB」とも呼ぶ。変換の符号「AΣB」の左側の上付き文字「A」は変換前の座標系を示し、右側の下付き文字「B」は変換後の座標系を示す。なお、変換AΣBは、座標系ΣAで見た座標系ΣBの基底ベクトルの成分と原点位置を表すものと考えることも可能である。
【００２５】
変換AΣBの逆行列AΣB-1（＝BΣA）は、次式で与えられる。
【数２】
2019155556.tif 000004
【００２６】
回転行列Rは、以下の重要な性質を有する。
＜回転行列Rの性質１＞
回転行列Rは正規直交行列であり、その逆行列R-1は転置行列ＲTに等しい。
＜回転行列Rの性質２＞
回転行列Rの３つの列成分Rx，Ry，Rzは、元の座標系ΣAで見た回転後の座標系ΣBの３つの基底ベクトルの成分に等しい。
【００２７】
或る座標系ΣAに変換AΣB，BΣCを順次施す場合に、合成された変換AΣCは各変換AΣB，BΣCを順次右側に乗じたものとなる。
【数３】
2019155556.tif 000005
【００２８】
また、回転行列Rに関しても（３）式と同様の関係が成立する。
【数４】
2019155556.tif 000006
【００２９】
C. 座標変換のＡＸ＝ＸＢ問題
図５において、座標系ΣR，ΣW，ΣC，ΣJの間には、以下の変換が成立する。
（１）変換RΣW：ロボット座標系ΣRから手首座標系ΣWへの変換
（２）変換WΣC：手首座標系ΣWからカメラ座標系ΣCへの変換
（３）変換CΣJ：カメラ座標系ΣCから校正ジグ座標系ΣJへの変換
（４）変換JΣR：校正ジグ座標系ΣJからロボット座標系ΣRへの変換
【００３０】
１番目の変換RΣWは、ロボット座標系ΣRから手首座標系ΣWへの変換である。通常、ロボット座標系ΣRに対するアームの各部位の位置姿勢を求める処理はフォワードキネマティクスと呼ばれ、アーム１６０の幾何学的な形状と各関節の動作量である回転角度が定まれば計算できる。つまり、この変換RΣWは計算可能な変換である。
【００３１】
２番目の変換WΣCは、手首座標系ΣWからカメラ座標系ΣCへの変換である。この変換WΣCは未知であり、この変換WΣCを求めることがハンドアイ１７５の校正に相当する。一般には、カメラ設置部位座標系とカメラ座標系ΣCの間の変換行列を求めることが、ハンドアイ１７５の校正に相当する。
【００３２】
３番目の変換CΣJは、カメラ座標系ΣCから校正ジグ座標系ΣJへの変換である。この変換CΣJは、ハンドアイ１７５によって校正ジグ４００を撮影して校正パターン画像を取得し、その校正パターン画像に対して画像処理を行うことによって推定できる。この変換CΣJを推定する処理は、カメラの校正を行うための標準的なソフトウェアであるOpenCVやMATLABのカメラキャリブレーション関数を用いて実行可能である。
【００３３】
４番目の変換JΣRは、校正ジグ座標系ΣJからロボット座標系ΣRへの変換である。この変換JΣRは未知である。
【００３４】
上記の変換RΣW，WΣC，CΣJ，JΣRを順に辿れば最初のロボット座標系ΣRに戻るので、恒等変換Ｉを用いて以下の式が成立する。
【数５】
2019155556.tif 000007
【００３５】
（５）式の両辺に、左から各変換の逆行列RΣW-1 , WΣC -1 , CΣJ -1 を順に乗算すれば次式が得られる。
【数６】
2019155556.tif 000008
【００３６】
（６）式において、変換CΣJはカメラキャリブレーション関数を用いて推定でき、変換RΣWは計算可能である。従って、変換WΣCが既知であれば、右辺は計算可能であり、左辺の変換JΣRを知ることができる。
【００３７】
一方、変換WΣCが未知であれば、（６）式の右辺は計算できず、別の処理が必要となる。例えば、図５において左アーム１６０Ｌの２つの姿勢ｉ，ｊを考えれば、それぞれにおいて上記（５）式が成立し、次式となる。
【数７】
2019155556.tif 000009
【００３８】
（７ａ），（７ｂ）式の両辺にそれぞれ右から変換JΣRの逆行列JΣR-1 を乗算すれば次式が得られる。
【数８】
2019155556.tif 000010
【００３９】
（８ａ），（８ｂ）式の右辺は未知ではあるが同じ変換なので、次式が成立する。
【数９】
2019155556.tif 000011
【００４０】
（９）式の両辺に、左からRΣW (j)−１を、右からCΣJ(i)−１を乗算すると次式となる。
【数１０】
2019155556.tif 000012
【００４１】
ここで、（１０）式の左辺と右辺の中括弧｛ ｝の中の変換の積をそれぞれＡ，Ｂと書き、未知の変換WΣCをＸと書くと、次式が得られる。
【数１１】
2019155556.tif 000013
【００４２】
これは、ＡＸ＝ＸＢ問題として良く知られた処理であり、未知の行列Ｘを解くためには非線形の最適化処理が必要となる。しかし、この非線形最適化処理には、最適な解に収束する保証が無いと言う問題がある。
【００４３】
以下に詳述するように、実施形態では、ハンドアイ１７５が設置されたアーム１６０を任意に制御可能であることを利用し、ハンドアイ１７５を特定の基準座標系の座標軸回りに回転させた複数の回転位置において校正ジグ４００をハンドアイ１７５で撮影して複数の校正パターン画像を取得し、これらの複数の校正パターン画像を用いてハンドアイ１７５の外部パラメーターを推定する。
【００４４】
図６は、実施形態の校正処理で使用する座標系を示している。ここでは、１本のアーム１６０のみを簡略化して描いている。実施形態では、ロボット座標系ΣRに対して、制御装置２００にとって既知である相対位置姿勢を有する基準座標系ΣBを使用する。この基準座標系ΣBは、ハンドアイ１７５の校正を行うために便宜的に設定する座標系であり、ロボット座標系ΣRに対して既知の相対位置姿勢を有する限り、どのような位置に設定してもよい。例えば、ロボット座標系ΣRそのものを基準座標系ΣBとして使用してもよい。後述するように、ハンドアイ１７５の校正処理において、ハンドアイ１７５が基準座標系ΣBの各座標軸を回転中心として回転した複数の回転位置を取るようにアーム１６０が制御され、それぞれの回転位置において校正ジグ４００の校正パターン画像が取得される。このため、基準座標系ΣBは、ロボット座標系ΣRと異なる位置に設ける方が便利である。以下の説明では、基準座標系ΣBをロボット座標系ΣRと異なる位置に設定した例について説明する。但し、基準座標系ΣBをロボット座標系ΣRと同じ座標系としても、処理内容は同一である。なお、図６から理解できるように、以下で説明する実施形態は、双腕ロボットに限らず、単腕ロボットにも適用可能である。また、校正ジグ４００を移動させる必要が無いので、校正ジグ４００をロボットアームで保持する必要はなく、任意の場所に設置可能である。
【００４５】
図６に示した座標系間の変換に関しては、恒等変換Ｉを用いて以下の式が成立する。
【数１２】
2019155556.tif 000014
【００４６】
上記（１２）式の右辺のうち、カメラ座標系ΣCと、カメラ設置部位座標系としての手首座標系ΣWの間の変換CΣWが、校正で求めるべきハンドアイ１７５の外部パラメーターである。（１２）式は以下のように変形できる。
【数１３】
2019155556.tif 000015
【００４７】
上記（１３）式の右辺の変換のうちで、校正ジグ座標系ΣJと基準座標系ΣBの間の変換JΣBのみが未知であり、他の変換は既知又は推定可能である。そこで、以下で説明するハンドアイ１７５の校正処理では、この変換JΣBを求め、上記（１３）式に従ってハンドアイ１７５の外部パラメーターである変換CΣWを決定する。
【００４８】
D. 実施形態の校正処理手順
図７は、ハンドアイ１７５の校正処理の手順を示すフローチャートである。以下で説明する校正処理は、図４に示したアーム制御部２１１と、カメラ制御部２１２と、カメラ校正実行部２１３とが協調して実行される。すなわち、ハンドアイ１７５を複数の位置姿勢に変更する動作は、アーム制御部２１１がアーム１６０を制御することによって実現される。また、ハンドアイ１７５による撮像は、カメラ制御部２１２によって制御される。ハンドアイ１７５の外部パラメーターは、カメラ校正実行部２１３によって決定される。また、ハンドアイ１７５の外部パラメーターの決定において、各種の行列やベクトルの推定は、変換行列推定部２１４によって行われる。なお、ハンドアイ１７５の内部パラメーターは、図７の校正処理の前に予め決定されているものと仮定する。
【００４９】
ステップＳ１２０では、アーム１６０を用いて、ハンドアイ１７５を基準座標系ΣBの座標原点を回転中心として３つの座標軸の回りにそれぞれ回転させた複数の回転位置で停止させ、複数の回転位置において校正ジグ４００をハンドアイ１７５で撮影する。以下では、校正ジグ４００をハンドアイ１７５で撮像して得られた画像を「校正パターン画像」と呼ぶ。
【００５０】
図８は、ステップＳ１２０において、ハンドアイ１７５を複数の回転位置に回転させた状態を示す説明図であり、図９は各回転位置においてハンドアイ１７５で撮影された校正パターン画像を示す説明図である。図８では、基準座標系ΣBの３つの座標軸をＸ軸，Ｙ軸，Ｚ軸としており、座標軸Ｘの回りにハンドアイ１７５を回転させることによって、３つの回転位置で静止するようにアーム１６０が制御されている。３つの回転位置のうちの中央の位置は基本回転位置であり、他の２つの回転位置は基本回転位置から回転角度φ1及びφ2だけ回転した位置である。なお、基本回転位置における回転角はゼロと定義する。基本回転位置における基準座標系ΣBと手首座標系ΣWの間の変換BΣW(0)は、フォワードキネマティクスから算出可能である。他の回転位置における変換BΣW(φ1)，変換BΣW(φ2)も同様に算出可能である。
【００５１】
図８において、基本回転位置からの回転角度φ1，φ2は、例えば＋５度及び−５度に設定されている。但し、回転角度φ1，φ2としては、０度でない任意の角度を採用可能である。但し、回転角度φ1，φ2が過度に小さいと、回転による校正パターン画像の違いが識別し難くなり、回転角度φ1，φ2が過度に大きいと校正パターン画像から校正ジグ座標系ΣJを推定し難くなる。これらの点を考慮すると、回転角度φ1，φ2は、例えば３度以上３０度以下の範囲に設定することが好ましい。なお、図８の例ではＸ軸回りの回転位置を３つ設定しているが、回転位置としては、各座標軸について、基本回転位置と１つ以上の他の回転位置とが設定されていれば良い。Ｙ軸及びＺ軸についても、Ｘ軸と同様に、それぞれの軸を回転中心としてハンドアイ１７５を回転させた複数の回転位置において校正ジグ４００を撮影することによって校正パターン画像を取得する。なお、３つの座標軸における基本回転位置は、同一としてもよい。
【００５２】
なお、基本回転位置は、少なくとも１つ設定すれば良いが、ハンドアイ１７５の撮像範囲のほぼ全体にわたるように複数の基本回転位置を設定することが好ましい。こうすれば、ハンドアイ１７５の撮像範囲に亘ってその外部パラメーターをより正確に決定することが可能である。基本回転位置を複数設定する場合には、各基本回転位置における校正パターン画像と、各基本回転位置から３つの座標軸Ｘ，Ｙ，Ｚの回りにそれぞれ回転させた複数の回転位置における複数の校正パターン画像とがそれぞれ撮像される。
【００５３】
校正ジグ４００は、例えば、黒丸を格子状に配置した校正パターンを有する。但し、校正ジグ４００は、図９のように白丸を格子状に配置した校正パターンを使用してもよいし、チェッカーボードパターンのような他の校正パターンを使用してもよい。校正ジグ座標系ΣJの座標原点は、校正ジグ４００上の予め定められた位置にある。
【００５４】
ステップＳ１４０〜ステップＳ１８０は、変換行列推定部２１４が、ステップＳ１２０で撮像された複数の校正パターン画像を用いて、校正ジグ座標系ΣJと基準座標系ΣBの間の変換JΣBを推定する処理である。すなわち、ステップＳ１４０では変換JΣBを構成する回転行列JRBを推定し、ステップＳ１６０では変換JΣBを構成する並進ベクトルJTBを推定し、ステップＳ１８０では回転行列JRBと並進ベクトルJTBを組み合わせて変換JΣBを求める。ステップＳ１４０，Ｓ１６０の詳細手順については後述する。ステップＳ２００では変換行列推定部２１４が、ステップＳ１８０で得られた変換JΣBを用い、上記（１３）式に従ってハンドアイ１７５の外部パラメーターである変換CΣWを求める。
【００５５】
図１０は、図７のステップＳ１４０における回転行列JRBの推定処理の詳細手順を示すフローチャートである。図８及び図９で説明したように、ステップＳ１４０の前には、基準座標系ΣBの３つの座標軸のそれぞれについて、基本回転位置での校正パターン画像と、１つ以上の他の回転位置での校正パターン画像とが取得されている。
【００５６】
ステップＳ１４１では、基準座標系ΣBの３つの座標軸について、ステップＳ１４２〜Ｓ１４６の処理がすべて終了したか否かが判定される。終了した場合には後述するステップＳ１４７に進み、終了していなければ処理対象とする座標軸を選択してステップＳ１４２に進む。
【００５７】
ステップＳ１４２では、処理対象とする座標軸回りの複数の回転位置のうち、基本回転位置におけるカメラ座標系ΣCと校正ジグ座標系ΣJの変換CΣJ(0)を求める。この変換CΣJ(0)の括弧内の「0」は回転角度を示す。この変換CΣJ(0)は、基本回転位置における校正パターン画像から、カメラの外部パラメーターを推定する標準的なソフトウェアであるOpenCVの関数「FindExtrinsicCameraParams2」等を用いて推定することが可能である。
【００５８】
ステップＳ１４３では、処理対象とする座標軸について、ステップＳ１４４〜Ｓ１４６の処理がすべて終了したか否かが判定される。終了していればステップＳ１４１に戻り、終了していなければ処理対象とする回転角度φjを選択してステップＳ１４４に進む。以下では主として基準座標系ΣBのＸ軸を処理対象とした場合についてステップＳ１４４〜Ｓ１４６の処理を説明する。
【００５９】
ステップＳ１４４では、回転角度φjにおける変換CΣJ(φj)を求める。この変換CΣJ(φj)も、回転角度φjにおける校正パターン画像から、カメラの外部パラメーターを推定する標準的なソフトウェアを用いて推定することが可能である。
【００６０】
ステップＳ１４５では、ステップＳ１４２，Ｓ１４４で得られた２つの変換CΣJ(0)，CΣJ(φj)を用いて、次式に従って回転行列R(φj)を推定する。
【数１４】
2019155556.tif 000016
【００６１】
上記（１４ａ）式において、CRJ(0)，CRJ(φj)は変換CΣJ(0)，CΣJ(φj)の回転行列成分である。また、回転行列R(φj)は、基本回転位置からφjだけ座標系を回転させる回転行列である。（１４ｂ）式に示すように、回転行列R(φj)は、基本回転位置における回転行列CRJ(0)の逆行列CRJ(0)-1と、基本回転位置からφjだけ回転させた位置における回転行列CRJ(φj)との積として計算できる。
【００６２】
ステップＳ１４６では、回転行列R(φj)から、校正ジグ座標系ΣJから見た基準座標系ΣBのＸ軸の方向を示す座標軸ベクトル[n0，n1，n2]Tを求める。この計算は、以下のように行われる。
【００６３】
一般に、座標系の３つの軸回りの任意の回転は、回転行列や３つのオイラー角で表現する場合が多いが、その代わりに、１つの回転軸と、その回転軸回りの回転角度とで表現することも可能である。後者の表現を利用すると、回転行列R(φj)は、次式で与えられる回転ベクトルRod(φj)に変換できる。
【数１５】
2019155556.tif 000017
ここで、n0，n1，n2は、回転軸の方向を示す単位ベクトルを表す３軸のベクトル成分である。すなわち、「回転ベクトルRod」は、回転軸の方向をベクトル方向とし、回転の角度をベクトル長さとするベクトルである。回転行列R(φj)から回転ベクトルRod(φj)への変換は、例えば、OpenCVの関数「Rodrigues2」を用いて実行することが可能である。
【００６４】
上述したように、回転行列R(φj)は、基本回転位置から基準座標系ΣBのＸ軸の回りにφjだけ座標系を回転させることを表す行列である。従って、回転行列R(φj)と等価な回転ベクトルRod(φj)のベクトル方向は、回転軸の方向、すなわち、校正ジグ座標系ΣJから見た基準座標系ΣBのＸ軸の方向を示している。
【００６５】
ステップＳ１４３〜ステップＳ１４６を繰り返すことにより、各座標軸回りの回転を表す回転ベクトルRodX(φj)，RodY(φj)，RodZ(φj)を得ることができる。これらの回転ベクトルRodX(φj)，RodY(φj)，RodZ(φj)は、次式に示すように、校正ジグ座標系ΣJから見た基準座標系ΣBの座標軸Ｘ，Ｙ，Ｚの方向を示す座標軸ベクトル[nX0，nX1，nX2]T，[nY0，nY1，nY2]T，[nZ0，nZ1，nZ2]Tで表現される。
【数１６】
2019155556.tif 000018
【００６６】
ステップＳ１４７では、次式に示すように、座標軸Ｘ，Ｙ，Ｚの方向を示す座標軸ベクトルを列成分として配列することによって、校正ジグ座標系ΣJから基準座標系ΣBへの回転行列JRBを得ることができる。
【数１７】
2019155556.tif 000019
【００６７】
なお、回転行列JRBの逆変換BRJは、回転行列JRBの転置行列に等しい。従って、座標軸ベクトル[nX0，nX1，nX2]T，[nY0，nY1，nY2]T，[nZ0，nZ1，nZ2]Tを列成分として配列する代わりに、行成分として配列すれば、基準座標系ΣBから校正ジグ座標系ΣJへの回転行列BRJを得ることが可能である。
【００６８】
ステップＳ１４８では、ステップＳ１４７で得られた回転行列JRBを正規直交化する。正規直交化を行う理由は、ステップＳ１４３〜Ｓ１４６で得られた３つの座標軸ベクトルは独立に推定されたものであり、また推定には誤差が発生するので、ステップＳ１４７で求めた回転行列JRBが正規直交していることが保証されていないからである。一方、回転行列は正規直交行列であることが要求されるので、ステップＳ１４７で求めた回転行列JRBが正規直交となるように補正をすることが好ましい。正規直交化を行う方法としては、Gram-Schmidt直交化法やFrobenius法などの任意の方法を用いることが可能である。但し、ステップＳ１４７で求められた回転行列JRBが正規直交に十分近いものである場合には、ステップＳ１４８は省略可能である。
【００６９】
以上のように、図１０に示した回転行列推定処理では、基準座標軸ΣBの３つの座標軸のそれぞれを回転中心としてハンドアイ１７５をそれぞれ回転させた複数の回転位置で撮影された複数の校正パターン画像を用いて、校正ジグ座標系ΣJと基準座標系ΣBの変換行列JΣBの成分である回転行列JRBを得ることが可能である。特に、図１０の回転行列推定処理では、複数の回転位置で撮像された校正パターン画像から、基準座標系ΣBの各座標軸の方向をベクトル方向とし回転の角度をベクトル長さとする３つの回転ベクトルを推定し、これらの３つの回転ベクトルを行成分又は列成分として配列することによって回転行列JRBを決定する。従って、複数の回転位置で撮像された校正パターン画像から、校正ジグ座標系ΣJと基準座標系ΣBの間の回転行列JRBを容易に求めることが可能である。
【００７０】
図１１は、図７のステップＳ１６０における並進ベクトルJTBの推定に使用するベクトルｄを示す説明図である。ここでは、図８に示した基本回転位置おける基準座標系ΣB(0)と校正ジグ座標系ΣJ(0)を実線で示し、基本回転位置から回転角φ1だけ回転させた回転位置における基準座標系ΣB(φ1)と校正ジグ座標系ΣJ(φ1)を示している。但し、これらの座標系は、ハンドアイ１７５を固定した視点で見た状態を示している。これらの２つの回転位置におけるカメラ座標系ΣCと校正ジグ座標系ΣJの間の変換をCΣJ(0), CΣJ(φ1)とし、これらの変換CΣJ(0), CΣJ(φ1)の並進ベクトル成分をCTJ(0), CTJ(φ1)とする。このとき、２つの回転位置における校正ジグ座標系ΣJ(0)，ΣJ(φ1)の座標原点の差を表す差ベクトルｄは、２つの並進ベクトルCTJ(0), CTJ(φ1)を用いて次式で与えられる。
【数１８】
2019155556.tif 000020
【００７１】
図１２は、図７のステップＳ１６０における並進ベクトルJTBの推定処理の詳細手順を示すフローチャートである。
【００７２】
ステップＳ１６１では、基準座標系ΣBの３つの座標軸について、ステップＳ１６２〜Ｓ１６７の処理がすべて終了したか否かが判定される。終了した場合には後述するステップＳ１６８に進み、終了していなければ処理対象とする座標軸を選択してステップＳ１６２に進む。
【００７３】
ステップＳ１６２では、処理対象とする座標軸回りの複数の回転位置のうち、基本回転位置におけるカメラ座標系ΣBと校正ジグ座標系ΣJの変換CΣJ(0)を取得する。この変換CΣJ(0)は、図１０のステップＳ１４２で求められていたものと同じである。
【００７４】
ステップＳ１６３では、処理対象とする座標軸について、ステップＳ１６４〜Ｓ１６７の処理がすべて終了したか否かが判定される。終了していればステップＳ１６１に戻り、終了していなければ処理対象とする回転角度φjを選択してステップＳ１６４に進む。以下では主として基準座標系ΣBのＸ軸を処理対象とした場合についてステップＳ１６４〜Ｓ１６７の処理を説明する。
【００７５】
ステップＳ１６４では、回転角度φjにおける変換CΣJ(φj)を取得する。この変換CΣJ(φj)は、図１０のステップＳ１６２で求められていたものと同じである。
【００７６】
ステップＳ１６６では、ステップＳ１６２，Ｓ１６４で取得された変換CΣJ(0)，CΣJ(φj)の並進ベクトル成分を使用して、上述した（１８）式に従って差ベクトルｄを求める。
【００７７】
ステップＳ１６７では、差ベクトルｄの非直交成分を補正する。図１１に示すように、差ベクトルｄは、基準座標系ΣBのＹＺ平面上にある。従って、図１０で説明した回転行列の推定処理で求められた座標軸Ｘの回転軸ベクトル[nX0，nX1，nX2]Tと、差ベクトルｄは直交しているはずである。しかし、誤差などの状況により、実際には両者は直交していない可能性がある。そのため、ステップＳ１６５で得られた差ベクトルｄの非直交成分を補正することが好ましい。この補正は、次式で与えられる。
【数１９】
2019155556.tif 000021
ここで、ベクトルd*は非直交成分の補正後の差ベクトルｄを示し、ベクトルｎは回転軸ベクトルを示す。
【００７８】
但し、差ベクトルｄと回転軸ベクトルが十分に直交している場合には、ステップＳ１６６は省略可能である。
【００７９】
ステップＳ１６７では、差ベクトルｄと回転角φj  から、ｉ番目の座標軸に直交する平面に投影した座標間距離Ｌi を求める。図１１の例では、ｉ番目の座標軸はＸ軸であり、Ｘ軸に直交する平面はＹＺ平面である。「座標間距離Ｌi 」とは、基準座標系ΣBの座標原点と校正ジグ座標系ΣJの座標原点の間の距離を意味する。座標間距離Ｌi は、次式で算出できる。
【数２０】
2019155556.tif 000022
【００８０】
ステップＳ１６３〜ステップＳ１６７を繰り返すことにより、基準座標系ΣBの３つの座標軸をそれぞれ中心に回転したときの座標間距離Ｌ0，Ｌ1，Ｌ2が得られる。
【００８１】
ステップＳ１６８では、こうして得られた座標間距離Ｌ0，Ｌ1，Ｌ2を用いて、校正ジグ座標系ΣJと基準座標系ΣBの並進ベクトルJTBの成分r0，r1，r2を算出する。この演算は、次式で与えられる。
【数２１】
2019155556.tif 000023
【００８２】
ステップＳ１６９では、ステップＳ１６８で得られた並進ベクトルJTBの成分r0，r1，r2の正負符号を決定する。ステップＳ１６８では並進ベクトルJTBの成分r0，r1，r2の絶対値が得られたが、それらの正負符号については任意性がある。３つの成分r0，r1，r2の符号を決定するための方法はいくつか考えられる。例えば、３つの成分r0，r1，r2に対してプラス又はマイナスを設定する組み合わせは全部で８通り存在する。そこで、この８通りの組み合わせを全て試行し、その中で最適な組み合わせを選択することが可能である。この際の最適化の判断方法としては、種々の方法を使用可能である。例えば、図１１に示した差ベクトルｄの向きと、回転角φ1から符号を決めることができる。或いは、複数の校正パターン画像から得られるカメラ座標系ΣCと手首座標系ΣWの変換CΣWの並進ベクトル成分の分散又は標準偏差が最小となる符号を選択するという方法も使用可能である。後者の方法は、適切な符号の組み合わせを確実に選択できるので、安定性が高いと言う利点がある。
【００８３】
以上のように、図１２に示した並進ベクトル推定処理では、基準座標軸ΣBの３つの座標軸のそれぞれを回転中心としてハンドアイ１７５をそれぞれ回転させた複数の回転位置で撮影された複数の校正パターン画像を用いて、校正ジグ座標系ΣJと基準座標系ΣBの変換行列JΣBの成分である並進ベクトルJTBを得ることが可能である。
【００８４】
特に、図１２の並進ベクトル推定処理では、基準座標系ΣBの各座標軸に関して以下の（ａ）〜（ｄ）の処理を順次実行している。すなわち、まず、（ａ）複数の回転位置のうちの２つの回転位置において撮像された２つの校正パターン画像からカメラ座標系ΣCと校正ジグ座標系ΣJの間の変換行列CΣJをそれぞれ推定する。次に、（ｂ）２つの回転位置に関して推定された変換行列CΣJの並進ベクトル成分の差から、カメラ座標系ΣCから見た２つの回転位置における校正ジグ座標系ΣJの座標原点の差を表す差ベクトルｄを求める。また、（ｃ）差ベクトルｄから、回転中心として用いた座標軸に直交する平面に基準座標系ΣBと校正ジグ座標系ΣJを投影したときの基準座標系ΣBと校正ジグ座標系ΣJの座標原点の間の距離Ｌiを求める。そして、（ｄ）基準座標系ΣBの３つの座標軸について得られた基準座標系ΣBと校正ジグ座標系ΣJの座標原点の間の距離Ｌ0，Ｌ1，Ｌ2から、校正ジグ座標系ΣJと基準座標系ΣBの間の並進ベクトルJTBを算出する。この結果、複数の回転位置で撮像された校正パターン画像から、校正ジグ座標系ΣJと基準座標系ΣBの間の並進ベクトルJTBを容易に求めることが可能である。
【００８５】
こうして並進ベクトルJTBが得られると、この並進ベクトルJTBと図７のステップＳ１４０で得られた回転行列JRBとステップＳ１６０とを組み合わせることによって、上記（１ａ）式で与えられる変換行列JΣBを求めることが可能である。また、この変換行列JΣBを用いて、上記（１３）式に従ってハンドアイ１７５の外部パラメーターである変換行列CΣWを決定することができる。
【００８６】
以上のように、本実施形態では、基準座標系ΣBの３つの座標軸の各座標軸を回転中心としてハンドアイ１７５をそれぞれ回転させて複数の回転位置で停止させるようにアーム１６０を動作させ、これらの複数の回転位置において校正ジグ４００の校正パターン画像を取得し、複数の回転位置で撮像された校正パターン画像を用いてハンドアイ１７５の外部パラメーターとしての変換行列CΣWを決定することが可能である。この結果、カメラ座標系ΣCとロボット座標系ΣBの間の座標変換を計算可能なカメラの外部パラメーターが得られるので、ハンドアイ１７５を用いた対象物の位置検出を行うことが可能となる。
【００８７】
E. 他の形態
本発明は、上述した実施形態に限られるものではなく、その趣旨を逸脱しない範囲において種々の形態で実現することができる。例えば、本発明は、以下の形態（aspect）によっても実現可能である。以下に記載した各形態中の技術的特徴に対応する上記実施形態中の技術的特徴は、本発明の課題の一部又は全部を解決するために、あるいは、本発明の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。
【００８８】
（１）本発明の第１形態によれば、カメラが設置されたアームを有するロボットを制御する制御装置が提供される。この制御装置は、前記アームを制御するアーム制御部と；前記カメラを制御するカメラ制御部と；前記ロボットのロボット座標系における座標と前記カメラのカメラ座標系における座標を相互に変換する座標変換を計算するための前記カメラの外部パラメーターを決定するカメラ校正実行部と；を備える。前記アーム制御部は、前記アームを動作させ、前記ロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させる。前記カメラ制御部は、前記複数の回転位置における校正ジグの校正パターンを前記カメラに撮像させる。前記カメラ校正実行部は、前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記外部パラメーターを決定する。
この制御装置によれば、基準座標系の各座標軸回りの回転における複数の回転位置で撮影された校正パターン画像を用いて、校正ジグ座標系と基準座標系の間の座標変換行列を決定でき、これを用いてカメラの外部パラメーターを決定できる。この結果、カメラ座標系とロボット座標系の間の座標変換を計算可能なカメラの外部パラメーターが得られるので、カメラを用いた対象物の位置検出を行うことが可能となる。
【００８９】
（２）上記制御装置において、前記カメラ校正実行部は、（ｉ）前記複数の回転位置で撮像された前記校正パターン画像を用いて、前記校正ジグ座標系と前記基準座標系の回転を示す回転行列を推定する回転行列推定処理と、（ｉｉ）前記複数の回転位置で撮像された前記校正パターン画像を用いて、前記校正ジグ座標系と前記基準座標系の並進移動を示す並進ベクトルを推定する並進ベクトル推定処理と、を実行し、（ｉｉｉ）推定された前記回転行列と前記並進ベクトルとを組み合わせることによって、前記校正ジグ座標系の座標と前記基準座標系の座標を相互に変換する座標変換行列を求めるようにしても良い。
この制御装置によれば、基準座標系の各座標軸を中心とした回転における複数の回転位置で撮像されたパターン画像から、校正ジグ座標系と基準座標系の間の回転行列と並進ベクトルをそれぞれ推定するので、それらを組み合わせることによって座標変換回転を容易に求めることが可能である。
【００９０】
（３）上記制御装置において、前記カメラ校正実行部は、前記回転行列推定処理において、前記複数の回転位置で撮像された前記校正パターン画像から、前記基準座標系の各座標軸の方向をベクトル方向とし前記回転の角度をベクトル長さとする３つの回転ベクトルを推定し、前記３つの回転ベクトルを行成分又は列成分として配列することによって前記回転行列を決定するものとしてもよい。
この制御装置によれば、基準座標系の各座標軸を中心とした回転における複数の回転位置で撮像された校正パターン画像から、校正ジグ座標系と基準座標系の間の回転行列を容易に求めることが可能である。
【００９１】
（４）上記制御装置において、前記カメラ校正実行部は、前記基準座標系の各座標軸に関して、（ａ）前記複数の回転位置のうちの第１回転位置と第２回転位置において撮像された前記校正パターン画像から前記カメラ座標系の座標と前記校正ジグ座標系の座標を相互に変換する変換行列をそれぞれ推定し、（ｂ）前記第１回転位置と前記第２回転位置に関して推定された前記変換行列の並進ベクトル成分の差から、前記カメラ座標系から見た前記第１回転位置と前記第２回転位置における前記校正ジグ座標系の座標原点の差を表す差ベクトルを求め、（ｃ）前記差ベクトルから、回転中心として用いた座標軸に直交する平面に前記基準座標系と前記校正ジグ座標系を投影したときの前記基準座標系と前記校正ジグ座標系の座標原点の間の距離を求め、（ｄ）前記基準座標系の３つの座標軸について得られた前記基準座標系と前記校正ジグ座標系の座標原点の間の前記距離から、前記校正ジグ座標系と前記基準座標系の並進移動を示す並進ベクトルを算出するものとしてもよい。
この制御装置によれば、複数の回転位置で撮像された校正パターン画像から、校正ジグ座標系と基準座標系の間の並進ベクトルを容易に求めることが可能である。
【００９２】
（５）本発明の第２形態によれば、カメラが設置されたアームを有するロボットを制御する制御装置が提供される。この制御装置は、プロセッサーを備える。前記プロセッサーは；前記ロボットの前記アームを動作させ、ロボット座標系に対して既知の相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させ；前記複数の回転位置における校正ジグの校正パターンを前記カメラに撮像させ；前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記カメラの外部パラメーターを決定する。
この制御装置によれば、基準座標系の各座標軸回りの回転における複数の回転位置で撮影された校正パターン画像を用いて、校正ジグ座標系と基準座標系の間の座標変換行列を決定でき、これを用いてカメラの外部パラメーターを決定できる。この結果、カメラ座標系とロボット座標系の間の座標変換を計算可能なカメラの外部パラメーターが得られるので、カメラを用いた対象物の位置検出を行うことが可能となる。
【００９３】
（６）本発明の第３形態によれば、上記制御装置に接続されたロボットが提供される。
このロボットによれば、カメラ座標系と基準座標系の間の座標変換を行って、カメラを用いた対象物の位置検出を行うことが可能となる。
【００９４】
（７）本発明の第４形態によれば、ロボットと、前記ロボットに接続された上記制御装置と、を備えるロボットシステムが提供される。
このロボットシステムによれば、カメラ座標系と基準座標系の間の座標変換を行って、カメラを用いた対象物の位置検出を行うことが可能となる。
【００９５】
（８）本発明の第５形態によれば、カメラが設置されたアームを有するロボットを備えるロボットシステムにおいて前記カメラの外部パラメーターを決定するため校正を行う方法が提供される。この方法は、前記ロボットの前記アームを動作させ、前記ロボットのロボット座標系に対して、前記ロボットシステムにとって既知である相対位置姿勢を有する基準座標系の３つの座標軸の各座標軸を回転中心として前記カメラを回転させて複数の回転位置で停止させ；前記複数の回転位置において校正ジグの校正パターンを前記カメラに撮像させ；前記複数の回転位置で撮像された校正パターン画像を用いて、前記校正ジグの校正ジグ座標系における座標と前記基準座標系における座標を相互に変換する座標変換行列を推定し、前記座標変換行列を用いて前記外部パラメーターを決定する。
この方法によれば、基準座標系の各座標軸回りの回転における複数の回転位置で撮影された校正パターン画像を用いて、校正ジグ座標系と基準座標系の間の座標変換行列を決定でき、これを用いてカメラの外部パラメーターを決定できる。この結果、カメラ座標系とロボット座標系の間の座標変換を計算可能なカメラの外部パラメーターが得られるので、カメラを用いた対象物の位置検出を行うことが可能となる。
【符号の説明】
【００９６】
１００…ロボット、１１０…基台、１２０…胴部、１３０…肩部、１４０…首部、１５０…頭部、１６０…アーム、１７５…ハンドアイ、１８０…ハンド、１９０…力覚センサー、２００…制御装置、２１０…プロセッサー、２１１…アーム制御部、２１２…カメラ制御部、２１３…カメラ校正実行部、２１４…変換行列推定部、２２０…メインメモリー、２３０…不揮発性メモリー、２３１…プログラム命令、２３２…カメラ内部パラメーター、２３３…カメラ外部パラメーター、２４０…表示制御部、２５０…表示部、２６０…Ｉ／Ｏインターフェース、４００…校正ジグ、５００…クラウドサービス、５１０，５２０…パーソナルコンピューター
【図１】
2019155556.tif 000024
【図２】
2019155556.tif 000025
【図３】
2019155556.tif 000026
【図４】
2019155556.tif 000027
【図５】
2019155556.tif 000028
【図６】
2019155556.tif 000029
【図７】
2019155556.tif 000030
【図８】
2019155556.tif 000031
【図９】
2019155556.tif 000032
【図１０】
2019155556.tif 000033
【図１１】
2019155556.tif 000034
【図１２】
2019155556.tif 000035