JP3442941B2 - Robot vibration suppression control device and control method thereof - Google Patents
Robot vibration suppression control device and control method thereofInfo
- Publication number
- JP3442941B2 JP3442941B2 JP26004796A JP26004796A JP3442941B2 JP 3442941 B2 JP3442941 B2 JP 3442941B2 JP 26004796 A JP26004796 A JP 26004796A JP 26004796 A JP26004796 A JP 26004796A JP 3442941 B2 JP3442941 B2 JP 3442941B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- joint
- tip
- acceleration
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Manipulator (AREA)
Description
【０００１】[0001]
【発明の属する技術分野】本発明はロボットの振動抑制
制御装置およびその制御方法に係り、とりわけ関節部を
介して互いに連結された複数のアームリンク部を有する
アームに生じる振動を効果的に抑制することができるロ
ボットの振動抑制制御装置およびその制御方法に関す
る。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a vibration suppression control device for a robot and a control method therefor, and more particularly, it effectively suppresses vibrations generated in arms having a plurality of arm link parts connected to each other via joint parts. The present invention relates to a vibration suppression control device for a robot and a control method thereof.
【０００２】[0002]
【従来の技術】従来から、関節部を介して互いに連結さ
れた複数のアームリンク部を有するアームを備え、この
アームを２次元平面内または３次元空間内の目標位置に
位置決めするとともに、アームの先端に取り付けた手先
効果器等のツールにより各種の作業を行うロボットが知
られている。このようなロボットでは一般に、アームの
先端と反対側の端部においてアーム全体を支える構造と
なっており、また各関節部はアームリンク部を回転させ
るトルク伝達機構や減速機構が有するバネ要素の影響に
よりアームリンク部に比べて剛性が低くなっている。こ
のため、特に関節部の数が多く、またアームの全長が長
いロボットにおいては、アームの運動やアームに加わる
外乱等によってアームに振動が生じやすく、アームの先
端を速やかに目標位置に位置決めしたり、アームの先端
が位置決め保持状態にあるときに加わった外乱による振
動を速やかに抑制することが困難であるという問題があ
る。2. Description of the Related Art Conventionally, an arm having a plurality of arm link portions connected to each other through a joint portion is provided, the arm is positioned at a target position in a two-dimensional plane or a three-dimensional space, and Robots that perform various work using tools such as a hand-effector attached to the tip are known. In general, such a robot has a structure in which the end of the arm opposite to the tip of the arm supports the entire arm, and each joint has an effect of a spring element included in a torque transmission mechanism or a reduction mechanism that rotates an arm link. Therefore, the rigidity is lower than that of the arm link part. Therefore, particularly in a robot having a large number of joints and a long total length of the arm, the arm is likely to vibrate due to movement of the arm, disturbance applied to the arm, etc., and the tip of the arm can be quickly positioned at the target position. However, there is a problem that it is difficult to quickly suppress the vibration due to the disturbance applied when the tip of the arm is in the positioning and holding state.
【０００３】このようなロボットにおける振動を抑制す
るための方法としては一般に、次のようものが提案され
ている。例えば、１つまたは２つという少数の関節部を
有するアームを備えたロボットにおいて、アームに生じ
た歪みや加速度をセンサにより検出し、このような検出
信号に基づいてフィードバック補償を行う方法が提案さ
れている。しかし、この方法はあくまでも少数の関節部
を有するアームを前提にしており、アームの関節数が多
くなった場合に、各センサをどの位置にどのように配置
するのか、また検出信号から各関節部へのフィードバッ
ク補償をどのように行うのかといったことについては問
題が多く残されている。The following methods have been generally proposed as methods for suppressing the vibration in such a robot. For example, in a robot provided with an arm having a small number of joints such as one or two, a method is proposed in which a sensor detects strain or acceleration generated in the arm, and feedback compensation is performed based on such a detection signal. ing. However, this method is based on the assumption that the arm has a small number of joints, and when the number of joints in the arm is large, how to position each sensor and how to detect each joint based on the detection signal. Many questions remain about how to provide feedback compensation to.
【０００４】またこの他に、関節部を駆動するモータの
回転角および回転速度からアームリンク部の回転角およ
び回転速度を推定するオブザーバ制御が組み込まれた産
業用ロボットにおいて、推定された回転角および回転速
度に応じて防振制御を行う方法も提案されている。しか
し、この方法は制御系の設計が複雑であるとともに制御
のための計算量も多く、また数Ｈｚ以下の低い固有振動
数を有するロボットにおいては十分な振動抑制効果が期
待できないという問題がある。In addition to the above, in an industrial robot incorporating an observer control for estimating the rotation angle and the rotation speed of the arm link portion from the rotation angle and the rotation speed of the motor that drives the joint portion, the estimated rotation angle and A method of performing anti-vibration control according to the rotation speed has also been proposed. However, this method has a problem that the design of the control system is complicated, the amount of calculation for control is large, and a sufficient vibration suppressing effect cannot be expected in a robot having a low natural frequency of several Hz or less.
【０００５】なお、ロボットの振動抑制制御に関しての
従来の研究および開発は、主として宇宙等における利用
を目的にしたフレキシブル・マニピュレータ、すなわち
アームリンク部の軽量化を図りアームリンク部自体も弾
性によりたわみが生じるようなロボットを対象としたも
のが多い。このようなロボットにおいては、アームリン
ク部を分布定数系として定式化しなければならず、また
高次の振動モードまで考慮する必要が生じることから、
挙動を表す動特性モデルも非常に複雑なものとなる。こ
のため、このようなロボットにおいては、主として自由
度が１または２のアームを制御対象として振動抑制制御
を実行した報告例がほとんどであり、実用的な多くの自
由度を有するアームに対して適用するには複雑な制御系
の設計が必要であったり、制御のための計算時間も多く
かかるなどの問題も多い。Conventional research and development relating to vibration suppression control of robots has been intended to reduce the weight of a flexible manipulator, that is, an arm link portion, which is mainly intended for use in space, etc. Many of them are aimed at robots that will occur. In such a robot, the arm link part must be formulated as a distributed constant system, and it is necessary to consider even higher-order vibration modes.
The dynamic characteristic model representing the behavior is also very complicated. For this reason, in such a robot, most of the reported examples are mainly those in which vibration suppression control is executed mainly for an arm having one or two degrees of freedom, and it is applied to an arm having many practical degrees of freedom. In order to do so, complicated control system design is required, and there are many problems such as long calculation time for control.
【０００６】[0006]
【発明が解決しようとする課題】上述したように、従来
のロボットにおいては、多数の関節部を有する自由度の
大きなロボットの振動を効果的に抑制する方法は確立さ
れていない。すなわち、本発明が対象とするようなアー
ムリンク部を連結する関節部の数が多く、またアームの
全長が長いロボットにおいては、固有振動数が数Ｈｚ程
度にまで低下して非常にゆっくりした振動が生じやすく
なり、ツールが取り付けられるアームの先端を速やかに
目標位置に位置決めしたり、アームの先端が位置決め保
持状態にあるときに加わった外乱による振動を速やかに
抑制することが非常に困難である。As described above, in the conventional robot, a method for effectively suppressing the vibration of the robot having a large number of joints and having a large degree of freedom has not been established. That is, in a robot having a large number of joints connecting the arm link portions and a long total length of the arm, which is the object of the present invention, the natural frequency is lowered to about several Hz and a very slow vibration occurs. Is likely to occur, and it is very difficult to quickly position the tip of the arm to which the tool is attached to the target position, and to quickly suppress the vibration due to the disturbance applied when the tip of the arm is in the positioning and holding state. .
【０００７】本発明はこのような点を考慮してなされた
ものであり、アームリンク部に比べて剛性が低い複数の
関節部を有し、数Ｈｚ以下の低い固有振動数によるゆっ
くりした固有振動を持つアームを備えたロボットにおい
て十分な振動抑制効果を得ることができるロボットの振
動抑制制御装置およびその制御方法を提供することを目
的とする。The present invention has been made in consideration of such a point, and has a plurality of joint portions having a rigidity lower than that of the arm link portion and has a slow natural vibration due to a low natural frequency of several Hz or less. It is an object of the present invention to provide a vibration suppression control device for a robot and a control method therefor capable of obtaining a sufficient vibration suppression effect in a robot provided with an arm having.
【０００８】[0008]
【課題を解決するための手段】本発明は、関節部を介し
て互いに連結された複数のアームリンク部を有するアー
ムと、前記アームの先端を目標位置に位置決めするため
前記アームの各関節部を所定指令値に基づいて駆動する
駆動部とを備えたロボットで用いられる振動抑制制御装
置において、前記アームの先端近傍に設けられ前記アー
ムの先端の加速度量を検出する加速度センサと、前記加
速度センサにより検出された加速度量に基づいて前記ア
ームの先端に生じた振動を抑制するように前記各関節部
ごとの前記駆動部への所定指令値を補償する前記各関節
部ごとの補償成分を算出するとともに、算出された前記
各関節部ごとの補償成分を対応する前記各関節部ごとの
所定指令値から減算する演算部とを備え、前記演算部
は、前記加速度センサにより検出された前記加速度量を
前記アームの先端に作用した等価な力相当成分とみな
し、この力相当成分に準静力学的扱いで前記アームの構
造に応じた所定の変換を施すことにより、前記アームの
前記各関節部を駆動する所定指令値に対する補償成分を
算出することを特徴とする、ロボットの振動抑制制御装
置を提供する。According to the present invention, an arm having a plurality of arm link portions connected to each other via a joint portion and each joint portion of the arm for positioning a tip of the arm at a target position are provided. In a vibration suppression control device used in a robot provided with a drive unit that is driven based on a predetermined command value, an acceleration sensor that is provided in the vicinity of the tip of the arm and detects an acceleration amount at the tip of the arm, and the acceleration sensor Based on the detected acceleration amount, while calculating a compensation component for each joint unit that compensates a predetermined command value to the drive unit for each joint unit so as to suppress vibration generated at the tip of the arm, A calculation unit that subtracts the calculated compensation component for each joint unit from a corresponding predetermined command value for each joint unit, wherein the calculation unit is the acceleration sensor. The acceleration amount detected by is regarded as an equivalent force equivalent component acting on the tip of the arm, and the force equivalent component is subjected to predetermined conversion according to the structure of the arm by quasi-static treatment, A vibration suppression control device for a robot, characterized in that a compensation component for a predetermined command value for driving each joint of the arm is calculated.
【０００９】また本発明は、関節部を介して互いに連結
された複数のアームリンク部を有するアームと、前記ア
ームの先端を目標位置に位置決めするため前記アームの
各関節部を所定指令値に基づいて駆動する駆動部とを備
えたロボットで用いられる振動抑制制御方法において、
前記アームの先端近傍の加速度量を検出する工程と、こ
の工程により検出された加速度量に基づいて前記アーム
の先端に生じた振動を抑制するように前記各関節部ごと
の前記駆動部への所定指令値を補償する前記各関節部ご
との補償成分を算出する工程と、この工程により算出さ
れた前記各関節部ごとの補償成分を対応する前記各関節
部ごとの所定指令値から減算する工程とを備え、前記補
償成分を算出する工程において、前記アームの先端近傍
で検出された前記加速度量を前記アームの先端に作用し
た等価な力相当成分とみなし、この力相当成分に準静力
学的扱いで前記アームの構造に応じた所定の変換を施す
ことにより、前記アームの前記各関節部を駆動する所定
指令値に対する補償成分を算出することを特徴とする、
ロボットの振動抑制制御方法を提供する。Further, according to the present invention, an arm having a plurality of arm link portions connected to each other via a joint portion, and each joint portion of the arm based on a predetermined command value for positioning the tip of the arm at a target position. In a vibration suppression control method used in a robot provided with a drive unit driven by
A step of detecting an acceleration amount in the vicinity of the tip of the arm, and a predetermined step for the drive unit for each joint so as to suppress vibration generated at the tip of the arm based on the acceleration amount detected in this step. Calculating a compensation component for each joint that compensates a command value, and subtracting the compensation component for each joint calculated in this step from a corresponding predetermined command value for each joint In the step of calculating the compensation component, the acceleration amount detected near the tip of the arm is regarded as an equivalent force equivalent component acting on the tip of the arm, and this force equivalent component is treated as quasi-static. By performing a predetermined conversion according to the structure of the arm, a compensation component for a predetermined command value for driving each joint portion of the arm is calculated.
A vibration suppression control method for a robot is provided.
【００１０】本発明によれば、アームリンク部に比べて
剛性が低い複数の関節部を有し、数Ｈｚ以下の低い固有
振動数によるゆっくりした固有振動を持つアームを備え
たロボットにおいて、加速度センサにより検出された加
速度量をアームの先端に作用した等価な力相当成分とみ
なし、この力相当成分に基づいてアームの先端に生じた
振動を抑制するように各関節部に対応する駆動部への各
関節部ごとの所定指令値を補償するので、アームの先端
を速やかに目標位置に位置決めすることができ、またア
ームの先端が位置決め保持状態にあるときに加わった外
乱による振動を速やかに抑制することができる。According to the present invention, an acceleration sensor is provided in a robot having a plurality of joints having a rigidity lower than that of an arm link and having an arm having a slow natural vibration due to a low natural frequency of several Hz or less. The acceleration amount detected by is regarded as an equivalent force equivalent component acting on the tip of the arm, and based on this force equivalent component, the vibrations generated at the tip of the arm are suppressed by the drive unit corresponding to each joint. Since the predetermined command value for each joint is compensated, the tip of the arm can be quickly positioned at the target position, and the vibration due to the disturbance applied while the tip of the arm is in the positioning and holding state is quickly suppressed. be able to.
【００１１】また、関節部の数が多くなってもアームの
運動可能方向に対応してアームの先端に２方向または３
方向の加速度成分を検出できる加速度センサを取り付け
るだけでよく、各関節部に対応する駆動部に加えられる
所定指令値の補償成分を計算する方法も複雑でなく、ま
た計算時間も多くを必要としない。さらに、ロボットの
アームの全ての関節部がそれぞれアームの先端での振動
を抑制するように作用するので、補償成分のフィードバ
ックゲイン等を大きく採らなくても十分な振動抑制効果
を得ることができ制御系を不安定化させるおそれが少な
い。In addition, even if the number of joints is large, the direction of the arm can move in two directions or three at the tip of the arm.
It is only necessary to attach an acceleration sensor that can detect the acceleration component in the direction, and the method of calculating the compensation component of the predetermined command value applied to the drive unit corresponding to each joint is not complicated and does not require much calculation time. . Furthermore, since all joints of the robot arm act to suppress the vibration at the tip of each arm, a sufficient vibration suppression effect can be obtained without using a large feedback gain of the compensation component. Less likely to destabilize the system.
【００１２】[0012]
【発明の実施の形態】以下、図面を参照して本発明の実
施の形態について説明する。BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described below with reference to the drawings.
【００１３】［第１の実施の形態］図１および図２は本
発明によるロボットの振動抑制制御装置の第１の実施の
形態を示す図である。まず、図１により、本発明の第１
の実施の形態が適用される２次元平面内を運動するアー
ムを有するロボットについて説明する。図１に示すよう
に、ロボットは、水平方向に回転可能な複数（ｎ個）の
関節部２−１〜２−ｎと、関節部２−１〜２−ｎを介し
て互いに連結された複数（ｎ個）のアームリンク部３−
１〜３−ｎとを有するアーム１０を備えており、関節部
２−１〜２−ｎにてアームリンク部３−１〜３−ｎを水
平方向に互いに相対的に回転させることによりアーム１
０の先端を２次元平面内の目標位置に移動できるように
なっている。[First Embodiment] FIGS. 1 and 2 are views showing a first embodiment of a vibration suppression control apparatus for a robot according to the present invention. First, referring to FIG.
A robot having an arm that moves in a two-dimensional plane to which the above embodiment is applied will be described. As shown in FIG. 1, the robot includes a plurality of (n) joints 2-1 to 2-n that can rotate in the horizontal direction and a plurality of joints connected to each other through the joints 2-1 to 2-n. (N) arm link unit 3-
1 to 3-n, and the arm 1 by rotating the arm link parts 3-1 to 3-n horizontally relative to each other at the joint parts 2-1 to 2-n.
The tip of 0 can be moved to the target position in the two-dimensional plane.
【００１４】アーム１０の先端（アームリンク部３−ｎ
の先端）には、物体を把持する機能等を実現する手先効
果器４が装着されている。またアーム１０の先端と反対
側の端部（アームリンク部３−１の１つの端部）は、第
１関節部２−１を介してアーム１０全体を支持するベー
ス部１に取り付けられている。The tip of the arm 10 (arm link portion 3-n
A hand effector 4 that realizes a function of gripping an object and the like is attached to the tip end of the. The end of the arm 10 opposite to the tip (one end of the arm link 3-1) is attached to the base 1 that supports the entire arm 10 via the first joint 2-1. .
【００１５】関節部２−１〜２−ｎの各々には、互いに
連結されたアームリンク部３−１〜３−ｎを相対的に回
転させる関節駆動モータ１５−１〜１５−ｎ（図２参
照）および減速機構（図示せず）が設けられている。な
お、これらの関節駆動モータや減速機構に含まれるバネ
要素の影響により、関節部２−１〜２−ｎはアームリン
ク部３−１〜３−ｎに比べて剛性が低く、アーム１０の
先端に機械的な振動を生じやすい構造となっている。Joint drive motors 15-1 to 15-n (FIG. 2) for relatively rotating the arm link portions 3-1 to 3-n connected to each other to the joint portions 2-1 to 2-n (FIG. 2). And a deceleration mechanism (not shown). Note that the joint parts 2-1 to 2-n have lower rigidity than the arm link parts 3-1 to 3-n due to the influence of the spring elements included in these joint drive motors and reduction mechanisms, and the tip of the arm 10 is The structure is prone to mechanical vibration.
【００１６】またアーム１０の先端の手先効果器４の取
り付け部近傍には、アーム１０の先端の加速度量を検出
する加速度センサ５が装着されている。加速度センサ５
は、アーム先端座標系６のｘ，ｙ２方向の加速度成分を
検出するために各方向に対応した２つの加速度センサ５
ａ，５ｂ（図２参照）を有している。An acceleration sensor 5 for detecting the amount of acceleration at the tip of the arm 10 is mounted near the attachment portion of the end effector 4 at the tip of the arm 10. Acceleration sensor 5
Is the two acceleration sensors 5 corresponding to the respective directions in order to detect acceleration components in the x and y2 directions of the arm tip coordinate system 6.
a, 5b (see FIG. 2).
【００１７】なお図１において、アーム先端座標系６は
アーム１０の先端の移動に合わせてその位置および姿勢
が変化する座標系であり、基準座標系７はベース部１を
基準にしてその位置および姿勢が変化しない座標系であ
る。In FIG. 1, the arm tip coordinate system 6 is a coordinate system whose position and attitude change in accordance with the movement of the tip of the arm 10, and the reference coordinate system 7 has its position and posture with reference to the base portion 1. It is a coordinate system in which the posture does not change.
【００１８】次に、図２により、図１に示すロボットの
振動抑制制御装置について説明する。図２に示すよう
に、ロボットの振動抑制制御装置は、関節部２−１〜２
−ｎの各々の駆動を制御してアーム１０全体の運動を制
御する制御装置８のサーボ制御系として実現されてい
る。なおサーボ制御系は、関節部２−１〜２−ｎの各々
に対応して複数設けられている。すなわち第ｉ関節部２
−ｉ（１≦ｉ≦ｎ）を例にとれば、そのサーボ制御系
は、アーム１０を運動させるために第ｉ関節部２−ｉの
駆動を制御する既存のサーボ制御系と、この既存のサー
ボ制御系に追加されるアーム１０の先端に生じる振動を
抑制するための加速度フィードバック補償系とからなっ
ている。Next, the vibration suppression control device for the robot shown in FIG. 1 will be described with reference to FIG. As shown in FIG. 2, the vibration suppression control device of the robot has joint units 2-1 and 2-2.
It is realized as a servo control system of the control device 8 that controls the drive of each of the −n to control the movement of the entire arm 10. A plurality of servo control systems are provided for each of the joint parts 2-1 to 2-n. That is, the i-th joint 2
Taking −i (1 ≦ i ≦ n) as an example, the servo control system includes an existing servo control system that controls the drive of the i-th joint section 2-i to move the arm 10, and this existing servo control system. It is composed of an acceleration feedback compensation system for suppressing vibration generated at the tip of the arm 10 added to the servo control system.
【００１９】アーム１０を運動させるための既存のサー
ボ制御系は、第ｉ関節部２−ｉの回転角度を検出する角
度検出器１６−ｉと、角度検出器１６−ｉにより検出さ
れた信号に基づいて現時点の関節角θi を関節角目標値
θref i に近づけるようにフィードバック制御を行う位
置決めフィードバック制御系１３−ｉと、第ｉ関節部２
−ｉを駆動する関節駆動モータ１５−ｉと、位置決めフ
ィードバック制御系１３−ｉと関節駆動モータ１５−ｉ
との間に設けられ位置決めフィードバック制御系１３−
ｉから加えられる速度指令値に基づいて関節駆動モータ
１５−ｉを駆動する速度指令型のサーボドライバ１４−
ｉとから構成されている。なお、角度検出器１６−ｉに
はエンコーダを用いることができる。The existing servo control system for moving the arm 10 uses an angle detector 16-i for detecting the rotation angle of the i-th joint 2-i and a signal detected by the angle detector 16-i. Based on the positioning feedback control system 13-i that performs feedback control so that the current joint angle θ i approaches the joint angle target value θ ref i , and the i-th joint 2
-I, the joint drive motor 15-i, the positioning feedback control system 13-i, and the joint drive motor 15-i.
Positioning feedback control system 13-
speed command type servo driver 14 that drives the joint drive motor 15-i based on the speed command value added from i
i and. An encoder can be used as the angle detector 16-i.
【００２０】また、このような既存のサーボ制御系に追
加される加速度フィードバック補償系は、上述した２つ
の加速度センサ５ａ，５ｂと、加速度センサ５ａ，５ｂ
により検出されたｘ，ｙ２方向の信号を取り込み所定の
増幅および濾波を行うアンプおよびローパスフィルター
１１とを有し、アンプおよびローパスフィルター１１を
通過した信号（ｘ，ｙ２方向の加速度成分）は演算器１
２に入力される。演算器１２はアンプおよびローパスフ
ィルター１１を通過した信号に基づいて上述したサーボ
ドライバ１４に加えられる第ｉ関節部２−ｉの速度指令
値をアーム１０の先端に生じた振動を抑制するように補
償する第ｉ関節部２−ｉの加速度フィードバック補償成
分を算出するとともに、この算出された値に所定のフィ
ードバックゲインＫαi を乗じた値を位置決めフィード
バック制御系１３−ｉからの速度指令値から減算するよ
うになっている。The acceleration feedback compensation system added to such an existing servo control system is the above-described two acceleration sensors 5a and 5b and the acceleration sensors 5a and 5b.
And a low-pass filter 11 for taking in signals in the x and y2 directions detected by the amplifier and performing predetermined amplification and filtering, and a signal (acceleration component in the x and y2 directions) passing through the amplifier and the low-pass filter 11 is a calculator. 1
Entered in 2. The calculator 12 compensates the speed command value of the i-th joint section 2-i applied to the servo driver 14 based on the signal passed through the amplifier and the low-pass filter 11 so as to suppress the vibration generated at the tip of the arm 10. The acceleration feedback compensation component of the i-th joint 2-i is calculated, and a value obtained by multiplying the calculated value by a predetermined feedback gain Kα i is subtracted from the speed command value from the positioning feedback control system 13-i. It is like this.
【００２１】次に、このような構成からなる本発明の第
１の実施の形態の作用について説明する。Next, the operation of the first embodiment of the present invention having the above structure will be described.
【００２２】ロボットのアーム１０の先端を目標位置に
位置決めするため、まずアーム１０の先端を目標位置に
位置決めするために必要とされる関節部２−１〜２−ｎ
の各々の回転角度、すなわち各関節部２−ｉ（１≦ｉ≦
ｎ）の関節角目標値θref iを求める。次に、位置決め
フィードバック制御系１３−ｉにより、角度検出器１６
−ｉにより検出された信号に基づいて各関節部２−ｉの
現時点の関節角θi を関節角目標値θref i に近づける
ようにフィードバック制御が行われ、サーボドライバ１
４−ｉに加えられる速度指令値が求められる。In order to position the tip of the arm 10 of the robot at the target position, first, the joint portions 2-1 to 2-n required to position the tip of the arm 10 at the target position.
Rotation angle of each, that is, each joint 2-i (1 ≦ i ≦
n) The joint angle target value θ ref i is calculated. Next, the angle detector 16 is controlled by the positioning feedback control system 13-i.
Feedback control is performed based on the signal detected by -i so that the current joint angle θ i of each joint 2-i approaches the joint angle target value θ ref i , and the servo driver 1
The speed command value added to 4-i is obtained.
【００２３】一方、加速度センサ５ａ，５ｂにより検出
されたｘ，ｙ２方向の信号はアンプおよびローパスフィ
ルター１１に取り込まれ、アンプおよびローパスフィル
タ１１により所定の増幅および濾波が行われた後に演算
部１２に読み込まれる。次に、演算部１２は、読み込ま
れた信号（ｘ，ｙ２方向の加速度成分）に基づいてアー
ム１０の先端に生じた振動を抑制するように第ｉ関節部
２−ｉの速度指令値を補償する第ｉ関節部２−ｉの加速
度フィードバック補償成分を算出する。続いて、演算部
１２は、算出された値に所定のフィードバックゲインＫ
αi を乗じた値を算出し、この算出された値を上述した
ようにして求められた位置決めフィードバック制御系１
３−ｉからの速度指令値から減算する。On the other hand, the signals in the x and y2 directions detected by the acceleration sensors 5a and 5b are taken in by the amplifier and the low-pass filter 11, and after being amplified and filtered by the amplifier and the low-pass filter 11 in a predetermined manner, they are sent to the arithmetic unit 12. Is read. Next, the calculation unit 12 compensates the speed command value of the i-th joint unit 2-i so as to suppress the vibration generated at the tip of the arm 10 based on the read signal (acceleration components in the x and y2 directions). The acceleration feedback compensation component of the i-th joint section 2-i is calculated. Subsequently, the calculation unit 12 sets the calculated value to a predetermined feedback gain K.
A value multiplied by α i is calculated, and the calculated value is used as described above to determine the positioning feedback control system 1
Subtract from the speed command value from 3-i.
【００２４】ここで、演算部１２における計算手順の詳
細について説明する。加速度センサ５ａ，５ｂにより検
出されたアーム先端座標系６におけるｘ，ｙ２方向の加
速度成分をαxtip，αytipとすると、まず次式（１）に
より、これらの加速度成分αxtip，αytipを基準座標系
７における加速度成分α0 ＝（αx0，αy0）に変換す
る。Here, the details of the calculation procedure in the calculation unit 12 will be described. Assuming that the acceleration components in the x and y2 directions in the arm tip coordinate system 6 detected by the acceleration sensors 5a and 5b are α xtip and α ytip , these acceleration components α xtip and α ytip are first used as a reference according to the following equation (1). The acceleration component in the coordinate system 7 is converted into α 0 = (α x0 , α y0 ).
【００２５】[0025]
【数１】
上式（１）において、0 Ｒtip はアーム先端座標系６か
ら基準座標系７への変換回転行列であり、具体的には次
式（２）のように表される。[Equation 1] In the above equation (1), 0 R tip is a transformation rotation matrix from the arm tip coordinate system 6 to the reference coordinate system 7, and is specifically expressed as the following equation (2).
【００２６】[0026]
【数２】
上式（２）において、ロボット１０の関節部２−１〜２
−ｎの各々の角度をθ1 ，θ2 ，…，θn と表すと、Ｃ
12..n はｃｏｓ（θ1 ＋θ2 ＋…＋θn ）であり、Ｓ
12..n はｓｉｎ（θ1 ＋θ2 ＋…＋θn ）である。[Equation 2] In the above formula (2), the joint parts 2-1 to 2-1 of the robot 10
When each angle of −n is represented by θ 1 , θ 2 , ..., θ n , C
12..n is cos (θ 1 + θ 2 + ... + θ n ), and S
12..n is sin (θ 1 + θ 2 + ... + θ n ).
【００２７】上式（２）により変換された加速度成分α
x0，αy0をアーム１０の先端に作用した等価な力相当成
分とみなすことにより、加速度成分αx0，αy0はロボッ
トのアーム１０の構造で決まるヤコビ行列Ｊの転置行列
ＪT を用いて準静力学的に関節部２−１〜２−ｎの各々
の駆動トルク成分に変換することができる。すなわち、
次式（３）により、アーム１０の先端に生じた振動を抑
制するための関節部２−１〜２−ｎの各々に対する加速
度フィードバック補償成分αθ＝（αθ1 ，αθ2 ，
…，αθn ）が求められる。Acceleration component α converted by the above equation (2)
By considering x0 and α y0 as equivalent force equivalent components acting on the tip of the arm 10, the acceleration components α x0 and α y0 are quasi-quantized using the transposed matrix J T of the Jacobian matrix J determined by the structure of the robot arm 10. It can be statically converted into each driving torque component of the joint portions 2-1 to 2-n. That is,
According to the following equation (3), the acceleration feedback compensation component α θ = (α θ1 , α θ2 , for each of the joint parts 2-1 to 2-n for suppressing the vibration generated at the tip of the arm 10
…, Α θn ) is obtained.
【数３】 [Equation 3]
【００２８】ここで、図２に示すような速度指令型のサ
ーボドライバ１４−ｉを用いた場合には、上式（３）に
より求められた加速度フィードバック補償成分にフィー
ドバックゲインＫαi を乗じた値を、位置制御ループ系
である位置決めフィードバック制御系１３−ｉからサー
ボドライバ１４−ｉに加えられる速度指令値から減算す
ることにより、補償後の速度指令値を求める。この関係
を次式（４）に示す。Here, when the speed command type servo driver 14-i as shown in FIG. 2 is used, a value obtained by multiplying the acceleration feedback compensation component obtained by the above equation (3) by the feedback gain Kα i. Is subtracted from the speed command value applied to the servo driver 14-i from the positioning feedback control system 13-i, which is a position control loop system, to obtain the compensated speed command value. This relationship is shown in the following equation (4).
【数４】 [Equation 4]
【００２９】このようにしてアーム１０の先端に生じた
振動を抑制するように補償された速度指令値がサーボド
ライバ１４−ｉに加えられ、サーボドライバ１４−ｉに
より最終的に関節駆動モータ１５−ｉに加えられるトル
ク指令値が求められ、アーム１０の先端に生じた振動を
抑制するように第ｉ関節部２−ｉが駆動される。同様に
して関節部２−１〜２−ｎの各々がアーム１０の先端に
生じた振動を抑制するように駆動されることにより、ア
ームリンク部３−１〜３−ｎは水平方向に互いに相対的
に回転され、アーム１０の先端が２次元平面内の目標位
置にその振動がきわめて抑制された状態で速やかに移動
することができる。In this way, the speed command value compensated so as to suppress the vibration generated at the tip of the arm 10 is added to the servo driver 14-i, and the servo driver 14-i finally causes the joint drive motor 15-. The torque command value applied to i is obtained, and the i-th joint section 2-i is driven so as to suppress the vibration generated at the tip of the arm 10. Similarly, each of the joint parts 2-1 to 2-n is driven so as to suppress the vibration generated at the tip of the arm 10, so that the arm link parts 3-1 to 3-n are horizontally opposed to each other. The arm 10 can be rapidly rotated to the target position in the two-dimensional plane with the vibration thereof being extremely suppressed, and swiftly moved.
【００３０】なお、トルク指令型のサーボドライバを用
いた場合には、上式（３）により求められた加速度フィ
ードバック補償成分にフィードバックゲインＫ′αｉを
乗じた値を、位置・速度制御ループ系からサーボドライ
バに加えられるトルク指令値から減算することにより、
補償後のトルク指令値を求める。ここで、フィードバッ
クゲインＫαi ，Ｋ′αi はそれぞれ、速度指令値また
はトルク指令値への単位換算係数が含められた加速度フ
ィードバック補償系が不安定にならないように設定され
た値である。また、Ｋｐi は単位換算係数が含められた
位置決め制御ループ系１３−ｉにおけるフィードバック
ゲインである。When a torque command type servo driver is used, the value obtained by multiplying the acceleration feedback compensation component obtained by the above equation (3) by the feedback gain K'α i is used as the position / speed control loop system. By subtracting from the torque command value applied to the servo driver,
Calculate the torque command value after compensation. Here, each of the feedback gains Kα i and K′α i is a value that is set so that the acceleration feedback compensation system including the unit conversion coefficient to the speed command value or the torque command value does not become unstable. Further, Kp i is a feedback gain in the positioning control loop system 13-i including a unit conversion coefficient.
【００３１】また、アーム１０の動作において各関節部
の動作速度が速く、アーム１０の先端での各関節部２−
ｉの動作による運動加速度成分αｍ＝（αxm，αym）を
考慮する必要がある場合には、上式（３）においてｘ，
ｙ２方向の加速度成分（αx0，αy0）から、次式（５）
により算出される運動加速度成分（αxm，αym）をそれ
ぞれ引いた値（α′x0，α′y0）をそのｘ，ｙ２方向の
加速度成分として用いるようにする。In addition, in the operation of the arm 10, the operating speed of each joint portion is high, and each joint portion 2-at the tip of the arm 10
When it is necessary to consider the motion acceleration component α m = (α xm , α ym ) due to the motion of i, in the above equation (3), x,
From the acceleration component (α x0 , α y0 ) in the y2 direction, the following equation (5)
The values (α ′ x0 , α ′ y0 ) obtained by subtracting the motion acceleration components (α xm , α ym ) calculated by are used as the acceleration components in the x and y2 directions.
【数５】 [Equation 5]
【００３２】［第２の実施の形態］次に、本発明の第２
の実施の形態について図５により説明する。本発明の第
２の実施の形態は、本発明によるロボットの振動抑制制
御装置およびその制御方法を、図５に示すような３次元
空間内を運動するアームを有するロボットに適用したも
のである。本発明の第２の実施の形態は、本発明が適用
されるロボットのアーム２０の構造が異なる点を除い
て、他は図１および図２に示す第１の実施の形態と略同
一である。本発明の第２の実施の形態において、図１お
よび図２に示す第１の実施の形態と同一部分には同一符
号を付して詳細な説明は省略する。[Second Embodiment] Next, the second embodiment of the present invention will be described.
The embodiment will be described with reference to FIG. The second embodiment of the present invention applies the robot vibration suppression control apparatus and control method according to the present invention to a robot having an arm that moves in a three-dimensional space as shown in FIG. The second embodiment of the present invention is substantially the same as the first embodiment shown in FIGS. 1 and 2 except that the structure of the arm 20 of the robot to which the present invention is applied is different. . In the second embodiment of the present invention, the same parts as those of the first embodiment shown in FIGS. 1 and 2 are designated by the same reference numerals, and detailed description thereof will be omitted.
【００３３】図５に示すように、ロボットは、水平方向
または、垂直方向等の傾いた方向に回転可能な複数（ｎ
個）の関節部２−１′〜２−ｎ′と、関節部２−１′〜
２−ｎ′を介して互いに連結された複数（ｎ個）のアー
ムリンク部３−１′〜３−ｎ′とを有するアーム２０を
備えており、関節部２−１′〜２−ｎ′にてアームリン
ク部３−１′〜３−ｎ′を水平方向または、垂直方向等
の傾いた方向に互いに相対的に回転させることによりア
ーム２０の先端を３次元空間内の目標位置に移動できる
ようになっている。As shown in FIG. 5, the robot is composed of a plurality of (n) which are rotatable in an inclined direction such as a horizontal direction or a vertical direction.
Joints 2-1 'to 2-n' and joints 2-1 'to 2-1'
An arm 20 having a plurality (n) of arm link portions 3-1 ′ to 3-n ′ connected to each other via 2-n ′ is provided, and the joint portions 2-1 ′ to 2-n ′. By rotating the arm link portions 3-1 'to 3-n' relative to each other in the inclined direction such as the horizontal direction or the vertical direction, the tip of the arm 20 can be moved to the target position in the three-dimensional space. It is like this.
【００３４】またアーム２０の先端の手先効果器４の取
り付け部近傍には、アーム２０の先端の加速度量を検出
する加速度センサ５′が装着されている。加速度センサ
５′は、アーム先端座標系６のｘ，ｙ，ｚ３方向の加速
度成分を検出するために各方向に対応した３つの加速度
センサを有している。An acceleration sensor 5'for detecting the amount of acceleration at the tip of the arm 20 is mounted near the attachment portion of the end effector 4 at the tip of the arm 20. The acceleration sensor 5'has three acceleration sensors corresponding to the respective directions in order to detect acceleration components in the x, y, z3 directions of the arm tip coordinate system 6.
【００３５】加速度センサ５′により検出されたｘ，
ｙ，ｚ３方向の信号は、図２に示すように、アーム２０
の先端に生じる振動を抑制するための加速度フィードバ
ック補償系においてアンプおよびローパスフィルター１
１を介して演算部１２に読み込まれるようになってい
る。X detected by the acceleration sensor 5 ',
Signals in the y and z3 directions are transmitted to the arm 20 as shown in FIG.
Amplifier and low-pass filter 1 in the acceleration feedback compensation system for suppressing the vibration generated at the tip of
The data is read into the calculation unit 12 via 1.
【００３６】次に、このような構成からなる本発明の第
２の実施の形態の作用について説明する。Next, the operation of the second embodiment of the present invention having the above structure will be described.
【００３７】図２に示すように、アーム２０を運動させ
るための既存のサーボ制御系において、位置決めフィー
ドバック制御系１３−ｉにより各々の関節部２−ｉ′の
現時点の関節角θi を関節角目標値θref i に近づける
ようにフィードバック制御が行われ、速度指令型のサー
ボドライバ１４−ｉに加えられる速度指令値が求められ
る。また同時に、加速度フィードバック補償系におい
て、演算部１２により、３つの加速度センサから読み込
まれた信号（ｘ，ｙ，ｚ３方向の加速度成分）に基づい
てアーム２０の先端に生じた振動を抑制するように位置
決めフィードバック制御系１３−ｉからサーボドライバ
１４−ｉに加えられる第ｉ関節部２−ｉ′の速度指令値
を補償する第ｉ関節部２−ｉ′の加速度フィードバック
補償成分が算出される。As shown in FIG. 2, in the existing servo control system for moving the arm 20, the positioning feedback control system 13-i determines the current joint angle θ i of each joint 2-i '. Feedback control is performed so as to approach the target value θref i, and the speed command value applied to the speed command type servo driver 14-i is obtained. At the same time, in the acceleration feedback compensation system, the calculation unit 12 suppresses the vibration generated at the tip of the arm 20 based on the signals (acceleration components in the x, y, z3 directions) read from the three acceleration sensors. The acceleration feedback compensation component of the i-th joint portion 2-i 'that compensates the speed command value of the i-th joint portion 2-i' applied to the servo driver 14-i from the positioning feedback control system 13-i is calculated.
【００３８】ここで、演算部１２における計算手順は基
本的に上述した第１の実施の形態と同様である。加速度
センサ５′により検出されたアーム先端座標系６におけ
るｘ，ｙ，ｚ３方向の加速度成分をαxtip，αytip，α
ztipとすると、まずこれらの加速度成分αxtip，
αytip，αztipをアーム先端座標系６から基準座標系７
への変換回転行列0 Ｒtip を用いて基準座標系７におけ
る加速度成分α0 ＝（αx0，αy0，αz0）に変換する。Here, the calculation procedure in the arithmetic unit 12 is basically the same as that in the above-described first embodiment. The acceleration components in the x, y, z3 directions in the arm tip coordinate system 6 detected by the acceleration sensor 5'are expressed as α xtip , α ytip , α
Let ztip be the acceleration component α xtip ,
Set α ytip and α ztip from the arm tip coordinate system 6 to the reference coordinate system 7
Conversion to the acceleration component α 0 = (α x0 , α y0 , α z0 ) in the reference coordinate system 7 using the rotation matrix 0 R tip .
【００３９】次に、上式（３）により、アーム２０の先
端に生じた振動を抑制するための関節部２−１′〜２−
ｎ′の各々に対する加速度フィードバック補償成分αθ
＝（αθ1 ，αθ2 ，…，αθn ）を求める。なお、こ
の第２の実施の形態においては、加速度センサ５′によ
り検出された加速度量α0 のｚ方向の加速度成分αz0か
ら重力加速度ｇを引いた値α′z0をそのｚ方向の加速度
成分として用いるようにする。Next, according to the above expression (3), the joint portions 2-1 'to 2- for suppressing the vibration generated at the tip of the arm 20.
Acceleration feedback compensation component α θ for each of n ′
= (Α θ1 , α θ2 , ..., α θn ) is calculated . In the second embodiment, the value α ′ z0 obtained by subtracting the gravitational acceleration g from the acceleration component α z0 in the z direction of the acceleration amount α 0 detected by the acceleration sensor 5 ′ is the acceleration component in the z direction. To use as.
【００４０】その後、図２に示すような速度指令型のサ
ーボドライバ１４−ｉを用いた場合には、上式（４）に
従って、上式（３）により求められた加速度フィードバ
ック補償成分にフィードバックゲインＫαi を乗じた値
を、位置制御ループ系である位置決めフィードバック制
御系１３−ｉからサーボドライバ１４−ｉに加えられる
速度指令値から減算することにより、補償後の速度指令
値を求める。そして、この求められた補償後の速度指令
値がサーボドライバ１４−ｉに加えられ、サーボドライ
バ１４−ｉによりアーム２０の先端に生じた振動を抑制
するように各関節部２−ｉ′が駆動される。After that, when the speed command type servo driver 14-i as shown in FIG. 2 is used, the feedback gain is added to the acceleration feedback compensation component obtained by the above equation (3) according to the above equation (4). The value obtained by multiplying Kα i is subtracted from the speed command value applied to the servo driver 14-i from the positioning feedback control system 13-i which is the position control loop system to obtain the compensated speed command value. Then, the obtained velocity command value after compensation is added to the servo driver 14-i, and each joint portion 2-i 'is driven so as to suppress the vibration generated at the tip of the arm 20 by the servo driver 14-i. To be done.
【００４１】なお、上述した第１および第２の実施の形
態においては、１方向の加速度成分を検出することがで
きる加速度センサを、アーム１０，２０の運動可能方向
（２方向または３方向）に合わせて必要な個数だけ所定
の位置および姿勢で装着するようにしているが、３方向
の加速度成分を同時に検出することができる加速度セン
サを用いるようにしてもよいことはもちろんである。In the first and second embodiments described above, an acceleration sensor capable of detecting an acceleration component in one direction is provided in the movable direction (two directions or three directions) of the arms 10 and 20. In addition, a required number of devices are mounted at predetermined positions and postures, but it goes without saying that an acceleration sensor capable of simultaneously detecting acceleration components in three directions may be used.
【００４２】[0042]
【実施例】次に、図１および図２に示すロボットの振動
抑制制御装置の具体的実施例について述べる。図３(a)
(b)(c) および図４(a)(b)(c)(d)は、水平方向に回転可
能な６つの関節部２−１〜２−６により連結されたアー
ム１０を有する実機ロボットを用いて、上式（４）とし
て定式化された振動抑制制御を行った場合の実験結果を
示す図である。ここでアーム１０を有する実機ロボット
は、本発明の第１の実施の形態に対応している。EXAMPLE Next, a specific example of the vibration suppression control device for a robot shown in FIGS. 1 and 2 will be described. Figure 3 (a)
4 (b) (c) and FIGS. 4 (a) (b) (c) (d) show an actual robot having an arm 10 connected by six joints 2-1 to 2-6 that can rotate in the horizontal direction. FIG. 6 is a diagram showing an experimental result when the vibration suppression control formulated as the above equation (4) is performed by using FIG. The actual robot having the arm 10 corresponds to the first embodiment of the present invention.
【００４３】まず、図３(a)(b)(c) により、関節角目標
値０度で関節部２−１〜２−６の各々を位置決め保持状
態にしているときにアーム１０の先端に外乱を加えて強
制的に振動させた場合の実験結果について説明する。図
３(a)(b)(c) は従来の位置決め制御のみを行った場合
と、本発明によるロボットの振動抑制制御方法を適用し
て制御を行った場合とのアーム１０の先端に生じた振動
の減衰する様子を示す図である。このうち図３(a) は従
来の位置決め制御のみを行った場合の振動の減衰する様
子を加速度センサ出力αytipにより示す図、図３(b) は
本発明によるロボットの振動抑制制御方法を適用して制
御を行った場合の振動の減衰する様子を加速度センサ出
力αytipにより示す図である。また図３(c) はアーム１
０の第２関節部２−２を駆動するために対応するサーボ
ドライバに加えられる補償後の速度指令値を示す図であ
る。First, referring to FIGS. 3 (a), 3 (b) and 3 (c), when each of the joint portions 2-1 to 2-6 is positioned and held at the joint angle target value of 0 degree, the tip of the arm 10 is touched. The experimental results in the case of forcibly vibrating by adding disturbance will be described. 3 (a) (b) (c) occurred at the tip of the arm 10 when only the conventional positioning control was performed and when the robot vibration suppression control method according to the present invention was applied to control. It is a figure which shows a mode that vibration is attenuated. Of these, FIG. 3 (a) is a diagram showing how vibration is attenuated when only conventional positioning control is performed, by the acceleration sensor output α ytip , and FIG. 3 (b) applies the robot vibration suppression control method according to the present invention. FIG. 7 is a diagram showing how vibration is attenuated when the control is performed by an acceleration sensor output α ytip . Also, FIG. 3 (c) shows the arm 1
It is a figure which shows the speed command value after compensation added to the corresponding servo driver in order to drive the 0th 2nd joint part 2-2.
【００４４】図３(a) に示すように、従来の位置決め制
御のみを行った場合には、外乱が加えられてから１６秒
程度が経過してもアーム１０の先端に生じた振動が十分
に減衰しない。これに対して、本発明によるロボットの
振動抑制制御方法を適用して制御を行った場合には、図
３(b) に示すように、外乱が加えられてから２秒程度が
経過した時点で十分な振動の減衰が見られ、４秒程度が
経過した時点ではほぼ完全に振動がなくなっていること
が分かる。As shown in FIG. 3A, when only the conventional positioning control is performed, the vibration generated at the tip of the arm 10 is sufficiently generated even after about 16 seconds have passed since the disturbance was applied. Does not decay. On the other hand, when control is performed by applying the vibration suppression control method for the robot according to the present invention, as shown in FIG. 3 (b), when about 2 seconds have passed after the disturbance was applied, Sufficient vibration damping is seen, and it can be seen that the vibration has almost completely disappeared after about 4 seconds.
【００４５】次に、図４(a)(b)(c)(d)により、アーム１
０の先端を目標位置に位置決めする場合の一例として、
アーム１０の第２関節部２−２に関節角目標値θref 2
をステップ状に与えて２０度だけ回転させた場合の実験
結果について説明する。図４(a)(b)(c)(d)はアーム１０
の第２関節部２−２を駆動するために対応するサーボド
ライバに加えられる速度指令値と、加速度センサ出力α
ytipにより表されたアーム１０の先端に生じる振動の減
衰する様子とを示す図である。このうち図４(a)(b)は従
来の位置決め制御のみを行った場合を示す図、図４(c)
(d)は本発明のロボットの振動抑制制御方法を適用して
制御を行った場合を示す図である。Next, referring to FIGS. 4 (a) (b) (c) (d), the arm 1
As an example of positioning the tip of 0 at the target position,
The joint angle target value θref 2 is applied to the second joint portion 2-2 of the arm 10.
Will be described in the case of being given in steps and rotated by 20 degrees. 4 (a) (b) (c) (d) shows the arm 10
Of the speed sensor applied to the corresponding servo driver for driving the second joint section 2-2 of the
It is a figure which shows a mode that the vibration generate | occur | produced at the front-end | tip of the arm 10 represented by ytip is attenuated. Of these, FIGS. 4 (a) and 4 (b) are views showing a case where only conventional positioning control is performed, and FIG. 4 (c).
(d) is a diagram showing a case where control is performed by applying the vibration suppression control method for a robot of the present invention.
【００４６】図４(b) に示すように、従来の位置決め制
御のみを行った場合には、ステップ状の速度指令値が加
えられた時点で生じた振動が第２関節部２−２の回転期
間中において十分な減衰が行われることなく遷移する。
またステップ状の速度指令値が加えられなくなる時点に
おいてまた新たな振動が生じ、この振動がかなりの期間
にわたって継続している。これに対して、本発明による
ロボットの振動抑制制御方法を適用して制御を行った場
合には、図４(d) に示すように、ステップ状の速度指令
値が加えられた時点で生じた振動は回転期間中において
３秒程度で十分に減衰し、かつステップ状の速度指令値
が加えられなくなる時点において生じた新たな振動も同
様に３秒程度で十分に減衰することが分かる。As shown in FIG. 4B, when only the conventional positioning control is performed, the vibration generated when the stepped speed command value is applied causes the rotation of the second joint 2-2. Transition occurs without sufficient damping during the period.
Further, new vibration occurs again when the stepped speed command value is not added, and this vibration continues for a considerable period. On the other hand, when the robot vibration suppression control method according to the present invention is applied to perform control, as shown in FIG. 4 (d), it occurs when a stepwise speed command value is added. It can be seen that the vibration is sufficiently damped in about 3 seconds during the rotation period, and the new vibration generated at the time when the stepped speed command value is no longer added is similarly damped in about 3 seconds.
【００４７】このように、図３(a)(b)(c) および図４
(a)(b)(c)(d)の実験結果から明らかなように、本発明に
よるロボットの振動抑制制御方法により、十分な振動抑
制効果が得られることが確認できた。As described above, FIG. 3 (a) (b) (c) and FIG.
As is clear from the experimental results of (a), (b), (c), and (d), it was confirmed that the vibration suppression control method for the robot according to the present invention can obtain a sufficient vibration suppression effect.
【００４８】[0048]
【発明の効果】以上説明したように、本発明によれば、
アームリンク部に比べて剛性が低い複数の関節部を有
し、数Ｈｚ以下の低い固有振動数によるゆっくりした固
有振動を持つアームを備えたロボットにおいて、アーム
の先端を速やかに目標位置に位置決めすることができ、
またアームの先端が位置決め保持状態にあるときに加わ
った外乱による振動を速やかに抑制することができる。As described above, according to the present invention,
In a robot that has multiple joints that have lower rigidity than the arm link and that has a slow natural vibration at a low natural frequency of several Hz or less, quickly position the tip of the arm to the target position. It is possible,
Further, it is possible to quickly suppress the vibration due to the disturbance applied when the tip of the arm is in the positioning and holding state.
【００４９】また、関節部の数が多くなってもアームの
先端にアームの運動可能方向に対応して２方向または３
方向の加速度成分を検出できる加速度センサを取り付け
るだけでよく、各関節部に対応する駆動部に加えられる
所定指令値の補償成分を計算する方法も複雑でなく、ま
た計算時間も多くを必要としない。さらに、ロボットの
アームの全ての関節部がそれぞれアームの先端での振動
を抑制するように作用するので、補償成分のフィードバ
ックゲイン等を大きく採らなくても十分な振動抑制効果
を得ることができ制御系を不安定化させるおそれが少な
い。Even if the number of joints increases, the tip of the arm may be in two or three directions depending on the movable direction of the arm.
It is only necessary to attach an acceleration sensor that can detect the acceleration component in the direction, and the method of calculating the compensation component of the predetermined command value applied to the drive unit corresponding to each joint is not complicated and does not require much calculation time. . Furthermore, since all joints of the robot arm act to suppress the vibration at the tip of each arm, a sufficient vibration suppression effect can be obtained without using a large feedback gain of the compensation component. Less likely to destabilize the system.
【図１】本発明が適用される２次元平面内を運動するア
ームを有するロボットの外観を示す斜視図である。FIG. 1 is a perspective view showing an appearance of a robot having an arm that moves in a two-dimensional plane to which the present invention is applied.
【図２】本発明によるロボットの振動抑制制御装置の構
成を示すブロック図である。FIG. 2 is a block diagram showing a configuration of a vibration suppression control device for a robot according to the present invention.
【図３】本発明による振動抑制効果を示す実機ロボット
での実験結果の一例を示す図である。FIG. 3 is a diagram showing an example of an experimental result in an actual robot showing a vibration suppressing effect according to the present invention.
【図４】本発明による振動抑制効果を示す実機ロボット
での実験結果の別の例を示す図である。FIG. 4 is a diagram showing another example of an experimental result in an actual robot showing a vibration suppressing effect according to the present invention.
【図５】本発明が適用される３次元空間内を運動するア
ームを有するロボットの外観を示す斜視図である。FIG. 5 is a perspective view showing the appearance of a robot having an arm that moves in a three-dimensional space to which the present invention is applied.
１ ベース部 ２−１〜２−ｎ，２−１′〜２−ｎ′ 関節部 ３−１〜３−ｎ，３−１′〜３−ｎ′ アームリンク部 ４ 手先効果器 ５，５′ 加速度センサ ８ 制御装置 １０，２０ アーム １２ 演算部 １３−ｉ 位置決めフィードバック制御系 １４−ｉ サーボドライバ １５−ｉ 関節駆動モータ 1 base 2-1 to 2-n, 2-1 'to 2-n' joint part 3-1 to 3-n, 3-1 'to 3-n' arm link part 4 hand effector 5,5 'acceleration sensor 8 control device 10,20 arms 12 Operation part 13-i Positioning feedback control system 14-i Servo driver 15-i Joint drive motor
───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.7，ＤＢ名) B25J 9/10 B25J 13/00 B25J 19/06 ─────────────────────────────────────────────────── ─── Continuation of front page (58) Fields surveyed (Int.Cl. 7 , DB name) B25J 9/10 B25J 13/00 B25J 19/06
Claims (4)
ームリンク部を有するアームと、前記アームの先端を目
標位置に位置決めするため前記アームの各関節部を所定
指令値に基づいて駆動する駆動部とを備えたロボットで
用いられる振動抑制制御装置において、 前記アームの先端近傍に設けられ前記アームの先端の加
速度量を検出する加速度センサと、 前記加速度センサにより検出された加速度量に基づいて
前記アームの先端に生じた振動を抑制するように前記各
関節部ごとの前記駆動部への所定指令値を補償する前記
各関節部ごとの補償成分を算出するとともに、算出され
た前記各関節部ごとの補償成分を対応する前記各関節部
ごとの所定指令値から減算する演算部とを備え、 前記演算部は、前記加速度センサにより検出された前記
加速度量を前記アームの先端に作用した等価な力相当成
分とみなし、この力相当成分に準静力学的扱いで前記ア
ームの構造に応じた所定の変換を施すことにより、前記
アームの前記各関節部を駆動する所定指令値に対する補
償成分を算出することを特徴とする、ロボットの振動抑
制制御装置。1. An arm having a plurality of arm link portions connected to each other via a joint portion, and each joint portion of the arm is driven based on a predetermined command value in order to position a tip of the arm at a target position. In a vibration suppression control device used in a robot including a drive unit, based on an acceleration sensor that is provided near the tip of the arm and detects an acceleration amount of the tip of the arm, and an acceleration amount detected by the acceleration sensor. The compensation component for each joint is calculated for compensating a predetermined command value to the drive for each joint so as to suppress the vibration generated at the tip of the arm, and the calculated joint is calculated. And a computing unit that subtracts a compensation component for each joint from the corresponding predetermined command value for each joint unit, the computing unit including the addition unit detected by the acceleration sensor. The degree of force is regarded as an equivalent force equivalent component acting on the tip of the arm, and the force equivalent component is subjected to a predetermined conversion according to the structure of the arm by quasi-static treatment so that each joint portion of the arm is treated. A vibration suppression control device for a robot, which calculates a compensation component for a predetermined command value for driving a robot.
能方向に対応して２方向または３方向の加速度成分を検
出することを特徴とする、請求項１記載のロボットの振
動抑制制御装置。2. The vibration suppression control device for a robot according to claim 1, wherein the acceleration sensor detects acceleration components in two or three directions corresponding to the movable direction of the arm.
ームリンク部を有するアームと、前記アームの先端を目
標位置に位置決めするため前記アームの各関節部を所定
指令値に基づいて駆動する駆動部とを備えたロボットで
用いられる振動抑制制御方法において、 前記アームの先端近傍の加速度量を検出する工程と、 この工程により検出された加速度量に基づいて前記アー
ムの先端に生じた振動を抑制するように前記各関節部ご
との前記駆動部への所定指令値を補償する前記各関節部
ごとの補償成分を算出する工程と、 この工程により算出された前記各関節部ごとの補償成分
を対応する前記各関節部ごとの所定指令値から減算する
工程とを備え、 前記補償成分を算出する工程において、前記アームの先
端近傍で検出された前記加速度量を前記アームの先端に
作用した等価な力相当成分とみなし、この力相当成分に
準静力学的扱いで前記アームの構造に応じた所定の変換
を施すことにより、前記アームの前記各関節部を駆動す
る所定指令値に対する補償成分を算出することを特徴と
する、ロボットの振動抑制制御方法。3. An arm having a plurality of arm link parts connected to each other via a joint part, and each joint part of the arm is driven based on a predetermined command value in order to position the tip of the arm at a target position. In a vibration suppression control method used in a robot provided with a drive unit, a step of detecting an acceleration amount near the tip of the arm, and a vibration generated at the tip of the arm based on the acceleration amount detected in this step Calculating a compensation component for each joint that compensates a predetermined command value to the drive unit for each joint so as to suppress the compensation component for each joint calculated in this step; Subtracting from a predetermined command value for each of the corresponding joints, in the step of calculating the compensation component, the acceleration amount detected near the tip of the arm It is regarded as an equivalent force equivalent component acting on the tip of the arm, and each joint part of the arm is driven by subjecting this equivalent force component to a predetermined conversion according to the structure of the arm by quasi-static treatment. A vibration suppression control method for a robot, characterized in that a compensation component for a predetermined command value is calculated.
記アームの運動可能方向に対応して２方向または３方向
の加速度成分を検出することを特徴とする、請求項３記
載のロボットの振動抑制制御方法。4. The vibration suppression of a robot according to claim 3, wherein in the step of detecting the acceleration amount, an acceleration component in two directions or three directions corresponding to the movable direction of the arm is detected. Control method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP26004796A JP3442941B2 (en) | 1996-09-30 | 1996-09-30 | Robot vibration suppression control device and control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP26004796A JP3442941B2 (en) | 1996-09-30 | 1996-09-30 | Robot vibration suppression control device and control method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH10100085A JPH10100085A (en) | 1998-04-21 |
| JP3442941B2 true JP3442941B2 (en) | 2003-09-02 |
Family
ID=17342572
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP26004796A Expired - Fee Related JP3442941B2 (en) | 1996-09-30 | 1996-09-30 | Robot vibration suppression control device and control method thereof |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3442941B2 (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9014849B2 (en) | 2010-08-06 | 2015-04-21 | Seiko Epson Corporation | Robotic device, method for controlling robotic device, and computer program |
| US9095979B2 (en) | 2012-07-31 | 2015-08-04 | Seiko Epson Corporation | Control device and control method for robot and the robot |
Families Citing this family (30)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3030555B1 (en) * | 1999-06-22 | 2000-04-10 | 株式会社ユーシン精機 | Molded product removal device |
| US20070288124A1 (en) * | 2004-08-25 | 2007-12-13 | Kabushiki Kaisha Yaskawa Denki | Evaluating System And Evaluating Method Of Robot |
| JP5333813B2 (en) * | 2007-12-27 | 2013-11-06 | 株式会社安川電機 | Motor control device and mechanical system |
| JP5381039B2 (en) * | 2008-11-19 | 2014-01-08 | セイコーエプソン株式会社 | Articulated manipulator tip position control method and articulated manipulator |
| DE102008062978B4 (en) * | 2008-12-23 | 2019-02-14 | Robert Bosch Gmbh | Handling machine and method for its operation |
| JP5549129B2 (en) | 2009-07-06 | 2014-07-16 | セイコーエプソン株式会社 | Position control method, robot |
| JP5378908B2 (en) * | 2009-08-11 | 2013-12-25 | 川崎重工業株式会社 | Robot accuracy adjustment method and robot |
| JP5370128B2 (en) * | 2009-12-18 | 2013-12-18 | 株式会社デンソーウェーブ | Robot failure detection device |
| JP5417161B2 (en) * | 2009-12-28 | 2014-02-12 | 川崎重工業株式会社 | Robot vibration control method and robot control apparatus |
| US8886359B2 (en) | 2011-05-17 | 2014-11-11 | Fanuc Corporation | Robot and spot welding robot with learning control function |
| JP5383756B2 (en) * | 2011-08-17 | 2014-01-08 | ファナック株式会社 | Robot with learning control function |
| JP5438175B2 (en) | 2012-07-09 | 2014-03-12 | ファナック株式会社 | Vibration control robot system |
| JP5962340B2 (en) * | 2012-08-31 | 2016-08-03 | セイコーエプソン株式会社 | robot |
| JP6332899B2 (en) | 2012-08-31 | 2018-05-30 | セイコーエプソン株式会社 | robot |
| JP6111562B2 (en) | 2012-08-31 | 2017-04-12 | セイコーエプソン株式会社 | robot |
| JP6111563B2 (en) * | 2012-08-31 | 2017-04-12 | セイコーエプソン株式会社 | robot |
| KR101383726B1 (en) | 2012-12-17 | 2014-04-08 | 현대자동차(주) | Method for improving sensitivity of robot |
| KR101470164B1 (en) * | 2013-06-03 | 2014-12-05 | 현대자동차주식회사 | Method for improving sensitivity of robot |
| JP6248544B2 (en) * | 2013-10-30 | 2017-12-20 | セイコーエプソン株式会社 | Robot, control device, robot system |
| JP6314426B2 (en) | 2013-10-31 | 2018-04-25 | セイコーエプソン株式会社 | Robot control apparatus and robot control method |
| CN104669244A (en) * | 2013-12-02 | 2015-06-03 | 精工爱普生株式会社 | Robot |
| JP5854083B2 (en) * | 2014-05-21 | 2016-02-09 | セイコーエプソン株式会社 | Position control method, robot |
| JP6512790B2 (en) * | 2014-10-24 | 2019-05-15 | キヤノン株式会社 | Robot control method, robot apparatus, program, recording medium, and article manufacturing method |
| JP6575200B2 (en) * | 2015-07-27 | 2019-09-18 | セイコーエプソン株式会社 | Robot, control device and robot system |
| CN105459108B (en) * | 2015-07-31 | 2017-07-07 | 宁夏巨能机器人系统有限公司 | A kind of servo motor driving device and its driving method for mechanical hand |
| US10350754B2 (en) | 2016-09-27 | 2019-07-16 | Denso Wave Incorporated | Control device for robot |
| JP6790669B2 (en) * | 2016-09-27 | 2020-11-25 | 株式会社デンソーウェーブ | Robot control device |
| JP6790668B2 (en) * | 2016-09-27 | 2020-11-25 | 株式会社デンソーウェーブ | Robot control device |
| JP7147290B2 (en) * | 2018-06-26 | 2022-10-05 | セイコーエプソン株式会社 | Robots and robotic systems |
| JP7358744B2 (en) | 2019-02-22 | 2023-10-11 | セイコーエプソン株式会社 | Robot system, control device, and control method |
-
1996
- 1996-09-30 JP JP26004796A patent/JP3442941B2/en not_active Expired - Fee Related
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9014849B2 (en) | 2010-08-06 | 2015-04-21 | Seiko Epson Corporation | Robotic device, method for controlling robotic device, and computer program |
| US9545721B2 (en) | 2010-08-06 | 2017-01-17 | Seiko Epson Corporation | Robotic device, method for controlling robotic device, and computer program |
| US9956689B2 (en) | 2010-08-06 | 2018-05-01 | Seiko Epson Corporation | Robotic device, method for controlling robotic device, and computer program |
| US9095979B2 (en) | 2012-07-31 | 2015-08-04 | Seiko Epson Corporation | Control device and control method for robot and the robot |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH10100085A (en) | 1998-04-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3442941B2 (en) | Robot vibration suppression control device and control method thereof | |
| EP3651942B1 (en) | Vibration control of systems with configuration dependent dynamics | |
| US5767648A (en) | Base force/torque sensor apparatus for the precise control of manipulators with joint friction and a method of use thereof | |
| Morel et al. | The precise control of manipulators with high joint-friction using base force/torque sensing | |
| JPS62193784A (en) | Master/slave manipulator device | |
| Morel et al. | The precise control of manipulators with joint friction: A base force/torque sensor method | |
| Su et al. | Hybrid adaptive/robust motion control of rigid-link electrically-driven robot manipulators | |
| JP4639417B2 (en) | Robot control device | |
| JPH10128688A (en) | Non-interfering control method of robot | |
| KR20220145387A (en) | robot device | |
| Stockić et al. | Historical perspectives and state of the art in joint force sensory feedback control of manipulation robots | |
| JPH02205489A (en) | Control method for impedance of manipulator | |
| JP3080791B2 (en) | Auxiliary device for direct teaching of manipulators | |
| WO2017026045A1 (en) | Hand-force measurement device, hand-force measurement method, and hand-force measurement program | |
| JP3163346B2 (en) | Control method and apparatus for object-cooperative transport robot | |
| JPH0630012B2 (en) | Control method for industrial robot | |
| JPH07104725B2 (en) | Manipulator drive system | |
| JPH04343690A (en) | Joint friction compensating method for multi-joint manipulator | |
| JP2718687B2 (en) | Control device for position and force of multi-degree-of-freedom work machine | |
| CN113246107B (en) | Method and device for dragging teaching speed limit of mechanical arm joint, electronic equipment and medium | |
| JP2628846B2 (en) | Industrial robot control method and apparatus | |
| JP3196145B2 (en) | Contact point and contact normal detection method for machining / assembly equipment | |
| JP3302797B2 (en) | Manipulator and control method thereof | |
| JPH05313746A (en) | Controller for manipulator | |
| Iagnemma et al. | A model-free fine position control method using the base-sensor: With application to a hydraulic manipulator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090620 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090620 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100620 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100620 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110620 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120620 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120620 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130620 Year of fee payment: 10 |
|
| LAPS | Cancellation because of no payment of annual fees |