JP2024017442A - 自律ロボットとそのキャリブレーション方法 - Google Patents

Abstract

【課題】付加軸を有する自律ロボットにおいて、キャリブレーションにかかる工数を低減する。【解決手段】初期位置におけるロボットと撮像手段との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータ２を推定する初期校正実行工程と、校正対象の付加軸を１つずつ選択し、校正対象の付加軸を２地点以上に移動させ、各地点で撮像手段により撮影した画像のデータ群に基づき、校正対象の付加軸の傾き角度のパラメータである付加軸傾き角パラメータ４を推定する付加軸傾き角度推定工程と、を有する。初期位置姿勢変換パラメータ２と付加軸傾き角パラメータ４と付加軸の移動量とに基づいて任意の付加軸の位置の組合せにおけるロボットと撮像手段との間の相対的な位置と姿勢の変換パラメータ（現在位置姿勢変換パラメータ１０）を推定し、自律作業を実行する。【選択図】図１

Description

本発明は、ロボットと撮像手段とを用いて自律作業を行う自律ロボットとそのキャリブレーション方法に関し、特に、付加軸を有する自律ロボットとそのキャリブレーション方法に関する。

近年の労働力不足を解消するため、物流分野におけるピッキング作業や産業分野における組み立て作業など、従来は人が行っていた何らかのワーク（作業対象物）を取り扱う各種作業を自動化する自律ロボットのニーズが高まっている。ロボットに適切な自律作業を実行させるには、ワークなどの周辺環境を高精度に認識するための撮像手段が必要となる。

撮像手段に基づく認識結果によりロボットが自律作業するには、ロボットと撮像手段との間の相対的な位置と姿勢の変換パラメータを決定するキャリブレーション（校正）の実施が必要である。

このキャリブレーションには、例えば、形状が既知のマーカ（ターゲットマーカとも呼ばれる）をロボットに取り付け、様々な姿勢に変更したロボット上のマーカを周辺に設置された撮像手段により撮影し、撮影した画像とロボットの姿勢との複数の組を用いて、ロボットと撮像手段との間の相対的な位置と姿勢の変換パラメータを推定するハンドアイキャリブレーションとよばれる方法がある。

ここで、一般的な自律ロボットでは、ロボットおよび撮像手段がそれぞれ環境に固定されており、ロボットに対する撮像手段の配置が単一であるため、ハンドアイキャリブレーションを１度実行することで、自律作業を実行できる。

しかし、ロボットおよび／または撮像手段を移動させる付加軸（スライダによる並進軸またはモータによる回転軸の総称）を有する自律ロボットの場合、付加軸による移動によってロボットと撮像手段との間の配置が変更されるため、移動後の位置で再度ハンドアイキャリブレーションを行う必要がある。

また、撮像手段の配置や向きが変更される場合に、カメラ座標系におけるワークの位置とロボット座標系におけるワークの位置との間を変換する技術として、例えば特許文献１がある。

特許文献１の請求項１には、「溶接ロボットで自動溶接される溶接対象物に形成された開先の断面形状を、前記溶接ロボットに対する位置が変更可能なカメラを用いて測定する開先形状測定方法であって、前記カメラが撮影した被写体のカメラ座標系の画像データを前記溶接ロボットのロボット座標系に対応させるための準備データを、前記カメラの位置に応じて準備しておく準備工程と、前記カメラが前記開先を撮影する撮影工程と、前記準備工程で準備された前記準備データを用い、前記撮影工程で撮影された前記開先の画像データから、前記開先の断面形状情報を検出する測定工程と、を有することを特徴とする開先形状測定方法」が記載されている。

また、特許文献１の段落００４３には、「本実施形態では、撮影工程Ｓ２および測定工程Ｓ３により、開先３の任意の位置を測定部位３１として選択し、その断面形状３２を画像処理により測定（センシング）することができる。断面形状３２の測定（撮影工程Ｓ２および測定工程Ｓ３）は、溶接ロボット１０を開先３に沿って移動させることで、開先３の複数の測定部位３１について間欠的または連続的に行うことができる。」ことが記載されている。

すなわち、特許文献１には、あらかじめ作業時のカメラの位置に応じて、カメラが撮影した被写体のカメラ座標系の画像データを溶接ロボットのロボット座標系に対応させるための準備データを準備しておくこと、および、溶接ロボットを移動させて複数の測定部位において間欠的または連続的に測定を行い、準備データを準備することが記載されている。

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

しかしながら、ロボットおよび／または撮像手段が付加軸により任意の位置に移動可能で作業時のワークの状況に基づき作業中でもロボットや撮像手段の相対的な配置が動的に変化する自律ロボットに対して特許文献１の技術を適用しようとすると、すべてのロボットおよび／またはカメラの位置に対応して準備データを準備しておく必要があり、キャリブレーションにかかる工数が膨大になってしまうという問題がある。

同様に、ハンドアイキャリブレーションについても、付加軸を有する自律ロボットでは、付加軸が設計値に対して傾きを有している可能性があるため、自律作業の精度を確保するためにはすべてのロボットおよび／またはカメラの位置に対応してハンドアイキャリブレーションにより変換パラメータを推定する必要があり、キャリブレーションにかかる工数が膨大になってしまうという問題がある。

本発明が解決しようとする課題は、付加軸を有する自律ロボットにおいて、キャリブレーションにかかる工数を低減できる自律ロボットとそのキャリブレーション方法を提供することである。

上記課題を解決するために、本発明の自律ロボットは、例えば、ロボットと、前記ロボットの周辺に設置され前記ロボットの可動域の少なくとも一部を視野に収めて撮影する撮像手段と、前記ロボットおよび／または前記撮像手段を移動させる１つ以上の付加軸と、を有する自律ロボットであって、前記付加軸の位置を移動させる付加軸移動制御部と、前記付加軸の位置の移動前後における移動量を取得する付加軸移動量取得部と、初期位置における前記ロボットと前記撮像手段との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータを推定する初期校正実行部と、１つずつ選択された校正対象の前記付加軸を前記付加軸移動制御部により２地点以上に移動させて各地点で前記撮像手段により撮影した画像のデータ群に基づき、前記校正対象の前記付加軸の傾き角度のパラメータである付加軸傾き角パラメータを推定する付加軸傾き角度推定部と、前記初期位置姿勢変換パラメータと前記付加軸傾き角パラメータと前記付加軸の移動量とに基づいて任意の前記付加軸の位置の組合せにおける前記ロボットと前記撮像手段との間の相対的な位置と姿勢の変換パラメータを推定する相対位置姿勢推定部と、前記相対位置姿勢推定部で推定した変換パラメータを用いて任意の前記付加軸の位置の組合せにおいて前記ロボットと前記撮像手段との間の相対的な位置と姿勢を既知として自律作業を実行する自律作業実行部と、を有することを特徴とする。

また、本発明の自律ロボットのキャリブレーション方法は、例えば、ロボットと、前記ロボットの周辺に設置され前記ロボットの可動域の少なくとも一部を視野に収めて撮影する撮像手段と、前記ロボットおよび／または前記撮像手段を移動させる１つ以上の付加軸と、を有する自律ロボットのキャリブレーション方法であって、初期位置における前記ロボットと前記撮像手段との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータを推定する初期校正実行工程と、校正対象の前記付加軸を１つずつ選択し、前記校正対象の前記付加軸を２地点以上に移動させ、各地点で前記撮像手段により撮影した画像のデータ群に基づき、前記校正対象の前記付加軸の傾き角度のパラメータである付加軸傾き角パラメータを推定する付加軸傾き角度推定工程と、を有することを特徴とする。

本発明によれば、付加軸を有する自律ロボットにおいて、キャリブレーションにかかる工数を低減できる。

実施例の自律ロボットの機能ブロック図。 実施例の自律ロボットのハードウェア構成の例を示す斜視図。 実施例の自律ロボットの座標系の例を示す斜視図。 実施例の自律ロボットの座標系間の初期関係を示す図。 実施例の初期校正実行部による初期校正実行工程のフローチャート。 実施例の付加軸傾き角度推定部によるロボットスライダの校正の例を示す図。 実施例の付加軸傾き角度推定部によるカメラスライダの校正の例を示す図。 実施例の付加軸傾き角度推定部によるカメラ回転体の校正の例を示す図。

以下、図面を用いて本発明の実施例を説明する。各図において、同一または類似の構成要素については同じ符号を付け、重複する説明は省略する。

（全体構成）
図１は、実施例の自律ロボットの機能ブロック図である。図２は、実施例の自律ロボットのハードウェア構成の例を示す斜視図である。

図２に示すように、実施例の自律ロボットは、ハードウェア構成として、例えば、ロボット３１と、ロボット３１の周辺に設置されロボット３１の可動域の少なくとも一部を視野に収めて撮影する撮像手段４１と、ロボット３１および／または撮像手段４１を移動させる１つ以上の付加軸とを有する。付加軸としては、例えば、スライダによる並進移動やモータによる回転運動などが考えられる。図２では、付加軸の例として、ロボットスライダ３３と、カメラスライダ４２と、カメラ回転体４３とを有する例を示している。

図１に示すように、実施例の自律ロボットは、機能ブロックとして、例えば、初期校正実行部１と、付加軸傾き角度推定部３と、自律作業司令部５と、付加軸位置計画部６と、付加軸移動制御部７と、付加軸移動量取得部８と、相対位置姿勢推定部９と、自律作業実行部１１とを有する。各機能ブロックは、一般的なコンピュータや制御装置などによって実現および実行できる。

付加軸移動制御部７は、付加軸の位置を移動させる。付加軸移動量取得部８は、付加軸の位置の移動前後における移動量を取得する。

初期校正実行部１は、初期位置におけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータ２を推定する。なお、図１では図を見やすくするために矢印を図示していないが、付加軸移動制御部７は、初期校正実行部１からの指令を受けて付加軸の位置を初期位置に移動させることができる。

付加軸傾き角度推定部３は、校正対象の付加軸の傾き角度のパラメータである付加軸傾き角パラメータ４を推定する。付加軸の傾き角度のパラメータは、付加軸の向き、あるいは、付加軸の姿勢のパラメータと言い換えることもできる。より具体的には、付加軸傾き角度推定部３は、１つずつ選択された校正対象の付加軸を付加軸移動制御部７により２地点以上に移動させて各地点で撮像手段４１により撮影した画像のデータ群に基づき、校正対象の付加軸の付加軸傾き角パラメータ４を推定する。校正対象の付加軸が１つの場合は１つ選択して校正を行ったら終了し、校正対象の付加軸が複数ある場合は１つずつ選択して順番に校正を行い、すべて選択し終えたら終了する。なお、図１では図を見やすくするために矢印を図示していないが、付加軸移動制御部７は、付加軸傾き角度推定部３からの指令を受けて付加軸の位置を移動させることができるとともに、付加軸傾き角度推定部３は、付加軸移動量取得部８から付加軸の位置の移動前後における移動量を取得することができる。ここで、校正対象の付加軸が並進移動を行うスライダである場合は２地点、回転軸である場合は３地点で撮影を行えば付加軸傾き角パラメータ４を推定することが可能である。

ここまでが、キャリブレーション工程に関連する機能ブロックである。次に、キャリブレーション工程で推定した初期位置姿勢変換パラメータ２と付加軸傾き角パラメータ４とを用いて自律作業を実行するのに関連する機能ブロックについて説明する。

自律作業司令部５は、自律作業を指示する司令部である。本実施例では、ピッキング作業を指示する例を用いて説明する。付加軸位置計画部６は、自律作業司令部５からの指示を受け、ワークの位置に基づき作業が実行可能な付加軸の位置を計画する。付加軸移動制御部７は、付加軸位置計画部６で計画された計画情報に基づき、付加軸の位置を移動させる。付加軸移動量取得部８は、付加軸の移動量を取得する。

相対位置姿勢推定部９は、初期位置姿勢変換パラメータ２と付加軸傾き角パラメータ４と付加軸の移動量とに基づいて任意の付加軸の位置の組合せにおけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータを推定する。自律作業の実行時においては、相対位置姿勢推定部９は、付加軸の移動があった場合（図１の「移動有」の場合）には、付加軸の移動後である現在作業位置におけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータである現在位置姿勢変換パラメータ１０を推定する。なお、付加軸の移動がなかった場合（図１の「移動無」の場合）には、現在位置姿勢変換パラメータ１０の再計算による更新は省略できる。

自律作業実行部１１は、相対位置姿勢推定部９で推定した変換パラメータである現在位置姿勢変換パラメータ１０を用いて任意の付加軸の位置の組合せにおいてロボット３１と撮像手段４１との間の相対的な位置と姿勢を既知として自律作業を実行する。図１では図を見やすくするために矢印を図示省略しているが、自律作業実行部１１は、自律作業司令部５からの指令を受けてロボット３１の制御も行い、例えばピッキングなどの自律作業を行う。

本実施例によれば、付加軸が設計値に対して傾きを有している場合であっても、少ないキャリブレーション工数（各付加軸毎に２地点または３地点の撮影）で付加軸傾き角パラメータ４を推定でき、初期位置姿勢変換パラメータ２と付加軸傾き角パラメータ４と付加軸の移動量とに基づいて任意の付加軸の位置の組合せにおけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータを推定して自律作業を行うことができる。したがって、すべての付加軸の位置の組合せ（すべてのロボット３１と撮像手段４１の位置の組合せ）に対してその都度ハンドアイキャリブレーションを行って変換パラメータを推定する必要がないので、キャリブレーションにかかる工数を低減できる。

以降は、説明の分かりやすさのため、はじめに自律作業実行部１１におけるハードウェア装置の概要を述べ、次に初期校正実行部１と付加軸傾き角度推定部３にて使用するハードウェア装置および座標系について示した後、初期校正実行部１と、付加軸傾き角度推定部３の詳細について述べる。そして、自律作業司令部５、付加軸位置計画部６、付加軸移動制御部７、付加軸移動量取得部８、相対位置姿勢推定部９、自律作業実行部１１による自律作業実行のフローについて詳細に説明する。

（自律作業実行部１１におけるハードウェア装置）
図２は、実施例の自律ロボットのハードウェア構成の例を示す斜視図である。図２は、自律作業実行部１１にてピッキング作業を行うときのハードウェア構成の例を示している。本実施例における作業内容は、作業台２１の上に置かれた容器２２に、容器２３および容器２４に格納されたワーク群を、自律作業司令部５による作業指示（ワーク種類、個数など）に基づき移動させるピッキングアンドプレイス（以降、「ピッキング作業」とも称する）とする。

本実施例の自律ロボットのハードウェア構成としては、ロボット３１と、ロボット３１の先端に設置されワークを把持するハンド３２と、ロボット３１を水平方向に移動させる単軸のロボットスライダ３３と、ロボット３１とロボットスライダ３３とを接続するロボット支柱３４と、ワークを撮影することでワークに関する２次元画像および／または３次元点群を取得する撮像手段４１と、撮像手段４１を上下方向に移動させるカメラスライダ４２と、回転軸を中心に撮像手段４１をモータで水平方向に回転移動させるカメラ回転体４３と、カメラスライダ４２およびカメラ回転体４３が設けられたカメラ支柱４４とを有する。

ロボット３１としては、例えば多軸のロボットアームを用いることができる。撮像手段４１としては、例えばＲＧＢ－Ｄカメラを用いることができる。ＲＧＢ－Ｄカメラは、可視画像を取得するＲＧＢカメラ以外に、対象物までの距離情報を画像として取得可能なＤｅｐｔｈカメラを有しているカメラである。撮像手段４１としては、これに限られず、可視画像または距離画像を取得可能な撮像装置であればよいが、距離画像を取得可能である方が望ましい。

なお、本実施例では、自律ロボットが行う作業はワークを所定の位置へと移動させるピッキング作業とし、ロボット３１として多軸のロボットアームを用い、撮像手段４１としてＲＧＢ－Ｄカメラを用い、ロボット３１を移動させる付加軸として水平方向のロボットスライダ３３を用い、撮像手段４１を移動させる付加軸として上下方向のカメラスライダ４２および水平方向に回転するカメラ回転体４３を用いる場合について説明するが、この例に限定されず、本実施例で説明するキャリブレーション方法は付加軸を有する様々な自律ロボットに対して適用が可能である。

（初期校正実行部１と付加軸傾き角度推定部３にて使用するハードウェア装置および座標系）
図３は、実施例の自律ロボットの座標系の例を示す斜視図である。図３は、初期校正実行部１および付加軸傾き角度推定部３にて使用するハードウェア装置についても示している。

図３では、ロボット３１の手先に、形状が既知であり、撮像手段４１で撮影したデータにより撮像手段４１に対する相対的な位置と姿勢が特定可能な模様を印刷した平面状のマーカ３５を取り付けた例を示しているが、この例に限らない。例えば、マーカ３５としては、撮像手段４１で撮影したデータにより撮像手段４１に対する相対的な位置と姿勢が特定可能であれば、球状のマーカでも良いし、ロボット３１自体に形状が既知の模様を印刷してもよく、また、模様が印刷されたシールを貼り付けてもよい。

本実施例における座標系は、ロボット基部の座標系であるロボット座標系Ｃ１と、ロボット支柱座標系Ｃ２と、ロボットスライダ座標系Ｃ３と、ロボット先端座標系Ｃ４と、マーカ座標系Ｃ５と、撮像手段の光学中心の座標系であるカメラ座標系Ｃ６と、カメラ回転体座標系Ｃ７と、カメラスライダ座標系Ｃ８とがある。以降は、本実施例におけるこれらの座標系について説明する。なお、座標系の原点や向きおよび座標系間の関係は、本実施例における定義に限定するものではない。

ロボット座標系Ｃ１は、ロボット３１の基部に定義された座標系であり、一般的にロボット３１の上部をＺ軸の正方向とする座標系である。

ロボット支柱座標系Ｃ２は、形式的に定義される。本実施例では、ロボット座標系Ｃ１から形式的に並進移動させた位置に原点を設定し、向きはロボット座標系Ｃ１と同様と定義する。つまり、ロボット座標系Ｃ１とロボット支柱座標系Ｃ２との間の相対的な位置と姿勢のパラメータは常に一定である。多くの場合、ロボットスライダ３３は床などの水平面に設置されるため、その場合はロボット座標系Ｃ１からロボット支柱３４の高さ分だけ下方向にオフセットした位置に、ロボット支柱座標系Ｃ２を定義することができる。

ロボットスライダ座標系Ｃ３は、ロボットスライダ３３に定義された座標系であり、ロボット３１の移動方向をＹ軸、それと直交する方向をＸ軸とする座標系である。原点は、ロボットスライダ３３の移動を指示する際のゼロ位置として設定するが、例えばゼロ位置からオフセットさせた任意の位置に定義してもよい。

ロボットスライダ座標系Ｃ３と、ロボット座標系Ｃ１およびロボット支柱座標系Ｃ２との間の相対的な位置は、ロボットスライダ３３上でロボット３１の位置を移動させるたびに変化するが、ロボットスライダ座標系Ｃ３におけるロボット支柱座標系Ｃ２の原点の位置および移動量は、ロボットスライダ３３のエンコーダ値より高精度に取得可能であり、ロボット支柱３４との間の相対的な位置と姿勢が既知のロボット座標系Ｃ１についても、ロボットスライダ座標系Ｃ３からの相対的な位置を高精度に取得可能である。

ロボットスライダ座標系Ｃ３と、ロボット座標系Ｃ１およびロボット支柱座標系Ｃ２との間の相対的な姿勢は、ロボットスライダ３３上でロボット３１の位置を移動させた場合でも不変であり、単一の姿勢のパラメータで表現することができる。このパラメータは、例として自律ロボットの設計値として取得することも考えられるが、この値は設置時のズレなどにより誤差が生じる可能があるので、本実施例では未知として扱う。

ロボット先端座標系Ｃ４は、ロボット３１の先端に定義された座標系であり、ロボット３１のエンコーダ値により、ロボット座標系Ｃ１に対する相対的な位置と姿勢を高精度に取得可能である。本実施例では、ロボット先端座標系Ｃ４をハンド３２の座標系と異なるものとして定義しているが、ロボット先端座標系Ｃ４とロボットハンド座標系との間の相対的な位置と姿勢のパラメータをツールキャリブレーション等の公知技術により既知とした上で、ロボットハンド座標系をロボット先端座標系Ｃ４として扱ってもよい。

マーカ座標系Ｃ５は、マーカ３５の表面中央を原点、マーカ平面と直交する方向をＺ軸とするよう定義する。マーカ３５を固定する治具により、ロボット先端座標系Ｃ４に対して、相対的な位置と姿勢は常に一定であるとみなせる。一方で、ロボット先端座標系Ｃ４に対する相対的な位置と姿勢のパラメータは、治具の設計値として取得することも考えられるが、この値は治具の作成・設置時のズレにより誤差が生じる可能性があり、未知として扱うことが多く、本実施例でも未知として扱う。

カメラ座標系Ｃ６は、撮像手段４１の光学中心を原点として、光軸方向をＺ軸、光軸に直交する方向をＸＹ軸とする座標系である。詳細は自律作業実行部１１にて述べるが、自律作業実行部１１が自律作業を実施するには、撮像手段４１で撮影したワークの位置情報をロボット３１が用いるために、ロボット座標系Ｃ１とカメラ座標系Ｃ６との間の相対的な位置と姿勢の変換パラメータが必要となる。しかしこれらは未知であり、また、ロボットスライダ３３やカメラ回転体４３やカメラスライダ４２の位置が変化することで、変換パラメータも変化する。本実施例は、このような場合でも、ロボット座標系Ｃ１とカメラ座標系Ｃ６との間の相対的な位置と姿勢の変換パラメータである現在位置姿勢変換パラメータ１０（図１参照）を推定し、自律作業を行うためのキャリブレーション方法およびこれを実施する自律ロボットについて説明する。

カメラ回転体座標系Ｃ７は、カメラ座標系Ｃ６と長さが未知のリンクを介して接続された座標系である。カメラ回転体座標系Ｃ７とカメラ座標系Ｃ６との間の相対的な位置と姿勢についても、正確なリンクの長さや撮像手段４１の光軸の向きが不明のため未知となる。原点は後述するカメラスライダ座標系Ｃ８のＺ軸上の位置、姿勢はカメラスライダ座標系Ｃ８と同様の方向となるよう形式的に定義する。本実施例では、カメラ回転体４３の回転角が０度の時、カメラ回転体座標系Ｃ７のｘ軸上にカメラ接続部のリンクが位置し、撮像手段４１が容器２３の中央部を見るようハードウェア設計されていることを前提とする。

カメラスライダ座標系Ｃ８は、撮像手段４１およびカメラ回転体４３を上下に移動させる機構のゼロ位置を原点として持ち、カメラスライダ４２の移動方向をＺ軸として有する座標系である。カメラスライダ座標系Ｃ８に対するカメラ回転体座標系Ｃ７の位置および移動量は、カメラスライダ４２のエンコーダ値により高精度に既知とできる。

例えばカメラスライダ座標系Ｃ８のＺ軸方向が、ロボット座標系Ｃ１のＺ軸と同様の軸線上を向いていれば、カメラスライダ４２を動かした際の撮像手段４１のロボット３１に対する相対的な位置の変化は、カメラスライダ４２の移動量の分だけＺ軸方向にオフセットすることで求めることも考えられるが、据え付け時の誤差によって生じるカメラスライダ４２の傾き角度や、カメラスライダ４２の軸方向をどの方向にするかという設計によって、カメラスライダ座標系Ｃ８のＺ軸方向がロボット座標系Ｃ１のＺ軸と必ずしも同じ軸線上を向くとは限らない。

図４は、実施例の自律ロボットの座標系間の初期関係を示す図である。校正を行う前の座標系間の結合の関係（初期関係）を整理して図４に示した。座標系間を結ぶ線の種類と座標系間の関係との対応表についても図４に示している。

Ｃ１とＣ２との間は、相対的な位置と姿勢が不変かつ既知である。Ｃ２とＣ３との間は、相対的な位置と姿勢がロボットスライダ３３により変動するが、位置および移動量はロボットスライダ３３のエンコーダ値より既知で、ロボット３１を設置するときのＺ軸周りの回転とロボットスライダ３３の傾き角度により姿勢は未知である。Ｃ１とＣ４との間は、相対的な位置と姿勢が変動するが、ロボット３１のエンコーダ値により位置と姿勢は既知である。Ｃ４とＣ５との間は、相対的な位置と姿勢が不変であるが、位置と姿勢は未知である。Ｃ４とＣ６との間は、相対的な位置と姿勢が各種付加軸により変動し、また、相対的な位置と姿勢が未知である。Ｃ６とＣ７との間は、相対的な位置と姿勢が変動し、また、リンクの長さと回転軸の傾き角度も未知であるため位置と姿勢は未知である。Ｃ７とＣ８との間は、相対的な位置と姿勢はカメラスライダ４２により変動するが、位置および移動量はエンコーダ値により既知であり、姿勢は形式的に同一としているため既知である。

本実施例では、自律作業を行う上で必要となるロボット座標系Ｃ１とカメラ座標系Ｃ６との間の相対的な位置と姿勢の変換パラメータ（現在位置姿勢変換パラメータ１０）を推定するため、はじめに初期位置におけるロボット座標系Ｃ１とカメラ座標系Ｃ６との間の相対的な位置と姿勢の変換パラメータ（初期位置姿勢変換パラメータ２）をハンドアイキャリブレーションにより推定する。次に、付加軸であるロボットスライダ３３、カメラスライダ４２のＺ軸の傾き角度を、それぞれのスライダを移動させた際に撮像手段４１より撮影した画像のデータ群とスライダの移動量に基づいて推定する。また、カメラ回転体４３により撮像手段４１を移動させた際に撮影した画像のデータ群とカメラ回転体４３の回転角度に基づいて、撮像手段４１の回転の半径と回転軸の傾き角度を推定する。そして、これらの情報を付加軸傾き角パラメータ４として格納し、各付加軸によりロボット３１および／または撮像手段４１が移動した場合に、初期位置姿勢変換パラメータ２と付加軸の移動量と付加軸傾き角パラメータ４とを用いることで、現在位置姿勢変換パラメータ１０を推定するキャリブレーション手段を提供する。

なお、ロボットスライダ３３およびカメラスライダ４２のＺ軸の傾き角度の推定と、カメラ回転体４３による撮像手段４１の回転の半径と回転軸の傾き角度の推定の順序は、どちらが先でも良く、また、必ずしもすべての付加軸について推定を行わず、設計値を使うことで十分自律作業が可能な場合は、設計値を用いてもよい。また、回転の半径についても推定を行わず設計値を用いてもよい。

（初期校正実行部１）
図５は、実施例の初期校正実行部による初期校正実行工程のフローチャートである。

初期校正実行部１は、初期位置におけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータ２を推定する初期校正実行工程を行う。

本実施例における付加軸の初期位置として、ロボットスライダ３３はロボット３１が容器２３にアプローチ可能な位置、カメラスライダ４２は撮像手段４１が容器２３の上面高さを３次元計測可能な高さ、カメラ回転体４３は回転角度が０度であり撮像手段４１が容器２３の中央を見る位置にそれぞれ設定するものとして説明する。

初期校正実行部１で行う初期校正実行工程の作業の手順を説明すると、事前準備として、（Ｓ１）ロボット３１にマーカ３５を設置し、（Ｓ２）付加軸を制御してロボット３１と撮像手段４１を初期位置に移動し、（Ｓ３）以降の作業で付加軸が動かないよう付加軸を固定（ロック）する。次に、校正用のデータ収集として、（Ｓ４）ロボット３１の姿勢を変更することで撮像手段４１に対するマーカ３５の相対的な位置と姿勢を変更し、（Ｓ５）撮像手段４１によりマーカ３５を撮影し、（Ｓ６）撮影時のロボット３１の姿勢と画像のデータの組を記憶し、（Ｓ７）記憶域に所定の数のデータの組が格納されているかを判定し、格納されていない場合には（Ｓ４）に戻りデータの収集を継続する。格納されている場合には、（Ｓ８）記憶した複数のロボット３１の姿勢と画像のデータの組から、初期位置におけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータ（初期位置姿勢変換パラメータ２）を推定する。

なお、ロボット３１自体にマーカ３５を常に設置している場合は（Ｓ１）を省略してもよい。（Ｓ２）における初期位置は、事前に登録しておいてもよいし、以降のデータ収集および付加軸の校正の作業が容易になるよう、環境情報を用いて決定してもよい。例えば、環境情報としてＣＡＤデータを用いて障害物がなさそうな場所を推定して初期位置を決定するなどである。（Ｓ４）にてロボット３１が変更する姿勢の集合は、事前にデータベースに登録しておいてもよいし、データ収集が容易になる姿勢の集合を、環境情報をもとに決定してもよいし、作業者がロボット３１を制御するティーチングペンダントなどのコントローラを用いて決定してもよい。また、初回のステップではロボット３１の姿勢を変更せず、初期姿勢のまま撮影してもよい。また、（Ｓ５）の各ステップでは撮影した画像からマーカ３５を検出可能か否かの判定を行い、検出可能な場合にのみ（Ｓ６）にてロボット３１の姿勢と画像のデータの組を記憶域に保存することで、（Ｓ８）の推定に使用できるデータの組の数を担保してもよい。以降は、（Ｓ８）の処理について数式を用いて詳しく説明する。

（Ｓ８）における推定の原理について説明する。初期位置におけるロボット座標系Ｃ１からカメラ座標系Ｃ６への変換パラメータをＺ、ロボット先端座標系Ｃ４からマーカ座標系Ｃ５への変換パラメータをＸとする。また、（Ｓ６）にて記憶したＮ個のロボット先端座標系Ｃ４からロボット座標系Ｃ１への変換パラメータをＢｉ（ｉ＝１，２，…，Ｎ）とし、そのときの撮影画像から推定したマーカ座標系Ｃ５からカメラ座標系Ｃ６への変換パラメータをＡｉ（ｉ＝１，２，…，Ｎ）とする。この時、各ＡｉおよびＢｉの組は、
ＺＢｉ＝ＡｉＸ …（１）
という拘束条件を満たす。ただし、実際には観測データ（ロボット３１のエンコーダ値や撮影した画像から推定したマーカ３５の位置と姿勢など）の誤差により厳密に式（１）は成立しないため、すべての観測データの組に対して尤もらしく式（１）を満たす変換パラメータＺを求める手法を一般的にハンドアイキャリブレーションと呼び、最小二乗法などの手法を用いて、最終的な変換パラメータＺを推定する。このとき、Ｂｉはロボット３１のエンコーダ値に基づいて既知にでき、Ａｉは撮影された画像に基づいて既知にでき、ＸとＺは未知としてＺを推定する。この変換パラメータＺが初期位置姿勢変換パラメータ２である。

なお、初期校正実行部１は、撮像手段４１により撮影したマーカ３５の可視画像または距離画像に基づいて撮像手段４１に対するマーカ３５の相対的な位置と姿勢を推定するマーカ位置姿勢推定部を有しており、初期校正実行部１は、この推定結果に基づいてＡｉを推定できる。

（付加軸傾き角度推定部３）
付加軸傾き角度推定部３では、各付加軸の傾き角度の校正を行う。付加軸傾き角度推定部３は、校正対象の付加軸を１つずつ選択し、校正対象の付加軸を２地点以上に移動させ、各地点で撮像手段４１により撮影した画像のデータ群に基づき、校正対象の付加軸の傾き角度のパラメータである付加軸傾き角パラメータ４を推定する付加軸傾き角度推定工程を行う。本実施例では、図６を用いてロボットスライダ３３、図７を用いてカメラスライダ４２、図８を用いてカメラ回転体４３における校正の方法を説明する。

なお、付加軸傾き角度推定部３は、初期校正実行部１と同様に、撮像手段４１により撮影したマーカ３５の可視画像または距離画像に基づいて撮像手段４１に対するマーカ３５の相対的な位置と姿勢を推定するマーカ位置姿勢推定部を有している。なお、このマーカ位置姿勢推定部の機能は初期校正実行部１と同様であるため、初期校正実行部１と付加軸傾き角度推定部３とでマーカ位置姿勢推定部を共有する構成としてもよい。

そして、付加軸傾き角度推定部３は、各地点で撮影された画像に基づいてマーカ位置姿勢推定部により推定されたマーカ３５の位置の変化に基づいて、校正対象の付加軸における付加軸傾き角パラメータ４を推定する。

（付加軸傾き角度推定部３：ロボットスライダ３３の校正）
図６は、実施例の付加軸傾き角度推定部によるロボットスライダの校正の例を示す図である。図６では、ロボットスライダ３３を校正するフローチャートと、対応する上面図を示している。図の見やすさのため、作業台２１の記載は省略している。以降の作業では、カメラスライダ４２およびカメラ回転体４３の位置と姿勢は初期校正実行部１による初期校正実行時と同じとし、動かないように固定しておく。

付加軸傾き角度推定部３は、校正対象の付加軸が並進移動を行うスライダである場合に、２地点で撮影されたマーカ３５の画像に基づいてマーカ位置姿勢推定部により推定されたマーカの２地点の位置に基づいて、スライダの軸の傾き角度を付加軸傾き角パラメータ４として推定する。

校正の手順は、（Ｓ１１）ロボットスライダ３３を制御しロボット３１を地点Ａに移動し、（Ｓ１２）ロボット３１の姿勢を制御しマーカ３５を撮像手段４１に向けた後、以降の工程でロボット３１の姿勢が動かないように姿勢を固定（ロック）し、（Ｓ１３）撮像手段４１でマーカ３５を撮影し、撮影した画像により撮像手段４１に対するマーカ３５の相対的な位置Ｐ１を推定し、（Ｓ１４）ロボットスライダ３３を制御しロボット３１を地点Ｂに移動し、（Ｓ１５）撮像手段４１でマーカ３５を撮影し、撮影した画像により撮像手段４１に対するマーカ３５の相対的な位置Ｐ２を推定する。最後に、（Ｓ１６）位置Ｐ１と位置Ｐ２に基づいてロボットスライダ３３の付加軸傾き角パラメータ４を推定する。具体的には、位置Ｐ１と位置Ｐ２に基づいてカメラ座標系Ｃ６おけるマーカ３５の移動の軌跡である直線の傾きを計算する。２回の撮影の間にロボット３１の姿勢が変動していないので、この直線の傾きは、カメラ座標系Ｃ６に対するロボットスライダ座標系Ｃ３の傾きに等しい。したがって、この直線の傾きから、カメラ座標系Ｃ６に対するロボットスライダ３３の姿勢のパラメータ、すなわち、付加軸傾き角パラメータ４が求まる。

ここで、（Ｓ１６）ではカメラ座標系Ｃ６におけるロボットスライダ３３の付加軸傾き角パラメータ４を求めて格納する例を示したが、これに限られず、初期校正実行部１で推定した初期位置姿勢変換パラメータ２を用いてカメラ座標系Ｃ６におけるロボットスライダ３３の付加軸傾き角パラメータ４をロボット座標系Ｃ１におけるロボットスライダ３３の付加軸傾き角パラメータ４に変換して格納してもよいし、両者を格納するようにしてもよい。

なお、地点Ａと地点Ｂにおける撮影時のロボット３１の姿勢は、撮像手段４１からマーカ３５を高精度に計測可能な姿勢である必要がある。これらのロボット３１の姿勢は、事前に定めておいてもよいし、撮像手段４１の画角や環境情報から自動で決定してもよい。特に、位置Ｐ１と位置Ｐ２の２点の位置関係から直線の傾きを推定する際は、２点間の距離が離れている方が推定時の誤差が小さくなる。そこで、画像の左右、もしくは上下など離れた位置にマーカ３５が映るよう、２点の位置を定めることで、高精度に付加軸傾き角パラメータ４を推定することができる。また、撮像手段４１の視野周辺部に画像歪みがある場合は、事前に画像歪みが大きい領域を避けて設定した視野中央領域の範囲を定義し、設定した視野中央領域の範囲内で、互いに離れた位置、例えば、互いに異なる辺に近い位置となるようにそれぞれマーカ３５を撮影した画像を用いることで、画像歪みが大きい撮像手段でも高精度に付加軸傾き角パラメータ４を推定できる。これらの手法は、付加軸が、撮像手段４１の光軸に直交する方向に移動するスライダまたは撮像手段４１の光軸に沿った軸を有する回転軸である場合に特に有効であるが、このような付加軸に限られずに適用してもよい。また、撮像手段４１からマーカ３５を高精度に計測可能となるよう、例えば焦点距離付近にマーカ３５が映るようロボット３１の姿勢を決定することも有効である。

また、地点Ａと地点Ｂのうちの一方、例えば、地点Ａとして、初期校正実行部１で用いた初期位置を用い、地点Ａにおいて撮像するデータを、初期校正実行部１にて収集したデータで代用することで、初期校正実行部１と付加軸傾き角度推定部３におけるトータルの撮影回数を減らしてもよい。

また、位置Ｐ１と位置Ｐ２の２点に加えて、追加で地点Ａと地点Ｂとは異なる地点で追加の撮影を行い、３点以上の位置のデータを用いて直線の傾きを求めることで、より高精度にロボットスライダ３３の付加軸傾き角パラメータ４を推定してもよい。ただし、その場合は撮影回数が増加するため、キャリブレーションにかかる工数が増えてしまうというデメリットもある。

以上により、ロボットスライダ３３の付加軸傾き角パラメータ４を推定できる。

（付加軸傾き角度推定部３：カメラスライダ４２の校正）
図７は、実施例の付加軸傾き角度推定部によるカメラスライダの校正の例を示す図である。図７では、カメラスライダ４２を校正するフローチャートと、対応する斜視図を示している。図の見やすさのため、作業台２１の記載は省略している。以降の作業では、ロボットスライダ３３およびカメラ回転体４３の位置と姿勢は初期校正実行部１による初期校正実行時と同じとし、動かないように固定しておく。

以降の説明では、ロボットスライダ３３の校正からの違いを中心に説明し、重複する説明は省略する。

校正の手順の（Ｓ２１）から（Ｓ２６）は、（Ｓ１１）から（Ｓ１６）に対応する。なお、校正対象の付加軸であるロボットスライダ３３はカメラスライダ４２に変更されている。また、地点Ａは地点Ｃに、地点Ｂは地点Ｄに、位置Ｐ１は位置Ｐ３に、位置Ｐ２は位置Ｐ４に変更されている。最終的に、（Ｓ２６）においてカメラスライダ４２の付加軸傾き角パラメータ４を推定する。

なお、直線の傾きを高精度に推定するためには、カメラスライダ４２の移動量は大きい方が望ましいが、撮像手段４１のピントが合わない距離でマーカ３５を撮影し位置を求めた場合は誤差が大きくなる可能性がある。そこで、撮像手段４１の合焦距離の範囲内の手前と奥のそれぞれにおいてマーカ３５を撮影した画像に基づいて、付加軸傾き角パラメータ４を推定することが望ましい。また、画像の視野中央部にマーカ３５を映すことで、画像の歪みの影響を低減することが望ましい。これらの手法は、付加軸が、撮像手段４１の光軸に沿った方向に移動するスライダまたは撮像手段４１の光軸に直交する軸を有する回転軸である場合に特に有効であるが、このような付加軸に限られずに適用してもよい。

また、撮像手段４１を移動させる付加軸の場合は、ロボット３１に設置したマーカ３５用いる以外の方法もある。例えば、作業台２１上に設置した形状が既知のマーカ３５を用いる方法もある。具体的には、（Ｓ２２）ではロボット３１は撮像手段４１の画角から外れるよう姿勢を変更し、（Ｓ２３）および（Ｓ２５）では作業台２１上に設置したマーカ３５を撮影し、（Ｓ２６）でその位置Ｐ３と位置Ｐ４に基づいてカメラスライダ４２の付加軸傾き角パラメータ４を推定することもできる。この方法は、作業台２１とロボット３１とが接触しやすいような環境において特に有効である。この時、マーカ３５としては、ロボット３１に設置したものを取り外して作業台２１上に設置してもよいし、別のマーカ３５を用意して設置してもよい。

以上により、カメラスライダ４２の付加軸傾き角パラメータ４を推定できる。

（付加軸傾き角度推定部３：カメラ回転体４３の校正）
図８は、実施例の付加軸傾き角度推定部によるカメラ回転体の校正の例を示す図である。図８では、カメラ回転体４３を校正するフローチャートと、対応する斜視図を示している。図の見やすさのため、作業台２１の記載は省略している。以降の作業では、ロボットスライダ３３およびカメラスライダ４２の位置と姿勢は初期校正実行部１による初期校正実行時と同じとし、動かないように固定しておく。

付加軸傾き角度推定部３は、校正対象の付加軸が撮像手段４１を移動させる回転軸である場合に、３地点で撮影されたマーカ３５の画像に基づいてマーカ位置姿勢推定部により推定されたマーカの３地点の位置と、回転軸の回転角度とに基づいて、回転軸から撮像手段４１までの半径（リンクの長さ）を推定するとともに、回転軸の傾き角度を付加軸傾き角パラメータ４として推定する。

以降の説明では、ロボットスライダ３３の校正からの違いを中心に説明し、重複する説明は省略する。

回転軸の校正の場合には、３地点で撮影を行うため、校正の手順の（Ｓ３１）から（Ｓ３５）は、（Ｓ１１）から（Ｓ１５）に対応するが、新たに（Ｓ３６）（Ｓ３７）が追加されており、また、（Ｓ１６）に代えて（Ｓ３８）を行っている。校正対象の付加軸であるロボットスライダ３３はカメラ回転体４３に変更されている。また、地点Ａは地点Ｅに、地点Ｂは地点Ｆに、位置Ｐ１は位置Ｐ５に、位置Ｐ２は位置Ｐ６に変更されている。さらに、（Ｓ３６）でカメラ回転体４３を制御し、撮像手段４１を地点Ｇに移動し、（Ｓ３７）で撮像手段４１でマーカ３５を撮影し、マーカ３５の相対的な位置Ｐ７を推定する。最後に、（Ｓ３８）において、位置Ｐ５～位置Ｐ７と回転角度に基づいて半径とカメラ回転体４３の付加軸傾き角パラメータ４を推定する。

（Ｓ３８）における具体的な推定方法には様々な方法が考えられ、以下に例示するが、これに限られるものではない。一例としては、位置Ｐ５、位置Ｐ６、位置Ｐ７とカメラ回転体４３のエンコーダ値から得た回転角度とに基づいて、カメラ座標系Ｃ６おけるマーカ３５の移動の軌跡である円弧の中心と半径と傾きを計算する。３回の撮影の間にロボット３１の姿勢とリンクの長さが変動していないので、この円弧の半径は回転軸から撮像手段４１までの半径（リンクの長さ）に等しく、円弧の傾きは、カメラ座標系Ｃ６に対するカメラ回転体座標系Ｃ７の傾きに等しい。したがって、この円弧の半径と傾きから、回転軸から撮像手段４１までの半径（リンクの長さ）と、カメラ座標系Ｃ６に対するカメラ回転体４３の姿勢のパラメータ、すなわち、付加軸傾き角パラメータ４が求まる。

また、例えば、リンクの長さは回転体の移動前後でも不変であるため、３次元空間上で位置Ｐ５、位置Ｐ６、位置Ｐ７のうちの２点を結ぶ線の長さを底辺とし、その２点間の回転角度を頂角とする２等辺三角形が少なくとも１つ計算できるので、その２等辺三角形の底辺ではない辺の長さがリンクの長さに相当するので、半径が求まる。また、３次元空間上で位置Ｐ５、位置Ｐ６、位置Ｐ７を用いて２つのベクトルを求め、２つのベクトルが作る平面の法線方向が円弧の傾きに相当するので、付加軸傾き角パラメータ４が求まる。

高精度に付加軸傾き角パラメータ４を推定するために、撮像手段４１の視野周辺部に存在する画像歪みの領域を避けることや、合焦距離の範囲内で撮影することや、なるべく離れた位置となるように撮影することについては、ロボットスライダ３３やカメラスライダ４２の場合と同様である。また、カメラスライダ４２の場合と同様に、作業台２１上に設置したマーカ３５を撮影するようにしてもよい。また、４点以上の位置のデータを用いてより高精度に付加軸傾き角パラメータ４を推定してもよい。

以上により、カメラ回転体４３の付加軸傾き角パラメータ４を推定できる。

（自律作業司令部５から自律作業実行部１１まで）
次に、初期校正実行部１により推定した初期位置姿勢変換パラメータ２と、付加軸傾き角度推定部３により推定した付加軸傾き角パラメータ４を用いて、自律作業司令部５により指示した作業を自律作業実行部１１で実際に実行する手順を説明する。本実施例では、単一種類のワークが複数格納された容器２３および容器２４から、ワークを５つずつピックし、容器２２内にプレイスする作業を想定する。

はじめに、自律作業司令部５により、容器２３、容器２４の順でワークを５つずつピックし、容器２２へプレイスするよう指示が生成される。この時、各付加軸は初期位置にあるため、事前に推定した初期位置姿勢変換パラメータ２を、現在位置姿勢変換パラメータ１０として登録する。

次に、付加軸位置計画部６により、ワークの高精度な計測が可能なカメラスライダ４２およびカメラ回転体４３の位置、並びに、ワークをピックして持ち上げる動作の軌道計画が可能なロボットスライダ３３の位置を計画する。

具体的には、まず容器の種類に基づき撮影の基準位置となるカメラスライダ４２の位置およびカメラ回転体４３の角度を決定する。本実施例では、初期校正実行部１で用いた初期位置として、容器２３内にロボット３１がアプローチ可能であり、かつ撮像手段４１が容器２３の上面中央を向くような初期位置を生成しているため、容器２３のワークがピック対象である場合は初期位置を基準位置とする。容器２４については別途、カメラ回転体４３により容器２４が画角中心付近に映る位置を事前に定義して基準位置として登録しておく。そして、基準位置から撮像手段４１を用いて容器２３内のワークを計測し、容器２３内に高精度な３次元計測および認識が可能なワークの有無を判定する。

例えば、容器２３内にワークが多く格納され容器２３の上面までワークがある場合は、初期位置における撮像手段４１の計測精度が許容誤差内かつロボット３１もピックの軌道が生成可能であり、付加軸が初期位置でピック作業ができるため、付加軸の移動は不要と計画する。一方、容器２３の底にピック対象のワークがある場合は、撮像手段４１の距離が遠く計測精度が許容誤差範囲外のため移動要と判定し、この場合はカメラスライダ４２をワークに対して、事前に定めた撮像手段４１が高精度に計測可能な範囲内となる高さに撮像手段４１を移動するよう計画する。また、ロボット３１の軌道計画の可否についても考慮し、例としてワークが容器２３の角にあり、初期姿勢ではピック動作が困難であると判定した場合には、ロボットスライダ３３の位置を横にずらすことで、軌道計画が可能となる付加軸の位置を計画する。

次に、付加軸移動制御部７は、付加軸位置計画部６において付加軸の移動が要と判定された場合は、付加軸を計画された付加軸の位置へ移動させる。

次に、付加軸移動量取得部８は、付加軸位置計画部６において付加軸の移動が要と判定された場合には、移動した付加軸の移動量を取得し、相対位置姿勢推定部９に入力する。なお、この時に取得する移動量は、付加軸位置計画部６において計画した指令値でも、付加軸から取得したエンコーダ値でも、いずれでも良い。付加軸位置計画部６において付加軸の移動が不要と判定された場合には、そのまま自律作業実行部１１へ遷移する。

相対位置姿勢推定部９では、事前に登録した初期位置姿勢変換パラメータ２および付加軸傾き角パラメータ４と、付加軸移動量取得部８から取得した付加軸の移動量とに基づいて、付加軸の移動後である現在の付加軸の位置の組合せにおけるロボット３１と撮像手段４１との間の相対的な位置と姿勢の変換パラメータである現在位置姿勢変換パラメータ１０を推定する。

現在のロボット座標系Ｃ１とカメラ座標系Ｃ６との間の相対的な位置と姿勢の変換パラメータｒｃＴｃｃ（現在位置姿勢変換パラメータ１０）は、現在位置のロボット３１から初期位置のロボット３１への相対的な位置と姿勢の変換パラメータｒｃＴｒｓと、初期位置におけるロボット３１から撮像手段４１への相対的な位置と姿勢の変換パラメータｒｓＴｃｓ（初期位置姿勢変換パラメータ２）と、初期位置のカメラ位置から現在のカメラ位置への相対的な位置と姿勢の変換パラメータｃｓＴｃｃとを用いて、次の式（２）の通り表現でき、
ｒｃＴｃｃ＝ｒｃＴｒｓ×ｒｓＴｃｓ×ｃｓＴｃｃ …（２）
現在位置姿勢変換パラメータ１０は式（２）により計算できる。

ここで、現在位置のロボット３１から初期位置のロボット３１への相対的な位置と姿勢の変換パラメータｒｃＴｒｓは、ロボットスライダ３３の移動量およびロボットスライダ３３の付加軸傾き角パラメータ４により求まるので既知とできる。また、初期位置のカメラ位置から現在のカメラ位置への相対的な位置と姿勢の変換パラメータｃｓＴｃｃも同様に、カメラスライダ４２およびカメラ回転体４３の移動量、カメラスライダ４２およびカメラ回転体４３の付加軸傾き角パラメータ４、並びに、回転の半径により求まるので既知とできる。さらに、初期位置におけるロボット３１から撮像手段４１への相対的な位置と姿勢の変換パラメータｒｓＴｃｓは、初期位置姿勢変換パラメータ２であるから、既知である。したがって、相対位置姿勢推定部９は、式（２）により現在位置姿勢変換パラメータ１０を推定できる。なお、付加軸傾き角パラメータ４がカメラ座標系Ｃ６を基準に表現されている場合は、必要に応じて初期位置姿勢変換パラメータ２を使ってロボット座標系Ｃ１を基準にするよう変換する。

自律作業実行部１１では、撮像手段４１により撮像した画像に基づいて、カメラ座標系Ｃ６におけるワークの位置と姿勢のパラメータＰｃを推定する。続いて、格納された現在位置姿勢変換パラメータ１０、すなわち、現在のロボット座標系Ｃ１とカメラ座標系Ｃ６との間の相対的な位置と姿勢の変換パラメータｒｃＴｃｃを用いて、ロボット座標系Ｃ１におけるワークの位置と姿勢のパラメータＰｒを、
Ｐｒ＝ｒｃＴｃｃ×Ｐｃ …（３）
式（３）に基づき計算する。最後に、パラメータＰｒを用いて軌道計画を行い、ロボット３１でワークをピックし、容器２２にプレイスする。この時、撮像手段４１による撮像データは、撮像手段４１を移動させる付加軸の位置が付加軸位置計画部６および付加軸移動制御部７により変更されていない場合は、付加軸位置計画部６で撮影したデータを用いることで、撮影に必要なタクトタイムを短縮することが可能である。

なお、パラメータＰｒを一度求めた後であれば、ピックアンドプレイスの実行時に、ロボット３１と撮像手段４１との間の相対的な位置と姿勢が既知である必要はない。そのため、例えばピック動作の際に撮像手段４１のカメラ回転体４３を用いて撮像手段４１をロボット３１の動作範囲外に移動させておくことで、軌道計画を容易にしてもよい。また、プレイス動作においても、ロボットスライダ３３を移動させ容器２２にプレイスしやすい位置にロボット３１を移動させるよう軌道計画してもよい。

以上により１つのワークをピッキングした後、再び付加軸位置計画部６に戻る。付加軸位置計画部６では再び容器内のワークの撮像を行うが、前回のピッキング時と同一の容器からワークをピッキングする場合には、前回ピッキング作業を行った際にワークを認識した時の撮像手段４１の位置を初期位置とするよう各付加軸の位置を決定しておくことで、付加軸の移動が不要なケースでは、付加軸位置計画部６において撮影したデータにより自律作業実行部１１による自律作業が可能となり、付加軸の位置の計画と自律作業の実行向けに２度の撮影を行う頻度が低減するため、タクトタイム短縮に有効である。

以上のループを繰り返す中で、ロボット３１と撮像手段４１との間の相対的な配置は動的に変動するが、事前に初期位置姿勢変換パラメータ２と、各付加軸の付加軸傾き角パラメータ４とを推定しておくことで、自律作業の実行中に付加軸の位置が変動した場合であっても、初期位置姿勢変換パラメータ２と付加軸傾き角パラメータ４と付加軸の移動量を用いて現在の付加軸の位置におけるロボット３１と撮像手段４１との間の相対的な位置と姿勢を既知とすることができ、自律作業を実行することができる。

以上、本発明の実施例を説明したが、本発明は実施例に記載された構成に限定されず、本発明の技術的思想の範囲内で種々の変更が可能である。また、実施例内で説明した変形例の構成の一部または全部を組み合わせて適用してもよい。

１ 初期校正実行部
２ 初期位置姿勢変換パラメータ
３ 付加軸傾き角度推定部
４ 付加軸傾き角パラメータ
５ 自律作業司令部
６ 付加軸位置計画部
７ 付加軸移動制御部
８ 付加軸移動量取得部
９ 相対位置姿勢推定部
１０ 現在位置姿勢変換パラメータ
１１ 自律作業実行部
２１ 作業台
２２ 容器
２３ 容器
２４ 容器
３１ ロボット
３２ ハンド
３３ ロボットスライダ
３４ ロボット支柱
３５ マーカ
４１ 撮像手段
４２ カメラスライダ
４３ カメラ回転体
４４ カメラ支柱
Ｃ１ ロボット座標系
Ｃ２ ロボット支柱座標系
Ｃ３ ロボットスライダ座標系
Ｃ４ ロボット先端座標系
Ｃ５ マーカ座標系
Ｃ６ カメラ座標系
Ｃ７ カメラ回転体座標系
Ｃ８ カメラスライダ座標系

Claims (8)

1. ロボットと、
   前記ロボットの周辺に設置され前記ロボットの可動域の少なくとも一部を視野に収めて撮影する撮像手段と、
   前記ロボットおよび／または前記撮像手段を移動させる１つ以上の付加軸と、を有する自律ロボットであって、
   前記付加軸の位置を移動させる付加軸移動制御部と、
   前記付加軸の位置の移動前後における移動量を取得する付加軸移動量取得部と、
   初期位置における前記ロボットと前記撮像手段との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータを推定する初期校正実行部と、
   １つずつ選択された校正対象の前記付加軸を前記付加軸移動制御部により２地点以上に移動させて各地点で前記撮像手段により撮影した画像のデータ群に基づき、前記校正対象の前記付加軸の傾き角度のパラメータである付加軸傾き角パラメータを推定する付加軸傾き角度推定部と、
   前記初期位置姿勢変換パラメータと前記付加軸傾き角パラメータと前記付加軸の移動量とに基づいて任意の前記付加軸の位置の組合せにおける前記ロボットと前記撮像手段との間の相対的な位置と姿勢の変換パラメータを推定する相対位置姿勢推定部と、
   前記相対位置姿勢推定部で推定した変換パラメータを用いて任意の前記付加軸の位置の組合せにおいて前記ロボットと前記撮像手段との間の相対的な位置と姿勢を既知として自律作業を実行する自律作業実行部と、を有することを特徴とする自律ロボット。
2. 請求項１において、
   前記付加軸傾き角度推定部は、
   前記撮像手段により撮影したマーカの可視画像または距離画像に基づいて前記撮像手段に対する前記マーカの相対的な位置と姿勢を推定するマーカ位置姿勢推定部を有し、
   各地点で撮影された画像に基づいて前記マーカ位置姿勢推定部により推定された前記マーカの位置の変化に基づいて、前記校正対象の前記付加軸における前記付加軸傾き角パラメータを推定することを特徴とする自律ロボット。
3. 請求項２において、
   前記付加軸傾き角度推定部は、前記校正対象の前記付加軸が並進移動を行うスライダである場合に、２地点で撮影された前記マーカの画像に基づいて前記マーカ位置姿勢推定部により推定された前記マーカの２地点の位置に基づいて、前記スライダの軸の傾き角度を前記付加軸傾き角パラメータとして推定することを特徴とする自律ロボット。
4. 請求項２おいて、
   前記付加軸傾き角度推定部は、前記校正対象の前記付加軸が前記撮像手段を移動させる回転軸である場合に、３地点で撮影された前記マーカの画像に基づいて前記マーカ位置姿勢推定部により推定された前記マーカの３地点の位置と、前記回転軸の回転角度とに基づいて、前記回転軸から前記撮像手段までの半径を推定するとともに、前記回転軸の傾き角度を前記付加軸傾き角パラメータとして推定することを特徴とする自律ロボット。
5. 請求項２において、
   前記付加軸傾き角度推定部は、前記画像の視野周辺部に存在する画像歪みが大きい領域を避けて設定した視野中央領域の範囲内で、互いに異なる辺に近い位置となるようにそれぞれ前記マーカを撮影した前記画像に基づいて、前記付加軸傾き角パラメータを推定することを特徴とする自律ロボット。
6. 請求項２において、
   前記付加軸傾き角度推定部は、前記撮像手段の合焦距離の範囲内の手前と奥のそれぞれにおいて前記マーカを撮影した前記画像に基づいて、前記付加軸傾き角パラメータを推定することを特徴とする自律ロボット。
7. 請求項１において、
   前記付加軸傾き角度推定部は、前記２地点以上のうちの１地点として、前記初期校正実行部で用いた前記初期位置を用いることを特徴とする自律ロボット。
8. ロボットと、前記ロボットの周辺に設置され前記ロボットの可動域の少なくとも一部を視野に収めて撮影する撮像手段と、前記ロボットおよび／または前記撮像手段を移動させる１つ以上の付加軸と、を有する自律ロボットのキャリブレーション方法であって、
   初期位置における前記ロボットと前記撮像手段との間の相対的な位置と姿勢の変換パラメータである初期位置姿勢変換パラメータを推定する初期校正実行工程と、
   校正対象の前記付加軸を１つずつ選択し、前記校正対象の前記付加軸を２地点以上に移動させ、各地点で前記撮像手段により撮影した画像のデータ群に基づき、前記校正対象の前記付加軸の傾き角度のパラメータである付加軸傾き角パラメータを推定する付加軸傾き角度推定工程と、を有することを特徴とする自律ロボットのキャリブレーション方法。

Images