JP5708091B2 - ロボットの制御方法およびロボットの制御装置 - Google Patents

Description

本発明は、複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させる際におけるロボットの制御方法およびロボットの制御装置に関する。

工場などにおいてロボットが導入される目的としては、生産効率の向上、つまり生産タクトタイムの短縮を図るという点が大きい。生産タクトタイムの短縮を図るためには、ロボットにより行われる１つ１つの作業における動作時間を短くする必要がある。そのため、ロボットの動作時間が極力短くなるようなティーチング（教示）が作業者により行われている。すなわち、現状においてロボットの動作時間を短くする作業は、ティーチングを担当する作業者の個人能力に大きく依存していることになる。

ロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させるために行われるティーチングでは、目標位置におけるツールの先端位置および向きを決定するための各変数の値は、作業者により全て指定されるようになっている。上記各変数とは、ツール先端の各座標値に加え、ツール（手先）の所定軸まわりの回転角度（例えば、４軸の水平多関節型ロボットの場合であれば第４軸まわりの回転角度）のことである。

さて、ロボットの手先に取り付けられるツールの種類および作業内容によっては、そのツールの向き（＝手先の向き）にかかわらず、その作業を達成可能なケースがある。例えば、４軸の水平多関節型ロボットにツールとしてエアチャックが用いられる場合、ツールの向き（第４軸まわりの向き）にかかわらずワークを把持することができる。このように、ワークを把持可能な位置（目標位置）におけるツールの特定の向きが、どのような向きでもよい場合がある。

図７は、ツール先端を現在位置から目標位置に向けて移動させる際におけるロボットの態様を模式的に示している。図７に示すロボットは、４軸の水平多関節型ロボットであり、その手先にツールとしてエアチャックが取り付けられている。なお、図７では、各軸（各関節）および各リンクをそれぞれ丸印および直線で示すとともに、ツール先端を矢印で示している。図７において、実線で示す現在位置から点線または破線で示す目標位置までツール先端を移動させ、ツールであるエアチャックによりワークを把持する作業が行われる。

ロボットが破線で示す状態となる場合、ツール先端がワークの正面（図７中、下側の面）に正対する向きになっている。一方、ロボットが点線で示す状態となる場合、ツール先端がワークの正面に対して斜め向きとなっている。これらの各状態では、互いに各軸（関節）の回転角度が異なっている。すなわち、ツールの向きに応じてロボットの姿勢が変化する。そのため、上記各状態では、現在位置からの移動時間が互いに異なる。具体的には、目標位置におけるロボットの姿勢を破線で示した状態とした場合に比べ、点線で示した状態とした場合のほうが、現在位置から目標位置までの移動時間が短くなる。その理由は、点線の状態のほうが、破線の状態に比べ、最も移動時間が長くなる第１軸の移動量（回転量）が小さくなるためである。

上記した目標位置のツール（手先）の向きを不問とするケースにおけるティーチングでも、目標位置におけるツールの先端位置および向きを決定するための変数の値は、作業者により全て指定されることになる。言い換えると、目標位置におけるロボットの姿勢（各軸の回転角度）が作業者により決定されることになる。しかし、前述したとおり、目標位置におけるツールの向きを不問とするケースでは、ツールの先端位置が同じ目標位置となるロボットの姿勢が多数存在する。作業者は、それら多数の姿勢の中から、最適な姿勢を選んで決定する必要がある。つまり、作業者は、ロボットの動作時間が出来るだけ短くなるように、目標位置におけるツールの向きを決定するための変数の値を設定する必要がある。図７のケースであれば、目標位置におけるロボットの姿勢（ツールの向き）を、破線で示した状態ではなく、点線で示した状態となるようなティーチング作業が要求される。

このように、目標位置におけるツールの向きを不問とするケースにおいて、多数存在するロボットの姿勢の中から、動作時間が最短となる目標位置におけるロボットの姿勢（各軸の回転角度）を見つけ出す作業は非常に困難であり、たとえ熟練の作業者であっても、その作業に多大な時間を要することが多く、その作業負担が非常に大きい。

ところで、ロボットを用いた作業のサイクルタイムを短縮するための従来技術としては、例えば特許文献１に記載された技術が挙げられる。特許文献１に記載の技術は、所定の方向や回転方向については位置制御を行わず、その代わりに、力やトルク値による制御を行うというものである。

特開平５−２３９８２号公報

しかしながら、特許文献１に記載の技術は、正しく目標位置が計算可能となるように、ユーザにより位置情報および力やトルク値情報が与えられるということを前提としている。そのため、目標位置におけるロボットの姿勢を求める必要があると考えられる。つまり、特許文献１に記載の技術は、従来と手法が異なるだけで同じ程度の作業負担を要するものであり、ユーザによる作業負担が軽減されることは少ないと考えられるため、上記課題を解決する手段であるとは言えない。

本発明は上記事情に鑑みてなされたものであり、その目的は、目標位置におけるツールの向きを決定するための変数の値を設定する際におけるユーザの作業負担を軽減しつつ、ロボットの移動時間の短縮を図ることができるロボットの制御方法およびロボットの制御装置を提供することにある。

請求項１または３に記載の手段によれば、複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させる際の制御において、任意変数設定処理と、変数値決定処理とが実行される。一般的なティーチング作業では、ツールの先端位置および向きを決定するための各変数の値は、全てユーザにより指定される。なお、各変数の値とは、ツール先端の各座標値（Ｘ軸座標、Ｙ軸座標およびＺ軸座標の値）およびツール（手先）の所定軸まわりの回転角度（例えば、４軸の水平多関節型ロボットの場合であれば第４軸（Ｚ軸）まわりの回転角度）の値である。しかし、手先に取り付けられるツールの種類および作業内容によっては、そのツールの向き（＝手先の向き）にかかわらず、その作業を達成することが可能な場合もある。例えば、４軸の水平多関節型ロボットにツールとしてエアチャックが用いられる場合、ツールの向き（第４軸まわりの向き）にかかわらずワークを把持することができる。この場合、目標位置におけるツールの向きは、どのような向きでも構わないと言える。従来では、このような場合であっても、ツールの向きを決定するための変数（例えばＺ軸まわりの回転角度）をユーザが指定していた。

これに対し、本手段によれば、ツールの向きを決定するための変数のうち、ユーザにより指定される１つまたは２つの変数が任意の値に設定可能な任意変数として設定される（任意変数設定処理）。そして、ツールの先端が現在位置から目標位置に移動する際に要する移動時間と、移動後のツールの先端位置の目標位置に対する位置誤差とに基づく評価指標が設定され、その評価指標が最適化されて任意変数の値が決定される（変数値決定処理）。例えば、４軸の水平多関節型ロボットの手先にエアチャックが取り付けられる場合において、ツール（手先）のＺ軸まわりの回転角度が任意変数として指定されれば、目標位置におけるツールのＺ軸まわりの回転角度は、ツールの移動時間が極力短くなるような値に自動的に決定される。

すなわち、変数値決定処理では、現在位置が計算対象位置として設定され、その計算対象位置におけるツールの先端位置および向きを決定するための変数（Ｘ、Ｙ、Ｚ軸座標値およびＺ軸まわりの回転角度）のうち任意変数に指定されたＺ軸まわりの回転角度を除いた変数と、計算対象位置における各軸（関節）の回転角度とに関するヤコビ行列Ｊ_ｋが求められる。また、下記（１）式に示す評価指標Ｑ_ｋが設定される。ただし、位置誤差の座標値を要素とする行列をε_ｋとし、各軸の回転角度の変化量を要素とする行列をΔ_ｋとし、対角成分が各軸の最大回転速度の逆数の二乗である対角行列をλとしている。なお、ε_ｋ ^Ｔはε_ｋの転置行列であり、Δ_ｋ ^ＴはΔ_ｋの転置行列である。
Ｑ_ｋ＝ε_ｋ ^Ｔ・ε_ｋ＋Δ_ｋ ^Ｔ・λ・Δ_ｋ …（１）

上記（１）式において、右辺第一項は位置誤差の二乗和である。また、右辺第二項は各軸の回転角度の変化量（移動量）の二乗和に対角行列λを乗じたものであり、移動時間の二乗和に対して各軸の最大回転速度に応じた重み付けが与えられたものである。すなわち、移動時間の概算値に相当する。このように設定された評価指標Ｑ_ｋを最小化することにより、下記（２）式に示す変化量Δ_ｋが演算される。ただし、目標位置の座標値を要素とする行列をＰ_ｄとし、計算対象位置の座標値を要素とする行列をＰ_ｋとしている。なお、Ｊ_ｋ ^ＴはＪ_ｋの転置行列である。
Δ_ｋ＝（λ＋Ｊ_ｋ ^Ｔ・Ｊ_ｋ）^−１・Ｊ_ｋ ^Ｔ（Ｐ_ｄ−Ｐ_ｋ） …（２）

計算対象位置における各軸の回転角度に対して上記演算された変化量Δ_ｋを加えた各軸の回転角度、つまり移動後の各軸の回転角度について順変換を行うことにより、移動後のツールの先端位置が演算される。そして、演算された移動後のツールの先端位置の目標位置に対する位置誤差が求められ、その位置誤差が所定の許容誤差以下であるか否かが判断される。つまり、演算により求められた移動後のツールの先端位置が十分に目標位置に近づいているか否かが判断される。その結果、位置誤差が許容誤差を超える場合、演算した移動後のツールの先端位置を次の計算対象位置とした上で、次の移動後のツールの先端位置を求めるための上記各演算が再度実行される。

一方、上記判断の結果、位置誤差が許容誤差以下である場合、計算対象位置における各軸の回転角度に対して上記演算された変化量Δ_ｋを加えた各軸の回転角度（移動後の各軸の回転角度）に基づいて、目標位置におけるＺ軸まわりの回転角度の値が決定される。すなわち、上記演算により求められた移動後のツールの先端位置が十分に目標位置に近づいていると判断された場合、そのときのＺ軸まわりの回転角度（任意変数）の値が、目標位置におけるＺ軸まわりの回転角度（任意変数）の値として設定される。その設定されたＺ軸まわりの回転角度の値は、移動時間の概算値が含まれた評価指標Ｑ_ｋを最小化することにより設定されたものであるから、ツール（ロボット）の移動時間が極力短くなるような値となっている。

ロボット側において上記した制御が自動的に実行されるため、ユーザがティーチング時に行う作業としては、目標位置におけるツール先端の各座標値を指定するとともに、ツール（手先）のＺ軸まわりの回転角度を任意変数として指定するだけでよい。そして、値が指定された目標位置におけるツール先端のＸ、Ｙ、Ｚ軸座標の値に加え、上記したように自動的に決定された目標位置におけるツールのＺ軸まわりの回転角度（任意変数）の値に基づいて、ツールの先端を現在位置から目標位置に向けて移動させることにより、ティーチング時におけるユーザ（作業者）の作業負担を軽減しつつ、ロボットの移動時間の短縮を図ることができる。

なお、ここでは、４軸の水平多関節型ロボットの場合を例に説明したが、例えば６軸の垂直多関節型ロボットの場合であれば次のようになる。すなわち、ツールの向きを決定するための変数（Ｘ、Ｙ、Ｚ軸まわりの各回転角度）のうち、いずれか１つ、あるいは、いずれか２つが任意変数として指定されれば、目標位置におけるツール（手先）の特定の軸まわりの回転角度が、ロボットの移動時間が極力短くなるような値に自動的に決定される。その結果、上記した４軸の水平多関節型ロボットの場合と同様の効果が得られる。すなわち、本手段によれば、複数の軸を有するロボット全般について、上記した作用および効果が得られる。

本手段では、移動時間の概算値が含まれた二次形式の評価指標Ｑ_ｋを用い、ヤコビ行列を用いた最適化演算を行っている。そのため、任意変数の値を決定するための演算処理を比較的高速に行うことができる。ところで、ヤコビ行列は、各軸（各関節）における微小変位と、それに伴う手先の位置および向き（姿勢）の微小変位との比率を表すものである。本手段においては、このようなヤコビ行列を用いた演算が繰り返される結果、その演算により求められる移動後のツールの先端位置が徐々に目標位置に近づいていくことになる。そのため、上記変数値決定処理により決定された任意変数の値に基づくロボットの目標位置における姿勢（形態）は、現在位置における姿勢に対し、大きく変化することはない。姿勢の変化が小さいということは、各軸（関節）の移動量が小さいことを意味する。このようなことからも、上述したロボットの移動時間短縮の効果が得られるということが明らかである。

請求項１または３に記載の手段では、ヤコビ行列を用いた演算により求められる移動後のツールの先端位置が十分に目標位置に近づいていると判断された場合、そのときの任意変数の値を、目標位置における任意変数として設定していた。しかし、ヤコビ行列は上記した特性を有しているため、上記演算による最終的な移動後のツールの先端位置であっても目標位置から僅かに外れることになる。そのため、決定した任意変数の値が、必ずしもロボットの移動時間を確実に最小化できるものであるとは言いきれない。このように、請求項１または３に記載の手段により決定される任意変数の値には改善の余地がある。

請求項２または４に記載の手段によれば、請求項１または３に記載の手段により決定された任意変数の値を変更する変数値変更処理が実行される。その変数値変更処理は、現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その評価指標を最適化して、決定された任意変数の値を変更するというものである。例えば、前述した４軸の水平多関節型ロボットのケースであれば、目標位置におけるツールの先端位置および向きを決定するための変数のうち、ツールのＺ軸まわりの回転角度（任意変数）を除く変数の値が固定された上で、各軸移動距離に基づく評価指標を最小化する演算が繰り返し実行される。その結果、目標位置におけるツールのＺ軸まわりの回転角度は、移動時間がさらに短くなるような値に変更される。

具体的には、変数値変更処理では、ユーザにより指定されるツール先端の目標位置（Ｘ、Ｙ、Ｚ軸座標の値）と、決定された目標位置におけるツールのＺ軸まわりの回転角度（任意変数）の値（ツールの向き）とが、計算対象位置として設定される。このように、本手段における計算対象位置は、ツール先端の各軸座標値だけでなく、ツールの向きについても含むものとなっている。そして、その計算対象位置を決定するための各変数のうち、任意変数であるＺ軸まわりの回転角度を除いた変数と、計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊ_ｋが求められる。また、下記（３）式に示す評価指標Ｑ_ｋが設定される。ただし、本手段では、各軸移動距離を要素とする行列をε_ｋとしている。なお、ε_ｋ ^Ｔはε_ｋの転置行列である。
Ｑ_ｋ＝ε_ｋ ^Ｔ・λ・ε_ｋ …（３）

上記（３）式に示す評価指標Ｑ_ｋは、各軸移動距離の二乗和に対角行列λを乗じたものであり、移動時間の二乗和に対して各軸の最大回転速度に応じた重み付けが与えられたものである。すなわち、移動時間の概算値に相当する。このように設定された評価指標Ｑ_ｋを、ツールの先端位置を変化させないという束縛条件（Ｊ_ｋ・Δ_ｋ＝０）を満たしつつ最小化することにより、下記（４）式に示す変化量Δ_ｋが演算される。ただし、計算対象位置における各軸の回転角度を要素とする行列をθ_ｋとし、現在位置における各軸の回転角度を要素とする行列をθ_ｃとしている。
Δ_ｋ＝｛λ^−１・Ｊ_ｋ ^Ｔ・（Ｊ_ｋ・λ^−１・Ｊ_ｋ ^Ｔ）^−１・Ｊ_ｋ−Ｉ｝（θ_ｋ−θ_ｃ）
…（４）

計算対象位置における各軸の回転角度に対して上記演算された変化量Δ_ｋを加えた各軸の回転角度（移動後の各軸の回転角度）について順変換を行うことにより、移動後のツールの向きが演算される。そして、評価指標Ｑ_ｋの変化の傾向が判断される。その結果、評価指標Ｑ_ｋが所定の傾き以上の減少傾向にある場合、評価指標Ｑ_ｋをさらに最小化する余地があると考えられる。その場合、目標位置および演算されたツールの向きを次の計算対象位置として設定した上で、次の移動後のツールの向きを求めるための上記各演算が再度実行される。

一方、上記判断の結果、評価指標Ｑ_ｋが所定の傾き未満の減少傾向にある場合、または、変化がない場合、評価指標Ｑ_ｋをさらに最小化する余地が少ないと考えられる。その場合、計算対象位置における各軸の回転角度に対して上記演算された変化量Δ_ｋを加えた各軸の回転角度（移動後の各軸の回転角度）に基づいて、目標位置におけるツールのＺ軸まわりの回転角度の値が変更される。その変更されたツールのＺ軸まわりの回転角度の値は、移動時間の概算値が含まれた評価指標Ｑ_ｋを最小化することにより決定された値であるから、ツール（ロボット）の移動時間が極力短くなるような値となっている。さらに、上記演算は、目標位置におけるツールの先端位置および向きを決定するための変数のうち、ツールのＺ軸まわりの回転角度（任意変数）を除く変数の値が固定された上で行われている。つまり、ツール先端の位置を目標位置に固定した状態でもって、移動時間が極力短くなるツールのＺ軸まわりの回転角度が求められている。従って、ロボットの移動時間を一層短縮することができるという効果が得られる。

本発明の第１の実施形態を示すもので、ロボットシステムの概略構成図 変数値決定処理の内容を示すフローチャート 現在位置および目標位置におけるロボットの態様を模式的に表す図 目標領域内のいずれかの位置に目標位置が設定される場合の図３相当図 本発明の第２の実施形態を示すもので、変数値変更処理の内容を示すフローチャート 本発明の第３の実施形態を示す図１相当図 従来技術を示す図３相当図

（第１の実施形態）
以下、本発明の第１の実施形態について図１〜図４を参照しながら説明する。
図１は、一般的な産業用ロボットのシステム構成を示している。図１に示すロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。

ロボット２は、例えば４軸の水平多関節型ロボットとして構成されている。ロボット２は、設置面に固定されるベース５と、ベース５上にＺ軸（垂直軸）方向の軸心を持つ第１軸Ｊ１を中心に回転可能に連結された第１のアーム６と、第１のアーム６の先端部上にＺ軸方向の軸心を持つ第２軸Ｊ２を中心に回転可能に連結された第２のアーム７と、第２のアーム７の先端部に上下動可能で且つ回転可能に設けられたシャフト８とから構成されている。シャフト８を上下動させる際の軸が第３軸Ｊ３であり、回転させる際の軸が第４軸Ｊ４である。シャフト８の先端部（下端部）には、フランジ９が位置決めされて着脱可能に取り付けられる。

ベース５、第１のアーム６、第２のアーム７、シャフト８およびフランジ９は、ロボット２のアームとして機能する。アーム先端であるフランジ９（手先に相当）には、図示はしないが、例えばエアチャックなどのツールが取り付けられる。ロボット２に設けられる複数の軸（Ｊ１〜Ｊ４）はそれぞれに対応して設けられるモータ（図示せず）により駆動される。各モータの近傍には、それぞれの回転軸の回転角度（回転位置）を検出するための位置検出器（図示せず）が設けられている。

一般に、産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作するようになっている。コントローラ３（ロボットの制御装置に相当）は、その動作プログラムに基づいてモータの駆動をフィードバック制御し、ロボット２のアームの動作制御を行う。

ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。

一般に、ロボット２の手先であるフランジ９に取り付けられるツールの先端を、現在位置から目標位置に向けて移動させる動作のティーチング作業が行われる際、目標位置におけるロボット２の姿勢をどのようなものにするかはユーザにより決定される。すなわち、このようなティーチング作業の際、ツール先端の目標位置を決定するための変数（ツール先端のＸ軸座標値、Ｙ軸座標値およびＺ軸座標値）と、目標位置におけるツール（手先）の向きを決定するための変数（ツールのＺ軸まわりの回転角度）とは、全てユーザにより指定される。ただし、フランジ９に取り付けられるツールの種類（形状）およびそのツールを用いた作業の内容によっては、ツールの向きを不問とすることが可能である。例えば、ツールとしてエアチャックが用いられる場合、ツールのＺ軸（第４軸Ｊ４）まわりの向きがどのような向きであっても、ワークを把持することができる。

本実施形態のロボットシステム１では、上記したツールの向きを不問とすることが可能なケースにおいて、ユーザは、ツール先端の目標位置を示すＸ、Ｙ、Ｚ軸座標の値を指定するだけでよく、目標位置におけるツールのＺ軸まわりの回転角度Ｒｚの値を指定しなくてよい。その代わりに、ユーザは、ツールのＺ軸まわりの回転角度Ｒｚを任意変数として指定する。これを受けて、コントローラ３は、回転角度Ｒｚを、コントローラ３側で任意の値に設定可能な任意変数として設定する（任意変数設定処理）。また、詳細は後述するが、コントローラ３は、ツールの先端が現在位置から目標位置に移動する際に要する移動時間と、移動後のツールの先端位置の目標位置に対する位置誤差とに基づく評価指標を設定し、その評価指標を最適化することで任意変数の値を決定する（変数値決定処理）。このように、コントローラ３は、任意変数設定処理手段および変数値決定処理手段として機能する。本実施形態では、コントローラ３により上記各処理が実行されることにより、目標位置におけるツールの向きが、ロボット２の移動時間が極力短くなるようなものに決定される。

図２は、変数値決定処理の内容を示すフローチャートである。図２に示すように、変数値決定処理が開始されると（スタート）、現在位置が計算対象位置として設定される（ステップＳ１）。ステップＳ２では、計算対象位置におけるツールの先端位置および向きを決定するための変数（Ｘ、Ｙ、Ｚ軸座標値および回転角度Ｒｚ）のうち、任意変数に指定された回転角度Ｒｚを除いた変数と、計算対象位置における各軸（関節）の回転角度とに関するヤコビ行列Ｊ_ｋが求められる。そのヤコビ行列Ｊ_ｋは、下記（５）式のように表される。ただし、Ｘ、Ｙ、Ｚ軸座標値をそれぞれＸ_ｋ、Ｙ_ｋ、Ｚ_ｋとし、第１軸Ｊ１〜第４軸Ｊ４のそれぞれの回転角度をθ１_ｋ〜θ４_ｋとしている。

ステップＳ３では、下記（６）式に示す評価指標Ｑ_ｋが設定される。ただし、位置誤差の各座標値を要素とする行列をε_ｋとし、各軸Ｊ１〜Ｊ４の各回転角度θ１〜θ４の変化量を要素とする行列をΔ_ｋとし、対角成分が各軸Ｊ１〜Ｊ４のそれぞれの最大回転速度の逆数の二乗である対角行列をλとし、位置誤差および変化量の重み付けのための定数をβとしている。また、ε_ｋ ^Ｔはε_ｋの転置行列を示し、Δ_ｋ ^ＴはΔ_ｋの転置行列を示している。なお、ε_ｋ、λは、それぞれ下記（７）、（８）式により表される。ただし、目標位置のＸ、Ｙ、Ｚ軸座標値を要素とする行列をＰ_ｄとし、計算対象位置のＸ、Ｙ、Ｚ軸座標値を要素とする行列をＰ_ｋとし、第１軸Ｊ１〜第４軸Ｊ４のそれぞれについての最大回転速度をＶ１_ｍａｘ〜Ｖ４_ｍａｘとしている。
Ｑ_ｋ＝ε_ｋ ^Ｔ・ε_ｋ＋β・Δ_ｋ ^Ｔ・λ・Δ_ｋ …（６）
ε_ｋ＝Ｐ_ｄ−Ｐ_ｋ−Ｊ_ｋ・Δ_ｋ …（７）

上記（６）式において、右辺第一項は位置誤差の二乗和である。また、右辺第二項は各軸Ｊ１〜Ｊ４の回転角度の変化量（移動量）の二乗和に対角行列λを乗じたものであり、移動時間の概算値（近似値）に相当する。定数βは、ロボット２の各関節の可動範囲と動作速度に比例する。従って、定数βは、ロボット２の機種ごとに適宜設定すればよい。なお、評価指標Ｑ_ｋにおける位置誤差および移動時間の概算値の重み付けが均等であれば、定数βを省くことができる。ステップＳ４では、このように設定された評価指標Ｑ_ｋが最小化されることにより、下記（９）式に示す変化量Δ_ｋが演算される。ただし、Ｊ_ｋ ^ＴはＪ_ｋの転置行列を示している。
Δ_ｋ＝（β・λ＋Ｊ_ｋ ^Ｔ・Ｊ_ｋ）^−１・Ｊ_ｋ ^Ｔ（Ｐ_ｄ−Ｐ_ｋ） …（９）

ステップＳ５では、下記（１０）式に示すように、移動後の各軸の回転角度が求められる。ただし、移動後の各軸Ｊ１〜Ｊ４の回転角度を要素とする行列をθ_ｋ＋１とし、計算対象位置における各軸Ｊ１〜Ｊ４の回転角度を要素とする行列をθ_ｋとしている。また、定数γは目標位置への収束速度を調整するための定数を示している。なお、本実施形態では、定数γの値を１としているが、目標位置への収束速度を早くする場合には定数γの値を大きくすればよいし、収束速度を遅くする場合には定数γの値を小さくすればよい。
θ_ｋ＋１＝θ_ｋ＋γ・Δ_ｋ …（１０）

ステップＳ６では、下記（１１）式に示すように、移動後のツールの先端位置が求められる。すなわち、移動後の各軸の回転角度を要素とする行列θ_ｋ＋１について順変換を行うことにより、移動後のツールの先端位置のＸ、Ｙ、Ｚ軸座標値を要素とする行列Ｐ_ｋ＋１が求められる。ただし、ツールのオフセット長や各アームのリンク長などを考慮した上で各軸の回転角度からツール先端位置を求める関数をＪ２Ｐとして表している。
Ｐ_ｋ＋１＝Ｊ２Ｐ（θ_ｋ＋１） …（１１）

ステップＳ７では、演算により求められた移動後のツールの先端位置Ｐ_ｋ＋１の目標位置Ｐ_ｄに対する位置誤差が求められる。ステップＳ８では、ステップＳ７で求められた位置誤差が所定の許容誤差以下であるか否かが判断される。つまり、演算により求められた移動後のツールの先端位置Ｐ_ｋ＋１が十分に目標位置Ｐ_ｄに近づいているか否かが判断される。その結果、位置誤差が許容誤差を超える場合（ＮＯ）、ステップＳ９に進む。ステップＳ９では、移動後のツールの先端位置Ｐ_ｋ＋１が次の計算位置に設定される。このように次の計算位置が設定された上で、ステップＳ２に戻り、次の移動後のツールの先端位置を求めるための各演算（ステップＳ２〜Ｓ８）が再度実行される。なお、上記許容誤差は、例えば、ロボット２における位置制御の精度に基づいて適宜設定すればよい。

一方、ステップＳ８による判断の結果、位置誤差が許容誤差以下である場合（ＹＥＳ）、ステップＳ１０に進む。ステップＳ１０では、上記（１０）式に示した移動後の各軸Ｊ１〜Ｊ４の回転角度θ_ｋ＋１に基づいて、目標位置におけるＺ軸まわりの回転角度Ｒｚの値が決定され、処理が終了する（エンド）。すなわち、上記演算により求められた移動後のツールの先端位置が十分に目標位置に近づいていると判断された場合、そのときのＺ軸まわりの回転角度Ｒｚの値が、目標位置におけるＺ軸まわりの回転角度Ｒｚの値として設定される。その設定されたＺ軸まわりの回転角度Ｒｚは、移動時間の概算値が含まれた評価指標Ｑ_ｋを最小化することにより設定されたものであるから、ツール（ロボット２）の移動時間が極力短くなるような値になっている。

コントローラ３により上記した制御が自動的に実行されるため、ユーザがティーチング時に行う作業としては、前述したように目標位置におけるツール先端の各座標値を指定するとともに、ツールのＺ軸まわりの回転角度Ｒｚを任意変数として指定するだけでよい。そして、コントローラ３は、値が指定された目標位置におけるツール先端のＸ、Ｙ、Ｚ軸座標の値に加え、上記した各処理により自動的に決定された目標位置におけるツールのＺ軸まわりの回転角度Ｒｚの値（ツールの向き）に基づいて、ツール先端を現在位置から目標位置に向けて移動させることができる。これにより、ティーチング時におけるユーザ（作業者）の作業負担が軽減されるとともに、ロボット２の移動時間が短縮されることになる。

変数値決定処理においては、移動時間の概算値が含まれた二次形式の評価指標を用い、ヤコビ行列を用いた最適化演算を行っている。そのため、変数値決定処理における各種演算処理を比較的高速に行うことが可能となっている。ところで、ヤコビ行列は、各軸（関節）における微小変位と、それに伴う手先の位置および向き（ロボット２の姿勢）の微小変位との比率を表すものである。本実施形態では、このようなヤコビ行列を用いた演算が繰り返される結果、その演算により求められる移動後のツールの先端位置が徐々に目標位置に近づいていくことになる。そのため、上記変数値決定処理により決定された任意変数の値に基づいて定まるロボット２の目標位置での姿勢は、現在位置での姿勢に対し、大きく変化することはない。姿勢の変化が小さいということは、各関節の移動量も小さいということになる。このようなことからも、本実施形態によれば、ロボット２の移動時間が短縮されるという効果が得られることが分かる。

次に、本実施形態のロボットシステム１により得られる具体的な効果について、図３および図４も参照して説明する。
図３は、ツール先端を現在位置Ｐｃから目標位置Ｐｄに向けて移動させる際におけるロボットの態様を模式的に示している。図３に示すロボットは、４軸の水平多関節型ロボットであり、その手先にツールとしてエアチャックが取り付けられている。なお、図３では、各軸および各リンクをそれぞれ丸印および直線で示すとともに、ツール先端を矢印で示している。また、現在位置におけるロボットの状態を点線で示すとともに、目標位置におけるロボットの状態を実線で示している。ここでは、現在位置Ｐｃから目標位置Ｐｄまでツール先端を移動させ、ツールであるエアチャックによりワークを把持する作業が行われるものとする。

上記ケースにおけるティーチング作業は、従来の手法によれば次のように行われる。すなわち、ティーチングを担当する作業者は、まず、図３の（ａ）、（ｂ）、（ｃ）のそれぞれに示す状態（姿勢）について検討する。つまり、作業者は、ツールの向きを大まかに変えてみて、どの向きが良さそうであるかを推測する。その結果、（ｃ）の状態が良さそうであると判断した場合、（ｃ）の状態をベースにし、その（ｃ）状態からツールの向きを少しずつ変化させたもの（図３の（ｄ）、（ｅ）、（ｆ）のそれぞれに示す状態）について検討する。その結果、図３（ｆ）の状態が最適である（移動時間が短くなる）という判断に至る。なお、ここでは、説明を簡略化するため、作業者が検討する状態の数を少なくするとともに、少ない繰り返し作業で最適な状態が見つかるケースとしている。実際には、さらに多くの状態について検討したり、繰り返し回数が増えたりすることもある。

これに対し、本実施形態のロボットシステム１によれば、目標位置Ｐｄにおけるツールの向きは、コントローラ３により、ロボット２の移動時間が極力短くなるような向きに自動的に決定される。従って、従来の手法のような繰り返しの過程を経ることなく、図３の（ｆ）の状態（ツールの向き）が最適である（移動時間を極力短くできる）という結果が容易に得られる。

このように、従来の手法によれば、ティーチングを担当する作業者の経験と推測により、少しずつ目標位置におけるロボットの状態（姿勢）を変化させる。そして、その作業を何回（例えば５回）か繰り返した結果、例えば、現在位置および目標位置におけるツールの向きが互いに同じロボットの姿勢（図３の（ａ））に比べ、ロボットの動作時間が短くなる（例えば２５％程度の改善）ツールの向きを得ることが可能となっていた。これに対し、本実施形態のロボットシステム１によれば、ティーチングを担当する作業者の経験などに関係なく、従来の手法による効果と同程度の効果を得ることができる。さらに、その際、作業者は目標位置におけるロボットの姿勢について具体的に検討する必要がないため、その作業負担が大きく軽減されることになる。

図４は、ツール先端を現在位置Ｐｃから目標領域Ａｄ内の任意の目標位置に向けて移動させる際における図３相当図である。この場合、図４の（ａ）に示す目標領域Ａｄ内におけるどの位置に移動すればよいかについては実作業時にカメラなどの外部機器から与えられ、ティーチングの時点において作業者は知ることができないものとする。このようなケースでは、目標領域Ａｄ内のどの位置に目標位置が設定されるかによってツールの最適な向きが変化する。そのため、従来の手法では、図４の（ｂ）に示すように、目標領域Ａｄを複数の小領域に分割し、それら小領域ごとに最適なツールの向きを教示するなど、非常に手間がかかっていた。また、どのように目標領域Ａｄを分割するかによって、最適なツールの向きが得られるかどうかが変化するため、ティーチングを担当する作業者の個人能力に大きく依存する部分があった。

これに対し、本実施形態のロボットシステム１によれば、実作業時に初めてカメラなどの外部機器から目標位置が与えられるケースであっても、コントローラ３は、与えられた目標位置に基づいて、その目標位置における最適なツールの向きを自動的に決定する。そのため、従来の手法のように目標領域Ａｄを小領域に分割し、それら小領域ごとにティーチングをするといった手間のかかる作業を省くことが可能となる。

（第２の実施形態）
以下、第１の実施形態に対し、コントローラ３の制御内容を変更した第２の実施形態について図５を参照しながら説明する。
第１の実施形態では、ヤコビ行列を用いた演算により求められる移動後のツールの先端位置が十分に目標位置に近づいていると判断された場合、そのときの任意変数（ツールのＺ軸まわりの回転角度Ｒｚ）の値を、目標位置における任意変数の値として設定していた。しかし、ヤコビ行列は、第１の実施形態において述べたような特性を有しているため、上記演算による最終的な移動後のツールの先端位置であっても目標位置から僅かに外れることになる。そのため、決定された任意変数の値が、必ずしもロボット２の移動時間を確実に最小化できるものであるとは言いきれない。このように、変数値決定処理により決定される任意変数の値には改善の余地がある。

本実施形態のコントローラ３は、図２に示した任意変数値決定処理の実行後、決定された目標位置におけるツールの回転角度Ｒｚ（任意変数）の値を変更する任意変数変更処理を実行する。つまり、本実施形態のコントローラ３は、変数値変更処理手段としても機能する。その変数値変更処理は、現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その評価指標を最適化して、決定された任意変数の値を変更するというものである。すなわち、目標位置におけるツールの先端位置および向きを決定するための変数のうち、ツールのＺ軸まわりの回転角度Ｒｚを除く変数（Ｘ、ＹＺ軸座標値）の値が固定された上で、各軸移動距離に基づく評価指標を最小化する演算が繰り返し実行される。その結果、目標位置におけるツールのＺ軸まわりの回転角度Ｒｚは、移動時間がさらに短くなるような値に変更される。

図５は、任意変数変更処理の内容を示すフローチャートである。図５に示すように、任意変数変更処理が開始されると（スタート）、ステップＴ１が実行される。ステップＴ１では、ユーザにより指定されるツール先端の目標位置（Ｘ、Ｙ、Ｚ軸座標値）が計算対象位置として設定される。また、その計算対象位置におけるツールのＺ軸まわりの回転角度Ｒｚ（ツールの向き）は、変数値決定処理にて決定された値に設定される。このように、変数値変更処理における計算対象位置は、ツール先端の各軸座標値だけでなく、ツールの向きについても含むものとなっている。ステップＴ２では、計算対象位置（ツールの向きも含む）を決定するための各変数のうち、任意変数であるＺ軸まわりの回転角度Ｒｚを除いた変数と、計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊ_ｋが求められる。そのヤコビ行列Ｊ_ｋは、上記（５）式のように表される。

ステップＴ３では、下記（１２）式に示す評価指標Ｑ_ｋが設定される。ただし、ここでは、各軸移動距離を要素とする行列をε_ｋとしている。また、ε_ｋ ^Ｔはε_ｋの転置行列を示している。
Ｑ_ｋ＝ε_ｋ ^Ｔ・λ・ε_ｋ …（１２）

上記（１２）式に示す評価指標Ｑ_ｋは、各軸移動距離の二乗和に対角行列λを乗じたものであり、移動時間の二乗和に対して各軸の最大回転速度に応じた重み付けが与えられたものである。すなわち、移動時間の概算値（近似値）に相当する。ステップＴ４では、このように設定された評価指標Ｑ_ｋを、下記（１３）式に示す束縛条件ｇ_ｋを満たしつつ、最小化することにより、下記（１４）式に示す変化量Δ_ｋが演算される。ただし、現在位置における各軸Ｊ１〜Ｊ４の回転角度を要素とする行列をθ_ｃとしている。なお、上記演算の手法としては、例えばラグランジュの未定乗数法などを用いることができる。
Ｊ_ｋ・Δ_ｋ＝０ …（１３）
Δ_ｋ＝｛λ^−１・Ｊ_ｋ ^Ｔ・（Ｊ_ｋ・λ^−１・Ｊ_ｋ ^Ｔ）^−１・Ｊ_ｋ−Ｉ｝（θ_ｋ−θ_ｃ）
…（１４）

ステップＴ５では、第１の実施形態におけるステップＳ５と同様に、上記（１０）式に示した移動後の各軸の回転角度が求められる。ステップＴ６では、下記（１５）式に示すように、移動後のツールの先端位置および向きが求められる。すなわち、移動後の各軸の回転角度θ_ｋ＋１について順変換を行うことにより、移動後のツールの先端位置のＸ、Ｙ、Ｚ軸座標値およびＺ軸まわりの回転角度Ｒｚを要素とする行列Ｐ_ｋ＋１が求められる。
Ｐ_ｋ＋１＝Ｊ２Ｐ（θ_ｋ＋１） …（１５）

ステップＴ７、Ｔ８では、今回の最適化演算（ステップＴ３、Ｔ４）により最適化された評価指標Ｑ_ｋの値が、前回の最適化演算により最適化された評価指標Ｑ_ｋ−１からどのように変化したのかが判断される。すなわち、ステップＴ７、Ｔ８は、評価指標の変化の傾向を判断するものである。従って、変数値変更処理が実行されてから最初にステップＴ７に移行した際には、それらステップＴ７、Ｔ８をスキップしてステップＴ９に移行する。２回目以降に実行されるステップＴ７では、今回の評価指標Ｑ_ｋの値が、前回の評価指標Ｑ_ｋ−１の値から減少したか否かが判断される。

その結果、減少したと判断された場合（ステップＴ７で「ＹＥＳ」）、ステップＴ８に進む。ステップＴ８では、評価指標の減少量が所定のしきい値以上であるか否かが判断される。すなわち、ステップＴ８では、評価指標が所定の傾き以上の減少傾向にあるか否かが判断される。その結果、評価指標の減少量が所定のしきい値以上である、つまり評価指標が所定の傾き以上の減少傾向にあると判断された場合（ステップＴ８で「ＹＥＳ」）、ステップＴ９に進む。このような場合、評価指標をさらに最小化する余地が大きいと考えられる。

ステップＴ９では、次のように計算対象位置が再設定される。すなわち、ステップＴ９では、ツール先端のＸ、Ｙ、Ｚ軸座標値は目標位置のままとし、ステップＴ６で演算された移動後のツールの向きが、次の計算対象位置におけるツールの向きとして設定される。このように次の計算対象位置が設定された後、ステップＴ２に戻り、次の移動後のツールの向きを求めるための各演算（ステップＴ２〜Ｔ８）が再度実行される。

一方、ステップＴ７にて、評価指標が減少していないと判断された場合（ＮＯ）、または、ステップＴ８にて、評価指標の減少量が所定のしきい値未満である、つまり評価指標が所定の傾き未満の減少傾向にあると判断された場合（ＮＯ）、ステップＴ１０に進む。これらの場合、評価指標をさらに最小化する余地が少ないと考えられる。なお、ステップＴ７にて、評価指標が減少していないと判断された場合、評価指標が変化していないと考えられる。なぜなら、ここでは評価指標を最小化する演算を行っているため、評価指標が増加することはほとんど考えられない。従って、評価指標が減少していないということは、すなわち評価指標が変化していないということになる。ステップＴ１０では、上記（１０）式に示した移動後の各軸Ｊ１〜Ｊ４の回転角度θ_ｋ＋１に基づいて、目標位置におけるＺ軸まわりの回転角度Ｒｚの値が変更され、処理が終了する（エンド）。

ステップＴ１０により変更された目標位置におけるツールのＺ軸まわりの回転角度Ｒｚの値は、移動時間の概算値が含まれた評価指標Ｑ_ｋを最小化することにより決定された値であるから、ツール（ロボット２）の移動時間が極力短くなるような値となっている。さらに、上記各演算（ステップＴ２〜Ｔ８）は、目標位置におけるツールの先端位置および向きを決定するための変数のうち、ツールのＺ軸まわりの回転角度Ｒｚ（任意変数）を除く変数の値が固定された上で行われている。つまり、ツール先端の位置を目標位置に固定した状態でもって、移動時間が極力短くなるツールのＺ軸まわりの回転角度Ｒｚの値（ツールの向き）が求められている。従って、本実施形態によれば、ロボット２の移動時間を一層短縮することができるという効果が得られる。

さて、上記しきい値を小さくするほど、評価指標の最適化（最小化）の精度が高まり、ロボット２の移動時間短縮の効果が高まる。しかし、しきい値を小さくし過ぎると、最適化演算に多大な時間を要することになる。上記しきい値は、このようなトレードオフの関係を考慮した上で適切な値に設定するとよい。ただし、しきい値をどれだけ小さくしても、ロボット２を移動させるための指令値の出力分解能により移動時間の短縮効果は制限される。そのため、例えば、コントローラ３における指令値の出力分解能に応じた値に設定するということも一案である。例えば、出力分解能が８ｍｓであれば、その十分の一の０．８ｍｓなどに設定すれば、最適化に要する演算時間がむやみに増大することなく、移動時間の短縮効果が十分に得られると考えられる。

（第３の実施形態）
以下、上記各実施形態に対し、対象とするロボットの種類を変更した第３の実施形態について図６を参照しながら説明する。
図６は、第１の実施形態における図１相当図であり、上記各実施形態と同一部分には同一符号を付して説明を省略する。図６に示す本実施形態のロボットシステム２１は、図１に示した第１の実施形態のロボットシステム１に対し、ロボット２に代えてロボット２２を備えている点が異なる。

ロボット２２は、例えば６軸の垂直多関節型ロボットとして構成されている。すなわち、ベース２５上には、Ｚ軸方向の軸心を持つ第１軸Ｊ２１を介してショルダ部２６が水平方向に回転可能に連結されている。ショルダ部２６には、Ｙ軸方向の軸心を持つ第２軸Ｊ２２を介して上方に延びる下アーム２７の下端部が垂直方向に回転可能に連結されている。下アーム２７の先端部には、Ｙ軸方向の軸心を持つ第３軸Ｊ２３を介して第１の上アーム２８が垂直方向に回転可能に連結されている。第１の上アーム２８の先端部には、Ｘ軸方向の軸心を持つ第４軸Ｊ２４を介して第２の上アーム２９が捻り回転可能に連結されている。第２の上アーム２９の先端部には、Ｙ軸方向の軸心を持つ第５軸Ｊ２５を介して手首３０が垂直方向に回転可能に連結されている。手首３０には、Ｘ軸方向の軸心を持つ第６軸Ｊ２６を介してフランジ３１が捻り回転可能に連結されている。

ベース２５、ショルダ部２６、下アーム２７、第１の上アーム２８、第２の上アーム２９、手首３０およびフランジ３１は、ロボット２２のアームとして機能する。アーム先端であるフランジ３１（手先に相当）には、図示はしないが、例えばエアチャックなどのツールが取り付けられる。ロボット２２に設けられる複数の軸（Ｊ２１〜Ｊ２６）は、第１の実施形態のロボット２と同様、それぞれに対応して設けられるモータ（図示せず）により駆動される。また、各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器（図示せず）が設けられている。

このような本実施形態の構成においても、コントローラ３は、上記各実施形態と同様の任意変数設定処理および変数値決定処理や、任意変数変更処理を実行することが可能である。例えば、ツールの向きを決定するための変数（Ｘ、Ｙ、Ｚ軸まわりの回転角度Ｒｘ、Ｒｙ、Ｒｚ）のうち、いずれか１つ、あるいは、いずれか２つが任意変数として指定されれば、その指定された変数が任意変数として設定される（任意変数設定処理）。そして、コントローラ３により変数値決定処理（および変数値変更処理）が行われる。

なお、本実施形態では、図２のステップＳ２および図５のステップＴ２にて求められるヤコビ行列Ｊ_ｋについて、軸数の変更に基づいて変更すればよい。そのヤコビ行列Ｊ_ｋは、例えばツールのＸ軸まわりの回転角度ＲｘおよびＹ軸まわりの回転角度Ｒｙが任意変数として設定された場合、下記（１６）式のように表される。ただし、第１軸Ｊ２１〜第６軸Ｊ２６のそれぞれの回転角度をθ１_ｋ〜θ６_ｋとし、ツールのＺ軸まわりの回転角度をＲｚ_ｋとしている。

これにより、目標位置における任意変数の値（ツールの向き）が、ロボット２２の移動時間が極力短くなるような値に自動的に決定される。その結果、本実施形態の構成においても、上記各実施形態と同様の作用および効果が得られる。

（その他の実施形態）
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
上記各実施形態では、本発明を４軸の水平多関節型のロボット２、または６軸の垂直多関節型のロボット２２に適用した例を説明したが、本発明は、複数の軸を有するロボット全般に適用可能である。

図面中、２、２２はロボット、３はコントローラ（ロボットの制御装置、任意変数設定処理手段、変数値決定処理手段、変数値変更処理手段）、９、３１はフランジ（手先）、Ｊ１〜Ｊ４、Ｊ２１〜Ｊ２６は軸を示す。

Claims (4)

1. 複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させる際におけるロボットの制御方法であって、
   移動後のツールの向きを決定するための各変数のうち、ユーザにより指定される１つまたは２つの変数を任意の値に設定可能な任意変数として設定する任意変数設定処理と、
   ツールの先端が前記現在位置から前記目標位置に移動する際に要する移動時間と、移動後のツールの先端位置の前記目標位置に対する位置誤差とに基づく評価指標を設定し、その評価指標を最適化して前記目標位置における任意変数の値を決定する変数値決定処理と、
   を含み、
   前記変数値決定処理は、
   前記現在位置を計算対象位置として設定する第１ステップと、
   前記計算対象位置におけるツールの先端位置および向きを決定するための変数のうち前記任意変数を除いた変数と前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊ_ｋを求める第２ステップと、
   前記位置誤差の座標値を要素とする行列をε_ｋとし、各軸の回転角度の変化量を要素とする行列をΔ_ｋとし、対角成分を各軸の最大回転速度の逆数の二乗とした対角行列をλとし、前記目標位置の座標値を要素とする行列をＰ_ｄとし、前記計算対象位置の座標値を要素とする行列をＰ_ｋとして、下記（１）式に示す評価指標Ｑ_ｋを最小化することにより、下記（２）式に示す変化量Δ_ｋを演算する第３ステップと、
   Ｑ_ｋ＝ε_ｋ ^Ｔ・ε_ｋ＋Δ_ｋ ^Ｔ・λ・Δ_ｋ …（１）
   Δ_ｋ＝（λ＋Ｊ_ｋ ^Ｔ・Ｊ_ｋ）^−１・Ｊ_ｋ ^Ｔ（Ｐ_ｄ−Ｐ_ｋ） …（２）
   前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの先端位置を演算する第４ステップと、
   前記演算した移動後のツールの先端位置の前記目標位置に対する位置誤差を求め、その位置誤差が所定の許容誤差以下であるか否か判断する第５ステップと、
   を含み、
   前記第５ステップによる判断の結果、前記位置誤差が前記許容誤差を超える場合、前記演算した移動後のツールの先端位置を次の計算対象位置として設定した上で、前記第２〜第５ステップを再度実行し、
   前記第５ステップによる判断の結果、前記位置誤差が前記許容誤差以下である場合、前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度に基づいて、前記目標位置におけるツールの向きを決定するための前記任意変数の値を決定することを特徴とするロボットの制御方法。
2. 前記現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その評価指標を最適化して前記決定した任意変数の値を変更する変数値変更処理を含み、
   前記変数値変更処理は、
   前記目標位置および前記決定した任意変数の値に基づいて定まるツールの向きを、計算対象位置として設定する第６ステップと、
   前記計算対象位置を決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊ_ｋを求める第７ステップと、
   前記各軸移動距離を要素とする行列をε_ｋとし、前記計算対象位置における各軸の回転角度を要素とする行列をθ_ｋとし、前記現在位置における各軸の回転角度を要素とする行列をθ_ｃとして、下記（３）式に示す評価指標Ｑ_ｋを、前記束縛条件を満たしつつ最小化することにより、下記（４）式に示す変化量Δ_ｋを演算する第８ステップと、
   Ｑ_ｋ＝ε_ｋ ^Ｔ・λ・ε_ｋ …（３）
   Δ_ｋ＝｛λ^−１・Ｊ_ｋ ^Ｔ・（Ｊ_ｋ・λ^−１・Ｊ_ｋ ^Ｔ）^−１・Ｊ_ｋ−Ｉ｝（θ_ｋ−θ_ｃ）
   …（４）
   前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの向きを演算する第９ステップと、
   前記評価指標Ｑ_ｋの変化の傾向を判断する第１０ステップと、
   を含み、
   前記第１０ステップによる判断の結果、前記評価指標Ｑ_ｋが所定の傾き以上の減少傾向にある場合、前記目標位置および前記演算した移動後のツールの向きを次の計算対象位置として設定した上で、前記第７〜第１０ステップを再度実行し、
   前記第１０ステップによる判断の結果、前記評価指標Ｑ_ｋが所定の傾き未満の減少傾向にある場合または前記評価指標Ｑ_ｋに変化がない場合、前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度に基づいて、前記目標位置におけるツールの向きを決定するための前記任意変数の値を変更することを特徴とする請求項１に記載のロボットの制御方法。
3. 複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させるロボットの制御装置であって、
   移動後のツールの向きを決定するための各変数のうち、ユーザにより指定される１つまたは２つの変数を任意の値に設定可能な任意変数として設定する任意変数設定処理手段と、
   ツールの先端が前記現在位置から前記目標位置に移動する際に要する移動時間と、移動後のツールの先端位置の前記目標位置に対する位置誤差とに基づく評価指標を設定し、その評価指標を最適化して前記目標位置における任意変数の値を決定する変数値決定処理手段と、
   を備え、
   前記変数値決定処理手段は、
   前記現在位置を計算対象位置として設定し、
   前記計算対象位置におけるツールの先端位置および向きを決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊ_ｋを求め、
   前記位置誤差の座標値を要素とする行列をε_ｋとし、各軸の回転角度の変化量を要素とする行列をΔ_ｋとし、対角成分を各軸の最大回転速度の逆数の二乗とした対角行列をλとし、前記目標位置の座標値を要素とする行列をＰ_ｄとし、前記計算対象位置の座標値を要素とする行列をＰ_ｋとして、下記（１）式に示す評価指標Ｑ_ｋを最小化することにより、下記（２）式に示す変化量Δ_ｋを演算し、
   Ｑ_ｋ＝ε_ｋ ^Ｔ・ε_ｋ＋Δ_ｋ ^Ｔ・λ・Δ_ｋ …（１）
   Δ_ｋ＝（λ＋Ｊ_ｋ ^Ｔ・Ｊ_ｋ）^−１・Ｊ_ｋ ^Ｔ（Ｐ_ｄ−Ｐ_ｋ） …（２）
   前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの先端位置を演算し、
   前記演算した移動後のツールの先端位置の前記目標位置に対する位置誤差を求め、その位置誤差が所定の許容誤差以下であるか否か判断し、
   前記位置誤差が前記許容誤差を超える場合、前記演算した移動後のツールの先端位置を次の計算対象位置として設定した上で、次の移動後のツールの先端位置を求めるための前記各演算を再度実行し、
   前記位置誤差が前記許容誤差以下である場合、前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度に基づいて、前記目標位置におけるツールの向きを決定するための前記任意変数の値を決定することを特徴とするロボットの制御装置。
4. 前記現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その評価指標を最適化して前記決定した任意変数の値を変更する変数値変更処理手段を備え、
   前記変数値変更処理手段は、
   前記目標位置および前記決定した任意変数の値に基づいて定まるツールの向きを、計算対象位置として設定し、
   前記計算対象位置を決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊ_ｋを求め、
   前記各軸移動距離を要素とする行列をε_ｋとし、前記計算対象位置における各軸の回転角度を要素とする行列をθ_ｋとし、前記現在位置における各軸の回転角度を要素とする行列をθ_ｃとして、下記（３）式に示す評価指標Ｑ_ｋを、前記束縛条件を満たしつつ最小化することにより、下記（４）式に示す変化量Δ_ｋを演算し、
   Ｑ_ｋ＝ε_ｋ ^Ｔ・λ・ε_ｋ …（３）
   Δ_ｋ＝｛λ^−１・Ｊ_ｋ ^Ｔ・（Ｊ_ｋ・λ^−１・Ｊ_ｋ ^Ｔ）^−１・Ｊ_ｋ−Ｉ｝（θ_ｋ−θ_ｃ）
   …（４）
   前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの向きを演算し、
   前記評価指標Ｑ_ｋの変化の傾向を判断し、
   前記評価指標Ｑ_ｋが所定の傾き以上の減少傾向にある場合、前記目標位置および前記演算した移動後のツールの向きを次の計算対象位置として設定した上で、次の移動後のツールの向きを求めるための前記各演算を再度実行し、
   前記評価指標Ｑ_ｋが所定の傾き未満の減少傾向にある場合または前記評価指標Ｑ_ｋに変化がない場合、前記計算対象位置における各軸の回転角度に対し前記演算した変化量Δ_ｋを加えた各軸の回転角度に基づいて、前記目標位置におけるツールの向きを決定するための前記任意変数の値を変更することを特徴とする請求項３に記載のロボットの制御装置。

Images