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

Abstract

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

Description

【発明の詳細な説明】 （目次） 概要 産業上の利用分野 従来の技術　（第８図、第９図） 発明が解決しようとする課題 課題を解決するための手段　（第１図）作用　　（第１
図） 実施例　（第２図乃至第６図） 発明の効果 （概要） 速度指令信号に基づいてマニピュレータを作動させる操
作部と、マニピュレータが受ける力を検出する力検出部
と、この検出された力及び目標とする力に基づいて速度
指令信号を発する力制御部とを備えた制御装置に関し、 マニピュレータに対する操作部の応答性能を損なうこと
なく、マニピュレータの固有周波数に関する速度指令信
号をカットし、マニピュレータの共振状態を十分に除去
できるようにすることを目的とし、 上記速度指令信号のうちマニピュレータの固有振動数の
みに係る成分を減衰させるフィルタ部を設けて構成する
。

（産業上の利用分野） 本発明は、・速度指令信号に基づいてマニピュレータを
作動させる操作部と、マニピュレータが受ける力を検出
する力検出部と、この検出された力及び目標とする力に
基づいて速度指令信号を発する力制御部とを備えた制御
装置に関する。

（従来の技術） 従来、この種の制御装置として、対象物の表面に沿って
加工、パリ取り、拭き取り等の倣い動作をするハンド部
を備えたマニピュレータに用いる制御装置がある。

これは、第８図に示すように°、速度指令信号に基づい
てマニピュレータ１１の操作を行う操作部１２と、マニ
ピュレータ１１のハンド部１１ａ（第９図）の現在位置
を検出する位置検出部１６と、ハンド部１１ａが受ける
力の検出を行う力検出部１３と、位置検出部１６が検出
した位置、マニピュレータ１１の目標位置及び対象物の
形状などによって定められた位置パラメータに基づいて
マニピュレータ１１の位置制御方向（移動方向）の速度
指令信号を発する位置制御部１７と、力検出部１３が検
出した力、目標とするハンド部の押しつけ力（以下「設
定力」という）及び対象物の形状などによって定められ
た力制御パラメータに基づいて速度指令信号を発する力
制御部１４とを有するものである。上記力検出部１３は
、第９図に示すように、力覚センサ１３ａを備えており
、この力覚センサ１３ａは、例えば、外力によって撓む
弾性梁の歪を歪ゲージで検出するもので、マニピュレー
タ１１の手首部とハンド部１１ａとの間に装着されてお
り、ハンド部１１ａの一作用点にかかる力及びモーメン
トを検出するものである。

これによりマニピュレータを作動させるには、位置制御
部１７及び力制御部１４から操作部１２へ発信された速
度指令信号によりハンド部１１ａを動かすものである。

そして、常に上記設定力がバンド部１１ａに与えられる
ように、力検出部１３でハンド部１１ａに作用する力を
検出し、この検出に基づいて力制御部１４から対応した
速度指令信号な送出（フィードバック制御）している。

ところで、上記力検出部１３においては、対象物に対す
るハンド部１１ａの押しつけ力だけでなく、ハンド部１
１ａの重さ、慣性力、対象物との摩擦力、マニピュレー
タ１１を伝播する振動に起因した力、例えばマニピュレ
ータ１１の減速機が発生する振動に起因した力等のノイ
ズも検出してしまう。特に、マニピュレータ１１を伝播
してきた振動は、マニピュレータの固有振動数を含んで
いるので、上記フィードバック制御を行なうと、固有振
動数に対応した速度指令信号が発信され、そのため、マ
ニピュレータ１１が直ちに共振してしまう。

そこで従来においては、この固有周波数に関する速度指
令信号を与えないために、第８図に示すように、ある周
波数（以下「基準周波数」という）よりも高い周波数帯
に関する速度指令信号をカットするフィル多（所謂ロー
パスフィルタ１５）を操作部の手前に設け、共振が生じ
ないようにしていた。

（発明が解決しようとする課題） ところで、この従来の制御装置にあっては、特に−次共
振周波数のように比較的低い周波数帯にある固有振動数
に関する速度指令信号をカットすることができないので
、マニピュレータの共振状態を十分に除去できないとい
う問題があった。

それは、ローパスフィルターを用いて上記の比較的低い
周波数帯にある固有振動数に関する速度指令信号をカッ
トするようにすると、基準周波数が低い周波数帯に設定
されることからカットされる周波数帯域が広くなりすぎ
てしまうので、即ち、固有振動数以外の他の周波数に関
する速度指令信号がカットされすぎてしまうので、それ
だけ、フィードバック制御の応答性が悪いものになって
しまうからである。そのため、従来においては、上記基
準周波数を比較的高い周波数帯に設定し、この基準周波
数よりも高い周波数帯にある固有振動数に関する速度指
令信号をカットし、上記の比較的低い周波数帯にある固
有振動数に関する速度指令信号はそのままにして、フィ
ードバック制御の応答性をある程度維持しつつ、共振状
態を除去するようにしていた。

そこで、本発明の技術的課題は、操作部のマニピュレー
タに対する応答性（フィードバック制御の応答性）を損
なうことなく、マニピュレータの固有周波数に関する速
度指令信号をカットし、マニピュレータの共振状態を十
分に除去できるようにする点にある。

〔課題を解決するための手段〕

以上の課題を解決する本発明の技術的手段は、第１図に
示すように、速度指令信号に基づいてマニピュレータ１
を作動させる操作部２と、マニピュレータ１が受ける力
を検出する力検出部３と、この検出された力及び目標と
する力に基づいて速度指令信号を発する力制御部４とを
備えた制御装置を前提にし、上記速度指令信号のうちマ
ニピュレータ１の固有振動数のみに係る成分を減衰させ
るフィルタ部５を設けたものである。

（作用） この手段によれば、操作部２によりマニピュレータ１が
作動させられると、力検出部３によりマニピュレータ１
が現在受けている力が検出され、力制御部４は、この検
出された力及び目標とする力に基づいて速度指令信号を
発する。

そして、フィルタ部５において、この発せられた速度指
令信号のうちマニピュレータ１の固有振動数のみに係る
成分が減衰させられる。そのため、マニピュレータｌの
共振状態が十分に除去されるとともに、固有振動数に関
する速度指令信号をカットしても他の周波数に関する速
度指令信号はカットされないので、操作部２のマニピュ
レータ１に対する応答性は損なわれない。

〔実施例〕

以下、添付図面に基づいて実施例に係る制御装置を詳細
に説明する。

実施例に係る制御装置は、第２図に示すように、マニピ
ュレータ２１の制御を行う操作部２２を備えている。こ
の操作部２２はサーボ・モータ２２ａと、パワー・アン
プ２２ｂと、Ｄ／Ａコンバータ２２ｃと、補償器２２ｄ
とを有している。

また、制御装置は、マニピュレータ２１のハンド部２１
ａの先端位置の検出を行う位置検出部２６をそなえてお
り、この位置検出部２６はカウンタ及びエンコーダ２６
ａとタコ・メータ２６ｂとを有している。

更に、制御装置は、マニピュレータ２１のハンド部２１
ａが受ける力の検出を行う力検出部２３を備えている。

この力検出部２３は上記と同様の力覚センサ２３ａとハ
ンド部座標系からロボット基準座標系への座標変換部２
３ｂとを有している。

更にまた、制御装置は、倣い動作時に、力検出部２３に
より検出された力゛Ｆｒ、設定力（力指令Ｆ０）及び力
制御パラメータに基づいて力制御方向の速度指令信号Ｖ
ｆを発する力制御部２４と、位置検出部２６に検出され
た位置Ｘ、、、目標位置Ｘ０及び位置パラメータに基づ
いて位置制御方向の速度指令信号Ｖｐを発する位置制御
部２７とを備えている。

この位置制御部２７の具体的構成は、第３図に示すよう
に、転置直交変換行列（ＲＴ）演算部３１と、選択行列
（Ｉ−３ｒ）演算部３２と、直交行列（Ｒ）演算部３３
と、位置フィードバックゲイン（Ｃ９）演算部３４とを
有する。一方、力制御部２４は転置直交行列（Ｒ”）演
算部３８と、選択行列（ｓｒ）演算部３７と、他の選択
行列（Ｉ−３ｒ）演算部３９ｂと、不感帯演算部３９ａ
と、直交行列演算部３６と、カフィードバックゲイン（
Ｃ，）演算部３５とを有する。尚、符号２４ａ及び２７
ａは、偏差部である。

ここで、第５図に示すように、対象物への押し付は力を
示す力指令Ｆ。で与えられる力制御方向の単位ベクトル
（倣い動作が行われる対象物の表面の法線ベクトル）を
“バ゛、倣い方向の移動速度指令信号ｖ０で与えられる
位置制御方向の単位ベクトル゛δ′”で表したとき、位
置制御されるさらにもう１つの直交ベクトル“言′°は ａ＝ｎ　　Ｘ。

で与えられる。。

これを各々ロボット基準座標系による成分表示を行うと
、 ｎ＝（ｎｘ＊ｎｙｔｎｚ）７ ０”　（ｏ、　＊ＯＶ　＊Ｏ１）Ｔ となる。ここで、Ｔは転置行列を示す。

これらのベクトルを用いて当該ロボット基準座標系（ｘ
＊ｙｓｚ）から倣い座標系（Ｘ’５ｙＺＺ’）への座標
変換を表示する直交座標変換行列Ｒは次のように表され
ることになる。

第５図において、当該倣い座標系（ｘ’　ｓｙ’　＋ｚ
’）のｙ゛及び２”方向を位置制御方向、ｘ′力方向力
制御方向とすることで選択行列演算部３２゜３７．３９
ｂの選択行列は次の式に表示される。

で与えられる。この場合、一般に例えばねじ締め、缶の
蓋締め等押し付は方向の軸まわりにトルクを与えるので
トルクを与える場合にはＳ＝１とし、与えない場合には
Ｓ＝０とする。

カフィードバックゲインｃ、は基準座標系に関して、 で与えられる。

また、位置フィードバックゲインｃｐは同様にして、 で与えられる。

選択行列のＩは単位行列を表している。

さらに、本実施例では不感帯演算部３９ａを設けている
。当該演算部３９ａは倣い動作中に位置制御方向が対象
物接触点における接平面に一致していない場合に対象物
表面に多大な摩擦力が生じて表面をかじり、対象物表面
あるいはワークに損傷を与えるのを防止することができ
るものである。すなわち、対象物の接触点に許容範囲以
上の力が作用したときには位置制御方向にもカフィード
バックを作用させるためである。

更に、制御装置は、力制御部２４及び前記位置制御部２
７から出力された速度についての加算を行う加算部３０
ｂと、加算された速度をマニピュレータ２１の各関節の
角速度すに変換する逆ヤコビ変換部３０ａとを有してい
るとともに、加算部３０ｂと逆ヤコビ変換部３０ａとの
間にフィルタ部４０を設けである。

フィルタ部４０の具体的構成は、第４図に示すように、
カットしようとする振動系の固有周波数ｆｃ、振動系の
減衰係数ξ及びサンプル周期Ｔを入力するキーボード４
１と、この入力された各個から必要な係数（ａｏ　ｓ　
ａｉ　Ｉ　ａ２　）を算出するホストコンピュータ４２
と、この算出された係数を記憶するメモリ４４を備える
とともに記憶された係数に基いて上記入力した固、有振
動数にかかる速度指令信号をカットするフィルタ演算部
を備えたコントローラ４３とから構成される。コントロ
ーラ４３のフィルタ演算部は第一のフィルタ演算部４５
と第二のフィルタ演算部４６とを直列に接続してあり、
速度指令イぎ号のうち２種類の固有振動数に係る速度指
令信号をカットできるようになっている。本実施例にお
いては、ハン１く部２１ａが拘束されていないときのマ
ニピュレータ２１の一次共振周波数（以下「無拘束共振
周波数ｆ０１」という）及びその減衰係数（以下「無拘
束減衰係数ξ、」という）と、ハント部２１ａが倣い動
作をしているときなど拘束された時のマニピュレータ２
１の一次共振周波数（以下「拘束共振周波数ｆｃ２」と
いう）及びその減衰係数（以下「拘束減衰係数ξ２」と
いう）とが入力され、第一のフィルタ演算部４５におい
て無拘束共振周波数ｆｅｎに関する処理を行ない、第二
のフィルタ演算部４６において拘束共振周波数Ｌ２に関
する処理を行なうようにしている。

上記ホストコンピュータ４２における演算は以下の通り
である。

まず連続系の伝達関数Ｇ　（Ｓ）が得られる。

尚、ωｃ：＝：２πｆｃであり、Ｓはラプラス演算子で
ある。

これをデジタル化するために、双線形変換を施すと、以
下の式が得られる。

Ｙｋ　＝　ａ６　　ｕｋ　＋　ａ　１　　ｕｈ−ｉ　　
＋　ａｏ　　ｕｋ−ｚ−ａｌ　Ｙｋ−ｔ　　＋Ｂ２　Ｙ
ｋ−２ここで、Ｙｋ、Ｙｋ−、、ｙｋ−、はフィルタの
出力、ｕｋ　＋　ｕｋ−１＊　ｕｋ−１１は入力である
。

また、 ａ（、＝’（’２ｉｊ。′＋４） α ａ、＝−！−（窓。２−４） α ａ　２　＝　−’　（４−４ξ窃。＋）。′）α ここに、α＝４＋４ξ”ａｃ＋’；ａｃ”、寄。ヨωｃ
丁であり、Ｔはサンプル周期である。

そして、上記の演算によって、無拘束共振周波数ｆ０１
に対する係数（ａ　Ｏｌｓ　ａｌｉｅ　ａ　２１）及び
拘束共振周波数ｆｃ２に対する係数（ａ０２＋ａ工、。

ａ２２）が算出される。

次に、この実施例の作用を説明する。

マニピュレータ２１の動作時においては、力覚センサ２
３ａによりハンド部２１ａに加えられる力がハンド部２
１ａ座標系の成分Ｆｈとして検出され、座標変換部２３
ａに送出して力の成分なロボット基準座標系で表示した
成分Ｆ、、に変換する。偏差部２４ａにおいては、上記
Ｆ、と力指令Ｆｏとの偏差が取られる。また、ハンド部
２１ａの位置姿勢はエンコーダ及びカウンタ２６ａによ
り検出された各関節角θ、から位置姿勢演算部２０によ
り算出される。

そして、位置制御部２７及び力制御部２４においては、
前記直交変換行列Ｒ及び転置直交変換行列ＲＴが０式か
ら求められ、こうして得られたＲ、Ｒ”は前述した位置
制御部２７及び力制御部２４の（転置）直交変換行列演
算部３３，３１，３６゜３８に送出される。

すなわち、力制御部２４から出力される速度指令信号Ｖ
ｆは式■、■、■及び出力Ｆｒと力指令Ｆｏどの偏差に
より Ｖ（＝ＣｆＲ８４Ｒ”（Ｆｏ　−Ｆｒ　）　　　　”’
　　■また、位置制御部２７から出力される速度指令信
号ｖｐは式■、■、■及び現在位置Ｘｒと目標位置Ｘｏ
との偏差によって、 Ｖｐ　＝Ｃｐ　Ｒ（Ｉ−３ｆ）Ｒ”　（Ｘｏ　−Ｘｒ）
　　　”’　　■で与えられる。

この場合に、力制御部２４において、位置制御方向に作
用する力も監視するため、選択行列演算部３９ｂと不感
帯演算部３９ａを設ける。これによって、ハンド部２１
ａに許容範囲以上の力が作用した場合には位置制御方向
にもカフィードバックを作用させている。

第３図において、位置制御方向に作用する力としての監
視力Ｆｃｒは Ｆ　ｏｒ（Ｉ−３ｒ）Ｒ”　（Ｆｏ−Ｆｒ）で与えられ
、倣い座標系で表示した力の許容範囲Ｕｒ００）を不感
帯中と設定した不感帯演算部３９ａにおいて、 １Ｆ、ｒｌ≦Ｕ。

のとき、即ち、監視力が力の許容範囲より小さいときは
、弐〇及び■を加算した速度指令信号ｖｋが得られ、 １　Ｆ、、、Ｉ　＞Ｕ。

のとき、即ち、監視力が力の許容範囲より大なるときは
、力制御部２４の速度指令信号Ｖ、はＶｒ　＝Ｃｔ　Ｒ
（ＳＪ”（Ｆｏ　−Ｆｒ）　”　Ｆｃｒａ）で与えられ
る。

この場合Ｆｃｒａは力の許容範囲と監視力との大小に応
じて Ｆｏｒａ＝Ｆｃｒ−０ｒ（Ｆｃｒ〉０）＝Ｆｏｒ”　　
Ｕｒ（Ｆｃｒく０） で与えられる。当然、位置制御部２７における速度指令
信号Ｖ、は式■で良い。

そして、加算部３０ｂにおいて、速度指令信号ｖｋが得
られる。

Ｖｋ　”Ｖｏ　＋　Ｖｔ　＋　ＶＤ こうして、得られた速度指令信号ｖｋは、上記のフィル
タ部４０で処理される。即ち、フィルタ部４０において
は、第４図に示すように、キーボード４１に入力された
無拘束共振周波数ｆ　ｃｌ＊拘束減衰係数ξ２．拘束共
振周波数ｆ　Ｃ！＋拘束拘束減衰係数及２サンプル周期
Ｔにより、ホストコンピュータ４２において、必要な係
数（ａｏｔ＋ａ１□ｓ　ａ２１）　＊　　（ａ０２＋　
ａ１２ｓ　ａ２２）が算出され、この係数がコントロー
ラ４３のメモリ４４に記憶される。そして、コントロー
ラ４３の第一のフィルタ演算部４５は、上記記憶された
係数（ａ　Ｏ１＋　８１１　＊　ａ　１１□）に基づい
て無拘束共振周波数ｆｃｌに関する速度指令信号成分を
カットする演算処理を行なうとともに、第二のフィルタ
演算部４６は、上記記憶された係数（ａ　Ｏ２，ａ　１
２ｓａ２□）に基づいて拘束共振周波数ｆｃ２に関する
速度指令信号成分をカットする演算処理を行なう。

こうして、得られた速度指令信号ｖ、ｎは逆ヤコビ行列
演算部３０ａでθ−Ｊ−１・ｖ、ｎに基づいて、関節速
度θが求められ、この関節速度は補償器２２ｄに入力さ
れＤ／Ａ変換された後、増幅されてサーボ・モータ２２
ａを駆動する。

第６図（ａ）（ｂ）（ｃ）及び第７図（ａ）（ｂ）（Ｃ
）は、本実施例にかかる制御装置における倣い動作の実
験結果及びローパスフィルタを用いた従来の制御装置に
おける倣い動作の実験結果を示す図である。

この実験は、基準座標系におけるｘｒ、ｙｒ力方向位置
制御し、Ｚｒ方向を力制御したものであり、Ｘ、、方向
に４０ｍｍ移動させ（第６図（ａ）及び第７図（ａ））
、Ｚ、方向に０．５　Ｋｇｆ　　（設定力）を指令した
ものである。そして、このときの、ハンド部２１ａに加
えられたＸｒ力方向力の変化（第６図（ｂ）及び第７図
（ｂ））及びハント部２１ａに加えられたＺｒ方向の力
の変化（第６図（ｃ）及び第７図（Ｃ））を検出したも
のである。尚、対象物はアルミ製の立方体である。

このグラフから判る様に、本実施例では従来に比較して
ハンド部２１ａに加わる力の変化量が著しく少なくなっ
ており、マニピュレータ２１の共振が十分に押えられて
いる。

〔発明の効果〕

以上説明したように、本発明によれば、フィルタ部によ
り速度指令信号のうちマニピュレータの固有振動数のみ
に係る成分を減衰させることができるので、マニピュレ
ータの共振状態を十分に除去でき、しかも、この減衰は
部分的なので他の速度指令信号に影響をあたえないこと
からフィードバック制御の応答性を悪くすることがない
。特に−次共振周波数のように比較的低い周波数帯にあ
る固有振動数に関する速度指令信号をカットできるので
、従来に比較して、マニピュレータの共振状態の除去性
能が向上した。

【図面の簡単な説明】

第１図は本発明の原理ブロック図、第２図は実施例に係
る制御装置のブロック図、第３図は実施例に係る位置制
御部及び力制御部を示す図、第４図は実施例に係るフィ
ルタ部の構成を示す図、第５図はマニピュレータの基準
座標系における単位ベクトルを説明する図、第６図（ａ
）（ｂ）（Ｃ）は本実施例に係る制御装置における倣い
動作の実験結果を示すグラフ、第７図（ａ）（ｂ）（Ｃ
）は従来の制御装置における倣い動作の実験結果を示す
グラフ、第８図は従来例に係る制御装置のブロック図、
第９図は本考案が適用されるマニピュレータをそのロ゛
ボット基準座標系及びハンド部座標系とともに示す図で
ある：１．１１．２１・・・マニピュレータ ２，１２．２２・・・操作部 ３．１３．２３・・・力検出部 ４．１４．２４・・・力制御部 ５．１５．４０・・・フィルタ部 ４１・・・キーボード ４２・・・ホストコンピュータ ４５・・・第一のフィルタ演算部 ４６・・・第二のフィルタ演算部 矛屍哨の原理図 ｉｌ１図 笑施例１て、係るフィルタ令Ｐ４俸的構成ｔホ１図第４
図 第５図 咬束例１ｚ係、るフ′°ロツ７図 第８図 第９図

Claims (1)

1. 【特許請求の範囲】 速度指令信号に基づいてマニピュレータ（１）を作動さ
   せる操作部（２）と、マニピュレータ（１）が受ける力
   を検出する力検出部（３）と、この検出された力及び目
   標とする力に基づいて上記速度指令信号を発する力制御
   部（４）とを備えた制御装置において、 上記速度指令信号のうちマニピュレータ（１）の固有振
   動数のみに係る成分を減衰させるフィルタ部（５）を設
   けたことを特徴とするマニピュレータの制御装置。