JPH1148180A - ロボット制御装置 - Google Patents

Abstract

(57)【要約】 【課題】ワークとツールとを所定の位置関係に保持し,
加工精度を向上させること。 【解決手段】教示点におけるツールの指令先端位置及び
指令姿勢を示す位置姿勢行列[K_i]を読み込み(ステップ1
02),[K_i]に基づいて補間点における位置姿勢行列[K_t]を
求める(ステップ104)。次に[K_t] に基づいてツール座標
系原点の位置姿勢行列[K_tT]を求め(ステップ106),[K_tT]
を逆変換して補間点における各軸の指令回転角を求める
(ステップ108)。次に各指令回転角信号を出力し(ステッ
プ110), 各エンコーダ値に基づいてツールの現先端位置
を求める (ステップ112)。次にツールの現先端位置と指
令姿勢とを用いて[K_t']を得て(ステップ114),[K_t']の逆
変換により各指令回転角を補正し(ステップ116),補正さ
れた各指令回転角信号を出力することにより(ステップ1
18),ツールの姿勢を指令姿勢に保持できる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ツールに作用する
外力に対して、従動的に動作するロボットの制御装置に
関し、特にツールの姿勢を所定方向に保持させるように
したロボットの制御装置に関する。

【０００２】

【従来の技術】従来、ツールに作用する外力に対して従
動的に動作するロボットの制御装置としては、例えば、
特開平８−１５５８６８号公報に開示されている制御装
置がある。この制御装置による制御方法を図７に模式的
に示す。ダイキャストマシーンから成形品を取り出すと
き、油圧シリンダによる従動動作によりツール３０に対
して矢印３２の方向に外力Ｆが作用すると（図７
（ａ））、手首３１が変位し、ツール３０が矢印３２の
方向に変位する（図７（ｂ））。そして、ツール３０を
予め定まった移動方向３３に射影した位置３４を求め
（図７（ｃ））、ツール３０を位置３４に位置させると
共に、ツール３０の姿勢を初期の姿勢に保持できるよう
に制御するというものである（図７（ｄ））。他の従動
的に動作するロボットの制御装置としては、ツールの姿
勢及び先端の位置により電流指令値の制限値を変化させ
ることにより、所定レベル以上の力がツールの先端に作
用したとき、その力を吸収する方向にロボットを移動さ
せるようにした装置がある。

【０００３】

【発明が解決しようとする課題】しかしながら、上記従
来技術のうち前者では、ツール３０の移動方向が所定の
一定方向であり、その移動の間ツール３０は初期の姿勢
を保持するので、適用分野はダイキャスト成形品に限定
され、他の従動動作に対しては有効ではない。又、後者
の従来技術では、ロボットの各軸の電流を制限するた
め、ツールの先端に作用する外力の方向によってツール
の先端がふらつくという問題がある。例えば、ワークの
バリ取りロボットの場合には、図８に模式的に示される
ように、バリにツール（グラインダー）の先端が当接す
ることにより、ツールの先端が外力を受け、バリの大き
さや材質などの条件により指令姿勢（実線）に比べてフ
ィードバックされた姿勢にふらつきを生じる。このよう
に、ツールの先端がふらつくと、ワークとツールとを一
定の位置関係に保持することができず、加工精度の低下
をもたらすという問題がある。

【０００４】従って、本発明の目的は、上記課題に鑑
み、ロボットの従動動作時でツールの先端位置が変化し
たときにツールの姿勢を指令姿勢に保持することによ
り、ワークとツールとを一定の位置関係に保持し、加工
精度を向上させることである。

【０００５】

【課題を解決するための手段】上記課題を解決するため
に、請求項１に記載の手段は、サーボモータによって駆
動される複数の軸を備え、外部から作用する外力に対し
て従動的に動作するロボットの制御装置であって、教示
点とその教示点間の任意の補間点とにおいてロボットの
手首部に装着されたツールの指令姿勢及び指令先端位置
を表す位置姿勢行列を逆変換することにより、ロボット
の各軸の指令回転角を求める指令回転角演算手段と、指
令回転角演算手段により得られた指令回転角に基づいて
サーボモータを駆動させる出力手段と、ロボットの各軸
の回転角を検出する回転角検出手段と、回転角検出手段
により検出されたロボットの各軸の回転角を用いて、教
示点及び補間点における指令姿勢にツールの姿勢が保持
されるように、位置姿勢行列を補正する姿勢補正手段
と、姿勢補正手段により補正された位置姿勢行列を逆変
換することにより、指令回転角を補正する回転角補正手
段と、回転角補正手段により得られた補正値に基づいて
サーボモータを駆動する出力補正手段とを備えたことを
特徴とする。

【０００６】又、請求項２に記載の手段は、手首部の装
着される位置の基準点であるリスト点の位置及び姿勢を
制御する第１種の軸群の駆動トルクを制限するトルク制
限手段を備えたことを特徴とする。

【０００７】請求項３に記載の手段は、回転角検出手段
により検出されたロボットの各軸の教示点及び補間点に
おける回転角の順変換により、ツールの現先端位置を求
める第１演算手段と、第１演算手段により求められたツ
ールの現先端位置を変化させずに、ツールの姿勢が教示
点及び補間点における指令姿勢に保持されるように位置
姿勢行列を補正する第２演算手段とで姿勢補正手段が構
成されることを特徴とする。

【０００８】請求項４に記載の手段は、出力補正手段
は、トルク制限手段によりトルクが制限されていない第
２種の軸群に対してのみ補正値に基づいてサーボモータ
を駆動することを特徴とする。

【０００９】

【発明の作用及び効果】上記の課題を解決する請求項１
に記載の手段によれば、ツールに対する外力の作用によ
りツールが従動的に動作し、ツールの先端位置が指令先
端位置から変化したとき、姿勢補正手段及び出力補正手
段により、ツールの姿勢を指令姿勢に保持することがで
きるので、ワークとツールとを一定の位置関係に保持で
き、加工精度を向上できる。

【００１０】請求項２に記載の手段によれば、リスト点
の位置及び姿勢を制御する第１種の軸群の駆動トルクが
トルク制限手段により制限されるので、例えば、ツール
先端がバリに当接したとき、リスト点の位置を変化させ
てツールが従動動作でき、指令姿勢に対するツールの姿
勢の変化の程度を小さくできる。

【００１１】請求項３に記載の手段によれば、ツールの
現先端位置を変化させずに、ツールの姿勢が教示点及び
補間点における指令姿勢に保持されるので、加工精度を
より向上できる。

【００１２】請求項４に記載の手段によれば、トルクが
制限されていない第２種の軸群に対してのみ補正値に基
づいてサーボモータが駆動されるので、従動動作により
変位したリスト点を基準にしてツールの姿勢が指令姿勢
に保持でき、高速な姿勢制御が可能となる。

【００１３】

【発明の実施の形態】以下、本発明を具体的な実施例に
基づいて説明する。図１は、本発明の具体的な実施例に
係わる６軸多関節ロボット１０及びその制御装置１００
の構成を示した模式図である。このロボット１０は、例
えば設置台２０上に固定されたワーク２１のバリ取りな
どに用いられる。ロボット１０は、固定ベース２２上に
ローテータ１１が鉛直な軸線（１軸）Ｘ１を中心に水平
面内で回転自在に支持され、ローテータ１１上には、第
１アーム１２が水平な軸線（２軸）Ｘ２を中心に揺動自
在に支持されている。この第１アーム１２には第２アー
ム１３が水平な軸線（３軸）Ｘ３を中心に揺動自在に支
持されている。第２アーム１３の先端のフランジ１３ａ
には、第１リンク１４が軸線（４軸）Ｘ４を中心に回転
自在に支持されている。尚、フランジ１３ａの原点が、
請求項でいうリスト点に相当し、ローテータ１１、第１
アーム１２及び第２アーム１３が第１種の軸群に相当す
る。第１リンク１４には、第２リンク１５が水平な軸線
（５軸）Ｘ５を中心に揺動自在に支持されている。第２
リンク１５には、第３リンク１６が軸線（６軸）Ｘ６を
中心に回転自在に支持されている。第１リンク１４、第
２リンク１５及び第３リンク１６より、手首部１７が構
成されている。手首部１７の先端のフランジ（図略）に
は、１対の把持爪１９ａを有するハンド１９が装着され
ており、このハンド１９はツール（グラインダー）１８
を把持している。尚、ハンド１９に後述するツール座標
系が設けられており、第１リンク１４〜第３リンク１６
が第２種の軸群に相当する。

【００１４】次に、図２に基づいて制御装置１００の主
な構成について説明する。制御装置１００は中央処理装
置（ＣＰＵ）８０を有しており、このＣＰＵ８０にはメ
モリ８１、各軸に対応するサーボユニット９１〜９６及
びティーチングボックス７０が接続されている。各サー
ボユニット９１〜９６は、図示しないＣＰＵとメモリを
備えており、ＣＰＵ８０から出力される指令回転角信号
θ1 〜θ6 、重力トルクＧf1〜Ｇf6及びイナーシャ値Ｊ
L1〜ＪL6に基づいてロボット１０が有する１軸から６軸
を駆動するためのサーボモータＭ１〜Ｍ６を制御する。
各サーボモータＭ１〜Ｍ６にエンコーダＥ１〜Ｅ６（回
転角検出手段）が連結され、それぞれの出力α１〜α６
はＣＰＵ８０及びサーボユニット９１〜９６に入力して
おり、ＣＰＵ８０による各軸の重力トルク値及びイナー
シャ値の演算とサーボユニット９１〜９６による位置フ
ィードバック制御及び速度フィードバック制御等に用い
られる。

【００１５】メモリ８１は、ロボット１０の動作プログ
ラムを記憶するプログラムエリア、教示点等の加工に必
要なデータを記憶する加工データエリア、エンコーダＥ
１〜Ｅ６の出力α１〜α６等を記憶する制御データエリ
アなどを備えている。ティーチングボックス７０は、ロ
ボット１０の教示作業や、動作プログラムを入力するた
めのものであり、動作プログラム等を表示するディスプ
レイ７０ａと、ロボット１０に対する動作指令や、動作
プログラム等の入力を行うキーボード７０ｂを備えてい
る。

【００１６】各サーボユニット９１〜９６は、図３に示
す回路の機能をデジタル制御によって達成する。即ち、
サーボユニット９１〜９６は、位置ループ４０１、速度
ループ４０２を備えると共に、重力トルクフィードフォ
ワード４０５、加速度トルクフィードフォワード４０６
及び速度フィードフォワード４０７を備えている。重力
トルクフィードフォワード４０５及び加速度トルクフィ
ードフォワード４０６は、速度ループ４０２後の指令値
に入力され、速度フィードフォワード４０７は、位置ル
ープ４０１後の指令値に入力される。又、速度ループ４
０２後に、重力トルクフィードフォワード４０５及び加
速度トルクフィードフォワード４０６が入力された後に
は、電流制限部４０３が設けられており、この電流制限
部４０３で定められた電流指令値は、アンプ部４０４に
出力されるようになっている。電流制限部４０３は、重
力トルクフィードフォワード４０５の重力トルク値に基
づいて電流制限値を決定する。

【００１７】以上の構成に基づいて制御装置１００の作
用を説明する。図５は、制御装置１００の処理手順を示
したフローチャートである。従動動作が開始されると、
ＣＰＵ８０の指令によりモータＭ１〜Ｍ３に対する最大
電流の制限幅が通常の制限幅より低減された電流制限状
態になる。この電流制限によって、ロボット１０は外力
に対して従動的に変位可能となる。尚、本実施例では、
１軸Ｘ１、２軸Ｘ２及び３軸Ｘ３の３つの軸に対して電
流制限を行っている。電流制限状態になると、まず、教
示点におけるツール１８のワールド座標系での指令位置
及び指令先端位置を示す位置姿勢行列〔Ｋ_i 〕を読み込
む（ステップ１０２）。位置姿勢行列〔Ｋ_i 〕は、図４
に示すようにツール１８の指令姿勢を示す３×３の行列
〔Ｃ_i 〕と、ツール１８の先端の指令位置を示す３×１
の行列〔Ｐ_i 〕とを有する４×４の同時行列であり、
〔Ｋ_i 〕＝〔Ｃ_i ，Ｐ_i 〕と表すことができる。尚、上
記に示される各行列のサフィックスは教示点の番号に対
応している。

【００１８】次に、位置姿勢行列〔Ｋ_i 〕に基づいて、
教示点間の任意の補間点におけるツール１８先端の指令
位置及び指令姿勢を示す位置姿勢行列〔Ｋ_t 〕（＝〔Ｃ
_t ，Ｐ_t 〕）を求める（ステップ１０４）。ここで、補
間点は、よく知られたように回転主軸法により求められ
る。即ち、隣接する２つの教示点間の位置姿勢行列を回
転させる主軸を求め、その主軸の回りの微小回転角Δα
だけ回転させる変換行列〔Ｓt(Δα,j) 〕を求める。そ
して、補間点の位置姿勢行列を〔Ｋ_t 〕＝〔Ｋ_i 〕×
〔Ｓt(Δα,j) 〕により求める。よって、ツール１８は
回転主軸の回りに一定速度で移動でき、一定速度で回転
する。これにより、補間点におけるツール１８先端の指
令位置は教示点間の曲線補間により得られ、その指令姿
勢は教示点間の姿勢回転の均等割りにより得られる。

【００１９】次に、ハンド１９に固定されたツール座標
系におけるツール１８の姿勢及び先端位置を示す位置姿
勢行列を〔Ｔ〕とすると、補間点におけるツール座標系
原点の指令位置及び指令姿勢を示す位置姿勢行列
〔Ｋ_tT〕は、式（１）より得られる（ステップ１０
６）。ここで、位置姿勢行列〔Ｔ〕は、姿勢を示す３×
３の単位行列と、ツール１８の先端位置を示す３×１の
行列とを有する４×４の行列である。

【００２０】

【数１】 〔Ｋ_tT〕 ＝ 〔Ｋ_t 〕×〔Ｔ〕^-1 ─（１）

【００２１】次に、位置姿勢行列〔Ｋ_tT〕を逆変換し、
補間点におけるロボット１０の各軸の指令回転角を求め
る（ステップ１０８、指令回転角演算手段）。尚、ツー
ル座標系原点の位置及び姿勢と各軸の回転角θ1 〜θ6
との間には、ロボット１０に固有の関係式が成立し、回
転角θ1 〜θ6 から位置姿勢への順変換、位置姿勢から
回転角θ1 〜θ6 への解析的な逆変換が可能である。次
に、ステップ１０８で得られた指令回転角に基づいて各
サーボモータＭ１〜Ｍ６を駆動させる（ステップ１１
０、出力手段）。次に、各エンコーダＥ１〜Ｅ６の出力
を入力し、その値を順変換し、ツール座標系原点の現在
の位置姿勢行列〔Ｋ_tT ^c 〕を求め、さらに、ツール１８
の位置姿勢行列〔Ｔ〕をかけて、ツール１８先端の位置
姿勢行列〔Ｋ_t ^c 〕を求める。尚、〔Ｋ_t ^c 〕＝〔Ｃ_t
^c ，Ｐ_t ^c 〕である。この手法により、ツール１８の現
在の先端位置〔Ｐ_t ^c 〕が求められる（ステップ１１
２、第１演算手段）。次に、ステップ１１２で得られた
ツール１８の現在の先端位置〔Ｐ_t ^c 〕と、ステップ１
０６で得られた指令姿勢〔Ｃ_t 〕とを用いて、式（２）
により補正された位置姿勢行列〔Ｋ_t ' 〕を求める（ス
テップ１１４、第２演算手段）。

【００２２】

【数２】 〔Ｋ_t ' 〕 ＝ 〔Ｃ_t ，Ｐ_t ^c 〕 ─（２）

【００２３】次に、この補正された位置姿勢行列
〔Ｋ_t ' 〕と同様な処理でツール座標系原点の位置姿勢
行列〔Ｋ_tT' 〕に変換し、ステップ１０６、ステップ１
０８と同様な処理で〔Ｋ_tT' 〕を逆変換し、ロボット１
０の各軸の指令回転角を補正する（ステップ１１６、回
転角補正手段）。次に、補正された指令回転角θ1'〜θ
6'に基づいて各サーボモータＭ１〜Ｍ６を駆動させる
（ステップ１１８、出力補正手段）。そして、次の補間
点があれば、ステップ１０４に戻って次の補間点に対し
て上記処理を繰り返す。ステップ１２０にて、次の補間
点がなければステップ１２２にて次の教示点があるか否
かを判定し、次の教示点がある場合にはステップ１０２
に戻って上記処理を繰り返し、次の教示点がない場合に
は処理を終了する。

【００２４】上記作用により、ツール１８の先端がバリ
に当接することにより、指令位置と異なる位置を示して
も、ツール１８の姿勢が指令姿勢に維持される。この様
子を模式的に図６に示す。この図に見られるように、ツ
ール１８が指令位置と異なる位置にあってもツール１８
が指令姿勢を維持するので、ツール１８とワーク２１と
を一定の位置関係に保持できる。これにより、加工精度
を向上できる。

【００２５】上記実施例では、ツール１８の先端位置を
現在位置から変化させずに１軸〜６軸を制御することで
姿勢を指令姿勢に制御する構成としたが、ツール１８と
ワーク２１とを一定の位置関係に保持できれば他の制御
としてもよい。例えば、ステップ１１６の出力を、４軸
〜６軸に対してのみ行うことにより、ツール１８の姿勢
を指令姿勢に制御する構成としてもよい。この姿勢変化
において、大きな負荷がかかる場合には１軸〜３軸が電
流制限されているので、リスト点が柔軟となっており、
姿勢変化は容易に行われる。このようにして、ツール１
８とワーク２１との位置関係を一定に保持できるので、
上記実施例と同様に加工精度を向上できると共に、制御
速度を上げることができる。又、ツール１８の現在の先
端位置と指令先端位置との偏差に応じてツール１８の移
動速度を制御する構成を付加してもよい。これにより、
例えばツール１８の指令先端位置に対する現在の先端位
置の偏差が大きい場合には、ツール１８の移動速度を小
さくすることによって、より加工精度を向上させること
ができる。又、ツール１８の指令先端位置に対する現在
の先端位置の偏差が小さい場合には、ツール１８の移動
速度を大きくすることによって、加工時間を短縮でき
る。又、上記実施例では、制御装置１００をバリ取りロ
ボット１０に適用したが、本発明はワークの挿入などを
行う組付けロボット等への適用も可能である。

【００２６】上記に示されるように、本発明によれば、
ロボットの従動動作時に外力の作用によってツール先端
が指令位置と異なる位置に位置したとき、ツールの姿勢
を指令姿勢に保持させることにより、ツールとワークと
の関係を一定の位置関係に保持できるので、加工精度を
向上させることが可能である。

【図面の簡単な説明】

【図１】本発明の具体的な実施例に係わる６軸多関節ロ
ボットの構成を示した模式図。

【図２】本発明の具体的な実施例に係わるコントローラ
の構成を示したブロック図。

【図３】本発明の具体的な実施例に係わるサーボユニッ
トの処理を示したブロック図。

【図４】ツールの姿勢とツール先端位置とを示す位置姿
勢行列の構成を示した説明図。

【図５】本発明の具体的な実施例に係わる制御装置の処
理手順を示したフローチャート。

【図６】本発明の具体的な実施例に係わる制御装置の作
用によりツールの姿勢が指令姿勢に保持された様子を示
した模式図。

【図７】従来の、ツールに作用する外力に対して従動的
に動作するロボットの制御装置による作用を示した模式
図。

【図８】従来の制御装置によるツールの姿勢のふらつき
を示した模式図。

【符号の説明】

１０ ロボット １１ ローテータ（１軸） １２ 第１アーム １３ 第２アーム １４ 第１リンク １５ 第２リンク １６ 第３リンク １７ 手首部 １８ ツール １９ ハンド ２０ 設置台 ２１ ワーク ２２ 固定ベース ８０ ＣＰＵ ８１ メモリ ９１〜９６ サーボユニット １００ 制御装置

Claims (4)

【特許請求の範囲】
1. 【請求項１】 サーボモータによって駆動される複数の
   軸を備え、外部から作用する外力に対して従動的に動作
   するロボットの制御装置であって、 教示点と該教示点間の任意の補間点とにおいて前記ロボ
   ットの手首部に装着されたツールの指令姿勢及び指令先
   端位置を表す位置姿勢行列を逆変換することにより、前
   記ロボットの各軸の指令回転角を求める指令回転角演算
   手段と、 前記指令回転角演算手段により得られた前記指令回転角
   に基づいて前記サーボモータを駆動させる出力手段と、 前記ロボットの各軸の回転角を検出する回転角検出手段
   と、 前記回転角検出手段により検出された前記ロボットの各
   軸の回転角を用いて、前記教示点及び前記補間点におけ
   る前記指令姿勢に前記ツールの姿勢が保持されるよう
   に、前記位置姿勢行列を補正する姿勢補正手段と、 前記姿勢補正手段により補正された前記位置姿勢行列を
   逆変換することにより、前記指令回転角を補正する回転
   角補正手段と、 前記回転角補正手段により得られた補正値に基づいて前
   記サーボモータを駆動する出力補正手段とを備えたこと
   を特徴とするロボット制御装置。
2. 【請求項２】 前記手首部の装着される位置の基準点で
   あるリスト点の位置及び姿勢を制御する第１種の軸群の
   駆動トルクを制限するトルク制限手段を備えたことを特
   徴とする請求項１に記載のロボット制御装置。
3. 【請求項３】 前記姿勢補正手段は、 前記回転角検出手段により検出された前記ロボットの各
   軸の前記教示点及び前記補間点における回転角の順変換
   により、前記ツールの現先端位置を求める第１演算手段
   と、 前記第１演算手段により求められた前記ツールの前記現
   先端位置を変化させずに、前記ツールの姿勢が前記教示
   点及び前記補間点における前記指令姿勢に保持されるよ
   うに前記位置姿勢行列を補正する第２演算手段とから成
   ることを特徴とする請求項１又は２に記載のロボット制
   御装置。
4. 【請求項４】 前記出力補正手段は、前記トルク制限手
   段によりトルクが制限されていない第２種の軸群に対し
   てのみ前記補正値に基づいて前記サーボモータを駆動す
   ることを特徴とする請求項１又は２に記載のロボット制
   御装置。