JPS62286101A - 多自由度マニピユレ−タの制御装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 ３、発明の詳細な説明 ［発明の目的］ （産業上の利用分野） 本発明は、多自由度マニピュレータの制御装置に係り、
特に、制御用演算の演算回数の増大化を招くことなくマ
ニピュレータの動特性の変化等に対処でき、制御性能の
向上化を図れるようにした制御装置に関する。

（従来の技術） 多自由度を有するマニピュレータの制御装置には１通常
１次の３つの方式が採用されている。

すなわち、その１つは、第３図に示すように、マニピュ
レータ１の非線形特性や各自由度間の干渉等を全て無視
し、各関節２−１．２−２．・・・２−ｎ毎に閉ループ
制御系をイ■［み、それぞれのループにＰＩＤ制御器３
−１．３−２．・・・３−ｎ等を介挿させた制御装置４
を用いる方式である。

しかし、この制御装置４ては、マニピュレータ１を嘉速
運動させようとすると、無視されていた非線形力や各自
由度間の干渉力の影響が大きくなり１制御性能が劣化す
ると言う問題かあった。

また、その２つ目は、第４図に示すように、マニピュレ
ータ１の動特性モデルか完全に既知であるとし、各関節
２−１．・２−ｎの角度、角速度等を用いて非線形補償
要素５て非線形力および干渉力を計算で算出し、その計
算結果を用いて各関節毎に非線形補償を行なう＋Ｉ’ｌ
＋御装置６を用いる方式である。

しかし、この制御装置６ではマニピュレータ１の動特性
が既知でなければならないところに問題がある。たとえ
ば、同一構成の多自由度マニピュレータであっても１通
常、それぞれの動特性モデルには多少のばらつきがある
。また、動特性モデルの中のパラメータのなかには経年
変化によって変動するものもある。したがって、この制
御装置６では、各マニピュレータ毎にその制御性能が異
なったり、またパラメータの変動によって制御性能が劣
化するなどの問題があった。

また、その３つ目は、第５図に示すように、マニピュレ
ータ１の動特性モデルを同定器７でオンラインで同定し
、この同定結果で適応的非線形補償器８を制御して非線
形補償を行なうようにした制御装置９を用いる方式であ
る。

しかし、この制御装置９では、動特性モデルの同定と制
御とを同時に行なっているため、計算量が非常に多くな
り、この結果、制御周期の短縮化が困難であると言う問
題があった。

（発明が解決しようとする問題点） 上述の如く、従来の制御装置は、マニピュレータの非線
形力や干渉力の影響を受けたり、あるいは動特性モデル
のモデル誤差などによって制御性能が劣化したり、ある
いはまた制御用演算の演算回数の増大化を招き制御周期
の短縮化か図れないなどの問題があった。

そこで本発明は、マニピュレータの非線形力８干渉力、
動特性モデルのモデル誤差等の影響による制御性能の劣
化を防止できるとともに制御周期の短縮化を実現できる
多自由度マニピュレータの制御装置を提供することを目
的としている。

［発明の構成コ （問題点を解決するだめの手段） 本発明に係る制御装置は、指定した時にマニピュレータ
の動特性モデルを同定する動特性モデル同定部と、この
同定部の同定結果にしたがって各制御定数を演算する制
御用パラメータ演算部と。

この演算部の演算結果を用いて前記マニピュレータをリ
アルタイムでｊｌ；ＩＩ　Ｊするソフトウェアサーボ部
とを備えている。

（作用） 一般に、制御すべきマニピュレータが変わると、その動
特性モデルも変わる。また、同じマニピュレータでも、
負荷変動や経年変化によって。

その動特性モデルが変わる。

本発明の制御装置では１　たとえばオペレータが指定し
たとき、制御すべきマニピュレータの動特性モデルの同
定が行われ７その結果から制御用／＜ラメータか決定さ
れる。そして、このパラメータから制御しようとするマ
ニピュレータに適したソフトウェアサーボ系か構成され
、このサーボ系によってマニピュレータかリアルタイム
で制御される。

（実施例） 以下１本発明の実施例を図面の簡単な説明する。

第１図は本発明の一実寵例に係る制御装置で２自由度の
マニピュレータを制御している例を示すものである。

すなわち１図中１１は制御対象である２自由度のマニピ
ュレータであり、３１はその制御装置である。

マニピュレータ１１は、ベース１２に回転ジヨイント部
１３を介して第１のリンク１４の一端側か支持され、こ
の第１のリンク１４の他端側に回転ンヨイント部１５を
介して第２のリンク１６か支ｒ１ｊされている。そして
、各回転ジヨイント部１３．１５にはそれぞれ独立にア
クチュエータ１７．１８と角度センサー１９．２０とか
取り付けられている。

一方、制御装置３１は、角度センサー１９゜２０の出力
を導入して角度θ１．θ２および角速度ｂ１．δ２を検
出する検出部３２と、アクチュエータ１７．１８へパワ
ーを供給するドライバ部３３と、後述する；ｔｉｌ＋御
用パラメータ演算部３７から与えられた仮想パラメータ
を用い１公知のＮｅｗｔｏｎ−Ｅｕｌｅｒ法でソフトウ
ェアサーボ系を溝成し。

常時は検出部３２の出力θ１．θ２．θ１．θ２と人力
信号θｄ１．θｄ２とに対応させてリアルタイムでドラ
イバ部３３を制御するソフトウェアサーボ部３４と、モ
ート切換え部３５が図示状態に切換えられたときマニピ
ュレータ１１の動特性モデルを同定するパラメータ同定
部３６と、この同定部３６の同定結果を用いて個々の仮
想パラメータを演算し、これをソフトウェアサーボ部３
４へ与える制御用パラメータ演算部３７とで構成されて
いる。なお、モード切換え部３５は、常時は図示状態と
は反対側に切換えられており、たとえばオペレータの操
作によって図示状態に所定期間たけ切換えられる。

このような構成であると、マニピュレータ１１をセット
したときや使用中のマニピュレータ１１について定期的
または不定期的に、モード切換え部３５を図示状態に切
換えることによって、マニピュレータの個体差による動
特性の違いや動特性変化に対処することかでき、しかも
リアルタイムで計算する回数を少なくして制御周期の短
縮化を図ることができる。

すなわち１今、組方方向が鉛直下向き方向であるとし、
アクチュエータ１７の入力をｕｌ　＋　アクチュエータ
１８の入力をＢ２　、角度センサ１９の出力をθ１．角
度センサ２０の出力をＢ２とすると、マニピュレータ１
１の運動方程式は次のようになる。

−２（Ｒ２ｓｉｎ　Ｂ２）　　ｂ１δ２−（Ｒ２ｓｉｎ
　　　Ｂ２　）Ｂ２　　＋Ｒ，ｇｓ＋ｎ　θ［＋Ｒ２ｇ
ｓｉｎ（Ｂ１　＋０２　）＋Ｂ、　　ｂ　、　　＋　　
ｆ、　　（Ｍ、　　）−ｕ。

・・（１） ［Ｊ２＋Ｒ２ｃｏｓ　　Ｂ２　コ”９１　　＋［Ｊ２］
　’ｅ９　　＋（Ｒ２ｓｉｎθ２　）θ１＋１１２ｇ５
ｉｎ（θ１＋０２　）”Ｂ２１９２”ｆ’２（６＋２）
”　Ｌｌ　２ココテ、　Ｊｌ、Ｊ２．Ｒ，、Ｒ２，ｒｌ
（ｆ９１）　、ｆ’２（Ｂ２）　ハ。

Ｊｌ−１１”１ＴＩＬＩ’２”　ｍ２１゜Ｊ２＝　＋１
″１１１ｒ２ Ｒ１””　（Ｉｎ　ｔ　”　ｍ　２　）　Ｉ　１．　”
　ｍ　１ｒ　ｌＲ２−ｍ２ｒ２ である。ただし、上記の各式において。

１１：第１リンクの重心に関する慣性モーメント ｍｉ＋第１リンクの質量 ｒ１１第１　リンクの根元から第ｉ　リンクの重心まで
の距離 １１；第１　リンクの長さ ｒｉ：動摩擦トルク ｇ　：組方加速度 である。

しかして１今７回転ジヨイント部１３を第１関節とし２
回転ンヨイント部１５を第２関節とし。

モード切換え部３５が図示状態に切換えられると。

パラメータ同定部３６は次のようなアルゴリズムにした
がって動作する。

５ｔｅｐｌ）静的試験 （１−１，）θ１＝９０°、θ２−９０°となるように
各関節を動かす。

（Ｌ−２）第１関節をブレーキで固定し、第２関節につ
いてはジヨイントトルクで静力学的にほぼ吊り有ってい
る状態にする。このときの力学的な吊り合い方程式は。

Ｒｇｓｉｎ（Ｂ１　＋θ２　）十ｒ２ｃ＝ｕ　２　　　
　　　・・・（３）とｔヱる。

（Ｌ−３）　第２関節のトルクを徐々に上げて（下げて
）、第２のリンク１６か動き出す直前のトルクｕ”　（
ｕ−）を測定し１次式よりＲ２，および［２ｃを求める
。

１ン２＝（ｕ　；”　ＩＪ門）／□２ｇ５ｉｒ＋（θ１
＋θ２）　　　　−（４）Ｉｒ２！＝しｕ；−ｕＨ）　
／　２１　　　・１５）（１−４）第２関節をブレーキ
で固定し、第１関節についてはジヨイントトルクによっ
て静力学的にほぼ吊り合っている状、態にする。このと
きの力学的な吊り合い方程式は、 Ｒ，ｇｓｉｎθ、　＋Ｒ２ｇｓｉｎ（θ１＋０２）＋ｆ
’１ｃ＝ｕ　。

・・（６） となる。

（Ｌ−５）第１関節のトルクを徐々に上げて（下げて）
、第１のリンク１４が動き出す直前のトルクｕｘ（ｕｔ
）を測定し１次式よりＲ，、ｆ’１ｃを求める。

Ｒ−［（ｕ１＋ｕｌ）／２−Ｒ２ｇｓｉｎ（θ１＋θ２
　）］　／　ｇｓｉｎθ１・・・（７） （１−８）以上の動作で、　Ｒ２，ｆ２ｃ、ＲＬ、ｆ’
、ｃが算出される。

５ｔｅｐ２）等角速度運動試験 （２−１）第１関節をブレーキで固定し、第２関節に。

ｕ２−　Ｔ２ｕ（ｔ）＋Ｒ２ｇｓｉｎ（θＩ　＋θ２）
・・・（９）なるトルクを加える。ここで　ｒｌは適当
な整数。

ｕ（ｔ）はステップ関数である。（９）式の右辺の全パ
ラメータは全て既知となっている。したかって、粘性抵
抗の存在により、（９）式の入力で等角速度運動が得ら
れる。このときの運動方程式は。

Ｂ２Ｏ２”ｆ’２−　　Ｔ２　　　　　　−　（１０）
となる。

（２−２）　Ｔ２≠Ｔ２なるＴ２を選び、（９）式と同
様な人力を加えると次の運動方程式が得られる。

′″　２ Ｂ２Ｏ２”ｆ’２””ｒ２”’　＜１１）（２−３）　
（１０）、（１１）式より１次式を得る。

（２−４）第２関節をブレーキで固定し、第１関節（２
−５）前記（２−１）〜（２−３）と同様の試験を行な
って次式を得る。

（２−８）以上のようにして、　　（１２＞、（１４）
式より。

Ｂ、、Ｂ２．ｒｌ、　ｒ２を求める。

５ｔｅｐ３）角加速度運動試験 （３−１）第１関節をブレーキで固定し、第２関節に任
意人力。

ｕ２”　ｖ２（ｔ）　　　　　　　　・・（１５）を加
える。このときの運動方程式は、（２）式より。

−Ｖ２（１）　　　・・・（１ｅ） となる。（１６）式から次式を用いてＪを求める。

ま たたし、　　ｔｏは運動開始時刻、　　ｔｌは運動終了
時刻である。（１７）式において、右辺に現われるパラ
メータ、角度データ、角速度データ、トルクデータは全
て既知である。したかって、　　Ｊ２を求めることがで
きる。

（３−２）第２関節をブレーキで固定（θ２−０°）し
、第１関節に任意の入力。

ｕ＋　−Ｖ＋　（ｔ）　　　　　　　　・・・（１８）
を加える。このときの運動方程式は、（１）式より。

［：Ｊ１＋Ｊ２＋２Ｒ２コ　　Ｂ１　＋Ｂ１θ１　＋Ｒ
１ｇｓｉｎθ１＋Ｒ，，ｇｓｉｎ（θ１＋θ２　）＋４
１ｓｇｎｅ　ｒ　−Ｖ■（ｔ）・・（１９） となる。これから次式を用いてｊｌを求める。

−Ｒｇｓｉｎθ　−ＲｇＳｉｎ（θ　＋θ　）−ｒｓｇ
ｎθ　ｌ　ｄ　ｒ　−Ｊ２−２１’？２　　　（２０）
このく２０）式の右辺は全て既知である。したかって。

Ｊｌ　を求めることかできる。

二のようにしてパラメータ同定部３６はマニピュレータ
１１の動特性モデルを同定する。

続いて、制御用パラメータ演算部３７は、前記パラメー
タ同定部３６で得られた動特性モデルに基つき、ソフト
ウェアサーボ部３４て用いる制御用のパラメータを演算
する。前述のように、パラメータ同定部３６で同定でき
るのはｌｉ、ｍｉ、ｒｉなどの物理パラメータそのもの
ではなく。

’ｌ”　’ｌ＋ｆｆ１ｌ’ｌ＋Ｉｎ２１１”　２＝　’
２＋ｆｆ１２’２′Ｒ１＝　’ｌ’ｌ’Ｒ−（ｍｔ”ｍ
２）　Ｉｔ”１ｌｌｔｒｔのようにそれらが纏められた
ものである。制御用パラメータ演算部３７は。

この纏められたパラメータＪ１．Ｊ２．Ｉ？■、Ｒ２を
もう一度１分解するための演算を次のようなアルゴリズ
ムにしたがって行なう。

５ｔｅｐ−Ｌ）　　　ｍ　ｍ　ｍ　−１−（２１）ｓｔ
ｅｐ−２）　　　　ｒｌ−Ｒ１−２１１−（２２）ｒ２
−　Ｒ２−（２３） ｓｔｅｐ−３）　　　　Ｉ　−Ｊ　−ｒ　−ｌ　　　　
・＝　（２４）１２−Ｊ２−ｒ２・−（２５） この演算によって求められたパラメータが仮想パラメー
タとしてソフトウェアサーボ部３４へ与えられる。そし
て、ソフトウェアサーボ部３４は上記仮想パラメータを
用いてＮｅｗｔｏｎ−Ｅｕｌｅｒ法でソフトウェアサー
ボ系を構成する。

このように、モード切換え部３５を選択的に切換えるこ
とによって、マニピュレーター１の動特性モデルを同定
でき、しかも物理的な制御パラメータを求めることがで
き、これによってソフトウェアサーボ部３４の制御パラ
メータを変更することができる。したがって、マニピュ
レータの個体差による動特性の違いや動特性の変化に容
易に対処できる。また、制御パラメータを求めるときだ
けパラメータ同定部３６を動作させるようにしているの
で、リアルタイムで計算する演算回数を少なくでき、そ
の結果、制御周期を短縮化でき制御性能を向上させるこ
とができる。

なお１本発明は上述した実施例に限定されるものではな
い。たとえば第２図に示すように、ソフトウェアサーボ
部３４で発生した各関節の目標角θｄとマニピュレータ
の実際の角度θとの差を減算器３８で求め、この差が一
定値を越えたときモード切換えを促すアラームを発生す
るモデル誤差検出部３９を設けるようにしてもよい。こ
のようにすると使い易さを一層向上させることかできる
。

［発明の効果］ 以上述べたように１本発明によれば、マニビュ８．　レ
ークの個体差による動特性の違いや動特性の変化に速や
かに対処できるとともに制御性能の向上化を図れる多自
由度マニピュレータの制御装置を提供できる。

【図面の簡単な説明】

第１図は本発明の一実施例に係る制御装置のブロック的
構成図、第２図は本発明の別の実施例に係る制御装置に
おける要部のブロック的構成図２第３図から第５図は従
来の制御装置の構成をそれぞれ説明するための図である
。 ］１・・・マニピュレータ、３１・・・制御装置、３２
・・・検出部、３３・・・ドライバ部、３４・・・ソフ
トウェアサーボ部、３５・・・モード切換え部、３６・
・・パラメータ同定部、３７・・・制御用パラメータ演
算部。 ３９・・・モデル誤差検出部。 出願人代理人　弁理士　鈴江武彦 第１図 ｅｄ１ｅｄ２ 第２因 第３因

Claims (1)

【特許請求の範囲】
1. 多自由度を有するマニピュレータを制御する制御装置に
   おいて、指定した時にマニピュレータの動特性モデルを
   同定する動特性モデル同定部と、この同定部の同定結果
   にしたがって各制御定数を演算する制御用パラメータ演
   算部と、この演算部の演算結果を用いて前記マニピュレ
   ータをリアルタイムで制御するソフトウェアサーボ部と
   を具備してなることを特徴とする多自由度マニピュレー
   タの制御装置。