JP2006116635A - ロボットの制御装置 - Google Patents

Abstract

【課題】 演算式が簡単で処理時間が短く、動作指令が急激に変化する場合にも誤検出することなく、周辺機器との予期せぬ接触をロボットアーム全体にわたって高感度に検出できるロボットの制御装置を提供する。
【解決手段】 各関節のアクチュエータの出力を制御する手段と前記各関節の変位量を計測する手段を有するロボットを備えたロボットの制御装置において、ロボット１０１を模擬するロボットモデル１０２と、制御演算部１０３を模擬する制御演算モデル部１０４と、複数のリンクの位置姿勢についてロボット１０１とロボットモデル１０２とを比較する位置姿勢比較部１０７とを備えたものである。これにより、サーボ遅れによる誤検出を防ぐことができ、また、複数のリンクを監視することで、ロボット全体の高感度な接触検出を行うことができる。
【選択図】 図１

Description

本発明は、サーボモータ、減速機からなる各関節のアクチュエータの出力を制御するロボットの制御装置に関する。

従来、ロボットの動作軌道上に対象物であるワークなどの周辺機器が存在し、ロボットの可動部が予期せずして周辺機器に接触した際、ロボット制御装置は与えられた位置指令を実現するためにロボットの関節部に大きなトルクを発生させることになる。ロボットの関節部における大トルク発生は、ロボットの機構部やモータあるいは周辺機器を破損させ、危険であるため、触覚センサ、力センサ、超音波センサあるいは赤外線センサなどの近接センサなどを用いて周辺機器との接触や接近を検出し、接触の前後直ちにロボットを停止させるなどの保護手段を講じる必要があった。しかし、ロボットの動作範囲全体にわたって接触あるいは近接を検出可能なセンサを配置することはコストの増大を招く。また、ロボットの動作範囲を広範囲にわたって計測できる信頼性の高いセンサは提供されていない。したがって、コストをかけずに新たなセンサを使用することなく、ロボットと周辺機器との予期せぬ接触により生じる危険を回避する方法が求められる。

このような問題に対して、ロボットの到達許容範囲を設定して周辺機器との予期しない接触を防ぐ方法、ロボットの周辺機器との予期しない接触を検出してロボットを停止させる方法がある。

第１従来技術として、ロボットの到達許容範囲を設定して周辺機器との予期しない接触を防ぐ方法では、ロボットの各関節変位量の到達許容範囲をあらかじめ定義しておき、計測される関節変位量（以下、実機関節応答と表記）が到達許容範囲内から外れたときにロボットを停止させる方法が一般的である。また、ロボットの位置および姿勢の到達許容範囲をロボットの作業空間内であらかじめ定義しておき、計測される位置および姿勢（以下、実機位置応答と表記）が到達許容範囲内から外れたときにロボットを停止させる方法も知られている。

また、第２従来技術として、ロボットの周辺機器との予期しない接触を検出してロボットを停止させる方法には、作業空間におけるロボットへの指令位置と実機位置応答との差あるいは関節空間における指令関節変位量と実機関節応答との差を監視し、その差が閾値を超えると、接触が起きたものとしてロボットを停止させる方法がある（特許文献１）。
図９は、第２従来技術となる特許文献１におけるロボットに搭載したモータの制御系の１軸分の回路構成を示したブロック図である。
図９において、１１はサーボアンプ、１２はサーボモータである。１３は位置偏差であり、サーボモータ１２の指令回転角度から実際の回転角度を引いたものである。位置偏差１３と回転角度設定値（作業座標系設定値）１４は比較手段１５で比較され、位置偏差１３が回転角度設定値１４を上回っているときを検出する。

さらに、上記に述べたロボットの周辺機器との接触を検出してロボットを停止させる方法には、第３従来技術として状態推定オブザーバを用いる方法もある（例えば、特許文献２）。
図１０は第３従来技術となる特許文献２におけるロボット制御装置の基本的な構成を示した図である。
図１０において、２１はサーボモータの制御手段、２２はサーボモータ、２３はロボットアーム、２４は２慣性系状態推定オブザーバ、２５は接触判定手段、２６は装置保護手段、２７は減速機である。２慣性系状態推定オブザーバ２４はモータ回転角度、モータ回転角速度およびモータ加速度指令を用いて負荷側外乱力を推定する。接触判定手段２５は外乱力推定値が設定閾値を超えたときにロボットアーム２３が外部装置に接触したと判定し、装置保護手段２６に信号を出力する。

その他の方法に第４従来技術として、モータに流れる電流波形の乱れからロボットの接触を検出する方法が開示されている（特許文献３）。
特開平０９−１７９６３２号公報（明細書第１８頁、図７） 特開２０００−５２２８６号公報（明細書第６頁、図１） 特開平０２−０５９２９１号公報（明細書第３頁、図１）

以上に示した従来のロボットの制御装置には以下の問題があった。
第１従来技術のように、ロボットの到達許容範囲を設定して周辺機器との接触を防ぐ方法では、ロボットの作業環境が変化した場合であって、予め設定していた到達許容範囲内に周辺機器が入り込んだ場合には、ロボットと周辺機器の接触が起きる可能性があり、接触時に両者が破壊する可能性があった。

また、第２従来技術のように、ロボットの周辺機器との接触をロボットの実機関節応答や実機位置応答をロボットに与えた関節角度指令や位置指令と比較して検出する方法では、実機の応答の位置指令に対するサーボ遅れが存在するため、関節角度指令や位置指令が急激に変化した場合に一時的に実機関節応答や実機位置応答との差が大きくなり、この差が閾値を超えると誤検出になる可能性があった。
ここで、図５は第３従来技術を説明するための１方向における位置応答波形の例を示したものである。
図において、５０１は位置指令、５０２は位置指令５０１に対応する位置応答、５０３は位置指令と位置応答との差の最大値である。一般には５０１のようなステップ上の位置指令を与えることはないが、問題点を明確にするための例として用いることにする。指令が急激に変化すると、ロボットが周辺機器と接触しなくとも指令の変化にロボットが追従できないため、位置指令と位置応答との差が大きくなってしまう。これはサーボ遅れによるものであり、位置指令と位置応答との差が大きくなって閾値を超えると誤検出となる。更に、ロボットの実機関節応答をロボットに与えた関節角度指令と比較する方法では閾値を角度で与えねばならないため、作業空間におけるロボットの動作範囲を適切に制限するのには向かなかった。またロボットの実機位置応答をロボットに与えた位置指令と比較する方法では、手先以外のリンクが周辺機器に接触したときに高感度に接触検出できないという欠点があった。

また、第３従来技術のように、状態推定オブザーバによって外乱力を推定する方法では、オブザーバが実機応答に追従していくためオブザーバの時定数だけ検出が遅れるという問題があった。また演算式が複雑になり計算時間が長くなるという問題もあった。

また、第４従来技術のように、電流波形の乱れから接触を検出する方法では、位置指令が急激に変化したときの急激なトルク増大なども検出されてしまい、誤検出になるという問題があった。

本発明はこのような問題点に鑑みてなされたものであり、演算式が簡単で処理時間が短く、動作指令が急激に変化する場合にも誤検出することなく、周辺機器との予期せぬ接触をロボットアーム全体にわたって高感度に検出できるロボットの制御装置を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したものである。
請求項１のロボットに係る発明は、各関節のアクチュエータの出力を制御する手段と前記各関節の変位量を計測する手段を有するロボットを備えたロボットの制御装置において、前記アクチュエータへの制御入力値を計算し出力する制御演算部と、前記ロボットを模擬するロボットモデルと、前記制御演算部を模擬する制御演算モデル部と、前記ロボットの少なくとも１つ以上のリンク位置および姿勢を関節変位量から計算するリンク位置姿勢算出部と、前記ロボットモデルのモデル関節変位量から前記リンクに対応するモデルリンク位置および姿勢を計算するモデルリンク位置姿勢算出部と、前記リンク位置とモデルリンク位置とを比較して結果を出力する位置姿勢比較部と、を備えたことを特徴としている。

請求項２の発明は、請求項１に記載のロボットの制御装置において、前記位置姿勢比較部の比較結果に応じて該結果を操作者に知らせる通知手段、ロボットを停止させる停止手段、ロボットの動作指令を切り換える動作指令切替手段あるいはロボットの制御入力を切り換える制御入力切替手段のうち、少なくとも１つを備えたことを特徴としている。

請求項３の発明は、請求項１または２に記載のロボットの制御装置において、前記ロボットモデルの動作を画像として前記ロボットの動作に同期して操作者に提供するモデル動作可視化手段を有することを特徴としている。

請求項４の発明は、請求項１〜３のいずれか１項に記載のロボットの制御装置において、前記位置姿勢比較部は、前記リンク位置および姿勢を表すリンク位置姿勢ベクトルと前記モデルリンク位置および姿勢を表すモデルリンク位置姿勢ベクトルとの差の大きさを監視し、差の大きさが閾値を上回ったときに接触検出信号を出力することを特徴としている。

請求項５の発明は、請求項１〜３のいずれか１項に記載のロボットの制御装置において、前記位置姿勢比較部は、前記リンク位置姿勢ベクトルの各成分と前記モデルリンク位置姿勢ベクトルの各成分との差の大きさを個別に監視し、それぞれの差の大きさのうち少なくとも１つの大きさが閾値を上回ったときに接触検出信号を出力することを特徴としている。

請求項６の発明は、請求項１〜５のいずれか１項に記載のロボットの制御装置において、前記ロボットモデルは、１慣性系モデルをロボットの関節数と同数だけ並べて構成することを特徴としている。

請求項７の発明は、請求項１〜６のいずれか１項に記載のロボットの制御装置において、冗長自由度を持ったロボットの動作を監視する手段を有することを特徴としている。

請求項１に記載の発明によると、ロボットの関節変位量は接触の影響を受けるが、モデル関節変位量は接触の影響を受けないため、両者を比較することで接触検出をすることができる。ロボットモデルでは実機部のサーボ遅れも模擬できるため、ロボットへの指令値が急激に変化することによる誤検出がないという利点もある。また複数のリンクについて位置姿勢を監視することで、手先以外の部分で周辺機器との接触が起こった場合も高感度に検出することができる。

請求項２に記載の発明によれば、接触が起こったときにロボットや周辺機器に与える被害を少なくするような措置を確実にとることができる。

請求項３に記載の発明によると、操作者はロボットモデルの動作状況をロボットの動作状況と同時に観察することができる。両者を比較することで操作者はロボットが正常に動作しているかを確認でき、異常があった場合には異常箇所の発見が容易にできる。

請求項４に記載の発明によれば、接触によって実機部の手先位置・姿勢がロボットモデルの手先位置・姿勢から乖離したときを確実に検出することができる。また、ロボットとロボットモデルのリンク位置・姿勢の作業空間における乖離許容距離が閾値となるため、閾値設定が容易である。

請求項５に記載の発明によれば、ある方向には確実に接触検出をして、別の方向には接触を許容することが可能となる。ロボットのリンク位置姿勢の成分毎に接触検出閾値を個別に設定することができる。

請求項６に記載の発明によれば、極めて簡単なモデルによってロボットの実機部を模擬することができる。ロボットモデルの状態量を更新するための計算量を少なくすることができる。

請求項７に記載の発明によれば、冗長ロボットアームに対しても、ロボットアーム全体にわたって周辺機器との接触有無の監視ができ、関節角度や手先の位置・姿勢のみを監視する場合に比べて高感度に接触を検出することができる。

以下、本発明の実施例を図に基づいて説明する。

図１は、本発明の実施例を示すロボットの制御装置のブロック図である。
図において、１０１は制御対象とするロボット、１０２はロボット１０１を模擬したロボットモデル、１０３は制御演算部、１０４は制御演算モデル部、１０５はリンク位置姿勢算出部、１０６はモデルリンク位置姿勢算出部、１０７は位置姿勢比較部、１０８は装置保護手段、１０９は通知手段、１１０はロボットモデルの動作画像を提供する動作可視化手段である。また１１１はロボット１０１の目標関節変位量、１１２は関節変位量、１１３はモデル関節変位量、１１４はアクチュエータへの制御入力値、１１５はロボットモデルへの制御入力値、１１６は監視対象とする１つ以上のリンクの位置と姿勢を表すリンク位置姿勢、１１７はロボットモデル１０２における前記リンク位置姿勢に対応する信号であるモデルリンク位置姿勢、１１８は接触検出信号である。ロボットモデル１０２および制御演算モデル部１０４は、目標関節変位量１１１に対する関節変位量１１２の応答特性を模擬するように構成する。なお、ロボット１０１は、各関節のアクチュエータの出力を制御する手段と該各関節の変位量を計測する手段を有するものとなっている（何れも不図示）。

図２は図１に基づいてその詳細を説明するための制御系のブロック図を示したものである。すなわち、図１に示すロボット１０１、ロボットモデル１０２、制御演算部１０３、制御演算モデル部１０４の構成が、それぞれ図２における１０１’、１０２’、１０３’、１０４’の構成に対応している。
図２において、１０１’はｎ個の回転関節と各関節のトルクを制御可能なモータを有するロボットアーム、１０２’はロボットアーム１０１’の各関節トルクに対する関節角度の応答特性を模擬する１慣性系モデルをｎ個並べたアームモデル、１０３’はロボットアーム１０１’の各関節角度を制御するための制御演算部、１０４’は制御演算モデル部、１０５’は監視対象とするｋ個のリンク位置姿勢を計算する位置姿勢算出部、１０６’は監視対象のｋ個のリンク位置姿勢に対応するモデルリンク位置姿勢算出部、１０７’は位置姿勢比較部である。また１０８’はロボットを停止させるための装置保護手段となるブレーキ、１０９’は接触検出を操作者に伝えるためのアラーム、１１０’はモデルの動作を動画として提供するためのモニタである。
また、ブレーキ１０８’は、接触検出信号１１８’をうけると直ちにロボットアーム１０１’に停止信号を送り関節部の動作を停止させる。その結果、ロボットアームが動き続けて接触した周辺機器やロボットアーム自体の破壊を防止するようになっている。
また、アームモデル１０２’の動作はモニタ１１０’によって実時間表示し、操作者の状況把握を支援するようになっている。操作者自身がロボットアーム１０１’とアームモデル１０２’とを比較して異常を発見することができる他、接触検出時にはロボットがどのような動作をしようとした結果、接触が起こったのかを容易に知ることができる。また操作者とロボットとの距離が離れている場合でも、モニタ１１０’の表示によってロボットの動作状況がある程度把握できるようになっている。

以下の説明のため、図中の信号の数式表現を与える。
１１１’はロボットアーム１０１’の関節角度指令であり
θ^r=[θ₁ ^r,・・・,θ_n ^r]^T ・・・（１）
と表現し、１１２’はロボットアーム１０１’の関節角度であり
θ=[θ₁,・・・,θ_n]^T ・・・（２）
と表現し、１１３’はモデルの関節角度であり
θ^m=[θ₁ ^m,・・・,θ_n ^m]^T ・・・（３）
と表現し、１１４’はモータへのトルク指令値であり
τ=[τ₁,・・・,τ_n]^T ・・・（４）
と表現し、１１５’はロボットモデルへのトルク入力値であり
τ^m-=[τ₁ ^m,・・・,τ_n ^m]^T ・・・（５）
と表現し、１１６’は監視対象とするｋ個のリンクに関するリンク位置姿勢であり
X₁,・・,X_k, X_i==[x_i ¹, x_i ², x_i ³, x_i ⁴, x_i ⁵, x_i ⁶]^T, i=0,・・・,k ・・・（６）
とそれぞれ表現し、１１７’はリンク位置姿勢１１６’に対応するモデルリンク位置姿勢であり、
X^m ₁,・・・,X^m _k, X^m _i==[x_i ^m1, x_i ^m2, x_i ^m3, x_i ^m4, x_i ^m5, x_i ^m6]^T, i=0,・・・,k ・・・（７）
とそれぞれ表現する。１１８’は接触検出信号である。また２２１はロボットアームの第１関節に印加するトルクであり、トルク指令値１１４’の第１成分である。２２２はロボットアームの第ｎ関節に印加するトルクで、トルク指令値１１４’の第ｎ成分である。２２３は関節角度１１２’の第１成分、２２４は第ｎ成分である。２２５はロボットモデルへのトルク入力値１１５’の第１成分、２２６は第ｎ成分である。２２７はモデルの関節角度１１３’の第１成分、２２８は第ｎ成分である。

また、図２中の制御演算部１０３’において、２０１は位置制御ゲイン回路、２０２は微分器、２０３は速度制御ゲイン回路、２０４は出力制限器、２０５は重力・摩擦力補償器、２０６は出力変換定数回路である。このように制御演算部を構成することで、ロボットアーム１０１’は関節角度指令１１１に追従しようとする。しかし関節角度指令が急激に変化するときは、ロボットアーム１０１’が目標に対して遅れて追従する、すなわちサーボ遅れがあるため、一時的に関節角度指令１１１’と関節角度１１２’との差が増大することがある。

図２中のアームモデル１０２’において、２１１はロボットアームの第１関節の印加トルク指令に対する応答を模擬する１慣性系剛体モデルであり、２１２はロボットアームの第ｎ関節の印加トルクに対する応答を模擬する１慣性系剛体モデルである。第ｉ関節の１慣性系剛体モデルの入出力関係は次の式とする。
Ｌ(θ_i ^m)=1/(j_i s²)・Ｌ(τ_i) i=0、・・・、n ・・・（８）
ここでＬ（・）は、ラプラス変換作用素を表し、ｓはラプラス変換のｓである。Ｊ_ｉはロボットアームの第ｉ関節まわりの等価慣性モーメントである。実際にはロボットアームの関節回りの慣性モーメントはアーム姿勢によって複雑に変化するが、モデルでは定数Ｊ_ｉで代表する。また、２０７から２１０は制御演算モデル部１０４を構成しており、２０７は位置制御ゲイン回路、２０８は微分器、２０９は速度制御ゲイン回路、２１０は出力制限器である。

したがって、第１実施例に係るロボット制御装置は、上記のごとくアームモデル１０２’と制御演算モデル部１０４’を構成するようにしたので、制御演算部１０３’とロボットアーム１０１’における関節角度指令１１１’に対する関節角度１１２’の応答を模擬することができる。その結果、ロボットアームと周辺機器との接触が起こらなければ、モデルの関節角度１１３は関節角度１１２’にほぼ一致する。
図６は本発明の実施例を説明するための１つのリンクの１方向におけるロボットおよびロボットモデルの位置応答波形の例を示した図である。図において、６０１がモデル位置応答の波形である。仮に位置指令が急激に変化しても、位置応答とモデル位置応答との相違は僅かであるため誤検出は起こらない。ロボットアームが接触力を受けるとリンク位置姿勢１１６’はその影響を受けるが、モデルリンク位置姿勢１１７’は接触の影響を受けないため、両者の間に乖離が生じる。これを位置姿勢比較部で監視して接触検出をするのである。

次に、本発明の第２実施例について説明する。
図３は、本発明の第２実施例を示す位置姿勢比較部１０５’のブロック図である（構成例１）。
以下ではロボットアーム１０１’のリンクのうち監視対象とするｋ個のリンクに１からｋまでの番号をつけ、それぞれリンクｉ（ｉ＝０，・・・，ｋ）のように表現する。構成例１では、位置姿勢比較部１０５’をリンク１の位置姿勢比較部からリンクｋの位置姿勢比較部までを並列に並べたものとする。各リンクの位置姿勢比較部には対応するリンク位置姿勢とモデルリンク位置姿勢が入力され、並列に行われる比較演算の結果に基づいて接触検出信号を出力する。
図３において、３０１はリンク１の位置姿勢比較部、３０２はリンクｉの位置姿勢比較部、３０３は絶対値計算手段、３０４は比較演算手段、３１１はリンク１の位置姿勢、３１２はリンクｉの位置姿勢、３１３はリンクｋの位置姿勢、３１４はモデルリンク１の位置姿勢、３１５はモデルリンクｉの位置姿勢、３１６はモデルリンクｋの位置姿勢、３１７はリンクｉの位置姿勢偏差絶対値、３１８はリンクｉの閾値
δ_ii=1,・・・,k ・・・（９）
である。まず、入力されたリンクｉの位置姿勢とモデルリンクｉの位置姿勢との差をとり、絶対値計算手段３０３で次の式によりリンクｉの位置姿勢偏差絶対値３１７を計算する。
| X_i- X^m _i|=( X_i - X^m _i ) ^T( X_i - X^m _i )^1/2 ・・・（１０）
比較演算手段３０４ではリンクｉの位置姿勢偏差絶対値３１７とリンクｉの閾値３１８とを比較し、
δ_i<| X_i - X^m _i| ・・・（１１）
となったときに接触検出信号を出力する。
したがって、第２実施例は上記のように状態比較部１０５’を構成すれば、単純な演算によってロボットアームと周辺機器との接触を精度よく検出することができる。

図７はロボットアームの手先と周辺機器との間で接触が起こった場合の、ロボットアームモデルとの比較を示した模式図である。
図において、１０１’’は３リンクのロボットアーム、１０２’’は対応するアームモデル、７０１は周辺機器、７１１はリンク１の位置偏差絶対値、７１２はリンク２の位置偏差絶対値、７１３はリンク３の位置偏差絶対値である。
リンク３は周辺機器との接触の影響を受けるため、接触の影響を受けないモデルリンク３との乖離が大きくなっている。図では位置のみの偏差を表示しているが、リンクの姿勢についてもロボットアームとモデルとの乖離が生じている。偏差の絶対値が閾値を超えたときに接触検出信号が出力される。

次に、本発明の第３実施例について説明する（構成例２）。
図４は、本発明の第３実施例を示す状態比較部１０５’のブロック図である（構成例２）。
第３実施例が第２実施例と異なる点は、状態比較部１０５’において、ロボットアームとアームモデルの位置姿勢偏差を成分毎に計算し個別の閾値と比較する点である。なお、リンク１の位置姿勢比較部からリンクｋの位置姿勢比較部までを並列に並べたものとするのは構成例１と同様である。
図４において、４０１はリンク１の位置姿勢比較部、４０２はリンクｉの位置姿勢比較部、４０３は絶対値ベクトル計算手段、４０４は成分比較演算手段、４１１はリンクｉの閾値ベクトル
[δx_i ¹,δx_i ², ・・・, δx_i ⁶]^T ・・・（１２）
、４１２はリンクｉの位置姿勢偏差絶対値ベクトル
[ |x_i ¹ - x_i ^m1|, |x_i ²- x_i ^m2|,・・・, |x_i ⁶ - x_i ^m6|]^T ・・・（１３）
である。まず、入力されたリンクｉの位置姿勢とモデルリンクｉの位置姿勢との差をとり、絶対値ベクトル計算手段４０３で式１３のようにリンクｉ位置姿勢偏差の各成分の絶対値を計算する。成分比較演算手段４０４ではリンクｉの位置姿勢偏差絶対値ベクトル４１２とリンクｉの閾値ベクトル４１１とを比較し、
δx_i ^j<| x_i - x_i ^mj| ・・・（１４）
がｊ＝１，２，・・・，６のうち少なくとも１つについて満足される場合に接触検出信号を出力する。
したがって、第３実施例は上記のようにすれば、ある方向には感度よく接触を検出し、ある方向には接触を検出しないようにすることが可能となる。ロボットに周辺機器との接触を伴う作業を行わせる際には、作業に伴って接触が起こる向きの閾値を大きくして不要な接触検出が起きないようにし、作業に伴う接触が起こらない向きの閾値は通常通りとして予期せぬ接触を確実に検出するようにすればよい。

次に、本実施例が有するもう１つの効果を、図８を用いて説明する。
図８は多自由度のロボットアームにおける途中のリンクが周辺機器と接触した場合の、ロボットアームとの比較を示した模式図である。
図８において、８０１は冗長自由度を有する多自由度ロボットアーム、８０２は対応するアームモデル、８０３は位置・姿勢を監視するリンク１、８０４は位置・姿勢を監視するリンク２、８０５は周辺機器を表す。
周辺機器８０５との接触によってリンク８０４を含むロボットアーム８０１の一部がアームモデル８０２から大きく離れているが、リンク８０３についてはロボットアーム８０１とアームモデル８０２はほぼ一致している。
したがってリンク８０３のみ、すなわち手先のみの位置・姿勢監視では、図のような周辺機器との接触を検出することができない。しかし監視対象となるリンクを８０３と８０４の２つに増やし、位置・姿勢監視リンク２の位置・姿勢についても実機部とモデルとを比較・監視していれば、このような場合でもロボットアームと周辺機器との接触を高感度に検出することができる。その結果、ロボットアーム全体にわたって接触有無の監視ができ、関節角度や手先の位置・姿勢のみを監視する場合に比べて高感度に接触を検出できるのである。また、ロボットモデルは、実際にロボットを動作させる前にあらかじめロボットモデルにだけ指令を与えて位置に限らず速度やトルクなどが達成可能であるかを検証するのにも用いることができる。これにより、ロボットが予期せぬ動きをしないかを事前に確認できる。特に冗長自由度を持ったロボットにおける異常解による急激な動作の防止に有効である。

最後に、本発明が従来技術２〜４に相当する特許文献１〜３と異なる点を以下に整理して述べる。
本発明が特許文献１と異なる点は次の２点である。
（１）ロボットを模擬するロボットモデルを備え、ロボットの応答とロボットモデルの応答とを比較するようにした点である。これにより、ロボットモデルを用いることによってロボットがあるべき状態をロボットモデルから得ることができ、それと実際のロボットの状態とを比較することで精度の高い接触検出が可能となる。
また、ロボットモデルではロボットの目標関節変位量に対する応答の遅れも模擬するため、指令が急激に変化することによって誤検出をし得るという特許文献１の方法の問題点を解決できる。
（２）位置姿勢比較部において、リンク位置姿勢のロボットモデルに対する差の監視を複数のリンクに対して行うようにした点である。これにより、ロボット全体の監視ができ、手先以外の部分が接触した場合にも高感度な検出が可能である。

本発明が特許文献２と異なる点は、オブザーバによる外乱力推定を行うことなく実機の応答とモデルの応答を比較することで接触検出を行う点である。これにより、比較に要する計算は非常に単純なものであり、少ない計算量で接触検出をすることができる。

本発明が特許文献３と異なる点は、ロボットを模擬するロボットモデルを備え、ロボットの応答とロボットモデルの応答とを複数リンクの位置姿勢について比較するようにした部分である。これにより、位置指令の急激な変化を誤検出してしまうという特許文献３の問題点は解決される。

なお、第１実施例では、制御演算部１０３’と制御演算モデル部１０４’とが同じ構造を有する必要はなく、ロボットアーム１０１とアームモデル１０２とが厳密に一致している必要もない。関節角度指令１１１’に対するモデルの関節角度１１３’の応答特性が、関節角度指令１１１’に対する関節角度１１２’の応答特性を模擬できていればよい。

また、アラーム１０９’は、接触検出信号をうけるとアラームランプを点灯させ、操作者に接触検出を通知するものが使用されるが、ランプの他にブザー、操作パネル画面上の文字表示あるいは操作者が感知できるものであれば何でも良い。

ロボットと制御演算部を模擬するモデルを備え複数のリンクの位置姿勢についてロボットとロボットモデルとを比較することによって、目標関節変位量の変化に関係なく少ない計算量で接触検出ができ、また新たなセンサを必要としないので、多くの産業用ロボットやホームロボットなど、周辺環境との接触が起こり得るロボットに広く適用できる。

本発明の第１実施例を示すロボット制御装置のブロック図 図１に基づいて詳細に説明するための制御系のブロック図 本発明の第２実施例を示す状態比較部のブロック図（構成例１） 本発明の第３実施例を示す状態比較部のブロック図（構成例２） 第３従来技術を説明するための１方向における位置応答波形の例を示した図 本発明の実施例を説明するためのの１つのリンクの１方向におけるロボットおよびロボットモデルの位置応答波形の例を示した図 ロボットアームの手先と周辺機器との間で接触が起こった場合の、ロボットとアームモデルとの比較を示した模式図 多自由度のロボットアームにおける途中のリンクが周辺機器と接触した場合の、ロボットとアームモデルとの比較を示した模式図 第２従来技術となる特許文献１におけるロボットに搭載したモータの制御系の１軸分の回路構成を示したブロック図 第３従来技術となる特許文献２におけるロボット制御装置の基本的な構成を示した図

符号の説明

１０１ ロボット
１０１’ ロボットアーム
１０１’’ ３リンクロボットアーム
１０２’、１０２’’、８０２ アームモデル
１０２ ロボットモデル
１０３、１０３’ 制御演算部
１０４、１０４’ 制御演算モデル部
１０５、１０５’ リンク位置姿勢算出部
１０６、１０６’ モデルリンク位置姿勢算出部
１０７、１０７’ 位置姿勢比較部
１０８ 装置保護手段
１０８’ ブレーキ
１０９ 通知手段
１０９’ アラーム
１１０ 動作可視化手段
１１０’ モニタ
１１１ 目標関節変位量
１１１’ 関節角度指令
１１２ 関節変位量
１１２’ 関節角度
１１３ モデル関節変位量
１１３’ モデルの関節角度
１１４ アクチュエータへの制御入力値
１１４’ トルク指令値
１１５ ロボットモデルへの制御入力値
１１５’ ロボットモデルへのトルク入力値
１１６、１１６’ リンク位置姿勢
１１７、１１７’ モデルリンク位置姿勢
１１８、１１８’ 接触検出信号
２０１、２０７ 位置制御ゲイン回路
２０２、２０８ 微分器
２０３、２０９ 速度制御ゲイン回路
２０４、２１０ 出力制限器
２０５ 重力・摩擦力補償器
２０６ 出力変換定数回路
２１１ 第１関節の印加トルクに対する応答を模擬する１慣性系剛体モデル
２１２ 第ｎ関節の印加トルクに対する応答を模擬する１慣性系剛体モデル
２２１ トルク信号１１４の第１成分
２２２ トルク信号１１４の第ｎ成分
２２３ 実機部の関節角度１１２の第１成分
２２４ 実機部の関節角度１１３の第ｎ成分
２２５ アームモデルへのトルク信号１１５の第１成分
２２６ アームモデルへのトルク信号１１５の第ｎ成分
２２７ アームモデルの第１関節の角度
２２８ アームモデルの第ｎ関節の角度
３０１、４０１ リンク１の位置姿勢比較部
３０２、４０２ リンクｉの位置姿勢比較部
３０３ 絶対値計算手段
３０４ 比較演算手段
３１１ リンク１の位置姿勢
３１２ リンクｉの位置姿勢
３１３ リンクｋの位置姿勢
３１４ モデルリンク１の位置姿勢
３１５ モデルリンクｉの位置姿勢
３１６ モデルリンクｋの位置姿勢
３１７ リンクｉの位置姿勢偏差絶対値
３１８ リンクｉの閾値
４０３ 絶対値ベクトル計算手段
４０４ 成分比較演算手段
４１１ リンクｉの閾値ベクトル
４１２ リンクｉの位置姿勢偏差絶対値ベクトル
５０１ 位置指令
５０２ 位置応答
５０３ 位置指令と位置応答との差の最大値
６０１ モデル位置応答
７０１ 周辺機器８０５ 周辺機器
７１１ リンク１の位置偏差絶対値
７１２ リンク２の位置偏差絶対値
７１３ リンク３の位置偏差絶対値
８０１ 多自由度のロボットアーム
８０３ 位置姿勢を監視するリンク１
８０４ 位置姿勢を監視するリンク２

Claims (7)

1. 各関節のアクチュエータの出力を制御する手段と前記各関節の変位量を計測する手段を有するロボットを備えたロボットの制御装置において、
   前記アクチュエータへの制御入力値を計算し出力する制御演算部と、
   前記ロボットを模擬するロボットモデルと、
   前記制御演算部を模擬する制御演算モデル部と、
   前記ロボットの少なくとも１つ以上のリンク位置および姿勢を関節変位量から計算するリンク位置姿勢算出部と、
   前記ロボットモデルのモデル関節変位量から前記リンクに対応するモデルリンク位置および姿勢を計算するモデルリンク位置姿勢算出部と、
   前記リンク位置とモデルリンク位置とを比較して結果を出力する位置姿勢比較部と、
   を備えたことを特徴とするロボットの制御装置。
2. 前記位置姿勢比較部の比較結果に応じて該結果を操作者に知らせる通知手段、ロボットを停止させる停止手段、ロボットの動作指令を切り換える動作指令切替手段あるいはロボットの制御入力を切り換える制御入力切替手段のうち、少なくとも１つを備えたことを特徴とする請求項１に記載のロボットの制御装置。
3. 前記ロボットモデルの動作を画像として前記ロボットの動作に同期して操作者に提供するモデル動作可視化手段を有することを特徴とする請求項１または２に記載のロボットの制御装置。
4. 前記位置姿勢比較部は、前記リンク位置および姿勢を表すリンク位置姿勢ベクトルと前記モデルリンク位置および姿勢を表すモデルリンク位置姿勢ベクトルとの差の大きさを監視し、差の大きさが閾値を上回ったときに接触検出信号を出力することを特徴とする請求項１〜３のいずれか１項に記載のロボットの制御装置。
5. 前記位置姿勢比較部は、前記リンク位置姿勢ベクトルの各成分と前記モデルリンク位置姿勢ベクトルの各成分との差の大きさを個別に監視し、それぞれの差の大きさのうち少なくとも１つの大きさが閾値を上回ったときに接触検出信号を出力することを特徴とする請求項１〜３のいずれか１項に記載のロボットの制御装置。
6. 前記ロボットモデルは、１慣性系モデルをロボットの関節数と同数だけ並べて構成することを特徴とする請求項１〜５のいずれか１項に記載のロボットの制御装置。
7. 冗長自由度を持ったロボットの動作を監視する手段を有することを特徴とする、請求項１〜６のいずれか１項に記載のロボットの制御装置。

Images