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

Abstract

(57)【要約】 【課題】 ロボットをコンプライアンス制御しながら、
外力とは別の能動的な動作を可能とする。 【解決手段】 産業用ロボット１のハンド２は、ワーク
４の穴２１に部品３を嵌め合わせる。部品３を穴２１に
挿入したとき、わずかの偏心があっても、コンプライア
ンス動作によって、挿入方向に垂直な方向にずれる変位
が可能である。部品３の先端と穴２１の斜面との接触部
から、挿入位置を矯正する反力が発生するので、部品３
は変位して嵌め合いを容易に行うことができる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、産業用ロボット、
特に、外力の影響を受けてコンプライアンス動作を行わ
せるためのロボットの制御方法および装置に関する。

【０００２】

【従来の技術】従来から、産業用ロボットでは、高速度
かつ高精度での位置制御の実現を目的として、位置剛性
を高めるような制御が行われている。このような位置剛
性が高い状態のロボットでは、外力が加わるときに、そ
の外力に反抗して予め定めた指令位置を保持しようとす
る。たとえば、部品間のはめ合いを伴う組み立て作業を
位置剛性が高い産業用ロボットを使って行おうとする場
合、一方の部品の位置を少し変えれば容易に組み立てが
可能なときでも、そのような柔軟性のある動作を行うこ
とができない。

【０００３】ロボットに外力が加わる場合に、位置剛性
を緩和し、予め定める一定の方向への逃げ追従動作を可
能とするような制御方法は、コンプライアンス動作とし
て実現されている。コンプライアンス動作についての先
行技術の１つは、たとえば特開昭５８−４５８９１に開
示されている。この先行技術では、ロボットの手首に、
専用のコンプライアンスツールを装着し、その弾力性を
利用して２つの部品を、よじれを生じることなく円滑に
嵌め合わせる動作を達成することができる。

【０００４】他の先行技術は、たとえば特開昭６３−１
３９６７８に開示されている。この先行技術では、ロボ
ットのハンドに把持された一方の部品による他方の組立
部品への接触力を力センサで検出し、その力センサから
の出力によってコンプライアンス動作を達成する。さら
に他の先行技術は、たとえば特開昭６０−１３２２１３
に開示されている。この先行技術では、ロボットの制御
系のパラメータを調整することによって、装置の見掛け
のスチフネスを変えるようにプログラム制御する。

【０００５】

【発明が解決しようとする課題】特開昭５８−４５８９
１の先行技術のように専用のコンプライアンスツールを
装着したり、特開昭６３−１３９６７８の先行技術のよ
うに力センサによる接触力の検出に基づいてコンプライ
アンス動作を達成する構成では、ロボット本来の構成に
新たな構成を付加する必要があり、ロボットとしての汎
用性を損なう。

【０００６】特開昭６０−１３２２１３の先行技術のよ
うに、ロボットの制御系のパラメータを調整してコンプ
ライアンス動作を実現する手法では、外力を正確に検出
してコンプライアンス動作を行わせることはできず、し
たがって制御系の中に力制御系を組込むことは困難であ
る。しかも、あくまでも外力による受動的な動作であ
り、ロボットの機能を能動的に利用して動作させること
はできない。

【０００７】本発明の目的は、ロボットの本来の構成を
有効に利用してコンプライアンス動作を行わせることが
可能で、しかもコンプライアンス動作中も外力とは別の
能動的な動作を行わせることができるロボットの制御方
法および装置を提供することである。

【０００８】

【課題を解決するための手段】本発明は、予め指定され
るロボットの手首の動作目標位置Ｐe および単位処理段
階当たりの移動幅ｗに従うロボットの制御方法であっ
て、コンプライアンス動作開始直前に、手首に装着され
たツールの初期姿勢Ｓおよび手首の初期位置Ｐs を算出
し、ロボットのベース座標系から見た移動ベクトルを、
動作目標位置Ｐe および初期位置Ｐs から算出し、ベー
ス座標系で予め定める方向を、算出された移動ベクトル
に平行とするような変換行列Ｑを算出しておき、コンプ
ライアンス動作中のロボットの各軸角度θ1 を検出し、
各軸角度θ1 から手首の現在位置Ｐr を算出し、現在位
置Ｐr に変換行列Ｑを掛けて、Ｑ座標系での位置^qＰr＝
Ｑ・Ｐr を算出する段階と、位置^qＰrの前記予め定める
方向の成分に前記移動幅ｗを加算した位置^qＰr’を算出
し、ベース座標系での手首位置Ｐr'＝Ｑ^-1・^qＰr’と初
期姿勢Ｓとに基づく姿勢行列を含む位置指令値Ｔを算出
する段階と、位置指令値Ｔを逆変換し、各軸指令値を算
出した後、各軸を駆動する段階とをロボットの手首が動
作目標位置Ｐe に到達するまで繰返すことを特徴とする
ロボットの制御方法である。 本発明に従えば、コンプライアンス動作によって外力に
沿った動きを行うロボットのコンプライアンス動作開始
直前に、ロボットの動作指定軸に装着されたツールの初
期姿勢Ｓおよび初期位置Ｐs が算出される。また、ロボ
ットのベース座標系から見た移動ベクトルを、予め指定
される動作目標位置Ｐe および初期位置Ｐs から算出
し、ベース座標系で予め定める方向を、算出された移動
ベクトルに平行とするような変換行列Ｑを算出してお
く。コンプライアンス動作中は、ロボットの各軸角度θ
1 が検出され、各軸角度θ1 から手首の現在位置Ｐr を
算出し、現在位置Ｐr に変換行列Ｑを掛けて、Ｑ座標系
での位置^qＰr＝Ｑ・Ｐr を算出する。Ｑ座標系で、位置
^qＰrの予め定める移動方向の成分のみに移動幅ｗを加算
したＱ座標系での位置^qＰr’に対応するベース座標系で
の手首の移動位置は、Ｐr'＝Ｑ^-1・^qＰr’として算出さ
れる。移動位置^qＰr’および初期姿勢Ｓに基づいて、コ
ンプライアンス動作を可能とする位置指令値Ｔが算出さ
れる。位置指令値Ｔを逆変換し、各軸指令値を算出する
ことによって、予め定める方向に能動的に動作させなが
ら、コンプライアンス動作が可能となる。

【０００９】また本発明でコンプライアンス動作時の軸
ゲイン値は、任意に設定可能であることを特徴とする。 本発明に従えば、コンプライアンス動作時の軸ゲイン値
が任意に指定可能であるので、ゲインを小さくしてコン
プライアンスを大きくしたり、ゲインを大きくしてスチ
フネスを大きくしたりして、コンプライアンス度の調整
が可能となる。

【００１０】また本発明で、コンプライアンス動作中の
移動は、検出した各軸角度θ1 が予め定める動作制限角
度を超えるとき、またはベース座標系およびツール座標
系での移動量が予め定める移動範囲を超えるときに制限
されることを特徴とする。 本発明に従えば、コンプライアンス動作中の移動範囲
は、各軸角度θ1 、またはベース座標系でのｘｙｚ方
向，ツール座標系でのｘｙｚ方向の移動量として設定す
ることができる。移動量が予め設定される移動範囲を超
えると、移動が制限されるので、ロボットに備えられて
いる安全機能をコンプライアンス動作中も活用すること
ができる。

【００１１】また本発明は、コンプライアンス動作中に
重力補償演算を行い、演算結果を各軸のトルク指令値に
加算することを特徴とする。 本発明に従えば、コンプライアンス動作中に重力補償演
算を行って、演算結果を各軸のトルク指令値に加算する
ので、コンプライアンス動作中にロボットの各軸の姿勢
が変化しても、その姿勢に対応した重力補償を正確に行
うことが可能になる。重力補償のために制御系に積分器
を用いる必要がなくなるので、コンプライアンス制御と
通常制御との切換えの際に急激な動作を行う可能性を解
消させることができる。

【００１２】さらに本発明は、ロボットの手首にツール
が装着され、駆動源によって駆動されるロボットの各軸
角度θ1 を軸角度検出手段によって検出し、検出した軸
角度θ1 と軸角度の指令値との偏差が零になるように駆
動源を負帰還制御するロボットの制御装置において、コ
ンプライアンス動作開始直前に、ツールの初期姿勢Ｓお
よび初期位置Ｐs を算出し、ロボットのベース座標系か
ら見た移動ベクトルを、動作目標位置Ｐe および初期位
置Ｐs から算出し、ベース座標系で予め定める方向を、
算出された移動ベクトルに平行とするような変換行列Ｑ
を算出する第１演算手段と、コンプライアンス動作中の
ロボットの各軸角度θ1 を検出し、各軸角度θ1 から手
首の現在位置Ｐr を算出し、現在位置Ｐr に変換行列Ｑ
を掛けて、Ｑ座標系での位置^qＰr＝Ｑ・Ｐr を算出する
第２演算手段と、第２演算手段からの位置^qＰrの予め指
定される方向の成分に、予め指定される移動幅ｗを加算
した位置^qＰr’を算出し、ベース座標系での手首位置Ｐ
r'＝Ｑ^-1・^qＰr’と初期姿勢Ｓとに基づく姿勢行列を含
む位置指令値Ｔを算出する第３演算手段と、第３演算手
段の出力に応答し、位置指令値Ｔに基づく前記駆動源の
負帰還制御を、ロボットの手首が動作目標位置Ｐe に到
達するまで行う第４演算手段とを含むことを特徴とする
ロボットの制御装置である。 本発明に従えば、第１演算手段によってコンプライアン
ス動作開始直前に、ツールの初期姿勢Ｓ、初期位置Ｐs
、および変換行列Ｑが算出される。第２演算手段は、
コンプライアンス動作中のロボットの各軸角度θ1 を検
出し、Ｑ座標系で予め定める移動方向のみに移動幅ｗを
加算し、対応するベース座標系の移動位置Ｐr'を算出
し、初期姿勢Ｓに基づく姿勢行列を含む位置指令値Ｔを
算出する。第３演算手段は、第２演算手段からの位置指
令値Ｔを逆変換し、各軸指令値を算出後、各軸を駆動す
る。第４演算手段は、第３演算手段の出力に応答し、位
置指令値Ｔに含まれる軸角度の指定値と、軸角度検出手
段によって検出される軸角度θ1 とを偏差が零になるよ
うに駆動源を負帰還制御する。第４演算手段の負帰還制
御によって、外力に沿ったコンプライアンス動作を行い
ながら、予め定める移動方向についての能動的な作業を
行うことができる。

【００１３】

【発明の実施の形態】図１は、本発明の実施の一形態に
用いる産業用のロボット１の構成を示す。この産業用の
ロボット１は、先端のハンド２を用いて部品３をワーク
４に挿入する作業を自動的に行う。ロボット１は、床面
５に設置されるベース６上のアーム７およびアーム８を
有し、アーム８の先端のフランジ９には、ツール装着用
アーム部１０が接続される。ベース６は、床面５に垂直
な第１軸１１まわりに角変位可能である。アーム７およ
びアーム８は、ベース６およびアーム７に対して、床面
５に平行な第２軸１２および第３軸１３まわりにそれぞ
れ角変位可能である。ツール装着用アーム部１０は、ア
ーム８の中心を通る第４軸１４まわりに角変位可能であ
る。ツール装着用アーム部１０は、第５軸１５および第
６軸１６まわりに各変位可能な回転関節を有し、これら
によって連結されるアーム１７，１８，１９を有する。
アーム１９内には歯車機構が設けられ、第６軸１６と平
行な軸線２ａの延長上に手先であるハンド２が装着さ
れ、部品３を把持することができる。ハンド２によって
把持された部品３は、作業台２０上に保持されるワーク
４に設けられる穴２１に挿入され、嵌め合わされる。

【００１４】図２は、部品３をワーク４の穴２１に挿入
する過程を示す。なお、説明の便宜上、ハンド２は図示
を省略する。図２（１）では、部品３を穴２１に接近さ
せる状態を示す。図２（２）では、部品３と穴２１とが
偏心しているので、部品３の先端が穴２１の斜面に接触
し、反力によるコンプライアンス動作に従って、挿入方
向とは異なる偏心を矯正する方向に移動する状態を示
す。図２（３）では、穴２１の斜面で部品３の先端が案
内され、ワーク４の穴２１内に正しく挿入されている状
態を示す。部品３とワーク４の当接面では、部品３の挿
入による押付力と反力とがつり合う位置で停止する。

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

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

【００１７】

【数１】

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

【００１９】

【数２】

【００２０】ここでαは定数である。位相補償器３６の
出力は、もう１つの加算手段３７に与えられる。加算手
段３７には、エンコーダ２７の出力に応答する重力補償
演算回路３８からの出力がライン３８ａを介して与えら
れて加算され、その加算出力はライン３９から各軸のト
ルク指令値として制御回路２２の増幅回路２６に入力さ
れる。重力補償を行うことによって、コンプライアンス
動作時は積分器３５の機能を停止させても、ロボットの
各アームなどが落下するのを防ぐことができる。積分器
３５の機能を停止させることによって、コンプライアン
ス動作と通常動作とを切換えるときに、アームに予期し
ない急激な運動を生じさせるおそれを解消することがで
きる。

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

【００２２】次に、ベース座標系でのｚ軸方向に移動幅
ｗで移動させながら、コンプライアンス動作をさせる原
理を説明する。コンプライアンス動作を開始したときの
ツール座標系の原点位置をtＰΣt（θ0）とする。ロボ
ットに外力が加わったとき、ツール座標系の原点位置が
tＰΣt（θ1）に移動したとすると、次の第３式が得ら
れる。なお、括弧の右肩の「Ｔ」は、転置記号を示す。

【００２３】 ^tＰΣ_t（θ1）＝（ｄ^tＰΣ_t.x ｄ^tＰΣ_t.y ｄ^tＰΣ_t.z）^T …（３） このとき、移動方向がベースｚ方向なので変換行列Ｑに
よる座標変換は省略するが、^tＰΣ_tを変更することによ
って、ｚ軸方向にのみ移動するようにする。ｘ，ｙ軸成
分の位置指令値は零とし、ｚ軸成分の位置指令値に外力
により移動したｚ軸成分を与えると、次の第４式が得ら
れる。

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

【００２５】図６は、より一般的な場合についての本実
施形態によるコンプライアンス動作を示す。ステップａ
１から動作を開始し、ステップａ２に移って各軸角度θ
0 を検出する。ステップａ３では、第５式および第６式
にそれぞれ示すツール初期位置Ｐs およびツール初期姿
勢Ｓを検出し、第７式に示すようなＲ行列を求める。

【００２６】

【数３】

【００２７】ステップａ４では、本件ロボット１の各制
御回路２２，２３，２４への重力補償を開始する。ま
た、これらの制御回路２２，２３，２４の係数器３１，
３４および増幅回路などのゲインを小さく変更して設定
する。たとえばツール１０が装着される第６軸１６の位
置ゲインＫｐおよび速度ゲインＫｖを、小さく設定し、
また積分器３５のゲインＫi を零とし、この積分器３５
の内容を零にクリアし、さらに位相補償器３６の機能を
停止する。換言すると、減算手段３２の出力をそのまま
加算手段３７に与えたときの状態とする。ステップａ５
では、各軸移動範囲を設定する。ステップａ６では、直
線動作時の移動範囲を設定する。ステップａ７では、移
動幅ｗを指定する。次にステップａ８でコンプライアン
ス動作を開始し、ステップａ９で動作目標位置Ｐe を指
定し、ステップａ１０で移動ベクトルＲseを算出する。
さらに次の第８式に示す変換によって移動ベクトルＲse
を座標変換するとき、変換されたＱ座標系で移動ベクト
ルが座標軸ｘ’ｙ’ｚ’のうちの一つ、たとえばｘ’軸
と平行になるような変換を表す変換行列Ｑを算出する。

【００２８】 ^qＰse＝Ｑ・Ｐse …（８） ステップａ１１では終了のときの時間を設定し、ステッ
プａ１２で設定時間が経過していると判断されないとき
は、ステップａ１３で、第１軸１１〜第６軸１６につい
ての各軸角度θ1 を、次の第９式のように検出する。

【００２９】 θ1 ＝ （θ_JT1.1 θ_JT2.1 θ_JT3.1 θ_JT4.1 θ_JT5.1 θ_JT6.1）^T …（９） ステップａ１４で各軸値が設定範囲内であると判断され
るときは、ステップａ１５でさらに各軸角度θ1 に従っ
て、手首の現在位置Ｐr を算出する。次にステップａ１
６に移り、次の第１０式に従って、現在位置Ｐr をＱ座
標系に変換する。

【００３０】 ^qＰr ＝ Ｑ・Ｐr …（10） ステップａ１７では目標位置に到達したか否かを判断す
る。すなわちステップａ１２と併せて、次の，のう
ちのいずれかの条件を満たしているか否かで、動作終了
を判断する。 手首の現在位置Ｐr が動作目標位置Ｐe に到達してい
るか。 動作開始後、予め設定される時間が経過したか。

【００３１】ステップａ１８で移動量が設定範囲内であ
ると判断されるときは、ステップａ１９に移り、Ｑ座標
系で、移動幅ｗだけ、たとえばｘ’方向に移動した位置
^qＰr’を求める。ステップａ２０では、Ｑ座標系の移動
位置^qＰr’を次の第１１式に従ってベース座標系に変更
し、ベース座標系での移動位置Ｐr'を算出する。

【００３２】 Ｐr'＝Ｑ^-1・^qＰr' …（11） ステップａ２１では、移動位置Ｐr'および姿勢Ｓに対応
するＲ行列を求め、逆変換により各軸の位置指令値Ｔを
生成する。ステップａ２２では、生成された各軸の位置
指令値Ｔに基づいて、各軸の駆動を行う。次にステップ
ａ１３に戻り、以下ステップａ２２までの動作を繰返
す。

【００３３】ステップａ１２で設定時間経過と判断され
るときは、ステップａ２３でコンプライアンス動作を終
了する。ステップａ１４で各軸値が設定範囲内でないと
判断されるときは、ステップａ２４で動作を終了する。
ステップａ１７で目標位置到達と判断されるときは、ス
テップａ２５で動作を終了する。ステップａ１８で移動
量が設定範囲内でないと判断されるときは、ステップａ
２６で動作を終了する。

【００３４】図７は、変換行列Ｑによる座標変換の状態
を簡略化して示す。初期位置Ｐs から動作目標位置Ｐe
までの移動ベクトルＰseに、ｘ軸をＱ座標系に変換した
ｘ’軸が平行になるような変換として、変換行列Ｑを求
める。ｘ’ｙ’ｚ’のいずれか１軸に平行に変換するこ
とによって、移動幅ｗの加算処理が容易となる。変換行
列Ｑは、第７式に示したように４行×４列で表される。
このうち、第４行目を除く部分をＱ’とすれば、次の第
１２式が得られる。

【００３５】 Ｑ’＝［ｎ，ｏ，ａ，ｐ］^T …（12） ここで、ｐは零ベクトルであり、ｎ，ｏ，ａは、次の第
１３、１４および１５式でそれぞれ表されるベクトルで
ある。

【００３６】

【数４】

【００３７】図８は、本発明の実施の他の形態を概略的
に示す。本実施形態では、産業用ロボット５１のハンド
５２が被研磨材５３を把持し、研磨装置５４によって研
磨する。研磨装置５４では、研磨ベルト５５がプーリ５
６，５７間に掛け渡され、高速度で搬送される。産業用
ロボット５１は、被研磨材５３を設定した移動幅に比例
した一定の押圧力で研磨ベルト５５に押し付ける動作
を、コンプライアンス動作を行いながら実現する。産業
用ロボット５１の制御系の構成は、図３〜図５に示した
ものと同様であり、コンプライアンス動作中は積分器３
５を切っておく。研磨ベルト５５からのベルト反力が変
動しても、偏差が一定となるように位置フィードバック
され、偏差量が押し付け量に比例するので、安定した研
磨作業か可能である。

【００３８】以上説明したような動作は、６軸型ロボッ
トなどの多軸型ロボットや、他の形式のロボットで同様
に適用可能である。

【００３９】

【発明の効果】以上のように本発明によれば、予め定め
る移動方向についての能動的な動作と、外力を逃がすよ
うなコンプライアンス動作による追従動作とを容易に行
うことができる。

【００４０】また本発明によれば、能動的な動作による
移動方向は、外力の作用する方向と逆方向であるので、
外力の大きさに応じて、移動方向への移動の継続、停
止、移動方向と逆方向への戻りが切り換えられるような
動作を行うことが可能となる。

【００４１】また本発明によれば、コンプライアンス動
作時の軸ゲイン値が任意に指定可能であるので、外力の
発生状態に応じて位置剛性とコンプライアンスとの調整
を行うことができる。

【００４２】また本発明によれば、コンプライアンス動
作中でも、各軸角度θ1 やベース座標系およびツール座
標系での移動量がそれぞれ予め定める範囲内に制限され
るので、ツールを作動させてもロボット自身の動作範囲
を超えるような動作を行うことがなく、安全性や信頼性
を容易に確保することができる。

【００４３】また本発明によれば、各軸のトルク指令値
には重力補償演算結果が加算されるので、コンプライア
ンス動作中に姿勢が変わっても正確に重力補償を行うこ
とができる。

【００４４】さらに本発明によれば、コンプライアンス
動作開始直前に、ツールの初期姿勢Ｓ、初期位置Ｐs 、
および変換行列Ｑが算出される。コンプライアンス動作
中には、ロボットの各軸角度θ1 が検出され、各軸角度
θ1 に基づく現在位置Ｐr に対して、Ｑ座標系で予め定
める移動方向のみに移動幅ｗが加算され、対応するベー
ス座標系での移動位置Ｐr'が算出され、初期姿勢Ｓに基
づく姿勢行列を含む位置指令値Ｔが算出される。位置指
令値Ｔに含まれる軸角度の指定値と、検出される軸角度
θ1 とを偏差が零になるように駆動源が負帰還制御され
るので、外力に沿ったコンプライアンス動作を行いなが
ら、予め定める移動方向についての能動的な作業を行う
ことができる。

【図面の簡単な説明】

【図１】本発明の実施の一形態の産業用ロボット１の正
面図である。

【図２】図１の部品３とワーク４との関係を示す簡略化
した断面図である。

【図３】図１の産業用ロボット１の簡略化した電気的構
成を示すブロック図である。

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

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

【図６】図３のサーボ制御回路２１およびホスト制御回
路２２の動作を示すフローチャートである。

【図７】図６の動作におけるＱ座標系への変換の考え方
を示す模式図である。

【図８】本発明の実施の他の形態を示す簡略化した正面
図である。

【符号の説明】

１，５１ 産業用ロボット ２，５２ ハンド ３ プロテクタ ４ パイプ １０ ツール １１ 第１軸 １２ 第２軸 １３ 第３軸 １４ 第４軸 １５ 第５軸 １６ 第６軸 ２１ 穴 ２２ 制御回路 ２３ サーボ制御回路 ２４ ホスト制御回路 ２５ サーボモータ ２６ 増幅回路 ２７ エンコーダ ２９ 減算手段 ３１，３４ 係数器 ３２，３７ 加算手段 ３８ 重力補償回路 ５３ 被研磨材 ５４ 研磨装置 ５５ 研磨ベルト

Claims (5)

【特許請求の範囲】
1. 【請求項１】 予め指定されるロボットの手首の動作目
   標位置Ｐe および単位処理段階当たりの移動幅ｗに従う
   ロボットの制御方法であって、 コンプライアンス動作開始直前に、手首に装着されたツ
   ールの初期姿勢Ｓおよび手首の初期位置Ｐs を算出し、 ロボットのベース座標系から見た移動ベクトルを、動作
   目標位置Ｐe および初期位置Ｐs から算出し、 ベース座標系で予め定める方向を、算出された移動ベク
   トルに平行とするような変換行列Ｑを算出しておき、 コンプライアンス動作中のロボットの各軸角度θ1 を検
   出し、各軸角度θ1 から手首の現在位置Ｐr を算出し、
   現在位置Ｐr に変換行列Ｑを掛けて、Ｑ座標系での位置
   ^qＰr＝Ｑ・Ｐr を算出する段階と、 位置^qＰrの前記予め定める方向の成分に前記移動幅ｗを
   加算した位置^qＰr’を算出し、ベース座標系での手首位
   置Ｐr'＝Ｑ^-1・^qＰr’と初期姿勢Ｓとに基づく姿勢行列
   を含む位置指令値Ｔを算出する段階と、 位置指令値Ｔを逆変換し、各軸指令値を算出した後、各
   軸を駆動する段階とをロボットの手首が動作目標位置Ｐ
   e に到達するまで繰返すことを特徴とするロボットの制
   御方法。
2. 【請求項２】 コンプライアンス動作時の軸ゲイン値
   は、任意に設定可能であることを特徴とする請求項１記
   載のロボットの制御方法。
3. 【請求項３】 コンプライアンス動作中の移動は、検出
   した各軸角度θ1 が予め定める動作制限角度を超えると
   き、またはベース座標系およびツール座標系での移動量
   が予め定める移動範囲を超えるときに制限されることを
   特徴とする請求項１または２に記載のロボットの制御方
   法。
4. 【請求項４】 コンプライアンス動作中に重力補償演算
   を行い、演算結果を各軸のトルク指令値に加算すること
   を特徴とする請求項１〜３のいずれかに記載のロボット
   の制御方法。
5. 【請求項５】 ロボットの手首にツールが装着され、駆
   動源によって駆動されるロボットの各軸角度θ1 を軸角
   度検出手段によって検出し、検出した軸角度θ1 と軸角
   度の指令値との偏差が零になるように駆動源を負帰還制
   御するロボットの制御装置において、 コンプライアンス動作開始直前に、ツールの初期姿勢Ｓ
   および初期位置Ｐs を算出し、ロボットのベース座標系
   から見た移動ベクトルを、動作目標位置Ｐe および初期
   位置Ｐs から算出し、ベース座標系で予め定める方向
   を、算出された移動ベクトルに平行とするような変換行
   列Ｑを算出する第１演算手段と、 コンプライアンス動作中のロボットの各軸角度θ1 を検
   出し、各軸角度θ1 から手首の現在位置Ｐr を算出し、
   現在位置Ｐr に変換行列Ｑを掛けて、Ｑ座標系での位置
   ^qＰr＝Ｑ・Ｐr を算出する第２演算手段と、 第２演算手段からの位置^qＰrの予め指定される方向の成
   分に、予め指定される移動幅ｗを加算した位置^qＰr’を
   算出し、ベース座標系での手首位置Ｐr'＝Ｑ^-1・^qＰr’
   と初期姿勢Ｓとに基づく姿勢行列を含む位置指令値Ｔを
   算出する第３演算手段と、 第３演算手段の出力に応答し、位置指令値Ｔに基づく前
   記駆動源の負帰還制御を、ロボットの手首が動作目標位
   置Ｐe に到達するまで行う第４演算手段とを含むことを
   特徴とするロボットの制御装置。