(57)【要約】【課題】サーボモータの動作について適正に異常検知を行う。【解決手段】  サーボモータ１に接続されたモータ駆動線Ｕ，Ｖ，Ｗからサーボモータの磁極位置信号を取り出す磁極位置算定手段２０と、サーボモータの位置検出手段からの現在位置信号を入力する現在位置信号入力手段３０と、サーボモータの磁極位置信号と現在位置信号の二系統の信号に基づくサーボモータの動作を比較し、不一致が検出された場合に、サーボモータへの電源供給を遮断する停止指令信号を出力する監視手段４０とを有している。【選択図】図１
2008022590.tif 000002
【特許請求の範囲】
【請求項１】
サーボモータに取り付けられた位置検出手段で検出される現在位置により前記サーボモータの制御を行うサーボモータ制御装置に装備されるサーボモータ監視装置であって、
前記サーボモータに接続されたモータ駆動線から前記サーボモータの磁極位置信号を取り出す磁極位置算定手段と、
前記サーボモータに取り付けられた位置検出手段からの現在位置信号を入力する現在位置信号入力手段と、
前記サーボモータの磁極位置信号と現在位置信号の二系統の信号に基づく前記サーボモータの動作を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する監視手段とを有することを特徴とするサーボモータ監視装置。
【請求項２】
前記監視手段は、前記磁極位置信号及び現在位置信号のそれぞれの変化により速度を計算する速度計算手段を有し、
求められた前記各速度を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する機能を有することを特徴とする請求項１記載のサーボモータ監視装置。
【請求項３】
前記磁極位置算定手段は、前記モータ駆動線に接続する磁極位置信号取り出し手段と、取り出した磁極位置信号を、モータ軸角度を特定するための所定の数値に変換する磁極位置信号変換手段とを備えることを特徴とする請求項１又は２記載のサーボモータ監視装置。
【請求項４】
前記磁極位置算定手段は、前記磁極位置信号が示す磁極位置が一巡すると回転量として計数する回転量計数手段を有することを特徴とする請求項３記載のサーボモータ監視装置。
【請求項５】
前記磁極位置取り出し手段は、三本のモータ駆動線にそれぞれが接続された三本の抵抗素子からなる抵抗ブリッジ回路と、当該抵抗ブリッジから取り出した正弦波信号を矩形波に変換するコンパレータ回路とを備えることを特徴とする請求項３又は４記載のサーボモータ監視装置。
【請求項６】
前記磁極位置算定手段は、前記モータ駆動線を流れるモータ駆動電流信号を取得するモータ駆動電流信号入力手段と、前記モータ駆動電流信号から電気角信号成分として磁極位置を取り出す磁極位置信号変換手段とからなることを特徴とする請求項１又は２記載のサーボモータ監視装置。
【請求項７】
前記磁極位置算定手段は、三本あるモータ駆動線の内の少なくとも二本のモータ駆動線のモータ駆動電流を検出する駆動電流検出手段を備え、
前記磁極位置信号変換手段は、前記三本のモータ駆動線に流れるそれぞれのモータ駆動電流が、磁極位置の位相又は当該磁極位置から一定間隔で位相差を生じている位相の関数であることを前提として、前記二つのモータ駆動電流信号から、算術演算により磁極位置を求める演算手段であることを特徴とする請求項６記載のサーボモータ監視装置。
【請求項８】
複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、
前記各サーボモータの現在位置信号からツール位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、
前記ロボットの動作可能領域又は動作禁止領域を記憶する作業エリア記憶手段と、
前記座標位置計算手段により計算されたツール位置あるいは所定部位の位置が、前記動作可能領域の外側又は前記動作禁止領域の内側となった場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する作業エリア監視手段とを有することを特徴とする請求項１から７のいずれか一項に記載のサーボモータ監視装置。
【請求項９】
複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、
前記各サーボモータの現在位置信号からツール位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、
前記ツール位置又はアーム部の所定部位の位置の時系列データの差分により前記ツール位置又はアーム部の移動速度を計算する移動速度計算手段と、
前記ロボットが動作可能な移動速度の上限を記憶する制限速度記憶手段と、
前記計算されたツール位置又は所定部位の移動速度が、動作可能な移動速度の上限を越えた場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する制限速度監視手段とを有することを特徴とする請求項１から７のいずれか一項に記載のサーボモータ監視装置。
【請求項１０】
少なくともいずれかの前記サーボモータについて、当該サーボモータの動作の監視を二つの演算処理部の処理により個別に行うと共に、前記二つの演算処理部での演算処理結果が不一致の場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する演算結果照合手段を有することを特徴とする請求項８又は９記載のサーボモータ監視装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、産業用ロボットに使用するサーボモータの動作監視装置に関する。
【背景技術】
【０００２】
一般的に産業用ロボットは、ロボット本体に多関節からなるアームが連結され、アームの先端にはツールが設けられて構成されている。ロボット本体にはサーボモータが取り付けられており、このサーボモータには位置検出手段が取り付けられている。この位置検出手段により検出されたツールの現在位置情報に基づいて、サーボ制御部によりツールの位置、アームの位置、アームの角度等が制御されている。
このロボットの動作監視をする方法として、例えば、本願特許出願人の特許出願発明である先行技術、即ち、位置制御部から受け取った指令位置と、位置検出手段から入力される現在位置との差を、指令速度に対応した閾値と比較して異常を検出する方法が開示されている（例えば、特許文献１参照）。
また更に、上記従来技術を発展させて、ロボットアームの動作をモデル化し、位置制御部からの指令位置から実際位置を推定し、推定実際位置と位置検出手段から入力される現在位置とを比較することで、より高精度に異常検出を行う方法が開示されている（例えば、特許文献２又は３参照）。
【特許文献１】特開平４−６７２０９号公報
【特許文献２】特開平１０−３１５１７３号公報
【特許文献３】特開２００４−１８１５４３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、これらの指令位置と現在位置との差で動作監視する方法は、指令位置の入力を受けて、サーボ制御部はモータを駆動し、結果として現在位置がその通りに動作したかを監視する用途には適するが、現在位置が許容範囲内にあることを監視する用途においては不適である。
即ち、近年のサーボ制御部はマイコンが用いられ、その制御を自由に切り換えることができることとなってきており、例えば、指令位置が階段状に変化する場合に、サーボモータやロボットが容易に追従できるようにサーボ制御部で加減速の変化をなだらかにするカーブを生じるように補正制御を行ったり、現在位置の変化を見て、アンダーシュートやオーバシュートを予測して指令位置に補正をかけて動作させる、あるいは動作の一部分でモータが一定速度や一定出力トルクで動作するように切り換えて動作させることをサーボ制御器で行う場合、指令位置と現在位置の差で現在位置の値の妥当性を評価することはできないという問題があった。
【０００４】
また、現在位置の監視の万全を図るために、独立した信号に対して監視の二重化の考え方を適用することが望ましい。二つの位置検出手段を用いて現在位置信号を二重化すると、独立性の証明の煩雑さは無いが、二つの位置検出手段を用いることで、余計なコストが必要となり、位置検出手段の故障率も２倍となり、装置の信頼性低下も招く。
本発明は、上記の課題を解決するためになされたものであり、位置検出手段を二重にすることなく、サーボ制御部の構成の複雑さに影響もされることなく、サーボモータの動作の異常検知を実現するものである。
【課題を解決するための手段】
【０００５】
請求項１記載の発明は、サーボモータに取り付けられた位置検出手段で検出される現在位置により前記サーボモータの制御を行うサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記サーボモータに接続されたモータ駆動線から前記サーボモータの磁極位置信号を取り出す磁極位置算定手段と、前記サーボモータに取り付けられた位置検出手段からの現在位置信号を入力する現在位置信号入力手段と、前記サーボモータの磁極位置信号と現在位置信号の二系統の信号に基づく前記サーボモータの動作を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する監視手段とを有する、という構成を採っている。
【０００６】
請求項２記載の発明は、請求項１記載の発明と同様の構成を備えると共に、前記監視手段は、前記磁極位置信号及び現在位置信号のそれぞれの変化により速度を計算する速度計算手段を有し、求められた前記各速度を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する機能を有する、という構成を採っている。
【０００７】
請求項３記載の発明は、請求項１又は２記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、前記モータ駆動線に接続する磁極位置信号取り出し手段と、取り出した磁極位置信号を、モータ軸角度を特定するための所定の数値に変換する磁極位置信号変換手段とを備える、という構成を採っている。
【０００８】
請求項４記載の発明は、請求項３記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、前記磁極位置信号が示す磁極位置が一巡すると回転量として計数する回転量計数手段を有する、という構成を採っている。
【０００９】
請求項５記載の発明は、請求項３又は４記載の発明と同様の構成を備えると共に、前記磁極位置取り出し手段は、三本のモータ駆動線にそれぞれが接続された三本の抵抗素子からなる抵抗ブリッジ回路と、当該抵抗ブリッジから取り出した正弦波信号を矩形波に変換するコンパレータ回路とを備える、という構成を採っている。
【００１０】
請求項６記載の発明は、請求項１又は２記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、前記モータ駆動線を流れるモータ駆動電流信号を取得するモータ駆動電流信号入力手段と、前記モータ駆動電流信号から電気角信号成分として磁極位置を取り出す磁極位置信号変換手段とからなる、という構成を採っている。
【００１１】
請求項７記載の発明は、請求項６記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、三本あるモータ駆動線の内の少なくとも二本のモータ駆動線のモータ駆動電流を検出する駆動電流検出手段を備え、前記磁極位置信号変換手段は、前記三本のモータ駆動線に流れるそれぞれのモータ駆動電流が、磁極位置の位相又は当該磁極位置から一定間隔で位相差を生じている位相の関数であることを前提として、前記二つのモータ駆動電流信号から、算術演算により磁極位置を求める演算手段である、という構成を採っている。
【００１２】
請求項８記載の発明は、請求項１から７のいずれか一項に記載の発明と同様の構成を備えると共に、複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記各サーボモータの現在位置信号からツール位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、前記ロボットの動作可能領域又は動作禁止領域を記憶する作業エリア記憶手段と、前記座標位置計算手段により計算されたツール位置あるいは所定部位の位置が、前記動作可能領域の外側又は前記動作禁止領域の内側となった場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する作業エリア監視手段とを有する、という構成を採っている。
【００１３】
請求項９記載の発明は、請求項１から７のいずれか一項に記載の発明と同様の構成を備えると共に、複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記各サーボモータの現在位置信号からツール位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、前記ツール位置又はアーム部の所定部位の位置の時系列データの差分により前記ツール位置又はアーム部の移動速度を計算する移動速度計算手段と、前記ロボットが動作可能な移動速度の上限を記憶する制限速度記憶手段と、前記計算されたツール位置又は所定部位の移動速度が、動作可能な移動速度の上限を越えた場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する制限速度監視手段とを有する、という構成を採っている。
【００１４】
請求項１０記載の発明は、請求項８又は９記載の発明と同様の構成を備えると共に、少なくともいずれかの前記サーボモータについて、当該サーボモータの動作の監視を二つの演算処理部の処理により個別に行うと共に、前記二つの演算処理部での演算処理結果が不一致の場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する演算結果照合手段を有する、という構成を採っている。
【発明の効果】
【００１５】
請求項１記載の発明では、サーボモータの制御時において、監視手段が、モータ駆動線からサーボモータの磁極位置信号を取り出して磁極位置とサーボモータの位置検出手段からの現在位置とを比較し、異常の発生によるサーボモータの停止判断を行う。このため、例えば指令位置等のような目標値に対してサーボモータの制御の過程の途中で補正が行われるような場合であっても、当該補正が反映された磁極位置信号が現在位置信号と比較されることとなり、前記補正を行う場合のようなサーボ制御部の構成の複雑さに影響もされることなく、当該補正の影響を受けることなく、サーボモータそのものの異常や位置検出手段の異常や監視装置そのものの異常などの発生を検知し、サーボモータの適正な停止を行うことが可能となる。また、位置検出手段を二つ設けることなく、サーボモータの磁極位置信号と現在位置信号の二系統の信号に基づくサーボモータの動作を比較して異常検知を行うことが可能となる。
なお、本発明は、サーボモータの指令位置に基づくフィードバック制御を行うサーボモータ制御装置に適用する場合に限らず、例えば、速度入力アンプをサーボ制御部としたり、トルク入力アンプをサーボ制御部として、現代制御理論に基づくインピーダンス制御を行うサーボモータ制御装置等に適用しても良い。
【００１６】
請求項２記載の発明では、監視手段が磁極位置信号に基づく速度と現在位置に基づく速度との比較からサーボモータの速度監視を行うことから、サーボモータの動作位置に限らず速度の観点からも異常検知を行うことができ、サーボモータのさらなる適正な異常検知を行うことが可能となる。
なお、この請求項２記載の発明は、位置比較と速度比較の双方で異常が検知された場合に停止指令信号を出力する構成としても良いが、位置比較と速度比較のいずれか一方でも異常が検知された場合には停止指令信号を出力するように構成することがより望ましい。
【００１７】
請求項３記載の発明では、磁極位置信号をモータ軸角度を特定するための所定の数値に変換することから、アナログ信号による比較を行う場合と異なり、外乱によるノイズ等の影響を抑制し、異常検知をより適正且つ安定的に行うことが可能となる。
なお、モータ軸角度を特定するための所定の数値とは、例えば、軸角度そのものの値を求めても良いし、軸の一周を複数区間に分割し、いずれの区間に属するかを数値で表しても良い。
【００１８】
請求項４記載の発明では、回転量計数手段を有することから、軸位置のみならず、回転数の一致をも判断して異常検知を行うことができ、サーボモータのさらなる適正な異常検知を行うことが可能となる。
請求項５記載の発明は、抵抗ブリッジ回路とコンパレータ回路とを用いるので装置の簡易化、コスト低減を図ることができると共に、矩形波を使用して判断を行うことから、処理情報量の低減を図り、処理の迅速化を図ることが可能となる。
【００１９】
請求項６記載の発明は、磁極位置信号変換手段によりモータ駆動電流信号から電気角信号成分として磁極位置を取り出すことから、軸位置を細分化された角度単位で検出することができ、サーボモータのさらなる適正な異常検知を行うことが可能となる。
請求項７記載の発明は、モータ駆動電流が磁極位置の位相の関数であることに着目して少なくとも二つのモータ駆動電流信号から算術演算により磁極位置を求めるため、容易な演算処理で異常検知が可能となり、異常検知のための構成の簡易化、処理の迅速化を図るることが可能となる。
【００２０】
請求項８記載の発明は、サーボモータの動作異常検知に加えて、ロボットの予定外の領域への動作を防止することができ、ロボットの動作制御について適正化を図ることが可能となる。
請求項９記載の発明は、サーボモータの動作異常検知に加えて、ロボットの過剰速度での動作を防止することができ、ロボットの動作制御について適正化を図ることが可能となる。
請求項１０記載の発明は、サーボモータの動作の監視を二つの演算処理部の処理により個別に行うと共に、それぞれの演算処理結果が不一致の場合に各サーボモータへの動力供給を遮断する指令信号を出力するため、位置検出手段を二つ設けることなく、サーボモータの動作の監視を二つの系統で行うことが可能となると共に、いずれか一方の監視系統そのものに異常が発生した場合にも、ロボットの動作を停止させることができ、サーボモータ及びロボットのさらなる適正な異常検知を行うことが可能となる。
【発明を実施するための最良の形態】
【００２１】
（第一の実施形態：サーボモータ制御装置）
以下、図面をもとに本発明の第一の実施形態の内容を説明する。
図１は、本願発明であるサーボモータ監視装置１０が装備されたサーボモータ制御装置１００の構成図である。
サーボモータ制御装置１００は、指令位置の時系列データを生成する位置制御部１０１と、時系列データが示す指令位置とサーボモータ１に併設された位置検出手段２により検出される現在のサーボモータ１の現在位置との偏差に応じてサーボモータ１を駆動するモータ駆動電流を出力するサーボ制御部１２０と、サーボモータ１の動作を監視して所定条件下でモータ動力供給源（電源）３とサーボ制御部１２０との間に設けられた緊急停止スイッチ４に対して電源供給を遮断する停止信号を出力するサーボモータ監視装置１０とを備えている。
【００２２】
（サーボ制御部）
図２は、上記サーボ制御部１２０とにより構成されるフィードバック制御系を示すブロック図である。
上記サーボモータ制御装置１００の制御系では、位置制御、速度制御、電流制御の各々はＰＩＤ（比例−積分−微分）制御で構成される。
即ち、サーボ制御部１２０は、位置制御部１０１からの指令位置に対して位置検出手段２からの現在位置を減算して位置偏差を求める減算器１２１と、この位置偏差をゼロとするように指令速度を求める位置制御部１２２と、現在位置信号を微分して現在速度信号を求める微分器１２３と、指令速度信号から現在速度信号を減算して速度偏差を求める減算器１２４と、速度偏差がゼロとなるように指令電流信号（トルク指令）を求める速度制御部１２５と、指令電流信号から後述する現在電流信号を減算して電流偏差を求める減算器１２６と、電流偏差がゼロとなるように電流制御を行う電流制御部１２７と、サーボモータ１が備える三つのモータ巻き線に通電する三つのモータ駆動線Ｕ，Ｖ，Ｗに電流を分配する転流制御部１２８と、モータ駆動線Ｕ，Ｖ，Ｗに対して駆動レベルの大電流に変換して通電するドライバ１２９と、モータ駆動線Ｕ，Ｖに流れる電流信号を検出して合成した現在電流信号を減算器１２６に入力する電流合成部１３０とを備えている。
即ち、サーボ制御部１２０のブロック構成は、ＰＩＤ制御によるフィードバック制御を行うための基本的且つ一般的な構成となっている。
【００２３】
サーボ制御部１２０は、上記各構成を具備するマイコンで構成されている。また、サーボ制御部１２０は、例えば、階段状の指令位置の入力に対して速度変化を緩やかとなるように加減速のカーブを作る補正を加えたり、現在位置の変化からアンダーシュートやオーバシュートの発生を予測して指令位置に補正をかけたり、あるいは動作の一部分でモータが一定速度や一定出力トルクで動作するように切り換えて動作させる補正処理部をマイコン内に加えても良い。
【００２４】
（サーボモータ）
図３は制御対象となるサーボモータ１を模式的に示した説明図である。サーボモータ１は、回転を行う回転子１１と、回転子１１に固定装備された永久磁石１２と、モータ駆動線Ｕ，Ｖ，Ｗにそれぞれ接続された電機子巻き線１３，１４，１５とを備え、三本の電機子巻き線１３，１４，１５にモータ駆動電流を通電することにより回転を行ういわゆるブラシレスＤＣモータである。三本の電機子巻き線１３，１４，１５は、実際には120°間隔で回転子１１及び永久磁石１２を取り囲むように配置されている。
磁石１２の取り付けられた回転子１１はサーボモータ１の出力軸であり、この軸に位置検出手段２（例えばレゾルバや光学式／磁気式エンコーダの位置検出器）が取り付けられている。
【００２５】
（サーボモータ監視装置）
図４はサーボモータ監視装置１０のより詳細な構成を示す構成図である。サーボモータ監視装置１０は、モータ駆動線Ｕ，Ｖ，Ｗからサーボモータ１の磁極位置信号を取り出す磁極位置算定手段２０と、サーボモータ１に取り付けられた位置検出手段２からの現在位置信号の入力を受ける現在位置信号入力手段３０と、サーボモータ１の磁極位置信号と現在位置信号の二系統の信号で当該サーボモータ１の動作を比較対照し、不一致が検出された場合に、モータへの動力供給を遮断する信号を出力する監視手段４０とを備えている。
【００２６】
磁極位置算定手段２０は、モータ駆動線Ｕ，Ｖ，Ｗから磁極位置信号を取り出す磁極位置信号取り出し手段２１と、取り出した磁極位置信号をモータ軸角度の特定のための所定数値に変換する磁極位置信号変換手段２６と、磁極位置信号が示す磁極位置が一巡するたびに回転量として計数する回転量計数手段２７と、サーボモータ１の加速時と減速時の磁極位置信号を識別するための基準磁極位置を現在位置信号に基づいて設定する基準磁極位置設定手段２８とを備えている。
【００２７】
磁極位置信号取り出し手段２１は、サーボモータ１のモータ駆動線Ｕ，Ｖ，Ｗに別々に一端部が接続された抵抗素子２２ａ，２２ｂ，２２ｃの他端部をまとめて結線してなる抵抗ブリッジ回路２２と、各抵抗素子２２ａ，２２ｂ，２２ｃを流れる信号の波形をそれぞれ二値化するコンパレータ回路２３，２４，２５とを備えている。
【００２８】
図５（Ａ）はサーボモータ１の回転子１１を回転した場合に各電機子巻き線１３，１４，１５に出てくる信号を示すものである。この場合には、サーボモータ１は発電機として動作し、それぞれの電機子巻き線１３，１４，１５には120゜ずつ位相のずれた正弦波信号が出てくる。回転速度が速くすると、正弦波の振幅は大きくなり、正弦波の周波数も速度に比例して大きくなる。また、サーボモータ１が駆動してトルク出力を行う場合には、上記の発電機として動作する誘起電圧波形と一致する位相関係及び電流分配比でドライバ１２９から各モータ駆動線Ｕ，Ｖ，Ｗに通電することにより、サーボモータ１を回転させることができる。
こうしたモータの特性については、例えば技術文献「ACサーボシステムの理論と設計の実際」（総合電子出版社1990年）に詳しく記載されているので、詳細な記述を省略する。
【００２９】
図５（Ｂ）は、図５（Ａ）の正弦波のアナログ信号を各コンパレータ回路２３，２４，２５により振幅に影響されないように二値化したものである。
磁極位置信号変換手段２６は、各コンパレータ回路２３，２４，２５から出力されるＵＶＷ３本分の３ビット信号を、回転角度に対応する二進数に変換する（グレイ−バイナリ変換として良く知られる変換方式である）。即ち、高いレベルを1、低いレベルを0で表現すると、磁極位置信号変換手段２６は、0又は1の信号をＵＶＷの順に並べてなる二進数表記101から回転に伴って、100，110，010，011，001，101，…のように順次出力を行う。これにより、正弦波一周期分を六分割すると共に回転子１１が六分割されたいずれの位置にあるかを認識することができる。
【００３０】
なお、サーボモータ１の運転形態で、モータ動力線に現れる図５（Ａ）の波形の持つ意味は次のようになる。ドライバ１２９からモータ駆動動力電源を供給する場合には、駆動電流の波形が現れ、駆動側の指令する磁極位置を検出していることになる。そして、駆動における加速時と減速時とでは、電流の正負が反転した180゜違いの信号となるため、これを判別する必要がある。
この為、基準磁極位置設定手段２８は、現在位置信号入力手段３０から入力された現在位置に基づいてＵＶＷ各相の基準磁極位置信号を設定して磁極位置信号変換手段２６に入力している。即ち、基準磁極位置設定手段２８では、サーボモータ１が位置検出手段２により検出された現在位置である場合に各モータ駆動線Ｕ，Ｖ，Ｗに現れるであろう駆動電流の波形に基づく二進数の値（101，100，110，010，011，001のいずれか）を求め、これを磁極位置信号変換手段２６に入力する。
一方、磁極位置信号変換手段２６では、磁極位置信号取り出し手段２１から磁極位置に基づく二進数の値（αとする）が入力されると、当該値αと位相が180°違いの反転磁極位置に基づく二進数の値（βとする）を算出し、磁極位置に基づく値αと反転磁極位置に基づく値βのそれぞれを基準磁極位置設定手段２８から入力された現在位置に基づく値（γとする）と比較し、値γに対して値αと値βのいずれの方が一致する桁数が多いか判定が行われる。そして、値γに対して値αがより一致する場合にはサーボモータ１は加速中と判断されて当該値αが磁束位置として監視手段４０に入力される。また、値γに対して値βがより一致する場合にはサーボモータ１は減速中と判断されて当該値βが磁束位置として監視手段４０に入力される。
【００３１】
なお、駆動電流を流さず、例えば重力等の外力により回転している場合、あるいは慣性モーメントにより一定速で回転している場合には、発電機として動作するモータの誘起電圧波形により、磁極位置を検出する。
更に、回転数計数手段２７は、磁極位置信号変換手段２６から入力される二進数の信号が、正弦波の次の周期の切り換わり目を示す001から101に変化するときに回転量のカウント値を＋１し、101から001に変化する時に回転量のカウント値を−１することにより、正弦波の複数周期分、即ち、サーボモータ１の回転回数を計数する事ができる。
【００３２】
現在位置信号入力手段３０は、サーボモータ１の位置検出手段２が検出する現在位置信号の入力を受けてサーボモータ１の回転数をカウントすると共に、基準磁極位置設定手段２８に対して現在位置信号を入力し、位置比較待避手段４３にカウントした回転数と現在位置信号を入力する。
【００３３】
監視手段４０は、磁極位置算定手段２０から入力される磁極位置信号及び回転量に基づいてサーボモータ１の動作速度を求める第一の速度計算手段４１と、現在位置信号入力手段３０から入力される現在位置信号及び回転量に基づいてサーボモータ１の動作速度を求める第二の速度計算手段４２と、磁極位置算定手段２０から入力される磁極位置信号及び回転量と現在位置信号入力手段３０から入力される現在位置信号及び回転量とを比較して予め設定された許容値以上の差を生じている場合に位置異常信号を出力する位置比較対比手段４３と、第一の速度計算手段４１で求められたサーボモータ１の動作速度と第二の速度計算手段４２で求められたサーボモータ１の動作速度とを比較して予め設定された許容値以上の差を生じている場合に速度異常信号を出力する速度比較対比手段４４と、位置異常信号と速度異常信号のいずれか一方又は双方の入力があると緊急停止スイッチ４に対して電源供給を遮断する停止信号を入力するＯＲ回路４５とを備えている。
【００３４】
上記構成により、サーボモータ監視装置１０では、モータ駆動線Ｕ，Ｖ，Ｗで観測した位置信号と、現在位置信号入力手段３０から入力した位置検出手段４の検出による現在位置とを比較対比し、不一致の場合に位置異常としてドライバ１２９への電源供給を遮断する。
また、同様にして、モータ駆動線Ｕ，Ｖ，Ｗで観測した位置信号に基づく速度と、現在位置信号入力手段３０から入力した現在位置に基づく速度とを比較対比し、不一致の場合に速度異常としてドライバ１２９への電源供給を遮断する。位置異常或いは速度異常のいずれが検出された場合でもドライバ１２９への電源供給を遮断する。
【００３５】
サーボモータ制御装置１００は、上記構成により、サーボ制御部１２０について指令位置に対する補正処理などの新た構成の影響を受けずに、サーボモータ１の動作を監視することができ、低コストかつ高信頼性で確実に異常発生の検出を行うことが可能となる。
【００３６】
なお、本実施形態に示したサーボモータ１は、回転子１１に永久磁石１２を用いた例を図示したが、サーボモータ１の形式に限定はない。例えば、永久磁石に限らず電磁石を用い、回転トランスを経由して励磁するサーボモータでも良いし、あるいは、固定側に永久磁石を配置し、回転子に電機子巻き線を用いるサーボモータでも良い。さらには、固定側の永久磁石を直線状に並べた、いわゆるリニアモータであっても良い。いずれの場合も、上記監視装置を適用することが可能である。
【００３７】
（第二の実施形態：サーボモータ監視装置）
第二の実施形態であるサーボモータ監視装置１０Ａを図６に示す。このサーボモータ監視装置１０Ａは、磁極位置算定手段２０Ａ以外は前述したサーボモータ監視装置１０と重複する構成を備えており、また、図１に示すサーボモータ制御装置１００に装備することが可能である。
かかるサーボモータ監視装置１０Ａについてサーボモータ監視装置１０と異なる磁極位置算定手段２０Ａについてのみ説明することとする。
【００３８】
磁極位置算定手段２０Ａは、モータ駆動線Ｕ，Ｖ，Ｗの中の少なくとも二本に直列に挿入された低抵抗のシャント抵抗２１Ａ，２２Ａと、シャント抵抗２１Ａ，２２Ａの電圧降下からモータ駆動線Ｕ，Ｖ，Ｗに流れる電流値を求めるモータ駆動電流信号入力手段２３Ａと、モータ駆動電流信号入力手段２３Ａで求められた各モータ駆動線の電流値から磁極位置θを算出する磁極位置信号変換手段２６Ａと、磁極位置θの周期の切り換わり目をカウントしてサーボモータ１の回転回数を計数する回転量計数手段２７Ａとを備えている。そして、上記シャント抵抗２１Ａ，２２Ａは駆動電流検出手段を構成している。
【００３９】
サーボモータ１の三つの電機子巻き線１３，１４，１５が回転子１１の周囲で120°間隔で配置されている場合、各モータ駆動線Ｕ，Ｖ，Ｗを流れるモータ駆動電流ＩＵ，ＩＶ，ＩＷは、磁極位置をθとすると、以下の関係を生じる。
ＩＵ＝Ｋ・Ｓｉｎθ
ＩＶ＝Ｋ・Ｓｉｎ（θ＋１２０゜）
ＩＷ＝Ｋ・Ｓｉｎ（θ＋２４０゜）
従って、モータ駆動電流信号入力手段２３Ａでは三つのモータ駆動電流ＩＵ，ＩＶ，ＩＷの内の二以上を検出し、磁極位置信号変換手段２６Ａでは、モータ駆動電流信号から、算術演算により磁極位置θを求めるものである。
この場合も、前述した磁極位置信号変換手段２６の場合と同様に、サーボモータ１の加速時と減速時とでは流す電流の向きが反転した180゜違いの角度となる。このため、現在位置信号入力手段３０から得られた現在位置から基準磁極位置を求め、磁極位置信号変換手段２６Ａでは、計算により求めた角度とこれと180゜違いの角度とを現在位置から求めた基準磁極位置と比較し、近い方の角度を選択する。
【００４０】
そして、磁極位置信号変換手段２６Ａと回転量計数手段２７Ａとにより求められた磁極位置及び回転数が位置比較対比手段４３に入力され、その後の処理はサーボモータ監視装置１０と同様である。
これにより、サーボモータ監視装置１０Ａは、前述したサーボモータ監視装置１０と同様の効果を奏することが可能となる。
【００４１】
なお、磁極位置信号取り出し手段であるシャント抵抗２１Ａ，２２Ａはモータ駆動線Ｕ，Ｖ，Ｗから電流値を取り出す他のいかなる手段を用いても良い。例えば、モータ駆動線Ｕ，Ｖ，Ｗの少なくとも二本にホール素子を設けて電流値を求めても良い。また、上記例示ではモータ駆動線Ｕ，Ｖ，Ｗ農地の二本のみについて電流値を検出したが、三本全てから電流値の検出を行って、二つの組み合わせから求まる三つの磁極位置の平均値を採用しても良い。
【００４２】
（第三の実施形態：ロボット制御装置）
前述したサーボモータ制御装置１００を
図７は第三の実施形態であるロボット制御装置２００の構成図である。かかるロボット制御装置２００は、ロボット１２０に設けられた複数のサーボモータ１の制御に対して、前述したサーボモータ制御装置１００の基本構成を備える新たなサーボモータ制御装置を適用するものであり、サーボモータ制御装置１００について既に言及した構成については同一の符号を付して重複する説明は省略するものとする。
【００４３】
ロボット１２０は、土台となるベース１２１と、関節１２３で連結された複数のアーム１２２と、各関節１２３ごとに設けられた駆動源としてのサーボモータ１（図７では一つのみ図示）と、各サーボモータ１の軸角度をそれぞれ検出する位置検出手段（エンコーダ）２と、連結されたアーム１２２の最先端部に設けられたツール１２４とを備えている。
上記ロボット１２０は、六つの関節１２３を具備したいわゆる多関節型ロボットであり、各関節１２３に連結された全アーム１２２の最先端部（以下、単に「アーム先端部」という）に位置するツール１２４を任意の位置に位置決めし任意の姿勢を取らせることが可能となっている。
【００４４】
ロボット制御装置２００は、六つのサーボモータ１に対してフィードバック制御を行うサーボ制御部１２０を備えており、位置制御部１０１は、例えばツール１２４を目標位置まで移動させるための移動軌跡と当該移動軌跡を描いてロボット１２０を動作させるための各サーボモータ１に対する時系列的な指令位置を算出し、サーボ制御部１２０に入力を行う。
さらに、ロボット制御装置２００は、各サーボモータ１の監視を行うサーボモータ監視装置３００を備えている。
【００４５】
サーボモータ監視装置３００は、各サーボモータ１の磁極位置及び回転数を求める磁極位置算定手段２０と、各サーボモータ１の位置検出手段２から現在位置及び回転数を取得する現在位置入力手段３０と、磁極位置算定手段２０及び現在位置入力手段３０の出力に基づいて各サーボモータ１の動作を監視する二つのＣＰＵ３１０，３２０と、二つのＣＰＵ３１０，３２０の処理結果が異なる場合に全てのサーボモータ１の緊急停止スイッチ４を遮断させる停止信号を出力する演算結果照合手段３６０とを備えている。
【００４６】
ＣＰＵ３１０，３２０は、以下に説明する同一の構成を備え、且つ当該各構成は図示しないメモリ内に格納された各種プログラムの実行により実現するものである。
即ち、ＣＰＵ３１０，３２０は、いずれも、監視手段４０と、各サーボモータ１の現在位置からツール１２４の位置を計算する座標位置計算手段３３１と、ロボット１２０の動作可能領域又は動作禁止領域を記憶する作業エリア記憶手段３３２と、座標位置計算手段３３１により計算されたツール位置が、動作可能領域の外側又は動作禁止領域の内側となった場合に、各サーボモータ１への電源供給を遮断する指令信号を出力する作業エリア監視手段３３３と、各サーボモータ１の現在位置信号からツール位置の所定部位の位置を計算する座標位置計算手段と、ツール位置の時系列データの差分（各サーボモータ１の現在位置から求まるツール位置と前回検出された各サーボモータ１の現在位置から求まるツール位置との差分）によりツール位置の移動速度を計算する移動速度計算手段３４１と、ロボット１２０のツール位置の動作可能な移動速度の上限を記憶する制限速度記憶手段３４２と、計算されたツール位置の移動速度が動作可能な移動速度の上限を越えた場合に各サーボモータ２への電源供給を遮断する指令信号を出力する制限速度監視手段３４３と、監視手段４０と作業エリア監視手段３３３と制限速度監視手段３４３のいずれか一つ以上から電源供給を遮断する指令信号が出力されると、それを演算結果照合手段３６０に入力するＯＲ回路３５０とを備えている。
【００４７】
そして、各ＣＰＵ３１０，３２０のそれぞれにおいて、監視手段４０により異常検知が行われた場合、或いはロボット１２０の動作が予定しない領域に及んだ場合、或いはロボット１２０の動作速度が制限速度を超えた場合のいずれか一つでも検知されると、演算結果照合手段３６０に対して各サーボモータ１の電源供給を遮断させる指令信号が入力される。
【００４８】
一方、演算結果照合手段３６０は、ＣＰＵ３１０，３２０の双方から各サーボモータ１の電源供給を遮断させる指令信号が入力されると、それを緊急停止スイッチ４に入力し、全てのサーボモータ１を停止させる。また、ＣＰＵ３１０，３２０のいずれか一方からのみ電源供給を遮断させる指令信号が入力されると、二つのＣＰＵ３１０，３２０における異常検知の判断に食い違いを生じたこととなるので、やはり、電源供給を遮断させる指令信号を緊急停止スイッチ４に入力し、全てのサーボモータ１を停止させる。
【００４９】
なお、上記サーボ監視装置３００では、ロボット１２０のツール位置及びその移動速度について監視を行っているが、ツール位置に限らず、ロボット１２０について予め定められた所定部位（例えばモータ取り付け部の出っ張り部位等の他の物体との接触可能性がある部位）や、各アーム部１２２を例えば円筒形状等の近似形状で近似してなる当該近似形状の外面位置について監視を行うように構成しても良い。
【００５０】
上記ロボット制御装置２００では、監視手段４０やロボットの動作領域或いは制限速度に関する演算そのものを２つのＣＰＵ３１０，３２０で二重化し、２つのＣＰＵ３１０，３２０の演算結果を照合することで、サーボモータ監視装置３００の二重化を行っている。
これにより、サーボ制御部１２０について指令位置に対する補正処理などの新た構成の影響を受けずに、サーボモータ１の動作を監視することができ、複数サーボモータ１を使用したロボット１２０が設定作業エリアを越えようとしたり、或いは禁止動作エリアに侵入しようとすることを、低コストかつ高信頼性で確実に検出することが可能となる。
【図面の簡単な説明】
【００５１】
【図１】本願発明であるサーボモータ監視装置が装備されたサーボモータ制御装置の構成図である。
【図２】サーボ制御部とにより構成されるフィードバック制御系を示すブロック図である。
【図３】制御対象となるサーボモータを模式的に示した説明図である。
【図４】サーボモータ監視装置のより詳細な構成を示す構成図である。
【図５】図５（Ａ）はサーボモータの回転子を回転した場合に各電機子巻き線に出てくる信号を示す線図であり、図５（Ｂ）は図５（Ａ）の正弦波のアナログ信号を各コンパレータ回路により振幅に影響されないように二値化したものを示す線図である。
【図６】第二の実施形態であるサーボモータ監視装置を示す構成図である。
【図７】第三の実施形態であるロボット制御装置の構成図である。
【符号の説明】
【００５２】
１  サーボモータ
２  位置検出手段
１０，３００  サーボモータ監視装置
２０  磁極位置算定手段
２１  磁極位置信号取り出し手段
２１Ａ，２２Ａ  シャント抵抗（駆動電流検出手段）
２２  抵抗ブリッジ回路
２２ａ，２２ｂ，２２ｃ  抵抗素子
２３，２４，２５  コンパレータ回路
２３Ａ  モータ駆動電流信号入力手段
２６，２６Ａ  磁極位置信号変換手段
２７，２７Ａ  回転量計数手段
３０  現在位置信号入力手段
４０  監視手段
４１  第一の速度計算手段
４２  第二の速度計算手段
１００  サーボモータ制御装置
１２０  ロボット
１２２  アーム
１２３  関節
１２４  ツール
２００  ロボット制御装置
３１０，３２０  ＣＰＵ（演算処理部）
３３１  座標位置計算手段
３３２  作業エリア記憶手段
３３３  作業エリア監視手段
３４１  移動速度計算手段
３４２  制限速度記憶手段
３４３  制限速度監視手段
３６０  演算結果照合手段
Ｕ，Ｖ，Ｗ  モータ駆動線
【図１】
2008022590.tif 000003
【図２】
2008022590.tif 000004
【図３】
2008022590.tif 000005
【図４】
2008022590.tif 000006
【図５】
2008022590.tif 000007
【図６】
2008022590.tif 000008
【図７】
2008022590.tif 000009