JP2016093863A

JP2016093863A - ロボット制御装置

Info

Publication number: JP2016093863A
Application number: JP2014231170A
Authority: JP
Inventors: 健司長松; Kenji Nagamatsu
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2014-11-14
Filing date: 2014-11-14
Publication date: 2016-05-26
Anticipated expiration: 2034-11-14
Also published as: JP6539989B2

Abstract

【課題】第１ロボットの動作態様にかかわらず、第２ロボットの動作を追従させることのできるロボット制御装置を提供する。【解決手段】第１ロボット４０Ａ及び第２ロボット４０Ｂを制御するロボット制御装置２０であって、動作周期毎に、第１ロボット４０Ａの動作目標である第１目標を算出し、第１目標に対して、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係を維持するように、第２ロボット４０Ｂの動作目標である第２目標を算出し、第２目標へ第２ロボット４０Ｂが動作する速度が上限速度を超えているか否か判定し、第２ロボット４０Ｂの速度が前記上限速度を超えていると判定された場合に、第１ロボット４０Ａの現在の位置及び姿勢に第１目標を近付けるように第１目標を修正する。【選択図】図１

Description

本発明は、複数のロボットを制御するロボット制御装置に関する。

従来、複数のロボットにより１つのワークを把持して搬送する協調動作を行うものがある（例えば、特許文献１参照）。特許文献１に記載のものは、マスタロボット（第１ロボット）のアームの通過点と、搬送中のワークの状態に対応する両アームの相対的な位置及び姿勢関係とに基づいて、スレーブロボット（第２ロボット）のアームの通過点を決定している。

特許第３５７７０２８号公報

しかしながら、マスタロボットのアームの動作態様によっては、マスタロボットのアームの動作量とスレーブロボットのアームの動作量とが大きく異なる点に、本願発明者は着目した。そして、両アームの相対的な位置及び姿勢関係を維持して、スレーブロボットのアームを動作させようとしても、スレーブロボットのアームが追従できない場合があることを、本願発明者は見出した。

本発明は、こうした実情に鑑みてなされたものであり、第１ロボットの動作態様にかかわらず、第２ロボットの動作を追従させることのできるロボット制御装置を提供することを主たる目的とするものである。

第１の手段は、第１ロボット及び第２ロボットを制御するロボット制御装置であって、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する第１目標算出手段と、前記第１目標に対して、前記第１ロボット及び前記第２ロボットの相対的な位置及び姿勢関係を維持するように、前記第２ロボットの動作目標である第２目標を算出する第２目標算出手段と、前記第２目標へ前記第２ロボットが動作する速度が上限速度を超えているか否か判定する判定手段と、前記判定手段により前記第２ロボットの速度が前記上限速度を超えていると判定された場合に、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付けるように前記第１目標を修正する第１目標修正手段と、前記第１目標へ前記第１ロボットを動作させ、前記第２目標へ前記第２ロボットを動作させる動作手段と、を備えることを特徴とする。

上記構成によれば、第１目標算出手段により、動作周期毎に、第１ロボットの動作目標である第１目標が算出される。そして、第２目標算出手段により、第１目標に対して、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持するように、第２ロボットの動作目標である第２目標が算出される。

判定手段により、第２目標へ第２ロボットが動作する速度が上限速度を超えているか否か判定される。そして、判定手段により第２ロボットの速度が上限速度を超えていないと判定された場合は、動作手段により、第１目標へ第１ロボットが動作させられ、第２目標へ第２ロボットを動作させられる。これにより、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持しつつ、第１ロボット及び第２ロボットをそれぞれ第１目標及び第２目標へ動作させることができる。その結果、第１ロボット及び第２ロボットにより、１つのワークを把持して搬送する協調動作等を行うことができる。

一方、判定手段により第２ロボットの速度が上限速度を超えていると判定された場合は、第１目標修正手段により、第１ロボットの現在の位置及び姿勢に第１目標を近付けるように第１目標が修正される。そして、第２目標算出手段により、修正された第１目標に対して、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持するように第２目標が算出される。このため、修正される前の第１目標に対する第２目標と比較して、修正された後の第１目標に対する第２目標を、第２ロボットの現在の位置及び姿勢に近付けることができる。したがって、第２目標へ第２ロボットが動作する速度を低下させることができ、第１ロボットの動作に第２ロボットの動作を追従させることができる。なお、第１目標が修正された後も、第２目標へ第２ロボットが動作する速度が上限速度を超えている場合は、第１目標が再修正される。

第２の手段では、前記第１目標修正手段は、前記第２ロボットの速度が前記上限速度を超えている量が大きいほど、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付ける量を大きくする。

上記構成によれば、第１目標修正手段により、第２ロボットの速度が上限速度を超えている量が大きいほど、第１ロボットの現在の位置及び姿勢に第１目標を近付ける量が大きくされる。このため、第２ロボットの速度が上限速度を超えている量に応じて、適切に第１目標を修正することができる。

第３の手段は、第１ロボット及び第２ロボットを制御するロボット制御装置であって、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する第１目標算出手段と、前記第１目標に対して、前記第１ロボット及び前記第２ロボットの相対的な位置及び姿勢関係を維持するように、前記第２ロボットの動作目標である第２目標を算出する第２目標算出手段と、前記第２目標へ前記第２ロボットが動作する加速度が上限加速度を超えているか否か判定する判定手段と、前記判定手段により前記第２ロボットの加速度が前記上限加速度を超えていると判定された場合に、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付けるように前記第１目標を修正する第１目標修正手段と、前記第１目標へ前記第１ロボットを動作させ、前記第２目標へ前記第２ロボットを動作させる動作手段と、を備えることを特徴とする。

上記構成によれば、判定手段により、第２目標へ第２ロボットが動作する加速度が上限加速度を超えているか否か判定される。そして、判定手段により第２ロボットの加速度が上限速度を超えていないと判定された場合は、動作手段により、第１目標へ第１ロボットが動作させられ、第２目標へ第２ロボットを動作させられる。

一方、判定手段により第２ロボットの加速度が上限加速度を超えていると判定された場合は、第１目標修正手段により、第１ロボットの現在の位置及び姿勢に第１目標を近付けるように第１目標が修正される。したがって、第２目標へ第２ロボットが動作する加速度を低下させることができ、第１ロボットの動作に第２ロボットの動作を追従させることができる。

第４の手段では、前記第１目標修正手段は、前記第２ロボットの加速度が前記上限加速度を超えている量が大きいほど、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付ける量を大きくする。

上記構成によれば、第１目標修正手段により、第２ロボットの加速度が上限加速度を超えている量が大きいほど、第１ロボットの現在の位置及び姿勢に第１目標を近付ける量が大きくされる。このため、第２ロボットの加速度が上限加速度を超えている量に応じて、適切に第１目標を修正することができる。

具体的には、第５の手段のように、前記相対的な位置及び姿勢関係は、前記第１ロボット及び前記第２ロボットにより１つのワークを把持させた状態において、前記第２ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットの位置及び姿勢を特定する行列の逆行列、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットのベース座標系の原点の位置及び姿勢を特定する行列の逆行列、並びに前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第１ロボットの位置及び姿勢を特定する行列を掛けた行列で特定されるといった構成を採用することができる。こうした構成によれば、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を、行列を用いて容易に特定することができる。

さらに、第５の手段の構成を前提として、第６の手段のように、前記第２目標は、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットのベース座標系の原点の位置及び姿勢を特定する行列の逆行列、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第１ロボットの位置及び姿勢を特定する行列、並びに前記相対的な位置及び姿勢関係を特定する前記行列の逆行列を掛けた行列で特定されるといった構成を採用することができる。

第７の手段では、前記第２目標が前記第２ロボットの動作可能な位置及び姿勢でない場合に、前記第１ロボット及び第２ロボットの動作を禁止する禁止手段を備える。

上記構成によれば、禁止手段により、第２目標が第２ロボットの動作可能な位置及び姿勢でない場合に、第１ロボット及び第２ロボットの動作が禁止される。このため、第１ロボットの動作に第２ロボットの動作が追従できない場合に、第１ロボットだけが動作させられることを避けることができる。したがって、第１ロボット及び第２ロボットにより把持したワーク等が破損することを抑制することができる。

第８の手段では、前記第１目標算出手段は、前記第１ロボットの動作を手動操作する操作器からの指令に基づいて、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する。

上記構成によれば、第１目標算出手段により、第１ロボットの動作を手動操作する操作器からの指令に基づいて、動作周期毎に、第１ロボットの動作目標である第１目標が算出される。このため、ユーザが操作器により第１ロボットを手動操作する際に、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持しつつ、第１ロボット及び第２ロボットをそれぞれ動作させることができる。

第９の手段では、前記第１目標算出手段は、前記第１ロボットの動作目標として教示された教示点に基づいて、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する。

上記構成によれば、第１目標算出手段により、第１ロボットの動作目標として教示された教示点に基づいて、動作周期毎に、第１ロボットの動作目標である第１目標が算出される。このため、第１ロボットの動作目標として教示点を教示することにより、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持しつつ、第１ロボット及び第２ロボットをそれぞれ動作させることができる。

ロボットシステムの概要を示すブロック図。協調動作における２台のロボットの動作量を示す斜視図。ロボットの協調動作制御を示すフローチャート。２台のロボットの相対的な位置及び姿勢関係を示す模式図。第２ロボットの動作目標を示す模式図。ロボットの協調動作制御の変更例を示すフローチャート。２台のロボットの教示点及び目標軌道を示す模式図。

以下、一実施形態について、図面を参照しつつ説明する。本実施形態は、機械組立工場等において、ワークの搬送や機械の組み立て等を行うロボットシステムとして具体化している。

図１は、ロボットシステム１０の概要を示すブロック図である。ロボットシステム１０は、ロボットコントローラ２０、ロボット機構部４１（ロボット４０）、及び操作器５０の組を複数を備えている。

ロボットコントローラ２０（ロボット制御装置）は、ロボット４０を全体的に制御するＣＰＵ２１を有している。このＣＰＵ２１には、バス２９を介してＲＯＭ２２、ＲＡＭ２３、不揮発性メモリ２４、ロボット軸制御部２５、通信モジュール２６、操作器インタフェース２７等が接続されている。

ＲＯＭ２２には、ＣＰＵ２１が実行するシステムプログラムが格納されており、不揮発性メモリ２４には各種パラメータ等の設定値及びロボット４０が行う作業の教示プログラムが格納される。また、ＲＡＭ２３はデータの一時記憶等に利用される。ロボット軸制御部２５は、ロボット４０の各軸への移動指令に基づいて、各軸の位置、速度及びモータトルク（加速度）のフィードバック制御を行う。ロボット軸制御部２５は、サーボアンプ２８を介してロボット４０におけるロボット機構部４１の各軸のサーボモータを駆動制御する。通信モジュール２６は別のロボットコントローラと通信路で接続される。操作器インタフェース２７には、操作器５０（ティーチングペンダント）が接続されている。

図２は、協調動作における２台のロボット４０Ａ，４０Ｂの動作量を示す斜視図である。図２（ａ）は協調動作開始前の状態を示しており、図２（ｂ）は協調動作終了後の状態を示している。ここでは、第１ロボット４０Ａと第２ロボット４０Ｂとにより、１つのワークＷが把持されており、このワークＷを変形させることなく、ワークＷをｘ軸周りに回転させる動作を行う。すなわち、両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係を維持しつつ、ワークＷを回転させる。

この場合、第２ロボット４０Ｂの動作量は、第１ロボット４０Ａの動作量よりも大きくなる。このため、第１ロボット４０Ａを高速で動作させると、両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係を維持しようとしても、第２ロボット４０Ｂの動作速度が上限速度を超えて第２ロボット４０Ｂが追従できなくなる。

そこで、本実施形態では、第１ロボット４０Ａの動作目標（第１目標）に対して、上記相対的な位置及び姿勢関係を維持するように第２ロボット４０Ｂの動作目標（第２目標）を算出し、第２目標へ第２ロボット４０Ｂが動作する速度が上限速度を超えている場合に、第１ロボット４０Ａの現在の位置及び姿勢に上記第１目標を近付けるように第１目標を修正する。図３は、本実施形態におけるロボット４０Ａ，４０Ｂの協調動作制御を示すフローチャートである。この一連の処理は、Ｓ１１の処理を除いてＣＰＵ２１により実行される。

まず、ユーザは、第１ロボット４０Ａ及び第２ロボット４０Ｂにより、ワークＷを把持させる（Ｓ１１）。すなわち、図２（ａ）に示すように、ユーザは第１ロボット４０Ａの操作器５０を操作して第１ロボット４０ＡによりワークＷを把持させるとともに、第２ロボット４０Ｂの操作器５０を操作して第２ロボット４０ＢによりワークＷを把持させる。

続いて、両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係Ｔｖを算出する（Ｓ１２）。詳しくは、以下の数式により、上記相対的な位置及び姿勢関係Ｔｖを算出する。

Ｔｖ＝（Ｔｒ２ｔ２）^−１×（Ｔｒ１ｒ２）^−１×（Ｔｒ１ｔ１）
ここで、図４に示すように、Ｔｒ２ｔ２は第２ロボット４０Ｂのベース座標系の原点Ｏ２の位置及び姿勢に対する第２ロボット４０Ｂのツール先端ｔ２の位置及び姿勢を特定する行列であり、Ｔｒ１ｒ２は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第２ロボット４０Ｂのベース座標系の原点Ｏ２の位置及び姿勢を特定する行列であり、Ｔｒ１ｔ１は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第１ロボット４０Ａのツール先端ｔ１の位置及び姿勢を特定する行列である。行列Ｔｒ２ｔ２等は４×４の同時変換行列であり、（Ｔｒ２ｔ２）^−１は行列Ｔｒ２ｔ２の逆行例を表す。

続いて、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作があるか否か判定する（Ｓ１３）。この判定において、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作がないと判定した場合（Ｓ１３：ＮＯ）、この一連の処理を一端終了する（ＥＮＤ）。

一方、Ｓ１３の判定において、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作があると判定した場合（Ｓ１３：ＹＥＳ）、操作器５０からの動作指令に基づいて第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を算出する（Ｓ１４）。詳しくは、操作器５０に対するユーザの動作操作（設定された速度で各軸を回転させる操作等）に対応する動作指令が、操作器５０から操作器インタフェース２７を介してＣＰＵ２１へ出力される。ＣＰＵ２１は、この動作指令（各軸の回転指令）に従って第１ロボット４０Ａを動作させるように、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊（目標位置及び目標姿勢）を算出する。この第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊は、第１ロボット４０Ａの１動作周期ｔｓ（１制御周期）後の動作目標である。

続いて、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊に対して、上記相対的な位置及び姿勢関係Ｔｖを維持するように、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊を算出する（Ｓ１５）。詳しくは、以下の数式により、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊を算出する。この第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊は、第２ロボット４０Ｂの１動作周期ｔｓ（１制御周期）後の動作目標である。

Ｔｒ２ｔ２＊＝（Ｔｒ１ｒ２）^−１×（Ｔｒ１ｔ１＊）×（Ｔｖ）^−１
ここで、図５に示すように、（Ｔｒ１ｒ２）^−１は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第２ロボット４０Ｂのベース座標系の原点Ｏ２の位置及び姿勢を特定する行列の逆行例であり、Ｔｒ１ｔ１＊は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第１ロボット４０Ａのツール先端ｔ１の目標位置及び目標姿勢（動作目標）を特定する行列であり、（Ｔｖ）^−１は上記対的な位置及び姿勢関係を特定する行列の逆行列である。

続いて、１動作周期ｔｓ後に動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する速度Ｖ２が、上限速度Ｖ２ｍｘを超えているか否か判定する（Ｓ１６）。具体的には、速度Ｖ２は、１動作周期ｔｓと動作目標Ｔｒ２ｔ２＊とに基づいて算出される。上限速度Ｖ２ｍｘは、第２ロボット４０Ｂの仕様に基づいて設定されている。

Ｓ１６の判定において、速度Ｖ２が上限速度Ｖ２ｍｘを超えていると判定された場合（Ｓ１６：ＹＥＳ）、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊を修正する（Ｓ１７）。詳しくは、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を大きくする。すなわち、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの動作量を小さくする。その後、Ｓ１５の処理から再度実行し、修正された動作目標Ｔｒ１ｔ１＊に対して、上記相対的な位置及び姿勢関係Ｔｖを維持するように、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊を算出する。

一方、Ｓ１６の判定において、速度Ｖ２が上限速度Ｖ２ｍｘを超えていないと判定された場合（Ｓ１６：ＮＯ）、動作目標Ｔｒ１ｔ１＊へ第１ロボット４０Ａを動作させ、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂを動作させる（Ｓ１８）。

続いて、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作が終了したか否か判定する（Ｓ１９）。この判定において、ユーザによる操作器５０に対する動作操作が終了していないと判定した場合（Ｓ１９：ＮＯ）、Ｓ１４の処理から再度実行する。

一方、Ｓ１９の判定において、ユーザによる操作器５０に対する動作操作が終了したと判定した場合（Ｓ１９：ＹＥＳ）、操作器５０からの停止指令に基づいて第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を算出する（Ｓ２０）。詳しくは、操作器５０に対するユーザの停止操作（各軸を回転させる操作等の終了）に対応する停止指令が、操作器５０から操作器インタフェース２７を介してＣＰＵ２１へ出力される。ＣＰＵ２１は、この停止指令（各軸の回転停止指令）に従って第１ロボット４０Ａを停止させるように、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊（目標位置及び目標姿勢）を算出する。

続いて、上記Ｓ１５〜Ｓ１８の処理と同様に、Ｓ２１〜Ｓ２４の処理を実行する。そして、第１ロボット４０Ａ及び第２ロボット４０Ｂが停止したか否か判定する（Ｓ２５）。この判定において、第１ロボット４０Ａ及び第２ロボット４０Ｂが停止していないと判定した場合（Ｓ２５：ＮＯ）、Ｓ２０の処理から再度実行する。一方、この判定において、第１ロボット４０Ａ及び第２ロボット４０Ｂが停止したと判定した場合（Ｓ２５：ＹＥＳ）、この一連の処理を一旦終了する（ＥＮＤ）。

なお、Ｓ１４及びＳ２０の処理が第１目標算出手段としての処理に相当し、Ｓ１５及びＳ２１の処理が第２目標算出手段としての処理に相当し、Ｓ１６及びＳ２２の処理が判定手段としての処理に相当し、Ｓ１７及びＳ２３の処理が第１目標修正手段としての処理に相当し、Ｓ１８及びＳ２４の処理が動作手段としての処理に相当する。

以上詳述した本実施形態は、以下の利点を有する。

・動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する速度Ｖ２が上限速度Ｖ２ｍｘを超えているか否か判定される。そして、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えていないと判定された場合は、動作目標Ｔｒ１ｔ１＊へ第１ロボット４０Ａが動作させられ、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂを動作させられる。これにより、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持しつつ、第１ロボット４０Ａ及び第２ロボット４０Ｂをそれぞれ動作目標Ｔｒ１ｔ１＊及び動作目標Ｔｒ２ｔ２＊へ動作させることができる。その結果、第１ロボット４０Ａ及び第２ロボット４０Ｂにより、１つのワークＷを把持して搬送する協調動作等を行うことができる。

・第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えていると判定された場合は、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊が修正される。そして、修正された動作目標Ｔｒ１ｔ１＊に対して、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持するように動作目標Ｔｒ２ｔ２＊が算出される。このため、修正される前の動作目標Ｔｒ１ｔ１＊に対する動作目標Ｔｒ２ｔ２＊と比較して、修正された後の動作目標Ｔｒ１ｔ１＊に対する動作目標Ｔｒ２ｔ２＊を、第２ロボット４０Ｂの現在の位置及び姿勢に近付けることができる。したがって、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する速度Ｖ２を低下させることができ、第１ロボット４０Ａの動作に第２ロボット４０Ｂの動作を追従させることができる。

・第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量が大きくされる。このため、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量に応じて、適切に動作目標Ｔｒ１ｔ１＊を修正することができる。

・図４に示すように、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを、行列を用いて容易に特定することができる。さらに、図５に示すように、動作目標Ｔｒ２ｔ２＊を、相対的な位置及び姿勢関係Ｔｖを表す行列を用いて容易に特定することができる。

・第１ロボット４０Ａの動作を手動操作する操作器５０からの指令に基づいて、動作周期ｔｓ毎に、第１ロボット４０Ａの動作目標である動作目標Ｔｒ１ｔ１＊が算出される。このため、ユーザが操作器５０により第１ロボット４０Ａを手動操作する際に、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持しつつ、第１ロボット４０Ａ及び第２ロボット４０Ｂをそれぞれ動作させることができる。

なお、上記実施形態を、以下のように変更して実施することもできる。

・上記実施形態では、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を大きくした。これに対して、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量にかかわらず、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を一定にすることもできる。

・上記実施形態では、速度Ｖ２が上限速度Ｖ２ｍｘを超えていると判定された場合（Ｓ１６：ＹＥＳ）、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊を修正した（Ｓ１７）。これに対して、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように、Ｓ１４において動作目標Ｔｒ１ｔ１＊を再算出してもよい。この場合も、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊が修正されることとなる。また、動作目標Ｔｒ１ｔ１＊が修正される場合に、修正される前の動作目標Ｔｒ１ｔ１＊を記憶しておいてもよい。そして、記憶しておいた修正される前の動作目標Ｔｒ１ｔ１＊を、修正された動作目標Ｔｒ１ｔ１＊の次動作周期での動作目標Ｔｒ１ｔ１＊とすることにより、ＣＰＵ２１の演算負荷を軽減することができる。

・図３のＳ１６及びＳ２２を、以下のように変更することもできる。すなわち、１動作周期ｔｓ後に動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する加速度Ａ２が、上限加速度Ａ２ｍｘを超えているか否か判定する。具体的には、加速度Ａ２は、１動作周期ｔｓと動作目標Ｔｒ２ｔ２＊とに基づいて算出される。上限加速度Ａ２ｍｘは、第２ロボット４０Ｂの仕様に基づいて設定されている。

そして、Ｓ１７及びＳ２３を以下のように変更することができる。第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊を修正する。詳しくは、第２ロボット４０Ｂの加速度Ａ２が上限加速度Ａ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を大きくする。すなわち、第２ロボット４０Ｂの加速度Ａ２が上限加速度Ａ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの動作量を小さくする。

こうした構成によれば、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する加速度Ａ２を低下させることができ、第１ロボット４０Ａの動作に第２ロボット４０Ｂの動作を追従させることができる。さらに、第２ロボット４０Ｂの加速度Ａ２が上限加速度Ａ２ｍｘを超えている量に応じて、適切に動作目標Ｔｒ１ｔ１＊を修正することができる。なお、加速度Ａ２は、速度を上昇させる時の加速度であってもよいし、速度を低下させる時の減速度であってもよい。

・図６は、ロボット４０Ａ，４０Ｂの協調動作制御の変更例を示すフローチャートである。この一連の処理は、Ｓ３１の処理を除いてＣＰＵ２１により実行される。

まず、ユーザは、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊として、全ての教示点ｔ１１〜ｔ１ｅを教示する（Ｓ３１）。すなわち、図７に示すように、ユーザは第１ロボット４０Ａの操作器５０を操作して、ロボット４０Ａ，４０ＢによりワークＷを把持させて協調動作を行う際の第１ロボット４０Ａの各動作目標Ｔｒ１ｔ１＊として、教示点ｔ１１〜ｔ１ｅを教示する。

続いて、協調動作の開始点ｔ１０，ｔ２０において両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係Ｔｖを算出する（Ｓ３２）。Ｓ３２の処理は、図６のＳ１２の処理と同様である。ただし、両ロボット４０Ａ，４０ＢによりワークＷを把持させておらず、両ロボット４０Ａ，４０Ｂはバーチャルツールで接続されていると考えることができる。

続いて、教示された教示点ｔ１１〜ｔ１ｅに基づいて第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を算出する（Ｓ３３）。詳しくは、各教示点の間を補間して、教示点間の第１ロボット４０Ａの速度パターンを算出する。そして、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊（目標位置及び目標姿勢）を算出する。この第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊は、第１ロボット４０Ａの１動作周期ｔｓ（１制御周期）後の動作目標である。

続いて、図３のＳ１５〜Ｓ１７の処理と同様に、Ｓ３４〜Ｓ３６の処理を実行する。そして、Ｓ３５の判定において、速度Ｖ２が上限速度Ｖ２ｍｘを超えていないと判定された場合（Ｓ３５：ＮＯ）、動作目標Ｔｒ１ｔ１＊及び動作目標Ｔｒ２ｔ２＊を不揮発性メモリ２４に記録する（Ｓ３７）。すなわち、ロボット４０Ａ，４０Ｂを動作させる代わりに、ロボット４０Ａ，４０Ｂの各制御周期ｔｓにおける動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊を記録する。

続いて、動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊の算出及び記録が、最後の教示点ｔ１ｅ，ｔ２ｅに到達したか否か判定する（Ｓ３８）。すなわち、図７に示すように、全ての教示点ｔ１１〜ｔ１ｅに対して、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊（動作点ｔ２１〜ｔ２ｅ）を算出したか否か判定する。この判定において、動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊の算出及び記録が、最後の教示点ｔ１ｅ，ｔ２ｅに到達していないと判定した場合（Ｓ３８：ＮＯ）、Ｓ３３の処理から再度実行する。一方、この判定において、動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊の算出及び記録が、最後の教示点ｔ１ｅ，ｔ２ｅに到達したと判定した場合（Ｓ３８：ＹＥＳ）、この一連の処理を一旦終了する（ＥＮＤ）。

なお、Ｓ３３の処理が第１目標算出手段としての処理に相当し、Ｓ３４の処理が第２目標算出手段としての処理に相当し、Ｓ３５の処理が判定手段としての処理に相当し、Ｓ３６の処理が第１目標修正手段としての処理に相当する。

こうした構成によれば、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊として教示された教示点ｔ１１〜ｔ１ｅに基づいて、動作周期ｔｓ毎に、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊が算出される。このため、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊として教示点ｔ１１〜ｔ１ｅを教示することにより、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持しつつ、第１ロボット４０Ａ及び第２ロボット４０Ｂの動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊を算出して記録することができる。ひいては、記録された動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊を用いた自動動作により、第１ロボット４０Ａ及び第２ロボット４０Ｂを協調動作させることができる。

・第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊が第２ロボット４０Ｂの動作可能な位置及び姿勢でない場合に、第１ロボット４０Ａ及び第２ロボット４０Ｂの動作を禁止する禁止手段を備えてもよい。こうした構成によれば、禁止手段により、動作目標Ｔｒ２ｔ２＊が第２ロボット４０Ｂの動作可能な位置及び姿勢でない場合に、第１ロボット４０Ａ及び第２ロボット４０Ｂの動作が禁止される。このため、第１ロボット４０Ａの動作に第２ロボット４０Ｂの動作が追従できない場合に、第１ロボット４０Ａだけが動作させられることを避けることができる。したがって、第１ロボット４０Ａ及び第２ロボット４０Ｂにより把持したワークＷ等が破損することを抑制することができる。

・ロボットコントローラ２０は、各ロボット４０Ａ，４０Ｂを制御するものでもよいし、ロボット４０Ａ，４０Ｂを統括して制御するものでもよい。

・ロボットは２台に限らず、３台以上であってもよい。その場合は、１台を第１ロボット４０Ａとし、その他の少なくとも１台を第２ロボット４０Ｂとすればよい。

２０…ロボットコントローラ（ロボット制御装置）、４０…ロボット、４０Ａ…第１ロボット、４０Ｂ…第２ロボット、５０…操作器。

Claims

第１ロボット及び第２ロボットを制御するロボット制御装置であって、
動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する第１目標算出手段と、
前記第１目標に対して、前記第１ロボット及び前記第２ロボットの相対的な位置及び姿勢関係を維持するように、前記第２ロボットの動作目標である第２目標を算出する第２目標算出手段と、
前記第２目標へ前記第２ロボットが動作する速度が上限速度を超えているか否か判定する判定手段と、
前記判定手段により前記第２ロボットの速度が前記上限速度を超えていると判定された場合に、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付けるように前記第１目標を修正する第１目標修正手段と、
前記第１目標へ前記第１ロボットを動作させ、前記第２目標へ前記第２ロボットを動作させる動作手段と、
を備えることを特徴とするロボット制御装置。
前記第１目標修正手段は、前記第２ロボットの速度が前記上限速度を超えている量が大きいほど、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付ける量を大きくする請求項１に記載のロボット制御装置。
第１ロボット及び第２ロボットを制御するロボット制御装置であって、
動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する第１目標算出手段と、
前記第１目標に対して、前記第１ロボット及び前記第２ロボットの相対的な位置及び姿勢関係を維持するように、前記第２ロボットの動作目標である第２目標を算出する第２目標算出手段と、
前記第２目標へ前記第２ロボットが動作する加速度が上限加速度を超えているか否か判定する判定手段と、
前記判定手段により前記第２ロボットの加速度が前記上限加速度を超えていると判定された場合に、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付けるように前記第１目標を修正する第１目標修正手段と、
前記第１目標へ前記第１ロボットを動作させ、前記第２目標へ前記第２ロボットを動作させる動作手段と、
を備えることを特徴とするロボット制御装置。
前記第１目標修正手段は、前記第２ロボットの加速度が前記上限加速度を超えている量が大きいほど、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付ける量を大きくする請求項３に記載のロボット制御装置。
前記相対的な位置及び姿勢関係は、前記第１ロボット及び前記第２ロボットにより１つのワークを把持させた状態において、前記第２ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットの位置及び姿勢を特定する行列の逆行列、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットのベース座標系の原点の位置及び姿勢を特定する行列の逆行列、並びに前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第１ロボットの位置及び姿勢を特定する行列を掛けた行列で特定される請求項１〜４のいずれか１項に記載のロボット制御装置。
前記第２目標は、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットのベース座標系の原点の位置及び姿勢を特定する行列の逆行列、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第１ロボットの位置及び姿勢を特定する行列、並びに前記相対的な位置及び姿勢関係を特定する前記行列の逆行列を掛けた行列で特定される請求項５に記載のロボット制御装置。
前記第２目標が前記第２ロボットの動作可能な位置及び姿勢でない場合に、前記第１ロボット及び第２ロボットの動作を禁止する禁止手段を備える請求項１〜６のいずれか１項に記載のロボット制御装置。
前記第１目標算出手段は、前記第１ロボットの動作を手動操作する操作器からの指令に基づいて、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する請求項１〜７のいずれか１項に記載のロボット制御装置。
前記第１目標算出手段は、前記第１ロボットの動作目標として教示された教示点に基づいて、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する請求項１〜７のいずれか１項に記載のロボット制御装置。