JP2009051003A - ロボット装置の制御方法及びロボット装置 - Google Patents

Abstract

【課題】ロボット装置のマニピュレータを制御するにあたり、多くの基準画像を用意する必要がなく、計算負荷が軽く、迅速な制御が可能でありながら、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することができるロボット装置の制御方法を提供する。
【解決手段】マニピュレータ１の手先の方向を旋回させたときの対象物１０１の画像のカメラ６による画像における移動量に基づいて、マニピュレータ１の手先の方向を制御して対象物１０１の画像をカメラ６の視野の中心に位置させ、カメラ６によって得られた対象物１０１の画像の大きさに基づいてマニピュレータ１の手先と対象物１０１との距離を算出し、マニピュレータ１の手先をカメラ６のレンズの光軸方向に移動させて対象物１０１に接近させる。
【選択図】図３

Description

本発明は、マニピュレータの先端にカメラを備えたロボット装置において、事前に目標状態から算出し記憶した目標特徴量とカメラにより取得した画像から得た特徴量とが一致するようにマニピュレータを動作させるロボット装置の制御方法及びロボット装置に関する。

従来、マニピュレータを備えたロボット装置が提案されており、また、このようなロボット装置のマニピュレータの動作を制御するためのロボット装置の制御方法が提案されている。そして、ロボット装置の制御方法として、マニピュレータにより把持を行う対象物を目標として、マニピュレータの手先を移動させるための制御方法が種々提案されている。

例えば、特許文献１には、対象物の基準画像を複数用意しておくとともに、マニピュレータの手先に取り付けたカメラによる画像情報を取得し、画像情報における対象物の形状に近い基準画像を検索することによって、対象物とマニピュレータの手先との位置関係を計算するようにしたロボット装置が記載されている。

また、特許文献２には、対象物の基準画像を用意しておき、マニピュレータの手先に取り付けたカメラにより画像を撮影しながら、基準画像に近い画像が得られるまで、マニピュレータを一定量移動させるようにしたロボット装置が記載されている。

特開２００３−２３１０７８公報 特開２０００−２６３４８２公報

ところで、特許文献１に記載のロボット装置においては、制御の精度を高めるためには、多くの基準画像を用意しておき、マニピュレータの動作のたびに全ての基準画像の検索を行なう必要があるため、計算負荷が高く、迅速な制御ができない。

また、特許文献２に記載のロボット装置においては、２次元平面内におけるマニピュレータの位置及び姿勢合わせにしか対応できず、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することはできない。さらに、このロボット装置においては、撮像、計測とマニピュレータの動作とを繰り返さなければならないため、迅速な制御ができない。

そこで、本発明は、前記の実情に鑑みてなされたものであり、その目的は、ロボット装置のマニピュレータを制御するにあたり、多くの基準画像を用意する必要がなく、計算負荷が軽く、迅速な制御が可能でありながら、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することができるようになされたロボット装置の制御方法及びロボット装置を提供することにある。

前述の課題を解決し、前記目的を達成するため、本発明に係るロボット装置の制御方法は、以下の構成のいずれか一を有するものである。

〔構成１〕
６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置を制御するロボット装置の制御方法であって、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいて、対象物の画像が撮像手段の視野の中心に位置するように、マニピュレータの手先の方向を制御し、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、撮像手段によって得られた対象物の画像の大きさに基づいて、マニピュレータの手先と対象物との間の距離を算出して、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させ、該手先を対象物に接近させることを特徴とするものである。

〔構成２〕
構成１を有するロボット装置の制御方法において、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっていることを特徴とするものである。

〔構成３〕
構成１を有するロボット装置の制御方法において、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有していることを特徴とするものである。

〔構成４〕
構成１乃至構成３のいずれか一を有するロボット装置の制御方法において、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とすることを特徴とするものである。

〔構成５〕
構成４を有するロボット装置の制御方法において、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、対象物の画像が撮像手段の視野の中心に位置するようにマニピュレータの手先の方向を制御し、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御することを特徴とするものである。

また、本発明に係るロボット装置は、以下の構成のいずれか一を有するものである。

〔構成６〕
６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置であって、マニピュレータの動作を制御する制御手段を備え、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいて、対象物の画像が撮像手段の視野の中心に位置するように、マニピュレータの手先の方向を制御し、次に、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、撮像手段によって得られた対象物の画像の大きさに基づいて、マニピュレータの手先と対象物との間の距離を算出して、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させ、該手先を対象物に接近させる
ことを特徴とするものである。

〔構成７〕
構成６を有するロボット装置において、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっていることを特徴とするものである。

〔構成８〕
構成６を有するロボット装置において、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有していることを特徴とするものである。

〔構成９〕
構成６乃至構成８のいずれか一を有するロボット装置において、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とすることを特徴とするものである。

〔構成１０〕
構成９を有するロボット装置において、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報とを照合して、対象物の画像が撮像手段の視野の中心に位置するようにマニピュレータの手先の方向を制御し、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御することを特徴とするものである。

本発明に係る複数ロボットの制御方法においては、構成１を有することにより、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいてマニピュレータの手先の方向を制御して、対象物の画像を撮像手段の視野の中心に位置させ、次に、撮像手段によって得られた対象物の画像の大きさに基づいてマニピュレータの手先と対象物との間の距離を算出し、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させることにより、マニピュレータの手先を対象物に接近させることができる。

本発明に係る複数ロボットの制御方法においては、構成２を有することにより、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっているので、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。

本発明に係る複数ロボットの制御方法においては、構成３を有することにより、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有しているので、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。

本発明に係る複数ロボットの制御方法においては、構成４を有することにより、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とするので、マニピュレータと対象物との正確な位置調整が可能となるとともに、マニピュレータによる対象物の把持を確実に行うことができる。

本発明に係る複数ロボットの制御方法においては、構成５を有することにより、対象物の画像が撮像手段の視野の中心に位置するようにマニピュレータの手先の方向を制御し、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御するので、マニピュレータと対象物との正確な位置調整がなされ、マニピュレータによる対象物の把持を確実に行うことができる。

すなわち、本発明に係るロボット装置の制御方法においては、３次元空間においてマニピュレータと対象物との位置合わせ及び姿勢合わせを行うことができ、かつ、任意の関節を指定角度だけ動かし、かつ、撮像手段のレンズの光軸方向に動かすなど、一般的な産業用マニュピレータが有する動作を繰り返して位置合わせを行なうので、制御手段の構成を簡素化でき、演算付加を軽減することができる。

そして、本発明に係る複数ロボットにおいては、構成６を有することにより、制御手段は、マニピュレータの手先の方向を旋回させたときの対象物の画像の撮像手段における移動量に基づいてマニピュレータの手先の方向を制御して、対象物の画像を撮像手段の視野の中心に位置させ、次に、撮像手段によって得られた対象物の画像の大きさに基づいてマニピュレータの手先と対象物との間の距離を算出し、マニピュレータの手先を撮像手段のレンズの光軸方向に移動させるので、任意の関節を指定角度だけ動かし、かつ、撮像手段のレンズの光軸方向に動かすなど、一般的な産業用マニュピレータが有する動作を繰り返すことにより、マニピュレータの手先を対象物に接近させることができる。

本発明に係る複数ロボットにおいては、構成７を有することにより、対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっているので、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。

本発明に係る複数ロボットにおいては、構成８を有することにより、対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有しているので、制御手段は、撮像手段によって得られる対象物の画像と対象物の形状についての事前に記憶された情報との照合を迅速に行うことができる。

本発明に係る複数ロボットにおいては、構成９を有することにより、制御手段は、マニピュレータの手先の方向を、対象物の計測面に垂直な方向とするので、マニピュレータと対象物との正確な位置調整が可能となるとともに、マニピュレータによる対象物の把持を確実に行うことができる。

本発明に係る複数ロボットにおいては、構成１０を有することにより、制御手段は、対象物の画像が撮像手段の視野の中心に位置するようにマニピュレータの手先の方向を制御し、次に、撮像手段の視野内における対象物の画像が所定の方向となるように、マニピュレータの手先を回転制御するので、マニピュレータと対象物との正確な位置調整がなされ、マニピュレータによる対象物の把持を確実に行うことができる。

すなわち、本発明に係るロボット装置においては、３次元空間においてマニピュレータと対象物との位置合わせ及び姿勢合わせを行うことができ、かつ、任意の関節を指定角度だけ動かし、かつ、撮像手段のレンズの光軸方向に動かすなど、一般的な産業用マニュピレータが有する動作を繰り返して位置合わせを行なうので、制御手段の構成を簡素化でき、演算付加を軽減することができる。

すなわち、本発明は、ロボット装置のマニピュレータを制御するにあたり、多くの基準画像を用意する必要がなく、計算負荷が軽く、迅速な制御が可能でありながら、マニピュレータの対象物に対する位置及び姿勢を３次元空間内において制御することができるようになされたロボット装置の制御方法及びロボット装置を提供することができるものである。

以下、本発明を実施するための最良の形態について図面を参照して説明する。

〔ロボット装置の構成〕
図１は、本発明に係るロボット装置の構成を示す模式的な側面図である。

本発明に係るロボット装置は、図１に示すように、マニピュレータ１を有し、このマニピュレータ１により、対象物１０１の把持、運搬及び他の部材への組立が可能となされているものである。

マニピュレータ１は、複数のアクチュエータ（駆動装置）とリンク（剛体の構造物）とによって構成されており、６自由度を有している。すなわち、各リンク間は、回動（屈曲）、または、旋回可能な関節２ａ，２ｂ，２ｃ，２ｄ，２ｅ，２ｆを介して接続されており、それぞれアクチュエータによって相対駆動されるようになっている。各アクチュエータは、制御手段となるコンピュータ３によって制御される。

このマニピュレータ１において、第１のリンク（基端部のリンク）１ａは、基台部４に対し、基端側を第１の関節２ａを介して接続されて設置されている。第１の関節２ａは、鉛直軸（ｚ軸）回りに旋回可能な関節である。この第１のリンク１ａの先端側には、第２の関節２ｂを介して、第２のリンク１ｂの基端側が接続されている。第２の関節２ｂは、水平な軸回りに第２のリンク１ｂを回動可能とする関節である。第２のリンク１ｂの先端側には、第３の関節２ｃを介して、第３のリンク１ｃの基端側が接続されている。第３の関節２ｃは、水平な軸回りに第３のリンク１ｃを回動可能とする関節である。

そして、第３のリンク１ｃの先端側には、第４の関節２ｄを介して、第４のリンク１ｄの基端側が接続されている。第４の関節２ｄは、第３のリンク１ｃの軸回りに第４のリンク１ｄを旋回可能とする関節である。第４のリンク１ｄの先端側には、第５の関節２ｅを介して、第５のリンク１ｅの基端側が接続されている。第５の関節２ｅは、第４のリンク１ｄの軸に直交する軸回りに第５のリンク１ｅを回動可能とする関節である。第５のリンク１ｅの先端側には、第６の関節２ｆを介して、第６のリンク１ｆの基端側が接続されている。第６の関節２ｆは、第５のリンク１ｅの軸回りに第６のリンク１ｆを旋回可能とする関節である。

このように、マニピュレータ１においては、回動可能な関節と、旋回可能な関節とが、交互に計６個設けられていることにより、６自由度が確保されている。

第６のリンク１ｆの先端側（以下、「手先」という。）には、対象物１０１を把持したり加工したりする先端ツール機構５が設けられている。この先端ツール機構５は、コンピュータ３や図示しないその他の制御装置によって制御される。また、手先の近傍には、撮像レンズ及びＣＣＤ（固体撮像素子）からなる撮像手段となるカメラ６が取付けられている。

コンピュータ３は、画像特徴データテーブル３ａを有している。この画像特徴データテーブル３ａには、複数の対象物１０１の形状を示す画像データ及び数値データが記憶されている。また、コンピュータ３は、画像処理部３ｂを有しており、カメラ６から送られる画像信号から対象部品１０１の形状を抽出し、この形状と、画像特徴データテーブル３ａに記憶された画像データ及び数値データとを照合する。そして、コンピュータ３は、動作指令生成部３ｃを有している。この動作指令生成部３ｃは、画像処理部３ｂにおける画像信号と画像特徴データテーブル３ａ内のデータとの照合結果に応じて、マニピュレータ１の各アクチュエータに対する動作指令信号を生成し、各アクチュエータに送る。

そして、マニピュレータ１においては、第１のリンク１ａに対する第２のリンク１ｂの位置、第２のリンク１ｂに対する第３のリンク１ｃの位置、第３のリンク１ｃに対する第４のリンク１ｄの位置と、順次先端側のリンクの位置が制御されることにより、手先の位置が制御され、この手先において先端ツール機構５によって把持したワークを所定の位置に搬送することができる。

そして、このロボット装置においては、以下に示す本発明に係るロボット装置の制御方法が実行されることにより、カメラ６のレンズの光軸に対して対象物１０１の計測面が垂直となるように、マニピュレータ１が自動制御される。なお、この実施の形態においては、対象物１０１が円形の部品であるものとして説明する。

図２は、本発明に係るロボット装置の制御方法の事前準備（ａ）及び制御動作（ｂ）を示すフローチャートである。

マニピュレータ１の自動制御を行うための事前準備として、図２中の（ａ）に示すように、Ｓ１において事前準備を開始すると、Ｓ２において、制御対象となるロボット装置のマニピュレータ１に固有のパラメータを算出し、コンピュータ３の記憶装置にテーブルとして記憶しておく。

すなわち、カメラ６の撮像素子の縦横の有効素子数（Ｘｐｘ，Ｙｐｘ）（解像度（ピクセル単位））、有効画素の縦横の大きさ（Ｘｌｎ，Ｙｌｎ）（物理長さ）に基づき、撮像素子におけるピクセルと物理量長さとの変換係数を、以下のように算定しておく。
（Ｋｘｌｎ，Ｋｙｌｎ）＝（Ｘｌｎ／Ｘｐｘ，Ｙｌｎ／Ｙｐｘ）

図４は、カメラ６をピンホールカメラに模して表現した構成を示す斜視図である。

また、図４に示すように、カメラ６の撮像レンズ６ａから撮像素子６ｂまでの距離Ｄｐｌ（物理長さ）と、撮像レンズ６ａから対象物１０１までの距離Ｌとに基づき、撮像画像における長さＰＳと実際の物理長さＲＳとの関係を算定しておく。なお、撮像レンズ６ａから撮像素子６ｂまでの距離Ｄｐｌと、撮像レンズ６ａから対象物１０１までの距離Ｌとの関係は、撮像レンズ６ａの焦点距離に基づいて算出することができる。したがって、撮像レンズ６ａから撮像素子６ｂまでの距離Ｄｐｌと撮像レンズ６ａの焦点距離とに基づいて、撮像画像における長さＰＳと実際の物理長さＲＳとの関係を算定することができる。

さらに、ロボット装置に固有のパラメータとしては、手先座標系からカメラ座標系への変換行列Ｔも算定しておく。

次に、Ｓ３において、対象物１０１に固有のパラメータを算出し、コンピュータ３の画像特徴データテーブル３ａに記憶しておく。

すなわち、目標とする対象物１０１の画像特徴量として、カメラ６により対象物１０１を撮像したときの対象物１０１の形状の特徴量を記憶しておく。具体的には、対象物１０１の画像上の目標位置（Ｐｘ，Ｐｙ）（中心位置や重心位置など）をピクセル単位で記憶し、また、方向を示す姿勢θ（degree、あるいは、rad単位）、距離の判定に用いる大きさＰＳ（ピクセル単位）などをテーブルとして記憶しておく。例えば、撮像された対象物１０１の形状が楕円である場合には、画像上の楕円の中心位置（Ｐｘ、Ｐｙ）、ＰＳ（長径、または、短径）、θ（長径の向き角度）などを記憶しておく。

対象物１０１は、画像上において円に近似させ、例えば、円で覆い、その中心位置と半径とを求める。なお、対象物１０１の面積（上面の面積）を算出して記憶しておいてもよい。

そして、Ｓ４において事前準備を終了する。事前準備が終了していれば、図２中の（ｂ）に示すように、制御動作を実行することができる。すなわち、Ｓ５において制御動作を開始すると、Ｓ６においてセンタリングを実行する。

図３は、本発明に係るロボット装置の制御方法を実行している状態における各段階に応じた撮像画像を示す正面図である。

センタリングは、図３中のフェーズ１に示すように、対象物１０１の画像が、カメラ６により撮像された画像の中心にくるように、マニピュレータ１を動作させる制御である。

図５は、対象物１０１のセンタリングを行っている状態における撮像画像を示す正面図である。

このセンタリングについては、図５に示すように、カメラ６により撮像された画像の中心位置をＣ０（Ｘ０，Ｙ０）とし、この画像上における対象物１０１の初期位置をＣ１（Ｐｘ１，Ｐｙ１）として説明する。

図６は、センタリング動作の手順を示すフローチャートである。

このセンタリングは、以下の手順により行う。すなわち、図６に示すように、Ｓ１１でセンタリングを開始すると、Ｓ１２において、カメラ６により撮像された画像の中心位置をＣ０、画像上における対象物１０１の初期位置Ｃ１を取得したうえで、Ｓ１３に進む。Ｓ１３において、第４の関節２ｄを選択し、Ｓ１４において、適当な値ｑｔ４だけ駆動させる。駆動量は、任意の角度及び向きでよいが、例えば、３deg乃至５deg程度とすることが好ましい。

このとき、図５に示すように、カメラ６により再度撮像を行い、このときに取得された画像における対象物１０１の位置をＣ２（Ｐｘ２，Ｐｙ２）とし、図６中のＳ１４に示すように、位置Ｃ２を取得して記憶する。

そして、Ｓ１５に進み、図５に示すように、画像上における対象物１０１の位置Ｃ０と位置Ｃ２の距離ｄ、位置Ｃ１と位置Ｃ２の距離ｄ１２を求める。これらは、それぞれの点の位置（画像上における座標）がわかっているので、以下の式により求めることができる。
ｄ＝√｛（Ｘ０−Ｐｘ２）^２＋（Ｙ０−Ｐｙ２）^２｝
ｄ１２＝√｛（Ｐｘ１−Ｐｘ２）^２＋（Ｐｙ１−Ｐｙ２）^２｝

次に、図６のＳ１６に進み、直線Ｃ０〜Ｃ１の傾きｋ１と、直線Ｃ０〜Ｃ２の傾きｋ２と、直線Ｃ１〜Ｃ２の傾きｋ１２とを求める。これらも、それぞれの点の位置（画像上における座標）がわかっているので、以下の式により求めることができる。
ｋ１＝（Ｐｙ１−Ｐｙ０）／（Ｐｘ１−Ｐｘ０）
ｋ２＝（Ｐｙ２−Ｐｙ０）／（Ｐｘ２−Ｐｘ０）
ｋ１２＝（Ｐｙ１−Ｐｙ２）／（Ｐｘ１−Ｐｘ２）

ここで、図５に示すように、直線Ｃ１〜Ｃ２に垂直で位置Ｃ０を通る直線と、直線Ｃ１〜Ｃ２との交点を点Ａとする。図６のＳ１７に進み、点Ａと位置Ｃ０との距離ｄ０Ａを求める。これは、直線Ｃ１〜Ｃ２と点Ｃ０の距離であるので、以下の式により求めることができる。
ｄ０Ａ＝｜ｋ１２・Ｘ０−Ｙ０＋Ｐｙ１−ｋ１２・Ｐｘ１｜／√（ｋ１２^２＋１）

次に、Ｓ１８に進み、位置Ｃ１から点Ａまでの距離ｄ２Ａを求める。この距離ｄ２Ａは、直角三角形Ｃ０−Ａ−Ｃ２の辺の長さの関係より、以下のようして求められる。
ｄ２Ａ＝√（ｄ１２^２−ｄ０Ａ^２）

Ｓ１９に進み、第４の関節２ｄを所定値ｑ４回転させる。この所定値ｑ４の大きさは、以下の式により得る。
ｑ４＝ｄ２Ａ・ｑ４ｔ／ｄ１２

すなわち、第４の関節２ｄをｑ４ｔ回転させて画像上の位置がｄ１２動いたので、ｄ２Ａだけ動かすための回転角度ｑ４はいくらかを求める。

所定値ｑ４の符号は、以下のルールにより決める。すなわち、（−１／ｋ１２−ｋ１）と（ｋ２−ｋ１）とが同符号ならば、ｑ４ｔと同じ方向とし、（−１／ｋ１２−ｋ１）と（ｋ２−ｋ１）とが異符号ならば、ｑ４ｔと逆の方向とする。

次に、Ｓ２０に進み、第５の関節２ｅについて、Ｓ１２からＳ１９までと同様の制御を行う。そして、カメラ６により撮像される画像における中心位置Ｃ０と、対象物１０１の位置（Ｐｘ，Ｐｙ）との差が一定以下の距離になれば、Ｓ２１に進み、センタリング動作を終了する。画像の中心位置Ｃ０と対象物１０１の位置（Ｐｘ，Ｐｙ）との差が一定以下になっていない場合には、Ｓ１２からＳ２０までの動作を再び行う。

前述のようにしてセンタリングが終了した場合には、次に、図２中の（ｂ）においてＳ７に進み、カメラ６を対象物１０１に接近させる動作を行う。

センタリングの終了により、図３中のフェーズ２に示すように、カメラ６のレンズの光軸方向に手先を移動させても、取得される画像上における対象物１０１の位置は、画像の中心位置に停止したままとなる。この状態で、カメラ６のレンズの光軸方向に、手先を移動させて対象物１０１に接近させる。

手先をカメラ６のレンズの光軸方向に移動させるためには、マニピュレータ１とカメラ６のレンズの光軸との位置関係が既知である必要がある。図１に示すように、光軸座標系はＴｃ、手先座標系はＴ６であり、これを変換する行列Ｔを事前に求めておけば、Ｔ・Ｔ６＝Ｔｃにより、光軸座標系Ｔｃを求めることができ、カメラ６のレンズの光軸方向（ｚｃ軸方向）に手先を移動させればよい。

ｚｃ軸方向ヘの移動量Ｌは、図４に示した撮像画像における長さＰＳと実際の物理長さＲＳとの関係により、以下のように求めることができる。
Ｌ＝Ｄｐｌ／（Ｋｘｐｌ・ＰＳ／cos（θ１））・ＲＳ

ただし、これは、ＫｘｐｌとＫｙｐｌとが等しい値である場合である。ＫｘｐｌとＫｙｐｌとが同じ値でない場合には、ＰＳのｘ軸成分とｙ軸成分を取り出し、これらをＰＳｘとＰＳｙとして、以下のようにして求める。
Ｌ＝Ｄｐｌ／〔√｛（Ｋｘｐｌ・ＰＳｘ）^２＋（Ｋｙｐｌ・ＰＳｙ）^２｝／cos（θ１）〕・ＲＳ

なお、θ１は、鉛直軸に対して手先軸ｚ６がどの程度傾いているかを示す角度であり、以下の式により得られる。
θ１＝acos（（ｚ６×ｚｃ）／（｜ｚ６｜・｜ｚｃ｜））

ここで、（ｚ６×ｚｃ）は、２つのベクトルｚ６とｚｃとの内積であり、｜ｚ６｜、｜ｚｃ｜は、ベクトルｚ６，ｚｃのノルム（長さ）である。acos（ａ）は、cos（θ）＝ａなるθであり、ただし、〔０≦θ≦１８０deg〕である。

これは、斜めから対象物１０１を見ることにより、見かけ上の大きさが小さく見えることを補正する操作である。なお、対象物１０１の形状が円であったり、または、円近似している場合には、最も長い径を求めれば、斜めから見ることによる影響を減らすことができるので、このようなcos（θ１）による補正を行う必要はない。

このようにして求められた距離Ｌに基づいて、カメラ６及び手先を、カメラ６のレンズの光軸ｚｃ方向に、対象物１０１に向けて移動させる。

次に、図２中の（ｂ）においてＳ８に進み、姿勢合わせを行う。この姿勢合わせは、図３中のフェーズ３に示すように、カメラ６のレンズの光軸を対象物１０１の計測面（上面）に垂直となるように手先の姿勢を制御する動作である。

例えば、計測面が床面と平行で、マニピュレータ１も計測面に平行な面上に置かれている場合において、手先軸ｚ６とカメラのレンズの光軸ｚｃとが平行である場合には、手先軸ｚ６が床面に垂直となるように、すなわち、カメラ６が真下を向くように、マニピュレータ１の手先の姿勢を制御する。

そして、図２中の（ｂ）においてＳ９に進み、２次元位置制御及び位相合わせを行う。

図７は、２次元位置制御及び位相合わせの手順を示すフローチャートである。

前述のように、姿勢合わせを行ったことにより、図３中のフェーズ４に示すように、対象物１０１の画像は、カメラ６により取得される画像の中心からずれている。しかし、計測面に対して、カメラ６のレンズの光軸は垂直になっている。したがって、この状態では、２次元画像フィードバックにより、位置及び位相を補正できる。ここで、位相とは、カメラ６の視野内における対象物１０１の画像の方向のことであり、マニピュレータの手先をカメラ６のレンズの光軸回りに回転させることによって補正される。

すなわち、図７のＳ２２において、２次元位置制御及び位相合わせ制御を開始すると、Ｓ２３に進み、画像上の対象物１０１の大きさＰＳと、画像処理により求めた大きさＰＳ１とを比較する。画像処理により求めた大きさＰＳ１のほうが小さければ、手先が対象物１０１に近づきすぎなので、対象物１０１から遠ざかる方向に手先を動作させ、ＰＳ１のほうが大きければ、手先が対象物１０１から遠すぎるので、対象物１０１に近づく方向に手先を動作させる。

Ｓ２４では、大きさＰＳと大きさＰＳ１との偏差が所定の値以下となっているかを判別し、所定の値以下となっていなければ、Ｓ２３に戻り、手先の移動操作を繰り返す。大きさＰＳと大きさＰＳ１との偏差が所定の値以下となっていれば、Ｓ２５に進む。

Ｓ２５では、画像処理により対象物１０１の位置（Ｐｘ，Ｐｙ）を求め、以下の式により、手先の移動量を決定する。
〔カメラのｘ軸方向の手先移動量〕＝Ｋｘｐｌ・（Ｐｘ−Ｘ０）
〔カメラのｙ軸方向の手先移動量〕＝Ｋｙｐｌ・（Ｐｙ−Ｙ０）

なお、実際の移動方向は、画像上における軸の向きと、手先の方向との関係から求める。そして、Ｓ２６に進み、中心位置の偏差（Ｐｘ−Ｘ０，Ｐｙ−Ｙ０）が所定の値以下になっているかを判別し、所定の値以下となっていなければ、Ｓ２５に戻り、手先の移動操作を繰り返す。中心位置の偏差（Ｐｘ−Ｘ０，Ｐｙ−Ｙ０）が所定の値以下となっていれば、Ｓ２７に進む。

Ｓ２７では、対象物１０１の位相θ１が目的の角度θとなるように、θ−θ１だけ第６の関節２ｆを回転操作する。そして、Ｓ２８に進み、２次元位置制御及び位相合わせ制御が完了する。すなわち、図２中の（ｂ）においてＳ１０に進み、制御動作が完了する。

ところで、前述の実施の形態においては、対象物１０１の形状を円形として説明している。しかし、対象物１０１の形状は、円形に限定されるわけではなく、対象物１０１の表面上に何らかの円形の特徴部分があり、画像処理により円形の部分が抽出できればよいのであり、例えば、外形は多角形だが、その中に円形の孔が空いているなどの形状でもよい。

本発明に係るロボット装置の構成を示す模式的な側面図である。 本発明に係るロボット装置の制御方法の事前準備（ａ）及び制御動作（ｂ）を示すフローチャートである。 本発明に係るロボット装置の制御方法を実行している状態における各段階に応じた撮像画像を示す正面図である。 本発明に係るロボット装置におけるカメラをピンホールカメラに模して表現した構成を示す斜視図である。 本発明に係るロボット装置の制御方法においてセンタリングを行っている状態における撮像画像を示す正面図である。 本発明に係るロボット装置の制御方法におけるセンタリング動作の手順を示すフローチャートである。 本発明に係るロボット装置の制御方法における２次元位置制御及び位相合わせの手順を示すフローチャートである。

符号の説明

１ マニピュレータ
１ａ 第１のリンク
１ｂ 第２のリンク
１ｃ 第３のリンク
１ｄ 第４のリンク
１ｅ 第５のリンク
１ｆ 第６のリンク
２ａ 第１の関節
２ｂ 第２の関節
２ｃ 第３の関節
２ｄ 第４の関節
２ｅ 第５の関節
２ｆ 第６の関節
３ コンピュータ
５ 先端ツール機構
６ カメラ
１０１ 対象物

Claims (10)

1. ６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置を制御するロボット装置の制御方法であって、
   前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を旋回させたときの前記対象物の画像の前記撮像手段における移動量に基づいて、前記対象物の画像が前記撮像手段の視野の中心に位置するように、前記マニピュレータの手先の方向を制御し、
   次に、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記撮像手段によって得られた対象物の画像の大きさに基づいて、前記マニピュレータの手先と前記対象物との間の距離を算出して、前記マニピュレータの手先を前記撮像手段のレンズの光軸方向に移動させ、該手先を前記対象物に接近させる
   ことを特徴とするロボット装置の制御方法。
2. 前記対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっている
   ことを特徴とする請求項１記載のロボット装置の制御方法。
3. 前記対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有している
   ことを特徴とする請求項１記載のロボット装置の制御方法。
4. 前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を、前記対象物の計測面に垂直な方向とする
   ことを特徴とする請求項１乃至請求項３のいずれか一に記載のロボット装置の制御方法。
5. 前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記対象物の画像が前記撮像手段の視野の中心に位置するように前記マニピュレータの手先の方向を制御し、
   次に、前記撮像手段の視野内における前記対象物の画像が所定の方向となるように、前記マニピュレータの手先を回転制御する
   ことを特徴とする請求項４記載のロボット装置の制御方法。
6. ６自由度以上のマニピュレータを有し、マニピュレータの手先位置に撮像手段を有するロボット装置であって、
   前記マニピュレータの動作を制御する制御手段を備え、
   前記制御手段は、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を旋回させたときの前記対象物の画像の前記撮像手段における移動量に基づいて、前記対象物の画像が前記撮像手段の視野の中心に位置するように、前記マニピュレータの手先の方向を制御し、次に、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記撮像手段によって得られた対象物の画像の大きさに基づいて、前記マニピュレータの手先と前記対象物との間の距離を算出して、前記マニピュレータの手先を前記撮像手段のレンズの光軸方向に移動させ、該手先を前記対象物に接近させる
   ことを特徴とするロボット装置。
7. 前記対象物は、円形、または、円形に近似された形状の特徴量を有し、上面が計測面となっている
   ことを特徴とする請求項６記載のロボット装置。
8. 前記対象物は、円形以外の任意の形状を有しており、円形、または、円形に近似された形状の特徴量を有する計測面を有している
   ことを特徴とする請求項６記載のロボット装置。
9. 前記制御手段は、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記マニピュレータの手先の方向を、前記対象物の計測面に垂直な方向とする
   ことを特徴とする請求項６乃至請求項８のいずれか一に記載のロボット装置。
10. 前記制御手段は、前記撮像手段によって得られる対象物の画像と前記対象物の形状についての事前に記憶された情報とを照合して、前記対象物の画像が前記撮像手段の視野の中心に位置するように前記マニピュレータの手先の方向を制御し、次に、前記撮像手段の視野内における前記対象物の画像が所定の方向となるように、前記マニピュレータの手先を回転制御する
    ことを特徴とする請求項９記載のロボット装置。

Images