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

Abstract

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

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は，マニピュレータのワー
ク取扱い部の目標とする動力学インピーダンスに対応し
たトルクを上記ワーク取扱い部に与えるいわゆるインピ
ーダンス制御を行うことにより，ワークを安定して取扱
うことのできるマニピュレータの制御装置に関する。

【０００２】

【従来の技術】上記したようなマニピュレータの制御装
置としては，特開平２−２０５４８９号公報に開示され
た装置がある。上記制御装置に対応する制御ブロック図
を図３に示す。同図において，マニピュレータの制御装
置１ａ　は，マニピュレータのワーク取扱い部の目標と
する動力学的な挙動を表現する動力学インピーダンスの
設定部３を備えている。上記動力学インピーダンス設定
部３には，目標弾性係数行列４（Ｋ），目標粘性係数行
列５（Ｂ），目標慣性係数行列６（Ｍ）が予め設定され
ている。そして，上記制御装置１ａ　は，マニピュレー
タの関節モータ９の，例えば角度検出器より出力された
関節の揺動角（角度θ）に基づいて，上記ワーク取扱い
部の状態量としての位置ｘ，速度ｄｘ／ｄｔ，加速度ｄ
２　ｘ／ｄｔ２　をディジタルフィルタ１１，変換部２
を用いて演算する。更に，上記制御装置１ａ　は，上記
位置ｘと，目標位置設定部７より設定された目標位置ｘ
０　との偏差ｘ−ｘ０　と上記演算された速度ｄｘ／ｄ
ｔ，加速度ｄ２　ｘ／ｄｔ２　と，上記動力学インピー
ダンス設定部３の各係数行列４，５，６とよりそれぞれ
求められた力の総和としての外力Ｆｅを求めるようにな
っている。上記外力は，ワーク取扱い部にかかる力であ
って，変換部８において上記角度θの関数に変換される
。一方，逆動力学演算器１０では，上記トルクを補償す
るための，ワーク取扱い部に係る慣性項やコリオリ力，
求心力，粘性，クーロン摩擦等の項が演算される。そし
て，これらの各項により上記外力Ｆｅが補償されて，ワ
ーク取扱い部の動力学特性が上記動力学インピーダンス
設定部３に設定された目標インピーダンス（Ｋ，Ｂ，Ｍ
）になるようなトルクτが求められる。ちなみに，上記
制御装置１ａ　に設定され，上記トルクτに対応する演
算式を以下の（１）式に示す。 　　　　　　τ＝Ｉ（θ）・ｄ２　θ／ｄｔ２　＋Ｃ（
θ，ｄθ／ｄｔ）−ＪＴ　｛Ｍ・　　　　　　　　　　
ｄ２　ｘ／ｄｔ２　＋Ｂ・ｄｘ／ｄｔ＋Ｋ（ｘ−ｘ０　
）｝・・・（１）但し，　　　　　　　　　　　　　　
θ：リンク間の角度Ｉ（θ）：慣性項 Ｃ（θ，ｄθ／ｄｔ）：コリオリ力，求心力，粘性，ク
ーロン摩擦等の項 ＪＴ　：ヤコビアンの転置行列 ｔ：時間 ｘ：ワーク取扱い部のｘ方向位置 ｘ０　：ワーク取扱い部が作用しようとする仮想平衡点
のｘ方向の目標位置 上記制御装置１ａ　により制御されるマニピュレータは
，図４に示すような，例えばバリ取り作業に用いられる
。 ここで，ワーク４０は，マニピュレータの関節モータ９
のトルクにより生じた力でワーク取扱い部２５ａ　によ
り押圧されている。この状態において，ワーク取扱い部
２５ａ　が広幅矢印Ｋ方向に徐々に移動することにより
ワーク４０上のバリが削り取られるようになっている。 このとき，ワーク取扱い部２５ａ　からの力が加えられ
る目標点となる仮想平衡点Ｑは，ｘ方向に一定の目標位
置ｘ０　であって，ワーク４０の内部に設定されている
。そこで，上記制御装置１ａ　は，ｘ方向に関して現時
点のワーク取扱い部２５ａ　先端の位置ｘと上記目標位
置ｘ０　との偏差（ｘ−ｘ０　）と上記変換部２からの
状態フィードバック量である速度及び加速度に基づいて
上記ワーク取扱い部２５ａ　にかかる外力を求め，更に
この外力に基づいて関節モータ９のトルクτを演算し上
記関節モータ９を駆動制御するようになっている。この
ようなマニピュレータの制御装置１ａ　は，ワーク取扱
い部２５ａ　に係る外力を検出する力センサやトルクセ
ンサを用いることなく，関節モータ９のトルクを制御す
ることができるため構成が簡単になるといった利点があ
る。

【０００３】

【発明が解決しようとする課題】ところで，上記マニピ
ュレータの制御装置１ａ　では，ワーク取扱い部２５ａ
　のｘ方向の目標位置ｘ０がほぼ一定に設定されている
場合，上記ワーク取扱い部２５ａ　の目標速度や目標加
速度はいずれも０である。従って，上記目標粘性係数行
列５（Ｂ），目標慣性係数行列６（Ｍ）に関しては，変
換部２からフィードバックされたワーク取扱い部２５ａ
　の状態量としての，速度ｄｘ／ｄｔ，加速度ｄ２　ｘ
／ｄｔ２　を用いて外力の演算を行なえばよい。しかし
ながら，上記仮想平衡点Ｑのｘ方向の目標位置ｘ０　が
時間と共に移動するように設定されている場合には，上
記目標位置ｘ０　の移動に伴って，ワーク取扱い部の速
度や加速度も変化するはずである。ところが，上記従来
の制御装置１ａ　では，上記目標位置ｘ０　の移動のみ
しか考慮されていないので，上記目標位置ｘ０　が移動
した場合に，上記目標粘性係数行列５や目標慣性係数行
列６に関しては，ワーク取扱い部２５ａ　に加わる外力
が見かけ上大きく演算される。そのため，上記演算され
た外力に基づいて求められたトルクτによっては，ワー
ク取扱い部２５ａ　が力不足となり，ワーク取扱い部２
５ａ　がその目標位置ｘ０　の移動軌跡に追随すること
ができず遅れを生じることがあった。従って，本発明の
目的とするところは，ワーク取扱い部の目標位置が移動
する場合でも，目標とするワーク取扱い部の動力学イン
ピーダンスに対応したトルクをワーク取扱い部に与える
ことが可能で，ワークを安定して取扱うことのできるマ
ニピュレータの制御装置を提供することにある。

【０００４】

【課題を解決するための手段】上記目的を達成するため
に，本発明が採用する主たる手段は，その要旨とすると
ころが，複数のリンクを関節を介して連結してなるマニ
ピュレータの，あるリンクに設けられたワーク取扱い部
の少なくとも位置のフィードバック値と予め設定された
位置の目標値との偏差と，上記ワーク取扱い部の動力学
的挙動を表現する動力学インピーダンスとによって上記
ワーク取扱い部にかかる外力を求め，この外力に基づい
て上記関節を駆動する駆動源のトルクを演算し出力する
制御装置において，上記ワーク取扱い部の速度の目標値
を設定する目標速度設定手段と，上記ワーク取扱い部の
加速度の目標値を設定する目標加速度設定手段とを備え
，上記ワーク取扱い部の位置，速度，加速度のそれぞれ
のフィードバック値とこれらに対応する目標値との偏差
と，各偏差に対応してそれぞれ予め設定された動力学イ
ンピーダンスとに基づいて上記外力を演算する点に係る
マニピュレータの制御装置として構成されている。

【０００５】

【作用】本発明のマニピュレータの制御装置においては
，マニピュレータのワーク取扱い部の位置，速度，加速
度の各目標値を設定するそれぞれの設定手段が設けられ
ている。そして，上記ワーク取扱い部の位置，速度，加
速度のそれぞれのフィードバック値とこれらに対応する
目標値との偏差と，各偏差に対応してそれぞれ予め設定
された，例えば弾性，粘性，慣性といった動力学インピ
ーダンスとに基づいて，上記ワーク取扱い部に係る外力
が演算される。これにより，上記位置の目標値が移動す
るような場合でも上記速度及び加速度の目標値を上記位
置の目標値の移動量に応じて変更設定することができ，
且つ上記各目標値に対応する全ての偏差が上記外力の演
算に用いられるので，上記ワーク取扱い部にかかる外力
を精度良く演算することができる。

【０００６】

【実施例】以下添付図面を参照して，本発明を具体化し
た実施例につき説明し，本発明の理解に供する。尚，以
下の実施例は，本発明を具体化した一例であって，本発
明の技術的範囲を限定する性格のものではない。ここに
，図１は本発明の一実施例に係るマニピュレータの制御
装置を示すブロック構成図，図２は上記マニピュレータ
によりクランク機構が駆動される状態を示す説明図であ
る。但し，図３に示した上記従来のマニピュレータの制
御装置１ａ　と共通する要素には，同一の符号を使用す
ると共に，その詳細な説明は省略する。本実施例に係る
マニピュレータ２０は，先ず図２に示すように，図示せ
ぬ基台側に固定された第１の関節モータ９ａ　と，この
関節モータ９ａ　の回転駆動軸に固設された第１のリン
ク２３と，このリンク２３の先端に固設された第２の関
節モータ９ｂ　と，この関節モータ９ｂ　の回転駆動軸
に設けられた第２のリンク２４とリンク２４の先端に配
備されたワーク取扱い部２５とよりなっている。上記関
節モータ９ａ　及び９ｂ　には，それぞれエンコーダ（
３６００００ｐ／回転）が一体的に設けられており，上
記基台に対して揺動するリンク２３の角度θａ　やリン
ク２３の軸芯に対して揺動するリンク２４の角度θｂ　
を検出し出力するようになっている。そして，上記リン
ク２３及び２４のリンク長は予め決まっているので，こ
れらの角度θａ　及びθｂ　により上記ワーク取扱い部
２５の，例えばｘ方向の位置が決定される。なお，図１
中の関節モータ９は上記関節モータ９ａ　及び９ｂ　の
いずれも含むものであって，同じく角度θは上記角度θ
ａ　及びθｂ　を表わしている。そして，上記マニピュ
レータ２０を制御する制御装置１は，図１に示すように
，上記従来の制御装置１ａ　と基本的構成をほぼ同様と
し，上記従来の制御装置１ａ　と構造上異なる点は，ワ
ーク取扱い部２５の目標速度ｄｘ０　／ｄｔ（フィード
フォワード項）を設定する目標速度設定部１２と目標加
速度ｄ２　ｘ０　／ｄｔ２　（フィードフォワード項）
を設定する目標加速度設定部１３とを備え，上記変換部
２により演算されフィードバックされるワーク取扱い部
の位置，速度，加速度と目標位置，目標速度，目標加速
度とのそれぞれの偏差（ｘ−ｘ０　），（ｄｘ／ｄｔ−
ｄｘ０　／ｄｔ），（ｄ２　ｘ／ｄｔ２　−ｄ２　ｘ０
　／ｄｔ２　）と，これらの偏差の種類に対応して予め
設定されている動力学インピーダンス（Ｋ，Ｂ，Ｍ）と
に基づいて，上記ワーク取扱い部２５に係る外力Ｆｅを
演算する制御ブロック構成が採用されたことである。 即ち，上記外力Ｆｅは，次の（２）式により求められる
。 　　　　　　Ｆｅ＝Ｍ（ｄ２　ｘ／ｄｔ−ｄ２　ｘ０　
／ｄｔ）＋Ｂ（ｄｘ／ｄｔ−ｄｘ０　／　　　　　　　
　　　　　ｄｔ）＋Ｋ（ｘ−ｘ０　）・・・（２）そし
て，上記外力Ｆｅは変換部８により角度θの関数ＪＴ　
Ｆｅに変換される。

【０００７】一方，逆動力学演算器１０は上記関節モー
タ９のエンコーダから出力された角度θ及びこの角度θ
に基づいてディジタルフィルタ１１により演算された角
速度，角加速度に基づいて，慣性項Ｉ（θ）と，コリオ
リ力，求心力，粘性，クーロン摩擦等の項Ｃ（θ，ｄθ
／ｄｔ）との和（以下，トルク補償量という）を求める
。上記トルク補償量は，ワーク取扱い部２５が動力学イ
ンピーダンス設定部３に設定されている目標インピーダ
ンス（Ｋ，Ｂ，Ｍ）の状態でワークを取扱い得るように
，外力Ｆｅに基づいて得たトルクＪＴ　Ｆｅを補償する
ためのものである。即ち，上記関節モータ９に与えられ
るべきトルクτは次の（３）式により求められる。 　　　　　　τ＝Ｉ（θ）・ｄ２　θ／ｄｔ２　＋Ｃ（
θ，ｄθ／ｄｔ）　　　　　　　　　　−ＪＴ　Ｆｅ・
・・（３）なお，上記変換部２，動力学インピーダンス
設定部３，変換部８，逆動力学演算器１０，ディジタル
フィルタ１１は，図外のＣＰＵ上で動くソフトウエアに
より全て構成され，プログラムとして同じく図外のメモ
リに格納されている。また，上記マニピュレータ２０は
制御装置１のＣＰＵにより４ミリ秒のサンプリング周期
で制御される。なお，上記トルク補償量の各項について
，関節モータ９の駆動軸の粘性とクーロン摩擦とは停止
中の駆動軸に力（トルク）をステップ状に変えて与え，
上記駆動軸が動き始めた時のトルクを実測しこれに基づ
いて予め求めた。また，上記トルク補償量に係る慣性項
は，上記関節モータ９に白色ノイズ状のトルクを与え，
このとき同時に計測して得た。

【０００８】上記した構成のマニピュレータ２０の制御
装置１を用いてクランク機構３０のクランク回し動作を
行なった例に付き図２に示す。この場合，上記クランク
機構３０はクランク軸３１に固設されたクランクロッド
３２が上記クランク軸３１回りに回転する。そして，上
記マニピュレータのワーク取扱い部２５が上記クランク
ロッド３２の先端の孔部に摺動自在に取り付けられてい
る。なお，上記クランク機構３０は，クランクロッド３
２及びクランク軸３１の重量が比較的軽く，且つクラン
ク軸３１は図外の基台に対して回動し易い構成になって
いる。そこで，上記制御装置１において，ワーク取扱い
部２５の目標位置は，目標位置設定部７より，例えばク
ランクロッド３２のロッド長を半径とするクランク軸３
１回りの円軌跡を描いて移動するように設定されている
。そこで，上記マニピュレータ２０の関節モータ９に上
記（３）式により求めたトルクτを与えると，クランク
機構３０がワーク取扱い部２５により駆動される。この
とき，上記ワーク取扱い部２５が受けた反力を図２の曲
線Ｐに示す。但し，上記曲線Ｐを構成する直線の向き及
び長さは，上記反力の方向及び大きさを示す。それによ
れば，上述したようなリンク機構３０の構成上の特徴か
ら，接線方向の反力は半径方向にかかる反力と比べて小
さいために，半径方向の反力が大きくクローズアップさ
れている。しかしながら，上記ワーク取扱い部２５は全
体的には大きな反力を受けることなく安定した回転運動
を行なっている。上記したように，本実施例に係る制御
装置１はマニピュレータ２０の目標位置ｘ０　が時間経
過と共に移動する場合に，設定された目標弾性Ｋのみな
らず，目標粘性Ｂ及び目標慣性Ｍに関する外力をも適切
に求めることができる。従って，上記制御装置１は動力
学インピーダンス設定部３に設定された目標インピーダ
ンス（Ｋ，Ｂ，Ｍ）に対応したトルクをワーク取扱い部
２５に与えることが可能で，上記ワーク取扱い部２５を
上記目標インピーダンスの状態に保持しつつ，このワー
ク取扱い部２５を移動する目標位置ｘ０　の移動軌跡に
遅れることなく適切に動作させることができる。これに
より，クランク機構３０は無理な力を与えられることな
く円滑に回転される。なお，上記した実施例では，目標
位置設定部７とは別に，目標速度，目標加速度を設定す
るための目標速度設定部１２及び目標加速度設定部１３
を設けたが，上記目標位置設定部７に設定された目標位
置ｘ０　に基づいて，速度及び加速度を演算し，これら
を状態フィードバック量との偏差の計算に供してもよい
。

【０００９】

【発明の効果】本発明によれば，複数のリンクを関節を
介して連結してなるマニピュレータの，あるリンクに設
けられたワーク取扱い部の少なくとも位置のフィードバ
ック値と予め設定された位置の目標値との偏差と，上記
ワーク取扱い部の動力学的挙動を表現する動力学インピ
ーダンスとによって上記ワーク取扱い部にかかる外力を
求め，この外力に基づいて上記関節を駆動する駆動源の
トルクを演算し出力する制御装置において，上記ワーク
取扱い部の速度の目標値を設定する目標速度設定手段と
，上記ワーク取扱い部の加速度の目標値を設定する目標
加速度設定手段とを備え，上記ワーク取扱い部の位置，
速度，加速度のそれぞれのフィードバック値とこれらに
対応する目標値との偏差と，各偏差に対応してそれぞれ
予め設定された動力学インピーダンスとに基づいて上記
外力を演算することを特徴とするマニピュレータの制御
装置が提供される。それにより，ワーク取扱い部の目標
位置が移動するように設定された場合でも，上記目標速
度設定手段及び目標加速度設定手段より設定された速度
及び加速度の各目標値により，これらに対応した外力が
それぞれ補償される。従って，上記ワーク取扱い部にか
かる外力を精度良く求めることが可能で，上記マニピュ
レータの関節を駆動する駆動源のトルクを上記求められ
た外力に基づいて精度良く演算することができる。 その結果，上記ワーク取扱い部は移動する目標位置に対
して遅れを生じることなくワークを取扱うことができる
。

【図面の簡単な説明】

【図１】　　本発明の一実施例に係るマニピュレータの
制御装置を示すブロック構成図。

【図２】　　上記マニピュレータによりクランク機構が
駆動される状態を示す説明図。

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

【図４】　　上記従来のマニピュレータを用いてワーク
のバリ取り作業を行なう態様を示す説明図。

【符号の説明】

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

Claims (1)

【特許請求の範囲】
1. 【請求項１】　　複数のリンクを関節を介して連結して
   なるマニピュレータの，あるリンクに設けられたワーク
   取扱い部の少なくとも位置のフィードバック値と予め設
   定された位置の目標値との偏差と，上記ワーク取扱い部
   の動力学的挙動を表現する動力学インピーダンスとによ
   って上記ワーク取扱い部にかかる外力を求め，この外力
   に基づいて上記関節を駆動する駆動源のトルクを演算し
   出力する制御装置において，上記ワーク取扱い部の速度
   の目標値を設定する目標速度設定手段と，上記ワーク取
   扱い部の加速度の目標値を設定する目標加速度設定手段
   とを備え，上記ワーク取扱い部の位置，速度，加速度の
   それぞれのフィードバック値とこれらに対応する目標値
   との偏差と，各偏差に対応してそれぞれ予め設定された
   動力学インピーダンスとに基づいて上記外力を演算する
   ことを特徴とするマニピュレータの制御装置。