JP2020151815A - Control device, control method and program - Google Patents

Control device, control method and program Download PDF

Info

Publication number
JP2020151815A
JP2020151815A JP2019053319A JP2019053319A JP2020151815A JP 2020151815 A JP2020151815 A JP 2020151815A JP 2019053319 A JP2019053319 A JP 2019053319A JP 2019053319 A JP2019053319 A JP 2019053319A JP 2020151815 A JP2020151815 A JP 2020151815A
Authority
JP
Japan
Prior art keywords
curve
acceleration
synchronization
slave device
component
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.)
Granted
Application number
JP2019053319A
Other languages
Japanese (ja)
Other versions
JP6973436B2 (en
Inventor
徹 小河原
Toru Ogawara
徹 小河原
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Omron Corp
Original Assignee
Omron Corp
Omron Tateisi Electronics Co
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Omron Corp, Omron Tateisi Electronics Co filed Critical Omron Corp
Priority to JP2019053319A priority Critical patent/JP6973436B2/en
Publication of JP2020151815A publication Critical patent/JP2020151815A/en
Application granted granted Critical
Publication of JP6973436B2 publication Critical patent/JP6973436B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

To suppress magnitude of synthetic acceleration including acceleration components for detachment operation for detaching a slave device from a master device and acceleration components for synchronization completing operation of completing synchronization of the slave device with the master device.SOLUTION: A first curve representing variation with time of velocity components for detachment operation of a slave device is calculated (S1). A synthetic curve representing variation with time of synthetic velocity including velocity components for the detachment operation and velocity components for synchronization completing operation is calculated based on the first curve and synchronization velocity of the slave device (S2). It is determined whether or not synthetic acceleration providing the synthetic curve falls below a predetermined negative threshold (S3). When the synthetic acceleration falls below the negative threshold, the synthetic curve is corrected so that the synthetic acceleration is equal to or more than the negative threshold (S4). A second curve representing variation with time of velocity components for the synchronization completing operation of the slave device is calculated based on the first curve and the corrected synthetic curve (S7).SELECTED DRAWING: Figure 3

Description

この発明は制御装置に関し、より詳しくは、マスタ装置とスレーブ装置とを同期させて制御する制御装置に関する。また、この発明は、そのような制御を行う制御方法およびプログラムに関する。 The present invention relates to a control device, and more particularly to a control device that controls a master device and a slave device in synchronization with each other. The present invention also relates to a control method and a program for performing such control.

従来、この種の制御装置としては、例えば次に掲げる非特許文献１に開示されているように、一定周期（例えば、０．５ｍｓｅｃ〜１ｍｓｅｃ程度の周期）で、マスタ装置の各軸（本明細書を通して「制御軸」を意味する。）への指令値（または各軸の測定された現在値）に基づいてスレーブ装置の各軸への指令値を演算によって求めて、マスタ装置とスレーブ装置とを同期させて制御する装置が開発されている。これにより、マスタ装置とスレーブ装置とを精度良く同期させることができる。 Conventionally, as a control device of this type, for example, as disclosed in Non-Patent Document 1 listed below, each axis of the master device (for example, a cycle of about 0.5 msec to 1 msec) (the present specification). Based on the command value (or the measured current value of each axis) to the "control axis" throughout the document, the command value to each axis of the slave device is calculated by calculation, and the master device and the slave device Devices have been developed to synchronize and control. As a result, the master device and the slave device can be synchronized with each other with high accuracy.

“NJ501-4 NJシリーズ NJ Robotics CPUユニット/特長 | オムロン制御機器”、［online］、２０１７年７月２０日、［２０１９年１月２９日検索］、インターネット＜ URL ：https://www.fa.omron.co.jp/products/family/3387/＞"NJ501-4 NJ Series NJ Robotics CPU Unit / Features | OMRON Control Equipment", [online], July 20, 2017, [Search January 29, 2019], Internet <URL: https://www.fa .omron.co.jp/products/family/3387/>

ところで、生産現場では、マスタ装置としてのベルトコンベアの動きに同期して、スレーブ装置としての６軸多関節ロボットを制御する場合がある。例えば、ベルトコンベアによって一方向に搬送されるワーク（作業対象物）を、ベルトコンベアの一方向の動きに同期して、ロボットの先端に取り付けられたエンドエフェクタによって、拾い上げる場合がある。ワークを拾い上げた後、上記制御装置の制御に応じて、上記ロボットのエンドエフェクタは、拾い上げられたワークがベルトコンベア上の後続のワークと衝突するのを避けるために、上記ベルトコンベアから離れる動作（離脱動作）を行いながら、上記一方向の動きを止める動作（同期終了動作）を行う。このように、マスタ装置からスレーブ装置を離脱させる離脱動作と、マスタ装置に対するスレーブ装置の同期を終了させる同期終了動作とが並行して行われる（同期終了モード）。これにより、タクトタイムが短縮されて、生産性の向上が図られる。 By the way, at a production site, a 6-axis articulated robot as a slave device may be controlled in synchronization with the movement of a belt conveyor as a master device. For example, a work (work object) conveyed in one direction by a belt conveyor may be picked up by an end effector attached to the tip of the robot in synchronization with the movement in one direction of the belt conveyor. After picking up the work, under the control of the control device, the end effector of the robot moves away from the belt conveyor in order to prevent the picked up work from colliding with the subsequent work on the belt conveyor ( While performing the detachment operation), the operation of stopping the movement in one direction (synchronization end operation) is performed. In this way, the detachment operation of detaching the slave device from the master device and the synchronization end operation of ending the synchronization of the slave device with the master device are performed in parallel (synchronization end mode). As a result, the tact time is shortened and the productivity is improved.

同期終了モードでは、例えば図７（Ａ）に示すように、離脱動作（鉛直方向であるｚ方向の動きとする。）に関して、上記ロボットのエンドエフェクタの速度成分ｖｚと加速度成分ａｚの変化が設定される。この例では、加速度成分ａｚは時刻ｔ＝０でゼロから増加し始め、時刻ｔ２１で正のピークを示し、時刻ｔ２３でゼロとなり、時刻ｔ２４で負のピークを示し、時刻ｔ２６でゼロとなっている。加速度成分ａｚの変化は直線的であり、離脱動作の加速期間（ｔ＝０〜ｔ２３）と減速期間（ｔ＝ｔ２３〜ｔ２６）とで、それぞれ三角制御がなされている。これに応じて、速度成分ｖｚは、時刻ｔ＝０でゼロから緩やかに増加し始め、時刻ｔ２３で緩やかな極大を示し、時刻ｔ２６で緩やかにゼロとなっている。速度成分ｖｚが示す曲線と横軸（時間軸）とで囲まれた領域（斜線で示す）の面積Ｓｚは、上記ロボットのエンドエフェクタがベルトコンベアの搬送面（上面）からｚ方向に離脱した距離（離脱距離）に相当する。また、図７（Ｂ）に示すように、同期終了動作（水平方向であるｘ方向の動きとする。）に関して、上記ロボットのエンドエフェクタの速度成分ｖｘと加速度成分ａｘの変化が設定される。この例では、この例では、時刻ｔ＝０〜ｔ２２の間は、上記ロボットのエンドエフェクタとベルトコンベアとの同期が維持されていることから、上記ロボットのエンドエフェクタの速度成分ｖｘはベルトコンベアのｘ方向の速度ｖｘｍ（一定値）に一致し、これに応じて加速度成分ａｘはゼロに維持されている。この例では、加速度成分ａｘは時刻ｔ２２で減少し始め、時刻ｔ２５で負のピークを示し、時刻ｔ２７でゼロとなっている。加速度成分ａｘの変化は直線的であり、減速期間（ｔ＝ｔ２２〜ｔ２７）では、三角制御がなされている。上述のような離脱動作、同期終了動作における三角制御は、それぞれ速度の変化を緩やかに開始し、かつ、緩やかに終了するために採用されている。 In the synchronous end mode, for example, as shown in FIG. 7A, changes in the velocity component vz and the acceleration component az of the end effector of the robot are set for the detachment operation (movement in the z direction which is the vertical direction). Will be done. In this example, the acceleration component az starts to increase from zero at time t = 0, shows a positive peak at time t21, becomes zero at time t23, shows a negative peak at time t24, and becomes zero at time t26. There is. The change in the acceleration component az is linear, and triangular control is performed in the acceleration period (t = 0 to t23) and the deceleration period (t = t23 to t26) of the detachment operation, respectively. Correspondingly, the velocity component vz starts to gradually increase from zero at time t = 0, shows a gradual maximum at time t23, and gradually becomes zero at time t26. The area Sz of the area (indicated by diagonal lines) surrounded by the curve indicated by the velocity component vz and the horizontal axis (time axis) is the distance at which the end effector of the robot separates from the transport surface (upper surface) of the belt conveyor in the z direction. Corresponds to (leaving distance). Further, as shown in FIG. 7B, changes in the velocity component vx and the acceleration component ax of the end effector of the robot are set for the synchronous end operation (movement in the x direction which is the horizontal direction). In this example, in this example, since the synchronization between the end effector of the robot and the belt conveyor is maintained during the time t = 0 to t22, the velocity component vx of the end effector of the robot is that of the belt conveyor. It corresponds to the velocity vxm (constant value) in the x direction, and the acceleration component ax is maintained at zero accordingly. In this example, the acceleration component ax begins to decrease at time t22, shows a negative peak at time t25, and becomes zero at time t27. The change of the acceleration component ax is linear, and triangular control is performed during the deceleration period (t = t22 to t27). The triangular control in the detachment operation and the synchronous end operation as described above is adopted to slowly start and slowly change the speed, respectively.

ここで、図７（Ｃ）は、離脱動作の速度成分ｖｚと同期終了動作の速度成分ｖｘとを合成して得られる合成速度ｖｃ、離脱動作の加速度成分ａｚと同期終了動作の加速度成分ａｘとを合成して得られる合成加速度ａｃを示している。この図７（Ｃ）から分かるように、図７（Ａ）に示す離脱動作と、図７（Ｂ）に示す同期終了動作とが並行して行われた場合、合成加速度ａｃが示す負のピークａｃｐの大きさ（絶対値）が大きくなる。このため、上記ロボットのモータにかかる負荷が大きくなり過ぎる可能性がある。また、ワークの慣性力が大きくなって、上記ロボットのエンドエフェクタがワークを落とす可能性がある。 Here, FIG. 7C shows the combined speed vc obtained by synthesizing the speed component vz of the detachment operation and the speed component vx of the synchronous end operation, the acceleration component az of the detachment operation, and the acceleration component ax of the synchronous end operation. The synthetic acceleration ac obtained by synthesizing is shown. As can be seen from FIG. 7 (C), when the withdrawal operation shown in FIG. 7 (A) and the synchronous end operation shown in FIG. 7 (B) are performed in parallel, the negative peak indicated by the combined acceleration ac. The magnitude (absolute value) of ac becomes large. Therefore, the load applied to the motor of the robot may become too large. In addition, the inertial force of the work increases, and the end effector of the robot may drop the work.

そこで、この発明の課題は、マスタ装置とスレーブ装置とを同期させて制御する制御装置であって、上記マスタ装置から上記スレーブ装置を離脱させる離脱動作の加速度成分と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作の加速度成分とを含む合成加速度の大きさを抑制できるものを提供することにある。また、この発明の課題は、そのような制御装置のための制御方法およびプログラムを提供することにある。 Therefore, an object of the present invention is a control device that controls the master device and the slave device in synchronization with each other, and the acceleration component of the detachment operation for separating the slave device from the master device and the slave device for the master device. It is an object of the present invention to provide a device capable of suppressing the magnitude of the combined acceleration including the acceleration component of the synchronization end operation. An object of the present invention is to provide a control method and a program for such a control device.

上記課題を解決するため、第１の局面では、この開示の制御装置は、
マスタ装置とスレーブ装置とを同期させて制御する制御装置であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出する第１カーブ算出部と、
上記第１カーブと上記スレーブ装置の同期速度とに基づいて、上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度の経時変化を表す合成カーブを算出する合成カーブ算出部と、
上記合成カーブを与える合成加速度が予め定められた負の閾値を下回っているか否かを判定する加速度判定部と、
上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように上記合成カーブを補正する合成カーブ補正部と、
上記第１カーブと上記補正された合成カーブとに基づいて、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する第２カーブ算出部と
を備えたことを特徴とする。 In order to solve the above problem, in the first aspect, the control device of this disclosure is
A control device that synchronizes and controls the master device and slave device.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
A first curve calculation unit that calculates a first curve that represents a change over time in the speed component of the detachment operation of the slave device,
A composite curve calculation unit that calculates a composite curve representing a change over time in the composite speed including the speed component of the detachment operation and the speed component of the synchronous end operation based on the first curve and the synchronous speed of the slave device. ,
An acceleration determination unit that determines whether or not the combined acceleration that gives the composite curve is below a predetermined negative threshold value, and
When the combined acceleration is below the negative threshold, the composite curve correction unit that corrects the composite curve so that the composite acceleration becomes the same as or exceeds the negative threshold.
Based on the first curve and the corrected composite curve, a second curve calculation unit for calculating a second curve representing a change over time in the speed component of the synchronous end operation of the slave device is provided. To do.

本明細書で、「離脱動作の速度成分」、「同期終了動作の速度成分」、「合成速度」を求めるときは、それぞれ「速度成分」、「速度」を与える加速度成分、加速度を求めてもよい。 In the present specification, when the "velocity component of the withdrawal motion", the "velocity component of the synchronous end motion", and the "composite velocity" are obtained, the acceleration component and the acceleration giving the "velocity component" and the "velocity" are also obtained. Good.

「スレーブ装置の同期速度」とは、上記スレーブ装置が上記マスタ装置に同期して動作しているときの速度を意味する。例えば、上記マスタ装置が一方向に一定の速度で移動している場合は、上記スレーブ装置の同期速度は、そのマスタ装置の速度を意味する。 The “synchronous speed of the slave device” means the speed at which the slave device is operating in synchronization with the master device. For example, when the master device is moving in one direction at a constant speed, the synchronous speed of the slave device means the speed of the master device.

上記合成カーブを与える合成加速度のための「予め定められた負の閾値」は、上記スレーブ装置の仕様の範囲内で、例えばそのスレーブ装置が取り扱うワークの慣性力を考慮して、予め設定される。より具体的には、例えば、上記スレーブ装置が６軸多関節ロボットからなる場合、そのロボットのエンドエフェクタがワークを落とさないような値に設定される。 The "predetermined negative threshold value" for the composite acceleration that gives the composite curve is preset within the specifications of the slave device, for example, in consideration of the inertial force of the work handled by the slave device. .. More specifically, for example, when the slave device is composed of a 6-axis articulated robot, the value is set so that the end effector of the robot does not drop the work.

この開示の制御装置では、第１カーブ算出部が、上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出する。合成カーブ算出部は、上記第１カーブと上記スレーブ装置の同期速度とに基づいて、上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度の経時変化を表す合成カーブを算出する。加速度判定部は、上記合成カーブを与える合成加速度が予め定められた負の閾値を下回っているか否かを判定する。上記合成加速度が上記負の閾値を下回っているとき、合成カーブ補正部は、上記合成加速度が上記負の閾値と同じになるか又は上回るように上記合成カーブを補正する。第２カーブ算出部は、上記第１カーブと上記補正された合成カーブとに基づいて、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する。このようにして、上記第１カーブと上記第２カーブとが作成される。 In the control device of this disclosure, the first curve calculation unit calculates the first curve representing the time-dependent change of the speed component of the detachment operation of the slave device. The composite curve calculation unit calculates a composite curve representing a change over time in the composite speed including the speed component of the detachment operation and the speed component of the synchronization end operation based on the first curve and the synchronization speed of the slave device. To do. The acceleration determination unit determines whether or not the combined acceleration that gives the combined curve is below a predetermined negative threshold value. When the combined acceleration is below the negative threshold, the composite curve correction unit corrects the combined curve so that the combined acceleration is equal to or exceeds the negative threshold. The second curve calculation unit calculates a second curve representing a change with time of the speed component of the synchronous end operation of the slave device based on the first curve and the corrected composite curve. In this way, the first curve and the second curve are created.

上記制御装置が実行する同期終了モードでは、上記スレーブ装置の離脱動作の速度成分が上記第１カーブに従って制御され、また、上記スレーブ装置の同期終了動作の速度成分が上記第２カーブに従って制御される。これに伴って、上記スレーブ装置は上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度で動作する。このとき、上記合成速度を与える合成加速度（上記離脱動作の加速度成分と上記同期終了動作の加速度成分とを含む）は上記負の閾値を下回らないものになっている。したがって、この制御装置によれば、上記合成加速度の大きさを抑制できる。 In the synchronous end mode executed by the control device, the speed component of the detachment operation of the slave device is controlled according to the first curve, and the speed component of the synchronous end operation of the slave device is controlled according to the second curve. .. Along with this, the slave device operates at a combined speed including the speed component of the detachment operation and the speed component of the synchronization end operation. At this time, the combined acceleration that gives the combined speed (including the acceleration component of the withdrawal operation and the acceleration component of the synchronous end operation) does not fall below the negative threshold value. Therefore, according to this control device, the magnitude of the combined acceleration can be suppressed.

一実施形態の制御装置では、
上記合成カーブ算出部は、
上記第１カーブにおいて上記離脱動作の速度成分がゼロから極大値まで増加する加速期間については、上記合成速度は上記離脱動作の速度成分と上記同期速度がなす速度成分とを合成した値をとり、
上記第１カーブにおいて上記離脱動作の速度成分が上記極大値からゼロまで減少する減速期間については、この減速期間の始期に、上記合成速度が極大値から減少し始め、かつ、上記減速期間の終期に、上記合成速度がゼロとなる態様で、
上記合成カーブを算出することを特徴とする。 In the control device of one embodiment,
The above composite curve calculation unit
For the acceleration period in which the velocity component of the detachment motion increases from zero to the maximum value in the first curve, the combined velocity is a value obtained by combining the velocity component of the detachment motion and the velocity component formed by the synchronous velocity.
Regarding the deceleration period in which the speed component of the withdrawal operation decreases from the maximum value to zero in the first curve, the combined speed starts to decrease from the maximum value at the beginning of the deceleration period, and the end of the deceleration period. In addition, in a mode in which the synthesis rate becomes zero,
It is characterized in that the composite curve is calculated.

この一実施形態の制御装置では、上記合成カーブ算出部は、上記第１カーブにおいて上記離脱動作の速度成分がゼロから極大値まで増加する加速期間については、上記合成速度は上記離脱動作の速度成分と上記同期速度がなす速度成分とを合成した値をとり、上記第１カーブにおいて上記離脱動作の速度成分が上記極大値からゼロまで減少する減速期間については、この減速期間の始期に、上記合成速度が極大値から減少し始め、かつ、上記減速期間の終期に、上記合成速度がゼロとなる態様で、上記合成カーブを算出する。つまり、上記合成カーブのうち上記減速期間に相当する部分を、上記合成カーブが表されるべき座標平面上で、２点を定めて、それら２点の間を滑らかに接続する態様で作成する。このようにして、上記第１カーブと上記スレーブ装置の同期速度とに基づいて、上記合成カーブが算出される。 In the control device of this one embodiment, the composite curve calculation unit determines that the combined speed is the speed component of the detachment operation for the acceleration period in which the speed component of the detachment operation increases from zero to the maximum value in the first curve. The combined value of the speed component formed by the synchronous speed and the speed component formed by the synchronous speed is taken, and the deceleration period in which the speed component of the detachment operation decreases from the maximum value to zero in the first curve is the combined value at the beginning of the deceleration period. The combined curve is calculated in such a manner that the speed starts to decrease from the maximum value and the combined speed becomes zero at the end of the deceleration period. That is, the portion of the composite curve corresponding to the deceleration period is created in such a manner that two points are defined on the coordinate plane on which the composite curve should be represented and the two points are smoothly connected. In this way, the composite curve is calculated based on the first curve and the synchronization speed of the slave device.

一実施形態の制御装置では、
上記合成カーブ補正部は、上記合成カーブを与える上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように、上記減速期間の終期に対して上記合成速度がゼロになる時刻を遅らせて、上記合成カーブを補正する
ことを特徴とする。 In the control device of one embodiment,
At the end of the deceleration period, the composite curve correction unit sets the composite acceleration so that when the composite acceleration giving the composite curve is below or above the negative threshold value, the composite acceleration becomes equal to or exceeds the negative threshold value. On the other hand, the composite curve is corrected by delaying the time when the composite speed becomes zero.

この一実施形態の制御装置では、さらに、上記合成カーブ補正部は、上記合成カーブを与える上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように、上記減速期間の終期に対して上記合成速度がゼロになる時刻を遅らせて、上記合成カーブを補正する。このようにして、上記合成カーブが補正される。 In the control device of this one embodiment, the composite curve correction unit further determines whether the composite acceleration becomes the same as the negative threshold when the composite acceleration giving the composite curve is lower than the negative threshold. Or, the composite curve is corrected by delaying the time when the composite speed becomes zero with respect to the end of the deceleration period so as to exceed the speed. In this way, the composite curve is corrected.

第２の局面では、この開示の制御装置は、
マスタ装置とスレーブ装置とを同期させて制御する制御装置であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間を含んで、上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出する第１カーブ算出部と、
上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間を含んで、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する第２カーブ算出部と、
上記第１カーブにおける上記離脱動作の速度成分を与える加速度成分と、上記第２カーブにおける上記同期終了動作の速度成分を与える加速度成分とに基づいて、合成加速度を算出する合成加速度算出部と、
上記合成加速度が予め定められた負の閾値を下回っているか否かを判定する加速度判定部と、
上記合成加速度が上記負の閾値を下回っているとき、上記減速期間と上記同期終了期間とをそれぞれ延長して、上記合成加速度が上記負の閾値と同じになるか又は上回るように補正する合成加速度補正部と、
上記延長された減速期間、上記延長された同期終了期間に基づいて、それぞれ上記第１カーブ、上記第２カーブを更新するカーブ更新部と
を備えたことを特徴とする。 In the second aspect, the control device of this disclosure is
A control device that synchronizes and controls the master device and slave device.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
A first curve calculation unit that calculates a first curve representing a change over time in the speed component of the detachment operation of the slave device, including a deceleration period in which the speed component of the detachment operation decreases from the maximum value to zero.
A second curve for calculating a second curve representing a change over time in the speed component of the synchronization end operation of the slave device, including a synchronization end period in which the speed component of the synchronization end operation decreases from the synchronization speed of the slave device to zero. Calculation part and
A composite acceleration calculation unit that calculates a composite acceleration based on an acceleration component that gives a velocity component of the detachment motion in the first curve and an acceleration component that gives a velocity component of the synchronization end motion in the second curve.
An acceleration determination unit that determines whether or not the combined acceleration is below a predetermined negative threshold value,
When the combined acceleration is below the negative threshold, the deceleration period and the synchronization end period are extended, respectively, and the combined acceleration is corrected so that it becomes the same as or exceeds the negative threshold. Correction part and
It is characterized by including a curve updating unit for updating the first curve and the second curve, respectively, based on the extended deceleration period and the extended synchronization end period, respectively.

この開示の制御装置では、第１カーブ算出部が、上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間を含んで、上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出する。第２カーブ算出部が、上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間を含んで、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する。合成加速度算出部は、上記第１カーブと上記第２カーブとに基づいて、上記離脱動作の速度成分を与える加速度成分と、上記同期終了動作の速度成分を与える加速度成分とを用いて、合成加速度を算出する。加速度判定部は、上記合成加速度が予め定められた負の閾値を下回っているか否かを判定する。上記合成加速度が上記負の閾値を下回っているとき、合成加速度補正部は、上記減速期間と上記同期終了期間とをそれぞれ延長して、上記合成加速度が上記負の閾値と同じになるか又は上回るように補正する。カーブ更新部は、上記延長された減速期間、上記延長された同期終了期間に基づいて、それぞれ上記第１カーブ、上記第２カーブを更新する。このようにして、上記第１カーブと上記第２カーブとが作成される。 In the control device of the present disclosure, the first curve calculation unit includes a deceleration period in which the speed component of the detachment operation decreases from the maximum value to zero, and represents the time course of the velocity component of the detachment operation of the slave device. Calculate the curve. The second curve calculation unit represents the time-dependent change of the speed component of the synchronization end operation of the slave device, including the synchronization end period in which the speed component of the synchronization end operation decreases from the synchronization speed of the slave device to zero. Calculate the curve. Based on the first curve and the second curve, the combined acceleration calculation unit uses the acceleration component that gives the speed component of the withdrawal operation and the acceleration component that gives the speed component of the synchronous end operation, and uses the combined acceleration. Is calculated. The acceleration determination unit determines whether or not the combined acceleration is below a predetermined negative threshold value. When the combined acceleration is below the negative threshold, the combined acceleration correction unit extends the deceleration period and the synchronization end period, respectively, so that the combined acceleration becomes the same as or exceeds the negative threshold. Correct as follows. The curve update unit updates the first curve and the second curve, respectively, based on the extended deceleration period and the extended synchronization end period, respectively. In this way, the first curve and the second curve are created.

上記制御装置が実行する同期終了モードでは、上記スレーブ装置の離脱動作の速度成分が上記第１カーブに従って制御され、また、上記スレーブ装置の同期終了動作の速度成分が上記第２カーブに従って制御される。これに伴って、上記スレーブ装置は上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度で動作する。このとき、上記合成速度を与える合成加速度（上記離脱動作の加速度成分と上記同期終了動作の加速度成分とを含む）は上記負の閾値を下回らないものになっている。したがって、この制御装置によれば、上記合成加速度の大きさを抑制できる。 In the synchronous end mode executed by the control device, the speed component of the detachment operation of the slave device is controlled according to the first curve, and the speed component of the synchronous end operation of the slave device is controlled according to the second curve. .. Along with this, the slave device operates at a combined speed including the speed component of the detachment operation and the speed component of the synchronization end operation. At this time, the combined acceleration that gives the combined speed (including the acceleration component of the withdrawal operation and the acceleration component of the synchronous end operation) does not fall below the negative threshold value. Therefore, according to this control device, the magnitude of the combined acceleration can be suppressed.

一実施形態の制御装置では、上記第２カーブ算出部は、上記減速期間に対して上記同期終了期間が一致する態様で、上記第２カーブを算出することを特徴とする。 The control device of one embodiment is characterized in that the second curve calculation unit calculates the second curve in a manner in which the synchronization end period coincides with the deceleration period.

本明細書で、減速期間に対して同期終了期間が「一致」するとは、上記減速期間の始期と上記同期終了期間の始期とが一致し、かつ、上記減速期間の終期と上記同期終了期間の終期とが一致することを意味する。 In the present specification, when the synchronization end period "matches" the deceleration period, the beginning of the deceleration period and the beginning of the synchronization end period coincide with each other, and the end of the deceleration period and the synchronization end period It means that it coincides with the end.

一実施形態の制御装置では、上記第２カーブ算出部は、上記減速期間に対して上記同期終了期間が一致する態様で、上記第２カーブを算出する。上記減速期間と上記同期終了期間とが一致していれば、上記離脱動作と上記同期終了動作とが同時に終了する。したがって、離脱動作と同期終了動作とが別々に終了する場合とは異なり、ユーザに違和感を与え難い。 In the control device of one embodiment, the second curve calculation unit calculates the second curve in such a manner that the synchronization end period coincides with the deceleration period. If the deceleration period and the synchronization end period match, the withdrawal operation and the synchronization end operation end at the same time. Therefore, unlike the case where the withdrawal operation and the synchronous end operation are ended separately, it is unlikely that the user feels uncomfortable.

一実施形態の制御装置では、上記合成加速度補正部は、上記減速期間と上記同期終了期間とが一致する態様で、上記減速期間と上記同期終了期間の終期を延長することを特徴とする。 In the control device of one embodiment, the combined acceleration correction unit is characterized in that the deceleration period and the end of the synchronization end period are extended in such a manner that the deceleration period and the synchronization end period coincide with each other.

この一実施形態の制御装置では、上記第１カーブ、上記第２カーブが更新されたとしても、上記減速期間と上記同期終了期間とが一致しているので、上記離脱動作と上記同期終了動作とが同時に終了する。したがって、ユーザに違和感を与え難い。 In the control device of this one embodiment, even if the first curve and the second curve are updated, the deceleration period and the synchronization end period are the same, so that the withdrawal operation and the synchronization end operation are performed. Ends at the same time. Therefore, it is difficult to give the user a sense of discomfort.

一実施形態の制御装置では、
上記第１カーブにおける上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間の始期、終期で、それぞれ上記離脱動作の速度成分を与える加速度成分がゼロであり、
上記第２カーブにおける上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間の始期、終期で、それぞれ上記同期終了動作の速度成分を与える加速度成分がゼロである
ことを特徴とする。 In the control device of one embodiment,
At the beginning and end of the deceleration period in which the velocity component of the detachment motion in the first curve decreases from the maximum value to zero, the acceleration component that gives the velocity component of the detachment motion is zero, respectively.
At the beginning and end of the synchronization end period in which the speed component of the synchronization end operation in the second curve decreases from the synchronization speed of the slave device to zero, the acceleration component that gives the speed component of the synchronization end operation is zero. It is characterized by.

この一実施形態の制御装置では、上記第１カーブは、上記減速期間の始期で緩やかに減速を開始し、上記減速期間の終期で緩やかにゼロになる。上記第２カーブは、上記同期終了期間の始期で緩やかに減速を開始し、上記同期終了期間の終期で緩やかにゼロになる。したがって、上記第１カーブと上記第２カーブとを合成してなる合成カーブが緩やかになって、上記スレーブ装置にかかる負荷が軽くなる。 In the control device of this one embodiment, the first curve gradually starts decelerating at the beginning of the deceleration period and gradually becomes zero at the end of the deceleration period. The second curve gradually starts decelerating at the beginning of the synchronization end period and gradually becomes zero at the end of the synchronization end period. Therefore, the combined curve formed by combining the first curve and the second curve becomes gentle, and the load applied to the slave device becomes lighter.

第３の局面では、この開示の制御方法は、
マスタ装置とスレーブ装置とを同期させて制御する制御方法であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出し、
上記第１カーブと上記スレーブ装置の同期速度とに基づいて、上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度の経時変化を表す合成カーブを算出し、
上記合成カーブを与える合成加速度が予め定められた負の閾値を下回っているか否かを判定し、
上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように上記合成カーブを補正し、
上記第１カーブと上記補正された合成カーブとに基づいて、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する
ことを特徴とする。 In the third aspect, the control method of this disclosure is
It is a control method that controls the master device and the slave device in synchronization.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
The first curve representing the time-dependent change of the speed component of the detachment operation of the slave device was calculated.
Based on the first curve and the synchronization speed of the slave device, a synthesis curve representing a time-dependent change in the synthesis speed including the speed component of the detachment operation and the speed component of the synchronization end operation is calculated.
It is determined whether or not the composite acceleration that gives the composite curve is below a predetermined negative threshold value.
When the combined acceleration is below or above the negative threshold, the composite curve is corrected so that the combined acceleration is equal to or greater than the negative threshold.
Based on the first curve and the corrected composite curve, a second curve representing a time-dependent change in the speed component of the synchronous end operation of the slave device is calculated.

この開示の制御方法によれば、上記合成加速度の大きさを抑制できる。 According to the control method of this disclosure, the magnitude of the combined acceleration can be suppressed.

第４の局面では、この開示の制御方法は、
マスタ装置とスレーブ装置とを同期させて制御する制御方法であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間を含んで、上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出し、
上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間を含んで、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出し、
上記第１カーブにおける上記離脱動作の速度成分を与える加速度成分と、上記第２カーブにおける上記同期終了動作の速度成分を与える加速度成分とに基づいて、合成加速度を算出し、
上記合成加速度が予め定められた負の閾値を下回っているか否かを判定し、
上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように、上記減速期間と上記同期終了期間とをそれぞれ延長し、
上記延長された減速期間、上記延長された同期終了期間に基づいて、それぞれ上記第１カーブ、上記第２カーブを更新する
ことを特徴とする。 In the fourth aspect, the control method of this disclosure is
It is a control method that controls the master device and the slave device in synchronization.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
A first curve representing the change over time of the speed component of the detachment operation of the slave device is calculated, including the deceleration period in which the speed component of the detachment operation decreases from the maximum value to zero.
A second curve representing the change over time of the speed component of the synchronization end operation of the slave device is calculated, including the synchronization end period in which the speed component of the synchronization end operation decreases from the synchronization speed of the slave device to zero.
The combined acceleration is calculated based on the acceleration component that gives the velocity component of the withdrawal motion in the first curve and the acceleration component that gives the velocity component of the synchronous end motion in the second curve.
It is determined whether or not the combined acceleration is below a predetermined negative threshold value.
When the combined acceleration is below or above the negative threshold, the deceleration period and the synchronization end period are extended so that the combined acceleration is equal to or exceeds the negative threshold.
It is characterized in that the first curve and the second curve are updated based on the extended deceleration period and the extended synchronization end period, respectively.

この開示の制御方法によれば、上記合成加速度の大きさを抑制できる。 According to the control method of this disclosure, the magnitude of the combined acceleration can be suppressed.

第５の局面では、この開示のプログラムは、第３の局面または第４の局面の制御方法をコンピュータに実行させるためのプログラムである。 In the fifth aspect, the program of this disclosure is a program for causing a computer to execute the control method of the third aspect or the fourth aspect.

この開示のプログラムをコンピュータに実行させることによって、上記第３の局面または第４の局面の制御方法を実施することができる。 By having the computer execute the program of this disclosure, the control method of the third aspect or the fourth aspect can be implemented.

以上より明らかなように、この開示の制御装置、制御方法およびプログラムによれば、上記マスタ装置から上記スレーブ装置を離脱させる離脱動作の加速度成分と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作の加速度成分とを含む合成加速度の大きさを抑制できる。 As is clear from the above, according to the control device, the control method, and the program of the present disclosure, the acceleration component of the detachment operation for separating the slave device from the master device and the synchronization of the slave device with the master device are terminated. The magnitude of the combined acceleration including the acceleration component of the synchronous end operation can be suppressed.

この発明の一実施形態の制御装置と、ベルトコンベアを含むマスタ装置と、６軸多関節ロボットを含むスレーブ装置とを備えた制御システムの外観を模式的に示す図である。It is a figure which shows typically the appearance of the control system which includes the control device of one Embodiment of this invention, the master device which includes a belt conveyor, and the slave device which includes a 6-axis articulated robot. 上記制御システムのブロック構成を示す図である。It is a figure which shows the block structure of the said control system. 上記制御装置による第１の制御方法のフローを示す図である。It is a figure which shows the flow of the 1st control method by the said control device. 図４（Ａ）は、上記第１の制御方法によって算出された、上記マスタ装置から上記スレーブ装置を離脱させる離脱動作の速度成分ｖｚ１１、加速度成分ａｚ１１の経時変化を示す図である。図４（Ｂ）は、上記第１の制御方法によって一旦算出された合成速度ｖｃ１０、合成加速度ａｃ１０の経時変化、および、補正された合成速度ｖｃ１０′、合成加速度ａｃ１０′の経時変化を示す図である。図４（Ｃ）は、上記第１の制御方法によって算出された、上記マスタ装置に対する上記ロボットの同期を終了させる同期終了動作の速度成分ｖｘ１２、加速度成分ａｘ１２の経時変化を示す図である。FIG. 4A is a diagram showing changes over time in the speed component vz11 and the acceleration component az11 of the detachment operation for detaching the slave device from the master device, which is calculated by the first control method. FIG. 4B is a diagram showing the time-dependent changes in the combined speed vc10 and the combined acceleration ac10 once calculated by the first control method, and the corrected combined speed vc10'and the combined acceleration ac10'. is there. FIG. 4C is a diagram showing changes over time in the speed component vx12 and the acceleration component ax12 of the synchronization end operation for ending the synchronization of the robot with the master device, which is calculated by the first control method. 上記制御装置による第２の制御方法のフローを示す図である。It is a figure which shows the flow of the 2nd control method by the said control device. 図６（Ａ）は、上記第２の制御方法によって一旦算出された離脱動作の速度成分ｖｚ２１、加速度成分ａｚ２１の経時変化、および、更新された離脱動作の速度成分ｖｚ２１′、加速度成分ａｚ２１′の経時変化を示す図である。図６（Ｂ）は、上記第２の制御方法によって一旦算出された合成速度ｖｃ２０、合成加速度ａｃ２０の経時変化、および、補正された合成速度ｖｃ２０′、合成加速度ａｃ２０′の経時変化を示す図である。図６（Ｃ）は、上記第２の制御方法によって一旦算出された同期終了動作の速度成分ｖｘ２２、加速度成分ａｘ２２の経時変化、および、更新された同期終了動作の速度成分ｖｘ２２′、加速度成分ａｘ２２′の経時変化を示す図である。FIG. 6A shows the time-dependent changes in the velocity component vz21 and the acceleration component az21 of the detachment motion once calculated by the second control method, and the updated velocity component vz21'and the acceleration component az21'of the detachment motion. It is a figure which shows the time-dependent change. FIG. 6B is a diagram showing the time course of the combined speed vc20 and the combined acceleration ac20 once calculated by the second control method, and the time-dependent changes of the corrected combined speed vc20'and the combined acceleration ac20'. is there. FIG. 6C shows the velocity component vx22 and the acceleration component ax22 of the synchronous end operation once calculated by the second control method, and the speed component vx22'and the acceleration component ax22 of the updated synchronous end operation. It is a figure which shows the time-dependent change of ′. 図７（Ａ）は、従来の制御方法による、ベルトコンベアからロボットを離脱させる離脱動作の速度成分ｖｚ、加速度成分ａｚの経時変化を示す図である。図７（Ｂ）は、上記従来の制御方法による、上記ベルトコンベアに対する上記ロボットの同期を終了させる同期終了動作の速度成分ｖｘ、加速度成分ａｘの経時変化を示す図である。図７（Ｃ）は、上記従来の制御方法による、上記離脱動作の速度成分ｖｚと上記同期終了動作の速度成分ｖｘとを含む合成速度ｖｃ、上記離脱動作の加速度成分ａｚと上記同期終了動作の加速度成分ａｘとを含む合成加速度ａｃの経時変化を示す図である。FIG. 7A is a diagram showing changes over time in the speed component vz and the acceleration component az of the detachment operation of detaching the robot from the belt conveyor by the conventional control method. FIG. 7B is a diagram showing changes over time in the speed component vx and the acceleration component ax of the synchronization end operation for ending the synchronization of the robot with respect to the belt conveyor by the conventional control method. FIG. 7C shows the combined speed vc including the speed component vz of the withdrawal operation and the speed component vx of the synchronous end operation, the acceleration component az of the withdrawal operation, and the synchronous end operation according to the conventional control method. It is a figure which shows the time-dependent change of the synthetic acceleration ac including the acceleration component ax.

以下、この発明の実施の形態を、図面を参照しながら詳細に説明する。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

図１は、この発明の一実施形態の制御装置１０を適用した制御システム１００の外観を模式的に示している。また、図２は、その制御システム１００のブロック構成を示している。これらの図に示すように、この制御システム１００は、大別して、１軸のベルトコンベアであるマスタ装置１０１と、６軸多関節ロボットを含むスレーブ装置１０２と、これらのマスタ装置１０１とスレーブ装置１０２とを一定周期ｔｓ（例えば、ｔｓ＝０．５ｍｓｅｃ〜１ｍｓｅｃ程度の周期）で同期させて制御する制御装置１０とを備えている。 FIG. 1 schematically shows the appearance of a control system 100 to which the control device 10 according to the embodiment of the present invention is applied. Further, FIG. 2 shows a block configuration of the control system 100. As shown in these figures, the control system 100 is roughly classified into a master device 101 which is a 1-axis belt conveyor, a slave device 102 including a 6-axis articulated robot, and these master device 101 and a slave device 102. It is provided with a control device 10 for synchronizing and controlling with and at a constant cycle ts (for example, a cycle of about 0.5 msec to 1 msec).

図１に示すように、マスタ装置１０１は、この例では、制御装置１０からの指令値ＣＶｍに応じてベルト１０１ＡをＸ軸方向に駆動するモータ１１１と、このモータ１１１と一体に構成され、モータ１１１の現在値（現在位置）ＣＶｍ′を測定するエンコーダ１１２と、制御装置１０からの指令値ＣＶｍおよびエンコーダ１１２からの現在値ＣＶｍ′を表す信号に基づいてモータ１１１を駆動するサーボアンプ１１３とを含んでいる。この例では、ベルト１０１Ａ上の作業対象物（以下「ワーク」と呼ぶ。）９０は、矢印Ａで示すように、Ｘ軸方向に移動される。この例では、マスタ装置１０１のＸ軸は、ベルト１０１Ａの搬送面（この例では、水平面）に沿っているものとする。 As shown in FIG. 1, in this example, the master device 101 is integrally configured with a motor 111 that drives the belt 101A in the X-axis direction in response to a command value CVm from the control device 10, and the motor 111. The encoder 112 that measures the current value (current position) CVm'of 111, and the servo amplifier 113 that drives the motor 111 based on the signal representing the command value CVm from the control device 10 and the current value CVm' from the encoder 112. Includes. In this example, the work object (hereinafter referred to as “work”) 90 on the belt 101A is moved in the X-axis direction as shown by the arrow A. In this example, it is assumed that the X-axis of the master device 101 is along the transport surface (horizontal plane in this example) of the belt 101A.

スレーブ装置１０２は、この例では、６軸多関節ロボット１２１と、制御装置１０からの指令値ＣＶｎを表す信号に応じてロボット１２１を駆動するロボットアンプ１２２とを含んでいる。 In this example, the slave device 102 includes a 6-axis articulated robot 121 and a robot amplifier 122 that drives the robot 121 in response to a signal representing a command value CVn from the control device 10.

この例では、マスタ装置１０１は、１軸（Ｘ軸）の自由度を有している。スレーブ装置１０２は、６軸ｘ，ｙ，ｚ，ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌの自由度を有している。 In this example, the master device 101 has one axis (X-axis) of freedom. The slave device 102 has 6 degrees of freedom of x, y, z, yaw, pitch, and roll.

図２に示すように、制御装置１０は、ユーザによって指定されたプログラムを実行するプログラム実行部５０と、マスタ装置指令値演算部２０と、中央演算部３０と、スレーブ装置指令値演算部４０とを備えている。この例では、マスタ装置指令値演算部２０、中央演算部３０、およびスレーブ装置指令値演算部４０が、演算部を構成している。 As shown in FIG. 2, the control device 10 includes a program execution unit 50 that executes a program specified by the user, a master device command value calculation unit 20, a central calculation unit 30, and a slave device command value calculation unit 40. It has. In this example, the master device command value calculation unit 20, the central calculation unit 30, and the slave device command value calculation unit 40 constitute the calculation unit.

マスタ装置指令値演算部２０は、プログラム実行部５０からの指示を受けて、或る軸数ｍ（この例では、ｍ＝１）のマスタ装置１０１を制御するために、その軸数ｍと同数ｍの要素からなる、マスタ装置１０１への指令値（マスタ装置指令値）ＣＶｍを演算して作成する。このマスタ装置指令値ＣＶｍを表す信号は、マスタ装置１０１の各軸（この例では、Ｘ軸）の動作計画を表す情報として、マスタ装置１０１へ送信される。マスタ装置１０１のサーボアンプ１１３は、各軸（この例では、Ｘ軸）のマスタ装置指令値ＣＶｍを、エンコーダ１１２からの現在値ＣＶｍ′を反映して一定周期ｔｓで更新して、モータ１１１を駆動する。現在値ＣＶｍ′は、マスタ装置指令値演算部２０へ送信される。これにより、制御装置１０（特に、マスタ装置指令値演算部２０）によって、マスタ装置１０１が制御される。以下の例では、マスタ装置１０１は、Ｘ軸方向に一定の速度ｖｘｍで移動しているものとする。 The master device command value calculation unit 20 receives an instruction from the program execution unit 50 and has the same number as the number of axes m in order to control the master device 101 having a certain number of axes m (m = 1 in this example). The command value (master device command value) CVm to the master device 101, which is composed of the elements of m, is calculated and created. The signal representing the master device command value CVm is transmitted to the master device 101 as information representing the operation plan of each axis (X axis in this example) of the master device 101. The servo amplifier 113 of the master device 101 updates the master device command value CVm of each axis (X axis in this example) at a constant cycle ts reflecting the current value CVm'from the encoder 112, and updates the motor 111. Drive. The current value CVm'is transmitted to the master device command value calculation unit 20. As a result, the master device 101 is controlled by the control device 10 (particularly, the master device command value calculation unit 20). In the following example, it is assumed that the master device 101 is moving in the X-axis direction at a constant speed vxm.

スレーブ装置指令値演算部４０は、中央演算部３０からの指示を受けて、或る軸数ｎ（この例では、ｎ＝６）のスレーブ装置１０２を制御するために、その軸数ｎと同数ｎの要素からなる、スレーブ装置１０２への指令値（スレーブ装置指令値）ＣＶｎを演算して作成する。このスレーブ装置指令値ＣＶｎを表す信号は、スレーブ装置１０２の各軸（この例では、ｘ，ｙ，ｚ，ｙａｗ，ｐｉｔｃｈ，ｒｏｌｌ軸）の動作計画を表す情報として、スレーブ装置１０２へ送信される。スレーブ装置１０２のロボットアンプ１２２は、各軸のスレーブ装置指令値ＣＶｎを、ロボット１２１からの各軸の現在値ＣＶｎ′を反映して一定周期ｔｓで更新して、ロボット１２１を駆動する。現在値ＣＶｎ′は、スレーブ装置指令値演算部４０へ送信される。これにより、制御装置１０（特に、スレーブ装置指令値演算部４０）によって、スレーブ装置１０２が制御される。 The slave device command value calculation unit 40 receives an instruction from the central calculation unit 30 and has the same number as the number of axes n in order to control the slave device 102 having a certain number of axes n (n = 6 in this example). The command value (slave device command value) CVn to the slave device 102, which is composed of n elements, is calculated and created. The signal representing the slave device command value CVn is transmitted to the slave device 102 as information representing the operation plan of each axis (x, y, z, yaw, pitch, roll axis in this example) of the slave device 102. .. The robot amplifier 122 of the slave device 102 drives the robot 121 by updating the slave device command value CVn of each axis at a fixed cycle ts reflecting the current value CVn'of each axis from the robot 121. The current value CVn'is transmitted to the slave device command value calculation unit 40. As a result, the slave device 102 is controlled by the control device 10 (particularly, the slave device command value calculation unit 40).

中央演算部３０は、マスタ装置指令値演算部２０からのマスタ装置指令値ＣＶｍに基づいて、スレーブ装置指令値演算部４０を介して、マスタ装置１０１とスレーブ装置１０２とを同期させて制御する（同期制御）。この同期制御では、スレーブ装置１０２は、指令値ＣＶｎを表す信号を受けて、図１中に矢印Ｂで示すように、マスタ装置１０１の位置の変化（すなわち、矢印Ａで示したワーク９０の移動）に同期して、ｘ軸に関して単純追従を行い、ｚ軸に関してカム曲線ｐに従って加速または減速されて移動する。 The central calculation unit 30 controls the master device 101 and the slave device 102 in synchronization with each other via the slave device command value calculation unit 40 based on the master device command value CVm from the master device command value calculation unit 20 ( Synchronous control). In this synchronous control, the slave device 102 receives a signal representing the command value CVn and changes the position of the master device 101 (that is, the movement of the work 90 indicated by the arrow A) as shown by the arrow B in FIG. ), A simple follow-up is performed with respect to the x-axis, and the movement is accelerated or decelerated according to the cam curve p with respect to the z-axis.

この同期制御により、例えば、ベルト１０１ＡによってＸ方向に搬送されるワーク９０を、ベルト１０１ＡのＸ方向の動きに同期して、ロボット１２１の先端に取り付けられたエンドエフェクタ１２１ｅによって、拾い上げることができる。 By this synchronous control, for example, the work 90 conveyed in the X direction by the belt 101A can be picked up by the end effector 121e attached to the tip of the robot 121 in synchronization with the movement of the belt 101A in the X direction.

特に、この例では、制御装置１０は、同期制御を終了するための同期終了モードを有する。この同期終了モードでは、マスタ装置１０１からスレーブ装置１０２をｚ方向に離脱させる離脱動作と、マスタ装置１０１に対するスレーブ装置１０２のｘ軸に関する同期を終了させる同期終了動作とが並行して実行される。 In particular, in this example, the control device 10 has a synchronous end mode for terminating the synchronous control. In this synchronization end mode, the detachment operation of separating the slave device 102 from the master device 101 in the z direction and the synchronization end operation of ending the synchronization of the slave device 102 with respect to the master device 101 with respect to the x-axis are executed in parallel.

この同期終了モードでは、例えば、ワーク９０を拾い上げた後、ロボット１２１のエンドエフェクタ１２１ｅは、拾い上げられたワーク９０がベルト１０１Ａ上の後続のワーク（図示せず）と衝突するのを避けるために、ベルト１０１Ａから上方（ｚ方向）へ離れる動作（離脱動作）を行いながら、ｘ方向の動きを止める動作（同期終了動作）を行う。これにより、タクトタイムが短縮されて、生産性の向上が図られる。 In this synchronous end mode, for example, after picking up the work 90, the end effector 121e of the robot 121 avoids the picked up work 90 colliding with a subsequent work (not shown) on the belt 101A. While performing an operation of moving away from the belt 101A upward (z direction) (disengagement operation), an operation of stopping the movement in the x direction (synchronization end operation) is performed. As a result, the tact time is shortened and the productivity is improved.

ここで、同期終了モードでは、スレーブ装置１０２は上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度で動作する。このため、ロボット１２１のモータにかかる負荷が大きくなり過ぎる可能性がある。また、ワーク９０の慣性力が大きくなって、ロボット１２１のエンドエフェクタ１２１ｅがワーク９０を落とす可能性がある。そこで、この制御装置１０では、次のような第１の制御方法または第２の制御方法によって、合成加速度の大きさを抑制する。 Here, in the synchronization end mode, the slave device 102 operates at a combined speed including the speed component of the detachment operation and the speed component of the synchronization end operation. Therefore, the load applied to the motor of the robot 121 may become too large. Further, the inertial force of the work 90 may increase, and the end effector 121e of the robot 121 may drop the work 90. Therefore, in this control device 10, the magnitude of the combined acceleration is suppressed by the following first control method or second control method.

（第１の制御方法）
図３は、制御装置１０による第１の制御方法のフローを示している。 (First control method)
FIG. 3 shows the flow of the first control method by the control device 10.

まず、同期終了命令が起動されると、図３のステップＳ１で、中央演算部３０は第１カーブ算出部として働いて、図４（Ａ）に示すように、スレーブ装置１０２の離脱動作の速度成分ｖｚ１１の経時変化を表す第１カーブＣ１１を算出する。具体的には、まず速度成分ｖｚ１１を与える加速度成分ａｚ１１の経時変化を設定し、これに応じて、速度成分ｖｚ１１の経時変化を算出する。なお、図３中のステップＳ１〜Ｓ４の処理は、この例では時刻ｔ＝０の直前に行われるものとする。図４（Ａ）〜図４（Ｃ）の時間軸ｔは、同期制御の周期ｔｓ（＝０．５ｍｓｅｃ〜１ｍｓｅｃ程度）よりも極めて大きい範囲（秒オーダ）を表している。この点、後述の図６（Ａ）〜図６（Ｃ）でも同様である。 First, when the synchronization end command is activated, in step S1 of FIG. 3, the central calculation unit 30 acts as the first curve calculation unit, and as shown in FIG. 4A, the speed of the detachment operation of the slave device 102. The first curve C11 representing the change with time of the component vz11 is calculated. Specifically, first, the time-dependent change of the acceleration component az11 that gives the velocity component vz11 is set, and the time-dependent change of the velocity component vz11 is calculated accordingly. It should be noted that the processing of steps S1 to S4 in FIG. 3 is assumed to be performed immediately before the time t = 0 in this example. The time axis t in FIGS. 4 (A) to 4 (C) represents a range (second order) extremely larger than the period ts (= about 0.5 msec to 1 msec) of the synchronous control. This point is the same in FIGS. 6 (A) to 6 (C) described later.

この例では、図４（Ａ）中に実線で示すように、加速度成分ａｚ１１は、時刻ｔ＝０でゼロから増加し始め、時刻ｔ１で正のピークを示し、時刻ｔ２でゼロとなり、時刻ｔ３までゼロを維持し、時刻ｔ３でゼロから減少し始め、時刻ｔ４で負のピークａｚ１１ｐを示し、時刻ｔ６でゼロとなっている。離脱動作の加速期間ｔａ（ｔ＝０〜ｔ２）と減速期間ｔｄ（ｔ＝ｔ３〜ｔ６）とで、それぞれ三角制御がなされている。これに応じて、図４（Ａ）中に１点鎖線で示すように、速度成分ｖｚ１１は、時刻ｔ＝０でゼロから緩やかに増加し始め、時刻ｔ２で緩やかに極大値ｖｚ１１ｐに達し、時刻ｔ３まで極大値ｖｚ１１ｐを維持し、時刻ｔ３で極大値ｖｚ１１ｐから緩やかに減少し始め、時刻ｔ６で緩やかにゼロとなっている。中央演算部３０は、このような速度成分ｖｚ１１の経時変化を表す第１カーブＣ１１を算出する。 In this example, as shown by the solid line in FIG. 4 (A), the acceleration component az11 starts to increase from zero at time t = 0, shows a positive peak at time t1, becomes zero at time t2, and becomes zero at time t3. It keeps zero until, starts decreasing from zero at time t3, shows a negative peak az11p at time t4, and becomes zero at time t6. Triangular control is performed for the acceleration period ta (t = 0 to t2) and the deceleration period td (t = t3 to t6) of the detachment operation, respectively. Correspondingly, as shown by the one-point chain line in FIG. 4 (A), the velocity component vz11 begins to gradually increase from zero at time t = 0, gradually reaches the maximum value vz11p at time t2, and time. The maximum value vz11p is maintained until t3, and it starts to gradually decrease from the maximum value vz11p at time t3 and gradually becomes zero at time t6. The central calculation unit 30 calculates the first curve C11 representing such a change with time of the velocity component vz11.

なお、速度成分ｖｚ１１が示す第１カーブＣ１１と横軸（時間軸）とで囲まれた領域（図４（Ａ）中に斜線で示す）の面積Ｓｚ１１は、ロボット１２１のエンドエフェクタ１２１ｅがベルト１０１Ａの搬送面（上面）からｚ方向に離脱する距離（離脱距離）に相当する。 The area Sz11 of the region surrounded by the first curve C11 indicated by the velocity component vz11 and the horizontal axis (time axis) (indicated by diagonal lines in FIG. 4A) is such that the end effector 121e of the robot 121 has the belt 101A. Corresponds to the distance (separation distance) from the transport surface (upper surface) of the above in the z direction.

次に、図３のステップＳ２で、中央演算部３０は合成カーブ算出部として働いて、図４（Ｂ）に中に１点鎖線で示すように、第１カーブＣ１１とスレーブ装置１０２の同期速度ｖｘｍとに基づいて、離脱動作の速度成分ｖｚ１１と同期終了動作の速度成分ｖｘ１２とを含む合成速度ｖｃ１０の経時変化を表す合成カーブＣ１０を算出する。 Next, in step S2 of FIG. 3, the central calculation unit 30 acts as a composite curve calculation unit, and as shown by the alternate long and short dash line in FIG. 4B, the synchronization speed of the first curve C11 and the slave device 102. Based on vxm, a synthesis curve C10 representing a time-dependent change in the synthesis speed vc10 including the speed component vz11 of the detachment operation and the speed component vx12 of the synchronous end operation is calculated.

この例では、合成速度ｖｃ１０は次のような態様になっている。 In this example, the synthesis rate vc10 has the following aspects.

まず、合成速度ｖｃ１０は、時刻ｔ＝０〜ｔ３の期間については、第１カーブＣ１１が表す速度成分ｖｚ１１と一定の同期速度ｖｘｍがなす速度成分（ｖｘｍと同じ値をもつ）とを合成して得られるものに相当する。具体的には、図４（Ｂ）中に示す合成速度ｖｃ１０は、時刻ｔ＝０でｖｘｍ（同期速度の値）から緩やかに増加し始め、時刻ｔ２で緩やかに極大値ｖｃ１０ｐに達し、時刻ｔ３までｖｃ１０ｐを維持する。合成速度ｖｃ１０の極大値ｖｃ１０ｐは、離脱動作の速度成分ｖｚ１１の極大値ｖｚ１１ｐと同期速度がなす速度成分ｖｘｍとを合成した値になっている。この例では、ｚ軸とｘ軸とが直交していることから、ｖｃ１０ｐ＝｛（ｖｚ１１ｐ）２＋（ｖｘｍ）２｝１／２になっている。 First, the synthesis speed vc10 synthesizes the speed component vz11 represented by the first curve C11 and the speed component (having the same value as vxm) formed by the constant synchronization speed vxm for the period from time t = 0 to t3. Corresponds to what is obtained. Specifically, the combined speed vc10 shown in FIG. 4B starts to gradually increase from vxm (synchronous speed value) at time t = 0, gradually reaches the maximum value vc10p at time t2, and gradually reaches the maximum value vc10p at time t3. Maintain vc10p until. The maximum value vc10p of the combined speed vc10 is a value obtained by combining the maximum value vz11p of the speed component vz11 of the detachment operation and the speed component vxm formed by the synchronous speed. In this example, since the z-axis and the x-axis are orthogonal to each other, vc10p = {(vz11p) 2 + (vxm) 2 } 1/2 .

次に、合成速度ｖｃ１０は、減速期間ｔｄ（時刻ｔ＝ｔ３〜ｔ６の期間）については、減速期間の始期（時刻ｔ３）で極大値ｖｃ１０ｐから緩やかに減少し始め、減速期間の終期（時刻ｔ６）で緩やかにゼロとなる。つまり、合成カーブＣ１０のうち減速期間ｔｄに相当する部分は、合成カーブＣ１０が表されるべき座標平面上で、２点（ｔ３，ｖｃ１０ｐ）、（ｔ６，０）を定めて、それら２点の間を滑らかに接続する態様で算出される。 Next, the combined speed vc10 begins to gradually decrease from the maximum value vc10p at the beginning of the deceleration period (time t3) for the deceleration period td (time t = t3 to t6), and ends at the end of the deceleration period (time t6). ) Will gradually become zero. That is, in the portion of the composite curve C10 corresponding to the deceleration period td, two points (t3, vc10p) and (t6, 0) are defined on the coordinate plane on which the composite curve C10 should be represented, and the two points are defined. It is calculated in a mode in which the spaces are smoothly connected.

このとき、図４（Ｂ）中に実線で示すように、合成速度ｖｃ１０を与える合成加速度ａｃ１０は、加速期間ｔａを含む時刻ｔ＝０〜ｔ３の間については、離脱動作の加速度成分ａｚ１１と全く同じ経時変化を示す。また、合成加速度ａｃ１０は、減速期間ｔｄについては、時刻ｔ３でゼロから減少し始め、時刻ｔ４で負のピークａｃ１０ｐを示し、時刻ｔ６でゼロとなっている（三角制御）。ここで、この例では、離脱動作の速度成分ｖｚ１１の極大値ｖｚ１１ｐに比して合成速度ｖｃ１０の極大値ｖｃ１０ｐの方が大きい（ｖｘｍ分だけ大きい）ことに由来して、合成加速度ａｃ１０が示す負のピークａｃ１０ｐの大きさ（絶対値）は、離脱動作の加速度成分ａｚ１１が示す負のピークａｚ１１ｐの大きさ（絶対値）に比して大きくなっている。 At this time, as shown by the solid line in FIG. 4 (B), the combined acceleration ac10 that gives the combined speed vc10 is completely the same as the acceleration component az11 of the detachment operation during the time t = 0 to t3 including the acceleration period ta. It shows the same change over time. Further, the combined acceleration ac10 starts to decrease from zero at time t3, shows a negative peak ac10p at time t4, and becomes zero at time t6 for the deceleration period td (triangular control). Here, in this example, the maximum value vc10p of the combined speed vc10 is larger (larger by vxm) than the maximum value vz11p of the speed component vz11 of the detachment operation, so that the combined acceleration ac10 shows a negative value. The magnitude (absolute value) of the peak ac10p of No. 1 is larger than the magnitude (absolute value) of the negative peak az11p indicated by the acceleration component az11 of the detachment operation.

次に、図３のステップＳ３で、中央演算部３０は加速度判定部として働いて、合成カーブＣ１０を与える合成加速度ａｃ１０が予め定められた負の閾値ａｃｔｈを下回っているか否かを判定する。ここで、負の閾値ａｃｔｈは、スレーブ装置１０２の仕様の範囲内で、例えばスレーブ装置１０２が取り扱うワークの慣性力を考慮して、予め設定される。より具体的には、そのロボット１２１のエンドエフェクタ１２１ｅがワーク９０を落とさないような値に設定される。この例では、図４（Ｂ）中に示すように、負のピークａｃ１０ｐ（時刻ｔ４）を含む前後の期間で、合成加速度ａｃ１０が負の閾値ａｃｔｈを下回っているものとする（図３のステップＳ３でＹＥＳ）。 Next, in step S3 of FIG. 3, the central calculation unit 30 acts as an acceleration determination unit to determine whether or not the combined acceleration ac10 that gives the composite curve C10 is below a predetermined negative threshold value act. Here, the negative threshold value act is set in advance within the specifications of the slave device 102, for example, in consideration of the inertial force of the work handled by the slave device 102. More specifically, the end effector 121e of the robot 121 is set to a value that does not drop the work 90. In this example, as shown in FIG. 4B, it is assumed that the combined acceleration ac10 is below the negative threshold value act in the period before and after including the negative peak ac10p (time t4) (step in FIG. 3). YES in S3).

合成加速度ａｃ１０が負の閾値ａｃｔｈを下回っているとき、図３のステップＳ４に進んで、中央演算部３０は合成カーブ補正部として働いて、合成加速度ａｃ１０が負の閾値ａｃｔｈと同じになるか又は上回るように合成カーブＣ１０を補正する。この例では、図４（Ｂ）中に破線で示すように、合成加速度ａｃ１０が負の閾値ａｃｔｈで飽和するように、減速期間の終期（時刻ｔ６）に対して合成速度ｖｃ１０がゼロになる時刻をｔ７まで遅らせる。図４（Ｂ）中に、補正された合成加速度を符号ａｃ１０′、補正された合成カーブを符号Ｃ１０′、補正された合成速度を符号ｖｃ１０′で、それぞれ表している。このようにして、合成カーブＣ１０がＣ１０′に補正される。 When the combined acceleration ac10 is below the negative threshold value, the process proceeds to step S4 of FIG. 3, and the central calculation unit 30 acts as the composite curve correction unit to make the combined acceleration ac10 the same as the negative threshold value act. The composite curve C10 is corrected so as to exceed it. In this example, as shown by the broken line in FIG. 4B, the time when the combined speed vc10 becomes zero with respect to the end of the deceleration period (time t6) so that the combined acceleration ac10 saturates at the negative threshold value act. Is delayed to t7. In FIG. 4B, the corrected composite acceleration is represented by the symbol ac10', the corrected composite curve is represented by the symbol C10', and the corrected composite speed is represented by the reference numeral vc10'. In this way, the composite curve C10 is corrected to C10'.

次に、この例では、概念的には、スレーブ装置指令値演算部４０とスレーブ装置１０２のロボットアンプ１２２とが第２カーブ算出部として働いて、第１カーブＣ１１と補正された合成カーブＣ１０′とに基づいて、図４（Ｃ）中に１点鎖線で示すように、スレーブ装置１０２の同期終了動作の速度成分ｖｘ１２の経時変化を表す第２カーブＣ１２を算出する（図３のステップＳ５〜Ｓ１０）。この例では、補正された合成カーブＣ１０′が表す合成速度ｖｃ１０′から第１カーブＣ１１が表す離脱動作の速度成分ｖｚ１１を、時刻（この例では、同期制御の周期ｔｓ）毎に除去して、同期終了動作の速度成分ｖｘ１２を算出する。これにより、第２カーブＣ１２が得られる。第１カーブＣ１１が終端する時刻（離脱動作が終了する時刻）ｔ６に対して、第２カーブＣ１２が終端する時刻（同期終了動作が終了する時刻）ｔ７は、遅れた態様になる。この例では、ｚ軸とｘ軸とが直交していることから、速度成分ｖｘ１２の大きさ（絶対値）は、｜ｖｘ１２｜＝｛（ｖｃ１０′）２−（ｖｚ１１）２｝１／２に相当する。同期終了動作の速度成分ｖｘ１２を与える加速度成分ａｘ１１は、時刻ｔ４でゼロから減少し始め、時刻ｔ６で負のピークを示し、時刻ｔ７でゼロとなっている。 Next, in this example, conceptually, the slave device command value calculation unit 40 and the robot amplifier 122 of the slave device 102 act as the second curve calculation unit, and the first curve C11 and the corrected composite curve C10'are used. Based on the above, as shown by the one-point chain line in FIG. 4C, the second curve C12 representing the time-dependent change of the velocity component vx12 of the synchronous end operation of the slave device 102 is calculated (steps S5 to 3 in FIG. 3). S10). In this example, the speed component vz11 of the detachment operation represented by the first curve C11 is removed from the composite speed vc10'represented by the corrected composite curve C10'for each time (in this example, the synchronous control cycle ts). The velocity component vx12 of the synchronous end operation is calculated. As a result, the second curve C12 is obtained. The time t7 at which the second curve C12 ends (the time at which the synchronization end operation ends) is delayed from the time t6 at which the first curve C11 ends (the time at which the detachment operation ends). In this example, since the z-axis and the x-axis are orthogonal to each other, the magnitude (absolute value) of the velocity component vx12 is set to | vx12 | = {(vc10') 2- (vz11) 2 } 1/2 . Equivalent to. The acceleration component ax11 that gives the velocity component vx12 of the synchronous end operation starts to decrease from zero at time t4, shows a negative peak at time t6, and becomes zero at time t7.

これらの第１カーブＣ２１と第２カーブＣ２２とを表す情報によって、ロボット１２１が駆動される。 The robot 121 is driven by the information representing the first curve C21 and the second curve C22.

具体的には、スレーブ装置１０２のロボットアンプ１２２は、第１カーブＣ１１と補正された合成カーブＣ１０′を表す情報を、スレーブ装置指令値演算部４０を介してスレーブ装置指令値ＣＶｎとして受けながら、図３のステップＳ５に示すように、制御周期ｔｓ毎の処理を開始する。或る制御周期ｔｓについて、第１カーブＣ１１が表す離脱動作の速度成分ｖｚ１１に基づいて、離脱動作の軌跡を算出する（ステップＳ６）。その制御周期ｔｓについて、合成速度ｖｃ１０′から離脱動作の速度成分ｖｚ１１を除去して、同期終了動作の速度成分ｖｘ１２を算出する（ステップＳ７）。その制御周期ｔｓについて、同期終了動作の速度成分ｖｘ１２に基づいて、同期終了動作の軌跡を算出する（ステップＳ８）。その制御周期ｔｓについて、離脱動作と同期終了動作の軌跡を合成する（ステップＳ９）。次の制御周期ｔｓについて、ステップＳ６〜Ｓ９の処理を繰り返す。このようにして、ロボット１２１が停止するまで、制御周期ｔｓ毎にステップＳ６〜Ｓ９の処理を繰り返す（ステップＳ１０）。 Specifically, the robot amplifier 122 of the slave device 102 receives information representing the first curve C11 and the corrected composite curve C10'as the slave device command value CVn via the slave device command value calculation unit 40. As shown in step S5 of FIG. 3, processing for each control cycle ts is started. For a certain control cycle ts, the locus of the withdrawal operation is calculated based on the velocity component vz11 of the withdrawal operation represented by the first curve C11 (step S6). With respect to the control cycle ts, the speed component vz11 of the detachment operation is removed from the synthesis speed vc10', and the speed component vx12 of the synchronous end operation is calculated (step S7). With respect to the control cycle ts, the locus of the synchronous end operation is calculated based on the speed component vx12 of the synchronous end operation (step S8). With respect to the control cycle ts, the trajectories of the withdrawal operation and the synchronous end operation are synthesized (step S9). The processing of steps S6 to S9 is repeated for the next control cycle ts. In this way, the processes of steps S6 to S9 are repeated every control cycle ts until the robot 121 stops (step S10).

なお、合成加速度ａｃ１０が負の閾値ａｃｔｈを下回っていないとき（図３のステップＳ３でＮＯ）、合成カーブＣ１０は、補正されること無く維持される。その場合は、合成カーブＣ１０が表す合成速度ｖｃ１０から第１カーブＣ１１が表す離脱動作の速度成分ｖｚ１１を時刻（この例では、同期制御の周期ｔｓ）毎に除去することによって、第２カーブＣ１２が得られる（図３のステップＳ５〜Ｓ１０）。 When the combined acceleration ac10 does not fall below the negative threshold value act (NO in step S3 of FIG. 3), the combined curve C10 is maintained without being corrected. In that case, the second curve C12 is created by removing the velocity component vz11 of the detachment operation represented by the first curve C11 from the composite speed vc10 represented by the composite curve C10 for each time (in this example, the synchronous control cycle ts). It is obtained (steps S5 to S10 in FIG. 3).

この制御装置１０が実行する同期終了モードでは、スレーブ装置１０２の離脱動作の速度成分ｖｚ１１が第１カーブＣ１１に従って制御され、また、スレーブ装置１０２の同期終了動作の速度成分ｖｘ１２が第２カーブＣ１２に従って制御される。これに伴って、スレーブ装置１０２は離脱動作の速度成分ｖｚ１１と同期終了動作の速度成分ｖｘ１２とを含む合成速度ｖｃ１０（合成カーブＣ１０に従う）で動作する。このとき、この第１の制御方法によれば、合成速度ｖｃ１０を与える合成加速度ａｃ１０（離脱動作の加速度成分ａｚ１１と同期終了動作の加速度成分ａｘ１２とを含む）は負の閾値ａｃｔｈを下回らないものになっている。したがって、合成加速度ａｃ１０の大きさを抑制できる。 In the synchronous end mode executed by the control device 10, the speed component vz11 of the detachment operation of the slave device 102 is controlled according to the first curve C11, and the speed component vx12 of the synchronous end operation of the slave device 102 follows the second curve C12. Be controlled. Along with this, the slave device 102 operates at a combined speed vc10 (according to the combined curve C10) including the speed component vz11 of the detachment operation and the speed component vx12 of the synchronous end operation. At this time, according to this first control method, the combined acceleration ac10 (including the acceleration component az11 of the detachment operation and the acceleration component ax12 of the synchronous end operation) that gives the combined speed vc10 does not fall below the negative threshold value act. It has become. Therefore, the magnitude of the combined acceleration ac10 can be suppressed.

また、上の例では、第１カーブＣ１１における減速期間ｔｄの始期（時刻ｔ３）、終期（時刻ｔ６）で、それぞれ加速度成分ａｚ１１がゼロになっている。同様に、第２カーブＣ１２における同期終了期間ｔｃの始期（時刻ｔ３）、終期（時刻ｔ７）で、それぞれ加速度成分ａｘ１２がゼロになっている。この結果、第１カーブＣ１１と第２カーブＣ１２とを合成してなる合成カーブＣ１０′が緩やかになって、スレーブ装置１０２にかかる負荷が軽くなる。 Further, in the above example, the acceleration component az11 becomes zero at the beginning (time t3) and the end (time t6) of the deceleration period td in the first curve C11, respectively. Similarly, the acceleration component ax12 becomes zero at the beginning (time t3) and the end (time t7) of the synchronization end period ct in the second curve C12. As a result, the combined curve C10 ′ formed by combining the first curve C11 and the second curve C12 becomes gentle, and the load applied to the slave device 102 becomes lighter.

（第２の制御方法）
図５は、制御装置１０による第２の制御方法のフローを示している。 (Second control method)
FIG. 5 shows the flow of the second control method by the control device 10.

まず、同期終了命令が起動されると、図５のステップＳ１１で、中央演算部３０は第１カーブ算出部として働いて、図６（Ａ）に示すように、マスタ装置１０１からスレーブ装置１０２を離脱させる離脱動作の速度成分ｖｚ２１の経時変化を表す第１カーブＣ２１を算出する。具体的には、まず速度成分ｖｚ２１を与える加速度成分ａｚ２１の経時変化を設定し、これに応じて、速度成分ｖｚ２１の経時変化を算出する。なお、図５中のステップＳ１１〜Ｓ１６の処理は、この例では時刻ｔ＝０の直前に行われるものとする。 First, when the synchronization end command is activated, in step S11 of FIG. 5, the central calculation unit 30 acts as the first curve calculation unit, and as shown in FIG. 6 (A), the slave device 102 is moved from the master device 101. The first curve C21 representing the time-dependent change of the velocity component vz21 of the detaching motion to be detached is calculated. Specifically, first, the time-dependent change of the acceleration component az21 that gives the velocity component vz21 is set, and the time-dependent change of the velocity component vz21 is calculated accordingly. It should be noted that the processing of steps S11 to S16 in FIG. 5 is assumed to be performed immediately before the time t = 0 in this example.

この例では、図６（Ａ）中に実線で示すように、加速度成分ａｚ２１は、時刻ｔ＝０でゼロから増加し始め、時刻ｔ１１で正のピークを示し、時刻ｔ１２でゼロとなり、時刻ｔ１３までゼロを維持し、時刻ｔ＝１３でゼロから減少し始め、時刻ｔ１４で負のピークａｚ２１ｐを示し、時刻ｔ１６でゼロとなっている。離脱動作の加速期間ｔａ（時刻ｔ＝０〜ｔ１２の期間）と減速期間ｔｄ（時刻ｔ＝ｔ１３〜ｔ１６の期間）とで、それぞれ三角制御がなされている。これに応じて、図６（Ａ）中に１点鎖線で示すように、速度成分ｖｚ２１は、時刻ｔ＝０でゼロから緩やかに増加し始め、時刻ｔ１２で緩やかに極大値ｖｚ２１ｐに達し、時刻ｔ１３まで極大値ｖｚ２１ｐを維持し、時刻ｔ１３で極大値ｖｚ２１ｐから緩やかに減少し始め、時刻ｔ１６で緩やかにゼロとなっている。中央演算部３０は、このような速度成分ｖｚ２１の経時変化を表す第１カーブＣ２１を算出する。 In this example, as shown by the solid line in FIG. 6A, the acceleration component az21 starts to increase from zero at time t = 0, shows a positive peak at time t11, becomes zero at time t12, and becomes zero at time t13. It keeps zero until, starts decreasing from zero at time t = 13, shows a negative peak az21p at time t14, and becomes zero at time t16. Triangular control is performed for the acceleration period ta (time t = 0 to t12) and deceleration period td (time t = t13 to t16) of the withdrawal operation. Correspondingly, as shown by the one-point chain line in FIG. 6A, the velocity component vz21 starts to gradually increase from zero at time t = 0, gradually reaches the maximum value vz21p at time t12, and reaches the time. The maximum value vz21p is maintained until t13, and it starts to gradually decrease from the maximum value vz21p at time t13 and gradually becomes zero at time t16. The central calculation unit 30 calculates the first curve C21 representing such a change with time of the velocity component vz21.

次に、図５のステップＳ１２で、中央演算部３０は、離脱動作の減速期間ｔｄを取得して、同期終了動作のための同期終了期間ｔｃとする。すなわち、減速期間ｔｄの始期（時刻ｔ１３）を同期終了期間ｔｃの始期とし、また、減速期間ｔｄの終期（時刻ｔ１６）を同期終了期間ｔｃの終期とする。これにより、減速期間ｔｄと一致する態様で、同期終了期間ｔｃを定める。 Next, in step S12 of FIG. 5, the central calculation unit 30 acquires the deceleration period td of the withdrawal operation and sets it as the synchronization end period ct for the synchronization end operation. That is, the start of the deceleration period td (time t13) is set as the start of the synchronization end period ct, and the end of the deceleration period td (time t16) is set as the end of the synchronization end period ct. Thereby, the synchronization end period tc is determined in a manner that coincides with the deceleration period td.

次に、図５のステップＳ１３で、中央演算部３０は第２カーブ算出部として働いて、図６（Ｃ）に示すように、スレーブ装置１０２の同期終了動作の速度成分ｖｘ２２の経時変化を表す第２カーブＣ２２を算出する。具体的には、まず同期終了期間ｔｃにおいて、速度成分ｖｘ２２を与える加速度成分ａｘ２２の経時変化を設定し、これに応じて、速度成分ｖｘ２２の経時変化を算出する。 Next, in step S13 of FIG. 5, the central calculation unit 30 acts as a second curve calculation unit, and as shown in FIG. 6C, represents the time-dependent change of the speed component vx22 of the synchronous end operation of the slave device 102. The second curve C22 is calculated. Specifically, first, in the synchronization end period ct, the time-dependent change of the acceleration component ax22 that gives the velocity component vx22 is set, and the time-dependent change of the velocity component vx22 is calculated accordingly.

この例では、図６（Ｃ）中に実線で示すように、加速度成分ａｘ２２は、時刻ｔ＝０〜ｔ１３までゼロを維持し、時刻ｔ１３でゼロから減少し始め、時刻ｔ１４で負のピークａｘ２２ｐを示し、時刻ｔ１６でゼロとなっている。同期終了期間ｔｃにおいて、三角制御がなされている。これに応じて、図６（Ｃ）中に１点鎖線で示すように、速度成分ｖｘ２２は、時刻ｔ＝０〜ｔ１３までｖｘｍ（同期速度の値）を維持し、時刻ｔ１３でｖｘｍから緩やかに減少し始め、時刻ｔ１６で緩やかにゼロとなっている。中央演算部３０は、このような速度成分ｖｘ２２の経時変化を表す第２カーブＣ２２を算出する。 In this example, as shown by the solid line in FIG. 6C, the acceleration component ax22 maintains zero until time t = 0 to t13, starts decreasing from zero at time t13, and has a negative peak ax22p at time t14. Is shown, and it becomes zero at time t16. Triangular control is performed during the synchronization end period ct. Correspondingly, as shown by the alternate long and short dash line in FIG. 6C, the velocity component vx22 maintains vxm (synchronous velocity value) from time t = 0 to t13, and gradually from vxm at time t13. It started to decrease and gradually became zero at time t16. The central calculation unit 30 calculates a second curve C22 representing such a change with time of the velocity component vx22.

次に、図５のステップＳ１４で、中央演算部３０は合成加速度算出部として働いて、第１カーブＣ２１における離脱動作の速度成分ｖｚ２１を与える加速度成分ａｚ２１と、第２カーブＣ２２における同期終了動作の速度成分ｖｘ２２を与える加速度成分ａｘ２２とに基づいて、図６（Ｂ）に示すように、合成加速度ａｃ２０を算出する。この例では、離脱動作の加速度成分ａｚ２１と同期終了動作の加速度成分ａｘ２２とを、時刻（この例では、同期制御の周期ｔｓ）毎に合成して、合成加速度ａｃ２０を算出する。なお、この例では、ｚ軸とｘ軸とが直交していることから、合成加速度ａｃ２０の大きさ（絶対値）は、｜ａｃ２０｜＝｛（ａｚ２１）２＋（ａｘ２２）２｝１／２に相当する。 Next, in step S14 of FIG. 5, the central calculation unit 30 acts as a combined acceleration calculation unit, and the acceleration component az21 that gives the velocity component vz21 of the detachment operation in the first curve C21 and the synchronous end operation in the second curve C22. As shown in FIG. 6B, the combined acceleration ac20 is calculated based on the acceleration component ax22 that gives the velocity component vx22. In this example, the acceleration component az21 of the detachment operation and the acceleration component ax22 of the synchronization end operation are combined for each time (in this example, the synchronization control cycle ts) to calculate the combined acceleration ac20. In this example, since the z-axis and the x-axis are orthogonal to each other, the magnitude (absolute value) of the combined acceleration ac20 is | ac20 | = {(az21) 2 + (ax22) 2 } 1/2. Corresponds to.

この例では、図６（Ｂ）中に実線で示すように、合成加速度ａｃ２０は、加速期間ｔａを含む時刻ｔ＝０〜ｔ１３の間については、離脱動作の加速度成分ａｚ２１と全く同じ経時変化を示す。また、合成加速度ａｃ２０は、減速期間ｔｄについては、時刻ｔ１３でゼロから減少し始め、時刻ｔ１４で負のピークａｃ２０ｐを示し、時刻ｔ１６でゼロとなっている（三角制御）。これに応じて、図６（Ｂ）中に１点鎖線で示すように、合成速度ｖｃ２０（合成カーブＣ２０）は、時刻ｔ＝０でｖｘｍ（同期速度の値）から緩やかに増加し始め、時刻ｔ１２で緩やかに極大値ｖｃ２０ｐ（＝｛（ｖｚ２１ｐ）２＋（ｖｘｍ）２｝１／２）に達し、時刻ｔ１３まで極大値ｖｃ２０ｐを維持し、時刻ｔ１３で極大値ｖｃ２０ｐから緩やかに減少し始め、かつ、時刻ｔ１６で緩やかにゼロとなる（三角制御）。 In this example, as shown by the solid line in FIG. 6B, the combined acceleration ac20 changes with time exactly the same as the acceleration component az21 of the withdrawal operation during the time t = 0 to t13 including the acceleration period ta. Shown. Further, the combined acceleration ac20 starts to decrease from zero at time t13, shows a negative peak ac20p at time t14, and becomes zero at time t16 for the deceleration period td (triangular control). Correspondingly, as shown by the one-point chain line in FIG. 6B, the composite speed vc20 (composite curve C20) begins to gradually increase from vxm (synchronous speed value) at time t = 0, and the time At t12, the maximum value vc20p (= {(vz21p) 2 + (vxm) 2 } 1/2 ) was gradually reached, the maximum value vc20p was maintained until time t13, and at time t13, it began to gradually decrease from the maximum value vc20p. Moreover, it gradually becomes zero at time t16 (triangular control).

なお、この例では、減速期間ｔｄ（＝同期終了期間ｔｃ）における合成加速度ａｃ２０について注目しているため、時刻ｔ＝０〜ｔ１３の間については、合成加速度ａｃ２０の算出を省略してもよい。 In this example, since the combined acceleration ac20 in the deceleration period td (= synchronization end period ct) is focused on, the calculation of the combined acceleration ac20 may be omitted during the time t = 0 to t13.

次に、図５のステップＳ１５で、中央演算部３０は加速度判定部として働いて、合成カーブＣ２０を与える合成加速度ａｃ２０が予め定められた負の閾値ａｃｔｈを下回っているか否かを判定する。ここで、負の閾値ａｃｔｈは、先の例と同様に、スレーブ装置１０２の仕様の範囲内で、例えばスレーブ装置１０２が取り扱うワークの慣性力を考慮して、予め設定される。より具体的には、そのロボット１２１のエンドエフェクタ１２１ｅがワーク９０を落とさないような値に設定される。この例では、図６（Ｂ）中に示すように、負のピークａｃ２０ｐ（時刻ｔ１４）を含む前後の期間で、合成加速度ａｃ２０が負の閾値ａｃｔｈを下回っているものとする（図５のステップＳ１５でＹＥＳ）。 Next, in step S15 of FIG. 5, the central calculation unit 30 acts as an acceleration determination unit to determine whether or not the combined acceleration ac20 that gives the composite curve C20 is below a predetermined negative threshold value act. Here, the negative threshold value act is set in advance within the specifications of the slave device 102, for example, in consideration of the inertial force of the work handled by the slave device 102, as in the previous example. More specifically, the end effector 121e of the robot 121 is set to a value that does not drop the work 90. In this example, as shown in FIG. 6B, it is assumed that the synthetic acceleration ac20 is below the negative threshold value act in the period before and after including the negative peak ac20p (time t14) (step in FIG. 5). YES in S15).

合成加速度ａｃ２０が負の閾値ａｃｔｈを下回っているとき、図５のステップＳ１６に進んで、中央演算部３０は合成加速度補正部として働いて、減速期間ｔｄと同期終了期間ｔｃとをそれぞれ延長して、合成加速度ａｃ２０が負の閾値ａｃｔｈと同じになるか又は上回るように補正する。この例では、図６（Ｂ）中に破線で示すように、合成加速度ａｃ２０の三角制御の面積（合成加速度ａｃ２０が示すＶ状の線分と時間軸ｔとが作る三角形の面積）を維持しながら、負のピークａｃ２０ｐが負の閾値ａｃｔｈと同じになるまで、三角制御の終期（時刻ｔ１６）を時刻ｔ１７まで遅らせる。図６（Ｂ）中に、補正された合成加速度を符号ａｃ２０′で表している。この例では、補正された合成加速度ａｃ２０′の負のピークは、時刻ｔ１５で負の閾値ａｃｔｈと同じになっている。これに伴って、減速期間ｔｄと同期終了期間ｔｃの終期は、時刻ｔ１７まで延長されている。図６（Ｂ）中に、延長された減速期間を符号ｔｄ′、延長された同期終了期間ｔｃ′で、それぞれ表している。このようにして、減速期間ｔｄと同期終了期間ｔｃとが一致する態様で、減速期間ｔｄと同期終了期間ｔｃとがそれぞれ延長される。この例では、減速期間ｔｄと同期終了期間ｔｃの長さは、約１．４倍に延長されたものとする。 When the combined acceleration ac20 is below the negative threshold value act, the process proceeds to step S16 in FIG. 5, and the central calculation unit 30 acts as the combined acceleration correction unit to extend the deceleration period dt and the synchronization end period ct, respectively. , The combined acceleration ac20 is corrected so that it becomes the same as or exceeds the negative threshold value act. In this example, as shown by the broken line in FIG. 6B, the area of the triangular control of the combined acceleration ac20 (the area of the triangle formed by the V-shaped line segment indicated by the combined acceleration ac20 and the time axis t) is maintained. However, the end of the triangular control (time t16) is delayed to time t17 until the negative peak ac20p becomes the same as the negative threshold act. In FIG. 6B, the corrected combined acceleration is represented by the reference numeral ac20'. In this example, the negative peak of the corrected combined acceleration ac20'is the same as the negative threshold act at time t15. Along with this, the end of the deceleration period td and the synchronization end period ct is extended to time t17. In FIG. 6B, the extended deceleration period is represented by the symbol td'and the extended synchronization end period tc', respectively. In this way, the deceleration period td and the synchronization end period ct are extended in such a manner that the deceleration period td and the synchronization end period ct coincide with each other. In this example, the lengths of the deceleration period td and the synchronization end period ct are assumed to be extended by about 1.4 times.

次に、図５のステップＳ１７で、中央演算部３０はカーブ更新部として働いて、延長された減速期間ｔｄ′、延長された同期終了期間ｔｃ′に基づいて、それぞれ第１カーブＣ２１、第２カーブＣ２２を更新する。図６（Ａ）中に、更新された第１カーブを符号Ｃ２１′、更新された離脱動作の速度成分を符号ｖｚ２１′、それを与える加速度成分を符号ａｚ２１′で、それぞれ表している。また、図６（Ｃ）中に、更新された第２カーブを符号Ｃ２２′、更新された離脱動作の速度成分を符号ｖｘ２２′、それを与える加速度成分を符号ａｘ２２′で、それぞれ表している。これらの速度成分ｖｚ２１′，ｖｘ２２′、加速度成分ａｚ２１′，ａｘ２２′は、減速期間ｔｄと同期終了期間ｔｃの長さが延長された分だけ、経時変化が緩やかになっている（この例では、約１／１．４倍になっている。）。このようにして、第１カーブＣ２１、第２カーブＣ２２が、それぞれ更新される。 Next, in step S17 of FIG. 5, the central calculation unit 30 acts as a curve update unit, and based on the extended deceleration period td'and the extended synchronization end period tc', the first curve C21 and the second curve, respectively. The curve C22 is updated. In FIG. 6A, the updated first curve is represented by the reference numeral C21', the updated velocity component of the detachment operation is represented by the reference numeral vz21', and the acceleration component giving the same is represented by the reference numeral az21'. Further, in FIG. 6C, the updated second curve is represented by the reference numeral C22', the updated velocity component of the detachment operation is represented by the reference numeral vx22', and the acceleration component giving the same is represented by the reference numeral ax22'. These velocity components vz21', vx22', and acceleration components az21', ax22' change slowly over time by the amount that the lengths of the deceleration period td and the synchronization end period ct are extended (in this example, It is about 1 / 1.4 times larger.) In this way, the first curve C21 and the second curve C22 are updated, respectively.

なお、合成加速度ａｃ２０が負の閾値ａｃｔｈを下回っていないとき（図５のステップＳ１５でＮＯ）、合成加速度ａｃ２０は、補正されること無く維持される。これに伴って、第１カーブＣ２１、第２カーブＣ２２は、更新されること無く維持される。 When the combined acceleration ac20 does not fall below the negative threshold value act (NO in step S15 of FIG. 5), the combined acceleration ac20 is maintained without being corrected. Along with this, the first curve C21 and the second curve C22 are maintained without being updated.

以下の例では、合成加速度ａｃ２０は補正されたものとし（図５のステップＳ１６）、それに伴って、更新された第１カーブＣ２１′と第２カーブＣ２２′とが作成されたものとする（図５のステップＳ１７）。その場合、これらの第１カーブＣ２１′と第２カーブＣ２２′とを表す情報によって、ロボット１２１が駆動される。 In the following example, it is assumed that the combined acceleration ac20 is corrected (step S16 in FIG. 5), and that the updated first curve C21'and the second curve C22'are created accordingly (FIG. 5). Step S17 of step 5. In that case, the robot 121 is driven by the information representing the first curve C21'and the second curve C22'.

具体的には、スレーブ装置１０２のロボットアンプ１２２は、第１カーブＣ２１′と第２カーブＣ２２′を表す情報を、スレーブ装置指令値演算部４０を介してスレーブ装置指令値ＣＶｎとして受けながら、図５のステップＳ１８に示すように、制御周期ｔｓ毎の処理を開始する。或る制御周期ｔｓについて、第１カーブＣ２１′が表す離脱動作の速度成分ｖｚ２１′に基づいて、離脱動作の軌跡を算出するとともに、第２カーブＣ２２′が表す同期終了動作の速度成分ｖｘ２２′に基づいて、同期終了動作の軌跡を算出する（ステップＳ１９）。その制御周期ｔｓについて、離脱動作と同期終了動作の軌跡を合成する（ステップＳ２０）。次の制御周期ｔｓについて、ステップＳ１９〜Ｓ２０の処理を繰り返す。このようにして、ロボット１２１が停止するまで、制御周期ｔｓ毎にステップＳ１９〜Ｓ２０の処理を繰り返す（ステップＳ２１）。 Specifically, the robot amplifier 122 of the slave device 102 receives information representing the first curve C21'and the second curve C22' as the slave device command value CVn via the slave device command value calculation unit 40, and is shown in FIG. As shown in step S18 of 5, the processing for each control cycle ts is started. For a certain control cycle ts, the locus of the detachment operation is calculated based on the velocity component vz21'of the detachment operation represented by the first curve C21', and the velocity component vx22'of the synchronous end operation represented by the second curve C22'is used. Based on this, the trajectory of the synchronization end operation is calculated (step S19). With respect to the control cycle ts, the trajectories of the withdrawal operation and the synchronous end operation are synthesized (step S20). The processing of steps S19 to S20 is repeated for the next control cycle ts. In this way, the processes of steps S19 to S20 are repeated every control cycle ts until the robot 121 stops (step S21).

この制御装置１０が実行する同期終了モードでは、スレーブ装置１０２の離脱動作の速度成分ｖｚ２１′が第１カーブＣ２１′に従って制御され、また、スレーブ装置１０２の同期終了動作の速度成分ｖｘ２２′が第２カーブＣ２２′に従って制御される。これに伴って、スレーブ装置１０２は離脱動作の速度成分ｖｚ２１′と同期終了動作の速度成分ｖｘ２２′とを含む合成速度ｖｃ２０′で動作する。このとき、この第２の制御方法によれば、合成速度ｖｃ２０′を与える合成加速度ａｃ２０′（離脱動作の加速度成分ａｚ２１′と同期終了動作の加速度成分ａｘ２２′とを含む）は負の閾値ａｃｔｈを下回らないものになっている。したがって、合成加速度ａｃ２０′の大きさを抑制できる。 In the synchronous end mode executed by the control device 10, the speed component vz21'of the detachment operation of the slave device 102 is controlled according to the first curve C21', and the speed component vx22'of the synchronous end operation of the slave device 102 is the second. It is controlled according to the curve C22'. Along with this, the slave device 102 operates at the combined speed vc20'including the speed component vz21'of the detachment operation and the speed component vx22' of the synchronous end operation. At this time, according to this second control method, the synthetic acceleration ac20'(including the acceleration component az21' of the withdrawal operation and the acceleration component ax22' of the synchronous end operation) that gives the combined speed vc20'sets a negative threshold value act. It is not less than that. Therefore, the magnitude of the combined acceleration ac20'can be suppressed.

また、この第２の制御方法によれば、第１カーブＣ２１′における減速期間ｔｄ′と、第２カーブＣ２２′における同期終了期間ｔｃ′とが一致しているので、離脱動作と同期終了動作とが同時に終了する。したがって、離脱動作と同期終了動作とが別々に終了する場合とは異なり、同期終了モードの実行が、ユーザに違和感を与えることがない。この点は、第１カーブＣ２１と第２カーブＣ２２が更新されなかった場合であっても、同様である。 Further, according to this second control method, since the deceleration period td'in the first curve C21'and the synchronization end period tc' in the second curve C22' coincide with each other, the withdrawal operation and the synchronization end operation are performed. Ends at the same time. Therefore, unlike the case where the withdrawal operation and the synchronization end operation are ended separately, the execution of the synchronization end mode does not give a sense of discomfort to the user. This point is the same even when the first curve C21 and the second curve C22 are not updated.

また、上の例では、第１カーブＣ２１′における減速期間ｔｄの始期（時刻ｔ１３）、終期（時刻ｔ１７）で、それぞれ加速度成分ａｚ２１がゼロになっている。同様に、第２カーブＣ２２′における同期終了期間ｔｃの始期（時刻ｔ１３）、終期（時刻ｔ１７）で、それぞれ加速度成分ａｘ２２がゼロになっている。この結果、第１カーブＣ２１′と第２カーブＣ２２′とを合成してなる合成カーブＣ２０′が緩やかになって、スレーブ装置１０２にかかる負荷が軽くなる。 Further, in the above example, the acceleration component az21 becomes zero at the beginning (time t13) and the end (time t17) of the deceleration period td in the first curve C21', respectively. Similarly, the acceleration component ax22 becomes zero at the beginning (time t13) and the end (time t17) of the synchronization end period tc in the second curve C22', respectively. As a result, the combined curve C20', which is a combination of the first curve C21'and the second curve C22', becomes gentle, and the load applied to the slave device 102 becomes lighter.

上述の実施形態では、離脱動作の速度成分、同期終了動作の速度成分、および、合成速度を緩やかに変化させるために、加速度（または加速度成分）の三角制御を採用したが、これに限られるものではない。離脱動作の速度成分、同期終了動作の速度成分、および、合成速度は、例えば余弦曲線の半周期に相当する形状で、極大値から緩やかに減少し始め、緩やかにゼロになってもよい。 In the above-described embodiment, the triangular control of the acceleration (or the acceleration component) is adopted in order to gradually change the speed component of the detachment operation, the speed component of the synchronous end operation, and the combined speed, but the present invention is limited to this. is not. The speed component of the withdrawal operation, the speed component of the synchronous end operation, and the combined speed may, for example, have a shape corresponding to a half cycle of the cosine curve, start to gradually decrease from the maximum value, and gradually become zero.

上述の制御装置１０は、実質的にコンピュータ装置（例えば、プログラマブルロジックコントローラ（programmable logic controller；PLC）など）によって構成され得る。したがって、上述の第１、第２の制御方法は、コンピュータに実行させるためのプログラムとして構成されるのが望ましい。また、それらのプログラムは、それぞれコンピュータ読み取り可能な非一時的（non-transitory）な記録媒体に記録されるのが望ましい。その場合、記録媒体に記録されたそれらのプログラムをコンピュータ装置に読み取らせ、実行させることによって、上述の第１、第２の制御方法を実施することができる。 The control device 10 described above may be substantially composed of a computer device (for example, a programmable logic controller (PLC) or the like). Therefore, it is desirable that the above-mentioned first and second control methods are configured as a program to be executed by a computer. It is also desirable that each of these programs be recorded on a computer-readable non-transitory recording medium. In that case, the above-mentioned first and second control methods can be implemented by having a computer device read and execute those programs recorded on the recording medium.

上の例では、マスタ装置１０１は１軸のベルトコンベアであり、また、スレーブ装置１０２は６軸多関節ロボットを含むものとした。しかしながら、これに限られるものではない。マスタ装置、スレーブ装置としては、例えばベルトコンベアのような１軸の装置、Ｘ−Ｙテーブルのような２軸の装置、４軸パラレルリンクロボットのような４軸の装置、５軸水平多関節ロボットのような５軸の装置、６軸多関節ロボットのような６軸の装置など、様々な軸数ｍ，ｎの装置が対象となり得る。 In the above example, the master device 101 is a single-axis belt conveyor, and the slave device 102 includes a 6-axis articulated robot. However, it is not limited to this. As the master device and slave device, for example, a 1-axis device such as a belt conveyor, a 2-axis device such as an XY table, a 4-axis device such as a 4-axis parallel link robot, and a 5-axis horizontal articulated robot. Various devices with a number of axes m and n can be targeted, such as a 5-axis device such as the above, and a 6-axis device such as a 6-axis articulated robot.

以上の実施形態は例示であり、この発明の範囲から離れることなく様々な変形が可能である。上述した複数の実施の形態は、それぞれ単独で成立し得るものであるが、実施の形態同士の組みあわせも可能である。また、異なる実施の形態の中の種々の特徴も、それぞれ単独で成立し得るものであるが、異なる実施の形態の中の特徴同士の組みあわせも可能である。 The above embodiment is an example, and various modifications can be made without departing from the scope of the present invention. Each of the plurality of embodiments described above can be established independently, but combinations of the embodiments are also possible. Further, although various features in different embodiments can be established independently, it is also possible to combine features in different embodiments.

１０ 制御装置
３０ 中央演算部
１００ 制御システム
１０１ マスタ装置
１０２ スレーブ装置 10 Control device 30 Central calculation unit 100 Control system 101 Master device 102 Slave device

Claims (10)

マスタ装置とスレーブ装置とを同期させて制御する制御装置であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出する第１カーブ算出部と、
上記第１カーブと上記スレーブ装置の同期速度とに基づいて、上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度の経時変化を表す合成カーブを算出する合成カーブ算出部と、
上記合成カーブを与える合成加速度が予め定められた負の閾値を下回っているか否かを判定する加速度判定部と、
上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように上記合成カーブを補正する合成カーブ補正部と、
上記第１カーブと上記補正された合成カーブとに基づいて、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する第２カーブ算出部と
を備えたことを特徴とする制御装置。 A control device that synchronizes and controls the master device and slave device.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
A first curve calculation unit that calculates a first curve that represents a change over time in the speed component of the detachment operation of the slave device,
A composite curve calculation unit that calculates a composite curve representing a change over time in the composite speed including the speed component of the detachment operation and the speed component of the synchronous end operation based on the first curve and the synchronous speed of the slave device. ,
An acceleration determination unit that determines whether or not the combined acceleration that gives the composite curve is below a predetermined negative threshold value, and
When the combined acceleration is below the negative threshold, the composite curve correction unit that corrects the composite curve so that the composite acceleration becomes the same as or exceeds the negative threshold.
Based on the first curve and the corrected composite curve, a second curve calculation unit for calculating a second curve representing a time-dependent change in the speed component of the synchronous end operation of the slave device is provided. Control device. 請求項１に記載の制御装置において、
上記合成カーブ算出部は、
上記第１カーブにおいて上記離脱動作の速度成分がゼロから極大値まで増加する加速期間については、上記合成速度は上記離脱動作の速度成分と上記同期速度がなす速度成分とを合成した値をとり、
上記第１カーブにおいて上記離脱動作の速度成分が上記極大値からゼロまで減少する減速期間については、この減速期間の始期に、上記合成速度が極大値から減少し始め、かつ、上記減速期間の終期に、上記合成速度がゼロとなる態様で、
上記合成カーブを算出する
ことを特徴とする制御装置。 In the control device according to claim 1,
The above composite curve calculation unit
For the acceleration period in which the velocity component of the detachment motion increases from zero to the maximum value in the first curve, the combined velocity is a value obtained by combining the velocity component of the detachment motion and the velocity component formed by the synchronous velocity.
Regarding the deceleration period in which the speed component of the withdrawal operation decreases from the maximum value to zero in the first curve, the combined speed starts to decrease from the maximum value at the beginning of the deceleration period, and the end of the deceleration period. In addition, in a mode in which the synthesis rate becomes zero,
A control device characterized by calculating the composite curve. 請求項２に記載の制御装置において、
上記合成カーブ補正部は、上記合成カーブを与える上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように、上記減速期間の終期に対して上記合成速度がゼロになる時刻を遅らせて、上記合成カーブを補正する
ことを特徴とする制御装置。 In the control device according to claim 2,
At the end of the deceleration period, the composite curve correction unit sets the composite acceleration so that when the composite acceleration giving the composite curve is below or above the negative threshold value, the composite acceleration becomes the same as or exceeds the negative threshold value. On the other hand, a control device characterized in that the composite curve is corrected by delaying the time when the composite speed becomes zero. マスタ装置とスレーブ装置とを同期させて制御する制御装置であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間を含んで、上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出する第１カーブ算出部と、
上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間を含んで、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する第２カーブ算出部と、
上記第１カーブにおける上記離脱動作の速度成分を与える加速度成分と、上記第２カーブにおける上記同期終了動作の速度成分を与える加速度成分とに基づいて、合成加速度を算出する合成加速度算出部と、
上記合成加速度が予め定められた負の閾値を下回っているか否かを判定する加速度判定部と、
上記合成加速度が上記負の閾値を下回っているとき、上記減速期間と上記同期終了期間とをそれぞれ延長して、上記合成加速度が上記負の閾値と同じになるか又は上回るように補正する合成加速度補正部と、
上記延長された減速期間、上記延長された同期終了期間に基づいて、それぞれ上記第１カーブ、上記第２カーブを更新するカーブ更新部と
を備えたことを特徴とする制御装置。 A control device that synchronizes and controls the master device and slave device.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
A first curve calculation unit that calculates a first curve representing a change over time in the speed component of the detachment operation of the slave device, including a deceleration period in which the speed component of the detachment operation decreases from the maximum value to zero.
A second curve for calculating a second curve representing a change over time in the speed component of the synchronization end operation of the slave device, including a synchronization end period in which the speed component of the synchronization end operation decreases from the synchronization speed of the slave device to zero. Calculation part and
A composite acceleration calculation unit that calculates a composite acceleration based on an acceleration component that gives a velocity component of the detachment motion in the first curve and an acceleration component that gives a velocity component of the synchronization end motion in the second curve.
An acceleration determination unit that determines whether or not the combined acceleration is below a predetermined negative threshold value,
When the combined acceleration is below the negative threshold, the deceleration period and the synchronization end period are extended, respectively, and the combined acceleration is corrected so that it becomes the same as or exceeds the negative threshold. Correction part and
A control device including a curve updating unit for updating the first curve and the second curve, respectively, based on the extended deceleration period and the extended synchronization end period, respectively. 請求項４に記載の制御装置において、
上記第２カーブ算出部は、上記減速期間に対して上記同期終了期間が一致する態様で、上記第２カーブを算出することを特徴とする制御装置。 In the control device according to claim 4,
The second curve calculation unit is a control device for calculating the second curve in a manner in which the synchronization end period coincides with the deceleration period. 請求項５に記載の制御装置において、
上記合成加速度補正部は、上記減速期間と上記同期終了期間とが一致する態様で、上記減速期間と上記同期終了期間の終期を延長する
ことを特徴とする制御装置。 In the control device according to claim 5.
The combined acceleration correction unit is a control device characterized in that the deceleration period and the end of the synchronization end period are extended in such a manner that the deceleration period and the synchronization end period coincide with each other. 請求項１から６までのいずれか一つに記載の制御装置において、
上記第１カーブにおける上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間の始期、終期で、それぞれ上記離脱動作の速度成分を与える加速度成分がゼロであり、
上記第２カーブにおける上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間の始期、終期で、それぞれ上記同期終了動作の速度成分を与える加速度成分がゼロである
ことを特徴とする制御装置。 In the control device according to any one of claims 1 to 6.
At the beginning and end of the deceleration period in which the velocity component of the detachment motion in the first curve decreases from the maximum value to zero, the acceleration component that gives the velocity component of the detachment motion is zero, respectively.
At the beginning and end of the synchronization end period in which the speed component of the synchronization end operation in the second curve decreases from the synchronization speed of the slave device to zero, the acceleration component that gives the speed component of the synchronization end operation is zero. A control device characterized by. マスタ装置とスレーブ装置とを同期させて制御する制御方法であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出し、
上記第１カーブと上記スレーブ装置の同期速度とに基づいて、上記離脱動作の速度成分と上記同期終了動作の速度成分とを含む合成速度の経時変化を表す合成カーブを算出し、
上記合成カーブを与える合成加速度が予め定められた負の閾値を下回っているか否かを判定し、
上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように上記合成カーブを補正し、
上記第１カーブと上記補正された合成カーブとに基づいて、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出する
ことを特徴とする制御方法。 It is a control method that controls the master device and the slave device in synchronization.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
The first curve representing the time-dependent change of the speed component of the detachment operation of the slave device was calculated.
Based on the first curve and the synchronization speed of the slave device, a synthesis curve representing a time-dependent change in the synthesis speed including the speed component of the detachment operation and the speed component of the synchronization end operation is calculated.
It is determined whether or not the composite acceleration that gives the composite curve is below a predetermined negative threshold value.
When the combined acceleration is below or above the negative threshold, the composite curve is corrected so that the combined acceleration is equal to or greater than the negative threshold.
A control method characterized in that a second curve representing a change with time of a speed component of a synchronous end operation of the slave device is calculated based on the first curve and the corrected composite curve. マスタ装置とスレーブ装置とを同期させて制御する制御方法であって、
上記マスタ装置から上記スレーブ装置を離脱させる離脱動作と、上記マスタ装置に対する上記スレーブ装置の同期を終了させる同期終了動作とを並行して実行する同期終了モードを有し、
上記離脱動作の速度成分が極大値からゼロまで小さくなる減速期間を含んで、上記スレーブ装置の離脱動作の速度成分の経時変化を表す第１カーブを算出し、
上記同期終了動作の速度成分が上記スレーブ装置の同期速度からゼロまで小さくなる同期終了期間を含んで、上記スレーブ装置の同期終了動作の速度成分の経時変化を表す第２カーブを算出し、
上記第１カーブにおける上記離脱動作の速度成分を与える加速度成分と、上記第２カーブにおける上記同期終了動作の速度成分を与える加速度成分とに基づいて、合成加速度を算出し、
上記合成加速度が予め定められた負の閾値を下回っているか否かを判定し、
上記合成加速度が上記負の閾値を下回っているとき、上記合成加速度が上記負の閾値と同じになるか又は上回るように、上記減速期間と上記同期終了期間とをそれぞれ延長し、
上記延長された減速期間、上記延長された同期終了期間に基づいて、それぞれ上記第１カーブ、上記第２カーブを更新する
ことを特徴とする制御方法。 It is a control method that controls the master device and the slave device in synchronization.
It has a synchronization end mode in which a detachment operation for detaching the slave device from the master device and a synchronization end operation for ending the synchronization of the slave device with the master device are executed in parallel.
A first curve representing the change over time of the speed component of the detachment operation of the slave device is calculated, including the deceleration period in which the speed component of the detachment operation decreases from the maximum value to zero.
A second curve representing the change over time of the speed component of the synchronization end operation of the slave device is calculated, including the synchronization end period in which the speed component of the synchronization end operation decreases from the synchronization speed of the slave device to zero.
The combined acceleration is calculated based on the acceleration component that gives the velocity component of the withdrawal motion in the first curve and the acceleration component that gives the velocity component of the synchronous end motion in the second curve.
It is determined whether or not the combined acceleration is below a predetermined negative threshold value.
When the combined acceleration is below or above the negative threshold, the deceleration period and the synchronization end period are extended so that the combined acceleration is equal to or exceeds the negative threshold.
A control method comprising updating the first curve and the second curve, respectively, based on the extended deceleration period and the extended synchronization end period, respectively. 請求項８または９に記載の制御方法をコンピュータに実行させるためのプログラム。 A program for causing a computer to execute the control method according to claim 8 or 9.
JP2019053319A 2019-03-20 2019-03-20 Controls, control methods, and programs Active JP6973436B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2019053319A JP6973436B2 (en) 2019-03-20 2019-03-20 Controls, control methods, and programs

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2019053319A JP6973436B2 (en) 2019-03-20 2019-03-20 Controls, control methods, and programs

Publications (2)

Publication Number Publication Date
JP2020151815A true JP2020151815A (en) 2020-09-24
JP6973436B2 JP6973436B2 (en) 2021-11-24

Family

ID=72557145

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019053319A Active JP6973436B2 (en) 2019-03-20 2019-03-20 Controls, control methods, and programs

Country Status (1)

Country Link
JP (1) JP6973436B2 (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002192487A (en) * 2000-12-25 2002-07-10 Seiko Epson Corp Control method of pick-and-place device and pick-and- place device applying this method
JP2009027816A (en) * 2007-07-18 2009-02-05 Toyota Motor Corp Traveling speed acquisition device
JP2009028851A (en) * 2007-07-27 2009-02-12 Nachi Fujikoshi Corp Robot control device

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002192487A (en) * 2000-12-25 2002-07-10 Seiko Epson Corp Control method of pick-and-place device and pick-and- place device applying this method
JP2009027816A (en) * 2007-07-18 2009-02-05 Toyota Motor Corp Traveling speed acquisition device
JP2009028851A (en) * 2007-07-27 2009-02-12 Nachi Fujikoshi Corp Robot control device

Also Published As

Publication number Publication date
JP6973436B2 (en) 2021-11-24

Similar Documents

Publication Publication Date Title
JP4467625B2 (en) Numerical control apparatus and numerical control method
US9678500B2 (en) Machining program creating device numerical control device, machining system, machining program creating method, numerical control method, and machining program
US20150174760A1 (en) Method for adjusting robot control parameters, robot system, and robot controller
US9764462B2 (en) Robot apparatus and robot controlling method
JP2880590B2 (en) Synchronous control method for industrial robot
JP5897648B2 (en) Numerical control device having function of switching gain of position control during synchronous control
JP6410826B2 (en) Numerical controller
JP5850996B1 (en) Numerical control device with corner path generation function considering acceleration / deceleration after interpolation
KR20130143006A (en) Electronic cam control device and electronic cam curve generating method
WO2013140679A1 (en) Track control device
US10710242B2 (en) Controller, control system, and control method
JP6954193B2 (en) Controls, control methods, and programs
JP5980977B1 (en) A numerical controller that re-establishes machine coordinate values using an absolute position detector
JP2002132349A (en) Acceleration and deceleration control method
JP6973436B2 (en) Controls, control methods, and programs
JP6954192B2 (en) Controls, control methods, and programs
CN103513656A (en) Setting of synchronized robot movements
KR100729403B1 (en) Programmable controller
US10915093B2 (en) Numerical controller
JP5573664B2 (en) Numerical control apparatus, movement control method, movement control program, and storage medium
WO2020079829A1 (en) Numerical control device and method for controlling additive manufacturing device
CN113858213B (en) Robot dynamic track planning method for target synchronization
JP2016034669A (en) Robot control device and robot control method
JP2018060242A (en) Numerical control unit
JP4262973B2 (en) Irradiation energy control method in laser processing

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20201214

TRDD Decision of grant or rejection written
A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20210928

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20211005

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20211018

R150 Certificate of patent or registration of utility model

Ref document number: 6973436

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150