(57)【要約】【課題】フィードフォワード制御が導入された制御装置の位置偏差異常をいち早く検出する。【解決手段】位置偏差推定処理部４０において、例えば式Ｖ／ＰＧ×（１−α）（ただし、Ｖ：位置指令の１階微分値、ＰＧ：位置制御器１６におけるポジションゲイン、α：フィードフォワード項算出部３２におけるフィードフォワード係数）により位置偏差推定値Ｅｒｒを算出し、それにマージンΔＥを加算した誤差過大検出レベルにより位置偏差の異常を検出する。【選択図】図５
2008269349.tif 000002
【特許請求の範囲】
【請求項１】
位置指令と制御対象からの位置フィードバックとの間の位置偏差にポジションゲインを乗じて第１の速度指令を算出する位置制御器と、
位置指令の時間微分にフィードフォワード係数を乗じてフィードフォワード項を算出するフィードフォワード項算出部と、
第１の速度指令にフィードフォワード項を加算して得られる第２の速度指令を速度指令として制御対象の速度を制御する速度制御器と、
位置指令、ポジションゲイン、およびフィードフォワード係数から位置偏差推定値を算出する位置偏差推定値算出部と、
位置偏差が位置偏差推定値よりも所定値以上大きいとき、異常と判定する位置偏差異常判定部とを具備する制御装置。
【請求項２】
位置偏差推定値算出部は、速度制御器と制御対象を積分器で置き換えたときの位置偏差を位置偏差推定値として算出する請求項１記載の制御装置。
【請求項３】
位置偏差異常判定部はさらに、位置偏差が位置偏差推定値よりも所定値以上小さいとき、異常と判定する請求項１または２記載の制御装置。
【請求項４】
フィードフォワード係数をα、位置ゲインをＰＧ、位置指令の１階微分値をＶとするとき、位置偏差推定値算出部は、位置偏差推定値Ｅｒｒ（Ｖ，ＰＧ，α）を、式
Ｅｒｒ（Ｖ，ＰＧ，α）＝Ｖ／ＰＧ×（１−α）
により算出する請求項１〜３のいずれか１項記載の制御装置。
【請求項５】
フィードフォワード係数をα、位置ゲインをＰＧ、前回の位置指令をｕ（ｎ−１）、今回の位置指令をｕ（ｎ）、前回の位置偏差推定値をＥｒｒ（ｎ−１）とするとき、位置偏差推定値算出部は、今回の位置偏差推定値Ｅｒｒ（ｎ）を、漸化式
Ｅｒｒ（ｎ）＝｛Ｅｒｒ（ｎ−１）＋（１−α）（ｕ（ｎ）−ｕ（ｎ−１））｝／（１＋ＰＧ・Ｔ）
により算出する請求項１〜３のいずれか１項記載の制御装置。
【請求項６】
制御対象はサーボモータであり、位置はサーボモータの回転角度であり、速度はサーボモータの回転角速度である請求項１〜５のいずれか１項記載の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置偏差の異常を検出する誤差過大検出機能を有する制御装置に関する。
【背景技術】
【０００２】
例えばサーボモータの回転を制御する数値制御（ＮＣ）装置においては、モータの回転角度指令（より一般的には位置指令）と実際の回転角度（より一般的には位置フィードバック）との間の角度偏差（より一般的には位置偏差）を予め定められた誤差過大検出レベルと比較することによって、サーボ系の異常、過大な負荷、周辺機器との干渉などを原因とする異常が検出される。従来では、この誤差過大検出レベルとしては、速度指令と位置制御ループのゲイン（ポジションゲイン）から計算される理想的な位置偏差（定常偏差）に一定のマージンを加えたものを使用するのが一般的であった。
【０００３】
しかしながら、位置制御にフィードフォワード制御を導入した場合、正常動作時の位置偏差は速度指令とポジションゲインから計算される上記の理想的な位置偏差よりも小さくなる。この理想的な位置偏差に基いて設定された誤差過大検出レベルを異常検出の閾値として使用すると、異常状態の初期では偏差が検出レベルに達しないので、異常状態の検出に遅れを生じるという問題がある。
【特許文献１】特許第２６７３５４号
【発明の開示】
【発明が解決しようとする課題】
【０００４】
したがって本発明の目的は、位置制御にフィードフォワード制御が導入された制御装置において、位置偏差の異常を適切に検出することの可能な誤差過大検出レベルを使用した制御装置を提供することにある。
【課題を解決するための手段】
【０００５】
位置指令と制御対象からの位置フィードバックとの間の位置偏差にポジションゲインを乗じて第１の速度指令を算出する位置制御器と、位置指令の時間微分にフィードフォワード係数を乗じてフィードフォワード項を算出するフィードフォワード項算出部と、第１の速度指令にフィードフォワード項を加算して得られる第２の速度指令を速度指令として制御対象の速度を制御する速度制御器と、位置指令、ポジションゲイン、およびフィードフォワード係数から位置偏差推定値を算出する位置偏差推定値算出部と、位置偏差が位置偏差推定値よりも所定値以上大きいとき、異常と判定する位置偏差異常判定部とを具備する制御装置が提供される。
位置偏差推定値算出部は、例えば、速度制御器と制御対象を積分器で置き換えたときの位置偏差を位置偏差推定値として算出する。
【発明の効果】
【０００６】
速度制御器と制御対象を積分器で置き換えたとき、すなわち、速度制御に遅れがないときに計算される位置偏差を位置偏差推定値とし、これに基づいて誤差過大検出レベルが設定されるので、フィードフォワード制御が導入された制御装置の誤差過大検出レベルを適切に設定することができる。
【発明を実施するための最良の形態】
【０００７】
サーボモータの回転を制御する制御装置を例にとって本発明を説明する、図１は、フィードフォワード制御が導入されていない制御装置の構成を示す。減算器１０において、位置指令と、サーボモータ１２からの位置フィードバック（エンコーダ１４において検出される回転角度）との間の位置偏差が算出される。位置制御器１６において位置偏差にポジションゲインＰＧが乗じられ、これがサーボモータの回転速度を制御する速度制御器１８に速度指令として与えられる。
【０００８】
速度制御器１８は、例えば図２に示すように、減算器２２において、速度指令と、角速度検出部２０において検出される角速度との間の速度偏差を算出し、速度制御処理部２４において、速度偏差から電流指令を算出し、減算器２８において、電流指令と、電流検出部２６において検出される電流との間の電流偏差を検出し、電流制御処理部３０において電流偏差に基いてサーボモータ１２に洗れる電流を制御する構成を有している。
【０００９】
図３は、図１の制御装置にフィードフォワード制御を導入した例を示す。フィードフォワード項算出部３２は位置指令を時間微分してフィードフォワード係数を乗じてフィードフォワード項を算出する。加算器３４は位置制御器１６が出力する速度指令にこのフィードフォワード項を加算して速度制御器１８へ速度指令として与える。
【００１０】
本発明では、図４に示すように図３の速度制御器１８とサーボモータ１２を理想的な積分器３６で置換した時、すなわち、速度制御器１８とサーボモータ１２に遅れがないときの偏差を算出してこれを位置偏差推定値とし、これに基いて決定された誤差過大検出レベルを用いて位置偏差の異常を検出する。以下、この位置偏差推定値の算出について説明する。
【００１１】
位置指令をｕ（ｔ）、位置偏差をＥｒｒ（ｔ）とし、これらのラプラス変換をそれぞれｕ（ｓ）、Ｅｒｒ（ｓ）とする。位置指令から位置偏差までの伝達関数をＦ（ｓ）とすると、
Ｅｒｒ（ｓ）＝Ｆ（ｓ）・ｕ（ｓ）
となり、伝達関数Ｆ（ｓ）が与えられれば、位置指令ｕ（ｔ）のラプラス変換ｕ（ｓ）より、位置偏差Ｅｒｒ（ｔ）を求めることが可能である。
【００１２】
図４に示す系では、位置指令から位置偏差までの伝達関数Ｆ（ｓ）は
Ｆ（ｓ）＝（１−α）・ｓ／（ｓ＋ＰＧ）
となるので、この伝達関数と位置指令ｕ（ｔ）のラプラス変換ｕ（ｓ）を用いて、図４の系における位置偏差、すなわち図３の系における位置偏差推定値を算出することができる。ただし、現実には時系列で与えられるｕ（ｔ）のラプラス変換を事前に取得する事は困難であり、近似的な関数を使用する事になる。
【００１３】
位置指令の時間微分がＶで一定、すなわち一定速度Ｖでの回転が指令されているときは位置指令のラプラス変換ｕ（ｓ）はＶ／ｓ2であるから、位置偏差推定値Ｅｒｒはｓ→０の極限において
Ｅｒｒ（Ｖ，ＰＧ，α）＝Ｖ／ＰＧ×（１−α）
となる。この式は位置指令の変化が緩やかな場合には十分利用可能で、多少の誤差は出るものの、位置制御の異常を検出するための値として利用する事は可能である。
また、指令の加速度や加速度変化が大きい所では誤差を生じやすいので、次式のように補正する事が出来る。
Ｅｒｒ（Ｖ，ＰＧ，α）＝Ｖ／ＰＧ×（１−α）
＋（位置指令の２階微分値）×Ｋａ＋（位置指令の３階微分値）×Ｋｊ
ディジタル処理によるサンプル値制御の場合、位置偏差推定値を漸化式で計算することができる。
【００１４】
位置指令ｕ（ｓ）から位置偏差Ｅｒｒ（ｓ）までの伝達関数は
Ｅｒｒ（ｓ）／ｕ（ｓ）＝（１−α）・ｓ／（ｓ＋ＰＧ）
として与えられるが、これを変形して、
（ｓ＋ＰＧ）・Ｅｒｒ（ｓ）＝（１−α）・ｓ・ｕ（ｓ）
とする。ここで、Ｅｒｒ（ｓ）・ｓ→（Ｅｒｒ（ｎ）−Ｅｒｒ（ｎ−１）／Ｔ，ｕ（ｓ）・ｓ→（ｕ（ｎ）−ｕ（ｎ−１））／Ｔ，Ｅｒｒ（ｓ）→Ｅｒｒ（ｎ））と離散値系に変換することにより、次式が得られる。
【００１５】
Ｅｒｒ（ｎ）＝｛Ｅｒｒ（ｎ−１）＋（１−α）（ｕ（ｎ）−ｕ（ｎ−１））｝／（１＋ＰＧ・Ｔ）
この漸化式を計算する事により位置偏差推定値を算出することができる。
【００１６】
図５は本発明の一実施形態に係る制御装置の構成を示す。図３と同一の構成要素については、同一の参照番号を付してその説明を省略する。
【００１７】
位置偏差推定処理部４０は、位置指令、フィードフォワード項算出部３２において用いられるフィードフォワード係数α、および位置制御器１６におけるポジションゲインＰＧから、前述した手法のいずれかにより、位置偏差推定値Ｅｒｒを算出する。なお、位置偏差推定処理部４０は位置偏差推定値Ｅｒｒの計算を繰り返し行っているので、フィードフォワード係数αの設定値が変われば、位置偏差推定値もそれに応じて変わる。誤差過大検出レベル算出部４２は、位置偏差推定値Ｅｒｒの絶対値（Ｅｒｒ）に所定のマージンΔＥを加算して誤差過大検出レベルＥｒｒＬｅｖｅｌを算出する。一方、絶対値計算部４４において、減算器１０が出力する位置偏差ＰｏｓＥｒｒの絶対値｜ＰｏｓＥｒｒ｜が算出され、減算器４６において誤差過大検出レベルＥｒｒＬｅｖｅｌが差し引かれる。誤差過大検出部４８はこの減算結果が負であるとき異常と判定する。なおこのマージンΔＥは、速度制御器１８およびサーボモータ１２の応答の遅れにより生じるオーバーシュートなどの実測値を考慮して設定することができる。指令加速度が高くなる事で位置偏差が増加する可能性があり、その対策として、あらかじめ直線加減速時の位置偏差量を調査し、異常を検出する際のマージンとして考慮する事が考えられる。
【００１８】
また、指令加速度が変化する部分において制御が追従出来ずに位置偏差が増加する傾向にあり、この点も同様に指令加速度の変化率と位置偏差量との関係を調査し、異常を検出する際のマージンとして考慮する事が考えられる。
【００１９】
図５に示す実施形態においては、図６に示すように、位置偏差｜ＰｏｓＥｒｒ｜が位置偏差推定値ＥｒｒよりもΔＥ以上大きい、ハッチングを付した領域において異常と判定される。
【００２０】
図７は本発明の第２の実施形態に係る制御装置の構成を示す。本実施形態においては、減算器５０において位置偏差ＰｏｓＥｒｒから位置偏差推定値Ｅｒｒが減算され、絶対値計算部５２においてその絶対値が計算される。誤差過大検出部５４ではこれとマージンΔＥが比較され、ΔＥよりも大であるとき異常と判定される。
【００２１】
図７に示す実施形態においては、図８に示すように、位置偏差ＰｏｓＥｒｒが位置偏差推定値ＥｒｒよりもΔＥ以上大きいときだけでなく、ΔＥ以上小さいときも異常と判定される。位置偏差ＰｏｓＥｒｒが正常な状態よりもΔＥ以上大きいときだけでなくΔＥ以上小さいときも何らかの異常があると考えられる。
【００２２】
図９は、図５を参照して説明した制御をソフトウェアで実現する場合の処理のフローチャートである。図９において、位置偏差推定値Ｅｒｒが算出され（ステップ１０００）、それから誤差過大レベルＥｒｒＬｅｖｅｌが算出される（ステップ１００２）。位置偏差の絶対値｜ＰｏｓＬｅｖｅ｜がこのＥｒｒＬｅｖｅｌよりも大であるとき（ステップ１００４）誤差過大として検出され（ステップ１００６）、そうでなければ正常と判定される（ステップ１００８）。
【００２３】
図１０は、図７を参照して説明した制御をソフトウェアで実現する場合の処理のフローチャートである。図１０において、位置偏差推定値Ｅｒｒが算出され（ステップ１１００）、これと位置偏差との差の絶対値｜ＰｏｓＥｒｒ−Ｅｒｒ｜が算出される（ステップ１１０２）。ステップ１１０４においてこれとマージンΔＥとが比較され、ΔＥよりも大であるとき誤差過大として検出され（ステップ１１０６）、そうでなければ正常と判定される（ステップ１１０８）。
【図面の簡単な説明】
【００２４】
【図１】フィードフォワード制御が導入されていない制御装置の構成の一例を示すブロック図である。
【図２】図１の速度制御器１８の構成の一例を示すブロック図である。
【図３】フィードフォワード制御が導入された制御装置の構成の一例を示すブロック図である。
【図４】位置偏差推定値の算出のために図３の速度制御器１８とサーボモータ１２を積分器で置き換えたブロック図である。
【図５】本発明の第１の実施形態に係る制御装置の構成を示すブロック図である。
【図６】図５の制御装置の異常判定動作を説明するグラフである。
【図７】本発明の第２の実施形態に係る制御装置の構成を示すブロック図である。
【図８】図７の制御装置の異常判定動作を説明するグラフである。
【図９】図５を参照して説明した制御をソフトウェアで実現する場合の制御のフローチャ−トである。
【図１０】図７を参照して説明した制御をソフトウェアで実現する場合の制御のフローチャートである。
【図１】
2008269349.tif 000003
【図２】
2008269349.tif 000004
【図３】
2008269349.tif 000005
【図４】
2008269349.tif 000006
【図５】
2008269349.tif 000007
【図６】
2008269349.tif 000008
【図７】
2008269349.tif 000009
【図８】
2008269349.tif 000010
【図９】
2008269349.tif 000011
【図１０】
2008269349.tif 000012