WO2008007541A1

WO2008007541A1 - Dispositif de contrôle d'un servomoteur

Info

Publication number: WO2008007541A1
Application number: PCT/JP2007/062784
Authority: WO
Inventors: Mitsuharu Hamahata
Original assignee: Nachi-Fujikoshi Corp.; Daihen Corporation
Priority date: 2006-07-10
Filing date: 2007-06-26
Publication date: 2008-01-17
Also published as: EP2040367A4; US20090309531A1; EP2040367A1; US8072173B2; JP2008022590A

Description

明細書

サーボモータ監視装置

技術分野

[0001] 本発明は、産業用ロボットに使用するサーボモータの動作監視装置に関する。

背景技術

[0002] 一般的に産業用ロボットは、ロボット本体に多関節からなるアームが連結され、ァームの先端にはツールが設けられて構成されてレ、る。ロボット本体にはサーボモータが取り付けられており、このサーボモータには位置検出手段が取り付けられている。この位置検出手段により検出されたツールの現在位置情報に基づいて、サーボ制御部によりツールの位置、アームの位置、アームの角度等が制御されている。

このロボットの動作監視をする方法として、例えば、本願特許出願人の特許出願発明である先行技術、即ち、位置制御部から受け取った指令位置と、位置検出手段から入力される現在位置との差を、指令速度に対応した閾値と比較して異常を検出する方法が開示されている (例えば、特許文献 1参照)。

また更に、上記従来技術を発展させて、ロボットアームの動作をモデル化し、位置制御部からの指令位置から実際位置を推定し、推定実際位置と位置検出手段から入力される現在位置とを比較することで、より高精度に異常検出を行う方法が開示されている（例えば、特許文献 2又は 3参照）。

特許文献 1 :特開平 4一 67209号公報

特許文献 2 :特開平 10— 315173号公報

特許文献 3 :特開 2004— 181543号公報

発明の開示

発明が解決しょうとする課題

[0003] しかしながら、これらの指令位置と現在位置との差で動作監視する方法は、指令位置の入力を受けて、サーボ制御部はモータを駆動し、結果として現在位置がその通りに動作した力 ^監視する用途には適するが、現在位置が許容範囲内にあることを監視する用途においては不適である。即ち、近年のサーボ制御部はマイコンが用いられ、その制御を自由に切り換えることができることとなってきており、例えば、指令位置が階段状に変化する場合に、サーボモータやロボットが容易に追従できるようにサーボ制御部で加減速の変化をなだらかにするカーブを生じるように補正制御を行ったり、現在位置の変化を見て、アンダ一シュートやオーバシュートを予測して指令位置に補正をかけて動作させる、あるレヽは動作の一部分でモータが一定速度や一定出力トノレクで動作するように切り換えて動作させることをサーボ制御器で行う場合、指令位置と現在位置の差で現在位置の値の妥当性を評価することはできないという問題があった。

[0004] また、現在位置の監視の万全を図るために、独立した信号に対して監視の二重化の考え方を適用することが望ましい。二つの位置検出手段を用いて現在位置信号を二重化すると、独立性の証明の煩雑さは無いが、二つの位置検出手段を用いることで、余計なコストが必要となり、位置検出手段の故障率も 2倍となり、装置の信頼性低下も招く。

本発明は、上記の課題を解決するためになされたものであり、位置検出手段を二重にすることなぐサーボ制御部の構成の複雑さに影響もされることなぐサーボモータの動作の異常検知を実現するものである。課題を解決するための手段

[0005] 請求項 1記載の発明は、サーボモータに取り付けられた位置検出手段で検出される現在位置により前記サーボモータの制御を行うサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記サーボモータに接続されたモータ駆動線から前記サーボモータの磁極位置信号を取り出す磁極位置算定手段と、前記サーボモータに取り付けられた位置検出手段からの現在位置信号を入力する現在位置信号入力手段と、前記サーボモータの磁極位置信号と現在位置信号の二系統の信号に基づく前記サーボモータの動作を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する監視手段とを有する、とレ、う構成を採っている。

[0006] 請求項 2記載の発明は、請求項 1記載の発明と同様の構成を備えると共に、前記監視手段は、前記磁極位置信号及び現在位置信号のそれぞれの変化により速度を計算する速度計算手段を有し、求められた前記各速度を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する機能を有する、という構成を採っている。

[0007] 請求項 3記載の発明は、請求項 1又は 2記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、前記モータ駆動線に接続する磁極位置信号取り出し手段と、取り出した磁極位置信号を、モータ軸角度を特定するための所定の数値に変換する磁極位置信号変換手段とを備える、という構成を採っている。

[0008] 請求項 4記載の発明は、請求項 3記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、前記磁極位置信号が示す磁極位置が一巡すると回転量として計数する回転量計数手段を有する、という構成を採っている。

[0009] 請求項 5記載の発明は、請求項 3又は 4記載の発明と同様の構成を備えると共に、前記磁極位置取り出し手段は、三本のモータ駆動線にそれぞれが接続された三本の抵抗素子からなる抵抗ブリッジ回路と、当該抵抗ブリッジ力取り出した正弦波信号を矩形波に変換するコンパレータ回路とを備える、という構成を採っている。

[0010] 請求項 6記載の発明は、請求項 1又は 2記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、前記モータ駆動線を流れるモータ駆動電流信号を取得するモータ駆動電流信号入力手段と、前記モータ駆動電流信号から電気角信号成分として磁極位置を取り出す磁極位置信号変換手段とからなる、という構成を採っている。

[0011] 請求項 7記載の発明は、請求項 6記載の発明と同様の構成を備えると共に、前記磁極位置算定手段は、三本あるモータ駆動線の内の少なくとも二本のモータ駆動線のモータ駆動電流を検出する駆動電流検出手段を備え、前記磁極位置信号変換手段は、前記三本のモータ駆動線に流れるそれぞれのモータ駆動電流が、磁極位置の位相又は当該磁極位置から一定間隔で位相差を生じている位相の関数であることを前提として、前記二つのモータ駆動電流信号から、算術演算により磁極位置を求める演算手段である、という構成を採っている。

[0012] 請求項 8記載の発明は、請求項 1から 7のいずれか一項に記載の発明と同様の構成を備えると共に、複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記各サーボモータの現在位置信号からツーノレ位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、前記ロボットの動作可能領域又は動作禁止領域を記憶する作業エリア記憶手段と、前記座標位置計算手段により計算されたツール位置あるいは所定部位の位置が、前記動作可能領域の外側又は前記動作禁止領域の内側となった場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する作業エリア監視手段とを有する、とレ、う構成を採っている。

[0013] 請求項 9記載の発明は、請求項 1から 7のいずれか一項に記載の発明と同様の構成を備えると共に、複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記各サーボモータの現在位置信号からツーノレ位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、前記ツーノレ位置又はアーム部の所定部位の位置の時系列データの差分により前記ツール位置又はアーム部の移動速度を計算する移動速度計算手段と、前記ロボットが動作可能な移動速度の上限を記憶する制限速度記憶手段と、前記計算されたツール位置又は所定部位の移動速度が、動作可能な移動速度の上限を越えた場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する制限速度監視手段とを有する、という構成を採っている。

[0014] 請求項 10記載の発明は、請求項 8又は 9記載の発明と同様の構成を備えると共に、少なくともいずれかの前記サーボモータについて、当該サーボモータの動作の監視を二つの演算処理部の処理により個別に行うと共に、前記二つの演算処理部での演算処理結果が不一致の場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する演算結果照合手段を有する、という構成を採っている。

発明の効果

[0015] 請求項 1記載の発明では、サーボモータの制御時において、監視手段が、モータ駆動線からサーボモータの磁極位置信号を取り出して磁極位置とサーボモータの位置検出手段からの現在位置とを比較し、異常の発生によるサーボモータの停止判断を行う。このため、例えば指令位置等のような目標値に対してサーボモータの制御の過程の途中で補正が行われるような場合であっても、当該補正が反映された磁極位置信号が現在位置信号と比較されることとなり、前記補正を行う場合のようなサーボ制御部の構成の複雑さに影響もされることなぐ当該補正の影響を受けることなぐサーボモータそのものの異常や位置検出手段の異常や監視装置そのものの異常などの発生を検知し、サーボモータの適正な停止を行うことが可能となる。また、位置検出手段を二つ設けることなぐサーボモータの磁極位置信号と現在位置信号の二系統の信号に基づくサーボモータの動作を比較して異常検知を行うことが可能となる。なお、本発明は、サーボモータの指令位置に基づくフィードバック制御を行うサーボモータ制御装置に適用する場合に限らず、例えば、速度入力アンプをサーボ制御部としたり、トルク入力アンプをサーボ制御部として、現代制御理論に基づくインピーダンス制御を行うサーボモータ制御装置等に適用しても良い。

[0016] 請求項 2記載の発明では、監視手段が磁極位置信号に基づく速度と現在位置に基づく速度との比較からサーボモータの速度監視を行うことから、サーボモータの動作位置に限らず速度の観点からも異常検知を行うことができ、サーボモータのさらなる適正な異常検知を行うことが可能となる。

なお、この請求項 2記載の発明は、位置比較と速度比較の双方で異常が検知された場合に停止指令信号を出力する構成としても良いが、位置比較と速度比較のいずれか一方でも異常が検知された場合には停止指令信号を出力するように構成することがより望ましい。

[0017] 請求項 3記載の発明では、磁極位置信号をモータ軸角度を特定するための所定の数値に変換することから、アナログ信号による比較を行う場合と異なり、外舌 Lによるノィズ等の影響を抑制し、異常検知をより適正且つ安定的に行うことが可能となる。なお、モータ軸角度を特定するための所定の数値とは、例えば、軸角度そのものの値を求めても良いし、軸の一周を複数区間に分割し、いずれの区間に属するかを数値で表しても良い。 [0018] 請求項 4記載の発明では、回転量計数手段を有することから、軸位置のみならず、回転数の一致をも判断して異常検知を行うことができ、サーボモータのさらなる適正な異常検知を行うことが可能となる。

請求項 5記載の発明は、抵抗ブリッジ回路とコンパレータ回路とを用いるので装置の簡易化、コスト低減を図ることができると共に、矩形波を使用して判断を行うことから、処理情報量の低減を図り、処理の迅速化を図ることが可能となる。

[0019] 請求項 6記載の発明は、磁極位置信号変換手段によりモータ駆動電流信号から電気角信号成分として磁極位置を取り出すことから、軸位置を細分化された角度単位で検出することができ、サーボモータのさらなる適正な異常検知を行うことが可能となる。

請求項 7記載の発明は、モータ駆動電流が磁極位置の位相の関数であることに着目して少なくとも二つのモータ駆動電流信号力算術演算により磁極位置を求めるため、容易な演算処理で異常検知が可能となり、異常検知のための構成の簡易化、処理の迅速化を図るることが可能となる。

[0020] 請求項 8記載の発明は、サーボモータの動作異常検知に加えて、ロボットの予定外の領域への動作を防止することができ、ロボットの動作制御について適正化を図ることが可能となる。

請求項 9記載の発明は、サーボモータの動作異常検知に加えて、ロボットの過剰速度での動作を防止することができ、ロボットの動作制御について適正化を図ることが可能となる。

請求項 10記載の発明は、サーボモータの動作の監視を二つの演算処理部の処理により個別に行うと共に、それぞれの演算処理結果が不一致の場合に各サーボモータへの動力供給を遮断する指令信号を出力するため、位置検出手段を二つ設けることなぐサーボモータの動作の監視を二つの系統で行うことが可能となると共に、いずれか一方の監視系統そのものに異常が発生した場合にも、ロボットの動作を停止させることができ、サーボモータ及びロボットのさらなる適正な異常検知を行うことが可能となる。

図面の簡単な説明 [0021] [図 1]本願発明であるサーボモータ監視装置が装備されたサーボモータ制御装置の構成図である。

[図 2]サーボ制御部とにより構成されるフィードバック制御系を示すブロック図である。

[図 3]制御対象となるサーボモータを模式的に示した説明図である。

[図 4]サーボモータ監視装置のより詳細な構成を示す構成図である。

[図 5]サーボモータの回転子を回転した場合に各電機子巻き線に出てくる信号と当該信号を各コンパレータ回路により振幅に影響されないように二値化したものとを示す線図である。

[図 6]第二の実施形態であるサーボモータ監視装置を示す構成図である。

[図 7]第三の実施形態であるロボット制御装置の構成図である。

発明を実施するための最良の形態

[0022] (第一の実施形態:サーボモータ制御装置）

以下、図面をもとに本発明の第一の実施形態の内容を説明する。

図 1は、本願発明であるサーボモータ監視装置 10が装備されたサーボモータ制御装置 100の構成図である。

サーボモータ制御装置 100は、指令位置の時系列データを生成する位置制御部 1 01と、時系列データが示す指令位置とサーボモータ 1に併設された位置検出手段 2 により検出される現在のサーボモータ 1の現在位置との偏差に応じてサーボモータ 1 を駆動するモータ駆動電流を出力するサーボ制御部 120と、サーボモータ 1の動作を監視して所定条件下でモータ動力供給源 (電源） 3とサーボ制御部 120との間に設けられた緊急停止スィッチ 4に対して電源供給を遮断する停止信号を出力するサーボモータ監視装置 10とを備えている。

[0023] (サーボ制御部）

図 2は、上記サーボ制御部 120とにより構成されるフィードバック制御系を示すプロック図である。

上記サーボモータ制御装置 100の制御系では、位置制御、速度制御、電流制御の各々は PID (比例一積分微分)制御で構成される。

即ち、サーボ制御部 120は、位置制御部 101からの指令位置に対して位置検出手段 2からの現在位置を減算して位置偏差を求める減算器 121と、この位置偏差をゼロとするように指令速度を求める位置制御部 122と、現在位置信号を微分して現在速度信号を求める微分器 123と、指令速度信号から現在速度信号を減算して速度偏差を求める減算器 124と、速度偏差がゼロとなるように指令電流信号 (トルク指令)を求める速度制御部 125と、指令電流信号から後述する現在電流信号を減算して電流偏差を求める減算器 126と、電流偏差がゼロとなるように電流制御を行う電流制御部 127と、サーボモータ 1が備える三つのモータ卷き線に通電する三つのモータ駆動線 U， V, Wに電流を分配する転流制御部 128と、モータ駆動線 U， V, Wに対して駆動レベルの大電流に変換して通電するドライバ 129と、モータ駆動線 U， Vに流れる電流信号を検出して合成した現在電流信号を減算器 126に入力する電流合成部 130とを備えている。

即ち、サーボ制御部 120のブロック構成は、 PID制御によるフィードバック制御を行うための基本的且つ一般的な構成となっている。

[0024] サーボ制御部 120は、上記各構成を具備するマイコンで構成されている。また、サーボ制御部 120は、例えば、階段状の指令位置の入力に対して速度変化を緩やかとなるように加減速のカーブを作る補正を加えたり、現在位置の変化からアンダーシユートやオーバシュートの発生を予測して指令位置に補正をかけたり、あるいは動作の一部分でモータが一定速度や一定出力トルクで動作するように切り換えて動作させる補正処理部をマイコン内にカ卩えても良い。

[0025] (サーボモータ）

図 3は制御対象となるサーボモータ 1を模式的に示した説明図である。サーボモータ 1は、回転を行う回転子 11と、回転子 11に固定装備された永久磁石 12と、モータ駆動線 U， V, Wにそれぞれ接続された電機子卷き線 13, 14， 15とを備え、三本の電機子卷き線 13， 14, 15にモータ駆動電流を通電することにより回転を行ういわゆるブラシレス DCモータである。三本の電機子卷き線 13， 14, 15は、実際には 120° 間隔で回転子 11及び永久磁石 12を取り囲むように配置されている。

磁石 12の取り付けられた回転子 11はサーボモータ 1の出力軸であり、この軸に位置検出手段 2 (例えばレゾルバや光学式/磁気式エンコーダの位置検出器）が取り付けられている。

[0026] (サーボモータ監視装置）

図 4はサーボモータ監視装置 10のより詳細な構成を示す構成図である。サーボモータ監視装置 10は、モータ駆動線 U, V, Wからサーボモータ 1の磁極位置信号を取り出す磁極位置算定手段 20と、サーボモータ 1に取り付けられた位置検出手段 2 からの現在位置信号の入力を受ける現在位置信号入力手段 30と、サーボモータ 1の磁極位置信号と現在位置信号の二系統の信号で当該サーボモータ 1の動作を比較対照し、不一致が検出された場合に、モータへの動力供給を遮断する信号を出力する監視手段 40とを備えてレ、る。

[0027] 磁極位置算定手段 20は、モータ駆動線 U, V, Wから磁極位置信号を取り出す磁極位置信号取り出し手段 21と、取り出した磁極位置信号をモータ軸角度の特定のための所定数値に変換する磁極位置信号変換手段 26と、磁極位置信号が示す磁極位置が一巡するたびに回転量として計数する回転量計数手段 27と、サーボモータ 1 の加速時と減速時の磁極位置信号を識別するための基準磁極位置を現在位置信号に基づレ、て設定する基準磁極位置設定手段 28とを備えてレ、る。

[0028] 磁極位置信号取り出し手段 21は、サーボモータ 1のモータ駆動線 U, V, Wに別々に一端部が接続された抵抗素子 22a, 22b, 22cの他端部をまとめて結線してなる抵抗ブリッジ回路 22と、各抵抗素子 22a， 22b, 22cを流れる信号の波形をそれぞれ二値化するコンパレータ回路 23, 24, 25とを備えている。

[0029] 図 5の上の線図は、サーボモータ 1の回転子 11を回転した場合に各電機子卷き線 13, 14, 15に出てくる信号を示すものである。この場合には、サーボモータ 1は発電機として動作し、それぞれの電機子卷き線 13， 14, 15には 120°ずつ位相のずれた正弦波信号が出てくる。回転速度が速くすると、正弦波の振幅は大きくなり、正弦波の周波数も速度に比例して大きくなる。また、サーボモータ 1が駆動してトルク出力を行う場合には、上記の発電機として動作する誘起電圧波形と一致する位相関係及び電流分配比でドライバ 129から各モータ駆動線 U, V, Wに通電することにより、サーボモータ 1を回転させることができる。

こうしたモータの特性については、例えば技術文献「ACサーボシステムの理論と設計の実際」（総合電子出版社 1990年）に詳しく記載されているので、詳細な記述を省略する。

[0030] 図 5の下の線図は、各電機子卷き線 13, 14, 15に出てくる正弦波のアナログ信号を各コンパレータ回路 23, 24, 25により振幅に影響されないように二値化したものである。

磁極位置信号変換手段 26は、各コンパレータ回路 23, 24, 25から出力される UV W3本分の 3ビット信号を、回転角度に対応する二進数に変換する（グレイ一バイナリ変換として良く知られる変換方式である）。即ち、高いレベルを 1、低いレベルを 0で表現すると、磁極位置信号変換手段 26は、 0又は 1の信号を UVWの順に並べてなる二進数表記 101から回転に伴って、 100, 110, 010, 011， 001， 101,…のように順次出力を行う。

これにより、正弦波一周期分を六分割すると共に回転子 11が六分割されたいずれの位置にあるかを認識することができる。

[0031] なお、サーボモータ 1の運転形態で、モータ動力線に現れる図 5の上の線図の波形の持つ意味は次のようになる。ドライバ 129からモータ駆動動力電源を供給する場合には、駆動電流の波形が現れ、駆動側の指令する磁極位置を検出していることになる。そして、駆動における加速時と減速時とでは、電流の正負が反転した 180°違いの信号となるため、これを判別する必要がある。

この為、基準磁極位置設定手段 28は、現在位置信号入力手段 30から入力された現在位置に基づいて UVW各相の基準磁極位置信号を設定して磁極位置信号変換手段 26に入力している。即ち、基準磁極位置設定手段 28では、サーボモータ 1が位置検出手段 2により検出された現在位置である場合に各モータ駆動線 U, V， Wに現れるであろう駆動電流の波形に基づく二進数の値（101， 100, 110， 010, 011, 001のいずれか）を求め、これを磁極位置信号変換手段 26に入力する。

一方、磁極位置信号変換手段 26では、磁極位置信号取り出し手段 21から磁極位置に基づく二進数の値（ひとする）が入力されると、当該値ひと位相が 180° 違いの反転磁極位置に基づく二進数の値（ /3とする）を算出し、磁極位置に基づく値ひと反転磁極位置に基づく値 βのそれぞれを基準磁極位置設定手段 28から入力された現在位置に基づく値（ γとする）と比較し、値 γに対して値 αと値 βのレ、ずれの方が一致する桁数が多いか判定が行われる。そして、値 γに対して値 αがより一致する場合にはサーボモータ 1は加速中と判断されて当該値ひが磁束位置として監視手段 40 に入力される。また、値 γに対して値 /3がより一致する場合にはサーボモータ 1は減速中と判断されて当該値 /3が磁束位置として監視手段 40に入力される。

[0032] なお、駆動電流を流さず、例えば重力等の外力により回転している場合、あるいは慣性モーメントにより一定速で回転している場合には、発電機として動作するモータの誘起電圧波形により、磁極位置を検出する。

更に、回転数計数手段 27は、磁極位置信号変換手段 26から入力される二進数の信号が、正弦波の次の周期の切り換わり目を示す 001から 101に変化するときに回転量のカウント値を + 1し、 101から 001に変化する時に回転量のカウント値を _ 1することにより、正弦波の複数周期分、即ち、サーボモータ 1の回転回数を計数する事ができる。

[0033] 現在位置信号入力手段 30は、サーボモータ 1の位置検出手段 2が検出する現在位置信号の入力を受けてサーボモータ 1の回転数をカウントすると共に、基準磁極位置設定手段 28に対して現在位置信号を入力し、位置比較待避手段 43にカウントした回転数と現在位置信号を入力する。

[0034] 監視手段 40は、磁極位置算定手段 20から入力される磁極位置信号及び回転量に基づいてサーボモータ 1の動作速度を求める第一の速度計算手段 41と、現在位置信号入力手段 30から入力される現在位置信号及び回転量に基づいてサーボモータ 1の動作速度を求める第二の速度計算手段 42と、磁極位置算定手段 20から入力される磁極位置信号及び回転量と現在位置信号入力手段 30から入力される現在位置信号及び回転量とを比較して予め設定された許容値以上の差を生じている場合に位置異常信号を出力する位置比較対比手段 43と、第一の速度計算手段 41で求められたサーボモータ 1の動作速度と第二の速度計算手段 42で求められたサーボモータ 1の動作速度とを比較して予め設定された許容値以上の差を生じている場合に速度異常信号を出力する速度比較対比手段 44と、位置異常信号と速度異常信号のいずれか一方又は双方の入力があると緊急停止スィッチ 4に対して電源供給を遮断する停止信号を入力する OR回路 45とを備えている。

[0035] 上記構成により、サーボモータ監視装置 10では、モータ駆動線 U, V, Wで観測した位置信号と、現在位置信号入力手段 30から入力した位置検出手段 4の検出による現在位置とを比較対比し、不一致の場合に位置異常としてドライバ 129への電源供給を遮断する。

また、同様にして、モータ駆動線 U， V, Wで観測した位置信号に基づく速度と、現在位置信号入力手段 30から入力した現在位置に基づく速度とを比較対比し、不一致の場合に速度異常としてドライバ 129への電源供給を遮断する。位置異常或いは速度異常のいずれが検出された場合でもドライバ 129への電源供給を遮断する。

[0036] サーボモータ制御装置 100は、上記構成により、サーボ制御部 120について指令位置に対する補正処理などの新た構成の影響を受けずに、サーボモータ 1の動作を監視することができ、低コストかつ高信頼性で確実に異常発生の検出を行うことが可能となる。

[0037] なお、本実施形態に示したサーボモータ 1は、回転子 11に永久磁石 12を用いた例を図示したが、サーボモータ 1の形式に限定はなレ、。例えば、永久磁石に限らず電磁石を用い、回転トランスを経由して励磁するサーボモータでも良いし、あるいは、固定側に永久磁石を配置し、回転子に電機子卷き線を用いるサーボモータでも良い。さらには、固定側の永久磁石を直線状に並べた、いわゆるリニアモータであっても良レ、。いずれの場合も、上記監視装置を適用することが可能である。

[0038] また、磁極位置信号変換手段 26、回転量計数手段 27、基準磁極位置設定手段 2 8、第一の速度計算手段 41、第二の速度計算手段 42、位置比較対比手段 43、速度比較対比手段 44、 OR回路 45の全部又は一部については、マイコン等のソフトゥェァを用いる演算装置によりその機能を実現して良いし、アナログ回路等のハードウヱァを用レ、てその機能を実現しても良レ、。

[0039] (第二の実施形態:サーボモータ監視装置）

第二の実施形態であるサーボモータ監視装置 1 OAを図 6に示す。このサーボモータ監視装置 10Aは、磁極位置算定手段 20A以外は前述したサーボモータ監視装置 10と重複する構成を備えており、また、図 1に示すサーボモータ制御装置 100に装備することが可能である。

力かるサーボモータ監視装置 10Aについてサーボモータ監視装置 10と異なる磁極位置算定手段 20Aについてのみ説明することとする。

[0040] 磁極位置算定手段 20Aは、モータ駆動線 U, V, Wの中の少なくとも二本に直列に揷入された低抵抗のシャント抵抗 21A, 22Aと、シャント抵抗 21A, 22Aの電圧降下からモータ駆動線 U， V, Wに流れる電流値を求めるモータ駆動電流信号入力手段 23Aと、モータ駆動電流信号入力手段 23Aで求められた各モータ駆動線の電流値から磁極位置 Θを算出する磁極位置信号変換手段 26Aと、磁極位置 Θの周期の切り換わり目をカウントしてサーボモータ 1の回転回数を計数する回転量計数手段 27A とを備えている。そして、上記シャント抵抗 21A, 22Aは駆動電流検出手段を構成している。

[0041] サーボモータ 1の三つの電機子卷き線 13， 14, 15が回転子 11の周囲で 120° 間隔で配置されている場合、各モータ駆動線 U， V, Wを流れるモータ駆動電流 IU， I V, IWは、磁極位置を Θとすると、以下の関係を生じる。

IU=K- Sin 0

IV=K- Sin ( θ + 120° )

IW=K- Sin ( θ + 240° )

従って、モータ駆動電流信号入力手段 23Αでは三つのモータ駆動電流 IU, IV, I Wの内の二以上を検出し、磁極位置信号変換手段 26Aでは、モータ駆動電流信号から、算術演算により磁極位置 Θを求めるものである。

この場合も、前述した磁極位置信号変換手段 26の場合と同様に、サーボモータ 1 の加速時と減速時とでは流す電流の向きが反転した 180°違いの角度となる。このため、現在位置信号入力手段 30から得られた現在位置から基準磁極位置を求め、磁極位置信号変換手段 26Aでは、計算により求めた角度とこれと 180°違いの角度とを現在位置から求めた基準磁極位置と比較し、近い方の角度を選択する。

[0042] そして、磁極位置信号変換手段 26Aと回転量計数手段 27Aとにより求められた磁極位置及び回転数が位置比較対比手段 43に入力され、その後の処理はサーボモータ監視装置 10と同様である。これにより、サーボモータ監視装置 10Aは、前述したサーボモータ監視装置 10と同様の効果を奏することが可能となる。

[0043] なお、磁極位置信号取り出し手段であるシャント抵抗 21A, 22Aはモータ駆動線 U , V, Wから電流値を取り出す他のいかなる手段を用いても良レ、。例えば、モータ駆動線 U， V, Wの少なくとも二本にホール素子を設けて電流値を求めても良レ、。また、上記例示ではモータ駆動線 U， V, W農地の二本のみについて電流値を検出したが、三本全てから電流値の検出を行って、二つの組み合わせから求まる三つの磁極位置の平均値を採用しても良レ、。

[0044] また、磁極位置信号変換手段 26A、回転量計数手段 27A、基準磁極位置設定手段 28Aの全部又は一部については、マイコン等のソフトウェアを用いる演算装置によりその機能を実現して良いし、アナログ回路等のハードウェアを用いてその機能を実現しても良い。

[0045] (第三の実施形態：ロボット制御装置）

図 7は第三の実施形態であるロボット制御装置 200の構成図である。力かるロボット制御装置 200は、ロボット 120に設けられた複数のサーボモータ 1の制御に対して、前述したサーボモータ制御装置 100の基本構成を備える新たなサーボモータ制御装置を適用するものであり、サーボモータ制御装置 100について既に言及した構成については同一の符号を付して重複する説明は省略するものとする。

[0046] ロボット 120は、土台となるベース 121と、関節 123で連結された複数のアーム 122 と、各関節 123ごとに設けられた駆動源としてのサーボモータ 1 (図 7では一つのみ図示）と、各サーボモータ 1の軸角度をそれぞれ検出する位置検出手段（エンコーダ） 2 と、連結されたアーム 122の最先端部に設けられたツール 124とを備えている。上記ロボット 120は、六つの関節 123を具備したいわゆる多関節型ロボットであり、各関節 123に連結された全アーム 122の最先端部（以下、単に「アーム先端部」という）に位置するツール 124を任意の位置に位置決めし任意の姿勢を取らせることが可能となっている。

[0047] ロボット制御装置 200は、六つのサーボモータ 1に対してフィードバック制御を行うサーボ制御部 120を備えており、位置制御部 101は、例えばツール 124を目標位置まで移動させるための移動軌跡と当該移動軌跡を描いてロボット 120を動作させるための各サーボモータ 1に対する時系列的な指令位置を算出し、サーボ制御部 120に入力を行う。

さらに、ロボット制御装置 200は、各サーボモータ 1の監視を行うサーボモータ監視装置 300を備えている。

[0048] サーボモータ監視装置 300は、各サーボモータ 1の磁極位置及び回転数を求める磁極位置算定手段 20と、各サーボモータ 1の位置検出手段 2から現在位置及び回転数を取得する現在位置入力手段 30と、磁極位置算定手段 20及び現在位置入力手段 30の出力に基づいて各サーボモータ 1の動作を監視する二つの CPU310, 32 0と、二つの CPU310, 320の処理結果が異なる場合に全てのサーボモータ 1の緊急停止スィッチ 4を遮断させる停止信号を出力する演算結果照合手段 360とを備えている。

[0049] CPU310, 320は、以下に説明する同一の構成を備え、且つ当該各構成は図示しなレ、メモリ内に格納された各種プログラムの実行により実現するものである。

即ち、 CPU310, 320は、いずれも、監視手段 40と、各サーボモータ 1の現在位置力らツール 124の位置を計算する座標位置計算手段 331と、ロボット 120の動作可能領域又は動作禁止領域を記憶する作業エリア記憶手段 332と、座標位置計算手段 331により計算されたツール位置が、動作可能領域の外側又は動作禁止領域の内側となった場合に、各サーボモータ 1への電源供給を遮断する指令信号を出力する作業エリア監視手段 333と、各サーボモータ 1の現在位置信号からツール位置の所定部位の位置を計算する座標位置計算手段と、ツール位置の時系列データの差分 (各サーボモータ 1の現在位置から求まるツール位置と前回検出された各サーボモータ 1の現在位置から求まるツール位置との差分）によりツール位置の移動速度を計算する移動速度計算手段 341と、ロボット 120のツール位置の動作可能な移動速度の上限を記憶する制限速度記憶手段 342と、計算されたツール位置の移動速度が動作可能な移動速度の上限を越えた場合に各サーボモータ 2への電源供給を遮断する指令信号を出力する制限速度監視手段 343と、監視手段 40と作業エリア監視手段 333と制限速度監視手段 343のいずれか一つ以上から電源供給を遮断する指令信号が出力されると、それを演算結果照合手段 360に入力する OR回路 350とを備えている。

[0050] そして、各 CPU310, 320のそれぞれにおいて、監視手段 40により異常検知が行われた場合、或いはロボット 120の動作が予定しない領域に及んだ場合、或いは口ボット 120の動作速度が制限速度を超えた場合のいずれか一つでも検知されると、演算結果照合手段 360に対して各サーボモータ 1の電源供給を遮断させる指令信号が入力される。

[0051] 一方、演算結果照合手段 360は、 CPU310, 320の双方から各サーボモータ 1の電源供給を遮断させる指令信号が入力されると、それを緊急停止スィッチ 4に入力し、全てのサーボモータ 1を停止させる。また、 CPU310, 320のいずれか一方からのみ電源供給を遮断させる指令信号が入力されると、二つの CPU310, 320における異常検知の判断に食い違いを生じたこととなるので、やはり、電源供給を遮断させる指令信号を緊急停止スィッチ 4に入力し、全てのサーボモータ 1を停止させる。

[0052] なお、上記サーボ監視装置 300では、ロボット 120のツール位置及びその移動速度について監視を行っている力ツール位置に限らず、ロボット 120について予め定められた所定部位 (例えばモータ取り付け部の出っ張り部位等の他の物体との接触可能性がある部位)や、各アーム部 122を例えば円筒形状等の近似形状で近似してなる当該近似形状の外面位置にっレ、て監視を行うように構成しても良レ、。

[0053] 上記ロボット制御装置 200では、監視手段 40やロボットの動作領域或いは制限速度に関する演算そのものを 2つの CPU310, 320で二重ィ匕し、 2つの CPU310, 32 0の演算結果を照合することで、サーボモータ監視装置 300の二重化を行っている。これにより、サーボ制御部 120について指令位置に対する補正処理などの新た構成の影響を受けずに、サーボモータ 1の動作を監視することができ、複数サーボモータ 1を使用したロボット 120が設定作業エリアを越えようとしたり、或いは禁止動作エリァに侵入しょうとすることを、低コストかつ高信頼性で確実に検出することが可能となる。

産業上の利用可能性

[0054] 以上のように、本発明にかかるサーボモータの動作監視装置は、ロボット等の動作に用いられるサーボモータについて、位置フィードバック制御、速度フィードバック制御、インピーダンス制御などを行う場合のサーボモータの異常検知に適している。符号の説明

1 サーボモータ

2 位置検出手段

10, 300 サーボモータ監視装置

20 磁極位置算定手段

21 磁極位置信号取り出し手段

21A, 22A シャント抵抗 (駆動電流検出手段）

22 抵抗ブリッジ回路

22a, 22b, 22c 抵抗素子

23, 24， 25 コンノルータ回路

23A モータ駆動電流信号入力手段

26, 26 A 磁極位置信号変換手段

27, 27A 回転量計数手段

30 現在位置信号入力手段

40 監視手段

41 第一の速度計算手段

42 第二の速度計算手段

100 サーボモータ制御装置

120 ロボッ卜

122 アーム

123 関節

124 ツール

200 ロボット制御装置

310, 320 CPU (演算処理部)

331 座標位置計算手段

332 作業エリア記憶手段 333 作業エリア監視手段 341 移動速度計算手段 342 制限速度記憶手段 343 制限速度監視手段 360 演算結果照合手段 U, V， W モータ駆動線

Claims

請求の範囲

[1] サーボモータに取り付けられた位置検出手段で検出される現在位置により前記サーボモータの制御を行うサーボモータ制御装置に装備されるサーボモータ監視装置であって、

前記サーボモータに接続されたモータ駆動線から前記サーボモータの磁極位置信号を取り出す磁極位置算定手段と、

前記サーボモータに取り付けられた位置検出手段からの現在位置信号を入力する現在位置信号入力手段と、

前記サーボモータの磁極位置信号と現在位置信号の二系統の信号に基づく前記サーボモータの動作を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する監視手段とを有することを特徴とするサーボモータ監視装置。

[2] 前記監視手段は、前記磁極位置信号及び現在位置信号のそれぞれの変化により速度を計算する速度計算手段を有し、

求められた前記各速度を比較し、不一致が検出された場合に、前記サーボモータへの電源供給を遮断する停止指令信号を出力する機能を有することを特徴とする請求項 1記載のサーボモータ監視装置。

[3] 前記磁極位置算定手段は、前記モータ駆動線に接続する磁極位置信号取り出し手段と、取り出した磁極位置信号を、モータ軸角度を特定するための所定の数値に変換する磁極位置信号変換手段とを備えることを特徴とする請求項 1又は 2記載のサーボモータ監視装置。

[4] 前記磁極位置算定手段は、前記磁極位置信号が示す磁極位置が一巡すると回転量として計数する回転量計数手段を有することを特徴とする請求項 3記載のサーボモータ監視装置。

[5] 前記磁極位置取り出し手段は、三本のモータ駆動線にそれぞれが接続された三本の抵抗素子からなる抵抗ブリッジ回路と、当該抵抗ブリッジ力取り出した正弦波信号を矩形波に変換するコンパレータ回路とを備えることを特徴とする請求項 3又は 4 記載のサーボモータ監視装置。

[6] 前記磁極位置算定手段は、前記モータ駆動線を流れるモータ駆動電流信号を取得するモータ駆動電流信号入力手段と、前記モータ駆動電流信号から電気角信号成分として磁極位置を取り出す磁極位置信号変換手段とからなることを特徴とする請求項 1又は 2記載のサーボモータ監視装置。

[7] 前記磁極位置算定手段は、三本あるモータ駆動線の内の少なくとも二本のモータ駆動線のモータ駆動電流を検出する駆動電流検出手段を備え、

前記磁極位置信号変換手段は、前記三本のモータ駆動線に流れるそれぞれのモータ駆動電流が、磁極位置の位相又は当該磁極位置から一定間隔で位相差を生じている位相の関数であることを前提として、前記二つのモータ駆動電流信号から、算術演算により磁極位置を求める演算手段であることを特徴とする請求項 6記載のサーボモータ監視装置。

[8] 複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、

前記各サーボモータの現在位置信号からツール位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、

前記ロボットの動作可能領域又は動作禁止領域を記憶する作業エリア記憶手段と前記座標位置計算手段により計算されたツール位置あるいは所定部位の位置が、前記動作可能領域の外側又は前記動作禁止領域の内側となった場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する作業エリア監視手段とを有することを特徴とする請求項 1から 7のいずれか一項に記載のサーボモータ監視装置

[9] 複数の関節により連結された複数のアームからなるアーム部と前記各関節に設けられたサーボモータとを備えて前記アーム部の先端にはツールが装備されるロボットの前記各サーボモータを制御するサーボモータ制御装置に装備されるサーボモータ監視装置であって、前記各サーボモータの現在位置信号からツール位置又はアーム部の所定部位の位置を計算する座標位置計算手段と、

前記ツール位置又はアーム部の所定部位の位置の時系列データの差分により前記ツール位置又はアーム部の移動速度を計算する移動速度計算手段と、

前記ロボットが動作可能な移動速度の上限を記憶する制限速度記憶手段と、前記計算されたツール位置又は所定部位の移動速度が、動作可能な移動速度の上限を越えた場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する制限速度監視手段とを有することを特徴とする請求項 1から 7のいずれか一項に記載のサーボモータ監視装置。

少なくともいずれかの前記サーボモータについて、当該サーボモータの動作の監視を二つの演算処理部の処理により個別に行うと共に、前記二つの演算処理部での演算処理結果が不一致の場合に、前記各サーボモータへの動力供給を遮断する指令信号を出力する演算結果照合手段を有することを特徴とする請求項 8又は 9記載のサーボモータ監視装置。