JP3200496B2 - Articulated robot controller - Google Patents

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【産業上の利用分野】本発明は多関節ロボット制御装置
に係り、特に多関節型ロボットのロバストな非干渉化制
御装置に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an articulated robot controller, and more particularly to a robust decoupling controller for an articulated robot.

【０００２】[0002]

【従来の技術】スカラ型ロボットのような多関節ロボッ
トの場合、通常の各軸の慣性力や摩擦力のほかに、各軸
間に慣性干渉力やコリオリ力・遠心力などが働いてい
る。このような相互干渉力があると、ある軸だけに対す
る指令が他の軸の出力にも影響を与えるため、本来望ま
ない応答が現われることになる。特に慣性干渉力は加速
度に依存するので、停止時のオーバーシュートやタクト
に悪い影響を与える。2. Description of the Related Art In the case of an articulated robot such as a scalar type robot, an inertial interference force, a Coriolis force, a centrifugal force, etc. are applied between the axes in addition to the normal inertial force and frictional force of each axis. With such a mutual interference force, a command for only one axis affects the output of another axis, so that an originally undesirable response appears. In particular, since the inertial interference force depends on the acceleration, it has a bad influence on overshoot and tact at the time of stop.

【０００３】従来は各軸の駆動装置が高減速比でその影
響が弱められ、これらの干渉力は「その他外乱」として
積分補償のみですませ、構造の簡単な各軸ごとのＰＩサ
ーボ系を構成するのが一般的であった。しかし、年々ロ
ボットの動作性能に対する要求が高まって、アームスピ
ードは増加し、タクトも従来の半分以下というレベルに
なってきている。このような要求に対し、ＰＩ制御では
補償しきれないダイナミクスの影響や、高減速比駆動で
あっても干渉力などの影響が無視できなくなる。Conventionally, the influence of the drive unit of each axis is reduced at a high reduction ratio, and these interference forces are only other compensations as "other disturbances" and constitute a PI servo system for each axis with a simple structure. It was common to do. However, the demands on the operation performance of the robot have been increasing year by year, and the arm speed has increased, and the tact has been reduced to less than half of the conventional level. In response to such demands, the effects of dynamics that cannot be compensated for by PI control and the effects of interference force and the like cannot be ignored even with high reduction ratio driving.

【０００４】これに対し、最近、外乱推定によるロバス
ト制御が注目されている。これは外乱推定オブザーバに
より外乱を補償した後、その補償後の系に対して、状態
（速度）オブザーバを付けて制御系を構成するもので、
演算量も多い。また、外乱としてステップ状外乱を想定
しているため、過渡的な応答には偏差が大きくなるとい
うことには変わりはない。さらに精度を向上するために
は、ダイレクトドライブ方式が有利であるが、そのとき
はこのような外乱の影響は非常に大きなものとなり、も
っと積極的に対処することが不可欠となる。[0004] On the other hand, robust control based on disturbance estimation has recently attracted attention. In this method, after a disturbance is compensated by a disturbance estimation observer, a state (speed) observer is added to the system after the compensation to constitute a control system.
The amount of calculation is also large. Further, since a step-like disturbance is assumed as the disturbance, the deviation is still large in the transient response. In order to further improve the accuracy , the direct drive method is advantageous. However, in such a case, the influence of such a disturbance becomes very large, and it is essential to take more aggressive measures.

【０００５】[0005]

【発明が解決しようとする課題】一方、相互干渉力を補
償し、指令の出力応答に見かけ上何の干渉もないよう制
御する非干渉化制御の手法があり、これまで種々の手法
が提案されてきている。しかし、これらは理論や構成が
複雑であったり、正確なパラメータや計算精度を要する
ことが多かった。さらに、ロバスト性を付加するために
補償器を挿入すると、次数が増大し演算量を増すことに
なる。On the other hand, there is a decoupling control method for compensating for mutual interference force and controlling the output response of the command so that there is no apparent interference. Various methods have been proposed so far. Is coming. However, these are often complicated in theory and configuration, and require accurate parameters and calculation accuracy. Furthermore, if a compensator is inserted to add robustness, the order will increase and the amount of calculation will increase.

【０００６】最近、マイクロプロセッサの速度は向上し
ているとはいえ、経済的理由などで実用上まだ演算量の
多少は大きな問題である。そして、整数演算をさせるこ
とがほとんどであるため、演算誤差が外乱として制御系
に影響を及ぼすことになる。Although the speed of microprocessors has recently been improved, the amount of computation is still a serious problem in practice for economic reasons. In most cases, an integer operation is performed, so that an operation error affects the control system as a disturbance.

【０００７】本発明の目的は以上のような問題を解決す
る簡易かつ計算誤差や外乱にも強い実用的な多関節ロボ
ットの非干渉化制御装置を提供することにある。An object of the present invention is to provide a practical and non-interacting control apparatus for an articulated robot which solves the above-described problems and is simple and resistant to calculation errors and disturbances.

【０００８】[0008]

【課題を解決するための手段】本発明は上記目的を達成
するため、所定の位置指令信号ｖ（ｋ）に応じて所定の
演算装置により算出された制御入力ｕ（ｋ）＝Ｑ
^-1（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝＋Ｇ
ｖ（ｋ）（但し、Ｋ（δ）＝Ｋ _１ δ＋Ｋ _０ ，Ｈ（δ）＝
Ｈ _２ δ ^２ ＋Ｈ _１ δ＋Ｈ _０ ，Ｑ ^-1 （δ）＝１／Ｉ（δ ^２ ＋
ｑ _１ δ＋ｑ _０ ））により多関節型ロボットを駆動制御
し、該ロボットの角位置出力信号ｙ（ｋ）を出力するよ
うに構成された多関節型ロボット制御装置において、前
記演算装置は、前記位置指令信号ｖ（ｋ）から角位置出
力信号ｙ（ｋ）に至る伝達関数行列が、干渉要素がな
く、対角な２次の希望伝達関数Ｎ（δ）Ｍ ^−１ （δ）
（但し、Ｍ（δ）＝Ｉ（δ ^２ ＋ｍ _１ δ＋ｍ _０ ），Ｎ
（δ）＝Ｉｍ _０ ））を持つ伝達関数行列となるように構
成されており、前記制御入力ｕ（ｋ）の、状態オブザー
バによる状態フィードバック係数を含んだ部分Ｑ
^-1（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝に、
その次数を外乱の次数ｄに応じて上げ、Ｋ（δ）の係数
Ｋｉ（ｉ＝０〜ｄ）をＱ（δ）の係数Ｑｉ（ｉ＝０〜
ｄ）に等しくすることによって、外乱補償機能を付加
し、このような外乱ロバスト性を持たせた制御法則に基
づいて前記制御入力信号を算出するようにしたことを要
旨とする。The present invention achieves the above object.
To order, predetermined position command signal v (k) depending on the calculated by a predetermined calculation device control input u (k) = Q
^-1 (δ) {K (δ) u (k) + H (δ) y (k)} + G
v (k) (where K (δ) = K ₁ δ + K ₀ , H (δ) =
H ₂ δ ² + H ₁ δ + H ₀ , Q ⁻¹ (δ) = 1 / I (δ ² +
q ₁ δ + q ₀₎₎ by the drive control of the articulated robot, the articulated robot control device configured to output the robot angular position output signal y (k), the computing device, the position The transfer function matrix from the command signal v (k) to the angular position output signal y (k) has a diagonal second-order desired transfer function N (δ) M ⁻¹ (δ) without interference elements.
(However, M (δ) = I (δ ² + m ₁ δ + m ₀ ), N
(Δ) = I m ₀ )), and a portion Q of the control input u (k) including a state feedback coefficient by a state observer.
^-1 (δ) {K (δ) u (k) + H (δ) y (k)},
The order is increased according to the order d of the disturbance, and the coefficient Ki (i = 0 to d) of K (δ) is changed to the coefficient Qi (i = 0 to Q) of Q (δ).
The gist of the gist is that a disturbance compensation function is added by making d equal to d), and the control input signal is calculated based on a control law having such disturbance robustness.

【０００９】[0009]

【作用】本発明では、制御入力ｕ（ｋ）＝Ｑ^-1（δ）
｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝＋Ｇｖ（ｋ）
の、状態オブザーバとその状態オブザーバによる状態フ
ィードバック係数を含んだ部分Ｑ^-1（δ）｛Ｋ（δ）ｕ
（ｋ）＋Ｈ（δ）ｙ（ｋ）｝に、その次数を外乱の次数
ｄに応じて上げ、Ｋ（δ）の係数Ｋｉ（ｉ＝０〜ｄ）を
Ｑ（δ）の係数Ｑｉ（ｉ＝０〜ｄ）に等しくすることに
よって、外乱補償機能を含ませている。これにより、ロ
ボットに入力するトルク指令とロボットの位置出力だけ
から、少ない制御器次数（すなわち少ない演算量）でロ
バスト性を保ちつつ制御目的を達成している。この場
合、制御目的は多関節型ロボットの非干渉化と各軸ごと
の希望モデル（伝達関数）への極配置である。すなわ
ち、単に制御器次数が少ないだけでなく、外乱補償、非
干渉化及び極配置という三つの機能を１ステップで同時
に達成する、極めてシンプルな設計法と制御アルゴリズ
ムを提供することができる。According to the present invention, the control input u (k) = Q ^-1 (δ)
{K (δ) u (k) + H (δ) y (k)} + G v (k)
Q ^-1 (δ) ｛K (δ) u containing the state observer and the state feedback coefficient of the state observer
(K) + H (δ) y (k)}, the order thereof is increased according to the order d of the disturbance, and the coefficient Ki (i = 0 to d) of K (δ) is increased to the coefficient Qi (i) of Q (δ). = 0 to d) to include a disturbance compensation function. As a result, the control purpose is achieved while maintaining robustness with a small controller order (ie, a small amount of computation) based on only the torque command input to the robot and the robot position output. In this case, the control objectives are decoupling of the articulated robot and pole assignment to a desired model (transfer function) for each axis. That is, it is possible to provide an extremely simple design method and control algorithm that not only has a small controller order but also simultaneously achieves three functions of disturbance compensation, decoupling, and pole placement in one step.

【００１０】[0010]

【実施例】一般にｎ軸の多関節型ロボットの運動方程式
は次式のように表わせる。DESCRIPTION OF THE PREFERRED EMBODIMENTS In general, the equation of motion of an n-axis articulated robot can be expressed as follows.

【００１１】[0011]

【数１】 (Equation 1)

【００１２】ここで、Ｍ（ ）はｎ×ｎ慣性行列、Ｄ
（ ）は粘性摩擦・コリオリ力・遠心力・逆起電力に関
するｎ×ｎ行列で各々非対角要素が干渉項である。Ｋは
モータ・ドライバ・駆動機構などのｎ×ｎの対角なゲイ
ン行列、ｆはクーロン摩擦・重力を表わすｎ次元ベクト
ルである。またθ，ｄθ／ｄｔ，ｄ²θ／ｄｔ²はそれぞ
れ軸角度、角速度、角加速度を表わし、ｕはドライバに
印加されるｎ次元制御入力信号である。Here, M () is an n × n inertia matrix, D
() Is an n × n matrix relating to viscous friction, Coriolis force, centrifugal force, and back electromotive force, and each off-diagonal element is an interference term. K is an n × n diagonal gain matrix of a motor, driver, drive mechanism, etc., and f is an n-dimensional vector representing Coulomb friction and gravity. Θ, dθ / dt, d ² θ / dt ² represent the axis angle, angular velocity, and angular acceleration, respectively, and u is an n-dimensional control input signal applied to the driver.

【００１３】実際の制御アルゴリズムは、マイクロプロ
セッサ上にソフトウエアとしてインプリメントされるこ
とを前提として、入力をＤＡコンバータで０次ホールド
し、サンプリング時間τ_s（ｓ）で離散化する。このよ
うにして式（１）を離散系状態方程式で表わすと、Assuming that the actual control algorithm is implemented as software on a microprocessor, the input is held at the 0th order by a DA converter and is discretized at a sampling time τ _s (s). In this way, when Expression (1) is expressed by a discrete state equation,

【００１４】[0014]

【数２】 (Equation 2)

【００１５】ここで、η（ｋ）はクーロン摩擦・重力等
の定値外乱項である。もとの連続系には零点（伝達関数
の分子多項式の根）はないが、このような離散化によっ
て単位円近傍に零点をもつようになり、サンプリング時
間を短くすると零点が単位円の外側にある非最小位相系
となる。このことはまた後で扱うことにする。ここで後
述する外乱補償機能により、式（２）の定値外乱η
（ｋ）は完全に補償されるためη項を除いて考え、式
（２），（３）をｒｒｐ分解して進み演算子ｚ（時系列
信号ｓ（ｋ）に対し、ｓ（ｋ＋１）＝ｚｓ（ｋ）となる
ような演算子）のｎ×ｎ多項式行列Ｐ（ｚ），Ｒ（ｚ）
によって表わすと、Here, η (k) is a constant disturbance term such as Coulomb friction and gravity. Although the original continuous system has no zeros (roots of the numerator polynomial of the transfer function), such discretization results in having zeros near the unit circle. When the sampling time is shortened, the zeros fall outside the unit circle. Some non-minimum phase system. This will be dealt with later. Here, the constant value disturbance η of the equation (2) is obtained by a disturbance compensation function described later.
Since (k) is completely compensated, it is considered excluding the η term, and the equations (2) and (3) are rrp-decomposed and the advance operator z (for the time-series signal s (k), s (k + 1) = zs (k) operator) n × n polynomial matrix P (z), R (z)
Expressed by

【００１６】[0016]

【数３】 ｙ（ｋ）＝Ｒ（ｚ）Ｐ^-1（ｚ）ｕ（ｋ） （４） となる。この式（４）が制御対象の離散系伝達関数行列
で、多変数系を扱う場合のひとつの表現方法である。Y (k) = R (z) P ^-1 (z) u (k) (4) Equation (4) is a discrete transfer function matrix to be controlled, and is one expression method when a multivariable system is handled.

【００１７】ここで以降の論議を簡便にするため、新た
に演算子δ＝ｚ−１を用いる。これにより制御対象の多
項式行列を表わすと次のようになる。Here, in order to simplify the following discussion, a new operator δ = z−1 is used. Thus, the polynomial matrix to be controlled is expressed as follows.

【数４】 Ｐ（δ）＝Ｉδ²＋Ｐ₁δ＋Ｐ₀ （５） Ｒ（δ）＝Ｒ₁δ＋Ｒ₀ （６） ロボットの位置決め制御の場合、Ｐ₀＝０であるが、
Ｐ₁，Ｒ₁，Ｒ₀は対角要素のほかに非対角の０でない干
渉要素がある。また、Ｒ（δ）は正則とする（逆行列が
存在する）が、この仮定は通常のロボットでは成立す
る。P (δ) = Iδ ² + P ₁ δ + P ₀ (5) R (δ) = R ₁ δ + R ₀ (6) In the case of robot positioning control, P ₀ = 0.
P ₁ , R ₁ , and R ₀ include non-diagonal non-zero interference elements in addition to diagonal elements. Also, R (δ) is regular (there is an inverse matrix), but this assumption holds for a normal robot.

【００１８】次に、外乱モデルを求める。式（２）でη
を表わす外乱モデルは、各軸ごとにη（ｔ）＝η₀＋η₁
ｔ＋……＋η_dｔ_dで表わされ、そのｚ変換をδで表わす
と、Next, a disturbance model is obtained. In equation (2), η
Is represented by η (t) = η ₀ + η ₁ for each axis.
t +... + η _d t _d , and the z-transform is represented by δ.

【数５】 η（δ）＝η₀／δ＋……＋η_d／δ^d+1 （７） のように定義できる。ηは定値外乱であるからｄ＝０で
ある。なお、η₁は未知である。Η (δ) = η ₀ / δ +... + Η _d / δ ^{d + 1} (7) Since η is a constant value disturbance, d = 0. Note that η ₁ is unknown.

【００１９】さて本発明の制御目的は、ロボットの動特
性が非干渉化され、さらに各軸ごとの伝達関数が極配置
され、希望伝達関数に一致することである。つまり、各
軸ごとに伝達関数ｍ（δ）＝ｍ₀／（δ²＋ｍ₁δ＋ｍ₀）
となるように、指令ｖ（ｋ）からｙ（ｋ）に至る希望伝
達関数行列をAn object of the control of the present invention is to make the dynamic characteristics of the robot non-interfering, and to further arrange the transfer function for each axis so as to match the desired transfer function. That is, the transfer function m (δ) = m ₀ / (δ ² + m ₁ δ + m ₀ ) for each axis
The desired transfer function matrix from the command v (k) to y (k) is expressed as

【数６】 ｙ（ｋ）＝Ｎ（δ）Ｍ^-1（δ）ｖ（ｋ） （８） Ｍ（δ）＝Ｉ（δ²＋ｍ₁δ＋ｍ₀） Ｎ（δ）＝Ｉｍ₀ と設定すれば、伝達行列が対角化され干渉要素が０とな
り、非干渉化されることになる。Y (k) = N (δ) M ⁻¹ (δ) v (k) (8) M (δ) = I (δ ² + m ₁ δ + m ₀ ) N (δ) = Im ₀ In this case, the transfer matrix is diagonalized, the interference element becomes 0, and the interference matrix is decoupling.

【００２０】次に、干渉要素をもった伝達関数行列で表
わされる制御対象（式（４））に対し、位置指令ｖ
（ｋ）からｙ（ｋ）に至る伝達関数行列を干渉要素のな
い対角なＮ（δ）Ｍ^-1（δ）に一致させる制御法則を求
める。Next, for a control object (Equation (4)) represented by a transfer function matrix having an interference element, a position command v
A control law is determined to make the transfer function matrix from (k) to y (k) match the diagonal N (δ) M ^-1 (δ) without interference elements.

【００２１】まず、制御対象の軸ごとの次数（最大列次
数）が２、外乱の次数がｄ＋１＝１であるから、これら
の次数により、任意の（２−１）＋（ｄ＋１）＝２次の
モニック安定多項式をｑ（δ）＝δ²＋ｑ₁δ＋ｑ₀と
し、これを対角要素とするｎ×ｎ多項式行列をFirst, since the order (maximum column order) of each axis to be controlled is 2 and the order of disturbance is d + 1 = 1, an arbitrary (2-1) + (d + 1) = 2nd order is obtained by these orders. Is a monic stable polynomial of q (δ) = δ ² + q ₁ δ + q ₀ , and an n × n polynomial matrix having this as a diagonal element is

【数７】 Ｑ（δ）＝Ｉｑ（δ） （９） とする。このｑ（δ）がオブザーバの特性多項式とな
り、制御系の極配置モデルとは独立に設定できる。そし
て次式のような、Ｑ（δ）の次数より１次低い１次のｎ
×ｎ多項式行列Ｋ（δ）、およびＱ（δ）と同じ２次の
ｎ×ｎ多項式行列Ｈ（δ）を考える。It is assumed that Q (δ) = Iq (δ) (9) This q (δ) becomes a characteristic polynomial of the observer, and can be set independently of the pole arrangement model of the control system. Then, as in the following equation, a first-order n lower than the order of Q (δ)
Consider a × n polynomial matrix K (δ) and a second-order n × n polynomial matrix H (δ) that is the same as Q (δ).

【００２２】[0022]

【数８】 Ｋ（δ）＝Ｋ₁δ＋Ｋ₀ （１０） Ｈ（δ）＝Ｈ₂δ²＋Ｈ₁δ＋Ｈ₀ （１１） 最大列行次数が３次の任意の多項式行列Ｆ（δ）に対
し、多項式方程式（１２）を満足する式（１０），（１
１）のような多項式行列Ｋ（δ），Ｈ（δ）を係数比較
により求める。K (δ) = K ₁ δ + K ₀ (10) H (δ) = H ₂ δ ² + H ₁ δ + H ₀ (11) For an arbitrary polynomial matrix F (δ) having a cubic maximum column order of three. Equations (10) and (1) satisfying the polynomial equation (12)
Polynomial matrices K (δ) and H (δ) as in 1) are obtained by coefficient comparison.

【００２３】[0023]

【数９】 Ｋ（δ）Ｐ（δ）＋Ｈ（δ）Ｒ（δ）＝Ｆ（δ） （１２） このとき、Ｋ₁＝Ｉｑ₁，ｉ＝０〜ｄとすることで、η
（ｋ）を漸近的に０とする外乱補償機能を制御法則に内
包させ、定常偏差を生じさせなくすることができる。さ
らに、このようにＫ₁，ｉ＝０〜ｄを選ぶことで、その
他のＫ（δ），Ｈ（δ）の係数を唯一に決めることがで
きる。さて式（１２）において、Ｆ（δ）をK (δ) P (δ) + H (δ) R (δ) = F (δ) (12) At this time, by setting K ₁ = Iq ₁ and i = 0 to d, η
A disturbance compensation function for asymptotically setting (k) to 0 can be included in the control law so that a steady-state error does not occur. Further, by selecting K ₁ and i = 0 to d in this manner, the other coefficients of K (δ) and H (δ) can be uniquely determined. Now, in equation (12), F (δ) is

【００２４】[0024]

【数１０】 Ｆ（δ）＝Ｑ（δ）｛Ｐ（δ）−ＧＭ（δ）Ｎ^-1（δ）Ｒ₀｝ （１３） Ｇ＝Ｎ（δ）Ｒ₀ ^-1 としたとき、以下の制御法則F (δ) = Q (δ) ｛P (δ) −GM (δ) N ⁻¹ (δ) R ₀ ｝ (13) When G = N (δ) R ₀ ⁻¹ , Control law

【００２５】[0025]

【数１１】 ｕ（ｋ）＝Ｑ^-1（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝＋Ｇｖ（ｋ） （１４） を考える。式（４），（１４）よりU (k) = Q ⁻¹ (δ) {K (δ) u (k) + H (δ) y (k)} + Gv (k) (14) From equations (4) and (14)

【００２６】[0026]

【数１２】 ｙ（ｋ）＝Ｒ（δ）｛Ｑ（δ）Ｐ（δ）−Ｋ（δ）Ｐ（δ） −Ｈ（δ）Ｒ（δ）｝^-1Ｑ（δ）Ｇｖ（ｋ） （１５）Y (k) = R (δ) ｛Q (δ) P (δ) -K (δ) P (δ) -H (δ) R (δ)｝ ^-1 Q (δ) Gv (k ) (15)

【００２７】式（１５）に式（１２），（１３）を代入
すると、By substituting equations (12) and (13) into equation (15),

【数１３】 ｙ（δ）＝Ｒ(δ)｛Ｑ（δ）ＧＭ(δ)Ｎ^-1(δ)Ｒ₀｝^-1Ｑ（δ）Ｇｖ(ｋ) ＝Ｒ(δ)Ｒ₀ ^-1Ｎ(δ)Ｍ^-1(δ)Ｇ^-1Ｑ^-1(δ)Ｑ(δ)Ｇｖ(ｋ) ＝Ｒ(δ)Ｒ₀ ^-1Ｎ(δ)Ｍ^-1(δ)ｖ(ｋ) （１６） 式（１６）第２行目のＱ（δ）は、その対角要素ｑ
（δ）を安定多項式に選んであるので、Ｑ^-1（δ）Ｑ
（δ）＝Ｉというようにキャンセル可能である。Y (δ) = R (δ) ｛Q (δ) GM (δ) N ^-1 (δ) R ₀ ｝ ^-1 Q (δ) Gv (k) = R (δ) R ₀ ^-1 N (δ) M ^-1 (δ) G ^-1 Q ^-1 (δ) Q (δ) Gv (k) = R (δ) R ₀ ^-1 N (δ) M ^-1 (δ) v (k) (16) Q (δ) on the second line is the diagonal element q
Since (δ) is selected as a stable polynomial, Q ⁻¹ (δ) Q
Cancellation is possible as (δ) = I.

【００２８】ここでＲ（δ）は、ロボットを０次ホール
ドして、短いサンプリング時間で離散化したことによっ
て、単位円近傍に零点をもつため、式（１６）においてHere, R (δ) has a zero point near the unit circle by holding the robot at the 0th order and discretizing it with a short sampling time.

【数１４】 Ｒ（δ）Ｒ₀ ^-1＝Ｒ₁Ｒ₀ ^-1δ＋Ｉ ≒Ｉ（０．５δ＋１） ＝Ｉ（ｚ＋１）／２ （１７）R (δ) R ₀ ⁻¹ = R ₁ R ₀ ⁻¹ δ + I ≒ I (0.5δ + 1) = I (z + 1) / 2 (17)

【００２９】という近似を行う。したがって、式（１
７）を式（１６）に代入すると、The following approximation is performed. Therefore, equation (1)
Substituting 7) into equation (16) gives

【数１５】 ｙ（ｋ）≒Ｎ（δ）Ｍ^-1(δ)（ｚ＋１）ｖ（ｋ）／２ （１８） となって、新たにｖ（ｋ）＝（ｚ＋１）ｖ（ｋ）／２と
なるように規範入力を細工することで、非干渉化と極配
置が同時に達成できる。Y (k) ≒ N (δ) M ⁻¹ (δ) (z + 1) v (k) / 2 (18), and v (k) = (z + 1) v (k) / By modifying the reference input so that it becomes 2, decoupling and pole placement can be achieved simultaneously.

【００３０】図１は上述した式（１４）により規定され
る制御法則に基づく本発明の制御系全体の構成を示す。
同図において、１は制御対象としての多関節型ロボッ
ト、２は演算装置、３は位置指令信号発生装置、４は駆
動装置、５は角位置出力信号検出装置である。ロボット
１の実際の伝達関数（離散系伝達関数行列）はＲ（ｚ）
Ｐ^-1（ｚ）で、演算装置２は干渉要素をもった上記伝達
関数の制御対象１に対し、干渉要素のない対角なＮ
（δ）Ｍ^-1（δ）の希望伝達関数としての処理を可能と
するため、前記式（１４）に基づく演算により位置指令
信号ｖ（ｋ）に応じた最適な制御入力信号ｕ（ｋ）を出
力する。なお、以上においてはｎ×ｎの多項式行列で説
明したが、ｎ＝１の場合でも本発明は成立する。FIG. 1 shows the configuration of the entire control system according to the present invention based on the control law defined by the above-mentioned equation (14).
In FIG. 1, reference numeral 1 denotes an articulated robot to be controlled, 2 denotes an arithmetic unit, 3 denotes a position command signal generating device, 4 denotes a driving device, and 5 denotes an angular position output signal detecting device. The actual transfer function (discrete transfer function matrix) of the robot 1 is R (z)
At P ^-1 (z), the arithmetic unit 2 sets the diagonal N with no interference element to the control target 1 of the transfer function having the interference element.
(Δ) In order to enable processing of M ⁻¹ (δ) as a desired transfer function, an optimal control input signal u (k) corresponding to the position command signal v (k) by an operation based on the above equation (14). Is output. Although the above description has been made with reference to an n × n polynomial matrix, the present invention is also valid when n = 1.

【００３１】次に、図１の制御系の外乱応答特性の解析
においては、規範入力をｖ（ｋ）＝０として、外乱η
（ｋ）から出力ｙ（ｋ）に至る伝達特性を調べる。ま
ず、外乱η（ｋ）は式（２）のように入力ｕ（ｋ）に加
算された形で印加されるので、式（４）は、Next, in the analysis of the disturbance response characteristic of the control system shown in FIG. 1, when the reference input is v (k) = 0, the disturbance η
The transfer characteristic from (k) to the output y (k) is examined. First, since the disturbance η (k) is applied in a form added to the input u (k) as in equation (2), equation (4) is

【数１６】 ｙ（ｋ）＝Ｒ（δ）Ｐ^-1（δ）｛ｕ（ｋ）＋η（ｋ）｝ （１９） となる。Y (k) = R (δ) P ⁻¹ (δ) {u (k) + η (k)} (19)

【００３２】式（１２），（１３），（１４）および
（１９）より、From equations (12), (13), (14) and (19),

【数１７】 ｙ（ｋ）＝Ｒ（δ）Ｒ₀ ^-1Ｎ（δ）Ｇ^-1｛Ｑ（δ）−Ｋ（δ）｝ ×Ｍ^-1（δ）Ｑ^-1（δ）η（ｋ） （２０）Y (k) = R (δ) R ₀ ⁻¹ N (δ) G ⁻¹ {Q (δ) −K (δ)} × M ⁻¹ (δ) Q ⁻¹ (δ) η ( k) (20)

【００３３】この式で、定常応答、すなわちｋ→∞とし
たときのｙ（ｋ）は、In this equation, the steady-state response, that is, y (k) when k → ∞ is

【００３４】[0034]

【数１８】 (Equation 18)

【００３５】である。ここで、η（δ）＝η’（δ）／
δ^d+1と表わせる。また、Ｋ₁＝Ｉｑ₁，ｉ＝０〜ｄのよ
うに選んであるから、Ｑ（δ）−Ｋ（δ）＝δ
^d+1｛Ｑ’（δ）−Ｋ’（δ）｝と表わせ、η（δ）の
分母δ^d+1がキャンセルされる。したがって式（２１）
は、Is as follows. Here, η (δ) = η ′ (δ) /
δ ^{d + 1} . Further, since K ₁ = Iq ₁ and i = 0 to d are selected, Q (δ) −K (δ) = δ
^{d + 1} {Q ′ (δ) −K ′ (δ)}, and the denominator δ ^{d + 1} of η (δ) is cancelled. Therefore, equation (21)
Is

【００３６】[0036]

【数１９】 [Equation 19]

【００３７】となり、ここで、分母行列Ｍ^-1（δ）Ｑ^-1
（δ）は安定に選んであるから、式（２２）の値は０に
なる。すなわち、ｋ→∞のとき外乱η（δ）の出力への
影響は０に収束する。したがって、この系は式（７）の
ような外乱ベクトルη（δ）に対して定常偏差を生じな
い。Where the denominator matrix M ^-1 (δ) Q ^-1
Since (δ) is stably selected, the value of equation (22) becomes zero. That is, when k → ∞, the influence of the disturbance η (δ) on the output converges to zero. Therefore, this system does not generate a steady-state deviation with respect to the disturbance vector η (δ) as in the equation (7).

【００３８】図２は図１の制御系に基づく本発明の多関
節ロボット制御装置の一実施例で、多関節型ロボット１
は、例えば、スカラ型ロボットが用いられ、このロボッ
トの制御軸数は４軸であるが、主に干渉のある水平２リ
ンク機構を構成するθ₁軸（第１軸）とθ₂軸（第２軸）
に本発明の制御方式を適用する。演算装置２としては、
例えばディジタル信号処理装置（ＤＳＰ）が用いられ、
この装置による演算制御のためＰ（ｚ）等は下記のよう
にして定める。すなわち、制御対象１のパラメータは、
駆動装置４を含めて行った最小自乗法によるパラメータ
同定試験により得られ、FIG. 2 shows an embodiment of the articulated robot control device of the present invention based on the control system of FIG.
Is, for example, SCARA robots are used, the number of control axes of the robot is a 4-axis, mainly theta ₁ axis constituting a horizontal two-link mechanism with interference (first axis) and theta ₂ axes (a 2 axes)
The control method of the present invention is applied to the present invention. As the arithmetic unit 2,
For example, a digital signal processor (DSP) is used,
P (z) and the like for arithmetic control by this device are determined as follows. That is, the parameter of the control target 1 is
It is obtained by a parameter identification test by the least squares method including the driving device 4,

【００３９】[0039]

【数２０】 (Equation 20)

【００４０】である。希望モデルの二つの極は２０ｒａ
ｄ／ｓおよび８００ｒａｄ／ｓ、オブザーバは２次のバ
タワース極１２０ｒａｄ／ｓに設定した。制御法則（式
（１４））のパラメータは、以下のようになる。Is as follows. The two poles of the desired model are 20ra
The d / s and 800 rad / s, and the observer were set to the secondary Butterworth pole 120 rad / s. The parameters of the control law (Equation (14)) are as follows.

【００４１】[0041]

【数２１】 (Equation 21)

【００４２】図３（ｂ）は同図（ａ）のようにロボット
１のθ₁軸のみ動作させた時の、位置指令信号ｖ（ｋ）
に対する実位置の偏差をプロットしたもので、点線は従
来方式、実際は本発明の方式による結果を示す。同図か
ら明らかなように、非干渉化を行わない従来の各軸サー
ボ制御方式の場合、干渉力が高減速でかなり小さくなっ
ているにもかかわらず、０指令のはずのθ₂軸は大きく
振れているが、本発明の方式により非干渉化制御した場
合、θ₂軸はほとんど振れず、干渉力の影響が十分に補
償されているのがわかる。[0042] FIG. 3 (b) when operating only theta ₁ axis of the robot 1 as shown in FIG. (A), the position command signal v (k)
Is plotted against the actual position deviation, and the dotted line indicates the result according to the conventional method, actually the result according to the method of the present invention. As apparent from the figure, when the non-interference conventional individual axes servo control system is not performed, the interference power despite the significantly smaller in the high reduction, theta ₂ axes supposed zero command is greater Although deflection, when the non-interacting control the method of the present invention, theta ₂ axes little deflection, it can be seen that the effect of interference power has been sufficiently compensated.

【００４３】また、図４（ｂ）は図３と同様にして図４
（ａ）のようにθ₂軸のみ動作させた結果を示す。この
場合は、両方式のどちらも干渉による影響は少なく、非
動作軸のθ₁軸の振れは小さい。しかし明らかに本発明
の非干渉化制御した方が振れが少なく、特性は良好であ
ることがわかる。FIG. 4B is similar to FIG.
Shows the results of operating only theta ₂ axis as shown in (a). In this case, less affected by both type Both interference, deflection of theta ₁ axis of the non-action axis is small. However, it is apparent that the decoupling control of the present invention has less vibration and good characteristics.

【００４４】更に、図５（ｂ）は同図（ａ）のようにθ
₁，θ₂の両軸を同時に動作させたときの位置偏差の時間
応答特性を示す。同図から明らかなように、従来方式の
非干渉化をしない場合、減速・停止時に大きな動作の乱
れが生じ、それによってタクトが遅れるが、本発明方式
の非干渉化制御した場合は、動作の乱れも小さく、タク
ト遅れもなくなっていることがわかる。Further, FIG. 5B shows the case where θ is as shown in FIG.
The time response characteristics of the position deviation when both axes ₁ and ₂ are operated simultaneously are shown. As is clear from the figure, when decoupling in the conventional method is not performed, a large disturbance in operation occurs at the time of deceleration and stop, and the tact is delayed. It can be seen that the disturbance is small and the tact delay is eliminated.

【００４５】[0045]

【発明の効果】以上説明したように本発明によれば、外
乱補償機能をオブザーバに含ませることで、多関節ロボ
ットのロバストな非干渉化および極配置を同時に達成す
る制御アルゴリズムをシンプルに構成できる。このアル
ゴリズムは多変数系を扱って、さらに外乱補償機能をも
っているにもかかわらず、演算量が非常に少ない。例え
ばパラメータを最小にすると、スカラ型ロボット２軸分
で、積和演算が約２０回である。さらに、制御パラメー
タの計算（パラメータ設計）も、多項式の係数比較をす
る程度で、外乱補償、非干渉化および極配置のすべてを
同時に達成するパラメータが得られる。したがってＣＡ
Ｄ化も容易であり、非常に実用的である。また、スカラ
型ロボットによる実施例では、駆動機構が高減速比であ
るにもかかわらず、従来行われてきた各軸ごとのサーボ
系に比べ、停止時のオーバーシュートはなくなり、これ
によってタクトも短縮され、その有効性は明らかであ
る。As described above, according to the present invention, by including a disturbance compensation function in the observer, a control algorithm for simultaneously achieving robust decoupling and pole arrangement of an articulated robot can be simply configured. . This algorithm handles a multivariable system and has a very small amount of calculation despite having a disturbance compensation function. For example, when the parameters are minimized, the product-sum operation is performed about 20 times for two axes of the scalar robot. Further, in the calculation of control parameters (parameter design), a parameter that simultaneously achieves all of disturbance compensation, decoupling, and pole placement can be obtained to the extent that a polynomial coefficient comparison is performed. Therefore CA
D conversion is also easy and very practical. Also, in the embodiment using the SCARA type robot, despite the drive mechanism having a high reduction ratio, there is no overshoot at the time of stop compared to the conventional servo system for each axis, thereby shortening the tact time. Its effectiveness is clear.

【図面の簡単な説明】[Brief description of the drawings]

【図１】本発明の制御系の全体構成を示すブロック図で
ある。FIG. 1 is a block diagram showing the overall configuration of a control system according to the present invention.

【図２】本発明の一実施例を示すブロック図である。FIG. 2 is a block diagram showing one embodiment of the present invention.

【図３】ロボットのθ₁軸のみ動作させた時の位置指令
信号に対する実位置の偏差を示す特性図である。3 is a characteristic diagram showing a deviation of the actual position with respect to the position command signal when operating only theta ₁ axis robot.

【図４】θ₂軸のみ動作させた時の図３と同様の特性図
である。4 is a similar characteristic diagram and FIG. 3 when operating only theta ₂ axis.

【図５】θ₁，θ₂両軸を同時に動作させたときの位置偏
差の時間応答特性図である。FIG. 5 is a time response characteristic diagram of a position deviation when both θ ₁ and θ ₂ axes are simultaneously operated.

【符号の説明】[Explanation of symbols]

１ ロボット ２ 演算装置 ３ 位置指令信号発生装置 ４ 駆動装置 ５ 角位置出力信号検出装置 DESCRIPTION OF SYMBOLS 1 Robot 2 Computing device 3 Position command signal generator 4 Drive device 5 Angular position output signal detection device

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 9/10 B25J 9/16 G05B 19/18 ──────────────────────────────────────────────────続 き Continued on front page (58) Field surveyed (Int.Cl. ⁷ , DB name) B25J 9/10 B25J 9/16 G05B 19/18

Claims (4)

(57)【特許請求の範囲】(57) [Claims] 【請求項１】 所定の位置指令信号ｖ（ｋ）に応じて所
定の演算装置により算出された制御入力ｕ（ｋ）＝Ｑ^-1
（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝＋Ｇｖ
（ｋ）（但し、Ｋ（δ）＝Ｋ _１ δ＋Ｋ _０ ，Ｈ（δ）＝Ｈ
_２ δ ^２ ＋Ｈ _１ δ＋Ｈ _０ ，Ｑ ^-1 （δ）＝１／Ｉ（δ ^２ ＋ｑ
_１ δ＋ｑ _０ ））により多関節型ロボットを駆動制御し、
該ロボットの角位置出力信号ｙ（ｋ）を出力するように
構成された多関節型ロボット制御装置において、 前記演算装置は、前記位置指令信号ｖ（ｋ）から角位置
出力信号ｙ（ｋ）に至る伝達関数行列が、干渉要素がな
く、対角な２次の希望伝達関数Ｎ（δ）Ｍ ^−１ （δ）
（但し、Ｍ（δ）＝Ｉ（δ ^２ ＋ｍ _１ δ＋ｍ _０ ），Ｎ
（δ）＝Ｉｍ _０ ））を持つ伝達関数行列となるように構
成されており、前記制御入力ｕ（ｋ）の、状態オブザー
バによる状態フィードバック係数を含んだ部分Ｑ
^-1（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝に、
その次数を外乱の次数ｄに応じて上げ、Ｋ（δ）の係数
Ｋｉ（ｉ＝０〜ｄ）をＱ（δ）の係数Ｑｉ（ｉ＝０〜
ｄ）に等しくすることによって、外乱補償機能を付加
し、このような外乱ロバスト性を持たせた制御法則に基
づいて前記制御入力信号を算出するようにしたことを特
徴とする多関節型ロボット制御装置。1. A control input u (k) = Q ⁻¹ calculated by a predetermined arithmetic unit according to a predetermined position command signal v (k).
(Δ) {K (δ) u (k) + H (δ) y (k)} + Gv
(K) (where, K (δ) = K ₁ δ + K ₀ , H (δ) = H
₂ δ ² + H ₁ δ + H ₀ , Q ⁻¹ (δ) = 1 / I (δ ² + q
₁ δ + q ₀ )) to drive and control the articulated robot,
In the articulated robot control device configured to output an angular position output signal y (k) of the robot, the arithmetic unit, the position command signal v (k) from the angular position output signal y (k) The transfer function matrix that leads to a diagonal second-order desired transfer function N (δ) M ⁻¹ (δ) without interference elements
(However, M (δ) = I (δ ² + m ₁ δ + m ₀ ), N
(Δ) = I m ₀ )), and a portion Q of the control input u (k) including a state feedback coefficient by a state observer.
^-1 (δ) {K (δ) u (k) + H (δ) y (k)},
The order is raised according to the order d of the disturbance, and the coefficient Ki (i = 0 to d) of K (δ) is increased to the coefficient Qi (i = 0 to Q) of Q (δ).
By equal d), by adding the disturbance compensation function, articulated robot control, characterized in that to calculate the control input signal on the basis of such a disturbance robustness to have allowed control law apparatus. 【請求項２】 ｙ（ｋ）＝Ｎ（δ）Ｍ^−１（δ）ｖ
（ｋ） と設定することにより、上記伝達関数行列を対角化さ
れ、干渉要素が０となって非干渉化されるように構成し
たことを特徴とする請求項１記載の多関節型ロボット制
御装置。2. y (k) = N (δ) M ⁻¹ (δ) v
By setting a (k), is diagonalized the transfer function matrix, articulated robot control according to claim 1, wherein the interference element is characterized by being configured to be decoupled is 0 apparatus. 【請求項３】 ｙ（ｋ）≒Ｎ（δ）Ｍ^−１（δ）（ｚ＋
１）ｖ（ｋ）／２で、ｖ（ｋ）＝（ｚ＋１）ｖ（ｋ）／
２となるように設定して前記伝達関数行列の非干渉化と
極配置を同時に行ったことを特徴とする請求項１記載の
多関節型ロボット制御装置。3. y (k) ≒ N (δ) M ⁻¹ (δ) (z +
1) v (k) / 2, v (k) = (z + 1) v (k) /
2. The multi-joint type robot control device according to claim 1, wherein the transfer function matrix is set to be 2 so that the transfer function matrix is deinteracted and the poles are simultaneously arranged. 【請求項４】 前記多関節型ロボットは干渉要素を有し
ていて、前記演算装置は ｕ（ｋ）＝Ｑ^-1（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ
（ｋ）｝＋Ｇｖ（ｋ） に、基づく演算により位置指令信号ｖ（ｋ）に応じた最
適な制御入力信号ｕ（ｋ）を出力するように構成したこ
とを特徴とする請求項１記載の多関節型ロボット制御装
置。4. The multi-joint robot has an interfering element, and the arithmetic unit calculates u (k) = Q ⁻¹ (δ) ｛K (δ) u (k) + H (δ) y
2. The multi-function device according to claim 1, wherein an optimum control input signal u (k) corresponding to the position command signal v (k) is output by a calculation based on (k)｝ + Gv (k). articulated robot control device.