JPH07205072A - Cooperation controller of plural robot - Google Patents
Cooperation controller of plural robotInfo
- Publication number
- JPH07205072A JPH07205072A JP682094A JP682094A JPH07205072A JP H07205072 A JPH07205072 A JP H07205072A JP 682094 A JP682094 A JP 682094A JP 682094 A JP682094 A JP 682094A JP H07205072 A JPH07205072 A JP H07205072A
- Authority
- JP
- Japan
- Prior art keywords
- equation
- end effector
- robot
- robots
- work
- 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.)
- Pending
Links
Landscapes
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Abstract
Description
【０００１】[0001]
【産業上の利用分野】本発明は、複数ロボットの協調制
御装置に係わり、特に単一の搬送ワークを複数ロボット
のハンドにより把持して運搬する複数ロボットの協調制
御装置に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a coordinated control system for a plurality of robots, and more particularly to a coordinated control system for a plurality of robots that grips and conveys a single transfer work by the hands of the plurality of robots.
【０００２】[0002]
【従来の技術】産業用ロボットを使用して重量物長物ワ
ークの搬送・組立の作業を行う場合、従来は、位置決め
精度を補償する機能を持ったハンドをワークの大きさに
あわせて使用していた。このため、ハンドが巨大で重量
の大きなものとなり、ワーク重量に対して過剰な可搬重
量を持つ巨大なロボットを必要としていた。このような
問題を解決するため、２台のロボットにより作業させる
ことが提案されている（例えば、特開平３−１３８２２
０号公報参照）。2. Description of the Related Art When an industrial robot is used to carry and assemble heavy-duty long workpieces, conventionally, a hand having a function of compensating for positioning accuracy is used according to the size of the workpiece. It was For this reason, the hand becomes huge and heavy, and a huge robot having an excessive portable weight with respect to the work weight is required. In order to solve such a problem, it has been proposed to work with two robots (for example, Japanese Patent Laid-Open No. 3-13822).
No. 0 publication).
【０００３】[0003]
【発明が解決しようとする課題】しかしながら、このよ
うに２台のロボットを用いてワークの搬送・組立の作業
を行う場合、先ず、ロボット相互の位置ずれにより、ワ
ークに内力が発生し、この内力が大きくなった場合に
は、ワークやロボットが破損する場合がある。このよう
なワーク内力によるワークやロボットの破損を防止する
ため、上記特許公報には、一方のロボットのハンドにフ
ローティング機構を設け、このフローティング機構によ
り位置ずれを吸収するようにしたものが提案されてい
る。しかしながら、この公報に記載されたものは、ワー
ク搬送時にフローティング機構により振動が発生し、こ
の振動により、搬送・組立の作業を行うことができなく
なる場合がある。さらに、ロボット自体が位置ずれを起
こすと、ワークの位置ずれを防止することができず、そ
のため、搬送・組立の作業を行うことが出来なくなる。
そこで、本発明は、上記の従来の技術の問題点を解決す
るためになされたものであり、ワークに内力が作用する
ことがなく且つロボットの位置ずれが生じてものワーク
の位置ずれを防止することができる複数ロボットの協調
制御装置を提供することを目的としている。However, when the work of transferring and assembling the work is performed using the two robots as described above, first, an internal force is generated in the work due to the positional displacement between the robots, and the internal force is generated. If the size becomes large, the work or robot may be damaged. In order to prevent the damage of the work or the robot due to such internal force of the work, the above patent publication proposes that one robot hand is provided with a floating mechanism to absorb the positional deviation. There is. However, in the device described in this publication, vibration may occur due to the floating mechanism when the work is transferred, and this vibration may make it impossible to perform the work of transfer / assembly. Furthermore, if the robot itself is displaced, the displacement of the work cannot be prevented, and therefore, the work of transfer / assembly cannot be performed.
Therefore, the present invention has been made in order to solve the above-mentioned problems of the conventional technique, and prevents the work from being displaced even if the internal force does not act on the work and the robot is displaced. It is an object of the present invention to provide a coordinated control device for a plurality of robots capable of performing.
【０００４】[0004]
【課題を解決するための手段】上記の目的を達成するた
め本発明は、単一の搬送ワークを複数ロボットのハンド
により把持して運搬する複数ロボットの協調制御装置に
おいて、複数ロボットの各ハンドの手首部に取り付けら
れた力センサ及びエンドエフェクタと、上記各ロボット
自体を制御するロボットコントローラと、上記エンドエ
フェクタを制御するエンドエフェクタコントローラと、
を有し、このエンドエフェクタコントローラが、ロボッ
トの位置、所定の搬送目標軌道に基づく搬送時の力セン
サからの信号及び搬送ワークの目標軌道に基づき上記エ
ンドエフェクタを作動制御させることを特徴としてい
る。このように構成された本発明の複数ロボットの協調
制御装置においては、単一の搬送ワークを複数ロボット
のハンドにより把持して運搬する場合、複数ロボットの
各ハンドの手首部に力センサ及びエンドエフェクタを取
り付けて、このエンドエフェクタをロボットの位置、所
定の搬送目標軌道に基づく搬送時の力センサからの信号
及び搬送ワークの目標軌道に基づき作動制御するように
している。このため、搬送ワークを搬送中にロボットが
目標軌道からずれても、その位置ずれを検出してエンド
エフェクタにより修正することができる。この結果、ロ
ボットの目標軌道に対する位置ずれが防止される。さら
に、エンドエフェクタの作動により、搬送ワークに内力
が生じることもない。In order to achieve the above object, the present invention provides a cooperative control apparatus for a plurality of robots which grips and carries a single transfer work by the hands of a plurality of robots. A force sensor and an end effector attached to the wrist, a robot controller that controls the robots themselves, and an end effector controller that controls the end effector,
The end effector controller controls the operation of the end effector based on the position of the robot, a signal from a force sensor during transfer based on a predetermined transfer target track, and the target track of the transfer work. In the coordinated control apparatus for a plurality of robots of the present invention thus configured, when a single transfer work is gripped and carried by the hands of a plurality of robots, a force sensor and an end effector are provided on the wrists of the respective hands of the plurality of robots. Is attached to control the operation of the end effector based on the position of the robot, the signal from the force sensor during the transportation based on a predetermined transport target trajectory, and the target trajectory of the transport work. Therefore, even if the robot deviates from the target trajectory during the conveyance of the conveyed work, the positional deviation can be detected and corrected by the end effector. As a result, displacement of the robot with respect to the target trajectory is prevented. Further, the operation of the end effector does not generate an internal force on the conveyed work.
【０００５】また、本発明においては、さらに、制御ゲ
イン設定手段を有し、この制御ゲイン設定手段が、エン
ドエフェクタと上記目標軌道との誤差がロボットのハン
ドと上記目標軌道と誤差より速く収束するように制御ゲ
インを設定することが好ましい。Further, the present invention further comprises a control gain setting means, and the control gain setting means converges the error between the end effector and the target trajectory faster than the error between the robot hand and the target trajectory. It is preferable to set the control gain as follows.
【０００６】[0006]
【実施例】以下、添付図面を参照して本発明の一実施例
について説明する。図１は、本発明の複数ロボットの協
調制御装置の一実施例を示す全体構成図である。この図
１に示すように、１は第１のロボットであり、この第１
のロボットは、垂直多関節型のアーム２を有している。
このアーム２の先端部には、手首部４とハンド６が取り
付けられている。さらに、この手首部４とハンド６の間
には、エンドエフェクタ８と力センサ１０とが、それぞ
れユニット化されて取り付けられている。このエンドエ
フェクタ８は、図２に示されたように、３軸直交テーブ
ル即ち、Ｘ軸方向に移動可能なＸ軸テーブル８ａ、Ｙ軸
方向に移動可能なＹ軸テーブル８ｂ、及びＺ軸方向に移
動可能なＺ軸テーブル８ｃにより構成され、これらの各
テーブル８ａ，８ｂ，８ｃは、それぞれ独立してサーボ
モータ（図示せず）により能動的に駆動される。また、
力センサ１０は、Ｘ軸方向、Ｙ軸方向及びＺ軸方向の変
位を検出し、この変位を力に換算して各軸方向の力を検
出するものである。DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT An embodiment of the present invention will be described below with reference to the accompanying drawings. FIG. 1 is an overall configuration diagram showing an embodiment of a cooperative control apparatus for a plurality of robots of the present invention. As shown in FIG. 1, reference numeral 1 is a first robot.
The robot has a vertical articulated arm 2.
A wrist 4 and a hand 6 are attached to the tip of the arm 2. Further, an end effector 8 and a force sensor 10 are unitized and attached between the wrist 4 and the hand 6. As shown in FIG. 2, the end effector 8 is a triaxial orthogonal table, that is, an X-axis table 8a movable in the X-axis direction, a Y-axis table 8b movable in the Y-axis direction, and a Z-axis direction. It is composed of a movable Z-axis table 8c, and each of these tables 8a, 8b, 8c is independently and actively driven by a servomotor (not shown). Also,
The force sensor 10 detects displacements in the X-axis direction, the Y-axis direction, and the Z-axis direction, converts the displacements into forces, and detects the forces in each axial direction.
【０００７】一方、１１は、第２のロボットであり、こ
の第２のロボット１１も、第１のロボット１と同様な構
成を有している。即ち、この第２のロボット１１は、ア
ーム１２を有している。このアーム１２の先端部には、
手首部１４とハンド１６が取り付けられている。さら
に、この手首部１４とハンド１６の間には、エンドエフ
ェクタ１８と力センサ２０とが、それぞれユニット化さ
れて取り付けられている。このエンドエフェクタ１８
も、図２に示されたものと同様に、３軸直交テーブルを
備え、これらの各テーブルは、それぞれ独立してサーボ
モータにより能動的に駆動される。また、力センサ２０
も、上記力センサ１０と同様であり、Ｘ軸方向、Ｙ軸方
向及びＺ軸方向の変位を検出し、この変位を力に換算し
て各軸方向の力を検出するものである。これらの第１の
ロボット１及び第２のロボット１１は、それぞれのハン
ド６，１６により、インストルメントパネル等の搬送ワ
ーク２２を把持し、所定の目標軌道に沿って搬送する。On the other hand, 11 is a second robot, and this second robot 11 also has the same configuration as that of the first robot 1. That is, the second robot 11 has the arm 12. At the tip of this arm 12,
A wrist portion 14 and a hand 16 are attached. Further, the end effector 18 and the force sensor 20 are unitized and attached between the wrist portion 14 and the hand 16. This end effector 18
Similarly to the one shown in FIG. 2, a three-axis orthogonal table is provided, and each of these tables is actively driven independently by a servo motor. Also, the force sensor 20
This is also similar to the force sensor 10 described above, and detects displacements in the X-axis direction, the Y-axis direction, and the Z-axis direction, converts the displacements into forces, and detects the forces in each axial direction. The first robot 1 and the second robot 11 hold the transfer work 22 such as an instrument panel with the respective hands 6 and 16 and transfer it along a predetermined target trajectory.
【０００８】また、第１のロボット１は、ロボットコン
トローラ２４及びエンドエフェクタコントローラ２６を
有している。このロボットコントローラ２４から所定の
動作指令が出力され、この動作指令により、アーム２が
制御される。エンドエフェクタコントローラ２６は、力
センサ１０から入力される力（変位）情報に基づきエン
ドエフェクタ８に力制御動作指令を出力し、この力制御
動作指令によりエンドエフェクタ８が制御される。第２
のロボット１１も第１のロボット１と同様に、ロボット
コントローラ２８及びエンドエフェクタコントローラ３
０を有している。次に、これらの第１のロボット１のロ
ボットコントローラ２４及びエンドエフェクタコントロ
ーラ２６、及び第２のロボット１１のロボットコントロ
ーラ２８及びエンドエフェクタコントローラ３０によ
り、実行される複数ロボットの協調制御の内容を説明す
る。図３は、本発明の複数ロボットの協調制御装置によ
る１自由度方向への搬送作業をモデル化して示した平面
図である。この図３において、Ｍ0 は搬送ワーク２２の
質量、Ｍ1 は第１のロボット１の質量、Ｍ2 は第２のロ
ボット１１の質量、ｍ1 は第１のエンドエフェクタ８の
質量、ｍ2 は第２のエンドエフェクタ１８のの質量、ｋ
1 は第１の力センサ１０のバネ定数、ｋ2 は第２の力セ
ンサ２０のバネ定数、ｘは搬送ワーク２２の軌道（位
置）、ｘ1 は第１のロボット１の軌道（位置）、ｘ2 は
第２のロボット１１の軌道（位置）、ｙ1 は第１のロボ
ット１の軌道（位置）、ｙ2 は第２のロボット１１の軌
道（位置）、ｆ1 は第１のエンドエフェクタ８に加える
べき目標力、ｆ2 は第２のエンドエフェクタ１８に加え
るべき目標力、をそれぞれ示している。The first robot 1 also has a robot controller 24 and an end effector controller 26. A predetermined operation command is output from the robot controller 24, and the arm 2 is controlled by this operation command. The end effector controller 26 outputs a force control operation command to the end effector 8 based on the force (displacement) information input from the force sensor 10, and the end effector 8 is controlled by this force control operation command. Second
Similarly to the first robot 1, the robot 11 of FIG.
Has 0. Next, contents of cooperative control of a plurality of robots executed by the robot controller 24 and the end effector controller 26 of the first robot 1 and the robot controller 28 and the end effector controller 30 of the second robot 11 will be described. . FIG. 3 is a plan view showing a modeled transfer work in the direction of one degree of freedom by the cooperative control apparatus for a plurality of robots of the present invention. In FIG. 3, M 0 is the mass of the transfer work 22, M 1 is the mass of the first robot 1, M 2 is the mass of the second robot 11, m 1 is the mass of the first end effector 8, and m 2 Is the mass of the second end effector 18, k
1 is the spring constant of the first force sensor 10, k 2 is the spring constant of the second force sensor 20, x is the trajectory (position) of the transfer work 22, x 1 is the trajectory (position) of the first robot 1, x 2 is the trajectory (position) of the second robot 11, y 1 is the trajectory (position) of the first robot 1, y 2 is the trajectory (position) of the second robot 11, and f 1 is the first end effector 8 is a target force to be applied to the second end effector 18, and f 2 is a target force to be applied to the second end effector 18.
【０００９】また、大型の第１のモータ４１及び第２の
モータ４２、小型の第３のモータ４３及び第４のモータ
４４がそれぞれ設けられ、これらの各モータ４１〜４４
は、それぞれボールネジ等の減速機を介してロボット及
びエンドエフェクタをｘ軸方向に駆動する。制御系の構
成として、まず搬送ワーク２２の目標軌道を設計し、ロ
ボット１，１１を大型の第１及び第２のモータ４１，４
２により搬送ワークの内力の変動を無視して目標軌道に
沿ってそれぞれ速度制御する。この際、ロボット位置の
相対誤差により発生する目標力の誤差を、小型の第３及
び第４のモータ４３，４４の加速度制御により補正す
る。以下、図３に示されたモデルに基づいて、数式を用
いて詳細に説明する。搬送ワーク２２の運動方程式は、
次に示す式（１）となる。Further, a large-sized first motor 41 and a second motor 42, and a small-sized third motor 43 and a fourth motor 44 are respectively provided, and these respective motors 41 to 44 are provided.
Respectively drive the robot and the end effector in the x-axis direction via a speed reducer such as a ball screw. As the configuration of the control system, first, the target trajectory of the transfer work 22 is designed, and the robots 1 and 11 are driven by the large first and second motors 41 and 4.
By 2, the speed control is performed along the target trajectory, ignoring the fluctuation of the internal force of the conveyed work. At this time, the error of the target force caused by the relative error of the robot position is corrected by the acceleration control of the small third and fourth motors 43 and 44. Hereinafter, a detailed description will be given using mathematical expressions based on the model shown in FIG. The equation of motion of the transfer work 22 is
It becomes the following formula (1).
【００１０】[0010]
【数１】 [Equation 1]
【００１１】ここで、λ1 は第１のロボットが有する粘
性抵抗（ボールネジ等によるもの）、λ2 は第２のロボ
ットが有する粘性抵抗（ボールネジ等によるもの）を示
している。また、第１のエンドエフェクタと第２のエン
ドエフェクタの運動方程式は、それぞれ以下の式（２）
及び式（３）となる。Here, λ 1 is a viscous resistance (by a ball screw or the like) of the first robot, and λ 2 is a viscous resistance (by a ball screw or the like) of the second robot. Further, the equations of motion of the first end effector and the second end effector are respectively expressed by the following equation (2).
And equation (3).
【００１２】[0012]
【数２】 [Equation 2]
【００１３】[0013]
【数３】 [Equation 3]
【００１４】ここで、搬送ワークの目的軌道がｘ
d （ｔ）と与えられたとき、搬送ワークに加えるべき合
力ｆd が、以下の式（４）により表される。Here, the target trajectory of the transferred work is x
when given as d (t), the resultant force f d to be added to the transport work is represented by the following equation (4).
【００１５】[0015]
【数４】 [Equation 4]
【００１６】但し、ｆ1d＝ｋ1 （ｘ1 ＋ｙ1d−ｘ），ｆ
2d＝ｋ2 （ｘ2 ＋ｙ2d−ｘ）である。また、ｋmvとｋmp
はそれぞれフィードバックゲインであり、ｙ1dとｙ2dは
それぞれ第１及び第２のエンドエフェクタの目標軌道を
表している。この式（４）が成立するように、それぞれ
の第１及び第２のエンドエフェクタの位置ｙ1 ，ｙ2 を
ｙ1d，ｙ2dとなるように制御する。このように、搬送ワ
ークの制御に必要な力ｆ1d，ｆ2dをｙ1d，ｙ2dで作る
際、ｘ1 ，ｘ2 を考慮することにより、２つのロボット
の相対誤差の搬送ワークへの影響を無くすることが可能
となる。式（４）を式（１）を代入し、誤差変数をそれ
ぞれ式（５）のように定義すると、誤差方程式である式
（６）が得られる。However, f 1d = k 1 (x 1 + y 1d -x), f
2d = k 2 (x 2 + y 2d −x). Also, k mv and k mp
Are feedback gains, and y 1d and y 2d represent target trajectories of the first and second end effectors, respectively. The positions y 1 , y 2 of the respective first and second end effectors are controlled to be y 1d , y 2d so that this expression (4) is satisfied. In this way, when the forces f 1d and f 2d required to control the transferred work are created with y 1d and y 2d , the influence of the relative error between the two robots on the transferred work is considered by taking x 1 and x 2 into consideration. Can be eliminated. By substituting equation (1) into equation (4) and defining error variables as in equation (5), equation (6), which is an error equation, is obtained.
【００１７】[0017]
【数５】 [Equation 5]
【００１８】ここで、ｅm は搬送ワークの目標軌道に対
する位置誤差、ｅy1は第１のエンドウエフェクタにおけ
る誤差、ｅy2は第２のエンドエフェクタにおける位置誤
差、をそれぞれ示している。[0018] Here, e m is the position error with respect to the target track of the transport work, the e y1 indicates an error in the first pea effector, e y2 is position error in the second end effector, respectively.
【００１９】[0019]
【数６】 [Equation 6]
【００２０】この式（６）は、第１及び第２のエンドエ
フェクタの位置誤差ｅｙ1 ，ｅｙ2が０となれば、搬送
ワークの目標軌道に対する位置誤差ｅm がフィードバッ
クゲインｋmv，ｋmpに従って収束することを示してい
る。次に、第１及び第２のエンドエフェクタに加えるべ
き目標力ｆ1 ，ｆ2 を式（７），式（８）のように決定
する。[0020] The equation (6) is, if the position error ey 1, ey 2 0 of the first and second end effectors, the position error e m feedback gain k mv typical trajectories of conveying the workpiece, k mp It shows that it converges according to. Next, the target forces f 1 and f 2 to be applied to the first and second end effectors are determined as in equations (7) and (8).
【００２１】[0021]
【数７】 [Equation 7]
【００２２】[0022]
【数８】 [Equation 8]
【００２３】但し、ｋv1，ｋv2，ｋp1，ｋp2は、それぞ
れフィードバックゲインである。また、ここで、例え
ば、ｆ1d＝ｆ2d＝1/2 ｆd とすると、ｙ1d，ｙ’1d，
ｙ''1dはそれぞれ以下のようになる。ここで、ｆ1d＝ｆ
2d＝1/2 ｆd とするのは、搬送ワークの重心がワークの
中心にある場合であり、この場合には、２台のエンドエ
フェクタにより負荷を等分した方が好ましいからであ
る。However, k v1 , k v2 , k p1 and k p2 are feedback gains, respectively. Further, here, for example, if f 1d = f 2d = 1/2 f d , then y 1d , y ′ 1d ,
y '' 1d is as follows. Where f 1d = f
The reason why 2d = 1/2 f d is that the center of gravity of the conveyed work is at the center of the work, and in this case, it is preferable to divide the load equally by two end effectors.
【００２４】[0024]
【数９】 [Equation 9]
【００２５】ｙ2d，ｙ’2d，ｙ''2dについても同様に以
下のようになる。The same applies to y 2d , y ′ 2d and y ″ 2d as follows.
【００２６】[0026]
【数１０】 [Equation 10]
【００２７】これらの数式から指令を生成するに当たっ
てｆd の２階微分すなわち目標軌道の４階微分が必要な
ことが理解できる。さて、式（７）を式（２）に代入す
ることにより、第１のエンドエフェクタの質量ｍ1 の運
動方程式は、It can be understood that the second derivative of f d , that is, the fourth derivative of the target trajectory is required to generate the command from these mathematical expressions. By substituting the equation (7) into the equation (2), the equation of motion of the mass m 1 of the first end effector is
【００２８】[0028]
【数１１】 [Equation 11]
【００２９】となり、これを変形して最終的に以下の式
（９）となる。## EQU3 ## This is transformed into the following expression (9).
【００３０】[0030]
【数１２】 [Equation 12]
【００３１】第２のエンドエフェクタの質量ｍ2 の運動
方程式は、式（８）を式（３）に代入することにより、
以下の式（１０）となる。The equation of motion of the mass m 2 of the second end effector is obtained by substituting the equation (8) into the equation (3).
The following expression (10) is obtained.
【００３２】[0032]
【数１３】 [Equation 13]
【００３３】これらの式（９）と式（１０）より、それ
ぞれの第１及び第２のエンドエフェクタの誤差は、他の
ロボットや搬送ワークの目標に対する誤差とは、独立に
収束することがわかる。従って、第１及び第２のエンド
エフェクタの誤差をできるだけ早く収束するようにフィ
ードバックゲインｋv1，ｋp1，ｋv2，ｋp2を設計してお
くことにより、式（６）の搬送ワークの誤差の状態方程
式は、以下の式（１１）ように２次系モデルとなる。From these equations (9) and (10), it can be seen that the error of each of the first and second end effectors converges independently of the error with respect to the targets of other robots and transfer works. . Therefore, by designing the feedback gains k v1 , k p1 , k v2 , and k p2 so that the errors of the first and second end effectors are converged as soon as possible, the error of the transfer work of the formula (6) can be reduced. The state equation becomes a quadratic system model as shown in the following equation (11).
【００３４】[0034]
【数１４】 [Equation 14]
【００３５】従って、２つのフィードバックゲイン
ｋmv，ｋmpの設計により搬送ワークの目標軌道に対する
誤差を簡単に収束させることができる。ここで、モータ
はＤＣサーボモータであり、電圧入力による速度指令で
制御される。この特性は１次遅れ系でモデル化されるの
で、指令電圧と第３のモータの回転角θ、即ち、微動機
構のベースに対する相対位置ｙ1 の間には、以下の関係
が成立する。Therefore, by designing the two feedback gains k mv and k mp , the error with respect to the target trajectory of the transferred work can be easily converged. Here, the motor is a DC servo motor and is controlled by a speed command by voltage input. Since this characteristic is modeled in the first-order lag system, the following relationship is established between the command voltage and the rotation angle θ of the third motor, that is, the relative position y 1 with respect to the base of the fine movement mechanism.
【００３６】[0036]
【数１５】 [Equation 15]
【００３７】但し、Ｔはモータ時定数、Ｋは速度／電圧
比、Ｅは入力電圧である。さて、協調系をモデル通り運
動させるには、指令トルク、つまりｆ1 を与えた時にエ
ンドエフェクタが理想モデル上で行う運動を入力電圧
（指令電圧）により強制的に実現させてしまえば良い。
目標力を与えた時のエンドエフェクタの加速度は、However, T is the motor time constant, K is the speed / voltage ratio, and E is the input voltage. Now, in order to model as motion coordination system, command torque, i.e. the end effector may once forcibly implemented by the input voltage exercise performed on the ideal model (command voltage) when given f 1.
The acceleration of the end effector when a target force is applied is
【００３８】[0038]
【数１６】 [Equation 16]
【００３９】より、入力電圧（指令電圧）をFrom the input voltage (command voltage)
【００４０】[0040]
【数１７】 [Equation 17]
【００４１】として与えればよい。第４のモータについ
ても、同様である。次に、上述した誤差方程式における
フィードバックゲインの決定方法を説明する。先ず、第
１及び第２のエンドエフェクタの位置フィードバックゲ
インｋp1，ｋ p2及び速度フィードバックゲインｋv1，ｋ
v2の決定方法を説明する。上述したように、第１及び第
２のエンドエフェクの動作に関する誤差方程式は、式
（９）および式（１０）となる。It may be given as For the fourth motor
However, it is the same. Then in the error equation above
A method of determining the feedback gain will be described. First,
Position feedback sensors for the first and second end effectors.
Inkp1, K p2And velocity feedback gain kv1, K
v2The method of determining will be described. As mentioned above,
The error equation for the motion of the end effect of 2 is
(9) and equation (10).
【００４２】[0042]
【数１８】 [Equation 18]
【００４３】[0043]
【数１９】 [Formula 19]
【００４４】これらの式（９）および式（１０）を変形
すると、以下のように、式（９）より式（１２）が、式
（１０）より式（１３）がそれぞれ得られる。When these equations (9) and (10) are modified, equation (12) is obtained from equation (9) and equation (13) is obtained from equation (10) as follows.
【００４５】[0045]
【数２０】 [Equation 20]
【００４６】[0046]
【数２１】 [Equation 21]
【００４７】これらの式（１２）および式（１３）は、
２次の自励振動系であり、ｅy1，ｅ y2の値は、右辺の２
×２行列の要素によって決まる。制御目的は、第１及び
第２のエンドエフェクタの動作の誤差をなくすること、
即ち、「ｅy1，ｅy2→０（ｔ→∞）」とすることであ
る。これは、式（１２）および式（１３）の右辺の２×
２行列の固有値の実部を全て負にすれば実現できる。そ
のため、式（１２）において、右辺の２×２行列の固有
値をλ1 とすると、λ1 は式（１４）となる。These equations (12) and (13) are
It is a second-order self-excited vibration system, andy1, E y2The value of is 2 on the right side
It depends on the elements of the × 2 matrix. The control purpose is the first and
Eliminating error in the operation of the second end effector,
That is, "ey1, Ey2→ 0 (t → ∞) "
It This is 2 × on the right side of equations (12) and (13).
This can be realized by making all the real parts of the eigenvalues of the two matrices negative. So
Therefore, in equation (12), the eigenvalue of the 2 × 2 matrix on the right side
Value λ1Then λ1Becomes equation (14).
【００４８】[0048]
【数２２】 [Equation 22]
【００４９】この固有値λ1 の実部を全て負にするため
の条件は、次の式（１５）となる。The condition for making all the real parts of the eigenvalue λ 1 negative is the following expression (15).
【００５０】[0050]
【数２３】 [Equation 23]
【００５１】ここで、一つの設定法として式（１５）に
おいて、Here, as one setting method, in equation (15),
【００５２】[0052]
【数２４】 [Equation 24]
【００５３】とすると、誤差ｅy1が、図４のように収束
する。この図４は、フィードバックゲインｋp1，ｋv1を
式（１６）のように設定したときの誤差ｅy1の収束を表
す線図である。フィードバックゲインｋp1，ｋv1を式
（１６）のように設定することにより、オーバーシュー
トなく最も速く０に収束させることができる。また、式
（１３）についても、同様に、右辺の２×２行列の固有
値をλ2 とすると、λ2 は式（１７）となる。Then, the error e y1 converges as shown in FIG. FIG. 4 is a diagram showing the convergence of the error e y1 when the feedback gains k p1 and k v1 are set as in Expression (16). By setting the feedback gains k p1 and k v1 as in Expression (16), it is possible to converge to 0 fastest without overshoot. Similarly, with respect to the expression (13), if the eigenvalue of the 2 × 2 matrix on the right side is λ 2 , then λ 2 becomes the expression (17).
【００５４】[0054]
【数２５】 [Equation 25]
【００５５】この固有値λ2 の実部を全て負にするため
の条件は、式（１８）となる。The condition for making all the real parts of the eigenvalue λ 2 negative is the expression (18).
【００５６】[0056]
【数２６】 [Equation 26]
【００５７】式（１５）と同様に、式（１８）におい
て、Similar to equation (15), in equation (18)
【００５８】[0058]
【数２７】 [Equation 27]
【００５９】とすると、誤差ｅy2が、図２と同様に収束
する。次に、搬送物Ｍ0 の位置フィードバックゲインｋ
mp及び速度フィードバックゲインｋmvの決定方法を説明
する。上述したように、搬送ワークＭ0 の動作に関する
誤差方程式は、式（１１）となる。Then, the error e y2 converges as in FIG. Next, the position feedback gain k of the conveyed product M 0
A method of determining mp and velocity feedback gain kmv will be described. As described above, the error equation regarding the operation of the transported work M 0 is given by the equation (11).
【００６０】[0060]
【数２８】 [Equation 28]
【００６１】この式（１１）は、式（６）において、
「ｅy1＝ｅy2＝０」としたものである。制御目的は、第
１及び第２のロボットによる搬送ワークＭ0 の誤差をな
くすること、即ち、「ｅy1＝ｅy2＝０」とできるという
前提で、「ｅm →０（ｔ→∞）」とすることである。こ
れは、式（１２）および式（１３）の右辺の２×２行列
の固有値の実部を全て負にすれば実現できる。上記の式
（１６）と式（１９）と同様にして、以下の式（２０）
のようにフィードバックゲインｋmp，ｋmvを設定するこ
とにより、ｅm →０（ｔ→∞）とすることができる。This equation (11) is the same as the equation (6).
It is assumed that "e y1 = e y2 = 0". The control purpose is to eliminate the error of the transferred work M 0 by the first and second robots, that is, “e y1 = e y2 = 0”, and “e m → 0 (t → ∞)”. ". This can be realized by making all the real parts of the eigenvalues of the 2 × 2 matrix on the right side of Expressions (12) and (13) negative. Similar to the above equations (16) and (19), the following equation (20)
Feedback gain k mp as, by setting k mv, it is possible to e m → 0 (t → ∞ ).
【００６２】[0062]
【数２９】 [Equation 29]
【００６３】次に、それぞれの誤差の収束速度の関係に
ついて説明する。式（１１）が成立するためには、
ｅy1，ｅy2がｅm より速く０に収束する必要がある。以
下、そのための条件について説明する。式（１１）を、
式（１２），（１３）と同様に、変形すると、式（２
１）となる。Next, the relationship between the convergence speeds of the respective errors will be described. In order for Equation (11) to hold,
It is necessary that e y1 and e y2 converge to 0 faster than e m . The conditions therefor will be described below. Equation (11)
Similar to the equations (12) and (13), when transformed, the equation (2
It becomes 1).
【００６４】[0064]
【数３０】 [Equation 30]
【００６５】ここで、式（２１）の右辺の２×２行列の
固有値をλm とすると、λm は式（２２）となる。Here, if the eigenvalue of the 2 × 2 matrix on the right side of equation (21) is λ m , then λ m is given by equation (22).
【００６６】[0066]
【数３１】 [Equation 31]
【００６７】ｅy1，ｅy2がｅm より速く０に収束する条
件は、λ1 ，λ2 ，λm が、式（２３）を満たせばよ
い。The conditions for e y1 and e y2 to converge to 0 faster than e m are that λ 1 , λ 2 and λ m satisfy equation (23).
【００６８】[0068]
【数３２】 [Equation 32]
【００６９】ここで、Ｒe （λ1 ），Ｒe （λ2 ），Ｒ
e （λm ）は、それぞれ固有値の実部の値を示してい
る。すなわち、λ1 ，λ2 の実部の値が、λm のそれよ
り小さければ、式（１１）が成立する。上述したよう
に、フィードバックゲインの関係式は、式（１６），
（１９），（２０）となる。Here, R e (λ 1 ), R e (λ 2 ), R
e (λ m ) represents the value of the real part of the eigenvalue. That is, if the values of the real parts of λ 1 and λ 2 are smaller than that of λ m , equation (11) holds. As described above, the relational expression of the feedback gain is expressed by Expression (16),
(19) and (20).
【００７０】[0070]
【数３３】 [Expression 33]
【００７１】[0071]
【数３４】 [Equation 34]
【００７２】[0072]
【数３５】 [Equation 35]
【００７３】また、各誤差方程式の固有値λ1 ，λ2 ，
λm を求める式は、式（１４），（１７），（２２）と
なる。Also, the eigenvalues λ 1 , λ 2 , and
The equations for obtaining λ m are equations (14), (17), (22).
【００７４】[0074]
【数３６】 [Equation 36]
【００７５】[0075]
【数３７】 [Equation 37]
【００７６】[0076]
【数３８】 [Equation 38]
【００７７】式（１４），（１７），（２２）に式（１
６），（１９），（２０）を代入すると、以下の式（２
４），（２５），（２６）が得られる。The equation (1) is added to the equations (14), (17) and (22).
Substituting 6), (19), and (20), the following equation (2
4), (25) and (26) are obtained.
【００７８】[0078]
【数３９】 [Formula 39]
【００７９】[0079]
【数４０】 [Formula 40]
【００８０】[0080]
【数４１】 [Formula 41]
【００８１】従って、式（２４），（２５），（２６）
において、固有値λ1 ，λ2 ，λmを式（２３）を満た
すようにして与えることにより、フィードバックゲイン
ｋv1，ｋv2，ｋmvの値が決まる。次に、これらのフィー
ドバックゲインｋv1，ｋv2，ｋmvの値を式（１６），
（１９），（２０）に代入することにより、フィードバ
ックゲインｋp1，ｋp2，ｋmpの値が決まる。図５は、こ
のようなフィードバックゲイン設定の手順を示したフロ
ーチャートである。この図５において、既知の値である
ｍ1 ，ｋ1 ，ｍ2 ，ｋ2 ，Ｍ0 を入力し、次に、式（２
４），（２５），（２６）により求めた固有値λ1 ，λ
2 ，λm の値を入力する。次に、フィードバックゲイン
ｋv1，ｋv2，ｋmvの値を算出する。その後、フィードバ
ックゲインｋp1，ｋp2，ｋmpの値を算出する。本発明の
実施例に基づいた具体例は、以下のようになる。 搬送ワークＭ0 の質量＝３０ｋｇ 第１及び第２のエンドエフェクタの可動部分の質量ｍ1
＝ｍ2 ＝４ｋｇ 力センサのばね定数ｋ1 ＝ｋ2 ＝１００００Ｎ／ｍ λ1 ＝−５（重根） λ2 ＝−２０（重根） λm ＝−２０（重根） この条件で、フィードバックゲインを設定すると、以下
のようになる。 ｋp1＝ｋp2＝−８４００ ｋv1＝ｋv2＝１６０ ｋmp＝７５０ ｋmv＝３００Therefore, equations (24), (25) and (26)
In, the values of the feedback gains k v1 , k v2 , and kmv are determined by giving the eigenvalues λ 1 , λ 2 , and λ m so as to satisfy the equation (23). Next, the values of these feedback gains k v1 , k v2 , and kmv are given by equation (16),
By substituting into (19) and (20), the values of the feedback gains k p1 , k p2 , and k mp are determined. FIG. 5 is a flowchart showing the procedure of such feedback gain setting. In FIG. 5, known values m 1 , k 1 , m 2 , k 2 and M 0 are input, and then the expression (2
4), (25), and (26) eigenvalues λ 1 , λ
2 Enter the value of λ m . Next, the values of the feedback gains k v1 , k v2 , and kmv are calculated. After that, the values of the feedback gains k p1 , k p2 , and k mp are calculated. A specific example based on the embodiment of the present invention is as follows. Mass of transferred work M 0 = 30 kg Mass of movable part of the first and second end effectors m 1
= M 2 = 4 kg Spring constant of force sensor k 1 = k 2 = 10000 N / m λ 1 = −5 (double root) λ 2 = −20 (double root) λ m = −20 (double root) Feedback gain under these conditions The settings are as follows. k p1 = k p2 = -8400 k v1 = k v2 = 160 k mp = 750 k mv = 300
【００８２】[0082]
【発明の効果】以上説明したように本発明の複数ロボッ
トの協調制御装置によれば、ワークに内力が作用するこ
とがなく且つロボットの位置ずれが生じてものワークの
位置ずれを防止することができる。As described above, according to the coordinated control system for a plurality of robots of the present invention, the internal force does not act on the workpieces and the displacement of the workpieces can be prevented even if the displacement of the robots occurs. it can.
【図面の簡単な説明】[Brief description of drawings]
【図１】本発明の複数ロボットの協調制御装置の一実施
例を示す全体構成図FIG. 1 is an overall configuration diagram showing an embodiment of a cooperative control apparatus for a plurality of robots of the present invention.
【図２】エンドエフェクタを示す斜視図FIG. 2 is a perspective view showing an end effector.
【図３】本発明の複数ロボットの協調制御装置による１
自由度方向への搬送作業をモデル化して示した平面図FIG. 3 is a block diagram of a multi-robot cooperative control device according to the present invention.
Plan view showing modeled transfer work in the direction of freedom
【図４】誤差ｅy1の収束する状態を示す線図FIG. 4 is a diagram showing a state in which an error e y1 converges.
【図５】フィードバックゲインを設定するためにフロー
チャートFIG. 5 is a flowchart for setting a feedback gain.
１ 第１のロボット ２ アーム ４ 手首部 ６ ハンド ８ エンドエフェクタ １０ 力センサ １１ 第２のロボット １２ アーム １４ 手首部 １６ ハンド １８ エンドエフェクタ ２０ 力センサ ２２ 搬送ワーク ２４ ロボットコントローラ ２６ エンドエフェクタコントローラ ２８ ロボットコントローラ ３０ エンドエフェクタコントローラ ４１ 第１のモータ ４２ 第２のモータ ４３ 第３のモータ ４４ 第４のモータ 1 First Robot 2 Arm 4 Wrist 6 Hand 8 End Effector 10 Force Sensor 11 Second Robot 12 Arm 14 Wrist 16 Hand 18 End Effector 20 Force Sensor 22 Transport Work 24 Robot Controller 26 End Effector Controller 28 Robot Controller 30 End effector controller 41 First motor 42 Second motor 43 Third motor 44 Fourth motor
Claims (2)
ドにより把持して運搬する複数ロボットの協調制御装置
において、 複数ロボットの各ハンドの手首部に取り付けられた力セ
ンサ及びエンドエフェクタと、 上記各ロボット自体を制御するロボットコントローラ
と、 上記エンドエフェクタを制御するエンドエフェクタコン
トローラと、を有し、 このエンドエフェクタコントローラが、ロボットの位
置、所定の搬送目標軌道に基づく搬送時の力センサから
の信号及び搬送ワークの目標軌道に基づき上記エンドエ
フェクタを作動制御させることを特徴とする複数ロボッ
トの協調制御装置。1. In a coordinated control apparatus for a plurality of robots that grips and conveys a single transfer work by the hands of a plurality of robots, a force sensor and an end effector attached to the wrists of the hands of the plurality of robots, and It has a robot controller for controlling the robot itself, and an end effector controller for controlling the end effector. The end effector controller has a position of the robot, a signal from a force sensor at the time of transfer based on a predetermined transfer target trajectory, and A cooperative control apparatus for a plurality of robots, wherein the end effector is actuated and controlled based on a target trajectory of a transfer work.
の制御ゲイン設定手段が、エンドエフェクタと上記目標
軌道との誤差がロボットのハンドと上記目標軌道と誤差
より速く収束するように制御ゲインを設定することを特
徴とする請求項１記載の複数ロボットの協調制御装置。2. The control gain setting means further comprises a control gain setting means for setting a control gain so that an error between the end effector and the target trajectory converges faster than an error between the robot hand and the target trajectory. The cooperative control device for a plurality of robots according to claim 1, wherein the cooperative control device is set.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP682094A JPH07205072A (en) | 1994-01-26 | 1994-01-26 | Cooperation controller of plural robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP682094A JPH07205072A (en) | 1994-01-26 | 1994-01-26 | Cooperation controller of plural robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH07205072A true JPH07205072A (en) | 1995-08-08 |
Family
ID=11648855
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP682094A Pending JPH07205072A (en) | 1994-01-26 | 1994-01-26 | Cooperation controller of plural robot |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH07205072A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6522952B1 (en) | 1999-06-01 | 2003-02-18 | Japan As Represented By Secretary Of Agency Of Industrial Science And Technology | Method and system for controlling cooperative object-transporting robot |
| WO2014128766A1 (en) * | 2013-02-25 | 2014-08-28 | 株式会社エイチアンドエフ | Transportation device and workpiece transportation method using same |
| US9517556B2 (en) | 2012-06-29 | 2016-12-13 | Mitsubishi Electric Corporation | Robot control apparatus and robot control method |
| CN109676642A (en) * | 2017-10-19 | 2019-04-26 | 广州中国科学院先进技术研究所 | A kind of multi-robot cooperation system and implementation method |
-
1994
- 1994-01-26 JP JP682094A patent/JPH07205072A/en active Pending
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6522952B1 (en) | 1999-06-01 | 2003-02-18 | Japan As Represented By Secretary Of Agency Of Industrial Science And Technology | Method and system for controlling cooperative object-transporting robot |
| US6654665B2 (en) | 1999-06-01 | 2003-11-25 | Japan As Represented By Secretary Of Agency Of Industrial Science And Technology | Method and system for controlling cooperative object-transporting robot |
| WO2004103651A1 (en) * | 1999-06-01 | 2004-12-02 | Hirohiko Arai | Control method for cooperative object-carrying robot and device therefor |
| US9517556B2 (en) | 2012-06-29 | 2016-12-13 | Mitsubishi Electric Corporation | Robot control apparatus and robot control method |
| WO2014128766A1 (en) * | 2013-02-25 | 2014-08-28 | 株式会社エイチアンドエフ | Transportation device and workpiece transportation method using same |
| JP5798689B2 (en) * | 2013-02-25 | 2015-10-21 | 株式会社エイチアンドエフ | Conveying apparatus and work conveying method using the same |
| CN109676642A (en) * | 2017-10-19 | 2019-04-26 | 广州中国科学院先进技术研究所 | A kind of multi-robot cooperation system and implementation method |
| CN109676642B (en) * | 2017-10-19 | 2023-05-02 | 深圳市中科德睿智能科技有限公司 | Multi-robot collaboration system and implementation method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| TWI673150B (en) | Robot teaching method and robot arm control device | |
| JP3473834B2 (en) | Robot control device | |
| EP0159131B1 (en) | Drive system for a movable apparatus | |
| TWI630081B (en) | Direct teaching method of robot | |
| Bodie et al. | Dynamic end effector tracking with an omnidirectional parallel aerial manipulator | |
| JPH07295650A (en) | Method for controlling articulated robot | |
| KR101688344B1 (en) | Weaving control device of multi-joint robot | |
| KR100359577B1 (en) | Controller of multi-axis robot | |
| JPH07205072A (en) | Cooperation controller of plural robot | |
| JP2003071760A (en) | Assembly robot | |
| Hähn et al. | Hybrid compliance compensation for path accuracy enhancement in robot machining | |
| JPH08155868A (en) | Control of robot and its device | |
| JP2003216243A (en) | Robot controller | |
| JPS61169905A (en) | Track control device of robot | |
| CN112060094B (en) | Mechanical arm control method based on impedance control | |
| JP6896824B2 (en) | Robot devices, robot systems, control methods for robot devices, manufacturing methods for articles using robot devices, information processing devices, information processing methods, control programs and recording media | |
| Caccavale et al. | Achieving a cooperative behavior in a dual‐arm robot system via a modular control structure | |
| JPH04343690A (en) | Joint friction compensating method for multi-joint manipulator | |
| JPS6190207A (en) | Robot controlling device | |
| Aoustin et al. | On the feedforward torques and reference trajectory for flexible two-link arm | |
| Liu | A comparison of controller design and simulation for an industrial manipulator | |
| JPH0760667A (en) | Weaving control device for robot | |
| JPS59220806A (en) | Controlling method of industrial robot | |
| JPH05150836A (en) | Robot controller | |
| JPS61217802A (en) | Robot controller |