JP5902911B2 - 制御指令値生成プログラム、制御指令値生成方法および制御指令値生成装置 - Google Patents

Description

本発明は、ロボットなどの指令値および加加速度指令値などを生成する制御指令値生成プログラム、制御指令値生成方法および制御指令値生成装置に関する。

一般的に、産業ロボット等として用いられるロボットは、１体のロボットに複数のモータが設けられた多軸のロボットとして構成されている。このようなロボットの制御においては、各軸の位置指令値に基づいてその軸の速度、加速度、加加速度などの制御指令値を求めて制御することが知られている。一般的に、ロボット制御においてはデジタル制御を行うため、制御指令値が離散的な値となる。したがって、制御指令生成周期がモータ制御周期に比べて大きいと、制御指令生成周期ごとに生成される位置指令値が大きく変化してしまう。位置指令値が大きく変化すると、速度、加速度、加加速度などの変化が急激になりサージが発生する場合がある。そこで、位置指令値を微分して速度などの制御指令値を生成するとともに位置指令値の変化を滑らかにすることができるデジタルフィルタを適用することが知られている（例えば特許文献１参照）。特許文献１においては、位置指令値から位置、速度、加速度の各制御指令値を求めるために、１次フィルタを複数入れ子状にしたようなデジタルフィルタが設けられている。

特開２０１１−１６４６８０号公報

しかし、上記のようなデジタルフィルタでは、前回の演算で求められた速度、位置の各制御指令値の微分値を演算することで、加速度、速度の各制御指令値を生成しており、各制御指令値が小さくならない場合がある。生成される制御指令値の変化を連続系に近い形で実現できるデジタルフィルタとしては、ＩＩＲ（無限インパルス応答）フィルタが考えられるが、フィルタの次数を高くするほど、および／または、演算周期を短くするほど、演算誤差が大きくなり、特に高次の微分の出力が振動的になる問題がある。例えば位置指令値から加加速度に関する制御指令値を演算するために、３次のＩＩＲフィルタを適用するとすると、連続系の伝達関数Ｈ（ｓ）は、以下のように表される。

ここで、ａ_０〜ａ_３は入力側における各次数のフィルタ係数を示し、ｂ_０〜ｂ_３は出力側における各次数のフィルタ係数を示す。

これを離散化するために、双一次変換を行って、上記伝達関数Ｈ（ｓ）を以下に示す離散的な伝達関数Ｈ（ｚ）に変換する。

ここで、Ｐ_０〜Ｐ_３は変換後の入力側における各次数のフィルタ係数を示し、ｑ_０〜ｑ_３は変換後の出力側における各次数のフィルタ係数を示し、Ｔはサンプリング周期を示す。

このように、双一次変換において連続系の変数（ラプラス変数）ｓを離散系の変数ｚに変換する際に、サンプリング周期Ｔが用いられるため、離散化された伝達関数Ｈ（ｚ）におけるｎ次のフィルタ係数には、サンプリング周期Ｔのｎ乗が乗算されることになる。したがって、各制御指令値の精度を高めるためにサンプリング周期を短くすると、高次のフィルタ係数（上記式においてはｐ_３やｑ_３）が指数関数的に小さくなり、結果として出力される制御指令値が振動的になり誤差が大きくなる。前述したように制御指令値の変化を小さく抑えるためには、できるだけサンプリング周期は短い方がよいが、サンプリング周期を短くすると制御指令値の誤差が大きくなってしまい、ロボット制御にうまく適用できない問題がある。

また、上記従来のデジタルフィルタにおいては、一の入力指令値（位置指令値）から複数の制御指令値（速度、加速度、加加速度など）を演算する場合には、１つずつ制御指令値を演算する必要があるため、演算負荷が増大する問題もある。

本発明は、以上のような課題を解決すべくなされたものであり、一の入力指令値から複数の制御指令値を高精度かつ安定的に生成するとともにその際の演算負荷を低減することができる制御指令値生成プログラム、制御指令値生成方法および制御指令値生成装置を提供することを目的とする。

本発明に係る制御指令値生成プログラムは、コンピュータを、所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧０）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタとして機能させるものである。

上記プログラムによれば、生成される制御指令値の連続系の伝達関数に対応する離散的な状態方程式を用いることにより、連続系の伝達関数を誤差なく離散化できるため、制御指令値を高精度かつ安定的に生成することができる。さらに、生成される制御指令値が入力指令値のｎ階までの微分であることより、各制御指令値の連続系の伝達関数が互いに共通の分母を有する。このため、連続系の伝達関数から状態方程式への当てはめを行う際に、全ての制御指令値が１組の状態方程式で表せる。したがって、上記プログラムによれば、一の入力指令値から複数の制御指令値を高精度かつ安定的に生成するとともにその際の演算負荷を低減することができる。

前記所定の入力指令値は、制御対象の位置指令値であり、前記デジタルフィルタは、前記位置指令値から当該位置指令値の３階までの微分で表せる、位置指令値、速度指令値、加速度指令値および加加速度指令値の４つの制御指令値を生成する３次のデジタルフィルタであってもよい。

前記入力指令値の伝達関数Ｇ_ｉ（ｓ）を以下の式（１）で表したとき、前記入力指令値を１〜ｎ階微分した制御指令値Ｇ_１（ｓ）〜Ｇ_ｎ（ｓ）は、以下の式（２）で示され、前記ｎ次のデジタルフィルタの状態方程式は、入力ｕに対する出力をｙとして以下の式（３）で示されるものでもよい。

生成される制御指令値が入力指令値のｎ階微分であることより、各制御指令値の連続系の伝達関数が互いに共通の分母を有するため、状態方程式におけるＡ行列をすべての制御指令値に対して共通化することができる。また、各制御指令値の連続系の伝達関数の分子に対応するＣ行列は各制御指令値の連続系の分子を示す１×ｎ行列をｎ＋１個並べて（ｎ＋１）×ｎ行列とすることにより、１つの状態方程式で表すことができる。ここで、入力指令値のｎ階微分で表される制御指令値における連続系の伝達関数は、分母と分子との次数が同じになるため、そのままでは状態方程式に当てはめることができない。しかし、式（２）のＧ_ｎ（ｓ）に示すように、当該制御指令値における連続系の伝達関数における分子のｎ次の項を分数表記の外へ出すように変形することにより、分数表記において分母より分子の次数を少なくして、Ａ〜Ｃ行列をその他の制御指令値と同様に適用することができる。さらに、ｎ次の項については別途Ｄ行列として加えることにより状態方程式として表現可能である。これにより、１組の行列の組み合せで、ｎ＋１個の制御指令値を生成する状態方程式とすることができる。

本発明に係る制御指令値生成方法は、所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧０）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタを前記入力指令値に適用することにより、前記ｎ＋１個の制御指令値を生成するものである。

また、本発明に係る制御指令値生成装置は、所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧０）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタを備えている。

本発明は以上に説明したように構成され、一の入力指令値から複数の制御指令値を高精度かつ安定的に生成するとともにその際の演算負荷を低減することができるという効果を奏する。

本発明の一実施形態に係る制御指令値生成装置が適用されたロボット制御システムの概略構成を示すブロック図である。 本発明の一実施形態に係る制御指令値生成装置において演算されるシステムを示すブロック線図である。 本実施例の制御指令値生成プログラムに基づく３次のデジタルフィルタに、入力指令値としてステップ入力を入力した場合の各制御指令値の応答結果を比較例とともに示すグラフである。 図３に示す加速度指令値および加加速度指令値を部分的に拡大したグラフである。

以下、本発明の実施の形態を、図面を参照しながら説明する。なお、以下では全ての図を通じて同一または相当する要素には同一の参照符号を付して、その重複する説明を省略する。

まず、本発明の一実施形態に係る制御指令値生成装置が適用されたロボット制御システムの概略構成について説明する。図１は、本発明の一実施形態に係る制御指令値生成装置が適用されたロボット制御システムの概略構成を示すブロック図である。

図１に示されるように、本実施形態のロボット制御システムは、多軸のロボットの各軸に設けられるサーボモータを制御してロボットの位置制御を行うロボット制御装置２を有している。また、ロボット制御システムは、このロボット制御装置２への制御指令値を生成する制御指令値生成装置１を備えている。なお、制御指令値生成装置１とロボット制御装置２とは、シリアル接続されていてもよいし、有線または無線による通信ネットワークにより接続されていてもよい。また、ロボット制御装置２の内部に（ロボット制御装置２と一体化して）制御指令値生成装置１が設けられてもよい。また、ロボット制御装置２と制御指令値生成装置１とは完全に分離していてもよい。すなわち、制御指令値生成装置１で生成された制御指令値をロボット制御装置２に手入力などで入力することとしてもよい。

制御指令値生成装置１は、一般的なコンピュータで構成され、入力指令値が入力される入力部１１と、入力部１１によって入力された入力指令値に基づいて制御指令値を生成する演算部１２と、演算部１２の演算に用いられる各種データおよび制御指令値生成プログラムを記憶する記憶部１３とを備えている。制御指令値生成装置１の各構成は、バス１４を介して電気的に接続され、演算部１２で演算された制御指令値をバス１４を介してロボット制御装置１へ出力するよう構成されている。

記憶部１３に記憶される制御指令値生成プログラムは、制御指令値生成装置１を構成するコンピュータを、所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧０）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタとして機能させるようプログラミングされている。

次に、本実施形態におけるｎ次のデジタルフィルタを構成するための概要について説明する。

初めに、１つの入力ｕが入力され、１つの出力ｙが出力される線形システムにおける状態方程式は、以下のように表せる。

上記式（４）をｘの初期条件を０として、ラプラス変換すると、以下のようになる。

ｓＸ（ｓ）＝ＡＸ（ｓ）＋ＢＵ（ｓ）…（５）
Ｙ（ｓ）＝ＣＸ（ｓ）…（６）
ここで、ｓはラプラス変数を示す。

上記式（５）より、Ｘ（ｓ）＝（ｓＩ−Ａ）^−１ＢＵ（ｓ）となる（Ｉは単位行列を示す）ため、これを上記式（６）へ代入すると、Ｙ（ｓ）＝Ｃ（ｓＩ−Ａ）^−１ＢＵ（ｓ）となる。本線形システムにおける伝達関数Ｇ（ｓ）は入力のラプラス変換に対する出力のラプラス変換の比で表される。すなわち、伝達関数は、Ｇ（ｓ）＝Ｙ（ｓ）／Ｕ（ｓ）＝Ｃ（ｓＩ−Ａ）^−１Ｂで表される。

ここで、上記状態方程式の各行列を以下のように表したシステムを考える。

このとき、伝達関数Ｇ（ｓ）は、以下のように表せる。

本発明の発明者らは、このような状態方程式と伝達関数Ｇ（ｓ）との間で変換が可能であることに着目して、鋭意研究の末、本発明を発明したものである。

具体例を用いて説明する。本実施形態においては、入力指令値ｕが、制御対象であるロボットの位置指令値であり、デジタルフィルタは、位置指令値から当該位置指令値の３階までの微分（ｎ＝３）で表せる、位置指令値ｐ（０階微分）、速度指令値ｖ（１階微分）、加速度指令値ａ（２階微分）および加加速度指令値ｊ（３階微分）の４つの制御指令値ｙを生成する３次のデジタルフィルタである場合を例示する。

３次のデジタルフィルタの時定数をＴとすると、入力指令値である位置指令値から制御指令値である位置指令値ｐ、速度指令値ｖ、加速度指令値ａおよび加加速度指令値ｊを得るための各伝達関数Ｇ_ｐ（Ｓ），Ｇ_ｖ（Ｓ），Ｇ_ａ（Ｓ），Ｇ_ｊ（Ｓ）は、それぞれ以下のようになる。

上記式（９）に示されるように、各制御指令値ｙは、入力指令値ｕである位置指令値から当該位置指令値の３階までの微分で表せるため、各制御指令値ｙを得るための各伝達関数は、分子における微分演算子（ラプラス変数）ｓの次数が異なるものとなる。ここで、位置指令値から２階までの微分で表せる位置指令値ｐ（０階微分）、速度指令値ｖ（１階微分）および加速度指令値ａ（２階微分）については、分子におけるｓの次数が分母におけるｓの次数より小さいため、そのまま、上記式（８）の伝達関数Ｇ（ｓ）の形に変換できる。また、加加速度指令値ｊ（３階微分）については、分子におけるｓの次数と分母におけるｓの次数とが同じになるため、そのままでは、上記式（８）を適用できない。このため、分子におけるｓの最大次数（３次）の項を分数から取り出すことにより、ｓのある項とない項とに分離した形に変形した上で、ｓのある項については上記式（８）の伝達関数Ｇ（ｓ）の形に変換する。以上より、上記式（９）は以下のように変形できる。

上記式（１０）に示されるように、伝達関数Ｇ_ｐ（ｓ），Ｇ_ｖ（ｓ），Ｇ_ａ（ｓ）の分母を加加速度指令値ｊを得るための伝達関数Ｇ_ｊ（ｓ）の第２項（ｓのある項）の分母と共通となるように変形することができる。これにより、上記式（１０）に示される各伝達関数を上記式（４），（５）で示される状態方程式にそれぞれ変換した場合に、Ａ行列を共通化することができる。すなわち、上記式（５）で示されるＡ行列（ｎ＝３）における各係数ａ_０，ａ_１，ａ_２は、いずれの伝達関数に対してもａ_０＝１／Ｔ^３，ａ_１＝３／Ｔ^２，ａ_２＝３／Ｔとなる。また、Ｂ行列には伝達関数による変数は存在しないのでＢ行列も共通化できる。

Ｃ行列については、伝達関数Ｇ_ｊ（ｓ）の第２項の分子におけるｓの各次数の係数およびその他の伝達関数の分子におけるｓの各次数の係数に基づいて伝達関数ごとに１×３行列で表せる。このような各伝達関数ごとのＣ行列を同じ行列において複数列（４列）に配置して４×３行列を構成することにより、１つのＣ行列として共通化できる。この際、出力ｙは４出力となる。

さらに、伝達関数Ｇ_ｊ（ｓ）のｓのない項については、別途、状態方程式の第２式に定数項を加えるためのＤ行列を設けることで対応する。Ｄ行列は、出力ｙが４出力（ｐ，ｖ，ａ，ｊ）であることに合わせて１×４行列とする。

以上をまとめると、時定数Ｔの３次フィルタ（ｎ＝３）を用いて入力指令値ｕである位置指令値から制御指令値ｙである位置指令値ｐ、速度指令値ｖ、加速度指令値ａおよび加加速度指令値ｊを得るための状態方程式は、以下のように表せる。

図２は、本発明の一実施形態に係る制御指令値生成装置において演算されるシステムを示すブロック線図である。図２に示すように、上記式（１１）に示される状態方程式を用いて、１入力（実際には３入力あるが１入力のみを使用する）に対して４出力が得られる３次のデジタルフィルタを構成することができる。

これをｎ次のデジタルフィルタに拡張することも可能である。入力指令値ｕの伝達関数Ｇ_ｉ（ｓ）を以下の式（１２）で表したとき、入力指令値ｕを１〜ｎ階微分した制御指令値ｙを求めるための伝達関数Ｇ_１（ｓ）〜Ｇ_ｎ（ｓ）は、以下の式（１３）で示され、ｎ次のデジタルフィルタの状態方程式は、入力ｕに対する出力をｙとして以下の式（１４）で示される。

生成される制御指令値が入力指令値のｎ階微分であることより、各制御指令値の連続系の伝達関数が互いに共通の分母を有するため、状態方程式におけるＡ行列をすべての制御指令値に対して共通化することができる。また、各制御指令値の連続系の伝達関数の分子に対応するＣ行列は各制御指令値の連続系の分子を示す１×ｎ行列をｎ＋１個並べて（ｎ＋１）×ｎ行列とすることにより、１つの状態方程式で表すことができる。ここで、入力指令値のｎ階微分で表される制御指令値における連続系の伝達関数は、分母と分子との次数が同じになるため、そのままでは状態方程式に当てはめることができない。しかし、式（１３）のＧ_ｎ（ｓ）に示すように、当該制御指令値における連続系の伝達関数における分子のｎ次の項を分数表記の外へ出すように変形することにより、分数表記において分母より分子の次数を少なくして、Ａ〜Ｃ行列をその他の制御指令値と同様に適用することができる。さらに、ｎ次の項については別途Ｄ行列として加えることにより状態方程式として表現可能である。これにより、１組の行列の組み合せで、ｎ＋１個の制御指令値を生成する状態方程式とすることができる。

上記プログラムが実行される制御指令値生成装置１によれば、生成される制御指令値の連続系の伝達関数に対応する離散的な状態方程式を用いることにより、連続系の伝達関数を誤差なく離散化できるため、制御指令値を高精度かつ安定的に生成することができる。さらに、生成される制御指令値が入力指令値のｎ階までの微分であることより、各制御指令値の連続系の伝達関数が互いに共通の分母を有する。このため、連続系の伝達関数から状態方程式への当てはめを行う際に、全ての制御指令値が１組の状態方程式で表せる。制御指令値生成装置１は、公知の好適なプログラムを用いて１組の状態方程式を一括して演算し、複数の制御指令値を一度に演算することが可能である。したがって、上記制御指令値生成装置１によれば、一の入力指令値から複数の制御指令値を高精度かつ安定的に生成するとともにその際の演算負荷を低減することができる。

以上、本発明の実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、その趣旨を逸脱しない範囲内で種々の改良、変更、修正が可能である。

例えば、上記実施形態においては、１つの入力指令値に対して０〜ｎ階微分で表せるすべての制御指令値を求めることとして説明したが、本発明はこれに限られない。例えばｎ＝３の場合において、入力指令値である位置指令値から制御指令値として速度指令値（１階微分）と加加速度指令値（３階微分）のみを得ることとしてもよい。また、例えばｎ＝３の場合において、入力指令値である位置指令値から制御指令値として位置指令値、速度指令値および加速度指令値を得ることとしてもよい。これらの場合には、Ｃ行列において対応する行の値をすべて０にすると演算負荷をさらに低減することができる。例えば、入力指令値の０階微分で表される制御指令値および入力指令値の２階微分で表される制御指令値について出力しない場合には上記式（１４）において０階微分に対応する行にある係数Ｃ_０と２階微分に対応する行にある係数Ｃ_２とをそれぞれ０にすることで演算負荷が低減する。

また、入力指令値は位置指令値に限られず、例えば、速度指令値、加速度指令値、加加速度指令値など種々の指令値を適用可能である。

上記実施形態の制御指令値生成プログラムに基づく３次のデジタルフィルタを構築し、入力指令値である位置指令値としてステップ入力が入力されたときに出力される位置指令値、速度指令値、加速度指令値、加加速度指令値の応答を解析した。比較例として同じ次数（３次）のＩＩＲフィルタを用いて、同様の解析を行い、実施例と比較した。

図３は本実施例の制御指令値生成プログラムに基づく３次のデジタルフィルタに、入力指令値としてステップ入力を入力した場合の各制御指令値の応答結果を比較例とともに示すグラフである。図３（ａ）はステップ入力および位置制御指令値を示すグラフであり、図３（ｂ）は速度指令値を示すグラフであり、図３（ｃ）は加速度指令値を示すグラフであり、図３（ｄ）は加加速度指令値を示すグラフである。また、図４は図３に示す加速度指令値および加加速度指令値を部分的に拡大したグラフである。図４（ａ）は加速度指令値を示すグラフであり、図４（ｂ）は加加速度指令値を示すグラフである。

図３（ａ）および図３（ｂ）に示すように、ステップ入力に対して次数の小さい位置制御指令値ｐ（０階微分）および速度指令値ｖ（１階微分）については実施例および比較例のいずれも振動成分の少ないグラフが得られている。しかし、ステップ入力に対して次数の比較的大きい加速度制御指令値ａ（２階微分）および加加速度指令値ｊ（３階微分）については、特に図４（ａ）および図４（ｂ）に示すように、比較例において振動成分が重畳していることが分かる。

これに対し、本実施例の３次フィルタによって演算された加速度指令値ａおよび加加速度指令値ｊは、振動成分がほとんどなく非常に安定した波形が得られた。このように、本発明に基づく制御指令値生成プログラムおよびこれを備えた制御指令値生成装置において、生成される制御指令値の連続系の伝達関数に対応する離散的な状態方程式を用いることにより、入力指令値に対して次数の大きい制御指令値であっても、高精度かつ安定的に生成できることが示された。

本発明の制御指令値生成プログラム、制御指令値生成方法および制御指令値生成装置は、一の入力指令値から複数の制御指令値を高精度かつ安定的に生成するとともにその際の演算負荷を低減するために有用である。

１ 制御指令値生成装置（コンピュータ）
２ ロボット制御装置
１１ 入力部
１２ 演算部
１３ 記憶部
１４ バス

Claims (5)

1. コンピュータを、
   所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧１）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタとして機能させ、
   前記ｎ＋１個の制御指令値を求めるために、前記入力指令値を１〜ｎ階微分した制御指令値を求めるための伝達関数のそれぞれは、分数表記を含む関数として表され、
   前記の伝達関数のうち、分子にｎ次の項がある伝達関数が、当該ｎ次の項を分数表記の外へ出すように変形されることにより、当該変形後の前記伝達関数の各分数表記において、共通の分母を有し、かつ、前記分母より分子の次数が少なくなっている、制御指令値生成プログラム。
2. 前記所定の入力指令値は、制御対象の位置指令値であり、
   前記デジタルフィルタは、前記位置指令値から当該位置指令値の３階までの微分で表せる、位置指令値、速度指令値、加速度指令値および加加速度指令値の４つの制御指令値を生成する３次のデジタルフィルタである、請求項１に記載の制御指令値生成プログラム。
3. 前記入力指令値の伝達関数Ｇ_ｉ（ｓ）を以下の式（１）で表したとき、前記入力指令値を１〜ｎ階微分した制御指令値を求めるための伝達関数Ｇ_１（ｓ）〜Ｇ_ｎ（ｓ）は、以下の式（２）で示され、前記ｎ次のデジタルフィルタの状態方程式は、入力ｕに対する出力をｙとして以下の式（３）で示される、請求項１に記載の制御指令値生成プログラム。
   

   

   

   
4. 所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧１）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタを前記入力指令値に適用することにより、前記ｎ＋１個の制御指令値を生成する工程を含み、
   前記ｎ＋１個の制御指令値を求めるために、前記入力指令値を１〜ｎ階微分した制御指令値を求めるための伝達関数のそれぞれは、分数表記を含む関数として表され、
   前記伝達関数のうち分子にｎ次の項がある伝達関数が、当該ｎ次の項を分数表記の外へ出すように変形されることにより、当該変形後の前記伝達関数の各分数表記において、共通の分母を有し、かつ、前記分母より分子の次数が少なくなっている、制御指令値生成方法。
5. 所定の入力指令値から当該入力指令値のｎ階までの微分（ｎ≧１）で表せるｎ＋１個の制御指令値を、入力数が１であり出力数がｎ＋１である状態方程式を用いて生成するｎ次のデジタルフィルタを備え、
   前記ｎ＋１個の制御指令値を求めるために、前記入力指令値を１〜ｎ階微分した制御指令値を求めるための伝達関数のそれぞれは、分数表記を含む関数として表され、
   前記伝達関数のうち分子にｎ次の項がある伝達関数が、当該ｎ次の項を分数表記の外へ出すように変形されることにより、当該変形後の前記伝達関数の各分数表記において、共通の分母を有し、かつ、前記分母より分子の次数が少なくなっている、制御指令値生成装置。

Images