JP5220241B1 - 指令生成装置および指令生成方法 - Google Patents

Abstract

可動軸の運転パターンを変更することなく干渉回避動作を実現するために、指令生成装置（１００）は、第１の可動軸と第２の可動軸との間の干渉距離と、前記第１の可動軸および前記第２の可動軸の運転パターンとに基づいて、運転中に双方の可動軸間の距離が干渉距離よりも常時大きくなるように前記第１の可動軸の運転開始タイミングを遅延せしめる待機時間を算出する起動待機時間演算部（１１０）と、前記第２の可動軸の運転パターンに基づいて前記第２の可動軸を駆動する指令の出力を開始し、前記第２の可動軸にかかる指令の出力開始後から前記算出された待機時間が経過したとき、前記第１の可動軸の運転パターンに基づいて前記第１の可動軸を駆動する指令の出力を開始する、指令出力部としての起動待機部（１２０）および指令演算部（１３０）と、を備える。

Description

本発明は、実装機、工作機械、ＸＹテーブル、ロボットアームなど、複数の可動軸を備えた各種産業機械（被制御装置）を駆動するための指令を生成する指令生成装置および指令生成方法に関する。

複数の可動軸の可動領域が互いに交差している場合、これらの可動軸が互いに干渉しないように位置決め運転を行う必要がある。複数の可動軸が干渉しないように位置決め運転を行うための最も単純な技術としては、他軸を干渉しない位置まで移動させてから自軸の位置決め運転を開始する方法がある。しかしながら、この方法によれば、他軸が移動完了するまで自軸は待機する必要があるため、被制御装置のタクトタイムが長くなり、作業効率が低下するといった課題がある。

これに対し、例えば特許文献１には、他軸が干渉しない位置に退避するまでは自軸の移動速度を小さくして移動を開始しておくことで、干渉を回避しつつ待機時間を削減することを可能にする技術が開示されている。

特開平９−２５１３２０号公報

例えば実装機が駆動対象である場合には、指令生成装置は、振動抑制のため速度・移動場所・移動量に応じてあらかじめ最適化されて設定されたモータ制御ゲインを用いて、可動軸の指令を生成する。しかしながら、特許文献１に記載の技術によれば、干渉を回避するために一方の軸の移動速度が小さくなるため、あらかじめ設定されたゲインが適切ではなくなる。そのため、実装機に意図しない振動が発生する虞がある。また、干渉しないと判断できるエリアでは移動途中で再度加速するため、振動発生の要因となる。その結果、位置決めの整定時間が長くなり、被制御装置のタクトタイムが長くなるといった問題がある。

本発明は、上記に鑑みてなされたものであって、可動軸の運転パターンを変更することなく干渉回避動作を実現する指令生成装置および指令生成方法を得ることを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、被制御装置が具備する第１の可動軸と第２の可動軸とを夫々駆動する指令を生成する指令生成装置であって、前記第１の可動軸と前記第２の可動軸との間の干渉距離と、時間の関数である指令位置関数または前記指令位置関数を時間微分して得られる指令速度関数で記述される前記第１の可動軸および前記第２の可動軸の運転パターンとに基づいて、運転中に双方の可動軸間の距離が干渉距離よりも常時大きくなるように前記第１の可動軸の運転開始タイミングを遅延せしめる待機時間を算出する待機時間演算部と、前記第２の可動軸の運転パターンに基づいて前記第２の可動軸を駆動する指令の出力を開始し、前記第２の可動軸にかかる指令の出力開始後から前記算出された待機時間が経過したとき、前記第１の可動軸の運転パターンに基づいて前記第１の可動軸を駆動する指令の出力を開始する、指令出力部と、を備え、前記待機時間演算部は、前記第２の可動軸の運転パターンに基づく指令位置が前記第１の可動軸の目標位置と前記干渉距離との合算値と一致する時刻Ｔｂを演算し、前記第１の可動軸の運転パターンに基づく指令速度を含む第１の関数が前記第２の可動軸の運転パターンに基づく指令速度を含む第２の関数の時刻Ｔｂにおける値に一致する時刻Ｔａを演算し、時刻Ｔｂから時刻Ｔａを減算し、当該減算により得られた値を前記待機時間とする、ことを特徴とする。

本発明にかかる指令生成装置は、自軸（第１の可動軸）の運転パターンに基づく指令の出力を遅延せしめることで自軸と他軸（第２の可動軸）との干渉を回避することができるので、可動軸の運転パターンを変更することなく干渉回避動作を実現することができる。

図１−１は、本発明の実施の形態１の指令生成装置による位置決め動作の特徴を説明する図である。 図１−２は、本発明の実施の形態１の指令生成装置による位置決め動作の特徴を説明する図である。 図２は、本発明の実施の形態１にかかる指令生成装置の機能構成を示すブロック図である。 図３−１は、実施の形態１において使用される安全距離を説明する図である。 図３−２は、実施の形態１において使用される安全距離を説明する図である。 図４は、起動待機時間Ｔｗの算出方法の概要と、自軸と他軸との間の干渉を回避できることの原理を説明する図である。 図５は、実施の形態１の指令生成装置のハードウェア構成例を説明する図である。 図６は、実施の形態１の指令生成方法を説明するフローチャートである。 図７は、実施の形態１の待機時間算出処理を説明するフローチャートである。 図８−１は、実施の形態２において使用される安全距離を説明する図である。 図８−２は、実施の形態２において使用される安全距離を説明する図である。 図９は、本発明の実施の形態２にかかる指令生成装置の機能構成を示すブロック図である。 図１０は、実施の形態２の待機時間算出処理を説明するフローチャートである。 図１１は、実施の形態２の指令生成装置により干渉が回避される様子を説明する図である。

以下に、本発明にかかる指令生成装置および指令生成方法の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。

実施の形態１．
図１−１および図１−２は、本発明の実施の形態１の指令生成装置による位置決め動作の特徴を説明する図である。なお、ここでは、説明を簡単にするため、指令生成装置の制御対象とする機械（被制御装置）が有する可動軸（以下、単に軸と記述）は２つ（自軸および他軸）であるとする。

図１−１は、自軸（第１の可動軸）および他軸（第２の可動軸）が互いに近づく方向に移動する場合の動作例を示している。実施の形態１の指令生成装置によれば、自軸および他軸が互いに近づく方向に移動する場合であって、双方の目標位置の間が安全距離以上離れている場合には、干渉回避の制御を実行しない。安全距離とは、予め設定される自軸と他軸との間の距離であって、干渉を回避するための自軸と他軸との間の距離をいう。

図１−２は、他軸が自軸に対して離れる方向に移動する場合の動作例を示している。他軸が自軸から離れる方向に移動中においては、双方の可動軸の現在位置の間、および双方の可動軸の目標位置の間が安全距離以上の距離だけ離れていたとしても、移動の途中で互いに干渉する可能性がある。実施の形態１の指令生成装置によれば、他軸が自軸に対して離れる方向に移動する場合には、干渉回避の制御を実行する。即ち、実施の形態１の指令生成装置は、停止中または動作中の他軸に自軸が干渉することを回避するための待機時間を算出し、待機時間が経過した後に自軸の目標位置までの位置決めを開始する。

図２は、本発明の実施の形態１にかかる指令生成装置の機能構成を示すブロック図である。図示するように、指令生成装置１００は、起動待機時間演算部１１０、起動待機部１２０、指令演算部１３０、および位置更新部１４０を備えている。指令生成装置１００は、目標位置（自軸目標位置Ｘ１および他軸目標位置Ｘ２）、目標速度（自軸目標速度Ｖ１および他軸目標速度Ｖ２）、および安全距離Ｄｓが入力される。そして、指令生成装置１００は、各軸に対する指令速度（出力指令速度ｖｏ１、ｖｏ２）をサーボアンプなどのモータ駆動制御部１５０に出力する。なお、目標位置（自軸目標位置Ｘ１および他軸目標位置Ｘ２）および目標速度（自軸目標速度Ｖ１および他軸目標速度Ｖ２）は、ＮＣプログラムなどから与えられる。

なお、実施の形態１においては、安全距離とは、ユーザの停止指令・エラー発生などの要因により他軸が停止を開始しても自軸の停止が間に合う（干渉を回避できる）だけの減速時の移動量（以下、減速移動量）を含むものとする。減速移動量とは、言い換えると、緊急停止開始から停止完了するまでの可動軸の移動量である。図３−１および図３−２は、実施の形態１において使用される安全距離を説明する図である。図示するように、自軸および他軸が互いに近づく方向に移動する場合であっても、他軸が自軸に対して離れる方向に移動する場合であっても、安全距離は、軸間のオフセットに、自軸および他軸の減速移動量の絶対値を夫々加算したものとなっている。軸間のオフセットとは、軸同士を物理的に近づけることができる最小の距離をいう。減速移動量は次式１で定義される。ここで、安全距離には減速移動量の最大値を考慮する必要があるため、指令速度は運転パターン中の最大値が採用される。
(減速移動量)=(１／２)×(指令速度)^２／(減速度) （式１）

起動待機時間演算部１１０は、目標位置Ｘ１、Ｘ２と、目標速度Ｖ１、Ｖ２と、指令生成装置１００内部の位置更新部１４０で演算される各軸の現在の指令位置ｘｏ１、ｘｏ２と、安全距離と、を入力とし、自軸および他軸の運転パターンとに基づいて、運転中に双方の可動軸間の距離が安全距離よりも常時大きくなるように自軸の運転開始タイミングを他軸の運転開始タイミングから遅延せしめる遅延量である起動待機時間Ｔｗを演算する。

自軸の運転パターンは、時刻ｔを変数とした指令位置関数ｐ１［ｔ］または／および指令位置関数ｐ１［ｔ］を時間微分して得られる指令速度関数ｗ１［ｔ］で記述される。同様に、他軸の運転パターンは、指令位置関数ｐ２［ｔ］または／および指令位置関数ｐ２［ｔ］を時間微分して得られる指令速度関数ｗ２［ｔ］で記述される。起動待機時間演算部１１０は、運転パターンが指令速度関数ｗ１［ｔ］、ｗ２［ｔ］で記述される場合には、指令速度関数ｗ１［ｔ］、ｗ２［ｔ］を夫々積分して指令位置関数ｐ１［ｔ］、ｐ２［ｔ］を得ることができるし、運転パターンが指令位置関数ｐ１［ｔ］、ｐ２［ｔ］で与えられる場合には、指令位置関数ｐ１［ｔ］、ｐ２［ｔ］を夫々微分して指令速度関数ｗ１［ｔ］、ｗ２［ｔ］を得ることができる。夫々の可動軸の運転パターンは、例えば、時間ｔ以外の変数を含む関数（例えば台形加減速を実現する関数やＳ字加減速を実現する関数）で表現される。そして、時間ｔ以外の変数は、目標位置Ｘ１、Ｘ２、目標速度Ｖ１、Ｖ２、および現在の指令位置ｘｏ１、ｘｏ２に基づいて決定され、各軸の運転パターンを記述する関数が確定される。起動待機時間演算部１１０は、自軸目標位置Ｘ１および指令位置関数ｐ２［ｔ］に基づいて自軸と他軸の距離が安全距離未満になる時間Ｔｂを演算する。そして、指令速度関数ｗ１［ｔ］およびｗ２［ｔ］に基づいて時間Ｔｂにおける他軸の指令速度ｗ２［Ｔｂ］と自軸の指令速度が一致する時間Ｔａを演算する。そして、時間Ｔｂと時間Ｔａから式「Ｔ＝Ｔｂ−Ｔａ」により起動待機時間Ｔｗを演算する。

起動待機部１２０および指令演算部１３０は、協働して、実施の形態１の指令出力部として機能する。指令出力部は、他軸の運転パターンに基づいて他軸を駆動する指令の出力を開始し、他軸を駆動する指令の出力を開始してから起動待機時間Ｔｗが経過したとき、自軸の運転パターンに基づいて自軸を駆動する指令の出力を開始する。

具体的には、起動待機部１２０は、起動待機時間演算部１１０で演算される自軸に対する起動待機時間Ｔｗだけ起動指令の出力を遅らせる。即ち、起動待機部１２０は、他軸の起動指令を出力し、当該他軸の起動指令の出力後、起動待機時間Ｔｗだけ経過すると、自軸の起動指令を出力する。なお、算出された起動待機時間Ｔｗがゼロ値以下の値である場合、他軸の起動指令を出力すると、即、自軸の起動指令を出力する。指令演算部１３０は、起動指令、目標位置、目標速度を入力とし、各軸の起動指令入力後は該当軸の目標位置、目標速度から、制御周期ごとに各軸指令速度を演算する。算出された自軸の指令速度をｖｏ１、他軸の指令速度をｖｏ２とする。ここで、指令演算部１３０は、他軸の起動指令を受信すると、他軸の演算周期毎の指令速度ｖｏ２の出力を開始し、自軸の起動指令を受信すると、自軸の演算周期毎の指令速度ｖｏ１の出力を開始する。

位置更新部１４０は、各軸に出力される指令速度ｖｏ１、ｖｏ２を夫々積分して、各軸の現在指令位置ｘｏ１、ｘｏ２を算出する。

なお、モータ駆動制御部１５０は、例えばサーボアンプであって、指令速度ｖｏ１、ｖｏ２を入力とし、モータの駆動の制御を行う。

ここで、起動待機時間Ｔｗの算出方法の概要と、自軸の位置決め開始を起動待機時間Ｔｗだけ遅らせることで、自軸と他軸との間の干渉を回避できることの原理を、図４を使用して説明する。なお、ここでは、説明を単純化するため安全距離がゼロ値であるとして説明する。

まず、起動待機時間演算部１１０は、自軸の目標位置Ｘ１と指令位置関数ｐ２［ｔ］の交点の時間Ｔｂを求めることで、指令位置関数ｐ２［ｔ］において、干渉の考慮が必要な時間区分を算出する。即ち、時刻Ｔｂ以降の時間は、自軸と他軸との間が安全距離以上に離れるため、干渉は発生しない時間区分であり、時刻０〜時刻Ｔｂは、干渉の考慮が必要な時間区分である。次に、起動待機時間演算部１１０は、自軸の指令速度が時刻Ｔｂでの他軸の指令速度（指令位置の微分）Ｖに一致する時刻Ｔａを求める。そして、起動待機時間演算部１１０は、Ｔｂ−Ｔａの値を起動待機時間Ｔｗとする。Ｔｗ＝Ｔｂ−Ｔａだけ自軸の指令位置関数が遅延せしめられると（ｐ１［ｔ−Ｔｗ］）、時刻Ｔｂでの自軸の速度はＶと等しくなる。自軸の指令位置関数は単調増加関数であるため、時刻Ｔｂでの自軸の指令位置（ｐ１［Ｔｂ−Ｔｗ］＝ｐ１［Ｔａ］）は明らかに自軸の目標位置Ｘ１以下である。さらに時刻Ｔｂでの自軸と他軸の指令位置関数の傾き（指令速度）はＶと等しいため、時刻Ｔｂで自軸と他軸の指令位置関数は接するかまたは離れている（安全距離０であるため、接する場合も干渉無しである）。よって、２つの指令位置関数は交点を持たないことから、干渉を回避するための起動待機時間が算出できる。

図５は、実施の形態１の指令生成装置１００のハードウェア構成例を説明する図である。指令生成装置１００は、ＣＰＵ（Central Processing Unit）１、ＲＡＭ（Random Access Memory）２、およびＲＯＭ（Read Only Memory）３を備えている。ＣＰＵ１、ＲＡＭ２およびＲＯＭ３は、バスラインを介して夫々接続されている。

ＲＯＭ３は、実施の形態１の指令生成方法を実行する指令生成プログラム４を予め記憶している記録媒体である。ＣＰＵ１は、ＲＯＭ３から指令生成プログラム４をバスラインを介して読み出して、ＲＡＭ２へロードし、ＲＡＭ２内にロードされた指令生成プログラム４を実行する。指令生成プログラム４がＲＡＭ２にロードされることによって、ＲＡＭ２上に、起動待機時間演算部１１０、起動待機部１２０、指令演算部１３０、および位置更新部１４０の夫々に対応するプログラムモジュールが生成される。ＣＰＵ１は、夫々のプログラムモジュールを実行することによって、対応する機能構成部として機能する。なお、指令生成プログラム４をインターネット等のネットワーク経由で提供または配布するように構成してもよい。また、指令生成プログラム４のロード元となる記録媒体として、外部記憶装置、着脱可能なメモリデバイス、光ディスク装置を採用することが可能である。

なお、起動待機時間演算部１１０、起動待機部１２０、指令演算部１３０、および位置更新部１４０は、ＣＰＵ１、即ちソフトウェアにより実現されるものとして説明するが、これらのうちの一部もしくは全部は、ハードウェアおよびソフトウェアのいずれか、または両者の組み合わせとして実現するようにしてもよい。

図６は、指令生成装置１００によって実行される、実施の形態１の指令生成方法を説明するフローチャートである。図示するように、まず、起動待機時間演算部１１０は、外部から入力される目標位置Ｘ１、Ｘ２および目標速度Ｖ１、Ｖ２と、位置更新部１４０で演算される各軸の現在の指令位置ｘｏ１、ｘｏ２とを取得する（ステップＳ１）。そして、起動待機時間演算部１１０は、自軸および他軸の目標位置の差を計算し、目標位置が安全距離Ｄｓ以内であるか否かを判定する（ステップＳ２）。自軸および他軸の目標位置の差が安全距離Ｄｓ未満である場合（ステップＳ２、Ｙｅｓ）、干渉を回避することなく自軸の移動を実行せしめることができないので、指令生成装置１００は、自軸の運転開始をキャンセルし（ステップＳ３）、指令生成装置１００は動作を終了する。

なお、指令生成装置１００が自軸の運転開始をキャンセルするとは、例えば、指令生成装置１００が、自軸を駆動する指令の出力を禁止し、指令生成装置１００が自軸を駆動する指令の出力を禁止している場合には、指令演算部１３０が指令速度ｘｏ１を出力しないようにすることで実現される。自軸および他軸の目標位置の差が安全距離Ｄｓ以上である場合には、指令生成装置１００は、自軸を駆動する指令の出力を許可し、自軸を駆動する指令の出力が許可されている場合には、指令演算部１３０は、起動待機部１２０からの起動指令を受信したときに自軸を駆動する指令を出力することができる。

なお、指令生成装置１００は、自軸の運転開始をキャンセルした場合に、オペレータにエラーを通知するなどの所定のエラー処理を実行するようにしてもよい。また、起動待機時間演算部１１０は、ステップＳ２の処理に併せて、他軸と自軸とが交差するか否かを判定するようにし、他軸と自軸とが交差する場合にもステップＳ３の処理が実行されるようにしてもよい。

自軸および他軸の目標位置の差が安全距離Ｄｓ以上である場合（ステップＳ２、Ｎｏ）、指令生成装置１００は、他軸の位置決めを開始する（ステップＳ４）。具体的には、起動待機時間演算部１１０は、起動待機部１２０に指令して他軸を起動する起動指令を指令演算部１３０に出力せしめ、前記他軸を起動する起動指令を受信した指令演算部１３０は、目標位置Ｘ２および目標速度Ｖ２に基づいて制御周期毎の他軸の指令速度ｖｏ２を算出し、出力する。

続いて、起動待機時間演算部１１０は、他軸が停止中であるか否かの判定（ステップＳ５）と、他軸が自軸から離れる方向に移動中であるか否かの判定（ステップＳ６）とを実行する。起動待機時間演算部１１０は、ステップＳ５およびステップＳ６の判定を、位置更新部１４０が算出する現在の指令位置ｘｏ２と目標位置ｘｏ２との差分に基づいて判定することができる。他軸が停止している場合（ステップＳ５、Ｙｅｓ）、または他軸が自軸に近づく方向に移動中である場合（ステップＳ６、Ｎｏ）、起動待機時間演算部１１０は、起動待機時間Ｔｗをゼロ値とする（ステップＳ７）。

他軸が移動中であり（ステップＳ５、Ｎｏ）、かつ、他軸の移動方向が自軸から離れる方向である場合（ステップＳ６、Ｙｅｓ）、起動待機時間演算部１１０は、干渉を回避するための自軸の起動待機時間Ｔｗを算出する待機時間算出処理を実行する（ステップＳ８）。

図７は、実施の形態１の待機時間算出処理を説明するフローチャートである。なお、ここでは、便宜上、自軸および他軸の動作方向がともにプラス方向である場合について説明するが、自軸および他軸の動作方向がともにマイナス方向である場合であっても同様の動作が可能である。まず、起動待機時間演算部１１０は、ｐ２［ｔ］＝Ｘ１＋Ｄｓとなる時間Ｔｂを算出する（ステップＳ２１）。なお、起動待機時間演算部１１０は、時刻Ｔｂを算出するためには、例えば、一定時間間隔ごとにｔを増減させてｐ２［ｔ］＝Ｘ１＋Ｄｓとなる時間Ｔｂを探索することができる。ただし、繰り返し判断を行うため、計算コスト（処理時間）が問題になる場合、起動待機時間演算部１１０は、反復法（挟み撃ち法・二分法など）に代表される数値解析に基づいて時刻Ｔｂを演算するようにしてもよい。続いて、起動待機時間演算部１１０は、ｗ１［ｔ］＝ｗ２［Ｔｂ］となる時間Ｔａを算出する（ステップＳ２２）。なお、ｗ１［ｔ］の減速区間にｗ２［ｔ］が含まれる場合、起動待機時間演算部１１０は、ｗ１［ｔ］の逆関数に基づいて時刻Ｔａを演算するようにしてもよい。また、ｗ１［ｔ］＜ｗ２［Ｔｂ］の場合（Ｔａが求まらない場合）、起動待機時間演算部１１０は、減速開始時の時刻をＴａとするようにしてもよい。続いて、起動待機時間演算部１１０は、起動待機時間Ｔｗ＝Ｔｂ−Ｔａを算出し（ステップＳ２３）、待機時間算出処理を終了する。

ステップＳ７またはステップＳ８にて算出された起動待機時間Ｔｗは、起動待機部１２０に入力される。ステップＳ７またはステップＳ８の処理の後、起動待機部１２０は、起動待機時間Ｔｗだけ待機する（ステップＳ９）。即ち、起動待機部１２０は、他軸の起動指令を出力してから起動待機時間Ｔｗが経過するまで待機する。起動待機時間Ｔｗが経過した後、指令生成装置１００は、自軸の位置決めを開始する（ステップＳ１０）。具体的には、起動待機部１２０は、自軸の起動指令を指令演算部１３０に入力し、自軸の起動指令が入力された指令演算部１３０は、目標速度Ｖ１および目標速度Ｖ１に基づいて速度指令ｖｏ１を算出し、出力する。自軸および他軸が位置決めによりともに目標位置に到達すると、指令生成装置１００は、動作を終了する。

なお、以上の説明においては、他軸が１つしかない場合について説明したが、他軸が複数存在する場合には、指令生成装置１００は、ステップＳ２〜ステップＳ８の処理を他軸毎に実行し、ステップＳ３の処理を実行しない場合において、他軸毎の起動待機時間Ｔｗの最大値を用いてステップＳ９の処理を実行することで、自軸と他軸との干渉を回避することができる。

このように、実施の形態１によれば、自軸（第１の可動軸）と他軸（第２の可動軸）との間の安全距離（干渉距離）と、自軸および他軸の運転パターンとに基づいて、運転中に双方の可動軸間の距離が安全距離よりも常時大きくなるように自軸の運転開始タイミングを遅延せしめる待機時間を算出する起動待機時間演算部１１０と、他軸の運転パターンに基づいて他軸を駆動する指令の出力を開始し、他軸にかかる指令の出力開始後から前記算出された待機時間が経過したとき、自軸の運転パターンに基づいて自軸を駆動する指令の出力を開始する、指令出力部として機能する、起動待機部１２０および指令演算部１３０と、を備えるように指令生成装置１００を構成したので、指令生成装置１００は、自軸の運転パターンに基づく指令の出力を遅延せしめることで自軸と他軸との干渉を回避することができるので、可動軸の運転パターンを変更することなく干渉回避動作を実現することができる。

また、起動待機時間演算部１１０は、自軸および他軸の目標位置および現在指令位置に基づいて、他軸が自軸に近づく方向に移動しているか、他軸が自軸から離れる方向に移動しているか、または他軸が停止しているかを判定し、他軸が自軸に近づく方向に移動している場合と、他軸が停止している場合には、待機時間をゼロ値とする、ようにしたので、双方の可動軸の目標位置のみから干渉が回避不可能であると判断できる場合には起動待機時間演算部１１０は待機時間の演算を省略することができるので、指令生成装置１００の計算コストを低減することができるようになる。

また、起動待機時間演算部１１０は、指令位置ｐ２［ｔ］が自軸の目標位置と安全距離との合算値と一致する時刻Ｔｂを演算し、自軸の指令速度ｗ１［ｔ］が他軸の指令速度ｗ２［Ｔｂ］と一致する時刻Ｔａを演算し、時刻Ｔｂから時刻Ｔａを減算し、当該減算により得られた値を前記待機時間とする、ように構成したので、安全距離が双方の可動軸間の静止状態でのオフセット距離と双方の可動軸の緊急停止開始から停止完了するまでの移動量とを合算して得られる値である場合に、可動軸の運転パターンを変更することなく干渉回避動作を実現することができる。

また、起動待機時間演算部１１０は、双方の可動軸の目標位置間の距離が安全距離よりも大きいか否かを判定し、双方の可動軸の目標位置間の距離が安全距離よりも大きい場合、自軸を駆動する指令の出力を許可し、双方の可動軸の目標位置間の距離が安全距離よりも小さい場合、自軸を駆動する指令の出力を禁止する、安全確認部として機能する、ようにしたので、双方の可動軸の目標位置のみから干渉が回避不可能であると判断できる場合には、待機時間を算出することなく自軸の運転をキャンセルすることができる。

また、起動待機時間演算部１１０は、双方の可動軸が交差するか否かを判定し、双方の可動軸が交差しない場合、自軸を駆動する指令の出力を許可し、双方の可動軸が交差する場合には、自軸を駆動する指令の出力を禁止する、ように構成したので、双方の可動軸間の距離が安全距離よりも離れていたとしても双方の可動軸が交差する場合には自軸の運転をキャンセルすることができる。

実施の形態２．
実施の形態２の指令生成装置は、安全距離として、減速移動量を含まない値を入力とすることができる。図８−１および図８−２は、実施の形態２において使用される安全距離を説明する図である。図示するように、自軸および他軸が互いに近づく方向に移動する場合であっても、他軸が自軸に対して離れる方向に移動する場合であっても、軸間のオフセットが安全距離として使用される。

図９は、本発明の実施の形態２にかかる指令生成装置の機能構成を示すブロック図である。ここでは、実施の形態１の構成要素と同様の構成要素には、実施の形態１と同じ名称および符号を付して、重複する説明を省略する。

図示するように、指令生成装置２００は、起動待機時間演算部２１０、起動待機部１２０、指令演算部１３０、および位置更新部１４０を備えている。

起動待機時間演算部２１０は、目標位置Ｘ１、Ｘ２と、目標速度Ｖ１、Ｖ２と、安全距離と、指令生成装置１００内部の位置更新部１４０で演算される各軸の現在の指令位置ｘｏ１、ｘｏ２と、に基づいて、実施の形態２の待機時間算出処理を実行する。

実施の形態２の待機時間算出においては、自軸および他軸について、指令速度ｗ［ｔ］から減速度ｄｃｃに従って減速停止させた場合の減速移動量を加算した指令位置関数ｐｄ［ｔ］と、指令位置関数ｐｄ［ｔ］の微分から得られる、指令速度関数ｗｄ［ｔ］が使用される。即ち、指令位置関数ｐｄ［ｔ］、指令速度関数ｗｄ［ｔ］は、夫々、以下の式２および式３により記述される。
ｐｄ［ｔ］＝ｐ［ｔ］＋ｗ［ｔ］ ^２ ／（２×ｄｃｃ） （式２）
ｗｄ［ｔ］＝ｗ［ｔ］×（１＋ｗ’［ｔ］／ｄｃｃ） （式３）
なお、ｗ’［ｔ］は、ｗ［ｔ］を微分したものを示す。

図１０は、実施の形態２の待機時間算出処理を説明するフローチャートである。図示するように、起動待機時間演算部２１０は、まず、ｐ２［ｔ］＝Ｘ1＋Ｄｓとなる時間Ｔｂを算出する（ステップＳ３１）。そして、起動待機時間演算部２１０は、ｗｄ１［ｔ］＝ｗｄ２［Ｔｂ］となる時間Ｔａを算出する（ステップＳ３２）。そして、起動待機時間演算部２１０は、起動待機時間Ｔｗ＝Ｔｂ−Ｔａを算出し（ステップＳ３３）、待機時間算出処理を終了する。

図１１は、実施の形態２の指令生成装置２００により干渉が回避される様子を説明する図である。図示するように、指令生成装置２００によれば、減速移動量を含まない安全距離が入力されても、他軸の指令位置関数と自軸の指令位置関数とが交差しないように、減速移動量を考慮して自軸の指令位置関数が遅延せしめられるので、干渉が回避される。

このように、実施の形態２によれば、起動待機時間演算部２１０は、指令位置ｐ１［ｔ］に自軸の減速移動量を加算して得られる関数ｐｄ１［ｔ］と、関数ｐｄ１［ｔ］を時間微分して得られる関数ｗｄ１［ｔ］と、指令位置ｐ２［ｔ］に他軸の減速移動量を加算して得られる関数ｐｄ２［ｔ］と、関数ｐｄ２［ｔ］を時間微分して得られる関数ｗｄ２［ｔ］と、を求め、関数ｐｄ２［ｔ］の値が自軸の目標位置と安全距離との合算値と一致する時刻Ｔｂを演算し、関数ｗｄ１［ｔ］の値が関数ｗｄ２［ｔ］にｔ＝Ｔｂを代入した値と一致する時刻Ｔａを演算し、時刻Ｔｂから時刻Ｔａを減算し、当該減算により得られた値を前記待機時間とする、ように構成したので、ユーザは、双方の軸の減速移動量を考慮することなく、双方の可動軸間の静止状態でのオフセット距離を安全距離として入力することが可能となるので、安全距離の設定値の検討を簡略化することが可能となる。

以上のように、本発明にかかる指令生成装置および指令生成方法は、複数の可動軸を備えた各種産業機械を駆動するための指令を生成する指令生成装置および指令生成方法に適用して好適である。

１ ＣＰＵ
２ ＲＡＭ
３ ＲＯＭ
４ 指令生成プログラム
１００ 指令生成装置
１１０ 起動待機時間演算部
１２０ 起動待機部
１３０ 指令演算部
１４０ 位置更新部
１５０ モータ駆動制御部
２００ 指令生成装置
２１０ 起動待機時間演算部

Claims (9)

1. 被制御装置が具備する第１の可動軸と第２の可動軸とを夫々駆動する指令を生成する指令生成装置であって、
   前記第１の可動軸と前記第２の可動軸との間の干渉距離と、時間の関数である指令位置関数または前記指令位置関数を時間微分して得られる指令速度関数で記述される前記第１の可動軸および前記第２の可動軸の運転パターンとに基づいて、運転中に双方の可動軸間の距離が干渉距離よりも常時大きくなるように前記第１の可動軸の運転開始タイミングを遅延せしめる待機時間を算出する待機時間演算部と、
   前記第２の可動軸の運転パターンに基づいて前記第２の可動軸を駆動する指令の出力を開始し、前記第２の可動軸にかかる指令の出力開始後から前記算出された待機時間が経過したとき、前記第１の可動軸の運転パターンに基づいて前記第１の可動軸を駆動する指令の出力を開始する、指令出力部と、
   を備え、
   前記待機時間演算部は、前記第２の可動軸の運転パターンに基づく指令位置が前記第１の可動軸の目標位置と前記干渉距離との合算値と一致する時刻Ｔｂを演算し、前記第１の可動軸の運転パターンに基づく指令速度を含む第１の関数が前記第２の可動軸の運転パターンに基づく指令速度を含む第２の関数の時刻Ｔｂにおける値に一致する時刻Ｔａを演算し、時刻Ｔｂから時刻Ｔａを減算し、当該減算により得られた値を前記待機時間とする、
   ことを特徴とする指令生成装置。
2. 前記干渉距離は、双方の可動軸間の静止状態でのオフセット距離と双方の可動軸の緊急停止開始から停止完了するまでの移動量である減速移動量との合算値である、外部から与えられる値であって、
   前記第１の関数は前記第１の可動軸の運転パターンを記述する指令速度関数であって、
   前記第２の関数は前記第２の可動軸の運転パターンを記述する指令速度関数である、
   ことを特徴とする請求項１に記載の指令生成装置。
3. 前記双方の可動軸の目標位置間の距離が前記干渉距離よりも大きいか否かを判定し、前記双方の可動軸の目標位置間の距離が前記干渉距離よりも大きい場合、前記第１の可動軸を駆動する指令の出力を許可し、前記双方の可動軸の目標位置間の距離が前記干渉距離よりも小さい場合、前記第１の可動軸を駆動する指令の出力を禁止する、安全確認部をさらに備える、ことを特徴とする請求項２に記載の指令生成装置。
4. 前記安全確認部は、双方の可動軸が交差するか否かを判定し、双方の可動軸が交差しない場合、前記第１の可動軸を駆動する指令の出力を許可し、双方の可動軸が交差する場合には、前記第１の可動軸を駆動する指令の出力を禁止する、
   ことを特徴とする請求項３に記載の指令生成装置。
5. 前記干渉距離は、外部から与えられる双方の可動軸間の静止状態でのオフセット距離であって、
   前記第１の関数は前記第１の可動軸の運転パターンに基づく指令位置と前記第１の可動軸が緊急停止開始してから停止完了するまでの移動量との合算値を時間微分して得られる関数であって、
   前記第２の関数は前記第２の可動軸の運転パターンに基づく指令位置と前記第２の可動軸が緊急停止開始してから停止完了するまでの移動量との合算値を時間微分して得られる関数である、
   ことを特徴とする請求項１に記載の指令生成装置。
6. 前記双方の可動軸の目標位置間の距離が前記オフセット距離よりも大きいか否かを判定し、前記双方の可動軸の目標位置間の距離が前記オフセット距離よりも大きい場合、前記第１の可動軸を駆動する指令の出力を許可し、前記双方の可動軸の目標位置間の距離が前記オフセット距離よりも小さい場合、前記第１の可動軸を駆動する指令の出力を禁止する、安全確認部をさらに備える、ことを特徴とする請求項５に記載の指令生成装置。
7. 前記安全確認部は、双方の可動軸が交差するか否かを判定し、双方の可動軸が交差しない場合、前記第１の可動軸を駆動する指令の出力を許可し、双方の可動軸が交差する場合には、前記第１の可動軸を駆動する指令の出力を禁止する、
   ことを特徴とする請求項６に記載の指令生成装置。
8. 前記待機時間演算部は、双方の可動軸の目標位置および現在指令位置に基づいて、前記第２の可動軸が前記第１の可動軸に近づく方向に移動しているか、前記第１の可動軸から離れる方向に移動しているか、または停止しているかを判定し、前記第２の可動軸が前記第１の可動軸に近づく方向に移動している場合と、前記第２の可動軸が停止している場合には、前記待機時間をゼロ値とする、
   ことを特徴とする請求項１乃至請求項７の何れか一項に記載の指令生成装置。
9. 被制御装置が具備する第１の可動軸と第２の可動軸とを夫々駆動する指令を生成する指令生成方法であって、
   前記第１の可動軸と前記第２の可動軸との間の干渉距離と、時間の関数である指令位置関数または前記指令位置関数を時間微分して得られる指令速度関数で記述される前記第１の可動軸および前記第２の可動軸の運転パターンとに基づいて、運転中に双方の可動軸間の距離が干渉距離よりも常時大きくなるように前記第１の可動軸の運転開始タイミングを遅延せしめる待機時間を算出する待機時間演算ステップと、
   前記第２の可動軸の運転パターンに基づいて前記第２の可動軸を駆動する指令の出力を開始する第１開始ステップと、
   前記第１開始ステップの実行後、前記算出された待機時間が経過したとき、前記第１の可動軸の運転パターンに基づいて前記第１の可動軸を駆動する指令の出力を開始する第２開始ステップと、
   を備え、
   前記待機時間演算ステップは、
   前記第２の可動軸の運転パターンに基づく指令位置が前記第１の可動軸の目標位置と前記干渉距離との合算値と一致する時刻Ｔｂを演算するステップと、
   前記第１の可動軸の運転パターンに基づく指令速度を含む第１の関数が前記第２の可動軸の運転パターンに基づく指令速度を含む第２の関数の時刻Ｔｂにおける値に一致する時刻Ｔａを演算するステップと、
   時刻Ｔｂから時刻Ｔａを減算し、当該減算により得られた値を前記待機時間とするステップと、
   を含む、
   ことを特徴とする指令生成方法。

Images