(57)【特許請求の範囲】
【請求項１】
動作対象物を動作させるサーボモータと、位置指令により前記サーボモータを制御するサーボモータ制御部とを備えるサーボモータ制御装置であって、
前記サーボモータ制御部は、
前記位置指令に対応した速度次元信号を出力する位置指令速度次元信号出力部と、
前記サーボモータの速度次元信号を出力するサーボモータ速度次元信号出力部と、
前記位置指令速度次元信号出力部により出力された速度次元信号と、前記サーボモータ速度次元信号出力部により出力された速度次元信号との差分値である速度偏差の絶対値が特定値以上になった場合、衝突として検出する第一衝突検出部と、
前記位置指令速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力する位置指令加速度次元変換部と、
前記サーボモータ速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力するサーボモータ加速度次元変換部と、
前記位置指令加速度次元変換部により出力された加速度次元信号と、前記サーボモータ加速度次元変換部により出力された加速度次元信号との差分値である加速度偏差の絶対値が特定値以上になった場合、衝突として検出する第二衝突検出部と、
前記第一衝突検出部と前記第二衝突検出部とを、特定のパラメータにより選択する選択部とを備え、
前記サーボモータ速度次元信号出力部は、
前記サーボモータの位置を検出する位置検出センサにより検出された位置信号が微分された信号を、速度次元信号として出力し、
前記第一衝突検出部は、
前記速度偏差の代わりに、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、該疑似速度偏差の絶対値が特定値以上になった場合も、衝突として検出する
ことを特徴とするサーボモータ制御装置。
【請求項２】
動作対象物を動作させるサーボモータと、速度指令により前記サーボモータを制御するサーボモータ制御部とを備えるサーボモータ制御装置であって、
前記サーボモータ制御部は、
前記速度指令に対応した速度次元信号を出力する速度指令速度次元信号出力部と、
前記サーボモータの速度次元信号を出力するサーボモータ速度次元信号出力部と、
前記速度指令速度次元信号出力部により出力された速度次元信号と、前記サーボモータ速度次元信号出力部により出力された速度次元信号との差分値である速度偏差の絶対値が特定値以上になった場合、衝突として検出する第一衝突検出部と、
前記速度指令速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力する速度指令加速度次元変換部と、
前記サーボモータ速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力するサーボモータ加速度次元変換部と、
前記速度指令加速度次元変換部により出力された加速度次元信号と、前記サーボモータ加速度次元変換部により出力された加速度次元信号との差分値である加速度偏差の絶対値が特定値以上になった場合、衝突として検出する第二衝突検出部と、
前記第一衝突検出部と前記第二衝突検出部とを、特定のパラメータにより選択する選択部とを備え、
前記サーボモータ速度次元信号出力部は、
前記サーボモータの位置を検出する位置検出センサにより検出された位置信号が微分された信号を、速度次元信号として出力し、
前記第一衝突検出部は、
前記速度偏差の代わりに、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、該疑似速度偏差の絶対値が特定値以上になった場合も、衝突として検出する
ことを特徴とするサーボモータ制御装置。
【請求項３】
前記サーボモータ制御部は、
前記サーボモータ加速度次元変換部により出力された加速度次元信号の絶対値が特定値以上になった場合、衝突として検出する第三衝突検出部を更に備え、
前記選択部は、前記特定のパラメータにより、前記第三衝突検出部も選択する
ことを特徴とする請求項１又は２に記載のサーボモータ制御装置。
【請求項４】
前記サーボモータ速度次元信号出力部は、
モデルの制御対象への入力信号と前記制御対象の出力信号とにより速度を推定する速度オブザーバで算出された速度推定信号を、速度次元信号として出力する
ことを特徴とする請求項１乃至３のいずれか１項に記載のサーボモータ制御装置。
【請求項５】
前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分フィルタにより微分された信号との偏差を算出するフィードバックループを備え、
前記サーボモータ速度次元信号出力部は、前記フィードバックループ内に配した前記微分フィルタの出力信号を、速度次元信号として出力する
ことを特徴とする請求項１乃至３のいずれか１項に記載のサーボモータ制御装置。
【請求項６】
前記第二衝突検出部は、
前記加速度偏差の代わりに、前記疑似速度偏差を微分した信号を疑似加速度偏差として算出し、該疑似加速度偏差の絶対値が特定値以上になった場合、衝突として検出する
ことを特徴とする請求項１乃至５のいずれか１項に記載のサーボモータ制御装置。
【請求項７】
前記選択部は、前記第一衝突検出部、前記第二衝突検出部、及び前記第三衝突検出部のいずれを選択するか、外部から設定された前記パラメータにより選択する
ことを特徴とする請求項３乃至６のいずれか１項に記載のサーボモータ制御装置。
【請求項８】
前記選択部は、前記第一衝突検出部の特定値、前記第二衝突検出部の特定値、前記第三衝突検出部の特定値は、外部から設定される
ことを特徴とする請求項１乃至３のいずれか１項に記載のサーボモータ制御装置。
【請求項９】
前記速度次元信号は、制御系モデルとして１／（τｓ＋１）を伝達関数とする要素を含むフィルタの信号である
ことを特徴とする請求項１乃至８のいずれか１項に記載のサーボモータ制御装置。
【請求項１０】
前記フィルタは、カットオフ周波数が外部から設定される
ことを特徴とする請求項９に記載のサーボモータ制御装置。
【請求項１１】
動作対象物を動作させるサーボモータと、位置指令により前記サーボモータを制御するサーボモータ制御部とを備えるサーボモータ制御装置により実行される衝突検出方法であって、
前記位置指令に対応した速度次元信号を出力し、
前記サーボモータの速度次元信号を出力し、
出力される速度次元信号は、前記サーボモータの位置を検出する位置検出センサにより検出された位置信号が微分された信号であり、
出力された前記位置指令に対応した速度次元信号と、出力された前記サーボモータの速度次元信号との差分値である速度偏差を算出し、
前記位置指令に対応した加速度次元信号を出力し、
前記サーボモータの加速度次元信号を出力し、
出力された前記位置指令に対応した加速度次元信号と、出力された前記サーボモータの加速度次元信号との差分値である加速度偏差を算出し、
特定のパラメータにより選択された前記速度偏差及び加速度偏差のいずれかの絶対値が特定値以上になった場合、衝突として検出し、
前記速度偏差の代わりに、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、該疑似速度偏差の絶対値が特定値以上になった場合も、衝突として検出する
ことを特徴とする衝突検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サーボモータ制御装置及び衝突検出方法に係り、特に動作対象物を動作させるサーボモータを備えるサーボモータ制御装置及び衝突検出方法に関する。
【背景技術】
【０００２】
従来から、サーボモータの制御装置において、ロボットアーム等の動作対象物の衝突検出を行う技術が存在する。
たとえば、特許文献１には、移動体を有するアクチュエータを制御するアクチュエータ制御装置が記載されている。特許文献１の装置は、アクチュエータの駆動中であって移動体が加速の終了後から減速の開始前まで移動する間においてアクチュエータ内のモータに流れる電流値を検出する電流検出手段と、電流検出手段により検出された電流値が所定の閾値を超える場合に、移動体の衝突を検出する衝突検出手段とを備える。すなわち、特許文献１の技術では、トルク指令値が閾値を超えたら衝突検出信号を出力している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１４−８７２３５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１の技術では、偏荷重がかかっている場合や加減速の時に衝突を検出することができないという問題があった。このため、衝突検出の検出精度が十分ではなかった。
【０００５】
本発明は、このような状況に鑑みてなされたものであり、上述の問題を解消し、衝突検出の精度を高めるサーボモータ制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明のサーボモータ制御装置は、動作対象物を動作させるサーボモータと、位置指令により前記サーボモータを制御するサーボモータ制御部とを備えるサーボモータ制御装置であって、前記サーボモータ制御部は、前記位置指令に対応した速度次元信号を出力する位置指令速度次元信号出力部と、前記サーボモータの速度次元信号を出力するサーボモータ速度次元信号出力部と、前記位置指令速度次元信号出力部により出力された速度次元信号と、前記サーボモータ速度次元信号出力部により出力された速度次元信号との差分値である速度偏差の絶対値が特定値以上になった場合、衝突として検出する第一衝突検出部と、前記位置指令速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力する位置指令加速度次元変換部と、前記サーボモータ速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力するサーボモータ加速度次元変換部と、前記位置指令加速度次元変換部により出力された加速度次元信号と、前記サーボモータ加速度次元変換部により出力された加速度次元信号との差分値である加速度偏差の絶対値が特定値以上になった場合、衝突として検出する第二衝突検出部と、前記第一衝突検出部と前記第二衝突検出部とを、特定のパラメータにより選択する選択部とを備え、前記サーボモータ速度次元信号出力部は、前記サーボモータの位置を検出する位置検出センサにより検出された位置信号が微分された信号を、速度次元信号として出力し、前記第一衝突検出部は、前記速度偏差の代わりに、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、該疑似速度偏差の絶対値が特定値以上になった場合も、衝突として検出することを特徴とする。
このように構成することで、位置制御において、アプリケーションの用途等に対応し、第一衝突検出部と第二衝突検出部とを適切に選択することができ、衝突検出の精度を高められる。
また、微分器により簡単に速度次元信号を得ることができ、構成を簡略化できる。
また、演算を簡略化して、構成を簡略化できる。
【０００７】
本発明のサーボモータ制御装置は、動作対象物を動作させるサーボモータと、速度指令により前記サーボモータを制御するサーボモータ制御部とを備えるサーボモータ制御装置であって、前記サーボモータ制御部は、前記速度指令に対応した速度次元信号を出力する速度指令速度次元信号出力部と、前記サーボモータの速度次元信号を出力するサーボモータ速度次元信号出力部と、前記速度指令速度次元信号出力部により出力された速度次元信号と、前記サーボモータ速度次元信号出力部により出力された速度次元信号との差分値である速度偏差の絶対値が特定値以上になった場合、衝突として検出する第一衝突検出部と、前記速度指令速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力する速度指令加速度次元変換部と、前記サーボモータ速度次元信号出力部により出力された速度次元信号を加速度次元信号に変換して出力するサーボモータ加速度次元変換部と、前記速度指令加速度次元変換部により出力された加速度次元信号と、前記サーボモータ加速度次元変換部により出力された加速度次元信号との差分値である加速度偏差の絶対値が特定値以上になった場合、衝突として検出する第二衝突検出部と、前記第一衝突検出部と前記第二衝突検出部とを、特定のパラメータにより選択する選択部とを備え、前記サーボモータ速度次元信号出力部は、前記サーボモータの位置を検出する位置検出センサにより検出された位置信号が微分された信号を、速度次元信号として出力し、前記第一衝突検出部は、前記速度偏差の代わりに、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、該疑似速度偏差の絶対値が特定値以上になった場合も、衝突として検出することを特徴とする。
このように構成することで、速度制御において、アプリケーションの用途等に対応し、第一衝突検出部と第二衝突検出部とを適切に選択することができ、衝突検出の精度を高められる。
【０００８】
本発明のサーボモータ制御装置は、前記サーボモータ制御部は、前記サーボモータ加速度次元変換部により出力された加速度次元信号の絶対値が特定値以上になった場合、衝突として検出する第三衝突検出部を更に備え、前記選択部は、前記特定のパラメータにより、前記第三衝突検出部も選択することを特徴とする。
このように構成することで、加減速が緩やかな状況では、加速度次元信号自体と特定値との比較をすることで、衝突検出の精度を高めることができる。
【００１０】
本発明のサーボモータ制御装置は、前記サーボモータ速度次元信号出力部は、モデルの制御対象への入力信号と前記制御対象の出力信号とにより速度を推定する速度オブザーバで算出された速度推定信号を、速度次元信号として出力することを特徴とする。
このように構成することで、オブザーバを用いる構成においても、衝突検出を行うことができる。
【００１１】
本発明のサーボモータ制御装置は、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分フィルタにより微分された信号との偏差を算出するフィードバックループを備え、前記サーボモータ速度次元信号出力部は、前記フィードバックループ内に配した前記微分フィルタの出力信号を、速度次元信号として出力することを特徴とする。
このように構成することで、演算を簡略化して、構成を簡略化できる。
【００１３】
本発明のサーボモータ制御装置は、前記第二衝突検出部は、前記加速度偏差の代わりに、前記疑似速度偏差を微分した信号を疑似加速度偏差として算出し、該疑似加速度偏差の絶対値が特定値以上になった場合、衝突として検出することを特徴とする。
このように構成することで、演算を簡略化して、構成を簡略化できる。
【００１４】
本発明のサーボモータ制御装置は、前記選択部は、前記第一衝突検出部、前記第二衝突検出部、及び前記第三衝突検出部のいずれを選択するか、外部から設定された前記パラメータにより選択することを特徴とする。
このように構成することで、外部から衝突検出に適した回路等を随時設定でき、回路変更の負荷が少なく簡単に変更することができる。
【００１５】
本発明のサーボモータ制御装置は、前記選択部は、前記第一衝突検出部の特定値、前記第二衝突検出部の特定値、前記第三衝突検出部の特定値は、外部から設定されることを特徴とする。
このように構成することで、外部から特定値を随時設定でき、変更の負荷が少なく簡単に変更することができる。
【００１６】
本発明のサーボモータ制御装置は、前記速度次元信号は、制御系モデルとして１／（τｓ＋１）を伝達関数とする要素を含むフィルタの信号であることを特徴とする。
このように構成することで、フィルタに制御モデルの要素を含ませることができ、擬似的に実際の信号に近い信号を得ることができる。
【００１７】
本発明のサーボモータ制御装置は、前記フィルタは、カットオフ周波数が外部から設定されることを特徴とする。
このように構成することで、変更負荷が少なくなり、カットオフ周波数を適宜、簡単に選択できる。
【００１８】
本発明の衝突検出方法は、動作対象物を動作させるサーボモータと、位置指令により前記サーボモータを制御するサーボモータ制御部とを備えるサーボモータ制御装置により実行される衝突検出方法であって、前記位置指令に対応した速度次元信号を出力し、前記サーボモータの速度次元信号を出力し、出力される速度次元信号は、前記サーボモータの位置を検出する位置検出センサにより検出された位置信号が微分された信号であり、出力された前記位置指令に対応した速度次元信号と、出力された前記サーボモータの速度次元信号との差分値である速度偏差を算出し、前記位置指令に対応した加速度次元信号を出力し、前記サーボモータの加速度次元信号を出力し、出力された前記位置指令に対応した加速度次元信号と、出力された前記サーボモータの加速度次元信号との差分値である加速度偏差を算出し、特定のパラメータにより選択された前記速度偏差及び加速度偏差のいずれかの絶対値が特定値以上になった場合、衝突として検出し、前記速度偏差の代わりに、前記位置指令の値に比例ゲインを掛けた信号と、前記位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、該疑似速度偏差の絶対値が特定値以上になった場合も、衝突として検出することを特徴とする。
このように構成することで、アプリケーションの用途等に対応し、衝突検出の方式を適切に選択することができ、衝突検出の精度を高められる。
【発明の効果】
【００１９】
本発明によれば、速度偏差による衝突検出と加速度偏差による衝突検出とを特定のパラメータにより選択することで、衝突検出の検出精度がよいサーボモータ制御装置を提供することができる。
【図面の簡単な説明】
【００２０】
【図１】本発明の実施の形態に係るサーボモータ制御装置のシステム構成図である。
【図２】図１に示すサーボモータ制御部が位置制御を実行する場合の制御構成を示すブロック図である。
【図３】図１に示すサーボモータ制御部が速度制御を実行する場合の制御構成を示すブロック図である。
【図４】図１又は図２に示す制御系を含む制御構成を示すブロック図である。
【発明を実施するための形態】
【００２１】
＜実施の形態＞
〔制御システムＸの構成〕
図１を参照して、本発明の実施の形態に係る制御システムＸの構成について説明する。制御システムＸは、ロボット、工作機械、車両、船舶、航空機、工場プラント等の各種機器における制御を行うためのシステムである。
【００２２】
また、本実施形態の制御システムＸは、サーボモータ制御装置１、動作対象物２、及びホスト装置３を含んでいる。
【００２３】
サーボモータ制御装置１は、位置指令又は速度指令に対応して制御量を調整しつつサーボモータ２０を制御し、動作対象物２を動作させる装置である。また、サーボモータ制御装置１は、動作対象物２の衝突検出を行う。
サーボモータ制御装置１は、この衝突検出として、位置指令による位置制御を実行する場合には、ロボットアーム等が障害物に接触した等により停止したことを検出する。また、サーボモータ制御装置１は、速度指令による速度制御を実行する場合には、工作機械の回転部材の歯欠けやベアリング故障等により停止したことを検出する。
また、サーボモータ制御装置１は、本実施形態に係る衝突検出方法を実行するハードウェア資源となる。
【００２４】
動作対象物２は、サーボモータ制御装置１による動作制御の対象となる部材である。動作対象物２は、例えば、産業用ロボットのアーム、工作機械の回転部材、車両の車輪や歯車やベルト、船舶のシャフト、航空機のプロペラ、工場プラントのアクチュエータ等である。
【００２５】
ホスト装置３は、各種機器を制御、管理等するための外部の機器である。具体的に、ホスト装置３は、例えば、ＰＬＣ（Programmable Logic Controller）、ＦＣ（Factory Computer）、サーバ（Server）、ＰＣ（Personal Computer）等である。ホスト装置３は、サーボモータ制御装置１を制御、管理等するためのアプリケーション（Application Program）を実行する。これにより、ホスト装置３は、サーボモータ制御装置１に位置指令又は速度指令を送信し、サーボモータ制御装置１から各種情報を受信する。また、ホスト装置３は、ユーザーの指示を取得して、後述するパラメータ設定部１９０（図２、図３）に各種設定値を設定することも可能である。
【００２６】
また、サーボモータ制御装置１は、サーボモータ制御部１０、サーボモータ２０、及び検出部３０を含んでいる。
【００２７】
サーボモータ制御部１０は、ホスト装置３からの位置指令又は速度指令によりサーボモータ２０を制御する。具体的に、サーボモータ制御部１０は、例えば、ＦＰＧＡ（Field Programmable Gate Array）、ＡＳＩＣ（Application Specific Integrated Circuit）、ＤＳＰ（Digital Signal Processor）、ＣＰＵ（Central Processing Unit）、ＭＰＵ（Micro Processing Unit）等の制御演算手段と、サーボモータ２０に電力を供給して駆動するためのアナログ又はデジタルのドライバ部（アンプ）を含んでいる。
ここで、サーボモータ制御部１０は、後述するように、ホスト装置３のアプリケーションにより、位置指令による位置制御を実行する場合と、速度指令により速度制御を実行する場合において、構成を変更することが可能である。
【００２８】
サーボモータ２０は、ＡＣサーボモータ２０、ＤＣサーボモータ２０、リニアアクチュエータ等である。サーボモータ２０は、動作対象物２を動作させる。
【００２９】
検出部３０は、サーボモータ２０の軸等の位置を検出する、具体的に、検出部３０は、サーボモータ２０の位置を検出して出力する位置検出センサを含んでいる。この位置検出センサは、例えば、磁気式や光学式のエンコーダ（Encoder）等である。
また、検出部３０の出力信号は、サーボモータ制御部１０に入力され、位置制御や速度制御のフィードバック（Feedback）制御に使用される。
【００３０】
なお、サーボモータ制御部１０の制御演算手段により実現される、後述する各部は、特定のデジタル回路で構成されてもよい。また、デジタル回路以外に、アナログ回路により構成されていてもよいし、ＲＯＭ（Read Only Memory）に記憶された制御プログラムをＲＡＭ（Random Access Memory）に展開して実行することでソフトウェアをハードウェア資源で実行した回路として構成されてもよい。
また、モータの速度次元信号を得るために擬似微分器を用いない構成も可能である。この場合、後述するように、オブザーバ３００（図４）により、動作対象物２の動作モデルによりサーボモータ２０の速度を推定する等により制御する。
【００３１】
〔サーボモータ制御部１０の構成〕
次に、図２〜図４を参照して、図１のサーボモータ制御部１０の詳細な制御構成について説明する。
図２は、サーボモータ２０の位置指令を入力とし、サーボモータ２０の位置を出力とする位置制御を実行し、衝突検出を行う場合のサーボモータ制御部１０ａの構成である。
【００３２】
サーボモータ制御部１０ａは、位置指令速度次元信号出力部１１０ａ、サーボモータ速度次元信号出力部１２０、第一衝突検出部１３０、位置指令加速度次元変換部１４０ａ、サーボモータ加速度次元変換部１５０、第二衝突検出部１６０、第三衝突検出部１７０、選択部１８０、パラメータ設定部１９０を含む。
【００３３】
制御系１００は、動作対象物２のモデルを備え、フィードバックにより制御を行う制御系１００の全体を示す。
【００３４】
位置指令速度次元信号出力部１１０ａは、ホスト装置３等から位置指令を入力し、この位置指令に対応した速度次元信号を出力する。
【００３５】
また、位置指令速度次元信号出力部１１０ａは、微分器１１１及びフィルタ１１２を含んでいる。
微分器１１１は、位置指令を微分して速度次元に変換する疑似微分部等である。
フィルタ１１２は、例えば、一次のＩＩＲフィルタ（Infinite Impulse Response Filter）である。これにより、フィルタ１１２は、時間遅れの信号を出力する。また、フィルタ１１２は、制御系１００モデルとして１／（τｓ＋１）を伝達関数とする要素を含む。ここで、τは時定数、ｓはラプラス演算子である。また、フィルタ１１２のカットオフ周波数等のパラメータは、外部から設定可能であり、具体的には、パラメータ設定部１９０のカットオフ設定１９３により設定される。
なお、このカットオフ設定１９３は、フィルタのカットオフ周波数はサーボゲインの設定値から算出されてもよい。
また、フィルタ１１２として、一次フィルタ以外の各種フィルタを使用することも可能である。
【００３６】
サーボモータ速度次元信号出力部１２０は、サーボモータ２０の速度次元信号を出力する。
サーボモータ速度次元信号出力部１２０は、一例として、検出部３０の位置検出センサにより検出されたサーボモータ２０の位置信号を取得し、これを微分器で微分し、速度次元信号として出力する。この場合、サーボモータ速度次元信号出力部１２０は、実際の制御系１００から得られる位置フィードバック値の微分値である速度フィードバック値を算出する。
なお、サーボモータ速度次元信号出力部１２０は、他の方式により速度次元信号を算出することが可能である。この速度次元信号の取得、算出方法については後述する。
【００３７】
第一衝突検出部１３０は、位置指令速度次元信号出力部１１０ａにより出力された速度次元信号と、サーボモータ速度次元信号出力部１２０により出力された速度次元信号との差分値である速度偏差が入力される。第一衝突検出部１３０は、この上で、入力された速度偏差の絶対値が特定値以上になった場合、衝突として検出する。第一衝突検出部１３０は、衝突を検出した場合、衝突検出信号を出力する。
【００３８】
位置指令加速度次元変換部１４０ａは、位置指令速度次元信号出力部１１０ａにより出力された速度次元信号を加速度次元信号に変換して出力する。具体的には、位置指令加速度次元変換部１４０ａは、入力された位置指令に係る速度次元信号を、微分器で更に微分して加速度次元に変換し、これを加速度次元信号として出力する。
【００３９】
サーボモータ加速度次元変換部１５０は、サーボモータ２０の速度次元信号を加速度次元信号に変換して出力する。具体的には、サーボモータ加速度次元変換部１５０は、一例として、サーボモータ速度次元信号出力部１２０により出力された速度次元信号を、微分器で更に微分して加速度次元に変換し、これを加速度次元信号として出力する。
【００４０】
第二衝突検出部１６０は、位置指令加速度次元変換部１４０ａにより出力された加速度次元信号と、サーボモータ加速度次元変換部１５０により出力された加速度次元信号との差分値である加速度偏差が入力される。この上で、第二衝突検出部１６０は、加速度偏差の絶対値が特定値以上になった場合、衝突として検出する。第二衝突検出部１６０は、衝突を検出した場合、衝突検出信号を出力する。
【００４１】
第三衝突検出部１７０は、サーボモータ加速度次元変換部１５０により出力された加速度次元信号の絶対値が特定値以上になった場合、衝突として検出する。第三衝突検出部１７０は、衝突を検出した場合、衝突検出信号を出力する。
【００４２】
選択部１８０は、第一衝突検出部１３０、第二衝突検出部１６０、及び第三衝突検出部１７０のいずれを衝突判定に使用するか、パラメータ設定部１９０の検出方法選択設定１９１により選択する。また、選択部１８０は、この選択された部の衝突検出信号を、ホスト装置３等へ出力する。
【００４３】
パラメータ設定部１９０は、主に各種設定を設定し、ＲＡＭ、ＥＥＰＲＯＭ等の一時的でない記録媒体に記憶する。パラメータ設定部１９０の各設定値は、ホスト装置３等の外部の機器により設定可能である。また、各設定値は、ディップスイッチ等で設定されてもよい。
【００４４】
また、パラメータ設定部１９０は、検出方法選択設定１９１、特定値設定１９２、及びカットオフ設定１９３を含んでいる。
検出方法選択設定１９１は、選択部１８０が第一衝突検出部１３０、第二衝突検出部１６０、及び第三衝突検出部１７０のいずれかにより衝突検出を設定するかを指定する特定のパラメータの設定情報である。この特定のパラメータは、通常の速度でサーボモータ２０を作動させるアプリケーションでは、速度偏差を用いた第一衝突検出部１３０を選択することが好適である。また、特定のパラメータは、加減速が速いアプリケーションにおいては、加速度偏差を用いた第二衝突検出部１６０を選択することが好適である。また、特定のパラメータは、加減速が緩やかなアプリケーションにおいては、加速度のフィードバックを用いた第三衝突検出部１７０を選択することが好適である。
なお、この特定のパラメータは、単なる数値ではなく、特定の数学モデル、ファジィ関数、人工ニューラルネットのような関数形式で指定されてもよい。
【００４５】
特定値設定１９２は、第一衝突検出部１３０の特定値、第二衝突検出部１６０の特定値、第三衝突検出部１７０の特定値である。この特定値設定１９２は、単一の値を用意して、アプリケーション毎に切り替えて使用してもよい。なお、特定値設定１９２の特定値は、第一衝突検出部１３０、第二衝突検出部１６０、及び第三衝突検出部１７０のそれぞれ用に用意してもよい。
【００４６】
カットオフ設定１９３は、フィルタ１１２のカットオフ周波数の設定である。なお、このカットオフ周波数は、サーボゲインの設定値から算出できるので、カットオフ設定１９３によりサーボゲインを変更設定することでカットオフ周波数を設定してもよい。
【００４７】
なお、第一衝突検出部１３０、第二衝突検出部１６０、及び第三衝突検出部１７０は、他の方式により衝突検出を行うことも可能である。他の衝突検出の方法については後述する。
また、第三衝突検出部１７０が存在しない構成も可能である。
【００４８】
次に、図３により、サーボモータ２０の速度指令を入力としサーボモータ２０の速度を出力とする速度制御を実行する場合のサーボモータ制御部１０ｂの構成について説明する。図２と図３とにおいて、同様の構成要素には、同一の符号を付している。
【００４９】
サーボモータ制御部１０ｂは、速度指令速度次元信号出力部１１０ｂ、及び速度指令加速度次元変換部１４０ｂを備えている。
【００５０】
速度指令速度次元信号出力部１１０ｂは、ホスト装置３等から速度指令を入力し、この速度指令に対応した速度次元信号を出力する。
速度指令速度次元信号出力部１１０ｂは、この際、速度指令は速度次元の値であるので微分せず、位置指令速度次元信号出力部１１０ａに含まれるものと同様のフィルタ１１２に入力させる。
【００５１】
速度指令加速度次元変換部１４０ｂは、速度指令速度次元信号出力部１１０ｂにより出力された速度次元信号を加速度次元信号に変換して出力する。この処理は、位置指令加速度次元変換部１４０ａと同様である。
【００５２】
〔制御系１００のモデルの構成〕
次に、図４により、制御系１００において、速度次元信号及び加速度次元信号等を取得する際の詳細、他の衝突検出方式等について、内部状態を考慮した状態空間表現によるモデルを用いて説明する。
【００５３】
制御系１００は、動作対象物２に対応してサーボモータ２０を適切に制御するための希望特性を有する伝達関数である希望伝達関数（モデル）に合わせるモデルマッチング制御を行っている。
このモデルは、ラプラス演算子をｓとすると、ｍ０／（ｓ2＋ｍ１ｓ＋ｍ０）と表現可能である。このモデルは、例えば、下記のように変形することができる。
ｍ０／（ｓ2＋ｍ１ｓ＋ｍ０）＝ω１ω２／（ｓ＋ω１）（ｓ＋ω２）
ここで、ω１、ω２は、モデルのカットオフ周波数であり、以下の関係が成立する。
ｍ０＝ω１ω２、ｍ１＝ω１＋ω２  ……  式（１）
なお、動作対象物２およびサーボモータ２０の特性や制御の目的に応じてω１、ω２を設定することで、所望の制御応答特性を得ることができる。
【００５４】
また、制御系１００は、比例ゲイン要素２００、積分フィルタ要素２１０、モータゲイン要素２２０、サーボモータ及び動作対象物を含んだ制御対象要素２３０、微分フィルタ要素２４０、前向き経路２５０、第一帰還経路２６０、及び第二帰還経路２７０を備えている。
【００５５】
ここで、サーボモータ２０に電力を供給するアンプ（図示せず）の特定のゲインと、サーボモータ２０のトルク定数とを含む特定値を、動作対象物２およびサーボモータ２０の慣性モーメントの値（イナーシャ、inertia）で割った値（ゲイン）をＫとする。
また、動作対象物２およびサーボモータ２０の粘性に関わる項を動作対象物２およびサーボモータ２０のイナーシャで割った値であるゲインをｐとする。
この場合、各要素は、以下のように表現される：
【００５６】
比例ゲイン要素２００は、ｍ０である。
また、積分フィルタ要素２１０は、（ｓ2＋ｑ１ｓ＋ｑ０）／（ｓ2＋ａ１ｓ）で表わされる伝達関数である。
また、モータゲイン要素２２０は、１／Ｋである。
また、サーボモータ及び動作対象物を含んだ制御対象要素２３０は、制御対象を示し、Ｋ／（ｓ2＋ｐｓ）で表わされる伝達関数である。
また、微分フィルタ要素２４０は、（ｂ２ｓ2＋ｂ１ｓ）／（ｓ2＋ｑ１ｓ＋ｑ０）で表わされる伝達関数である。
また、前向き経路２５０は、制御系１００の入力から出力へ向かう経路である。
また、第一帰還経路２６０は、制御系１００の出力部から入力側に向かう第一のフィードバックループである。
また、第二帰還経路２７０は、制御系１００の出力部から入力側に向かう第二のフィードバックループである。ここで、第二帰還経路２７０は、位置指令の値に比例ゲインを掛けた信号の偏差を算出する。つまり、第二帰還経路２７０は、位置指令の値に比例ゲインを掛けた信号と、位置検出センサにより検出された位置信号が微分フィルタ要素２４０により微分された信号との偏差を算出するフィードバックループである。
【００５７】
なお、ａ１、ｂ１、ｂ２は以下の関係を満足する。
ａ１  ＝  ｑ１＋ｍ１−ｐ  ……  式（２）
ｂ１  ＝  ｑ０×ｍ１  ……  式（３）
ｂ２  ＝  （ｑ１−ｐ）×（ｍ１−ｐ）＋ｑ０  ……  式（４）
また、上述のｑ０、ｑ１は、動作対象物２およびサーボモータ２０を適切に制御するために任意に設定される値である。
【００５８】
このような構成により、サーボモータ速度次元信号出力部１２０は、速度次元信号として、図４に示す速度フィードバック（ａ）、速度フィードバック（ｂ）、速度フィードバック（ｃ）のいずれかの値を選択し、速度次元信号の算出や出力に使用可能である。
【００５９】
速度フィードバック（ａ）は、上述したように、検出部３０の位置検出センサ等により検出されたサーボモータ２０の位置信号（位置フィードバック値）を微分器にかけて算出された速度フィードバック値である。
【００６０】
速度フィードバック（ｂ）は、オブザーバ３００により推定された速度推定信号である。オブザーバ３００は、モデルの制御対象への入力信号と、制御対象の出力信号とにより速度を推定する速度オブザーバである。具体的には、オブザーバ３００は、制御対象要素２３０への入力と制御対象要素２３０からの出力とに基づいて、ゲインＫやｐを推定する。この際、オブザーバ３００は、例えば、最小二乗法等によってゲインＫを推定する構成とすることもできる。この場合、オブザーバ３００は、ゲインＫやｐが分かっている場合は、その値を使うことができる。また、これらの値が未知の場合は、ゲインＫやｐの推定を、特定時間の間隔で逐次実行する。
なお、速度フィードバック（ｂ）を用いない場合、オブザーバ３００を使用しない構成も可能である。
【００６１】
速度フィードバック（ｃ）は、第二帰還経路２７０の微分フィルタ要素２４０の出力信号である。すなわち、速度フィードバック（ｃ）は、第二のフィードバックループ内に配した微分フィルタの出力信号を、速度次元信号として出力する。
【００６２】
なお、サーボモータ速度次元信号出力部１２０が、どの速度フィードバック値を使用するのかについては、パラメータ設定部１９０に設定可能である。
【００６３】
また、第一衝突検出部１３０は、上述の速度偏差の代わりに、位置指令値に比例ゲインを掛けた信号と、位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、これを速度偏差の代わりに使用して、衝突検出をすることも可能である。つまり、第一衝突検出部１３０は、比例ゲイン要素２００の出力値と、第二帰還経路２７０の微分フィルタ要素２４０の出力値との差分値である疑似速度偏差として算出する。そして、この場合、第一衝突検出部１３０は、算出された疑似速度偏差の絶対値が特定値以上になった場合、衝突として検出する。
【００６４】
また、同様に、第二衝突検出部１６０は、加速度偏差の代わりに、疑似速度偏差を、微分器３１０で微分した信号を疑似加速度偏差として算出することも可能である。この場合、算出された疑似加速度偏差の絶対値が特定値以上になった場合、衝突として検出する。また、第三衝突検出部１７０についても、疑似加速度偏差の値を使用して、衝突検出を行うことが可能である。
【００６５】
〔本実施形態の効果〕
以上のように構成することで、以下のような効果を得ることができる。
従来、特許文献１に記載されたようなトルクを使用した衝突検出を行う方式では、十分な衝突検出の精度が得られなかった。
本発明の実施の形態に係るサーボモータ制御装置１は、動作対象物を動作させるサーボモータ２０と、位置指令によりサーボモータ２０を制御するサーボモータ制御部１０ａとを備えるサーボモータ制御装置１であって、サーボモータ制御部１０ａは、位置指令に対応した速度次元信号を出力する位置指令速度次元信号出力部１１０ａと、サーボモータ２０の速度次元信号を出力するサーボモータ速度次元信号出力部１２０と、位置指令速度次元信号出力部１１０ａにより出力された速度次元信号と、サーボモータ速度次元信号出力部１２０により出力された速度次元信号との差分値である速度偏差の絶対値が特定値以上になった場合、衝突として検出する第一衝突検出部１３０と、位置指令速度次元信号出力部１１０ａにより出力された速度次元信号を加速度次元信号に変換して出力する位置指令加速度次元変換部１４０ａと、サーボモータ速度次元信号出力部１２０により出力された速度次元信号を加速度次元信号に変換して出力するサーボモータ加速度次元変換部１５０と、位置指令加速度次元変換部１４０ａにより出力された加速度次元信号と、サーボモータ加速度次元変換部１５０により出力された加速度次元信号との差分値である加速度偏差の絶対値が特定値以上になった場合、衝突として検出する第二衝突検出部１６０と、第一衝突検出部１３０と第二衝突検出部１６０とを、パラメータ設定部１９０の検出方法選択設定１９１により選択する選択部１８０とを備えることを特徴とする。
このように構成することで、アプリケーションの用途等に対応し、第一衝突検出部１３０と第二衝突検出部１６０とを適切に選択して用いることができる。ここで、第一衝突検出部１３０は、位置偏差やトルクを用いる場合よりも、高精度に衝突検出できる。また、第二衝突検出部１６０は、サーボモータ２０の加減速が速い状況で、より精度高く衝突検出することが可能である。このため、これらをアプリケーションに対応した特定のパラメータにより選択可能とすることで、衝突検出の検出精度を向上させることができる。
また、特許文献１の技術では、加減速の時に衝突を検出することができなかったが、第二衝突検出部１６０を選択することでこれも可能となる。
【００６６】
また、本発明の実施の形態に係るサーボモータ制御装置１は、動作対象物を動作させるサーボモータ２０と、速度指令によりサーボモータ２０を制御するサーボモータ制御部１０ｂとを備えるサーボモータ制御装置１であって、サーボモータ制御部１０ｂは、速度指令に対応した速度次元信号を出力する速度指令速度次元信号出力部１１０ｂと、サーボモータ２０の速度次元信号を出力するサーボモータ速度次元信号出力部１２０と、速度指令速度次元信号出力部１１０ｂにより出力された速度次元信号と、サーボモータ速度次元信号出力部１２０により出力された速度次元信号との差分値である速度偏差の絶対値が特定値以上になった場合、衝突として検出する第一衝突検出部１３０と、速度指令速度次元信号出力部１１０ｂにより出力された速度次元信号を加速度次元信号に変換して出力する速度指令加速度次元変換部１４０ｂと、サーボモータ速度次元信号出力部１２０により出力された速度次元信号を加速度次元信号に変換して出力するサーボモータ加速度次元変換部１５０と、速度指令加速度次元変換部１４０ｂにより出力された加速度次元信号と、サーボモータ加速度次元変換部１５０により出力された加速度次元信号との差分値である加速度偏差の絶対値が特定値以上になった場合、衝突として検出する第二衝突検出部１６０と、第一衝突検出部１３０と第二衝突検出部１６０とを、パラメータ設定部１９０の検出方法選択設定１９１により選択する選択部１８０とを備えることを特徴とする。
このように構成することで、上述のサーボモータ制御部１０ａと同様に衝突検出の精度を向上させることができる。また、速度指令を用いると、微分要素を減らすことができるため部を簡素化できる。
また、従来、位置指令だけで衝突検出を行う技術では、速度制御の際の衝突検出ができなかった。これに対して、本実施形態のサーボモータ制御部１０ｂにより、速度制御時にも高精度に衝突検出をすることができる。
【００６７】
また、本発明の実施の形態に係るサーボモータ制御部１０は、サーボモータ加速度次元変換部１５０により出力された加速度次元信号の絶対値が特定値以上になった場合、衝突として検出する第三衝突検出部１７０を更に備え、選択部１８０は、特定のパラメータにより、第三衝突検出部１７０も選択することを特徴とする。
このように構成することで、加速度の値である次元信号自体と特定値との比較をし、加減速が緩やかな状況で衝突検出の精度を高めることができる。
【００６８】
また、本発明の実施の形態に係るサーボモータ制御装置１は、サーボモータ速度次元信号出力部１２０は、サーボモータ２０の位置を検出する位置検出センサにより検出された位置信号が微分された信号を、速度次元信号として出力することを特徴とする。
このように構成することで、微分器により簡単に速度次元信号を得ることができる。このため、構成を簡略化でき、コストを削減できる。
【００６９】
また、本発明の実施の形態に係るサーボモータ制御装置１は、サーボモータ速度次元信号出力部１２０は、モデルの制御対象への入力信号と制御対象の出力信号とにより速度を推定するオブザーバ３００で算出された速度推定信号を、速度次元信号として出力することを特徴とする。
このように構成することで、サーボモータ速度次元信号出力回路１２０ではなくオブザーバ３００を用いる構成においても、衝突検出を行うことが可能となる。
【００７０】
また、本発明の実施の形態に係るサーボモータ制御装置１は、位置指令の値に比例ゲインを掛けた信号と、位置検出センサにより検出された位置信号が微分フィルタ要素２４０により微分された信号との偏差を算出するフィードバックループである第二帰還経路２７０を備え、サーボモータ速度次元信号出力部１２０は、第二帰還経路２７０内に配した微分フィルタ要素２４０の出力信号を、速度次元信号として出力することを特徴とする。
このように構成することで、フィードバック内の出力をそのまま速度次元信号として使用可能になるため、演算を簡略化して、構成を簡略化し、コストを削減できる。
【００７１】
また、本発明の実施の形態に係るサーボモータ制御装置１は、第一衝突検出部１３０は、速度偏差の代わりに、位置指令値に比例ゲインを掛けた信号と、位置検出センサにより検出された位置信号が微分された信号との差分値を疑似速度偏差として算出し、疑似速度偏差の絶対値が特定値以上になった場合、衝突として検出することを特徴とする。
このように構成することで、伝達関数の演算途中の値である疑似速度偏差を、そのまま速度次元信号として使用可能になるため、演算を簡略化して、構成を簡略化し、コストを削減できる。
【００７２】
また、本発明の実施の形態に係るサーボモータ制御装置１は、第二衝突検出部１６０は、加速度偏差の代わりに、疑似速度偏差を微分した信号を疑似加速度偏差として算出し、疑似加速度偏差の絶対値が特定値以上になった場合、衝突として検出することを特徴とする。
このように構成することで、伝達関数の演算途中の値を微分するだけで、疑似加速度偏差を、そのまま加速度次元信号として使用可能になるため、演算を簡略化して、構成を簡略化し、コストを削減できる。
【００７３】
また、本発明の実施の形態に係るサーボモータ制御装置１は、選択部１８０は、第一衝突検出部１３０、第二衝突検出部１６０、及び第三衝突検出部１７０のいずれを選択するか、外部から設定されたパラメータにより選択することを特徴とする。
このように構成することで、どの衝突検出に適した回路を使用するか、外部から随時設定できる。このため、変更の負荷が少なく、簡単に衝突検出の方式を変更可能となる。
【００７４】
また、本発明の実施の形態に係るサーボモータ制御装置１は、選択部１８０は、第一衝突検出部１３０の特定値、第二衝突検出部１６０の特定値、第三衝突検出部１７０の特定値は、外部から設定されることを特徴とする。
このように構成することで、特定値の設定も外部から随時設定できる。このため、変更負荷が少なく、簡単に衝突検出の閾値となる特定値を変更が可能となる。
【００７５】
また、本発明の実施の形態に係るサーボモータ制御装置１は、速度次元信号は、制御系１００のモデルとして１／（τｓ＋１）を伝達関数とする要素を含むフィルタ１１２の信号であることを特徴とする。
このように構成することで、フィルタ１１２に制御モデルの要素を含ませることができ、位置指令値又は速度指令値を、実際の制御対象に入力したものとして使用することが可能となる。これにより、擬似的に実際の信号に近い信号を得ることができ、従来より高精度に衝突検出ができる。
【００７６】
また、本発明の実施の形態に係るサーボモータ制御装置１は、フィルタ１１２は、カットオフ周波数が外部から設定されることを特徴とする。
このように構成することで、外部からカットオフ周波数を随時設定できるため、変更負荷が少なくなる。また、サーボモータ２０に合わせて適宜、カットオフ周波数を簡単に選択できる。なお、上述したように、このカットオフ周波数をサーボゲインの設定値から算出してもよい。
【００７７】
また、本発明の実施の形態に係る衝突検出方法は、動作対象物を動作させるサーボモータ２０と、位置指令により前記サーボモータ２０を制御するサーボモータ制御部１０とを備えるサーボモータ制御装置１により実行される衝突検出方法であって、前記位置指令に対応した速度次元信号を出力し、前記サーボモータ２０の速度次元信号を出力し、出力された前記位置指令に対応した速度次元信号と、出力された前記サーボモータ２０の速度次元信号との差分値である速度偏差を算出し、前記位置指令に対応した加速度次元信号を出力し、前記サーボモータ２０の加速度次元信号を出力し、出力された前記位置指令に対応した加速度次元信号と、出力された前記サーボモータ２０の加速度次元信号との差分値である加速度偏差を算出し、特定のパラメータにより選択された前記速度偏差及び加速度偏差のいずれかの絶対値が特定値以上になった場合、衝突として検出することを特徴とする
このように構成することで、アプリケーションの用途等に対応し、衝突検出の方式を適切に選択して用いることができる。このため、衝突検出の精度を高めることができる。
【００７８】
〔他の実施の形態〕
なお、上述の実施の形態においては、サーボモータ２０の衝突検出を行う例について説明した。
しかしながら、本発明の衝突検出方法は、制御系１００が不安定になって、振動を起こした等の状態を検出する用途にも使用することができる。これにより、サーボモータ２０の制御を外乱等に対して、安定させることが可能となる。
【００７９】
また、上述の実施の形態では、第一衝突検出部１３０、第二衝突検出部１６０、第三衝突検出部１７０で、速度偏差の絶対値、加速度偏差の絶対値、加速度次元信号の絶対値を特定値と比較して衝突検出するように記載した。
しかしながら、速度偏差の絶対値や加速度偏差の絶対値や加速度次元信号の絶対値を選択回路１８０に入力して検出方法選択設定１９１によりどの絶対値を使用するか選択した後、選択部１８０後のコンパレータ等により、パラメータ設定部１９０の特定値設定１９２で設定された特定値以上になった場合に衝突として検出するような構成であってもよい。
このように構成することで、一つのコンパレータで衝突検出の出力を処理することができ、回路規模を縮小させ、コストを削減できる。
【００８０】
なお、上記実施の形態の構成及び動作は例であって、本発明の趣旨を逸脱しない範囲で適宜変更して実行することができることは言うまでもない。
【符号の説明】
【００８１】
１  サーボモータ制御装置
２  動作対象物
３  ホスト装置
１０、１０ａ、１０ｂ  サーボモータ制御部
２０  サーボモータ
３０  検出部
１００  制御系
１１０ａ  位置指令速度次元信号出力部
１１０ｂ  速度指令速度次元信号出力部
１１１、３１０  微分器
１１２  フィルタ
１２０  サーボモータ速度次元信号出力部
１３０  第一衝突検出部
１４０ａ  位置指令加速度次元変換部
１４０ｂ  速度指令加速度次元変換部
１５０  サーボモータ加速度次元変換部
１６０  第二衝突検出部
１７０  第三衝突検出部
１８０  選択部
１９０  パラメータ設定部
１９１  検出方法選択設定
１９２  特定値設定
１９３  カットオフ設定
２００  比例ゲイン要素
２１０  積分フィルタ要素
２２０  モータゲイン要素
２３０  制御対象要素
２４０  微分フィルタ要素
２５０  前向き経路
２６０  第一帰還経路
２７０  第二帰還経路
３００  オブザーバ
Ｘ  制御システム
【図１】
0006517567.tif 000002
【図２】
0006517567.tif 000003
【図３】
0006517567.tif 000004
【図４】
0006517567.tif 000005