(57)【特許請求の範囲】
【請求項１】
ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作における手首形態決定方法であって、
前記姿勢制御動作時に各補間点における手首形態を決定する際に、ソフトウェア処理によって、前回の処理で定められた補間点におけるβ軸の軸値及びγ軸の軸値を考慮し、前記前回の処理で定められた補間点におけるγ軸の軸値から測った前記当該補間点におけるγ軸の軸値の変化量が９０゜を越えないように前記同軸手首の形態が選択され、
前記ソフトウェア処理が次式、
ｐ＝（ａｚ × cosγ0 ）＋（ａｙ × sinγ0 ）、［但し、γ0 は手首形態を決定しようとしている補間点の１つ手前の補間点のγ値、ａｚ ，ａｙ は手首根元座標系から見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分］、
で定義される指標を用いて各補間点における姿勢制御条件を満たす値の組（β，γ）に関し、｜γ−γ0 ｜の値が相対的に大きくない方の組が選択されるように値の組（β，γ）を順次定めるアルゴリズムを利用している同軸手首ロボットの姿勢制御動作における手首形態決定方法。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本願発明は、工場における省力化、自動化等に利用される産業用ロボット（以下、単にロボットと言う。）の制御技術に関し、更に詳しく言えば、同軸手首を有するロボットの姿勢制御時に適切な手首形態を実現させる為の手首形態決定方法に関する。
【０００２】
【従来技術】
ロボットの手首側の３軸が図１に示したような軸構成を有している場合、これを同軸手首と言い、また、同軸手首を備えたロボットを同軸手首ロボットと呼んでいる。図１に記したように、これらの手首３軸は最も手先側の軸から順にα軸、β軸、γ軸と呼ばれ、各軸の回転移動方向及び向き（移動位置を表わす軸変数の正負）は同図に示した通りである。なお、以下α，β，γは各軸の角度位置（「軸値」とも言う。）を表わす記号としても使用される。
【０００３】
このような同軸手首ロボットの姿勢制御動作時には手首形態を決定する必要が生じる。これは、ロボットの姿勢を完全に制御しても、逆変換の方程式を解く過程で、軸値の組（α，β，γ）が必ずしも一意的に定まらないことによる。手首形態を完全に定める為には手首３軸の内、β軸の値の符号を指定してやれば良い事が知られており、図１に併記したように、βに正値をとらせた状態を「フリップ」と呼び、βに負値をとらせた状態を「ノンフリップ」と呼んでいる。
【０００４】
従来、ロボットの姿勢制御動作時の手首形態は、一連の姿勢制御動作の間を通して「フリップ」の状態あるいは「ノンフリップ」の状態を維持するように定められていた。
【０００５】
【発明が解決しようとする課題】
しかし、フリップ／ノンフリップを不変に保つ上記従来の手首形態決定方法を適用した場合、姿勢制御動作時の移動に関して一補間あたりのγ軸の移動量が非常に大きくなる。この現象は同軸手首が特異姿勢（β＝０°の姿勢）近辺にある時に特に顕著となり、一補間あたりのγ軸の移動量が９０°〜１８０°にも達する。
【０００６】
図２はこの現象を説明する図で、ツール姿勢をワールド座標系に対して一定に保つ姿勢制御を実行した事例について、手首形態をフリップ（β＞０）に保ったままの場合の推移が符号（Ａ）→（Ｂ）→（Ｃ）→（Ｄ）で示されている。
【０００７】
記号Σｗｒで記された座標系は、γ軸の手前側のアーム軸線（軸間のオフセットが無いとすれば、基本３軸の第３軸の原点からγ軸の原点に向かう直線）Ｇをｘ軸方向（ｘｗｒで表示）とした座標系である。本明細書においてはこれを手首根元座標系と呼ぶことにする。本事例では、図示されているように、（Ａ）→（Ｂ）→（Ｃ）→（Ｄ）の推移に従って手首根元座標系のｘ軸（ｘｗｒ）の方向が変化する。
【０００８】
後述するように、この手首根元座標系Σｗｒから見たツール姿勢を表わす行列の一部をなすアプローチベクトルに基づいてβ及びγの値が各補間点について決定される。なお、符号（Ａ）の状態にはツール先端点ＴＣＰを原点とするツール座標系のｚ軸の方向を記号ｚｔ  で併記した。
【０００９】
同図に示した事例では、フリップ状態（β＞０）でスタートした（Ａ）の状態から（Ｂ）に至る過程では、γ値に急激な変化を与えることなくβを正値に保つことが出来る。しかし、（Ｂ）→（Ｃ）の中間においてβ＝０の特異姿勢を通過する前後でフリップ（β＞０の状態）を持続する為には、γ値をほぼ１８０°ジャンプさせなければならない。γ値をジャンプさせた後の（Ｃ）→（Ｄ）の過程については、（Ａ）→（Ｂ）の過程と同様、γ値の急激な変化は要求されない。
【００１０】
姿勢制御動作時にノンフリップ（β＜０の状態）を特異姿勢通過の前後で持続させる際にも同様のことが起る。このような急激な軸値変化を伴う動作は作業者にとって危険であり、また、大きな加速度が要求されるから軌跡精度の劣化も生じ易い。
【００１１】
そこで本願発明の目的は、同軸手首ロボットの姿勢制御動作に関して、完全に姿勢を制御しながら、なお且つγ軸の値の急激な変化を伴うような動作（特に、１補間について９０°〜１８０°ものγ値の変化が要求される動作）を回避することが出来る手首形態の決定方法を提供することにある。
【００１２】
また、本願発明は、そのことを通して同軸手首ロボットを用いた作業の安全性を向上させるとともに、特に特異姿勢近辺で生じ易い軌跡精度の劣化を防止することを目指している。
【００１３】
【課題を解決するための手段】
本願発明の方法によれば、手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作に関して、γ軸の隣り合う補間点間のγ軸の軸値の変化が９０゜以下に抑制される。即ち、姿勢制御動作時に各補間点における手首形態を決定する際に、前回定められた補間点におけるβ軸の軸値及びγ軸の軸値を考慮し、前回の処理で定められた補間点におけるγ軸の軸値から測ったγ軸の軸値の変化量が９０゜を越えないように同軸手首の形態を選択するようにした同軸手首の形態が決定される。
【００１４】
各補間点において前回定められた補間点におけるβ軸の軸値及びγ軸の軸値を考慮し、前回の処理で定められた補間点におけるγ値から測ったγ軸の軸値の変化量が９０°を越えないように同軸手首の形態を選択する為の一連のソフトウェア処理は同軸手首ロボットを制御するロボット制御装置内で実行される。
【００１５】
そして、ソフトウェア処理に際しては、ｐ＝（ａｚ × cosγ0 ）＋（ａｙ × sinγ0 ）、［但し、γ0 は手首形態を決定しようとしている補間点の１つ手前の補間点のγ値であり、ａｚ ，ａｙ は手首根元座標系から見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分とｙ成分］、で定義される指標を用いたアルゴリズムによって各補間点における姿勢制御条件を満たす値の組（β，γ）が順次決定される。各回の決定について、｜γ−γ0 ｜の値が相対的に大きい方の組が選択されないように値の組（β，γ）を定めることで所期の目的が達成される。
【００１６】
【作用】
本願発明は、α軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作に関して、γ軸の隣り合う補間点間のγ値の変化を９０°以下に抑制するものである。そこで、この考え方の基礎となる原理について数式並びに図を使って説明する。
同軸ロボットの姿勢制御動作時には、各補間点で同軸手首のβ軸及びγ軸の値を定める為に、一般に次の（１）式で与えられるような形の方程式を解くことが求められる。
【００１７】
【数１】
0003580885.tif 000002
ここで、（ａx ，ａy ，ａz ）は、手首根元座標系から見たツール先端点の姿勢を表わす行列に含まれるアプローチベクトル＜ａ＞の各成分である。アプローチベクトルは、ツール先端点のｚ軸周りのスピン成分以外の姿勢を表現するベクトルで、その値は、動作プログラムに付随した位置データ、基本３軸の軸値（逆変換演算で決定される）、軸長データ等から補間点毎に計算される。
【００１８】
（ａｘ  ，ａｙ  ，ａｚ  ）を既知とすれば、上記方程式（１）の解は次の（２）式または（３）式で与えられる。
【００１９】
【数２】
0003580885.tif 000003
姿勢制御動作を行なう上で問題となるのは、上記（２）式の場合は±いずれの符号を選択するかであり、上記（３）式の場合はγの値をどう定めるかである。（２）式における±の記号の選択が手首形態の決定に対応しており、本願発明では、γ値に急激な変化を回避するようにソフトウェア処理によって前回補間点のγ値を考慮してこの選択が行われる（一般に、１補間点当りのγ値の変化量は最大でも９０°までに抑えられる）。また、上記（３）式における不定値を確定する際にも、前回の補間点のγ値を採用することが出来る。前回の補間点のγ値を考慮する為の指標としては、次式（４）で定義されるｐが用いられる。
【００２０】
ｐ＝（ａz × cosγ0 ）＋（ａy × sinγ0 ）          ・・・（４）
但し、γ0 は手首形態を決定しようとしている補間点の１つ手前の補間点（前回の補間点とも言う。）のγ値であり、ａz ，ａy は手首根元座標系から見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分である。
【００２１】
本願発明によれば、姿勢制御動作中にロボットが特異姿勢近辺を通過しても、γ軸が急激に回転することが回避される。また、上記（３）式における不定値を確定する際にも、前回の補間点のγ値を採用することが出来る。なお、指標ｐを用いたアルゴリズムによるソフトウェア処理については、次の実施例で説明する。
【００２２】
【実施例】
図３は本実施例で使用されるロボット制御装置の概略構成を要部ブロック図で示したものである。同図において符号１０で表示されたロボット制御装置は、マイクロプロセッサを含む中央演算処理装置（以下、ＣＰＵという。）１１を備えている。ＣＰＵ１１にはＲＯＭからなるメモリ１２、ＲＡＭからなるメモリ１３、不揮発性メモリ１４、液晶ディスプレイ（ＬＣＤ）を備えた教示操作盤１５、サーボ回路１７を経て各軸のモータを内蔵するロボット機構部に接続されたロボット軸制御部１６が各々バス１９を介して接続されている。
【００２３】
ＲＯＭ１２には、ＣＰＵ１１がロボット本体１及びロボット制御装置１０自身の制御を行なう為の各種のプログラムが格納される。ＲＡＭ１３はデ−タの一時記憶や演算の為に利用されるメモリである。不揮発性メモリ１４には、各種パラメータ設定値や動作プログラムが格納される。以上の構成は、通常のロボット制御装置と特に変わるところはない。
【００２４】
以下、動作プログラム中に指定された経路移動中に図２に示した従来例と同様の姿勢制御を行なう場合について、本願発明に従って手首形態を決定する処理について説明する。図４は、本実施例における姿勢制御動作時の推移を示したもので、姿勢推移（Ａ’）→（Ｂ’）→（Ｃ’）→（Ｄ’）は図２における姿勢推移（Ａ）→（Ｂ）→（Ｃ）→（Ｄ）に対応するものである。また、手首根元座標系Σｗｒ等の意味は、図２の場合と同様である。
【００２５】
このような区間（Ａ’）〜（Ｄ’）について本願発明を適用する場合には、区間（Ａ’）〜（Ｄ’）内の各補間点毎に図５のフローチャートに示したアルゴリズムを用いた処理を実行し、手首形態を決定する。この処理を実行する為に必要なプログラム並びに関連設定値は、予めロボット制御装置１０内の各メモリ１２，１３，１４に適宜格納される。
【００２６】
図５のフローチャートに示された処理中で、姿勢制御動作の区間（Ａ’）〜（Ｄ’）の最初の補間点に対する「前回の補間点」は始点自体であるとする。以上の前提で、図５のフローチャートに示された処理の概要を説明する。なお、アプローチベクトルの値を求める処理自体については周知であり、本願発明と直接関係が無いので、ここでは説明を省略する。
【００２７】
先ず、補間計算の過程で求められたアプローチベクトルのｘ成分ａｘ  が１であるかを判断する（ステップＳ１）。もし、イエスであれば、ステップＳ８へ進み、前記（３）式に従ってβ＝０（フリップとノンフリップの境界に相当する状態）とする。また、この姿勢が特異姿勢である為に（３）式で不定とされているγについては、前回の補間点における値γ０  をそのまま採用する（ステップＳ８）。これにより、特異姿勢をとる補間点に関してもγ値の急激な変化が回避される。
【００２８】
ステップＳ１でノーであればステップＳ２へ進み、前記（２）式に従ってｃｏｓ  β＝ａｘ  からβ値の候補を決定する（この段階では、β値の符号に正負の自由度が残されている）。次いで、前記（４）式で定義された指標ｐの符号を用いて前回のγ値を考慮する為の一連の処理が実行される。
【００２９】
先ず、ステップＳ３でｐ＝０であるかが判断される。ｐ≠０と判断された場合には、更にステップＳ４で正負の符号が判定される。もし、ｐ＞０と判定された場合には、ステップ６へ進み、前記（２）式における±符号の内＋符号を選択して、β値及びγ値を決定する。β値については、ステップＳ２で求められた正負２つの候補値の内、正値のものが選択されることになる（フリップの選択）。
【００３０】
ステップＳ４で、ｐ＜０と判定された場合には、ステップ７へ進み、前記（２）式における±符号の内−符号を選択して、β値及びγ値を決定する。β値については、ステップＳ２で求められた正負２つの候補値の内、負値のものが選択されることになる（ノンフリップの選択）。
【００３１】
ステップＳ３でｐ＝０であると判断された場合には、ステップＳ５で前回の補間点におけるβ値自体の符号が参照される。もし、β０  ≧０と判断された場合にはステップ６へ進み、ｐ＞０の場合と同様に、前記（２）式における±符号の内＋符号を選択して、β値及びγ値を決定する。
【００３２】
また、β０  ＜０と判断された場合にはステップ７へ進み、ｐ＜０の場合と同様に、前記（２）式における±符号の内−符号を選択して、β値及びγ値を決定する。
ステップＳ６、ステップＳ７、ステップＳ８のいずれかの完了後にその補間点に関する処理が終了される。このように、図５のフローチャートに示したアルゴリズムを用いることによって、すべてのケースについてβ値及びγ値が定められる。
【００３３】
上記アルゴリズムにおける指標ｐの符号の考慮は、（２）式におけるベクトル（  ｃｏｓγ，  ｓｉｎγ）の正負の反対符号を持つ２解の内、前回の補間点におけるベクトル（  ｃｏｓγ０  ，  ｓｉｎγ０  ）から遠くない方の解を選択していることに他ならない。従って、どのようなケースにおいても｜γ−γ０  ｜が９０°を上回ることが原理的に起こり得ない。
【００３４】
図４に示されたケースについてフリップ／ノンフリップの選択状況を考えてみる。フリップ状態（β＞０）で姿勢制御が開始された直後の補間点では、ステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４を経てステップＳ６が実行される。従って、β＝０となることがない区間（Ａ’）〜（Ｂ’）内の補間点では、毎回同じ処理サイクル（ステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４→ステップＳ６）が繰り返される。
【００３５】
区間（Ｂ’）〜（Ｃ’）に入っても、しばらくはβ＝０（ａｘ  ＝１）となることがないので、同様にステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４→ステップＳ６の処理サイクルが繰り返される。しかし、ａｘ  が１に近づくに従い（図４の事例では、ワーク座標系上で見たツール姿勢は変化しないが手首根元座標系Σｗｒが変化することに注意）、βが正値側から徐々に０に近づく。
【００３６】
区間（Ｂ’）〜（Ｃ’）の中間の特異姿勢付近でａｘ  ＝１（手首根元座標系のｘ軸の方向とツール座標系のｚ軸ｚｔ  の方向が一致）の判定が出された場合には、その補間点ではステップＳ１→ステップＳ８が実行される。
【００３７】
その直後の補間点では再びステップＳ１からステップＳ２を経てステップＳ３へ進む。ステップＳ３における判定基準値にもよるが、特異姿勢からの姿勢変化が余り小さくない限り判断結果はノーとなる。もし、イエスとされた場合には、前回のβの値β０  の符号を直接確認した上で、正であればフリップ状態を選択し、負であればノンフリップ状態を選択する。
【００３８】
ｐ＝０の判定がなされた場合でも、やがてロボット姿勢は特異姿勢から離れる方向に変化するので、次回の補間点以降でステップＳ３でイエスの判定が出され続けることはない。
【００３９】
最初にステップＳ３またはステップＳ５でノーの判断が出されると、ステップＳ７が初めて実行され、ノンフリップが選択される。これにより、特異姿勢通過時のγ値のジャンプが回避される。
【００４０】
もし、従来技術の如く特異姿勢通過前後と通してフリップ状態を維持した場合を仮定すると（図２を参照）、（２）式の形から判るように、特異姿勢通過前後でａｙ  及びａｚ  の符号が反転するので、最悪の場合γを１８０°変化させなければならなくなる。
【００４１】
本願発明では、特異姿勢通過前後で起るａｙ及びａｚの符号の反転時にβ値の正負を反転させるので、γ値の変化は最悪の場合でも９０°を越えることはない。図４の事例では、（Ｂ’），（Ｃ’）間で遭遇する特異姿勢の前後におけるγ値の変化は極く僅かとなる。ツール姿勢をワーク座標系上で滑らかに変化させるような一般のケースであっても、図５に示したフローチャートのアルゴリズムはそのまま有効である。
【００４２】
【発明の効果】
本願発明によれば、同軸手首ロボットの姿勢制御動作に際して、γ軸の値の急激な変化を伴うような動作（例えば、１補間について９０°〜１８０°ものγ値の変化が要求される動作）が回避される。従って、同軸手首ロボットを用いた作業の安全性が向上し、また、特異姿勢近辺で生じ易い軌跡精度の劣化を防止することが出来る。
【図面の簡単な説明】
【図１】同軸手首の軸構成について説明する図である。
【図２】従来技術の問題点（姿勢制御時のγ軸値の急激な変化）について説明する図である。
【図３】本実施例で使用されるロボット制御装置の概略構成を記した要部ブロック図である。
【図４】本実施例における姿勢制御動作時の手首形態の状況を説明する図である。
【図５】実施例における手首形態決定の為の実行される処理（β，γ決定処理）の概要を説明するフローチャートである。
【符号の説明】
１  ロボット
１０  ロボット制御装置
１１  中央演算処理装置（ＣＰＵ）
１２  ＲＯＭ
１３  ＲＡＭ
１４  不揮発性メモリ
１５  教示操作盤
１６  ロボット軸制御部
１７  サーボ回路
１９  バス
Ｇ  γ軸の手前側のアーム軸線
Σｗｒ  手首根元座標系
ｘｗｒ  手首根元座標系のｘ軸の方向
ｚｔ    ツール座標系のｚ軸の方向
【図１】
0003580885.tif 000004
【図２】
0003580885.tif 000005
【図３】
0003580885.tif 000006
【図４】
0003580885.tif 000007
【図５】
0003580885.tif 000008