JP2009297827A - ロボット暴走判定方法およびロボット制御装置 - Google Patents

Abstract

【課題】
ロボットの各関節の動作範囲データを事前に計算・記憶させる必要が無く、ロボットの動作範囲や動作軌道を時々刻々に変化しても、また、急に未知の負荷を加えても、ロボット関節とロボット全体の暴走を正確で確実に防止できるロボット制御装置を提供すること。
【解決手段】
ロボットの暴走判定方法において、一定周期毎に、関節位置と関節指令位置に基づきロボットの関節へトルク指令を出力することで関節位置を制御し、前回の周期におけるトルク指令と関節位置を記憶し、前回の周期におけるトルク指令と関節位置を用い、現在の周期の関節位置推定値を含む各種状態推定値を外乱オブザーバで演算し、関節位置推定値と今回の周期の関節位置との差の絶対値が、所定の位置誤差閾値より大きい場合に関節を駆動するアクチュエータが暴走していると判定し、前記各関節の暴走判定結果を用いて、ロボットの暴走を判定する。
【選択図】図２

Description

本発明は、特にロボットの暴走判定に関し、精度よく暴走を判定できるロボットの制御装置およびその方法に関する。

ロボットの関節を駆動する駆動源には、制御性やメインテナンス性からサーボモータが多く採用されている。サーボモータは、モータに取り付けられている位置検出器であるエンコーダからモータ位置が検出され、そのモータのモータ位置と指令位置とに基づき演算したトルク指令がモータへ出力されることで位置制御が行われる。その結果としてロボット関節も位置制御されることとなる。これらサーボモータの位置制御を複数のロボット関節に適用することでロボット制御装置が構成される。
しかし、エンコーダの位置情報をロボット制御装置に伝送するに際し、信号線の断線や周囲のノイズの影響などにより、ロボット制御装置がモータ位置を取得できなくなることや、誤ったモータ位置を取得してしまうことがある。係る場合に、誤ったモータ位置を用いて位置制御が行われると、サーボモータに過大なトルク指令が出力されるためにロボットが暴走し、ロボット自身又は周囲の機器を損傷させ、さらには人体へ危害を加えることにもなる。よって、ロボットの暴走を防止することはロボット制御において非常に重要な技術要素の一つである。
ロボットの暴走を防止する従来の技術として、特許文献１および特許文献２に記載された技術が知られている。

特許文献１に記載されている技術は、図６に示すように、ステップ６５１にてロボットの動作時に各軸（関節）の位置を検出し、ステップ６５２にて予め設定された各軸（関節）動作範囲を読み出し、ステップ６５３にて各軸（関節）の位置データと各軸（関節）の動作範囲データと比較してロボット動作時の位置が動作範囲内を外れていると判断した場合、ステップ６５４にてロボットの動作を停止させ、ステップ６５５で警報を発するようにしている。
また、特許文献２に記載されている技術は、図７に示すように、位置ループゲインの逆数を時定数とするローパスフィルタ７０６により指令位置に基づいて推定現在位置を演算し、サーボモータ７０３に取り付けられたエンコーダにて検出された実際の現在位置を取得し、現在位置と推定現在位置との差を差分器７１０にて算出する。サーボモータ７０３を駆動するためのトルク指令値、前記実際の現在位置の微分値、および既知の外乱トルクに基づいて、総オブザーバフィードバック量を状態推定オブザーバ７０４で算出し、この総オブザーバフィードバック量と前記推定現在位置を微分器７０７で微分することにより得られる推定速度とに基づいて異常検知閾値を異常検知閾値算出部７０８にて演算し、前記差分器７１０で得られた値と異常検知閾値算出部７０８にて得られた異常検知閾値を比較して異常状態を判断している。
特開昭６３−１０２８９２ 特開２００５−１８６１８３

しかしながら、特許文献１に記載された従来技術においては、各関節の動作範囲データを予め計算してメモリ等に記憶しておく必要がある。そのため、力制御ロボットや複数台のロボットを協調して動作させる場合には、ロボットの動作範囲や動作軌跡がリアルタイムで変更されるため、動作範囲データを予め計算して記憶しておくことができず、ロボットの暴走を防止できないという問題点があった。
さらに、正常な負荷の範囲であってもロボットに急激な負荷変動を加えた場合には、関節位置が通常の軌道から短時間で大きくずれることがある。このような場合を考慮し、正常負荷範囲でロボットが暴走と誤検知しないように各関節の動作範囲の設定を大きくすると、暴走の誤検知は減るものの暴走検出時間が遅くなるという問題点があった。
また、特許文献２に記載された従来技術においては、状態推定オブザーバ７０４の入力として、実際の現在位置を微分した値である実際の速度を使用している。そのため、サーボモータに取り付けられたエンコーダからの位置情報が断線等の原因で誤った現在位置情報であった場合、状態推定オブザーバ７０４には誤った実際の速度が入力されるため、総オブザーバフィードバック量も誤った値が出力されることとなる。前記の通り、異常検知閾値は総オブザーバフィードバック量に基づき計算されるため、誤った総オブザーバフィードバック量に基づき誤った異常検知閾値が設定されることとなり、適切に暴走検出ができないという問題点があった。

さらに、特許文献２に記載された従来技術では、既知の外乱トルクを状態推定オブザーバ７０４に入力して総オブザーバフィードバック量を算出しているが、ロボットに質量が未知の対象物を搬送させるような場合、適切な既知の外乱トルクを状態推定オブザーバ７０４に入力することができず、総オブザーバフィードバック量の算出値に大きな誤差を生じるため、誤った異常検知閾値が設定されることとなり、適切に暴走検出できないという問題点もあった。
本件発明は、前記のような課題を解決するためになされたもので、ロボットの各関節の動作範囲データを事前に計算・記憶させる必要が無く、ロボットの動作範囲や動作軌道が時々刻々に変化しても、また、急に未知の負荷を加えても、ロボット関節とロボット全体の暴走を確実に防止できるロボット制御装置を提供することにある。

上記課題を解決するため、請求項１に係る発明は、関節を駆動するアクチュエータと、前記関節の関節位置を検出する位置検出器とを備えた関節駆動部を有する複数のロボット関節からなるロボットの暴走判定方法において、一定周期毎に、現在の周期における前記関節位置と関節指令位置に基づいて前記関節駆動部へのトルク指令を演算して出力する位置制御を実行し、少なくとも前回の周期における前記トルク指令と前記関節位置とを記憶しておき、記憶されている前回の周期における前記トルク指令と前記関節位置とを用い、現在の周期における関節推定位置を含む各種状態推定値を外乱オブザーバで演算し、現在の周期における前記関節位置と前記関節推定位置との差の絶対値が、所定の位置誤差閾値より大きい場合に前記ロボット関節が暴走状態と判定すること、を特徴とする。
請求項２に係る発明は、 関節を駆動するアクチュエータと、前記関節の関節位置を検出する位置検出器とを備えた関節駆動部を有する複数のロボット関節からなり、各ロボット関節に指令生成モジュールから関節指令位置を与えて所望の動作をさせるロボットにおいて、一定周期毎に、現在の周期における前記関節位置と前記関節指令位置に基づいて前記関節駆動部へのトルク指令を演算して出力する位置制御を実行する関節制御器と、少なくとも前回の周期における前記トルク指令を記憶するトルク指令記憶手段と、少なくとも前回の周期における前記関節位置を記憶する関節位置記憶手段と、前記トルク指令記憶手段と前記関節位置記憶手段とに記憶している前回の周期における関節位置とトルク指令とを用い、現在の周期における関節推定位置を含む各種状態推定値を演算する外乱オブザーバと、現在の周期における前記関節位置と前記関節推定位置との差の絶対値が、所定の位置誤差閾値より大きい場合に前記ロボット関節が暴走状態と判定するロボット関節暴走判定手段と、各ロボット関節における前記ロボット関節暴走判定手段による暴走判定結果を用い、ロボット全体の暴走を判定するロボット暴走判定手段と、を備えることを特徴とする。

請求項３に係る発明は、請求項２に記載のロボット制御装置において、前記外乱オブザーバは少なくとも外乱トルク推定値および関節推定位置を演算し、前記ロボット関節暴走判定手段は、前記外乱トルク推定値に基づき前記位置誤差閾値を計算する位置誤差閾値計算手段を有し、当該計算結果を前記位置誤差閾値に設定すること、を特徴とする。
請求項４に係る発明は、請求項３に記載のロボット制御装置において、前記位置誤差計算手段が、前記外乱オブザーバから出力された前記外乱トルク推定値に比例して前記位置誤差閾値を計算すること、を特徴とする。
請求項５に係る発明は、請求項３に記載のロボット制御装置において、前記位置誤差計算手段が、前記外乱オブザーバから出力された前記外乱トルク推定値の微分値に比例して前記位置誤差閾値を計算すること、を特徴とする。
請求項６に係る発明は、請求項３に記載のロボット制御装置において、前記位置誤差計算手段が、前記外乱オブザーバから出力された前記外乱トルク推定値および前記外乱トルク推定値の微分値に比例して前記位置誤差閾値を計算すること、を特徴とする。
請求項７に係る発明は、請求項３に記載のロボット制御装置において、前記位置誤差計算手段が、前記外乱トルク推定値に対応する前記位置誤差閾値を予め記憶しておき、前記外乱オブザーバから出力された前記外乱トルク推定値が入力された際に記憶されている前記位置誤差閾値を読み出して決定すること、を特徴とする。

本発明によれば、外乱オブザーバの入力として前回の周期におけるトルク指令と前回の周期における関節位置を用いることで、仮に前回の周期の演算後にエンコーダ断線等の故障が発生しても外乱オブザーバで演算される関節推定位置に影響しないため、関節推定位置とエンコーダ故障後の関節位置とを比較することで、誤ったトルク指令でロボット関節が動作する前に暴走状態を判定できる。
さらに、前回の周期におけるトルク指令と前回の周期における関節位置を用いて外乱オブザーバにて関節推定位置と外乱トルク推定値とを計算するため、未知の荷重を加えても外乱オブザーバが加えた荷重に対応する外乱トルクも推定できる。
以上より、動作範囲データを事前に計算・記憶させる必要が無く、ロボットの動作範囲や動作軌道が時々刻々に変化しても、さらに未知の負荷が加えられても、ロボットの暴走を適切に検知できる。
さらに、請求項３の発明によれば、上記に加え外乱オブザーバで演算された外乱トルク推定値に基づきロボット関節の暴走判定に用いる位置誤差閾値を自動的に設定するため、負荷が変化しても暴走の誤検出や検出遅れも無く、ロボットの暴走を正確で確実に防止できる。

また、請求項４から７の発明によれば、上記に加えロボットの動作状態に適した位置誤差閾値の計算方法を選択することができるようになる。

以下、図に沿って本発明に基づく実施形態について説明する。
図１は、本発明に基づくロボット制御装置の実施形態を示す全体のブロック図である。ロボット制御装置はロボットの運転パターンや軌道データよりロボットの関節への関節指令位置を生成して出力する指令生成モジュール１００、前記関節指令位置に基づきロボットの関節を駆動する複数のロボット関節２００から構成されている。また、各ロボット関節２００は後述する暴走判定結果を相互に共有している。
図２は、一のロボット関節２００の実施形態を示すブロック図である。関節制御部１０は、指令生成モジュール１００から関節指令位置と、後述する関節位置検出器３２より出力された関節位置とに基づき、後述するサーボモータ３１へ一定周期でトルク指令を出力することにより位置制御を実行している。つまり、ロボット関節２００は一定周期で位置制御が実行されている。なお、関節制御部１０における位置制御はサーボモータの位置制御として通常行われるものであり、本発明における主要部分ではないので詳細な説明は省略する。
指令選択部２０は、後述する暴走判定部６０からの暴走判定結果に基づき、サーボモータ３１へのトルク指令の出力を切り替える。具体的には、暴走判定結果が正常（暴走状態ではない）の場合には関節制御器１０より出力されたトルク指令値をそのままサーボモータ３１に出力する。一方、暴走判定結果が暴走である場合には緊急停止用のトルク指令値を出力し、ロボットを緊急停止させる処理を実行する。例えば、サーボモータ３１にはトルク指令値を０として出力し、図示していないがサーボモータ３１がブレーキ付きである場合には、さらにブレーキ指令を出力する。

関節駆動部３０は、駆動源であるサーボモータ３１を備え、サーボモータ３１に付随しているエンコーダを関節位置検出器３２として使用する。また、前述のように図示していないが、サーボモータ３１はブレーキ付きでも良く、指令選択部２０からのブレーキ指令によってサーボモータを機械的に停止できる。関節駆動部３０は、指令選択部２０から出力されたトルク指令値に基づき駆動され、関節位置検出器３２から現在の関節位置を出力する。
トルク指令記憶部４１は前回の周期における関節制御部１０より出力されたトルク指令値を、関節位置記憶部４２は前回の周期における関節位置検出器３２より出力された関節位置を記憶している。外乱オブザーバ５０は、トルク指令記憶部４１に記憶された前回の周期におけるトルク指令値と関節位置記憶部４２に記憶された前回の周期における関節位置とを用いて、少なくとも現在の周期における関節推定位置と外乱トルク推定値を演算する。前回の周期におけるトルク指令値と前回の周期における関節位置とを用いて現在の周期における関節推定位置と外乱トルク推定値を外乱オブザーバ５０で演算することで、仮に前回の周期における演算後に関節位置検出器３２に故障が発生しても、故障の影響がない関節推定位置や外乱トルク推定値を得ることができる。

ここで、外乱オブザーバ５０について詳説する。モータの回転速度をω、慣性モーメントをＪ、制御器から出力したトルクをＴ、外乱によるトルクをＴｄとすると、一般的にモータのダイナミックは下記のように表すことができる。

モータの回転角度をθとして数式１を状態方程式で表すと下記のように表すことができる。

次に、Ｔｄをゼロ次外乱と仮定して外乱項をシステム行列に拡張すると下記のように表すことができる。

上記の数式３を用いて外乱オブザーバ５０を構築すると、図３に示すブロック図のようになる。なお、図３の中のＡ，Ｂ，Ｃは下記のようになり、Ｌは外乱オブザーバゲイン行列であり、オブザーバの極位置によって決定する。

なお、図３の４１は図２のトルク指令記憶部４１、４２は関節位置記憶部４２に該当する部分である。
以上より、図３のようにトルク指令記憶部４１、関節位置記憶部４２、外乱オブザーバ５０を構築することで、現在の周期における関節推定位置と外乱トルク推定値を演算により求めることができる。
暴走判定部６０は、外乱オブザーバ５０から出力された関節推定位置および外乱トルク推定値、関節位置検出器３２から出力された関節位置、に基づき一のロボット関節２００の暴走を判定し、さらに他のロボット関節２００からの暴走判定結果を合わせ考慮し、最終的に暴走判定結果を指令選択部２０に出力する。
図４は暴走判定部の詳細ブロック図であり、これに基づき説明する。
外乱オブサーバ５０から出力された関節推定位置と関節位置検出器３２から出力された関節位置を差分器６１に入力して差分値を取り、その差分値を絶対値変換器６２にて差分絶対値に変換して比較器６４に入力する。一方、位置誤差閾値計算部６３にて、外乱オブサーバ５０から出力された外乱トルク推定値に基づき位置誤差閾値を決定し、位置誤差閾値を比較器６４に入力する。比較器６４は、入力された差分絶対値と位置誤差閾値を比較し、差分絶対値が位置誤差閾値より大きい場合にはロボット関節２００が暴走状態と判定し暴走判定結果として１を出力し、差分絶対値が位置誤差閾値より小さい場合にはロボット関節２００が正常と判定し暴走判定結果として０を出力する。

係る構成により、負荷が変動することによりロボット関節の位置精度が変化しても、負荷等による外乱トルクを推定し、その外乱トルク推定値に基づき位置誤差閾値を自動的に調整するため、暴走の誤判定による誤停止や暴走判定の遅れがない。
論理和計算器６５は、比較器６４から出力された当該ロボット関節２００の暴走判定結果と、他のロボット関節２００の暴走判定結果との論理和を出力する。つまり、全てのロボット関節２００が正常と判定された場合にのみ暴走判定結果は正常として０を出力し、いずれか１つでもロボット関節２００が暴走と判定された場合には暴走判定結果は暴走として１を出力する。以上より、ロボットの暴走を確実に検出して停止させることができる。
ここで、実際の位置誤差閾値計算部６３での位置誤差閾値の計算方法を詳しく説明する。位置誤差閾値は、重い物をゆっくり搬送するのか、軽い物を短時間で搬送するのか等のロボットの使用状況により適切な値が変わるため、以下の計算方法から使用状況にあった適切な計算方法を選択して閾値を設定することが望ましい。
第１の位置誤差閾値計算方法として、外乱トルクに比例した位置誤差閾値とすることができる。具体的には、外乱オブザーバ５０で推定した外乱トルク推定値をＴｄ、ロボット関節２００が許容できる外乱トルクの最大値をＴｄ_max、外乱が無い場合の位置誤差許容範囲をＥ_min、最大外乱Ｔｄ_maxを受けた時の位置誤差許容範囲の最大値をＥ_maxとしたときに、数式５により位置誤差閾値を求める。

第２の位置誤差閾値計算方法として、外乱トルクの微分に比例した位置誤差閾値とすることができる。具体的には、外乱オブザーバ５０で推定した外乱トルク推定値の微分をｄＴｄ／ｄｔ、ロボット関節２００が許容できる外乱トルクの最大変化率をｄＴｄ_max／ｄｔ、数式５と同様に外乱トルクが無い場合の位置誤差許容範囲をＥ_min、ｄＴｄ_max／ｄｔを受けた時の位置誤差許容範囲の最大値をＥ_maxとしたときに、数式６により位置誤差閾値を求める。

第３の位置誤差閾値計算方法として、外乱トルクおよび外乱トルクの微分に比例して位置誤差閾値とすることができる。具体的には、外乱オブザーバ５０で推定した外乱トルク推定値をＴｄ、ロボット関節２００が許容できる外乱トルクの最大値をＴｄ_max、外乱オブザーバ５０で推定した外乱トルクの微分をｄＴｄ／ｄｔ、ロボット関節２００が許容できる外乱トルクの最大変化率をｄＴｄ_max／ｄｔ、外乱トルクが無い場合の位置誤差許容範囲をＥ_min、最大外乱トルクＴｄ_maxを受けた時の位置誤差許容範囲の最大値をＥ_maxとし、さらに、外乱トルクの比例成分のウェートｋ₁、外乱トルクの微分成分のウェートｋ₂としたときに、数式７により位置誤差閾値を求める。なお、ｋ₁およびｋ₂は位置誤差閾値を適切に調整するために任意に変更できる。

第４の位置誤差閾値計算方法として、外乱オブザーバ５０で推定された外乱トルクに対応する予め記憶された位置誤差閾値を選択することができる。具体的には、図５に示す関係のように、外乱トルクが無い場合の位置誤差許容範囲Ｅ_minから最大外乱トルクＴｄ_maxを受けた時の位置誤差許容範囲Ｅ_maxの間で、外乱トルク推定値に対応する位置誤差閾値を予め設定して記憶する。外乱オブザーバ５０で推定した外乱トルク推定値に基づき、記憶された位置誤差閾値を読み出して設定する。

本発明に係るロボット制御装置の全体ブロック図である。 本発明に係るロボット関節のブロック図である。 本発明に係る外乱オブザーバの詳細ブロック図である。 本発明に係る暴走判定部および指令選択部の詳細ブロック図である。 本発明に係る第４の位置誤差閾値設定方法における外乱トルク値と位置誤差閾値の関係を示した図である。 特許文献１に記載されているロボットの暴走を防止する技術のフローチャートである。 特許文献２に記載されているロボットの暴走を防止する技術のブロック図である。

符号の説明

１０ 関節制御部
２０ 指令選択部
３０ 関節駆動部
４１ トルク指令記憶部
４２ 関節位置記憶部
５０ 外乱オブザーバ
６０ 暴走判定部
１００ 指令生成モジュール
２００ ロボット関節

Claims (7)

1. 関節を駆動するアクチュエータと、前記関節の関節位置を検出する位置検出器とを備えた関節駆動部を有する複数のロボット関節からなるロボットの暴走判定方法において、
   一定周期毎に、現在の周期における前記関節位置と関節指令位置に基づいて前記関節駆動部へのトルク指令を演算して出力する位置制御を実行し、
   少なくとも前回の周期における前記トルク指令と前記関節位置とを記憶しておき、
   記憶されている前回の周期における前記トルク指令と前記関節位置とを用い、現在の周期における関節推定位置を含む各種状態推定値を外乱オブザーバで演算し、
   現在の周期における前記関節位置と前記関節推定位置との差の絶対値が、所定の位置誤差閾値より大きい場合に前記ロボット関節が暴走状態と判定すること、
   を特徴とするロボット暴走判定方法。
2. 関節を駆動するアクチュエータと、前記関節の関節位置を検出する位置検出器とを備えた関節駆動部を有する複数のロボット関節からなり、各ロボット関節に指令生成モジュールから関節指令位置を与えて所望の動作をさせるロボットにおいて、
   一定周期毎に、現在の周期における前記関節位置と前記関節指令位置に基づいて前記関節駆動部へのトルク指令を演算して出力する位置制御を実行する関節制御器と、
   少なくとも前回の周期における前記トルク指令を記憶するトルク指令記憶手段と、
   少なくとも前回の周期における前記関節位置を記憶する関節位置記憶手段と、
   前記トルク指令記憶手段と前記関節位置記憶手段とに記憶している前回の周期における関節位置とトルク指令とを用い、現在の周期における関節推定位置を含む各種状態推定値を演算する外乱オブザーバと、
   現在の周期における前記関節位置と前記関節推定位置との差の絶対値が、所定の位置誤差閾値より大きい場合に前記ロボット関節が暴走状態と判定するロボット関節暴走判定手段と、
   各ロボット関節における前記ロボット関節暴走判定手段による暴走判定結果を用い、ロボット全体の暴走を判定するロボット暴走判定手段と、
   を備えることを特徴とするロボット制御装置。
3. 前記外乱オブザーバは少なくとも外乱トルク推定値および関節推定位置を演算し、
   前記ロボット関節暴走判定手段は、前記外乱トルク推定値に基づき前記位置誤差閾値を計算する位置誤差閾値計算手段を有し、当該計算結果を前記位置誤差閾値に設定すること、を特徴とする請求項２記載のロボット制御装置。
4. 前記位置誤差計算手段が、前記外乱オブザーバから出力された前記外乱トルク推定値に比例して前記位置誤差閾値を計算すること、を特徴とする請求項３に記載のロボット制御装置。
5. 前記位置誤差計算手段が、前記外乱オブザーバから出力された前記外乱トルク推定値の微分値に比例して前記位置誤差閾値を計算すること、を特徴とする請求項３に記載のロボット制御装置。
6. 前記位置誤差計算手段が、前記外乱オブザーバから出力された前記外乱トルク推定値および前記外乱トルク推定値の微分値に比例して前記位置誤差閾値を計算すること、を特徴とする請求項３に記載のロボット制御装置。
7. 前記位置誤差計算手段が、前記外乱トルク推定値に対応する前記位置誤差閾値を予め記憶しておき、前記外乱オブザーバから出力された前記外乱トルク推定値が入力された際に記憶されている前記位置誤差閾値を読み出して決定すること、を特徴とする請求項３に記載のロボット制御装置。
   
   

Images