JP5509859B2 - ロボット制御装置及び方法 - Google Patents

Description

本発明は、ロボットをビジュアルサーボ制御してワークを把持するロボット制御装置及び方法に関する。

ビジュアルサーボ制御とは、移動する対象物（ワーク）の位置と姿勢を、ロボットアームに搭載したカメラなどの視覚センサで計測し続け、計測結果に基づいてロボットアームをフィードバック制御し、ワークの移動に追従させる制御である。
すなわち、ビジュアルサーボ制御では、「カメラによる撮影→画像処理→ワークの認識」と「位置計測→ワークのある方にアーム移動」の処理を高速に繰り返して、ワークの移動にカメラを追従させるものである。

しかし、手先のハンド把持点ＴＣＰがカメラの視野に入っていない場合、ワークを把持する把持動作中は、ワークがカメラの視野から外れることになる。従って、把持動作中はワークの移動に手先の作用点を追従させることができず、いわゆる盲運転となる。
そこで、把持動作中におけるワークの移動量を予測する手段が提案されている（例えば特許文献１）。

特許文献１の手段は、把持動作開始前のビジュアルサーボ制御中（追従時）のワークの速度と加速度を計測し、把持動作に要する時間の間、ワークが等速運動又は等加速度運動を維持すると仮定して、ワークの速度又は加速度から把持位置を求めるものである。

特許第４２６５０８８号公報、「ロボット装置及びその制御方法」

特許文献１の手段でワークの速度又は加速度から把持位置を求める場合、以下の要因によって誤差が生じる。
（１）ワークの速度と加速度を計測する際に誤差が生じる。
（２）ワークが理想的な等速運動又は等加速度運動をしていない。

そのため、従来の手段では、把持動作に要する時間（把持時間）が存在する限り、把持位置に誤差が生じ、この誤差は、把持時間に比例して大きくなる問題点があった。

把持時間には、制御遅れに起因する「定常偏差」と、物理的な位置に起因する「位置偏差」が含まれる。
定常偏差は、ＰＤ制御などの追従制御における制御系の遅れに由来する偏差である。位置偏差は、カメラとアーム手先の相対位置に由来する偏差である。
従来の把持動作では、定常偏差分と位置偏差を補償して移動する必要があり、これらが大きいほど、把持時間が長くなり、その分、把持位置の誤差が大きくなっていた。

本発明は、上述した問題点を解決するために創案されたものである。
すなわち、本発明の目的は、ビジュアルサーボ制御中に、ワークを把持する場合に、把持動作に要する把持時間を短縮し、把持時間に比例する把持位置の誤差を低減することができるロボット制御装置及び方法を提供することにある。

本発明によれば、ワークを把持するハンドと、該ハンドに取り付けられワークを撮影するカメラとを有し、前記ハンドを３次元的に移動可能なロボットを制御するロボット制御装置であって、
前記カメラで撮影した画像を画像処理する画像処理装置と、該ロボットを制御するロボットコントローラと、前記ロボットに、カメラの光軸と平行な軸を中心に前記ハンドの把持位置に対しカメラを旋回させる旋回機構と、を備え、
ワークの位置と姿勢をカメラで計測し、計測結果に基づいてロボットをフィードバック制御し、カメラをワークの移動に追従させるビジュアルサーボ制御を実施し、
ビジュアルサーボ制御中に、追従点とワーク位置との定常偏差を補償する位置に追従制御点を移動させ、かつ前記把持位置がカメラよりワーク側に位置するように、カメラを旋回させた後に、ハンドでワークを把持する、ことを特徴とするロボット制御装置が提供される。

本発明の実施形態によれば、前記カメラの計測範囲内にあり、前記ハンドの把持位置に最も近い点を前記追従点に設定する。

また、本発明によれば、ワークを把持するハンドと、該ハンドに取り付けられワークを撮影するカメラとを有し、前記ハンドを３次元的に移動可能なロボットを制御するロボット制御方法であって、
（Ａ） ワークの位置と姿勢をカメラで計測し、計測結果に基づいてロボットをフィードバック制御し、カメラをワークの移動に追従させるビジュアルサーボ制御と、
（Ｂ） ビジュアルサーボ制御中に、追従点とワーク位置との定常偏差を補償する位置に追従制御点を移動させる制御点移動制御と、を有し、
前記ロボットに、カメラの光軸と平行な軸を中心に前記ハンドの把持位置に対しカメラを旋回させる旋回機構を備え、
ビジュアルサーボ制御中に、前記把持位置がカメラよりワーク側に位置するように、カメラを旋回させる旋回制御を実施し、
前記制御点移動制御と旋回制御の後に、ハンドでワークを把持する、ことを特徴とするロボット制御方法が提供される。

本発明の実施形態によれば、前記カメラの計測範囲内にあり、前記ハンドの把持位置に最も近い点を前記追従点に設定する。
また、前記制御点移動制御において、カメラで撮影した画像上の追従制御点を追従点として前記定常偏差を計測し、次いで、前記定常偏差の変動が少なくなり前記追従が安定したことを確認し、次いで、前記追従点近傍にワークが位置するように、前記追従制御点を移動させる。

上記本発明の装置及び方法によれば、ビジュアルサーボ制御により、ワークの位置と姿勢をカメラで計測し、計測結果に基づいてロボットをフィードバック制御し、カメラをワークの移動に追従させることができる。
また、制御点移動制御により、追従点とワーク位置との定常偏差を補償する位置に追従制御点を移動させるので、追従点をワーク位置に近づけることができる。追従点は任意の場所に設定できるので、例えばハンド把持点ＴＣＰを追従点としておけば、最短距離で把持動作ができる。

また、旋回制御により、ハンド把持点ＴＣＰがカメラよりワーク側に位置するので、定常偏差のある方向にハンド把持点ＴＣＰを移動させることで、把持動作を短くできる。

従って、ビジュアルサーボ制御中に、ワークを把持する場合に、把持動作に要する把持時間を短縮し、把持時間に比例する把持位置の誤差を低減することができる。

本発明によるロボット制御装置の構成図である。 図１におけるカメラ座標系の説明図である。 本発明によるロボット制御方法の全体フロー図である。 制御点移動制御時の画像の模式図である。 旋回機構を備えたロボットの説明図である。 旋回制御時の画像の模式図である。 旋回機構の第２実施形態図である。 図７における旋回制御の説明図である。 図７における別の旋回制御の説明図である。 旋回機構の第３実施形態図である。

以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明によるロボット制御装置の構成図である。この図において、１はワーク（対象物）、２ａはハンド、２ｂはカメラ、３はロボットである。

ワーク１は、この例ではテーブル４の上面に沿って移動する。ワーク１は、この例では円板状部材であるが、本発明はこれに限定されず、上方（又は横方向）から見て一定の形状を有する部材であればよい。この形状は、予め記憶した形状とパターンマッチングできる限りで、円形、矩形、楕円、その他の形状であってもよい。
ワーク１の移動は、例えばコンベア装置による。コンベア装置は、ローラコンベア、ベルトコンベア、その他であってもよい。また、その移動方向は、テーブル上の水平面内に限定されず、上方（又は横方向）から見て一定の形状を維持する限りで３次元的に移動してもよい。

ハンド２ａは、例えば吸着パッドであり、その下端のハンド把持点（ＴＣＰ：Ｔｏｏｌ Ｃｅｎｔｅｒ Ｐｏｉｎｔ）でワーク１を把持するようになっている。ハンド２ａは、吸着パッドに限定されず、電磁パッドや機械的なハンドであってもよい。

カメラ２ｂは、例えばＣＣＤカメラ又はＣＭＯＳカメラであり、ハンド２ａに一体的に取り付けられ、ワーク１をその上方（又は横方向）から撮影してデジタル画像５を出力するようになっている。
デジタル画像５の画素数は、任意であるが、例えば、約３０万画素（横６４０ピクセル×縦４８０ピクセル）を有する。また、カメラ２ｂは、デジタル画像４を一定の制御周期（例えば３０ｆｐｓ：１秒間に３０回）で撮影するようになっている。以下、デジタル画像を単に画像という。

ロボット３は、ハンド２ａとカメラ２ｂを３次元的に移動する。この例でハンド２ａは、ロボット３のロボットアーム３ａの手先部（ハンド）に搭載され、ロボット３により、カメラ２ｂを下向きに維持できるようになっている。
ロボット３は、この例では、テーブル４の上面に固定された多間接ロボットであるが、本発明はこれに限定されず、その他のロボットであってもよい。

図１において、本発明によるロボット制御装置１０は、画像処理装置１２とロボットコントローラ１４からなる。

画像処理装置１２は、カメラ２ｂで撮影した画像（デジタル画像５）を画像処理する。この画像処理により、予め設定した形状とパターンマッチングで一致する画像５内のワーク１を検出し、ワーク１の基準点の画像上の位置と向きを出力する。
ワーク１の基準点は、例えば中心、図心、その他の特徴点である。またワーク１の向きは、例えばその形状の対称軸であり、その形状に応じて任意に設定する。

ロボットコントローラ１４は、ロボット３を制御してロボットアーム３ａの手先目標速度から各関節の回転量を算出し、ロボットアーム３ａを動作させて、ハンド２ａとカメラ２ｂを３次元的に移動する。また、このロボットコントローラ１４は、画像処理装置１２と協働して後述するビジュアルサーボ制御、制御点移動制御、及び旋回制御を実行する。なお、旋回制御は省略してもよい。

図２は、図１におけるカメラ座標系の説明図である。
この図に示すように、カメラ上に原点を有する直交３軸ｘ−ｙ−ｚ軸を定義する。ｘ軸はカメラの横方向、ｙ軸はカメラの縦方向、ｚ軸は上下方向である。
この場合、ハンド２ａのハンド把持点ＴＣＰは、通常、カメラ２ｂの視野から外れており、ハンド把持点ＴＣＰとカメラ２ｂの軸線間位置（水平距離）をＬ０とする。

また、カメラ２ｂで検出されたワーク１の画像上の位置を（ｃｘ，ｃｙ）とする。ｃｘは画像上の横方向位置（単位：ピクセル）であり、ｃｙは画像上の縦方向位置（単位：ピクセル）である。
また、画像上のワークの大きさと、予め設定した形状との相違からワーク１とカメラ２ｂの距離ｃｚを算出することができる。ｃｚはこの例ではワーク１からカメラ２ｂまでの高さであり、長さの単位（例えばｍｍ）を有する。
なお、ｃｘ，ｃｙ，ｃｚは、同一単位に変換してもよい。
以下、検出されたワーク１の位置を（ｃｘ，ｃｙ，ｃｚ）と表示する。

図３は、本発明によるロボット制御方法の全体フロー図である。この図において、本発明のロボット制御方法は、ビジュアルサーボ制御と制御点移動制御とを有する。

ビジュアルサーボ制御は、Ｓ１〜Ｓ５のステップ（工程）からなり、ワーク１の上方（又は横方向）からその位置と姿勢をカメラ２ｂで計測し、計測結果に基づいてロボット３をフィードバック制御し、カメラ２ｂをワーク１の移動に追従させる。

すなわち、図３において、カメラ２ｂで画像５を取り込み（Ｓ１）、画像処理装置１２により画像認識処理してワーク１の位置（ｃｘ，ｃｙ，ｃｚ）を認識し（Ｓ２）する。次いで、ワーク１の位置を目標点に設定し（Ｓ３）、目標速度を設定して（Ｓ４）、ロボットを設定した目標点に目標速度で制御する（Ｓ５）。
ここで、目標点に追従させるのは画像上の追従制御点６（ｐｘ，ｐｙ，ｐｚ）であり、動作開始時には、追従制御点６を追従点７（ｃｘ_０，ｃｙ_０，ｃｚ_０）（例えば画像上の原点（０、０）、ｃｚ_０はカメラとワークの目標距離３００ｍｍ）に設定する。
従って、ビジュアルサーボ制御中は、画像上の追従制御点６（ｐｘ，ｐｙ，ｐｚ）がワーク１の位置（ｃｘ，ｃｙ，ｃｚ）に追従する。

なお高さの目標位置ｃｚ_０は、ワーク全体を正確に計測できる任意の高さである。
上述したビジュアルサーボ制御は、好ましくは、デジタル画像４の取り込みと同期して同一の制御周期で行う。

図３において、制御点移動制御は、Ｓ１１〜Ｓ１７のステップ（工程）からなり、カメラで撮影した画像上の追従制御点６を追従点７として定常偏差Ｌ１を計測し（Ｓ１１）、次いで、定常偏差Ｌ１の変動が少なくなり追従が安定したことを確認する（Ｓ１２，Ｓ１３）。追従が安定するまでの間（Ｓ１３でＮＯ）は，上述のビジュアルサーボ制御を行なう。次いで、追従点７の近傍にワーク１が位置するように、追従制御点６を移動させる（Ｓ１４，Ｓ１５，Ｓ１６）。制御点移動後に再び追従が安定するまで，ビジュアルサーボ制御を行なう。

図４は、制御点移動制御時の画像の模式図である。この図において、（Ａ）は制御点移動制御の開始時、（Ｂ）は追従安定時、（Ｃ）は制御点移動時、（Ｄ）は追従再安定時を示している。
制御点移動制御の開始時（Ａ）には、追従制御点６は追従点７（例えば画像上の原点）であり、追従制御点６と追従点７は一致している。
追従安定時（Ｂ）には、カメラ及び制御系の遅れに起因する定常偏差Ｌ１が安定する。この定常偏差Ｌ１は、ワークの移動速度Ｖと制御遅れ（例えば、画像取得間隔と、アームの機械的な動作遅れなどを含めて、０．２秒ほどの遅れがあり得る）に比例するほぼ一定の距離であり、例えば実寸法で約１００ｍｍである。この状態では、ワーク１に対し、追従制御点６（及び追従点７）の位置が定常偏差Ｌ１の分、遅れている。
制御点移動時（Ｃ）では、追従制御点６を追従点７（画像上の原点）に対し、ワーク１の反対側に移動している。
追従再安定時（Ｄ）は、制御点移動後に、再度追従が安定した状態である。この状態において、ワーク１に対し、追従制御点６の位置が定常偏差Ｌ１の分、遅れているが、追従点７の位置はワーク１とほぼ一致している。
すなわち、追従再安定時（Ｄ）において、追従点７の位置はワーク１とほぼ一致しており、追従点７を仮想的に追従制御点６と考えると、制御遅れに起因する定常偏差が、ゼロまたはゼロに近くなっていることになる。

図３において、追従点７とワーク１との偏差が所定の閾値Ｃより小さくなったとき（すなわち追従再安定時（Ｄ））に（Ｓ１５，Ｓ１６）、ワーク把持を実施する（Ｓ１７）。
追従再安定時（Ｄ）には、ワークの移動に対し、追従点７の遅れはなく、追従点７の位置はワーク１とほぼ一致しているので、従来の把持時間に含まれる定常偏差分を低減することができ、その分、把持時間を短縮することができる。

追従点７の位置は、上述の例では画像上の原点であるが、ビジュアルサーボ制御中のカメラの画像上にハンド２ａのハンド把持点ＴＣＰが存在する場合には、ハンド把持点ＴＣＰの位置を追従点７とすることが好ましい。
この場合、把持動作における水平方向移動は実質的にゼロとなり、把持動作は上下動のみとなる。

図５は、旋回機構を備えたロボットの説明図である。この図において、（Ａ）はカメラの旋回前、（Ｂ）は旋回後である。
この例では、ロボット３に、鉛直軸を中心にハンド２ａのハンド把持点ＴＣＰに対しカメラ２ｂを旋回させる旋回機構８を備える。旋回機構８の鉛直軸（鉛直軸）は、カメラ２ｂの光軸、ハンド把持点ＴＣＰ、又はその中間位置であってもよい。

図６は、旋回制御時の画像の模式図である。この図において、（Ａ）はカメラの旋回前、（Ｂ）は旋回後である。

図５（Ａ）と図６（Ａ）のカメラの旋回前（Ａ）において、上述したように、移動するワーク１に対し、追従制御点６の位置が定常偏差Ｌ１の分、遅れている。この状態は、図４の追従安定時（Ｂ）でも追従再安定時（Ｄ）でもよい。
この状態から、ビジュアルサーボ制御中に、把持位置２ａがカメラ２ｂよりワーク側に位置するように、カメラ２ｂを旋回させると、旋回後（Ｂ）に示す位置関係となる。
なお、カメラを旋回させても、カメラ画像上のＴＣＰの位置は動かない。すなわち、カメラ２ｂはハンドと一体的に取り付けられ、ワーク１直上より撮影しているため、カメラとハンドＴＣＰの位置関係は変わらない。

図５（Ｂ）と図６（Ｂ）のカメラの旋回後（Ｂ）において、移動するワーク１に対し、追従制御点６の位置は旋回前（Ａ）と同様に定常偏差Ｌ１の分、遅れている。しかし、把持位置２ａがカメラ２ｂよりワーク側に位置するので、ハンド２ａとカメラ２ｂの水平距離Ｌ０の分、ハンド２ａはワーク１に近くなっている。
すなわち、旋回後（Ｂ）において、移動するワーク１に対し、ハンド２ａのハンド把持点ＴＣＰの位置（水平距離）は、定常偏差Ｌ１と水平距離Ｌ０の差のみとなる。
従って、ビジュアルサーボ制御中のカメラの画像上にハンド２ａのハンド把持点ＴＣＰが存在する場合もしない場合でも、従来の把持時間に含まれる定常偏差分を定常偏差Ｌ１と水平距離Ｌ０の差まで低減することができ、その分、把持時間を短縮することができる。

図７は、旋回機構の第２実施形態図である。この例では、ハンド２ａが４つの吸着パッド９を有しており、ハンド２ａとワーク１の相対姿勢（回転位置）が４方向に限定される場合である。

図８は、図７における旋回制御の説明図である。この図において、（Ａ）〜（Ｄ）は、ハンド２ａとワーク１の４通りの相対姿勢を示している。
図８（Ａ）〜（Ｄ）において、ワーク１と追従制御点６の定常偏差Ｌ１は同一である。この場合、４通りの相対姿勢のうち、把持位置２ａがワークに最も近い（Ｄ）の姿勢をとることにより、図８（Ａ）（Ｂ）（Ｃ）の姿勢に比較して、把持時間に含まれる定常偏差分を低減することができ、その分、把持時間を短縮することができる。

図９は、旋回機構の第３実施形態図である。この例では、図７の４つの吸着パッド９を鉛直軸を中心に旋回させる第２旋回機構１１を備えている。
従って、カメラとハンド２ａの相対姿勢を独立に制御することができ、上述した４通りの相対姿勢に吸着パッド９を旋回させると同時に、ビジュアルサーボ制御中に、把持位置２ａがカメラ２ｂよりワーク側に位置するように、カメラ２ｂを旋回させることができる。
この状態において、把持位置２ａがカメラ２ｂよりワーク側に位置するので、ハンド２ａとカメラ２ｂの水平距離Ｌ０の分、把持時間に含まれる定常偏差分を低減することができ、その分、把持時間を短縮することができる。

図１０は、図７における別の旋回制御の説明図である。この例では、上述した４通りの相対姿勢を考慮せずに、ビジュアルサーボ制御中に、把持位置２ａがカメラ２ｂよりワーク側に位置するように、カメラ２ｂを旋回させる。この状態において、把持位置２ａがカメラ２ｂよりワーク側に位置するので、ハンド２ａとカメラ２ｂの水平距離Ｌ０の分、ハンド２ａはワーク１に近くなっている。
次いで、把持動作（Ｓ１７）の開始後に、把持動作と並行して、ハンド２ａを旋回させてワーク１の相対姿勢（回転位置）に限定される４方向に位置決めする。
この方法により、図８（Ａ）〜（Ｄ）の姿勢に比較して、把持時間に含まれる定常偏差分を最小まで低減することができ、その分、把持時間を短縮することができる。また、旋回は把持動作と並行するので、旋回に要する時間は把持時間に含まれない。

上述した本発明の装置及び方法によれば、ビジュアルサーボ制御により、ワーク１の上方（又は横方向）からその位置と姿勢をカメラで計測し、計測結果に基づいてロボット３をフィードバック制御し、カメラ２ｂをワーク１の移動に追従させることができる。
また、制御点移動制御により、追従点７とワーク位置との定常偏差Ｌ１を補償する位置に追従制御点６を移動させるので、追従点７をワーク位置に近づけることができる。
さらに追従点７は任意の場所に設定できるので、例えばハンド把持点ＴＣＰを追従点としておけば、最短距離で把持動作ができる。

また、旋回制御により、ハンド把持点ＴＣＰがカメラよりワーク側に位置するので、定常偏差のある方向にハンド把持点ＴＣＰを移動させることで、把持動作を短くできる。

従って、ビジュアルサーボ制御中に、ワークを把持する場合に、把持動作に要する把持時間を短縮し、把持時間に比例する把持位置の誤差を低減することができる。

なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。

１ ワーク（対象物）、２ａ ハンド、２ｂ カメラ、
３ ロボット、３ａ ロボットアーム、
４ テーブル、５ 画像、６ 追従制御点、
７ 追従点（原点）、８ 旋回機構、９ 吸着パッド、
１０ ロボット制御装置、１１ 第２旋回機構、
１２ 画像処理装置、１４ ロボットコントローラ
ｃｘ，ｃｙ，ｃｚ 検出されたワークの位置座標（目標点）、
ｃｘ_０，ｃｙ_０，ｃｚ_０ 追従点の位置座標、
ｐｘ，ｐｙ，ｐｚ 追従制御点の位置座標、
Ｌ０ ハンドとカメラの水平距離、
Ｌ１ 定常偏差、
ＴＣＰ ハンド把持点、
Ｖ ワークの移動速度

Claims (5)

1. ワークを把持するハンドと、該ハンドに取り付けられワークを撮影するカメラとを有し、前記ハンドを３次元的に移動可能なロボットを制御するロボット制御装置であって、
   前記カメラで撮影した画像を画像処理する画像処理装置と、該ロボットを制御するロボットコントローラと、前記ロボットに、カメラの光軸と平行な軸を中心に前記ハンドの把持位置に対しカメラを旋回させる旋回機構と、を備え、
   ワークの位置と姿勢をカメラで計測し、計測結果に基づいてロボットをフィードバック制御し、カメラをワークの移動に追従させるビジュアルサーボ制御を実施し、
   ビジュアルサーボ制御中に、追従点とワーク位置との定常偏差を補償する位置に追従制御点を移動させ、かつ前記把持位置がカメラよりワーク側に位置するように、カメラを旋回させた後に、ハンドでワークを把持する、ことを特徴とするロボット制御装置。
2. 前記カメラの計測範囲内にあり、前記ハンドの把持位置に最も近い点を前記追従点に設定する、ことを特徴とする請求項１に記載のロボット制御装置。
3. ワークを把持するハンドと、該ハンドに取り付けられワークを撮影するカメラとを有し、前記ハンドを３次元的に移動可能なロボットを制御するロボット制御方法であって、
   （Ａ） ワークの位置と姿勢をカメラで計測し、計測結果に基づいてロボットをフィードバック制御し、カメラをワークの移動に追従させるビジュアルサーボ制御と、
   （Ｂ） ビジュアルサーボ制御中に、追従点とワーク位置との定常偏差を補償する位置に追従制御点を移動させる制御点移動制御と、を有し、
   前記ロボットに、カメラの光軸と平行な軸を中心に前記ハンドの把持位置に対しカメラを旋回させる旋回機構を備え、
   ビジュアルサーボ制御中に、前記把持位置がカメラよりワーク側に位置するように、カメラを旋回させる旋回制御を実施し、
   前記制御点移動制御と旋回制御の後に、ハンドでワークを把持する、ことを特徴とするロボット制御方法。
4. 前記カメラの計測範囲内にあり、前記ハンドの把持位置に最も近い点を前記追従点に設定する、ことを特徴とする請求項３に記載のロボット制御方法。
5. 前記制御点移動制御において、
   カメラで撮影した画像上の追従制御点を追従点として前記定常偏差を計測し、
   次いで、前記定常偏差の変動が少なくなり前記追従が安定したことを確認し、
   次いで、前記追従点の近傍にワークが位置するように、前記追従制御点を移動させる、ことを特徴とする請求項３に記載のロボット制御方法。