JP6539989B2

JP6539989B2 - Robot controller

Info

Publication number: JP6539989B2
Application number: JP2014231170A
Authority: JP
Inventors: 健司長松
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2014-11-14
Filing date: 2014-11-14
Publication date: 2019-07-10
Anticipated expiration: 2034-11-14
Also published as: JP2016093863A

Description

本発明は、複数のロボットを制御するロボット制御装置に関する。 The present invention relates to a robot control apparatus that controls a plurality of robots.

従来、複数のロボットにより１つのワークを把持して搬送する協調動作を行うものがある（例えば、特許文献１参照）。特許文献１に記載のものは、マスタロボット（第１ロボット）のアームの通過点と、搬送中のワークの状態に対応する両アームの相対的な位置及び姿勢関係とに基づいて、スレーブロボット（第２ロボット）のアームの通過点を決定している。 Conventionally, there is one that performs a coordinated operation in which one robot is held by a plurality of robots and transported (for example, see Patent Document 1). The one described in Patent Document 1 is a slave robot based on a passing point of an arm of a master robot (first robot) and a relative position and posture relationship between both arms corresponding to the state of a workpiece being The passing point of the arm of the second robot is determined.

特許第３５７７０２８号公報Patent No. 3577028

しかしながら、マスタロボットのアームの動作態様によっては、マスタロボットのアームの動作量とスレーブロボットのアームの動作量とが大きく異なる点に、本願発明者は着目した。そして、両アームの相対的な位置及び姿勢関係を維持して、スレーブロボットのアームを動作させようとしても、スレーブロボットのアームが追従できない場合があることを、本願発明者は見出した。 However, the inventors of the present application focused attention on the point that the amount of movement of the arm of the master robot and the amount of movement of the arm of the slave robot greatly differ depending on the operation mode of the arm of the master robot. The inventors of the present invention have found that there is a case where the arms of the slave robot can not follow even when trying to operate the arms of the slave robot while maintaining the relative position and posture relationship of both arms.

本発明は、こうした実情に鑑みてなされたものであり、第１ロボットの動作態様にかかわらず、第２ロボットの動作を追従させることのできるロボット制御装置を提供することを主たる目的とするものである。 The present invention has been made in view of such circumstances, and its main object is to provide a robot control apparatus capable of following the operation of a second robot regardless of the operation mode of the first robot. is there.

第１の手段は、第１ロボット及び第２ロボットを制御するロボット制御装置であって、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する第１目標算出手段と、前記第１目標に対して、前記第１ロボット及び前記第２ロボットの相対的な位置及び姿勢関係を維持するように、前記第２ロボットの動作目標である第２目標を算出する第２目標算出手段と、前記第２目標へ前記第２ロボットが動作する速度が上限速度を超えているか否か判定する判定手段と、前記判定手段により前記第２ロボットの速度が前記上限速度を超えていると判定された場合に、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付けるように前記第１目標を修正する第１目標修正手段と、前記第１目標へ前記第１ロボットを動作させ、前記第２目標へ前記第２ロボットを動作させる動作手段と、を備えることを特徴とする。 A first means is a robot control apparatus for controlling a first robot and a second robot, and a first target calculation means for calculating a first target which is an operation target of the first robot for each operation cycle; A second target calculation for calculating a second target which is an operation target of the second robot so as to maintain a relative position and posture relationship between the first robot and the second robot with respect to the first target Means, determining means for determining whether the speed at which the second robot operates to the second target exceeds the upper limit speed, and the speed of the second robot exceeds the upper limit speed by the determining means First target correction means for correcting the first target so that the first target approaches the current position and attitude of the first robot when it is determined; and operating the first robot to the first target Let And operating means for operating said second robot to second target, characterized in that it comprises a.

上記構成によれば、第１目標算出手段により、動作周期毎に、第１ロボットの動作目標である第１目標が算出される。そして、第２目標算出手段により、第１目標に対して、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持するように、第２ロボットの動作目標である第２目標が算出される。 According to the above configuration, the first target calculation means calculates, for each operation cycle, the first target which is the operation target of the first robot. Then, the second target calculation means calculates the second target, which is the operation target of the second robot, so as to maintain the relative position and posture relationship between the first robot and the second robot with respect to the first target. Be done.

判定手段により、第２目標へ第２ロボットが動作する速度が上限速度を超えているか否か判定される。そして、判定手段により第２ロボットの速度が上限速度を超えていないと判定された場合は、動作手段により、第１目標へ第１ロボットが動作させられ、第２目標へ第２ロボットを動作させられる。これにより、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持しつつ、第１ロボット及び第２ロボットをそれぞれ第１目標及び第２目標へ動作させることができる。その結果、第１ロボット及び第２ロボットにより、１つのワークを把持して搬送する協調動作等を行うことができる。 The determination means determines whether the speed at which the second robot moves to the second target exceeds the upper limit speed. Then, when it is determined by the determination means that the speed of the second robot does not exceed the upper limit speed, the first robot is operated to the first target by the operation means, and the second robot is operated to the second target. Be Thus, the first robot and the second robot can be moved to the first target and the second target, respectively, while maintaining the relative position and posture relationship between the first robot and the second robot. As a result, the first robot and the second robot can perform a coordinated operation or the like for gripping and transporting one work.

一方、判定手段により第２ロボットの速度が上限速度を超えていると判定された場合は、第１目標修正手段により、第１ロボットの現在の位置及び姿勢に第１目標を近付けるように第１目標が修正される。そして、第２目標算出手段により、修正された第１目標に対して、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持するように第２目標が算出される。このため、修正される前の第１目標に対する第２目標と比較して、修正された後の第１目標に対する第２目標を、第２ロボットの現在の位置及び姿勢に近付けることができる。したがって、第２目標へ第２ロボットが動作する速度を低下させることができ、第１ロボットの動作に第２ロボットの動作を追従させることができる。なお、第１目標が修正された後も、第２目標へ第２ロボットが動作する速度が上限速度を超えている場合は、第１目標が再修正される。 On the other hand, when the determination means determines that the speed of the second robot exceeds the upper limit speed, the first target correction means causes the first target to approach the current position and posture of the first robot. The goal is corrected. Then, the second target calculating means calculates the second target so as to maintain the relative position and posture relationship between the first robot and the second robot with respect to the corrected first target. Thus, the second target for the first target after correction can be made closer to the current position and posture of the second robot, as compared to the second target for the first target before correction. Therefore, the speed at which the second robot operates to the second target can be reduced, and the operation of the second robot can be made to follow the operation of the first robot. In addition, even after the first target is corrected, if the speed at which the second robot moves to the second target exceeds the upper limit speed, the first target is recorrected.

第２の手段では、前記第１目標修正手段は、前記第２ロボットの速度が前記上限速度を超えている量が大きいほど、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付ける量を大きくする。 In the second means, the first target correction means brings the first target closer to the current position and posture of the first robot as the amount by which the speed of the second robot exceeds the upper limit speed increases. Increase the amount.

上記構成によれば、第１目標修正手段により、第２ロボットの速度が上限速度を超えている量が大きいほど、第１ロボットの現在の位置及び姿勢に第１目標を近付ける量が大きくされる。このため、第２ロボットの速度が上限速度を超えている量に応じて、適切に第１目標を修正することができる。 According to the above configuration, the amount by which the first target approaches the current position and posture of the first robot is increased by the first target correction means as the amount by which the speed of the second robot exceeds the upper limit speed increases. . Therefore, the first target can be appropriately corrected according to the amount by which the speed of the second robot exceeds the upper limit speed.

第３の手段は、第１ロボット及び第２ロボットを制御するロボット制御装置であって、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する第１目標算出手段と、前記第１目標に対して、前記第１ロボット及び前記第２ロボットの相対的な位置及び姿勢関係を維持するように、前記第２ロボットの動作目標である第２目標を算出する第２目標算出手段と、前記第２目標へ前記第２ロボットが動作する加速度が上限加速度を超えているか否か判定する判定手段と、前記判定手段により前記第２ロボットの加速度が前記上限加速度を超えていると判定された場合に、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付けるように前記第１目標を修正する第１目標修正手段と、前記第１目標へ前記第１ロボットを動作させ、前記第２目標へ前記第２ロボットを動作させる動作手段と、を備えることを特徴とする。 A third means is a robot control device for controlling the first robot and the second robot, and a first target calculation means for calculating a first target which is an operation target of the first robot for each operation cycle; A second target calculation for calculating a second target which is an operation target of the second robot so as to maintain a relative position and posture relationship between the first robot and the second robot with respect to the first target Means, determining means for determining whether the acceleration at which the second robot operates to the second target exceeds the upper limit acceleration, and the acceleration of the second robot using the determining means exceeds the upper limit acceleration First target correction means for correcting the first target so that the first target approaches the current position and attitude of the first robot when it is determined; and operating the first robot to the first target The Characterized by and a operation means for operating the second robot to the second target.

上記構成によれば、判定手段により、第２目標へ第２ロボットが動作する加速度が上限加速度を超えているか否か判定される。そして、判定手段により第２ロボットの加速度が上限速度を超えていないと判定された場合は、動作手段により、第１目標へ第１ロボットが動作させられ、第２目標へ第２ロボットを動作させられる。 According to the above configuration, the determination means determines whether the acceleration at which the second robot moves to the second target exceeds the upper limit acceleration. Then, when it is determined by the determination means that the acceleration of the second robot does not exceed the upper limit speed, the first robot is operated to the first target by the operation means, and the second robot is operated to the second target. Be

一方、判定手段により第２ロボットの加速度が上限加速度を超えていると判定された場合は、第１目標修正手段により、第１ロボットの現在の位置及び姿勢に第１目標を近付けるように第１目標が修正される。したがって、第２目標へ第２ロボットが動作する加速度を低下させることができ、第１ロボットの動作に第２ロボットの動作を追従させることができる。 On the other hand, when the determination means determines that the acceleration of the second robot exceeds the upper limit acceleration, the first target correction means causes the first target to approach the current position and posture of the first robot. The goal is corrected. Therefore, the acceleration at which the second robot operates to the second target can be reduced, and the operation of the second robot can be made to follow the operation of the first robot.

第４の手段では、前記第１目標修正手段は、前記第２ロボットの加速度が前記上限加速度を超えている量が大きいほど、前記第１ロボットの現在の位置及び姿勢に前記第１目標を近付ける量を大きくする。 In the fourth means, the first target correction means brings the first target closer to the current position and posture of the first robot as the amount by which the acceleration of the second robot exceeds the upper limit acceleration is larger. Increase the amount.

上記構成によれば、第１目標修正手段により、第２ロボットの加速度が上限加速度を超えている量が大きいほど、第１ロボットの現在の位置及び姿勢に第１目標を近付ける量が大きくされる。このため、第２ロボットの加速度が上限加速度を超えている量に応じて、適切に第１目標を修正することができる。 According to the above configuration, the amount by which the first target approaches the current position and posture of the first robot is increased by the first target correction means as the amount by which the acceleration of the second robot exceeds the upper limit acceleration is larger. . Therefore, the first target can be appropriately corrected according to the amount by which the acceleration of the second robot exceeds the upper limit acceleration.

具体的には、第５の手段のように、前記相対的な位置及び姿勢関係は、前記第１ロボット及び前記第２ロボットにより１つのワークを把持させた状態において、前記第２ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットの位置及び姿勢を特定する行列の逆行列、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットのベース座標系の原点の位置及び姿勢を特定する行列の逆行列、並びに前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第１ロボットの位置及び姿勢を特定する行列を掛けた行列で特定されるといった構成を採用することができる。こうした構成によれば、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を、行列を用いて容易に特定することができる。 Specifically, as in the fifth means, the relative position and orientation relationship is determined based on the base coordinates of the second robot in a state in which one workpiece is held by the first robot and the second robot. Inverse matrix of a matrix specifying the position and attitude of the second robot with respect to the position and attitude of the origin of the system, the origin of the base coordinate system of the second robot with respect to the position and attitude of the origin of the base coordinate system of the first robot An inverse matrix of a matrix specifying the position and posture, and a matrix specified by multiplying the position and posture of the first robot with respect to the position and posture of the origin of the base coordinate system of the first robot It can be adopted. According to such a configuration, the relative position and posture relationship between the first robot and the second robot can be easily identified using a matrix.

さらに、第５の手段の構成を前提として、第６の手段のように、前記第２目標は、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第２ロボットのベース座標系の原点の位置及び姿勢を特定する行列の逆行列、前記第１ロボットのベース座標系の原点の位置及び姿勢に対する前記第１ロボットの位置及び姿勢を特定する行列、並びに前記相対的な位置及び姿勢関係を特定する前記行列の逆行列を掛けた行列で特定されるといった構成を採用することができる。 Furthermore, on the premise of the configuration of the fifth means, as in the sixth means, the second target is the base coordinate system of the second robot with respect to the position and posture of the origin of the base coordinate system of the first robot. An inverse matrix of a matrix specifying the position and attitude of the origin, a matrix specifying the position and attitude of the first robot with respect to the position and attitude of the origin of the base coordinate system of the first robot, and the relative position and attitude relationship It is possible to adopt a configuration in which identification is performed using a matrix obtained by multiplying the inverse matrix of the matrix specifying.

第７の手段では、前記第２目標が前記第２ロボットの動作可能な位置及び姿勢でない場合に、前記第１ロボット及び第２ロボットの動作を禁止する禁止手段を備える。 A seventh means includes prohibiting means for prohibiting the operation of the first robot and the second robot when the second target is not the operable position and posture of the second robot.

上記構成によれば、禁止手段により、第２目標が第２ロボットの動作可能な位置及び姿勢でない場合に、第１ロボット及び第２ロボットの動作が禁止される。このため、第１ロボットの動作に第２ロボットの動作が追従できない場合に、第１ロボットだけが動作させられることを避けることができる。したがって、第１ロボット及び第２ロボットにより把持したワーク等が破損することを抑制することができる。 According to the above configuration, when the second target is not the operable position and posture of the second robot, the operation of the first robot and the second robot is prohibited by the prohibiting means. Therefore, when the operation of the second robot can not follow the operation of the first robot, it can be avoided that only the first robot is operated. Therefore, it is possible to suppress breakage of the workpiece or the like held by the first robot and the second robot.

第８の手段では、前記第１目標算出手段は、前記第１ロボットの動作を手動操作する操作器からの指令に基づいて、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する。 In an eighth means, the first target calculating means is a first target which is an operation target of the first robot for each operation cycle, based on a command from an operating device for manually operating the operation of the first robot. Calculate

上記構成によれば、第１目標算出手段により、第１ロボットの動作を手動操作する操作器からの指令に基づいて、動作周期毎に、第１ロボットの動作目標である第１目標が算出される。このため、ユーザが操作器により第１ロボットを手動操作する際に、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持しつつ、第１ロボット及び第２ロボットをそれぞれ動作させることができる。 According to the above configuration, the first target calculation means calculates the first target, which is the movement target of the first robot, for each operation cycle based on the command from the operating device that manually operates the movement of the first robot. Ru. Therefore, when the user manually operates the first robot with the operation device, the first robot and the second robot are operated while maintaining the relative position and posture relationship of the first robot and the second robot. Can.

第９の手段では、前記第１目標算出手段は、前記第１ロボットの動作目標として教示された教示点に基づいて、動作周期毎に、前記第１ロボットの動作目標である第１目標を算出する。 In a ninth means, the first target calculating means calculates, based on the teaching point taught as the motion target of the first robot, the first target which is the motion target of the first robot in each operation cycle. Do.

上記構成によれば、第１目標算出手段により、第１ロボットの動作目標として教示された教示点に基づいて、動作周期毎に、第１ロボットの動作目標である第１目標が算出される。このため、第１ロボットの動作目標として教示点を教示することにより、第１ロボット及び第２ロボットの相対的な位置及び姿勢関係を維持しつつ、第１ロボット及び第２ロボットをそれぞれ動作させることができる。 According to the above configuration, the first target calculation means calculates the first target as the movement target of the first robot for each operation cycle based on the teaching point taught as the movement target of the first robot. Therefore, by teaching the teaching point as the operation target of the first robot, the first robot and the second robot are operated while maintaining the relative position and posture relationship of the first robot and the second robot. Can.

ロボットシステムの概要を示すブロック図。The block diagram which shows the outline | summary of a robot system. 協調動作における２台のロボットの動作量を示す斜視図。FIG. 7 is a perspective view showing the amount of movement of two robots in a coordinated operation. ロボットの協調動作制御を示すフローチャート。The flowchart which shows the cooperation operation control of a robot. ２台のロボットの相対的な位置及び姿勢関係を示す模式図。The schematic diagram which shows the relative position and attitude | position relationship of two robots. 第２ロボットの動作目標を示す模式図。The schematic diagram which shows the operation | movement target of a 2nd robot. ロボットの協調動作制御の変更例を示すフローチャート。The flowchart which shows the example of modification of cooperative operation control of the robot. ２台のロボットの教示点及び目標軌道を示す模式図。The schematic diagram which shows the teaching point and target track of two robots.

以下、一実施形態について、図面を参照しつつ説明する。本実施形態は、機械組立工場等において、ワークの搬送や機械の組み立て等を行うロボットシステムとして具体化している。 Hereinafter, an embodiment will be described with reference to the drawings. The present embodiment is embodied as a robot system that carries a workpiece, assembles a machine, and the like in a machine assembly factory or the like.

図１は、ロボットシステム１０の概要を示すブロック図である。ロボットシステム１０は、ロボットコントローラ２０、ロボット機構部４１（ロボット４０）、及び操作器５０の組を複数を備えている。 FIG. 1 is a block diagram showing an outline of a robot system 10. The robot system 10 includes a plurality of sets of a robot controller 20, a robot mechanism unit 41 (robot 40), and an operating unit 50.

ロボットコントローラ２０（ロボット制御装置）は、ロボット４０を全体的に制御するＣＰＵ２１を有している。このＣＰＵ２１には、バス２９を介してＲＯＭ２２、ＲＡＭ２３、不揮発性メモリ２４、ロボット軸制御部２５、通信モジュール２６、操作器インタフェース２７等が接続されている。 The robot controller 20 (robot control device) has a CPU 21 that controls the robot 40 as a whole. A ROM 22, a RAM 23, a non-volatile memory 24, a robot axis control unit 25, a communication module 26, an operation unit interface 27 and the like are connected to the CPU 21 via a bus 29.

ＲＯＭ２２には、ＣＰＵ２１が実行するシステムプログラムが格納されており、不揮発性メモリ２４には各種パラメータ等の設定値及びロボット４０が行う作業の教示プログラムが格納される。また、ＲＡＭ２３はデータの一時記憶等に利用される。ロボット軸制御部２５は、ロボット４０の各軸への移動指令に基づいて、各軸の位置、速度及びモータトルク（加速度）のフィードバック制御を行う。ロボット軸制御部２５は、サーボアンプ２８を介してロボット４０におけるロボット機構部４１の各軸のサーボモータを駆動制御する。通信モジュール２６は別のロボットコントローラと通信路で接続される。操作器インタフェース２７には、操作器５０（ティーチングペンダント）が接続されている。 The ROM 22 stores a system program to be executed by the CPU 21, and the non-volatile memory 24 stores setting values of various parameters and the like and a teaching program of an operation performed by the robot 40. The RAM 23 is also used for temporary storage of data. The robot axis control unit 25 performs feedback control of the position, the velocity, and the motor torque (acceleration) of each axis based on the movement command to the axis of the robot 40. The robot axis control unit 25 drives and controls servomotors of respective axes of the robot mechanism unit 41 in the robot 40 via the servo amplifier 28. The communication module 26 is connected to another robot controller via a communication path. An operating unit 50 (teaching pendant) is connected to the operating unit interface 27.

図２は、協調動作における２台のロボット４０Ａ，４０Ｂの動作量を示す斜視図である。図２（ａ）は協調動作開始前の状態を示しており、図２（ｂ）は協調動作終了後の状態を示している。ここでは、第１ロボット４０Ａと第２ロボット４０Ｂとにより、１つのワークＷが把持されており、このワークＷを変形させることなく、ワークＷをｘ軸周りに回転させる動作を行う。すなわち、両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係を維持しつつ、ワークＷを回転させる。 FIG. 2 is a perspective view showing the amount of movement of the two robots 40A and 40B in the cooperative operation. FIG. 2 (a) shows the state before the start of the cooperative operation, and FIG. 2 (b) shows the state after the end of the cooperative operation. Here, one work W is held by the first robot 40A and the second robot 40B, and the work W is rotated about the x-axis without being deformed. That is, the workpiece W is rotated while maintaining the relative position and posture relationship between the two robots 40A and 40B.

この場合、第２ロボット４０Ｂの動作量は、第１ロボット４０Ａの動作量よりも大きくなる。このため、第１ロボット４０Ａを高速で動作させると、両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係を維持しようとしても、第２ロボット４０Ｂの動作速度が上限速度を超えて第２ロボット４０Ｂが追従できなくなる。 In this case, the amount of movement of the second robot 40B is larger than the amount of movement of the first robot 40A. For this reason, when the first robot 40A is operated at high speed, the operating speed of the second robot 40B exceeds the upper limit speed and the second robot 40B even if trying to maintain the relative position and posture relationship between both robots 40A and 40B. Can not follow.

そこで、本実施形態では、第１ロボット４０Ａの動作目標（第１目標）に対して、上記相対的な位置及び姿勢関係を維持するように第２ロボット４０Ｂの動作目標（第２目標）を算出し、第２目標へ第２ロボット４０Ｂが動作する速度が上限速度を超えている場合に、第１ロボット４０Ａの現在の位置及び姿勢に上記第１目標を近付けるように第１目標を修正する。図３は、本実施形態におけるロボット４０Ａ，４０Ｂの協調動作制御を示すフローチャートである。この一連の処理は、Ｓ１１の処理を除いてＣＰＵ２１により実行される。 Therefore, in the present embodiment, the motion target (second target) of the second robot 40B is calculated so as to maintain the relative position and posture relationship with the motion target (first target) of the first robot 40A. If the speed at which the second robot 40B moves to the second target exceeds the upper limit speed, the first target is corrected so as to bring the first target closer to the current position and posture of the first robot 40A. FIG. 3 is a flowchart showing cooperative operation control of the robots 40A and 40B in the present embodiment. This series of processing is executed by the CPU 21 except for the processing of S11.

まず、ユーザは、第１ロボット４０Ａ及び第２ロボット４０Ｂにより、ワークＷを把持させる（Ｓ１１）。すなわち、図２（ａ）に示すように、ユーザは第１ロボット４０Ａの操作器５０を操作して第１ロボット４０ＡによりワークＷを把持させるとともに、第２ロボット４０Ｂの操作器５０を操作して第２ロボット４０ＢによりワークＷを把持させる。 First, the user causes the first robot 40A and the second robot 40B to hold the workpiece W (S11). That is, as shown in FIG. 2A, the user operates the operating unit 50 of the first robot 40A to hold the workpiece W by the first robot 40A, and operates the operating unit 50 of the second robot 40B. The workpiece W is held by the second robot 40B.

続いて、両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係Ｔｖを算出する（Ｓ１２）。詳しくは、以下の数式により、上記相対的な位置及び姿勢関係Ｔｖを算出する。 Subsequently, the relative position-attitude relationship Tv of both robots 40A and 40B is calculated (S12). Specifically, the relative position / posture relationship Tv is calculated by the following equation.

Ｔｖ＝（Ｔｒ２ｔ２）^−１×（Ｔｒ１ｒ２）^−１×（Ｔｒ１ｔ１）
ここで、図４に示すように、Ｔｒ２ｔ２は第２ロボット４０Ｂのベース座標系の原点Ｏ２の位置及び姿勢に対する第２ロボット４０Ｂのツール先端ｔ２の位置及び姿勢を特定する行列であり、Ｔｒ１ｒ２は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第２ロボット４０Ｂのベース座標系の原点Ｏ２の位置及び姿勢を特定する行列であり、Ｔｒ１ｔ１は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第１ロボット４０Ａのツール先端ｔ１の位置及び姿勢を特定する行列である。行列Ｔｒ２ｔ２等は４×４の同時変換行列であり、（Ｔｒ２ｔ２）^−１は行列Ｔｒ２ｔ２の逆行例を表す。 Tv = (Tr2t2) ^-1 * (Tr1r2) ^-1 * (Tr1t1)
Here, as shown in FIG. 4, Tr2t2 is a matrix that specifies the position and orientation of the tool tip t2 of the second robot 40B with respect to the position and orientation of the origin O2 of the base coordinate system of the second robot 40B. 1 is a matrix that specifies the position and orientation of the origin O2 of the base coordinate system of the second robot 40B with respect to the position and orientation of the origin O1 of the base coordinate system of the robot 40A; Tr1t1 is the origin O1 of the base coordinate system of the first robot 40A The position and orientation of the tool tip t1 of the first robot 40A with respect to the position and orientation of The matrices Tr2t2 and the like are 4 × 4 simultaneous transformation matrices, and (Tr2t2) ^-1 represents an example of a reverse row of the matrix Tr2t2.

続いて、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作があるか否か判定する（Ｓ１３）。この判定において、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作がないと判定した場合（Ｓ１３：ＮＯ）、この一連の処理を一端終了する（ＥＮＤ）。 Subsequently, it is determined whether or not there is an operation by the user to operate the first robot 40A on the controller 50 (S13). In this determination, when it is determined that the user has not made an operation to operate the first robot 40A with respect to the operation device 50 (S13: NO), this series of processing is ended at one end (END).

一方、Ｓ１３の判定において、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作があると判定した場合（Ｓ１３：ＹＥＳ）、操作器５０からの動作指令に基づいて第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を算出する（Ｓ１４）。詳しくは、操作器５０に対するユーザの動作操作（設定された速度で各軸を回転させる操作等）に対応する動作指令が、操作器５０から操作器インタフェース２７を介してＣＰＵ２１へ出力される。ＣＰＵ２１は、この動作指令（各軸の回転指令）に従って第１ロボット４０Ａを動作させるように、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊（目標位置及び目標姿勢）を算出する。この第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊は、第１ロボット４０Ａの１動作周期ｔｓ（１制御周期）後の動作目標である。 On the other hand, when it is determined in S13 that there is an operation by the user to operate the first robot 40A to the operating device 50 (S13: YES), the operation target of the first robot 40A based on the operation command from the operating device 50 Tr1t1 * is calculated (S14). Specifically, an operation command corresponding to the user's operation operation (operation of rotating each axis at a set speed, etc.) to the operation device 50 is output from the operation device 50 to the CPU 21 via the operation device interface 27. The CPU 21 calculates an operation target Tr1t1 * (target position and target attitude) of the first robot 40A so as to operate the first robot 40A according to the operation command (rotation command of each axis). The operation target Tr1t1 * of the first robot 40A is an operation target after one operation cycle ts (one control cycle) of the first robot 40A.

続いて、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊に対して、上記相対的な位置及び姿勢関係Ｔｖを維持するように、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊を算出する（Ｓ１５）。詳しくは、以下の数式により、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊を算出する。この第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊は、第２ロボット４０Ｂの１動作周期ｔｓ（１制御周期）後の動作目標である。 Subsequently, an operation target Tr2t2 * of the second robot 40B is calculated so as to maintain the relative position and posture relationship Tv with respect to the operation target Tr1t1 * of the first robot 40A (S15). Specifically, the motion target Tr2t2 * of the second robot 40B is calculated by the following formula. The operation target Tr2t2 * of the second robot 40B is an operation target after one operation cycle ts (one control cycle) of the second robot 40B.

Ｔｒ２ｔ２＊＝（Ｔｒ１ｒ２）^−１×（Ｔｒ１ｔ１＊）×（Ｔｖ）^−１
ここで、図５に示すように、（Ｔｒ１ｒ２）^−１は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第２ロボット４０Ｂのベース座標系の原点Ｏ２の位置及び姿勢を特定する行列の逆行例であり、Ｔｒ１ｔ１＊は第１ロボット４０Ａのベース座標系の原点Ｏ１の位置及び姿勢に対する第１ロボット４０Ａのツール先端ｔ１の目標位置及び目標姿勢（動作目標）を特定する行列であり、（Ｔｖ）^−１は上記対的な位置及び姿勢関係を特定する行列の逆行列である。 Tr2t2 * = (Tr1r2) ^-1 * (Tr1t1 *) * (Tv) ^-1
Here, as shown in FIG. 5, (Tr1r2) ^-1 specifies the position and orientation of the origin O2 of the base coordinate system of the second robot 40B with respect to the position and orientation of the origin O1 of the base coordinate system of the first robot 40A. Tr1t1 * is a matrix for specifying the target position and target attitude (operation target) of the tool tip t1 of the first robot 40A with respect to the position and attitude of the origin O1 of the base coordinate system of the first robot 40A. , (Tv) ⁻¹ is an inverse matrix of the above matrix specifying the relative position and attitude relationship.

続いて、１動作周期ｔｓ後に動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する速度Ｖ２が、上限速度Ｖ２ｍｘを超えているか否か判定する（Ｓ１６）。具体的には、速度Ｖ２は、１動作周期ｔｓと動作目標Ｔｒ２ｔ２＊とに基づいて算出される。上限速度Ｖ２ｍｘは、第２ロボット４０Ｂの仕様に基づいて設定されている。 Subsequently, it is determined whether the speed V2 at which the second robot 40B operates to the operation target Tr2t2 * after one operation cycle ts exceeds the upper limit speed V2mx (S16). Specifically, the speed V2 is calculated based on one operation cycle ts and the operation target Tr2t2 *. The upper limit speed V2 mx is set based on the specification of the second robot 40B.

Ｓ１６の判定において、速度Ｖ２が上限速度Ｖ２ｍｘを超えていると判定された場合（Ｓ１６：ＹＥＳ）、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊を修正する（Ｓ１７）。詳しくは、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を大きくする。すなわち、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの動作量を小さくする。その後、Ｓ１５の処理から再度実行し、修正された動作目標Ｔｒ１ｔ１＊に対して、上記相対的な位置及び姿勢関係Ｔｖを維持するように、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊を算出する。 If it is determined in S16 that the velocity V2 exceeds the upper limit velocity V2 mx (S16: YES), the operation target Tr1t1 * of the first robot 40A approaches the current position and orientation of the first robot 40A. The operation target Tr1t1 * is corrected (S17). Specifically, the amount by which the operation target Tr1t1 * approaches the current position and posture of the first robot 40A is increased as the amount by which the velocity V2 of the second robot 40B exceeds the upper limit velocity V2mx is larger. That is, as the amount by which the velocity V2 of the second robot 40B exceeds the upper limit velocity V2mx is larger, the amount of movement of the first robot 40A is reduced. Thereafter, the processing of S15 is executed again, and the motion target Tr2t2 * of the second robot 40B is calculated so as to maintain the relative position and posture relationship Tv with respect to the corrected motion target Tr1t1 *.

一方、Ｓ１６の判定において、速度Ｖ２が上限速度Ｖ２ｍｘを超えていないと判定された場合（Ｓ１６：ＮＯ）、動作目標Ｔｒ１ｔ１＊へ第１ロボット４０Ａを動作させ、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂを動作させる（Ｓ１８）。 On the other hand, when it is determined in S16 that the velocity V2 does not exceed the upper limit velocity V2 mx (S16: NO), the first robot 40A is operated to the operation target Tr1t1 *, and the second robot 40B is operated to the operation target Tr2t2 * Is operated (S18).

続いて、ユーザによる操作器５０に対する第１ロボット４０Ａを動作させる操作が終了したか否か判定する（Ｓ１９）。この判定において、ユーザによる操作器５０に対する動作操作が終了していないと判定した場合（Ｓ１９：ＮＯ）、Ｓ１４の処理から再度実行する。 Subsequently, it is determined whether or not the operation by the user to operate the first robot 40A with respect to the operation device 50 has ended (S19). In this determination, when it is determined that the operation operation on the operation device 50 by the user is not completed (S19: NO), the process is executed again from the process of S14.

一方、Ｓ１９の判定において、ユーザによる操作器５０に対する動作操作が終了したと判定した場合（Ｓ１９：ＹＥＳ）、操作器５０からの停止指令に基づいて第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を算出する（Ｓ２０）。詳しくは、操作器５０に対するユーザの停止操作（各軸を回転させる操作等の終了）に対応する停止指令が、操作器５０から操作器インタフェース２７を介してＣＰＵ２１へ出力される。ＣＰＵ２１は、この停止指令（各軸の回転停止指令）に従って第１ロボット４０Ａを停止させるように、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊（目標位置及び目標姿勢）を算出する。 On the other hand, if it is determined in S19 that the user's operation on the operation unit 50 has ended (S19: YES), the operation target Tr1t1 * of the first robot 40A is calculated based on the stop command from the operation unit 50. (S20). Specifically, a stop command corresponding to the user's stop operation (end of operation for rotating each axis, etc.) on the operation unit 50 is output from the operation unit 50 to the CPU 21 via the operation unit interface 27. The CPU 21 calculates an operation target Tr1t1 * (target position and target attitude) of the first robot 40A so as to stop the first robot 40A according to the stop command (rotation stop command of each axis).

続いて、上記Ｓ１５〜Ｓ１８の処理と同様に、Ｓ２１〜Ｓ２４の処理を実行する。そして、第１ロボット４０Ａ及び第２ロボット４０Ｂが停止したか否か判定する（Ｓ２５）。この判定において、第１ロボット４０Ａ及び第２ロボット４０Ｂが停止していないと判定した場合（Ｓ２５：ＮＯ）、Ｓ２０の処理から再度実行する。一方、この判定において、第１ロボット４０Ａ及び第２ロボット４０Ｂが停止したと判定した場合（Ｓ２５：ＹＥＳ）、この一連の処理を一旦終了する（ＥＮＤ）。 Subsequently, the processes of S21 to S24 are performed in the same manner as the processes of S15 to S18. Then, it is determined whether or not the first robot 40A and the second robot 40B have stopped (S25). In this determination, when it is determined that the first robot 40A and the second robot 40B are not stopped (S25: NO), the processing from S20 is executed again. On the other hand, in this determination, when it is determined that the first robot 40A and the second robot 40B have stopped (S25: YES), this series of processing is temporarily ended (END).

なお、Ｓ１４及びＳ２０の処理が第１目標算出手段としての処理に相当し、Ｓ１５及びＳ２１の処理が第２目標算出手段としての処理に相当し、Ｓ１６及びＳ２２の処理が判定手段としての処理に相当し、Ｓ１７及びＳ２３の処理が第１目標修正手段としての処理に相当し、Ｓ１８及びＳ２４の処理が動作手段としての処理に相当する。 The processes of S14 and S20 correspond to the process of the first target calculating means, the processes of S15 and S21 correspond to the process of the second target calculating means, and the processes of S16 and S22 to the process of the determining means. Correspondingly, the processes of S17 and S23 correspond to the process as the first target correction means, and the processes of S18 and S24 correspond to the process as the operation means.

以上詳述した本実施形態は、以下の利点を有する。 The embodiment described above has the following advantages.

・動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する速度Ｖ２が上限速度Ｖ２ｍｘを超えているか否か判定される。そして、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えていないと判定された場合は、動作目標Ｔｒ１ｔ１＊へ第１ロボット４０Ａが動作させられ、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂを動作させられる。これにより、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持しつつ、第１ロボット４０Ａ及び第２ロボット４０Ｂをそれぞれ動作目標Ｔｒ１ｔ１＊及び動作目標Ｔｒ２ｔ２＊へ動作させることができる。その結果、第１ロボット４０Ａ及び第２ロボット４０Ｂにより、１つのワークＷを把持して搬送する協調動作等を行うことができる。 It is determined whether the speed V2 at which the second robot 40B operates to the operation target Tr2t2 * exceeds the upper limit speed V2mx. When it is determined that the velocity V2 of the second robot 40B does not exceed the upper limit velocity V2 mx, the first robot 40A is operated to the operation target Tr1t1 *, and the second robot 40B is operated to the operation target Tr2t2 *. Be Thus, the first robot 40A and the second robot 40B are operated to the operation target Tr1t1 * and the operation target Tr2t2 * while maintaining the relative position-attitude relationship Tv of the first robot 40A and the second robot 40B. Can. As a result, the first robot 40A and the second robot 40B can perform a coordinated operation or the like for gripping and transporting one work W.

・第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えていると判定された場合は、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊が修正される。そして、修正された動作目標Ｔｒ１ｔ１＊に対して、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持するように動作目標Ｔｒ２ｔ２＊が算出される。このため、修正される前の動作目標Ｔｒ１ｔ１＊に対する動作目標Ｔｒ２ｔ２＊と比較して、修正された後の動作目標Ｔｒ１ｔ１＊に対する動作目標Ｔｒ２ｔ２＊を、第２ロボット４０Ｂの現在の位置及び姿勢に近付けることができる。したがって、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する速度Ｖ２を低下させることができ、第１ロボット４０Ａの動作に第２ロボット４０Ｂの動作を追従させることができる。 When it is determined that the velocity V2 of the second robot 40B exceeds the upper limit velocity V2 mx, the operation target Tr1 t1 * is corrected so that the operation target Tr1 t1 * approaches the current position and orientation of the first robot 40 A . Then, with respect to the corrected motion target Tr1t1 *, the motion target Tr2t2 * is calculated so as to maintain the relative position-posture relationship Tv of the first robot 40A and the second robot 40B. For this reason, compared with the operation target Tr2t2 * for the operation target Tr1t1 * before being corrected, the operation target Tr2t2 * for the operation target Tr1t1 * after being corrected is made closer to the current position and posture of the second robot 40B. be able to. Therefore, the speed V2 at which the second robot 40B operates can be reduced to the operation target Tr2t2 *, and the operation of the second robot 40B can follow the operation of the first robot 40A.

・第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量が大きくされる。このため、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量に応じて、適切に動作目標Ｔｒ１ｔ１＊を修正することができる。 The amount by which the operation target Tr1t1 * approaches the current position and orientation of the first robot 40A is increased as the amount by which the velocity V2 of the second robot 40B exceeds the upper limit velocity V2mx is larger. Therefore, it is possible to appropriately correct the operation target Tr1t1 * according to the amount by which the velocity V2 of the second robot 40B exceeds the upper limit velocity V2 mx.

・図４に示すように、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを、行列を用いて容易に特定することができる。さらに、図５に示すように、動作目標Ｔｒ２ｔ２＊を、相対的な位置及び姿勢関係Ｔｖを表す行列を用いて容易に特定することができる。 As shown in FIG. 4, the relative position and orientation relationship Tv of the first robot 40A and the second robot 40B can be easily identified using a matrix. Furthermore, as shown in FIG. 5, the motion target Tr2t2 * can be easily identified using a matrix representing the relative position-attitude relationship Tv.

・第１ロボット４０Ａの動作を手動操作する操作器５０からの指令に基づいて、動作周期ｔｓ毎に、第１ロボット４０Ａの動作目標である動作目標Ｔｒ１ｔ１＊が算出される。このため、ユーザが操作器５０により第１ロボット４０Ａを手動操作する際に、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持しつつ、第１ロボット４０Ａ及び第２ロボット４０Ｂをそれぞれ動作させることができる。 The movement target Tr1t1 *, which is the movement target of the first robot 40A, is calculated for each movement cycle ts based on a command from the controller 50 for manually operating the movement of the first robot 40A. Therefore, when the user manually operates the first robot 40A with the operation device 50, the first robot 40A and the second robot 40A are maintained while maintaining the relative position and posture relationship Tv of the first robot 40A and the second robot 40B. The robots 40B can be operated respectively.

なお、上記実施形態を、以下のように変更して実施することもできる。 The above embodiment can be modified as follows.

・上記実施形態では、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を大きくした。これに対して、第２ロボット４０Ｂの速度Ｖ２が上限速度Ｖ２ｍｘを超えている量にかかわらず、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を一定にすることもできる。 In the above embodiment, the amount by which the operation target Tr1t1 * approaches the current position and posture of the first robot 40A is increased as the amount by which the velocity V2 of the second robot 40B exceeds the upper limit velocity V2mx is larger. On the other hand, regardless of the amount by which the velocity V2 of the second robot 40B exceeds the upper limit velocity V2 mx, the amount by which the operation target Tr1t1 * approaches the current position and posture of the first robot 40A can be made constant. .

・上記実施形態では、速度Ｖ２が上限速度Ｖ２ｍｘを超えていると判定された場合（Ｓ１６：ＹＥＳ）、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊を修正した（Ｓ１７）。これに対して、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように、Ｓ１４において動作目標Ｔｒ１ｔ１＊を再算出してもよい。この場合も、第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊が修正されることとなる。また、動作目標Ｔｒ１ｔ１＊が修正される場合に、修正される前の動作目標Ｔｒ１ｔ１＊を記憶しておいてもよい。そして、記憶しておいた修正される前の動作目標Ｔｒ１ｔ１＊を、修正された動作目標Ｔｒ１ｔ１＊の次動作周期での動作目標Ｔｒ１ｔ１＊とすることにより、ＣＰＵ２１の演算負荷を軽減することができる。 In the above embodiment, when it is determined that the velocity V2 exceeds the upper limit velocity V2 mx (S16: YES), the operation target Tr1t1 * of the first robot 40A approaches the current position and orientation of the first robot 40A. The operation target Tr1t1 * is corrected to (S17). On the other hand, the motion target Tr1t1 * may be recalculated in S14 so that the motion target Tr1t1 * of the first robot 40A approaches the current position and orientation of the first robot 40A. Also in this case, the operation target Tr1t1 * is corrected so that the operation target Tr1t1 * of the first robot 40A approaches the current position and attitude of the first robot 40A. Further, when the operation target Tr1t1 * is corrected, the operation target Tr1t1 * before being corrected may be stored. The operation load of the CPU 21 can be reduced by setting the stored operation target Tr1t1 * before correction as the operation target Tr1t1 * in the next operation cycle of the corrected operation target Tr1t1 *. .

・図３のＳ１６及びＳ２２を、以下のように変更することもできる。すなわち、１動作周期ｔｓ後に動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する加速度Ａ２が、上限加速度Ａ２ｍｘを超えているか否か判定する。具体的には、加速度Ａ２は、１動作周期ｔｓと動作目標Ｔｒ２ｔ２＊とに基づいて算出される。上限加速度Ａ２ｍｘは、第２ロボット４０Ｂの仕様に基づいて設定されている。 S16 and S22 of FIG. 3 can be modified as follows. That is, it is determined whether or not the acceleration A2 at which the second robot 40B moves to the operation target Tr2t2 * after one operation cycle ts exceeds the upper limit acceleration A2mx. Specifically, the acceleration A2 is calculated based on one operation cycle ts and the operation target Tr2t2 *. The upper limit acceleration A2 mx is set based on the specification of the second robot 40B.

そして、Ｓ１７及びＳ２３を以下のように変更することができる。第１ロボット４０Ａの現在の位置及び姿勢に第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を近付けるように動作目標Ｔｒ１ｔ１＊を修正する。詳しくは、第２ロボット４０Ｂの加速度Ａ２が上限加速度Ａ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの現在の位置及び姿勢に動作目標Ｔｒ１ｔ１＊を近付ける量を大きくする。すなわち、第２ロボット４０Ｂの加速度Ａ２が上限加速度Ａ２ｍｘを超えている量が大きいほど、第１ロボット４０Ａの動作量を小さくする。 And S17 and S23 can be changed as follows. The motion target Tr1t1 * is corrected so that the motion target Tr1t1 * of the first robot 40A approaches the current position and posture of the first robot 40A. Specifically, as the amount by which the acceleration A2 of the second robot 40B exceeds the upper limit acceleration A2mx is larger, the amount by which the operation target Tr1t1 * approaches the current position and posture of the first robot 40A is increased. That is, as the amount of acceleration A2 of the second robot 40B exceeding the upper limit acceleration A2 mx is larger, the amount of movement of the first robot 40A is reduced.

こうした構成によれば、動作目標Ｔｒ２ｔ２＊へ第２ロボット４０Ｂが動作する加速度Ａ２を低下させることができ、第１ロボット４０Ａの動作に第２ロボット４０Ｂの動作を追従させることができる。さらに、第２ロボット４０Ｂの加速度Ａ２が上限加速度Ａ２ｍｘを超えている量に応じて、適切に動作目標Ｔｒ１ｔ１＊を修正することができる。なお、加速度Ａ２は、速度を上昇させる時の加速度であってもよいし、速度を低下させる時の減速度であってもよい。 According to such a configuration, the acceleration A2 at which the second robot 40B operates can be reduced to the operation target Tr2t2 *, and the operation of the second robot 40B can follow the operation of the first robot 40A. Furthermore, the operation target Tr1t1 * can be appropriately corrected according to the amount by which the acceleration A2 of the second robot 40B exceeds the upper limit acceleration A2 mx. The acceleration A2 may be an acceleration when increasing the speed, or may be a deceleration when reducing the speed.

・図６は、ロボット４０Ａ，４０Ｂの協調動作制御の変更例を示すフローチャートである。この一連の処理は、Ｓ３１の処理を除いてＣＰＵ２１により実行される。 FIG. 6 is a flowchart showing a modified example of the coordinated operation control of the robots 40A and 40B. This series of processing is executed by the CPU 21 except for the processing of S31.

まず、ユーザは、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊として、全ての教示点ｔ１１〜ｔ１ｅを教示する（Ｓ３１）。すなわち、図７に示すように、ユーザは第１ロボット４０Ａの操作器５０を操作して、ロボット４０Ａ，４０ＢによりワークＷを把持させて協調動作を行う際の第１ロボット４０Ａの各動作目標Ｔｒ１ｔ１＊として、教示点ｔ１１〜ｔ１ｅを教示する。 First, the user teaches all the teaching points t11 to t1e as the operation target Tr1t1 * of the first robot 40A (S31). That is, as shown in FIG. 7, the user operates the operation unit 50 of the first robot 40A to hold the workpiece W by the robots 40A and 40B and perform the coordinated operation. Each operation target Tr1t1 of the first robot 40A The teaching points t11 to t1 e are taught as *.

続いて、協調動作の開始点ｔ１０，ｔ２０において両ロボット４０Ａ，４０Ｂの相対的な位置及び姿勢関係Ｔｖを算出する（Ｓ３２）。Ｓ３２の処理は、図６のＳ１２の処理と同様である。ただし、両ロボット４０Ａ，４０ＢによりワークＷを把持させておらず、両ロボット４０Ａ，４０Ｂはバーチャルツールで接続されていると考えることができる。 Subsequently, the relative position-attitude relationship Tv of both robots 40A and 40B is calculated at the start points t10 and t20 of the coordinated operation (S32). The process of S32 is similar to the process of S12 of FIG. However, it can be considered that the two robots 40A and 40B are connected by virtual tools without holding the work W by the two robots 40A and 40B.

続いて、教示された教示点ｔ１１〜ｔ１ｅに基づいて第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊を算出する（Ｓ３３）。詳しくは、各教示点の間を補間して、教示点間の第１ロボット４０Ａの速度パターンを算出する。そして、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊（目標位置及び目標姿勢）を算出する。この第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊は、第１ロボット４０Ａの１動作周期ｔｓ（１制御周期）後の動作目標である。 Subsequently, an operation target Tr1t1 * of the first robot 40A is calculated based on the taught points t11 to t1e taught (S33). Specifically, the velocity pattern of the first robot 40A between the teaching points is calculated by interpolating between the teaching points. Then, the motion target Tr1t1 * (target position and target attitude) of the first robot 40A is calculated. The operation target Tr1t1 * of the first robot 40A is an operation target after one operation cycle ts (one control cycle) of the first robot 40A.

続いて、図３のＳ１５〜Ｓ１７の処理と同様に、Ｓ３４〜Ｓ３６の処理を実行する。そして、Ｓ３５の判定において、速度Ｖ２が上限速度Ｖ２ｍｘを超えていないと判定された場合（Ｓ３５：ＮＯ）、動作目標Ｔｒ１ｔ１＊及び動作目標Ｔｒ２ｔ２＊を不揮発性メモリ２４に記録する（Ｓ３７）。すなわち、ロボット４０Ａ，４０Ｂを動作させる代わりに、ロボット４０Ａ，４０Ｂの各制御周期ｔｓにおける動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊を記録する。 Subsequently, the processes of S34 to S36 are executed as in the processes of S15 to S17 of FIG. 3. When it is determined in S35 that the velocity V2 does not exceed the upper limit velocity V2 mx (S35: NO), the operation target Tr1t1 * and the operation target Tr2t2 * are recorded in the non-volatile memory 24 (S37). That is, instead of operating the robots 40A and 40B, motion targets Tr1t1 * and Tr2t2 * in the control cycles ts of the robots 40A and 40B are recorded.

続いて、動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊の算出及び記録が、最後の教示点ｔ１ｅ，ｔ２ｅに到達したか否か判定する（Ｓ３８）。すなわち、図７に示すように、全ての教示点ｔ１１〜ｔ１ｅに対して、第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊（動作点ｔ２１〜ｔ２ｅ）を算出したか否か判定する。この判定において、動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊の算出及び記録が、最後の教示点ｔ１ｅ，ｔ２ｅに到達していないと判定した場合（Ｓ３８：ＮＯ）、Ｓ３３の処理から再度実行する。一方、この判定において、動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊の算出及び記録が、最後の教示点ｔ１ｅ，ｔ２ｅに到達したと判定した場合（Ｓ３８：ＹＥＳ）、この一連の処理を一旦終了する（ＥＮＤ）。 Subsequently, it is determined whether or not calculation and recording of the operation targets Tr1t1 * and Tr2t2 * have reached the last teaching points t1e and t2e (S38). That is, as shown in FIG. 7, it is determined whether or not the operation target Tr2t2 * (the operation point t21 to t2e) of the second robot 40B has been calculated for all the teaching points t11 to t1e. In this determination, when it is determined that the calculation and recording of the operation targets Tr1t1 * and Tr2t2 * have not reached the last teaching points t1e and t2e (S38: NO), the processing from S33 is performed again. On the other hand, in this determination, when it is determined that the calculation and recording of operation targets Tr1t1 * and Tr2t2 * have reached the last teaching point t1e and t2e (S38: YES), this series of processing is temporarily ended (END) .

なお、Ｓ３３の処理が第１目標算出手段としての処理に相当し、Ｓ３４の処理が第２目標算出手段としての処理に相当し、Ｓ３５の処理が判定手段としての処理に相当し、Ｓ３６の処理が第１目標修正手段としての処理に相当する。 The process of S33 corresponds to the process of the first target calculating means, the process of S34 corresponds to the process of the second target calculating means, the process of S35 corresponds to the process of the determining means, and the process of S36 Corresponds to the processing as the first target correction means.

こうした構成によれば、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊として教示された教示点ｔ１１〜ｔ１ｅに基づいて、動作周期ｔｓ毎に、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊が算出される。このため、第１ロボット４０Ａの動作目標Ｔｒ１ｔ１＊として教示点ｔ１１〜ｔ１ｅを教示することにより、第１ロボット４０Ａ及び第２ロボット４０Ｂの相対的な位置及び姿勢関係Ｔｖを維持しつつ、第１ロボット４０Ａ及び第２ロボット４０Ｂの動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊を算出して記録することができる。ひいては、記録された動作目標Ｔｒ１ｔ１＊，Ｔｒ２ｔ２＊を用いた自動動作により、第１ロボット４０Ａ及び第２ロボット４０Ｂを協調動作させることができる。 According to such a configuration, the operation target Tr1t1 * of the first robot 40A is calculated for each operation cycle ts based on the teaching points t11 to t1e taught as the operation target Tr1t1 * of the first robot 40A. For this reason, by teaching the teaching points t11 to t1e as the operation target Tr1t1 * of the first robot 40A, the first robot can be maintained while maintaining the relative position / posture relationship Tv of the first robot 40A and the second robot 40B. The motion targets Tr1t1 * and Tr2t2 * of the 40A and the second robot 40B can be calculated and recorded. As a result, the first robot 40A and the second robot 40B can operate in cooperation by the automatic operation using the recorded operation targets Tr1t1 * and Tr2t2 *.

・第２ロボット４０Ｂの動作目標Ｔｒ２ｔ２＊が第２ロボット４０Ｂの動作可能な位置及び姿勢でない場合に、第１ロボット４０Ａ及び第２ロボット４０Ｂの動作を禁止する禁止手段を備えてもよい。こうした構成によれば、禁止手段により、動作目標Ｔｒ２ｔ２＊が第２ロボット４０Ｂの動作可能な位置及び姿勢でない場合に、第１ロボット４０Ａ及び第２ロボット４０Ｂの動作が禁止される。このため、第１ロボット４０Ａの動作に第２ロボット４０Ｂの動作が追従できない場合に、第１ロボット４０Ａだけが動作させられることを避けることができる。したがって、第１ロボット４０Ａ及び第２ロボット４０Ｂにより把持したワークＷ等が破損することを抑制することができる。 In the case where the operation target Tr2t2 * of the second robot 40B is not the operable position and posture of the second robot 40B, the second robot 40B may be provided with prohibiting means for prohibiting the operations of the first robot 40A and the second robot 40B. According to such a configuration, the operation of the first robot 40A and the second robot 40B is prohibited by the prohibiting means when the operation target Tr2t2 * is not the operable position and posture of the second robot 40B. Therefore, when the operation of the second robot 40B can not follow the operation of the first robot 40A, it can be avoided that only the first robot 40A is operated. Therefore, it is possible to suppress breakage of the workpiece W or the like held by the first robot 40A and the second robot 40B.

・ロボットコントローラ２０は、各ロボット４０Ａ，４０Ｂを制御するものでもよいし、ロボット４０Ａ，４０Ｂを統括して制御するものでもよい。 The robot controller 20 may control the robots 40A and 40B, or may control the robots 40A and 40B in an integrated manner.

・ロボットは２台に限らず、３台以上であってもよい。その場合は、１台を第１ロボット４０Ａとし、その他の少なくとも１台を第２ロボット４０Ｂとすればよい。 The number of robots is not limited to two, and may be three or more. In that case, one may be the first robot 40A, and at least one other may be the second robot 40B.

２０…ロボットコントローラ（ロボット制御装置）、４０…ロボット、４０Ａ…第１ロボット、４０Ｂ…第２ロボット、５０…操作器。 20: robot controller (robot control device) 40: robot 40A: first robot 40B: second robot 50: controller

Claims

A robot controller for controlling a first robot and a second robot, wherein
First target calculating means for calculating a first target which is an operation target of the first robot in each operation cycle;
A second target calculation for calculating a second target which is an operation target of the second robot so as to maintain a relative position and posture relationship between the first robot and the second robot with respect to the first target Means,
Determining means for determining whether the speed at which the second robot operates to the second target exceeds the upper limit speed;
If the determination means determines that the speed of the second robot exceeds the upper limit speed, the first target is corrected so that the first target approaches the current position and posture of the first robot. First target correction means to
Operating means for operating the first robot to the first target and operating the second robot to the second target;
Equipped with
The relative position and orientation relationship is determined based on the position and orientation of the origin of the base coordinate system of the second robot in a state in which one workpiece is held by the first robot and the second robot. Inverse matrix of matrix specifying position and attitude, inverse matrix of matrix specifying position and attitude of origin of base coordinate system of the second robot with respect to position and attitude of origin of base coordinate system of the first robot, and the matrix A robot control apparatus characterized in that it is specified by a matrix obtained by multiplying the position and posture of the origin of the base coordinate system of the first robot with the matrix specifying the position and posture of the first robot .

The first target correction means increases the amount by which the first target approaches the current position and posture of the first robot as the amount by which the speed of the second robot exceeds the upper limit speed increases. The robot control device according to 1.

A robot controller for controlling a first robot and a second robot, wherein
First target calculating means for calculating a first target which is an operation target of the first robot in each operation cycle;
A second target calculation for calculating a second target which is an operation target of the second robot so as to maintain a relative position and posture relationship between the first robot and the second robot with respect to the first target Means,
Determining means for determining whether the acceleration at which the second robot operates to the second target exceeds the upper limit acceleration;
If the determination means determines that the acceleration of the second robot exceeds the upper limit acceleration, the first target is corrected so that the first target approaches the current position and posture of the first robot. First target correction means to
Operating means for operating the first robot to the first target and operating the second robot to the second target;
Equipped with
The relative position and orientation relationship is determined based on the position and orientation of the origin of the base coordinate system of the second robot in a state in which one workpiece is held by the first robot and the second robot. Inverse matrix of matrix specifying position and attitude, inverse matrix of matrix specifying position and attitude of origin of base coordinate system of the second robot with respect to position and attitude of origin of base coordinate system of the first robot, and the matrix A robot control apparatus characterized in that it is specified by a matrix obtained by multiplying the position and posture of the origin of the base coordinate system of the first robot with the matrix specifying the position and posture of the first robot .

The first target correction means increases the amount by which the first target approaches the current position and posture of the first robot as the amount by which the acceleration of the second robot exceeds the upper limit acceleration is larger. The robot control device according to 3.

The second target is an inverse matrix of a matrix specifying the position and orientation of the origin of the base coordinate system of the second robot with respect to the position and orientation of the origin of the base coordinate system of the first robot, and the base coordinates of the first robot origin position and matrix specifying the position and orientation of the first robot with respect to the attitude of the system, as well as claim 1 which is specified by a matrix obtained by multiplying an inverse matrix of the matrix that identifies the relative position and orientation relationship The robot control device according to any one of 4 .

The method according to any one of claims 1 to 5 , further comprising prohibiting means for prohibiting the operation of the first robot and the second robot when the second target is not the operable position and posture of the second robot. Robot control unit.

The first target calculation means calculates a first target, which is an operation target of the first robot, for each operation cycle, based on a command from an operating device that manually operates the operation of the first robot. the robot control apparatus according to any one of 1-6.

The first target calculating means, based on the teaching point taught as an operation target of the first robot, in each operating cycle, claim to calculate a first target is an operation target of the first robot 1-6 The robot control device according to any one of the above.