JP2017030068A

JP2017030068A - Robot control device, robot and robot system

Info

Publication number: JP2017030068A
Application number: JP2015150428A
Authority: JP
Inventors: 泰裕下平; Yasuhiro Shimodaira; 正樹元▲吉▼; Masaki Motoyoshi; 馨竹内; Kaoru Takeuchi; 克司五十嵐; Katsushi Igarashi
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2015-07-30
Filing date: 2015-07-30
Publication date: 2017-02-09
Anticipated expiration: 2035-07-30
Also published as: JP6668629B2

Abstract

【課題】ロボットに対する力制御の再現性を高める技術を提供する。
【解決手段】ロボット制御装置は、マニピュレーターと、前記マニピュレーターに作用する力を検出する力検出器と、を備えるロボットを制御するロボット制御装置において、前記マニピュレーターの動作を制御する制御部は、前記マニピュレーターを動作させるコマンドを実行するとき、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するか、前記力検出器の出力に基づかずに前記マニピュレーターの動作を制御するかを当該コマンドのパラメータに応じて切り換える。
【選択図】図４A technique for improving the reproducibility of force control for a robot is provided.
A robot control device controls a robot including a manipulator and a force detector that detects a force acting on the manipulator, and the control unit that controls the operation of the manipulator includes the manipulator. When executing a command for operating the manipulator, whether to control the operation of the manipulator based on the output of the force detector or to control the operation of the manipulator without based on the output of the force detector Switch according to.
[Selection] Figure 4

Description

本発明は、ロボット制御装置、ロボットおよびロボットシステムに関する。 The present invention relates to a robot control device, a robot, and a robot system.

ロボットの分野においては、マニュピレーターに加わる力に応じてマニピュレーターを制御する力制御が用いられている。たとえば、特許文献１の図２には、「Activate ForceControl;」コマンドと、「Deactivate ForceControl;」コマンドとによって、マニュピレーターに加わる力に応じてマニピュレーターを制御する力制御モードと、マニュピレーターに加わる力と無関係にマニピュレーターを制御する位置制御モードとの切換が行われるプログラムが記載されている。 In the field of robots, force control is used to control the manipulator according to the force applied to the manipulator. For example, FIG. 2 of Patent Document 1 shows that the “Activate ForceControl;” command and the “Deactivate ForceControl;” command are used to control the manipulator in accordance with the force applied to the manipulator and the force applied to the manipulator. Describes a program for switching to a position control mode for controlling a manipulator.

米国特許７３４０３２３号公報US Pat. No. 7,340,323

しかし、特許文献１に記載されているようにコマンドによって位置制御モードから力制御モードに切り換えた後に、加わる力に応じてマニピュレーターを動作させるコマンドを実行する場合、マニピュレーターの動作結果の再現性が低くなるという問題がある。すなわち、水晶の圧電効果を用いている場合など、力検出器によっては出力が時間に依存するため、力検出器のどの時間の出力に基づいてマニピュレーターを制御するかによって、動作結果が異なることになる。具体的には、位置制御モードから力制御モードに切り換わった時点における力検出器の出力を基準としてマニピュレーターに加わる力を検出すると、力制御モードに切り換わってからマニピュレーターを実際に動かすコマンドが実行されるまでの時間の違いにより、マニピュレーターの動作結果が異なることになる。 However, when the command for operating the manipulator according to the applied force is executed after switching from the position control mode to the force control mode by a command as described in Patent Document 1, the reproducibility of the operation result of the manipulator is low. There is a problem of becoming. In other words, since the output depends on time depending on the force detector, such as when using the piezoelectric effect of quartz, the operation result varies depending on which time output of the force detector controls the manipulator. Become. Specifically, when the force applied to the manipulator is detected based on the output of the force detector when the position control mode is switched to the force control mode, a command to actually move the manipulator is executed after switching to the force control mode. The operation result of the manipulator varies depending on the time until the operation is performed.

本発明は、このような問題を解決するために創作されたものであって、ロボットに対する力制御の再現性を高める技術を提供することを目的とする。 The present invention has been created to solve such a problem, and an object of the present invention is to provide a technique for improving the reproducibility of force control for a robot.

前記目的を達成するためのロボット制御装置は、マニピュレーターと、前記マニピュレーターに作用する力を検出する力検出器と、を備えるロボットを制御するロボット制御装置であって、前記マニピュレーターの動作を制御する制御部は、前記マニピュレーターを動作させるコマンドを実行するとき、前記力検出器の出力に基づいて前記マニピュレーターの動作を制御するか、前記力検出器の出力に基づかずに前記マニピュレーターの動作を制御するかを当該コマンドのパラメータに応じて切り換える。 A robot control apparatus for achieving the object is a robot control apparatus that controls a robot including a manipulator and a force detector that detects a force acting on the manipulator, and controls the operation of the manipulator. When executing a command to operate the manipulator, the unit controls the operation of the manipulator based on the output of the force detector, or controls the operation of the manipulator without being based on the output of the force detector Is switched according to the parameter of the command.

ここで「マニピュレーターを動作させるコマンド」は、それ自体でマニピュレーターを動作させるコマンドを意味し、設定、モードの切換といった、それ自体ではマニピュレーターを動作させることがないコマンドを含まない。すなわち例えば、本発明では、マニピュレーターを動作させるコマンドの引数やコマンドの本体自体によって、力検出器の出力に基づいてマニピュレーターの動作を制御するか否かが決定される。したがって本発明によると、力検出器の出力に基づいてマニピュレーターの動作を制御する場合には、マニピュレーターを実際に動かすコマンドが実行される時点を出力の基準とする力検出器の出力に基づいてマニピュレーターの動作を制御することができる。このため本発明によるとロボットに対する力制御の再現性を高めることができる。 Here, the “command for operating the manipulator” means a command for operating the manipulator by itself, and does not include a command that does not operate the manipulator by itself, such as setting and mode switching. That is, for example, in the present invention, whether or not to control the operation of the manipulator is determined based on the output of the force detector based on the argument of the command for operating the manipulator and the command body itself. Therefore, according to the present invention, when the operation of the manipulator is controlled based on the output of the force detector, the manipulator is based on the output of the force detector based on the output when the command for actually moving the manipulator is executed. Can be controlled. Therefore, according to the present invention, the reproducibility of the force control for the robot can be improved.

なお請求項に記載された各手段の機能は、構成自体で機能が特定されるハードウェア資源、プログラムにより機能が特定されるハードウェア資源、又はそれらの組み合わせにより実現される。また、これら各手段の機能は、各々が物理的に互いに独立したハードウェア資源で実現されるものに限定されない。 Note that the function of each means described in the claims is realized by hardware resources whose function is specified by the configuration itself, hardware resources whose function is specified by a program, or a combination thereof. The functions of these means are not limited to those realized by hardware resources that are physically independent of each other.

ロボットシステムの斜視図である。It is a perspective view of a robot system. ロボットシステムのブロック図である。It is a block diagram of a robot system. 制御装置で実行される動作制御コマンドを示す表である。It is a table | surface which shows the operation control command performed with a control apparatus. 力制御対応コマンドが実行される場合の処理順序を示すフローチャートである。It is a flowchart which shows the process order in case a force control corresponding | compatible command is performed. 力覚センサーの出力を示す折れ線グラフである。It is a line graph which shows the output of a force sensor. 引数オブジェクトの階層構造を説明するための図である。It is a figure for demonstrating the hierarchical structure of an argument object. 引数オブジェクトの設定画面を示す画面構成図である。It is a screen block diagram which shows the setting screen of an argument object. プログラムコードの実施例と比較例である。It is the Example and comparative example of a program code.

以下、本発明の実施形態について添付図面を参照しながら説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。 Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In addition, the same code | symbol is attached | subjected to the corresponding component in each figure, and the overlapping description is abbreviate | omitted.

（１）ロボットシステムの構成
本発明の一実施例としてのロボットシステムは、図１に示すように、ロボット１と、エンドエフェクター２と、制御装置３と、教示装置４（ティーチングペンダント）と、を備えている。制御装置３は、本発明のロボット制御装置の構成例である。制御装置３は図示しないケーブルによりロボット１と通信可能に接続される。なお、制御装置３の構成要素がロボット１に備えられていても良い。制御装置３は、ロボット１に駆動電力を供給する電源部３１とロボット１を制御するための制御部３２とを備える。制御装置３と教示装置４とはケーブルで、または無線通信可能に接続される。教示装置４は、専用のコンピューターであってもよいし、ロボット１を教示するためのプログラムがインストールされた汎用のコンピューターであってもよい。例えばロボット１を教示するための専用装置であるティーチングペンダント５を教示装置４の代わりに用いても良い。さらに、制御装置３と教示装置４とは、図１に示すように別々の筐体を備えていてもよいし、一体に構成されていてもよい。 (1) Configuration of Robot System As shown in FIG. 1, a robot system according to an embodiment of the present invention includes a robot 1, an end effector 2, a control device 3, and a teaching device 4 (teaching pendant). I have. The control device 3 is a configuration example of the robot control device of the present invention. The control device 3 is communicably connected to the robot 1 via a cable (not shown). The components of the control device 3 may be provided in the robot 1. The control device 3 includes a power supply unit 31 that supplies driving power to the robot 1 and a control unit 32 for controlling the robot 1. The control device 3 and the teaching device 4 are connected by a cable or wireless communication. The teaching device 4 may be a dedicated computer or a general-purpose computer in which a program for teaching the robot 1 is installed. For example, a teaching pendant 5 that is a dedicated device for teaching the robot 1 may be used instead of the teaching device 4. Furthermore, the control device 3 and the teaching device 4 may be provided with separate housings as shown in FIG. 1 or may be configured integrally.

ロボット１は、アームＡに各種のエンドエフェクター２を装着して使用される単腕ロボットである。アームＡは６つの関節Ｊ１〜Ｊ６を備える。関節Ｊ１〜Ｊ６によって６個のアーム部材Ａ１〜Ａ６が連結される。関節Ｊ２、Ｊ３、Ｊ５は曲げ関節であり、関節Ｊ１、Ｊ４、Ｊ６はねじり関節である。関節Ｊ６には、ワークに対して把持や加工等を行うための各種のエンドエフェクター２が装着される。先端の関節Ｊ６の回転軸上の所定位置をツールセンターポイント（ＴＣＰ）と表す。ＴＣＰの位置は各種のエンドエフェクター２の位置の基準となる。アームＡとエンドエフェクター２とは、本発明のマニピュレーターの構成例である。 The robot 1 is a single-arm robot that is used with various end effectors 2 attached to the arm A. The arm A includes six joints J1 to J6. Six arm members A1 to A6 are connected by joints J1 to J6. Joints J2, J3, and J5 are bending joints, and joints J1, J4, and J6 are torsional joints. Various end effectors 2 for gripping and processing the workpiece are mounted on the joint J6. A predetermined position on the rotation axis of the joint J6 at the tip is represented as a tool center point (TCP). The position of the TCP is a reference for the positions of various end effectors 2. The arm A and the end effector 2 are configuration examples of the manipulator of the present invention.

関節Ｊ６には力覚センサーＦＳが備えられている。力覚センサーＦＳは、６軸の力検出器である。力覚センサーＦＳは、固有の座標系であるセンサー座標系において互いに直交する３個の検出軸と平行な力の大きさと、当該３個の検出軸まわりのトルクの大きさとを検出する。なお、力覚センサーＦＳは本発明の力検出器の構成例であるが、関節Ｊ６以外の関節Ｊ１〜Ｊ５のいずれか１つ以上に力検出器としての力覚センサーを備えても良い。 The joint J6 is provided with a force sensor FS. The force sensor FS is a 6-axis force detector. The force sensor FS detects the magnitude of the force parallel to the three detection axes orthogonal to each other in the sensor coordinate system, which is a unique coordinate system, and the magnitude of the torque around the three detection axes. The force sensor FS is a configuration example of the force detector of the present invention, but a force sensor as a force detector may be provided in any one or more of the joints J1 to J5 other than the joint J6.

ロボット１が設置された空間を規定する座標系をロボット座標系というとき、ロボット座標系は、水平面上において互いに直交するＸ軸とＹ軸と、鉛直上向きを正方向とするＺ軸とによって規定される３次元の直交座標系である。Ｚ軸における負の方向は概ね重力方向と一致する。またＸ軸周りの回転角をＲＸで表し、Ｙ軸周りの回転角をＲＹで表し、Ｚ軸周りの回転角をＲＺで表す。Ｘ，Ｙ，Ｚ方向の位置により３次元空間における任意の位置を表現でき、ＲＸ，ＲＹ，ＲＺ方向の回転角により３次元空間における任意の姿勢を表現できる。以下、位置と表記した場合、姿勢も意味し得ることとする。また、力と表記した場合、トルクも意味し得ることとする。制御装置３は、アームＡを駆動することによって、ロボット座標系においてＴＣＰの位置を制御する。 When the coordinate system that defines the space in which the robot 1 is installed is referred to as a robot coordinate system, the robot coordinate system is defined by an X axis and a Y axis that are orthogonal to each other on a horizontal plane, and a Z axis that has a vertical upward direction as a positive direction. This is a three-dimensional orthogonal coordinate system. The negative direction on the Z-axis generally coincides with the direction of gravity. A rotation angle around the X axis is represented by RX, a rotation angle around the Y axis is represented by RY, and a rotation angle around the Z axis is represented by RZ. An arbitrary position in the three-dimensional space can be expressed by a position in the X, Y, and Z directions, and an arbitrary posture in the three-dimensional space can be expressed by a rotation angle in the RX, RY, and RZ directions. Hereinafter, when it is described as a position, it can also mean a posture. In addition, when expressed as force, it can also mean torque. The controller 3 controls the position of the TCP in the robot coordinate system by driving the arm A.

図２は、ロボットシステムのブロック図である。制御部３２はロボット１の制御を行うための制御プログラムがインストールされたコンピューターである。制御部３２は、プロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源がプログラムと協働することによりロボット１を制御する。 FIG. 2 is a block diagram of the robot system. The control unit 32 is a computer in which a control program for controlling the robot 1 is installed. The control unit 32 includes a processor, a RAM, and a ROM, and controls the robot 1 when these hardware resources cooperate with the program.

ロボット１は、図１に示した構成のほかに、アクチュエーターとしてのモーターＭ１〜Ｍ６と、センサーとしてのエンコーダーＥ１〜Ｅ６とを備える。アームＡを制御することはモーターＭ１〜Ｍ６を制御することを意味する。モーターＭ１〜Ｍ６とエンコーダーＥ１〜Ｅ６とは、関節Ｊ１〜Ｊ６のそれぞれに対応して備えられており、エンコーダーＥ１〜Ｅ６はモーターＭ１〜Ｍ６の回転角度を検出する。制御装置３は、モーターＭ１〜Ｍ６の回転角度の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１を記憶している。また、制御装置３は、ロボット１が行う作業の工程ごとに目標位置Ｓ_tと目標力ｆ_Stの少なくともいずれかをコマンドに基づいて記憶する。目標位置Ｓ_tと目標力ｆ_Stを引数（パラメーター）とするコマンドはロボット１が行う作業の工程ごとに設定される。 The robot 1 includes motors M1 to M6 as actuators and encoders E1 to E6 as sensors in addition to the configuration shown in FIG. Controlling the arm A means controlling the motors M1 to M6. Motors M1 to M6 and encoders E1 to E6 are provided corresponding to the joints J1 to J6, respectively, and the encoders E1 to E6 detect the rotation angles of the motors M1 to M6. The control device 3 stores a correspondence U1 between a combination of rotation angles of the motors M1 to M6 and a TCP position in the robot coordinate system. Further, the control unit 3 stores on the basis of each step of the work robot 1 performs at least one of the target position S _t and the target force f _St to the command. Command to the target position S _t and the target force f _St argument (parameter) is set for each step of the work robot 1 performs.

制御装置３は、設定された目標位置と目標力とがＴＣＰにて実現されるように、コマンドに基づいてアームＡを制御する。目標力とは、アームＡの動作に応じて力覚センサーＦＳが検出すべき力である。ここでＳの文字は、ロボット座標系を規定する軸の方向（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）のなかのいずれか１個の方向を表すこととする。また、Ｓは、Ｓ方向の位置も表すこととする。例えば、Ｓ＝Ｘの場合、ロボット座標系にて設定された目標位置のＸ方向成分がＳ_t＝Ｘ_tと表記され、目標力のＸ方向成分がｆ_St＝ｆ_Xtと表記される。 The control device 3 controls the arm A based on the command so that the set target position and target force are realized by TCP. The target force is a force that should be detected by the force sensor FS in accordance with the operation of the arm A. Here, the letter S represents one of the directions (X, Y, Z, RX, RY, RZ) of the axes that define the robot coordinate system. S represents the position in the S direction. For example, in the case of S = X, X-direction component of the position specified in the robot coordinate system is denoted as S _{_t} = X _t, X-direction component of the desired force is denoted as f _St = f _Xt.

制御装置３は、モーターＭ１〜Ｍ６の回転角度Ｄ_aを取得すると、対応関係Ｕ１に基づいて、当該回転角度Ｄ_aをロボット座標系におけるＴＣＰの位置Ｓ（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）に変換する。また制御装置３は、ＴＣＰの位置Ｓと、力覚センサーＦＳの検出値とに基づいて、力覚センサーＦＳに現実に作用している作用力ｆ_Sをロボット座標系において特定する。作用力ｆ_ｓの作用点は、ＴＣＰとは別に原点Ｏとして定義される。原点Ｏは、力覚センサーＦＳが力を検出している点に対応する。なお制御装置３は、ロボット座標系におけるＴＣＰの位置Ｓごとに、力覚センサーＦＳのセンサー座標系における検出軸の方向を規定した対応関係Ｕ２を記憶している。従って、制御装置３は、ロボット座標系におけるＴＣＰの位置Ｓと対応関係Ｕ２とに基づいて、ロボット座標系における作用力ｆ_Sを特定できる。また、ロボットに作用するトルクは、作用力ｆ_Sと、ツール接触点（エンドエフェクター２とワークの接触点）から力覚センサーＦＳまでの距離とから算出することができ、図示されないfsトルク成分として特定される。 The controller 3 acquires the rotation angle D _a motor M1-M6, based on the correspondence relationship U1, the rotation angle D _a position of TCP in the robot coordinate system S (X, Y, Z, RX, RY, RZ). Further, the control device 3 specifies the acting force f _S actually acting on the force sensor FS in the robot coordinate system based on the position S of the TCP and the detection value of the force sensor FS. The point of action of the acting force f _s is defined as the origin O separately from TCP. The origin O corresponds to a point where the force sensor FS detects a force. The control device 3 stores a correspondence U2 that defines the direction of the detection axis in the sensor coordinate system of the force sensor FS for each TCP position S in the robot coordinate system. Therefore, the control device 3 can specify the acting force f _S in the robot coordinate system based on the TCP position S and the correspondence U2 in the robot coordinate system. Further, the torque acting on the robot can be calculated from the acting force f _S and the distance from the tool contact point (contact point between the end effector 2 and the workpiece) to the force sensor FS. Identified.

制御装置３は、作用力ｆ_Sに対して重力補償を行う。重力補償とは、作用力ｆ_Sから重力に起因する力やトルクの成分を除去することである。重力補償を行った作用力ｆ_Sは、エンドエフェクター２に作用している重力以外の力と見なすことができる。 The control device 3 performs gravity compensation for the acting force f _S. Gravity compensation is to remove components of force and torque caused by gravity from the acting force f _S. The acting force f _S subjected to gravity compensation can be regarded as a force other than gravity acting on the end effector 2.

本実施例のインピーダンス制御は、仮想の機械的インピーダンスをモーターＭ１〜Ｍ６によって実現する能動インピーダンス制御である。制御装置３は、このようなインピーダンス制御を、ワークの嵌合作業、研磨作業など、エンドエフェクター２が対象物（ワーク）から力を受ける接触状態の工程で適用する。インピーダンス制御では、目標力を後述する運動方程式に代入してモーターＭ１〜Ｍ６の回転角度を導出する。制御装置３がモーターＭ１〜Ｍ６を制御する信号は、ＰＷＭ（Pulse Width Modulation）変調された信号である。運動方程式に基づいて目標力から回転角度を導出してモーターＭ１〜Ｍ６を制御するモードを力制御モードというものとする。また制御装置３は、エンドエフェクター２がワークから力を受けない非接触状態の工程では、目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６を制御する。目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６を制御するモードを位置制御モードというものとする。また制御装置３は、目標位置から線形演算で導出する回転角度と目標力を運動方程式に代入して導出する回転角度とを例えば線型結合によって統合し、統合した回転角度でモーターＭ１〜Ｍ６を制御するハイブリッドモードでもロボット１を制御する。制御装置３は、位置制御モードと力制御モードとハイブリッドモードを力覚センサーＦＳまたはエンコーダーＥ１〜Ｒ６の検出値に基づいて自律的に切り替えることもできるし、コマンドに応じて位置制御モードと力制御モードとハイブリッドモードを切り替えることもできる。以上の構成より制御装置３は、エンドエフェクター２が目標の位置において目標の姿勢となり、かつ、エンドエフェクター２に目標の力とモーメントとが作用するようにアームＡを駆動することができる。 The impedance control of the present embodiment is active impedance control that realizes a virtual mechanical impedance by the motors M1 to M6. The control device 3 applies such impedance control in a contact state process in which the end effector 2 receives a force from the object (workpiece), such as work fitting work and polishing work. In the impedance control, the target force is substituted into an equation of motion described later to derive the rotation angles of the motors M1 to M6. The signal for controlling the motors M1 to M6 by the control device 3 is a PWM (Pulse Width Modulation) modulated signal. A mode in which the rotation angle is derived from the target force based on the equation of motion to control the motors M1 to M6 is referred to as a force control mode. The control device 3 controls the motors M <b> 1 to M <b> 6 at a rotation angle derived from the target position by linear calculation in a non-contact state in which the end effector 2 does not receive force from the workpiece. A mode in which the motors M1 to M6 are controlled at a rotation angle derived from the target position by linear calculation is referred to as a position control mode. The control device 3 integrates the rotation angle derived from the target position by linear calculation and the rotation angle derived by substituting the target force into the equation of motion, for example, by linear coupling, and controls the motors M1 to M6 with the integrated rotation angle. The robot 1 is also controlled in the hybrid mode. The control device 3 can autonomously switch between the position control mode, the force control mode, and the hybrid mode based on the detection values of the force sensor FS or the encoders E1 to R6, or the position control mode and the force control according to the command. You can also switch between mode and hybrid mode. With the above configuration, the control device 3 can drive the arm A so that the end effector 2 assumes a target posture at the target position and the target force and moment act on the end effector 2.

制御装置３は、目標力ｆ_Stと作用力ｆ_Sとをインピーダンス制御の運動方程式に代入することにより、力由来補正量ΔＳを特定する。力由来補正量ΔＳとは、ＴＣＰが機械的インピーダンスを受けた場合に、目標力ｆ_Stとの力偏差Δｆ_S（ｔ）を解消するために、ＴＣＰが移動すべき位置Ｓの大きさを意味する。下記の（１）式は、インピーダンス制御の運動方程式である。

The control device 3 specifies the force-derived correction amount ΔS by substituting the target force f _St and the acting force f _S into the equation of motion for impedance control. The force-derived correction amount ΔS means the size of the position S where the TCP should move in order to eliminate the force deviation Δf _S (t) from the target force f _St when the TCP receives mechanical impedance. To do. The following equation (1) is an equation of motion for impedance control.

（１）式の左辺は、ＴＣＰの位置Ｓの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｓの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｓに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆ_Stから現実の力ｆを減算した力偏差Δｆ_S（ｔ）によって構成される。（１）式における微分とは、時間による微分を意味する。ロボット１が行う工程において、目標力ｆ_Stとして一定値が設定される場合もあるし、目標力ｆ_Stとして時間の関数が設定される場合もある。 The left side of the equation (1) is a first term obtained by multiplying the second-order differential value of the TCP position S by the virtual inertia coefficient m, a second term obtained by multiplying the differential value of the TCP position S by the virtual viscosity coefficient d, And a third term obtained by multiplying the position S of the TCP by the virtual elastic coefficient k. The right side of the equation (1) is constituted by a force deviation Δf _S (t) obtained by subtracting the actual force f from the target force f _St. The differentiation in the equation (1) means differentiation with time. In the step of the robot 1 performs, to some cases a constant value is set as the target force f _St, there is a case where a function of time as the target force f _St is set.

仮想慣性係数ｍはＴＣＰが仮想的に有する質量を意味し、仮想粘性係数ｄはＴＣＰが仮想的に受ける粘性抵抗を意味し、仮想弾性係数ｋはＴＣＰが仮想的に受ける弾性力のバネ定数を意味する。各係数ｍ，ｄ，ｋは方向ごとに異なる値に設定されてもよいし、方向に拘わらず共通の値に設定されてもよい。 The virtual inertia coefficient m means the mass that the TCP virtually has, the virtual viscosity coefficient d means the viscous resistance that the TCP virtually receives, and the virtual elastic coefficient k is the spring constant of the elastic force that the TCP virtually receives. means. Each coefficient m, d, k may be set to a different value for each direction, or may be set to a common value regardless of the direction.

そして、制御装置３は、対応関係Ｕ１に基づいて、ロボット座標系を規定する各軸の方向の動作位置を、各モーターＭ１〜Ｍ６の目標の回転角度である目標角度Ｄ_tに変換する。そして、制御装置３は、目標角度Ｄ_tからモーターＭ１〜Ｍ６の現実の回転角度であるエンコーダーＥ１〜Ｅ６の出力Ｄ_aを減算することにより、駆動位置偏差Ｄ_e（＝Ｄ_t−Ｄ_a）を算出する。そして、制御装置３は、駆動位置偏差Ｄ_eに位置制御ゲインＫ_pを乗算した値と、現実の回転角度Ｄ_aの時間微分値である駆動速度との差である駆動速度偏差に、速度制御ゲインＫ_vを乗算した値とを加算することにより、制御量Ｄ_cを導出する。なお、位置制御ゲインＫ_pおよび速度制御ゲインＫ_vは、比例成分だけでなく微分成分や積分成分にかかる制御ゲインを含んでもよい。制御量Ｄ_cは、モーターＭ１〜Ｍ６のそれぞれについて特定される。以上説明した構成により、制御装置３は、目標力ｆ_Stとに基づいてアームＡを力制御モードで制御することができる。ハイブリッドモードでは、制御装置３は、目標位置Ｓ_tに、力由来補正量ΔＳを加算することにより動作位置（Ｓ_t＋ΔＳ）を特定する。 Then, the control unit 3, based on the correspondence relationship U1, the operation position in the direction of each axis defining a robot coordinate system, and converts the target angle D _t is a rotation angle of the target of the motors M1-M6. Then, the control unit 3, by subtracting the output D _a of the encoder E1~E6 from the target angle D _t is a rotation angle of the real motor M1-M6, the drive position deviation _{_{_{D e (= D t -D a}}} ) Is calculated. Then, the control device 3, a value obtained by multiplying the position control gain K _p to the driving position deviation D _e, the driving speed deviation which is a difference between the driving speed is the time differential value of the actual rotational angle D _a, the speed control The control amount D _c is derived by adding the value multiplied by the gain K _v . Note that the position control gain K _p and the speed control gain K _v may include not only a proportional component but also a control gain related to a differential component and an integral component. The control amount D _c is specified for each of the motors M1 to M6. With the configuration described above, the control device 3 can control the arm A in the force control mode based on the target force f _St. In the hybrid mode, the control device 3, the target position S _t, identifies the operating position by adding a force from the correction amount ΔS (S _t + ΔS).

教示装置４には、制御装置３に目標位置Ｓ_tと目標力ｆ_Stとを引数とする実行プログラムを生成して制御装置３にロードするための教示プログラムがインストールされている。教示装置４は、ディスプレイ４３やプロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源が教示プログラムと協働して実行プログラムを生成する。 The teaching device 4, the teaching program for loading the control unit 3 generates an execution program for the target position S _t and the target force f _St arguments is installed in the control unit 3. The teaching device 4 includes a display 43, a processor, a RAM, and a ROM, and these hardware resources generate an execution program in cooperation with the teaching program.

（２）実行プログラム
実行プログラムは予め定められたプログラム言語によって記述され、翻訳プログラムにより中間言語を経て機械語プログラムに変換される。制御部３２のＣＰＵはクロックサイクルで機械語プログラムを実行する。翻訳プログラムは教示装置４で実行しても良いし、制御装置３で実行しても良い。実行プログラムのコマンドは本体と引数とから構成される。コマンドには、アームＡとエンドエフェクター２を動作させる動作制御コマンド、エンコーダーやセンサーの検出値を読み出すモニターコマンド、各種の変数を設定する設定コマンド等が含まれる。なお、本明細書において、コマンドの実行は、当該コマンドが翻訳された機械語プログラムの実行と同義である。 (2) Execution program The execution program is described in a predetermined program language, and is converted into a machine language program through an intermediate language by a translation program. The CPU of the control unit 32 executes the machine language program in a clock cycle. The translation program may be executed by the teaching device 4 or may be executed by the control device 3. The command of the execution program is composed of a main body and arguments. The commands include an operation control command for operating the arm A and the end effector 2, a monitor command for reading detection values of the encoder and sensor, a setting command for setting various variables, and the like. In this specification, execution of a command is synonymous with execution of a machine language program in which the command is translated.

図３に動作制御コマンド（本体）の一例を示す。図３に示すように動作制御コマンドは、力制御モードでアームＡを動作させることができる力制御対応コマンドと、力制御モードでアームＡを動作させることができない位置制御コマンドとを含む。力制御対応コマンドでは、引数により力制御モードのオンを指定できる。当該引数により力制御モードのオンが指定されない場合には、位置制御モードで当該力制御対応コマンドが実行され、当該引数により力制御モードのオンが指定される場合には、当該力制御対応コマンドは力制御モードで実行される。また力制御対応コマンドは力制御モードにおいて実行可能であり、位置制御コマンドは力制御モードでは実行不能である。力制御モードにおいて位置制御コマンドが実行されることがないように、翻訳プログラムによる構文エラーチェックが実行される。さらに、力制御対応コマンドでは、引数により力制御モードの継続を指定できる。力制御モードで実行される力制御対応コマンドにおいて当該引数により力制御モードの継続が指定された場合、力制御モードは継続され、当該引数により力制御モードの継続が指定されない場合、当該力制御対応コマンドの実行完了までに力制御モードは終了する。すなわち力制御対応コマンドが力制御モードで実行されるとしても、引数により明示的に指定されない限り、力制御モードは当該力制御対応コマンドに応じて自律的に終了し、当該力制御対応コマンドの実行終了後においてまで力制御モードが継続することはない。なお図３において「ＣＰ」は移動方向を指定できるコマンドの分類、「ＰＴＰ」は目標位置を指定できるコマンドの分類、「ＣＰ＋ＰＴＰ」は移動方向と目標位置を指定できるコマンドの分類である。 FIG. 3 shows an example of the operation control command (main body). As shown in FIG. 3, the operation control command includes a force control corresponding command that can operate the arm A in the force control mode and a position control command that cannot operate the arm A in the force control mode. In the force control compatible command, the force control mode can be turned on by an argument. When the force control mode is not designated by the argument, the force control corresponding command is executed in the position control mode. When the force control mode is designated by the argument, the force control correspondence command is It is executed in force control mode. Further, the force control corresponding command can be executed in the force control mode, and the position control command cannot be executed in the force control mode. A syntax error check by the translation program is executed so that the position control command is not executed in the force control mode. Further, in the force control compatible command, the continuation of the force control mode can be designated by an argument. If the continuation of the force control mode is specified by the argument in the force control support command executed in the force control mode, the force control mode is continued, and if the continuation of the force control mode is not specified by the argument, the force control support The force control mode ends before the command execution is completed. That is, even if a force control command is executed in force control mode, unless explicitly specified by an argument, the force control mode ends autonomously according to the force control command and executes the force control command. The force control mode will not continue until after the end. In FIG. 3, “CP” is a command classification that can specify a movement direction, “PTP” is a command classification that can specify a target position, and “CP + PTP” is a command classification that can specify a movement direction and a target position.

ここで、引数により力制御モードのオンが指定され、かつ、力制御モードの継続が指定されていない「Move」コマンドが力制御モードでない状態において実行開始される場合を例にして、力制御対応コマンドの実行手順、すなわち制御部３２の動作順序について図４を参照しながら説明する。 Here, force control is supported by taking the case where the “Move” command, which is specified by the argument to turn on the force control mode and is not specified to continue the force control mode, starts executing in a state other than the force control mode. The command execution procedure, that is, the operation sequence of the control unit 32 will be described with reference to FIG.

まずはじめに制御部３２は、力制御モードのオンを指定する引数があるか無いかを判定する（Ｓ１０２）。
力制御モードのオンを指定する引数がある場合、制御部３２は、力覚センサーＦＳをリセットする（Ｓ１１２）。力覚センサーＦＳのリセットは、力覚センサーＦＳの出力が、このリセットタイミングでゼロとして検出されるように力覚センサーＦＳ自体または制御部３２が記憶するオフセット値を設定することである。 First, the control unit 32 determines whether or not there is an argument for designating ON of the force control mode (S102).
When there is an argument for designating ON of the force control mode, the control unit 32 resets the force sensor FS (S112). The reset of the force sensor FS is to set an offset value stored in the force sensor FS itself or the control unit 32 so that the output of the force sensor FS is detected as zero at this reset timing.

続いて制御部３２は、力制御モードに遷移してインピーダンス制御を実行し、アームＡを動作させる（Ｓ１１４）。すなわち制御部３２は、力覚センサーＦＳの出力に基づいてモーターＭ１〜Ｍ６の回転角度を制御する。ここで目標力は、力制御対応コマンドの引数として指定されていてもよいし、力制御対応コマンドより先に実行される設定コマンドで指定されていてもよい。位置制御モードから力制御モードへの切換は、例えば図２に示すΔＳとＳｔとを足し合わせる算術要素においてΔＳの入力をオンにしてＳｔの入力をオフ（ゼロ）にする操作によって実施される。 Subsequently, the control unit 32 transitions to the force control mode, executes impedance control, and operates the arm A (S114). That is, the control unit 32 controls the rotation angles of the motors M1 to M6 based on the output of the force sensor FS. Here, the target force may be specified as an argument of the force control corresponding command, or may be specified by a setting command executed before the force control corresponding command. Switching from the position control mode to the force control mode is performed, for example, by an operation of turning on the input of ΔS and turning off the input of St (zero) in the arithmetic element adding ΔS and St shown in FIG.

続いて制御部３２は、力制御モードを終了する（Ｓ１１６）。力制御モードから位置制御モードへの切換は、例えば図２に示すΔＳとＳｔとを足し合わせる算術要素においてΔＳの入力をオフ（ゼロ）にしてＳｔの入力をオンにする操作によって実施される。 Subsequently, the control unit 32 ends the force control mode (S116). Switching from the force control mode to the position control mode is performed, for example, by an operation of turning off the input of ΔS and turning on the input of St in the arithmetic element adding ΔS and St shown in FIG.

力制御モードのオンを指定する引数が無い場合、制御部３２は位置制御モードにおいて位置制御を実行し、アームＡを動作させる（Ｓ１２０）。すなわち制御部３２は、目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６の回転角度を制御する。 When there is no argument for designating ON of the force control mode, the control unit 32 performs position control in the position control mode and operates the arm A (S120). That is, the control unit 32 controls the rotation angles of the motors M1 to M6 with the rotation angle derived from the target position by linear calculation.

ところで水晶を圧電素子として用いる力覚センサーでは、力覚センサーに加わる力に変化が無くても、図５Ａに示すように、時間の経過に伴って出力が大きくなる傾向にある。したがって、例えば図５Ｂに示す時間ｔ_０で力制御モードがオンになるとすると、同じ力が加わっているにもかかわらず、ｔ_０からｔ_１が経過した時点で検出される力ｆ１と、ｔ_０からｔ_２が経過した時点で検出される力ｆ_２とが異なる結果となる。本実施例では、上述したように力制御対応コマンドの実行時点で力制御モードに遷移して力覚センサーＦＳがリセットされるため、力制御対応コマンドが実行される時点を出力の基準とする力覚センサーＦＳの出力に基づいてアームＡの動作を制御することができる。このためアームＡに対する力制御の再現性を高めることができる。例えば、動作制御コマンドに対して力覚センサーＦＳのリセットが同期しない場合、すなわち動作制御コマンドの実行に先行して力覚センサーＦＳがリセットされる場合、動作制御コマンドの実行が３ｍｓｅｃ程度（ｔ_２−ｔ_１）遅れることになる。力制御が１０ｍｓｅｃ間隔で実行されるとすると、この３ｍｓｅｃは力制御の間隔に対し大きな実行の遅れであり再現性を悪化させる。 By the way, in the force sensor using quartz as a piezoelectric element, even if there is no change in the force applied to the force sensor, as shown in FIG. 5A, the output tends to increase with time. Thus, for example, when the force control mode at the time t ₀ shown in FIG. 5B is a turned on, even though the same force is applied, the force f1 of t ₁ from t ₀ is detected at the time has elapsed, t ₀ Results in a difference from the force f ₂ detected when t ₂ elapses. In the present embodiment, as described above, the force sensor FS is reset when the force control corresponding command is executed and the force sensor FS is reset. The operation of the arm A can be controlled based on the output of the sense sensor FS. For this reason, the reproducibility of the force control for the arm A can be improved. For example, when the reset of the force sensor FS is not synchronized with the motion control command, that is, when the force sensor FS is reset prior to the execution of the motion control command, the execution of the motion control command is about 3 msec (t _{2 -} it becomes t ₁₎ delayed it. If force control is executed at intervals of 10 msec, this 3 msec is a large execution delay with respect to the force control interval and deteriorates reproducibility.

実行プログラムのコマンドの引数は階層化されたオブジェクト構造となっている。力制御コマンドに関連する引数のクラスを図６に示す。「Force Coordinate System Object」は、座標系の設定に用いられるクラスであって、ＦＣＳｘ（ｘは任意の整数）で定義することができる。「Force Control Object」は、力制御の対象となる座標軸等の設定に用いられるクラスであって、ＦＣｘ（ｘは任意の整数）で定義することができる。「Force Trigger Object」は、分岐条件、停止条件、割り込みの設定に用いられるクラスであって、ＦＴｘ（ｘは任意の整数）で定義することができる。「Force Monitor Object」は、力データの取得、ログ記録等の設定に用いられるクラスであって、ＦＭｘ（ｘは任意の整数）で定義することができる。「Force Control Object」、「Force Trigger Object」および「Force Monitor Object」で指定される座標系は「Force Coordinate System Object」で定義された座標系となる。すなわち、「Force Control Object」、「Force Trigger Object」および「Force Monitor Object」は、上位層クラスの「Force Coordinate System Object」に従属する下位層クラスである。「Force Sensor Object」は、力覚センサーＦＳの制御と力覚センサーＦＳからの情報取得の設定に用いるクラスであって、ＦＳｘ（ｘは任意の整数）で定義することができる。「Mass Properties Object」は、重心位置の設定に用いるクラスであって、ＭＰｘ（ｘは任意の整数）で定義することができる。「Robot Object」は、力制御と関連したロボット１の情報を取得するための設定に用いるクラスであって、Ｒｏｂｏｔｘ（ｘは任意の整数）で定義することができる。 The argument of the command of the execution program has a hierarchical object structure. FIG. 6 shows an argument class related to the force control command. “Force Coordinate System Object” is a class used for setting a coordinate system, and can be defined by FCSx (x is an arbitrary integer). “Force Control Object” is a class used for setting coordinate axes and the like that are targets of force control, and can be defined by FCx (x is an arbitrary integer). “Force Trigger Object” is a class used for setting a branch condition, a stop condition, and an interrupt, and can be defined by FTx (x is an arbitrary integer). “Force Monitor Object” is a class used for setting force data acquisition, log recording, and the like, and can be defined by FMx (x is an arbitrary integer). The coordinate system specified by “Force Control Object”, “Force Trigger Object”, and “Force Monitor Object” is the coordinate system defined by “Force Coordinate System Object”. That is, “Force Control Object”, “Force Trigger Object”, and “Force Monitor Object” are lower layer classes subordinate to the upper layer class “Force Coordinate System Object”. “Force Sensor Object” is a class used for control of the force sensor FS and information acquisition setting from the force sensor FS, and can be defined by FSx (x is an arbitrary integer). “Mass Properties Object” is a class used for setting the position of the center of gravity, and can be defined by MPx (x is an arbitrary integer). “Robot Object” is a class used for setting for acquiring information of the robot 1 related to force control, and can be defined by Robotx (x is an arbitrary integer).

次に、このように階層化された引数の設定方法について説明する。図７Ａは「Force Coordinate System Object」クラスの引数オブジェクトを設定する画面の構成例である。図７Ａは、「ハンド面３先端」というラベルが付された「力覚座標４」（プログラムコードではＦＣＳ４と記述される引数オブジェクト）の原点のＴＣＰを基準とするロボット座標系における相対的な位置と、ＴＣＰの姿勢（ＴＣＰに固定された座標系）を基準とする相対的な姿勢とを設定した状態を示している。ここで制御部３２は、モーターＭ１〜Ｍ６の回転角度の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１を記憶しているため、「Force Coordinate System Object」クラスの引数オブジェクトにおいてＴＣＰに対する相対的な位置と姿勢を設定することにより、基準座標系に対して任意の座標系を定義することができる。 Next, a method for setting the arguments hierarchized in this way will be described. FIG. 7A is a configuration example of a screen for setting an argument object of the “Force Coordinate System Object” class. FIG. 7A shows a relative position in the robot coordinate system based on the origin TCP of “force coordinate 4” (argument object described as FCS4 in the program code) labeled “tip of hand surface 3”. And a relative posture based on the posture of TCP (a coordinate system fixed to TCP). Here, since the control unit 32 stores the correspondence U1 between the combination of the rotation angles of the motors M1 to M6 and the position of the TCP in the robot coordinate system, in the argument object of the “Force Coordinate System Object” class, By setting the relative position and orientation, an arbitrary coordinate system can be defined with respect to the reference coordinate system.

図７Ｂは、「Force Control Object」クラスの引数オブジェクトを設定する画面の構成例である。図７Ｂは、「姿勢倣い」というラベルが付された「力制御２」（プログラムコードではＦＣ２と記述される引数オブジェクト）の座標系と、力制御の有効軸と、インピーダンス制御の運動方程式の係数（仮想バネ係数、仮想粘性係数、仮想質量係数）とを設定した状態を示している。ここで座標系として「４」が選択されているため、図７Ａの画面で設定された「力覚座標４」（ＦＣＳ４）の座標系の各軸について、「有効」の項目において力制御のオン（True）とオフ（False）が設定される。すなわち、力制御が有効になる方向は、上位の「Force Coordinate System Object」クラスのオブジェクトの設定に依存する。図７Ｃは「Force Trigger Object」クラスの引数オブジェクトを設定する画面の構成例である。図７Ｃにおいても、「接触検知」というラベルが付された「力トリガー１」（プログラムコードではＦＴ１と記述される引数オブジェクト）の座標系として「４」が選択されているため、図７Ａの画面で設定された「力覚座標４」（ＦＣＳ４）の座標系の各軸について処理条件が定められる。すなわち、停止条件の各変数が示す値やベクトルの方向は、上位の「Force Coordinate System Object」クラスのオブジェクトの設定に依存する。このように本実施例のコマンドの引数は階層化されているため、「Force Coordinate System Object」クラスのオブジェクトとして１つの座標系を設定すれば、その座標系を様々なコマンドに適用することができ、設定作業が容易になる。 FIG. 7B is a configuration example of a screen for setting an argument object of the “Force Control Object” class. FIG. 7B shows the coordinate system of “force control 2” (argument object described as FC2 in the program code) labeled “posture tracing”, the effective axis of force control, and the coefficient of the equation of motion for impedance control. A state in which (virtual spring coefficient, virtual viscosity coefficient, virtual mass coefficient) is set is shown. Since “4” is selected as the coordinate system here, the force control is turned on in the “valid” item for each axis of the coordinate system of “force coordinate 4” (FCS4) set on the screen of FIG. 7A. (True) and off (False) are set. In other words, the direction in which force control becomes effective depends on the setting of an object of the higher-level “Force Coordinate System Object” class. FIG. 7C is a configuration example of a screen for setting an argument object of the “Force Trigger Object” class. Also in FIG. 7C, “4” is selected as the coordinate system of “force trigger 1” (argument object described as FT1 in the program code) labeled “contact detection”. The processing conditions are determined for each axis of the coordinate system of “force coordinate 4” (FCS4) set in (1). In other words, the value or vector direction indicated by each variable of the stop condition depends on the setting of the object of the higher-level “Force Coordinate System Object” class. As described above, since the command argument of this embodiment is hierarchical, if one coordinate system is set as an object of the “Force Coordinate System Object” class, the coordinate system can be applied to various commands. The setting work becomes easy.

力制御対応コマンドを用いたプログラムコードの実施例を比較例とともに図８に示す。図８に示す実施例は、「ＦＣ１」という引数オブジェクトが６つの設定コマンド「Ｆｓｅｔ」で定義された後に、力制御対応コマンド「Ｍｏｖｅ」が実行され、その後に「ＦＣ１」オブジェクトの変数の１つである「Fz_TargetForce」が設定コマンド「Ｆｓｅｔ」で変更されるプログラムコードである。 An example of the program code using the force control corresponding command is shown in FIG. 8 together with a comparative example. In the example shown in FIG. 8, after the argument object “FC1” is defined by the six setting commands “Fset”, the force control corresponding command “Move” is executed, and then one of the variables of the “FC1” object. “Fz_TargetForce” is a program code that is changed by the setting command “Fset”.

図８に示す比較例では、コマンドの引数が階層化されておらず、力制御モードで用いられる全ての設定項目が「ForceControlSetting」コマンドにより一度に設定される。そして、カンマで区切られたそれぞれの引数の定義は、コマンド本体と引数の間に記述するカンマの数で決まる。したがって比較例のプログラムコードの意味を即座に理解することは困難である。これに対し、本発明の実施例では、力制御対応コマンドに関連する引数は、階層化されたオブジェクト毎に「Fset」コマンドにより設定されるため、プログラムコードの意味を解釈することも、プログラムコードを記述することもはるかに容易である。 In the comparative example shown in FIG. 8, the command arguments are not hierarchized, and all setting items used in the force control mode are set at once by the “ForceControlSetting” command. The definition of each argument separated by commas is determined by the number of commas described between the command body and the arguments. Therefore, it is difficult to immediately understand the meaning of the program code of the comparative example. On the other hand, in the embodiment of the present invention, since the argument related to the force control corresponding command is set by the “Fset” command for each layered object, the meaning of the program code can be interpreted. Is much easier to describe.

本実施例のコマンド「Move P1 FC1 CF」では、「Fset」コマンドにより設定された「ＦＣ１」オブジェクトの他に、図示しない設定コマンドにより設定された「Ｐ１」オブジェクトと、「ＣＦ」が引数として設定されている。ここで「Ｐ１」は、接触位置を指定する引数である。「ＣＦ」は、コマンドの実行終了後も力制御モードの継続を指定する引数である。コマンド「Move P1 FC1 CF」は、「Force Control Object」クラスの引数オブジェクト「ＦＣ１」が指定されており、目標位置が引数「Ｐ１」で指定されているため、コマンド「Move P1 FC1 CF」が実行されると、力覚センサーＦＳがリセットされ、力覚センサーＦＳの出力に基づくハイブリッドモードでアームＡが制御される。また引数「ＣＦ」が指定されているため、コマンドの実行終了後も力制御モードが継続する。 In the command “Move P1 FC1 CF” of this embodiment, in addition to the “FC1” object set by the “Fset” command, the “P1” object set by a setting command (not shown) and “CF” are set as arguments. Has been. Here, “P1” is an argument for designating the contact position. “CF” is an argument that specifies the continuation of the force control mode even after the execution of the command is completed. The command “Move P1 FC1 CF” is executed by the command “Move P1 FC1 CF” because the argument object “FC1” of the “Force Control Object” class is specified and the target position is specified by the argument “P1”. Then, the force sensor FS is reset, and the arm A is controlled in the hybrid mode based on the output of the force sensor FS. Also, since the argument “CF” is designated, the force control mode continues even after the command execution is completed.

比較例では、「ForceControl On」コマンドの実行により、力覚センサーＦＳがリセットされ、力覚センサーＦＳの出力に基づいてアームＡを制御可能な状態となる。続いて「Move P1」コマンドの実行により、力覚センサーＦＳの出力に基づいてアームＡが制御される。これに対して、本実施例のコマンド「Move P1 FC1 CF」の実行が開始されてから力覚センサーＦＳがリセットされるまでの実時間は、対応する機械語プログラムが常に同じになるため、制御部３２のクロック数によって予め定まる一定の時間となる。したがって上述したように、本実施例では力制御の再現性が保証される。 In the comparative example, the force sensor FS is reset by executing the “ForceControl On” command, and the arm A can be controlled based on the output of the force sensor FS. Subsequently, by executing the “Move P1” command, the arm A is controlled based on the output of the force sensor FS. In contrast, the actual time from the start of execution of the command “Move P1 FC1 CF” in this embodiment to the reset of the force sensor FS is always the same as the corresponding machine language program. The predetermined time is determined in advance by the number of clocks of the unit 32. Therefore, as described above, the reproducibility of the force control is ensured in this embodiment.

（３）他の実施形態
本発明の技術的範囲は上述した実施例に限定されるものではなく、本発明の要旨を逸脱しない限りにおいて変更を加え得ることは勿論である。例えば、上記実施例ではコマンドの引数で力制御モードのオンを指定できたが、コマンドの本体で力制御モードのオンを指定できてもよい。具体的にはコマンドの本体自体が、力制御モードをオンするものと、力制御モードをオンしないものとに分かれているプログラム言語をロボットシステムに実装しても良い。 (3) Other Embodiments The technical scope of the present invention is not limited to the above-described embodiments, and it goes without saying that changes can be made without departing from the gist of the present invention. For example, in the above embodiment, the force control mode can be turned on by the command argument, but the force control mode may be turned on by the command body. Specifically, a program language in which the command body itself is divided into one that turns on the force control mode and one that does not turn on the force control mode may be installed in the robot system.

また力制御対応コマンドにより力覚センサーをリセットしてから力制御によりアームを動作させるまでの時間をタイマーで制御しても良い。すなわち、力制御対応コマンドの実行手順にタイマーのセットを組み込んでも良い。
また力検出器の出力に基づいてマニピュレーターの動作を制御する例としてインピーダンス制御を取り上げたが、運動方程式を用いずに、例えば力検出器の出力に対して線形な制御量でマニピュレーターの動作を制御しても良い。 Further, a time from when the force sensor is reset by a force control corresponding command until the arm is operated by force control may be controlled by a timer. That is, a set of timers may be incorporated in the execution procedure of the force control compatible command.
In addition, although impedance control was taken up as an example of controlling the operation of the manipulator based on the output of the force detector, the operation of the manipulator is controlled by a control amount that is linear with respect to the output of the force detector, for example, without using the equation of motion. You may do it.

１…ロボット、２…エンドエフェクター、３…制御装置、４…教示装置、５…ティーチングペンダント、３１…電源部、３２…制御部、４３…ディスプレイ、Ａ…アーム、Ａ１-Ａ６…アーム部材、Ｅ１-Ｅ６…エンコーダー、ＦＳ…力覚センサー、Ｊ１-Ｊ６…関節、Ｍ１-Ｍ６…モーター DESCRIPTION OF SYMBOLS 1 ... Robot, 2 ... End effector, 3 ... Control apparatus, 4 ... Teaching apparatus, 5 ... Teaching pendant, 31 ... Power supply part, 32 ... Control part, 43 ... Display, A ... Arm, A1-A6 ... Arm member, E1 -E6 ... Encoder, FS ... Force sensor, J1-J6 ... Joint, M1-M6 ... Motor

Claims

A manipulator,
A force detector for detecting a force acting on the manipulator;
In a robot control device for controlling a robot comprising:
The controller that controls the operation of the manipulator controls the operation of the manipulator based on the output of the force detector when executing a command to operate the manipulator, or not based on the output of the force detector. Switch whether to control the operation of the manipulator according to the parameter of the command,
Robot control device.

The controller, when executing the command for controlling the operation of the manipulator based on the output of the force detector, resets the force detector, and after a predetermined time has elapsed since the reset of the force detector, Starting the operation control of the manipulator based on the output of the force detector;
The robot control apparatus according to claim 1.

The control unit, when executing the command for controlling the operation of the manipulator based on the output of the force detector, ends the operation control of the manipulator based on the output of the force detector according to the command.
The robot control apparatus according to claim 1.

A manipulator,
A force detector for detecting a force acting on the manipulator;
In a robot control device for controlling a robot comprising:
A control unit in which commands that can be executed and commands that cannot be executed in a mode for controlling the operation of the manipulator based on the output of the force detector are determined in advance;
Robot control device.

The control unit executes impedance control when controlling the operation of the manipulator based on the output of the force detector.
The robot control device according to claim 4.

A manipulator,
A force detector for detecting a force acting on the manipulator;
In a robot control device for controlling a robot comprising:
A control unit that controls the operation of the manipulator by executing a command in which an argument object is hierarchized;
Robot control device.

A manipulator,
A force detector for detecting a force acting on the manipulator;
In a robot control device for controlling a robot comprising:
A controller that controls the operation of the manipulator by executing a command having an arbitrary coordinate system defined with respect to a reference coordinate system as an argument;
Robot control device.

A manipulator,
A force detector for detecting a force acting on the manipulator;
A robot comprising:
When a command to operate the manipulator is executed, whether to control the operation of the manipulator based on the output of the force detector or to control the operation of the manipulator without being based on the output of the force detector Can be switched according to command parameters,
robot.

A robot comprising a manipulator and a force detector for detecting a force acting on the manipulator;
When the controller that controls the operation of the manipulator executes a command for operating the manipulator, the operation of the manipulator is controlled based on the output of the force detector, or not based on the output of the force detector. A robot control device comprising a control unit that switches whether to control the operation of the manipulator according to the parameter of the command;
A robot system comprising: