JP3545486B2 - 同軸手首ロボットの姿勢制御動作における急動作回避方法 - Google Patents
同軸手首ロボットの姿勢制御動作における急動作回避方法 Download PDFInfo
- Publication number
- JP3545486B2 JP3545486B2 JP06894995A JP6894995A JP3545486B2 JP 3545486 B2 JP3545486 B2 JP 3545486B2 JP 06894995 A JP06894995 A JP 06894995A JP 6894995 A JP6894995 A JP 6894995A JP 3545486 B2 JP3545486 B2 JP 3545486B2
- Authority
- JP
- Japan
- Prior art keywords
- axis
- wrist
- value
- posture
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【０００１】
【産業上の利用分野】
本願発明は、工場における省力化、自動化等に利用される産業用ロボット（以下、単にロボットと言う。）の制御技術に関し、更に詳しく言えば、同軸手首を有するロボットの姿勢制御時に急動作を回避する方法に関する。
【０００２】
【従来技術】
ロボットの手首側の３軸が図１に示したような軸構成を有している場合、これを同軸手首と言い、また、同軸手首を備えたロボットを同軸手首ロボットと呼んでいる。図１に記したように、これらの手首３軸は最も手先側の軸から順にα軸、β軸、γ軸と呼ばれ、各軸の回転移動方向及び向き（移動位置を表わす軸変数の正負）は同図に示した通りである。なお、以下α，β，γは各軸の角度位置（「軸値」とも言う。）を表わす記号としても使用される。
【０００３】
このような同軸手首ロボットの姿勢制御動作時には手首形態を決定する必要が生じる。これは、ロボットの姿勢を完全に制御しても、逆変換の方程式を解く過程で、軸値の組（α，β，γ）が必ずしも一意的に定まらないことによる。手首形態を完全に定める為には手首３軸の内、β軸の値の符号を指定してやれば良い事が知られており、図１に併記したように、βに正値をとらせた状態を「フリップ」と呼び、βに負値をとらせた状態を「ノンフリップ」と呼んでいる。
【０００４】
従来、このような同軸手首ロボットに特異姿勢（β＝０の姿勢）近傍で姿勢制御動作を行なわせると、α及びγの値が隣合う補間点で急変する現象が生じていた。この現象を図２〜図５を用いて説明する。
【０００５】
先ず図２は、α＝β＝γ＝０の姿勢を示したものである。図中Σwrは手首の根元に固定された軸ｘｙｚを持つ座標系である。また、＜ｖ＞は手首先端に固定されたベクトルを表わしている。α＝β＝γ＝０の姿勢において、ベクトル＜ｖ＞の方向は座標系Σwrの＋ｚ軸方向と一致するものとする。
【０００６】
これら座標系Σwr及びベクトル＜ｖ＞は、任意の手首形態におけるα，β，γの値を記述する（測る）基準となるものである。α，β，γの値は、図３に示した態様で測られる。図３（１），（２）はα及びγの測り方を、β＞０の場合とβ＜０の場合に分けて表わしている。
【０００７】
図中、直線ｇ1 はβ軸の原点とα軸の原点を通る直線に垂直な直線の内、ｘ軸を通るものを表わしている。また、直線ｇ2 はα軸の原点を通りｚ軸に平行な直線である。そして、直線ｇ3 はα軸の原点からｘ軸に降ろした垂線である。
【０００８】
これらの事項を前提に、図４に示された姿勢制御動作について考察してみる。図４は、従来方式で制御を行なった場合に急動作を起こす姿勢制御動作の一例を表わしている。このケースでは、手首先端が曲線ＰＱ上を矢印方向に動き、その間ベクトル＜ｖ＞の方向は座標系Σwrのｚｘ平面に平行に保たれる。このような動作においては、曲線ＰＱ上を通る間に、図２に示した特異姿勢あるいはその近傍を通過することになる。
【０００９】
従来方式では、このような場合でも、「フリップ」の状態あるいは「ノンフリップ」の状態を維持するような処理が行なわれている。その結果、α及びγについて、特異姿勢あるいはその近傍を通過する際に値の急激な変化が避けられなかった。図５はこれを説明する図で、（１）はγの急変を、（２）はαの急変を表わしている。このような急動作は、β値の符号を維持しようとすることに起因して引き起こされるものと考えることが出来る。
【００１０】
そこで、本発明者は既に特異姿勢通過時に、フリップ／ノンフリップの反転を行なう手首形態の決定方法を提案した（特願平７−２３３３５号に添付された明細書及び図面を参照）。図４に示したケースにおいて、曲線ＰＱが特異姿勢を正確に通過、あるいは特異姿勢を通過したとみなせるほどの極く近傍を通過する場合（以下、単に「特異姿勢を通過」と言う。）には、上記提案された方法を適用することによって、急動作は回避される。
【００１１】
【発明が解決しようとする課題】
しかし、図４に示したケースにおいて、曲線ＰＱが特異姿勢の近傍は通過するが特異姿勢は通過しないような場合には、上記提案の方法を適用してもフリップ／ノンフリップの反転は起らない。そして、急動作は特異姿勢のある程度の拡がりを持った範囲内で特異姿勢からの隔たりに応じた強さで発生する性質があるから、特異姿勢近傍通過時の急動作の危険は回避出来なかった。
【００１２】
そこで、本願発明の目的は、同軸手首ロボットの姿勢制御動作時について、特異姿勢の近傍を通過する際に確実に急動作を回避する方法を提供し、急動作発生時の弊害（人身の危険、軌跡精度の顕著な劣化）を未然に防止することにある。
【００１３】
【課題を解決するための手段】
本願発明は、ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作について、上記提案済みの方法が有効に適用される範囲を実質的に拡張することを通して上記課題を解決したものである。
本願発明の方法に従えば、姿勢制御動作時に各補間点について実行されるソフトウェア処理が、特異姿勢への接近を判断する処理と、該処理によって特異姿勢に接近したと判断された際には、前記特異姿勢を通過するように動作指令を修正する処理と、手首形態を決定する処理とを含んでいる。
【００１４】
手首形態を決定する処理においては、前回の補間処理における手首軸の値、例えば、β軸の軸値及びγ軸の軸値が考慮され、前記前回の補間処理で定められた補間点における手首軸の値（例えばγ値）から測った前記当該補間点における手首軸の値（例えばγ値）軸の軸値の変化量が９０°を越えないように、あるいは、同変化量が相対的に大きくない方の手首軸位置が選択されるように、前記同軸手首の形態が選択される。
【００１５】
該選択に際しては、次式、
ｐ＝（ａz × cosγ0 ）＋（ａy × sinγ0 ）、［但し、γ 0 は手首形態を決定しようとしている補間点の１つ手前の補間点のγ値、ａz，ａy は手首根元座標系から見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分］、で定義される指標を用いて各補間点における姿勢制御条件を満たす値の組（β，γ）に関し、｜γ−γ0 ｜の値が相対的に大きくない方の組が選択されるように値の組（β，γ）を順次定めるアルゴリズムを利用することが出来る。
【００１６】
また、前記特異姿勢への接近を判断する処理は、予め設定されるβ値の絶対値｜β｜の臨界値Ｋを基準として実行することが出来る。
【００１７】
【作用】
本願発明によって、α軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作について、特異姿勢近傍における急動作が回避される原理の概要は以下の通りである。
同軸ロボットの姿勢制御動作時には、各補間点で同軸手首の形態（α，β，γの値）を定める為に、一般に次の（１）式で与えられるような形の方程式を解くことが求められる。
【００１８】
【数１】
ここで、（ａx ，ａy ，ａz ）は、手首根元座標系から見たツール先端点の姿勢を表わす行列に含まれるアプローチベクトル＜ａ＞の各成分である。アプローチベクトルは、ツール先端点のｚ軸周りのスピン成分以外の姿勢を表現するベクトルで、その値は、動作プログラムに付随した位置データ、基本３軸の軸値（逆変換演算で決定される）、軸長データ等から補間点毎に計算される。
【００１９】
（ａx ，ａy ，ａz ）を既知とすれば、上記方程式（１）の解は次の（２）式または（３）式で与えられる。
【００２０】
【数２】
姿勢制御動作を行なう上で問題となるのは、上記（２）式の場合は±いずれの符号を選択するかであり、上記（３）式の場合はγの値をどう定めるかである。（２）式における±の記号の選択が手首形態の決定に対応している。本願発明で利用される手首形態決定の為の処理では、γ値に急激な変化を回避すべく前回補間点のγ値を考慮してこの選択が行われる（一般に、１補間点当りのγ値の変化量は最大でも９０°までに抑えられる）。
【００２１】
また、上記（３）式における不定値を確定する際にも、前回の補間点のγ値を採用することが出来る。前回の補間点のγ値を考慮する為の指標としては、次式（４）で定義されるｐが用いられる。
【００２２】
ｐ＝（ａz × cosγ0 ）＋（ａy × sinγ0 ） ・・・（４）
但し、β0 ，γ0 は手首形態を決定しようとしている補間点の１つ手前の補間点（前回の補間点とも言う。）のβ値及びγ値であり、ａz ，ａy は手首根元に設定された座標系Σwrから見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分である。
【００２３】
上記指標ｐを用いたアルゴリズム（詳細内容は後述）により、姿勢制御動作中に同軸手首ロボットが特異姿勢自身（β＝０とみなされる範囲）を通過した場合には、フリップ／ノンフリップを反転させる処理が行なわれ、γ軸とα軸の急激な回転が回避される。また、上記（３）式における不定値を確定する際にも、前回の補間点のγ値を採用することが出来る。
【００２４】
しかし、急動作が発生するのは、同軸手首ロボットが特異姿勢自身（β＝０とみなされる範囲）を通過した場合に限られるわけではなく、ある程度の近傍（例えばβ＝５°以内）を通過する際にもα軸、γ軸の急回転は起こり得る。そこで、本願発明では、同軸手首ロボットが特異姿勢の近傍にさしかかった段階で、動作指令を少なくとも前記特異姿勢の極く近傍を通過するように修正する処理を実行する。
【００２５】
これにより、上記手首形態決定の為の処理の中でフリップ／ノンフリップの反転が実現される。その結果、α軸、γ軸の急回転を起こすような手首形態が決定されることがなくなり、同軸手首ロボットの急動作が確実に回避される。
【００２６】
【実施例】
ここでは、図４に例示した姿勢制御動作に本願発明の方法を適用するケースについて説明する。
図６は、図４に例示した姿勢制御動作に本願発明の方法を適用した場合の動作指令の修正内容を説明する図である。
【００２７】
図４に関連して述べたように、従来方式で制御を行なった場合には、手首先端が曲線ＰＱ上（但し、破線部分Ｐ’Ｑ’を通過。実線Ｐ’Ｑ’の意味は後述。）を矢印方向に動き、その間ベクトル＜ｖ＞の方向は座標系Σwrのｚｘ平面に平行に保たれる。
【００２８】
但し、ここでは、曲線ＰＱの破線部分は図２に示した特異姿勢からある程度離れた近傍（即ち、特異姿勢とはみなされない近傍、例えばβ＝３°）を通過するものとする。このような場合に前述の提案済みの手首形態決定方法に従った処理を実行しても、フリップ／ノンフリップの反転は実現されず、特異姿勢への接近時に急動作を起こすおそれがある。
【００２９】
そこで、同軸手首ロボットが急動作を起こす危険領域へ進入する手前で、特異姿勢とみなされる範囲Ｂ（即ち、前述の提案方法で、フリップ／ノンフリップの反転を実現出来る範囲）を確実に通過するように動作軌跡が変更されるように動作指令を修正する。Ｐ’は修正開始点を表わしている。ここでは、特異姿勢を表わす点Ａへ向かう動作軌跡を実現させる為の移動指令を補間処理毎に作成する。
【００３０】
同軸手首ロボットがＰ’から点Ａに到達（厳密には最接近）する前後でフリップ／ノンフリップの反転が実現され、α軸、γ軸の急回転は回避される。動作軌跡はなるべく早く滑らかに元来のものに復帰させることが好ましいので、点Ａからは、徐々に破線Ｐ’Ｑ’に接近させ、点Ｑ’からは元来の動作軌跡に復帰させる。
【００３１】
点Ｐ’は特異姿勢への接近を判断する基準点であり、これに対応するものとして、｜β｜の臨界値Ｋを予め設定しておく。Ｋの値は設計的に定められるが、実際的な値としては、例えばＫ＝５°が考えられる。元来の動作軌跡の中から最初に｜β｜＝Ｋを満たすを与える点を捜し出すことで、動作指令修正開始時期を決定することが出来る。
【００３２】
点Ｑ’については、｜β｜＝Ｋを与える点を元来の動作軌跡の中から捜し出せば良い。元来の動作軌跡（Ｐ→Ｐ’→破線部分通過→Ｑ’→Ｑ）は教示データから随時計算可能である。例えば、動作指令修正開始後も破線部分の軌跡に対応した補間点のアプローチベクトルデータを先行的に継続し、同軸手首ロボットが点Ａに到達する前に、｜β｜＝Ｋとなる点をＱ’として定めておく。点Ｑ’のデータ（アプローチベクトルのデータ）は、動作軌跡ＡＱ’の計算に利用される。
【００３３】
以上が、本実施例における動作指令の修正内容の概要であるが、以下、その為の処理を含む処理全体について説明する。
図７は本実施例で使用されるロボット制御装置の概略構成を要部ブロック図で示したものである。同図において符号１０で表示されたロボット制御装置は、マイクロプロセッサを含む中央演算処理装置（以下、ＣＰＵという。）１１を備えている。ＣＰＵ１１にはＲＯＭからなるメモリ１２、ＲＡＭからなるメモリ１３、不揮発性メモリ１４、液晶ディスプレイ（ＬＣＤ）を備えた教示操作盤１５、サーボ回路１７を経て各軸のモータを内蔵するロボット機構部に接続されたロボット軸制御部１６が各々バス１９を介して接続されている。
【００３４】
ＲＯＭ１２には、ＣＰＵ１１がロボット本体１及びロボット制御装置１０自身の制御を行なう為の各種のプログラムが格納される。ＲＡＭ１３はデ−タの一時記憶や演算の為に利用されるメモリである。不揮発性メモリ１４には、各種設定値や動作プログラムが格納される。設定値には点Ｐ’，Ｑ’を定める為の値Ｋが含まれる。なお、ここで説明した構成は、通常のロボット制御装置と特に変わるところはない。
【００３５】
以下、図８及び図９のフローチャートを参照し、図６のＰＱ間で姿勢制御動作中に、ロボット制御装置内で実行される処理の概要を説明する。
図８のフローチャートは処理全体の流れを表わしている。先ず、元来の動作軌跡上の補間処理で求められたβの絶対値｜β｜と、設定値Ｋの大小関係を判定する（ステップＴ１）。
【００３６】
同軸手首ロボットが点Ｐ’に達する迄は、ステップＴ１の判断はＮＯであるから、ステップＴ２へ進み、元来の動作軌跡ＰＰ’に従った動作指令を作成し、ステップＴ６で手首形態を決定する処理（内容は後述）を実行する。この処理サイクル（Ｔ１→Ｔ２→Ｔ６）は、同軸手首ロボットが点Ｐ’に達する迄、補間周期毎に繰り返される。
【００３７】
同軸手首ロボットが点Ｐ’に達すると、ステップＴ１の判断はイエスに切り替わるので、ステップＴ３へ進み、特異姿勢点Ａへの未到達を確認した上でステップＴ４へ進む。ステップＴ４では、動作軌跡Ｐ’Ａに従った動作指令を作成し、ステップＴ６で手首形態を決定する処理を実行する。この処理サイクル（Ｔ１→Ｔ３→Ｔ４→Ｔ６）は、同軸手首ロボットが点Ｐ’に達する迄、補間周期毎に繰り返される。なお、この間も、元来の動作軌跡（破線Ｐ’Ｑ’）について補間点の計算自体は、点Ｑ’が求められるまで並行的に続行される（フローチャートには記載省略。動作指令はあくまで実線Ｐ’Ａに基づいて作成する）。
【００３８】
同軸手首ロボットが点Ａに達すると、ステップＴ３の判断はイエスに切り替わるので、ステップＴ５へ進み、動作軌跡ＡＱ’に従った動作指令を作成し、ステップＴ６で手首形態を決定する処理を実行する。この処理サイクル（Ｔ１→Ｔ３→Ｔ５→Ｔ６）は、同軸手首ロボットが点Ｑ’に達する迄、補間周期毎に繰り返される。
【００３９】
同軸手首ロボットが点Ｑ’に達すると、ステップＴ１の判断は再度ノーに切り替わるので、ステップＴ２へ進み、元来の動作軌跡Ｑ’Ｑに従った動作指令を作成し、ステップＴ６で手首形態を決定する処理を実行する。この処理サイクル（Ｔ１→Ｔ２→Ｔ６）は、同軸手首ロボットが点Ｑに達する迄、補間周期毎に繰り返される。
【００４０】
次にステップＴ６で実行される手首形態決定処理について図９のフローチャートを参照して説明する。この処理の内容は、前述した提案方法と実質的に同じものである（特願平７−２３３３５号に添付された明細書及び図面を参照）。
先ず、補間計算の過程で求められたアプローチベクトルのｘ成分ａx が１であるかを判断する（ステップＳ１）。もし、イエスであれば、ステップＳ８へ進み、前記（３）式に従ってβ＝０（フリップとノンフリップの境界に相当する状態）とする。また、この姿勢が特異姿勢である為に（３）式で不定とされているγについては、前回の補間点における値γ0 をそのまま採用する（ステップＳ８）。これにより、特異姿勢をとる補間点に関してもγ値の急激な変化が回避される。
【００４１】
ステップＳ１でノーであればステップＳ２へ進み、前記（２）式に従ってcos β＝ａx からβ値の候補を決定する（この段階では、β値の符号に正負の自由度が残されている）。次いで、前記（４）式で定義された指標ｐの符号を用いて前回のγ値を考慮する為の一連の処理が実行される。
【００４２】
先ず、ステップＳ３でｐ＝０であるかが判断される。ｐ≠０と判断された場合には、更にステップＳ４で正負の符号が判定される。もし、ｐ＞０と判定された場合には、ステップ６へ進み、前記（２）式における±符号の内＋符号を選択して、β値及びγ値を決定する。β値については、ステップＳ２で求められた正負２つの候補値の内、正値のものが選択されることになる（フリップの選択）。
【００４３】
ステップＳ４で、ｐ＜０と判定された場合には、ステップＳ７へ進み、前記（２）式における±符号の内−符号を選択して、β値及びγ値を決定する。β値については、ステップＳ２で求められた正負２つの候補値の内、負値のものが選択されることになる（ノンフリップの選択）。
【００４４】
ステップＳ３でｐ＝０であると判断された場合には、ステップＳ５で前回の補間点におけるβ値自体の符号が参照される。もし、β0 ≧０と判断された場合にはステップ６へ進み、ｐ＞０の場合と同様に、前記（２）式における±符号の内＋符号を選択して、β値及びγ値を決定する。
【００４５】
また、β0 ＜０と判断された場合にはステップＳ７へ進み、ｐ＜０の場合と同様に、前記（２）式における±符号の内−符号を選択して、β値及びγ値を決定する。
ステップＳ６、ステップＳ７、ステップＳ８のいずれかの完了後にその補間点に関する処理が終了される。このように、図５のフローチャートに示したアルゴリズムを用いることによって、すべてのケースについてβ値及びγ値が定められる。
【００４６】
上記アルゴリズムにおける指標ｐの符号の考慮は、（２）式におけるベクトル（ cosγ， sinγ）の正負の反対符号を持つ２解の内、前回の補間点におけるベクトル（ cosγ0 ， sinγ0 ）から遠くない方の解を選択していることに他ならない。従って、どのようなケースにおいても｜γ−γ0 ｜が９０°を上回ることが原理的に起こり得ない。
【００４７】
図６に示されたケースについてフリップ／ノンフリップの選択状況を考えてみる。フリップ状態（β＞０）で姿勢制御が開始された直後の補間点では、明らかに、ステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４を経てステップＳ６が実行される。区間ＰＰ’では、β＝０となることがないから、このサイクルが毎回繰り返される。
【００４８】
区間Ｐ’Ａに入っても、しばらくはβ＝０（ａx ＝１）となることがないので、同様にステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４→ステップＳ６の処理サイクルが繰り返される。しかし、ａx が１に近づくに従い（図６の事例では、ワーク座標系上で見たツール姿勢は変化しないが手首根元座標系Σwrが変化することに注意）、βが正値側から徐々に０に近づく。
【００４９】
特異姿勢点Ａの付近で、ａx ＝１（手首根元座標系Σwrの＋ｚ軸の方向とベクトル＜ｖ＞の方向が一致）の判定が出された場合には、その補間点ではステップＳ１→ステップＳ８が実行される。
【００５０】
その直後の補間点では再びステップＳ１からステップＳ２を経てステップＳ３へ進む。ステップＳ３における判定基準値にもよるが、特異姿勢からの姿勢変化が余り小さくない限り判断結果はノーとなる。もし、イエスとされた場合には、前回のβの値β0 の符号を直接確認した上で、正であればフリップ状態を選択し、負であればノンフリップ状態を選択する。
【００５１】
ｐ＝０の判定がなされた場合でも、やがてロボット姿勢は特異姿勢から離れる方向に変化するので、次回の補間点以降でステップＳ３でイエスの判定が出され続けることはない。
【００５２】
最初にステップＳ３またはステップＳ５でノーの判断が出されると、ステップＳ７が初めて実行され、ノンフリップが選択される。これにより、特異姿勢通過時のγ値のジャンプが回避される。
【００５３】
もし、特異姿勢通過前後を通してフリップ状態を維持した場合を仮定すると、（２）式の形から判るように、特異姿勢通過前後でａy 及びａz の符号が反転するので、最悪の場合γを１８０°変化させなければならなくなる。
【００５４】
本処理では、特異姿勢通過前後で起るａy 及びａz の符号の反転時にβ値の正負を反転させるので、γ値の変化は最悪の場合でも９０°を越えることはない。図１０（１），（２）は、本実施例で急動作が回避される状況を図示したものである。図１０（１）にはγ値の変化が小さいことが描かれ、図１０（２）にはα値の変化が小さいことが描かれている。図中βP ，βQ は各々点Ｐ，Ｑにおけるβ値を表わしている。特異姿勢通過時にフリップ／ノンフリップの反転が実行されるので、βP ，βQ は異符号である。従って、図５（１），（２）の場合のように、γ値及びα値の変化が大きくなることが避けられる。
【００５５】
なお、特異姿勢の前後におけるγ値の変化が極く僅かであれば、姿勢制御動作が滑らかなものである限り、α値の変化も小さくなることは、β値は元々特異姿勢付近で急変しないことに注意すれば、直感的にも明らかである。
【００５６】
結局、図４に示された姿勢制御の事例に本願発明の方法を適用した場合には、図１０（１），（２）に示したように、図６に示したような若干の動作軌跡の修正を伴う形で、同軸手首ロボットの急動作が確実に回避される。これは、もし本願発明の方法を適用しなかった場合に予測される急動作発生の事態に比べれば（図６の破線区間Ｐ’Ｑ’の途上で発生が予測される。）、作業の安全上、遥かに好ましいことである。
【００５７】
以上、図４に示された姿勢制御の事例に本願発明の方法を適用した場合について説明したが、ここで述べた手法、特に処理内容は他の姿勢制御の事例に関しても、基本的に適用可能なもあのである。
【００５８】
【発明の効果】
本願発明によれば、同軸手首ロボットの特異姿勢近傍における姿勢制御動作について、姿勢の指令値を若干変更し、特異姿勢通過時にフリップ／ノンフリップの反転を起こさせる手首形態決定処理を実行するだけで、α軸、γ軸の急激な回転を伴う急動作を回避することが出来る。従って、同軸手首ロボットに安全に姿勢制御動作を行なわせることが出来る範囲が拡張される。
【図面の簡単な説明】
【図１】同軸手首の軸構成について説明する図である。
【図２】同軸手首ロボットの特異姿勢について説明する図である。
【図３】同軸手首の軸の角度位置の測り方を説明する図である。（１）はαの測り方、（２）はγの測り方を、各々βの符号に関連させて示している。
【図４】同軸手首ロボットの姿勢制御動作の一例を説明する図である。
【図５】図４に示した姿勢制御動作を従来方式で実行した場合に発生する急動作について説明する図である。（１）にはγ値の急変が描かれ、（２）にはα値の急変が描かれている。
【図６】図４に示した姿勢制御動作に本願発明の方法を適用した場合の動作軌跡を説明する図である。
【図７】実施例で使用されるロボット制御装置の概略構成を要部ブロック図で示したものである。
【図８】実施例における処理全体の流れを説明する為のフローチャートである。
【図９】実施例で実行される手首形態決定処理について説明する為のフローチャートである。
【図１０】図４に示した姿勢制御動作に本願発明の方法を適用した場合に急動作が回避される状況を説明する図である。（１）にはγ値の急変回避が描かれ、（２）にはα値の急変回避が描かれている。
【符号の説明】
１ ロボット（同軸手首ロボットの機構部）
１０ ロボット制御装置
１１ 中央演算処理装置（ＣＰＵ）
１２ ＲＯＭ
１３ ＲＡＭ
１４ 不揮発性メモリ
１５ 教示操作盤
１６ ロボット軸制御部
１７ サーボ回路
１９ バス
Ｐ〜Ｑ 姿勢制御動作が行なわれる経路区間
Ｐ’ 姿勢の指令の修正を開始する点
Ｑ’ 姿勢の指令の修正を終了する点
Ａ 特異姿勢点
Ｂ 特異姿勢とみなされる範囲
Σwr 手首根元に固定された座標系
【産業上の利用分野】
本願発明は、工場における省力化、自動化等に利用される産業用ロボット（以下、単にロボットと言う。）の制御技術に関し、更に詳しく言えば、同軸手首を有するロボットの姿勢制御時に急動作を回避する方法に関する。
【０００２】
【従来技術】
ロボットの手首側の３軸が図１に示したような軸構成を有している場合、これを同軸手首と言い、また、同軸手首を備えたロボットを同軸手首ロボットと呼んでいる。図１に記したように、これらの手首３軸は最も手先側の軸から順にα軸、β軸、γ軸と呼ばれ、各軸の回転移動方向及び向き（移動位置を表わす軸変数の正負）は同図に示した通りである。なお、以下α，β，γは各軸の角度位置（「軸値」とも言う。）を表わす記号としても使用される。
【０００３】
このような同軸手首ロボットの姿勢制御動作時には手首形態を決定する必要が生じる。これは、ロボットの姿勢を完全に制御しても、逆変換の方程式を解く過程で、軸値の組（α，β，γ）が必ずしも一意的に定まらないことによる。手首形態を完全に定める為には手首３軸の内、β軸の値の符号を指定してやれば良い事が知られており、図１に併記したように、βに正値をとらせた状態を「フリップ」と呼び、βに負値をとらせた状態を「ノンフリップ」と呼んでいる。
【０００４】
従来、このような同軸手首ロボットに特異姿勢（β＝０の姿勢）近傍で姿勢制御動作を行なわせると、α及びγの値が隣合う補間点で急変する現象が生じていた。この現象を図２〜図５を用いて説明する。
【０００５】
先ず図２は、α＝β＝γ＝０の姿勢を示したものである。図中Σwrは手首の根元に固定された軸ｘｙｚを持つ座標系である。また、＜ｖ＞は手首先端に固定されたベクトルを表わしている。α＝β＝γ＝０の姿勢において、ベクトル＜ｖ＞の方向は座標系Σwrの＋ｚ軸方向と一致するものとする。
【０００６】
これら座標系Σwr及びベクトル＜ｖ＞は、任意の手首形態におけるα，β，γの値を記述する（測る）基準となるものである。α，β，γの値は、図３に示した態様で測られる。図３（１），（２）はα及びγの測り方を、β＞０の場合とβ＜０の場合に分けて表わしている。
【０００７】
図中、直線ｇ1 はβ軸の原点とα軸の原点を通る直線に垂直な直線の内、ｘ軸を通るものを表わしている。また、直線ｇ2 はα軸の原点を通りｚ軸に平行な直線である。そして、直線ｇ3 はα軸の原点からｘ軸に降ろした垂線である。
【０００８】
これらの事項を前提に、図４に示された姿勢制御動作について考察してみる。図４は、従来方式で制御を行なった場合に急動作を起こす姿勢制御動作の一例を表わしている。このケースでは、手首先端が曲線ＰＱ上を矢印方向に動き、その間ベクトル＜ｖ＞の方向は座標系Σwrのｚｘ平面に平行に保たれる。このような動作においては、曲線ＰＱ上を通る間に、図２に示した特異姿勢あるいはその近傍を通過することになる。
【０００９】
従来方式では、このような場合でも、「フリップ」の状態あるいは「ノンフリップ」の状態を維持するような処理が行なわれている。その結果、α及びγについて、特異姿勢あるいはその近傍を通過する際に値の急激な変化が避けられなかった。図５はこれを説明する図で、（１）はγの急変を、（２）はαの急変を表わしている。このような急動作は、β値の符号を維持しようとすることに起因して引き起こされるものと考えることが出来る。
【００１０】
そこで、本発明者は既に特異姿勢通過時に、フリップ／ノンフリップの反転を行なう手首形態の決定方法を提案した（特願平７−２３３３５号に添付された明細書及び図面を参照）。図４に示したケースにおいて、曲線ＰＱが特異姿勢を正確に通過、あるいは特異姿勢を通過したとみなせるほどの極く近傍を通過する場合（以下、単に「特異姿勢を通過」と言う。）には、上記提案された方法を適用することによって、急動作は回避される。
【００１１】
【発明が解決しようとする課題】
しかし、図４に示したケースにおいて、曲線ＰＱが特異姿勢の近傍は通過するが特異姿勢は通過しないような場合には、上記提案の方法を適用してもフリップ／ノンフリップの反転は起らない。そして、急動作は特異姿勢のある程度の拡がりを持った範囲内で特異姿勢からの隔たりに応じた強さで発生する性質があるから、特異姿勢近傍通過時の急動作の危険は回避出来なかった。
【００１２】
そこで、本願発明の目的は、同軸手首ロボットの姿勢制御動作時について、特異姿勢の近傍を通過する際に確実に急動作を回避する方法を提供し、急動作発生時の弊害（人身の危険、軌跡精度の顕著な劣化）を未然に防止することにある。
【００１３】
【課題を解決するための手段】
本願発明は、ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作について、上記提案済みの方法が有効に適用される範囲を実質的に拡張することを通して上記課題を解決したものである。
本願発明の方法に従えば、姿勢制御動作時に各補間点について実行されるソフトウェア処理が、特異姿勢への接近を判断する処理と、該処理によって特異姿勢に接近したと判断された際には、前記特異姿勢を通過するように動作指令を修正する処理と、手首形態を決定する処理とを含んでいる。
【００１４】
手首形態を決定する処理においては、前回の補間処理における手首軸の値、例えば、β軸の軸値及びγ軸の軸値が考慮され、前記前回の補間処理で定められた補間点における手首軸の値（例えばγ値）から測った前記当該補間点における手首軸の値（例えばγ値）軸の軸値の変化量が９０°を越えないように、あるいは、同変化量が相対的に大きくない方の手首軸位置が選択されるように、前記同軸手首の形態が選択される。
【００１５】
該選択に際しては、次式、
ｐ＝（ａz × cosγ0 ）＋（ａy × sinγ0 ）、［但し、γ 0 は手首形態を決定しようとしている補間点の１つ手前の補間点のγ値、ａz，ａy は手首根元座標系から見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分］、で定義される指標を用いて各補間点における姿勢制御条件を満たす値の組（β，γ）に関し、｜γ−γ0 ｜の値が相対的に大きくない方の組が選択されるように値の組（β，γ）を順次定めるアルゴリズムを利用することが出来る。
【００１６】
また、前記特異姿勢への接近を判断する処理は、予め設定されるβ値の絶対値｜β｜の臨界値Ｋを基準として実行することが出来る。
【００１７】
【作用】
本願発明によって、α軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作について、特異姿勢近傍における急動作が回避される原理の概要は以下の通りである。
同軸ロボットの姿勢制御動作時には、各補間点で同軸手首の形態（α，β，γの値）を定める為に、一般に次の（１）式で与えられるような形の方程式を解くことが求められる。
【００１８】
【数１】
【００１９】
（ａx ，ａy ，ａz ）を既知とすれば、上記方程式（１）の解は次の（２）式または（３）式で与えられる。
【００２０】
【数２】
【００２１】
また、上記（３）式における不定値を確定する際にも、前回の補間点のγ値を採用することが出来る。前回の補間点のγ値を考慮する為の指標としては、次式（４）で定義されるｐが用いられる。
【００２２】
ｐ＝（ａz × cosγ0 ）＋（ａy × sinγ0 ） ・・・（４）
但し、β0 ，γ0 は手首形態を決定しようとしている補間点の１つ手前の補間点（前回の補間点とも言う。）のβ値及びγ値であり、ａz ，ａy は手首根元に設定された座標系Σwrから見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分である。
【００２３】
上記指標ｐを用いたアルゴリズム（詳細内容は後述）により、姿勢制御動作中に同軸手首ロボットが特異姿勢自身（β＝０とみなされる範囲）を通過した場合には、フリップ／ノンフリップを反転させる処理が行なわれ、γ軸とα軸の急激な回転が回避される。また、上記（３）式における不定値を確定する際にも、前回の補間点のγ値を採用することが出来る。
【００２４】
しかし、急動作が発生するのは、同軸手首ロボットが特異姿勢自身（β＝０とみなされる範囲）を通過した場合に限られるわけではなく、ある程度の近傍（例えばβ＝５°以内）を通過する際にもα軸、γ軸の急回転は起こり得る。そこで、本願発明では、同軸手首ロボットが特異姿勢の近傍にさしかかった段階で、動作指令を少なくとも前記特異姿勢の極く近傍を通過するように修正する処理を実行する。
【００２５】
これにより、上記手首形態決定の為の処理の中でフリップ／ノンフリップの反転が実現される。その結果、α軸、γ軸の急回転を起こすような手首形態が決定されることがなくなり、同軸手首ロボットの急動作が確実に回避される。
【００２６】
【実施例】
ここでは、図４に例示した姿勢制御動作に本願発明の方法を適用するケースについて説明する。
図６は、図４に例示した姿勢制御動作に本願発明の方法を適用した場合の動作指令の修正内容を説明する図である。
【００２７】
図４に関連して述べたように、従来方式で制御を行なった場合には、手首先端が曲線ＰＱ上（但し、破線部分Ｐ’Ｑ’を通過。実線Ｐ’Ｑ’の意味は後述。）を矢印方向に動き、その間ベクトル＜ｖ＞の方向は座標系Σwrのｚｘ平面に平行に保たれる。
【００２８】
但し、ここでは、曲線ＰＱの破線部分は図２に示した特異姿勢からある程度離れた近傍（即ち、特異姿勢とはみなされない近傍、例えばβ＝３°）を通過するものとする。このような場合に前述の提案済みの手首形態決定方法に従った処理を実行しても、フリップ／ノンフリップの反転は実現されず、特異姿勢への接近時に急動作を起こすおそれがある。
【００２９】
そこで、同軸手首ロボットが急動作を起こす危険領域へ進入する手前で、特異姿勢とみなされる範囲Ｂ（即ち、前述の提案方法で、フリップ／ノンフリップの反転を実現出来る範囲）を確実に通過するように動作軌跡が変更されるように動作指令を修正する。Ｐ’は修正開始点を表わしている。ここでは、特異姿勢を表わす点Ａへ向かう動作軌跡を実現させる為の移動指令を補間処理毎に作成する。
【００３０】
同軸手首ロボットがＰ’から点Ａに到達（厳密には最接近）する前後でフリップ／ノンフリップの反転が実現され、α軸、γ軸の急回転は回避される。動作軌跡はなるべく早く滑らかに元来のものに復帰させることが好ましいので、点Ａからは、徐々に破線Ｐ’Ｑ’に接近させ、点Ｑ’からは元来の動作軌跡に復帰させる。
【００３１】
点Ｐ’は特異姿勢への接近を判断する基準点であり、これに対応するものとして、｜β｜の臨界値Ｋを予め設定しておく。Ｋの値は設計的に定められるが、実際的な値としては、例えばＫ＝５°が考えられる。元来の動作軌跡の中から最初に｜β｜＝Ｋを満たすを与える点を捜し出すことで、動作指令修正開始時期を決定することが出来る。
【００３２】
点Ｑ’については、｜β｜＝Ｋを与える点を元来の動作軌跡の中から捜し出せば良い。元来の動作軌跡（Ｐ→Ｐ’→破線部分通過→Ｑ’→Ｑ）は教示データから随時計算可能である。例えば、動作指令修正開始後も破線部分の軌跡に対応した補間点のアプローチベクトルデータを先行的に継続し、同軸手首ロボットが点Ａに到達する前に、｜β｜＝Ｋとなる点をＱ’として定めておく。点Ｑ’のデータ（アプローチベクトルのデータ）は、動作軌跡ＡＱ’の計算に利用される。
【００３３】
以上が、本実施例における動作指令の修正内容の概要であるが、以下、その為の処理を含む処理全体について説明する。
図７は本実施例で使用されるロボット制御装置の概略構成を要部ブロック図で示したものである。同図において符号１０で表示されたロボット制御装置は、マイクロプロセッサを含む中央演算処理装置（以下、ＣＰＵという。）１１を備えている。ＣＰＵ１１にはＲＯＭからなるメモリ１２、ＲＡＭからなるメモリ１３、不揮発性メモリ１４、液晶ディスプレイ（ＬＣＤ）を備えた教示操作盤１５、サーボ回路１７を経て各軸のモータを内蔵するロボット機構部に接続されたロボット軸制御部１６が各々バス１９を介して接続されている。
【００３４】
ＲＯＭ１２には、ＣＰＵ１１がロボット本体１及びロボット制御装置１０自身の制御を行なう為の各種のプログラムが格納される。ＲＡＭ１３はデ−タの一時記憶や演算の為に利用されるメモリである。不揮発性メモリ１４には、各種設定値や動作プログラムが格納される。設定値には点Ｐ’，Ｑ’を定める為の値Ｋが含まれる。なお、ここで説明した構成は、通常のロボット制御装置と特に変わるところはない。
【００３５】
以下、図８及び図９のフローチャートを参照し、図６のＰＱ間で姿勢制御動作中に、ロボット制御装置内で実行される処理の概要を説明する。
図８のフローチャートは処理全体の流れを表わしている。先ず、元来の動作軌跡上の補間処理で求められたβの絶対値｜β｜と、設定値Ｋの大小関係を判定する（ステップＴ１）。
【００３６】
同軸手首ロボットが点Ｐ’に達する迄は、ステップＴ１の判断はＮＯであるから、ステップＴ２へ進み、元来の動作軌跡ＰＰ’に従った動作指令を作成し、ステップＴ６で手首形態を決定する処理（内容は後述）を実行する。この処理サイクル（Ｔ１→Ｔ２→Ｔ６）は、同軸手首ロボットが点Ｐ’に達する迄、補間周期毎に繰り返される。
【００３７】
同軸手首ロボットが点Ｐ’に達すると、ステップＴ１の判断はイエスに切り替わるので、ステップＴ３へ進み、特異姿勢点Ａへの未到達を確認した上でステップＴ４へ進む。ステップＴ４では、動作軌跡Ｐ’Ａに従った動作指令を作成し、ステップＴ６で手首形態を決定する処理を実行する。この処理サイクル（Ｔ１→Ｔ３→Ｔ４→Ｔ６）は、同軸手首ロボットが点Ｐ’に達する迄、補間周期毎に繰り返される。なお、この間も、元来の動作軌跡（破線Ｐ’Ｑ’）について補間点の計算自体は、点Ｑ’が求められるまで並行的に続行される（フローチャートには記載省略。動作指令はあくまで実線Ｐ’Ａに基づいて作成する）。
【００３８】
同軸手首ロボットが点Ａに達すると、ステップＴ３の判断はイエスに切り替わるので、ステップＴ５へ進み、動作軌跡ＡＱ’に従った動作指令を作成し、ステップＴ６で手首形態を決定する処理を実行する。この処理サイクル（Ｔ１→Ｔ３→Ｔ５→Ｔ６）は、同軸手首ロボットが点Ｑ’に達する迄、補間周期毎に繰り返される。
【００３９】
同軸手首ロボットが点Ｑ’に達すると、ステップＴ１の判断は再度ノーに切り替わるので、ステップＴ２へ進み、元来の動作軌跡Ｑ’Ｑに従った動作指令を作成し、ステップＴ６で手首形態を決定する処理を実行する。この処理サイクル（Ｔ１→Ｔ２→Ｔ６）は、同軸手首ロボットが点Ｑに達する迄、補間周期毎に繰り返される。
【００４０】
次にステップＴ６で実行される手首形態決定処理について図９のフローチャートを参照して説明する。この処理の内容は、前述した提案方法と実質的に同じものである（特願平７−２３３３５号に添付された明細書及び図面を参照）。
先ず、補間計算の過程で求められたアプローチベクトルのｘ成分ａx が１であるかを判断する（ステップＳ１）。もし、イエスであれば、ステップＳ８へ進み、前記（３）式に従ってβ＝０（フリップとノンフリップの境界に相当する状態）とする。また、この姿勢が特異姿勢である為に（３）式で不定とされているγについては、前回の補間点における値γ0 をそのまま採用する（ステップＳ８）。これにより、特異姿勢をとる補間点に関してもγ値の急激な変化が回避される。
【００４１】
ステップＳ１でノーであればステップＳ２へ進み、前記（２）式に従ってcos β＝ａx からβ値の候補を決定する（この段階では、β値の符号に正負の自由度が残されている）。次いで、前記（４）式で定義された指標ｐの符号を用いて前回のγ値を考慮する為の一連の処理が実行される。
【００４２】
先ず、ステップＳ３でｐ＝０であるかが判断される。ｐ≠０と判断された場合には、更にステップＳ４で正負の符号が判定される。もし、ｐ＞０と判定された場合には、ステップ６へ進み、前記（２）式における±符号の内＋符号を選択して、β値及びγ値を決定する。β値については、ステップＳ２で求められた正負２つの候補値の内、正値のものが選択されることになる（フリップの選択）。
【００４３】
ステップＳ４で、ｐ＜０と判定された場合には、ステップＳ７へ進み、前記（２）式における±符号の内−符号を選択して、β値及びγ値を決定する。β値については、ステップＳ２で求められた正負２つの候補値の内、負値のものが選択されることになる（ノンフリップの選択）。
【００４４】
ステップＳ３でｐ＝０であると判断された場合には、ステップＳ５で前回の補間点におけるβ値自体の符号が参照される。もし、β0 ≧０と判断された場合にはステップ６へ進み、ｐ＞０の場合と同様に、前記（２）式における±符号の内＋符号を選択して、β値及びγ値を決定する。
【００４５】
また、β0 ＜０と判断された場合にはステップＳ７へ進み、ｐ＜０の場合と同様に、前記（２）式における±符号の内−符号を選択して、β値及びγ値を決定する。
ステップＳ６、ステップＳ７、ステップＳ８のいずれかの完了後にその補間点に関する処理が終了される。このように、図５のフローチャートに示したアルゴリズムを用いることによって、すべてのケースについてβ値及びγ値が定められる。
【００４６】
上記アルゴリズムにおける指標ｐの符号の考慮は、（２）式におけるベクトル（ cosγ， sinγ）の正負の反対符号を持つ２解の内、前回の補間点におけるベクトル（ cosγ0 ， sinγ0 ）から遠くない方の解を選択していることに他ならない。従って、どのようなケースにおいても｜γ−γ0 ｜が９０°を上回ることが原理的に起こり得ない。
【００４７】
図６に示されたケースについてフリップ／ノンフリップの選択状況を考えてみる。フリップ状態（β＞０）で姿勢制御が開始された直後の補間点では、明らかに、ステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４を経てステップＳ６が実行される。区間ＰＰ’では、β＝０となることがないから、このサイクルが毎回繰り返される。
【００４８】
区間Ｐ’Ａに入っても、しばらくはβ＝０（ａx ＝１）となることがないので、同様にステップＳ１→ステップＳ２→ステップＳ３→ステップＳ４→ステップＳ６の処理サイクルが繰り返される。しかし、ａx が１に近づくに従い（図６の事例では、ワーク座標系上で見たツール姿勢は変化しないが手首根元座標系Σwrが変化することに注意）、βが正値側から徐々に０に近づく。
【００４９】
特異姿勢点Ａの付近で、ａx ＝１（手首根元座標系Σwrの＋ｚ軸の方向とベクトル＜ｖ＞の方向が一致）の判定が出された場合には、その補間点ではステップＳ１→ステップＳ８が実行される。
【００５０】
その直後の補間点では再びステップＳ１からステップＳ２を経てステップＳ３へ進む。ステップＳ３における判定基準値にもよるが、特異姿勢からの姿勢変化が余り小さくない限り判断結果はノーとなる。もし、イエスとされた場合には、前回のβの値β0 の符号を直接確認した上で、正であればフリップ状態を選択し、負であればノンフリップ状態を選択する。
【００５１】
ｐ＝０の判定がなされた場合でも、やがてロボット姿勢は特異姿勢から離れる方向に変化するので、次回の補間点以降でステップＳ３でイエスの判定が出され続けることはない。
【００５２】
最初にステップＳ３またはステップＳ５でノーの判断が出されると、ステップＳ７が初めて実行され、ノンフリップが選択される。これにより、特異姿勢通過時のγ値のジャンプが回避される。
【００５３】
もし、特異姿勢通過前後を通してフリップ状態を維持した場合を仮定すると、（２）式の形から判るように、特異姿勢通過前後でａy 及びａz の符号が反転するので、最悪の場合γを１８０°変化させなければならなくなる。
【００５４】
本処理では、特異姿勢通過前後で起るａy 及びａz の符号の反転時にβ値の正負を反転させるので、γ値の変化は最悪の場合でも９０°を越えることはない。図１０（１），（２）は、本実施例で急動作が回避される状況を図示したものである。図１０（１）にはγ値の変化が小さいことが描かれ、図１０（２）にはα値の変化が小さいことが描かれている。図中βP ，βQ は各々点Ｐ，Ｑにおけるβ値を表わしている。特異姿勢通過時にフリップ／ノンフリップの反転が実行されるので、βP ，βQ は異符号である。従って、図５（１），（２）の場合のように、γ値及びα値の変化が大きくなることが避けられる。
【００５５】
なお、特異姿勢の前後におけるγ値の変化が極く僅かであれば、姿勢制御動作が滑らかなものである限り、α値の変化も小さくなることは、β値は元々特異姿勢付近で急変しないことに注意すれば、直感的にも明らかである。
【００５６】
結局、図４に示された姿勢制御の事例に本願発明の方法を適用した場合には、図１０（１），（２）に示したように、図６に示したような若干の動作軌跡の修正を伴う形で、同軸手首ロボットの急動作が確実に回避される。これは、もし本願発明の方法を適用しなかった場合に予測される急動作発生の事態に比べれば（図６の破線区間Ｐ’Ｑ’の途上で発生が予測される。）、作業の安全上、遥かに好ましいことである。
【００５７】
以上、図４に示された姿勢制御の事例に本願発明の方法を適用した場合について説明したが、ここで述べた手法、特に処理内容は他の姿勢制御の事例に関しても、基本的に適用可能なもあのである。
【００５８】
【発明の効果】
本願発明によれば、同軸手首ロボットの特異姿勢近傍における姿勢制御動作について、姿勢の指令値を若干変更し、特異姿勢通過時にフリップ／ノンフリップの反転を起こさせる手首形態決定処理を実行するだけで、α軸、γ軸の急激な回転を伴う急動作を回避することが出来る。従って、同軸手首ロボットに安全に姿勢制御動作を行なわせることが出来る範囲が拡張される。
【図面の簡単な説明】
【図１】同軸手首の軸構成について説明する図である。
【図２】同軸手首ロボットの特異姿勢について説明する図である。
【図３】同軸手首の軸の角度位置の測り方を説明する図である。（１）はαの測り方、（２）はγの測り方を、各々βの符号に関連させて示している。
【図４】同軸手首ロボットの姿勢制御動作の一例を説明する図である。
【図５】図４に示した姿勢制御動作を従来方式で実行した場合に発生する急動作について説明する図である。（１）にはγ値の急変が描かれ、（２）にはα値の急変が描かれている。
【図６】図４に示した姿勢制御動作に本願発明の方法を適用した場合の動作軌跡を説明する図である。
【図７】実施例で使用されるロボット制御装置の概略構成を要部ブロック図で示したものである。
【図８】実施例における処理全体の流れを説明する為のフローチャートである。
【図９】実施例で実行される手首形態決定処理について説明する為のフローチャートである。
【図１０】図４に示した姿勢制御動作に本願発明の方法を適用した場合に急動作が回避される状況を説明する図である。（１）にはγ値の急変回避が描かれ、（２）にはα値の急変回避が描かれている。
【符号の説明】
１ ロボット（同軸手首ロボットの機構部）
１０ ロボット制御装置
１１ 中央演算処理装置（ＣＰＵ）
１２ ＲＯＭ
１３ ＲＡＭ
１４ 不揮発性メモリ
１５ 教示操作盤
１６ ロボット軸制御部
１７ サーボ回路
１９ バス
Ｐ〜Ｑ 姿勢制御動作が行なわれる経路区間
Ｐ’ 姿勢の指令の修正を開始する点
Ｑ’ 姿勢の指令の修正を終了する点
Ａ 特異姿勢点
Ｂ 特異姿勢とみなされる範囲
Σwr 手首根元に固定された座標系
Claims (5)
- ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作における急動作回避方法であって、
前記姿勢制御動作時に各補間点について実行されるソフトウェア処理が、
特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には前記特異姿勢を通過するように動作指令を修正する処理と、
手首形態を決定する処理を含み、
前記手首形態を決定する処理において、前回の補間処理における手首軸値が考慮され、前記前回の補間処理で定められた補間点におけるα値並びにγ値から、前記当該補間点におけるα軸の軸値並びにγ軸の軸値への変化量が９０゜を越えないように前記同軸手首の形態が選択される、前記同軸手首ロボットの姿勢制御動作における急動作回避方法。 - ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作における急動作回避方法であって、
前記姿勢制御動作時に各補間点について実行されるソフトウェア処理が、
特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には前記特異姿勢を通過するように動作指令を修正する処理と、
手首形態を決定する処理を含み、
前記手首形態を決定する処理において、前回の補間処理における手首軸値が考慮され、前記前回の補間処理で定められた補間点におけるγ値から、前記当該補間点におけるγ軸の軸値への変化量が９０゜を越えないように前記同軸手首の形態が選択される、前記同軸手首ロボットの姿勢制御動作における急動作回避方法。 - ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作における急動作回避方法であって、
前記姿勢制御動作時に各補間点について実行されるソフトウェア処理が、
特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には前記特異姿勢を通過するように動作指令を修正する処理と、
手首形態を決定する処理を含み、
前記手首形態を決定する処理において、前回の補間処理におけるβ軸の軸値とγ軸の軸値が考慮され、前記前回の補間処理で定められた補間点におけるγ値から、前記当該補間点におけるγ軸の軸値への変化量が９０゜を越えないように前記同軸手首の形態が選択される、前記同軸手首ロボットの姿勢制御動作における急動作回避方法。 - ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作における急動作回避方法であって、
前記姿勢制御動作時に各補間点について実行されるソフトウェア処理が、
特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には前記特異姿勢を通過するように動作指令を修正する処理と、
手首形態を決定する処理を含み、
前記手首形態を決定する処理において、前回の補間処理における手首軸値が考慮され、前記前回の補間処理で定められた補間点におけるα値及びγ値から、前記当該補間点におけるα軸の軸値並びにγ軸の軸値への変化量が相対的に大きくない方の手首軸位置が選択されるように前記同軸手首の形態が選択される、前記同軸手首ロボットの姿勢制御動作における急動作回避方法。 - ソフトウェア処理能力を有するロボット制御装置によって制御されるとともに手先側から順にα軸、β軸、γ軸の３軸で構成される同軸手首を備えたロボットの姿勢制御動作における急動作回避方法であって、
前記姿勢制御動作時に各補間点について実行されるソフトウェア処理が、
特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には前記特異姿勢を通過するように動作指令を修正する処理と、
手首形態を決定する処理を含み、
前記手首形態を決定する処理において、前回の補間処理におけるβ軸の軸値及びγ軸の軸値が考慮され、前記前回の補間処理で定められた補間点におけるγ値から測った前記当該補間点におけるγ軸の軸値の変化量が９０°を越えないように前記同軸手首の形態が選択され、
該選択が次式、
ｐ＝（ａz × cosγ0 ）＋（ａy × sinγ0 ）、［但し、γ 0 は手首形態を決定しようとしている補間点の１つ手前の補間点のγ値、ａz，ａy は手首根元座標系から見たツール姿勢を表わす行列の一部をなすアプローチベクトルのｚ成分並びにｙ成分］、
で定義される指標を用いて各補間点における姿勢制御条件を満たす値の組（β，γ）に関し、｜γ−γ0 ｜の値が相対的に大きくない方の組が選択されるように値の組（β，γ）を順次定めるアルゴリズムを利用して行なわれる前記同軸手首ロボットの姿勢制御動作における急動作回避方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP06894995A JP3545486B2 (ja) | 1995-03-03 | 1995-03-03 | 同軸手首ロボットの姿勢制御動作における急動作回避方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP06894995A JP3545486B2 (ja) | 1995-03-03 | 1995-03-03 | 同軸手首ロボットの姿勢制御動作における急動作回避方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH08241108A JPH08241108A (ja) | 1996-09-17 |
| JP3545486B2 true JP3545486B2 (ja) | 2004-07-21 |
Family
ID=13388432
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP06894995A Expired - Fee Related JP3545486B2 (ja) | 1995-03-03 | 1995-03-03 | 同軸手首ロボットの姿勢制御動作における急動作回避方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3545486B2 (ja) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4494247B2 (ja) * | 2005-02-15 | 2010-06-30 | 三菱電機株式会社 | ロボットの制御装置および制御方法 |
| JP5283541B2 (ja) * | 2009-03-05 | 2013-09-04 | 株式会社神戸製鋼所 | ロボットの動作経路生成方法 |
-
1995
- 1995-03-03 JP JP06894995A patent/JP3545486B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH08241108A (ja) | 1996-09-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP1696294A2 (en) | Method and apparatus for generating teaching data for a robot | |
| CN101901281B (zh) | 消耗电力估计装置 | |
| EP2835228B1 (en) | Robot apparatus and robot controlling method | |
| EP3566822A1 (en) | Robot joint space point-to-point movement trajectory planning method | |
| CN105855672B (zh) | 基于示教机器人的空间圆弧插补焊接方法 | |
| US10507585B2 (en) | Robot system that displays speed | |
| US20150177730A1 (en) | Robot, robot control method and robot control program | |
| CN108687758B (zh) | 机器人的移动速度控制装置及方法 | |
| EP3088142B1 (en) | Operation program creating method and control method of robot | |
| JPH07261821A (ja) | 負荷による撓みを考慮したロボット軌道計画方法 | |
| US10532460B2 (en) | Robot teaching device that sets teaching point based on motion image of workpiece | |
| CN113084792A (zh) | 关节奇异区域的确定方法、机器人及存储装置 | |
| JPH04111006A (ja) | ロボットの経路補間方法 | |
| JP3053501B2 (ja) | ロボットの位置教示方法 | |
| JP3545486B2 (ja) | 同軸手首ロボットの姿勢制御動作における急動作回避方法 | |
| CN112356032B (zh) | 一种姿态平滑过渡方法及系统 | |
| JP3580885B2 (ja) | 同軸手首ロボットの姿勢制御動作における手首形態決定方法 | |
| CN117555293A (zh) | 一种运动控制器的控制方法、系统、电子设备及存储介质 | |
| US12269165B2 (en) | Robot control apparatus, robot control system, robot control method, and computer-readable storage medium storing a robot control program | |
| JP6000496B1 (ja) | 数値制御装置 | |
| JP2001328092A (ja) | ロボットの干渉回避装置及び方法 | |
| JP3000101B2 (ja) | 軌跡制御補間方法 | |
| JP3146550B2 (ja) | 産業用ロボットの制御装置 | |
| JPH0732279A (ja) | ロボット位置教示方式 | |
| JP2752784B2 (ja) | ロボットの姿勢修正方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20040309 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20040408 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080416 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090416 Year of fee payment: 5 |
|
| LAPS | Cancellation because of no payment of annual fees |