WO2013183190A1

WO2013183190A1 - Robot system, robot control device, and robot system control method

Info

Publication number: WO2013183190A1
Application number: PCT/JP2012/083299
Authority: WO
Inventors: 陽介神谷; 安田　賢一; 慎悟安藤
Original assignee: 株式会社安川電機
Priority date: 2012-06-04
Filing date: 2012-12-21
Publication date: 2013-12-12

Abstract

Provided are a robot system (10), a robot control device (14), and a robot system (10) control method capable of automatically generating teaching positions and movement tracks that reduce the energy consumed by a redundant robot. The robot system (10) is provided with a robot (12) having multiple joint axes that include a redundant axis and a robot control device (14). The robot control device (14) comprises: a first command-generating unit (14a) for generating an angular velocity command for a first joint of the robot (12) to reduce movement energy; a first optimized action command-generating unit (14b) that generates a joint angular velocity command (A) for the robot (12) to perform an arm-lowering movement; a zero space matrix-calculating unit (14d) for calculating a zero space matrix of a Jacobi matrix relating to a control point; a second command-generating unit (14e) for generating an angular velocity command for a second joint of the robot (12) on the basis of the joint angular velocity command (A) and the zero space matrix; and a control unit (14f) for controlling the servo motors.

Description

ロボットシステム、ロボット制御装置、及びロボットシステムの制御方法Robot system, robot control apparatus, and robot system control method

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

　特許文献１には、ロボットによる作業を実施する際に、ロボットの動作に必要な電力を低減することができるロボットの制御装置が記載されている。
　このロボットの制御装置は、ロボットを動作させるための動作データを生成する動作指令生成部と、その動作データにより動作に必要なエネルギー量を算出するエネルギー算出手段と、その算出したエネルギー量よりも減少させることが可能となるロボットの位置指令を生成する第１の経路生成手段と、動作指令生成部より出力される動作データに基づいてロボットの位置指令を生成する第２の経路生成手段と、第１の経路生成手段と第２の経路生成手段からの入力を切り替えて制御手段に位置指令を出力する制御方式切替手段とを備えている。 Patent Document 1 describes a robot control device that can reduce the power required for the operation of the robot when performing the work by the robot.
The robot control device includes an operation command generation unit that generates operation data for operating the robot, an energy calculation unit that calculates an energy amount necessary for the operation based on the operation data, and a reduction from the calculated energy amount. A first path generation unit that generates a robot position command that can be generated, a second path generation unit that generates a robot position command based on motion data output from the motion command generation unit, And a control method switching means for switching the input from the first route generation means and the second route generation means and outputting a position command to the control means.

特開２０００－３５４９８８号公報Japanese Patent Laid-Open No. 2000-354988

　ここで、ロボットが冗長ロボット（例えば７軸の産業用ロボット）である場合には、ロボットは手先の運動に対して冗長性を有する。そのため、消費エネルギーを低減する観点から教示位置が最適であるか否かを判断することが困難である。
　本発明は、冗長ロボットの消費エネルギーを低減する教示位置及び動作軌道を自動で生成できるロボットシステム、ロボット制御装置、及びロボットシステムの制御方法を提供することを目的とする。 Here, when the robot is a redundant robot (for example, a 7-axis industrial robot), the robot has redundancy with respect to hand movement. Therefore, it is difficult to determine whether or not the teaching position is optimal from the viewpoint of reducing energy consumption.
An object of the present invention is to provide a robot system, a robot control device, and a robot system control method capable of automatically generating a teaching position and an operation trajectory for reducing energy consumption of a redundant robot.

　上記課題を解決するため、本発明の一の観点によれば、複数のリンクを連結する、冗長軸を含む複数の関節軸を有し、前記複数の関節軸がサーボモータによって駆動されるロボットと、
　前記ロボットを制御するロボット制御装置とを備え、
　前記ロボット制御装置は、入力された前記ロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成する第１の指令生成部と、
　前記ロボットが腕下げ動作をするための関節角速度指令Ａを生成する第１の最適化動作指令生成部と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するゼロ空間行列計算部と、
　前記第１の最適化動作指令生成部が生成した前記関節角速度指令Ａ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成する第２の指令生成部と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記サーボモータを制御する制御部と、を有するロボットシステムが適用される。 In order to solve the above-described problem, according to one aspect of the present invention, there is provided a robot having a plurality of joint axes that connect a plurality of links, including redundant axes, and wherein the plurality of joint axes are driven by a servo motor. ,
A robot control device for controlling the robot,
The robot control device generates a first joint angular velocity command of the robot for reducing kinetic energy based on the input velocity command of the control point of the robot;
A first optimizing operation command generation unit that generates a joint angular velocity command A for the robot to perform the arm lowering operation;
A zero-space matrix calculator for calculating a zero-space matrix of a Jacobian matrix related to the control points;
A second command generation unit that generates a second joint angular velocity command of the robot based on the joint angular velocity command A and the zero-space matrix generated by the first optimization operation command generation unit;
A robot system including a control unit that controls the servo motor based on a third joint angular velocity command that is the sum of the first and second joint angular velocity commands is applied.

　また、他の観点によれば、冗長軸を含む複数の関節軸を有し、前記複数の関節軸がサーボモータによって駆動されるロボットと、
　前記ロボットを制御するロボット制御装置とを備え、
　前記ロボット制御装置は、入力された前記ロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成する第１の指令生成部と、
　前記ロボットの姿勢保持トルクを抑制するための関節角速度指令を生成する最適化動作指令生成部と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するゼロ空間行列計算部と、
　前記最適化動作指令生成部が生成した前記関節角速度指令及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成する第２の指令生成部と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記サーボモータを制御する制御部とを有するロボットシステムが適用される。 According to another aspect, the robot has a plurality of joint axes including a redundant axis, and the plurality of joint axes are driven by a servo motor;
A robot control device for controlling the robot,
The robot control device generates a first joint angular velocity command of the robot for reducing kinetic energy based on the input velocity command of the control point of the robot;
An optimization operation command generation unit for generating a joint angular velocity command for suppressing the posture holding torque of the robot;
A zero-space matrix calculator for calculating a zero-space matrix of a Jacobian matrix related to the control points;
A second command generation unit that generates a second joint angular velocity command of the robot based on the joint angular velocity command generated by the optimization operation command generation unit and the zero space matrix;
A robot system having a control unit that controls the servo motor based on a third joint angular velocity command that is the sum of the first and second joint angular velocity commands is applied.

　また、他の観点によれば、冗長軸を含む複数の関節軸を有するロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成する第１の指令生成部と、
　前記ロボットが腕下げ動作をするための関節角速度指令Ａを生成する第１の最適化動作指令生成部と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するゼロ空間行列計算部と、
　前記関節角速度指令Ａ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成する第２の指令生成部と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、サーボモータを制御する制御部とを有するロボット制御装置が適用される。 According to another aspect, the robot generates a first joint angular velocity command for reducing the kinetic energy based on a velocity command of a control point of a robot having a plurality of joint axes including redundant axes. 1 command generator,
A first optimizing operation command generation unit that generates a joint angular velocity command A for the robot to perform the arm lowering operation;
A zero-space matrix calculator for calculating a zero-space matrix of a Jacobian matrix related to the control points;
A second command generation unit that generates a second joint angular velocity command of the robot based on the joint angular velocity command A and the zero space matrix;
A robot control device having a control unit for controlling a servo motor based on a third joint angular velocity command that is the sum of the first and second joint angular velocity commands is applied.

　また、他の観点によれば、冗長軸を含む複数の関節軸を有するロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成するステップＳＡ１と、
　前記ロボットが、前記冗長軸の張り出しを、前記制御点の位置姿勢を維持したまま、冗長自由度を利用して抑える腕下げ動作を実行するための関節角速度指令Ａを生成するステップＳＡ２と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するステップＳＡ３と、
　前記関節角速度指令Ａ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成するステップＳＡ４と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記各関節軸を駆動するサーボモータを制御し、前記ロボットが前記腕下げ動作を実行するステップＳＡ５と、を実行するロボットシステムの制御方法が適用される。 According to another aspect, the step of generating a first joint angular velocity command for the robot for reducing kinetic energy based on a velocity command for a control point of a robot having a plurality of joint axes including redundant axes. SA1 and
Step SA2 for generating a joint angular velocity command A for performing an arm lowering operation in which the robot suppresses the extension of the redundant axis by using the redundancy degree of freedom while maintaining the position and orientation of the control point;
Calculating SA3 of the Jacobian matrix for the control points;
Generating a second joint angular velocity command for the robot based on the joint angular velocity command A and the zero-space matrix; and
Step SA5 in which a servo motor for driving each joint axis is controlled based on a third joint angular velocity command that is the sum of the first and second joint angular velocity commands, and the robot performs the arm lowering operation; The control method of the robot system that executes is applied.

　本発明によれば、冗長ロボットの消費エネルギーを低減する教示位置及び動作軌道を自動で生成できる。 According to the present invention, it is possible to automatically generate the teaching position and the motion trajectory that reduce the energy consumption of the redundant robot.

本発明の一実施例に係るロボットシステムの構成図である。It is a block diagram of the robot system which concerns on one Example of this invention. 同ロボットシステムが有するロボットの外観図である。It is an external view of the robot which the robot system has. 同ロボットシステムが有するロボットが腰部を曲げた状態を示す外観図である。It is an external view which shows the state which the robot which the robot system has bent the waist | hip | lumbar part. 同ロボットシステムのロボット制御装置の機能ブロック図である。It is a functional block diagram of the robot control apparatus of the robot system. 同ロボットシステムの動作を示すフローチャートである。It is a flowchart which shows operation | movement of the robot system. 同ロボットシステムのロボットの腕下げ動作の説明図（１）である。It is explanatory drawing (1) of the arm lowering operation | movement of the robot of the robot system. 同ロボットシステムのロボットの腕下げ動作の説明図（２）である。It is explanatory drawing (2) of the arm lowering operation | movement of the robot of the robot system. 同ロボットシステムが有するロボットの操作モードＡによる動作を示す説明図である。It is explanatory drawing which shows the operation | movement by the operation mode A of the robot which the robot system has. 同ロボットシステムの有するロボットの操作モードＢによる動作を示す説明図である。It is explanatory drawing which shows the operation | movement by the operation mode B of the robot which the robot system has.

　続いて、添付した図面を参照しつつ、本発明を具体化した実施例につき説明し、本発明の理解に供する。なお、各図において、説明に関連しない部分は図示を省略する場合がある。 Next, embodiments of the present invention will be described with reference to the accompanying drawings to provide an understanding of the present invention. In each drawing, portions not related to the description may be omitted.

　本発明の一実施例に係るロボットシステム１０は、図１に示すように、ロボット１２と、このロボット１２の動作を制御するロボット制御装置１４と、ロボット１２の位置姿勢を教示するための教示ペンダント１６と、を備えている。 As shown in FIG. 1, a robot system 10 according to an embodiment of the present invention includes a robot 12, a robot controller 14 that controls the operation of the robot 12, and a teaching pendant for teaching the position and orientation of the robot 12. 16.

　なお、図１に示すロボット座標系Ｃｒは、例えば、ロボット１２の移動台車１２ａ上にある点を原点とし、ロボット１２の前後方向に延びるＸ軸、左右方向に延びるＹ軸、及び上下方向に延びるＺ軸からなる直交座標系である。 The robot coordinate system Cr shown in FIG. 1 has, for example, a point on the moving carriage 12a of the robot 12 as an origin, the X axis extending in the front-rear direction of the robot 12, the Y axis extending in the left-right direction, and the vertical direction. It is an orthogonal coordinate system consisting of the Z axis.

　ロボット１２は、図１、図２Ａ及び図２Ｂに示すように、移動台車１２ａと、腰部１２ｂと、胴体１２ｃと、左アームＬＡと、右アームＲＡと、を備えている。
　移動台車１２ａは、例えばオムニホイールを用いて床面上を走行できる。移動台車１２ａは、Ｘ軸方向に並進運動するための走行用軸Ｖｘ、Ｙ軸方向に並進運動するための走行用軸Ｖｙ、及びＺ軸回りに回転運動するための走行用軸Ｖｚを有している。 As shown in FIGS. 1, 2A, and 2B, the robot 12 includes a moving carriage 12a, a waist 12b, a body 12c, a left arm LA, and a right arm RA.
The movable carriage 12a can travel on the floor surface using, for example, an omni wheel. The movable carriage 12a has a travel axis Vx for translational movement in the X-axis direction, a travel axis Vy for translational movement in the Y-axis direction, and a travel axis Vz for rotational movement about the Z-axis. ing.

　腰部１２ｂは、互いに間隔を空けて配置され、それぞれ床面に沿う方向に延びる軸回りに回転可能な腰関節軸Ｌ１及び腰関節軸Ｌ２を有している。また、腰部１２ｂは、胴体１２ｃの長手方向に延びる軸回りに回転可能な腰関節軸Ｌ３を有している。従ってロボット１２は、図２Ｂに示すように、腰関節軸Ｌ１～Ｌ３を駆動して、腰部１２ｂを曲げたり、ひねったりすることができる。 The waist part 12b has a waist joint axis L1 and a waist joint axis L2 which are arranged with a space between each other and are rotatable about axes extending in the direction along the floor surface. The waist 12b has a waist joint axis L3 that can rotate around an axis extending in the longitudinal direction of the body 12c. Therefore, as shown in FIG. 2B, the robot 12 can drive the waist joint axes L1 to L3 to bend or twist the waist 12b.

　胴体１２ｃは、腰部１２ｂの上側に設けられている。胴体１２ｃの上部には、図２Ａに示すように、頭部１２ｄが設けられている。
　左アームＬＡは、胴体１２ｃの左側部に設けられ、複数のリンクＬＮＫが関節軸ＬＪ１～ＬＪ７を介して連結されている。左アームＬＡの先端部には、左ハンドＬＨが設けられている。
　右アームＲＡは、胴体１２ｃの右側部に設けられ、複数のリンクＬＮＫが関節軸ＲＪ１～ＲＪ７を介して連結されている。右アームＲＡの先端部には、右ハンドＲＨが設けられている。 The trunk 12c is provided on the upper side of the waist 12b. As shown in FIG. 2A, a head portion 12d is provided on the upper portion of the body 12c.
The left arm LA is provided on the left side of the body 12c, and a plurality of links LNK are connected via joint axes LJ1 to LJ7. A left hand LH is provided at the tip of the left arm LA.
The right arm RA is provided on the right side of the trunk 12c, and a plurality of links LNK are connected via joint axes RJ1 to RJ7. A right hand RH is provided at the tip of the right arm RA.

　各走行用軸Ｖｘ、Ｖｙ、Ｖｚ、腰関節軸Ｌ１～Ｌ３、及び各関節軸ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７は、それぞれ、エンコーダが内蔵されたサーボモータ（不図示）により駆動される。
　なお、以下、走行用軸Ｖｘ、Ｖｙ、Ｖｚ及び腰関節軸Ｌ１～Ｌ３については、それぞれ単に関節軸Ｖｘ、Ｖｙ、Ｖｚ及び関節軸Ｌ１～Ｌ３と呼ぶ。 Each of the travel axes Vx, Vy, Vz, the waist joint axes L1 to L3, and each of the joint axes RJ1 to RJ7, LJ1 to LJ7 is driven by a servo motor (not shown) having an encoder built therein.
Hereinafter, the traveling axes Vx, Vy, Vz and the waist joint axes L1 to L3 are simply referred to as joint axes Vx, Vy, Vz and joint axes L1 to L3, respectively.

　ここで、ロボット１２は、ロボット１２の作業空間の自由度に対して、関節軸を冗長に有するロボットであれば任意の構成を取りうる。冗長ロボットとは、制御点（例えば、ロボット１２の手先に設定する。）の運動の自由度に対して、関節の運動の自由度が冗長性を有するロボットを指す。例えば、ロボット座標系ＣｒのＹ軸回りにそれぞれ回転する３つの関節軸のみを有する３軸のロボットは、ＸＺ平面に対しては冗長性を有している。従って、この３軸のロボットは、ＸＺ平面上の動作に関しては、関節軸を冗長に有するロボットである。つまり、ロボットは、関節の軸数や軸構成に関係なく、要求される動作に対して冗長自由度を有する冗長ロボットであればよい。
　なお制御点とは、ロボットの制御対象となる箇所に設定された座標系の原点を指し、一般に手先に設定されることが多い。また制御点は位置だけでなく姿勢も含む意味で用いられることもあり、本実施例でもロボット座標系Ｃｒを基準とした手先の座標系の位置、姿勢を「制御点の位置姿勢」と表記する。 Here, the robot 12 can take any configuration as long as it has redundant joint axes with respect to the degree of freedom of the work space of the robot 12. A redundant robot refers to a robot that has redundancy in the degree of freedom of movement of the joints relative to the degree of freedom of movement of the control point (for example, set at the hand of the robot 12). For example, a three-axis robot having only three joint axes that rotate about the Y axis of the robot coordinate system Cr has redundancy with respect to the XZ plane. Therefore, this three-axis robot is a robot having redundant joint axes with respect to the movement on the XZ plane. That is, the robot may be a redundant robot having a redundancy degree of freedom for a required operation regardless of the number of axes of the joints and the axis configuration.
The control point refers to the origin of the coordinate system set at a location to be controlled by the robot, and is generally set at the hand. In addition, the control point may be used to include not only the position but also the posture. In this embodiment, the position and posture of the hand coordinate system based on the robot coordinate system Cr are expressed as “control point position and posture”. .

　ロボット制御装置１４は、ロボット１２にケーブルを介して接続されている。ロボット制御装置１４は、ロボット１２に搭載されていてもよい。ロボット制御装置１４は、図３に示すように、第１の指令生成部１４ａと、第１の最適化動作指令生成部１４ｂと、第２の最適化動作指令生成部１４ｃと、ゼロ空間行列計算部１４ｄと、第２の指令生成部１４ｅと、位置速度制御部（制御部の一例）１４ｆと、を有している。ロボット制御装置１４は、図示しないＣＰＵを搭載しており、図３に示す各ブロックの機能は、ＣＰＵにより実行されるソフトウェアプログラムやハードウェアによって実現される。 The robot control device 14 is connected to the robot 12 via a cable. The robot control device 14 may be mounted on the robot 12. As shown in FIG. 3, the robot controller 14 includes a first command generation unit 14a, a first optimization operation command generation unit 14b, a second optimization operation command generation unit 14c, and a zero space matrix calculation. 14d, 2nd command generation part 14e, and position speed control part (an example of a control part) 14f. The robot control device 14 is equipped with a CPU (not shown), and the function of each block shown in FIG. 3 is realized by a software program or hardware executed by the CPU.

　第１の指令生成部１４ａには、ロボット１２の手先（制御点の一例）の速度指令（ベクトル）Ｖが入力される。第１の指令生成部１４ａは、入力された手先の速度指令Ｖに基づいて手先を移動させる関節角速度指令（第１の関節角速度指令）ω_ＴＡＳＫを生成できる。この関節角速度指令ω_ＴＡＳＫは、ロボット１２の運動を効率化し、各関節の運動に必要な運動エネルギーを低減するための指令である。
　以下、ロボット１２の手先の速度指令Ｖを、単に「手先速度指令Ｖ」という。 A speed command (vector) V of the hand (an example of a control point) of the robot 12 is input to the first command generation unit 14a. The first command generation unit 14a can generate a joint angular velocity command (first joint angular velocity command) ω _TASK for moving the hand based on the input hand speed command V. This joint angular velocity command ω _TASK is a command for improving the efficiency of the movement of the robot 12 and reducing the kinetic energy necessary for the movement of each joint.
Hereinafter, the speed command V of the hand of the robot 12 is simply referred to as “hand speed command V”.

　第１の最適化動作指令生成部１４ｂは、ロボット１２が腕下げ動作をするための関節角速度指令ω_ＥＬＢ（関節角速度指令Ａ）を出力できる。ここで、腕下げ動作とは、ロボット１２の各関節の運動により生ずるアームの肘（冗長軸）の張り出しを、制御点の位置姿勢を維持したまま、冗長自由度を利用して内側に抑える動作をいう。関節角速度指令ω_ＥＬＢの演算方法については後述する。 The first optimization operation command generation unit 14b can output a joint angular velocity command ω _ELB (joint angular velocity command A) for the robot 12 to perform the arm lowering operation. Here, the arm lowering operation is an operation for suppressing the extension of the arm elbow (redundant axis) caused by the movement of each joint of the robot 12 to the inside using the redundancy degree of freedom while maintaining the position and orientation of the control point. Say. The calculation method of the joint angular velocity command ω _ELB will be described later.

　第２の最適化動作指令生成部１４ｃは、ロボット１２の姿勢保持に必要なトルク（姿勢保持トルク）を低減する観点に基づいて、ロボット１２の運動を効率化するための関節角速度指令ω_ＧＲＶ（関節角速度指令Ｂ）を出力できる。関節角速度指令ω_ＧＲＶの演算方法については後述する。
　ロボット制御装置１４は、手先速度指令Ｖを実現する関節角速度指令ω_ＴＡＳＫに連動して、自動で冗長軸の関節角速度指令ω_ＥＬＢ又は関節角速度指令ω_ＧＲＶを生成できる。 The second optimization operation command generation unit 14c is configured to reduce the torque necessary for holding the posture of the robot 12 (posture holding torque), and the joint angular velocity command ω _GRV (for improving the motion of the robot 12). The joint angular velocity command B) can be output. The calculation method of the joint angular velocity command ω _GRV will be described later.
The robot controller 14 can automatically generate a redundant axis joint angular velocity command ω _ELB or a joint angular velocity command ω _GRV in conjunction with the joint angular velocity command ω _TASK that realizes the hand velocity command V.

　ゼロ空間行列計算部１４ｄは、ロボット１２の関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の関節角度θ_ＣＵＲ（ベクトル）に基づいて、ロボット１２の手先に関するヤコビ行列Ｊのゼロ空間行列Ｎを計算できる。詳細には、ゼロ空間行列計算部１４ｄは、関節角度θ_ＣＵＲからロボット１２の手先の位置姿勢及び各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７のそれぞれの方向を求め、ロボット１２の手先に関するヤコビ行列Ｊのゼロ空間行列Ｎを計算できる。 The zero-space matrix calculation unit 14d calculates the Jacobian matrix J related to the hand of the robot 12 based on the joint angles θ _CUR (vector) of the joint axes Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7 of the robot 12. Can be calculated. Specifically, the zero space matrix calculation unit 14d calculates the position and orientation of the hand of the robot 12 from the joint angle θ _CUR and the directions of the joint axes Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7. The zero space matrix N of the Jacobian matrix J related to the hand of the robot 12 can be calculated.

　ここで、関節角度θ_ＣＵＲは、ロボット１２を駆動するサーボモータのエンコーダから計算される関節角度の現在値θｆｂ、又はソフトウェア上で計算される関節角度の指令値θｒｅｆのいずれかの値が用いられる。関節角度の現在値θｆｂ及び関節角度の指令値θｒｅｆは、１制御周期前（１サンプリング周期前）の値である。なお、ロボット１２の関節角度の現在値θｆｂは、各サーボモータのエンコーダからフィードバックされたエンコーダ値に減速比を乗算した結果に基づいて求められる。 Here, as the joint angle θ _CUR , either a current value θfb of the joint angle calculated from the encoder of the servo motor that drives the robot 12 or a command value θref of the joint angle calculated on software is used. . The current value θfb of the joint angle and the command value θref of the joint angle are values before one control period (one sampling period). The current value θfb of the joint angle of the robot 12 is obtained based on the result of multiplying the encoder value fed back from the encoder of each servo motor by the reduction ratio.

　ゼロ空間行列Ｎとは、冗長ロボットにおいて、手先の位置姿勢を維持したまま、冗長自由度の動作指令を生成するための演算行列である。この動作指令は、冗長軸という特定の軸のみに対する指令ではなく、冗長自由度（手先の位置姿勢に影響の無い関節自由度）に対する指令であって、制御対象軸は、冗長軸を含む全ての軸又は一部の軸であり、制御周期毎のロボットの姿勢、動作方向に応じて異なる。 The zero-space matrix N is an operation matrix for generating an operation command with a redundancy degree of freedom while maintaining the position and orientation of the hand in the redundant robot. This motion command is not a command for a specific axis as a redundant axis, but a command for a redundancy degree of freedom (joint degree of freedom that does not affect the position and orientation of the hand), and the control target axis includes all redundant axes. This is an axis or a part of the axis, and differs depending on the posture and operation direction of the robot for each control cycle.

　ここで、この制御対象軸について、ロボット座標系ＣｒのＹ軸回りにそれぞれ回転する３つの関節軸（第１～第３の関節軸）のみを有し、ＸＺ平面上の位置を任意に移動する３軸ロボットを例に説明する。この場合の冗長自由度としての制御対象軸は、３軸ロボットの第１～第３の関節軸のうちの１つの関節軸に限られものではなく、手先位置を維持したまま、ロボット座標系ＣｒのＹ軸回りに回転できる（姿勢を変化させる事ができる）第１～第３の関節軸のうちの少なくとも１つの関節軸である。いずれの関節軸を制御対象軸とするかはゼロ空間行列を用いて計算される。例えば、第３の関節軸が動作リミットに到達し、Ｚ軸方向又はＸ軸方向へ手先を移動させる動作に寄与できない場合には、冗長自由度としての制御対象軸は第１の関節軸及び第２の関節軸となる。 Here, this control target axis has only three joint axes (first to third joint axes) that rotate about the Y axis of the robot coordinate system Cr, and arbitrarily moves the position on the XZ plane. A three-axis robot will be described as an example. In this case, the control target axis as the redundancy degree of freedom is not limited to one of the first to third joint axes of the three-axis robot, and the robot coordinate system Cr is maintained while maintaining the hand position. At least one of the first to third joint axes that can be rotated about the Y axis (the posture can be changed). Which joint axis is the control target axis is calculated using a zero-space matrix. For example, when the third joint axis reaches the motion limit and cannot contribute to the movement of moving the hand in the Z-axis direction or the X-axis direction, the control target axis as the redundancy degree of freedom is the first joint axis and the first joint axis. 2 joint axes.

　ロボット１２の手先以外の箇所を制御対象箇所（制御点）とする場合には、その制御対象箇所（制御点）に対応した別のヤコビ行列が用いられる。 When a part other than the hand of the robot 12 is set as a control target part (control point), another Jacobian matrix corresponding to the control target part (control point) is used.

　第２の指令生成部１４ｅは、関節角速度指令ω_ＥＬＢ、関節角速度指令ω_ＧＲＶ、及びゼロ空間行列Ｎに基づいて、関節角速度指令（第２の関節角速度指令）ωｒｅｆ２を生成できる。生成された関節角速度指令ωｒｅｆ２は、第１の指令生成部１４ａが生成した関節角速度指令ω_ＴＡＳＫに加算され、関節角速度指令（第３の関節角速度指令）ωｒｅｆ３となる（図３参照）。関節角度の指令値θｒｅｆは、電源投入時のロボット１２の関節角度の現在値θｆｂ（エンコーダ値に減速比を乗算した値）に、関節角速度指令ωｒｅｆ３を加算して求められ、制御周期毎に積算されて、更新される。 The second command generator 14e can generate a joint angular velocity command (second joint angular velocity command) ωref2 based on the joint angular velocity command ω _ELB , the joint angular velocity command ω _GRV , and the zero space matrix N. The generated joint angular velocity command ωref2 is added to the joint angular velocity command ω _TASK generated by the first command generation unit 14a to become a joint angular velocity command (third joint angular velocity command) ωref3 (see FIG. 3). The joint angle command value θref is obtained by adding the joint angular velocity command ωref3 to the current joint angle value θfb of the robot 12 when the power is turned on (a value obtained by multiplying the encoder value by the reduction ratio), and is integrated every control cycle. And updated.

　位置速度制御部１４ｆは、関節角速度指令ωｒｅｆ３に基づいて、サーボモータを制御できる。詳細には、位置速度制御部１４ｆは、関節角速度指令ωｒｅｆ３の積分値及び関節角度θ_ＣＵＲの和である関節角度の指令値θｒｅｆと、関節角度の現在値θｆｂと、に基づいて、サーボモータをフィードバック制御できる。すなわち、位置速度制御部１４ｆは、各サーボモータを制御し、ロボット１２の各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の位置速度を制御できる。
　位置速度制御部１４ｆへの指令として、関節角速度指令ωｒｅｆ３を使用し、関節角速度指令ωｒｅｆ３と、各サーボモータのエンコーダからフィードバックされたエンコーダ値に減速比を乗算した結果に基づいて求められた関節角速度の現在値と、に基づいて、フィードバック制御してもよい。 The position speed control unit 14f can control the servo motor based on the joint angular speed command ωref3. Specifically, the position / speed control unit 14f controls the servo motor based on the joint angle command value θref which is the sum of the integral value of the joint angular velocity command ωref3 and the joint angle θ _CUR and the current value θfb of the joint angle. Feedback control is possible. That is, the position / velocity control unit 14f can control each servo motor to control the position / speed of each joint axis Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7 of the robot 12.
The joint angular velocity command ωref3 is used as a command to the position / speed control unit 14f, and the joint angular velocity command ωref3 is obtained based on the result of multiplying the encoder value fed back from the encoder of each servo motor by the reduction ratio. The feedback control may be performed based on the current value.

　位置速度制御部１４ｆは、ロボット１２が有する関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の数に対応する分だけ設けられている。 The position / speed control unit 14f is provided in an amount corresponding to the number of joint axes Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7 of the robot 12.

　図１に示す教示ペンダント１６は、ロボット制御装置１４に接続されている。操作者は、教示ペンダント１６を用いて、ロボット１２の制御対象箇所（制御点）を指定の座標系に基づいて動作させることができる。 1 is connected to the robot controller 14. The operator can use the teaching pendant 16 to move a control target portion (control point) of the robot 12 based on a designated coordinate system.

　ここで、制御対象箇所（制御点）は、例えば、左ハンドＬＨ上の特定部位や右ハンドＲＨ上の特定部位である。制御対象箇所の他の例として、ロボット１２の関節の特定部位や手先に取り付けたエンドエフェクタの先端等が挙げられる。また、制御対象箇所を左アームＬＡ、右アームＲＡ、又は両アームＬＡ、ＲＡとして切り替えることができる。 Here, the control target part (control point) is, for example, a specific part on the left hand LH or a specific part on the right hand RH. Other examples of the control target location include a specific part of the joint of the robot 12 and the tip of an end effector attached to the hand. Moreover, a control object location can be switched as left arm LA, right arm RA, or both arms LA and RA.

　また、指定の座標系とは、例えば、ロボット座標系Ｃｒである。指定の座標系としてロボット座標系Ｃｒが選択されている場合には、ロボット１２の手先をＸ軸方向、Ｙ軸方向及びＺ軸方向に移動させたり、Ｘ軸回りの回転方向、Ｙ軸回りの回転方向及びＺ軸回りの回転方向に姿勢を変化させたりすることができる。指定の座標系の他の例として、関節座標系やロボット１２の操作者が任意に設定した座標系等が挙げられる。 Also, the designated coordinate system is, for example, a robot coordinate system Cr. When the robot coordinate system Cr is selected as the designated coordinate system, the hand of the robot 12 is moved in the X-axis direction, the Y-axis direction, and the Z-axis direction, the rotation direction around the X axis, and the rotation around the Y axis. The posture can be changed in the rotation direction and the rotation direction around the Z axis. Other examples of the designated coordinate system include a joint coordinate system and a coordinate system arbitrarily set by the operator of the robot 12.

　教示ペンダント１６には、図示しないジョグ操作ボタン、操作モード切り替えボタンＡ、操作モード切り替えボタンＢ、及び再生ボタンが設けられている。 The teaching pendant 16 is provided with a jog operation button, an operation mode switching button A, an operation mode switching button B, and a playback button (not shown).

　ジョグ操作ボタンは、操作者がロボット１２をジョグ操作するためのボタンである。ジョグ操作ボタンが押されると、そのジョグ操作ボタンについて予め設定された手先速度指令Ｖが第１の指令生成部１４ａに入力される。 The jog operation button is a button for the operator to jog the robot 12. When the jog operation button is pressed, a hand speed command V preset for the jog operation button is input to the first command generation unit 14a.

　ジョグ操作ボタンは、詳細には、Ｘ＋ボタン、Ｘ－ボタン、Ｙ＋ボタン、Ｙ－ボタン、Ｚ＋ボタン、Ｚ－ボタン、Ｒｘ＋ボタン、Ｒｘ－ボタン、Ｒｙ＋ボタン、Ｒｙ－ボタン、Ｒｚ＋ボタン、Ｒｚ－ボタンである。例えば、Ｘ＋ボタンが押下されると、予め設定された速度で指定の座標系のＸ軸の正方向に手先が移動するように、手先速度指令Ｖが第１の指令生成部１４ａに入力される。また、Ｒｘ＋ボタンが押下されると、予め設定された速度で指定の座標系のＸ軸回りの正方向に手先の姿勢が変化するように、手先速度指令Ｖが第１の指令生成部１４ａに入力される。 Jog operation buttons are, in detail, X + button, X- button, Y + button, Y- button, Z + button, Z- button, Rx + button, Rx- button, Ry + button, Ry- button, Rz + button, Rz- button It is. For example, when the X + button is pressed, the hand speed command V is input to the first command generator 14a so that the hand moves in the positive direction of the X axis of the designated coordinate system at a preset speed. . When the Rx + button is pressed, the hand speed command V is sent to the first command generator 14a so that the hand posture changes in the positive direction around the X axis of the designated coordinate system at a preset speed. Entered.

　操作モード切り替えボタンＡ、Ｂは、操作者が、ロボット１２の操作モードを切り替えるためのボタンである。操作モード切り替えボタンＡが押下されると、腕下げ操作モード（以下、操作モードＡという場合がある。）に切り替わる。操作モード切り替えボタンＢが押下されると、姿勢保持トルクを低減する操作モード（以下、操作モードＢという場合がある。）に切り替わる。 Operation mode switching buttons A and B are buttons for the operator to switch the operation mode of the robot 12. When the operation mode switching button A is pressed, the arm switching operation mode (hereinafter sometimes referred to as operation mode A) is switched. When the operation mode switching button B is pressed, the operation mode is switched to an operation mode (hereinafter sometimes referred to as operation mode B) for reducing the posture holding torque.

　操作者が操作モード切り替えボタンＡ、Ｂを押下すると、それぞれ、図３に示す信号Ａが第１の最適化動作指令生成部１４ｂに入力され、信号Ｂが第２の最適化動作指令生成部１４ｃに入力される。信号Ａが第１の最適化動作指令生成部１４ｂに入力されると、第１の最適化動作指令生成部１４ｂが動作する。信号Ｂが第２の最適化動作指令生成部１４ｃに入力されると、第２の最適化動作指令生成部１４ｃが動作する。 When the operator presses the operation mode switching buttons A and B, the signal A shown in FIG. 3 is input to the first optimization operation command generation unit 14b, and the signal B is input to the second optimization operation command generation unit 14c. Is input. When the signal A is input to the first optimization operation command generation unit 14b, the first optimization operation command generation unit 14b operates. When the signal B is input to the second optimization operation command generation unit 14c, the second optimization operation command generation unit 14c operates.

　再生ボタンは、教示された作業プログラムの再生動作を行うためのボタンである。 The playback button is a button for performing the playback operation of the taught work program.

　ロボット１２の操作者は、ジョグ操作ボタンを用いて、ロボット１２をジョグ操作し、ロボット１２の作業プログラムの教示を行うことができる。
　また、操作者は、再生ボタンを用いて作業プログラムの再生動作を行うことができる。 An operator of the robot 12 can jog the robot 12 using a jog operation button to teach a work program for the robot 12.
Further, the operator can perform a reproduction operation of the work program using the reproduction button.

　次に、図４を参照して、ロボットシステム１０の動作について説明する。特に、操作者がジョグ操作ボタンを押して、図６Ａの上図で示すように、ロボット１２にテーブル４０に載った物品ＯＢＪを両アームＬＡ、ＲＡのハンドＬＨとハンドＲＨにて挟んで把持させる作業プログラムを教示する際の動作について説明する。 Next, the operation of the robot system 10 will be described with reference to FIG. In particular, the operator presses the jog operation button, and as shown in the upper diagram of FIG. 6A, the robot 12 grips the article OBJ placed on the table 40 between the hands LH and RH of both arms LA and RA. The operation when teaching the program will be described.

　以下のステップＳＡ１～ＳＡ５においては、ロボット１２がテーブル４０に載った物品ＯＢＪを把持する際に、腕下げ動作をするように、操作モードＡにてロボット１２の動作が制御される。 In the following steps SA1 to SA5, the operation of the robot 12 is controlled in the operation mode A so that the arm 12 performs the arm lowering operation when the robot 12 grips the article OBJ placed on the table 40.

（ステップＳＡ１）
　操作者が操作モード切り替えボタンＡを押して、操作モードを操作モードＡ（腕下げモード）とする。この操作により、図３に示す信号Ａが第１の最適化動作指令生成部１４ｂに入力される。 (Step SA1)
The operator presses the operation mode switching button A to set the operation mode to the operation mode A (arm lowering mode). By this operation, the signal A shown in FIG. 3 is input to the first optimization operation command generation unit 14b.

　操作者がジョグ操作ボタン（例えば、Ｘ＋ボタン）を押して、ロボット１２の手先を物品ＯＢＪを把持するための位置姿勢に移動させる。すなわち、操作者は、ロボット１２をジョグ操作する。
　ジョグ操作により、図３に示す第１の指令生成部１４ａに、手先速度指令Ｖが入力される。 The operator presses a jog operation button (for example, X + button) to move the hand of the robot 12 to a position and posture for gripping the article OBJ. That is, the operator jogs the robot 12.
As a result of the jog operation, the hand speed command V is input to the first command generator 14a shown in FIG.

　なお、ジョグ操作の代わりに、ロボット１２がセンサ（不図示）から得られた情報に基づいて自律的に移動してもよい。例えば、ロボット１２に取り付けられたビジョンセンサを用いて物品ＯＢＪを認識し、ロボット１２が現在の位置から物品ＯＢＪに向かって手先を自律的に移動させる動作であってもよい。この場合、第１の指令生成部１４ａに入力される手先速度指令Ｖは、例えば次の手順によって求められる。まず、現在のロボット１２の制御点と認識結果としての物品ＯＢＪの位置との間の距離を、予め設定された動作時間で割ることによって速度指令を計算する。次に、計算された速度指令をフィルタリングすることによって、所定の加減速が付与された手先速度指令Ｖとする。フィルタリングには、例えば、移動平均フィルタやローパスフィルタを用いる。フィルタリングによって、ロボットの急激な加減速がソフトウェア上で回避される。 Note that instead of the jog operation, the robot 12 may move autonomously based on information obtained from a sensor (not shown). For example, it may be an operation of recognizing the article OBJ using a vision sensor attached to the robot 12 and autonomously moving the hand from the current position toward the article OBJ. In this case, the hand speed command V input to the first command generation unit 14a is obtained, for example, by the following procedure. First, a speed command is calculated by dividing the distance between the current control point of the robot 12 and the position of the article OBJ as a recognition result by a preset operation time. Next, the calculated speed command is filtered to obtain a hand speed command V to which a predetermined acceleration / deceleration is applied. For the filtering, for example, a moving average filter or a low-pass filter is used. By filtering, rapid acceleration / deceleration of the robot is avoided on the software.

　ロボット制御装置１４の第１の指令生成部１４ａ、第１の最適化動作指令生成部１４ｂ、ゼロ空間行列計算部１４ｄ、第２の指令生成部１４ｅ、及び位置速度制御部１４ｆは、以下のように動作する。
　第１の指令生成部１４ａは、式（２）、（３）を用いて、運動エネルギーを抑制するための関節角速度指令ω_ＴＡＳＫを生成する。すなわち、重み行列Ｗを用いて重み付きヤコビ逆行列が求められ、求められた重み付きヤコビ逆行列に制御点の速度指令を乗じて関節角速度指令ω_ＴＡＳＫが求められる。 The first command generation unit 14a, the first optimization operation command generation unit 14b, the zero space matrix calculation unit 14d, the second command generation unit 14e, and the position / speed control unit 14f of the robot control device 14 are as follows. To work.
The first command generation unit 14a generates a joint angular velocity command ω _TASK for suppressing kinetic energy using the equations (2) and (3). That is, the weighted Jacobian inverse matrix is obtained using the weight matrix W, and the joint angular velocity command ω _TASK is obtained by multiplying the obtained weighted Jacobian inverse matrix by the speed command of the control point.

　また、Ｗは重み行列である。重み行列Ｗは、例えば、１）動力学計算により得られた慣性行列（重み行列Ｗ１）、２）リンクＬＮＫ（図１参照）等の駆動対象の負荷重量及び負荷イナーシャに基づいて設定した定数行列（重み行列Ｗ２）、又は３）各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の最大トルクや最大速度に基づいて設定した定数行列（重み行列Ｗ３）等である。 W is a weight matrix. The weight matrix W is, for example, 1) an inertia matrix (weight matrix W1) obtained by dynamic calculation, 2) a constant matrix set based on the load weight and load inertia of the drive target such as the link LNK (see FIG. 1). (Weight matrix W2) or 3) Constant matrix (weight matrix W3) set based on the maximum torque and maximum speed of each joint axis Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, LJ1 to LJ7 .

　重み行列Ｗ１は、関節座標系の慣性行列であり、例えば、ロボット制御基礎論（吉川恒夫著、コロナ社、１９８８）ｐ．８６－９７を参照して計算できる。
　重み行列Ｗ２、Ｗ３は、それぞれ以下のように設定される。ただし、重み行列Ｗ２、Ｗ３は、それぞれ以下の設定例に限定されるものではない。
　重み行列Ｗ２は、例えばロボット１２の関節軸Ｖｘ、Ｖｙ、Ｖｚの駆動対象の重量がそれぞれ１０ｋｇであり、関節軸Ｌ１～Ｌ３の駆動対象の重量がそれぞれ５ｋｇであり、各関節軸ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７が駆動するリンクＬＮＫの重量がそれぞれ１ｋｇである場合、対角項は、各リンクの重量に正の定数Ｋ２を乗じた値となる対角行列に設定される。
　すなわち、重み行列Ｗ２は、例えば以下のように設定される。 The weight matrix W1 is an inertia matrix of the joint coordinate system. For example, the robot control basic theory (written by Tsuneo Yoshikawa, Corona, 1988) p. 86-97.
The weight matrices W2 and W3 are set as follows, respectively. However, the weight matrices W2 and W3 are not limited to the following setting examples.
In the weight matrix W2, for example, the weight of the driving target of the joint axes Vx, Vy, and Vz of the robot 12 is 10 kg, the weight of the driving target of the joint axes L1 to L3 is 5 kg, and the joint axes RJ1 to RJ7, When the weight of each link LNK driven by LJ1 to LJ7 is 1 kg, the diagonal term is set to a diagonal matrix that is a value obtained by multiplying the weight of each link by a positive constant K2.
That is, the weight matrix W2 is set as follows, for example.

Ｗ２　＝　ｄｉａｇ［１０×Ｋ２，１０×Ｋ２，１０×Ｋ２，５×Ｋ２，５×Ｋ２，
　５×Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，Ｋ２，
　Ｋ２，Ｋ２，Ｋ２］ W2 = diag [10 × K2, 10 × K2, 10 × K2, 5 × K2, 5 × K2,
5 × K2, K2, K2, K2, K2, K2, K2, K2, K2, K2, K2, K2,
K2, K2, K2]

　重み行列Ｗ３は、例えばロボット１２の関節軸Ｖｘ、Ｖｙの最大並進推力が、それぞれ１０Ｎであり、関節軸Ｖｚ、Ｌ１～Ｌ３の最大トルクが、それぞれ１０Ｎｍ、５Ｎｍ、５Ｎｍ、５Ｎｍであり、関節軸ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の最大トルクがそれぞれ１Ｎｍである場合、対角項は、各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の最大並進推力又は最大トルクに正の定数Ｋ３を乗じた値となる対角行列に設定される。
　すなわち、重み行列Ｗ３は、例えば以下のように設定される。 In the weight matrix W3, for example, the maximum translational thrusts of the joint axes Vx and Vy of the robot 12 are each 10 N, the maximum torques of the joint axes Vz and L1 to L3 are 10 Nm, 5 Nm, 5 Nm, and 5 Nm, respectively. When the maximum torque of RJ1 to RJ7 and LJ1 to LJ7 is 1 Nm, the diagonal term is the maximum translational thrust or maximum torque of each joint axis Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, LJ1 to LJ7 It is set to a diagonal matrix that is a value multiplied by a positive constant K3.
That is, the weight matrix W3 is set as follows, for example.

Ｗ３　＝　ｄｉａｇ［１０×Ｋ３，１０×Ｋ３，１０×Ｋ３，５×Ｋ３，５×Ｋ３，
　５×Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，Ｋ３，
　Ｋ３，Ｋ３，Ｋ３］ W3 = diag [10 × K3, 10 × K3, 10 × K3, 5 × K3, 5 × K3
5 × K3, K3, K3, K3, K3, K3, K3, K3, K3, K3, K3, K3
K3, K3, K3]

　ここで、前述の重み行列Ｗ１を重み行列Ｗとして用いることにより、理論的には制御周期毎のロボット１２の各関節の運動に必要な運動エネルギーを最小化できる。しかし、実際には、各関節に備わるサーボモータの最大回転速度や最大出力トルクには上限があり、生成した関節速度指令に従って、ロボット１２が動作できない場合がある。そこで、前述の重み行列Ｗ２、又は重み行列Ｗ３のように、各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７の動作性能等を考慮して、運動エネルギーが抑制されるように重み行列Ｗが決定される。 Here, by using the aforementioned weight matrix W1 as the weight matrix W, it is theoretically possible to minimize the kinetic energy required for the motion of each joint of the robot 12 for each control cycle. However, actually, there is an upper limit for the maximum rotation speed and the maximum output torque of the servo motor provided in each joint, and the robot 12 may not be able to operate according to the generated joint speed command. Therefore, as in the above-described weight matrix W2 or weight matrix W3, the kinetic energy is suppressed in consideration of the operation performance of each joint axis Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7. Thus, the weight matrix W is determined.

　重み行列Ｗを用いることで、負荷重量や負荷イナーシャの大小に応じて角速度指令を与え、必要な運動エネルギーを抑えることが可能となる。具体的には、リンク等の駆動対象の負荷重量や負荷イナーシャが大きい関節軸（例えば、関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＬＪ１、ＬＪ２、ＲＪ１、ＲＪ２）ほど、より小さな角速度指令を与え、駆動対象の負荷重量や負荷イナーシャの小さい関節軸（例えば、関節軸ＬＪ３～ＬＪ７、ＲＪ３～ＲＪ７）ほど、より大きな角速度指令を与えるための重み行列Ｗとする。重み By using the weight matrix W, it is possible to give an angular velocity command according to the load weight and load inertia, and to suppress the necessary kinetic energy. Specifically, a smaller angular velocity command is issued for a joint axis (for example, joint axes Vx, Vy, Vz, L1 to L3, LJ1, LJ2, RJ1, RJ2) having a large load weight or load inertia of a link or the like to be driven. A weight matrix W for giving a larger angular velocity command to a joint axis (for example, joint axes LJ3 to LJ7, RJ3 to RJ7) having a smaller load weight or load inertia to be driven is given.

（ステップＳＡ２）
　第１の最適化動作指令生成部１４ｂは、信号Ａが入力されると、ロボット１２が腕下げ動作をするための関節角速度指令ω_ＥＬＢを計算する。関節角速度指令ω_ＥＬＢは、少なくとも以下３つの方法により計算することができる。これらの方法には、例えば、１）重力トルクのノルムの勾配を利用する計算方法Ａ１、２）位置エネルギーの勾配を利用する計算方法Ａ２、又は３）腕下げした姿勢へ位置決めする計算方法Ａ３がある。いずれの計算方法を用いるかは、使用するロボットコントローラの演算能力や、対象ロボットの動力学計算パラメータが既知であるか等の状況に応じて決定される。 (Step SA2)
When the signal A is input, the first optimization operation command generation unit 14b calculates a joint angular velocity command ω _ELB for the robot 12 to perform the arm lowering operation. The joint angular velocity command ω _ELB can be calculated by at least the following three methods. These methods include, for example, 1) a calculation method A1 that uses the gradient of the norm of gravity torque, 1) a calculation method A2 that uses the gradient of potential energy, or 3) a calculation method A3 that positions the arm in a lowered posture. is there. Which calculation method is used is determined according to the situation such as the calculation capability of the robot controller to be used and the dynamic calculation parameters of the target robot.

　左アームＬＡ、右アームＲＡ、又は両アームＬＡ、ＲＡのうち、いずれを腕下げ動作の対象とするかは、制御対象箇所により決定される。例えば、ある教示過程において、制御対象箇所が右アームＲＡの場合には右アームＲＡのみの腕下げ動作が実行される。 Which of the left arm LA, the right arm RA, or both the arms LA, RA is to be subjected to the arm lowering operation is determined by the location to be controlled. For example, in a certain teaching process, when the control target location is the right arm RA, the arm lowering operation of only the right arm RA is executed.

　以下、関節角速度指令ω_ＥＬＢの各計算方法Ａ１、Ａ２、Ａ３について説明する。
（計算方法Ａ１）
　計算方法Ａ１では、次式（４ａ１）に基づき、重力トルクのノルムの勾配を用いて、重力トルクのノルムを小さくする方向に各関節軸を移動させるための関節角速度指令ω_ＥＬＢが計算される。重力トルクのノルムとして、例えば、各関節の重力トルク（重力によって各関節軸に加えられるトルク）τ_ＧＲＶのユークリッドノルムや最大ノルム等が用いられる。 Hereinafter, each calculation method A1, A2, A3 of the joint angular velocity command ω _ELB will be described.
(Calculation method A1)
In the calculation method A1, based on the following equation (4a1), a joint angular velocity command ω _ELB for moving each joint axis in a direction to reduce the norm of the gravitational torque is calculated using the gradient of the norm of the gravitational torque. As the norm of the gravity torque, for example, the Euclidean norm or the maximum norm of the gravity torque (torque applied to each joint axis by gravity) τ _GRV of each joint is used.

ω_ＥＬＢ　＝　Ｋ　ｇｒａｄ∥τ_ＧＲＶ∥　　　式（４ａ１） ω _ELB = K grad ∥τ _GRV ∥ Equation (4a1)

　ここで、Ｋは腕下げ動作の制御を拘束するゲインである。ゲインＫは、大きな値にすればロボット１２の動作を高速化できる。ただし、ゲインＫは、ロボット１２が発振しない程度に抑えた値に設定しておく。 Here, K is a gain that restricts the control of the arm lowering operation. If the gain K is set to a large value, the operation of the robot 12 can be speeded up. However, the gain K is set to a value suppressed to such an extent that the robot 12 does not oscillate.

　重力トルクのノルム∥τ_ＧＲＶ∥の勾配（ｇｒａｄ）を用いて冗長軸を動作させることにより、第１の最適化動作指令生成部１４ｂは、姿勢保持トルク（重力トルクに抗するトルク）を抑制する姿勢を生成することができる。しかし、姿勢保持トルクを抑制する姿勢には２つの形態が存在する。例えば、腰関節軸Ｌ１について、ロボット１２の胴体１２ｃが直立した姿勢Ｐ１及び頭部１２ｄ（図２Ａ参照）が鉛直方向真下を向いた姿勢Ｐ２は、姿勢保持トルクを低減できる。しかし、関節可動域の制約により、頭部１２ｄが鉛直方向真下を向いた姿勢Ｐ２はとり得ない。このため、実用上は、２つの形態の姿勢のうち、いずれの姿勢について姿勢保持トルクを抑制するかを選択し、姿勢保持トルクの低減化を実現する。つまり、腰関節軸のように、姿勢保持トルクを抑制する姿勢に２つの形態が存在する関節軸については、関節可動域の制約を考慮して、関節軸の正方向及び負方向のうち、いずれかの方向に姿勢保持トルクを低減させるかを決定する。 By operating the redundant axis using the gravitational torque norm ∥τ _GRV ∥, the first optimization operation command generation unit 14b suppresses the posture maintenance torque (torque against the gravity torque). A posture can be generated. However, there are two forms of postures for suppressing the posture holding torque. For example, with respect to the waist joint axis L1, the posture P1 in which the body 12c of the robot 12 stands upright and the posture P2 in which the head 12d (see FIG. 2A) faces directly below in the vertical direction can reduce the posture holding torque. However, due to the limitation of the range of motion of the joint, the posture P2 in which the head 12d faces directly below the vertical direction cannot be taken. For this reason, in practice, the posture holding torque is selected for which of the two postures to suppress the posture holding torque. That is, for joint axes that have two forms of postures that suppress posture holding torque, such as the hip joint shaft, considering the joint range of motion, either the positive or negative direction of the joint axis It is determined whether to reduce the posture holding torque in one direction.

（計算方法Ａ２）
　計算方法Ａ２では、次式（４ａ２）に基づき、位置エネルギーＰの勾配を用いて、位置エネルギーを小さくする方向に各リンクＬＮＫを移動させるための関節角速度指令ω_ＥＬＢが計算される。位置エネルギーは次式（４ａ３）に基づいて計算される。 (Calculation method A2)
In the calculation method A2, based on the following equation (4a2), a joint angular velocity command ω _ELB for moving each link LNK in the direction of decreasing the position energy is calculated using the gradient of the position energy P. The potential energy is calculated based on the following equation (4a3).

ω_ＥＬＢ　＝　－Ｋ　ｇｒａｄ（Ｐ）　　　式（４ａ２） ω _ELB = -K grad (P) Equation (4a2)

Ｐ＝Σ_＊（Ｍ_＊ｇ^ＣｒＴ_＊ｌｇ_＊）　　　式（４ａ３） _{_{^{P = Σ * (M * g}}} Cr T * lg *) formula (4a3)

　ここで、＊は両アームＬＡ、ＲＡを構成する複数のリンクのうち、いずれのリンクであるかを特定するための添え字である。Ｍはロボット１２の各リンクの質量を示す。ｇは重力加速度を示す。^ＣｒＴ_＊はロボット座標上Ｃｒにおける、第＊リンク（複数のリンクの中から特定されたリンク）までの同次変換行列である。ｌｇ_＊は第＊リンクの第＊リンク座標（複数のリンクの中から特定されたリンクのリンク座標）上での重心位置である。 Here, * is a subscript for specifying which link among a plurality of links constituting both arms LA and RA. M represents the mass of each link of the robot 12. g represents gravitational acceleration. ^Cr T _* is a homogeneous transformation matrix up to the * th link (a link specified from a plurality of links) in the robot coordinate Cr. lg _* is the barycentric position of the * link on the * link coordinates (link coordinates of links specified from a plurality of links).

　位置エネルギーＰの勾配（ｇｒａｄ）を用いて冗長軸を動作させることにより、位置エネルギーを抑制する姿勢を生成することができる。ここで、腰関節軸Ｌ１、Ｌ２によって支持される胴体１２ｃについては、頭部１２ｄが鉛直方向下向きを向いた姿勢が位置エネルギーが最も小さくなる。しかし、関節可動域の制約により、頭部１２ｄが鉛直方向真下を向く姿勢はとり得ない。そこで、腰関節軸Ｌ１、Ｌ２に対して本計算方法Ａ２を適用せず、胴体１２ｃが直立した姿勢に保持する。一方、腰関節軸Ｌ１、Ｌ２以外の関節に対して本計算方法Ａ２を適用し、姿勢保持トルクを抑制する。 By operating the redundant axis using the gradient of potential energy P, a posture that suppresses potential energy can be generated. Here, with respect to the body 12c supported by the hip joint axes L1 and L2, the posture in which the head 12d faces downward in the vertical direction has the smallest potential energy. However, due to the limitation of the range of motion of the joint, the posture in which the head 12d faces directly below the vertical direction cannot be taken. Therefore, the calculation method A2 is not applied to the hip joint axes L1 and L2, and the body 12c is held in an upright posture. On the other hand, this calculation method A2 is applied to joints other than the hip joint axes L1 and L2, and the posture holding torque is suppressed.

（計算方法Ａ３）
　計算方法Ａ３では、次式（４ｂ）に基づき、関節軸の現在角度θ_ＣＵＲと、最適な関節軸の角度θ_ＯＰＴ（ベクトル）と、の差を用いて、関節角速度指令ω_ＥＬＢが計算される。最適な関節軸の角度θ_ＯＰＴとは、１）ロボット１２の右アームＲＡ又は左アームＬＡが肘の側方への張り出すこと、２）右アームＲＡ又は左アームＬＡが胴体と干渉すること、３）各関節軸の可動範囲、及び４）ロボット１２の特異姿勢等を考慮して、操作者が経験的に判断した値である。この最適な関節軸の角度θ_ＯＰＴが明示的に決まっている場合にのみ、本計算方法Ａ３が適用される。
　なお、θ_ＣＵＲは、ロボット１２を駆動するサーボモータのエンコーダから計算される関節角度の現在値θｆｂ、又はソフトウェア上で計算される関節角度の指令値θｒｅｆのいずれかの値が用いられる。関節角度の現在値θｆｂ及び関節角度の指令値θｒｅｆは、１制御周期前の値である。 (Calculation method A3)
In the calculation method A3, the joint angular velocity command ω _ELB is calculated using a difference between the current angle θ _CUR of the joint axis and the optimum angle θ _OPT (vector) of the joint axis based on the following equation (4b). . The optimal joint axis angle θ _OPT is 1) that the right arm RA or left arm LA of the robot 12 projects to the side of the elbow, and 2) that the right arm RA or left arm LA interferes with the torso, It is a value determined by the operator empirically in consideration of 3) the movable range of each joint axis and 4) the specific posture of the robot 12. This calculation method A3 is applied only when the optimum joint axis angle θ _OPT is explicitly determined.
Note that for θ _CUR , either the current value θfb of the joint angle calculated from the encoder of the servo motor that drives the robot 12 or the command value θref of the joint angle calculated on software is used. The current value θfb of the joint angle and the command value θref of the joint angle are values before one control cycle.

ω_ＥＬＢ　＝　Ｋ（θ_ＯＰＴ－θ_ＣＵＲ）　　　式（４ｂ） ω _ELB = K (θ _OPT −θ _CUR ) Formula (4b)

　ここで、この関節軸を、例えば関節軸ＬＪ２や関節軸ＲＪ２とすることで、ロボット１２は、腕下げ動作（図５Ａ参照）を行うことができる。
　以上示した関節角速度指令ω_ＥＬＢの計算方法Ａ１～Ａ３は、いずれも腕下げ動作に関する関節にのみ適用される。例えば、第２の指令生成部１４ｅが、関節軸ＬＪ２に関する関節角速度指令ω_ＥＬＢを計算することによって、左アームＬＡについて、腕下げ動作を行うことができる。また例えば、第２の指令生成部１４ｅが、関節軸ＲＪ１、ＲＪ２に関する関節角速度指令ω_ＥＬＢを計算することによって、右アームＲＡについて、腕下げ動作を行うことができる。 Here, by setting the joint axis as, for example, the joint axis LJ2 or the joint axis RJ2, the robot 12 can perform the arm lowering operation (see FIG. 5A).
All of the calculation methods A1 to A3 of the joint angular velocity command ω _ELB described above are applied only to the joint related to the arm lowering operation. For example, the second command generation unit 14e can perform the arm lowering operation for the left arm LA by calculating the joint angular velocity command ω _ELB related to the joint axis LJ2. Further, for example, the second command generation unit 14e can perform the arm lowering operation for the right arm RA by calculating the joint angular velocity command ω _ELB regarding the joint axes RJ1 and RJ2.

　腕下げ動作によって、ロボット１２が人に与える威圧感が軽減される。また、腕下げ動作によってアームＬＡ、ＲＡの位置が低い位置にて保持されるので、アームＬＡ、ＲＡの位置が高い位置にある場合と比較して、より広いロボット１２の作業空間が確保される。腕 The intimidating feeling given to the human by the robot 12 is reduced by the arm lowering operation. Further, since the positions of the arms LA and RA are held at a low position by the arm lowering operation, a wider working space for the robot 12 is secured as compared with the case where the positions of the arms LA and RA are at a high position. .

　ただし、各アームＬＡ、ＲＡを下げ過ぎることによって、各アームＬＡ、ＲＡと胴体１２ｃとの干渉が発生したり、各アームＬＡ、ＲＡが特異姿勢をとったりする可能性がある。その場合には、図５Ｂに示すように関節角速度指令ω_ＥＬＢに基づいて移動するアーム位置よりも、高い位置にアームＬＡ、ＲＡを移動させてもよい。その際の移動量は、作業空間を確保する上で障害とならない程度、又は威圧感を軽減する上で障害とならない程度の移動量であることが好ましい。 However, if the arms LA and RA are lowered too much, interference between the arms LA and RA and the body 12c may occur, or the arms LA and RA may take a peculiar posture. In that case, as shown in FIG. 5B, the arms LA and RA may be moved to a position higher than the arm position that moves based on the joint angular velocity command _ωELB . The movement amount at that time is preferably a movement amount that does not become an obstacle in securing the work space or does not become an obstacle in reducing the intimidation.

（ステップＳＡ３）
　ゼロ空間行列計算部１４ｄは、前述の通り、ロボット１２の関節角度θ_ＣＵＲ（関節角度の現在値θｆｂ、又は関節角度の指令値θｒｅｆ）に基づいて、ロボット１２の手先に関するヤコビ行列Ｊのゼロ空間行列Ｎを計算する。 (Step SA3)
As described above, the zero-space matrix calculation unit 14d determines the zero space of the Jacobian matrix J related to the hand of the robot 12 based on the joint angle θ _CUR of the robot 12 (the current value θfb of the joint angle or the command value θref of the joint angle). The matrix N is calculated.

（ステップＳＡ４）
　第２の指令生成部１４ｅは、次式（５）で示すように、関節角速度指令ω_ＥＬＢ、及びゼロ空間行列Ｎに基づいて、関節角速度指令ωｒｅｆ２を計算する。 (Step SA4)
The second command generation unit 14e calculates the joint angular velocity command ωref2 based on the joint angular velocity command ω _ELB and the zero space matrix N as shown in the following equation (5).

ωｒｅｆ２　＝　Ｎω_ＥＬＢ　　　式（５） ωref2 = Nω _ELB equation (5)

（ステップＳＡ５）
　関節角速度指令ωｒｅｆ３は、次式（６）に基づいて計算される。 (Step SA5)
The joint angular velocity command ωref3 is calculated based on the following equation (6).

ωｒｅｆ３　＝　ω_ＴＡＳＫ＋ωｒｅｆ２　　　式（６） ωref3 = ω _TASK + ωref2 Equation (6)

　位置速度制御部１４ｆは、関節角度の指令値θｒｅｆと、関節角度の現在値θｆｂと、に基づき、フィードバック制御を実行する。位置速度制御部１４ｆは、各サーボモータを制御し、ロボット１２の各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７を制御する。その結果、ロボット１２は、腕下げ動作を実行する。 The position / speed control unit 14f executes feedback control based on the command value θref of the joint angle and the current value θfb of the joint angle. The position / speed controller 14f controls each servo motor and controls each joint axis Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7 of the robot 12. As a result, the robot 12 performs an arm lowering operation.

　図６Ａは、操作モードＡによるロボット１２の姿勢（右下図）と、操作モードＡが選択されていない場合のロボット１２の姿勢（左下図）とを比較して示した図である。右下図に示すように、操作モードＡによって、駆動対象の負荷重量や負荷イナーシャの小さい関節軸がより大きく動く。一方、負荷重量や負荷イナーシャの大きい関節軸（移動台車１２ａの走行用軸）はあまり動かない。
　このように、ステップＳＡ１～ＳＡ５により、必要な運動エネルギーを低減するようにロボット１２が制御される。 FIG. 6A is a diagram showing a comparison between the posture of the robot 12 in the operation mode A (lower right diagram) and the posture of the robot 12 when the operation mode A is not selected (lower left diagram). As shown in the lower right figure, according to the operation mode A, the joint shaft with a small load weight or load inertia of the drive target moves more greatly. On the other hand, the joint shaft having a large load weight and load inertia (the traveling shaft of the movable carriage 12a) does not move much.
As described above, the robot 12 is controlled by steps SA1 to SA5 so as to reduce the necessary kinetic energy.

（ステップＳＢ１）
　続いて、操作者は、ロボット１２に物品ＯＢＪを把持した姿勢を一定時間保持させる。以下のステップＳＢ１～ＳＢ５においては、ロボット１２の手先の位置姿勢を維持したまま（物品ＯＢＪを把持したまま）ロボット１２が各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７のトルクを抑制する姿勢をとるように、操作モードＢにてロボット１２の動作が制御される。
　例えば、左アームＬＡ及び右アームＲＡが水平方向に伸びた姿勢は、胴体１２ｃの側に近い、左アームＬＡ及び右アームＲＡを構成する関節軸ほど、一般に負荷トルクが増大する。そこで、移動台車１２ａによりロボット１２がテーブル４０に近づくよう移動し、手先の位置姿勢を維持したまま、冗長自由度を使用して左アームＬＡ及び右アームＲＡを曲げた姿勢をとる結果、姿勢保持トルクが抑制される。 (Step SB1)
Subsequently, the operator causes the robot 12 to hold the posture holding the article OBJ for a certain period of time. In the following steps SB1 to SB5, the robot 12 keeps the position and orientation of the hand of the robot 12 (while holding the article OBJ), and the robot 12 has the joint axes Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, LJ1 to The operation of the robot 12 is controlled in the operation mode B so as to take a posture to suppress the torque of the LJ7.
For example, in the posture in which the left arm LA and the right arm RA extend in the horizontal direction, the load torque generally increases as the joint shafts of the left arm LA and the right arm RA are closer to the body 12c. Therefore, the robot 12 is moved closer to the table 40 by the movable carriage 12a, and the posture is maintained as a result of taking the posture in which the left arm LA and the right arm RA are bent using the redundancy degree of freedom while maintaining the position and posture of the hand. Torque is suppressed.

　また例えば、腰部１２ｂを曲げて胴体１２ｃを前傾させた姿勢は、移動台車１２ａの側の関節軸Ｌ１、Ｌ２の負荷トルクが増大する。そこで、手先の位置姿勢を維持したまま、冗長自由度を使用して腰部１２ｂを伸ばした姿勢をとる結果、姿勢保持トルクが抑制される。 Also, for example, in the posture in which the waist 12b is bent and the body 12c is tilted forward, the load torque of the joint axes L1 and L2 on the movable carriage 12a side increases. Therefore, the posture holding torque is suppressed as a result of taking the posture in which the waist 12b is extended using the redundancy degree of freedom while maintaining the position and posture of the hand.

　操作者が操作モード切り替えボタンＢを押して、操作モードを操作モードＢ（姿勢保持トルクを低減する操作モード）とする。この操作により、図３に示す第１の最適化動作指令生成部１４ｂに信号Ａが入力されなくなり、代わって信号Ｂが第２の最適化動作指令生成部１４ｃに入力される。信号Ａが入力されないので、第１の最適化動作指令生成部１４ｂは動作しない。 The operator presses the operation mode switching button B to set the operation mode to the operation mode B (operation mode for reducing the posture holding torque). By this operation, the signal A is not input to the first optimization operation command generation unit 14b shown in FIG. 3, and the signal B is input to the second optimization operation command generation unit 14c instead. Since the signal A is not input, the first optimization operation command generation unit 14b does not operate.

　操作モード切り替えボタンＢが押下されている間、図３に示す第１の指令生成部１４ａに、手先速度指令Ｖが入力される。
　ロボット制御装置１４の第１の指令生成部１４ａ、第２の最適化動作指令生成部１４ｃ、ゼロ空間行列計算部１４ｄ、第２の指令生成部１４ｅ、及び位置速度制御部１４ｆは、以下のように動作する。 While the operation mode switching button B is pressed, the hand speed command V is input to the first command generation unit 14a shown in FIG.
The first command generation unit 14a, the second optimization operation command generation unit 14c, the zero space matrix calculation unit 14d, the second command generation unit 14e, and the position / speed control unit 14f of the robot control device 14 are as follows. To work.

　第１の指令生成部１４ａは、前述の式（２）、（３）を用いて運動エネルギーを抑制するための関節角速度指令ω_ＴＡＳＫを生成する。関節角速度指令ω_ＴＡＳＫに基づいてロボット１２を動作させることにより、制御周期毎のロボットの各関節の運動に必要な運動エネルギーを抑制することができる。しかし、ロボット１２を連続的に動作させていく中で、姿勢保持を伴う場面では、エネルギー消費の観点から必ずしも効率的ではない。このため、以下の姿勢保持トルク低減化の手段を用いることにより消費エネルギーが抑えられる。 The first command generation unit 14a generates a joint angular velocity command ω _TASK for suppressing kinetic energy using the aforementioned equations (2) and (3). By operating the robot 12 based on the joint angular velocity command ω _TASK , the kinetic energy necessary for the motion of each joint of the robot for each control cycle can be suppressed. However, when the robot 12 is continuously operated, it is not always efficient from the viewpoint of energy consumption in a scene involving posture maintenance. For this reason, energy consumption can be suppressed by using the following means for reducing the posture holding torque.

（ステップＳＢ２）
　第２の最適化動作指令生成部１４ｃは、信号Ｂが入力されると、関節角速度指令ω_ＧＲＶを計算する。関節角速度指令ω_ＧＲＶは、少なくとも以下３つの方法により計算することができる。これらの方法には、例えば、１）重力トルクのノルムの勾配を利用する計算方法Ｂ１、２）位置エネルギーの勾配を利用する計算方法Ｂ２、又は３）姿勢保持トルクを抑制する姿勢へ位置決めする計算方法Ｂ３がある。 (Step SB2)
When the signal B is input, the second optimization operation command generation unit 14c calculates the joint angular velocity command ω _GRV . The joint angular velocity command ω _GRV can be calculated by at least the following three methods. These methods include, for example, 1) a calculation method B1 that uses the gradient of the norm of gravitational torque, 1) a calculation method B2 that uses the gradient of potential energy, or 3) a calculation that positions to a posture that suppresses posture holding torque. There is method B3.

　以下、関節角速度指令ω_ＧＲＶの各計算方法Ｂ１、Ｂ２、Ｂ３について説明する。
（計算方法Ｂ１）
　計算方法Ｂ１では、次式（７ａ１）に基づき、重力トルクのノルムの勾配を用いて重力トルクのノルムを小さくする方向に各関節軸を移動させるための関節角速度指令ω_ＧＲＶが計算される。重力トルクのノルムとして、例えば、各関節の重力トルク（重力によって各関節軸に加えられるトルク）τ_ＧＲＶのユークリッドノルム、又は最大ノルム等が用いられる。 Hereinafter, each calculation method B1, B2, and B3 of the joint angular velocity command ω _GRV will be described.
(Calculation method B1)
In the calculation method B1, based on the following equation (7a1), a joint angular velocity command ω _GRV for moving each joint axis in the direction of decreasing the norm of the gravitational torque is calculated using the gradient of the norm of the gravitational torque. As the norm of the gravity torque, for example, the gravity torque of each joint (torque applied to each joint axis by gravity) τ _GRV or the maximum norm is used.

ω_ＧＲＶ　＝　Ｋ　ｇｒａｄ∥τ_ＧＲＶ∥　　　式（７ａ１） ω _{_GRV} = K grad∥τ _GRV ∥ formula (7a1)

（計算方法Ｂ２）
　計算方法Ｂ２では、次式（７ａ２）に基づき、位置エネルギーＰの勾配を用いて、各リンクＬＮＫの位置エネルギーを小さくする方向に各リンクＬＮＫを移動させるための関節角速度指令ω_ＧＲＶが計算される。位置エネルギーは次式（７ａ３）に基づいて計算される。 (Calculation method B2)
In the calculation method B2, a joint angular velocity command ω _GRV for moving each link LNK in the direction of decreasing the positional energy of each link LNK is calculated using the gradient of the positional energy P based on the following equation (7a2). . The potential energy is calculated based on the following equation (7a3).

ω_ＧＲＶ　＝　－Ｋ　ｇｒａｄ（Ｐ）　　　式（７ａ２） ω _GRV = −K grad (P) Equation (7a2)

Ｐ＝Σ_＊（Ｍ_＊ｇ^ＣｒＴ_＊ｌｇ_＊）　　　式（７ａ３） _{_{^{P = Σ * (M * g}}} Cr T * lg *) formula (7a3)

（計算方法Ｂ３）
　計算方法Ｂ３では、次式（７ｂ）に基づき、関節軸の現在角度θ_ＣＵＲと、最適な関節軸の角度θ_ＯＰＴと、の差を用いて、関節角速度指令ω_ＧＲＶが計算される。最適な関節軸の角度θ_ＯＰＴとは、例えば、ロボット１２が直立し、右アームＲＡ及び左アームＬＡが鉛直方向下向きに伸びた姿勢である。この姿勢は、静力学的な観点から、ロボット１２の姿勢保持トルクを抑制させる姿勢である。 (Calculation method B3)
In the calculation method B3, based on the following equation (7b), the joint angular velocity command ω _GRV is calculated using the difference between the current angle θ _CUR of the joint axis and the optimum angle θ _{OPT of the} joint axis. The optimal joint axis angle θ _OPT is, for example, a posture in which the robot 12 stands upright and the right arm RA and the left arm LA extend downward in the vertical direction. This posture is a posture that suppresses the posture holding torque of the robot 12 from a static viewpoint.

ω_ＧＲＶ　＝　Ｋ（θ_ＯＰＴ－θ_ＣＵＲ）　　　式（７ｂ） ω _GRV = K (θ _OPT −θ _CUR ) Formula (7b)

　以上示した関節角速度指令ω_ＧＲＶの計算方法Ｂ１～Ｂ３は、制御対象となる関節軸全てに適用される。 The calculation methods B1 to B3 of the joint angular velocity command ω _{GRV described} above are applied to all joint axes to be controlled.

（ステップＳＢ３）
　ゼロ空間行列計算部１４ｄは、前述の式（１）にて示したように、ロボット１２の関節角度θ_ＣＵＲに基づいて、ロボット１２の手先に関するヤコビ行列Ｊのゼロ空間行列Ｎを計算する。 (Step SB3)
The zero space matrix calculation unit 14d calculates the zero space matrix N of the Jacobian matrix J related to the hand of the robot 12 based on the joint angle θ _CUR of the robot 12 as shown in the above-described equation (1).

（ステップＳＢ４）
　第２の指令生成部１４ｅは、次式（８）で示すように、関節角速度指令ω_ＧＲＶ、及びゼロ空間行列Ｎに基づいて、関節角速度指令ωｒｅｆ２を計算する。 (Step SB4)
The second command generation unit 14e calculates the joint angular velocity command ωref2 based on the joint angular velocity command ω _GRV and the zero space matrix N as shown in the following equation (8).

ωｒｅｆ２　＝　Ｎω_ＧＲＶ　　　式（８） ωref2 = Nω _GRV equation (8)

（ステップＳＢ５）
　関節角速度指令ωｒｅｆ３は、前述の式（６）に基づいて計算される。
　位置速度制御部１４ｆは、関節角度の指令値θｒｅｆと、関節角度の現在値θｆｂと、に基づき、フィードバック制御を実行する。位置速度制御部１４ｆは、各サーボモータを制御し、ロボット１２の各関節軸Ｖｘ、Ｖｙ、Ｖｚ、Ｌ１～Ｌ３、ＲＪ１～ＲＪ７、ＬＪ１～ＬＪ７を制御する。その結果、ロボット１２は、姿勢保持に必要なトルクを抑制する。 (Step SB5)
The joint angular velocity command ωref3 is calculated based on the above equation (6).
The position / speed control unit 14f performs feedback control based on the command value θref of the joint angle and the current value θfb of the joint angle. The position / speed controller 14f controls each servo motor and controls each joint axis Vx, Vy, Vz, L1 to L3, RJ1 to RJ7, and LJ1 to LJ7 of the robot 12. As a result, the robot 12 suppresses the torque necessary for maintaining the posture.

　図６Ｂは、操作モードＡによる動作が完了した状態（ステップＳＡ５の実行が完了した状態）のロボット１２の姿勢（上図）と、操作モードＢによるロボット１２の姿勢（下図）とを比較して示した図である。本操作モードＢにより、ロボット１２の姿勢が操作モードＡによる動作が完了した姿勢（図６Ｂの上図参照）から、左アームＬＡ及び右アームＲＡを曲げ、腰部１２ｂを伸ばした姿勢（図６Ｂの下図参照）へと変化する。すなわち、姿勢保持に必要なトルクを抑制した状態でロボット１２の姿勢が保持される。 FIG. 6B compares the posture of the robot 12 in the state where the operation in the operation mode A is completed (the state in which the execution of step SA5 is completed) (upper diagram) and the posture of the robot 12 in the operation mode B (lower diagram). FIG. In this operation mode B, the posture of the robot 12 is the posture in which the operation in the operation mode A is completed (refer to the upper diagram in FIG. 6B), and the left arm LA and the right arm RA are bent and the waist 12b is extended (in FIG. 6B). (See the figure below). That is, the posture of the robot 12 is held in a state where the torque necessary for holding the posture is suppressed.

　なお、前述のステップＳＢ１～ＳＢ５は必ずしも実行されなくてもよく、ロボット１２に物品ＯＢＪを把持した姿勢を保持させない場合には、ステップＳＡ５から次のステップＳＣに進む。例えば、ロボット１２が、物品ＯＢＪを把持し、物品ＯＢＪを移動させ、把持した物品ＯＢＪを開放する動作のように、ロボット１２が連続的に動作する場合は、ステップＳＢ１～ＳＢ５を実行することなく所定の次の動作を実行する方が、消費エネルギーを抑えることができる。しかし、ロボット１２が右アームＲＡで物品ＯＢＪを把持した後、右アームＲＡの位置を保持しながら左アームＬＡで作業したり、ビジョン認識処理又は音声によるコミュニケーション処理をしたりする場合には、姿勢保持トルクを抑制するためにステップＳＢ１～ＳＢ５を実行することによって、消費エネルギーが抑えられる。 Note that the above-described steps SB1 to SB5 do not necessarily have to be executed. If the robot 12 does not hold the posture of gripping the article OBJ, the process proceeds from step SA5 to the next step SC. For example, when the robot 12 operates continuously, such as an operation of gripping the article OBJ, moving the article OBJ, and releasing the gripped article OBJ, steps SB1 to SB5 are not executed. It is possible to reduce energy consumption by executing a predetermined next operation. However, when the robot 12 holds the article OBJ with the right arm RA and then works with the left arm LA while holding the position of the right arm RA, or when performing vision recognition processing or voice communication processing, By executing steps SB1 to SB5 to suppress the holding torque, energy consumption can be suppressed.

（ステップＳＣ）
　図４に示すように、教示が完了した場合には、ロボットシステム１０の動作は終了する。教示が完了していない場合には、ステップＳＡ１を実行する。 (Step SC)
As shown in FIG. 4, when the teaching is completed, the operation of the robot system 10 ends. If the teaching has not been completed, step SA1 is executed.

　このように、ステップＳＡ１～ＳＡ５、ステップＳＢ１～ＳＢ５、及びステップＳＣを繰り返して実行することにより、作業プログラムを教示できる。
　教示された作業プログラムは、教示ペンダント１６（図１参照）に設けられた再生ボタンが押下されることにより、再生動作される。
　なお、これら一連のステップＳＡ１～ＳＡ５、ステップＳＢ１～ＳＢ５、及びステップＳＣは、可能な場合には、並行して実行したり、順番を入れ替えて実行したりしてもよい。 As described above, the work program can be taught by repeatedly executing Steps SA1 to SA5, Steps SB1 to SB5, and Step SC.
The taught work program is reproduced by pressing a reproduction button provided on the teaching pendant 16 (see FIG. 1).
It should be noted that the series of steps SA1 to SA5, steps SB1 to SB5, and step SC may be executed in parallel or in a different order, if possible.

　以上説明したように、本実施例に係るロボットシステム１０によれば、冗長ロボットの消費エネルギーを低減する教示位置及び動作軌道を自動で生成できる。 As described above, according to the robot system 10 according to the present embodiment, it is possible to automatically generate the teaching position and the motion trajectory that reduce the energy consumption of the redundant robot.

　本発明は、前述の実施例に限定されるものではなく、本発明の要旨を変更しない範囲での変更は可能である。例えば、前述の実施例や変形例の一部又は全部を組み合わせて発明を構成する場合も本発明の技術的範囲に含まれる。 The present invention is not limited to the above-described embodiments, and modifications can be made without changing the gist of the present invention. For example, a case where the invention is configured by combining some or all of the above-described embodiments and modifications is also included in the technical scope of the present invention.

　ロボット制御装置１４には、第１の最適化動作指令生成部１４ｂ及び第２の最適化動作指令生成部１４ｃのうち、いずれか一方のみを設けることもできる。すなわち、第１の最適化動作指令生成部１４ｂのみを設けて、腕下げ動作をするようにロボット１２を制御することもできるし、第２の最適化動作指令生成部１４ｃのみを設けて、姿勢保持トルクを抑制するようにロボット１２を制御することもできる。すなわち、操作モードＡ及び操作モードＢのうち、いずれか一方の操作モードのみによりロボット１２が制御されてもよい。 The robot controller 14 may be provided with only one of the first optimization operation command generation unit 14b and the second optimization operation command generation unit 14c. That is, it is possible to control the robot 12 so as to perform the arm lowering operation by providing only the first optimization operation command generation unit 14b, or provide only the second optimization operation command generation unit 14c. The robot 12 can also be controlled so as to suppress the holding torque. That is, the robot 12 may be controlled by only one of the operation modes A and B.

　各ボタンは、ハードウェアによるボタンに限定されるものではない。ボタンは、ソフトウェアによって実現されるボタンであっても良い。 Each button is not limited to a hardware button. The button may be a button realized by software.

　教示デバイスは、教示ペンダントに限定されるものではない。教示デバイスの他の例として、パーソナルコンピュータ等のロボット制御装置１４との通信機能を備えた遠隔操作機器が挙げられる。 The teaching device is not limited to the teaching pendant. Another example of the teaching device is a remote operation device having a communication function with the robot control device 14 such as a personal computer.

　本発明は、前述の実施例に示したように実機のロボットを用いる場合だけでなく、ロボットのシミュレーションモデルを用いる場合にも適用できる。即ち、オフラインシミュレータを用いて作業プログラムの妥当性を確認するためにも利用できる。 The present invention can be applied not only to the case of using an actual robot as shown in the above-described embodiments, but also to the case of using a robot simulation model. That is, it can also be used to confirm the validity of a work program using an offline simulator.

１０：ロボットシステム、１２：ロボット、１２ａ：移動台車、１２ｂ：腰部、１２ｃ：胴体、１２ｄ：頭部、１４：ロボット制御装置、１４ａ：第１の指令生成部、１４ｂ：第１の最適化動作指令生成部、１４ｃ：第２の最適化動作指令生成部、１４ｄ：ゼロ空間行列計算部、１４ｅ：第２の指令生成部、１４ｆ：位置速度制御部、１６：教示ペンダント、４０：テーブル、Ｌ１、Ｌ２、Ｌ３：腰関節軸、ＬＪ１～ＬＪ７：関節軸、ＬＡ：左アーム、ＬＨ：左ハンド、ＬＮＫ：リンク、ＯＢＪ：物品、ＲＡ：右アーム、ＲＨ：右ハンド、ＲＪ１～ＲＪ７：関節軸、Ｖｘ、Ｖｙ、Ｖｚ：走行用軸

10: Robot system, 12: Robot, 12a: Moving carriage, 12b: Waist, 12c: Torso, 12d: Head, 14: Robot controller, 14a: First command generator, 14b: First optimization operation Command generation unit, 14c: second optimization operation command generation unit, 14d: zero space matrix calculation unit, 14e: second command generation unit, 14f: position speed control unit, 16: teaching pendant, 40: table, L1 , L2, L3: Lumbar joint axis, LJ1 to LJ7: Joint axis, LA: Left arm, LH: Left hand, LNK: Link, OBJ: Article, RA: Right arm, RH: Right hand, RJ1 to RJ7: Joint axis , Vx, Vy, Vz: Traveling shaft

Claims

　複数のリンクを連結する、冗長軸を含む複数の関節軸を有し、前記複数の関節軸がサーボモータによって駆動されるロボットと、
　前記ロボットを制御するロボット制御装置とを備え、
　前記ロボット制御装置は、入力された前記ロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成する第１の指令生成部と、
　前記ロボットが腕下げ動作をするための関節角速度指令Ａを生成する第１の最適化動作指令生成部と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するゼロ空間行列計算部と、
　前記第１の最適化動作指令生成部が生成した前記関節角速度指令Ａ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成する第２の指令生成部と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記サーボモータを制御する制御部と、を有するロボットシステム。 A robot that has a plurality of joint axes including redundant axes, connecting a plurality of links, and wherein the plurality of joint axes are driven by a servo motor;
A robot control device for controlling the robot,
The robot control device generates a first joint angular velocity command of the robot for reducing kinetic energy based on the input velocity command of the control point of the robot;
A first optimizing operation command generation unit that generates a joint angular velocity command A for the robot to perform the arm lowering operation;
A zero-space matrix calculator for calculating a zero-space matrix of a Jacobian matrix related to the control points;
A second command generation unit that generates a second joint angular velocity command of the robot based on the joint angular velocity command A and the zero-space matrix generated by the first optimization operation command generation unit;
And a control unit that controls the servo motor based on a third joint angular velocity command that is a sum of the first and second joint angular velocity commands.
　請求項１記載のロボットシステムにおいて、
　前記第１の関節角速度指令は、前記制御点に関する重み付きヤコビ逆行列に該制御点の速度指令を乗じて求められるロボットシステム。 The robot system according to claim 1, wherein
The first joint angular velocity command is obtained by multiplying the weighted Jacobian inverse matrix related to the control point by the speed command of the control point.
　請求項２記載のロボットシステムにおいて、
　前記重み付きヤコビ逆行列は、重み行列Ｗを用いて求められ、該重み行列Ｗは、前記複数の関節軸のうち、負荷の大きい前記関節軸ほど、より小さな角速度指令を与え、前記負荷の小さい前記関節軸ほど、より大きな角速度指令を与えるための行列であるロボットシステム。 The robot system according to claim 2, wherein
The weighted Jacobian inverse matrix is obtained by using a weight matrix W, and the weight matrix W gives a smaller angular velocity command to the joint axis having a larger load among the plurality of joint axes, and the load having a smaller load. The robot system which is a matrix for giving a larger angular velocity command to the joint axis.
　請求項２又は３記載のロボットシステムにおいて、
　前記関節角速度指令Ａは、重力によって前記各関節軸に加えられる重力トルクを小さくする方向に、該各関節軸を移動させるための角速度指令であるロボットシステム。 The robot system according to claim 2 or 3,
The joint angular velocity command A is an angular velocity command for moving each joint axis in a direction to reduce the gravitational torque applied to each joint axis by gravity.
　請求項２又は３記載のロボットシステムにおいて、
　前記関節角速度指令Ａは、前記各リンクの位置エネルギーを小さくする方向に、該各リンクを移動させるための角速度指令であるロボットシステム。 The robot system according to claim 2 or 3,
The joint angular velocity command A is a robot system that is an angular velocity command for moving each link in a direction to reduce the positional energy of each link.
　請求項２又は３記載のロボットシステムにおいて、
　前記関節角速度指令Ａは、前記関節軸の角度θ_ＣＵＲと最適な前記関節軸の角度θ_ＯＰＴとの差を用いて計算されるロボットシステム。 The robot system according to claim 2 or 3,
The joint angular velocity command A is calculated using a difference between the joint axis angle θ _CUR and the optimum joint axis angle θ _OPT .
　請求項１～６のいずれか１項に記載のロボットシステムにおいて、
　前記ロボット制御装置は、前記ロボットの姿勢保持トルクを抑制するための関節角速度指令Ｂを生成する第２の最適化動作指令生成部を更に有し、
　前記第２の指令生成部は、前記関節角速度指令Ｂ及び前記ゼロ空間行列に基づいて、前記第２の関節角速度指令を生成するロボットシステム。 The robot system according to any one of claims 1 to 6,
The robot control device further includes a second optimization operation command generation unit that generates a joint angular velocity command B for suppressing the posture holding torque of the robot,
The second command generation unit is a robot system that generates the second joint angular velocity command based on the joint angular velocity command B and the zero space matrix.
　請求項７記載のロボットシステムにおいて、
　前記関節角速度指令Ｂは、重力によって前記各関節軸に加えられる重力トルクを小さくする方向に、該各関節軸を移動させるための角速度指令であるロボットシステム。 The robot system according to claim 7, wherein
The joint angular velocity command B is an angular velocity command for moving each joint axis in a direction to reduce the gravitational torque applied to each joint axis by gravity.
　請求項７記載のロボットシステムにおいて、
　前記関節角速度指令Ｂは、前記各リンクの位置エネルギーを小さくする方向に、該各リンクを移動させるための角速度指令であるロボットシステム。 The robot system according to claim 7, wherein
The joint angular velocity command B is an angular velocity command for moving each link in a direction to reduce the positional energy of each link.
　請求項７記載のロボットシステムにおいて、
　前記関節角速度指令Ｂは、前記関節軸の現在角度θ_ＣＵＲと、最適な前記関節軸の角度θ_ＯＰＴと、の差を用いて計算されるロボットシステム。 The robot system according to claim 7, wherein
The joint angular velocity command B is calculated using a difference between a current angle θ _CUR of the joint axis and an optimum angle θ _{OPT of the} joint axis.
　冗長軸を含む複数の関節軸を有し、前記複数の関節軸がサーボモータによって駆動されるロボットと、
　前記ロボットを制御するロボット制御装置とを備え、
　前記ロボット制御装置は、入力された前記ロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成する第１の指令生成部と、
　前記ロボットの姿勢保持トルクを抑制するための関節角速度指令を生成する最適化動作指令生成部と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するゼロ空間行列計算部と、
　前記最適化動作指令生成部が生成した前記関節角速度指令及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成する第２の指令生成部と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記サーボモータを制御する制御部とを有するロボットシステム。 A robot having a plurality of joint axes including a redundant axis, the plurality of joint axes being driven by a servo motor;
A robot control device for controlling the robot,
The robot control device generates a first joint angular velocity command of the robot for reducing kinetic energy based on the input velocity command of the control point of the robot;
An optimization operation command generation unit for generating a joint angular velocity command for suppressing the posture holding torque of the robot;
A zero-space matrix calculator for calculating a zero-space matrix of a Jacobian matrix related to the control points;
A second command generation unit that generates a second joint angular velocity command of the robot based on the joint angular velocity command generated by the optimization operation command generation unit and the zero space matrix;
A robot system comprising: a control unit that controls the servo motor based on a third joint angular velocity command that is a sum of the first and second joint angular velocity commands.
　冗長軸を含む複数の関節軸を有するロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成する第１の指令生成部と、
　前記ロボットが腕下げ動作をするための関節角速度指令Ａを生成する第１の最適化動作指令生成部と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するゼロ空間行列計算部と、
　前記関節角速度指令Ａ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成する第２の指令生成部と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、サーボモータを制御する制御部とを有するロボット制御装置。 A first command generation unit configured to generate a first joint angular velocity command of the robot for reducing kinetic energy based on a velocity command of a control point of a robot having a plurality of joint axes including redundant axes;
A first optimizing operation command generation unit that generates a joint angular velocity command A for the robot to perform the arm lowering operation;
A zero-space matrix calculator for calculating a zero-space matrix of a Jacobian matrix related to the control points;
A second command generation unit that generates a second joint angular velocity command of the robot based on the joint angular velocity command A and the zero space matrix;
A robot control apparatus comprising: a control unit that controls a servo motor based on a third joint angular velocity command that is a sum of the first and second joint angular velocity commands.
　請求項１２記載のロボット制御装置において、
　前記ロボットの姿勢保持トルクを抑制するための関節角速度指令Ｂを生成する第２の最適化動作指令生成部を更に有するロボット制御装置。 The robot controller according to claim 12, wherein
The robot control apparatus which further has a 2nd optimization operation command generation part which produces | generates the joint angular velocity command B for suppressing the attitude | position holding torque of the said robot.
　冗長軸を含む複数の関節軸を有するロボットの制御点の速度指令に基づいて、運動エネルギーを低減するための前記ロボットの第１の関節角速度指令を生成するステップＳＡ１と、
　前記ロボットが、前記冗長軸の張り出しを、前記制御点の位置姿勢を維持したまま、冗長自由度を利用して抑える腕下げ動作を実行するための関節角速度指令Ａを生成するステップＳＡ２と、
　前記制御点に関するヤコビ行列のゼロ空間行列を計算するステップＳＡ３と、
　前記関節角速度指令Ａ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成するステップＳＡ４と、
　前記第１及び第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記各関節軸を駆動するサーボモータを制御し、前記ロボットが前記腕下げ動作を実行するステップＳＡ５と、を実行するロボットシステムの制御方法。 Generating a first joint angular velocity command of the robot for reducing kinetic energy based on a velocity command of a control point of a robot having a plurality of joint axes including redundant axes;
Step SA2 for generating a joint angular velocity command A for performing an arm lowering operation in which the robot suppresses the extension of the redundant axis by using the redundancy degree of freedom while maintaining the position and orientation of the control point;
Calculating SA3 of the Jacobian matrix for the control points;
Generating a second joint angular velocity command for the robot based on the joint angular velocity command A and the zero-space matrix; and
Step SA5 in which a servo motor for driving each joint axis is controlled based on a third joint angular velocity command that is the sum of the first and second joint angular velocity commands, and the robot performs the arm lowering operation; , Execute the robot system control method.
　請求項１４記載のロボットシステムの制御方法において、
　前記ステップＳＡ５を実行した後、
　前記第１の関節角速度指令を生成するステップＳＢ１と、
　前記ロボットの姿勢保持トルクを抑制するための関節角速度指令Ｂを生成するステップＳＢ２と、
　前記ゼロ空間行列を計算するステップＳＢ３と、
　前記関節角速度指令Ｂ及び前記ゼロ空間行列に基づいて、前記ロボットの第２の関節角速度指令を生成するステップＳＢ４と、
　前記ステップＳＢ１で生成された前記第１の関節角速度指令及び前記ステップＳＢ４で生成された前記第２の関節角速度指令の和である第３の関節角速度指令に基づいて、前記各関節軸を駆動するサーボモータを制御し、前記ロボットが、姿勢保持に必要なトルクを抑制するステップＳＢ５と、を更に実行するロボットシステムの制御方法。
The method of controlling a robot system according to claim 14,
After executing step SA5,
Step SB1 for generating the first joint angular velocity command;
Step SB2 for generating a joint angular velocity command B for suppressing the posture holding torque of the robot;
Calculating step SB3 of the zero-space matrix;
Generating a second joint angular velocity command for the robot based on the joint angular velocity command B and the zero-space matrix;
Each joint axis is driven based on a third joint angular velocity command that is the sum of the first joint angular velocity command generated in step SB1 and the second joint angular velocity command generated in step SB4. A control method for a robot system, further comprising a step SB5 of controlling a servo motor so that the robot suppresses a torque necessary for maintaining a posture.