JPH04306712A - マニピュレータの制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は，マニピュレータのワー
ク取扱い部を目標とする動力学インピーダンスで動作さ
せるためのトルクを上記ワーク取扱い部に与えるいわゆ
るインピーダンス制御を行う制御装置に係り，特にワー
ク取扱い部をその目標軌道に精度良く追随させることの
できるマニピュレータの制御装置に関する。

【０００２】

【従来の技術】上記したようなマニピュレータの制御装
置としては，論文”Ｉｍｐｅｄａｎｃｅ　Ｃｏｎｔｒｏ
ｌＡｎ　Ａｐｐｒｏａｃｈ　ｔｏ　Ｍａｎｉｐｕｌａｔ
ｉｏｎ”，　　ＡＳＭＥ　Ｊ．　ｏｆ　Ｄｙｎａｍｉｃ
　Ｓｙｓｔｅｍｓ，　　Ｍｅａｓｕｒｅｍｅｎｔ，ａｎ
ｄ　Ｃｏｎｔｒｏｌ，　Ｎ．Ｈｏｇａｎ，　Ｖｏｌ．１
０７−１，　ｐｐ．１／２２　（１９８５）　に開示さ
れた装置がある。上記制御装置に対応する制御ブロック
図を図４に示す。同図において，マニピュレータの制御
装置１ａ　は，マニピュレータ２０のワーク取扱い部の
目標とする動力学的な挙動を表現する動力学インピーダ
ンスの設定部３を備えている。上記動力学インピーダン
ス設定部３には，それぞれ目標弾性ｍにより除されてな
る目標弾性係数行列４（ｋ／ｍ），目標粘性係数行列５
（ｂ／ｍ），目標慣性係数行列６（１／ｍ）が予め設定
されている。又，上記マニピュレータ２０のワーク取扱
い部には，外部環境からの反力Ｆのベクトルを検出する
，例えば歪センサよりなる力センサ１４が設けられてい
る。上記マニピュレータ２０の動作は次の運動方程式で
与えられる。 　　　　　　　　Ｍ（θ）・ｄ２　θ／ｄｔ２　＋Ｃ（
ｄθ／ｄｔ，θ）　　　　　　　　　　　　　　　　＝
τ＋Ｊ（θ）Ｔ　・Ｆ＋Ｄ　　　　　　　　　　　　　
　　　　　　　　　…（１）ここで，　　　　　　　　
　　　　　　　　　　θ：関節角（ベクトル）Ｍ（θ）
：慣性項（行列） Ｃ（ｄθ／ｄｔ，θ）：コリオリ力，求心力，粘性，ク
ーロン摩擦等の非線形項（行列） τ：関節モータに与えられるトルク Ｄ：トルクリップル等の外乱 Ｊ（θ）Ｔ　：ヤコビアンの転置行列 Ｆ：ワーク取扱い部が外部環境から受ける反力そして，
上記制御装置１ａ　は，マニピュレータ２０の関節モー
タに設けられた角度センサ２１より出力された関節の揺
動角（角度θ）に基づいて，上記ワーク取扱い部の状態
量としての位置ｘ，速度ｄｘ／ｄｔを微分器３４，変換
部２を用いて演算する。更に，上記制御装置１ａ　は，
上記位置ｘと目標位置設定部７より設定された目標位置
ｘ０　との偏差ｘ−ｘ０　と，上記演算された速度ｄｘ
／ｄｔと，上記力センサ１４より出力された反力Ｆと，
上記動力学インピーダンス設定部３の各係数行列４，５
，６とより設定インピーダンスｇを演算するようになっ
ている。上記設定インピーダンスｇは，ワーク取扱い部
に指令として与えられる加速度に相当し，変換部８にお
いて上記角度θの関数に変換される。即ち，上記設定イ
ンピーダンスｇを求める構成が本発明にいう加速度演算
手段である。上記変換部８では，次の（２）式を用いて
上記設定インピーダンスｇに相当する加速度ｄ２　ｘ／
ｄｔ２　を角加速度ｄ２　θ／ｄｔ２　に変換する。 　　　　　　　　ｄ２　θ／ｄｔ２　＝Ｊ（θ）−１｛
ｄ２　ｘ／ｄｔ２　−（ｄθ／ｄｔ）２　　　　　　　
　　　　　　　　　　　　　　　　　　・ｄＪ（θ）／
ｄθ｝　　　　　　　　　　　　　　　　　　…（２）
又，慣性項演算部３１はワーク取扱い部に係る慣性項を
求めるための数学モデルを備えている。そして，上記変
換部８を経て慣性項演算部３１に入力された角度θと上
記数学モデルとに基づいて，そのときのマニピュレータ
２０の慣性項の推定値Ｍ＊　（θ）が求められ，この推
定値Ｍ＊　（θ）と上記変換された角加速度ｄ２　θ／
ｄｔ２　とにより慣性力の推定値Ｍ＊　（θ）・ｄ２　
θ／ｄｔ２　が求められる。即ち，上記変換部８及び慣
性項演算部３１が，上記演算された加速度指令値ｄ２　
ｘ／ｄｔ２　と上記関節の角度θ（位置），角加速度ｄ
θ／ｄｔに基づいて上記加速度指定値を実現するために
必要なマニピュレータ２０の慣性力の推定値Ｍ＊　（θ
）・ｄ２　θ／ｄｔ２　を演算する推定値演算手段であ
る。一方，非線形補償演算器３０はコリオリ力，求心力
，粘性，クーロン摩擦等の項（以下，非線形項という）
を求めるための数学モデルを備えている。上記非線形項
は，上記関節の角度，角速度に基づいて上記数学モデル
を解いて得た推定値Ｃ＊　（ｄθ／ｄｔ，θ）である。 そして，上記慣性力の推定値Ｍ＊　（θ）・ｄ２　θ／
ｄｔ２　と，非線形項の推定値Ｃ＊　（ｄθ／ｄｔ，θ
）と，上記力センサ１４により検出され変換部３２によ
り角度θの関数に変換された反力Ｆ・Ｊ（θ）Ｔ　とに
基づいて，ワーク取扱い部の動力学特性が上記動力学イ
ンピーダンス設定部３に設定された目標弾性・粘性・慣
性ｋ，ｂ，ｍになるようなトルクτが求められる。ちな
みに，上記制御装置１ａ　に設定され，上記トルクτに
対応する演算式を以下の（３）式に示す。 　　　　　　　　τ＝Ｃ＊　（θ，ｄθ／ｄｔ）−Ｊ（
θ）Ｔ　・Ｆ＋Ｍ＊　（θ）Ｊ（θ）−１　　　　　　
　　　　　　・｛−（ｄθ／ｄｔ）２　・ｄＪ（θ）／
ｄθ＋ｍ−１｛Ｆ−ｂ　　　　　　　　　　　　・ｄｘ
／ｄｔ−ｋ（ｘ−ｘ０　）｝｝　　　　　　　　　　　
　　　　　　　…（３）但し，ｘ＝Ｌ（θ），ｄｘ／ｄ
ｔ＝Ｊ（θ）・ｄθ／ｄｔが成立し，Ｌ（θ）は関節座
標系から直行座標系への変換行列であるとする。又，上
記設定インピーダンスｇは次式で表わされる。 　　　　ｇ＝ｇ（Ｆ，ｄｘ／ｄｔ，ｘ，ｄ２　ｘ０　／
ｄｔ２　，ｄｘ０　／ｄｔ，ｘ０　）　　　　　　＝ｍ
−１・｛Ｆ−ｂ・ｄｘ／ｄｔ−ｋ（ｘ−ｘ０　）｝　　
　　　　　　　　…（４）上記制御装置１ａ　により制
御されるマニピュレータ２０を用いて，予め設定された
目標軌道に対するワーク取扱い部の倣い動作を行った例
を図５に示す。同図において，ワーク取扱い部の目標軌
道Ｐ０　上には障害物４０が存在している。上記ワーク
取扱い部は，予め設定された目標軌道Ｐ０　に追随する
ように，関節モータの駆動により始点Ｓから図中時計廻
りに移動する。このときのワーク取扱い部の実際の移動
軌跡を実線Ｐ２　で示す。そして，ワーク取扱い部が障
害物４０に接触すると，ワーク取扱い部は障害物４０の
表面に沿って移動し，同時に障害物４０からの反力Ｆを
受ける。上記反力Ｆはワーク取扱い部の力センサ１４に
より検出される。但し，図中反力Ｆを示す複数の直線は
，それぞれの直線が交差する障害物４０上からの反力の
ベクトルを示す。

【０００３】

【発明が解決しようとする課題】ところで，上記従来の
マニピュレータの制御装置１ａ　において，上記慣性項
や非線形項の各推定値Ｍ＊　，Ｃ＊　を用いてトルクを
演算する場合，演算されたトルク（（３）式）には，上
記各推定値の推定誤差ΔＭ（θ）（＝Ｍ（θ）−Ｍ＊　
（θ）），ΔＣ（θ）（＝Ｃ（ｄθ／ｄｔ，θ）−Ｃ＊
　（ｄθ／ｄｔ，θ））（以下，モデル化誤差という。 ただし，Ｍ（θ），Ｃ（ｄθ／ｄｔ，θ）は未知であっ
て，上記各項の真値を示す。）が当然ながら含まれ上記
反力Ｆにも反映される。そのため，上記反力Ｆを用いて
演算される加速度は上記モデル化誤差により見掛け上小
さく演算される。従って，上記ワーク取扱い部の移動軌
跡Ｐ２　は図示の如く目標軌道Ｐ０　からずれるという
問題があった。そこで，上記制御装置１ａ　では，上記
設定部３の各係数行列４，５，６（ｋ／ｍ，ｂ／ｍ，１
／ｍ）の値を大きく設定変更することにより，上記目標
軌道Ｐ０　に対する移動軌跡の追随精度を高くすること
ができる。例えば，特に目標弾性ｋの値を図５の場合と
比べて３倍大きく設定したときのワーク取扱い部の倣い
動作を図６に示す。図示のように，上記移動軌跡Ｐ３　
は目標軌道Ｐ０　に近付いており，その追随精度が向上
している。 しかしながら，上記ｋの値の設定変更により，ワーク取
扱い部が受ける反力Ｆも約３倍の大きさになる。これで
は，上記ワーク取扱い部の目標軌道に対する追随精度と
所定の動力学特性に対応した動作とを同時に満足させる
ことができない。従って，本発明の目的とするところは
，ワーク取扱い部が設定された目標軌道に精度良く追随
し且つ設定された動力学特性で動作することのできるマ
ニピュレータの制御装置を提供することにある。

【０００４】

【課題を解決するための手段】上記目的を達成するため
に，本発明が採用する主たる手段は，その要旨とすると
ころが，複数のリンクを関節を介して連結してなるマニ
ピュレータの，あるリンクに設けられたワーク取扱い部
の位置，速度，反力と上記ワーク取扱い部の設定された
動力学特性とに基づいて上記ワーク取扱い部の加速度指
令値を演算する加速度演算手段と，上記加速度指令値を
実現するために必要な上記マニピュレータの慣性力の推
定値を上記加速度指令値と上記関節の位置，速度に基づ
いて演算する推定値演算手段とを少なくとも備え，少な
くとも上記慣性力の推定値と上記反力とによって上記関
節を駆動する駆動源のトルクを演算し出力する制御装置
において，上記ワーク取扱い部の加速度を検出する加速
度検出手段と，上記演算された加速度指令値と上記加速
度検出手段により検出された加速度との差にゲインを乗
じた値を上記演算された加速度指令値に加算する制御ル
ープを具備してなる点に係るマニピュレータの制御装置
として構成されている。

【０００５】

【作用】本発明のマニピュレータの制御装置では，マニ
ピュレータの関節を駆動する駆動源に出力されるトルク
の演算の際に，推定値演算手段により演算されたワーク
取扱い部の慣性力の推定値が少なくとも用いられる。推
定値というのは一般的に推定誤差を含んでいるので，ワ
ーク取扱い部が上記トルクに基づいて駆動された結果そ
のときのワーク取扱い部の加速度や外部から受けた反力
にも上記推定誤差が反映される。即ち，加速度検出手段
により検出された加速度と上記加速度演算手段により演
算された加速度指令値との差は上記推定誤差や例えば外
乱を表わす。そこで，上記検出された加速度と上記演算
された加速度指令値との差にゲインを乗じ，このゲイン
を設定変更することにより上記慣性力の推定値に含まれ
る推定誤差を調整することができる。従って，上記制御
装置は，ワーク取扱い部が設定された動力学特性に基づ
く動作になるように，ワーク取扱い部を精度良く駆動制
御することができる。

【０００６】

【実施例】以下添付図面を参照して，本発明を具体化し
た実施例につき説明し，本発明の理解に供する。尚，以
下の実施例は，本発明を具体化した一例であって，本発
明の技術的範囲を限定する性格のものではない。ここに
，図１は本発明の一実施例に係るマニピュレータを示す
斜視図，図２は上記マニピュレータの制御装置を示すブ
ロック構成図，図３は上記マニピュレータのワーク取扱
い部の目標軌道に対する倣い動作を示す説明図である。 但し，図４に示した上記従来のマニピュレータの制御装
置１ａ　と共通する要素には，同一の符号を使用すると
共に，その詳細な説明は省略する。本実施例に係るマニ
ピュレータ２０は，図１に示すように，図示せぬ基台側
に固定された第１の関節モータ９ａ　と，この関節モー
タ９ａ　の回転駆動軸に固設された第１のリンク２３と
，このリンク２３の先端に固設された第２の関節モータ
９ｂ　と，この関節モータ９ｂ　の回転駆動軸に設けら
れた第２のリンク２４と，リンク２４の先端に配備され
たワーク取扱い部２５とよりなっている。上記関節モー
タ９ａ　及び９ｂ　には，それぞれ角度センサ２１とし
てのエンコーダ（３６００００ｐ／回転）が一体的に設
けられており，上記基台に対して揺動するリンク２３の
角度θａ　やリンク２３の軸芯に対して揺動するリンク
２４の角度θｂ　を検出し出力するようになっている。 そして，上記リンク２３及び２４のリンク長は予め決ま
っているので，これらの角度θａ　及びθｂ　により上
記ワーク取扱い部２５の，例えばｘ方向の位置が決定さ
れる。なお，図１中の関節モータ９は上記関節モータ９
ａ　及び９ｂ　のいずれも含むものであって，同じく角
度θは上記角度θａ　及びθｂ　を表わしている。そし
て，上記マニピュレータ２０を制御する制御装置１は，
図２に示すように，上記従来の制御装置１ａ　と基本的
構成をほぼ同様とし，上記従来の制御装置１ａ　と構造
上異なる点は，上記角度センサ２１により検出された角
度θに基づいて，角加速度ｄ２　θ／ｄｔ２　，加速度
ｄ２　ｘ／ｄｔ２　を演算する微分器３３及び変換部２
と，上記目標インピーダンス設定部３からの設定インピ
ーダンスｇ（加速度指令値）と上記変換部２からの加速
度ｄ２　ｘ／ｄｔ２　との差（ｇ−ｄ２　ｘ／ｄｔ２　
）を求める減算器１７と，上記差（ｇ−ｄ２　ｘ／ｄｔ
２　）に乗じられるゲインＧを設定するゲイン設定部１
５と，上記差にゲインＧを乗じた値Ｇ（ｇ−ｄ２　ｘ／
ｄｔ２　）を上記設定インピーダンスｇに加算する加算
器１６とを備え，上記減算器１７，ゲイン設定部１５，
加算器１６よりなる制御ループ構成が採用されたことで
ある。又，上記制御装置１では，ワーク取扱い部２５の
目標速度ｄｘ０　／ｄｔを設定する目標速度設定部１２
と，目標加速度ｄ２　ｘ０　／ｄｔ２　を設定する目標
加速度設定部１３とが設けられ，上記設定インピーダン
スｇの演算において上記変換部２からの位置，速度とそ
れぞれの目標値との偏差と，上記目標加速度設定部１３
からの目標加速度とが用いられるようになっている。従
って，上記設定インピーダンスｇは次の（５）式のよう
に示される。 　　　　　　　　ｇ＝ｄ２　ｘ０　／ｄｔ２　＋ｍ−１
｛Ｆ−ｂ（ｄｘ／ｄｔ−ｄｘ０　／ｄｔ）　　　　　　
　　　　　　−ｋ（ｘ−ｘｏ　）｝　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　…（
５）そして，上記設定インピーダンスｇは変換部８にお
いて（２）式により角加速度に変換される。

【０００７】一方，慣性項演算部３１，非線形補償演算
器３０は上記関節モータ９の角度センサ２１から出力さ
れた角度θ，及びこの角度θから微分器３３，３４によ
り演算された角速度，角加速度に基づいて，慣性項の推
定値Ｍ＊　（　θ）と，上記非線形項の推定値Ｃ＊　（
θ，ｄθ／ｄｔ）を求める。即ち，上記角度センサ２１
，微分器３３が上記ワーク取扱い部２５の加速度を検出
する加速度検出手段である。そして，上記関節モータ９
に与えられるべきトルクτは次の（６）式により求めら
れる。 即ち，（６）式が上記マニピュレータ２０の制御則を表
わす。 　　　　　　　　τ＝Ｃ＊　（θ，ｄθ／ｄｔ）−Ｊ（
θ）Ｔ　Ｆ＋Ｍ＊　（θ）Ｊ（θ）−１　　　　　　　
　　　　　｛−（ｄθ／ｄｔ）２　・ｄＪ（θ）／ｄθ
＋ｇ　　＋Ｇ（ｇ−ｄ２　ｘ　　　　　　　　　　　　
／ｄｔ２　）｝　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　…（６）従
って，上記ワーク取扱い部２５の加速度ｄ２　ｘ／ｄｔ
２　は，（１）式，（５）式及びモデル化誤差を示す式
（ΔＭ＝Ｍ（θ）−Ｍ＊　（θ），ΔＣ＝Ｃ（ｄθ／ｄ
ｔ，θ）−Ｃ＊　（ｄθ／ｄｔ，θ））より，次の（７
）式のように示される。 　　　　　　　　ｄ２　ｘ／ｄｔ２　＝ｇ＋｛Ｉ＋Ｇ｝
−１｛Ｊ（θ）｝ｍ−１｛ΔＭ（θ）　　　　　　　　
　　　　　　　　　　　　　　　　・ｄ２　θ／ｄｔ２
　＋ΔＣ（ｄθ／ｄｔ，θ）−Ｄ｝　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　　…（７）但し，Ｉは単位行列である。上記（
７）式において，右辺第２項は，モデル化誤差及び外乱
Ｄによる加速度ｄ２　ｘ／ｄｔ２　への影響を表わして
いる。そこで，上記ゲイン設定部１５からゲインＧを設
定変更することにより，上記モデル化誤差及び外乱の影
響を調整することができる。例えば，上記ゲインＧを十
分大きくすることにより上記モデル化誤差及び外乱の影
響を極めて小さくすることが可能で，これによって，演
算された加速度ｄ２　ｘ／ｄｔ２　を上記設定インピー
ダンスｇに限りなく近づけることができる。なお，上記
変換部２，８，３２，動力学インピーダンス設定部３，
非線形補償演算器３０，慣性項演算部３１，微分器３３
，３４，ゲイン設定部１５，加算器１６，減算器１７は
，図外のＣＰＵ上で動くソフトウエアにより全て構成さ
れ，プログラムとして同じく図外のメモリに格納されて
いる。また，上記マニピュレータ２０は制御装置１のＣ
ＰＵにより４ミリ秒のサンプリング周期で制御される。 なお，上記非線形補償算器３０の数学モデルにおいて，
関節モータ９の駆動軸の粘性とクーロン摩擦とは停止中
の駆動軸に力（トルク）をステップ状に変えて与え，上
記駆動軸が動き始めた時のトルクを実測しこれに基づい
て予め求めた。また，上記慣性項に係る係数は，上記関
節モータ９に白色ノイズ状のトルクを与え，このとき同
時に計測して得た。

【０００８】上記した構成のマニピュレータ２０の制御
装置１を用いて，ワーク取扱い部２５の目標軌道に対す
る倣い動作を行なった例に付き図３に示す。この場合，
上記制御装置１には，上記ワーク取扱い部２５の移動す
る目標位置ｘ０　として従来と同様の目標軌道Ｐ０　が
予め設定されている。又，上記動力学インピーダンス設
定部３の目標弾性ｋ，目標粘性ｂ，目標慣性ｍとしては
，例えばｋ＝１０００Ｎ／ｍ，ｂ＝２００Ｎｓ／ｍ，ｍ
＝１０Ｋｇが設定されている。これらは従来（図５）に
おいて設定されたものと同じ値である。更に，上記ゲイ
ンＧとしては，Ｇ＝２Ｉが設定されている。従来手法で
あれば上記ゲインＧはＧ＝０に相当するが，上記ゲイン
Ｇ（２Ｉ）の設定により，上記（７）式の右辺第２項の
値が従来と比べて１／３になり，制御精度が３倍になる
。そこで，上記マニピュレータ２０の関節モータ９に上
記（６）式により求めたトルクτを与えると，ワーク取
扱い部２５が始点Ｓから移動を始める。このとき，上記
ワーク取扱い部２５が移動した移動軌跡Ｐ１　及びワー
ク取扱い部２５が受けた反力Ｆを同図に示す。それによ
れば，上記マニピュレータ２０のワーク取扱い部２５は
，従来（図５）と比べて，同等の反力Ｆを障害物４０か
ら受けつつ目標軌道Ｐ０　により近い移動軌跡Ｐ１　で
移動している。上記したように，本実施例のマニピュレ
ータの制御装置１は，上記モデル化誤差や外乱の影響を
ゲインＧの調整によって任意に小さくすることにより，
ワーク取扱い部２５を，設定された動力学特性に対応し
た動作で駆動し得る制御手法を採用しており，ワーク取
扱い部２５の目標軌道追随精度を飛躍的に改善すること
ができる。しかも，従来と比べて，ＣＰＵ演算に関して
減算（減算器１７），加算（加算器１６），乗算（ゲイ
ン設定部１５）をひとつずつ増やすだけですむので，Ｃ
ＰＵ負荷は大きく増大しない。又，一般的に設定インピ
ーダンスｇの設定範囲はワーク取扱い部２５の位置，速
度，加速度のフィードバック量の上限により制限される
。しかしながら，本実施例装置によれば，ゲインＧを変
化させることにより設定インピーダンスｇと上記フィー
ドバック量との関係を従来よりも自由に決定することが
できる。従って，上記ゲインＧを調整してフィードバッ
ク量の上限を超えないようにすることにより，設定イン
ピーダンスｇの設定範囲を広げることができる。一方，
ワーク取扱い部２５が，例えば硬い外部環境と接触する
場合，上記目標インピーダンス設定部３の目標弾性ｋの
値を小さく設定する必要がある。このような場合，従来
手法によれば，上記目標弾性ｋを小さく設定すると目標
軌道追随精度が極めて悪くなる。ところが，本願手法に
よれば，上記目標弾性ｋの値が小さくとも上記ゲインＧ
を大きく設定することにより，良好な追随精度でインピ
ーダンス制御を行うことができる。従って，硬い外部環
境と接触しながら高い追随精度が要求される作業や，例
えば破損しやすい部品の組立作業を精度良く行うことが
できる。

【発明の効果】本発明は上記したように構成されている
。従って，上記ゲインを調整することにより，上記設定
された動力学特性に依存することなく制御精度を向上さ
せることができる。それにより，上記ワーク取扱い部は
上記設定された動力学特性で精度良く駆動制御される。

【図面の簡単な説明】

【図１】　　本発明の一実施例に係るマニピュレータを
示す斜視図。

【図２】　　上記マニピュレータの制御装置を示すブロ
ック構成図。

【図３】　　上記マニピュレータのワーク取扱い部の目
標軌道に対する倣い動作を示す説明図。

【図４】　　本発明の背景の一例となる従来のマニピュ
レータの制御装置を示すブロック構成図。

【図５】　　上記従来のマニピュレータのワーク取扱い
部の倣い動作を示す説明図。

【図６】　　図５の倣い動作とは異なる設定条件により
行った倣い動作を示す説明図。

【符号の説明】

１，１ａ　…制御装置 ３…動力学インピーダンス設定部 ４…目標弾性係数行列 ５…目標粘性係数行列 ６…目標慣性係数行列 ８…変換部 ９，９ａ　，９ｂ　…関節モータ １４…力センサ １５…ゲイン設定部 １６…加算器 １７…減算器 ２０…マニピュレータ ２１…角度センサ ２３…リンク ２４…リンク ２５…ワーク取扱い部 ３１…慣性項演算部

Claims (1)

【特許請求の範囲】
1. 【請求項１】　　複数のリンクを関節を介して連結して
   なるマニピュレータの，あるリンクに設けられたワーク
   取扱い部の位置，速度，反力と上記ワーク取扱い部の設
   定された動力学特性とに基づいて上記ワーク取扱い部の
   加速度指令値を演算する加速度演算手段と，上記加速度
   指令値を実現するために必要な上記マニピュレータの慣
   性力の推定値を上記加速度指令値と上記関節の位置，速
   度に基づいて演算する推定値演算手段とを少なくとも備
   え，少なくとも上記慣性力の推定値と上記反力とによっ
   て上記関節を駆動する駆動源のトルクを演算し出力する
   制御装置において，上記ワーク取扱い部の加速度を検出
   する加速度検出手段と，上記演算された加速度指令値と
   上記加速度検出手段により検出された加速度との差にゲ
   インを乗じた値を上記演算された加速度指令値に加算す
   る制御ループを具備してなることを特徴とするマニピュ
   レータの制御装置。