JP5889104B2 - 位置決め制御用指令生成装置 - Google Patents

Description

本発明は、サーボモータなどのモータを用いて機械負荷を位置決め制御するに際し、位置決め制御時の目標参照信号となる指令値を生成する位置決め制御用指令生成装置に関するものである。

位置決め制御を実現する典型的な例としてエンコーダの付いたサーボモータと、エンコーダの情報を用いてサーボモータを制御するサーボアンプと、サーボアンプに位置決め制御用の指令値を与えるモーションコントローラに代表される指令値生成装置を用いて実現する例が挙げられる。エンコーダは整数値の分解能（例えば、モータ１回転につき１０００００パルスを出力するなど）を有し、モーションコントローラは所望の移動量に応じて、所定周期毎に整数の値をとる指令値をサーボアンプに与え、サーボアンプはエンコーダの位置が指令値に追従するようにフィードバック制御等を行うことにより、サーボモータに供給する電流を計算し、電流をサーボモータに与えるという処理を行う。

モーションコントローラは、位置決め制御時に、指令値（位置指令値の差分信号である速度や、速度の差分信号である加速度）を所望の形状で、所望の移動量に対して１パルスのずれのない指令値を生成する必要がある。また、位置決め制御時の機械振動を励起しないためには、指令値の形状として、単純な三角や台形だけではなく、加速時間や減速時間、加々速度時間をきめ細かく設定することが求められている。

このような位置決め制御用の指令値を生成する技術として、例えば、以下に示す特許文献１〜３が公開されている。特許文献１は、減速開始位置を予め入力しておき、加速度増加モード、一定加速度モード、加速度減少モード、一定速モードなどのモード毎の指令値を計算する式を予め用意しておき、位置が減速開始位置になったら減速モードに対応した計算式にする技術が公開されている。特許文献２は、移動平均フィルタを用いてＳ字加減速を実現することが公開されている。特許文献３では、減速に必要な移動距離を、加速度一定で減速停止させる場合の移動距離（速度＾２／加速度／２）と、加々速度時間の情報をもとに計算される補正係数の積で算出し、算出した減速に必要な移動量に基づいて減速停止を開始するタイミングを決定し、減速停止のための位置指令を計算することが公開されている。

特開平２−１２１００５号公報 特開２００８−１７１２３０号公報 国際公開第２０１０／１２５９５８号

しかしながら、上記従来の技術によれば、たとえば特許文献１の技術においては、減速開始位置を予め入力しておかなければならないという手間が生じるという問題がある。さらに、減速開始位置を不適切に設定すると、減速動作時の指令値の形状が所望の形状通りにならないという問題があった。

さらに、特許文献２の技術においては、移動平均処理を行うが移動平均処理はバッファとなるメモリを移動平均時間に比例して用意する必要があり、指令生成装置を構成するにあたりメモリを非常に多く用意しなければならないという問題があった。

また、特許文献３においては、加速度一定で減速停止させるのに必要な移動距離は、速度＾２／加速度／２であることが公開されている。指令値をモーションコントローラに搭載されたＣＰＵを用いて生成することを考えると、ある制御周期ごと（例えば１ｍｓごと）に指令値を生成することが通常行われるが、例えば、制御周期が１ｍｓ、減速開始の速度が１００パルス／ｍｓ、減速時の加速度が１０パルス／ｍｓ＾２でかつ、一定加速度で減速する場合、速度＾２／加速度／２を適用して減速に必要な移動量を算出すると５００パルスとなる。しかし、実際の減速動作時の速度を計算してみると、１００，９０，８０，・・・，２０，１０と速度１００パルス／ｍｓから１ｍｓごとに減速時の加速度に相当する値１０を減算していくことにより、減速時の速度が算出され、さらに、これらを全部合計して減速に必要な移動量を計算すると、１００＋９０＋８０＋・・・＋２０＋１０＝５５０パルスとなる。これは、上記の計算式に対して誤差が生じていることを意味している。加速度一定で減速停止した場合、速度は直線的に減少するが、このとき減速時間は速度／加速度となり、減速に必要な移動量は速度×減速時間÷２＝速度＾２／加速度／２となる関係を利用している。この計算式は連続的な時間に対しては正確な計算式となるが、所定周期ごとに（離散的な時間ごとに）指令値を計算する際に適用すると誤差を生じるようになる。補正係数をかけても同様にやはり連続的な時間を仮定して算出しているので、誤差が生じる。誤差が生じると、減速開始が適切なタイミングで開始できなくなり、これにより指令値の形状に乱れが生じるという問題があった。

本発明は、上記に鑑みてなされたものであって、加々速度時間が加速開始時、加速終了時、減速開始時、減速終了時でそれぞれ異なるＳ字指令等複雑な指令値に対しても、減速停止に必要な移動量を自動的に、かつ正確に算出し、さらに、メモリ等計算機負荷が少なくても実装が可能な位置決め制御用指令生成装置を提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、経過時間、速度、加速時間、および加々速度時間に基づいて位置指令値を算出する位置指令計算部と、加速動作時には、前記経過時間を位置指令が開始されてからの所定周期毎の経過時間、前記速度を目標速度、前記加々速度時間を加速動作時の加々速度時間、として前記位置指令計算部が算出する位置指令値を出力する加速時位置指令算出部と、前記経過時間および前記加速時間を減速時間、前記加々速度時間を減速時の加々速度時間、として前記位置指令計算部が算出する位置指令値を減速停止に必要な移動量として出力する停止移動量算出部と、目標移動量と前記位置指令値との差である残距離と、前記減速停止に必要な移動量と、を比較することにより減速停止動作を開始するか否かを判断する減速開始タイミング判定部と、前記減速開始タイミング判定部が減速停止動作を開始すると判定したら、前記位置指令計算部の算出結果を用いて、減速停止動作の位置指令値を算出して出力する減速停止制御部と、を備えることを特徴とする。

本発明によれば、指定された形状となるように精度よく指令値を生成する位置決め制御用指令生成装置を得るという効果を奏する。

図１は、実施の形態１によって得られる指令値の時間変化を表すグラフである。 図２は、実施の形態にかかる位置決め制御用指令生成装置、サーボアンプ、サーボモータ、および機械負荷の構成の一例を説明するブロック図である。 図３は、実施の形態１にかかる位置決め制御用指令生成装置が指令値を生成する処理の流れを詳細に説明するフローチャートである。 図４は、実施の形態１における減速停止処理の流れを詳細に説明するフローチャートである。 図５は、実施の形態２によって得られる指令値の時間変化を表すグラフである。 図６は、実施の形態２にかかる位置決め制御用指令生成装置が指令値を生成する処理の流れを詳細に説明するフローチャートである。 図７は、実施の形態２における速度変更時の位置指令値の算出手順についての詳細な内容を説明するフローチャートである。 図８は、実施の形態２における速度変更時の速度波形、速度変更要求を示す図である。 図９は、実施の形態１にかかる位置決め制御用指令生成装置の構成を示すブロック図である。 図１０は、実施の形態２にかかる位置決め制御用指令生成装置の構成を示すブロック図である。

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

実施の形態１．
位置決め制御用指令値の一例として、図１のように加速度の形状が台形であり、速度がＳ字形状を有する指令値を生成する場合について説明を行う。図１に示される指令値は、移動量、加速時間、減速時間、目標速度、および、加速時に加速度が０からピーク加速度に到達するまでの時間である加々速度時間１、加速時に加速度がピーク加速度から０に到達するまでの時間である加々速度時間２、減速時に加速度が０からピーク加速度に到達するまでの時間である加々速度時間３、減速時に加速度がピーク加速度から０に到達するまでの時間である加々速度時間４の情報をもとに、生成される。

図２は、本実施の形態にかかる位置決め制御用指令生成装置１、サーボアンプ３、サーボモータ５、および機械負荷８の構成の一例を説明するブロック図である。モーションコントローラなどである位置決め制御用指令生成装置１は、位置決め制御用の位置指令値２をサーボアンプ３へ出力する。典型的には、位置決め制御用指令生成装置１においては、位置指令値２を生成する手続きやアルゴリズムがＣＰＵやＦＰＧＡなどに実装されることにより実現される。図９は、位置決め制御用指令生成装置１の構成を示すブロック図である。位置決め制御用指令生成装置１は、位置指令計算部１１、加速時位置指令算出部１２、停止移動量算出部１３、減速開始タイミング判定部１４、および減速停止制御部１５を備える。これら各部の機能については、以下で説明する。

サーボアンプ３はサーボモータ５を制御し、サーボモータ５に取り付けられたエンコーダ６がサーボモータ５の位置７をサーボアンプ３に出力する。エンコーダ６は、例えばサーボモータ５の１回転あたり１０００００パルスを発生し、サーボモータ５の回転位置に応じたパルスを出力することが例として挙げられる。サーボモータ５の位置７が位置指令値２に追従するように、位置決め動作を行うためのトルクを発生させる電流４をサーボアンプ３はサーボモータ５に供給する。サーボアンプ３においては、位置指令値２にサーボモータ５の位置７が追従するようにフィードバック制御などが行われている。このように制御されたサーボモータ５は、機械負荷８を駆動する。

図３は、図２および図９に示した位置決め制御用指令生成装置１が位置指令値２を生成するための処理の流れを詳細に説明するフローチャートである。ステップＳ１では、目標移動量Ｄ、目標（トップ）速度ｖ、加速時定数Ｔａ、減速時定数Ｔｄ、加々速度時間比率１（ｒｄ１）、加々速度時間比率２（ｒｄ２）、加々速度時間比率３（ｒｄ３）、加々速度時間比率４（ｒｄ４）をそれぞれ位置決め制御用指令生成装置１に入力する。

ここで、加速時定数Ｔａは、サーボモータ５が速度０からモータ定格速度まで加速した場合に要する時間を表すパラメータで、加速時間を計算するためのパラメータである。減速時定数Ｔｄは、サーボモータ５がモータ定格速度から速度０に減速した場合に要する時間を表すパラメータで、減速時間を計算するためのパラメータである。加々速度時間比率１は加速度が０からピーク加速度１に達するまでの時間が加速時間に占める比率を表すパラメータ、加々速度時間比率２はピーク加速度１から加速度０に達する時間が加速時間に占める比率を表すパラメータ、加々速度時間比率３は加速度０からピーク加速度２に達するまでの時間が減速時間に占める比率を表すパラメータ、加々速度時間比率４はピーク加速度２から加速度０に達する時間が減速時間に占める比率を表すパラメータである。従って、加々速度時間比率１、２、３、４はいずれも０〜１００％の値で指定されるものとする。また、目標速度ｖは１制御周期（所定周期）あたりに位置が増加する量（例えば、１００００パルス／制御周期）として与えられるものとする。さらに、以降の説明では、速度は１制御周期あたりの位置が増加する量、加速度は１制御周期あたりに速度が増加する量をあらわすものとして説明を行う。

ステップＳ２では、以降の処理に使用する変数を初期化する。すなわち、ｉ＝１，Ｒ＝Ｄ，Ｘ［０］＝０とする。ここで、ｉは位置指令値２の出力が開始されてからの経過時間を表し、制御周期ごとにｉが＋１ずつ増加する。Ｘ［ｉ］は時間ｉにおける位置指令値を表し、ｉ＝０すなわち位置指令値２の出力が開始される前の位置を０とする。Ｒは、後述詳細するが、目標移動量Ｄに対して現制御時間での行き足りていない残距離を表す。Ｒ＝Ｄとしているのは、指令が開始される前では位置指令は０としていることに対応している。

ステップＳ３では、目標速度ｖ、加速時定数Ｔａ、加々速度時間比率１（ｒｄ１）、および加々速度時間比率２（ｒｄ２）から、加速時間ｔａ、加々速度時間１（ｄ１）、加々速度時間２（ｄ２）を計算する。具体的には、
ｔａ＝（ｖ×Ｔａ）／Ｖｒ
ｄ１＝（ｔａ×ｒｄ１）／１００
ｄ２＝（ｔａ×ｒｄ２）／１００
に従って計算する。ここで、Ｖｒはモータの定格速度を表し、使用するモータが決まれば一意に決まる値である。また、加速時間ｔａ、加々速度時間１（ｄ１）、２（ｄ２）はいずれも整数として計算するものとする（制御周期の何倍に相当するかの値）。また、上記を計算する際の除算（割り算）は四捨五入で演算するものとし、以降の説明でも除算が発生した場合は四捨五入で計算するものとする。

ステップＳ４では経過時間ｉが、加速時間ｔａ以下かどうかを判定する。もし、経過時間ｉが加速時間ｔａ以下（ステップＳ４：Ｙｅｓ）であれば、ステップＳ５で、指令が開始されてから入力経過した時間ｉ（ｉ＝１，２，３，・・・）に応じて、位置指令計算部１１が位置指令値の候補ＹをＹ＝ｆ（ｉ，ｖ，ｔａ，ｄ１，ｄ２）により計算する。ただし、加速動作時における位置指令値の算出式ｆ（ｉ，ｖ，ｔａ，ｄ１，ｄ２）は以下の（１）式のように定義される。ステップＳ５の後はステップＳ７に進む。

また、ステップＳ４でｉ＞ｔａとなる場合（ステップＳ４：Ｎｏ）、即ち、経過時間ｉが加速時間ｔａを過ぎた場合には、一定速動作になるので、ステップＳ６で、前時間における位置指令値Ｘ［ｉ−１］を用いて、Ｙ＝Ｘ［ｉ−１］＋ｖとして位置指令値の候補値Ｙを計算する。ステップＳ６の後はステップＳ７に進む。

ステップＳ７では、停止移動量算出部１３は、位置指令値の候補Ｙが時間ｉの位置指令値となった場合の速度である予測速度ＶｐをＶｐ＝Ｙ−Ｘ［ｉ−１］により計算する。また、減速時定数Ｔｄと予測速度Ｖｐから減速度から減速停止するのに要する減速時間ｔｄを以下のように計算する。
ｔｄ＝（Ｔｄ×Ｖｐ）／Ｖｒ
減速時間ｔｄを計算したら、さらに、加々速度時間３であるｄ３、加々速度時間４であるｄ４を以下のようにそれぞれ計算する。
ｄ３＝（ｔｄ×ｒｄ３）／１００
ｄ４＝（ｔｄ×ｒｄ４）／１００
ここで、減速時間ｔｄ、加々速度時間３（ｄ３）、４（ｄ４）はいずれも整数データであるとして計算する。

ステップＳ８では、ステップＳ７で算出した予測速度Ｖｐ、減速時間ｔｄ、加々速度時間３のｄ３、加々速度時間４のｄ４に対し、ｖ＝Ｖｐ、ｔａ＝ｔｄ、ｄ１＝ｄ４、ｄ２＝ｄ３とし、さらに、ｉ＝ｔｄを上記した（１）式に代入することにより減速停止に必要な移動量Ｄｄを位置指令計算部１１が算出する。すなわち、
Ｄｄ＝ｆ(ｔｄ,Ｖｐ,ｔｄ,ｄ４,ｄ３)
を算出する。これにより、減速停止に必要な移動量を正確に算出することができる。停止移動量算出部１３は、これを減速停止に必要な移動量Ｄｄとして減速開始タイミング判定部１４に渡す。

ステップＳ９では、減速開始タイミング判定部１４が、ステップＳ８で算出した減速に必要な移動量Ｄｄと残距離Ｒを比較する。比較の結果、残距離Ｒが減速停止に必要な移動量Ｄｄより大きければ（ステップＳ９：Ｙｅｓ）、ステップＳ１０に移行する。ステップＳ１０では、加速時位置指令算出部１２が、時間ｉの位置指令値Ｘ［ｉ］＝Ｙとして、これをサーボアンプ３へ出力する。さらに、ステップＳ１１では、残距離ＲをＲ＝Ｄ−Ｘ［ｉ］により更新する。また、ｉをインクリメント、すなわち、ｉ＝ｉ＋１とする。さらに、ステップＳ７で計算した予測速度Ｖｐ、減速時間ｔｄ、加々速度時間３であるｄ３、加々速度時間４であるｄ４、減速停止に必要な移動量Ｄｄを、それぞれ、Ｖｍｐ＝Ｖｐ、ｔｍｄ＝ｔｄ、ｄｍ３＝ｄ３、ｄｍ４＝ｄ４、Ｄｍｄ＝Ｄｄとして保存する。その後、次の制御周期でステップＳ４以降を再度実行する。

また、ステップＳ９において、減速開始タイミング判定部１４が残距離Ｒが減速に必要な移動量Ｄｄより小さいと判断すれば（ステップＳ９：Ｎｏ）、ステップＳ１２の減速停止処理に移行し処理を完了する。図４は、ステップＳ１２の減速停止処理、すなわち、減速停止時の位置指令値を算出する詳細について説明するフローチャートである。ステップＳ１２の減速停止処理即ち、図４のフローチャートは、減速停止制御部１５により実行される。

図４のステップＳ１０１では、減速停止時に補正分残距離Ｈを、残距離Ｒと減速停止に必要な移動量Ｄｍｄとの差により計算する。すなわち、Ｈ＝Ｒ−Ｄｍｄとする。なお、ここで用いるＤｍｄは、ステップＳ９で残距離Ｒが減速に必要な移動量Ｄｄより小さいとはじめて判定されるときに保存されていたＤｍｄ、即ち前回の制御周期におけるＤｄなので、Ｈは負にならない。

ステップＳ１０２では、後の処理に使用する変数を初期化する。すなわち、ｊ＝１、Ｖｏｌｄ＝Ｖｍｐとする。ここで、ｊは減速動作を開始してからの時間を表し、Ｖｏｌｄは前制御周期での速度を表す変数である。

ステップＳ１０３では、（１）式においてｖ＝Ｖｍｐ、ｔａ＝ｔｍｄ、ｄ２＝ｄｍ３、ｄ１＝ｄｍ４として、位置指令計算部１１が
Ｙ１＝ｆ（ｔｍｄ−ｊ＋１，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）
Ｙ２＝ｆ（ｔｍｄ−ｊ，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）
を算出する。さらに、ステップＳ１０４では、減速時の速度Ｖｄ＝Ｙ１−Ｙ２を計算する。

ステップＳ１０５では、ステップＳ１０４で計算した減速時の速度Ｖｄに対し、
Ｖｄ≦Ｈ＜Ｖｏｌｄを満たすかどうかを判定する。

もし、満たさなければ（ステップＳ１０５：Ｎｏ）、ステップＳ１０６に進み、変数ｊをインクリメント(ｊ＝ｊ＋１)とし、ステップＳ１０８へ移行する。また、満たしていれば（ステップＳ１０５：Ｙｅｓ）、ステップＳ１０７に進み、減速時の速度Ｖｄ＝Ｈとする。これは、残距離補正を行うための処理である。その後、ステップＳ１０８へ移行する。

ステップＳ１０８では、今回の位置指令値Ｘ［ｉ］を、前回の制御周期における前回の位置指令値Ｘ［ｉ−１］にＶｄを加えることにより算出する。すなわち、
Ｘ［ｉ］＝Ｘ［ｉ−１］＋Ｖｄ
とする。そして減速停止制御部１５は、これをサーボアンプ３へ出力する。

ステップＳ１０９では、Ｖｏｌｄを更新し、変数ｉをインクリメントする。すなわち、以下を実行する。
Ｖｏｌｄ＝Ｖｄ，ｉ＝ｉ＋１

ステップＳ１１０では、減速動作を開始してからの経過時間を表す変数であるｊが、減速時間ｔｍｄに等しいかどうかを確認する。もし、等しくなければ（ステップＳ１１０：Ｎｏ）、次の制御周期でステップＳ１０３以降を再度実行する。また、ｊ＝ｔｍｄであれば（ステップＳ１１０：Ｙｅｓ）、処理を終了する。処理を終了した時点で、移動量が所望の（目標）移動量Ｄであり、かつ、図３のステップＳ１で指定した目標速度ｖ、加速時定数Ｔａ、減速時定数Ｔｄ、加々速度時間比率１（ｒｄ１）、加々速度時間比率２（ｒｄ２）、加々速度時間比率３（ｒｄ３）、加々速度時間比率４（ｒｄ４）で指定される図１に示すようなＳ字状の位置指令値を算出している。即ち、減速停止処理における位置指令値を指定された形状通りに精度よく算出することができる。

次に、本実施の形態に従って指令値を生成したときに得られる効果について説明をする。本実施の形態において、中心となる処理は、加速時の位置指令値を計算する（１）式である。

（１）式において、１≦ｉ≦ｄ１に対応する計算式は、図１の指令値の加速動作中の加々速度時間１での位置を計算する式である。また、ｄ１＜ｉ≦ｔａ−ｄ２に対応する計算式は、図１の指令値の加速動作中の一定加速度時間での位置を計算する式である。また、ｔａ−ｄ２＜ｉ≦ｔａに対応する計算式は、図１の指令値の加速動作中の加々速度時間２での位置を計算する式に対応している。（１）式により、加速動作時、減速動作時における位置指令値、および、減速に必要な移動量を精度良く算出することで、図１のような形状の指令値を精度良く生成することができる。

図３のフローチャートにおいて、ステップＳ１で入力された諸情報に基づいて、ステップＳ３では加速動作時の加速時間ｔａ、加々速度時間１（ｄ１）、加々速度時間２（ｄ２）を計算し、これらの値を用いて、加速動作時には、経過時間ｉに応じて（１）式に従って経過時間ｉ（今回の制御周期）における位置指令値の候補値Ｙを計算する（ステップＳ５）。また、経過時間ｉが加速時間ｔａを過ぎれば、一定速動作になるので、ステップＳ６では、前時間（前制御周期）の位置指令値Ｘ［ｉ−１］に目標速度に相当するｖを加えることで、一定の速度を保つための位置指令値を生成する（ステップＳ６）。

ステップＳ５およびステップＳ６で、位置指令値の候補Ｙを算出しながら、これと同じ制御周期で候補Ｙが、現制御周期での位置指令値となった場合に、減速停止するのに必要な移動量が確保されるかどうかのチェックをステップＳ８、Ｓ９で行う。具体的には、位置指令値の候補Ｙが位置指令値となった場合の予測速度Ｖｐを、Ｙと前回位置指令値Ｘ［ｉ−１］の差から算出し、この速度から、減速時間ｔｄ、加々速度時間３（ｄ３）、４（ｄ４）を算出し（ステップＳ７）、これらの情報と、（１）式を用いて減速に必要な移動量Ｄｄを算出する。

ここで、（１）式を用いて減速に必要な移動量を算出できる理由は、（１）式において経過時間に加速時間を代入して算出される値は、加速時間における位置指令値であるのは勿論であるが、加速に必要な移動量ともみなすことができるからである。同様に、経過時間に減速時間、および、目標速度に減速を開始する速度、加速時間に減速時間、加速時の加々速度時間に減速時の加々速度時間を代入して算出される値は、減速に必要な移動量Ｄｄとみなすことができる。

もし、残距離Ｒが減速に必要な移動量Ｄｄよりも大きければ、位置指令値の候補Ｙを今回の制御周期の位置指令値としても減速に必要な移動量が確保できるものと判断して、Ｙを今回の制御周期の位置指令値とする（ステップＳ１０）。また、残距離Ｒが減速に必要な移動量Ｄｄよりも小さければ、今回の制御周期では候補Ｙを位置指令値にするのではなく、減速停止処理を行うことによって位置指令値を算出するという処理を行う（ステップＳ１２）。減速停止処理を開始するタイミングが早すぎると減速停止処理終了時に目標移動量に対して行き足りず減速停止動作時に補正する補正量が大きくなりすぎ、これにより指令値の形状が大きく乱れたり、遅すぎれば目標移動量をオーバーするが、本実施の形態のように減速停止処理を行うことで、適切なタイミングで減速停止処理を開始することができる。

図１に示す指令値は加速動作中も減速動作中も、加速時間、減速時間、加々速度時間の値に差はあるにしろ、減速時の加速度の形状が台形となる同一のパターンをとるので、図４に示した減速停止処理中の各時間ｊ＝１，２，・・・，ｔｍｄにおいても（１）式を用いて位置指令値を生成する。

このとき、目標速度ｖにあたるパラメータには、減速を開始するときの速度であるＶｍｐを、また、加速時間にあたるパラメータには減速時間ｔｍｄを、加々速度時間１にあたるパラメータには加々速度時間４（ｄｍ４）を、加々速度時間２にあたるパラメータには加々速度時間３（ｄｍ３）をそれぞれ代入する。また、（１）式は経過時間ｉが増加するにつれて速度が増加する指令値を計算するので、減速停止動作をする指令値を計算するためには、減速時間から減速停止処理を引いたｔｍｄ−ｊを経過時間として適用することにより、ｊが増加するにつれて速度が減少する指令値を算出することができる。

このとき、減速停止処理中の位置指令値の差分である速度は、経過時間ｔｍｄ−ｊ＋１のときの位置指令値Ｙ１と経過時間のｔｍｄ−ｊのときの位置指令値Ｙ２との差、すなわち、
Ｙ１＝ｆ（ｔｍｄ−ｊ＋１，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）
Ｙ２＝ｆ（ｔｍｄ−ｊ，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）
としたときの
Ｖｄ＝Ｙ１−Ｙ２
で表すことができる(ステップＳ１０３，Ｓ１０４)。この値Ｖｄを前回の制御周期における位置指令値Ｘ［ｉ−１］に加えることで、今回の制御周期における位置指令値Ｘ［ｉ］を算出する(ステップＳ１０８)。

また、この減速動作中の各時間ｊ＝１，２，・・・，ｔｍｄにおいて算出される速度を合計して、減速停止に必要な移動量を計算すると、
｛ｆ（ｔｍｄ，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）−ｆ（ｔｍｄ−１，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）｝＋
｛ｆ（ｔｍｄ−１，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）−ｆ（ｔｍｄ−２，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）｝＋
｛ｆ（ｔｍｄ−２，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）−ｆ（ｔｍｄ−３，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）｝＋
・・・
｛ｆ（２，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）−ｆ（１，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）｝＋
｛ｆ（１，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）−ｆ（０，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）｝
＝ｆ（ｔｍｄ，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）−ｆ（０，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）
＝ｆ（ｔｍｄ，Ｖｍｐ，ｔｍｄ，ｄｍ４，ｄｍ３）
となるが、これはステップＳ８で計算した減速に必要な移動量Ｄｄと一致する。

なぜならば、減速停止動作(ステップＳ１２)へ移行した際に、保存されている予測速度、減速時間、加々速度時間３、加々速度時間４は、ステップＳ９でＲ＞Ｄｄと判断されたときの制御周期における予測速度Ｖｐ、減速時間ｔｄ、加々速度時間３(ｄ３)、４（ｄ４）ではなく、その１つ前の制御周期の予測速度、減速時間、加々速度時間３、４であり、これらはステップＳ１１でそれぞれＶｍｐ，ｔｍｄ，ｄｍ３，ｄｍ４として保存された値である。これらを（１）式に代入したものは上式に代入したものに等しい。よって、減速に必要な移動量を（１）式により算出し、減速に必要な移動量と残距離を比較し、比較結果によって減速開始動作を開始するタイミングを決定し、さらに、減速動作時の位置指令値も同じ（１）式を用いて算出することにより、指定された形状の指令値を精度良く算出することができるという効果がある。これは、指令値生成に要する処理を浮動小数点演算ではなく、整数演算（固定小数点演算）で計算しても、減速停止に必要な移動量を１パルスのずれなく算出することができるという効果もある。

減速停止処理の制御は基本的には、前述のようにＶｄ＝Ｙ１−Ｙ２で速度を計算し、これを前回の制御周期の位置指令値Ｘ［ｉ−１］に加えることで、今回の制御周期の位置指令値Ｘ［ｉ］を算出する。減速を開始する直前の位置指令値の値と減速に必要な移動量Ｄｍｄの合計は、所望の移動量Ｄに一般には一致するとは限らないため、減速停止処理を開始する前に、一致しない分を補正分残距離Ｈとして算出し（ステップＳ１０１）、減速停止処理の途中で、補正分残距離Ｈを何らかの形で位置指令値に加えて、減速停止処理完了後の位置指令値が所望の移動量Ｄに一致する補正を行う。本実施の形態で説明した具体的な補正分残距離を補正する方法は、補正分残距離に相当する速度を減速停止処理中の速度として挿入することにより実現する。減速停止処理時には速度が徐々に減少する位置指令値を生成することから、補正分残距離Ｈが、ある時刻ｊに対して
時刻ｊのＶｄ≦Ｈ＜時刻ｊ−１のＶｄ（＝Ｖｏｌｄ）
が成立する制御周期において（ステップＳ１０５：Ｙｅｓ）、速度Ｖｄを補正分残距離Ｈとする処理を行う（ステップＳ１０７）。減速時間の各制御の周期において速度が漸減するＶｄ＝Ｙ１−Ｙ２の系列に対して、このように補正分残距離Ｈを指令値に反映させても、速度が漸減する性質は失われず、指令値の形状をほとんど崩すことなく減速停止時の位置指令値を生成できるという効果がある。

減速停止処理を開始する直前の時間ｉ＝ｋとしたとき、補正分残距離を補正して減速停止処理を終了したときの移動量は、時間ｋでの位置指令値Ｘ［ｋ］と減速停止に必要な移動量Ｄｍｄと補正分残距離Ｈの合計であることから、
Ｘ［ｋ］＋Ｄｍｄ＋Ｈ＝Ｘ［ｋ］＋Ｄｍｄ＋（Ｒ−Ｄｍｄ）
＝Ｘ［ｋ］＋Ｒ
＝Ｘ［ｋ］＋（Ｄ−Ｘ［ｋ］）
＝Ｄ
となり、減速停止終了後に所望の移動量にずれを生じさせることなく、位置決め制御用の指令値が生成できる。ここで、Ｈ＝Ｒ−Ｄｍｄ(ステップＳ１０１)、Ｒ＝Ｄ−Ｘ［ｋ］(ステップＳ１１)であることを利用している。

なお、補正分残距離Ｈを補正する方法はこれに限られるものではなく、例えば、補正分残距離Ｈを、合計して補正分残距離Ｈになるように減速時間数ｔｍｄ個に分割して、分割した値を減速処理動作の各制御時間の減速速度に加えてもよい。具体的な分割方法としては、（１）式にしたがって減速処理動作の指令値を計算した際の、加速度の絶対値に比例して計算するようにすることが例として挙げられる。具体的には、減速停止処理を開始してから経過時間ｊが経った時の速度をｖ（ｊ）としたときに、
｛｜ｖ（ｊ−１）−ｖ（ｊ）｜×Ｈ｝／Ｖｍｐ
を時間ｊにおける速度Ｖｄに加え、さらに、前回の制御周期の位置指令値を加えることによって、減速停止処理における位置指令値を算出する。

なお、減速停止に必要な移動量が減速停止処理を開始する前に正確に算出できていないと、補正分残距離Ｈもまた正確に算出されない。不正確な補正分残距離をもとに補正を行っても、減速停止処理終了時に所望の移動量Ｄに一致しないという不都合が生じるが、本実施の形態によれば、（１）式に従って減速停止動作時の位置指令値を計算した際の、減速に必要な移動量が減速開始前に１パルスのずれなく正確に算出できるため、上記の不都合は生じないという効果がある。この結果、指定された形状となるように精度よく指令値を生成することができる。

なお、本実施の形態では、加速時間、減速時間を決定するにあたり、加速時定数、減速時定数を設け、速度と加速、減速時定数から加速時間および減速時間を決定したが、加速時の加速度、減速時の加速度を指定して、加速時間（もしくは減速時間）＝速度／加速度の関係から算出してもよい。本実施の形態では、加々速度時間において加速度が直線的に増加、あるいは減少するタイプの指令値を例にあげて説明したが、加々速度時間を用いて加速度が三角関数を用いて増加、あるいは減少するタイプのユニバーサルカム曲線などの指令値に適用されてもよい。ユニバーサルカム曲線に適用する場合、加速時の位置指令値を計算する（１）式が、ユニバーサルカム曲線に対応した式になるだけで、他の処理は全く同じでよい。また、本実施の形態では、位置決め制御を回転型サーボモータ、回転型エンコーダを使用した構成について説明したが、リニアタイプのサーボモータ、および、エンコーダとしてリニアスケールを使用してもよい。また、サーボモータのかわりにステッピングモータを用いても良い。

実施の形態２．
実施の形態１では、目標速度を与え、この目標速度に従って、加速動作、速度一定の動作、減速動作を順次行う位置決め制御用の位置指令値を生成する位置決め制御用指令生成装置について説明を行った。同じ考え方で、加速もしくは一定速度中に速度変更要求信号を受信し、速度変更要求信号を受信後に、変更速度への加速（もしくは減速）動作時の加速度に関しても指定された形状通りにしながら、速度を目標速度から変更速度に変更して指令値を生成することが可能であり、本実施の形態ではこの内容について説明を行う。本実施の形態における位置決め制御用指令生成装置１の構成を図１０に示す。図１０では、図９の構成に、速度変更時位置指令計算部１６が追加された構成になっている。

図５は、本実施の形態によって得られる指令値の速度、加速度と速度変更要求信号を表すグラフである。図１で示したグラフとの違いは、始動開始直後は目標速度ｖで加速し、動作中に速度変更要求信号を受信すると目標速度を変更速度ｖ２に変更して、所望の移動量Ｄに位置決め制御するための指令値を生成する点である。ここで、目標速度ｖから変更速度ｖ２に加速（あるいは減速）する際には、速度変更加速時間で加速し、さらに、速度変更中に加速度がピーク加速度に到達するまでの加々速度時間５を要し、またピーク加速度から加速度が０になるまでの加々速度時間６を要するように指令値を生成する。

図６は、本実施の形態にかかる位置決め制御用指令生成装置１が指令値を生成する処理の流れを詳細に説明するフローチャートである。図３と同じ番号を付したステップは、図３と同じであるため説明を省略する。図３と異なるのは、ステップＳ３の処理の後に速度変更要求を受信したか否かを判定するステップＳ２０と、速度変更要求を受信した場合（ステップＳ２０：Ｙｅｓ）に、速度変更時の位置指令値の候補Ｙを算出するステップＳ２１と、ステップＳ２１を実行した後に、その後ステップＳ７以降を実行する点である。また、ステップＳ２０で速度変更要求が無いと判断された場合（ステップＳ２０：Ｎｏ）には、図３と同様ステップＳ４以降を実行する。

図７は、速度変更時の位置指令値の候補Ｙを算出するステップＳ２１の詳細な内容を説明するフローチャートである。ステップＳ２０１では、速度変更要求直後であるかどうかを判定する。速度変更要求直後であるとは、速度変更要求信号を受信した（図５の速度変更要求信号が立ち上がったとき）直後の制御周期であることを指す。速度変更要求直後であれば（ステップＳ２０１：Ｙｅｓ）、ステップＳ２０２に移行し、そうでなければ（ステップＳ２０１：Ｎｏ）、ステップＳ２０６に移行する。

ステップＳ２０２で、変更速度ｖ２、速度変更加速度時定数Ｔｃ、速度変更加々速度時間比率５、６であるｒ５，ｒ６を取得する。ここで、これらの値は、速度変更要求直後に受信されてもよいし、指令開始時に予め受信されていてもよい。

ステップＳ２０３では、速度変更要求を受信する直前の位置Ｘ０と速度ｖ０（現在速度）それぞれを、Ｘ０＝Ｘ［ｉ−１］、ｖ０＝Ｘ［ｉ−１］−Ｘ［ｉ−２］として取得する。

ステップＳ２０４では、現在速度と変更速度の差ＤＶ＝ｖ２−ｖ０を算出し、速度変更要求受信時間ｉ０を記憶しておく。

ステップＳ２０５では、差速度ＤＶと、速度変更加減速時定数Ｔｃと速度変更加々速度時間比率５、６から、速度変更加速時間ｔｃと速度変更加々速度時間５，６であるｄ５，ｄ６を算出する。具体的には、
ｔｃ＝（ＤＶ×Ｔｃ／Ｖｒ）
ｄ５＝（ｒｄ５×ｔｃ）／１００
ｄ６＝（ｒｄ６×ｔｃ）／１００
として計算する。

ステップＳ２０６では、ｉ−ｉ０が速度変更加速時間ｔｃより小さいかどうかを判定する。ここで、ｉ−ｉ０は現在の時間から速度変更要求受信時間を引いた差なので、速度変更要求を受信してからの経過時間を表す。ステップＳ２０６で、ｉ−ｉ０が速度変更加速時間（ｔｃ）より小さい場合（ステップＳ２０６：Ｙｅｓ）はステップＳ２０７に移行し、大きい場合（ステップＳ２０６：Ｎｏ）はステップＳ２０８に移行する。

ステップＳ２０７では、位置指令値の候補値Ｙを、ステップＳ２０３で算出した位置Ｘ０、速度ｖ０、および（１）式を用いて、以下の（２）式により速度変更時位置指令計算部１６が計算する。なお、（１）式の演算は上述したように位置指令計算部１１が実行する。
Ｙ＝Ｘ０＋ｖ０＊（ｉ−ｉ０）＋ｆ（ｉ−ｉ０，ＤＶ，ｔｃ，ｄ５，ｄ６）・・・（２）

ステップＳ２０８では、位置指令値の候補値Ｙを、前回の制御周期における位置Ｘ［ｉ−１］に変更速度ｖ２を加えることによって、すなわち、
Ｙ＝Ｘ［ｉ−１］＋ｖ２
により計算する。ステップＳ２０７もしくはＳ２０８の処理が終了したら、図６の中のステップＳ２１が終了したことになるので、その後、ステップＳ７以降を実行する。ステップＳ７以降は、実施の形態１と同じ処理を行うので説明を省略する。

次に、本実施の形態で得られる効果について説明する。位置決め制御のための位置指令値を算出する点は実施の形態１と同じである。実施の形態１と異なるのは、速度変更要求信号の有無に応じて、位置指令値の候補Ｙの算出方法が異なる。速度変更要求信号を受信した直前の制御周期における位置Ｘ０と速度ｖ０を計算し（ステップＳ２０３）、変更速度と速度変更要求信号受信時の速度との差速度ＤＶを計算し(ステップＳ２０４)、差速度ＤＶから速度変更時の加速動作に必要な情報である速度変更加速時間ｔｃ、速度変更加々速度時間５、６であるｄ５，ｄ６を算出する（ステップＳ２０５）。もし、現在時間ｉと速度変更要求受信時間ｉ０との差が速度変更加速時間ｔｃ以下であれば、速度変更時の加速動作のための位置指令値を（２）式により算出し（ステップＳ２０７）、ｔｃ以上であれば変更速度に到達しているものとして、位置指令値の候補Ｙを前回の制御周期の位置指令値Ｘ［ｉ−１］に変更速度ｖ２を加えることで算出する。

（２）式の計算式の意味を、速度変更時の速度波形、速度変更要求を示す図８を参照しながら説明する。図８において黒丸は、各時間における速度を表すものとする（黒丸を接続したものが速度として表されている）。（２）式は、３つの項から構成されており、第一項のＸ０は、指令が開始されてから速度変更要求信号を受信する直前の時間までに（図８において時間１から時間ｉ０−１までに）移動した移動量を表している。第二項のｖ０＊（ｉ−ｉ０）は、速度変更要求信号が受信される直前の時間ｉ０−１における速度（図８の中では目標速度ｖに等しい）を、速度変更要求を受信した時間ｉ０以降も、継続した場合の移動量を表している。第三項のｆ（ｉ−ｉ０，ＤＶ，ｔｃ，ｄ５，ｄ６）は、速度変更要求信号を受信した直後に、停止状態から速度ＤＶ（図８の中では、ＤＶ＝ｖ２−ｖに等しい）に加速時間ｔｃ、加々速度時間がそれぞれｄ５，ｄ６であるパターンに従って移動したときの、時間ｉ−ｉ０における位置指令値に相当する。

ここで、第三項は、図８において速度＝ｖの破線と時間区間［ｉ０，ｉ］と太線の速度曲線とで囲まれる部分の面積で表される移動量に等しいことに注意すると、第一項、第二項、第三項をすべて合計することで得られる位置指令値の候補Ｙは、図８の太線で表されている速度曲線と時間軸と時間ｉの破線とで囲まれる部分の面積で表される移動量を表している。よって、速度変更を行うような指令値を（１）式を利用して計算することで、簡単に計算できるという効果がある。また、減速停止処理を開始するタイミングの決定、および減速停止処理などのステップＳ７以降の処理を実施の形態１と同様の処理を行うので、減速停止に必要な移動量を１パルスのずれもなく正確に算出することができ、これにより、加速時定数、減速時定数、加々速度時間１、２、３、４で指定された形状に対し精度良く、かつ、計算負荷の小さい指令値を計算することが可能になるという効果がある。従って、速度変更処理を行っても、指定されて形状通りに正確に指令値を算出することができる。

なお、図６、図８では変更速度ｖ２は、目標速度ｖより大きい場合を例に示して説明したが、目標速度より小さくてもよい。また、指令値が一定の速度を保っている間に速度変更処理を行っているが、加速動作中に速度変更処理を行ってもよく、図７のフローチャートに従って計算を行うことで実現することができる。

さらに、本願発明は上記実施の形態に限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で種々に変形することが可能である。また、上記実施の形態には種々の段階の発明が含まれており、開示される複数の構成要件における適宜な組み合わせにより種々の発明が抽出されうる。例えば、上記実施の形態に示される全構成要件からいくつかの構成要件が削除されても、発明が解決しようとする課題の欄で述べた課題が解決でき、発明の効果の欄で述べられている効果が得られる場合には、この構成要件が削除された構成が発明として抽出されうる。更に、異なる実施の形態にわたる構成要素を適宜組み合わせてもよい。

以上のように、本発明にかかる位置決め制御用指令生成装置は、サーボモータなどのモータを用いて機械負荷を位置決め制御するに際し、位置決め制御時の目標参照信号となる指令値を生成する場合に有用であり、特に、指定された形状となるように精度よく指令値を生成する位置決め制御用指令生成装置に適している。

１ 位置決め制御用指令生成装置
２ 位置指令値
３ サーボアンプ
４ 電流
５ サーボモータ
６ エンコーダ
７ 位置
８ 機械負荷
１１ 位置指令計算部
１２ 加速時位置指令算出部
１３ 停止移動量算出部
１４ 減速開始タイミング判定部
１５ 減速停止制御部
１６ 速度変更時位置指令計算部

Claims (8)

1. 経過時間、速度、加速時間、および加々速度時間に基づいて位置指令値を算出する位置指令計算部と、
   加速動作時には、前記経過時間を位置指令が開始されてからの所定周期毎の経過時間、前記速度を目標速度、前記加々速度時間を加速動作時の加々速度時間、として前記位置指令計算部が算出する位置指令値を出力する加速時位置指令算出部と、
   前記経過時間および前記加速時間を減速時間、前記加々速度時間を減速時の加々速度時間、として前記位置指令計算部が算出する位置指令値を減速停止に必要な移動量として出力する停止移動量算出部と、
   目標移動量と前記位置指令値との差である残距離と、前記減速停止に必要な移動量と、を比較することにより減速停止動作を開始するか否かを判断する減速開始タイミング判定部と、
   前記減速開始タイミング判定部が減速停止動作を開始すると判定したら、前記残距離と、減速停止を開始する直前の制御周期で算出された減速に必要な移動量との差により補正分残距離を算出した後、前記位置指令計算部の算出結果を用いて、減速停止動作の位置指令値を算出して出力し、さらに、前記補正分残距離が前記減速時の速度を下回ったタイミングにて、前記補正分残距離に前回制御周期前の位置指令値を加えた値を現在の位置指令値として算出する減速停止制御部と、
   を備える
   ことを特徴とする位置決め制御用指令生成装置。
2. 経過時間、速度、加速時間、および加々速度時間に基づいて位置指令値を算出する位置指令計算部と、
   加速動作時には、前記経過時間を位置指令が開始されてからの所定周期毎の経過時間、前記速度を目標速度、前記加々速度時間を加速動作時の加々速度時間、として前記位置指令計算部が算出する位置指令値を出力する加速時位置指令算出部と、
   前記経過時間および前記加速時間を減速時間、前記加々速度時間を減速時の加々速度時間、として前記位置指令計算部が算出する位置指令値を減速停止に必要な移動量として出力する停止移動量算出部と、
   目標移動量と前記位置指令値との差である残距離と、前記減速停止に必要な移動量と、を比較することにより減速停止動作を開始するか否かを判断する減速開始タイミング判定部と、
   前記減速開始タイミング判定部が減速停止動作を開始すると判定したら、前記残距離と、減速停止を開始する直前の制御周期で算出された減速に必要な移動量との差により補正分残距離を算出した後、前記位置指令計算部の算出結果を用いて、減速停止動作の位置指令値を算出して出力し、前記補正分残距離を、合計して前記補正分残距離になるように前記所定周期単位でみた前記減速時間の数に分割し、分割した値、減速動作時の各制御周期の速度および前回制御周期前の位置指令値を加えた値を、現在の位置指令値として算出する減速停止制御部と、
   を備える
   ことを特徴とする位置決め制御用指令生成装置。
3. 前記減速停止制御部は、前記補正分残距離を、減速動作における加速度の絶対値に比例した値となるように分割して前記分割した値を求める
   ことを特徴とする請求項２に記載の位置決め制御用指令生成装置。
4. 前記減速停止制御部は、前記経過時間を前記減速時間から前記所定周期ずつ減らした時間、前記加速時間を前記減速時間、前記加々速度時間を減速時の加々速度時間、として前記位置指令計算部が算出する位置指令値の差分を用いて、減速停止動作の位置指令値を算出して出力する
   ことを特徴とする請求項１から３のいずれか１項に記載の位置決め制御用指令生成装置。
5. 速度変更要求の受信の直前の位置の値、前記受信の直前の現在速度の値に現在時間と前記受信の時間の差時間を乗じて得られる値、および、前記時間を前記差時間、前記速度を変更速度と前記現在速度との差速度、前記加速時間を速度変更加速時間、前記加々速度時間を速度変更加々速度時間として前記位置指令計算部が算出する位置指令値の合計値により、前記受信の後の位置指令値を算出する速度変更時位置指令計算部と、
   をさらに備える
   ことを特徴とする請求項１から４のいずれか１項に記載の位置決め制御用指令生成装置。
6. 前記停止移動量算出部は、前記速度を加速動作時に前記位置指令計算部が算出する現在の位置指令値と前記所定周期前の位置指令値との差から求めた現在の予測速度、として前記位置指令計算部が算出する位置指令値を減速停止に必要な移動量として出力する
   ことを特徴とする請求項１から５のいずれか１項に記載の位置決め制御用指令生成装置。
7. 前記減速停止制御部は、
   前記経過時間を前記減速時間から減速を開始してからの前記所定周期単位での経過時間を減じた時間に前記所定周期を加えた時間、前記加速時間を前記減速時間、前記加々速度時間を減速時の加々速度時間、として前記位置指令計算部が算出する第１位置指令値から
   前記経過時間を前記減速時間から減速を開始してからの前記所定周期単位での経過時間を減じた時間、前記加速時間を前記減速時間、前記加々速度時間を減速時の加々速度時間、として前記位置指令計算部が算出する第２位置指令値を減じる
   ことにより求めた減速時の速度に前記所定周期前の位置指令値を加えた値を現在の位置指令値として算出する
   ことを特徴とする請求項１から６のいずれか１項に記載の位置決め制御用指令生成装置。
8. 前記位置指令計算部は、前記経過時間をｉ、前記速度をｖ、前記加速時間をｔ_a、加速度０からピーク加速度に到達するまでの加々速度時間をｄ₁、前記ピーク加速度から加速度０に到達するまでの加々速度時間をｄ₂、とおいた場合に以下のｆ（ｉ，ｖ，ｔ_a，ｄ₁，ｄ₂）で示される値を前記位置指令値として算出する
   

   

   ことを特徴とする請求項１から７のいずれか１項に記載の位置決め制御用指令生成装置。

Images