(57)【要約】【課題】演算式が簡単で処理時間が短く、動作指令がステップ波形のように高周波成分を含んで急激に変化する場合や負荷側の振動や軸間の干渉がある場合にも誤検出することなく、高感度に衝突を検出できるモータの制御装置を提供することを目的とする。【解決手段】モータ制御部１１の指令に基づいてモータ１３を駆動し、モータ１３に接続される駆動機構部１２を有するモータの制御装置において、モータ１３を模擬したモータモデル２３と機構部を模擬した機構モデル部２２とモータ制御部を模擬した制御モデル部２１とを有するシミュレーション部２０と、モータ制御部１１、モータ１３及び機構部１２を含む実機部１０とシミュレーション部２０の同一状態量を比較する状態量比較部３１と、状態量比較部３１の出力である状態量比較値と予め設定された検出しきい値を比較して外部との接触を検出する接触検出部３２とを備える。【選択図】図１
2004364396.tif 000002
【特許請求の範囲】
【請求項１】
モータ制御部の指令に基づいてモータを駆動し、前記モータに接続される機構部を有するモータの制御装置において、
前記モータを模擬したモータモデルと前記機構部を模擬した機構モデル部と前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、
前記モータ制御部、前記モータ及び前記機構部を含む実機部と前記シミュレーション部の同一状態量を比較する状態量比較部と、
前記状態量比較部の出力である状態量比較値と予め設定された検出しきい値を比較して外部との接触を検出する接触検出部と、
を備えることを特徴とするモータの制御装置。
【請求項２】
前記状態量比較値が予め設定された検出しきい値よりも大きい場合には、前記モータを停止させる、若しくはトルクを制限する、又は予め設定された退避動作をさせる、又は外部への信号出力を行う停止処理部とを備えることを特徴とする請求項１記載のモータの制御装置。
【請求項３】
前記接触検出部は、前記動作指令の加減速区間に応じて変化させた値を検出しきい値とし、前記実機部の状態量と比較して外部との接触を検出することを特徴とする請求項１乃至２記載のモータの制御装置。
【請求項４】
前記接触検出部は、前記シミュレーション部の状態量を検出しきい値とし、前記実機部の状態量と比較して外部との接触を検出することを特徴とする請求項１乃至３記載のモータの制御装置。
【請求項５】
前記シミュレーション部の前記機構モデル部は、多慣性モデルであることを特徴とする請求項１乃至４記載のモータの制御装置。
【請求項６】
前記モータが複数接続された機械装置の場合、前記シミュレーション部の前記機構モデル部に各モータ間の干渉モデルを備えることを特徴とする請求項１乃至５記載のモータの制御装置。
【請求項７】
前記状態量比較値および検出しきい値を操作画面上に表示し、検出しきい値を調整することを特徴とする請求項１乃至６記載のモータの制御装置。
【請求項８】
外部との衝突を検出するセンサと、
前記センサ信号の基準値を前記シミュレーション部の状態量から作成するセンサ基準値演算部とを備え、
前記接触検出部は、前記センサ基準値演算部の出力である前記基準値により修正された前記センサ信号と前記実機部の内部状態量を比較して外部との接触を検出することを特徴とする請求項１乃至７記載のモータの制御装置。
【請求項９】
モータ制御部の指令に基づいてモータを駆動し、前記モータに接続される機構部を有するモータの制御方法において、
前記モータを模擬したモータモデルと前記機構部を模擬した機構モデル部と前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部を有し、前記モータ制御部及び前記モータ及び前記機構部を含む実機部と前記シミュレーション部の同一状態量を比較して状態量比較値を作成し、
前記状態量比較値と予め設定された検出しきい値を比較して外部との接触を検出し、
前記モータを停止させるか、又は非常停止させるか、又はトルクを制限するか、又は予め設定された退避動作をさせるか、又は外部への信号出力を行うことを特徴とするモータの制御方法。
【請求項１０】
外部との衝突を検出するセンサを備え、
前記センサ信号の基準値を前記シミュレーション部の状態量から作成し、
前記センサ基準値演算部の出力である前記基準値により修正された前記センサ信号と前記実機部の内部状態量を比較して外部との接触を検出することを特徴とする請求項９記載のモータの制御方法。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、ロボットや工作機械等のサーボモータを動力源とする機械において、工作機械のカッタやロボットアーム等のサーボモータで駆動される可動部がワークなどの他の物体と衝突したことを検出するモータの制御装置および制御方法に関する。
【０００２】
【従来の技術】
従来、工作機械の可動部（カッタ若しくはテーブル等）やロボットのアーム等が対象物であるワークなどの周辺機器に衝突した時に、これら可動部やアームを駆動するサーボモータは移動命令に従って移動しようとして、位置決め精度を高くするために大きく設定されたゲインや積分器の作用により、大きなトルクを発生する。そのため、可動部やアームの機構部やワークを破損させる場合がある。このような問題に対して、周辺機器との衝突を検出して工作機械やロボットを停止させる方法がある。
特別なセンサを用いずにソフト的に検出する方法として、図１５に示すように外乱推定オブザーバによって外乱トルクを推定し、この推定外乱トルクが設定値以上になったとき、衝突等の異常負荷が生じているものとして検出する方法が公知である（例えば、非特許文献１）。また、減速機のバネ要素を考慮したモデルを使用したオブザーバによって検出する方法がある（例えば、特許文献１）。その他の方法として、電流波形の乱れから衝突を検出する方法が開示されている（例えば、特許文献２）。
また、衝突検出用に特別なセンサを用いる方法として、モータに取り付けたトルクセンサからの信号の乱れで衝突を検出するもの（例えば、特許文献３）や、エンドエフェクタ先端に取り付けた加速度センサからの振動に比例した電圧の大きさと継続時間から接触を検出するものがある（例えば、特許文献４）。
【０００３】
【特許文献１】
特開２０００−０５２２８６号公報
【特許文献２】
特開平０２−５９２９１号公報
【特許文献３】
特開平０５−２０８３９４号公報
【特許文献４】
特開平１０−２８６５０６号公報
【非特許文献１】
「多軸加速制御における自己診断機能」、産業電力電気応用研究会ＩＥＡ−８９−１０〜１４、村上俊之、駒田諭、大西公平
【０００４】
【発明が解決しようとする課題】
ところが、非特許文献１や特許文献１に示すように、外乱推定オブザーバを用いる方法では、検出精度を上げるために減速機のバネ要素まで含めてモデル化すると、演算式が複雑で処理時間が長くなるという問題がある。また、オブザーバの時定数だけ検出時間が遅れるという問題がある。
また、特許文献２に示すように、電流波形の乱れから衝突を検出する方法では、動作指令がステップ波形のように高周波成分を含んで急激に変化する場合には、その加減速度に応じてトルクが必要となり、それが電流波形の乱れとなるため、周辺機器との衝突がない場合でも誤検出してしまうという問題点がある。また、減速機をバネ要素とする負荷側の振動や複数軸を持ったロボットの軸間に作用する干渉などの外乱により電流波形の乱れが生じるため、同様に誤検出してしまうという問題点がある。
また、特許文献３及び特許文献４に示すように、衝突検出用に加速度センサやトルクセンサなどの特別なセンサを用いる場合、センサ信号には動作指令による加減速情報と周辺機器との接触情報の２種類の情報が混在して検出されるという問題がある。そのため、動作指令によるロボットの加減速情報により誤検出させないように、予め検出しきい値を大きく設定することで検出感度が低下するという問題点も有している。
そこで本発明は、演算式が簡単で処理時間が短く、動作指令がステップ波形のように高周波成分を含んで急激に変化する場合や負荷側の振動や軸間の干渉がある場合にも誤検出することなく、高感度に衝突を検出できることを目的とする。また、センサを用いた場合には、動作指令による加減速情報と周辺機器との接触情報を分離し、検出しきい値を小さくして、高感度に衝突を検出できることを目的とする。
【０００５】
【課題を解決するための手段】
本発明では、上記問題点を解決するため、請求項１記載のモータの制御装置は、モータ制御部の指令に基づいてモータを駆動し、前記モータに接続される機構部を有するモータの制御装置において、前記モータを模擬したモータモデルと前記機構部を模擬した機構モデル部と前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、前記モータ制御部、前記モータ及び前記機構部を含む実機部と前記シミュレーション部の同一状態量を比較する状態量比較部と、前記状態量比較部の出力である状態量比較値と予め設定された検出しきい値を比較して外部との接触を検出する接触検出部と、を備えることを特徴とするものである。
【０００６】
また、請求項２記載のモータの制御装置は、前記状態量比較値が予め設定された検出しきい値よりも大きい場合には、前記モータを停止させる、若しくはトルクを制限する、又は予め設定された退避動作をさせる、又は外部への信号出力を行う停止処理部とを備えることを特徴とするものである。
また、請求項３記載のモータの制御装置は、前記接触検出部は、前記動作指令の加減速区間に応じて変化させた値を検出しきい値とし、前記実機部の状態量と比較して外部との接触を検出することを特徴とするものである。
また、請求項４記載のモータの制御装置は、前記接触検出部は、前記シミュレーション部の状態量を検出しきい値とし、前記実機部の状態量と比較して外部との接触を検出することを特徴とするものである。
【０００７】
また、請求項５記載のモータの制御装置は、前記シミュレーション部の前記機構モデル部は、多慣性モデルであることを特徴とするものである。
また、請求項６記載のモータの制御装置は、前記モータが複数接続された機械装置の場合、前記シミュレーション部の前記機構モデル部に各モータ間の干渉モデルを備えることを特徴とするものである。
【０００８】
また、請求項７記載のモータの制御装置は、前記状態量比較値および検出しきい値を操作画面上に表示し、検出しきい値を調整することを特徴とするものである。
また、請求項８記載のモータの制御装置は、外部との衝突を検出するセンサと、
前記センサ信号の基準値を前記シミュレーション部の状態量から作成するセンサ基準値演算部とを備え、前記接触検出部は、前記センサ基準値演算部の出力である前記基準値により修正された前記センサ信号と前記実機部の内部状態量を比較して外部との接触を検出することを特徴とするものである。
【０００９】
また、請求項９記載のモータの制御方法は、モータ制御部の指令に基づいてモータを駆動し、前記モータに接続される機構部を有するモータの制御装置において、前記モータを模擬したモータモデルと前記機構部を模擬した機構モデル部と前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部を有し、前記モータ制御部及び前記モータ及び前記機構部を含む実機部と前記シミュレーション部の同一状態量を比較して状態量比較値を作成し、前記状態量比較値と予め設定された検出しきい値を比較して外部との接触を検出し、前記モータを停止させるか、又は非常停止させるか、又はトルクを制限するか、又は予め設定された退避動作をさせるか、又は外部への信号出力を行うことを特徴とするものである。
【００１０】
また、請求項１０記載のモータの制御方法は、外部との衝突を検出するセンサを備え、前記センサ信号の基準値を前記シミュレーション部の状態量から作成し、
前記センサ基準値演算部の出力である前記基準値により修正された前記センサ信号と前記実機部の内部状態量を比較して外部との接触を検出することを特徴とするものである。
【００１１】
【発明の実施の形態】
本発明の第１の具体的実施例を図１に示して説明する。ここでは、説明の簡略化のため、本制御装置で駆動するモータは１軸とする。
図１において、実機部１０は、機構部１２と、前記機構部１２に接続されたモータ１３と、前記モータ１３を制御するモータ制御部１１で構成されている。シミュレーション部２０は、前記モータ１３の動力学系を模擬したモータモデル２３と、前記機構部１２の動力学系を模擬した機構モデル部２２と、前記モータ制御部１１を模擬した制御モデル部２１で構成されている。ここで、機構部を剛体モデルと仮定すると、前記シミュレーション部２０の演算式は、以下の（式１）〜（式４）のように非常に簡単な式で表すことができる。
【００１２】
【数１】
2004364396.tif 000003
【００１３】
【数２】
2004364396.tif 000004
【００１４】
【数３】
2004364396.tif 000005
【００１５】
【数４】
2004364396.tif 000006
【００１６】
ここで、
θＲＥＦ：位置指令
θＦＢ：モデル位置フィードバック
θＦＢＯＬＤ：モデル位置フィードバック（１サンプリング前の演算結果）
ＶＦＢ：モデル速度フィードバック
ＶＦＢＯＬＤ：モデル速度フィードバック（１サンプリング前の演算結果）
ＴＲＥＦ：モデルトルク指令
Ｋｐ：位置ゲイン
Ｋｖ：速度ゲイン
Ｋｉ：積分ゲイン
Ｊ：イナーシャ
ａ：加速度指令
Δｔ：サンプリング時間
Σ：積算
である。
【００１７】
次回のサンプリング時、式１を演算する前に、θＦＢＯＬＤ＝θＦＢとする。
モータを制御する制御装置内で、前記実機部１０及び前記シミュレーション部２０は並列に配置されており、それぞれ上位指令生成部５０から同一の指令を入力とする。前記実機部１０及び前記シミュレーション部２０では、入力された指令に応じて機構部１２及び機構部モデル２２を制御するためのトルク指令をそれぞれモータ１３及びモータモデル２３に出力する。ここで、シミュレーション部２０では実機部１０のサーボ系の遅れや機構の特性などを含んだモータの運動特性を反映できているので、実機部１０とシミュレーション部２０の内部状態量（位置指令、位置フィードバック、位置偏差、速度指令、速度フィードバック、速度偏差など）は、ほぼ一致する。そのため、モータ１３に接続された負荷と外部との予期せぬ衝突は、実機部１０の内部状態量にのみ作用し、シミュレーション部２０の内部状態量には変化を与えない。この違いにより衝突を検出するものである。
【００１８】
ここで、シミュレーション部２０には実機部１０と同一の指令を入力しているので、指令が急激に変化した場合でも、シミュレーション部２０と実機部１０の内部状態量は同じ応答を示すために、両者の内部状態量を比較することで衝突検出の感度に影響を与えない。
状態量比較部３１では、前記実機部１０と前記シミュレーション部２０における前記モータ制御部１１と前記制御モデル部２１の同一状態量（例えば、位置偏差や速度フィードバックやトルク指令など）を、または、前記機構部１２と前記機構モデル部２２の同一状態量（例えば、位置フィードバックなど）を比較して、状態量比較値を作成する。ここで、状態量比較値は、状態量の瞬間値か、状態量の一定時間毎の蓄積値か、状態量の微分値で作成される。
接触検出部３２では、前記状態量比較値と予め作業内容に応じて設定された検出しきい値を比較することで、モータ１３に接続された負荷と外部との衝突を検出することができる。衝突検出された時点で衝突検出情報を発生させる。
【００１９】
次に、内部状態量としてトルク指令を用い、衝突検出後にモータを停止させる例を図２を用いて説明する。具体的には、前記実機部１０のモータ制御部１１の出力である実機トルク指令と、前記シミュレーション部２０の制御モデル部２１の出力であるモデルトルク指令を状態量比較部３１に入力する。ここで、実機部１０には重力及び摩擦が作用するが、シミュレーション部２０には作用しない。そこで、重力摩擦モデルを用い、重力トルク及び摩擦トルクをシミュレーション部２０のモデルトルク指令に加算することで、実機部と同等の環境を構成する。
状態量比較部３１内で、実機トルク指令とモデルトルク指令の比較を行う。具体的には、実機トルク指令とモデルトルク指令の絶対値の差分を取り、状態量比較値（トルク指令差分値）を作成する。ここで、実機部１０とシミュレーション部２０のモデル化誤差を吸収するため、前記差分値を時間微分することで衝突が無い状態から衝突した状態への値の時間変化を求めたり、前記差分値の時間積分することで衝突した際の値の変化を増幅させて、より検出感度を上げたりする方法もある。
【００２０】
状態量比較部３１内で作成された状態量比較値を接触検出部３２に転送する。前記接触検出部３２内で、前記状態量比較値と予め作業内容に応じて設定された検出しきい値を比較することで、モータ１３に接続された負荷と外部との衝突を検出する。この検出情報に基づいて、実機部１０のモータ制御部１１にモータ停止命令を送る。モータ制御部１１では、モータ停止命令を受け取ると、衝突検出直前のサーボモータの回転方向と逆方向のトルクで所定の大きさで所定の時間だけ与えて、モータを減速停止させる。又は、図示しないが、前記モータを停止させる以外に、非常停止させるか、トルク指令をリミットにより制限することで柔軟性を持たせるか、予め設定された退避動作を取らせるか、外部の周辺機器へ停止命令などの信号出力を行うなどの処理を選択しても良い。
【００２１】
次に本実施例の効果を、図３を用いて説明する。
外部との衝突が無い場合、図３（ａ）に示すように、実機部１０やシミュレーション部２０のトルク指令はほぼ一致した値となる。そこで、状態量比較部３１内で両者の差分を取り、図３（ｂ）に示すトルク指令差分値を求めることができる。この時、予め設定された接触検出しきい値は、前記トルク指令差分値よりも大きく設定しておく。
外部との衝突がある場合、図３（ｃ）に示すように、実機部１０やシミュレーション部２０のトルク指令は衝突した時点で異なる波形となる。そこで、状態量比較部３１内で両者の差分を取り、図３（ｄ）に示すトルク指令差分値を求めることができる。この時、前記トルク指令差分値は接触検出しきい値よりも大きくなるため、衝突検出をすることができる。
【００２２】
以下に、本発明の第２の具体的実施例を図４に示して説明する。
前記第１の実施例では、予め設定された固定の接触検出しきい値と状態量比較値を比較することで、衝突検出していた。本実施例では、検出しきい値演算部３３を設け、指令の変化に応じて検出しきい値を可変にした点が異なる。ここでは、指令に応じて２つの可変方法を例に説明する。
【００２３】
（１）加減速区間で可変
前記シミュレーション部２０は前記実機部１０を模擬しているが、図５（ａ）に示すように負荷イナーシャなどにモデル化誤差がある場合には、図５（ｂ）に示すように加減速時にトルク指令などの状態量に誤差が発生し、トルク指令差分値も大きくなり、誤検出の可能性がある。そのため、加減速時と等速度時で検出しきい値を変化させる必要がある。
図４に示すように、検出しきい値演算部３３内では、上位指令生成部からの指令をもとに、位置指令を２回微分することで、図５（ｃ）に示す加速度を作成する。この加速度の絶対値の大きさにより、現在の状態が加減速時であるか、等速度時であるかを判断し、加減速時には予め設定された加減速検出しきい値を用い、等速時には予め設定された等速検出しきい値（加減速検出しきい値＞等速検出しきい値）を用いる。そのため、検出しきい値は図５（ｄ）に示すように、加減速時にトルク指令差分値が検出しきい値を越えることが無くなり、誤検出を無くすことができる。
【００２４】
（２）指令に応じて可変
前記シミュレーション部２０の内部状態量から直接、検出しきい値を作成し、前記検出しきい値と実機部１０の内部状態量を比較することで衝突検出する。
図６に示すように、検出しきい値演算部３３内で、前記シミュレーション部２０からの内部状態量（トルク指令）をもとに、予め設定されたオフセット量αを加算して検出しきい値を作成する。これは元々、図７（ａ）のように前記シミュレーション部２０と前記実機部１０のトルク指令はほぼ一致しているため、前記シミュレーション部２０のモデルトルク指令をそのまま検出しきい値に用いることは出来ない。そこで、前記モデルトルク指令の上下に前記オフセット量αを持たせて作成することで、検出しきい値とする。
【００２５】
次に、本実施例の効果を説明する。外部との衝突が無い場合、図７（ｂ）に示すように、上下の検出しきい値で実機部１０のトルク指令を挟む構成になるが、オフセット量αだけ余裕があるため誤検出はしない。また、前記シミュレーション部２０でサーボ系の遅れも模擬できているため、加減速の傾きに一致したリミットを作成できる。
外部との衝突がある場合、図７（ｃ）に示すように、外乱トルクがオフセット量α分の値だけ作用すると、高感度に衝突検出をすることができる。
【００２６】
以下に、本発明の第３の具体的実施例を図８に示して説明する。
本実施例では、前記第１の実施例のモデル演算式（式１〜４）が異なる。前記第１の実施例では機構部モデルを剛体モデルとしたが、本実施例では減速機を介した２慣性モデルとして減速機をバネ要素とする負荷側の振動をモデル化して、衝突により発生したトルク変動と２次側の振動によるトルク変動を分離するものである。
実機の減速機の剛性が低い場合には、減速機をバネ要素とした２慣性の振動系となり、負荷側の振動が実機部１０のトルク指令に与える影響が大きくなり、単純に前記シミュレーション部２０のトルク指令と比較するだけでは、誤検出の原因となる。ここで、オブザーバなどの制振制御則を追加することで、実機部１０の負荷側の振動を抑えた場合でも、実機部１０のモータ制御部１１からの出力であるトルク指令には機構部１２を振動させない逆位相の振動成分を持ったトルク指令となり、このトルク指令を単純に前記シミュレーション部２０のトルク指令と比較と、同様に誤検出の原因となる。
【００２７】
そこで、前記シミュレーション部２０の前記機構モデル部２２において、図８に示す２慣性モデルとすることで、負荷側の振動によるトルク指令への影響を模擬し、キャンセルするものである。前記、２慣性モデルは、以下の式５〜９で表すことができる。
【００２８】
【数５】
2004364396.tif 000007
【００２９】
【数６】
2004364396.tif 000008
【００３０】
【数７】
2004364396.tif 000009
【００３１】
【数８】
2004364396.tif 000010
【００３２】
【数９】
2004364396.tif 000011
【００３３】
ここで、
ＴＲＥＦ：モデルトルク指令
Ｊｍ：ロータイナーシャ
ＪＬ：負荷イナーシャ
Ｎ：減速比
Ｋｃ：減速比バネ定数
Ｘｍ：モデル位置フィードバック
ＸｍＯＬＤ：モデル位置フィードバック（１サンプリング前の演算結果）
Ｖｍ：モデル速度フィードバック
ＶｍＯＬＤ：モデル速度フィードバック（１サンプリング前の演算結果）
ＸＬ：モデル負荷側位置フィードバック
ＶＬ：モデル負荷側速度フィードバック
Δｔ：サンプリング時間
Σ：積算
である。
【００３４】
次回のサンプリング時、式５〜８を演算する前に、ＸｍＯＬＤ＝Ｘｍ、ＶｍＯＬＤ＝Ｖｍ、ＶＬＯＬＤ＝ＶＬ、ＸＬＯＬＤ＝ＸＬを実行する。
上記演算により、２慣性モデルの負荷側の振動をモデル化できたので、前記シミュレーション部２０の前記制御モデル部２１におけるトルク指令には負荷側の振動が含まれたものとなる。よって、前記第１の実施例と同様に、状態量比較部３１内で、実機トルク指令とモデルトルク指令の比較を行うことで、実機部１０に作用している負荷側の振動を前記シミュレーション部２０でも模擬できる。そのため、負荷側の振動をキャンセルでき、衝突により発生するトルク変動をより高精度に検出することができる。
【００３５】
以下に、本発明の第４の具体的実施例を図９及び１０に示して説明する。
本実施例では、モータが複数接続された水平多関節型ロボットに関して、モータ軸間に作用する干渉トルクの補償方法を説明する。説明を簡略化するため、図９に示すように制御対象の機構部は、２軸の水平多関節型ロボットとする。
ロボットは作業内容に応じて様々に姿勢が変化し、姿勢によっては、ある軸の運動が他の軸の運動に影響を与える干渉力が大きくなるため、負荷側の振動が実機部１０のトルク指令に与える影響が大きくなり、単純に前記シミュレーション部２０のトルク指令と比較するだけでは、誤検出の原因となる。ここで、非干渉化制御などの制御則を追加することで、実機部１０の軸間の非干渉化を行った場合でも、実機部１０のモータ制御部１１からの出力であるトルク指令には機構部１２に作用する干渉力を打ち消す逆位相の干渉トルクを含んだトルク指令となり、このトルク指令を単純に前記シミュレーション部２０のトルク指令と比較すると、同様に誤検出の原因となる。
【００３６】
そこで、前記シミュレーション部２０の前記機構モデル部２２において、図８に示す２慣性モデルとすることで、負荷側の振動によるトルク指令への影響を模擬し、キャンセルするものである。前記、２慣性モデルは、以下の式５〜９で表すことができる。
本実施例では、図１０に示すように、前記シミュレーション部２０において、実機部１０の相互干渉３４と同じ相互干渉モデル３５を設けることで、軸間に作用する干渉トルクによるトルク指令への影響を模擬し、実機部１０とシミュレーション部２０の内部状態量を一致させるものである。
以下では、各軸の主慣性（前述の負荷イナーシャＪＬ）及び相互慣性を演算することで、他の軸に影響を与える干渉トルクをモデル化し、実機部のトルク指令からキャンセルする方法について説明する。
【００３７】
２軸の水平多関節型ロボットの運動方程式は次のように表される。
【００３８】
【数１０】
2004364396.tif 000012
【００３９】
【数１１】
2004364396.tif 000013
【００４０】
ここで、ｍｉはリンクｉの質量、Ｌｉは軸から重心までの距離、ａｉはリンク長さである。また、速度項は速度に依存しており、速度制御ループで補償されるため、無視して考えられるので、式（１０）と（１１）は次のように表せる。
【００４１】
【数１２】
2004364396.tif 000014
【００４２】
【数１３】
2004364396.tif 000015
【００４３】
よって、式（１２）と（１３）は以下のように表現することができる。
【００４４】
【数１４】
2004364396.tif 000016
【００４５】
ここで、
【００４６】
【数１５】
2004364396.tif 000017
【００４７】
【数１６】
2004364396.tif 000018
【００４８】
【数１７】
2004364396.tif 000019
【００４９】
また、Ｉｉは重心周りの慣性モーメント、Ｎｉはｉ軸モータの減速機の減速比、Ｉｉｍはモータのモータロータのイナーシャ（等価慣性モーメント）である。
ｍｉ、Ｌｉ、ａｉ、Ｎｉは既知であり、主慣性であるＪ１１とＪ２２は、負荷イナーシャＪＬとして求められ、特定の姿勢θ１とθ２での重心周りの慣性モーメントＩｉを式（１５）〜（１７）から求め、相互慣性であるＪ１２とＪ２１を求めることができる。
（式１４）は、加減速時に２軸目が１軸目に作用する干渉トルクτ１２と１軸目が２軸目に作用する干渉トルクτ２１が、以下のように表せることを意味する。
【００５０】
【数１８】
2004364396.tif 000020
【００５１】
【数１９】
2004364396.tif 000021
【００５２】
よって、図１０に示すように、求めた相互慣性のパラメータＪ１２（＝Ｊ２１）を元に、シミュレーション部２０に相互干渉モデル３５を配置する。これにより、実機部１０の相互干渉３４と同様の干渉トルクをシミュレーション部２０の相互干渉モデル３５でも模擬できるため、実機部１０とシミュレーション部２０の内部状態量（トルク指令など）を比較する際に、相互干渉の影響を取り除くことができ、衝突検出をより高感度に行うことができる。
【００５３】
以下に、本発明の第５の具体的実施例を図１１に示して説明する。
本実施例では、検出しきい値の調整を容易にするため、前記実機部１０及び前記シミュレーション部２０の内部状態量や状態量比較値や検出しきい値などを作業者が直感的に分かりやすいように表示し、更に検出しきい値などの制御パラメータの調整を容易にしている。
図１１は、モータの制御装置に付属した操作ペンダント上に、横軸に時間、縦軸にトルクを取り、状態量比較値としてトルク指令差分値と検出しきい値を表示させた例である。
この例以外にも、図示しないが、シミュレーションでロボットの動作を３次元表示させ、ロボットの動作と状態量比較値や検出しきい値が同時に見られる構成にしても良い。
【００５４】
以下に、本発明の第６の具体的実施例を、図１２、図１３及び図１４に示して説明する。前記第１の実施例では、センサを用いずにシミュレーション部と実機部の内部状態量の比較により衝突検出を行っていた。本実施例では、加速度センサやトルクセンサなどのセンサを使用する衝突検出において、センサ検出の基準値をシミュレーション部の内部状態量から求めることで、センサの検出値が指令による動作に起因するものか、衝突に起因するものかを区別して、より高感度に衝突検出できる点が異なる。
ここでは、図１２に示すように、制御対象の機構部は１軸の水平多関節型ロボットとし、前記ロボットの手先に加速度センサを配置することで、周辺機器との衝突を検出する場合を考える。
【００５５】
ロボットを上位指令生成部からの動作指令により動作させた場合、ロボットの手先に配置した加速度センサの出力信号には、ロボットが動作指令により動作する際の加減速運動の情報も含まれてしまうため、周辺機器との衝突が無い場合にも、図１３（ａ）に示すような信号波形になる。そのため、加減速時に誤検出しないようにするためには、検出しきい値を加減速時の加速度よりも大きく設定する必要があった。しかし、検出しきい値をあまりに大きく設定すると、図１３（ｂ）に示すように、実際に実機部１０に衝突が発生した場合でも検出できないことがある。
そこで、図１４に示すように、シミュレーション部２０のモータモデル２３又は機構モデル部２２の内部状態量である加速度情報を演算する。前記加速度情報とセンサ信号を比較することで、ロボットが動作指令により動作する際の加減速運動の情報は、センサ信号から分離して補償できる。
【００５６】
加速度センサ３６からのセンサ信号は、センサ信号座標変換部３８内で作業座標系の加速度信号から関節座標系の加速度信号に変換される。この変換は、本実施例のように１軸のロボットの場合には、軸の接線方向と加速度センサの検出方向を一致させることで簡単に求めることができる。２軸以上のモータ軸を持ったロボットである場合は、手先の加速度センサ３６の出力信号を、一般的にヤコビアンと呼ばれる関節座標系と作業座標系の微小変位関係式を用いることで、作業座標系の加速度信号から関節座標系における加速度信号を算出することが可能である。次に、センサ信号比較部３７を設け、前記シミュレーション部２０の前記加速度情報と、前記センサ信号座標変換部３８の出力信号を入力する。前記シミュレーション部２０の前記加速度情報は、図１３（ｃ）に示すような動作指令による加減速運動の情報が得られる。よって、前記シミュレーション部２０の前記加速度情報と前記センサ信号座標変換部３８の出力信号を比較（ここでは差分）することで、センサ信号修正値を作成する。前記センサ信号修正値は、図１３（ｄ）に示すように、ロボットが上位からの指令により動作する際の加減速運動の情報をキャンセルできており、外部からの衝突状態のみを検出できる。また、検出しきい値も小さく設定することができるため、高感度に衝突検出が行える。
【００５７】
この前記センサ信号修正値に基づき、接触検出部３２では検出しきい値と比較して衝突検出を行う。衝突が検出された場合には、実機部１０のモータ制御部１１にモータ停止命令を送る。モータ制御部１１では、モータ停止命令を受け取ると、衝突検出直前のサーボモータの回転方向と逆方向のトルクで所定の大きさで所定の時間だけ与えて、モータを停止させる。又は図示しないが、前記モータを停止させる以外に、非常停止か、トルク指令をリミットにより制限して柔軟な特性を持たせるか、予め設定された退避動作をさせるか、外部への信号出力を行うなどの処理を選択しても良い。
また、図示しないが、必要に応じて、第３の実施例のように実機部１０の機構部１２の負荷側振動をシミュレーション部でモデル化したり、第４の実施例のようにモータが複数軸ある場合には軸間に作用する干渉トルクをシミュレーション部でモデル化したりすることで、実機の挙動をシミュレーションモデルでも反映して、センサ信号から衝突時の信号成分をより高精度に抽出することができる。
【発明の効果】
以上述べたように、請求項１記載のモータの制御装置によれば、実機部及びシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力し、前記実機部及び前記シミュレーション部の内部状態量を比較して内部状態量比較値を作成し、予め設定された検出しきい値と比較して衝突検出及びモータを停止することにより、演算式が簡単で処理時間が短く、上位からの動作指令がステップ波形のように高周波成分を含んで急激に変化する場合にも対応でき、高感度に衝突を検出できる。
請求項２記載のモータの制御装置によれば、衝突検出後にモータ停止か、非常停止か、トルク制限か、予め設定された退避動作か、外部への信号出力かを選択できることで、作業内容に応じて、より安全に復帰時間も短くできる。
請求項３記載のモータの制御装置によれば、前記検出しきい値は前記動作指令の加減速区間に応じて変化させることにより、シミュレーション部の機構モデル部のモデル化誤差による誤検出を防ぐことができる。
請求項４記載のモータの制御装置によれば、前記接触検出部は、前記シミュレーション部の状態量を検出しきい値とし、前記実機部の状態量と比較して外部との接触を検出することにより、演算処理が簡単でより動作指令の変動に対応して高感度な衝突検出ができる。
請求項５記載のモータの制御装置によれば、前記シミュレーション部の前記機構モデル部を多慣性モデルとし、実機の挙動をシミュレーションモデルで忠実に再現することで、減速機のバネ要素による負荷側の振動を状態量比較値から分離でき、より高感度な衝突検出ができる。
請求項６記載のモータの制御装置によれば、前記モータが複数接続された機械装置の場合、前記シミュレーション部の前記機構モデル部に各モータ間の干渉モデルを備え、実機の挙動をシミュレーションモデルで忠実に再現することで、軸間に作用する干渉トルクを状態量比較値から分離して、より高感度な衝突検出ができる。
請求項７記載のモータの制御装置によれば、前記状態量比較値および検出しきい値を操作画面上に表示し、検出しきい値を調整することにより、作業者が直感的定量的に検出状態を把握でき、検出しきい値を容易に調整することができる。請求項８記載のモータの制御装置によれば、外部との衝突を検出するセンサを備え、前記センサ信号の基準値を前記シミュレーション部の状態量から作成し、前記基準値により修正された前記センサ信号と前記実機部の内部状態量を比較して外部との接触を検出し、衝突検出及びモータを停止することにより、ロボットが上位からの指令により動作する際の加減速運動の情報をキャンセルして、外部からの衝突状態のみを高感度に検出できる。また、衝突検出後にモータ停止か、非常停止か、トルク制限か、予め設定された退避動作か、外部への信号出力かを選択できることで、作業内容に応じて、より安全に復帰時間も短くできる。
請求項９記載のモータの制御方法によれば、実機部及びシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力し、前記実機部及び前記シミュレーション部の内部状態量を比較して内部状態量比較値を作成し、予め設定された検出しきい値と比較して衝突検出及びモータを停止することにより、演算式が簡単で処理時間が短く、指令が急激に変化する場合にも対応でき、特別なセンサなどを用いることなく、高感度に衝突を検出できる。
請求項１０記載のモータの制御方法によれば、外部との衝突を検出するセンサと、実機部及びシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力し、前記センサ信号の基準値を前記シミュレーション部の状態量から作成し、前記基準値により修正された前記センサ信号と前記実機部の内部状態量を比較して外部との接触を検出し、衝突検出及びモータを停止することにより、ロボットが上位からの指令により動作する際の加減速運動の情報をキャンセルして、外部からの衝突状態のみを高感度に検出できる。また、衝突検出後にモータ停止か、非常停止か、トルク制限か、予め設定された退避動作か、外部への信号出力かを選択できることで、作業内容に応じてより安全に復帰時間も短くできる。
【図面の簡単な説明】
【図１】本発明の第１の具体的実施例における基本構成を表す図
【図２】本発明の第１の具体的実施例を表す図
【図３】本発明の第１の具体的実施例における応答波形
【図４】本発明の第２の具体的実施例を表す第１の図
【図５】本発明の第２の具体的実施例における第１の応答波形
【図６】本発明の第２の具体的実施例を表す第２の図
【図７】本発明の第２の具体的実施例を表す第２の図
【図８】本発明の第３の具体的実施例を表す図
【図９】本発明の第４の具体的実施例を表す機構図
【図１０】本発明の第４の具体的実施例を表す図
【図１１】本発明の第５の具体的実施例を表す図
【図１２】本発明の第６の具体的実施例を表す機構図
【図１３】本発明の第６の具体的実施例における応答波形
【図１４】本発明の第６の具体的実施例を表す図
【図１５】従来の制御方式を示す図
【符号の説明】
１０：実機部
１１：モータ制御部
１２：機構部
１３：モータ
２０：シミュレーション部
２１：制御モデル部
２２：機構モデル部
２３：モータモデル
３０：第２のシミュレーション部
３１：状態量比較部
３２：接触検出部
３３：検出しきい値演算部
３４：相互干渉
３５：相互干渉モデル
３６：加速度センサ
３７：センサ信号比較部
３８：センサ信号座標変換部
【図１】
2004364396.tif 000022
【図２】
2004364396.tif 000023
【図３】
2004364396.tif 000024
【図４】
2004364396.tif 000025
【図５】
2004364396.tif 000026
【図６】
2004364396.tif 000027
【図７】
2004364396.tif 000028
【図８】
2004364396.tif 000029
【図９】
2004364396.tif 000030
【図１０】
2004364396.tif 000031
【図１１】
2004364396.tif 000032
【図１２】
2004364396.tif 000033
【図１３】
2004364396.tif 000034
【図１４】
2004364396.tif 000035
【図１５】
2004364396.tif 000036