JPH10244482A

JPH10244482A - ロボット制御装置

Info

Publication number: JPH10244482A
Application number: JP4947497A
Authority: JP
Inventors: Katsuhiro Komuro; 克弘小室; Yoshiji Yamamoto; 吉二山本
Original assignee: Toyoda Koki KK
Current assignee: Toyoda Koki KK
Priority date: 1997-03-04
Filing date: 1997-03-04
Publication date: 1998-09-14
Anticipated expiration: 2017-03-04
Also published as: JP3588956B2

Abstract

(57)【要約】【課題】ロボットの外力に対する柔軟度合いを一定と
し、安定した従動動作が可能なロボット制御装置を提供
することを目的とする。【解決手段】重力トルク値に基づいてサーボモータに供
給する電流の制限値が決定されるため、ロボットのその
時の姿勢を維持するのに最低必要なトルク（電流値）を
基準として制限値を設定することができる。従って、ロ
ボットの従動動作中にロボットの姿勢が変化しても各姿
勢に対応した制限値となるため、外力の方向に関係なく
柔軟度合いを一定にでき、従動動作を安定したものにで
きる。また、ロボットの姿勢を維持するのに必要なトル
クは常に確保されるため、電流制限を作用させた時に、
トルクが不足してロボットの姿勢が変化することはな
い。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、外力に対してロボ
ットが従動的に動作するロボットの制御装置に関するも
のである。

【０００２】

【従来の技術】従来、ロボットが作業する際にワークま
たは加工機側からロボットに対して外力が作用する場合
がある。このような場合、ロボットをその外力に追従す
るように同期制御を行うか、ハンド部に弾性部材等を有
するフローティングブラケットを取り付けて、機械的に
外力を逃がす手段が行われていた。

【０００３】しかし、上記した同期制御の場合には制御
プログラムが複雑になり、フローティングブラケットの
場合には、コスト高になるという欠点がある。このため
特開平２−１４５２７８号に開示されているようなロボ
ットの各軸を駆動するサーボモータに供給する電流を制
限する方法が行われている。この技術は、サーボモータ
に供給する最大電流を通常よりも低く制限することによ
って、ロボットにその最大電流よりも大きな電流を必要
とする負荷が作用すると、ロボットはその負荷に負け柔
軟な状態になることを利用して、ロボットを外力に対し
て従動させるようにしたものである。この電流制限を用
いることによって、簡単な方法で外力に対するロボット
の従動動作を実現することができる。

【０００４】

【発明が解決しようとする課題】上述したような従来の
電流制限は、図１２に示すようにサーボモータのトルク
（電流値）０を基準としてプラス、マイナス両方向にα
％という形式で設定されている。しかし、ロボットのあ
る動作に対して電流制限を行う場合、各サーボモータレ
ベルで見た場合の電流値の変化は、必ずしもトルク０を
中心として変化するわけではない。このため、例えばロ
ボットの特定の動作における外力が作用しない状態での
あるサーボモータの電流指令値の変化が図１２の曲線Ｌ
１で示すように不規則に変動するものである場合、電流
指令値に対する電流制限幅は、プラス方向にｄ１、マイ
ナス方向にｄ２となり、異なったものとなる。これは方
向によって、ロボットが外力に耐え得る力が異なること
になり、柔軟度合いが一定にならないという問題があ
る。

【０００５】また、曲線Ｌ１の二点鎖線部分で示すよう
にロボットの姿勢を維持するのに必要なトルクが、既に
電流制限幅を越えている状態で電流制限を作用させた場
合、トルクの不足分ｄ３だけロボットの姿勢が外力に関
係なく重力に負けて変化してしまうという問題があっ
た。本発明は以上述べた問題を解決するためになされた
ものであり、ロボットの外力に対する柔軟度合いを一定
とし、安定した従動動作が可能なロボット制御装置を提
供することを目的とする。

【０００６】

【課題を解決するための手段】上記した目的を達成する
ための請求項１に記載された手段は、サーボモータによ
って駆動される複数の軸を備えたロボットを制御するロ
ボット制御装置において、前記ロボットの姿勢に応じた
重力トルク値を演算する演算手段と、この演算手段によ
って演算された前記重力トルク値に基づいて前記サーボ
モータに供給する電流の制限値を決定する制限値決定手
段と、この制限値決定手段によって決定された制限値内
に前記サーボモータに供給する電流を制限して前記サー
ボモータを駆動する電流制限手段とを備えたものであ
る。

【０００７】請求項２に記載された手段は、請求項１に
記載のロボット制御装置おいて、前記演算手段は、前記
重力トルク値と共に前記ロボットの加速度トルク値を演
算し、前記制限値決定手段は、前記重力トルク値と前記
加速度トルク値に基づいて前記サーボモータに供給する
電流の制限値を決定し、前記電流制限手段は、前記重力
トルク値と前記加速度トルク値に基づいた制限値内に前
記サーボモータに供給する電流を制限するものである。

【０００８】請求項３に記載された手段は、外部装置か
らロボットに作用する外力に対して前記ロボットを従動
的に動作させる従動動作手段を備えたロボット制御装置
において、外力に対して前記ロボットを移動させる方向
を記憶する移動方向記憶手段と、前記従動動作手段によ
って前記ロボットが従動動作を行う間、前記移動方向記
憶手段に記憶された移動方向と直交する方向に前記ロボ
ットが移動した誤差量を検出する誤差量検出手段と、こ
の誤差量検出手段によって検出された誤差量があらかじ
め設定された設定範囲以内かを判別する判別手段と、こ
の判別手段によって前記誤差量が前記設定範囲より外れ
る場合、前記移動方向記憶手段に記憶された移動方向に
基づいて前記ロボットの従動動作を補正する動作補正手
段とを備えたものである。（作用）請求項１において、演算手段はロボットの姿勢
に応じた重力トルク値を演算し、制限値決定手段はこの
重力トルク値に基づいてサーボモータに供給する電流の
制限値を決定する。そして、電流制限手段は、この制限
値内にサーボモータに供給する電流を制限する。このた
め制限値以上の電流が必要な外力がロボットに作用した
時には、ロボットは外力に対して柔軟に従動する状態と
なる。

【０００９】請求項２では、請求項１の重力トルクに加
えて加速度トルク値も考慮して電流の制限値が決定され
る。請求項３において、従動動作手段によって、ロボッ
トが外力に対して従動的に動作している間、誤差量検出
手段は移動方向記憶手段に記憶された移動方向と直交す
る方向にロボットが移動した誤差量を検出し、この誤差
量があらかじめ設定された設定範囲から外れることが判
別手段によって判別された場合には、動作補正手段によ
り移動方向記憶手段に記憶された移動方向に基づいてロ
ボットの従動動作が補正される。従って、ロボットの従
動動作があらかじめ定められた方向から一定値以上ずれ
た場合のみ移動方向が補正される。

【００１０】

【発明の実施の形態】本発明の実施の形態について図面
を参照して説明する。図１は本実施の形態の全体構成を
示した図である。図１において１０は６軸垂直多関節ロ
ボット、３０は搬出装置３１（外部装置）を備えたダイ
キャストマシン、４０はダイキャストマシン３０から搬
出したワークＷを載置する載置台である。

【００１１】ロボット１０は、固定ベース２７上にロー
テータ１１（１軸）が鉛直な軸線を中心に水平面内で回
転自在に支持され、ローテータ１１上には、第１アーム
１２（２軸）が水平な軸線を中心に揺動自在に支持され
ている。この第１アーム１２には第２アーム１３（３
軸）が水平な軸線を中心に揺動自在に支持されている。
また、ローテータ１１上には、水平な軸線を中心に揺動
自在に第１リンク１８が支持されており、この第１リン
ク１８と第２アーム１３の末端が第２リンク１９で連結
されいる。従って、第２アーム１３は、第１リンク１８
が駆動されることによって揺動するようになっている。

【００１２】第２アーム１３の先端部には、４軸、５
軸、６軸を備える手首部１４が回動可能に支持されてい
る。手首部１４の先端のフランジには、一対の把持爪１
７ａ（図５参照）を有するハンド１７が取り付けられて
いる。ダイキャストマシン３０は、鋳造されたワークＷ
をガイド３７に沿って搬出する搬出装置３１を備えてい
る。搬出装置３１は、図略のシリンダによって駆動され
るものであり、ワークＷの搬出を開始する位置（後退
端）にあることを検出するリミットスイッチ３５と、搬
出装置３１がワークＷの搬出を完了する位置（前進端）
にあることを検出するリミットスイッチ３６を備えてい
る。

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

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

【００１５】また、中央処理装置８０は、ダイキャスト
マシン３０の制御装置３７からの出力信号を入力するよ
うになっている。この出力信号には、搬出装置３１に設
けられたリミットスイッチ３５，３６の出力信号等があ
る。上記サーボユニット９１〜９６は、図３に示す回路
の機能をディジタル制御によって達成するものである。
即ち、サーボユニット９１〜９６は、位置ループ４０
１、速度ループ４０２を備えると共に、重力トルクフィ
ードフォワード４０５、加速度トルクフィードフォワー
ド４０６、速度フィードフォワード４０７を備えてい
る。重力トルクフィードフォワード４０５および加速度
トルクフィードフォワード４０６は、速度ループ４０２
後の指令値に入力され、速度フィードフォワード４０７
は、位置ループ４０１後の指令値に入力されるようにな
っている。

【００１６】また、速度ループ４０２に重力トルクフィ
ードフォワード４０５および加速度トルクフィードフォ
ワード４０６が入力された後には、電流制限部４０３が
設けられており、この電流制限部４０３で定められた電
流指令値は、アンプ部４０４に出力されるようになって
いる。後述するように電流制限部４０３は、重力トルク
フィードフォワード４０５の重力トルク値Ｇｆに基づい
て電流制限値を決定するようになっている。

【００１７】以上の構成に基づいて本実施の形態の作用
を説明する。図４に示すように本実施の形態では、ロボ
ット１０は初期位置Ｐ０（図略）から移動を開始し、Ｐ
１の位置においてダイキャストマシン３０から搬出され
たワークＷを把持する。そして搬出装置３１の搬出動作
に従動しながら搬出装置３１が搬出を完了する搬出完了
点Ｐ１’までワークＷを把持し続ける。この従動動作に
おいて本実施の形態の特徴部分である後述する電流制限
が実行される。また、従動動作をより安定的なものとす
るため、従動動作時にロボットの動作軌跡を補正する補
正演算が行われる。

【００１８】ロボット１０は、搬出が完了したＰ１’の
位置から載置台４０上の位置Ｐ２にワークＷを移載した
後、初期位置Ｐ０に復帰して１サイクルを終了する。以
上の動作において、上記Ｐ０，Ｐ１，Ｐ２の位置におけ
るロボット１０の位置および姿勢が教示点データとして
メモリ８１に記憶されている。この教示点データは作業
者があらかじめティーチングボックス７０により教示す
るものである。なお、本実施の形態では、ティーチング
をより容易とするために、位置Ｐ１’は教示点とせず、
単にロボット１０が搬出装置３１の動作に従動して移動
する位置となっているが、教示点として記憶しておくこ
とも可能である。

【００１９】また、作業者は、ハンド１７の位置および
姿勢の基準となる工具座標系をあらかじめ設定しておく
必要ある。本実施の形態の場合、工具座標系は図５に示
すようにハンド１７が有する２つの把持爪１７ａの中間
に座標原点ＰＴが設定され、この座標原点ＰＴに対して
３つの姿勢ベクトルが設定されている。即ち、ハンド１
７がワークＷに接近する方向を示すアプローチベクトル
Ａは、ハンド１７がワークＷに取り付けられている突設
部Ｗａを把持した時、搬出装置３１がワークＷを搬出す
る方向Ａｗと平行な方向となるように設定されている。
ただし、図５の場合、アプローチベクトルＡとワークＷ
の搬出方向Ａｗとは逆向きとなる。また、一方の把持爪
１７ａから他方の把持爪１７ａに向かう方向にオリエン
トベクトルＯ、アプローチベクトルＡとオリエントベク
トルＯの両者に直交し、右手系をなす方向にノーマルベ
クトルＮが設定されている。

【００２０】次に図６に示すフローチャートにより本実
施の形態の全体の流れを説明する。動作開始時のステッ
プ１００おいてロボット１０は、初期位置Ｐ０に位置し
ている。ステップ１０２においてダイキャストマシン３
０の加工が終了し、搬出装置３１のリミットスイッチ３
５がＯＮとなり、ワークＷが搬出開始位置に設置された
ことが検出されると、ステップ１０４に移行し、ロボッ
ト１０は教示点Ｐ１に移動して、ワークＷの突設部Ｗａ
を把持する。

【００２１】ロボット１０に設置された図略のリミット
スイッチがＯＮとなり、ワークＷの把持が完了すると、
ステップ１０６においてロボット１０は、従動動作モー
ドとなる。従動動作モードが開始されると、ロボット１
０のモータＭ１〜Ｍ６に対する最大電流の制限幅が後述
する通常の制限幅より低減された電流制限状態となる。
この電流制限については後に図７，８にて詳述する。こ
の電流制限によって、ロボット１０は搬出装置３１から
の外力によって従動的に変位可能となる。なお、本実施
の形態では６軸全てに対して電流制限を行っているが、
従動動作に関与しない軸がある場合は必ずしも全ての軸
に電流制限を行う必要はない。

【００２２】ロボット１０がワークＷを把持したことに
対応して、搬出装置３１がワークＷの搬出動作を開始す
ると、ステップ１０８では、従動動作モードとなったロ
ボット１０が搬出装置３１の搬出方向に沿って滑らかに
移動するように補正演算を行う。この補正演算について
は後に図９，１０によって詳述する。ステップ１１０で
は、搬出装置３１のリミットスイッチ３６がＯＮとな
り、ワークＷの搬出動作が完了すると、従動動作モード
から通常の動作を実行する通常モードに変更する。即
ち、モータＭ１〜Ｍ６に対する電流制限状態を解除し、
電流の制限幅を通常の状態とする。この後、ステップ１
１２に移行し、ロボット１０は現在位置Ｐ１’から載置
台４０上の教示点Ｐ２までワークＷを移載する。これに
より一連のワークＷの取り出し作業は終了し、ステップ
１１４に移行して、ロボット１０は初期位置Ｐ０に戻
り、動作を終了する（ステップ１１６）。

【００２３】次に上記した電流制限について説明する。
本実施の形態は、従来サーボモータのトルク０を基準に
設定されていた電流制限幅を、サーボユニット９１〜９
６の制御で使用するために中央処理装置８０が逐次演算
する重力トルク値Ｇｆを利用して、この重力トルク値を
基準に電流制限幅を設定するものである。まず重力トル
ク値Ｇｆの求め方の概要について説明する。

【００２４】各軸の重力トルク値Ｇｆ１〜Ｇｆ６は、ロ
ボット１０をモデル化することによって以下のように求
められる。なお、１軸の重力トルク値Ｇｆ１は０となる
理由は、ロボット１０を床面に設置した場合、１軸は床
面に対して垂直な軸となり重力トルクが作用しないため
である。Ｇｆ１＝０Ｇｆ２＝ｇ１・ｍ1 ＋ｇ４・ｍ４＋Ｒ１（ｍ２＋ｍ３＋
ｍ６）＋ｇ６・ｍ６Ｇｆ３＝ｇ２・ｍ２＋ｇ３・ｍ３−ｇ５・ｍ５＋ｇ６・
ｍ６Ｇｆ４＝−ｇ３・ｍ３−ｇ６・ｍ６Ｇｆ５＝ｇ３・ｍ３＋ｇ６・ｍ６Ｇｆ６＝ｇ６・ｍ６ただし、ｇ１：１軸から第１アーム１２の重心までの水平距離ｇ２：２軸から第２アーム１３の重心までの水平距離ｇ３：手首部軸心（４軸、５軸）から手首部１４重心ま
での水平距離ｇ４：第２リンク１９軸心より第２リンク１９の重心ま
での水平距離ｇ５：第１リンク１８軸心より第１リンク１８の重心ま
での水平距離ｇ６：６軸よりハンド１７の重心までの水平距離Ｒ１：第１アーム１２の長さｍ１：第１アーム１２の質量ｍ２：第２アーム１３の質量ｍ３：手首部１４の質量ｍ４：第２リンク１９の質量ｍ５：第１リンク１８の質量ｍ６：ハンド１７の質量なお、ｇ１からｇ６はロボット１０の姿勢に応じて変化
する変数である。

【００２５】次に図７により補間周期毎に各サーボユニ
ット９１〜９６にて実行される各軸における電流制限部
４０３の作用を説明する。ステップ２００で電流制限部
４０３が実行される。この時、電流制限部４０３には、
上記式に基づいて中央処理装置８０において演算された
ロボット１０の現在の姿勢に対応した重力トルク値Ｇｆ
と、重力トルクフィードフォワード４０５および加速度
フィードフォワード４０６が入力された速度ループ４０
２からの電流指令値Ｉｏが入力されている。なお、重力
トルク値Ｇｆは電流指令値に換算された値となってい
る。

【００２６】ステップ２０２では、現在が従動動作モー
ドであるか通常モードであるかを判別する。上記したス
テップ１０６で従動動作モードに設定されている場合は
（ＹＥＳ）、ステップ２０４に移行して重力トルク値Ｇ
ｆを基準とした電流制限を行う。また、通常モードに設
定されている場合は（ＮＯ）、従来のサーボモータのト
ルク０を基準とした通常の電流制限を行うため、ステッ
プ２１６に移行する。

【００２７】従動動作モードでは、ステップ２０４にて
電流指令値Ｉｏが電流制限の上限値を越えていないかを
判断する。この上限値は、重力トルク値Ｇｆを基準とし
て設定されてＧｆ＋ｄとなり、制限幅ｄは後述する通常
モードの制限幅Ｉｍａｘ，Ｉｍｉｎより小さい値に設定
されている。このステップ２０４において、電流指令値
Ｉｏが電流制限の上限値Ｇｆ＋ｄを越えていないならば
（ＹＥＳ）、ステップ２０８に移行する。また、電流指
令値Ｉｏが電流制限の上限値Ｇｆ＋ｄを越えているなら
ば（ＮＯ）、ステップ２０６に移行して、電流指令値Ｉ
ｏを上限値Ｇｆ＋ｄとし、ステップ２１２に移行する。

【００２８】ステップ２０８では、電流指令値Ｉｏが電
流制限の下限値を下回っていないかを判断する。電流制
限の下限値は、上限値と同様に重力トルク値Ｇｆを基準
として設定され、Ｇｆ−ｄとなる。電流指令値Ｉｏが下
限値Ｇｆ−ｄを下回っていないならば（ＹＥＳ）、ステ
ップ２１２に移行する。また、電流指令値Ｉｏが電流制
限の上限値Ｇｆ−ｄｉを越えているならば（ＮＯ）、ス
テップ２１０に移行して、電流指令値Ｉｏを下限値Ｇｆ
−ｄｉとし、ステップ２１２に移行する。

【００２９】ステップ２１２では、ステップ２０６から
２１０で決定した電流指令値Ｉｏをアンプ部４０４に出
力し、電流制限部の作用を終了する（ステップ２１
４）。一方、ステップ２１６以降の通常モードの場合
は、上記したように電流制限幅がトルク０を基準とした
上限値Ｉｍａｘと下限値Ｉｍｉｎに設定される点が異な
るのみで基本的を流れは同じであるため説明を省略す
る。

【００３０】以上の過程によって、電流指令値Ｉｏは、
ロボットの姿勢に対応した重力トルク値Ｇｆを基準とし
て電流が制限され、この制限幅を越える電流値を必要と
する外力がロボット１０に作用した場合には、ロボット
１０は柔軟な状態となる。図８は、あるサーボユニット
におけるロボット１０が教示点Ｐ１から搬出完了点Ｐ
１’まで従動動作する時の電流制限の様子を示したもの
である。曲線Ｌ２は、ロボット１０のモデルより中央処
理装置８０が演算した重力トルク値Ｇｆの変化を示した
ものであり、曲線Ｌ２−１，Ｌ２−２は、曲線Ｌ２の重
力トルク値Ｇｆに基づいて設定された制限幅の上限値お
よび下限値の変化を示したものである。また、曲線Ｌ３
は実際にロボット１０を教示点Ｐ１から搬出完了点Ｐ
１’まで外力を作用させずに移動させた場合に出力され
た電流指令値の変化を示したものである。これから判る
ように、制限幅を実際のロボット１０の電流変化にほぼ
対応して設定することが可能となる。

【００３１】次に図９および図１０のフローチャートに
基づいて上記したステップ１０８における補正演算につ
いて説明する。この補正演算は、ロボット１０が搬出装
置３１からの外力によって従動的に変位する際、ハンド
１７の姿勢を一定に保ちつつ、搬出装置３１の動きに滑
らかに追従して、ロボット１０に余分な負荷を生じさせ
ないための制御を行うものである。以下の説明では、ロ
ボット１０の移動における補間時ｔでの同時変換行列を
Ｋｔ、この同時変換行列Ｋｔにおける姿勢データをＣ
ｔ、位置データＰｔとする。従って、同時変換行列Ｋｔ
は数１で表される。

【００３２】

【数１】

【００３３】なお、上記数１において、（Ｎｔｘ，Ｎｔ
ｙ，Ｎｔｚ），（Ｏｔｘ，Ｏｔｙ，Ｏｔｚ），（Ａｔ
ｘ，Ａｔｙ，Ａｔｚ），（Ｐｔｘ，Ｐｔｙ，Ｐｔｚ）
は、ロボット座標系から見た補間時ｔにおけるノーマル
ベクトルＮｔ、オリエントベクトルＯｔ、アプローチベ
クトルＡｔ、位置ベクトルＰｔの各成分である。上記し
たステップ１０８において補正演算プログラムが起動さ
れると（ステップ３００）、ステップ３０２では、補間
時ｔにおけるエンコーダＥ１〜Ｅ６の出力αｔ１〜αｔ
６を入力して、順変換し、同時変換行列Ｋｔ’を求め
る。

【００３４】ステップ３０４では、現在従動動作中のロ
ボット１０が所定の範囲以上に搬出装置３１の搬出方向
Ａｗから誤差を生じていないかを判断する。この誤差ｈ
は次のように求める。図９に示すように、まず教示点Ｐ
１から教示点Ｐ１におけるアプローチ方向Ａ１方向に点
Ｂを設定する。この教示点Ｐ１から点Ｂまでの距離は、
教示点Ｐ１から搬出完了位置Ｐ１’までの距離より十分
長い値にあらかじめ設定されたものである。このように
点Ｂを設定することによって、教示点Ｐ１と点Ｂを通る
直線Ｓが求まる。誤差ｈはロボット１０の現在位置であ
る同時変換行列Ｋｔ’の位置ベクトルＰｔ’から直線Ｓ
までの直線距離であるから以下の数２の関係より位置ベ
クトルＰｔ’から直線Ｓへ垂直に下ろした点Ｈの位置が
求まる。

【００３５】

【数２】

【００３６】この数２によって求めた点Ｈの位置と位置
ベクトルＰｔ’の距離を求めることにより誤差ｈを求め
ることができる。従って、誤差ｈがあらかじめ設定され
た範囲ｒ内にある場合、即ち、図９のように直線Ｓを中
心とする許容半径ｒの円柱内に位置ベクトルＰｔ’が存
在する場合は（ＹＥＳ）、ロボット１０の補正演算を行
う必要がないため、ステップ３０６にて目標同時変換行
列Ｋｔ＝Ｋｔ’としてステップ３１２に移行する。

【００３７】また、誤差ｈがあらかじめ設定された範囲
内にない場合は（ＮＯ）、ロボット１０の追従動作の軌
跡が許容範囲以上に誤差を生じていることを示している
ため、ステップ３０８に移行して補正演算を行う。な
お、このように補正演算を行う前にステップ３０４にて
補正を必要とするか否かを判断する理由は後に詳述す
る。

【００３８】ステップ３０８では、同時変換行列Ｋｔ’
の位置ベクトルＰｔ’を用いて目標位置データＰｔを求
める。即ち、目標位置ベクトルＰｔは以下の数３で表さ
れる。

【００３９】

【数３】

【００４０】数３において、Ｐｐ１は教示点Ｐ１におけ
る位置ベクトルであり、Ｓｉｇｎは、現在の位置ベクト
ルＰｔ’が教示点Ｐ１におけるアプローチベクトルＡ１
に対して正逆いずれの方向かを示すものであり、アプロ
ーチベクトルＡ１に対してロボット１０が移動している
方向を示している。また、Ｓｐは、ロボット１０が教示
点Ｐ１からｔまでの間に移動した距離であり以下の数４
によって求まる。

【００４１】

【数４】

【００４２】ステップ３１０では、ステップ３０８によ
って求めた目標位置ベクトルＰｔと、教示点Ｐ１の姿勢
ベクトルＣｐ１より、目標同時変換行列Ｋｔ＝〔Ｃｐ
１，Ｐｔ〕を求め、ステップ３１２にてこれを逆変換
し、指令回転角信号θｔ１〜θｔ６を求めてサーボユニ
ット９１〜９６に出力する。以上のステップで従動動作
モードの１サイクルを終了する。

【００４３】ステップ３１４では、ロボット１０がＰ
１’の位置まで移動し、搬出装置３１によるワークＷの
搬出が完了したか否かを判別する。この判別はワークＷ
の搬出完了を示す搬出装置３１のリミットスイッチ３６
がＯＮしたか否かを判別する。ワークＷの搬出が完了し
たと判別された場合（ＹＥＳ）、ステップ３２０に移行
して補正演算を終了し、図６におけるステップ１１０に
移行する。また、ワークＷの搬出が完了していないと判
別された場合（ＮＯ）、ステップ３１６に移行する。

【００４４】ステップ３１６は、従動動作モードが開始
されて所定の時間が経過したかを判別する。これは、搬
出装置３１のリミットスイッチ３６に異常が生じた等の
理由によりリミットスイッチ３６がＯＮしない場合が考
えられるためである。従って、従動動作モードが開始さ
れて所定の時間が経過した場合には（ＹＥＳ）、ステッ
プ３１８に移行して警告を発する等の異常処理を行う。
また、所定の時間が経過していない場合には（ＮＯ）、
現在まだ正常な状態でワークＷの搬出が行われているた
め、ステップ３０２に戻り、補正演算を繰り返す。

【００４５】以上述べたように本実施の形態では、搬出
装置３１からワークＷが搬出される動作に従動的にロボ
ット１０が移動する際、ワークＷが搬出される方向に常
にロボット１０の位置を補正すると共に、姿勢はワーク
Ｗを把持した教示点Ｐ１の姿勢を保持するようにしたた
め、ロボット１０が搬出装置３１からの外力によって従
動的に移動しても余分な力が作用することはなく、滑ら
かな動作が実現でき、ロボット１０の耐久性が向上す
る。

【００４６】次に上記ステップ３０４における補正演算
を行うか否かを判断する理由について説明する。図１１
（ａ）の模式図に示すように搬出装置３１から搬出され
るワークＷは、搬出が開始されてしばらくの間は、ガイ
ド３７によって支持されており、この状態では、仮に上
記補正演算を行わなくとも、ガイド３７の存在によって
ロボット１０が従動動作によって移動する方向と搬出装
置３１の搬出方向Ａｗとはほぼ一致した状態となる。し
かし、図１１（ｂ）のように搬出の最終段階でワークＷ
がガイド３７から外れると、ワークＷは自由な状態とな
り、ロボット１０の従動的に移動する方向と搬出装置３
１の搬出方向Ａｗとがロボット１０の剛性等によって、
一致しない状態となり結果的にロボット１０に余分な負
荷がかかることになる。このため、上記したような補正
演算によって、ロボット１０の従動的に移動する方向を
搬出方向Ａｗに補正するのである。

【００４７】しかし、この補正方向は、上記したように
教示点Ｐ１にて教示したアプローチベクトルＡ１に基づ
くものであるため、アプローチベクトルＡ１と搬出方向
Ａｗとの間には、教示誤差が存在し必ずしも一致しな
い。このため、図１１（ｃ）に示すように教示誤差があ
る状態で厳密にアプローチベクトルＡ１に基づいて補正
を行うと、むしろロボット１０の移動に伴って位置誤差
ｄＤが増大し、ワークＷをガイド３７に押し付け、特に
ガイド３７の出口近傍Ｃでは大きな力が作用し、ワーク
Ｗまたはガイド３７を傷める可能性がある。このため、
本実施の形態では、ある一定の範囲では補正を行わずガ
イド３７による案内に従う遊びの状態を設けることによ
って、ワークＷまたはガイド３７を傷めず、かつある一
定以上の誤差が生じた場合には適切な補正が行われるよ
うにしているのである。

【００４８】次に他の実施の形態について説明する。上
記実施の形態では、ロボット１０のアプローチ方向と、
搬送装置３１の搬出方向とを平行とすることによって、
ロボット１０の従動動作の際に常に教示時のアプローチ
方向Ａ１に動作を補正するようにしているが、搬送装置
３１の搬出方向をあらかじめ記憶するようにしておき、
この方向にロボット１０をの動作を補正するようにして
も良い。例えば、上記実施の形態では教示しなかった点
Ｐ１’を教示し、教示点Ｐ１から教示点Ｐ１’に向かう
方向をベクトルＢｖとして記憶する。このベクトルＢｖ
の方向にロボット１０を補正すれば良く、上記数３は以
下の数５となる。なお、この場合は上記した工具座標系
の設定において、アプローチベクトルＡをワークＷの搬
出方向Ａｗと平行となるように設定する必要はない。

【００４９】〔数５〕Ｐｔ＝Ｐｐ１＋Ｓｉｇｎ・Ｓｐ・Ｂｖまた、上記実施の形態では、ロボット１０の姿勢を常に
教示点Ｐ１の姿勢データＣｐ１に保持している。しか
し、搬送装置３１からの外力に応じてロボット１０の姿
勢を変化させたほうが都合が良い場合は、ステップ３０
２で求めた同時変換行列Ｋｔ’の姿勢データＣｔ’を用
いて、ステップ３１０の目標同時変換行列ＫｔをＫｔ＝
〔Ｃｔ’，Ｐｔ〕として求めれば良い。このように上記
した式は、搬送装置３１とロボット１０の構成に応じて
種々の変更が許されることは勿論である。

【００５０】さらに、以上述べた実施の形態では、図３
における重力トルクフィードフォワード４０５の重力ト
ルク値Ｇｆに基づいて電流制限幅を設定したが、重力ト
ルク値Ｇｆに加えてロボット１０の加速度トルクを考慮
して電流制限幅を設定しても良い。即ち、加速度トルク
フィードフォワード４０６は、下記の式に基づいて設定
されているため、この加速度トルクフィードフォワード
４０６から得られる値を利用することができる。

【００５１】（Ｊｍ＋ＪＬ）／Ｋｔただし、ＪＬはロボット１０のイナーシャ値、Ｊｍは各
サーボモータのロータのイナーシャ値であり固定値であ
る。また、Ｋｔはトルク定数であり固定値である。ま
た、加速度トルクの求め方は、特開平６−１９０７５０
号等の公知技術を利用することができる。このように重
力トルク値Ｇｆに加えて加速度トルクを考慮することに
よって、ロボット１０の動作によって生じる加速度トル
クに対応して電流制限幅を設定できるため、より安定し
たロボット１０の従動動作を実現できる。

【００５２】

【発明の効果】請求項１では、重力トルク値に基づいて
サーボモータに供給する電流の制限値が決定されるた
め、ロボットのその時の姿勢を維持するのに最低必要な
トルク（電流値）を基準として制限値を設定することが
できる。従って、ロボットの従動動作中にロボットの姿
勢が変化しても各姿勢に対応した制限値となるため、外
力の方向に関係なく柔軟度合いを一定にでき、従動動作
を安定したものにできる。

【００５３】また、ロボットの姿勢を維持するのに必要
なトルクは常に確保されるため、電流制限を作用させた
時に、トルクが不足してロボットの姿勢が変化すること
はない。請求項２では、請求項１の重力トルク値に加速
度トルク値を加えて電流の制限値を決定するため、姿勢
変化に応じて生じる加速度の影響が考慮され、より安定
した従動動作が可能となる。

【００５４】請求項３では、ロボットに外力に対する従
動動作を行わせる際、ロボットの従動動作があらかじめ
定められた方向から一定値以上ずれた場合にのみ移動方
向を補正するようにしたため、設定されたロボットの従
動方向と外力の方向との間に誤差があったとしても、そ
の誤差を増大させることなく安定した従動動作が可能と
なる。

【００５５】なお、この請求項３による従動動作の補正
は、請求項１，２のような電流制限によってロボットを
柔軟な状態にする場合だけでなく、サーボモータに対す
る位置ループゲインや速度ループゲイン等を通常よりも
低減させてロボットを柔軟な状態にする場合にも用いる
ことかできる。

【図面の簡単な説明】

【図１】本発明の実施の形態の全体構成を示した図であ
る。

【図２】本発明の実施の形態の電気的構成を示した図で
ある。

【図３】本発明の実施の形態のサーボユニットの処理を
示したブロック図である。

【図４】本発明の実施の形態の作用を説明するための図
である。

【図５】本発明の実施の形態の工具座標系の設定を説明
するための図である。

【図６】本発明の実施の形態の全体的流れを示したフロ
ーチャートである。

【図７】本発明の実施の形態の電流制限部の作用を示し
たフローチャートである。

【図８】本発明の実施の形態の電流制限の様子を示した
図である。

【図９】本発明の実施の形態の補正演算を示した図であ
る。

【図１０】本発明の実施の形態の補正演算を示したフロ
ーチャートである。

【図１１】本発明の実施の形態の補正演算の作用を説明
するための図である。

【図１２】従来の技術を説明するための図である。

【符号の説明】

１０ロボット１７ハンド３０ダイキャストマシン３１搬出装置ＡアプローチベクトルＷワークＡｗ搬出方向

Claims

【特許請求の範囲】

【請求項１】サーボモータによって駆動される複数の
軸を備えたロボットを制御するロボット制御装置におい
て、前記ロボットの姿勢に応じた重力トルク値を演算する演
算手段と、この演算手段によって演算された前記重力トルク値に基
づいて前記サーボモータに供給する電流の制限値を決定
する制限値決定手段と、この制限値決定手段によって決定された制限値内に前記
サーボモータに供給する電流を制限して前記サーボモー
タを駆動する電流制限手段とを備えたことを特徴とする
ロボット制御装置。
【請求項２】前記演算手段は、前記重力トルク値と共
に前記ロボットの加速度トルク値を演算し、前記制限値
決定手段は、前記重力トルク値と前記加速度トルク値に
基づいて前記サーボモータに供給する電流の制限値を決
定し、前記電流制限手段は、前記重力トルク値と前記加
速度トルク値に基づいた制限値内に前記サーボモータに
供給する電流を制限することを特徴とする請求項１に記
載のロボット制御装置。
【請求項３】外部装置からロボットに作用する外力に
対して前記ロボットを従動的に動作させる従動動作手段
を備えたロボット制御装置において、外力に対して前記ロボットを移動させる方向を記憶する
移動方向記憶手段と、前記従動動作手段によって前記ロボットが従動動作を行
う間、前記移動方向記憶手段に記憶された移動方向と直
交する方向に前記ロボットが移動した誤差量を検出する
誤差量検出手段と、この誤差量検出手段によって検出された誤差量があらか
じめ設定された設定範囲以内かを判別する判別手段と、この判別手段によって前記誤差量が前記設定範囲より外
れる場合、前記移動方向記憶手段に記憶された移動方向
に基づいて前記ロボットの従動動作を補正する動作補正
手段とを備えたことを特徴とするロボット制御装置。