JP7064586B2 - カメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボット - Google Patents

Description

本発明は、例えばロボットに搭載されて使用されるカメラにおけるカメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボットに関する。

カメラを搭載したロボットを用いたシステムにおいては、カメラとロボットの相対位置姿勢の校正が必要である。

係るカメラ位置姿勢校正手法について、特許文献１には「本発明に係るロボットアームの視覚センサ補正方法は、ロボットアームの先端に取り付けられた視覚センサによってワークの位置を認識することにより前記ロボットアームの先端での前記視覚センサの取り付け状態値を補正するロボットアームの視覚センサ補正方法において、前記ロボットアームの移動による前記ロボットアームの先端の移動量を基準座標で特定する移動量特定処理ステップと、前記移動の前後それぞれで前記視覚センサにより取得したワークの画像に対して画像処理を施すことにより移動前後での処理画像を求める画像処理ステップと、前記移動前後の処理画像に対して前記視覚センサの取り付け状態値の推定値としての仮パラメータを適用して前記移動前後での処理画像を基準座標に座標変換することにより前記移動前後に対するワークの位置変化量を前記仮パラメータに対応させて求める位置変化量抽出処理ステップと、前記移動量と前記位置変化量との差を状態差として算出する状態差算出処理ステップと、前記仮パラメータを変更して位置変化量抽出処理ステップを複数回繰り返すことにより複数の前記状態差を求め、複数の前記状態差の中で最小の状態差に対応する前記仮パラメータを視覚センサの取り付け状態値として抽出する状態値抽出処理ステップとを備えることを特徴とする。」という記載がある。

特開２００７－６１９７９号公報

特許文献１に記載されている発明では、ロボットに搭載されたカメラにより撮影されたロボットの周辺環境の画像を用いることで、マーカの設置などの人手の作業を必要とせずにカメラとロボットの相対位置姿勢を校正することができる。

しかし、校正に用いる画像はロボットに校正専用の動作を実行させることで取得しており、校正専用の動作を実施している間はロボットに設定されているタスクが中断される。また、校正専用の動作の実行に時間が必要である。この従来手法によれば、タスク動作の実行に先立って、校正動作を行う２段階動作を時系列的に順次実行することになる。

以上のことから本発明においては、ロボットに設定されているタスクを中断することなく、カメラとロボットの相対位置姿勢を校正することができるカメラ位置姿勢校正装置、カメラ位置姿勢校正方法並びにロボットを提供することを目的とする。

以上のことから本発明においては、「ロボットに搭載されたカメラにより撮影された画像とロボットの位置姿勢から、ロボットに設定されたタスクを実行するための動作を計画するタスク動作計画部と、画像とロボットの位置姿勢から、カメラとロボットの相対位置姿勢の校正に必要な動作を計画する校正動作計画部と、タスク動作計画部によって計画されたタスク動作計画と校正動作計画部によって計画された校正動作計画を統合することで動作を計画する統合動作計画部とを備え、ロボットを操作することを特徴とするカメラ位置姿勢校正装置」としたものである。

また本発明は、「カメラ位置姿勢校正装置により操作されるロボット」としたものである。

また本発明は、「ロボットに搭載されたカメラにより撮影された画像とロボットの位置姿勢から、ロボットに設定されたタスクを実行するための動作を計画したタスク動作計画と、画像とロボットの位置姿勢から、カメラとロボットの相対位置姿勢の校正に必要な動作を計画した校正動作計画と、タスク動作計画と校正動作計画を統合した統合動作計画と、タスクの状況および目標を得、タスクの状況および目標に基づいて、タスク動作計画に対する校正動作計画の優先度を算出し、優先度に基づいて、動作計画を選択し、選択した動作計画によりロボットを操作することを特徴とするカメラ位置姿勢校正方法」としたものである。

本発明によれば、ロボットに設定されているタスクを中断することなく、カメラとロボットの相対位置姿勢を校正することができるカメラ位置姿勢校正装置を実現することが可能である。

カメラ位置姿勢校正装置１００のブロック構成例を示す図。 ロボットとロボット制御装置で構成されるロボットシステムの構成例を示す図。 本発明のカメラ位置姿勢校正装置を適用可能な典型的なロボットの一例としてピッキングロボットを示す図。 取得済み動作データベースＤＢ１に記憶された取得済み動作データＤ１の一例を示す図。 校正動作データベースＤＢ２に記憶された校正動作データデータＤ２の一例を示す図。 校正動作計画表ＴＢ１の一例を示す図。 校正動作計画部１０２において実行される処理を示すフローチャート。 校正動作計画表ＴＢ１の初期状態の一例を示す図。 タスク状況目標表ＴＢ２の一例を示す図。 校正優先度算出部１０４において実行される処理を示すフローチャート。 統合動作計画部１０３において実行される処理を示すフローチャート。 処理ステップＳ７１０において実行される処理を示すフローチャート。 自動運転車２３０を駐車スペースに駐車する状況を示す図。 自動運転車２３０におけるタスク状況目標表ＴＢ２の一例を示す図。 校正優先度算出部１０４において実行される処理を示すフローチャート。

以下、本発明の実施例について図面を参照して説明する。

以下、図１～図１２を参照して、本発明の実施例１に係るカメラ位置姿勢校正装置を説明する。

最初に、本発明のカメラ位置姿勢校正装置を適用可能な典型的なロボットの一例について図３を用いて説明する。図３では、ロボット２００としてピッキングロボットを例に説明する。

ピッキングロボット２００は、作業台２２０にバラ積みされたワーク２２１を１つずつ取り出し、所定の場所に並べるロボットである。ピッキングロボット２００は、複数の関節２１２やリンク２１４、並びにエンドエフェクタ２１５から構成されており、さらにカメラ装置としてカメラ２０１をカメラ取り付け治具２１３によりロボット２００の適宜位置に搭載している。またロボット２００の各関節２１２には、各関節２１２の角度を計測するセンサである位置姿勢センサ２０２が適宜備えられている。

ピッキングロボット２００は、カメラ２０１により作業台２２０上のワーク２２１を認識し、関節２１２の角度を制御することで、ワーク２２１を持ち上げることが可能な場所にエンドエフェクタ２１５を移動させる。エンドエフェクタ２１５はたとえば吸着パッドやロボットハンドであり、ワーク２２１を吸着や把持することで持ち上げる。なお、ピッキングロボット２００の関節２１２やリンク２１４の数といった構成は任意である。また、カメラ２０１として画像に加えて距離も取得可能なＲＧＢ－Ｄカメラを用いても良い。

ピッキングロボット２００は、エンドエフェクタ２１５をワーク２２１に近接させるに際し、その位置姿勢を確認する必要があり、各関節２１２の角度を計測するセンサである位置姿勢センサ２０２を用いて、たとえばワーク２２１を持ち上げる際に重要となるエンドエフェクタ２１５の位置姿勢を確認する。なお各関節２１２の角度を計測することで、ピッキングロボット２００の幾何モデルに基づき、公知の手法により順運動学を解くことでエンドエフェクタ２１５の位置姿勢を得ることができる。

ロボット２００において、カメラ２０１による認識結果を用いて、タスク（図３のピッキングロボット２００の例では、ワーク２２１を取り出して所定の場所に並べること）を実行することが可能な場所にロボット２００を移動させるためには、カメラ２０１とロボット２００の相対位置姿勢が必要となる。これは例えば図３においてカメラ２０１の設置点Ｎと、ロボット２００側として例えばエンドエフェクタ２１５の先端位置Ｍの間の相対位置姿勢である。

しかし、カメラ２０１とロボット２００の相対位置姿勢は、ロボット２００の動作時の振動や、動作時にカメラ２０１およびロボット２００が周辺環境に存在する物体などに衝突することにより変化し、初期に設定された値に対して誤差が生じることがある。カメラ２０１とロボット２００の相対位置姿勢の誤差は、ロボット２００の位置姿勢の誤差となるため、誤差が大きい場合には、タスクの失敗につながる。

このため、ロボット２００の制御装置は、カメラ２０１とロボット２００の相対位置姿勢を校正する機能であるカメラ位置姿勢校正装置を備えている。カメラ位置姿勢校正装置は、カメラ２０１とロボット２００の相対位置姿勢の誤差が、タスクに失敗するほど大きくなる前に、タスクの実行に必要な動作計画とカメラ位置姿勢の校正に必要な動作計画を統合した動作を計画することで、タスクを中断することなくカメラの位置姿勢を校正可能なロボット２００の動作を計画する。

図２は、ロボット２００とロボット制御装置１０で構成されるロボットシステムの構成例を示す図である。

図２のロボットシステムの内、ロボット２００は少なくともカメラ２０１と位置姿勢センサ２０２を備えており、夫々画像情報とロボット位置姿勢情報をロボット制御装置１０に与える。

図２のロボットシステムの内、ロボット制御装置１０はカメラ位置姿勢校正装置１００、タスク情報・目標取得部３１０、カメラ位置姿勢推定部３００を含んで構成されている。

図２のロボットシステムについて、以下に詳細に説明する。まず、ロボット２００はカメラ２０１と位置姿勢センサ２０２を搭載している。カメラ２０１はロボット２００の周辺環境の画像を撮影する。位置姿勢センサ２０２はロボット２００の位置姿勢を出力する。

なお、位置姿勢センサ２０２は、直接ロボット２００の位置姿勢を計測するセンサでも良いし、ロボット２００の状態を計測するセンサでも良い。ロボット２００の状態を計測するセンサを用いる場合には、ロボット２００は、ロボット２００の状態からロボット２００の位置姿勢を推定し、出力する。

次に図２のロボット制御装置１０内のカメラ位置姿勢推定部３００は、ロボット２００の動作中に、ロボット２００に搭載されたカメラ２０１によって撮影された画像と、ロボット２００に搭載された位置姿勢センサ２０２によって取得されたロボット２００の位置姿勢と、からカメラ２０１とロボット２００の相対位置姿勢を推定する。

画像とロボット２００の位置姿勢から、カメラ２０１とロボット２００の相対位置姿勢を推定する問題は、Ｈａｎｄ－Ｅｙｅキャリブレーションと呼ばれる。本発明の実施例１に係るカメラ位置姿勢推定部３００は、公知のＨａｎｄ－Ｅｙｅキャリブレーション手法を用いるのがよい。ただし、本発明においては、Ｊ．Ｈｅｌｌｅｒ、Ｍ．Ｈａｖｌｅｎａ,Ａ．Ｓｕｇｉｍｏｔｏ,Ｔ．Ｐａｊｄｌａ,Ｓｔｒｕｃｔｕｒｅ－ｆｒｏｍ－ｍｏｔｉｏｎ ｂａｓｅｄ ｈａｎｄ－ｅｙｅ ｃａｌｉｂｒａｔｉｏｎ ｕｓｉｎｇ Ｌ∞ ｍｉｎｉｍｉｚａｔｉｏｎ, ＩＥＥＥ Ｃｏｎｆ．ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｐａｔｔｅｒｎ Ｒｅｃｏｇｎｉｔｉｏｎ,ｐｐ．３４９７－３５０３（２０１１）のように、マーカを用いない手法が好適である。ロボット２００はカメラ位置姿勢推定部３００によって推定されたカメラ位置姿勢を用いてタスクを実行する。

タスク状況・目標取得部３１０はロボット２００に設定されたタスクの状況および目標を取得する。ここでタスクとは、詳細を後述するように、図３のピッキングロボット２００の例では、ワーク２２１を取り出して所定の場所に並べることであり、タスクの状況とは、たとえば、ワーク２２１の種類や形状、並べ終えたワーク２２１の数などである。またタスクの目標とは、たとえば、並べるワーク２２１の数や、ワーク２２１を並べ終える時間などである。

カメラ位置姿勢校正装置１００は、ロボット２００に搭載されたカメラ２０１によって撮影された画像と、ロボット２００に搭載された位置姿勢センサ２０２によって取得されたロボットの位置姿勢と、タスク状況・目標取得部３１０によって取得されたタスク状況・目標と、からロボット２００の動作を計画する。ロボット２００はカメラ位置姿勢校正装置１００によって計画された動作を実行する。このためロボットは、関節などを駆動する図示せぬアクチュエータを備えている。

これによりカメラ位置姿勢校正装置１００は、カメラ２０１とロボット２００の相対位置姿勢の誤差が、タスクに失敗するほど大きくなる前に、タスクの実行に必要な動作計画とカメラ位置姿勢の校正に必要な動作計画を統合した動作を計画することで、タスクを中断することなくカメラの位置姿勢を校正可能なロボット２００の動作を計画する。

図１は、カメラ位置姿勢校正装置１００のブロック構成例を示す図である。カメラ位置姿勢校正装置１００は、タスク動作計画部１０１と、校正動作計画部１０２と、統合動作計画部１０３と、校正優先度算出部１０４と、を備える。

タスク動作計画部１０１は、画像とロボットの位置姿勢のデータから、ロボットに設定されたタスクを実行するための動作であるタスク動作計画Ｌ１を作成する。校正動作計画部１０２は、画像とロボットの位置姿勢から、カメラ位置姿勢の校正に必要な動作である校正動作計画Ｌ２を作成する。統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画Ｌ１と校正動作計画部１０２によって計画された校正動作計画Ｌ２を、タスクの状況および目標に基づいて統合することで動作計画Ｌを作成する。統合動作計画部１０３は校正優先度算出部１０４を備える。校正優先度算出部１０４は、タスク動作計画Ｌ１に対する校正動作計画Ｌ２の優先度を算出する。

図１に示すカメラ位置姿勢校正装置１００の各部機能について、さらに詳細に説明する。

タスク動作計画部１０１は、画像とロボット２００の位置姿勢のデータから、ロボット２００に設定されたタスクを実行するためのタスク動作計画Ｌ１を作成する。ピッキングロボット２００において、タスク動作計画部１０１は、カメラ２０１によって撮影されたワーク２２１の画像と、ピッキングロボット２００の位置姿勢のデータから、ワーク２２１を持ち上げて所定の場所に並べるための動作であるタスク動作計画Ｌ１を作成する。たとえば、タスク動作計画部１０１は、公知のパターンマッチング手法や機械学習手法により、画像からワーク２２１の位置姿勢を推定することで、ワーク２２１を持ち上げることが可能なピッキングロボット２００の位置姿勢を算出する。複数のワーク２２１が認識された場合には、それぞれのワーク２２１に対応した、ワーク２２１を持ち上げることが可能なピッキングロボット２００の位置姿勢を算出する。

なおタスク動作計画部１０１が計画した位置姿勢のタスク動作計画の時の動作軌跡の一例を、図３においてはＬ１として示している。なお図３には複数のワーク２２１のうちワーク２２１Ａに対するタスク動作計画をＬ１として示しており、他のワークに対するタスク動作計画も別途求められる。

校正動作計画部１０２は、画像とロボット２００の位置姿勢から、カメラ位置姿勢の校正に必要な動作（校正動作計画Ｌ２）を計画する。なおカメラ位置姿勢の校正に必要な動作について図３で例示すると、カメラ位置姿勢の校正にはＸ点における画像とロボットの位置姿勢の情報が必要であり、現在位置からＸ点に向かわせて、新たにＸ点における画像とロボットの位置姿勢の情報を取得するための計画が校正動作計画Ｌ２である。校正動作計画処理の詳細は、図４から図７を用いて後述する。

統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画Ｌ１と校正動作計画部１０２によって計画された校正動作計画Ｌ２を、タスクの状況および目標に基づいて統合することで動作（動作計画Ｌ）を計画する。統合動作計画処理の詳細は図１０、図１１を用いて後述する。

なお、ロボット２００とカメラ位置姿勢校正装置１００の接続は、ＵＳＢやイーサネット(登録商標)といった有線接続でも良いし、無線ネットワークを介した無線接続でも良い。また、カメラ位置姿勢校正装置１００はロボット２００内に設けられても良いし、ロボット２００と接続されたＰＣやサーバ内に設けられても良い。

次に図４～図７を用いて、校正動作計画部１０２における処理の内容について具体的に説明する。

校正動作計画部１０２は、画像とロボット２００の位置姿勢から、カメラ位置姿勢の校正に必要な動作である校正動作計画Ｌ２を計画する。校正動作計画部１０２は、取得済み動作データＤ１を記憶する取得済み動作データベースＤＢ１と、校正動作データＤ２を記憶する校正動作データベースＤＢ２を備えている。また、校正動作計画部１０２は校正に必要な動作（校正動作計画Ｌ２）を校正動作計画表ＴＢ１として出力する。

最初に、取得済み動作データベースＤＢ１を形成することについて説明する。取得済み動作データベースＤＢ１は、ロボット稼働中の適宜の時点において、様々な位置姿勢で画像を取得し、記憶内容を補充される。

図４は取得済み動作データベースＤＢ１に記憶された取得済み動作データＤ１の一例を示す図である。取得済み動作データベースＤＢ１の各行に示す取得済み動作データＤ１は、カメラ２０１が撮影した画像データＤ１１と画像撮影時のロボット２００の位置姿勢データＤ１２がペアとして記憶されたものである。ペアの情報には、個別に識別符号ＩＤが付与されている。なおロボット２００の位置姿勢は、３パラメータの位置と３パラメータの姿勢により、合計６パラメータで表現される。以降では、取得済み動作データベースＤＢ１の各行の識別子をｉとする。

なお図４の例では、ｉ＝１（ＩＤ＝１）の画像取得時における、位置と姿勢による合計６パラメータで表現される位置姿勢ｐ１は（０、０、０、０、０、０）であり、ｉ＝２（ＩＤ＝２）の画像取得時における位置姿勢ｐ２は（－５、５、１０、０、０、０）であり、ｉ＝ｉ（ＩＤ＝ｉ）の画像取得時における位置姿勢ｐｉは（－１０、１０、３０、１０、０、０）である。この取得済み動作データＤ１によれば、各画像を取得したときの位置姿勢データがペアとして順次蓄積、記憶されている。

図４の取得済み動作データＤ１は、カメラ２０１とロボット２００の相対位置姿勢に変更がない場合には、過去と同じ場所で画像を取得したときの位置姿勢のデータは、過去取得の位置姿勢のデータと同じ値を示すはずであるが、カメラ２０１とロボット２００の相対位置姿勢に変化が生じた場合には、この関係が失われることになる。つまり過去と同じ場所から画像取得していても、位置姿勢が相違することになる。

このため、取得済み動作データベースＤＢ１に記憶された取得済み動作データＤ１は、カメラ２０１とロボット２００の相対位置姿勢に変化が生じたと考えられる場合に、全ての画像と位置姿勢を消去し、初期化され、再度ロボット２００の様々な位置姿勢で画像を取得し、形成されることになる。たとえば、タスクに失敗した場合や、カメラ２０１およびロボット２００が周辺環境の物体などに接触した場合には、カメラ２０１とロボット２００の相対位置姿勢に変化が生じたと判定する。

ピッキングロボット２００において、タスクの失敗は、ワーク２２１の持ち上げ失敗にあたり、たとえば、エンドエフェクタ２１５として吸着パッドを用いる場合には、吸着パッド内の圧力によって持ち上げ成否を判定する。また、カメラ２０１およびロボット２００の周辺環境の物体などへの接触は、たとえば、画像を用いた周辺環境の認識結果において、ロボット２００のタスクにより変化すると想定される変化以外が生じた場合に、接触が発生したと判定する。

図５は校正動作データベースＤＢ２に記憶された校正動作データＤ２の一例を示す図である。校正動作データベースＤＢ２の各行に記憶された校正動作データＤ２は、以前に実施した１回の校正において用いたロボット２００の複数の位置姿勢データＤ２１と、校正結果による各作業対象Ｗ（図３のワーク２２１）に対するタスクの成否データＤ２２で構成されている。

各作業対象Ｗに対するタスクの成否において、「○」は成功を、「×」は失敗を、「－」は該当の作業対象Ｗに対するタスクを試みていないことを示す。ピッキングロボット２００において作業対象Ｗはワーク２２１であり、タスクの成否はワーク２２１の持ち上げ成否である。以降では、校正動作データベースＤＢ２の各行の識別子をｊ、各行に含まれる位置姿勢の識別子をｐ_ｊ、作業対象Ｗの識別子をｗ_ｊとする。なお、校正動作データベースＤＢ２の作成・更新方法については後述する。

図５の表記について具体的に説明すると、ＩＤ＝１に示される位置姿勢ｐ１（０、０、０、０、０、０）、位置姿勢ｐ２（１０、０、０、０、０、０）、位置姿勢ｐｊ（１０、１０、１０、１０、１０、４５）の一連の位置姿勢情報群Ｄ２１は、１組のパターンを形成しており、この位置姿勢のパターンを使用して図２のカメラ位置姿勢推定部３００が位置姿勢を推定し、カメラの校正を行い、タスクを実行した場合に作業対象Ｗa、Ｗb、Ｗｊに対するタスクが全て成功したことをここでは表している。

同様にＩＤ＝２のパターンでは、ここに記述された一連の位置姿勢情報群Ｄ２１のデータを使用して図２のカメラ位置姿勢推定部３００が位置姿勢を推定し、カメラの校正を行い、タスクを実行したところ、作業対象Ｗaに対するタスクに成功したが、作業対象Ｗbに対するタスクは失敗した、作業対象Ｗjに対するタスクはまだ実行していない、ことを表している。

ＩＤ＝jのパターンでは、ここに記述された一連の位置姿勢情報群Ｄ２１のデータを使用して図２のカメラ位置姿勢推定部３００が位置姿勢を推定し、カメラの校正を行い、タスクを実行したところ、作業対象Ｗjに対するタスクに成功したが、作業対象Ｗbに対するタスクは失敗した、作業対象Ｗaに対するタスクはまだ実行していない、ことを表している。

なお図５に表記された位置姿勢のデータｐには、図４の取得済み動作データベースＤＢ１に格納された取得済み動作データＤ１と、画像とロボット２００の位置姿勢のペア情報としては未だ取得されず図４の取得済み動作データＤ１に格納されていないデータとがある。この場合に、取得すべき画像と位置姿勢データのペア情報を決定、計画したものが校正動作計画表ＴＢ１に記述されており、画像と位置姿勢データのペア情報を取得するための動作（図３のＸ点への移動）を示しているのが、校正動作計画Ｌ２である。

次に、校正動作データベースＤＢ２を形成することについて説明する。校正動作データベースＤＢ２は、推定したカメラ２０１とロボット２００の相対位置姿勢およびタスク動作計画部１０１による動作計画を用いてタスクを実施し、タスクに成功したか否かを判定し、いずれかの作業対象に対するタスクに成功した場合には、校正に用いた位置姿勢とタスクの成否を校正動作データベースＤＢ２に加えることで形成される。

なおここで、実際にタスクを実施し、タスクの成否を判定するには、時間が必要である。そこで、校正動作データベースＤＢ２の作成時間を短縮するために、たとえば、最初は、取得済み動作データベースＤＢ１に記憶した十分な数の画像と位置姿勢のペア（取得済み動作データＤ１）を用いて校正を実施し、上述の方法で校正動作データベースＤＢ２に登録するのがよい。具体的には例えば、作業環境の６自由度空間を等間隔のグリッドで分割し、各グリッドに対応する位置姿勢でデータを取得する。次に、さきほど校正に用いた画像と位置姿勢のペア（取得済み動作データＤ１）から、ランダムにいくつかのペアを抽出し、抽出したペアのみを用いて校正を実施する。全てのペアを用いた場合の校正結果と、抽出したペアのみを用いた校正結果を、後述する校正動作計画部１０２の処理ステップＳ５４０と同様の方法で比較し、２つが一致している場合には、校正動作データベースＤＢ２に抽出したペアに含まれる位置姿勢と、全てのペアを用いた場合のワークの持ち上げ結果を加える。これにより、実際のタスクの試行回数を減らしながら、校正動作データベースＤＢ２に含まれる動作の種類を増やすことができる。

校正動作データベースＤＢ２は、カメラ位置姿勢推定部３００による校正が実施されると更新される。具体的には、カメラ位置姿勢推定部３００が校正に用いた取得済み動作データベースＤＢ１に含まれる位置姿勢と、カメラ位置姿勢推定部３００によって推定されたカメラ位置姿勢を用いたタスクの成否を校正動作データベースＤＢ２の１行として加える。

図６は校正動作計画表ＴＢ１の一例を示す図である。校正動作計画表ＴＢ１の各行は校正動作データベースＤＢ２の各行に対応しており、校正動作データベースＤＢ２の各行に含まれる複数の位置姿勢の内、取得済み動作データベースＤＢ１に存在しない位置姿勢と、各作業対象に対するタスクの成否が含まれている。

なお、各作業対象に対するタスクの成否は、校正動作データベースＤＢ２と校正動作計画表ＴＢ１で同一である。なお校正動作計画表ＴＢ１の位置姿勢において「－」は位置姿勢が含まれないことを表す。すなわち、図６の例では、校正動作計画表ＴＢ１の２行目には１つの位置姿勢しか含まれない。

図７は、校正動作計画部１０２において実行される処理を示すフローチャートである。

校正動作計画部１０２における最初の処理ステップＳ５００では、校正動作計画表ＴＢ１を初期化する。具体的には、位置姿勢ｐの情報はひとつも含まず空欄とし、各作業対象Ｗに対するタスクの成否は、校正動作データベースＤＢ２と同じとなるように、校正動作計画表ＴＢ１を作成する。この時に作成された校正動作計画表ＴＢ１は、図８に示す内容のものである。

処理ステップＳ５１０ａでは、校正動作データベースＤＢ２の各行ｊに対するループ処理を開始する。ループ処理は、処理ステップＳ５１０ａと処理ステップＳ５１０ｂの間で条件を変更して繰り返し実行される。

処理ステップＳ５２０ａでは、校正動作データベースＤＢ２のｊ行に含まれる各位置姿勢ｐ_ｊに対するループ処理を開始する。ループ処理は、処理ステップＳ５２０ａと処理ステップＳ５２０ｂの間で条件を変更して繰り返し実行される。

処理ステップＳ５１０ａ、処理ステップＳ５２０ａにおける繰り返し処理により、図５の校正動作データベースＤＢ２の各位置姿勢ｐの情報を順次呼び出すことになる。例えば、位置姿勢ｐ１、ＩＤ＝１の位置姿勢情報（０、０、０、０、０、０）、位置姿勢ｐ２、ＩＤ＝１の位置姿勢情報（１０、０、０、０、０、０）、位置姿勢ｐｊ、ＩＤ＝１の位置姿勢情報（１０、１０、１０、１０、１０、４５）、位置姿勢ｐ１、ＩＤ＝２の位置姿勢情報（０、０、０、１０、０、－５）、位置姿勢ｐ２、ＩＤ＝２の位置姿勢情報（－１０、－５、５、０、１０、１０）、位置姿勢ｐｊ、ＩＤ＝２の位置姿勢情報（０、５、０、２０、－２０、５）の順番での呼び出しを行う。

処理ステップＳ５３０ａでは、取得済み動作データベースＤＢ１の各行ｉに対するループ処理を開始する。ループ処理は、処理ステップＳ５３０ａと処理ステップＳ５３０ｂの間で条件を変更して繰り返し実行される。これにより図４の取得済み動作データベースＤＢ１のペア情報（画像情報Ｄ１１と位置姿勢情報Ｄ１２）が順次呼び出される。

処理ステップＳ５４０では、校正動作データベースＤＢ２のｊ行に含まれる位置姿勢ｐ_ｊと取得済み動作データベースＤＢ１のｉ行に含まれる位置姿勢を比較する。位置姿勢が一致していない場合はループ処理を続け、位置姿勢が一致している場合は、取得済み動作データベースＤＢ１の各行ｉに対するループ処理をぬけ、次の校正動作データベースＤＢ２のｊ行に含まれる各位置姿勢ｐ_ｊに対するループ処理に進む。

上記の例では最初に、図５の位置姿勢ｐ１、ＩＤ＝１の位置姿勢情報（０、０、０、０、０、０）について、図４の位置姿勢ｐ、ＩＤ＝１の位置姿勢情報（０、０、０、０、０、０）と比較し、次に位置姿勢ｐ、ＩＤ＝２の位置姿勢情報（－５、５、１０、０、０、０）と比較するといった処理を全ての組み合わせについて順次実行する。

位置姿勢の一致判定は、たとえば、２つの位置の差が事前に設定した閾値以内、かつ、２つの姿勢の差が事前に設定した閾値以内の場合に、２つの位置姿勢が一致していると判定する。

処理ステップＳ５５０では、校正動作計画表ＴＢ１のｊ行に位置姿勢ｐ_ｊを追加する。このようにして最終的に作成されたものが図６の校正動作計画表ＴＢ１である。この事例では、図５のＩＤ＝１の位置姿勢ｐ１（０、０、０、０、０、０）について図４の取得済みデータベースＤＢ１を検索し、一致するデータが存在したので、校正動作計画表ＴＢ１のＩＤ＝１の行には記述せず、同様に位置姿勢ｐ２（１０、０、０、０、０、０）、位置姿勢ｐｊ（１０、１０、１０、１０、１０、４５）について、図４の取得済みデータベースＤＢ１を検索し、一致するデータが存在しないので、校正動作計画表ＴＢ１のＩＤ＝１の行に記述した。

同様に位置姿勢ｐ１、ＩＤ＝２の位置姿勢ｐ１（０、０、０、１０、０、－５）、位置姿勢ｐ２（－１０、－５、５、０、１０、１０）について図４の取得済みデータベースＤＢ１を検索し、一致するデータが存在したので、校正動作計画表ＴＢ１のＩＤ＝２の行には記述せず、取得済みデータベースＤＢ１に存在しない位置姿勢ｐｊ（０、５、０、２０、－２０、５）を校正動作計画表ＴＢ１のＩＤ＝１の行に記述した。

図６の校正動作計画表ＴＢ１に抽出された位置姿勢情報ｐは、いずれも図４の取得済み動作データベースＤＢ１には存在しない情報である。このため、これら不存在の位置姿勢データについて、画像と位置姿勢のペアである取得済み動作データＤ１を新たに入手して取得済み動作データベースＤＢ１に補充、記憶する。そのうえで、図５の入手済み位置姿勢情報ｐを含むパターンについて、図２のカメラ位置姿勢推定部３００において推定を行い、校正データを用いてタスク実行することにより、タスク成功の確率を向上させることができる。

なお、校正動作データベースＤＢ２に記憶される校正動作データＤ２は、カメラ位置姿勢校正装置１００を最初に使用する際に作成し、その後、カメラ位置姿勢を校正するたびに更新する。

次に、統合動作計画部１０３における処理の内容について説明する。統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画Ｌ１と校正動作計画部１０２によって計画された校正動作計画Ｌ２を、タスクの状況および目標から優先度算出部１０４によって算出された校正優先度に基づいて統合することで動作計画Ｌを作成するものである。

なお、統合動作計画部１０３における処理の内容を理解するうえでは、優先度算出部１０４によって算出される校正優先度の算出手法を理解しておくのがよいので、以下においてはまず優先度算出部１０４についての説明を行う。

図９、図１０、図１１を用いて、校正優先度算出部１０４における処理の内容について説明する。校正優先度算出部１０４は、タスクの状況および目標に基づいて、タスク動作計画Ｌ１に対する校正動作計画Ｌ２の優先度を算出する。

タスクの状況および目標は、タスク状況目標取得部３１０が与えるタスク状況目標表ＴＢ２で管理される。図９は、タスク状況目標表ＴＢ２の一例を示す図である。タスク状況目標表ＴＢ２の各行はロボット２００が処理するタスクの状況および目標であり、ロボット２００が処理する順番に並んでいる。たとえば、タスクの目標は作業対象の管理番号Ｄ３１、終了目標時間Ｄ３２であり、タスクの状況は達成度Ｄ３３、終了予定時間Ｄ３４、タスク失敗回数Ｄ３５である。

図９のタスク状況目標表ＴＢ２の事例によれば、ＩＤ＝１のタスクについて、１０：００までに終了すべきところ、９：５５に終了予定であり、現在時点において５０％ほどの処理が完了しており、かつ過去に１度の失敗をしていることがわかる。なお、ＩＤ＝１以外のタスクは全て今後実施予定のものである。

タスク状況目標表ＴＢ２は、タスク状況・目標取得部３１０により更新される。たとえば、ピッキングロボット２００においては、タスクの目標である作業対象の管理番号Ｄ３１、終了目標時間Ｄ３２は、オペレータや生産管理システムにより入力される。

タスクの状況である達成度Ｄ３３や失敗回数Ｄ３５は、ピッキングロボット２００によるピッキング動作の結果に基づいて更新する。終了予定時間Ｄ３４は、たとえば、過去の動作時間と達成度から計算する。達成度Ｄ３３はワークの総数および残数などから計算する。また、終了予定時間Ｄ３４は、タスク動作計画部１０１が出力したタスク動作計画Ｌ１から、動作に必要な時間を算出してもよい。

校正優先度算出部１０４は、たとえば、出力動作種類と校正使用可能時間を、タスク動作計画Ｌ１に対する校正動作計画Ｌ２の優先度として算出する。ここで出力動作種類は、統合動作計画部１０３が出力する動作計画の種類であり、校正動作計画Ｌ１もしくは統合動作計画Ｌである。なお統合動作計画Ｌは、タスク動作計画Ｌ１と校正動作計画Ｌ２を実行するための計画である。校正使用可能時間は、校正動作にかけることが可能な時間である。

図１０は校正優先度算出部１０４において実行される処理を示すフローチャートである。校正優先度算出部１０４の最初の処理ステップＳ６００では、ロボット２００に現在設定されているタスクに対する余裕時間である作業余裕時間を算出する。具体的には、現在のタスクに対応するタスク状況目標表ＴＢ２の終了目標時間Ｄ３２から終了予定時間Ｄ３５を引いた結果を作業余裕時間とする。図９のＩＤ＝１の場合、タスク状況目標表ＴＢ２の終了目標時間Ｄ３２（１０：００）から終了予定時間Ｄ３５（９：５５）を引いた結果である５分を作業余裕時間とする。ただし、終了目標時間Ｄ３２から終了予定時間Ｄ３５を引いた結果が０より小さい場合には、作業余裕時間は０とする。

処理ステップＳ６１０では、ロボット２００に現在設定されているタスクに対する失敗回数Ｄ３５が、事前に設定された閾値以上の場合は処理ステップＳ６２０に、そうでない場合は処理ステップＳ６３０に進む。

処理ステップＳ６２０では、出力動作種類を校正動作計画Ｌ１に設定し、校正使用可能時間を作業余裕時間に設定する。これにより、校正作業を実施せずにタスクを継続することはできないと判断し、作業余裕時間を用いて校正動作を実施するように設定する。先の例では、５分間で校正動作を実施するように設定する。

処理ステップＳ６３０では、作業余裕時間が０より大きい場合は処理ステップＳ６４０に、そうでない場合は処理ステップＳ６５０に進む。

処理ステップＳ６４０では、出力動作種類を統合動作計画Ｌに設定し、校正使用可能時間を作業余裕時間に設定する。これにより、タスクを継続しながら、作業余裕時間を用いて統合動作を実施するように設定する。先の例では、１０：００までにタスクが完了するように、校正動作とタスク動作による統合動作を実施するように設定する。

処理ステップＳ６５０では、校正作業を実施することなくタスクを継続した場合に、タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出する。たとえば、現在ロボット２００に設定されているタスクに対する達成度、失敗回数から、今後タスクに失敗する回数を算出する。最後に、過去の実績から算出した１動作にかかる時間の平均、もしくは、タスクに失敗した際の動作時間の平均、と失敗回数から、推定失敗時間を算出する。

処理ステップＳ６６０では、出力動作種類を統合動作計画Ｌに設定し、校正使用可能時間を推定失敗時間に設定する。これにより、作業余裕時間が０の場合においても、校正作業を実施することなくタスクを継続した場合にタスク失敗によって失う時間よりも短い時間で実行可能な統合動作を実施するように設定する。

図１１は、統合動作計画部１０３において実行される処理を示すフローチャートである。統合動作計画部１０３の最初の処理ステップＳ７００では、優先度算出部１０４によって算出された出力動作計画が統合動作計画Ｌの場合には処理ステップＳ７１０に進み、それ以外の場合には処理ステップＳ７３０に進む。

処理ステップＳ７１０では、校正動作計画表ＴＢ１に含まれる校正動作計画毎に、統合動作計画Ｌと、統合動作計画Ｌによってタスク動作計画Ｌ１よりも余分に必要となる時間である校正動作時間を算出する。処理の詳細は後述する。

処理ステップＳ７２０では、校正動作計画表ＴＢ１と、校正動作計画表ＴＢ１に含まれる各校正動作に対応する統合動作および校正動作時間から、統合動作計画部１０３が出力する統合動作を選択する。

たとえば、まず、校正使用可能時間よりも短い校正動作時間の統合動作を選択候補とする。次に校正動作計画表ＴＢ１とタスク状況目標表ＴＢ２を参照し、選択候補のうち、最も先のタスクで対象となる作業対象Ｗに対してタスクを成功可能な統合動作を出力する。なお、最も先のタスクまで成功可能な選択候補が複数あった場合には、校正動作時間が最も短い統合動作を出力する。また、選択候補が１つも存在しない場合には、統合動作ではなく、タスク動作計画部１０１が出力したタスク動作を出力する。

処理ステップＳ７３０は校正動作計画表ＴＢ１に含まれる校正動作計画毎に、校正動作に必要となる校正動作時間を算出する。処理の詳細は後述する。

処理ステップＳ７４０は、校正動作計画表ＴＢ１と、校正動作計画表ＴＢ１に含まれる各校正動作に対応する校正動作時間から、統合動作計画部１０３が出力する校正動作を選択する。

たとえば、まず、校正使用可能時間よりも短い校正動作時間の校正動作を選択候補とする。次に校正動作計画表ＴＢ１とタスク状況目標表ＴＢ２を参照し、選択候補のうち、最も先のタスクで対象となる作業対象に対してタスクを成功可能な校正動作を出力する。なお、最も先のタスクまで成功可能な選択候補が複数あった場合には、校正動作時間が最も短い校正動作を出力する。また、選択候補が１つも存在しない場合には、現在のタスクを成功可能な校正動作のうち、校正動作時間が最も短い校正動作を出力する。

図１２は処理ステップＳ７１０において実行される処理を示すフローチャートである。

処理ステップＳ７１１では、Ｓ７１１ａとＳ７１１ｂの間で校正動作計画表ＴＢ１の各行ｋに対するループ処理を実行する。

処理ステップＳ７１２では、Ｓ７１２ａとＳ７１２ｂの間で校正動作計画表ＴＢ１のｋ行に含まれる各位置姿勢ｐ_ｋのタスク動作計画への割当パターンに対するループ処理を開始する。

校正動作計画表ＴＢ１のｋ行には複数の位置姿勢ｐ_ｋが含まれている。また、タスク動作計画部１０１は複数のタスク動作計画を出力している。すなわち、複数の位置姿勢ｐ_ｋを、複数のタスク動作計画と統合する際には、位置姿勢ｐ_ｋとタスク動作計画の組み合わせが複数パターン存在する。また、ひとつのタスク動作計画に対して、複数の位置姿勢ｐ_ｋを統合する場合には、位置姿勢ｐ_ｋを統合する順番にも複数のパターンが存在する。処理ステップＳ７１２は、これら全てのパターンに対するループ処理を開始する。

処理ステップＳ７１３では、校正動作計画表ＴＢ１に含まれる位置姿勢ｐ_ｋとタスク動作計画を統合した統合動作を計画する。具体的には、処理対象のパターンにおいて、校正動作計画表ＴＢ１に含まれる位置姿勢ｐ_ｋが割り当てられたタスク動作計画毎に、公知の動作計画手法により、ロボット２００の基準位置姿勢からスタートし、校正動作計画表ＴＢ１に含まれる位置姿勢ｐ_ｋを経由し、タスク動作計画部１０１によって動作計画として出力されたタスクを実行可能な位置姿勢に到達する動作を計画する。

たとえば動作計画手法としては、Ｊ．Ｊ．Ｋｕｆｆｎｅｒ,ａｎｄ Ｓ．Ｍ．ＬａＶａｌｌｅ,ＲＲＴ－ｃｏｎｎｅｃｔ：Ａｎ ｅｆｆｉｃｉｅｎｔ ａｐｐｒｏａｃｈ ｔｏ ｓｉｎｇｌｅ－ｑｕｅｒｙ ｐａｔｈ ｐｌａｎｎｉｎｇ,Ｉｎｔ．Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ、 ｐｐ．９９５－１００１（２０００）を用いることができる。

処理ステップＳ７１４では、校正動作にかかる時間を算出する。具体的には、校正動作計画表ＴＢ１に含まれる位置姿勢ｐ_ｋが割り当てられたタスク動作計画毎に、処理ステップＳ７１３で計画された統合動作をロボット２００が実施した際に必要な時間を、ロボット２００の仕様に基づいて算出する。

また、校正動作を経由しない場合のロボット２００の動作時間、すなわち、ロボット２００の基準位置をスタートし、タスクを実行可能な場所に到達する動作を計画し、動作にかかる時間を算出する。

次に、統合動作をロボット２００が実施した際に必要な時間と、校正動作を経由しない場合のロボット２００の動作時間の差を、校正動作にかかる時間とする。最後に、校正動作計画表ＴＢ１に含まれる位置姿勢ｐ_ｋが割り当てられたタスク動作計画毎に計算した校正動作にかかる時間を合計することで、処理対象のパターンに対する校正動作時間とする。

処理ステップＳ７１５は、処理ステップＳ７１２で開始された、校正動作計画表ＴＢ１のｋ行に含まれる各位置姿勢ｐ_ｋのタスク動作計画への割当パターンに対するループ処理で算出された全ての校正動作時間から、最も短い校正動作時間と、対応する統合動作を選択する。

図１１に戻り、処理ステップＳ７３０の処理を説明する。処理ステップＳ７３０は処理ステップＳ７１０と同様の処理により、校正動作計画と校正動作時間を算出する。ただし、処理ステップＳ７１０とは異なり、校正動作計画とタスク動作計画の統合は実施せず、校正動作のみからなる校正動作計画と校正動作時間を算出する。

具体的には、校正動作計画表ＴＢ１の行毎に、行に含まれる複数の位置姿勢の順番の全てのパターンに対して、ロボット２００の基準位置から位置姿勢に達する動作計画を、公知の動作計画手法により計画し、校正動作計画とする。また、ロボット２００の仕様から校正動作計画を実施するために必要な時間を校正動作時間とする。最後に、校正動作計画表ＴＢ１の行毎に、最も短い校正動作時間と対応する校正動作を選択する。

上述した本発明の実施例１によれば、次の作用効果が得られる。

（１）カメラ位置姿勢校正装置１００は、タスク動作計画部１０１と、校正動作計画部１０２と、統合動作計画部１０３と、を備える。タスク動作計画部１０１は、画像とロボット２００の位置姿勢のデータから、ロボット２００に設定されたタスクを実行するための動作を計画する。校正動作計画部１０２は、画像とロボット２００の位置姿勢から、カメラ位置姿勢の校正に必要な動作を計画する。統合動作計画部１０３は、タスク動作計画部１０１によって計画されたタスク動作計画と校正動作計画部１０２によって計画された校正動作計画を統合することで動作を計画する（図１）。そのため、タスク動作計画と校正動作計画が統合され、ロボット２００に設定されているタスクを中断することなく、カメラ２０１とロボット２００の相対位置姿勢を校正することができる。また、カメラ２０１とロボット２００の相対位置姿勢の校正に必要となる時間を短くすることができる。

（２）校正動作計画部１０２は、取得済みの画像とロボット２００の位置姿勢を含む取得済み動作データベースＤＢ１と、校正に用いたロボット２００の位置姿勢および校正結果によるタスクの成否を含む校正動作データベースＤＢ２に基づいて校正に必要な動作を計画し、校正動作計画表ＴＢ１として出力する（図４～図８）。そのため、取得済み動作が考慮されることにより、カメラ２０１とロボット２００の相対位置姿勢を校正に必要となる時間を短くすることができる。

（３）統合動作計画部１０３は、校正優先度算出部１０４を備える。校正優先度算出部１０４は、タスクの状況および目標を含むタスク動作目標表ＴＢ２に基づいて、タスク動作計画に対する校正動作計画の優先度を算出する。統合動作計画部１０３は、校正優先度算出部１０４によって算出された校正動作計画の優先度に基づいて、タスク動作計画部１０１によって計画されたタスク動作計画と校正動作計画部１０２によって計画された校正動作計画を統合することで動作を計画する（図９～図１２）。そのため、タスクの状況および目標を考慮しながら、カメラ２０１とロボット２００の相対位置姿勢を校正することで、ロボット２００は与えられたタスクを目標どおりに実施することができる。

（４）校正優先度算出部１０４は、タスクの終了目標時間と終了予定時間から、タスクに対する余裕時間である作業余裕時間を算出し、作業余裕時間に基づいて、校正動作にかけることが可能な時間である校正使用可能時間を、校正動作計画の優先度として算出する（図９、図１０）。そのため、作業余裕時間を考慮しながら、カメラ２０１とロボット２００の相対位置姿勢を校正することで、ロボット２００は与えられたタスクを目標どおりに実施することができる。

（５）校正優先度算出部１０４は、ロボット２００に設定されているタスクに対する失敗回数に基づいて、統合動作計画部１０３が出力する動作計画の種類である出力動作種類と校正使用可能時間を、校正動作計画の優先度として算出する（図９、図１０）。そのため、タスクに対する失敗回数を考慮し、失敗が多い場合には校正動作を実施し、カメラ２０１とロボット２００の相対位置姿勢を校正することで、ロボット２００は与えられたタスクを目標どおりに実施することができる。

（６）校正優先度算出部１０４は、ロボット２００に設定されているタスクに対する達成度、失敗回数から校正作業を実施することなくタスクを継続した場合に、タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出し、推定失敗時間に基づいて、校正動作にかけることが可能な時間である校正使用可能時間を、校正動作計画の優先度として算出する（図９、図１０）。そのため、推定失敗時間を考慮し、推定失敗時間よりも短い時間でカメラ位置姿勢を校正できる場合には、カメラ位置姿勢を校正することで、校正を実施しない場合よりも短い時間でタスクを完了することができる。

（７）統合動作計画部１０３は、タスク動作計画部１０１によって計画された各タスク動作計画と、校正動作計画部１０２によって計画された各校正動作計画を、複数の組み合わせパターンで統合し、統合することでタスク動作計画に対して追加された動作時間である校正動作時間が最も短い統合動作を、各校正動作計画に対する統合動作として選択する（図１１、図１２）。そのため、カメラ２０１とロボット２００の相対位置姿勢の校正に必要となる時間を短くすることができる。

（８）統合動作計画部１０３は、校正動作データベースＴＢ１に含まれる校正結果によるタスクの成否と、タスク状況目標表ＴＢ２に含まれるロボット２００にタスクとして与えられる作業対象の予定とに基づいて、統合動作を選択する（図９、図１１）。そのため、作業対象の予定が考慮されることにより、長期的にみて短い時間でタスクを完了することができる。

なお、本発明の実施例１を実現するに当たり、さらに以下の変形実施例とすることができる。

まず、校正動作データベースＤＢ２はカメラ位置姿勢校正装置１００を最初に使用する際に、様々なロボット２００の位置姿勢で画像を取得し、取得した画像とロボット２００の位置姿勢を用いた校正結果によるタスクを実施することで作成した。しかし、校正動作データベースＤＢ２の作成方法はこれに限定されない。

また統合動作計画部は、タスク動作計画部によって計画された各タスク動作計画と、校正動作計画部によって計画された各校正動作計画を、複数の組み合わせパターンで統合し、統合することでタスク動作計画に対して追加された動作時間である校正動作時間が最も短い統合動作を、各校正動作計画に対する統合動作として選択するものであってもよい。

また統合動作計画部は、校正動作データベースと、複数の予定されているタスクの目標とを用いて統合動作を選択するものであってもよい。

さらに校正動作データベースをシミュレーションにより作成するものであってもよい。

校正動作データベースＤＢ２は、ロボット２００を動作させることなく、シミュレーションにより作成しても良い。たとえば、ロボット２００の構成、配置や、作業対象の形状などからシミュレーション環境を作成し、仮想的に画像とロボット２００の位置姿勢のペアを複数取得し、実データを用いる場合と同様の方法で校正動作データベースＤＢ２を作成しても良い。また、シミュレーションと実環境を一致させるために、実際の作業環境の画像を取得し、画像を用いた三次元復元手法や物体認識手法によって、実環境を模擬したシミュレーション環境を作成し、校正動作データベースＤＢ２の作成に用いても良い。

上述した変形例１によれば、次の作用効果が得られる。すなわち、カメラ位置姿勢校正装置１００は、シミュレーションにより、校正動作データベースＤＢ２を作成する。そのため、校正動作データベースＤＢ２の作成にかかる時間が短くなることにより、カメラ位置姿勢校正装置を早期に使用開始することができる。

以下、図１３～図１５を参照して、本発明の実施例２に係るカメラ位置姿勢校正装置について説明する。なお以下の説明では、実施例１と同じ構成要素には同じ符号を付して相違点を主に説明する。特に説明しない点については、実施例１と同じである。本実施例では、ロボット２００が自動運転車の場合を対象とする。

実施例２におけるカメラ位置姿勢校正装置の構成は、基本的に図１と同じであり、優先度算出部１０４の処理が異なる点でのみ相違する。

実施例２では、ロボット２００は自動運転車である。図１３は自動運転車２３０を駐車スペースに駐車する状況を示す図である。自動運転車２３０は、駐車場内において駐車スペースである目標位置姿勢２４１へ自動的に走行する。自動運転車２３０はカメラ２３１を搭載しており、カメラ２３１により周辺環境を認識し、自動運転車２３０を制御することで、自動運転車２３０を目標位置姿勢２４１へ移動させる。

図１３に示す例では、自動運転車２３０は、他の車両２４２が駐車していない駐車スペースを認識することで目標位置姿勢２４１とし、自動で走行することで自動駐車を実現する。なお、自動運転車２３０の構成は任意である。たとえば、自動運転車２３０は複数のカメラ２３１を搭載しても良いし、ＬｉＤＡＲやレーダーなど他のセンサを搭載しても良い。また、自動運転車２３０は、通信装置により監視センタと通信しても良い。

自動運転車２３０において、図２のロボットにおける位置姿勢センサ２０２は、たとえば、タイヤとステアリングの回転量を計測するセンサである。自動運転車２３０の位置姿勢としては、車両の制御に重要となる後輪車軸中心の位置姿勢を用いる。タイヤとステアリングの回転量を計測することで、自動運転車２３０の幾何モデルに基づき、公知の手法であるホイールオドメトリにより自動運転車２３０の位置姿勢を得ることができる。

自動運転車２３０において、設定されたタスクは目標位置姿勢２４１まで走行することである。タスクの状況は、たとえば、目標位置姿勢２４１の種類や形状、目標位置姿勢２４１までの距離などである。タスクの目標は、たとえば、目標位置姿勢２４１に到達する時間などである。

自動運転車２３０において、図１のタスク動作計画部１０１は、カメラ２３１によって撮影された周辺環境の画像と、自動運転車２３０の位置姿勢のデータから、自動運転車２３０を目標位置姿勢２４１まで移動させるための動作を計画する。たとえば、タスク動作計画部１０１は、公知のパターンマッチング手法や機械学習手法により、駐車スペースの位置姿勢を推定することで、駐車可能な位置姿勢を算出し、目標位置姿勢２４１とする。

複数の駐車スペースが認識された場合には、それぞれの駐車スペースに対応した、駐車可能な位置姿勢を目標位置姿勢２４１とする。なお、目標位置姿勢２４１は駐車スペースに限らず、ユーザが求める任意の位置姿勢でも良い。また、カメラ２３１に加えて他のセンサや地図情報を用いて目標位置姿勢２４１を算出しても良い。

次にロボットが自動運転車２３０である場合の校正優先度算出部１０４の動作、処理内容について、図１４、図１５を用いて説明する。

図１４は、自動運転車２３０におけるタスク状況目標表ＴＢ２の一例を示す図である。図９に示した実施例１のタスク状況目標表ＴＢ２の項目に搭乗者の人数Ｄ３０が追加されている。

自動運転車２３０におけるタスク状況目標表ＴＢ２において、作業対象Ｄ３１は目標位置姿勢２４１である。また、タスクの失敗Ｄ３５は、目標位置姿勢２４１への走行の失敗にあたり、たとえば、ドライバが周辺環境の物体への接触の危険性を察知し、ブレーキを踏んだ場合には失敗と判断する。また、超音波センサなどにより、周辺環境の物体への接触の危険性を検知した場合にも失敗と判断する。

図１４のタスク状況目標表ＴＢ２は、タスク状況・目標取得部３１０により更新される。たとえば、自動運転車２３０においては、タスクの目標である作業対象の管理番号、終了目標時間Ｄ３２は、ユーザにより入力される、もしくは、自動運転システムによって設定される。タスクの状況である達成度Ｄ３３や失敗回数Ｄ３５は、自動運転車２３０による走行結果に基づいて更新する。終了予定時間Ｄ３４は、たとえば、過去の動作時間と達成度から計算する。達成度Ｄ３３は目標位置姿勢２４１までの距離および走行距離から計算する。また、終了予定時間Ｄ３４は、タスク動作計画部１０１が出力した動作計画から、動作に必要な時間を算出してもよい。

図１５は、校正優先度算出部１０４において実行される処理を示すフローチャートである。実施例２においては、図１に示す実施例１の校正優先度算出部１０４において実行される図１０のフローチャートに、処理ステップＳ９７０および処理ステップＳ９８０が追加されている。

処理ステップＳ９７０では、搭乗者が０の場合には処理ステップＳ９８０に進み、それ以外の場合は処理ステップＳ６５０に進む。

処理ステップＳ９８０では、出力動作種類を統合動作計画Ｌ、校正使用可能時間を事前に設定した値に設定する。搭乗者が０の場合には、校正使用可能時間を事前に設定した値に設定することで、終了目標時間にタスクを終えることよりも、校正作業を優先する。

実施例２によれば、次の作用効果が得られる。すなわち、校正優先度算出部１０４は、自動運転車２３０の搭乗者の数に基づいて校正動作にかけることが可能な時間である校正使用可能時間を、校正動作計画の優先度として算出する。そのため、搭乗者の数が０のときに校正を実施することにより、搭乗者がいる場合における校正動作時間を短くすることで、校正を実施することによる搭乗者への負担を小さくすることができる。

なお、本発明は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。本発明の技術的思想の範囲内で考えられるその他の態様も本発明の範囲内に含まれる。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。

１００：カメラ位置姿勢校正装置、１０１：タスク動作計画部、１０２：校正動作計画部、１０３：統合動作計画部、１０４：校正優先度算出部、２００：ロボット（ピッキングロボット）、２０１：カメラ、２０２：位置姿勢センサ、２２０：作業台、２３０：自動運転車、３００：カメラ位置姿勢推定部、３１０：タスク状況・目標取得部、ＤＢ１：取得済み動作データベース、ＤＢ２：校正動作データベース、ＴＢ１：校正動作計画表、ＴＢ２：タスク状況目標表

Claims (14)

1. ロボットに搭載されたカメラにより撮影された画像と前記ロボットの位置姿勢から、前記ロボットに設定されたタスクを実行するための動作を計画するタスク動作計画部と、
   前記画像と前記ロボットの位置姿勢から、前記カメラと前記ロボットの相対位置姿勢の校正に必要な動作を計画する校正動作計画部と、
   前記タスク動作計画部によって計画されたタスク動作計画と前記校正動作計画部によって計画された校正動作計画を統合することで動作を計画する統合動作計画部と、
   を備え、
   前記校正動作計画部は、複数組の取得済みの前記画像と前記ロボットの位置姿勢を含む取得済み動作データと、複数組の前記ロボットの位置姿勢と前記ロボットの位置姿勢を用いた校正結果による前記タスクの成否を含む校正動作データと、を用いて前記カメラと前記ロボットの相対位置姿勢の校正に必要な動作を計画することを特徴とするカメラ位置姿勢校正装置。
2. 請求項１に記載のカメラ位置姿勢校正装置であって、
   前記タスクの状況および目標を与えるタスク状況・目標取得部と、前記タスク状況・目標取得部からの前記タスクの状況および目標に基づいて、前記タスク動作計画に対する前記校正動作計画の優先度を算出する校正優先度算出部を備え、
   前記統合動作計画部は、前記校正優先度算出部によって算出された前記優先度に基づいて、前記タスク動作計画と前記校正動作計画を統合することで動作を計画することを特徴とするカメラ位置姿勢校正装置。
3. 請求項２に記載のカメラ位置姿勢校正装置であって、
   前記校正優先度算出部は、前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間を用いて、校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
4. 請求項２または請求項３に記載のカメラ位置姿勢校正装置であって、
   前記校正優先度算出部は、前記タスクに対する失敗回数を用いて、前記統合動作計画部が出力する動作計画の種類である出力動作種類と校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
5. 請求項２ないし請求項４のいずれか一項に記載のカメラ位置姿勢校正装置であって、
   前記校正優先度算出部は、前記タスクに対する達成度および失敗回数から校正作業を実施することなく前記タスクを継続した場合に、前記タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出し、前記推定失敗時間を用いて校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
6. 請求項２ないし請求項５のいずれか一項に記載のカメラ位置姿勢校正装置であって、
   前記校正優先度算出部は、前記ロボットの搭乗者の数に基づいて校正動作にかけることが可能な時間である校正使用可能時間を、前記優先度として算出することを特徴とするカメラ位置姿勢校正装置。
7. 請求項１ないし請求項６のいずれか一項に記載のカメラ位置姿勢校正装置であって、
   前記統合動作計画部は、前記タスク動作計画部によって計画された各タスク動作計画と、前記校正動作計画部によって計画された各校正動作計画を、複数の組み合わせパターンで統合し、統合することで前記タスク動作計画に対して追加された動作時間である校正動作時間が最も短い統合動作を、各校正動作計画に対する統合動作として選択することを特徴とするカメラ位置姿勢校正装置。
8. 請求項１ないし請求項７のいずれか一項に記載のカメラ位置姿勢校正装置であって、
   前記統合動作計画部は、前記校正動作計画と、複数の予定されている前記タスクの目標とを用いて統合動作を選択することを特徴とするカメラ位置姿勢校正装置。
9. 請求項１ないし請求項８のいずれか一項に記載のカメラ位置姿勢校正装置であって、
   前記校正動作計画をシミュレーションにより作成することを特徴とするカメラ位置姿勢校正装置。
10. 請求項１ないし請求項８のいずれか一項に記載のカメラ位置姿勢校正装置により操作されるロボット。
11. ロボットに搭載されたカメラにより撮影された画像と前記ロボットの位置姿勢から、前記ロボットに設定されたタスクを実行するための動作を計画したタスク動作計画と、前記画像と前記ロボットの位置姿勢から、前記カメラと前記ロボットの相対位置姿勢の校正に必要な動作を計画した校正動作計画と、前記タスク動作計画と前記校正動作計画を統合した統合動作計画と、前記タスクの状況および目標を得、
    前記タスクの状況および目標に基づいて、前記タスク動作計画に対する前記校正動作計画の優先度を算出し、前記優先度に基づいて、動作計画を選択し、選択した動作計画により前記ロボットを操作することを特徴とするカメラ位置姿勢校正方法。
12. 請求項１１に記載のカメラ位置姿勢校正方法であって、
    前記タスクに失敗した場合に、前記校正動作計画を優先し、前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間を用いて、校正動作を実施することを特徴とするカメラ位置姿勢校正方法。
13. 請求項１１に記載のカメラ位置姿勢校正方法であって、
    前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間があるとき、前記統合動作計画を優先し、タスクを継続実施することを特徴とするカメラ位置姿勢校正方法。
14. 請求項１１に記載のカメラ位置姿勢校正方法であって、
    前記タスクの終了目標時間と終了予定時間から算出した作業余裕時間がないとき、前記タスクを継続した場合に、前記タスクに失敗する動作にかかると推定される時間である推定失敗時間を算出し、推定失敗時間よりも短い時間で実行可能な前記統合動作計画を実施することを特徴とするカメラ位置姿勢校正方法。

Images