JP2019014031A - ロボットの制御装置、ロボット、ロボットシステム、並びに、ロボット用カメラの校正方法 - Google Patents

Abstract

【課題】ロボット用カメラの校正を容易に行うことのできる技術を提供する。【解決手段】アーム制御部は、互いに一次独立な３つの回転軸の各回転軸を中心として校正用パターンをそれぞれ回転させて複数の回転位置で停止させるようにアームを動作させる。カメラ制御部は、複数の回転位置における校正用パターンのパターン画像をカメラに撮像させる。カメラ校正実行部は、複数の回転位置で撮像されたパターン画像を用いて、ターゲット座標系とカメラ座標系との間の座標変換を計算可能なカメラのパラメーターを推定する。【選択図】図３

Description

本発明は、ロボット用カメラの校正（キャリブレーション）に関するものである。

ロボットに高度な処理を行わせるために、ロボットにカメラを設置して眼の機能を持たせる場合がある。カメラの設置方法として、カメラをロボットアームとは独立して設置する方法と、カメラをロボットアームの動きと連動するように手先に設置する方法（ハンドアイ）とがある。

特許文献１には、ロボットアームとは独立して設置されたカメラに関する校正（キャリブレーション）を行うシステムが記載されている。このシステムは、校正治具の特徴部を照明条件に依存せず安定的に精度よく検出すること、及び、低コストで取り扱いを容易なシステムとすることを目的としている。

特開２０１０−１３９３２９号公報

特許文献１に記載された技術では、事前に校正治具の特徴部と校正対象との相対位置関係を高精度に把握する必要がある。例えば、カメラの外部パラメーターを求める場合に、校正治具の特徴部とカメラが装着された支持具との相対位置および相対姿勢が指定値になるように、特徴部を配置する必要がある。しかし、事前に校正治具の特徴部と校正対象との相対位置関係を高精度に設定することは必ずしも容易でない。そこで、特許文献１とは異なる方法で、カメラの校正を容易に行うことのできる技術が望まれている。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態（aspect）として実現することが可能である。

（１）本発明の第１の形態によれば、カメラの校正用パターンを設置可能に構成されたアームを備えるロボットと、前記アームとは独立して設置されたカメラと、を制御する制御装置が提供される。この制御装置は、前記アームを制御するアーム制御部と；前記カメラを制御するカメラ制御部と；前記ロボットのロボット座標系に対して既知の相対位置姿勢を有するターゲット座標系と、前記カメラのカメラ座標系との間の座標変換を計算可能な前記カメラのパラメーターを決定するカメラ校正実行部と；を備える。前記アーム制御部は、互いに一次独立な３つの回転軸の各回転軸を中心として前記校正用パターンをそれぞれ回転させて複数の回転位置で停止させるように前記アームを動作させる。前記カメラ制御部は、前記複数の回転位置における前記校正用パターンのパターン画像を前記カメラに撮像させる。前記カメラ校正実行部は、前記複数の回転位置で撮像された前記パターン画像を用いて前記パラメーターを決定する。
この制御装置によれば、各回転軸回りの回転における複数の回転位置のパターン画像を用いて、カメラ座標系で見た３つの回転軸の方向を推定できる。また、３つの回転軸は互いに一次独立なので、これらの回転軸の方向から、ターゲット座標系とカメラ座標系との間の座標変換行列を決定できる。この結果、ターゲット座標系とカメラ座標系との間の座標変換を計算可能なカメラのパラメーターが得られるので、カメラを用いた対象物の位置検出を行うことが可能となる。

（２）上記制御装置において、前記３つの回転軸は、前記ターゲット座標系の原点の回りに設定されるものとしてもよい。
この制御装置によれば、３つの回転軸とターゲット座標系との対応関係が単純なので、カメラ座標系で見た回転軸の方向から、ターゲット座標系とカメラ座標系との間の座標変換行列を容易に決定できる。

（３）上記制御装置において、前記カメラ校正実行部は、前記複数の回転位置で撮像された前記パターン画像から、各回転軸の方向をベクトル方向とし前記回転の角度をベクトル長さとする３つの回転ベクトルを推定し、前記３つの回転ベクトルをそれぞれ正規化して３つの正規化回転ベクトルを求め、前記３つの正規化回転ベクトルを行成分又は列成分として配列することによって前記ターゲット座標系と前記カメラ座標系との間の座標変換行列を構成する回転行列を決定するようにしてもよい。
この制御装置によれば、各回転軸を中心とした回転における複数の回転位置で撮像されたパターン画像から、ターゲット座標系とカメラ座標系との間の座標変換行列を構成する回転行列を容易に求めることが可能である。

（４）上記制御装置において、前記ターゲット座標系と前記カメラ座標系との間の座標変換行列は、前記カメラ座標系と前記校正用パターンのパターン座標系との間の第１変換行列と、前記パターン座標系と前記ターゲット座標系との間の第２変換行列と、の積で表されるものとしてもよい。この場合に、前記カメラ校正実行部は、（ａ）前記複数の回転位置のうちの１つの特定回転位置において撮像された前記パターン画像から前記第１変換行列を推定し、（ｂ）前記複数の回転位置で撮像された前記パターン画像から、前記第２変換行列を構成する並進ベクトルの３つの成分のうちで各回転軸と直交する２つの座標軸方向における２つの並進ベクトル成分の２乗和を推定し、前記３つの回転軸についてそれぞれ推定された前記並進ベクトル成分の２乗和から前記第２変換行列を構成する前記並進ベクトルを算出し、（ｃ）前記特定回転位置で推定された前記第１変換行列と、前記第２変換行列の並進ベクトルから、前記座標変換行列を構成する並進ベクトルを算出するものとしてもよい。
この制御装置によれば、各回転軸を中心とした回転における複数の回転位置で撮像されたパターン画像から、ターゲット座標系とカメラ座標系との間の座標変換行列を構成する並進ベクトルを容易に求めることが可能である。

（５）上記制御装置において、前記ターゲット座標系は、前記アームとは独立に前記ロボットのロボット座標系に対して固定された相対位置姿勢を有する座標系であるものとしてもよい。
この制御装置によれば、アームとは独立に設けられたターゲット座標系とカメラ座標系との間の座標変換行列を求めるので、アームから離れた位置において、カメラを用いた対象物の位置検出の精度を向上させることが可能である。

（６）上記制御装置において、前記ターゲット座標系は、前記アームの手先座標系であるものとしてもよい。
この制御装置によれば、アームの手先位置において、カメラを用いた対象物の位置検出の精度を向上させることが可能である。

（７）本発明の第２の形態によれば、カメラの校正用パターンを設置可能に構成されたアームを備えるロボットと、前記アームとは独立して設置されたカメラと、を備えるロボットシステムを制御する制御装置が提供される。この制御装置は、プロセッサーを備える。前記プロセッサーは、互いに一次独立な３つの回転軸の各回転軸を中心として前記校正用パターンをそれぞれ回転させて複数の回転位置で停止させるように前記アームを動作させ；前記複数の回転位置における前記校正用パターンのパターン画像を前記カメラに撮像させ；前記複数の回転位置で撮像された前記パターン画像を用いて、前記ロボットのロボット座標系に対して既知の相対位置姿勢を有するターゲット座標系と前記カメラのカメラ座標系との間の座標変換を計算可能な前記カメラのパラメーターを決定する。
この制御装置によれば、各回転軸回りの回転における複数の回転位置のパターン画像を用いて、カメラ座標系で見た３つの回転軸の方向を推定できる。また、３つの回転軸は互いに一次独立なので、これらの回転軸の方向から、ターゲット座標系とカメラ座標系との間の座標変換行列を決定できる。この結果、ターゲット座標系とカメラ座標系との間の座標変換を計算可能なカメラのパラメーターが得られるので、カメラを用いた対象物の位置検出を行うことが可能となる。

（８）本発明の第３の形態は、上記制御装置に接続されたロボットである。
このロボットによれば、ターゲット座標系とカメラ座標系との間の座標変換を行って、カメラを用いた対象物の位置検出を行うことが可能となる。

（９）本発明の第４の形態は、ロボットと、前記ロボットに接続された上記制御装置と、を備えるロボットシステムである。
このロボットシステムによれば、ターゲット座標系とカメラ座標系との間の座標変換を行って、カメラを用いた対象物の位置検出を行うことが可能となる。

（１０）本発明の第５の形態によれば、カメラの校正用パターンを設置可能に構成されたアームを備えるロボットと、前記アームとは独立して設置されたカメラと、を備えるロボットシステムにおいて前記カメラの校正を行う方法が提供される。この方法は、互いに一次独立な３つの回転軸の各回転軸を中心として前記校正用パターンをそれぞれ回転させて複数の回転位置で停止させるように前記アームを動作させ；前記複数の回転位置における前記校正用パターンのパターン画像を前記カメラに撮像させ；前記複数の回転位置で撮像された前記パターン画像を用いて、前記ロボットのロボット座標系に対して既知の相対位置姿勢を有するターゲット座標系と前記カメラのカメラ座標系との間の座標変換を計算可能な前記カメラのパラメーターを決定する。
この方法によれば、各回転軸回りの複数の回転位置のパターン画像を用いて、カメラ座標系で見た３つの回転軸の方向を推定できる。また、３つの回転軸は互いに一次独立なので、これらの回転軸の方向から、ターゲット座標系とカメラ座標系との間の座標変換行列を決定できる。この結果、ターゲット座標系とカメラ座標系との間の座標変換を計算可能なカメラのパラメーターが得られるので、カメラを用いた対象物の位置検出を行うことが可能となる。

本発明は、上記以外の種々の形態で実現することも可能である。例えば、制御装置の機能を実現するためのコンピュータプログラム、そのコンピュータプログラムを記録した一時的でない記録媒体（non-transitory storage medium）等の形態で実現することができる。

ロボットシステムの概念図。 ロボットと制御装置の機能を示すブロック図。 ロボットの座標系を示す説明図。 実施形態の処理手順を示すフローチャート。 複数の回転位置でのパターン画像の例を示す説明図。 図４のステップＳ１６０で得られた回転行列の例を示す図。 カメラ座標系のＹＺ平面上に並進ベクトルを投影した図。 図４のステップＳ１８０で得られた並進ベクトルの例を示す図。 第２実施形態におけるロボットの座標系を示す説明図。

A. ロボットシステムの構成
図１は、一実施形態におけるロボットシステムの概念図である。このロボットシステムは、ロボット１００と、制御装置２００とを備えている。このロボット１００は、カメラで作業対象を認識し、自在に力を加減して、自律的に判断しながら作業を行える自律型ロボットである。また、ロボット１００は、予め作成された教示データに従って作業を実行するティーチングプレイバックロボットとして動作することも可能である。

ロボット１００は、基台１１０と、胴部１２０と、肩部１３０と、首部１４０と、頭部１５０と、２つのアーム１６０Ｌ，１６０Ｒとを備えている。アーム１６０Ｌ，１６０Ｒには、ハンド１８０Ｌ，１８０Ｒが着脱可能に取り付けられている。これらのハンド１８０Ｌ，１８０Ｒは、ワークや道具を把持するエンドエフェクターである。頭部１５０には、カメラ１７０Ｌ，１７０Ｒが設置されている。これらのカメラ１７０Ｌ，１７０Ｒは、アーム１６０Ｌ，１６０Ｒとは独立して設けられており、その位置姿勢が変化しない固定カメラである。アーム１６０Ｌ，１６０Ｒには、カメラ１７０Ｌ，１７０Ｒの校正用パターン４００を設置可能である。

アーム１６０Ｌ，１６０Ｒの手首部には、力覚センサー１９０Ｌ，１９０Ｒが設けられている。力覚センサー１９０Ｌ，１９０Ｒは、ハンド１８０Ｌ，１８０Ｒがワークに及ぼしている力に対する反力やモーメントを検出するセンサーである。力覚センサー１９０Ｌ，１９０Ｒとしては、例えば、並進３軸方向の力成分と、回転３軸回りのモーメント成分の６成分を同時に検出することができる６軸力覚センサーを用いることができる。なお、力覚センサー１９０Ｌ，１９０Ｒは省略可能である。

アーム１６０Ｌ，１６０Ｒと、カメラ１７０Ｌ，１７０Ｒと、ハンド１８０Ｌ，１８０Ｒと、力覚センサー１９０Ｌ，１９０Ｒの符号の末尾に付された「Ｌ」「Ｒ」の文字は、それぞれ「左」「右」を意味する。これらの区別が不要の場合には、「Ｌ」「Ｒ」の文字を省略した符号を使用して説明する。

制御装置２００は、プロセッサー２１０と、メインメモリー２２０と、不揮発性メモリー２３０と、表示制御部２４０と、表示部２５０と、Ｉ／Ｏインターフェース２６０とを有している。これらの各部は、バスを介して接続されている。プロセッサー２１０は、例えばマイクロプロセッサー又はプロセッサー回路である。制御装置２００は、Ｉ／Ｏインターフェース２６０を介してロボット１００に接続される。なお、制御装置２００をロボット１００の内部に収納してもよい。

制御装置２００の構成としては、図１に示した構成以外の種々の構成を採用することが可能である。例えば、プロセッサー２１０とメインメモリー２２０を図１の制御装置２００から削除し、この制御装置２００と通信可能に接続された他の装置にプロセッサー２１０とメインメモリー２２０を設けるようにしてもよい。この場合には、当該他の装置と制御装置２００とを合わせた装置全体が、ロボット１００の制御装置として機能する。他の実施形態では、制御装置２００は、２つ以上のプロセッサー２１０を有していてもよい。更に他の実施形態では、制御装置２００は、互いに通信可能に接続された複数の装置によって実現されていてもよい。これらの各種の実施形態において、制御装置２００は、１つ以上のプロセッサー２１０を備える装置又は装置群として構成される。

図２は、ロボット１００と制御装置２００の機能を示すブロック図である。制御装置２００のプロセッサー２１０は、不揮発性メモリー２３０に予め格納された各種のプログラム命令２３１を実行することにより、アーム制御部２１１と、カメラ制御部２１２と、カメラ校正実行部２１３との機能をそれぞれ実現する。カメラ校正実行部２１３は、変換行列推定部２１４を有している。但し、これらの各部２１１〜２１４の機能の一部又は全部をハ―ドウェア回路で実現しても良い。これらの各部２１１〜２１４の機能については後述する。不揮発性メモリー２３０には、プログラム命令２３１の他に、カメラ内部パラメーター２３２とカメラ外部パラメーター２３３とが格納される。これらのパラメーター２３２，２３３については後述する。

B. ロボットの座標系と座標変換
図３は、ロボット１００のアーム１６０の構成と各種の座標系を示す説明図である。２つのアーム１６０Ｌ，１６０Ｒのそれぞれには、７つの関節Ｊ１〜Ｊ７が設けられている。関節Ｊ１，Ｊ３，Ｊ５，Ｊ７はねじり関節であり、関節Ｊ２，Ｊ４，Ｊ６は曲げ関節である。なお、図１の胴部１２０と肩部１３０の間にはねじり関節が設けられているが、図３では図示が省略されている。個々の関節には、それらを動作させるためのアクチュエーターと、回転角度を検出するための位置検出器とが設けられている。

アーム１６０の手先には、ツール中心点ＴＣＰ（Tool Center Point）が設定されている。典型的には、ロボット１００の制御は、ツール中心点ＴＣＰの位置姿勢を制御するために実行される。なお、位置姿勢（position and attitude）とは、３次元の座標系における３つ座標値と、各座標軸回りの回転とで規定される状態を意味する。図３の例では、右アーム１６０Ｒの手先には、カメラ１７０の校正（キャリブレーション）に使用される校正用パターン４００が固定されている。校正用パターン４００をアーム１６０Ｒに取り付ける際には、ハンド１８０Ｒを取り外すようにしてもよい。

カメラ１７０の校正は、カメラ１７０の内部パラメーターと外部パラメーターとを決定する処理である。内部パラメーターは、カメラ１７０及びそのレンズ系の固有のパラメーターであり、例えば射影変換パラメーターや歪パラメーターなどを含んでいる。外部パラメーターは、カメラ１７０とロボット１００のアーム１６０との間の相対的な位置姿勢を計算する際に使用されるパラメーターであり、ロボット座標系Σ₀とカメラ座標系Σ_Cとの間の並進や回転を表現するパラメーターを含んでいる。但し、外部パラメーターは、ロボット座標系Σ₀以外のターゲット座標系とカメラ座標系Σ_Cとの間の並進や回転を表現するパラメーターとしても構成可能である。ターゲット座標系は、ロボット座標系Σ₀から求め得る座標系であれば良い。例えば、ロボット座標系Σ₀に対して固定された既知の相対位置姿勢を有する座標系や、アーム１６０の関節の動作量に応じてロボット座標系Σ₀との相対位置姿勢が決まる座標系をターゲット座標系として選択してもよい。外部パラメーターは、「ターゲット座標系とカメラのカメラ座標系との間の座標変換を計算可能なカメラのパラメーター」に相当する。

図３には、ロボット１００に関する座標系として、以下の座標系が描かれている。
（１）ロボット座標系Σ₀：ロボット１００の基準点Ｒ０を座標原点とする座標系
（２）アーム座標系Σ_A：アーム１６０の基準点Ａ０を座標原点とする座標系
（３）手先座標系Σ_T：ＴＣＰ（ツール中心点）を座標原点とする座標系
（４）パターン座標系Σ_P：校正用パターン４００上の所定の位置を座標原点とする座標系
（５）カメラ座標系Σ_C：カメラ１７０に設定された座標系

アーム座標系Σ_Aと手先座標系Σ_Tは、右アーム１６０Ｒと左アーム１６０Ｌにそれぞれ別個に設定される。図３の例では校正用パターン４００が右アーム１６０Ｒの手先に固定されているので、以下の説明では、右アーム１６０Ｒのアーム座標系Σ_Aと手先座標系Σ_Tを使用する。アーム座標系Σ_Aとロボット座標系Σ₀の相対位置姿勢は既知である。カメラ座標系Σ_Cも、右目カメラ１７０Ｒと左目カメラ１７０Ｌにそれぞれ別個に設定される。以下の説明では、カメラ座標系Σ_Cとして主として左目カメラ１７０Ｌの座標系を使用するが、右目カメラ１７０Ｒの座標系を使用してもよい。なお、図３では、図示の便宜上、個々の座標系の原点が実際の位置からずれた位置に描かれている。

一般に、或る座標系Σ_Aから他の座標系Σ_Bへの変換、又は、これらの座標系での位置姿勢の変換は、以下に示す同次変換行列^ＡＨ_Ｂ（Homogeneous transformation matrix）で表現される。
ここで、Ｒは回転行列（Rotation matrix）、Ｔは並進ベクトル（Translation vector）、Ｒ_ｘ，Ｒ_ｙ，Ｒ_ｚは回転行列Ｒの列成分である。以下では、同次変換行列^ＡＨ_Ｂを「座標変換行列^ＡＨ_Ｂ」、「変換行列^ＡＨ_Ｂ」又は、単に「変換^ＡＨ_Ｂ」とも呼ぶ。変換の符号「^ＡＨ_Ｂ」の左側の上付き文字「^Ａ」は変換前の座標系を示し、右側の下付き文字「_Ｂ」は変換後の座標系を示す。なお、変換^ＡＨ_Ｂは、座標系Σ_Aで見た座標系Σ_Bの基底ベクトルの成分と原点位置を表すものと考えることも可能である。

変換^ＡＨ_Ｂの逆行列^ＡＨ_Ｂ ^−１（＝^ＢＨ_Ａ）は、次式で与えられる。

回転行列Ｒは、以下の重要な性質を有する。
＜回転行列Ｒの性質１＞
回転行列Ｒは正規直交行列であり、その逆行列Ｒ^−１は転置行列Ｒ^Tに等しい。
＜回転行列Ｒの性質２＞
回転行列Ｒの３つの列成分Ｒ_ｘ，Ｒ_ｙ，Ｒ_ｚは、元の座標系Σ_Aで見た回転後の座標系Σ_Bの３つの基底ベクトルの成分に等しい。

或る座標系Σ_Aに変換^AＨ_B，^BＨ_Cを順次施す場合に、合成された変換^AＨ_Cは各変換^AＨ_B，^BＨ_Cを順次右側に乗じたものとなる。

また、回転行列Ｒに関しても（３）式と同様の関係が成立する。

C. 座標変換のＡＸ＝ＸＢ問題
図３において、複数の座標系Σ₀，Σ_T，Σ_P，Σ_Cの間には、以下の変換が成立する。
（１）変換⁰Ｈ_T（計算可）：ロボット座標系Σ₀から手先座標系Σ_Tへの変換
（２）変換^TＨ_P（未知）：手先座標系Σ_Tからパターン座標系Σ_Pへの変換
（３）変換^PＨ_C（推定可）：パターン座標系Σ_Pからカメラ座標系Σ_Cへの変換
（４）変換^CＨ₀（未知）：カメラ座標系Σ_Cからロボット座標系Σ₀への変換

ロボット座標系Σ₀とカメラ座標系Σ_Cを関連付けるパラメーターは、変換^CＨ₀である。通常は、この変換^CＨ₀を求めることがカメラ１７０の校正（キャリブレーション）に相当する。

第１実施形態におけるカメラ１７０の校正では、ＴＣＰを校正対象点とし、その手先座標系Σ_Tを校正対象のターゲット座標系として選択する。そして、手先座標系Σ_Tとカメラ座標系Σ_Cとの間の変換^TＨ_C（=^TＨ_P・^PＨ_C）又は^CＨ_T（=^CＨ_P・^PＨ_T）を推定する。手先座標系Σ_Tとロボット座標系Σ₀との間の変換^TＨ₀（又は ⁰Ｈ_T）は計算可能なので、手先座標系Σ_Tとカメラ座標系Σ_Cとの間の変換^TＨ_C（又は ^CＨ_T）が得られれば、ロボット座標系Σ₀とカメラ座標系Σ_Cの間の変換^CＨ₀（又は ⁰Ｈ_C）も計算可能となる。なお、ターゲット座標系としては、手先座標系Σ_T以外の座標系を選択することも可能であり、ロボット座標系Σ₀に対して既知の相対位置姿勢を有する任意の座標系を選択可能である。手先座標系Σ_T以外の座標系をターゲット座標系として選択した場合については、第２実施形態で説明する。

上述の４つの変換⁰Ｈ_T，^TＨ_P，^PＨ_C，^CＨ₀のうち、変換⁰Ｈ_Tは、ロボット座標系Σ₀と校正対象点としてのＴＣＰの手先座標系Σ_Tとを結びつける変換である。通常、ロボット座標系Σ₀に対するＴＣＰの位置姿勢を求める処理はフォワードキネマティクスと呼ばれ、アーム１６０の幾何学的な形状と各関節の動作量（回転角度）が定まれば計算できる。つまり、この変換⁰Ｈ_Tは計算可能な変換である。なお、ロボット座標系Σ₀からアーム座標系Σ_Aへの変換⁰Ｈ_Aは固定されており、既知である。

変換^TＨ_Pは、手先座標系Σ_Tから校正用パターン４００のパターン座標系Σ_Pへの変換である。特許文献１においては、この変換^TＨ_Pは既知の固定された変換であることが要求されているが、本実施形態では未知であると仮定する。

変換^PＨ_Cは、パターン座標系Σ_Pからカメラ座標系Σ_Cへの変換であり、カメラ１７０によって校正用パターン４００を撮像し、その画像に対して画像処理を行うことによって推定できる。この変換^PＨ_Cを推定する処理は、カメラの校正を行うための標準的なソフトウェア（例えばOpenCVやMATLABのカメラキャリブレーション関数）を用いて実行可能である。

上記の変換⁰Ｈ_T，^TＨ_P，^PＨ_C，^CＨ₀を順に辿れば最初のロボット座標系Σ₀に戻るので、恒等変換Ｉを用いて以下の式が成立する。

（５）式の両辺に、左から各変換の逆行列⁰Ｈ_T ^−１, ^TＨ_P ^−１, ^PＨ_C ^−１を順に乗算すれば次式が得られる。

（６）式において、変換^PＨ_Cはカメラキャリブレーション関数を用いて推定でき、変換⁰Ｈ_Tは計算可能である。従って、変換^TＨ_Pが既知であれば、右辺は計算可能であり、左辺の変換^CＨ₀を知ることができる。これが、従来技術において変換^TＨ_Pを既知であると仮定していた理由である。

一方、変換^TＨ_Pが未知であれば、（６）式の右辺は計算できず、別の処理が必要となる。例えば、図３においてアーム１６０Ｒの２つの姿勢ｉ，ｊを考えれば、それぞれにおいて上記（５）式が成立し、次式となる。

（７ａ），（７ｂ）式の両辺にそれぞれ右から変換^CＨ₀の逆行列^CＨ₀ ^−１ を乗算すれば次式が得られる。

（８ａ），（８ｂ）式の右辺は未知ではあるが同じ変換なので、次式が成立する。

（９）式の両辺に、左から⁰Ｈ_T(j)^−１を、右から^PＨ_C(i)^−１を乗算すると次式となる。

ここで、（１０）式の左辺と右辺の括弧の中の変換の積をそれぞれＡ，Ｂと書き、未知の変換^TＨ_PをＸと書くと、次式が得られる。

これは、ＡＸ＝ＸＢ問題として良く知られた処理であり、未知の行列Ｘを解くためには非線形の最適化処理が必要となる。しかし、この非線形最適化処理には、最適な解に収束する保証が無いと言う問題がある。

以下に詳述するように、第１実施形態では、校正用パターン４００が設置されたアーム１６０を任意に制御可能であることを利用し、校正用パターン４００に所定の位置姿勢の変化をさせることにより、ターゲット座標系である手先座標系Σ_Tとカメラ座標系Σ_Cとの間の変換^TＨ_C（=^TＨ_P・^PＨ_C）又は^CＨ_T（=^CＨ_P・^PＨ_T）を推定する。この結果、カメラ１７０の外部パラメーターを決定することができる。

D. 実施形態の処理手順
図４は、実施形態におけるカメラ１７０の校正処理の手順を示すフローチャートである。ロボット１００が有する２つのカメラ１７０Ｒ，１７０Ｌは、それぞれ別個に校正されるが、以下では特に区別することなく「カメラ１７０」と呼ぶ。以下で説明する校正処理は、図２に示したアーム制御部２１１と、カメラ制御部２１２と、カメラ校正実行部２１３とが協調して実行される。すなわち、校正用パターン４００を複数の位置姿勢に変更する動作は、アーム制御部２１１がアーム１６０を制御することによって実現される。また、カメラ１７０による撮像は、カメラ制御部２１２によって制御される。カメラ１７０の内部パラメーターや外部パラメーターは、カメラ校正実行部２１３によって決定される。また、カメラ１７０の外部パラメーターの決定において、各種の行列やベクトルの推定は、変換行列推定部２１４によって行われる。

ステップＳ１１０とステップＳ１２０は、カメラ１７０の内部パラメーターを決定する処理である。まず、ステップＳ１１０では、カメラ１７０を用いて、校正用パターン４００を複数の位置姿勢で撮影する。これらの複数の位置姿勢は、カメラ１７０の内部パラメーターを決定するためのものなので、任意の位置姿勢を採用可能である。ステップＳ１２０では、カメラ校正実行部２１３が、ステップ１１０で得られた複数のパターン画像を用いて、カメラ１７０の内部パラメーターを推定する。前述したように、カメラ１７０の内部パラメーターは、カメラ１７０及びそのレンズ系の固有のパラメーターであり、例えば射影変換パラメーターや歪パラメーターなどを含んでいる。内部パラメーターの推定は、カメラの校正を行う標準的なソフトウェア（例えばOpenCVやMATLABのカメラキャリブレーション関数）を用いて実行可能である。

ステップＳ１３０〜Ｓ１８０は、カメラ１７０の外部パラメーターを決定する処理である。ステップＳ１３０では、校正用パターン４００を、手先座標系Σ_Tの３つの回転軸回りで回転させ、各回転軸回りの回転における複数の回転位置で校正用パターン４００の画像を撮影する。以下では、校正用パターン４００をカメラ１７０で撮像して得られた画像を「パターン画像」と呼ぶ。

図５は、ステップＳ１３０で得られる複数のパターン画像の例を示す説明図である。これらのパターン画像は、手先座標系Σ_Tの原点であるＴＣＰを空間的に固定した状態で、手先座標系Σ_TのＸＹＺ軸の各軸回りに±θx，±θy，±θzの回転をそれぞれ独立に行ってアーム１６０を停止させ、これらの複数の回転位置において撮像された画像である。すなわち、複数の回転位置は、基本回転位置と、基本回転位置からＸ軸回りに回転させた２つの回転位置と、基本回転位置からＹ軸回りに回転させた２つの回転位置と、基本回転位置からＺ軸回りに回転させた２つの回転位置と、を含んでいる。基本回転位置からの回転角度θx，θy，θzは、それぞれ５度に設定されているが、０度でない任意の回転角度を採用可能である。但し、回転角度θが過度に小さいと回転によるパターン画像の違いが識別し難くなり、回転角度θが過度に大きいとパターン画像から校正用パターン４００の配列を識別し難くなる。これらの点を考慮すると、回転角度θx，θy，θzは、例えば３度以上３０度以下の範囲に設定することが好ましい。校正用パターン４００は、黒丸を９×７の格子状に配置したパターンである。但し、チェッカーボードパターンのような他の校正用パターンを使用してもよい。パターン座標系Σ_Pの座標原点は、校正用パターン４００上の予め定められた位置にある。

ステップＳ１４０では、ステップＳ１３０で撮像された個々のパターン画像について、パターン座標系Σ_Pとカメラ座標系Σ_Cの間の変換^PＨ_C又は^CＨ_Pを推定する。この推定は、ステップＳ１２０で得られた内部パラメーターを利用し、カメラの外部パラメーターを推定する標準的なソフトウェア（例えばOpenCVの関数「FindExtrinsicCameraParams2」）を用いて実行可能である。

ステップＳ１５０では、ステップＳ１４０で得られた変換^PＨ_C又は^CＨ_Pを用いて、カメラ座標系Σ_Cと手先座標系Σ_Tの間の回転行列^CＲ_T又は^TＲ_Cを推定する。以下では、まず、Ｘ軸回りの回転を例として説明する。

先ず、基本回転位置のパターン画像から得られた変換^PＨ_Cの回転行列^PＲ_Cを、簡単にＲ（θ₀）と記載する。また、Ｘ軸の回りに±θx回転させた状態におけるパターン画像から得られた変換^PＨ_Cの回転行列^PＲ_Cを、それぞれＲ（θ₀＋θx）及びＲ（θ₀−θx）と記載する。このとき、次式が成立する。
ここで、回転行列Ｒ（θx）は、基本回転位置から＋θxだけ座標系を回転させる回転行列である。（１２ｂ）式に示すように、回転行列Ｒ（θx）は、基本回転位置における回転行列Ｒ（θ_０）の逆行列Ｒ（θ_０）^−１と、基本回転位置から＋θxだけ回転させた位置における回転行列Ｒ（θ_０＋θx）との積として計算できる。

ところで、一般に、座標系の３つの軸回りの任意の回転は、回転行列や３つのオイラー角で表現する場合が多いが、その代わりに、１つの回転軸と、その回転軸回りの回転角度とで表現することも可能である。後者の表現を利用すると、回転行列Ｒ（θx）は、次式で与えられる回転ベクトルＲｏｄ（θx）に変換できる。
ここで、ｎ_x，ｎ_y，ｎ_zは、回転軸の方向を示す３軸の成分である。すなわち、「回転ベクトルＲｏｄ」は、回転軸の方向をベクトル方向とし、回転の角度をベクトル長さとするベクトルである。回転行列Ｒ（θx）から回転ベクトルＲｏｄ（θx）への変換は、例えば、OpenCVの関数「Rodrigues2」を用いて実行することが可能である。

上述したように、回転行列Ｒ（θx）は、基本回転位置から手先座標系Σ_TのＸ軸の回りに＋θxだけ座標系を回転させることを表す行列である。従って、回転行列Ｒ（θx）と等価な回転ベクトルＲｏｄ（θx）のベクトル方向は、回転軸の方向、すなわち、カメラ座標系Σ_Cで見た手先座標系Σ_TのＸ軸の方向を示している。

ここで、カメラ座標系Σ_Cから手先座標系Σ_Tへの回転行列^CＲ_Tを考える。上述した（１ａ）〜（１ｄ）式に示した一般の同次変換行列に関して＜回転行列Ｒの性質２＞として説明したように、任意の回転行列Ｒの３つの列成分Ｒ_ｘ，Ｒ_ｙ，Ｒ_ｚは、元の座標系から見た回転後の座標系の３つの基底ベクトルを表している。従って、上述の回転ベクトルＲｏｄ（θx）の長さを１に正規化した正規化回転ベクトルＲｏｄ*（θx）は、カメラ座標系Σ_Cから手先座標系Σ_Tへの回転行列^CＲ_TのＸ成分（最も左側の列成分）となっている。

Ｙ軸、Ｚ軸についても同様の処理を行うことにより、カメラ座標系Σ_Cから手先座標系Σ_Tへの回転行列^CＲ_Tの３つの列成分Ｒｏｄ*（θx），Ｒｏｄ*（θy），Ｒｏｄ*（θz）を得ることができる。

なお、回転行列^CＲ_Tの逆変換^TＲ_Cは、回転行列^CＲ_Tの転置行列に等しい。従って、正規化回転ベクトルＲｏｄ*（θx），Ｒｏｄ*（θy），Ｒｏｄ*（θy）を列成分として配列する代わりに、行成分として配列すれば、手先座標系Σ_Tからカメラ座標系Σ_Cへの回転行列^TＲ_Cを直接得ることが可能である。

このように、ステップＳ１５０では、ターゲット座標系である手先座標系Σ_Tの各回転軸を中心とした回転における複数の回転位置で撮像されたパターン画像から、各回転軸の方向をベクトル方向とし回転の角度をベクトル長さとする３つの回転ベクトルＲｏｄ（θx），Ｒｏｄ（θy），Ｒｏｄ（θy）を推定する。そして、これらの回転ベクトルを正規化した正規化回転ベクトルＲｏｄ*（θx），Ｒｏｄ*（θy），Ｒｏｄ*（θy）の成分を行成分又は列成分として配列することによって、手先座標系Σ_Tとカメラ座標系Σ_Cとの間の座標変換行列^CＨ_T又は^TＨ_Cを構成する回転行列^CＲ_T又は^TＲ_Cを決定することができる。

なお、ステップＳ１５０における処理には、検出誤差が含まれる可能性がある。この場合は、図５に示した例において、基本回転位置と、基本回転位置からＸ軸回りに±θx回転させた２つの回転位置と、の３つの回転位置で撮像された３つのパターン画像を利用すれば、回転行列Ｒ（θx）の他に、他の回転行列Ｒ（−θx），Ｒ（２θx）も推定できる。そこで、これらの他の回転行列Ｒ（−θx），Ｒ（２θx）をそれぞれ利用して、上述した手順で回転行列^TＲ_Pを求め、得られた複数の回転行列^TＲ_Pの平均を求めてもよい。なお、複数の回転行列の平均を求める処理は、例えば、各回転行列をクォータニオン（四元数）に変換し、複数のクォータニオンの平均を求めた後に回転行列に逆変換することによって実行可能である。

また、上述の処理で得られた回転行列^TＲ_Pが正規直交性を持たない可能性がある。この場合には、何らかの直交化手段（例えばGram-Schmidt直交化法）を用いて、回転行列^TＲ_Pの各列を直交化することが好ましい。なお、直交化を行う際の基点となる軸は、画像面と直交する軸（図５の例ではＺ軸）を選択することが好ましい。この理由は、図５から明らかなように、画像上の変位は画像面と直交する軸回りの回転の場合が最も大きいので、相対的な誤差が最も小さくなると考えられるからである。

なお、Ｘ，Ｙ，Ｚ軸での回転角度θx，θy，θzは既知である。従って、上述の処理で検出された回転角度と既知の回転角度と差が、検出誤差を考慮した許容範囲を超えている場合には、その処理結果を異常であると判定しても良い。

ステップＳ１６０では、手先座標系Σ_Tとパターン座標系Σ_Pとの間の回転行列^TＲ_P又は^PＲ_Tを算出する。前述したステップＳ１４０では、各パターン画像について、パターン座標系Σ_Pとカメラ座標系Σ_Cの間の変換^PＨ_C又は^CＨ_Pが推定されており、それらの変換^PＨ_C又は^CＨ_Pを構成する回転行列^PＲ_C又は^CＲ_Pも既知である。例えば、特定の回転位置（例えば基本回転位置）で推定された回転行列^CＲ_Pと、ステップＳ１５０で得られた回転行列^TＲ_Cとを用いて、手先座標系Σ_Tとパターン座標系Σ_Pとの間の回転行列^TＲ_Pを次式で算出できる。

図６は、ステップＳ１６０で得られた回転行列^TＲ_Pの値を示している。なお、本実施形態においては手先座標系Σ_Tとパターン座標系Σ_Pの間の変換^TＨ_Pを未知としているので、回転行列^TＲ_Pの正解とすべき値がない。そこで、図６では、図３に示したロボット１００の右目カメラ１７０Ｒと左目カメラ１７０Ｌをそれぞれ独立して用いて推定された結果を示している。２つの回転行列^TＲ_Pは良く一致しているので、回転行列^TＲ_Pが精度良く推定されていることが理解できる。なお、ステップＳ１６０は省略してもよい。

ステップＳ１７０では、手先座標系Σ_Tとパターン座標系Σ_Pの間の並進ベクトル^TＴ_P又は^PＴ_Tを推定する。ここでは、まず、校正用パターン４００を手先座標系Σ_TのＸ軸回りに回転させた場合を考える。

図７は、基本回転位置における並進ベクトル^TＴ_P（θ₀）と、校正用パターン４００を手先座標系Σ_TのＸ軸回りに回転させた回転位置における並進ベクトル^TＴ_P（θ₀＋θx），^TＴ_P（θ₀−θx）を、ＹＺ平面上に投影した図である。ここで、並進ベクトル^TＴ_Pの長さをｒ_ｘとし、並進ベクトル^TＴ_PのＸＹＺ成分を（Ｔx，Ｔy，Ｔz）とし、２つの並進ベクトル^TＴ_P（θ₀＋θx），^TＴ_P（θ₀−θx）の差分をΔＴ_x とすると、次式が成立する。

（１７ａ）〜（１７ｃ）式と同様の式は、Ｙ軸回りの回転及びＺ軸回りの回転についても成立するので、以下の通りとなる。

（１８ａ）〜（１８ｃ）を変形すると、次式が得られる。

前述した図５で説明したように、校正用パターン４００は、手先座標系Σ_Tの座標原点であるＴＣＰを固定して回転している。また、パターン座標系Σ_Pの原点位置は、校正用パターン４００上の既知の点に設定されているので、パターン画像を解析することによってパターン座標系Σ_Pの原点位置を検出できる。従って、基本回転位置から＋θx回転後の第１パターン画像から得られるパターン座標系Σ_Pの原点位置と、−θx回転後の第２パターン画像から得られるパターン座標系Σ_Pの原点位置との差分は、図７に示した並進ベクトル^TＴ_P（θ₀＋θx），^TＴ_P（θ₀−θx）の差分ΔＴ_x に等しい。Ｙ軸回りの回転及びＺ軸回りの回転についても同様である。従って、上述した（１８ａ）〜（１８ｃ）式及び（１９ａ）〜（１９ｃ）式に従って、手先座標系Σ_Tからパターン座標系Σ_Pへの並進ベクトル^TＴ_Pを推定できる。

なお、上述したステップＳ１６０では手先座標系Σ_Tとパターン座標系Σ_Pの間の回転行列^TＲ_P又は^PＲ_Tが得られている。従って、上述したステップＳ１７０の処理によって手先座標系Σ_Tからパターン座標系Σ_Pへの並進ベクトル^TＴ_Pを推定できれば、パターン座標系Σ_Pから手先座標系Σ_Tへの並進ベクトル^PＴ_Tは、上記（２）式で算出できる。

このように、ステップＳ１７０では、ターゲット座標系である手先座標系Σ_Tの各回転軸を中心とした回転の複数の回転位置で撮像されたパターン画像から、パターン座標系Σ_Pと手先座標系Σ_Tとの間の変換行列^PＨ_T又は^TＨ_Pを構成する並進ベクトル^PＴ_T又は^TＴ_Pの３つの成分Ｔ_x，Ｔ_y，Ｔ_zのうちで、各回転軸と直交する２つの座標軸方向における２つの並進ベクトル成分の２乗和ｒ_x ²，ｒ_y ²，ｒ_z ²を推定する。また、３つの回転軸においてそれぞれ推定された並進ベクトル成分の２乗和ｒ_x ²，ｒ_y ²，ｒ_z ²から、変換行列^PＨ_T又は^TＨ_Pを構成する並進ベクトル^PＴ_T又は^TＴ_Pを算出することができる。

図８は、ステップＳ１７０で得られた並進ベクトル^TＴ_Pの値を示している。なお、ここでも図６と同様に、右目カメラ１７０Ｒと左目カメラ１７０Ｌをそれぞれ独立して用いて推定された結果を示している。２つの並進ベクトル^TＴ_Pは良く一致しているので、並進ベクトル^TＴ_Pが精度良く推定されていることが理解できる。

ステップＳ１８０では、ステップＳ１４０において特定の回転位置（例えば基本回転位置）で推定された変換行列^CＨ_P又は^PＨ_Cと、ステップＳ１７０で得られた並進ベクトル^PＴ_T又は^TＴ_Pから、カメラ座標系Σ_Cと手先座標系Σ_Tの間の並進ベクトル^CＴ_T又は^TＴ_Cを算出する。例えば、カメラ座標系Σ_Cから手先座標系Σ_Tへの並進ベクトル^CＴ_Tは次式で算出できる。
ここで、^CＨ_PはステップＳ１４０において特定の回転位置（例えば基本回転位置）のパターン画像から推定された同次変換行列であり、^PＴ_TはステップＳ１７０で得られた並進ベクトルである。手先座標系Σ_Tからカメラ座標系Σ_Cへの並進ベクトル^TＴ_Cも、同様の式で算出可能である。

図４の処理によって、ターゲット座標系である手先座標系Σ_Tとカメラ座標系Σ_Cとの間の座標変換を表す同次変換行列^CＨ_T又は^TＨ_Cについて、その回転行列^CＲ_T又は^TＲ_Cと並進ベクトル^CＴ_T又は^TＴ_Cを推定できる。こうして得られた同次変換行列^CＨ_T又は^TＨ_Cは、カメラ１７０の外部パラメーター２３３として不揮発性メモリー２３０に格納される。カメラ１７０の外部パラメーター２３３と内部パラメーター２３２を利用すれば、カメラ１７０を用いた各種の検出処理や制御を実行することが可能となる。なお、カメラ１７０の外部パラメーター２３３としては、ターゲット座標系Σ_Tとカメラ座標系Σ_Cとの間の座標変換を算出可能な種々のパラメーターを採用することが可能である。例えば、ロボット座標系Σ₀とカメラ座標系Σ_Cとの間の座標変換を表す同次変換行列⁰Ｈ_C又は^CＨ₀を外部パラメーター２３３として格納するようにしてもよい。

以上のように、本実施形態では、ターゲット座標系である手先座標系Σ_Tの原点の回りに３つの回転軸Ｘ，Ｙ，Ｚを設定し、各回転軸を中心として校正用パターン４００をそれぞれ回転させて複数の回転位置で停止させるようにアーム１６０を動作させる。そして、各回転軸を中心とした回転の複数の回転位置における校正用パターン４００のパターン画像をカメラ１７０で撮像し、これらのパターン画像を用いて手先座標系系Σ_Tとカメラ座標系系Σ_Cとの間の座標変換行列^TＨ_C又は^CＨ_Tを推定する。この処理手順では、各回転軸回りの複数の回転位置のパターン画像を用いて、カメラ座標系Σ_Cで見た３つの回転軸の方向を推定できる。また、３つの回転軸Ｘ，Ｙ，Ｚは互いに一次独立なので、これらの回転軸の方向から、手先座標系Σ_Tとカメラ座標系Σ_Cとの間の座標変換行列^TＨ_C又は^CＨ_Tを決定できる。この結果、手先座標系Σ_Tとカメラ座標系Σ_Cとの間の座標変換を計算可能な外部パラメーターが得られるので、カメラ１７０を用いた対象物の位置検出を行うことが可能となる。

なお、上述した実施形態では、手先座標系Σ_Tの原点の回りの回転軸として、Ｘ軸とＹ軸とＺ軸を選択したが、３つの回転軸は１次独立であれば良く、任意の３つの回転軸を選択することが可能である。Ｘ軸とＹ軸とＺ軸以外の３つの回転軸を使用する場合には、推定された結果の各軸の成分から、手先座標系Σ_TのＸ軸とＹ軸とＺ軸の成分に変換すれば良い。但し、手先座標系Σ_Tの３つの基底ベクトルの方向（Ｘ，Ｙ、Ｚ軸）を回転軸として選択すれば、上述した処理が容易になるという利点がある。また、３つの回転軸は、ターゲット座標系である手先座標系Σ_Tの原点の回りに設定する必要はなく、他の位置に設定してもよい。但し、３つの回転軸をターゲット座標系の原点の回りに設定すれば、３つの回転軸とターゲット座標系との対応関係が単純になるなので、カメラ座標系で見た回転軸の方向から、ターゲット座標系とカメラ座標系との間の座標変換行列を容易に決定できるという利点がある。

また、上述した実施形態では、各回転軸回りの回転において基本回転位置からプラス側とマイナス側の両方に回転させていたが、いずれか一方側にのみ回転させるようにしてもよい。但し、基本回転位置からプラス側とマイナス側の両方に回転させるようにすれば、上述した処理がより容易である。また、プラス側とマイナス側の回転角度は、等しい値とすることが好ましい。

図９は、第２実施形態におけるロボットの座標系を示す説明図である。第１実施形態の図３との違いは、校正のターゲット座標系Σ_tが手先座標系Σ_Tとは別の位置に設定されている点だけであり、他の構成は第１実施形態と同じである。このターゲット座標系Σ_tは、例えば、ロボット座標系Σ₀に対して固定された相対位置姿勢を有する。第２実施形態におけるカメラ１７０の校正処理では、第１実施形態における図４の処理において、「手先座標系Σ_T」を「ターゲット座標系Σ_t」を読み替え、「ＴＣＰ」を「ターゲット座標系Σ_tの座標原点Ｔ０」と読み替えるだけで良く、処理手順は第１実施形態と同じである。

このように、手先座標系Σ_Tとは異なる位置に校正のターゲット座標系Σ_tを設定すれば、このターゲット座標系Σ_tの近傍において、カメラ１７０による物体の検出精度を高めることが可能である。例えば、作業スペースの小さな場所に、物理的に大きなハンド１８０が入らない場合がある。一方、図９に示したターゲット座標系Σ_tは、狭い隙間や、別の物体の内部に設定することも可能となる。従って、手先座標系Σ_Tとは異なる位置に校正のターゲット座標系Σ_tを設定すれば、任意の場所について、カメラ１７０による物体の検出精度を向上できる。

なお、カメラ１７０の校正処理は、ロボット座標系Σ₀に対して既知の相対位置姿勢を有するターゲット座標系Σ_tとカメラ座標系Σ_Cとの間の座標変換を計算可能な外部パラメーターを決定する処理である。ターゲット座標系Σ_tとカメラ座標系Σ_Cとの間の座標変換行列^CＨ_t（又は^tＨ_C）は、カメラ座標系Σ_Cとパターン座標系Σ_Pとの間の第１変換行列^CＨ_P（又は^PＨ_C）と、パターン座標系Σ_Pとターゲット座標系Σ_tとの間の第２変換行列^PＨ_t（又は^tＨ_P）との積で表される。このとき、図４のステップＳ１４０の処理は、ターゲット座標系Σ_tの原点の回りの３つの回転軸を中心として回転させた複数の回転位置のうちの１つの特定回転位置（第１実施形態では基本回転位置）で撮像されたパターン画像から、第１変換行列^CＨ_P（又は^PＨ_C）を推定する処理に相当する。ステップＳ１５０の処理は、複数の回転位置で撮像されたパターン画像から、各回転軸の方向をベクトル方向とし回転の角度をベクトル長さとする３つの回転ベクトルを推定し、これらの３つの回転ベクトルをそれぞれ正規化し、正規化された３つの回転ベクトルの成分を行成分又は列成分として配列することによって、ターゲット座標系Σ_tとカメラ座標系Σ_Cとの間の座標変換行列^CＨ_t（又は^tＨ_C）を構成する回転行列^CＲ_t（又は^tＲ_C）を決定する処理に相当する。また、ステップＳ１７０の処理は、複数の回転位置で撮像されたパターン画像から、第２変換行列^PＨ_t（又は^tＨ_P）を構成する並進ベクトルの３つの成分のうちで各回転軸と直交する２つの座標軸方向における２つの並進ベクトル成分の２乗和を推定し、３つの回転軸においてそれぞれ推定された並進ベクトル成分の２乗和から第２変換行列^PＨ_t（又は^tＨ_P）を構成する並進ベクトル^PＴ_t（又は^tＴ_P）を算出する処理に相当する。そして、ステップＳ１８０の処理は、特定回転位置で推定された第１変換行列^CＨ_P（又は^PＨ_C）と、第２変換行列^PＨ_t（又は^tＨ_P）の並進ベクトル^PＴ_t（又は^tＴ_P）から、座標変換行列^CＨ_t（又は^tＨ_C）の並進ベクトル^CＴ_t（又は^tＴ_C）を算出する処理に相当する。このような処理を実行することにより、各回転軸を中心とした回転の複数の回転位置で撮像されたパターン画像から、ターゲット座標系Σ_tとカメラ座標系Σ_Cとの間の座標変換行列^CＨ_t（又は^tＨ_C）を構成する回転行列と並進ベクトルとを容易に求めることが可能である。

上述した実施形態では、ロボット１００の頭部１５０のカメラ１７０に関する校正を説明したが、本発明は、頭部１５０以外の場所に設置されたロボット内蔵型カメラや、ロボット１００とは別個に設置されたカメラの校正にも適用可能である。また、本発明は、双腕ロボットに限らず、単腕ロボットにも適用可能である。

本発明は、上述の実施形態や実施例、変形例に限られるものではなく、その趣旨を逸脱しない範囲において種々の構成で実現することができる。例えば、発明の概要の欄に記載した各形態中の技術的特徴に対応する実施形態、実施例、変形例中の技術的特徴は、上述の課題の一部又は全部を解決するために、あるいは、上述の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。

１００…ロボット、１１０…基台、１２０…胴部、１３０…肩部、１４０…首部、１５０…頭部、１６０，１６０Ｌ，１６０Ｒ…アーム、１７０，１７０Ｌ，１７０Ｒ…カメラ、１８０Ｌ，１８０Ｒ…ハンド、１９０Ｌ，１９０Ｒ…力覚センサー、２００…制御装置、２１０…プロセッサー、２１１…アーム制御部、２１２…カメラ制御部、２１３…カメラ校正実行部、２１４…変換行列推定部、２２０…メインメモリー、２３０…不揮発性メモリー、２３１…プログラム命令、２３２…カメラ内部パラメーター、２３３…カメラ外部パラメーター、２４０…表示制御部、２５０…表示部、２６０…Ｉ／Ｏインターフェース、４００…校正用パターン

Claims (10)

1. カメラの校正用パターンを設置可能に構成されたアームを備えるロボットと、前記アームとは独立して設置されたカメラと、を制御する制御装置であって、
   前記アームを制御するアーム制御部と、
   前記カメラを制御するカメラ制御部と、
   前記ロボットのロボット座標系に対して既知の相対位置姿勢を有するターゲット座標系と、前記カメラのカメラ座標系との間の座標変換を計算可能な前記カメラのパラメーターを決定するカメラ校正実行部と、
   を備え、
   前記アーム制御部は、互いに一次独立な３つの回転軸の各回転軸を中心として前記校正用パターンをそれぞれ回転させて複数の回転位置で停止させるように前記アームを動作させ、
   前記カメラ制御部は、前記複数の回転位置における前記校正用パターンのパターン画像を前記カメラに撮像させ、
   前記カメラ校正実行部は、前記複数の回転位置で撮像された前記パターン画像を用いて前記パラメーターを決定する、制御装置。
2. 請求項１に記載の制御装置であって、
   前記３つの回転軸は、前記ターゲット座標系の原点の回りに設定される、制御装置。
3. 請求項１又は２に記載の制御装置であって、
   前記カメラ校正実行部は、
   前記複数の回転位置で撮像された前記パターン画像から、各回転軸の方向をベクトル方向とし前記回転の角度をベクトル長さとする３つの回転ベクトルを推定し、
   前記３つの回転ベクトルをそれぞれ正規化して３つの正規化回転ベクトルを求め、
   前記３つの正規化回転ベクトルを行成分又は列成分として配列することによって前記ターゲット座標系と前記カメラ座標系との間の座標変換行列を構成する回転行列を決定する、
   制御装置。
4. 請求項３に記載の制御装置であって、
   前記ターゲット座標系と前記カメラ座標系との間の座標変換行列は、前記カメラ座標系と前記校正用パターンのパターン座標系との間の第１変換行列と、前記パターン座標系と前記ターゲット座標系との間の第２変換行列と、の積で表され、
   前記カメラ校正実行部は、
   （ａ）前記複数の回転位置のうちの１つの特定回転位置において撮像された前記パターン画像から前記第１変換行列を推定し、
   （ｂ）前記複数の回転位置で撮像された前記パターン画像から、前記第２変換行列を構成する並進ベクトルの３つの成分のうちで各回転軸と直交する２つの座標軸方向における２つの並進ベクトル成分の２乗和を推定し、前記３つの回転軸についてそれぞれ推定された前記並進ベクトル成分の２乗和から前記第２変換行列を構成する前記並進ベクトルを算出し、
   （ｃ）前記特定回転位置で推定された前記第１変換行列と、前記第２変換行列の並進ベクトルから、前記座標変換行列を構成する並進ベクトルを算出する、制御装置。
5. 請求項１〜４のいずれか一項に記載の制御装置であって、
   前記ターゲット座標系は、前記アームとは独立に前記ロボットのロボット座標系に対して固定された相対位置姿勢を有する座標系である、制御装置。
6. 請求項１〜４のいずれか一項に記載の制御装置であって、
   前記ターゲット座標系は、前記アームの手先座標系である、制御装置。
7. カメラの校正用パターンを設置可能に構成されたアームを備えるロボットと、前記アームとは独立して設置されたカメラと、を制御する制御装置であって、
   プロセッサーを備え、
   前記プロセッサーは、
   互いに一次独立な３つの回転軸の各回転軸を中心として前記校正用パターンをそれぞれ回転させて複数の回転位置で停止させるように前記アームを動作させ、
   前記複数の回転位置における前記校正用パターンのパターン画像を前記カメラに撮像させ、
   前記複数の回転位置で撮像された前記パターン画像を用いて、前記ロボットのロボット座標系に対して既知の相対位置姿勢を有するターゲット座標系と前記カメラのカメラ座標系との間の座標変換を計算可能な前記カメラのパラメーターを決定する、制御装置。
8. 請求項１〜７のいずれか一項に記載の制御装置に接続されたロボット。
9. ロボットと、
   前記ロボットに接続された請求項１〜７のいずれか一項に記載の制御装置と、
   を備えるロボットシステム。
10. カメラの校正用パターンを設置可能に構成されたアームを備えるロボットと、前記アームとは独立して設置されたカメラと、を備えるロボットシステムにおいて前記カメラの校正を行う方法であって、
    互いに一次独立な３つの回転軸の各回転軸を中心として前記校正用パターンをそれぞれ回転させて複数の回転位置で停止させるように前記アームを動作させ、
    前記複数の回転位置における前記校正用パターンのパターン画像を前記カメラに撮像させ、
    前記複数の回転位置で撮像された前記パターン画像を用いて、前記ロボットのロボット座標系に対して既知の相対位置姿勢を有するターゲット座標系と前記カメラのカメラ座標系との間の座標変換を計算可能な前記カメラのパラメーターを決定する、方法。