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

Abstract

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

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、カメラにより撮影
した画像データに基づいて位置姿勢の制御を行なうよう
にしたビジュアルサーボによる位置姿勢制御方法に関す
る。

【０００２】

【発明が解決しようとする課題】ロボットなどによりワ
ークを把持させる場合などで、ハンドに取り付けたカメ
ラによりワークを撮影してハンドの位置姿勢を制御する
ようにしたビジュアルサーボの技術がある。これは、あ
らかじめ参照画像を登録してその参照画像と同じように
ワークが見える位置姿勢となるように制御することで、
迅速且つ正確にハンドの位置姿勢制御を行なうようにし
た方法である。この場合、参照画像は、ロボットのハン
ドを位置姿勢制御したい場所に配置してワークを把持す
るときの状態にし、ハンドに取り付けたカメラで撮影し
たものを用いる。

【０００３】このようなビジュアルサーボの技術を採用
することで、ロボットのハンドをカメラが捕らえる画像
の分解能の範囲で正確に位置姿勢制御を行なうことがで
きるが、研究レベルにおいては技術的な問題は生じてい
ないように見えていたが、実用上においては次のような
点で不具合が生じることが発明者の研究により明らかに
なりつつある。

【０００４】例えば、ロボットのハンドにより対象物を
把持したり部品を取り付けたりするなどの作業におい
て、位置決め精度が要求される場合に生ずるもので、Ｃ
ＣＤカメラなどでワークを撮影する画像の性質上、画素
数と分解能の関係で精度の向上に限界が生ずるものであ
る。すなわち、カメラの光軸方向をｚ軸とし、撮影した
画面をｘｙ平面とすると、ビジュアルサーボでは、カメ
ラで撮影した二次元画像データから得られるｘｙ平面内
の位置（ｘ，ｙ）およびｚ軸回りの回転（Ｒｚ）に対し
ては高精度の制御を行なうことができるが、ｘ軸あるい
はｙ軸回りの回転（Ｒｘ，Ｒｙ）や奥行き方向（ｚ）の
補正に対しては精度に限界がある。

【０００５】例えば、図１４（ａ），（ｂ）に示すよう
に、カメラ１により対象物Ｐ１およびＰ２を撮影する場
合において、ｘ軸回りＲｘの回転角度である縦揺れ角の
微小な変化に対して、カメラ１ａ（Ｒｘ方向に微小角度
だけ傾いた位置）の撮影画像におけるＣＣＤの画素数の
変化は図１５（ａ）の位置から（ｂ）あるいは（ｃ）に
変化した状態では、量子化誤差があるため画像上の変化
として捕らえることができる範囲には限界がある。この
場合、現状では例えば±２°程度の分解能である。

【０００６】対象物Ｐ１とＰ２とでは、それぞれの高さ
寸法ｈ１，ｈ２がｈ１＞ｈ２となっており、奥行き方向
の寸法（ｚ軸方向の寸法）が対象物Ｐ１の方が大きいの
で、Ｒｘの変化に対して変化量Ｅｒｒｏｒ１の方がＥｒ
ｒｏｒ２よりも大きくなり、その変化角度を捉ええやす
いが、奥行き方向の寸法が小さい対象物Ｐ２の場合には
縦揺れ角の変化角度の補正精度が低くなる（図１５
（ｄ）参照）。

【０００７】これは、図１６（ａ）に示すような対象物
Ｐ１において、幅寸法がＤの場合に、Ｒｘの変化に対す
る撮影画像上での幅寸法ｄは、同図（ｂ）に示すような
関係として表される。すなわち、この関係は、 ｄ／Ｄ＝ cos（Ｒｘ） と表されるから、Ｒｘが小さい領域では、角度に対する
変化量が小さくなり、図１７に示すように、画素ｐｉｘ
の数の違いとしてカウントできる範囲が広くなり、その
分だけ分解能が低下するのである。つまり、ビジュアル
サーボでは、姿勢角の要素である縦揺れ角（pitch ）お
よび偏揺れ角（yaw ）の精度について、カメラ１の量子
化精度に大きく依存することになるのである。

【０００８】上述の不具合を回避して分解能を高めるた
めには、例えば、カメラ１以外に別途にセンサを設け
て、ビジュアルサーボによる位置姿勢の制御が収束した
状態では、センサの検出信号に基づいて位置姿勢の補正
をすることで制御精度の向上を図ることが必要となる。

【０００９】本発明は、上記事情に鑑みてなされたもの
で、その目的は、ビジュアルサーボによる位置姿勢制御
のみを用いる場合においても、位置姿勢制御が収束した
状態でさらに参照画像の基準位置に近い位置姿勢となる
ように高い精度で制御することができるようにしたビジ
ュアルサーボによる位置姿勢制御方法を提供することに
ある。

【００１０】

【課題を解決するための手段】請求項１の発明によれ
ば、第１の過程で、異なる姿勢でカメラにより対象物を
撮影した画像を記憶し、第２の過程で、記憶した複数の
画像のデータから推定される少なくとも縦揺れ角および
偏揺れ角を示す姿勢データと、画像のデータおよび参照
画像のデータの間の誤差を示す誤差データとの相関か
ら、誤差データが許容誤差量以下となる基準位置に相当
する位置姿勢を推定し、第３の過程で、推定した基準位
置に相当する位置姿勢への補正量を演算し、第４の過程
で、ビジュアルサーボによる位置姿勢制御が収束した状
態での位置姿勢に対して第３の過程で演算した補正量に
基づいて制御体操装置の位置姿勢の補正を行なうように
したので、カメラで撮影した画像からは位置姿勢を示す
縦揺れ角および偏揺れ角の不感帯となる領域、つまりビ
ジュアルサーボの位置姿勢制御では収束状態となって量
子化誤差に含まれて画像データとして差異が現れない領
域にある場合でも、さらに位置姿勢の補正を行なってよ
り正確な位置姿勢に制御することができるようになる。
これにより、別途にセンサなどを設ける構成としなくと
も、位置姿勢の制御精度を高めることができるようにな
る。

【００１１】請求項２の発明によれば、上記請求項１の
発明において、第１の過程を、ビジュアルサーボによる
位置姿勢制御が収束した状態で、カメラの位置姿勢を姿
勢データが判定角度以上変化するようにしたときの画像
を記憶することにより実行するようにしたので、ビジュ
アルサーボによる位置姿勢制御が収束している状態すな
わち、最も対象物に近接して基準位置に近い位置を起点
としてここから第１の過程以降の処理を実施すること
で、位置姿勢の補正量を正確且つ迅速に検出して位置姿
勢を補正することができるようになる。

【００１２】請求項３の発明によれば、請求項１の発明
において、第１の過程を、ビジュアルサーボによる位置
姿勢制御が収束する状態となるまでの期間中におけるカ
メラの画像を記憶することにより実行するようにしたの
で、ビジュアルサーボによる位置姿勢制御を実行しなが
ら対象物に近接する過程でカメラが撮影した画像を第１
の過程として取り込んでいくことができ、ビジュアルサ
ーボの制御と処理を並行して補正量の算出を行なうこと
ができ、迅速に位置姿勢の制御を行なうことができるよ
うになる。

【００１３】請求項４の発明によれば、上記請求項３の
発明において、第１の過程を実施するに際して、ビジュ
アルサーボによる位置姿勢制御が収束する状態となるま
での期間中のカメラの位置姿勢の制御を行なうときの姿
勢データの変化量が判定角度に達しない場合には、その
姿勢データの変化量が判定角度以上となるように変化さ
せるようにしたので、上述の場合において、対象物に近
接するときに実際にカメラがあまり振れない場合つまり
姿勢データの変化量が補正量を求めるための判定角度以
上振れない場合には、位置姿勢の補正を正確に行なえる
ようにするために、画像取り込みのための姿勢をとるよ
うに制御するようにしたので、第１の過程を実行しなが
ら近接する場合でも確実に補正量の値を算出することが
できるようになる。

【００１４】請求項５の発明によれば、上記各発明にお
いて、参照画像として記憶する対象物の画像データは、
基準点に対して複数の特徴点を設定し、その基準点との
間を結ぶ複数のベクトルを含んだものとして設定し、第
２の過程を実施する際には、少なくともベクトルの長さ
もしくはベクトル間の長さの比に基づいて算出した誤差
データにより位置姿勢の推定を行なうようにしたので、
例えば、カメラにより撮影した画像と参照画像との間
に、カメラの姿勢データの縦揺れ角や偏揺れ角のずれが
生じている場合などで、対象物の基準点に対して一方側
の特徴点とを結んでできるベクトルが長くなると共に、
他方側の特徴点とをむすんでできるベクトルが短くなる
ような場合に、対象物の全長をもとに判断する場合と異
なり、双方の差を検出することにより、姿勢データに対
する検出能力を高めて、精度の高い補正量算出を行なう
ことができるようになる。

【００１５】請求項６の発明によれば、上記各発明にお
いて、参照画像として記憶する対象物の画像データは、
基準点に対して複数の特徴点を設定し、それら特徴点間
を結ぶ複数のベクトルを含んだものとして設定され、第
２の過程を実施する際には、少なくともベクトルの長さ
もしくはベクトル間の長さの比に基づいて算出した誤差
データにより位置姿勢の推定を行なうようにしたので、
上述と同様にして姿勢データに対する検出能力を高め
て、精度の高い補正量算出を行なうことができるように
なる。

【００１６】請求項７の発明によれば、請求項５および
６の発明において、参照画像として記憶する対象物の画
像データは、設定された複数のベクトルについて、ベク
トル間の角度に基づいて算出した誤差データにより位置
姿勢の推定を行なうようにしたので、２つのベクトルの
なす角度も補正量算出のデータとして捕らえて参照画像
のデータに対する比較をより厳密に行なって精度の高い
補正量算出処理を行なうことができるようになる。

【００１７】請求項８の発明によれば、請求項５ないし
７の発明において、誤差データの算出を、ベクトルにつ
いての長さあるいは角度について撮影した画像と参照画
像との間の差分の自乗和として求め、位置姿勢の推定
を、上記方法により算出される誤差データの最小値を得
るときの姿勢データとするようにしたので、変数として
の姿勢データの値に対して参照画像との差分の絶対値の
和に相当する値を関数値としてその最小値を求めること
で、関数の最小値つまりもっとも参照画像に近接した位
置姿勢の姿勢データを求めることができるようになる。
この場合、上述した関数を変数に対応してプロットし、
その最小値を求めるように演算を行なうようにしても良
いし、簡易的に関数の値が許容誤差値以下となるような
範囲の変数値の中間値を採用するということでも求める
ことができる。

【００１８】この場合、請求項９の発明のように、位置
姿勢の推定を、誤差データの値と姿勢データの縦揺れ角
および偏揺れ角の値とから、姿勢データを変数とした誤
差データ関数を求め、その誤差データ関数の最小値を演
算による求めることで行なうようにすると良い。

【００１９】請求項１０の発明によれば、上記各発明に
おいて、ビジュアルサーボによる位置姿勢制御が収束し
た状態で、カメラの位置をその収束位置から近接した位
置に移動させて第１ないし第４の過程を実施するので、
収束位置では撮影された画像の範囲では精度が得られな
い場合でも、近接した位置で撮影することで拡大した状
態の撮影画像を得ることができ、これによって姿勢デー
タに関して精度を向上した画像データを得ることができ
るようになり、位置姿勢の補正をより高い精度で行なう
ことができるようになる。また、これによって、姿勢デ
ータのうちで対象物の形状に起因して精度を上げ難い姿
勢データがある場合でも、位置姿勢の補正精度を高める
ことができるようになる。

【００２０】請求項１１の発明によれば、上記した請求
項１０の発明において、ビジュアルサーボによる位置姿
勢制御が収束した状態で、姿勢データのうちの検出精度
の向上が必要とされる姿勢データについてその収束位置
から近接した位置に移動させて実施するので、必要な姿
勢データについてのみ、近接した位置まで移動した状態
で位置姿勢の補正のための対象物の撮影画像を得ること
で、拡大した画像に基づいた精度の良い補正を行なうこ
とができる。

【００２１】請求項１２の発明によれば、上記各発明に
おいて、対象物の形状がその参照画像のデータとして姿
勢データのうちの検出精度の向上が望めない姿勢データ
については位置姿勢の補正を行なう対象から除外する判
定動作を実行し、ビジュアルサーボによる位置姿勢制御
が収束した状態では、姿勢データのうちの除外されてい
ないものについて位置姿勢の補正のための演算処理を実
行するので、検出精度の向上が望めない姿勢データにつ
いて無駄な検出処理のための時間を費やすことなく迅速
に位置姿勢の制御の処理を行なうことができるようにな
る。

【００２２】

【発明の実施の形態】（第１の実施形態）以下、本発明
を組立用の多関節ロボットに適用した場合の第１の実施
形態について図１ないし図８を参照しながら説明する。
図１は組立用ロボットのシステムを概略的に示してい
る。このシステムは、制御対象装置としてのロボット本
体１１と、このロボット本体１１を制御する制御装置１
２とを備えて構成され、制御装置１２は、ロボットコン
トローラ１３、視覚装置１４およびＣＣＤカメラ１５と
から構成されている。

【００２３】ロボット本体１１は、組立用の６軸の小型
垂直多関節型ロボットとして構成されている。このロボ
ット本体１１において、ベース部１６上にショルダ部１
７が垂直軸を中心として回動（旋回）可能に設けられ、
そのショルダ部１６の左右両端部に上方に延出される一
対の下アーム１８が水平軸を中心に回動可能に連結され
ている。この下アーム部１８の上端部に両側から挟まれ
るようにして、中間アーム部１９の前端部が水平軸を中
心に回動可能に連結されている。

【００２４】また、中間アーム部１９の前端部には、前
方に延びる上アーム部２０の後端部が同軸回転可能に連
結されており、その上アーム部２０の前端側は、左右に
二股をなすように延びており、そらの間に位置してリス
ト部２１が水平軸を中心に回動可能に連結されている。
さらに、リスト部１０の前端部には、円形のフランジ部
２２が同軸回動可能に連結され、そのフランジ部２２
は、対象物としてワークＷを把持するためのハンドが取
り付け可能な状態に設けられている。なお、図示はしな
いが、このロボット本体１１の内部には、各軸を駆動す
るためのサーボモータが組み込まれている。

【００２５】前述したＣＣＤカメラ１５は、ワークＷを
撮影するもので、上アーム部２０に取り付けられてい
る。ＣＣＤカメラ１５の画像信号を伝達する信号線はロ
ボット本体１１を制御するためのケーブル２３と一体に
して配設されている。ロボットコントローラ１３は、Ｃ
ＰＵを主体として制御用プログラムを記憶するＲＯＭ、
ＲＡＭを備えると共に、制御指令を駆動信号としてロボ
ット本体１１に出力するサーボ制御部などから構成され
ている。

【００２６】視覚装置１４は、ビジュアルサーボを行な
うためのＣＰＵを主体として、位置姿勢制御用のプログ
ラムを記憶したＲＯＭ、ＲＡＭを備えると共に、ＣＣＤ
カメラ１５からの画像信号を入力するインターフェース
回路、画像処理部、画像メモリなどの各種回路を備えた
構成である。

【００２７】そして、制御装置１２は、後述するよう
に、視覚装置１４により、ＣＣＤカメラ１５の画像信号
を取り込み、あらかじめ登録されている参照画像と見え
方が一致するような位置姿勢となる状態の制御信号をロ
ボットコントローラ１３に出力すると、ロボットコント
ローラ１３は、これに基づいてロボット本体１１の移動
すべき位置姿勢に駆動制御する。

【００２８】図２は、対象物としてのワークＷを基準位
置からＣＣＤカメラ１５により撮影した参照画像として
登録する場合の、登録データを示している。ここでは、
ワークＷの特徴を繰り入れた参照画像を設定する例とし
て、図示のような部材を例にとって説明する。まず、こ
のワークＷについて、任意の基準点Ｐ０を例えば中心部
の所定部位に定め、その周辺部の特徴的な点を特徴点Ｐ
１〜Ｐ５として設定する。基準点Ｐ０と各特徴点Ｐ１〜
Ｐ５のそれぞれを結んでできるベクトルＶ１〜Ｖ５のデ
ータを設定する。また、各ベクトルＶ１〜Ｖ５の隣接す
る角度φ１〜φ５のデータを設定し、これを参照画像の
データとして登録する。

【００２９】次に本実施形態の作用について図３ないし
図８も参照して説明する。制御装置１２は、図３に示す
フローチャートにしたがってロボット本体１１の位置姿
勢の制御を実施する。制御装置１２は、制御動作を開始
すると、初期設定などの種々の設定を経て後述するビジ
ュアルサーボ制御に移行する（ステップＳ１）。そし
て、ビジュアルサーボ制御を進めて行くことで制御サイ
クルが収束すると、その誤差が許容値以下になり（ステ
ップＳ２で「ＹＥＳ」と判断）、次のＲｘ，Ｒｙの検出
処理に移行する（ステップＳ３）。

【００３０】ここでは、本発明でいうところの補正量の
算出処理を行ない、その誤差値が許容値以下になると
（ステップＳ４で「ＹＥＳ」と判断）、そのときに得ら
れた補正値を記憶し、ロボット本体１１の位置姿勢を目
的位置すなわち補正値に従った状態となるように移動さ
せるようになる（ステップＳ５）。この結果、従来のビ
ジュアルサーボのみの位置姿勢制御では得られない正確
な位置姿勢制御を行なうことができるようになる。

【００３１】次に、上述のステップＳ１で実行したビジ
ュアルサーボの位置姿勢制御について簡単に説明する。
図４（ａ）は、ワークＷを基準位置においてＣＣＤカメ
ラ１５により撮影したときの画像を参照画像として登録
するための登録データを示すものである。ここでいう基
準位置は、ロボット本体１１の制御を行なうことにより
最終的に得た位置姿勢の状態を示すものである。なお、
このビジュアルサーボの位置姿勢制御の段階では、図２
に示した画像データを用いた制御を行なわないので、ワ
ークＷの形状を直方体のものとして簡略的に示してい
る。

【００３２】さて、ロボット本体１１が任意の位置姿勢
にあるときに、ＣＣＤカメラ１５により撮影したワーク
Ｗの画像Ｗ１，Ｗ２（図４（ｂ），（ｃ）参照）に基づ
いて、視覚装置１４により、参照画像との比較を行なっ
て制御すべき位置姿勢を示すデータをロボットコントロ
ーラ１３に出力する。ロボットコントローラ１３は、Ｃ
ＣＤカメラ１５による撮影画像が参照画像と同じ見え方
になるようにロボット本体１１の位置姿勢の制御を行な
う。

【００３３】このようにしてロボット本体１１の位置姿
勢の制御を進めていき、ＣＣＤカメラ１５によるビジュ
アルサーボの位置姿勢制御が収束する状態すなわち、参
照画像と同じ撮影画像が得られる状態になると、その位
置姿勢が確定されるようになる。しかし、この状態で
は、前述のようにＣＣＤカメラ１５の縦揺れ角と偏揺れ
角の精度が低いことから、これらの姿勢データには±２
°程度の範囲を上限として検出不能な誤差が含まれてい
る場合がある。

【００３４】次に、図３のフローチャートのステップＳ
３において実施したＲｘ，Ｒｙの検出処理について説明
する。この検出処理においては、まず、上述のようにビ
ジュアルサーボによる位置姿勢制御が収束した状態で、
図５に示すように、ＣＣＤカメラ１５の姿勢を例えばｘ
軸回りの回転角度Ｒｘが所定以上変化するように移動さ
せる（図中、収束した状態を１５の位置として、１５
ａ，１５ｂの位置）。この場合、Ｒｘの変化量は、ＣＣ
Ｄカメラ１５による撮影画像で見たときに、画素数の変
化として現れる程度に大きく設定している。

【００３５】これにより、画素数の変化に基づいてｘ軸
回りＲｘあるいはｙ軸回りＲｙの回転角すなわち姿勢デ
ータとしての縦揺れ角あるいは偏揺れ角の値が求められ
る。そして、このとき変化させたＲｘ，Ｒｙの値に対す
る誤差値の関数ｆ（Ｒｘ，Ｒｙ）を次のように定義す
る。この場合の誤差値の関数ｆ（Ｒｘ，Ｒｙ）は、図２
で説明したベクトルＶ１〜Ｖ５及びφ１〜φ５の値を用
いて参照画像のデータとの比較を行なうものである。

【００３６】

【数１】

【００３７】いま、例えば、上述の図５に示したＣＣＤ
カメラ１５の移動を角度を変えて８回分の画像データを
取り込んだ場合について考える。一方の姿勢角Ｒｘある
いはＲｙを固定して他方の姿勢角ＲｙあるいはＲｘにつ
いて変化させたときに、ワークＷのベクトルＶ１〜Ｖ５
のそれぞれの比の値Ｌλ（λ＝１〜５）と、角度φ１〜
φ５の比の値θλ（λ＝１〜５）との値を、式（１）に
従って関数値ｆ（Ｒｘ，Ｒｙ）として求める。８回分の
画像データに対応して８個の関数値ｆ１〜ｆ８が得られ
る。

【００３８】ここで、得られた関数値ｆ１〜ｆ８をそれ
ぞれ検出されている姿勢角ＲｘあるいはＲｙに対してプ
ロットすると、図６に示すように、誤差値を示す関数値
ｆ１〜ｆ８を結んだ曲線として得られる。この関数曲線
は、参照画像を撮影した位置つまり基準位置を通るとき
にゼロになる筈である。ただし、固定している姿勢角が
ずれている場合や、測定の誤差を考慮すると、図中破線
で示すようにゼロを取らない場合があるので、判定基準
は、誤差値の関数値をゼロで判断するのではなく、許容
誤差値以下であることを条件とする。

【００３９】さて、ビジュアルサーボによる位置姿勢制
御では、関数値ｆ４〜ｆ６に対応するデータはすべて収
束した状態としてこれらの間の違いをＣＣＤカメラ１５
の画像の違いとして認識することができないとすると、
この範囲では姿勢角の検出精度が低いことになる。ここ
で、ビジュアルサーボによる位置姿勢制御で、例えば、
ｆ５に対応した姿勢角を検出位置Ｒｄとして得られてい
る場合を考えると、関数値ｆがゼロになる基準位置Ｒｏ
に対してΔＲだけずれていることになる。そして、この
ずれた量ΔＲを補正値として取り込んでメモリに記憶す
る。

【００４０】このような補正値を姿勢角Ｒｘ，Ｒｙのそ
れぞれについて求めることで、得られた補正値の分だけ
位置姿勢を修正することで、ビジュアルサーボによる位
置姿勢の制御では得られなかった精度で、基準位置に略
一致する位置姿勢とすることができるようになる。

【００４１】図７及び図８は、ベクトルＶ１〜Ｖ５を定
めて位置姿勢の検出を行なうことで、精度の向上を図る
ことができる根拠を示したものである。いま、ＣＣＤカ
メラ１５の位置Ｐから、距離ｄだけ離れた位置にある長
さ２Ｌの線分ＡＢを撮影した場合について考える。ＣＣ
Ｄカメラ１５の光軸ｚに対して線分ＡＢが中点Ｍで直交
する状態である場合には、ＣＣＤカメラ１５のスクリー
ン（距離ｆ）に右半分の長さＡＭ及び左半分ＢＭの長さ
は等しくＩとして得ることができる（下記の式（２）参
照）。この場合、中点Ｍは前述の基準点Ｐ０に相当し、
両端部Ａ，Ｂは特徴点に相当している。

【００４２】また、この線分ＡＢが角度θだけ傾いた状
態つまり相対的にＣＣＤカメラ１５の姿勢角がθだけ傾
いた状態を想定すると、そのときＣＣＤカメラ１５で撮
影した線分Ａ′Ｂ′のスクリーン上での画素数Ｉ′(pi
x) 、Ｉ″(pix) は下記の式（３）、（４）で示され
る。

【００４３】

【数２】

【００４４】上式中、ｓの値はＣＣＤカメラ１５の画像
面上での実寸を画素に換算する計数であり、例えば、図
８の計算をするに際して用いた値では、ＣＣＤカメラ１
５の１画素サイズが０．００７４ｍｍ（７．４μｍ）で
あるから、画像面上で０．０７４ｍｍの長さがある場合
には１０画素分の長さとして観測される。したがって、
Ｉ(pix) をｍｍから画素に換算する計数ｓの値は１３５
（１／０．００７４の近似値）となる。

【００４５】上述の場合、線分Ａ′Ｂ′が遠ざかった右
半分Ａ′Ｍの画素数Ｉ′(pix) は、少なく見えるように
なり、近付いた左半分Ｂ′Ｍの画素数Ｉ″(pix) は一旦
増加して見えるようになり角度θが大きくなるに従って
減少するように変化していく。これらＩ′(pix) および
Ｉ″(pix) の値について、それぞれＩ(pix) の値との差
を演算した結果ΔＩ′、ΔＩ″を、姿勢角θを横軸にと
ってプロットすると、図８に示すような曲線として得ら
れる。また、図８には、線分の全体の画素数Ｉ′(pix)
＋Ｉ″(pix) について２Ｉ(pix) との差を演算した結果
を、同様にしてプロットした結果も示している。

【００４６】この図８のプロットから、線分を中点Ｍで
分割したそれぞれについて画素数を比較すると、姿勢角
θに対する変化量が全体の画素数について比較した値よ
りも大きい変化を示していることがわかる。つまり、図
２に示したように、基準点を中心として特徴点との間を
結んでできるベクトルＶ１〜Ｖ５のそれぞれについて比
較をすることで検出能力を高めることができるのであ
る。このことは、特徴点間を結んで得られるベクトルを
定義して、これらについて同様の演算を行なうことでも
同じ効果を得ることができる。

【００４７】このような本実施形態によれば、ＣＣＤカ
メラ１５の位置姿勢を示す縦揺れ角および偏揺れ角に対
応した姿勢データを、ビジュアルサーボによる位置姿勢
制御が収束した状態で、ＣＣＤカメラ１５を所定角度以
上傾けて得られる画像データから補正量を推定するよう
にしたので、センサなどを別途に設けることなくビジュ
アルサーボによる位置姿勢制御で精度を上げることが難
しい姿勢角についての制御を精度良く行なうことができ
るようになる。

【００４８】また、本実施形態によれば、ワークＷの参
照画像として、基準点と複数の特徴点との間を結んで得
られるベクトルＶ１〜Ｖ５およびそれらの間の角度φ１
〜φ５を定義して、これらを補正量の算出に際して誤差
値の関数として設定するようにしたので、分解能を高め
て精度の良い補正量算出を行なうことができるようにな
る。

【００４９】（第２の実施形態）図９および図１０は、
本発明の第２の実施形態を示すもので、以下、第１の実
施形態と異なる部分について説明する。この実施形態に
おいては、ＣＣＤカメラ１５による図３のステップＳ３
で行なわれると同様のＲｘ，Ｒｙ検出処理のための画像
データの取り込みを、ビジュアルサーボによる位置姿勢
制御が収束した後に行なうのではなく、図１０に示すよ
うに、ビジュアルサーボによる位置姿勢制御が収束する
までの間、つまりワークＷへのアプローチを行なってい
る期間中（ステップＰ１）に行なうようにしたところで
ある。

【００５０】図示のように、ビジュアルサーボによる位
置姿勢制御を行なっている期間中（ステップＰ１）で
は、ＣＣＤカメラ１５をビジュアルサーボによる位置姿
勢制御のために、角度αだけ大きく変化させながらアプ
ローチさせる（ステップＰ２）。このようにしてアプロ
ーチしながらそのときＣＣＤカメラ１５により撮影した
画像データを取り込んで記憶させ（ステップＰ３）、誤
差が許容値以下になったら（ステップＰ４で「ＹＥＳ」
と判断）、ビジュアルサーボを終了する。ここで角度α
は、Ｒｘ，Ｒｙの検出処理に必要となる程度の縦揺れ角
および偏揺れ角に相当する角度を示している。

【００５１】次に、それまでに得られた画像データを元
にして、Ｒｘ，Ｒｙの検出処理を実施する（ステップＰ
５）。この検出処理の内容は、第１の実施形態で述べた
と同様である。これによって補正量が算出されると、補
正量による位置姿勢の制御を行なって目的位置に移動さ
せる（ステップＰ６）。

【００５２】このような第２の実施形態によっても、第
１の実施形態と同様の効果を得ることができると共に、
ビジュアルサーボによる位置姿勢制御のアプローチの間
にＲｘ，Ｒｙ検出処理のための画像データを取得するの
で、アプローチ期間中の動作が若干遅くなるものの、収
束位置に達した状態では、計算を行なうのみで迅速に位
置姿勢の補正を行なうことができるようになる。

【００５３】（第３の実施形態）図１１は、本発明の第
３の実施形態を示すもので、第２の実施形態と異なると
ころは、ＣＣＤカメラ１５の傾き角度をチェックして本
来のビジュアルサーボによるアプローチで傾き角度がα
よりも大きい場合には（ステップＰ７で「ＹＥＳ」と判
断）、上述したステップＰ２をジャンプするようにした
ところである。

【００５４】これにより、必要以上にＣＣＤカメラ１５
を傾けたりすることなく、迅速且つ正確に補正量の演算
を行なうことができ、このような第３の実施形態によっ
ても同様の作用効果を得ることができるようになる。

【００５５】（第４の実施形態）図１２および図１３は
本発明の第４の実施形態を示すもので、上記各実施形態
と異なるところは、図２に示したワークＷに対してこれ
とは形状の異なるワークＷａを対象とする場合の位置姿
勢の補正を行なう場合を取り扱うようにしたところであ
る。ここで、ワークＷａは、図１２に示すように、前述
したワークＷよりもＸ軸方向とＹ軸方向との長さの比が
大きく、上述した方法でＲｘ，Ｒｙを検出しようとする
と、短い側の検出精度を上げることが難しくなるような
条件のものである。

【００５６】すなわち、図１２に示すワークＷａは上述
した条件を示す模式的な形状を示しており、４個の特徴
点ｐ１〜ｐ４が菱形の頂点に対応した位置にある。この
場合、Ｙ軸方向の長さを示す特徴点ｐ１とｐ３との間の
距離Ｌ１とＸ軸方向の長さを示す特徴点ｐ２とｐ４との
間の距離Ｌ２との関係は、Ｌ１がＬ２よりも大きく、そ
の比が所定以上となる形状である。

【００５７】前述のように、距離Ｌ１はＸ軸回りの回転
角Ｒｘの検出精度に関係し、距離Ｌ２はＹ軸回りの回転
角Ｒｙの検出精度に関係するので、上記したようにＬ
１，Ｌ２の長さの関係からすると、式（２）で示した関
係を考慮すると距離Ｌ２の回転角Ｒｙの変位に対する変
化量は小さく、距離Ｌ１の場合に比べて検出精度が低い
ことがわかる。極端な場合、距離Ｌ２がゼロに近いワー
クの場合には、実効的にはＹ軸回りの回転角Ｒｙの検出
は不可能となる。

【００５８】そこで、このような場合に対処するため
に、第１に、検出精度を向上させるための方法を提案す
ると共に、第２に、検出精度に限界があるような条件を
認識するように設定して検出精度を高めることができな
い場合に処理時間を短縮することで、実用上の効果を得
るようにした。

【００５９】まず、上記した第１の方法について説明す
る。この場合には、図１２に示しているように、特徴点
ｐ２とｐ４とを結ぶ線分の長さが短いので、そのままで
は精度を上げることができない。そこで、このような場
合には、あらかじめ設定した条件により、対象となるワ
ークＷａの特徴点間を結ぶ線分から得られるベクトルが
所定寸法以下（カメラ１５の撮像画像の範囲内における
寸法で決める）のときに、検出精度の向上を図るため
に、カメラ１５の位置をビジュアルサーボで得られる基
準位置よりもワークＷａに近付けて検出処理を行なう。

【００６０】これにより、図１３に示すように、短い方
の線分Ｌ２がカメラ１５の撮影画面内で長くとることが
できるようになり、検出精度の向上を図ることができる
ようになる。なお、この場合に、カメラ１５をワークＷ
ａに近付けると、長い方の線分Ｌ１は撮影画面からはみ
出してしまうので、先に前述同様の方法でＸ軸回りの回
転角Ｒｘについて検出処理を行ない、この後、カメラ１
５を近接させて図１３に示すような撮影画像を得て、Ｙ
軸回りの回転角Ｒｙの検出処理を行なうことでＲｘ，Ｒ
ｙの両者について検出を行なう。

【００６１】この結果、Ｒｘ，Ｒｙの双方の値について
検出精度を確保しつつ、適切な検出処理時間で行なうこ
とができ、得られた結果に基づいて精度良く補正量を演
算して求めることができる。

【００６２】次に、第２の方法について説明する。これ
は、検出精度に限界がある場合に、第１の方法のように
して検出精度を上げることが難しい場合や、その回転角
の姿勢データの精度が要求されないような場合に適用す
るものである。まず、このような条件が成立するか否か
をあらかじめ設定した条件に基づいて判定し、検出精度
を上げなくとも良いと判断された場合には、その回転軸
に関する補正量の検出を行なわないようにする。

【００６３】このことは、検出精度の向上が望めない状
況で検出精度の向上のための検出処理を継続すること
が、いたずらに時間を要する結果とならぬように、あら
かじめ判定した結果に基づいて検出処理を停止すること
で、処理速度の向上を図るようにしたものである。これ
により、必要とされる精度のみの向上を図ることで、短
時間で効率の良い検出動作を行なうことができるように
なり、実用上で大きなメリットを得ることができるよう
になる。

【００６４】（他の実施形態）本発明は、上記実施形態
にのみ限定されるものではなく、次のように変形また拡
張できる。第１の実施形態と第２の実施形態を複合的に
行なうようにすることもできる。例えば、第２の実施形
態のようにワークＷにアプローチする過程で角度を振っ
て画像データを記憶しながらビジュアルサーボを行な
い、収束した時点で補正量を求める場合の精度を向上さ
せるために、さらに第１の実施形態のように画像データ
を取り込むようにすることができる。これにより、両者
の利点を柔軟に取り入れて、迅速且つ正確な位置姿勢制
御を行なうことができるようになる。

【００６５】対象物の参照画像の登録において、ベクト
ルを設定する方法は、必要に応じて選択的に実施するこ
とができる。ベクトルを設定しない方法を採用しても、
従来技術のビジュアルサーボによる位置姿勢の制御に比
べて、位置姿勢の精度を向上させることは可能である。

【００６６】上記各実施形態においては、Ｒｘ，Ｒｙの
検出を行なうための誤差値を求める関係式として式
（１）を用いているが、これに限らず、基準位置におけ
る位置姿勢との誤差をＲｘ，Ｒｙの関係において評価す
ることができて、その誤差値を元にしてＲｘ，Ｒｙの検
出を行なうことができる関係式であれば他の式を用いる
こともできる。

【００６７】上記各実施形態において説明したロボット
本体１１は、対象物であるワークＷとの相対的に移動し
ていない場合を想定しているが、これに限らず、対象物
が移動している場合や、ロボット本体が移動可能な場合
あるいは、双方が移動している状態でも適用することが
でき、これらの場合のように相対的に移動する物体に対
してその速度ベクトルを補間する処理を実施することで
同様にして位置姿勢制御を精度良く行なうことができる
ものである。

【００６８】さらに、制御対象装置としては、ロボット
のみならず、位置姿勢を制御すべき車両や、対象物の形
状や動作などを正確に認識する認識装置などについても
適用することができる。

【図面の簡単な説明】

【図１】本発明の第１の実施形態を示す全体の概略構成
図

【図２】ワークの基準点、特徴点の設定とベクトルの設
定状態の説明図

【図３】ビジュアルサーボと補正量の算出の処理過程を
示すフローチャート

【図４】ビジュアルサーボによる位置姿勢制御の概略説
明図

【図５】ＣＣＤカメラのアプローチと角度を振る場合の
作用説明図

【図６】カメラの姿勢角に対する誤差値の関数の値を示
した図

【図７】基準点と特徴点との間のベクトルの傾きに対す
る変化を説明するための図

【図８】傾き角度と画素の関係を示す図

【図９】本発明の第２の実施形態を示す図５相当図

【図１０】図３相当図

【図１１】本発明の第３の実施形態を示す図３相当図

【図１２】本発明の第４の実施形態を示す基準位置から
ワークＷａを撮影している状態を示す作用説明図

【図１３】基準位置よりも近接した位置における図１２
相当図

【図１４】従来例を示す対象物の説明図

【図１５】対象物の傾きと誤差値の説明図

【図１６】傾き角度に対する線分幅の変化を説明する図

【図１７】画素データの説明図

【符号の説明】

１１はロボット本体（制御対象装置）、１２は制御装
置、１３はロボットコントローラ、１４は視覚装置、１
５はＣＣＤカメラ（カメラ）、Ｗはワーク（対象物）で
ある。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 内藤 貴志 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 Ｆターム(参考） 3C007 AS06 BS10 KS03 KS04 KS05 KS07 KT01 KT05 LT06 LT12 LV04 LV05 MT04 5B057 AA05 BA02 BA17 DA07 DB02 DB05 DC03 DC05 DC08 DC33

Claims (12)

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