(57)【要約】【課題】ロボットのアームに設置されたカメラの校正を容易に行うことのできる技術を提供する。【解決手段】ロボットの制御装置は、アームの手先座標系とカメラのカメラ座標系との間の座標変換行列を含むカメラのパラメーターを作成するカメラ校正実行部を備える。カメラ校正実行部は、校正用パターンのパターン画像の撮像時におけるアーム座標系とパターン座標系の関係を算出可能であり、パターン画像の撮像時におけるアームの位置姿勢と、パターン画像とを用いて、アームの手先座標系とカメラのカメラ座標系との間の座標変換行列を推定する。【選択図】図３
2019014030.tif 000002
【特許請求の範囲】
【請求項１】
カメラが設置されたアームを備えるロボットを制御する制御装置であって、
前記アームを制御するアーム制御部と、
前記カメラを制御するカメラ制御部と、
前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と、
を備え、
前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、
前記カメラ校正実行部は、
前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、
前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定する、制御装置。
【請求項２】
請求項１に記載の制御装置であって、
前記カメラ校正実行部は、
前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し、
前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し、
前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し、
前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出する、制御装置。
【請求項３】
請求項２に記載の制御装置であって、
前記ロボットは、前記校正用パターンを予め定めた設置状態で設置可能に構成された第２アームを有し、
前記カメラ校正実行部は、前記パターン画像の撮像時における前記第２アームの位置姿勢から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を算出する、
制御装置。
【請求項４】
請求項２に記載の制御装置であって、
前記カメラ制御部は、前記校正用パターンの第２パターン画像を前記アームとは独立して配置された固定カメラで撮像させ、
前記カメラ校正実行部は、前記第２パターン画像から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を推定する、
制御装置。
【請求項５】
請求項４に記載の制御装置であって、
前記固定カメラはステレオカメラである、制御装置。
【請求項６】
カメラが設置されたアームを備えるロボットを制御する制御装置であって、
プロセッサー、を備え、
前記プロセッサーは、
前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、
前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の間の座標変換を実行し、
前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する、制御装置。
【請求項７】
請求項１〜６のいずれか一項に記載の制御装置に接続されたロボット。
【請求項８】
ロボットと、
前記ロボットに接続された請求項１〜６のいずれか一項に記載の制御装置と、
を備えるロボットシステム。
【請求項９】
カメラが設置されたアームを備えるロボットにおいて前記カメラの校正を行う方法であって、
前記カメラの校正用パターンのパターン画像を前記カメラで撮像し、
前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の間の関係を算出し、
前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する、方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットのカメラの校正（キャリブレーション）に関するものである。
【背景技術】
【０００２】
ロボットに高度な処理を行わせるために、ロボットにカメラを設置して眼の機能を持たせる場合がある。カメラの設置方法として、カメラをロボットアームとは独立して設置する方法と、カメラをロボットアームに設置する方法（ハンドアイ）とがある。ハンドアイは、より広い視野を得ることができ、また作業する手先部の視野が確保できるなどの利点がある。
【０００３】
特許文献１には、アームに設置されたカメラを利用したロボットシステムにおける座標系の校正方法が記載されている。特許文献１にも記載されているように、アームに設置されたカメラを利用する場合には、カメラ座標系とロボットの座標系との間の未知の変換行列Ｘに関していわゆるＡＸ＝ＸＢ問題を解く必要があり、カメラの校正が困難であるという課題がある。ＡＸ＝ＸＢ問題の解法には、非線形最適化処理が必要であり、また、最適解が与えられる保証がない。ＡＸ＝ＸＢ問題を回避するため、特許文献１では、ロボットの動きに制限を加えることにより、座標系の変換行列を求める問題を線形化する技術が記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１２−９１２８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１に記載された技術では、処理結果として得られる変換行列が、画像を用いた校正用パターンの位置推定の精度に依存するという課題がある。すなわち、校正用パターンの位置推定の精度を上げるためには、ロボットの動きが大きい方が有利であるが、ロボットの大きな動きは、動きの精度が低くなるという問題がある。一方、ロボットの動きの精度を上げようとすれば、動きを小さくするのが好ましいが、この場合は画像を用いた校正パターンの位置推定の精度が低くなるという問題がある。そこで、特許文献１とは異なる方法で、アームに設置されたカメラの校正を容易に行うことのできる技術が望まれている。
【課題を解決するための手段】
【０００６】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態（aspect）として実現することが可能である。
【０００７】
（１）本発明の第１の形態によれば、カメラが設置されたアームを備えるロボットを制御する制御装置が提供される。この制御装置は、前記アームを制御するアーム制御部と；前記カメラを制御するカメラ制御部と；前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定し、前記座標変換行列を含む前記カメラのパラメーターを作成するカメラ校正実行部と；を備える。前記カメラ制御部は、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させる。前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出可能であり、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記座標変換行列を推定する。
この制御装置によれば、パターン画像の撮像時におけるアームの位置姿勢からアーム座標系と手先座標系の関係を決定できる。また、カメラ校正実行部は、アーム座標系とパターン座標系の関係を算出可能なので、これらの関係に加えて、カメラで撮像したパターン画像から得られるパターン座標系とカメラ座標系との関係を利用すれば、手先座標系とカメラ座標系との間の座標変換行列を推定することが可能である。この結果、その座標変換行列を含むカメラのパラメーターを作成することができ、カメラを用いた対象物の位置検出を行うことが可能となる。
【０００８】
（２）上記制御装置において、前記カメラ校正実行部は、前記パターン画像の撮像時における前記アームの位置姿勢から、前記アーム座標系と前記手先座標系との間の第１変換行列を算出し；前記パターン座標系と前記アーム座標系との間の第２変換行列を算出又は推定し；前記パターン画像から、前記カメラ座標系と前記パターン座標系との間の第３変換行列を推定し；前記第１変換行列と前記第２変換行列と前記第３変換行列から、前記座標変換行列を算出するものとしてもよい。
この制御装置によれば、アームの位置姿勢から第１変換行列を算出できる。また、カメラ校正実行部は、アーム座標系とパターン座標系の座標変換を表す第２変換行列を算出又は推定し、更に、パターン画像から第３変換行列を推定するので、これらの変換行列から手先座標系とカメラ座標系との間の座標変換行列を含むカメラのパラメーターを容易に求めることが可能である。
【０００９】
（３）上記制御装置において、前記ロボットは、前記校正用パターンを予め定めた設置状態で設置可能に構成された第２アームを有し、前記カメラ校正実行部は、前記パターン画像の撮像時における前記第２アームの位置姿勢から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を算出するものとしてもよい。
この制御装置によれば、第２アームの位置姿勢から第２変換行列を算出できるので、手先座標系とカメラ座標系との間の座標変換行列を容易に求めることが可能である。
【００１０】
（４）上記制御装置において、前記カメラ制御部は、前記校正用パターンの第２パターン画像を前記アームとは独立して配置された固定カメラで撮像させ、前記カメラ校正実行部は、前記第２パターン画像から、前記パターン座標系と前記アーム座標系との間の前記第２変換行列を推定するものとしてもよい。
この制御装置によれば、第２パターン画像から第２変換行列を推定できるので、手先座標系とカメラ座標系との間の座標変換行列を容易に求めることが可能である。
【００１１】
（５）上記制御装置において、前記固定カメラはステレオカメラであるものとしてもよい。
この制御装置によれば、ステレオカメラで撮像した第２パターン画像から、第２変換行列を精度良く推定できるので、手先座標系とカメラ座標系との間の座標変換行列を精度良く求めることが可能である。
【００１２】
（６）本発明の第２の形態によれば、カメラが設置されたアームを備えるロボットを制御する制御装置が提供される。この制御装置は、プロセッサーを備える。前記プロセッサーは、前記カメラの校正用パターンのパターン画像を前記カメラに撮像させ、前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出し、前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する。
この制御装置によれば、パターン画像の撮像時におけるアームの位置姿勢からアーム座標系と手先座標系の関係を決定できる。また、アーム座標系とパターン座標系の関係を算出可能なので、これらの関係に加えて、カメラで撮像したパターン画像から得られるパターン座標系とカメラ座標系との関係を利用すれば、手先座標系とカメラ座標系との間の座標変換行列を推定することが可能である。この結果、その座標変換行列を含むカメラのパラメーターを作成することができ、カメラを用いた対象物の位置検出を行うことが可能となる。
【００１３】
（７）本発明の第３の形態は、上記制御装置に接続されたロボットである。
このロボットによれば、手先座標系とカメラ座標系との間の座標変換行列を容易に推定することが可能である。
【００１４】
（８）本発明の第４の形態は、ロボットと、前記ロボットに接続された上記制御装置と、を備えるロボットシステムである。
このロボットシステムによれば、手先座標系とカメラ座標系との間の座標変換行列を容易に推定することが可能である。
【００１５】
（９）本発明の第５の形態によれば、カメラが設置されたアームを備えるロボットにおいて前記カメラの校正を行う方法が提供される。この方法は、前記カメラの校正用パターンのパターン画像を前記カメラで撮像し；前記パターン画像の撮像時における前記アームのアーム座標系と前記校正用パターンのパターン座標系の関係を算出し；前記パターン画像の撮像時における前記アームの位置姿勢と、前記パターン画像とを用いて、前記アームの手先座標系と前記カメラのカメラ座標系との間の座標変換行列を推定する。
この方法によれば、パターン画像の撮像時におけるアームの位置姿勢からアーム座標系と手先座標系の関係を決定できる。また、アーム座標系とパターン座標系の関係を算出可能なので、これらの関係に加えて、カメラで撮像したパターン画像から得られるパターン座標系とカメラ座標系との関係を利用すれば、手先座標系とカメラ座標系との間の座標変換行列を推定することが可能である。この結果、その座標変換行列を含むカメラのパラメーターを作成することができ、カメラを用いた対象物の位置検出を行うことが可能となる。
【００１６】
本発明は、上記以外の種々の形態で実現することも可能である。例えば、制御装置の機能を実現するためのコンピュータプログラム、そのコンピュータプログラムを記録した一時的でない記録媒体（non-transitory storage medium）等の形態で実現することができる。
【図面の簡単な説明】
【００１７】
【図１】ロボットシステムの概念図。
【図２】ロボットと制御装置の機能を示すブロック図。
【図３】第１実施形態のロボットの座標系を示す説明図。
【図４】第１実施形態の処理手順を示すフローチャート。
【図５】第２実施形態のロボットの座標系を示す説明図。
【図６】第２実施形態の処理手順を示すフローチャート。
【図７】第３実施形態のロボットの座標系を示す説明図。
【発明を実施するための形態】
【００１８】
A. ロボットシステムの構成
図１は、一実施形態におけるロボットシステムの概念図である。このロボットシステムは、ロボット１００と、制御装置２００とを備えている。このロボット１００は、カメラで作業対象を認識し、自在に力を加減して、自律的に判断しながら作業を行える自律型ロボットである。また、ロボット１００は、予め作成された教示データに従って作業を実行するティーチングプレイバックロボットとして動作することも可能である。
【００１９】
ロボット１００は、基台１１０と、胴部１２０と、肩部１３０と、首部１４０と、頭部１５０と、２つのアーム１６０Ｌ，１６０Ｒとを備えている。アーム１６０Ｌ，１６０Ｒには、ハンド１８０Ｌ，１８０Ｒが着脱可能に取り付けられている。これらのハンド１８０Ｌ，１８０Ｒは、ワークや道具を把持するエンドエフェクターである。頭部１５０には、カメラ１７０Ｌ，１７０Ｒが設置されている。これらのカメラ１７０Ｌ，１７０Ｒは、アーム１６０Ｌ，１６０Ｒとは独立して設けられており、その位置姿勢が変化しない固定カメラである。アーム１６０Ｌ，１６０Ｒの手首部には、カメラとしてのハンドアイ１７５Ｌ，１７５Ｒが設けられている。アーム１６０Ｌ，１６０Ｒには、カメラ１７０Ｌ，１７０Ｒやハンドアイ１７５Ｌ，１７５Ｒの校正用パターン４００を設置可能である。以下では、ハンドアイ１７５Ｌ，１７５Ｒと区別するために、頭部１５０に設けられたカメラ１７０Ｌ，１７０Ｒを「固定カメラ１７０Ｌ，１７０Ｒ」とも呼ぶ。
【００２０】
アーム１６０Ｌ，１６０Ｒの手首部には、更に、力覚センサー１９０Ｌ，１９０Ｒが設けられている。力覚センサー１９０Ｌ，１９０Ｒは、ハンド１８０Ｌ，１８０Ｒがワークに及ぼしている力に対する反力やモーメントを検出するセンサーである。力覚センサー１９０Ｌ，１９０Ｒとしては、例えば、並進３軸方向の力成分と、回転３軸回りのモーメント成分の６成分を同時に検出することができる６軸力覚センサーを用いることができる。なお、力覚センサー１９０Ｌ，１９０Ｒは省略可能である。
【００２１】
アーム１６０Ｌ，１６０Ｒと、カメラ１７０Ｌ，１７０Ｒと、ハンドアイ１７５Ｌ、１７５Ｒと、ハンド１８０Ｌ，１８０Ｒと、力覚センサー１９０Ｌ，１９０Ｒの符号の末尾に付された「Ｌ」「Ｒ」の文字は、それぞれ「左」「右」を意味する。これらの区別が不要の場合には、「Ｌ」「Ｒ」の文字を省略した符号を使用して説明する。
【００２２】
制御装置２００は、プロセッサー２１０と、メインメモリー２２０と、不揮発性メモリー２３０と、表示制御部２４０と、表示部２５０と、Ｉ／Ｏインターフェース２６０とを有している。これらの各部は、バスを介して接続されている。プロセッサー２１０は、例えばマイクロプロセッサー又はプロセッサー回路である。制御装置２００は、Ｉ／Ｏインターフェース２６０を介してロボット１００に接続される。なお、制御装置２００をロボット１００の内部に収納してもよい。
【００２３】
制御装置２００の構成としては、図１に示した構成以外の種々の構成を採用することが可能である。例えば、プロセッサー２１０とメインメモリー２２０を図１の制御装置２００から削除し、この制御装置２００と通信可能に接続された他の装置にプロセッサー２１０とメインメモリー２２０を設けるようにしてもよい。この場合には、当該他の装置と制御装置２００とを合わせた装置全体が、ロボット１００の制御装置として機能する。他の実施形態では、制御装置２００は、２つ以上のプロセッサー２１０を有していてもよい。更に他の実施形態では、制御装置２００は、互いに通信可能に接続された複数の装置によって実現されていてもよい。これらの各種の実施形態において、制御装置２００は、１つ以上のプロセッサー２１０を備える装置又は装置群として構成される。
【００２４】
図２は、ロボット１００と制御装置２００の機能を示すブロック図である。制御装置２００のプロセッサー２１０は、不揮発性メモリー２３０に予め格納された各種のプログラム命令２３１を実行することにより、アーム制御部２１１と、カメラ制御部２１２と、カメラ校正実行部２１３との機能をそれぞれ実現する。カメラ校正実行部２１３は、変換行列推定部２１４を有している。但し、これらの各部２１１〜２１４の機能の一部又は全部をハ―ドウェア回路で実現しても良い。これらの各部２１１〜２１４の機能については後述する。不揮発性メモリー２３０には、プログラム命令２３１の他に、カメラ内部パラメーター２３２とカメラ外部パラメーター２３３とが格納される。これらのパラメーター２３２，２３３は、固定カメラ１７０のパラメーターと、ハンドアイ１７５のパラメーターをそれぞれ含んでいる。なお、本実施形態では、固定カメラ１７０のパラメーター２３２，２３３は既知であり、ハンドアイ１７５のパラメーター２３２，２３３は未知であるものと仮定する。後述する校正処理では、ハンドアイ１７５のパラメーター２３２，２３３が生成される。これらのパラメーター２３２，２３３については更に後述する。
【００２５】
B. ロボットの座標系と座標変換
図３は、ロボット１００のアーム１６０の構成と各種の座標系を示す説明図である。２つのアーム１６０Ｌ，１６０Ｒのそれぞれには、７つの関節Ｊ１〜Ｊ７が設けられている。関節Ｊ１，Ｊ３，Ｊ５，Ｊ７はねじり関節であり、関節Ｊ２，Ｊ４，Ｊ６は曲げ関節である。なお、図１の胴部１２０と肩部１３０の間にはねじり関節が設けられているが、図３では図示が省略されている。個々の関節には、それらを動作させるためのアクチュエーターと、回転角度を検出するための位置検出器とが設けられている。
【００２６】
アーム１６０の手先には、ツール中心点ＴＣＰ（Tool Center Point）が設定されている。典型的には、ロボット１００の制御は、ツール中心点ＴＣＰの位置姿勢を制御するために実行される。なお、位置姿勢（position and attitude）とは、３次元の座標系における３つ座標値と、各座標軸回りの回転とで規定される状態を意味する。
【００２７】
アーム１６０Ｌ，１６０Ｒには、校正用パターン４００を予め定めた設置状態で設置可能である。図３の例では、左アーム１６０Ｌのハンドアイ１７５Ｌの校正（キャリブレーション）に使用される校正用パターン４００が、右アーム１６０Ｒの手先部に固定されている。校正用パターン４００を右アーム１６０Ｒに取り付ける際には、右アーム１６０Ｒのハンド１８０Ｒを取り外すようにしてもよい。左アーム１６０Ｌのハンド１８０Ｌも同様である。
【００２８】
ハンドアイ１７５Ｌの校正は、ハンドアイ１７５Ｌの内部パラメーターと外部パラメーターとを推定する処理である。内部パラメーターは、ハンドアイ１７５Ｌ及びそのレンズ系の固有のパラメーターであり、例えば射影変換パラメーターや歪パラメーターなどを含んでいる。外部パラメーターは、ハンドアイ１７５Ｌとロボット１００のアーム１６０Ｌとの間の相対的な位置姿勢を計算する際に使用されるパラメーターであり、アーム１６０Ｌの手先座標系ΣT1とハンドアイ座標系ΣEとの間の並進や回転を表現するパラメーターを含んでいる。但し、外部パラメーターは、手先座標系ΣT1以外のターゲット座標系とハンドアイ座標系ΣEとの間の並進や回転を表現するパラメーターとしても構成可能である。ターゲット座標系は、ロボット座標系Σ0から求め得る座標系であれば良い。例えば、ロボット座標系Σ0に対して固定された既知の相対位置姿勢を有する座標系や、アーム１６０Ｌの関節の動作量に応じてロボット座標系Σ0との相対位置姿勢が決まる座標系をターゲット座標系として選択してもよい。外部パラメーターは、「アームの手先座標系とカメラのカメラ座標系との間の座標変換行列を含むカメラのパラメーター」に相当する。
【００２９】
図３には、ロボット１００に関する座標系として、以下の座標系が描かれている。
（１）ロボット座標系Σ0：ロボット１００の基準点を座標原点とする座標系
（２）アーム座標系ΣA1，ΣA2：アーム１６０Ｌ，１６０Ｒの基準点Ａ１，Ａ２を座標原点とする座標系
（３）手先座標系ΣT1，ΣT2：アーム１６０Ｌ，１６０ＲのＴＣＰ（ツール中心点）を座標原点とする座標系
（４）パターン座標系ΣP：校正用パターン４００上の所定の位置を座標原点とする座標系
（５）ハンドアイ座標系ΣE：ハンドアイ１７５に設定された座標系
【００３０】
アーム座標系ΣA1，ΣA2と手先座標系ΣT1，ΣT2は、左アーム１６０Ｌと右アーム１６０Ｒにそれぞれ別個に設定される。以下では、左アーム１６０Ｌに関する座標系を、「第１アーム座標系ΣA1」「第１手先座標系ΣT1」とも呼び、右アーム１６０Ｒに関する座標系を「第２アーム座標系ΣA2」「第２手先座標系ΣT2」と呼ぶ。なお、アーム座標系ΣA1，ΣA2とロボット座標系Σ0の相対位置姿勢は既知である。ハンドアイ座標系ΣEも、ハンドアイ１７５Ｌ，１７５Ｒにそれぞれ別個に設定される。以下の説明では、左アーム１６０Ｌのハンドアイ１７５Ｌを校正対象とするので、ハンドアイ座標系ΣEとして左アーム１６０Ｌのハンドアイ１７５Ｌの座標系を使用する。なお、図３では、図示の便宜上、個々の座標系の原点が実際の位置からずれた位置に描かれている。
【００３１】
一般に、或る座標系ΣAから他の座標系ΣBへの変換、又は、これらの座標系での位置姿勢の変換は、以下に示す同次変換行列ＡＨＢ（Homogeneous transformation matrix）で表現される。
【数１】
2019014030.tif 000003
ここで、Ｒは回転行列（Rotation matrix）、Ｔは並進ベクトル（Translation vector）、Ｒｘ，Ｒｙ，Ｒｚは回転行列Ｒの列成分である。以下では、同次変換行列ＡＨＢを「座標変換行列ＡＨＢ」、「変換行列ＡＨＢ」又は、単に「変換ＡＨＢ」とも呼ぶ。変換の符号「ＡＨＢ」の左側の上付き文字「Ａ」は変換前の座標系を示し、右側の下付き文字「Ｂ」は変換後の座標系を示す。なお、変換ＡＨＢは、座標系ΣAで見た座標系ΣBの基底ベクトルの成分と原点位置を表すものと考えることも可能である。
【００３２】
変換ＡＨＢの逆行列ＡＨＢ−１（＝ＢＨＡ）は、次式で与えられる。
【数２】
2019014030.tif 000004
【００３３】
回転行列Ｒは、以下の重要な性質を有する。
＜回転行列Ｒの性質１＞
回転行列Ｒは正規直交行列であり、その逆行列Ｒ−１は転置行列ＲTに等しい。
＜回転行列Ｒの性質２＞
回転行列Ｒの３つの列成分Ｒｘ，Ｒｙ，Ｒｚは、元の座標系ΣAで見た回転後の座標系ΣBの３つの基底ベクトルの成分に等しい。
【００３４】
或る座標系ΣAに変換AＨB，BＨCを順次施す場合に、合成された変換AＨCは各変換AＨB，BＨCを順次右側に乗じたものとなる。
【数３】
2019014030.tif 000005
【００３５】
また、回転行列Ｒに関しても（３）式と同様の関係が成立する。
【数４】
2019014030.tif 000006
【００３６】
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に戻るので、恒等変換Ｉを用いて以下の式が成立する。
【数５】
2019014030.tif 000007
【００４２】
（５）式の両辺に、左から各変換の逆行列A1ＨT1−１, T1ＨE−１, EＨP−１を順に乗算すれば次式が得られる。
【数６】
2019014030.tif 000008
【００４３】
（６）式において、変換EＨPはカメラキャリブレーション関数を用いて推定でき、変換A1ＨT1は計算可能である。従って、変換T1ＨEが既知であれば、右辺は計算可能であり、左辺の変換PＨA1を知ることができる。
【００４４】
一方、変換T1ＨEが未知であれば、（６）式の右辺は計算できず、別の処理が必要となる。例えば、図３において左アーム１６０Ｌの２つの姿勢ｉ，ｊを考えれば、それぞれにおいて上記（５）式が成立し、次式となる。
【数７】
2019014030.tif 000009
【００４５】
（７ａ），（７ｂ）式の両辺にそれぞれ右から変換PＨA1の逆行列PＨA1−１ を乗算すれば次式が得られる。
【数８】
2019014030.tif 000010
【００４６】
（８ａ），（８ｂ）式の右辺は未知ではあるが同じ変換なので、次式が成立する。
【数９】
2019014030.tif 000011
【００４７】
（９）式の両辺に、左からA1ＨT1(j)−１を、右からEＨP(i)−１を乗算すると次式となる。
【数１０】
2019014030.tif 000012
【００４８】
ここで、（１０）式の左辺と右辺の括弧の中の変換の積をそれぞれＡ，Ｂと書き、未知の変換T1ＨEをＸと書くと、次式が得られる。
【数１１】
2019014030.tif 000013
【００４９】
これは、ＡＸ＝ＸＢ問題として良く知られた処理であり、未知の行列Ｘを解くためには非線形の最適化処理が必要となる。しかし、この非線形最適化処理には、最適な解に収束する保証が無いと言う問題がある。
【００５０】
以下に詳述するように、第１実施形態では、校正用パターン４００が設置された第２アーム１６０Ｒを任意に制御可能であることを利用し、第２アーム１６０Ｒの位置姿勢から第２アーム座標系Σ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は、次式で算出できる。
【数１２】
2019014030.tif 000014
【００５７】
（１２）式の右辺の３つの変換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については、図３において次式が成立する。
【数１３】
2019014030.tif 000015
【００６１】
（１３）式の右辺の５つの変換のうちは、第１の変換T1ＨA1は、ステップＳ１４０で算出されている。第２の変換A1ＨA2は、既知である。第３の変換A2ＨT2は、アーム１６０Ｒのフォワードキネマティクスによって算出可能である。第４の変換T2ＨPは、既知である。第５の変換PＨEは、ステップＳ１６０で推定されている。従って、第１手先座標系ΣT1とハンドアイ座標系ΣEの変換T1ＨEを、（１３）式に従って算出することができる。
【００６２】
こうして得られた同次変換行列T1ＨE又はEＨT1は、ハンドアイ１７５の外部パラメーター２３３として不揮発性メモリー２３０に格納される。ハンドアイ１７５の外部パラメーター２３３と内部パラメーター２３２を利用すれば、ハンドアイ１７５を用いた各種の検出処理や制御を実行することが可能となる。なお、ハンドアイ１７５の外部パラメーター２３３としては、ロボット座標系Σ0とハンドアイ座標系Σ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は、次式で算出できる。
【数１４】
2019014030.tif 000016
（１４）式の右辺の２つの変換のうち、第１の変換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を含むハンドアイ１７５の外部パラメーターを求めることが可能である。
【００７４】
本発明は、上述の実施形態や実施例、変形例に限られるものではなく、その趣旨を逸脱しない範囲において種々の構成で実現することができる。例えば、発明の概要の欄に記載した各形態中の技術的特徴に対応する実施形態、実施例、変形例中の技術的特徴は、上述の課題の一部又は全部を解決するために、あるいは、上述の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。
【符号の説明】
【００７５】
１００，１００ａ…ロボット、１１０…基台、１２０…胴部、１３０…肩部、１４０…首部、１５０…頭部、１６０，１６０Ｌ，１６０Ｒ…アーム、１７０，１７０Ｌ，１７０Ｒ…固定カメラ、１７５，１７５Ｌ，１７５Ｒ…ハンドアイ、１８０Ｌ，１８０Ｒ…ハンド、１９０Ｌ，１９０Ｒ…力覚センサー、２００…制御装置、２１０…プロセッサー、２１１…アーム制御部、２１２…カメラ制御部、２１３…カメラ校正実行部、２１４…変換行列推定部、２２０…メインメモリー、２３０…不揮発性メモリー、２３１…プログラム命令、２３２…カメラ内部パラメーター、２３３…カメラ外部パラメーター、２４０…表示制御部、２５０…表示部、２６０…Ｉ／Ｏインターフェース、４００…校正用パターン
【図１】
2019014030.tif 000017
【図２】
2019014030.tif 000018
【図３】
2019014030.tif 000019
【図４】
2019014030.tif 000020
【図５】
2019014030.tif 000021
【図６】
2019014030.tif 000022
【図７】
2019014030.tif 000023