JP3981778B2 - Visual feedback method of mechanical device using imaging device and mechanical device - Google Patents

Description

【０００７】
ここでｓは同次座標値であり、ｘ，ｙ，ｚは作業座標系３２で表現した対象点（Ｐ）３３の座標値（Ｘ）である。また、ｕ，ｖは投影点３７の画像座標系３１の座標値（ｆ）であり、ｍ₁₁〜ｍ₄₃は作業座標系３２と画像座標系３１との位置及び姿勢関係とＣＣＤカメラ２６の内部パラメータを関係づける座標変換マトリクスの各パラメータである。
対象点（Ｐ）３３の動きを拘束する平面の方程式が以下の式（４）で表される。
ｚ＝０ …（４）
この式（４）によって式（３）は式（５）で表される。
【０００８】
【数２】

【０００９】
ここでｎ₁₁〜ｎ₃₃は対象点（Ｐ）３３が式（４）で表される拘束平面上のみを動く場合の画像座標系３１と作業座標系３２との位置及び姿勢関係とＣＣＤカメラ２６の内部パラメータとを関係付ける座標変換マトリクスの各パラメータである。
なお、ここでは式（４）に限定したが、一般的に、ある拘束平面上のみを動く場合は、作業座標系３２の位置及び姿勢を考慮することによって式（４）に示す制限に置換できる。また、ここでは式（５）で表されるように画像座標系３１上の２次元情報から作業座標系３２上の２次元情報への変換に限定する。
式（５）において、画像座標系３１上の微少移動変化量に対する作業座標系３２上の微少移動変化量の関係が式（６）で表される。
【００１０】
【数３】

【００１１】
ここでΔｘ，Δｙは作業座標系３２上の微少移動変化量であり、Δｕ，Δｖは画像座標系３１上の微少移動変化量である。ａ₁₁〜ａ₂₂は画像座標系３１上の微少移動変化量と作業座標系３２上の微少移動変化量とを対応付ける座標変換マトリクスの各パラメータであり、それぞれｕ，ｖの関数である。なお、（Δｘ，Δｙ）を作業座標系３２上の２次元速度ベクトル（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 を用いて式（６）から式（７）で求めることができる。
【００１２】
【数４】

【００１３】
この式（７）で利用した２次元速度ベクトル座標変換マトリクスＴ_i-1 を用いて、推定補助ベクトル（Δｕ_i-1'，Δｖ_i-1'）に対応する作業座標系４２上の推定補助ベクトル（Δｘ_i-1'，Δｙ_i-1'）を求めると式（６）から式（８）となる。
【００１４】
【数５】

【００１５】
ここで式（６）を変形し、画像座標系４１上の２次元速度ベクトル（Δｕ_i-1 ，Δｖ_i-1 ）と、画像座標系４１上の推定補助ベクトル（Δｕ_i-1'，Δｖ_i-1'）と、作業座標系４２上の２次元速度ベクトル（Δｘ_i-1 ，Δｙ_i-1 ）と、作業座標系４２上の推定補助ベクトル（Δｕ_i-1'，Δｖ_i-1'）を代入すると式（９）で表すことができる。
【００１６】
【数６】

【００１７】
式（９）における左辺の４×４マトリクスは、逆マトリクスが存在するため、２次元速度ベクトル座標変換マトリクスＴ_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 よりも小さい場合のいずれでも良い。Ｓ１０５では、式（１０）に基づいて作業座標系上の２次元速度ベクトル（Δｘ，Δｙ）を求める。
【００１８】
【数７】

【００１９】
ここで、Ｔは２次元速度ベクトル座標変換マトリクスであり、Ｋは、ゲインマトリクスである。なお、ビジュアルフィードバックの処理の初回は、２次元速度ベクトル座標変換マトリクスＴが求められていないため、人手によって適当な２次元速度ベクトル座標変換マトリクスＴの各パラメータの値を指定する、ロボットマニピュレータ２１を適当に移動させて２次元速度ベクトル座標変換マトリクスＴの各パラメータ値を求めておくなどの処理で求めた２次元速度ベクトル座標変換マトリクスＴを用いる。
このようにして求められた作業座標系上の２次元速度ベクトル（Δｘ，Δｙ）に基づいてロボットマニピュレータ２１を操作して対象点を移動する。この移動した対象点を画像座標系上で追跡し、実際の移動量、すなわち、画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）を測定する（Ｓ１０６）。その後、画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）と１次独立の関係にある推定補助ベクトル（Δｕ’，Δｖ’）、例えば、画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）と垂直なベクトルを求め式（８）に基づいて、作業座標系上の推定補助ベクトル（Δｘ’，Δｙ’）を演算する（Ｓ１０７）。
そして、求められた画像座標系上の２次元速度ベクトル（Δｕ，Δｖ）と、画像座標系上の推定補助ベクトル（Δｕ’，Δｖ’）と、作業座標系上の２次元速度ベクトル（Δｘ，Δｙ）と、作業座標系上の推定補助ベクトル（Δｘ’，Δｙ’）とを用いて式（９）から２次元速度ベクトル座標変換マトリクスＴの各パラメータを推定する（Ｓ１０８）。Ｓ１０８の後はＳ１０２に推移し、処理を繰り返す。
なお、この実施形態では、ビジュアルフィードバック中に、画像座標系と作業座標系との相対的な位置及び姿勢関係に変化が生じた場合、また、撮像装置の持つ内部パラメータに変化が生じた場合、あるいは、目標点が変更になった場合、その変化又は変更が多少であれば、目標点の位置（ｕ_d ，ｖ_d ）を変更し、又は、逐次、２次元速度ベクトル座標変換マトリクスを推定しているため、そのままビジュアルフィードバックを継続することができる。
また、一度、ビジュアルフィードバック処理を終了し、再度、ビジュアルフィードバック処理を実行する前に、画像座標系と作業座標系との相対的な位置及び姿勢関係に変化が生じた場合、また、撮像装置が有する内部パラメータに変化が生じた際にも、その変化が多少であれば、逐次、２次元速度ベクトル座標変換マトリクスを推定していることから、そのままビジュアルフィードバックを継続することができる。
図５は、図４の処理手順によるビジュアルフィードバック処理に基づいたシミュレーションを示す図である。図５（ａ）は画像座標系６１であり、図５（ｂ）は、作業座標系６２である。この画像座標系６１及び作業座標系６２には、画像座標系６１上の開始点６３と、画像座標系６１上の目標点６４と、作業座標系６２上の開始点６５と、作業座標系６２上の目標点６６とを有している。更に、この実施形態のビジュアルフィードバック処理に基づいて推移する画像座標系６１上の途中の特徴点６７，６８、及び作業座標系６２上の途中の特徴点６９，７０を有している。
ここでは式（１０）におけるゲインマトリクスＫを以下のように設定している。
【００２０】
【数８】

【００２１】
図４に示したビジュアルフィードバック処理を用いて、開始時に作業座標系６２上の開始点６５及び画像座標系６１上の開始点６３にあった対象点が、画像座標系６１では、画像座標系６１上の途中の特徴点６７，６８を通じて画像座標系６１上の目標点６４に収束していることが判明する。また、作業座標系６２では、作業座標系６２上の途中の特徴点６９，７０を通じて作業座標系６２上の目標点６６に収束していることが判明する。
図６はビジュアルフィードバックによる移動機構制御装置の構成を示している。
図６に示す構成は、ＣＣＤカメラ１０１と、ロボットコントローラ１０２と、ロボットマニピュレータ１０３と、撮像シーン１０４と、画像処理装置１０５と、変換マトリクス推定器１０６と、座標変換器１０７と、対象物体１０８と、速度指令調節器１０９と、フィルタ処理部１１０と、物体モデル１１１とを有している。
次に、このビジュアルフィードバックによる移動機構制御装置の動作について説明する。
ロボットマニピュレータ１０３のロボットハンドは対象物体１０８を把持して、対象物体１０８を目標位置へ移動させる。ＣＣＤカメラ１０１は撮像シーン１０４のように対象物体１０８の移動範囲を撮像し、画像データを画像処理装置１０５へ出力する。画像処理装置１０５では対象物体１０８の画像座標系上での特徴部分を追跡し、その現在位置ｆ（＝（ｕ，ｖ））を出力する。
なお、対象物体１０８の画像座標系上での特徴部分の目標位置ｆ_d （＝（ｕ_d ，ｖ_d ））は予め与えられている。変換マトリクス推定器１０６は、ロボットコントローラ１０２から得られるロボットハンドの作業座標系上での現在位置と、対象物体１０８の物体モデル１１１との和、すなわち、対象物体１０８の作業座標系上での特徴部分の現在位置Ｘと、画像処理装置１０５から得られる対象物体１０８の画像座標系上での特徴部分の現在位置ｆとを入力し、式（１２）の関係を満たす変換マトリクスＴを推定する。
dX/dt ＝Ｔdf/dt …（１２）
ここでdX/dt は対象物体１０８の作業座標系上での特徴部分の速度ベクトルであり、df/dt は対象物体１０８の画像座標系上での特徴部分の速度ベクトルである。座標変換器１０７は、式（１３）によって対象物体の作業座標系上での偏差ベクトルΔＸを生成する。
ΔＸ＝Ｔ（ｆ_d −ｆ） …（１３）
速度指令調節器１０９は、作業座標系上での偏差ベクトルと現在の速度指令ベクトルから、ロボットマニピュレータ１０３の動作が滑らかになるように予め設定された速度の加減速パターンに対応するように新たな速度指令を生成する。
フィルタ処理部１１０は、急激な速度変化がロボットマニピュレータ１０３に生じないよう、速度指令調節器１０９で生成した速度指令を滑らかにする。
図７は速度指令調節器１０９の動作を説明するための図である。
図７に示す例は、第１のケース（ａ）、第２のケース（ｂ）、第３のケース（ｃ）であり、かつ、速度２０１、時間２０２、加速パターン２０３、減速パターン２０４、加速時間２０５、減速時間２０６、リミット速度２０７、第１の点２０８、第２の点２０９、第３の点２１０からなる。
速度指令調節器１０９には、加速時間２０５と減速時間２０６とリミット速度２０７が予め設定されている。ここで第１のケース（ａ）に示されるように、現在の速度指令が第１の点２０８における速度の場合には式（１３）によって求めた偏差ベクトルΔＸと第１のケース（ａ）の図中における斜線部の面積が等しくなるように第１の点２０８の位置を決定する。第１の点２０８からは加速パターン２０３と傾きが等しくなるように新たに速度指令を決定し、リミット速度２０７となるまで繰り返し行う。
また、第２のケース（ｂ）に示すように、現在の速度指令が第２の点２０９における速度の場合には、式（１３）によって求めた偏差ベクトルΔＸと第２のケース（ｂ）の図中における斜線部の面積が等しくなるように第２の点２０９の位置を決定する。第２の点２０９からは減速パターン２０４と傾きが等しくなるように新たに速度指令を決定し、リミット速度２０７となるまで繰り返し行う。更に、第３のケース（ｃ）に示すように、現在の速度指令が第３の点２１０における速度であり、式（１３）によって求めた偏差ベクトルΔＸが第３のケース（ｃ）における図中の斜線部の場合は、リミット速度２０７までは到達しない以外は、第１のケース（ａ）と同一の処理である。なお、式（１３）における偏差ベクトルΔＸはビジュアルフィードバックのサンプリング間隔Δｔで毎回求められる。したがって、以上の処理もΔｔ時間で行われ、設定した加減速パターンに従うものとなる。
フィルタ処理部１１０は、速度指令調節器１０９で作成された新たな速度指令を滑らかにするために、例えば、式（１４）に示される特性の１次フィルタを用いれば良い。
Ｖ（ｋ）＝Ｈ｛dX/dt （ｋ）−Ｖ（ｋ−１）｝＋Ｖ（ｋ−１）…（１４）
ここで、Ｖ（ｋ）はサンプリング時間ｋにおけるフィルタ出力、Ｈはゲイン行列である。
【００２２】
【発明の効果】
以上の説明から明らかなように、請求項１、５および７記載の撮像装置を用いた機械装置のビジュアルフィードバック方法によれば、画像座標系上の２次元速度ベクトルと作業座標系上の２次元速度ベクトルとを対応づける２次元速度ベクトル座標変換マトリクスを推定しているため、煩雑なキャリブレーションを不要にできる。さらに、画像座標系上及び作業座標系上の２次元速度ベクトルの対応をマトリクス表現しているため、画像座標系上の対象物体の特徴部分の動き（２次元速度ベクトル）を作業座標系上の対象物体の特徴部分の動きに変換する際に必要となる局所的な２次元速度ベクトルの座標変換マトリクスが、作業座標系上の対象物体の動きから簡便に推定できるようになる。同様にして画像座標系上の情報から作業座標系の情報を求める際にも演算量を少なくできる。
本発明の請求項２および６記載の撮像装置を用いた機械装置のビジュアルフィードバック方法によれば、逐次、２次元速度ベクトル座標変換マトリクスを推定しているため、画像座標系と作業座標系との相対的な位置及び姿勢関係に変化が生じた場合、また、撮像装置が有する内部パラメータに変化が生じた場合、それらが多少の変化の際に、例えば、変化がビジュアルフィードバック処理中、又は、この処理間であっても、ビジュアルフィードバック処理が実行できるようになる。本発明の請求項３及び４記載の撮像装置を用いた機械装置のビジュアルフィードバック方法によれば、偏差ベクトルと現在の速度指令から、予め設定された移動機構の速度の加減速パターンに基づくように、新しい速度指令を計算しており、移動機構が初期に急激に動くことなく、動作が滑らかになる。
【図面の簡単な説明】
【図１】本発明の撮像装置を用いた機械装置のビジュアルフィードバック方法を実現するための撮像システムの構成図である。
【図２】実施形態で処理される座標系を説明するための図である。
【図３】実施形態にあって画像座標系上の対象点の推移と作業座標系上の対象点の推移を示す図である。
【図４】実施形態にあってビジュアルフィードバックの処理手順を示すフローチャートである。
【図５】実施形態にあってビジュアルフィードバック処理に基づいたシミュレーションを示す図である。
【図６】実施形態にあってビジュアルフィードバックによる移動機構制御装置の構成を示すブロック図である。
【図７】実施形態にあって速度指令調節器の動作を説明するための図である。
【図８】従来のビジュアルフィードバック方法による移動機構制御装置の構成を示すブロック図である。
【図９】従来例にあってフィードバックされる速度の加減速パターンである。
【符号の説明】
２１、１０３、３０３ ロボットマニピュレータ（移動機構）
２２ ロボットハンド
２３ ペグ（対象物体）
２４ ワーク
２５ ホール
２６、１０１、３０１ ＣＣＤカメラ（撮像装置）
２７、１０４、３０４ 撮像シーン
２８、１０５、３０５ 画像処理装置（画像処理手段）
２９ ペグの特徴部分
３１、４１、６１ 画像座標系
３２、４２、６２ 作業座標系
３３ 対象点（Ｐ）
３４ 焦点距離（ｆ）
３５ 焦点
３６ 撮像面
３７ 投影点
３８ 光軸
４３ ｉ−２番目の画像座標系上の対象点
４４ ｉ−１番目の画像座標系上の対象点
４５ ｉ番目の画像座標系上の対象点
４６ 画像座標系上の目標点
４７ 推定補助点
４８ ｉ−２番目の作業座標系上の対象点
４９ ｉ−１番目の作業座標系上の対象点
５０ ｉ番目の作業座標系上の対象点
５１ 作業座標系上の目標点
Ｓ１０１〜Ｓ１０９ ビジュアルフィードバックステップ
６３ 画像座標系上の開始点
６４ 画像座標系上の目標点
６５ 作業座標系上の開始点
６６ 作業座標系上の目標点
６７、６８ 画像座標系上の途中の特徴点
６９、７０ 作業座標系上の途中の特徴点
１０２、３０２ ロボットコントローラ（制御手段）
１０６ 変換マトリクス推定器（変換マトリクス推定手段）
１０７ 座標変換器（座標変換手段）
１０８ 対象物体
１０９ 速度指令調節器
１１０ フィルタ処理部
１１１ 物体モデル
２０１ 速度
２０２ 時間
２０３ 加速パターン
２０４ 減速パターン
２０５ 加速時間
２０６ 減速時間
２０７ リミット速度
２０８ 第１の点
２０９ 第２の点
２１０ 第３の点
（ａ） 第１のケース
（ｂ） 第２のケース
（ｃ） 第３のケース
３０６ ヤコビ行列推定器
３０７ 速度指令生成器[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a visual feedback method for a mechanical device using an imaging device for feeding back a suitable position or speed command to a moving mechanism of a transport device based on a two-dimensional image signal.And its machineryAbout.
[0002]
[Prior art]
Conventionally, a visual feedback method for a mechanical apparatus using this type of imaging apparatus is based on image data obtained by capturing a target object two-dimensionally with an ITV camera or the like with respect to a moving mechanism of a transport apparatus such as a robot manipulator. It is used for the purpose of feeding back speed commands.
In the visual feedback method of a mechanical device using this imaging device, for example, as shown in Japanese Patent Publication No. 8-23889 (method of extracting an object using a robot), the characteristic part of the imaging target object in the captured image data Are tracked on an image coordinate system that is extracted by image processing and fixed to the imaging surface. Then, a position or speed command is fed back to the moving mechanism so as to move the target object to the target position on the image coordinate system.
When performing an operation such as moving the target object based on the image data output from the imaging device in this way, an image coordinate system fixed on the imaging surface of the imaging device and a work coordinate system that can be recognized by the moving mechanism. It is necessary to obtain in advance the relative position, posture relationship, and internal parameters of the imaging apparatus.
In addition, as shown in Japanese Patent Laid-Open No. 8-71972 (automatic adjustment method of robot and camera), when obtaining the relationship between the image coordinate system and the work coordinate system and internal parameters, the measurement is performed manually or Use a dedicated object for measurement. In other words, when the relationship between the image coordinate system and the working coordinate system and the internal parameters are obtained, the measurement is performed by executing a precise calibration.
FIG. 8 shows a configuration of a moving mechanism control apparatus according to a conventional visual feedback method.
The configuration illustrated in FIG. 8 includes a CCD camera 301, a robot controller 302, a robot manipulator 303, an imaging scene 304, an image processing device 305, a Jacobian matrix estimator 306, and a speed command generator 307. The robot hand of the robot manipulator 303 is moved to the target position.
Here, the CCD camera 301 images the moving range of the robot hand of the robot manipulator 303 as in the imaging scene 304, and outputs this imaging signal to the image processing device 305.
The image processing apparatus 305 tracks the feature portion of the robot manipulator 303 on the image coordinate system of the robot hand, and outputs the current position f of the feature portion of the robot manipulator 303 on the image coordinate system of the robot hand. Target position f of the characteristic portion on the image coordinate system of the robot hand of the robot manipulator 303_dIs given in advance.
The Jacobian matrix estimator 306 includes a current position X on the work coordinate system of the robot hand obtained from the robot controller 302, and a current position f of the feature portion on the image coordinate system of the robot hand obtained from the image processing device 305. , And estimate the Jacobian matrix J satisfying equation (1) (Hosoda, Asada: “Configuration of an adaptive visual servo system with a feedforward compensator that does not require a priori knowledge about structure and parameters”, (See Journal of the Robotics Society of Japan, Vol. 14, No. 2).
df / dt = JdX / dt (1)
Here, df / dt is the velocity vector of the feature portion on the image coordinate system of the robot hand, and dX / dt is the velocity vector of the feature portion on the work coordinate system of the robot hand.
The speed command generator 307 generates a speed command according to equation (2).
dX / dt = KJ^-1(F_d-F) (2)
A speed command dX / dt to be supplied to the robot controller 302 is generated by Expression (2). Here, K is a gain matrix.
FIG. 9 shows a speed acceleration / deceleration pattern fed back to the robot controller 302.
As shown in FIG. 9, an acceleration / deceleration pattern having a speed dX / dt on the vertical axis and a time t on the horizontal axis is fed back to the robot controller 302.
[0003]
[Problems to be solved by the invention]
In such a visual feedback method of a mechanical device using the conventional imaging device, the calibration accuracy directly affects the correspondence between the position of the feature portion of the target object in the image coordinate system and the position in the work coordinate system. Therefore, it is necessary to accurately obtain the relative position and orientation relationship between the image coordinate system and the work coordinate system and the internal parameters of the imaging apparatus. In this case, there is a drawback that a great amount of processing time is required. In addition, after performing precise calibration, if the subtle changes occur in the relative position and orientation relationship between the work coordinate system and the image coordinate system, for example, due to reasons such as moving the imaging device. Therefore, it is necessary to re-calibrate the complicated process. Similarly, when the internal parameter of the imaging apparatus has a slight change due to, for example, changing the focal length of the lens, there is an inconvenience that it is necessary to re-calibrate complicated processing.
Further, the configurations shown in FIGS. 8 and 9 have a drawback that the speed command fed back to the robot controller becomes large at an initial stage, and the robot to be controlled moves rapidly.
The present invention solves such a problem in the prior art, and does not require complicated calibration, and the motion of the characteristic part of the target object on the image coordinate system (two-dimensional velocity vector) is converted into the working coordinates. The local two-dimensional velocity vector coordinate transformation matrix required for converting to the motion of the feature part of the target object on the system can be simplified by reducing the number of parameters estimated from the motion of the target object on the work coordinate system. It is an object of the present invention to provide a visual feedback method for a mechanical device using an imaging device that can be estimated in a simple manner. It is another object of the present invention to create an acceleration / deceleration pattern at a speed that does not cause the controlled moving mechanism (robot) to move suddenly and to smoothly operate the controlled moving mechanism.
[0004]
[Means for Solving the Problems]
The object of the present invention is achieved by the following configurations.
(1) A scene including a target object that is moved by a moving mechanism is photographed by at least one imaging device, a feature portion of the target object is extracted from an imaging signal obtained by the imaging device by image processing, and the position of the feature portion Is output on an image coordinate system fixed to the imaging surface of the imaging device, and a two-dimensional velocity vector of the target object on the image coordinate system is obtained based on the position of the feature portion on the image coordinate system. A two-dimensional velocity vector coordinate transformation matrix is locally estimated based on a two-dimensional velocity vector on the image coordinate system and a two-dimensional velocity vector on a work coordinate system recognized by the moving mechanism, and the image coordinate system From the difference between the position of the target point above and the position of the target point, the gain matrix, and the two-dimensional velocity vector coordinate transformation matrix, the two-dimensional velocity vector of the sequential work coordinate system is sequentially obtained. And a position or speed command is fed back to the moving mechanism so as to move the target object to a target position on the image coordinate system based on the two-dimensional velocity vector on a work coordinate system. A visual feedback method of a mechanical device using an imaging device.
(2) The two-dimensional velocity vector coordinate transformation matrix includes a two-dimensional velocity vector of a characteristic portion of the target object on the image coordinate system generated by the operation of the moving mechanism, and a first-order independent relationship with the two-dimensional velocity vector. A visual feedback method for a mechanical device using the imaging device according to (1), characterized in that it is estimated by using at least one estimated auxiliary vector.
(3) A scene including a target object that is moved by a moving mechanism is photographed by at least one imaging device, a feature portion of the target object is extracted from an imaging signal obtained by the imaging device by image processing, and the feature portion is Tracking on the image coordinate system fixed on the imaging surface of the imaging apparatus, and converting the deviation between the target position and the current position of the target object on the image coordinate system into the work coordinate system recognized by the moving mechanism. A new speed command corresponding to a preset acceleration / deceleration pattern of the speed of the moving mechanism is calculated based on the converted deviation and the current speed command. Visual feedback method.
(4) The visual feedback method for a mechanical device using the imaging device according to (3), wherein the new speed command is filtered to be smoothed.
(5) A moving mechanism that moves the target object, an imaging device that captures a scene including the moving object, and a feature portion of the target object is extracted from an imaging signal obtained by the imaging device by image processing, and the feature The position of the part is output on an image coordinate system fixed to the imaging surface of the imaging device, and based on the position of the characteristic part on the image coordinate system, the two-dimensional image of the target object on the image coordinate system An image processing means for obtaining a velocity vector, a two-dimensional velocity vector coordinate transformation matrix locally based on the two-dimensional velocity vector on the image coordinate system and the two-dimensional velocity vector on the work coordinate system recognized by the moving mechanism Conversion matrix estimation means for estimating the position, a difference between the target position and the position of the target object on the image coordinate system, a gain matrix, and the two-dimensional velocity vector Coordinate conversion means for sequentially obtaining the two-dimensional velocity vector of the work coordinate system from the coordinate coordinate transformation matrix, and the target object to the target position on the image coordinate system based on the two-dimensional velocity vector on the work coordinate system. Control means for feeding back a position or speed command to the moving mechanism so as to move.
(6) The conversion matrix estimation means converts the two-dimensional velocity vector coordinate transformation matrix into a two-dimensional velocity vector of a characteristic part of the target object on the image coordinate system generated by the operation of the moving mechanism, and the two-dimensional velocity vector. And at least one estimation auxiliary vector that is in a first-order independent relationship with each other.
  (7) In a mechanical device that includes a robot controller and an imaging device that images a manipulator and moves the manipulator to a target position based on image data captured by the imaging device, a target object that is gripped by the robot hand of the manipulator And the image processing means for outputting the position f of the target object in the image coordinate system, and the current position X of the work coordinate system of the robot hand obtained from the robot controller and the image processing means for output. A transformation matrix estimation means for estimating a transformation matrix T by inputting a position f of the image coordinate system of the target object, and a deviation vector of a work coordinate system by multiplying the difference between the target position and the position f by the transformation matrix T And a coordinate converter for obtaining ΔX.
  Configuration (1), (2), (5), (6), (7)Then, since the two-dimensional velocity vector coordinate transformation matrix that associates the two-dimensional velocity vector on the image coordinate system with the two-dimensional velocity vector on the work coordinate system is locally estimated, complicated calibration is not necessary. . Further, since the correspondence of the two-dimensional velocity vector on the image coordinate system and the work coordinate system is expressed in a matrix, the image coordinate system is compared with the corresponding matrix representation of the positions on the image coordinate system and the work coordinate system. The local two-dimensional velocity vector coordinate transformation matrix necessary for converting the motion of the feature portion of the target object (two-dimensional velocity vector) into the motion of the feature portion of the target object on the work coordinate system is the work coordinates. The estimation is simple from the movement of the target object on the system, that is, with a small amount of calculation that reduces the number of parameters to be estimated. Similarly, the amount of calculation is reduced when obtaining information on the work coordinate system from information on the image coordinate system.
  Configurations (3) and (4) aboveIs to calculate a new speed command corresponding to a preset acceleration / deceleration pattern of the speed of the moving mechanism based on the deviation and the current speed command, and the moving mechanism such as a robot moves rapidly in the initial stage. It works smoothly without it.
[0005]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 is a configuration diagram of an imaging system for realizing a visual feedback method of a mechanical device using the imaging device of the present invention. The example shown in FIG. 1 includes a robot manipulator 21 that performs a transfer operation by control, a robot hand 22 attached to the tip of the robot manipulator 21, and a peg 23 of a target object. In addition, this example has a work 24, and a hole 25 is provided in the work 24. Further, it has a CCD camera 26 as an imaging device. In addition, an imaging scene 27 photographed by the CCD camera 26 is displayed on the screen, and an image processing device 28 that processes image data from the CCD camera 26 is provided. In addition, a characteristic portion 29 of the peg 23 of the target object is illustrated.
Next, the operation and function of the configuration shown in FIG. 1 will be described.
In FIG. 1, a peg-in hole for inserting the peg 23 into the hole 25 is executed. First, the peg 23 is sandwiched and held by the robot hand 22 connected to the robot manipulator 21. Based on the position or speed command to the robot manipulator 21, the robot manipulator 21 operates and the peg 23 moves.
On the other hand, the CCD camera 26 captures a scene including the robot manipulator 21, the robot hand 22, the peg 23, the work 24 and the hole 25. The image data of this scene is sent from the CCD camera 26 to the image processing device 28. In the image processing device 28, the characteristic portion 29 of the peg 23 is tracked on the image, and the movement of the characteristic portion 29 is converted into the movement of the robot manipulator 21 and fed back as a position or speed command of the robot manipulator 21. What is required here is to convert the movement of the characteristic portion 29 of the peg 23 in the imaging scene 27 into the movement of the robot manipulator 21.
FIG. 2 is a diagram for explaining a coordinate system processed in this embodiment.
The coordinate system shown in FIG. 2 has an image coordinate system 31 and a work coordinate system 32. The image coordinate system 31 has a focal length (f) 34, a focal point 35, an imaging surface 36, a projection point 37, and an optical axis 38. Consists of. Furthermore, it has a target point (P) 33.
1 and 2, an image coordinate system 31 is fixed to an imaging surface 36 of a CCD camera 26 that is an imaging device. The work coordinate system 32 is fixed on a space that can be recognized by the robot manipulator 21. The target point (P) 33 is a point on the space and corresponds to the vicinity of the center of the characteristic portion 29 of the peg 23, and a straight line formed by the focal point 35 of the CCD camera 26 and the target point (P) 33 and the imaging surface 36. Projected to the intersection (projection point 37).
In order to determine the movement of the robot manipulator 21 based on the position information of the projection point 37 of the target point (P) 33 that is the tracking target obtained from the image output from the CCD camera 26, the image coordinate system 31 and the robot It is necessary to obtain the position and posture relationship with the work coordinate system 32 that can be recognized by the manipulator 21 and the internal parameters of the CCD camera 26. For example, the focal length (f) 34 needs to be obtained. In general, the position and orientation relationship between the image coordinate system 31 and the work coordinate system 32 and the internal parameters of the CCD camera 26 are expressed by Expression (3).
[0006]
[Expression 1]

[0007]
Here, s is a homogeneous coordinate value, and x, y, and z are the coordinate value (X) of the target point (P) 33 expressed in the work coordinate system 32. U and v are coordinate values (f) of the image coordinate system 31 of the projection point 37, and m₁₁~ M₄₃Are the parameters of the coordinate transformation matrix that relate the position and orientation relationship between the work coordinate system 32 and the image coordinate system 31 and the internal parameters of the CCD camera 26.
The equation of the plane that restrains the movement of the target point (P) 33 is expressed by the following equation (4).
z = 0 (4)
By this formula (4), formula (3) is expressed by formula (5).
[0008]
[Expression 2]

[0009]
Where n₁₁~ N₃₃Relates the position and orientation relationship between the image coordinate system 31 and the work coordinate system 32 and the internal parameters of the CCD camera 26 when the target point (P) 33 moves only on the constraining plane represented by the equation (4). Each parameter of the coordinate transformation matrix.
Here, although limited to Expression (4), in general, when moving only on a certain constraining plane, it can be replaced with the restriction shown in Expression (4) by considering the position and orientation of the work coordinate system 32. . In addition, here, the conversion is limited to the conversion from the two-dimensional information on the image coordinate system 31 to the two-dimensional information on the work coordinate system 32 as expressed by Expression (5).
In Expression (5), the relationship of the minute movement change amount on the work coordinate system 32 to the minute movement change amount on the image coordinate system 31 is expressed by Expression (6).
[0010]
[Equation 3]

[0011]
Here, Δx and Δy are minute movement changes on the work coordinate system 32, and Δu and Δv are minute movement changes on the image coordinate system 31. a₁₁~ A_{twenty two}Is a parameter of a coordinate conversion matrix that associates a minute movement change amount on the image coordinate system 31 with a minute movement change amount on the work coordinate system 32, and is a function of u and v, respectively. Note that (Δx, Δy) is a two-dimensional velocity vector (dX / dt) on the work coordinate system 32, and (Δu, Δv) is a two-dimensional velocity vector (df / dt) on the image coordinate system 31, In addition, the 2 × 2 matrix of Expression (6) is defined as a two-dimensional velocity vector coordinate conversion matrix (T). FIG. 3 is a diagram showing the transition of the target point on the image coordinate system and the transition of the target point on the work coordinate system. FIG. 3A shows an image coordinate system 41, and FIG. 3B shows a work coordinate system 42. The image coordinate system 41 in FIG. 3A includes a target point 43 on the i-2th image coordinate system 41, a target point 44 on the i-1th image coordinate system 41, and an i-th image coordinate system 41. An upper target point 45, a target point 46 on the image coordinate system 41, and an estimation auxiliary point 47 are included. 3B includes a target point 48 on the i-2th work coordinate system 42, a target point 49 on the i-1th work coordinate system 42, and the i-th work coordinate system 42. The target point 50 on the work coordinate system 42 and the target point 51 on the work coordinate system 42 are included.
Here, the difference (two-dimensional velocity vector) between the target point 45 on the i-th image coordinate system 41 and the target point 44 on the i−1-th image coordinate system 41 is (Δu_i-1, Δv_i-1) And the difference (two-dimensional velocity vector) between the target point 44 on the i−1th image coordinate system 41 and the target point 43 on the i−2th image coordinate system 41 is (Δu)._i-2, Δv_i-2). Further, the difference (estimated auxiliary vector) between the estimated auxiliary point 47 and the target point 44 on the (i−1) -th image coordinate system 41 is (Δu_i-1', Δv_i-1').
Further, the difference (two-dimensional velocity vector) between the target point 50 on the i-th work coordinate system 42 and the target point 49 on the i-1th work coordinate system 42 is expressed as (Δx_i-1, Δy_i-1) And the difference (two-dimensional velocity vector) between the target point 49 on the i-1th work coordinate system 42 and the target point 48 on the i-2th work coordinate system 42 is (Δx)._i-2, Δy_i-2). Here, the two-dimensional velocity vector (Δu on the image coordinate system 41 is_i-1, Δv_i-1) And the estimated auxiliary vector (Δu) on the image coordinate system 41_i-1', Δv_i-1It has a vertical relationship with ').
Here, the peg 23 is moved (the target point (P) 33 is moved) by the operation of the robot manipulator 21, and passes the position of the target point 49 on the i−1th work coordinate system 42 of the work coordinate system 42. When the object point 50 moves to the position of the object point 50 on the i-th work coordinate system 42, the object point (P) 33 is moved to the (i−1) -th image coordinate system 41 in accordance with the movement of the object point (P) 33. The position of the target point 44 on the image coordinate system 41 is passed to the position of the target point 45 on the i-th image coordinate system 41.
The movement target point of the target point (P) 33 is the target point 46 on the image coordinate system 41 on the image coordinate system 41 and the target point 51 on the work coordinate system 42 on the work coordinate system 42. Although the movement target point of the target point (P) 33 can be recognized in the image coordinate system 41, the robot manipulator 21 operates according to the position or speed command based on the information obtained from the image coordinate system 41. 42 is not recognized. Here, estimation of each parameter of the two-dimensional velocity vector coordinate transformation matrix (T) that associates the two-dimensional velocity vector on the image coordinate system 41 with the two-dimensional velocity vector on the work coordinate system 42 is considered. The two-dimensional velocity vector (Δx) when the target point (P) 33 moves from the target point 49 on the i−1th work coordinate system 42 to the target point 50 on the ith work coordinate system 42._i-1, Δy_i-1) Is the two-dimensional velocity vector coordinate transformation matrix T obtained by the previous estimation._i-1Can be obtained from Equation (6) to Equation (7).
[0012]
[Expression 4]

[0013]
Two-dimensional velocity vector coordinate transformation matrix T used in this equation (7)_i-1Is used to estimate the auxiliary vector (Δu_i-1', Δv_i-1') Corresponding to the estimated auxiliary vector (Δx) on the work coordinate system 42_i-1', Δy_i-1When ') is obtained, Equation (6) to Equation (8) are obtained.
[0014]
[Equation 5]

[0015]
Here, the equation (6) is transformed to a two-dimensional velocity vector (Δu on the image coordinate system 41._i-1, Δv_i-1) And the estimated auxiliary vector (Δu) on the image coordinate system 41_i-1', Δv_i-1') And the two-dimensional velocity vector (Δx) on the work coordinate system 42._i-1, Δy_i-1) And the estimated auxiliary vector (Δu) on the work coordinate system 42_i-1', Δv_i-1Substituting ') can be expressed by equation (9).
[0016]
[Formula 6]

[0017]
Since the inverse matrix exists in the 4 × 4 matrix on the left side in Equation (9), the two-dimensional velocity vector coordinate transformation matrix T_iEach parameter can be obtained. The two-dimensional velocity vector coordinate transformation matrix T obtained here_iIs used to obtain a two-dimensional velocity vector on the next work coordinate system 42 from Equation (6).
The estimated auxiliary vector (Δu on the image coordinate system 41)_i-1', Δv_i-1') Is a two-dimensional velocity vector (Δu) on the image coordinate system 41._i-1, Δv_i-1), But not necessarily vertical. For example, a two-dimensional velocity vector (Δu_i-1, Δv_i-1) And a two-dimensional velocity vector that is linearly independent. Further, the use of the estimated auxiliary vector is not limited to one, and two may be used. For example, a two-dimensional velocity vector (Δu on the image coordinate system 41)_i-1, Δv_i-1Two two-dimensional velocity vectors perpendicular to) may be used. In this case, one estimated auxiliary vector is converted into a two-dimensional velocity vector (Δu on the image coordinate system 41._i-1, Δv_i-1) With respect to primary independence.
In this embodiment, visual feedback is executed based on the equation (6). However, it is possible to realize visual feedback processing based on the equation (5). However, since the method based on equation (6) has a smaller number of matrix parameters to be estimated than the method based on equation (5), the amount of computation required for matrix estimation based on equation (9) is reduced. be able to. Further, when the information on the target point on the work coordinate system 42 is obtained from the information on the target point on the image coordinate system 41, the processing based on the formula (6) is performed by comparing the formulas (6) and (5). The amount of calculation can be reduced compared with the process based on Formula (5).
FIG. 4 is a flowchart showing a visual feedback processing procedure. 4, at the start of visual feedback (S101), the position (x, y) of the target point on the work coordinate system (corresponding to the center of the characteristic part 29 of the peg 23 in FIG. 1) is determined by the robot manipulator 21. Since the work coordinate system can be recognized, it can be measured, and the position (u, v) of the target point on the image coordinate system and the position (u corresponding to the hole 25 in FIG. 1) on the image coordinate system (u_d, V_d) Can be measured because the CCD camera 26 images a scene including the target point and the target point.
Next, the position (u, v) of the target point on the image coordinate system, the position (u of the target point on the image coordinate system)_d, V_d), The position (x, y) of the target point in the work coordinate system is measured (S102).
After that, the position of the target point on the image coordinate system (u_d, V_d) And the position (u, v) of the target point on the image coordinate system (e_u, E_v) (S103), and the difference (e_u, E_v) And a certain threshold (S_u, S_v) Is compared (S104).
And the difference (e_u, E_v) Is a predetermined threshold (S_u, S_v) Is smaller (S104: Yes), the visual feedback is terminated (S109). If larger (S104: No), the process proceeds to the next step S105. In this case, the condition for transitioning to the end of S109 is e_uIs S_uSmaller than e_vIs S_vLess than or e_uIs S_uSmaller than or e_vIs S_vAny smaller case may be used. In S105, a two-dimensional velocity vector (Δx, Δy) on the work coordinate system is obtained based on Expression (10).
[0018]
[Expression 7]

[0019]
Here, T is a two-dimensional velocity vector coordinate conversion matrix, and K is a gain matrix. Note that since the two-dimensional velocity vector coordinate transformation matrix T is not obtained at the first time of the visual feedback processing, the robot manipulator 21 that designates the values of the parameters of the appropriate two-dimensional velocity vector coordinate transformation matrix T manually is used. A two-dimensional velocity vector coordinate transformation matrix T obtained by processing such as appropriately moving each parameter value of the two-dimensional velocity vector coordinate transformation matrix T is used.
The target point is moved by operating the robot manipulator 21 based on the two-dimensional velocity vector (Δx, Δy) on the work coordinate system thus obtained. The moved target point is tracked on the image coordinate system, and the actual movement amount, that is, the two-dimensional velocity vector (Δu, Δv) on the image coordinate system is measured (S106). Thereafter, an estimated auxiliary vector (Δu ′, Δv ′) that has a linearly independent relationship with the two-dimensional velocity vector (Δu, Δv) on the image coordinate system, for example, a two-dimensional velocity vector (Δu, Δv on the image coordinate system). ) And a perpendicular vector (Δx ′, Δy ′) on the work coordinate system are calculated based on the equation (8) (S107).
Then, the obtained two-dimensional velocity vector (Δu, Δv) on the image coordinate system, the estimated auxiliary vector (Δu ′, Δv ′) on the image coordinate system, and the two-dimensional velocity vector (Δx, Each parameter of the two-dimensional velocity vector coordinate transformation matrix T is estimated from equation (9) using Δy) and the estimated auxiliary vector (Δx ′, Δy ′) on the work coordinate system (S108). After S108, the process proceeds to S102 and the process is repeated.
In this embodiment, during visual feedback, when a change occurs in the relative position and posture relationship between the image coordinate system and the work coordinate system, or when an internal parameter of the imaging device changes, Alternatively, when the target point is changed, if the change or change is slight, the position of the target point (u_d, V_d) Is changed, or the two-dimensional velocity vector coordinate transformation matrix is sequentially estimated, so that visual feedback can be continued as it is.
Also, if the relative position and orientation relationship between the image coordinate system and the work coordinate system change before the visual feedback process is finished and the visual feedback process is performed again, the imaging device Even when a change occurs in the internal parameters, if the change is slight, since the two-dimensional velocity vector coordinate conversion matrix is estimated successively, visual feedback can be continued as it is.
FIG. 5 is a diagram showing a simulation based on the visual feedback processing according to the processing procedure of FIG. FIG. 5A shows an image coordinate system 61, and FIG. 5B shows a work coordinate system 62. The image coordinate system 61 and the work coordinate system 62 include a start point 63 on the image coordinate system 61, a target point 64 on the image coordinate system 61, a start point 65 on the work coordinate system 62, and a work coordinate system 62. And an upper target point 66. Furthermore, it has feature points 67 and 68 on the image coordinate system 61 that change based on the visual feedback processing of this embodiment, and feature points 69 and 70 on the work coordinate system 62.
Here, the gain matrix K in the equation (10) is set as follows.
[0020]
[Equation 8]

[0021]
Using the visual feedback processing shown in FIG. 4, the target point that was at the start point 65 on the work coordinate system 62 and the start point 63 on the image coordinate system 61 at the start is the image coordinate system 61 in the image coordinate system 61. It is found that the target point 64 on the image coordinate system 61 is converged through the upper halfway feature points 67 and 68. In the work coordinate system 62, it is found that the target point 66 on the work coordinate system 62 is converged through the feature points 69 and 70 on the work coordinate system 62.
FIG. 6 shows the configuration of the moving mechanism control device by visual feedback.
The configuration shown in FIG. 6 includes a CCD camera 101, a robot controller 102, a robot manipulator 103, an imaging scene 104, an image processing device 105, a transformation matrix estimator 106, a coordinate converter 107, and a target object 108. , A speed command adjuster 109, a filter processing unit 110, and an object model 111.
Next, the operation of the moving mechanism control apparatus using this visual feedback will be described.
The robot hand of the robot manipulator 103 holds the target object 108 and moves the target object 108 to the target position. The CCD camera 101 images the moving range of the target object 108 as in the imaging scene 104 and outputs the image data to the image processing device 105. The image processing apparatus 105 tracks the characteristic portion of the target object 108 on the image coordinate system and outputs the current position f (= (u, v)).
Note that the target position f of the characteristic portion of the target object 108 on the image coordinate system._d(= (U_d, V_d)) Is given in advance. The transformation matrix estimator 106 is the sum of the current position on the work coordinate system of the robot hand obtained from the robot controller 102 and the object model 111 of the target object 108, that is, the feature of the target object 108 on the work coordinate system. The current position X of the part and the current position f of the characteristic part on the image coordinate system of the target object 108 obtained from the image processing apparatus 105 are input, and a transformation matrix T that satisfies the relationship of Expression (12) is estimated.
dX / dt = Tdf / dt (12)
Here, dX / dt is a velocity vector of the characteristic portion of the target object 108 on the work coordinate system, and df / dt is a velocity vector of the characteristic portion of the target object 108 on the image coordinate system. The coordinate converter 107 generates a deviation vector ΔX on the work coordinate system of the target object by Expression (13).
ΔX = T (f_d-F) (13)
The speed command adjuster 109 is newly adapted to correspond to a speed acceleration / deceleration pattern set in advance so that the operation of the robot manipulator 103 becomes smooth from the deviation vector on the work coordinate system and the current speed command vector. Generate a speed command.
The filter processing unit 110 smoothes the speed command generated by the speed command adjuster 109 so that a rapid speed change does not occur in the robot manipulator 103.
FIG. 7 is a diagram for explaining the operation of the speed command adjuster 109.
The example shown in FIG. 7 is the first case (a), the second case (b), and the third case (c), and the speed 201, time 202, acceleration pattern 203, deceleration pattern 204, acceleration It consists of time 205, deceleration time 206, limit speed 207, first point 208, second point 209, and third point 210.
In the speed command adjuster 109, an acceleration time 205, a deceleration time 206, and a limit speed 207 are set in advance. Here, as shown in the first case (a), when the current speed command is the speed at the first point 208, the deviation vector ΔX obtained by the expression (13) and the first case (a) The position of the first point 208 is determined so that the shaded area in the figure is equal. From the first point 208, a new speed command is determined so that the inclination becomes the same as the acceleration pattern 203, and this is repeated until the limit speed 207 is reached.
Further, as shown in the second case (b), when the current speed command is the speed at the second point 209, the deviation vector ΔX obtained by the expression (13) and the second case (b) The position of the second point 209 is determined so that the areas of the hatched portions in the figure are equal. From the second point 209, a new speed command is determined so that the gradient is the same as that of the deceleration pattern 204, and this is repeated until the limit speed 207 is reached. Further, as shown in the third case (c), the current speed command is the speed at the third point 210, and the deviation vector ΔX obtained by the equation (13) is shown in the figure in the third case (c). In the case of the shaded area, the processing is the same as in the first case (a) except that the speed limit 207 is not reached. The deviation vector ΔX in the equation (13) is obtained every time at the visual feedback sampling interval Δt. Therefore, the above processing is also performed in Δt time, and follows the set acceleration / deceleration pattern.
The filter processing unit 110 may use, for example, a first-order filter having the characteristics shown in Equation (14) in order to smooth the new speed command created by the speed command adjuster 109.
V (k) = H {dX / dt (k) -V (k-1)} + V (k-1) (14)
Here, V (k) is a filter output at the sampling time k, and H is a gain matrix.
[0022]
【The invention's effect】
  As is clear from the above description, the claim 15 and 7According to the visual feedback method of a mechanical device using the described imaging device, a two-dimensional velocity vector coordinate transformation matrix that associates a two-dimensional velocity vector on the image coordinate system with a two-dimensional velocity vector on the work coordinate system is estimated. Therefore, complicated calibration can be eliminated. Further, since the correspondence between the two-dimensional velocity vectors on the image coordinate system and the work coordinate system is expressed in a matrix, the movement of the characteristic part of the target object on the image coordinate system (two-dimensional velocity vector) A local two-dimensional velocity vector coordinate conversion matrix required for conversion into the motion of the characteristic part of the target object can be easily estimated from the movement of the target object on the work coordinate system. Similarly, the amount of calculation can be reduced when obtaining information on the work coordinate system from information on the image coordinate system.
  Claim 2 of the present inventionAnd 6According to the visual feedback method of the mechanical device using the described imaging device, since the two-dimensional velocity vector coordinate transformation matrix is sequentially estimated, the relative position and orientation relationship between the image coordinate system and the work coordinate system is determined. If a change occurs, or if there is a change in the internal parameters of the imaging device, when they change slightly, for example, even if the change is during the visual feedback process or during this process, Feedback processing can be executed. According to the visual feedback method of the mechanical device using the imaging device according to the third and fourth aspects of the present invention, based on the acceleration / deceleration pattern of the speed of the moving mechanism set in advance from the deviation vector and the current speed command. A new speed command is calculated, and the movement mechanism does not move suddenly and the operation becomes smooth.
[Brief description of the drawings]
FIG. 1 is a configuration diagram of an imaging system for realizing a visual feedback method of a mechanical device using an imaging device of the present invention.
FIG. 2 is a diagram for explaining a coordinate system processed in the embodiment.
FIG. 3 is a diagram illustrating a transition of a target point on an image coordinate system and a transition of a target point on a work coordinate system in the embodiment.
FIG. 4 is a flowchart illustrating a processing procedure of visual feedback in the embodiment.
FIG. 5 is a diagram illustrating a simulation based on visual feedback processing in the embodiment.
FIG. 6 is a block diagram illustrating a configuration of a moving mechanism control device using visual feedback in the embodiment.
FIG. 7 is a diagram for explaining the operation of the speed command adjuster in the embodiment.
FIG. 8 is a block diagram showing a configuration of a moving mechanism control device according to a conventional visual feedback method.
FIG. 9 is a speed acceleration / deceleration pattern fed back in a conventional example.
[Explanation of symbols]
21, 103, 303 Robot manipulator(Movement mechanism)
22 Robot Hand
23 pegs(Target object)
24 work
25 holes
26, 101, 301 CCD camera(Imaging device)
27, 104, 304 Imaging scene
28, 105, 305 Image processing apparatus(Image processing means)
29 peg features
31, 41, 61 Image coordinate system
32, 42, 62 Working coordinate system
33 Target point (P)
34 Focal length (f)
35 focus
36 Imaging surface
37 Projection points
38 optical axes
43 Target point on the i-2nd image coordinate system
44 Target point on the i-1st image coordinate system
45 Target point on the i-th image coordinate system
46 Target point on image coordinate system
47 Estimated auxiliary points
48 Target point on the i-2nd work coordinate system
49 i-1 Target point on the working coordinate system
50 Target point on the ith work coordinate system
51 Target point on work coordinate system
S101 to S109 Visual feedback step
63 Starting point on the image coordinate system
64 Target points on the image coordinate system
65 Starting point on working coordinate system
66 Target point on working coordinate system
67, 68 Feature points along the image coordinate system
69, 70 Feature points along the work coordinate system
102, 302 Robot controller(Control means)
106 Transformation matrix estimator(Conversion matrix estimation means)
107 coordinate converter(Coordinate conversion means)
108 Target object
109 Speed command controller
110 Filter processing unit
111 object model
201 speed
202 hours
203 Acceleration pattern
204 Deceleration pattern
205 Acceleration time
206 Deceleration time
207 Limit speed
208 First point
209 Second point
210 Third point
(A) First case
(B) Second case
(C) Third case
306 Jacobian Matrix Estimator
307 Speed command generator

Claims (7)

Photographing a scene including a target object that is moved by a moving mechanism by at least one imaging device,
Extracting the characteristic part of the target object from the imaging signal obtained by the imaging device by image processing,
Outputting the position of the characteristic portion on an image coordinate system fixed to the imaging surface of the imaging device ;
Obtaining a two-dimensional velocity vector of the target object on the image coordinate system based on the position of the feature portion on the image coordinate system;
Locally estimating a two-dimensional velocity vector coordinate transformation matrix based on a two-dimensional velocity vector on the image coordinate system and a two-dimensional velocity vector on a work coordinate system recognized by the moving mechanism;
From the difference between the position of the target point on the image coordinate system and the position of the target point, the gain matrix, and the two-dimensional velocity vector coordinate conversion matrix, the two-dimensional velocity vector of the work coordinate system is obtained sequentially.
A position or speed command is fed back to the moving mechanism so as to move the target object to a target position on the image coordinate system based on the two-dimensional velocity vector on a work coordinate system . The visual feedback method of the machine equipment that was. The two-dimensional velocity vector coordinate transformation matrix is
Utilizing a two-dimensional velocity vector of the characteristic portion of the object on the image coordinate system generated by the operation of the moving mechanism, and at least one of the estimated auxiliary vector in the relation of the two-dimensional velocity vector and the linearly independent Visual feedback method of mechanical apparatus using the imaging device according to claim 1, wherein the estimated by. Photographing a scene including a target object that is moved by a moving mechanism by at least one imaging device,
Extracting the characteristic part of the target object from the imaging signal obtained by the imaging device by image processing,
Tracking the feature portion on an image coordinate system fixed to the imaging surface of the imaging device;
Converting the deviation between the target position and the current position of the target object on the image coordinate system into a work coordinate system recognized by the moving mechanism ;
A new speed command corresponding to a preset acceleration / deceleration pattern of the speed of the moving mechanism is calculated on the basis of the deviation after conversion and the current speed command. Visual feedback method. The visual feedback method of the mechanical device using the imaging device according to claim 3, wherein a filtering process is performed to smooth the new speed command.   A moving mechanism for moving the target object;
  An imaging device for capturing a scene including the moving object;
  A feature portion of the target object is extracted from an imaging signal obtained by the imaging device by image processing, and a position of the feature portion is output on an image coordinate system fixed to an imaging surface of the imaging device, and Image processing means for obtaining a two-dimensional velocity vector of the target object on the image coordinate system based on the position of the feature portion on the image coordinate system;
  Transformation matrix estimation means for locally estimating a two-dimensional velocity vector coordinate transformation matrix based on a two-dimensional velocity vector on the image coordinate system and a two-dimensional velocity vector on a work coordinate system recognized by the moving mechanism;
  From the difference between the target position on the image coordinate system and the position of the target object, the gain matrix, and the two-dimensional velocity vector coordinate transformation matrix, the two-dimensional velocity of the sequential work coordinate system. Coordinate conversion means for obtaining a vector;
  Control means for feeding back a position or speed command to the moving mechanism so as to move the target object to a target position on the image coordinate system based on the two-dimensional velocity vector on a work coordinate system.
  A mechanical device characterized by that.   The conversion matrix estimation means includes
  2D velocity vector coordinate transformation matrix
  Using a two-dimensional velocity vector of a characteristic part of a target object on an image coordinate system generated by the operation of the moving mechanism, and at least one estimated auxiliary vector having a first-order independent relationship with the two-dimensional velocity vector Estimated by
  The mechanical device according to claim 5.   In a mechanical device that includes a robot controller and an imaging device that images a manipulator, and moves the manipulator to a target position based on image data captured by the imaging device,
  Image processing means for photographing a target object held by the robot hand of the manipulator and outputting a position f of the target object in an image coordinate system;
  Transformation matrix estimation means for estimating the transformation matrix T by inputting the current position X of the work coordinate system of the robot hand obtained from the robot controller and the position f of the image coordinate system of the target object output by the image processing means; ,
  A coordinate converter that obtains a deviation vector ΔX of a work coordinate system by multiplying the difference between the target position and the position f by the conversion matrix T.
  A mechanical device characterized by that.

Images