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

Abstract

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

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

【従来の技術】上記したようなマニピュレータの制御装
置としては，特開平２−２０５４８９号公報に開示され
た装置がある。上記制御装置に対応する制御ブロック図
を図５に示す。同図において，マニピュレータの制御装
置１ａ　は，マニピュレータのワーク取扱い部２０の目
標とする動力学的な挙動を表現する動力学インピーダン
スの設定部３を備えている。上記動力学インピーダンス
設定部３には，目標弾性係数行列４（ｋ），目標粘性係
数行列５（ｂ），目標慣性係数行列６（ｍ）が予め設定
されている。上記マニピュレータ２０の動作は次の運動
方程式で与えられる。 　　　　　　　　Ｍ（θ）・ｄ２　θ／ｄｔ２　＋Ｃ（
ｄθ／ｄｔ，θ）　　　　　　　　　　　　　　　　＝
τ＋Ｊ（θ）Ｔ　・Ｆ＋Ｄ　　　　　　　　　　　　　
　　　　　　　　　…（１）ここで，　　　　　　　　
　　　　　　　　　　θ：関節角（ベクトル）Ｍ（θ）
：慣性項（行列） Ｃ（ｄθ／ｄｔ，θ）：コリオリ力，求心力，粘性，ク
ーロン摩擦等の非線形項（行列） τ：関節モータに与えられるトルク Ｄ：トルクリップル等の外乱 Ｊ（θ）Ｔ　：ヤコビアンの転置行列 Ｆ：ワーク取扱い部が外部環境から受ける反力そして，
上記制御装置１ａ　は，マニピュレータ２０の関節モー
タの，例えば角度検出器より出力された関節の揺動角（
角度θ）に基づいて，上記ワーク取扱い部の状態量とし
ての位置ｘ，速度ｄｘ／ｄｔ，加速度ｄ２　ｘ／ｄｔ２
　をディジタルフィルタ１１，変換部２を用いて演算す
る。更に，上記制御装置１ａ　は，上記位置ｘと，目標
位置設定部７より設定された目標位置ｘ０　との偏差ｘ
−ｘ０　と上記演算された速度ｄｘ／ｄｔ，加速度ｄ２
　ｘ／ｄｔ２　と，上記動力学インピーダンス設定部３
の各係数行列４，５，６とよりそれぞれ求められた力の
総和としての設定インピーダンスｆを求めるようになっ
ている。 上記設定インピーダンスｆは，ワーク取扱い部に与えら
れるべき付与力を示し，変換部８において上記角度θの
関数に変換される。即ち，上記設定インピーダンスｆを
求める構成が本発明にいう付与力演算手段である。一方
，逆動力学演算器１０は，ワーク取扱い部に係る慣性項
やコリオリ力，求心力，粘性，クーロン摩擦等の項（以
下，非線形項という）を求めるための数学モデルを備え
ている。上記慣性項や非線形項は，上記関節の角度，角
速度，角加速度に基づいて上記数学モデルを解いて得た
推定値である。そして，これらの各項の推定値によりワ
ーク取扱い部自身の慣性力，粘性力，クーロン摩擦力等
が求められ，これらの力と上記設定インピーダンスｆと
に基づいて，ワーク取扱い部の動力学特性が上記動力学
インピーダンス設定部３に設定された目標弾性・粘性・
慣性ｋ，ｂ，ｍになるようなトルクτが求められる。ち
なみに，上記制御装置１ａ　に設定され，上記トルクτ
に対応する演算式を以下の（２）式に示す。 　　　　　　　　τ＝Ｍ＊　（θ）・ｄ２　θ／ｄｔ２
　＋Ｃ＊　（θ，ｄθ／ｄｔ）　　　　　　　　　　　
　−Ｊ（θ）Ｔ　・ｆ　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　…（２）但し
，　　　　　　　　　　　　　　Ｍ＊　（θ）：慣性項
の推定値Ｃ＊　（θ，ｄθ／ｄｔ）：上記非線形項の推
定値又，上記設定インピーダンスｆは次式で表される。 　　　　　　　　ｆ＝ｆ（ｄ２　ｘ／ｄｔ２　，ｄｘ／
ｄｔ，ｘ，ｄ２　ｘ０　／ｄｔ２　，　　　　　　　　
　　　　　　　　ｄｘ０　／ｄｔ，ｘ０　）　　　　　
　　　　　＝ｍ・ｄ２　ｘ／ｄｔ２　＋ｂ・ｄｘ／ｄｔ
＋ｋ（ｘ−ｘ０　）…（３）上記制御装置１ａ　により
制御されるマニピュレータ２０を用いて，予め設定され
た目標軌道に対するワーク取扱い部の倣い動作を行った
例を図６に示す。同図において，ワーク取扱い部の目標
軌道Ｐ０　上には障害物４０が存在している。上記ワー
ク取扱い部は，予め設定された目標軌道Ｐ０　に追随す
るように，関節モータの駆動により始点Ｓから図中時計
廻りに移動する。このときのワーク取扱い部の実際の移
動軌跡を実線Ｐ３　で示す。そして，ワーク取扱い部が
障害物４０に接触すると，ワーク取扱い部は障害物４０
の表面に沿って移動し，同時に障害物４０からの反力Ｆ
を受ける。上記反力Ｆを示す複数の直線は，それぞれの
直線が交差する障害物４０上からの反力のベクトルを示
す。このようなマニピュレータの制御装置１ａ　は，ワ
ーク取扱い部に係る外力を検出する力センサやトルクセ
ンサを用いることなく，関節モータのトルクを制御する
ことができるため構成が簡単になるといった利点がある
。

【０００３】

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

【０００４】

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

【０００５】

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

【０００６】

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

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

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

【発明の効果】本発明によれば，複数のリンクを関節を
介して連結してなるマニピュレータの，あるリンクに設
けられたワーク取扱い部の位置，速度，加速度と上記ワ
ーク取扱い部の設定された動力学特性とに基づいて上記
ワーク取扱い部に与えるべき付与力を求める付与力演算
手段を少なくとも備え，少なくとも上記求められた付与
力を用いて上記関節を駆動する駆動源のトルクを演算し
出力する制御装置において，上記ワーク取扱い部にかか
る反力を検出する力センサと，上記求められた付与力と
上記力センサにより検出された反力との差にゲインを乗
じた値を上記付与力に加算する制御ループとを具備して
なることを特徴とするマニピュレータの制御装置が提供
される。従って，上記ゲインを調整することにより，上
記設定された動力学特性に依存することなく制御精度を
向上させることができる。それにより，上記ワーク取扱
い部は上記設定された動力学特性で精度良く駆動制御さ
れる。

【図面の簡単な説明】

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

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

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

【図４】　　上記ワーク取扱い部の目標軌道に対する倣
い動作の別例を示す説明図。

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

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

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

【符号の説明】

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

Claims (1)

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