JPH08241108A

JPH08241108A - 同軸手首ロボットの姿勢制御動作における急動作回避方法

Info

Publication number: JPH08241108A
Application number: JP6894995A
Authority: JP
Inventors: Hidetoshi Kumitani; 英俊組谷
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1995-03-03
Filing date: 1995-03-03
Publication date: 1996-09-17
Anticipated expiration: 2019-07-21
Also published as: JP3545486B2

Abstract

(57)【要約】【目的】同軸手首ロボットの特異姿勢近傍における姿
勢制御動作について、急動作を回避すること。【構成】 α軸、β軸、γ軸で構成される同軸手首を有
するロボットについて、点Ｐから点Ｑまで滑らかに姿勢
制御動作を行なわせる。急動作を起こす危険領域へ進入
する手前Ｐ’から、特異姿勢とみなされる範囲Ｂを確実
に通過するような動作軌跡がＰ’Ａへ変更されるように
姿勢を決める動作指令を修正する。Ａは特異姿勢点を表
わす。同軸手首ロボットがＰ’から点Ａに到達する前後
では、フリップ／ノンフリップを反転させるように手首
形態決定処理が実行され、α軸、γ軸の急回転は回避さ
れる。点Ａからは、徐々に破線Ｐ’Ｑ’に接近させ、点
Ｑ’からは元来の動作軌跡に復帰させる。特異姿勢への
接近（Ｐ’への到達）の判断は、予め設定された｜β｜
の臨界値Ｋと元来の動作軌跡上の補間点におけるβ値と
の比較に基づいて行なわれる。手首形態決定処理では、
前回の補間点の（β，γ）値を考慮する為の指標を用い
たアルゴリズムが利用される。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本願発明は、工場における省力
化、自動化等に利用される産業用ロボット（以下、単に
ロボットと言う。）の制御技術に関し、更に詳しく言え
ば、同軸手首を有するロボットの姿勢制御時に急動作を
回避する方法に関する。

【０００２】

【従来技術】ロボットの手首側の３軸が図１に示したよ
うな軸構成を有している場合、これを同軸手首と言い、
また、同軸手首を備えたロボットを同軸手首ロボットと
呼んでいる。図１に記したように、これらの手首３軸は
最も手先側の軸から順にα軸、β軸、γ軸と呼ばれ、各
軸の回転移動方向及び向き（移動位置を表わす軸変数の
正負）は同図に示した通りである。なお、以下α，β，
γは各軸の角度位置（「軸値」とも言う。）を表わす記
号としても使用される。

【０００３】このような同軸手首ロボットの姿勢制御動
作時には手首形態を決定する必要が生じる。これは、ロ
ボットの姿勢を完全に制御しても、逆変換の方程式を解
く過程で、軸値の組（α，β，γ）が必ずしも一意的に
定まらないことによる。手首形態を完全に定める為には
手首３軸の内、β軸の値の符号を指定してやれば良い事
が知られており、図１に併記したように、βに正値をと
らせた状態を「フリップ」と呼び、βに負値をとらせた
状態を「ノンフリップ」と呼んでいる。

【０００４】従来、このような同軸手首ロボットに特異
姿勢（β＝０の姿勢）近傍で姿勢制御動作を行なわせる
と、α及びγの値が隣合う補間点で急変する現象が生じ
ていた。この現象を図２〜図５を用いて説明する。

【０００５】先ず図２は、α＝β＝γ＝０の姿勢を示し
たものである。図中Σwrは手首の根元に固定された軸ｘ
ｙｚを持つ座標系である。また、＜ｖ＞は手首先端に固
定されたベクトルを表わしている。α＝β＝γ＝０の姿
勢において、ベクトル＜ｖ＞の方向は座標系Σwrの＋ｚ
軸方向と一致するものとする。

【０００６】これら座標系Σwr及びベクトル＜ｖ＞は、
任意の手首形態におけるα，β，γの値を記述する（測
る）基準となるものである。α，β，γの値は、図３に
示した態様で測られる。図３（１），（２）はα及びγ
の測り方を、β＞０の場合とβ＜０の場合に分けて表わ
している。

【０００７】図中、直線ｇ1 はβ軸の原点とα軸の原点
を通る直線に垂直な直線の内、ｘ軸を通るものを表わし
ている。また、直線ｇ2 はα軸の原点を通りｚ軸に平行
な直線である。そして、直線ｇ3 はα軸の原点からｘ軸
に降ろした垂線である。

【０００８】これらの事項を前提に、図４に示された姿
勢制御動作について考察してみる。図４は、従来方式で
制御を行なった場合に急動作を起こす姿勢制御動作の一
例を表わしている。このケースでは、手首先端が曲線Ｐ
Ｑ上を矢印方向に動き、その間ベクトル＜ｖ＞の方向は
座標系Σwrのｚｘ平面に平行に保たれる。このような動
作においては、曲線ＰＱ上を通る間に、図２に示した特
異姿勢あるいはその近傍を通過することになる。

【０００９】従来方式では、このような場合でも、「フ
リップ」の状態あるいは「ノンフリップ」の状態を維持
するような処理が行なわれている。その結果、α及びγ
について、特異姿勢あるいはその近傍を通過する際に値
の急激な変化が避けられなかった。図５はこれを説明す
る図で、（１）はγの急変を、（２）はαの急変を表わ
している。このような急動作は、β値の符号を維持しよ
うとすることに起因して引き起こされるものと考えるこ
とが出来る。

【００１０】そこで、本発明者は既に特異姿勢通過時
に、フリップ／ノンフリップの反転を行なう手首形態の
決定方法を提案した（特願平７−２３３３５号に添付さ
れた明細書及び図面を参照）。図４に示したケースにお
いて、曲線ＰＱが特異姿勢を正確に通過、あるいは特異
姿勢を通過したとみなせるほどの極く近傍を通過する場
合（以下、単に「特異姿勢を通過」と言う。）には、上
記提案された方法を適用することによって、急動作は回
避される。

【００１１】

【発明が解決しようとする課題】しかし、図４に示した
ケースにおいて、曲線ＰＱが特異姿勢の近傍は通過する
が特異姿勢は通過しないような場合には、上記提案の方
法を適用してもフリップ／ノンフリップの反転は起らな
い。そして、急動作は特異姿勢のある程度の拡がりを持
った範囲内で特異姿勢からの隔たりに応じた強さで発生
する性質があるから、特異姿勢近傍通過時の急動作の危
険は回避出来なかった。

【００１２】そこで、本願発明の目的は、同軸手首ロボ
ットの姿勢制御動作時について、特異姿勢の近傍を通過
する際に確実に急動作を回避する方法を提供し、急動作
発生時の弊害（人身の危険、軌跡精度の顕著な劣化）を
未然に防止することにある。

【００１３】

【課題を解決するための手段】本願発明は、ソフトウェ
ア処理能力を有するロボット制御装置によって制御され
るとともに手先側から順にα軸、β軸、γ軸の３軸で構
成される同軸手首を備えたロボットの姿勢制御動作につ
いて、上記提案済みの方法が有効に適用される範囲を実
質的に拡張することを通して上記課題を解決したもので
ある。本願発明の方法に従えば、姿勢制御動作時に各補
間点について実行されるソフトウェア処理が、特異姿勢
への接近を判断する処理と、該処理によって特異姿勢に
接近したと判断された際には少なくとも前記特異姿勢の
極く近傍を通過するように動作指令を修正する処理と、
手首形態を決定する処理とを含んでいる。

【００１４】手首姿勢を決定する処理においては、前回
の補間処理におけるβ軸の軸値及びγ軸の軸値が考慮さ
れ、前記前回の補間処理で定められた補間点におけるγ
値から測った前記当該補間点におけるγ軸の軸値の変化
量が９０°を越えないように前記同軸手首の形態が選択
される。

【００１５】該選択に際しては、次式、ｐ＝（ａ_z × c
osγ0 ）＋（ａ_y × sinγ0 ）、［但し、β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 手首根元に固定された座標系

Claims

【特許請求の範囲】

【請求項１】ソフトウェア処理能力を有するロボット
制御装置によって制御されるとともに手先側から順にα
軸、β軸、γ軸の３軸で構成される同軸手首を備えたロ
ボットの姿勢制御動作における急動作回避方法であっ
て、前記姿勢制御動作時に各補間点について実行されるソフ
トウェア処理が、特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には
少なくとも前記特異姿勢の極く近傍を通過するように動
作指令（動作指令の手首姿勢関連部分。以下、同様。）
を修正する処理と、手首形態を決定する処理を含み、前記手首姿勢を決定する処理において、前回の補間処理
におけるβ軸の軸値及びγ軸の軸値が考慮され、前記前
回の補間処理で定められた補間点におけるγ値から測っ
た前記当該補間点におけるγ軸の軸値の変化量が９０°
を越えないように前記同軸手首の形態が選択される、前
記同軸手首ロボットの姿勢制御動作における急動作回避
方法。
【請求項２】ソフトウェア処理能力を有するロボット
制御装置によって制御されるとともに手先側から順にα
軸、β軸、γ軸の３軸で構成される同軸手首を備えたロ
ボットの姿勢制御動作における急動作回避方法であっ
て、前記姿勢制御動作時に各補間点について実行されるソフ
トウェア処理が、特異姿勢への接近を判断する処理と、
該処理によって特異姿勢に接近したと判断された際には
少なくとも前記特異姿勢の極く近傍を通過するように動
作指令を修正する処理と、手首形態を決定する処理を含
み、前記手首姿勢を決定する処理において、前回の補間処理
におけるβ軸の軸値及びγ軸の軸値が考慮され、前記前
回の補間処理で定められた補間点におけるγ値から測っ
た前記当該補間点におけるγ軸の軸値の変化量が９０°
を越えないように前記同軸手首の形態が選択され、該選択が次式、ｐ＝（ａ_z × cosγ0 ）＋（ａ_y × sinγ0 ）、［但
し、β0 ，γ0 は手首形態を決定しようとしている補間
点の１つ手前の補間点のβ値並びにγ値、ａ_z ，ａ_y は
手首根元座標系から見たツール姿勢を表わす行列の一部
をなすアプローチベクトルのｚ成分並びにｙ成分］、で定義される指標を用いて各補間点における姿勢制御条
件を満たす値の組（β，γ）に関し、｜γ−γ0 ｜の値
が相対的に大きくない方の組が選択されるように値の組
（β，γ）を順次定めるアルゴリズムを利用して行なわ
れる前記同軸手首ロボットの姿勢制御動作における急動
作回避方法。
【請求項３】前記特異姿勢への接近を判断する処理
が、予め設定されるβ値の絶対値｜β｜の臨界値Ｋを基
準として実行される請求項１または請求項２に記載され
た同軸手首ロボットの姿勢制御動作における急動作回避
方法。