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

Description

本発明は、ロボットの制御装置、ロボットシステム、並びに、カメラの校正（キャリブレーション）方法に関するものである。

ロボットに高度な処理を行わせるために、ロボットにカメラを設置して眼の機能を持たせる場合がある。カメラの設置方法として、カメラをロボットアームとは独立して設置する方法と、カメラをロボットアームに設置する方法（ハンドアイ）とがある。ハンドアイは、より広い視野を得ることができ、また作業する手先部の視野が確保できるなどの利点がある。

特許文献１には、アームに設置されたカメラを利用したロボットシステムにおける座標系の校正方法が記載されている。特許文献１にも記載されているように、アームに設置されたカメラを利用する場合には、カメラ座標系とロボットの座標系との間の未知の変換行列Ｘに関していわゆるＡＸ＝ＸＢ問題を解く必要があり、カメラの校正が困難であるという課題がある。ＡＸ＝ＸＢ問題の解法には、非線形最適化処理が必要であり、また、最適解が与えられる保証がない。ＡＸ＝ＸＢ問題を回避するため、特許文献１では、ロボットの動きに制限を加えることにより、座標系の変換行列を求める問題を線形化する技術が記載されている。

特開２０１２－９１２８０号公報

しかしながら、上記特許文献１に記載された技術では、処理結果として得られる変換行列が、画像を用いた校正用パターンの位置推定の精度に依存するという課題がある。すなわち、校正用パターンの位置推定の精度を上げるためには、ロボットの動きが大きい方が有利であるが、ロボットの大きな動きは、動きの精度が低くなるという問題がある。一方、ロボットの動きの精度を上げようとすれば、動きを小さくするのが好ましいが、この場合は画像を用いた校正パターンの位置推定の精度が低くなるという問題がある。そこで、特許文献１とは異なる方法で、アームに設置されたカメラの校正を容易に行うことのできる技術が望まれている。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態（aspect）として実現することが可能である。
本発明の一形態は、カメラが設置されたアームを備えるロボットを制御する制御装置であって、前記アームを制御するアーム制御部と、前記カメラを制御するカメラ制御部と、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と、を備える。前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定する。前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する。 本発明の他の形態は、カメラが設置されたアームを備えるロボットと、 前記ロボットに接続された制御装置と、を備えるロボットシステムである。前記制御装置は、前記アームを制御するアーム制御部と、前記カメラを制御するカメラ制御部と、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と、を有し、前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定する。前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する。
本発明の更に他の形態は、カメラが設置されたアームを備えるロボットにおいて前記カメラの校正を行う方法であって、前記カメラの校正用パターンのパターン画像を前記カメラで撮像し、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の間の関係を算出し、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する。前記座標変換行列の推定において、前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する。

（１）本発明の第１の形態によれば、カメラが設置されたアームを備えるロボットを制御する制御装置が提供される。この制御装置は、前記アームを制御するアーム制御部と；前記カメラを制御するカメラ制御部と；前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と；を備える。前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させる。前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定する。
この制御装置によれば、パターン画像の撮像時におけるアームの位置姿勢からアーム座標系と手先座標系の関係を決定できる。また、カメラ校正実行部は、アーム座標系とパターン座標系の関係を算出可能なので、これらの関係に加えて、カメラで撮像したパターン画像から得られるパターン座標系とカメラ座標系との関係を利用すれば、手先座標系とカメラ座標系との間の座標変換行列を推定することが可能である。この結果、その座標変換行列を含むカメラのパラメーターを作成することができ、カメラを用いた対象物の位置検出を行うことが可能となる。

（２）上記制御装置において、前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し；前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し；前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し；前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出するものとしてもよい。
この制御装置によれば、アームの位置姿勢から第１変換行列を算出できる。また、カメラ校正実行部は、アーム座標系とパターン座標系の座標変換を表す第２変換行列を算出又は推定し、更に、パターン画像から第３変換行列を推定するので、これらの変換行列から手先座標系とカメラ座標系との間の座標変換行列を含むカメラのパラメーターを容易に求めることが可能である。

（３）上記制御装置において、前記ロボットは、前記校正用パターンを予め定めた設置状態で設置可能に構成された第２アームを有し、前記カメラ校正実行部は、前記パターン画像の撮像時における前記第２アームの位置姿勢から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を算出するものとしてもよい。
この制御装置によれば、第２アームの位置姿勢から第２変換行列を算出できるので、手先座標系とカメラ座標系との間の座標変換行列を容易に求めることが可能である。

（４）上記制御装置において、前記カメラ制御部は、前記校正用パターンの第２パターン画像を前記アームとは独立して配置された固定カメラで撮像させ、前記カメラ校正実行部は、前記第２パターン画像から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を推定するものとしてもよい。
この制御装置によれば、第２パターン画像から第２変換行列を推定できるので、手先座標系とカメラ座標系との間の座標変換行列を容易に求めることが可能である。

（５）上記制御装置において、前記固定カメラはステレオカメラであるものとしてもよい。
この制御装置によれば、ステレオカメラで撮像した第２パターン画像から、第２変換行列を精度良く推定できるので、手先座標系とカメラ座標系との間の座標変換行列を精度良く求めることが可能である。

（６）本発明の第２の形態によれば、カメラが設置されたアームを備えるロボットを制御する制御装置が提供される。この制御装置は、プロセッサーを備える。前記プロセッサーは、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出し、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する。
この制御装置によれば、パターン画像の撮像時におけるアームの位置姿勢からアーム座標系と手先座標系の関係を決定できる。また、アーム座標系とパターン座標系の関係を算出可能なので、これらの関係に加えて、カメラで撮像したパターン画像から得られるパターン座標系とカメラ座標系との関係を利用すれば、手先座標系とカメラ座標系との間の座標変換行列を推定することが可能である。この結果、その座標変換行列を含むカメラのパラメーターを作成することができ、カメラを用いた対象物の位置検出を行うことが可能となる。

（７）本発明の第３の形態は、上記制御装置に接続されたロボットである。
このロボットによれば、手先座標系とカメラ座標系との間の座標変換行列を容易に推定することが可能である。

（８）本発明の第４の形態は、ロボットと、前記ロボットに接続された上記制御装置と、を備えるロボットシステムである。
このロボットシステムによれば、手先座標系とカメラ座標系との間の座標変換行列を容易に推定することが可能である。

（９）本発明の第５の形態によれば、カメラが設置されたアームを備えるロボットにおいて前記カメラの校正を行う方法が提供される。この方法は、前記カメラの校正用パターンのパターン画像を前記カメラで撮像し；前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出し；前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する。
この方法によれば、パターン画像の撮像時におけるアームの位置姿勢からアーム座標系と手先座標系の関係を決定できる。また、アーム座標系とパターン座標系の関係を算出可能なので、これらの関係に加えて、カメラで撮像したパターン画像から得られるパターン座標系とカメラ座標系との関係を利用すれば、手先座標系とカメラ座標系との間の座標変換行列を推定することが可能である。この結果、その座標変換行列を含むカメラのパラメーターを作成することができ、カメラを用いた対象物の位置検出を行うことが可能となる。

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

ロボットシステムの概念図。 ロボットと制御装置の機能を示すブロック図。 第１実施形態のロボットの座標系を示す説明図。 第１実施形態の処理手順を示すフローチャート。 第２実施形態のロボットの座標系を示す説明図。 第２実施形態の処理手順を示すフローチャート。 第３実施形態のロボットの座標系を示す説明図。

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

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

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

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

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

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

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

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

アーム１６０の手先には、ツール中心点ＴＣＰ（Tool Center Point）が設定されている。典型的には、ロボット１００の制御は、ツール中心点ＴＣＰの位置姿勢を制御するために実行される。なお、位置姿勢（position and attitude）とは、３次元の座標系における３つ座標値と、各座標軸回りの回転とで規定される状態を意味する。

アーム１６０Ｌ，１６０Ｒには、校正用パターン４００を予め定めた設置状態で設置可能である。図３の例では、左アーム１６０Ｌのハンドアイ１７５Ｌの校正（キャリブレーション）に使用される校正用パターン４００が、右アーム１６０Ｒの手先部に固定されている。校正用パターン４００を右アーム１６０Ｒに取り付ける際には、右アーム１６０Ｒのハンド１８０Ｒを取り外すようにしてもよい。左アーム１６０Ｌのハンド１８０Ｌも同様である。

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

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

アーム座標系Σ_A1，Σ_A2と手先座標系Σ_T1，Σ_T2は、左アーム１６０Ｌと右アーム１６０Ｒにそれぞれ別個に設定される。以下では、左アーム１６０Ｌに関する座標系を、「第１アーム座標系Σ_A1」「第１手先座標系Σ_T1」とも呼び、右アーム１６０Ｒに関する座標系を「第２アーム座標系Σ_A2」「第２手先座標系Σ_T2」と呼ぶ。なお、アーム座標系Σ_A1，Σ_A2とロボット座標系Σ₀の相対位置姿勢は既知である。ハンドアイ座標系Σ_Eも、ハンドアイ１７５Ｌ，１７５Ｒにそれぞれ別個に設定される。以下の説明では、左アーム１６０Ｌのハンドアイ１７５Ｌを校正対象とするので、ハンドアイ座標系Σ_Eとして左アーム１６０Ｌのハンドアイ１７５Ｌの座標系を使用する。なお、図３では、図示の便宜上、個々の座標系の原点が実際の位置からずれた位置に描かれている。

一般に、或る座標系Σ_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. 座標変換のＡＸ＝ＸＢ問題
図３において、座標系Σ_A1，Σ_T1，Σ_E，Σ_Pの間には、以下の変換が成立する。
（１）変換^A1Ｈ_T1（計算可）：第１アーム座標系Σ_A1から第１手先座標系Σ_T1への変換
（２）変換^T1Ｈ_E（未知）：第１手先座標系Σ_T1からハンドアイ座標系Σ_Eへの変換
（３）変換^EＨ_P（推定可）：ハンドアイ座標系Σ_Eからパターン座標系Σ_Pへの変換
（４）変換^PＨ_A1（未知）：パターン座標系Σ_Pから第１アーム座標系Σ_A1への変換

上述の４つの変換^A1Ｈ_T1，^T1Ｈ_E，^EＨ_P，^PＨ_A1のうち、変換^A1Ｈ_T1は、第１アーム座標系Σ_A1から第１手先座標系Σ_T1への変換である。第１手先座標系Σ_T1は、第１アーム１６０ＬのＴＣＰの位置姿勢を示している。通常、第１アーム座標系Σ_A1に対するＴＣＰの位置姿勢を求める処理はフォワードキネマティクスと呼ばれ、アーム１６０Ｌの幾何学的な形状と各関節の動作量（回転角度）が定まれば計算できる。つまり、この変換^A1Ｈ_T1は計算可能な変換である。

変換^T1Ｈ_Eは、第１手先座標系Σ_T1からハンドアイ座標系Σ_Eへの変換である。この変換^T1Ｈ_Eは未知であり、この変換^T1Ｈ_Eを求めることがハンドアイ１７５の校正（キャリブレーション）に相当する。

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

変換^PＨ_A1は、パターン座標系Σ_Pから第１アーム座標系Σ_A1への変換である。この変換^PＨ_A1は未知である。

上記の変換^A1Ｈ_T1，^T1Ｈ_E，^EＨ_P，^PＨ_A1を順に辿れば最初の第１アーム座標系Σ_A1に戻るので、恒等変換Ｉを用いて以下の式が成立する。

（５）式の両辺に、左から各変換の逆行列^A1Ｈ_T1 ^－１, ^T1Ｈ_E ^－１, ^EＨ_P ^－１を順に乗算すれば次式が得られる。

（６）式において、変換^EＨ_Pはカメラキャリブレーション関数を用いて推定でき、変換^A1Ｈ_T1は計算可能である。従って、変換^T1Ｈ_Eが既知であれば、右辺は計算可能であり、左辺の変換^PＨ_A1を知ることができる。

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

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

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

（９）式の両辺に、左から^A1Ｈ_T1(j)^－１を、右から^EＨ_P(i)^－１を乗算すると次式となる。

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

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

以下に詳述するように、第１実施形態では、校正用パターン４００が設置された第２アーム１６０Ｒを任意に制御可能であることを利用し、第２アーム１６０Ｒの位置姿勢から第２アーム座標系Σ_A2とパターン座標系Σ_Pの関係を計算することにより第１手先座標系Σ_T1とハンドアイ座標系Σ_Eの間の変換^T1Ｈ_E又は^EＨ_T1を推定する。この結果、ハンドアイ１７５の外部パラメーターを決定することができる。

このような処理を行うために、第１実施形態では、上記の変換^A1Ｈ_T1，^T1Ｈ_E，^EＨ_P，^PＨ_A1の他に、以下の変換を利用する。
（５）変換^A1Ｈ_A2（既知）：第１アーム座標系Σ_A1から第２アーム座標系Σ_A2への変換
（６）変換^A2Ｈ_T2（計算可）：第２アーム座標系Σ_A2から第２手先座標系Σ_T2への変換
（７）変換^T2Ｈ_P（既知）：第２手先座標系Σ_T2からパターン座標系Σ_Pへの変換
第２手先座標系Σ_T2からパターン座標系Σ_Pへの変換^T2Ｈ_Pは、既知であると仮定する。アーム１６０Ｒの手首部に校正用パターン４００を設置するための治具（例えばフランジ）が高精度に設計・製造されていれば、その設計データからこの変換^T2Ｈ_Pを決定することが可能である。或いは、アーム１６０Ｒの手首部に設置された校正用パターン４００を固定カメラ１７０で撮像し、そのパターン画像からカメラ座標系Σ_Cとパターン座標系Σ_Pの変換^CＨ_Pを推定し、この変換^CＨ_Pを利用して、第２手先座標系Σ_T2からパターン座標系Σ_Pへの変換^T2Ｈ_Pを予め求めるようにしてもよい。

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

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

ステップＳ１３０～Ｓ１７０は、ハンドアイ１７５の外部パラメーターを推定する処理である。ステップＳ１３０では、ハンドアイ１７５を用いて、校正用パターン４００を特定の位置姿勢で撮像する。なお、上述したステップＳ１１０では、校正用パターン４００を複数の位置姿勢で撮像しているので、それらの複数の位置姿勢のうちの１つを「特定の位置姿勢」として利用してもよい。この場合には、ステップＳ１３０は省略可能である。以下では、校正用パターン４００が特定の位置姿勢を取るロボット１００の状態を、単に「特定位置姿勢状態」と呼ぶ。

ステップＳ１４０では、特定位置姿勢状態における第１アーム座標系Σ_A1と第１手先座標系Σ_T1の間の変換^A1Ｈ_T1又は^T1Ｈ_A1を算出する。この変換^A1Ｈ_T1又は^T1Ｈ_A1は、アーム１６０Ｌのフォワードキネマティクスによって算出可能である。

ステップＳ１５０では、特定位置姿勢状態における第１アーム座標系Σ_A1とパターン座標系Σ_Pの間の変換^A1Ｈ_P又は^A1Ｈ_Pを算出する。例えば、変換^A1Ｈ_Pは、次式で算出できる。

（１２）式の右辺の３つの変換^A1Ｈ_A2，^A2Ｈ_T2，^T2Ｈ_Pのうち、１番目の変換^A1Ｈ_A2と３番目の変換^T2Ｈ_Pは一定であり、２番目の変換^A2Ｈ_T2は第２アーム１６０Ｒの位置姿勢によって算出される。

このように、ステップＳ１５０では、特定位置姿勢状態における第２アーム１６０Ｒの位置姿勢から、第１アーム座標系Σ_A1とパターン座標系Σ_Pの間の変換^A1Ｈ_P又は^A1Ｈ_Pを算出する。すなわち、カメラ校正実行部２１３は、特定位置姿勢状態における第１アーム座標系Σ_A1とパターン座標系Σ_Pの関係を算出可能である。

ステップＳ１６０では、特定位置姿勢状態においてハンドアイ１７５で撮像されたパターン画像を用いて、ハンドアイ座標系Σ_Eとパターン座標系Σ_Pの間の変換^EＨ_P又は^PＨ_Eを推定する。この推定は、ステップＳ１２０で得られた内部パラメーターを利用し、カメラの外部パラメーターを推定する標準的なソフトウェア（例えばOpenCVの関数「FindExtrinsicCameraParams2」）を用いて実行可能である。

ステップＳ１７０では、第１手先座標系とハンドアイ座標系の変換^T1Ｈ_E，^EＨ_T1を算出する。例えば変換^T1Ｈ_Eについては、図３において次式が成立する。

（１３）式の右辺の５つの変換のうちは、第１の変換^T1Ｈ_A1は、ステップＳ１４０で算出されている。第２の変換^A1Ｈ_A2は、既知である。第３の変換^A2Ｈ_T2は、アーム１６０Ｒのフォワードキネマティクスによって算出可能である。第４の変換^T2Ｈ_Pは、既知である。第５の変換^PＨ_Eは、ステップＳ１６０で推定されている。従って、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eの変換^T1Ｈ_Eを、（１３）式に従って算出することができる。

こうして得られた同次変換行列^T1Ｈ_E又は^EＨ_T1は、ハンドアイ１７５の外部パラメーター２３３として不揮発性メモリー２３０に格納される。ハンドアイ１７５の外部パラメーター２３３と内部パラメーター２３２を利用すれば、ハンドアイ１７５を用いた各種の検出処理や制御を実行することが可能となる。なお、ハンドアイ１７５の外部パラメーター２３３としては、ロボット座標系Σ₀とハンドアイ座標系Σ_Eとの間の座標変換を算出可能な種々のパラメーターを採用することが可能である。

このように、第１実施形態では、パターン画像の撮像時におけるアーム１６０の位置姿勢と、パターン画像とを用いて、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eとの間の座標変換行列^T1Ｈ_Eを推定することができる。特に、第１実施形態では、カメラ校正実行部２１３は、ステップＳ１４０において、パターン画像の撮像時におけるアーム１６０の位置姿勢から、第１アーム座標系Σ_A1と第１手先座標系Σ_T1との間の第１変換行列^A1Ｈ_T1又は^T1Ｈ_A1を算出する。また、ステップＳ１５０において、パターン座標系Σ_Pと第１アーム座標系Σ_A1との間の第２変換行列^A1Ｈ_P又は^A1Ｈ_Pを算出する。更に、ステップＳ１６０において、パターン画像から、ハンドアイ座標系Σ_Eとパターン座標系Σ_Pとの間の第３変換行列^EＨ_P又は^PＨ_Eを推定する。そして、ステップＳ１７０において、これらの変換行列から、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eの間の座標変換行列^T1Ｈ_E又は^T1Ｈ_Eを算出する。従って、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eの間の座標変換行列^T1Ｈ_E又は^EＨ_T1を含むハンドアイ１７５の外部パラメーターを容易に求めることが可能である。

E. 第２実施形態
図５は、第２実施形態におけるロボット１００の座標系を示す説明図である。第１実施形態の図３との違いは、第２手先座標系Σ_T2からパターン座標系Σ_Pへの変換^T2Ｈ_Pを既知とする代わりに、固定カメラ１７０を用いて、固定カメラ１７０のカメラ座標系Σ_Cとパターン座標系Σ_Pとの間の変換^CＨ_P又は^PＨ_Cを推定する点にある。図１及び図２に示したロボット１００の構成は第１実施形態と同一である。

固定カメラ１７０としては、２つのカメラ１７０Ｌ，１７０Ｒのうちの一方又は両方が使用される。但し、２つのカメラ１７０Ｌ，１７０Ｒをステレオカメラとして利用すれば、校正用パターン４００の位置姿勢をより正確に推定することができる。なお、第２実施形態において、カメラ１７０については、予め校正が完了しており、その内部パラメーターと外部パラメーターが決定されているものと仮定する。また、第１アーム座標系Σ_A1とカメラ座標系Σ_Cとの間の変換^A1Ｈ_Cは、既知であると仮定する。

図６は、第２実施形態におけるハンドアイ１７５の校正処理の手順を示すフローチャートである。第１実施形態の図４との違いは、図４のステップＳ１５０が、３つのステップＳ１５１～Ｓ１５３を含むステップＳ１５０ａに置き換えられている点だけであり、他のステップは同一である。

ステップＳ１５１では、固定カメラ１７０を用いて、校正用パターン４００を特定の位置姿勢で撮像する。この特定の位置姿勢は、ステップＳ１３０における特定の位置姿勢と同じである。ステップＳ１５２では、特定位置姿勢状態において固定カメラ１７０で撮像されたパターン画像（第２パターン画像）を用いて、カメラ座標系Σ_Cとパターン座標系Σ_Pの間の変換^CＨ_P又は^PＨ_Cを推定する。例えば、固定カメラ１７０としてステレオカメラを利用すれば、校正用パターン４００を撮像したパターン画像から、パターン座標系Σ_Pの位置姿勢を決定できるので、カメラ座標系Σ_Cとパターン座標系Σ_Pの間の変換^CＨ_P又は^PＨ_Cを推定することが可能である。一方、１個の固定カメラ１７０を使用した場合には、カメラの外部パラメーターを推定する標準的なソフトウェア（例えばOpenCVの関数「FindExtrinsicCameraParams2」）を用いてカメラ座標系Σ_Cとパターン座標系Σ_Pの間の変換^CＨ_P又は^PＨ_Cを推定可能である。

ステップＳ１５３では、特定位置姿勢状態における第１アーム座標系Σ_A1とパターン座標系Σ_Pの間の変換^A1Ｈ_P又は^A1Ｈ_Pを算出する。例えば、変換^A1Ｈ_Pは、次式で算出できる。

（１４）式の右辺の２つの変換のうち、第１の変換^A1Ｈ_Cは、既知である。第２の変換^CＨ_Pは、ステップＳ１５２で推定されている。

このように、第２実施形態では、ステップＳ１５０ａにおいて、固定カメラ１７０で撮像された第２パターン画像から、第１アーム座標系Σ_A1とパターン座標系Σ_Pの間の変換^A1Ｈ_P又は^A1Ｈ_Pを推定することが可能である。すなわち、カメラ校正実行部２１３は、特定位置姿勢状態における第１アーム座標系Σ_A1とパターン座標系Σ_Pの関係を推定可能である。

第１アーム座標系Σ_A1とパターン座標系Σ_Pの間の変換^A1Ｈ_P又は^A1Ｈ_Pが決定されると、第１実施形態と同様に、ステップＳ１６０，Ｓ１７０の処理によって、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eの同次変換行列^T1Ｈ_E又は^EＨ_T1を含むハンドアイ１７５の外部パラメーターを求めることが可能である。

このように、第２実施形態においても、パターン画像の撮像時におけるアーム１６０の位置姿勢と、パターン画像とを用いて、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eとの間の座標変換行列^T1Ｈ_Eを推定することができる。特に、第２実施形態では、ステップＳ１５０ａにおいて、校正用パターン４００の第２パターン画像をアーム１６０とは独立して配置された固定カメラ１７０で撮像し、この第２パターン画像から、パターン座標系Σ_Pと第１アーム座標系Σ_A1との間の第２変換行列^A1Ｈ_P又は^A1Ｈ_Pを推定する。すなわち、第２パターン画像から第２変換行列^A1Ｈ_P又は^A1Ｈ_Pを推定できるので、第１手先座標系Σ_T1とハンドアイ座標系Σ_Eとの間の座標変換行列^T1Ｈ_E又は^EＨ_T1を容易に求めることが可能である。

F. 第３実施形態
図７は、第３実施形態におけるロボット１００ａの座標系を示す説明図である。第２実施形態の図６との違いは、ロボット１００ａが１つアーム１６０を有する単腕ロボットである点と、固定カメラ１７０がロボット１００ａとは独立に設置されている点である。第２実施形態と同様に、アーム座標系Σ_A1とカメラ座標系Σ_Cの間の変換^A1Ｈ_Cは、既知であると仮定する。第３実施形態の処理手順は、第２実施形態の図６の処理手順と同じなので、説明を省略する。

第３実施形態も、第２実施形態と同様に、パターン画像の撮像時におけるアーム１６０の位置姿勢と、パターン画像とを用いて、手先座標系Σ_Tとハンドアイ座標系Σ_Eとの間の座標変換行列^T1Ｈ_E又は^EＨ_T1を推定することができる。また、この座標変換行列^T1Ｈ_E又は^EＨ_T1を含むハンドアイ１７５の外部パラメーターを求めることが可能である。

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

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

Claims (6)

1. カメラが設置されたアームを備えるロボットを制御する制御装置であって、
   前記アームを制御するアーム制御部と、
   前記カメラを制御するカメラ制御部と、
   前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と、
   を備え、
   前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、
   前記カメラ校正実行部は、
   前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、
   前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定し、
   前記カメラ校正実行部は、
   前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、
   前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、
   前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、
   前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する、制御装置。
2. 請求項１に記載の制御装置であって、
   前記ロボットは、前記校正用パターンを予め定めた設置状態で設置可能に構成された第２アームを有し、
   前記カメラ校正実行部は、前記パターン画像の撮像時における前記第２アームの位置姿勢から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を算出する、
   制御装置。
3. 請求項１に記載の制御装置であって、
   前記カメラ制御部は、前記校正用パターンの第２パターン画像を前記アームとは独立して配置された固定カメラで撮像させ、
   前記カメラ校正実行部は、前記第２パターン画像から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を推定する、
   制御装置。
4. 請求項３に記載の制御装置であって、
   前記固定カメラはステレオカメラである、制御装置。
5. カメラが設置されたアームを備えるロボットと、 前記ロボットに接続された制御装置と、を備え、
   前記制御装置は、
   前記アームを制御するアーム制御部と、
   前記カメラを制御するカメラ制御部と、
   前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と、
   を有し、
   前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、
   前記カメラ校正実行部は、
   前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、
   前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定し、
   前記カメラ校正実行部は、
   前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、
   前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、
   前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、
   前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する、
   ロボットシステム。
6. カメラが設置されたアームを備えるロボットにおいて前記カメラの校正を行う方法であって、
   前記カメラの校正用パターンのパターン画像を前記カメラで撮像し、
   前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の間の関係を算出し、
   前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、
   前記座標変換行列の推定において、
   前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、
   前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、
   前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、
   前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する、方法。

Images