(57)【特許請求の範囲】
【請求項１】
少なくとも１台の撮像装置によって移動機構で移動する対象物体を含むシーンを撮影し、
前記撮像装置で得られる撮像信号から対象物体の特徴部分を画像処理によって抽出し、
前記特徴部分の位置を、前記撮像装置の撮像面に固定された画像座標系上で出力し、
前記特徴部分の前記画像座標系上の位置に基づき、前記対象物体の前記画像座標系上の２次元速度ベクトルを求め、
前記画像座標系上の２次元速度ベクトルと前記移動機構によって認識される作業座標系上の２次元速度ベクトルとに基づいて２次元速度ベクトル座標変換マトリクスを局所的に推定し、
前記画像座標系上の目標点の位置と対象点の位置との差分と、ゲインマトリクスと、前記２次元速度ベクトル座標変換マトリクスとから、逐次作業座標系の前記２次元速度ベクトルを求め、
作業座標系上の前記２次元速度ベクトルに基づいて前記対象物体を前記画像座標系上の目標位置へ移動するように前記移動機構へ位置又は速度指令をフィードバックする
ことを特徴とする撮像装置を用いた機械装置のビジュアルフィードバック方法。
【請求項２】
前記２次元速度ベクトル座標変換マトリクスは、
前記移動機構の動作によって発生する画像座標系上での対象物体の特徴部分の２次元速度ベクトルと、該２次元速度ベクトルと１次独立の関係にある少なくとも一つの推定補助ベクトルとを利用することによって推定される
ことを特徴とする請求項１記載の撮像装置を用いた機械装置のビジュアルフィードバック方法。
【請求項３】
少なくとも１台の撮像装置によって移動機構で移動する対象物体を含むシーンを撮影し、
前記撮像装置で得られる撮像信号から対象物体の特徴部分を画像処理によって抽出し、
前記特徴部分を前記撮像装置の撮像面に固定された画像座標系上を追跡し、
前記画像座標系上での対象物体の目標位置と現在位置との偏差を前記移動機構によって認識される作業座標系に変換し、
変換後の偏差及び現在の速度指令に基づいて、予め設定された前記移動機構の速度の加減速パターンに対応する、新たな速度指令を計算する
ことを特徴とする撮像装置を用いた機械装置のビジュアルフィードバック方法。
【請求項４】
前記新たな速度指令を平滑化するためにフィルタ処理する
ことを特徴とする請求項３記載の撮像装置を用いた機械装置のビジュアルフィードバック方法。
【請求項５】
対象物体を移動させる移動機構と、
前記移動物体を含むシーンを撮影する撮像装置と、
前記撮像装置で得られる撮像信号から前記対象物体の特徴部分を画像処理によって抽出して、前記特徴部分の位置を、前記撮像装置の撮像面に固定された画像座標系上で出力するとともに、前記画像座標系上の前記特徴部分の位置に基づき、前記対象物体の前記画像座標系上の２次元速度ベクトルを求める画像処理手段と、
前記画像座標系上の２次元速度ベクトルと前記移動機構によって認識される作業座標系上の２次元速度ベクトルとに基づいて２次元速度ベクトル座標変換マトリクスを局所的に推定する変換マトリクス推定手段と、
前記画像座標系上における目標位置と対象物体の位置との差分と、ゲインマトリクスと、前記２次元速度ベクトル座標変換マトリクスとから、逐次作業座標系の前記２次元速度ベクトルを求める座標変換手段と、
作業座標系上の前記２次元速度ベクトルに基づいて前記対象物体を前記画像座標系上の目標位置へ移動するように前記移動機構へ位置又は速度指令をフィードバックする制御手段と、を備える
ことを特徴とする機械装置。
【請求項６】
前記変換マトリクス推定手段は、
２次元速度ベクトル座標変換マトリクスを、
前記移動機構の動作によって発生する画像座標系上での対象物体の特徴部分の２次元速度ベクトルと、該２次元速度ベクトルと１次独立の関係にある少なくとも一つの推定補助ベクトルとを利用することによって推定する
ことを特徴とする請求項５記載の機械装置。
【請求項７】
ロボットコントローラと、マニピュレータを撮影する撮像装置とを備え、前記撮像装置が撮影した画像データに基づいて前記マニピュレータを目標位置に移動させる機械装置において、
前記マニピュレータのロボットハンドが把持する対象物体を撮影して、その対象物体の画像座標系の位置ｆを出力する画像処理手段と、
前記ロボットコントローラから得られる前記ロボットハンドの作業座標系の現在位置Ｘと前記画像処理手段が出力する前記対象物体の画像座標系の位置ｆを入力して変換マトリクスＴを推定する変換マトリクス推定手段と、
前記目標位置と前記位置ｆの差に前記変換マトリクスＴを乗じて作業座標系の偏差ベクトルΔＸを求める座標変換器と、を備える
ことを特徴とする機械装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は２次元の画像信号に基づいて搬送装置の移動機構に対して好適な位置又は速度指令を、フィードバックするための撮像装置を用いた機械装置のビジュアルフィードバック方法およびその機械装置に関する。
【０００２】
【従来の技術】
従来、この種の撮像装置を用いた機械装置のビジュアルフィードバック方法は、ＩＴＶカメラなどで対象物体を２次元で撮影した画像データに基づいてロボットのマニピュレータなどの搬送装置の移動機構に対して位置又は速度指令をフィードバックする目的で用いられている。
この撮像装置を用いた機械装置のビジュアルフィードバック方法では、例えば、特公平８−２３８８９号公報（ロボットを用いた物体の取出し方法）に示されるように、撮影した画像データにおける撮影対象物体の特徴部分を、画像処理によって抽出して撮像面に固定された画像座標系上を追跡している。そして、対象物体を画像座標系上の目標位置へ移動するように移動機構に対して位置又は速度指令をフィードバックする。
このように撮像装置から出力される画像データに基づいて対象物体を移動させるなどの操作を実行する場合、撮像装置の撮像面に固定された画像座標系と移動機構によって認識できる作業座標系との相対位置、及び、姿勢関係と撮像装置が有する内部パラメータを予め求めておく必要がある。
また、特開平８−７１９７２号公報（ロボットとカメラの自動調整方法）に示されるように、画像座標系と作業座標系との関係及び内部パラメータを求める場合、人手によって正確に計測する、あるいは、測定のための専用の物体を使用する等を行う。換言すれば、画像座標系と作業座標系との関係及び内部パラメータとを求める場合、精密なキャリブレーションを実行して測定している。
図８は従来のビジュアルフィードバック方法による移動機構制御装置の構成を示している。
図８に示した構成は、ＣＣＤカメラ３０１と、ロボットコントローラ３０２と、ロボットマニピュレータ３０３と、撮像シーン３０４と、画像処理装置３０５と、ヤコビ行列推定器３０６と、速度指令生成器３０７とを有し、ロボットマニピュレータ３０３のロボットハンドを目標位置へ移動させるものである。
ここで、ＣＣＤカメラ３０１は撮像シーン３０４のようにロボットマニピュレータ３０３のロボットハンドの移動範囲を撮像し、この撮像信号を画像処理装置３０５へ出力する。
画像処理装置３０５は、ロボットマニピュレータ３０３のロボットハンドの画像座標系上での特徴部分を追跡し、ロボットマニピュレータ３０３のロボットハンドの画像座標系上での特徴部分の現在位置ｆを出力する。ロボットマニピュレータ３０３のロボットハンドの画像座標系上での特徴部分の目標位置ｆd は予め与えられている。
ヤコビ行列推定器３０６は、ロボットコントローラ３０２から得られるロボットハンドの作業座標系上での現在位置Ｘと、画像処理装置３０５から得られるロボットハンドの画像座標系上での特徴部分の現在位置ｆとを入力し、式（１）を満たすヤコビ行列Ｊを推定する（細田、浅田：「構造やパラメータに関する先験的な知識を必要としないフィードフォワード補償器を持つ適応型ビジュアルサーボ系の構成」、日本ロボット学会誌１４巻２号参照）。
df/dt ＝ＪdX/dt                                 …（１）
ここで、df/dt はロボットハンドの画像座標系上での特徴部分の速度ベクトル、dX/dt はロボットハンドの作業座標系上での特徴部分の速度ベクトルである。
速度指令生成器３０７は、式（２）によって速度指令を生成する。
dX/dt ＝ＫＪ-1（ｆd −ｆ）                      …（２）
式（２）によってロボットコントローラ３０２に供給する速度指令dX/dt を生成する。ここで、Ｋはゲイン行列である。
図９はロボットコントローラ３０２にフィードバックされる速度の加減速パターンを示している。
図９に示すように、縦軸を速度dX/dt 、横軸を時間ｔとする速度の加減速パターンがロボットコントローラ３０２にフィードバックされる。
【０００３】
【発明が解決しようとする課題】
このような上記従来例の撮像装置を用いた機械装置のビジュアルフィードバック方法では、キャリブレーション精度が、対象物体の特徴部分の画像座標系での位置と作業座標系での位置との対応に直接影響するため、画像座標系及び作業座標系との相対的な位置及び姿勢関係と撮像装置が有する内部パラメータを精密に求める必要がある。この場合、多大な処理時間が必要になるという欠点がある。また、精密なキャリブレーションを実行した後に、作業座標系と画像座標系との相対的な位置及び姿勢関係に、例えば、撮像装置を動かすなどの理由で、その微妙な変化が生じた場合には、煩雑な処理のキャリブレーションをやり直す必要がある。同様に撮像装置が有する内部パラメータに、例えば、レンズの焦点距離を変更するなどの理由で、その微妙な変化が生じた場合も、煩雑な処理のキャリブレーションをやり直す必要があるという不都合がある。
さらに、図８及び図９に示した構成では、ロボットコントローラにフィードバックされる速度指令が初期で大きくなり、制御対象のロボットが急激に動いてしまうという欠点がある。
本発明は、このような従来の技術における課題を解決するものであり、煩雑なキャリブレーションを必要とせずに、画像座標系上の対象物体の特徴部分の動き（２次元速度ベクトル）を作業座標系上の対象物体の特徴部分の動きに変換する際に必要となる局所的な２次元速度ベクトル座標変換マトリクスを、作業座標系上の対象物体の動きから推定するパラメータ数を低減して、簡便に推定できる撮像装置を用いた機械装置のビジュアルフィードバック方法の提供を目的とする。さらに、制御対象の移動機構（ロボット）が急激に動いてしまうことのない速度の加減速パターンを作成して、その制御対象の移動機構を滑らかに作動させることを目的とする。
【０００４】
【課題を解決するための手段】
本発明の目的は、下記構成により達成される。
（１）  少なくとも１台の撮像装置によって移動機構で移動する対象物体を含むシーンを撮影し、前記撮像装置で得られる撮像信号から対象物体の特徴部分を画像処理によって抽出し、前記特徴部分の位置を、前記撮像装置の撮像面に固定された画像座標系上で出力し、前記特徴部分の前記画像座標系上の位置に基づき、前記対象物体の前記画像座標系上の２次元速度ベクトルを求め、前記画像座標系上の２次元速度ベクトルと前記移動機構によって認識される作業座標系上の２次元速度ベクトルとに基づいて２次元速度ベクトル座標変換マトリクスを局所的に推定し、前記画像座標系上の目標点の位置と対象点の位置との差分と、ゲインマトリクスと、前記２次元速度ベクトル座標変換マトリクスとから、逐次作業座標系の前記２次元速度ベクトルを求め、作業座標系上の前記２次元速度ベクトルに基づいて前記対象物体を前記画像座標系上の目標位置へ移動するように前記移動機構へ位置又は速度指令をフィードバックすることを特徴とする撮像装置を用いた機械装置のビジュアルフィードバック方法。
（２）  前記２次元速度ベクトル座標変換マトリクスは、前記移動機構の動作によって発生する画像座標系上での対象物体の特徴部分の２次元速度ベクトルと、該２次元速度ベクトルと１次独立の関係にある少なくとも一つの推定補助ベクトルとを利用することによって推定されることを特徴とする上記（１）記載の撮像装置を用いた機械装置のビジュアルフィードバック方法。
（３）  少なくとも１台の撮像装置によって移動機構で移動する対象物体を含むシーンを撮影し、前記撮像装置で得られる撮像信号から対象物体の特徴部分を画像処理によって抽出し、前記特徴部分を前記撮像装置の撮像面に固定された画像座標系上を追跡し、前記画像座標系上での対象物体の目標位置と現在位置との偏差を前記移動機構によって認識される前記作業座標系に変換し、変換後の偏差及び現在の速度指令に基づいて、予め設定された前記移動機構の速度の加減速パターンに対応する、新たな速度指令を計算することを特徴とする撮像装置を用いた機械装置のビジュアルフィードバック方法。
（４）  前記新たな速度指令を平滑化するためにフィルタ処理することを特徴とする上記（３）記載の撮像装置を用いた機械装置のビジュアルフィードバック方法。
（５）  対象物体を移動させる移動機構と、前記移動物体を含むシーンを撮影する撮像装置と、前記撮像装置で得られる撮像信号から前記対象物体の特徴部分を画像処理によって抽出して、前記特徴部分の位置を、前記撮像装置の撮像面に固定された画像座標系上で出力するとともに、前記画像座標系上の前記特徴部分の位置に基づき、前記対象物体の前記画像座標系上の２次元速度ベクトルを求める画像処理手段と、前記画像座標系上の２次元速度ベクトルと前記移動機構によって認識される作業座標系上の２次元速度ベクトルとに基づいて２次元速度ベクトル座標変換マトリクスを局所的に推定する変換マトリクス推定手段と、前記画像座標系上における目標位置と対象物体の位置との差分と、ゲインマトリクスと、前記２次元速度ベクトル座標変換マトリクスとから、逐次作業座標系の前記２次元速度ベクトルを求める座標変換手段と、作業座標系上の前記２次元速度ベクトルに基づいて前記対象物体を前記画像座標系上の目標位置へ移動するように前記移動機構へ位置又は速度指令をフィードバックする制御手段と、を備えることを特徴とする機械装置。
（６）  前記変換マトリクス推定手段は、２次元速度ベクトル座標変換マトリクスを、前記移動機構の動作によって発生する画像座標系上での対象物体の特徴部分の２次元速度ベクトルと、該２次元速度ベクトルと１次独立の関係にある少なくとも一つの推定補助ベクトルとを利用することによって推定することを特徴とする上記（５）記載の機械装置。
（７）  ロボットコントローラと、マニピュレータを撮影する撮像装置とを備え、前記撮像装置が撮影した画像データに基づいて前記マニピュレータを目標位置に移動させる機械装置において、前記マニピュレータのロボットハンドが把持する対象物体を撮影して、その対象物体の画像座標系の位置ｆを出力する画像処理手段と、前記ロボットコントローラから得られる前記ロボットハンドの作業座標系の現在位置Ｘと前記画像処理手段が出力する前記対象物体の画像座標系の位置ｆを入力して変換マトリクスＴを推定する変換マトリクス推定手段と、前記目標位置と前記位置ｆの差に前記変換マトリクスＴを乗じて作業座標系の偏差ベクトルΔＸを求める座標変換器と、を備えることを特徴とする機械装置。
上記（１）、（２）、（５）、（６）、（７）の構成では、画像座標系上の２次元速度ベクトルと作業座標系上の２次元速度ベクトルとを対応づける２次元速度ベクトル座標変換マトリクスを局所的に推定しているため、煩雑なキャリブレーションが不要になる。さらに、画像座標系上及び作業座標系上の２次元速度ベクトルの対応をマトリクス表現しているため、画像座標系上及び作業座標系上の位置の対応のマトリクス表現に比較して、画像座標系上の対象物体の特徴部分の動き（２次元速度ベクトル）を作業座標系上の対象物体の特徴部分の動きに変換する際に必要となる局所的な２次元速度ベクトル座標変換マトリクスが、作業座標系上の対象物体の動きから簡便、すなわち、推定するパラメータ数が低減する少ない演算量で推定される。同様にして画像座標系上の情報から作業座標系の情報を求める際にも演算量が少なくなる。
上記（３）、（４）の構成は、偏差及び現在の速度指令に基づいて、予め設定された移動機構の速度の加減速パターンに対応する、新たな速度指令を計算するものであり、ロボットなどの移動機構が初期に急激に動くことなく、滑らかに動作する。
【０００５】
【発明の実施の形態】
以下、本発明の実施の形態を図面を参照して説明する。
図１は本発明の撮像装置を用いた機械装置のビジュアルフィードバック方法を実現するための撮像システムの構成図である。図１に示す例は制御によって搬送作業するロボットマニピュレータ２１と、このロボットマニピュレータ２１の先端に取り付けられたロボットハンド２２と、対象物体のペグ２３とを有している。  また、この例はワーク２４を有し、このワーク２４には貫通するホール２５が設けられている。更に、撮像装置としてのＣＣＤカメラ２６を有している。また、ＣＣＤカメラ２６で撮影した撮像シーン２７が画面表示されるとともに、ＣＣＤカメラ２６からの画像データを処理する画像処理装置２８を有している。また、対象物体のペグ２３の特徴部分２９が図示されている。
次に、図１に示す構成の動作及び機能について説明する。
図１において、ペグ２３をホール２５に挿入するペグインホールを実行する。まず、ロボットマニピュレータ２１に接続されたロボットハンド２２によってペグ２３を挟み込んで保持する。ロボットマニピュレータ２１への位置又は速度指令に基づいてロボットマニピュレータ２１が動作して、ペグ２３が移動する。
一方、ＣＣＤカメラ２６がロボットマニピュレータ２１、ロボットハンド２２、ペグ２３、ワーク２４及びホール２５を含むシーンを撮影する。このシーンの画像データがＣＣＤカメラ２６から画像処理装置２８に送出される。画像処理装置２８では、ペグ２３の特徴部分２９を画像上で追跡し、この特徴部分２９の動きがロボットマニピュレータ２１の動きに変換されて、ロボットマニピュレータ２１の位置又は速度指令としてフィードバックされる。ここで必要となるのは、撮像シーン２７におけるペグ２３の特徴部分２９の動きを、ロボットマニピュレータ２１の動きに変換することである。
図２は、この実施形態で処理される座標系を説明するための図である。
図２に示す座標系は、画像座標系３１及び作業座標系３２を有し、画像座標系３１は焦点距離（ｆ）３４と、焦点３５と、撮像面３６と、投影点３７及び光軸３８からなる。更に、対象点（Ｐ）３３を有している。
図１及び図２において、画像座標系３１は撮像装置であるＣＣＤカメラ２６の撮像面３６に固定される。作業座標系３２はロボットマニピュレータ２１が認識できる空間上に固定される。対象点（Ｐ）３３は、空間上の点でペグ２３の特徴部分２９の中心付近に相当し、ＣＣＤカメラ２６の焦点３５と対象点（Ｐ）３３とからなる直線と、撮像面３６との交点（投影点３７）に投影される。
ＣＣＤカメラ２６から出力される画像から得られる追跡対象である対象点（Ｐ）３３の投影点３７の位置情報に基づいて、ロボットマニピュレータ２１の動きを決定するためには、画像座標系３１とロボットマニピュレータ２１の認識できる作業座標系３２との位置、及び、姿勢関係とＣＣＤカメラ２６の内部パラメータを求める必要がある。例えば、焦点距離（ｆ）３４を求める必要がある。一般的に、画像座標系３１と作業座標系３２との位置、及び、姿勢関係とＣＣＤカメラ２６の内部パラメータとは式（３）で表される。
【０００６】
【数１】
0003981778.tif 000002
【０００７】
ここでｓは同次座標値であり、ｘ，ｙ，ｚは作業座標系３２で表現した対象点（Ｐ）３３の座標値（Ｘ）である。また、ｕ，ｖは投影点３７の画像座標系３１の座標値（ｆ）であり、ｍ11〜ｍ43は作業座標系３２と画像座標系３１との位置及び姿勢関係とＣＣＤカメラ２６の内部パラメータを関係づける座標変換マトリクスの各パラメータである。
対象点（Ｐ）３３の動きを拘束する平面の方程式が以下の式（４）で表される。
ｚ＝０                                            …（４）
この式（４）によって式（３）は式（５）で表される。
【０００８】
【数２】
0003981778.tif 000003
【０００９】
ここでｎ11〜ｎ33は対象点（Ｐ）３３が式（４）で表される拘束平面上のみを動く場合の画像座標系３１と作業座標系３２との位置及び姿勢関係とＣＣＤカメラ２６の内部パラメータとを関係付ける座標変換マトリクスの各パラメータである。
なお、ここでは式（４）に限定したが、一般的に、ある拘束平面上のみを動く場合は、作業座標系３２の位置及び姿勢を考慮することによって式（４）に示す制限に置換できる。また、ここでは式（５）で表されるように画像座標系３１上の２次元情報から作業座標系３２上の２次元情報への変換に限定する。
式（５）において、画像座標系３１上の微少移動変化量に対する作業座標系３２上の微少移動変化量の関係が式（６）で表される。
【００１０】
【数３】
0003981778.tif 000004
【００１１】
ここでΔｘ，Δｙは作業座標系３２上の微少移動変化量であり、Δｕ，Δｖは画像座標系３１上の微少移動変化量である。ａ11〜ａ22は画像座標系３１上の微少移動変化量と作業座標系３２上の微少移動変化量とを対応付ける座標変換マトリクスの各パラメータであり、それぞれｕ，ｖの関数である。なお、（Δｘ，Δｙ）を作業座標系３２上の２次元速度ベクトル（dX/dt ）とし、かつ、（Δｕ，Δｖ）を画像座標系３１上の２次元速度ベクトル（df/dt ）とし、かつ、式（６）の２×２マトリクスを２次元速度ベクトル座標変換マトリクス（Ｔ）とする。図３は画像座標系上の対象点の推移と作業座標系上の対象点の推移を示す図である。図３（ａ）は画像座標系４１であり、図３（ｂ）は作業座標系４２である。図３（ａ）の画像座標系４１はｉ−２番目の画像座標系４１上の対象点４３と、ｉ−１番目の画像座標系４１上の対象点４４と、ｉ番目の画像座標系４１上の対象点４５と、画像座標系４１上の目標点４６、及び、推定補助点４７とを有している。また、図３（ｂ）の作業座標系４２は、ｉ−２番目の作業座標系４２上の対象点４８と、ｉ−１番目の作業座標系４２上の対象点４９、及び、ｉ番目の作業座標系４２上の対象点５０及び作業座標系４２上の目標点５１とを有している。
ここではｉ番目の画像座標系４１上の対象点４５とｉ−１番目の画像座標系４１上の対象点４４との差分（２次元速度ベクトル）を（Δｕi-1 ，Δｖi-1 ）とし、かつ、ｉ−１番目の画像座標系４１上の対象点４４とｉ−２番目の画像座標系４１上の対象点４３との差分（２次元速度ベクトル）を（Δｕi-2 ，Δｖi-2 ）とする。更に、推定補助点４７とｉ−１番目の画像座標系４１上の対象点４４との差分（推定補助ベクトル）を（Δｕi-1'，Δｖi-1'）とする。
また、ｉ番目の作業座標系４２上の対象点５０とｉ−１番目の作業座標系４２上の対象点４９との差分（２次元速度ベクトル）を（Δｘi-1 ，Δｙi-1 ）とし、かつ、ｉ−１番目の作業座標系４２上の対象点４９とｉ−２番目の作業座標系４２上の対象点４８との差分（２次元速度ベクトル）を（Δｘi-2 ，Δｙi-2 ）とする。ここで画像座標系４１上の２次元速度ベクトル（Δｕi-1 ，Δｖi-1 ）と画像座標系４１上の推定補助ベクトル（Δｕi-1'，Δｖi-1'）とは垂直の関係にある。
ここでロボットマニピュレータ２１の動作によって、ペグ２３が移動（対象点（Ｐ）３３が移動）し、作業座標系４２のｉ−１番目の作業座標系４２上の対象点４９の位置を通過する。そして、ｉ番目の作業座標系４２上の対象点５０の位置に移動した場合、この対象点（Ｐ）３３の移動に伴って、対象点（Ｐ）３３は画像座標系４１のｉ−１番目の画像座標系４１上の対象点４４の位置を通過してｉ番目の画像座標系４１上の対象点４５の位置に移動する。
対象点（Ｐ）３３の移動目標点は、画像座標系４１上では画像座標系４１上の目標点４６であり、また、作業座標系４２上では作業座標系４２上の目標点５１である。この対象点（Ｐ）３３の移動目標点は、画像座標系４１では認識できるが、ロボットマニピュレータ２１が、画像座標系４１から得られる情報に基づいた位置又は速度指令に従って動作するため、作業座標系４２では認識できない。ここでは画像座標系４１上の２次元速度ベクトルと作業座標系４２上の２次元速度ベクトルとを対応づける２次元速度ベクトル座標変換マトリクス（Ｔ）の各パラメータの推定を考える。対象点（Ｐ）３３が、ｉ−１番目の作業座標系４２上の対象点４９からｉ番目の作業座標系４２上の対象点５０まで移動する際の２次元速度ベクトル（Δｘi-1 ，Δｙi-1 ）は、前回推定して求めた２次元速度ベクトル座標変換マトリクスＴi-1 を用いて式（６）から式（７）で求めることができる。
【００１２】
【数４】
0003981778.tif 000005
【００１３】
この式（７）で利用した２次元速度ベクトル座標変換マトリクスＴi-1 を用いて、推定補助ベクトル（Δｕi-1'，Δｖi-1'）に対応する作業座標系４２上の推定補助ベクトル（Δｘi-1'，Δｙi-1'）を求めると式（６）から式（８）となる。
【００１４】
【数５】
0003981778.tif 000006
【００１５】
ここで式（６）を変形し、画像座標系４１上の２次元速度ベクトル（Δｕi-1 ，Δｖi-1 ）と、画像座標系４１上の推定補助ベクトル（Δｕi-1'，Δｖi-1'）と、作業座標系４２上の２次元速度ベクトル（Δｘi-1 ，Δｙi-1 ）と、作業座標系４２上の推定補助ベクトル（Δｕi-1'，Δｖi-1'）を代入すると式（９）で表すことができる。
【００１６】
【数６】
0003981778.tif 000007
【００１７】
式（９）における左辺の４×４マトリクスは、逆マトリクスが存在するため、２次元速度ベクトル座標変換マトリクスＴi の各パラメータを求めることができる。ここで求めた２次元速度ベクトル座標変換マトリクスＴi を用いて式（６）から次の作業座標系４２上の２次元速度ベクトルを求める。
なお、画像座標系４１上の推定補助ベクトル（Δｕi-1'，Δｖi-1'）では、画像座標系４１上の２次元速度ベクトル（Δｕi-1 ，Δｖi-1 ）に対して垂直としているが、必ずしも垂直である必要はない。例えば、２次元速度ベクトル（Δｕi-1 ，Δｖi-1 ）と一次独立の関係にある２次元速度ベクトルを利用すれば良い。また、推定補助ベクトルの利用は一つに限定されず二つを利用しても良い。例えば、画像座標系４１上の２次元速度ベクトル（Δｕi-1 ，Δｖi-1 ）に対して垂直な二つの２次元速度ベクトルを利用しても良い。この場合、一つの推定補助ベクトルが画像座標系４１上の２次元速度ベクトル（Δｕi-1 ，Δｖi-1 ）に対して１次独立であれば良い。
また、この実施形態では式（６）を基本にしてビジュアルフィードバックを実行しているが、式（５）に基づいたビジュアルフィードバック処理の実現も考えられる。しかしながら式（６）に基づいた方式は式（５）に基づいた方式と比較すると、推定するマトリクスのパラメータの数が少ないため、式（９）に基づくマトリクス推定に必要となる演算量を少なくすることができる。更に、画像座標系４１上の対象点の情報から作業座標系４２上の対象点の情報を求める際にも式（６）、式（５）の比較によって、式（６）に基づいた処理が式（５）に基づいた処理よりも、その演算量を少なくすることができる。
図４は、ビジュアルフィードバックの処理手順を示すフローチャートである。図４において、ビジュアルフィードバックの開始時にあって（Ｓ１０１）、作業座標系上の対象点（図１におけるペグ２３の特徴部分２９の中心に相当）の位置（ｘ，ｙ）は、ロボットマニピュレータ２１が作業座標系を認識できることから測定できものであり、この画像座標系上の対象点の位置（ｕ，ｖ）及び画像座標系上の目標点（図１におけるホール２５に相当）の位置（ｕd ，ｖd ）は、ＣＣＤカメラ２６が対象点及び目標点を含むシーンを撮像していることから測定できる。
次に、画像座標系上の対象点の位置（ｕ，ｖ）、画像座標系上の目標点の位置（ｕd ，ｖd ）、作業座標系の対象点の位置（ｘ，ｙ）をそれぞれ測定する（Ｓ１０２）。
その後、画像座標系上の目標点の位置（ｕd ，ｖd ）と画像座標系上の対象点の位置（ｕ，ｖ）との差分（ｅu ，ｅv ）を演算し（Ｓ１０３）、この差分（ｅu ，ｅv ）と、ある閾値（Ｓu ，Ｓv ）とを比較する（Ｓ１０４）。
そして、差分（ｅu ，ｅv ）が、所定の閾値（Ｓu ，Ｓv ）より小さい場合は（Ｓ１０４：Ｙｅｓ）、ビジュアルフィードバックを終了し（Ｓ１０９）、大きい場合は（Ｓ１０４：Ｎｏ）、次の処理Ｓ１０５に移る。この場合、Ｓ１０９の終了に推移する条件は、ｅu がＳu よりも小さく、かつ、ｅv がＳv よりも小さい場合、又は、ｅu がＳu よりも小さく、あるいは、ｅv がＳv よりも小さい場合のいずれでも良い。Ｓ１０５では、式（１０）に基づいて作業座標系上の２次元速度ベクトル（Δｘ，Δｙ）を求める。
【００１８】
【数７】
0003981778.tif 000008
【００１９】
ここで、Ｔは２次元速度ベクトル座標変換マトリクスであり、Ｋは、ゲインマトリクスである。なお、ビジュアルフィードバックの処理の初回は、２次元速度ベクトル座標変換マトリクスＴが求められていないため、人手によって適当な２次元速度ベクトル座標変換マトリクスＴの各パラメータの値を指定する、ロボットマニピュレータ２１を適当に移動させて２次元速度ベクトル座標変換マトリクスＴの各パラメータ値を求めておくなどの処理で求めた２次元速度ベクトル座標変換マトリクスＴを用いる。
このようにして求められた作業座標系上の２次元速度ベクトル（Δｘ，Δｙ）に基づいてロボットマニピュレータ２１を操作して対象点を移動する。この移動した対象点を画像座標系上で追跡し、実際の移動量、すなわち、画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）を測定する（Ｓ１０６）。その後、画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）と１次独立の関係にある推定補助ベクトル（Δｕ’，Δｖ’）、例えば、画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）と垂直なベクトルを求め式（８）に基づいて、作業座標系上の推定補助ベクトル（Δｘ’，Δｙ’）を演算する（Ｓ１０７）。
そして、求められた画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）と、画像座標系上の推定補助ベクトル（Δｕ’，Δｖ’）と、作業座標系上の２次元速度ベクトル（Δｘ，Δｙ）と、作業座標系上の推定補助ベクトル（Δｘ’，Δｙ’）とを用いて式（９）から２次元速度ベクトル座標変換マトリクスＴの各パラメータを推定する（Ｓ１０８）。Ｓ１０８の後はＳ１０２に推移し、処理を繰り返す。
なお、この実施形態では、ビジュアルフィードバック中に、画像座標系と作業座標系との相対的な位置及び姿勢関係に変化が生じた場合、また、撮像装置の持つ内部パラメータに変化が生じた場合、あるいは、目標点が変更になった場合、その変化又は変更が多少であれば、目標点の位置（ｕd ，ｖd ）を変更し、又は、逐次、２次元速度ベクトル座標変換マトリクスを推定しているため、そのままビジュアルフィードバックを継続することができる。
また、一度、ビジュアルフィードバック処理を終了し、再度、ビジュアルフィードバック処理を実行する前に、画像座標系と作業座標系との相対的な位置及び姿勢関係に変化が生じた場合、また、撮像装置が有する内部パラメータに変化が生じた際にも、その変化が多少であれば、逐次、２次元速度ベクトル座標変換マトリクスを推定していることから、そのままビジュアルフィードバックを継続することができる。
図５は、図４の処理手順によるビジュアルフィードバック処理に基づいたシミュレーションを示す図である。図５（ａ）は画像座標系６１であり、図５（ｂ）は、作業座標系６２である。この画像座標系６１及び作業座標系６２には、画像座標系６１上の開始点６３と、画像座標系６１上の目標点６４と、作業座標系６２上の開始点６５と、作業座標系６２上の目標点６６とを有している。更に、この実施形態のビジュアルフィードバック処理に基づいて推移する画像座標系６１上の途中の特徴点６７，６８、及び作業座標系６２上の途中の特徴点６９，７０を有している。
ここでは式（１０）におけるゲインマトリクスＫを以下のように設定している。
【００２０】
【数８】
0003981778.tif 000009
【００２１】
図４に示したビジュアルフィードバック処理を用いて、開始時に作業座標系６２上の開始点６５及び画像座標系６１上の開始点６３にあった対象点が、画像座標系６１では、画像座標系６１上の途中の特徴点６７，６８を通じて画像座標系６１上の目標点６４に収束していることが判明する。また、作業座標系６２では、作業座標系６２上の途中の特徴点６９，７０を通じて作業座標系６２上の目標点６６に収束していることが判明する。
図６はビジュアルフィードバックによる移動機構制御装置の構成を示している。
図６に示す構成は、ＣＣＤカメラ１０１と、ロボットコントローラ１０２と、ロボットマニピュレータ１０３と、撮像シーン１０４と、画像処理装置１０５と、変換マトリクス推定器１０６と、座標変換器１０７と、対象物体１０８と、速度指令調節器１０９と、フィルタ処理部１１０と、物体モデル１１１とを有している。
次に、このビジュアルフィードバックによる移動機構制御装置の動作について説明する。
ロボットマニピュレータ１０３のロボットハンドは対象物体１０８を把持して、対象物体１０８を目標位置へ移動させる。ＣＣＤカメラ１０１は撮像シーン１０４のように対象物体１０８の移動範囲を撮像し、画像データを画像処理装置１０５へ出力する。画像処理装置１０５では対象物体１０８の画像座標系上での特徴部分を追跡し、その現在位置ｆ（＝（ｕ，ｖ））を出力する。
なお、対象物体１０８の画像座標系上での特徴部分の目標位置ｆd （＝（ｕd ，ｖd ））は予め与えられている。変換マトリクス推定器１０６は、ロボットコントローラ１０２から得られるロボットハンドの作業座標系上での現在位置と、対象物体１０８の物体モデル１１１との和、すなわち、対象物体１０８の作業座標系上での特徴部分の現在位置Ｘと、画像処理装置１０５から得られる対象物体１０８の画像座標系上での特徴部分の現在位置ｆとを入力し、式（１２）の関係を満たす変換マトリクスＴを推定する。
dX/dt ＝Ｔdf/dt                         …（１２）
ここでdX/dt は対象物体１０８の作業座標系上での特徴部分の速度ベクトルであり、df/dt は対象物体１０８の画像座標系上での特徴部分の速度ベクトルである。座標変換器１０７は、式（１３）によって対象物体の作業座標系上での偏差ベクトルΔＸを生成する。
ΔＸ＝Ｔ（ｆd −ｆ）                          …（１３）
速度指令調節器１０９は、作業座標系上での偏差ベクトルと現在の速度指令ベクトルから、ロボットマニピュレータ１０３の動作が滑らかになるように予め設定された速度の加減速パターンに対応するように新たな速度指令を生成する。
フィルタ処理部１１０は、急激な速度変化がロボットマニピュレータ１０３に生じないよう、速度指令調節器１０９で生成した速度指令を滑らかにする。
図７は速度指令調節器１０９の動作を説明するための図である。
図７に示す例は、第１のケース（ａ）、第２のケース（ｂ）、第３のケース（ｃ）であり、かつ、速度２０１、時間２０２、加速パターン２０３、減速パターン２０４、加速時間２０５、減速時間２０６、リミット速度２０７、第１の点２０８、第２の点２０９、第３の点２１０からなる。
速度指令調節器１０９には、加速時間２０５と減速時間２０６とリミット速度２０７が予め設定されている。ここで第１のケース（ａ）に示されるように、現在の速度指令が第１の点２０８における速度の場合には式（１３）によって求めた偏差ベクトルΔＸと第１のケース（ａ）の図中における斜線部の面積が等しくなるように第１の点２０８の位置を決定する。第１の点２０８からは加速パターン２０３と傾きが等しくなるように新たに速度指令を決定し、リミット速度２０７となるまで繰り返し行う。
また、第２のケース（ｂ）に示すように、現在の速度指令が第２の点２０９における速度の場合には、式（１３）によって求めた偏差ベクトルΔＸと第２のケース（ｂ）の図中における斜線部の面積が等しくなるように第２の点２０９の位置を決定する。第２の点２０９からは減速パターン２０４と傾きが等しくなるように新たに速度指令を決定し、リミット速度２０７となるまで繰り返し行う。更に、第３のケース（ｃ）に示すように、現在の速度指令が第３の点２１０における速度であり、式（１３）によって求めた偏差ベクトルΔＸが第３のケース（ｃ）における図中の斜線部の場合は、リミット速度２０７までは到達しない以外は、第１のケース（ａ）と同一の処理である。なお、式（１３）における偏差ベクトルΔＸはビジュアルフィードバックのサンプリング間隔Δｔで毎回求められる。したがって、以上の処理もΔｔ時間で行われ、設定した加減速パターンに従うものとなる。
フィルタ処理部１１０は、速度指令調節器１０９で作成された新たな速度指令を滑らかにするために、例えば、式（１４）に示される特性の１次フィルタを用いれば良い。
Ｖ（ｋ）＝Ｈ｛dX/dt （ｋ）−Ｖ（ｋ−１）｝＋Ｖ（ｋ−１）…（１４）
ここで、Ｖ（ｋ）はサンプリング時間ｋにおけるフィルタ出力、Ｈはゲイン行列である。
【００２２】
【発明の効果】
以上の説明から明らかなように、請求項１、５および７記載の撮像装置を用いた機械装置のビジュアルフィードバック方法によれば、画像座標系上の２次元速度ベクトルと作業座標系上の２次元速度ベクトルとを対応づける２次元速度ベクトル座標変換マトリクスを推定しているため、煩雑なキャリブレーションを不要にできる。さらに、画像座標系上及び作業座標系上の２次元速度ベクトルの対応をマトリクス表現しているため、画像座標系上の対象物体の特徴部分の動き（２次元速度ベクトル）を作業座標系上の対象物体の特徴部分の動きに変換する際に必要となる局所的な２次元速度ベクトルの座標変換マトリクスが、作業座標系上の対象物体の動きから簡便に推定できるようになる。同様にして画像座標系上の情報から作業座標系の情報を求める際にも演算量を少なくできる。
本発明の請求項２および６記載の撮像装置を用いた機械装置のビジュアルフィードバック方法によれば、逐次、２次元速度ベクトル座標変換マトリクスを推定しているため、画像座標系と作業座標系との相対的な位置及び姿勢関係に変化が生じた場合、また、撮像装置が有する内部パラメータに変化が生じた場合、それらが多少の変化の際に、例えば、変化がビジュアルフィードバック処理中、又は、この処理間であっても、ビジュアルフィードバック処理が実行できるようになる。本発明の請求項３及び４記載の撮像装置を用いた機械装置のビジュアルフィードバック方法によれば、偏差ベクトルと現在の速度指令から、予め設定された移動機構の速度の加減速パターンに基づくように、新しい速度指令を計算しており、移動機構が初期に急激に動くことなく、動作が滑らかになる。
【図面の簡単な説明】
【図１】本発明の撮像装置を用いた機械装置のビジュアルフィードバック方法を実現するための撮像システムの構成図である。
【図２】実施形態で処理される座標系を説明するための図である。
【図３】実施形態にあって画像座標系上の対象点の推移と作業座標系上の対象点の推移を示す図である。
【図４】実施形態にあってビジュアルフィードバックの処理手順を示すフローチャートである。
【図５】実施形態にあってビジュアルフィードバック処理に基づいたシミュレーションを示す図である。
【図６】実施形態にあってビジュアルフィードバックによる移動機構制御装置の構成を示すブロック図である。
【図７】実施形態にあって速度指令調節器の動作を説明するための図である。
【図８】従来のビジュアルフィードバック方法による移動機構制御装置の構成を示すブロック図である。
【図９】従来例にあってフィードバックされる速度の加減速パターンである。
【符号の説明】
２１、１０３、３０３ ロボットマニピュレータ（移動機構）
２２ ロボットハンド
２３ ペグ（対象物体）
２４ ワーク
２５ ホール
２６、１０１、３０１ ＣＣＤカメラ（撮像装置）
２７、１０４、３０４ 撮像シーン
２８、１０５、３０５ 画像処理装置（画像処理手段）
２９ ペグの特徴部分
３１、４１、６１ 画像座標系
３２、４２、６２ 作業座標系
３３ 対象点（Ｐ）
３４ 焦点距離（ｆ）
３５ 焦点
３６ 撮像面
３７ 投影点
３８ 光軸
４３ ｉ−２番目の画像座標系上の対象点
４４ ｉ−１番目の画像座標系上の対象点
４５ ｉ番目の画像座標系上の対象点
４６ 画像座標系上の目標点
４７ 推定補助点
４８ ｉ−２番目の作業座標系上の対象点
４９ ｉ−１番目の作業座標系上の対象点
５０ ｉ番目の作業座標系上の対象点
５１ 作業座標系上の目標点
Ｓ１０１〜Ｓ１０９ ビジュアルフィードバックステップ
６３ 画像座標系上の開始点
６４ 画像座標系上の目標点
６５ 作業座標系上の開始点
６６ 作業座標系上の目標点
６７、６８ 画像座標系上の途中の特徴点
６９、７０ 作業座標系上の途中の特徴点
１０２、３０２ ロボットコントローラ（制御手段）
１０６ 変換マトリクス推定器（変換マトリクス推定手段）
１０７ 座標変換器（座標変換手段）
１０８ 対象物体
１０９ 速度指令調節器
１１０ フィルタ処理部
１１１ 物体モデル
２０１ 速度
２０２ 時間
２０３ 加速パターン
２０４ 減速パターン
２０５ 加速時間
２０６ 減速時間
２０７ リミット速度
２０８ 第１の点
２０９ 第２の点
２１０ 第３の点
（ａ） 第１のケース
（ｂ） 第２のケース
（ｃ） 第３のケース
３０６ ヤコビ行列推定器
３０７ 速度指令生成器
【図１】
0003981778.tif 000010
【図２】
0003981778.tif 000011
【図３】
0003981778.tif 000012
【図４】
0003981778.tif 000013
【図５】
0003981778.tif 000014
【図６】
0003981778.tif 000015
【図７】
0003981778.tif 000016
【図８】
0003981778.tif 000017
【図９】
0003981778.tif 000018