JP2008207304A - Robot controlling method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To solve, in a method of calculating load mass and gravity center positions contained in a robot based on motor torque values and arm angles of respective axes of the robot, a problem of the necessity to increase moving amounts of the axes or an estimated calculation amount in order to raise the detection accuracy, while motor torque values and arm angles of a plurality of arms including two rotational center axes perpendicular to each other are measured, a geometric relationship of the rotational center axes being perpendicular to each other is derived based on torque components arising from the load gravity calculated based on the above load mass and gravity positions, and the load mass and the gravity center positions are calculated, to thereby perform the calculation at a small operation region for only two axes of leading ends of the arms. <P>SOLUTION: By calculating and averaging load gravity torques with a plurality of sets of the load gravity torques paired at predetermined angular intervals, when obtaining amplitudes and phases of the load gravity toques, the detection accuracy is improved without increasing the measurement operation angle and the calculation amount. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

本発明は、モータにより駆動されるロボットに取り付けられた負荷の重量と負荷の重心位置を算出する方法に関する。   The present invention relates to a method of calculating the weight of a load attached to a robot driven by a motor and the position of the center of gravity of the load.

近年、モータにより駆動される複数軸のアームを備えるロボットにおいて、衝突時の安全性向上や破壊による損失防止のため衝突検出の高精度化が求められている。   In recent years, in a robot having a multi-axis arm driven by a motor, high accuracy of collision detection has been demanded in order to improve safety at the time of collision and to prevent loss due to destruction.

また、ロボットの実タクト性能の更なる向上を目的として、加減速運動性能や振動抑制制御性能の高性能化、ロボットの利用分野の拡大とバリ取り作業や複雑な形状部品の嵌め合い作業など各種使用用途の要求を満足することを目的とした柔軟制御の実現が求められている。   In addition, with the aim of further improving the actual tact performance of the robot, various kinds of performance such as acceleration / deceleration motion performance and vibration suppression control performance, expansion of the field of use of the robot, deburring work, and fitting work of complex shaped parts, etc. There is a demand for the realization of flexible control aimed at satisfying the usage requirements.

このような用途を実現するための各用途の制御方法においては、ロボットのアームおよびアーム先端に装着される負荷の質量と重心位置を必要とする。このうち、ロボットが備えるアームの質量と重心位置は、設計時に使用されたＣＡＤ等の設計ツールによって予め同定することが可能である。しかし、ロボットのアーム先端へ任意の負荷が装着される場合には、別途、負荷の質量と重心位置の値を知ることが必要となる。   In the control method for each application for realizing such an application, the mass of the load and the position of the center of gravity attached to the arm of the robot and the tip of the arm are required. Among these, the mass and the position of the center of gravity of the arm provided in the robot can be identified in advance by a design tool such as CAD used at the time of design. However, when an arbitrary load is attached to the tip of the robot arm, it is necessary to separately know the load mass and the value of the center of gravity.

従来、負荷の質量と重心位置を知る方法としては、ロボットコントローラからロボットの操作者がそれらの値を手入力する方法がある。   Conventionally, as a method of knowing the mass of the load and the position of the center of gravity, there is a method in which the robot operator manually inputs these values from the robot controller.

一方、手入力しない方法として、ロボットが備えるモータ各軸の電流やトルクや角度を測定し、負荷の質量と重心位置を算出する方法があった。この方法には、ロボットを静止させて行う方法と、ロボットのモータ各軸を駆動させて行う方法の２つの方法があった。   On the other hand, as a method without manual input, there is a method of measuring the current, torque, and angle of each axis of the motor provided in the robot and calculating the load mass and the position of the center of gravity. This method has two methods: a method in which the robot is stationary and a method in which each axis of the robot is driven.

このうち、ロボットを静止させて行う方法では、ロボットの静止状態におけるモータ各軸の電流値から算出したトルクを用い、ロボット単体での重力に起因するトルク成分と静止時の摩擦トルクを引くことにより負荷の重力に起因するトルクだけを抽出し、負荷の質量と重心位置を算出していた（例えば、特許文献１参照）。   Of these methods, the method of making the robot stand still uses the torque calculated from the current value of each axis of the motor in the stationary state of the robot, and subtracts the torque component caused by the gravity of the robot alone and the friction torque when stationary. Only the torque resulting from the gravity of the load is extracted, and the mass and the center of gravity of the load are calculated (for example, see Patent Document 1).

また、ロボットのモータ各軸を駆動させて行う方法では、ロボットのモータ各軸を駆動させる時の各軸モータトルクとアーム角度を算出しようとする負荷の質量と重心位置の数だけ立式された重力に起因する関係式へ代入することによって、負荷の質量と重心位置を算出していた（例えば、特許文献２参照）。   Further, in the method of driving each axis of the motor of the robot, the number of axes and the center of gravity position of the load for calculating the motor torque and arm angle of each axis when driving each axis of the robot motor was established. The mass of the load and the position of the center of gravity were calculated by substituting them into the relational expression caused by gravity (see, for example, Patent Document 2).

しかしながら、上記した従来の負荷の質量と重心位置を知る方法では、ロボットの操作者が負荷の質量と重心位置を予め算出や測定によって求めておく必要があり、作業者の作業負担や入力忘れによる弊害発生があった。   However, in the conventional method of knowing the mass and center of gravity of the load described above, it is necessary for the operator of the robot to obtain the mass and center of gravity of the load in advance by calculation or measurement. There was an evil occurrence.

また、手入力しない方法として、上記したロボットを静止させて行う方法では、静止時の摩擦トルクは負荷の質量やロボットの姿勢によって大きく変動することから、正確な値を推定することが難しく、算出される負荷の質量と重心位置の精度に大きく影響する可能性があった。   In addition, as a method that does not input manually, the above-mentioned method in which the robot is stationary makes it difficult to estimate an accurate value because the friction torque when stationary varies greatly depending on the mass of the load and the posture of the robot. The load mass and the accuracy of the center of gravity position could be greatly affected.

また、ロボットのモータ各軸を駆動させて行う方法では、負荷の質量と重心位置を算出するために、４つの独立な計算式を備えることに起因し、主要駆動軸を含む４つのモータトルク値とアーム角度の測定を行う必要があった。そのため、ロボットの動作範囲が制限される場合や動作自体が制限される場合には、負荷の質量と重心位置を算出することができない可能性があった。   In the method of driving each axis of the motor of the robot, four motor torque values including the main drive axis are derived from having four independent calculation formulas for calculating the load mass and the center of gravity. It was necessary to measure the arm angle. For this reason, when the movement range of the robot is restricted or when the movement itself is restricted, there is a possibility that the load mass and the gravity center position cannot be calculated.

以上のように、負荷の質量と重心位置を推定する従来の方法では、それぞれ少ない動作において負荷の質量と重心位置を高精度に推定するという点で問題があった。そこで、回転中心軸が直交するロボットの手首２軸のみの動作で、負荷の質量と重心位置を推定する方法が提案されている（例えば、特許文献３参照）。   As described above, the conventional method for estimating the load mass and the position of the center of gravity has a problem in that the load mass and the position of the center of gravity are accurately estimated with a small number of operations. In view of this, a method has been proposed in which the mass of the load and the position of the center of gravity are estimated by only the operation of the two wrist axes of the robot whose rotation center axes are orthogonal (see, for example, Patent Document 3).

この方法では、２つの回転軸を中心に回転する各アームを１軸毎に等角速度運動させた時のモータトルクを算出し、算出したモータトルクから負荷の質量に起因するトルク成分を各回転軸ごとに求め、求めた負荷トルク成分（正弦波）の極大値（振幅）と、トルク成分が０となるアーム関節角度（アーム基準角度からの位相差）とを各回転軸ごとに計算して求め、負荷の質量及び重心位置を算出している。   In this method, the motor torque when each arm rotating around two rotation axes is moved at an equal angular velocity for each axis is calculated, and the torque component due to the mass of the load is calculated from the calculated motor torque. The maximum value (amplitude) of the obtained load torque component (sine wave) and the arm joint angle at which the torque component becomes zero (phase difference from the arm reference angle) are calculated for each rotation axis. The load mass and the position of the center of gravity are calculated.

この時、負荷トルク成分の振幅と位相差を、簡便な方法で精度良く決定するには、測定値にトルク成分が極大値になる部分と０になる部分が含まれていることが好ましい。そして、負荷トルク成分が０になったアーム関節角度を位相差として確定できるので、そこから９０°ずれた角度での負荷トルク値を振幅として採用することができる。
特開平９−９１００４号公報 特開平１１−４８１８１号公報 特開２００６−０７５９３１号公報 At this time, in order to accurately determine the amplitude and phase difference of the load torque component by a simple method, it is preferable that the measured value includes a portion where the torque component is a maximum value and a portion where the torque component is zero. Since the arm joint angle at which the load torque component becomes 0 can be determined as the phase difference, the load torque value at an angle shifted by 90 ° from the arm joint angle can be adopted as the amplitude.
JP-A-9-91004 Japanese Patent Laid-Open No. 11-48181 JP 2006-075931 A

上記した特許文献３に記載の方法では、極大値と０になる部分を測定するために当該軸を９０°以上回転させる必要があり、推定するための動作範囲を狭めるためには最小２乗法を用いて正弦波の振幅や位相を同定させる方法が掲載されている。   In the method described in Patent Document 3, it is necessary to rotate the axis by 90 ° or more in order to measure the maximum value and the portion where the value becomes 0. In order to narrow the motion range for estimation, the least square method is used. A method for identifying the amplitude and phase of a sine wave by using it is described.

ただし、最小２乗法はパラメータを変化させながら繰り返し計算で誤差を収束させていくものであり計算量が多くなる。その上、非線形の三角関数（正弦波）を取り扱っているためさらに計算量が増え、ロボットコントローラに搭載されたサーボ制御等のリアルタイム制御用ＣＰＵで処理するのは難しい。   However, the method of least squares converges the error by iterative calculation while changing parameters, and the amount of calculation increases. In addition, since a nonlinear trigonometric function (sine wave) is handled, the amount of calculation further increases, and it is difficult to process with a CPU for real-time control such as servo control mounted on the robot controller.

そこで本発明では、２つの回転軸を中心に回転する各アームを１軸毎に等角速度運動させ、負荷質量に起因する電流の振幅と位相を求める際に、計算量の増加を最小限に抑えた上で、回転動作角度を減少させることができる負荷の質量及び重心位置を算出する制御方法を提供することを目的とする。   Therefore, in the present invention, the increase in the amount of calculation is minimized when the arms rotating around the two rotation axes are moved at equal angular speeds for each axis to obtain the amplitude and phase of the current due to the load mass. In addition, an object of the present invention is to provide a control method for calculating the mass of the load and the position of the center of gravity that can reduce the rotational operation angle.

上記課題を解決するために、本発明のロボットの制御方法は、モータにより駆動され、回転中心軸が略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させて所定の位相間隔を有しており所定の角度に位置する２点における各点の角度とその角度における電流測定値とを求め、これらの角度と電流測定値とから前記負荷の質量に起因するトルクの振幅と位相とを各軸ごとに求め、前記トルクの振幅と位相に基づいて前記負荷の質量及び重心位置を算出するものである。   In order to solve the above-described problem, a robot control method according to the present invention includes a plurality of arms that are driven by a motor and have at least one set of two rotation axes whose rotation center axes are substantially orthogonal to each other. A method for controlling a robot using a load mounted on an arm that rotates about a rotation axis on the tip side of the arm, wherein each arm that rotates about the two rotation axes moves at an equal angular velocity for each axis. The angle of each point at two points having a predetermined phase interval and positioned at a predetermined angle and the current measurement value at the angle are obtained, and the current and the current measurement value are obtained from these angles and the current measurement value, and are caused by the mass of the load. The torque amplitude and phase are obtained for each axis, and the mass and center of gravity of the load are calculated based on the torque amplitude and phase.

また、本発明のロボットの制御方法は、モータにより駆動され、２つの回転中心軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸と略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させて所定の位相間隔を有しており所定の角度に位置する２点における各点の角度とその角度における電流測定値とを求め、これらの角度と電流測定値とから前記負荷の質量に起因するトルクの振幅と位相とを各軸ごとに求め、前記トルクの振幅と位相に基づいて前記負荷の質量及び重心位置を算出するものである。   In the robot control method of the present invention, the two rotations driven by the motor and in which the plane formed by the straight line connecting the two rotation center axes and the one rotation center axis is substantially orthogonal to the other rotation center axis. A control method for a robot comprising a plurality of arms having at least one set of axes and mounting a load on an arm that rotates around a rotation axis on the distal end side of the arm among the two rotation axes. The arms rotating around the axis are moved at equal angular speeds for each axis, and the angle of each point at two points having a predetermined phase interval and positioned at a predetermined angle, and the current measurement value at that angle are obtained. Obtain the amplitude and phase of the torque due to the mass of the load for each axis from these angles and current measurement values, and calculate the mass and center of gravity of the load based on the amplitude and phase of the torque Things .

また、本発明のロボットの制御方法は、所定の位相間隔を有する２点の位相間隔は９０度より小さく１０度より大きいものである。   In the robot control method of the present invention, the phase interval between two points having a predetermined phase interval is smaller than 90 degrees and larger than 10 degrees.

また、本発明のロボットの制御方法は、所定の位相間隔を有しており所定の角度に位置する２点を複数設定し、各２点において求めた負荷質量に起因するトルクの位相と振幅の平均を算出し、この位相と振幅の平均に基づいて負荷の質量と重心位置を算出するものである。   In the robot control method of the present invention, a plurality of two points having a predetermined phase interval and positioned at a predetermined angle are set, and the phase and amplitude of the torque caused by the load mass obtained at each of the two points are set. An average is calculated, and the mass and the center of gravity of the load are calculated based on the average of the phase and amplitude.

また、本発明のロボットの制御方法では、複数の２点は、所定の２点から各軸の移動方向に微小角度移動した位置にある点である。   In the robot control method of the present invention, the plurality of two points are points that are moved by a minute angle in the movement direction of each axis from the predetermined two points.

以上のように、本発明のロボットの制御方法によれば、負荷重力トルクの振幅及び位相を求める際に、所定の角度間隔で負荷重力トルクを複数組ペアリングして演算、平均化することで、測定動作角度や演算量を増加させることなく検出精度を向上させることができる。   As described above, according to the robot control method of the present invention, when determining the amplitude and phase of the load gravity torque, a plurality of pairs of load gravity torques are calculated and averaged at predetermined angular intervals. The detection accuracy can be improved without increasing the measurement operation angle and the amount of calculation.

以下、本発明を実施するための最良の形態について、図１から図７を用いて説明する。   Hereinafter, the best mode for carrying out the present invention will be described with reference to FIGS.

（実施の形態１）
本実施の形態では、垂直座標系において表現される図１や水平座標表現において表現される図２のような回転中心軸を有するアームを備えたロボットについて説明する。また、本実施の形態では、図１や図２で示すようにアーム先端側の２つの回転軸第５、６軸の回転中心軸はねじれの位置にあり、軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸とが略直交して配置されたロボットの例を示している。なお、この本実施の形態における回転中心軸は、ねじれの位置にある例を示しているが、これ以外に直交する場合であっても以後の説明は成り立つので、本実施の形態では説明の簡略化のため転軸がねじれの位置にある場合も回転軸が直交しているとして説明する。 (Embodiment 1)
In the present embodiment, a robot including an arm having a rotation center axis as shown in FIG. 1 expressed in a vertical coordinate system and in FIG. 2 expressed in a horizontal coordinate expression will be described. Further, in this embodiment, as shown in FIGS. 1 and 2, the rotation center axes of the two rotation shafts 5 and 6 on the arm tip side are in a twisted position, and a straight line connecting the shortest axis and one An example of a robot is shown in which the plane formed by the rotation center axis is arranged so that the other rotation center axis is substantially orthogonal. In addition, although the rotation center axis in this embodiment shows an example at the position of twist, the following description is valid even in the case of being orthogonal to other than this, so the description is simplified in this embodiment. For the sake of simplicity, the description will be made assuming that the rotation axis is orthogonal even when the rotation axis is in the twisted position.

なお、図１および図２において、Ｘｌ ２９、Ｚｌ ３０、およびＹｌ ３１は、第６軸のアーム回転中心位置からアーム先端に装着する負荷の重心位置までの３方向の距離を示す。また、図１において、Ｌｉｉ ４６とＬｊ ４７は、第５軸４４の回転中心から第６軸４５の回転中心までの２方向のアーム長を示す。なお、もう１方向のアーム長は、図２において第５軸４４と第６軸４５が回転中心を同一線上に有する例を示しているので０としている。   1 and 2, Xl 29, Zl 30, and Yl 31 indicate distances in three directions from the arm rotation center position of the sixth axis to the gravity center position of the load attached to the tip of the arm. In FIG. 1, Lii 46 and Lj 47 indicate the arm lengths in two directions from the rotation center of the fifth shaft 44 to the rotation center of the sixth shaft 45. Note that the arm length in the other direction is set to 0 because the fifth shaft 44 and the sixth shaft 45 in FIG. 2 have the rotation centers on the same line.

なお、これら寸法のうち、負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌが求めたい未知の数値であり、第５軸と第６軸のアーム長ＬｉｉとＬｊは予め既知の数値である。   Of these dimensions, the load center-of-gravity positions Xl, Zl, and Yl are unknown numerical values, and the arm lengths Lii and Lj of the fifth axis and the sixth axis are known numerical values in advance.

また、図３は図１の垂直座標系に相当しており第５軸を回転中心として回転するアーム５１を回転させた状態を示す図であり、図４は図２の水平座標系に相当しており第６軸に装着した負荷４８を第６軸を回転中心として回転させた状態を示す図である。そして、各図とも（ａ）は初期位置、（ｂ）は負方向回転位置（ｃ）は正方向回転位置状態を示す。なお、図３における５０は図２で示す第４軸４３のアームの一部を示している。   3 corresponds to the vertical coordinate system of FIG. 1 and shows a state in which the arm 51 rotating around the fifth axis is rotated, and FIG. 4 corresponds to the horizontal coordinate system of FIG. It is a figure which shows the state which rotated the load 48 attached to the 6th axis | shaft centering on the 6th axis | shaft. In each figure, (a) shows the initial position, (b) shows the negative rotation position (c), and the positive rotation position. 3 denotes a part of the arm of the fourth shaft 43 shown in FIG.

さらに、図３におけるＬ５ ２６および図４におけるＬ６ ３４は、第５軸の回転中心を原点とした時の負荷重心位置までの３方向の長さを示しており、図３や図４に示すように、アーム長Ｌｉｉ、Ｌｊ、および負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌの合成長さで示される。   Further, L526 in FIG. 3 and L634 in FIG. 4 indicate the length in three directions to the load center of gravity when the rotation center of the fifth axis is the origin, as shown in FIG. 3 and FIG. Are the combined lengths of the arm lengths Lii, Lj and the center of gravity positions Xl, Zl, Yl of the load.

次に、上記で説明したようなアーム先端に備えられた負荷について、その質量と重心位置を算出する方法について説明する。   Next, a method for calculating the mass and the position of the center of gravity of the load provided at the arm tip as described above will be described.

まず、図１および図２に示す第５軸４４および第６軸４５を中心に回転するアームを１軸毎に等角速度運動させ、各軸に印加される負荷質量起因のトルク（正弦波）の振幅とアーム基準角度から位相を求める例について説明する。   First, the arms rotating around the fifth axis 44 and the sixth axis 45 shown in FIGS. 1 and 2 are moved at an equal angular velocity for each axis, and the torque (sinusoidal wave) caused by the load mass applied to each axis An example in which the phase is obtained from the amplitude and the arm reference angle will be described.

図５は、本実施の形態における制御方法を実施するためのブロック図である。なお、ロボット全体としては図５と同様のブロック図が複数軸分存在するが、図５を代表としてモータ１軸分の制御について示す。   FIG. 5 is a block diagram for carrying out the control method in the present embodiment. In addition, although the block diagram similar to FIG. 5 exists for a plurality of axes for the entire robot, FIG. 5 is representatively shown for the control of one motor axis.

図５において、６は位置制御ブロックであり、ロボット関節角指令θｒｃ１に減速比Ｒｇ３７を乗じて計算されたモータ位置指令θｃｏｍ３とモータ位置フィードバックθｍ２の差分値から速度ループ指令ωｃｏｍ７を生成する。   In FIG. 5, 6 is a position control block, which generates a speed loop command ωcom7 from the difference value between the motor position command θcom3 and the motor position feedback θm2 calculated by multiplying the robot joint angle command θrc1 by the reduction ratio Rg37.

図５の１０は速度制御ブロックであり、速度ループ指令ωｃｏｍ７とモータ速度フィードバックωｍ４の差分値からモータ電流指令Ｉｍ１１を生成する。   Reference numeral 10 in FIG. 5 denotes a speed control block, which generates a motor current command Im11 from a difference value between the speed loop command ωcom7 and the motor speed feedback ωm4.

図５の１８はモータと外力を示したブロックである。τｍ１３はモータ発生トルクであり、減速機は剛体と仮定すると、モータ駆動側から見れば式（数１）で表され、負荷側から見れば式（数２）で表される。   Reference numeral 18 in FIG. 5 is a block showing the motor and external force. τm13 is a motor-generated torque, and assuming that the speed reducer is a rigid body, it is expressed by Expression (1) when viewed from the motor drive side, and is expressed by Expression (Expression 2) when viewed from the load side.

ただし、式（数１）、式（数２）における記号は以下の通りである。     However, the symbols in the equations (Equation 1) and (Equation 2) are as follows.

Ｋｔ（１２）：モータトルク定数
Ｉｍ（１１）：モータ電流
αｍ ：モータ角加速度（ωｍの微分値）
ωｍ（４） ：モータ角速度
Ｊｍ ：モータイナーシャ（ロータ＋減速機１次側、減速機２次側に換算）
Ｄ ：粘性摩擦係数
τμ（１４）：動摩擦トルク
τｄｙｎ（１６）：動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和）
Ｒｇ ：減速機の減速比
式（数２）における動摩擦トルクτμは以下の式で計算できる。 Kt (12): Motor torque constant Im (11): Motor current αm: Motor angular acceleration (differential value of ωm)
ωm (4): Motor angular velocity Jm: Motor inertia (converted to rotor + reducer primary side, reducer secondary side)
D: Coefficient of viscous friction τμ (14): Dynamic friction torque τdyn (16): Dynamic torque (the sum of gravity torque, inertial force, centrifugal force, and Coriolis force)
Rg: Reduction gear reduction ratio The dynamic friction torque τμ in the equation (Equation 2) can be calculated by the following equation.

ただし、
Ｋμ：動摩擦の大きさ

However,
Kμ: Size of dynamic friction

式（数２）における動力学トルクτｄｙｎは、低速で等角速度動作時には、角速度や角加速度に起因する慣性力、遠心力、コリオリ力は無視でき、重力トルク成分として近似できるので、式（数５）の様に表すことが出来る。   The dynamic torque τdyn in equation (2) can be approximated as a gravitational torque component because inertial force, centrifugal force, and Coriolis force due to angular velocity and angular acceleration can be ignored during constant angular velocity operation at low speed. ).

ただし、
θｒ：ロボット関節角（減速機出力角度）＝θｍ／Ｒｇ
τｇＡ［θｒ］：ロボット本体アームに起因する重力トルク
τｇＬ［θｒ］：ロボットに装着された負荷に起因する重力トルク
τｇＡ［θｒ］とτｇＬ［θｒ］両方ともロボット関節角θｒにより一意に決定される。 However,
θr: Robot joint angle (speed reducer output angle) = θm / Rg
τgA [θr]: Gravitational torque caused by the robot arm τgL [θr]: Gravitational torque caused by the load attached to the robot Both τgA [θr] and τgL [θr] are uniquely determined by the robot joint angle θr .

式（数５）におけるロボット本体アームに起因する重力トルクτｇＡ［θｒ］は、ロボットを構成するアーム等の部品の質量と形状が既知であるので、ロボットの姿勢を確定できれば（全軸のロボット関節角度が判明すれば）計算が可能である。   The gravity torque τgA [θr] resulting from the robot body arm in Equation (5) is known because the mass and shape of parts such as the arm constituting the robot are known. Calculation is possible (if the angle is known).

よって、図５のアーム重力トルク演算ブロック２０は、当該軸のロボット関節角θｒ３６と静止状態にある他軸のロボット関節角θｒ１９が入力され、ロボットを構成するアーム等の部品の質量と形状のパラメータと合わせて、アーム重力トルクτｇＡ［θｒ］２１を計算する。   Therefore, the arm gravity torque calculation block 20 in FIG. 5 receives the robot joint angle θr36 of the relevant axis and the robot joint angle θr19 of the other axis in a stationary state, and parameters of the mass and shape of parts such as arms constituting the robot. And the arm gravity torque τgA [θr] 21 is calculated.

低速等速動作時においては、モータ角加速度αｍ＝０であるので、式（数１）〜（数５）に基づいて負荷に起因する重力トルクτｇＬ［θｒ］２４を求める式を導出すると以下の式（数６）の様になる。   Since the motor angular acceleration αm = 0 at low speed and constant speed operation, a formula for obtaining the gravitational torque τgL [θr] 24 caused by the load based on the formulas (Equation 1) to (Equation 5) is derived as follows. It becomes like a formula (Formula 6).

この重力トルクτｇＬ［θｒ］を求めるにあたっては、図３と図４に示すようにそれぞれ（ａ）〜（ｃ）で示すように第５軸４４、第６軸４５をそれぞれ１軸毎に正負方向に所定の角度Δθｒ１２５だけ等角速度往復運動させ、各方向のモータ電流値Ｉｍとロボット関節角度θｒを測定する。   In obtaining the gravitational torque τgL [θr], as shown in FIGS. 3 and 4, the fifth axis 44 and the sixth axis 45 are respectively positive and negative in each direction as shown in (a) to (c). Are reciprocated at a constant angular velocity by a predetermined angle Δθr125, and the motor current value Im and the robot joint angle θr in each direction are measured.

ここで、−方向（ｓｇｎ＝−１）に−ωｍ０で等角速度動作させた時の電流をＩｍ［θｒ］ｍ、＋方向（ｓｇｎ＝１）にωｍ０で等角速度動作させた時のモータ電流をＩｍ［θｒ］ｐとして上記式（数６）を表すと、それぞれ以下の式（数７）、（数８）となる。   Here, Im [θr] m is the current when operated at a constant angular velocity at -ωm0 in the negative direction (sgn = -1), and the motor current when operated at a constant angular velocity at ωm0 in the positive direction (sgn = 1). When the above formula (formula 6) is expressed as Im [θr] p, the following formulas (formula 7) and (formula 8) are obtained, respectively.

式（数７）、式（数８）を加算してまとめると以下の式（数９）の様になる。   When the equations (Equation 7) and (Equation 8) are added together, the following equation (Equation 9) is obtained.

ただし、

However,

つまり、同じ速度の低速で往復動作させ、負方向の場合はロボット関節角度θｒとその角度でのモータ電流値Ｉｍ［θｒ］ｍ、正方向の場合はロボット関節角度θｒとその角度でのモータ電流値Ｉｍ［θｒ］ｐのデータを蓄積し、所定の角度Δθｒの往復動作の後で同じ関節角度θｒ同士で正負方向の平均をとれば（足して２で割れば）、摩擦力に関係する項が除去され、粘性摩擦係数Ｄや動摩擦Ｋμの値を求める必要なくτｇＩ［θｒ］２３を求めることができる。   That is, the robot is reciprocated at a low speed of the same speed, the robot joint angle θr and the motor current value Im [θr] m at the negative direction in the negative direction, and the robot joint angle θr and the motor current at the angle in the positive direction. If the data of the value Im [θr] p is accumulated, and the average of the positive and negative directions is taken between the same joint angles θr after the reciprocating motion of the predetermined angle Δθr (addition and divided by 2), the term related to the friction force Is removed, and τgI [θr] 23 can be obtained without having to obtain the values of the viscous friction coefficient D and the dynamic friction Kμ.

実際に上記内容を実行するためには、所定の動作角Δθｒを２ｎ分割した間隔ｄｒでデータ記録し、計算する。   In order to actually execute the above contents, data is recorded and calculated at intervals dr obtained by dividing a predetermined operating angle Δθr by 2n.

ただし、
θ０：計測開始時のロボット関節角度
回転方向負の場合：ｋ＝１，２……，２ｎ−１，２ｎ
回転方向正の場合：ｋ＝２ｎ，２ｎ−１……，２，１

However,
θ0: Robot joint angle at the start of measurement When rotation direction is negative: k = 1, 2,..., 2n−1, 2n
When the rotation direction is positive: k = 2n, 2n−1..., 2, 1

本実施の形態においては、９０°以上動作させるものではなく、所定の角度Δθｒしか動作させないので、τｇＩ［θｒ］は正弦波の１部となる。なお、所定の角度△θｒは９０°より小さく１０°より大きいことが好ましく、より好ましくは３０°である。図６はこの時のτｇＩ［θｒ］、 Ｋｇ・Ｒｇ・Ｉｍ［θｒ］、及びＫｇ・Ｒｇ・Ｉｍ［θｒ］ｐの波形の１例を示したものであり、Δθｒ＝３０°、ｄｒ＝１°（ｎ＝１５）である。   In this embodiment, it is not operated 90 ° or more, and only a predetermined angle Δθr is operated, so τgI [θr] is a part of a sine wave. The predetermined angle Δθr is preferably smaller than 90 ° and larger than 10 °, and more preferably 30 °. FIG. 6 shows an example of waveforms of τgI [θr], Kg · Rg · Im [θr], and Kg · Rg · Im [θr] p at this time, and Δθr = 30 °, dr = 1. ° (n = 15).

図５に示すデータ蓄積・摩擦トルク除去ブロック２２において上記処理を実行してτｇＩ［θｒ］２３を求め、このτｇＩ［θｒ］２３からアーム重力トルク演算ブロック２０で求めたアーム重力トルクτｇＡ［θｒ］２１を減算して負荷に起因する重力トルクτｇＬ（θｒ）２４を計算する。すなわち、式（数９）を間隔ｄｒで計算し、図５に示す振幅・位相算出手段２５にデータを保存する。   The above processing is executed in the data accumulation / friction torque removal block 22 shown in FIG. 5 to obtain τgI [θr] 23, and the arm gravity torque τgA [θr] obtained in the arm gravity torque calculation block 20 from this τgI [θr] 23. 21 is subtracted to calculate the gravity torque τgL (θr) 24 caused by the load. That is, the equation (Equation 9) is calculated at the interval dr, and the data is stored in the amplitude / phase calculation means 25 shown in FIG.

アーム重力トルクτｇＡ［θｒ］も、当該軸のみの動作であるので、正弦波の一部となり、以下の式（数１３）で計算される負荷重力トルクτｇＬ［θｒ［ｋ］］も正弦波の一部となる。その振幅をＡ５［ｋ］、位相φ５［ｋ］とすると以下の式（数１４）で表すことが出来る。   Since the arm gravity torque τgA [θr] is also an operation of only the axis, it becomes a part of the sine wave, and the load gravity torque τgL [θr [k]] calculated by the following equation (Equation 13) is also a sine wave. Become part. If the amplitude is A5 [k] and the phase φ5 [k], it can be expressed by the following equation (Formula 14).

次に、この負荷重力トルクτｇＬ（θｒ［ｋ］）２４の振幅Ａ５（２６）と位相φ５（２７）を求める方法について説明する。   Next, a method for obtaining the amplitude A5 (26) and the phase φ5 (27) of the load gravity torque τgL (θr [k]) 24 will be described.

図５に示す振幅・位相算出手段２５においては、間隔ｄｒで２ｎ個保存された負荷重力トルクτｇＬ（θｒ［ｋ］）を、ｎ個離れた（Δθｒ／２離れた）下記式（数）１５の角度でペアリングを行う。   In the amplitude / phase calculation means 25 shown in FIG. 5, 2n load gravity torques τgL (θr [k]) stored at an interval dr are separated by n (Δθr / 2 apart). Pair with an angle of.

図７に、ｋ＝３とｋ＝１２の時のペアリングの例を示す。   FIG. 7 shows an example of pairing when k = 3 and k = 12.

ｋ＝３の時、τｇＬ［θｒ１［３］］５８の矢印で示す値となり、 τｇＬ［θｒ２［３］］５９の矢印で示す値となる。また、ｋ＝１２の時 τｇＬ［θｒ１［１２］］６０の矢印で示す値となり、τｇＬ［θｒ２［１２］］６１で示す値となる。     When k = 3, the value is indicated by the arrow of τgL [θr1 [3]] 58 and the value indicated by the arrow of τgL [θr2 [3]] 59. Further, when k = 12, the value indicated by the arrow of τgL [θr1 [12]] 60 and the value indicated by τgL [θr2 [12]] 61 are obtained.

このペアリングされたデータが上記した式（数１４）の関係を満足しているとすると、以下の式（数１６）の様になる。   Assuming that the paired data satisfies the relationship of the above equation (Equation 14), the following equation (Equation 16) is obtained.

余弦定理を用いて、式（数１６）を、φ５［ｋ］、Ａ５［ｋ］について解くと以下の式（数１７）が得られる。   When the equation (Equation 16) is solved for φ5 [k] and A5 [k] using the cosine theorem, the following equation (Equation 17) is obtained.

式（数１７）において、所定の間隔Δθｒ／２を持つθｒ１［ｋ］とθｒ２［ｋ］の１組を計算すれば位相φ５［ｋ］は求めることができる。なお、１組ではなく、複数組としてもよく、ｋ＝１〜ｎまでｎ個計算し、下記の式（数１８）に示すようにｎ組の平均をとることで、推定精度を向上させることが出来る。   In the equation (Equation 17), if one set of θr1 [k] and θr2 [k] having a predetermined interval Δθr / 2 is calculated, the phase φ5 [k] can be obtained. It should be noted that multiple sets may be used instead of one set, and n is calculated from k = 1 to n, and n sets are averaged as shown in the following equation (Equation 18) to improve estimation accuracy. I can do it.

上記、ｎ組の平均値φ５を式（数１６）に代入して、式（数１９）に示すように振幅Ａ５［ｋ］を求めることが出来る。   By substituting the n sets of average values φ5 into the equation (Equation 16), the amplitude A5 [k] can be obtained as shown in the equation (Equation 19).

あるいは、φ５［ｋ］を複数求め、この複数のφ５［ｋ］から複数のＡ５［ｋ］を求め、位相φ５と同様に下記式（数２０）に示すようにｎ組の平均をとることで、振幅Ａ５の精度を向上させることができる。   Alternatively, by obtaining a plurality of φ5 [k], obtaining a plurality of A5 [k] from the plurality of φ5 [k], and taking the average of n sets as shown in the following equation (Equation 20) in the same manner as the phase φ5. The accuracy of the amplitude A5 can be improved.

第６軸でも上記と同様の処理を行う。第５軸と同様、負荷重力トルクτｇＬ［θｒ［ｋ］］は正弦波の一部となる。その振幅をＡ６［ｋ］、位相φ６［ｋ］とすると以下の式（数２１）で表すことが出来る。   The same processing as described above is performed for the sixth axis. As with the fifth axis, the load gravity torque τgL [θr [k]] is a part of a sine wave. If the amplitude is A6 [k] and the phase φ6 [k], it can be expressed by the following equation (Equation 21).

式（数２１）で式（数１４）のｃｏｓと違ってｓｉｎで表しているのは、関節角度θｒ＝０°のとき、第６軸アームが垂直方向に近い姿勢で保持されているので、τｇＬ［θｒ［ｋ］］も最小値に近いと考えられるからである。一方、第５軸では関節角度θＬ＝０°のとき、駆動するアームが水平方向に近い姿勢で保持されているので、τｇＬ［θｒ［ｋ］］も最大値に近い。それ故、ｃｏｓで表示した。いずれで表示しても、位相が９０°ずれて表現されるだけであるので、特に問題はないが、鋭角で表現できる方を選択する方が、式（数１７）でのｔａｎ−１の処理が容易となる。   Unlike the cos in the equation (Equation 14) in the equation (Equation 21), it is expressed by sin, because when the joint angle θr = 0 °, the sixth axis arm is held in a posture close to the vertical direction. This is because τgL [θr [k]] is considered to be close to the minimum value. On the other hand, on the fifth axis, when the joint angle θL = 0 °, the arm to be driven is held in a posture close to the horizontal direction, so τgL [θr [k]] is also close to the maximum value. Therefore, it is displayed in cos. In either case, there is no particular problem because the phase is only expressed with a 90 ° shift, but it is better to select the one that can be expressed with an acute angle. Becomes easy.

式（数１６）に対応した式は以下の式（数２２）の様になる。   The equation corresponding to the equation (Equation 16) is as the following equation (Equation 22).

正弦定理を用いて、式（数２２）を、φ６、Ａ６について解くと以下の式が得られる。   When the equation (Equation 22) is solved for φ6 and A6 using the sine theorem, the following equation is obtained.

以上、図１、図２に示す第５軸４４、第６軸４５の回転軸を中心に回転するアームを１軸毎に等角速度運動させ、各軸に印加される負荷質量起因のトルク（正弦波）の振幅とアーム基準角度からの位相を求める例を示した。   As described above, the arms rotating around the rotation axes of the fifth shaft 44 and the sixth shaft 45 shown in FIGS. 1 and 2 are moved at a constant angular velocity for each axis, and the torque (sinusoidal force) applied to each axis is applied. The example of obtaining the phase from the amplitude of the wave) and the arm reference angle is shown.

本実施の形態では、当該軸を９０°以上回転させなくても負荷質量起因のトルク（正弦波）の振幅と位相を求めることができ、さらに最小２乗法を用いた場合の様な繰り返し計算の必要が無く、計算量の増大も抑えることができる。   In the present embodiment, the amplitude and phase of the torque (sinusoidal wave) caused by the load mass can be obtained without rotating the shaft by 90 ° or more, and iterative calculation as in the case of using the least square method can be obtained. There is no need, and an increase in calculation amount can be suppressed.

以後は、負荷質量起因のトルク（正弦波）の振幅と位相を用いて、負荷質量Ｍ、及び負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌを求める方式を述べるが、基本的に特許文献３に記載されているものと同様である。   Hereinafter, a method for obtaining the load mass M and the gravity center positions Xl, Zl, Yl of the load mass using the amplitude and phase of the torque (sinusoidal wave) caused by the load mass will be described. It is the same as that.

ところで、式（数２０）、式（数２６）で算出した振幅Ａ５、振幅Ａ６は、トルク成分ＴｇＬの最大値であるから、図１から図４で示すように、負荷の質量をｍ、重力加速度をｇとすると、トルク成分Ｔｇが最大値を取る時は負荷の重心が回転中心の水平方向にある時であるので、各回転中心から負荷の重心位置までの合成長さＬ５、Ｌ６と負荷による力を掛けると、下記の式（数２７）と式（数２８）で表される。   By the way, the amplitude A5 and the amplitude A6 calculated by the equations (Equation 20) and (Equation 26) are the maximum values of the torque component TgL. Therefore, as shown in FIGS. When acceleration is g, the torque component Tg takes the maximum value when the center of gravity of the load is in the horizontal direction of the center of rotation. Therefore, the combined lengths L5 and L6 from the center of rotation to the position of the center of gravity of the load and the load When the force of is applied, it is expressed by the following equations (Equation 27) and (Equation 28).

なお、前述したように合成長さＬ５は、負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌと第５軸４４と第６軸４５のアーム長ＬｉｉとＬｊの合成により 下記の式（数２９）のように表される。   As described above, the combined length L5 is obtained by combining the load center-of-gravity positions Xl, Zl, Yl and the arm lengths Lii and Lj of the fifth shaft 44 and the sixth shaft 45 as shown in the following equation (Equation 29). expressed.

また、式（数２８）において、Ｌ６は負荷の重心位置の垂直成分Ｚｌと負荷の重心位置の水平成分Ｙｌの合成長さであり、下記の式（数３０）で表される。   In the equation (Equation 28), L6 is the combined length of the vertical component Zl of the load centroid position and the horizontal component Yl of the load centroid location, and is represented by the following equation (Equation 30).

また、位相差φ５およびφ６については、第５軸４４、第６軸４５のアーム角度とアーム配置の条件からそれぞれ下記の式（数３１）と式（数３２）となる。   The phase differences φ5 and φ6 are expressed by the following equations (Equation 31) and Equation (Equation 32) from the arm angle of the fifth shaft 44 and the sixth shaft 45 and the condition of the arm arrangement, respectively.

なお、式（数３１）は、図３（ａ）で示す負荷位置が式（数１４）における関節角度θｒ＝０の時であり、この時の第５軸の回転中心と負荷重心位置とを結ぶ線が水平軸となす角度がφ５であることから導かれる。また、式（数３２）は、図４（ａ）で示す負荷位置が式（数２１）における関節角度θｒ＝０の時であり、この時の第６軸の回転中心と負荷重心位置とを結ぶ線が鉛直軸となす角度がφ６であることから導かれる。   Equation (Equation 31) is when the load position shown in FIG. 3A is the joint angle θr = 0 in Equation (Equation 14), and the rotation center of the fifth axis and the load gravity center position at this time are expressed as follows. This is derived from the fact that the angle between the connecting line and the horizontal axis is φ5. Equation (32) is when the load position shown in FIG. 4A is the joint angle θr = 0 in Equation (21), and the rotation center of the sixth axis and the load gravity center position at this time are expressed as follows. This is derived from the fact that the angle between the connecting line and the vertical axis is φ6.

さらに式（数３１）、式（数３２）に式（数２７）、式（数２８）を代入して整理すると、下記の式（数３３）、（数３４）が導かれる。   Further, by substituting Expression (Expression 27) and Expression (Expression 28) into Expression (Expression 31) and Expression (Expression 32), the following Expressions (Expression 33) and (Expression 34) are derived.

ところで、式（数３３）、式（数３４）は同一の負荷の重心位置について計算しているので、当然、式（数３３）と式（数３４）のＺｌは等しい。従って、下記の式（数３５）となる。   By the way, since Equation (Equation 33) and Equation (Equation 34) are calculated for the center of gravity position of the same load, naturally, Zl in Equation (Equation 33) and Equation (Equation 34) are equal. Therefore, the following equation (Equation 35) is obtained.

さらに、式（数３５）から負荷の質量Ｍを計算する関係式へと導出することができ、下記イの式（数３６）が得られる。   Furthermore, it can derive | lead-out to the relational expression which calculates the mass M of a load from Formula (Formula 35), The following formula | equation (Formula 36) is obtained.

さらに、式（数３６）にて算出した質量Ｍを式（数３３）もしくは式（数３５）へ代入することでＺｌを算出できる。   Furthermore, Zl can be calculated by substituting the mass M calculated by the equation (Equation 36) into the equation (Equation 33) or the equation (Equation 35).

また、算出したＺｌとロボットのアーム長さＬｉｉとＬｊを式（数２９）、式（数３０）に代入し、Ｘｌ、Ｙｌは、式（数３７）、式（数３８）のようにして求められる。   Also, the calculated Zl and robot arm lengths Lii and Lj are substituted into equations (Equation 29) and (Equation 30), and Xl and Yl are expressed as in Equation (Equation 37) and Equation (Equation 38). Desired.

なお、既に説明したように、本実施の形態では、ロボットが備えるアーム及び負荷の質量に起因するトルク成分を監視し、回転中心軸が異なる２軸のモータトルクから得られる振幅値と位相差からロボットが持つモータの回転中心軸が異なる特性を用いて連立方程式を導出し、さらに連立方程式より先端負荷の質量と重心を求める過程において、負荷重力トルクの振幅及び位相を求める際に、測定角度の１／２の間隔で負荷重力トルクを複数組ペアリングして演算する方式を示した。従来は、検出精度を上げるためには、測定動作角度を増やすか、最小２乗法で演算量を増やす必要が有ったが、この方式を採用することにより、測定動作角度や演算量を増加させることなく、検出精度を向上させることができる。   As already described, in this embodiment, the torque component caused by the mass of the arm and load of the robot is monitored, and the amplitude value and phase difference obtained from the two-axis motor torque with different rotation center axes are used. In the process of deriving simultaneous equations using the characteristics of the rotation center axis of the motor of the robot, and calculating the mass and center of gravity of the tip load from the simultaneous equations, when determining the amplitude and phase of the load gravity torque, the measurement angle A method of calculating a plurality of pairs of load gravity torques at intervals of 1/2 was shown. Conventionally, in order to increase the detection accuracy, it has been necessary to increase the measurement operation angle or increase the amount of calculation by the least square method. By adopting this method, the measurement operation angle and the amount of calculation are increased. Therefore, the detection accuracy can be improved.

本発明は、モータにより駆動されるロボットの負荷の重量と負荷の重心位置を簡単な測定方法と計算方法により、迅速かつ容易に算出する制御方法を提供するものであり、衝突時の安全性向上や破壊による損失防止、衝突検出の高精度化、ロボットの実タクト性能を向上でき、ロボットの利用分野の拡大とバリ取り作業や、複雑な形状部品の嵌め合い作業など各種用途に利用できる。   The present invention provides a control method for quickly and easily calculating the weight of a load of a robot driven by a motor and the position of the center of gravity of the load by a simple measurement method and calculation method. It can be used for various purposes such as expanding the field of use of robots, deburring work, and fitting work of complex shaped parts.

本発明の実施の形態におけるロボットを構成する駆動軸の垂直座標系における配置の一例を示す図The figure which shows an example of arrangement | positioning in the vertical coordinate system of the drive shaft which comprises the robot in embodiment of this invention 本発明の実施の形態におけるロボットを構成する駆動軸の水平座標系における配置の一例を示す図The figure which shows an example of arrangement | positioning in the horizontal coordinate system of the drive shaft which comprises the robot in embodiment of this invention 本発明の実施の形態における一方の駆動軸が正負方向の等加速運動を行う一例を示す図The figure which shows an example in which one drive shaft in embodiment of this invention performs equal acceleration movement of a positive / negative direction 本発明の実施の形態における他方の駆動軸が正負回転方向の等加速運動を行う一例を示す図The figure which shows an example which the other drive shaft in embodiment of this invention performs the equal acceleration motion of a positive / negative rotation direction 本発明の実施の形態における制御方法を実施するためのブロック図Block diagram for implementing the control method in the embodiment of the present invention 本発明の実施の形態における図３の等速運動を行った場合のモータ発生トルクの一例を示す図The figure which shows an example of the motor generation torque at the time of performing the constant velocity motion of FIG. 3 in embodiment of this invention 本発明の実施の形態における図３の等速運動を行った場合における負荷が起因するトルク成分の特性の一例を示す図The figure which shows an example of the characteristic of the torque component which originates in load at the time of performing constant velocity motion of FIG. 3 in embodiment of this invention

符号の説明Explanation of symbols

１ ロボット関節角指令 θｒｃ
２ モータ位置フィードバック θｍ
３ モータ位置指令 θｃｏｍ
４ モータ速度フィードバック（モータ角速度） ωｍ
５ 位置比例ゲイン ＫＰＰ
６ 位置制御ブロック
７ 速度ループ指令 ωｃｏｍ
８ 速度比例ゲイン ＫＰ
９ 速度積分ゲイン ＫＩ
１０ 速度制御ブロック
１１ モータ電流指令 Ｉｍ
１２ モータトルク定数 Ｋｔ
１３ モータ発生トルク τｍ
１４ 動摩擦トルク τμ
１５ 微分要素
１６ 動力学トルク τｄｙｎ
１７ モータ伝達関数
１８ モータと外力を示したブロック
１９ 他軸のロボット関節角 θｒ
２０ アーム重力トルク演算ブロック
２１ アーム重力トルク τｇＡ［θｒ］
２２ データ蓄積・摩擦トルク除去ブロック
２３ τｇＩ［θｒ］（モータ発生トルク重力成分）
２４ 負荷に起因する重力トルク τｇＬ［θｒ］
２５ 振幅・位相算出手段
２６ 振幅 Ａ５
２７ 位相 φ５
２８ 重心位置・質量算出手段
２９ Ｘｌ（負荷オフセットＸ成分）
３０ Ｚｌ（負荷オフセットＺ成分）
３１ Ｙｌ（負荷オフセットＹ成分）
３２ 負荷質量 Ｍ
３３ 負荷質量・オフセット推定ブロック
３４ 減速比逆数 １／Ｒｇ
３５ トルク定数、減速比の積
３６ ロボット関節角 θｒ
３７ 減速比 Ｒｇ
４０ 第１軸
４１ 第２軸
４２ 第３軸
４３ 第４軸
４４ 第５軸
４５ 第６軸
４６ アーム長
４７ アーム長
４８ 負荷
５０ 第４軸２３のアームの一部
５２ 測定動作角 Δθｒ（第５軸）
５３ 測定動作角 Δθｒ（第６軸）
５４ 正方向モータ発生トルク Ｋｔ・Ｒｇ・Ｉｍ［θｒ］ｐ
５５ 負方向モータ発生トルク Ｋｔ・Ｒｇ・Ｉｍ［θｒ］ｍ
５６ データ測定間隔 ｄｒ
５７ データペアリング間隔 Δθｒ／２ 1 Robot joint angle command θrc
2 Motor position feedback θm
3 Motor position command θcom
4 Motor speed feedback (motor angular speed) ωm
5 Position proportional gain KPP
6 Position control block 7 Speed loop command ωcom
8 Speed proportional gain KP
9 Speed integral gain KI
10 Speed control block 11 Motor current command Im
12 Motor torque constant Kt
13 Motor generated torque τm
14 Dynamic friction torque τμ
15 Differential element 16 Dynamic torque τdyn
17 Motor transfer function 18 Block showing motor and external force 19 Robot joint angle of other axis θr
20 Arm gravity torque calculation block 21 Arm gravity torque τgA [θr]
22 Data accumulation / friction torque removal block 23 τgI [θr] (motor generated torque gravity component)
24 Gravity torque due to load τgL [θr]
25 Amplitude / phase calculation means 26 Amplitude A5
27 Phase φ5
28 Center of gravity position / mass calculation means 29 Xl (load offset X component)
30 Zl (load offset Z component)
31 Yl (load offset Y component)
32 Load mass M
33 Load mass / offset estimation block 34 Inverse reduction ratio 1 / Rg
35 Product of torque constant and reduction ratio 36 Robot joint angle θr
37 Reduction ratio Rg
40 1st axis 41 2nd axis 42 3rd axis 43 4th axis 44 5th axis 45 6th axis 46 Arm length 47 Arm length 48 Load 50 Part of arm of 4th axis 23 52 Measurement operating angle Δθr (5th axis)
53 Measurement operating angle Δθr (6th axis)
54 Positive motor generated torque Kt · Rg · Im [θr] p
55 Negative direction motor generated torque Kt ・ Rg ・ Im [θr] m
56 Data measurement interval dr
57 Data pairing interval Δθr / 2

Claims (5)

モータにより駆動され、回転中心軸が略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、
前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させ所定の位相間隔を有しており所定の角度に位置する２点における各点の角度とその角度における電流測定値とを求め、
これら角度と電流測定値とから前記負荷の質量に起因するトルクの振幅と位相とを各軸ごとに求め、
前記トルクの振幅と位相に基づいて前記負荷の質量及び重心位置を算出するロボットの制御方法。 A plurality of arms driven by a motor and having at least one pair of two rotation shafts whose rotation center axes are substantially orthogonal to each other, and a load is mounted on the arm rotating around the rotation shaft on the tip end side of the two rotation shafts A robot control method used as
The arms rotating around the two rotation axes are moved at equal angular speeds for each axis, and the angle of each point at two points having a predetermined phase interval and located at a predetermined angle, and current measurement at that angle. Find the value and
Obtain the amplitude and phase of torque due to the mass of the load from these angles and current measurement values for each axis,
A robot control method for calculating a mass and a gravity center position of the load based on an amplitude and a phase of the torque. モータにより駆動され、２つの回転中心軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸と略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、
前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させ所定の位相間隔を有しており所定の角度に位置する２点における各点の角度とその角度における電流測定値とを求め、
これら角度と電流測定値とから前記負荷の質量に起因するトルクの振幅と位相とを各軸ごとに求め、
前記トルクの振幅と位相に基づいて前記負荷の質量及び重心位置を算出するロボットの制御方法。 Provided with a plurality of arms driven by a motor and having at least one set of two rotation axes in which a plane formed by the straight line connecting the two rotation center axes and the one rotation center axis is substantially orthogonal to the other rotation center axis A method for controlling a robot used by mounting a load on an arm that rotates around a rotation axis on the arm tip side of the two rotation axes,
The arms rotating around the two rotation axes are moved at equal angular speeds for each axis, and the angle of each point at two points having a predetermined phase interval and located at a predetermined angle, and current measurement at that angle. Find the value and
Obtain the amplitude and phase of torque due to the mass of the load from these angles and current measurement values for each axis,
A robot control method for calculating a mass and a gravity center position of the load based on an amplitude and a phase of the torque. 所定の位相間隔を有する２点の位相間隔は９０度より小さく１０度より大きい請求項１または２記載のロボットの制御方法。 The robot control method according to claim 1 or 2, wherein a phase interval between two points having a predetermined phase interval is smaller than 90 degrees and larger than 10 degrees. 所定の位相間隔を有しており所定の角度に位置する２点を複数設定し、各２点において求めた負荷質量に起因するトルクの位相と振幅の平均を算出し、この位相と振幅の平均に基づいて負荷の質量と重心位置を算出する請求項１から３のいずれか１項に記載のロボットの制御方法。 A plurality of two points having a predetermined phase interval and positioned at a predetermined angle are set, and the average of the phase and amplitude of the torque caused by the load mass obtained at each of the two points is calculated. The robot control method according to any one of claims 1 to 3, wherein a load mass and a center of gravity position are calculated based on the equation (1). 複数の２点は、所定の２点から各軸の移動方向に微小角度移動した位置にある点である請求項４記載のロボットの制御方法。 The robot control method according to claim 4, wherein the plurality of two points are points that are moved by a minute angle in a moving direction of each axis from two predetermined points.

Images