JP2005335010A - 把持制御装置 - Google Patents

Abstract

【課題】 任意形状の物体を確実に且つ効率良く把持することができるロボットハンドの把持制御装置を提供する。
【解決手段】 この把持制御装置７は、画像認識ユニット１０と、ロボットハンド制御ユニット１１と、複数のモータドライバ１２とを有している。画像認識ユニット１０は、物体上の目標接触点の座標を決定し、目標接触点における法線ベクトルを求める。ロボットハンド制御ユニット１１は、指先部上の任意点の中から目標接触点における法線の方向と沿う方向の法線を有する候補接触点を求め、更に、その目標接触点と候補接触点とが実質的に接するような目標関節角度を求め、その目標関節角度に応じた関節角度指令値を作成する。各モータドライバ１２は、関節角度指令値に応じて各モータ５を駆動制御する。
【選択図】 図２

Description

本発明は、任意形状の物体を把持する複数本の指を有するロボットハンドの把持制御装置に関するものである。

ロボットにより物体を把持する従来技術としては、例えば特許文献１に記載されているものが知られている。この文献に記載のロボット制御装置は、ロボットをマニュアル動作させ、ロボット先端位置を記憶させることにより、移動経路上の代表的な位置・姿勢を表す参照点と運転時のロボット先端の動作位置の参照点に対する相対位置とを教示し、参照点の列で運転時のロボット先端の動作位置経路を指定し、その動作位置経路に従ってワークを把持するものである。
特開平２−５９２８５号公報

しかしながら、上記従来技術においては、把持すべき物体（ワーク）の種類が変わるたびに、どのような動作位置経路が適当かを教示によって決める必要がある。このため、ワークの種類が多いと、教示作業に手間がかかり、効率が悪くなる。また、任意形状のワークに対応するのは困難である。

本発明の目的は、任意形状の物体を確実に且つ効率良く把持することができるロボットハンドの把持制御装置を提供することである。

本発明の把持制御装置は、任意形状の物体を把持する複数本の指と、各指の関節を駆動させる複数のアクチュエータとを備えたロボットハンドの把持制御装置であって、物体上における各指の指先部と接する目標接触点を決定し、目標接触点における法線を求める接触位置設定手段と、各指について、指先部上に設定された任意点の中から、目標接触点における法線の方向に最も近い方向の法線を有する候補接触点を求める候補位置選定手段と、目標接触点と候補接触点が実質的に接するように関節の目標角度を求める把持姿勢演算手段と、各指について、関節の目標角度に応じてアクチュエータを制御する駆動制御手段と、を備える。

本発明のロボットハンドの把持制御装置によれば、互いの法線の方向が最も近い目標接触点と候補接触点とが実質的に接するように指関節の目標角度を求めるので、その目標角度になるように指関節を駆動すれば任意形状の物体を把持可能な状態とすることができる。

また本発明の把持制御装置では、把持姿勢演算手段が任意座標系における目標接触点と候補接触点との座標差を減少させるように関節の目標角度を求めることも好ましい。目標接触点と候補接触点との座標差を減少させるように指関節の目標角度を求めるので、簡便な計算で目標角度を算出できる。

本発明によれば、任意形状の物体を確実に且つ効率良く把持することができる。

本発明の知見は、例示のみのために示された添付図面を参照して以下の詳細な記述を考慮することによって容易に理解することができる。引き続いて、添付図面を参照しながら本発明の実施の形態を説明する。可能な場合には、同一の部分には同一の符号を付して、重複する説明を省略する。

図１は、本発明に係る把持制御装置の一実施形態が適用されるロボットハンドの主要部の外観を示す斜視図である。同図において、ロボットハンド１は、図示しないロボットアームに取り付けられたものであり、任意形状の物体２を把持する４本の指３（親指３Ａ、人差指３Ｂ、中指３Ｃ、薬指３Ｄ）を有している。これらの指３は、図示しないハンド掌部に連結されている。

親指３Ａは、４自由度のリンク系であり、４つの関節４Ａと、各関節４Ａを駆動させる４つのモータ５Ａとを有している。人差指３Ｂは、３自由度のリンク系であり、４つの関節４Ｂと、各関節４Ｂを駆動させる３つのモータ５Ｂとを有している。最も先端側の関節４Ｂは、連動関節構造となっている。中指３Ｃは、３自由度のリンク系であり、４つの関節４Ｃと、各関節４Ｃを駆動させる３つのモータ５Ｃとを有している。最も先端側の関節４Ｃは、連動関節構造となっている。薬指３Ｄは、３自由度のリンク系であり、４つの関節４Ｄと、各関節４Ｄを駆動させる３つのモータ５Ｄとを有している。最も先端側の関節４Ｄは、連動関節構造となっている。各指３Ａ〜３Ｄは、弾性体からなる曲面状の指先部６Ａ〜６Ｄ（以下、指先部６）を有している。

図２は、上記のロボットハンド１の各指３の把持姿勢を制御する把持制御装置を示す構成図である。同図において、把持制御装置７は、複数の６軸力センサ８と、複数のエンコーダ・ポテンショメータ９と、画像認識ユニット１０と、ロボットハンド制御ユニット１１と、複数のモータドライバ１２とを有している。

６軸力センサ７は、各指３の根元部に設けられ（図１参照）、把持反力を検出する。エンコーダ・ポテンショメータ９は、各モータ５Ａ〜５Ｄ（以下、モータ５）に設けられ、各関節４Ａ〜４Ｄ（以下、関節４）の角度（関節角度）θを検出する。関節角度θは、図３に示すように、関節４と連結された２本のリンク１３の長手方向に延びる線が交差する角度である。

画像認識ユニット１０は、図示はしないが、光源、撮像部及び画像処理部等を有している。画像認識ユニット１０は、図３に示すような物体２上における各指３の指先部６と接する目標接触点Ｐの座標を決定し、その目標接触点Ｐにおける物体曲面の法線ベクトルＶ₀を求める。法線ベクトルＶ₀は次式で与えられる。

なお、この法線ベクトルＶ₀は、ロボットハンド１の手首（図示せず）に固定された絶対座標系における座標である。

ロボットハンド制御ユニット１１は、把持姿勢演算部１４と、制御演算部１５とを有している。把持姿勢演算部１４は、画像認識ユニット１０で予め設定された物体上の目標接触点Ｐの座標及び法線ベクトルＶ₀を入力し、所定の演算を行い、物体２を把持するための指３の各関節４の目標角度（目標関節角度）を求める。なお、把持姿勢演算部１４による演算処理については、後で詳述する。

制御演算部１５は、把持姿勢演算部１４で求めた各指３の目標関節角度に応じた時系列の関節角度指令値を作成し、この指令値をモータドライバ１２に送出する。このとき、エンコーダ・ポテンショメータ９の検出信号を入力し、関節４の角度（関節角度）をフィードバック制御する。また、６軸力センサ８の検出信号を入力し、各指３による把持力をフィードバック制御してもよい。

モータドライバ１２は、制御演算部１５で作成された関節角度指令値に応じた駆動電流を各モータ５に供給して、各モータ５を回転駆動させる。

次に、ロボットハンド制御ユニット１１の把持姿勢演算部１４において、ロボットハンド１の把持姿勢を求める考え方について、以下に説明する。

（１）指先部の要素分割
図４に示すように、指先部６の表面を要素分割することにより、数多くの格子点Ｑを生成する。そして、関節４の回転中心を原点とし、指先リンク１６に固定されたリンク座標系（ローカル座標系）における各格子点Ｑの座標のデータベースを予め作成しておく。

（２）絶対座標系における指先部上の格子点座標の計算
リンク座標系と絶対座標系（前述）との間の座標変換を行うことにより、指先部６上の格子点Ｑの絶対座標系における座標を計算する。

図５に、人差指３ｂ、中指３ｃ、薬指３ｄの座標系の取り方を示す。同図において、ｘ_sｙ_sｚ_s座標系は、上述したようにロボットハンド１の手首に固定された絶対座標系である。ｘ₀ｙ₀ｚ₀座標系は、第１関節４ａにおけるモータの静止部分に固定された相対座標系である。ｘ_iｙ_iｚ_i（ｉ＝１〜４）座標系は、第２関節４ｂ、第３関節４ｃ及び第４関節４ｄの回転軸に固定されたリンク座標系である。

ｘ_iｙ_iｚ_i座標系の原点は、関節４ｂ〜４ｄまたはモータの中心点に位置している。ｘ_iｙ_iｚ_i座標系のｚ軸は、関節４ｂ〜４ｄの回転軸に対応している。ｘ_iｙ_iｚ_i座標系のｘ軸は、関節４ｂ〜４ｄの回転軸に直交し、且つ指先側のリンク１６ｂ〜１６ｄの長手方向に延びた軸である。ｘ_iｙ_iｚ_i座標系のｙ軸は、ｘ_iｙ_iｚ_i座標系のｘ軸及びｚ軸に直交する軸である。各リンク１６ａ〜１６ｃの長さはＬ₁〜Ｌ₃であり、各関節４ａ〜４ｄの関節角度はθ₁〜θ₄である。ここで、関節４ｃ，４ｄは連動関節であるため、関節４ｃの関節角度θ₃と関節４ｄの関節角度θ₄とは等しくなっている。

指先部６上の任意の格子点ｊのｘ₄ｙ₄ｚ₄座標系（指先リンク１６ｄのリンク座標系）における座標(x_j ⁽⁴⁾,y_j ⁽⁴⁾,z_j ⁽⁴⁾) は、関節角θ_iにより変わらず、前記データベースから事前に決められる既知量である。このとき、ｘ_sｙ_sｚ_s座標系における格子点ｊの座標(x_j ^(s),y_j ^(s),z_j ^(s)) は、次式で表される。

ここで、^sＴ₄は、ｘ₄ｙ₄ｚ₄座標系からｘ_sｙ_sｚ_s座標系への座標変換行列であり、次式のように隣り合うリンク座標系の座標変換行列により求められる。

ここで、ｘ₀ｙ₀ｚ₀座標系は、ｘ_sｙ_sｚ_s座標系の各座標軸の並進移動を施すことによって得られるので、^sＴ₀は次式で表される。

また、ｘ_iｙ_iｚ_i座標系は、ｘ_i-1ｙ_i-1ｚ_i-1座標系に次のような変換を施すことによって得られる。
（ａ）ｘ_i-1軸に沿ってａ_i-1だけ並進
（ｂ）ｘ_i-1軸回りにα_i-1だけ回転
（ｃ）回転後のｚ_i-1軸（即ちｚ_i軸）に沿ってｄ_iだけ並進
（ｄ）回転後のｚ_i-1軸（即ちｚ_i軸）回りにθ_iだけ回転

ｘ_iｙ_iｚ_i座標系からｘ_i-1ｙ_i-1ｚ_i-1座標系への座標変換行列^i-1Ｔ_iは、以上の（ａ）〜（ｄ）の同次変換の積で表せる。

上記（４）式のパラメータは、表１に示される。

表１の各パラメータを（４）式に代入して、座標変換行列⁰Ｔ₁，¹Ｔ₂，²Ｔ₃，³Ｔ₄を計算する。さらに、（３）式と（４）式を（２）式に代入して、座標変換行列^sＴ₄を計算する。最後に、（２）式を（１）式に代入して、指先部６上の任意点ｊのｘ_sｙ_sｚ_s座標系（絶対座標系）における座標を得る。

（３）指先部上の任意点の法線ベクトルの計算
上記と同様な方法で次のように指先部６上の任意点ｊの法線ベクトルを計算する。^sＴ₄は４×４行列、^sＴ'₄はその左上方の３×３行列である。

一方、任意点ｊのリンク４のｘ₄ｙ₄ｚ₄座標系における法線ベクトルＶ_j ⁽⁴⁾は指先局面により決められる既知量で、次式で表現される。

ｘ_sｙ_sｚ_s座標系（絶対座標系）における法線ベクトルＶ_j ^(S)は次式で計算される。

各関節角度θ_ｉが与えられる場合に、指先面上の任意点ｊの座標と法線ベクトルをそれぞれ（１）式及び（７）式で計算する。

（４）指先部上の候補接触点の選出
目標接触点Ｐにおける物体曲面の法線ベクトルＶ₀と平行な法線方向を持つ点であって、指先部６上の点を候補接触点と定義する。この候補接触点Aを求めるために、指先面上の全ての点に対して、次式によって法線方向誤差ｅを計算する。

指先面上の全ての点における法線方向誤差ｅを比較し、ｅが最小値となる点を候補接触点Ａとする。

（４）目標関節角度の逐次計算
ロボットハンド１の手首に固定された絶対座標系であるｘ_sｙ_sｚ_s座標系における目標接触点Ｐと候補接触点Ａとの座標差を、それぞれΔｘ、Δｙ、Δｚと仮定する。目標接触点Ｐで把持すれば、座標差Δｘ、Δｙ、Δｚは同時にゼロとなる。換言すれば、各関節角度を調整して、座標差Δｘ、Δｙ、Δｚを同時にゼロとすれば、確実に把持状態となる。

本実施形態では、上述のロボットハンド１の第４関節４ｄを省略することで形態を更に単純化し、３つの自由度を持つ指を例にとって逐次計算の手法を説明する。この逐次計算の手法は、３つの自由度を持つ指の各関節角度の調整量が、座標差Δｘ、Δｙ、Δｚへ及ぼす影響の寄与率を機構的に考察することに基づいている。この考察に基づいて、簡単な調整則で座標差Δｘ、Δｙ、Δｚをゼロに収束させ、候補接触点Ａを目標接触点Ｐに一致させて把持状態とするような調整則を検討する。

まず、座標差Δｘ、Δｙ、Δｚのそれぞれの方向（すなわち、ｘ_sｙ_sｚ_s座標系におけるｘ軸方向、ｙ軸方向、ｚ軸方向）に沿った各関節角度に対する寄与率について、図７及び図８を参照しながら説明する。図７は、目標接触点Ｐと候補接触点Ａとの関係をｘｚ平面に投影した図である。図８は、目標接触点Ｐと候補接触点Ａとの関係をｙｚ平面に投影した図である。

図７において、第１関節４ａだけを微小角度Δθ_１回転すれば、Δｘ、Δｙ、Δｚの３方向における候補接触点Ａの変位量Δｘ_θ１、Δｙ_θ１、Δｚ_θ１は次式で与えられる。

ただし、Ｓ_１は、第１関節４ａの回転中心から候補接触点Ａまでの距離がｘｚ平面に投影された長さである。

また、図８において、第２関節４ｂだけを微小角度Δθ_２だけ回転すれば、Δｘ、Δｙ、Δｚの３方向における候補接触点Ａの変位量Δｘ_θ２、Δｙ_θ２、Δｚ_θ２は次式で与えられる。

ただし、Ｓ_２は、第２関節４ｂの回転中心から候補接触点Ａまでの距離がｙｚ平面に投影された長さである。また、αはｙ軸からのＳ_２の立ち上がり角度と等しい角度である。

また、図８において、第３関節４ｃだけを微小角度Δθ_３だけ回転すれば、Δｘ、Δｙ、Δｚの３方向における候補接触点Ａの変位量Δｘ_θ３、Δｙ_θ３、Δｚ_θ３は次式で与えられる。

ただし、Ｓ_３は、第３関節４ｃの回転中心から候補接触点Ａまでの距離がｙｚ平面に投影された長さである。また、βはｙ軸からのＳ_３の立ち上がり角度と等しい角度である。

ここで、Δｘ方向への任意な関節の回転角度Δθ_ｉ（ｉ＝１〜３）の寄与率を次式で定義する。

尚、ｙ方向変位及びｚ方向変位についても同様に定義できる。

上記（９）〜（１２）式をまとめて、Δｘ、Δｙ、Δｚの各方向における、第１関節４ａの回転角度Δθ_１、第２関節４ｂの回転角度Δθ_２、第３関節４ｃの回転角度Δθ_３の寄与率をまとめると次の表に示すようになる。

この寄与率解析によって、Δｘ、Δｙ、Δｚの各方向における誤差を減少させるためにどの関節を動かせば効率がよいかが分かる。すなわち、Δｘの方向においてはΔθ_１の寄与率ｃｏｓθ_１が最も大きいので、第１関節４ａを回転させるのが最も効率的である。同様に、Δｙの方向においてはΔθ_２の寄与率ｓｉｎαが最も大きいので、第２関節４ｂを回転させるのが最も効率的である。同様に、Δｚの方向においてはΔθ_３の寄与率ｃｏｓβが最も大きいので、第３間接４ｃを回転させるのが最も効率的である。

上述の考察から、各関節の角度調整値をΔｘ、Δｙ、Δｚの各方向における誤差に比例して設定すればよいことが分かった。具体的な調整アルゴリズムを次式に示す。

ここで、ｋ１、ｋ２、ｋ３は定数で、各方向における誤差が減少するように正負が設定される。

引き続いて、ロボットハンド制御ユニット１１の把持姿勢演算部１４による演算処理手順について図９を参照しながら説明する。図９は、把持姿勢演算部１４の演算処理手順を示すフローチャートである。尚、この説明においても、上述のロボットハンド１の第４関節４ｄを省略することで形態を更に単純化し、３つの自由度を持つ指を例にとって逐次計算の手法を説明する。

まず、画像認識ユニット１０によって、物体２上の目標接触点Ｐの座標を決定し、その目標接触点Ｐにおける法線ベクトルＶ₀を検出する（ステップＳ０１）。

すると、把持姿勢演算部１４に、その物体２上の目標接触点Ｐの座標と法線ベクトルＶ₀とが入力される。把持姿勢演算部１４は、指の各関節角度θ_１〜θ_３の初期値を設定する（ステップＳ０２）。

続いて、各関節角度θ_１〜θ_３の初期値を上記（１）式に代入して、指先部６上の各点ｉの座標を計算する。また、（７）式により指先部６上の各点ｉにおける法線ベクトルＶ_iを計算する（ステップＳ０３）。

続いて、（８）式に基づいて法線方向誤差ｅを計算し、指先面上の全ての点における法線方向誤差ｅを比較し、ｅが最小値となる点を候補接触点Ａとする（ステップＳ０４）。

続いて、候補接触点Ｐと候補接触点Ａとの座標差Δｘ、Δｙ、Δｚを算出する（ステップＳ０５）。この座標差Δｘ、Δｙ、Δｚが次式を満たすか否か判断する（ステップＳ０６）。

ここで、εは閾値である。この（１４）式を満たさない場合には、関節角を次式に基づいて調整する（ステップＳ０７）。

その後、更に関節角を調整すべくステップＳ０３に戻る。

一方、ステップＳ０６において、（１４）式を満たしている場合には、その時に与えた関節角度θ_１〜θ_３を各関節４の目標関節角度に設定する（ステップＳ０８）。

以上のような演算処理を各指３Ａ〜３Ｄについて実行することにより、指３Ａ〜３Ｄの各関節４の目標関節角度を求め、ロボットハンド１全体としての把持姿勢角度を得る。

このように把持姿勢演算部１４で求められた指３Ａ〜３Ｄの各関節４の目標関節角度は制御演算部１５に送られ、制御演算部１５において関節角度指令値が作成される。そして、その関節角度指令値は各モータドライバ１２に送られ、各モータドライバ１２により各モータ５が駆動制御される。これにより、ロボットハンド１の各指３Ａ〜３Ｄが把持姿勢となり、各指３Ａ〜３Ｄにより物体２が把持されることとなる（ステップＳ０９）。また、６軸力センサ８の情報に基づいて把持力が制御される（ステップＳ１０）。

以上において、画像認識ユニット１０は、物体２上における各指３の指先部６と接する目標接触点を決定し、目標接触点における法線ベクトルＶ₀を求める接触位置設定手段を構成する。ロボットハンド制御ユニット１１の把持姿勢演算部１４は、各指３の指先部６上に設定された任意点の中から、法線ベクトルＶ₀に沿った線の方向に最も近い方向の法線を有する点を候補接触点とする候補位置選定手段として機能する。

ロボットハンド制御ユニット１１の把持姿勢演算部１４は、前記各指３について、目標接触点と候補接触点とが実質的に接するような関節４の目標角度を求める把持姿勢演算手段を構成する。ロボットハンド制御ユニット１１の制御演算部１５及びモータドライバ１２は、各指３について、関節４の目標角度に応じてアクチュエータとしてのモータ５を制御する駆動制御手段を構成する。

以上のように本実施形態にあっては、任意形状の物体２上に設定された目標接触点における法線の方向と沿う方向の法線を有する指先部６上の候補接触点を定め、更にこの目標接触点と候補接触点とが接するように目標関節角度を求め、この目標関節角度に応じて各指３のモータ５を駆動制御する。従って、各指３の指先部６が曲面状をなしている場合であっても、ロボットハンド１はあらゆる形状の物体２を確実に把持することができる。このとき、各指３の目標関節角度は、計算によって自動的に求められるので、ロボットハンド１の動作に関する教示作業が不要であり、これにより物体２を効率良く把持することが可能となる。

また、関節角度の調整量Δθを決定するために、（１５）式のような簡便な数式を用いることができるので、例えば、評価関数の導関数を求めるといった手法に比較して簡便な計算で調整量を求めることができる。

関節角度の調整量Δθが座標差に比例するので、指先から物体が離れている場合には大きなステップで早く接近させることができる。一方、指先が物体に近づいた場合には小牧ステップで正確に収束することにより、探索の効率が向上する。

また、上記実施形態のロボットハンドは４本の指を有するものであるが、指の数は複数本であればよく、また各指の関節数及び自由度数も、特に上記のものには限られない。

本実施形態のロボットハンドの主要部の外観を示す図である。 本実施形態の把持制御装置の構成を示す図である。 ロボットハンドの指と物体との位置姿勢関係を示す図である。 ロボットハンドの要素分割を示す図である。 ロボットハンドの人差指、中指、薬指の絶対座標系とリンク座標系を示す図である。 ロボットハンドの親指の絶対座標系とリンク座標系を示す図である。 ロボットハンドの候補接触点と目標接触点との誤差を収斂させる方法を説明するための図である。 ロボットハンドの候補接触点と目標接触点との誤差を収斂させる方法を説明するための図である。 図２に示す把持姿勢演算部の演算処理手順を説明するためのフローチャートである。

符号の説明

１…ロボットハンド、２…物体、３…指、３Ａ…親指、３Ｂ…人差指、３Ｃ…中指、３Ｄ…薬指、４…関節、５…モータ、６…指先部、７…把持制御装置、８…６軸力センサ、１０…画像認識ユニット、１１…ロボットハンド制御ユニット、１２…モータドライバ、１４…把持姿勢演算部、１５…制御演算部。

Claims (2)

1. 任意形状の物体を把持する複数本の指と、前記各指の関節を駆動させる複数のアクチュエータとを備えたロボットハンドの把持制御装置であって、
   前記物体上における前記各指の指先部と接する目標接触点を決定し、前記目標接触点における法線を求める接触位置設定手段と、
   前記各指について、前記指先部上に設定された任意点の中から、前記目標接触点における法線の方向に最も近い方向の法線を有する候補接触点を求める候補位置選定手段と、
   前記目標接触点と前記候補接触点が実質的に接するように前記関節の目標角度を求める把持姿勢演算手段と、
   前記各指について、前記関節の目標角度に応じて前記アクチュエータを制御する駆動制御手段と、
   を備える把持制御装置。
2. 前記把持姿勢演算手段は、任意座標系における前記目標接触点と前記候補接触点との座標差を減少させるように前記関節の目標角度を求める、請求項１に記載の把持制御装置。

Images