JP4329257B2

JP4329257B2 - ロボット装置

Info

Publication number: JP4329257B2
Application number: JP2000326036A
Authority: JP
Inventors: 貴志内藤; 秀樹野村
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2000-10-25
Filing date: 2000-10-25
Publication date: 2009-09-09
Anticipated expiration: 2020-10-25
Also published as: JP2002127055A

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボット装置に係り、特に、ビジュアルサーボを用いてロボットアームを制御するロボット装置に関する。
【０００２】
【従来の技術】
従来、ビジュアルサーボによりロボットアームを制御する方法として、文献 Bernard Espiau,rancois Chaumette and Patrick Rives,A New Approach to Visual Servoing in Robotics,IEEE Trans.Robotics and Automation,8(3),313-326,1992に記載されているように、目標位置のｎ個の画像特徴量Ｆ^*＝[Ｆ^* _i]（ｉ＝１,２,・・・,Ｎ）を予め定義しておき、現在位置での画像特徴量ＦがＦ^*に一致するように制御する方法が知られている。具体的には、画像特徴量とロボット手先位置の速度の関係を線形に表した画像ヤコビアンという行列を用いて、手先速度を生成している。
【０００３】
ここで、手先速度をＴ＝（ｔ_xｔ_yｔ_zω_xω_yω_z）^Tと表し、画像特徴量をＦ_i（ｕ_iｖ_i）^Tと表すと、速度Ｔは、以下の式（１）により求められる。
【０００４】
Ｔ＝−λＬ^T+ｅ（１）
ここで、λはゲインであり、ｅは以下の式（２）で表されるベクトルである。
【０００５】
【数１】

【０００６】
また、Ｌ^T+は画像ヤコビアン行列Ｌ^Tの擬似逆行列であり、Ｌ^T+は以下の式（３）で表される。
【０００７】
【数２】

【０００８】
なお、ｚ_iは、カメラからみた画像特徴量Ｆ_iまでの距離である。
【０００９】
【発明が解決しようとする課題】
上述のように、目標位置での画像特徴量Ｆ^*と現在位置での画像特徴量Ｆとの差ｅとヤコビアン行列Ｌ^Tの擬似逆行列との積に基づいて、６次元の手先速度Ｔを一度に求めている。このため、例えば現在位置が目標位置に対してｚ軸回りの回転だけがずれていた場合、ｚ軸回りの回転速度だけでなく、ｚ軸方向の速度成分も生成される。
【００１０】
また、式（１）で計算される手先速度Ｔの各成分がどのような値になるのかをｅ,Ｌ^T+から推測するのは困難である。そのため、対象物が画像周縁部で観測される場合には、対象物が視野から外れてしまうなどの問題が生じる。
【００１１】
さらに、例えば図１６に示すように、目標位置において対象物の全体を撮像して、すべての画像特徴量を抽出することができる場合でなければ、ロボットアームを精度よく制御することができないという問題もあった。
【００１２】
また、例えば図１７に示すように、画像特徴量が撮像画像のある領域に偏在している場合では、ｘｙ軸方向の位置精度と比較して、奥行きの位置精度が顕著に低下してしまうことがあった。さらに、図１８に示すように、画像特徴量が直線状に並んでいる場合では、ｘｚ軸回りの姿勢精度と比較して、ｙ軸回りの姿勢精度が低下してしまうことがあった。したがって、このような場合であっても、ロボットアームを精度よく制御することが要求されていた。
【００１３】
本発明は、上述した課題を解決するために提案されたものであり、ある速度成分だけに変化が生じた場合や、画像特徴量の抽出に偏りが生じていても、精度よくロボットアームを制御することができるロボット装置を提供することを目的とする。
【００１４】
【課題を解決するための手段】
請求項１記載の発明は、所定の作業を行うロボットアームと、前記ロボットアームの手先の近傍に固定された状態で対象物を撮像する撮像手段と、前記撮像手段で撮像された対象物の画像に基づいて、前記対象物の特徴部分の座標を画像特徴量として抽出する特徴量抽出手段と、前記ロボットアームの手先が前記対象物に対して相対的な目標位置にあったときに、前記特徴量抽出手段によって予め抽出された画像特徴量を記憶する特徴量記憶手段と、前記特徴量記憶手段に記憶されている画像特徴量と、前記手先が現在位置にあるときに前記特徴量抽出手段で抽出された画像特徴量とに基づいて、複数の軸方向の並進速度と各軸回りの回転速度を算出するにあたり、少なくとも、所定の軸方向の並進速度を第１の速度成分に振り分け、その他の軸方向の並進速度を第２の速度成分に振り分け、（１）各軸回りの回転速度の全部を第１の速度成分に振り分け、又は、（２）各軸回りの回転速度の全部を第１及び第２の速度成分と異なる他の速度成分に振り分け、又は、（３）各軸回りの回転速度の一部を第１の速度成分に振り分け、残りの回転速度を第１及び第２の速度成分と異なる他の速度成分に振り分け、各速度成分を他の速度成分を用いることなく算出する速度算出手段と、前記速度算出手段で算出された各速度成分に含まれる並進速度及び回転速度に基づいて、前記ロボットアームの手先を前記対象物に対して相対的な目標位置に移動させる制御を行うアーム制御手段と、を備えて構成されている。
【００１５】
この発明は、対象物に対して相対的な目標位置において抽出された画像特徴量と、現在位置において抽出された画像特徴量とを略一致させるために、ロボットアームのアーム手先の速度を算出する。アーム手先の速度は、一般に、複数の軸方向の並進速度及びこれらの軸回りの回転速度から構成される。ここで、アーム手先の速度を算出の仕方によっては、所定の軸方向の並進速度に変化が生じると、所定軸回りの回転速度に変化が生じることがあり、またその逆もある。そこで、少なくとも、所定の軸方向の並進速度を第１の速度成分に振り分け、その他の軸方向の並進速度を第２の速度成分に振り分け、（１）各軸回りの回転速度の全部を第１の速度成分に振り分け、又は、（２）各軸回りの回転速度の全部を第１及び第２の速度成分と異なる他の速度成分に振り分け、又は、（３）各軸回りの回転速度の一部を第１の速度成分に振り分け、残りの回転速度を第１及び第２の速度成分と異なる他の速度成分に振り分け、各速度成分を他の速度成分を用いることなく算出する。これにより、上述した影響を回避して、アーム手先の位置決めを精密に行うことができる。
【００１６】
また、請求項２記載のように、前記速度算出手段は、前記ロボットアームの手先が１つ以上の回転速度の回転方向において制限されている場合に、制限されている回転速度のみを含む第３の速度成分を算出せず、当該第３の速度成分以外の速度成分を算出してもよい。
【００１７】
請求項３記載の発明は、所定の作業を行うロボットアームと、前記ロボットアームの手先の周辺に固定された状態で対象物及びその周辺を撮像する撮像手段と、前記撮像手段で撮像された対象物及びその周辺の画像に基づいて、前記対象物及びその周辺の特徴部分の座標を画像特徴量としてそれぞれ抽出する特徴量抽出手段と、前記ロボットアームの手先が前記対象物に対して相対的な目標位置にあったときに、前記特徴量抽出手段によって予め抽出された対象物及びその周辺の画像特徴量を、第１の対象物特徴量及び第１の周辺特徴量として記憶する特徴量記憶手段と、前記特徴量記憶手段に記憶されている第１の対象物特徴量及び第１の周辺特徴量と、前記ロボットアームの手先が現在位置にあるときに前記特徴量抽出手段で抽出された対象物及びその周辺の画像特徴量である第２の対象物特徴量及び第２の周辺特徴量とに基づいて、前記手先を前記対象物に移動させる速度を算出する速度算出手段と、前記速度算出手段で算出された速度に基づいて、前記ロボットアームの手先を前記対象物に対して相対的な目標位置に移動させる制御を行うアーム制御手段と、を備えて構成されている。
【００１８】
特徴量抽出手段は、撮像手段によって撮像された画像に基づいて、対象物の特徴量だけでなく、その周辺の特徴量も抽出する。ここで、第１及び第２の対象物特徴量だけであっても、これらに基づいてアーム手先を対象物に対して相対的な目標位置に制御することは可能である。しかし、撮像された画像中において第１及び第２の対象物特徴量の領域が偏っていたり、それらがどのように配列されていても、常に精度よくアーム手先を位置決めすることが必要である。そこで、さらに、対象物の周辺の画像特徴量である第１及び第２の周辺特徴量を用いて、アーム手先を移動させる速度を正確に算出して、精密に位置決めを行っている。
【００１９】
ここで、第１及び第２の対象物特徴量だけでは、所定軸方向の並進速度を正確に算出することが困難な場合は、請求項４記載のようにするのが好ましい。すなわち、前記速度算出手段は、前記第１及び第２の周辺特徴量間の線分の長さを算出し、各線分の長さに基づいて所定軸方向の並進速度を算出すればよい。例えば、第１の周辺特徴量間の線分の長さと第２の周辺特徴量間の線分の長さとの比を算出し、線分の長さの比から所定軸方向の並進速度を算出することができる。さらに、第１の周辺特徴量間の線分の長さから当該線分で囲まれる面積を算出し、同様にして第２の周辺特徴量間の線分の長さから当該線分で囲まれる面積を算出して、２つの面積の比から所定軸方向の並進速度を算出することができる。
【００２０】
また、第１及び第２の対象物特徴量だけでは、所定軸回りの回転速度を正確に算出することが困難な場合は、請求項５記載のようにするのが好ましい。すなわち、前記速度算出手段は、前記第１の周辺特徴量の位置から前記第２の周辺特徴量の位置に姿勢を変換する姿勢変換行列に基づいて、所定軸回りの回転速度を算出すればよい。
【００２１】
【発明の実施の形態】
以下、本発明の実施の形態について図面を参照しながら詳細に説明する。
【００２２】
［第１の実施の形態］
図１に示すように、本発明の第１の実施の形態に係るロボット装置は、対象物設置台１１に置かれた対象物１２に対する相対的な目標位置まで移動するように、ロボットアーム１を制御するものである。
【００２３】
なお、図１では、対象物１２は対象物設置台１１上に静止した状態で置かれており、ロボット装置のベース部分は固定されている。しかし、本発明は、このような実施の形態に限定されるものではなく、対象物１２がロボット装置に対して相対的に静止している場合や相対的に移動している場合であってもよい。例えば、対象物１２とロボット装置が共に同じ速度で移動している場合、対象物１２が図示しないコンベアによって搬送されている場合、ロボット装置が台車等に設置されて移動するような場合であっても、適用することができる。なお、第２の実施の形態でも同様である。
【００２４】
このロボット装置は、対象物１２に所定の作業を行うロボットアーム１と、対象物１２を撮像するＣＣＤカメラ２と、ＣＣＤカメラ２が撮像した画像から特徴量を抽出するセンサコントローラ３と、座標変換や軌道生成等を行い、図示しないサーボ回路やアンプなどを有し、ロボットアーム１の動作を制御するロボットコントローラ４とを備えている。
【００２５】
ＣＣＤカメラ２は、ロボットアーム１のアーム手先１Ａの近傍に固着されている。なお、カメラ座標系は、図２に示すように、ＣＣＤカメラ２の撮像方向をｚ軸とし、さらに互いに直交するようにｘ軸及びｙ軸を設けている。
【００２６】
最初に準備作業として、目標位置における対象物１２の画像特徴量を取得する。具体的には、ロボットコントローラ４は、図２に示すように、ロボットアーム１のアーム手先１Ａを、対象物１２に対して相対的な目標位置に配置する。そして、ＣＣＤカメラ２は、図３に示すように、対象物１２を撮像し、この画像をセンサコントローラ３に供給する。
【００２７】
センサコントローラ３は、ＣＣＤカメラ２からの撮像画像に基づいて、対象物１２の特徴量を抽出する。例えば、対象物１２の上面に６つのネジ穴が形成されていた場合、これらの穴の中心位置（Ｆ^* ₁〜Ｆ^* ₆）の画像中での座標値（ｕ^* _i,ｖ^* _i）を、目標位置での画像特徴量Ｆ^* _i（ｉ＝１,・・・,６）として抽出する。ロボットコントローラ４は、センサコントローラ３で抽出された目標位置での画像特徴量Ｆ^* _iを記憶し、これを後述する処理で用いる。
【００２８】
なお、ＣＣＤカメラ２で撮像された画像からネジ穴の中心位置を計測する方法としては、例えば二値化画像処理やエッジ処理などの公知の手法を適用すればよい。また、ここではネジ穴の中心位置の座標を画像特徴量Ｆ^* _iとしたが、線分の端点、コーナ、二値化画像の孤立領域の面積の重心など、他の画像特徴量を用いてもよい。さらに、画像特徴量Ｆ^* _iである座標値（ｕ，ｖ）は、図３に示すように、画像中心を原点、横方向をｘ軸、縦方向をｙ軸とする画像座標系によって定義されているが、その他カメラ座標系によって定義してもよい。
【００２９】
このような準備作業が終了すると、ロボットコントローラ４は、現在位置（目標位置とは異なる位置）にあるロボットアーム１のアーム手先１Ａを、対象物１２に対する目標位置まで移動させる制御を行う。
【００３０】
アーム手先１Ａの現在位置が例えば図４に示す位置にある場合、ＣＣＤカメラ２は、対象物１２を撮像して、図５に示すような画像を出力する。そして、図６に示すステップＳＴ１以下の処理を実行する。
【００３１】
ステップＳＴ１において、センサコントローラ３は、目標位置における画像特徴量Ｆ^* _iの抽出と同様にして、現在位置における画像特徴量Ｆを抽出する。すなわち、対象物１２の各穴（Ｆ₁〜Ｆ₆）の中心位置の画像中での座標値（ｕ_i，ｖ_i）を、現在位置における画像特徴量Ｆ_i（ｉ＝１，・・・，６）として抽出して、ステップＳＴ２に進む。
【００３２】
ステップＳＴ２において、ロボットコントローラ４は、現在位置での画像特徴量Ｆと目標位置の画像特徴量Ｆ^*の差から、以下の式（４）に示すベクトルｅを求める。
【００３３】
【数３】

【００３４】
そして、ベクトルｅの各要素の平均を基準として、画像特徴量の差residを以下の式（５）から求めて、ステップＳＴ３に進む。
【００３５】
【数４】

【００３６】
なお、Ｎは画像特徴量Ｆ，Ｆ^* _iの個数を示し、ここではＮ＝６である。また、画像特徴量の差residは、例えば現在位置での画像特徴量Ｆと目標位置の画像特徴量Ｆ^*の距離の平均値を基準にしてもよい。すなわち、以下の式（６）を求めてもよい。
【００３７】
【数５】

【００３８】
ステップＳＴ３において、ロボットコントローラ４は、画像特徴量の差residが所定の閾値ε以下であるかを判定し、それが閾値ε以下であるときはロボットアーム１のアーム手先１Ａが目標位置に制御されたと判定して、処理を終了する。一方、画像特徴量の差residが所定の閾値ε以下でないときは、ステップＳＴ４に進む。
【００３９】
その他、以下の式（７）が成り立つかを判定してもよい。
【００４０】
【数６】

【００４１】
このとき、ベクトルｅの絶対値の最大が閾値ε以下であれば、目標位置に制御されたと判定することができる。
【００４２】
また、現在位置での画像特徴量Ｆと目標位置の画像特徴量Ｆ^*から画像特徴量の差residを求めるのではなく、例えば後述するステップＳＴ４で得られるアーム手先１Ａの速度（以下「アーム手先速度」という。）Ｔが十分に小さくなった時に目標位置に制御できたと判定してもよい。
【００４３】
ステップＳＴ４において、ロボットコントローラ４は、画像特徴量Ｆ^*,Ｆを用いて、６つのパラメータからなるアーム手先速度Ｔを算出する。なお、第１の実施の形態では、以下に説明する第１から第３の算出方法を用いて、アーム手先速度Ｔを算出する。
【００４４】
（第１の算出方法）
第１の算出方法では、アーム手先速度Ｔを、ｘｙ方向の並進速度及び各軸回りの回転速度（ｔ_xｔ_yω_xω_yω_z）と、ｚ方向の並進速度（ｔ_z）の２組に分けて算出する。
【００４５】
ｘｙ方向の並進速度と各軸回りの回転速度（ｔ_xｔ_yω_xω_yω_z）については、式（３）における１,２,４,５,６行目の値からなる画像ヤコビアン行列を用いて算出する。その画像ヤコビアン行列をＬ₅ ^Tとすると、以下の式（８）が成り立つ。
【００４６】
【数７】

【００４７】
そして、画像ヤコビアン行列をＬ₅ ^Tの擬似逆行列Ｌ₅ ^T+を用いて、式（１）と同様に、ｘｙ方向の並進速度及び各軸回りの回転速度（ｔ_xｔ_yω_xω_yω_z）を求める。これにより、以下の式（９）が求められる。
【００４８】
【数８】

【００４９】
ここで、λ₅はｘｙ方向の並進速度及び各軸回りの回転速度（ｔ_xｔ_yω_xω_yω_z）を求めるためのゲインであり、λ₅＞０である。以下の説明では、ゲインλはすべて正の値をもつものとする。
【００５０】
ｚ方向の並進速度（ｔ_z）については、以下のように算出する。ここでは、画像特徴量としてＦ_i＝（ｕ_iｖ_i）を直接用いるのではなく、各ネジ穴の中心位置を結んだ線分の長さを用いる。ここで、以下の式（１０）に示すように、線分の長さｄ_i,jを定義する。
【００５１】
【数９】

【００５２】
目標位置における画像特徴量ＦがＮ個ある場合は、｛Ｎ（Ｎ−１）／２｝個の線分の長さｄ_i,jが求められる。本実施の形態では、Ｎ＝６であるので、
ｄ_1,2，ｄ_1,3，・・・，ｄ_5,6
の１５個の線分が得られる。
【００５３】
一方、現在位置の画像特徴量Ｆ^*についても同様にして、各ネジ穴の中心位置を結んだ線分の長さをｄ^* _i,jと表す。そして、ｚ方向の並進速度（ｔ_z）を、ｄ^* _i,j及びｄ^* _i,jを用いて以下の式（１１）から求める。
【００５４】
【数１０】

【００５５】
ｄ^* _i,jに対してｄ_i,jが小さい場合、つまり、（ｄ^* _i,j−ｄ_i,j）＞０の場合、ＣＣＤカメラ２は対象物１２に対して近付かなければならない。例えば図２に示す座標系においては、ＣＣＤカメラ２はｚ軸の正方向に動かなければならない。そこで、（ｄ^* _i,j−ｄ_i,j）をｄ^* _i,jで正規化した値にゲインλt_zを乗じた値をｚ方向の速度成分としている。なお、式（１１）では、｛Ｎ（Ｎ−１）／２｝個の線分に対する平均値からｚ方向の速度を求めている。
【００５６】
（第２の算出方法）
第２の算出方法では、アーム手先速度Ｔを、ｘｙ方向の並進速度（ｔ_xｔ_y）、Ｚ方向の並進速度（ｔ_z）、および各軸回りの回転速度（ω_xω_yω_z）の３組のパラメータに分けて算出する。なお、ｚ方向の並進速度（ｔ_z）については、第１の算出方法と同様にして求めればよい。
【００５７】
最初に、ｘｙ方向の並進速度（ｔ_xｔ_y）については、式（３）における第１行目及び第２行目の値からなる画像ヤコビアン行列を用いて求める。まず、ｘｙ方向の並進速度（ｔ_xｔ_y）を求めるための画像ヤコビアン行列を、以下の式（１２）で表す。
【００５８】
【数１１】

【００５９】
そして、これの擬似逆行列を用いて、式（１）と同様の方法で、ｘｙ方向の並進速度（ｔ_xｔ_y）を求める。すなわち、式（１３）を求める。
【００６０】
【数１２】

【００６１】
ここでは、λt_x,t_yはｘｙ方向の並進速度（ｔ_xｔ_y）を求めるためのゲインである。
【００６２】
また、各軸回りの回転速度（ω_xω_yω_z）については、以下のようにして求める。ここで、目標位置におけるアーム手先１Ａの位置を示す行列をＰ^*、現在位置におけるアーム手先１Ａの位置を示す行列をＰとする。目標位置行列Ｐは、回転行列Ｒと並進ベクトルｔとを用いて、以下の式（１４）で表される。
【００６３】
【数１３】

【００６４】
このとき、現在位置での姿勢Ｒから目標位置での姿勢Ｒ^*への姿勢変換行列をΔＲとすると、ΔＲ＝Ｒ^-1Ｒ^*の関係が成り立つ。
【００６５】
画像から解析する手法として知られているように、位置Ｐでの画像特徴量Ｆと位置Ｐ^*での画像特徴量Ｆ^*とから、姿勢変換行列ΔＲを求めることができる。例えば” 金谷健一著画像理解（森北出版）１９９０”の第４章において、空間上の４点以上の点を観測したカメラ移動前と移動後の画像からカメラの移動量を算出する手法が記載されている。なお、姿勢変換行列ΔＲの算出は、前記文献に記載された手法に限らず、他の公知の手法を用いてもよい。
【００６６】
そして、姿勢変換行列ΔＲからｘｙｚ軸回りの回転角度ψ，θ，φを算出することについては、よく知られているように等価回転角及び等価回転軸を求める方法に基づいて行えばよい。例えば”R.P.Paul著、吉川恒夫訳、ロボットマニピュレータ（コロナ社）１９８４”の第３０ページに記載されている方法を用いればよい。つまり、等価回転角Θ、等価回転軸ａ＝（ａ_xａ_yａ_z）^Tとすれば、ｘｙｚ軸回りの回転角度ψ，θ，φは、それぞれａ_xΘ,ａ_yΘ,ａ_zΘで求められる。
【００６７】
このように、画像特徴量Ｆ,Ｆ^*とから姿勢変換行列ΔＲを求め、さらに、姿勢変換行列ΔＲから各軸回りの回転角度ψ，θ，φを算出する。そして、各軸回りの回転速度（ω_xω_yω_z）を、算出された回転角度ψ，θ，φを用いて以下の式（１５）から求める。
【００６８】
【数１４】

【００６９】
ここで、λ_ωは、回転速度のためのゲインである。
【００７０】
（第３の算出方法）
第３の算出方法では、アーム手先速度Ｔを、ｘｙ方向の並進速度（ｔ_xｔ_y）、Ｚ方向の並進速度（ｔ_z）、ｚ軸回りの回転速度（ω_z）、ｘｙ軸回りの回転速度（ω_xω_y）の４組に分ける。なお、ｘｙ方向の並進速度（ｔ_xｔ_y）及びｚ方向の並進速度（ｔ_z）については、第１又は第２の算出方法と同様にして求めればよい。
【００７１】
最初に、ｚ軸回りの回転速度（ω_z）の算出方法について説明する。ここでは、画像特徴量Ｆの重心位置Ｃを求める。ここで重心位置Ｃは、以下の式（１６）により求められる。
【００７２】
【数１５】

【００７３】
重心位置Ｃから各画像特徴量Ｆへ向かう単位ベクトルｆ＝（ｕｆ_iｖｆ_i）^Tを求める。単位ベクトルｆは、以下の式（１７）より求められる。
【００７４】
【数１６】

【００７５】
同様に、目標位置での画像特徴量Ｆ^*についても、その重心位置Ｃ^*から各画像特徴量Ｆ^*へ向かう単位ベクトルｆ^*を求める。そして、単位ベクトルｆから単位ベクトルｆ^*への回転角度φを最小２乗法により求める。
【００７６】
つまり、ｃ＝cosφ、ｓ＝sinφと表すと、ｆ＝（ｕｆｖｆ）^Tとｆ＝（ｕｆ^* ｖｆ^*）^Tの間には、以下の式（１８）が成り立つと推定される。
【００７７】
【数１７】

【００７８】
そこで、すべてのｆ及びｆ^*について、以下の式（１９）のようにおく。
【００７９】
【数１８】

【００８０】
そして、式（１９）の左辺を行列Ｂ、その右辺を行列Ａとおくと、以下の式（２０）により（ｃ，ｓ）が求められる。
【００８１】
【数１９】

【００８２】
ただし、ｃ²＋ｓ²＝１の拘束条件を用いない。すなわち、単純に最小２乗法により（ｃ，ｓ）を求めたため、必ずしもｃ²＋ｓ²＝１は保証されない。このため、φ＝cos^-1ｃまたはφ＝sin^-1ｓから回転角度φを求めればよい。
【００８３】
また、図７に示すように、目標位置に対して現在位置がｘｙ軸回りの回転を含んでいる場合は、前記方法では正確なｚ軸回りの回転角度φを求めることができない。しかし、得られた回転角度φ及び後述の方法で求められるｘｙ軸回りの回転角度に基づいて逐次姿勢制御することで、最終的に目標位置に制御することができる。
【００８４】
このようにして、ｚ軸回りの回転速度ω_zは、以下の式（２１）により算出する。
【００８５】
【数２０】

【００８６】
最後に、ｚ軸回りの回転速度ω_z及びω_yは、第１及び第２の算出方法と同様にして、姿勢変換行列ΔＲからｘｙ軸回りの回転角度ψ，θを求める。つまり、式（１５）と同様にして、以下の式（２２）によって求められる。
【００８７】
【数２１】

【００８８】
このように、ロボットコントローラ４は、第１から第３の算出方法のいずれか１つに従ってアーム手先速度Ｔ＝（ｔ_xｔ_yｔ_zω_xω_yω_z）を算出して、ステップＳＴ５に進む。
【００８９】
なお、同様にして、アーム手先速度Ｔを、（ｔ_xｔ_yω_xω_yω_z）及び（ｔ_z）に分けて算出してもよい。さらに、（ｔ_xｔ_yω_xω_y）、（ｔ_z）、（ω_z）に分けて算出してもよい。
【００９０】
ステップＳＴ５において、ロボットコントローラ４は、算出されたアーム手先速度Ｔになるようにアーム手先１Ａを制御して、再びステップＳＴ１に戻る。
【００９１】
以上のように、ロボット装置は、アーム手先速度Ｔをいくつかの速度成分に分解し、その速度成分毎に独立して画像特徴量Ｆ,Ｆ^*を用いることによって、ロボットアーム１の制御に好適なアーム手先速度Ｔを算出することができる。特に、第１の算出方法では、アーム手先速度Ｔの速度成分ｔ_zを独立して求めているので、他の速度成分の影響を受けることなく、より正確に速度成分ｔ_zを算出することができる。また、この方法では、各速度成分の組毎にゲインを調整できるため、対象物１２に対するロボットアーム１の動きを細かく制御することができる。例えば図３に示すように、対象物１２に対する目標位置において対象物１２が画像中心に観測され、一方現在位置では対象物１２が画像周縁部に観測されている場合、第２及び第３の算出方法における（ｔ_x，ｔ_y）の速度ゲインλt_x,t_yを大きな値にすることで、アーム制御時には、対象物１２をすばやく画像中心位置で観測できるように制御可能である。
【００９２】
本実施の形態ではアーム手先速度Ｔを構成する６つの速度成分を求めていたが、ロボットアーム１の制御に制限を加える場合は、その制限に応じて一部の速度成分だけを求めてもよい。
【００９３】
例えば図８に示すように、現在位置から目標位置への姿勢変換行列ΔＲにおいてｘｙ軸回り回転速度が零であり、かつアーム手先１Ａの座標系に対してｘｙ軸回りに姿勢の変換をしない対象物１２に対しては、アーム手先１Ａのｘｙｚ方向の並進及びｚ軸回りの回転の４自由度の制御のみによって目標位置への制御が可能なこともある。この場合、必ずしもロボットアーム１の６自由度を制御する必要はなく、作業内容によっては制御すべき自由度に制限を加えてもよい。
【００９４】
これは、例えば図９に示すように、パレット２１から部品２２を取り出す場合に適用することができる。ここで、部品２２底面は平面であり、アーム手先１Ａのｘｙ平面に対してパレット２１、部品２２の底面は平行であるものとする。一方、パレット２１は２段積み重ねで供給されるか、あるいは１段のみで供給されるかは不明であるものとする。さらに、上段の部品２２を取り出した後、下段の部品２２も続けて取り出す作業を実施するものとする。
【００９５】
この場合、アーム手先１Ａの制御すべき自由度は、アーム手先座標系におけるｘｙｚ方向の並進成分とｚ軸回りの回転成分の４自由度である。したがって、上述したように、その他の方向の制御に制限を加えることによって、制御内容を簡単にすることができる。この場合では、ロボットコントローラ４は、ｘｙ軸回りの回転速度（ω_xω_y）を算出する必要がないので、簡易にロボットアーム１を制御することができる。
【００９６】
［第２の実施の形態］
つぎに、本発明の第２の実施の形態について説明する。上述した第１の実施の形態と同一の部位には同一の符号を付し、また重複する部分の説明は省略する。
【００９７】
第２の実施の形態に係るロボット装置は、図１と同様に構成されているが、対象物１２の画像特徴量だけでなく、対象物１２以外から観測される画像特徴量も用いてロボットアーム１を制御する。
【００９８】
最初に準備作業として、第１の実施の形態と同様に、目標位置における対象物１２の画像特徴量を取得する。ロボットコントローラ４は、図１０に示すように、ロボットアーム１のアーム手先１Ａを、対象物１２に対して相対的な目標位置に配置する。そして、ＣＣＤカメラ２は、図１１に示すように、対象物１２を撮像し、この画像をセンサコントローラ３に供給する。
【００９９】
センサコントローラ３は、第１の実施の形態と同様にして、ＣＣＤカメラ２からの撮像画像に基づいて対象物１２の画像特徴量を抽出する。例えば、対象物１２の上面に６つのネジ穴が形成されていた場合、目標位置での画像特徴量Ｆ^* _i（ｉ＝１,・・・,６）を抽出する。
【０１００】
さらに、センサコントローラ３は、対象物１２以外についても画像特徴量Ｘ^*を抽出する。例えば図１１に示すように、対象物設置台１１の表面の４つの十字図形の各交点について、画像中の座標値（ｕ^' _i，ｖ^' _i）を画像特徴量Ｘ^* _i（ｉ＝１,・・・,４）として抽出する。ロボットコントローラ４は、センサコントローラ３で抽出された目標位置での画像特徴量Ｆ^*，Ｘ^*を記憶し、これを後述する処理で用いる。
【０１０１】
なお、十字図形の各線分の交点は、従来から用いられているエッジ検出や線分当てはめ等の手法を用いればよい。また、本実施の形態では、対象物設置台１１の表面にある十字画像の交点を画像特徴量Ｘ^*としたが、その他、線分端点、コーナ、二値化画像の孤立領域の面積の重心など、他の画像特徴量を用いてもよい。さらに、画像特徴量Ｘ^*を示す座標値（ｕ，ｖ）は、画像の中心を原点とし、その画像の横方向をｘ軸、その縦方向をｙ軸とする画像座標系で定義されたものであるが、その他、例えばカメラ座標系で定義したものでもよい。
【０１０２】
このような準備作業が終了すると、次にロボットコントローラ４は、現在位置にあるアーム手先１Ａを、対象物１２に対する目標位置に移動させる制御を行う。
【０１０３】
アーム手先１Ａの現在位置が例えば図１２に示す位置にある場合、ＣＣＤカメラ２は、対象物１２を撮像して、図１３に示すような画像を出力する。そして、図１４に示すステップＳＴ１１以下の処理を実行する。
【０１０４】
ステップＳＴ１１において、センサコントローラ３は、第１の実施の形態と同様にして、現在位置における画像特徴量Ｆ_i（ｉ＝１，・・・，６）を抽出する。さらに、センサコントローラ３は、対象物設置台１１の表面にある各十字図形の交点画像中での座標値（ｕ_i，ｖ_i）を、現在位置における画像特徴量Ｆ_i（ｉ＝１，・・・，６）として抽出して、ステップＳＴ１２に進む。
【０１０５】
ステップＳＴ１２において、ロボットコントローラ４は、現在位置での画像特徴量Ｆと目標位置の画像特徴量Ｆ^*の差、つまり上述した式（４）に示すベクトルｅを求める。さらに、ロボットコントローラ４は、画像特徴量Ｘ^*，Ｘについて、各点間を結んだ線分の長さを以下の式（２３）及び式（２４）により求める。
【０１０６】
【数２２】

【０１０７】
そして、ｅ、ｄ_i,j，ｄ^* _i,jを用いて、画像特徴量の差residを以下の式（２５）から求める。
【０１０８】
【数２３】

【０１０９】
ここで、Ｎは対象物１２から観測された画像特徴量Ｆ，Ｆ^*の個数であり、ここではＮ＝６である。また、Ｍは対象物１２以外から観測された画像特徴量Ｘ，Ｘ^*の個数であり、ここではＭ＝４である。
【０１１０】
なお、式（２５）の右辺第１項では、判定基準としてベクトルｅの各要素の大きさの平均を用いたが、例えば画像特徴量Ｆ_iと画像特徴量Ｆ_i ^*の距離の平均を用いてもよい。すなわち、ロボットコントローラ４は、画像特徴量の差residとして以下の式（２６）を求めてもよい。
【０１１１】
【数２４】

【０１１２】
なお、式（２５）及び式（２６）の右辺第２項は、画像特徴量Ｘ，Ｘ^*の各点を結んだ線分の長さを用いている。これにより、第１の実施の形態に比べて、より精密にアーム手先１Ａの位置決めをすることができる。ロボットコントローラ４は、以上のようにして画像特徴量の差residを求めると、ステップＳＴ１３に進む。
【０１１３】
ステップＳＴ１３において、ロボットコントローラ４は、画像特徴量の差residが所定の閾値ε以下であるかを判定し、それが閾値ε以下であるときはアーム手先１Ａが目標位置に制御されたと判定して、処理を終了する。一方、画像特徴量の差residが所定の閾値ε以下でないときは、ステップＳＴ１４に進む。
【０１１４】
ステップＳＴ１４において、ロボットコントローラ４は、画像特徴量Ｆ^*，Ｘ^*，Ｆ，Ｘに基づいてアーム手先速度Ｔを算出する。なお、第２の実施の形態では、以下に説明する第１又は第２の算出方法を用いて、アーム手先速度Ｔを算出する。
【０１１５】
（第１の算出方法）
第１の算出方法では、アーム手先速度Ｔを、ｘｙ方向の並進速度及びｚ軸回りの回転速度の（ｔ_xｔ_yω_z）、ｚ方向の並進速度（ｔ_z）、ｘｙ軸回りの回転速度（ω_xω_y）の３組に分けて算出する。
【０１１６】
最初に、ｘｙ方向の並進速度及びｚ軸回りの回転速度の（ｔ_xｔ_yω_z）の算出方法について説明する。まず、上述した式（３）に示す行列の１，２，６行目の値を用いて、以下の式（２７）に示す画像ヤコビアン行列を求める。
【０１１７】
【数２５】

【０１１８】
上記式（２７）の擬似逆行列を用いて、式（１）と同様にして速度（ｔ_xｔ_yω_z）を求める。具体的には、以下の式（２８）を演算して速度（ｔ_xｔ_yω_z）を求める。
【０１１９】
【数２６】

【０１２０】
ここで、ｅは式（４）で定義したベクトル、λ_tx,ty,ω_zは速度（ｔ_xｔ_yω_z）を求めるためのゲインであり、λ_tx,ty,ω_z＞０である。
【０１２１】
つぎに、ｚ方向の並進速度（ｔ_z）の算出方法について説明する。ここでは、画像特徴量Ｘ^*，Ｘを用いて、各特徴点を結んだ直線の長さを求める。
【０１２２】
対象物１２と対象物設置台１１の十字図形の位置関係は、図１０に示す目標位置の場合と、図１２に示す現在位置の場合とでは、それぞれ異なっている。したがって、アーム手先１Ａを対象物１２に対する目標位置に制御するためには、対象物設置台１１の画像特徴量Ｘ^*，Ｘを直接用いることはできない。
【０１２３】
そこで、上述した式（２３）で定義された線分の長さｄ_i,jを求めている。なお、Ｍ個の画像特徴量Ｘがある場合は、[Ｍ（Ｍ−１）／２]個のｄ_i,jが得られる。ここでは、Ｍ＝４であるので、ｄ_1,2，ｄ_1,3，・・・，ｄ_3,4の６個の線分が得られる。式（２４）に示した画像特徴量Ｘ^*から求められた線分の長さをｄ^* _i,jとおくと、ｚ方向の並進速度（ｔ_z）は、ｄ_i,jとｄ^* _i,jから、式（２９）によって求められる。
【０１２４】
【数２７】

【０１２５】
ｄ^* _i,jに対してｄ_i,jが小さい場合、すなわち[（ｄ^* _i,j−ｄ_i,j）＞０]の場合、ＣＣＤカメラ２は対象物１２に近付かなければならない。つまり、図１０に示す手先の座標系において、ＣＣＤカメラ２はｚ軸の正方向に動かなければならない。よって、（ｄ^* _i,j−ｄ_i,j）をｄ^* _i,jで正規化した値に、ゲインλ_tzを乗じた値をｚ方向の速度としている。なお、式（２９）では、[Ｍ（Ｍ−１）／２]個の線分に対する平均値からｚ方向の速度を求めている。
【０１２６】
このように、画像特徴量Ｘ，Ｘ^*の線分の長さの比を用いることによって、奥行きの速度成分であるｚ方向の並進速度（ｔ_z）を求めている。これにより、上述した図１７に示すような場合は画像特徴量Ｆ，Ｆ^*だけではｚ方向の並進速度を正確に求めることが困難であるが、対象物１２の周囲にある対象物設置台１１の画像特徴量Ｘ，Ｘ^*を用いることによってより正確に求めることができる。
【０１２７】
なお、式（２９）において、画像特徴量Ｘ，Ｘ^*の線分の長さの比だけでなく、画像特徴量Ｆ，Ｆ^*の線分の長さの比も混在させてｚ方向の並進速度を求めてもよい。また、抽出された画像特徴量Ｆ，Ｆ^*が画像の一部分に偏在していなければ、画像特徴量Ｆ，Ｆ^*の線分の長さの比だけを用いてもよい。
【０１２８】
最後に、ｘｙ軸回りの回転速度（ω_xω_y）の算出方法については、第１の実施の形態で説明した式（１４）及び式（１５）と同様にすればよい。すなわち、第１の実施の形態と同様に公知の手法を用いて、位置Ｐでの画像特徴量Ｘと、位置Ｐ^*での画像特徴量Ｘ^*とから、姿勢変換行列ΔＲを求める。そして、例えばロール、ピッチ、ヨー変換の解法に基づいて、姿勢変換行列ΔＲからｘ，ｙ軸回りの回転角度ψ，φを算出する。この結果、式（１５）より、ｘ，ｙ軸回りの回転速度（ω_xω_y）を算出することができる。なお、本実施の形態では、式（１５）におけるω_zは考慮しなくてよい。
【０１２９】
（第２の算出方法）
第２の算出方法では、アーム手先速度Ｔを、ｘｙ方向の並進速度（ｔ_xｔ_y）、ｚ方向の並進速度（ｔ_z）、ｚ軸回りの回転速度（ω_z）、ｘｙ軸回りの回転速度（ω_xω_y）の４組に分けて算出する。
【０１３０】
ｚ方向の並進速度（ｔ_z）及びｘｙ軸回りの回転速度（ω_xω_y）については、本実施の形態の第１の算出方法と同様にして算出すればよい。また、ｘｙ方向の並進速度（ｔ_xｔ_y）については、第１の実施の形態の第２の算出方法で説明した式（１２）及び式（１３）を用いて算出すればよい。ｚ軸回りの回転速度（ω_z）については、第１の実施の形態の第３の算出方法で説明した式（１６）から式（２１）を用いて算出すればよい。
【０１３１】
ロボットコントローラ４は、このように第１又は第２の算出方法に従ってアーム手先速度Ｔを算出するとステップＳＴ２５に進み、アーム手先速度Ｔになるようにアーム手先１Ａを制御して、ステップＳＴ２１に戻る。
【０１３２】
このように、このロボット装置は、対象物１２の画像特徴量Ｆ^*，Ｆと、対象物１２の周辺、例えば対象物設置台１１の画像特徴量Ｘ^*，Ｘとを用いて、アーム手先速度Ｔを算出する。すなわち、図１５に示すように、画面全体にわたって分布している対象物１２以外の画像特徴量Ｘ，Ｘ^*を用いて、その画像特徴量Ｘ，Ｘ^*から得られる線分長さの比及び姿勢変換行列ΔＲを求めることによって、正確にｚ方向の並進速度及びｘｙ軸回りの回転速度を求めることができる。
【０１３３】
なお、本実施の形態の第１及び第２の算出方法において、ｚ方向の並進速度ｔ_zを画像特徴量を結ぶ線分の比によって式（２９）から求めているが、本発明はこれに限定されるものではない。例えば画像特徴量Ｘ^*，Ｘによって構成される凸包図形の面積を用いて並進速度ｔ_zを求めてもよい。図１１に示す目標位置での画像特徴量Ｘ^*１〜Ｘ^*４で構成される四角形の面積をＳ^*、図１３に示す現在位置での画像特徴量Ｘ１〜Ｘ４で構成される四角形の面積をＳとすると、
ｔ_z＝λｚ（Ｓ^*−Ｓ）／Ｓ^*
から並進速度ｔ_zを求めることができる。
【０１３４】
この結果、上述した図１７に示すように、対象物１２から得られる画像特徴量Ｆ，Ｆ^*が画像中のある領域に偏在している場合であっても、従来に比べて、ロボットアーム１のアーム手先１Ａのｚ方向の奥行き位置、ｘｙ軸回りの姿勢を精度よく制御することができる。また、上述した図１８に示すように、画像特徴量がｙ軸方向に沿って直線状に並んでいる場合であっても、ｘ軸回りだけでなくｙ軸回りの姿勢も精度よく制御することができる。
【０１３５】
なお、本発明は、上述した図９に示すように、パレット２１が図示しないフォークリフトやＡＧＶ（移動台車）によって保持されている状態において、ロボットアーム１が対象物１２として部品２２を取り出す場合にも適用することができる。
【０１３６】
この場合、フォークリフトの傾きなどにより、アーム手先１Ａのｘｙ平面に対して、パレット２１や部品２２の底面は必ずしも平行ではない。そのため、アーム手先１Ａのｘｙ軸回りの姿勢については、パレット２１のコーナを用いて制御する。すなわち、パレット２１のコーナから画像特徴量Ｘ，Ｘ^*を抽出すればよい。
【０１３７】
このとき、ロボットコントローラ４は、対象物１２から得られる画像特徴量Ｆ，Ｆ^*を用いて（ｔ_x，ｔ_y，ω_z）を算出し、パレット２１コーナの画像特徴量Ｘ，Ｘ^*から得られる線分の長さを用いて（ｔ_z）を求め、さらに、この画像特徴量Ｘ，Ｘ^*を用いてｘｙ軸回りの回転速度（ω_x，ω_y）を算出すればよい。
【０１３８】
また、第２の実施の形態では、ロボットアーム１を６自由度で制御するために、アーム手先速度Ｔとして６つのパラメータを求めていた。しかし、第１の実施の形態と同様に４自由度で制御する場合は、アーム手先速度Ｔとして４つのパラメータだけを求めてもよい。
【０１３９】
【発明の効果】
本発明は、少なくとも、所定の軸方向の並進速度を第１の速度成分に振り分け、その他の軸方向の並進速度を第２の速度成分に振り分け、（１）各軸回りの回転速度の全部を第１の速度成分に振り分け、又は、（２）各軸回りの回転速度の全部を第１及び第２の速度成分と異なる他の速度成分に振り分け、又は、（３）各軸回りの回転速度の一部を第１の速度成分に振り分け、残りの回転速度を第１及び第２の速度成分と異なる他の速度成分に振り分け、各速度成分を他の速度成分を用いることなく算出することによって、アーム手先の各速度成分の影響を回避して、精密にアーム手先の位置決めをすることができる。
【０１４０】
また、本発明は、第１の対象物特徴量及び第１の周辺特徴量と、ロボットアームの手先が現在位置にあるときに抽出された第２の対象物特徴量及び第２の周辺特徴量とに基づいて、手先を対象物に移動させる速度を算出することによって、より正確にアーム手先の速度を算出して、精密にアーム手先の位置決めをすることができる。
【図面の簡単な説明】
【図１】本発明の実施の形態に係るロボット装置の構成を示すブロック図である。
【図２】ロボットアームのアーム手先が対象物に対する目標位置にあるときの状態を説明する図である。
【図３】目標位置で撮像された対象物の画像特徴量を示す図である。
【図４】ロボットアームのアーム手先が現在位置にあるときの状態を説明する図である。
【図５】現在位置で撮像された対象物の画像を示す図である。
【図６】ロボット装置の動作内容を説明するフローチャートである。
【図７】目標位置に対して現在位置がｘｙ軸回りの回転を含んでいる状態を説明する図である。
【図８】アーム手先のｘｙｚ方向の並進及びｚ軸回りの回転の４自由度で制御する状態を説明する図である。
【図９】２段積みのパレットに部品が入っている場合のアプリケーションに適用した場合を説明する図である。
【図１０】第４の実施の形態において、ロボットアームのアーム手先が対象物に対する目標位置にあるときの状態を説明する図である。
【図１１】目標位置で撮像された対象物及び対象物設置台の画像特徴量を示す図である。
【図１２】ロボットアームのアーム手先が現在位置にあるときの状態を説明する図である。
【図１３】現在位置で撮像された対象物の画像を示す図である。
【図１４】ロボット装置の動作内容を説明するフローチャートである。
【図１５】画面全体にわたって分布している対象物以外の画像特徴量を用いて、アーム手先速度を算出することを説明する図である。
【図１６】目標位置において対象物の全体を撮像して、すべての画像特徴量を抽出する状態を説明する図である。
【図１７】画像特徴量が撮像画像のある領域に偏在している状態を説明する図である。
【図１８】画像特徴量が直線状に並んでいる状態を説明する図である。
【符号の説明】
１ロボットアーム
１Ａアーム手先
２ＣＣＤカメラ
３センサコントローラ
４ロボットコントローラ

Claims

所定の作業を行うロボットアームと、
前記ロボットアームの手先の近傍に固定された状態で対象物を撮像する撮像手段と、
前記撮像手段で撮像された対象物の画像に基づいて、前記対象物の特徴部分の座標を画像特徴量として抽出する特徴量抽出手段と、
前記ロボットアームの手先が前記対象物に対して相対的な目標位置にあったときに、前記特徴量抽出手段によって予め抽出された画像特徴量を記憶する特徴量記憶手段と、
前記特徴量記憶手段に記憶されている画像特徴量と、前記手先が現在位置にあるときに前記特徴量抽出手段で抽出された画像特徴量とに基づいて、複数の軸方向の並進速度と各軸回りの回転速度を算出するにあたり、少なくとも、所定の軸方向の並進速度を第１の速度成分に振り分け、その他の軸方向の並進速度を第２の速度成分に振り分け、
（１）各軸回りの回転速度の全部を第１の速度成分に振り分け、又は、
（２）各軸回りの回転速度の全部を第１及び第２の速度成分と異なる他の速度成分に振り分け、又は、
（３）各軸回りの回転速度の一部を第１の速度成分に振り分け、残りの回転速度を第１及び第２の速度成分と異なる他の速度成分に振り分け、
各速度成分を他の速度成分を用いることなく算出する速度算出手段と、
前記速度算出手段で算出された各速度成分に含まれる並進速度及び回転速度に基づいて、前記ロボットアームの手先を前記対象物に対して相対的な目標位置に移動させる制御を行うアーム制御手段と、
を備えたロボット装置。
前記速度算出手段は、前記ロボットアームの手先が１つ以上の回転速度の回転方向において制限されている場合に、制限されている回転速度のみを含む第３の速度成分を算出せず、当該第３の速度成分以外の速度成分を算出する請求項１記載のロボット装置。
所定の作業を行うロボットアームと、
前記ロボットアームの手先の周辺に固定された状態で対象物及びその周辺を撮像する撮像手段と、
前記撮像手段で撮像された対象物及びその周辺の画像に基づいて、前記対象物及びその周辺の特徴部分の座標を画像特徴量としてそれぞれ抽出する特徴量抽出手段と、
前記ロボットアームの手先が前記対象物に対して相対的な目標位置にあったときに、前記特徴量抽出手段によって予め抽出された対象物及びその周辺の画像特徴量を、第１の対象物特徴量及び第１の周辺特徴量として記憶する特徴量記憶手段と、
前記特徴量記憶手段に記憶されている第１の対象物特徴量及び第１の周辺特徴量と、前記ロボットアームの手先が現在位置にあるときに前記特徴量抽出手段で抽出された対象物及びその周辺の画像特徴量である第２の対象物特徴量及び第２の周辺特徴量とに基づいて、前記手先を前記対象物に移動させる速度を算出する速度算出手段と、
前記速度算出手段で算出された速度に基づいて、前記ロボットアームの手先を前記対象物に対して相対的な目標位置に移動させる制御を行うアーム制御手段と、
を備えたロボット装置。
前記速度算出手段は、前記第１及び第２の周辺特徴量間の線分の長さを算出し、各線分の長さに基づいて所定軸方向の並進速度を算出する請求項３記載のロボット装置。
前記速度算出手段は、前記第１の周辺特徴量の位置から前記第２の周辺特徴量の位置に姿勢を変換する姿勢変換行列に基づいて、所定軸回りの回転速度を算出する請求項３または４記載のロボット装置。