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

Abstract

(57)【要約】 【目的】 専用ツールおよび力センサを不要にし、しか
も静摩擦力が大きい基本軸であってもそれを動かして、
コンプライアンス動作を達成すること。 【構成】 コンプライアンス動作中に、外力によって位
置変化した各軸角度θ１をリアルタイムで取り込み、指
定したツール座標系での位置／姿勢の現在値を演算し、
そのうち、指定したベクトル成分の現在値を指令値に短
時間に逐次置換してゆき、これによってワークをツール
で把持した状態で外力がそのワークに作用したとき、ワ
ークを予め定める一方向に、そのワークの姿勢を変える
ことなく、移動させることが可能になるとともに、コン
プライアンス動作中であっても、ロボットの各軸は位置
指令値によって駆動されるので、静摩擦力の大きい基本
軸であっても、動かすことが可能であり、僅かな外力に
応じたコンプライアンス動作を確実に達成することがで
きる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、産業用ロボットのツー
ルに作用する外力に対する予め定める一定の方向への逃
げ追従動作、すなわちコンプライアンス動作を行うため
の制御方法および装置に関する。

【０００２】

【従来の技術】従来から、位置制御を行う産業用ロボッ
トでは、その位置剛性が高く、高速度、高精度での位置
制御の実現を目的として構成されている。このような位
置制御を行うロボットでは、そのロボットのたとえば手
首に装着されたツールに外力が加わっても、その外力に
反抗して、予め定めた指令位置を維持する。このような
従来からのロボットは、たとえばダイキャスト成形品で
あるワークを、ロボットのツールで把持してダイキャス
トマシーンから取出すにあたり、油圧シリンダでワーク
がダイキャストマシーンから強制的に押出される際に、
そのワークをダイキャストマシーンから予め定める方向
に把持して取出すためには、そのまま使用することはで
きず、そのロボットには適切な柔らかさであるコンプラ
イアンス動作が必要となる。

【０００３】このようなコンプライアンス動作を実現す
るための１つの先行技術は、たとえば特開昭５８−４５
８９１に開示されている。この先行技術では、ロボット
の手首に、専用のコンプライアンスツールを装着し、そ
の弾性力を利用して２つの部品を、こじれを生じること
なく、円滑に嵌め合わせる動作を達成する。

【０００４】この先行技術では、コンプライアンス動作
を行うための各種の作業に併せて、ワークと接触する専
用のツールを設計して開発する必要があるという問題が
ある。またロボットの手首によってワークと専用ツール
とを持つことになるので、その専用ツールの重量分だ
け、ロボットの可搬重量が減少してしまう。

【０００５】他の先行技術は特開昭６３−１３９６７８
に開示されている。この先行技術では、ロボットのハン
ドに把持された部品の相手組立部品への接触力を力セン
サで検出し、その力センサからの出力によってコンプラ
イアンス動作を達成する。この先行技術では、力センサ
を必要とし、このような力センサは比較的構造が複雑で
あり、高価である。またこの力センサの機械的強度は、
ロボットの手首の機械的強度に比較してかなり低く、そ
の力センサの許容トルクを超えて力センサに外力が作用
すると、著しい性能の劣化が起こるという問題がある。

【０００６】さらに他の先行技術は、特開昭６０−１３
２２１３に開示されており、これはロボットの制御系の
パラメータを調整する構成を有し、すなわちサーボ制御
されるコンプライアンスを直接的にロボットの作業によ
って要求されるとおり、装置のみかけのスチフネスを変
えるようにプログラム制御する。

【０００７】この先行技術では、力センサを用いないの
で、前述の先行技術の問題は生じないけれども、ロボッ
トの制御系のパラメータを調整してコンプライアンスを
実現する手法であるので、外力を正確に検出してコンプ
ライアンス動作を行わせることはできず、したがって制
御系の中に力制御系を組み込むことは難しい。

【０００８】このような先行技術において、ロボットに
加わる外力によってロボットの各軸を動かす受動的コン
プライアンス制御動作では、ロボットの各軸を動かすに
必要な外力をロボットに加えなければならない。したが
ってロボットのたとえばベース付近の静摩擦力が大きい
軸を動かすには、そのロボットの関節の摩擦力に打ち勝
つだけの外力が必要であり、したがって所定のコンプラ
イアンス動作を実現することができないことがある。手
首に装着したツールに小さい外力が作用したときであっ
ても、静摩擦力の大きい軸をも併せて動かすことができ
るようにしたロボットの制御を実現することが望まれ
る。

【０００９】またこの先行技術では、力センサを用いる
ことなく、ロボットの制御系のパラメータを調節してコ
ンプライアンス動作を実現するので、ばねおよびダンパ
の動作をするために、コンプライアンス動作中の可動範
囲が狭く制限されるという問題がある。たとえばロボッ
トのアームを水平軸線まわりに角変位駆動するとき、重
力による悪影響の変化が小さい範囲内でのみ、アームを
動かす必要があり、したがってコンプライアンス動作中
の可動範囲が狭くなるという問題がある。

【００１０】さらにこの先行技術では、ツールの姿勢を
一定に保つことができないという欠点がある。

【００１１】さらにこの先行技術では、通常の位置制御
動作からコンプライアンス制御動作へ切換えるとき、各
軸の位置制御に用いられている積分器の内部状態に依存
してロボットが積分器の出力によって動くことがある。

【００１２】さらに他の欠点は、コンプライアンス制御
状態において、ツールの現在位置が、位置制御時の指令
位置からずれていたとき、そのロボットの位置制御動作
に切換えると、ツールが、前記指令位置に急激に戻り、
したがって危険であるという問題がある。

【００１３】さらに先行技術では、ツールの移動時にお
ける重力を補償する力を達成するための制御を実現する
ために、ロボットの制御系における積分器の働きによっ
て行っている。このようなロボットの制御系の積分器に
よる重力補償の手法は、そのロボットに設けられたツー
ルの予め定める姿勢でしか、有効ではなく、コンプライ
アンス動作中にロボットの各軸の姿勢が変わると、正確
な重力補償を保証することはできなくなってしまう。

【００１４】

【発明が解決しようとする課題】本発明の目的は、コン
プライアンス動作のための専用ツールを用いることな
く、また力センサを必要とすることなしに、しかも静摩
擦力の大きい軸を、小さい外力を受けたときにおいても
動かすことができるようにし、しかもツールの姿勢を一
定にして希望するコンプライアンス動作を達成すること
ができるようにしたロボットの制御方法および装置を提
供することである。

【００１５】本発明の他の目的は、コンプライアンス動
作中における可動範囲を広くすることができるようにし
たロボットの制御方法および装置を提供することであ
る。

【００１６】本発明のさらに他の目的は、コンプライア
ンス制御状態においてツールの現在位置が位置制御動作
中における予め定める指令位置からずれているとき、コ
ンプライアンス制御動作から位置制御動作に切換えたと
きにおいても、ツールが急激に前記指令位置に戻ること
を防ぐことができるようにして安全性を確保したロボッ
トの制御方法および装置を提供することである。

【００１７】本発明の他の目的は、ロボットの各軸の各
種の姿勢においても、正確な重力補償を行うことができ
るようにして、ツールの重力による悪影響を防ぐことが
できるようにしたロボットの制御方法および装置を提供
することである。

【００１８】

【課題を解決するための手段】本発明は、コンプライア
ンス動作開始直前のロボットの初期の各軸角度θ₀を検
出し、その検出した初期の各軸角度θ₀に基づいて、ロ
ボットの手首に装着されたツールの初期の姿勢Ｒ
（θ₀）を算出し、コンプライアンス動作中のロボット
の各軸角度θ₁を検出し、前記検出した各軸角度θ₁に基
づいて、ツールの予め定める移動方向にのみ現在位置を
位置指令値に置き換えて位置^tＰ_Σt.comを求め、前記初
期の姿勢Ｒ（θ₀）と前記位置^tＰ_Σt.comとに基づい
て、初期の姿勢Ｒ（θ₀）が一定のままとなるための位
置指令値Ｔ_comを求め、位置指令値Ｔ_comに基づいて、各
軸角度の指令値θ_comを算出して、各軸を駆動し、再び
コンプライアンス動作中のロボットの各軸角度θ₁の検
出、位置^tＰ_Σt.comの算出、位置指令値Ｔ_comの算出、
および各軸角度の指令値θ_comの算出を、繰返すことを
特徴とするロボットの制御方法である。また本発明は、
ツールが装着される軸のゲインを小さく設定して、外力
によってツールが装着される軸が変位しやすくすること
を特徴とする。 また本発明は、コンプライアンス動作中の前記検出した
軸角度θ₁が予め定める動作制限角度を超えるとき、ま
たはｘｙｚ軸座標系での動作範囲を越えるとき、その軸
の予め定める位置にもたらされるように位置制御動作を
することを特徴とする。 また本発明は、各軸を予め定める位置にもたらされるよ
うに位置制御動作をするために用いられる積分器を、コ
ンプライアンス動作中には、クリアしたままにすること
を特徴とする。 また本発明は、各軸に作用する重力を補償するためのト
ルクが加算されて発生されるように各軸毎の駆動源を重
力補償動作することを特徴とする。 また本発明は、ロボットの手首にツールが装着され、ロ
ボットの各軸を駆動する駆動源による各軸の位置を、軸
角度検出手段によって検出し、その検出軸角度と予め定
める軸角度の指令値との差を減算手段によって求めて、
その差が零になるように駆動源を負帰還制御するロボッ
トの制御装置において、コンプライアンス動作開始直前
の軸角度検出手段の検出軸角度θ₀に基づいて、ツール
の初期姿勢Ｒ（θ₀）を算出する第１演算手段と、コン
プライアンス動作中の検出軸角度θ₁に基づいて、ツー
ルの予め定める移動方向にのみ現在値を位置指令値に置
き換えた位置⁰Ｐ_Σt.comを算出する第２演算手段と、第
１および第２演算手段の出力に応答し、初期の姿勢Ｒ
（θ₀）と位置⁰Ｐ_Σt.comとに基づいて、初期の姿勢Ｒ
（θ₀）は一定のままとなるための位置指令値Ｔ_comを算
出する第３演算手段と、第３演算手段の出力に応答し、
位置指令値Ｔ_comに基づいて、軸角度の指令値θ_comを算
出して前記減算手段に与える第４演算手段とを含むこと
を特徴とするロボットの制御装置である。 また本発明は、減算手段の出力を、可変ゲインの係数器
を介して駆動源に与え、コンプライアンス動作中には、
係数器のゲインを小さく設定することを特徴とする。 また本発明は、軸角度検出手段の出力に応答し、コンプ
ライアンス動作中の検出軸角度が予め定める動作制限角
度を超えるとき、前記予め定める軸角度の指令値を減算
手段に与える手段を含むことを特徴とする。 また本発明は、減算手段と駆動源との間に積分器が介在
され、コンプライアンス動作中には、積分器をクリアし
たままにする手段を含むことを特徴とする。 また本発明は、積分器と駆動源との間に介在される加算
手段と、軸角度検出手段の出力に応答して、軸の重力補
償トルクを発生する指令値を発生して加算手段に与える
重力補償演算手段とを含むことを特徴とする。

【００１９】

【作用】本発明に従えば、コンプライアンス動作開始直
前またはその開始直後におけるロボットの複数の各軸の
初期の各軸角度θ₀とツールの初期の姿勢Ｒ（θ₀）とを
求めておき、その後のコンプライアンス動作中における
各軸角度θ₁を検出し、ツールの予め定める移動方向に
射影し、すなわちその移動方向にのみ現在位置を位置指
令値に置き換え、位置^tＰ_Σt.comを求めて前記初期の姿
勢Ｒ（θ₀）を一定のままに保ちつつ、前記予め定める
移動方向に沿ってツールを移動することができるよう
に、その位置指令値Ｔ_comによる各軸角度の指令値θ_com
を算出して各軸を動作させ、このような演算動作を、た
とえば１６ｍｓｅｃなどの短時間に繰り返して行うこと
によって、位置指令値を逐次変更することができる。す
なわちコンプライアンス動作中、外力によって各軸の位
置変化した各軸角度θ₁をリアルタイムで検出して取り
込み、ツール座標系などの予め指定した座標系での位置
／姿勢の現在値を演算し、そのうち、指定したベクトル
成分の現在値を指令値に逐次変換してゆく。これによっ
てコンプライアンス動作中であっても、ロボットの各軸
は、位置指令値によって駆動することができ、ツールの
姿勢を一定に保つことができ、したがって静摩擦力が大
きい軸、たとえばベース付近の基本軸などをも動かすこ
とができる。

【００２０】さらに位置指令値を上述のように逐次変更
することによって、コンプライアンス制御動作から通常
の位置制御動作に切換えるときにおける移行を滑らかに
行うことができる。

【００２１】さらに本発明に従えば、ツールが装着され
る軸、すなわち手首軸の駆動のためのゲインを小さく設
定することによって、外力によってツールが装着される
軸が変位しやすくなり、したがって僅かの外力に対して
も、コンプライアンス動作を達成することができ、この
コンプライアンス動作中には、前述のように静摩擦力の
大きい基本軸をも動かすことができるようになる。

【００２２】さらに本発明に従えば、コンプライアンス
動作中における検出した軸角度θ１が、予め定めた動作
制限角度および動作範囲を超えると、その軸に関して
は、位置制御動作時における予め定める指令位置にもた
らされるように位置制御動作を行うようにし、またその
コンプライアンス動作中における安全性を確保すること
ができる。すなわちコンプライアンス動作中の安全を確
保するために、各軸毎に、コンプライアンス動作中の動
作角度および動作範囲を指定し、コンプライアンス動作
中に指定した角度を超えた軸および動作範囲に関して
は、通常の位置制御動作に切換える。このような機能に
よって、コンプライアンス動作中のロボットの異常動作
を停止させる監視機能を持たせることもまた、可能とな
る。

【００２３】さらに本発明に従えば、位置制御動作をす
るために用いられる積分器は、コンプライアンス動作中
には、クリアしたままにされ、すなわちその積分値は零
にリセットされたままに保たれ、これによってコンプラ
イアンス動作中のバネおよびダンパの動作を解消するた
めに、ロボット制御系内の積分器を、上述のように全
て、零クリアし、これによって通常の位置制御動作から
コンプライアンス制御へ切換えたときのロボットの各軸
の急激な動作を防止することができる。

【００２４】またコンプライアンス制御動作から通常の
位置制御動作へ切換えるとき、コンプライアンス動作中
現在値を位置指令値に置き換えているので、ツールなど
の各軸が位置指令値へ向かって各軸が急激に戻る動作を
防ぐことができ、安全性を確保することができる。

【００２５】さらに本発明に従えば、積分器を上述のよ
うに全て零クリアするので、各軸の重力によるアームの
落下が発生するおそれがある。この問題を解決するため
に本発明に従えば、各軸に作用する重力トルクを計算す
る重力補償の式によって、重力補償動作を行わせる。こ
れによってコンプライアンス動作中に、ロボットの各軸
の姿勢が変化しても、その姿勢に対応した重力補償を正
確に行うことが可能になる。

【００２６】

【実施例】図１は、本発明の一実施例の産業用ロボット
１の簡略化した斜視図である。固定位置に据え付けられ
たベース２上では移動体３は鉛直な第１軸４まわりに角
変位駆動され、アーム５は水平な第２軸６まわりに角変
位駆動され、さらにアーム７は水平な第３軸８まわりに
角変位駆動され、さらにそのアーム７の先端の手首９の
フランジには、ツール１０が着脱可能に装着され、この
ツール１０は、ツール座標系ｘｙｚにおける図３の第４
軸１１、第５軸１２および第６軸１３まわりに角変位駆
動されることができる。本発明ではこのような６軸の産
業用ロボット１だけでなく、その他の複数軸を有するロ
ボットに関連してもまた実施することができる。

【００２７】図１に示されるロボット１は、図２に示さ
れるダイキャストマシン１４に関連して実施することが
できる。ダイキャストマシン１４の金型１５によって成
形されたワーク１６を、金型１５から強制的に押し出す
ために、油圧シリンダ１７が設けられ、この油圧シリン
ダ１７は、矢符１８で示される一方向に、ワーク１６に
外力を作用する。これによってワーク１６は、矢符１８
の方向に、押し出しピン１９によって力が作用される。
そのためワーク１６は金型１５の形状に応じて、予め定
める移動方向２０に移動する。本発明の産業用ロボット
１に従えば、そのツール１０はワーク１６を把持し、矢
符１８方向の外力が作用するとき、そのワーク１６を移
動方向２０に向けて移動して、そのツール１０が外力に
応じて逃げ動作をし、ワーク１６を掴んで、金型１５か
らワーク１６を取り出すことができる。こうしてコンプ
ライアンス動作を達成することができる。

【００２８】図４は、ロボット１を位置制御動作および
コンプライアンス動作させるための制御系の簡略化した
構成を示すブロック図である。ロボット１の各軸毎の制
御回路２２には、サーボ制御回路２３が接続され、さら
にホスト制御回路２４が接続される。各軸毎の制御回路
２２は、第１〜第６軸４，６，８，１１〜１３毎に設け
られ、それらは類似の構成を有しており、代表的に、１
つの制御回路２２についての説明を行う。各軸を角変位
駆動するサーボモータ２５には、ベースアンプと呼ぶこ
とができる増幅回路２６から、駆動電流が与えられ、こ
れによってサーボモータ２５が対応する各軸を駆動す
る。各軸の各角度は、エンコーダ２７によって検出され
る。

【００２９】図５はサーボ制御回路２３の具体的な構成
を示す電気回路図である。ホスト制御回路２４からライ
ン２８を介して各軸毎の位置指令値が減算手段２９の一
方の入力に与えられる。エンコーダ２７からの各軸の軸
角度を表す信号は、ライン３０から、減算手段２９の他
方の入力に与えられる。減算手段２９の出力は、係数器
３１に与えられ、ここで位置ゲインＫｐで増幅され、加
算手段３２に与えられる。エンコーダ２７の出力はま
た、微分回路３３で微分され、係数器３４で速度ゲイン
Ｋｖで増幅され、加算手段３２に与えられる。加算手段
３２の出力は、積分器３５に与えられ、積分演算が行わ
れる。積分器３５のゲインＧ１は、式１で示される。

【００３０】

【数１】

【００３１】ここでＫ１は定数であり、ｓは演算子であ
る。積分器３５の出力は位相補償器３６に与えられて位
相制御動作時に位相補償の演算が行われ、このゲインＧ
２は式２で示される。

【００３２】

【数２】

【００３３】ここでαは定数である。位相補償器３６の
出力は、もう１つの加算手段３７に与えられる。加算手
段３７には、エンコーダ２７の出力に応答する重力補償
演算回路３８からの出力が与えられて加算され、その加
算出力はライン３９から各軸のトルク指令値として制御
回路２２の増幅回路２６に入力される。

【００３４】図６は、制御回路２２の構成および動作を
設定するためのブロック図である。ライン３９を介する
サーボ制御回路２３からの各軸のトルク指令信号は、ラ
イン３９を経て増幅回路２６に与えられ、これによって
サーボモータ２５が駆動される。モータ２５の負荷であ
るアームには、参照符４１で示されるように重力が作用
し、したがってモータ２５によって駆動されるアームな
どの負荷には、重力４１が引き算され、その重力４１
が、動補償回路５１の出力により、キャンセルされて補
償され、アームなどが移動される。アームなどの慣性力
４３の角加速度は、積分手段４４によって積分されるこ
とによって、角速度が求められ、さらに積分手段４５に
よってその角速度を積分することによって、アームなど
の回転角が求められ、この回転角はエンコーダ２７によ
って前述のように検出される。

【００３５】図７は、本発明の一実施例の動作を簡略化
して示す図である。ロボット１に適用作業をさせると
き、ロボット１のツール１０に加わる外力Ｆに対して、
任意の希望する移動方向２０にそのツール１０が逃げ動
作をさせてコンプライアンス機能を達成するにあたり、
外力に対して受動的にそのコンプライアンス機能が達成
され、ツール１０は図７（１）に示されるようにコンプ
ライアンス動作開始直前の一定の姿勢を保持し、その移
動方向２０は、ツール１０の座標系で定める方向とし、
コンプライアンス度を調整可能とし、各軸の重力補償
は、計算式によって補償する構成とし、ワーク１６（前
述の図３参照）への反力は、粘性抵抗力とし、静摩擦力
が大きい基本軸である第１軸４などもまた動くように構
成し、安全対策として、各軸の動作制限角度を設定する
ように構成される。このコンプライアンス度の調整のた
めに、図５に示されるサーボ制御回路２３では、係数器
３１および３４における位置ゲインＫｐおよび速度ゲイ
ンＫｖを調整することによって達成される。外力による
ツール１０の各軸の変位を、エンコーダ２７によって検
出することによって、力センサなどを用いることが不要
になる。このエンコーダ２７は、通常の位置制御動作の
ためにもまた、用いられ、こうしてエンコーダ２７は、
位置制御動作とコンプライアンス制御動作とに兼用され
る。ツール１０のコンプライアンス制御動作中における
現在位置は、ツール座標系におけるｘ，ｙ，ｚの各成分
に分解し、位置指令値を作成してコンプライアンス制御
動作を達成する。こうしてツール１０によって把持して
いるワーク１６に外力Ｆが作用したとき、そのツール１
０の姿勢を保持したままで予め定める指定したツール座
標系の移動方向２０にのみツール１０を移動し、こうし
てダイキャストマシン１０からのワーク１６の取出し作
業などを行うことができ。コンプライアンス制御動作中
に、外力Ｆが矢符１８方向に作用したとき、図７（２）
に示されるようにそのツール１０が取付けられている手
首９のｘ，ｙ，ｚの各軸１１〜１３の位置および速度の
各ゲインは低く設定されており、したがってそれらの手
首１１〜１３に変位が生じる。このことがエンコーダ２
７によって検出され、そのツール１０の変位を、図７
（３）に示されるように移動方向２０に射影し、すなわ
ち移動方向にのみ現在位置を置き換えた位置４６、すな
わち^tＰ_Σt.comをホスト制御回路２４によって演算して
求める。図８では、参照符４７は、ツール座標系におけ
る原点位置への初期位置^tＰ_Σt（θ₀）を示し、外力Ｆ
によって図７（２）のように移動したツール座標系の原
点位置４８は、^tＰ_Σt（θ₁）で示される。移動方向２
０への射影し、すなわち移動方向にのみ現在位置を置き
換えた前記原点位置４９は、^tＰ_Σt.com（θ₀）で示さ
れている。

【００３６】図７（４）では、ツール１０が、前記射影
した位置となるように、しかもツールの姿勢が図７
（１）で示される初期の姿勢に保たれるように、各軸の
位置指令値を作成する。こうして図７（２）および図７
（３）の各軸の位置は仮想線５０で示されるとおりとな
り、これを補正した後には、図７（４）の実線５１で示
されるように各軸が駆動され。このような演算動作を繰
り返すことによって、図７（５）で示されるように、ツ
ール１０は移動方向２０に沿ってツール１０の姿勢を図
７（１）の初期の姿勢に保ったままで、移動してコンプ
ライアンス動作を達成することができる。

【００３７】図９は図３に示されるサーボ制御回路２３
およびホスト制御回路２４の動作を説明するためのフロ
ーチャートである。

【００３８】ここではツール座標系でｚ軸方向にコンプ
ライアンス動作をさせる場合について説明する。本発明
の原理を述べると、ツール座標軸コンプライアンスを開
始したときのツール座標系の原点位置を^tＰ_Σt（θ₀）
とすると、ロボットに外力が加わったとき、ツール座標
系の原点位置が^tＰ_Σt（θ₁）に移動したとする。

【００３９】 ^tＰ_Σt（θ₁）＝（ｄ^tＰ_Σt.x ｄ^tＰ_Σt.y ｄ^tＰ_Σt.z）^T …（３） このとき、位置指令値^tＰ_Σt.comを変更することにより
ｚ軸方向にのみ移動とする。ｘ，ｙ軸成分の位置指令値
は零とし、ｚ軸成分の位置指令値に外力により移動した
ｚ軸成分を与える。

【００４０】 ^tＰ_Σt.com ＝ （０ ０ ｄ^tＰ_Σt.z）^T …（４） 姿勢の指令値は、コンプライアンス開始時の姿勢の値を
与える。この位置と姿勢の指令値を逆変換することによ
り、各軸の角度指令値θ_com を求めることができる。こ
の角度指令値をサーボモータ２５のための制御回路２２
に指令することによりｚ軸方向へのみツール座標系の原
点が移動し、その姿勢を維持することができる。

【００４１】さらに具体的に図９を参照して、ツール座
標軸コンプライアンスの実現手順について説明する。ス
テップａ１からステップａ２に移り、各軸角度θ₀を得
る。

【００４２】 θ₀ ＝ （θ_JT1.0 θ_JT2.0 θ_JT3.0 θ_JT4.0 θ_JT5.0 θ_JT6.0）^T …（５） ここで、θ₀：コンプライアンス動作をする前の検出し
た角度である。

【００４３】ステップａ３，ａ４では、次の動作ａ，ｂ
によって座標変換行列を計算する。

【００４４】（ａ）ツール座標系を絶対座標系に変換す
る順座標変換行列

【００４５】

【数３】

【００４６】を計算する（ステップａ３）。

【００４７】ここで、

【００４８】

【数４】

【００４９】は姿勢回転行列、⁰Ｐ_Σtは原点位置行列と
する。

【００５０】

【数５】

【００５１】（ｂ）絶対座標系をツール座標系に変換す
る逆座標変換行列

【００５２】

【数６】

【００５３】を計算する（ステップａ４）。

【００５４】

【数７】

【００５５】ステップａ５では、本件ロボット１の各制
御回路２２，２３，２４への動補償を開始する。ステッ
プａ６では、これらの制御回路２２，２３，２４の係数
器３１，３４および増幅回路などのゲインを変更して設
定し、たとえばツール１０が装着される手首９の各軸の
位置ゲインＫｐおよび速度ゲインＫｖを、小さく設定
し、また積分器３５のゲインＫ_i を零とし、この積分器
３５の内容を零にクリアし、さらに位相補償器３６の機
能を停止し、換言すると、減算手段３２の出力をそのま
ま加算手段３７に与えたときの状態とする。

【００５６】ステップａ７では、外力が加わってロボッ
トの位置、姿勢が移動したとする。

【００５７】ステップａ８では、ツール１０の移動した
ときの各軸角度θ₁を得る。

【００５８】 θ₁ ＝ （θ_JT1.1 θ_JT2.1 θ_JT3.1 θ_JT4.1 θ_JT5.1 θ_JT6.1）^T …（８） そこで次に、ツール１０の移動したときの座標変換行列
を次の動作ｃ，ｄによって計算する。

【００５９】（ｃ）ツール座標系を絶対座標系に変換す
る順座標変換行列

【００６０】

【数８】

【００６１】を計算する。

【００６２】

【数９】

【００６３】（ｄ）絶対座標系をツール座標系に変換す
る逆座標変換行列

【００６４】

【数１０】

【００６５】を計算する。

【００６６】

【数１１】

【００６７】上述のようにステップａ８では、各軸角度
θ１を得る。動作制限角内かどうかは、ステップａ８で
行う。その後、次のステップａ９では、ツール座標系の
位置を求め、次のステップａ１０では、移動量をツール
座標に変換する。動作範囲内かどうかは、ステップａ１
０で行う。

【００６８】ツール座標系原点の移動量ｄ⁰Ｐ_Σtを計算
する。

【００６９】 ｄ⁰Ｐ_Σt ＝ （ｄ⁰Ｐ_Σt.x ｄ⁰Ｐ_Σt.y ｄ⁰Ｐ_Σt.z）^T ＝ ⁰Ｐ_Σt（θ₁） …（11） 次に移動量ｄ⁰Ｐ_Σtをツール座標系での移動量ｄ^tＰ_Σt
に変換する。

【００７０】

【数１２】

【００７１】ステップａ１１，ａ１３，ａ１５では、設
定した移動方向へのみ現在位置に置き換えた位置指令値
を作成する。ステップａ１１，ａ１３，ａ１５は、設定
した移動方向への位置指令値を作成するためのアルゴリ
ズムであって、移動方向へのみ現在値を指令値に置き換
える。

【００７２】(ａ）ｚ軸方向への移動 ^tＰ_Σt.com ＝（０ ０ ｄ^tＰ_Σt.x）^T …（13） （ｂ）ｘｙ平面内での移動 ^tＰ_Σt.com ＝（ｄ^tＰ_Σt.x ｄ^tＰ_Σt.y ０）^T …（14） （ｃ）姿勢一定での移動 ^tＰ_Σt.com ＝（ｄ^tＰ_Σt.x ｄ^tＰ_Σt.y ｄ^tＰ_Σt.z）^T …（15） ツール座標系での指令値^tＰ_Σt.comを絶対座標系での指
令値⁰Ｐ_Σt.comに変換する。

【００７３】

【数１３】

【００７４】ツール１０の姿勢一定のまま、指定のツー
ル座標系の原点位置を持つ座標変換行列

【００７５】

【数１４】

【００７６】を作成する。

【００７７】

【数１５】

【００７８】ステップａ１８では、座標変換行列

【００７９】

【数１６】

【００８０】を行列変換Γして各軸角度の指令値θ_com
を計算する。

【００８１】

【数１７】

【００８２】ステップａ１９では、各軸角度指令値θ
_comをサーボ側へ指令する。

【００８３】ステップａ２０では、コンプライアンス動
作の一連の演算が終了したかどうかが判断され、さらに
引続きコンプライアンス動作を行うときには、ステップ
ａ７に戻って上述の各動作を短時間で、たとえば１６ｍ
ｓｅｃの時間間隔毎に繰り返す。

【００８４】ステップａ２１では、コンプライアンス制
御を終了して通常の位置制御を行うにあたり、ステップ
ａ２１において現在位置およびツール１０の姿勢を、指
令値とし、ステップａ２２では、制御回路２２，２３，
２４の各ゲインを、その位置制御の予め定める値に戻
す。

【００８５】ステップａ２３では、重力補償動作を終了
し、ステップａ２４で、一連の動作を終了する。

【００８６】再び図５を参照して、加算手段３７に関連
して設けられている重力補償回路５１について説明す
る。この重力補償回路５１は、エンコーダ２７からの各
軸の回転角出力に応答し、重力補償トルクが発生される
ように、制御回路２２を動作させる。この各軸毎の重力
補償トルクは、次の式２０〜式２７に示される。

【００８７】

【数１８】

【００８８】ここで、

【００８９】

【数１９】

【００９０】である。

【００９１】

【発明の効果】以上のように本発明によれば、コンプラ
イアンス動作を達成するための前述の先行技術に関連し
て述べた専用ツールが不要となり、したがってその専用
ツールを準備する必要がないのは勿論、ロボットの可搬
重量の減少を防ぐことができ、また力センサを用いない
ので、製品価格が低くなり、また力センサの性能の劣化
による誤動作を防ぐことができる。

【００９２】特に本発明によれば、コンプライアンス動
作中に、外力によって位置変化した各軸角度をリアルタ
イムで取り込んで、その指定したベクトル成分の現在位
置を指令値に逐次置換していくので、コンプライアンス
動作中であっても、ロボットはその位置指令値によって
動くことができ、したがって静摩擦力の大きい基本軸で
あっても動かすことができ、そのため僅かな外力に対し
ても確実なコンプライアンス動作を達成することができ
る。さらにこのような位置指令値を逐次変更することに
よって、コンプライアンス制御動作から通常の位置制御
動作に切換えたときにおけるロボットの各軸の移行を滑
らかに行うことができるという効果が達成される。

【００９３】さらに上述のように位置指令値を操作する
ことによって、ロボットの各軸をコンプライアンス動作
中に、移動方向を変化して指令することもまた、ツール
姿勢を一定にすることから可能であり、用途を拡大する
ことができるるという効果もある。

【００９４】さらに本発明によれば、ロボットの各軸毎
にコンプライアンス動作中の動作制限角度および動作範
囲を指定し、コンプライアンス動作中に指定した動作制
限角度および動作範囲を超えた軸は、通常の位置制御動
作に切換え、これによってコンプライアンス動作中のロ
ボットの異常動作を停止させる監視機能を達成すること
ができるとともに、この動作制限角および動作範囲を設
定することによって、コンプライアンス動作中の安全性
を確保することができる。

【００９５】さらに本発明によれば、ツールが装着され
る軸のゲインを小さく設定し、たとえば位置および速度
ゲインを、調整して小さく設定することによって、各軸
のコンプライアンス動作を瞬時にかつ詳細に調整するこ
とができるようになる。

【００９６】さらに本発明によれば、コンプライアンス
動作中におけるバネおよびダンパの動作を解消するため
に、位置制御系内に含まれている積分器の積分内容を零
にクリアし、これによって通常の位置制御からコンプラ
イアンス制御動作へ切換えたときにおけるロボットの各
軸の急激な動作を防止することができる。

【００９７】また指令値を現在位置に置き換えることに
よってコンプライアンス制御動作から通常の位置制御動
作への切換え時にも、位置制御の指令値へ向かってロボ
ットの各軸が急激に戻る動作を防ぐことができる。これ
によって動作の安全性を確保することができる。

【００９８】さらに本発明によれば、ロボットの各軸に
作用する重力トルクを補償することによって、コンプラ
イアンス動作中にロボットの各軸の姿勢が変化しても、
その姿勢に対応した重力補償を正確に行うことができる
という効果も達成される。

【図面の簡単な説明】

【図１】本発明の一実施例の産業用ロボット１の簡略化
した斜視図である。

【図２】ツール１０付近の各軸１１，１２，１３を拡大
して示す図である。

【図３】本発明が関連して実施されるダイキャストマシ
ン１４の簡略化した断面図である。

【図４】本発明の一実施例の制御系の電気的構成を簡略
化して示すブロック図である。

【図５】サーボ制御回路２３の１軸の構成を具体的に示
す電気回路図である。

【図６】制御回路２２の機能を説明するためのブロック
図である。

【図７】本発明の一実施例の動作を簡略化して示す側面
図である。

【図８】その動作をさらに具体的に示す図である。

【図９】サーボ制御回路２３およびホスト制御回路２４
の動作を説明するためのフローチャートである。

【符号の説明】

１ 産業用ロボット １０ ツール １６ ワーク ２０ 移動方向 ２２ 制御回路 ２３ サーボ制御回路 ２４ ホスト制御回路 ２５ サーボモータ ２６ 増幅回路 ２７ エンコーダ ２９ 減算手段 ３２ 加算手段 ３３ 微分回路 ３５ 積分器 ３６ 位相補償器

フロントページの続き (72)発明者 佐野 正俊 兵庫県明石市川崎町１番１号 川崎重工業 株式会社明石工場内 (72)発明者 前原 毅 兵庫県明石市川崎町１番１号 川崎重工業 株式会社明石工場内 (72)発明者 服部 信也 兵庫県明石市川崎町１番１号 川崎重工業 株式会社明石工場内

Claims (10)

【特許請求の範囲】
1. 【請求項１】 コンプライアンス動作開始直前のロボッ
   トの初期の各軸角度θ₀を検出し、その検出した初期の
   各軸角度θ₀に基づいて、ロボットの手首に装着された
   ツールの初期の姿勢Ｒ（θ₀）を算出し、 コンプライアンス動作中のロボットの各軸角度θ₁を検
   出し、 前記検出した各軸角度θ₁に基づいて、ツールの予め定
   める移動方向にのみ現在位置を位置指令値に置き換えて
   位置^tＰ_Σt.comを求め、 前記初期の姿勢Ｒ（θ₀）と前記位置^tＰ_Σt.comとに基
   づいて、初期の姿勢Ｒ（θ₀）が一定のままとなるため
   の位置指令値Ｔ_comを求め、 位置指令値Ｔ_comに基づいて、各軸角度の指令値θ_comを
   算出して、各軸を駆動し、再びコンプライアンス動作中
   のロボットの各軸角度θ₁の検出、位置^tＰ_Σt.comの算
   出、位置指令値Ｔ_comの算出、および各軸角度の指令値
   θ_comの算出を、繰返すことを特徴とするロボットの制
   御方法。
2. 【請求項２】 ツールが装着される軸のゲインを小さく
   設定して、外力によってツールが装着される軸が変位し
   やすくすることを特徴とする請求項１記載のロボットの
   制御方法。
3. 【請求項３】 コンプライアンス動作中の前記検出した
   軸角度θ₁が予め定める動作制限角度を超えるとき、ま
   たはｘｙｚ軸座標系での動作範囲を越えるとき、その軸
   の予め定める位置にもたらされるように位置制御動作を
   することを特徴とする請求項１記載のロボットの制御方
   法。
4. 【請求項４】 各軸を予め定める位置にもたらされるよ
   うに位置制御動作をするために用いられる積分器を、 コンプライアンス動作中には、クリアしたままにするこ
   とを特徴とする請求項１記載のロボットの制御方法。
5. 【請求項５】 各軸に作用する重力を補償するためのト
   ルクが加算されて発生されるように各軸毎の駆動源を重
   力補償動作することを特徴とする請求項４記載のロボッ
   トの制御方法。
6. 【請求項６】 ロボットの手首にツールが装着され、ロ
   ボットの各軸を駆動する駆動源による各軸の位置を、軸
   角度検出手段によって検出し、その検出軸角度と予め定
   める軸角度の指令値との差を減算手段によって求めて、
   その差が零になるように駆動源を負帰還制御するロボッ
   トの制御装置において、 コンプライアンス動作開始直前の軸角度検出手段の検出
   軸角度θ₀に基づいて、ツールの初期姿勢Ｒ（θ₀）を算
   出する第１演算手段と、 コンプライアンス動作中の検出軸角度θ₁に基づいて、
   ツールの予め定める移動方向にのみ現在値を位置指令値
   に置き換えた位置⁰Ｐ_Σt.comを算出する第２演算手段
   と、 第１および第２演算手段の出力に応答し、初期の姿勢Ｒ
   （θ₀）と位置⁰Ｐ_Σt.comとに基づいて、初期の姿勢Ｒ
   （θ₀）は一定のままとなるための位置指令値Ｔ_comを算
   出する第３演算手段と、 第３演算手段の出力に応答し、位置指令値Ｔ_comに基づ
   いて、軸角度の指令値θ_comを算出して前記減算手段に
   与える第４演算手段とを含むことを特徴とするロボット
   の制御装置。
7. 【請求項７】 減算手段の出力を、可変ゲインの係数器
   を介して駆動源に与え、 コンプライアンス動作中には、係数器のゲインを小さく
   設定することを特徴とする請求項６記載のロボットの制
   御装置。
8. 【請求項８】 軸角度検出手段の出力に応答し、コンプ
   ライアンス動作中の検出軸角度が予め定める動作制限角
   度を超えるとき、前記予め定める軸角度の指令値を減算
   手段に与える手段を含むことを特徴とする請求項６記載
   のロボットの制御装置。
9. 【請求項９】 減算手段と駆動源との間に積分器が介在
   され、 コンプライアンス動作中には、積分器をクリアしたまま
   にする手段を含むことを特徴とする請求項１記載のロボ
   ットの制御装置。
10. 【請求項１０】 積分器と駆動源との間に介在される加
    算手段と、 軸角度検出手段の出力に応答して、軸の重力補償トルク
    を発生する指令値を発生して加算手段に与える重力補償
    演算手段とを含むことを特徴とする請求項９記載のロボ
    ットの制御装置。