JP3588956B2 - ロボット制御装置 - Google Patents
ロボット制御装置 Download PDFInfo
- Publication number
- JP3588956B2 JP3588956B2 JP04947497A JP4947497A JP3588956B2 JP 3588956 B2 JP3588956 B2 JP 3588956B2 JP 04947497 A JP04947497 A JP 04947497A JP 4947497 A JP4947497 A JP 4947497A JP 3588956 B2 JP3588956 B2 JP 3588956B2
- Authority
- JP
- Japan
- Prior art keywords
- robot
- current
- limit value
- external force
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Description
【０００１】
【発明の属する技術分野】
本発明は、外力に対してロボットが従動的に動作するロボットの制御装置に関するものである。
【０００２】
【従来の技術】
従来、ロボットが作業する際にワークまたは加工機側からロボットに対して外力が作用する場合がある。このような場合、ロボットをその外力に追従するように同期制御を行うか、ハンド部に弾性部材等を有するフローティングブラケットを取り付けて、機械的に外力を逃がす手段が行われていた。
【０００３】
しかし、上記した同期制御の場合には制御プログラムが複雑になり、フローティングブラケットの場合には、コスト高になるという欠点がある。このため特開平２−１４５２７８号に開示されているようなロボットの各軸を駆動するサーボモータに供給する電流を制限する方法が行われている。この技術は、サーボモータに供給する最大電流を通常よりも低く制限することによって、ロボットにその最大電流よりも大きな電流を必要とする負荷が作用すると、ロボットはその負荷に負け柔軟な状態になることを利用して、ロボットを外力に対して従動させるようにしたものである。この電流制限を用いることによって、簡単な方法で外力に対するロボットの従動動作を実現することができる。
【０００４】
【発明が解決しようとする課題】
上述したような従来の電流制限は、図１２に示すようにサーボモータのトルク（電流値）０を基準としてプラス、マイナス両方向にα％という形式で設定されている。しかし、ロボットのある動作に対して電流制限を行う場合、各サーボモータレベルで見た場合の電流値の変化は、必ずしもトルク０を中心として変化するわけではない。このため、例えばロボットの特定の動作における外力が作用しない状態でのあるサーボモータの電流指令値の変化が図１２の曲線Ｌ１で示すように不規則に変動するものである場合、電流指令値に対する電流制限幅は、プラス方向にｄ１、マイナス方向にｄ２となり、異なったものとなる。これは方向によって、ロボットが外力に耐え得る力が異なることになり、柔軟度合いが一定にならないという問題がある。
【０００５】
また、曲線Ｌ１の二点鎖線部分で示すようにロボットの姿勢を維持するのに必要なトルクが、既に電流制限幅を越えている状態で電流制限を作用させた場合、トルクの不足分ｄ３だけロボットの姿勢が外力に関係なく重力に負けて変化してしまうという問題があった。
本発明は以上述べた問題を解決するためになされたものであり、ロボットの外力に対する柔軟度合いを一定とし、安定した従動動作が可能なロボット制御装置を提供することを目的とする。
【０００６】
【課題を解決するための手段】
上記した目的を達成するための請求項１に記載された手段は、サーボモータによって駆動される複数の軸を備えたロボットを制御するロボット制御装置において、前記ロボットの姿勢に応じた重力トルク値を演算する演算手段と、前記演算手段によって演算された前記重力トルク値に基づいて前記ロボットの姿勢を維持する姿勢維持手段と、前記演算手段によって演算された前記重力トルク値に基づいて前記サーボモータに供給する電流の制限値を補間周期毎に決定する制限値決定手段と、この制限値決定手段によって決定された制限値内に前記サーボモータに供給する電流を制限して前記サーボモータを駆動する電流制限手段と、外部装置から前記ロボットに作用する外力に対して前記電流制限手段により前記サーボモータを駆動すると共に、前記ロボットを前記電流の制限値を越えて作用する外力に対して従動的に動作させる従動動作手段とを備え、前記従動動作手段は、外力に対して前記ロボットを移動させる方向を記憶する移動方向記憶手段と、前記移動方向記憶手段に記憶された移動方向と直交する方向に前記ロボットが移動した誤差量を検出する誤差量検出手段と、この誤差量検出手段によって検出された誤差量があらかじめ設定された設定範囲以内かを判別する判別手段と、この判別手段によって前記誤差量が前記設定範囲より外れる場合、前記移動方向記憶手段に記憶された移動方向に基づいて前記ロボットの従動動作を補正する動作補正手段とを備えたものである。
【０００８】
（作用）
請求項１において、演算手段はロボットの姿勢に応じた重力トルク値を演算し、姿勢維持手段はこの重力トルク値に基づいてロボットの姿勢を維持し、制限値決定手段はこの重力トルク値に基づいてサーボモータに供給する電流の制限値を決定する。そして、電流制限手段は、この制限値内にサーボモータに供給する電流を制限する。このため制限値以上の電流が必要な外力がロボットに作用した時には、ロボットは外力に対して柔軟に従動する状態となる。
【０００９】
請求項１において、従動動作手段によって、ロボットが外力に対して従動的に動作している間、誤差量検出手段は移動方向記憶手段に記憶された移動方向と直交する方向にロボットが移動した誤差量を検出し、この誤差量があらかじめ設定された設定範囲から外れることが判別手段によって判別された場合には、動作補正手段により移動方向記憶手段に記憶された移動方向に基づいてロボットの従動動作が補正される。従って、ロボットの従動動作があらかじめ定められた方向から一定値以上ずれた場合のみ移動方向が補正される。
【００１０】
【発明の実施の形態】
本発明の実施の形態について図面を参照して説明する。
図１は本実施の形態の全体構成を示した図である。図１において１０は６軸垂直多関節ロボット、３０は搬出装置３１（外部装置）を備えたダイキャストマシン、４０はダイキャストマシン３０から搬出したワークＷを載置する載置台である。
【００１１】
ロボット１０は、固定ベース２７上にローテータ１１（１軸）が鉛直な軸線を中心に水平面内で回転自在に支持され、ローテータ１１上には、第１アーム１２（２軸）が水平な軸線を中心に揺動自在に支持されている。この第１アーム１２には第２アーム１３（３軸）が水平な軸線を中心に揺動自在に支持されている。また、ローテータ１１上には、水平な軸線を中心に揺動自在に第１リンク１８が支持されており、この第１リンク１８と第２アーム１３の末端が第２リンク１９で連結されいる。従って、第２アーム１３は、第１リンク１８が駆動されることによって揺動するようになっている。
【００１２】
第２アーム１３の先端部には、４軸、５軸、６軸を備える手首部１４が回動可能に支持されている。手首部１４の先端のフランジには、一対の把持爪１７ａ（図５参照）を有するハンド１７が取り付けられている。
ダイキャストマシン３０は、鋳造されたワークＷをガイド３７に沿って搬出する搬出装置３１を備えている。搬出装置３１は、図略のシリンダによって駆動されるものであり、ワークＷの搬出を開始する位置（後退端）にあることを検出するリミットスイッチ３５と、搬出装置３１がワークＷの搬出を完了する位置（前進端）にあることを検出するリミットスイッチ３６を備えている。
【００１３】
次に図２に基づいてロボット１０の制御装置の主な構成について説明する。図２において８０は中央処理装置（ＣＰＵ）である。この中央処理装置８０には、メモリ８１、各軸に対応するサーボユニット９１〜９６およびティーチングボックス７０が接続されている。
サーボユニット９１〜９６はそれぞれサーボＣＰＵとメモリを備えており、中央処理装置８０から出力される指令回転角信号θ１〜θ６、重力トルク値Ｇｆ１〜Ｇｆ６、イナーシャ値ＪＬ１〜ＪＬ６等に基づいて、ロボット１０が有する１軸から６軸を駆動するためのサーボモータＭ１〜Ｍ６を制御する。各サーボモータＭ１〜Ｍ６に連結されたエンコーダＥ１〜Ｅ６の出力α１〜α６は中央処理装置８０およびサーボユニット９１〜９６に入力しており、中央処理装置８０による各軸の重力トルク値およびイナーシャ値の演算とサーボユニット９１〜９６による位置フィードバック制御および速度フィードバック制御等に用いられる。
【００１４】
メモリ８１はロボット１０の動作プログラムを記憶するプログラムエリアや、教示点等の加工に必要なデータを記憶する加工データエリア、エンコーダＥ１〜Ｅ６の出力α１〜α６等を記憶する制御データエリア等を備えている。
ティーチングボックス７０は、ロボットの教示作業や、動作プログラムを入力するためのものであり、動作プログラム等を表示するディスプレイ７０ａと、ロボット１０に対する動作指令や、動作プログラム等の入力を行うキーボード７０ｂを備えている。
【００１５】
また、中央処理装置８０は、ダイキャストマシン３０の制御装置３７からの出力信号を入力するようになっている。この出力信号には、搬出装置３１に設けられたリミットスイッチ３５，３６の出力信号等がある。
上記サーボユニット９１〜９６は、図３に示す回路の機能をディジタル制御によって達成するものである。即ち、サーボユニット９１〜９６は、位置ループ４０１、速度ループ４０２を備えると共に、重力トルクフィードフォワード４０５、加速度トルクフィードフォワード４０６、速度フィードフォワード４０７を備えている。重力トルクフィードフォワード４０５および加速度トルクフィードフォワード４０６は、速度ループ４０２後の指令値に入力され、速度フィードフォワード４０７は、位置ループ４０１後の指令値に入力されるようになっている。
【００１６】
また、速度ループ４０２に重力トルクフィードフォワード４０５および加速度トルクフィードフォワード４０６が入力された後には、電流制限部４０３が設けられており、この電流制限部４０３で定められた電流指令値は、アンプ部４０４に出力されるようになっている。後述するように電流制限部４０３は、重力トルクフィードフォワード４０５の重力トルク値Ｇｆに基づいて電流制限値を決定するようになっている。
【００１７】
以上の構成に基づいて本実施の形態の作用を説明する。
図４に示すように本実施の形態では、ロボット１０は初期位置Ｐ０（図略）から移動を開始し、Ｐ１の位置においてダイキャストマシン３０から搬出されたワークＷを把持する。そして搬出装置３１の搬出動作に従動しながら搬出装置３１が搬出を完了する搬出完了点Ｐ１’までワークＷを把持し続ける。この従動動作において本実施の形態の特徴部分である後述する電流制限が実行される。また、従動動作をより安定的なものとするため、従動動作時にロボットの動作軌跡を補正する補正演算が行われる。
【００１８】
ロボット１０は、搬出が完了したＰ１’の位置から載置台４０上の位置Ｐ２にワークＷを移載した後、初期位置Ｐ０に復帰して１サイクルを終了する。以上の動作において、上記Ｐ０，Ｐ１，Ｐ２の位置におけるロボット１０の位置および姿勢が教示点データとしてメモリ８１に記憶されている。この教示点データは作業者があらかじめティーチングボックス７０により教示するものである。なお、本実施の形態では、ティーチングをより容易とするために、位置Ｐ１’は教示点とせず、単にロボット１０が搬出装置３１の動作に従動して移動する位置となっているが、教示点として記憶しておくことも可能である。
【００１９】
また、作業者は、ハンド１７の位置および姿勢の基準となる工具座標系をあらかじめ設定しておく必要ある。本実施の形態の場合、工具座標系は図５に示すようにハンド１７が有する２つの把持爪１７ａの中間に座標原点ＰＴが設定され、この座標原点ＰＴに対して３つの姿勢ベクトルが設定されている。即ち、ハンド１７がワークＷに接近する方向を示すアプローチベクトルＡは、ハンド１７がワークＷに取り付けられている突設部Ｗａを把持した時、搬出装置３１がワークＷを搬出する方向Ａｗと平行な方向となるように設定されている。ただし、図５の場合、アプローチベクトルＡとワークＷの搬出方向Ａｗとは逆向きとなる。また、一方の把持爪１７ａから他方の把持爪１７ａに向かう方向にオリエントベクトルＯ、アプローチベクトルＡとオリエントベクトルＯの両者に直交し、右手系をなす方向にノーマルベクトルＮが設定されている。
【００２０】
次に図６に示すフローチャートにより本実施の形態の全体の流れを説明する。動作開始時のステップ１００おいてロボット１０は、初期位置Ｐ０に位置している。ステップ１０２においてダイキャストマシン３０の加工が終了し、搬出装置３１のリミットスイッチ３５がＯＮとなり、ワークＷが搬出開始位置に設置されたことが検出されると、ステップ１０４に移行し、ロボット１０は教示点Ｐ１に移動して、ワークＷの突設部Ｗａを把持する。
【００２１】
ロボット１０に設置された図略のリミットスイッチがＯＮとなり、ワークＷの把持が完了すると、ステップ１０６においてロボット１０は、従動動作モードとなる。従動動作モードが開始されると、ロボット１０のその時の姿勢を維持するのに最低必要な重力トルクが重力トルクフィードフォワード４０５により指令され（姿勢維持手段）、ロボット１０のモータＭ１〜Ｍ６に対する最大電流の制限幅が後述する通常の制限幅より低減された電流制限状態となる。この電流制限については後に図７，８にて詳述する。ロボット１０は、重力トルクフィードフォワード４０５によりその時の姿勢を維持しつつ、この電流制限によって、搬出装置３１からの外力によって従動的に変位可能となる。なお、本実施の形態では６軸全てに対して電流制限を行っているが、従動動作に関与しない軸がある場合は必ずしも全ての軸に電流制限を行う必要はない。
【００２２】
ロボット１０がワークＷを把持したことに対応して、搬出装置３１がワークＷの搬出動作を開始すると、ステップ１０８では、従動動作モードとなったロボット１０が搬出装置３１の搬出方向に沿って滑らかに移動するように補正演算を行う。この補正演算については後に図９，１０によって詳述する。
ステップ１１０では、搬出装置３１のリミットスイッチ３６がＯＮとなり、ワークＷの搬出動作が完了すると、従動動作モードから通常の動作を実行する通常モードに変更する。即ち、モータＭ１〜Ｍ６に対する電流制限状態を解除し、電流の制限幅を通常の状態とする。この後、ステップ１１２に移行し、ロボット１０は現在位置Ｐ１’から載置台４０上の教示点Ｐ２までワークＷを移載する。これにより一連のワークＷの取り出し作業は終了し、ステップ１１４に移行して、ロボット１０は初期位置Ｐ０に戻り、動作を終了する（ステップ１１６）。
【００２３】
次に上記した電流制限について説明する。本実施の形態は、従来サーボモータのトルク０を基準に設定されていた電流制限幅を、サーボユニット９１〜９６の制御で使用するために中央処理装置８０が逐次演算する重力トルク値Ｇｆを利用して、この重力トルク値を基準に電流制限幅を設定するものである。まず重力トルク値Ｇｆの求め方の概要について説明する。
【００２４】
各軸の重力トルク値Ｇｆ１〜Ｇｆ６は、ロボット１０をモデル化することによって以下のように求められる。なお、１軸の重力トルク値Ｇｆ１は０となる理由は、ロボット１０を床面に設置した場合、１軸は床面に対して垂直な軸となり重力トルクが作用しないためである。
Ｇｆ１＝０
Ｇｆ２＝ｇ１・ｍ１ ＋ｇ４・ｍ４＋Ｒ１（ｍ２＋ｍ３＋ｍ６）＋ｇ６・ｍ６
Ｇｆ３＝ｇ２・ｍ２＋ｇ３・ｍ３−ｇ５・ｍ５＋ｇ６・ｍ６
Ｇｆ４＝−ｇ３・ｍ３−ｇ６・ｍ６
Ｇｆ５＝ｇ３・ｍ３＋ｇ６・ｍ６
Ｇｆ６＝ｇ６・ｍ６
ただし、
ｇ１：１軸から第１アーム１２の重心までの水平距離
ｇ２：２軸から第２アーム１３の重心までの水平距離
ｇ３：手首部軸心（４軸、５軸）から手首部１４重心までの水平距離
ｇ４：第２リンク１９軸心より第２リンク１９の重心までの水平距離
ｇ５：第１リンク１８軸心より第１リンク１８の重心までの水平距離
ｇ６：６軸よりハンド１７の重心までの水平距離
Ｒ１：第１アーム１２の長さ
ｍ１：第１アーム１２の質量
ｍ２：第２アーム１３の質量
ｍ３：手首部１４の質量
ｍ４：第２リンク１９の質量
ｍ５：第１リンク１８の質量
ｍ６：ハンド１７の質量
なお、ｇ１からｇ６はロボット１０の姿勢に応じて変化する変数である。
【００２５】
次に図７により補間周期毎に各サーボユニット９１〜９６にて実行される各軸における電流制限部４０３の作用を説明する。
ステップ２００で電流制限部４０３が実行される。この時、電流制限部４０３には、上記式に基づいて中央処理装置８０において演算されたロボット１０の現在の姿勢に対応した重力トルク値Ｇｆと、重力トルクフィードフォワード４０５および加速度フィードフォワード４０６が入力された速度ループ４０２からの電流指令値Ｉｏが入力されている。なお、重力トルク値Ｇｆは電流指令値に換算された値となっている。
【００２６】
ステップ２０２では、現在が従動動作モードであるか通常モードであるかを判別する。上記したステップ１０６で従動動作モードに設定されている場合は（ＹＥＳ）、ステップ２０４に移行して重力トルク値Ｇｆを基準とした電流制限を行う。また、通常モードに設定されている場合は（ＮＯ）、従来のサーボモータのトルク０を基準とした通常の電流制限を行うため、ステップ２１６に移行する。
【００２７】
従動動作モードでは、ステップ２０４にて電流指令値Ｉｏが電流制限の上限値を越えていないかを判断する。この上限値は、重力トルク値Ｇｆを基準として設定されてＧｆ＋ｄとなり、制限幅ｄは後述する通常モードの制限幅Ｉｍａｘ，Ｉｍｉｎより小さい値に設定されている。このステップ２０４において、電流指令値Ｉｏが電流制限の上限値Ｇｆ＋ｄを越えていないならば（ＹＥＳ）、ステップ２０８に移行する。また、電流指令値Ｉｏが電流制限の上限値Ｇｆ＋ｄを越えているならば（ＮＯ）、ステップ２０６に移行して、電流指令値Ｉｏを上限値Ｇｆ＋ｄとし、ステップ２１２に移行する。
【００２８】
ステップ２０８では、電流指令値Ｉｏが電流制限の下限値を下回っていないかを判断する。電流制限の下限値は、上限値と同様に重力トルク値Ｇｆを基準として設定され、Ｇｆ−ｄとなる。電流指令値Ｉｏが下限値Ｇｆ−ｄを下回っていないならば（ＹＥＳ）、ステップ２１２に移行する。また、電流指令値Ｉｏが電流制限の上限値Ｇｆ−ｄｉを越えているならば（ＮＯ）、ステップ２１０に移行して、電流指令値Ｉｏを下限値Ｇｆ−ｄｉとし、ステップ２１２に移行する。
【００２９】
ステップ２１２では、ステップ２０６から２１０で決定した電流指令値Ｉｏをアンプ部４０４に出力し、電流制限部の作用を終了する（ステップ２１４）。
一方、ステップ２１６以降の通常モードの場合は、上記したように電流制限幅がトルク０を基準とした上限値Ｉｍａｘと下限値Ｉｍｉｎに設定される点が異なるのみで基本的を流れは同じであるため説明を省略する。
【００３０】
以上の過程によって、電流指令値Ｉｏは、ロボットの姿勢に対応した重力トルク値Ｇｆを基準として電流が制限され、この制限幅を越える電流値を必要とする外力がロボット１０に作用した場合には、ロボット１０は柔軟な状態となる。図８は、あるサーボユニットにおけるロボット１０が教示点Ｐ１から搬出完了点Ｐ１’まで従動動作する時の電流制限の様子を示したものである。曲線Ｌ２は、ロボット１０のモデルより中央処理装置８０が演算した重力トルク値Ｇｆの変化を示したものであり、曲線Ｌ２−１，Ｌ２−２は、曲線Ｌ２の重力トルク値Ｇｆに基づいて設定された制限幅の上限値および下限値の変化を示したものである。また、曲線Ｌ３は実際にロボット１０を教示点Ｐ１から搬出完了点Ｐ１’まで外力を作用させずに移動させた場合に出力された電流指令値の変化を示したものである。これから判るように、制限幅を実際のロボット１０の電流変化にほぼ対応して設定することが可能となる。
【００３１】
次に図９および図１０のフローチャートに基づいて上記したステップ１０８における補正演算について説明する。この補正演算は、ロボット１０が搬出装置３１からの外力によって従動的に変位する際、ハンド１７の姿勢を一定に保ちつつ、搬出装置３１の動きに滑らかに追従して、ロボット１０に余分な負荷を生じさせないための制御を行うものである。以下の説明では、ロボット１０の移動における補間時ｔでの同時変換行列をＫｔ、この同時変換行列Ｋｔにおける姿勢データをＣｔ、位置データＰｔとする。従って、同時変換行列Ｋｔは数１で表される。
【００３２】
【数１】
【００３３】
なお、上記数１において、（Ｎｔｘ，Ｎｔｙ，Ｎｔｚ），（Ｏｔｘ，Ｏｔｙ，Ｏｔｚ），（Ａｔｘ，Ａｔｙ，Ａｔｚ），（Ｐｔｘ，Ｐｔｙ，Ｐｔｚ）は、ロボット座標系から見た補間時ｔにおけるノーマルベクトルＮｔ、オリエントベクトルＯｔ、アプローチベクトルＡｔ、位置ベクトルＰｔの各成分である。
上記したステップ１０８において補正演算プログラムが起動されると（ステップ３００）、ステップ３０２では、補間時ｔにおけるエンコーダＥ１〜Ｅ６の出力αｔ１〜αｔ６を入力して、順変換し、同時変換行列Ｋｔ’を求める。
【００３４】
ステップ３０４では、現在従動動作中のロボット１０が所定の範囲以上に搬出装置３１の搬出方向Ａｗから誤差を生じていないかを判断する。この誤差ｈは次のように求める。
図９に示すように、まず教示点Ｐ１から教示点Ｐ１におけるアプローチ方向Ａ１方向に点Ｂを設定する。この教示点Ｐ１から点Ｂまでの距離は、教示点Ｐ１から搬出完了位置Ｐ１’までの距離より十分長い値にあらかじめ設定されたものである。このように点Ｂを設定することによって、教示点Ｐ１と点Ｂを通る直線Ｓが求まる。誤差ｈはロボット１０の現在位置である同時変換行列Ｋｔ’の位置ベクトルＰｔ’から直線Ｓまでの直線距離であるから以下の数２の関係より位置ベクトルＰｔ’から直線Ｓへ垂直に下ろした点Ｈの位置が求まる。
【００３５】
【数２】
【００３６】
この数２によって求めた点Ｈの位置と位置ベクトルＰｔ’の距離を求めることにより誤差ｈを求めることができる。従って、誤差ｈがあらかじめ設定された範囲ｒ内にある場合、即ち、図９のように直線Ｓを中心とする許容半径ｒの円柱内に位置ベクトルＰｔ’が存在する場合は（ＹＥＳ）、ロボット１０の補正演算を行う必要がないため、ステップ３０６にて目標同時変換行列Ｋｔ＝Ｋｔ’としてステップ３１２に移行する。
【００３７】
また、誤差ｈがあらかじめ設定された範囲内にない場合は（ＮＯ）、ロボット１０の追従動作の軌跡が許容範囲以上に誤差を生じていることを示しているため、ステップ３０８に移行して補正演算を行う。なお、このように補正演算を行う前にステップ３０４にて補正を必要とするか否かを判断する理由は後に詳述する。
【００３８】
ステップ３０８では、同時変換行列Ｋｔ’の位置ベクトルＰｔ’を用いて目標位置データＰｔを求める。即ち、目標位置ベクトルＰｔは以下の数３で表される。
【００３９】
【数３】
【００４０】
数３において、Ｐｐ１は教示点Ｐ１における位置ベクトルであり、Ｓｉｇｎは、現在の位置ベクトルＰｔ’が教示点Ｐ１におけるアプローチベクトルＡ１に対して正逆いずれの方向かを示すものであり、アプローチベクトルＡ１に対してロボット１０が移動している方向を示している。また、Ｓｐは、ロボット１０が教示点Ｐ１からｔまでの間に移動した距離であり以下の数４によって求まる。
【００４１】
【数４】
【００４２】
ステップ３１０では、ステップ３０８によって求めた目標位置ベクトルＰｔと、教示点Ｐ１の姿勢ベクトルＣｐ１より、目標同時変換行列Ｋｔ＝〔Ｃｐ１，Ｐｔ〕を求め、ステップ３１２にてこれを逆変換し、指令回転角信号θｔ１〜θｔ６を求めてサーボユニット９１〜９６に出力する。以上のステップで従動動作モードの１サイクルを終了する。
【００４３】
ステップ３１４では、ロボット１０がＰ１’の位置まで移動し、搬出装置３１によるワークＷの搬出が完了したか否かを判別する。この判別はワークＷの搬出完了を示す搬出装置３１のリミットスイッチ３６がＯＮしたか否かを判別する。ワークＷの搬出が完了したと判別された場合（ＹＥＳ）、ステップ３２０に移行して補正演算を終了し、図６におけるステップ１１０に移行する。また、ワークＷの搬出が完了していないと判別された場合（ＮＯ）、ステップ３１６に移行する。
【００４４】
ステップ３１６は、従動動作モードが開始されて所定の時間が経過したかを判別する。これは、搬出装置３１のリミットスイッチ３６に異常が生じた等の理由によりリミットスイッチ３６がＯＮしない場合が考えられるためである。従って、従動動作モードが開始されて所定の時間が経過した場合には（ＹＥＳ）、ステップ３１８に移行して警告を発する等の異常処理を行う。また、所定の時間が経過していない場合には（ＮＯ）、現在まだ正常な状態でワークＷの搬出が行われているため、ステップ３０２に戻り、補正演算を繰り返す。
【００４５】
以上述べたように本実施の形態では、搬出装置３１からワークＷが搬出される動作に従動的にロボット１０が移動する際、ワークＷが搬出される方向に常にロボット１０の位置を補正すると共に、姿勢はワークＷを把持した教示点Ｐ１の姿勢を保持するようにしたため、ロボット１０が搬出装置３１からの外力によって従動的に移動しても余分な力が作用することはなく、滑らかな動作が実現でき、ロボット１０の耐久性が向上する。
【００４６】
次に上記ステップ３０４における補正演算を行うか否かを判断する理由について説明する。図１１（ａ）の模式図に示すように搬出装置３１から搬出されるワークＷは、搬出が開始されてしばらくの間は、ガイド３７によって支持されており、この状態では、仮に上記補正演算を行わなくとも、ガイド３７の存在によってロボット１０が従動動作によって移動する方向と搬出装置３１の搬出方向Ａｗとはほぼ一致した状態となる。しかし、図１１（ｂ）のように搬出の最終段階でワークＷがガイド３７から外れると、ワークＷは自由な状態となり、ロボット１０の従動的に移動する方向と搬出装置３１の搬出方向Ａｗとがロボット１０の剛性等によって、一致しない状態となり結果的にロボット１０に余分な負荷がかかることになる。このため、上記したような補正演算によって、ロボット１０の従動的に移動する方向を搬出方向Ａｗに補正するのである。
【００４７】
しかし、この補正方向は、上記したように教示点Ｐ１にて教示したアプローチベクトルＡ１に基づくものであるため、アプローチベクトルＡ１と搬出方向Ａｗとの間には、教示誤差が存在し必ずしも一致しない。このため、図１１（ｃ）に示すように教示誤差がある状態で厳密にアプローチベクトルＡ１に基づいて補正を行うと、むしろロボット１０の移動に伴って位置誤差ｄＤが増大し、ワークＷをガイド３７に押し付け、特にガイド３７の出口近傍Ｃでは大きな力が作用し、ワークＷまたはガイド３７を傷める可能性がある。このため、本実施の形態では、ある一定の範囲では補正を行わずガイド３７による案内に従う遊びの状態を設けることによって、ワークＷまたはガイド３７を傷めず、かつある一定以上の誤差が生じた場合には適切な補正が行われるようにしているのである。
【００４８】
次に他の実施の形態について説明する。上記実施の形態では、ロボット１０のアプローチ方向と、搬送装置３１の搬出方向とを平行とすることによって、ロボット１０の従動動作の際に常に教示時のアプローチ方向Ａ１に動作を補正するようにしているが、搬送装置３１の搬出方向をあらかじめ記憶するようにしておき、この方向にロボット１０をの動作を補正するようにしても良い。例えば、上記実施の形態では教示しなかった点Ｐ１’を教示し、教示点Ｐ１から教示点Ｐ１’に向かう方向をベクトルＢｖとして記憶する。このベクトルＢｖの方向にロボット１０を補正すれば良く、上記数３は以下の数５となる。なお、この場合は上記した工具座標系の設定において、アプローチベクトルＡをワークＷの搬出方向Ａｗと平行となるように設定する必要はない。
【００４９】
〔数５〕
Ｐｔ＝Ｐｐ１ ＋ Ｓｉｇｎ・Ｓｐ・Ｂｖ
また、上記実施の形態では、ロボット１０の姿勢を常に教示点Ｐ１の姿勢データＣｐ１に保持している。しかし、搬送装置３１からの外力に応じてロボット１０の姿勢を変化させたほうが都合が良い場合は、ステップ３０２で求めた同時変換行列Ｋｔ’の姿勢データＣｔ’を用いて、ステップ３１０の目標同時変換行列ＫｔをＫｔ＝〔Ｃｔ’，Ｐｔ〕として求めれば良い。このように上記した式は、搬送装置３１とロボット１０の構成に応じて種々の変更が許されることは勿論である。
【００５０】
さらに、以上述べた実施の形態では、図３における重力トルクフィードフォワード４０５の重力トルク値Ｇｆに基づいて電流制限幅を設定したが、重力トルク値Ｇｆに加えてロボット１０の加速度トルクを考慮して電流制限幅を設定しても良い。即ち、加速度トルクフィードフォワード４０６は、下記の式に基づいて設定されているため、この加速度トルクフィードフォワード４０６から得られる値を利用することができる。
【００５１】
（Ｊｍ＋ＪＬ）／Ｋｔ
ただし、ＪＬはロボット１０のイナーシャ値、Ｊｍは各サーボモータのロータのイナーシャ値であり固定値である。また、Ｋｔはトルク定数であり固定値である。
また、加速度トルクの求め方は、特開平６−１９０７５０号等の公知技術を利用することができる。このように重力トルク値Ｇｆに加えて加速度トルクを考慮することによって、ロボット１０の動作によって生じる加速度トルクに対応して電流制限幅を設定できるため、より安定したロボット１０の従動動作を実現できる。
【００５２】
請求項１では、重力トルク値に基づいてサーボモータに供給する電流の制限値が決定されるため、ロボットのその時の姿勢を維持するのに最低必要なトルク（電流値）を基準として制限値を設定することができる。従って、ロボットの従動動作中にロボットの姿勢が変化しても各姿勢に対応した制限値となるため、外力の方向に関係なく柔軟度合いを一定にでき、従動動作を安定したものにでき、ロボットに外力に対する従動動作を行わせる際、ロボットの従動動作があらかじめ定められた方向から一定値以上ずれた場合にのみ移動方向を補正するようにしたため、設定されたロボットの従動方向と外力の方向との間に誤差があったとしても、その誤差を増大させることなく安定した従動動作が可能となる。
【図面の簡単な説明】
【図１】本発明の実施の形態の全体構成を示した図である。
【図２】本発明の実施の形態の電気的構成を示した図である。
【図３】本発明の実施の形態のサーボユニットの処理を示したブロック図である。
【図４】本発明の実施の形態の作用を説明するための図である。
【図５】本発明の実施の形態の工具座標系の設定を説明するための図である。
【図６】本発明の実施の形態の全体的流れを示したフローチャートである。
【図７】本発明の実施の形態の電流制限部の作用を示したフローチャートである。
【図８】本発明の実施の形態の電流制限の様子を示した図である。
【図９】本発明の実施の形態の補正演算を示した図である。
【図１０】本発明の実施の形態の補正演算を示したフローチャートである。
【図１１】本発明の実施の形態の補正演算の作用を説明するための図である。
【図１２】従来の技術を説明するための図である。
【符号の説明】
１０ ロボット
１７ ハンド
３０ ダイキャストマシン
３１ 搬出装置
Ａ アプローチベクトル
Ｗ ワーク
Ａｗ 搬出方向
【発明の属する技術分野】
本発明は、外力に対してロボットが従動的に動作するロボットの制御装置に関するものである。
【０００２】
【従来の技術】
従来、ロボットが作業する際にワークまたは加工機側からロボットに対して外力が作用する場合がある。このような場合、ロボットをその外力に追従するように同期制御を行うか、ハンド部に弾性部材等を有するフローティングブラケットを取り付けて、機械的に外力を逃がす手段が行われていた。
【０００３】
しかし、上記した同期制御の場合には制御プログラムが複雑になり、フローティングブラケットの場合には、コスト高になるという欠点がある。このため特開平２−１４５２７８号に開示されているようなロボットの各軸を駆動するサーボモータに供給する電流を制限する方法が行われている。この技術は、サーボモータに供給する最大電流を通常よりも低く制限することによって、ロボットにその最大電流よりも大きな電流を必要とする負荷が作用すると、ロボットはその負荷に負け柔軟な状態になることを利用して、ロボットを外力に対して従動させるようにしたものである。この電流制限を用いることによって、簡単な方法で外力に対するロボットの従動動作を実現することができる。
【０００４】
【発明が解決しようとする課題】
上述したような従来の電流制限は、図１２に示すようにサーボモータのトルク（電流値）０を基準としてプラス、マイナス両方向にα％という形式で設定されている。しかし、ロボットのある動作に対して電流制限を行う場合、各サーボモータレベルで見た場合の電流値の変化は、必ずしもトルク０を中心として変化するわけではない。このため、例えばロボットの特定の動作における外力が作用しない状態でのあるサーボモータの電流指令値の変化が図１２の曲線Ｌ１で示すように不規則に変動するものである場合、電流指令値に対する電流制限幅は、プラス方向にｄ１、マイナス方向にｄ２となり、異なったものとなる。これは方向によって、ロボットが外力に耐え得る力が異なることになり、柔軟度合いが一定にならないという問題がある。
【０００５】
また、曲線Ｌ１の二点鎖線部分で示すようにロボットの姿勢を維持するのに必要なトルクが、既に電流制限幅を越えている状態で電流制限を作用させた場合、トルクの不足分ｄ３だけロボットの姿勢が外力に関係なく重力に負けて変化してしまうという問題があった。
本発明は以上述べた問題を解決するためになされたものであり、ロボットの外力に対する柔軟度合いを一定とし、安定した従動動作が可能なロボット制御装置を提供することを目的とする。
【０００６】
【課題を解決するための手段】
上記した目的を達成するための請求項１に記載された手段は、サーボモータによって駆動される複数の軸を備えたロボットを制御するロボット制御装置において、前記ロボットの姿勢に応じた重力トルク値を演算する演算手段と、前記演算手段によって演算された前記重力トルク値に基づいて前記ロボットの姿勢を維持する姿勢維持手段と、前記演算手段によって演算された前記重力トルク値に基づいて前記サーボモータに供給する電流の制限値を補間周期毎に決定する制限値決定手段と、この制限値決定手段によって決定された制限値内に前記サーボモータに供給する電流を制限して前記サーボモータを駆動する電流制限手段と、外部装置から前記ロボットに作用する外力に対して前記電流制限手段により前記サーボモータを駆動すると共に、前記ロボットを前記電流の制限値を越えて作用する外力に対して従動的に動作させる従動動作手段とを備え、前記従動動作手段は、外力に対して前記ロボットを移動させる方向を記憶する移動方向記憶手段と、前記移動方向記憶手段に記憶された移動方向と直交する方向に前記ロボットが移動した誤差量を検出する誤差量検出手段と、この誤差量検出手段によって検出された誤差量があらかじめ設定された設定範囲以内かを判別する判別手段と、この判別手段によって前記誤差量が前記設定範囲より外れる場合、前記移動方向記憶手段に記憶された移動方向に基づいて前記ロボットの従動動作を補正する動作補正手段とを備えたものである。
【０００８】
（作用）
請求項１において、演算手段はロボットの姿勢に応じた重力トルク値を演算し、姿勢維持手段はこの重力トルク値に基づいてロボットの姿勢を維持し、制限値決定手段はこの重力トルク値に基づいてサーボモータに供給する電流の制限値を決定する。そして、電流制限手段は、この制限値内にサーボモータに供給する電流を制限する。このため制限値以上の電流が必要な外力がロボットに作用した時には、ロボットは外力に対して柔軟に従動する状態となる。
【０００９】
請求項１において、従動動作手段によって、ロボットが外力に対して従動的に動作している間、誤差量検出手段は移動方向記憶手段に記憶された移動方向と直交する方向にロボットが移動した誤差量を検出し、この誤差量があらかじめ設定された設定範囲から外れることが判別手段によって判別された場合には、動作補正手段により移動方向記憶手段に記憶された移動方向に基づいてロボットの従動動作が補正される。従って、ロボットの従動動作があらかじめ定められた方向から一定値以上ずれた場合のみ移動方向が補正される。
【００１０】
【発明の実施の形態】
本発明の実施の形態について図面を参照して説明する。
図１は本実施の形態の全体構成を示した図である。図１において１０は６軸垂直多関節ロボット、３０は搬出装置３１（外部装置）を備えたダイキャストマシン、４０はダイキャストマシン３０から搬出したワークＷを載置する載置台である。
【００１１】
ロボット１０は、固定ベース２７上にローテータ１１（１軸）が鉛直な軸線を中心に水平面内で回転自在に支持され、ローテータ１１上には、第１アーム１２（２軸）が水平な軸線を中心に揺動自在に支持されている。この第１アーム１２には第２アーム１３（３軸）が水平な軸線を中心に揺動自在に支持されている。また、ローテータ１１上には、水平な軸線を中心に揺動自在に第１リンク１８が支持されており、この第１リンク１８と第２アーム１３の末端が第２リンク１９で連結されいる。従って、第２アーム１３は、第１リンク１８が駆動されることによって揺動するようになっている。
【００１２】
第２アーム１３の先端部には、４軸、５軸、６軸を備える手首部１４が回動可能に支持されている。手首部１４の先端のフランジには、一対の把持爪１７ａ（図５参照）を有するハンド１７が取り付けられている。
ダイキャストマシン３０は、鋳造されたワークＷをガイド３７に沿って搬出する搬出装置３１を備えている。搬出装置３１は、図略のシリンダによって駆動されるものであり、ワークＷの搬出を開始する位置（後退端）にあることを検出するリミットスイッチ３５と、搬出装置３１がワークＷの搬出を完了する位置（前進端）にあることを検出するリミットスイッチ３６を備えている。
【００１３】
次に図２に基づいてロボット１０の制御装置の主な構成について説明する。図２において８０は中央処理装置（ＣＰＵ）である。この中央処理装置８０には、メモリ８１、各軸に対応するサーボユニット９１〜９６およびティーチングボックス７０が接続されている。
サーボユニット９１〜９６はそれぞれサーボＣＰＵとメモリを備えており、中央処理装置８０から出力される指令回転角信号θ１〜θ６、重力トルク値Ｇｆ１〜Ｇｆ６、イナーシャ値ＪＬ１〜ＪＬ６等に基づいて、ロボット１０が有する１軸から６軸を駆動するためのサーボモータＭ１〜Ｍ６を制御する。各サーボモータＭ１〜Ｍ６に連結されたエンコーダＥ１〜Ｅ６の出力α１〜α６は中央処理装置８０およびサーボユニット９１〜９６に入力しており、中央処理装置８０による各軸の重力トルク値およびイナーシャ値の演算とサーボユニット９１〜９６による位置フィードバック制御および速度フィードバック制御等に用いられる。
【００１４】
メモリ８１はロボット１０の動作プログラムを記憶するプログラムエリアや、教示点等の加工に必要なデータを記憶する加工データエリア、エンコーダＥ１〜Ｅ６の出力α１〜α６等を記憶する制御データエリア等を備えている。
ティーチングボックス７０は、ロボットの教示作業や、動作プログラムを入力するためのものであり、動作プログラム等を表示するディスプレイ７０ａと、ロボット１０に対する動作指令や、動作プログラム等の入力を行うキーボード７０ｂを備えている。
【００１５】
また、中央処理装置８０は、ダイキャストマシン３０の制御装置３７からの出力信号を入力するようになっている。この出力信号には、搬出装置３１に設けられたリミットスイッチ３５，３６の出力信号等がある。
上記サーボユニット９１〜９６は、図３に示す回路の機能をディジタル制御によって達成するものである。即ち、サーボユニット９１〜９６は、位置ループ４０１、速度ループ４０２を備えると共に、重力トルクフィードフォワード４０５、加速度トルクフィードフォワード４０６、速度フィードフォワード４０７を備えている。重力トルクフィードフォワード４０５および加速度トルクフィードフォワード４０６は、速度ループ４０２後の指令値に入力され、速度フィードフォワード４０７は、位置ループ４０１後の指令値に入力されるようになっている。
【００１６】
また、速度ループ４０２に重力トルクフィードフォワード４０５および加速度トルクフィードフォワード４０６が入力された後には、電流制限部４０３が設けられており、この電流制限部４０３で定められた電流指令値は、アンプ部４０４に出力されるようになっている。後述するように電流制限部４０３は、重力トルクフィードフォワード４０５の重力トルク値Ｇｆに基づいて電流制限値を決定するようになっている。
【００１７】
以上の構成に基づいて本実施の形態の作用を説明する。
図４に示すように本実施の形態では、ロボット１０は初期位置Ｐ０（図略）から移動を開始し、Ｐ１の位置においてダイキャストマシン３０から搬出されたワークＷを把持する。そして搬出装置３１の搬出動作に従動しながら搬出装置３１が搬出を完了する搬出完了点Ｐ１’までワークＷを把持し続ける。この従動動作において本実施の形態の特徴部分である後述する電流制限が実行される。また、従動動作をより安定的なものとするため、従動動作時にロボットの動作軌跡を補正する補正演算が行われる。
【００１８】
ロボット１０は、搬出が完了したＰ１’の位置から載置台４０上の位置Ｐ２にワークＷを移載した後、初期位置Ｐ０に復帰して１サイクルを終了する。以上の動作において、上記Ｐ０，Ｐ１，Ｐ２の位置におけるロボット１０の位置および姿勢が教示点データとしてメモリ８１に記憶されている。この教示点データは作業者があらかじめティーチングボックス７０により教示するものである。なお、本実施の形態では、ティーチングをより容易とするために、位置Ｐ１’は教示点とせず、単にロボット１０が搬出装置３１の動作に従動して移動する位置となっているが、教示点として記憶しておくことも可能である。
【００１９】
また、作業者は、ハンド１７の位置および姿勢の基準となる工具座標系をあらかじめ設定しておく必要ある。本実施の形態の場合、工具座標系は図５に示すようにハンド１７が有する２つの把持爪１７ａの中間に座標原点ＰＴが設定され、この座標原点ＰＴに対して３つの姿勢ベクトルが設定されている。即ち、ハンド１７がワークＷに接近する方向を示すアプローチベクトルＡは、ハンド１７がワークＷに取り付けられている突設部Ｗａを把持した時、搬出装置３１がワークＷを搬出する方向Ａｗと平行な方向となるように設定されている。ただし、図５の場合、アプローチベクトルＡとワークＷの搬出方向Ａｗとは逆向きとなる。また、一方の把持爪１７ａから他方の把持爪１７ａに向かう方向にオリエントベクトルＯ、アプローチベクトルＡとオリエントベクトルＯの両者に直交し、右手系をなす方向にノーマルベクトルＮが設定されている。
【００２０】
次に図６に示すフローチャートにより本実施の形態の全体の流れを説明する。動作開始時のステップ１００おいてロボット１０は、初期位置Ｐ０に位置している。ステップ１０２においてダイキャストマシン３０の加工が終了し、搬出装置３１のリミットスイッチ３５がＯＮとなり、ワークＷが搬出開始位置に設置されたことが検出されると、ステップ１０４に移行し、ロボット１０は教示点Ｐ１に移動して、ワークＷの突設部Ｗａを把持する。
【００２１】
ロボット１０に設置された図略のリミットスイッチがＯＮとなり、ワークＷの把持が完了すると、ステップ１０６においてロボット１０は、従動動作モードとなる。従動動作モードが開始されると、ロボット１０のその時の姿勢を維持するのに最低必要な重力トルクが重力トルクフィードフォワード４０５により指令され（姿勢維持手段）、ロボット１０のモータＭ１〜Ｍ６に対する最大電流の制限幅が後述する通常の制限幅より低減された電流制限状態となる。この電流制限については後に図７，８にて詳述する。ロボット１０は、重力トルクフィードフォワード４０５によりその時の姿勢を維持しつつ、この電流制限によって、搬出装置３１からの外力によって従動的に変位可能となる。なお、本実施の形態では６軸全てに対して電流制限を行っているが、従動動作に関与しない軸がある場合は必ずしも全ての軸に電流制限を行う必要はない。
【００２２】
ロボット１０がワークＷを把持したことに対応して、搬出装置３１がワークＷの搬出動作を開始すると、ステップ１０８では、従動動作モードとなったロボット１０が搬出装置３１の搬出方向に沿って滑らかに移動するように補正演算を行う。この補正演算については後に図９，１０によって詳述する。
ステップ１１０では、搬出装置３１のリミットスイッチ３６がＯＮとなり、ワークＷの搬出動作が完了すると、従動動作モードから通常の動作を実行する通常モードに変更する。即ち、モータＭ１〜Ｍ６に対する電流制限状態を解除し、電流の制限幅を通常の状態とする。この後、ステップ１１２に移行し、ロボット１０は現在位置Ｐ１’から載置台４０上の教示点Ｐ２までワークＷを移載する。これにより一連のワークＷの取り出し作業は終了し、ステップ１１４に移行して、ロボット１０は初期位置Ｐ０に戻り、動作を終了する（ステップ１１６）。
【００２３】
次に上記した電流制限について説明する。本実施の形態は、従来サーボモータのトルク０を基準に設定されていた電流制限幅を、サーボユニット９１〜９６の制御で使用するために中央処理装置８０が逐次演算する重力トルク値Ｇｆを利用して、この重力トルク値を基準に電流制限幅を設定するものである。まず重力トルク値Ｇｆの求め方の概要について説明する。
【００２４】
各軸の重力トルク値Ｇｆ１〜Ｇｆ６は、ロボット１０をモデル化することによって以下のように求められる。なお、１軸の重力トルク値Ｇｆ１は０となる理由は、ロボット１０を床面に設置した場合、１軸は床面に対して垂直な軸となり重力トルクが作用しないためである。
Ｇｆ１＝０
Ｇｆ２＝ｇ１・ｍ１ ＋ｇ４・ｍ４＋Ｒ１（ｍ２＋ｍ３＋ｍ６）＋ｇ６・ｍ６
Ｇｆ３＝ｇ２・ｍ２＋ｇ３・ｍ３−ｇ５・ｍ５＋ｇ６・ｍ６
Ｇｆ４＝−ｇ３・ｍ３−ｇ６・ｍ６
Ｇｆ５＝ｇ３・ｍ３＋ｇ６・ｍ６
Ｇｆ６＝ｇ６・ｍ６
ただし、
ｇ１：１軸から第１アーム１２の重心までの水平距離
ｇ２：２軸から第２アーム１３の重心までの水平距離
ｇ３：手首部軸心（４軸、５軸）から手首部１４重心までの水平距離
ｇ４：第２リンク１９軸心より第２リンク１９の重心までの水平距離
ｇ５：第１リンク１８軸心より第１リンク１８の重心までの水平距離
ｇ６：６軸よりハンド１７の重心までの水平距離
Ｒ１：第１アーム１２の長さ
ｍ１：第１アーム１２の質量
ｍ２：第２アーム１３の質量
ｍ３：手首部１４の質量
ｍ４：第２リンク１９の質量
ｍ５：第１リンク１８の質量
ｍ６：ハンド１７の質量
なお、ｇ１からｇ６はロボット１０の姿勢に応じて変化する変数である。
【００２５】
次に図７により補間周期毎に各サーボユニット９１〜９６にて実行される各軸における電流制限部４０３の作用を説明する。
ステップ２００で電流制限部４０３が実行される。この時、電流制限部４０３には、上記式に基づいて中央処理装置８０において演算されたロボット１０の現在の姿勢に対応した重力トルク値Ｇｆと、重力トルクフィードフォワード４０５および加速度フィードフォワード４０６が入力された速度ループ４０２からの電流指令値Ｉｏが入力されている。なお、重力トルク値Ｇｆは電流指令値に換算された値となっている。
【００２６】
ステップ２０２では、現在が従動動作モードであるか通常モードであるかを判別する。上記したステップ１０６で従動動作モードに設定されている場合は（ＹＥＳ）、ステップ２０４に移行して重力トルク値Ｇｆを基準とした電流制限を行う。また、通常モードに設定されている場合は（ＮＯ）、従来のサーボモータのトルク０を基準とした通常の電流制限を行うため、ステップ２１６に移行する。
【００２７】
従動動作モードでは、ステップ２０４にて電流指令値Ｉｏが電流制限の上限値を越えていないかを判断する。この上限値は、重力トルク値Ｇｆを基準として設定されてＧｆ＋ｄとなり、制限幅ｄは後述する通常モードの制限幅Ｉｍａｘ，Ｉｍｉｎより小さい値に設定されている。このステップ２０４において、電流指令値Ｉｏが電流制限の上限値Ｇｆ＋ｄを越えていないならば（ＹＥＳ）、ステップ２０８に移行する。また、電流指令値Ｉｏが電流制限の上限値Ｇｆ＋ｄを越えているならば（ＮＯ）、ステップ２０６に移行して、電流指令値Ｉｏを上限値Ｇｆ＋ｄとし、ステップ２１２に移行する。
【００２８】
ステップ２０８では、電流指令値Ｉｏが電流制限の下限値を下回っていないかを判断する。電流制限の下限値は、上限値と同様に重力トルク値Ｇｆを基準として設定され、Ｇｆ−ｄとなる。電流指令値Ｉｏが下限値Ｇｆ−ｄを下回っていないならば（ＹＥＳ）、ステップ２１２に移行する。また、電流指令値Ｉｏが電流制限の上限値Ｇｆ−ｄｉを越えているならば（ＮＯ）、ステップ２１０に移行して、電流指令値Ｉｏを下限値Ｇｆ−ｄｉとし、ステップ２１２に移行する。
【００２９】
ステップ２１２では、ステップ２０６から２１０で決定した電流指令値Ｉｏをアンプ部４０４に出力し、電流制限部の作用を終了する（ステップ２１４）。
一方、ステップ２１６以降の通常モードの場合は、上記したように電流制限幅がトルク０を基準とした上限値Ｉｍａｘと下限値Ｉｍｉｎに設定される点が異なるのみで基本的を流れは同じであるため説明を省略する。
【００３０】
以上の過程によって、電流指令値Ｉｏは、ロボットの姿勢に対応した重力トルク値Ｇｆを基準として電流が制限され、この制限幅を越える電流値を必要とする外力がロボット１０に作用した場合には、ロボット１０は柔軟な状態となる。図８は、あるサーボユニットにおけるロボット１０が教示点Ｐ１から搬出完了点Ｐ１’まで従動動作する時の電流制限の様子を示したものである。曲線Ｌ２は、ロボット１０のモデルより中央処理装置８０が演算した重力トルク値Ｇｆの変化を示したものであり、曲線Ｌ２−１，Ｌ２−２は、曲線Ｌ２の重力トルク値Ｇｆに基づいて設定された制限幅の上限値および下限値の変化を示したものである。また、曲線Ｌ３は実際にロボット１０を教示点Ｐ１から搬出完了点Ｐ１’まで外力を作用させずに移動させた場合に出力された電流指令値の変化を示したものである。これから判るように、制限幅を実際のロボット１０の電流変化にほぼ対応して設定することが可能となる。
【００３１】
次に図９および図１０のフローチャートに基づいて上記したステップ１０８における補正演算について説明する。この補正演算は、ロボット１０が搬出装置３１からの外力によって従動的に変位する際、ハンド１７の姿勢を一定に保ちつつ、搬出装置３１の動きに滑らかに追従して、ロボット１０に余分な負荷を生じさせないための制御を行うものである。以下の説明では、ロボット１０の移動における補間時ｔでの同時変換行列をＫｔ、この同時変換行列Ｋｔにおける姿勢データをＣｔ、位置データＰｔとする。従って、同時変換行列Ｋｔは数１で表される。
【００３２】
【数１】
なお、上記数１において、（Ｎｔｘ，Ｎｔｙ，Ｎｔｚ），（Ｏｔｘ，Ｏｔｙ，Ｏｔｚ），（Ａｔｘ，Ａｔｙ，Ａｔｚ），（Ｐｔｘ，Ｐｔｙ，Ｐｔｚ）は、ロボット座標系から見た補間時ｔにおけるノーマルベクトルＮｔ、オリエントベクトルＯｔ、アプローチベクトルＡｔ、位置ベクトルＰｔの各成分である。
上記したステップ１０８において補正演算プログラムが起動されると（ステップ３００）、ステップ３０２では、補間時ｔにおけるエンコーダＥ１〜Ｅ６の出力αｔ１〜αｔ６を入力して、順変換し、同時変換行列Ｋｔ’を求める。
【００３４】
ステップ３０４では、現在従動動作中のロボット１０が所定の範囲以上に搬出装置３１の搬出方向Ａｗから誤差を生じていないかを判断する。この誤差ｈは次のように求める。
図９に示すように、まず教示点Ｐ１から教示点Ｐ１におけるアプローチ方向Ａ１方向に点Ｂを設定する。この教示点Ｐ１から点Ｂまでの距離は、教示点Ｐ１から搬出完了位置Ｐ１’までの距離より十分長い値にあらかじめ設定されたものである。このように点Ｂを設定することによって、教示点Ｐ１と点Ｂを通る直線Ｓが求まる。誤差ｈはロボット１０の現在位置である同時変換行列Ｋｔ’の位置ベクトルＰｔ’から直線Ｓまでの直線距離であるから以下の数２の関係より位置ベクトルＰｔ’から直線Ｓへ垂直に下ろした点Ｈの位置が求まる。
【００３５】
【数２】
この数２によって求めた点Ｈの位置と位置ベクトルＰｔ’の距離を求めることにより誤差ｈを求めることができる。従って、誤差ｈがあらかじめ設定された範囲ｒ内にある場合、即ち、図９のように直線Ｓを中心とする許容半径ｒの円柱内に位置ベクトルＰｔ’が存在する場合は（ＹＥＳ）、ロボット１０の補正演算を行う必要がないため、ステップ３０６にて目標同時変換行列Ｋｔ＝Ｋｔ’としてステップ３１２に移行する。
【００３７】
また、誤差ｈがあらかじめ設定された範囲内にない場合は（ＮＯ）、ロボット１０の追従動作の軌跡が許容範囲以上に誤差を生じていることを示しているため、ステップ３０８に移行して補正演算を行う。なお、このように補正演算を行う前にステップ３０４にて補正を必要とするか否かを判断する理由は後に詳述する。
【００３８】
ステップ３０８では、同時変換行列Ｋｔ’の位置ベクトルＰｔ’を用いて目標位置データＰｔを求める。即ち、目標位置ベクトルＰｔは以下の数３で表される。
【００３９】
【数３】
数３において、Ｐｐ１は教示点Ｐ１における位置ベクトルであり、Ｓｉｇｎは、現在の位置ベクトルＰｔ’が教示点Ｐ１におけるアプローチベクトルＡ１に対して正逆いずれの方向かを示すものであり、アプローチベクトルＡ１に対してロボット１０が移動している方向を示している。また、Ｓｐは、ロボット１０が教示点Ｐ１からｔまでの間に移動した距離であり以下の数４によって求まる。
【００４１】
【数４】
ステップ３１０では、ステップ３０８によって求めた目標位置ベクトルＰｔと、教示点Ｐ１の姿勢ベクトルＣｐ１より、目標同時変換行列Ｋｔ＝〔Ｃｐ１，Ｐｔ〕を求め、ステップ３１２にてこれを逆変換し、指令回転角信号θｔ１〜θｔ６を求めてサーボユニット９１〜９６に出力する。以上のステップで従動動作モードの１サイクルを終了する。
【００４３】
ステップ３１４では、ロボット１０がＰ１’の位置まで移動し、搬出装置３１によるワークＷの搬出が完了したか否かを判別する。この判別はワークＷの搬出完了を示す搬出装置３１のリミットスイッチ３６がＯＮしたか否かを判別する。ワークＷの搬出が完了したと判別された場合（ＹＥＳ）、ステップ３２０に移行して補正演算を終了し、図６におけるステップ１１０に移行する。また、ワークＷの搬出が完了していないと判別された場合（ＮＯ）、ステップ３１６に移行する。
【００４４】
ステップ３１６は、従動動作モードが開始されて所定の時間が経過したかを判別する。これは、搬出装置３１のリミットスイッチ３６に異常が生じた等の理由によりリミットスイッチ３６がＯＮしない場合が考えられるためである。従って、従動動作モードが開始されて所定の時間が経過した場合には（ＹＥＳ）、ステップ３１８に移行して警告を発する等の異常処理を行う。また、所定の時間が経過していない場合には（ＮＯ）、現在まだ正常な状態でワークＷの搬出が行われているため、ステップ３０２に戻り、補正演算を繰り返す。
【００４５】
以上述べたように本実施の形態では、搬出装置３１からワークＷが搬出される動作に従動的にロボット１０が移動する際、ワークＷが搬出される方向に常にロボット１０の位置を補正すると共に、姿勢はワークＷを把持した教示点Ｐ１の姿勢を保持するようにしたため、ロボット１０が搬出装置３１からの外力によって従動的に移動しても余分な力が作用することはなく、滑らかな動作が実現でき、ロボット１０の耐久性が向上する。
【００４６】
次に上記ステップ３０４における補正演算を行うか否かを判断する理由について説明する。図１１（ａ）の模式図に示すように搬出装置３１から搬出されるワークＷは、搬出が開始されてしばらくの間は、ガイド３７によって支持されており、この状態では、仮に上記補正演算を行わなくとも、ガイド３７の存在によってロボット１０が従動動作によって移動する方向と搬出装置３１の搬出方向Ａｗとはほぼ一致した状態となる。しかし、図１１（ｂ）のように搬出の最終段階でワークＷがガイド３７から外れると、ワークＷは自由な状態となり、ロボット１０の従動的に移動する方向と搬出装置３１の搬出方向Ａｗとがロボット１０の剛性等によって、一致しない状態となり結果的にロボット１０に余分な負荷がかかることになる。このため、上記したような補正演算によって、ロボット１０の従動的に移動する方向を搬出方向Ａｗに補正するのである。
【００４７】
しかし、この補正方向は、上記したように教示点Ｐ１にて教示したアプローチベクトルＡ１に基づくものであるため、アプローチベクトルＡ１と搬出方向Ａｗとの間には、教示誤差が存在し必ずしも一致しない。このため、図１１（ｃ）に示すように教示誤差がある状態で厳密にアプローチベクトルＡ１に基づいて補正を行うと、むしろロボット１０の移動に伴って位置誤差ｄＤが増大し、ワークＷをガイド３７に押し付け、特にガイド３７の出口近傍Ｃでは大きな力が作用し、ワークＷまたはガイド３７を傷める可能性がある。このため、本実施の形態では、ある一定の範囲では補正を行わずガイド３７による案内に従う遊びの状態を設けることによって、ワークＷまたはガイド３７を傷めず、かつある一定以上の誤差が生じた場合には適切な補正が行われるようにしているのである。
【００４８】
次に他の実施の形態について説明する。上記実施の形態では、ロボット１０のアプローチ方向と、搬送装置３１の搬出方向とを平行とすることによって、ロボット１０の従動動作の際に常に教示時のアプローチ方向Ａ１に動作を補正するようにしているが、搬送装置３１の搬出方向をあらかじめ記憶するようにしておき、この方向にロボット１０をの動作を補正するようにしても良い。例えば、上記実施の形態では教示しなかった点Ｐ１’を教示し、教示点Ｐ１から教示点Ｐ１’に向かう方向をベクトルＢｖとして記憶する。このベクトルＢｖの方向にロボット１０を補正すれば良く、上記数３は以下の数５となる。なお、この場合は上記した工具座標系の設定において、アプローチベクトルＡをワークＷの搬出方向Ａｗと平行となるように設定する必要はない。
【００４９】
〔数５〕
Ｐｔ＝Ｐｐ１ ＋ Ｓｉｇｎ・Ｓｐ・Ｂｖ
また、上記実施の形態では、ロボット１０の姿勢を常に教示点Ｐ１の姿勢データＣｐ１に保持している。しかし、搬送装置３１からの外力に応じてロボット１０の姿勢を変化させたほうが都合が良い場合は、ステップ３０２で求めた同時変換行列Ｋｔ’の姿勢データＣｔ’を用いて、ステップ３１０の目標同時変換行列ＫｔをＫｔ＝〔Ｃｔ’，Ｐｔ〕として求めれば良い。このように上記した式は、搬送装置３１とロボット１０の構成に応じて種々の変更が許されることは勿論である。
【００５０】
さらに、以上述べた実施の形態では、図３における重力トルクフィードフォワード４０５の重力トルク値Ｇｆに基づいて電流制限幅を設定したが、重力トルク値Ｇｆに加えてロボット１０の加速度トルクを考慮して電流制限幅を設定しても良い。即ち、加速度トルクフィードフォワード４０６は、下記の式に基づいて設定されているため、この加速度トルクフィードフォワード４０６から得られる値を利用することができる。
【００５１】
（Ｊｍ＋ＪＬ）／Ｋｔ
ただし、ＪＬはロボット１０のイナーシャ値、Ｊｍは各サーボモータのロータのイナーシャ値であり固定値である。また、Ｋｔはトルク定数であり固定値である。
また、加速度トルクの求め方は、特開平６−１９０７５０号等の公知技術を利用することができる。このように重力トルク値Ｇｆに加えて加速度トルクを考慮することによって、ロボット１０の動作によって生じる加速度トルクに対応して電流制限幅を設定できるため、より安定したロボット１０の従動動作を実現できる。
【００５２】
請求項１では、重力トルク値に基づいてサーボモータに供給する電流の制限値が決定されるため、ロボットのその時の姿勢を維持するのに最低必要なトルク（電流値）を基準として制限値を設定することができる。従って、ロボットの従動動作中にロボットの姿勢が変化しても各姿勢に対応した制限値となるため、外力の方向に関係なく柔軟度合いを一定にでき、従動動作を安定したものにでき、ロボットに外力に対する従動動作を行わせる際、ロボットの従動動作があらかじめ定められた方向から一定値以上ずれた場合にのみ移動方向を補正するようにしたため、設定されたロボットの従動方向と外力の方向との間に誤差があったとしても、その誤差を増大させることなく安定した従動動作が可能となる。
【図面の簡単な説明】
【図１】本発明の実施の形態の全体構成を示した図である。
【図２】本発明の実施の形態の電気的構成を示した図である。
【図３】本発明の実施の形態のサーボユニットの処理を示したブロック図である。
【図４】本発明の実施の形態の作用を説明するための図である。
【図５】本発明の実施の形態の工具座標系の設定を説明するための図である。
【図６】本発明の実施の形態の全体的流れを示したフローチャートである。
【図７】本発明の実施の形態の電流制限部の作用を示したフローチャートである。
【図８】本発明の実施の形態の電流制限の様子を示した図である。
【図９】本発明の実施の形態の補正演算を示した図である。
【図１０】本発明の実施の形態の補正演算を示したフローチャートである。
【図１１】本発明の実施の形態の補正演算の作用を説明するための図である。
【図１２】従来の技術を説明するための図である。
【符号の説明】
１０ ロボット
１７ ハンド
３０ ダイキャストマシン
３１ 搬出装置
Ａ アプローチベクトル
Ｗ ワーク
Ａｗ 搬出方向
Claims (1)
- サーボモータによって駆動される複数の軸を備えたロボットを制御するロボット制御装置において、前記ロボットの姿勢に応じた重力トルク値を演算する演算手段と、前記演算手段によって演算された前記重力トルク値に基づいて前記ロボットの姿勢を維持する姿勢維持手段と、前記演算手段によって演算された前記重力トルク値に基づいて前記サーボモータに供給する電流の制限値を補間周期毎に決定する制限値決定手段と、この制限値決定手段によって決定された制限値内に前記サーボモータに供給する電流を制限して前記サーボモータを駆動する電流制限手段と、外部装置から前記ロボットに作用する外力に対して前記電流制限手段により前記サーボモータを駆動すると共に、前記ロボットを前記電流の制限値を越えて作用する外力に対して従動的に動作させる従動動作手段とを備え、前記従動動作手段は、外力に対して前記ロボットを移動させる方向を記憶する移動方向記憶手段と、前記移動方向記憶手段に記憶された移動方向と直交する方向に前記ロボットが移動した誤差量を検出する誤差量検出手段と、この誤差量検出手段によって検出された誤差量があらかじめ設定された設定範囲以内かを判別する判別手段と、この判別手段によって前記誤差量が前記設定範囲より外れる場合、前記移動方向記憶手段に記憶された移動方向に基づいて前記ロボットの従動動作を補正する動作補正手段とを備えたことを特徴とするロボット制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP04947497A JP3588956B2 (ja) | 1997-03-04 | 1997-03-04 | ロボット制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP04947497A JP3588956B2 (ja) | 1997-03-04 | 1997-03-04 | ロボット制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH10244482A JPH10244482A (ja) | 1998-09-14 |
| JP3588956B2 true JP3588956B2 (ja) | 2004-11-17 |
Family
ID=12832159
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP04947497A Expired - Fee Related JP3588956B2 (ja) | 1997-03-04 | 1997-03-04 | ロボット制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3588956B2 (ja) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4281696B2 (ja) | 2005-03-14 | 2009-06-17 | セイコーエプソン株式会社 | 加減速制御方法及び装置並びに加減速制御方法のプログラム |
| JP5443782B2 (ja) * | 2009-02-24 | 2014-03-19 | 川崎重工業株式会社 | ロボット、電流制限装置及び電流制限方法 |
| KR20160117617A (ko) * | 2014-02-21 | 2016-10-10 | 에스케이 테크놀로지스 유쥐 | 가공 동안 단일-스테이션 공작 기계를 로딩 및 언로딩 하기 위한 로봇 셀 |
-
1997
- 1997-03-04 JP JP04947497A patent/JP3588956B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH10244482A (ja) | 1998-09-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20070030271A1 (en) | Robot controller and robot control method using spline interpolation | |
| EP2450157B1 (en) | Robot controlling device | |
| US7102311B2 (en) | Drive control method and drive controller | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| EP0383944B1 (en) | Action teaching method and control apparatus for a robot | |
| JP2002307344A (ja) | ロボット制御装置 | |
| US4967125A (en) | Tool posture control method for a robot | |
| JPH1133960A (ja) | ロボット制御方法 | |
| EP0411139A1 (en) | Positioning correction for robots | |
| WO1990011166A1 (fr) | Procede d'intervention manuelle pour un robot industriel | |
| JPH07319547A (ja) | ロボットの倣い制御方法 | |
| JP3588956B2 (ja) | ロボット制御装置 | |
| EP0271590B1 (en) | Robot control apparatus | |
| JP2786874B2 (ja) | 可動位置制御装置 | |
| JPH03130808A (ja) | ロボットの制御方法およびその制御装置 | |
| JP2723994B2 (ja) | ベンディング装置のゲージング処理方法 | |
| JPH1190868A (ja) | ロボット制御装置 | |
| JPS62251901A (ja) | 多軸ロボツトの経路制御装置 | |
| JPH11198072A (ja) | ロボットの最短時間速度制御装置 | |
| JPH1177572A (ja) | ロボット制御装置 | |
| JPH1177574A (ja) | ロボット制御装置 | |
| JP3402378B2 (ja) | ７軸マニピュレータの制御方法 | |
| JPH11345010A (ja) | ロボットの制御装置 | |
| JP2020104178A (ja) | ロボット装置、ロボット装置の制御方法、ロボット装置を用いた物品の製造方法、制御プログラム及び記録媒体 | |
| JP3174352B2 (ja) | ロボットの加減速制御方法および装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20040427 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20040628 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20040727 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20040809 |
|
| S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313111 |
|
| R371 | Transfer withdrawn |
Free format text: JAPANESE INTERMEDIATE CODE: R371 |
|
| S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313111 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080827 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080827 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090827 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090827 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100827 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110827 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120827 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130827 Year of fee payment: 9 |
|
| LAPS | Cancellation because of no payment of annual fees |