JP2016068171A - Robot control device and control method - Google Patents

Robot control device and control method Download PDF

Info

Publication number
JP2016068171A
JP2016068171A JP2014198047A JP2014198047A JP2016068171A JP 2016068171 A JP2016068171 A JP 2016068171A JP 2014198047 A JP2014198047 A JP 2014198047A JP 2014198047 A JP2014198047 A JP 2014198047A JP 2016068171 A JP2016068171 A JP 2016068171A
Authority
JP
Japan
Prior art keywords
slave
master
speed
current command
command value
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
JP2014198047A
Other languages
Japanese (ja)
Other versions
JP6564175B2 (en
Inventor
英紀 田中
Hidenori Tanaka
英紀 田中
信恭 下村
Nobutaka Shimomura
信恭 下村
信高 坪井
Nobutaka Tsuboi
信高 坪井
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.)
Kawasaki Heavy Industries Ltd
Original Assignee
Kawasaki Heavy Industries Ltd
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 Kawasaki Heavy Industries Ltd filed Critical Kawasaki Heavy Industries Ltd
Priority to JP2014198047A priority Critical patent/JP6564175B2/en
Publication of JP2016068171A publication Critical patent/JP2016068171A/en
Application granted granted Critical
Publication of JP6564175B2 publication Critical patent/JP6564175B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)
  • Feedback Control In General (AREA)
  • Control Of Multiple Motors (AREA)

Abstract

PROBLEM TO BE SOLVED: To provide a robot control device and a robot control method which can reduce positional deviations between motors in a servo control structure configured to drive one driving shaft with at least two motors.SOLUTION: A robot control device, which comprises an original current command value generating portion that generates an original current command value on the basis of a position command value and a master position, and one or more compensation elements for compensating slave speed corresponding to respective deviations of the slave speed on the basis of one or more deviations of the slave speed relative to master speed, distributes the original current command value so as to generate the master current command value and one or more slave current command values, drives a master motor on the basis of the master current command value, and performs control so as to drive one or more slave motors on the basis of current command values to which are added respectively outputs of compensation elements corresponding to one or more slave current command values respectively corresponding to one or more slave motors respectively.SELECTED DRAWING: Figure 1

Description

本発明は、ロボットの制御装置および制御方法に関し、特に、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造を備えたロボットの制御装置および制御方法に関する。   The present invention relates to a robot control apparatus and control method, and more particularly to a robot control apparatus and control method having a servo control structure that drives one drive shaft with at least two motors.

工作機械等のロボットにおいて、ロボットの動作を規定する可動部分を駆動する駆動軸にかかる負荷が大きく１つのモータだけで駆動軸を駆動することができない場合に、２つのモータを互いに連結し、１つの駆動軸を駆動するように２つのモータを制御するデュアルサーボ制御構造が知られている。従来のデュアルサーボ制御構造においては、２つのモータのうちの何れか一方のモータへの位置指令値に基づいて当該一のモータの現在位置を２つのモータの制御系にフィードバックすることにより２つのモータに対してフィードバック制御が行われる。   In a robot such as a machine tool, when the load applied to the drive shaft that drives the movable part that defines the operation of the robot is large and the drive shaft cannot be driven by only one motor, the two motors are connected to each other. A dual servo control structure is known that controls two motors so as to drive one drive shaft. In the conventional dual servo control structure, the two motors are fed back by feeding the current position of the one motor to the control system of the two motors based on the position command value to one of the two motors. Is subjected to feedback control.

さらに、デュアルサーボ制御構造においてより安定的な制御を行うための構成として、例えば特許文献１のような構成が提案されている。特許文献１の構成においては、一方のモータ（マスタモータ）へのトルク指令と、他方のモータ（スレーブモータ）へのトルク指令との差に応じてスレーブモータへのトルク指令を補正している。   Furthermore, as a configuration for performing more stable control in the dual servo control structure, for example, a configuration as in Patent Document 1 has been proposed. In the configuration of Patent Document 1, the torque command to the slave motor is corrected according to the difference between the torque command to one motor (master motor) and the torque command to the other motor (slave motor).

特開２００３−７９１８０号公報JP 2003-79180 A

しかし、このような構成であっても、同じ性能のモータを同じように制御しても一方のモータに対して他方のモータの位置の偏差が生じる問題があった。このような問題は、３つ以上のモータを互いに連結し、１つの駆動軸を駆動するように３つ以上のモータを制御するサーバ制御構造でも同様に発生する。   However, even with such a configuration, there is a problem that even if a motor having the same performance is controlled in the same manner, a deviation in the position of the other motor occurs with respect to one motor. Such a problem also occurs in a server control structure in which three or more motors are connected to each other and three or more motors are controlled to drive one drive shaft.

本発明は、以上のような課題を解決すべくなされたものであり、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造においてモータ間の位置偏差を小さくすることができるロボットの制御装置およびロボットの制御方法を提供することを目的とする。   The present invention has been made to solve the above problems, and a robot capable of reducing a positional deviation between motors in a servo control structure in which one drive shaft is driven by at least two motors. An object of the present invention is to provide a control device and a robot control method.

本発明の一態様に係るロボットの制御装置は、ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御装置であって、前記マスタモータの回転位置であるマスタ位置を取得するマスタ位置取得部と、位置指令値と前記マスタ位置とに基づいて原電流指令値を生成する原電流指令値生成部と、前記マスタモータの速度であるマスタ速度を取得するマスタ速度取得部と、１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得するスレーブ速度取得部と、前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための１以上の補償要素と、を備え、前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償要素の出力をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するように制御するものである。   A robot control apparatus according to an aspect of the present invention includes a drive shaft that drives a movable part that defines the operation of the robot, a master motor that rotationally drives the drive shaft, and one or more slaves that rotationally drive the drive shaft. A master position acquisition unit that acquires a master position that is a rotational position of the master motor, and generates an original current command value based on the position command value and the master position. An original current command value generation unit that performs a master speed acquisition unit that acquires a master speed that is the speed of the master motor, and a slave speed acquisition unit that acquires one or more slave speeds that are the speed of the one or more slave motors. And one or more for compensating the slave speed corresponding to each deviation based on one or more deviations of the slave speed with respect to the master speed. A compensation element, distributing the original current command value to generate a master current command value and one or more slave current command values, driving the master motor based on the master current command value, Control is performed to drive one or more slave motors based on current command values obtained by adding outputs of the compensation elements corresponding to the one or more slave current command values respectively corresponding to the slave motors. Is.

一態様によれば、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造においてモータ間の位置偏差を小さくすることができるという効果を奏する。   According to one aspect, there is an effect that the positional deviation between the motors can be reduced in the servo control structure in which one drive shaft is driven by at least two motors.

図１は本発明の第１の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。FIG. 1 is a block diagram showing a schematic configuration of a robot control apparatus according to the first embodiment of the present invention. 図２は本発明の第１の実施の形態の変形例に係るロボットの制御装置の概略構成を示すブロック図である。FIG. 2 is a block diagram showing a schematic configuration of a robot control apparatus according to a modification of the first embodiment of the present invention. 図３は本発明の第２の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。FIG. 3 is a block diagram showing a schematic configuration of a robot control apparatus according to the second embodiment of the present invention. 図４は本実施例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。FIG. 4 is a graph showing temporal changes in torque and speed of the master motor and slave motor in this embodiment. 図５は図４に示す速度の時間的変化を示すグラフにおけるＶ部拡大図である。FIG. 5 is an enlarged view of a portion V in the graph showing the temporal change of the speed shown in FIG. 本実施例における補償後スレーブ電流指令値の時間的変化を比較例とともに示すグラフである。It is a graph which shows the time change of the slave current command value after compensation in a present Example with a comparative example. 図７は比較例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。FIG. 7 is a graph showing temporal changes in torque and speed of the master motor and slave motor in the comparative example. 図８は図７に示す速度の時間的変化を示すグラフにおけるＶＩＩＩ部拡大図である。FIG. 8 is an enlarged view of a part VIII in the graph showing the temporal change of the speed shown in FIG.

＜本発明の一態様を得るに至った経緯＞
上述したように、従来のデュアルサーボ制御構造においては、同じ性能のモータを同じように制御しても一方のモータに対して他方のモータの位置の偏差が生じる問題があった。 <Background of obtaining one embodiment of the present invention>
As described above, in the conventional dual servo control structure, there is a problem that even if a motor having the same performance is controlled in the same manner, a deviation of the position of the other motor occurs with respect to one motor.

そこで、本発明の発明者らは上記課題を解決するため、鋭意研究を行った。この結果、本発明の発明者らは、デュアルサーボ制御を行った２つのモータのそれぞれの速度を解析することによって、一方のモータ（例えばマスタモータ）の速度変化に対して、他方のモータ（例えばスレーブモータ）の速度変化が追従できず、振動が発生したりして両者のモータ間の速度差が広がるという知見を得た。そこで、本発明の発明者らは、２つのモータの変位差は、両者の速度差に比例すると仮定し、両者に共通の電流指令値を用いつつ、スレーブモータの電流指令値に両者の速度差に基づいた補償値を加えるという制御態様を想到するに至った。これにより、マスタモータとスレーブモータとの間の速度変化を抑え、モータ間の位置偏差を小さくすることができる。   Therefore, the inventors of the present invention have conducted intensive research to solve the above problems. As a result, the inventors of the present invention analyze the speed of each of the two motors that have performed dual servo control, so that the other motor (for example, the master motor) is changed with respect to the speed change of the other motor (for example, the master motor). It was found that the change in the speed of the slave motor could not follow, and the difference in speed between the two motors widened due to vibration. Therefore, the inventors of the present invention assume that the displacement difference between the two motors is proportional to the speed difference between the two motors, and use the current command value common to both, while the current command value of the slave motor uses the speed difference between the two motors. The control mode of adding a compensation value based on the above has been conceived. Thereby, the speed change between a master motor and a slave motor can be suppressed, and the position deviation between motors can be made small.

＜本発明の一態様の概要＞
本発明は、以上のような発明者の知見に基づいて想到されたものである。本発明の一態様に係るロボットの制御装置は、ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御装置であって、前記マスタモータの回転位置であるマスタ位置を取得するマスタ位置取得部と、位置指令値と前記マスタ位置とに基づいて原電流指令値を生成する原電流指令値生成部と、前記マスタモータの速度であるマスタ速度を取得するマスタ速度取得部と、１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得するスレーブ速度取得部と、前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための１以上の補償要素と、を備え、前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償要素の出力をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するように制御するものである。 <Outline of One Embodiment of the Present Invention>
The present invention has been conceived based on the inventors' knowledge as described above. A robot control apparatus according to an aspect of the present invention includes a drive shaft that drives a movable part that defines the operation of the robot, a master motor that rotationally drives the drive shaft, and one or more slaves that rotationally drive the drive shaft. A master position acquisition unit that acquires a master position that is a rotational position of the master motor, and generates an original current command value based on the position command value and the master position. An original current command value generation unit that performs a master speed acquisition unit that acquires a master speed that is the speed of the master motor, and a slave speed acquisition unit that acquires one or more slave speeds that are the speed of the one or more slave motors. And one or more for compensating the slave speed corresponding to each deviation based on one or more deviations of the slave speed with respect to the master speed. A compensation element, distributing the original current command value to generate a master current command value and one or more slave current command values, driving the master motor based on the master current command value, Control is performed to drive one or more slave motors based on current command values obtained by adding outputs of the compensation elements corresponding to the one or more slave current command values respectively corresponding to the slave motors. Is.

上記構成によれば、位置指令値からマスタモータおよびスレーブモータへのそれぞれに共通する原電流指令値が生成される。マスタモータへは、原電流指令値から分配されたマスタ電流指令値が入力され、スレーブモータへは、原電流指令値から分配されたスレーブ電流指令値に、マスタ速度とスレーブ速度との偏差に基づいたスレーブ速度の補償分が加えられた電流指令値が入力される。このようにして、両者に共通の電流指令値を用いつつ、スレーブモータの電流指令値に両者の速度差に基づいた補償値を加えている。これにより、マスタモータとスレーブモータとの間の速度変化を抑え、モータ間の位置偏差を小さくすることができる。   According to the above configuration, the common current command value common to the master motor and the slave motor is generated from the position command value. The master current command value distributed from the original current command value is input to the master motor, and the slave motor is supplied to the slave motor based on the deviation between the master speed and the slave speed. The current command value with the compensated slave speed added is input. In this way, while using a current command value common to both, a compensation value based on the speed difference between the two is added to the current command value of the slave motor. Thereby, the speed change between a master motor and a slave motor can be suppressed, and the position deviation between motors can be made small.

前記補償要素は、比例要素を含んでもよい。これにより、マスタモータとスレーブモータとの間の速度偏差に比例した補償が行われるため、マスタモータとスレーブモータとの間の速度変化を抑えることができる。さらに、前記比例要素のゲインは、前記マスタ速度と前記スレーブ速度との偏差が単調に０に近づくような値に設定されてもよい。このように、両者間の速度偏差が単調に０に近づくような値にゲインが設定されることにより、より効率的に両者間の速度変化を抑えることができる。   The compensation element may include a proportional element. As a result, compensation proportional to the speed deviation between the master motor and the slave motor is performed, so that a speed change between the master motor and the slave motor can be suppressed. Furthermore, the gain of the proportional element may be set to a value such that the deviation between the master speed and the slave speed monotonously approaches zero. In this way, by setting the gain to a value such that the speed deviation between both approaches monotonically, the speed change between the two can be suppressed more efficiently.

前記マスタ速度取得部は、前記マスタ位置取得部と、前記マスタ位置を微分して前記マスタ速度を出力する微分器と、を備え、前記スレーブ速度取得部は、前記スレーブモータの回転位置であるスレーブ位置を取得するスレーブ位置取得部と、前記スレーブ位置を微分して前記スレーブ速度を出力する微分器と、を備えてもよい。これにより、既存の構成で容易に本発明の構成を実現することができる。   The master speed acquisition unit includes the master position acquisition unit and a differentiator that differentiates the master position and outputs the master speed, and the slave speed acquisition unit is a slave that is a rotational position of the slave motor. You may provide the slave position acquisition part which acquires a position, and the differentiator which differentiates the said slave position and outputs the said slave speed. Thereby, the structure of this invention is easily realizable with the existing structure.

本発明の他の態様に係るロボットの制御方法は、ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御方法であって、前記マスタモータの回転位置であるマスタ位置を取得し、位置指令値と前記マスタ位置とに基づいて生成された原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタモータの速度であるマスタ速度を取得し、１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得し、前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための補償値を算出し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償値をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するものである。   A robot control method according to another aspect of the present invention includes a drive shaft that drives a movable part that defines the operation of the robot, a master motor that rotationally drives the drive shaft, and one or more that rotationally drives the drive shaft. A master motor that is a rotational position of the master motor, and distributes an original current command value generated based on the position command value and the master position. Generating a master current command value and one or more slave current command values, obtaining a master speed that is the speed of the master motor, and obtaining one or more slave speeds that are the speed of the one or more slave motors, respectively. Calculating a compensation value for compensating the slave speed corresponding to each deviation based on one or more deviations of the slave speed with respect to the master speed; The master motor is driven based on a current command value, and the compensation value corresponding to each of the one or more slave current command values corresponding to the one or more slave motors is added. One or more slave motors are driven.

上記方法によれば、位置指令値からマスタモータおよびスレーブモータへのそれぞれに共通する原電流指令値が生成される。マスタモータへは、現電流指令値から分配されたマスタ電流指令値が入力され、スレーブモータへは、原電流指令値から分配されたスレーブ電流指令値に、マスタ速度とスレーブ速度との偏差に基づいてスレーブ速度を補償した電流指令値が入力される。このようにして、両者に共通の電流指令値を用いつつ、スレーブモータの電流指令値に両者の速度差に基づいた補償値を加えている。これにより、マスタモータとスレーブモータとの間の速度変化を抑え、モータ間の位置偏差を小さくすることができる。   According to the above method, the common current command value common to the master motor and the slave motor is generated from the position command value. A master current command value distributed from the current current command value is input to the master motor, and a slave current command value distributed from the original current command value is input to the slave motor based on a deviation between the master speed and the slave speed. The current command value that compensates for the slave speed is input. In this way, while using a current command value common to both, a compensation value based on the speed difference between the two is added to the current command value of the slave motor. Thereby, the speed change between a master motor and a slave motor can be suppressed, and the position deviation between motors can be made small.

＜実施の形態＞
以下、本発明の実施の形態を、図面を参照しながら説明する。なお、以下では全ての図を通じて同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。 <Embodiment>
Hereinafter, embodiments of the present invention will be described with reference to the drawings. In the following description, the same or corresponding elements are denoted by the same reference symbols throughout the drawings, and redundant description thereof is omitted.

＜第１の実施の形態＞
まず、本発明の第１の実施の形態について説明する。図１は本発明の第１の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。図１に示すように、本実施の形態におけるロボットの制御装置１は、ロボットの動作を規定する可動部分を駆動する駆動軸２と、マスタ動力伝達機構３を介して駆動軸２を回転駆動するマスタモータ４と、スレーブ動力伝達機構５を介して駆動軸２を回転駆動するスレーブモータ６と、を備えている。さらに、制御装置１は、マスタ電流指令値ＩＭ（後述）に基づいてマスタモータ４を駆動するマスタモータ駆動部１４と、補償後スレーブ電流指令値ＩＣＳ（後述）に基づいてスレーブモータ６を駆動するスレーブモータ駆動部１５と、を備えている。 <First Embodiment>
First, a first embodiment of the present invention will be described. FIG. 1 is a block diagram showing a schematic configuration of a robot control apparatus according to the first embodiment of the present invention. As shown in FIG. 1, the robot control apparatus 1 in the present embodiment rotationally drives the drive shaft 2 via a drive shaft 2 that drives a movable part that defines the operation of the robot and a master power transmission mechanism 3. A master motor 4 and a slave motor 6 that rotationally drives the drive shaft 2 via a slave power transmission mechanism 5 are provided. Furthermore, the control device 1 controls the master motor drive unit 14 that drives the master motor 4 based on the master current command value I M (described later) and the slave motor 6 based on the compensated slave current command value I CS (described later). And a slave motor drive unit 15 for driving.

本実施の形態が適用されるロボットは、例えば複数のアーム部材と、これらを接続する複数の関節（回転軸）とを備えた多関節ロボットとして構成される。すなわち、多関節ロボットにおける特定のアーム部材を可動部分として回動させる回転軸が本実施の形態における駆動軸２となる。マスタ動力伝達機構３は、マスタモータ４から出力される回転動力を駆動軸２に伝達する機構であれば特に限定されない。例えばマスタ動力伝達機構３は、ギヤ機構、アーム機構またはこれらを組み合わせた構成等、種々の構成を採用可能である。また、スレーブ動力伝達機構５もスレーブモータ６から出力される回転動力を駆動軸２に伝達する機構としてマスタ動力伝達機構３と同様に構成される。   The robot to which this embodiment is applied is configured as a multi-joint robot including, for example, a plurality of arm members and a plurality of joints (rotating shafts) that connect these members. That is, the rotation shaft that rotates a specific arm member in the multi-joint robot as a movable portion is the drive shaft 2 in the present embodiment. The master power transmission mechanism 3 is not particularly limited as long as it is a mechanism that transmits the rotational power output from the master motor 4 to the drive shaft 2. For example, the master power transmission mechanism 3 can employ various configurations such as a gear mechanism, an arm mechanism, or a combination thereof. The slave power transmission mechanism 5 is also configured similarly to the master power transmission mechanism 3 as a mechanism for transmitting the rotational power output from the slave motor 6 to the drive shaft 2.

マスタ動力伝達機構３およびスレーブ動力伝達機構５は、それぞれ駆動軸２に対する所定の減速比を有している。本実施の形態においては、マスタ動力伝達機構３の減速比とスレーブ動力伝達機構５の減速比とは等しい値に設定されている。   Each of the master power transmission mechanism 3 and the slave power transmission mechanism 5 has a predetermined reduction ratio with respect to the drive shaft 2. In the present embodiment, the reduction ratio of the master power transmission mechanism 3 and the reduction ratio of the slave power transmission mechanism 5 are set to the same value.

さらに、制御装置１は、マスタモータ４の速度（現在速度）を示す値（マスタ速度ωＭ）を取得するマスタ速度取得部７と、スレーブモータ６の速度（現在速度）を示す値（スレーブ速度ωＳ）を取得するスレーブ速度取得部８と、を備えている。本実施の形態において、マスタ速度取得部７は、マスタモータ４の回転位置（現在位置）を示す値（マスタ位置ＰＭ）を取得するマスタ位置取得部９と、マスタ位置ＰＭを微分してマスタ速度ωＭを出力する微分器１０と、を備えている。同様に、スレーブ速度取得部８は、スレーブモータ６の回転位置を示す値（スレーブ位置ＰＳ）を取得するスレーブ位置取得部１１と、スレーブ位置ＰＳを微分してスレーブ速度ωＳを出力する微分器１２と、を備えている。位置取得部９，１１が取得する回転位置は、各モータ４，６の出力軸（モータ軸）の回転角度位置（基準位置からの回転数および回転角度）を意味する。例えば、位置取得部９，１１は、エンコーダまたはレゾルバ等により構成される。 Furthermore, the control device 1 includes a master speed acquisition unit 7 that acquires a value (master speed ω M ) indicating the speed (current speed) of the master motor 4 and a value (slave speed) indicating the speed (current speed) of the slave motor 6. and a slave speed acquisition unit 8 that acquires ω S ). In the present embodiment, the master speed acquisition unit 7 differentiates the master position P M with a master position acquisition unit 9 that acquires a value (master position P M ) indicating the rotational position (current position) of the master motor 4. a differentiator 10 for outputting a master speed omega M, and a. Similarly, the slave speed acquiring unit 8, the slave position acquiring unit 11 for acquiring the value (slave position P S) indicating the rotational position of the slave motor 6, and outputs the slave speed omega S by differentiating the slave position P S A differentiator 12. The rotation position acquired by the position acquisition units 9 and 11 means the rotation angle position (the number of rotations and the rotation angle from the reference position) of the output shafts (motor shafts) of the motors 4 and 6. For example, the position acquisition units 9 and 11 are configured by an encoder, a resolver, or the like.

さらに、制御装置１は、位置指令値Ｐｏとマスタ位置ＰＭとに基づいて原電流指令値Ｉｏを生成する原電流指令値生成部１３を備えている。原電流指令値生成部１３は、ＣＰＵ、メモリおよび各種信号の入出力装置等を備えたコンピュータの機能ブロックとして構成される。位置指令値Ｐｏは、マスタモータ４のモータ軸の回転位置（基準位置からの回転数および回転角度）を示す値として外部から入力される。 Furthermore, the control device 1 includes an original current command value generation unit 13 that generates an original current command value Io based on the position command value P o and the master position P M. The original current command value generation unit 13 is configured as a functional block of a computer including a CPU, a memory, an input / output device for various signals, and the like. The position command value Po is input from the outside as a value indicating the rotational position (the rotational speed and rotational angle from the reference position) of the motor shaft of the master motor 4.

原電流指令値生成部１３で生成された原電流指令値Ｉｏは、マスタモータ４への経路およびスレーブモータ６への経路に分岐することにより均等に分配されてマスタ電流指令値ＩＭおよびスレーブ電流指令値ＩＳとなる。マスタ電流指令値ＩＭは、マスタモータ駆動部１４に入力される。マスタモータ駆動部１４は、入力されたマスタ電流指令値ＩＭに基づいた出力トルクＴＭを発生させるようにマスタモータ４を駆動し、マスタモータ４は、回転動力を出力する。 The original current command value I o generated by the original current command value generation unit 13 is evenly distributed by branching to the route to the master motor 4 and the route to the slave motor 6, so that the master current command value I M and the slave the current command value I S. The master current command value IM is input to the master motor drive unit 14. Master motor driver 14 drives the master motor 4 so as to generate an output torque T M based on a master current command value I M that is input, the master motor 4 outputs the rotational power.

さらに、制御装置１は、マスタ速度ωＭに対するスレーブ速度ωＳの偏差Δωに基づいてスレーブ速度ωＳを補償する補償要素１６を備えている。補償要素１６も、原電流指令値生成部１３と同様にコンピュータの機能ブロックとして構成される。本実施の形態において、補償要素１６は、比例要素（ゲインＧ）を含むように構成される。さらに、補償要素１６は、速度偏差Δωを電流指令値に加えるための換算値を含むように構成される。 Further, the control unit 1 is provided with a compensating element 16 for compensating the slave speed omega S based on the deviation Δω slave velocity omega S to the master speed omega M. The compensation element 16 is also configured as a functional block of the computer, like the original current command value generation unit 13. In the present embodiment, the compensation element 16 is configured to include a proportional element (gain G). Furthermore, the compensation element 16 is configured to include a converted value for adding the speed deviation Δω to the current command value.

なお、補償要素１６は、ゲインＧとして比例要素、積分要素、および微分要素の少なくとも何れか１つを有する。これらの要素を複数組み合わせることとしてもよい。すなわち、比例補償だけでなく、ＰＩ補償やＰＩＤ補償を行うこととしてもよい。   The compensation element 16 has at least one of a proportional element, an integral element, and a derivative element as the gain G. A plurality of these elements may be combined. That is, not only proportional compensation but also PI compensation or PID compensation may be performed.

補償要素１６から出力された補償値ＧΔωは、スレーブ電流指令値ＩＳに加えられ、補償後スレーブ電流指令値ＩＣＳとしてスレーブモータ駆動部１５に入力される。スレーブモータ駆動部１５は、入力された補償後スレーブ電流指令値ＩＣＳに基づいた出力トルクＴＳを発生させるようにスレーブモータ６を駆動し、スレーブモータ６は、回転動力を出力する。 The compensation value GΔω output from the compensation element 16 is added to the slave current command value I S and is input to the slave motor drive unit 15 as the compensated slave current command value I CS . The slave motor drive unit 15 drives the slave motor 6 to generate an output torque T S based on the input compensated slave current command value I CS, the slave motor 6 outputs rotational power.

マスタモータ４は、発生した出力トルクＴＭに応じた電流値をフィードバックし、当該フィードバック電流値とマスタ電流指令値ＩＭとの偏差に対してＰＩ補償を行うように構成されている。同様に、スレーブモータ６は、発生した出力トルクＴＳに応じた電流値をフィードバックし、当該フィードバック電流値と補償後スレーブ電流指令値ＩＣＳとの偏差に対してＰＩ補償を行うように構成されている。 The master motor 4 and feeds back the current value corresponding to the output torque T M generated, and is configured to perform PI compensation for the deviation between the feedback current value and the master current command value I M. Similarly, the slave motor 6 feeds back a current value corresponding to the output torque T S generated, is configured to perform a PI compensation for the deviation of the feedback current value and the compensated slave current command value I CS ing.

本実施の形態におけるロボットの制御装置１によれば、位置指令値Ｐｏからマスタモータ４およびスレーブモータ６へのそれぞれに共通する原電流指令値Ｉｏが生成される。マスタモータ４へは、原電流指令値Ｉｏから分配されたマスタ電流指令値ＩＭが入力され、スレーブモータ６へは、マスタ速度ωＭとスレーブ速度ωＳとの偏差Δωに基づいたスレーブ速度ωＳの補償分が加えられた電流指令値（補償後スレーブ電流指令値）ＩＣＳが入力される。このようにして、両者に共通の原電流指令値Ｉｏを用いつつ、スレーブモータ６への電流指令値ＩＣＳとして両者の速度差Δωに基づいた補償値ＧΔωを加えている。これにより、マスタモータ４とスレーブモータ６との間の速度変化を抑え、モータ４，６間の位置偏差を小さくすることができる。 According to the control apparatus 1 of the robot in this embodiment, the original current command value I o which is common to each of the position command value P o to the master motor 4 and slave motor 6 is generated. To the master motor 4 is input original current command value I o master current distributed from the command value I M is the the slave motor 6, the slave speed based on the deviation Δω between the master velocity omega M and a slave speed omega S ω current command value compensation amount is added to S (compensated slave current command value) I CS is input. In this way, while using a common source current command value I o to both, they are added to the compensation value GΔω based on both the speed difference Δω as a current command value I CS to the slave motor 6. Thereby, the speed change between the master motor 4 and the slave motor 6 can be suppressed, and the positional deviation between the motors 4 and 6 can be reduced.

また、補償要素１６が、比例要素（ゲインＧ）を含むことにより、マスタモータ４とスレーブモータ６との間の速度偏差Δωに比例した補償が行われるため、２つのモータに共通の原電流指令値Ｉｏを用いつつ、スレーブモータ６の電流指令値ＩＣＳに両者の速度差Δωに基づいた補償値を加えるという制御態様を容易かつシンプルに実現することができる。さらに、ゲインＧは、速度偏差Δωが単調に０に近づくような値に設定されることが好ましい。このように、両者間の速度偏差Δωが単調に０に近づくような値にゲインＧが設定されることにより、より効率的に両者間の速度変化を抑えることができる。 In addition, since the compensation element 16 includes a proportional element (gain G), compensation proportional to the speed deviation Δω between the master motor 4 and the slave motor 6 is performed, so that an original current command common to the two motors is obtained. while using the value I o, the control mode of adding the compensation value based on both the speed difference Δω can be achieved easily and simply to a current command value I CS slave motor 6. Furthermore, the gain G is preferably set to a value such that the speed deviation Δω approaches zero monotonously. Thus, by setting the gain G to such a value that the speed deviation Δω between the two approaches monotonically, the speed change between the two can be suppressed more efficiently.

本実施の形態において、第１電流指令値生成部１３は、位置指令値Ｐｏとマスタ位置ＰＭとの位置偏差（時間的偏差）ΔＰに基づいて速度指令値ωＯを算出する速度指令値算出部１７と、速度指令値ωｏとマスタ速度ωＭとの速度偏差（時間的偏差）Δωから第１電流指令値Ｉ１を算出する電流指令値算出部１８と、を備えている。より具体的には、速度指令値算出部１７は、位置ループゲインＫＰＭを用いて位置偏差ΔＰに対してＰＩ補償を行うことにより速度指令値ωｏを生成するよう構成される。また、電流指令値算出部１８は、速度ループゲインＫωＭを用いて速度偏差Δωに対してＰＩ補償を行い、第１電流指令値Ｉ１を出力する。なお、位置ループゲインＫＰＭおよび速度ループゲインＫωＭは、それぞれ比例ゲイン（ＫＰＰ，ＫωＰ）および積分ゲイン（ＫＰＩ，ＫωＩ）を含み、微分ゲインと積分ゲインとの値は個別に設定される。 In this embodiment, the first current command value generating unit 13, a position command value P o and the speed command value to calculate the speed command value omega O on the basis of the position deviation (time difference) [Delta] P between the master position P M A calculation unit 17 and a current command value calculation unit 18 that calculates a first current command value I 1 from a speed deviation (temporal deviation) Δω between the speed command value ω o and the master speed ω M are provided. More specifically, the speed command value calculation unit 17 is configured to generate a speed command value ω o by performing PI compensation on the position deviation ΔP using the position loop gain K PM . In addition, the current command value calculation unit 18 performs PI compensation on the speed deviation Δω using the speed loop gain K ωM and outputs a first current command value I 1 . The position loop gain K PM and the velocity loop gain K ωM include proportional gains (K PP , K ωP ) and integral gains (K PI , K ωI ), respectively, and the values of the differential gain and the integral gain are set individually. Is done.

このように、原電流指令値Ｉｏに対してマスタモータ４の位置ＰＭおよび速度ωＭに基づいてフィードバック制御を行うことにより、マスタモータ４およびスレーブモータ６への共通の原電流指令値Ｉｏに対して有効なフィードバック制御を行うことができる。 In this way, by performing feedback control on the original current command value Io based on the position P M and the speed ω M of the master motor 4, a common original current command value I to the master motor 4 and the slave motor 6 is obtained. Effective feedback control can be performed on o .

また、マスタ位置取得部９で検出されたマスタ位置ＰＭとこれを微分器１０で微分して得られたマスタ速度ωＭとを用いて位置フィードバック制御および速度フィードバック制御を行うため、速度検出器を別途設けることなく２つのフィードバック制御を容易に実現することができる。さらに、このような位置フィードバック制御および速度フィードバック制御を行う構成に予め設けられたマスタ速度取得部７を用いてスレーブモータ６への電流指令値ＩＣＳが生成されるため、既存の構成で容易に本発明の構成（すなわち、２つのモータに共通の原電流指令値Ｉｏを用いつつ、スレーブモータ６の電流指令値ＩＣＳに両者の速度差Δωに基づいた補償値を加えるという制御態様）を実現することができる。 Also, in order to perform position feedback control and velocity feedback control using a master speed omega M obtained this with the master position P M detected by the master position acquisition unit 9 is differentiated by a differentiator 10, a speed detector The two feedback controls can be easily realized without separately providing. Furthermore, since the current command value I CS to the slave motor 6 is generated using the master speed acquisition unit 7 provided in advance in the configuration for performing such position feedback control and speed feedback control, the existing configuration can be easily used. The configuration of the present invention (that is, a control mode in which a compensation value based on the speed difference Δω is added to the current command value I CS of the slave motor 6 while using the common current command value I o common to the two motors). Can be realized.

なお、本実施の形態においては、マスタ動力伝達機構３の減速比とスレーブ動力伝達機構５の減速比とが同じ値であるため、電流指令値算出部１８は、速度ループゲインＫωＭを用いて速度偏差Δωに対してＰＩ補償を行った後の値に各モータへの分配のための０．５のゲインを掛けた値を原電流指令値Ｉｏとして出力する。したがって、本実施の形態においては、実質的に原電流指令値Ｉｏとマスタ電流指令値ＩＭおよびスレーブ電流指令値ＩＳとは同じ値となっている。 In the present embodiment, since the reduction ratio of master power transmission mechanism 3 and the reduction ratio of slave power transmission mechanism 5 are the same value, current command value calculation unit 18 uses speed loop gain KωM. A value obtained by multiplying the value after the PI compensation for the speed deviation Δω by a gain of 0.5 for distribution to each motor is output as the original current command value Io . Therefore, in the present embodiment, the original current command value Io, the master current command value IM, and the slave current command value IS are substantially the same value.

また、マスタモータ駆動部１４に入力される電流指令値として、マスタ電流指令値ＩＭにロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づく電流補正値を加えてもよい。同様に、スレーブモータ駆動部１５に入力される電流指令値として、補償後スレーブ電流指令値ＩＣＳにロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づく電流補正値を加えてもよい。 Further, as a current command value that is input to the master motor drive unit 14, gravity may be added to the current correction value based on the consideration to dynamic torque applied to the master current command value I M to the drive shaft 2 of the robot. Similarly, as a current command value input to the slave motor drive unit 15, a current correction value based on a dynamic torque considering gravity acting on the drive shaft 2 of the robot may be added to the compensated slave current command value ICS. Good.

これらの電流補正値の基となる重力を考慮した動力学トルクは、例えば重力トルク、加減速トルク、摩擦トルク、他の軸の動きを支えるトルク（干渉トルク）等を含む。重力を考慮した動力学トルクは、ロボット動作における理論値に基づく値として規定され、予め設定される。このような動力学トルクに基づく電流補正値を加えるフィードフォワード制御を行うことにより、マスタモータ４とスレーブモータ６との位置偏差および速度偏差を予め小さくすることができ、より迅速な制御を行うことができる。   The dynamic torque in consideration of gravity as a basis of these current correction values includes, for example, gravity torque, acceleration / deceleration torque, friction torque, torque (interference torque) that supports the movement of other shafts, and the like. The dynamic torque in consideration of gravity is defined as a value based on a theoretical value in robot operation and is set in advance. By performing feedforward control that adds a current correction value based on such dynamic torque, the positional deviation and speed deviation between the master motor 4 and the slave motor 6 can be reduced in advance, and more rapid control is performed. Can do.

＜変形例＞
図２は本発明の第１の実施の形態の変形例に係るロボットの制御装置の概略構成を示すブロック図である。本変形例において第１の実施の形態と同様の構成については同じ符号を付し、説明を省略する。 <Modification>
FIG. 2 is a block diagram showing a schematic configuration of a robot control apparatus according to a modification of the first embodiment of the present invention. In the present modification, the same components as those in the first embodiment are denoted by the same reference numerals, and description thereof is omitted.

本変形例のロボットの制御装置１Ｂが第１の実施の形態におけるロボットの制御装置１と異なる点は、マスタ動力伝達機構３Ｂの減速比αとスレーブ動力伝達機構５Ｂの減速比βとが異なることである。これに応じて、制御装置１Ｂは、減速比の割合に応じたゲインを各モータ４，６を駆動するための第１電流指令値Ｉ１にそれぞれ掛けるための分配要素１９，２０を備えている。さらに、制御装置１Ｂは、マスタ速度ωＭとスレーブ速度ωＳとの速度偏差Δωを算出するために、減速比の影響をなくすゲインを掛けるための速度調整要素２１を備えている。 The robot control device 1B of the present modification is different from the robot control device 1 in the first embodiment in that the reduction ratio α of the master power transmission mechanism 3B and the reduction ratio β of the slave power transmission mechanism 5B are different. It is. In response thereto, the control apparatus 1B is provided with a distribution element 19, 20 for applying each gain corresponding to the ratio of the reduction ratio in the first current command value I 1 for driving the motors 4 and 6 . Furthermore, the control apparatus 1B, in order to calculate the velocity deviation Δω between the master velocity omega M and a slave speed omega S, and a speed adjustment element 21 for applying a gain to eliminate the effect of the reduction ratio.

分配要素１９は、原電流指令値生成部１３（電流指令値算出部１８）とマスタモータ駆動部１４との間に設けられる。分配要素２０は、原電流指令値生成部１３（電流指令値算出部１８）とスレーブモータ駆動部１５との間であって、補償要素１６から出力される補償値ＧΔωが加えられる加算器より前段に設けられる。   The distribution element 19 is provided between the original current command value generation unit 13 (current command value calculation unit 18) and the master motor drive unit 14. The distribution element 20 is between the original current command value generation unit 13 (current command value calculation unit 18) and the slave motor drive unit 15, and is upstream of the adder to which the compensation value GΔω output from the compensation element 16 is added. Is provided.

マスタ動力伝達機構３Ｂとスレーブ動力伝達機構５Ｂとの減速比の割合がα：βであるため、マスタモータ４へ原電流指令値Ｉｏを分配する分配要素１９の比例ゲインは、α／（α＋β）であり、スレーブモータ６へ原電流指令値Ｉｏを分配する分配要素２０の比例ゲインは、β／（α＋β）である。したがって、本変形例においては、マスタ電流指令値ＩＭは、原電流指令値Ｉｏに分配要素１９の比例ゲインα／（α＋β）を掛けたものであり、スレーブ電流指令値ＩＳは、原電流指令値Ｉｏに分配要素２０の比例ゲインβ／（α＋β）を掛けたものである。 Ratio of the reduction ratio of the master power transmission mechanism 3B and slave power transmission mechanism 5B is alpha: for a beta, proportional gain distribution element 19 for distributing the original current command value I o to the master motor 4, α / (α + β ), and the proportional gain of the distributing element 20 for distributing the original current command value I o to the slave motor 6 is β / (α + β). Therefore, in this modification, the master current command value I M is obtained by multiplying the original current command value I o by the proportional gain α / (α + β) of the distribution element 19, and the slave current command value IS is the original current command value I S. The current command value Io is multiplied by the proportional gain β / (α + β) of the distribution element 20.

また、マスタ動力伝達機構３Ｂの減速比αとスレーブ動力伝達機構５Ｂの減速比βとが異なるため、マスタ速度ωＭとスレーブ速度ωＳとは、目標とする速度がそもそも異なる。そこで、両者の速度ωＭ，ωＳを同じ目標速度に換算して比較するために速度取得部７，８の出力の少なくとも何れか一方に速度を調整するためのゲインを掛ける。本実施の形態において、制御装置１Ｂは、マスタ速度ωＭをスレーブ速度ωＳの速度に換算するために、マスタ速度ωＭを速度調整要素２１に入力し、速度調整要素２１の出力とスレーブ速度ωＳとの差を速度偏差Δωとして算出するように構成される。すなわち、微分器１０の出力端と速度偏差Δωを算出する減算器の入力端との間に速度調整要素２１が設けられる。この場合、速度調整要素の比例ゲインは、β／αである。 Further, since the reduction ratio of the master power transmission mechanism 3B alpha and the reduction ratio β of the slave power transmission mechanism 5B is different, the master speed omega M and a slave speed omega S, the speed of the target is originally different. Therefore, in order to convert both speeds ω M and ω S into the same target speed and compare them, at least one of the outputs of the speed acquisition units 7 and 8 is multiplied by a gain for adjusting the speed. In this embodiment, the control apparatus 1B, in order to convert the master speed omega M to the speed of the slave velocity omega S, enter the master velocity omega M to the speed adjustment element 21, the output and slave speed of the speed adjusting element 21 A difference from ω S is calculated as a speed deviation Δω. That is, the speed adjustment element 21 is provided between the output terminal of the differentiator 10 and the input terminal of the subtractor that calculates the speed deviation Δω. In this case, the proportional gain of the speed adjustment element is β / α.

なお、速度偏差Δωを算出するための速度調整要素２１は、スレーブ速度ωＳをマスタ速度ωＭの目標速度に対応する速度に換算するようにしてもよいし、マスタ速度ωＭおよびスレーブ速度ωＳをその他の所定の目標速度に対応する速度（例えば駆動軸２の目標速度に対応する速度）に換算するようにしてもよい。これらの場合には、補償要素１６において、再度スレーブモータ６の本来の目標速度に対応する速度に換算するためのゲインを掛けることが好ましい。 Incidentally, the speed adjustment element 21 for calculating the speed deviation Δω may be so as to convert the speed corresponding slave velocity omega S to the target speed of the master speed omega M, the master speed omega M and a slave speed omega S may be converted into a speed corresponding to another predetermined target speed (for example, a speed corresponding to the target speed of the drive shaft 2). In these cases, it is preferable to multiply the compensation element 16 by a gain for conversion into a speed corresponding to the original target speed of the slave motor 6 again.

また、本変形例は、マスタ動力伝達機構３Ｂの減速比αとスレーブ動力伝達機構５Ｂの減速比βとが同じ場合（α＝β）でも適用され得る。この場合、分配要素１９，２０の比例ゲインは、何れも０．５となり、速度調整要素の比例ゲインは１となる。   The present modification can also be applied when the reduction ratio α of the master power transmission mechanism 3B and the reduction ratio β of the slave power transmission mechanism 5B are the same (α = β). In this case, the proportional gains of the distribution elements 19 and 20 are both 0.5, and the proportional gain of the speed adjustment element is 1.

＜第２の実施の形態＞
次に、本発明の第２の実施の形態について説明する。図３は本発明の第２の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。第２の実施の形態において第１の実施の形態と同様の構成については同じ符号を付し、説明を省略する。 <Second Embodiment>
Next, a second embodiment of the present invention will be described. FIG. 3 is a block diagram showing a schematic configuration of a robot control apparatus according to the second embodiment of the present invention. In the second embodiment, the same components as those in the first embodiment are denoted by the same reference numerals, and description thereof is omitted.

図３に示すように、本実施の形態におけるロボットの制御装置１Ｃが第１の実施の形態におけるロボットの制御装置１と異なる点は、１つのマスタモータ４と複数（図３の例では２つ）のスレーブモータ６ａ，６ｂとを備えることである。スレーブモータ６ａ，６ｂが複数となることにより、スレーブモータ６ａ，６ｂに関連する構成５，８，１１，１２，１５，１６についても複数（２つずつ）設けられている。図３ではこれらの各構成の相違を各符号にａまたはｂを付すことにより区別して示している。以下では場合によって２つのスレーブモータ６ａ，６ｂを第１スレーブモータ６ａおよび第２スレーブモータ６ｂと呼称する。   As shown in FIG. 3, the robot control device 1C in the present embodiment is different from the robot control device 1 in the first embodiment in that there is one master motor 4 and a plurality of (two in the example of FIG. 3). ) Slave motors 6a and 6b. By providing a plurality of slave motors 6a and 6b, a plurality (two each) of configurations 5, 8, 11, 12, 15, and 16 related to the slave motors 6a and 6b are also provided. In FIG. 3, the difference between these components is distinguished by adding a or b to each symbol. Hereinafter, in some cases, the two slave motors 6a and 6b are referred to as a first slave motor 6a and a second slave motor 6b.

本実施の形態においても、第１の実施の形態と同様に、マスタ速度ωＭと第１スレーブモータ６ａの速度ωＳａとの速度偏差Δωａに補償要素１６ａの比例ゲインＧａを掛けた補償値ＧａΔωａをスレーブ電流指令値ＩＳａに加えた補償後スレーブ電流指令値ＩＣＳａが第１スレーブモータ６ａを駆動するためのスレーブモータ駆動部１５ａに入力される。また、マスタ速度ωＭと第２スレーブモータ６ｂの速度ωＳｂとの速度偏差Δωｂに補償要素１６ｂの比例ゲインＧｂを掛けた補償値ＧｂΔωｂをスレーブ電流指令値ＩＳｂに加えた補償後スレーブ電流指令値ＩＣＳｂが第２スレーブモータ６ｂを駆動するためのスレーブモータ駆動部１５ｂに入力される。スレーブモータ６の数が３つ以上の場合でも同様に、マスタモータ４のマスタ速度ωＭと各スレーブモータ６のスレーブ速度ωＳとの速度偏差Δωに基づいて各スレーブモータ６への電流指令値を補償することができる。 Also in this embodiment, like the first embodiment, the compensation obtained by multiplying a proportional gain G a master speed omega M and compensating element 16a to the speed deviation [Delta] [omega a of the speed omega Sa of the first slave motor 6a The compensated slave current command value I CSa obtained by adding the value G a Δω a to the slave current command value I Sa is input to the slave motor drive unit 15a for driving the first slave motor 6a. Further, a compensation value G b Δω b obtained by multiplying the speed deviation Δω b between the master speed ω M and the speed ω Sb of the second slave motor 6 b by the proportional gain G b of the compensation element 16 b is added to the slave current command value I Sb . The compensated slave current command value I CSb is input to the slave motor drive unit 15b for driving the second slave motor 6b. Similarly, in case the number of the slave motor 6 is three or more, based on the speed deviation Δω of the master speed omega M of the master motor 4 and slave speed omega S of the slave motor 6 current command value to the slave motor 6 Can be compensated.

本実施の形態において、マスタ動力伝達機構３およびスレーブ動力伝達機構５ａ，５ｂの減速比が同じ場合、電流指令値算出部１８は、速度ループゲインＫωＭを用いて速度偏差Δωに対してＰＩ補償を行った後の原電流指令値Ｉｏに１／３のゲインを掛けた値をマスタ電流指令値ＩＭおよびスレーブ電流指令値ＩＳａ，ＩＳｂとして出力する。これにより、共通の原電流指令値Ｉｏをマスタモータ４とスレーブモータ６ａ，６ｂとに均等に分配している。 In the present embodiment, when the reduction ratio of master power transmission mechanism 3 and slave power transmission mechanisms 5a and 5b is the same, current command value calculation unit 18 uses PI speed loop gain KωM to perform PI compensation for speed deviation Δω. A value obtained by multiplying the original current command value Io after 1/3 by a gain of 1/3 is output as a master current command value IM and slave current command values ISa , ISb . Thus, the common original current command value Io is evenly distributed to the master motor 4 and the slave motors 6a and 6b.

なお、本実施の形態においても第１の実施の形態の変形例と同様に、マスタ動力伝達機構３の減速比とスレーブ動力伝達機構５ａの減速比との割合が異なる場合、および／またはマスタ動力伝達機構３の減速比とスレーブ動力伝達機構５ｂの減速比との割合が異なる場合には、第１の実施の形態の分配要素１９，２０および速度調整要素２１に対応する箇所に各要素を設けることにより、本実施の形態の構成を実現することができる。   Also in the present embodiment, as in the modification of the first embodiment, when the ratio of the reduction ratio of the master power transmission mechanism 3 and the reduction ratio of the slave power transmission mechanism 5a is different, and / or the master power When the ratio of the reduction ratio of the transmission mechanism 3 and the reduction ratio of the slave power transmission mechanism 5b is different, each element is provided at a location corresponding to the distribution elements 19 and 20 and the speed adjustment element 21 of the first embodiment. Thus, the configuration of the present embodiment can be realized.

以上、本発明の実施の形態について説明したが、本発明は上記実施の形態に限定されるものではなく、その趣旨を逸脱しない範囲内で種々の改良、変更、修正が可能である。   Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and various improvements, changes, and modifications can be made without departing from the spirit of the present invention.

例えば、第１の実施の形態およびその変形例において説明した構成の一部を第２の実施の形態において適用することも可能である。   For example, a part of the configuration described in the first embodiment and the modifications thereof can be applied in the second embodiment.

また、上記実施の形態のマスタ速度取得部７およびスレーブ速度取得部８は、マスタモータ４およびスレーブモータ６の位置を検出し、速度に変換するように構成されているが、例えばエンコーダ等の速度検出器を用いてマスタモータ４およびスレーブモータ６の速度を直接取得することとしてもよい。   Further, the master speed acquisition unit 7 and the slave speed acquisition unit 8 of the above embodiment are configured to detect the positions of the master motor 4 and the slave motor 6 and convert them into speeds. It is good also as acquiring the speed of the master motor 4 and the slave motor 6 directly using a detector.

＜実施例＞
以下に、上記第１の実施の形態の構成に基づいてマスタモータ４およびスレーブモータ６間のトルク差、速度差および電流指令値差をシミュレーションした結果を示す。比較例としてスレーブモータ６の電流指令値として補償要素１６の出力ＧΔωを加えない（スレーブ電流指令値ＩＳをそのままスレーブモータ駆動部１５に入力する）態様におけるマスタモータ４およびスレーブモータ６間のトルク差、速度差および電流指令値差もシミュレーションした。 <Example>
Below, the result of having simulated the torque difference between the master motor 4 and the slave motor 6, the speed difference, and the electric current command value difference based on the structure of the said 1st Embodiment is shown. No added output GΔω the compensating element 16 as a current command value for the slave motor 6 as a comparative example (entering a slave current instruction value I S directly to the slave motor drive unit 15) the torque between the master motor 4 and the slave motor 6 in aspects Differences, speed differences and current command value differences were also simulated.

図４は本実施例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。図４の上側のグラフがトルクの時間的変化を示すグラフであり、下側のグラフが速度の時間的変化を示すグラフである。何れのグラフもマスタモータとスレーブモータとを重ねて示している。図５は図４に示す速度の時間的変化を示すグラフにおけるＶ部拡大図である。また、図７は比較例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。図８は図７に示す速度の時間的変化を示すグラフにおけるＶＩＩＩ部拡大図である。図７は実施例における図４に対応し、図８は実施例における図５に対応するグラフである。図５および図８の拡大図においては、マスタモータの速度波形とスレーブモータの速度波形が分かり易いように、概略的な波形のみをトレースするように加工している。   FIG. 4 is a graph showing temporal changes in torque and speed of the master motor and slave motor in this embodiment. The upper graph in FIG. 4 is a graph showing a temporal change in torque, and the lower graph is a graph showing a temporal change in speed. In both graphs, the master motor and the slave motor are overlapped. FIG. 5 is an enlarged view of a portion V in the graph showing the temporal change of the speed shown in FIG. FIG. 7 is a graph showing temporal changes in torque and speed of the master motor and slave motor in the comparative example. FIG. 8 is an enlarged view of a part VIII in the graph showing the temporal change of the speed shown in FIG. 7 corresponds to FIG. 4 in the embodiment, and FIG. 8 is a graph corresponding to FIG. 5 in the embodiment. In the enlarged views of FIGS. 5 and 8, only the schematic waveform is traced so that the speed waveform of the master motor and the speed waveform of the slave motor can be easily understood.

まず、図４と図７とのそれぞれのトルクの時間的変化を示すグラフ（上側のグラフ）を比較する。速度偏差Δωに基づく補償を行っていない比較例（図７）においては、特に、発生するトルクの向きが変わるＡｃ部においてノイズのような振動現象が大きく出ている。これは、トルク変動が大きいときに、一方のモータ（マスタモータ）に対して他方のモータ（スレーブモータ）がうまく追従できておらず、一方のモータのトルク値に対して他方のモータのトルク値が上回ったり下回ったりすることによるものと考えられる。   First, the graphs (upper graphs) showing temporal changes in torque in FIG. 4 and FIG. 7 are compared. In the comparative example (FIG. 7) in which compensation based on the speed deviation Δω is not performed, a vibration phenomenon such as noise appears greatly particularly in the Ac portion where the direction of the generated torque changes. This is because when the torque fluctuation is large, the other motor (slave motor) is not following the one motor (master motor) well, and the torque value of the other motor is compared to the torque value of one motor. This is thought to be due to the above or below.

これに対し、速度偏差Δωに基づく補償を行った本実施例（図４）においては、発生するトルクの向きが変わるＡｅ部においてもノイズのような振動現象は比較的抑えられ、全体的に滑らかな波形となっている。このことから、本実施例においては、マスタモータ４およびスレーブモータ６でそれぞれ発生するトルク値に差が比較的生じず、マスタモータ４のトルク変動に対してスレーブモータ６のトルク変動がよく追従していると考えられる。   In contrast, in the present embodiment (FIG. 4) in which compensation based on the speed deviation Δω is performed, the vibration phenomenon such as noise is relatively suppressed even in the Ae portion where the direction of the generated torque changes, and the overall smoothness is achieved. It has a simple waveform. Therefore, in this embodiment, there is relatively no difference in the torque values generated by the master motor 4 and the slave motor 6, and the torque fluctuation of the slave motor 6 follows the torque fluctuation of the master motor 4 well. It is thought that.

次に、図４と図７とのそれぞれの速度の時間的変化を示すグラフ（下側のグラフ）を比較する。比較例（図７）においては、特に、速度が連続的に変化する領域（例えばＶＩＩＩ部）において、図８に示すように、マスタモータの速度が比較的滑らかに変化するのに対し、スレーブモータの速度はマスタモータの速度にうまく追従できておらず、マスタモータの速度に対してスレーブモータの速度が近づいたり離れたりする結果となった。   Next, the graph (lower graph) which shows the time change of each speed of FIG. 4 and FIG. 7 is compared. In the comparative example (FIG. 7), the master motor speed changes relatively smoothly as shown in FIG. 8, particularly in the region where the speed changes continuously (for example, the VIII section), whereas the slave motor This speed did not follow the master motor speed well, resulting in the slave motor speed approaching or leaving the master motor speed.

これに対し、本実施例（図４）においては、同じ領域（Ｖ部）において、図５に示すように、マスタモータ４の速度およびスレーブモータ６の速度が何れも比較的滑らかに変化する結果が得られた。このように、本実施例によれば、マスタモータ４の速度とスレーブモータ６の速度とが離れることなくよく追従できていることが示された。   On the other hand, in the present embodiment (FIG. 4), as shown in FIG. 5, both the speed of the master motor 4 and the speed of the slave motor 6 change relatively smoothly in the same region (V section). was gotten. Thus, according to the present embodiment, it was shown that the speed of the master motor 4 and the speed of the slave motor 6 were able to follow well without leaving.

図６は本実施例における補償後スレーブ電流指令値ＩＣＳの時間的変化を比較例とともに示すグラフである。図６に示すように、速度偏差Δωに基づく補償を行っていない比較例においては電流指令値の振動幅が比較的大きいのに対し、本実施例では電流指令値ＩＣＳの振動的変化が比較的抑えられ、滑らかに変化する結果が得られた。このように、本実施例によればスレーブモータ６への電流指令値ＩＣＳ自体の振動的変化が抑えられ、比較的滑らかなトルク変動制御が行われることが示された。 FIG. 6 is a graph showing a temporal change of the compensated slave current command value ICS together with the comparative example in the present embodiment. As shown in FIG. 6, in the comparative example in which compensation based on the speed deviation Δω is not performed, the vibration width of the current command value is relatively large, whereas in this embodiment, the vibrational change in the current command value ICS is compared. The results were smooth and varied. Thus, according to the present embodiment, it was shown that the vibration change of the current command value ICS itself to the slave motor 6 is suppressed, and relatively smooth torque fluctuation control is performed.

本発明のロボットの制御装置およびロボットの制御方法は、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造においてモータ間の位置偏差を小さくするために有用である。   The robot control apparatus and the robot control method of the present invention are useful for reducing the positional deviation between motors in a servo control structure in which one drive shaft is driven by at least two motors.

１，１Ｂ ロボットの制御装置
２ 駆動軸
３ マスタ動力伝達機構
４ マスタモータ
５ スレーブ動力伝達機構
６ スレーブモータ
７ マスタ速度取得部
８ スレーブ速度取得部
９ マスタ位置取得部
１０，１２ 微分器
１１ スレーブ位置取得部
１３ 原電流指令値生成部
１６ 補償要素
１７ 速度指令値算出部
１８ 電流指令値算出部
1, 1B Robot control device 2 Drive shaft 3 Master power transmission mechanism 4 Master motor 5 Slave power transmission mechanism 6 Slave motor 7 Master speed acquisition unit 8 Slave speed acquisition unit 9 Master position acquisition units 10 and 12 Differentiator 11 Slave position acquisition Unit 13 Original current command value generation unit 16 Compensation element 17 Speed command value calculation unit 18 Current command value calculation unit

Claims (5)

ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御装置であって、
前記マスタモータの回転位置であるマスタ位置を取得するマスタ位置取得部と、
位置指令値と前記マスタ位置とに基づいて原電流指令値を生成する原電流指令値生成部と、
前記マスタモータの速度であるマスタ速度を取得するマスタ速度取得部と、
１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得するスレーブ速度取得部と、
前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための１以上の補償要素と、を備え、
前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償要素の出力をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するように制御する、ロボットの制御装置。 A robot control apparatus comprising: a drive shaft that drives a movable part that defines the operation of the robot; a master motor that rotationally drives the drive shaft; and one or more slave motors that rotationally drive the drive shaft. ,
A master position acquisition unit for acquiring a master position which is a rotational position of the master motor;
An original current command value generating unit that generates an original current command value based on a position command value and the master position;
A master speed acquisition unit that acquires a master speed that is the speed of the master motor;
A slave speed acquisition unit for acquiring one or more slave speeds, each of which is a speed of the one or more slave motors;
One or more compensation elements for compensating the slave speed corresponding to each deviation based on one or more deviations of the slave speed with respect to the master speed,
The master current command value and one or more slave current command values are generated by distributing the original current command value, the master motor is driven based on the master current command value, and each corresponds to the one or more slave motors. A controller for a robot that controls to drive one or more slave motors based on current command values obtained by adding outputs of the corresponding compensation elements to the one or more slave current command values. 前記補償要素は、比例要素を含む、請求項１に記載のロボットの制御装置。   The robot control apparatus according to claim 1, wherein the compensation element includes a proportional element. 前記比例要素の比例ゲインは、前記マスタ速度と前記スレーブ速度との偏差が単調に０に近づくような値に設定される、請求項２に記載のロボットの制御装置。   The robot control apparatus according to claim 2, wherein the proportional gain of the proportional element is set to a value such that a deviation between the master speed and the slave speed monotonously approaches zero. 前記マスタ速度取得部は、前記マスタ位置取得部と、前記マスタ位置取得部で取得された前記マスタ位置を微分して前記マスタ速度を出力する微分器と、を備え、
前記スレーブ速度取得部は、前記スレーブモータの回転位置であるスレーブ位置を取得するスレーブ位置取得部と、前記スレーブ位置を微分して前記スレーブ速度を出力する微分器と、を備えた、請求項１から３の何れかに記載のロボットの制御装置。 The master speed acquisition unit includes the master position acquisition unit, and a differentiator that differentiates the master position acquired by the master position acquisition unit and outputs the master speed,
The slave speed acquisition unit includes a slave position acquisition unit that acquires a slave position that is a rotational position of the slave motor, and a differentiator that differentiates the slave position and outputs the slave speed. 4. The robot control device according to any one of items 1 to 3. ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御方法であって、
前記マスタモータの回転位置であるマスタ位置を取得し、
位置指令値と前記マスタ位置とに基づいて原電流指令値を生成し、
前記マスタモータの速度であるマスタ速度を取得し、
１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得し、
前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための補償値を算出し、
前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償値をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動する、ロボットの制御方法。
A robot control method comprising: a drive shaft that drives a movable part that defines the operation of the robot; a master motor that rotationally drives the drive shaft; and one or more slave motors that rotationally drive the drive shaft. ,
Obtaining a master position which is the rotational position of the master motor;
An original current command value is generated based on the position command value and the master position,
Obtaining a master speed which is the speed of the master motor;
Obtain one or more slave speeds, each of which is the speed of one or more of the slave motors;
Calculating a compensation value for compensating the slave speed corresponding to each deviation based on one or more deviations of the slave speed with respect to the master speed;
The master current command value and one or more slave current command values are generated by distributing the original current command value, the master motor is driven based on the master current command value, and each corresponds to the one or more slave motors. A robot control method for driving one or more slave motors based on current command values obtained by adding the corresponding compensation values to the one or more slave current command values.
JP2014198047A 2014-09-29 2014-09-29 Robot control apparatus and control method Active JP6564175B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2014198047A JP6564175B2 (en) 2014-09-29 2014-09-29 Robot control apparatus and control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2014198047A JP6564175B2 (en) 2014-09-29 2014-09-29 Robot control apparatus and control method

Publications (2)

Publication Number Publication Date
JP2016068171A true JP2016068171A (en) 2016-05-09
JP6564175B2 JP6564175B2 (en) 2019-08-21

Family

ID=55865455

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2014198047A Active JP6564175B2 (en) 2014-09-29 2014-09-29 Robot control apparatus and control method

Country Status (1)

Country Link
JP (1) JP6564175B2 (en)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0884492A (en) * 1994-09-12 1996-03-26 Mitsubishi Heavy Ind Ltd Method and apparatus for operating servo motor synchronously
JP2000069782A (en) * 1998-08-21 2000-03-03 Yamaha Motor Co Ltd Linear-direction drive
JP2001273037A (en) * 2000-03-27 2001-10-05 Fanuc Ltd Servo control unit
JP2004086434A (en) * 2002-08-26 2004-03-18 Yaskawa Electric Corp Speed command type synchronization controller
JP2009106034A (en) * 2007-10-22 2009-05-14 Fanuc Ltd Control device for motor with learning control function
JP2013150493A (en) * 2012-01-23 2013-08-01 Canon Inc Servo device
JP2013219884A (en) * 2012-04-06 2013-10-24 Okuma Corp Position controller

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0884492A (en) * 1994-09-12 1996-03-26 Mitsubishi Heavy Ind Ltd Method and apparatus for operating servo motor synchronously
JP2000069782A (en) * 1998-08-21 2000-03-03 Yamaha Motor Co Ltd Linear-direction drive
JP2001273037A (en) * 2000-03-27 2001-10-05 Fanuc Ltd Servo control unit
JP2004086434A (en) * 2002-08-26 2004-03-18 Yaskawa Electric Corp Speed command type synchronization controller
JP2009106034A (en) * 2007-10-22 2009-05-14 Fanuc Ltd Control device for motor with learning control function
JP2013150493A (en) * 2012-01-23 2013-08-01 Canon Inc Servo device
JP2013219884A (en) * 2012-04-06 2013-10-24 Okuma Corp Position controller

Also Published As

Publication number Publication date
JP6564175B2 (en) 2019-08-21

Similar Documents

Publication Publication Date Title
JP5192571B2 (en) Control device for driving one driven body with two electric motors
TWI558089B (en) Synchronous control system for multi-axis motors and method thereof
JP5112100B2 (en) Servo control device
JP5919346B2 (en) Motor controller for correcting inter-axis interference
JP5642848B1 (en) Motor control device for controlling two motors to drive one driven body
JP6653542B2 (en) Motor control device
CN109495026B (en) Dual-drive gantry platform driving system, method and equipment and computer readable memory
US20130057194A1 (en) Motor control device
JPWO2008041598A1 (en) Servo control device
US9876448B2 (en) Position control apparatus
JP3981773B2 (en) Robot controller
CN104520066B (en) Torque control device
JP6564175B2 (en) Robot control apparatus and control method
JP2014046425A (en) Driving device
JP5778750B2 (en) Control device for injection molding machine having function of reducing synchronization error
CN113595449B (en) Device driving control method and system
JP5199682B2 (en) Motor control device
CN109491269A (en) Control device of electric motor and method of motor control
JP3954818B2 (en) Motor control device
JP2019221032A (en) Electric motor control device
JP2016005296A (en) Control method of motor drive device, motor drive device and robot device
JP6391489B2 (en) Motor control device
JP5409877B1 (en) Motor control device for electric vehicle and motor control method for electric vehicle
JP4367041B2 (en) Machine control device
JP5063981B2 (en) Electric motor position control device

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20170802

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20180619

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20180621

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20180808

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20181204

A601 Written request for extension of time

Free format text: JAPANESE INTERMEDIATE CODE: A601

Effective date: 20181219

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20190702

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20190726

R150 Certificate of patent or registration of utility model

Ref document number: 6564175

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250