(57)【特許請求の範囲】
【請求項１】
複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させる際におけるロボットの制御方法であって、
移動後のツールの向きを決定するための各変数のうち、ユーザにより指定される１つまたは２つの変数を任意の値に設定可能な任意変数として設定する任意変数設定処理と、
前記現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく第１評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その第１評価指標を最適化して前記目標位置における任意変数の値を決定する変数値決定処理と、
を含み、
前記変数値決定処理は、
前記ロボットの姿勢を各軸の回転角度に基づいて予め複数に区別した形態に従って、前記ロボットが所定の形態をとり且つ前記目標位置にツールの先端が到達する各軸の回転角度の値を、前記ロボットがとり得る全ての形態のそれぞれについて取得する第１ステップと、
前記全ての形態のうちいずれか１つを、前記目標位置における任意変数の候補値を求める演算の演算対象として設定する第２ステップと、
前記演算対象について取得された前記各軸の回転角度の値から前記任意変数の値を演算する第３ステップと、
前記目標位置および前記演算した任意変数の値に基づいて定まるツールの向きを、計算対象位置として設定する第４ステップと、
前記計算対象位置を決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊkを求める第５ステップと、
前記各軸移動距離を要素とする行列をεkとし、対角成分を各軸の最大回転速度の逆数の二乗とした対角行列をλとし、各軸の回転角度の変化量を要素とする行列をΔkとし、前記計算対象位置における各軸の回転角度を要素とする行列をθkとし、前記現在位置における各軸の回転角度を要素とする行列をθcとして、下記（１）式に示す第１評価指標Ｑkを、前記束縛条件を満たしつつ最小化することにより、下記（２）式に示す変化量Δkを演算する第６ステップと、
Ｑk＝εkT・λ・εk      …（１）
Δk＝｛λ-1・ＪkT・（Ｊk・λ-1・ＪkT）-1・Ｊk−Ｉ｝（θk−θc）  …（２）
前記計算対象位置における各軸の回転角度に対し前記（２）式で演算した変化量Δkを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの向きを演算する第７ステップと、
前記第１評価指標Ｑkの変化の傾向を判断する第８ステップと、
を含み、
前記第８ステップによる判断の結果、前記第１評価指標Ｑkが所定の傾き以上の減少傾向にある場合、前記目標位置および前記演算した移動後のツールの向きを次の計算対象位置として設定した上で、前記第５〜第８ステップを再度実行し、
前記第８ステップによる判断の結果、前記第１評価指標Ｑkが所定の傾き未満の減少傾向にある場合または前記第１評価指標Ｑkに変化がない場合、前記計算対象位置における各軸の回転角度に対し前記（２）式で演算した変化量Δkを加えた各軸の回転角度に基づいて求められる前記任意変数の値を、前記目標位置における前記任意変数の候補値とし、
前記任意変数の候補値を求めた後、前記演算対象に設定されていない形態が存在する場合、それら形態のうち、いずれか１つを次の演算対象として設定した上で、前記第３〜第８ステップを再度実行し、
前記任意変数の候補値を求めた後、前記演算対象に設定されていない形態が存在しない場合、前記候補値のうち、前記第１評価指標Ｑkを最も小さくする値を、前記目標位置における任意変数の値として決定することを特徴とするロボットの制御方法。
【請求項２】
前記変数値決定処理の第１ステップは、
前記ロボットが所定の形態をとる各軸の回転角度の値が、前記ロボットがとり得る全ての形態のそれぞれについて与えられると、それら全ての形態のうちいずれか１つを、前記目標位置にツールの先端が到達する各軸の回転角度を取得するための演算の演算対象として設定する第９ステップと、
前記演算対象について与えられた前記各軸の回転角度の値から前記ツールの先端位置を求める第１０ステップと、
前記求められたツールの先端位置を計算対象位置として設定する第１１ステップと、
前記計算対象位置におけるツールの先端位置および向きを決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊkを求める第１２ステップと、
移動後のツールの先端位置の前記目標位置に対する位置誤差の座標値を要素とする行列をεkとし、前記目標位置の座標値を要素とする行列をＰdとし、前記計算対象位置の座標値を要素とする行列をＰkとして、下記（３）式に示す第２評価指標Ｑkを最小化することにより、下記（４）式に示す変化量Δkを演算する第１３ステップと、
Ｑk＝εkT・εk＋ΔkT・λ・Δk                  …（３）
Δk＝（λ＋ＪkT・Ｊk）-1・ＪkT（Ｐd−Ｐk）      …（４）
前記計算対象位置における各軸の回転角度に対し前記（４）式で演算した変化量Δkを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの先端位置を演算する第１４ステップと、
前記演算した移動後のツールの先端位置の前記目標位置に対する位置誤差を求め、その位置誤差が所定の許容誤差以下であるか否か判断する第１５ステップと、
を含み、
前記第１５ステップによる判断の結果、前記位置誤差が前記許容誤差を超える場合、前記演算した移動後のツールの先端位置を次の計算対象位置として設定した上で、前記第１２〜第１５ステップを再度実行し、
前記第１５ステップによる判断の結果、前記位置誤差が前記許容誤差以下である場合、前記計算対象位置における各軸の回転角度に対し前記（４）式で演算した変化量Δkを加えた各軸の回転角度の値を、前記ロボットが前記演算対象の形態をとり且つ前記目標位置にツールの先端が到達する各軸の回転角度の値として取得し、
前記各軸の回転角度の値を取得した後、前記演算対象に設定されていない形態が存在しなくなるまで、それら形態のうち、いずれか１つを次の演算対象として設定した上で、前記第１０〜第１５ステップを再度実行することを特徴とする請求項１に記載のロボットの制御方法。
【請求項３】
複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させるロボットの制御装置であって、
移動後のツールの向きを決定するための各変数のうち、ユーザにより指定される１つまたは２つの変数を任意の値に設定可能な任意変数として設定する任意変数設定処理と、
前記現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく第１評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その第１評価指標を最適化して前記目標位置における任意変数の値を決定する変数値決定処理と、
を備え、
前記変数値決定処理手段は、
前記ロボットの姿勢を各軸の回転角度に基づいて予め複数に区別した形態に従って、前記ロボットが所定の形態をとり且つ前記目標位置にツールの先端が到達する各軸の回転角度の値を、前記ロボットがとり得る全ての形態のそれぞれについて取得し、
前記全ての形態のうちいずれか１つを、前記目標位置における任意変数の候補値を求める演算の演算対象として設定し、
前記演算対象について取得された前記各軸の回転角度の値から前記任意変数の値を演算し、
前記目標位置および前記演算した任意変数の値に基づいて定まるツールの向きを、前記第１評価指標の計算対象位置として設定し、
前記計算対象位置を決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊkを求め、
前記各軸移動距離を要素とする行列をεkとし、対角成分を各軸の最大回転速度の逆数の二乗とした対角行列をλとし、各軸の回転角度の変化量を要素とする行列をΔkとし、前記計算対象位置における各軸の回転角度を要素とする行列をθkとし、前記現在位置における各軸の回転角度を要素とする行列をθcとして、下記（１）式に示す第１評価指標Ｑkを、前記束縛条件を満たしつつ最小化することにより、下記（２）式に示す変化量Δkを演算し、
Ｑk＝εkT・λ・εk      …（１）
Δk＝｛λ-1・ＪkT・（Ｊk・λ-1・ＪkT）-1・Ｊk−Ｉ｝（θk−θc）      …（２）
前記計算対象位置における各軸の回転角度に対し前記（２）式で演算した変化量Δkを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの向きを演算し、
前記第１評価指標Ｑkの変化の傾向を判断し、
前記第１評価指標Ｑkが所定の傾き以上の減少傾向にある場合、前記目標位置および前記演算した移動後のツールの向きを次の計算対象位置として設定した上で、次の移動後のツールの向きを求めるための前記各演算を再度実行し、
前記第１評価指標Ｑkが所定の傾き未満の減少傾向にある場合または前記第１評価指標Ｑkに変化がない場合、前記計算対象位置における各軸の回転角度に対し前記（２）式で演算した変化量Δkを加えた各軸の回転角度に基づいて求められる前記任意変数の値を、前記目標位置における前記任意変数の候補値とし、
前記任意変数の候補値を求めた後、前記演算対象に設定されていない形態が存在する場合、それら形態のうち、いずれか１つを次の演算対象として設定した上で、前記任意変数の候補値を求めるための前記各演算を再度実行し、
前記任意変数の候補値を求めた後、前記演算対象に設定されていない形態が存在しない場合、前記候補値のうち、前記第１評価指標Ｑkを最も小さくする値を、前記目標位置における任意変数の値として決定することを特徴とするロボットの制御装置。
【請求項４】
前記変数値決定処理手段は、
前記ロボットが所定の形態をとる各軸の回転角度の値が、前記ロボットがとり得る全ての形態のそれぞれについて与えられると、それら全ての形態のうちいずれか１つを、前記目標位置にツールの先端が到達する各軸の回転角度を取得するための演算の演算対象として設定し、
前記演算対象について与えられた前記各軸の回転角度の値から前記ツールの先端位置を求め、
前記求められたツールの先端位置を計算対象位置として設定し、
前記計算対象位置におけるツールの先端位置および向きを決定するための変数のうち前記任意変数を除いた変数と、前記計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊkを求め、
移動後のツールの先端位置の前記目標位置に対する位置誤差の座標値を要素とする行列をεkとし、前記目標位置の座標値を要素とする行列をＰdとし、前記計算対象位置の座標値を要素とする行列をＰkとして、下記（３）式に示す第２評価指標Ｑkを最小化することにより、下記（４）式に示す変化量Δkを演算し、
Ｑk＝εkT・εk＋ΔkT・λ・Δk                    …（３）
Δk＝（λ＋ＪkT・Ｊk）-1・ＪkT（Ｐd−Ｐk）      …（４）
前記計算対象位置における各軸の回転角度に対し前記（４）式で演算した変化量Δkを加えた各軸の回転角度について順変換を行うことにより、移動後のツールの先端位置を演算し、
前記演算した移動後のツールの先端位置の前記目標位置に対する位置誤差を求め、その位置誤差が所定の許容誤差以下であるか否か判断し、
前記位置誤差が前記許容誤差を超える場合、前記演算した移動後のツールの先端位置を次の計算対象位置として設定した上で、次の移動後のツールの先端位置を求めるための前記各演算を再度実行し、
前記位置誤差が前記許容誤差以下である場合、前記計算対象位置における各軸の回転角度に対し前記（４）式で演算した変化量Δkを加えた各軸の回転角度の値を、前記ロボットが前記演算対象の形態をとり且つ前記目標位置にツールの先端が到達する各軸の回転角度の値として取得し、
前記各軸の回転角度の値を取得した後、前記演算対象に設定されていない形態が存在しなくなるまで、それら形態のうち、いずれか１つを次の演算対象として設定した上で、各軸の回転角度を取得するための前記各演算を再度実行することを特徴とする請求項３に記載のロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させる際におけるロボットの制御方法およびロボットの制御装置に関する。
【背景技術】
【０００２】
工場などにおいてロボットが導入される目的としては、生産効率の向上、つまり生産タクトタイムの短縮を図るという点が大きい。生産タクトタイムの短縮を図るためには、ロボットにより行われる１つ１つの作業における動作時間を短くする必要がある。そのため、ロボットの動作時間が極力短くなるようなティーチング（教示）が作業者により行われている。すなわち、現状においてロボットの動作時間を短くする作業は、ティーチングを担当する作業者の個人能力に大きく依存していることになる。
【０００３】
ロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させるために行われるティーチングでは、目標位置におけるツールの先端位置および向きを決定するための各変数の値は、作業者により全て指定されるようになっている。上記各変数とは、ツール先端の各座標値に加え、ツール（手先）の所定軸まわりの回転角度（例えば、４軸の水平多関節型ロボットの場合であれば第４軸まわりの回転角度）のことである。
【０００４】
さて、ロボットの手先に取り付けられるツールの種類および作業内容によっては、そのツールの向き（＝手先の向き）にかかわらず、その作業を達成可能なケースがある。例えば、４軸の水平多関節型ロボットにツールとしてエアチャックが用いられる場合、ツールの向き（第４軸まわりの向き）にかかわらずワークを把持することができる。このように、ワークを把持可能な位置（目標位置）におけるツールの特定の向きが、どのような向きでもよい場合がある。
【０００５】
図１５は、ツール先端を現在位置から目標位置に向けて移動させる際におけるロボットの態様を模式的に示している。図１５に示すロボットは、４軸の水平多関節型ロボットであり、その手先にツールとしてエアチャックが取り付けられている。なお、図１５では、各軸（各関節）および各リンクをそれぞれ丸印および直線で示すとともに、ツール先端を矢印で示している。図１５において、実線で示す現在位置から点線または破線で示す目標位置までツール先端を移動させ、ツールであるエアチャックによりワークを把持する作業が行われる。
【０００６】
ロボットが破線で示す状態となる場合、ツール先端がワークの正面（図１５中、下側の面）に正対する向きになっている。一方、ロボットが点線で示す状態となる場合、ツール先端がワークの正面に対して斜め向きとなっている。これらの各状態では、互いに各軸（関節）の回転角度が異なっている。すなわち、ツールの向きに応じてロボットの姿勢が変化する。そのため、上記各状態では、現在位置からの移動時間が互いに異なる。具体的には、目標位置におけるロボットの姿勢を破線で示した状態とした場合に比べ、点線で示した状態とした場合のほうが、現在位置から目標位置までの移動時間が短くなる。その理由は、点線の状態のほうが、破線の状態に比べ、最も移動時間が長くなる第１軸の移動量（回転量）が小さくなるためである。
【０００７】
上記した目標位置のツール（手先）の向きを不問とするケースにおけるティーチングでも、目標位置におけるツールの先端位置および向きを決定するための変数の値は、作業者により全て指定されることになる。言い換えると、目標位置におけるロボットの姿勢（各軸の回転角度）が作業者により決定されることになる。しかし、前述したとおり、目標位置におけるツールの向きを不問とするケースでは、ツールの先端位置が同じ目標位置となるロボットの姿勢が多数存在する。作業者は、それら多数の姿勢の中から、最適な姿勢を選んで決定する必要がある。つまり、作業者は、ロボットの動作時間が出来るだけ短くなるように、目標位置におけるツールの向きを決定するための変数の値を設定する必要がある。図１５のケースであれば、目標位置におけるロボットの姿勢（ツールの向き）を、破線で示した状態ではなく、点線で示した状態となるようなティーチング作業が要求される。
【０００８】
このように、目標位置におけるツールの向きを不問とするケースにおいて、多数存在するロボットの姿勢の中から、動作時間が最短となる目標位置におけるロボットの姿勢（各軸の回転角度）を見つけ出す作業は非常に困難であり、たとえ熟練の作業者であっても、その作業に多大な時間を要することが多く、その作業負担が非常に大きい。
【０００９】
ところで、ロボットを用いた作業のサイクルタイムを短縮するための従来技術としては、例えば特許文献１に記載された技術が挙げられる。特許文献１に記載の技術は、所定の方向や回転方向については位置制御を行わず、その代わりに、力やトルク値による制御を行うというものである。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】特開平５−２３９８２号公報
【発明の概要】
【発明が解決しようとする課題】
【００１１】
しかしながら、特許文献１に記載の技術は、正しく目標位置が計算可能となるように、ユーザにより位置情報および力やトルク値情報が与えられるということを前提としている。そのため、目標位置におけるロボットの姿勢を求める必要があると考えられる。つまり、特許文献１に記載の技術は、従来と手法が異なるだけで同じ程度の作業負担を要するものであり、ユーザによる作業負担が軽減されることは少ないと考えられるため、上記課題を解決する手段であるとは言えない。
【００１２】
本発明は上記事情に鑑みてなされたものであり、その目的は、目標位置におけるツールの向きを決定するための変数の値を設定する際におけるユーザの作業負担を軽減しつつ、ロボットの移動時間の短縮を図ることができるロボットの制御方法およびロボットの制御装置を提供することにある。
【課題を解決するための手段】
【００１３】
請求項１または３に記載の手段によれば、複数の軸を有するロボットの手先に取り付けられるツールの先端を現在位置から目標位置に向けて移動させる際の制御において、任意変数設定処理と、変数値決定処理とが実行される。一般的なティーチング作業では、ツールの先端位置および向きを決定するための各変数の値は、全てユーザにより指定される。なお、各変数の値とは、ツール先端の各座標値（Ｘ軸座標、Ｙ軸座標およびＺ軸座標の値）およびツール（手先）の所定軸まわりの回転角度（例えば、４軸の水平多関節型ロボットの場合であれば第４軸（Ｚ軸）まわりの回転角度）の値である。しかし、手先に取り付けられるツールの種類および作業内容によっては、そのツールの向き（＝手先の向き）にかかわらず、その作業を達成することが可能な場合もある。例えば、４軸の水平多関節型ロボットにツールとしてエアチャックが用いられる場合、ツールの向き（第４軸まわりの向き）にかかわらずワークを把持することができる。この場合、目標位置におけるツールの向きは、どのような向きでも構わないと言える。従来では、このような場合であっても、ツールの向きを決定するための変数（例えばＺ軸まわりの回転角度）をユーザが指定していた。
【００１４】
これに対し、本手段によれば、ツールの向きを決定するための変数のうち、ユーザにより指定される１つまたは２つの変数が任意の値に設定可能な任意変数として設定される（任意変数設定処理）。そして、現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく第１評価指標が設定され、ツールの先端位置を変化させないという束縛条件の下で、その第１評価指標が最適化されて任意変数の値が決定される（変数値決定処理）。例えば、４軸の水平多関節型ロボットの手先にエアチャックが取り付けられる場合において、ツール（手先）のＺ軸まわりの回転角度が任意変数として指定されれば、目標位置におけるツールのＺ軸まわりの回転角度は、ツールの移動時間が極力短くなるような値に自動的に決定される。
【００１５】
さて、目標位置における任意変数（例えばツールのＺ軸まわりの回転角度）が取り得る全ての値について第１評価指標を演算し、任意変数の関数（以下では評価関数と言う）として表す（グラフ化する）と、次のようなことが分かる。すなわち、その評価関数において、第１評価指標の最小値（最適値）の部分が谷の底になるとともに、第１評価指標の最小値ではない別の値（最小値に準ずる値であり、以下では局所値と言う）の部分も谷の底になる。このようになる理由は次のとおりである。
【００１６】
ロボットは、ツール先端の位置（各座標値）が同じであっても、その姿勢が全く異なることがある。そのため、ロボットが取り得る様々な姿勢を、複数の形態に分けて区別するということが通常行われている。例えば４軸の水平多関節型ロボットの場合、第２軸の回転角度の極性が互いに異なる２種類の形態（腕形態）が存在する。つまり、４軸の水平多関節型ロボットの姿勢は、２種類の形態に分類される。また、６軸の垂直多関節型ロボットの場合、第１軸の回転角度が互いに１８０度異なる２種類の形態（腕形態）、第３軸の回転角度の極性が互いに異なる２種類の形態（肘形態）および第４軸の回転角度が互いに１８０度異なる２種類の形態（手首形態）が存在する。つまり、それら腕形態、肘形態および手首形態の組み合わせにより合計で８種類の形態に分類される。
【００１７】
このような形態が同じであれば、ロボットの姿勢はあまり変化しない。しかし、形態が異なれば、ロボットの姿勢は大きく異なる。ロボットの姿勢が大きく異なるということは、移動時間に基づく第１評価指標の変化の傾向についても大きく異なることになる。そのため、ロボットの形態ごとに評価関数の谷が存在することになる。言い換えると、上記評価関数には、ロボットの形態が切り替わる境界である特異点を中心として、その両側に谷の部分が存在することになる。このようなことから、例えば、４軸の水平多関節型ロボットの場合、評価関数は少なくとも２つの谷を有し、６軸の垂直多関節型ロボットの場合、評価関数は少なくとも８つの谷を有することになる。
【００１８】
変数値決定処理において、最小値ではなく局所値に対応した値が目標位置における任意変数の値として設定されてしまうと、ロボットの動作時間を極力短くするという効果が若干低減する。上述したとおり、ロボットには形態という概念があり、その形態が切り替わる境界の特異点を中心として両側に評価関数の谷の部分が存在する。つまり、形態の境界である特異点が谷の境界である。そこで、本手段では、このようなロボットに特有の点を考慮し、以下のように、目標位置の任意変数の値として第１評価指標の局所値に対応した値が設定されることを避けつつ（局所値回避）、第１評価指標の最小値に対応した値が設定されるようにしている（最適化）。なお、以下では、前述した４軸の水平多関節型ロボットの場合を例にして変数値決定処理の内容を説明する。
【００１９】
すなわち、変数値決定処理では、ロボットが所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が、ロボットがとり得る全ての形態（２つの形態）のそれぞれについて取得される。なお、それら各軸の回転角度の値は、第１評価指標を最小化する値である必要はなく、任意の値でよいため、ユーザが準備することは容易である。そして、全ての形態のうち、いずれか１つの形態が、目標位置における任意変数の候補値を求める演算の演算対象として設定される。
【００２０】
設定された演算対象について取得された各軸の回転角度の値から、ツールのＺ軸まわりの回転角度の値が演算される。そして、ユーザにより指定されるツール先端の目標位置（Ｘ、Ｙ、Ｚ軸座標の値）と、演算されたツールのＺ軸まわりの回転角度（任意変数）の値（ツールの向き）とが、計算対象位置として設定される。このように、本手段における計算対象位置は、ツール先端の各軸座標値だけでなく、ツールの向きについても含むものとなっている。そして、その計算対象位置を決定するための各変数のうち、任意変数であるＺ軸まわりの回転角度を除いた変数と、計算対象位置における各軸の回転角度とに関するヤコビ行列Ｊｋが求められる。また、下記（１）式に示す第１評価指標Ｑｋが設定される。ただし、各軸移動距離を要素とする行列をεｋとし、対角成分を各軸の最大回転速度の逆数の二乗とした対角行列をλとしている。なお、εｋＴはεｋの転置行列である。
Ｑｋ＝εｋＴ・λ・εｋ      …（１）
【００２１】
上記（１）式に示す第１評価指標Ｑｋは、各軸移動距離の二乗和に対角行列λを乗じたものであり、移動時間の二乗和に対して各軸の最大回転速度に応じた重み付けが与えられたものである。すなわち、移動時間の概算値に相当する。このように設定された第１評価指標Ｑｋを、ツールの先端位置を変化させないという束縛条件（Ｊｋ・Δｋ＝０）を満たしつつ最小化することにより、下記（２）式に示す変化量Δｋが演算される。ただし、各軸の回転角度の変化量を要素とする行列をΔｋとし、計算対象位置における各軸の回転角度を要素とする行列をθｋとし、現在位置における各軸の回転角度を要素とする行列をθｃとしている。なお、ＪｋＴはＪｋの転置行列である。
Δｋ＝｛λ−１・ＪｋＴ・（Ｊｋ・λ−１・ＪｋＴ）−１・Ｊｋ−Ｉ｝（θｋ−θｃ）
…（２）
【００２２】
計算対象位置における各軸の回転角度に対して上記演算された変化量Δｋを加えた各軸の回転角度（移動後の各軸の回転角度）について順変換を行うことにより、移動後のツールの向きが演算される。そして、第１評価指標Ｑｋの変化の傾向が判断される。その結果、第１評価指標Ｑｋが所定の傾き以上の減少傾向にある場合、第１評価指標Ｑｋをさらに最小化する余地があると考えられる。その場合、目標位置および演算されたツールの向きを次の計算対象位置として設定した上で、次の移動後のツールの向きを求めるための上記各演算が再度実行される。
【００２３】
一方、上記判断の結果、第１評価指標Ｑｋが所定の傾き未満の減少傾向にある場合、または、変化がない場合、第１評価指標Ｑｋをさらに最小化する余地が少ないと考えられる。その場合、計算対象位置における各軸の回転角度に対して上記演算された変化量Δｋを加えた各軸の回転角度（移動後の各軸の回転角度）に基づいて求められるツールのＺ軸まわりの回転角度が、目標位置におけるツールのＺ軸まわりの回転角度の候補値とされる。この候補値は、移動時間の概算値が含まれた第１評価指標Ｑｋを最小化することにより決定された値である。そのため、ロボットが演算対象の形態（所定の形態）となっている状態でツール先端を目標位置まで移動する際の移動時間が極力短くなる値になっている。すなわち、第１評価指標の最小値に対応するもの、あるいは、第１評価指標の局所値に対応するもののうち、いずれかとなっている。
【００２４】
このようにして候補値が求められた後、全ての形態のうち演算対象に設定されていない形態が存在する場合、それら形態のうち、いずれか１つが次の演算対象に設定された上で、上記した候補値を求めるための各演算が再度実行される。一方、全ての形態のうち、演算対象に設定されていない形態が存在しない場合、求められた各候補値のうち、第１評価指標を最も小さくする値が、目標位置におけるツールのＺ軸まわりの回転角度（任意変数）の値として決定される。
【００２５】
このようにして決定された目標位置におけるツールのＺ軸まわりの回転角度の値は、移動時間の概算値が含まれた第１評価指標Ｑｋが最小となる値、あるいは、それに極めて近い値である。すなわち、決定された目標位置における任意変数の値は、現在位置から目標位置に移動する際におけるツール（ロボット）の移動時間が極力短くなるような値となっている。ロボット側において上記した制御が自動的に実行されるため、ユーザがティーチング時に行う作業としては、目標位置におけるツール先端の各座標値を指定するとともに、ツール（手先）のＺ軸まわりの回転角度を任意変数として指定するだけでよい。そして、値が指定された目標位置におけるツール先端のＸ、Ｙ、Ｚ軸座標の値に加え、上記したように自動的に決定された目標位置におけるツールのＺ軸まわりの回転角度（任意変数）の値に基づいて、現在位置から目標位置に向けてツールが移動されることにより、ティーチング時におけるユーザ（作業者）の作業負担を軽減しつつ、ロボットの移動時間の短縮を図ることができる。
【００２６】
なお、ここでは、４軸の水平多関節型ロボットの場合を例に説明したが、例えば６軸の垂直多関節型ロボットの場合であれば次のようになる。すなわち、ツールの向きを決定するための変数（Ｘ、Ｙ、Ｚ軸まわりの各回転角度）のうち、いずれか１つ、あるいは、いずれか２つが任意変数として指定されれば、目標位置におけるツール（手先）の特定の軸まわりの回転角度が、ロボットの移動時間が極力短くなるような値に自動的に決定される。その結果、上記した４軸の水平多関節型ロボットの場合と同様の効果が得られる。すなわち、本手段によれば、複数の軸を有するロボット全般について、上記した作用および効果が得られる。
【００２７】
また、本手段によれば、ロボットの形態ごとに第１評価指標の最小値または局所値（評価関数の谷）が存在するという点を考慮し、ロボットの形態ごとに任意変数の値が最適化され、最適化された各任意変数の値のうち、第１評価指標を最も小さくすることができる値が、目標位置における任意変数の値に設定される。これにより、目標位置における任意変数の値として第１評価指標の局所値に対応した値が設定されることを確実に防止することができる。つまり、ロボットの動作時間を極力短くするという効果が最大限に得られるように、目標位置における任意変数の値が設定されるようになっている。
【００２８】
本手段では、移動時間の概算値が含まれた二次形式の評価指標Ｑｋを用い、ヤコビ行列を用いた最適化演算を行っている。すなわち、最適化演算を比較的容易な形式で行うことが可能となっている。そのため、任意変数の値を決定するための演算処理を比較的高速に行うことができる。
【００２９】
請求項２または４に記載の手段によれば、変数値決定処理では、ロボットが所定の形態をとる各軸の回転角度が与えられることにより、ロボットが上記所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が求められる。すなわち、ロボットが所定の形態をとる各軸の回転角度の値が、ロボットがとり得る全ての形態のそれぞれについて与えられると、それら全ての形態のうちいずれか１つが、目標位置にツール先端が到達する各軸の回転角度を取得するための演算の演算対象として設定される。なお、上記与えられる各軸の回転角度の値は、第１評価指標を最小化する値である必要はなく、また、ツール先端が目標位置に到達する値である必要もなく、任意の値でよい。
【００３０】
そして、演算対象に設定された形態について与えられた各軸の回転角度の値からツールの先端位置を求め、そのツールの先端位置が計算対象位置として設定される。続いて、その計算対象位置におけるツールの先端位置および向きを決定するための変数のうち任意変数に指定された変数を除いた変数と、計算対象位置における各軸（各関節）の回転角度とに関するヤコビ行列Ｊｋが求められる。また、下記（３）式に示す第２評価指標Ｑｋが設定される。ただし、移動後のツールの先端位置の目標位置に対する位置誤差の座標値を要素とする行列をεｋとしている。なお、εｋＴはεｋの転置行列であり、ΔｋＴはΔｋの転置行列である。
Ｑｋ＝εｋＴ・εｋ＋ΔｋＴ・λ・Δｋ                  …（３）
【００３１】
上記（３）式において、右辺第一項は位置誤差の二乗和である。また、右辺第二項は各軸の回転角度の変化量（移動量）の二乗和に対角行列λを乗じたものであり、移動時間の二乗和に対して各軸の最大回転速度に応じた重み付けが与えられたものである。すなわち、移動時間の概算値に相当する。このように設定された第２評価指標Ｑｋを最小化することにより、下記（４）式に示す変化量Δｋが演算される。ただし、目標位置の座標値を要素とする行列をＰｄとし、計算対象位置の座標値を要素とする行列をＰｋとしている。
Δｋ＝（λ＋ＪｋＴ・Ｊｋ）−１・ＪｋＴ（Ｐｄ−Ｐｋ）      …（４）
【００３２】
計算対象位置における各軸の回転角度に対して上記演算された変化量Δｋを加えた各軸の回転角度、つまり移動後の各軸の回転角度について順変換を行うことにより、移動後のツールの先端位置が演算される。そして、演算された移動後のツールの先端位置の目標位置に対する位置誤差が求められ、その位置誤差が所定の許容誤差以下であるか否かが判断される。つまり、演算により求められた移動後のツールの先端位置が十分に目標位置に近づいているか否かが判断される。その結果、位置誤差が許容誤差を超える場合、演算した移動後のツールの先端位置を次の計算対象位置とした上で、次の移動後のツールの先端位置を求めるための上記各演算が再度実行される。
【００３３】
一方、上記判断の結果、位置誤差が許容誤差以下である場合、計算対象位置における各軸の回転角度に対して上記演算された変化量Δｋを加えた各軸の回転角度（移動後の各軸の回転角度）の値を、ロボットが演算対象の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値として取得する。すなわち、上記演算により求められた移動後のツールの先端位置が十分に目標位置に近づいていると判断された場合、そのときの各軸の回転角度の値が、ロボットが演算対象の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値として取得される。このように各軸の回転角度の値が取得された後、演算対象に設定されていない形態が存在しなくなるまで、それら形態のうち、いずれか１つを次の演算対象として設定した上で、各軸の回転角度を取得するための上記各演算が再度実行される。その結果、ロボットが所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が、ロボットがとり得る全ての形態のそれぞれについて取得されることになる。
【００３４】
上記した各演算では、ヤコビ行列を用いている。ヤコビ行列は、各軸（各関節）における微小変位と、それに伴う手先の位置および向き（姿勢）の微小変位との比率を表すものである。このようなヤコビ行列を用いるとともに、移動時間の概算値が含まれた第２評価指標Ｑｋを最小化する演算が繰り返される結果、その演算により求められる移動後のツールの先端位置が徐々に目標位置に近づいていくことになる。そのため、最終的に取得される各軸の回転角度に基づく目標位置におけるロボットの姿勢は、最初に与えられた各軸の回転角度に基づくロボットの姿勢から大きく変化することがない。すなわち、目標位置におけるロボットの形態は、当初の形態と同じになる。このように、本手段によれば、ロボットが所定の形態をとる各軸の回転角度が与えられるだけで、ロボットが上記所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値を求めることが可能となる。上記与えられる各軸の回転角度の値は、ロボットが所定の形態をとる値であれば任意の値でよい。そのため、ツール先端の目標位置にかかわらず、そのロボットがとり得る形態ごとに１種類ずつの値が予め準備されていればよいことになり、このような面からもユーザ（作業者）の作業負担の軽減を図ることができる。
【図面の簡単な説明】
【００３５】
【図１】本発明の第１の実施形態を示すもので、ロボットシステムの概略構成図
【図２】評価指標を任意変数の関数として表す図
【図３】４軸の水平多関節型ロボットの各形態を示す図
【図４】変数値決定処理の内容を示すフローチャート
【図５】現在位置および目標位置におけるロボットの態様を模式的に表す図
【図６】目標領域内のいずれかの位置に目標位置が設定される場合の図５相当図
【図７】本発明の第２の実施形態を示すもので、図４のステップＳ１に相当するフローチャート
【図８】本発明の第３の実施形態を示す図１相当図
【図９】６軸の垂直多関節型ロボットの腕形態を示す図
【図１０】６軸の垂直多関節型ロボットの肘形態を示す図
【図１１】６軸の垂直多関節型ロボットの手首形態を示す図
【図１２】本発明の第４の実施形態を示すもので、図４のステップＳ１に相当するフローチャート
【図１３】４軸の水平多関節型ロボットを模式的に表す図
【図１４】図１３相当図
【図１５】従来技術を示す図５相当図
【発明を実施するための形態】
【００３６】
（第１の実施形態）
以下、本発明の第１の実施形態について図１〜図６を参照しながら説明する。
図１は、一般的な産業用ロボットのシステム構成を示している。図１に示すロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。
【００３７】
ロボット２は、例えば４軸の水平多関節型ロボットとして構成されている。ロボット２は、設置面に固定されるベース５と、ベース５上にＺ軸（垂直軸）方向の軸心を持つ第１軸Ｊ１を中心に回転可能に連結された第１のアーム６と、第１のアーム６の先端部上にＺ軸方向の軸心を持つ第２軸Ｊ２を中心に回転可能に連結された第２のアーム７と、第２のアーム７の先端部に上下動可能で且つ回転可能に設けられたシャフト８とから構成されている。シャフト８を上下動させる際の軸が第３軸Ｊ３であり、回転させる際の軸が第４軸Ｊ４である。シャフト８の先端部（下端部）には、フランジ９が位置決めされて着脱可能に取り付けられる。
【００３８】
ベース５、第１のアーム６、第２のアーム７、シャフト８およびフランジ９は、ロボット２のアームとして機能する。アーム先端であるフランジ９（手先に相当）には、図示はしないが、例えばエアチャックなどのツールが取り付けられる。ロボット２に設けられる複数の軸（Ｊ１〜Ｊ４）はそれぞれに対応して設けられるモータ（図示せず）により駆動される。各モータの近傍には、それぞれの回転軸の回転角度（回転位置）を検出するための位置検出器（図示せず）が設けられている。
【００３９】
一般に、産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作するようになっている。コントローラ３（ロボットの制御装置に相当）は、その動作プログラムに基づいてモータの駆動をフィードバック制御し、ロボット２のアームの動作制御を行う。
【００４０】
ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。
【００４１】
一般に、ロボット２の手先であるフランジ９に取り付けられるツールの先端を、現在位置から目標位置に向けて移動させる動作のティーチング作業が行われる際、目標位置におけるロボット２の姿勢をどのようなものにするかはユーザにより決定される。すなわち、このようなティーチング作業の際、ツール先端の目標位置を決定するための変数（ツール先端のＸ軸座標値、Ｙ軸座標値およびＺ軸座標値）と、目標位置におけるツール（手先）の向きを決定するための変数（ツールのＺ軸まわりの回転角度）とは、全てユーザにより指定される。ただし、フランジ９に取り付けられるツールの種類（形状）およびそのツールを用いた作業の内容によっては、ツールの向きを不問とすることが可能である。例えば、ツールとしてエアチャックが用いられる場合、ツールのＺ軸（第４軸Ｊ４）まわりの向きがどのような向きであっても、ワークを把持することができる。
【００４２】
本実施形態のロボットシステム１では、上記したツールの向きを不問とすることが可能なケースにおいて、ユーザは、ツール先端の目標位置を示すＸ、Ｙ、Ｚ軸座標の値を指定するだけでよく、目標位置におけるツールのＺ軸まわりの回転角度Ｒｚの値を指定しなくてよい。その代わりに、ユーザは、ツールのＺ軸まわりの回転角度Ｒｚを任意変数として指定する。これを受けて、コントローラ３は、回転角度Ｒｚを、コントローラ３側で任意の値に設定可能な任意変数として設定する（任意変数設定処理）。また、詳細は後述するが、コントローラ３は、現在位置における各軸の回転角度を基準とした各軸の回転角度の変化量を示す各軸移動距離に基づく第１評価指標を設定し、ツールの先端位置を変化させないという束縛条件の下で、その第１評価指標を最適化することで任意変数の値を決定する（変数値決定処理）。このように、コントローラ３は、任意変数設定処理手段および変数値決定処理手段として機能する。本実施形態では、コントローラ３により上記各処理が実行されることにより、目標位置におけるツールの向きが、ロボット２の移動時間が極力短くなるようなものに決定される。
【００４３】
図２は、目標位置におけるツールのＺ軸まわりの回転角度Ｒｚ（任意変数）が取り得る全ての値について評価指標を演算し、任意変数の関数（以下では評価関数と言う）として表した（グラフ化した）ものである。この図２に示すように、評価関数は、２つの谷を有しており、評価指標の最小値（最適値）の部分が一方の谷の底に位置し、評価指標の最小値ではない他の値（最小値に準ずる値であり、以下では局所値と言う）の部分が他方の谷の底に位置している。評価関数にこのような２つの谷が存在する理由は、次のとおりである。
【００４４】
図３は、４軸の水平多関節型ロボットの各形態を示している。なお、図３では、各軸および各リンクをそれぞれ丸印および直線で示すとともに、ツールの図示を省略している。ロボットは、ツール先端の位置（Ｘ、Ｙ、Ｚ軸座標値）が同じであっても、その姿勢が全く異なることがある。そのため、通常、ロボットが取り得る様々な姿勢を、複数の形態に分けて区別するということが行われている。例えば、本実施形態のロボット２（４軸の水平多関節型ロボット）の場合、２つの形態に分類される。すなわち、第２軸Ｊ２の回転角度がプラス（＋）になる腕形態（図３（ａ）参照）と、第２軸Ｊ２の回転角度がマイナス（−）になる腕形態（図３（ｂ）参照）とに分類される。このような形態が同じであれば、ロボット２の姿勢はあまり変化しない。しかし、形態が異なれば、ロボット２の姿勢は大きく異なる。ロボット２の姿勢が大きく異なるということは、移動時間に基づく評価指標の変化の傾向についても大きく異なることになる。そのため、ロボット２の形態ごとに前述した評価関数の谷が存在することになる。言い換えると、上記評価関数には、ロボット２の形態が切り替わる境界である特異点を中心として、その両側に谷の部分が存在することになる。このようなことから、本実施形態のロボット２の場合、評価関数は少なくとも２つの谷を有することになる。
【００４５】
変数値決定処理において、最小値ではなく局所値が目標位置における任意変数の値として設定されてしまうと、ロボット２の動作時間を極力短くするという効果が若干低減する。上述したとおり、ロボットには形態という概念があり、その形態が切り替わる境界（特異点）を中心として両側に評価関数の谷の部分が存在する。つまり、形態の境界である特異点が谷の境界である。そこで、本実施形態では、このようなロボットに特有の点を考慮し、以下のように、目標位置の任意変数の値として第１評価指標の局所値に対応した値が設定されることを避けつつ（局所値回避）、第１評価指標の最小値（最適値）に対応した値が設定されるようにしている（最適化）。
【００４６】
図４は、変数値決定処理の内容を示すフローチャートである。図４に示すように、変数値決定処理が開始されると（スタート）、ロボット２が所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が、ロボット２がとり得る全ての形態（図３に示した２つの腕形態）のそれぞれについて取得される（ステップＳ１）。なお、それら各軸の回転角度の値は、ユーザにより予め準備されているものとする。ただし、準備する各軸の回転角度の値は、第１評価指標を最小化する値である必要はなく、上記各条件を満たすものであれば任意の値でよい。そのため、上記値をユーザが準備することは比較的容易である。
【００４７】
ステップＳ２では、全ての形態のうち、いずれか１つの形態が、目標位置におけるツールのＺ軸まわりの回転角度Ｒｚ（任意変数）の候補値を求める演算の演算対象として設定される。ステップＳ３では、設定された演算対象について取得された各軸の回転角度の値から、ツールのＺ軸まわりの回転角度Ｒｚの値が演算される。そして、ステップＳ４では、ユーザにより指定されるツール先端の目標位置（Ｘ、Ｙ、Ｚ軸座標値）が計算対象位置として設定される。また、その計算対象位置におけるツールのＺ軸まわりの回転角度Ｒｚ（ツールの向き）は、ステップＳ３で演算された値に設定される。このように、変数値変更処理における計算対象位置は、ツール先端の各軸座標値だけでなく、ツールの向きについても含むものとなっている。
【００４８】
ステップＳ５では、計算対象位置（ツールの向きも含む）を決定するための各変数のうち、任意変数であるＺ軸まわりの回転角度Ｒｚを除いた変数と、計算対象位置における各軸（各関節）の回転角度とに関するヤコビ行列Ｊｋが求められる。そのヤコビ行列Ｊｋは、下記（５）式のように表される。ただし、Ｘ、Ｙ、Ｚ軸座標値をそれぞれＸｋ、Ｙｋ、Ｚｋとし、第１軸Ｊ１〜第４軸Ｊ４のそれぞれの回転角度をθ１ｋ〜θ４ｋとしている。
【００４９】
【数１】
0005803178.tif 000002
【００５０】
ステップＳ６では、下記（６）式に示す第１評価指標Ｑｋが設定される。ただし、各軸移動距離を要素とする行列をεｋとし、対角成分が各軸Ｊ１〜Ｊ４のそれぞれの最大回転速度の逆数の二乗である対角行列をλとしている。また、εｋＴはεｋの転置行列を示している。なお、λは、下記（７）式により表される。ただし、第１軸Ｊ１〜第４軸Ｊ４のそれぞれについての最大回転速度をＶ１ｍａｘ〜Ｖ４ｍａｘとしている。
Ｑｋ＝εｋＴ・λ・εｋ      …（６）
【００５１】
【数２】
0005803178.tif 000003
【００５２】
上記（６）式に示す第１評価指標Ｑｋは、各軸移動距離の二乗和に対角行列λを乗じたものであり、移動時間の二乗和に対して各軸の最大回転速度に応じた重み付けが与えられたものである。すなわち、移動時間の概算値（近似値）に相当する。ステップＳ７では、このように設定された第１評価指標Ｑｋを、下記（８）式に示す束縛条件ｇｋを満たしつつ、最小化することにより、下記（９）式に示す変化量Δｋが演算される。ただし、各軸Ｊ１〜Ｊ４の各回転角度θ１〜θ４の変化量を要素とする行列をΔｋとし、計算対象位置における各軸Ｊ１〜Ｊ４の回転角度を要素とする行列をθｋとし、現在位置における各軸Ｊ１〜Ｊ４の回転角度を要素とする行列をθｃとしている。なお、ＪｋＴはＪｋの転置行列である。また、上記演算の手法としては、例えばラグランジュの未定乗数法などを用いることができる。
Ｊｋ・Δｋ＝０                                                      …（８）
Δｋ＝｛λ−１・ＪｋＴ・（Ｊｋ・λ−１・ＪｋＴ）−１・Ｊｋ−Ｉ｝（θｋ−θｃ）
…（９）
【００５３】
ステップＳ８では、下記（１０）式に示すように、移動後の各軸の回転角度が求められる。ただし、移動後の各軸Ｊ１〜Ｊ４の回転角度を要素とする行列をθｋ＋１としている。また、定数γは目標位置への収束速度を調整するための定数を示している。なお、本実施形態では、定数γの値を１としているが、目標位置への収束速度を早くする場合には定数γの値を大きくすればよいし、収束速度を遅くする場合には定数γの値を小さくすればよい。
θｋ＋１＝θｋ＋γ・Δｋ            …（１０）
【００５４】
ステップＳ９では、下記（１１）式に示すように、移動後のツールの先端位置および向きが求められる。すなわち、移動後の各軸の回転角度を要素とするθｋ＋１について順変換を行うことにより、移動後のツールの先端位置のＸ、Ｙ、Ｚ軸座標値およびＺ軸まわりの回転角度Ｒｚを要素とする行列Ｐｋ＋１が求められる。ただし、ツールのオフセット長や各アームのリンク長などを考慮した上で各軸の回転角度からツール先端位置を求める関数をＪ２Ｐとして表している。
Ｐｋ＋１＝Ｊ２Ｐ（θｋ＋１）            …（１１）
【００５５】
ステップＳ１０、Ｓ１１では、今回の最適化演算（ステップＳ６、Ｓ７）により最適化された第１評価指標Ｑｋの値が、前回の最適化演算により最適化された第１評価指標Ｑｋ−１からどのように変化したのかが判断される。すなわち、ステップＳ１０、Ｓ１１は、評価指標の変化の傾向を判断するものである。従って、変数値変更処理が実行されてから最初にステップＳ１０に移行した際には、それらステップＳ１０、Ｓ１１をスキップしてステップＳ１２に移行する。２回目以降に実行されるステップＳ１０では、今回の第１評価指標Ｑｋの値が、前回の第１評価指標Ｑｋ−１の値から減少したか否かが判断される。
【００５６】
その結果、減少したと判断された場合（ステップＳ１０で「ＹＥＳ」）、ステップＳ１１に進む。ステップＳ１１では、第１評価指標の減少量が所定のしきい値以上であるか否かが判断される。すなわち、ステップＳ１１では、第１評価指標が所定の傾き以上の減少傾向にあるか否かが判断される。その結果、第１評価指標の減少量が所定のしきい値以上である、つまり第１評価指標が所定の傾き以上の減少傾向にあると判断された場合（ステップＳ１１で「ＹＥＳ」）、ステップＳ１２に進む。このような場合、第１評価指標をさらに最小化する余地が大きいと考えられる。
【００５７】
ステップＳ１２では、次のように計算対象位置が再設定される。すなわち、ステップＳ１２では、ツール先端のＸ、Ｙ、Ｚ軸座標値は目標位置のままとし、ステップＳ９で演算された移動後のツールの向きが、次の計算対象位置におけるツールの向きとして設定される。このように次の計算対象位置が設定された後、ステップＳ５に戻り、次の移動後のツールの向きを求めるための各演算（ステップＳ５〜Ｓ１１）が再度実行される。
【００５８】
一方、ステップＳ１０にて、第１評価指標が減少していないと判断された場合（ＮＯ）、または、ステップＳ１１にて、第１評価指標の減少量が所定のしきい値未満である、つまり第１評価指標が所定の傾き未満の減少傾向にあると判断された場合（ＮＯ）、ステップＳ１３に進む。これらの場合、第１評価指標をさらに最小化する余地が少ないと考えられる。なお、ステップＳ１０にて、第１評価指標が減少していないと判断された場合、第１評価指標が変化していないと考えられる。なぜなら、ここでは第１評価指標を最小化する演算を行っているため、第１評価指標が増加することはほとんど考えられない。従って、第１評価指標が減少していないということは、すなわち第１評価指標が変化していないということになる。
【００５９】
ステップＳ１３では、上記（１０）式に示した移動後の各軸Ｊ１〜Ｊ４の回転角度θｋ＋１に基づいて求められるツールのＺ軸まわりの回転角度が、目標位置における任意変数の候補値とされる。この候補値は、移動時間の概算値が含まれた第１評価指標Ｑｋを最小化することにより決定された値である。そのため、ロボット２が演算対象の形態（所定の形態）となっている状態で、ツール先端を目標位置まで移動する際の移動時間が極力短くなる値になっている。すなわち、第１評価指標の最小値に対応するもの、あるいは、第１評価指標の局所値に対応するもののうち、いずれかとなっている。
【００６０】
ステップＳ１４では、全ての形態のうち未だ演算対象に設定されていない形態が存在するか否かが判断される。その判断の結果、演算対象に設定されていない形態が存在する場合（ＹＥＳ）、ステップＳ１５に進む。ステップＳ１５では、未だ演算対象に設定されていない形態のうちいずれか１つが次の演算対象に設定される。このように次の演算対象が設定された上で、ステップＳ３に戻り、次の候補値を求めるための各演算（ステップＳ３〜Ｓ１３）が再度実行される。一方、ステップＳ１４による判断の結果、演算対象に設定されていない形態が存在しない場合（ＮＯ）、ステップＳ１６に進む。ステップＳ１６では、求められた各候補値のうち、第１評価指標を最も小さくする値が、目標位置におけるＺ軸まわりの回転角度Ｒｚ（任意変数）の値として決定され、処理が終了する（エンド）。
【００６１】
このようにして決定された目標位置におけるツールのＺ軸まわりの回転角度Ｒｚの値は、移動時間の概算値が含まれた第１評価指標Ｑｋが最小となる値、あるいは、それに極めて近い値である。すなわち、決定された目標位置における任意変数の値は、現在位置から目標位置に移動する際におけるツール（ロボット２）の移動時間が極力短くなるような値となっている。
【００６２】
コントローラ３により上記した制御が自動的に実行されるため、ユーザがティーチング時に行う作業としては、前述したように目標位置におけるツール先端の各座標値を指定するとともに、ツールのＺ軸まわりの回転角度Ｒｚを任意変数として指定するだけでよい。そして、コントローラ３は、値が指定された目標位置におけるツール先端のＸ、Ｙ、Ｚ軸座標の値に加え、上記した各処理により自動的に決定された目標位置におけるツールのＺ軸まわりの回転角度Ｒｚの値（ツールの向き）に基づいて、ツール先端を現在位置から目標位置に向けて移動させることができる。これにより、ティーチング時におけるユーザ（作業者）の作業負担が軽減されるとともに、ロボット２の移動時間が短縮されることになる。
【００６３】
また、本実施形態では、ロボットの形態ごとに第１評価指標の最小値または局所値（評価関数の谷）が存在するという点を考慮し、ロボット２の形態ごとに任意変数の値を最適化し、それら最適化された任意変数の値のうち、第１評価指標を最も小さくすることができる値を、目標位置における任意変数の値に設定する。これにより、目標位置における任意変数の値として第１評価指標の局所値に対応した値が設定されることを確実に防止することができる。つまり、ロボット２の動作時間を極力短くするという効果が最大限に得られるように、目標位置における任意変数の値が設定されるようになっている。また、変数値決定処理においては、移動時間の概算値が含まれた二次形式の評価指標を用い、ヤコビ行列を用いた最適化演算を行っている。そのため、変数値決定処理における各種演算処理を比較的高速に行うことが可能となっている。
【００６４】
次に、本実施形態のロボットシステム１により得られる具体的な効果について、図５および図６も参照して説明する。
図５は、ツール先端を現在位置Ｐｃから目標位置Ｐｄに向けて移動させる際におけるロボットの態様を模式的に示している。図５に示すロボットは、４軸の水平多関節型ロボットであり、その手先にツールとしてエアチャックが取り付けられている。なお、図５では、各軸および各リンクをそれぞれ丸印および直線で示すとともに、ツール先端を矢印で示している。また、現在位置におけるロボットの状態を点線で示すとともに、目標位置におけるロボットの状態を実線で示している。ここでは、現在位置Ｐｃから目標位置Ｐｄまでツール先端を移動させ、ツールであるエアチャックによりワークを把持する作業が行われるものとする。
【００６５】
上記ケースにおけるティーチング作業は、従来の手法によれば次のように行われる。すなわち、ティーチングを担当する作業者は、まず、図５の（ａ）、（ｂ）、（ｃ）のそれぞれに示す状態（姿勢）について検討する。つまり、作業者は、ツールの向きを大まかに変えてみて、どの向きが良さそうであるかを推測する。その結果、（ｃ）の状態が良さそうであると判断した場合、（ｃ）の状態をベースにし、その（ｃ）状態からツールの向きを少しずつ変化させたもの（図５の（ｄ）、（ｅ）、（ｆ）のそれぞれに示す状態）について検討する。その結果、図５（ｆ）の状態が最適である（移動時間が短くなる）という判断に至る。なお、ここでは、説明を簡略化するため、作業者が検討する状態の数を少なくするとともに、少ない繰り返し作業で最適な状態が見つかるケースとしている。実際には、さらに多くの状態について検討したり、繰り返し回数が増えたりすることもある。
【００６６】
これに対し、本実施形態のロボットシステム１によれば、目標位置Ｐｄにおけるツールの向きは、コントローラ３により、ロボット２の移動時間が極力短くなるような向きに自動的に決定される。従って、従来の手法のような繰り返しの過程を経ることなく、図５の（ｆ）の状態（ツールの向き）が最適である（移動時間を極力短くできる）という結果が容易に得られる。
【００６７】
このように、従来の手法によれば、ティーチングを担当する作業者の経験と推測により、少しずつ目標位置におけるロボットの状態（姿勢）を変化させる。そして、その作業を何回（例えば５回）か繰り返した結果、例えば、現在位置および目標位置におけるツールの向きが互いに同じロボットの姿勢（図５の（ａ））に比べ、ロボットの動作時間が短くなる（例えば２５％程度の改善）ツールの向きを得ることが可能となっていた。これに対し、本実施形態のロボットシステム１によれば、ティーチングを担当する作業者の経験などに関係なく、従来の手法による効果と同程度の効果を得ることができる。さらに、その際、作業者は目標位置におけるロボットの姿勢について具体的に検討する必要がないため、その作業負担が大きく軽減されることになる。
【００６８】
図６は、ツール先端を現在位置Ｐｃから目標領域Ａｄ内の任意の目標位置に向けて移動させる際における図５相当図である。この場合、図６の（ａ）に示す目標領域Ａｄ内におけるどの位置に移動すればよいかについては実作業時にカメラなどの外部機器から与えられ、ティーチングの時点において作業者は知ることができないものとする。このようなケースでは、目標領域Ａｄ内のどの位置に目標位置が設定されるかによってツールの最適な向きが変化する。そのため、従来の手法では、図６の（ｂ）に示すように、目標領域Ａｄを複数の小領域に分割し、それら小領域ごとに最適なツールの向きを教示するなど、非常に手間がかかっていた。また、どのように目標領域Ａｄを分割するかによって、最適なツールの向きが得られるかどうかが変化するため、ティーチングを担当する作業者の個人能力に大きく依存する部分があった。
【００６９】
これに対し、本実施形態のロボットシステム１によれば、実作業時に初めてカメラなどの外部機器から目標位置が与えられるケースであっても、コントローラ３は、与えられた目標位置に基づいて、その目標位置における最適なツールの向きを自動的に決定する。そのため、従来の手法のように目標領域Ａｄを小領域に分割し、それら小領域ごとにティーチングをするといった手間のかかる作業を省くことが可能となる。
【００７０】
（第２の実施形態）
以下、第１の実施形態に対し、変数値決定処理の内容を変更した第２の実施形態について図７を参照しながら説明する。
本実施形態では、ロボット２が所定の形態をとる各軸の回転角度が与えられることにより、ロボット２が上記形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が求められる。すなわち、変数値決定処理において、図４のステップＳ１に代えて図７のフローチャートに示す内容の処理が実行される。
【００７１】
図７の処理が開始されると（スタート）、ロボット２が所定の形態をとる各軸の回転角度の値が、ロボット２がとり得る全ての形態（２種類の腕形態）のそれぞれについて取得される（ステップＴ１）。なお、それら各軸の回転角度の値は、ユーザにより予め準備されているものとする。ただし、準備する各軸の回転角度の値は、第１評価指標を最小化する値である必要はなく、また、ツール先端が目標位置に到達する値である必要もなく、上記各条件を満たすものであれば任意の値でよい。そのため、上記値をユーザが準備することは非常に容易である。
【００７２】
ステップＴ２では、全ての形態のうち、いずれか１つの形態が、目標位置にツール先端が到達する各軸の回転角度を取得するための演算の演算対象として設定される。ステップＴ３では、演算対象に設定された形態について与えられた各軸の回転角度の値からツールの先端位置が求められる。ステップＴ４では、ステップＴ３で求められたツールの先端位置が計算対象位置として設定される。
【００７３】
ステップＴ５では、計算対象位置におけるツールの先端位置および向きを決定するための変数（Ｘ、Ｙ、Ｚ軸座標値および回転角度Ｒｚ）のうち、任意変数に指定された回転角度Ｒｚを除いた変数と、計算対象位置における各軸（関節）の回転角度とに関するヤコビ行列Ｊｋが求められる。そのヤコビ行列Ｊｋは、上記（５）式のように表される。
【００７４】
ステップＴ６では、下記（１２）式に示す第２評価指標Ｑｋが設定される。ただし、移動後のツールの先端位置の目標位置に対する位置誤差の座標値を要素とする行列をεｋとし、位置誤差および変化量の重み付けのための定数をβとしている。また、εｋＴはεｋの転置行列を示し、ΔｋＴはΔｋの転置行列を示している。なお、εｋは、下記（１３）式により表される。ただし、目標位置のＸ、Ｙ、Ｚ軸座標値を要素とする行列をＰｄとし、計算対象位置のＸ、Ｙ、Ｚ軸座標値を要素とする行列をＰｋとしている。
Ｑｋ＝εｋＴ・εｋ＋β・ΔｋＴ・λ・Δｋ      …（１２）
εｋ＝Ｐｄ−Ｐｋ−Ｊｋ・Δｋ                  …（１３）
【００７５】
上記（１２）式において、右辺第一項は位置誤差の二乗和である。また、右辺第二項は各軸Ｊ１〜Ｊ４の回転角度の変化量（移動量）の二乗和に対角行列λを乗じたものであり、移動時間の概算値（近似値）に相当する。定数βは、ロボット２の各関節の可動範囲と動作速度に比例する。従って、定数βは、ロボット２の機種ごとに適宜設定すればよい。なお、第２評価指標Ｑｋにおける位置誤差および移動時間の概算値の重み付けが均等であれば、定数βを省くことができる。ステップＴ７では、このように設定された第２評価指標Ｑｋが最小化されることにより、下記（１４）式に示す変化量Δｋが演算される。
Δｋ＝（β・λ＋ＪｋＴ・Ｊｋ）−１・ＪｋＴ（Ｐｄ−Ｐｋ）      …（１４）
【００７６】
ステップＴ８では、第１の実施形態におけるステップＳ８と同様に、上記（１０）式に示した移動後の各軸の回転角度が求められる。ステップＴ９では、下記（１５）式に示すように、移動後のツールの先端位置が求められる。すなわち、移動後の各軸の回転角度を要素とする行列θｋ＋１について順変換を行うことにより、移動後のツールの先端位置のＸ、Ｙ、Ｚ軸座標値を要素とする行列Ｐｋ＋１が求められる。ただし、ツールのオフセット長や各アームのリンク長などを考慮した上で各軸の回転角度からツール先端位置を求める関数をＪ２Ｐとして表している。
Ｐｋ＋１＝Ｊ２Ｐ（θｋ＋１）            …（１５）
【００７７】
ステップＴ１０では、演算により求められた移動後のツールの先端位置Ｐｋ＋１の目標位置Ｐｄに対する位置誤差が求められる。ステップＴ１１では、ステップＴ１０で求められた位置誤差が所定の許容誤差以下であるか否かが判断される。つまり、演算により求められた移動後のツールの先端位置Ｐｋ＋１が十分に目標位置Ｐｄに近づいているか否かが判断される。その結果、位置誤差が許容誤差を超える場合（ＮＯ）、ステップＴ１２に進む。ステップＴ１２では、移動後のツールの先端位置Ｐｋ＋１が次の計算位置に設定される。このように次の計算位置が設定された上で、ステップＴ５に戻り、次の移動後のツールの先端位置を求めるための各演算（ステップＴ５〜Ｔ１１）が再度実行される。なお、上記許容誤差は、例えば、ロボット２における位置制御の精度に基づいて適宜設定すればよい。
【００７８】
一方、ステップＴ１１による判断の結果、位置誤差が許容誤差以下である場合（ＹＥＳ）、ステップＴ１３に進む。ステップＴ１３では、上記（１０）式に示した移動後の各軸Ｊ１〜Ｊ４の回転角度θｋ＋１の値が、ロボット２が演算対象の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値として取得される。そして、ステップＴ１４では、全ての形態のうち未だ演算対象に設定されていない形態が存在するか否かが判断される。その判断の結果、演算対象に設定されていない形態が存在する場合（ＹＥＳ）、ステップＴ１５に進む。ステップＴ１５では、未だ演算対象に設定されていない形態のうちいずれか１つが次の演算対象に設定される。このように次の演算対象が設定された上で、ステップＴ３に戻り、各軸の回転角度を取得するための各演算（ステップＴ３〜Ｔ１３）が再度実行される。一方、ステップＴ１４による判断の結果、演算対象に設定されていない形態が存在しない場合（ＮＯ）、処理が終了する（エンド）。このような処理が実行される結果、ロボット２が所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が、ロボット２がとり得る全ての形態のそれぞれについて取得されることになる。
【００７９】
上記した各演算では、ヤコビ行列を用いている。ヤコビ行列は、各軸（各関節）における微小変位と、それに伴う手先の位置および向き（姿勢）の微小変位との比率を表すものである。このようなヤコビ行列を用いるとともに、移動時間の概算値が含まれた第２評価指標を最小化する演算が繰り返される結果、その演算により求められる移動後のツールの先端位置が徐々に目標位置に近づいていくことになる。そのため、最終的に取得される各軸の回転角度に基づく目標位置におけるロボット２の姿勢は、最初に与えられた各軸の回転角度に基づくロボット２の姿勢から大きく変化することがない。すなわち、目標位置におけるロボット２の形態は、当初の形態と同じになる。
【００８０】
このように、本実施形態によれば、ロボット２が所定の形態をとる各軸の回転角度が与えられるだけで、ロボット２が上記所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値を求めることが可能になる。上記与えられる各軸の回転角度の値は、ロボット２が所定の形態をとる値であれば任意の値でよい。そのため、ツール先端の目標位置にかかわらず、そのロボット２がとり得る形態ごとに１種類ずつの値が予め準備されていればよいことになり、このような面からもユーザ（作業者）の作業負担の軽減を図ることができる。
【００８１】
（第３の実施形態）
以下、上記各実施形態に対し、対象とするロボットの種類を変更した第３の実施形態について図８〜図１１を参照しながら説明する。
図８は、第１の実施形態における図１相当図であり、第１の実施形態と同一部分には同一符号を付して説明を省略する。図８に示す本実施形態のロボットシステム２１は、図１に示した第１の実施形態のロボットシステム１に対し、ロボット２に代えてロボット２２を備えている点が異なる。
【００８２】
ロボット２２は、例えば６軸の垂直多関節型ロボットとして構成されている。すなわち、ベース２５上には、Ｚ軸方向の軸心を持つ第１軸Ｊ２１を介してショルダ部２６が水平方向に回転可能に連結されている。ショルダ部２６には、Ｙ軸方向の軸心を持つ第２軸Ｊ２２を介して上方に延びる下アーム２７の下端部が垂直方向に回転可能に連結されている。下アーム２７の先端部には、Ｙ軸方向の軸心を持つ第３軸Ｊ２３を介して第１の上アーム２８が垂直方向に回転可能に連結されている。第１の上アーム２８の先端部には、Ｘ軸方向の軸心を持つ第４軸Ｊ２４を介して第２の上アーム２９が捻り回転可能に連結されている。第２の上アーム２９の先端部には、Ｙ軸方向の軸心を持つ第５軸Ｊ２５を介して手首３０が垂直方向に回転可能に連結されている。手首３０には、Ｘ軸方向の軸心を持つ第６軸Ｊ２６を介してフランジ３１が捻り回転可能に連結されている。
【００８３】
ベース２５、ショルダ部２６、下アーム２７、第１の上アーム２８、第２の上アーム２９、手首３０およびフランジ３１は、ロボット２２のアームとして機能する。アーム先端であるフランジ３１（手先に相当）には、図示はしないが、例えばエアチャックなどのツールが取り付けられる。ロボット２２に設けられる複数の軸（Ｊ２１〜Ｊ２６）は、第１の実施形態のロボット２と同様、それぞれに対応して設けられるモータ（図示せず）により駆動される。また、各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器（図示せず）が設けられている。
【００８４】
図９〜図１１は、第１の実施形態における図３相当図であり、それぞれ６軸の垂直多関節型ロボットの腕形態、肘形態および手首形態を示している。図９（ａ）は正面から目標位置にあるワークなどを取りに行くケースであり、図９（ｂ）は背中越しにワークなどを取りに行くケースである。つまり、図９（ａ）および図９（ｂ）では、点線で示した部分の面が、上下入れ替わる。このように、第１軸Ｊ２１の回転角度が互いに１８０度異なる２種類の腕形態が存在する。図１０（ａ）は第３軸Ｊ２３が上方に存在するケースであり、図１０（ｂ）は第３軸Ｊ２３が下方に存在するケースである。なお、ここで言う上下は、Ｚ軸方向の位置を示している。このように、第３軸Ｊ２３の回転角度の極性が互いに異なる２種類の肘形態が存在する。図１１（ａ）および図１１（ｂ）は、点線で示した部分の面が、上下入れ替わる。このように、第４軸Ｊ２４の回転角度が互いに１８０度異なる２種類の手首形態が存在する。
【００８５】
本実施形態のロボット２２（６軸の垂直多関節型ロボット）の場合、腕形態、肘形態および手首形態がそれぞれ２種類ずつ存在するため、それらの組み合わせにより合計で８つの形態に分類される。このようなことから、本実施形態のロボット２２の場合、評価関数は少なくとも８つの谷を有することになる。このような本実施形態の構成においても、コントローラ３は、上記各実施形態と同様の任意変数設定処理および変数値決定処理を実行することが可能である。例えば、ツールの向きを決定するための変数（Ｘ、Ｙ、Ｚ軸まわりの回転角度Ｒｘ、Ｒｙ、Ｒｚ）のうち、いずれか１つ、あるいは、いずれか２つが任意変数として指定されれば、その指定された変数が任意変数として設定される（任意変数設定処理）。そして、コントローラ３により変数値決定処理が行われる。
【００８６】
なお、本実施形態では、図４のステップＳ５および図７のステップＴ５にて求められるヤコビ行列Ｊｋについて、軸数の変更に基づいて変更すればよい。そのヤコビ行列Ｊｋは、例えばツールのＸ軸まわりの回転角度ＲｘおよびＹ軸まわりの回転角度Ｒｙが任意変数として設定された場合、下記（１６）式のように表される。ただし、第１軸Ｊ２１〜第６軸Ｊ２６のそれぞれの回転角度をθ１ｋ〜θ６ｋとし、ツールのＺ軸まわりの回転角度をＲｚｋとしている。
【００８７】
【数３】
0005803178.tif 000004
【００８８】
これにより、目標位置における任意変数の値（ツールの向き）が、ロボット２２の移動時間が極力短くなるような値に自動的に決定される。その結果、本実施形態の構成においても、第１の実施形態および第２の実施形態と同様の作用および効果が得られる。
【００８９】
（第４の実施形態）
以下、第１の実施形態に対し、変数値決定処理の内容を変更した第４の実施形態について図１２〜図１４を参照しながら説明する。
本実施形態では、ツール先端の目標位置（Ｘ、Ｙ、Ｚ軸座標値）が与えられることにより、ロボット２が所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が、ロボット２がとり得る全ての形態（図３に示した２つの腕形態）のそれぞれについて求められる。すなわち、変数値決定処理において、図４のステップＳ１に代えて図１２のフローチャートに示す内容の処理が実行される。図１３および図１４は、設定されるツール先端の目標位置に応じたロボット２の姿勢（各軸の回転角度）を模式的に示している。なお、図１３および図１４では、ロボット２のベース５およびフランジ９（手先）を丸印で示し、その他のアームなどの図示は省略している。
【００９０】
図１２の処理が開始されると（スタート）、ツール先端距離にツール長を加えた長さが、ロボット２のリンク長より長いか否かが判断される（ステップＵ１）。ツール先端距離とは、ロボット２のベース５からツール先端の目標位置までの距離である。また、ツール長とは、手先（フランジ９）に取り付けられたツールのオフセット長のことである（図１３および図１４参照）。すなわち、ツール長は、図１３および図１４において点線で描かれた円の半径に相当する。また、リンク長は、図１３および図１４において一点鎖線で描かれた円の半径に相当する。なお、本実施形態における長さ（距離）とは、Ｘ−Ｙ平面上での長さ（距離）に相当する。
【００９１】
ステップＵ１による判断の結果、ツール先端距離＋ツール長のほうがリンク長よりも長い場合（ＹＥＳ）、ステップＵ２に進む。ロボット２のリンク長よりも遠くには、手先位置を動かすことはできない。従って、この場合には、ステップＵ２で移動不可能と判断され、処理が終了する（エンド）。
【００９２】
一方、ステップＵ１による判断の結果、ツール先端距離＋ツール長よりもリンク長のほうが長い場合（ＮＯ）、ステップＵ３に進む。この場合、ツール先端が目標位置に到達するように手先を動かすことが可能である。ただし、手先位置がベース５に近付き過ぎると、手先およびベース５が衝突するおそれがある。そこで、ベース５から手先（フランジ９）までの距離の目標値（目標距離）を下記（２１）式のとおり定める。
ベースと衝突するおそれのある距離＜目標距離＜リンク長      …（２１）
【００９３】
上記（２１）式に示す目標距離は、図１３および図１４において実線で描かれた円の半径に相当する。ステップＵ３では、下記（２２）式の条件を満たしているか否かが判断される。すなわち、図１３（ａ）の状態のように、点線および実線の円が互いに交差し、且つ、実線の円の外側から一点鎖線の円の内側までの間にツール先端の目標位置があるという条件を満たしているか否かが判断される。
ツール先端距離−ツール長≦目標距離≦ツール先端距離      …（２２）
【００９４】
上記（２２）式の条件を満たしていない場合（ステップＵ３で「ＮＯ」）、ステップＵ４に進む。ステップＵ４では、下記（２３）式の条件を満たしているか否かが判断される。すなわち、図１３（ｂ）の状態のように、点線および実線の円が互いに交差し、且つ、実線の円の内側にツール先端の目標位置があるという条件を満たしているか否かが判断される。
ツール先端距離＋ツール長≧目標距離≧ツール先端距離      …（２３）
【００９５】
上記（２２）式の条件または上記（２３）式の条件を満たしている場合（ステップＵ３またはＵ４で「ＹＥＳ」）、ステップＵ５に進む。ステップＵ５では、ベース５から手先（フランジ９）までの距離（手先距離）が目標距離に一致するとともに、手先からツール先端までの距離がツール長に一致するという条件を満たす手先の位置が導出される。すなわち、図１３において、点線および実線の円が交差する２つの点（図１３で、白抜きの丸印および黒塗りの丸印で示す点）に位置するフランジ９の位置が候補値として導出される。ステップＵ６では、ステップＵ５で導出された２つの手先位置（候補値）のうち、正面を指す方向ベクトル（１，０）との内積が最大になるものが選択される。つまり、候補値のうち、よりロボット２の正面に近い側の値（図１３で白抜きの丸印で示す点）が選択される。
【００９６】
一方、上記（２２）式の条件および上記（２３）式の条件をいずれも満たしていない場合（ステップＵ３およびＵ４の双方で「ＮＯ」）、ステップＵ７に進む。すなわち、図１４（ａ）、（ｂ）の状態のように、点線および実線の円が互いに交差していない場合、ステップＵ７に進む。この場合、ベース５から手先（フランジ９）までの距離が目標距離に一致するように、手先の位置を設定することが不可能である。ステップＵ７では、ツール先端距離が目標距離以上であるか否かが判断される。すなわち、ツール先端の目標位置が実線の円の外側に位置する状態（図１４（ａ）の状態）か、または、ツール先端の目標位置が実線の円の内側に位置する状態（図１４（ｂ）の状態）かが判断される。その結果、ツール先端距離が目標距離以上であると判断された場合（ＹＥＳ）、つまり図１４（ａ）のような状態である場合、ステップＵ８に進む。
【００９７】
ステップＵ８では、下記（２４）式に基づいて手先の位置が導出される。すなわち、点線の円上であり、且つ、実線の円との距離が最も短くなる位置（図１４（ａ）におけるフランジ９の位置）が手先の位置として導出される。ただし、ツール先端位置は、ツール先端のＸ、Ｙ軸座標値である。つまり、下記（２４）式では、距離（ツール先端距離およびツール長）から求まる比率を、ツール先端位置（Ｘ、Ｙ軸座標値）にかけることで、手先位置（Ｘ、Ｙ軸座標値）を算出している。
手先位置＝（（ツール先端距離−ツール長）÷ツール先端距離）×ツール先端位置
…（２４）
【００９８】
一方、ステップＵ７による判断の結果、ツール先端距離が目標距離未満であると判断された場合（ＮＯ）、つまり図１４（ｂ）のような状態であると判断された場合、ステップＵ９に進む。ステップＵ９では、下記（２５）式に基づいて手先の位置が導出される。すなわち、点線の円上であり、且つ、実線の円との距離が最も短くなる位置（図１４（ｂ）におけるフランジ９の位置）が手先の位置として導出される。下記（２５）式についても、上記（２４）式と同様の考えに基づいて手先位置（Ｘ、Ｙ軸座標値）を算出している。
手先位置＝（（ツール先端距離＋ツール長）÷ツール先端距離）×ツール先端位置
…（２５）
【００９９】
ステップＵ１０では、ステップＵ６で選択された手先位置またはステップＵ８、Ｕ９で導出された手先位置を実現するための各軸の回転角度が、全ての形態（２つの腕形態）のそれぞれについて求められ、処理が終了する（エンド）。このような処理が実行される結果、ロボット２が所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値が、ロボット２がとり得る全ての形態のそれぞれについて取得される。
【０１００】
このように、本実施形態によれば、ツール先端の目標位置（各軸座標値）が与えられるだけで、４軸の水平多関節型ロボットであるロボット２が所定の形態をとり且つ目標位置にツール先端が到達する各軸の回転角度の値を、ロボット２がとり得る全ての形態（２つの腕形態）のそれぞれについて求めることが可能になる。そのため、ユーザ（作業者）の作業負担が一層軽減されるという効果が得られる。
【０１０１】
（その他の実施形態）
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
上記各実施形態では、本発明を４軸の水平多関節型のロボット２、または６軸の垂直多関節型のロボット２２に適用した例を説明したが、本発明は、複数の軸を有するロボット全般に適用可能である。
【符号の説明】
【０１０２】
図面中、２、２２はロボット、３はコントローラ（ロボットの制御装置、任意変数設定処理手段、変数値決定処理手段）、９、３１はフランジ（手先）、Ｊ１〜Ｊ４、Ｊ２１〜Ｊ２６は軸を示す。
【図１】
0005803178.tif 000005
【図２】
0005803178.tif 000006
【図３】
0005803178.tif 000007
【図４】
0005803178.tif 000008
【図５】
0005803178.tif 000009
【図６】
0005803178.tif 000010
【図７】
0005803178.tif 000011
【図８】
0005803178.tif 000012
【図９】
0005803178.tif 000013
【図１０】
0005803178.tif 000014
【図１１】
0005803178.tif 000015
【図１２】
0005803178.tif 000016
【図１３】
0005803178.tif 000017
【図１４】
0005803178.tif 000018
【図１５】
0005803178.tif 000019