JP2015178156A

JP2015178156A - Vertical articulated robot position error suppression method

Info

Publication number: JP2015178156A
Application number: JP2014056604A
Authority: JP
Inventors: 大介川瀬; Daisuke Kawase
Original assignee: Denso Wave Inc
Current assignee: Denso Wave Inc
Priority date: 2014-03-19
Filing date: 2014-03-19
Publication date: 2015-10-08
Anticipated expiration: 2034-03-19
Also published as: JP6252272B2

Abstract

PROBLEM TO BE SOLVED: To improve position controllability over a vertical articulated robot irrespective of the shape of an operation track and free from orbital errors and irregular speeds.SOLUTION: A vertical articulated robot configured so that a motor 15 driving a third link 9 is provided near a rotation center of a second link 8 and this motor 15 is connected to a gear speed reduction device 16 by a belt transmission mechanism 20, calculates a correction amount of the motor 15 for a second rotation joint Lc-2 on the basis of a difference between a position command to the motor 15 for a third rotation joint Lc-3 and a current position detected by an angle sensor detecting a rotation angle of the motor 15, and corrects a position of the third link 9 by adding the correction amount to a next position command to the motor 15 for the second rotation joint Lc-2.

Description

本発明は垂直多関節型ロボットにおいて、上下方向に回転動作する第３のリンクの位置誤差を抑制する方法に関する。 The present invention relates to a method for suppressing a position error of a third link that rotates in a vertical direction in a vertical articulated robot.

例えば６軸の垂直多関節型ロボットは、床面に固定されるベース（ベースリンク）にショルダ（第１のリンク）を水平方向に回転可能に連結し、ショルダに下アーム（第２のリンク）を上下方向に回転可能に連結し、下アームの先端部に第１の上アーム（第３のリンク）を上下方向に回転可能に連結し、そして第１の上アームに第２の上アーム、手首アーム、フランジを順に回転可能に連結した構造になっている。 For example, in a 6-axis vertical articulated robot, a shoulder (first link) is rotatably connected to a base (base link) fixed to the floor surface, and a lower arm (second link) is connected to the shoulder. And a first upper arm (third link) rotatably connected to the tip of the lower arm, and a second upper arm connected to the first upper arm, The wrist arm and flange are connected in order to be rotatable.

小形の６軸垂直多関節型ロボットにおいては、産業用ロボットとして使用された場合の生産性向上の観点から高速化が要求され、また作業性向上の観点からアーム幅の狭小化が望まれている。そのために、下アームを駆動する第２軸モータにかかる慣性モーメントを小さくすることにより高速化を図り、下アーム内に設けられる第３軸モータを小型にすることによりアーム幅の狭小化を図っている。 In a small 6-axis vertical articulated robot, high speed is required from the viewpoint of improving productivity when used as an industrial robot, and reduction of the arm width is desired from the viewpoint of improving workability. . To that end, the moment of inertia applied to the second axis motor that drives the lower arm is reduced to increase the speed, and the third axis motor provided in the lower arm is reduced in size to reduce the arm width. Yes.

具体的には、重量物である第３軸モータを下アームの回転中心側（根元側である回転中心側）に配置し、第３軸モータの回転をベルト伝導機構によって第３軸に伝達する構造を採用することにより、下アームの慣性モーメントを小さくし、また、第３軸モータとして体積に対して大出力の小型モータを採用することにより、下アームの幅を狭小化するようにしている。 Specifically, the third shaft motor, which is a heavy object, is arranged on the rotation center side of the lower arm (the rotation center side that is the base side), and the rotation of the third axis motor is transmitted to the third shaft by the belt transmission mechanism. By adopting the structure, the moment of inertia of the lower arm is reduced, and by adopting a small motor with a large output relative to the volume as the third axis motor, the width of the lower arm is reduced. .

最大可搬質量が５Ｋｇ程度の小型の６軸垂直多関節型ロボットでは、第３軸モータとして出力１００Ｗクラスのサーボモータが使用されることが多い。一般的に産業用サーボモータの最小サイズは対角４０ｍｍ程度の角形で、出力１００Ｗ程度である。このクラスのモータは、小型・高出力を主眼とした設計がなされている。そのため、コギングトルクなどのトルク脈動が他クラスのモータに比べて大きく出る傾向にある。 In a small 6-axis vertical articulated robot with a maximum payload of about 5 kg, a servo motor with a 100 W output is often used as the third axis motor. Generally, the minimum size of an industrial servomotor is a square with a diagonal of about 40 mm and an output of about 100 W. This class of motors is designed mainly for small size and high output. For this reason, torque pulsation such as cogging torque tends to be larger than other class motors.

また、第３軸モータの回転は、ベルトによって第３軸（第１の上アーム）に伝達されるため、モータに制御電流を印加してからモータ位置応答までの伝達特性には位相遅れが現れる。そのため、フィードバック制御系が不安定になり易く、フィードバックゲインを大きくすることができない。 Further, since the rotation of the third axis motor is transmitted to the third axis (first upper arm) by the belt, a phase delay appears in the transmission characteristics from the application of the control current to the motor until the motor position response. . Therefore, the feedback control system tends to become unstable, and the feedback gain cannot be increased.

以上により、小形の６軸垂直多関節型ロボットの第１の上アーム（第３軸）は、他と比べて外乱の影響を受け易いにもかかわらず、その外乱を制御によって抑制することが困難である（制御性が低い）という問題を生じていた。 As described above, the first upper arm (third axis) of the small 6-axis vertical articulated robot is more susceptible to the disturbance than others, but it is difficult to suppress the disturbance by control. (The controllability is low).

産業用多関節形ロボットアームの相互位置同期制御を図った高精度輪郭制御法日本ロボット学会誌Ｖｏｌ．１５Ｎｏ．５，ｐ７３６−７４３，１９９７年発行Highly accurate contour control method for mutual position synchronization control of industrial articulated robot arm Journal of the Robotics Society of Japan Vol. 15 No. 5, p 733-743, issued in 1997 多関節ロボットアームにおける外乱対策を図った主軸従軸切替え位置同期制御法電気学会論文誌ＣＶｏｌ．１２１，Ｎｏ．１，ｐ８３−８９，２００１年発行Spindle-slave shaft switching position synchronous control method for disturbance countermeasures in articulated robot arms IEEJ Transactions C Vol. 121, no. 1, p83-89, published in 2001

非特許文献１は、ロボットの直線軌道動作に対して、直線からのずれ成分を三次元位置座標上でフィードバックするというものである。しかしながら、この非特許文献１の制御法では、第３軸だけを対象にして特別に計算することはできない。また、ある程度の時間サンプル分は直線軌道動作をしなければ位置誤差の抑制ができないため、毎サンプル動作方向の変わる円弧軌道動作やスプライン軌道動作といった曲線状軌跡動作の場合には、この制御法を適用することはできない。つまり、特定の軸に対する位置ずれを補正する計算ができないのである。 Non-Patent Document 1 is to feed back a deviation component from a straight line on a three-dimensional position coordinate with respect to a linear trajectory operation of a robot. However, according to the control method of Non-Patent Document 1, it is not possible to calculate specifically for only the third axis. In addition, since a position error cannot be suppressed unless a linear trajectory operation is performed for a certain amount of time samples, this control method is used in the case of a curved trajectory motion such as an arc trajectory motion or a spline trajectory motion in which the motion direction of each sample changes. It cannot be applied. That is, the calculation for correcting the positional deviation with respect to a specific axis cannot be performed.

非特許文献２は、主軸とした軸（本発明の場合の第３の回転関節の回転軸）の動作進捗具合に応じて、従軸とした別の軸（本発明の場合の第２の回転関節の回転軸）の動作指令を決定する方法である。この方法では、主軸がほとんど動かずに従軸が大きく動く場合に、主軸に僅かな振動が発生したとしても、従軸に大きく振動する指令値が送られる。主軸の動作がゼロの場合には、僅かな振動でも、この振動の振幅は無限大となり、異常な指令値としてシステムの停止を招く。また、振幅が無限大となる程でなくても、従軸を第２の回転関節の回転軸のみとした場合では、主軸および従軸以外とのバランスが崩れ、これによる軌跡誤差が発生する。更に、従軸を第３の回転関節の回転軸以外とした場合には、速度むらとして現れる。 Non-Patent Document 2 describes another axis (second rotation in the case of the present invention) as a slave axis in accordance with the progress of the operation of the axis (the rotation axis of the third rotary joint in the case of the present invention) as the main axis. This is a method for determining an operation command of a joint rotation axis. In this method, when the main shaft hardly moves and the driven shaft moves largely, even if a slight vibration occurs in the main shaft, a command value that greatly vibrates is sent to the driven shaft. When the operation of the main shaft is zero, even a slight vibration causes the amplitude of the vibration to be infinite, causing the system to stop as an abnormal command value. Even if the amplitude is not infinite, when the slave shaft is only the rotary shaft of the second rotary joint, the balance between the main shaft and the slave shaft is lost, and a trajectory error is caused thereby. Further, when the slave shaft is other than the rotation shaft of the third rotary joint, it appears as uneven speed.

本発明は上記した事情に鑑みてなされたもので、その目的は、動作軌跡の形状に関係なく、また、軌道誤差や速度むらが現れることなく制御性を高めることができる垂直多関節型ロボットの位置誤差抑制方法を提供することにある。 The present invention has been made in view of the above-described circumstances, and the purpose thereof is a vertical articulated robot that can enhance controllability regardless of the shape of the motion trajectory and without causing trajectory error or speed unevenness. It is to provide a position error suppressing method.

請求項１の発明は、第３のリンクの駆動モータに対する位置指令と、当該駆動モータの回転角度を検出する角度センサにより検出される現在位置との差分を位置誤差とし、この位置誤差を基に、第２のリンクの駆動モータの補正量を算出し、当該補正量を前記第２のリンクの駆動モータに対する次の位置指令に加算することにより、第３のリンクの位置を補正するので、どのような軌道形状であっても、第２のリンクの位置補正により第２のリンクの位置誤差を抑制できると共に、当該第２のリンクが大きく振動する恐れがなく、軌道誤差や速度むらが現れることを極力抑制できる。 According to the first aspect of the present invention, the difference between the position command for the drive motor of the third link and the current position detected by the angle sensor for detecting the rotation angle of the drive motor is used as a position error. The position of the third link is corrected by calculating the correction amount of the drive motor of the second link and adding the correction amount to the next position command for the drive motor of the second link. Even with such a trajectory shape, the position error of the second link can be suppressed by correcting the position of the second link, and the second link is not likely to vibrate greatly, resulting in a trajectory error and uneven speed. Can be suppressed as much as possible.

請求項２の発明は、第３のリンクの先端の速度から、当該第３のリンクの先端の位置指令から求められる移動方向以外の位置誤差を求め、この位置誤差から前記第２のリンクの駆動モータの補正量を算出し、当該補正量を第２のリンクの駆動モータに対する次の位置指令に加算して第２のリンクを動作させるので、目標とする軌道から外れないようにしながら第３のリンクの位置誤差を抑制できる。 According to the second aspect of the present invention, a position error other than the moving direction obtained from the position command of the tip of the third link is obtained from the speed of the tip of the third link, and the driving of the second link is obtained from this position error. The correction amount of the motor is calculated, and the correction amount is added to the next position command for the driving motor of the second link to operate the second link. Link position error can be suppressed.

本発明の第１の実施形態を示す要部の概略的な側面図The schematic side view of the principal part which shows the 1st Embodiment of this invention アームのある瞬間での位置を示すスケルトンSkeleton showing the position of the arm at a certain moment ロボット装置の全体を示す斜視図The perspective view which shows the whole robot apparatus 回転関節の概略構成を示す断面図Sectional drawing which shows schematic structure of rotation joint ロボットの座標系を示す斜視図Perspective view showing the coordinate system of the robot 制御装置の電気的構成を示すブロック図Block diagram showing electrical configuration of control device

以下に本発明の実施形態を図面に基づいて説明する。
［第１の実施形態］
図面は本発明の第１の実施形態を示すもので、図３には、６軸の垂直多回転関節型ロボット１が示されている。このロボット１は、６軸垂直多関節型ロボット本体２と、ロボット本体２の動作を制御する制御装置（ロボット制御手段）３と、制御装置３に接続されてロボット本体２を手動操作によって動作させるためのティーチングペンダント（手動操作手段）４とを備えている。 Embodiments of the present invention will be described below with reference to the drawings.
[First Embodiment]
The drawing shows a first embodiment of the present invention, and FIG. 3 shows a six-axis vertical multi-rotation articulated robot 1. The robot 1 includes a 6-axis vertical articulated robot main body 2, a control device (robot control means) 3 that controls the operation of the robot main body 2, and a robot main body 2 that is connected to the control device 3 and operates by manual operation. A teaching pendant (manual operation means) 4 is provided.

ロボット本体２は、工場の床面などの据え付け面に固定されるベース（ベースリンク）５に対し、ロボットアーム６を構成するショルダ（第の１リンク）７、下アーム（第２のリンク）８、第１の上アーム（第３のリンク）９、第２の上アーム（第４のリンク）１０と、手首アーム（第５のリンク）１１、フランジ（第６のリンク）１２を第１〜第６の各回転関節Ｊ１〜Ｊ６（図３には図示せず）により順次連結して構成されている。 The robot body 2 has a shoulder (first link) 7 and a lower arm (second link) 8 constituting a robot arm 6 with respect to a base (base link) 5 fixed to an installation surface such as a factory floor. First upper arm (third link) 9, second upper arm (fourth link) 10, wrist arm (fifth link) 11, flange (sixth link) 12 The sixth rotary joints J1 to J6 (not shown in FIG. 3) are sequentially connected.

つまり、ショルダ７はベース５上に第１回転関節Ｊ１によって第１軸Ｌｃ−１を中心に水平方向に回転可能に連結され、下アーム８はショルダ７の先端部に第２回転関節Ｊ２によって第２軸Ｌｃ−２を中心に上下（鉛直）方向に回転可能に支持され、第１の上アーム９は下アーム８の先端部に第３回転関節Ｊ３によって第３軸Ｌｃ−３を中心に上下方向に回転可能に支持され、第２の上アーム１０は第１の上アーム９の先端部に第４回転関節Ｊ４によって第４軸Ｌｃ−４を中心に捻り回転可能に支持され、手首アーム１１は第２の上アーム１０の先端部に第５回転関節Ｊ５によって第５軸Ｌｃ−５を中心に上下方向に回転可能に支持され、フランジ１２は手首アーム１１に第６回転関節Ｊ６によって第６軸Ｌｃ−６を中心に捻り回転可能に支持されている。 That is, the shoulder 7 is connected to the base 5 by the first rotary joint J1 so as to be horizontally rotatable about the first axis Lc-1, and the lower arm 8 is connected to the tip of the shoulder 7 by the second rotary joint J2. The first upper arm 9 is supported so as to be rotatable in the vertical (vertical) direction around the two axes Lc-2. The first upper arm 9 is vertically moved around the third axis Lc-3 by the third rotary joint J3 at the tip of the lower arm 8. The second upper arm 10 is supported at the tip of the first upper arm 9 so as to be rotatable about the fourth axis Lc-4 by the fourth rotary joint J4. Is supported at the tip of the second upper arm 10 by a fifth rotary joint J5 so as to be rotatable in the vertical direction about the fifth axis Lc-5, and the flange 12 is supported on the wrist arm 11 by a sixth rotary joint J6. Supports rotation by twisting around the axis Lc-6 It is.

前記第１〜第６の各回転関節Ｊ１〜Ｊ６は、図４にその概略が示されている。図４に示す通り、各リンク７〜１２の回転関節Ｊ１〜Ｊ６は、前段のリンクに軸受１３を介して回転可能に支持された回転軸１４からなり、この回転軸１４に次段のリンクが直接或いは継ぎ手材を介して連結される。回転軸１４は、前段のリンクに固定された駆動源としてサーボモータ１５に減速装置、例えば歯車減速装置１６を介して回転駆動され、この回転軸１４の回転によって次段のリンクがせんかい回転動作或いは捻り回転動作を行うようになっている。なお、前記第１軸Ｌｃ−１〜第６軸Ｌｃ−６は、第１〜第６の各回転関節Ｊ１〜Ｊ６の回転軸の回転中心線Ｌｃに相当するものである。 The outline of the first to sixth rotary joints J1 to J6 is shown in FIG. As shown in FIG. 4, the rotary joints J <b> 1 to J <b> 6 of each of the links 7 to 12 include a rotary shaft 14 that is rotatably supported by a previous-stage link via a bearing 13, and the next-stage link is connected to the rotary shaft 14. They are connected directly or via a joint material. The rotary shaft 14 is rotationally driven by a servo motor 15 as a drive source fixed to the previous link via a speed reducer, for example, a gear speed reducer 16, and the rotation of the rotary shaft 14 causes the next link to rotate. Alternatively, a twisting rotation operation is performed. The first axis Lc-1 to the sixth axis Lc-6 correspond to the rotation center line Lc of the rotation axes of the first to sixth rotary joints J1 to J6.

第３軸Ｌｃ−３を除き、サーボモータ１５と歯車減速装置１６とは一体化され、歯車減速装置１６が回転軸１４に直接連結されている。第３軸Ｌｃ−３については、図１に示すように、サーボモータ１５と歯車減速装置１６とは分離され、サーボモータ１５が下アーム８の回転中心側、つまり第２軸Ｌｃ−２側に位置して設けられ、歯車減速装置１６が第３軸Ｌｃ−３に連結されている。そして、サーボモータ１５と歯車減速装置１６との間が歯付きプーリ１７，１８間に歯付きベルト１９を掛け渡してなるベルト伝動機構２０により連結され、サーボモータ１５の回転がベルト伝動機構２０を介して歯車減速装置１６に伝達されるようになっている。 Except for the third axis Lc-3, the servo motor 15 and the gear reduction device 16 are integrated, and the gear reduction device 16 is directly connected to the rotary shaft 14. As for the third axis Lc-3, as shown in FIG. 1, the servo motor 15 and the gear reduction device 16 are separated, and the servo motor 15 is located on the rotation center side of the lower arm 8, that is, on the second axis Lc-2 side. The gear reduction device 16 is connected to the third shaft Lc-3. The servo motor 15 and the gear reduction device 16 are connected by a belt transmission mechanism 20 in which a toothed belt 19 is stretched between the toothed pulleys 17 and 18, and the rotation of the servo motor 15 causes the belt transmission mechanism 20 to rotate. Via the gear reduction device 16.

ショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２には、図５に示すように、それぞれ３次元の座標Ｒ１〜Ｒ６が規定されている。これらの座標Ｒ１〜Ｒ６の原点Ｏ１〜Ｏ６は、ショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転中心線である第１軸Ｌｃ−1〜第６軸Ｌｃ−６上の所定位置に定められ、各座標Ｒ１〜Ｒ６のＺ軸であるＺ１〜Ｚ６軸は第１軸Ｌｃ−1〜第６軸Ｌｃ−６と一致している。 As shown in FIG. 5, three-dimensional coordinates R1 to R6 are defined for the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12, respectively. . The origins O1 to O6 of these coordinates R1 to R6 are the first axis Lc that is the rotation center line of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12. -1 to the sixth axis Lc-6 are determined at predetermined positions, and the Z1 to Z6 axes that are the Z axes of the coordinates R1 to R6 coincide with the first axis Lc-1 to the sixth axis Lc-6. .

ベース５およびロボットアーム６の先端であるフランジ１２の先端面にも座標Ｒ０およびＲｆが規定されている。上記座標Ｒ１〜Ｒ６がショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転に伴って位置と向きが変化するのに対して、ベース５の座標Ｒ０は不動であり、ロボット座標とされている。このロボット座標Ｒ０の原点Ｏ０は、ショルダ７の回転中心線である第１軸Ｌｃ−１上にあって本実施形態では座標Ｒ１の原点Ｏ１と一致した位置に定められている。また、Ｚ軸であるＺ０軸は第１軸Ｌｃ−１に一致し、Ｘ軸であるＸ０軸およびＹ軸であるＹ０軸は、ショルダ７が原点位置にあるときの当該ショルダ７の座標Ｒ１のＸ１軸およびＹ１軸に一致している。 Coordinates R0 and Rf are also defined on the tip surface of the flange 12 which is the tip of the base 5 and the robot arm 6. While the coordinates R1 to R6 change positions and orientations with the rotation of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12, the base A coordinate R0 of 5 is immobile and is a robot coordinate. The origin O0 of the robot coordinate R0 is located on the first axis Lc-1 that is the rotation center line of the shoulder 7, and in this embodiment is determined at a position that coincides with the origin O1 of the coordinate R1. Further, the Z0 axis that is the Z axis coincides with the first axis Lc-1, and the X0 axis that is the X axis and the Y0 axis that is the Y axis are the coordinates R1 of the shoulder 7 when the shoulder 7 is at the origin position. It corresponds to the X1 axis and the Y1 axis.

フランジ１２の先端面の座標Ｒｆは手先座標とされ、その原点Ｏｆは、第６軸Ｌｃ−６とフランジ１２の先端面との交点、つまり、フランジ１２の円形をなす先端面の中心と一致するように定められている。そして、フランジ１２の先端面の中心がロボットアーム６の先端である手先に定められている。 The coordinate Rf of the front end surface of the flange 12 is the hand coordinate, and the origin Of thereof coincides with the intersection of the sixth axis Lc-6 and the front end surface of the flange 12, that is, the center of the front end surface forming the circle of the flange 12. It is prescribed as follows. The center of the front end surface of the flange 12 is set to the hand that is the front end of the robot arm 6.

以上の座標Ｒ０〜Ｒ６，Ｒｆの原点Ｏ０〜Ｏ６，Ｏｆは、本実施形態においては、座標Ｒ１のＸ１軸とＺ１軸を含む一つの平面（鉛直面）を仮定したとき、この一平面上に位置されている。 In the present embodiment, the origins O0 to O6 and Of of the coordinates R0 to R6 and Rf described above are on a single plane (vertical plane) including the X1 axis and the Z1 axis of the coordinate R1. Is located.

前記制御装置３は、図６に示すように、ＣＰＵ（制御手段）２１と、駆動回路（駆動手段）２２と、位置検出回路（位置検出手段）２３とを備えている。ＣＰＵ２１には、ロボット本体２全体のシステムプログラムや動作プログラムを作成するためのロボット言語やＤＨパラメータなどの各種データなどを記憶するＲＯＭ２４およびロボット本体２の動作プログラムを記憶するＲＡＭ２５が接続されていると共に、ティーチング作業などを行う際に使用する前記ティーチングペンダント４が接続されている。 As shown in FIG. 6, the control device 3 includes a CPU (control means) 21, a drive circuit (drive means) 22, and a position detection circuit (position detection means) 23. Connected to the CPU 21 are a ROM 24 for storing various data such as a robot language and DH parameters for creating a system program and an operation program for the entire robot body 2, and a RAM 25 for storing an operation program for the robot body 2. The teaching pendant 4 used when performing teaching work or the like is connected.

位置検出回路２３には、各サーボモータ１５の回転軸（図示せず）に連結された回転センサとしてのロータリエンコーダ２６が接続されている。位置検出回路２３は、各ロータリーエンコーダ２６の回転角検出情報に基づいてショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角度を検出し、その回転角検出情報をＣＰＵ２１に与える。 A rotary encoder 26 as a rotation sensor connected to a rotation shaft (not shown) of each servo motor 15 is connected to the position detection circuit 23. The position detection circuit 23 detects the rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, and the flange 12 based on the rotation angle detection information of each rotary encoder 26. Then, the rotation angle detection information is given to the CPU 21.

ここで、ＣＰＵ２１は、ロボット座標Ｒ０上での手先の位置（手先座標Ｒｆの原点Ｏｆの位置）と姿勢（手先座標ＲｆのＸｆ軸、Ｙｆ軸の向き）が与えられると、手先がその位置と姿勢をとるようなショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角度を、ヤコビ行列を用いた逆運動学の周知の計算手法によって求める。また、逆に、ショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角度が与えられると、それら回転角度から座標変換を用いた順運動学の周知の計算手法によってロボット座標Ｒ０上での手先の位置と姿勢を求めることができるように構成されている。これらの逆運動学および順運動学の計算手法による演算には、周知のように各座標Ｒ０〜Ｒ６，Ｒｆの相互の関係を示す前記ＤＨパラメータが用いられる。 Here, the CPU 21 is given the position of the hand on the robot coordinate R0 (the position of the origin Of of the hand coordinate Rf) and the posture (the directions of the Xf axis and the Yf axis of the hand coordinate Rf), and the hand is moved to the position. A well-known calculation method of inverse kinematics using a Jacobian matrix is used to calculate the rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 that take a posture. Ask for. Conversely, when the rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are given, the order in which coordinate conversion is used from these rotation angles. The position and posture of the hand on the robot coordinate R0 can be obtained by a well-known calculation method of kinematics. As is well known, the DH parameter indicating the mutual relationship between the coordinates R0 to R6 and Rf is used for the calculation by the inverse kinematics and forward kinematics calculation methods.

そして、ＣＰＵ２１は、動作プログラムから一定時間毎（サンプリングタイム）に手先の目標位置を取得し、その手先の目標位置からショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の目標動作角度を演算し、その目標動作角度を指令位置とする位置指令を駆動回路２２に与える。駆動回路２２は与えられた位置指令に基づいて各サーボモータ１５を駆動する。このとき、ＣＰＵ２１は、位置検出回路２３から与えられるショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角検出情報をフィードバック情報として各サーボモータ１５をフィードバック制御する。 Then, the CPU 21 acquires the target position of the hand from the operation program at regular intervals (sampling time), and from the target position of the hand, the shoulder 7, the lower arm 8, the first upper arm 9, and the second upper arm 10 are obtained. The target operation angle of the wrist arm 11 and the flange 12 is calculated, and a position command with the target operation angle as a command position is given to the drive circuit 22. The drive circuit 22 drives each servo motor 15 based on the given position command. At this time, the CPU 21 uses the rotation angle detection information of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 given from the position detection circuit 23 as feedback information. The servo motor 15 is feedback controlled.

ところで、第３軸Ｌｃ‐３を駆動する駆動装置、即ちサーボモータ１５と歯車減速装置１６とは離間して設けられ、両者間はベルト伝動機構２０によって連結されている。以下の説明では、この第３軸Ｌｃ‐３のサーボモータ１５と歯車減速装置１６を他の軸のものと区別する場合、符号１５，１６の次に「‐３」を付け加え示すこととする。
比較的重量のあるサーボモータ１５‐３を下アーム８の回転中心側（第２軸Ｌｃ−２側）に配置した理由は、当該下アーム８の慣性モーメントを小さくし、下アーム８の高速化を図るためである。 By the way, the driving device for driving the third shaft Lc-3, that is, the servo motor 15 and the gear reduction device 16 are provided apart from each other, and both are connected by the belt transmission mechanism 20. In the following description, in order to distinguish the servo motor 15 and the gear reduction device 16 of the third axis Lc-3 from those of the other axes, “−3” is added after the reference numerals 15 and 16.
The reason why the relatively heavy servo motor 15-3 is arranged on the rotation center side (second axis Lc-2 side) of the lower arm 8 is that the moment of inertia of the lower arm 8 is reduced and the speed of the lower arm 8 is increased. It is for aiming at.

サーボモータ１５‐３は下アーム８の慣性モーメントの更なる減少化を図るために小型軽量であることを要求される半面、第１の上アーム９以降のリンク９〜１２を１本のアームと見たとき、そのアームの動作の高速化を図るために高出力化が必要である。そこで、本実施形態のロボット本体２が最大可搬質量５Ｋｇ以下の小型ロボットであるとした場合、サーボモータ１５‐３としては、一般に、対角が４０ｍｍ、出力が１００Ｗクラスの角型モータが採用される。この小形高出力のサーボモータ１５‐３では、他クラスのモータに比べコギングなどのトルク脈動が大きく出る傾向にある。 The servo motor 15-3 is required to be small and light in order to further reduce the moment of inertia of the lower arm 8. On the other hand, the links 9 to 12 after the first upper arm 9 are connected to one arm. When viewed, higher output is required to speed up the operation of the arm. Therefore, assuming that the robot body 2 of the present embodiment is a small robot having a maximum payload of 5 kg or less, a servo motor 15-3 is generally a square motor having a diagonal of 40 mm and an output of 100 W class. Is done. In this small high-power servomotor 15-3, torque pulsation such as cogging tends to occur more than other class motors.

一方、ベルト伝動機構２０の歯付きベルト１９は弾性を有し、伸縮する。このため、サーボモータ１５に電流が流されて当該サーボモータ１５が駆動力を発すると、歯付きベルト１９は伸び、直後に縮むという挙動を呈する。
このトルク脈動が大きく出る傾向にあること、および歯付きベルト１９が伸縮し易いことから、駆動回路２２が与えられた位置指令に基づいてサーボモータ１５‐３を駆動したとき、位置検出回路２３から与えられる第１の上アーム９の回転角検出情報は位相遅れが現れる。そのため、フィードバック制御が不安定になり易く、フィードバックゲインを大きくすることができない。 On the other hand, the toothed belt 19 of the belt transmission mechanism 20 has elasticity and expands and contracts. For this reason, when a current is supplied to the servo motor 15 and the servo motor 15 generates a driving force, the toothed belt 19 extends and contracts immediately thereafter.
Since this torque pulsation tends to increase and the toothed belt 19 is easily expanded and contracted, when the drive circuit 22 drives the servo motor 15-3 based on the given position command, the position detection circuit 23 In the given rotation angle detection information of the first upper arm 9, a phase delay appears. Therefore, the feedback control is likely to be unstable, and the feedback gain cannot be increased.

以上により、第１の上アーム９は外乱の影響を受け易い。本発明は、この外乱による第１の上アーム９の位置誤差を小さくしようとするものである。そのために本発明は、位置指令で出されて指令位置とロータリーエンコーダ２６により検出されるサーボモータ１５‐３の回転角度から求められる第３軸Ｌｃ−３の現在位置（角度）との差分を位置誤差とし、この位置誤差を基に、第２軸Ｌｃ−２の補正量を計算し、この補正量を第２軸Ｌｃ−２の位置指令（角度）に加算するものである。 As described above, the first upper arm 9 is easily affected by disturbance. The present invention is intended to reduce the position error of the first upper arm 9 due to this disturbance. Therefore, the present invention determines the difference between the command position and the current position (angle) of the third axis Lc-3 obtained from the command position and the rotation angle of the servo motor 15-3 detected by the rotary encoder 26. Based on this position error, the correction amount of the second axis Lc-2 is calculated, and this correction amount is added to the position command (angle) of the second axis Lc-2.

下アーム８の補正量は以下の方法により導出される。
いま、ある位置指令によりモータ１５が動作したとする。このとき、図２に示すように、ロータリーエンコーダ２６により検出された下アーム８の現在角度がθ_２、第１の上アーム９の現在角度がθ_３であったとする。そして、指令位置に対する第１の上アーム９の位置誤差をｄ_３、下アーム８の位置指令に対する補正量をｄ_２とする。なお、補正量ｄ_２は、下アーム８の次の位置指令に当該補正量ｄ_２を加えれば第１の上アーム９の位置誤差ｄ_３が０になる量をいう。 The correction amount of the lower arm 8 is derived by the following method.
Assume that the motor 15 is operated by a certain position command. At this time, it is assumed that the current angle of the lower arm 8 detected by the rotary encoder 26 is θ ₂ and the current angle of the first upper arm 9 is θ _{3 as shown} in FIG. The position error of the first upper arm 9 relative to the command position is d ₃ , and the correction amount for the position command of the lower arm 8 is d ₂ . The correction amount d ₂ is an amount by which the position error d _{3 of} the first upper arm 9 becomes 0 when the correction amount d ₂ is added to the next position command of the lower arm 8.

以下において、
下アーム８のヤコビアン：Ｊ_２＝（Ｊ_２ｘ，Ｊ_２ｙ，Ｊ_２ｚ）
第１の上アーム９のヤコビアン：Ｊ_３＝（Ｊ_３ｘ，Ｊ_３ｙ，Ｊ_３ｚ）
第１の上アーム９の先端Ｏ５の位置誤差：ｅ＝（ｅ_ｘ，ｅ_ｙ，ｅ_ｚ）＝Ｊ_２ｄ_２＋Ｊ_３ｄ_３
第１の上アーム９の先端Ｏ５の位置誤差の絶対値：Ｅ
下アーム８のアーム長（Ｏ２からＯ３までの長さ）：Ｌ１
第１の上アーム９のアーム長（Ｏ３からＯ５までの長さ）：Ｌ２
とする。ここで、第２の上アーム１０は第１の上アーム９と上下方向の回転に関して一体に回転するので、第１の上アーム１０の先端をＯ５点と定めている。 In the following,
Jacobian of the lower arm 8: J ₂ = (J _2x , J _2y , J _2z )
Jacobian of the first upper arm 9: J ₃ = (J _3x , J _3y , J _3z )
Position error of the tip O5 of the first upper arm 9: e = (e _x , e _y , e _z ) = J ₂ d ₂ + J ₃ d ₃
Absolute value of position error of tip O5 of first upper arm 9: E
Arm length of lower arm 8 (length from O2 to O3): L1
Arm length of first upper arm 9 (length from O3 to O5): L2
And Here, since the second upper arm 10 rotates integrally with the first upper arm 9 in the vertical direction, the tip of the first upper arm 10 is defined as the point O5.

位置誤差の絶対値Ｅを最小化するように第２軸Ｌｃ−２の位置指令を補正する。この場合の第２軸Ｌｃ−２の補正量ｄ_２は以下のように求まる。
まずＯ５の位置誤差ｅは、ｅ＝（ｅ_ｘ，ｅ_ｙ，ｅ_ｚ）＝Ｊ_２ｄ_２＋Ｊ_３ｄ_３
位置誤差の絶対値Ｅは、Ｅ＝｜ｅ｜＝（ｅ_ｘ ^２＋ｅ_ｙ ^２＋ｅ_ｚ ^２）^１／２、であるから、
Ｅ^２＝（Ｊ_２ｄ_２＋Ｊ_３ｄ_３）（Ｊ_２ｄ_２＋Ｊ_３ｄ_３）^Ｔ
＝（Ｊ_２ｘ ^２＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）ｄ_２ ^２＋２（Ｊ_２ｘＪ_３ｘ＋Ｊ_２ｙＪ_３ｙ＋
Ｊ_２ｚＪ_３ｚ）ｄ_２ｄ_３＋（Ｊ_３ｘ ^２＋Ｊ_３ｙ ^２＋Ｊ_３ｚ ^２）ｄ_３ ^２…（１）
この（１）式をｄ_２について偏微分すると、次の（２）式となる。
∂Ｅ^２／∂ｄ_２＝２（Ｊ_２ｘ ^２＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）ｄ_２＋２（Ｊ_２ｘＪ_３ｘ＋
Ｊ_２ｙＪ_３ｙ＋Ｊ_２ｚＪ_３ｚ）ｄ_３＝０…（２） The position command of the second axis Lc-2 is corrected so as to minimize the absolute value E of the position error. Correction amount d ₂ of the second axis Lc-2 in this case is determined as follows.
Position error e of the first O5 _{_{_{is, e = (e x, e}}} y, e z) = J 2 d 2 + J 3 d 3
Since the absolute value E of the position error is E = | e | = (e _x ² + e _y ² + e _z ² ) ^1/2 ,
E ² = (J ₂ d ₂ + J ₃ d ₃ ) (J ₂ d ₂ + J ₃ d ₃ ) ^T
= (J _2x ² + J _2y ² + J _2z ² ) d ₂ ² +2 (J _2x J _3x + J _2y J _3y +
J _2z J _3z ) d ₂ d ₃ + (J _3x ² + J _3y ² + J _3z ² ) d ₃ ² (1)
When this equation (1) is partially differentiated with respect to d ₂ , the following equation (2) is obtained.
^{_{_{^{∂E 2 / ∂d 2 = 2 (}}}} J 2x 2 + J 2y 2 + J 2z 2) d 2 +2 (J 2x J 3x +
J _2y J _3y + J _2z J _3z ) d ₃ = 0 (2)

そして、この（２）式からｄ_２を求める。
ｄ_２＝−（Ｊ_２ｘＪ_３ｘ＋Ｊ_２ｙＪ_３ｙ＋Ｊ_２ｚＪ_３ｚ）ｄ_３／（Ｊ_２ｘ ^２
＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）…（３）
ここで、Ｊ_２ｘは第２軸Ｌｃ−２が動作した際に、Ｏ５がＸ軸方向にどれだけ動くかを示す値であり、他のＪ_２ｙ，Ｊ_２ｚ，Ｊ_３ｘ，Ｊ_３ｙ，Ｊ_３ｚも同様の値を意味している。式（３）の分母は第２軸Ｌｃ−３が１ラジアン回転した際のＯ５の移動距離の２乗を示している。
つまり、円弧の長さ＝円弧の半径×円弧の中心角度、であるから、分母は第２軸Ｌｃ−２の回転中心Ｏ２からＯ５までの、第２軸Ｌｃ−２を法線とする２次元平面における距離の２乗に等しく、その値は常に０より大きい。 Then, a _{d 2} from the equation (2).
d ₂ = − (J _2x J _3x + J _2y J _3y + J _2z J _3z ) d ₃ / (J _2x ²
+ J _2y ² + J _2z ² ) (3)
Here, J _2x is a value indicating how much O5 moves in the X-axis direction when the second axis Lc-2 operates, and other J _2y , J _2z , J _3x , J _3y , J _3z. Means the same value. The denominator of Expression (3) indicates the square of the moving distance of O5 when the second axis Lc-3 rotates by 1 radian.
That is, since the length of the arc = the radius of the arc × the center angle of the arc, the denominator is a two-dimensional line having the second axis Lc-2 as a normal line from the rotation center O2 to O5 of the second axis Lc-2. It is equal to the square of the distance in the plane and its value is always greater than zero.

ヤコビアンＪを各アーム８，９の角度θ_２，θ_３とアーム長Ｌ１，Ｌ２から導出し、（３）式に代入すると、ｄ_２は次のようになる。
ｄ_２＝−（Ｊ_２ｘＪ_３ｘ＋Ｊ_２ｙＪ_３ｙ＋Ｊ_２ｚＪ_３ｚ）ｄ_３／（Ｊ_２ｘ ^２＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）
＝−［｛Ｌ１ｃｏｓθ_２＋Ｌ２ｃｏｓ（θ_２＋θ_３）｝Ｌ２ｃｏｓ（θ_２＋θ_３）
＋｛Ｌ１ｓｉｎθ_２＋Ｌ２ｓｉｎ（θ_２＋θ_３）｝Ｌ２ｓｉｎ（θ_２＋θ_３）｝ｄ_３］
／（Ｌ１^２＋Ｌ２^２＋２Ｌ１Ｌ２ｃｏｓθ_３）
＝−｛Ｌ１Ｌ２ｃｏｓθ_２ｃｏｓ（θ_２＋θ_３）＋Ｌ２^２ｃｏｓ^２（θ_２＋θ_３）
＋Ｌ１Ｌ２ｓｉｎθ_２ｓｉｎ（θ_２＋θ_３）＋Ｌ２^２ｓｉｎ^２（θ_２＋θ_３）｝ｄ_３
／（Ｌ１^２＋Ｌ２^２＋２Ｌ１Ｌ２ｃｏｓθ_３）
＝−（Ｌ２^２＋Ｌ１Ｌ２ｃｏｓθ_３）ｄ_３／（Ｌ１^２＋Ｌ２^２＋２Ｌ１Ｌ２ｃｏｓθ_３）
…（４） When the Jacobian J is derived from the angles θ ₂ and θ _{3 of the} arms 8 and 9 and the arm lengths L 1 and L ₂ and substituted into the equation (3), d ₂ is as follows.
d ₂ = − (J _2x J _3x + J _2y J _3y + J _2z J _3z ) d ₃ / (J _2x ² + J _2y ² + J _2z ² )
= − [{L1 cos θ ₂ + L2 cos (θ ₂ + θ ₃ )} L2 cos (θ ₂ + θ ₃ )
+ {L 1 sin θ ₂ + L ₂ sin (θ ₂ + θ ₃ )} L ₂ sin (θ ₂ + θ ₃ )} d ₃ ]
/ (L1 ² + L2 ² + 2L1L2 cos θ ₃ )
= − {L1L2 cos θ ₂ cos (θ ₂ + θ ₃ ) + L2 ² cos ² (θ ₂ + θ ₃ )
+ L1L2sin θ ₂ sin (θ ₂ + θ ₃ ) + L2 ² sin ² (θ ₂ + θ ₃ )} d ₃
/ (L1 ² + L2 ² + 2L1L2 cos θ ₃ )
=-(L2 ² + L1L2cosθ ₃ ) d ₃ / (L1 ² + L2 ² + 2L1L2cosθ ₃ )
... (4)

このように、第１の上アーム９の角度に応じて誤差に対する適切な補正量ｄ_２が変化する。この補正量ｄ_２を下アーム８の次のサンプリングタイム時の位置指令に加算し、この加算値を真の位置指令として駆動回路２２に出力する。これにより、第１の上アーム９が下アーム８により補正されてより正しい位置に動作する。 Thus, an appropriate correction amount d ₂ varies with respect to errors in accordance with the angle of the first upper arm 9. The correction amount d ₂ is added to the position command at the next sampling time the lower arm 8, and outputs the sum value to the drive circuit 22 as the true position command. As a result, the first upper arm 9 is corrected by the lower arm 8 and moved to a more correct position.

本実施形態によれば、外乱の影響を受けて制御し難い第１の上アーム９の位置誤差を、第１の上アーム９と同じ回転方向（上下方向）に回転する下アーム８の位置を補正することで抑制するので、比較的容易に第１の上アーム９をより正しい位置に動作させることができる。 According to the present embodiment, the position error of the first upper arm 9 that is difficult to control due to the influence of disturbance is set to the position of the lower arm 8 that rotates in the same rotational direction (vertical direction) as the first upper arm 9. Since it suppresses by correct | amending, the 1st upper arm 9 can be moved to a more correct position comparatively easily.

この第１の上アーム９の位置誤差の抑制は、Ｏ５が動作する軌道の形状に関係なく適用可能で、直線軌道動作であっても曲線状軌道動作であっても、より正しい位置に制御できる。 The suppression of the position error of the first upper arm 9 can be applied regardless of the shape of the trajectory on which the O5 operates, and can be controlled to a more correct position regardless of the linear trajectory motion or the curved trajectory motion. .

また、第１の上アーム９が振動したとしても、その振動を抑制するために、下アーム８の位置指令にはＯ４の位置誤差を修正する程度の補正値が加算されるだけであるから、下アーム８に大きく振動する指令値が与えられることはなく、システムの停止を招く事態を生ずる恐れがない。しかも、下アーム８および第１の上アーム９以外の他のリンクとのバランスが崩れる恐れもなく、速度むらが発生するおそれもなく、正常に動作させることができる。 Further, even if the first upper arm 9 vibrates, in order to suppress the vibration, only a correction value for correcting the position error of O4 is added to the position command of the lower arm 8. The lower arm 8 is not given a command value that vibrates greatly, and there is no possibility of causing a system stoppage. In addition, the balance with the links other than the lower arm 8 and the first upper arm 9 is not lost, and there is no risk of uneven speed, and the operation can be performed normally.

［第２の実施形態］
本発明の第２の実施形態は、アーム先端Ｏ５の移動速度から下アーム８の位置指令に対する補正値ｄ_２を求めるものである。移動速度は、前回のサンプリングタイム時のＯ５の位置と今回のサンプリングタイム時のＯ５の位置からＯ５の移動距離が求められるので、この移動距離を前回のサンプリングタイムと今回のサンプリングタイムとの時間間隔で除すことにより求めることができる。 [Second Embodiment]
Second embodiment of the present invention is to obtain a correction value d ₂ with respect to the position command of the lower arm 8 from the moving speed of the arm tip O5. Since the moving speed is obtained as the moving distance of O5 from the position of O5 at the previous sampling time and the position of O5 at the current sampling time, this moving distance is the time interval between the previous sampling time and the current sampling time. It can be obtained by dividing by.

移動方向に対する誤差は、軌道精度には影響しない。そのため、移動方向以外の位置誤差の抑制は軌道精度の観点から効果的である。但し、移動方向への誤差は速度むらとなるので注意が必要である。 The error with respect to the moving direction does not affect the trajectory accuracy. Therefore, suppression of position errors other than the moving direction is effective from the viewpoint of orbit accuracy. However, care must be taken because errors in the moving direction cause uneven speed.

以下において、
下アーム８のヤコビアン：Ｊ_２＝（Ｊ_２ｘ，Ｊ_２ｙ，Ｊ_２ｚ）
第１の上アーム９のヤコビアン：Ｊ_３＝（Ｊ_３ｘ，Ｊ_３ｙ，Ｊ_３ｚ）
第１の上アーム９の先端Ｏ５の位置誤差：ｅ＝（ｅ_ｘ，ｅ_ｙ，ｅ_ｚ）＝Ｊ_２ｄ_２＋Ｊ_３ｄ_３
第１の上アーム９の先端Ｏ５の位置誤差の絶対値：Ｅ
下アーム８のアーム長：Ｌ１
第１の上アーム９のアーム長；Ｌ２
第１の上アーム９の先端Ｏ５の速度ｖ＝［ｖ_ｘ，ｖ_ｙ，ｖ_ｚ］
とする。 In the following,
Jacobian of the lower arm 8: J ₂ = (J _2x , J _2y , J _2z )
Jacobian of the first upper arm 9: J ₃ = (J _3x , J _3y , J _3z )
Position error of the tip O5 of the first upper arm 9: e = (e _x , e _y , e _z ) = J ₂ d ₂ + J ₃ d ₃
Absolute value of position error of tip O5 of first upper arm 9: E
Arm length of lower arm 8: L1
Arm length of first upper arm 9; L2
Speed v = [v _x , v _y , v _z ] of the tip O5 of the first upper arm 9
And

移動方向以外の位置誤差Ｄの２乗は、
Ｄ^２＝｛（ｖ_ｙ ^２＋ｖ_ｚ ^２）ｅ_ｘ ^２＋（ｖ_ｚ ^２＋ｖ_ｘ ^２）ｅ_ｙ ^２＋（ｖ_ｘ ^２＋ｖ_ｙ ^２）ｅ_ｚ ^２−２ｖ_ｘｖ_ｙｅ_ｘｅ_ｙ−２ｖ_ｙｖ_ｚｅ_ｙｅ_ｚ−２ｖ_ｚｖ_ｘｅ_ｚｅ_ｘ｝／（ｖ_ｘ ^２＋ｖ_ｙ ^２＋ｖ_ｚ ^２）…（５） The square of the position error D other than the moving direction is
D ² = {(v _y ² + v _z ² ) e _x ² + (v _z ² + v _x ² ) e _y ² + (v _x ² + v _y ² ) e _z ² -2v _x v _y e _x e _y -2v _{_{_{_{y v z e y e z -2v}}}} z v x e z e x} / (v x 2 + v y 2 + v z 2) ... (5)

この位置誤差Ｄを最小化する下アーム８の補正量ｄ_２ａは次の（６）式となる。
ｄ_２ａ＝−｛Ｊ_２ｘＪ_３ｘ（ｖ_ｙ ^２＋ｖ_ｚ ^２）＋Ｊ_２ｙＪ_３ｙ（ｖ_ｚ ^２＋ｖ_ｘ ^２）＋Ｊ_２ｚＪ_３ｚ（ｖ_ｘ ^２＋ｖ_ｙ ^２）−（Ｊ_２ｘＪ_３ｙ＋Ｊ_２ｙＪ_３ｘ）ｖ_ｘｖ_ｙ−（Ｊ_２ｙＪ_３ｚ＋Ｊ_２ｚＪ_３ｙ）ｖ_ｙｖ_ｚ−（Ｊ_２ｚＪ_３ｘ＋Ｊ_２ｘＪ_３ｙ）ｖ_ｚｖ_ｘ｝ｄ_３／｛Ｊ_２ｘ ^２（ｖ_ｙ ^２＋ｖ_ｚ ^２）＋Ｊ_２ｙ ^２（ｖ_ｚ ^２＋ｖ_ｘ ^２）＋Ｊ_２ｚ ^２（ｖ_ｘ ^２＋ｖ_ｙ ^２）−２Ｊ_２ｘＪ_２ｙｖ_ｘｖ_ｙ−２Ｊ_２ｙＪ_２ｚｖ_ｙｖ_ｚ−２Ｊ_２ｚＪ_２ｘｖ_ｚｖ_ｘ｝…（６） Correction amount d _2a of the lower arm 8 to minimize this position error D is the following equation (6).
d _2a = − {J _2x J _3x (v _y ² + v _z ² ) + J _2y J _3y (v _z ² + v _x ² ) + J _2z J _3z (v _x ² + v _y ² ) − (J ₂ xJ ₃ y + J ₂ yJ _{_{_{_{3 x) v x v y -}}}} (J 2 yJ 3 z + J 2 zJ 3 y) v y v z - (J 2 zJ 3 x + J 2 xJ 3 y) v z v x} d 3 / {J 2x 2 (v y ² + v _z ² ) + J _2y ² (v _z ² + v _x ² ) + J _2z ² (v _x ² + v _y ² ) -2J _2x J _2y v _x v _y −2J _2y J _2z v _y v _z −2J _2z J _2x v _z v _x } (6)

このｄ_２ａは速度が０、即ち停止中には計算できない。停止中には全方向において位置誤差Ｅが最小化するようにする。この場合、下アーム８の補正量ｄ_２ｂは下の（９）式で求めることができる。
Ｅ^２＝（Ｊ_２ｄ_２＋Ｊ_３ｄ_３）（Ｊ_２ｄ_２＋Ｊ_３ｄ_３）^Ｔ＝Ｊ_２Ｊ_２ ^Ｔｄ_２ ^２＋２Ｊ_２Ｊ_３ ^Ｔｄ_２ｄ_３＋Ｊ_３Ｊ_３ ^Ｔｄ_３ ^２＝（Ｊ_２ｘ ^２＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）ｄ_２ ^２＋２（Ｊ_２ｘＪ_３ｘ＋Ｊ_２ｙＪ_３ｙ＋Ｊ_２ｚＪ_３ｚ）＋（Ｊ_３ｘ ^２＋Ｊ_３ｙ ^２＋Ｊ_３ｚ ^２）ｄ_３ ^２…（７） This d _2a cannot be calculated when the speed is 0, that is, while the vehicle is stopped. During the stop, the position error E is minimized in all directions. In this case, the correction amount d _2b of the lower arm 8 can be obtained by the following equation (9).
^{_{_{_{E 2 = (J 2 d 2}}}} + J 3 d 3) (J 2 d 2 + J 3 d 3) T = J 2 J 2 T d 2 2 + 2J 2 J 3 T d 2 d 3 + J 3 J 3 T d 3 2 = (J _2x ² + J _2y ² + J _2z ² ) d ₂ ² +2 (J _2x J _3x + J _2y J _3y + J _2z J _3z ) + (J _3x ² + J _3y ² + J _3z ² ) d ₃ ² (7)

このＥ^２をｄ_２について偏微分すると、次の（８）式となる。
∂Ｅ^２／∂ｄ_２＝２（Ｊ_２ｘ ^２＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）ｄ_２＋２（Ｊ_２ｘＪ_３ｘ＋Ｊ_２ｙＪ_３ｙ＋Ｊ_２ｚＪ_３ｚ）ｄ_３＝０…（８）
そして、（８）式からｄ_２ｂを求めると、
ｄ_２ｂ＝−（Ｊ_２ｘＪ_３ｘ＋Ｊ_２ｙＪ_３ｙ＋Ｊ_２ｚＪ_３ｚ）ｄ_３
／（Ｊ_２ｘ ^２＋Ｊ_２ｙ ^２＋Ｊ_２ｚ ^２）…（９） When this E ² is partially differentiated with respect to d ₂ , the following equation (8) is obtained.
^{_{_{^{∂E 2 / ∂d 2 = 2 (}}}} J 2x 2 + J 2y 2 + J 2z 2) d 2 +2 (J 2x J 3x + J 2y J 3y + J 2z J 3z) d 3 = 0 ... (8)
And if _d2b is calculated _| required from (8) Formula,
d _2b = − (J _2x J _3x + J _2y J _3y + J _2z J _3z ) d ₃
/ (J _2x ² + J _2y ² + J _2z ² ) (9)

停止中から移動する際、ｄ_２ｂとｄ_２ａを滑らかに接続するために、速度ｖ_１以下（ｖ１は０を含む）の場合には、ｄ_２ｂの補正量を用い、速度ｖ_２以上のときは、ｄ_２ａで補正し、その間はｄ_２ｂとｄ_２ａとの間を例えば線形補間するとすれば、速度をｖとすると、補正値ｄ_２は
ｄ_２＝ｄ_２ａ＋（ｄ_２ｂ−ｄ_２ａ）（ｖ−ｖ_１）／（ｖ_２−ｖ_１）
となる。 In order to smoothly connect d ₂ b and d ₂ a when moving from the stop, when the speed is v ₁ or less (v ₁ includes 0), the correction amount of d ₂ b is used, and the speed v ₂ when the above, corrected with _{d 2} a, if during which the example linear interpolation between _{d 2} b and _{d 2} a, when the speed and v, the correction value _{d 2} is d ₂ = _d 2 a + ( _{_{_{d 2 b-d 2 a)}}} (v-v 1) / (v 2 -v 1)
It becomes.

この方法によれば、動作中には移動方向以外の誤差を最小化するために、軌道精度がより改善される。また、移動方向の誤差を最小化している際には、第１の上アーム９の移動長に比例した速度むらが発生するが、第１の上アーム９の振動が速度に依存しないものであるとすれば、速度が高いほど速度むらの相対的な大きさは小さくなる。即ち、速度が高い領域のみ移動方向以外の誤差を最小化する本方法によれば、速度むらの小さなものとなり、問題とはならない。 According to this method, the trajectory accuracy is further improved in order to minimize errors other than the moving direction during operation. Further, when the error in the moving direction is minimized, speed unevenness proportional to the moving length of the first upper arm 9 occurs, but the vibration of the first upper arm 9 does not depend on the speed. If so, the higher the speed, the smaller the relative magnitude of the speed unevenness. That is, according to the present method for minimizing the error other than the moving direction only in the high speed region, the speed unevenness is small, which is not a problem.

なお、本発明の多関節型ロボットは６軸のものに限らない。少なくとも第１〜３軸のリンクからロボットアームを構成するものであれば良い。 The articulated robot of the present invention is not limited to a six-axis robot. Any robot may be used as long as it constitutes a robot arm from links of at least first to third axes.

図面中、２はロボット本体、５はベース（ベースリンク）、７はショルダ（第の１リンク）、８は下アーム（第２のリンク）、９は第１の上アーム（第３のリンク）、１０は第２の上アーム（第４のリンク）、１１は手首アーム（第５のリンク）、１２はフランジ（第６のリンク）、１５はサーボモータ、１６は歯車減速装置、１９は歯付きベルト、２０はベルト伝動機構を示す。 In the drawing, 2 is a robot body, 5 is a base (base link), 7 is a shoulder (first link), 8 is a lower arm (second link), and 9 is a first upper arm (third link). 10 is a second upper arm (fourth link), 11 is a wrist arm (fifth link), 12 is a flange (sixth link), 15 is a servo motor, 16 is a gear reduction device, and 19 is a tooth. A belt 20 and a belt transmission mechanism 20 are shown.

Claims

A vertical articulated robot robot in which the first to third links constituting the robot arm are sequentially connected to the base link fixed to the installation surface by the first to third rotary joints. The second link is connected to the first link so as to be vertically rotatable by the second rotary joint, and the third link is connected to the tip of the second link. A vertical articulated type that is connected to a rotary joint so as to be rotatable in the vertical direction, and that the drive motor of the third link is provided on the second link at the rotation center side of the second link. In the robot,
The difference between the position command for the drive motor of the third link and the current position detected by the angle sensor for detecting the rotation angle of the drive motor is defined as a position error, and the second link is based on this position error. The correction amount of the driving motor is calculated, and the correction amount is added to the next position command for the driving motor of the second link, thereby correcting the position of the third link. Position error suppression method for articulated robots.

In the vertical articulated robot position error suppression method according to claim 1,
When the motor is operated by a position command, the position error other than the moving direction obtained from the position command of the tip of the third link is obtained from the speed of the tip of the third link, and the position error is calculated from the position error. A vertical articulated type that calculates a correction amount of the drive motor of the second link and adds the correction amount to the next position command for the drive motor of the second link to operate the second link. Robot position error suppression method.