JP7323993B2 - 制御装置、ロボットシステム、制御装置の動作方法及びプログラム - Google Patents

Description

本発明は、制御装置、ロボットシステム、制御装置の動作方法及びプログラムに関し、特に、画像に基づいてロボットを制御するビジュアルサーボ技術に関する。

従来、カメラ画像に基づいてロボットの制御を行う手法の一つとして、ビジュアルサーボが知られている。ビジュアルサーボとは、所望の位置に対象物体が写るようにロボットをフィードバック制御する技術である。ビジュアルサーボを用いると、ロボットとカメラとの間やロボットとエンドエフェクタとの間の位置姿勢関係を精度よく求めておく必要がなく、それらの位置関係を求めるキャリブレーション作業の負荷を低減することができる。中でも、目標位置へ誘導する対象や目標位置にある対象の特徴を画像から抽出し、それらの位置の差に基づいてロボットを制御する手法は、誘導する対象や目標位置にある対象の位置変化に対応できる利点がある。

特許文献１では、組付け対象物と被組付け対象物とを撮像した画像から特徴量検出を行い、検出結果に基づいて組付け作業をすることで、被組付け対象物が位置ずれしている場合でも組付け作業を実現することが開示されている。

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

画像特徴を使ったビジュアルサーボを実現するためには、目標位置へ誘導する対象と目標位置にある対象とを同じ画像中に撮像すると共に、それぞれ複数の特徴を抽出することで位置姿勢を特定できるようにする必要がある。しかしながら、物体の形状によっては少ない特徴しか抽出することができないことがある。また、カメラの配置によっても物体の一部しか見えないため特徴を十分に抽出できないことがある。

本発明は、上記の課題に鑑みてなされたものであり、画像から十分に特徴を抽出できない場合においてもビジュアルサーボを実現して高精度にタスクを実行可能にするための技術を提供することを目的とする。

上記の目的を達成する本発明に係る制御装置は、
ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標位置であって凹部にある目標の位置である前記目標位置を撮像した画像を取得する画像取得手段と、
前記画像における前記制御対象のエッジに基づいて、前記制御対象の回転軸の進行方向であって前記制御対象の回転軸に沿っている前記進行方向を表す該画像中の２次元方向を取得する進行方向取得手段と、
前記画像から前記目標位置を表す該画像中の２次元位置を検出する目標位置検出手段と、
前記回転軸の進行方向の延長上に前記目標位置が存在し、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が前記目標位置に達する前に生成し、さらに姿勢を変更した制御対象の進行方向に沿って前記制御対象を前記目標位置に近づける動作を生成する動作生成手段と、
前記動作に従って前記制御対象を制御する制御手段と、
を備え、
前記動作生成手段は、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新することを特徴とする。

本発明によれば、画像から十分に特徴を抽出できない場合においてもビジュアルサーボを実現して高精度にタスクを実行することが可能となる。

第１の実施形態のロボットシステムの構成例を示す構成図。 第１の実施形態のロボットシステムの構成例を示すブロック図。 第１の実施形態のロボットシステムによる処理を説明するフローチャート。 第１の実施形態のロボットシステムの座標系を説明する図。 第１の実施形態の動作生成部を説明する図。 第１の実施形態の変形例１のロボットシステムによる処理を説明するフローチャート。 第１の実施形態の変形例２の動作生成部を説明する図。 第１の実施形態の変形例２の動作生成部を説明する図。 第１の実施形態の変形例３の動作表示部を説明する図。 第２の実施形態のロボットシステムの構成例を示すブロック図。 第２の実施形態のロボットシステムによる処理を説明するフローチャート。 第３の実施形態のロボットシステムの構成例を示すブロック図。 第３の実施形態のロボットシステムによる処理を説明するフローチャート。

以下、図面を参照しながら実施形態を説明する。なお、以下の実施形態において示す構成は一例に過ぎず、本発明は図示された構成に限定されるものではない。

（第１の実施形態）
第１の実施形態では、ロボットに取り付けた撮像装置で、制御対象と目標位置とを撮像し、ロボットにより制御対象を目標位置まで誘導する。本実施形態では、制御対象とはロボットに装着したエンドエフェクタであるドライバである。目標位置とはエンドエフェクタがタスクを実行する位置のことで、本実施形態では画像特徴１点の位置であるネジ中心を用いる。なお、目標位置は２点以上の画像特徴でもよい。ロボットによるタスクとして誘導後にエンドエフェクタであるドライバを用いてネジを取り外すことで、自動でネジを外すロボットシステムについても説明する。画像中のドライバとネジとの位置関係からロボットの動作を生成することで、ロボットと撮像装置とのキャリブレーションあるいは撮像装置とエンドエフェクタとのキャリブレーションに誤差がある場合でも、正確に制御できる。

［装置の構成］
図１の構成図および図２のブロック図により第１の実施形態の制御装置１００を備えるロボットシステム１０００の構成例を示す。ロボットシステム１０００は、撮像装置１と、ロボットアーム２と、エンドエフェクタ３と、制御装置１００とを備える。

撮像装置１は、エンドエフェクタ３に取り付けられ、エンドエフェクタ３および目標位置を含むシーンを撮像する装置である。たとえば、撮像装置１は２台のグレースケールカメラから構成され、制御部１０６からの撮像トリガにより撮像を行い、画像取得部１０１に画像信号を送る。

ロボットアーム２は、制御対象を移動させる装置である。たとえばロボットアーム２は６軸ロボットから構成され、制御装置１００により制御値を入力されて動作する。エンドエフェクタ３は、ロボットアームの先端に取り付けられた、目標を操作する装置である。たとえば、エンドエフェクタ３はドライバおよび近接センサから構成され、制御装置１００にドライバと目標との接触状態を入力する。また、エンドエフェクタ３は制御装置１００により制御値を入力されて動作する。

制御装置１００は、画像取得部１０１と、軸方向取得部１０２と、目標位置検出部１０３と、動作生成部１０４と、終了判断部１０５と、制御部１０６とを備える。

画像取得部１０１は、撮像装置１から画像信号を受け取り、軸方向取得部１０２及び目標位置検出部１０３に画像データを送る。たとえば画像取得部１０１はメモリから構成される。

軸方向取得部１０２は、画像取得部１０１から画像データを受け取り、図示しない軸方向設定部により設定された制御対象の軸方向を取得し、軸方向データを動作生成部１０４及び終了判断部１０５に送る。ここで、軸とは例えば回転軸であり、軸方向とは、制御対象と目標位置との位置姿勢関係を測るための制御対象に関連付けられた画像中の線分のことで、たとえばエンドエフェクタ３であるドライバの中心軸を、軸上の２次元点と、根本から先端に向かう２次元ベクトルとで表す。なお、ベクトルは逆に先端から根本に向かう方向でもよい。軸方向を設定することで、エンドエフェクタ３に取り付けられた撮像装置１ではエンドエフェクタ３を部分的にしか撮像できず、十分にエンドエフェクタ３の特徴を検出できない状況であっても、動作生成部１０４がロボットアーム２の先端部の動作を規定する位置姿勢を計算することができる。

目標位置検出部１０３は、画像取得部１０１から画像データを受け取り、画像データから目標位置を検出し、目標位置データを動作生成部１０４及び終了判断部１０５に送る。動作生成部１０４は、軸方向取得部１０２及び目標位置検出部１０３から受け取った軸方向データ及び目標位置データに基づいて、ロボットアーム２の先端部の動作を規定する位置姿勢を計算し、位置姿勢データを制御部１０６に送る。まずは、回転軸の延長上に目標位置が存在するようにロボットアーム２の先端部を移動させ、その後、軸方向に沿ってロボットアーム２の先端部を目標位置に到達させる。

終了判断部１０５は、軸方向取得部１０２及び目標位置検出部１０３から受け取った軸方向データ及び目標位置データに基づいて、制御対象が目標位置に到達したかを判断し、終了判断データを制御部１０６に送る。

制御部１０６は、動作生成部１０４から受け取った位置姿勢データに基づいてロボットアーム２を制御する。また、制御部１０６は、終了判断部１０５から受け取った終了判断データに基づいて、制御ループを続ける場合は撮像装置１に撮像トリガを送り、制御ループを終了する場合はタスク動作に基づいてロボットアーム２およびエンドエフェクタ３を制御する。

［制御処理］
図３のフローチャートおよび図４の座標系の定義により第１の実施形態の制御装置１００およびロボットシステム１０００による制御処理方法を説明する。図４の４１はロボットアーム２の基準であるロボット座標系、４２はロボットアーム２の先端部であるフランジ座標系、４３は撮像装置１の基準である撮像装置座標系、４４はエンドエフェクタ３の先端部であるエンドエフェクタ座標系である。Qはロボット座標系４１におけるフランジ座標系４２（フランジ位置姿勢）、Gはフランジ座標系４２から撮像装置座標系４３への変換行列、Hは撮像装置座標系４３からエンドエフェクタ座標系４４への変換行列を表す。

（ステップＳ１０１）
ステップＳ１０１では、制御部１０６が、ロボットアーム２を初期位置姿勢に移動するよう制御する。本実施形態では、制御部１０６がティーチングペンダントによる教示作業により設定した位置姿勢を初期位置姿勢として取得する。また、動作生成部１０４が変換行列Gおよび変換行列Hを取得する。なお、変換行列Gおよび変換行列Hは、ユーザがあらかじめ設計値やキャリブレーションにより求めておくが、誤差を含んでいてもかまわない。ロボットアーム２が初期位置姿勢に移動したら、制御部１０６が撮像装置１に撮像トリガを送る。

（ステップＳ１０２）
ステップＳ１０２では、撮像装置１が、制御部１０６から撮像トリガを受け取り、撮像を行い、撮像した画像信号を画像取得部１０１に送る。

（ステップＳ１０３）
ステップＳ１０３では、画像取得部１０１が、撮像装置１から画像信号を受け取り、軸方向取得部１０２及び目標位置検出部１０３に画像データを送る。

（ステップＳ１０４）
ステップＳ１０４では、軸方向取得部１０２が、画像取得部１０１から画像データを受け取り、制御対象の軸方向を取得し、取得した軸方向データを動作生成部１０４及び終了判断部１０５に送る。

本実施形態では、画像に写ったエンドエフェクタ３であるドライバの直線エッジのうち長い２本の線分を検出し、その中線を軸として、画像の下から上に向かう方向を正とする軸方向を取得する。軸方向は本実施形態のように画像から検出してもよいし、図示しない軸方向設定部によって事前に設定したデータを取得してもよい。軸方向設定部は、たとえばユーザが画像中から２点指定して２点を結ぶ線分を軸方向として設定してもよい。あるいは、撮像装置１とエンドエフェクタ３との３次元的な位置関係を計測した結果から画像にエンドエフェクタ３の軸方向を投影した値を用いて軸方向を設定してもよい。

（ステップＳ１０５）
ステップＳ１０５では、目標位置検出部１０３が、画像取得部１０１から画像データを受け取り、画像データに基づいて目標位置を検出し、検出した目標位置データを動作生成部１０４及び終了判断部１０５に送る。

本実施形態では、目標位置としてネジ中心位置を検出する。検出は事前に撮像したネジの画像をテンプレートとしたテンプレートマッチングで行ってもよいし、画像の輝度値に基づいてネジ領域を抽出しその重心を計算することで行ってもよい。

（ステップＳ１０６）
ステップＳ１０６では、動作生成部１０４が、軸方向取得部１０２及び目標位置検出部１０３から軸方向データおよび目標位置データを受け取り、ロボットアーム２の先端部の動作を規定する位置姿勢を計算する。また、動作生成部１０４は、計算した位置姿勢データを制御部１０６に送る。まずは、回転軸の延長上に目標位置が存在するようにロボットアーム２の先端部を移動させ、その後、軸方向に沿ってロボットアーム２の先端部を目標位置に到達させる。

以下、図５を用いて動作生成の一例を説明する。図５において、５１は撮像装置１であるカメラで撮像した画像、５２は画像に写ったエンドエフェクタ３であるドライバ、５３は画像に写ったネジ、５４は取得した軸方向、５５は検出した目標位置、５６は軸方向と目標位置との距離を表している。これらの情報から軸方向と目標位置とを近づけるフランジ制御量を計算する。具体的には、まず以下の式１～４で制御対象の制御量H'を解く。

ここで、式１は撮像装置１であるカメラとエンドエフェクタ３であるドライバとネジとの関係式である。P'は制御目標とする撮像装置座標系４３におけるネジ位置、H'はエンドエフェクタ３であるドライバの制御量、Pは制御目標とするエンドエフェクタ座標系４４におけるネジ位置である。式２はエンドエフェクタ３であるドライバの制御量H'の定義で、ここではエンドエフェクタ座標系４４のx軸方向、y軸方向に動かす場合である。dx、dyは制御量を表す。式３はネジ位置P'と画像との関係式で、u'およびv'はP'の画像中への投影位置、f_xおよびf_yは撮像装置１であるカメラの焦点距離、cxおよびcyは撮像装置１であるカメラの主点位置を表す。式４は求める画像ヤコビアンで、uおよびvは検出したネジ位置、u_dおよびv_dはネジ位置から軸へと降ろした垂線の足の位置である。これらを撮像装置１であるステレオカメラの各画像から求めることで、エンドエフェクタ３であるドライバの制御量H'が求まる。これを式５により現在のフランジ位置姿勢Qに反映する。

ここで、Q'は求めるフランジ位置姿勢、Qは撮像した時のフランジ位置姿勢である。これにより、画像中の軸方向と目標位置との距離に基づいてフランジ位置姿勢を制御できる。その結果、フランジ位置姿勢計算は目標位置の正確な３次元位置を必要としないため、たとえば撮像装置１である２台のカメラ間の位置姿勢キャリブレーション誤差による３次元計測精度の影響を低減することができる。

（ステップＳ１０７）
ステップＳ１０７では、終了判断部１０５が、軸方向取得部１０２及び目標位置検出部１０３から軸方向データ及び目標位置データを受け取り、制御ループを終了するか継続するかを判断する。また、終了判断部１０５は、終了判断データを制御部１０６に送る。ここで、制御ループとはＳ１０２からＳ１０９までの、撮像した結果に基づいてロボットアーム２を制御する一連のループ処理のことである。終了判断は、検出した軸と目標位置との距離で判断してもよいし、前回の距離と今回の距離との差で判断してもよいし、ループ回数で判断してもよい。たとえば、前回の距離と今回の距離との差が一定値以下である場合に「終了」、それ以外の場合に「継続」と判断してもよいし、ループ回数が一定値以上の場合に「終了」、それ以外の場合に「継続」と判断してもよい。本実施形態では、軸方向と目標位置との距離が一定値以下である場合に「終了」、それ以外の場合に「継続」と判断する。

（ステップＳ１０８）
ステップＳ１０８では、制御部１０６が、動作生成部１０４から動作を規定する位置姿勢データを受け取り、受け取った位置姿勢へロボットアーム２を移動するよう制御する。

（ステップＳ１０９）
ステップＳ１０９では、制御部１０６が、終了判断部１０５から終了判断データを受け取り、終了判断データが「継続」の場合は、制御部１０６が撮像装置１に撮像トリガを送り、Ｓ１０２に戻る。終了判断データが「終了」の場合は、Ｓ１１０へ進む。

（ステップＳ１１０）
ステップＳ１１０では、制御部１０６が、タスク動作を実行する。ここで、タスク動作とは、ロボットアーム２及びエンドエフェクタ３を、事前に設定された動きで制御することである。本実施形態のタスク動作は、エンドエフェクタ３であるドライバにより目標位置にあるネジを外すネジ外し動作である。たとえば、エンドエフェクタ３であるドライバを前進させて、近接センサによりエンドエフェクタ３であるドライバとネジとの接触を検知した後、ドライバを回転させながらネジを外してタスク動作を実行する前の位置姿勢に戻す。

（ステップＳ１１１）
ステップＳ１１１では、制御部１０６が、処理終了の判断をする。次の目標となるネジがある場合はＳ１０１に戻る。全ての目標に対してタスク実行が完了した場合は処理を終了する。

[効果]
本実施形態によれば、画像中の軸方向および目標位置からビジュアルサーボによるロボット制御を実現することができる。その結果、キャリブレーション作業の負荷を低減しながら、画像から十分に特徴を抽出できない場合においても、高精度にタスクを実行できる。

［第１の実施形態の変形例１］
制御対象の制御量H'を式２の定義により、エンドエフェクタ座標系４４におけるx軸、y軸の並進としたが、これに限らない。制御量H'はx軸、y軸まわりの回転でもよいし、並進と回転を併用してもよい。

以下、図６のフローチャートにより第１の実施形態の変形例１の制御処理方法を説明する。図６におけるステップＳ１０１～ステップＳ１０５、ステップＳ１０７～ステップＳ１１１は、それぞれ第１の実施形態の略同様のため省略する。

（ステップＳ１０６１）
ステップＳ１０６１では、動作生成部１０４が、軸方向取得部１０２及び目標位置検出部１０３から軸方向データ及び目標位置データを受け取り、生成する動作パラメータを変更するか判断する。ここで、動作パラメータとは、制御対象の制御量H'の成分である。具体的には、制御量H'のx軸、y軸、z軸の並進成分、x軸、y軸、z軸まわりの回転成分である。動作生成部１０４が動作パラメータを変更すると判断した場合は、Ｓ１０６２に進み、動作パラメータを変更しないと判断した場合はＳ１０６３に進む。判定はＳ１０６１を一定回数実行する毎に変更判定と非変更判定とを切り替えてもよいし、事前に登録したパターンで変更判定と非変更判定を切り替えてもよいし、検出した軸方向と目標位置との距離で判定してもよい。本実施形態では、軸方向と目標位置との距離が一定値以下である場合と、その次の判定とで１度ずつ変更判定とする。これにより、複数の動作パラメータを組み合わせてビジュアルサーボができる。

（ステップＳ１０６２）
ステップＳ１０６２では、動作生成部１０４が、動作パラメータのうち固定する成分と変動させる成分を切り替える。動作生成部１０４が動作パラメータを切り替える前の制御量H'は式２から、エンドエフェクタ座標系４４のx軸、y軸方向の並進成分以外の成分は0である。たとえば、動作パラメータをエンドエフェクタ座標系４４のx軸、y軸まわりの回転以外の成分を0にするように変更すると、制御量H'は式６になる。

別の例として、動作パラメータをエンドエフェクタ座標系４４のz軸方向に一定量L前進するように変更すると、制御量H'は式７になる。

本実施形態では、たとえば１回目の変更判定では、エンドエフェクタ座標系４４のz軸方向にP'_z/2の距離前進するように動作パラメータを変更する。２回目の変更判定では、エンドエフェクタ座標系４４のx軸、y軸まわりの回転以外の成分が0になるように変更する。

画像から十分に制御対象または目標位置の特徴を抽出できない場合、動作パラメータの並進成分と回転成分とを同時に制御することはできない。しかし、本実施形態のように、並進制御、前進制御、回転制御を順番に切り替えて実行することで、並進成分の誤差と回転成分の誤差とを切り分けて制御することができる。そのため、画像から十分に制御対象または目標位置の特徴を抽出できない場合においても、位置と姿勢のビジュアルサーボが実現できる。

［第１の実施形態の変形例２］
制御対象としてエンドエフェクタ３であるドライバ、目標位置としてネジを用いたが、これに限る必要はない。エンドエフェクタ３として吸着ハンドを用いてもよいし、多指ハンドを用いてもよい。目標位置として、被把持部品や被組付け部品を用いてもよい。

たとえば、図７の撮像装置１で撮像した画像７１のように、制御対象としてエンドエフェクタ３である多指ハンド７２、軸方向としてエンドエフェクタ３である多指ハンドの中心線７４、目標位置として被把持部品７３の中心位置７５を用いてもよい。第１の実施形態と略同様に軸方向と目標位置との距離を近づけるように制御を行う。その後、エンドエフェクタ３である多指ハンドを前進し、閉じて、元の位置姿勢に戻ることで、被把持部品を正確に把持することができる。

また別の例として、図８で制御対象の制御位置と目標の軸方向とを画像から検出する場合の説明をする（位置検出及び方向検出）。８１は撮像装置１で撮像した画像、８２はエンドエフェクタ３である多指ハンドで把持した制御対象である組付け部品を表す。８３は制御対象である組付け部品の中心を検出した制御位置、８４は被組付け部品、８５は被組付け部品に基づいて検出した目標の軸方向を表す。第１の実施形態と略同様に軸方向と制御位置との距離を近づけるように制御を行う。その後、エンドエフェクタ３である多指ハンドを前進し、開いて、元の位置姿勢に戻ることで、制御対象に制御位置を設定し、被制御対象に軸方向を設定した場合でも正確にタスクを実行できる。なお、本実施形態において、把持は、把握（例えば複数のフィンガーで掴んだり挟持すること）や、保持（例えば真空吸着パッドや電磁力を利用して吸着すること）という概念を含む。

［第１の実施形態の変形例３］
図示しない動作表示部を用いて、ロボットの動作を表示してもよい。以下、図９を用いて動作表示部の一例を説明する。図９において、９１はユーザに提示する動作表示部の画面、９２は画像取得部１０１から受け取った画像、９３は軸方向取得部１０２から受け取った軸方向を表している。９４は目標位置検出部１０３から受け取った目標位置、９５は軸が動く方向と距離、９６は制御ループを一時的に停止する際に押す一時停止ボタンを表している。９７は制御ループを１回実行する際に押すステップ実行ボタン、９８は一時停止している制御ループを再開する際に押す再開ボタンを表している。ボタン操作は図示しないＵＩ機器（たとえば、マウス）によって行う。図９において、９５は矢印の方向が軸の動く方向、矢印の長さが軸の動く距離を示している。ただし、軸が動く距離は数値（実寸単位、画素単位）であっても構わない。また、軸が動く方向と距離とは、両方表示してあっても良いし、いずれか一方のみ表示してあっても良い。

まず、動作表示部は、画像取得部１０１、軸方向取得部１０２および目標位置検出部１０３から画像データ、軸方向データおよび目標位置データを受け取り、軸方向および目標位置を画像に重畳して表示する。また、動作表示部は、動作生成部１０４から位置姿勢データを受け取り、制御対象が移動する方向および距離を重畳して表示する。次に、ユーザが一時停止ボタン９６を押した場合、制御部１０６はロボットアーム２の制御を一時停止する。ユーザがステップ実行ボタン９７を押した場合、制御部１０６はロボットの制御を再開し、次にステップＳ１０８に到達した後、再度ロボットの制御を一時停止する。最後に、ユーザが再開ボタン９８を押すと、制御部１０６はロボットの制御を再開する。

このように、動作表示部を用いてロボットの動作を表示し、ロボットの動きを検出結果と共に確認しながら少しずつ動かすことで、制御や検出に必要なパラメータの調整や、問題が発生した時の原因解析を補助することができる。

［第１の実施形態の変形例４］
Ｓ１０１においてティーチングペンダントによる教示作業により設定した位置姿勢を取得したが、これに限る必要はない。たとえば、撮像装置１であるステレオカメラで俯瞰して撮像した画像に基づいて、ネジを検出しておおよその３次元位置を計測した結果に基づいて位置姿勢を求めてもよい。あるいは、ＣＡＤデータのような３次元モデルを用いてシミュレータにより位置姿勢を設定してもよい。また、複数の目標位置をタスク実行する順番と共に設定してもよい。

（第２の実施形態）
第２の実施形態では、第１の実施形態と同様に、ロボットに取り付けた撮像装置で、制御対象と目標位置とを撮像し、制御対象を目標位置まで誘導する。ただし、第２の実施形態では、光を投射する光投射装置と、光を投射した画像から目標であるネジが締結されている平面を計測する平面計測部とを備える点で第１の実施形態と異なる。目標であるネジが締結されている平面を計測することで、平面と軸方向とが決められた角度（例えば垂直）になるよう制御する作業を実現することができる。すなわち、目標位置における目標姿勢を求めることができる。このように目標位置と目標姿勢を求め、ロボットを制御することで、ロボットがエンドエフェクタであるドライバをネジに対して概略垂直に当ててネジを安定的に外すことができる。すなわち、ロボットによるタスクを安定的に行うことができる。

［装置の構成］
図１０のブロック図により第２の実施形態の制御装置２００を備えるロボットシステム２０００の装置例を示す。ロボットシステム２０００は、撮像装置１と、ロボットアーム２と、エンドエフェクタ３と、光投射装置４と、制御装置２００とを備える。なお、撮像装置１、ロボットアーム２、エンドエフェクタ３は、第１の実施形態と略同様であるため、説明を省略する。

光投射装置４は、平面計測をする対象に特徴となる光を投射する装置である。たとえば、光投射装置４は、十字のラインレーザ光を投射するクロスラインレーザ投射機で構成され、制御部１０６から受け取った投射トリガによってレーザ光を投射する。

制御装置２００は、画像取得部１０１と、軸方向取得部１０２と、目標位置検出部１０３と、平面計測部２０１と、動作生成部２０２と、終了判断部１０５と、制御部１０６とを備える。なお、画像取得部１０１、軸方向取得部１０２、目標位置検出部１０３、終了判断部１０５、制御部１０６は、第１の実施形態と略同様であるため、説明を省略する。

平面計測部２０１は、画像取得部１０１から、レーザ光が投射された画像データを受け取り、画像データから平面の３次元の法線を検出し、目標平面のデータ（法線データ）を動作生成部２０２に送る。

動作生成部２０２は、軸方向取得部１０２、目標位置検出部１０３および平面計測部２０１から受け取った軸方向データ、目標位置データおよび法線データに基づいて、ロボットアーム２の先端部の動作を規定する位置姿勢を計算する。そして、計算した位置姿勢データを制御部１０６に送る。

［制御処理］
図１１のフローチャートにより第２の実施形態の制御装置２００およびロボットシステム２０００による制御処理方法を説明する。ステップＳ１０１～ステップＳ１０５、ステップＳ１０７～Ｓ１１１は、それぞれ第１の実施形態と略同様であるため、説明を省略する。

（ステップＳ２０１）
ステップＳ２０１では、光投射装置４が、制御部１０６から投射トリガを受け取り、レーザ光を投射する。これにより、特徴のない平面でも安定して撮像装置１であるステレオカメラにより平面計測ができる。

（ステップＳ２０２）
ステップＳ２０２では、撮像装置１が、制御部１０６から撮像トリガを受け取り、撮像を行い、撮像した画像信号を画像取得部１０１に送る。

（ステップＳ２０３）
ステップＳ２０３では、画像取得部１０１が、撮像装置１から画像信号を受け取り、平面計測部２０１に画像データを送る。

（ステップＳ２０４）
ステップＳ２０４では、平面計測部２０１が、画像取得部１０１から画像データを受け取り、画像データから平面の法線を検出し、法線データを動作生成部２０２に送る。法線の検出は、たとえば、撮像装置１であるステレオカメラの左右の画像に基づいて、ステレオマッチングにより画像に投射されたレーザ領域の３次元点群を計測する。平面計測部２０１が、計測した３次元点群を主成分分析した第３主成分を計算することで、平面の法線を検出する。

（ステップＳ２０５）
ステップＳ２０５では、動作生成部２０２が、軸方向取得部１０２、目標位置検出部１０３および平面計測部２０１から軸方向データ、目標位置データおよび法線データを受け取り、ロボットアーム２の先端部の動作を規定する位置姿勢を計算する。また、動作生成部２０２が位置姿勢データを制御部１０６に送る。

以下、動作生成の一例を説明する。本実施形態では、ネジが締結されている平面に対して、エンドエフェクタ３であるドライバの軸をおおよそ垂直に当てるように制御する。エンドエフェクタ座標系４４をロボット座標系４１で表すと、式８となる。

また、撮像装置座標系４３で計測した平面姿勢Oをロボット座標系４１で表すと、式９となる。

ドライバ軸を平面に対して垂直に当てるように制御するため、エンドエフェクタ座標系４４の位置成分はそのままに、姿勢成分のz軸を平面の法線とおおよそ一致させる。平面の法線を目標姿勢として、姿勢成分のz軸方向が目標姿勢となるよう求めたドライバ位置姿勢を初期位置姿勢として第１の実施形態と略同様の動作生成を行う。

このようにすることで、エンドエフェクタ３を前進させたときに、ネジが締結されている平面に対して、エンドエフェクタ３であるドライバの軸をおおよそ垂直に当てるように制御できる。なお、本実施形態では、平面計測部２０１が、Ｓ１０１の初期化後に１度だけ平面計測を行ったが、これに限る必要はない。平面計測部２０１が、Ｓ２０５である動作生成の直前に平面計測を行ってもよいし、平面計測部２０１および動作生成部２０２がＳ１１０であるタスク実行直前に平面計測および動作生成を再度行ってもよい。

［効果］
本実施形態によれば、平面計測をすることで、制御対象に関連付けられた軸方向と平面との角度を制御するようビジュアルサーボを実行できる。その結果、目標位置における制御対象の姿勢が重要なタスクを実現できる。

［第２の実施形態の変形例１］
Ｓ２０４においてレーザ光が投影された画像を用いて平面計測を行ったが、これに限る必要はない。たとえば、平面にある模様を使ってもよいし、プロジェクタによってパターンを投影してもよいし、撮像装置１としてDepthカメラを用いてもよい。

（第３の実施形態）
第３の実施形態では、第１の実施形態と同様に、ロボットに取り付けた撮像装置で、制御対象と目標位置とを撮像し、制御対象を目標位置まで誘導する。ただし、第３の実施形態では、誘導した結果をキャリブレーション値に反映する点で第１の実施形態と異なる。ここで、キャリブレーション値とは、撮像装置１とロボットアーム２とエンドエフェクタ３との位置姿勢関係を表すパラメータのことである。キャリブレーション値の誤差が大きいほどビジュアルサーボの動作収束に時間がかかるが、本実施形態の方法を用いることで素早く収束させることができる。

［装置の構成］
図１２のブロック図により第３の実施形態の制御装置３００を備えるロボットシステム３０００の装置例を示す。ロボットシステム３０００は、撮像装置１と、ロボットアーム２と、エンドエフェクタ３と、制御装置３００とを備える。なお、撮像装置１、ロボットアーム２、エンドエフェクタ３は、第１の実施形態と略同様であるため、説明を省略する。

制御装置３００は、画像取得部１０１と、軸方向取得部１０２と、目標位置検出部１０３と、動作生成部３０１と、キャリブレーション値補正部３０２と、キャリブレーション値保存部３０３と、終了判断部１０５と、制御部１０６とを備える。なお、画像取得部１０１、軸方向取得部１０２、目標位置検出部１０３、終了判断部１０５、制御部１０６は、第１の実施形態と略同様であるため、説明を省略する。

動作生成部３０１は、軸方向取得部１０２、目標位置検出部１０３およびキャリブレーション値保存部３０３から受け取った軸方向データ、目標位置データおよびキャリブレーション値に基づいてロボットアーム２の先端部の動作を規定する位置姿勢を計算する。そして、位置姿勢データを制御部１０６およびキャリブレーション値補正部３０２に送る。

キャリブレーション値補正部３０２は、動作生成部３０１から位置姿勢データを受け取り、キャリブレーション値を補正し、補正したキャリブレーション値をキャリブレーション値保存部３０３に送る。

キャリブレーション値保存部３０３は、キャリブレーション値補正部３０２からキャリブレーション値を受け取り、保存する。また、キャリブレーション値保存部３０３は、動作生成部３０１にキャリブレーション値を送る。たとえば、キャリブレーション値保存部３０３はメモリから構成される。

［制御処理］
図１３のフローチャートにより第３の実施形態の制御装置３００およびロボットシステム３０００による制御処理方法を説明する。ステップＳ１０１～ステップＳ１０５、ステップＳ１０７～Ｓ１１１は、それぞれ第１の実施形態と略同様であるため、説明を省略する。

（ステップＳ３０１）
ステップＳ３０１では、動作生成部３０１が、軸方向取得部１０２、目標位置検出部１０３およびキャリブレーション値保存部３０３から軸方向データ、目標位置データおよびキャリブレーション値を受け取る。動作生成部３０１は受け取ったキャリブレーション値を用いて実施形態１と略同様にロボットアーム２の先端部の動作を規定する位置姿勢を計算する。また、動作生成部３０１が位置姿勢データを制御部１０６およびキャリブレーション値補正部３０２に送る。

（ステップＳ３０２）
ステップＳ３０２では、キャリブレーション値補正部３０２が、動作生成部３０１から動作生成部３０１により生成された結果（位置姿勢データ）を受け取り、キャリブレーション値の補正を行う。そして、補正したキャリブレーション値をキャリブレーション値保存部３０３に送る。ビジュアルサーボを実行している中でキャリブレーション値の補正を行うことで、徐々に誤差を低減していくことができる。補正するキャリブレーション値は、ロボットアーム２の先端部と撮像装置１であるカメラとの位置姿勢の関係性を表す変換行列G でもよいし、撮像装置１であるカメラとエンドエフェクタ３であるドライバとの位置姿勢の関係性を表す変換行列Hでもよい。本実施形態では、変換行列Gを補正する。補正後の変換行列をG'とすると、式５より式１０が成り立つ。

（ステップＳ３０３）
ステップＳ３０３では、キャリブレーション値保存部３０３が、キャリブレーション値補正部３０２から補正されたキャリブレーション値を受け取り保存する。

［効果］
本実施形態によれば、制御対象を制御すると同時にキャリブレーション値を補正することで、キャリブレーション値の誤差を徐々に小さくできる。その結果、次のビジュアルサーボ実行時にはより素早く制御を終了することができる。

［第３の実施形態の変形例１］
Ｓ３０３においてキャリブレーション値のみを保存したが、これに限る必要はない。たとえば、現在のロボット位置姿勢とキャリブレーション値との組を保存してもよいし、教示作業により設定した初期位置姿勢のIDとキャリブレーション値との組を保存してもよい。これにより、ロボットの位置姿勢に依存するようなキャリブレーション誤差が存在する場合でも、各位置姿勢において誤差を低減することができる。

[ハードウェア構成]
図２、図１０、図１２に示した制御装置１００、２００、３００は、一般のＰＣ（パーソナルコンピュータ）を用いて実現することができる。制御装置１００、２００、３００において各部の機能をＰＣのＣＰＵに実行させるためのコンピュータプログラムやデータをハードディスク装置に格納しておく。ＣＰＵは、ハードディスク装置に格納されているコンピュータプログラムやデータを適宜ＲＡＭ等のメモリにロードし、該コンピュータプログラムやデータを用いて処理を実行できる。これにより、結果としてＰＣは、制御装置１００、２００、３００の機能を実現することができる。

［その他の変形例１］
すべての実施形態において、撮像装置１として２台のグレースケールカメラを用いたが、これに限る必要はない。３台以上のグレースケールカメラを用いてもよいし、ＲＧＢのカラーカメラを用いてもよいし、Depthカメラを用いてもよい。

［その他の変形例２］
すべての実施形態において、ロボットアーム２として６軸ロボットを用いたが、これに限る必要はない。その他の多関節ロボットやパラレルリンクロボットでもよいし、直交ロボットでもよい。

［その他の変形例３］
第１の実施形態において、エンドエフェクタ３であるドライバとネジとの接触の検知に近接センサを用いたが、これに限る必要はない。画像に基づいて検知してもよいし、距離センサを用いてもよいし、力覚センサを用いてもよい。

＜実施形態の効果＞
第１の実施形態によれば、画像中の軸方向および目標位置からビジュアルサーボによるロボット制御を実現することができる。その結果、キャリブレーション作業の負荷を低減しながら、画像から十分に特徴を抽出できない場合においても、高精度にタスクを実行できる。

第２の実施形態によれば、平面計測をすることで、制御対象に関連付けられた軸方向と平面との角度を制御するようビジュアルサーボを実行できる。その結果、目標位置における制御対象の姿勢が重要なタスクを実現できる。

第３の実施形態によれば、制御対象を制御すると同時にキャリブレーション値を補正することで、キャリブレーション値の誤差を徐々に小さくできる。その結果、次のビジュアルサーボ実行時にはより素早く制御を終了することができる。

＜定義＞
目標位置とはエンドエフェクタがタスクを実行する位置のことである。軸方向とは、制御対象と目標位置との位置姿勢関係を測るための制御対象に関連付けられた画像中の線分のことである。タスク動作とは、ロボットアーム及びエンドエフェクタを、事前に設定された動きで制御することである。動作パラメータとは、制御対象の制御量の成分のことである。キャリブレーション値とは、撮像装置とロボットアームとエンドエフェクタとの位置姿勢関係を表すパラメータのことである。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１０００：ロボットシステム、１：撮像装置、２：ロボットアーム、３：エンドエフェクタ、１００：制御装置、１０１：画像取得部、１０２：軸方向取得部、１０３：目標位置検出部、１０４：動作生成部、１０５：終了判断部、１０６：制御部

Claims (14)

1. ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標位置であって凹部にある目標の位置である前記目標位置を撮像した画像を取得する画像取得手段と、
   前記画像における前記制御対象のエッジに基づいて、前記制御対象の回転軸の進行方向であって前記制御対象の回転軸に沿っている前記進行方向を表す該画像中の２次元方向を取得する進行方向取得手段と、
   前記画像から前記目標位置を表す該画像中の２次元位置を検出する目標位置検出手段と、
   前記回転軸の進行方向の延長上に前記目標位置が存在し、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が前記目標位置に達する前に生成し、さらに姿勢を変更した制御対象の進行方向に沿って前記制御対象を前記目標位置に近づける動作を生成する動作生成手段と、
   前記動作に従って前記制御対象を制御する制御手段と、
   を備え、
   前記動作生成手段は、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新することを特徴とする制御装置。
2. 前記進行方向および前記目標位置に基づいて前記制御手段による制御を終了するか否かを判断する終了判断手段をさらに備えることを特徴とする請求項１に記載の制御装置。
3. 前記動作生成手段は、前記進行方向および前記目標位置に基づいて前記制御対象の制御量の成分である動作パラメータを切り替え、当該切り替えた動作パラメータに従って前記制御対象の動作を生成することを特徴とする請求項１に記載の制御装置。
4. 前記目標位置と、前記進行方向と、前記制御対象の軸が移動する方向または距離の少なくとも一方を、前記画像に重畳して表示する動作表示手段をさらに備えることを特徴とする請求項１に記載の制御装置。
5. 目標平面を計測する平面計測手段をさらに備え、
   前記動作生成手段は、前記目標平面に基づいて、前記凹部の深さ方向を求めることを特徴とする請求項１に記載の制御装置。
6. 前記撮像装置と、前記ロボットが有するロボットアームと、前記制御対象との位置姿勢関係を表すパラメータであるキャリブレーション値を保存するキャリブレーション値保存手段と、
   前記キャリブレーション値および前記動作生成手段により生成された結果に基づいて、前記キャリブレーション値を補正するキャリブレーション値補正手段と、
   をさらに備えることを特徴とする請求項１に記載の制御装置。
7. 前記制御対象は、前記ロボットアームに取り付けられたエンドエフェクタであることを特徴とする請求項６に記載の制御装置。
8. 前記凹部はネジ穴であり、前記制御対象はドライバであることを特徴とする請求項１に記載の制御装置。
9. ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標であって凹部にある前記目標を撮像した画像を取得する画像取得手段と、
   前記画像から前記制御対象に関連付けられた制御位置を表す該画像中の２次元位置を検出する位置検出手段と、
   前記画像における前記制御対象のエッジに基づいて、前記目標に関連付けられた前記制御対象の回転軸に沿った進行方向を表す該画像中の２次元方向を検出する方向検出手段と、
   前記回転軸の進行方向及び前記制御位置に基づいて、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が目標位置に達する前に生成し、さらに姿勢を変更した制御対象の回転軸の進行方向に沿って前記制御対象を前記制御位置に近付ける動作を生成する動作生成手段と、
   前記動作に従って前記制御対象を制御する制御手段と、
   を備え、
   前記動作生成手段は、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新することを特徴とする制御装置。
10. ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標位置であって凹部にある目標の位置である前記目標位置を撮像した画像を取得する画像取得手段と、
    前記画像における前記制御対象のエッジに基づいて、前記制御対象の回転軸の進行方向であって前記制御対象の回転軸に沿っている前記進行方向を表す該画像中の２次元方向を取得する進行方向取得手段と、
    前記画像から前記目標位置を表す該画像中の２次元位置を検出する目標位置検出手段と、
    前記回転軸の進行方向の延長上に前記目標位置が存在し、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が前記目標位置に達する前に生成し、さらに姿勢を変更した制御対象の進行方向に沿って前記制御対象を前記目標位置に近づける動作を生成する動作生成手段と、
    前記動作に従って前記制御対象を制御する制御手段と、
    を備え、
    前記動作生成手段は、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新する制御装置と、
    前記ロボットと、
    前記撮像装置と、
    を備えることを特徴とするロボットシステム。
11. 制御装置の動作方法であって、
    ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標位置であって凹部にある目標の位置である前記目標位置を撮像した画像を取得する工程と、
    前記画像における前記制御対象のエッジに基づいて、前記制御対象の回転軸の進行方向であって前記制御対象の回転軸に沿っている前記進行方向を表す該画像中の２次元方向を取得する工程と、
    前記画像から前記目標位置を表す該画像中の２次元位置を検出する工程と、
    前記回転軸の進行方向の延長上に前記目標位置が存在し、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が前記目標位置に達する前に生成し、さらに姿勢を変更した制御対象の進行方向に沿って前記制御対象を前記目標位置に近づける動作を生成する工程と、
    前記動作に従って前記制御対象を制御する工程と、
    を有し、
    前記動作を生成する工程では、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新することを特徴とする制御装置の動作方法。
12. 制御装置の動作方法であって、
    ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標であって凹部にある前記目標を撮像した画像を取得する工程と、
    前記画像から前記制御対象に関連付けられた制御位置を表す該画像中の２次元位置を検出する工程と、
    前記画像における前記制御対象のエッジに基づいて、前記目標に関連付けられた前記制御対象の回転軸に沿った進行方向を表す該画像中の２次元方向を検出する工程と、
    前記回転軸の進行方向及び前記制御位置に基づいて、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が目標位置に達する前に生成し、さらに姿勢を変更した制御対象の回転軸の進行方向に沿って前記制御対象を前記制御位置に近付ける動作を生成する工程と、
    前記動作に従って前記制御対象を制御する工程と、
    を有し、
    前記動作を生成する工程では、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新することを特徴とする制御装置の動作方法。
13. 制御装置の動作方法であって、
    ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標位置であって凹部にある目標の位置である前記目標位置を撮像した画像を取得する工程と、
    前記画像における前記制御対象のエッジに基づいて、前記制御対象の回転軸の進行方向であって前記制御対象の回転軸に沿っている前記進行方向を表す該画像中の２次元方向を取得する工程と、
    前記画像から前記目標位置を表す該画像中の２次元位置を検出する工程と、
    前記回転軸の進行方向の延長上に前記目標位置が存在し、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が前記目標位置に達する前に生成し、さらに姿勢を変更した制御対象の進行方向に沿って前記制御対象を前記目標位置に近づける動作を生成する工程と、
    前記動作に従って前記制御対象を制御する工程と、
    を有し、
    前記動作を生成する工程では、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新する制御装置の動作方法を、コンピュータに実行させるためのプログラム。
14. 制御装置の動作方法であって、
    ロボットに取り付けられた撮像装置により、前記ロボットが回転制御可能な制御対象及び前記制御対象の移動先である目標であって凹部にある前記目標を撮像した画像を取得する工程と、
    前記画像から前記制御対象に関連付けられた制御位置を表す該画像中の２次元位置を検出する工程と、
    前記画像における前記制御対象のエッジに基づいて、前記目標に関連付けられた前記制御対象の回転軸に沿った進行方向を表す該画像中の２次元方向を検出する工程と、
    前記回転軸の進行方向及び前記制御位置に基づいて、前記回転軸の進行方向と前記凹部の深さ方向とが一致するように前記制御対象の姿勢を変更する動作を前記制御対象が目標位置に達する前に生成し、さらに姿勢を変更した制御対象の回転軸の進行方向に沿って前記制御対象を前記制御位置に近付ける動作を生成する工程と、
    前記動作に従って前記制御対象を制御する工程と、
    を有し、
    前記動作を生成する工程では、前記２次元方向に沿った線分と前記２次元位置とを近づける前記制御対象の姿勢の制御量を求め、該制御量に基づいて前記制御対象の姿勢を更新する制御装置の動作方法を、コンピュータに実行させるためのプログラム。

Images