JP5458768B2 - ロボット制御装置および制御方法 - Google Patents

Description

本発明は、サーボモータを備えたロボットを制御する技術に関するものであり、特に、サーボモータに接続された角度センサに異常が生じた場合に、サーボモータを停止させる技術に関する。

ロボットの軸を駆動するサーボモータには、回転角度をフィードバック制御するために、ロータリーエンコーダやレゾルバといった角度センサが取り付けられている。この角度センサに故障が生じたり、角度センサと制御装置とを接続する信号線に断線が生じると、ロボットの軸を正確に駆動することができなくなる。

そこで、このような異常が生じた場合には、ロボットの不測の動作を抑制するため、サーボモータを速やかに停止させることが好ましい。サーボモータを停止させるには、例えば、ダイナミックブレーキなどの発電制動によって停止させる方法がある（例えば、特許文献１参照）。

しかし、ロボット用モータとして使用されるブラシレスＤＣサーボモータを停止させるためのダイナミックブレーキは、サーボモータの回転速度が高いほど制動トルクが低下するという特性を有することが一般的であるため、サーボモータが高速回転している場合には、停止するまでの時間が長くなるという問題があった。

特開２００８−３０７６１８号公報 国際公開第００／０４６３２号パンフレット

このような問題を考慮し、本発明が解決しようとする課題は、角度センサから正常に回転角度が取得できなくなった場合に、迅速にサーボモータを停止させることが可能な技術を提供することにある。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態又は適用例として実現することが可能である。
本発明の第１の形態は、サーボモータと、該サーボモータの回転軸に接続されて該サーボモータの回転角度を検出する角度センサと、を備えるロボットを制御するためのロボット制御装置であって、
前記角度センサから前記回転角度を取得し、該取得された回転角度に応じて前記サーボモータの駆動をフィードバック制御する第１の駆動制御部と、
前記サーボモータの電気的変量に基づいて、前記サーボモータの回転角度および回転速度を推定する推定部と、
前記第１の駆動制御部の代わりに、前記推定部によって推定された回転角度に応じて、前記サーボモータの駆動をセンサレス制御する第２の駆動制御部と、
前記サーボモータに対して発電制動を行う発電制動部と、
前記第１の駆動制御部による前記サーボモータの駆動時に、前記角度センサの異常の有無を検出する異常検出部と、
前記異常検出部によって異常が検出された場合において、前記推定部によって推定された回転速度が、予め定められた閾値以上の場合には、前記第２の駆動制御部に、前記サーボモータを減速させる減速制御を実行させ、前記推定された回転速度が前記閾値未満の場合には、前記発電制動部に、前記サーボモータに発電制動を行う制動制御を実行させることで、前記サーボモータを停止させる停止制御を行う停止制御部と、
を備え、
前記閾値は、前記発電制動部による制動トルクが前記サーボモータの定格トルクに一致する回転速度のうち高い側の回転速度よりも低く、かつ、前記発電制動部による制動トルクが最大となる回転速度よりも高い値に設定されている、
ロボット制御装置である。
このような形態のロボット制御装置では、角度センサに異常が検出された場合に、サーボモータの電気的変量（例えば、電圧、電流、インダクタンスなど）に基づいて推定されたサーボモータの回転速度と、上記閾値とを比較し、推定された回転速度が、この閾値以上であれば、センサレス制御によってサーボモータを減速させ、閾値未満であれば、発電制動によってサーボモータを減速させる。この結果、どのような回転速度において角度センサに異常が生じたとしても、センサレス制御による減速制御と発電制動による制動制御とを使い分けることで、迅速にサーボモータを停止させることが可能になる。

［適用例１］サーボモータと、該サーボモータの回転軸に接続されて該サーボモータの回転角度を検出する角度センサと、を備えるロボットを制御するためのロボット制御装置であって、前記角度センサから前記回転角度を取得し、該取得された回転角度に応じて前記サーボモータの駆動をフィードバック制御する第１の駆動制御部と、前記サーボモータの電気的変量に基づいて、前記サーボモータの回転角度および回転速度を推定する推定部と、
前記推定部によって推定された回転角度に応じて、前記サーボモータの駆動をセンサレス制御する第２の駆動制御部と、前記サーボモータに対して発電制動を行う発電制動部と、前記第１の駆動制御部による前記サーボモータの駆動時に、前記角度センサの異常の有無を検出する異常検出部と、前記異常検出部によって異常が検出された場合において、前記推定部によって推定された回転速度が、前記サーボモータの回転速度に応じた前記発電制動の制動トルクの変動特性に基づいて予め定められた閾値以上の場合には、前記第２の駆動制御部に、前記サーボモータを減速させる減速制御を実行させ、前記推定された回転速度が前記閾値未満の場合には、前記発電制動部に、前記サーボモータに発電制動を行う制動制御を実行させることで、前記サーボモータを停止させる停止制御を行う停止制御部とを備えるロボット制御装置。

このような構成のロボット制御装置では、角度センサに異常が検出された場合に、サーボモータの電気的変量（例えば、電圧、電流、インダクタンスなど）に基づいて推定されたサーボモータの回転速度と、サーボモータの発電制動時における回転速度に応じた制動トルクの変動特性に基づいて定められた閾値とを比較し、推定された回転速度が、この閾値以上であれば、センサレス制御によってサーボモータを減速させ、閾値未満であれば、発電制動によってサーボモータを減速させる。この結果、どのような回転速度において角度センサに異常が生じたとしても、センサレス制御による減速制御と発電制動による制動制御とを使い分けることで、迅速にサーボモータを停止させることが可能になる。

［適用例２］適用例１に記載のロボット制御装置であって、前記閾値は、前記サーボモータの発電制動時の制動トルクの最大値を生じさせる前記サーボモータの回転速度よりも低い値である、ロボット制御装置。
このような構成であれば、発電制動の作動によって生じるサーボモータの発熱を軽減することが可能になる。

［適用例３］適用例１または適用例２に記載のロボット制御装置であって、前記ロボットは、複数の前記サーボモータと該各サーボモータにそれぞれ接続された複数の前記角度センサを備え、前記異常検出部は、前記複数の角度センサのそれぞれについて異常の有無を検出し、前記停止制御部は、前記異常検出部によって異常が検出された角度センサに接続されたサーボモータに対して前記停止制御を実行し、異常が検出されていない角度センサに接続されたサーボモータに対しては、前記第１の駆動制御部によって該サーボモータを停止させる、ロボット制御装置。
このような構成であれば、異常が検出されていない角度センサに接続されたサーボモータについては、正常な角度に基づいてフィードバック制御を行うことで停止させることができる。そのため、ロボット全体の動作の軌道のずれを最小限に抑えつつ、各サーボモータを迅速に停止させることが可能になる。

［適用例４］適用例１ないし適用例３のいずれか一項に記載のロボット制御装置であって、前記ロボットは、更に、前記サーボモータの停止位置を保持するためのメカニカルブレーキを備え、前記停止制御部は、前記停止制御後に、前記メカニカルブレーキによって前記サーボモータの停止位置を保持させる、ロボット制御装置。
このような構成であれば、発電制動による停止後に、サーボモータの停止位置を確実に保持させることが可能になる。

本発明は、上述したロボット制御装置としての構成のほか、ロボットの制御方法や、ロボットを制御するためのコンピュータプログラムとしても構成することができる。コンピュータプログラムは、コンピュータが読取可能な記録媒体に記録されていてもよい。記録媒体としては、例えば、フレキシブルディスクやＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、光磁気ディスク、メモリカード、ハードディスク等の種々の有形的な媒体を利用することができる。

ロボットシステムの概略構成を示す説明図である。 ロボット本体およびロボット制御装置の内部構成を示すブロック図である。 ダイナミックブレーキの制動トルクの変動特性を例示する説明図である。 停止制御処理のフローチャートである。 第２実施例における停止制御処理のフローチャートである。 第３実施例における閾値を示す説明図である。 複数の閾値の設定例を示す説明図である。

以下、本発明の実施の形態をいくつかの実施例に基づき説明する。
Ａ．第１実施例：
（Ａ１）ロボットシステムの概略構成：
図１は、本発明の実施例としてのロボット制御装置２００を含むロボットシステム１０の概略構成を示す説明図である。ロボットシステム１０は、多関節型の産業用ロボットとして構成されたロボット本体１００と、ロボット本体１００を制御するロボット制御装置２００と、ロボット制御装置２００に接続され、ロボット本体１００の動作をプログラムするためのティーチングペンダント３００とを有している。

ロボット本体１００は、工場内等に固定されるベース部１０１と、水平方向に旋回可能にベース部１０１に支持されたショルダ部１０２と、鉛直方向に旋回可能にショルダ部１０２に下端が支持された下アーム１０３と、鉛直方向に旋回可能に下アーム１０３の先端に略中央部が支持された上アーム１０４と、鉛直方向に旋回可能に上アーム１０４の先端に支持された手首１０５とを備えている。手首１０５の先端には、手首１０５の円周方向に回転可能なフランジ部１０６が備えられている。フランジ部１０６には、ワークを把持するハンド（図示せず）が取り付けられる。

上述したロボット本体１００の各パーツを接続する関節部分には、各パーツを旋回させるためのサーボモータと、サーボモータの回転角度や回転速度を検出するための角度センサが組み込まれている。本実施例のロボット制御装置２００は、この角度センサに異常が生じた場合に、サーボモータを迅速に停止させるための機能を備えている。以下、この機能を実現するための構成および処理について詳細に説明する。

（Ａ２）ロボット本体およびロボット制御装置の内部構成：
図２は、ロボット本体１００およびロボット制御装置２００の内部構成を示すブロック図である。ロボット本体１００は、三相交流式のブラシレスＤＣモータとして構成されたサーボモータ１１０と、サーボモータ１１０の回転軸に接続されたロータリーエンコーダ１６２と、サーボモータ１１０の回転位置を機械的に保持するメカニカルブレーキ１５０と、を備えている。

ロータリーエンコーダ１６２には角度変換器１６４が接続されている。角度変換器１６４は、ロータリーエンコーダ１６２から、サーボモータ１１０の回転に応じた信号を入力し、この信号に基づいてサーボモータ１１０の回転角度を表す角度情報を生成する。角度変換器１６４によって生成された角度情報は、ロボット制御装置２００に入力され、後述するフィードバック制御を行うための帰還信号として用いられる。以下の説明では、ロータリーエンコーダ１６２と角度変換器１６４とを含めて、「角度センサ１６０」と呼ぶ。なお、本実施例では、サーボモータ１１０の回転角度をロータリーエンコーダ１６２によって検出することとしたが、レゾルバを用いて検出することとしてもよい。

ロボット制御装置２００は、ロボット本体１００のサーボモータ１１０に三相交流電力を供給するインバータ１２０と、後述する駆動制御部２１１からの指令に基づいてインバータ１２０のスイッチング制御を行う駆動回路１４０とを備えている。また、ロボット制御装置２００は、ＣＰＵ２１０とＲＯＭ２３０とＲＡＭ２４０とを備えている。ＣＰＵ２１０は、ＲＯＭ２３０に記憶された所定の制御プログラム２３５をＲＡＭ２４０にロードして実行することで、駆動制御部２１１、異常検出部２１２、停止制御部２１３、角度推定部２１４、セレクタ２１５、発電制動制御部２１６、および、機械制動制御部２１７として機能する。

駆動制御部２１１は、ＲＡＭ２４０に記憶された軌跡データ２４５に基づいて、サーボモータ１１０を駆動するためのパルス信号を駆動回路１４０に出力する。このとき、駆動制御部２１１は、ロボット本体１００の角度センサ１６０から角度情報を取得し、この角度情報に基づいて、サーボモータ１１０の駆動をフィードバック制御する。駆動回路１４０は、駆動制御部２１１から入力したパルス信号に基づいて、インバータ１２０を駆動し、サーボモータ１１０を回転させる。

異常検出部２１２は、角度センサ１６０に異常が生じたか否かを検出する。異常検出部２１２は、例えば、ロボット本体１００とロボット制御装置２００との間の信号線の断線などによって角度センサ１６０から角度情報を取得できない場合や、指令値とは大きく異なる回転角度や回転速度が取得された場合、あるいは、角度センサ１６０からエラー信号を受信した場合に、角度センサ１６０に異常が生じたと判断する。

停止制御部２１３は、異常検出部２１２によって角度センサ１６０の異常が検出された場合に、サーボモータ１１０を停止するための停止制御処理を実行する。停止制御処理の詳細については後述する。

角度推定部２１４は、インバータ１２０からロボット本体１００のサーボモータ１１０に印加される３相交流のうち、２相（Ｕ相、Ｖ相）の電流値をそれぞれ検出する。角度推定部２１４は、更に、駆動制御部２１１から電圧指令値を入力すると、これらの電流値と電圧指令値とに基づいて、サーボモータ１１０の角度情報（回転角度）を推定する。このような角度情報の推定は、例えば、実電流値と推定電流値との誤差に応じて回転角度を推定する周知の電流推定誤差法に基づいて行うことができる。

セレクタ２１５は、停止制御部２１３からの指令に基づいて、駆動制御部２１１に入力する角度情報を、角度センサ１６０から入力するか、角度推定部２１４から入力するかを選択する。つまり、セレクタ２１５によって角度センサ１６０が選択されれば、駆動制御部２１１は、ロータリーエンコーダ１６２（角度センサ１６０）によって実測された角度情報に基づいて、サーボモータ１１０に対して通常のフィードバック制御を行う。一方、セレクタ２１５によって角度推定部２１４が選択されれば、駆動制御部２１１は、角度推定部２１４によって推定された角度情報に基づいて、サーボモータ１１０に対してフィードバック制御を行う。このように、推定された角度情報に基づいてサーボモータ１１０をフィードバック制御することを、「センサレス制御」という。セレクタ２１５によって角度センサ１６０が選択された場合には、駆動制御部２１１は、「第１の駆動制御部」として機能し、セレクタ２１５によって角度推定部２１４が選択された場合には、駆動制御部２１１は、「第２の駆動制御部」として機能することになる。

発電制動制御部２１６は、停止制御部２１３からの指令に応じて、サーボモータ１１０を制御し、ダイナミックブレーキ（短絡ブレーキともいう）を作動させる。具体的には、発電制動制御部２１６は、駆動回路１４０を通じてインバータ１２０のスイッチング素子を制御し、サーボモータ１１０の端子間を強制的に短絡させる。そうすると、サーボモータ１１０の回転エネルギがサーボモータ１１０自体の発熱により消費されるため、サーボモータ１１０を停止させることができる。

図３は、ダイナミックブレーキの制動トルクの変動特性を例示する説明図である。一般的に、ロボット用のサーボモータ１１０に作動させるダイナミックブレーキは、その制動トルクが、ある回転速度（図３の場合には、約８００ｒｐｍ）で最大となり、その回転速度を超えると、徐々に低い値になるような変動特性を有することが知られている。図３に示した例では、回転速度が３０００ｒｐｍを超えると、ダイナミックブレーキの制動トルクはサーボモータ１１０の定格トルクを下回る値となる。

機械制動制御部２１７（図２）は、停止制御部２１３からの指令に応じて、ロボット本体１００のメカニカルブレーキ１５０を作動させて、サーボモータ１１０の回転位置を保持させる。機械制動制御部２１７は、また、停止制御部２１３からの指令に拘わらず、ロボットシステム１０の電源オフ時にもメカニカルブレーキ１５０を作動させてサーボモータ１１０の回転位置を保持させる機能を有する。

なお、図２には、一つの関節部分についてのロボット本体１００の内部構成とロボット制御装置２００の内部構成を示したが、他の関節部分も同様の内部構成を備えている。なお、ＣＰＵ２１０とＲＯＭ２３０とＲＡＭ２４０とは、１つのロボット制御装置２００につき１組備えられており、ＣＰＵ２１０が実現する各機能部２１１〜２１７が、各関節毎に備えられている。

（Ａ３）停止制御処理：
図４は、ロボット制御装置２００のＣＰＵ２１０が実行する停止制御処理のフローチャートである。この停止制御処理は、角度センサ１６０に異常が生じた場合に、サーボモータ１１０を停止させるための処理である。本実施例では、ロボット本体１００の１つの関節に取り付けられた角度センサ１６０に異常が生じた場合に、その角度センサ１６０に接続されたサーボモータ１１０を停止させる場合の処理について説明する。なお、この停止制御処理の開始時には、駆動制御部２１１は、セレクタ２１５によって角度センサ１６０に接続されていることとする。

本実施例の停止制御処理では、まず、ＣＰＵ２１０は、ＲＡＭ２４０から入力した軌跡データと、角度センサ１６０から入力した角度情報とに基づいて、駆動制御部２１１によりサーボモータ１１０の駆動をフィードバック制御する通常の制御処理を実行する（ステップＳ１０）。そして、異常検出部２１２によって、角度センサ１６０に異常が生じたかを検出する（ステップＳ１２）。角度センサ１６０に異常が生じていない場合には、ＣＰＵ２１０は、上述したステップＳ１０およびステップＳ１２の処理を繰り返し実行する。

異常検出部２１２によって、角度センサ１６０の異常が検出された場合には、ＣＰＵ２１０は、停止制御部２１３によって、センサレス制御を開始する（ステップＳ１４）。具体的には、セレクタ２１５によって、駆動制御部２１１を角度推定部２１４に接続することで、センサレス制御が開始される。

センサレス制御が開始されると、停止制御部２１３は、駆動制御部２１１に減速指令を与えて駆動制御部２１１に減速制御を行わせる（ステップＳ１６）。駆動制御部２１１は、停止制御部２１３から減速指令を受けると、角度推定部２１４によって推定された回転速度が目標速度（０ｒｐｍ）に近づくように、サーボモータ１１０の回転速度を減じるフィードバック制御を行う。

サーボモータ１１０の減速制御が開始されると、停止制御部２１３は、角度推定部２１４から入力した推定回転速度が、予め定められた閾値Ｔｈ１未満であるかを判断する（ステップＳ１８）。本実施例では、閾値Ｔｈ１は、図３に示すように、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクがサーボモータ１１０の定格トルクに一致する回転速度（図３では、３００ｒｐｍおよび３０００ｒｐｍ）のうち、高い側の回転速度である３０００ｒｐｍに設定されている。このような閾値Ｔｈ１であれば、サーボモータ１１０の回転速度が低下するほど、制動トルクが定格トルクよりも大きくなり、十分な制動力を得られるためである。現在の推定回転速度が閾値Ｔｈ１以上であれば、停止制御部２１３は処理をステップＳ１６に戻して、引き続き、センサレス制御に基づく減速制御を行う。

角度推定部２１４から入力した推定回転速度が、閾値Ｔｈ１未満であれば、停止制御部２１３は、センサレス制御を停止し（ステップＳ２０）、発電制動制御部２１６によって、ダイナミックブレーキを作動させる（ステップＳ２２）。ダイナミックブレーキの作動によってサーボモータ１１０が停止すると、最後に、停止制御部２１３は、機械制動制御部２１７に指令を与えてメカニカルブレーキ１５０を作動させ、サーボモータ１１０の回転位置を保持させる（ステップＳ２４）。ダイナミックブレーキによってサーボモータ１１０が停止したか否かは、例えば、角度推定部２１４によって推定された角度情報が、所定時間変化しない場合に、停止したと判断することができる。以上で説明した一連の処理により、当該停止制御処理は終了する。

以上で説明した本実施例のロボットシステム１０によれば、サーボモータ１１０の回転速度に応じて、センサレス制御による減速制御と、ダイナミックブレーキによる制動制御とを使い分けることができる。そのため、ダイナミックブレーキの制動トルクが低くなる高い回転速度では、センサレス制御による減速制御によって、サーボモータ１１０の回転速度を減じることができ、ダイナミックブレーキの制動トルクが高くなる低い回転速度では、ダイナミックブレーキによってサーボモータ１１０を停止させることができる。また、一般的に、センサレス制御を行う場合には、低い回転速度では、回転角度を正確に推定することが困難になるので制御の精度が低下するという問題があるが、本実施例では、回転速度が低い場合には、センサレス制御ではなく、ダイナミックブレーキによる制動制御を行うのでこのような問題を回避することができる。つまり、本実施例では、高速回転時に制動トルクが低くなるというダイナミックブレーキの特性を、センサレス制御による減速制御よって補償し、また、低速回転時に角度の推定が困難になるというセンサレス制御の特性を、ダイナミックブレーキによる制動制御で補償することができる。この結果、どのような回転速度でサーボモータ１１０が回転していても、角度センサ１６０に異常が検出されれば、迅速にサーボモータ１１０を停止させることが可能になる。

また、本実施例では、サーボモータ１１０の回転速度が高い場合にはセンサレス制御によって減速を行い、その後、ダイナミックブレーキによってサーボモータ１１０が停止する。そのため、ダイナミックブレーキが作動するまでは、ＲＡＭ２４０に記憶された軌跡データ２４５に基づく動作軌跡を辿るように各関節が動作することになるので、ロボット本体１００が不測の挙動を示すことを抑制することが可能になる。この結果、ロボット本体１００のアームやハンドが生産設備の進入禁止領域等に進入してしまうことを抑制することができる。また、ダイナミックブレーキが作動する際には、センサレス制御による減速によって、サーボモータ１１０の回転速度は十分に低下していることになるので、最低限の移動距離でサーボモータ１１０を停止させることができる。このように、本実施例では、角度センサ６０の異常時に、センサレス制御による減速、ダイナミックブレーキによる制動、の順にサーボモータ１１０を制御するので、急速に停止させるよりもサーボモータ１１０に掛かる負担や機械的なブレを抑制することができ、また、ロボット本体１００の周囲に対する安全性を向上させることができる。この結果、ロボット本体１００の保護と周囲への安全性との最適なバランスを取りながら、角度センサ１６０の異常時にロボット本体１００を迅速に停止させることが可能になる。

また、本実施例では、閾値Ｔｈ１を、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクがサーボモータ１１０の定格トルクに一致する回転速度（図３では、３００ｒｐｍおよび３０００ｒｐｍ）のうち、高い側の回転速度である３０００ｒｐｍに設定した。そのため、サーボモータ１１０が定格トルク付近で駆動されている最中に、角度センサ１６０が故障しても、定格トルクを超える制動トルクでダイナミックブレーキを作動させることが可能になる。よって、迅速にサーボモータ１１０を停止させることが可能になる。

また、本実施例では、特別なハードウェアを新たにロボット本体１００に新たに組み込むことなく、ＣＰＵ２１０が実現する機能によって、上述した停止制御処理を実行することができる。そのため、既に生産設備に配置されている既存のロボットシステムに対しても、上述した機能を容易に付加することが可能になる。

Ｂ．第２実施例：
上述した第１実施例の停止制御処理では、ロボット本体１００のある関節に取り付けられた角度センサ１６０に異常が生じた場合に、その角度センサ１６０に接続されたサーボモータ１１０を停止させる場合の処理について説明した。これに対して第２実施例では、複数の関節にそれぞれ取り付けられた複数の角度センサ１６０のうち、１つの角度センサ１６０に異常が生じた場合の各関節のサーボモータ１１０の停止方法について説明する。

図５は、第２実施例における停止制御処理のフローチャートである。第２実施例の停止制御処理は、図４に示した第１実施例の停止制御処理に対して、ステップＳ３０〜Ｓ３４の処理が追加されており、他の処理は第１実施例と同様である。

本実施例の停止制御処理では、第１実施例と同様に、まず、ＣＰＵ２１０は、ＲＡＭ２４０から入力した軌跡データと、角度センサ１６０から入力した角度情報とに基づいて、駆動制御部２１１によりサーボモータ１１０の駆動をフィードバック制御する通常の制御処理を実行する（ステップＳ１０）。そして、関節毎に用意された異常検出部２１２は、対応する角度センサ１６０に異常が生じたかを検出する（ステップＳ１２）。対応する角度センサ１６０に異常が検出された場合には、ＣＰＵ２１０は、その角度センサ１６０が接続されたサーボモータ１１０に対して、第１実施例と同様の処理を行い、センサレス制御とダイナミックブレーキとによってサーボモータ１１０を停止させる。

一方、対応する角度センサ１６０に異常が検出されなかった場合には、異常検出部２１２は、他の異常検出部２１２と通信を行い、他の角度センサ１６０に異常が生じたかを判断する（ステップＳ３０）。他の角度センサ１６０に異常が生じれば、停止制御部２１３は、駆動制御部２１１に指令を与えて、対応する角度センサ１６０からの角度情報に基づくフィードバック制御による減速処理を行わせる（ステップＳ３２）。つまり、この場合には、他の角度センサ１６０には異常が生じているものの、現在、角度情報を取得している角度センサ１６０には異常が生じていないため、正常な角度情報に基づいて、通常のフィードバック制御による減速処理を行うのである。そして、この減速処理によって、サーボモータ１１０が停止すれば（ステップＳ３４）メカニカルブレーキ１５０を作動させて、サーボモータ１１０の回転位置を保持させる（ステップＳ２４）。

以上で説明した第２実施例によれば、複数の角度センサ１６０のうち、異常が生じた角度センサ１６０に接続されたサーボモータ１１０に対しては、センサレス制御とダイナミックブレーキとによる停止処理が実行され、異常が生じていない角度センサ１６０に接続されたサーボモータ１１０に対しては、異常が生じていない角度センサ１６０から取得された角度情報に基づくフィードバック制御により減速処理が実行される。つまり、いずれかの角度センサ１６０に異常が生じていても、異常が生じていない角度センサ１６０に接続されたサーボモータ１１０は、正常な角度情報に基づいて停止されることになる。そのため、本実施例によれば、角度センサ１６０の異常によってロボット本体１００の各関節の軌道がずれてしまうことを最小限に抑えながら、ロボット本体１００全体を迅速に停止させることが可能になる。

なお、本実施例では、異常が生じていない角度センサ１６０に接続されたサーボモータ１１０に対しては、通常のフィードバック制御による減速処理を実行することとした。これに対して、例えば、異常が生じていない角度センサ１６０に接続されたサーボモータ１１０に対しても、第１実施例と同様の停止制御処理を実行することも可能である。

Ｃ．第３実施例：
上述した第１実施例および第２実施例では、センサレス制御とダイナミックブレーキとの切り換え時に用いられる閾値Ｔｈ１は、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクが、そのサーボモータ１１０の定格トルクに一致することになる回転速度のうち、高い側の回転速度付近に設定されていることとした（図３参照）。しかし、閾値は、以下のような値に設定することも可能である。

図６は、第３実施例における閾値Ｔｈ２の設定例を示す説明図である。閾値Ｔｈ２は、第１実施例および第２実施例の停止制御処理において、閾値Ｔｈ１に替えて適用される値である。本実施例では、この閾値Ｔｈ２を、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクの最大値を生じさせる回転速度（図６では、約８００ｒｐｍ）よりも低い回転速度で、かつ、サーボモータ１１０の定格トルクと一致する制動トルクを生じさせる回転速度（図６では、３００ｒｐｍおよび３０００ｒｐｍ）のうち低い側の回転速度（図６では、約３００ｒｐｍ）よりも高い回転速度であることとした。具体的には、図６に示すように、閾値Ｔｈ２を、５００ｒｐｍと設定することが可能である。

このように、閾値Ｔｈ２を、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクの最大値を生じさせる回転速度よりも低い回転速度に設定すれば、ダイナミックブレーキの作動によって、サーボモータ１１０内に長時間、電流が流れることを抑制することができる。そのため、サーボモータ１１０やインバータ１２０内のスイッチング素子の発熱を抑制することが可能になる。

なお、閾値は、上述した値（Ｔｈ１，Ｔｈ２）に限られず、種々の値を設定することが可能である。例えば、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクがサーボモータ１１０の定格トルクに一致する回転速度（図３では、３００ｒｐｍおよび３０００ｒｐｍ）のうち、高い側の回転速度よりも低く、かつ、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクの最大値を生じさせる回転速度（図３では、約８００ｒｐｍ）よりも高い値（例えば、１０００ｒｐｍ）とすることも可能である。また、サーボモータ１１０を迅速に停止させることが可能であれば、閾値を、定格トルクを下回る制動トルクとなる回転速度に設定することも可能です。つまり、閾値Ｔｈ１は、サーボモータ１１０の回転速度に応じたダイナミックブレーキの制動トルクの変動特性に基づいて、種々の値に設定することが可能である。

Ｄ．変形例：
以上、本発明の種々の実施例について説明したが、本発明はこれらの実施例に限定されず、その趣旨を逸脱しない範囲で種々の構成を採ることができる。例えば、ソフトウェアによって実現した機能は、ハードウェアによって実現するものとしてもよい。また、その逆も可能である。そのほか、以下のような変形が可能である。

（Ｄ１）変形例１：
上記実施例では、停止制御処理時において、１つの閾値Ｔｈ１（または閾値Ｔｈ２）に基づいて、サーボモータ１１０の制御方法を、センサレス制御による減速制御からダイナミックブレーキによる制動制御に切り換えた。これに対して、閾値は複数設定されていてもよい。

図７は、複数の閾値の設定例を示す説明図である。図７には、３つの閾値Ｔｈ３，Ｔｈ４，Ｔｈ５が設定された例を示した。１つめの閾値Ｔｈ３は、第１実施例と同様に、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクがサーボモータ１１０の定格トルクに一致する回転速度（図３では、３００ｒｐｍおよび３０００ｒｐｍ）のうち、高い側の回転速度（３０００ｒｐｍ）に設定されている。また２つ目の閾値Ｔｈ４は、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクの最大値を生じさせる回転速度（約８００ｒｐｍ）よりも高い回転速度で、かつ、閾値Ｔｈ３よりも低い回転速度（１５００ｒｐｍ）に設定している。更に、３つ目の閾値Ｔｈ５は、上述した第３実施例と同様に、ダイナミックブレーキ作動時のサーボモータ１１０の制動トルクの最大値を生じさせる回転速度（図６では、約８００ｒｐｍ）よりも低い回転速度で、かつ、サーボモータ１１０の定格トルクと一致する制動トルクを生じさせる回転速度（図６では、３００ｒｐｍおよび３０００ｒｐｍ）のうち低い側の回転速度（図６では、約３００ｒｐｍ）よりも高い回転速度（５００ｒｐｍ）に設定されている。

これらの閾値Ｔｈ３〜Ｔｈ５を用いれば、例えば、サーボモータ１１０の推定回転速度が閾値Ｔｈ３以上の場合には、センサレス制御によって減速制御を行い、推定回転速度が、閾値Ｔｈ３未満になった場合には、ダイナミックブレーキによる制動制御に制御方法を切り換えることができる。また、推定回転速度が閾値Ｔｈ４未満になった場合には、制御方法をセンサレス制御に戻し、最終的に推定回転速度が閾値Ｔｈ５未満になれば、再び、制御方法をセンサレス制御からダイナミックブレーキによる制動制御に切り換えることができる。このように、複数の閾値に基づいて制御方法を切り換えれば、サーボモータ１１０のダイナミックブレーキによる制動トルクの特性に応じて、センサレス制御による減速制御とダイナミックブレーキによる制動制御とを細かく切り換えることができる。例えば、図６に示した例では、ダイナミックブレーキを作動させた場合にサーボモータ１１０の制動トルクが最大となる付近の回転速度では、ダイナミックブレーキではなく、センサレス制御による減速制御が行われる。つまり、制動トルクが最大値付近になるような回転速度を避けるようにダイナミックブレーキを作動させることができるので、サーボモータ１１０やインバータ１２０内のスイッチング素子の発熱をより効果的に抑制することが可能になる。

（Ｄ２）変形例２：
上記実施例では、センサレス制御を行うにあたり、インバータ１２０からサーボモータ１１０に印加される電流値と、電圧指令値に基づいてサーボモータ１１０の角度情報を推定することとした。これに対して、角度情報は、サーボモータ１１０の逆起電力波形やインダクタンスの変化など、種々の電気的変量（電気的パラメータ）に基づいて推定することが可能である。角度情報の推定時には、前述した電気的変量のいずれかを単独で用いることとしてもよいし、２以上の電気的変量を組み合わせて用いてもよい。センサレス制御を行うための具体的な方式としては、例えば、上述した電流推定誤差法以外にも、拡張誘起電圧オブザーバ法など他の周知の方法を採用することが可能である。

（Ｄ３）変形例３：
上記実施例の停止制御処理では、ダイナミックブレーキの作動後、サーボモータ１１０が停止してからメカニカルブレーキを作動させることとした。これに対して、例えば、サーボモータ１１０が停止するよりも前に、メカニカルブレーキを作動させることとしてもよい。こうすることで、より迅速に、サーボモータ１１０を停止させることが可能になる。

１０…ロボットシステム
１００…ロボット本体
１０１…ベース部
１０２…ショルダ部
１０３…下アーム
１０４…上アーム
１０５…手首
１０６…フランジ部
１１０…サーボモータ
１２０…インバータ
１４０…駆動回路
１５０…メカニカルブレーキ
１６０…角度センサ
１６２…ロータリーエンコーダ
１６４…角度変換器
２００…ロボット制御装置
２１０…ＣＰＵ
２１１…駆動制御部
２１２…異常検出部
２１３…停止制御部
２１４…角度推定部
２１５…セレクタ
２１６…発電制動制御部
２１７…機械制動制御部
２３０…ＲＯＭ
２３５…制御プログラム
２４０…ＲＡＭ
２４５…軌跡データ
３００…ティーチングペンダント

Claims (4)

1. サーボモータと、該サーボモータの回転軸に接続されて該サーボモータの回転角度を検出する角度センサと、を備えるロボットを制御するためのロボット制御装置であって、
   前記角度センサから前記回転角度を取得し、該取得された回転角度に応じて前記サーボモータの駆動をフィードバック制御する第１の駆動制御部と、
   前記サーボモータの電気的変量に基づいて、前記サーボモータの回転角度および回転速度を推定する推定部と、
   前記第１の駆動制御部の代わりに、前記推定部によって推定された回転角度に応じて、前記サーボモータの駆動をセンサレス制御する第２の駆動制御部と、
   前記サーボモータに対して発電制動を行う発電制動部と、
   前記第１の駆動制御部による前記サーボモータの駆動時に、前記角度センサの異常の有無を検出する異常検出部と、
   前記異常検出部によって異常が検出された場合において、前記推定部によって推定された回転速度が、予め定められた閾値以上の場合には、前記第２の駆動制御部に、前記サーボモータを減速させる減速制御を実行させ、前記推定された回転速度が前記閾値未満の場合には、前記発電制動部に、前記サーボモータに発電制動を行う制動制御を実行させることで、前記サーボモータを停止させる停止制御を行う停止制御部と、
   を備え、
   前記閾値は、前記発電制動部による制動トルクが前記サーボモータの定格トルクに一致する回転速度のうち高い側の回転速度よりも低く、かつ、前記発電制動部による制動トルクが最大となる回転速度よりも高い値に設定されている、
   ロボット制御装置。
2. 請求項１に記載のロボット制御装置であって、
   前記ロボットは、複数の前記サーボモータと該各サーボモータにそれぞれ接続された複数の前記角度センサを備え、
   前記異常検出部は、前記複数の角度センサのそれぞれについて異常の有無を検出し、
   前記停止制御部は、前記異常検出部によって異常が検出された角度センサに接続されたサーボモータに対して前記停止制御を実行し、異常が検出されていない角度センサに接続されたサーボモータに対しては、前記第１の駆動制御部によって該サーボモータを停止させる、ロボット制御装置。
3. 請求項１または請求項２に記載のロボット制御装置であって、
   前記ロボットは、更に、前記サーボモータの停止位置を保持するためのメカニカルブレーキを備え、
   前記停止制御部は、前記停止制御後に、前記メカニカルブレーキによって前記サーボモータの停止位置を保持させる、ロボット制御装置。
4. サーボモータと、該サーボモータの回転軸に接続されて該サーボモータの回転角度を検出する角度センサと、を備えるロボットの制御方法であって、
   前記角度センサから前記回転角度を取得し、該取得された回転角度に応じて前記サーボモータの駆動をフィードバック制御する第１の駆動制御工程と、
   前記第１の駆動制御工程における前記サーボモータの駆動時に、前記角度センサの異常の有無を検出する異常検出工程と、
   前記異常検出工程において異常が検出された場合において、前記サーボモータの電気的変量に基づいて前記サーボモータの回転角度および回転速度を推定し、該推定された回転速度が、予め定められた閾値以上の場合には、前記推定された回転角度に応じて前記サーボモータの駆動をセンサレス制御することによって前記サーボモータを減速させる減速制御を実行し、前記推定された回転速度が前記所定の閾値未満の場合には、前記サーボモータに対して発電制動を行う制動制御を実行することで、前記サーボモータを停止させる停止制御工程と
   を備え、
   前記閾値は、前記発電制動部による制動トルクが前記サーボモータの定格トルクに一致する回転速度のうち高い側の回転速度よりも低く、かつ、前記発電制動部による制動トルクが最大となる回転速度よりも高い値に設定されている、
   制御方法。

Images