JPH0836411A - ロボット - Google Patents

Abstract

(57)【要約】 【目的】本発明の目的は、ロボットアームに構成された
複数の動作軸（自由度）をそれぞれ単独に動作させるこ
と可能にし、制御上柔軟性の高いロボットアームの制御
方法を提供することにある。 【構成】ロボットアームの制御装置１に、動作指令部及
びサーボ制御部を含む演算手段１５と、動作軸を駆動す
るパワー信号発生手段１２とを備え、前記サーボ制御部
を、サーボ系が安定に動作するサンプリングレートＴｓ
をロボットアームの動作軸数Ｎで除した時間Ｔｓ／Ｎよ
りも短かい一定周期Ｔｓmで起動される割込演算部と
し、かつ、起動される毎に循環的にロボットアームの動
作軸のうちの１軸のみのサーボ制御を実行し、Ｎ回の起
動によってロボットアームの全動作軸の１回のサーボ制
御を完了するようにする。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットアームの動作
制御のための演算処理方式に係り、特に演算能力の十分
でない演算手段を用いてロボットアームの動作制御をす
るに好適な演算処理方式に関する。

【０００２】

【従来の技術】ロボットアームの制御には、主としてマ
イクロコンピュータのような、演算能力の十分でない演
算手段が用いられる。この際の演算処理内容は、次のよ
うに大別することができる。

【０００３】（１）予め与えられているロボットアーム
の動作プログラムを順次呼び出し、解析処理するシーケ
ンス処理。

【０００４】（２）シーケンス処理の結果、動作命令が
見出だされたときに、動作命令により規定される動作条
件、及び動作点データをもとに、ロボットアームの動作
仕様を決定する軌道計画処理。

【０００５】（３）軌道計画に基づきロボットアームの
動作点を補間演算により求め、移動位置指令値であるサ
ーボ制御用の入力信号に変換する補間演算処理。

【０００６】（４）サーボ制御用の入力信号とロボット
アームを駆動するアクチュエータからフィードバックさ
れる信号とにもとづき所定のサーボ制御演算を行ない、
前記アクチュエータを駆動する制御信号を生成出力する
サーボ制御演算処理。

【０００７】初期のロボットアームの制御においては、
上記（４）は、サーボ制御用のアナログの外部回路とし
て構成されていた。従ってロボットアームを滑らかに連
続して動作させるために、一定のサンプリングレート、
特に演算手段がコンピュータであり逐次演算により上記
（１）〜（３）を実行するようなものにおいては、コン
ピュータの割込機能を用いて、定時的に割込を発生さ
せ、上記（１）〜（３）で得られた移動位置指令値をア
ナログの外部回路に与えていた（特公昭６１−２９６４
号公報）。

【０００８】上記外部回路は、当然ながら一定の遅れ要
素を含んでおり、従って、外部回路への上記移動位置指
令値の提供は、それ程短い周期である必要がなかった。
例えば５０msecに１回程度の割合で良かった。また、前
記定時割込処理部は、上記位置指令値を外部回路に出力
するのみであり、演算所要時間は、無視できる程度であ
った。従ってコンピュータ等の演算手段は、ほとんど、
上記（１）〜（３）の演算処理に専念でき、その演算所
要時間は例えば３０msec程度であり、ロボットの動作制
御上、問題にならなかった。

【０００９】その後、計算機技術の進歩、特にマイクロ
コンピュータ技術の進歩により、前記外部回路を、演算
手段の演算処理に置換する、すなわち、ソフトウェアサ
ーボ化が可能な状勢が生まれてきた。

【００１０】このように、外部回路をソフトウェアで置
換し、ソフトウェアでアクチュエータの駆動信号を直接
生成し、アクチュエータを動作させるようになると、前
記外部回路における遅れ要素がなくなるから、アクチュ
エータ系の安定かつ円滑な動作制御のために、前記
（４）の演算は短い周期、例えば定時的に２msec毎の定
時的演算で実行しなければならなくなる。

【００１１】これは、アクチュエータからのフィードバ
ックループを含むマイナで高速の制御ループをソフトウ
ェアに置換する場合の当然の帰結であり、サンプリング
制御理論からも明らかである。

【００１２】このようにして、従来外部回路で実行され
ていた制御理論を演算手段内部にとり込み、理論演算で
実行するソフトウェアサーボ方式とする場合、定時処理
であるサーボ制御のサンプリングレートの高周期化と共
に、サーボ制御演算負荷が増大するために、前記した
（１）〜（３）の演算をサーボ処理周期に同期させて実
行することはほとんど不可能になる。

【００１３】このような難点を回避するものの一例とし
て、特開昭５８−５８６１０号公報に開示された方法が
ある。そこでは、定時処理であるサーボ制御演算の空き
時間を利用して、前記（１）〜（３）の演算を実行し、
得られた位置指令値をＮ分割してバッファに設定し、こ
れを定期処理であるサーボ制御演算部がサーボ制御の指
令値として順次読み出しこれを用いてサーボ制御を実行
しているので、実質上の前記（１）〜（３）の演算量が
減少することになる。

【００１４】

【発明が解決しようとする課題】上記公知例には、次の
点が考慮されていない。

【００１５】（i）ロボットアームを等速直線運動させ
る場合について詳細に開示している。この場合は、前記
（２）の演算負荷は、ほとんどないが、ロボットアーム
を起動停止させる場合に急加速、急減速することにな
り、ロボットアームに振動が発生したり、駆動系に過大
な負荷がかかり、望ましいものではない。そのために
は、適切なる加減速制御方式を導入しなければらない
が、そのための演算が必要である。

【００１６】（ii）ロボットアームを直線及び円弧を連
ねた線路上を連続して動作させ、かつ線路上の教示点の
コーナ部で急激な動作方向変動などによる悪影響を避け
るためにコーナ近傍を円滑に動作させることが要求され
るような場合には、前記（２）の演算負荷は相当なもの
になる。従って、前記（３）の演算に加えて、前記
（２）の演算が、前記サーボ制御演算の空き時間を利用
してできないならば、前記（３）の演算のためのデータ
が供給されなくなり、その結果、サーボ制御のために位
置指令値が供給されなくなり、ロボットアームは制御不
能になってしまう。

【００１７】（iii）サーボ制御演算は定期処理であ
り、前記バッファに指令される位置指令値に応答して動
作する。従って、ロボットアーム動作中はもちろんであ
るが、停止中においても、前記バッファに位置指令値が
供給されていなければならず、そのための演算負荷が存
在する。

【００１８】（iv）前記バッファに供給される指令値に
より、ロボットアームの全自由度は常に協調して動作す
る。従って、ロボットアームの１部の自由度のみを単独
で動作させたい場合、自由度の１部毎を非同期で動作さ
せたいような場合に対処することが困難であり、そのた
めの演算負荷が増加する。

【００１９】（v）ロボットアームの全自由度は、１ヶ
の定時処理割込で起動されるサーボ制御プログラムで同
時に処理されているから、上記の問題に対する対処が困
難であり、制御上の柔軟性に欠ける。また、さらに、ロ
ボットアームの制御装置を用いて、全く独立な外部装置
をサーボ制御するような場合にも対処することが困難で
ある。

【００２０】本発明は、上記の難点を解消するためにな
されたものである。

【００２１】

【課題を解決するための手段】上記目的を達成するため
の手段は、以下のようである。

【００２２】（ａ）マイクロコンピュータ等の演算手段
の備える内部タイマもしくは、外部回路によって作成さ
れる外部タイマによって定時的に起動される割込処理プ
ログラムにより、ロボットアームの各自由度のサーボ制
御演算を行なう。

【００２３】この場合のタイマー設定値は、ロボットア
ームのサーボ制御系が安定に動作するサンプリングタイ
ムＴｓを、ロボットアームの動作自由度Ｎで割ったも
の、もしくは、ロボットアーム以外の外部装置のサーボ
制御を独立または同期して動作させる場合は、（上記ロ
ボットアームの動作自由度＋付加自由度）で割ったもの
よりも小さい値Ｔｓmとする。

【００２４】（ｂ）サーボ制御プログラムは、前記定時
割込により起動されたとき、前記自由度のうちの１個の
みについてのサーボ制御演算を行ない、該当する自由度
の駆動信号を生成し、該当する自由度を動作させる。

【００２５】（ｃ）サーボ制御プログラムは、前記定時
割込により起動される毎に、循環的にサーボ制御演算を
実行する自由度を切り換えてゆく。従って、複数回の定
時割込の起動により、前記自由度について１回のサーボ
制御演算を完了する。このための全所要時間は、前記サ
ンプリングタイムＴｓより短かいから、ロボットアーム
は、システムとして安定に動作する。

【００２６】（ｄ）サーボ制御プログラムには、複数の
処理モードを設け、ロボットの動作自由度を制御するた
めの関節角またはエンコーダ値指令に応答して、指定さ
れた処理モードの補間アルゴリズムにより前記指令値を
補間して、サーボ制御目標値を算出し、サーボ制御目標
値に対してサーボ制御演算を実行させる。

【００２７】（ｅ）ロボットアームを動作させるために
は、動作させる自由度についての位置（関節角／エンコ
ーダ値）指令値、補間演算のパラメータを指定したの
ち、動作させるべき自由度及び動作方法を主制御処理部
よりコマンドとして与える。これにより、動作させたい
自由度を、動作させたい方法で個別または協調して自由
に動作させることができる。

【００２８】（ｆ）サーボ制御プログラムは、前記コマ
ンドにより指定された動作を完了し、ロボットアームの
指定された自由度を位置指令値の位置まで動作させる
と、自動的に位置指令値の位置を保持するよう動作す
る。また、コマンドの実行完了信号を主制御処理部に返
す。

【００２９】（ｇ）主制御処理部は、前記（１）〜
（３）より構成され、前記定時処理であるサーボ制御プ
ログラムの実行空き時間により実行処理される。

【００３０】（ｈ）主制御処理部の処理手順は、ロボッ
トアームに動作を開始させるときは、（１）→（２）→
（３）の順に演算を実行し、サーボ処理部に必要なパラ
メータ及び動作コマンドを送って動作を開始させる。動
作開始後は、サーボ処理部からの要求信号に応じて、逆
に（３）→（２）→（１）の順に必要に応じて逆上る演
算を実行し、サーボ処理部に必要なデータを遅滞なく供
給する。

【００３１】

【作用】ロボットアーム及び追加軸の自由度は個別で
も、協調してでも任意に動作させることができる。ま
た、サーボ系に、エンコーダ値レベルでの補間機能を与
えたので、ロボットアームを特定する座標系以外の例え
ば直交座標系で動作させる場合においても、サーボ系に
はかなり荒っぽい指令値を与えるだけで良く、主制御処
理部の演算負荷を大幅に減じることができる。また、サ
ーボ制御中に逐次指令値を要求される。すなわち連続し
た経路動作を要求されるような場合においても、サーボ
制御に近い側から演算を実行し、必要に応じて、サーボ
制御に遠い側へ逆上るように演算処理するので指令値の
供給に遅滞が生じることがない。

【００３２】

【実施例】以下、本発明を実施例により詳細に説明す
る。

【００３３】図１に、本発明を実現するロボットシステ
ムの一構成例を示す。なお、図１では、追加軸について
は記載していない。

【００３４】ロボットシステムは、ロボット本体２と制
御装置１より構成される。

【００３５】制御装置１は、ロボットを手動で動作させ
たり、動作した位置を位置記憶手段１８に記憶指令をし
たり、ロボットの動作手順を入力し、動作手段記憶手段
１７に指令をしたり、動作手順記憶手段１７に記憶され
た動作手順に従ってロボットに動作するよう指令したり
するための複数の操作スイッチ及び動作状況他を表示す
るための表示装置類を含む教示手段１４と、演算手段１
５の出力するＰＷＭ指令を受けてＰＷＭ信号を発生する
ＰＷＭ信号発生回路１１、そのＰＷＭ信号によって動作
し、ロボット本体２、特にサーボモータに運転電流を供
給するパワー回路１２と、サーボモータの回転位置を測
定するためにサーボモータに取りつけられたエンコーダ
のエンコーダパルスを計数するパルスカウンタ１３と、
複数のロボットアーム先端部の経由する位置を記憶する
位置記憶手段１８と、前記位置記憶手段１８に記憶され
た複数の位置の動作手順を記憶する動作手順記憶手段１
７と、演算手段１５の演算手順と記憶する演算手順記憶
手段１６と、教示手段１４の入力信号に応じて定められ
た演算手順記憶手段１６内に格納されている適切な演算
手順を読み出し、必要に応じて動作手順記憶手段１７及
び位置記憶手段に格納されている情報を用いて演算を行
ないロボットを動作制御する演算手段１５より成ってい
る。

【００３６】なおＰＷＭ信号発生回路１１、パワー回路
１２、サーボモータ２、パルスカウンタ１３は図１に示
すような接続関係で、ロボットの動作自由度の数だけ設
けられるが、図１では１組しか示していない。

【００３７】また、図１には示していないが、外部装置
とのデータ送受をするためのＩ／Ｏインターフェイス、
上位装置との通信のための通信インターフェイスも設け
られる。

【００３８】図２に、本発明の説明のために利用するロ
ボットアームの一例を示す。図２のロボットアームは、
周知の水平多関節形ロボットであり、図示の如く、３個
の回転の自由度及び１ヶの直線運動の自由度を持ってい
る。

【００３９】本発明は、特定の機構に限定されるもので
はなく、応用対象は、垂直多関節形ロボットでも、直交
動作形ロボットでも良いことはもちろんである。

【００４０】図３に、教示手段１４の操作スイッチ及び
表示装置の例を示す。詳細な説明は省き、以下の実施例
の説明の項で必要に応じて参照することにする。

【００４１】先ず、図４を用いて、演算手段１５の構成
について説明する。図４は、演算手段の最小機能構成を
示したものである。電源入の初期設定が終ると、演算手
段１５は、動作モード釦取り込み部２０において、動作
モード釦（図３の位置ティーチ、プログラムティー４、
原点合せ、テスト運転、連続運転釦）の入力を持ってい
る。

【００４２】いずれかの釦が押されると、それに応じて
ブロック２１〜２５、すなわち、原合合せ、動作教示、
……再生動作の各処理部（主制御処理部）のいずれかを
実行する。動作教示処理部２２の場合を除き、他の場合
はすべてロボットの動作制御に関するものであり、これ
らの下位に、これらの指示に従ってロボットアームの動
作制御を実行するサーボ制御部３０が位置している。

【００４３】次に図５〜図１３を用いて、本発明の主要
部であるサーボ処理部について説明する。

【００４４】図５においては、共通メモリ上にループカ
ウンタ３４が準備される。このループカウンタは、定時
割込によりサーボ主制御３０が起動される毎にブロック
３３において、常に１ずつ増加されている。これは制御
軸数以下の値をサイクリックにとるよう動作する。従っ
て例えば、ロボットアームの動作自由度が４である場
合、ループカウンタの下位２ビットは、０→１→２→３
→０のように順次変化するから、これに動作軸１〜４を
対応させれば、上の理論により、制御すべき軸を決定す
ることができるから、判定された制御軸のサーボ制御の
準備ができる。これはブロック３１の作用である。

【００４５】ここでサーボ制御の準備とは、実際上の各
軸のサーボ制御アルゴリズムは余程特殊な場合を除き同
一であるから、各軸毎の参照したり記憶したりするデー
タエリア、入出力エリアのアドレッシング方式を制御軸
に対して設定することに相当する。これにより、ブロッ
ク３２において動作させるべき制御軸についてサーボ制
御を実行する。ブロック３３は前述の通りである。

【００４６】ここでブロック３１と３２は順次実行され
ねばならないが、ブロック３３の実行はどの位置であっ
ても別に演算上及び制御上の問題はない。以下の種々の
説明においても、この演算順序に依存しないものも出て
くるが、その指摘は省略する。要は、演算順序に依存せ
ず、同一の結果を与えるものは、同一であるとみなしう
るはずである。

【００４７】以上により、サーボ主制御部３０は、定時
割込により起動される毎に、ある１つの制御軸のサーボ
制御演算を実行し、起動毎に制御する動作軸を順次切り
換えていって、複数回の起動により全動作軸の１回のサ
ーボ制御を実行する形式となっている。

【００４８】なお、図５には記載していないが、このサ
ーボ主制御部で、前記図３で示した教示手段１４の操作
釦の取り込み、表示データの送出、並びに周辺装置との
入出力信号の取り込みまたは出力、さらには、定時性を
利用したタイマ処理等も行なうようになっている。これ
らも、サーボ制御部の演算負荷となるので、種々の負荷
低減策がとりいれられているが、ここでは詳述しない。

【００４９】図６は、図５のサーボ処理部の動作タイミ
ング図である。定時間Ｔsm毎に起動され、順次第１軸、
第２軸、第３軸、第４軸の制御を実行し、次にはまた第
１軸から繰り返す。

【００５０】□の部分が、サーボ制御に専有される時間
であり、残りの□の部分で主制御処理、すなわち、図４
のブロック２０〜２５が実行される。ここで、制御軸数
×Ｔsm＜ＴsとなるようにＴsmを決定しているので、サ
ーボ系は安定に動作し、発振を起すこともない。

【００５１】次に、図７を用いて、図５のブロック３２
で示される各軸毎のサーボ制御処理について説明する。

【００５２】ここで、動作軸は第ｎ軸とする。第ｎ軸を
サーボ制御する際には、図７に示すような共通メモリ及
び第ｎ軸用メモリを使用する。まず、ブロック１００に
おいて、第１図に示すパルスカウンタ１３より、第ｎ軸
のカウンタ値を取り込む。次にブロック１０１におい
て、第ｎ軸用メモリの前回取り込んだカウンタ値との差
をとり、速度を計算し、これを、第ｎ軸用メモリの前回
の現在値に加えて、今回の現在値として記憶する。

【００５３】また、取り込んだカウンタ値は、第ｎ軸用
メモリに記憶する。ここで、速度はメモリには記憶する
ようにしていないが、このサーボ制御の実行完了までは
特定のワークレジスタ中に保存してある。次にブロック
１０２において、主制御プログラムより設定される共通
メモリ内のコマンドの有無を調べる。

【００５４】ここで、共通メモリ内のコマンドは、次の
ような形式である。

【００５５】〔〔コマンドデータ〕〔軸指定〕 すなわち、コマンドデータは、サーボ制御すべき内容を
指定する。軸指定部は、その指定ビットに対して、コマ
ンドデータの指定するサーボ制御の実行を要求してい
る。従って、ブロック１０２において第ｎ軸の指定をチ
ェックする。第ｎ軸の指定があれば、ブロック１０３を
実行する。その実行後もしくは第ｎ軸指定がなければ、
ブロック１０４以下を実行する。

【００５６】第ｎ軸の指定があった場合は、ブロック１
０３の第ｎ軸コマンド受付処理を実行する。すなわち、
まず、コマンド受付完了を主制御処理に報知するため
に、軸指定を消去する。次にコマンド実行中であること
を示すために、共通メモリの各軸動作状況部に、前記コ
マンドの軸指定と同様の形式で、第ｎ軸ビットを設定す
る。

【００５７】次に、コマンドデータを第ｎ軸メモリのサ
ブコマンドエリアへ移す。次に、主制御の指令する第ｎ
軸用メモリ内の動作指令位置をサーボ制御の最終目標位
置としてとり込む。すなわち、サーボ目標位置→前回サ
ーボ目標位置動作指令位置→サーボ目標位置のようにメ
モリを置き換える。更に、ここでは、詳述しないが、指
定されたコマンド特有の準備処理を行なう。

【００５８】このようにして、ロボットアームの動作制
御を各軸単独の動作制御に置換する。ブロック１０４で
は、第ｎ軸メモリ内のサブコマンドを分析して、サブコ
マンドで指定されるサーボ制御処理を実行して、第ｎ軸
モータの駆動信号を生成する。これはブロック１０５に
おいて第ｎ軸モータの駆動信号として、図１に示す第ｎ
軸用ＰＷＭ信号発生回路１１に出力され、第ｎ軸のサー
ボモータが駆動制御される。

【００５９】上記サーボ制御演算部および、もしくは、
駆動信号出力部においては、動作制御の安全のために、
位置偏差の大小、速度の大小、加速度の大小、駆動信号
の大小、動作位置の正当性などのチェックがなされ、異
常時には、対応する処置または表示出力処理を行なう
が、ここでは詳述しない。

【００６０】また、上記説明では、コマンドは、共通メ
モリ内に配置され、コマンドデータと軸指定データとが
対になって与えられるとしているが、各軸毎のメモリ内
に配置され、コマンドデータのみ与えられるようにして
も良い。この場合は、コマンドがサーボ系に取り込まれ
た、もしくは、受けつけを示すために指定されたコマン
ドデータをクリアする、もしくは、コマンド発行側の認
知しうる別データを設定するような処理が必要である。

【００６１】ここで、コマンドもくしはサブコマンドと
して指定されるサーボ制御内容には、例えば次のような
ものがある。

【００６２】（i）原点合せのための （ａ）原点領域から脱出する制御 （ｂ）原点領域を探す制御 （ｃ）エンコーダのＺ相を探す制御 （ii）加減速制御 （iii）強制減速制御 （iv）１ステップ送り制御 （v）継続制御 （vi）位置保持制御 （vii）非常停止制御 以下、図７のブロック１０４で実行されるサブコマンド
処理のうち、主なものについて詳述する。

【００６３】図８は、位置保持制御を説明するブロック
図である。ここで、位置偏差ｅは、例えば、 ｅ＝サーボ目標位置−現在位置 である。ブロック１１０で、｜ｅ｜が所定値以内か否か
がチェックされる。所定値以内であれば、ブロック１１
１で図７の共通メモリ部の各軸動作状況エリアに設定し
た第ｎ軸を消去し、第ｎ軸の位置保持完了を主制御部へ
報告する。実際上はブロック１１０の判断は、前記判断
結果が所定回数以上連続するか否かなど、より安全のた
めの処理が含まれている。

【００６４】次にブロック１１２で例えば次式で示す、
位置保持制御演算 ＰＷＭ＝Ｋ_Iｅ＋ΣＫ_Sｅ Ｋ_I：比例ゲイン Ｋ_S：積分ゲイン が実行され、モータ駆動信号ＰＷＭが求められる。

【００６５】この位置保持制御は、他の動作モードのロ
ボットアームのサーボ制御が完了したときに常に最終サ
ブコマンドとして設定され、次の動作コマンドが指定さ
れるまで位置保持動作を行なう。従って、主制御部は、
次の動作を指令するときはロボットアームの動作の終了
を持って次のコマンドを発行すれば良く、また、次の動
作を指令する指令がないときは、何もしなくて良いの
で、主制御演算負荷低減及び演算理論の簡略化が得られ
る。

【００６６】図９は、１ステップ送り制御を説明するブ
ロック図である。ブロック１２０では第ｎ軸のサブコマ
ンドを位置保持コマンドに変更する。これにより、第ｎ
軸のサーボ制御が次回以降起動されると、位置保持制御
が実行されることになる。

【００６７】ブロック１２１は図８の対応ブロック１１
１と同様の処理である。これにより、１step送り動作は
即時に完了することになる。ブロック１２２も図８のブ
ロック１１２と同様の処理である。

【００６８】１ステップ送りは、位置教示の際に微妙な
位置決め点にロボットアームを誘導するのに有用であ
る。動作は即時に完了するので、連続くり返し１ステッ
プ送りが可能である。

【００６９】第１０図は、現在位置よりサーボ目標位置
まで、第ｎ軸を所定の加減速パターンに従って加速→等
速→減速によって動作させる場合のブロック図である。
ブロック１３０において、第ｎ軸位置偏差 ｅ＝第ｎサーボ目標位置−第ｎ軸現在位置 につき、｜ｅ｜＜所定値を調べる。

【００７０】所定値以内であれば、サーボ目標位置に到
達しているからブロック１３１において、位置保持コマ
ンドを第ｎ軸のサブコマンドに設定する。そして、ブロ
ック１３２で前述の位置保持制御を実行する。これによ
り次回以降第ｎ軸のサーボ制御が起動されたときは、位
置保持モードで動作する。

【００７１】所定値以上であれば、加減速制御を実行す
る。すなわち、ブロック１３３において、所定の方法で
加減速テーブルを検索して、動作速度指令を求める。テ
ーブル検索法（第２２図）については後で詳述する。

【００７２】次に、ブロック１３４において、速度サー
ボ処理を実行し、第ｎ軸のモータ駆動信号を求める。

【００７３】すなわち、例えば、 ＰＷＭ＝Ｋ_IV（Ｖ_D−Ｖ）＋ΣＫ_SV（Ｖ_D−Ｖ） Ｋ_IV：比例ゲイン Ｋ_SV：積分ゲイン Ｖ_D ：速度指令 Ｖ ：速度 によって、モータ駆動信号を求める。

【００７４】第１１図は、現在動作中のロボットアーム
を強制減速させ停止させる場合のブロック図である。こ
れは、位置ティーチ及びテスト運転時に有用である。

【００７５】ブロック１４０において、減速テーブルの
検索が終端に達したかどうかを調べる。

【００７６】達していればブロック１４１で位置保持コ
マンドを第ｎ軸のサブコマンドに設定し、次回以降の動
作モードを変更する。次にブロック１４２において、第
ｎ軸の現在位置、すなわち減速終了点位置を第ｎ軸のサ
ーボ目標位置に設定し、ブロック１４３で位置保持制御
を実行する。これにより、第ｎ軸は、減速終了点に位置
決めされる。

【００７７】減速テーブルの終端点に達していなけれ
ば、ブロック１４４で１ステップより減速側の減速テー
ブル値をとり出し、動作速度指令に変換し、ブロック１
４５において前述の速度サーボ処理を実行する。これに
より第ｎ軸は所定の減速パターンに従って減速してゆ
く。

【００７８】次に、継続制御について説明する。継続制
御は複数の教示点間を停止することなく連続して動作さ
せる場合とか、ロボットの動作関節角とは非線形の関係
にある座標系、例えば直交座標系でロボットアームを曲
線または直接経路に沿って動作させる場合に、ロボット
アームの動作経路を逐一動作指令位置として指定し、追
値制御の形式でロボットアームを動作させるコマンド方
式である。

【００７９】ここで、もし、主制御処理部が、遅滞な
く、動作経路順に動作指定位置をサーボ系に与えること
ができるならば、継続処理は、前記した１ステップ送り
制御もくしはその変形した方式で実行することができ
る。しかし、サーボ制御処理は、最も優先度の高い定時
割込処理であり、主制御処理部とは非同期で動作してい
るから、データの授受について完全に１：１の同期をと
ることは難かしい。

【００８０】また、実際のアームの動作状況及び動作経
路をみると、サーボ系の動作周期で動作指令を与える必
要はないことがわかる。すなわち、動作指令をサーボ系
内で何分割かして、その補間値をサーボ指令として動作
させた方が、主制御処理部の演算負荷低減の面でも、サ
ーボ処理部との同期の面でも好都合である。

【００８１】図１２は、上記思想にもとづく継続制御の
ブロック図である。図１２のブロック図の場合、図７の
ブロック１０３において説明したコマンド受付処理を少
し変更する要がある。

【００８２】図１２（ａ）は、そのコマンド受付処理を
示したものである。図１２（ａ）において、ブロック１
５０において、共通メモリエリアのコマンドに指示され
たコマンドデータと第ｎ軸のサブコマンドのデータを比
較する。

【００８３】両者が不一致である場合は、継続制御の開
始が指令されたものとして、ブロック１５１及び１５２
を実行する。すなわち、ブロック１５１では、コマンド
されたコマンドデータを第ｎ軸サブコマンドエリアへ取
り込む。さらに、ブロック１５２において、図７の第ｎ
軸用メモリ内に設けられた（図示しない）カウンタエリ
アをクリアする。これは、後述の補間演算の初期化に相
当する。両者が一致する場合は何もしない。このように
して、継続制御の初期化が行なわれたのちの実際の継続
処理は、図１２（ｂ）で実行される。

【００８４】図１２（ｂ）において、ブロック１５３
で、第ｎ軸カウンタ値の値がテストされる。カウンタ値
が０の場合は、まだ、コマンドで指定された動作指令位
置をとり込んでいないので、そのとり込みのための処理
を行なう。そこで、ブロック１５４においてコマンドに
第ｎ軸の指定があるかどうかを調べる。もし指定があれ
ば、ブロック１５５において、 第ｎ軸サーボ目標位置→第ｎ軸前回目標位置 第ｎ軸動作指令位置→第ｎ軸サーボ目標位置 のようにデータを転送して、第ｎ軸サーボ動作指令位置
をとり込み、ブロック１５６において、コマンドの第ｎ
軸の動作指定を消去する。これにより、主制御処理部
は、第ｎ軸の動作指令位置を再び指定することができ
る。以上の取り込みが終ると、共通処理であるブロック
１６０〜１６４を実行する。

【００８５】ブロック１５４において第ｎ軸の動作指定
がなければ、継続制御は、終了したものとみなしてブロ
ック１５８において、位置保持コマンドを第ｎ軸のサブ
コマンドに設定し、ブロック１５９において、前記した
図８の位置保持制御を実行する。ブロック１５８の作用
により、これ以降は、ロボットアームの第ｎ軸は位置保
持制御される。

【００８６】ブロック１５３のテストの結果、第ｎ軸の
カウンタ値が０でない場合は、ブロック１６０〜１６４
を実行する。ブロック１６０においては、例えば補間演
算（（カウンタ値＋１）×（サーボ目標位置−前回サー
ボ目標位置）／Ｎ＋前回サーボ目標位置）により、今実
行すべきサーボ制御の目標位置を求め、ブロック１６１
において、位置偏差ｅを求める。

【００８７】 今回サーボ目標位置−現在位置 → 位置偏差ｅ 次に、ブロック１６２においてカウンタ値をサイクリッ
クにカウントアップして、次の補間数値を定める。すな
わち、補間分割数がＮであるとき、 カウンタ値＝Ｎであるならば、０→カウンタ のように再設定する。これにより、補間の継続と次の第
ｎ軸動作指令位置のとり込み処理の実行が保証される。
ここで、補間分割数Ｎが２のｍ乗である場合は、上記リ
ミッタ処理は不要であり、カウンタの下位ｍビットを用
いて、上記説明方法が実行できることはいうまでもな
い。

【００８８】次に、ブロック１６３において、第ｎ軸の
動作中を示す信号を共通メモリの各軸動作状況エリアへ
設定する。これは、この実施例では、継続制御を位置保
持モードで実行しているから、前記したようにそこで、
第ｎ軸の動作完了信号が、各軸動作状況エリアへ設定さ
れるのを防ぐためである。

【００８９】最後に、ブロック１６１で求めた位置偏差
ｅを用いて、前記した図８の位置保持制御を実行する。

【００９０】上述の例では、補間演算を乗除算で実行す
る例を示しているが、演算時間面を考慮すれば、別にワ
ークエリアを設けて加算で実行した方がベターである。
そのための方法は簡単であるのでここでは省略する。

【００９１】要は、図１２において、次々と指令される
第ｎ軸の動作指令位置をとり込み、それを１次補間して
サーボ制御し、ロボットアームの第ｎ軸を継続動作さ
せ、動作指令がなくなれば最後の動作指令位置でロボッ
トアームの第ｎ軸を位置保持するようにサーボ系は作用
する。

【００９２】図１２の例では、動作位置指令を次々と指
定するためには、同時にコマンドも指令しなければなら
なかった。

【００９３】図１３では別の例を示す。図１３では、共
通メモリ内に複数の継続制御のための動作軸指定エリア
と各軸用のメモリ内に同じ大きさの継続制御のための動
作指令位置エリアを設け、これらを各軸用メモリ内のカ
ウンタ２を用いて循環的に順次参照してゆく構成として
いる。

【００９４】各軸用メモリ内のカウンタ１は、図１２の
場合と同じ働きをする。

【００９５】図１３（ａ）は、継続制御の受付処理部で
あり、ブロック１７０でコマンドを受けつけ、ブロック
１７１で継続制御実行のための初期化を行なっている。
この場合は、継続制御開始時に１回継続制御のコマンド
を主制御処理が発行すれば良いので、ブロック１７２に
おいて、コマンドの第ｎ軸の軸指定を消去する。

【００９６】図１３（ｂ）は、図１２（ｂ）と大略同様
であり、次の点が相異する。ブロック１７４で参照する
軸指定は、カウンタ２でインデックスされる共通メモリ
内の継続制御のための軸指定である。ブロック１７５で
取り込む動作指令位置は、カウンタ２でインデックスさ
れる第ｎ軸メモリ内の継続制御のための動作指令位置で
ある。ブロック１７６も同様の変更である。

【００９７】ここではブロック１７７で、インデックス
をサイクリックに更新する処理が追加されている。この
カウンタ２の値は、前記した追加エリアの大きさ内での
カウント値を示し、エリアをこの値を用いて参照するよ
うになっている。

【００９８】このようにして、循環的に参照し、取り込
みが完了すると軸指定を消去するようにしているので主
制御処理部は、この軸指定が消去されたエリアのデータ
は取り込まれたことがわかるので、最初は順次全エリア
に軸指定と動作指令位置を置き込み、その後は軸指定を
参照しながら順次データの設定を行なってゆけば良いこ
とになる。そのためには、併示したカウンタ３を用いれ
ば良い。

【００９９】またこの軸指定の参照は各軸毎に部分的に
行なっても、全体軸として行なっても良いことはもちろ
んであり、それは、コマンドの軸指定及び各軸動作状況
についても同様のことが云える。

【０１００】以上述べた、定時割込により起動されるサ
ーボ制御部についてまとめると次のようになる。

【０１０１】サーボ制御部は定時割込により起動された
とき、ロボットアームの動作自由度のうちの１ヶのみに
ついてのサーボ制御演算を行ない、循環的にサーボ制御
演算を実行する動作自由度をきりかえてゆくよう動作す
る共に、ロボットアームの関節角またはそれに相当する
量で指定される動作目標位置へ到達する方法及び動作軸
を示すコマンドにより、各動作軸を個別または協調して
動作制御する。

【０１０２】また、コマンドによって種々の動作方法、
補間方法または、継続動作等を自由に指定することがで
き、従来のようにサーボ制御部が起動されるとすべての
動作自由度が指定された目標位置に追随するよう動作
し、目標とする動作を実現するために常にサーボ制御部
に動作目標値を指定する必要のない、自律的サーボ系を
構成しているので、主制御部の演算負荷の低減のみなら
ず、理論演算量をも低減する多大の効果を奏する。

【０１０３】次に、主制御処理部の処理方式を説明する
に先立って、その演算内容、特に数値処理内容について
説明する。

【０１０４】まず、図１４に示すような２点間を直線で
動作させる場合の関係式は次のようである。

【０１０５】

【数１】

【０１０６】これより、直線上の中間点までの動作距離
をｌとすると、中間点の位置Ｐは、

【０１０７】

【数２】

【０１０８】となる。

【０１０９】次に、図１５に示すような３点で決定され
る円弧上を動作させる場合の関係式は、次のようであ
る。

【０１１０】直線Ｐ_i→Ｐ{i+1}，Ｐ{i+1}→Ｐ{i+2}の関
係式は、前記と同様である。ここで、{i+1}、{i+2}はそ
れぞれＰの下付きの添字を意味する。

【０１１１】

【数３】

【０１１２】これより、３角形の頂角θは

【０１１３】

【数４】

【０１１４】である。方向ベクトルｅ_iに垂直で円の中
心に向かう方向ベクトルは、

【０１１５】

【数５】

【０１１６】である。円の半径をγとしたとき、図１５
を参照して、次の関係があるから、

【０１１７】

【数６】

【０１１８】これを用いて、

【０１１９】

【数７】

【０１２０】より、円の半径γが求められる。

【０１２１】

【数８】

【０１２２】従って、点Ｐ_iより円の中心へ向かうベク
トルＰ_iＯが以下のように求められる。

【０１２３】

【数９】

【０１２４】長さγでベクトルＰ_iＯにが直交し、ロボ
ットアームの動作方向へ向かうベクトルγβ₁は、次の
ように求められる。

【０１２５】

【数１０】

【０１２６】である。

【０１２７】また、ロボットアームの円弧上の全動作距
離Ｌ₀は、

【０１２８】

【数１１】

【０１２９】である。

【０１３０】これより、全弧上の中間点Ｐの点Ｐ_iから
の動作距離をｌとすると、中間点Ｐの座標値は、次式で
与えられる。

【０１３１】

【数１２】

【０１３２】但し、 α＝ｌ／γ （０≦ｌ≦Ｌ₀） である。

【０１３３】以上は、位置のみについて言及した。しか
し、ロボットアームの動作制御の場合には、手先の姿勢
も同期して制御されねばならない。

【０１３４】第２図に示すロボットアームの例では、動
作始点（Ｘ《S》，Ｙ《S》，Ｚ《S》，θ₁₂₄《S》）と
動作終点（Ｘ《E》，Ｙ《E》，Ｚ《E》 ，θ₁₂₄《E》）
との間を直線または曲線補間する場合、位置成分と回転
角成分が混在し、動作制御（補間演算）の優先順位決定
が難しい（ここで、《S》及び《E》は上付きの添字であ
る）。そこで、グリップ回転角速度の設計仕様をみたす
ような仮想グリップ長さを導入する。これにより、グリ
ップ回転角θ₁₂₄《E》−θ₁₂₄《S》を長さに変換する定
数Ｋｌ及び逆に長さを角度Ｋ_Oに変換する定数を定める
ことができ、次式の補間演算の基礎数値を得る。

【０１３５】

【数１３】

【０１３６】これより、補間演算においては、

【０１３７】

【数１４】

【０１３８】により中間角度を得ることができる。

【０１３９】中間点の位置については、 ｌ＝ｌ'Ｌ_O／Ｌ_max を用いて、前記式により中間点の位置を得ることができ
る。Ｌ_O／Ｌ_maxは後述の長さの縮小率に相当する。

【０１４０】次に、直線または円弧上をロボットアーム
が動作するとき、ロボットアームを適当な加減速パター
ンに従って動作させるための加減速パターン処理方式に
ついて説明する。

【０１４１】これに関し従来用いられている方法は、台
形の速度パターンとする方法（特開昭６１−１９３２０
４号公報）、速度テーブル及び距離テーブルを用意し、
それを検索することにより加減速を含む動作速度を決定
する方法（特開昭５９−８３２１０号公報）、函数演算
を利用する方法（特開昭６１−５３１１号公報）などが
提案されているが、実用性またはシステム仕様の変更に
対し柔軟性を欠くものが多く、ティーチングによって多
様な動きをするロボットアームに適用するには問題が多
かった。

【０１４２】ここでは、より一般的で汎用性があり、か
つロボットアームの多様な動作条件にも自由に適合でき
る加減速処理方式を提供する。

【０１４３】図１６に、本発明にかかる加速パターンの
一例を示す。図１６において、横軸はテーブルアドレス
であり、縦軸は、無次元速度を表わす。図１６は、具体
的には、図１７に示す加速及び減速用のテーブルで構成
されている。

【０１４４】図１７において、加速及び減速テーブルの
パターンは任意のもので良く、ロボットアームの加減速
制御に最適なものを設計者が設定することができる。ま
た加速と減速テーブルの大きさも異なっていても良い。
両者を組み合せて使用するときの最大の無次元速度が、
使用時に同一または略同一で、両者を切り換えたとき
に、速度ギャップが生じないようになっていれば良い。

【０１４５】また、加速時と減速時で同一パターンを利
用する場合には、加速テーブルのみ設けて、減速時にも
共用するようにすることもできる。また、図１７に示す
加速及び減速テーブルは、ロボットの各動作軸毎の動作
用及び前述の直線動作用など個別に用意することも自由
であり、それによって、以下に述べる本発明の主旨がか
わるものではない。

【０１４６】以下では、加速テーブルと減速テーブルと
の２つを設け、すべての動作条件で共用する場合を例に
とって説明する。前記した変形例に対しても、多少の手
直しで適用できる。

【０１４７】加速及び減速用テーブルを共用して、かつ
任意のテーブル部分を参照できるようにするために、各
動作軸及び直線動作用に、図１７に示すような、第ｎ軸
用ＲＯＭメモリを用意する。以下の説明では、第ｎ軸用
の表現は、全動作軸及び直線動作用に個別に用意される
ものの一代表例の名称とする。

【０１４８】第ｎ軸用として加速及び減速テーブルを参
照するために、図１７のテーブルに対し、ラベルを付
す。

【０１４９】 ＡＳＴＲＮ：加速開始参照アドレス ＡＭＡＸＮ：加速終了参照アドレス ＤＭＩＮＮ：減速終了参照アドレス ＤＭＡＸＮ：減速開始参照アドレス 第ｎ軸ＲＯＭメモリのエリアＡＳＴＲ、ＡＭＡＸ、ＤＭ
ＩＮ、ＤＭＡＸＮに、上記ラベルの値を格納する。これ
は、例えば、アセンブリ言語で、下記のように記載し、 ＡＳＴＲ ＤＣ．Ｗ ＡＳＴＲＮ アセンブラにかければ、自動的に、ＡＳＴＲのラベルを
付されたエリアにＡＳＴＲＮの値、すなわちテーブルア
ドレスが格納されることにより達成される。これによ
り、加速時は、加速テーブルのＡＳＴＲＮからＡＭＡＸ
Ｎ、減速時は、減速テーブルのＤＭＩＮＮからＤＭＡＸ
Ｎの範囲を参照することが可能となる。また、各テーブ
ルのラベルの位置を任志に設定しなおすことにより、任
意の範囲でテーブルを参照することが可能となる。

【０１５０】第ｎ軸ＲＯＭメモリのＳＨＵＫＵで定義さ
れるエリアには、テーブルの参照縮少率を定義する。例
えば、定義した値が１.０であるときは、テーブル参照
間隔は、１個ずつ、すなわちテーブルを順次参照するこ
とをあらわし、０.５に定義したときは、テーブルを１
ヶ置きに参照することを表わす。すなわち、定義した値
の逆数が、テーブル参照間隔を表わす。あとで述べるよ
うに、この逆数値は整数となる必要はない。

【０１５１】第ｎ軸ＲＯＭメモリのＳＰＤＧ１〜ＳＰＤ
Ｇｎは、ロボットアームを動作させるときに指定される
速度に対応し、前記加速または減速テーブルの最大値に
その値をかけると結果が指定された速度になるように選
ばれた変換定数を格納するエリアである。ここでは、離
散的にｎ個の変換定数を準備するようにしているが、１
個だけ準備し、指定された速度との倍率をかけて実際の
変換定数を算出するようにしても良い。第ｎ軸ＲＡＭメ
モリのＳＰＤＧのエリアは、このＳＰＤＧｎの値または
その実動作のために補正された値を記憶するために設け
られている。

【０１５２】ＫＡＳＯＫＵ及びＧＥＮＳＯＫＵのエリア
は、テーブル参照時の現在参照する各テーブルのアドレ
スを格納している。ＤＥＬＴＢＬは、テーブルの参照間
隔、すなわち前記ＳＨＵＫＵの値の逆数もしくはその補
正された値を格納し、テーブル参照のアドレス計算に利
用される。

【０１５３】ＫＡＳＯＫＵ、ＧＥＮＳＯＫＵ及びＤＥＬ
ＴＢＬは、少数以下の値を含むテーブル参照アドレス及
び参照ステップを格納しているが、ＫＡＳＯＫＵまたは
ＧＥＮＳＯＫＵ内の値を使ってテーブルを参照するとき
は、少数以下の値を切り捨てて行なう。この機能によ
り、固定ピッチで作成されているテーブルをフリーピッ
チで参照することができる。

【０１５４】第ｎ軸ＲＡＭメモリのＩＫＰＵＬＳ及びＩ
ＤＰＵＬＳのエリアには、それぞれ第ｎ軸ＲＯＭメモリ
で参照定義されたテーブル範囲内のテーブル値の積算値
が記憶されている。それぞれの値×縮少率×変換定数は
それぞれ、加速距離及び減速距離を与える。

【０１５５】ＩＫＰＵＬＳ及びＩＤＰＵＬＳの値は、も
しテーブルが固定的に扱われる場合には、前もって計算
して第ｎ軸メモリ内に格納しておくことも可能である。
ここでは、演算システム初期化時に演算手段が第ｎ軸Ｒ
ＯＭメモリを参照して積算値を求め、前記エリアに格納
してあるものとする。

【０１５６】以上のテーブル及びメモリが準備され、ロ
ボットアームの動作距離Ｌ₀及び速度Ｖ₀（実際上は、第
ｎ軸ＲＯＭメモリのＳＰＤＧ１〜ＳＰＤＧｎを参照する
インデックス）が与えられた場合のテーブル参照の計画
手順を図１８を用いて説明する。

【０１５７】ブロック４０００は、指定された速度Ｖ₀
をインデックスとして、第ｎ軸ＲＯＭメモリのＳＰＤＧ
１〜ＳＰＤＧｎより、変換定数を読み出し、第ｎ軸ＲＡ
ＭメモリのＳＰＤＧエリアに設定する。

【０１５８】ブロック４００１は、加速及び減速テーブ
ルの総メンバ数を、第ｎ軸ＲＯＭメモリに格納されてい
るテーブル参照アドレスを用いて求める。テーブル参照
アドレスのピッチが１である場合は、ブロック４００１
に示す式となる。ピッチが１以外の場合は、それに応じ
て修正すれば良い。この値は、ロボットアームを加減速
のみで動作させ、逐一テーブルを参照するとしたときの
テーブル参照回数をふし、それはまた、ロボットアーム
の移動時間を示していることになる。

【０１５９】ブロック４００２は、ロボットアームを加
減速動作のみで動作させたときの動作距離Ｌを求めてい
る。

【０１６０】ブロック４００３では、指定された動作距
離Ｌ₀と、前記Ｌの比較を行なっている。もし、Ｌ₀≧Ｌ
の場合は、指定された動作距離Ｌ₀が、加減速テーブル
で規定される加減速距離より大である。すなわち、ロボ
ットアームを、加速→等速→減速のパターンで動作させ
る必要があることをふし、この場合は、ブロック４００
４の処理を行なう。

【０１６１】ブロック４００４では、等速部を含めたテ
ーブル参照回数、実質上は、ロボットアーム第ｎ軸の移
動時間ＩＴＩＭＥを求める。これは後述するように、ロ
ボットアームの全動作軸を同時スタート、同時ストップ
で動作させるような場合に利用される。

【０１６２】次に、減速積算値ＩＤＰＵＬＳをＤＰＵＬ
Ｓに修正し、さらに、テーブル参照ステップＤＥＬＴＢ
Ｌを求める。ＤＰＵＬＳは、減速時の減速制御の基準値
として、ＤＥＬＴＢＬは、テーブル参照のステップ増／
減分として用いられる。

【０１６３】もし、Ｌ₀＜Ｌの場合は、テーブルパター
ン通りに加減速を行なうと、ロボットアームが目標位置
を通りすぎてしまうことを示している。このような場合
に、テーブルパターンの高速側を切りすててしまう方法
が考えられるが、この方法によれば、せっかく最適に設
定準備したテーブルパターンが有効に利用されないため
に、ロボットアームの不測の振動及び加速度の急変が起
ったりするので好ましくない。

【０１６４】また、テーブル値を縮少、すなわちＳＰＤ
Ｇを小さくして、Ｌ₀＝Ｌとなるように修正して利用す
る方法も考えられるが、この場合は、動作距離が短かい
にも関らず、動作時間がテーブルパターンに設定した時
間と同一、一定の長い時間となり、好ましくない。

【０１６５】そこで、本発明では、テーブル値の縮少
と、テーブル参照ステップの拡大を行ない、Ｌ₀＝Ｌと
なるように補正する方法を提供する。その原理は、テー
ブル値を縮少すれば、最大速度及び加速度が減少する。
従ってその分だけテーブル参照ステップを拡大して、初
期のテーブルパターンに示される加速度になるよう補正
すれば、ロボットアームの動作特性を良好に保ったま
ま、動作時間の短縮ができることにある。ここで問題と
なるのは、縮少、拡大に伴なう配分率をいかに決定する
かである。

【０１６６】テーブル値が、関数式を用いて準備された
ものであるならば、テーブル値の縮少率をパラメータと
して、最大加速度が同一となる参照ステップの拡大率を
求めることが可能である。

【０１６７】ここでは、任意テーブルパターンに対処で
き、かつ、容易に配分率を決定する方法として、ブロッ
ク４００５を示している。テーブルパターン縮少率をＭ
＝√(Ｌ₀／Ｌ）とし、テーブルステップ拡大率を１／Ｍ
とする。これにより、変換定数ＳＰＤＧをＳＰＤＧ×Ｍ
→ＳＰＤＧに修正する。

【０１６８】テーブル参照回数もしくは移動時間ＩＴＩ
ＭＥは、Ｎ×ＳＨＵＫＵ×Ｍとなる。減速積算値は、Ｉ
ＤＰＵＬＳ×ＳＨＵＫＵ×Ｍとなる。テーブル参照ステ
ップＤＥＬＴＢＬは１／（ＳＨＵＫＵ＊Ｍ）となる。

【０１６９】以上の準備のあと、ブロック４００６にお
いて、テーブル参照の初期アドレスをＫＡＳＯＫＵ及び
ＧＥＮＳＯＫＵに設定する。

【０１７０】以上述べたように、各動作軸毎もしくは直
線／曲線動作時のテーブル参照計画が求められる。

【０１７１】次に、全動作軸を同時に動作開始させ、同
時に動作を終了させる場合のテーブル参照計画の補正法
について図１９を用いて述べる。

【０１７２】まず、ブロック４０１０において、図１８
を用いて求められた各動作軸の移動時間ＩＴＩＭＥか
ら、同時動作させるべき動作軸について最大移動時間Ｉ
ＴＩＭＥＭＡＸを求める。

【０１７３】続いて、ブロック４０１１において、同時
動作させる軸について、ブロック４０１２〜４０１４を
実行させる。ブロック４０１２では、第ｎ軸の移動時間
増分ΔＩＴＩＭＥを求める。ブロック４０１３では、時
間増分だけ、等速移動部を追加するとした場合の第ｎ軸
の動作距離Ｌを求める。

【０１７４】 Ｌ＝Ｌｎ＋ΔＴＩＭＥ×テーブル最大値×ＳＰＤＧ Ｌｎは、第ｎ軸の動作距離、テーブル最大値は、第ｎ軸
が参照するテーブル値の最大のもの、すなわち無次元最
大速度、ＳＰＤＧは、第ｎ軸のテーブル変換定数であ
る。次にブロック４０１４において第ｎ軸の変換定数を
縮少する。

【０１７５】ＳＰＤＧ＝ＳＰＤＧ×Ｌｎ／Ｌ これにより、前述のサーボ系で、各軸独立に動作させる
ようサーボ制御しても、ロボットアームは、同時スター
ト、同時ストップするように動作する。

【０１７６】次に、各動作軸を非同期で動作させる場合
について説明する。これは、例えば、ロボットアーム
は、同期して動作しているが、追加軸が同辺装置との関
連で別モードで動作する場合とか、図２０に示すよう
に、物品をＡ点よりＨ点へ移載する場合に、第２図に例
示したようなロボットアームで作業させる場合にこのよ
うな動作モードが起る。

【０１７７】ここでは図２０の場合について説明する。
図２０は、例えばＡ点で物をつかみ、Ｂ点まで直線上を
持ちあげ、Ｃ’，Ｄ，Ｅ，Ｆ’点を経由してＧ点よりま
た直線上を真下のＨ点へ移動して、Ｈ点に物品を置くよ
うな作業を実行する動作軌跡を示している。これは、通
常の５自由度または６自由度の垂直多関節形ロボットア
ームで作業を行なわせる場合、５または６つの自由度を
同期させて、図２０の経路に従うように動作させねばな
らない。これについては後述する。

【０１７８】図２に示すようなロボットの場合は、上下
方向の動きと、水平方向の動きは、独立に分離できる。
すなわち、上方向への点Ａ→Ｃへの動きと、水平方向の
点Ｃ→Ｆへの動きと、下方向への点Ｆ→Ｈへの動きに分
離できる。そして、物品を移載するような作業の場合、
ロボットアームが周辺の装置と干渉しないようにするた
め、直線部Ａ→Ｂ及びＧ→Ｈの動作さえ確保できれば、
Ｂ→Ｃ’→Ｄ→Ｅ→Ｆ’→Ｇの動作経路はどのようであ
ってもかまわない場合が多い。

【０１７９】このような場合には、点Ａから上方に向か
って上下軸を動作させ、点Ｂに到達した時点で、点Ｃ→
Ｆに向かって水平方向の動きを開始させる。

【０１８０】また、水平方向の動きが点Ｆへ到達する前
に、点Ｅに到達した時点で上下軸を下方に向かって動作
させ、水平方向の移動が完了し、点Ｆに到達したとき
に、上下軸が点Ｇを通過するように計画してやれば、そ
の作業目的を達成することができる。

【０１８１】このような動きに対しては、上下の動きと
水平の動きは、あるタイムラグの下で非同期の動きをす
る。なおここで、水平方向Ｃ→Ｆの動きは、直線上を運
動するものであっても、ロボットアームの各関節角を基
準にして動作するものであってもよい。このような動作
をさせるメリットは、加速−等速−減速の動作モードで
Ａ→Ｃ，Ｃ→Ｆ，Ｆ→Ｈを動作させる場合と比べて、ロ
ボットアームが先取りモードで動作するから、移載作業
のタクトタイムが大幅に短縮されることである。

【０１８２】この動作のタイムチャートを第２１図に示
す。第２１図よりわかるように、まず、上下動作を開始
し、上下軸が点Ｂに到達する時間ｔ_AB後に水平動作を開
始させる。水平動作中に上下軸は、直線Ｃ→Ｆで表わさ
れる高さに到達して停止する。水平動作開始後、水平動
作の所要時間ｔ_CFより上下軸がＦ→Ｇへの移動に要する
時間ｔ_FGを差しひいた時間後に、再び上下軸に下方動作
を指令してやれば良いことになる。

【０１８３】上述の説明からもわかるように、上記の動
作を規定するのは、２つのタイマである。上方動作開始
後水平動作開始までのタイマの設定値をｔ₁，水平動作
開始後、再び下方動作を指令するタイマの設定値をｔ₂
とするとロボットアームのすべての動きを満足させるタ
イマの設定値は、次のようになる。

【０１８４】ｔ₁＝ｔ_AB ｔ₂＝ｔ_CF−ｔ_FG（但しｔ₂≧ｔ_BC） ＝ｔ_BC （但しｔ₂≧ｔ_BC） ここでｔ₂をｔ_BCに設定するような条件、すなわち、上
下の動作距離が長く（動作時間が長く）、水平の動作距
離が短かい（動作時間が短かい）ような条件では、図２
０のＧ→Ｈの間よりも上下すなわち、ＦとＧの間の位置
から→Ｈまでの間を上下軸は直線で下降する。もし、常
にＧ→Ｈの間のみを直線にする場合は、水平動作の動作
時間ｔ_CFをｔ_BC＋ｔ_FGと長くしてやれば良い。それは、
前述した図１９の手法を適用して行なうことができる。

【０１８５】また、上下動作、すなわち点Ａ→Ｂ及び点
Ｇ→Ｈの間の動作時間は、点Ａ→Ｃ及び点Ｇ→Ｆについ
て、図１８で示したテーブル参照計画を実行することに
より得られる変換定数ＳＰＤＧ及びテーブル参照ステッ
プＤＥＬＴＢＬを用いて、テーブルを順次検索し、点Ａ
→Ｃ及び点Ｇ→Ｆ動作のためのテーブル参照回数を求め
ることにより得られる。ここでは、その詳細は簡単であ
るので省略する。

【０１８６】次に、ロボットアームを停止させることな
く、教示点間と連続して動作させる場合について説明す
る。一例として、図２０に示す３次元空間の曲線上Ａ→
Ｂ→Ｃ’→Ｄ→Ｅ→Ｆ’→Ｇ→Ｈを軌跡制御する場合に
ついて述べる。

【０１８７】軌跡は、直線Ａ→Ｂ，円弧Ｂ→Ｃ’→Ｄ、
直線Ｄ→Ｅ、円弧Ｅ→Ｆ’→Ｇ、直線Ｇ→Ｈより成り、
点Ａより加速を始めて、途中経路で等速となり、点Ｈに
減速して停止するとする。各直線及び円弧は、前記した
演算式により、決定される。

【０１８８】しかし、ロボットアームの動作は、図２０
の軌跡のすべてを事前に計算して制御することはできな
い。その理由は、軌跡の途中経路を記憶するメモリが膨
大なものとなること、ループするような軌跡の場合は、
最後の減速位置が決められないこと、動作中、操作スイ
ッチまたは外部入力等により停止信号が発せられたとき
停止しなければならないこと等による。

【０１８９】従ってこの例の場合は、最初の直線の軌道
が決定された段階で、図１８に示すテーブル参照計画に
つき、ブロック４０００、４００１、４００２、４００
４のみを実行し、テーブルの縮少を伴なわない加減速計
画を立て、この加速計画に伴ってロボットアームの動作
を開始し、次後、あとに継続する軌跡の軌道計画につ
き、動作を継続し、最後に停止すべき軌跡の軌道計画が
求まった段階で既に求められた減速計画に従って減速す
るようにする。

【０１９０】このようにすることにより、途中で停止信
号が入った場合の減速もスムーズにでき、またループす
る軌跡に対しては停止することなく連続して動作させる
ことができる。具体的内容については後述する。

【０１９１】以上のように軌道計画されたときに、ロボ
ットアームを加減速動作させる場合のテーブル参照法に
ついて図２２を用いて説明する。

【０１９２】図２２において、初期値として、加速テー
ブルの参照開始アドレスを示すアドレスがＫＡＳＯＫＵ
に、減速テーブルの最終アドレス（最小値）がＧＥＮＳ
ＯＫＵに、テーブル参照ステップがΔに、減速テーブル
値の総和がＤＰＵＬＳに、テーブル値をロボットアーム
を動作制御する値に変換する変換定数がＳＰＤＧに設定
されている。これはテーブル参照計画につき説明したも
のと同様である。

【０１９３】また、テーブル参照を制御するフラグとし
て、Ｆlagに加速モードが設定されている。このような
条件で、図２２のテーブル参照が実行される。

【０１９４】まず、最初はＦlagに加速モードが設定さ
れているから、ブロック４１００の判断によりブロック
４１０１が実行される。ブロック４１０１では、ＫＡＳ
ＯＫＵの示すアドレスが最大を越えたか否かがチェック
される。最初は越えていないから、ブロック４１０３が
実行される。すなわち、ＫＡＳＯＫＵの示す加速テーブ
ルアドレスより加速テーブル値を読み出し、レジスタに
設定する。

【０１９５】次に、テーブル参照ステップΔをＫＡＳＯ
ＫＵ及びＧＥＮＳＯＫＵに加える。ここでＧＥＮＳＯＫ
Ｕに加えているのはここで説明は省略するが、動作途中
でロボットアームを強制減速させる場合の減速テーブル
アドレスを設定するためである。これにより次に参照す
べきテーブルアドレスが更新される。

【０１９６】ここで前記したようにΔ，ＫＡＳＯＫＵ，
ＧＥＮＳＯＫＵは小数点以下の値を含み、実際のテーブ
ル参照時はＫＡＳＯＫＵまたはＧＥＮＳＯＫＵの整数部
が用いられる。そして得られたテーブル値は、ブロック
４１１０において変換定数が掛けられ、実際にロボット
アームを駆動する速度を得る。

【０１９７】上述のプロセスは繰り返し実行され、加速
度の速度パターンが次々にテーブルを利用して求められ
る。そして遂には、ＫＡＳＯＫＵテーブルの最大上限に
到達する。このときはブロック４１０２及び４１１０が
実行される。すなわち、加速が完了して以降は定速また
は減速モードで動作することになるから、減速の開始ア
ドレスをＧＥＮＳＯＫＵに設定し、判断フラグＦlagに
定速モードを設定し、ＧＥＮＳＯＫＵの示す減速テーブ
ル値をレジスタに読み出し、変換して動作速度を得る。

【０１９８】次にテーブル参照が起動されたときは、定
速モードであるから、ブロック４１０４以降が実行され
る。すなわち、 残り移動量＝目標位置−現在位置 または、 目標動作距離−既動作距離 減速所要量＝減速テーブルの積算値（ＤＰＵＬＳ）×変
換定数（ＳＰＤＧ） の比較が行なわれ、前者が後者より大であれば、減速の
必要がないから、ブロック４１０５及び４１１０の処理
が実行される。

【０１９９】ＧＥＮＳＯＫＵの示すテーブルアドレス値
が読み出されるから、ＧＥＮＳＯＫＵの値が変化しなけ
ればその値は一定であり、すなわち等速の速度指定が得
られ、ロボットアームは等速で動作することになる。こ
の状態が続くと、ロボットアームは順次動作目標点に近
づくから、ブロック４１０４の判断結果はＮＯとなり、
ブロック４１０６以降の処理が実行される。

【０２００】ブロック４１０６においては、ＧＥＮＳＯ
ＫＵの値がテーブル参照ステップΔだけ小さくされる。
すなわち、減速テーブルの低速側の値を示すアドレスを
示すような値が更新される。ブロック４１０７及び４１
０８は減速テーブルの定義外を参照するのを防止するた
めの安全対策である。

【０２０１】ブロック４１０９において、ＧＥＮＳＯＫ
Ｕの示すアドレスより減速テーブル値が読み出される。
また、その読み出した値だけＤＰＵＬＳの値が減じら
れ、次の参照のときの減速所重量の値を更新している。
ブロック４１１０は、前述した内容の共通処理である。
このようにしてロボットアームに加速→等速→減速また
は加速→減速を指令する指令値が得られる。

【０２０２】ここで、上述では示さなかったが、移動中
のロボットアームを強制的に減速して停止させる必要が
生じた場合には、前記したように、どこから減速しても
良いように、減速テーブルを参照するためのアドレスが
ＧＥＮＳＯＫＵに設定されているから、これを用いて常
時ブロック４１０６以降を実行させれば、ロボットアー
ムを減速制御することができる。この場合は、ブロック
４１０９のＤＰＵＬＳに関する補正を行なう必要はな
い。

【０２０３】以上のテーブルの参照計画及びテーブル参
照によってロボットアームの１軸を加減速制御した場合
の例を図２３に示す。図２３において、は動作距離が
短く、テーブルパターンが縮少されて加速→減速のモー
ドで動作した場合であり、は動作距離が十分長く、加
速→等速→減速のモードで動作した場合であり、は丁
度両者の境界に相当する場合である。

【０２０４】以上述べたように、本発明に係る加減速テ
ーブル処理法によれば、動作距離の大小に関せず、常に
最適なパターンでロボットアームを動作させることがで
きる。

【０２０５】以上述べた加減速パターン処理法において
は、無次元速度をテーブル値とする加速テーブル及び減
速テーブルを利用するものとして説明した。ここで、加
速パターンと減速パターンが同一である場合は、加速パ
ターンテーブルのみを設け、減速テーブルと共用するよ
うにしても本発明の内容とは何ら変更することなく適用
することができる。

【０２０６】また、テーブル値を無次元速度でなく、無
次元動作距離としてテーブルを構成しても良い。この場
合に無次元速度が必要とされたときは、テーブル値の差
を取ることにより容易に無次元速度を求めることがで
き、上述の説明におけるテーブル処理方式を多少変更す
ることで対処できる。

【０２０７】また加速テーブルは無次元速度を、減速テ
ーブルは無次元動作距離を基準として与えるようにして
も本発明に係る内容は容易に適用でき、実質同一結果が
得られる。

【０２０８】また、加減速パターンが、ある所定の関数
によって記述できるときは、前記テーブルに代えて、関
数値演算処理部を設け、テーブル参照を関数値演算とす
れば、容易に本発明を実現できる。この場合は、関数特
有の特性を利用して加減速計画を実現することもでき、
これは前述した通りである。

【０２０９】以上の説明では、直線／曲線動作をさせる
場合には、動作距離の表現によってあたかも位置のみに
ついてであるかのように説明した。しかし、ロボットア
ームの回転角についても上述の内容で実行されることは
いうまでもない。これについては考え方を明示するため
に、以下の座標変換の項で一つの実施例を示す。

【０２１０】次にロボットアームを直交空間上で一定の
軌跡を描いて動作させる場合に重要となる座標変換手法
について説明する。

【０２１１】図２４、図２ロボットアームの座標系の定
義を示す。ロボットの支柱軸上にＺ₀軸をとり、設置面
上にＸ₀軸、Ｙ₀軸をとり、これを静止座標系とする。こ
こでＸ₀軸は、ロボットの正面方向とする。

【０２１２】θ₁，θ₂，θ₄，Ｚ₃₀が与えられたとき
の、グリップ先端の位置（Ｘ₀，Ｙ₀，Ｚ₀）は次式で与
えられる。

【０２１３】 Ｘ₀＝ａ₁₀ｃθ₁＋ａ₂₀ｃθ₁₂＋ａ₄₀ｃθ₁₂₄ Ｙ₀＝ａ₁₀ｓθ₁＋ａ₂₀ｓθ₁₂＋ａ₄₀ｓθ₁₂₄ ｚ₀＝Ｚ₃₀−ａ₅₀ ここで、ａ₁₀，ａ₂₀は、それぞれ第１及び第２アームの
長さである。ａ₄₀，ａ_５０は、上下軸下端に取りつけら
れたグリップ先端位置の水平面内及び垂直方向成分であ
る。また、

【０２１４】

【数１５】

【０２１５】である。

【０２１６】通常のロボットの座標変換に関する演算の
基礎式は上式であるが、ここではａ_１０≧ａ₂₀＞＞
ａ₁₀，ａ₄₁と仮定して、次の正規化を行う。

【０２１７】

【数１６】

【０２１８】その結果、上式は次下による。

【０２１９】

【数１７】

【０２２０】上記では、正規化の定数を２a₁₀にしてい
るが、これに限定されるものではない。例えば、正規化
の定数をa₁₀またはa₂₀のように定めても良い。

【０２２１】位置（Ｘ，Ｙ，Ｚ，θ₁₂₄）が与えられた
ときの関節角（θ₁，θ₂，Ｚ₀，θ₄）を求める。但し、
ここではＺは扱わない。(後のエンコーダ値と関節角と
の関係式の項で扱う)。

【０２２２】まず、以下のように定義する。

【０２２３】

【数１８】

【０２２４】このとき、式（３.３）は次のようにな
る。

【０２２５】

【数１９】

【０２２６】幾何学的な位置関係は、図２５に示すとお
りである。図２５から、解（θ₁，θ₂）の組が２個存在
することが予想される。いずれの解を採るかは、θ₂の
符号により決定できる。θ₂の符号は、あらかじめＳＧ
Ｎ（θ₂*）として与えられているとする（ここで * は
θ₂の上付きの添字である）。

【０２２７】式（３.５）により、次のようにθ₁が求め
られる。

【０２２８】

【数２０】

【０２２９】同様に、式（３.５）より次のようにθ₂が
求められる。

【０２３０】

【数２１】

【０２３１】ロボットの機械原点、すなわち原点合せ姿
勢から、図２４に示す静止座標原点姿勢までロボットを
動作させるときのエンコーダ値の変化量をＥ_io、サーボ
モータの１回転当りのエンコーダ値をＥ_io*、減速機の
減速比をＲ_iとすると、ロボットの機械原点姿勢から静
止座標原点姿勢までの関節角の変化量、すなわち原点角
θ_ioは、次のように与えられる（ここで * はＥ_ioの上
付きの添字である）。

【０２３２】

【数２２】

【０２３３】但し、ボールスクリューで駆動される第３
軸の場合は、上式の減速比１／Ｒ_iをボールスクリュー
のリードｌ_i（mm）で置換することにより、正規化され
た移動距離Ｚ₃'が次式のように与えられる。

【０２３４】

【数２３】

【０２３５】同様の考察により、エンコーダ値Ｅ_iとロ
ボットの動作関節角θ_iとの間には、次の関係式が成立
する。

【０２３６】

【数２４】

【０２３７】ここで、式（３.１４）のθ₄の右辺第３項
は、ベルト伝達系に介在する歯車系の歯数比が異なるこ
とによって生じた第２軸回転の干渉成分である。また、
Ｒ₄*は第２軸干渉時の伝達系の減速比を表わす（ここで
* はＥ_ioの上付きの添字である）。

【０２３８】さらに、上式の○で囲んだ＋，−の符号
は、ロボット機械系のエンコーダ値が増加する場合に、
静止座標系のロボット関節角が増加するときに上式の符
号をとり、逆の場合は反転すべきものであることを示し
ている。

【０２３９】以上は、図２に示すロボットアームについ
て示したが、これに限定されるものでなく、一般の関節
角ロボットについても、同様の関係を導びくことができ
る。なお、ここで特記すべき事項は、本アルゴリズムは
正規化手法を導入したことにより、演算量が減少するこ
と、並びに固定少数点演算方式を採る場合には、演算有
効数字を増大することができるという顕著な効果を示し
ていることである。

【０２４０】また特公昭６１−２９６４に示されるよう
に通常は、ロボットアームを動作させるための教点点の
記憶に、Ｘ，Ｙ，Ｚのような直交座標値を用いる方法が
採られているが、直交座標値を用いる場合には、位置か
ら関節角を求める場合に多値解が存在すれば求解ができ
ないという問題がある。これに対し、エンコーダ値を記
憶するようにすれば、常に求解可能であり、ロボットア
ーム動作制御比の問題がなくなる。

【０２４１】以上、サーボ制御処理及びロボットアーム
を動作させるための基本要件について説明した。以下、
本発明を具体的に実現する処理内容について詳述する。

【０２４２】まず、図示しない電源スイッチを投入する
ことにより、ロボット制御部は初期化され、操作入力待
の状態となる。同時にサーボ制御処理部も初期化され、
動作軸すべてについて非常停止コマンドを与えられ、一
定のサンプリング周期毎に、各軸について循環的に非常
停止コマンドを実行する。

【０２４３】ここで非常停止コマンドは各動作軸のアク
チュエータに、何も駆動信号を与えないよう動作させる
指令である。従って、例えば図２のロボットアームは、
静止したままである。また、図２のロボットアームで上
下軸に負荷のかかるようなロボットアームの場合は、図
示しない負作動ブレーキが作動し、上下軸の落下を防ぐ
ようになっている。また、上下軸以外の軸は、パワーが
供給されない状態であるので、手で触れると自由に動く
状態である。

【０２４４】次に、図示しないサーボオン電源を投入す
る。この場合、図２のロボットアームで、上下軸に負荷
のかからないものであれば、サーボ処理部は、今まで通
り、非常停止コマンドを実行し続ける。

【０２４５】上下軸に負荷のかかるもので、上下軸に負
作動ブレーキを付けられたものであれば、ブレーキが解
除されるので、サーボ処理部の非常停止実行部で、サー
ボオン信号を検知して、上下軸のみについて、前記した
位置保持コマンドを実行するようモードを切り換えて、
処理するよう構成する場合もある。

【０２４６】また、図３のロボットアーム以外のもの、
すなわち、垂直多関節形ロボットアームの場合は、常に
重力負荷を受けているので、アームの全てまたはほとん
どの動作軸に負作動ブレーキをつけているので、これが
解除されることになり、そのため、全動作軸につき、前
述の位置保コマンドを実行する、すなわちいわゆるサー
ボロックされる。

【０２４７】図３に示すようなロボットアームについて
も、全軸サーボロックされるよう動作させる場合があ
る。これは、非常停止コマンド実行処理部に、サーボオ
ン信号検出部を設け、サーボ処理部が自律的に、位置保
持コマンドを実行するよう構成することにより、容易に
実現できる。

【０２４８】ロボット制御部は、操作入力待の状態にあ
るので、図４に示したように、操作入力に応じて種々の
動作を実行することになる。

【０２４９】まず、第１に操作入力すべきものは、図３
に示す原点合せ釦を押してロボットアームの原点合せを
実行することである。これにより、第１図のパルスカウ
ンタ初期化と、ロボットアーム姿勢の初期化及び演算処
理部の初期化が行なわれ、演算系と機構系の同期が得ら
れ、以降は、演算系の指示に従って、ロボットアームを
種々の動作モードを動作させることができるようにな
る。

【０２５０】図４の原点合せ処理部２１は、前述の原点
合せコマンドを、ロボットアームの所定の軸について、
順次サーボ処理部に提供し、その完了を待って別のコマ
ンド、別の動作軸に与えることにより実行する。ここで
は、これの詳細は省略する。

【０２５１】次に、図３に示すプログラムティーチ釦を
押すと、ロボットアームの演算処理は、図４の動作教示
処理部２２に移る。動作教示においては、図３のデータ
入力キーを操作して、オペコード表示部にロボット動作
制御コードである記号言語を設定し、セットキーを押す
ことにより、図１の動作手順記憶手段１７の所定のエリ
アに動作制御コードが格納される。これを繰り返すこと
により、一連のロボットアームの動作手順を作成するこ
とができる。動作制御コードについては、世にいうロボ
ット言語とほぼ同様であり、周知でもあるので、詳細は
省略する。

【０２５２】次に、図３に示す位置ティーチ釦を押す
と、演算処理は図４の位置教示処理部２３に移る。

【０２５３】位置教示処理部では、ロボットアームを図
３の位置調整キーを操作して、目標とする動作点まで動
かし、位置決めしたのち、データ入力キーを操作してス
テップ表示部へステップ番号（図１の位置記憶手段１８
の記憶アドレスを表わす）を入力し、セットキーを押し
て、位置決め点の位置データ（ここでは特にEncorder
値）を、ステップ番号の示す図１の位置記憶手段の記憶
アドレスに格納する。ここでステップ番号の入力は、い
つでも良く、セットキーを押すまでに入力されていれば
良い。

【０２５４】もう一つの別の処理は、既に位置の教示が
終っている教示点を修正したい場合のロボットアームの
動作制御であり、この場合は、図３のデータ入力キーを
操作して、既教示済のステップ番号をステップ表示部に
入力すると、図４の位置教示処理部２３は、ステップ番
号に対応する位置データの存在をチェックし、存在すれ
ば、オペコード表示部に、入力されたステップ番号位置
へのロボットアームの動作制御コードを表示する。

【０２５５】このとき、図３のステップキーを押せば、
位置教示手段は、ロボットアームをオペコード表示部に
表示された動作制御コードに従って、ロボットアームを
ステップ番号位置へ動作させる。したがって、この位置
で前記した位置調整キーを操作して、ロボットアームの
微調整を行ない、セットキーを押して、既教示点を修正
することができる。

【０２５６】また、この位置教示モードにおいては、ロ
ボットアームの動作方向を何種類か選択することができ
る。その１は、ロボットアーム固有の動作軸を個々に動
かす関節モードである。このモードでは、例えば図３の
アーム（＝右＞）の調整キーを押せば、ロボットの第１
アームがロボットに向かって右に動き、（＜前＝）の調
整キーを押せば、ロボットの第２アームがロボットに向
かって左に動く。

【０２５７】その２は、前記したように、ロボットアー
ム固有の動作軸とは異なる直交座標系の軸に沿ってロボ
ットアーム先端を動かす直交モードである。このモード
では、例えばアーム（＝右＞）の調整キーを押せば、ロ
ボットアームは＋Ｙ軸方向に、アーム（＜前＝）の調整
キーを押せば、ロボットアームは、＋Ｘ軸方向に動く。

【０２５８】その３は、ロボットアーム先端に取りつけ
られたグリップの方向（Ｘ'軸）とそれに直角な方向
（Ｙ'軸）に沿ってロボットアーム先端を動かす手先モ
ードである。このモードの動きは、直交モードの場合の
ＹをＹ'、ＸをＸ'のように置き換えたものである。この
ような動作方法は、例えば図３のユーティリティキーを
押し、データ入力キーで定められたコードを入力し、セ
ットキーを押すことにより選択または切り換えることが
できる。

【０２５９】また、この位置教示モードにおいては、ロ
ボットの動作速度をいく通りにか変更指定することがで
きる。例えば、図３のデータ入力キーのＨキーを押す
と、ジョブ表示部にＨの表示ができ、予かじめ定められ
た高速の動作速度、Ｌキーを押すと、ジョブ表示部にＬ
の表示が出、予かじめ定められた低速の動作速度を指定
することができる。

【０２６０】位置教示モードにおいて、調整キーを押し
てロボットアームを目標位置に誘導位置決めする手順
を、図２６を用いて説明する。ここで、アームの動作方
法、動作速度は、前記したようにしてすでに設定されて
いるものとする。また、ロボットアームの現在位置は、
エリアＮに設定されているものとする。

【０２６１】位置教示モードにおいて、図３の位置調整
キーのいずれかが押されると、図２６に示す処理が実行
される。まずブロック５０２０において、押された調整
キーにつき、指定された動作モードにおける動作限界位
置を求めエリアＴに設定する。動作限界位置は、例え
ば、動作モードが関節モードであり、押された調整キー
が、アーム（＝右＞）である場合、図２のロボットアー
ムの第１アームが右側へ動ける限界のエンコーダ値であ
る。

【０２６２】このような各動作軸の限界位置は、図示し
ないが、あらかじめシステム設計時に決められ、共通Ｒ
ＯＭまたはＲＡＭメモリ上に記憶されている。したがっ
て、押された調整キーに応じてメモリから該当するもの
を読み出してくれば良い。

【０２６３】そして、現在位置エリアＮに格納されてい
る各軸のエンコーダ値を目標位置エリアＴのエンコーダ
値エリアに移し、前記読み出した限界位置を調整キーで
指定されたエリアＴのエンコーダ値エリアに格納し、更
に、このエンコーダ値を前記した正変換手法により、関
節角、直交座標値及び手先姿勢に変換し、エリアＴの該
当エリアに設定すれば良い。

【０２６４】動作モードが直交モードである場合のロボ
ットアームの動作限界位置は図２のロボットアームの場
合次のように求めることができる。ロボットの現在位置
と教示釦により指示される動作方向ベクトルより、ロボ
ットの動作範囲境界上に位置する動作目標位置を算出す
る方法を示す。

【０２６５】（１）アームの伸縮から決まる動作目標値 グリップの姿勢は静止空間で一定に保つとする。このと
き、グリップ先端を直線動作させると、第２アームの先
端も直線動作する。第１アームの回転を自由とし、第２
アームの回転を上下限値内に押えると図２７の内側リミ
ット及び外側リミットで示される同心円ではさまれた第
２アームの先端部の動作領域が得られる。

【０２６６】図２７の位置（Ｘ₀，ｙ₀）から方向ベクト
ル（Ｖ_X，Ｖ_y）の方向へ動作させるときの動作境界上の
位置（ｘ，ｙ）は次のように求められる。

【０２６７】ロボットの動作距離をｌ（≧０）とする
と、動作直線は次式である。

【０２６８】Ｘ＝ｘ₀＋ｌｖ_X Ｙ＝ｙ₀＋ｌｖ_y 同心円の中心を通る動作直線への直交直線は、次式であ
る。

【０２６９】Ｘ＝ｌ₁ｖ_y Ｙ＝−ｌｖ_X ２直線の交点は、垂線の長さをｌ₁、点（Ｘ₀，ｙ₀）よ
り交点までの長さをｌ₀とすると、 ｌ₁ｖ_y＝ｘ₀＋ｌ₀ｖ_X −ｌ₁ｖ_X＝ｘ₀＋ｌ₀ｖ_y を解いて、次式のように求められる。

【０２７０】ｌ₁＝ｘ₀ｖ_y−ｙ₀ｖ_X ｌ₀＝−ｘ₀ｖ_X−ｙ₀ｖ_y ここで、ｌ₁は常に≧０である。ｌ₀は動作直線が交点の
方向へ向かうとき＜０、交点より離れる方向へ向かうと
き＞０である。

【０２７１】以上の図示により、点（ｘ₀，ｙ₀）は常に
動作範囲にあるとすると、動作目標点を与えるｌは次式
となる。

【０２７２】（i） ｌ₁≧Ｒ_i （動作直線が内側リミッ
トと交わらない） または ｌ₀≦０ （動作直線が外側リミットへ向かっている） のとき ｌ＝ｌ₀＋√（Ｒ₀×Ｒ₀−ｌ₁×ｌ₁） （ii）ｌ₁≦Ｒ_i （動作直線が内側リミットと交わる） かつ ｌ≧０ （動作直線が内側リミットへ向かっている） のとき ｌ＝ｌ₀−√（Ｒ_i×Ｒ_i−ｌ₁×ｌ₁） （２）グリップとロボット本体との干渉により制御され
る目標値 第２アーム先端にオフセット付のグリップがついた場
合、“周辺装置との干渉”及び“ロボット本体”との干
渉の問題が生じる。ここでは、“ロボット本体”との干
渉だけ扱う。グリップベクトルを（Ｇ_X，Ｇ_y）とすると
ロボットのグリップ先端の軌跡は、次のようになる。

【０２７３】Ｘ＝ｘ₀＋Ｇ_X＋ｌｖ_X Ｙ＝ｙ₀＋Ｇ_y＋ｌｖ_y 図２７において、干渉半径Ｒ_kの内側リミットのみ存在
するとすれば、（１）の場合と同様に求解できる。
（１）の場合と同様の記号を流用すれば、グリップとロ
ボット本体との干渉により制御される目標値は、次のよ
うになる。

【０２７４】（i）ｌ₁≦Ｒ_kかつｌ₀≧の場合 ｌ＝ｌ₀−√（Ｒ_k×Ｒ_k−ｌ₁×ｌ₁） （３）第１アーム関節角リミットにより制限される目標
値 第１アームが関節角リミットに固定されると、第２アー
ムの回転のみ自由となる。そこで、第１アーム先端を中
心とし、第２アーム長を半径とする円とロボットの第２
アーム先端の動作直線との交点が、第１アームの関節角
リミットにより制限される、以下に示す目標値となる。

【０２７５】ｌ＝ｌ₀±√（ａ₂×ａ₂−ｌ₁×ｌ₁） ただし、 第１アーム関節角が下限リミットにある場合 第２アーム関節角＞０（右きき）なら−符号 第２アーム関節角＜０（左きき）なら＋符号 第１アーム関節角が上限リミットにある場合 第２アーム関節角＞０（右きき）なら＋符号 第２アーム関節角＜０（左きき）なら−符号 をとるものとする。

【０２７６】（４）グリップ回転角リミットにより制限
される目標値 グリップ回転角θ₄がリミットにあると、θ₁＋θ₂＋θ₄
＝一定であるからθ₁＋θ₂＝一定すなわち、第２アーム
の姿勢が静止空間上で一定となる。したがって、位置
（ｘ₀，ｙ₀）を第２軸の位置ベクトル分だけシフトした
動作経路、 Ｘ＝ｘ₀−ａ_2Cθ₁₂＋ｌ_VX Ｙ＝ｙ₀−ａ_2Sθ₁₂＋ｌ_Vy と第１アーム先端の描く円軌跡との交点が、グリップ回
転角により制御される、次下に示す目標値となる。

【０２７７】ｌ＝ｌ₀−√（(1/2)×(1/2)−ｌ₁×ｌ₁） 以上の解析により得られた動作距離の中の最小のもの
が、実際の動作限界点を与える限界動作距離である。

【０２７８】以上の動作距離ｌ及び方向ベクトルυ及び
現在位置Ｎより、動作限界位置（Ｘ，Ｙ，Ｚ，θ₁₂₄）
を求めることができるから、これを逆変換することによ
り、メモリエリアＴに動作限界位置を設定することがで
きる。

【０２７９】ブロック５０２１においては、動作速度、
動作方向に対して与かじめ定められた量だけ移動する、
ブロック５０２０のデータをもとに、同様の考え方で、
ロボットアームの微小移動量を求めることができ、これ
をエリアＮに設定する。

【０２８０】ブロック５０２２においては、前記エリア
Ｎのエンコーダ値を、前記したサーボ制御用の各軸用メ
モリの動作指令位置エリアに設定し、サーボ処理部に１
ステップ送りコマンドを実行させる。１ステップ送り制
御コマンドを共通メモリのコマンドエリアに設定し、１
ステップ送りは、前記したように即座に完了するから、
エリアＮに設定した位置データは、ロボットアームの現
在位置を示すことになる。 従ってブロック５０２３に
おいて、所定時間の間に調整キーが離されたことが確認
されると、この処理は完了することになる。このモード
により、ロボットアームを微少送りさせて精密位置決め
することができる。ブロック５０２３において、ずっと
調整キーが押され続けていることが確認された場合に
は、ブロック５０２４及び５０２５を実行してロボット
アームを加減速制御モード動作させる。 すなわち、ブ
ロック５０２４において、動作モード、動作速度、及び
現在位置Ｎ、目標位置Ｔが与えられているから、前記ま
たは類似の方法によりロボットアームの軌道計画をす
る。

【０２８１】次に、５０２５において軌道計画に従が
い、定められた加減速パターンでロボットアームを動作
させる。ここで、図示していないが、ブロック５０２５
においても、調整キーの押されているか否かは常にチェ
ックされており、もし離されると、ロボットアームは、
強制的に定められた減速パターンに従って減速停止させ
られる。

【０２８２】このモードにより、ロボットアームを任意
の方向に動かして、任意の位置に大略の位置決めをする
ことができる。

【０２８３】次に、前記した既に教示した点へロボット
アームを動作させる場合について図２８を用いて説明す
る。前記したように図３のオペコード表示部にロボット
アームの動作制御部コードが表示されているとする。ま
た動作モード、動作速度も設定されているとする。

【０２８４】図２８において、図３のステップ釦が押さ
れると、ブロック５０４０において、図３のオペコード
部に表示されたオペコードを解説し、オペコードに含ま
れている位置No.から図１の位置記憶手段１８に記憶さ
れている位置（エンコーダ値）をとり出し、前記したエ
リアＴのエンコーダ値エリアに設定し、更に座標変換に
よって関節角及び直交座標位置及び姿勢を求め、これも
エリアＴの対応するエリアに設定する。これにより、現
在位置Ｎ及び動作目標位置Ｔが定まる。

【０２８５】ブロック５０４１及びブロック５０４２の
内容は、ブロック５０２１及びブロック５０２２と同じ
である。但し、ブロック５０２２においては、図３のス
テップ釦の継続的操作がチェックされる。これにより、
既に教示された位置へロボットアームを動作させること
ができる。

【０２８６】このようにして、ロボットアームの動作制
御プログラム及び動作位置の教示が完了すると、その教
示結果を再生することができる。図３のテスト運転釦を
押すと、図４のテスト動作処理部２４、図３の連続運転
釦を押すと、図４の再生動作処理部２５が起動される。

【０２８７】ここで、テスト動作処理部２４と再生動作
処理部２５の相異は、前者の場合は、与かじめ定められ
た低速でしかロボットアームが動作しないよう、すなわ
ち動作速度が制限されるテスト運転であるのに対し、後
者は、実運転モードであり、前記起動制御プログラムに
指定された速度通り動作することである。

【０２８８】以下では連続運転釦を押した場合について
説明する。

【０２８９】次に、データ入力キーを操作して２桁のジ
ョブ番号を入力すると、図３のジョブ部にそれが表示さ
れる。次にセットキーを押すと、もし指定された番号の
ジョブが存在すれば、その動作制御コードのステップ番
号がステップ部に、動作制御コードがオペコード部に表
示される。ここでステップ釦を押すと、表示されたオペ
コード部の動作制御コードが１ヶ実行されて終了する。

【０２９０】もし動作制御コードがロボットアームの動
作を指定するものであったときは、前記した位置教示の
際のステップ釦押しと同様の方法でロボットアームを動
作させる。順次ステップ釦を押すことにより、動作制御
コードが定められた順に１個ずつ実行される。これは、
ロボットアームの動作については、内容的には前記した
位置教示の際のステップ釦押しの場合と同様であるの
で、詳細は省略する。

【０２９１】リピート釦を押した場合は、次の動作制御
コードを読み出して実行する。これは、動作制御コード
の停止コードにそうぐうするか、または停止釦が押され
るまで継続される。従って、動作制御がループするよう
なコードが教示されていれば、ロボットアームは一連の
定められた順次で定められた経路で繰り返し動作するこ
とになる。

【０２９２】リピート釦が押されたときの処理を図２９
〜図３３を用いて説明する。

【０２９３】図２９において、ブロック６０００は、前
記した内容の再生運転開始のための釦操作を示してい
る。ブロック６００１で次にリピート釦を押すと、ブロ
ック６００２は、ストップ入力またはその他のここでは
示さない停止入力があるまで、ブロック６００３以降の
処理を繰り返し実行するよう作用する。もしストップ入
力他があった場合は、ブロック６００１へ戻り、リピー
ト釦入力を待ち、入力があると同じことを繰り返す。

【０２９４】ブロック６００３以降の繰り返しループで
は次のような処理を行なう。ブロック６００３では、順
次、動作手順コードを動作手順記憶手段より取り出しそ
の内容を解読する。ブロック６００４で、解読結果に応
じてブロック６００５〜６００９のいずれかの処理を実
行させ、完了すると、また繰り返して、ブロック６００
３から処理を実行する。

【０２９５】ブロック６００５は、直接本発明に関係し
ない動作コード処理を実行する部分である。ブロック６
００６は動作速度が指定されたとき、それを記憶する部
分である。ブロック６００７は、ロボットアームを間欠
運転または連続運転で動作させる指定を記憶する部分で
ある。

【０２９６】ここで、間欠運転とは、例えば、教示され
た２点間を直線で結ぶ動作、またはロボットアームの機
械座標系基準で動かす動作等において、加速−等速−減
速−停止のモードで運転する方法を示す。連続運転と
は、複数の教示点間を動作開始点及び終了点ではそれぞ
れ加速及び減速を実行させるが、途中の教示点では停止
することなく連続して運転する方法を示している。

【０２９７】ブロック６００７では、図３１に示す追加
的処理が必要であり、これは後述する。ブロック６００
８は、関節で動くか直交で動くかの指定を記憶する部分
である。ブロック６００９は、ロボットの動作命令コー
ドを実行する部分であり、詳細は、第３０図により説明
する。

【０２９８】ブロック６０２０は、ロボットアームの次
の軌道計画が終了した時点で停止入力等によりロボット
アームが停止させられたような場合の再開条件をチェッ
クしている。すなわち、既に軌道計画が完了している状
態では、今指令された動作命令を実行することができな
いから、ブロック６０２１以降の処理により、既軌道計
画値を実行させる。すなわち、ブロック６０２１で既軌
道計画を実行するようサーボ系に指令する。これは後述
する、図３２の処理により実行される。

【０２９９】次にブロック６０２２で指定された動作方
法が間欠モードか否かをチェックし、間欠モードであれ
ば、ブロック６０２３において、指令した軌道計画の実
行完了を待つ。これも後述する、図３２の処理を用いて
実行することができる。

【０３００】次にブロック６０２４において、同一ステ
ップの動作コードを再実行させるよう準備して、処理を
終了する。これにより、演算処理ループは、ブロック６
００３へ戻り、ブロック６００９、すなわち、再度図３
０の軌道計画が実行される。前記の処理の必要ない場合
は、ブロック６０２０の判断により、ブロック６０２５
以降の処理が実行される。

【０３０１】ブロック６０２５では、位置記憶手段よ
り、動作コードに指定された位置番号に相当する位置デ
ータ（エンコーダ値）をとり出し、更に、ブロック６０
２６で座標変換により、ロボットアームの関節角及び直
交座標値、姿勢を求める。

【０３０２】次に、ブロック６０２７において、軌道計
画ができるだけの位置データがそろったかどうかがチェ
ックされる。すなわち、例えば、図２０に示すような、
先行後続関係で動作するモードであれば、始点Ａ、終点
Ｈ及び中間点の高さを示す位置データ、直線で動くモー
ドであれば、始点と終点を示す位置データ、円弧上を動
くモードであれば、始点と中間点と終点を示す位置デー
タがそろっているかどうかがチェックされる。

【０３０３】もし、そろっていなければ、処理を終了
し、図２９のブロック６００３に戻る。従って、その後
の演算処理により、ブロック６０２７のチェック結果が
イエスとなり、ブロック６０２８以降の処理を実行す
る。ブロック６０２８では、指定されたロボットアーム
の軌道計画をし、前記したようなロボットアームの動作
定数を決定する。

【０３０４】次に、ブロック６０２９において、ロボッ
トアームが停止中であるか否かかがチェックされる。こ
れは、前記した共通メモリの各軸動作状況エリアを参照
することにより判断される。停止中であるならば、ロボ
ットアームを駆動開始する処理、すなわちブロック６０
３０、６０３１、６０３２の処理が行なわれる。これは
前記したブロック６０２１、６０２２、６０２３の処理
と同一である。これにより、間欠モードの時は、軌道計
画に従って加速−等速−減速−停止でロボットアームが
動き、連続モードのときは、ロボットアームの軌道命令
のみが出される。

【０３０５】ブロック６０２９において、ロボットアー
ムが動作中であると判断されたときは、ブロック６０３
３の処理へ移る。ブロック６０３３では、停止入力のチ
ェック例えば図３のストッパ釦入力のチェックが行なわ
れる。ストッパ入力がなければ、ブロック６０３４にお
いて、軌道計画完了のフラグを設定し、ブロック６０３
５において、補間部が新しい軌道計画をとり込むのを待
つ。そして、軌道計画が取り込まれると、次の軌道計画
を実行するために、図２９のブロック６００３へ戻る。

【０３０６】停止入力があった場合は、ブロック６０３
６において、サーボ系の動作完了を待ち、すなわち、以
前設定した軌道計画に従ったロボットアームの動作完了
を待つ。この場合、新しく設定された軌道計画は、実行
されないままで、図２９のブロック６００２の作用によ
り、ブロック６００１でのリピート釦入力待となる。こ
れにより、前記したブロック６０２０〜６０２４の処理
の意味が明らかとなっていよう。

【０３０７】以上のようにロボットアームが動作してい
る時に、図２９のブロック６００７の処理が指令された
場合の詳細を図３１によって説明する。図３１の処理は
図３０の処理でみたように、連続モードのときは、ロボ
ットが動いている状態で先行して動作コードの解読が行
なわれ、また軌道計画データが作成されるようになって
いるので、動作モードを切り換るような動作モードが発
生したときに、この先行分を処理するために必要とされ
ている。

【０３０８】図３１において、ブロック６０５０の処理
は、前記ブロック６００７に示したものと同一である。

【０３０９】次にブロック６０５１において指定された
動作モードのチェックが行なわれる。もし連続モードの
ときは、何もする必要がない。間欠モードであるとき
は、ブロック６０５２において、軌道計画されたデータ
があるか否かがチェックされる。もしあれば、更にブロ
ック６０５３において、ロボットアームは動いているか
否かがチェックされる。そしてロボットアームが動いて
いるならば、ブロック６０５４、６０５５、６０５６の
処理により、現在の動作に続けて、軌道計画データをも
実行させ、その完了を待って、処理を終了する。

【０３１０】ブロック６０５３の判断でロボットアーム
が動いていなかった場合は、軌道計画値をブロック６０
５７、６０５８において加減速モードで実行させてその
完了を待って処理を終了する。またブロック６０５２の
判断の結果、軌道計画されたデータがない場合は、ブロ
ック６０５９においてロボットアームが動いているか否
かを判断し、動いていればブロック６０６０で停止を待
って処理を終了する。

【０３１１】このようにして、すでに計画されたものを
すべて実行し、ロボットアームが停止するまで待つよう
にしているので、それ以降の欠間運転に整然と動作モー
ドを移すことができる。

【０３１２】上述の説明では、軌道計画時に例えば関節
モードと直交モードが混在して指定されるような場合に
ついては説明を省いているが、実用の演算処理では、位
置データ及び動作手順データの正当性をチェックして安
全側の動作するよう処理手順が作成されていることのみ
付言しておく。

【０３１３】次に、図３０、図３１において、サーボ系
を起動する。サーボ制御の完了を待つの表現で示したブ
ロックを実際に実行する手順を図３２を用いて説明す
る。

【０３１４】図３３では、ＳＴＡＴＥにより実行状態を
切り換えるようにしている。ＳＴＡＴＥ＝０は、全動作
軸のサーボ制御完了を表わしている。従って、図３０、
図３１においてサーボ制御の完了を待つの表現は、図３
２の演算ブロックを実行し、ＳＴＡＴＥの値をチェック
することにより、達成される。従って、完了を待つため
には、図３２の演算ブロックを繰り返し実行し、ＳＴＡ
ＴＥの値をチェックすることと等価である。

【０３１５】また、ＳＴＡＴＥ＝０である場合は、いく
ら実行指令が発行されても、実質上は何も実行しないの
で、サーボ指令制御のダミー処理ルーチンとして利用す
ることもできる。

【０３１６】また、この処理は、図２０で例をひいて説
明した先行、後続動作を含むロボットアームの動作を制
御できるように計画されている。従って、軌道計画処理
部も、そのように軌道計画データを準備するものとす
る。

【０３１７】図３２において、サーボ系を動作させるた
めには、求められた軌道計画のデータの動作始点データ
をエリアＮに、終点データエリアＴに、また動作に必要
な計画データをワークエリア（図示せず）にセットして
ＳＴＡＴＥ＝１として最初に実行させれば良い。

【０３１８】図３２において、ブロック６１００におい
てＳＴＡＴＥの値が調べられ、その値に応じて、各処理
部が実行される。ＳＴＡＴＥ＝０の場合は前記したよう
に、何もしない。

【０３１９】ＳＴＡＴＥ＝１の場合は、サーボ系駆動の
ための最初の処理である先行動作軸の起動を行なう。ブ
ロック６１０１において、２をｓｔａｔｅにセットし、
次の実行ｓｔａｔｅを設定する。次にブロック６１０２
において、先行タイマを設定する。但し、先行タイマの
計画された値は、先行動作軸がない場合は“０”とされ
ている。次にブロック６１０３において先行動作軸の有
無がチェックされ、有ればブロック６１０４において先
行動作軸につき、サーボ起動命令が発行される。

【０３２０】図２のロボットアームの例では先行動作軸
は上下軸であり、機械座標系で動作させることができる
から、サーボ処理部の先行動作軸用メモリの動作指令位
置エリアに、先行動作目標位置を設定し、その加減速制
御のためのテーブル検索用データを各種演算定数エリア
に設定したのちに、先行動作軸につき加減速動作させる
コマンドを発行する。これを受けて、サーボ系は、先行
動作軸につき加減速制御を開始する。

【０３２１】次に、ブロック６１０５で、再度サーボ指
令部の演算を実行させる。具体的には、ブロック６１０
５は、ブロック６１００へジャンプする命令であれば良
い。

【０３２２】これにより、再びブロック６１００が実行
され、ｓｔａｔｅ＝２に設定されているから、ブロック
６１１１以降が実行される。ブロック６１１１では、前
記設定された先行タイマがチェックされる。設定時間が
経過していなければ、前記したように図３２が繰り返し
実行される過程で経過を待つ。

【０３２３】先行動作軸がない場合は、前記したことか
ら当然ながら、即、設定時間の完了となる。設定時間が
経過していると、ブロック６１１２において、次のｓｔ
ａｔｅ＝３が設定される。次にブロック６１１３で後続
動作軸の有無がチェックされ、有れば、ブロック１１４
で後続軸を起動するためのタイマがセットされる。次に
ブロック６１１５において中間動作軸につきサーボ起動
命令が発行される。

【０３２４】中間動作軸の動作が関節モードである場合
は、中間動作の各々について中間動作軸用メモリの動作
指令位置エリアに、中間動作軸の目標位置を設定し、そ
の加減速制御のためのテーブル検索用データを各種演算
定数エリアに設定したのちに、中間動作軸につき、加減
速動作させるコマンドを発行し、サーボ系は中間動作軸
につき、加減速制御を開始する。

【０３２５】中間動作軸の動作が直線動作等の直交モー
ドである場合は、前記の動作指令位置エリアに、中間動
作軸動作の初期位置として現在位置を設定し、中間動作
軸につき、継続制御動作させるコマンドを発行する。こ
れによりサーボ系は、中間動作軸につき継続動作制御を
開始する。

【０３２６】この場合、サーボ制御部の項で説明したよ
うに別の起動方法がある。このためには、図１３に示す
カウンタ３を継続制御軸指定エリア及び継続制御動作指
令位置エリアのインデックスとして用いる。そのため
に、まず、継続制御軸指定エリアをすべてクリアし、カ
ウンタ３をクリアし、カウンタ３でインデックスされる
継続制御軸指定エリアに中間動作軸指定を設定し、継続
制御動作指令位置に中間動作軸の前記初期位置を設定
し、カウンタ２をサイクリック１増加したのち、前記コ
マンドを発行する。これによりサーボ系は、中間動作軸
につき継続動作制御を開始する。

【０３２７】なお、ここでは初期値を最初のエリアのみ
設定するように説明したが、全エリアに軸指定及び初期
位置を設定のたのち前記コマンドを発行しても良い。

【０３２８】またさらには、前記した最初のエリアに設
定したのち、後述の直交補間部の機能を用いて、残りの
１部または全部を設定したのち前記コマンドを発行して
も良い。上記いずれかの方法により、継続時のサーボ制
御が開始されるが、この場合は、後述のように順次継続
制御のための目標位置を与えてやらなければならない。
次に、図３２が実行されるとＳＴＡＴＥ＝３であるか
らブロック６１２１が実行される。ブロック６１２１で
は、先行動作軸なしであったかもしくはあっても先行軸
の動作が終了しているかがチェックされる。先行動作軸
の有無は軌道計画の結果として与えられている。

【０３２９】また、先行動作軸の動作終了は前記した図
７の各軸動作状況エリアの先行動作軸につき、チェック
することにより判別できる。もし、判定結果がｙｅｓで
あればブロック６１２１で後続タイマの完了がチェック
れ、完了であればブロック６１２２でＳＴＡＴＥに４が
セットされ、あれば、前記した先行動作軸の場合と同様
にして、ブロック６１２４で後続動作軸のサーボ制御が
開始される。

【０３３０】次に、図３２が実行されるとすでに動作す
べき軸について、すべてサーボ制御コマンドが発行され
ているｓｔａｔｅ＝４であるから、ブロック６１３１が
実行される。ブロック６１３１では全動作軸のサーボ制
御完了がチェックされる。これも前記した図７の各軸動
作状況エリアを参照することにより判別できる。チェッ
ク結果がｙｅｓであれば全動作完了フラグとしてｓｔａ
ｔｅ＝０とセットされ、さらに、エリアＴに設定されて
いる動作目標位置がエリアＮに移され、現在位置情報が
更新される。

【０３３１】なお、実用の演算処理では、図３２の中に
強制減速停止処理部が用意されているが、ここでは説明
は、省略する。

【０３３２】図３２の説明において、ロボットアームを
機械座標系で動作させる場合は、すべての動作制御を自
律的にサーボ処理部が自律的に実行できるので、上記説
明で十分である。しかし、ロボットアームが直交モード
で動作する部分については、サーボ系に逐次継続制御の
データを与えてやらなければならない。図３３を用い
て、その方法を示す。

【０３３３】図３３の直交補間部は、図３２のブロック
６１１５において、直交動作が必要であった場合に初期
化される。また、間欠モード下で初期化された場合は、
所定の加速及び減速パターン処理を実行する。連続モー
ドで初期化されたときは所定の加速パターン処理が実行
され、その後は等速モードが継続する。減速処理の指令
は、図３０のブロック６０３３で停止入力があったと
き、ブロック６０３６の処理の一部として指令される。

【０３３４】もう一つは図３１のブロック６０５６また
はブロック６０６０の処理の一部として指令される。こ
の指令の発行状況及び、図３３における受け取り及び利
用状況は明示していない。上述の条件処理は、図３３の
ブロック６２２１内に含まれているものとする。

【０３３５】図３３において、ブロック６２００におい
て、補間完了か否かがチェックされる。図３３の直交補
間部は初期時及び通常時は補間完了の条件に設定されて
いる。前記したように直交補間部が初期化され、以下に
示す処理で補間完了が設定されるまでの間のみ、補間未
完了の状態にある。従ってこの直交補間部も、補間完了
の状態でいくら起動されても何も実行しない。

【０３３６】前記したように初期されて、起動されたと
きは、図３３に明示される動作距離はＬ＝０とされてい
る。ブロック６２００のチェックが否であると、ブロッ
ク６２１０において補間が必要か否かがチェックされ
る。サーボ処理部の継続制御データの受け渡しをコマン
ドを介して行なう場合は、コマンドの動作軸指定部のデ
ータを参照し、軸指定部のデータが空であったとき、補
間必要となり、ブロック６２２１以下の処理により継続
制御用指令値が求められ、継続制御用サーボ起動時と同
じ方法でデータ設定が行なわれ、サーボ系にひきわたさ
れる。

【０３３７】前記したように、図１３のカウンタ３をイ
ンデックスとしてデータ受渡しを行なう場合には、カウ
ンタ３てインデックスされる継続制御軸指定エリアが空
であるときブロック６２２１以下の処理により継続制御
用指令値が求められ、カウンタでインデックスされる継
続制御軸指定エリアに軸指定が継続制御動作指令位置エ
リアに指令値が設定され、サーボ系にひき渡されると共
に、次のチェック用に、カウンタ３がサイクリックに＋
１されるようになっている。

【０３３８】補間が必要である場合には、上記説明及び
前述の図２２の加減速パターン処理方法によってブロッ
ク６２２１において、動作距離増分ΔＬが求められる。
次にブロック６２２２にて、動作距離Ｌが求められる。
次にブロック６２２３において、求められた動作距離Ｌ
と軌道計画された動作距離Ｌ₀が比較される。

【０３３９】もし、Ｌ≧Ｌ₀である場合はブロック６２
２４で動作モードが間欠であるか連続であるかがチェッ
クされ、間欠である場合は、ブロック６２２５において
補間完了コードが設定されると共に、動作距離Ｌが軌道
計画値に設定される。連続である場合は、ブロック６２
２６で次の軌道計画が存在するか否かがチェックされ
る。

【０３４０】軌道計画の存否は、図３０のブロック６０
３４または図３１のブロック６０５４において、この直
交補間部に報知されてる。もしなければブロック６２２
８で前記６２２５と同様の処理を行なう。存在すれば、
ブロック６２２７でＬ−Ｌ₀をＬに、すなわち現在の動
作距離の計画値からの超過分を設定し、次の軌道計画を
とり込み、直交補間演算のためのデータを更新し、さら
に、計画値を取り込んだことを前記計画値の準備完了を
報知したブロックにアンサバックする。

【０３４１】そしてブロック６２２３の判断のいかんに
関らず、求められた動作距離Ｌを用いて、ブロック６２
２９で前記した方法により、その動作距離Ｌに相当する
位置の直交座標値及び姿勢を求め、更に、ブロック６２
３０でこれを逆変換して、各軸のエンコーダ値を求め
る。そして得られたエンコーダ値をブロック６３３１で
サーボ処理部の所定のエリアに設定し、前記した方法
で、サーボ処理部にひきわたす。

【０３４２】従って、図３３の処理がくり返し実行され
ることにより、サーボ処理部に継続動作指令が提供さ
れ、それをサーボ処理部が実行することによりロボット
アームは、所定の直交動作を行なうことになる。

【０３４３】図３３の処理は、前記カウンタ３を利用す
るモードの場合は１回起動されると同じ動作を２回常に
実行するようにしても良い。このようにすると図１３の
継続制御軸及び動作指令位置エリアは常に一杯になるよ
うデータが供給されるので、エリアの有効活用と、サー
ボ処理及び直交補間部以外の演算部の演算負荷を軽減す
ることができる。

【０３４４】図３３の直交補間部は以上のように動作す
る。しかし、まだ、サーボ制御部及び軌道計画部との接
続関係が明確にされていない。その方法を以下に示す。

【０３４５】その１つは次のようである。図３３の直交
補間部は、前記したように、サーボ処理部の情報にもと
づきブロック６２１０を判断することによって実行され
る。従って、図３２に示した破線ブロック６１２５及び
６１３３を追加することにより、通常の間欠モードの加
減速制御を実行することができる。しかし、連続モード
時においては、図３０のブロック６０２５、６０２６、
６０２８の演算処理が膨大であるため、その期間中に継
続動作指令がとぎれる。従って、それを防止するため
に、ブロック６０２５と６０２６の間、ブロック６０２
６と６０２７の間に、前記図３２の処理をそう入し、か
つ、前記したような膨大な演算を実行するブロック６０
２８の演算を、例えば方向ベクトルを求める演算、頂角
を求める演算、加減速パターンを求める演算に分解し、
その間に前記３２図の処理をそう入する。

【０３４６】このようにした場合、図３２の演算は動作
が完了している場合は何も実行しないし、図３３の直交
補間部の演算も補間完了モードにあるときは何も実行し
ないので、不必要な演算を実行してロボットアームの動
作制御に悪影響を及ぼすことはない。逆に、動作条件が
設定されているときは、軌道計画演算の合間に、継続制
御のための演算を実行し、サーボ処理部に提供するの
で、ロボットアームの動作制御が予定通り行なわれると
いう効果を奏する。

【０３４７】これは見方を変えれば、ロボットアームの
動作開始時は、動作コード解読→軌道計画→サーボ指令
→（直交補間部初期化，サーボ起動）のようにトップダ
ウンで動作し、サーボ処理部が起動されたあとは、サー
ボ処理→直交補間→軌道計画→動作コード解読の優先順
位付で演算が実行され、サーボ処理、直交補間の連携に
よりロボットアームが動作している間に、その演算の空
時間を利用して、次にロボットアームを動作させるべき
軌道計画が完成することになる。

【０３４８】その別の実施例は、次のようである。図３
３の軌道補間部を、最優先のサーボ処理部に次ぐ優先度
を持つ割込処理部として構成し、サーボ処理部が継続動
作指令を、指定された動作軸につきすべて取り込みが完
了したときに、軌道補間部を軌道する割込信号を生成す
るようにする。この信号の流れを第３４図に示す。

【０３４９】このようにすると、サーボ処理部は、定時
割込により自律的に動作する。そして直交補間部の動作
指令信号が必要になると直交補間部の割込起動信号を生
成する。しかし、サーボ処理部は、最優先の割込処理部
であるから、そのすべての演算は、正常に割込まれるこ
となく終了する。

【０３５０】サーボ処理部が割込起動信号を出力した場
合は、直交補間部は、第２位の優先度を持つ割込みであ
るから、サーボ処理部の演算が終了すると起動され、演
算を開始する。しかし、サーボ処理部が定時割込により
演算を開始すると、直交補間部は割込まれて、演算が中
断するが、サーボ処理部の演算が終了すると再び中断し
た箇所から演算を再開し、所定の演算を終える。すなわ
ち、直交補間部は、定時割込で起動されるサーボ処理部
の演算空時間に処理される。

【０３５１】さらに、動作コード解読及び軌道計画部、
サーボ指令部は、直交補間部の演算空時間内に処理され
る。このようにして、最優先処理が実行され、その指令
により、次の優先処理が実行され、最後に、最も優先度
の低い軌道計画処理が実行され、しかも先行して軌道計
画をするようになっているので、演算論理が簡単にな
り、第１の実施例のような演算オーバーヘッドのない演
算処理が可能となった。

【０３５２】なお、上記は再生運転の場合について説明
したがテスト運転の場合にも同様であり、かつ、前記し
たように、位置教示運転の場合についても動作開始のた
めの前処理部が一部相異するのみで軌道計画以降の処理
は同一であることは容易に理解されよう。すなわち、本
発明に係る演算処理は、ロボットアームの全動作制御に
つき共通であり、共通の処理手順により実現されるもの
であることも明白であろう。

【０３５３】

【発明の効果】以上述べたごとく、本発明によれば、
（i）サーボ系が安定に動作するサンプリングレートＴ
ｓ msecをロボットアームの動作軸数Ｎで除した時間Ｔs
／Ｎよりも短い定時周期Ｔsm msecで起動される割込演
算処理部を設け、（ii）起動される毎に循環的にロボッ
トアームの動作軸のうちの１軸のみのサーボ制御を実行
し、Ｎ回の起動によりロボットアームの全動作軸の１回
のサーボ制御を完了するようにし、（iii）動作指令部
の発行するロボットアームの機械座標で与えられる動作
指令位置、必要ならＹの動作指令点へ動くための動作方
法を規定する定数と動作の実行方法及びロボットアーム
の動作軸を指定するコマンドに応答してロボットアーム
の指定された動作軸のサーボ制御を開始し、（iv）前記
動作制御が完了すると、前記指令位置にロボットアーム
各軸を保持するようにすると共に、（v）座標変換を伴
なうロボットアームの駆動制御時は、サーボ制御を最優
先とし、座標変換を伴なう直交補間演算を次の優先順と
して、ロボットアームの駆動制御を実行し、その演算の
合間を利用して、先行して次の軌道計画演算を実行する
ようにしたので、（ａ）ロボットアーム及び追加軸の自
由度は、個別でも、協調してでも任意に動作させること
ができ、（ｂ）または、サーボ系に、ロボットアームの
機械座標系レベルでの補間機能を与えたので、例えば、
座標変換を伴なう直交動作の場合においても、サーボ系
にはかなり荒っぽい指令値を与えるだけで良く、主制御
処理部の演算負荷を大幅に低減することができ、ロボッ
トアームを教示点毎に停止させることなく連続して動作
させることができるようになった。

【図面の簡単な説明】

【図１】本発明の全体制御システム構成図。

【図２】本発明の適用されるロボットの一例を表す図で
ある。

【図３】本発明に係る教示手段の一例を表す図。

【図４】本発明の演算処理部の概略構成図。

【図５】本発明のサーボ制御部の全体構成図。

【図６】本発明のサーボ制御部のタイミングチャート。

【図７】本発明のサーボ処理部の概略構成図。

【図８】本発明のサーボ処理部の各処理詳細フローチャ
ート。

【図９】本発明のサーボ処理部の各処理詳細フローチャ
ート。

【図１０】本発明のサーボ処理部の各処理詳細フローチ
ャート。

【図１１】本発明のサーボ処理部の各処理詳細フローチ
ャート。

【図１２】本発明のサーボ処理部の各処理詳細フローチ
ャート。

【図１３】本発明のサーボ処理部の各処理詳細フローチ
ャート。

【図１４】本発明の軌道計画処理を説明する図。

【図１５】本発明の軌道計画処理を説明する図。

【図１６】本発明の加減速制御法を説明する図。

【図１７】本発明の加減速制御法を説明する図。

【図１８】本発明の加減速制御法を説明する図。

【図１９】本発明の加減速制御法を説明する図。

【図２０】本発明の加減速制御法を説明する図。

【図２１】本発明の加減速制御法を説明する図。

【図２２】本発明の加減速制御法を説明する図。

【図２３】本発明の加減速制御法を説明する図。

【図２４】本発明の座標変換を説明する図。

【図２５】本発明の座標変換を説明する図。

【図２６】本発明の位置教示法を説明する図。

【図２７】本発明の位置教示法を説明する図。

【図２８】本発明の位置教示法を説明する図。

【図２９】本発明の再生運転法を説明する図。

【図３０】本発明の再生運転法を説明する図。

【図３１】本発明の再生運転法を説明する図。

【図３２】本発明の再生運転法を説明する図。

【図３３】本発明の再生運転法を説明する図。

【図３４】本発明の再生運転法を説明する図。

【符号の説明】

１……制御装置、２……サーボモータ、１４……教示手
段、１５……演算手段、１６……演算手順記憶手段、１
７……動作手順記憶手段、１８……位置記憶手段

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号 庁内整理番号 ＦＩ 技術表示箇所 Ｇ０５Ｂ 19/4155 21/02 Ｚ 7531−3Ｈ (72)発明者 篠崎 弘 栃木県下都賀郡大平町大字富田800番地 株式会社日立製作所栃木工場内 (72)発明者 久富 良一 栃木県下都賀郡大平町大字富田800番地 株式会社日立製作所栃木工場内 (72)発明者 大槻 治明 茨城県土浦市神立町502番地 株式会社日 立製作所機械研究所内

Claims (3)

【特許請求の範囲】
1. 【請求項１】複数の動作軸を持つロボットアームと前記
   ロボットアームの制御装置とを備え、前記制御装置は、
   前記ロボットアームの動作指令を生成する動作指令部
   と、前記動作指令及び前記動作軸からのフィードバック
   信号を用いて動作軸のサーボ制御演算を行うサーボ制御
   部を含む演算手段と、前記サーボ制御部より生成出力さ
   れる動作軸の駆動信号に応答し動作軸を駆動するパワー
   信号発生手段とを備えたロボットにおいて、 前記サーボ制御部を、サーボ系が安定に動作するサンプ
   リングレートＴｓをロボットアームの動作軸数Ｎで除し
   た時間Ｔｓ／Ｎよりも短かい一定周期Ｔｓmで起動され
   る割込演算部とし、かつ、起動される毎に循環的にロボ
   ットアームの動作軸のうちの１軸のみのサーボ制御を実
   行し、Ｎ回の起動によってロボットアームの全動作軸の
   １回のサーボ制御を完了するようにしたことを特徴とす
   るロボット。
2. 【請求項２】特許請求の範囲第１項において、前記サー
   ボ制御部は、前記動作指令部の発行するロボットアーム
   の機械座標で与えられる動作指令位置を用い、前記動作
   指令部から与えられるコマンドによって指定されたサー
   ボ制御内容及び動作軸についてサーボ制御を開始し、指
   定された動作軸のサーボ制御が完了すると、ロボットア
   ームを前記動作指令位置に保持するようサーボ制御する
   ことを特徴とするロボット。
3. 【請求項３】特許請求の範囲第１項において、前記動作
   指令部は、ロボットアームの動作手順を解読し、それに
   もとづき軌道計画し、サーボ制御部に起動指令を生成す
   る軌道計画部と、軌道計画値に従って座標変換を伴なう
   直交補間演算を行ない、ロボットアームの機械座標系で
   表わされるサーボ系の逐次動作指令位置を生成する直交
   補間演算部とを備え、前記直交補間演算部は、サーボ制
   御部に次ぐ優先度の割込処理部であり、サーボ制御部の
   演算要求に応じて補間演算を実行し、前記軌道計画部は
   サーボ制御部及び直交補間演算部の演算の空き時間を利
   用して、実行中のロボットアーム動作制御に先行する次
   の軌道計画を実行するようにしたことを特徴とするロボ
   ット。