JPH06270079A

JPH06270079A - 多関節ロボット制御装置

Info

Publication number: JPH06270079A
Application number: JP5085449A
Authority: JP
Inventors: Akihiro Ito; 彰啓伊藤
Original assignee: Sankyo Seiki Manufacturing Co Ltd
Current assignee: Nidec Instruments Corp
Priority date: 1993-03-18
Filing date: 1993-03-18
Publication date: 1994-09-27
Anticipated expiration: 2016-08-20
Also published as: JP3200496B2

Abstract

(57)【要約】【目的】簡易で、計算誤差や外乱にも強い実用的な多
関節ロボットの非干渉化制御装置を提案することであ
る。【構成】ロボット１の伝達関数行列Ｒ（ｚ）Ｐ
^-1（ｚ）は干渉要素をもったものであり、このロボット
に対する位置指令信号ｖ（ｋ）、制御入力信号ｕ（ｋ）
及び角位置出力信号ｙ（ｋ）は演算装置２に入力され
る。演算装置２は上記伝達関数行列のロボット１に対
し、ｖ（ｋ）からｙ（ｋ）に至る伝達関数行列を干渉要
素のない対角なＮ（δ）Ｍ^-1（δ）に一致させるため、
Ｋ（ｚ），Ｈ（ｚ）及びｑ^-1（ｚ）の各パラメータを予
め定めておき、ｖ（ｋ）に対し最適なｕ（ｋ）を算出し
出力する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は多関節ロボット制御装置
に係り、特に多関節型ロボットのロバストな非干渉化制
御装置に関する。

【０００２】

【従来の技術】スカラ型ロボットのような多関節ロボッ
トの場合、通常の各軸の慣性力や摩擦力のほかに、各軸
間に慣性干渉力やコリオリ力・遠心力などが働いてい
る。このような相互干渉力があると、ある軸だけに対す
る指令が他の軸の出力にも影響を与えるため、本来望ま
ない応答が現われることになる。特に慣性干渉力は加速
度に依存するので、停止時のオーバーシュートやタクト
に悪い影響を与える。

【０００３】従来は各軸の駆動装置が高減速比でその影
響が弱められ、これらの干渉力は「その他外乱」として
積分補償のみですませ、構造の簡単な各軸ごとのＰＩサ
ーボ系を構成するのが一般的であった。しかし、年々ロ
ボットの動作性能に対する要求が高まって、アームスピ
ードは増加し、タクトも従来の半分以下というレベルに
なってきている。このような要求に対し、ＰＩ制御では
補償しきれないダイナミクスの影響や、高減速比駆動で
あっても干渉力などの影響が無視できなくなる。

【０００４】これに対し、最近、外乱推定によるロバス
ト制御が注目されている。これは外乱推定オブザーバに
より外乱を補償した後、その補償後の系に対して、状態
（速度）オブザーバを付けて制御系を構成するもので、
演算量も多い。また、外乱としてステップ状外乱を想定
しているため、過渡的な応答には偏差が大きくなるとい
うことには変わりはない。さらに制度を向上するために
は、ダイレクトドライブ方式が有利であるが、そのとき
はこのような外乱の影響は非常に大きなものとなり、も
っと積極的に対処することが不可欠となる。

【０００５】

【発明が解決しようとする課題】一方、相互干渉力を補
償し、指令の出力応答に見かけ上何の干渉もないよう制
御する非干渉化制御の手法があり、これまで種々の手法
が提案されてきている。しかし、これらは理論や構成が
複雑であったり、正確なパラメータや計算精度を要する
ことが多かった。さらに、ロバスト性を付加するために
補償器を挿入すると、次数が増大し演算量を増すことに
なる。

【０００６】最近、マイクロプロセッサの速度は向上し
ているとはいえ、経済的理由などで実用上まだ演算量の
多少は大きな問題である。そして、整数演算をさせるこ
とがほとんどであるため、演算誤差が外乱として制御系
に影響を及ぼすことになる。

【０００７】本発明の目的は以上のような問題を解決す
る簡易かつ計算誤差や外乱にも強い実用的な多関節ロボ
ットの非干渉化制御装置を提供することにある。

【０００８】

【課題を解決するための手段】本発明の多関節ロボット
制御装置は、上記目的を達成するため、所定の位置指令
信号に応じて所定の演算装置により算出された制御入力
信号により多関節型ロボットを駆動制御し、該ロボット
の角位置出力信号を検出するように構成され、前記演算
装置は前記位置指令信号から角位置出力信号に至る伝達
関数行列が干渉要素のない希望伝達関数行列となるよう
に、所定の制御法則に基づいて前記制御入力信号を算出
するようになっていることを要旨とする。

【０００９】

【作用】本発明では、外乱補償機能をインプリシットに
状態（速度）オブザーバに含ませている。これにより、
ロボットに入力するトルク指令とロボットの位置出力だ
けから、少ない制御器次数（すなわち少ない演算量）で
ロバスト性を保ちつつ制御目的を達成している。この場
合、制御目的は多関節ロボットの非干渉化と各軸ごとの
希望モデル（伝達関数）への極配置である。すなわち、
単に制御器次数が少ないだけでなく、外乱補償、非干渉
化及び極配置という三つの機能を１ステップで同時に達
成する、極めてシンプルな設計法と制御アルゴリズムを
提供することができる。

【００１０】

【実施例】一般にｎ軸の多関節型ロボットの運動方程式
は次式のように表わせる。

【００１１】

【数１】

【００１２】ここで、Ｍ（）はｎ×ｎ慣性行列、Ｄ
（）は粘性摩擦・コリオリ力・遠心力・逆起電力に関
するｎ×ｎ行列で各々非対角要素が干渉項である。Ｋは
モータ・ドライバ・駆動機構などのｎ×ｎの対角なゲイ
ン行列、ｆはクーロン摩擦・重力を表わすｎ次元ベクト
ルである。またθ，ｄθ／ｄｔ，ｄ²θ／ｄｔ²はそれぞ
れ軸角度、角速度、角加速度を表わし、ｕはドライバに
印加されるｎ次元制御入力信号である。

【００１３】実際の制御アルゴリズムは、マイクロプロ
セッサ上にソフトウエアとしてインプリメントされるこ
とを前提として、入力をＤＡコンバータで０次ホールド
し、サンプリング時間τ_s（ｓ）で離散化する。このよ
うにして式（１）を離散系状態方程式で表わすと、

【００１４】

【数２】

【００１５】ここで、η（ｋ）はクーロン摩擦・重力等
の定値外乱項である。もとの連続系には零点（伝達関数
の分子多項式の根）はないが、このような離散化によっ
て単位円近傍に零点をもつようになり、サンプリング時
間を短くすると零点が単位円の外側にある非最小位相系
となる。このことはまた後で扱うことにする。ここで後
述する外乱補償機能により、式（２）の定値外乱η
（ｋ）は完全に補償されるためη項を除いて考え、式
（２），（３）をｒｒｐ分解して進み演算子ｚ（時系列
信号ｓ（ｋ）に対し、ｓ（ｋ＋１）＝ｚｓ（ｋ）となる
ような演算子）のｎ×ｎ多項式行列Ｐ（ｚ），Ｒ（ｚ）
によって表わすと、

【００１６】

【数３】ｙ（ｋ）＝Ｒ（ｚ）Ｐ^-1（ｚ）ｕ（ｋ）（４）となる。この式（４）が制御対象の離散系伝達関数行列
で、多変数系を扱う場合のひとつの表現方法である。

【００１７】ここで以降の論議を簡便にするため、新た
に演算子δ＝ｚ−１を用いる。これにより制御対象の多
項式行列を表わすと次のようになる。

【数４】Ｐ（δ）＝Ｉδ²＋Ｐ₁δ＋Ｐ₀ （５）Ｒ（δ）＝Ｒ₁δ＋Ｒ₀ （６）ロボットの位置決め制御の場合、Ｐ₀＝０であるが、
Ｐ₁，Ｒ₁，Ｒ₀は対角要素のほかに非対角の０でない干
渉要素がある。また、Ｒ（δ）は正則とする（逆行列が
存在する）が、この仮定は通常のロボットでは成立す
る。

【００１８】次に、外乱モデルを求める。式（２）でη
を表わす外乱モデルは、各軸ごとにη（ｔ）＝η₀＋η₁
ｔ＋……＋η_dｔ_dで表わされ、そのｚ変換をδで表わす
と、

【数５】 η（δ）＝η₀／δ＋……＋η_d／δ^d+1 （７）のように定義できる。ηは定値外乱であるからｄ＝０で
ある。なお、η₁は未知である。

【００１９】さて本発明の制御目的は、ロボットの動特
性が非干渉化され、さらに各軸ごとの伝達関数が極配置
され、希望伝達関数に一致することである。つまり、各
軸ごとに伝達関数ｍ（δ）＝ｍ₀／（δ²＋ｍ₁δ＋ｍ₀）
となるように、指令ｖ（ｋ）からｙ（ｋ）に至る希望伝
達関数行列を

【数６】ｙ（ｋ）＝Ｎ（δ）Ｍ^-1（δ）ｖ（ｋ）（８）Ｍ（δ）＝Ｉ（δ²＋ｍ₁δ＋ｍ₀）Ｎ（δ）＝Ｉｍ₀ と設定すれば、伝達行列が対角化され干渉要素が０とな
り、非干渉化されることになる。

【００２０】次に、干渉要素をもった伝達関数行列で表
わされる制御対象（式（４））に対し、位置指令ｖ
（ｋ）からｙ（ｋ）に至る伝達関数行列を干渉要素のな
い対角なＮ（δ）Ｍ^-1（δ）に一致させる制御法則を求
める。

【００２１】まず、制御対象の軸ごとの次数（最大列次
数）が２、外乱の次数がｄ＋１＝１であるから、これら
の次数により、任意の（２−１）＋（ｄ＋１）＝２次の
モニック安定多項式をｑ（δ）＝δ²＋ｑ₁δ＋ｑ₀と
し、これを対角要素とするｎ×ｎ多項式行列を

【数７】Ｑ（δ）＝Ｉｑ（δ）（９）とする。このｑ（δ）がオブザーバの特性多項式とな
り、制御系の極配置モデルとは独立に設定できる。そし
て次式のような、Ｑ（δ）の次数より１次低い１次のｎ
×ｎ多項式行列Ｋ（δ）、およびＱ（δ）と同じ２次の
ｎ×ｎ多項式行列Ｈ（δ）を考える。

【００２２】

【数８】Ｋ（δ）＝Ｋ₁δ＋Ｋ₀ （１０）Ｈ（δ）＝Ｈ₂δ²＋Ｈ₁δ＋Ｈ₀ （１１）最大列行次数が３次の任意の多項式行列Ｆ（δ）に対
し、多項式方程式（１２）を満足する式（１０），（１
１）のような多項式行列Ｋ（δ），Ｈ（δ）を係数比較
により求める。

【００２３】

【数９】Ｋ（δ）Ｐ（δ）＋Ｈ（δ）Ｒ（δ）＝Ｆ（δ）（１２）このとき、Ｋ₁＝Ｉｑ₁，ｉ＝０〜ｄとすることで、η
（ｋ）を漸近的に０とする外乱補償機能を制御法則に内
包させ、定常偏差を生じさせなくすることができる。さ
らに、このようにＫ₁，ｉ＝０〜ｄを選ぶことで、その
他のＫ（δ），Ｈ（δ）の係数を唯一に決めることがで
きる。さて式（１２）において、Ｆ（δ）を

【００２４】

【数１０】Ｆ（δ）＝Ｑ（δ）｛Ｐ（δ）−ＧＭ（δ）Ｎ^-1（δ）Ｒ₀｝（１３）Ｇ＝Ｎ（δ）Ｒ₀ ^-1 としたとき、以下の制御法則

【００２５】

【数１１】ｕ（ｋ）＝Ｑ^-1（δ）｛Ｋ（δ）ｕ（ｋ）＋Ｈ（δ）ｙ（ｋ）｝＋Ｇｖ（ｋ）（１４）を考える。式（４），（１４）より

【００２６】

【数１２】ｙ（ｋ）＝Ｒ（δ）｛Ｑ（δ）Ｐ（δ）−Ｋ（δ）Ｐ（δ） −Ｈ（δ）Ｒ（δ）｝^-1Ｑ（δ）Ｇｖ（ｋ）（１５）

【００２７】式（１５）に式（１２），（１３）を代入
すると、

【数１３】ｙ（δ）＝Ｒ(δ)｛Ｑ（δ）ＧＭ(δ)Ｎ^-1(δ)Ｒ₀｝^-1Ｑ（δ）Ｇｖ(ｋ) ＝Ｒ(δ)Ｒ₀ ^-1Ｎ(δ)Ｍ^-1(δ)Ｇ^-1Ｑ^-1(δ)Ｑ(δ)Ｇｖ(ｋ) ＝Ｒ(δ)Ｒ₀ ^-1Ｎ(δ)Ｍ^-1(δ)ｖ(ｋ) （１６）式（１６）第２行目のＱ（δ）は、その対角要素ｑ
（δ）を安定多項式に選んであるので、Ｑ^-1（δ）Ｑ
（δ）＝Ｉというようにキャンセル可能である。

【００２８】ここでＲ（δ）は、ロボットを０次ホール
ドして、短いサンプリング時間で離散化したことによっ
て、単位円近傍に零点をもつため、式（１６）において

【数１４】Ｒ（δ）Ｒ₀ ^-1＝Ｒ₁Ｒ₀ ^-1δ＋Ｉ ≒Ｉ（０．５δ＋１）＝Ｉ（ｚ＋１）／２（１７）

【００２９】という近似を行う。したがって、式（１
７）を式（１６）に代入すると、

【数１５】ｙ（ｋ）≒Ｎ（δ）Ｍ^-1(δ)（ｚ＋１）ｖ（ｋ）／２（１８）となって、新たにｖ（ｋ）＝（ｚ＋１）ｖ（ｋ）／２と
なるように規範入力を細工することで、非干渉化と極配
置が同時に達成できる。

【００３０】図１は上述した式（１４）により規定され
る制御法則に基づく本発明の制御系全体の構成を示す。
同図において、１は制御対象としての多関節型ロボッ
ト、２は演算装置、３は位置指令信号発生装置、４は駆
動装置、５は角位置出力信号検出装置である。ロボット
１の実際の伝達関数（離散系伝達関数行列）はＲ（ｚ）
Ｐ^-1（ｚ）で、演算装置２は干渉要素をもった上記伝達
関数の制御対象１に対し、干渉要素のない対角なＮ
（δ）Ｍ^-1（δ）の希望伝達関数としての処理を可能と
するため、前記式（１４）に基づく演算により位置指令
信号ｖ（ｋ）に応じた最適な制御入力信号ｕ（ｋ）を出
力する。なお、以上においてはｎ×ｎの多項式行列で説
明したが、ｎ＝１の場合でも本発明は成立する。

【００３１】次に、図１の制御系の外乱応答特性の解析
においては、規範入力をｖ（ｋ）＝０として、外乱η
（ｋ）から出力ｙ（ｋ）に至る伝達特性を調べる。ま
ず、外乱η（ｋ）は式（２）のように入力ｕ（ｋ）に加
算された形で印加されるので、式（４）は、

【数１６】ｙ（ｋ）＝Ｒ（δ）Ｐ^-1（δ）｛ｕ（ｋ）＋η（ｋ）｝（１９）となる。

【００３２】式（１２），（１３），（１４）および
（１９）より、

【数１７】ｙ（ｋ）＝Ｒ（δ）Ｒ₀ ^-1Ｎ（δ）Ｇ^-1｛Ｑ（δ）−Ｋ（δ）｝ ×Ｍ^-1（δ）Ｑ^-1（δ）η（ｋ）（２０）

【００３３】この式で、定常応答、すなわちｋ→∞とし
たときのｙ（ｋ）は、

【００３４】

【数１８】

【００３５】である。ここで、η（δ）＝η’（δ）／
δ^d+1と表わせる。また、Ｋ₁＝Ｉｑ₁，ｉ＝０〜ｄのよ
うに選んであるから、Ｑ（δ）−Ｋ（δ）＝δ
^d+1｛Ｑ’（δ）−Ｋ’（δ）｝と表わせ、η（δ）の
分母δ^d+1がキャンセルされる。したがって式（２１）
は、

【００３６】

【数１９】

【００３７】となり、ここで、分母行列Ｍ^-1（δ）Ｑ^-1
（δ）は安定に選んであるから、式（２２）の値は０に
なる。すなわち、ｋ→∞のとき外乱η（δ）の出力への
影響は０に収束する。したがって、この系は式（７）の
ような外乱ベクトルη（δ）に対して定常偏差を生じな
い。

【００３８】図２は図１の制御系に基づく本発明の多関
節ロボット制御装置の一実施例で、多関節型ロボット１
は、例えば、スカラ型ロボットが用いられ、このロボッ
トの制御軸数は４軸であるが、主に干渉のある水平２リ
ンク機構を構成するθ₁軸（第１軸）とθ₂軸（第２軸）
に本発明の制御方式を適用する。演算装置２としては、
例えばディジタル信号処理装置（ＤＳＰ）が用いられ、
この装置による演算制御のためＰ（ｚ）等は下記のよう
にして定める。すなわち、制御対象１のパラメータは、
駆動装置４を含めて行った最小自乗法によるパラメータ
同定試験により得られ、

【００３９】

【数２０】

【００４０】である。希望モデルの二つの極は２０ｒａ
ｄ／ｓおよび８００ｒａｄ／ｓ、オブザーバは２次のバ
タワース極１２０ｒａｄ／ｓに設定した。制御法則（式
（１４））のパラメータは、以下のようになる。

【００４１】

【数２１】

【００４２】図３（ｂ）は同図（ａ）のようにロボット
１のθ₁軸のみ動作させた時の、位置指令信号ｖ（ｋ）
に対する実位置の偏差をプロットしたもので、点線は従
来方式、実際は本発明の方式による結果を示す。同図か
ら明らかなように、非干渉化を行わない従来の各軸サー
ボ制御方式の場合、干渉力が高減速でかなり小さくなっ
ているにもかかわらず、０指令のはずのθ₂軸は大きく
振れているが、本発明の方式により非干渉化制御した場
合、θ₂軸はほとんど振れず、干渉力の影響が十分に補
償されているのがわかる。

【００４３】また、図４（ｂ）は図３と同様にして図４
（ａ）のようにθ₂軸のみ動作させた結果を示す。この
場合は、両方式のどちらも干渉による影響は少なく、非
動作軸のθ₁軸の振れは小さい。しかし明らかに本発明
の非干渉化制御した方が振れが少なく、特性は良好であ
ることがわかる。

【００４４】更に、図５（ｂ）は同図（ａ）のようにθ
₁，θ₂の両軸を同時に動作させたときの位置偏差の時間
応答特性を示す。同図から明らかなように、従来方式の
非干渉化をしない場合、減速・停止時に大きな動作の乱
れが生じ、それによってタクトが遅れるが、本発明方式
の非干渉化制御した場合は、動作の乱れも小さく、タク
ト遅れもなくなっていることがわかる。

【００４５】

【発明の効果】以上説明したように本発明によれば、外
乱補償機能をオブザーバに含ませることで、多関節ロボ
ットのロバストな非干渉化および極配置を同時に達成す
る制御アルゴリズムをシンプルに構成できる。このアル
ゴリズムは多変数系を扱って、さらに外乱補償機能をも
っているにもかかわらず、演算量が非常に少ない。例え
ばパラメータを最小にすると、スカラ型ロボット２軸分
で、積和演算が約２０回である。さらに、制御パラメー
タの計算（パラメータ設計）も、多項式の係数比較をす
る程度で、外乱補償、非干渉化および極配置のすべてを
同時に達成するパラメータが得られる。したがってＣＡ
Ｄ化も容易であり、非常に実用的である。また、スカラ
型ロボットによる実施例では、駆動機構が高減速比であ
るにもかかわらず、従来行われてきた各軸ごとのサーボ
系に比べ、停止時のオーバーシュートはなくなり、これ
によってタクトも短縮され、その有効性は明らかであ
る。

【図面の簡単な説明】

【図１】本発明の制御系の全体構成を示すブロック図で
ある。

【図２】本発明の一実施例を示すブロック図である。

【図３】ロボットのθ₁軸のみ動作させた時の位置指令
信号に対する実位置の偏差を示す特性図である。

【図４】θ₂軸のみ動作させた時の図３と同様の特性図
である。

【図５】θ₁，θ₂両軸を同時に動作させたときの位置偏
差の時間応答特性図である。

【符号の説明】

１ロボット２演算装置３位置指令信号発生装置４駆動装置５角位置出力信号検出装置

Claims

【特許請求の範囲】

【請求項１】所定の位置指令信号に応じて所定の演算
装置により算出された制御入力信号により多関節型ロボ
ットを駆動制御し、該ロボットの角位置出力信号を検出
するように構成され、前記演算装置は前記位置指令信号
から角位置出力信号に至る伝達関数行列が干渉要素のな
い希望伝達関数行列となるように、所定の制御法則に基
づいて前記制御入力信号を算出するようになっているこ
とを特徴とする多関節ロボット制御装置。