JPH0416803B2 - - Google Patents

Description

【発明の詳細な説明】 本発明はロボツト制御装置に関し、特にロボツ
トのアームやマニピユレータが目標指示軌跡へ高
い精度で追従しつつ移動する際にともなつて発生
するアームの振動を有効に抑制することができる
ロボツト制御装置に関する。

従来、ロボツトのアームのように、３次元空間
上を自由に移動するアームの制御装置において
は、アームの３次元空間上での位置を正確に検出
できる小型で使い易いセンサが得られないという
理由で、アームの位置については制御が行われて
いないのが現状である。このため従来のロボツト
の制御装置では、第１図に示す如く、関数発生部
１から逐次関節軸の目標指示位置信号（以後、指
示位置信号と書く）y_Dと関節軸の指示回転加速度
信号y_aを出力し、この指示回転加速度信号y_aに応
じて関節軸を駆動するモータ３を駆動するととも
に、このモータ３によりアームの如き機械系部を
駆動している。そしてモータ３に設けたエンコー
ダ（図示省略）によりモータ３の回転位置を検出
して得られる位置信号y_nと前記指示位置信号y_Dと
を比較して位置偏差信号e_nを求め、位置偏差補償
器２により位置偏差補償信号を得、この位置偏差
補償信号と前記回転加速度信号y_aを加算してモー
タ操作信号ｕを算出し、これによりモータ３の回
転を制御していた。

このように、モータ３にエンコーダ等を取りつ
けモータの回転角を位置信号としてフイードバツ
クすることにより、関節角を駆動するモータに対
しては指示位置に偏差なく追従できるが、このモ
ータにより駆動されるアーム等の機械系について
は、この機械系が開ループとなつているため、ア
ームの移動中、特に加速、減速、停止時に極めて
大きな振動が発生するという欠点を有していた。

例えば塗装や溶接作業等のように、移動経路が
重視される場合には、振動によるアームの指示軌
跡からのずれが作業内容を低下させ、また停止時
の振動が作業空間内の器具や組立部品の摩耗や破
損を招くという欠点があつた。さらにこのような
振動はロボツト自身の機械的要素をガタを生み、
ロボツトの性能を低下させる原因ともなり、みた
目もよくない。そのためこのような振動の発生を
防止するために従来は高速性を犠牲にして低速駆
動して加速度を小さくし、振動の抑制を計つてい
た。そのため作業能率の高速性を向上する場合、
大きな問題となつていた。

したがつて本発明の目的は、このような問題を
改善するために、ロボツトにセンサを取りつけて
機械系の振動を検出し、それを関数発生部の指示
値に追従するようにする振動抑制補償器を備える
ことにより、移動に伴つて発生する振動を抑制す
ること、さらその補償器がすでにある位置偏差補
償器と調和するように設定することにより、高速
性を失うことなくアームを忠実に追従させること
を満たすロボツト制御装置を提供することにあ
る。

そしてこの目的を達成するために、本発明のロ
ボツト制御装置は、ロボツトのアームやマニピユ
レータの３次元空間あるいは関節軸での指示加速
度信号、指示速度信号、及び指示位置信号を出力
する関数発生器と、モータ系からの位置信号と指
示位置信号との偏差であるモータ系の位置偏差信
号とモータ系の操作信号に基づきモータ系の状態
量を推定するモータ系状態観測器と、モータ系の
位置偏差信号を積分する位置偏差積分器を有する
位置偏差補償器と、上記アームやマニピユレータ
タに設けられ、加速度計により振動状態を検出す
る振動検出手段と、関数発生部からの指示加速度
信号と振動検出手段の加速度信号との偏差である
加速度偏差信号及びモータ系からの位置信号を入
力して機械系の状態量を推定する機械系状態観測
器及び加速度偏差信号を積分する振動抑制積分器
を有する振動抑制補償器と、関数発生部から出力
される指示加速度信号、指示速度信号及び指示位
置信号と、前記位置偏差補償器のモータ系の状態
観測器の出力信号と位置偏差積分器出力信号と、
前記振動抑制補償器の機械系の状態観測器の出力
信号と振動抑制積分器の出力信号とを、各々係数
器を介して加算する加算手段を具備し、その出力
をモータ系の操作信号に使用し、上記各係数器の
係数を過渡振動の抑制と高い位置決め精度を得る
よう最適な値に決定補正することを特徴とする。

以下本発明の一実施例を第２図〜第５図にもと
づき説明する。

第２図は本発明の一実施例構成図、第３図は本
発明の構成を伝達関数して示したもの、第４図は
本発明における補償器の詳細説明図であり、第５
図は指示軌跡を示すものである。

図中、５は振動検出器であつてロボツトのアー
ムの振動を検出するものであり、一例として加速
度を検出する加速度計が使用され、６は振動抑制
補償器であつて、関数発生部１から出力された３
次元空間上の振動検出器が設置されている場所で
の指示加速度信号y_aと振動検出器５から検出され
た機械系加速度信号y_pの差に応じた信号が入力さ
れ、これに応じてモータ系３を制御するものであ
る。

関数発生部１は、第５図ａ−３のような、始点
及び終点近傍で一定加速度の加減速を行わせる指
示値y_aを発生する場合の伝達関数モデルであり、
またモータ系３（モータ、モータ駆動回路、エン
コーダ位置カウンタ等）は、電流の２回積分が位
置信号y_nとなるのでKm／s²、アームのような機械
系４は、その振動検出器５まで含めると第３図に
示す如き２次の振動系K_pω₀ ²s²／（s²＋2ζω₀s＋
ω₀ ²）で表される。

まず、各制御系を状態方程式で記述する。

関数発生部は、x_rを位置x_r1、速度x_r2、加速度
x_r3からなる３次の状態ベクトルとすると、 x_r△＝x_r1 x_r2 x_r3 x〓（ｔ）＝A_rx_r（ｔ） (1) で表される。

ここで、A_rは３×３の係数行列である。

A_r＝０，１，０ ０，０，１ ０，０，０ 次にy_rを指示位置y_D、指示加速度y_aからなる２
次の出力ベクトルとすると y_r（ｔ）＝y_D（ｔ） y_a（ｔ） (2) ＝c′_rD c′_rax_r（ｔ） (3) で表される。

ここで、c′_rD、c′_raは３次の出力ベクトルであ
り、 c′_rD＝〔１，０，０〕 c′_ra＝〔０，０，K_p〕 であれば、指示位置y_Dはx_r1と、またK_p＝１であ
れば指示加速度y_aはx_r3と等しくなる。

モータ系はx_nを２次の状態ベクトル、ｕをス
カラーの操作量およびy_nをスカラー出力（モー
タの位置信号）とすると、 x〓_n（ｔ）＝A_nx_n（ｔ）＋b_nｕ（ｔ） (4) y_n（ｔ）＝c_nx_n（ｔ） (5) となる。ここで、A_nは系の２×２係数行列、b_n
は２次の駆動ベクトル、c′_nは２次の出力ベクト
ルである。ここで、 A_n＝０，１ ０，０，b_n＝０ K_n c′_n＝〔１，０〕 である。

なお、上記(4)、(5)式は下記のようにして得られ
る。

モータ系の伝達関数はK_n／s²で表される。こ
れをラプラス変換すると下記の如くなる。

Y_n（ｓ）＝K_n／s²Ｕ（ｓ） これを微分方程式で表すと、 y¨_n＝K_nｕ となる。ここで、 x_n1△＝y_n x_n2△＝x〓_n1 と定義して、状態方程式で表すと x〓_n1＝x_n2 x〓_n2＝K_nｕ となる。これをベクトル表現すると x〓_n1 x〓_n2＝０，１ ０，０x_n1 x_n2＋０ K_nｕ x〓_n＝A_nx_n＋b_nｕ y_n＝〔１，０〕x_n ＝c′_nx_n となる。

また機械系はx_pを２次の状態ベクトル、y_pをス
カラー出力（機械系の加速度信号）とすると、 x〓_p（ｔ）＝A_px_p（ｔ）＋b_py_n（ｔ） (6) y_p（ｔ）＝c′_px_p（ｔ）＋d_py_n（ｔ） (7) で表わすことができる。ここで、A_pは系の２×
２係数行列、b_pは２次の駆動ベクトル、c′_pは２次
の出力ベクトルであり、d_pはスカラーの伝達係数
である。ただし、「’」は転置を意味するもので
ある。ここで、 A_p＝０， −ω_p ²， １ −2ζω_p b_p＝０ K_pω_p ² c′_p＝〔−ω_p ²，−2ζω_p〕 d_p＝K_pω_p ²C_p で表される。なお、上記(6)、(7)式は下記のように
して得られる。機械系の伝達関数は K₀ω₀ ²s²／s²＋2ζω₀s＋ω₀ ² で表される。これをラプラス変換すると下記の如
くなる。

Y_p（ｓ）＝K₀ω₀ ²s²／s²＋2ζω₀s＋ω₀ ²Y_n（ｓ） これをさらに Y_p（ｓ）＝s²Z（ｓ） ただし、 Ｚ（ｓ）＝K₀ω₀ ²／s²＋2ζω₀s＋ω₀ ²Y_n（ｓ） 書き直して、微分方程式で表すと x¨＋2ζω₀z〓＋ω₀ ²z＝Koω₀ ²y_n となる。ここで、 x_p1△＝ｚ x_p2△＝x〓_p1 と定義して、状態方程式で表すと x〓_p1＝x_p2 x〓_p2＝−ω₀ ²x_p1−2ζω₀x_p2＋K_pω₀ ²y_n となる。これをベクトル表現すると x〓_p1 x〓_p2＝０， −ω_p ²， １ −2ζω_px_p1 x_p2 ＋０ K_pω_p ²y_n x〓_p＝A_px_p＋b_py_n また、 y_p＝x¨_p1 ＝x〓_p2 ＝−ω₀ ²x_p1−2ζω₀x_p2＋K_pω₀ ²y_n y_p＝〔−ω_p ²，−2ζω_p〕x_p1 x_p2 ＋K_pω₀ ²y_n ＝c′_px_p＋d_py_n となる。

モータ系についての位置偏差補償器２内の位置
偏差積分器および機械系の定常加速度偏差e_p（∞）
を零にするための振動抑制積分器については、そ
れぞれ、 x〓_ni（ｔ）＝e_n（ｔ） (8) ＝y_D（ｔ）−y_n（ｔ） (9) x〓_pi（ｔ）＝e_p（ｔ） (10) ＝y_a（ｔ）−y_p（ｔ） （11） と示すことができる。

さて振動抑制補償器６を位置偏差補償器２と調
和させて、 e_n（∞）＝０ （12） e_p（∞）＝０ （13） にするサーボ問題は、各制御系を合成することに
より最適レギユレータ問題に帰着できる。前記(1)
〜（11）式を合成した複合系を x〓（ｔ）＝Ax（ｔ）＋bu（ｔ） （14） ｙ（ｔ）＝Cx（ｔ） （15） ここでｙ（ｔ）は下記の通り記述できる。

ｙ（ｔ）＝e_n（ｔ） e_p（ｔ）＝y_D（ｔ）−y_n（ｔ） y_a（ｔ）−y_p（ｔ） ＝c′_rDx_r(t)−c′_nx_n(t) c′_rax_r(t)−（c′_pX_p(t)＋d_py_n(t)） ＝c′_rDx_r(t)−c′_nx_n(t) c′_rax_r(t)−（c_px_p(t)−d_pc′_nx_n(t) ＝c′_rD， c′_ra，０， ０， ０， −c′_p０， ０，−c′_n −d_pc′_nx_r x_pi x_p x_ni x_n ここで系の係数行列Ａ、駆動ベクトルｂおよび
出力行列Ｃは次のように表される。

ｘ＝x_r x_pi x_p x_ni x_n、 Ａ＝A_r， c′_ra， ０， −c′_rD， ０，０， ０， ０， ０， ０， ０， −C′_p， A_p， ０， ０，０， ０， ０， ０， ０， ０ −d_pc′_n b_pc′_n −c′_n A_n ｂ＝０ ０ ０ ０ b_n、 C′＝c′_rD， c′_ra，０， ０， ０ −c′_p，０， ０，−c′_n， −d_pc′_n （16） と表す。

ここで、高い位置決め精度を実現しつつ機械系
の振動を抑制するために次式に評価関数を設定す
る。

Ｊ（ｕ(t)）＝∫^∞ ₀x_ni ²(t) ＋σx_pi ²(t)＋λu²(t)dt （17） ただし、第１項は高い位置決め精度を実現する
ために挿入した位置偏差の積分値の２乗であり、
第２項は振動を抑制するために挿入した機械系に
ついての加速度偏差の積分値２乗である。第３項
はモータへの操作エネルギーを表している。ま
た、σ、λは第１項を１としたときの重み係数で
ある。（14）式〜（16）式を制約条件としてこの
評価関数を最小にする最適な操作量ｕ(t)は一意
に求めることができ、次式で与えられる。

ｕ(t)＝f′x(t) （18） f′＝−b′K／λ （19） ここで、Ｋは次のリツカチの代数方程式によつ
て求める。

Kbb′K／λ−KA−A′K−c_nic′_ni −c_pic′_piσ＝０ （20） ここで、 c′_ni＝〔０，０，０，１，０〕 c′_pi＝〔０，１，０，０，０〕 である。

また上記（18）式は、 ｕ(t)＝f′_rx_r(t)＋f_nix_ni(t)＋f_pix_pi(t) ＋f′_nx^_n(t)＋f′_px^_p(t) （21） と書ける。モータ系や機械系の内部状態ベクトル
x_n、x_pについては直接測定できないので、推定
値x^_n、x^_pを用いる。

x^_nは、 x^_n(t)＝A_nx^_n(t)＋b_nｕ(t) ＋g_n（e_n(t)＋c′_nx^_n(t)） （22） のモータ系状態観測器２−０から生成される。た
だし、g_nはこのモータ系状態観測器２−０の２
次のゲインベクトルである。

またx_pは、 x^_p(t)＝A_px^_p(t)＋b_py_n(t)＋g_p（e_p(t) ＋c′_px^_p(t)＋d_py_n(t)） （23） の機械系状態観測器６−０から生成される。ただ
し、g_pは機械系状態観測器６−０の２次のゲイン
ベクトルである。

これは閉ループ系を漸近的に安定する安定化補
償器としても働いている。また（21）式第１項は
関数発生部１からのフイードフオワードなループ
で、目標位置に関する値を加えるものであり、ア
ームの指示軌跡への追従特性を向上させる働きを
している。

以上のようにして得られた、本発明における位
置偏差補償器２および振動抑制補償器６の構成
は、第４図に示すようになる。また（18）式のｆ
については、評価関数の重みσ、λをパラメータ
として高い位置決め精度を実現でき、振動が抑制
するように（19）式、（20）式を用いて予め計算
される。

本発明の実施例によれば、第５図ａ−１〜ａ−
３のような場合に限らず、第５図ｂ，ｃのような
指示軌跡がステツプ関数や、ランプ関数の場合に
も、アームがそれらに偏差なく追従し、移動中お
よび停止時の振動を充分に抑制することができ
る。また関数発生部の伝達関数モデルの積分器の
数がＬ個の場合には、位置偏差補償器および振動
抑制補償器内の積分器の数をそれぞれＬ−２個以
上にすることにより、Ｌ−１次関数を含む指示軌
跡に対して、アームを偏差なく追従させ、振動を
抑制させることができる。このようにして第５図
ｄ−１〜ｄ−４のような、始点および終点近傍で
加速度の変分を一定とする場合においても極めて
有効である。さらに機械系の次数をｍ次にするこ
とによつて、ｍ／２個の共振点をもつような振動
に対しても十分抑制効果がある。

以上説明の如く、本発明よれば、ロボツトのア
ームやマニピユレータの移動のときに生じる振動
を有効に抑制することができるので、これらを速
い速度で駆動することが可能となり、その作業性
能を向上させることができる。

【図面の簡単な説明】

第１図は従来のロボツト制御装置を示し、第２
図は本発明の一実施例構成図、第３図は第２図を
伝達関数として示したもの、第４図は本発明にお
ける補償器の詳細説明図、第５図は各種の指示軌
跡を示すものである。 図中、１は関数発生部、２は位置偏差補償器、
３はモータ系、４は機械系、５は振動検出器、６
は振動抑制補償器をそれぞれ示す。

Claims (1)

1. 【特許請求の範囲】 １ ロボツトのアームやマニピユレータの３次元
   空間あるいは関節軸での指示加速度信号、指示速
   度信号、及び指示位置信号を出力する関数発生器
   と、 モータ系からの位置信号と指示位置信号との偏
   差であるモータ系の位置偏差信号とモータ系の操
   作信号に基づきモータ系の状態量を推定するモー
   タ系状態観測器及びモータ系の位置偏差信号を積
   分する位置偏差積分器を有する位置偏差補償器
   と、 上記アームやマニピユレータに設けられ、加速
   度計により振動状態を検出する振動検出手段と、 関数発生部からの指示加速度信号と振動検出手
   段の加速度信号との偏差である加速度偏差信号及
   びモータ系からの位置信号を入力して機械系の状
   態量を推定する機械系状態観測器及び加速度偏差
   信号を積分する振動抑制積分器を有する振動抑制
   補償器と、 関数発生部から出力される指示加速度信号、指
   示速度信号及び指示位置信号と、 前記位置偏差補償器のモータ系の状態観測器の
   出力信号と位置偏差積分器出力信号と、 前記振動抑制補償器の機械系の状態観測器の出
   力信号と振動抑制積分器の出力信号とを、 各々係数器を介して加算する加算手段を具備
   し、その出力をモータ系の操作信号に使用し、上
   記各係数器の係数を過渡振動の抑制と高い位置決
   め精度を得るよう最適な値に決定することを特徴
   とするロボツト制御装置。