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

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、組立作業などを行うロボットにおいて、これ
に及ぼされる外力を検知することによって危険回避動作
をおこなわせるようにしたロボットの制御方法およびそ
の制御装置に関する。

〔従来の技術〕

通常の産業用ロボットの制御系は、サーボモータを駆動
するサーボドライバと、このサーボドライバに指令を送
るコントローラとから構成されている。コントローラか
らは、パルス列またはアナログ電圧による、位置（回転
角度）、速度（回転速度）指令、及びデジタル出力によ
る、サーボオン、オフ信号をサーボドライバに送出し、
サーボドライバからは、やはりデジタル出力により、定
格電流オーバー信号、現在位置（回転角度）信号、及び
位置決め完了信号をコントローラに送出する。

〔発明が解決しようとする課題〕

ところで、産業用ロボットにおいては、実稼働時及び、
ティーチング時に周辺機器もしくは作業者と協同作業を
行うため、予期せぬ接触事故が起こることがある。上記
のようなロボットで、これを検知して運転停止等の危険
回避動作を行う手段は、コントローラにそなえつけであ
る緊急停止ボタンの押下、またはサーボドライバからの
定格電流オーバ信号に基づ（緊急動作以外にない。前者
では、危険回避動作が作業者に依存しており、後者では
作業者が挾まれるなどの比較的小さな外力、及びロボッ
ト動作中の外力を的確に検知することが出来ない。

このような不具合にたいして、公開特許公報昭６３−２
４５３８９号に見られる様に、出力軸へのトルクセンサ
の装備や、加速、定速、減速などの動作状態ごとに、ト
ルク比較基準しき（・値を変える等の工夫が行われて−
・るが、ロボットの力学的特性を考慮していない、トル
クセンナを新たに装備しなければならないなどの点で不
十分である。

本発明はこのような点に鑑み、ロボットが自動的に外力
を検知し、これにより危険回避動作を起こすことのでき
るロボット制御方法およびその制御装置を提供するもの
である。

〔課題を解決するための手段〕

上記課題を解決するために、本発明は、サーボドライバ
がその内部の電流ループによってモータに発生させてい
るトルク、及び現在位置（回転角度）、現在速度（回転
速度）を通信によって常時コントローラに通知し、一方
コントローラは、この清報と、コントローラ内部のメモ
リに格納された、ロボットの構造、アーム質量、慣性モ
ーメント、関節の粘性係数等の力学データを用いて、各
リンクの運動方程式に基づいた演算を行（・、ロボット
の運動状態にかかわらず、ロボットに外部から及ぼされ
る力とロボットそのものの慣性を駆動するに必要な力を
弁別し、前者を予め設定しである許容外力値と比較し、
これを越えた場合には、警告、サーボドライバへのサー
ボゲイン低減指令、ロボットの運転停止などを起こさせ
るように１−だものである。

〔実施例〕

以下、本発明による一実施例を図面を基に説明する。

第１図は、本発明を２リンク構造のロボットに応用した
場合の構造図であるが、本発明の適用が、特定のリンク
構造をもつロボットに限定されないことは言うまでもな
い。

ロボット２８には、２つの関節２８ａ、２８１）にそれ
ぞれ図示しないモータが取り付けられており、第１およ
び第２軸のサーボドライバ４，６が前記モータに電力を
供給することにより、ロボ。

トが運動を起こす。モータは、後で述べる理由により、
減速機なしのダイレクトドライバモータが望ましい。

第１、第２軸のサーボドライバ４．６は、ロボットコン
トローラにおける動作指令部１０から位置指令、速度指
令を受取り、位置速度制御部２２．２６において、１−
れら位置（回転角度）および速度（回転速度）指令と図
示しないエンコーダから送られる現在の位置（回転角度
）、速度（回転速度）とを比較し、演算を行って、モー
タコイルに流すべき電流を決定する。電流制御部２０．
２４は、位置速度制御部２２．２６から送られて来る電
流指令信号と、現在モータに流れている電流値が等しく
なるように、モータにかける電圧を制御する。一般に、
モータコイルに流れる電流と、モータに発生するトルク
には一意的な関係が存在するので、この電流値と発生ト
ルクを同一視してよい。ロボットコントローラ２では、
予め作成された動作プログラム８に従って、動作指令部
１０が、ある一定の時間間隔（例えば５ミリ秒）で、サ
ーボドライバに位置、速度指令を送出する。

さて本発明により、上記のようなロボットに、外力を検
知して安全に停止させる機能を付加する手順を第３図に
沿って説明する。

まず、外乱を検知してロボットに危険回避行動を起こさ
せるプログラムは、ある一定の時間間隔、例えば、１０
ミリ秒毎に起動される。（第３図の３０）　　この時間
間隔は、外乱検知に要求される応答性、及びＣＰＵの演
算速度を勘案して設定する。

第１および第２軸のサーボドライバの位置速度制御部２
２．２６は、ロボットコントローラの演算部１２に対し
て、第１、第２軸の現在の位置、速度信号ｐｏｓ−１、
ｖ　ｅ　ｌ−１、ｐｏｓ−２、ｖｅｌ−２を送出する。

さらに電流制御部２０．２４の管理しているモータトル
クをロボットコントローラ２の演算部に送出する。ただ
し、実際には処理時間等を考慮して、前記モータトルク
と同一視できる電流指令信号をモータトルクＴＲＱ−１
、ＴＲＱ−２として送出している。（第３図の３２）ロ
ボットコントローラ２の演算部１２は、これらの信号と
、ＲＯＭ２１８に格納されているロボット力学データ（
表１）とを用いて演算を行い、ロボット２８に外部から
作用しているトルクを算出する。（第３図の３４） ここで、有効長Ｌ１、Ｌ２とは、第２図に示づように、
第１、第２リンク、２５°ζｊ２１７１におゆる関節間
の距離であり、重心位置Ｇ１、Ｇ２とは、関節から重心
までの距離である。

この外部トルクはロボットコントローラの判定部１４に
送られる。この判定部１４は、ＲＯＭ　１１６に格納さ
れたしきい値データ（表２）と外力を比較し、もし外力
が最大許容外乱を越えていれば、重大な事故の可能性が
あるとみなして、口〆ットを速やかに停止させる。（第
３図の３６．３８）また、ロボットが高速運動状態にあ
るとき、外力が高速時許容外乱を越えた場合、警告信号
、あるいは、サーボドライバへのサーボゲイン低下指令
を発する。（第３図の４０．４４．４６）また、ロボッ
トが低速運動状態にあるとき、外力が低速時許容外乱を
越えた場合も同様に、警告信号、あるいは、サーボドラ
イバへのサーボゲイン低下指令を発する。（第５図の４
２．４６） 低速時と高速時に別々のしぎい値が設定されているのは
、高速時と低速時で外力算出の精度が異なるためと、高
速時と低速時ではロボットの使用状態が太き（異なるた
め、警告処置の内容も変える必要があるからである。

また、外部トルクの検出精度は、ロボットの運動方程式
の精度に大きく依存するため、運動力保式に繰り込むこ
との難しい要素（例えば関節での非線形摩擦）が少ない
機構、例えばＤＤモータを使用することが望ましい。

次に外力を算出する演算アルゴリズムを示す。

［Ｆ］関節の公称トルク ロボットの各関節の公称トルクは、例えば、Ｌｕｅ、　
Ｗａｌｋｅｒ、　Ｐａｕｌのアルゴリズムを用いて求め
られる。このアルゴリズムは、まずロボットの基底部か
ら、手先部へ向かう順方向計算によって、各リンクの角
速度、角加速度、線速度、線加速度を求め、次に手先部
から基底部への逆方向演算によって、各関節部で隣接す
るリンクが及ぼしあっているモーメントと力を求めるも
のである。ロボットの基底部をリンク０とし、基底部か
ら順番にリンクに番号をつけるとリンクｉ　−１とリン
クｉを接続しているモータのトルクは、次のようにして
求められる。

ここでｌ　−ＩＡ、を座標系ｉで表現されたベクトルを
座標系！−１での表現へ変換する回転変換行列、Ｓｉを
ｌ座標系によるｉ　−１座標原点からｉ座標原点へのベ
クトルとする。このとき、 １−ｍＡ、、ｓｌは次式で表される。

ｌ−１ＡＩ−Ｕ（Ｃ１）■（ｕｉ）　　　　・・・（１
）Ｓ　　ｉ”（ａ　ｌ　　　　　ｒｉｓ１ｎα　Ｉ　　
　　Ｉ”　　Ｉ　Ｃ０５（Ｅｌ）’　　　　　　　　　
−−−（２）ここに、ａｌはＺ　ｌ−１軸とｚｌ軸との
距離、ａｌはＸ１軸回りの２１軸の、Ｚｌ−１軸に対す
る角叫ｒｌはｚｌ−１軸に沿ったｘ、−１軸とＸｉ軸と
の距離、Ｃ１はＺｉ−１軸回りのｘｉ−１軸に対する角
度。

また、 とする。

上述の記号を用いて、関節トルクτ１は以下の手順で求
まる。

順方向計算（ｉ二１，２・・・・・・、ｎ）ωｔ＝’／
Ｊ−＋　（Ｊ−ｔ＋Ｚ＋−ビθｉ　）　　　　　　　　
・（５）”ａｒ＋＝’Ａ＋−＋じωｌ−１＋ＺＩ−１ｍ
”Ｃ１−１−ωＩ−ＩＸ（ｚｔ−１”０Ｉ））　　　　
　　　　　　　　　　・”（６）”Ｖｔ＝”ω１ＸＳｔ
＋ωｌＸ（ω＋Ｘｓ＋　）＋’Ａ＋−＋　”Ｖ＋−＋・
・（力 ”ｕ＋＝ω＋ｘ（ω＋ｘｐ＋）＋”ｇ＋Ｘｐ＋＋”ｖ＋
　　　　　　°・−（８）逆方向計算（ｌ＝ｎ、ｎ−１
，・・・・・・、１）ｆ　＋＝ｒｒ＋＋　”ｕ＋＋’Ａ
＋　＋＋　ｆ　＋＋＋　　　　　　　　・・・（９１ｎ
＋＝Ｊ＋”ｏ＋＋ω１ｘ（Ｊ＋ω＋）＋ｍｔ（Ｓ＋＋ｐ
’ｔ）ｘ”ｕｌ＋ｓ＋ｘ’Ａ＋−ｚ　ｆ　ｉ＋＋＋’Ａ
＋　＋１ｎ＋−ｚ　　　　　　−・・ＱＱ）Ｔｌ”（’
ＡＩ−＋ｚｉ−ｔ）”ｎｌ＋ｂ１”Ｃ１…（ｔｌ）ベク
トルの表記法については、Ｊ座標に関する任意のベクト
ルｑｊをｉ座標で表したものにたいして、　ｑｌと表記
し、もしｉ二」であれば、ｑコと略記する。

ここにｓ　”１％”（ｊＪｌ、＊　ｖ　、°、”ｕｉ、
ｆｌＳｎｉ　　は三次元の縦ベクトルであり、×はベク
トル積であり、右上の＄　　６１はそれぞれ時間に関す
る、階微分、二階微分を表す。右肩のＴは転位を表わす
。さらに、 ωｌはＩ座標系原点の角速度、 “ωｌはｌ座標系原点の角加速度、 Ｉｖｌはｌ座標系原点の線加速度、 ｍｕｌはリンクｉの重心の線加速度 ｆ＋Ｇまリンクｉ−１からリンクｉへ作用するカｎ＋は
リンクｉ−１からリンクｉへ作用するモーメント ｐｌはｌ座標系原点からリンクｉの重心へ至るベクトル Ｊｌはリンクｉの重心回りのリンク！の慣性行列ｍ１は
リンク１の質量 す、はｉ関節での粘性抵抗 ｚｌは１座標系の２方向単位ベクトル とし、これらのうちのベクトルはすべてｌ座標系での表
現である。

第１図の２リンクロボツトの場合、ＲＯＭ　２１８の力
学データ（表１）を用いて、次のようにパラメタを定め
ればよい。

ｐ＋＝（ＬＬ−Ｇｌ、０．０）Ｔ ｐ２＝（Ｔ２−Ｇ２．０．０）？ ｍ＋＝４ｖｆｌ　、ｒｒ＋ｚ＝Ｍ２　、　ｂ　ｔ＝Ｂ１
　、　ｂ　２＝Ｂ２　、α＋＝０　。

α２＝０ ａ　＋＝Ｌ１　、　ａ　２：Ｔ２　、θｔ＝ＰＯ８１，
θ２＝ＰＯ８−２”ｌ）＋＝ＶＥＬ−１、”ａ　ｚ＝Ｖ
ＥＬ−２，””ｆｌｓ＝”ＶＥＬ−１゜＄００２＝”Ｖ
ＥＬ−２ 各関節角度θ１と実トルクｐＨ（ｉ＝１・・・ｎ）、は
づ−ボドライバから、ある一定の時間間隔ごとに（例え
ハ５　ｍ５ｅｃ　）通信によってロボットコントローラ
に送られる。第１図の場合、ρ１＝ＴＲＱ−１゜ρ２二
ＴＲＱ、−２である。次に、上記のアルゴリズムによっ
て、各時刻の公称トルクτｌが計算される。

単純には、Ｔ１とＧ１の差分δτを外乱トルクとみなす
ことができるが、ＪＩ、ｍｌ、ｂｌ等のパラメタを正確
に同定するのが困難であることや、Ｇ１、′θ１．１＊
０１の測定値が雑音を含んでいることなどから、このま
までは不十分である。そこで、各リンクごとに０１に関
する微分方程式をもとめ、システム方程式の誤差および
、観測ノイズを考慮して、外乱トルクτｄを推定するの
がよい。具体的には、拡張カルマンフィルターのアル−
ｒ　＋）ズムな用いればよい。

■外乱推定の理論 式（１）から（９）を用いてＧｌに関する２階の非線形
微分方程式を導き、さらに外乱トルクτｄ１　を追加す
ると、 Ω１傘０θＨ＝−Ｚｉ−ｔ’［：’−’Ｊｌじω＋−＋
＋ω＋−５ＸＺ＋−ｓ　”θ＋　）＋ωｔ−＋Ｘ’−’
Ｊ＋（ω＋−１＋ｚｔ−ｔＩθ１）＋＋Ｈ’−’　（Ｓ
＋＋ｐ＋　）’（ω＋−１＋Ｚ＋−＋　”θ＋　）　（
１−１（ｓ、＋ｐｉ　）ＸωＩ−１）−４−ｍ＋　’−
’　（Ｓ＋＋ｐ＋　）Ｘ（じＧ１−１＋ωＩ−ＩＸＺ＋
−＋”θｉ）×’−１（Ｓ＋＋ｐ＋）ｌ＋ｍ１’−’（
Ｓ＋＋ｐ＋）Ｘ”ｖｌ−＋＋’−１Ａ＋（Ｓ＋ｘ’Ａ＋
＋ｔｆ＋＋＋）＋’−’Ａ、（’Ａ＋＋＋ｎ＋＋＋））
−１〕１傘θ１＋７．＋τ、１　　　　　　　　　　　
　　　　　　　　・・・αりここに、 Ωｒ＝Ｚ＋−ｓ？’−ＸＪ＋Ｚｔ−ｔ＋ｍ＋　（（Ｓ＋
＋ｐ＋　）”　（Ｓ＋＋？１−１ ｐ＋）−（Ｓｔ＋ｐ＋）　　Ａｔ−ｔＺｔ−＋ｚＩ−＋
　　　ＡＩ（ｓ＋＋ｐ＋）ｌ　　　　　　　　　　　　
　　　　　　　・・・ａ′５！１−１ であり、Ｚｉ−ＩＪ＋Ｚ＋−ｔ ＝Ｚ＋−＋ＴＬＪ（θ＋）Ｖ（ａｔ）ＪｔＶ（−ａｔ）
Ｕ（−ａｔ）ｚ＋−５＝Ｚｔ−＋ＴＶ（（Ｚｉ）ＪｔＶ
（−ｃｘｌ　）Ｚｔ−を等に注意すると、Ωはａｔに依
存しない定数であることが分かる。

また、１−ＩＪ、はｉ　−１座標系の原点まわりの、リ
ンクｌの慣性行列であり、 ”””Ｊｔ＝’−ＩＡ＋’ｌ’Ａ＋−ｔである。　　　
　　３１．α、ａ（ＩＺ式をΩＩで割り、つぎの、よう
に略記する。

”０１：／／ｌ（０，、′θ１、ＱＪ　ｉ　−１、”　
（ｊＪ　ｌ　−１、”Ｖｌ−１、ｆｌ−ｚ、ｎｌ＋Ｉ、
Ｔ１、τｄ＋　）　　　　　　　　　　・＝　（Ｔ５）
この方程式を　Ｇ１、“Ｇ１、Ｔ１、τｄ１の公称値の
回りに展開して、−次の項までとると、次のようになる
。

ａθ１　　　θ０θｌ ａτ１ ａτｄｌ ・・・Ｔ１６） ここに、各変数の公称値を、その右肩に　をつけてあら
れすものとして、 Δ傘申θ、：◆１θ１−００１″ Δθ１　　＝　　θｉ−０１″′ Δ９θ　　＝＝　　”０ｉ−＊θＣ Δτｔ　　　＝　　Ｔ１−　　　τｌ Δτｄにτｄ１−τｄＩＡ とし、また各偏微分は、下のようになり、それぞれ公称
値での値をとるものとする ΩＩａＷ　ンＺｌ−＋Ｔ（：’−’Ｊｔ（ω＋−＋ｘｚ
＋−ｔ）ａ４θｌ −＋−ω＋　−ｒ　Ｘ　’　−’　Ｊ　ｉ　Ｚ　Ｌ　−
を十ｍ＋’−１（ｓ＋−１−ｐ＋）”（Ｚｌ−＋）（’
−’（ＳＩ＋ｐ＋）Ｘω＋−＋）Ｚｌ−１”θ１　）＋
ωｌ−４ｘｃ？’−’Ｊｔ（＋ｕｇ−＋＋Ｚ＋−ビθ１
）ａθ１ ＋ｍ１　’−’　（ｓ＋＋ｐ＋　）ｘ（（ωｔ−ｔｘｚ
＋−１）ｘ’−’（ｓ＋−＋−ｐ＋））＋ｍ＋ａ’−’
（Ｓｔ　　＋）”（ωＩ−ｘ＋Ｚ＋−げθｔ）（’−１
（Ｓｉ−＋−ｐｔ）ＱＪｔ ×ω１−母＋ｍ１　′−’　（５１＋ｌ）ｌ　）”（ω
ｉ−１＋Ｚｉ−ビθｌ）ΩＩａＷ＝１ δτ１ ・・・ａ９ （”−’　（５１−１−４）ｔ　）Ｘω１−１　］＋ｍ
ｌδ’−’（Ｓｉ−１−１）ｉ）刈じＱＪｔ−１θθＩ
　　　　　　　　　　　　　ａθ１Ω１δ？７１′−１ ａτｄｉ ・・（２■ −よ一］×ｚＩ−ビθ＋）Ｘ’−’（ｓ＋＋ｐ＋））＋
ｍ＋’−’（ｓ＋＋ｐ＋）ａθ１ ａθ１ ＱＪｔ ａθｌ ・・・ＣＤ δＵｌ これを次のよ うに略記する δθ ａθ１ θθｉ ・・（２６） ・・・（２３） ここに、 は外乱 トルクの公称値であるから、 とする。

したがって、 Δ　τ　ｄ にτ ｌである。

（１０を状態方程式の形式にすると （１３）式を離散化すると Φ＋＝ｅｘｐ（ｆ−１日） ・・・（３０） とする。

τｄ＋（ｋ−１−＋）＝αｌτｄｌ（ｋ）＋Ｗ１３（ｋ
）　　　　　　・・・Ｇ３）ここにαｌはある正数であ
る。

さらに（１３式にシステムノイズ（Ｗｔ　１．　Ｗｔ　
２　）が加法的に加わるものとし、Ｅを平均操作とする
と診、次の様な統計的性質をもっているとする。

ｒＩ＝ｆｏΦ＋（ｓ）ｄｓ ・・・ＯＤ を用いて、 ・・・（３渇 となる。ここに、ｔはサンプリング時間である。

外乱トルクτａ＋（ｋ）は、離散的白色雑音Ｗｉ（ｋ）
によって、次の式に従って生成されるものニＱＩδｋｊ
　　　　　　　　　　　　　・・・１３４）ここに、Ｑ
＋はある対称正定値行列であり、δｋｊはいわゆるクロ
ネッカーのデルタである。

δｋｊ＝Ｑ（ｋ〜ｊのとき）、１（ｋ＝ｊのとき）する
とシステム方程式はつぎのようになる。

さらにＧ１、′θ１の測定値にＷｔ（ｋ）（ｋ＝Ｏ９・
・・）と無相関な観測ノイズｈ＋（ｋ）＝（ｂ＋　１（
ｋ）　、　ｈｔ　ｚ（ｋ乃“が加わるものとすると、観
測方程式は、 右肩の添え字〜は観測値を表わす。

簡単のため、０勺、（３６）式を次のように書き直す。

Ｘ＋（ｋ＋＋）＝Φ＋（ｋ）Ｘ＋（ｋ）＋Ｆ＋（ｋ）＋
Ｗｔ（−ｋ）Ｙ＋　（ｋ）＝ＮＸ＋　（ｋ）４−ｈｔ　
（ｋ）・・０８） ・・Ｃ３９） ここに、 （ｋ） ・・・（支）） ここにｈ　＋　（ｋ）は、Ｅｈ　＋　（ｋ）　ｈ　ｔ　
（ｊ）“＝Ｒｔδｋｊを満たすものとする。　　　　　
　　　　　　・・・（３７）（Ｒ＋はある対称止定値２
×２行列） ｐ＋（ｋ）＝（１−Ｋｉ（ｋ）Ｎ）可（ｋ）（１−に蔦
（ｋ）Ｎ）”＋Ｋｔ（ｋ）Ｐ＋Ｋ・（ｋ）“　　　　　
　　　　　　　　・・・（４９）従って外乱トルクの最
良推定値τｄｌはとした。

このような条件下でＸｌ（ｋ）の最良推定値Ｘ＋（ｋ）
　　は、次の漸化式を用いて得られる。

Ｘｌ　（ｋ）　＝Φｌ　（ｋ）　Ｘｔ　（ｋ−１）＋Ｆ
　ｔ　（ｋ）・・（４５） Ｘｌ　（ｋ）＝Ｘｔ　（ｋ）＋Ｋｔ（ｋ）（Ｙｔ　（ｋ
）−ＮＸ＋（ｋ））・・・顛ここでに＋（ｋ）はいわゆ
るカルマンゲインであり、次の漸化式で得られる〇 Ｐ＋（ｋ）＝Φ＋　（ｋ）　Ｐ　ｔ　（ｋ−１）Φ１（
ｋ）“＋ＱＩＫ＋（ｋ）・−Ｐ＋（ｋ）Ｎ’　（ＮＰ　
＋（ｋ）虻＋Ｒ＋ｌ−’・・・（４７） ・・・（４８） となる。

■外乱トルク推定の手順 ｍ、　（２）での議論を元にすると、外乱トルク推定の
アルゴリズムは以下のようになる。

ステップ］：初期設定 Ｑｌ、Ｒｔ、Ｐｔ（ｏ）、Ｘ　＋　（ｏ）　　を適箇に
定める。（を二１・・・ｎ） ステップ２：各時刻での計算 時刻にでの各関節の回転角θ１（ｋ）（ｉ二〇・・・）
がサーボドライバから得られ、式（５）、（６）、（力
、（８）、（９）、（１０）、Ｑｌ）をもちいて、公称
トルクτ１　を求める。

さらにサーボドライバから得られる実トルクτ饅（第１
図の場合、τ＋＝ＴＲＱ　　１．τ２＝ＴＲＱ　　２で
ある）を用いて、摂動トルクΔτ１を、Δτ１＝τ１−
τ１　とする。

次にｑη、α印、α９、（２０、（５）、弼、磯を用い
て、ＨｌＭ、Ｌをもとめる。

次に、■、００式を用いて、Φｔ、／’ｉをもとめる。

次に、（４Ｉ）、（４り式を用いて、ΦＩ（ｋ）、Ｆｔ
（ｋ）を求める。

次に（４７）、＋４８．（４９）式とＰ＋（ｋ−＋）を
用いて、Ｐｔ（ｋ）。

ＫＩ（ｋ）、ＰＩ（ｋ）ヲもトメ、（４５）、（４６）
、（５０１式とＸｌ（ｋ−ｔ）を用い・て、τｄＩ（ｋ
）とＸＩ（ｋ）をもとめる。

〔発明の効果〕

本発明により、任意の多関節ロボットは、特別のセンサ
を付加することな（、またその運動状態に拘らず、ロボ
ット外部から及ぼされる力による、外乱トルクを検知す
ることが可能になる。また、外乱トルクの生じたリンク
を特定できるため、障害の発生した部位を通告すること
が可能になる。

さらにこの外乱トルクの値とロボットの動作状態に応じ
て、警告信号の発生、非常停止などの処置をすることが
可能になる。これらのため、従来にない安全性がロボッ
トに付与されることになる。

【図面の簡単な説明】

第１図は本発明を２リンクロボツトに適用した場合の簡
単なブロック図、第２図は本発明を２す図である。 ２・・・・・・ロボットコントローラ、４・・・・・・
第１軸のサーボドライバ６・・・・・・第２軸のサーボ
ドライバ１０・・・・・・動作指令部、 １２・・・・・・演算部、 １４・・・・・・判定部、 ６・・・・・・ＲＯＭＩ（第１の記憶部）、８・・・・
・・ＲＯＭ２　（第２の記憶部）、０．２４・・・・・
・電流制御部、 ２．２６・・・・・・位置速度制御部、８・・・・・・
ロボット。

Claims (2)

【特許請求の範囲】
1. （１）軸を介して回動可能に連結された複数のリンクを
   有するロボットの制御方法であって、 前記それぞれのリンクの運動方程式に基づく演算の情報
   と、そのリンクに対応する軸の回転角度、回転速度およ
   びトルクの情報とによって、外力を推定し、この外力を
   あらかじめ設定したしきい値と比較し、この比較した結
   果により、危険状態の存在を認知した際には、危険回避
   動作を行なうことを特徴とするロボットの制御方法。
2. （２）軸を介して回動可能に連結された複数のリンクと
   、前記軸を回転させるモータとを有するロボットの制御
   装置であって、ロボットコントローラとサーボドライバ
   とを有し、 前記ロボットコントローラは、動作指令部と、第１の記
   憶部と、第２の記憶部と、演算部と、判定部とを有し、 前記動作指令部は、前記軸の回転角度指令および回転速
   度指令を送出し、 前記第１の記憶部は、しきい値データが記憶されるもの
   であり、 前記第２の記憶部は、前記リンクに関する力学データが
   記憶されるものであり、 前記演算部は、前記力学データと、前記軸の回転角度信
   号、回転速度信号およびモータトルク信号とから外力を
   計算し、 前記判定部は、前記しきい値データと外力とを比較し、
   許容値を越えているときは危険回避動作をさせ、 前記サーボドライバは、位置速度制御部と、電流制御部
   とを有し、 前記位置速度制御部は、前記軸の回転角度信号および回
   転速度信号を前記演算部に送出し、かつ回転角度指令お
   よび回転速度指令と、軸の回転角度および回転速度とを
   比較し、モータに流すべき電流を決定して電流指令信号
   として送出し、 前記電流制御部は、前記位置速度制御部から送出される
   電流指令信号と前記モータに流れている電流値とが等し
   くなるようにモータにかける電圧を制御することを特徴
   とするロボットの制御装置。