ビジュアルサーボによる位置姿勢制御方法

【課題】ビジュアルサーボによる位置姿勢制御においてカメラの縦揺れ角および偏揺れ角（姿勢角）の検出精度向上を図る。
【解決手段】ロボット本体１１は、制御装置１２により位置の移動と姿勢の駆動制御がなされる。ＣＣＤカメラ１５は、ロボット本体１１の上アーム２０に固定され、ワークＷを撮影する。あらかじめ参照画像として基準位置からワークＷを撮影したデータを記憶させる。制御装置１２は、カメラ１５で撮影した画像と参照画像のデータとに基づいてビジュアルサーボにより位置姿勢の制御を行なう。収束した段階では、カメラ１５を姿勢角を所定以上傾けて画像データを取得する。このとき各画像での姿勢角と参照画像のデータとの誤差値を演算し、誤差値が最も小さくなるときの姿勢角を推定し、補正量として求める。この補正量だけ位置姿勢を制御することで、精度の高い制御を行なうことができる。

【発明の詳細な説明】
【０００１】
【発明の属する技術分野】本発明は、カメラにより撮影した画像データに基づいて位置姿勢の制御を行なうようにしたビジュアルサーボによる位置姿勢制御方法に関する。
【０００２】
【発明が解決しようとする課題】ロボットなどによりワークを把持させる場合などで、ハンドに取り付けたカメラによりワークを撮影してハンドの位置姿勢を制御するようにしたビジュアルサーボの技術がある。これは、あらかじめ参照画像を登録してその参照画像と同じようにワークが見える位置姿勢となるように制御することで、迅速且つ正確にハンドの位置姿勢制御を行なうようにした方法である。この場合、参照画像は、ロボットのハンドを位置姿勢制御したい場所に配置してワークを把持するときの状態にし、ハンドに取り付けたカメラで撮影したものを用いる。
【０００３】このようなビジュアルサーボの技術を採用することで、ロボットのハンドをカメラが捕らえる画像の分解能の範囲で正確に位置姿勢制御を行なうことができるが、研究レベルにおいては技術的な問題は生じていないように見えていたが、実用上においては次のような点で不具合が生じることが発明者の研究により明らかになりつつある。
【０００４】例えば、ロボットのハンドにより対象物を把持したり部品を取り付けたりするなどの作業において、位置決め精度が要求される場合に生ずるもので、ＣＣＤカメラなどでワークを撮影する画像の性質上、画素数と分解能の関係で精度の向上に限界が生ずるものである。すなわち、カメラの光軸方向をｚ軸とし、撮影した画面をｘｙ平面とすると、ビジュアルサーボでは、カメラで撮影した二次元画像データから得られるｘｙ平面内の位置（ｘ，ｙ）およびｚ軸回りの回転（Ｒｚ）に対しては高精度の制御を行なうことができるが、ｘ軸あるいはｙ軸回りの回転（Ｒｘ，Ｒｙ）や奥行き方向（ｚ）の補正に対しては精度に限界がある。
【０００５】例えば、図１４（ａ），（ｂ）に示すように、カメラ１により対象物Ｐ１およびＰ２を撮影する場合において、ｘ軸回りＲｘの回転角度である縦揺れ角の微小な変化に対して、カメラ１ａ（Ｒｘ方向に微小角度だけ傾いた位置）の撮影画像におけるＣＣＤの画素数の変化は図１５（ａ）の位置から（ｂ）あるいは（ｃ）に変化した状態では、量子化誤差があるため画像上の変化として捕らえることができる範囲には限界がある。この場合、現状では例えば±２°程度の分解能である。
【０００６】対象物Ｐ１とＰ２とでは、それぞれの高さ寸法ｈ１，ｈ２がｈ１＞ｈ２となっており、奥行き方向の寸法（ｚ軸方向の寸法）が対象物Ｐ１の方が大きいので、Ｒｘの変化に対して変化量Ｅｒｒｏｒ１の方がＥｒｒｏｒ２よりも大きくなり、その変化角度を捉ええやすいが、奥行き方向の寸法が小さい対象物Ｐ２の場合には縦揺れ角の変化角度の補正精度が低くなる（図１５（ｄ）参照）。
【０００７】これは、図１６（ａ）に示すような対象物Ｐ１において、幅寸法がＤの場合に、Ｒｘの変化に対する撮影画像上での幅寸法ｄは、同図（ｂ）に示すような関係として表される。すなわち、この関係は、ｄ／Ｄ＝ cos（Ｒｘ）
と表されるから、Ｒｘが小さい領域では、角度に対する変化量が小さくなり、図１７に示すように、画素ｐｉｘの数の違いとしてカウントできる範囲が広くなり、その分だけ分解能が低下するのである。つまり、ビジュアルサーボでは、姿勢角の要素である縦揺れ角（pitch ）および偏揺れ角（yaw ）の精度について、カメラ１の量子化精度に大きく依存することになるのである。
【０００８】上述の不具合を回避して分解能を高めるためには、例えば、カメラ１以外に別途にセンサを設けて、ビジュアルサーボによる位置姿勢の制御が収束した状態では、センサの検出信号に基づいて位置姿勢の補正をすることで制御精度の向上を図ることが必要となる。
【０００９】本発明は、上記事情に鑑みてなされたもので、その目的は、ビジュアルサーボによる位置姿勢制御のみを用いる場合においても、位置姿勢制御が収束した状態でさらに参照画像の基準位置に近い位置姿勢となるように高い精度で制御することができるようにしたビジュアルサーボによる位置姿勢制御方法を提供することにある。
【００１０】
【課題を解決するための手段】請求項１の発明によれば、第１の過程で、異なる姿勢でカメラにより対象物を撮影した画像を記憶し、第２の過程で、記憶した複数の画像のデータから推定される少なくとも縦揺れ角および偏揺れ角を示す姿勢データと、画像のデータおよび参照画像のデータの間の誤差を示す誤差データとの相関から、誤差データが許容誤差量以下となる基準位置に相当する位置姿勢を推定し、第３の過程で、推定した基準位置に相当する位置姿勢への補正量を演算し、第４の過程で、ビジュアルサーボによる位置姿勢制御が収束した状態での位置姿勢に対して第３の過程で演算した補正量に基づいて制御体操装置の位置姿勢の補正を行なうようにしたので、カメラで撮影した画像からは位置姿勢を示す縦揺れ角および偏揺れ角の不感帯となる領域、つまりビジュアルサーボの位置姿勢制御では収束状態となって量子化誤差に含まれて画像データとして差異が現れない領域にある場合でも、さらに位置姿勢の補正を行なってより正確な位置姿勢に制御することができるようになる。これにより、別途にセンサなどを設ける構成としなくとも、位置姿勢の制御精度を高めることができるようになる。
【００１１】請求項２の発明によれば、上記請求項１の発明において、第１の過程を、ビジュアルサーボによる位置姿勢制御が収束した状態で、カメラの位置姿勢を姿勢データが判定角度以上変化するようにしたときの画像を記憶することにより実行するようにしたので、ビジュアルサーボによる位置姿勢制御が収束している状態すなわち、最も対象物に近接して基準位置に近い位置を起点としてここから第１の過程以降の処理を実施することで、位置姿勢の補正量を正確且つ迅速に検出して位置姿勢を補正することができるようになる。
【００１２】請求項３の発明によれば、請求項１の発明において、第１の過程を、ビジュアルサーボによる位置姿勢制御が収束する状態となるまでの期間中におけるカメラの画像を記憶することにより実行するようにしたので、ビジュアルサーボによる位置姿勢制御を実行しながら対象物に近接する過程でカメラが撮影した画像を第１の過程として取り込んでいくことができ、ビジュアルサーボの制御と処理を並行して補正量の算出を行なうことができ、迅速に位置姿勢の制御を行なうことができるようになる。
【００１３】請求項４の発明によれば、上記請求項３の発明において、第１の過程を実施するに際して、ビジュアルサーボによる位置姿勢制御が収束する状態となるまでの期間中のカメラの位置姿勢の制御を行なうときの姿勢データの変化量が判定角度に達しない場合には、その姿勢データの変化量が判定角度以上となるように変化させるようにしたので、上述の場合において、対象物に近接するときに実際にカメラがあまり振れない場合つまり姿勢データの変化量が補正量を求めるための判定角度以上振れない場合には、位置姿勢の補正を正確に行なえるようにするために、画像取り込みのための姿勢をとるように制御するようにしたので、第１の過程を実行しながら近接する場合でも確実に補正量の値を算出することができるようになる。
【００１４】請求項５の発明によれば、上記各発明において、参照画像として記憶する対象物の画像データは、基準点に対して複数の特徴点を設定し、その基準点との間を結ぶ複数のベクトルを含んだものとして設定し、第２の過程を実施する際には、少なくともベクトルの長さもしくはベクトル間の長さの比に基づいて算出した誤差データにより位置姿勢の推定を行なうようにしたので、例えば、カメラにより撮影した画像と参照画像との間に、カメラの姿勢データの縦揺れ角や偏揺れ角のずれが生じている場合などで、対象物の基準点に対して一方側の特徴点とを結んでできるベクトルが長くなると共に、他方側の特徴点とをむすんでできるベクトルが短くなるような場合に、対象物の全長をもとに判断する場合と異なり、双方の差を検出することにより、姿勢データに対する検出能力を高めて、精度の高い補正量算出を行なうことができるようになる。
【００１５】請求項６の発明によれば、上記各発明において、参照画像として記憶する対象物の画像データは、基準点に対して複数の特徴点を設定し、それら特徴点間を結ぶ複数のベクトルを含んだものとして設定され、第２の過程を実施する際には、少なくともベクトルの長さもしくはベクトル間の長さの比に基づいて算出した誤差データにより位置姿勢の推定を行なうようにしたので、上述と同様にして姿勢データに対する検出能力を高めて、精度の高い補正量算出を行なうことができるようになる。
【００１６】請求項７の発明によれば、請求項５および６の発明において、参照画像として記憶する対象物の画像データは、設定された複数のベクトルについて、ベクトル間の角度に基づいて算出した誤差データにより位置姿勢の推定を行なうようにしたので、２つのベクトルのなす角度も補正量算出のデータとして捕らえて参照画像のデータに対する比較をより厳密に行なって精度の高い補正量算出処理を行なうことができるようになる。
【００１７】請求項８の発明によれば、請求項５ないし７の発明において、誤差データの算出を、ベクトルについての長さあるいは角度について撮影した画像と参照画像との間の差分の自乗和として求め、位置姿勢の推定を、上記方法により算出される誤差データの最小値を得るときの姿勢データとするようにしたので、変数としての姿勢データの値に対して参照画像との差分の絶対値の和に相当する値を関数値としてその最小値を求めることで、関数の最小値つまりもっとも参照画像に近接した位置姿勢の姿勢データを求めることができるようになる。この場合、上述した関数を変数に対応してプロットし、その最小値を求めるように演算を行なうようにしても良いし、簡易的に関数の値が許容誤差値以下となるような範囲の変数値の中間値を採用するということでも求めることができる。
【００１８】この場合、請求項９の発明のように、位置姿勢の推定を、誤差データの値と姿勢データの縦揺れ角および偏揺れ角の値とから、姿勢データを変数とした誤差データ関数を求め、その誤差データ関数の最小値を演算による求めることで行なうようにすると良い。
【００１９】請求項１０の発明によれば、上記各発明において、ビジュアルサーボによる位置姿勢制御が収束した状態で、カメラの位置をその収束位置から近接した位置に移動させて第１ないし第４の過程を実施するので、収束位置では撮影された画像の範囲では精度が得られない場合でも、近接した位置で撮影することで拡大した状態の撮影画像を得ることができ、これによって姿勢データに関して精度を向上した画像データを得ることができるようになり、位置姿勢の補正をより高い精度で行なうことができるようになる。また、これによって、姿勢データのうちで対象物の形状に起因して精度を上げ難い姿勢データがある場合でも、位置姿勢の補正精度を高めることができるようになる。
【００２０】請求項１１の発明によれば、上記した請求項１０の発明において、ビジュアルサーボによる位置姿勢制御が収束した状態で、姿勢データのうちの検出精度の向上が必要とされる姿勢データについてその収束位置から近接した位置に移動させて実施するので、必要な姿勢データについてのみ、近接した位置まで移動した状態で位置姿勢の補正のための対象物の撮影画像を得ることで、拡大した画像に基づいた精度の良い補正を行なうことができる。
【００２１】請求項１２の発明によれば、上記各発明において、対象物の形状がその参照画像のデータとして姿勢データのうちの検出精度の向上が望めない姿勢データについては位置姿勢の補正を行なう対象から除外する判定動作を実行し、ビジュアルサーボによる位置姿勢制御が収束した状態では、姿勢データのうちの除外されていないものについて位置姿勢の補正のための演算処理を実行するので、検出精度の向上が望めない姿勢データについて無駄な検出処理のための時間を費やすことなく迅速に位置姿勢の制御の処理を行なうことができるようになる。
【００２２】
【発明の実施の形態】（第１の実施形態）以下、本発明を組立用の多関節ロボットに適用した場合の第１の実施形態について図１ないし図８を参照しながら説明する。図１は組立用ロボットのシステムを概略的に示している。このシステムは、制御対象装置としてのロボット本体１１と、このロボット本体１１を制御する制御装置１２とを備えて構成され、制御装置１２は、ロボットコントローラ１３、視覚装置１４およびＣＣＤカメラ１５とから構成されている。
【００２３】ロボット本体１１は、組立用の６軸の小型垂直多関節型ロボットとして構成されている。このロボット本体１１において、ベース部１６上にショルダ部１７が垂直軸を中心として回動（旋回）可能に設けられ、そのショルダ部１６の左右両端部に上方に延出される一対の下アーム１８が水平軸を中心に回動可能に連結されている。この下アーム部１８の上端部に両側から挟まれるようにして、中間アーム部１９の前端部が水平軸を中心に回動可能に連結されている。
【００２４】また、中間アーム部１９の前端部には、前方に延びる上アーム部２０の後端部が同軸回転可能に連結されており、その上アーム部２０の前端側は、左右に二股をなすように延びており、そらの間に位置してリスト部２１が水平軸を中心に回動可能に連結されている。さらに、リスト部１０の前端部には、円形のフランジ部２２が同軸回動可能に連結され、そのフランジ部２２は、対象物としてワークＷを把持するためのハンドが取り付け可能な状態に設けられている。なお、図示はしないが、このロボット本体１１の内部には、各軸を駆動するためのサーボモータが組み込まれている。
【００２５】前述したＣＣＤカメラ１５は、ワークＷを撮影するもので、上アーム部２０に取り付けられている。ＣＣＤカメラ１５の画像信号を伝達する信号線はロボット本体１１を制御するためのケーブル２３と一体にして配設されている。ロボットコントローラ１３は、ＣＰＵを主体として制御用プログラムを記憶するＲＯＭ、ＲＡＭを備えると共に、制御指令を駆動信号としてロボット本体１１に出力するサーボ制御部などから構成されている。
【００２６】視覚装置１４は、ビジュアルサーボを行なうためのＣＰＵを主体として、位置姿勢制御用のプログラムを記憶したＲＯＭ、ＲＡＭを備えると共に、ＣＣＤカメラ１５からの画像信号を入力するインターフェース回路、画像処理部、画像メモリなどの各種回路を備えた構成である。
【００２７】そして、制御装置１２は、後述するように、視覚装置１４により、ＣＣＤカメラ１５の画像信号を取り込み、あらかじめ登録されている参照画像と見え方が一致するような位置姿勢となる状態の制御信号をロボットコントローラ１３に出力すると、ロボットコントローラ１３は、これに基づいてロボット本体１１の移動すべき位置姿勢に駆動制御する。
【００２８】図２は、対象物としてのワークＷを基準位置からＣＣＤカメラ１５により撮影した参照画像として登録する場合の、登録データを示している。ここでは、ワークＷの特徴を繰り入れた参照画像を設定する例として、図示のような部材を例にとって説明する。まず、このワークＷについて、任意の基準点Ｐ０を例えば中心部の所定部位に定め、その周辺部の特徴的な点を特徴点Ｐ１〜Ｐ５として設定する。基準点Ｐ０と各特徴点Ｐ１〜Ｐ５のそれぞれを結んでできるベクトルＶ１〜Ｖ５のデータを設定する。また、各ベクトルＶ１〜Ｖ５の隣接する角度φ１〜φ５のデータを設定し、これを参照画像のデータとして登録する。
【００２９】次に本実施形態の作用について図３ないし図８も参照して説明する。制御装置１２は、図３に示すフローチャートにしたがってロボット本体１１の位置姿勢の制御を実施する。制御装置１２は、制御動作を開始すると、初期設定などの種々の設定を経て後述するビジュアルサーボ制御に移行する（ステップＳ１）。そして、ビジュアルサーボ制御を進めて行くことで制御サイクルが収束すると、その誤差が許容値以下になり（ステップＳ２で「ＹＥＳ」と判断）、次のＲｘ，Ｒｙの検出処理に移行する（ステップＳ３）。
【００３０】ここでは、本発明でいうところの補正量の算出処理を行ない、その誤差値が許容値以下になると（ステップＳ４で「ＹＥＳ」と判断）、そのときに得られた補正値を記憶し、ロボット本体１１の位置姿勢を目的位置すなわち補正値に従った状態となるように移動させるようになる（ステップＳ５）。この結果、従来のビジュアルサーボのみの位置姿勢制御では得られない正確な位置姿勢制御を行なうことができるようになる。
【００３１】次に、上述のステップＳ１で実行したビジュアルサーボの位置姿勢制御について簡単に説明する。図４（ａ）は、ワークＷを基準位置においてＣＣＤカメラ１５により撮影したときの画像を参照画像として登録するための登録データを示すものである。ここでいう基準位置は、ロボット本体１１の制御を行なうことにより最終的に得た位置姿勢の状態を示すものである。なお、このビジュアルサーボの位置姿勢制御の段階では、図２に示した画像データを用いた制御を行なわないので、ワークＷの形状を直方体のものとして簡略的に示している。
【００３２】さて、ロボット本体１１が任意の位置姿勢にあるときに、ＣＣＤカメラ１５により撮影したワークＷの画像Ｗ１，Ｗ２（図４（ｂ），（ｃ）参照）に基づいて、視覚装置１４により、参照画像との比較を行なって制御すべき位置姿勢を示すデータをロボットコントローラ１３に出力する。ロボットコントローラ１３は、ＣＣＤカメラ１５による撮影画像が参照画像と同じ見え方になるようにロボット本体１１の位置姿勢の制御を行なう。
【００３３】このようにしてロボット本体１１の位置姿勢の制御を進めていき、ＣＣＤカメラ１５によるビジュアルサーボの位置姿勢制御が収束する状態すなわち、参照画像と同じ撮影画像が得られる状態になると、その位置姿勢が確定されるようになる。しかし、この状態では、前述のようにＣＣＤカメラ１５の縦揺れ角と偏揺れ角の精度が低いことから、これらの姿勢データには±２°程度の範囲を上限として検出不能な誤差が含まれている場合がある。
【００３４】次に、図３のフローチャートのステップＳ３において実施したＲｘ，Ｒｙの検出処理について説明する。この検出処理においては、まず、上述のようにビジュアルサーボによる位置姿勢制御が収束した状態で、図５に示すように、ＣＣＤカメラ１５の姿勢を例えばｘ軸回りの回転角度Ｒｘが所定以上変化するように移動させる（図中、収束した状態を１５の位置として、１５ａ，１５ｂの位置）。この場合、Ｒｘの変化量は、ＣＣＤカメラ１５による撮影画像で見たときに、画素数の変化として現れる程度に大きく設定している。
【００３５】これにより、画素数の変化に基づいてｘ軸回りＲｘあるいはｙ軸回りＲｙの回転角すなわち姿勢データとしての縦揺れ角あるいは偏揺れ角の値が求められる。そして、このとき変化させたＲｘ，Ｒｙの値に対する誤差値の関数ｆ（Ｒｘ，Ｒｙ）を次のように定義する。この場合の誤差値の関数ｆ（Ｒｘ，Ｒｙ）は、図２で説明したベクトルＶ１〜Ｖ５及びφ１〜φ５の値を用いて参照画像のデータとの比較を行なうものである。
【００３６】
【数１】

【００３７】いま、例えば、上述の図５に示したＣＣＤカメラ１５の移動を角度を変えて８回分の画像データを取り込んだ場合について考える。一方の姿勢角ＲｘあるいはＲｙを固定して他方の姿勢角ＲｙあるいはＲｘについて変化させたときに、ワークＷのベクトルＶ１〜Ｖ５のそれぞれの比の値Ｌλ（λ＝１〜５）と、角度φ１〜φ５の比の値θλ（λ＝１〜５）との値を、式（１）に従って関数値ｆ（Ｒｘ，Ｒｙ）として求める。８回分の画像データに対応して８個の関数値ｆ１〜ｆ８が得られる。
【００３８】ここで、得られた関数値ｆ１〜ｆ８をそれぞれ検出されている姿勢角ＲｘあるいはＲｙに対してプロットすると、図６に示すように、誤差値を示す関数値ｆ１〜ｆ８を結んだ曲線として得られる。この関数曲線は、参照画像を撮影した位置つまり基準位置を通るときにゼロになる筈である。ただし、固定している姿勢角がずれている場合や、測定の誤差を考慮すると、図中破線で示すようにゼロを取らない場合があるので、判定基準は、誤差値の関数値をゼロで判断するのではなく、許容誤差値以下であることを条件とする。
【００３９】さて、ビジュアルサーボによる位置姿勢制御では、関数値ｆ４〜ｆ６に対応するデータはすべて収束した状態としてこれらの間の違いをＣＣＤカメラ１５の画像の違いとして認識することができないとすると、この範囲では姿勢角の検出精度が低いことになる。ここで、ビジュアルサーボによる位置姿勢制御で、例えば、ｆ５に対応した姿勢角を検出位置Ｒｄとして得られている場合を考えると、関数値ｆがゼロになる基準位置Ｒｏに対してΔＲだけずれていることになる。そして、このずれた量ΔＲを補正値として取り込んでメモリに記憶する。
【００４０】このような補正値を姿勢角Ｒｘ，Ｒｙのそれぞれについて求めることで、得られた補正値の分だけ位置姿勢を修正することで、ビジュアルサーボによる位置姿勢の制御では得られなかった精度で、基準位置に略一致する位置姿勢とすることができるようになる。
【００４１】図７及び図８は、ベクトルＶ１〜Ｖ５を定めて位置姿勢の検出を行なうことで、精度の向上を図ることができる根拠を示したものである。いま、ＣＣＤカメラ１５の位置Ｐから、距離ｄだけ離れた位置にある長さ２Ｌの線分ＡＢを撮影した場合について考える。ＣＣＤカメラ１５の光軸ｚに対して線分ＡＢが中点Ｍで直交する状態である場合には、ＣＣＤカメラ１５のスクリーン（距離ｆ）に右半分の長さＡＭ及び左半分ＢＭの長さは等しくＩとして得ることができる（下記の式（２）参照）。この場合、中点Ｍは前述の基準点Ｐ０に相当し、両端部Ａ，Ｂは特徴点に相当している。
【００４２】また、この線分ＡＢが角度θだけ傾いた状態つまり相対的にＣＣＤカメラ１５の姿勢角がθだけ傾いた状態を想定すると、そのときＣＣＤカメラ１５で撮影した線分Ａ′Ｂ′のスクリーン上での画素数Ｉ′(pix) 、Ｉ″(pix) は下記の式（３）、（４）で示される。
【００４３】
【数２】

【００４４】上式中、ｓの値はＣＣＤカメラ１５の画像面上での実寸を画素に換算する計数であり、例えば、図８の計算をするに際して用いた値では、ＣＣＤカメラ１５の１画素サイズが０．００７４ｍｍ（７．４μｍ）であるから、画像面上で０．０７４ｍｍの長さがある場合には１０画素分の長さとして観測される。したがって、Ｉ(pix) をｍｍから画素に換算する計数ｓの値は１３５（１／０．００７４の近似値）となる。
【００４５】上述の場合、線分Ａ′Ｂ′が遠ざかった右半分Ａ′Ｍの画素数Ｉ′(pix) は、少なく見えるようになり、近付いた左半分Ｂ′Ｍの画素数Ｉ″(pix) は一旦増加して見えるようになり角度θが大きくなるに従って減少するように変化していく。これらＩ′(pix) およびＩ″(pix) の値について、それぞれＩ(pix) の値との差を演算した結果ΔＩ′、ΔＩ″を、姿勢角θを横軸にとってプロットすると、図８に示すような曲線として得られる。また、図８には、線分の全体の画素数Ｉ′(pix) ＋Ｉ″(pix) について２Ｉ(pix) との差を演算した結果を、同様にしてプロットした結果も示している。
【００４６】この図８のプロットから、線分を中点Ｍで分割したそれぞれについて画素数を比較すると、姿勢角θに対する変化量が全体の画素数について比較した値よりも大きい変化を示していることがわかる。つまり、図２に示したように、基準点を中心として特徴点との間を結んでできるベクトルＶ１〜Ｖ５のそれぞれについて比較をすることで検出能力を高めることができるのである。このことは、特徴点間を結んで得られるベクトルを定義して、これらについて同様の演算を行なうことでも同じ効果を得ることができる。
【００４７】このような本実施形態によれば、ＣＣＤカメラ１５の位置姿勢を示す縦揺れ角および偏揺れ角に対応した姿勢データを、ビジュアルサーボによる位置姿勢制御が収束した状態で、ＣＣＤカメラ１５を所定角度以上傾けて得られる画像データから補正量を推定するようにしたので、センサなどを別途に設けることなくビジュアルサーボによる位置姿勢制御で精度を上げることが難しい姿勢角についての制御を精度良く行なうことができるようになる。
【００４８】また、本実施形態によれば、ワークＷの参照画像として、基準点と複数の特徴点との間を結んで得られるベクトルＶ１〜Ｖ５およびそれらの間の角度φ１〜φ５を定義して、これらを補正量の算出に際して誤差値の関数として設定するようにしたので、分解能を高めて精度の良い補正量算出を行なうことができるようになる。
【００４９】（第２の実施形態）図９および図１０は、本発明の第２の実施形態を示すもので、以下、第１の実施形態と異なる部分について説明する。この実施形態においては、ＣＣＤカメラ１５による図３のステップＳ３で行なわれると同様のＲｘ，Ｒｙ検出処理のための画像データの取り込みを、ビジュアルサーボによる位置姿勢制御が収束した後に行なうのではなく、図１０に示すように、ビジュアルサーボによる位置姿勢制御が収束するまでの間、つまりワークＷへのアプローチを行なっている期間中（ステップＰ１）に行なうようにしたところである。
【００５０】図示のように、ビジュアルサーボによる位置姿勢制御を行なっている期間中（ステップＰ１）では、ＣＣＤカメラ１５をビジュアルサーボによる位置姿勢制御のために、角度αだけ大きく変化させながらアプローチさせる（ステップＰ２）。このようにしてアプローチしながらそのときＣＣＤカメラ１５により撮影した画像データを取り込んで記憶させ（ステップＰ３）、誤差が許容値以下になったら（ステップＰ４で「ＹＥＳ」と判断）、ビジュアルサーボを終了する。ここで角度αは、Ｒｘ，Ｒｙの検出処理に必要となる程度の縦揺れ角および偏揺れ角に相当する角度を示している。
【００５１】次に、それまでに得られた画像データを元にして、Ｒｘ，Ｒｙの検出処理を実施する（ステップＰ５）。この検出処理の内容は、第１の実施形態で述べたと同様である。これによって補正量が算出されると、補正量による位置姿勢の制御を行なって目的位置に移動させる（ステップＰ６）。
【００５２】このような第２の実施形態によっても、第１の実施形態と同様の効果を得ることができると共に、ビジュアルサーボによる位置姿勢制御のアプローチの間にＲｘ，Ｒｙ検出処理のための画像データを取得するので、アプローチ期間中の動作が若干遅くなるものの、収束位置に達した状態では、計算を行なうのみで迅速に位置姿勢の補正を行なうことができるようになる。
【００５３】（第３の実施形態）図１１は、本発明の第３の実施形態を示すもので、第２の実施形態と異なるところは、ＣＣＤカメラ１５の傾き角度をチェックして本来のビジュアルサーボによるアプローチで傾き角度がαよりも大きい場合には（ステップＰ７で「ＹＥＳ」と判断）、上述したステップＰ２をジャンプするようにしたところである。
【００５４】これにより、必要以上にＣＣＤカメラ１５を傾けたりすることなく、迅速且つ正確に補正量の演算を行なうことができ、このような第３の実施形態によっても同様の作用効果を得ることができるようになる。
【００５５】（第４の実施形態）図１２および図１３は本発明の第４の実施形態を示すもので、上記各実施形態と異なるところは、図２に示したワークＷに対してこれとは形状の異なるワークＷａを対象とする場合の位置姿勢の補正を行なう場合を取り扱うようにしたところである。ここで、ワークＷａは、図１２に示すように、前述したワークＷよりもＸ軸方向とＹ軸方向との長さの比が大きく、上述した方法でＲｘ，Ｒｙを検出しようとすると、短い側の検出精度を上げることが難しくなるような条件のものである。
【００５６】すなわち、図１２に示すワークＷａは上述した条件を示す模式的な形状を示しており、４個の特徴点ｐ１〜ｐ４が菱形の頂点に対応した位置にある。この場合、Ｙ軸方向の長さを示す特徴点ｐ１とｐ３との間の距離Ｌ１とＸ軸方向の長さを示す特徴点ｐ２とｐ４との間の距離Ｌ２との関係は、Ｌ１がＬ２よりも大きく、その比が所定以上となる形状である。
【００５７】前述のように、距離Ｌ１はＸ軸回りの回転角Ｒｘの検出精度に関係し、距離Ｌ２はＹ軸回りの回転角Ｒｙの検出精度に関係するので、上記したようにＬ１，Ｌ２の長さの関係からすると、式（２）で示した関係を考慮すると距離Ｌ２の回転角Ｒｙの変位に対する変化量は小さく、距離Ｌ１の場合に比べて検出精度が低いことがわかる。極端な場合、距離Ｌ２がゼロに近いワークの場合には、実効的にはＹ軸回りの回転角Ｒｙの検出は不可能となる。
【００５８】そこで、このような場合に対処するために、第１に、検出精度を向上させるための方法を提案すると共に、第２に、検出精度に限界があるような条件を認識するように設定して検出精度を高めることができない場合に処理時間を短縮することで、実用上の効果を得るようにした。
【００５９】まず、上記した第１の方法について説明する。この場合には、図１２に示しているように、特徴点ｐ２とｐ４とを結ぶ線分の長さが短いので、そのままでは精度を上げることができない。そこで、このような場合には、あらかじめ設定した条件により、対象となるワークＷａの特徴点間を結ぶ線分から得られるベクトルが所定寸法以下（カメラ１５の撮像画像の範囲内における寸法で決める）のときに、検出精度の向上を図るために、カメラ１５の位置をビジュアルサーボで得られる基準位置よりもワークＷａに近付けて検出処理を行なう。
【００６０】これにより、図１３に示すように、短い方の線分Ｌ２がカメラ１５の撮影画面内で長くとることができるようになり、検出精度の向上を図ることができるようになる。なお、この場合に、カメラ１５をワークＷａに近付けると、長い方の線分Ｌ１は撮影画面からはみ出してしまうので、先に前述同様の方法でＸ軸回りの回転角Ｒｘについて検出処理を行ない、この後、カメラ１５を近接させて図１３に示すような撮影画像を得て、Ｙ軸回りの回転角Ｒｙの検出処理を行なうことでＲｘ，Ｒｙの両者について検出を行なう。
【００６１】この結果、Ｒｘ，Ｒｙの双方の値について検出精度を確保しつつ、適切な検出処理時間で行なうことができ、得られた結果に基づいて精度良く補正量を演算して求めることができる。
【００６２】次に、第２の方法について説明する。これは、検出精度に限界がある場合に、第１の方法のようにして検出精度を上げることが難しい場合や、その回転角の姿勢データの精度が要求されないような場合に適用するものである。まず、このような条件が成立するか否かをあらかじめ設定した条件に基づいて判定し、検出精度を上げなくとも良いと判断された場合には、その回転軸に関する補正量の検出を行なわないようにする。
【００６３】このことは、検出精度の向上が望めない状況で検出精度の向上のための検出処理を継続することが、いたずらに時間を要する結果とならぬように、あらかじめ判定した結果に基づいて検出処理を停止することで、処理速度の向上を図るようにしたものである。これにより、必要とされる精度のみの向上を図ることで、短時間で効率の良い検出動作を行なうことができるようになり、実用上で大きなメリットを得ることができるようになる。
【００６４】（他の実施形態）本発明は、上記実施形態にのみ限定されるものではなく、次のように変形また拡張できる。第１の実施形態と第２の実施形態を複合的に行なうようにすることもできる。例えば、第２の実施形態のようにワークＷにアプローチする過程で角度を振って画像データを記憶しながらビジュアルサーボを行ない、収束した時点で補正量を求める場合の精度を向上させるために、さらに第１の実施形態のように画像データを取り込むようにすることができる。これにより、両者の利点を柔軟に取り入れて、迅速且つ正確な位置姿勢制御を行なうことができるようになる。
【００６５】対象物の参照画像の登録において、ベクトルを設定する方法は、必要に応じて選択的に実施することができる。ベクトルを設定しない方法を採用しても、従来技術のビジュアルサーボによる位置姿勢の制御に比べて、位置姿勢の精度を向上させることは可能である。
【００６６】上記各実施形態においては、Ｒｘ，Ｒｙの検出を行なうための誤差値を求める関係式として式（１）を用いているが、これに限らず、基準位置における位置姿勢との誤差をＲｘ，Ｒｙの関係において評価することができて、その誤差値を元にしてＲｘ，Ｒｙの検出を行なうことができる関係式であれば他の式を用いることもできる。
【００６７】上記各実施形態において説明したロボット本体１１は、対象物であるワークＷとの相対的に移動していない場合を想定しているが、これに限らず、対象物が移動している場合や、ロボット本体が移動可能な場合あるいは、双方が移動している状態でも適用することができ、これらの場合のように相対的に移動する物体に対してその速度ベクトルを補間する処理を実施することで同様にして位置姿勢制御を精度良く行なうことができるものである。
【００６８】さらに、制御対象装置としては、ロボットのみならず、位置姿勢を制御すべき車両や、対象物の形状や動作などを正確に認識する認識装置などについても適用することができる。
【図面の簡単な説明】
【図１】本発明の第１の実施形態を示す全体の概略構成図
【図２】ワークの基準点、特徴点の設定とベクトルの設定状態の説明図
【図３】ビジュアルサーボと補正量の算出の処理過程を示すフローチャート
【図４】ビジュアルサーボによる位置姿勢制御の概略説明図
【図５】ＣＣＤカメラのアプローチと角度を振る場合の作用説明図
【図６】カメラの姿勢角に対する誤差値の関数の値を示した図
【図７】基準点と特徴点との間のベクトルの傾きに対する変化を説明するための図
【図８】傾き角度と画素の関係を示す図
【図９】本発明の第２の実施形態を示す図５相当図
【図１０】図３相当図
【図１１】本発明の第３の実施形態を示す図３相当図
【図１２】本発明の第４の実施形態を示す基準位置からワークＷａを撮影している状態を示す作用説明図
【図１３】基準位置よりも近接した位置における図１２相当図
【図１４】従来例を示す対象物の説明図
【図１５】対象物の傾きと誤差値の説明図
【図１６】傾き角度に対する線分幅の変化を説明する図
【図１７】画素データの説明図
【符号の説明】
１１はロボット本体（制御対象装置）、１２は制御装置、１３はロボットコントローラ、１４は視覚装置、１５はＣＣＤカメラ（カメラ）、Ｗはワーク（対象物）である。

【特許請求の範囲】
【請求項１】制御対象装置に取り付けられたカメラによりあらかじめ基準位置から対象物を参照画像として撮影してこれを登録し、前記カメラにより撮影した前記対象物の画像が前記参照画像と同じ状態になるように前記制御対象装置の位置姿勢を制御するようにしたビジュアルサーボによる位置姿勢制御方法において、異なる姿勢で前記カメラにより前記対象物を撮影した画像を記憶する第１の過程と、記憶された複数の画像のデータから推定される少なくとも縦揺れ角および偏揺れ角を示す姿勢データと、画像のデータおよび前記参照画像のデータの間の誤差を示す誤差データとの相関から、誤差データが許容誤差量以下となる前記基準位置に相当する位置姿勢を推定する第２の過程と、前記推定された基準位置に相当する位置姿勢への補正量を演算する第３の過程と、前記ビジュアルサーボによる位置姿勢制御が収束した状態での位置姿勢に対して前記第３の過程で演算された補正量に基づいて前記制御体操装置の位置姿勢の補正を行なう第４の過程とを実行することを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項２】請求項１に記載のビジュアルサーボによる位置姿勢制御方法において、前記第１の過程は、前記ビジュアルサーボによる位置姿勢制御が収束した状態で、前記カメラの位置姿勢を前記姿勢データが判定角度以上変化するようにしたときの画像を記憶することにより実行されることを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項３】請求項１に記載のビジュアルサーボによる位置姿勢制御方法において、前記第１の過程は、前記ビジュアルサーボによる位置姿勢制御が収束する状態となるまでの期間中における前記カメラの画像を記憶することにより実行されることを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項４】請求項３に記載のビジュアルサーボによる位置姿勢制御方法において、前記第１の過程を実施するに際して、前記ビジュアルサーボによる位置姿勢制御が収束する状態となるまでの期間中の前記カメラの位置姿勢の制御を行なうときの前記姿勢データの変化量が判定角度に達しない場合には、その姿勢データの変化量が判定角度以上となるように変化させることを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項５】請求項１ないし４に記載のビジュアルサーボによる位置姿勢制御方法において、前記参照画像として記憶する前記対象物の画像データは、基準点に対して複数の特徴点を設定し、その基準点との間を結ぶ複数のベクトルを含んだものとして設定され、前記第２の過程を実施する際には、少なくとも前記ベクトルの長さもしくはベクトル間の長さの比に基づいて算出した誤差データにより前記位置姿勢の推定を行なうことを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項６】請求項１ないし５のいずれかに記載のビジュアルサーボによる位置姿勢制御方法において、前記参照画像として記憶する前記対象物の画像データは、基準点に対して複数の特徴点を設定し、それら特徴点間を結ぶ複数のベクトルを含んだものとして設定され、前記第２の過程を実施する際には、少なくとも前記ベクトルの長さもしくはベクトル間の長さの比に基づいて算出した誤差データにより前記位置姿勢の推定を行なうことを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項７】請求項５または６に記載のビジュアルサーボによる位置姿勢制御方法において、前記参照画像として記憶する前記対象物の画像データは、設定された前記複数のベクトルについて、ベクトル間の角度に基づいて算出した誤差データにより前記位置姿勢の推定を行なうことを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項８】請求項５ないし７のいずれかに記載のビジュアルサーボによる位置姿勢制御方法において、前記誤差データの算出は、前記ベクトルについての長さあるいは角度について撮影した画像と参照画像との間の差分の自乗和として求め、前記位置姿勢の推定は、上記方法により算出される誤差データの最小値を得るときの姿勢データとすることを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項９】請求項８に記載のビジュアルサーボによる位置姿勢制御方法において、前記位置姿勢の推定は、前記誤差データの値と前記姿勢データの縦揺れ角および偏揺れ角の値とから、姿勢データを変数とした誤差データ関数を求め、その誤差データ関数の最小値を演算による求めることで行なうことを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項１０】請求項１ないし９のいずれかに記載のビジュアルサーボによる位置姿勢制御方法において、前記ビジュアルサーボによる位置姿勢制御が収束した状態で、前記カメラの位置をその収束位置から近接した位置に移動させて前記第１ないし第４の過程を実施することを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項１１】請求項１０に記載のビジュアルサーボによる位置姿勢制御方法において、前記ビジュアルサーボによる位置姿勢制御が収束した状態で、前記姿勢データのうちの検出精度の向上が必要とされる姿勢データについてその収束位置から近接した位置に移動させて実施することを特徴とすることを特徴とするビジュアルサーボによる位置姿勢制御方法。
【請求項１２】請求項１ないし１１のいずれかに記載のビジュアルサーボによる位置姿勢制御方法において、前記対象物の形状がその参照画像のデータとして前記姿勢データのうちの検出精度の向上が望めない姿勢データについては位置姿勢の補正を行なう対象から除外する判定動作を実行し、前記ビジュアルサーボによる位置姿勢制御が収束した状態では、前記姿勢データのうちの除外されていないものについて前記位置姿勢の補正のための演算処理を実行することを特徴とするビジュアルサーボによる位置姿勢制御方法。

【図１】