JP5088217B2 - 産業用ロボットの出力トルク制限回路 - Google Patents

Description

本発明は、サーボモータにより各関節軸が駆動されるようにされた産業用ロボットにおける、出力トルク制限回路に関する。

従来、サーボモータにより各関節軸が駆動されるようにされた産業用ロボットにおける、出力トルクを制限する技術については、非特許文献１に開示されているように、力センサを産業用ロボットの手首先端と作業ツールとの間に配置し、組み付け作業時出力トルクを制限するものであり、許容トルクを越えた場合やあるいは指令トルクと力センサで検出したトルクとが一致しない場合にサーボモータの動力を遮断して安全を確保するものでも示唆するものでも無かった。

これに対して、本出願人が出願した未公開の特許文献１では、力センサからのトルク信号と指令トルク信号とを比較することで、その力センサからのトルク信号の確実性を確保する手法を開示している。一方、特許文献１で記載している、力センサからのトルク信号の確実性を検証するための指令トルク信号は、相互に独立な信号であり、これらの信号を相互に比較することにより、力センサからのトルク信号を用いること無しに出力トルクを制限することが可能となることを意味する。

ロボット工学ハンドブック、日本ロボット学会、１９９０年、ｐ．３９４ 「ＡＣサーボシステムの理論と設計の実際」、総合電子出版社、１９９０年、ｐ．７４〜８１・８６〜８７ 特願２００８−０３９９５９号

本発明が解決しようとする課題は、前述した着眼点から、力センサからのトルク信号を用いることなく、指令トルク信号から演算により求めた産業用ロボットのツール先端での出力トルクが許容値を超えたときに駆動部の動力を遮断することで安全を確保する、出力トルクの制限の解決法を与えるものである。

更に、演算に用いた指令トルク信号が正しいかどうかを、独立な別の指令トルクと比較することで検証することにより、出力トルクが許容値を超えないことを確実にする、出力トルクの制限の解決法を与えるものである。

前述した課題を解決するために、請求項１に記載の発明では、多軸で構成される産業用ロボットの各軸の、モータ駆動電流をトルク合成回路で合成した電流指令トルク信号と、サーボモータドライバのトルク指令部から取り出したドライバ指令トルク信号と、サーボモータドライバのトルク指令モデル演算部で演算したドライバ推測指令トルク信号と、ロボット制御部の内部モデル演算部で演算したモデル推測指令トルク信号の、４つの独立した指令トルク信号から選択回路１で選択した２つの指令トルク信号との間で、設定した許容値以上の不一致を検出した場合に駆動回路を遮断することを特徴とする産業用ロボットの出力トルク制限回路とした。

以上述べたように、本発明の請求項１においては、選択した２つ指令トルク信号を相互に比較することで、指令トルク信号が正しいかどうかの検証を可能とするという効果を奏するものとなった。

本発明の実施の形態の一例を、図を参照して説明する。各図面において、図１は出力トルクの制限回路部、図２はサーボモータ駆動電流を合成するトルク合成回路部、図３はサーボモータを駆動するサーボドライバのドライバ指令トルク信号部、図４はサーボドライバのモデル演算部、図５はロボット制御部の内部モデル演算部、図６は力センサの信号からＸＹＺ各方向のトルク信号を合成する実施例を説明するために用いる３軸の垂直多関節ロボットのモデルを説明する図である。

図１の選択回路１には、サーボモータの駆動電流をトルク合成回路で合成した電流指令トルク信号、サーボドライバのドライバ指令トルク信号、サーボドライバのモデル演算部から出力されるドライバ推測指令トルク信号及び内部モデル演算部で演算されるモデル推測指令トルク信号の４つの指令トルク信号が入力される。この４つの信号はそれぞれ独立な信号であり、選択回路１で２つの信号が選択される。選択された信号をそれぞれ、第１指令トルク信号、第２指令トルク信号とする。

図１の比較回路２には、上記の第１指令トルク信号と第２指令トルク信号とが入力となり、あらかじめ設定された許容値範囲にあるかが比較される。比較の結果、許容値範囲を外れる場合には、不一致検出として、サーボ動力遮断信号を出力する。比較回路２はロボットの軸数分準備され、各軸の不一致検出がＯＲ回路で接続されて、どれか一つの軸で不一致が検出されるとサーボ動力遮断信号の出力となる。

さらに図１のＸＹＺ方向トルク合成回路４には、産業用ロボットの各軸の第１指令トルク信号が入力となり、ＸＹＺ各方向のトルクに合成される。ＸＹＺ各方向のトルクは、ＸＹＺ各方向比較回路５に入力され、ＸＹＺ方向比較回路５では、あらかじめ設定されたトルク規定値６（例えば１５０Ｎｍ）とＸＹＺ各方向別にあるいはＸＹＺ方向を合成した値と比較され、比較の結果、トルク既定値６を超える場合には、サーボ動力遮断の信号の出力となる。

垂直多関節構造のアームを有するロボットでは、アーム自体の質量や負荷の質量に働く重力に抗して姿勢を保持するための姿勢保持トルクが必要となる。この姿勢保持トルクを差し引いたトルクがアーム先端に出力されるトルクとなる。そこで、さらに図１には、第１指令トルク信号から姿勢保持トルクを減算する減算回路７を配置し、減算回路７の出力をＸＹＺ方向トルク合成回路４に入力する。

こうして、第１指令トルク信号から、ＸＹＺ各方向の出力トルクを求め、出力トルクが既定値を超えないようにすると共に、第１指令トルク信号と第２指令トルク信号とを比較することで、第１指令トルク信号そのものが正しいかどうかを検証することで、その回路動作の信頼性を保証できるものとなる。

図１の各軸比較回路８では、第１指令トルク信号から姿勢保持トルクを減算した実出力トルクが、各軸毎に、あらかじめ設定されたトルク規定値６（例えば１５０Ｎｍ）と比較され、比較の結果、トルク既定値６を超える場合には、サーボ動力遮断の信号の出力となる。

図２のサーボモータ動力線に配置されたサーボモータ駆動電流検出回路１０で検出されたサーボモータ駆動電流をトルク合成回路１１で合成して電流指令トルク信号を作り出す。この指令トルク信号を図１の選択回路の入力とする。サーボモータ駆動電流検出の方法及び駆動電流をトルク合成する方法については、非特許文献２に記載されている。

図３にはサーボドライバのトルク指令部を記載する。このトルク指令についても非特許文献２に記載されている。サーボドライバのドライバ指令トルク信号を、図１の選択回路の入力とする。

図４にはサーボドライバのトルク指令部をモデル化したトルク指令モデル演算部を記載する。図３のサーボドライバの位置制御部及び速度制御部をモデル化した演算回路であり、指令位置信号とサーボモータに取り付けられた位置検出器からの現在位置信号とを入力として、演算により、サーボドライバの指令トルク信号を推測するドライバ推測指令トルク信号を出力するものである。このドライバ推測指令トルク信号を図１の選択回路の入力とする。

図５には、ロボット制御部１２に配置した内部モデル演算部１４で演算した推測指令トルクを図１の選択回路の入力とする。内部モデル演算部１４の演算については、図６に基づいて以下に説明する。この内部モデル演算部１４は、ロボット制御部１２に配置したが、同じ演算を行うものであれば、ロボット制御部とは別に設けても良い。

図６に簡単化のために垂直多関節構造３軸のロボットモデルを記載する。３軸より多軸の６軸あるいは７軸のロボットにも順次数式を拡張することで適用可能であるが、数式が煩雑となるため、本願発明の作用を最も効果的に説明できる３軸の構造を例に、以下では説明を行う。

ロボットの動作軸は、ロボット接地面に垂直な軸（Ｚ軸）回りに回転する第１軸（θ１）、その先にＸ軸回りに、ＹＺ平面で回転する第２軸（θ２）、アーム長Ｌ２だけ隔たった位置に、同じくＹＺ平面で回転する第３軸（θ３）、アーム長Ｌ３だけ隔たった位置に負荷ＷＬが取り付けられている。この３軸の構成により、ＸＹＺの空間位置に負荷ＷＬを移動することができる。第２軸及び第３軸のアームには自身の質量Ｗ２及びＷ３がそれぞれ図６に図示するようにＬ２１及びＬ３１の距離の位置を重心位置として配置されている。第１軸は接地面に垂直な軸の回りに回転するため、重力による姿勢保持トルクは必要とされないが、第２軸は、自身の質量Ｗ２に加え、第３軸の質量Ｗ３と負荷の質量ＷＬを保持する、姿勢保持トルクが必要となり、第２軸の出力トルクは、これらの姿勢保持トルクを差し引いたトルクとなる。第３軸は、自身の質量Ｗ３と負荷の質量ＷＬとを保持する、姿勢保持トルクが必要となり、第３軸の出力トルクは、これらの姿勢保持トルクを差し引いたトルクとなる。各軸の角度位置をそれぞれラジアン単位でθ１、θ２、θ３とする。θ２及びθ３については、接地面に平行な線となる角度で表記する。数式を短く表記するため、Ｃｏｓθ１をＣ１、Ｓｉｎθ３をＳ３のように簡易表記する。

まず、図６の負荷ＷＬの位置（ＸＬ，ＹＬ，ＺＬ）は、それぞれ次の式（１）乃至（３）となる。

また、各関節軸の指令トルク信号をＴ１、Ｔ２、Ｔ３と表記すると、第１軸のＸＹＺ合成トルク（Ｔ１Ｘ、Ｔ１Ｙ、Ｔ１Ｚ）は、それぞれ次の式（４）乃至（６）となる。

また、第２軸のＸＹＺ合成トルク（Ｔ２Ｘ、Ｔ２Ｙ，Ｔ２Ｚ）は、それぞれ次の式（７）乃至（９）となる。

また、第３軸のＸＹＺ合成トルク（Ｔ３Ｘ、Ｔ３Ｙ，Ｔ３Ｚ）は、それぞれ次の式（１０）乃至（１２）となる。

また、第１軸から第３軸のＸＹＺ合成トルクを合成したトルク（ＴＸ，ＴＹ，ＴＺ）は、それぞれ次の式（１３）乃至（１５）となる。

これら式（１３）から（１５）により、前述した図１において、ＸＹＺのいずれかの方向について、トルク規定値６を超えるか否かを、ＸＹＺ方向比較回路５により比較し、超える場合にはＸＹＺ方向比較回路５はサーボ動力遮断信号を出力する。あるいは、ＸＹＺ方向のトルクを合成したトルクＴＡを次の式（１６）により演算し、このトルクＴＡがトルク規定値６を超えるか否かを、ＸＹＺ方向比較回路５により判定するようにしても良い。

また、第２軸の質量重心位置（Ｘ２Ｌ、Ｙ２Ｌ、Ｚ２Ｌ）及び第３軸の質量重心位置は、（Ｘ３Ｌ、Ｙ３Ｌ、Ｚ３Ｌ）は、前述した式（１）乃至（３）と同様にして、次の式（１７）乃至（２２）となる。

また、第２軸の姿勢保持トルクＴ２Ｈは、第２軸の質量Ｗ２、第３軸の質量Ｗ３および負荷の質量ＷＬのそれぞれを保持するトルクの和となり、次の式（２３）となる。

また、第３軸の姿勢保持トルクＴ３Ｈは、第３軸の質量Ｗ３および負荷の質量ＷＬのそれぞれを保持するトルクの和となり、次の式（２４）となる。

また、図１に示す減算回路７の動作は次の通りとなる。すなはち、式（７）乃至（９）のＴ２に換えて、Ｔ２’＝Ｔ２−Ｔ２Ｈ、式（１０）乃至（１２）のＴ３に換えて、Ｔ３’＝Ｔ３−Ｔ３Ｈをそれぞれ代入することにより、指令トルク信号から姿勢保持トルクを差し引いた、ＸＹＺ方向の出力トルクを求めることができる。

また、第１軸乃至第３軸の各関節軸の角度を２階微分した加速度をそれぞれα１、α２、α３と置くとき、各関節軸の指令演算トルクＴ１Ｅ、Ｔ２ＥとＴ３Ｅはそれぞれ次の式（２５）乃至（２７）となる。

この式（２５）乃至（２７）の演算を、図５に示すロボット制御部１２の内部モデル演算部１４において行い、得られた指令演算トルクＴ１Ｅ、Ｔ２Ｅ、Ｔ３Ｅを図１に示すモデル推測指令信号として用いる。モータ駆動電流を合成した指令トルク信号や、サーボドライバのトルク指令を用いる場合と比較して、内部モデル演算部で、内部モデルに基づいて演算する場合には、演算分量は増加するが、駆動部に存在する粘性抵抗分に起因する誤差を除外することが可能となる。また、式（２５）乃至（２７）の内部モデル演算は、ロボット制御部１２とは独立した演算部を設けて行っても良い。

上記の実施例では、数式表現を簡単にして、本願発明の内容をより具体的に説明するために垂直多関節構造の３軸ロボットで説明したが、一般的な６軸構成のロボットや冗長軸を有する７軸ロボットの場合にも、数式表現は長くなるが、同じ考え方で適用できることは言うまでもない。また、垂直多関節構造に限定されるものでなく、直動型の軸構成や水平多関節構造のロボットにも、より簡単な演算式で適用可能である。

多軸構成を有する産業用ロボットの出力トルク制限回路において、出力トルクが許容値を超えた場合には動力を遮断して、出力トルクを制限することが可能となる。本願発明の利用は、作業者と産業用ロボットが一緒に作業をする場合に、ロボットの出力トルクが１５０Ｎｍ以下であれば、作業者とロボットとを安全柵で仕切る必要が無くなり、作業者とロボットとの共存環境下での作業が可能となり、作業スペースはより小さくすることが可能となる。

本発明の実施の形態の一例である出力トルク制限回路の説明図である。 本発明の実施の形態の一例であるサーボモータの駆動電流を合成して電流指令トルク信号を作成する制御部の説明図である。 本発明の実施の形態の一例であるサーボドライバのトルク指令をドライバ指令トルク信号とする制御部の説明図である。 本発明の実施の形態の一例であるサーボドライバのモデル演算部の説明図である。 本発明の実施の形態の一例であるロボット制御部の内部モデル演算部の説明図である。 本発明の実施の形態の一例である垂直多関節構造の機構モデルの説明図である。

符号の説明

１ 選択回路
２ 比較回路
３ 許容値
４ ＸＹＺ方向トルク合成回路
５ ＸＹＺ方向比較回路
６ トルク規定値
７ 減算回路
８ 各軸比較回路
９ サーボモータ
１０ 電流検出回路力センサ
１１ トルク合成回路
１２ ロボット制御部
１３ サーボモータドライバ
１４ 内部モデル演算部

Claims (1)

1. 多軸で構成される産業用ロボットの各軸の、
   モータ駆動電流をトルク合成回路で合成した電流指令トルク信号と、
   サーボモータドライバのトルク指令部から取り出したドライバ指令トルク信号と、
   サーボモータドライバのトルク指令モデル演算部で演算したドライバ推測指令トルク信号と、
   ロボット制御部の内部モデル演算部で演算したモデル推測指令トルク信号の、
   ４つの独立した指令トルク信号から選択回路で選択した２つの指令トルク信号の間で、設定した許容値以上の不一致を検出した場合に駆動回路を遮断することを特徴とする産業用ロボットの出力トルク制限回路。
   

Images